tc/em_canid.c

   1 /*
   2  * em_canid.c  Ematch rule to match CAN frames according to their CAN identifiers
   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  * Documentation: http://rtime.felk.cvut.cz/can/socketcan-qdisc-final.pdf
  18  */
  19
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <unistd.h>
  23 #include <syslog.h>
  24 #include <fcntl.h>
  25 #include <sys/socket.h>
  26 #include <netinet/in.h>
  27 #include <arpa/inet.h>
  28 #include <string.h>
  29 #include <errno.h>
  30 #include <linux/can.h>
  31 #include <inttypes.h>
  32 #include "m_ematch.h"
  33
  34 #define EM_CANID_RULES_MAX 400 /* Main reason for this number is Nelink
  35         message size limit equal to Single memory page size. When dump()
  36         is invoked, there are even some ematch related headers sent from
  37         kernel to userspace together with em_canid configuration --
  38         400*sizeof(struct can_filter) should fit without any problems */
  39
  40 extern struct ematch_util canid_ematch_util;
  41 struct rules {
  42         struct can_filter *rules_raw;
  43         int rules_capacity;     /* Size of array allocated for rules_raw */
  44         int rules_cnt;          /* Actual number of rules stored in rules_raw */
  45 };
  46
  47 static void canid_print_usage(FILE *fd)
  48 {
  49         fprintf(fd,
  50                 "Usage: canid(IDLIST)\n" \
  51                 "where: IDLIST := IDSPEC [ IDLIST ]\n" \
  52                 "       IDSPEC := { ’sff’ CANID | ’eff’ CANID }\n" \
  53                 "       CANID := ID[:MASK]\n" \
  54                 "       ID, MASK := hexadecimal number (i.e. 0x123)\n" \
  55                 "Example: canid(sff 0x123 sff 0x124 sff 0x125:0xf)\n");
  56 }
  57
  58 static int canid_parse_rule(struct rules *rules, struct bstr *a, int iseff)
  59 {
  60         unsigned int can_id = 0;
  61         unsigned int can_mask = 0;
  62
  63         if (sscanf(a->data, "%"SCNx32 ":" "%"SCNx32, &can_id, &can_mask) != 2) {
  64                 if (sscanf(a->data, "%"SCNx32, &can_id) != 1) {
  65                         return -1;
  66                 } else {
  67                         can_mask = (iseff) ? CAN_EFF_MASK : CAN_SFF_MASK;
  68                 }
  69         }
  70
  71         /* Stretch rules array up to EM_CANID_RULES_MAX if necessary */
  72         if (rules->rules_cnt == rules->rules_capacity) {
  73                 if (rules->rules_capacity <= EM_CANID_RULES_MAX/2) {
  74                         rules->rules_capacity *= 2;
  75                         rules->rules_raw = realloc(rules->rules_raw,
  76                                 sizeof(struct can_filter) * rules->rules_capacity);
  77                 } else {
  78                         return -2;
  79                 }
  80         }
  81
  82         rules->rules_raw[rules->rules_cnt].can_id =
  83                 can_id | ((iseff) ? CAN_EFF_FLAG : 0);
  84         rules->rules_raw[rules->rules_cnt].can_mask =
  85                 can_mask | CAN_EFF_FLAG;
  86
  87         rules->rules_cnt++;
  88
  89         return 0;
  90 }
  91
  92 static int canid_parse_eopt(struct nlmsghdr *n, struct tcf_ematch_hdr *hdr,
  93                           struct bstr *args)
  94 {
  95         int iseff = 0;
  96         int ret = 0;
  97         struct rules rules = {
  98                 .rules_capacity = 25, /* Denominator of EM_CANID_RULES_MAX
  99                         Will be multiplied by 2 to calculate the size for realloc() */
 100                 .rules_cnt = 0
 101         };
 102
 103 #define PARSE_ERR(CARG, FMT, ARGS...) \
 104         em_parse_error(EINVAL, args, CARG, &canid_ematch_util, FMT, ##ARGS)
 105
 106         if (args == NULL)
 107                 return PARSE_ERR(args, "canid: missing arguments");
 108
 109         rules.rules_raw = calloc(rules.rules_capacity,
 110                                  sizeof(struct can_filter));
 111
 112         do {
 113                 if (!bstrcmp(args, "sff")) {
 114                         iseff = 0;
 115                 } else if (!bstrcmp(args, "eff")) {
 116                         iseff = 1;
 117                 } else {
 118                         ret = PARSE_ERR(args, "canid: invalid key");
 119                         goto exit;
 120                 }
 121
 122                 args = bstr_next(args);
 123                 if (args == NULL) {
 124                         ret = PARSE_ERR(args, "canid: missing argument");
 125                         goto exit;
 126                 }
 127
 128                 ret = canid_parse_rule(&rules, args, iseff);
 129                 if (ret == -1) {
 130                         ret = PARSE_ERR(args, "canid: Improperly formed CAN ID & mask\n");
 131                         goto exit;
 132                 } else if (ret == -2) {
 133                         ret = PARSE_ERR(args, "canid: Too many arguments on input\n");
 134                         goto exit;
 135                 }
 136         } while ((args = bstr_next(args)) != NULL);
 137
 138         addraw_l(n, MAX_MSG, hdr, sizeof(*hdr));
 139         addraw_l(n, MAX_MSG, rules.rules_raw,
 140                 sizeof(struct can_filter) * rules.rules_cnt);
 141
 142 #undef PARSE_ERR
 143 exit:
 144         free(rules.rules_raw);
 145         return ret;
 146 }
 147
 148 static int canid_print_eopt(FILE *fd, struct tcf_ematch_hdr *hdr, void *data,
 149                           int data_len)
 150 {
 151         struct can_filter *conf = data; /* Array with rules */
 152         int rules_count;
 153         int i;
 154
 155         rules_count = data_len / sizeof(struct can_filter);
 156
 157         for (i = 0; i < rules_count; i++) {
 158                 struct can_filter *pcfltr = &conf[i];
 159
 160                 if (pcfltr->can_id & CAN_EFF_FLAG) {
 161                         if (pcfltr->can_mask == (CAN_EFF_FLAG | CAN_EFF_MASK))
 162                                 fprintf(fd, "eff 0x%"PRIX32,
 163                                                 pcfltr->can_id & CAN_EFF_MASK);
 164                         else
 165                                 fprintf(fd, "eff 0x%"PRIX32":0x%"PRIX32,
 166                                                 pcfltr->can_id & CAN_EFF_MASK,
 167                                                 pcfltr->can_mask & CAN_EFF_MASK);
 168                 } else {
 169                         if (pcfltr->can_mask == (CAN_EFF_FLAG | CAN_SFF_MASK))
 170                                 fprintf(fd, "sff 0x%"PRIX32,
 171                                                 pcfltr->can_id & CAN_SFF_MASK);
 172                         else
 173                                 fprintf(fd, "sff 0x%"PRIX32":0x%"PRIX32,
 174                                                 pcfltr->can_id & CAN_SFF_MASK,
 175                                                 pcfltr->can_mask & CAN_SFF_MASK);
 176                 }
 177
 178                 if ((i + 1) < rules_count)
 179                         fprintf(fd, " ");
 180         }
 181
 182         return 0;
 183 }
 184
 185 struct ematch_util canid_ematch_util = {
 186         .kind = "canid",
 187         .kind_num = TCF_EM_CANID,
 188         .parse_eopt = canid_parse_eopt,
 189         .print_eopt = canid_print_eopt,
 190         .print_usage = canid_print_usage
 191 };