2 * cls_canprio.c -- Canprio classifier.
3 * Makes decisions accoring to controller area network (CAN) identifiers.
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@kormus.cz>
16 * Founded by: Volkswagen Group Research
18 * Some function descriptions are heavily inspired by article "Linux Network
19 * Traffic Control -- Implementation Overview" by Werner Almesberger
22 * Implementation notes;
23 * parameter of functions named "base" is pointer to some parent element
26 #include <linux/module.h>
27 #include <linux/slab.h>
28 #include <linux/types.h>
29 #include <linux/kernel.h>
30 #include <linux/string.h>
31 #include <linux/errno.h>
32 #include <linux/rtnetlink.h>
33 #include <linux/skbuff.h>
34 #include <net/netlink.h>
35 #include <net/act_api.h>
36 #include <net/pkt_cls.h>
37 #include <linux/can.h>
38 #include <linux/bitmap.h>
39 #include <linux/spinlock.h>
40 #include <linux/rcupdate.h>
41 #include <linux/can.h>
44 /* Definition of Netlink messages */
48 TCA_CANPRIO_RULES, /* Array of can_filter structs; We are able
49 to determine the length after receiving */
50 TCA_CANPRIO_INV_EN, /* enable inverse rules */
53 #define TCA_CANPRIO_MAX (__TCA_CANPRIO_MAX - 1)
55 static const struct nla_policy canprio_policy[TCA_CANPRIO_MAX + 1] = {
56 [TCA_CANPRIO_CLASSID] = { .type = NLA_U32 },
57 //FIXME Be aware of possible problems with 64bit kernel and 32bit userspace etc.
58 [TCA_CANPRIO_RULES] = { /*.len = (sizeof(struct can_filter))*/ }, //FIXME
59 [TCA_CANPRIO_INV_EN] = { .type = NLA_U32 },
62 struct canprio_rules {
63 struct can_filter *rules_raw; /* Raw rules copied from netlink message;
64 Used for sending information to userspace
65 (when 'tc filter show' is invoked) AND
66 when matching EFF frames*/
67 DECLARE_BITMAP(match_sff, CAN_SFF_MASK + 1); /* For each SFF Can ID (11 bit)
68 there is one record in this bitfield */
69 int inv_match_en; /* Inverted match flag */
79 struct list_head flist;
82 struct canprio_filter {
84 struct canprio_rules *rules; /* All rules necessary for classification */
85 struct tcf_result res; /* Class ID (flow id) the instance
86 of a filter is bound to */
87 struct list_head link;
91 * ----------------------------------------------------------------------------
94 static void canprio_sff_match_add(struct canprio_rules *rls, u32 can_id, u32 can_mask)
98 printk("%s() invoked\n", __FUNCTION__);
99 can_mask &= CAN_SFF_MASK;
102 if (can_mask == CAN_SFF_MASK) {
103 set_bit(can_id, rls->match_sff);
106 /* Add record (set bit to 1) for each ID that conforms particular rule */
107 for (i = 0; i <= CAN_SFF_MASK; i++) {
108 if ((i & can_mask) == can_id)
109 set_bit(i, rls->match_sff);
114 * Extracts Can ID ot ouf the sk_buff structure.
116 static canid_t canprio_get_id(struct sk_buff *skb)
118 /* Can ID is inside of data field */
119 struct can_frame *cf = (struct can_frame *)skb->data;
125 * Performs the classification. Iterates over all instances of filter
126 * checking for Can ID match.
128 * @skb: Socket buffer
130 * @res: Is used for setting Class ID as a result of classification
132 * Returns value relevant for policing. Used return values:
133 * TC_POLICE_OK if succesfully classified (without regard to policing rules)
134 * TC_POLICE_UNSPEC if no matching filter was found
136 static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
137 struct tcf_result *res)
139 struct canprio_head *head = (struct canprio_head *)tp->root;
140 struct canprio_filter *f;
141 struct canprio_rules *r;
145 printk(" canprio_classify() invoked\n");
146 can_id = (u32)canprio_get_id(skb);
149 list_for_each_entry(f, &head->flist, link) {
151 r = rcu_dereference(f->rules);
154 if (can_id & CAN_EFF_FLAG) {
155 can_id &= CAN_EFF_MASK;
157 for (i = 0; i < r->eff_rules_count; i++) {
158 if ((r->rules_raw[i].can_id & r->rules_raw[i].can_mask & CAN_EFF_MASK) ==
159 (can_id & r->rules_raw[i].can_mask & CAN_EFF_MASK)) {
166 match = test_bit(can_id, r->match_sff);
169 //if (r->inv_match_en)
174 printk( " canprio_classify() match ok: ID 0x%x\n", can_id);
181 return TC_POLICE_UNSPEC;
185 * Looks up a filter element by its handle and returns the internal
186 * filter ID (i.e. pointer)
188 static unsigned long canprio_get(struct tcf_proto *tp, u32 handle)
190 struct canprio_head *head = (struct canprio_head *)tp->root;
191 struct canprio_filter *f;
193 //printk("canprio_get(%d) invoked\n", handle);
197 //printk("[running for_each_entry]\n");
198 list_for_each_entry(f, &head->flist, link) {
199 //printk("[f->handle = %d]\n", f->handle);
200 if (f->handle == handle) {
201 //printk("found something\n");
202 return (unsigned long) f;
210 * Is invoked when a filter element previously referenced
211 * with get() is no longer used
213 static void canprio_put(struct tcf_proto *tp, unsigned long f)
218 static unsigned int canprio_gen_handle(struct tcf_proto *tp)
220 struct canprio_head *head = (struct canprio_head *)tp->root;
227 if ((head->hgenerator += 0x10000) == 0)
228 head->hgenerator = 0x10000;
230 h = head->hgenerator;
231 //if (canprio_get(tp, h) == 0)
233 tmp = canprio_get(tp, h);
234 //printk("___tried %d result %lu\n", h, tmp);
242 static void canprio_rules_free_rcu(struct rcu_head *rcu)
244 kfree(container_of(rcu, struct canprio_rules, rcu));
247 static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
248 unsigned long base, struct nlattr **tb,
251 struct can_filter *canprio_nl_rules;
252 struct canprio_rules *rules_tmp;
255 printk("%s() invoked\n", __FUNCTION__);
257 rules_tmp = kzalloc(sizeof(*rules_tmp), GFP_KERNEL);
258 if (rules_tmp == NULL)
261 if (tb[TCA_CANPRIO_CLASSID] == NULL) //FIXME is enough?
264 if (tb[TCA_CANPRIO_RULES]) {
265 canprio_nl_rules = nla_data(tb[TCA_CANPRIO_RULES]);
266 rules_tmp->sff_rules_count = 0;
267 rules_tmp->eff_rules_count = 0;
268 rules_tmp->rules_count = (nla_len(tb[TCA_CANPRIO_RULES]) / sizeof(struct can_filter));
269 printk(" rules_count = %u\n", rules_tmp->rules_count);
271 rules_tmp->rules_raw = kzalloc(sizeof(struct can_filter) * rules_tmp->rules_count, GFP_KERNEL);
272 if (rules_tmp->rules_raw == NULL)
275 /* Process EFF frames */
276 for (i = 0; i < rules_tmp->rules_count; i++) {
277 if ((canprio_nl_rules[i].can_id & CAN_EFF_FLAG) &&
278 (canprio_nl_rules[i].can_mask & CAN_EFF_FLAG)) {
280 memcpy(rules_tmp->rules_raw + rules_tmp->eff_rules_count,
281 &canprio_nl_rules[i], sizeof(struct can_filter));
282 rules_tmp->eff_rules_count ++;
284 printk(" can ID to match = 0x%x with mask 0x%x\n",
285 canprio_nl_rules[i].can_id, canprio_nl_rules[i].can_mask);
291 /* Process SFF frames
292 We need two for() loops for copying rules into two contiguous areas in rules_raw */
293 for (i = 0; i < rules_tmp->rules_count; i++) {
294 if ((canprio_nl_rules[i].can_id & CAN_EFF_FLAG) &&
295 (canprio_nl_rules[i].can_mask & CAN_EFF_FLAG)) {
299 memcpy(rules_tmp->rules_raw + rules_tmp->eff_rules_count + rules_tmp->sff_rules_count,
300 &canprio_nl_rules[i], sizeof(struct can_filter));
301 rules_tmp->sff_rules_count ++;
303 canprio_sff_match_add(rules_tmp, canprio_nl_rules[i].can_id,
304 canprio_nl_rules[i].can_mask);
306 printk(" can ID to match = 0x%x with mask 0x%x\n",
307 canprio_nl_rules[i].can_id, canprio_nl_rules[i].can_mask);
313 if (tb[TCA_CANPRIO_INV_EN] != NULL)
314 rules_tmp->inv_match_en = nla_get_u32(tb[TCA_CANPRIO_INV_EN]);
316 if (f->rules == NULL) { // Setting parameters for newly created filter
317 rcu_assign_pointer(f->rules, rules_tmp);
318 } else { // Changing existing filter
319 struct canprio_rules *rules_old;
321 rules_old = xchg(&f->rules, rules_tmp);
322 call_rcu(&rules_old->rcu, canprio_rules_free_rcu);
328 if (rules_tmp->rules_raw != NULL) //FIXME is ok?
329 kfree(rules_tmp->rules_raw);
335 * Called for changing properties of an existing filter or after addition
336 * of a new filter to a class (by calling bind_tcf which binds an instance
337 * of a filter to the class).
339 * @tp: Structure representing instance of a filter.
340 * Part of a linked list of all filters.
343 * @tca: Messages passed through the Netlink from userspace.
346 static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
347 struct nlattr **tca, unsigned long *arg)
349 struct canprio_head *head = (struct canprio_head *)tp->root;
350 struct canprio_filter *f = (struct canprio_filter *)*arg;
351 struct nlattr *tb[TCA_CANPRIO_MAX + 1];
354 printk("%s() invoked\n", __FUNCTION__);
356 if (tca[TCA_OPTIONS] == NULL)
359 /* Parses a stream of attributes and stores a pointer to each attribute in
360 the tb array accessible via the attribute type. Policy may be set to NULL
361 if no validation is required.*/
362 err = nla_parse_nested(tb, TCA_CANPRIO_MAX, tca[TCA_OPTIONS], canprio_policy);
365 /* Change existing filter (remove all settings and add
366 them thereafter as if filter was newly created) */
368 if (handle && f->handle != handle)
371 printk("[change existing filter]\n");
372 return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
375 /* Create new filter */
377 f = kzalloc(sizeof(*f), GFP_KERNEL);
382 if (tb[TCA_CANPRIO_CLASSID]) {
383 f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
384 tcf_bind_filter(tp, &f->res, base);
389 if (handle) /* handle passed from userspace */
393 //handle = canprio_gen_handle(tp);
394 //f->handle = handle;
399 err = canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
403 //Add newly created filter to list of all filters
405 list_add(&f->link, &head->flist);
407 *arg = (unsigned long) f;
413 if (*arg == 0UL && f)
420 static void canprio_delete_filter(struct tcf_proto *tp, struct canprio_filter *f)
422 tcf_unbind_filter(tp, &f->res);
425 kfree(f->rules->rules_raw);
431 * Remove whole filter.
433 static void canprio_destroy(struct tcf_proto *tp)
435 struct canprio_head *head = tp->root;
436 struct canprio_filter *f, *n;
438 list_for_each_entry_safe(f, n, &head->flist, link) {
440 canprio_delete_filter(tp, f);
446 * Delete one instance of a filter.
448 static int canprio_delete(struct tcf_proto *tp, unsigned long arg)
450 struct canprio_head *head = (struct canprio_head *)tp->root;
451 struct canprio_filter *t;
452 struct canprio_filter *f = (struct canprio_filter *)arg;
454 rcu_barrier(); /* Wait for completion of call_rcu()'s */
456 list_for_each_entry(t, &head->flist, link)
461 canprio_delete_filter(tp, t);
472 static int canprio_init(struct tcf_proto *tp)
474 struct canprio_head *head;
475 printk(" canprio_init invoked\n");
477 head = kzalloc(sizeof(*head), GFP_KERNEL);
481 INIT_LIST_HEAD(&head->flist);
483 tp->protocol = htons(ETH_P_CAN); /* Work only on AF_CAN packets - not tested! */
489 * Iterates over all elements of a filter and invokes a callback function
490 * for each of them. This is used to obtain diagnostic data
492 static void canprio_walk(struct tcf_proto *tp, struct tcf_walker *arg)
494 struct canprio_head *head = (struct canprio_head *) tp->root;
495 struct canprio_filter *f;
497 printk("%s() invoked\n", __FUNCTION__);
499 list_for_each_entry(f, &head->flist, link) {
500 if (arg->count < arg->skip)
503 if (arg->fn(tp, (unsigned long) f, arg) < 0) {
513 * Returns diagnostic data for a filter or one of its elements.
515 static int canprio_dump(struct tcf_proto *tp, unsigned long fh,
516 struct sk_buff *skb, struct tcmsg *t)
518 struct canprio_filter *f = (struct canprio_filter *) fh;
520 struct canprio_rules *r;
521 printk("%s() invoked\n", __FUNCTION__);
527 r = rcu_dereference(f->rules);
528 t->tcm_handle = f->handle;
530 nest = nla_nest_start(skb, TCA_OPTIONS);
532 goto nla_put_failure;
535 NLA_PUT_U32(skb, TCA_CANPRIO_CLASSID, f->res.classid);
537 NLA_PUT(skb, TCA_CANPRIO_RULES, r->rules_count *
538 sizeof(struct can_filter), r->rules_raw);
541 nla_nest_end(skb, nest);
547 nla_nest_cancel(skb, nest);
553 static struct tcf_proto_ops cls_canprio_ops __read_mostly = {
555 .classify = canprio_classify,
556 .init = canprio_init,
557 .destroy = canprio_destroy,
560 .change = canprio_change,
561 .delete = canprio_delete,
562 .walk = canprio_walk,
563 .dump = canprio_dump,
564 .owner = THIS_MODULE,
567 static int __init init_canprio(void)
569 printk("Canprio loaded\n");
570 return register_tcf_proto_ops(&cls_canprio_ops);
573 static void __exit exit_canprio(void)
575 printk("Canprio removed\n");
576 unregister_tcf_proto_ops(&cls_canprio_ops);
579 module_init(init_canprio);
580 module_exit(exit_canprio);
581 MODULE_LICENSE("GPL");
582 MODULE_AUTHOR(""); // FIXME
583 MODULE_DESCRIPTION("Controller Area Network can_id classifier");