Linux Audio

Check our new training course

Loading...
v3.5.6
  1/*
  2 * gw.c - CAN frame Gateway/Router/Bridge with netlink interface
  3 *
  4 * Copyright (c) 2011 Volkswagen Group Electronic Research
  5 * All rights reserved.
  6 *
  7 * Redistribution and use in source and binary forms, with or without
  8 * modification, are permitted provided that the following conditions
  9 * are met:
 10 * 1. Redistributions of source code must retain the above copyright
 11 *    notice, this list of conditions and the following disclaimer.
 12 * 2. Redistributions in binary form must reproduce the above copyright
 13 *    notice, this list of conditions and the following disclaimer in the
 14 *    documentation and/or other materials provided with the distribution.
 15 * 3. Neither the name of Volkswagen nor the names of its contributors
 16 *    may be used to endorse or promote products derived from this software
 17 *    without specific prior written permission.
 18 *
 19 * Alternatively, provided that this notice is retained in full, this
 20 * software may be distributed under the terms of the GNU General
 21 * Public License ("GPL") version 2, in which case the provisions of the
 22 * GPL apply INSTEAD OF those given above.
 23 *
 24 * The provided data structures and external interfaces from this code
 25 * are not restricted to be used by modules with a GPL compatible license.
 26 *
 27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 30 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 31 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 32 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 33 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 34 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 35 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 36 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 37 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 38 * DAMAGE.
 39 *
 40 */
 41
 42#include <linux/module.h>
 43#include <linux/init.h>
 44#include <linux/types.h>
 
 45#include <linux/list.h>
 46#include <linux/spinlock.h>
 47#include <linux/rcupdate.h>
 48#include <linux/rculist.h>
 49#include <linux/net.h>
 50#include <linux/netdevice.h>
 51#include <linux/if_arp.h>
 52#include <linux/skbuff.h>
 53#include <linux/can.h>
 54#include <linux/can/core.h>
 
 55#include <linux/can/gw.h>
 56#include <net/rtnetlink.h>
 57#include <net/net_namespace.h>
 58#include <net/sock.h>
 59
 60#define CAN_GW_VERSION "20101209"
 61static __initdata const char banner[] =
 62	KERN_INFO "can: netlink gateway (rev " CAN_GW_VERSION ")\n";
 63
 64MODULE_DESCRIPTION("PF_CAN netlink gateway");
 65MODULE_LICENSE("Dual BSD/GPL");
 66MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
 67MODULE_ALIAS("can-gw");
 68
 69static HLIST_HEAD(cgw_list);
 70static struct notifier_block notifier;
 
 
 
 
 
 
 
 
 
 71
 
 72static struct kmem_cache *cgw_cache __read_mostly;
 73
 74/* structure that contains the (on-the-fly) CAN frame modifications */
 75struct cf_mod {
 76	struct {
 77		struct can_frame and;
 78		struct can_frame or;
 79		struct can_frame xor;
 80		struct can_frame set;
 81	} modframe;
 82	struct {
 83		u8 and;
 84		u8 or;
 85		u8 xor;
 86		u8 set;
 87	} modtype;
 88	void (*modfunc[MAX_MODFUNCTIONS])(struct can_frame *cf,
 89					  struct cf_mod *mod);
 90
 91	/* CAN frame checksum calculation after CAN frame modifications */
 92	struct {
 93		struct cgw_csum_xor xor;
 94		struct cgw_csum_crc8 crc8;
 95	} csum;
 96	struct {
 97		void (*xor)(struct can_frame *cf, struct cgw_csum_xor *xor);
 98		void (*crc8)(struct can_frame *cf, struct cgw_csum_crc8 *crc8);
 99	} csumfunc;
 
100};
101
102
103/*
104 * So far we just support CAN -> CAN routing and frame modifications.
105 *
106 * The internal can_can_gw structure contains data and attributes for
107 * a CAN -> CAN gateway job.
108 */
109struct can_can_gw {
110	struct can_filter filter;
111	int src_idx;
112	int dst_idx;
113};
114
115/* list entry for CAN gateways jobs */
116struct cgw_job {
117	struct hlist_node list;
118	struct rcu_head rcu;
119	u32 handled_frames;
120	u32 dropped_frames;
 
121	struct cf_mod mod;
122	union {
123		/* CAN frame data source */
124		struct net_device *dev;
125	} src;
126	union {
127		/* CAN frame data destination */
128		struct net_device *dev;
129	} dst;
130	union {
131		struct can_can_gw ccgw;
132		/* tbc */
133	};
134	u8 gwtype;
 
135	u16 flags;
136};
137
138/* modification functions that are invoked in the hot path in can_can_gw_rcv */
139
140#define MODFUNC(func, op) static void func(struct can_frame *cf, \
141					   struct cf_mod *mod) { op ; }
142
143MODFUNC(mod_and_id, cf->can_id &= mod->modframe.and.can_id)
144MODFUNC(mod_and_dlc, cf->can_dlc &= mod->modframe.and.can_dlc)
145MODFUNC(mod_and_data, *(u64 *)cf->data &= *(u64 *)mod->modframe.and.data)
146MODFUNC(mod_or_id, cf->can_id |= mod->modframe.or.can_id)
147MODFUNC(mod_or_dlc, cf->can_dlc |= mod->modframe.or.can_dlc)
148MODFUNC(mod_or_data, *(u64 *)cf->data |= *(u64 *)mod->modframe.or.data)
149MODFUNC(mod_xor_id, cf->can_id ^= mod->modframe.xor.can_id)
150MODFUNC(mod_xor_dlc, cf->can_dlc ^= mod->modframe.xor.can_dlc)
151MODFUNC(mod_xor_data, *(u64 *)cf->data ^= *(u64 *)mod->modframe.xor.data)
152MODFUNC(mod_set_id, cf->can_id = mod->modframe.set.can_id)
153MODFUNC(mod_set_dlc, cf->can_dlc = mod->modframe.set.can_dlc)
154MODFUNC(mod_set_data, *(u64 *)cf->data = *(u64 *)mod->modframe.set.data)
155
156static inline void canframecpy(struct can_frame *dst, struct can_frame *src)
157{
158	/*
159	 * Copy the struct members separately to ensure that no uninitialized
160	 * data are copied in the 3 bytes hole of the struct. This is needed
161	 * to make easy compares of the data in the struct cf_mod.
162	 */
163
164	dst->can_id = src->can_id;
165	dst->can_dlc = src->can_dlc;
166	*(u64 *)dst->data = *(u64 *)src->data;
167}
168
169static int cgw_chk_csum_parms(s8 fr, s8 to, s8 re)
170{
171	/*
172	 * absolute dlc values 0 .. 7 => 0 .. 7, e.g. data [0]
173	 * relative to received dlc -1 .. -8 :
174	 * e.g. for received dlc = 8
175	 * -1 => index = 7 (data[7])
176	 * -3 => index = 5 (data[5])
177	 * -8 => index = 0 (data[0])
178	 */
179
180	if (fr > -9 && fr < 8 &&
181	    to > -9 && to < 8 &&
182	    re > -9 && re < 8)
183		return 0;
184	else
185		return -EINVAL;
186}
187
188static inline int calc_idx(int idx, int rx_dlc)
189{
190	if (idx < 0)
191		return rx_dlc + idx;
192	else
193		return idx;
194}
195
196static void cgw_csum_xor_rel(struct can_frame *cf, struct cgw_csum_xor *xor)
197{
198	int from = calc_idx(xor->from_idx, cf->can_dlc);
199	int to = calc_idx(xor->to_idx, cf->can_dlc);
200	int res = calc_idx(xor->result_idx, cf->can_dlc);
201	u8 val = xor->init_xor_val;
202	int i;
203
204	if (from < 0 || to < 0 || res < 0)
205		return;
206
207	if (from <= to) {
208		for (i = from; i <= to; i++)
209			val ^= cf->data[i];
210	} else {
211		for (i = from; i >= to; i--)
212			val ^= cf->data[i];
213	}
214
215	cf->data[res] = val;
216}
217
218static void cgw_csum_xor_pos(struct can_frame *cf, struct cgw_csum_xor *xor)
219{
220	u8 val = xor->init_xor_val;
221	int i;
222
223	for (i = xor->from_idx; i <= xor->to_idx; i++)
224		val ^= cf->data[i];
225
226	cf->data[xor->result_idx] = val;
227}
228
229static void cgw_csum_xor_neg(struct can_frame *cf, struct cgw_csum_xor *xor)
230{
231	u8 val = xor->init_xor_val;
232	int i;
233
234	for (i = xor->from_idx; i >= xor->to_idx; i--)
235		val ^= cf->data[i];
236
237	cf->data[xor->result_idx] = val;
238}
239
240static void cgw_csum_crc8_rel(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
241{
242	int from = calc_idx(crc8->from_idx, cf->can_dlc);
243	int to = calc_idx(crc8->to_idx, cf->can_dlc);
244	int res = calc_idx(crc8->result_idx, cf->can_dlc);
245	u8 crc = crc8->init_crc_val;
246	int i;
247
248	if (from < 0 || to < 0 || res < 0)
249		return;
250
251	if (from <= to) {
252		for (i = crc8->from_idx; i <= crc8->to_idx; i++)
253			crc = crc8->crctab[crc^cf->data[i]];
254	} else {
255		for (i = crc8->from_idx; i >= crc8->to_idx; i--)
256			crc = crc8->crctab[crc^cf->data[i]];
257	}
258
259	switch (crc8->profile) {
260
261	case CGW_CRC8PRF_1U8:
262		crc = crc8->crctab[crc^crc8->profile_data[0]];
263		break;
264
265	case  CGW_CRC8PRF_16U8:
266		crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
267		break;
268
269	case CGW_CRC8PRF_SFFID_XOR:
270		crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
271				   (cf->can_id >> 8 & 0xFF)];
272		break;
273
274	}
275
276	cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
277}
278
279static void cgw_csum_crc8_pos(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
280{
281	u8 crc = crc8->init_crc_val;
282	int i;
283
284	for (i = crc8->from_idx; i <= crc8->to_idx; i++)
285		crc = crc8->crctab[crc^cf->data[i]];
286
287	switch (crc8->profile) {
288
289	case CGW_CRC8PRF_1U8:
290		crc = crc8->crctab[crc^crc8->profile_data[0]];
291		break;
292
293	case  CGW_CRC8PRF_16U8:
294		crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
295		break;
296
297	case CGW_CRC8PRF_SFFID_XOR:
298		crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
299				   (cf->can_id >> 8 & 0xFF)];
300		break;
301	}
302
303	cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
304}
305
306static void cgw_csum_crc8_neg(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
307{
308	u8 crc = crc8->init_crc_val;
309	int i;
310
311	for (i = crc8->from_idx; i >= crc8->to_idx; i--)
312		crc = crc8->crctab[crc^cf->data[i]];
313
314	switch (crc8->profile) {
315
316	case CGW_CRC8PRF_1U8:
317		crc = crc8->crctab[crc^crc8->profile_data[0]];
318		break;
319
320	case  CGW_CRC8PRF_16U8:
321		crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
322		break;
323
324	case CGW_CRC8PRF_SFFID_XOR:
325		crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
326				   (cf->can_id >> 8 & 0xFF)];
327		break;
328	}
329
330	cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
331}
332
333/* the receive & process & send function */
334static void can_can_gw_rcv(struct sk_buff *skb, void *data)
335{
336	struct cgw_job *gwj = (struct cgw_job *)data;
337	struct can_frame *cf;
338	struct sk_buff *nskb;
339	int modidx = 0;
340
341	/* do not handle already routed frames - see comment below */
342	if (skb_mac_header_was_set(skb))
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
343		return;
 
344
345	if (!(gwj->dst.dev->flags & IFF_UP)) {
346		gwj->dropped_frames++;
347		return;
348	}
349
 
 
 
 
 
350	/*
351	 * clone the given skb, which has not been done in can_rcv()
352	 *
353	 * When there is at least one modification function activated,
354	 * we need to copy the skb as we want to modify skb->data.
355	 */
356	if (gwj->mod.modfunc[0])
357		nskb = skb_copy(skb, GFP_ATOMIC);
358	else
359		nskb = skb_clone(skb, GFP_ATOMIC);
360
361	if (!nskb) {
362		gwj->dropped_frames++;
363		return;
364	}
365
366	/*
367	 * Mark routed frames by setting some mac header length which is
368	 * not relevant for the CAN frames located in the skb->data section.
369	 *
370	 * As dev->header_ops is not set in CAN netdevices no one is ever
371	 * accessing the various header offsets in the CAN skbuffs anyway.
372	 * E.g. using the packet socket to read CAN frames is still working.
373	 */
374	skb_set_mac_header(nskb, 8);
375	nskb->dev = gwj->dst.dev;
376
377	/* pointer to modifiable CAN frame */
378	cf = (struct can_frame *)nskb->data;
379
380	/* perform preprocessed modification functions if there are any */
381	while (modidx < MAX_MODFUNCTIONS && gwj->mod.modfunc[modidx])
382		(*gwj->mod.modfunc[modidx++])(cf, &gwj->mod);
383
384	/* check for checksum updates when the CAN frame has been modified */
385	if (modidx) {
386		if (gwj->mod.csumfunc.crc8)
387			(*gwj->mod.csumfunc.crc8)(cf, &gwj->mod.csum.crc8);
388
389		if (gwj->mod.csumfunc.xor)
390			(*gwj->mod.csumfunc.xor)(cf, &gwj->mod.csum.xor);
391	}
392
393	/* clear the skb timestamp if not configured the other way */
394	if (!(gwj->flags & CGW_FLAGS_CAN_SRC_TSTAMP))
395		nskb->tstamp.tv64 = 0;
396
397	/* send to netdevice */
398	if (can_send(nskb, gwj->flags & CGW_FLAGS_CAN_ECHO))
399		gwj->dropped_frames++;
400	else
401		gwj->handled_frames++;
402}
403
404static inline int cgw_register_filter(struct cgw_job *gwj)
405{
406	return can_rx_register(gwj->src.dev, gwj->ccgw.filter.can_id,
407			       gwj->ccgw.filter.can_mask, can_can_gw_rcv,
408			       gwj, "gw");
409}
410
411static inline void cgw_unregister_filter(struct cgw_job *gwj)
412{
413	can_rx_unregister(gwj->src.dev, gwj->ccgw.filter.can_id,
414			  gwj->ccgw.filter.can_mask, can_can_gw_rcv, gwj);
415}
416
417static int cgw_notifier(struct notifier_block *nb,
418			unsigned long msg, void *data)
419{
420	struct net_device *dev = (struct net_device *)data;
 
421
422	if (!net_eq(dev_net(dev), &init_net))
423		return NOTIFY_DONE;
424	if (dev->type != ARPHRD_CAN)
425		return NOTIFY_DONE;
426
427	if (msg == NETDEV_UNREGISTER) {
428
429		struct cgw_job *gwj = NULL;
430		struct hlist_node *n, *nx;
431
432		ASSERT_RTNL();
433
434		hlist_for_each_entry_safe(gwj, n, nx, &cgw_list, list) {
435
436			if (gwj->src.dev == dev || gwj->dst.dev == dev) {
437				hlist_del(&gwj->list);
438				cgw_unregister_filter(gwj);
439				kfree(gwj);
440			}
441		}
442	}
443
444	return NOTIFY_DONE;
445}
446
447static int cgw_put_job(struct sk_buff *skb, struct cgw_job *gwj)
 
