tc/f_canprio.c

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