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