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