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
25struct 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 */
45static 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
53static 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
88static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
89{
90	return (struct canid_match *)m->data;
91}
92
93static 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
120static 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
183static void em_canid_destroy(struct tcf_ematch *m)
184{
185	struct canid_match *cm = em_canid_priv(m);
186
187	kfree(cm);
188}
189
190static 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
205static 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
215static int __init init_em_canid(void)
216{
217	return tcf_em_register(&em_canid_ops);
218}
219
220static void __exit exit_em_canid(void)
221{
222	tcf_em_unregister(&em_canid_ops);
223}
224
225MODULE_DESCRIPTION("ematch classifier to match CAN IDs embedded in skb CAN frames");
226MODULE_LICENSE("GPL");
227
228module_init(init_em_canid);
229module_exit(exit_em_canid);
230
231MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);
232