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(nr: can_id, addr: cm->match_sff); |
68 | return; |
69 | } |
70 | |
71 | /* All frames */ |
72 | if (can_mask == 0) { |
73 | bitmap_fill(dst: cm->match_sff, nbits: (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(nr: i, addr: 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(size: 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 | can_id: conf[i].can_id, can_mask: 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(objp: 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, attrlen: sizeof(struct can_filter) * cm->rules_count, |
199 | data: &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); |
231 | |