~hc/RK356X_SDK_RELEASE.git

..	..	@@ -1,7 +1,7 @@
1		-/*
2		- * gw.c - CAN frame Gateway/Router/Bridge with netlink interface
	1	+// SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
	2	+/* gw.c - CAN frame Gateway/Router/Bridge with netlink interface
3	3	*
4		- * Copyright (c) 2017 Volkswagen Group Electronic Research
	4	+ * Copyright (c) 2019 Volkswagen Group Electronic Research
5	5	* All rights reserved.
6	6	*
7	7	* Redistribution and use in source and binary forms, with or without
..	..	@@ -59,7 +59,6 @@
59	59	#include <net/net_namespace.h>
60	60	#include <net/sock.h>
61	61
62		-#define CAN_GW_VERSION "20170425"
63	62	#define CAN_GW_NAME "can-gw"
64	63
65	64	MODULE_DESCRIPTION("PF_CAN netlink gateway");
..	..	@@ -85,10 +84,10 @@
85	84	/* structure that contains the (on-the-fly) CAN frame modifications */
86	85	struct cf_mod {
87	86	struct {
88		- struct can_frame and;
89		- struct can_frame or;
90		- struct can_frame xor;
91		- struct can_frame set;
	87	+ struct canfd_frame and;
	88	+ struct canfd_frame or;
	89	+ struct canfd_frame xor;
	90	+ struct canfd_frame set;
92	91	} modframe;
93	92	struct {
94	93	u8 and;
..	..	@@ -96,7 +95,7 @@
96	95	u8 xor;
97	96	u8 set;
98	97	} modtype;
99		- void (modfunc[MAX_MODFUNCTIONS])(struct can_frame cf,
	98	+ void (modfunc[MAX_MODFUNCTIONS])(struct canfd_frame cf,
100	99	struct cf_mod *mod);
101	100
102	101	/* CAN frame checksum calculation after CAN frame modifications */
..	..	@@ -105,15 +104,15 @@
105	104	struct cgw_csum_crc8 crc8;
106	105	} csum;
107	106	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);
	107	+ void (xor)(struct canfd_frame cf,
	108	+ struct cgw_csum_xor *xor);
	109	+ void (crc8)(struct canfd_frame cf,
	110	+ struct cgw_csum_crc8 *crc8);
110	111	} csumfunc;
111	112	u32 uid;
112	113	};
113	114
114		-
115		-/*
116		- * So far we just support CAN -> CAN routing and frame modifications.
	115	+/* So far we just support CAN -> CAN routing and frame modifications.
117	116	*
118	117	* The internal can_can_gw structure contains data and attributes for
119	118	* a CAN -> CAN gateway job.
..	..	@@ -151,39 +150,88 @@
151	150
152	151	/* modification functions that are invoked in the hot path in can_can_gw_rcv */
153	152
154		-#define MODFUNC(func, op) static void func(struct can_frame *cf, \
	153	+#define MODFUNC(func, op) static void func(struct canfd_frame *cf, \
155	154	struct cf_mod *mod) { op ; }
156	155
157	156	MODFUNC(mod_and_id, cf->can_id &= mod->modframe.and.can_id)
158		-MODFUNC(mod_and_dlc, cf->can_dlc &= mod->modframe.and.can_dlc)
	157	+MODFUNC(mod_and_len, cf->len &= mod->modframe.and.len)
	158	+MODFUNC(mod_and_flags, cf->flags &= mod->modframe.and.flags)
159	159	MODFUNC(mod_and_data, (u64 )cf->data &= (u64 )mod->modframe.and.data)
160	160	MODFUNC(mod_or_id, cf->can_id \|= mod->modframe.or.can_id)
161		-MODFUNC(mod_or_dlc, cf->can_dlc \|= mod->modframe.or.can_dlc)
	161	+MODFUNC(mod_or_len, cf->len \|= mod->modframe.or.len)
	162	+MODFUNC(mod_or_flags, cf->flags \|= mod->modframe.or.flags)
162	163	MODFUNC(mod_or_data, (u64 )cf->data \|= (u64 )mod->modframe.or.data)
163	164	MODFUNC(mod_xor_id, cf->can_id ^= mod->modframe.xor.can_id)
164		-MODFUNC(mod_xor_dlc, cf->can_dlc ^= mod->modframe.xor.can_dlc)
	165	+MODFUNC(mod_xor_len, cf->len ^= mod->modframe.xor.len)
	166	+MODFUNC(mod_xor_flags, cf->flags ^= mod->modframe.xor.flags)
165	167	MODFUNC(mod_xor_data, (u64 )cf->data ^= (u64 )mod->modframe.xor.data)
166	168	MODFUNC(mod_set_id, cf->can_id = mod->modframe.set.can_id)
167		-MODFUNC(mod_set_dlc, cf->can_dlc = mod->modframe.set.can_dlc)
	169	+MODFUNC(mod_set_len, cf->len = mod->modframe.set.len)
	170	+MODFUNC(mod_set_flags, cf->flags = mod->modframe.set.flags)
168	171	MODFUNC(mod_set_data, (u64 )cf->data = (u64 )mod->modframe.set.data)
169	172
170		-static inline void canframecpy(struct can_frame dst, struct can_frame src)
	173	+static void mod_and_fddata(struct canfd_frame cf, struct cf_mod mod)
