Storing of EFF IDs implemented with RCU lists.

diff --git a/net/sched/cls_canprio.c b/net/sched/cls_canprio.c
index 0adb2ae00e738a446baf8f71c62d34b5d995f7af..69f0d820a6fc669677a62f413423581dbe0d4a43 100644 (file)
--- a/net/sched/cls_canprio.c
+++ b/net/sched/cls_canprio.c
@@ -19,6 +19,8 @@
 #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"?
@@ -29,7 +31,6 @@ struct canprio_rule {
 
 struct canprio_rule *canprio_rules;
 int canprio_rules_count = 0;
-int canprio_inv_en = 0;
 //--------------------------------------
 
 /* Definition of Netlink messages */
@@ -49,17 +50,27 @@ static const struct nla_policy canprio_policy[TCA_CANPRIO_MAX + 1] = {
        [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 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;
+       DECLARE_BITMAP(match_sff, CAN_SFF_MASK + 1);
+       int inv_match_en;
+       struct hlist_head match_eff;
+       struct tcf_result res;
+       struct list_head link;
 };
 
 /*
@@ -69,23 +80,43 @@ struct canprio_filter {
 static void sff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
 {
        int i;
+
+       mask &= CAN_SFF_MASK;
        canid = canid & mask;
 
-       if (mask >= CAN_SFF_MASK) {
-               set_bit(canid, f->match);
+       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);
+                       set_bit(i, f->match_sff);
        }
 }
 
-static void eff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
+static int eff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
 {
+       struct canprio_eff_item *eff;
+       int err = 0;
+
+       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;
 }
 
 /* 
@@ -95,7 +126,7 @@ static u32 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;
+       //canid_t canid = cf->canid;
 
        return (u32)cf->can_id;
 }
@@ -120,20 +151,40 @@ static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
        u32 canid;
 
        printk(" canprio_classify() invoked\n");
+       canid = canprio_get_id(skb);
 
        list_for_each_entry(f, &head->flist, link) {
-               canid = canprio_get_id(skb);
+               int match = 0;
                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 = 1;
+                                       break;
+                               }
+                       }
 
+                       rcu_read_unlock();
                } else {
-                       if(test_bit(canid, f->match) ^ canprio_inv_en) {
-                               printk( "   canprio_classify() match ok: ID 0x%x\n", canid);
-                               *res = f->res;
-                               return TC_POLICE_OK;
+                       if(test_bit(canid, f->match_sff)) {
+                               match = 1;
                        }
                }
+
+               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);
+               }
        }
 
        return TC_POLICE_UNSPEC;
@@ -151,7 +202,8 @@ static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
 
        if (tb[TCA_CANPRIO_CLASSID]) {
                f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
-               bitmap_zero(f->match, CAN_SFF_MASK + 1);
+               bitmap_zero(f->match_sff, CAN_SFF_MASK + 1);
+               INIT_HLIST_HEAD(&f->match_eff);
 
                tcf_bind_filter(tp, &f->res, base);
        }
@@ -271,7 +323,9 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
 
        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);
+                       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);
                }
@@ -280,7 +334,9 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
                        canprio_rules[i].canid, canprio_rules[i].canid_mask);
        }
 
-       canprio_inv_en = nla_get_u32(tb[TCA_CANPRIO_INV_EN]);
+       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);
@@ -300,9 +356,16 @@ errout:
 
 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);
 }
 
@@ -330,6 +393,8 @@ static int canprio_delete(struct tcf_proto *tp, unsigned long arg)
        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);