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