448{
449	struct cgw_frame_mod mb;
450	struct rtcanmsg *rtcan;
451	struct nlmsghdr *nlh = nlmsg_put(skb, 0, 0, 0, sizeof(*rtcan), 0);
 
 
452	if (!nlh)
453		return -EMSGSIZE;
454
455	rtcan = nlmsg_data(nlh);
456	rtcan->can_family = AF_CAN;
457	rtcan->gwtype = gwj->gwtype;
458	rtcan->flags = gwj->flags;
459
460	/* add statistics if available */
461
462	if (gwj->handled_frames) {
463		if (nla_put_u32(skb, CGW_HANDLED, gwj->handled_frames) < 0)
464			goto cancel;
465		else
466			nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
467	}
468
469	if (gwj->dropped_frames) {
470		if (nla_put_u32(skb, CGW_DROPPED, gwj->dropped_frames) < 0)
471			goto cancel;
472		else
473			nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
 
 
 
474	}
475
476	/* check non default settings of attributes */
477
 
 
 
 
 
478	if (gwj->mod.modtype.and) {
479		memcpy(&mb.cf, &gwj->mod.modframe.and, sizeof(mb.cf));
480		mb.modtype = gwj->mod.modtype.and;
481		if (nla_put(skb, CGW_MOD_AND, sizeof(mb), &mb) < 0)
482			goto cancel;
483		else
484			nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
485	}
486
487	if (gwj->mod.modtype.or) {
488		memcpy(&mb.cf, &gwj->mod.modframe.or, sizeof(mb.cf));
489		mb.modtype = gwj->mod.modtype.or;
490		if (nla_put(skb, CGW_MOD_OR, sizeof(mb), &mb) < 0)
491			goto cancel;
492		else
493			nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
494	}
495
496	if (gwj->mod.modtype.xor) {
497		memcpy(&mb.cf, &gwj->mod.modframe.xor, sizeof(mb.cf));
498		mb.modtype = gwj->mod.modtype.xor;
499		if (nla_put(skb, CGW_MOD_XOR, sizeof(mb), &mb) < 0)
500			goto cancel;
501		else
502			nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
503	}
504
505	if (gwj->mod.modtype.set) {
506		memcpy(&mb.cf, &gwj->mod.modframe.set, sizeof(mb.cf));
507		mb.modtype = gwj->mod.modtype.set;
508		if (nla_put(skb, CGW_MOD_SET, sizeof(mb), &mb) < 0)
509			goto cancel;
510		else
511			nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
 
 
 
512	}
513
514	if (gwj->mod.csumfunc.crc8) {
515		if (nla_put(skb, CGW_CS_CRC8, CGW_CS_CRC8_LEN,
516			    &gwj->mod.csum.crc8) < 0)
517			goto cancel;
518		else
519			nlh->nlmsg_len += NLA_HDRLEN + \
520				NLA_ALIGN(CGW_CS_CRC8_LEN);
521	}
522
523	if (gwj->mod.csumfunc.xor) {
524		if (nla_put(skb, CGW_CS_XOR, CGW_CS_XOR_LEN,
525			    &gwj->mod.csum.xor) < 0)
526			goto cancel;
527		else
528			nlh->nlmsg_len += NLA_HDRLEN + \
529				NLA_ALIGN(CGW_CS_XOR_LEN);
530	}
531
532	if (gwj->gwtype == CGW_TYPE_CAN_CAN) {
533
534		if (gwj->ccgw.filter.can_id || gwj->ccgw.filter.can_mask) {
535			if (nla_put(skb, CGW_FILTER, sizeof(struct can_filter),
536				    &gwj->ccgw.filter) < 0)
537				goto cancel;
538			else
539				nlh->nlmsg_len += NLA_HDRLEN +
540					NLA_ALIGN(sizeof(struct can_filter));
541		}
542
543		if (nla_put_u32(skb, CGW_SRC_IF, gwj->ccgw.src_idx) < 0)
544			goto cancel;
545		else
546			nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
547
548		if (nla_put_u32(skb, CGW_DST_IF, gwj->ccgw.dst_idx) < 0)
549			goto cancel;
550		else
551			nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
552	}
553
554	return skb->len;
 
555
556cancel:
557	nlmsg_cancel(skb, nlh);
558	return -EMSGSIZE;
559}
560
561/* Dump information about all CAN gateway jobs, in response to RTM_GETROUTE */
562static int cgw_dump_jobs(struct sk_buff *skb, struct netlink_callback *cb)
563{
 
564	struct cgw_job *gwj = NULL;
565	struct hlist_node *n;
566	int idx = 0;
567	int s_idx = cb->args[0];
568
569	rcu_read_lock();
570	hlist_for_each_entry_rcu(gwj, n, &cgw_list, list) {
571		if (idx < s_idx)
572			goto cont;
573
574		if (cgw_put_job(skb, gwj) < 0)
 
575			break;
576cont:
577		idx++;
578	}
579	rcu_read_unlock();
580
581	cb->args[0] = idx;
582
583	return skb->len;
584}
585
 
 
 
 
 
 
 
 
 
 
 
 
 
 
586/* check for common and gwtype specific attributes */
587static int cgw_parse_attr(struct nlmsghdr *nlh, struct cf_mod *mod,
588			  u8 gwtype, void *gwtypeattr)
589{
590	struct nlattr *tb[CGW_MAX+1];
591	struct cgw_frame_mod mb;
592	int modidx = 0;
593	int err = 0;
594
595	/* initialize modification & checksum data space */
596	memset(mod, 0, sizeof(*mod));
597
598	err = nlmsg_parse(nlh, sizeof(struct rtcanmsg), tb, CGW_MAX, NULL);
 
599	if (err < 0)
600		return err;
601
 
 
 
 
 
 
 
602	/* check for AND/OR/XOR/SET modifications */
603
604	if (tb[CGW_MOD_AND] &&
605	    nla_len(tb[CGW_MOD_AND]) == CGW_MODATTR_LEN) {
606		nla_memcpy(&mb, tb[CGW_MOD_AND], CGW_MODATTR_LEN);
607
608		canframecpy(&mod->modframe.and, &mb.cf);
609		mod->modtype.and = mb.modtype;
610
611		if (mb.modtype & CGW_MOD_ID)
612			mod->modfunc[modidx++] = mod_and_id;
613
614		if (mb.modtype & CGW_MOD_DLC)
615			mod->modfunc[modidx++] = mod_and_dlc;
616
617		if (mb.modtype & CGW_MOD_DATA)
618			mod->modfunc[modidx++] = mod_and_data;
619	}
620
621	if (tb[CGW_MOD_OR] &&
622	    nla_len(tb[CGW_MOD_OR]) == CGW_MODATTR_LEN) {
623		nla_memcpy(&mb, tb[CGW_MOD_OR], CGW_MODATTR_LEN);
624
625		canframecpy(&mod->modframe.or, &mb.cf);
626		mod->modtype.or = mb.modtype;
627
628		if (mb.modtype & CGW_MOD_ID)
629			mod->modfunc[modidx++] = mod_or_id;
630
631		if (mb.modtype & CGW_MOD_DLC)
632			mod->modfunc[modidx++] = mod_or_dlc;
633
634		if (mb.modtype & CGW_MOD_DATA)
635			mod->modfunc[modidx++] = mod_or_data;
636	}
637
638	if (tb[CGW_MOD_XOR] &&
639	    nla_len(tb[CGW_MOD_XOR]) == CGW_MODATTR_LEN) {
640		nla_memcpy(&mb, tb[CGW_MOD_XOR], CGW_MODATTR_LEN);
641
642		canframecpy(&mod->modframe.xor, &mb.cf);
643		mod->modtype.xor = mb.modtype;
644
645		if (mb.modtype & CGW_MOD_ID)
646			mod->modfunc[modidx++] = mod_xor_id;
647
648		if (mb.modtype & CGW_MOD_DLC)
649			mod->modfunc[modidx++] = mod_xor_dlc;
650
651		if (mb.modtype & CGW_MOD_DATA)
652			mod->modfunc[modidx++] = mod_xor_data;
653	}
654
655	if (tb[CGW_MOD_SET] &&
656	    nla_len(tb[CGW_MOD_SET]) == CGW_MODATTR_LEN) {
657		nla_memcpy(&mb, tb[CGW_MOD_SET], CGW_MODATTR_LEN);
658
659		canframecpy(&mod->modframe.set, &mb.cf);
660		mod->modtype.set = mb.modtype;
661
662		if (mb.modtype & CGW_MOD_ID)
663			mod->modfunc[modidx++] = mod_set_id;
664
665		if (mb.modtype & CGW_MOD_DLC)
666			mod->modfunc[modidx++] = mod_set_dlc;
667
668		if (mb.modtype & CGW_MOD_DATA)
669			mod->modfunc[modidx++] = mod_set_data;
670	}
671
672	/* check for checksum operations after CAN frame modifications */
673	if (modidx) {
674
675		if (tb[CGW_CS_CRC8] &&
676		    nla_len(tb[CGW_CS_CRC8]) == CGW_CS_CRC8_LEN) {
677
678			struct cgw_csum_crc8 *c = (struct cgw_csum_crc8 *)\
679				nla_data(tb[CGW_CS_CRC8]);
680
681			err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
682						 c->result_idx);
683			if (err)
684				return err;
685
686			nla_memcpy(&mod->csum.crc8, tb[CGW_CS_CRC8],
687				   CGW_CS_CRC8_LEN);
688
689			/*
690			 * select dedicated processing function to reduce
691			 * runtime operations in receive hot path.
692			 */
693			if (c->from_idx < 0 || c->to_idx < 0 ||
694			    c->result_idx < 0)
695				mod->csumfunc.crc8 = cgw_csum_crc8_rel;
696			else if (c->from_idx <= c->to_idx)
697				mod->csumfunc.crc8 = cgw_csum_crc8_pos;
698			else
699				mod->csumfunc.crc8 = cgw_csum_crc8_neg;
700		}
701
702		if (tb[CGW_CS_XOR] &&
703		    nla_len(tb[CGW_CS_XOR]) == CGW_CS_XOR_LEN) {
704
705			struct cgw_csum_xor *c = (struct cgw_csum_xor *)\
706				nla_data(tb[CGW_CS_XOR]);
707
708			err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
709						 c->result_idx);
710			if (err)
711				return err;
712
713			nla_memcpy(&mod->csum.xor, tb[CGW_CS_XOR],
714				   CGW_CS_XOR_LEN);
715
716			/*
717			 * select dedicated processing function to reduce
718			 * runtime operations in receive hot path.
719			 */
720			if (c->from_idx < 0 || c->to_idx < 0 ||
721			    c->result_idx < 0)
722				mod->csumfunc.xor = cgw_csum_xor_rel;
723			else if (c->from_idx <= c->to_idx)
724				mod->csumfunc.xor = cgw_csum_xor_pos;
725			else
726				mod->csumfunc.xor = cgw_csum_xor_neg;
727		}
 
 
 
 
728	}
729
730	if (gwtype == CGW_TYPE_CAN_CAN) {
731
732		/* check CGW_TYPE_CAN_CAN specific attributes */
733
734		struct can_can_gw *ccgw = (struct can_can_gw *)gwtypeattr;
735		memset(ccgw, 0, sizeof(*ccgw));
736
737		/* check for can_filter in attributes */
738		if (tb[CGW_FILTER] &&
739		    nla_len(tb[CGW_FILTER]) == sizeof(struct can_filter))
740			nla_memcpy(&ccgw->filter, tb[CGW_FILTER],
741				   sizeof(struct can_filter));
742
743		err = -ENODEV;
744
745		/* specifying two interfaces is mandatory */
746		if (!tb[CGW_SRC_IF] || !tb[CGW_DST_IF])
747			return err;
748
749		if (nla_len(tb[CGW_SRC_IF]) == sizeof(u32))
750			nla_memcpy(&ccgw->src_idx, tb[CGW_SRC_IF],
751				   sizeof(u32));
752
753		if (nla_len(tb[CGW_DST_IF]) == sizeof(u32))
754			nla_memcpy(&ccgw->dst_idx, tb[CGW_DST_IF],
755				   sizeof(u32));
756
757		/* both indices set to 0 for flushing all routing entries */
758		if (!ccgw->src_idx && !ccgw->dst_idx)
759			return 0;
760
761		/* only one index set to 0 is an error */
762		if (!ccgw->src_idx || !ccgw->dst_idx)
763			return err;
764	}
765
766	/* add the checks for other gwtypes here */
767
768	return 0;
769}
770
771static int cgw_create_job(struct sk_buff *skb,  struct nlmsghdr *nlh,
772			  void *arg)
773{
 
774	struct rtcanmsg *r;
775	struct cgw_job *gwj;
 
 
 
776	int err = 0;
777
 
 
 
778	if (nlmsg_len(nlh) < sizeof(*r))
779		return -EINVAL;
780
781	r = nlmsg_data(nlh);
782	if (r->can_family != AF_CAN)
783		return -EPFNOSUPPORT;
784
785	/* so far we only support CAN -> CAN routings */
786	if (r->gwtype != CGW_TYPE_CAN_CAN)
787		return -EINVAL;
788
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
789	gwj = kmem_cache_alloc(cgw_cache, GFP_KERNEL);
790	if (!gwj)
791		return -ENOMEM;
792
793	gwj->handled_frames = 0;
794	gwj->dropped_frames = 0;
 
795	gwj->flags = r->flags;
796	gwj->gwtype = r->gwtype;
 
797
798	err = cgw_parse_attr(nlh, &gwj->mod, CGW_TYPE_CAN_CAN, &gwj->ccgw);
799	if (err < 0)
800		goto out;
801
802	err = -ENODEV;
803
804	/* ifindex == 0 is not allowed for job creation */
805	if (!gwj->ccgw.src_idx || !gwj->ccgw.dst_idx)
806		goto out;
807
808	gwj->src.dev = dev_get_by_index(&init_net, gwj->ccgw.src_idx);
809
810	if (!gwj->src.dev)
811		goto out;
812
813	/* check for CAN netdev not using header_ops - see gw_rcv() */
814	if (gwj->src.dev->type != ARPHRD_CAN || gwj->src.dev->header_ops)
815		goto put_src_out;
816
817	gwj->dst.dev = dev_get_by_index(&init_net, gwj->ccgw.dst_idx);
818
819	if (!gwj->dst.dev)
820		goto put_src_out;
821
822	/* check for CAN netdev not using header_ops - see gw_rcv() */
823	if (gwj->dst.dev->type != ARPHRD_CAN || gwj->dst.dev->header_ops)
824		goto put_src_dst_out;
825
826	ASSERT_RTNL();
827
828	err = cgw_register_filter(gwj);
829	if (!err)
830		hlist_add_head_rcu(&gwj->list, &cgw_list);
831
832put_src_dst_out:
833	dev_put(gwj->dst.dev);
834put_src_out:
835	dev_put(gwj->src.dev);
836out:
837	if (err)
838		kmem_cache_free(cgw_cache, gwj);
839
840	return err;
841}
842
843static void cgw_remove_all_jobs(void)
844{
845	struct cgw_job *gwj = NULL;
846	struct hlist_node *n, *nx;
847
848	ASSERT_RTNL();
849
850	hlist_for_each_entry_safe(gwj, n, nx, &cgw_list, list) {
851		hlist_del(&gwj->list);
852		cgw_unregister_filter(gwj);
853		kfree(gwj);
854	}
855}
856
857static int cgw_remove_job(struct sk_buff *skb,  struct nlmsghdr *nlh, void *arg)
 
