#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>
+
+//--------------------------------------
+//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;
+//--------------------------------------
/* Definition of Netlink messages */
enum {
TCA_CANPRIO_A_UNSPEC,
TCA_CANPRIO_CLASSID,
- TCA_CANPRIO_MATCH,
- TCA_CANPRIO_XY,
+ TCA_CANPRIO_RULES,
+ 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 },
- [TCA_CANPRIO_MATCH] = { .type = NLA_U32 }, //{ .type = NLA_NESTED },
- [TCA_CANPRIO_XY] = { .type = NLA_U32 },
+ [TCA_CANPRIO_CLASSID] = { .type = NLA_U32 },
+ //FIXME Be aware of possible problems with 64bit kernel and 32bit userspace etc.
+ [TCA_CANPRIO_RULES] = { /*.len = (sizeof(struct canprio_rule)*32)*/ }, /* FIXME */
+ [TCA_CANPRIO_INV_EN] = { .type = NLA_U32 },
+};
+
+struct canprio_eff_item {
+ struct hlist_node list;
+ struct rcu_head rcu;
+ u32 canid;
+ u32 mask;
};
+static DEFINE_SPINLOCK(canprio_match_eff_lock);
struct canprio_head {
- u32 hgenerator;
- struct list_head flist;
+ u32 hgenerator;
+ struct list_head flist;
};
struct canprio_filter {
- u32 handle;
- u32 match; // Matching CAN ID. Will be sth like list os lists
- struct tcf_result res;
- struct list_head link;
+ u32 handle;
+ // For each SFF Can ID (11 bit) there is one record in this bitfield
+ DECLARE_BITMAP(match_sff, CAN_SFF_MASK + 1);
+ int inv_match_en; // Inverted match flag
+ struct hlist_head match_eff; // List of EFF frames to match
+ struct tcf_result res;
+ struct list_head link;
};
/*
* ----------------------------------------------------------------------------
*/
+static void sff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
+{
+ int i;
+
+ printk("%s() invoked\n", __FUNCTION__);
+ mask &= CAN_SFF_MASK;
+ canid = canid & mask;
+
+ if (mask == CAN_SFF_MASK) {
+ set_bit(canid, f->match_sff);
+ return;
+ }
+
+ for (i = 0; i <= CAN_SFF_MASK; i++) {
+ if ((i & mask) == canid)
+ set_bit(i, f->match_sff);
+ }
+}
+
+static int eff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
+{
+ struct canprio_eff_item *eff;
+ int err = 0;
+
+ printk("%s() invoked\n", __FUNCTION__);
+ mask &= CAN_EFF_MASK;
+ canid = canid & mask;
+
+ eff = kmalloc(sizeof(struct canprio_eff_item), GFP_KERNEL);
+ if (!eff)
+ return -ENOMEM;
+
+ spin_lock(&canprio_match_eff_lock);
+
+ eff->canid = canid;
+ eff->mask = mask;
+
+ hlist_add_head_rcu(&eff->list, &f->match_eff);
+
+ spin_unlock(&canprio_match_eff_lock);
+
+ return err;
+}
+
/*
* Extracts Can ID ot ouf the sk_buff structure.
*/
{
/* Can ID is inside of data field */
struct can_frame *cf = (struct can_frame *)skb->data;
- //canid_t can_id = cf->can_id;
+ //canid_t canid = cf->canid;
return (u32)cf->can_id;
}
{
struct canprio_head *head = (struct canprio_head *)tp->root;
struct canprio_filter *f;
+ u32 canid;
- printk(" canprio_classify invoked\n");
+ printk(" canprio_classify() invoked\n");
+ canid = canprio_get_id(skb);
list_for_each_entry(f, &head->flist, link) {
- printk(" canprio_classify can ID = 0x%x\n", canprio_get_id(skb));
- if (canprio_get_id(skb) == f->match) {
- printk( " canprio_classify match ID 0x%x\n", f->match);
+ bool match = false;
+ printk(" canprio_classify() can ID of received frame = 0x%x\n", canid);
+
+ if (canid & CAN_EFF_FLAG) {
+ struct canprio_eff_item *effi;
+ struct hlist_node *next;
+
+ rcu_read_lock();
+
+ hlist_for_each_entry_rcu(effi, next, &f->match_eff, list) {
+ if ((effi->canid ^ canid) & effi->mask) {
+ match = true;
+ break;
+ }
+ }
+
+ rcu_read_unlock();
+ } else {
+ match = test_bit(canid, f->match_sff);
+ }
+
+ if (f->inv_match_en)
+ match = !match;
+
+ if (match) {
*res = f->res;
return TC_POLICE_OK;
+ printk( " canprio_classify() match ok: ID 0x%x\n", canid);
}
}
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_sff, CAN_SFF_MASK + 1);
+ INIT_HLIST_HEAD(&f->match_eff);
+
tcf_bind_filter(tp, &f->res, base);
}
}
/*
- * 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;
+ //printk("canprio_get(%d) invoked\n", handle);
if (head == NULL)
return 0UL;
- list_for_each_entry(f, &head->flist, link)
- if (f->handle == handle)
- l = (unsigned long) f;
+ //printk("[running for_each_entry]\n");
+ list_for_each_entry(f, &head->flist, link) {
+ //printk("[f->handle = %d]\n", f->handle);
+ if (f->handle == handle) {
+ //printk("found something\n");
+ return (unsigned long) f;
+ }
+ }
- return l;
+ return 0UL;
}
/*
}
+/* FIXME all functions with prefixes? */
+static unsigned int gen_handle(struct tcf_proto *tp)
+{
+ struct canprio_head *head = (struct canprio_head *)tp->root;
+ int i = 0xFFFF;
+
+ while (i-- > 0) {
+ u32 h;
+ unsigned long tmp;
+
+ if ((head->hgenerator += 0x10000) == 0)
+ head->hgenerator = 0x10000;
+
+ h = head->hgenerator;
+ //if (canprio_get(tp, h) == 0)
+ // return h;
+ tmp = canprio_get(tp, h);
+ //printk("___tried %d result %lu\n", h, tmp);
+ if (tmp == 0)
+ return h;
+ }
+ return 0;
+}
+
+
/*
* 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
*/
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;
+
printk(" canprio_change invoked\n");
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.
err = nla_parse_nested(tb, TCA_CANPRIO_MAX, tca[TCA_OPTIONS], canprio_policy);
if (err < 0)
return err;
if (handle)
f->handle = handle;
else {
- //FIXME wat?
+ u32 handle;
+ /*
unsigned int i = 0x80000000;
do {
if (++head->hgenerator == 0x7FFFFFFF)
pr_err("Insufficient number of handles\n");
goto errout;
}
-
- f->handle = head->hgenerator;
+ */
+ handle = gen_handle(tp);
+ //printk("__new handle %d\n", handle);
+ f->handle = handle;
+ //FIXME where is hgenerator initialized
}
//Parse arguments
- if (tb[TCA_CANPRIO_MATCH] == NULL)
+ if (tb[TCA_CANPRIO_RULES] == NULL)
return -EINVAL;
- //f->match = nla_data(tb[TCA_CANPRIO_MATCH]);
- f->match = nla_get_u32(tb[TCA_CANPRIO_MATCH]);
- printk(" can ID to match = 0x%x\n", f->match);
+ 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++) {
+ /* FIXME: shouldn't use here the same logic as in
+ * can_rcv_filter() to filter for various combination
+ * of flags (EFF, RTR) */
+ if (canprio_rules[i].canid & CAN_EFF_FLAG) {
+ err = eff_match_add(f, canprio_rules[i].canid, canprio_rules[i].canid_mask);
+ if (err < 0)
+ goto errout;
+ } 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);
+ }
+
+ f->inv_match_en = 0;
+ if (tb[TCA_CANPRIO_INV_EN] != NULL)
+ f->inv_match_en = nla_get_u32(tb[TCA_CANPRIO_INV_EN]);
//Add newly created filter to list of all filters
tcf_tree_lock(tp);
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);
+ struct canprio_eff_item *effi;
+ struct hlist_node *p, *n;
+
+ rcu_barrier();
+
+ tcf_unbind_filter(tp, &f->res);
+ hlist_for_each_entry_safe(effi, p, n, &f->match_eff, list) {
+ kfree(effi);
+ }
+
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);
static int canprio_init(struct tcf_proto *tp)
{
struct canprio_head *head;
+ //printk("tp = %p\n", tp);
+ printk(" 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;
+}
- printk(" canprio_init invoked\n");
- head = kzalloc(sizeof(*head), GFP_KERNEL);
- if (head == NULL)
- return -ENOBUFS;
- INIT_LIST_HEAD(&head->flist);
- tp->root = head;
- return 0;
+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;
+
+ printk("%s() invoked\n", __FUNCTION__);
+
+ list_for_each_entry(f, &head->flist, link) {
+ if (arg->count < arg->skip)
+ goto skip;
+
+ printk("calling canprio_dump()\n");
+ if (arg->fn(tp, (unsigned long) f, arg) < 0) {
+ arg->stop = 1;
+ break;
+ }
+skip:
+ arg->count++;
+ }
+}
+
+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;
+
+ printk("%s() invoked\n", __FUNCTION__);
+
+ if (f == NULL)
+ return skb->len;
+
+ 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, canprio_rules_count * sizeof(struct canprio_rule),
+ // canprio_rules);
+
+ /* ...
+ ... */
+
+ nla_nest_end(skb, nest);
+
+ return skb->len;
+
+nla_put_failure:
+ nla_nest_cancel(skb, nest);
+ return -1;
}
.put = canprio_put,
.change = canprio_change,
.delete = canprio_delete,
+ .walk = canprio_walk,
+ .dump = canprio_dump,
.owner = THIS_MODULE,
};
module_init(init_canprio);
module_exit(exit_canprio);
MODULE_LICENSE("GPL");
+MODULE_AUTHOR(""); // FIXME
+MODULE_DESCRIPTION("");
projects / lisovros / linux_canprio.git / blobdiff