171	174	{
172		- /*
173		- * Copy the struct members separately to ensure that no uninitialized
	175	+ int i;
	176	+
	177	+ for (i = 0; i < CANFD_MAX_DLEN; i += 8)
	178	+ (u64 )(cf->data + i) &= (u64 )(mod->modframe.and.data + i);
	179	+}
	180	+
	181	+static void mod_or_fddata(struct canfd_frame cf, struct cf_mod mod)
	182	+{
	183	+ int i;
	184	+
	185	+ for (i = 0; i < CANFD_MAX_DLEN; i += 8)
	186	+ (u64 )(cf->data + i) \|= (u64 )(mod->modframe.or.data + i);
	187	+}
	188	+
	189	+static void mod_xor_fddata(struct canfd_frame cf, struct cf_mod mod)
	190	+{
	191	+ int i;
	192	+
	193	+ for (i = 0; i < CANFD_MAX_DLEN; i += 8)
	194	+ (u64 )(cf->data + i) ^= (u64 )(mod->modframe.xor.data + i);
	195	+}
	196	+
	197	+static void mod_set_fddata(struct canfd_frame cf, struct cf_mod mod)
	198	+{
	199	+ memcpy(cf->data, mod->modframe.set.data, CANFD_MAX_DLEN);
	200	+}
	201	+
	202	+static void canframecpy(struct canfd_frame dst, struct can_frame src)
	203	+{
	204	+ /* Copy the struct members separately to ensure that no uninitialized
174	205	* data are copied in the 3 bytes hole of the struct. This is needed
175	206	* to make easy compares of the data in the struct cf_mod.
176	207	*/
177	208
178	209	dst->can_id = src->can_id;
179		- dst->can_dlc = src->can_dlc;
	210	+ dst->len = src->can_dlc;
180	211	(u64 )dst->data = (u64 )src->data;
181	212	}
182	213
183		-static int cgw_chk_csum_parms(s8 fr, s8 to, s8 re)
	214	+static void canfdframecpy(struct canfd_frame dst, struct canfd_frame src)
184	215	{
185		- /*
186		- * absolute dlc values 0 .. 7 => 0 .. 7, e.g. data [0]
	216	+ /* Copy the struct members separately to ensure that no uninitialized
	217	+ * data are copied in the 2 bytes hole of the struct. This is needed
	218	+ * to make easy compares of the data in the struct cf_mod.
	219	+ */
	220	+
	221	+ dst->can_id = src->can_id;
	222	+ dst->flags = src->flags;
	223	+ dst->len = src->len;
	224	+ memcpy(dst->data, src->data, CANFD_MAX_DLEN);
	225	+}
	226	+
	227	+static int cgw_chk_csum_parms(s8 fr, s8 to, s8 re, struct rtcanmsg *r)
	228	+{
	229	+ s8 dlen = CAN_MAX_DLEN;
	230	+
	231	+ if (r->flags & CGW_FLAGS_CAN_FD)
	232	+ dlen = CANFD_MAX_DLEN;
	233	+
	234	+ /* absolute dlc values 0 .. 7 => 0 .. 7, e.g. data [0]
187	235	* relative to received dlc -1 .. -8 :
188	236	* e.g. for received dlc = 8
189	237	* -1 => index = 7 (data[7])
..	..	@@ -191,27 +239,27 @@
191	239	* -8 => index = 0 (data[0])
192	240	*/
193	241
194		- if (fr > -9 && fr < 8 &&
195		- to > -9 && to < 8 &&
196		- re > -9 && re < 8)
	242	+ if (fr >= -dlen && fr < dlen &&
	243	+ to >= -dlen && to < dlen &&
	244	+ re >= -dlen && re < dlen)
197	245	return 0;
198	246	else
199	247	return -EINVAL;
200	248	}
201	249
202		-static inline int calc_idx(int idx, int rx_dlc)
	250	+static inline int calc_idx(int idx, int rx_len)
203	251	{
204	252	if (idx < 0)
205		- return rx_dlc + idx;
	253	+ return rx_len + idx;
206	254	else
207	255	return idx;
208	256	}
209	257
210		-static void cgw_csum_xor_rel(struct can_frame cf, struct cgw_csum_xor xor)
	258	+static void cgw_csum_xor_rel(struct canfd_frame cf, struct cgw_csum_xor xor)
211	259	{
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);
	260	+ int from = calc_idx(xor->from_idx, cf->len);
	261	+ int to = calc_idx(xor->to_idx, cf->len);
	262	+ int res = calc_idx(xor->result_idx, cf->len);
215	263	u8 val = xor->init_xor_val;
216	264	int i;
217	265
..	..	@@ -229,7 +277,7 @@
229	277	cf->data[res] = val;
230	278	}
231	279
232		-static void cgw_csum_xor_pos(struct can_frame cf, struct cgw_csum_xor xor)
	280	+static void cgw_csum_xor_pos(struct canfd_frame cf, struct cgw_csum_xor xor)
233	281	{
234	282	u8 val = xor->init_xor_val;
235	283	int i;
..	..	@@ -240,7 +288,7 @@
240	288	cf->data[xor->result_idx] = val;
241	289	}
242	290
243		-static void cgw_csum_xor_neg(struct can_frame cf, struct cgw_csum_xor xor)
	291	+static void cgw_csum_xor_neg(struct canfd_frame cf, struct cgw_csum_xor xor)
