tc/f_canprio.c

   1 /* f_canprio.c          Filter for CAN packets */
   2
   3 #include <stdio.h>
   4 #include <stdlib.h>
   5 #include <unistd.h>
   6 #include <syslog.h>
   7 #include <fcntl.h>
   8 #include <sys/socket.h>
   9 #include <netinet/in.h>
  10 #include <arpa/inet.h>
  11 #include <string.h>
  12 #include <linux/if.h>
  13 #include <limits.h>
  14 #include <inttypes.h>
  15 #include "utils.h"
  16 #include "tc_util.h"
  17
  18 #ifdef CONFIG_HAVE_LINUX_CAN_H
  19   #include "linux/can.h"
  20
  21 #elif !defined(CAN_EFF_FLAG)
  22   /* special address description flags for the CAN_ID */
  23   #define CAN_EFF_FLAG 0x80000000U /* EFF/SFF is set in the MSB */
  24   #define CAN_RTR_FLAG 0x40000000U /* remote transmission request */
  25   #define CAN_ERR_FLAG 0x20000000U /* error frame */
  26
  27   /* valid bits in CAN ID for frame formats */
  28   #define CAN_SFF_MASK 0x000007FFU /* standard frame format (SFF) */
  29   #define CAN_EFF_MASK 0x1FFFFFFFU /* extended frame format (EFF) */
  30   #define CAN_ERR_MASK 0x1FFFFFFFU /* omit EFF, RTR, ERR flags */
  31 #endif
  32
  33
  34 struct canprio_rule {
  35         __u32 canid;
  36         __u32 canid_mask;
  37 };
  38 struct canprio_rule canprio_rules[32];
  39
  40 static void explain(void)
  41 {
  42         fprintf(stderr, "Usage: ... canprio [ MATCHSPEC ] [ flowid FLOWID ]\n"
  43                         "\n"
  44                         "Where: MATCHSPEC := { matchid FILTERID | matcheid FILTERID | \n"
  45                         "                   MATCHSPEC ... }\n"
  46                         "       FILTERID := CANID:MASK \n"
  47                         "\n"
  48                         "NOTE: CLASSID, CANID, MASK is parsed as hexadecimal input.\n");
  49 }
  50
  51
  52 static int canprio_parse_opt(struct filter_util *qu, char *handle,
  53                          int argc, char **argv, struct nlmsghdr *n)
  54 {
  55         struct tcmsg *t = NLMSG_DATA(n);  // Why?
  56         struct rtattr *tail;
  57         long h = 0;
  58         __u32 canid;
  59         __u32 canid_mask;
  60         int rules_count = 0;
  61
  62         if (argc == 0)
  63                 return 0;
  64
  65         if (handle) {
  66                 h = strtol(handle, NULL, 0);
  67                 if (h == LONG_MIN || h == LONG_MAX) {
  68                         fprintf(stderr, "Illegal handle \"%s\", must be numeric.\n",
  69                                 handle);
  70                         return -1;
  71                 }
  72         }
  73
  74         t->tcm_handle = h;  // Why?
  75
  76         tail = (struct rtattr*)(((void*)n)+NLMSG_ALIGN(n->nlmsg_len));
  77         addattr_l(n, MAX_MSG, TCA_OPTIONS, NULL, 0);
  78
  79         while (argc > 0) {
  80                 if (matches(*argv, "match") == 0) {
  81                         //printf("  [parsing match ...");
  82                         NEXT_ARG();
  83
  84                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &canid, &canid_mask) != 2) {
  85                                 fprintf(stderr, "improperly formed Can ID & mask '%s'\n", *argv);
  86                                 return -1;
  87                         }
  88
  89                         //printf("... 0x%x:0x%x]\n", canid, canid_mask);
  90                         if (canid >= 0) {// FIXME boundary check
  91                                 canprio_rules[rules_count].canid = canid;
  92                                 canprio_rules[rules_count].canid_mask = canid_mask;
  93                                 rules_count++;
  94
  95                         }
  96
  97                 } else if (matches(*argv, "matchid") == 0) {
  98                         NEXT_ARG();
  99
 100                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &canid, &canid_mask) != 2) {
 101                                 fprintf(stderr, "improperly formed Can ID & mask '%s'\n", *argv);
 102                                 return -1;
 103                         }
 104
 105                         if (canid & ~CAN_SFF_MASK) {
 106                                 fprintf(stderr, "Id 0x%lx exceeded standard CAN ID range.",
 107                                         (unsigned long)canid);
 108                                 return -1;
 109                         }
 110
 111                         canprio_rules[rules_count].canid = canid;
 112                         canprio_rules[rules_count].canid_mask = (canid_mask & CAN_SFF_MASK);
 113                         rules_count++;
 114
 115                 } else if (matches(*argv, "matcheid") == 0) {
 116                         NEXT_ARG();
 117
 118                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &canid, &canid_mask) != 2) {
 119                                 fprintf(stderr, "improperly formed Can ID & mask '%s'\n", *argv);
 120                                 return -1;
 121                         }
 122
 123                         if (canid & ~CAN_EFF_MASK) {
 124                                 fprintf(stderr, "Id 0x%lx exceeded extended CAN ID range.",
 125                                         (unsigned long)canid);
 126                                 return -1;
 127                         }
 128
 129                         canprio_rules[rules_count].canid = canid | CAN_EFF_FLAG;
 130                         canprio_rules[rules_count].canid_mask = (canid_mask & CAN_EFF_MASK);
 131                         rules_count++;
 132
 133                 } else if (matches(*argv, "classid") == 0 ||
 134                                 strcmp(*argv, "flowid") == 0) {
 135                         //printf("  [parsing flowid]\n");
 136                         unsigned handle;
 137                         NEXT_ARG();
 138                         if (get_tc_classid(&handle, *argv)) {
 139                                 fprintf(stderr, "Illegal \"classid\"\n");
 140                                 return -1;
 141                         }
 142                         addattr_l(n, MAX_MSG, TCA_CANPRIO_CLASSID, &handle, 4);
 143
 144                 } else if (strcmp(*argv, "help") == 0) {
 145                         explain();
 146                         return -1;
 147
 148                 } else {
 149                         fprintf(stderr, "What is \"%s\"?\n", *argv);
 150                         explain();
 151                         return -1;
 152                 }
 153                 argc--; argv++;
 154         }
 155
 156
 157         addattr_l(n, MAX_MSG, TCA_CANPRIO_RULES, &canprio_rules,
 158                 sizeof(struct canprio_rule) * rules_count);
 159
 160         tail->rta_len = (((void*)n)+n->nlmsg_len) - (void*)tail;
 161         return 0;
 162 }
 163
 164 static int canprio_print_opt(struct filter_util *qu, FILE *f, struct rtattr *opt,
 165                          __u32 handle)
 166 {
 167         struct rtattr *tb[TCA_CANPRIO_MAX+1];
 168
 169
 170         if (opt == NULL)
 171                 return 0;
 172
 173         parse_rtattr_nested(tb, TCA_CANPRIO_MAX, opt);
 174
 175         if (handle)
 176                 fprintf(f, "handle 0x%x ", handle);
 177
 178         if (tb[TCA_BASIC_CLASSID]) {
 179                 SPRINT_BUF(b1); //FIXME wat?
 180                 fprintf(f, "flowid %s ",
 181                         sprint_tc_classid(*(__u32*)RTA_DATA(tb[TCA_BASIC_CLASSID]), b1));
 182         }
 183
 184         //FIXME
 185         //if (tb[TCA_CANPRIO_RULES])
 186         //      fprintf(f, "Canid 0x%x ",
 187         //                      *(__u32 *)RTA_DATA(tb[TCA_CANPRIO_MATCH]));
 188
 189
 190         return 0;
 191 }
 192
 193
 194 struct filter_util canprio_filter_util = {
 195         .id = "canprio",
 196         .parse_fopt = canprio_parse_opt,
 197         .print_fopt = canprio_print_opt,
 198 };
 199