1 | // SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause) |
2 | /* gw.c - CAN frame Gateway/Router/Bridge with netlink interface |
3 | * |
4 | * Copyright (c) 2019 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_NAME "can-gw" |
63 | |
64 | MODULE_DESCRIPTION("PF_CAN netlink gateway" ); |
65 | MODULE_LICENSE("Dual BSD/GPL" ); |
66 | MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>" ); |
67 | MODULE_ALIAS(CAN_GW_NAME); |
68 | |
69 | #define CGW_MIN_HOPS 1 |
70 | #define CGW_MAX_HOPS 6 |
71 | #define CGW_DEFAULT_HOPS 1 |
72 | |
73 | static unsigned int max_hops __read_mostly = CGW_DEFAULT_HOPS; |
74 | module_param(max_hops, uint, 0444); |
75 | MODULE_PARM_DESC(max_hops, |
76 | "maximum " CAN_GW_NAME " routing hops for CAN frames " |
77 | "(valid values: " __stringify(CGW_MIN_HOPS) "-" |
78 | __stringify(CGW_MAX_HOPS) " hops, " |
79 | "default: " __stringify(CGW_DEFAULT_HOPS) ")" ); |
80 | |
81 | static struct notifier_block notifier; |
82 | static struct kmem_cache *cgw_cache __read_mostly; |
83 | |
84 | /* structure that contains the (on-the-fly) CAN frame modifications */ |
85 | struct cf_mod { |
86 | struct { |
87 | struct canfd_frame and; |
88 | struct canfd_frame or; |
89 | struct canfd_frame xor; |
90 | struct canfd_frame set; |
91 | } modframe; |
92 | struct { |
93 | u8 and; |
94 | u8 or; |
95 | u8 xor; |
96 | u8 set; |
97 | } modtype; |
98 | void (*modfunc[MAX_MODFUNCTIONS])(struct canfd_frame *cf, |
99 | struct cf_mod *mod); |
100 | |
101 | /* CAN frame checksum calculation after CAN frame modifications */ |
102 | struct { |
103 | struct cgw_csum_xor xor; |
104 | struct cgw_csum_crc8 crc8; |
105 | } csum; |
106 | struct { |
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); |
111 | } csumfunc; |
112 | u32 uid; |
113 | }; |
114 | |
115 | /* So far we just support CAN -> CAN routing and frame modifications. |
116 | * |
117 | * The internal can_can_gw structure contains data and attributes for |
118 | * a CAN -> CAN gateway job. |
119 | */ |
120 | struct can_can_gw { |
121 | struct can_filter filter; |
122 | int src_idx; |
123 | int dst_idx; |
124 | }; |
125 | |
126 | /* list entry for CAN gateways jobs */ |
127 | struct cgw_job { |
128 | struct hlist_node list; |
129 | struct rcu_head rcu; |
130 | u32 handled_frames; |
131 | u32 dropped_frames; |
132 | u32 deleted_frames; |
133 | struct cf_mod mod; |
134 | union { |
135 | /* CAN frame data source */ |
136 | struct net_device *dev; |
137 | } src; |
138 | union { |
139 | /* CAN frame data destination */ |
140 | struct net_device *dev; |
141 | } dst; |
142 | union { |
143 | struct can_can_gw ccgw; |
144 | /* tbc */ |
145 | }; |
146 | u8 gwtype; |
147 | u8 limit_hops; |
148 | u16 flags; |
149 | }; |
150 | |
151 | /* modification functions that are invoked in the hot path in can_can_gw_rcv */ |
152 | |
153 | #define MODFUNC(func, op) static void func(struct canfd_frame *cf, \ |
154 | struct cf_mod *mod) { op ; } |
155 | |
156 | MODFUNC(mod_and_id, cf->can_id &= mod->modframe.and.can_id) |
157 | MODFUNC(mod_and_len, cf->len &= mod->modframe.and.len) |
158 | MODFUNC(mod_and_flags, cf->flags &= mod->modframe.and.flags) |
159 | MODFUNC(mod_and_data, *(u64 *)cf->data &= *(u64 *)mod->modframe.and.data) |
160 | MODFUNC(mod_or_id, cf->can_id |= mod->modframe.or.can_id) |
161 | MODFUNC(mod_or_len, cf->len |= mod->modframe.or.len) |
162 | MODFUNC(mod_or_flags, cf->flags |= mod->modframe.or.flags) |
163 | MODFUNC(mod_or_data, *(u64 *)cf->data |= *(u64 *)mod->modframe.or.data) |
164 | MODFUNC(mod_xor_id, cf->can_id ^= mod->modframe.xor.can_id) |
165 | MODFUNC(mod_xor_len, cf->len ^= mod->modframe.xor.len) |
166 | MODFUNC(mod_xor_flags, cf->flags ^= mod->modframe.xor.flags) |
167 | MODFUNC(mod_xor_data, *(u64 *)cf->data ^= *(u64 *)mod->modframe.xor.data) |
168 | MODFUNC(mod_set_id, cf->can_id = mod->modframe.set.can_id) |
169 | MODFUNC(mod_set_len, cf->len = mod->modframe.set.len) |
170 | MODFUNC(mod_set_flags, cf->flags = mod->modframe.set.flags) |
171 | MODFUNC(mod_set_data, *(u64 *)cf->data = *(u64 *)mod->modframe.set.data) |
172 | |
173 | static void mod_and_fddata(struct canfd_frame *cf, struct cf_mod *mod) |
174 | { |
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 | /* retrieve valid CC DLC value and store it into 'len' */ |
203 | static void mod_retrieve_ccdlc(struct canfd_frame *cf) |
204 | { |
205 | struct can_frame *ccf = (struct can_frame *)cf; |
206 | |
207 | /* len8_dlc is only valid if len == CAN_MAX_DLEN */ |
208 | if (ccf->len != CAN_MAX_DLEN) |
209 | return; |
210 | |
211 | /* do we have a valid len8_dlc value from 9 .. 15 ? */ |
212 | if (ccf->len8_dlc > CAN_MAX_DLEN && ccf->len8_dlc <= CAN_MAX_RAW_DLC) |
213 | ccf->len = ccf->len8_dlc; |
214 | } |
215 | |
216 | /* convert valid CC DLC value in 'len' into struct can_frame elements */ |
217 | static void mod_store_ccdlc(struct canfd_frame *cf) |
218 | { |
219 | struct can_frame *ccf = (struct can_frame *)cf; |
220 | |
221 | /* clear potential leftovers */ |
222 | ccf->len8_dlc = 0; |
223 | |
224 | /* plain data length 0 .. 8 - that was easy */ |
225 | if (ccf->len <= CAN_MAX_DLEN) |
226 | return; |
227 | |
228 | /* potentially broken values are caught in can_can_gw_rcv() */ |
229 | if (ccf->len > CAN_MAX_RAW_DLC) |
230 | return; |
231 | |
232 | /* we have a valid dlc value from 9 .. 15 in ccf->len */ |
233 | ccf->len8_dlc = ccf->len; |
234 | ccf->len = CAN_MAX_DLEN; |
235 | } |
236 | |
237 | static void mod_and_ccdlc(struct canfd_frame *cf, struct cf_mod *mod) |
238 | { |
239 | mod_retrieve_ccdlc(cf); |
240 | mod_and_len(cf, mod); |
241 | mod_store_ccdlc(cf); |
242 | } |
243 | |
244 | static void mod_or_ccdlc(struct canfd_frame *cf, struct cf_mod *mod) |
245 | { |
246 | mod_retrieve_ccdlc(cf); |
247 | mod_or_len(cf, mod); |
248 | mod_store_ccdlc(cf); |
249 | } |
250 | |
251 | static void mod_xor_ccdlc(struct canfd_frame *cf, struct cf_mod *mod) |
252 | { |
253 | mod_retrieve_ccdlc(cf); |
254 | mod_xor_len(cf, mod); |
255 | mod_store_ccdlc(cf); |
256 | } |
257 | |
258 | static void mod_set_ccdlc(struct canfd_frame *cf, struct cf_mod *mod) |
259 | { |
260 | mod_set_len(cf, mod); |
261 | mod_store_ccdlc(cf); |
262 | } |
263 | |
264 | static void canframecpy(struct canfd_frame *dst, struct can_frame *src) |
265 | { |
266 | /* Copy the struct members separately to ensure that no uninitialized |
267 | * data are copied in the 3 bytes hole of the struct. This is needed |
268 | * to make easy compares of the data in the struct cf_mod. |
269 | */ |
270 | |
271 | dst->can_id = src->can_id; |
272 | dst->len = src->len; |
273 | *(u64 *)dst->data = *(u64 *)src->data; |
274 | } |
275 | |
276 | static void canfdframecpy(struct canfd_frame *dst, struct canfd_frame *src) |
277 | { |
278 | /* Copy the struct members separately to ensure that no uninitialized |
279 | * data are copied in the 2 bytes hole of the struct. This is needed |
280 | * to make easy compares of the data in the struct cf_mod. |
281 | */ |
282 | |
283 | dst->can_id = src->can_id; |
284 | dst->flags = src->flags; |
285 | dst->len = src->len; |
286 | memcpy(dst->data, src->data, CANFD_MAX_DLEN); |
287 | } |
288 | |
289 | static int cgw_chk_csum_parms(s8 fr, s8 to, s8 re, struct rtcanmsg *r) |
290 | { |
291 | s8 dlen = CAN_MAX_DLEN; |
292 | |
293 | if (r->flags & CGW_FLAGS_CAN_FD) |
294 | dlen = CANFD_MAX_DLEN; |
295 | |
296 | /* absolute dlc values 0 .. 7 => 0 .. 7, e.g. data [0] |
297 | * relative to received dlc -1 .. -8 : |
298 | * e.g. for received dlc = 8 |
299 | * -1 => index = 7 (data[7]) |
300 | * -3 => index = 5 (data[5]) |
301 | * -8 => index = 0 (data[0]) |
302 | */ |
303 | |
304 | if (fr >= -dlen && fr < dlen && |
305 | to >= -dlen && to < dlen && |
306 | re >= -dlen && re < dlen) |
307 | return 0; |
308 | else |
309 | return -EINVAL; |
310 | } |
311 | |
312 | static inline int calc_idx(int idx, int rx_len) |
313 | { |
314 | if (idx < 0) |
315 | return rx_len + idx; |
316 | else |
317 | return idx; |
318 | } |
319 | |
320 | static void cgw_csum_xor_rel(struct canfd_frame *cf, struct cgw_csum_xor *xor) |
321 | { |
322 | int from = calc_idx(idx: xor->from_idx, rx_len: cf->len); |
323 | int to = calc_idx(idx: xor->to_idx, rx_len: cf->len); |
324 | int res = calc_idx(idx: xor->result_idx, rx_len: cf->len); |
325 | u8 val = xor->init_xor_val; |
326 | int i; |
327 | |
328 | if (from < 0 || to < 0 || res < 0) |
329 | return; |
330 | |
331 | if (from <= to) { |
332 | for (i = from; i <= to; i++) |
333 | val ^= cf->data[i]; |
334 | } else { |
335 | for (i = from; i >= to; i--) |
336 | val ^= cf->data[i]; |
337 | } |
338 | |
339 | cf->data[res] = val; |
340 | } |
341 | |
342 | static void cgw_csum_xor_pos(struct canfd_frame *cf, struct cgw_csum_xor *xor) |
343 | { |
344 | u8 val = xor->init_xor_val; |
345 | int i; |
346 | |
347 | for (i = xor->from_idx; i <= xor->to_idx; i++) |
348 | val ^= cf->data[i]; |
349 | |
350 | cf->data[xor->result_idx] = val; |
351 | } |
352 | |
353 | static void cgw_csum_xor_neg(struct canfd_frame *cf, struct cgw_csum_xor *xor) |
354 | { |
355 | u8 val = xor->init_xor_val; |
356 | int i; |
357 | |
358 | for (i = xor->from_idx; i >= xor->to_idx; i--) |
359 | val ^= cf->data[i]; |
360 | |
361 | cf->data[xor->result_idx] = val; |
362 | } |
363 | |
364 | static void cgw_csum_crc8_rel(struct canfd_frame *cf, |
365 | struct cgw_csum_crc8 *crc8) |
366 | { |
367 | int from = calc_idx(idx: crc8->from_idx, rx_len: cf->len); |
368 | int to = calc_idx(idx: crc8->to_idx, rx_len: cf->len); |
369 | int res = calc_idx(idx: crc8->result_idx, rx_len: cf->len); |
370 | u8 crc = crc8->init_crc_val; |
371 | int i; |
372 | |
373 | if (from < 0 || to < 0 || res < 0) |
374 | return; |
375 | |
376 | if (from <= to) { |
377 | for (i = crc8->from_idx; i <= crc8->to_idx; i++) |
378 | crc = crc8->crctab[crc ^ cf->data[i]]; |
379 | } else { |
380 | for (i = crc8->from_idx; i >= crc8->to_idx; i--) |
381 | crc = crc8->crctab[crc ^ cf->data[i]]; |
382 | } |
383 | |
384 | switch (crc8->profile) { |
385 | case CGW_CRC8PRF_1U8: |
386 | crc = crc8->crctab[crc ^ crc8->profile_data[0]]; |
387 | break; |
388 | |
389 | case CGW_CRC8PRF_16U8: |
390 | crc = crc8->crctab[crc ^ crc8->profile_data[cf->data[1] & 0xF]]; |
391 | break; |
392 | |
393 | case CGW_CRC8PRF_SFFID_XOR: |
394 | crc = crc8->crctab[crc ^ (cf->can_id & 0xFF) ^ |
395 | (cf->can_id >> 8 & 0xFF)]; |
396 | break; |
397 | } |
398 | |
399 | cf->data[crc8->result_idx] = crc ^ crc8->final_xor_val; |
400 | } |
401 | |
402 | static void cgw_csum_crc8_pos(struct canfd_frame *cf, |
403 | struct cgw_csum_crc8 *crc8) |
404 | { |
405 | u8 crc = crc8->init_crc_val; |
406 | int i; |
407 | |
408 | for (i = crc8->from_idx; i <= crc8->to_idx; i++) |
409 | crc = crc8->crctab[crc ^ cf->data[i]]; |
410 | |
411 | switch (crc8->profile) { |
412 | case CGW_CRC8PRF_1U8: |
413 | crc = crc8->crctab[crc ^ crc8->profile_data[0]]; |
414 | break; |
415 | |
416 | case CGW_CRC8PRF_16U8: |
417 | crc = crc8->crctab[crc ^ crc8->profile_data[cf->data[1] & 0xF]]; |
418 | break; |
419 | |
420 | case CGW_CRC8PRF_SFFID_XOR: |
421 | crc = crc8->crctab[crc ^ (cf->can_id & 0xFF) ^ |
422 | (cf->can_id >> 8 & 0xFF)]; |
423 | break; |
424 | } |
425 | |
426 | cf->data[crc8->result_idx] = crc ^ crc8->final_xor_val; |
427 | } |
428 | |
429 | static void cgw_csum_crc8_neg(struct canfd_frame *cf, |
430 | struct cgw_csum_crc8 *crc8) |
431 | { |
432 | u8 crc = crc8->init_crc_val; |
433 | int i; |
434 | |
435 | for (i = crc8->from_idx; i >= crc8->to_idx; i--) |
436 | crc = crc8->crctab[crc ^ cf->data[i]]; |
437 | |
438 | switch (crc8->profile) { |
439 | case CGW_CRC8PRF_1U8: |
440 | crc = crc8->crctab[crc ^ crc8->profile_data[0]]; |
441 | break; |
442 | |
443 | case CGW_CRC8PRF_16U8: |
444 | crc = crc8->crctab[crc ^ crc8->profile_data[cf->data[1] & 0xF]]; |
445 | break; |
446 | |
447 | case CGW_CRC8PRF_SFFID_XOR: |
448 | crc = crc8->crctab[crc ^ (cf->can_id & 0xFF) ^ |
449 | (cf->can_id >> 8 & 0xFF)]; |
450 | break; |
451 | } |
452 | |
453 | cf->data[crc8->result_idx] = crc ^ crc8->final_xor_val; |
454 | } |
455 | |
456 | /* the receive & process & send function */ |
457 | static void can_can_gw_rcv(struct sk_buff *skb, void *data) |
458 | { |
459 | struct cgw_job *gwj = (struct cgw_job *)data; |
460 | struct canfd_frame *cf; |
461 | struct sk_buff *nskb; |
462 | int modidx = 0; |
463 | |
464 | /* process strictly Classic CAN or CAN FD frames */ |
465 | if (gwj->flags & CGW_FLAGS_CAN_FD) { |
466 | if (!can_is_canfd_skb(skb)) |
467 | return; |
468 | } else { |
469 | if (!can_is_can_skb(skb)) |
470 | return; |
471 | } |
472 | |
473 | /* Do not handle CAN frames routed more than 'max_hops' times. |
474 | * In general we should never catch this delimiter which is intended |
475 | * to cover a misconfiguration protection (e.g. circular CAN routes). |
476 | * |
477 | * The Controller Area Network controllers only accept CAN frames with |
478 | * correct CRCs - which are not visible in the controller registers. |
479 | * According to skbuff.h documentation the csum_start element for IP |
480 | * checksums is undefined/unused when ip_summed == CHECKSUM_UNNECESSARY. |
481 | * Only CAN skbs can be processed here which already have this property. |
482 | */ |
483 | |
484 | #define cgw_hops(skb) ((skb)->csum_start) |
485 | |
486 | BUG_ON(skb->ip_summed != CHECKSUM_UNNECESSARY); |
487 | |
488 | if (cgw_hops(skb) >= max_hops) { |
489 | /* indicate deleted frames due to misconfiguration */ |
490 | gwj->deleted_frames++; |
491 | return; |
492 | } |
493 | |
494 | if (!(gwj->dst.dev->flags & IFF_UP)) { |
495 | gwj->dropped_frames++; |
496 | return; |
497 | } |
498 | |
499 | /* is sending the skb back to the incoming interface not allowed? */ |
500 | if (!(gwj->flags & CGW_FLAGS_CAN_IIF_TX_OK) && |
501 | can_skb_prv(skb)->ifindex == gwj->dst.dev->ifindex) |
502 | return; |
503 | |
504 | /* clone the given skb, which has not been done in can_rcv() |
505 | * |
506 | * When there is at least one modification function activated, |
507 | * we need to copy the skb as we want to modify skb->data. |
508 | */ |
509 | if (gwj->mod.modfunc[0]) |
510 | nskb = skb_copy(skb, GFP_ATOMIC); |
511 | else |
512 | nskb = skb_clone(skb, GFP_ATOMIC); |
513 | |
514 | if (!nskb) { |
515 | gwj->dropped_frames++; |
516 | return; |
517 | } |
518 | |
519 | /* put the incremented hop counter in the cloned skb */ |
520 | cgw_hops(nskb) = cgw_hops(skb) + 1; |
521 | |
522 | /* first processing of this CAN frame -> adjust to private hop limit */ |
523 | if (gwj->limit_hops && cgw_hops(nskb) == 1) |
524 | cgw_hops(nskb) = max_hops - gwj->limit_hops + 1; |
525 | |
526 | nskb->dev = gwj->dst.dev; |
527 | |
528 | /* pointer to modifiable CAN frame */ |
529 | cf = (struct canfd_frame *)nskb->data; |
530 | |
531 | /* perform preprocessed modification functions if there are any */ |
532 | while (modidx < MAX_MODFUNCTIONS && gwj->mod.modfunc[modidx]) |
533 | (*gwj->mod.modfunc[modidx++])(cf, &gwj->mod); |
534 | |
535 | /* Has the CAN frame been modified? */ |
536 | if (modidx) { |
537 | /* get available space for the processed CAN frame type */ |
538 | int max_len = nskb->len - offsetof(struct canfd_frame, data); |
539 | |
540 | /* dlc may have changed, make sure it fits to the CAN frame */ |
541 | if (cf->len > max_len) { |
542 | /* delete frame due to misconfiguration */ |
543 | gwj->deleted_frames++; |
544 | kfree_skb(skb: nskb); |
545 | return; |
546 | } |
547 | |
548 | /* check for checksum updates */ |
549 | if (gwj->mod.csumfunc.crc8) |
550 | (*gwj->mod.csumfunc.crc8)(cf, &gwj->mod.csum.crc8); |
551 | |
552 | if (gwj->mod.csumfunc.xor) |
553 | (*gwj->mod.csumfunc.xor)(cf, &gwj->mod.csum.xor); |
554 | } |
555 | |
556 | /* clear the skb timestamp if not configured the other way */ |
557 | if (!(gwj->flags & CGW_FLAGS_CAN_SRC_TSTAMP)) |
558 | nskb->tstamp = 0; |
559 | |
560 | /* send to netdevice */ |
561 | if (can_send(skb: nskb, loop: gwj->flags & CGW_FLAGS_CAN_ECHO)) |
562 | gwj->dropped_frames++; |
563 | else |
564 | gwj->handled_frames++; |
565 | } |
566 | |
567 | static inline int cgw_register_filter(struct net *net, struct cgw_job *gwj) |
568 | { |
569 | return can_rx_register(net, dev: gwj->src.dev, can_id: gwj->ccgw.filter.can_id, |
570 | mask: gwj->ccgw.filter.can_mask, func: can_can_gw_rcv, |
571 | data: gwj, ident: "gw" , NULL); |
572 | } |
573 | |
574 | static inline void cgw_unregister_filter(struct net *net, struct cgw_job *gwj) |
575 | { |
576 | can_rx_unregister(net, dev: gwj->src.dev, can_id: gwj->ccgw.filter.can_id, |
577 | mask: gwj->ccgw.filter.can_mask, func: can_can_gw_rcv, data: gwj); |
578 | } |
579 | |
580 | static void cgw_job_free_rcu(struct rcu_head *rcu_head) |
581 | { |
582 | struct cgw_job *gwj = container_of(rcu_head, struct cgw_job, rcu); |
583 | |
584 | kmem_cache_free(s: cgw_cache, objp: gwj); |
585 | } |
586 | |
587 | static int cgw_notifier(struct notifier_block *nb, |
588 | unsigned long msg, void *ptr) |
589 | { |
590 | struct net_device *dev = netdev_notifier_info_to_dev(info: ptr); |
591 | struct net *net = dev_net(dev); |
592 | |
593 | if (dev->type != ARPHRD_CAN) |
594 | return NOTIFY_DONE; |
595 | |
596 | if (msg == NETDEV_UNREGISTER) { |
597 | struct cgw_job *gwj = NULL; |
598 | struct hlist_node *nx; |
599 | |
600 | ASSERT_RTNL(); |
601 | |
602 | hlist_for_each_entry_safe(gwj, nx, &net->can.cgw_list, list) { |
603 | if (gwj->src.dev == dev || gwj->dst.dev == dev) { |
604 | hlist_del(n: &gwj->list); |
605 | cgw_unregister_filter(net, gwj); |
606 | call_rcu(head: &gwj->rcu, func: cgw_job_free_rcu); |
607 | } |
608 | } |
609 | } |
610 | |
611 | return NOTIFY_DONE; |
612 | } |
613 | |
614 | static int cgw_put_job(struct sk_buff *skb, struct cgw_job *gwj, int type, |
615 | u32 pid, u32 seq, int flags) |
616 | { |
617 | struct rtcanmsg *rtcan; |
618 | struct nlmsghdr *nlh; |
619 | |
620 | nlh = nlmsg_put(skb, portid: pid, seq, type, payload: sizeof(*rtcan), flags); |
621 | if (!nlh) |
622 | return -EMSGSIZE; |
623 | |
624 | rtcan = nlmsg_data(nlh); |
625 | rtcan->can_family = AF_CAN; |
626 | rtcan->gwtype = gwj->gwtype; |
627 | rtcan->flags = gwj->flags; |
628 | |
629 | /* add statistics if available */ |
630 | |
631 | if (gwj->handled_frames) { |
632 | if (nla_put_u32(skb, attrtype: CGW_HANDLED, value: gwj->handled_frames) < 0) |
633 | goto cancel; |
634 | } |
635 | |
636 | if (gwj->dropped_frames) { |
637 | if (nla_put_u32(skb, attrtype: CGW_DROPPED, value: gwj->dropped_frames) < 0) |
638 | goto cancel; |
639 | } |
640 | |
641 | if (gwj->deleted_frames) { |
642 | if (nla_put_u32(skb, attrtype: CGW_DELETED, value: gwj->deleted_frames) < 0) |
643 | goto cancel; |
644 | } |
645 | |
646 | /* check non default settings of attributes */ |
647 | |
648 | if (gwj->limit_hops) { |
649 | if (nla_put_u8(skb, attrtype: CGW_LIM_HOPS, value: gwj->limit_hops) < 0) |
650 | goto cancel; |
651 | } |
652 | |
653 | if (gwj->flags & CGW_FLAGS_CAN_FD) { |
654 | struct cgw_fdframe_mod mb; |
655 | |
656 | if (gwj->mod.modtype.and) { |
657 | memcpy(&mb.cf, &gwj->mod.modframe.and, sizeof(mb.cf)); |
658 | mb.modtype = gwj->mod.modtype.and; |
659 | if (nla_put(skb, attrtype: CGW_FDMOD_AND, attrlen: sizeof(mb), data: &mb) < 0) |
660 | goto cancel; |
661 | } |
662 | |
663 | if (gwj->mod.modtype.or) { |
664 | memcpy(&mb.cf, &gwj->mod.modframe.or, sizeof(mb.cf)); |
665 | mb.modtype = gwj->mod.modtype.or; |
666 | if (nla_put(skb, attrtype: CGW_FDMOD_OR, attrlen: sizeof(mb), data: &mb) < 0) |
667 | goto cancel; |
668 | } |
669 | |
670 | if (gwj->mod.modtype.xor) { |
671 | memcpy(&mb.cf, &gwj->mod.modframe.xor, sizeof(mb.cf)); |
672 | mb.modtype = gwj->mod.modtype.xor; |
673 | if (nla_put(skb, attrtype: CGW_FDMOD_XOR, attrlen: sizeof(mb), data: &mb) < 0) |
674 | goto cancel; |
675 | } |
676 | |
677 | if (gwj->mod.modtype.set) { |
678 | memcpy(&mb.cf, &gwj->mod.modframe.set, sizeof(mb.cf)); |
679 | mb.modtype = gwj->mod.modtype.set; |
680 | if (nla_put(skb, attrtype: CGW_FDMOD_SET, attrlen: sizeof(mb), data: &mb) < 0) |
681 | goto cancel; |
682 | } |
683 | } else { |
684 | struct cgw_frame_mod mb; |
685 | |
686 | if (gwj->mod.modtype.and) { |
687 | memcpy(&mb.cf, &gwj->mod.modframe.and, sizeof(mb.cf)); |
688 | mb.modtype = gwj->mod.modtype.and; |
689 | if (nla_put(skb, attrtype: CGW_MOD_AND, attrlen: sizeof(mb), data: &mb) < 0) |
690 | goto cancel; |
691 | } |
692 | |
693 | if (gwj->mod.modtype.or) { |
694 | memcpy(&mb.cf, &gwj->mod.modframe.or, sizeof(mb.cf)); |
695 | mb.modtype = gwj->mod.modtype.or; |
696 | if (nla_put(skb, attrtype: CGW_MOD_OR, attrlen: sizeof(mb), data: &mb) < 0) |
697 | goto cancel; |
698 | } |
699 | |
700 | if (gwj->mod.modtype.xor) { |
701 | memcpy(&mb.cf, &gwj->mod.modframe.xor, sizeof(mb.cf)); |
702 | mb.modtype = gwj->mod.modtype.xor; |
703 | if (nla_put(skb, attrtype: CGW_MOD_XOR, attrlen: sizeof(mb), data: &mb) < 0) |
704 | goto cancel; |
705 | } |
706 | |
707 | if (gwj->mod.modtype.set) { |
708 | memcpy(&mb.cf, &gwj->mod.modframe.set, sizeof(mb.cf)); |
709 | mb.modtype = gwj->mod.modtype.set; |
710 | if (nla_put(skb, attrtype: CGW_MOD_SET, attrlen: sizeof(mb), data: &mb) < 0) |
711 | goto cancel; |
712 | } |
713 | } |
714 | |
715 | if (gwj->mod.uid) { |
716 | if (nla_put_u32(skb, attrtype: CGW_MOD_UID, value: gwj->mod.uid) < 0) |
717 | goto cancel; |
718 | } |
719 | |
720 | if (gwj->mod.csumfunc.crc8) { |
721 | if (nla_put(skb, attrtype: CGW_CS_CRC8, CGW_CS_CRC8_LEN, |
722 | data: &gwj->mod.csum.crc8) < 0) |
723 | goto cancel; |
724 | } |
725 | |
726 | if (gwj->mod.csumfunc.xor) { |
727 | if (nla_put(skb, attrtype: CGW_CS_XOR, CGW_CS_XOR_LEN, |
728 | data: &gwj->mod.csum.xor) < 0) |
729 | goto cancel; |
730 | } |
731 | |
732 | if (gwj->gwtype == CGW_TYPE_CAN_CAN) { |
733 | if (gwj->ccgw.filter.can_id || gwj->ccgw.filter.can_mask) { |
734 | if (nla_put(skb, attrtype: CGW_FILTER, attrlen: sizeof(struct can_filter), |
735 | data: &gwj->ccgw.filter) < 0) |
736 | goto cancel; |
737 | } |
738 | |
739 | if (nla_put_u32(skb, attrtype: CGW_SRC_IF, value: gwj->ccgw.src_idx) < 0) |
740 | goto cancel; |
741 | |
742 | if (nla_put_u32(skb, attrtype: CGW_DST_IF, value: gwj->ccgw.dst_idx) < 0) |
743 | goto cancel; |
744 | } |
745 | |
746 | nlmsg_end(skb, nlh); |
747 | return 0; |
748 | |
749 | cancel: |
750 | nlmsg_cancel(skb, nlh); |
751 | return -EMSGSIZE; |
752 | } |
753 | |
754 | /* Dump information about all CAN gateway jobs, in response to RTM_GETROUTE */ |
755 | static int cgw_dump_jobs(struct sk_buff *skb, struct netlink_callback *cb) |
756 | { |
757 | struct net *net = sock_net(sk: skb->sk); |
758 | struct cgw_job *gwj = NULL; |
759 | int idx = 0; |
760 | int s_idx = cb->args[0]; |
761 | |
762 | rcu_read_lock(); |
763 | hlist_for_each_entry_rcu(gwj, &net->can.cgw_list, list) { |
764 | if (idx < s_idx) |
765 | goto cont; |
766 | |
767 | if (cgw_put_job(skb, gwj, RTM_NEWROUTE, |
768 | NETLINK_CB(cb->skb).portid, |
769 | seq: cb->nlh->nlmsg_seq, NLM_F_MULTI) < 0) |
770 | break; |
771 | cont: |
772 | idx++; |
773 | } |
774 | rcu_read_unlock(); |
775 | |
776 | cb->args[0] = idx; |
777 | |
778 | return skb->len; |
779 | } |
780 | |
781 | static const struct nla_policy cgw_policy[CGW_MAX + 1] = { |
782 | [CGW_MOD_AND] = { .len = sizeof(struct cgw_frame_mod) }, |
783 | [CGW_MOD_OR] = { .len = sizeof(struct cgw_frame_mod) }, |
784 | [CGW_MOD_XOR] = { .len = sizeof(struct cgw_frame_mod) }, |
785 | [CGW_MOD_SET] = { .len = sizeof(struct cgw_frame_mod) }, |
786 | [CGW_CS_XOR] = { .len = sizeof(struct cgw_csum_xor) }, |
787 | [CGW_CS_CRC8] = { .len = sizeof(struct cgw_csum_crc8) }, |
788 | [CGW_SRC_IF] = { .type = NLA_U32 }, |
789 | [CGW_DST_IF] = { .type = NLA_U32 }, |
790 | [CGW_FILTER] = { .len = sizeof(struct can_filter) }, |
791 | [CGW_LIM_HOPS] = { .type = NLA_U8 }, |
792 | [CGW_MOD_UID] = { .type = NLA_U32 }, |
793 | [CGW_FDMOD_AND] = { .len = sizeof(struct cgw_fdframe_mod) }, |
794 | [CGW_FDMOD_OR] = { .len = sizeof(struct cgw_fdframe_mod) }, |
795 | [CGW_FDMOD_XOR] = { .len = sizeof(struct cgw_fdframe_mod) }, |
796 | [CGW_FDMOD_SET] = { .len = sizeof(struct cgw_fdframe_mod) }, |
797 | }; |
798 | |
799 | /* check for common and gwtype specific attributes */ |
800 | static int cgw_parse_attr(struct nlmsghdr *nlh, struct cf_mod *mod, |
801 | u8 gwtype, void *gwtypeattr, u8 *limhops) |
802 | { |
803 | struct nlattr *tb[CGW_MAX + 1]; |
804 | struct rtcanmsg *r = nlmsg_data(nlh); |
805 | int modidx = 0; |
806 | int err = 0; |
807 | |
808 | /* initialize modification & checksum data space */ |
809 | memset(mod, 0, sizeof(*mod)); |
810 | |
811 | err = nlmsg_parse_deprecated(nlh, hdrlen: sizeof(struct rtcanmsg), tb, |
812 | CGW_MAX, policy: cgw_policy, NULL); |
813 | if (err < 0) |
814 | return err; |
815 | |
816 | if (tb[CGW_LIM_HOPS]) { |
817 | *limhops = nla_get_u8(nla: tb[CGW_LIM_HOPS]); |
818 | |
819 | if (*limhops < 1 || *limhops > max_hops) |
820 | return -EINVAL; |
821 | } |
822 | |
823 | /* check for AND/OR/XOR/SET modifications */ |
824 | if (r->flags & CGW_FLAGS_CAN_FD) { |
825 | struct cgw_fdframe_mod mb; |
826 | |
827 | if (tb[CGW_FDMOD_AND]) { |
828 | nla_memcpy(dest: &mb, src: tb[CGW_FDMOD_AND], CGW_FDMODATTR_LEN); |
829 | |
830 | canfdframecpy(dst: &mod->modframe.and, src: &mb.cf); |
831 | mod->modtype.and = mb.modtype; |
832 | |
833 | if (mb.modtype & CGW_MOD_ID) |
834 | mod->modfunc[modidx++] = mod_and_id; |
835 | |
836 | if (mb.modtype & CGW_MOD_LEN) |
837 | mod->modfunc[modidx++] = mod_and_len; |
838 | |
839 | if (mb.modtype & CGW_MOD_FLAGS) |
840 | mod->modfunc[modidx++] = mod_and_flags; |
841 | |
842 | if (mb.modtype & CGW_MOD_DATA) |
843 | mod->modfunc[modidx++] = mod_and_fddata; |
844 | } |
845 | |
846 | if (tb[CGW_FDMOD_OR]) { |
847 | nla_memcpy(dest: &mb, src: tb[CGW_FDMOD_OR], CGW_FDMODATTR_LEN); |
848 | |
849 | canfdframecpy(dst: &mod->modframe.or, src: &mb.cf); |
850 | mod->modtype.or = mb.modtype; |
851 | |
852 | if (mb.modtype & CGW_MOD_ID) |
853 | mod->modfunc[modidx++] = mod_or_id; |
854 | |
855 | if (mb.modtype & CGW_MOD_LEN) |
856 | mod->modfunc[modidx++] = mod_or_len; |
857 | |
858 | if (mb.modtype & CGW_MOD_FLAGS) |
859 | mod->modfunc[modidx++] = mod_or_flags; |
860 | |
861 | if (mb.modtype & CGW_MOD_DATA) |
862 | mod->modfunc[modidx++] = mod_or_fddata; |
863 | } |
864 | |
865 | if (tb[CGW_FDMOD_XOR]) { |
866 | nla_memcpy(dest: &mb, src: tb[CGW_FDMOD_XOR], CGW_FDMODATTR_LEN); |
867 | |
868 | canfdframecpy(dst: &mod->modframe.xor, src: &mb.cf); |
869 | mod->modtype.xor = mb.modtype; |
870 | |
871 | if (mb.modtype & CGW_MOD_ID) |
872 | mod->modfunc[modidx++] = mod_xor_id; |
873 | |
874 | if (mb.modtype & CGW_MOD_LEN) |
875 | mod->modfunc[modidx++] = mod_xor_len; |
876 | |
877 | if (mb.modtype & CGW_MOD_FLAGS) |
878 | mod->modfunc[modidx++] = mod_xor_flags; |
879 | |
880 | if (mb.modtype & CGW_MOD_DATA) |
881 | mod->modfunc[modidx++] = mod_xor_fddata; |
882 | } |
883 | |
884 | if (tb[CGW_FDMOD_SET]) { |
885 | nla_memcpy(dest: &mb, src: tb[CGW_FDMOD_SET], CGW_FDMODATTR_LEN); |
886 | |
887 | canfdframecpy(dst: &mod->modframe.set, src: &mb.cf); |
888 | mod->modtype.set = mb.modtype; |
889 | |
890 | if (mb.modtype & CGW_MOD_ID) |
891 | mod->modfunc[modidx++] = mod_set_id; |
892 | |
893 | if (mb.modtype & CGW_MOD_LEN) |
894 | mod->modfunc[modidx++] = mod_set_len; |
895 | |
896 | if (mb.modtype & CGW_MOD_FLAGS) |
897 | mod->modfunc[modidx++] = mod_set_flags; |
898 | |
899 | if (mb.modtype & CGW_MOD_DATA) |
900 | mod->modfunc[modidx++] = mod_set_fddata; |
901 | } |
902 | } else { |
903 | struct cgw_frame_mod mb; |
904 | |
905 | if (tb[CGW_MOD_AND]) { |
906 | nla_memcpy(dest: &mb, src: tb[CGW_MOD_AND], CGW_MODATTR_LEN); |
907 | |
908 | canframecpy(dst: &mod->modframe.and, src: &mb.cf); |
909 | mod->modtype.and = mb.modtype; |
910 | |
911 | if (mb.modtype & CGW_MOD_ID) |
912 | mod->modfunc[modidx++] = mod_and_id; |
913 | |
914 | if (mb.modtype & CGW_MOD_DLC) |
915 | mod->modfunc[modidx++] = mod_and_ccdlc; |
916 | |
917 | if (mb.modtype & CGW_MOD_DATA) |
918 | mod->modfunc[modidx++] = mod_and_data; |
919 | } |
920 | |
921 | if (tb[CGW_MOD_OR]) { |
922 | nla_memcpy(dest: &mb, src: tb[CGW_MOD_OR], CGW_MODATTR_LEN); |
923 | |
924 | canframecpy(dst: &mod->modframe.or, src: &mb.cf); |
925 | mod->modtype.or = mb.modtype; |
926 | |
927 | if (mb.modtype & CGW_MOD_ID) |
928 | mod->modfunc[modidx++] = mod_or_id; |
929 | |
930 | if (mb.modtype & CGW_MOD_DLC) |
931 | mod->modfunc[modidx++] = mod_or_ccdlc; |
932 | |
933 | if (mb.modtype & CGW_MOD_DATA) |
934 | mod->modfunc[modidx++] = mod_or_data; |
935 | } |
936 | |
937 | if (tb[CGW_MOD_XOR]) { |
938 | nla_memcpy(dest: &mb, src: tb[CGW_MOD_XOR], CGW_MODATTR_LEN); |
939 | |
940 | canframecpy(dst: &mod->modframe.xor, src: &mb.cf); |
941 | mod->modtype.xor = mb.modtype; |
942 | |
943 | if (mb.modtype & CGW_MOD_ID) |
944 | mod->modfunc[modidx++] = mod_xor_id; |
945 | |
946 | if (mb.modtype & CGW_MOD_DLC) |
947 | mod->modfunc[modidx++] = mod_xor_ccdlc; |
948 | |
949 | if (mb.modtype & CGW_MOD_DATA) |
950 | mod->modfunc[modidx++] = mod_xor_data; |
951 | } |
952 | |
953 | if (tb[CGW_MOD_SET]) { |
954 | nla_memcpy(dest: &mb, src: tb[CGW_MOD_SET], CGW_MODATTR_LEN); |
955 | |
956 | canframecpy(dst: &mod->modframe.set, src: &mb.cf); |
957 | mod->modtype.set = mb.modtype; |
958 | |
959 | if (mb.modtype & CGW_MOD_ID) |
960 | mod->modfunc[modidx++] = mod_set_id; |
961 | |
962 | if (mb.modtype & CGW_MOD_DLC) |
963 | mod->modfunc[modidx++] = mod_set_ccdlc; |
964 | |
965 | if (mb.modtype & CGW_MOD_DATA) |
966 | mod->modfunc[modidx++] = mod_set_data; |
967 | } |
968 | } |
969 | |
970 | /* check for checksum operations after CAN frame modifications */ |
971 | if (modidx) { |
972 | if (tb[CGW_CS_CRC8]) { |
973 | struct cgw_csum_crc8 *c = nla_data(nla: tb[CGW_CS_CRC8]); |
974 | |
975 | err = cgw_chk_csum_parms(fr: c->from_idx, to: c->to_idx, |
976 | re: c->result_idx, r); |
977 | if (err) |
978 | return err; |
979 | |
980 | nla_memcpy(dest: &mod->csum.crc8, src: tb[CGW_CS_CRC8], |
981 | CGW_CS_CRC8_LEN); |
982 | |
983 | /* select dedicated processing function to reduce |
984 | * runtime operations in receive hot path. |
985 | */ |
986 | if (c->from_idx < 0 || c->to_idx < 0 || |
987 | c->result_idx < 0) |
988 | mod->csumfunc.crc8 = cgw_csum_crc8_rel; |
989 | else if (c->from_idx <= c->to_idx) |
990 | mod->csumfunc.crc8 = cgw_csum_crc8_pos; |
991 | else |
992 | mod->csumfunc.crc8 = cgw_csum_crc8_neg; |
993 | } |
994 | |
995 | if (tb[CGW_CS_XOR]) { |
996 | struct cgw_csum_xor *c = nla_data(nla: tb[CGW_CS_XOR]); |
997 | |
998 | err = cgw_chk_csum_parms(fr: c->from_idx, to: c->to_idx, |
999 | re: c->result_idx, r); |
1000 | if (err) |
1001 | return err; |
1002 | |
1003 | nla_memcpy(dest: &mod->csum.xor, src: tb[CGW_CS_XOR], |
1004 | CGW_CS_XOR_LEN); |
1005 | |
1006 | /* select dedicated processing function to reduce |
1007 | * runtime operations in receive hot path. |
1008 | */ |
1009 | if (c->from_idx < 0 || c->to_idx < 0 || |
1010 | c->result_idx < 0) |
1011 | mod->csumfunc.xor = cgw_csum_xor_rel; |
1012 | else if (c->from_idx <= c->to_idx) |
1013 | mod->csumfunc.xor = cgw_csum_xor_pos; |
1014 | else |
1015 | mod->csumfunc.xor = cgw_csum_xor_neg; |
1016 | } |
1017 | |
1018 | if (tb[CGW_MOD_UID]) |
1019 | nla_memcpy(dest: &mod->uid, src: tb[CGW_MOD_UID], count: sizeof(u32)); |
1020 | } |
1021 | |
1022 | if (gwtype == CGW_TYPE_CAN_CAN) { |
1023 | /* check CGW_TYPE_CAN_CAN specific attributes */ |
1024 | struct can_can_gw *ccgw = (struct can_can_gw *)gwtypeattr; |
1025 | |
1026 | memset(ccgw, 0, sizeof(*ccgw)); |
1027 | |
1028 | /* check for can_filter in attributes */ |
1029 | if (tb[CGW_FILTER]) |
1030 | nla_memcpy(dest: &ccgw->filter, src: tb[CGW_FILTER], |
1031 | count: sizeof(struct can_filter)); |
1032 | |
1033 | err = -ENODEV; |
1034 | |
1035 | /* specifying two interfaces is mandatory */ |
1036 | if (!tb[CGW_SRC_IF] || !tb[CGW_DST_IF]) |
1037 | return err; |
1038 | |
1039 | ccgw->src_idx = nla_get_u32(nla: tb[CGW_SRC_IF]); |
1040 | ccgw->dst_idx = nla_get_u32(nla: tb[CGW_DST_IF]); |
1041 | |
1042 | /* both indices set to 0 for flushing all routing entries */ |
1043 | if (!ccgw->src_idx && !ccgw->dst_idx) |
1044 | return 0; |
1045 | |
1046 | /* only one index set to 0 is an error */ |
1047 | if (!ccgw->src_idx || !ccgw->dst_idx) |
1048 | return err; |
1049 | } |
1050 | |
1051 | /* add the checks for other gwtypes here */ |
1052 | |
1053 | return 0; |
1054 | } |
1055 | |
1056 | static int cgw_create_job(struct sk_buff *skb, struct nlmsghdr *nlh, |
1057 | struct netlink_ext_ack *extack) |
1058 | { |
1059 | struct net *net = sock_net(sk: skb->sk); |
1060 | struct rtcanmsg *r; |
1061 | struct cgw_job *gwj; |
1062 | struct cf_mod mod; |
1063 | struct can_can_gw ccgw; |
1064 | u8 limhops = 0; |
1065 | int err = 0; |
1066 | |
1067 | if (!netlink_capable(skb, CAP_NET_ADMIN)) |
1068 | return -EPERM; |
1069 | |
1070 | if (nlmsg_len(nlh) < sizeof(*r)) |
1071 | return -EINVAL; |
1072 | |
1073 | r = nlmsg_data(nlh); |
1074 | if (r->can_family != AF_CAN) |
1075 | return -EPFNOSUPPORT; |
1076 | |
1077 | /* so far we only support CAN -> CAN routings */ |
1078 | if (r->gwtype != CGW_TYPE_CAN_CAN) |
1079 | return -EINVAL; |
1080 | |
1081 | err = cgw_parse_attr(nlh, mod: &mod, gwtype: CGW_TYPE_CAN_CAN, gwtypeattr: &ccgw, limhops: &limhops); |
1082 | if (err < 0) |
1083 | return err; |
1084 | |
1085 | if (mod.uid) { |
1086 | ASSERT_RTNL(); |
1087 | |
1088 | /* check for updating an existing job with identical uid */ |
1089 | hlist_for_each_entry(gwj, &net->can.cgw_list, list) { |
1090 | if (gwj->mod.uid != mod.uid) |
1091 | continue; |
1092 | |
1093 | /* interfaces & filters must be identical */ |
1094 | if (memcmp(p: &gwj->ccgw, q: &ccgw, size: sizeof(ccgw))) |
1095 | return -EINVAL; |
1096 | |
1097 | /* update modifications with disabled softirq & quit */ |
1098 | local_bh_disable(); |
1099 | memcpy(&gwj->mod, &mod, sizeof(mod)); |
1100 | local_bh_enable(); |
1101 | return 0; |
1102 | } |
1103 | } |
1104 | |
1105 | /* ifindex == 0 is not allowed for job creation */ |
1106 | if (!ccgw.src_idx || !ccgw.dst_idx) |
1107 | return -ENODEV; |
1108 | |
1109 | gwj = kmem_cache_alloc(cachep: cgw_cache, GFP_KERNEL); |
1110 | if (!gwj) |
1111 | return -ENOMEM; |
1112 | |
1113 | gwj->handled_frames = 0; |
1114 | gwj->dropped_frames = 0; |
1115 | gwj->deleted_frames = 0; |
1116 | gwj->flags = r->flags; |
1117 | gwj->gwtype = r->gwtype; |
1118 | gwj->limit_hops = limhops; |
1119 | |
1120 | /* insert already parsed information */ |
1121 | memcpy(&gwj->mod, &mod, sizeof(mod)); |
1122 | memcpy(&gwj->ccgw, &ccgw, sizeof(ccgw)); |
1123 | |
1124 | err = -ENODEV; |
1125 | |
1126 | gwj->src.dev = __dev_get_by_index(net, ifindex: gwj->ccgw.src_idx); |
1127 | |
1128 | if (!gwj->src.dev) |
1129 | goto out; |
1130 | |
1131 | if (gwj->src.dev->type != ARPHRD_CAN) |
1132 | goto out; |
1133 | |
1134 | gwj->dst.dev = __dev_get_by_index(net, ifindex: gwj->ccgw.dst_idx); |
1135 | |
1136 | if (!gwj->dst.dev) |
1137 | goto out; |
1138 | |
1139 | if (gwj->dst.dev->type != ARPHRD_CAN) |
1140 | goto out; |
1141 | |
1142 | /* is sending the skb back to the incoming interface intended? */ |
1143 | if (gwj->src.dev == gwj->dst.dev && |
1144 | !(gwj->flags & CGW_FLAGS_CAN_IIF_TX_OK)) { |
1145 | err = -EINVAL; |
1146 | goto out; |
1147 | } |
1148 | |
1149 | ASSERT_RTNL(); |
1150 | |
1151 | err = cgw_register_filter(net, gwj); |
1152 | if (!err) |
1153 | hlist_add_head_rcu(n: &gwj->list, h: &net->can.cgw_list); |
1154 | out: |
1155 | if (err) |
1156 | kmem_cache_free(s: cgw_cache, objp: gwj); |
1157 | |
1158 | return err; |
1159 | } |
1160 | |
1161 | static void cgw_remove_all_jobs(struct net *net) |
1162 | { |
1163 | struct cgw_job *gwj = NULL; |
1164 | struct hlist_node *nx; |
1165 | |
1166 | ASSERT_RTNL(); |
1167 | |
1168 | hlist_for_each_entry_safe(gwj, nx, &net->can.cgw_list, list) { |
1169 | hlist_del(n: &gwj->list); |
1170 | cgw_unregister_filter(net, gwj); |
1171 | call_rcu(head: &gwj->rcu, func: cgw_job_free_rcu); |
1172 | } |
1173 | } |
1174 | |
1175 | static int cgw_remove_job(struct sk_buff *skb, struct nlmsghdr *nlh, |
1176 | struct netlink_ext_ack *extack) |
1177 | { |
1178 | struct net *net = sock_net(sk: skb->sk); |
1179 | struct cgw_job *gwj = NULL; |
1180 | struct hlist_node *nx; |
1181 | struct rtcanmsg *r; |
1182 | struct cf_mod mod; |
1183 | struct can_can_gw ccgw; |
1184 | u8 limhops = 0; |
1185 | int err = 0; |
1186 | |
1187 | if (!netlink_capable(skb, CAP_NET_ADMIN)) |
1188 | return -EPERM; |
1189 | |
1190 | if (nlmsg_len(nlh) < sizeof(*r)) |
1191 | return -EINVAL; |
1192 | |
1193 | r = nlmsg_data(nlh); |
1194 | if (r->can_family != AF_CAN) |
1195 | return -EPFNOSUPPORT; |
1196 | |
1197 | /* so far we only support CAN -> CAN routings */ |
1198 | if (r->gwtype != CGW_TYPE_CAN_CAN) |
1199 | return -EINVAL; |
1200 | |
1201 | err = cgw_parse_attr(nlh, mod: &mod, gwtype: CGW_TYPE_CAN_CAN, gwtypeattr: &ccgw, limhops: &limhops); |
1202 | if (err < 0) |
1203 | return err; |
1204 | |
1205 | /* two interface indices both set to 0 => remove all entries */ |
1206 | if (!ccgw.src_idx && !ccgw.dst_idx) { |
1207 | cgw_remove_all_jobs(net); |
1208 | return 0; |
1209 | } |
1210 | |
1211 | err = -EINVAL; |
1212 | |
1213 | ASSERT_RTNL(); |
1214 | |
1215 | /* remove only the first matching entry */ |
1216 | hlist_for_each_entry_safe(gwj, nx, &net->can.cgw_list, list) { |
1217 | if (gwj->flags != r->flags) |
1218 | continue; |
1219 | |
1220 | if (gwj->limit_hops != limhops) |
1221 | continue; |
1222 | |
1223 | /* we have a match when uid is enabled and identical */ |
1224 | if (gwj->mod.uid || mod.uid) { |
1225 | if (gwj->mod.uid != mod.uid) |
1226 | continue; |
1227 | } else { |
1228 | /* no uid => check for identical modifications */ |
1229 | if (memcmp(p: &gwj->mod, q: &mod, size: sizeof(mod))) |
1230 | continue; |
1231 | } |
1232 | |
1233 | /* if (r->gwtype == CGW_TYPE_CAN_CAN) - is made sure here */ |
1234 | if (memcmp(p: &gwj->ccgw, q: &ccgw, size: sizeof(ccgw))) |
1235 | continue; |
1236 | |
1237 | hlist_del(n: &gwj->list); |
1238 | cgw_unregister_filter(net, gwj); |
1239 | call_rcu(head: &gwj->rcu, func: cgw_job_free_rcu); |
1240 | err = 0; |
1241 | break; |
1242 | } |
1243 | |
1244 | return err; |
1245 | } |
1246 | |
1247 | static int __net_init cangw_pernet_init(struct net *net) |
1248 | { |
1249 | INIT_HLIST_HEAD(&net->can.cgw_list); |
1250 | return 0; |
1251 | } |
1252 | |
1253 | static void __net_exit cangw_pernet_exit_batch(struct list_head *net_list) |
1254 | { |
1255 | struct net *net; |
1256 | |
1257 | rtnl_lock(); |
1258 | list_for_each_entry(net, net_list, exit_list) |
1259 | cgw_remove_all_jobs(net); |
1260 | rtnl_unlock(); |
1261 | } |
1262 | |
1263 | static struct pernet_operations cangw_pernet_ops = { |
1264 | .init = cangw_pernet_init, |
1265 | .exit_batch = cangw_pernet_exit_batch, |
1266 | }; |
1267 | |
1268 | static __init int cgw_module_init(void) |
1269 | { |
1270 | int ret; |
1271 | |
1272 | /* sanitize given module parameter */ |
1273 | max_hops = clamp_t(unsigned int, max_hops, CGW_MIN_HOPS, CGW_MAX_HOPS); |
1274 | |
1275 | pr_info("can: netlink gateway - max_hops=%d\n" , max_hops); |
1276 | |
1277 | ret = register_pernet_subsys(&cangw_pernet_ops); |
1278 | if (ret) |
1279 | return ret; |
1280 | |
1281 | ret = -ENOMEM; |
1282 | cgw_cache = kmem_cache_create(name: "can_gw" , size: sizeof(struct cgw_job), |
1283 | align: 0, flags: 0, NULL); |
1284 | if (!cgw_cache) |
1285 | goto out_cache_create; |
1286 | |
1287 | /* set notifier */ |
1288 | notifier.notifier_call = cgw_notifier; |
1289 | ret = register_netdevice_notifier(nb: ¬ifier); |
1290 | if (ret) |
1291 | goto out_register_notifier; |
1292 | |
1293 | ret = rtnl_register_module(THIS_MODULE, PF_CAN, RTM_GETROUTE, |
1294 | NULL, cgw_dump_jobs, flags: 0); |
1295 | if (ret) |
1296 | goto out_rtnl_register1; |
1297 | |
1298 | ret = rtnl_register_module(THIS_MODULE, PF_CAN, RTM_NEWROUTE, |
1299 | cgw_create_job, NULL, flags: 0); |
1300 | if (ret) |
1301 | goto out_rtnl_register2; |
1302 | ret = rtnl_register_module(THIS_MODULE, PF_CAN, RTM_DELROUTE, |
1303 | cgw_remove_job, NULL, flags: 0); |
1304 | if (ret) |
1305 | goto out_rtnl_register3; |
1306 | |
1307 | return 0; |
1308 | |
1309 | out_rtnl_register3: |
1310 | rtnl_unregister(PF_CAN, RTM_NEWROUTE); |
1311 | out_rtnl_register2: |
1312 | rtnl_unregister(PF_CAN, RTM_GETROUTE); |
1313 | out_rtnl_register1: |
1314 | unregister_netdevice_notifier(nb: ¬ifier); |
1315 | out_register_notifier: |
1316 | kmem_cache_destroy(s: cgw_cache); |
1317 | out_cache_create: |
1318 | unregister_pernet_subsys(&cangw_pernet_ops); |
1319 | |
1320 | return ret; |
1321 | } |
1322 | |
1323 | static __exit void cgw_module_exit(void) |
1324 | { |
1325 | rtnl_unregister_all(PF_CAN); |
1326 | |
1327 | unregister_netdevice_notifier(nb: ¬ifier); |
1328 | |
1329 | unregister_pernet_subsys(&cangw_pernet_ops); |
1330 | rcu_barrier(); /* Wait for completion of call_rcu()'s */ |
1331 | |
1332 | kmem_cache_destroy(s: cgw_cache); |
1333 | } |
1334 | |
1335 | module_init(cgw_module_init); |
1336 | module_exit(cgw_module_exit); |
1337 | |