It is possible to have multiple rules for one instance of a filter. Fully implemented...

diff --git a/net/sched/cls_canprio.c b/net/sched/cls_canprio.c
index b517e19046fc222f632d35358fe71aae8f4e94e2..b375d4741733fe39a7f82dc6f5ff7a75faa4053a 100644 (file)
--- a/net/sched/cls_canprio.c
+++ b/net/sched/cls_canprio.c
@@ -18,22 +18,33 @@
 #include <net/act_api.h>
 #include <net/pkt_cls.h>
 #include <linux/can.h>
+#include <linux/bitmap.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;
+//--------------------------------------
 
 /* Definition of Netlink messages */
 enum {
        TCA_CANPRIO_A_UNSPEC,
        TCA_CANPRIO_CLASSID,
-       TCA_CANPRIO_MATCH,
-       TCA_CANPRIO_MATCH_MASK,
+       TCA_CANPRIO_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_MATCH_MASK] = { .type = NLA_U32 },
+       //FIXME Be aware of possible problems with 64bit kernel and 32bit userspace etc.
+       [TCA_CANPRIO_RULES]      = { .len = sizeof(canprio_rules) }, 
 };
 
 struct canprio_head {
@@ -43,8 +54,8 @@ struct canprio_head {
 
 struct canprio_filter {
        u32                  handle;
-       u32                  canid;
-       u32                  canid_mask;
+       // 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;
 };
@@ -53,6 +64,28 @@ struct canprio_filter {
  * ----------------------------------------------------------------------------
  */
 
+static void sff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
+{
+       int i;
+       canid = canid & mask;
+
+       if (mask >= CAN_SFF_MASK) {
+               set_bit(canid, f->match);
+               return;
+       }
+
+       for (i = 0; i <= CAN_SFF_MASK; i++) {
+               if ((i & mask) == canid)
+                       set_bit(i, f->match);
+       }
+}
+
+static void eff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
+{
+
+
+}
+
 /* 
  * Extracts Can ID ot ouf the sk_buff structure.
  */
@@ -82,16 +115,22 @@ static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
 {
        struct canprio_head *head = (struct canprio_head *)tp->root;
        struct canprio_filter *f;
+       u32 canid;
 
        printk(" canprio_classify() invoked\n");
 
        list_for_each_entry(f, &head->flist, link) {
-               printk("  canprio_classify() can ID of received frame = 0x%x\n", canprio_get_id(skb));
-               if ((canprio_get_id(skb) & f->canid_mask) == (f->canid & f->canid_mask)) {
-                       printk( "   canprio_classify() match ok: ID 0x%x mask 0x%x\n", 
-                               f->canid, f->canid_mask);
-                       *res = f->res;
-                       return TC_POLICE_OK;
+               canid = canprio_get_id(skb);
+               printk("  canprio_classify() can ID of received frame = 0x%x\n", canid);
+
+               if (canid & CAN_EFF_FLAG) {
+
+               } else {
+                       if(test_bit(canid, f->match)) {
+                               printk( "   canprio_classify() match ok: ID 0x%x\n", canid);
+                               *res = f->res;
+                               return TC_POLICE_OK;
+                       }
                }
        }
 
@@ -110,6 +149,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);
+
                tcf_bind_filter(tp, &f->res, base);
        }
 
@@ -163,7 +204,8 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
        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)
@@ -218,13 +260,23 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
 
 
        //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->canid = nla_get_u32(tb[TCA_CANPRIO_MATCH]);
-       f->canid_mask = nla_get_u32(tb[TCA_CANPRIO_MATCH_MASK]);
-       printk(" can ID to match = 0x%x with mask 0x%x\n", f->canid, f->canid_mask);
+       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);
@@ -298,6 +350,7 @@ static int canprio_init(struct tcf_proto *tp)
                return -ENOBUFS;
        INIT_LIST_HEAD(&head->flist);
        tp->root = head;
+
        return 0;
 }