-/* cls_canprio.c -- Canprio classifier.
- * Makes decisions accoring to Can IDs.
+/*
+ * cls_canprio.c -- Canprio classifier.
+ * Makes decisions accoring to controller area network (CAN) identifiers.
+ *
+ * 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; version 2 of
+ * the License.
+ *
+ * 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@kormus.cz>
+ * Founded by: Volkswagen Group Research
+ *
+ * Some function descriptions are heavily inspired by article "Linux Network
+ * Traffic Control -- Implementation Overview" by Werner Almesberger
+ *
*
* Implementation notes;
* parameter of functions named "base" is pointer to some parent element
- *
*/
#include <linux/module.h>
#include <net/netlink.h>
#include <net/act_api.h>
#include <net/pkt_cls.h>
-#include <linux/can.h>
#include <linux/bitmap.h>
+#include <linux/spinlock.h>
+#include <linux/rcupdate.h>
+#include <linux/can.h>
-//--------------------------------------
-//FIXME put in some *.h file shared with userspace "tc"?
-struct canprio_rule {
- __u32 canid;
- __u32 canid_mask;
-};
-
-struct canprio_rule *canprio_rules;
-int canprio_rules_count = 0;
-int canprio_inv_en;
-//--------------------------------------
+#define SFF_BITMAP 1
+#undef SFF_BITMAP
/* Definition of Netlink messages */
enum {
TCA_CANPRIO_A_UNSPEC,
TCA_CANPRIO_CLASSID,
- TCA_CANPRIO_RULES,
+ TCA_CANPRIO_RULES, /* Array of can_filter structs; We are able
+ to determine the length after receiving */
+ TCA_CANPRIO_INV_EN, /* enable inverse rules */
__TCA_CANPRIO_MAX,
};
#define TCA_CANPRIO_MAX (__TCA_CANPRIO_MAX - 1)
static const struct nla_policy canprio_policy[TCA_CANPRIO_MAX + 1] = {
[TCA_CANPRIO_CLASSID] = { .type = NLA_U32 },
//FIXME Be aware of possible problems with 64bit kernel and 32bit userspace etc.
- [TCA_CANPRIO_RULES] = { .len = sizeof(canprio_rules) },
+ [TCA_CANPRIO_RULES] = { /*.len = (sizeof(struct can_filter))*/ }, //FIXME
+ [TCA_CANPRIO_INV_EN] = { .type = NLA_U32 },
+};
+
+struct canprio_rules {
+ struct can_filter *rules_raw; /* Raw rules copied from netlink message;
+ Used for sending information to userspace
+ (when 'tc filter show' is invoked) AND
+ when matching EFF frames*/
+#ifdef SFF_BITMAP
+ DECLARE_BITMAP(match_sff, CAN_SFF_MASK + 1); /* For each SFF Can ID (11 bit)
+ there is one record in this bitfield */
+#endif
+ int inv_match_en; /* Inverted match flag */
+ int rules_count;
+ int eff_rules_count;
+ int sff_rules_count;
+
+ struct rcu_head rcu;
};
struct canprio_head {
- u32 hgenerator;
- struct list_head flist;
+ u32 hgenerator;
+ struct list_head flist;
};
struct canprio_filter {
- u32 handle;
- // For each SFF Can ID (11 bit) there is one record in this bitfield
- DECLARE_BITMAP(match, CAN_SFF_MASK + 1);
- struct tcf_result res;
- struct list_head link;
+ u32 handle;
+ struct canprio_rules *rules; /* All rules necessary for classification */
+ struct tcf_result res; /* Class ID (flow id) the instance
+ of a filter is bound to */
+ struct list_head link;
};
/*
* ----------------------------------------------------------------------------
*/
-static void sff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
+#ifdef SFF_BITMAP
+static void canprio_sff_match_add(struct canprio_rules *rls, u32 can_id, u32 can_mask)
{
int i;
- canid = canid & mask;
- if (mask >= CAN_SFF_MASK) {
- set_bit(canid, f->match);
+ pr_debug("%s() invoked\n", __FUNCTION__);
+ can_mask &= CAN_SFF_MASK;
+ can_id &= can_mask;
+
+ if (can_mask == CAN_SFF_MASK) {
+ set_bit(can_id, rls->match_sff);
return;
}
-
+ /* Add record (set bit to 1) for each ID that conforms particular rule */
for (i = 0; i <= CAN_SFF_MASK; i++) {
- if ((i & mask) == canid)
- set_bit(i, f->match);
+ if ((i & can_mask) == can_id)
+ set_bit(i, rls->match_sff);
}
}
-
-static void eff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
-{
-
-
-}
+#endif
/*
* Extracts Can ID ot ouf the sk_buff structure.
*/
-static u32 canprio_get_id(struct sk_buff *skb)
+static canid_t canprio_get_id(struct sk_buff *skb)
{
/* Can ID is inside of data field */
struct can_frame *cf = (struct can_frame *)skb->data;
- //canid_t can_id = cf->can_id;
- return (u32)cf->can_id;
+ return cf->can_id;
}
/*
{
struct canprio_head *head = (struct canprio_head *)tp->root;
struct canprio_filter *f;
- u32 canid;
+ struct canprio_rules *r;
+ u32 can_id;
+ int i;
- printk(" canprio_classify() invoked\n");
+ pr_debug(" canprio_classify() invoked\n");
+ can_id = (u32)canprio_get_id(skb);
+ rcu_read_lock();
list_for_each_entry(f, &head->flist, link) {
- canid = canprio_get_id(skb);
- printk(" canprio_classify() can ID of received frame = 0x%x\n", canid);
-
- if (canid & CAN_EFF_FLAG) {
-
+ bool match = false;
+ r = rcu_dereference(f->rules);
+
+
+ if (can_id & CAN_EFF_FLAG) {
+ can_id &= CAN_EFF_MASK;
+
+ for (i = 0; i < r->eff_rules_count; i++) {
+ if ((r->rules_raw[i].can_id & r->rules_raw[i].can_mask & CAN_EFF_MASK) ==
+ (can_id & r->rules_raw[i].can_mask & CAN_EFF_MASK)) {
+
+ match = true;
+ break;
+ }
+ }
} else {
- if(test_bit(canid, f->match)) {
- printk( " canprio_classify() match ok: ID 0x%x\n", canid);
- *res = f->res;
- return TC_POLICE_OK;
+ can_id &= CAN_SFF_MASK;
+#ifdef SFF_BITMAP
+ match = test_bit(can_id, r->match_sff);
+#else
+ for (i = r->eff_rules_count; i < r->eff_rules_count + r->sff_rules_count; i++) {
+ if ((r->rules_raw[i].can_id & r->rules_raw[i].can_mask & CAN_SFF_MASK) ==
+ (can_id & r->rules_raw[i].can_mask & CAN_SFF_MASK)) {
+
+ match = true;
+ break;
+ }
}
+#endif
}
- }
-
- return TC_POLICE_UNSPEC;
-}
-
-/*
- * Configure filter
- */
-static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
- unsigned long base, struct nlattr **tb,
- struct nlattr *est)
-{
- //int err;
- printk(" canprio_set_parms invoked\n");
- if (tb[TCA_CANPRIO_CLASSID]) {
- f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
- bitmap_zero(f->match, CAN_SFF_MASK + 1);
+ //if (r->inv_match_en)
+ // match = !match;
- tcf_bind_filter(tp, &f->res, base);
+ if (match) {
+ *res = f->res;
+ pr_debug( " canprio_classify() match ok: ID 0x%x\n", can_id);
+ rcu_read_unlock();
+ return TC_POLICE_OK;
+ }
}
- return 0;
-}
+ rcu_read_unlock();
+ return TC_POLICE_UNSPEC;
+}
/*
- * Looks up a filter element by its handle and returns the internal filter ID
+ * Looks up a filter element by its handle and returns the internal
+ * filter ID (i.e. pointer)
*/
static unsigned long canprio_get(struct tcf_proto *tp, u32 handle)
{
- unsigned long l = 0UL;
- struct canprio_head *head = (struct canprio_head *) tp->root;
+ struct canprio_head *head = (struct canprio_head *)tp->root;
struct canprio_filter *f;
if (head == NULL)
return 0UL;
- list_for_each_entry(f, &head->flist, link)
- if (f->handle == handle)
- l = (unsigned long) f;
+ list_for_each_entry(f, &head->flist, link) {
+ if (f->handle == handle) {
+ return (unsigned long) f;
+ }
+ }
- return l;
+ return 0UL;
}
/*
}
+static unsigned int canprio_gen_handle(struct tcf_proto *tp)
+{
+ struct canprio_head *head = (struct canprio_head *)tp->root;
+ int i = 0xFFFF;
+
+ while (i-- > 0) {
+ u32 h;
+
+ if ((head->hgenerator += 0x10000) == 0)
+ head->hgenerator = 0x10000;
+
+ h = head->hgenerator;
+ if (canprio_get(tp, h) == 0);
+ return h;
+ }
+ return 0;
+}
+
+
+static void canprio_rules_free_rcu(struct rcu_head *rcu)
+{
+ kfree(container_of(rcu, struct canprio_rules, rcu));
+}
+
+static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
+ unsigned long base, struct nlattr **tb,
+ struct nlattr *est)
+{
+ struct can_filter *canprio_nl_rules;
+ struct canprio_rules *rules_tmp;
+ int err;
+ int i;
+ pr_debug("%s() invoked\n", __FUNCTION__);
+
+ rules_tmp = kzalloc(sizeof(*rules_tmp), GFP_KERNEL);
+ if (rules_tmp == NULL)
+ return -ENOBUFS;
+
+ err = -EINVAL;
+ if (tb[TCA_CANPRIO_CLASSID] == NULL)
+ goto errout;
+
+ if (tb[TCA_CANPRIO_RULES]) {
+ canprio_nl_rules = nla_data(tb[TCA_CANPRIO_RULES]);
+ rules_tmp->sff_rules_count = 0;
+ rules_tmp->eff_rules_count = 0;
+ rules_tmp->rules_count = (nla_len(tb[TCA_CANPRIO_RULES]) / sizeof(struct can_filter));
+ pr_debug(" rules_count = %u\n", rules_tmp->rules_count);
+
+ rules_tmp->rules_raw = kzalloc(sizeof(struct can_filter) * rules_tmp->rules_count, GFP_KERNEL);
+ err = -ENOMEM;
+ if (rules_tmp->rules_raw == NULL)
+ goto errout;
+
+ /* Process EFF frames */
+ for (i = 0; i < rules_tmp->rules_count; i++) {
+ if ((canprio_nl_rules[i].can_id & CAN_EFF_FLAG) &&
+ (canprio_nl_rules[i].can_mask & CAN_EFF_FLAG)) {
+
+ memcpy(rules_tmp->rules_raw + rules_tmp->eff_rules_count,
+ &canprio_nl_rules[i], sizeof(struct can_filter));
+ rules_tmp->eff_rules_count ++;
+
+ pr_debug(" can ID to match = 0x%x with mask 0x%x\n",
+ canprio_nl_rules[i].can_id, canprio_nl_rules[i].can_mask);
+ } else {
+ continue;
+ }
+ }
+
+ /* Process SFF frames
+ We need two for() loops for copying rules into two contiguous areas in rules_raw */
+ for (i = 0; i < rules_tmp->rules_count; i++) {
+ if ((canprio_nl_rules[i].can_id & CAN_EFF_FLAG) &&
+ (canprio_nl_rules[i].can_mask & CAN_EFF_FLAG)) {
+
+ continue;
+ } else {
+ memcpy(rules_tmp->rules_raw + rules_tmp->eff_rules_count + rules_tmp->sff_rules_count,
+ &canprio_nl_rules[i], sizeof(struct can_filter));
+ rules_tmp->sff_rules_count ++;
+
+#ifdef SFF_BITMAP
+ canprio_sff_match_add(rules_tmp, canprio_nl_rules[i].can_id,
+ canprio_nl_rules[i].can_mask);
+#endif
+
+ pr_debug(" can ID to match = 0x%x with mask 0x%x\n",
+ canprio_nl_rules[i].can_id, canprio_nl_rules[i].can_mask);
+ }
+ }
+ }
+
+
+ if (tb[TCA_CANPRIO_INV_EN] != NULL)
+ rules_tmp->inv_match_en = nla_get_u32(tb[TCA_CANPRIO_INV_EN]);
+
+ if (f->rules == NULL) { // Setting parameters for newly created filter
+ rcu_assign_pointer(f->rules, rules_tmp);
+ } else { // Changing existing filter
+ struct canprio_rules *rules_old;
+
+ rules_old = xchg(&f->rules, rules_tmp);
+ call_rcu(&rules_old->rcu, canprio_rules_free_rcu);
+ }
+
+ return 0;
+
+errout:
+ kfree(rules_tmp);
+ return err;
+}
+
/*
* Called for changing properties of an existing filter or after addition
* of a new filter to a class (by calling bind_tcf which binds an instance
* @tp: Structure representing instance of a filter.
* Part of a linked list of all filters.
* @base:
- * @handle:
+ * @handle:
* @tca: Messages passed through the Netlink from userspace.
* @arg: ??? FIXME
*/
static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
struct nlattr **tca, unsigned long *arg)
{
- int err;
struct canprio_head *head = (struct canprio_head *)tp->root;
struct canprio_filter *f = (struct canprio_filter *)*arg;
struct nlattr *tb[TCA_CANPRIO_MAX + 1];
- int i;
+ int err;
- printk(" canprio_change invoked\n");
+ pr_debug("%s() invoked\n", __FUNCTION__);
if (tca[TCA_OPTIONS] == NULL)
return -EINVAL;
- //Parses a stream of attributes and stores a pointer to each attribute in
- //the tb array accessible via the attribute type. Policy may be set to NULL
- //if no validation is required.
+ /* Parses a stream of attributes and stores a pointer to each attribute in
+ the tb array accessible via the attribute type. Policy may be set to NULL
+ if no validation is required.*/
err = nla_parse_nested(tb, TCA_CANPRIO_MAX, tca[TCA_OPTIONS], canprio_policy);
if (err < 0)
return err;
-
- //Change existing filter (?)
+ /* Change existing filter (remove all settings and add
+ them thereafter as if filter was newly created) */
if (f != NULL) {
if (handle && f->handle != handle)
return -EINVAL;
+
+ pr_debug("[change existing filter]\n");
return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
}
- //Create new filter (?)
+ /* Create new filter */
err = -ENOBUFS;
f = kzalloc(sizeof(*f), GFP_KERNEL);
if (f == NULL)
goto errout;
+ if (tb[TCA_CANPRIO_CLASSID]) {
+ f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
+ tcf_bind_filter(tp, &f->res, base);
+ }
- //FIXME why?
err = -EINVAL;
- if (handle)
+ if (handle) /* handle passed from userspace */
f->handle = handle;
else {
- //FIXME wat?
- unsigned int i = 0x80000000;
- do {
- if (++head->hgenerator == 0x7FFFFFFF)
- head->hgenerator = 1;
- } while (--i > 0 && canprio_get(tp, head->hgenerator));
-
- if (i <= 0) {
- pr_err("Insufficient number of handles\n");
- goto errout;
- }
-
- f->handle = head->hgenerator;
+ f->handle = canprio_gen_handle(tp);
}
-
- //Configure newly created filter
+ //Configure filter
err = canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
if (err < 0)
goto errout;
-
- //Parse arguments
- if (tb[TCA_CANPRIO_RULES] == NULL)
- return -EINVAL;
-
- canprio_rules = nla_data(tb[TCA_CANPRIO_RULES]);
- canprio_rules_count = (nla_len(tb[TCA_CANPRIO_RULES]) / sizeof(struct canprio_rule));
- printk(" rules_count = %u\n", canprio_rules_count);
-
- for (i = 0; i < canprio_rules_count; i++) {
- if (canprio_rules[i].canid & CAN_EFF_FLAG) {
- eff_match_add(f, canprio_rules[i].canid, canprio_rules[i].canid_mask);
- } else {
- sff_match_add(f, canprio_rules[i].canid, canprio_rules[i].canid_mask);
- }
-
- printk(" can ID to match = 0x%x with mask 0x%x\n",
- canprio_rules[i].canid, canprio_rules[i].canid_mask);
- }
-
//Add newly created filter to list of all filters
tcf_tree_lock(tp);
list_add(&f->link, &head->flist);
return 0;
errout:
- if (*arg == 0UL && f) //FIXME why 0UL?
+ if (*arg == 0UL && f)
kfree(f);
return err;
static void canprio_delete_filter(struct tcf_proto *tp, struct canprio_filter *f)
{
- tcf_unbind_filter(tp, &f->res);
- //tcf_exts_destroy(tp, &f->exts);
- //tcf_em_tree_destroy(tp, &f->ematches);
+ tcf_unbind_filter(tp, &f->res);
+
+ rcu_barrier();
+ kfree(f->rules->rules_raw);
+ kfree(f->rules);
kfree(f);
}
struct canprio_filter *t;
struct canprio_filter *f = (struct canprio_filter *)arg;
+ rcu_barrier(); /* Wait for completion of call_rcu()'s */
+
list_for_each_entry(t, &head->flist, link)
if (t == f) {
tcf_tree_lock(tp);
return -ENOENT;
}
-//FIXME copied from cls_basic, not sure if ok;
+
+/*
+ * Initialize filter
+ */
static int canprio_init(struct tcf_proto *tp)
{
struct canprio_head *head;
-
- printk(" canprio_init invoked\n");
+ pr_debug(" canprio_init invoked\n");
head = kzalloc(sizeof(*head), GFP_KERNEL);
if (head == NULL)
return -ENOBUFS;
+
INIT_LIST_HEAD(&head->flist);
tp->root = head;
+ tp->protocol = htons(ETH_P_CAN); /* Work only on AF_CAN packets - not tested! */
return 0;
}
+/*
+ * Iterates over all elements of a filter and invokes a callback function
+ * for each of them. This is used to obtain diagnostic data
+ */
+static void canprio_walk(struct tcf_proto *tp, struct tcf_walker *arg)
+{
+ struct canprio_head *head = (struct canprio_head *) tp->root;
+ struct canprio_filter *f;
+
+ pr_debug("%s() invoked\n", __FUNCTION__);
+
+ list_for_each_entry(f, &head->flist, link) {
+ if (arg->count < arg->skip)
+ goto skip;
+
+ if (arg->fn(tp, (unsigned long) f, arg) < 0) {
+ arg->stop = 1;
+ break;
+ }
+skip:
+ arg->count++;
+ }
+}
+
+/*
+ * Returns diagnostic data for a filter or one of its elements.
+ */
+static int canprio_dump(struct tcf_proto *tp, unsigned long fh,
+ struct sk_buff *skb, struct tcmsg *t)
+{
+ struct canprio_filter *f = (struct canprio_filter *) fh;
+ struct nlattr *nest;
+ struct canprio_rules *r;
+ pr_debug("%s() invoked\n", __FUNCTION__);
+
+ if (f == NULL)
+ return skb->len;
+
+ rcu_read_lock();
+ r = rcu_dereference(f->rules);
+ t->tcm_handle = f->handle;
+
+ nest = nla_nest_start(skb, TCA_OPTIONS);
+ if (nest == NULL)
+ goto nla_put_failure;
+
+ if (f->res.classid)
+ NLA_PUT_U32(skb, TCA_CANPRIO_CLASSID, f->res.classid);
+
+ NLA_PUT(skb, TCA_CANPRIO_RULES, r->rules_count *
+ sizeof(struct can_filter), r->rules_raw);
+
+
+ nla_nest_end(skb, nest);
+
+ rcu_read_unlock();
+ return skb->len;
+
+nla_put_failure:
+ nla_nest_cancel(skb, nest);
+ rcu_read_unlock();
+ return -1;
+}
+
static struct tcf_proto_ops cls_canprio_ops __read_mostly = {
.kind = "canprio",
.put = canprio_put,
.change = canprio_change,
.delete = canprio_delete,
+ .walk = canprio_walk,
+ .dump = canprio_dump,
.owner = THIS_MODULE,
};
static int __init init_canprio(void)
{
- printk("Canprio loaded\n");
+ pr_debug("Canprio loaded\n");
+#ifdef SFF_BITMAP
+ pr_debug("SFF frames stored in bitmap\n");
+#else
+ pr_debug("SFF frames stored in array\n");
+#endif
return register_tcf_proto_ops(&cls_canprio_ops);
}
static void __exit exit_canprio(void)
{
- printk("Canprio removed\n");
+ pr_debug("Canprio removed\n");
unregister_tcf_proto_ops(&cls_canprio_ops);
}
module_init(init_canprio);
module_exit(exit_canprio);
MODULE_LICENSE("GPL");
+MODULE_AUTHOR(""); // FIXME
+MODULE_DESCRIPTION("Controller Area Network can_id classifier");
projects / lisovros / linux_canprio.git / blobdiff