net/sched/em_canid.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * em_canid.c  Ematch rule to match CAN frames according to their CAN IDs
0004  *
0005  * Idea:       Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
0006  * Copyright:  (c) 2011 Czech Technical University in Prague
0007  *             (c) 2011 Volkswagen Group Research
0008  * Authors:    Michal Sojka <sojkam1@fel.cvut.cz>
0009  *             Pavel Pisa <pisa@cmp.felk.cvut.cz>
0010  *             Rostislav Lisovy <lisovy@gmail.cz>
0011  * Funded by:  Volkswagen Group Research
0012  */
0013
0014 #include <linux/slab.h>
0015 #include <linux/module.h>
0016 #include <linux/types.h>
0017 #include <linux/kernel.h>
0018 #include <linux/string.h>
0019 #include <linux/skbuff.h>
0020 #include <net/pkt_cls.h>
0021 #include <linux/can.h>
0022
0023 #define EM_CAN_RULES_MAX 500
0024
0025 struct canid_match {
0026     /* For each SFF CAN ID (11 bit) there is one record in this bitfield */
0027     DECLARE_BITMAP(match_sff, (1 << CAN_SFF_ID_BITS));
0028
0029     int rules_count;
0030     int sff_rules_count;
0031     int eff_rules_count;
0032
0033     /*
0034      * Raw rules copied from netlink message; Used for sending
0035      * information to userspace (when 'tc filter show' is invoked)
0036      * AND when matching EFF frames
0037      */
0038     struct can_filter rules_raw[];
0039 };
0040
0041 /**
0042  * em_canid_get_id() - Extracts Can ID out of the sk_buff structure.
0043  * @skb: buffer to extract Can ID from
0044  */
0045 static canid_t em_canid_get_id(struct sk_buff *skb)
0046 {
0047     /* CAN ID is stored within the data field */
0048     struct can_frame *cf = (struct can_frame *)skb->data;
0049
0050     return cf->can_id;
0051 }
0052
0053 static void em_canid_sff_match_add(struct canid_match *cm, u32 can_id,
0054                     u32 can_mask)
0055 {
0056     int i;
0057
0058     /*
0059      * Limit can_mask and can_id to SFF range to
0060      * protect against write after end of array
0061      */
0062     can_mask &= CAN_SFF_MASK;
0063     can_id &= can_mask;
0064
0065     /* Single frame */
0066     if (can_mask == CAN_SFF_MASK) {
0067         set_bit(can_id, cm->match_sff);
0068         return;
0069     }
0070
0071     /* All frames */
0072     if (can_mask == 0) {
0073         bitmap_fill(cm->match_sff, (1 << CAN_SFF_ID_BITS));
0074         return;
0075     }
0076
0077     /*
0078      * Individual frame filter.
0079      * Add record (set bit to 1) for each ID that
0080      * conforms particular rule
0081      */
0082     for (i = 0; i < (1 << CAN_SFF_ID_BITS); i++) {
0083         if ((i & can_mask) == can_id)
0084             set_bit(i, cm->match_sff);
0085     }
0086 }
0087
0088 static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
0089 {
0090     return (struct canid_match *)m->data;
0091 }
0092
0093 static int em_canid_match(struct sk_buff *skb, struct tcf_ematch *m,
0094              struct tcf_pkt_info *info)
0095 {
0096     struct canid_match *cm = em_canid_priv(m);
0097     canid_t can_id;
0098     int match = 0;
0099     int i;
0100     const struct can_filter *lp;
0101
0102     can_id = em_canid_get_id(skb);
0103
0104     if (can_id & CAN_EFF_FLAG) {
0105         for (i = 0, lp = cm->rules_raw;
0106              i < cm->eff_rules_count; i++, lp++) {
0107             if (!(((lp->can_id ^ can_id) & lp->can_mask))) {
0108                 match = 1;
0109                 break;
0110             }
0111         }
0112     } else { /* SFF */
0113         can_id &= CAN_SFF_MASK;
0114         match = (test_bit(can_id, cm->match_sff) ? 1 : 0);
0115     }
0116
0117     return match;
0118 }
0119
0120 static int em_canid_change(struct net *net, void *data, int len,
0121               struct tcf_ematch *m)
0122 {
0123     struct can_filter *conf = data; /* Array with rules */
0124     struct canid_match *cm;
0125     int i;
0126
0127     if (!len)
0128         return -EINVAL;
0129
0130     if (len % sizeof(struct can_filter))
0131         return -EINVAL;
0132
0133     if (len > sizeof(struct can_filter) * EM_CAN_RULES_MAX)
0134         return -EINVAL;
0135
0136     cm = kzalloc(sizeof(struct canid_match) + len, GFP_KERNEL);
0137     if (!cm)
0138         return -ENOMEM;
0139
0140     cm->rules_count = len / sizeof(struct can_filter);
0141
0142     /*
0143      * We need two for() loops for copying rules into two contiguous
0144      * areas in rules_raw to process all eff rules with a simple loop.
0145      * NB: The configuration interface supports sff and eff rules.
0146      * We do not support filters here that match for the same can_id
0147      * provided in a SFF and EFF frame (e.g. 0x123 / 0x80000123).
0148      * For this (unusual case) two filters have to be specified. The
0149      * SFF/EFF separation is done with the CAN_EFF_FLAG in the can_id.
0150      */
0151
0152     /* Fill rules_raw with EFF rules first */
0153     for (i = 0; i < cm->rules_count; i++) {
0154         if (conf[i].can_id & CAN_EFF_FLAG) {
0155             memcpy(cm->rules_raw + cm->eff_rules_count,
0156                 &conf[i],
0157                 sizeof(struct can_filter));
0158
0159             cm->eff_rules_count++;
0160         }
0161     }
0162
0163     /* append SFF frame rules */
0164     for (i = 0; i < cm->rules_count; i++) {
0165         if (!(conf[i].can_id & CAN_EFF_FLAG)) {
0166             memcpy(cm->rules_raw
0167                 + cm->eff_rules_count
0168                 + cm->sff_rules_count,
0169                 &conf[i], sizeof(struct can_filter));
0170
0171             cm->sff_rules_count++;
0172
0173             em_canid_sff_match_add(cm,
0174                 conf[i].can_id, conf[i].can_mask);
0175         }
0176     }
0177
0178     m->datalen = sizeof(struct canid_match) + len;
0179     m->data = (unsigned long)cm;
0180     return 0;
0181 }
0182
0183 static void em_canid_destroy(struct tcf_ematch *m)
0184 {
0185     struct canid_match *cm = em_canid_priv(m);
0186
0187     kfree(cm);
0188 }
0189
0190 static int em_canid_dump(struct sk_buff *skb, struct tcf_ematch *m)
0191 {
0192     struct canid_match *cm = em_canid_priv(m);
0193
0194     /*
0195      * When configuring this ematch 'rules_count' is set not to exceed
0196      * 'rules_raw' array size
0197      */
0198     if (nla_put_nohdr(skb, sizeof(struct can_filter) * cm->rules_count,
0199         &cm->rules_raw) < 0)
0200         return -EMSGSIZE;
0201
0202     return 0;
0203 }
0204
0205 static struct tcf_ematch_ops em_canid_ops = {
0206     .kind     = TCF_EM_CANID,
0207     .change   = em_canid_change,
0208     .match    = em_canid_match,
0209     .destroy  = em_canid_destroy,
0210     .dump     = em_canid_dump,
0211     .owner    = THIS_MODULE,
0212     .link     = LIST_HEAD_INIT(em_canid_ops.link)
0213 };
0214
0215 static int __init init_em_canid(void)
0216 {
0217     return tcf_em_register(&em_canid_ops);
0218 }
0219
0220 static void __exit exit_em_canid(void)
0221 {
0222     tcf_em_unregister(&em_canid_ops);
0223 }
0224
0225 MODULE_LICENSE("GPL");
0226
0227 module_init(init_em_canid);
0228 module_exit(exit_em_canid);
0229
0230 MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);