net/sched/cls_canprio.c

   1 /* cls_canprio.c  -- Canprio classifier.
   2  * Makes decisions accoring to Can IDs.
   3  *
   4  * Implementation notes;
   5  *   parameter of functions named "base" is pointer to some parent element
   6  *
   7  */
   8
   9 #include <linux/module.h>
  10 #include <linux/slab.h>
  11 #include <linux/types.h>
  12 #include <linux/kernel.h>
  13 #include <linux/string.h>
  14 #include <linux/errno.h>
  15 #include <linux/rtnetlink.h>
  16 #include <linux/skbuff.h>
  17 #include <net/netlink.h>
  18 #include <net/act_api.h>
  19 #include <net/pkt_cls.h>
  20 #include <linux/can.h>
  21
  22
  23 /* Definition of Netlink messages */
  24 enum {
  25         TCA_CANPRIO_A_UNSPEC,
  26         TCA_CANPRIO_CLASSID,
  27         TCA_CANPRIO_MATCH,
  28         TCA_CANPRIO_MATCH_MASK,
  29         __TCA_CANPRIO_MAX,
  30 };
  31 #define TCA_CANPRIO_MAX (__TCA_CANPRIO_MAX - 1)
  32
  33 static const struct nla_policy canprio_policy[TCA_CANPRIO_MAX + 1] = {
  34         [TCA_CANPRIO_CLASSID]    = { .type = NLA_U32 },
  35         [TCA_CANPRIO_MATCH]      = { .type = NLA_U32 }, //{ .type = NLA_NESTED },
  36         [TCA_CANPRIO_MATCH_MASK] = { .type = NLA_U32 },
  37 };
  38
  39 struct canprio_head {
  40         u32                  hgenerator;
  41         struct list_head     flist;
  42 };
  43
  44 struct canprio_filter {
  45         u32                  handle;
  46         u32                  canid;
  47         u32                  canid_mask;
  48         struct tcf_result    res;
  49         struct list_head     link;
  50 };
  51
  52 /*
  53  * ----------------------------------------------------------------------------
  54  */
  55
  56 /*
  57  * Extracts Can ID ot ouf the sk_buff structure.
  58  */
  59 static u32 canprio_get_id(struct sk_buff *skb)
  60 {
  61         /* Can ID is inside of data field */
  62         struct can_frame *cf = (struct can_frame *)skb->data;
  63         //canid_t can_id = cf->can_id;
  64
  65         return (u32)cf->can_id;
  66 }
  67
  68 /*
  69  * Performs the classification. Iterates over all instances of filter
  70  * checking for Can ID match.
  71  *
  72  * @skb: Socket buffer
  73  * @tp:
  74  * @res: Is used for setting Class ID as a result of classification
  75  *
  76  * Returns value relevant for policing. Used return values:
  77  *   TC_POLICE_OK if succesfully classified (without regard to policing rules)
  78  *   TC_POLICE_UNSPEC if no matching filter was found
  79  */
  80 static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
  81                           struct tcf_result *res)
  82 {
  83         struct canprio_head *head = (struct canprio_head *)tp->root;
  84         struct canprio_filter *f;
  85
  86         printk(" canprio_classify() invoked\n");
  87
  88         list_for_each_entry(f, &head->flist, link) {
  89                 printk("  canprio_classify() can ID of received frame = 0x%x\n", canprio_get_id(skb));
  90                 if ((canprio_get_id(skb) & f->canid_mask) == (f->canid & f->canid_mask)) {
  91                         printk( "   canprio_classify() match ok: ID 0x%x mask 0x%x\n",
  92                                 f->canid, f->canid_mask);
  93                         *res = f->res;
  94                         return TC_POLICE_OK;
  95                 }
  96         }
  97
  98         return TC_POLICE_UNSPEC;
  99 }
 100
 101 /*
 102  * Configure filter
 103  */
 104 static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
 105                            unsigned long base, struct nlattr **tb,
 106                            struct nlattr *est)
 107 {
 108         //int err;
 109         printk(" canprio_set_parms invoked\n");
 110
 111         if (tb[TCA_CANPRIO_CLASSID]) {
 112                 f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
 113                 tcf_bind_filter(tp, &f->res, base);
 114         }
 115
 116         return 0;
 117 }
 118
 119 /*
 120  * Looks up a filter element by its handle and returns the internal filter ID
 121  */
 122 static unsigned long canprio_get(struct tcf_proto *tp, u32 handle)
 123 {
 124         unsigned long l = 0UL;
 125         struct canprio_head *head = (struct canprio_head *) tp->root;
 126         struct canprio_filter *f;
 127
 128         if (head == NULL)
 129                 return 0UL;
 130
 131         list_for_each_entry(f, &head->flist, link)
 132                 if (f->handle == handle)
 133                         l = (unsigned long) f;
 134
 135         return l;
 136 }
 137
 138 /*
 139  * Is invoked when a filter element previously referenced
 140  * with get() is no longer used
 141  */
 142 static void canprio_put(struct tcf_proto *tp, unsigned long f)
 143 {
 144 }
 145
 146
 147 /*
 148  * Called for changing properties of an existing filter or after addition
 149  * of a new filter to a class (by calling bind_tcf which binds an instance
 150  * of a filter to the class).
 151  *
 152  * @tp:     Structure representing instance of a filter.
 153  *          Part of a linked list of all filters.
 154  * @base:
 155  * @handle:
 156  * @tca:    Messages passed through the Netlink from userspace.
 157  * @arg:    ??? FIXME
 158  */
 159 static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
 160                           struct nlattr **tca, unsigned long *arg)
 161 {
 162         int err;
 163         struct canprio_head *head = (struct canprio_head *)tp->root;
 164         struct canprio_filter *f = (struct canprio_filter *)*arg;
 165         struct nlattr *tb[TCA_CANPRIO_MAX + 1];
 166
 167         printk(" canprio_change invoked\n");
 168
 169         if (tca[TCA_OPTIONS] == NULL)
 170                 return -EINVAL;
 171
 172         //Parses a stream of attributes and stores a pointer to each attribute in
 173         //the tb array accessible via the attribute type. Policy may be set to NULL
 174         //if no validation is required.
 175         err = nla_parse_nested(tb, TCA_CANPRIO_MAX, tca[TCA_OPTIONS], canprio_policy);
 176         if (err < 0)
 177                 return err;
 178
 179         //Change existing filter (?)
 180         if (f != NULL) {
 181                 if (handle && f->handle != handle)
 182                         return -EINVAL;
 183                 return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 184         }
 185
 186         //Create new filter (?)
 187         err = -ENOBUFS;
 188         f = kzalloc(sizeof(*f), GFP_KERNEL);
 189         if (f == NULL)
 190                 goto errout;
 191
 192
 193         //FIXME why?
 194         err = -EINVAL;
 195         if (handle)
 196                 f->handle = handle;
 197         else {
 198                 //FIXME wat?
 199                 unsigned int i = 0x80000000;
 200                 do {
 201                         if (++head->hgenerator == 0x7FFFFFFF)
 202                                 head->hgenerator = 1;
 203                 } while (--i > 0 && canprio_get(tp, head->hgenerator));
 204
 205                 if (i <= 0) {
 206                         pr_err("Insufficient number of handles\n");
 207                         goto errout;
 208                 }
 209
 210                 f->handle = head->hgenerator;
 211         }
 212
 213
 214         //Configure newly created filter
 215         err = canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 216         if (err < 0)
 217                 goto errout;
 218
 219
 220         //Parse arguments
 221         if (tb[TCA_CANPRIO_MATCH] == NULL)
 222                 return -EINVAL;
 223
 224         //f->match = nla_data(tb[TCA_CANPRIO_MATCH]);
 225         f->canid = nla_get_u32(tb[TCA_CANPRIO_MATCH]);
 226         f->canid_mask = nla_get_u32(tb[TCA_CANPRIO_MATCH_MASK]);
 227         printk(" can ID to match = 0x%x with mask 0x%x\n", f->canid, f->canid_mask);
 228
 229         //Add newly created filter to list of all filters
 230         tcf_tree_lock(tp);
 231         list_add(&f->link, &head->flist);
 232         tcf_tree_unlock(tp);
 233         *arg = (unsigned long) f;
 234
 235         return 0;
 236
 237 errout:
 238         if (*arg == 0UL && f) //FIXME why 0UL?
 239                 kfree(f);
 240
 241         return err;
 242 }
 243
 244
 245 static void canprio_delete_filter(struct tcf_proto *tp, struct canprio_filter *f)
 246 {
 247         tcf_unbind_filter(tp, &f->res);
 248         //tcf_exts_destroy(tp, &f->exts);
 249         //tcf_em_tree_destroy(tp, &f->ematches);
 250         kfree(f);
 251 }
 252
 253 /*
 254  * Remove whole filter.
 255  */
 256 static void canprio_destroy(struct tcf_proto *tp)
 257 {
 258         struct canprio_head *head = tp->root;
 259         struct canprio_filter *f, *n;
 260
 261         list_for_each_entry_safe(f, n, &head->flist, link) {
 262                 list_del(&f->link);
 263                 canprio_delete_filter(tp, f);
 264         }
 265         kfree(head);
 266 }
 267
 268 /*
 269  * Delete one instance of a filter.
 270  */
 271 static int canprio_delete(struct tcf_proto *tp, unsigned long arg)
 272 {
 273         struct canprio_head *head = (struct canprio_head *)tp->root;
 274         struct canprio_filter *t;
 275         struct canprio_filter *f = (struct canprio_filter *)arg;
 276
 277         list_for_each_entry(t, &head->flist, link)
 278                 if (t == f) {
 279                         tcf_tree_lock(tp);
 280                         list_del(&t->link);
 281                         tcf_tree_unlock(tp);
 282                         canprio_delete_filter(tp, t);
 283                         return 0;
 284                 }
 285
 286         return -ENOENT;
 287 }
 288
 289 //FIXME copied from cls_basic, not sure if ok;
 290 static int canprio_init(struct tcf_proto *tp)
 291 {
 292         struct canprio_head *head;
 293
 294         printk(" canprio_init invoked\n");
 295
 296         head = kzalloc(sizeof(*head), GFP_KERNEL);
 297         if (head == NULL)
 298                 return -ENOBUFS;
 299         INIT_LIST_HEAD(&head->flist);
 300         tp->root = head;
 301         return 0;
 302 }
 303
 304
 305 static struct tcf_proto_ops cls_canprio_ops __read_mostly = {
 306         .kind           =       "canprio",
 307         .classify       =       canprio_classify,
 308         .init           =       canprio_init,
 309         .destroy        =       canprio_destroy,
 310         .get            =       canprio_get,
 311         .put            =       canprio_put,
 312         .change         =       canprio_change,
 313         .delete         =       canprio_delete,
 314         .owner          =       THIS_MODULE,
 315 };
 316
 317 static int __init init_canprio(void)
 318 {
 319         printk("Canprio loaded\n");
 320         return register_tcf_proto_ops(&cls_canprio_ops);
 321 }
 322
 323 static void __exit exit_canprio(void)
 324 {
 325         printk("Canprio removed\n");
 326         unregister_tcf_proto_ops(&cls_canprio_ops);
 327 }
 328
 329 module_init(init_canprio);
 330 module_exit(exit_canprio);
 331 MODULE_LICENSE("GPL");
 332