858{
 
859	struct cgw_job *gwj = NULL;
860	struct hlist_node *n, *nx;
861	struct rtcanmsg *r;
862	struct cf_mod mod;
863	struct can_can_gw ccgw;
 
864	int err = 0;
865
 
 
 
866	if (nlmsg_len(nlh) < sizeof(*r))
867		return -EINVAL;
868
869	r = nlmsg_data(nlh);
870	if (r->can_family != AF_CAN)
871		return -EPFNOSUPPORT;
872
873	/* so far we only support CAN -> CAN routings */
874	if (r->gwtype != CGW_TYPE_CAN_CAN)
875		return -EINVAL;
876
877	err = cgw_parse_attr(nlh, &mod, CGW_TYPE_CAN_CAN, &ccgw);
878	if (err < 0)
879		return err;
880
881	/* two interface indices both set to 0 => remove all entries */
882	if (!ccgw.src_idx && !ccgw.dst_idx) {
883		cgw_remove_all_jobs();
884		return 0;
885	}
886
887	err = -EINVAL;
888
889	ASSERT_RTNL();
890
891	/* remove only the first matching entry */
892	hlist_for_each_entry_safe(gwj, n, nx, &cgw_list, list) {
893
894		if (gwj->flags != r->flags)
895			continue;
896
897		if (memcmp(&gwj->mod, &mod, sizeof(mod)))
898			continue;
899
 
 
 
 
 
 
 
 
 
 
900		/* if (r->gwtype == CGW_TYPE_CAN_CAN) - is made sure here */
901		if (memcmp(&gwj->ccgw, &ccgw, sizeof(ccgw)))
902			continue;
903
904		hlist_del(&gwj->list);
905		cgw_unregister_filter(gwj);
906		kfree(gwj);
907		err = 0;
908		break;
909	}
910
911	return err;
912}
913
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
914static __init int cgw_module_init(void)
915{
916	printk(banner);
917
 
 
 
 
 
 
 
918	cgw_cache = kmem_cache_create("can_gw", sizeof(struct cgw_job),
919				      0, 0, NULL);
920
921	if (!cgw_cache)
922		return -ENOMEM;
923
924	/* set notifier */
925	notifier.notifier_call = cgw_notifier;
926	register_netdevice_notifier(&notifier);
927
928	if (__rtnl_register(PF_CAN, RTM_GETROUTE, NULL, cgw_dump_jobs, NULL)) {
 
 
929		unregister_netdevice_notifier(&notifier);
930		kmem_cache_destroy(cgw_cache);
931		return -ENOBUFS;
932	}
933
934	/* Only the first call to __rtnl_register can fail */
935	__rtnl_register(PF_CAN, RTM_NEWROUTE, cgw_create_job, NULL, NULL);
936	__rtnl_register(PF_CAN, RTM_DELROUTE, cgw_remove_job, NULL, NULL);
 
 
937
938	return 0;
939}
940
941static __exit void cgw_module_exit(void)
942{
943	rtnl_unregister_all(PF_CAN);
944
945	unregister_netdevice_notifier(&notifier);
946
947	rtnl_lock();
948	cgw_remove_all_jobs();
949	rtnl_unlock();
950
951	rcu_barrier(); /* Wait for completion of call_rcu()'s */
952
953	kmem_cache_destroy(cgw_cache);
954}
955
956module_init(cgw_module_init);
957module_exit(cgw_module_exit);
v4.17
   1/*
   2 * gw.c - CAN frame Gateway/Router/Bridge with netlink interface
   3 *
   4 * Copyright (c) 2017 Volkswagen Group Electronic Research
   5 * All rights reserved.
   6 *
   7 * Redistribution and use in source and binary forms, with or without
   8 * modification, are permitted provided that the following conditions
   9 * are met:
  10 * 1. Redistributions of source code must retain the above copyright
  11 *    notice, this list of conditions and the following disclaimer.
  12 * 2. Redistributions in binary form must reproduce the above copyright
  13 *    notice, this list of conditions and the following disclaimer in the
  14 *    documentation and/or other materials provided with the distribution.
  15 * 3. Neither the name of Volkswagen nor the names of its contributors
  16 *    may be used to endorse or promote products derived from this software
  17 *    without specific prior written permission.
  18 *
  19 * Alternatively, provided that this notice is retained in full, this
  20 * software may be distributed under the terms of the GNU General
  21 * Public License ("GPL") version 2, in which case the provisions of the
  22 * GPL apply INSTEAD OF those given above.
  23 *
  24 * The provided data structures and external interfaces from this code
  25 * are not restricted to be used by modules with a GPL compatible license.
  26 *
  27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  30 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  31 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  32 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  33 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  34 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  35 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  36 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  37 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  38 * DAMAGE.
  39 *
  40 */
  41
  42#include <linux/module.h>
  43#include <linux/init.h>
  44#include <linux/types.h>
  45#include <linux/kernel.h>
  46#include <linux/list.h>
  47#include <linux/spinlock.h>
  48#include <linux/rcupdate.h>
  49#include <linux/rculist.h>
  50#include <linux/net.h>
  51#include <linux/netdevice.h>
  52#include <linux/if_arp.h>
  53#include <linux/skbuff.h>
  54#include <linux/can.h>
  55#include <linux/can/core.h>
  56#include <linux/can/skb.h>
  57#include <linux/can/gw.h>
  58#include <net/rtnetlink.h>
  59#include <net/net_namespace.h>
  60#include <net/sock.h>
  61
  62#define CAN_GW_VERSION "20170425"
  63#define CAN_GW_NAME "can-gw"
 
  64
  65MODULE_DESCRIPTION("PF_CAN netlink gateway");
  66MODULE_LICENSE("Dual BSD/GPL");
  67MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
  68MODULE_ALIAS(CAN_GW_NAME);
  69
  70#define CGW_MIN_HOPS 1
  71#define CGW_MAX_HOPS 6
  72#define CGW_DEFAULT_HOPS 1
  73
  74static unsigned int max_hops __read_mostly = CGW_DEFAULT_HOPS;
  75module_param(max_hops, uint, 0444);
  76MODULE_PARM_DESC(max_hops,
  77		 "maximum " CAN_GW_NAME " routing hops for CAN frames "
  78		 "(valid values: " __stringify(CGW_MIN_HOPS) "-"
  79		 __stringify(CGW_MAX_HOPS) " hops, "
  80		 "default: " __stringify(CGW_DEFAULT_HOPS) ")");
  81
  82static struct notifier_block notifier;
  83static struct kmem_cache *cgw_cache __read_mostly;
  84
  85/* structure that contains the (on-the-fly) CAN frame modifications */
  86struct cf_mod {
  87	struct {
  88		struct can_frame and;
  89		struct can_frame or;
  90		struct can_frame xor;
  91		struct can_frame set;
  92	} modframe;
  93	struct {
  94		u8 and;
  95		u8 or;
  96		u8 xor;
  97		u8 set;
  98	} modtype;
  99	void (*modfunc[MAX_MODFUNCTIONS])(struct can_frame *cf,
 100					  struct cf_mod *mod);
 101
 102	/* CAN frame checksum calculation after CAN frame modifications */
 103	struct {
 104		struct cgw_csum_xor xor;
 105		struct cgw_csum_crc8 crc8;
 106	} csum;
 107	struct {
 108		void (*xor)(struct can_frame *cf, struct cgw_csum_xor *xor);
 109		void (*crc8)(struct can_frame *cf, struct cgw_csum_crc8 *crc8);
 110	} csumfunc;
 111	u32 uid;
 112};
 113
 114
 115/*
 116 * So far we just support CAN -> CAN routing and frame modifications.
 117 *
 118 * The internal can_can_gw structure contains data and attributes for
 119 * a CAN -> CAN gateway job.
 120 */
 121struct can_can_gw {
 122	struct can_filter filter;
 123	int src_idx;
 124	int dst_idx;
 125};
 126
 127/* list entry for CAN gateways jobs */
 128struct cgw_job {
 129	struct hlist_node list;
 130	struct rcu_head rcu;
 131	u32 handled_frames;
 132	u32 dropped_frames;
 133	u32 deleted_frames;
 134	struct cf_mod mod;
 135	union {
 136		/* CAN frame data source */
 137		struct net_device *dev;
 138	} src;
 139	union {
 140		/* CAN frame data destination */
 141		struct net_device *dev;
 142	} dst;
 143	union {
 144		struct can_can_gw ccgw;
 145		/* tbc */
 146	};
 147	u8 gwtype;
 148	u8 limit_hops;
 149	u16 flags;
 150};
 151
 152/* modification functions that are invoked in the hot path in can_can_gw_rcv */
 153
 154#define MODFUNC(func, op) static void func(struct can_frame *cf, \
 155					   struct cf_mod *mod) { op ; }
 156
 157MODFUNC(mod_and_id, cf->can_id &= mod->modframe.and.can_id)
 158MODFUNC(mod_and_dlc, cf->can_dlc &= mod->modframe.and.can_dlc)
 159MODFUNC(mod_and_data, *(u64 *)cf->data &= *(u64 *)mod->modframe.and.data)
 160MODFUNC(mod_or_id, cf->can_id |= mod->modframe.or.can_id)
 161MODFUNC(mod_or_dlc, cf->can_dlc |= mod->modframe.or.can_dlc)
 162MODFUNC(mod_or_data, *(u64 *)cf->data |= *(u64 *)mod->modframe.or.data)
 163MODFUNC(mod_xor_id, cf->can_id ^= mod->modframe.xor.can_id)
 164MODFUNC(mod_xor_dlc, cf->can_dlc ^= mod->modframe.xor.can_dlc)
 165MODFUNC(mod_xor_data, *(u64 *)cf->data ^= *(u64 *)mod->modframe.xor.data)
 166MODFUNC(mod_set_id, cf->can_id = mod->modframe.set.can_id)
 167MODFUNC(mod_set_dlc, cf->can_dlc = mod->modframe.set.can_dlc)
 168MODFUNC(mod_set_data, *(u64 *)cf->data = *(u64 *)mod->modframe.set.data)
 169
 170static inline void canframecpy(struct can_frame *dst, struct can_frame *src)
 171{
 172	/*
 173	 * Copy the struct members separately to ensure that no uninitialized
 174	 * data are copied in the 3 bytes hole of the struct. This is needed
 175	 * to make easy compares of the data in the struct cf_mod.
 176	 */
 177
 178	dst->can_id = src->can_id;
 179	dst->can_dlc = src->can_dlc;
 180	*(u64 *)dst->data = *(u64 *)src->data;
 181}
 182
 183static int cgw_chk_csum_parms(s8 fr, s8 to, s8 re)
 184{
 185	/*
 186	 * absolute dlc values 0 .. 7 => 0 .. 7, e.g. data [0]
 187	 * relative to received dlc -1 .. -8 :
 188	 * e.g. for received dlc = 8
 189	 * -1 => index = 7 (data[7])
 190	 * -3 => index = 5 (data[5])
 191	 * -8 => index = 0 (data[0])
 192	 */
 193
 194	if (fr > -9 && fr < 8 &&
 195	    to > -9 && to < 8 &&
 196	    re > -9 && re < 8)
 197		return 0;
 198	else
 199		return -EINVAL;
 200}
 201
 202static inline int calc_idx(int idx, int rx_dlc)
 203{
 204	if (idx < 0)
 205		return rx_dlc + idx;
 206	else
 207		return idx;
 208}
 209
 210static void cgw_csum_xor_rel(struct can_frame *cf, struct cgw_csum_xor *xor)
 211{
 212	int from = calc_idx(xor->from_idx, cf->can_dlc);
 213	int to = calc_idx(xor->to_idx, cf->can_dlc);
 214	int res = calc_idx(xor->result_idx, cf->can_dlc);
 215	u8 val = xor->init_xor_val;
 216	int i;
 217
 218	if (from < 0 || to < 0 || res < 0)
 219		return;
 220
 221	if (from <= to) {
 222		for (i = from; i <= to; i++)
 223			val ^= cf->data[i];
 224	} else {
 225		for (i = from; i >= to; i--)
 226			val ^= cf->data[i];
 227	}
 228
 229	cf->data[res] = val;
 230}
 231
 232static void cgw_csum_xor_pos(struct can_frame *cf, struct cgw_csum_xor *xor)
 233{
 234	u8 val = xor->init_xor_val;
 235	int i;
 236
 237	for (i = xor->from_idx; i <= xor->to_idx; i++)
 238		val ^= cf->data[i];
 239
 240	cf->data[xor->result_idx] = val;
 241}
 242
 243static void cgw_csum_xor_neg(struct can_frame *cf, struct cgw_csum_xor *xor)
 244{
 245	u8 val = xor->init_xor_val;
 246	int i;
 247
 248	for (i = xor->from_idx; i >= xor->to_idx; i--)
 249		val ^= cf->data[i];
 250
 251	cf->data[xor->result_idx] = val;
 252}
 253
 254static void cgw_csum_crc8_rel(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
 255{
 256	int from = calc_idx(crc8->from_idx, cf->can_dlc);
 257	int to = calc_idx(crc8->to_idx, cf->can_dlc);
 258	int res = calc_idx(crc8->result_idx, cf->can_dlc);
 259	u8 crc = crc8->init_crc_val;
 260	int i;
 261
 262	if (from < 0 || to < 0 || res < 0)
 263		return;
 264
 265	if (from <= to) {
 266		for (i = crc8->from_idx; i <= crc8->to_idx; i++)
 267			crc = crc8->crctab[crc^cf->data[i]];
 268	} else {
 269		for (i = crc8->from_idx; i >= crc8->to_idx; i--)
 270			crc = crc8->crctab[crc^cf->data[i]];
 271	}
 272
 273	switch (crc8->profile) {
 274
 275	case CGW_CRC8PRF_1U8:
 276		crc = crc8->crctab[crc^crc8->profile_data[0]];
 277		break;
 278
 279	case  CGW_CRC8PRF_16U8:
 280		crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
 281		break;
 282
 283	case CGW_CRC8PRF_SFFID_XOR:
 284		crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
 285				   (cf->can_id >> 8 & 0xFF)];
 286		break;
 287
 288	}
 289
 290	cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
 291}
 292
 293static void cgw_csum_crc8_pos(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
 294{
 295	u8 crc = crc8->init_crc_val;
 296	int i;
 297
 298	for (i = crc8->from_idx; i <= crc8->to_idx; i++)
 299		crc = crc8->crctab[crc^cf->data[i]];
 300
 301	switch (crc8->profile) {
 302
 303	case CGW_CRC8PRF_1U8:
 304		crc = crc8->crctab[crc^crc8->profile_data[0]];
 305		break;
 306
 307	case  CGW_CRC8PRF_16U8:
 308		crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
 309		break;
 310
 311	case CGW_CRC8PRF_SFFID_XOR:
 312		crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
 313				   (cf->can_id >> 8 & 0xFF)];
 314		break;
 315	}
 316
 317	cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
 318}
 319
 320static void cgw_csum_crc8_neg(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
 321{
 322	u8 crc = crc8->init_crc_val;
 323	int i;
 324
 325	for (i = crc8->from_idx; i >= crc8->to_idx; i--)
 326		crc = crc8->crctab[crc^cf->data[i]];
 327
 328	switch (crc8->profile) {
 329
 330	case CGW_CRC8PRF_1U8:
 331		crc = crc8->crctab[crc^crc8->profile_data[0]];
 332		break;
 333
 334	case  CGW_CRC8PRF_16U8:
 335		crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
 336		break;
 337
 338	case CGW_CRC8PRF_SFFID_XOR:
 339		crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
 340				   (cf->can_id >> 8 & 0xFF)];
 341		break;
 342	}
 343
 344	cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
 345}
 346
 347/* the receive & process & send function */
 348static void can_can_gw_rcv(struct sk_buff *skb, void *data)
 349{
 350	struct cgw_job *gwj = (struct cgw_job *)data;
 351	struct can_frame *cf;
 352	struct sk_buff *nskb;
 353	int modidx = 0;
 354
 355	/*
 356	 * Do not handle CAN frames routed more than 'max_hops' times.
 357	 * In general we should never catch this delimiter which is intended
 358	 * to cover a misconfiguration protection (e.g. circular CAN routes).
 359	 *
 360	 * The Controller Area Network controllers only accept CAN frames with
 361	 * correct CRCs - which are not visible in the controller registers.
 362	 * According to skbuff.h documentation the csum_start element for IP
 363	 * checksums is undefined/unused when ip_summed == CHECKSUM_UNNECESSARY.
 364	 * Only CAN skbs can be processed here which already have this property.
 365	 */
 366
 367#define cgw_hops(skb) ((skb)->csum_start)
 368
 369	BUG_ON(skb->ip_summed != CHECKSUM_UNNECESSARY);
 370
 371	if (cgw_hops(skb) >= max_hops) {
 372		/* indicate deleted frames due to misconfiguration */
 373		gwj->deleted_frames++;
 374		return;
 375	}
 376
 377	if (!(gwj->dst.dev->flags & IFF_UP)) {
 378		gwj->dropped_frames++;
 379		return;
 380	}
 381
 382	/* is sending the skb back to the incoming interface not allowed? */
 383	if (!(gwj->flags & CGW_FLAGS_CAN_IIF_TX_OK) &&
 384	    can_skb_prv(skb)->ifindex == gwj->dst.dev->ifindex)
 385		return;
 386
 387	/*
 388	 * clone the given skb, which has not been done in can_rcv()
 389	 *
 390	 * When there is at least one modification function activated,
 391	 * we need to copy the skb as we want to modify skb->data.
 392	 */
 393	if (gwj->mod.modfunc[0])
 394		nskb = skb_copy(skb, GFP_ATOMIC);
 395	else
 396		nskb = skb_clone(skb, GFP_ATOMIC);
 397
 398	if (!nskb) {
 399		gwj->dropped_frames++;
 400		return;
 401	}
 402
 403	/* put the incremented hop counter in the cloned skb */
 404	cgw_hops(nskb) = cgw_hops(skb) + 1;
 405
 406	/* first processing of this CAN frame -> adjust to private hop limit */
 407	if (gwj->limit_hops && cgw_hops(nskb) == 1)
 408		cgw_hops(nskb) = max_hops - gwj->limit_hops + 1;
 409
 
 
 410	nskb->dev = gwj->dst.dev;
 411
 412	/* pointer to modifiable CAN frame */
 413	cf = (struct can_frame *)nskb->data;
 414
 415	/* perform preprocessed modification functions if there are any */
 416	while (modidx < MAX_MODFUNCTIONS && gwj->mod.modfunc[modidx])
 417		(*gwj->mod.modfunc[modidx++])(cf, &gwj->mod);
 418
 419	/* check for checksum updates when the CAN frame has been modified */
 420	if (modidx) {
 421		if (gwj->mod.csumfunc.crc8)
 422			(*gwj->mod.csumfunc.crc8)(cf, &gwj->mod.csum.crc8);
 423
 424		if (gwj->mod.csumfunc.xor)
 425			(*gwj->mod.csumfunc.xor)(cf, &gwj->mod.csum.xor);
 426	}
 427
 428	/* clear the skb timestamp if not configured the other way */
 429	if (!(gwj->flags & CGW_FLAGS_CAN_SRC_TSTAMP))
 430		nskb->tstamp = 0;
 431
 432	/* send to netdevice */
 433	if (can_send(nskb, gwj->flags & CGW_FLAGS_CAN_ECHO))
 434		gwj->dropped_frames++;
 435	else
 436		gwj->handled_frames++;
 437}
 438
 439static inline int cgw_register_filter(struct net *net, struct cgw_job *gwj)
 440{
 441	return can_rx_register(net, gwj->src.dev, gwj->ccgw.filter.can_id,
 442			       gwj->ccgw.filter.can_mask, can_can_gw_rcv,
 443			       gwj, "gw", NULL);
 444}
 445
 446static inline void cgw_unregister_filter(struct net *net, struct cgw_job *gwj)
 447{
 448	can_rx_unregister(net, gwj->src.dev, gwj->ccgw.filter.can_id,
 449			  gwj->ccgw.filter.can_mask, can_can_gw_rcv, gwj);
 450}
 451
 452static int cgw_notifier(struct notifier_block *nb,
 453			unsigned long msg, void *ptr)
 454{
 455	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
 456	struct net *net = dev_net(dev);
 457
 
 
 458	if (dev->type != ARPHRD_CAN)
 459		return NOTIFY_DONE;
 460
 461	if (msg == NETDEV_UNREGISTER) {
 462
 463		struct cgw_job *gwj = NULL;
 464		struct hlist_node *nx;
 465
 466		ASSERT_RTNL();
 467
 468		hlist_for_each_entry_safe(gwj, nx, &net->can.cgw_list, list) {
 469
 470			if (gwj->src.dev == dev || gwj->dst.dev == dev) {
 471				hlist_del(&gwj->list);
 472				cgw_unregister_filter(net, gwj);
 473				kmem_cache_free(cgw_cache, gwj);
 474			}
 475		}
 476	}
 477
 478	return NOTIFY_DONE;
 479}
 480
 481static int cgw_put_job(struct sk_buff *skb, struct cgw_job *gwj, int type,
 482		       u32 pid, u32 seq, int flags)
 483{
 484	struct cgw_frame_mod mb;
 485	struct rtcanmsg *rtcan;
 486	struct nlmsghdr *nlh;
 487
 488	nlh = nlmsg_put(skb, pid, seq, type, sizeof(*rtcan), flags);
 489	if (!nlh)
 490		return -EMSGSIZE;
 491
 492	rtcan = nlmsg_data(nlh);
 493	rtcan->can_family = AF_CAN;
 494	rtcan->gwtype = gwj->gwtype;
 495	rtcan->flags = gwj->flags;
 496
 497	/* add statistics if available */
 498
 499	if (gwj->handled_frames) {
 500		if (nla_put_u32(skb, CGW_HANDLED, gwj->handled_frames) < 0)
 501			goto cancel;
 
 
 502	}
 503
 504	if (gwj->dropped_frames) {
 505		if (nla_put_u32(skb, CGW_DROPPED, gwj->dropped_frames) < 0)
 506			goto cancel;
 507	}
 508
 509	if (gwj->deleted_frames) {
 510		if (nla_put_u32(skb, CGW_DELETED, gwj->deleted_frames) < 0)
 511			goto cancel;
 512	}
 513
 514	/* check non default settings of attributes */
 515
 516	if (gwj->limit_hops) {
 517		if (nla_put_u8(skb, CGW_LIM_HOPS, gwj->limit_hops) < 0)
 518			goto cancel;
 519	}
 520
 521	if (gwj->mod.modtype.and) {
 522		memcpy(&mb.cf, &gwj->mod.modframe.and, sizeof(mb.cf));
 523		mb.modtype = gwj->mod.modtype.and;
 524		if (nla_put(skb, CGW_MOD_AND, sizeof(mb), &mb) < 0)
 525			goto cancel;
 
 
 526	}
 527
 528	if (gwj->mod.modtype.or) {
 529		memcpy(&mb.cf, &gwj->mod.modframe.or, sizeof(mb.cf));
 530		mb.modtype = gwj->mod.modtype.or;
 531		if (nla_put(skb, CGW_MOD_OR, sizeof(mb), &mb) < 0)
 532			goto cancel;
 
 
 533	}
 534
 535	if (gwj->mod.modtype.xor) {
 536		memcpy(&mb.cf, &gwj->mod.modframe.xor, sizeof(mb.cf));
 537		mb.modtype = gwj->mod.modtype.xor;
 538		if (nla_put(skb, CGW_MOD_XOR, sizeof(mb), &mb) < 0)
 539			goto cancel;
 
 
 540	}
 541
 542	if (gwj->mod.modtype.set) {
 543		memcpy(&mb.cf, &gwj->mod.modframe.set, sizeof(mb.cf));
 544		mb.modtype = gwj->mod.modtype.set;
 545		if (nla_put(skb, CGW_MOD_SET, sizeof(mb), &mb) < 0)
 546			goto cancel;
 547	}
 548
 549	if (gwj->mod.uid) {
 550		if (nla_put_u32(skb, CGW_MOD_UID, gwj->mod.uid) < 0)
 551			goto cancel;
 552	}
 553
 554	if (gwj->mod.csumfunc.crc8) {
 555		if (nla_put(skb, CGW_CS_CRC8, CGW_CS_CRC8_LEN,
 556			    &gwj->mod.csum.crc8) < 0)
 557			goto cancel;
 
 
 
 558	}
 559
 560	if (gwj->mod.csumfunc.xor) {
 561		if (nla_put(skb, CGW_CS_XOR, CGW_CS_XOR_LEN,
 562			    &gwj->mod.csum.xor) < 0)
 563			goto cancel;
 
 
 
 564	}
 565
 566	if (gwj->gwtype == CGW_TYPE_CAN_CAN) {
 567
 568		if (gwj->ccgw.filter.can_id || gwj->ccgw.filter.can_mask) {
 569			if (nla_put(skb, CGW_FILTER, sizeof(struct can_filter),
 570				    &gwj->ccgw.filter) < 0)
 571				goto cancel;
 
 
 
 572		}
 573
 574		if (nla_put_u32(skb, CGW_SRC_IF, gwj->ccgw.src_idx) < 0)
 575			goto cancel;
 
 
 576
 577		if (nla_put_u32(skb, CGW_DST_IF, gwj->ccgw.dst_idx) < 0)
 578			goto cancel;
 
 
 579	}
 580
 581	nlmsg_end(skb, nlh);
 582	return 0;
 583
 584cancel:
 585	nlmsg_cancel(skb, nlh);
 586	return -EMSGSIZE;
 587}
 588
 589/* Dump information about all CAN gateway jobs, in response to RTM_GETROUTE */
 590static int cgw_dump_jobs(struct sk_buff *skb, struct netlink_callback *cb)
 591{
 592	struct net *net = sock_net(skb->sk);
 593	struct cgw_job *gwj = NULL;
 
 594	int idx = 0;
 595	int s_idx = cb->args[0];
 596
 597	rcu_read_lock();
 598	hlist_for_each_entry_rcu(gwj, &net->can.cgw_list, list) {
 599		if (idx < s_idx)
 600			goto cont;
 601
 602		if (cgw_put_job(skb, gwj, RTM_NEWROUTE, NETLINK_CB(cb->skb).portid,
 603		    cb->nlh->nlmsg_seq, NLM_F_MULTI) < 0)
 604			break;
 605cont:
 606		idx++;
 607	}
 608	rcu_read_unlock();
 609
 610	cb->args[0] = idx;
 611
 612	return skb->len;
 613}
 614
 615static const struct nla_policy cgw_policy[CGW_MAX+1] = {
 616	[CGW_MOD_AND]	= { .len = sizeof(struct cgw_frame_mod) },
 617	[CGW_MOD_OR]	= { .len = sizeof(struct cgw_frame_mod) },
 618	[CGW_MOD_XOR]	= { .len = sizeof(struct cgw_frame_mod) },
 619	[CGW_MOD_SET]	= { .len = sizeof(struct cgw_frame_mod) },
 620	[CGW_CS_XOR]	= { .len = sizeof(struct cgw_csum_xor) },
 621	[CGW_CS_CRC8]	= { .len = sizeof(struct cgw_csum_crc8) },
 622	[CGW_SRC_IF]	= { .type = NLA_U32 },
 623	[CGW_DST_IF]	= { .type = NLA_U32 },
 624	[CGW_FILTER]	= { .len = sizeof(struct can_filter) },
 625	[CGW_LIM_HOPS]	= { .type = NLA_U8 },
 626	[CGW_MOD_UID]	= { .type = NLA_U32 },
 627};
 628
 629/* check for common and gwtype specific attributes */
 630static int cgw_parse_attr(struct nlmsghdr *nlh, struct cf_mod *mod,
 631			  u8 gwtype, void *gwtypeattr, u8 *limhops)
 632{
 633	struct nlattr *tb[CGW_MAX+1];
 634	struct cgw_frame_mod mb;
 635	int modidx = 0;
 636	int err = 0;
 637
 638	/* initialize modification & checksum data space */
 639	memset(mod, 0, sizeof(*mod));
 640
 641	err = nlmsg_parse(nlh, sizeof(struct rtcanmsg), tb, CGW_MAX,
 642			  cgw_policy, NULL);
 643	if (err < 0)
 644		return err;
 645
 646	if (tb[CGW_LIM_HOPS]) {
 647		*limhops = nla_get_u8(tb[CGW_LIM_HOPS]);
 648
 649		if (*limhops < 1 || *limhops > max_hops)
 650			return -EINVAL;
 651	}
 652
 653	/* check for AND/OR/XOR/SET modifications */
 654
 655	if (tb[CGW_MOD_AND]) {
 
 656		nla_memcpy(&mb, tb[CGW_MOD_AND], CGW_MODATTR_LEN);
 657
 658		canframecpy(&mod->modframe.and, &mb.cf);
 659		mod->modtype.and = mb.modtype;
 660
 661		if (mb.modtype & CGW_MOD_ID)
 662			mod->modfunc[modidx++] = mod_and_id;
 663
 664		if (mb.modtype & CGW_MOD_DLC)
 665			mod->modfunc[modidx++] = mod_and_dlc;
 666
 667		if (mb.modtype & CGW_MOD_DATA)
 668			mod->modfunc[modidx++] = mod_and_data;
 669	}
 670
 671	if (tb[CGW_MOD_OR]) {
 
 672		nla_memcpy(&mb, tb[CGW_MOD_OR], CGW_MODATTR_LEN);
 673
 674		canframecpy(&mod->modframe.or, &mb.cf);
 675		mod->modtype.or = mb.modtype;
 676
 677		if (mb.modtype & CGW_MOD_ID)
 678			mod->modfunc[modidx++] = mod_or_id;
 679
 680		if (mb.modtype & CGW_MOD_DLC)
 681			mod->modfunc[modidx++] = mod_or_dlc;
 682
 683		if (mb.modtype & CGW_MOD_DATA)
 684			mod->modfunc[modidx++] = mod_or_data;
 685	}
 686
 687	if (tb[CGW_MOD_XOR]) {
 
 688		nla_memcpy(&mb, tb[CGW_MOD_XOR], CGW_MODATTR_LEN);
 689
 690		canframecpy(&mod->modframe.xor, &mb.cf);
 691		mod->modtype.xor = mb.modtype;
 692
 693		if (mb.modtype & CGW_MOD_ID)
 694			mod->modfunc[modidx++] = mod_xor_id;
 695
 696		if (mb.modtype & CGW_MOD_DLC)
 697			mod->modfunc[modidx++] = mod_xor_dlc;
 698
 699		if (mb.modtype & CGW_MOD_DATA)
 700			mod->modfunc[modidx++] = mod_xor_data;
 701	}
 702
 703	if (tb[CGW_MOD_SET]) {
 
 704		nla_memcpy(&mb, tb[CGW_MOD_SET], CGW_MODATTR_LEN);
 705
 706		canframecpy(&mod->modframe.set, &mb.cf);
 707		mod->modtype.set = mb.modtype;
 708
 709		if (mb.modtype & CGW_MOD_ID)
 710			mod->modfunc[modidx++] = mod_set_id;
 711
 712		if (mb.modtype & CGW_MOD_DLC)
 713			mod->modfunc[modidx++] = mod_set_dlc;
 714
 715		if (mb.modtype & CGW_MOD_DATA)
 716			mod->modfunc[modidx++] = mod_set_data;
 717	}
 718
 719	/* check for checksum operations after CAN frame modifications */
 720	if (modidx) {
 721
 722		if (tb[CGW_CS_CRC8]) {
 723			struct cgw_csum_crc8 *c = nla_data(tb[CGW_CS_CRC8]);
 
 
 
 724
 725			err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
 726						 c->result_idx);
 727			if (err)
 728				return err;
 729
 730			nla_memcpy(&mod->csum.crc8, tb[CGW_CS_CRC8],
 731				   CGW_CS_CRC8_LEN);
 732
 733			/*
 734			 * select dedicated processing function to reduce
 735			 * runtime operations in receive hot path.
 736			 */
 737			if (c->from_idx < 0 || c->to_idx < 0 ||
 738			    c->result_idx < 0)
 739				mod->csumfunc.crc8 = cgw_csum_crc8_rel;
 740			else if (c->from_idx <= c->to_idx)
 741				mod->csumfunc.crc8 = cgw_csum_crc8_pos;
 742			else
 743				mod->csumfunc.crc8 = cgw_csum_crc8_neg;
 744		}
 745
 746		if (tb[CGW_CS_XOR]) {
 747			struct cgw_csum_xor *c = nla_data(tb[CGW_CS_XOR]);
 
 
 
 748
 749			err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
 750						 c->result_idx);
 751			if (err)
 752				return err;
 753
 754			nla_memcpy(&mod->csum.xor, tb[CGW_CS_XOR],
 755				   CGW_CS_XOR_LEN);
 756
 757			/*
 758			 * select dedicated processing function to reduce
 759			 * runtime operations in receive hot path.
 760			 */
 761			if (c->from_idx < 0 || c->to_idx < 0 ||
 762			    c->result_idx < 0)
 763				mod->csumfunc.xor = cgw_csum_xor_rel;
 764			else if (c->from_idx <= c->to_idx)
 765				mod->csumfunc.xor = cgw_csum_xor_pos;
 766			else
 767				mod->csumfunc.xor = cgw_csum_xor_neg;
 768		}
 769
 770		if (tb[CGW_MOD_UID]) {
 771			nla_memcpy(&mod->uid, tb[CGW_MOD_UID], sizeof(u32));
 772		}
 773	}
 774
 775	if (gwtype == CGW_TYPE_CAN_CAN) {
 776
 777		/* check CGW_TYPE_CAN_CAN specific attributes */
 778
 779		struct can_can_gw *ccgw = (struct can_can_gw *)gwtypeattr;
 780		memset(ccgw, 0, sizeof(*ccgw));
 781
 782		/* check for can_filter in attributes */
 783		if (tb[CGW_FILTER])
 
 784			nla_memcpy(&ccgw->filter, tb[CGW_FILTER],
 785				   sizeof(struct can_filter));
 786
 787		err = -ENODEV;
 788
 789		/* specifying two interfaces is mandatory */
 790		if (!tb[CGW_SRC_IF] || !tb[CGW_DST_IF])
 791			return err;
 792
 793		ccgw->src_idx = nla_get_u32(tb[CGW_SRC_IF]);
 794		ccgw->dst_idx = nla_get_u32(tb[CGW_DST_IF]);
 
 
 
 
 
 795
 796		/* both indices set to 0 for flushing all routing entries */
 797		if (!ccgw->src_idx && !ccgw->dst_idx)
 798			return 0;
 799
 800		/* only one index set to 0 is an error */
 801		if (!ccgw->src_idx || !ccgw->dst_idx)
 802			return err;
 803	}
 804
 805	/* add the checks for other gwtypes here */
 806
 807	return 0;
 808}
 809
 810static int cgw_create_job(struct sk_buff *skb,  struct nlmsghdr *nlh,
 811			  struct netlink_ext_ack *extack)
 812{
 813	struct net *net = sock_net(skb->sk);
 814	struct rtcanmsg *r;
 815	struct cgw_job *gwj;
 816	struct cf_mod mod;
 817	struct can_can_gw ccgw;
 818	u8 limhops = 0;
 819	int err = 0;
 820
 821	if (!netlink_capable(skb, CAP_NET_ADMIN))
 822		return -EPERM;
 823
 824	if (nlmsg_len(nlh) < sizeof(*r))
 825		return -EINVAL;
 826
 827	r = nlmsg_data(nlh);
 828	if (r->can_family != AF_CAN)
 829		return -EPFNOSUPPORT;
 830
 831	/* so far we only support CAN -> CAN routings */
 832	if (r->gwtype != CGW_TYPE_CAN_CAN)
 833		return -EINVAL;
 834
 835	err = cgw_parse_attr(nlh, &mod, CGW_TYPE_CAN_CAN, &ccgw, &limhops);
 836	if (err < 0)
 837		return err;
 838
 839	if (mod.uid) {
 840
 841		ASSERT_RTNL();
 842
 843		/* check for updating an existing job with identical uid */
 844		hlist_for_each_entry(gwj, &net->can.cgw_list, list) {
 845
 846			if (gwj->mod.uid != mod.uid)
 847				continue;
 848
 849			/* interfaces & filters must be identical */
 850			if (memcmp(&gwj->ccgw, &ccgw, sizeof(ccgw)))
 851				return -EINVAL;
 852
 853			/* update modifications with disabled softirq & quit */
 854			local_bh_disable();
 855			memcpy(&gwj->mod, &mod, sizeof(mod));
 856			local_bh_enable();
 857			return 0;
 858		}
 859	}
 860
 861	/* ifindex == 0 is not allowed for job creation */
 862	if (!ccgw.src_idx || !ccgw.dst_idx)
 863		return -ENODEV;
 864
 865	gwj = kmem_cache_alloc(cgw_cache, GFP_KERNEL);
 866	if (!gwj)
 867		return -ENOMEM;
 868
 869	gwj->handled_frames = 0;
 870	gwj->dropped_frames = 0;
 871	gwj->deleted_frames = 0;
 872	gwj->flags = r->flags;
 873	gwj->gwtype = r->gwtype;
 874	gwj->limit_hops = limhops;
 875
 876	/* insert already parsed information */
 877	memcpy(&gwj->mod, &mod, sizeof(mod));
 878	memcpy(&gwj->ccgw, &ccgw, sizeof(ccgw));
 879
 880	err = -ENODEV;
 881
 882	gwj->src.dev = __dev_get_by_index(net, gwj->ccgw.src_idx);
 
 
 
 
 883
 884	if (!gwj->src.dev)
 885		goto out;
 886
 887	if (gwj->src.dev->type != ARPHRD_CAN)
 888		goto out;
 
 889
 890	gwj->dst.dev = __dev_get_by_index(net, gwj->ccgw.dst_idx);
 891
 892	if (!gwj->dst.dev)
 893		goto out;
 894
 895	if (gwj->dst.dev->type != ARPHRD_CAN)
 896		goto out;
 
 897
 898	ASSERT_RTNL();
 899
 900	err = cgw_register_filter(net, gwj);
 901	if (!err)
 902		hlist_add_head_rcu(&gwj->list, &net->can.cgw_list);
 
 
 
 
 
 903out:
 904	if (err)
 905		kmem_cache_free(cgw_cache, gwj);
 906
 907	return err;
 908}
 909
 910static void cgw_remove_all_jobs(struct net *net)
 911{
 912	struct cgw_job *gwj = NULL;
 913	struct hlist_node *nx;
 914
 915	ASSERT_RTNL();
 916
 917	hlist_for_each_entry_safe(gwj, nx, &net->can.cgw_list, list) {
 918		hlist_del(&gwj->list);
 919		cgw_unregister_filter(net, gwj);
 920		kmem_cache_free(cgw_cache, gwj);
 921	}
 922}
 923
 924static int cgw_remove_job(struct sk_buff *skb, struct nlmsghdr *nlh,
 925			  struct netlink_ext_ack *extack)
 926{
 927	struct net *net = sock_net(skb->sk);
 928	struct cgw_job *gwj = NULL;
 929	struct hlist_node *nx;
 930	struct rtcanmsg *r;
 931	struct cf_mod mod;
 932	struct can_can_gw ccgw;
 933	u8 limhops = 0;
 934	int err = 0;
 935
 936	if (!netlink_capable(skb, CAP_NET_ADMIN))
 937		return -EPERM;
 938
 939	if (nlmsg_len(nlh) < sizeof(*r))
 940		return -EINVAL;
 941
 942	r = nlmsg_data(nlh);
 943	if (r->can_family != AF_CAN)
 944		return -EPFNOSUPPORT;
 945
 946	/* so far we only support CAN -> CAN routings */
 947	if (r->gwtype != CGW_TYPE_CAN_CAN)
 948		return -EINVAL;
 949
 950	err = cgw_parse_attr(nlh, &mod, CGW_TYPE_CAN_CAN, &ccgw, &limhops);
 951	if (err < 0)
 952		return err;
 953
 954	/* two interface indices both set to 0 => remove all entries */
 955	if (!ccgw.src_idx && !ccgw.dst_idx) {
 956		cgw_remove_all_jobs(net);
 957		return 0;
 958	}
 959
 960	err = -EINVAL;
 961
 962	ASSERT_RTNL();
 963
 964	/* remove only the first matching entry */
 965	hlist_for_each_entry_safe(gwj, nx, &net->can.cgw_list, list) {
 966
 967		if (gwj->flags != r->flags)
 968			continue;
 969
 970		if (gwj->limit_hops != limhops)
 971			continue;
 972
 973		/* we have a match when uid is enabled and identical */
 974		if (gwj->mod.uid || mod.uid) {
 975			if (gwj->mod.uid != mod.uid)
 976				continue;
 977		} else {
 978			/* no uid => check for identical modifications */
 979			if (memcmp(&gwj->mod, &mod, sizeof(mod)))
 980				continue;
 981		}
 982
 983		/* if (r->gwtype == CGW_TYPE_CAN_CAN) - is made sure here */
 984		if (memcmp(&gwj->ccgw, &ccgw, sizeof(ccgw)))
 985			continue;
 986
 987		hlist_del(&gwj->list);
 988		cgw_unregister_filter(net, gwj);
 989		kmem_cache_free(cgw_cache, gwj);
 990		err = 0;
 991		break;
 992	}
 993
 994	return err;
 995}
 996
 997static int __net_init cangw_pernet_init(struct net *net)
 998{
 999	INIT_HLIST_HEAD(&net->can.cgw_list);
1000	return 0;
1001}
1002
1003static void __net_exit cangw_pernet_exit(struct net *net)
1004{
1005	rtnl_lock();
1006	cgw_remove_all_jobs(net);
1007	rtnl_unlock();
1008}
1009
1010static struct pernet_operations cangw_pernet_ops = {
1011	.init = cangw_pernet_init,
1012	.exit = cangw_pernet_exit,
1013};
1014
1015static __init int cgw_module_init(void)
1016{
1017	int ret;
1018
1019	/* sanitize given module parameter */
1020	max_hops = clamp_t(unsigned int, max_hops, CGW_MIN_HOPS, CGW_MAX_HOPS);
1021
1022	pr_info("can: netlink gateway (rev " CAN_GW_VERSION ") max_hops=%d\n",
1023		max_hops);
1024
1025	register_pernet_subsys(&cangw_pernet_ops);
1026	cgw_cache = kmem_cache_create("can_gw", sizeof(struct cgw_job),
1027				      0, 0, NULL);
1028
1029	if (!cgw_cache)
1030		return -ENOMEM;
1031
1032	/* set notifier */
1033	notifier.notifier_call = cgw_notifier;
1034	register_netdevice_notifier(&notifier);
1035
1036	ret = rtnl_register_module(THIS_MODULE, PF_CAN, RTM_GETROUTE,
1037				   NULL, cgw_dump_jobs, 0);
1038	if (ret) {
1039		unregister_netdevice_notifier(&notifier);
1040		kmem_cache_destroy(cgw_cache);
1041		return -ENOBUFS;
1042	}
1043
1044	/* Only the first call to rtnl_register_module can fail */
1045	rtnl_register_module(THIS_MODULE, PF_CAN, RTM_NEWROUTE,
1046			     cgw_create_job, NULL, 0);
1047	rtnl_register_module(THIS_MODULE, PF_CAN, RTM_DELROUTE,
1048			     cgw_remove_job, NULL, 0);
1049
1050	return 0;
1051}
1052
1053static __exit void cgw_module_exit(void)
1054{
1055	rtnl_unregister_all(PF_CAN);
1056
1057	unregister_netdevice_notifier(&notifier);
1058
1059	unregister_pernet_subsys(&cangw_pernet_ops);
 
 
 
1060	rcu_barrier(); /* Wait for completion of call_rcu()'s */
1061
1062	kmem_cache_destroy(cgw_cache);
1063}
1064
1065module_init(cgw_module_init);
1066module_exit(cgw_module_exit);