Loading...
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 */
44static canid_t em_canid_get_id(struct sk_buff *skb)
45{
46 /* CAN ID is stored within the data field */
47 struct can_frame *cf = (struct can_frame *)skb->data;
48
49 return cf->can_id;
50}
51
52static void em_canid_sff_match_add(struct canid_match *cm, u32 can_id,
53 u32 can_mask)
54{
55 int i;
56
57 /*
58 * Limit can_mask and can_id to SFF range to
59 * protect against write after end of array
60 */
61 can_mask &= CAN_SFF_MASK;
62 can_id &= can_mask;
63
64 /* Single frame */
65 if (can_mask == CAN_SFF_MASK) {
66 set_bit(can_id, cm->match_sff);
67 return;
68 }
69
70 /* All frames */
71 if (can_mask == 0) {
72 bitmap_fill(cm->match_sff, (1 << CAN_SFF_ID_BITS));
73 return;
74 }
75
76 /*
77 * Individual frame filter.
78 * Add record (set bit to 1) for each ID that
79 * conforms particular rule
80 */
81 for (i = 0; i < (1 << CAN_SFF_ID_BITS); i++) {
82 if ((i & can_mask) == can_id)
83 set_bit(i, cm->match_sff);
84 }
85}
86
87static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
88{
89 return (struct canid_match *)m->data;
90}
91
92static int em_canid_match(struct sk_buff *skb, struct tcf_ematch *m,
93 struct tcf_pkt_info *info)
94{
95 struct canid_match *cm = em_canid_priv(m);
96 canid_t can_id;
97 int match = 0;
98 int i;
99 const struct can_filter *lp;
100
101 can_id = em_canid_get_id(skb);
102
103 if (can_id & CAN_EFF_FLAG) {
104 for (i = 0, lp = cm->rules_raw;
105 i < cm->eff_rules_count; i++, lp++) {
106 if (!(((lp->can_id ^ can_id) & lp->can_mask))) {
107 match = 1;
108 break;
109 }
110 }
111 } else { /* SFF */
112 can_id &= CAN_SFF_MASK;
113 match = (test_bit(can_id, cm->match_sff) ? 1 : 0);
114 }
115
116 return match;
117}
118
119static int em_canid_change(struct net *net, void *data, int len,
120 struct tcf_ematch *m)
121{
122 struct can_filter *conf = data; /* Array with rules */
123 struct canid_match *cm;
124 int i;
125
126 if (!len)
127 return -EINVAL;
128
129 if (len % sizeof(struct can_filter))
130 return -EINVAL;
131
132 if (len > sizeof(struct can_filter) * EM_CAN_RULES_MAX)
133 return -EINVAL;
134
135 cm = kzalloc(sizeof(struct canid_match) + len, GFP_KERNEL);
136 if (!cm)
137 return -ENOMEM;
138
139 cm->rules_count = len / sizeof(struct can_filter);
140
141 /*
142 * We need two for() loops for copying rules into two contiguous
143 * areas in rules_raw to process all eff rules with a simple loop.
144 * NB: The configuration interface supports sff and eff rules.
145 * We do not support filters here that match for the same can_id
146 * provided in a SFF and EFF frame (e.g. 0x123 / 0x80000123).
147 * For this (unusual case) two filters have to be specified. The
148 * SFF/EFF separation is done with the CAN_EFF_FLAG in the can_id.
149 */
150
151 /* Fill rules_raw with EFF rules first */
152 for (i = 0; i < cm->rules_count; i++) {
153 if (conf[i].can_id & CAN_EFF_FLAG) {
154 memcpy(cm->rules_raw + cm->eff_rules_count,
155 &conf[i],
156 sizeof(struct can_filter));
157
158 cm->eff_rules_count++;
159 }
160 }
161
162 /* append SFF frame rules */
163 for (i = 0; i < cm->rules_count; i++) {
164 if (!(conf[i].can_id & CAN_EFF_FLAG)) {
165 memcpy(cm->rules_raw
166 + cm->eff_rules_count
167 + cm->sff_rules_count,
168 &conf[i], sizeof(struct can_filter));
169
170 cm->sff_rules_count++;
171
172 em_canid_sff_match_add(cm,
173 conf[i].can_id, conf[i].can_mask);
174 }
175 }
176
177 m->datalen = sizeof(struct canid_match) + len;
178 m->data = (unsigned long)cm;
179 return 0;
180}
181
182static void em_canid_destroy(struct tcf_ematch *m)
183{
184 struct canid_match *cm = em_canid_priv(m);
185
186 kfree(cm);
187}
188
189static int em_canid_dump(struct sk_buff *skb, struct tcf_ematch *m)
190{
191 struct canid_match *cm = em_canid_priv(m);
192
193 /*
194 * When configuring this ematch 'rules_count' is set not to exceed
195 * 'rules_raw' array size
196 */
197 if (nla_put_nohdr(skb, sizeof(struct can_filter) * cm->rules_count,
198 &cm->rules_raw) < 0)
199 return -EMSGSIZE;
200
201 return 0;
202}
203
204static struct tcf_ematch_ops em_canid_ops = {
205 .kind = TCF_EM_CANID,
206 .change = em_canid_change,
207 .match = em_canid_match,
208 .destroy = em_canid_destroy,
209 .dump = em_canid_dump,
210 .owner = THIS_MODULE,
211 .link = LIST_HEAD_INIT(em_canid_ops.link)
212};
213
214static int __init init_em_canid(void)
215{
216 return tcf_em_register(&em_canid_ops);
217}
218
219static void __exit exit_em_canid(void)
220{
221 tcf_em_unregister(&em_canid_ops);
222}
223
224MODULE_LICENSE("GPL");
225
226module_init(init_em_canid);
227module_exit(exit_em_canid);
228
229MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);
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
29struct 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 */
48static 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
56static 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
91static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
92{
93 return (struct canid_match *)m->data;
94}
95
96static 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
123static 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
193static 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
200static 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
215static 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
225static int __init init_em_canid(void)
226{
227 return tcf_em_register(&em_canid_ops);
228}
229
230static void __exit exit_em_canid(void)
231{
232 tcf_em_unregister(&em_canid_ops);
233}
234
235MODULE_LICENSE("GPL");
236
237module_init(init_em_canid);
238module_exit(exit_em_canid);
239
240MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);