2 * cls_can.c -- Controller Area Network classifier.
3 * Makes decisions according to Controller Area Network identifiers (can_id).
5 * This program is free software; you can distribute it and/or
6 * modify it under the terms of the GNU General Public License
7 * as published by the Free Software Foundation; version 2 of
10 * Idea: Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
11 * Copyright: (c) 2011 Czech Technical University in Prague
12 * (c) 2011 Volkswagen Group Research
13 * Authors: Michal Sojka <sojkam1@fel.cvut.cz>
14 * Pavel Pisa <pisa@cmp.felk.cvut.cz>
15 * Rostislav Lisovy <lisovy@gmail.cz>
16 * Funded by: Volkswagen Group Research
18 * Some function descriptions are heavily inspired by article "Linux Network
19 * Traffic Control -- Implementation Overview" by Werner Almesberger
22 #include <linux/module.h>
23 #include <linux/slab.h>
24 #include <linux/types.h>
25 #include <linux/kernel.h>
26 #include <linux/string.h>
27 #include <linux/errno.h>
28 #include <linux/rtnetlink.h>
29 #include <linux/skbuff.h>
30 #include <net/netlink.h>
31 #include <net/act_api.h>
32 #include <net/pkt_cls.h>
33 #include <linux/bitmap.h>
34 #include <linux/spinlock.h>
35 #include <linux/rcupdate.h>
36 #include <linux/can.h>
38 /* Definition of Netlink message parts */
42 TCA_CANFLTR_RULES, /* Array of can_filter structs; We are able
43 to determine the length after receiving */
46 #define TCA_CANFLTR_MAX (__TCA_CANFLTR_MAX - 1)
48 static const struct nla_policy canfltr_policy[TCA_CANFLTR_MAX + 1] = {
49 [TCA_CANFLTR_CLASSID] = { .type = NLA_U32 }, /* Be aware of possible
50 problems with 64bit kernel and
51 32bit userspace etc. */
52 [TCA_CANFLTR_RULES] = { .type = NLA_NESTED }
55 struct canfltr_rules {
56 struct can_filter *rules_raw; /* Raw rules copied from netlink
57 message; Used for sending information
58 to userspace (when 'tc filter show' is
59 invoked) AND when matching EFF frames*/
60 DECLARE_BITMAP(match_sff, (1 << CAN_SFF_ID_BITS)); /* For each SFF CAN
61 ID (11 bit) there is one record in this
72 struct list_head flist;
75 struct canfltr_state {
77 struct canfltr_rules *rules; /* All rules necessary for
79 struct tcf_result res; /* Class ID (flow id) the instance
80 of a filter is bound to */
81 struct list_head link;
85 * ----------------------------------------------------------------------------
88 static void canfltr_sff_match_add(struct canfltr_rules *rls,
89 u32 can_id, u32 can_mask)
93 /* Limit can_mask and can_id to SFF range to
94 protect against write after end of array */
95 can_mask &= CAN_SFF_MASK;
99 if (can_mask == CAN_SFF_MASK) {
100 set_bit(can_id, rls->match_sff);
106 bitmap_fill(rls->match_sff, (1 << CAN_SFF_ID_BITS));
110 /* individual frame filter */
111 /* Add record (set bit to 1) for each ID that
112 conforms particular rule */
113 for (i = 0; i < (1 << CAN_SFF_ID_BITS); i++) {
114 if ((i & can_mask) == can_id)
115 set_bit(i, rls->match_sff);
120 * canfltr_get_id() - Extracts Can ID out of the sk_buff structure.
122 static canid_t canfltr_get_id(struct sk_buff *skb)
124 /* Can ID is inside of data field */
125 struct can_frame *cf = (struct can_frame *)skb->data;
131 * canfltr_classify() - Performs the classification.
133 * @skb: Socket buffer
135 * @res: Is used for setting Class ID as a result of classification
137 * Iterates over all instances of filter, checking for CAN ID match.
139 * Returns value relevant for policing. Used return values:
140 * TC_POLICE_OK if succesfully classified (without regard to policing rules)
141 * TC_POLICE_UNSPEC if no matching rule was found
143 static int canfltr_classify(struct sk_buff *skb, const struct tcf_proto *tp,
144 struct tcf_result *res)
146 struct canfltr_head *head = (struct canfltr_head *)tp->root;
147 struct canfltr_state *f;
148 struct canfltr_rules *r;
152 can_id = canfltr_get_id(skb);
155 list_for_each_entry(f, &head->flist, link) {
157 r = rcu_dereference(f->rules);
160 if (can_id & CAN_EFF_FLAG) {
161 can_id &= CAN_EFF_MASK;
163 for (i = 0; i < r->eff_rules_count; i++) {
164 if (!(((r->rules_raw[i].can_id ^ can_id) &
165 r->rules_raw[i].can_mask) & CAN_EFF_MASK)) {
171 can_id &= CAN_SFF_MASK;
172 match = test_bit(can_id, r->match_sff);
183 return TC_POLICE_UNSPEC;
187 * canfltr_get() - Looks up a filter element by its handle and returns the
188 * internal filter ID (i.e. pointer)
190 static unsigned long canfltr_get(struct tcf_proto *tp, u32 handle)
192 struct canfltr_head *head = (struct canfltr_head *)tp->root;
193 struct canfltr_state *f;
198 list_for_each_entry(f, &head->flist, link) {
199 if (f->handle == handle)
200 return (unsigned long) f;
207 * canfltr_put() - Is invoked when a filter element previously referenced
208 * with get() is no longer used
210 static void canfltr_put(struct tcf_proto *tp, unsigned long f)
215 * canfltr_gen_handle() - Generate handle for newly created filter
217 * This code is heavily inspired by handle generator in cls_basic.c
219 static unsigned int canfltr_gen_handle(struct tcf_proto *tp)
221 struct canfltr_head *head = (struct canfltr_head *)tp->root;
222 unsigned int i = 0x80000000;
225 if (++head->hgenerator == 0x7FFFFFFF)
226 head->hgenerator = 1;
227 } while (--i > 0 && canfltr_get(tp, head->hgenerator));
232 return head->hgenerator;
235 static void canfltr_rules_free_rcu(struct rcu_head *rcu)
237 kfree(container_of(rcu, struct canfltr_rules, rcu));
240 static int canfltr_set_parms(struct tcf_proto *tp, struct canfltr_state *f,
241 unsigned long base, struct nlattr **tb,
244 struct can_filter *canfltr_nl_rules;
245 struct canfltr_rules *rules_tmp;
249 rules_tmp = kzalloc(sizeof(*rules_tmp), GFP_KERNEL);
254 if (tb[TCA_CANFLTR_CLASSID] == NULL)
257 if (tb[TCA_CANFLTR_RULES]) {
258 canfltr_nl_rules = nla_data(tb[TCA_CANFLTR_RULES]);
259 rules_tmp->sff_rules_count = 0;
260 rules_tmp->eff_rules_count = 0;
261 rules_tmp->rules_count = (nla_len(tb[TCA_CANFLTR_RULES]) /
262 sizeof(struct can_filter));
264 rules_tmp->rules_raw = kzalloc(sizeof(struct can_filter) *
265 rules_tmp->rules_count, GFP_KERNEL);
267 if (rules_tmp->rules_raw == NULL)
270 /* We need two for() loops for copying rules into
271 two contiguous areas in rules_raw */
273 /* Process EFF frame rules*/
274 for (i = 0; i < rules_tmp->rules_count; i++) {
275 if ((canfltr_nl_rules[i].can_id & CAN_EFF_FLAG) &&
276 (canfltr_nl_rules[i].can_mask & CAN_EFF_FLAG)) {
277 memcpy(rules_tmp->rules_raw +
278 rules_tmp->eff_rules_count,
279 &canfltr_nl_rules[i],
280 sizeof(struct can_filter));
281 rules_tmp->eff_rules_count++;
287 /* Process SFF frame rules */
288 for (i = 0; i < rules_tmp->rules_count; i++) {
289 if ((canfltr_nl_rules[i].can_id & CAN_EFF_FLAG) &&
290 (canfltr_nl_rules[i].can_mask & CAN_EFF_FLAG)) {
293 memcpy(rules_tmp->rules_raw +
294 rules_tmp->eff_rules_count +
295 rules_tmp->sff_rules_count,
296 &canfltr_nl_rules[i],
297 sizeof(struct can_filter));
298 rules_tmp->sff_rules_count++;
299 canfltr_sff_match_add(rules_tmp,
300 canfltr_nl_rules[i].can_id,
301 canfltr_nl_rules[i].can_mask);
307 /* Setting parameters for newly created filter */
308 if (f->rules == NULL) {
309 rcu_assign_pointer(f->rules, rules_tmp);
310 } else { /* Changing existing filter */
311 struct canfltr_rules *rules_old;
313 rules_old = xchg(&f->rules, rules_tmp);
314 call_rcu(&rules_old->rcu, canfltr_rules_free_rcu);
325 * canfltr_change() - Called for changing properties of an existing filter or
326 * after addition of a new filter to a class (by calling bind_tcf which binds
327 * an instance of a filter to the class).
329 * @tp: Structure representing instance of a filter.
330 * Part of a linked list of all filters.
333 * @tca: Messages passed through the Netlink from userspace.
336 static int canfltr_change(struct tcf_proto *tp, unsigned long base, u32 handle,
337 struct nlattr **tca, unsigned long *arg)
339 struct canfltr_head *head = (struct canfltr_head *)tp->root;
340 struct canfltr_state *f = (struct canfltr_state *)*arg;
341 struct nlattr *tb[TCA_CANFLTR_MAX + 1];
344 if (tca[TCA_OPTIONS] == NULL)
347 /* Parses a stream of attributes and stores a pointer to each
348 attribute in the tb array accessible via the attribute type.
349 Policy may be set to NULL if no validation is required.*/
350 err = nla_parse_nested(tb, TCA_CANFLTR_MAX, tca[TCA_OPTIONS],
354 /* Change existing filter (remove all settings and add
355 them thereafter as if filter was newly created) */
357 if (handle && f->handle != handle)
360 return canfltr_set_parms(tp, f, base, tb, tca[TCA_RATE]);
363 /* Create new filter */
365 f = kzalloc(sizeof(*f), GFP_KERNEL);
369 if (tb[TCA_CANFLTR_CLASSID]) {
370 f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
371 tcf_bind_filter(tp, &f->res, base);
375 if (handle) /* handle passed from userspace */
378 f->handle = canfltr_gen_handle(tp);
383 /* Configure filter */
384 err = canfltr_set_parms(tp, f, base, tb, tca[TCA_RATE]);
388 /* Add newly created filter to list of all filters */
390 list_add(&f->link, &head->flist);
392 *arg = (unsigned long) f;
397 if (*arg == 0UL && f)
404 static void canfltr_delete_filter(struct tcf_proto *tp,
405 struct canfltr_state *f)
407 tcf_unbind_filter(tp, &f->res);
410 kfree(f->rules->rules_raw);
416 * canfltr_destroy() - Remove whole filter.
418 static void canfltr_destroy(struct tcf_proto *tp)
420 struct canfltr_head *head = tp->root;
421 struct canfltr_state *f, *n;
423 list_for_each_entry_safe(f, n, &head->flist, link) {
425 canfltr_delete_filter(tp, f);
431 * canfltr_delete() - Delete one instance of a filter.
433 static int canfltr_delete(struct tcf_proto *tp, unsigned long arg)
435 struct canfltr_head *head = (struct canfltr_head *)tp->root;
436 struct canfltr_state *t;
437 struct canfltr_state *f = (struct canfltr_state *)arg;
439 rcu_barrier(); /* Wait for completion of call_rcu()'s */
441 list_for_each_entry(t, &head->flist, link)
446 canfltr_delete_filter(tp, t);
455 * canfltr_init() - Initialize filter
457 static int canfltr_init(struct tcf_proto *tp)
459 struct canfltr_head *head;
461 if ((tp->protocol != htons(ETH_P_ALL)) &&
462 (tp->protocol != htons(ETH_P_CAN)))
465 /* Work only on CAN frames */
466 if (tp->protocol == htons(ETH_P_ALL))
467 tp->protocol = htons(ETH_P_CAN);
469 head = kzalloc(sizeof(*head), GFP_KERNEL);
473 INIT_LIST_HEAD(&head->flist);
480 * canfltr_walk() - Iterates over all elements of a filter and invokes a
481 * callback function for each of them. This is used to obtain diagnostic data.
483 static void canfltr_walk(struct tcf_proto *tp, struct tcf_walker *arg)
485 struct canfltr_head *head = (struct canfltr_head *) tp->root;
486 struct canfltr_state *f;
488 list_for_each_entry(f, &head->flist, link) {
489 if (arg->count < arg->skip)
492 if (arg->fn(tp, (unsigned long) f, arg) < 0) {
502 * canfltr_dump() - Returns diagnostic data for a filter or one of its elements.
504 static int canfltr_dump(struct tcf_proto *tp, unsigned long fh,
505 struct sk_buff *skb, struct tcmsg *t)
507 struct canfltr_state *f = (struct canfltr_state *) fh;
509 struct canfltr_rules *r;
515 r = rcu_dereference(f->rules);
516 t->tcm_handle = f->handle;
518 nest = nla_nest_start(skb, TCA_OPTIONS);
520 goto nla_put_failure;
523 NLA_PUT_U32(skb, TCA_CANFLTR_CLASSID, f->res.classid);
525 NLA_PUT(skb, TCA_CANFLTR_RULES, r->rules_count *
526 sizeof(struct can_filter), r->rules_raw);
529 nla_nest_end(skb, nest);
535 nla_nest_cancel(skb, nest);
541 static struct tcf_proto_ops cls_canfltr_ops __read_mostly = {
543 .classify = canfltr_classify,
544 .init = canfltr_init,
545 .destroy = canfltr_destroy,
548 .change = canfltr_change,
549 .delete = canfltr_delete,
550 .walk = canfltr_walk,
551 .dump = canfltr_dump,
552 .owner = THIS_MODULE,
555 static int __init init_canfltr(void)
557 pr_debug("canfltr: CAN filter loaded\n");
558 return register_tcf_proto_ops(&cls_canfltr_ops);
561 static void __exit exit_canfltr(void)
563 pr_debug("canfltr: CAN filter removed\n");
564 unregister_tcf_proto_ops(&cls_canfltr_ops);
567 module_init(init_canfltr);
568 module_exit(exit_canfltr);
569 MODULE_LICENSE("GPL");
570 MODULE_AUTHOR("Rostislav Lisovy <lisovy@gmail.cz>");
571 MODULE_DESCRIPTION("Controller Area Network classifier");