net/sched/em_canid.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * em_canid.c  Ematch rule to match CAN frames according to their CAN IDs
   4  *
   5  * Idea:       Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
   6  * Copyright:  (c) 2011 Czech Technical University in Prague
   7  *             (c) 2011 Volkswagen Group Research
   8  * Authors:    Michal Sojka <sojkam1@fel.cvut.cz>
   9  *             Pavel Pisa <pisa@cmp.felk.cvut.cz>
  10  *             Rostislav Lisovy <lisovy@gmail.cz>
  11  * Funded by:  Volkswagen Group Research
  12  */
  13
  14 #include <linux/slab.h>
  15 #include <linux/module.h>
  16 #include <linux/types.h>
  17 #include <linux/kernel.h>
  18 #include <linux/string.h>
  19 #include <linux/skbuff.h>
  20 #include <net/pkt_cls.h>
  21 #include <linux/can.h>
  22
  23 #define EM_CAN_RULES_MAX 500
  24
  25 struct canid_match {
  26         /* For each SFF CAN ID (11 bit) there is one record in this bitfield */
  27         DECLARE_BITMAP(match_sff, (1 << CAN_SFF_ID_BITS));
  28
  29         int rules_count;
  30         int sff_rules_count;
  31         int eff_rules_count;
  32
  33         /*
  34          * Raw rules copied from netlink message; Used for sending
  35          * information to userspace (when 'tc filter show' is invoked)
  36          * AND when matching EFF frames
  37          */
  38         struct can_filter rules_raw[];
  39 };
  40
  41 /**
  42  * em_canid_get_id() - Extracts Can ID out of the sk_buff structure.
  43  * @skb: buffer to extract Can ID from
  44  */
  45 static canid_t em_canid_get_id(struct sk_buff *skb)
  46 {
  47         /* CAN ID is stored within the data field */
  48         struct can_frame *cf = (struct can_frame *)skb->data;
  49
  50         return cf->can_id;
  51 }
  52
  53 static void em_canid_sff_match_add(struct canid_match *cm, u32 can_id,
  54                                         u32 can_mask)
  55 {
  56         int i;
  57
  58         /*
  59          * Limit can_mask and can_id to SFF range to
  60          * protect against write after end of array
  61          */
  62         can_mask &= CAN_SFF_MASK;
  63         can_id &= can_mask;
  64
  65         /* Single frame */
  66         if (can_mask == CAN_SFF_MASK) {
  67                 set_bit(can_id, cm->match_sff);
  68                 return;
  69         }
  70
  71         /* All frames */
  72         if (can_mask == 0) {
  73                 bitmap_fill(cm->match_sff, (1 << CAN_SFF_ID_BITS));
  74                 return;
  75         }
  76
  77         /*
  78          * Individual frame filter.
  79          * Add record (set bit to 1) for each ID that
  80          * conforms particular rule
  81          */
  82         for (i = 0; i < (1 << CAN_SFF_ID_BITS); i++) {
  83                 if ((i & can_mask) == can_id)
  84                         set_bit(i, cm->match_sff);
  85         }
  86 }
  87
  88 static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
  89 {
  90         return (struct canid_match *)m->data;
  91 }
  92
  93 static int em_canid_match(struct sk_buff *skb, struct tcf_ematch *m,
  94                          struct tcf_pkt_info *info)
  95 {
  96         struct canid_match *cm = em_canid_priv(m);
  97         canid_t can_id;
  98         int match = 0;
  99         int i;
 100         const struct can_filter *lp;
 101
 102         can_id = em_canid_get_id(skb);
 103
 104         if (can_id & CAN_EFF_FLAG) {
 105                 for (i = 0, lp = cm->rules_raw;
 106                      i < cm->eff_rules_count; i++, lp++) {
 107                         if (!(((lp->can_id ^ can_id) & lp->can_mask))) {
 108                                 match = 1;
 109                                 break;
 110                         }
 111                 }
 112         } else { /* SFF */
 113                 can_id &= CAN_SFF_MASK;
 114                 match = (test_bit(can_id, cm->match_sff) ? 1 : 0);
 115         }
 116
 117         return match;
 118 }
 119
 120 static int em_canid_change(struct net *net, void *data, int len,
 121                           struct tcf_ematch *m)
 122 {
 123         struct can_filter *conf = data; /* Array with rules */
 124         struct canid_match *cm;
 125         int i;
 126
 127         if (!len)
 128                 return -EINVAL;
 129
 130         if (len % sizeof(struct can_filter))
 131                 return -EINVAL;
 132
 133         if (len > sizeof(struct can_filter) * EM_CAN_RULES_MAX)
 134                 return -EINVAL;
 135
 136         cm = kzalloc(sizeof(struct canid_match) + len, GFP_KERNEL);
 137         if (!cm)
 138                 return -ENOMEM;
 139
 140         cm->rules_count = len / sizeof(struct can_filter);
 141
 142         /*
 143          * We need two for() loops for copying rules into two contiguous
 144          * areas in rules_raw to process all eff rules with a simple loop.
 145          * NB: The configuration interface supports sff and eff rules.
 146          * We do not support filters here that match for the same can_id
 147          * provided in a SFF and EFF frame (e.g. 0x123 / 0x80000123).
 148          * For this (unusual case) two filters have to be specified. The
 149          * SFF/EFF separation is done with the CAN_EFF_FLAG in the can_id.
 150          */
 151
 152         /* Fill rules_raw with EFF rules first */
 153         for (i = 0; i < cm->rules_count; i++) {
 154                 if (conf[i].can_id & CAN_EFF_FLAG) {
 155                         memcpy(cm->rules_raw + cm->eff_rules_count,
 156                                 &conf[i],
 157                                 sizeof(struct can_filter));
 158
 159                         cm->eff_rules_count++;
 160                 }
 161         }
 162
 163         /* append SFF frame rules */
 164         for (i = 0; i < cm->rules_count; i++) {
 165                 if (!(conf[i].can_id & CAN_EFF_FLAG)) {
 166                         memcpy(cm->rules_raw
 167                                 + cm->eff_rules_count
 168                                 + cm->sff_rules_count,
 169                                 &conf[i], sizeof(struct can_filter));
 170
 171                         cm->sff_rules_count++;
 172
 173                         em_canid_sff_match_add(cm,
 174                                 conf[i].can_id, conf[i].can_mask);
 175                 }
 176         }
 177
 178         m->datalen = sizeof(struct canid_match) + len;
 179         m->data = (unsigned long)cm;
 180         return 0;
 181 }
 182
 183 static void em_canid_destroy(struct tcf_ematch *m)
 184 {
 185         struct canid_match *cm = em_canid_priv(m);
 186
 187         kfree(cm);
 188 }
 189
 190 static int em_canid_dump(struct sk_buff *skb, struct tcf_ematch *m)
 191 {
 192         struct canid_match *cm = em_canid_priv(m);
 193
 194         /*
 195          * When configuring this ematch 'rules_count' is set not to exceed
 196          * 'rules_raw' array size
 197          */
 198         if (nla_put_nohdr(skb, sizeof(struct can_filter) * cm->rules_count,
 199             &cm->rules_raw) < 0)
 200                 return -EMSGSIZE;
 201
 202         return 0;
 203 }
 204
 205 static struct tcf_ematch_ops em_canid_ops = {
 206         .kind     = TCF_EM_CANID,
 207         .change   = em_canid_change,
 208         .match    = em_canid_match,
 209         .destroy  = em_canid_destroy,
 210         .dump     = em_canid_dump,
 211         .owner    = THIS_MODULE,
 212         .link     = LIST_HEAD_INIT(em_canid_ops.link)
 213 };
 214
 215 static int __init init_em_canid(void)
 216 {
 217         return tcf_em_register(&em_canid_ops);
 218 }
 219
 220 static void __exit exit_em_canid(void)
 221 {
 222         tcf_em_unregister(&em_canid_ops);
 223 }
 224
 225 MODULE_LICENSE("GPL");
 226
 227 module_init(init_em_canid);
 228 module_exit(exit_em_canid);
 229
 230 MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);