tc/em_canid.c

   1 /*
   2  * em_canid.c  Ematch rule to match CAN frames according to their CAN IDs
   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 <errno.h>
  28 #include <linux/can.h>
  29 #include <inttypes.h>
  30 #include "m_ematch.h"
  31
  32 #define EM_CAN_RULES_SIZE                               32
  33
  34 extern struct ematch_util canid_ematch_util;
  35
  36 static void canid_print_usage(FILE *fd)
  37 {
  38         fprintf(fd,
  39             "Usage: can(MATCH)\n" \
  40             "where:  MATCH = {sff CANID | eff CANID}\n" \
  41             "        CANID := ID:MASK\n" \
  42             "\n" \
  43             "Example: canid(sff 0x123 sff 0x124 sff 0x125:0xf)\n");
  44 }
  45
  46 static int canid_parse_rule(struct can_filter *rule, struct bstr *a, int iseff)
  47 {
  48         unsigned int can_id = 0;
  49         unsigned int can_mask = 0;
  50
  51         if (sscanf(a->data, "%"SCNx32 ":" "%"SCNx32, &can_id, &can_mask) != 2) {
  52                 if (sscanf(a->data, "%"SCNx32, &can_id) != 1) {
  53                         return -1;
  54                 } else {
  55                         can_mask = (iseff) ? CAN_EFF_MASK : CAN_SFF_MASK;
  56                 }
  57         }
  58         rule->can_id = can_id | ((iseff) ? CAN_EFF_FLAG : 0);
  59         rule->can_mask = can_mask | ((iseff) ? CAN_EFF_FLAG : 0);
  60
  61         return 0;
  62 }
  63
  64 static int canid_parse_eopt(struct nlmsghdr *n, struct tcf_ematch_hdr *hdr,
  65                           struct bstr *args)
  66 {
  67         struct bstr *a;
  68         struct can_filter rules_raw[EM_CAN_RULES_SIZE];
  69         int iseff = 0;
  70         int i = 0;
  71         int ret;
  72
  73         memset(&rules_raw, 0, sizeof(rules_raw));
  74
  75 #define PARSE_ERR(CARG, FMT, ARGS...) \
  76         em_parse_error(EINVAL, args, CARG, &canid_ematch_util, FMT ,##ARGS)
  77
  78         if (args == NULL)
  79                 return PARSE_ERR(args, "canid: missing arguments");
  80
  81         if (!bstrcmp(args, "sff"))
  82                 iseff = 0;
  83         else if (!bstrcmp(args, "eff"))
  84                 iseff = 1;
  85         else
  86                 return PARSE_ERR(args, "canid: invalid key");
  87
  88         a = bstr_next(args);
  89         if (a == NULL)
  90                 return PARSE_ERR(a, "canid: missing key");
  91
  92         ret = canid_parse_rule(&rules_raw[0], a, iseff);
  93         if (ret == -1)
  94                 return PARSE_ERR(a, "canid: Improperly formed CAN ID & mask\n");
  95
  96         for (i = 1; i < (sizeof(rules_raw)/sizeof(struct can_filter)); i++) {
  97                 /* There is no another keyword to parse */
  98                 a = bstr_next(a);
  99                 if (a == NULL) {
 100                         break;
 101                 }
 102
 103                 if (!bstrcmp(a, "sff"))
 104                         iseff = 0;
 105                 else if (!bstrcmp(a, "eff"))
 106                         iseff = 1;
 107                 else
 108                         return PARSE_ERR(args, "canid: invalid key");
 109
 110                 a = bstr_next(a);
 111                 if (a == NULL)
 112                         return PARSE_ERR(a, "canid: missing argument");
 113
 114                 ret = canid_parse_rule(&rules_raw[i], a, iseff);
 115                 if (ret == -1)
 116                         return PARSE_ERR(a, "canid: Improperly formed CAN ID & mask\n");
 117         }
 118
 119         // FIX THIS !@#$%
 120         //if (a->next)
 121         //      return PARSE_ERR(a->next, "canid: unexpected trailer");
 122
 123         addraw_l(n, MAX_MSG, hdr, sizeof(*hdr));
 124         addraw_l(n, MAX_MSG, &rules_raw, sizeof(struct can_filter) * i); // FIXME is size ok
 125
 126 #undef PARSE_ERR
 127         return 0;
 128 }
 129
 130 static int canid_print_eopt(FILE *fd, struct tcf_ematch_hdr *hdr, void *data,
 131                           int data_len)
 132 {
 133         struct can_filter *conf = data; /* Array with rules, fixed size EM_CAN_RULES_SIZE */
 134         int rules_count;
 135         int i;
 136
 137 #if 0
 138         if (data_len < sizeof(*conf)) {
 139                 fprintf(stderr, "U32 header size mismatch\n");
 140                 return -1;
 141         }
 142 #endif
 143
 144         rules_count = data_len / sizeof(struct can_filter);
 145
 146         for (i = 0; i < rules_count; i++) {
 147                 struct can_filter *pcfltr = &conf[i];
 148
 149                 if (pcfltr->can_id & CAN_EFF_FLAG) {
 150                         if (pcfltr->can_mask == (CAN_EFF_FLAG|CAN_EFF_MASK))
 151                                 fprintf(fd, "effid 0x%"PRIX32,
 152                                                 pcfltr->can_id & CAN_EFF_MASK);
 153                         else
 154                                 fprintf(fd, "effid 0x%"PRIX32":0x%"PRIX32,
 155                                                 pcfltr->can_id & CAN_EFF_MASK,
 156                                                 pcfltr->can_mask & CAN_EFF_MASK);
 157                 } else {
 158                         if (pcfltr->can_mask == CAN_SFF_MASK)
 159                                 fprintf(fd, "sffid 0x%"PRIX32,
 160                                                 pcfltr->can_id);
 161                         else
 162                                 fprintf(fd, "sffid 0x%"PRIX32":0x%"PRIX32,
 163                                                 pcfltr->can_id,
 164                                                 pcfltr->can_mask);
 165                 }
 166
 167                 if ((i + 1) < rules_count)
 168                         fprintf(fd, " ");
 169         }
 170
 171         return 0;
 172 }
 173
 174 struct ematch_util canid_ematch_util = {
 175         .kind = "canid",
 176         .kind_num = TCF_EM_CANID,
 177         .parse_eopt = canid_parse_eopt,
 178         .print_eopt = canid_print_eopt,
 179         .print_usage = canid_print_usage
 180 };