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 	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 
em_canid_change(struct net * net,void * data,int len,struct tcf_ematch * m)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 
em_canid_destroy(struct tcf_ematch * m)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 
em_canid_dump(struct sk_buff * skb,struct tcf_ematch * m)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 
init_em_canid(void)215 static int __init init_em_canid(void)
216 {
217 	return tcf_em_register(&em_canid_ops);
218 }
219 
exit_em_canid(void)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