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 int match = 0; 102 int i; 103 const struct can_filter *lp; 104 105 can_id = em_canid_get_id(skb); 106 107 if (can_id & CAN_EFF_FLAG) { 108 for (i = 0, lp = cm->rules_raw; 109 i < cm->eff_rules_count; i++, lp++) { 110 if (!(((lp->can_id ^ can_id) & lp->can_mask))) { 111 match = 1; 112 break; 113 } 114 } 115 } else { /* SFF */ 116 can_id &= CAN_SFF_MASK; 117 match = (test_bit(can_id, cm->match_sff) ? 1 : 0); 118 } 119 120 return match; 121 } 122 123 static int em_canid_change(struct tcf_proto *tp, void *data, int len, 124 struct tcf_ematch *m) 125 { 126 struct can_filter *conf = data; /* Array with rules */ 127 struct canid_match *cm; 128 struct canid_match *cm_old = (struct canid_match *)m->data; 129 int i; 130 131 if (!len) 132 return -EINVAL; 133 134 if (len % sizeof(struct can_filter)) 135 return -EINVAL; 136 137 if (len > sizeof(struct can_filter) * EM_CAN_RULES_MAX) 138 return -EINVAL; 139 140 cm = kzalloc(sizeof(struct canid_match) + len, GFP_KERNEL); 141 if (!cm) 142 return -ENOMEM; 143 144 cm->rules_count = len / sizeof(struct can_filter); 145 146 /* 147 * We need two for() loops for copying rules into two contiguous 148 * areas in rules_raw to process all eff rules with a simple loop. 149 * NB: The configuration interface supports sff and eff rules. 150 * We do not support filters here that match for the same can_id 151 * provided in a SFF and EFF frame (e.g. 0x123 / 0x80000123). 152 * For this (unusual case) two filters have to be specified. The 153 * SFF/EFF separation is done with the CAN_EFF_FLAG in the can_id. 154 */ 155 156 /* Fill rules_raw with EFF rules first */ 157 for (i = 0; i < cm->rules_count; i++) { 158 if (conf[i].can_id & CAN_EFF_FLAG) { 159 memcpy(cm->rules_raw + cm->eff_rules_count, 160 &conf[i], 161 sizeof(struct can_filter)); 162 163 cm->eff_rules_count++; 164 } 165 } 166 167 /* append SFF frame rules */ 168 for (i = 0; i < cm->rules_count; i++) { 169 if (!(conf[i].can_id & CAN_EFF_FLAG)) { 170 memcpy(cm->rules_raw 171 + cm->eff_rules_count 172 + cm->sff_rules_count, 173 &conf[i], sizeof(struct can_filter)); 174 175 cm->sff_rules_count++; 176 177 em_canid_sff_match_add(cm, 178 conf[i].can_id, conf[i].can_mask); 179 } 180 } 181 182 m->datalen = sizeof(struct canid_match) + len; 183 m->data = (unsigned long)cm; 184 185 if (cm_old != NULL) { 186 pr_err("canid: Configuring an existing ematch!\n"); 187 kfree(cm_old); 188 } 189 190 return 0; 191 } 192 193 static void em_canid_destroy(struct tcf_proto *tp, struct tcf_ematch *m) 194 { 195 struct canid_match *cm = em_canid_priv(m); 196 197 kfree(cm); 198 } 199 200 static int em_canid_dump(struct sk_buff *skb, struct tcf_ematch *m) 201 { 202 struct canid_match *cm = em_canid_priv(m); 203 204 /* 205 * When configuring this ematch 'rules_count' is set not to exceed 206 * 'rules_raw' array size 207 */ 208 if (nla_put_nohdr(skb, sizeof(struct can_filter) * cm->rules_count, 209 &cm->rules_raw) < 0) 210 return -EMSGSIZE; 211 212 return 0; 213 } 214 215 static struct tcf_ematch_ops em_canid_ops = { 216 .kind = TCF_EM_CANID, 217 .change = em_canid_change, 218 .match = em_canid_match, 219 .destroy = em_canid_destroy, 220 .dump = em_canid_dump, 221 .owner = THIS_MODULE, 222 .link = LIST_HEAD_INIT(em_canid_ops.link) 223 }; 224 225 static int __init init_em_canid(void) 226 { 227 return tcf_em_register(&em_canid_ops); 228 } 229 230 static void __exit exit_em_canid(void) 231 { 232 tcf_em_unregister(&em_canid_ops); 233 } 234 235 MODULE_LICENSE("GPL"); 236 237 module_init(init_em_canid); 238 module_exit(exit_em_canid); 239 240 MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID); 241