#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>
+#define SFF_BITMAP 1
+#undef SFF_BITMAP
/* Definition of Netlink messages */
enum {
[TCA_CANPRIO_INV_EN] = { .type = NLA_U32 },
};
-struct canprio_eff_item {
- struct hlist_node list;
- struct rcu_head rcu;
- struct can_filter rule;
-};
-static DEFINE_SPINLOCK(canprio_match_eff_lock);
-
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) */
+ (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 */
- struct hlist_head match_eff; /* List of EFF frames to match */
+#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 {
struct canprio_filter {
u32 handle;
- struct canprio_rules rules; /* All rules necessary for classification */
+ 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 canprio_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;
- printk("%s() invoked\n", __FUNCTION__);
- mask &= CAN_SFF_MASK;
- canid &= mask;
+ pr_debug("%s() invoked\n", __FUNCTION__);
+ can_mask &= CAN_SFF_MASK;
+ can_id &= can_mask;
- if (mask == CAN_SFF_MASK) {
- set_bit(canid, f->rules.match_sff);
+ 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->rules.match_sff);
+ if ((i & can_mask) == can_id)
+ set_bit(i, rls->match_sff);
}
}
-
-static int canprio_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 &= mask;
-
-
- eff = kmalloc(sizeof(struct canprio_eff_item), GFP_KERNEL);
- if (!eff)
- return -ENOMEM;
-
- spin_lock(&canprio_match_eff_lock);
-
- eff->rule.can_id = canid;
- eff->rule.can_mask = mask;
-
- hlist_add_head_rcu(&eff->list, &f->rules.match_eff);
-
- spin_unlock(&canprio_match_eff_lock);
-
- return err;
-}
+#endif
/*
* Extracts Can ID ot ouf the sk_buff structure.
{
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");
- canid = (u32)canprio_get_id(skb);
+ pr_debug(" canprio_classify() invoked\n");
+ can_id = (u32)canprio_get_id(skb);
+ rcu_read_lock();
list_for_each_entry(f, &head->flist, link) {
bool match = false;
+ r = rcu_dereference(f->rules);
+
- if (canid & CAN_EFF_FLAG) {
- struct canprio_eff_item *effi;
- struct hlist_node *next;
- canid &= CAN_EFF_MASK;
+ if (can_id & CAN_EFF_FLAG) {
+ can_id &= CAN_EFF_MASK;
- rcu_read_lock();
+ 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)) {
- hlist_for_each_entry_rcu(effi, next, &f->rules.match_eff, list) {
- //if (!(((effi->rule.can_id) ^ canid) & effi->rule.can_mask)) {
- if ((effi->rule.can_id & effi->rule.can_mask) ==
- (canid & effi->rule.can_mask)) {
match = true;
break;
}
}
-
- rcu_read_unlock();
} else {
- match = test_bit(canid, f->rules.match_sff);
+ 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
}
- //if (f->rules.inv_match_en)
+ //if (r->inv_match_en)
// match = !match;
if (match) {
*res = f->res;
- printk( " canprio_classify() match ok: ID 0x%x\n", canid);
+ pr_debug( " canprio_classify() match ok: ID 0x%x\n", can_id);
+ rcu_read_unlock();
return TC_POLICE_OK;
}
}
+ rcu_read_unlock();
return TC_POLICE_UNSPEC;
}
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;
- //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;
}
}
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)
+ if (canprio_get(tp, h) == 0);
return h;
}
return 0;
}
-/*
- * Reset filter to default (blank) settings
- */
-static void canprio_reset(struct canprio_filter *f)
-{
- struct canprio_eff_item *effi;
- struct hlist_node *p, *n;
-
- if (f->rules.rules_raw != NULL)
- kfree(f->rules.rules_raw);
- bitmap_zero(f->rules.match_sff, CAN_SFF_MASK + 1);
- f->rules.inv_match_en = 0;
- f->rules.rules_count = 0; //FIXME not necessary
-
- rcu_barrier();
- hlist_for_each_entry_safe(effi, p, n, &f->rules.match_eff, list) {
- kfree(effi);
- }
- INIT_HLIST_HEAD(&f->rules.match_eff);
+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,
struct nlattr *est)
{
struct can_filter *canprio_nl_rules;
+ struct canprio_rules *rules_tmp;
int err;
int i;
- printk("%s() invoked\n", __FUNCTION__);
+ pr_debug("%s() invoked\n", __FUNCTION__);
+
+ rules_tmp = kzalloc(sizeof(*rules_tmp), GFP_KERNEL);
+ if (rules_tmp == NULL)
+ return -ENOBUFS;
- //if (tb[TCA_CANPRIO_CLASSID] == NULL) //FIXME is enough?
- // return -EINVAL;
+ err = -EINVAL;
+ if (tb[TCA_CANPRIO_CLASSID] == NULL)
+ goto errout;
if (tb[TCA_CANPRIO_RULES]) {
canprio_nl_rules = nla_data(tb[TCA_CANPRIO_RULES]);
- f->rules.rules_count = (nla_len(tb[TCA_CANPRIO_RULES]) / sizeof(struct can_filter));
- printk(" rules_count = %u\n", f->rules.rules_count);
+ 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)) {
- f->rules.rules_raw = kzalloc(sizeof(struct can_filter) * f->rules.rules_count, GFP_KERNEL);
- if (f->rules.rules_raw == NULL)
- return -ENOMEM;
+ memcpy(rules_tmp->rules_raw + rules_tmp->eff_rules_count,
+ &canprio_nl_rules[i], sizeof(struct can_filter));
+ rules_tmp->eff_rules_count ++;
- memcpy(f->rules.rules_raw, canprio_nl_rules,
- sizeof(struct can_filter) * f->rules.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;
+ }
+ }
- for (i = 0; i < f->rules.rules_count; i++) {
+ /* 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)) {
-
- err = canprio_eff_match_add(f, canprio_nl_rules[i].can_id,
- canprio_nl_rules[i].can_mask);
- if (err < 0)
- goto errout;
+
+ continue;
} else {
- canprio_sff_match_add(f, canprio_nl_rules[i].can_id,
+ 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
- printk(" can ID to match = 0x%x with mask 0x%x\n",
- canprio_nl_rules[i].can_id, canprio_nl_rules[i].can_mask);
+ 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)
- f->rules.inv_match_en = nla_get_u32(tb[TCA_CANPRIO_INV_EN]);
+ 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:
- if (f->rules.rules_raw != NULL) //FIXME is ok?
- kfree(f->rules.rules_raw);
-
+ kfree(rules_tmp);
return err;
}
struct nlattr *tb[TCA_CANPRIO_MAX + 1];
int err;
- printk("%s() invoked\n", __FUNCTION__);
+ pr_debug("%s() invoked\n", __FUNCTION__);
if (tca[TCA_OPTIONS] == NULL)
return -EINVAL;
/* Change existing filter (remove all settings and add
them thereafter as if filter was newly created) */
if (f != NULL) {
- printk("%p\n", f);
if (handle && f->handle != handle)
return -EINVAL;
- printk("[change existing filter]\n");
- canprio_reset(f);
+ pr_debug("[change existing filter]\n");
return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
}
- printk("\n");
/* Create new filter */
err = -ENOBUFS;
tcf_bind_filter(tp, &f->res, base);
}
-
- bitmap_zero(f->rules.match_sff, CAN_SFF_MASK + 1);
- INIT_HLIST_HEAD(&f->rules.match_eff);
- f->rules.inv_match_en = 0;
-
-
err = -EINVAL;
if (handle) /* handle passed from userspace */
f->handle = handle;
else {
- //u32 handle;
- //handle = canprio_gen_handle(tp);
- //f->handle = handle;
- f->handle = 1;
+ f->handle = canprio_gen_handle(tp);
}
//Configure filter
static void canprio_delete_filter(struct tcf_proto *tp, struct canprio_filter *f)
{
- 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->rules.match_eff, list) {
- kfree(effi);
- }
- kfree(f->rules.rules_raw);
+ rcu_barrier();
+ kfree(f->rules->rules_raw);
+ kfree(f->rules);
kfree(f);
}
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;
+ 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;
}
/*
struct canprio_head *head = (struct canprio_head *) tp->root;
struct canprio_filter *f;
- printk("%s() invoked\n", __FUNCTION__);
+ pr_debug("%s() invoked\n", __FUNCTION__);
list_for_each_entry(f, &head->flist, link) {
if (arg->count < arg->skip)
{
struct canprio_filter *f = (struct canprio_filter *) fh;
struct nlattr *nest;
- int i;
- printk("%s() invoked\n", __FUNCTION__);
+ 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 (f->res.classid)
NLA_PUT_U32(skb, TCA_CANPRIO_CLASSID, f->res.classid);
- NLA_PUT(skb, TCA_CANPRIO_RULES, f->rules.rules_count *
- sizeof(struct can_filter), f->rules.rules_raw);
+ 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 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);
}
projects / lisovros / linux_canprio.git / blobdiff