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(canprio_rules) },
  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;
  71         struct hlist_head match_eff;
  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         mask &= CAN_SFF_MASK;
  85         canid = canid & mask;
  86
  87         if (mask == CAN_SFF_MASK) {
  88                 set_bit(canid, f->match_sff);
  89                 return;
  90         }
  91
  92         for (i = 0; i <= CAN_SFF_MASK; i++) {
  93                 if ((i & mask) == canid)
  94                         set_bit(i, f->match_sff);
  95         }
  96 }
  97
  98 static int eff_match_add(struct canprio_filter *f, u32 canid, u32 mask)
  99 {
 100         struct canprio_eff_item *eff;
 101         int err = 0;
 102
 103         mask &= CAN_EFF_MASK;
 104         canid = canid & mask;
 105
 106         eff = kmalloc(sizeof(struct canprio_eff_item), GFP_KERNEL);
 107         if (!eff)
 108                 return -ENOMEM;
 109
 110         spin_lock(&canprio_match_eff_lock);
 111
 112         eff->canid = canid;
 113         eff->mask = mask;
 114
 115         hlist_add_head_rcu(&eff->list, &f->match_eff);
 116
 117         spin_unlock(&canprio_match_eff_lock);
 118
 119         return err;
 120 }
 121
 122 /*
 123  * Extracts Can ID ot ouf the sk_buff structure.
 124  */
 125 static u32 canprio_get_id(struct sk_buff *skb)
 126 {
 127         /* Can ID is inside of data field */
 128         struct can_frame *cf = (struct can_frame *)skb->data;
 129         //canid_t canid = cf->canid;
 130
 131         return (u32)cf->can_id;
 132 }
 133
 134 /*
 135  * Performs the classification. Iterates over all instances of filter
 136  * checking for Can ID match.
 137  *
 138  * @skb: Socket buffer
 139  * @tp:
 140  * @res: Is used for setting Class ID as a result of classification
 141  *
 142  * Returns value relevant for policing. Used return values:
 143  *   TC_POLICE_OK if succesfully classified (without regard to policing rules)
 144  *   TC_POLICE_UNSPEC if no matching filter was found
 145  */
 146 static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
 147                           struct tcf_result *res)
 148 {
 149         struct canprio_head *head = (struct canprio_head *)tp->root;
 150         struct canprio_filter *f;
 151         u32 canid;
 152
 153         printk(" canprio_classify() invoked\n");
 154         canid = canprio_get_id(skb);
 155
 156         list_for_each_entry(f, &head->flist, link) {
 157                 int match = 0;
 158                 printk("  canprio_classify() can ID of received frame = 0x%x\n", canid);
 159
 160                 if (canid & CAN_EFF_FLAG) {
 161                         struct canprio_eff_item *effi;
 162                         struct hlist_node *next;
 163
 164                         rcu_read_lock();
 165
 166                         hlist_for_each_entry_rcu(effi, next, &f->match_eff, list) {
 167                                 if((effi->canid ^ canid) & effi->mask) {
 168                                         match = 1;
 169                                         break;
 170                                 }
 171                         }
 172
 173                         rcu_read_unlock();
 174                 } else {
 175                         if(test_bit(canid, f->match_sff)) {
 176                                 match = 1;
 177                         }
 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 filter ID
 216  */
 217 static unsigned long canprio_get(struct tcf_proto *tp, u32 handle)
 218 {
 219         unsigned long l = 0UL;
 220         struct canprio_head *head = (struct canprio_head *) tp->root;
 221         struct canprio_filter *f;
 222
 223         if (head == NULL)
 224                 return 0UL;
 225
 226         list_for_each_entry(f, &head->flist, link)
 227                 if (f->handle == handle)
 228                         l = (unsigned long) f;
 229
 230         return l;
 231 }
 232
 233 /*
 234  * Is invoked when a filter element previously referenced
 235  * with get() is no longer used
 236  */
 237 static void canprio_put(struct tcf_proto *tp, unsigned long f)
 238 {
 239 }
 240
 241
 242 /*
 243  * Called for changing properties of an existing filter or after addition
 244  * of a new filter to a class (by calling bind_tcf which binds an instance
 245  * of a filter to the class).
 246  *
 247  * @tp:     Structure representing instance of a filter.
 248  *          Part of a linked list of all filters.
 249  * @base:
 250  * @handle:
 251  * @tca:    Messages passed through the Netlink from userspace.
 252  * @arg:    ??? FIXME
 253  */
 254 static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
 255                           struct nlattr **tca, unsigned long *arg)
 256 {
 257         int err;
 258         struct canprio_head *head = (struct canprio_head *)tp->root;
 259         struct canprio_filter *f = (struct canprio_filter *)*arg;
 260         struct nlattr *tb[TCA_CANPRIO_MAX + 1];
 261         int i;
 262
 263         printk(" canprio_change invoked\n");
 264
 265         if (tca[TCA_OPTIONS] == NULL)
 266                 return -EINVAL;
 267
 268         //Parses a stream of attributes and stores a pointer to each attribute in
 269         //the tb array accessible via the attribute type. Policy may be set to NULL
 270         //if no validation is required.
 271         err = nla_parse_nested(tb, TCA_CANPRIO_MAX, tca[TCA_OPTIONS], canprio_policy);
 272         if (err < 0)
 273                 return err;
 274
 275         //Change existing filter (?)
 276         if (f != NULL) {
 277                 if (handle && f->handle != handle)
 278                         return -EINVAL;
 279                 return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 280         }
 281
 282         //Create new filter (?)
 283         err = -ENOBUFS;
 284         f = kzalloc(sizeof(*f), GFP_KERNEL);
 285         if (f == NULL)
 286                 goto errout;
 287
 288
 289         //FIXME why?
 290         err = -EINVAL;
 291         if (handle)
 292                 f->handle = handle;
 293         else {
 294                 //FIXME wat?
 295                 unsigned int i = 0x80000000;
 296                 do {
 297                         if (++head->hgenerator == 0x7FFFFFFF)
 298                                 head->hgenerator = 1;
 299                 } while (--i > 0 && canprio_get(tp, head->hgenerator));
 300
 301                 if (i <= 0) {
 302                         pr_err("Insufficient number of handles\n");
 303                         goto errout;
 304                 }
 305
 306                 f->handle = head->hgenerator;
 307         }
 308
 309
 310         //Configure newly created filter
 311         err = canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 312         if (err < 0)
 313                 goto errout;
 314
 315
 316         //Parse arguments
 317         if (tb[TCA_CANPRIO_RULES] == NULL)
 318                 return -EINVAL;
 319
 320         canprio_rules = nla_data(tb[TCA_CANPRIO_RULES]);
 321         canprio_rules_count = (nla_len(tb[TCA_CANPRIO_RULES]) / sizeof(struct canprio_rule));
 322         printk(" rules_count = %u\n", canprio_rules_count);
 323
 324         for (i = 0; i < canprio_rules_count; i++) {
 325                 if (canprio_rules[i].canid & CAN_EFF_FLAG) {
 326                         err = eff_match_add(f, canprio_rules[i].canid, canprio_rules[i].canid_mask);
 327                         if (err < 0)
 328                                 goto errout;
 329                 } else {
 330                         sff_match_add(f, canprio_rules[i].canid, canprio_rules[i].canid_mask);
 331                 }
 332
 333                 printk(" can ID to match = 0x%x with mask 0x%x\n",
 334                         canprio_rules[i].canid, canprio_rules[i].canid_mask);
 335         }
 336
 337         f->inv_match_en = 0;
 338         if (tb[TCA_CANPRIO_INV_EN] != NULL)
 339                 f->inv_match_en = nla_get_u32(tb[TCA_CANPRIO_INV_EN]);
 340
 341         //Add newly created filter to list of all filters
 342         tcf_tree_lock(tp);
 343         list_add(&f->link, &head->flist);
 344         tcf_tree_unlock(tp);
 345         *arg = (unsigned long) f;
 346
 347         return 0;
 348
 349 errout:
 350         if (*arg == 0UL && f) //FIXME why 0UL?
 351                 kfree(f);
 352
 353         return err;
 354 }
 355
 356
 357 static void canprio_delete_filter(struct tcf_proto *tp, struct canprio_filter *f)
 358 {
 359         struct canprio_eff_item *effi;
 360         struct hlist_node *p, *n;
 361
 362         rcu_barrier();
 363
 364         tcf_unbind_filter(tp, &f->res);
 365         hlist_for_each_entry_safe(effi, p, n, &f->match_eff, list) {
 366                 kfree(effi);
 367         }
 368
 369         kfree(f);
 370 }
 371
 372 /*
 373  * Remove whole filter.
 374  */
 375 static void canprio_destroy(struct tcf_proto *tp)
 376 {
 377         struct canprio_head *head = tp->root;
 378         struct canprio_filter *f, *n;
 379
 380         list_for_each_entry_safe(f, n, &head->flist, link) {
 381                 list_del(&f->link);
 382                 canprio_delete_filter(tp, f);
 383         }
 384         kfree(head);
 385 }
 386
 387 /*
 388  * Delete one instance of a filter.
 389  */
 390 static int canprio_delete(struct tcf_proto *tp, unsigned long arg)
 391 {
 392         struct canprio_head *head = (struct canprio_head *)tp->root;
 393         struct canprio_filter *t;
 394         struct canprio_filter *f = (struct canprio_filter *)arg;
 395
 396         rcu_barrier(); /* Wait for completion of call_rcu()'s */
 397
 398         list_for_each_entry(t, &head->flist, link)
 399                 if (t == f) {
 400                         tcf_tree_lock(tp);
 401                         list_del(&t->link);
 402                         tcf_tree_unlock(tp);
 403                         canprio_delete_filter(tp, t);
 404                         return 0;
 405                 }
 406
 407         return -ENOENT;
 408 }
 409
 410 //FIXME copied from cls_basic, not sure if ok;
 411 static int canprio_init(struct tcf_proto *tp)
 412 {
 413         struct canprio_head *head;
 414
 415         printk(" canprio_init invoked\n");
 416
 417         head = kzalloc(sizeof(*head), GFP_KERNEL);
 418         if (head == NULL)
 419                 return -ENOBUFS;
 420         INIT_LIST_HEAD(&head->flist);
 421         tp->root = head;
 422
 423         return 0;
 424 }
 425
 426
 427 static struct tcf_proto_ops cls_canprio_ops __read_mostly = {
 428         .kind           =       "canprio",
 429         .classify       =       canprio_classify,
 430         .init           =       canprio_init,
 431         .destroy        =       canprio_destroy,
 432         .get            =       canprio_get,
 433         .put            =       canprio_put,
 434         .change         =       canprio_change,
 435         .delete         =       canprio_delete,
 436         .owner          =       THIS_MODULE,
 437 };
 438
 439 static int __init init_canprio(void)
 440 {
 441         printk("Canprio loaded\n");
 442         return register_tcf_proto_ops(&cls_canprio_ops);
 443 }
 444
 445 static void __exit exit_canprio(void)
 446 {
 447         printk("Canprio removed\n");
 448         unregister_tcf_proto_ops(&cls_canprio_ops);
 449 }
 450
 451 module_init(init_canprio);
 452 module_exit(exit_canprio);
 453 MODULE_LICENSE("GPL");
 454