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