net/sched/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 <linux/slab.h>
  19 #include <linux/module.h>
  20 #include <linux/types.h>
  21 #include <linux/kernel.h>
  22 #include <linux/string.h>
  23 #include <linux/skbuff.h>
  24 #include <net/pkt_cls.h>
  25 #include <linux/can.h>
  26
  27 #define EM_CAN_RULES_MAX                                        500
  28
  29 struct canid_match {
  30         /* For each SFF CAN ID (11 bit) there is one record in this bitfield */
  31         DECLARE_BITMAP(match_sff, (1 << CAN_SFF_ID_BITS));
  32
  33         int rules_count;
  34         int sff_rules_count;
  35         int eff_rules_count;
  36
  37         /*
  38          * Raw rules copied from netlink message; Used for sending
  39          * information to userspace (when 'tc filter show' is invoked)
  40          * AND when matching EFF frames
  41          */
  42         struct can_filter rules_raw[];
  43 };
  44
  45 /**
  46  * em_canid_get_id() - Extracts Can ID out of the sk_buff structure.
  47  */
  48 static canid_t em_canid_get_id(struct sk_buff *skb)
  49 {
  50         /* CAN ID is stored within the data field */
  51         struct can_frame *cf = (struct can_frame *)skb->data;
  52
  53         return cf->can_id;
  54 }
  55
  56 static void em_canid_sff_match_add(struct canid_match *cm, u32 can_id,
  57                                         u32 can_mask)
  58 {
  59         int i;
  60
  61         /*
  62          * Limit can_mask and can_id to SFF range to
  63          * protect against write after end of array
  64          */
  65         can_mask &= CAN_SFF_MASK;
  66         can_id &= can_mask;
  67
  68         /* Single frame */
  69         if (can_mask == CAN_SFF_MASK) {
  70                 set_bit(can_id, cm->match_sff);
  71                 return;
  72         }
  73
  74         /* All frames */
  75         if (can_mask == 0) {
  76                 bitmap_fill(cm->match_sff, (1 << CAN_SFF_ID_BITS));
  77                 return;
  78         }
  79
  80         /*
  81          * Individual frame filter.
  82          * Add record (set bit to 1) for each ID that
  83          * conforms particular rule
  84          */
  85         for (i = 0; i < (1 << CAN_SFF_ID_BITS); i++) {
  86                 if ((i & can_mask) == can_id)
  87                         set_bit(i, cm->match_sff);
  88         }
  89 }
  90
  91 static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
  92 {
  93         return (struct canid_match *) m->data;
  94 }
  95
  96 static int em_canid_match(struct sk_buff *skb, struct tcf_ematch *m,
  97                          struct tcf_pkt_info *info)
  98 {
  99         struct canid_match *cm = em_canid_priv(m);
 100         canid_t can_id;
 101         unsigned int match = false;
 102         int i;
 103
 104         can_id = em_canid_get_id(skb);
 105
 106         if (can_id & CAN_EFF_FLAG) {
 107                 can_id &= CAN_EFF_MASK;
 108
 109                 for (i = 0; i < cm->eff_rules_count; i++) {
 110                         if (!(((cm->rules_raw[i].can_id ^ can_id) &
 111                             cm->rules_raw[i].can_mask) & CAN_EFF_MASK)) {
 112                                 match = true;
 113                                 break;
 114                         }
 115                 }
 116         } else { /* SFF */
 117                 can_id &= CAN_SFF_MASK;
 118                 match = test_bit(can_id, cm->match_sff);
 119         }
 120
 121         if (match)
 122                 return 1;
 123
 124         return 0;
 125 }
 126
 127 static int em_canid_change(struct tcf_proto *tp, void *data, int len,
 128                           struct tcf_ematch *m)
 129 {
 130         struct can_filter *conf = data; /* Array with rules,
 131                                          * fixed size EM_CAN_RULES_SIZE
 132                                          */
 133         struct canid_match *cm;
 134         struct canid_match *cm_old = (struct canid_match *) m->data;
 135         int err;
 136         int i;
 137         int rulescnt;
 138
 139         if (len < sizeof(struct can_filter))
 140                 return -EINVAL;
 141
 142         rulescnt = len / sizeof(struct can_filter);
 143
 144         err = -ENOBUFS;
 145         cm = kzalloc(sizeof(struct canid_match) + sizeof(struct can_filter) *
 146                 rulescnt, GFP_KERNEL);
 147         if (cm == NULL)
 148                 goto errout;
 149
 150         cm->sff_rules_count = 0;
 151         cm->eff_rules_count = 0;
 152         cm->rules_count = rulescnt;
 153         err = -EINVAL;
 154
 155         /* Be sure to fit into the array */
 156         if (cm->rules_count > EM_CAN_RULES_MAX)
 157                 goto errout_free;
 158
 159         /*
 160          * We need two for() loops for copying rules into
 161          * two contiguous areas in rules_raw
 162          */
 163
 164         /* Process EFF frame rules*/
 165         for (i = 0; i < cm->rules_count; i++) {
 166                 if ((conf[i].can_id & CAN_EFF_FLAG) &&
 167                     (conf[i].can_mask & CAN_EFF_FLAG)) {
 168                         memcpy(cm->rules_raw + cm->eff_rules_count,
 169                                 &conf[i],
 170                                 sizeof(struct can_filter));
 171
 172                         cm->eff_rules_count++;
 173                 } else {
 174                         continue;
 175                 }
 176         }
 177
 178         /* Process SFF frame rules */
 179         for (i = 0; i < cm->rules_count; i++) {
 180                 if ((conf[i].can_id & CAN_EFF_FLAG) &&
 181                     (conf[i].can_mask & CAN_EFF_FLAG)) {
 182                         continue;
 183                 } else {
 184                         memcpy(cm->rules_raw
 185                                 + cm->eff_rules_count
 186                                 + cm->sff_rules_count,
 187                                 &conf[i], sizeof(struct can_filter));
 188
 189                         cm->sff_rules_count++;
 190
 191                         em_canid_sff_match_add(cm,
 192                                 conf[i].can_id, conf[i].can_mask);
 193                 }
 194         }
 195
 196         m->datalen = sizeof(*cm);
 197         m->data = (unsigned long) cm;
 198
 199         if (cm_old != NULL) {
 200                 printk("canid: Configuring an existing ematch!\n");
 201                 kfree(cm_old);
 202         }
 203
 204         return 0;
 205
 206 errout_free:
 207         kfree(cm);
 208 errout:
 209         return err;
 210 }
 211
 212 static void em_canid_destroy(struct tcf_proto *tp, struct tcf_ematch *m)
 213 {
 214         struct canid_match *cm = em_canid_priv(m);
 215
 216         kfree(cm);
 217 }
 218
 219 static int em_canid_dump(struct sk_buff *skb, struct tcf_ematch *m)
 220 {
 221         struct canid_match *cm = em_canid_priv(m);
 222
 223         /*
 224          * When configuring this ematch 'rules_count' is set not to exceed
 225          * 'rules_raw' array size
 226          */
 227         if (nla_put_nohdr(skb, sizeof(cm->rules_raw[0]) * cm->rules_count,
 228             &cm->rules_raw) < 0)
 229                 goto nla_put_failure;
 230
 231         return 0;
 232
 233 nla_put_failure:
 234         return -1;
 235 }
 236
 237 static struct tcf_ematch_ops em_canid_ops = {
 238         .kind     = TCF_EM_CANID,
 239         .change   = em_canid_change,
 240         .match    = em_canid_match,
 241         .destroy  = em_canid_destroy,
 242         .dump     = em_canid_dump,
 243         .owner    = THIS_MODULE,
 244         .link     = LIST_HEAD_INIT(em_canid_ops.link)
 245 };
 246
 247 static int __init init_em_canid(void)
 248 {
 249         return tcf_em_register(&em_canid_ops);
 250 }
 251
 252 static void __exit exit_em_canid(void)
 253 {
 254         tcf_em_unregister(&em_canid_ops);
 255 }
 256
 257 MODULE_LICENSE("GPL");
 258
 259 module_init(init_em_canid);
 260 module_exit(exit_em_canid);
 261
 262 MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);