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 static unsigned int canfltr_gen_handle(struct tcf_proto *tp)
 215 {
 216         struct canfltr_head *head = (struct canfltr_head *)tp->root;
 217         int i = 0xFFFF;
 218
 219         while (i-- > 0) {
 220                 u32 h;
 221
 222                 head->hgenerator += 0x10000;
 223                 if (head->hgenerator == 0)
 224                         head->hgenerator = 0x10000;
 225
 226                 h = head->hgenerator;
 227                 if (canfltr_get(tp, h) == 0)
 228                         return h;
 229         }
 230         return 0;
 231 }
 232
 233 static void canfltr_rules_free_rcu(struct rcu_head *rcu)
 234 {
 235         kfree(container_of(rcu, struct canfltr_rules, rcu));
 236 }
 237
 238 static int canfltr_set_parms(struct tcf_proto *tp, struct canfltr_state *f,
 239                                 unsigned long base, struct nlattr **tb,
 240                                 struct nlattr *est)
 241 {
 242         struct can_filter *canfltr_nl_rules;
 243         struct canfltr_rules *rules_tmp;
 244         int err;
 245         int i;
 246
 247         rules_tmp = kzalloc(sizeof(*rules_tmp), GFP_KERNEL);
 248         if (!rules_tmp)
 249                 return -ENOBUFS;
 250
 251         err = -EINVAL;
 252         if (tb[TCA_CANFLTR_CLASSID] == NULL)
 253                 goto errout;
 254
 255         if (tb[TCA_CANFLTR_RULES]) {
 256                 canfltr_nl_rules = nla_data(tb[TCA_CANFLTR_RULES]);
 257                 rules_tmp->sff_rules_count = 0;
 258                 rules_tmp->eff_rules_count = 0;
 259                 rules_tmp->rules_count = (nla_len(tb[TCA_CANFLTR_RULES]) /
 260                         sizeof(struct can_filter));
 261
 262                 rules_tmp->rules_raw = kzalloc(sizeof(struct can_filter) *
 263                         rules_tmp->rules_count, GFP_KERNEL);
 264                 err = -ENOMEM;
 265                 if (rules_tmp->rules_raw == NULL)
 266                         goto errout;
 267
 268                 /* We need two for() loops for copying rules into
 269                 two contiguous areas in rules_raw */
 270
 271                 /* Process EFF frame rules*/
 272                 for (i = 0; i < rules_tmp->rules_count; i++) {
 273                         if ((canfltr_nl_rules[i].can_id & CAN_EFF_FLAG) &&
 274                             (canfltr_nl_rules[i].can_mask & CAN_EFF_FLAG)) {
 275                                 memcpy(rules_tmp->rules_raw +
 276                                         rules_tmp->eff_rules_count,
 277                                         &canfltr_nl_rules[i],
 278                                         sizeof(struct can_filter));
 279                                 rules_tmp->eff_rules_count++;
 280                         } else {
 281                                 continue;
 282                         }
 283                 }
 284
 285                 /* Process SFF 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                                 continue;
 290                         } else {
 291                                 memcpy(rules_tmp->rules_raw +
 292                                         rules_tmp->eff_rules_count +
 293                                         rules_tmp->sff_rules_count,
 294                                         &canfltr_nl_rules[i],
 295                                         sizeof(struct can_filter));
 296                                 rules_tmp->sff_rules_count++;
 297                                 canfltr_sff_match_add(rules_tmp,
 298                                         canfltr_nl_rules[i].can_id,
 299                                         canfltr_nl_rules[i].can_mask);
 300                         }
 301                 }
 302         }
 303
 304
 305         /* Setting parameters for newly created filter */
 306         if (f->rules == NULL) {
 307                 rcu_assign_pointer(f->rules, rules_tmp);
 308         } else { /* Changing existing filter */
 309                 struct canfltr_rules *rules_old;
 310
 311                 rules_old = xchg(&f->rules, rules_tmp);
 312                 call_rcu(&rules_old->rcu, canfltr_rules_free_rcu);
 313         }
 314
 315         return 0;
 316
 317 errout:
 318         kfree(rules_tmp);
 319         return err;
 320 }
 321
 322 /**
 323  * canfltr_change() - Called for changing properties of an existing filter or
 324  * after addition of a new filter to a class (by calling bind_tcf which binds
 325  * an instance of a filter to the class).
 326  *
 327  * @tp:     Structure representing instance of a filter.
 328  *          Part of a linked list of all filters.
 329  * @base:
 330  * @handle:
 331  * @tca:    Messages passed through the Netlink from userspace.
 332  * @arg:
 333  */
 334 static int canfltr_change(struct tcf_proto *tp, unsigned long base, u32 handle,
 335                           struct nlattr **tca, unsigned long *arg)
 336 {
 337         struct canfltr_head *head = (struct canfltr_head *)tp->root;
 338         struct canfltr_state *f = (struct canfltr_state *)*arg;
 339         struct nlattr *tb[TCA_CANFLTR_MAX + 1];
 340         int err;
 341
 342         if (tca[TCA_OPTIONS] == NULL)
 343                 return -EINVAL;
 344
 345         /* Parses a stream of attributes and stores a pointer to each
 346         attribute in the tb array accessible via the attribute type.
 347         Policy may be set to NULL if no validation is required.*/
 348         err = nla_parse_nested(tb, TCA_CANFLTR_MAX, tca[TCA_OPTIONS],
 349                 canfltr_policy);
 350         if (err < 0)
 351                 return err;
 352         /* Change existing filter (remove all settings and add
 353         them thereafter as if filter was newly created) */
 354         if (f != NULL) {
 355                 if (handle && f->handle != handle)
 356                         return -EINVAL;
 357
 358                 return canfltr_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 359         }
 360
 361         /* Create new filter */
 362         err = -ENOBUFS;
 363         f = kzalloc(sizeof(*f), GFP_KERNEL);
 364         if (f == NULL)
 365                 goto errout;
 366
 367         if (tb[TCA_CANFLTR_CLASSID]) {
 368                 f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
 369                 tcf_bind_filter(tp, &f->res, base);
 370         }
 371
 372         err = -EINVAL;
 373         if (handle) /* handle passed from userspace */
 374                 f->handle = handle;
 375         else {
 376                 f->handle = canfltr_gen_handle(tp);
 377         }
 378
 379         /* Configure filter */
 380         err = canfltr_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 381         if (err < 0)
 382                 goto errout;
 383
 384         /* Add newly created filter to list of all filters */
 385         tcf_tree_lock(tp);
 386         list_add(&f->link, &head->flist);
 387         tcf_tree_unlock(tp);
 388         *arg = (unsigned long) f;
 389
 390         return 0;
 391
 392 errout:
 393         if (*arg == 0UL && f)
 394                 kfree(f);
 395
 396         return err;
 397 }
 398
 399
 400 static void canfltr_delete_filter(struct tcf_proto *tp,
 401                                 struct canfltr_state *f)
 402 {
 403         tcf_unbind_filter(tp, &f->res);
 404
 405         rcu_barrier();
 406         kfree(f->rules->rules_raw);
 407         kfree(f->rules);
 408         kfree(f);
 409 }
 410
 411 /**
 412  * canfltr_destroy() - Remove whole filter.
 413  */
 414 static void canfltr_destroy(struct tcf_proto *tp)
 415 {
 416         struct canfltr_head *head = tp->root;
 417         struct canfltr_state *f, *n;
 418
 419         list_for_each_entry_safe(f, n, &head->flist, link) {
 420                 list_del(&f->link);
 421                 canfltr_delete_filter(tp, f);
 422         }
 423         kfree(head);
 424 }
 425
 426 /**
 427  * canfltr_delete() - Delete one instance of a filter.
 428  */
 429 static int canfltr_delete(struct tcf_proto *tp, unsigned long arg)
 430 {
 431         struct canfltr_head *head = (struct canfltr_head *)tp->root;
 432         struct canfltr_state *t;
 433         struct canfltr_state *f = (struct canfltr_state *)arg;
 434
 435         rcu_barrier(); /* Wait for completion of call_rcu()'s */
 436
 437         list_for_each_entry(t, &head->flist, link)
 438                 if (t == f) {
 439                         tcf_tree_lock(tp);
 440                         list_del(&t->link);
 441                         tcf_tree_unlock(tp);
 442                         canfltr_delete_filter(tp, t);
 443                         return 0;
 444                 }
 445
 446         return -ENOENT;
 447 }
 448
 449
 450 /**
 451  * canfltr_init() - Initialize filter
 452  */
 453 static int canfltr_init(struct tcf_proto *tp)
 454 {
 455         struct canfltr_head *head;
 456
 457         if ((tp->protocol != htons(ETH_P_ALL)) && (tp->protocol != htons(ETH_P_CAN)))
 458                 return -1;
 459
 460         /* Work only on CAN frames */
 461         if (tp->protocol == htons(ETH_P_ALL))
 462                 tp->protocol = htons(ETH_P_CAN);
 463
 464         head = kzalloc(sizeof(*head), GFP_KERNEL);
 465         if (head == NULL)
 466                 return -ENOBUFS;
 467
 468         INIT_LIST_HEAD(&head->flist);
 469         tp->root = head;
 470
 471         return 0;
 472 }
 473
 474 /**
 475  * canfltr_walk() - Iterates over all elements of a filter and invokes a
 476  * callback function for each of them. This is used to obtain diagnostic data.
 477  */
 478 static void canfltr_walk(struct tcf_proto *tp, struct tcf_walker *arg)
 479 {
 480         struct canfltr_head *head = (struct canfltr_head *) tp->root;
 481         struct canfltr_state *f;
 482
 483         list_for_each_entry(f, &head->flist, link) {
 484                 if (arg->count < arg->skip)
 485                         goto skip;
 486
 487                 if (arg->fn(tp, (unsigned long) f, arg) < 0) {
 488                         arg->stop = 1;
 489                         break;
 490                 }
 491 skip:
 492                 arg->count++;
 493         }
 494 }
 495
 496 /**
 497  * canfltr_dump() - Returns diagnostic data for a filter or one of its elements.
 498  */
 499 static int canfltr_dump(struct tcf_proto *tp, unsigned long fh,
 500                         struct sk_buff *skb, struct tcmsg *t)
 501 {
 502         struct canfltr_state *f = (struct canfltr_state *) fh;
 503         struct nlattr *nest;
 504         struct canfltr_rules *r;
 505
 506         if (f == NULL)
 507                 return skb->len;
 508
 509         rcu_read_lock();
 510         r = rcu_dereference(f->rules);
 511         t->tcm_handle = f->handle;
 512
 513         nest = nla_nest_start(skb, TCA_OPTIONS);
 514         if (nest == NULL)
 515                 goto nla_put_failure;
 516
 517         if (f->res.classid)
 518                 NLA_PUT_U32(skb, TCA_CANFLTR_CLASSID, f->res.classid);
 519
 520         NLA_PUT(skb, TCA_CANFLTR_RULES, r->rules_count *
 521                 sizeof(struct can_filter), r->rules_raw);
 522
 523
 524         nla_nest_end(skb, nest);
 525
 526         rcu_read_unlock();
 527         return skb->len;
 528
 529 nla_put_failure:
 530         nla_nest_cancel(skb, nest);
 531         rcu_read_unlock();
 532         return -1;
 533 }
 534
 535
 536 static struct tcf_proto_ops cls_canfltr_ops __read_mostly = {
 537         .kind           =       "can",
 538         .classify       =       canfltr_classify,
 539         .init           =       canfltr_init,
 540         .destroy        =       canfltr_destroy,
 541         .get            =       canfltr_get,
 542         .put            =       canfltr_put,
 543         .change         =       canfltr_change,
 544         .delete         =       canfltr_delete,
 545         .walk           =       canfltr_walk,
 546         .dump           =       canfltr_dump,
 547         .owner          =       THIS_MODULE,
 548 };
 549
 550 static int __init init_canfltr(void)
 551 {
 552         pr_debug("canfltr: CAN filter loaded\n");
 553         return register_tcf_proto_ops(&cls_canfltr_ops);
 554 }
 555
 556 static void __exit exit_canfltr(void)
 557 {
 558         pr_debug("canfltr: CAN filter removed\n");
 559         unregister_tcf_proto_ops(&cls_canfltr_ops);
 560 }
 561
 562 module_init(init_canfltr);
 563 module_exit(exit_canfltr);
 564 MODULE_LICENSE("GPL");
 565 MODULE_AUTHOR("Rostislav Lisovy <lisovy@gmail.cz>");
 566 MODULE_DESCRIPTION("Controller Area Network classifier");