Basic implementation of canprio_walk() and canprio_dump() functions.

diff --git a/net/sched/cls_canprio.c b/net/sched/cls_canprio.c
index 2d3b4a27e85f0c22f458ec795414a998e40bc0ae..4181df526267e7b137b32ff82fba3257e9877e13 100644 (file)
--- a/net/sched/cls_canprio.c
+++ b/net/sched/cls_canprio.c
@@ -30,6 +30,7 @@ struct canprio_rule {
 };
 
 struct canprio_rule *canprio_rules;
+int canprio_rules_count = 0;
 //--------------------------------------
 
 /* Definition of Netlink messages */
@@ -45,7 +46,7 @@ enum {
 static const struct nla_policy canprio_policy[TCA_CANPRIO_MAX + 1] = {
        [TCA_CANPRIO_CLASSID]    = { .type = NLA_U32 },
        //FIXME Be aware of possible problems with 64bit kernel and 32bit userspace etc.
-       [TCA_CANPRIO_RULES]      = { .len = sizeof(canprio_rules) }, /* FIXME */
+       [TCA_CANPRIO_RULES]      = { /*.len = (sizeof(struct canprio_rule)*32)*/ }, /* FIXME */
        [TCA_CANPRIO_INV_EN]     = { .type = NLA_U32 },
 };
 
@@ -66,8 +67,8 @@ struct canprio_filter {
        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;
-       struct hlist_head match_eff;
+       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;
 };
@@ -80,6 +81,7 @@ 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;
 
@@ -99,6 +101,7 @@ 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;
 
@@ -209,22 +212,28 @@ static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
 } 
 
 /*
- * 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;
 }
 
 /*
@@ -236,6 +245,31 @@ static void canprio_put(struct tcf_proto *tp, unsigned long f)
 }
 
 
+/* 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
@@ -244,7 +278,7 @@ static void canprio_put(struct tcf_proto *tp, unsigned long f)
  * @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
  */
@@ -255,7 +289,6 @@ 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 canprio_rules_count = 0;
        int i;
        
        printk(" canprio_change invoked\n");
@@ -289,7 +322,8 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
        if (handle)
                f->handle = handle;
        else {
-               //FIXME wat?
+               u32 handle;
+       /*
                unsigned int i = 0x80000000;
                do {
                        if (++head->hgenerator == 0x7FFFFFFF)
@@ -300,8 +334,11 @@ static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
                        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
        }
 
 
@@ -412,17 +449,75 @@ static int canprio_delete(struct tcf_proto *tp, unsigned long arg)
 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;
-       tp->protocol = htons(ETH_P_CAN); /* Work only on AF_CAN packets - not tested! */
+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__);
 
-       return 0;
+       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;
 }
 
 
@@ -435,6 +530,8 @@ static struct tcf_proto_ops cls_canprio_ops __read_mostly = {
        .put            =       canprio_put,
        .change         =       canprio_change,
        .delete         =       canprio_delete,
+       .walk           =       canprio_walk,
+       .dump           =       canprio_dump,
        .owner          =       THIS_MODULE,
 };
 
@@ -453,4 +550,6 @@ static void __exit exit_canprio(void)
 module_init(init_canprio);
 module_exit(exit_canprio);
 MODULE_LICENSE("GPL");
+MODULE_AUTHOR(""); // FIXME
+MODULE_DESCRIPTION("");