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