SFF rules can be stored either in bitmap or in array of struct can_filter + Some...

diff --git a/net/sched/cls_canprio.c b/net/sched/cls_canprio.c
index 7efed62009f3e8eeff3834dd4a0691096cbdf9c8..653d8a99dec886ad2ffa2536a7d9ee9a90a88f9e 100644 (file)
--- a/net/sched/cls_canprio.c
+++ b/net/sched/cls_canprio.c
@@ -34,12 +34,13 @@
 #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 {
@@ -64,8 +65,10 @@ struct canprio_rules {
                                        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;
@@ -91,11 +94,12 @@ struct canprio_filter {
  * ----------------------------------------------------------------------------
  */
 
+#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__);
+       pr_debug("%s() invoked\n", __FUNCTION__);
        can_mask &= CAN_SFF_MASK;
        can_id &= can_mask;
 
@@ -109,6 +113,7 @@ static void canprio_sff_match_add(struct canprio_rules *rls, u32 can_id, u32 can
                        set_bit(i, rls->match_sff);
        }
 }
+#endif
 
 /* 
  * Extracts Can ID ot ouf the sk_buff structure.
@@ -142,7 +147,7 @@ static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
        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();
@@ -163,7 +168,19 @@ static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
                                }
                        }
                } else {
+                       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 (r->inv_match_en)
@@ -171,7 +188,7 @@ static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
 
                if (match) {
                        *res = f->res;
-                       printk( "   canprio_classify() match ok: ID 0x%x\n", can_id);
+                       pr_debug( "   canprio_classify() match ok: ID 0x%x\n", can_id);
                        rcu_read_unlock();
                        return TC_POLICE_OK;
                }
@@ -190,15 +207,11 @@ static unsigned long canprio_get(struct tcf_proto *tp, u32 handle)
        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;
                }
        }
@@ -222,17 +235,12 @@ static unsigned int canprio_gen_handle(struct tcf_proto *tp)
 
        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;
@@ -252,25 +260,27 @@ static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
        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]);
                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));
-               printk(" rules_count = %u\n", rules_tmp->rules_count);
+               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)
-                       return -ENOMEM;
+                       goto errout;
 
                /* Process EFF frames */
                for (i = 0; i < rules_tmp->rules_count; i++) {
@@ -281,7 +291,7 @@ static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
                                        &canprio_nl_rules[i], sizeof(struct can_filter));
                                rules_tmp->eff_rules_count ++;
 
-                               printk(" can ID to match = 0x%x with mask 0x%x\n",
+                               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;
@@ -300,10 +310,12 @@ static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
                                        &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",
+                               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);
                        }
                }
@@ -325,9 +337,7 @@ static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
        return 0;
 
 errout:
-       if (rules_tmp->rules_raw != NULL) //FIXME is ok?
-               kfree(rules_tmp->rules_raw);
-
+       kfree(rules_tmp);
        return err;
 }
 
@@ -351,7 +361,7 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
        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;
@@ -368,7 +378,7 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
                if (handle && f->handle != handle)
                        return -EINVAL;
 
-               printk("[change existing filter]\n");
+               pr_debug("[change existing filter]\n");
                return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
        }
 
@@ -378,21 +388,16 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
        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);
        }
 
-
        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
@@ -408,7 +413,6 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
 
        return 0;
 
-
 errout:
        if (*arg == 0UL && f)
                kfree(f);
@@ -472,7 +476,7 @@ static int canprio_delete(struct tcf_proto *tp, unsigned long arg)
 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)
@@ -494,7 +498,7 @@ 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__);
+       pr_debug("%s() invoked\n", __FUNCTION__);
 
        list_for_each_entry(f, &head->flist, link) {
                if (arg->count < arg->skip)
@@ -518,7 +522,7 @@ static int canprio_dump(struct tcf_proto *tp, unsigned long fh,
        struct canprio_filter *f = (struct canprio_filter *) fh;
        struct nlattr *nest;
        struct canprio_rules *r;
-       printk("%s() invoked\n", __FUNCTION__);
+       pr_debug("%s() invoked\n", __FUNCTION__);
 
        if (f == NULL)
                return skb->len;
@@ -566,13 +570,18 @@ static struct tcf_proto_ops cls_canprio_ops __read_mostly = {
 
 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);
 }