net/sched/cls_canprio.c

   1 /* cls_canprio.c  -- Canprio classifier.
   2  * Makes decisions accoring to Can IDs.
   3  *
   4  * Implementation notes;
   5  *   parameter of functions named "base" is pointer to some parent element
   6  *
   7  */
   8
   9 #include <linux/module.h>
  10 #include <linux/slab.h>
  11 #include <linux/types.h>
  12 #include <linux/kernel.h>
  13 #include <linux/string.h>
  14 #include <linux/errno.h>
  15 #include <linux/rtnetlink.h>
  16 #include <linux/skbuff.h>
  17 #include <net/netlink.h>
  18 #include <net/act_api.h>
  19 #include <net/pkt_cls.h>
  20 #include <linux/can.h>
  21 #include <linux/bitmap.h>
  22 #include <linux/spinlock.h>
  23 #include <linux/rcupdate.h>
  24
  25 //--------------------------------------
  26 //FIXME put in some *.h file shared with userspace "tc"?
  27 struct canprio_rule {
  28         __u32 canid;
  29         __u32 canid_mask;
  30 };
  31
  32 struct canprio_rule *canprio_rules;
  33 int canprio_rules_count = 0;
  34 //--------------------------------------
  35
  36 /* Definition of Netlink messages */
  37 enum {
  38         TCA_CANPRIO_A_UNSPEC,
  39         TCA_CANPRIO_CLASSID,
  40         TCA_CANPRIO_RULES,
  41         TCA_CANPRIO_INV_EN, /* enable inverse rules */
  42         __TCA_CANPRIO_MAX,
  43 };
  44 #define TCA_CANPRIO_MAX (__TCA_CANPRIO_MAX - 1)
  45
  46 static const struct nla_policy canprio_policy[TCA_CANPRIO_MAX + 1] = {
  47         [TCA_CANPRIO_CLASSID]    = { .type = NLA_U32 },
  48         //FIXME Be aware of possible problems with 64bit kernel and 32bit userspace etc.
  49         [TCA_CANPRIO_RULES]      = { /*.len = (sizeof(struct canprio_rule)*32)*/ }, /* FIXME */
  50         [TCA_CANPRIO_INV_EN]     = { .type = NLA_U32 },
  51 };
  52
  53 struct canprio_eff_item {
  54         struct hlist_node list;
  55         struct rcu_head rcu;
  56         u32 canid;
  57         u32 mask;
  58 };
  59 static DEFINE_SPINLOCK(canprio_match_eff_lock);
  60
  61 struct canprio_head {
  62         u32 hgenerator;
  63         struct list_head flist;
  64 };
  65
  66 struct canprio_filter {
  67         u32 handle;
  68         // For each SFF Can ID (11 bit) there is one record in this bitfield
  69         DECLARE_BITMAP(match_sff, CAN_SFF_MASK + 1);
  70         int inv_match_en; // Inverted match flag
  71         struct hlist_head match_eff; // List of EFF frames to match
  72         struct tcf_result res;
  73         struct list_head link;
  74 };
  75
  76 /*
  77  * ----------------------------------------------------------------------------
  78  */
  79
  80 static void sff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
  81 {
  82         int i;
  83
  84         printk("%s() invoked\n", __FUNCTION__);
  85         mask &= CAN_SFF_MASK;
  86         canid = canid & mask;
  87
  88         if (mask == CAN_SFF_MASK) {
  89                 set_bit(canid, f->match_sff);
  90                 return;
  91         }
  92
  93         for (i = 0; i <= CAN_SFF_MASK; i++) {
  94                 if ((i & mask) == canid)
  95                         set_bit(i, f->match_sff);
  96         }
  97 }
  98
  99 static int eff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
 100 {
 101         struct canprio_eff_item *eff;
 102         int err = 0;
 103
 104         printk("%s() invoked\n", __FUNCTION__);
 105         mask &= CAN_EFF_MASK;
 106         canid = canid & mask;
 107
 108         eff = kmalloc(sizeof(struct canprio_eff_item), GFP_KERNEL);
 109         if (!eff)
 110                 return -ENOMEM;
 111
 112         spin_lock(&canprio_match_eff_lock);
 113
 114         eff->canid = canid;
 115         eff->mask = mask;
 116
 117         hlist_add_head_rcu(&eff->list, &f->match_eff);
 118
 119         spin_unlock(&canprio_match_eff_lock);
 120
 121         return err;
 122 }
 123
 124 /*
 125  * Extracts Can ID ot ouf the sk_buff structure.
 126  */
 127 static u32 canprio_get_id(struct sk_buff *skb)
 128 {
 129         /* Can ID is inside of data field */
 130         struct can_frame *cf = (struct can_frame *)skb->data;
 131         //canid_t canid = cf->canid;
 132
 133         return (u32)cf->can_id;
 134 }
 135
 136 /*
 137  * Performs the classification. Iterates over all instances of filter
 138  * checking for Can ID match.
 139  *
 140  * @skb: Socket buffer
 141  * @tp:
 142  * @res: Is used for setting Class ID as a result of classification
 143  *
 144  * Returns value relevant for policing. Used return values:
 145  *   TC_POLICE_OK if succesfully classified (without regard to policing rules)
 146  *   TC_POLICE_UNSPEC if no matching filter was found
 147  */
 148 static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
 149                           struct tcf_result *res)
 150 {
 151         struct canprio_head *head = (struct canprio_head *)tp->root;
 152         struct canprio_filter *f;
 153         u32 canid;
 154
 155         printk(" canprio_classify() invoked\n");
 156         canid = canprio_get_id(skb);
 157
 158         list_for_each_entry(f, &head->flist, link) {
 159                 bool match = false;
 160                 printk("  canprio_classify() can ID of received frame = 0x%x\n", canid);
 161
 162                 if (canid & CAN_EFF_FLAG) {
 163                         struct canprio_eff_item *effi;
 164                         struct hlist_node *next;
 165
 166                         rcu_read_lock();
 167
 168                         hlist_for_each_entry_rcu(effi, next, &f->match_eff, list) {
 169                                 if ((effi->canid ^ canid) & effi->mask) {
 170                                         match = true;
 171                                         break;
 172                                 }
 173                         }
 174
 175                         rcu_read_unlock();
 176                 } else {
 177                         match = test_bit(canid, f->match_sff);
 178                 }
 179
 180                 if (f->inv_match_en)
 181                         match = !match;
 182
 183                 if (match) {
 184                         *res = f->res;
 185                         return TC_POLICE_OK;
 186                         printk( "   canprio_classify() match ok: ID 0x%x\n", canid);
 187                 }
 188         }
 189
 190         return TC_POLICE_UNSPEC;
 191 }
 192
 193 /*
 194  * Configure filter
 195  */
 196 static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
 197                            unsigned long base, struct nlattr **tb,
 198                            struct nlattr *est)
 199 {
 200         //int err;
 201         printk(" canprio_set_parms invoked\n");
 202
 203         if (tb[TCA_CANPRIO_CLASSID]) {
 204                 f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
 205                 bitmap_zero(f->match_sff, CAN_SFF_MASK + 1);
 206                 INIT_HLIST_HEAD(&f->match_eff);
 207
 208                 tcf_bind_filter(tp, &f->res, base);
 209         }
 210
 211         return 0;
 212 }
 213
 214 /*
 215  * Looks up a filter element by its handle and returns the internal
 216  * filter ID (i.e. pointer)
 217  */
 218 static unsigned long canprio_get(struct tcf_proto *tp, u32 handle)
 219 {
 220         struct canprio_head *head = (struct canprio_head *)tp->root;
 221         struct canprio_filter *f;
 222
 223         //printk("canprio_get(%d) invoked\n", handle);
 224         if (head == NULL)
 225                 return 0UL;
 226
 227         //printk("[running for_each_entry]\n");
 228         list_for_each_entry(f, &head->flist, link) {
 229                 //printk("[f->handle = %d]\n", f->handle);
 230                 if (f->handle == handle) {
 231                         //printk("found something\n");
 232                         return (unsigned long) f;
 233                 }
 234         }
 235
 236         return 0UL;
 237 }
 238
 239 /*
 240  * Is invoked when a filter element previously referenced
 241  * with get() is no longer used
 242  */
 243 static void canprio_put(struct tcf_proto *tp, unsigned long f)
 244 {
 245 }
 246
 247
 248 /* FIXME all functions with prefixes? */
 249 static unsigned int gen_handle(struct tcf_proto *tp)
 250 {
 251         struct canprio_head *head = (struct canprio_head *)tp->root;
 252         int i = 0xFFFF;
 253
 254         while (i-- > 0) {
 255                 u32 h;
 256                 unsigned long tmp;
 257
 258                 if ((head->hgenerator += 0x10000) == 0)
 259                         head->hgenerator = 0x10000;
 260
 261                 h = head->hgenerator;
 262                 //if (canprio_get(tp, h) == 0)
 263                 //      return h;
 264                 tmp = canprio_get(tp, h);
 265                 //printk("___tried %d result %lu\n", h, tmp);
 266                 if (tmp == 0)
 267                         return h;
 268         }
 269         return 0;
 270 }
 271
 272
 273 /*
 274  * Called for changing properties of an existing filter or after addition
 275  * of a new filter to a class (by calling bind_tcf which binds an instance
 276  * of a filter to the class).
 277  *
 278  * @tp:     Structure representing instance of a filter.
 279  *          Part of a linked list of all filters.
 280  * @base:
 281  * @handle:
 282  * @tca:    Messages passed through the Netlink from userspace.
 283  * @arg:    ??? FIXME
 284  */
 285 static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
 286                           struct nlattr **tca, unsigned long *arg)
 287 {
 288         int err;
 289         struct canprio_head *head = (struct canprio_head *)tp->root;
 290         struct canprio_filter *f = (struct canprio_filter *)*arg;
 291         struct nlattr *tb[TCA_CANPRIO_MAX + 1];
 292         int i;
 293
 294         printk(" canprio_change invoked\n");
 295
 296         if (tca[TCA_OPTIONS] == NULL)
 297                 return -EINVAL;
 298
 299         //Parses a stream of attributes and stores a pointer to each attribute in
 300         //the tb array accessible via the attribute type. Policy may be set to NULL
 301         //if no validation is required.
 302         err = nla_parse_nested(tb, TCA_CANPRIO_MAX, tca[TCA_OPTIONS], canprio_policy);
 303         if (err < 0)
 304                 return err;
 305
 306         //Change existing filter (?)
 307         if (f != NULL) {
 308                 if (handle && f->handle != handle)
 309                         return -EINVAL;
 310                 return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 311         }
 312
 313         //Create new filter (?)
 314         err = -ENOBUFS;
 315         f = kzalloc(sizeof(*f), GFP_KERNEL);
 316         if (f == NULL)
 317                 goto errout;
 318
 319
 320         //FIXME why?
 321         err = -EINVAL;
 322         if (handle)
 323                 f->handle = handle;
 324         else {
 325                 u32 handle;
 326         /*
 327                 unsigned int i = 0x80000000;
 328                 do {
 329                         if (++head->hgenerator == 0x7FFFFFFF)
 330                                 head->hgenerator = 1;
 331                 } while (--i > 0 && canprio_get(tp, head->hgenerator));
 332
 333                 if (i <= 0) {
 334                         pr_err("Insufficient number of handles\n");
 335                         goto errout;
 336                 }
 337         */
 338                 handle = gen_handle(tp);
 339                 //printk("__new handle %d\n", handle);
 340                 f->handle = handle;
 341                 //FIXME where is hgenerator initialized
 342         }
 343
 344
 345         //Configure newly created filter
 346         err = canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 347         if (err < 0)
 348                 goto errout;
 349
 350
 351         //Parse arguments
 352         if (tb[TCA_CANPRIO_RULES] == NULL)
 353                 return -EINVAL;
 354
 355         canprio_rules = nla_data(tb[TCA_CANPRIO_RULES]);
 356         canprio_rules_count = (nla_len(tb[TCA_CANPRIO_RULES]) / sizeof(struct canprio_rule));
 357         printk(" rules_count = %u\n", canprio_rules_count);
 358
 359         for (i = 0; i < canprio_rules_count; i++) {
 360                 /* FIXME: shouldn't use here the same logic as in
 361                  * can_rcv_filter() to filter for various combination
 362                  * of flags (EFF, RTR) */
 363                 if (canprio_rules[i].canid & CAN_EFF_FLAG) {
 364                         err = eff_match_add(f, canprio_rules[i].canid, canprio_rules[i].canid_mask);
 365                         if (err < 0)
 366                                 goto errout;
 367                 } else {
 368                         sff_match_add(f, canprio_rules[i].canid, canprio_rules[i].canid_mask);
 369                 }
 370
 371                 printk(" can ID to match = 0x%x with mask 0x%x\n",
 372                         canprio_rules[i].canid, canprio_rules[i].canid_mask);
 373         }
 374
 375         f->inv_match_en = 0;
 376         if (tb[TCA_CANPRIO_INV_EN] != NULL)
 377                 f->inv_match_en = nla_get_u32(tb[TCA_CANPRIO_INV_EN]);
 378
 379         //Add newly created filter to list of all filters
 380         tcf_tree_lock(tp);
 381         list_add(&f->link, &head->flist);
 382         tcf_tree_unlock(tp);
 383         *arg = (unsigned long) f;
 384
 385         return 0;
 386
 387 errout:
 388         if (*arg == 0UL && f) //FIXME why 0UL?
 389                 kfree(f);
 390
 391         return err;
 392 }
 393
 394
 395 static void canprio_delete_filter(struct tcf_proto *tp, struct canprio_filter *f)
 396 {
 397         struct canprio_eff_item *effi;
 398         struct hlist_node *p, *n;
 399
 400         rcu_barrier();
 401
 402         tcf_unbind_filter(tp, &f->res);
 403         hlist_for_each_entry_safe(effi, p, n, &f->match_eff, list) {
 404                 kfree(effi);
 405         }
 406
 407         kfree(f);
 408 }
 409
 410 /*
 411  * Remove whole filter.
 412  */
 413 static void canprio_destroy(struct tcf_proto *tp)
 414 {
 415         struct canprio_head *head = tp->root;
 416         struct canprio_filter *f, *n;
 417
 418         list_for_each_entry_safe(f, n, &head->flist, link) {
 419                 list_del(&f->link);
 420                 canprio_delete_filter(tp, f);
 421         }
 422         kfree(head);
 423 }
 424
 425 /*
 426  * Delete one instance of a filter.
 427  */
 428 static int canprio_delete(struct tcf_proto *tp, unsigned long arg)
 429 {
 430         struct canprio_head *head = (struct canprio_head *)tp->root;
 431         struct canprio_filter *t;
 432         struct canprio_filter *f = (struct canprio_filter *)arg;
 433
 434         rcu_barrier(); /* Wait for completion of call_rcu()'s */
 435
 436         list_for_each_entry(t, &head->flist, link)
 437                 if (t == f) {
 438                         tcf_tree_lock(tp);
 439                         list_del(&t->link);
 440                         tcf_tree_unlock(tp);
 441                         canprio_delete_filter(tp, t);
 442                         return 0;
 443                 }
 444
 445         return -ENOENT;
 446 }
 447
 448 //FIXME copied from cls_basic, not sure if ok;
 449 static int canprio_init(struct tcf_proto *tp)
 450 {
 451         struct canprio_head *head;
 452         //printk("tp = %p\n", tp);
 453         printk(" canprio_init invoked\n");
 454
 455         head = kzalloc(sizeof(*head), GFP_KERNEL);
 456         if (head == NULL)
 457                 return -ENOBUFS;
 458         INIT_LIST_HEAD(&head->flist);
 459         tp->root = head;
 460         tp->protocol = htons(ETH_P_CAN); /* Work only on AF_CAN packets - not tested! */
 461
 462         return 0;
 463 }
 464
 465
 466 static void canprio_walk(struct tcf_proto *tp, struct tcf_walker *arg)
 467 {
 468         struct canprio_head *head = (struct canprio_head *) tp->root;
 469         struct canprio_filter *f;
 470
 471         printk("%s() invoked\n", __FUNCTION__);
 472
 473         list_for_each_entry(f, &head->flist, link) {
 474                 if (arg->count < arg->skip)
 475                         goto skip;
 476
 477                 printk("calling canprio_dump()\n");
 478                 if (arg->fn(tp, (unsigned long) f, arg) < 0) {
 479                         arg->stop = 1;
 480                         break;
 481                 }
 482 skip:
 483                 arg->count++;
 484         }
 485 }
 486
 487 static int canprio_dump(struct tcf_proto *tp, unsigned long fh,
 488                     struct sk_buff *skb, struct tcmsg *t)
 489 {
 490         struct canprio_filter *f = (struct canprio_filter *) fh;
 491         struct nlattr *nest;
 492
 493         printk("%s() invoked\n", __FUNCTION__);
 494
 495         if (f == NULL)
 496                 return skb->len;
 497
 498         t->tcm_handle = f->handle;
 499
 500         nest = nla_nest_start(skb, TCA_OPTIONS);
 501         if (nest == NULL)
 502                 goto nla_put_failure;
 503
 504         if (f->res.classid)
 505                 NLA_PUT_U32(skb, TCA_CANPRIO_CLASSID, f->res.classid);
 506
 507
 508         //NLA_PUT(skb, TCA_CANPRIO_RULES, canprio_rules_count * sizeof(struct canprio_rule),
 509         //      canprio_rules);
 510
 511         /* ...
 512                 ... */
 513
 514         nla_nest_end(skb, nest);
 515
 516         return skb->len;
 517
 518 nla_put_failure:
 519         nla_nest_cancel(skb, nest);
 520         return -1;
 521 }
 522
 523
 524 static struct tcf_proto_ops cls_canprio_ops __read_mostly = {
 525         .kind           =       "canprio",
 526         .classify       =       canprio_classify,
 527         .init           =       canprio_init,
 528         .destroy        =       canprio_destroy,
 529         .get            =       canprio_get,
 530         .put            =       canprio_put,
 531         .change         =       canprio_change,
 532         .delete         =       canprio_delete,
 533         .walk           =       canprio_walk,
 534         .dump           =       canprio_dump,
 535         .owner          =       THIS_MODULE,
 536 };
 537
 538 static int __init init_canprio(void)
 539 {
 540         printk("Canprio loaded\n");
 541         return register_tcf_proto_ops(&cls_canprio_ops);
 542 }
 543
 544 static void __exit exit_canprio(void)
 545 {
 546         printk("Canprio removed\n");
 547         unregister_tcf_proto_ops(&cls_canprio_ops);
 548 }
 549
 550 module_init(init_canprio);
 551 module_exit(exit_canprio);
 552 MODULE_LICENSE("GPL");
 553 MODULE_AUTHOR(""); // FIXME
 554 MODULE_DESCRIPTION("");
 555