tc/em_can.c

   1 /*
   2  * em_can.c
   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  * Authors:
  10  */
  11
  12 #include <stdio.h>
  13 #include <stdlib.h>
  14 #include <unistd.h>
  15 #include <syslog.h>
  16 #include <fcntl.h>
  17 #include <sys/socket.h>
  18 #include <netinet/in.h>
  19 #include <arpa/inet.h>
  20 #include <string.h>
  21 #include <errno.h>
  22 #include <linux/can.h>
  23 #include <inttypes.h>
  24 #include "m_ematch.h"
  25
  26 #define EM_CAN_RULES_SIZE                               32
  27
  28 extern struct ematch_util can_ematch_util;
  29
  30 static void can_print_usage(FILE *fd)
  31 {
  32         fprintf(fd,
  33             "Usage: can(MATCH)\n" \
  34             "where:  MATCH = {sff CANID | eff CANID}\n" \
  35             "        CANID := ID:MASK\n" \
  36             "\n" \
  37             "Example: u32(sff 0x123 sff 0x124 sff 0x125:0xf)\n");
  38 }
  39
  40 static int can_parse_rule(struct can_filter *rule, struct bstr *a, int iseff)
  41 {
  42         unsigned int can_id = 0;
  43         unsigned int can_mask = 0;
  44
  45         if (sscanf(a->data, "%"SCNx32 ":" "%"SCNx32, &can_id, &can_mask) != 2) {
  46                 if (sscanf(a->data, "%"SCNx32, &can_id) != 1) {
  47                         return -1;
  48                 } else
  49                         can_mask = CAN_SFF_MASK;
  50         }
  51         rule->can_id = can_id | ((iseff) ? CAN_EFF_FLAG : 0) ;
  52         rule->can_mask = can_mask;
  53
  54         return 0;
  55 }
  56
  57 static int can_parse_eopt(struct nlmsghdr *n, struct tcf_ematch_hdr *hdr,
  58                           struct bstr *args)
  59 {
  60         struct bstr *a;
  61         struct can_filter rules_raw[EM_CAN_RULES_SIZE];
  62         int iseff = 0;
  63         int i = 0;
  64         int ret;
  65
  66         memset(&rules_raw, 0, sizeof(rules_raw));
  67
  68 #define PARSE_ERR(CARG, FMT, ARGS...) \
  69         em_parse_error(EINVAL, args, CARG, &can_ematch_util, FMT ,##ARGS)
  70
  71         if (args == NULL)
  72                 return PARSE_ERR(args, "can: missing arguments");
  73
  74         if (!bstrcmp(args, "sff"))
  75                 iseff = 0;
  76         else if (!bstrcmp(args, "eff"))
  77                 iseff = 1;
  78         else
  79                 return PARSE_ERR(args, "can: invalid key");
  80
  81         a = bstr_next(args);
  82         if (a == NULL)
  83                 return PARSE_ERR(a, "can: missing key");
  84
  85         ret = can_parse_rule(&rules_raw[0], a, iseff);
  86         if (ret == -1)
  87                 return PARSE_ERR(a, "can: Improperly formed CAN ID & mask\n");
  88
  89         for (i = 1; i < (sizeof(rules_raw)/sizeof(struct can_filter)); i++) {
  90                 a = bstr_next(a);
  91                 if (a == NULL) {
  92                         break;
  93                 }
  94
  95                 ret = can_parse_rule(&rules_raw[i], a, iseff);
  96                 if (ret == -1)
  97                         return PARSE_ERR(a, "can: Improperly formed CAN ID & mask\n");
  98
  99 #if 0
 100                 can_id = bstrtoul(a);
 101                 if (can_id == ULONG_MAX)
 102                         return PARSE_ERR(a, "can: invalid ID, must be numeric");
 103
 104                 rules_raw[i].can_id = can_id | (iseff) ? CAN_EFF_FLAG : 0 ;
 105 #endif
 106         }
 107
 108         // FIX THIS !@#$%
 109         //if (a->next)
 110         //      return PARSE_ERR(a->next, "can: unexpected trailer");
 111
 112         addraw_l(n, MAX_MSG, hdr, sizeof(*hdr));
 113         addraw_l(n, MAX_MSG, &rules_raw, sizeof(struct can_filter) * i); // FIXME is size ok
 114
 115 #undef PARSE_ERR
 116         return 0;
 117 }
 118
 119 static int can_print_eopt(FILE *fd, struct tcf_ematch_hdr *hdr, void *data,
 120                           int data_len)
 121 {
 122         struct can_filter *conf = data; /* Array with rules, fixed size EM_CAN_RULES_SIZE */
 123
 124 #if 0
 125         if (data_len < sizeof(*conf)) {
 126                 fprintf(stderr, "U32 header size mismatch\n");
 127                 return -1;
 128         }
 129
 130         fprintf(fd, "%08x/%08x at %s%d",
 131             (unsigned int) ntohl(u_key->val),
 132             (unsigned int) ntohl(u_key->mask),
 133             u_key->offmask ? "nexthdr+" : "",
 134             u_key->off);
 135 #endif
 136
 137         return 0;
 138 }
 139
 140 struct ematch_util can_ematch_util = {
 141         .kind = "can",
 142         .kind_num = TCF_EM_CAN,
 143         .parse_eopt = can_parse_eopt,
 144         .print_eopt = can_print_eopt,
 145         .print_usage = can_print_usage
 146 };