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  * 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@kormus.cz>
  15  * Founded 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
  55
  56 static void explain(void)
  57 {
  58         fprintf(stderr, "Usage: ... canprio [ MATCHSPEC ] [ flowid FLOWID ]\n"
  59                         "\n"
  60                         "Where: MATCHSPEC := { matchid FILTERID | matcheid FILTERID | \n"
  61                         "                   MATCHSPEC ... }\n"
  62                         "       FILTERID := CANID:MASK \n"
  63                         "\n"
  64                         "NOTE: CLASSID, CANID, MASK is parsed as hexadecimal input.\n");
  65 }
  66
  67 static int canprio_parse_opt(struct filter_util *qu, char *handle,
  68                          int argc, char **argv, struct nlmsghdr *n)
  69 {
  70         struct tcmsg *t = NLMSG_DATA(n);  // Why?
  71         struct rtattr *tail;
  72         struct can_filter canprio_rules[32];
  73         int rules_count = 0;
  74         long h = 0;
  75         __u32 can_id;
  76         __u32 can_mask;
  77
  78         if (argc == 0)
  79                 return 0;
  80
  81         if (handle) {
  82                 h = strtol(handle, NULL, 0);
  83                 if (h == LONG_MIN || h == LONG_MAX) {
  84                         fprintf(stderr, "Illegal handle \"%s\", must be numeric.\n",
  85                                 handle);
  86                         return -1;
  87                 }
  88         }
  89
  90         t->tcm_handle = h;  // Why?
  91
  92         tail = (struct rtattr*)(((void*)n)+NLMSG_ALIGN(n->nlmsg_len));
  93         addattr_l(n, MAX_MSG, TCA_OPTIONS, NULL, 0);
  94
  95         while (argc > 0) {
  96 //---------------------- legacy option; will be deleted in future -------------
  97                 if (matches(*argv, "match") == 0) {
  98                         NEXT_ARG();
  99
 100                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &can_id, &can_mask) != 2) {
 101                                 fprintf(stderr, "improperly formed Can ID & mask '%s'\n", *argv);
 102                                 return -1;
 103                         }
 104
 105                         //printf("... 0x%x:0x%x]\n", can_id, can_mask);
 106                         if (can_id >= 0) {// FIXME boundary check
 107                                 canprio_rules[rules_count].can_id = can_id;
 108                                 canprio_rules[rules_count].can_mask = can_mask;
 109                                 rules_count++;
 110                         }
 111 //---------------------- legacy option; will be deleted in future -------------
 112
 113                 } else if (matches(*argv, "matchid") == 0) { //Parse SFF ID
 114                         NEXT_ARG();
 115
 116                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &can_id, &can_mask) != 2) {
 117                                 fprintf(stderr, "improperly formed Can ID & mask '%s'\n", *argv);
 118                                 return -1;
 119                         }
 120
 121                         if (can_id & ~CAN_SFF_MASK) { //FIXME what about RTR flags?
 122                                 fprintf(stderr, "ID 0x%lx exceeded standard CAN ID range.\n",
 123                                         (unsigned long)can_id);
 124                                 return -1;
 125                         }
 126
 127                         canprio_rules[rules_count].can_id = can_id;
 128                         canprio_rules[rules_count].can_mask = (can_mask & CAN_SFF_MASK);
 129                         rules_count++;
 130
 131                 } else if (matches(*argv, "matcheid") == 0) { //Parse EFF ID
 132                         NEXT_ARG();
 133
 134                         if (sscanf(*argv, "%"SCNx32 ":" "%"SCNx32, &can_id, &can_mask) != 2) {
 135                                 fprintf(stderr, "improperly formed Can ID & mask '%s'\n", *argv);
 136                                 return -1;
 137                         }
 138
 139                         if (can_id & ~CAN_EFF_MASK) {
 140                                 fprintf(stderr, "ID 0x%lx exceeded extended CAN ID range.",
 141                                         (unsigned long)can_id);
 142                                 return -1;
 143                         }
 144
 145                         canprio_rules[rules_count].can_id = can_id | CAN_EFF_FLAG;
 146                         canprio_rules[rules_count].can_mask =
 147                                 (can_mask & CAN_EFF_MASK) | CAN_EFF_FLAG;
 148                         rules_count++;
 149
 150                 } else if (matches(*argv, "classid") == 0 ||
 151                                 strcmp(*argv, "flowid") == 0) {
 152                         //printf("  [parsing flowid]\n");
 153                         unsigned handle;
 154                         NEXT_ARG();
 155                         if (get_tc_classid(&handle, *argv)) {
 156                                 fprintf(stderr, "Illegal \"classid\"\n");
 157                                 return -1;
 158                         }
 159                         addattr_l(n, MAX_MSG, TCA_CANPRIO_CLASSID, &handle, 4);
 160
 161                 } else if (strcmp(*argv, "help") == 0) {
 162                         explain();
 163                         return -1;
 164
 165                 } else {
 166                         fprintf(stderr, "What is \"%s\"?\n", *argv);
 167                         explain();
 168                         return -1;
 169                 }
 170                 argc--; argv++;
 171         }
 172
 173         addattr_l(n, MAX_MSG, TCA_CANPRIO_RULES, &canprio_rules,
 174                 sizeof(struct can_filter) * rules_count);
 175
 176         tail->rta_len = (((void*)n)+n->nlmsg_len) - (void*)tail;
 177         return 0;
 178 }
 179
 180 /* When "tc filter show dev XY" is executed, function canprio_walk() (in kernel)
 181  * is called (which calls canprio_dump() for each instance of a filter) which sends
 182  * information about each instance of a filters to userspace -- to this function
 183  * (which parses the message and prints it.
 184  */
 185 static int canprio_print_opt(struct filter_util *qu, FILE *f, struct rtattr *opt,
 186                          __u32 handle)
 187 {
 188         struct rtattr *tb[TCA_CANPRIO_MAX+1];
 189         struct can_filter *canprio_rules = NULL;
 190         int rules_count = 0;
 191         int i;
 192
 193         if (opt == NULL)
 194                 return 0;
 195
 196         parse_rtattr_nested(tb, TCA_CANPRIO_MAX, opt);
 197
 198         if (handle)
 199                 fprintf(f, "handle 0x%x ", handle);
 200
 201
 202         if (tb[TCA_BASIC_CLASSID]) {
 203                 SPRINT_BUF(b1); //allocate buffer b1
 204                 fprintf(f, "flowid %s ",
 205                         sprint_tc_classid(*(__u32*)RTA_DATA(tb[TCA_BASIC_CLASSID]), b1));
 206         }
 207
 208         if (tb[TCA_CANPRIO_RULES]) {
 209                 if (RTA_PAYLOAD(tb[TCA_CANPRIO_RULES]) < sizeof(struct can_filter))
 210                        return -1;
 211
 212                 canprio_rules = RTA_DATA(tb[TCA_CANPRIO_RULES]);
 213                 rules_count = (RTA_PAYLOAD(tb[TCA_CANPRIO_RULES]) / sizeof(struct can_filter));
 214
 215                 for(i = 0; i < rules_count; i++) {
 216                         if (canprio_rules[i].can_id & CAN_EFF_FLAG) {
 217                                 fprintf(f, "matcheid 0x%lx:0x%lx ",
 218                                         (unsigned long)(canprio_rules[i].can_id & CAN_EFF_MASK),
 219                                         (unsigned long)(canprio_rules[i].can_mask & CAN_EFF_MASK));
 220                         }
 221                         else {
 222                                 fprintf(f, "matchid 0x%lx:0x%lx ",
 223                                         (unsigned long)(canprio_rules[i].can_id),
 224                                         (unsigned long)(canprio_rules[i].can_mask));
 225                         }
 226                 }
 227         }
 228
 229         return 0;
 230 }
 231
 232
 233 struct filter_util canprio_filter_util = {
 234         .id = "canprio",
 235         .parse_fopt = canprio_parse_opt,
 236         .print_fopt = canprio_print_opt,
 237 };
 238