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_DESCRIPTION("ematch classifier to match CAN IDs embedded in skb CAN frames"); 226 MODULE_LICENSE("GPL"); 227 228 module_init(init_em_canid); 229 module_exit(exit_em_canid); 230 231 MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID); 232