244	292	{
245	293	u8 val = xor->init_xor_val;
246	294	int i;
..	..	@@ -251,11 +299,12 @@
251	299	cf->data[xor->result_idx] = val;
252	300	}
253	301
254		-static void cgw_csum_crc8_rel(struct can_frame cf, struct cgw_csum_crc8 crc8)
	302	+static void cgw_csum_crc8_rel(struct canfd_frame *cf,
	303	+ struct cgw_csum_crc8 *crc8)
255	304	{
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);
	305	+ int from = calc_idx(crc8->from_idx, cf->len);
	306	+ int to = calc_idx(crc8->to_idx, cf->len);
	307	+ int res = calc_idx(crc8->result_idx, cf->len);
259	308	u8 crc = crc8->init_crc_val;
260	309	int i;
261	310
..	..	@@ -264,96 +313,102 @@
264	313
265	314	if (from <= to) {
266	315	for (i = crc8->from_idx; i <= crc8->to_idx; i++)
267		- crc = crc8->crctab[crc^cf->data[i]];
	316	+ crc = crc8->crctab[crc ^ cf->data[i]];
268	317	} else {
269	318	for (i = crc8->from_idx; i >= crc8->to_idx; i--)
270		- crc = crc8->crctab[crc^cf->data[i]];
	319	+ crc = crc8->crctab[crc ^ cf->data[i]];
271	320	}
272	321
273	322	switch (crc8->profile) {
274		-
275	323	case CGW_CRC8PRF_1U8:
276		- crc = crc8->crctab[crc^crc8->profile_data[0]];
	324	+ crc = crc8->crctab[crc ^ crc8->profile_data[0]];
277	325	break;
278	326
279	327	case CGW_CRC8PRF_16U8:
280		- crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
	328	+ crc = crc8->crctab[crc ^ crc8->profile_data[cf->data[1] & 0xF]];
281	329	break;
282	330
283	331	case CGW_CRC8PRF_SFFID_XOR:
284		- crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
	332	+ crc = crc8->crctab[crc ^ (cf->can_id & 0xFF) ^
285	333	(cf->can_id >> 8 & 0xFF)];
286	334	break;
287		-
288	335	}
289	336
290		- cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
	337	+ cf->data[crc8->result_idx] = crc ^ crc8->final_xor_val;
291	338	}
292	339
293		-static void cgw_csum_crc8_pos(struct can_frame cf, struct cgw_csum_crc8 crc8)
	340	+static void cgw_csum_crc8_pos(struct canfd_frame *cf,
	341	+ struct cgw_csum_crc8 *crc8)
294	342	{
295	343	u8 crc = crc8->init_crc_val;
296	344	int i;
297	345
298	346	for (i = crc8->from_idx; i <= crc8->to_idx; i++)
299		- crc = crc8->crctab[crc^cf->data[i]];
	347	+ crc = crc8->crctab[crc ^ cf->data[i]];
300	348
301	349	switch (crc8->profile) {
302		-
303	350	case CGW_CRC8PRF_1U8:
304		- crc = crc8->crctab[crc^crc8->profile_data[0]];
	351	+ crc = crc8->crctab[crc ^ crc8->profile_data[0]];
305	352	break;
306	353
307	354	case CGW_CRC8PRF_16U8:
308		- crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
	355	+ crc = crc8->crctab[crc ^ crc8->profile_data[cf->data[1] & 0xF]];
309	356	break;
310	357
311	358	case CGW_CRC8PRF_SFFID_XOR:
312		- crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
	359	+ crc = crc8->crctab[crc ^ (cf->can_id & 0xFF) ^
313	360	(cf->can_id >> 8 & 0xFF)];
314	361	break;
315	362	}
316	363
317		- cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
	364	+ cf->data[crc8->result_idx] = crc ^ crc8->final_xor_val;
318	365	}
319	366
320		-static void cgw_csum_crc8_neg(struct can_frame cf, struct cgw_csum_crc8 crc8)
	367	+static void cgw_csum_crc8_neg(struct canfd_frame *cf,
	368	+ struct cgw_csum_crc8 *crc8)
