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