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