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
  39 static void explain(void)
  40 {
  41         fprintf(stderr, "Usage: ... canprio [ MATCHSPEC ] [ flowid FLOWID ]\n"
  42                         "\n"
  43                         "Where: MATCHSPEC := { matchid FILTERID | matcheid FILTERID | \n"
  44                         "                   MATCHSPEC ... }\n"
  45                         "       FILTERID := CANID:MASK \n"
  46                         "\n"
  47                         "NOTE: CLASSID, CANID, MASK is parsed as hexadecimal input.\n");
  48 }
  49
  50 static int canprio_parse_opt(struct filter_util *qu, char *handle,
  51                          int argc, char **argv, struct nlmsghdr *n)
  52 {
  53         struct tcmsg *t = NLMSG_DATA(n);  // Why?
  54         struct rtattr *tail;
  55         struct canprio_rule canprio_rules[32];
  56         int rules_count = 0;
  57         long h = 0;
  58         __u32 canid;
  59         __u32 canid_mask;
  60
  61         if (argc == 0)
  62                 return 0;
  63
  64         if (handle) {
  65                 h = strtol(handle, NULL, 0);
  66                 if (h == LONG_MIN || h == LONG_MAX) {
  67                         fprintf(stderr, "Illegal handle \"%s\", must be numeric.\n",
  68                                 handle);
  69                         return -1;
  70                 }
  71         }
  72
  73         t->tcm_handle = h;  // Why?
  74
  75         tail = (struct rtattr*)(((void*)n)+NLMSG_ALIGN(n->nlmsg_len));
  76         addattr_l(n, MAX_MSG, TCA_OPTIONS, NULL, 0);
  77
  78         while (argc > 0) {
  79                 if (matches(*argv, "match") == 0) {
  80                         //printf("  [parsing match ...");
  81                         NEXT_ARG();
  82
  83                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &canid, &canid_mask) != 2) {
  84                                 fprintf(stderr, "improperly formed Can ID & mask '%s'\n", *argv);
  85                                 return -1;
  86                         }
  87
  88                         //printf("... 0x%x:0x%x]\n", canid, canid_mask);
  89                         if (canid >= 0) {// FIXME boundary check
  90                                 canprio_rules[rules_count].canid = canid;
  91                                 canprio_rules[rules_count].canid_mask = canid_mask;
  92                                 rules_count++;
  93                         }
  94
  95                 } else if (matches(*argv, "matchid") == 0) {
  96                         NEXT_ARG();
  97
  98                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &canid, &canid_mask) != 2) {
  99                                 fprintf(stderr, "improperly formed Can ID & mask '%s'\n", *argv);
 100                                 return -1;
 101                         }
 102
 103                         if (canid & ~CAN_SFF_MASK) {
 104                                 fprintf(stderr, "Id 0x%lx exceeded standard CAN ID range.",
 105                                         (unsigned long)canid);
 106                                 return -1;
 107                         }
 108
 109                         canprio_rules[rules_count].canid = canid;
 110                         canprio_rules[rules_count].canid_mask = (canid_mask & CAN_SFF_MASK);
 111                         rules_count++;
 112
 113                 } else if (matches(*argv, "matcheid") == 0) {
 114                         NEXT_ARG();
 115
 116                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &canid, &canid_mask) != 2) {
 117                                 fprintf(stderr, "improperly formed Can ID & mask '%s'\n", *argv);
 118                                 return -1;
 119                         }
 120
 121                         if (canid & ~CAN_EFF_MASK) {
 122                                 fprintf(stderr, "Id 0x%lx exceeded extended CAN ID range.",
 123                                         (unsigned long)canid);
 124                                 return -1;
 125                         }
 126
 127                         canprio_rules[rules_count].canid = canid | CAN_EFF_FLAG;
 128                         canprio_rules[rules_count].canid_mask = (canid_mask & CAN_EFF_MASK);
 129                         rules_count++;
 130
 131                 } else if (matches(*argv, "classid") == 0 ||
 132                                 strcmp(*argv, "flowid") == 0) {
 133                         //printf("  [parsing flowid]\n");
 134                         unsigned handle;
 135                         NEXT_ARG();
 136                         if (get_tc_classid(&handle, *argv)) {
 137                                 fprintf(stderr, "Illegal \"classid\"\n");
 138                                 return -1;
 139                         }
 140                         addattr_l(n, MAX_MSG, TCA_CANPRIO_CLASSID, &handle, 4);
 141
 142                 } else if (strcmp(*argv, "help") == 0) {
 143                         explain();
 144                         return -1;
 145
 146                 } else {
 147                         fprintf(stderr, "What is \"%s\"?\n", *argv);
 148                         explain();
 149                         return -1;
 150                 }
 151                 argc--; argv++;
 152         }
 153
 154         addattr_l(n, MAX_MSG, TCA_CANPRIO_RULES, &canprio_rules,
 155                 sizeof(struct canprio_rule) * rules_count);
 156
 157         tail->rta_len = (((void*)n)+n->nlmsg_len) - (void*)tail;
 158         return 0;
 159 }
 160
 161 /* FIXME I don't know how to test if function is correct.
 162 I was not able to figure out how to call this function */
 163 static int canprio_print_opt(struct filter_util *qu, FILE *f, struct rtattr *opt,
 164                          __u32 handle)
 165 {
 166         struct rtattr *tb[TCA_CANPRIO_MAX+1];
 167         struct canprio_rule *canprio_rules = NULL;
 168         int rules_count = 0;
 169         int i;
 170
 171         printf("canprio_print_opt() invoked \n");
 172
 173         if (opt == NULL)
 174                 return 0;
 175
 176         parse_rtattr_nested(tb, TCA_CANPRIO_MAX, opt);
 177
 178         if (handle)
 179                 fprintf(f, "handle 0x%x ", handle);
 180
 181         if (tb[TCA_BASIC_CLASSID]) {
 182                 SPRINT_BUF(b1); //allocate buffer b1
 183                 fprintf(f, "flowid %s ",
 184                         sprint_tc_classid(*(__u32*)RTA_DATA(tb[TCA_BASIC_CLASSID]), b1));
 185         }
 186
 187         if (tb[TCA_CANPRIO_RULES]) {
 188                 if (RTA_PAYLOAD(tb[TCA_CANPRIO_RULES]) < sizeof(struct canprio_rule))
 189                        return -1;
 190
 191                 canprio_rules = RTA_DATA(tb[TCA_CANPRIO_RULES]);
 192                 rules_count = RTA_LENGTH(canprio_rules);
 193
 194                 for(i = 0; i < rules_count; i++) {
 195                         fprintf(f, "CAN ID 0x%lx ",
 196                                 (unsigned long)(canprio_rules[i].canid));
 197                         fprintf(f, "CAN ID mask 0x%lx ",
 198                                 (unsigned long)(canprio_rules[i].canid_mask));
 199                 }
 200         }
 201
 202         return 0;
 203 }
 204
 205
 206 struct filter_util canprio_filter_util = {
 207         .id = "canprio",
 208         .parse_fopt = canprio_parse_opt,
 209         .print_fopt = canprio_print_opt,
 210 };
 211