tc/f_can.c

   1 /*
   2  * f_can.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  * Idea:       Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
  10  * Copyright:  (c) 2011 Czech Technical University in Prague
  11  *             (c) 2011 Volkswagen Group Research
  12  * Authors:    Michal Sojka <sojkam1@fel.cvut.cz>
  13  *             Pavel Pisa <pisa@cmp.felk.cvut.cz>
  14  *             Rostislav Lisovy <lisovy@gmail.cz>
  15  * Funded by:  Volkswagen Group Research
  16  */
  17
  18 #include <stdio.h>
  19 #include <stdlib.h>
  20 #include <unistd.h>
  21 #include <syslog.h>
  22 #include <fcntl.h>
  23 #include <sys/socket.h>
  24 #include <netinet/in.h>
  25 #include <arpa/inet.h>
  26 #include <string.h>
  27 #include <linux/if.h>
  28 #include <limits.h>
  29 #include <inttypes.h>
  30 #include "utils.h"
  31 #include "tc_util.h"
  32
  33 #ifdef CONFIG_HAVE_LINUX_CAN_H
  34   #include "linux/can.h"
  35
  36 #elif !defined(CAN_EFF_FLAG)
  37   /* special address description flags for the CAN_ID */
  38   #define CAN_EFF_FLAG 0x80000000U /* EFF/SFF is set in the MSB */
  39   #define CAN_RTR_FLAG 0x40000000U /* remote transmission request */
  40   #define CAN_ERR_FLAG 0x20000000U /* error frame */
  41
  42   /* valid bits in CAN ID for frame formats */
  43   #define CAN_SFF_MASK 0x000007FFU /* standard frame format (SFF) */
  44   #define CAN_EFF_MASK 0x1FFFFFFFU /* extended frame format (EFF) */
  45   #define CAN_ERR_MASK 0x1FFFFFFFU /* omit EFF, RTR, ERR flags */
  46
  47   typedef __u32 canid_t;
  48   struct can_filter {
  49         canid_t can_id;
  50         canid_t can_mask;
  51   };
  52 #endif
  53
  54 #define RULES_SIZE              128 /* Maximum number of rules sent via the
  55                                 netlink message during creation/configuration */
  56
  57
  58 static void explain(void)
  59 {
  60         fprintf(stderr, "Usage: ... can [ MATCHSPEC ] [ flowid FLOWID ]\n"
  61                         "\n"
  62                         "Where: MATCHSPEC := { sffid FILTERID | effid FILTERID | \n"
  63                         "                   MATCHSPEC ... }\n"
  64                         "       FILTERID := CANID[:MASK] \n"
  65                         "\n"
  66                         "NOTE: CLASSID, CANID, MASK is parsed as hexadecimal input.\n");
  67 }
  68
  69 static int canfltr_parse_opt(struct filter_util *qu, char *handle,
  70                          int argc, char **argv, struct nlmsghdr *n)
  71 {
  72         struct tcmsg *t = NLMSG_DATA(n);
  73         struct rtattr *tail;
  74         struct can_filter canfltr_rules[RULES_SIZE];
  75         int rules_count = 0;
  76         long h = 0;
  77         __u32 can_id;
  78         __u32 can_mask;
  79
  80         if (argc == 0)
  81                 return 0;
  82
  83         if (handle) {
  84                 h = strtol(handle, NULL, 0);
  85                 if (h == LONG_MIN || h == LONG_MAX) {
  86                         fprintf(stderr, "Illegal handle \"%s\", must be numeric.\n",
  87                                 handle);
  88                         return -1;
  89                 }
  90         }
  91
  92         t->tcm_handle = h;
  93
  94         tail = NLMSG_TAIL(n);
  95         addattr_l(n, MAX_MSG, TCA_OPTIONS, NULL, 0);
  96
  97         while (argc > 0) {
  98                 if (matches(*argv, "sffid") == 0) {
  99                         /* parse SFF CAN ID optionally with mask */
 100                         if (rules_count >= RULES_SIZE) {
 101                                 fprintf(stderr, "Too much rules on input. "
 102                                         "Maximum number of rules is: %d\n", RULES_SIZE);
 103                                 return -1;
 104                         }
 105
 106                         NEXT_ARG();
 107
 108                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &can_id, &can_mask) != 2) {
 109                                 if (sscanf(*argv, "%"SCNx32, &can_id) != 1) {
 110                                         fprintf(stderr, "Improperly formed CAN ID & mask '%s'\n", *argv);
 111                                         return -1;
 112                                 } else
 113                                         can_mask = CAN_SFF_MASK;
 114                         }
 115
 116                         /* we do not support extra handling for RTR frames due to the bitmap approach */
 117                         if (can_id & ~CAN_SFF_MASK) {
 118                                 fprintf(stderr, "ID 0x%lx exceeded standard CAN ID range.\n",
 119                                         (unsigned long)can_id);
 120                                 return -1;
 121                         }
 122
 123                         canfltr_rules[rules_count].can_id = can_id;
 124                         canfltr_rules[rules_count].can_mask = (can_mask & CAN_SFF_MASK);
 125                         rules_count++;
 126
 127                 } else if (matches(*argv, "effid") == 0) {
 128                         /* parse EFF CAN ID optionally with mask */
 129                         if (rules_count >= RULES_SIZE) {
 130                                 fprintf(stderr, "Too much rules on input. "
 131                                         "Maximum number of rules is: %d\n", RULES_SIZE);
 132                                 return -1;
 133                         }
 134
 135                         NEXT_ARG();
 136
 137                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &can_id, &can_mask) != 2) {
 138                                 if (sscanf(*argv, "%"SCNx32, &can_id) != 1) {
 139                                         fprintf(stderr, "Improperly formed CAN ID & mask '%s'\n", *argv);
 140                                         return -1;
 141                                 } else
 142                                         can_mask = CAN_EFF_MASK;
 143                         }
 144
 145                         if (can_id & ~CAN_EFF_MASK) {
 146                                 fprintf(stderr, "ID 0x%lx exceeded extended CAN ID range.",
 147                                         (unsigned long)can_id);
 148                                 return -1;
 149                         }
 150
 151                         canfltr_rules[rules_count].can_id = can_id | CAN_EFF_FLAG;
 152                         canfltr_rules[rules_count].can_mask = (can_mask & CAN_EFF_MASK) | CAN_EFF_FLAG;
 153                         rules_count++;
 154
 155                 } else if (matches(*argv, "classid") == 0 || strcmp(*argv, "flowid") == 0) {
 156                         unsigned handle;
 157                         NEXT_ARG();
 158                         if (get_tc_classid(&handle, *argv)) {
 159                                 fprintf(stderr, "Illegal \"classid\"\n");
 160                                 return -1;
 161                         }
 162                         addattr_l(n, MAX_MSG, TCA_CANFLTR_CLASSID, &handle, 4);
 163
 164                 } else if (strcmp(*argv, "help") == 0) {
 165                         explain();
 166                         return -1;
 167
 168                 } else {
 169                         fprintf(stderr, "What is \"%s\"?\n", *argv);
 170                         explain();
 171                         return -1;
 172                 }
 173                 argc--; argv++;
 174         }
 175
 176         addattr_l(n, MAX_MSG, TCA_CANFLTR_RULES, &canfltr_rules,
 177                 sizeof(struct can_filter) * rules_count);
 178
 179         tail->rta_len = (void *)NLMSG_TAIL(n) - (void *)tail;
 180         return 0;
 181 }
 182
 183 /* When "tc filter show dev XY" is executed, function canfltr_walk() (in kernel)
 184  * is called (which calls canfltr_dump() for each instance of a filter) which sends
 185  * information about each instance of a filters to userspace -- to this function
 186  * (which parses the message and prints it).
 187  */
 188 static int canfltr_print_opt(struct filter_util *qu, FILE *f, struct rtattr *opt,
 189                          __u32 handle)
 190 {
 191         struct rtattr *tb[TCA_CANFLTR_MAX+1];
 192         struct can_filter *canfltr_rules = NULL;
 193         int rules_count = 0;
 194         int i;
 195
 196         if (opt == NULL)
 197                 return 0;
 198
 199         parse_rtattr_nested(tb, TCA_CANFLTR_MAX, opt);
 200
 201         if (handle)
 202                 fprintf(f, "handle 0x%x ", handle);
 203
 204
 205         if (tb[TCA_BASIC_CLASSID]) {
 206                 SPRINT_BUF(b1); /* allocates buffer b1 */
 207                 fprintf(f, "flowid %s ",
 208                         sprint_tc_classid(*(__u32*)RTA_DATA(tb[TCA_BASIC_CLASSID]), b1));
 209         }
 210
 211         if (tb[TCA_CANFLTR_RULES]) {
 212                 if (RTA_PAYLOAD(tb[TCA_CANFLTR_RULES]) < sizeof(struct can_filter))
 213                        return -1;
 214
 215                 canfltr_rules = RTA_DATA(tb[TCA_CANFLTR_RULES]);
 216                 rules_count = (RTA_PAYLOAD(tb[TCA_CANFLTR_RULES]) / sizeof(struct can_filter));
 217
 218                 for(i = 0; i < rules_count; i++) {
 219                         if (canfltr_rules[i].can_id & CAN_EFF_FLAG) {
 220                                 if (canfltr_rules[i].can_mask == (CAN_EFF_FLAG|CAN_EFF_MASK))
 221                                         fprintf(f, "effid 0x%lx ",
 222                                                 (unsigned long)(canfltr_rules[i].can_id & CAN_EFF_MASK));
 223                                 else
 224                                         fprintf(f, "effid 0x%lx:0x%lx ",
 225                                                 (unsigned long)(canfltr_rules[i].can_id & CAN_EFF_MASK),
 226                                                 (unsigned long)(canfltr_rules[i].can_mask & CAN_EFF_MASK));
 227                         } else {
 228                                 if (canfltr_rules[i].can_mask == CAN_SFF_MASK)
 229                                         fprintf(f, "sffid 0x%lx ",
 230                                                 (unsigned long)(canfltr_rules[i].can_id));
 231                                 else
 232                                         fprintf(f, "sffid 0x%lx:0x%lx ",
 233                                                 (unsigned long)(canfltr_rules[i].can_id),
 234                                                 (unsigned long)(canfltr_rules[i].can_mask));
 235                         }
 236                 }
 237         }
 238
 239         return 0;
 240 }
 241
 242 struct filter_util can_filter_util = {
 243         .id = "can",
 244         .parse_fopt = canfltr_parse_opt,
 245         .print_fopt = canfltr_print_opt,
 246 };
 247