net/sched/cls_canprio.c

   1 /*
   2  * cls_canprio.c  -- Canprio classifier.
   3  * Makes decisions accoring to controller area network (CAN) identifiers.
   4  *
   5  *             This program is free software; you can distribute it and/or
   6  *             modify it under the terms of the GNU General Public License
   7  *             as published by the Free Software Foundation; version 2 of
   8  *             the License.
   9  *
  10  * Idea:       Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
  11  * Copyright:  (c) 2011 Czech Technical University in Prague
  12  *             (c) 2011 Volkswagen Group Research
  13  * Authors:    Michal Sojka <sojkam1@fel.cvut.cz>
  14  *             Pavel Pisa <pisa@cmp.felk.cvut.cz>
  15  *             Rostislav Lisovy <lisovy@kormus.cz>
  16  * Founded by: Volkswagen Group Research
  17  *
  18  * Some function descriptions are heavily inspired by article "Linux Network
  19  * Traffic Control -- Implementation Overview" by Werner Almesberger
  20  *
  21  *
  22  * Implementation notes;
  23  *   parameter of functions named "base" is pointer to some parent element
  24  */
  25
  26 #include <linux/module.h>
  27 #include <linux/slab.h>
  28 #include <linux/types.h>
  29 #include <linux/kernel.h>
  30 #include <linux/string.h>
  31 #include <linux/errno.h>
  32 #include <linux/rtnetlink.h>
  33 #include <linux/skbuff.h>
  34 #include <net/netlink.h>
  35 #include <net/act_api.h>
  36 #include <net/pkt_cls.h>
  37 #include <linux/can.h>
  38 #include <linux/bitmap.h>
  39 #include <linux/spinlock.h>
  40 #include <linux/rcupdate.h>
  41 #include <linux/can.h>
  42
  43
  44 struct can_filter *canprio_rules;
  45 int canprio_rules_count = 0;
  46 //--------------------------------------
  47
  48 /* Definition of Netlink messages */
  49 enum {
  50         TCA_CANPRIO_A_UNSPEC,
  51         TCA_CANPRIO_CLASSID,
  52         TCA_CANPRIO_RULES,
  53         TCA_CANPRIO_INV_EN, /* enable inverse rules */
  54         __TCA_CANPRIO_MAX,
  55 };
  56 #define TCA_CANPRIO_MAX (__TCA_CANPRIO_MAX - 1)
  57
  58 static const struct nla_policy canprio_policy[TCA_CANPRIO_MAX + 1] = {
  59         [TCA_CANPRIO_CLASSID]    = { .type = NLA_U32 },
  60         //FIXME Be aware of possible problems with 64bit kernel and 32bit userspace etc.
  61         [TCA_CANPRIO_RULES]      = { .len = (sizeof(struct can_filter)) }, //FIXME
  62         [TCA_CANPRIO_INV_EN]     = { .type = NLA_U32 },
  63 };
  64
  65 struct canprio_eff_item {
  66         struct hlist_node list;
  67         struct rcu_head rcu;
  68         u32 canid;
  69         u32 mask;
  70 };
  71 static DEFINE_SPINLOCK(canprio_match_eff_lock);
  72
  73 struct canprio_head {
  74         u32 hgenerator;
  75         struct list_head flist;
  76 };
  77
  78 struct canprio_filter {
  79         u32 handle;
  80         // For each SFF Can ID (11 bit) there is one record in this bitfield
  81         DECLARE_BITMAP(match_sff, CAN_SFF_MASK + 1);
  82         int inv_match_en; // Inverted match flag
  83         struct hlist_head match_eff; // List of EFF frames to match
  84         struct tcf_result res;
  85         struct list_head link;
  86 };
  87
  88 /*
  89  * ----------------------------------------------------------------------------
  90  */
  91
  92 static void canprio_sff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
  93 {
  94         int i;
  95
  96         printk("%s() invoked\n", __FUNCTION__);
  97         mask &= CAN_SFF_MASK;
  98         canid = canid & mask;
  99
 100         if (mask == CAN_SFF_MASK) {
 101                 set_bit(canid, f->match_sff);
 102                 return;
 103         }
 104
 105         for (i = 0; i <= CAN_SFF_MASK; i++) {
 106                 if ((i & mask) == canid)
 107                         set_bit(i, f->match_sff);
 108         }
 109 }
 110
 111 static int canprio_eff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
 112 {
 113         struct canprio_eff_item *eff;
 114         int err = 0;
 115
 116         printk("%s() invoked\n", __FUNCTION__);
 117         mask &= CAN_EFF_MASK;
 118         canid = canid & mask;
 119
 120         eff = kmalloc(sizeof(struct canprio_eff_item), GFP_KERNEL);
 121         if (!eff)
 122                 return -ENOMEM;
 123
 124         spin_lock(&canprio_match_eff_lock);
 125
 126         eff->canid = canid;
 127         eff->mask = mask;
 128
 129         hlist_add_head_rcu(&eff->list, &f->match_eff);
 130
 131         spin_unlock(&canprio_match_eff_lock);
 132
 133         return err;
 134 }
 135
 136 /*
 137  * Extracts Can ID ot ouf the sk_buff structure.
 138  */
 139 static u32 canprio_get_id(struct sk_buff *skb)
 140 {
 141         /* Can ID is inside of data field */
 142         struct can_frame *cf = (struct can_frame *)skb->data;
 143         //canid_t canid = cf->canid;
 144
 145         return (u32)cf->can_id;
 146 }
 147
 148 /*
 149  * Performs the classification. Iterates over all instances of filter
 150  * checking for Can ID match.
 151  *
 152  * @skb: Socket buffer
 153  * @tp:
 154  * @res: Is used for setting Class ID as a result of classification
 155  *
 156  * Returns value relevant for policing. Used return values:
 157  *   TC_POLICE_OK if succesfully classified (without regard to policing rules)
 158  *   TC_POLICE_UNSPEC if no matching filter was found
 159  */
 160 static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
 161                           struct tcf_result *res)
 162 {
 163         struct canprio_head *head = (struct canprio_head *)tp->root;
 164         struct canprio_filter *f;
 165         u32 canid;
 166
 167         printk(" canprio_classify() invoked\n");
 168         canid = canprio_get_id(skb);
 169
 170         list_for_each_entry(f, &head->flist, link) {
 171                 bool match = false;
 172                 printk("  canprio_classify() can ID of received frame = 0x%x\n", canid);
 173
 174                 if (canid & CAN_EFF_FLAG) {
 175                         struct canprio_eff_item *effi;
 176                         struct hlist_node *next;
 177
 178                         rcu_read_lock();
 179
 180                         hlist_for_each_entry_rcu(effi, next, &f->match_eff, list) {
 181                                 if ((effi->canid ^ canid) & effi->mask) {
 182                                         match = true;
 183                                         break;
 184                                 }
 185                         }
 186
 187                         rcu_read_unlock();
 188                 } else {
 189                         match = test_bit(canid, f->match_sff);
 190                 }
 191
 192                 if (f->inv_match_en)
 193                         match = !match;
 194
 195                 if (match) {
 196                         *res = f->res;
 197                         return TC_POLICE_OK;
 198                         printk( "   canprio_classify() match ok: ID 0x%x\n", canid);
 199                 }
 200         }
 201
 202         return TC_POLICE_UNSPEC;
 203 }
 204
 205 /*
 206  * Configure filter
 207  */
 208 static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
 209                            unsigned long base, struct nlattr **tb,
 210                            struct nlattr *est)
 211 {
 212         //int err;
 213         printk(" canprio_set_parms invoked\n");
 214
 215         if (tb[TCA_CANPRIO_CLASSID]) {
 216                 f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
 217                 bitmap_zero(f->match_sff, CAN_SFF_MASK + 1);
 218                 INIT_HLIST_HEAD(&f->match_eff);
 219
 220                 tcf_bind_filter(tp, &f->res, base);
 221         }
 222
 223         return 0;
 224 }
 225
 226 /*
 227  * Looks up a filter element by its handle and returns the internal
 228  * filter ID (i.e. pointer)
 229  */
 230 static unsigned long canprio_get(struct tcf_proto *tp, u32 handle)
 231 {
 232         struct canprio_head *head = (struct canprio_head *)tp->root;
 233         struct canprio_filter *f;
 234
 235         //printk("canprio_get(%d) invoked\n", handle);
 236         if (head == NULL)
 237                 return 0UL;
 238
 239         //printk("[running for_each_entry]\n");
 240         list_for_each_entry(f, &head->flist, link) {
 241                 //printk("[f->handle = %d]\n", f->handle);
 242                 if (f->handle == handle) {
 243                         //printk("found something\n");
 244                         return (unsigned long) f;
 245                 }
 246         }
 247
 248         return 0UL;
 249 }
 250
 251 /*
 252  * Is invoked when a filter element previously referenced
 253  * with get() is no longer used
 254  */
 255 static void canprio_put(struct tcf_proto *tp, unsigned long f)
 256 {
 257 }
 258
 259
 260 static unsigned int canprio_gen_handle(struct tcf_proto *tp)
 261 {
 262         struct canprio_head *head = (struct canprio_head *)tp->root;
 263         int i = 0xFFFF;
 264
 265         while (i-- > 0) {
 266                 u32 h;
 267                 unsigned long tmp;
 268
 269                 if ((head->hgenerator += 0x10000) == 0)
 270                         head->hgenerator = 0x10000;
 271
 272                 h = head->hgenerator;
 273                 //if (canprio_get(tp, h) == 0)
 274                 //      return h;
 275                 tmp = canprio_get(tp, h);
 276                 //printk("___tried %d result %lu\n", h, tmp);
 277                 if (tmp == 0)
 278                         return h;
 279         }
 280         return 0;
 281 }
 282
 283
 284 /*
 285  * Called for changing properties of an existing filter or after addition
 286  * of a new filter to a class (by calling bind_tcf which binds an instance
 287  * of a filter to the class).
 288  *
 289  * @tp:     Structure representing instance of a filter.
 290  *          Part of a linked list of all filters.
 291  * @base:
 292  * @handle:
 293  * @tca:    Messages passed through the Netlink from userspace.
 294  * @arg:    ??? FIXME
 295  */
 296 static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
 297                           struct nlattr **tca, unsigned long *arg)
 298 {
 299         int err;
 300         struct canprio_head *head = (struct canprio_head *)tp->root;
 301         struct canprio_filter *f = (struct canprio_filter *)*arg;
 302         struct nlattr *tb[TCA_CANPRIO_MAX + 1];
 303         int i;
 304
 305         printk(" canprio_change invoked\n");
 306
 307         if (tca[TCA_OPTIONS] == NULL)
 308                 return -EINVAL;
 309
 310         /*Parses a stream of attributes and stores a pointer to each attribute in
 311           the tb array accessible via the attribute type. Policy may be set to NULL
 312           if no validation is required.*/
 313         err = nla_parse_nested(tb, TCA_CANPRIO_MAX, tca[TCA_OPTIONS], canprio_policy);
 314         if (err < 0)
 315                 return err;
 316
 317         //Change existing filter
 318         if (f != NULL) {
 319                 if (handle && f->handle != handle)
 320                         return -EINVAL;
 321                 return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 322         }
 323
 324         //Create new filter
 325         err = -ENOBUFS;
 326         f = kzalloc(sizeof(*f), GFP_KERNEL);
 327         if (f == NULL)
 328                 goto errout;
 329
 330
 331         err = -EINVAL;
 332         if (handle)
 333                 f->handle = handle;
 334         else {
 335                 u32 handle;
 336         /*
 337                 unsigned int i = 0x80000000;
 338                 do {
 339                         if (++head->hgenerator == 0x7FFFFFFF)
 340                                 head->hgenerator = 1;
 341                 } while (--i > 0 && canprio_get(tp, head->hgenerator));
 342
 343                 if (i <= 0) {
 344                         pr_err("Insufficient number of handles\n");
 345                         goto errout;
 346                 }
 347         */
 348                 handle = canprio_gen_handle(tp);
 349                 //printk("__new handle %d\n", handle);
 350                 f->handle = handle;
 351                 //FIXME where is hgenerator initialized
 352         }
 353
 354
 355         //Configure newly created filter
 356         err = canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 357         if (err < 0)
 358                 goto errout;
 359
 360
 361         //Parse arguments
 362         if (tb[TCA_CANPRIO_RULES] == NULL)
 363                 return -EINVAL;
 364
 365         canprio_rules = nla_data(tb[TCA_CANPRIO_RULES]);
 366         canprio_rules_count = (nla_len(tb[TCA_CANPRIO_RULES]) / sizeof(struct can_filter));
 367         printk(" rules_count = %u\n", canprio_rules_count);
 368
 369         for (i = 0; i < canprio_rules_count; i++) {
 370                 /* FIXME: shouldn't use here the same logic as in
 371                  * can_rcv_filter() to filter for various combination
 372                  * of flags (EFF, RTR) */
 373                 if (canprio_rules[i].can_id & CAN_EFF_FLAG) {
 374                         err = canprio_eff_match_add(f, canprio_rules[i].can_id, canprio_rules[i].can_mask);
 375                         if (err < 0)
 376                                 goto errout;
 377                 } else {
 378                         canprio_sff_match_add(f, canprio_rules[i].can_id, canprio_rules[i].can_mask);
 379                 }
 380
 381                 printk(" can ID to match = 0x%x with mask 0x%x\n",
 382                         canprio_rules[i].can_id, canprio_rules[i].can_mask);
 383         }
 384
 385         f->inv_match_en = 0;
 386         if (tb[TCA_CANPRIO_INV_EN] != NULL)
 387                 f->inv_match_en = nla_get_u32(tb[TCA_CANPRIO_INV_EN]);
 388
 389         //Add newly created filter to list of all filters
 390         tcf_tree_lock(tp);
 391         list_add(&f->link, &head->flist);
 392         tcf_tree_unlock(tp);
 393         *arg = (unsigned long) f;
 394
 395         return 0;
 396
 397 errout:
 398         if (*arg == 0UL && f)
 399                 kfree(f);
 400
 401         return err;
 402 }
 403
 404
 405 static void canprio_delete_filter(struct tcf_proto *tp, struct canprio_filter *f)
 406 {
 407         struct canprio_eff_item *effi;
 408         struct hlist_node *p, *n;
 409
 410         rcu_barrier();
 411
 412         tcf_unbind_filter(tp, &f->res);
 413         hlist_for_each_entry_safe(effi, p, n, &f->match_eff, list) {
 414                 kfree(effi);
 415         }
 416
 417         kfree(f);
 418 }
 419
 420 /*
 421  * Remove whole filter.
 422  */
 423 static void canprio_destroy(struct tcf_proto *tp)
 424 {
 425         struct canprio_head *head = tp->root;
 426         struct canprio_filter *f, *n;
 427
 428         list_for_each_entry_safe(f, n, &head->flist, link) {
 429                 list_del(&f->link);
 430                 canprio_delete_filter(tp, f);
 431         }
 432         kfree(head);
 433 }
 434
 435 /*
 436  * Delete one instance of a filter.
 437  */
 438 static int canprio_delete(struct tcf_proto *tp, unsigned long arg)
 439 {
 440         struct canprio_head *head = (struct canprio_head *)tp->root;
 441         struct canprio_filter *t;
 442         struct canprio_filter *f = (struct canprio_filter *)arg;
 443
 444         rcu_barrier(); /* Wait for completion of call_rcu()'s */
 445
 446         list_for_each_entry(t, &head->flist, link)
 447                 if (t == f) {
 448                         tcf_tree_lock(tp);
 449                         list_del(&t->link);
 450                         tcf_tree_unlock(tp);
 451                         canprio_delete_filter(tp, t);
 452                         return 0;
 453                 }
 454
 455         return -ENOENT;
 456 }
 457
 458
 459 /*
 460  * Initialize filter
 461  */
 462 static int canprio_init(struct tcf_proto *tp)
 463 {
 464         struct canprio_head *head;
 465         //printk("tp = %p\n", tp);
 466         printk(" canprio_init invoked\n");
 467
 468         head = kzalloc(sizeof(*head), GFP_KERNEL);
 469         if (head == NULL)
 470                 return -ENOBUFS;
 471
 472         INIT_LIST_HEAD(&head->flist);
 473         tp->root = head;
 474         tp->protocol = htons(ETH_P_CAN); /* Work only on AF_CAN packets - not tested! */
 475
 476         return 0;
 477 }
 478
 479 /*
 480  * Iterates over all elements of a filter and invokes a callback function
 481  * for each of them. This is used to obtain diagnostic data
 482  */
 483 static void canprio_walk(struct tcf_proto *tp, struct tcf_walker *arg)
 484 {
 485         struct canprio_head *head = (struct canprio_head *) tp->root;
 486         struct canprio_filter *f;
 487
 488         printk("%s() invoked\n", __FUNCTION__);
 489
 490         list_for_each_entry(f, &head->flist, link) {
 491                 if (arg->count < arg->skip)
 492                         goto skip;
 493
 494                 printk("calling canprio_dump()\n");
 495                 if (arg->fn(tp, (unsigned long) f, arg) < 0) {
 496                         arg->stop = 1;
 497                         break;
 498                 }
 499 skip:
 500                 arg->count++;
 501         }
 502 }
 503
 504 /*
 505  * Returns diagnostic data for a filter or one of its elements.
 506  */
 507 static int canprio_dump(struct tcf_proto *tp, unsigned long fh,
 508                     struct sk_buff *skb, struct tcmsg *t)
 509 {
 510         struct canprio_filter *f = (struct canprio_filter *) fh;
 511         struct nlattr *nest;
 512
 513         printk("%s() invoked\n", __FUNCTION__);
 514
 515         if (f == NULL)
 516                 return skb->len;
 517
 518         t->tcm_handle = f->handle;
 519
 520         nest = nla_nest_start(skb, TCA_OPTIONS);
 521         if (nest == NULL)
 522                 goto nla_put_failure;
 523
 524         if (f->res.classid)
 525                 NLA_PUT_U32(skb, TCA_CANPRIO_CLASSID, f->res.classid);
 526
 527
 528         //NLA_PUT(skb, TCA_CANPRIO_RULES, canprio_rules_count * sizeof(struct canprio_rule),
 529         //      canprio_rules);
 530
 531         /* ...
 532                 ... */
 533
 534         nla_nest_end(skb, nest);
 535
 536         return skb->len;
 537
 538 nla_put_failure:
 539         nla_nest_cancel(skb, nest);
 540         return -1;
 541 }
 542
 543
 544 static struct tcf_proto_ops cls_canprio_ops __read_mostly = {
 545         .kind           =       "canprio",
 546         .classify       =       canprio_classify,
 547         .init           =       canprio_init,
 548         .destroy        =       canprio_destroy,
 549         .get            =       canprio_get,
 550         .put            =       canprio_put,
 551         .change         =       canprio_change,
 552         .delete         =       canprio_delete,
 553         .walk           =       canprio_walk,
 554         .dump           =       canprio_dump,
 555         .owner          =       THIS_MODULE,
 556 };
 557
 558 static int __init init_canprio(void)
 559 {
 560         printk("Canprio loaded\n");
 561         return register_tcf_proto_ops(&cls_canprio_ops);
 562 }
 563
 564 static void __exit exit_canprio(void)
 565 {
 566         printk("Canprio removed\n");
 567         unregister_tcf_proto_ops(&cls_canprio_ops);
 568 }
 569
 570 module_init(init_canprio);
 571 module_exit(exit_canprio);
 572 MODULE_LICENSE("GPL");
 573 MODULE_AUTHOR(""); // FIXME
 574 MODULE_DESCRIPTION("Controller Area Network can_id classifier");
 575