aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorOliver Hartkopp <socketcan@hartkopp.net>2011-09-01 00:23:23 -0400
committerDavid S. Miller <davem@davemloft.net>2011-09-16 17:37:51 -0400
commitc1aabdf379bc2feeb0df7057ed5bad96f492133e (patch)
tree36adaf1c2e8b18b63f741bb2b3f7ef2da56ab77f
parent13225977f5429fc5a8c0c1933e3283ab4c7042d8 (diff)
can-gw: add netlink based CAN routing
This patch adds a CAN Gateway/Router to route (and modify) CAN frames. It is based on the PF_CAN core infrastructure for msg filtering and msg sending and can optionally modify routed CAN frames on the fly. CAN frames can *only* be routed between CAN network interfaces (one hop). They can be modified with AND/OR/XOR/SET operations as configured by the netlink configuration interface known e.g. from iptables. From the netlink view this can-gw implements RTM_{NEW|DEL|GET}ROUTE for PF_CAN. The CAN specific userspace tool to manage CAN routing entries can be found in the CAN utils http://svn.berlios.de/wsvn/socketcan/trunk/can-utils/cangw.c at the SocketCAN SVN on BerliOS. Signed-off-by: Oliver Hartkopp <socketcan@hartkopp.net> Signed-off-by: David S. Miller <davem@davemloft.net>
Diffstat
-rw-r--r--include/linux/can/Kbuild1
-rw-r--r--include/linux/can/gw.h164
-rw-r--r--net/can/Kconfig11
-rw-r--r--net/can/Makefile3
-rw-r--r--net/can/gw.c959
5 files changed, 1138 insertions, 0 deletions
diff --git a/include/linux/can/Kbuild b/include/linux/can/Kbuild
index 8cb05aae661c..c62b7f1728f9 100644
--- a/include/linux/can/Kbuild
+++ b/include/linux/can/Kbuild
@@ -1,4 +1,5 @@
1header-y += raw.h 1header-y += raw.h
2header-y += bcm.h 2header-y += bcm.h
3header-y += gw.h
3header-y += error.h 4header-y += error.h
4header-y += netlink.h 5header-y += netlink.h
diff --git a/include/linux/can/gw.h b/include/linux/can/gw.h
new file mode 100644
index 000000000000..5527b54a7cc4
--- /dev/null
+++ b/include/linux/can/gw.h
@@ -0,0 +1,164 @@
1/*
2 * linux/can/gw.h
3 *
4 * Definitions for CAN frame Gateway/Router/Bridge
5 *
6 * Author: Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
7 * Copyright (c) 2011 Volkswagen Group Electronic Research
8 * All rights reserved.
9 *
10 * Send feedback to <socketcan-users@lists.berlios.de>
11 *
12 */
13
14#ifndef CAN_GW_H
15#define CAN_GW_H
16
17#include <linux/types.h>
18#include <linux/can.h>
19
20struct rtcanmsg {
21 __u8 can_family;
22 __u8 gwtype;
23 __u16 flags;
24};
25
26/* CAN gateway types */
27enum {
28 CGW_TYPE_UNSPEC,
29 CGW_TYPE_CAN_CAN, /* CAN->CAN routing */
30 __CGW_TYPE_MAX
31};
32
33#define CGW_TYPE_MAX (__CGW_TYPE_MAX - 1)
34
35/* CAN rtnetlink attribute definitions */
36enum {
37 CGW_UNSPEC,
38 CGW_MOD_AND, /* CAN frame modification binary AND */
39 CGW_MOD_OR, /* CAN frame modification binary OR */
40 CGW_MOD_XOR, /* CAN frame modification binary XOR */
41 CGW_MOD_SET, /* CAN frame modification set alternate values */
42 CGW_CS_XOR, /* set data[] XOR checksum into data[index] */
43 CGW_CS_CRC8, /* set data[] CRC8 checksum into data[index] */
44 CGW_HANDLED, /* number of handled CAN frames */
45 CGW_DROPPED, /* number of dropped CAN frames */
46 CGW_SRC_IF, /* ifindex of source network interface */
47 CGW_DST_IF, /* ifindex of destination network interface */
48 CGW_FILTER, /* specify struct can_filter on source CAN device */
49 __CGW_MAX
50};
51
52#define CGW_MAX (__CGW_MAX - 1)
53
54#define CGW_FLAGS_CAN_ECHO 0x01
55#define CGW_FLAGS_CAN_SRC_TSTAMP 0x02
56
57#define CGW_MOD_FUNCS 4 /* AND OR XOR SET */
58
59/* CAN frame elements that are affected by curr. 3 CAN frame modifications */
60#define CGW_MOD_ID 0x01
61#define CGW_MOD_DLC 0x02
62#define CGW_MOD_DATA 0x04
63
64#define CGW_FRAME_MODS 3 /* ID DLC DATA */
65
66#define MAX_MODFUNCTIONS (CGW_MOD_FUNCS * CGW_FRAME_MODS)
67
68struct cgw_frame_mod {
69 struct can_frame cf;
70 __u8 modtype;
71} __attribute__((packed));
72
73#define CGW_MODATTR_LEN sizeof(struct cgw_frame_mod)
74
75struct cgw_csum_xor {
76 __s8 from_idx;
77 __s8 to_idx;
78 __s8 result_idx;
79 __u8 init_xor_val;
80} __attribute__((packed));
81
82struct cgw_csum_crc8 {
83 __s8 from_idx;
84 __s8 to_idx;
85 __s8 result_idx;
86 __u8 init_crc_val;
87 __u8 final_xor_val;
88 __u8 crctab[256];
89 __u8 profile;
90 __u8 profile_data[20];
91} __attribute__((packed));
92
93/* length of checksum operation parameters. idx = index in CAN frame data[] */
94#define CGW_CS_XOR_LEN sizeof(struct cgw_csum_xor)
95#define CGW_CS_CRC8_LEN sizeof(struct cgw_csum_crc8)
96
97/* CRC8 profiles (compute CRC for additional data elements - see below) */
98enum {
99 CGW_CRC8PRF_UNSPEC,
100 CGW_CRC8PRF_1U8, /* compute one additional u8 value */
101 CGW_CRC8PRF_16U8, /* u8 value table indexed by data[1] & 0xF */
102 CGW_CRC8PRF_SFFID_XOR, /* (can_id & 0xFF) ^ (can_id >> 8 & 0xFF) */
103 __CGW_CRC8PRF_MAX
104};
105
106#define CGW_CRC8PRF_MAX (__CGW_CRC8PRF_MAX - 1)
107
108/*
109 * CAN rtnetlink attribute contents in detail
110 *
111 * CGW_XXX_IF (length 4 bytes):
112 * Sets an interface index for source/destination network interfaces.
113 * For the CAN->CAN gwtype the indices of _two_ CAN interfaces are mandatory.
114 *
115 * CGW_FILTER (length 8 bytes):
116 * Sets a CAN receive filter for the gateway job specified by the
117 * struct can_filter described in include/linux/can.h
118 *
119 * CGW_MOD_XXX (length 17 bytes):
120 * Specifies a modification that's done to a received CAN frame before it is
121 * send out to the destination interface.
122 *
123 * <struct can_frame> data used as operator
124 * <u8> affected CAN frame elements
125 *
126 * CGW_CS_XOR (length 4 bytes):
127 * Set a simple XOR checksum starting with an initial value into
128 * data[result-idx] using data[start-idx] .. data[end-idx]
129 *
130 * The XOR checksum is calculated like this:
131 *
132 * xor = init_xor_val
133 *
134 * for (i = from_idx .. to_idx)
135 * xor ^= can_frame.data[i]
136 *
137 * can_frame.data[ result_idx ] = xor
138 *
139 * CGW_CS_CRC8 (length 282 bytes):
140 * Set a CRC8 value into data[result-idx] using a given 256 byte CRC8 table,
141 * a given initial value and a defined input data[start-idx] .. data[end-idx].
142 * Finally the result value is XOR'ed with the final_xor_val.
143 *
144 * The CRC8 checksum is calculated like this:
145 *
146 * crc = init_crc_val
147 *
148 * for (i = from_idx .. to_idx)
149 * crc = crctab[ crc ^ can_frame.data[i] ]
150 *
151 * can_frame.data[ result_idx ] = crc ^ final_xor_val
152 *
153 * The calculated CRC may contain additional source data elements that can be
154 * defined in the handling of 'checksum profiles' e.g. shown in AUTOSAR specs
155 * like http://www.autosar.org/download/R4.0/AUTOSAR_SWS_E2ELibrary.pdf
156 * E.g. the profile_data[] may contain additional u8 values (called DATA_IDs)
157 * that are used depending on counter values inside the CAN frame data[].
158 * So far only three profiles have been implemented for illustration.
159 *
160 * Remark: In general the attribute data is a linear buffer.
161 * Beware of sending unpacked or aligned structs!
162 */
163
164#endif
diff --git a/net/can/Kconfig b/net/can/Kconfig
index 89395b2c8bca..03200699d274 100644
--- a/net/can/Kconfig
+++ b/net/can/Kconfig
@@ -40,5 +40,16 @@ config CAN_BCM
40 CAN messages are used on the bus (e.g. in automotive environments). 40 CAN messages are used on the bus (e.g. in automotive environments).
41 To use the Broadcast Manager, use AF_CAN with protocol CAN_BCM. 41 To use the Broadcast Manager, use AF_CAN with protocol CAN_BCM.
42 42
43config CAN_GW
44 tristate "CAN Gateway/Router (with netlink configuration)"
45 depends on CAN
46 default N
47 ---help---
48 The CAN Gateway/Router is used to route (and modify) CAN frames.
49 It is based on the PF_CAN core infrastructure for msg filtering and
50 msg sending and can optionally modify routed CAN frames on the fly.
51 CAN frames can be routed between CAN network interfaces (one hop).
52 They can be modified with AND/OR/XOR/SET operations as configured
53 by the netlink configuration interface known e.g. from iptables.
43 54
44source "drivers/net/can/Kconfig" 55source "drivers/net/can/Kconfig"
diff --git a/net/can/Makefile b/net/can/Makefile
index 2d3894b32742..cef49eb1f5c7 100644
--- a/net/can/Makefile
+++ b/net/can/Makefile
@@ -10,3 +10,6 @@ can-raw-y := raw.o
10 10
11obj-$(CONFIG_CAN_BCM) += can-bcm.o 11obj-$(CONFIG_CAN_BCM) += can-bcm.o
12can-bcm-y := bcm.o 12can-bcm-y := bcm.o
13
14obj-$(CONFIG_CAN_GW) += can-gw.o
15can-gw-y := gw.o
diff --git a/net/can/gw.c b/net/can/gw.c
new file mode 100644
index 000000000000..ac11407d3b54
--- /dev/null
+++ b/net/can/gw.c
@@ -0,0 +1,959 @@
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 * Send feedback to <socketcan-users@lists.berlios.de>
41 *
42 */
43
44#include <linux/module.h>
45#include <linux/init.h>
46#include <linux/types.h>
47#include <linux/list.h>
48#include <linux/spinlock.h>
49#include <linux/rcupdate.h>
50#include <linux/rculist.h>
51#include <linux/net.h>
52#include <linux/netdevice.h>
53#include <linux/if_arp.h>
54#include <linux/skbuff.h>
55#include <linux/can.h>
56#include <linux/can/core.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 "20101209"
63static __initdata const char banner[] =
64 KERN_INFO "can: netlink gateway (rev " CAN_GW_VERSION ")\n";
65
66MODULE_DESCRIPTION("PF_CAN netlink gateway");
67MODULE_LICENSE("Dual BSD/GPL");
68MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
69MODULE_ALIAS("can-gw");
70
71HLIST_HEAD(cgw_list);
72static struct notifier_block notifier;
73
74static struct kmem_cache *cgw_cache __read_mostly;
75
76/* structure that contains the (on-the-fly) CAN frame modifications */
77struct cf_mod {
78 struct {
79 struct can_frame and;
80 struct can_frame or;
81 struct can_frame xor;
82 struct can_frame set;
83 } modframe;
84 struct {
85 u8 and;
86 u8 or;
87 u8 xor;
88 u8 set;
89 } modtype;
90 void (*modfunc[MAX_MODFUNCTIONS])(struct can_frame *cf,
91 struct cf_mod *mod);
92
93 /* CAN frame checksum calculation after CAN frame modifications */
94 struct {
95 struct cgw_csum_xor xor;
96 struct cgw_csum_crc8 crc8;
97 } csum;
98 struct {
99 void (*xor)(struct can_frame *cf, struct cgw_csum_xor *xor);
100 void (*crc8)(struct can_frame *cf, struct cgw_csum_crc8 *crc8);
101 } csumfunc;
102};
103
104
105/*
106 * So far we just support CAN -> CAN routing and frame modifications.
107 *
108 * The internal can_can_gw structure contains data and attributes for
109 * a CAN -> CAN gateway job.
110 */
111struct can_can_gw {
112 struct can_filter filter;
113 int src_idx;
114 int dst_idx;
115};
116
117/* list entry for CAN gateways jobs */
118struct cgw_job {
119 struct hlist_node list;
120 struct rcu_head rcu;
121 u32 handled_frames;
122 u32 dropped_frames;
123 struct cf_mod mod;
124 union {
125 /* CAN frame data source */
126 struct net_device *dev;
127 } src;
128 union {
129 /* CAN frame data destination */
130 struct net_device *dev;
131 } dst;
132 union {
133 struct can_can_gw ccgw;
134 /* tbc */
135 };
136 u8 gwtype;
137 u16 flags;
138};
139
140/* modification functions that are invoked in the hot path in can_can_gw_rcv */
141
142#define MODFUNC(func, op) static void func(struct can_frame *cf, \
143 struct cf_mod *mod) { op ; }
144
145MODFUNC(mod_and_id, cf->can_id &= mod->modframe.and.can_id)
146MODFUNC(mod_and_dlc, cf->can_dlc &= mod->modframe.and.can_dlc)
147MODFUNC(mod_and_data, *(u64 *)cf->data &= *(u64 *)mod->modframe.and.data)
148MODFUNC(mod_or_id, cf->can_id |= mod->modframe.or.can_id)
149MODFUNC(mod_or_dlc, cf->can_dlc |= mod->modframe.or.can_dlc)
150MODFUNC(mod_or_data, *(u64 *)cf->data |= *(u64 *)mod->modframe.or.data)
151MODFUNC(mod_xor_id, cf->can_id ^= mod->modframe.xor.can_id)
152MODFUNC(mod_xor_dlc, cf->can_dlc ^= mod->modframe.xor.can_dlc)
153MODFUNC(mod_xor_data, *(u64 *)cf->data ^= *(u64 *)mod->modframe.xor.data)
154MODFUNC(mod_set_id, cf->can_id = mod->modframe.set.can_id)
155MODFUNC(mod_set_dlc, cf->can_dlc = mod->modframe.set.can_dlc)
156MODFUNC(mod_set_data, *(u64 *)cf->data = *(u64 *)mod->modframe.set.data)
157
158static inline void canframecpy(struct can_frame *dst, struct can_frame *src)
159{
160 /*
161 * Copy the struct members separately to ensure that no uninitialized
162 * data are copied in the 3 bytes hole of the struct. This is needed
163 * to make easy compares of the data in the struct cf_mod.
164 */
165
166 dst->can_id = src->can_id;
167 dst->can_dlc = src->can_dlc;
168 *(u64 *)dst->data = *(u64 *)src->data;
169}
170
171static int cgw_chk_csum_parms(s8 fr, s8 to, s8 re)
172{
173 /*
174 * absolute dlc values 0 .. 7 => 0 .. 7, e.g. data [0]
175 * relative to received dlc -1 .. -8 :
176 * e.g. for received dlc = 8
177 * -1 => index = 7 (data[7])
178 * -3 => index = 5 (data[5])
179 * -8 => index = 0 (data[0])
180 */
181
182 if (fr > -9 && fr < 8 &&
183 to > -9 && to < 8 &&
184 re > -9 && re < 8)
185 return 0;
186 else
187 return -EINVAL;
188}
189
190static inline int calc_idx(int idx, int rx_dlc)
191{
192 if (idx < 0)
193 return rx_dlc + idx;
194 else
195 return idx;
196}
197
198static void cgw_csum_xor_rel(struct can_frame *cf, struct cgw_csum_xor *xor)
199{
200 int from = calc_idx(xor->from_idx, cf->can_dlc);
201 int to = calc_idx(xor->to_idx, cf->can_dlc);
202 int res = calc_idx(xor->result_idx, cf->can_dlc);
203 u8 val = xor->init_xor_val;
204 int i;
205
206 if (from < 0 || to < 0 || res < 0)
207 return;
208
209 if (from <= to) {
210 for (i = from; i <= to; i++)
211 val ^= cf->data[i];
212 } else {
213 for (i = from; i >= to; i--)
214 val ^= cf->data[i];
215 }
216
217 cf->data[res] = val;
218}
219
220static void cgw_csum_xor_pos(struct can_frame *cf, struct cgw_csum_xor *xor)
221{
222 u8 val = xor->init_xor_val;
223 int i;
224
225 for (i = xor->from_idx; i <= xor->to_idx; i++)
226 val ^= cf->data[i];
227
228 cf->data[xor->result_idx] = val;
229}
230
231static void cgw_csum_xor_neg(struct can_frame *cf, struct cgw_csum_xor *xor)
232{
233 u8 val = xor->init_xor_val;
234 int i;
235
236 for (i = xor->from_idx; i >= xor->to_idx; i--)
237 val ^= cf->data[i];
238
239 cf->data[xor->result_idx] = val;
240}
241
242static void cgw_csum_crc8_rel(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
243{
244 int from = calc_idx(crc8->from_idx, cf->can_dlc);
245 int to = calc_idx(crc8->to_idx, cf->can_dlc);
246 int res = calc_idx(crc8->result_idx, cf->can_dlc);
247 u8 crc = crc8->init_crc_val;
248 int i;
249
250 if (from < 0 || to < 0 || res < 0)
251 return;
252
253 if (from <= to) {
254 for (i = crc8->from_idx; i <= crc8->to_idx; i++)
255 crc = crc8->crctab[crc^cf->data[i]];
256 } else {
257 for (i = crc8->from_idx; i >= crc8->to_idx; i--)
258 crc = crc8->crctab[crc^cf->data[i]];
259 }
260
261 switch (crc8->profile) {
262
263 case CGW_CRC8PRF_1U8:
264 crc = crc8->crctab[crc^crc8->profile_data[0]];
265 break;
266
267 case CGW_CRC8PRF_16U8:
268 crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
269 break;
270
271 case CGW_CRC8PRF_SFFID_XOR:
272 crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
273 (cf->can_id >> 8 & 0xFF)];
274 break;
275
276 }
277
278 cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
279}
280
281static void cgw_csum_crc8_pos(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
282{
283 u8 crc = crc8->init_crc_val;
284 int i;
285
286 for (i = crc8->from_idx; i <= crc8->to_idx; i++)
287 crc = crc8->crctab[crc^cf->data[i]];
288
289 switch (crc8->profile) {
290
291 case CGW_CRC8PRF_1U8:
292 crc = crc8->crctab[crc^crc8->profile_data[0]];
293 break;
294
295 case CGW_CRC8PRF_16U8:
296 crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
297 break;
298
299 case CGW_CRC8PRF_SFFID_XOR:
300 crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
301 (cf->can_id >> 8 & 0xFF)];
302 break;
303 }
304
305 cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
306}
307
308static void cgw_csum_crc8_neg(struct can_frame *cf, struct cgw_csum_crc8 *crc8)
309{
310 u8 crc = crc8->init_crc_val;
311 int i;
312
313 for (i = crc8->from_idx; i >= crc8->to_idx; i--)
314 crc = crc8->crctab[crc^cf->data[i]];
315
316 switch (crc8->profile) {
317
318 case CGW_CRC8PRF_1U8:
319 crc = crc8->crctab[crc^crc8->profile_data[0]];
320 break;
321
322 case CGW_CRC8PRF_16U8:
323 crc = crc8->crctab[crc^crc8->profile_data[cf->data[1] & 0xF]];
324 break;
325
326 case CGW_CRC8PRF_SFFID_XOR:
327 crc = crc8->crctab[crc^(cf->can_id & 0xFF)^
328 (cf->can_id >> 8 & 0xFF)];
329 break;
330 }
331
332 cf->data[crc8->result_idx] = crc^crc8->final_xor_val;
333}
334
335/* the receive & process & send function */
336static void can_can_gw_rcv(struct sk_buff *skb, void *data)
337{
338 struct cgw_job *gwj = (struct cgw_job *)data;
339 struct can_frame *cf;
340 struct sk_buff *nskb;
341 int modidx = 0;
342
343 /* do not handle already routed frames - see comment below */
344 if (skb_mac_header_was_set(skb))
345 return;
346
347 if (!(gwj->dst.dev->flags & IFF_UP)) {
348 gwj->dropped_frames++;
349 return;
350 }
351
352 /*
353 * clone the given skb, which has not been done in can_rcv()
354 *
355 * When there is at least one modification function activated,
356 * we need to copy the skb as we want to modify skb->data.
357 */
358 if (gwj->mod.modfunc[0])
359 nskb = skb_copy(skb, GFP_ATOMIC);
360 else
361 nskb = skb_clone(skb, GFP_ATOMIC);
362
363 if (!nskb) {
364 gwj->dropped_frames++;
365 return;
366 }
367
368 /*
369 * Mark routed frames by setting some mac header length which is
370 * not relevant for the CAN frames located in the skb->data section.
371 *
372 * As dev->header_ops is not set in CAN netdevices no one is ever
373 * accessing the various header offsets in the CAN skbuffs anyway.
374 * E.g. using the packet socket to read CAN frames is still working.
375 */
376 skb_set_mac_header(nskb, 8);
377 nskb->dev = gwj->dst.dev;
378
379 /* pointer to modifiable CAN frame */
380 cf = (struct can_frame *)nskb->data;
381
382 /* perform preprocessed modification functions if there are any */
383 while (modidx < MAX_MODFUNCTIONS && gwj->mod.modfunc[modidx])
384 (*gwj->mod.modfunc[modidx++])(cf, &gwj->mod);
385
386 /* check for checksum updates when the CAN frame has been modified */
387 if (modidx) {
388 if (gwj->mod.csumfunc.crc8)
389 (*gwj->mod.csumfunc.crc8)(cf, &gwj->mod.csum.crc8);
390
391 if (gwj->mod.csumfunc.xor)
392 (*gwj->mod.csumfunc.xor)(cf, &gwj->mod.csum.xor);
393 }
394
395 /* clear the skb timestamp if not configured the other way */
396 if (!(gwj->flags & CGW_FLAGS_CAN_SRC_TSTAMP))
397 nskb->tstamp.tv64 = 0;
398
399 /* send to netdevice */
400 if (can_send(nskb, gwj->flags & CGW_FLAGS_CAN_ECHO))
401 gwj->dropped_frames++;
402 else
403 gwj->handled_frames++;
404}
405
406static inline int cgw_register_filter(struct cgw_job *gwj)
407{
408 return can_rx_register(gwj->src.dev, gwj->ccgw.filter.can_id,
409 gwj->ccgw.filter.can_mask, can_can_gw_rcv,
410 gwj, "gw");
411}
412
413static inline void cgw_unregister_filter(struct cgw_job *gwj)
414{
415 can_rx_unregister(gwj->src.dev, gwj->ccgw.filter.can_id,
416 gwj->ccgw.filter.can_mask, can_can_gw_rcv, gwj);
417}
418
419static int cgw_notifier(struct notifier_block *nb,
420 unsigned long msg, void *data)
421{
422 struct net_device *dev = (struct net_device *)data;
423
424 if (!net_eq(dev_net(dev), &init_net))
425 return NOTIFY_DONE;
426 if (dev->type != ARPHRD_CAN)
427 return NOTIFY_DONE;
428
429 if (msg == NETDEV_UNREGISTER) {
430
431 struct cgw_job *gwj = NULL;
432 struct hlist_node *n, *nx;
433
434 ASSERT_RTNL();
435
436 hlist_for_each_entry_safe(gwj, n, nx, &cgw_list, list) {
437
438 if (gwj->src.dev == dev || gwj->dst.dev == dev) {
439 hlist_del(&gwj->list);
440 cgw_unregister_filter(gwj);
441 kfree(gwj);
442 }
443 }
444 }
445
446 return NOTIFY_DONE;
447}
448
449static int cgw_put_job(struct sk_buff *skb, struct cgw_job *gwj)
450{
451 struct cgw_frame_mod mb;
452 struct rtcanmsg *rtcan;
453 struct nlmsghdr *nlh = nlmsg_put(skb, 0, 0, 0, sizeof(*rtcan), 0);
454 if (!nlh)
455 return -EMSGSIZE;
456
457 rtcan = nlmsg_data(nlh);
458 rtcan->can_family = AF_CAN;
459 rtcan->gwtype = gwj->gwtype;
460 rtcan->flags = gwj->flags;
461
462 /* add statistics if available */
463
464 if (gwj->handled_frames) {
465 if (nla_put_u32(skb, CGW_HANDLED, gwj->handled_frames) < 0)
466 goto cancel;
467 else
468 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
469 }
470
471 if (gwj->dropped_frames) {
472 if (nla_put_u32(skb, CGW_DROPPED, gwj->dropped_frames) < 0)
473 goto cancel;
474 else
475 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
476 }
477
478 /* check non default settings of attributes */
479
480 if (gwj->mod.modtype.and) {
481 memcpy(&mb.cf, &gwj->mod.modframe.and, sizeof(mb.cf));
482 mb.modtype = gwj->mod.modtype.and;
483 if (nla_put(skb, CGW_MOD_AND, sizeof(mb), &mb) < 0)
484 goto cancel;
485 else
486 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
487 }
488
489 if (gwj->mod.modtype.or) {
490 memcpy(&mb.cf, &gwj->mod.modframe.or, sizeof(mb.cf));
491 mb.modtype = gwj->mod.modtype.or;
492 if (nla_put(skb, CGW_MOD_OR, sizeof(mb), &mb) < 0)
493 goto cancel;
494 else
495 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
496 }
497
498 if (gwj->mod.modtype.xor) {
499 memcpy(&mb.cf, &gwj->mod.modframe.xor, sizeof(mb.cf));
500 mb.modtype = gwj->mod.modtype.xor;
501 if (nla_put(skb, CGW_MOD_XOR, sizeof(mb), &mb) < 0)
502 goto cancel;
503 else
504 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
505 }
506
507 if (gwj->mod.modtype.set) {
508 memcpy(&mb.cf, &gwj->mod.modframe.set, sizeof(mb.cf));
509 mb.modtype = gwj->mod.modtype.set;
510 if (nla_put(skb, CGW_MOD_SET, sizeof(mb), &mb) < 0)
511 goto cancel;
512 else
513 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
514 }
515
516 if (gwj->mod.csumfunc.crc8) {
517 if (nla_put(skb, CGW_CS_CRC8, CGW_CS_CRC8_LEN,
518 &gwj->mod.csum.crc8) < 0)
519 goto cancel;
520 else
521 nlh->nlmsg_len += NLA_HDRLEN + \
522 NLA_ALIGN(CGW_CS_CRC8_LEN);
523 }
524
525 if (gwj->mod.csumfunc.xor) {
526 if (nla_put(skb, CGW_CS_XOR, CGW_CS_XOR_LEN,
527 &gwj->mod.csum.xor) < 0)
528 goto cancel;
529 else
530 nlh->nlmsg_len += NLA_HDRLEN + \
531 NLA_ALIGN(CGW_CS_XOR_LEN);
532 }
533
534 if (gwj->gwtype == CGW_TYPE_CAN_CAN) {
535
536 if (gwj->ccgw.filter.can_id || gwj->ccgw.filter.can_mask) {
537 if (nla_put(skb, CGW_FILTER, sizeof(struct can_filter),
538 &gwj->ccgw.filter) < 0)
539 goto cancel;
540 else
541 nlh->nlmsg_len += NLA_HDRLEN +
542 NLA_ALIGN(sizeof(struct can_filter));
543 }
544
545 if (nla_put_u32(skb, CGW_SRC_IF, gwj->ccgw.src_idx) < 0)
546 goto cancel;
547 else
548 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
549
550 if (nla_put_u32(skb, CGW_DST_IF, gwj->ccgw.dst_idx) < 0)
551 goto cancel;
552 else
553 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
554 }
555
556 return skb->len;
557
558cancel:
559 nlmsg_cancel(skb, nlh);
560 return -EMSGSIZE;
561}
562
563/* Dump information about all CAN gateway jobs, in response to RTM_GETROUTE */
564static int cgw_dump_jobs(struct sk_buff *skb, struct netlink_callback *cb)
565{
566 struct cgw_job *gwj = NULL;
567 struct hlist_node *n;
568 int idx = 0;
569 int s_idx = cb->args[0];
570
571 rcu_read_lock();
572 hlist_for_each_entry_rcu(gwj, n, &cgw_list, list) {
573 if (idx < s_idx)
574 goto cont;
575
576 if (cgw_put_job(skb, gwj) < 0)
577 break;
578cont:
579 idx++;
580 }
581 rcu_read_unlock();
582
583 cb->args[0] = idx;
584
585 return skb->len;
586}
587
588/* check for common and gwtype specific attributes */
589static int cgw_parse_attr(struct nlmsghdr *nlh, struct cf_mod *mod,
590 u8 gwtype, void *gwtypeattr)
591{
592 struct nlattr *tb[CGW_MAX+1];
593 struct cgw_frame_mod mb;
594 int modidx = 0;
595 int err = 0;
596
597 /* initialize modification & checksum data space */
598 memset(mod, 0, sizeof(*mod));
599
600 err = nlmsg_parse(nlh, sizeof(struct rtcanmsg), tb, CGW_MAX, NULL);
601 if (err < 0)
602 return err;
603
604 /* check for AND/OR/XOR/SET modifications */
605
606 if (tb[CGW_MOD_AND] &&
607 nla_len(tb[CGW_MOD_AND]) == CGW_MODATTR_LEN) {
608 nla_memcpy(&mb, tb[CGW_MOD_AND], CGW_MODATTR_LEN);
609
610 canframecpy(&mod->modframe.and, &mb.cf);
611 mod->modtype.and = mb.modtype;
612
613 if (mb.modtype & CGW_MOD_ID)
614 mod->modfunc[modidx++] = mod_and_id;
615
616 if (mb.modtype & CGW_MOD_DLC)
617 mod->modfunc[modidx++] = mod_and_dlc;
618
619 if (mb.modtype & CGW_MOD_DATA)
620 mod->modfunc[modidx++] = mod_and_data;
621 }
622
623 if (tb[CGW_MOD_OR] &&
624 nla_len(tb[CGW_MOD_OR]) == CGW_MODATTR_LEN) {
625 nla_memcpy(&mb, tb[CGW_MOD_OR], CGW_MODATTR_LEN);
626
627 canframecpy(&mod->modframe.or, &mb.cf);
628 mod->modtype.or = mb.modtype;
629
630 if (mb.modtype & CGW_MOD_ID)
631 mod->modfunc[modidx++] = mod_or_id;
632
633 if (mb.modtype & CGW_MOD_DLC)
634 mod->modfunc[modidx++] = mod_or_dlc;
635
636 if (mb.modtype & CGW_MOD_DATA)
637 mod->modfunc[modidx++] = mod_or_data;
638 }
639
640 if (tb[CGW_MOD_XOR] &&
641 nla_len(tb[CGW_MOD_XOR]) == CGW_MODATTR_LEN) {
642 nla_memcpy(&mb, tb[CGW_MOD_XOR], CGW_MODATTR_LEN);
643
644 canframecpy(&mod->modframe.xor, &mb.cf);
645 mod->modtype.xor = mb.modtype;
646
647 if (mb.modtype & CGW_MOD_ID)
648 mod->modfunc[modidx++] = mod_xor_id;
649
650 if (mb.modtype & CGW_MOD_DLC)
651 mod->modfunc[modidx++] = mod_xor_dlc;
652
653 if (mb.modtype & CGW_MOD_DATA)
654 mod->modfunc[modidx++] = mod_xor_data;
655 }
656
657 if (tb[CGW_MOD_SET] &&
658 nla_len(tb[CGW_MOD_SET]) == CGW_MODATTR_LEN) {
659 nla_memcpy(&mb, tb[CGW_MOD_SET], CGW_MODATTR_LEN);
660
661 canframecpy(&mod->modframe.set, &mb.cf);
662 mod->modtype.set = mb.modtype;
663
664 if (mb.modtype & CGW_MOD_ID)
665 mod->modfunc[modidx++] = mod_set_id;
666
667 if (mb.modtype & CGW_MOD_DLC)
668 mod->modfunc[modidx++] = mod_set_dlc;
669
670 if (mb.modtype & CGW_MOD_DATA)
671 mod->modfunc[modidx++] = mod_set_data;
672 }
673
674 /* check for checksum operations after CAN frame modifications */
675 if (modidx) {
676
677 if (tb[CGW_CS_CRC8] &&
678 nla_len(tb[CGW_CS_CRC8]) == CGW_CS_CRC8_LEN) {
679
680 struct cgw_csum_crc8 *c = (struct cgw_csum_crc8 *)\
681 nla_data(tb[CGW_CS_CRC8]);
682
683 err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
684 c->result_idx);
685 if (err)
686 return err;
687
688 nla_memcpy(&mod->csum.crc8, tb[CGW_CS_CRC8],
689 CGW_CS_CRC8_LEN);
690
691 /*
692 * select dedicated processing function to reduce
693 * runtime operations in receive hot path.
694 */
695 if (c->from_idx < 0 || c->to_idx < 0 ||
696 c->result_idx < 0)
697 mod->csumfunc.crc8 = cgw_csum_crc8_rel;
698 else if (c->from_idx <= c->to_idx)
699 mod->csumfunc.crc8 = cgw_csum_crc8_pos;
700 else
701 mod->csumfunc.crc8 = cgw_csum_crc8_neg;
702 }
703
704 if (tb[CGW_CS_XOR] &&
705 nla_len(tb[CGW_CS_XOR]) == CGW_CS_XOR_LEN) {
706
707 struct cgw_csum_xor *c = (struct cgw_csum_xor *)\
708 nla_data(tb[CGW_CS_XOR]);
709
710 err = cgw_chk_csum_parms(c->from_idx, c->to_idx,
711 c->result_idx);
712 if (err)
713 return err;
714
715 nla_memcpy(&mod->csum.xor, tb[CGW_CS_XOR],
716 CGW_CS_XOR_LEN);
717
718 /*
719 * select dedicated processing function to reduce
720 * runtime operations in receive hot path.
721 */
722 if (c->from_idx < 0 || c->to_idx < 0 ||
723 c->result_idx < 0)
724 mod->csumfunc.xor = cgw_csum_xor_rel;
725 else if (c->from_idx <= c->to_idx)
726 mod->csumfunc.xor = cgw_csum_xor_pos;
727 else
728 mod->csumfunc.xor = cgw_csum_xor_neg;
729 }
730 }
731
732 if (gwtype == CGW_TYPE_CAN_CAN) {
733
734 /* check CGW_TYPE_CAN_CAN specific attributes */
735
736 struct can_can_gw *ccgw = (struct can_can_gw *)gwtypeattr;
737 memset(ccgw, 0, sizeof(*ccgw));
738
739 /* check for can_filter in attributes */
740 if (tb[CGW_FILTER] &&
741 nla_len(tb[CGW_FILTER]) == sizeof(struct can_filter))
742 nla_memcpy(&ccgw->filter, tb[CGW_FILTER],
743 sizeof(struct can_filter));
744
745 err = -ENODEV;
746
747 /* specifying two interfaces is mandatory */
748 if (!tb[CGW_SRC_IF] || !tb[CGW_DST_IF])
749 return err;
750
751 if (nla_len(tb[CGW_SRC_IF]) == sizeof(u32))
752 nla_memcpy(&ccgw->src_idx, tb[CGW_SRC_IF],
753 sizeof(u32));
754
755 if (nla_len(tb[CGW_DST_IF]) == sizeof(u32))
756 nla_memcpy(&ccgw->dst_idx, tb[CGW_DST_IF],
757 sizeof(u32));
758
759 /* both indices set to 0 for flushing all routing entries */
760 if (!ccgw->src_idx && !ccgw->dst_idx)
761 return 0;
762
763 /* only one index set to 0 is an error */
764 if (!ccgw->src_idx || !ccgw->dst_idx)
765 return err;
766 }
767
768 /* add the checks for other gwtypes here */
769
770 return 0;
771}
772
773static int cgw_create_job(struct sk_buff *skb, struct nlmsghdr *nlh,
774 void *arg)
775{
776 struct rtcanmsg *r;
777 struct cgw_job *gwj;
778 int err = 0;
779
780 if (nlmsg_len(nlh) < sizeof(*r))
781 return -EINVAL;
782
783 r = nlmsg_data(nlh);
784 if (r->can_family != AF_CAN)
785 return -EPFNOSUPPORT;
786
787 /* so far we only support CAN -> CAN routings */
788 if (r->gwtype != CGW_TYPE_CAN_CAN)
789 return -EINVAL;
790
791 gwj = kmem_cache_alloc(cgw_cache, GFP_KERNEL);
792 if (!gwj)
793 return -ENOMEM;
794
795 gwj->handled_frames = 0;
796 gwj->dropped_frames = 0;
797 gwj->flags = r->flags;
798 gwj->gwtype = r->gwtype;
799
800 err = cgw_parse_attr(nlh, &gwj->mod, CGW_TYPE_CAN_CAN, &gwj->ccgw);
801 if (err < 0)
802 goto out;
803
804 err = -ENODEV;
805
806 /* ifindex == 0 is not allowed for job creation */
807 if (!gwj->ccgw.src_idx || !gwj->ccgw.dst_idx)
808 goto out;
809
810 gwj->src.dev = dev_get_by_index(&init_net, gwj->ccgw.src_idx);
811
812 if (!gwj->src.dev)
813 goto out;
814
815 /* check for CAN netdev not using header_ops - see gw_rcv() */
816 if (gwj->src.dev->type != ARPHRD_CAN || gwj->src.dev->header_ops)
817 goto put_src_out;
818
819 gwj->dst.dev = dev_get_by_index(&init_net, gwj->ccgw.dst_idx);
820
821 if (!gwj->dst.dev)
822 goto put_src_out;
823
824 /* check for CAN netdev not using header_ops - see gw_rcv() */
825 if (gwj->dst.dev->type != ARPHRD_CAN || gwj->dst.dev->header_ops)
826 goto put_src_dst_out;
827
828 ASSERT_RTNL();
829
830 err = cgw_register_filter(gwj);
831 if (!err)
832 hlist_add_head_rcu(&gwj->list, &cgw_list);
833
834put_src_dst_out:
835 dev_put(gwj->dst.dev);
836put_src_out:
837 dev_put(gwj->src.dev);
838out:
839 if (err)
840 kmem_cache_free(cgw_cache, gwj);
841
842 return err;
843}
844
845static void cgw_remove_all_jobs(void)
846{
847 struct cgw_job *gwj = NULL;
848 struct hlist_node *n, *nx;
849
850 ASSERT_RTNL();
851
852 hlist_for_each_entry_safe(gwj, n, nx, &cgw_list, list) {
853 hlist_del(&gwj->list);
854 cgw_unregister_filter(gwj);
855 kfree(gwj);
856 }
857}
858
859static int cgw_remove_job(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
860{
861 struct cgw_job *gwj = NULL;
862 struct hlist_node *n, *nx;
863 struct rtcanmsg *r;
864 struct cf_mod mod;
865 struct can_can_gw ccgw;
866 int err = 0;
867
868 if (nlmsg_len(nlh) < sizeof(*r))
869 return -EINVAL;
870
871 r = nlmsg_data(nlh);
872 if (r->can_family != AF_CAN)
873 return -EPFNOSUPPORT;
874
875 /* so far we only support CAN -> CAN routings */
876 if (r->gwtype != CGW_TYPE_CAN_CAN)
877 return -EINVAL;
878
879 err = cgw_parse_attr(nlh, &mod, CGW_TYPE_CAN_CAN, &ccgw);
880 if (err < 0)
881 return err;
882
883 /* two interface indices both set to 0 => remove all entries */
884 if (!ccgw.src_idx && !ccgw.dst_idx) {
885 cgw_remove_all_jobs();
886 return 0;
887 }
888
889 err = -EINVAL;
890
891 ASSERT_RTNL();
892
893 /* remove only the first matching entry */
894 hlist_for_each_entry_safe(gwj, n, nx, &cgw_list, list) {
895
896 if (gwj->flags != r->flags)
897 continue;
898
899 if (memcmp(&gwj->mod, &mod, sizeof(mod)))
900 continue;
901
902 /* if (r->gwtype == CGW_TYPE_CAN_CAN) - is made sure here */
903 if (memcmp(&gwj->ccgw, &ccgw, sizeof(ccgw)))
904 continue;
905
906 hlist_del(&gwj->list);
907 cgw_unregister_filter(gwj);
908 kfree(gwj);
909 err = 0;
910 break;
911 }
912
913 return err;
914}
915
916static __init int cgw_module_init(void)
917{
918 printk(banner);
919
920 cgw_cache = kmem_cache_create("can_gw", sizeof(struct cgw_job),
921 0, 0, NULL);
922
923 if (!cgw_cache)
924 return -ENOMEM;
925
926 /* set notifier */
927 notifier.notifier_call = cgw_notifier;
928 register_netdevice_notifier(&notifier);
929
930 if (__rtnl_register(PF_CAN, RTM_GETROUTE, NULL, cgw_dump_jobs, NULL)) {
931 unregister_netdevice_notifier(&notifier);
932 kmem_cache_destroy(cgw_cache);
933 return -ENOBUFS;
934 }
935
936 /* Only the first call to __rtnl_register can fail */
937 __rtnl_register(PF_CAN, RTM_NEWROUTE, cgw_create_job, NULL, NULL);
938 __rtnl_register(PF_CAN, RTM_DELROUTE, cgw_remove_job, NULL, NULL);
939
940 return 0;
941}
942
943static __exit void cgw_module_exit(void)
944{
945 rtnl_unregister_all(PF_CAN);
946
947 unregister_netdevice_notifier(&notifier);
948
949 rtnl_lock();
950 cgw_remove_all_jobs();
951 rtnl_unlock();
952
953 rcu_barrier(); /* Wait for completion of call_rcu()'s */
954
955 kmem_cache_destroy(cgw_cache);
956}
957
958module_init(cgw_module_init);
959module_exit(cgw_module_exit);
generated by cgit v1.2.2 (git 2.25.0) at 2024-11-12 05:14:50 -0500