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
  22
  23 /* Definition of Netlink messages */
  24 enum {
  25         TCA_CANPRIO_A_UNSPEC,
  26         TCA_CANPRIO_CLASSID,
  27         TCA_CANPRIO_MATCH,
  28         TCA_CANPRIO_XY,
  29         __TCA_CANPRIO_MAX,
  30 };
  31 #define TCA_CANPRIO_MAX (__TCA_CANPRIO_MAX - 1)
  32
  33 static const struct nla_policy canprio_policy[TCA_CANPRIO_MAX + 1] = {
  34         [TCA_CANPRIO_CLASSID]   = { .type = NLA_U32 },
  35         [TCA_CANPRIO_MATCH]     = { .type = NLA_U32 }, //{ .type = NLA_NESTED },
  36         [TCA_CANPRIO_XY]        = { .type = NLA_U32 },
  37 };
  38
  39 struct canprio_head {
  40         u32                  hgenerator;
  41         struct list_head     flist;
  42 };
  43
  44 struct canprio_filter {
  45         u32                  handle;
  46         u32                  match; // Matching CAN ID. Will be sth like list os lists
  47         struct tcf_result    res;
  48         struct list_head     link;
  49 };
  50
  51 /*
  52  * ----------------------------------------------------------------------------
  53  */
  54
  55 /*
  56  * Extracts Can ID ot ouf the sk_buff structure.
  57  */
  58 static u32 canprio_get_id(struct sk_buff *skb)
  59 {
  60         /* Can ID is inside of data field */
  61         struct can_frame *cf = (struct can_frame *)skb->data;
  62         //canid_t can_id = cf->can_id;
  63
  64         return (u32)cf->can_id;
  65 }
  66
  67 /*
  68  * Performs the classification. Iterates over all instances of filter
  69  * checking for Can ID match.
  70  *
  71  * @skb: Socket buffer
  72  * @tp:
  73  * @res: Is used for setting Class ID as a result of classification
  74  *
  75  * Returns value relevant for policing. Used return values:
  76  *   TC_POLICE_OK if succesfully classified (without regard to policing rules)
  77  *   TC_POLICE_UNSPEC if no matching filter was found
  78  */
  79 static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp,
  80                           struct tcf_result *res)
  81 {
  82         struct canprio_head *head = (struct canprio_head *)tp->root;
  83         struct canprio_filter *f;
  84
  85         printk(" canprio_classify invoked\n");
  86
  87         list_for_each_entry(f, &head->flist, link) {
  88                 printk("  canprio_classify can ID = 0x%x\n", canprio_get_id(skb));
  89                 if (canprio_get_id(skb) == f->match) {
  90                         printk( "   canprio_classify match ID 0x%x\n", f->match);
  91                         *res = f->res;
  92                         return TC_POLICE_OK;
  93                 }
  94         }
  95
  96         return TC_POLICE_UNSPEC;
  97 }
  98
  99 /*
 100  * Configure filter
 101  */
 102 static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f,
 103                            unsigned long base, struct nlattr **tb,
 104                            struct nlattr *est)
 105 {
 106         //int err;
 107
 108         if (tb[TCA_CANPRIO_CLASSID]) {
 109                 f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]);
 110                 tcf_bind_filter(tp, &f->res, base);
 111         }
 112
 113         return 0;
 114 }
 115
 116 /*
 117  * Looks up a filter element by its handle and returns the internal filter ID
 118  */
 119 static unsigned long canprio_get(struct tcf_proto *tp, u32 handle)
 120 {
 121         unsigned long l = 0UL;
 122         struct canprio_head *head = (struct canprio_head *) tp->root;
 123         struct canprio_filter *f;
 124
 125         if (head == NULL)
 126                 return 0UL;
 127
 128         list_for_each_entry(f, &head->flist, link)
 129                 if (f->handle == handle)
 130                         l = (unsigned long) f;
 131
 132         return l;
 133 }
 134
 135 /*
 136  * Is invoked when a filter element previously referenced
 137  * with get() is no longer used
 138  */
 139 static void canprio_put(struct tcf_proto *tp, unsigned long f)
 140 {
 141 }
 142
 143
 144 /*
 145  * Called for changing properties of an existing filter or after addition
 146  * of a new filter to a class (by calling bind_tcf which binds an instance
 147  * of a filter to the class).
 148  *
 149  * @tp:     Structure representing instance of a filter.
 150  *          Part of a linked list of all filters.
 151  * @base:
 152  * @handle:
 153  * @tca:    Messages passed through the Netlink from userspace.
 154  * @arg:    ??? FIXME
 155  */
 156 static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle,
 157                           struct nlattr **tca, unsigned long *arg)
 158 {
 159         int err;
 160         struct canprio_head *head = (struct canprio_head *)tp->root;
 161         struct canprio_filter *f = (struct canprio_filter *)*arg;
 162         struct nlattr *tb[TCA_CANPRIO_MAX + 1];
 163
 164         printk(" canprio_change invoked\n");
 165
 166         if (tca[TCA_OPTIONS] == NULL)
 167                 return -EINVAL;
 168
 169         err = nla_parse_nested(tb, TCA_CANPRIO_MAX, tca[TCA_OPTIONS], canprio_policy);
 170         if (err < 0)
 171                 return err;
 172
 173         //Change existing filter (?)
 174         if (f != NULL) {
 175                 if (handle && f->handle != handle)
 176                         return -EINVAL;
 177                 return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 178         }
 179
 180         //Create new filter (?)
 181         err = -ENOBUFS;
 182         f = kzalloc(sizeof(*f), GFP_KERNEL);
 183         if (f == NULL)
 184                 goto errout;
 185
 186
 187         //FIXME why?
 188         err = -EINVAL;
 189         if (handle)
 190                 f->handle = handle;
 191         else {
 192                 //FIXME wat?
 193                 unsigned int i = 0x80000000;
 194                 do {
 195                         if (++head->hgenerator == 0x7FFFFFFF)
 196                                 head->hgenerator = 1;
 197                 } while (--i > 0 && canprio_get(tp, head->hgenerator));
 198
 199                 if (i <= 0) {
 200                         pr_err("Insufficient number of handles\n");
 201                         goto errout;
 202                 }
 203
 204                 f->handle = head->hgenerator;
 205         }
 206
 207
 208         //Configure newly created filter
 209         err = canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]);
 210         if (err < 0)
 211                 goto errout;
 212
 213
 214         //Parse arguments
 215         if (tb[TCA_CANPRIO_MATCH] == NULL)
 216                 return -EINVAL;
 217
 218         //f->match = nla_data(tb[TCA_CANPRIO_MATCH]);
 219         f->match = nla_get_u32(tb[TCA_CANPRIO_MATCH]);
 220         printk(" can ID to match = 0x%x\n", f->match);
 221
 222         //Add newly created filter to list of all filters
 223         tcf_tree_lock(tp);
 224         list_add(&f->link, &head->flist);
 225         tcf_tree_unlock(tp);
 226         *arg = (unsigned long) f;
 227
 228         return 0;
 229
 230 errout:
 231         if (*arg == 0UL && f) //FIXME why 0UL?
 232                 kfree(f);
 233
 234         return err;
 235 }
 236
 237
 238 static void canprio_delete_filter(struct tcf_proto *tp, struct canprio_filter *f)
 239 {
 240         tcf_unbind_filter(tp, &f->res);
 241         //tcf_exts_destroy(tp, &f->exts);
 242         //tcf_em_tree_destroy(tp, &f->ematches);
 243         kfree(f);
 244 }
 245
 246 /*
 247  * Remove whole filter.
 248  */
 249 static void canprio_destroy(struct tcf_proto *tp)
 250 {
 251         struct canprio_head *head = tp->root;
 252         struct canprio_filter *f, *n;
 253
 254         list_for_each_entry_safe(f, n, &head->flist, link) {
 255                 list_del(&f->link);
 256                 canprio_delete_filter(tp, f);
 257         }
 258         kfree(head);
 259 }
 260
 261 /*
 262  * Delete one instance of a filter.
 263  */
 264 static int canprio_delete(struct tcf_proto *tp, unsigned long arg)
 265 {
 266         struct canprio_head *head = (struct canprio_head *)tp->root;
 267         struct canprio_filter *t;
 268         struct canprio_filter *f = (struct canprio_filter *)arg;
 269
 270         list_for_each_entry(t, &head->flist, link)
 271                 if (t == f) {
 272                         tcf_tree_lock(tp);
 273                         list_del(&t->link);
 274                         tcf_tree_unlock(tp);
 275                         canprio_delete_filter(tp, t);
 276                         return 0;
 277                 }
 278
 279         return -ENOENT;
 280 }
 281
 282 //FIXME copied from cls_basic, not sure if ok;
 283 static int canprio_init(struct tcf_proto *tp)
 284 {
 285         struct canprio_head *head;
 286
 287         printk(" canprio_init invoked\n");
 288
 289         head = kzalloc(sizeof(*head), GFP_KERNEL);
 290         if (head == NULL)
 291                 return -ENOBUFS;
 292         INIT_LIST_HEAD(&head->flist);
 293         tp->root = head;
 294         return 0;
 295 }
 296
 297
 298 static struct tcf_proto_ops cls_canprio_ops __read_mostly = {
 299         .kind           =       "canprio",
 300         .classify       =       canprio_classify,
 301         .init           =       canprio_init,
 302         .destroy        =       canprio_destroy,
 303         .get            =       canprio_get,
 304         .put            =       canprio_put,
 305         .change         =       canprio_change,
 306         .delete         =       canprio_delete,
 307         .owner          =       THIS_MODULE,
 308 };
 309
 310 static int __init init_canprio(void)
 311 {
 312         printk("Canprio loaded\n");
 313         return register_tcf_proto_ops(&cls_canprio_ops);
 314 }
 315
 316 static void __exit exit_canprio(void)
 317 {
 318         printk("Canprio removed\n");
 319         unregister_tcf_proto_ops(&cls_canprio_ops);
 320 }
 321
 322 module_init(init_canprio);
 323 module_exit(exit_canprio);
 324 MODULE_LICENSE("GPL");
 325