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  * canfltr_get_id() - 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  * canfltr_classify() - Performs the classification.
 144  *
 145  * @skb: Socket buffer
 146  * @tp:
 147  * @res: Is used for setting Class ID as a result of classification
 148  *
 149  * Iterates over all instances of filter, checking for CAN ID match.
 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, const 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  * canfltr_get() - Looks up a filter element by its handle and returns the
 211  * internal 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  * canfltr_put() - 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;
 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  * canfltr_change() - Called for changing properties of an existing filter or
 347  * after addition of a new filter to a class (by calling bind_tcf which binds
 348  * an instance 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  * canfltr_destroy() - 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  * canfltr_delete() - 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  * canfltr_init() - Initialize filter
 475  */
 476 static int canfltr_init(struct tcf_proto *tp)
 477 {
 478         struct canfltr_head *head;
 479
 480         if ((tp->protocol != htons(ETH_P_ALL)) && (tp->protocol != htons(ETH_P_CAN)))
 481                 return -1;
 482
 483         /* Work only on CAN frames */
 484         if (tp->protocol == htons(ETH_P_ALL))
 485                 tp->protocol = htons(ETH_P_CAN);
 486
 487         head = kzalloc(sizeof(*head), GFP_KERNEL);
 488         if (head == NULL)
 489                 return -ENOBUFS;
 490
 491         INIT_LIST_HEAD(&head->flist);
 492         tp->root = head;
 493
 494         return 0;
 495 }
 496
 497 /**
 498  * canfltr_walk() - Iterates over all elements of a filter and invokes a
 499  * callback function for each of them. This is used to obtain diagnostic data.
 500  */
 501 static void canfltr_walk(struct tcf_proto *tp, struct tcf_walker *arg)
 502 {
 503         struct canfltr_head *head = (struct canfltr_head *) tp->root;
 504         struct canfltr_state *f;
 505
 506         list_for_each_entry(f, &head->flist, link) {
 507                 if (arg->count < arg->skip)
 508                         goto skip;
 509
 510                 if (arg->fn(tp, (unsigned long) f, arg) < 0) {
 511                         arg->stop = 1;
 512                         break;
 513                 }
 514 skip:
 515                 arg->count++;
 516         }
 517 }
 518
 519 /**
 520  * canfltr_dump() - Returns diagnostic data for a filter or one of its elements.
 521  */
 522 static int canfltr_dump(struct tcf_proto *tp, unsigned long fh,
 523                         struct sk_buff *skb, struct tcmsg *t)
 524 {
 525         struct canfltr_state *f = (struct canfltr_state *) fh;
 526         struct nlattr *nest;
 527         struct canfltr_rules *r;
 528
 529         if (f == NULL)
 530                 return skb->len;
 531
 532         rcu_read_lock();
 533         r = rcu_dereference(f->rules);
 534         t->tcm_handle = f->handle;
 535
 536         nest = nla_nest_start(skb, TCA_OPTIONS);
 537         if (nest == NULL)
 538                 goto nla_put_failure;
 539
 540         if (f->res.classid)
 541                 NLA_PUT_U32(skb, TCA_CANFLTR_CLASSID, f->res.classid);
 542
 543         NLA_PUT(skb, TCA_CANFLTR_RULES, r->rules_count *
 544                 sizeof(struct can_filter), r->rules_raw);
 545
 546
 547         nla_nest_end(skb, nest);
 548
 549         rcu_read_unlock();
 550         return skb->len;
 551
 552 nla_put_failure:
 553         nla_nest_cancel(skb, nest);
 554         rcu_read_unlock();
 555         return -1;
 556 }
 557
 558
 559 static struct tcf_proto_ops cls_canfltr_ops __read_mostly = {
 560         .kind           =       "can",
 561         .classify       =       canfltr_classify,
 562         .init           =       canfltr_init,
 563         .destroy        =       canfltr_destroy,
 564         .get            =       canfltr_get,
 565         .put            =       canfltr_put,
 566         .change         =       canfltr_change,
 567         .delete         =       canfltr_delete,
 568         .walk           =       canfltr_walk,
 569         .dump           =       canfltr_dump,
 570         .owner          =       THIS_MODULE,
 571 };
 572
 573 static int __init init_canfltr(void)
 574 {
 575         pr_debug("canfltr: CAN filter loaded\n");
 576 #ifdef SFF_BITMAP
 577         pr_debug("canfltr: SFF rules stored in bitmap\n");
 578 #else
 579         pr_debug("canfltr: SFF rules stored in array\n");
 580 #endif
 581         return register_tcf_proto_ops(&cls_canfltr_ops);
 582 }
 583
 584 static void __exit exit_canfltr(void)
 585 {
 586         pr_debug("canfltr: CAN filter removed\n");
 587         unregister_tcf_proto_ops(&cls_canfltr_ops);
 588 }
 589
 590 module_init(init_canfltr);
 591 module_exit(exit_canfltr);
 592 MODULE_LICENSE("GPL");
 593 MODULE_AUTHOR("Rostislav Lisovy <lisovy@gmail.cz>");
 594 MODULE_DESCRIPTION("Controller Area Network classifier");