net/sched/cls_can.c

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