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);