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