321	369	{
322	370	u8 crc = crc8->init_crc_val;
323	371	int i;
324	372
325	373	for (i = crc8->from_idx; i >= crc8->to_idx; i--)
326		- crc = crc8->crctab[crc^cf->data[i]];
	374	+ crc = crc8->crctab[crc ^ cf->data[i]];
327	375
328	376	switch (crc8->profile) {
329		-
330	377	case CGW_CRC8PRF_1U8:
331		- crc = crc8->crctab[crc^crc8->profile_data[0]];
	378	+ crc = crc8->crctab[crc ^ crc8->profile_data[0]];
332	379	break;
333	380
334	381	case CGW_CRC8PRF_16U8:
335		- crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
	382	+ crc = crc8->crctab[crc ^ crc8->profile_data[cf->data[1] & 0xF]];
336	383	break;
337	384
338	385	case CGW_CRC8PRF_SFFID_XOR:
339		- crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
	386	+ crc = crc8->crctab[crc ^ (cf->can_id & 0xFF) ^
340	387	(cf->can_id >> 8 & 0xFF)];
341	388	break;
342	389	}
343	390
344		- cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
	391	+ cf->data[crc8->result_idx] = crc ^ crc8->final_xor_val;
345	392	}
346	393
347	394	/* the receive & process & send function */
348	395	static void can_can_gw_rcv(struct sk_buff skb, void data)
349	396	{
350	397	struct cgw_job gwj = (struct cgw_job )data;
351		- struct can_frame *cf;
	398	+ struct canfd_frame *cf;
352	399	struct sk_buff *nskb;
353	400	int modidx = 0;
354	401
355		- /*
356		- * Do not handle CAN frames routed more than 'max_hops' times.
	402	+ /* process strictly Classic CAN or CAN FD frames */
	403	+ if (gwj->flags & CGW_FLAGS_CAN_FD) {
	404	+ if (skb->len != CANFD_MTU)
	405	+ return;
	406	+ } else {
	407	+ if (skb->len != CAN_MTU)
	408	+ return;
	409	+ }
	410	+
	411	+ /* Do not handle CAN frames routed more than 'max_hops' times.
357	412	* In general we should never catch this delimiter which is intended
358	413	* to cover a misconfiguration protection (e.g. circular CAN routes).
359	414	*
..	..	@@ -384,8 +439,7 @@
384	439	can_skb_prv(skb)->ifindex == gwj->dst.dev->ifindex)
385	440	return;
386	441
387		- /*
388		- * clone the given skb, which has not been done in can_rcv()
	442	+ /* clone the given skb, which has not been done in can_rcv()
389	443	*
390	444	* When there is at least one modification function activated,
391	445	* we need to copy the skb as we want to modify skb->data.
..	..	@@ -410,7 +464,7 @@
410	464	nskb->dev = gwj->dst.dev;
411	465
412	466	/* pointer to modifiable CAN frame */
413		- cf = (struct can_frame *)nskb->data;
	467	+ cf = (struct canfd_frame *)nskb->data;
414	468
415	469	/* perform preprocessed modification functions if there are any */
416	470	while (modidx < MAX_MODFUNCTIONS && gwj->mod.modfunc[modidx])
..	..	@@ -419,26 +473,22 @@
419	473	/* Has the CAN frame been modified? */
420	474	if (modidx) {
421	475	/* get available space for the processed CAN frame type */
422		- int max_len = nskb->len - offsetof(struct can_frame, data);
	476	+ int max_len = nskb->len - offsetof(struct canfd_frame, data);
423	477
424	478	/* dlc may have changed, make sure it fits to the CAN frame */
425		- if (cf->can_dlc > max_len)
426		- goto out_delete;
	479	+ if (cf->len > max_len) {
	480	+ /* delete frame due to misconfiguration */
	481	+ gwj->deleted_frames++;
	482	+ kfree_skb(nskb);
	483	+ return;
	484	+ }
427	485
428		- /* check for checksum updates in classic CAN length only */
429		- if (gwj->mod.csumfunc.crc8) {
430		- if (cf->can_dlc > 8)
431		- goto out_delete;
432		-
	486	+ /* check for checksum updates */
	487	+ if (gwj->mod.csumfunc.crc8)
433	488	(*gwj->mod.csumfunc.crc8)(cf, &gwj->mod.csum.crc8);
434		- }
435	489
436		- if (gwj->mod.csumfunc.xor) {
437		- if (cf->can_dlc > 8)
438		- goto out_delete;
439		-
	490	+ if (gwj->mod.csumfunc.xor)
440	491	(*gwj->mod.csumfunc.xor)(cf, &gwj->mod.csum.xor);
441		- }
442	492	}
443	493
444	494	/* clear the skb timestamp if not configured the other way */
..	..	@@ -450,14 +500,6 @@
450	500	gwj->dropped_frames++;
451	501	else
452	502	gwj->handled_frames++;
453		-
454		- return;
455		-
456		- out_delete:
457		- /* delete frame due to misconfiguration */
458		- gwj->deleted_frames++;
459		- kfree_skb(nskb);
460		- return;
461	503	}
462	504
463	505	static inline int cgw_register_filter(struct net net, struct cgw_job gwj)
..	..	@@ -483,14 +525,12 @@
483	525	return NOTIFY_DONE;
484	526
485	527	if (msg == NETDEV_UNREGISTER) {
486		-
487	528	struct cgw_job *gwj = NULL;
488	529	struct hlist_node *nx;
489	530
490	531	ASSERT_RTNL();
491	532
492	533	hlist_for_each_entry_safe(gwj, nx, &net->can.cgw_list, list) {
493		-
494	534	if (gwj->src.dev == dev \|\| gwj->dst.dev == dev) {
495	535	hlist_del(&gwj->list);
496	536	cgw_unregister_filter(net, gwj);
..	..	@@ -506,7 +546,6 @@
506	546	static int cgw_put_job(struct sk_buff skb, struct cgw_job gwj, int type,
507	547	u32 pid, u32 seq, int flags)
508	548	{
509		- struct cgw_frame_mod mb;
510	549	struct rtcanmsg *rtcan;
511	550	struct nlmsghdr *nlh;
512	551
..	..	@@ -543,32 +582,66 @@
543	582	goto cancel;
544	583	}
545	584
546		- if (gwj->mod.modtype.and) {
547		- memcpy(&mb.cf, &gwj->mod.modframe.and, sizeof(mb.cf));
548		- mb.modtype = gwj->mod.modtype.and;
549		- if (nla_put(skb, CGW_MOD_AND, sizeof(mb), &mb) < 0)
550		- goto cancel;
551		- }
	585	+ if (gwj->flags & CGW_FLAGS_CAN_FD) {
	586	+ struct cgw_fdframe_mod mb;
552	587
553		- if (gwj->mod.modtype.or) {
554		- memcpy(&mb.cf, &gwj->mod.modframe.or, sizeof(mb.cf));
555		- mb.modtype = gwj->mod.modtype.or;
556		- if (nla_put(skb, CGW_MOD_OR, sizeof(mb), &mb) < 0)
557		- goto cancel;
558		- }
	588	+ if (gwj->mod.modtype.and) {
	589	+ memcpy(&mb.cf, &gwj->mod.modframe.and, sizeof(mb.cf));
	590	+ mb.modtype = gwj->mod.modtype.and;
	591	+ if (nla_put(skb, CGW_FDMOD_AND, sizeof(mb), &mb) < 0)
	592	+ goto cancel;
	593	+ }
559	594
560		- if (gwj->mod.modtype.xor) {
561		- memcpy(&mb.cf, &gwj->mod.modframe.xor, sizeof(mb.cf));
562		- mb.modtype = gwj->mod.modtype.xor;
563		- if (nla_put(skb, CGW_MOD_XOR, sizeof(mb), &mb) < 0)
564		- goto cancel;
565		- }
	595	+ if (gwj->mod.modtype.or) {
	596	+ memcpy(&mb.cf, &gwj->mod.modframe.or, sizeof(mb.cf));
	597	+ mb.modtype = gwj->mod.modtype.or;
	598	+ if (nla_put(skb, CGW_FDMOD_OR, sizeof(mb), &mb) < 0)
	599	+ goto cancel;
	600	+ }
566	601
567		- if (gwj->mod.modtype.set) {
568		- memcpy(&mb.cf, &gwj->mod.modframe.set, sizeof(mb.cf));
569		- mb.modtype = gwj->mod.modtype.set;
570		- if (nla_put(skb, CGW_MOD_SET, sizeof(mb), &mb) < 0)
571		- goto cancel;
	602	+ if (gwj->mod.modtype.xor) {
	603	+ memcpy(&mb.cf, &gwj->mod.modframe.xor, sizeof(mb.cf));
	604	+ mb.modtype = gwj->mod.modtype.xor;
	605	+ if (nla_put(skb, CGW_FDMOD_XOR, sizeof(mb), &mb) < 0)
	606	+ goto cancel;
	607	+ }
	608	+
	609	+ if (gwj->mod.modtype.set) {
	610	+ memcpy(&mb.cf, &gwj->mod.modframe.set, sizeof(mb.cf));
	611	+ mb.modtype = gwj->mod.modtype.set;
	612	+ if (nla_put(skb, CGW_FDMOD_SET, sizeof(mb), &mb) < 0)
	613	+ goto cancel;
	614	+ }
	615	+ } else {
	616	+ struct cgw_frame_mod mb;
	617	+
	618	+ if (gwj->mod.modtype.and) {
	619	+ memcpy(&mb.cf, &gwj->mod.modframe.and, sizeof(mb.cf));
	620	+ mb.modtype = gwj->mod.modtype.and;
	621	+ if (nla_put(skb, CGW_MOD_AND, sizeof(mb), &mb) < 0)
	622	+ goto cancel;
	623	+ }
	624	+
	625	+ if (gwj->mod.modtype.or) {
	626	+ memcpy(&mb.cf, &gwj->mod.modframe.or, sizeof(mb.cf));
	627	+ mb.modtype = gwj->mod.modtype.or;
	628	+ if (nla_put(skb, CGW_MOD_OR, sizeof(mb), &mb) < 0)
	629	+ goto cancel;
	630	+ }
	631	+
	632	+ if (gwj->mod.modtype.xor) {
	633	+ memcpy(&mb.cf, &gwj->mod.modframe.xor, sizeof(mb.cf));
	634	+ mb.modtype = gwj->mod.modtype.xor;
	635	+ if (nla_put(skb, CGW_MOD_XOR, sizeof(mb), &mb) < 0)
	636	+ goto cancel;
	637	+ }
	638	+
	639	+ if (gwj->mod.modtype.set) {
	640	+ memcpy(&mb.cf, &gwj->mod.modframe.set, sizeof(mb.cf));
	641	+ mb.modtype = gwj->mod.modtype.set;
	642	+ if (nla_put(skb, CGW_MOD_SET, sizeof(mb), &mb) < 0)
	643	+ goto cancel;
	644	+ }
572	645	}
573	646
574	647	if (gwj->mod.uid) {
..	..	@@ -589,7 +662,6 @@
589	662	}
590	663
591	664	if (gwj->gwtype == CGW_TYPE_CAN_CAN) {
592		-
593	665	if (gwj->ccgw.filter.can_id \|\| gwj->ccgw.filter.can_mask) {
594	666	if (nla_put(skb, CGW_FILTER, sizeof(struct can_filter),
595	667	&gwj->ccgw.filter) < 0)
..	..	@@ -624,8 +696,9 @@
624	696	if (idx < s_idx)
625	697	goto cont;
626	698
627		- if (cgw_put_job(skb, gwj, RTM_NEWROUTE, NETLINK_CB(cb->skb).portid,
628		- cb->nlh->nlmsg_seq, NLM_F_MULTI) < 0)
	699	+ if (cgw_put_job(skb, gwj, RTM_NEWROUTE,
	700	+ NETLINK_CB(cb->skb).portid,
	701	+ cb->nlh->nlmsg_seq, NLM_F_MULTI) < 0)
629	702	break;
630	703	cont:
631	704	idx++;
..	..	@@ -637,7 +710,7 @@
637	710	return skb->len;
638	711	}
639	712
640		-static const struct nla_policy cgw_policy[CGW_MAX+1] = {
	713	+static const struct nla_policy cgw_policy[CGW_MAX + 1] = {
641	714	[CGW_MOD_AND] = { .len = sizeof(struct cgw_frame_mod) },
642	715	[CGW_MOD_OR] = { .len = sizeof(struct cgw_frame_mod) },
643	716	[CGW_MOD_XOR] = { .len = sizeof(struct cgw_frame_mod) },
..	..	@@ -649,22 +722,26 @@
649	722	[CGW_FILTER] = { .len = sizeof(struct can_filter) },
650	723	[CGW_LIM_HOPS] = { .type = NLA_U8 },
651	724	[CGW_MOD_UID] = { .type = NLA_U32 },
	725	+ [CGW_FDMOD_AND] = { .len = sizeof(struct cgw_fdframe_mod) },
	726	+ [CGW_FDMOD_OR] = { .len = sizeof(struct cgw_fdframe_mod) },
	727	+ [CGW_FDMOD_XOR] = { .len = sizeof(struct cgw_fdframe_mod) },
	728	+ [CGW_FDMOD_SET] = { .len = sizeof(struct cgw_fdframe_mod) },
652	729	};
653	730
654	731	/* check for common and gwtype specific attributes */
655	732	static int cgw_parse_attr(struct nlmsghdr nlh, struct cf_mod mod,
656	733	u8 gwtype, void gwtypeattr, u8 limhops)
657	734	{
658		- struct nlattr *tb[CGW_MAX+1];
659		- struct cgw_frame_mod mb;
	735	+ struct nlattr *tb[CGW_MAX + 1];
	736	+ struct rtcanmsg *r = nlmsg_data(nlh);
660	737	int modidx = 0;
661	738	int err = 0;
662	739
663	740	/* initialize modification & checksum data space */
664	741	memset(mod, 0, sizeof(*mod));
665	742
666		- err = nlmsg_parse(nlh, sizeof(struct rtcanmsg), tb, CGW_MAX,
667		- cgw_policy, NULL);
	743	+ err = nlmsg_parse_deprecated(nlh, sizeof(struct rtcanmsg), tb,
	744	+ CGW_MAX, cgw_policy, NULL);
668	745	if (err < 0)
669	746	return err;
670	747
..	..	@@ -676,87 +753,166 @@
676	753	}
677	754
678	755	/* check for AND/OR/XOR/SET modifications */
	756	+ if (r->flags & CGW_FLAGS_CAN_FD) {
	757	+ struct cgw_fdframe_mod mb;
679	758
680		- if (tb[CGW_MOD_AND]) {
681		- nla_memcpy(&mb, tb[CGW_MOD_AND], CGW_MODATTR_LEN);
	759	+ if (tb[CGW_FDMOD_AND]) {
	760	+ nla_memcpy(&mb, tb[CGW_FDMOD_AND], CGW_FDMODATTR_LEN);
682	761
683		- canframecpy(&mod->modframe.and, &mb.cf);
684		- mod->modtype.and = mb.modtype;
	762	+ canfdframecpy(&mod->modframe.and, &mb.cf);
	763	+ mod->modtype.and = mb.modtype;
685	764
686		- if (mb.modtype & CGW_MOD_ID)
687		- mod->modfunc[modidx++] = mod_and_id;
	765	+ if (mb.modtype & CGW_MOD_ID)
	766	+ mod->modfunc[modidx++] = mod_and_id;
688	767
689		- if (mb.modtype & CGW_MOD_DLC)
690		- mod->modfunc[modidx++] = mod_and_dlc;
	768	+ if (mb.modtype & CGW_MOD_LEN)
	769	+ mod->modfunc[modidx++] = mod_and_len;
691	770
692		- if (mb.modtype & CGW_MOD_DATA)
693		- mod->modfunc[modidx++] = mod_and_data;
694		- }
	771	+ if (mb.modtype & CGW_MOD_FLAGS)
	772	+ mod->modfunc[modidx++] = mod_and_flags;
695	773
696		- if (tb[CGW_MOD_OR]) {
697		- nla_memcpy(&mb, tb[CGW_MOD_OR], CGW_MODATTR_LEN);
	774	+ if (mb.modtype & CGW_MOD_DATA)
	775	+ mod->modfunc[modidx++] = mod_and_fddata;
	776	+ }
698	777
699		- canframecpy(&mod->modframe.or, &mb.cf);
700		- mod->modtype.or = mb.modtype;
	778	+ if (tb[CGW_FDMOD_OR]) {
	779	+ nla_memcpy(&mb, tb[CGW_FDMOD_OR], CGW_FDMODATTR_LEN);
701	780
702		- if (mb.modtype & CGW_MOD_ID)
703		- mod->modfunc[modidx++] = mod_or_id;
	781	+ canfdframecpy(&mod->modframe.or, &mb.cf);
	782	+ mod->modtype.or = mb.modtype;
704	783
705		- if (mb.modtype & CGW_MOD_DLC)
706		- mod->modfunc[modidx++] = mod_or_dlc;
	784	+ if (mb.modtype & CGW_MOD_ID)
	785	+ mod->modfunc[modidx++] = mod_or_id;
707	786
708		- if (mb.modtype & CGW_MOD_DATA)
709		- mod->modfunc[modidx++] = mod_or_data;
710		- }
	787	+ if (mb.modtype & CGW_MOD_LEN)
	788	+ mod->modfunc[modidx++] = mod_or_len;
711	789
712		- if (tb[CGW_MOD_XOR]) {
713		- nla_memcpy(&mb, tb[CGW_MOD_XOR], CGW_MODATTR_LEN);
	790	+ if (mb.modtype & CGW_MOD_FLAGS)
	791	+ mod->modfunc[modidx++] = mod_or_flags;
714	792
715		- canframecpy(&mod->modframe.xor, &mb.cf);
716		- mod->modtype.xor = mb.modtype;
	793	+ if (mb.modtype & CGW_MOD_DATA)
	794	+ mod->modfunc[modidx++] = mod_or_fddata;
	795	+ }
717	796
718		- if (mb.modtype & CGW_MOD_ID)
719		- mod->modfunc[modidx++] = mod_xor_id;
	797	+ if (tb[CGW_FDMOD_XOR]) {
	798	+ nla_memcpy(&mb, tb[CGW_FDMOD_XOR], CGW_FDMODATTR_LEN);
720	799
721		- if (mb.modtype & CGW_MOD_DLC)
722		- mod->modfunc[modidx++] = mod_xor_dlc;
	800	+ canfdframecpy(&mod->modframe.xor, &mb.cf);
	801	+ mod->modtype.xor = mb.modtype;
723	802
724		- if (mb.modtype & CGW_MOD_DATA)
725		- mod->modfunc[modidx++] = mod_xor_data;
726		- }
	803	+ if (mb.modtype & CGW_MOD_ID)
	804	+ mod->modfunc[modidx++] = mod_xor_id;
727	805
728		- if (tb[CGW_MOD_SET]) {
729		- nla_memcpy(&mb, tb[CGW_MOD_SET], CGW_MODATTR_LEN);
	806	+ if (mb.modtype & CGW_MOD_LEN)
	807	+ mod->modfunc[modidx++] = mod_xor_len;
730	808
731		- canframecpy(&mod->modframe.set, &mb.cf);
732		- mod->modtype.set = mb.modtype;
	809	+ if (mb.modtype & CGW_MOD_FLAGS)
	810	+ mod->modfunc[modidx++] = mod_xor_flags;
733	811
734		- if (mb.modtype & CGW_MOD_ID)
735		- mod->modfunc[modidx++] = mod_set_id;
	812	+ if (mb.modtype & CGW_MOD_DATA)
	813	+ mod->modfunc[modidx++] = mod_xor_fddata;
	814	+ }
736	815
737		- if (mb.modtype & CGW_MOD_DLC)
738		- mod->modfunc[modidx++] = mod_set_dlc;
	816	+ if (tb[CGW_FDMOD_SET]) {
	817	+ nla_memcpy(&mb, tb[CGW_FDMOD_SET], CGW_FDMODATTR_LEN);
739	818
740		- if (mb.modtype & CGW_MOD_DATA)
741		- mod->modfunc[modidx++] = mod_set_data;
	819	+ canfdframecpy(&mod->modframe.set, &mb.cf);
	820	+ mod->modtype.set = mb.modtype;
	821	+
	822	+ if (mb.modtype & CGW_MOD_ID)
	823	+ mod->modfunc[modidx++] = mod_set_id;
	824	+
	825	+ if (mb.modtype & CGW_MOD_LEN)
	826	+ mod->modfunc[modidx++] = mod_set_len;
	827	+
	828	+ if (mb.modtype & CGW_MOD_FLAGS)
	829	+ mod->modfunc[modidx++] = mod_set_flags;
	830	+
	831	+ if (mb.modtype & CGW_MOD_DATA)
	832	+ mod->modfunc[modidx++] = mod_set_fddata;
	833	+ }
	834	+ } else {
	835	+ struct cgw_frame_mod mb;
	836	+
	837	+ if (tb[CGW_MOD_AND]) {
	838	+ nla_memcpy(&mb, tb[CGW_MOD_AND], CGW_MODATTR_LEN);
	839	+
	840	+ canframecpy(&mod->modframe.and, &mb.cf);
	841	+ mod->modtype.and = mb.modtype;
	842	+
	843	+ if (mb.modtype & CGW_MOD_ID)
	844	+ mod->modfunc[modidx++] = mod_and_id;
	845	+
	846	+ if (mb.modtype & CGW_MOD_LEN)
	847	+ mod->modfunc[modidx++] = mod_and_len;
	848	+
	849	+ if (mb.modtype & CGW_MOD_DATA)
	850	+ mod->modfunc[modidx++] = mod_and_data;
	851	+ }
	852	+
	853	+ if (tb[CGW_MOD_OR]) {
	854	+ nla_memcpy(&mb, tb[CGW_MOD_OR], CGW_MODATTR_LEN);
	855	+
	856	+ canframecpy(&mod->modframe.or, &mb.cf);
	857	+ mod->modtype.or = mb.modtype;
	858	+
	859	+ if (mb.modtype & CGW_MOD_ID)
	860	+ mod->modfunc[modidx++] = mod_or_id;
	861	+
	862	+ if (mb.modtype & CGW_MOD_LEN)
	863	+ mod->modfunc[modidx++] = mod_or_len;
	864	+
	865	+ if (mb.modtype & CGW_MOD_DATA)
	866	+ mod->modfunc[modidx++] = mod_or_data;
	867	+ }
	868	+
	869	+ if (tb[CGW_MOD_XOR]) {
	870	+ nla_memcpy(&mb, tb[CGW_MOD_XOR], CGW_MODATTR_LEN);
	871	+
	872	+ canframecpy(&mod->modframe.xor, &mb.cf);
	873	+ mod->modtype.xor = mb.modtype;
	874	+
	875	+ if (mb.modtype & CGW_MOD_ID)
	876	+ mod->modfunc[modidx++] = mod_xor_id;
	877	+
	878	+ if (mb.modtype & CGW_MOD_LEN)
	879	+ mod->modfunc[modidx++] = mod_xor_len;
	880	+
	881	+ if (mb.modtype & CGW_MOD_DATA)
	882	+ mod->modfunc[modidx++] = mod_xor_data;
	883	+ }
	884	+
	885	+ if (tb[CGW_MOD_SET]) {
	886	+ nla_memcpy(&mb, tb[CGW_MOD_SET], CGW_MODATTR_LEN);
	887	+
	888	+ canframecpy(&mod->modframe.set, &mb.cf);
	889	+ mod->modtype.set = mb.modtype;
	890	+
	891	+ if (mb.modtype & CGW_MOD_ID)
	892	+ mod->modfunc[modidx++] = mod_set_id;
	893	+
	894	+ if (mb.modtype & CGW_MOD_LEN)
	895	+ mod->modfunc[modidx++] = mod_set_len;
	896	+
	897	+ if (mb.modtype & CGW_MOD_DATA)
	898	+ mod->modfunc[modidx++] = mod_set_data;
	899	+ }
742	900	}
743	901
744	902	/* check for checksum operations after CAN frame modifications */
745	903	if (modidx) {
746		-
747	904	if (tb[CGW_CS_CRC8]) {
748	905	struct cgw_csum_crc8 *c = nla_data(tb[CGW_CS_CRC8]);
749	906
750	907	err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
751		- c->result_idx);
	908	+ c->result_idx, r);
752	909	if (err)
753	910	return err;
754	911
755	912	nla_memcpy(&mod->csum.crc8, tb[CGW_CS_CRC8],
756	913	CGW_CS_CRC8_LEN);
757	914
758		- /*
759		- * select dedicated processing function to reduce
	915	+ /* select dedicated processing function to reduce
760	916	* runtime operations in receive hot path.
761	917	*/
762	918	if (c->from_idx < 0 \|\| c->to_idx < 0 \|\|
..	..	@@ -772,15 +928,14 @@
772	928	struct cgw_csum_xor *c = nla_data(tb[CGW_CS_XOR]);
773	929
774	930	err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
775		- c->result_idx);
	931	+ c->result_idx, r);
776	932	if (err)
777	933	return err;
778	934
779	935	nla_memcpy(&mod->csum.xor, tb[CGW_CS_XOR],
780	936	CGW_CS_XOR_LEN);
781	937
782		- /*
783		- * select dedicated processing function to reduce
	938	+ /* select dedicated processing function to reduce
784	939	* runtime operations in receive hot path.
785	940	*/
786	941	if (c->from_idx < 0 \|\| c->to_idx < 0 \|\|
..	..	@@ -792,16 +947,14 @@
792	947	mod->csumfunc.xor = cgw_csum_xor_neg;
793	948	}
794	949
795		- if (tb[CGW_MOD_UID]) {
	950	+ if (tb[CGW_MOD_UID])
796	951	nla_memcpy(&mod->uid, tb[CGW_MOD_UID], sizeof(u32));
797		- }
798	952	}
799	953
800	954	if (gwtype == CGW_TYPE_CAN_CAN) {
801		-
802	955	/* check CGW_TYPE_CAN_CAN specific attributes */
803		-
804	956	struct can_can_gw ccgw = (struct can_can_gw )gwtypeattr;
	957	+
805	958	memset(ccgw, 0, sizeof(*ccgw));
806	959
807	960	/* check for can_filter in attributes */
..	..	@@ -862,12 +1015,10 @@
862	1015	return err;
863	1016
864	1017	if (mod.uid) {
865		-
866	1018	ASSERT_RTNL();
867	1019
868	1020	/* check for updating an existing job with identical uid */
869	1021	hlist_for_each_entry(gwj, &net->can.cgw_list, list) {
870		-
871	1022	if (gwj->mod.uid != mod.uid)
872	1023	continue;
873	1024
..	..	@@ -989,7 +1140,6 @@
989	1140
990	1141	/* remove only the first matching entry */
991	1142	hlist_for_each_entry_safe(gwj, nx, &net->can.cgw_list, list) {
992		-
993	1143	if (gwj->flags != r->flags)
994	1144	continue;
995	1145
..	..	@@ -1046,8 +1196,7 @@
1046	1196	/* sanitize given module parameter */
1047	1197	max_hops = clamp_t(unsigned int, max_hops, CGW_MIN_HOPS, CGW_MAX_HOPS);
1048	1198
1049		- pr_info("can: netlink gateway (rev " CAN_GW_VERSION ") max_hops=%d\n",
1050		- max_hops);
	1199	+ pr_info("can: netlink gateway - max_hops=%d\n", max_hops);
1051	1200
1052	1201	ret = register_pernet_subsys(&cangw_pernet_ops);
1053	1202	if (ret)