1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
|
/*
* em_canid.c Ematch rule to match CAN frames according to their CAN IDs
*
* This program is free software; you can distribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version
* 2 of the License, or (at your option) any later version.
*
* Idea: Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
* Copyright: (c) 2011 Czech Technical University in Prague
* (c) 2011 Volkswagen Group Research
* Authors: Michal Sojka <sojkam1@fel.cvut.cz>
* Pavel Pisa <pisa@cmp.felk.cvut.cz>
* Rostislav Lisovy <lisovy@gmail.cz>
* Funded by: Volkswagen Group Research
*/
#include <linux/slab.h>
#include <linux/module.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/string.h>
#include <linux/skbuff.h>
#include <net/pkt_cls.h>
#include <linux/can.h>
#define EM_CAN_RULES_MAX 500
struct canid_match {
/* For each SFF CAN ID (11 bit) there is one record in this bitfield */
DECLARE_BITMAP(match_sff, (1 << CAN_SFF_ID_BITS));
int rules_count;
int sff_rules_count;
int eff_rules_count;
/*
* Raw rules copied from netlink message; Used for sending
* information to userspace (when 'tc filter show' is invoked)
* AND when matching EFF frames
*/
struct can_filter rules_raw[];
};
/**
* em_canid_get_id() - Extracts Can ID out of the sk_buff structure.
*/
static canid_t em_canid_get_id(struct sk_buff *skb)
{
/* CAN ID is stored within the data field */
struct can_frame *cf = (struct can_frame *)skb->data;
return cf->can_id;
}
static void em_canid_sff_match_add(struct canid_match *cm, u32 can_id,
u32 can_mask)
{
int i;
/*
* Limit can_mask and can_id to SFF range to
* protect against write after end of array
*/
can_mask &= CAN_SFF_MASK;
can_id &= can_mask;
/* Single frame */
if (can_mask == CAN_SFF_MASK) {
set_bit(can_id, cm->match_sff);
return;
}
/* All frames */
if (can_mask == 0) {
bitmap_fill(cm->match_sff, (1 << CAN_SFF_ID_BITS));
return;
}
/*
* Individual frame filter.
* Add record (set bit to 1) for each ID that
* conforms particular rule
*/
for (i = 0; i < (1 << CAN_SFF_ID_BITS); i++) {
if ((i & can_mask) == can_id)
set_bit(i, cm->match_sff);
}
}
static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
{
return (struct canid_match *)m->data;
}
static int em_canid_match(struct sk_buff *skb, struct tcf_ematch *m,
struct tcf_pkt_info *info)
{
struct canid_match *cm = em_canid_priv(m);
canid_t can_id;
int match = 0;
int i;
const struct can_filter *lp;
can_id = em_canid_get_id(skb);
if (can_id & CAN_EFF_FLAG) {
for (i = 0, lp = cm->rules_raw;
i < cm->eff_rules_count; i++, lp++) {
if (!(((lp->can_id ^ can_id) & lp->can_mask))) {
match = 1;
break;
}
}
} else { /* SFF */
can_id &= CAN_SFF_MASK;
match = (test_bit(can_id, cm->match_sff) ? 1 : 0);
}
return match;
}
static int em_canid_change(struct tcf_proto *tp, void *data, int len,
struct tcf_ematch *m)
{
struct can_filter *conf = data; /* Array with rules */
struct canid_match *cm;
struct canid_match *cm_old = (struct canid_match *)m->data;
int i;
if (!len)
return -EINVAL;
if (len % sizeof(struct can_filter))
return -EINVAL;
if (len > sizeof(struct can_filter) * EM_CAN_RULES_MAX)
return -EINVAL;
cm = kzalloc(sizeof(struct canid_match) + len, GFP_KERNEL);
if (!cm)
return -ENOMEM;
cm->rules_count = len / sizeof(struct can_filter);
/*
* We need two for() loops for copying rules into two contiguous
* areas in rules_raw to process all eff rules with a simple loop.
* NB: The configuration interface supports sff and eff rules.
* We do not support filters here that match for the same can_id
* provided in a SFF and EFF frame (e.g. 0x123 / 0x80000123).
* For this (unusual case) two filters have to be specified. The
* SFF/EFF separation is done with the CAN_EFF_FLAG in the can_id.
*/
/* Fill rules_raw with EFF rules first */
for (i = 0; i < cm->rules_count; i++) {
if (conf[i].can_id & CAN_EFF_FLAG) {
memcpy(cm->rules_raw + cm->eff_rules_count,
&conf[i],
sizeof(struct can_filter));
cm->eff_rules_count++;
}
}
/* append SFF frame rules */
for (i = 0; i < cm->rules_count; i++) {
if (!(conf[i].can_id & CAN_EFF_FLAG)) {
memcpy(cm->rules_raw
+ cm->eff_rules_count
+ cm->sff_rules_count,
&conf[i], sizeof(struct can_filter));
cm->sff_rules_count++;
em_canid_sff_match_add(cm,
conf[i].can_id, conf[i].can_mask);
}
}
m->datalen = sizeof(struct canid_match) + len;
m->data = (unsigned long)cm;
if (cm_old != NULL) {
pr_err("canid: Configuring an existing ematch!\n");
kfree(cm_old);
}
return 0;
}
static void em_canid_destroy(struct tcf_proto *tp, struct tcf_ematch *m)
{
struct canid_match *cm = em_canid_priv(m);
kfree(cm);
}
static int em_canid_dump(struct sk_buff *skb, struct tcf_ematch *m)
{
struct canid_match *cm = em_canid_priv(m);
/*
* When configuring this ematch 'rules_count' is set not to exceed
* 'rules_raw' array size
*/
if (nla_put_nohdr(skb, sizeof(struct can_filter) * cm->rules_count,
&cm->rules_raw) < 0)
return -EMSGSIZE;
return 0;
}
static struct tcf_ematch_ops em_canid_ops = {
.kind = TCF_EM_CANID,
.change = em_canid_change,
.match = em_canid_match,
.destroy = em_canid_destroy,
.dump = em_canid_dump,
.owner = THIS_MODULE,
.link = LIST_HEAD_INIT(em_canid_ops.link)
};
static int __init init_em_canid(void)
{
return tcf_em_register(&em_canid_ops);
}
static void __exit exit_em_canid(void)
{
tcf_em_unregister(&em_canid_ops);
}
MODULE_LICENSE("GPL");
module_init(init_em_canid);
module_exit(exit_em_canid);
MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);
|
