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