1 // SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
2 /* isotp.c - ISO 15765-2 CAN transport protocol for protocol family CAN
3 *
4 * This implementation does not provide ISO-TP specific return values to the
5 * userspace.
6 *
7 * - RX path timeout of data reception leads to -ETIMEDOUT
8 * - RX path SN mismatch leads to -EILSEQ
9 * - RX path data reception with wrong padding leads to -EBADMSG
10 * - TX path flowcontrol reception timeout leads to -ECOMM
11 * - TX path flowcontrol reception overflow leads to -EMSGSIZE
12 * - TX path flowcontrol reception with wrong layout/padding leads to -EBADMSG
13 * - when a transfer (tx) is on the run the next write() blocks until it's done
14 * - use CAN_ISOTP_WAIT_TX_DONE flag to block the caller until the PDU is sent
15 * - as we have static buffers the check whether the PDU fits into the buffer
16 * is done at FF reception time (no support for sending 'wait frames')
17 *
18 * Copyright (c) 2020 Volkswagen Group Electronic Research
19 * All rights reserved.
20 *
21 * Redistribution and use in source and binary forms, with or without
22 * modification, are permitted provided that the following conditions
23 * are met:
24 * 1. Redistributions of source code must retain the above copyright
25 * notice, this list of conditions and the following disclaimer.
26 * 2. Redistributions in binary form must reproduce the above copyright
27 * notice, this list of conditions and the following disclaimer in the
28 * documentation and/or other materials provided with the distribution.
29 * 3. Neither the name of Volkswagen nor the names of its contributors
30 * may be used to endorse or promote products derived from this software
31 * without specific prior written permission.
32 *
33 * Alternatively, provided that this notice is retained in full, this
34 * software may be distributed under the terms of the GNU General
35 * Public License ("GPL") version 2, in which case the provisions of the
36 * GPL apply INSTEAD OF those given above.
37 *
38 * The provided data structures and external interfaces from this code
39 * are not restricted to be used by modules with a GPL compatible license.
40 *
41 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
42 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
43 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
44 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
45 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
46 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
47 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
48 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
49 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
50 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
51 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
52 * DAMAGE.
53 */
54
55 #include <linux/module.h>
56 #include <linux/init.h>
57 #include <linux/interrupt.h>
58 #include <linux/spinlock.h>
59 #include <linux/hrtimer.h>
60 #include <linux/wait.h>
61 #include <linux/uio.h>
62 #include <linux/net.h>
63 #include <linux/netdevice.h>
64 #include <linux/socket.h>
65 #include <linux/if_arp.h>
66 #include <linux/skbuff.h>
67 #include <linux/can.h>
68 #include <linux/can/core.h>
69 #include <linux/can/skb.h>
70 #include <linux/can/isotp.h>
71 #include <linux/slab.h>
72 #include <net/sock.h>
73 #include <net/net_namespace.h>
74
75 MODULE_DESCRIPTION("PF_CAN isotp 15765-2:2016 protocol");
76 MODULE_LICENSE("Dual BSD/GPL");
77 MODULE_AUTHOR("Oliver Hartkopp <socketcan@hartkopp.net>");
78 MODULE_ALIAS("can-proto-6");
79
80 #define ISOTP_MIN_NAMELEN CAN_REQUIRED_SIZE(struct sockaddr_can, can_addr.tp)
81
82 #define SINGLE_MASK(id) (((id) & CAN_EFF_FLAG) ? \
83 (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \
84 (CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG))
85
86 /* ISO 15765-2:2016 supports more than 4095 byte per ISO PDU as the FF_DL can
87 * take full 32 bit values (4 Gbyte). We would need some good concept to handle
88 * this between user space and kernel space. For now set the static buffer to
89 * something about 8 kbyte to be able to test this new functionality.
90 */
91 #define DEFAULT_MAX_PDU_SIZE 8300
92
93 /* maximum PDU size before ISO 15765-2:2016 extension was 4095 */
94 #define MAX_12BIT_PDU_SIZE 4095
95
96 /* limit the isotp pdu size from the optional module parameter to 1MByte */
97 #define MAX_PDU_SIZE (1025 * 1024U)
98
99 static unsigned int max_pdu_size __read_mostly = DEFAULT_MAX_PDU_SIZE;
100 module_param(max_pdu_size, uint, 0444);
101 MODULE_PARM_DESC(max_pdu_size, "maximum isotp pdu size (default "
102 __stringify(DEFAULT_MAX_PDU_SIZE) ")");
103
104 /* N_PCI type values in bits 7-4 of N_PCI bytes */
105 #define N_PCI_SF 0x00 /* single frame */
106 #define N_PCI_FF 0x10 /* first frame */
107 #define N_PCI_CF 0x20 /* consecutive frame */
108 #define N_PCI_FC 0x30 /* flow control */
109
110 #define N_PCI_SZ 1 /* size of the PCI byte #1 */
111 #define SF_PCI_SZ4 1 /* size of SingleFrame PCI including 4 bit SF_DL */
112 #define SF_PCI_SZ8 2 /* size of SingleFrame PCI including 8 bit SF_DL */
113 #define FF_PCI_SZ12 2 /* size of FirstFrame PCI including 12 bit FF_DL */
114 #define FF_PCI_SZ32 6 /* size of FirstFrame PCI including 32 bit FF_DL */
115 #define FC_CONTENT_SZ 3 /* flow control content size in byte (FS/BS/STmin) */
116
117 #define ISOTP_CHECK_PADDING (CAN_ISOTP_CHK_PAD_LEN | CAN_ISOTP_CHK_PAD_DATA)
118 #define ISOTP_ALL_BC_FLAGS (CAN_ISOTP_SF_BROADCAST | CAN_ISOTP_CF_BROADCAST)
119
120 /* Flow Status given in FC frame */
121 #define ISOTP_FC_CTS 0 /* clear to send */
122 #define ISOTP_FC_WT 1 /* wait */
123 #define ISOTP_FC_OVFLW 2 /* overflow */
124
125 #define ISOTP_FC_TIMEOUT 1 /* 1 sec */
126 #define ISOTP_ECHO_TIMEOUT 2 /* 2 secs */
127
128 enum {
129 ISOTP_IDLE = 0,
130 ISOTP_WAIT_FIRST_FC,
131 ISOTP_WAIT_FC,
132 ISOTP_WAIT_DATA,
133 ISOTP_SENDING,
134 ISOTP_SHUTDOWN,
135 };
136
137 struct tpcon {
138 u8 *buf;
139 unsigned int buflen;
140 unsigned int len;
141 unsigned int idx;
142 u32 state;
143 u8 bs;
144 u8 sn;
145 u8 ll_dl;
146 u8 sbuf[DEFAULT_MAX_PDU_SIZE];
147 };
148
149 struct isotp_sock {
150 struct sock sk;
151 int bound;
152 int ifindex;
153 canid_t txid;
154 canid_t rxid;
155 ktime_t tx_gap;
156 ktime_t lastrxcf_tstamp;
157 struct hrtimer rxtimer, txtimer, txfrtimer;
158 struct can_isotp_options opt;
159 struct can_isotp_fc_options rxfc, txfc;
160 struct can_isotp_ll_options ll;
161 u32 frame_txtime;
162 u32 force_tx_stmin;
163 u32 force_rx_stmin;
164 u32 cfecho; /* consecutive frame echo tag */
165 struct tpcon rx, tx;
166 struct list_head notifier;
167 wait_queue_head_t wait;
168 spinlock_t rx_lock; /* protect single thread state machine */
169 };
170
171 static LIST_HEAD(isotp_notifier_list);
172 static DEFINE_SPINLOCK(isotp_notifier_lock);
173 static struct isotp_sock *isotp_busy_notifier;
174
isotp_sk(const struct sock * sk)175 static inline struct isotp_sock *isotp_sk(const struct sock *sk)
176 {
177 return (struct isotp_sock *)sk;
178 }
179
isotp_bc_flags(struct isotp_sock * so)180 static u32 isotp_bc_flags(struct isotp_sock *so)
181 {
182 return so->opt.flags & ISOTP_ALL_BC_FLAGS;
183 }
184
isotp_register_rxid(struct isotp_sock * so)185 static bool isotp_register_rxid(struct isotp_sock *so)
186 {
187 /* no broadcast modes => register rx_id for FC frame reception */
188 return (isotp_bc_flags(so) == 0);
189 }
190
isotp_rx_timer_handler(struct hrtimer * hrtimer)191 static enum hrtimer_restart isotp_rx_timer_handler(struct hrtimer *hrtimer)
192 {
193 struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
194 rxtimer);
195 struct sock *sk = &so->sk;
196
197 if (so->rx.state == ISOTP_WAIT_DATA) {
198 /* we did not get new data frames in time */
199
200 /* report 'connection timed out' */
201 sk->sk_err = ETIMEDOUT;
202 if (!sock_flag(sk, SOCK_DEAD))
203 sk_error_report(sk);
204
205 /* reset rx state */
206 so->rx.state = ISOTP_IDLE;
207 }
208
209 return HRTIMER_NORESTART;
210 }
211
isotp_send_fc(struct sock * sk,int ae,u8 flowstatus)212 static int isotp_send_fc(struct sock *sk, int ae, u8 flowstatus)
213 {
214 struct net_device *dev;
215 struct sk_buff *nskb;
216 struct canfd_frame *ncf;
217 struct isotp_sock *so = isotp_sk(sk);
218 int can_send_ret;
219
220 nskb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv), gfp_any());
221 if (!nskb)
222 return 1;
223
224 dev = dev_get_by_index(sock_net(sk), so->ifindex);
225 if (!dev) {
226 kfree_skb(nskb);
227 return 1;
228 }
229
230 can_skb_reserve(nskb);
231 can_skb_prv(nskb)->ifindex = dev->ifindex;
232 can_skb_prv(nskb)->skbcnt = 0;
233
234 nskb->dev = dev;
235 can_skb_set_owner(nskb, sk);
236 ncf = (struct canfd_frame *)nskb->data;
237 skb_put_zero(nskb, so->ll.mtu);
238
239 /* create & send flow control reply */
240 ncf->can_id = so->txid;
241
242 if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
243 memset(ncf->data, so->opt.txpad_content, CAN_MAX_DLEN);
244 ncf->len = CAN_MAX_DLEN;
245 } else {
246 ncf->len = ae + FC_CONTENT_SZ;
247 }
248
249 ncf->data[ae] = N_PCI_FC | flowstatus;
250 ncf->data[ae + 1] = so->rxfc.bs;
251 ncf->data[ae + 2] = so->rxfc.stmin;
252
253 if (ae)
254 ncf->data[0] = so->opt.ext_address;
255
256 ncf->flags = so->ll.tx_flags;
257
258 can_send_ret = can_send(nskb, 1);
259 if (can_send_ret)
260 pr_notice_once("can-isotp: %s: can_send_ret %pe\n",
261 __func__, ERR_PTR(can_send_ret));
262
263 dev_put(dev);
264
265 /* reset blocksize counter */
266 so->rx.bs = 0;
267
268 /* reset last CF frame rx timestamp for rx stmin enforcement */
269 so->lastrxcf_tstamp = ktime_set(0, 0);
270
271 /* start rx timeout watchdog */
272 hrtimer_start(&so->rxtimer, ktime_set(ISOTP_FC_TIMEOUT, 0),
273 HRTIMER_MODE_REL_SOFT);
274 return 0;
275 }
276
isotp_rcv_skb(struct sk_buff * skb,struct sock * sk)277 static void isotp_rcv_skb(struct sk_buff *skb, struct sock *sk)
278 {
279 struct sockaddr_can *addr = (struct sockaddr_can *)skb->cb;
280
281 BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
282
283 memset(addr, 0, sizeof(*addr));
284 addr->can_family = AF_CAN;
285 addr->can_ifindex = skb->dev->ifindex;
286
287 if (sock_queue_rcv_skb(sk, skb) < 0)
288 kfree_skb(skb);
289 }
290
padlen(u8 datalen)291 static u8 padlen(u8 datalen)
292 {
293 static const u8 plen[] = {
294 8, 8, 8, 8, 8, 8, 8, 8, 8, /* 0 - 8 */
295 12, 12, 12, 12, /* 9 - 12 */
296 16, 16, 16, 16, /* 13 - 16 */
297 20, 20, 20, 20, /* 17 - 20 */
298 24, 24, 24, 24, /* 21 - 24 */
299 32, 32, 32, 32, 32, 32, 32, 32, /* 25 - 32 */
300 48, 48, 48, 48, 48, 48, 48, 48, /* 33 - 40 */
301 48, 48, 48, 48, 48, 48, 48, 48 /* 41 - 48 */
302 };
303
304 if (datalen > 48)
305 return 64;
306
307 return plen[datalen];
308 }
309
310 /* check for length optimization and return 1/true when the check fails */
check_optimized(struct canfd_frame * cf,int start_index)311 static int check_optimized(struct canfd_frame *cf, int start_index)
312 {
313 /* for CAN_DL <= 8 the start_index is equal to the CAN_DL as the
314 * padding would start at this point. E.g. if the padding would
315 * start at cf.data[7] cf->len has to be 7 to be optimal.
316 * Note: The data[] index starts with zero.
317 */
318 if (cf->len <= CAN_MAX_DLEN)
319 return (cf->len != start_index);
320
321 /* This relation is also valid in the non-linear DLC range, where
322 * we need to take care of the minimal next possible CAN_DL.
323 * The correct check would be (padlen(cf->len) != padlen(start_index)).
324 * But as cf->len can only take discrete values from 12, .., 64 at this
325 * point the padlen(cf->len) is always equal to cf->len.
326 */
327 return (cf->len != padlen(start_index));
328 }
329
330 /* check padding and return 1/true when the check fails */
check_pad(struct isotp_sock * so,struct canfd_frame * cf,int start_index,u8 content)331 static int check_pad(struct isotp_sock *so, struct canfd_frame *cf,
332 int start_index, u8 content)
333 {
334 int i;
335
336 /* no RX_PADDING value => check length of optimized frame length */
337 if (!(so->opt.flags & CAN_ISOTP_RX_PADDING)) {
338 if (so->opt.flags & CAN_ISOTP_CHK_PAD_LEN)
339 return check_optimized(cf, start_index);
340
341 /* no valid test against empty value => ignore frame */
342 return 1;
343 }
344
345 /* check datalength of correctly padded CAN frame */
346 if ((so->opt.flags & CAN_ISOTP_CHK_PAD_LEN) &&
347 cf->len != padlen(cf->len))
348 return 1;
349
350 /* check padding content */
351 if (so->opt.flags & CAN_ISOTP_CHK_PAD_DATA) {
352 for (i = start_index; i < cf->len; i++)
353 if (cf->data[i] != content)
354 return 1;
355 }
356 return 0;
357 }
358
359 static void isotp_send_cframe(struct isotp_sock *so);
360
isotp_rcv_fc(struct isotp_sock * so,struct canfd_frame * cf,int ae)361 static int isotp_rcv_fc(struct isotp_sock *so, struct canfd_frame *cf, int ae)
362 {
363 struct sock *sk = &so->sk;
364
365 if (so->tx.state != ISOTP_WAIT_FC &&
366 so->tx.state != ISOTP_WAIT_FIRST_FC)
367 return 0;
368
369 hrtimer_cancel(&so->txtimer);
370
371 if ((cf->len < ae + FC_CONTENT_SZ) ||
372 ((so->opt.flags & ISOTP_CHECK_PADDING) &&
373 check_pad(so, cf, ae + FC_CONTENT_SZ, so->opt.rxpad_content))) {
374 /* malformed PDU - report 'not a data message' */
375 sk->sk_err = EBADMSG;
376 if (!sock_flag(sk, SOCK_DEAD))
377 sk_error_report(sk);
378
379 so->tx.state = ISOTP_IDLE;
380 wake_up_interruptible(&so->wait);
381 return 1;
382 }
383
384 /* get static/dynamic communication params from first/every FC frame */
385 if (so->tx.state == ISOTP_WAIT_FIRST_FC ||
386 so->opt.flags & CAN_ISOTP_DYN_FC_PARMS) {
387 so->txfc.bs = cf->data[ae + 1];
388 so->txfc.stmin = cf->data[ae + 2];
389
390 /* fix wrong STmin values according spec */
391 if (so->txfc.stmin > 0x7F &&
392 (so->txfc.stmin < 0xF1 || so->txfc.stmin > 0xF9))
393 so->txfc.stmin = 0x7F;
394
395 so->tx_gap = ktime_set(0, 0);
396 /* add transmission time for CAN frame N_As */
397 so->tx_gap = ktime_add_ns(so->tx_gap, so->frame_txtime);
398 /* add waiting time for consecutive frames N_Cs */
399 if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN)
400 so->tx_gap = ktime_add_ns(so->tx_gap,
401 so->force_tx_stmin);
402 else if (so->txfc.stmin < 0x80)
403 so->tx_gap = ktime_add_ns(so->tx_gap,
404 so->txfc.stmin * 1000000);
405 else
406 so->tx_gap = ktime_add_ns(so->tx_gap,
407 (so->txfc.stmin - 0xF0)
408 * 100000);
409 so->tx.state = ISOTP_WAIT_FC;
410 }
411
412 switch (cf->data[ae] & 0x0F) {
413 case ISOTP_FC_CTS:
414 so->tx.bs = 0;
415 so->tx.state = ISOTP_SENDING;
416 /* send CF frame and enable echo timeout handling */
417 hrtimer_start(&so->txtimer, ktime_set(ISOTP_ECHO_TIMEOUT, 0),
418 HRTIMER_MODE_REL_SOFT);
419 isotp_send_cframe(so);
420 break;
421
422 case ISOTP_FC_WT:
423 /* start timer to wait for next FC frame */
424 hrtimer_start(&so->txtimer, ktime_set(ISOTP_FC_TIMEOUT, 0),
425 HRTIMER_MODE_REL_SOFT);
426 break;
427
428 case ISOTP_FC_OVFLW:
429 /* overflow on receiver side - report 'message too long' */
430 sk->sk_err = EMSGSIZE;
431 if (!sock_flag(sk, SOCK_DEAD))
432 sk_error_report(sk);
433 fallthrough;
434
435 default:
436 /* stop this tx job */
437 so->tx.state = ISOTP_IDLE;
438 wake_up_interruptible(&so->wait);
439 }
440 return 0;
441 }
442
isotp_rcv_sf(struct sock * sk,struct canfd_frame * cf,int pcilen,struct sk_buff * skb,int len)443 static int isotp_rcv_sf(struct sock *sk, struct canfd_frame *cf, int pcilen,
444 struct sk_buff *skb, int len)
445 {
446 struct isotp_sock *so = isotp_sk(sk);
447 struct sk_buff *nskb;
448
449 hrtimer_cancel(&so->rxtimer);
450 so->rx.state = ISOTP_IDLE;
451
452 if (!len || len > cf->len - pcilen)
453 return 1;
454
455 if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
456 check_pad(so, cf, pcilen + len, so->opt.rxpad_content)) {
457 /* malformed PDU - report 'not a data message' */
458 sk->sk_err = EBADMSG;
459 if (!sock_flag(sk, SOCK_DEAD))
460 sk_error_report(sk);
461 return 1;
462 }
463
464 nskb = alloc_skb(len, gfp_any());
465 if (!nskb)
466 return 1;
467
468 memcpy(skb_put(nskb, len), &cf->data[pcilen], len);
469
470 nskb->tstamp = skb->tstamp;
471 nskb->dev = skb->dev;
472 isotp_rcv_skb(nskb, sk);
473 return 0;
474 }
475
isotp_rcv_ff(struct sock * sk,struct canfd_frame * cf,int ae)476 static int isotp_rcv_ff(struct sock *sk, struct canfd_frame *cf, int ae)
477 {
478 struct isotp_sock *so = isotp_sk(sk);
479 int i;
480 int off;
481 int ff_pci_sz;
482
483 hrtimer_cancel(&so->rxtimer);
484 so->rx.state = ISOTP_IDLE;
485
486 /* get the used sender LL_DL from the (first) CAN frame data length */
487 so->rx.ll_dl = padlen(cf->len);
488
489 /* the first frame has to use the entire frame up to LL_DL length */
490 if (cf->len != so->rx.ll_dl)
491 return 1;
492
493 /* get the FF_DL */
494 so->rx.len = (cf->data[ae] & 0x0F) << 8;
495 so->rx.len += cf->data[ae + 1];
496
497 /* Check for FF_DL escape sequence supporting 32 bit PDU length */
498 if (so->rx.len) {
499 ff_pci_sz = FF_PCI_SZ12;
500 } else {
501 /* FF_DL = 0 => get real length from next 4 bytes */
502 so->rx.len = cf->data[ae + 2] << 24;
503 so->rx.len += cf->data[ae + 3] << 16;
504 so->rx.len += cf->data[ae + 4] << 8;
505 so->rx.len += cf->data[ae + 5];
506 ff_pci_sz = FF_PCI_SZ32;
507 }
508
509 /* take care of a potential SF_DL ESC offset for TX_DL > 8 */
510 off = (so->rx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
511
512 if (so->rx.len + ae + off + ff_pci_sz < so->rx.ll_dl)
513 return 1;
514
515 /* PDU size > default => try max_pdu_size */
516 if (so->rx.len > so->rx.buflen && so->rx.buflen < max_pdu_size) {
517 u8 *newbuf = kmalloc(max_pdu_size, GFP_ATOMIC);
518
519 if (newbuf) {
520 so->rx.buf = newbuf;
521 so->rx.buflen = max_pdu_size;
522 }
523 }
524
525 if (so->rx.len > so->rx.buflen) {
526 /* send FC frame with overflow status */
527 isotp_send_fc(sk, ae, ISOTP_FC_OVFLW);
528 return 1;
529 }
530
531 /* copy the first received data bytes */
532 so->rx.idx = 0;
533 for (i = ae + ff_pci_sz; i < so->rx.ll_dl; i++)
534 so->rx.buf[so->rx.idx++] = cf->data[i];
535
536 /* initial setup for this pdu reception */
537 so->rx.sn = 1;
538 so->rx.state = ISOTP_WAIT_DATA;
539
540 /* no creation of flow control frames */
541 if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
542 return 0;
543
544 /* send our first FC frame */
545 isotp_send_fc(sk, ae, ISOTP_FC_CTS);
546 return 0;
547 }
548
isotp_rcv_cf(struct sock * sk,struct canfd_frame * cf,int ae,struct sk_buff * skb)549 static int isotp_rcv_cf(struct sock *sk, struct canfd_frame *cf, int ae,
550 struct sk_buff *skb)
551 {
552 struct isotp_sock *so = isotp_sk(sk);
553 struct sk_buff *nskb;
554 int i;
555
556 if (so->rx.state != ISOTP_WAIT_DATA)
557 return 0;
558
559 /* drop if timestamp gap is less than force_rx_stmin nano secs */
560 if (so->opt.flags & CAN_ISOTP_FORCE_RXSTMIN) {
561 if (ktime_to_ns(ktime_sub(skb->tstamp, so->lastrxcf_tstamp)) <
562 so->force_rx_stmin)
563 return 0;
564
565 so->lastrxcf_tstamp = skb->tstamp;
566 }
567
568 hrtimer_cancel(&so->rxtimer);
569
570 /* CFs are never longer than the FF */
571 if (cf->len > so->rx.ll_dl)
572 return 1;
573
574 /* CFs have usually the LL_DL length */
575 if (cf->len < so->rx.ll_dl) {
576 /* this is only allowed for the last CF */
577 if (so->rx.len - so->rx.idx > so->rx.ll_dl - ae - N_PCI_SZ)
578 return 1;
579 }
580
581 if ((cf->data[ae] & 0x0F) != so->rx.sn) {
582 /* wrong sn detected - report 'illegal byte sequence' */
583 sk->sk_err = EILSEQ;
584 if (!sock_flag(sk, SOCK_DEAD))
585 sk_error_report(sk);
586
587 /* reset rx state */
588 so->rx.state = ISOTP_IDLE;
589 return 1;
590 }
591 so->rx.sn++;
592 so->rx.sn %= 16;
593
594 for (i = ae + N_PCI_SZ; i < cf->len; i++) {
595 so->rx.buf[so->rx.idx++] = cf->data[i];
596 if (so->rx.idx >= so->rx.len)
597 break;
598 }
599
600 if (so->rx.idx >= so->rx.len) {
601 /* we are done */
602 so->rx.state = ISOTP_IDLE;
603
604 if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
605 check_pad(so, cf, i + 1, so->opt.rxpad_content)) {
606 /* malformed PDU - report 'not a data message' */
607 sk->sk_err = EBADMSG;
608 if (!sock_flag(sk, SOCK_DEAD))
609 sk_error_report(sk);
610 return 1;
611 }
612
613 nskb = alloc_skb(so->rx.len, gfp_any());
614 if (!nskb)
615 return 1;
616
617 memcpy(skb_put(nskb, so->rx.len), so->rx.buf,
618 so->rx.len);
619
620 nskb->tstamp = skb->tstamp;
621 nskb->dev = skb->dev;
622 isotp_rcv_skb(nskb, sk);
623 return 0;
624 }
625
626 /* perform blocksize handling, if enabled */
627 if (!so->rxfc.bs || ++so->rx.bs < so->rxfc.bs) {
628 /* start rx timeout watchdog */
629 hrtimer_start(&so->rxtimer, ktime_set(ISOTP_FC_TIMEOUT, 0),
630 HRTIMER_MODE_REL_SOFT);
631 return 0;
632 }
633
634 /* no creation of flow control frames */
635 if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
636 return 0;
637
638 /* we reached the specified blocksize so->rxfc.bs */
639 isotp_send_fc(sk, ae, ISOTP_FC_CTS);
640 return 0;
641 }
642
isotp_rcv(struct sk_buff * skb,void * data)643 static void isotp_rcv(struct sk_buff *skb, void *data)
644 {
645 struct sock *sk = (struct sock *)data;
646 struct isotp_sock *so = isotp_sk(sk);
647 struct canfd_frame *cf;
648 int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
649 u8 n_pci_type, sf_dl;
650
651 /* Strictly receive only frames with the configured MTU size
652 * => clear separation of CAN2.0 / CAN FD transport channels
653 */
654 if (skb->len != so->ll.mtu)
655 return;
656
657 cf = (struct canfd_frame *)skb->data;
658
659 /* if enabled: check reception of my configured extended address */
660 if (ae && cf->data[0] != so->opt.rx_ext_address)
661 return;
662
663 n_pci_type = cf->data[ae] & 0xF0;
664
665 /* Make sure the state changes and data structures stay consistent at
666 * CAN frame reception time. This locking is not needed in real world
667 * use cases but the inconsistency can be triggered with syzkaller.
668 */
669 spin_lock(&so->rx_lock);
670
671 if (so->opt.flags & CAN_ISOTP_HALF_DUPLEX) {
672 /* check rx/tx path half duplex expectations */
673 if ((so->tx.state != ISOTP_IDLE && n_pci_type != N_PCI_FC) ||
674 (so->rx.state != ISOTP_IDLE && n_pci_type == N_PCI_FC))
675 goto out_unlock;
676 }
677
678 switch (n_pci_type) {
679 case N_PCI_FC:
680 /* tx path: flow control frame containing the FC parameters */
681 isotp_rcv_fc(so, cf, ae);
682 break;
683
684 case N_PCI_SF:
685 /* rx path: single frame
686 *
687 * As we do not have a rx.ll_dl configuration, we can only test
688 * if the CAN frames payload length matches the LL_DL == 8
689 * requirements - no matter if it's CAN 2.0 or CAN FD
690 */
691
692 /* get the SF_DL from the N_PCI byte */
693 sf_dl = cf->data[ae] & 0x0F;
694
695 if (cf->len <= CAN_MAX_DLEN) {
696 isotp_rcv_sf(sk, cf, SF_PCI_SZ4 + ae, skb, sf_dl);
697 } else {
698 if (can_is_canfd_skb(skb)) {
699 /* We have a CAN FD frame and CAN_DL is greater than 8:
700 * Only frames with the SF_DL == 0 ESC value are valid.
701 *
702 * If so take care of the increased SF PCI size
703 * (SF_PCI_SZ8) to point to the message content behind
704 * the extended SF PCI info and get the real SF_DL
705 * length value from the formerly first data byte.
706 */
707 if (sf_dl == 0)
708 isotp_rcv_sf(sk, cf, SF_PCI_SZ8 + ae, skb,
709 cf->data[SF_PCI_SZ4 + ae]);
710 }
711 }
712 break;
713
714 case N_PCI_FF:
715 /* rx path: first frame */
716 isotp_rcv_ff(sk, cf, ae);
717 break;
718
719 case N_PCI_CF:
720 /* rx path: consecutive frame */
721 isotp_rcv_cf(sk, cf, ae, skb);
722 break;
723 }
724
725 out_unlock:
726 spin_unlock(&so->rx_lock);
727 }
728
isotp_fill_dataframe(struct canfd_frame * cf,struct isotp_sock * so,int ae,int off)729 static void isotp_fill_dataframe(struct canfd_frame *cf, struct isotp_sock *so,
730 int ae, int off)
731 {
732 int pcilen = N_PCI_SZ + ae + off;
733 int space = so->tx.ll_dl - pcilen;
734 int num = min_t(int, so->tx.len - so->tx.idx, space);
735 int i;
736
737 cf->can_id = so->txid;
738 cf->len = num + pcilen;
739
740 if (num < space) {
741 if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
742 /* user requested padding */
743 cf->len = padlen(cf->len);
744 memset(cf->data, so->opt.txpad_content, cf->len);
745 } else if (cf->len > CAN_MAX_DLEN) {
746 /* mandatory padding for CAN FD frames */
747 cf->len = padlen(cf->len);
748 memset(cf->data, CAN_ISOTP_DEFAULT_PAD_CONTENT,
749 cf->len);
750 }
751 }
752
753 for (i = 0; i < num; i++)
754 cf->data[pcilen + i] = so->tx.buf[so->tx.idx++];
755
756 if (ae)
757 cf->data[0] = so->opt.ext_address;
758 }
759
isotp_send_cframe(struct isotp_sock * so)760 static void isotp_send_cframe(struct isotp_sock *so)
761 {
762 struct sock *sk = &so->sk;
763 struct sk_buff *skb;
764 struct net_device *dev;
765 struct canfd_frame *cf;
766 int can_send_ret;
767 int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
768
769 dev = dev_get_by_index(sock_net(sk), so->ifindex);
770 if (!dev)
771 return;
772
773 skb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv), GFP_ATOMIC);
774 if (!skb) {
775 dev_put(dev);
776 return;
777 }
778
779 can_skb_reserve(skb);
780 can_skb_prv(skb)->ifindex = dev->ifindex;
781 can_skb_prv(skb)->skbcnt = 0;
782
783 cf = (struct canfd_frame *)skb->data;
784 skb_put_zero(skb, so->ll.mtu);
785
786 /* create consecutive frame */
787 isotp_fill_dataframe(cf, so, ae, 0);
788
789 /* place consecutive frame N_PCI in appropriate index */
790 cf->data[ae] = N_PCI_CF | so->tx.sn++;
791 so->tx.sn %= 16;
792 so->tx.bs++;
793
794 cf->flags = so->ll.tx_flags;
795
796 skb->dev = dev;
797 can_skb_set_owner(skb, sk);
798
799 /* cfecho should have been zero'ed by init/isotp_rcv_echo() */
800 if (so->cfecho)
801 pr_notice_once("can-isotp: cfecho is %08X != 0\n", so->cfecho);
802
803 /* set consecutive frame echo tag */
804 so->cfecho = *(u32 *)cf->data;
805
806 /* send frame with local echo enabled */
807 can_send_ret = can_send(skb, 1);
808 if (can_send_ret) {
809 pr_notice_once("can-isotp: %s: can_send_ret %pe\n",
810 __func__, ERR_PTR(can_send_ret));
811 if (can_send_ret == -ENOBUFS)
812 pr_notice_once("can-isotp: tx queue is full\n");
813 }
814 dev_put(dev);
815 }
816
isotp_create_fframe(struct canfd_frame * cf,struct isotp_sock * so,int ae)817 static void isotp_create_fframe(struct canfd_frame *cf, struct isotp_sock *so,
818 int ae)
819 {
820 int i;
821 int ff_pci_sz;
822
823 cf->can_id = so->txid;
824 cf->len = so->tx.ll_dl;
825 if (ae)
826 cf->data[0] = so->opt.ext_address;
827
828 /* create N_PCI bytes with 12/32 bit FF_DL data length */
829 if (so->tx.len > MAX_12BIT_PDU_SIZE) {
830 /* use 32 bit FF_DL notation */
831 cf->data[ae] = N_PCI_FF;
832 cf->data[ae + 1] = 0;
833 cf->data[ae + 2] = (u8)(so->tx.len >> 24) & 0xFFU;
834 cf->data[ae + 3] = (u8)(so->tx.len >> 16) & 0xFFU;
835 cf->data[ae + 4] = (u8)(so->tx.len >> 8) & 0xFFU;
836 cf->data[ae + 5] = (u8)so->tx.len & 0xFFU;
837 ff_pci_sz = FF_PCI_SZ32;
838 } else {
839 /* use 12 bit FF_DL notation */
840 cf->data[ae] = (u8)(so->tx.len >> 8) | N_PCI_FF;
841 cf->data[ae + 1] = (u8)so->tx.len & 0xFFU;
842 ff_pci_sz = FF_PCI_SZ12;
843 }
844
845 /* add first data bytes depending on ae */
846 for (i = ae + ff_pci_sz; i < so->tx.ll_dl; i++)
847 cf->data[i] = so->tx.buf[so->tx.idx++];
848
849 so->tx.sn = 1;
850 }
851
isotp_rcv_echo(struct sk_buff * skb,void * data)852 static void isotp_rcv_echo(struct sk_buff *skb, void *data)
853 {
854 struct sock *sk = (struct sock *)data;
855 struct isotp_sock *so = isotp_sk(sk);
856 struct canfd_frame *cf = (struct canfd_frame *)skb->data;
857
858 /* only handle my own local echo CF/SF skb's (no FF!) */
859 if (skb->sk != sk || so->cfecho != *(u32 *)cf->data)
860 return;
861
862 /* cancel local echo timeout */
863 hrtimer_cancel(&so->txtimer);
864
865 /* local echo skb with consecutive frame has been consumed */
866 so->cfecho = 0;
867
868 if (so->tx.idx >= so->tx.len) {
869 /* we are done */
870 so->tx.state = ISOTP_IDLE;
871 wake_up_interruptible(&so->wait);
872 return;
873 }
874
875 if (so->txfc.bs && so->tx.bs >= so->txfc.bs) {
876 /* stop and wait for FC with timeout */
877 so->tx.state = ISOTP_WAIT_FC;
878 hrtimer_start(&so->txtimer, ktime_set(ISOTP_FC_TIMEOUT, 0),
879 HRTIMER_MODE_REL_SOFT);
880 return;
881 }
882
883 /* no gap between data frames needed => use burst mode */
884 if (!so->tx_gap) {
885 /* enable echo timeout handling */
886 hrtimer_start(&so->txtimer, ktime_set(ISOTP_ECHO_TIMEOUT, 0),
887 HRTIMER_MODE_REL_SOFT);
888 isotp_send_cframe(so);
889 return;
890 }
891
892 /* start timer to send next consecutive frame with correct delay */
893 hrtimer_start(&so->txfrtimer, so->tx_gap, HRTIMER_MODE_REL_SOFT);
894 }
895
isotp_tx_timer_handler(struct hrtimer * hrtimer)896 static enum hrtimer_restart isotp_tx_timer_handler(struct hrtimer *hrtimer)
897 {
898 struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
899 txtimer);
900 struct sock *sk = &so->sk;
901
902 /* don't handle timeouts in IDLE or SHUTDOWN state */
903 if (so->tx.state == ISOTP_IDLE || so->tx.state == ISOTP_SHUTDOWN)
904 return HRTIMER_NORESTART;
905
906 /* we did not get any flow control or echo frame in time */
907
908 /* report 'communication error on send' */
909 sk->sk_err = ECOMM;
910 if (!sock_flag(sk, SOCK_DEAD))
911 sk_error_report(sk);
912
913 /* reset tx state */
914 so->tx.state = ISOTP_IDLE;
915 wake_up_interruptible(&so->wait);
916
917 return HRTIMER_NORESTART;
918 }
919
isotp_txfr_timer_handler(struct hrtimer * hrtimer)920 static enum hrtimer_restart isotp_txfr_timer_handler(struct hrtimer *hrtimer)
921 {
922 struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
923 txfrtimer);
924
925 /* start echo timeout handling and cover below protocol error */
926 hrtimer_start(&so->txtimer, ktime_set(ISOTP_ECHO_TIMEOUT, 0),
927 HRTIMER_MODE_REL_SOFT);
928
929 /* cfecho should be consumed by isotp_rcv_echo() here */
930 if (so->tx.state == ISOTP_SENDING && !so->cfecho)
931 isotp_send_cframe(so);
932
933 return HRTIMER_NORESTART;
934 }
935
isotp_sendmsg(struct socket * sock,struct msghdr * msg,size_t size)936 static int isotp_sendmsg(struct socket *sock, struct msghdr *msg, size_t size)
937 {
938 struct sock *sk = sock->sk;
939 struct isotp_sock *so = isotp_sk(sk);
940 struct sk_buff *skb;
941 struct net_device *dev;
942 struct canfd_frame *cf;
943 int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
944 int wait_tx_done = (so->opt.flags & CAN_ISOTP_WAIT_TX_DONE) ? 1 : 0;
945 s64 hrtimer_sec = ISOTP_ECHO_TIMEOUT;
946 int off;
947 int err;
948
949 if (!so->bound || so->tx.state == ISOTP_SHUTDOWN)
950 return -EADDRNOTAVAIL;
951
952 while (cmpxchg(&so->tx.state, ISOTP_IDLE, ISOTP_SENDING) != ISOTP_IDLE) {
953 /* we do not support multiple buffers - for now */
954 if (msg->msg_flags & MSG_DONTWAIT)
955 return -EAGAIN;
956
957 if (so->tx.state == ISOTP_SHUTDOWN)
958 return -EADDRNOTAVAIL;
959
960 /* wait for complete transmission of current pdu */
961 err = wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
962 if (err)
963 goto err_event_drop;
964 }
965
966 /* PDU size > default => try max_pdu_size */
967 if (size > so->tx.buflen && so->tx.buflen < max_pdu_size) {
968 u8 *newbuf = kmalloc(max_pdu_size, GFP_KERNEL);
969
970 if (newbuf) {
971 so->tx.buf = newbuf;
972 so->tx.buflen = max_pdu_size;
973 }
974 }
975
976 if (!size || size > so->tx.buflen) {
977 err = -EINVAL;
978 goto err_out_drop;
979 }
980
981 /* take care of a potential SF_DL ESC offset for TX_DL > 8 */
982 off = (so->tx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
983
984 /* does the given data fit into a single frame for SF_BROADCAST? */
985 if ((isotp_bc_flags(so) == CAN_ISOTP_SF_BROADCAST) &&
986 (size > so->tx.ll_dl - SF_PCI_SZ4 - ae - off)) {
987 err = -EINVAL;
988 goto err_out_drop;
989 }
990
991 err = memcpy_from_msg(so->tx.buf, msg, size);
992 if (err < 0)
993 goto err_out_drop;
994
995 dev = dev_get_by_index(sock_net(sk), so->ifindex);
996 if (!dev) {
997 err = -ENXIO;
998 goto err_out_drop;
999 }
1000
1001 skb = sock_alloc_send_skb(sk, so->ll.mtu + sizeof(struct can_skb_priv),
1002 msg->msg_flags & MSG_DONTWAIT, &err);
1003 if (!skb) {
1004 dev_put(dev);
1005 goto err_out_drop;
1006 }
1007
1008 can_skb_reserve(skb);
1009 can_skb_prv(skb)->ifindex = dev->ifindex;
1010 can_skb_prv(skb)->skbcnt = 0;
1011
1012 so->tx.len = size;
1013 so->tx.idx = 0;
1014
1015 cf = (struct canfd_frame *)skb->data;
1016 skb_put_zero(skb, so->ll.mtu);
1017
1018 /* cfecho should have been zero'ed by init / former isotp_rcv_echo() */
1019 if (so->cfecho)
1020 pr_notice_once("can-isotp: uninit cfecho %08X\n", so->cfecho);
1021
1022 /* check for single frame transmission depending on TX_DL */
1023 if (size <= so->tx.ll_dl - SF_PCI_SZ4 - ae - off) {
1024 /* The message size generally fits into a SingleFrame - good.
1025 *
1026 * SF_DL ESC offset optimization:
1027 *
1028 * When TX_DL is greater 8 but the message would still fit
1029 * into a 8 byte CAN frame, we can omit the offset.
1030 * This prevents a protocol caused length extension from
1031 * CAN_DL = 8 to CAN_DL = 12 due to the SF_SL ESC handling.
1032 */
1033 if (size <= CAN_MAX_DLEN - SF_PCI_SZ4 - ae)
1034 off = 0;
1035
1036 isotp_fill_dataframe(cf, so, ae, off);
1037
1038 /* place single frame N_PCI w/o length in appropriate index */
1039 cf->data[ae] = N_PCI_SF;
1040
1041 /* place SF_DL size value depending on the SF_DL ESC offset */
1042 if (off)
1043 cf->data[SF_PCI_SZ4 + ae] = size;
1044 else
1045 cf->data[ae] |= size;
1046
1047 /* set CF echo tag for isotp_rcv_echo() (SF-mode) */
1048 so->cfecho = *(u32 *)cf->data;
1049 } else {
1050 /* send first frame */
1051
1052 isotp_create_fframe(cf, so, ae);
1053
1054 if (isotp_bc_flags(so) == CAN_ISOTP_CF_BROADCAST) {
1055 /* set timer for FC-less operation (STmin = 0) */
1056 if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN)
1057 so->tx_gap = ktime_set(0, so->force_tx_stmin);
1058 else
1059 so->tx_gap = ktime_set(0, so->frame_txtime);
1060
1061 /* disable wait for FCs due to activated block size */
1062 so->txfc.bs = 0;
1063
1064 /* set CF echo tag for isotp_rcv_echo() (CF-mode) */
1065 so->cfecho = *(u32 *)cf->data;
1066 } else {
1067 /* standard flow control check */
1068 so->tx.state = ISOTP_WAIT_FIRST_FC;
1069
1070 /* start timeout for FC */
1071 hrtimer_sec = ISOTP_FC_TIMEOUT;
1072
1073 /* no CF echo tag for isotp_rcv_echo() (FF-mode) */
1074 so->cfecho = 0;
1075 }
1076 }
1077
1078 hrtimer_start(&so->txtimer, ktime_set(hrtimer_sec, 0),
1079 HRTIMER_MODE_REL_SOFT);
1080
1081 /* send the first or only CAN frame */
1082 cf->flags = so->ll.tx_flags;
1083
1084 skb->dev = dev;
1085 skb->sk = sk;
1086 err = can_send(skb, 1);
1087 dev_put(dev);
1088 if (err) {
1089 pr_notice_once("can-isotp: %s: can_send_ret %pe\n",
1090 __func__, ERR_PTR(err));
1091
1092 /* no transmission -> no timeout monitoring */
1093 hrtimer_cancel(&so->txtimer);
1094
1095 /* reset consecutive frame echo tag */
1096 so->cfecho = 0;
1097
1098 goto err_out_drop;
1099 }
1100
1101 if (wait_tx_done) {
1102 /* wait for complete transmission of current pdu */
1103 err = wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
1104 if (err)
1105 goto err_event_drop;
1106
1107 err = sock_error(sk);
1108 if (err)
1109 return err;
1110 }
1111
1112 return size;
1113
1114 err_event_drop:
1115 /* got signal: force tx state machine to be idle */
1116 so->tx.state = ISOTP_IDLE;
1117 hrtimer_cancel(&so->txfrtimer);
1118 hrtimer_cancel(&so->txtimer);
1119 err_out_drop:
1120 /* drop this PDU and unlock a potential wait queue */
1121 so->tx.state = ISOTP_IDLE;
1122 wake_up_interruptible(&so->wait);
1123
1124 return err;
1125 }
1126
isotp_recvmsg(struct socket * sock,struct msghdr * msg,size_t size,int flags)1127 static int isotp_recvmsg(struct socket *sock, struct msghdr *msg, size_t size,
1128 int flags)
1129 {
1130 struct sock *sk = sock->sk;
1131 struct sk_buff *skb;
1132 struct isotp_sock *so = isotp_sk(sk);
1133 int ret = 0;
1134
1135 if (flags & ~(MSG_DONTWAIT | MSG_TRUNC | MSG_PEEK | MSG_CMSG_COMPAT))
1136 return -EINVAL;
1137
1138 if (!so->bound)
1139 return -EADDRNOTAVAIL;
1140
1141 skb = skb_recv_datagram(sk, flags, &ret);
1142 if (!skb)
1143 return ret;
1144
1145 if (size < skb->len)
1146 msg->msg_flags |= MSG_TRUNC;
1147 else
1148 size = skb->len;
1149
1150 ret = memcpy_to_msg(msg, skb->data, size);
1151 if (ret < 0)
1152 goto out_err;
1153
1154 sock_recv_cmsgs(msg, sk, skb);
1155
1156 if (msg->msg_name) {
1157 __sockaddr_check_size(ISOTP_MIN_NAMELEN);
1158 msg->msg_namelen = ISOTP_MIN_NAMELEN;
1159 memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
1160 }
1161
1162 /* set length of return value */
1163 ret = (flags & MSG_TRUNC) ? skb->len : size;
1164
1165 out_err:
1166 skb_free_datagram(sk, skb);
1167
1168 return ret;
1169 }
1170
isotp_release(struct socket * sock)1171 static int isotp_release(struct socket *sock)
1172 {
1173 struct sock *sk = sock->sk;
1174 struct isotp_sock *so;
1175 struct net *net;
1176
1177 if (!sk)
1178 return 0;
1179
1180 so = isotp_sk(sk);
1181 net = sock_net(sk);
1182
1183 /* wait for complete transmission of current pdu */
1184 while (wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE) == 0 &&
1185 cmpxchg(&so->tx.state, ISOTP_IDLE, ISOTP_SHUTDOWN) != ISOTP_IDLE)
1186 ;
1187
1188 /* force state machines to be idle also when a signal occurred */
1189 so->tx.state = ISOTP_SHUTDOWN;
1190 so->rx.state = ISOTP_IDLE;
1191
1192 spin_lock(&isotp_notifier_lock);
1193 while (isotp_busy_notifier == so) {
1194 spin_unlock(&isotp_notifier_lock);
1195 schedule_timeout_uninterruptible(1);
1196 spin_lock(&isotp_notifier_lock);
1197 }
1198 list_del(&so->notifier);
1199 spin_unlock(&isotp_notifier_lock);
1200
1201 lock_sock(sk);
1202
1203 /* remove current filters & unregister */
1204 if (so->bound) {
1205 if (so->ifindex) {
1206 struct net_device *dev;
1207
1208 dev = dev_get_by_index(net, so->ifindex);
1209 if (dev) {
1210 if (isotp_register_rxid(so))
1211 can_rx_unregister(net, dev, so->rxid,
1212 SINGLE_MASK(so->rxid),
1213 isotp_rcv, sk);
1214
1215 can_rx_unregister(net, dev, so->txid,
1216 SINGLE_MASK(so->txid),
1217 isotp_rcv_echo, sk);
1218 dev_put(dev);
1219 synchronize_rcu();
1220 }
1221 }
1222 }
1223
1224 hrtimer_cancel(&so->txfrtimer);
1225 hrtimer_cancel(&so->txtimer);
1226 hrtimer_cancel(&so->rxtimer);
1227
1228 so->ifindex = 0;
1229 so->bound = 0;
1230
1231 if (so->rx.buf != so->rx.sbuf)
1232 kfree(so->rx.buf);
1233
1234 if (so->tx.buf != so->tx.sbuf)
1235 kfree(so->tx.buf);
1236
1237 sock_orphan(sk);
1238 sock->sk = NULL;
1239
1240 release_sock(sk);
1241 sock_put(sk);
1242
1243 return 0;
1244 }
1245
isotp_bind(struct socket * sock,struct sockaddr * uaddr,int len)1246 static int isotp_bind(struct socket *sock, struct sockaddr *uaddr, int len)
1247 {
1248 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
1249 struct sock *sk = sock->sk;
1250 struct isotp_sock *so = isotp_sk(sk);
1251 struct net *net = sock_net(sk);
1252 int ifindex;
1253 struct net_device *dev;
1254 canid_t tx_id = addr->can_addr.tp.tx_id;
1255 canid_t rx_id = addr->can_addr.tp.rx_id;
1256 int err = 0;
1257 int notify_enetdown = 0;
1258
1259 if (len < ISOTP_MIN_NAMELEN)
1260 return -EINVAL;
1261
1262 if (addr->can_family != AF_CAN)
1263 return -EINVAL;
1264
1265 /* sanitize tx CAN identifier */
1266 if (tx_id & CAN_EFF_FLAG)
1267 tx_id &= (CAN_EFF_FLAG | CAN_EFF_MASK);
1268 else
1269 tx_id &= CAN_SFF_MASK;
1270
1271 /* give feedback on wrong CAN-ID value */
1272 if (tx_id != addr->can_addr.tp.tx_id)
1273 return -EINVAL;
1274
1275 /* sanitize rx CAN identifier (if needed) */
1276 if (isotp_register_rxid(so)) {
1277 if (rx_id & CAN_EFF_FLAG)
1278 rx_id &= (CAN_EFF_FLAG | CAN_EFF_MASK);
1279 else
1280 rx_id &= CAN_SFF_MASK;
1281
1282 /* give feedback on wrong CAN-ID value */
1283 if (rx_id != addr->can_addr.tp.rx_id)
1284 return -EINVAL;
1285 }
1286
1287 if (!addr->can_ifindex)
1288 return -ENODEV;
1289
1290 lock_sock(sk);
1291
1292 if (so->bound) {
1293 err = -EINVAL;
1294 goto out;
1295 }
1296
1297 /* ensure different CAN IDs when the rx_id is to be registered */
1298 if (isotp_register_rxid(so) && rx_id == tx_id) {
1299 err = -EADDRNOTAVAIL;
1300 goto out;
1301 }
1302
1303 dev = dev_get_by_index(net, addr->can_ifindex);
1304 if (!dev) {
1305 err = -ENODEV;
1306 goto out;
1307 }
1308 if (dev->type != ARPHRD_CAN) {
1309 dev_put(dev);
1310 err = -ENODEV;
1311 goto out;
1312 }
1313 if (dev->mtu < so->ll.mtu) {
1314 dev_put(dev);
1315 err = -EINVAL;
1316 goto out;
1317 }
1318 if (!(dev->flags & IFF_UP))
1319 notify_enetdown = 1;
1320
1321 ifindex = dev->ifindex;
1322
1323 if (isotp_register_rxid(so))
1324 can_rx_register(net, dev, rx_id, SINGLE_MASK(rx_id),
1325 isotp_rcv, sk, "isotp", sk);
1326
1327 /* no consecutive frame echo skb in flight */
1328 so->cfecho = 0;
1329
1330 /* register for echo skb's */
1331 can_rx_register(net, dev, tx_id, SINGLE_MASK(tx_id),
1332 isotp_rcv_echo, sk, "isotpe", sk);
1333
1334 dev_put(dev);
1335
1336 /* switch to new settings */
1337 so->ifindex = ifindex;
1338 so->rxid = rx_id;
1339 so->txid = tx_id;
1340 so->bound = 1;
1341
1342 out:
1343 release_sock(sk);
1344
1345 if (notify_enetdown) {
1346 sk->sk_err = ENETDOWN;
1347 if (!sock_flag(sk, SOCK_DEAD))
1348 sk_error_report(sk);
1349 }
1350
1351 return err;
1352 }
1353
isotp_getname(struct socket * sock,struct sockaddr * uaddr,int peer)1354 static int isotp_getname(struct socket *sock, struct sockaddr *uaddr, int peer)
1355 {
1356 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
1357 struct sock *sk = sock->sk;
1358 struct isotp_sock *so = isotp_sk(sk);
1359
1360 if (peer)
1361 return -EOPNOTSUPP;
1362
1363 memset(addr, 0, ISOTP_MIN_NAMELEN);
1364 addr->can_family = AF_CAN;
1365 addr->can_ifindex = so->ifindex;
1366 addr->can_addr.tp.rx_id = so->rxid;
1367 addr->can_addr.tp.tx_id = so->txid;
1368
1369 return ISOTP_MIN_NAMELEN;
1370 }
1371
isotp_setsockopt_locked(struct socket * sock,int level,int optname,sockptr_t optval,unsigned int optlen)1372 static int isotp_setsockopt_locked(struct socket *sock, int level, int optname,
1373 sockptr_t optval, unsigned int optlen)
1374 {
1375 struct sock *sk = sock->sk;
1376 struct isotp_sock *so = isotp_sk(sk);
1377 int ret = 0;
1378
1379 if (so->bound)
1380 return -EISCONN;
1381
1382 switch (optname) {
1383 case CAN_ISOTP_OPTS:
1384 if (optlen != sizeof(struct can_isotp_options))
1385 return -EINVAL;
1386
1387 if (copy_from_sockptr(&so->opt, optval, optlen))
1388 return -EFAULT;
1389
1390 /* no separate rx_ext_address is given => use ext_address */
1391 if (!(so->opt.flags & CAN_ISOTP_RX_EXT_ADDR))
1392 so->opt.rx_ext_address = so->opt.ext_address;
1393
1394 /* these broadcast flags are not allowed together */
1395 if (isotp_bc_flags(so) == ISOTP_ALL_BC_FLAGS) {
1396 /* CAN_ISOTP_SF_BROADCAST is prioritized */
1397 so->opt.flags &= ~CAN_ISOTP_CF_BROADCAST;
1398
1399 /* give user feedback on wrong config attempt */
1400 ret = -EINVAL;
1401 }
1402
1403 /* check for frame_txtime changes (0 => no changes) */
1404 if (so->opt.frame_txtime) {
1405 if (so->opt.frame_txtime == CAN_ISOTP_FRAME_TXTIME_ZERO)
1406 so->frame_txtime = 0;
1407 else
1408 so->frame_txtime = so->opt.frame_txtime;
1409 }
1410 break;
1411
1412 case CAN_ISOTP_RECV_FC:
1413 if (optlen != sizeof(struct can_isotp_fc_options))
1414 return -EINVAL;
1415
1416 if (copy_from_sockptr(&so->rxfc, optval, optlen))
1417 return -EFAULT;
1418 break;
1419
1420 case CAN_ISOTP_TX_STMIN:
1421 if (optlen != sizeof(u32))
1422 return -EINVAL;
1423
1424 if (copy_from_sockptr(&so->force_tx_stmin, optval, optlen))
1425 return -EFAULT;
1426 break;
1427
1428 case CAN_ISOTP_RX_STMIN:
1429 if (optlen != sizeof(u32))
1430 return -EINVAL;
1431
1432 if (copy_from_sockptr(&so->force_rx_stmin, optval, optlen))
1433 return -EFAULT;
1434 break;
1435
1436 case CAN_ISOTP_LL_OPTS:
1437 if (optlen == sizeof(struct can_isotp_ll_options)) {
1438 struct can_isotp_ll_options ll;
1439
1440 if (copy_from_sockptr(&ll, optval, optlen))
1441 return -EFAULT;
1442
1443 /* check for correct ISO 11898-1 DLC data length */
1444 if (ll.tx_dl != padlen(ll.tx_dl))
1445 return -EINVAL;
1446
1447 if (ll.mtu != CAN_MTU && ll.mtu != CANFD_MTU)
1448 return -EINVAL;
1449
1450 if (ll.mtu == CAN_MTU &&
1451 (ll.tx_dl > CAN_MAX_DLEN || ll.tx_flags != 0))
1452 return -EINVAL;
1453
1454 memcpy(&so->ll, &ll, sizeof(ll));
1455
1456 /* set ll_dl for tx path to similar place as for rx */
1457 so->tx.ll_dl = ll.tx_dl;
1458 } else {
1459 return -EINVAL;
1460 }
1461 break;
1462
1463 default:
1464 ret = -ENOPROTOOPT;
1465 }
1466
1467 return ret;
1468 }
1469
isotp_setsockopt(struct socket * sock,int level,int optname,sockptr_t optval,unsigned int optlen)1470 static int isotp_setsockopt(struct socket *sock, int level, int optname,
1471 sockptr_t optval, unsigned int optlen)
1472
1473 {
1474 struct sock *sk = sock->sk;
1475 int ret;
1476
1477 if (level != SOL_CAN_ISOTP)
1478 return -EINVAL;
1479
1480 lock_sock(sk);
1481 ret = isotp_setsockopt_locked(sock, level, optname, optval, optlen);
1482 release_sock(sk);
1483 return ret;
1484 }
1485
isotp_getsockopt(struct socket * sock,int level,int optname,char __user * optval,int __user * optlen)1486 static int isotp_getsockopt(struct socket *sock, int level, int optname,
1487 char __user *optval, int __user *optlen)
1488 {
1489 struct sock *sk = sock->sk;
1490 struct isotp_sock *so = isotp_sk(sk);
1491 int len;
1492 void *val;
1493
1494 if (level != SOL_CAN_ISOTP)
1495 return -EINVAL;
1496 if (get_user(len, optlen))
1497 return -EFAULT;
1498 if (len < 0)
1499 return -EINVAL;
1500
1501 switch (optname) {
1502 case CAN_ISOTP_OPTS:
1503 len = min_t(int, len, sizeof(struct can_isotp_options));
1504 val = &so->opt;
1505 break;
1506
1507 case CAN_ISOTP_RECV_FC:
1508 len = min_t(int, len, sizeof(struct can_isotp_fc_options));
1509 val = &so->rxfc;
1510 break;
1511
1512 case CAN_ISOTP_TX_STMIN:
1513 len = min_t(int, len, sizeof(u32));
1514 val = &so->force_tx_stmin;
1515 break;
1516
1517 case CAN_ISOTP_RX_STMIN:
1518 len = min_t(int, len, sizeof(u32));
1519 val = &so->force_rx_stmin;
1520 break;
1521
1522 case CAN_ISOTP_LL_OPTS:
1523 len = min_t(int, len, sizeof(struct can_isotp_ll_options));
1524 val = &so->ll;
1525 break;
1526
1527 default:
1528 return -ENOPROTOOPT;
1529 }
1530
1531 if (put_user(len, optlen))
1532 return -EFAULT;
1533 if (copy_to_user(optval, val, len))
1534 return -EFAULT;
1535 return 0;
1536 }
1537
isotp_notify(struct isotp_sock * so,unsigned long msg,struct net_device * dev)1538 static void isotp_notify(struct isotp_sock *so, unsigned long msg,
1539 struct net_device *dev)
1540 {
1541 struct sock *sk = &so->sk;
1542
1543 if (!net_eq(dev_net(dev), sock_net(sk)))
1544 return;
1545
1546 if (so->ifindex != dev->ifindex)
1547 return;
1548
1549 switch (msg) {
1550 case NETDEV_UNREGISTER:
1551 lock_sock(sk);
1552 /* remove current filters & unregister */
1553 if (so->bound) {
1554 if (isotp_register_rxid(so))
1555 can_rx_unregister(dev_net(dev), dev, so->rxid,
1556 SINGLE_MASK(so->rxid),
1557 isotp_rcv, sk);
1558
1559 can_rx_unregister(dev_net(dev), dev, so->txid,
1560 SINGLE_MASK(so->txid),
1561 isotp_rcv_echo, sk);
1562 }
1563
1564 so->ifindex = 0;
1565 so->bound = 0;
1566 release_sock(sk);
1567
1568 sk->sk_err = ENODEV;
1569 if (!sock_flag(sk, SOCK_DEAD))
1570 sk_error_report(sk);
1571 break;
1572
1573 case NETDEV_DOWN:
1574 sk->sk_err = ENETDOWN;
1575 if (!sock_flag(sk, SOCK_DEAD))
1576 sk_error_report(sk);
1577 break;
1578 }
1579 }
1580
isotp_notifier(struct notifier_block * nb,unsigned long msg,void * ptr)1581 static int isotp_notifier(struct notifier_block *nb, unsigned long msg,
1582 void *ptr)
1583 {
1584 struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1585
1586 if (dev->type != ARPHRD_CAN)
1587 return NOTIFY_DONE;
1588 if (msg != NETDEV_UNREGISTER && msg != NETDEV_DOWN)
1589 return NOTIFY_DONE;
1590 if (unlikely(isotp_busy_notifier)) /* Check for reentrant bug. */
1591 return NOTIFY_DONE;
1592
1593 spin_lock(&isotp_notifier_lock);
1594 list_for_each_entry(isotp_busy_notifier, &isotp_notifier_list, notifier) {
1595 spin_unlock(&isotp_notifier_lock);
1596 isotp_notify(isotp_busy_notifier, msg, dev);
1597 spin_lock(&isotp_notifier_lock);
1598 }
1599 isotp_busy_notifier = NULL;
1600 spin_unlock(&isotp_notifier_lock);
1601 return NOTIFY_DONE;
1602 }
1603
isotp_init(struct sock * sk)1604 static int isotp_init(struct sock *sk)
1605 {
1606 struct isotp_sock *so = isotp_sk(sk);
1607
1608 so->ifindex = 0;
1609 so->bound = 0;
1610
1611 so->opt.flags = CAN_ISOTP_DEFAULT_FLAGS;
1612 so->opt.ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
1613 so->opt.rx_ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
1614 so->opt.rxpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
1615 so->opt.txpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
1616 so->opt.frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME;
1617 so->frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME;
1618 so->rxfc.bs = CAN_ISOTP_DEFAULT_RECV_BS;
1619 so->rxfc.stmin = CAN_ISOTP_DEFAULT_RECV_STMIN;
1620 so->rxfc.wftmax = CAN_ISOTP_DEFAULT_RECV_WFTMAX;
1621 so->ll.mtu = CAN_ISOTP_DEFAULT_LL_MTU;
1622 so->ll.tx_dl = CAN_ISOTP_DEFAULT_LL_TX_DL;
1623 so->ll.tx_flags = CAN_ISOTP_DEFAULT_LL_TX_FLAGS;
1624
1625 /* set ll_dl for tx path to similar place as for rx */
1626 so->tx.ll_dl = so->ll.tx_dl;
1627
1628 so->rx.state = ISOTP_IDLE;
1629 so->tx.state = ISOTP_IDLE;
1630
1631 so->rx.buf = so->rx.sbuf;
1632 so->tx.buf = so->tx.sbuf;
1633 so->rx.buflen = ARRAY_SIZE(so->rx.sbuf);
1634 so->tx.buflen = ARRAY_SIZE(so->tx.sbuf);
1635
1636 hrtimer_init(&so->rxtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
1637 so->rxtimer.function = isotp_rx_timer_handler;
1638 hrtimer_init(&so->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
1639 so->txtimer.function = isotp_tx_timer_handler;
1640 hrtimer_init(&so->txfrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
1641 so->txfrtimer.function = isotp_txfr_timer_handler;
1642
1643 init_waitqueue_head(&so->wait);
1644 spin_lock_init(&so->rx_lock);
1645
1646 spin_lock(&isotp_notifier_lock);
1647 list_add_tail(&so->notifier, &isotp_notifier_list);
1648 spin_unlock(&isotp_notifier_lock);
1649
1650 return 0;
1651 }
1652
isotp_poll(struct file * file,struct socket * sock,poll_table * wait)1653 static __poll_t isotp_poll(struct file *file, struct socket *sock, poll_table *wait)
1654 {
1655 struct sock *sk = sock->sk;
1656 struct isotp_sock *so = isotp_sk(sk);
1657
1658 __poll_t mask = datagram_poll(file, sock, wait);
1659 poll_wait(file, &so->wait, wait);
1660
1661 /* Check for false positives due to TX state */
1662 if ((mask & EPOLLWRNORM) && (so->tx.state != ISOTP_IDLE))
1663 mask &= ~(EPOLLOUT | EPOLLWRNORM);
1664
1665 return mask;
1666 }
1667
isotp_sock_no_ioctlcmd(struct socket * sock,unsigned int cmd,unsigned long arg)1668 static int isotp_sock_no_ioctlcmd(struct socket *sock, unsigned int cmd,
1669 unsigned long arg)
1670 {
1671 /* no ioctls for socket layer -> hand it down to NIC layer */
1672 return -ENOIOCTLCMD;
1673 }
1674
1675 static const struct proto_ops isotp_ops = {
1676 .family = PF_CAN,
1677 .release = isotp_release,
1678 .bind = isotp_bind,
1679 .connect = sock_no_connect,
1680 .socketpair = sock_no_socketpair,
1681 .accept = sock_no_accept,
1682 .getname = isotp_getname,
1683 .poll = isotp_poll,
1684 .ioctl = isotp_sock_no_ioctlcmd,
1685 .gettstamp = sock_gettstamp,
1686 .listen = sock_no_listen,
1687 .shutdown = sock_no_shutdown,
1688 .setsockopt = isotp_setsockopt,
1689 .getsockopt = isotp_getsockopt,
1690 .sendmsg = isotp_sendmsg,
1691 .recvmsg = isotp_recvmsg,
1692 .mmap = sock_no_mmap,
1693 };
1694
1695 static struct proto isotp_proto __read_mostly = {
1696 .name = "CAN_ISOTP",
1697 .owner = THIS_MODULE,
1698 .obj_size = sizeof(struct isotp_sock),
1699 .init = isotp_init,
1700 };
1701
1702 static const struct can_proto isotp_can_proto = {
1703 .type = SOCK_DGRAM,
1704 .protocol = CAN_ISOTP,
1705 .ops = &isotp_ops,
1706 .prot = &isotp_proto,
1707 };
1708
1709 static struct notifier_block canisotp_notifier = {
1710 .notifier_call = isotp_notifier
1711 };
1712
isotp_module_init(void)1713 static __init int isotp_module_init(void)
1714 {
1715 int err;
1716
1717 max_pdu_size = max_t(unsigned int, max_pdu_size, MAX_12BIT_PDU_SIZE);
1718 max_pdu_size = min_t(unsigned int, max_pdu_size, MAX_PDU_SIZE);
1719
1720 pr_info("can: isotp protocol (max_pdu_size %d)\n", max_pdu_size);
1721
1722 err = can_proto_register(&isotp_can_proto);
1723 if (err < 0)
1724 pr_err("can: registration of isotp protocol failed %pe\n", ERR_PTR(err));
1725 else
1726 register_netdevice_notifier(&canisotp_notifier);
1727
1728 return err;
1729 }
1730
isotp_module_exit(void)1731 static __exit void isotp_module_exit(void)
1732 {
1733 can_proto_unregister(&isotp_can_proto);
1734 unregister_netdevice_notifier(&canisotp_notifier);
1735 }
1736
1737 module_init(isotp_module_init);
1738 module_exit(isotp_module_exit);
1739