linux/net/sched/em_canid.c
<<
>>
Prefs
   1/*
   2 * em_canid.c  Ematch rule to match CAN frames according to their CAN IDs
   3 *
   4 *              This program is free software; you can distribute it and/or
   5 *              modify it under the terms of the GNU General Public License
   6 *              as published by the Free Software Foundation; either version
   7 *              2 of the License, or (at your option) any later version.
   8 *
   9 * Idea:       Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
  10 * Copyright:  (c) 2011 Czech Technical University in Prague
  11 *             (c) 2011 Volkswagen Group Research
  12 * Authors:    Michal Sojka <sojkam1@fel.cvut.cz>
  13 *             Pavel Pisa <pisa@cmp.felk.cvut.cz>
  14 *             Rostislav Lisovy <lisovy@gmail.cz>
  15 * Funded by:  Volkswagen Group Research
  16 */
  17
  18#include <linux/slab.h>
  19#include <linux/module.h>
  20#include <linux/types.h>
  21#include <linux/kernel.h>
  22#include <linux/string.h>
  23#include <linux/skbuff.h>
  24#include <net/pkt_cls.h>
  25#include <linux/can.h>
  26
  27#define EM_CAN_RULES_MAX 500
  28
  29struct canid_match {
  30        /* For each SFF CAN ID (11 bit) there is one record in this bitfield */
  31        DECLARE_BITMAP(match_sff, (1 << CAN_SFF_ID_BITS));
  32
  33        int rules_count;
  34        int sff_rules_count;
  35        int eff_rules_count;
  36
  37        /*
  38         * Raw rules copied from netlink message; Used for sending
  39         * information to userspace (when 'tc filter show' is invoked)
  40         * AND when matching EFF frames
  41         */
  42        struct can_filter rules_raw[];
  43};
  44
  45/**
  46 * em_canid_get_id() - Extracts Can ID out of the sk_buff structure.
  47 */
  48static canid_t em_canid_get_id(struct sk_buff *skb)
  49{
  50        /* CAN ID is stored within the data field */
  51        struct can_frame *cf = (struct can_frame *)skb->data;
  52
  53        return cf->can_id;
  54}
  55
  56static void em_canid_sff_match_add(struct canid_match *cm, u32 can_id,
  57                                        u32 can_mask)
  58{
  59        int i;
  60
  61        /*
  62         * Limit can_mask and can_id to SFF range to
  63         * protect against write after end of array
  64         */
  65        can_mask &= CAN_SFF_MASK;
  66        can_id &= can_mask;
  67
  68        /* Single frame */
  69        if (can_mask == CAN_SFF_MASK) {
  70                set_bit(can_id, cm->match_sff);
  71                return;
  72        }
  73
  74        /* All frames */
  75        if (can_mask == 0) {
  76                bitmap_fill(cm->match_sff, (1 << CAN_SFF_ID_BITS));
  77                return;
  78        }
  79
  80        /*
  81         * Individual frame filter.
  82         * Add record (set bit to 1) for each ID that
  83         * conforms particular rule
  84         */
  85        for (i = 0; i < (1 << CAN_SFF_ID_BITS); i++) {
  86                if ((i & can_mask) == can_id)
  87                        set_bit(i, cm->match_sff);
  88        }
  89}
  90
  91static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
  92{
  93        return (struct canid_match *)m->data;
  94}
  95
  96static int em_canid_match(struct sk_buff *skb, struct tcf_ematch *m,
  97                         struct tcf_pkt_info *info)
  98{
  99        struct canid_match *cm = em_canid_priv(m);
 100        canid_t can_id;
 101        int match = 0;
 102        int i;
 103        const struct can_filter *lp;
 104
 105        can_id = em_canid_get_id(skb);
 106
 107        if (can_id & CAN_EFF_FLAG) {
 108                for (i = 0, lp = cm->rules_raw;
 109                     i < cm->eff_rules_count; i++, lp++) {
 110                        if (!(((lp->can_id ^ can_id) & lp->can_mask))) {
 111                                match = 1;
 112                                break;
 113                        }
 114                }
 115        } else { /* SFF */
 116                can_id &= CAN_SFF_MASK;
 117                match = (test_bit(can_id, cm->match_sff) ? 1 : 0);
 118        }
 119
 120        return match;
 121}
 122
 123static int em_canid_change(struct tcf_proto *tp, void *data, int len,
 124                          struct tcf_ematch *m)
 125{
 126        struct can_filter *conf = data; /* Array with rules */
 127        struct canid_match *cm;
 128        struct canid_match *cm_old = (struct canid_match *)m->data;
 129        int i;
 130
 131        if (!len)
 132                return -EINVAL;
 133
 134        if (len % sizeof(struct can_filter))
 135                return -EINVAL;
 136
 137        if (len > sizeof(struct can_filter) * EM_CAN_RULES_MAX)
 138                return -EINVAL;
 139
 140        cm = kzalloc(sizeof(struct canid_match) + len, GFP_KERNEL);
 141        if (!cm)
 142                return -ENOMEM;
 143
 144        cm->rules_count = len / sizeof(struct can_filter);
 145
 146        /*
 147         * We need two for() loops for copying rules into two contiguous
 148         * areas in rules_raw to process all eff rules with a simple loop.
 149         * NB: The configuration interface supports sff and eff rules.
 150         * We do not support filters here that match for the same can_id
 151         * provided in a SFF and EFF frame (e.g. 0x123 / 0x80000123).
 152         * For this (unusual case) two filters have to be specified. The
 153         * SFF/EFF separation is done with the CAN_EFF_FLAG in the can_id.
 154         */
 155
 156        /* Fill rules_raw with EFF rules first */
 157        for (i = 0; i < cm->rules_count; i++) {
 158                if (conf[i].can_id & CAN_EFF_FLAG) {
 159                        memcpy(cm->rules_raw + cm->eff_rules_count,
 160                                &conf[i],
 161                                sizeof(struct can_filter));
 162
 163                        cm->eff_rules_count++;
 164                }
 165        }
 166
 167        /* append SFF frame rules */
 168        for (i = 0; i < cm->rules_count; i++) {
 169                if (!(conf[i].can_id & CAN_EFF_FLAG)) {
 170                        memcpy(cm->rules_raw
 171                                + cm->eff_rules_count
 172                                + cm->sff_rules_count,
 173                                &conf[i], sizeof(struct can_filter));
 174
 175                        cm->sff_rules_count++;
 176
 177                        em_canid_sff_match_add(cm,
 178                                conf[i].can_id, conf[i].can_mask);
 179                }
 180        }
 181
 182        m->datalen = sizeof(struct canid_match) + len;
 183        m->data = (unsigned long)cm;
 184
 185        if (cm_old != NULL) {
 186                pr_err("canid: Configuring an existing ematch!\n");
 187                kfree(cm_old);
 188        }
 189
 190        return 0;
 191}
 192
 193static void em_canid_destroy(struct tcf_proto *tp, struct tcf_ematch *m)
 194{
 195        struct canid_match *cm = em_canid_priv(m);
 196
 197        kfree(cm);
 198}
 199
 200static int em_canid_dump(struct sk_buff *skb, struct tcf_ematch *m)
 201{
 202        struct canid_match *cm = em_canid_priv(m);
 203
 204        /*
 205         * When configuring this ematch 'rules_count' is set not to exceed
 206         * 'rules_raw' array size
 207         */
 208        if (nla_put_nohdr(skb, sizeof(struct can_filter) * cm->rules_count,
 209            &cm->rules_raw) < 0)
 210                return -EMSGSIZE;
 211
 212        return 0;
 213}
 214
 215static struct tcf_ematch_ops em_canid_ops = {
 216        .kind     = TCF_EM_CANID,
 217        .change   = em_canid_change,
 218        .match    = em_canid_match,
 219        .destroy  = em_canid_destroy,
 220        .dump     = em_canid_dump,
 221        .owner    = THIS_MODULE,
 222        .link     = LIST_HEAD_INIT(em_canid_ops.link)
 223};
 224
 225static int __init init_em_canid(void)
 226{
 227        return tcf_em_register(&em_canid_ops);
 228}
 229
 230static void __exit exit_em_canid(void)
 231{
 232        tcf_em_unregister(&em_canid_ops);
 233}
 234
 235MODULE_LICENSE("GPL");
 236
 237module_init(init_em_canid);
 238module_exit(exit_em_canid);
 239
 240MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);
 241
lxr.linux.no kindly hosted by Redpill Linpro AS, provider of Linux consulting and operations services since 1995.