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 */
em_canid_get_id(struct sk_buff * skb)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
em_canid_sff_match_add(struct canid_match * cm,u32 can_id,u32 can_mask)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
em_canid_priv(struct tcf_ematch * m)88 static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
89 {
90 return (struct canid_match *)m->data;
91 }
92
em_canid_match(struct sk_buff * skb,struct tcf_ematch * m,struct tcf_pkt_info * info)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 if (!pskb_may_pull(skb, CAN_MTU))
103 return 0;
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
em_canid_change(struct net * net,void * data,int len,struct tcf_ematch * m)123 static int em_canid_change(struct net *net, 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 int i;
129
130 if (!len)
131 return -EINVAL;
132
133 if (len % sizeof(struct can_filter))
134 return -EINVAL;
135
136 if (len > sizeof(struct can_filter) * EM_CAN_RULES_MAX)
137 return -EINVAL;
138
139 cm = kzalloc(sizeof(struct canid_match) + len, GFP_KERNEL);
140 if (!cm)
141 return -ENOMEM;
142
143 cm->rules_count = len / sizeof(struct can_filter);
144
145 /*
146 * We need two for() loops for copying rules into two contiguous
147 * areas in rules_raw to process all eff rules with a simple loop.
148 * NB: The configuration interface supports sff and eff rules.
149 * We do not support filters here that match for the same can_id
150 * provided in a SFF and EFF frame (e.g. 0x123 / 0x80000123).
151 * For this (unusual case) two filters have to be specified. The
152 * SFF/EFF separation is done with the CAN_EFF_FLAG in the can_id.
153 */
154
155 /* Fill rules_raw with EFF rules first */
156 for (i = 0; i < cm->rules_count; i++) {
157 if (conf[i].can_id & CAN_EFF_FLAG) {
158 memcpy(cm->rules_raw + cm->eff_rules_count,
159 &conf[i],
160 sizeof(struct can_filter));
161
162 cm->eff_rules_count++;
163 }
164 }
165
166 /* append SFF frame rules */
167 for (i = 0; i < cm->rules_count; i++) {
168 if (!(conf[i].can_id & CAN_EFF_FLAG)) {
169 memcpy(cm->rules_raw
170 + cm->eff_rules_count
171 + cm->sff_rules_count,
172 &conf[i], sizeof(struct can_filter));
173
174 cm->sff_rules_count++;
175
176 em_canid_sff_match_add(cm,
177 conf[i].can_id, conf[i].can_mask);
178 }
179 }
180
181 m->datalen = sizeof(struct canid_match) + len;
182 m->data = (unsigned long)cm;
183 return 0;
184 }
185
em_canid_destroy(struct tcf_ematch * m)186 static void em_canid_destroy(struct tcf_ematch *m)
187 {
188 struct canid_match *cm = em_canid_priv(m);
189
190 kfree(cm);
191 }
192
em_canid_dump(struct sk_buff * skb,struct tcf_ematch * m)193 static int em_canid_dump(struct sk_buff *skb, struct tcf_ematch *m)
194 {
195 struct canid_match *cm = em_canid_priv(m);
196
197 /*
198 * When configuring this ematch 'rules_count' is set not to exceed
199 * 'rules_raw' array size
200 */
201 if (nla_put_nohdr(skb, sizeof(struct can_filter) * cm->rules_count,
202 &cm->rules_raw) < 0)
203 return -EMSGSIZE;
204
205 return 0;
206 }
207
208 static struct tcf_ematch_ops em_canid_ops = {
209 .kind = TCF_EM_CANID,
210 .change = em_canid_change,
211 .match = em_canid_match,
212 .destroy = em_canid_destroy,
213 .dump = em_canid_dump,
214 .owner = THIS_MODULE,
215 .link = LIST_HEAD_INIT(em_canid_ops.link)
216 };
217
init_em_canid(void)218 static int __init init_em_canid(void)
219 {
220 return tcf_em_register(&em_canid_ops);
221 }
222
exit_em_canid(void)223 static void __exit exit_em_canid(void)
224 {
225 tcf_em_unregister(&em_canid_ops);
226 }
227
228 MODULE_DESCRIPTION("ematch classifier to match CAN IDs embedded in skb CAN frames");
229 MODULE_LICENSE("GPL");
230
231 module_init(init_em_canid);
232 module_exit(exit_em_canid);
233
234 MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);
235