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