1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56#include <linux/module.h>
57#include <linux/init.h>
58#include <linux/interrupt.h>
59#include <linux/spinlock.h>
60#include <linux/hrtimer.h>
61#include <linux/wait.h>
62#include <linux/uio.h>
63#include <linux/net.h>
64#include <linux/netdevice.h>
65#include <linux/socket.h>
66#include <linux/if_arp.h>
67#include <linux/skbuff.h>
68#include <linux/can.h>
69#include <linux/can/core.h>
70#include <linux/can/skb.h>
71#include <linux/can/isotp.h>
72#include <linux/slab.h>
73#include <net/sock.h>
74#include <net/net_namespace.h>
75
76MODULE_DESCRIPTION("PF_CAN isotp 15765-2:2016 protocol");
77MODULE_LICENSE("Dual BSD/GPL");
78MODULE_AUTHOR("Oliver Hartkopp <socketcan@hartkopp.net>");
79MODULE_ALIAS("can-proto-6");
80
81#define ISOTP_MIN_NAMELEN CAN_REQUIRED_SIZE(struct sockaddr_can, can_addr.tp)
82
83#define SINGLE_MASK(id) (((id) & CAN_EFF_FLAG) ? \
84 (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \
85 (CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG))
86
87
88
89
90
91
92#define MAX_MSG_LENGTH 8200
93
94
95#define N_PCI_SF 0x00
96#define N_PCI_FF 0x10
97#define N_PCI_CF 0x20
98#define N_PCI_FC 0x30
99
100#define N_PCI_SZ 1
101#define SF_PCI_SZ4 1
102#define SF_PCI_SZ8 2
103#define FF_PCI_SZ12 2
104#define FF_PCI_SZ32 6
105#define FC_CONTENT_SZ 3
106
107#define ISOTP_CHECK_PADDING (CAN_ISOTP_CHK_PAD_LEN | CAN_ISOTP_CHK_PAD_DATA)
108
109
110#define ISOTP_FC_CTS 0
111#define ISOTP_FC_WT 1
112#define ISOTP_FC_OVFLW 2
113
114enum {
115 ISOTP_IDLE = 0,
116 ISOTP_WAIT_FIRST_FC,
117 ISOTP_WAIT_FC,
118 ISOTP_WAIT_DATA,
119 ISOTP_SENDING
120};
121
122struct tpcon {
123 unsigned int idx;
124 unsigned int len;
125 u32 state;
126 u8 bs;
127 u8 sn;
128 u8 ll_dl;
129 u8 buf[MAX_MSG_LENGTH + 1];
130};
131
132struct isotp_sock {
133 struct sock sk;
134 int bound;
135 int ifindex;
136 canid_t txid;
137 canid_t rxid;
138 ktime_t tx_gap;
139 ktime_t lastrxcf_tstamp;
140 struct hrtimer rxtimer, txtimer;
141 struct can_isotp_options opt;
142 struct can_isotp_fc_options rxfc, txfc;
143 struct can_isotp_ll_options ll;
144 u32 force_tx_stmin;
145 u32 force_rx_stmin;
146 struct tpcon rx, tx;
147 struct list_head notifier;
148 wait_queue_head_t wait;
149 spinlock_t rx_lock;
150};
151
152static LIST_HEAD(isotp_notifier_list);
153static DEFINE_SPINLOCK(isotp_notifier_lock);
154static struct isotp_sock *isotp_busy_notifier;
155
156static inline struct isotp_sock *isotp_sk(const struct sock *sk)
157{
158 return (struct isotp_sock *)sk;
159}
160
161static enum hrtimer_restart isotp_rx_timer_handler(struct hrtimer *hrtimer)
162{
163 struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
164 rxtimer);
165 struct sock *sk = &so->sk;
166
167 if (so->rx.state == ISOTP_WAIT_DATA) {
168
169
170
171 sk->sk_err = ETIMEDOUT;
172 if (!sock_flag(sk, SOCK_DEAD))
173 sk_error_report(sk);
174
175
176 so->rx.state = ISOTP_IDLE;
177 }
178
179 return HRTIMER_NORESTART;
180}
181
182static int isotp_send_fc(struct sock *sk, int ae, u8 flowstatus)
183{
184 struct net_device *dev;
185 struct sk_buff *nskb;
186 struct canfd_frame *ncf;
187 struct isotp_sock *so = isotp_sk(sk);
188 int can_send_ret;
189
190 nskb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv), gfp_any());
191 if (!nskb)
192 return 1;
193
194 dev = dev_get_by_index(sock_net(sk), so->ifindex);
195 if (!dev) {
196 kfree_skb(nskb);
197 return 1;
198 }
199
200 can_skb_reserve(nskb);
201 can_skb_prv(nskb)->ifindex = dev->ifindex;
202 can_skb_prv(nskb)->skbcnt = 0;
203
204 nskb->dev = dev;
205 can_skb_set_owner(nskb, sk);
206 ncf = (struct canfd_frame *)nskb->data;
207 skb_put_zero(nskb, so->ll.mtu);
208
209
210 ncf->can_id = so->txid;
211
212 if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
213 memset(ncf->data, so->opt.txpad_content, CAN_MAX_DLEN);
214 ncf->len = CAN_MAX_DLEN;
215 } else {
216 ncf->len = ae + FC_CONTENT_SZ;
217 }
218
219 ncf->data[ae] = N_PCI_FC | flowstatus;
220 ncf->data[ae + 1] = so->rxfc.bs;
221 ncf->data[ae + 2] = so->rxfc.stmin;
222
223 if (ae)
224 ncf->data[0] = so->opt.ext_address;
225
226 ncf->flags = so->ll.tx_flags;
227
228 can_send_ret = can_send(nskb, 1);
229 if (can_send_ret)
230 pr_notice_once("can-isotp: %s: can_send_ret %pe\n",
231 __func__, ERR_PTR(can_send_ret));
232
233 dev_put(dev);
234
235
236 so->rx.bs = 0;
237
238
239 so->lastrxcf_tstamp = ktime_set(0, 0);
240
241
242 hrtimer_start(&so->rxtimer, ktime_set(1, 0), HRTIMER_MODE_REL_SOFT);
243 return 0;
244}
245
246static void isotp_rcv_skb(struct sk_buff *skb, struct sock *sk)
247{
248 struct sockaddr_can *addr = (struct sockaddr_can *)skb->cb;
249
250 BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
251
252 memset(addr, 0, sizeof(*addr));
253 addr->can_family = AF_CAN;
254 addr->can_ifindex = skb->dev->ifindex;
255
256 if (sock_queue_rcv_skb(sk, skb) < 0)
257 kfree_skb(skb);
258}
259
260static u8 padlen(u8 datalen)
261{
262 static const u8 plen[] = {
263 8, 8, 8, 8, 8, 8, 8, 8, 8,
264 12, 12, 12, 12,
265 16, 16, 16, 16,
266 20, 20, 20, 20,
267 24, 24, 24, 24,
268 32, 32, 32, 32, 32, 32, 32, 32,
269 48, 48, 48, 48, 48, 48, 48, 48,
270 48, 48, 48, 48, 48, 48, 48, 48
271 };
272
273 if (datalen > 48)
274 return 64;
275
276 return plen[datalen];
277}
278
279
280static int check_optimized(struct canfd_frame *cf, int start_index)
281{
282
283
284
285
286
287 if (cf->len <= CAN_MAX_DLEN)
288 return (cf->len != start_index);
289
290
291
292
293
294
295
296 return (cf->len != padlen(start_index));
297}
298
299
300static int check_pad(struct isotp_sock *so, struct canfd_frame *cf,
301 int start_index, u8 content)
302{
303 int i;
304
305
306 if (!(so->opt.flags & CAN_ISOTP_RX_PADDING)) {
307 if (so->opt.flags & CAN_ISOTP_CHK_PAD_LEN)
308 return check_optimized(cf, start_index);
309
310
311 return 1;
312 }
313
314
315 if ((so->opt.flags & CAN_ISOTP_CHK_PAD_LEN) &&
316 cf->len != padlen(cf->len))
317 return 1;
318
319
320 if (so->opt.flags & CAN_ISOTP_CHK_PAD_DATA) {
321 for (i = start_index; i < cf->len; i++)
322 if (cf->data[i] != content)
323 return 1;
324 }
325 return 0;
326}
327
328static int isotp_rcv_fc(struct isotp_sock *so, struct canfd_frame *cf, int ae)
329{
330 struct sock *sk = &so->sk;
331
332 if (so->tx.state != ISOTP_WAIT_FC &&
333 so->tx.state != ISOTP_WAIT_FIRST_FC)
334 return 0;
335
336 hrtimer_cancel(&so->txtimer);
337
338 if ((cf->len < ae + FC_CONTENT_SZ) ||
339 ((so->opt.flags & ISOTP_CHECK_PADDING) &&
340 check_pad(so, cf, ae + FC_CONTENT_SZ, so->opt.rxpad_content))) {
341
342 sk->sk_err = EBADMSG;
343 if (!sock_flag(sk, SOCK_DEAD))
344 sk_error_report(sk);
345
346 so->tx.state = ISOTP_IDLE;
347 wake_up_interruptible(&so->wait);
348 return 1;
349 }
350
351
352 if (so->tx.state == ISOTP_WAIT_FIRST_FC) {
353 so->txfc.bs = cf->data[ae + 1];
354 so->txfc.stmin = cf->data[ae + 2];
355
356
357 if (so->txfc.stmin > 0x7F &&
358 (so->txfc.stmin < 0xF1 || so->txfc.stmin > 0xF9))
359 so->txfc.stmin = 0x7F;
360
361 so->tx_gap = ktime_set(0, 0);
362
363 so->tx_gap = ktime_add_ns(so->tx_gap, so->opt.frame_txtime);
364
365 if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN)
366 so->tx_gap = ktime_add_ns(so->tx_gap,
367 so->force_tx_stmin);
368 else if (so->txfc.stmin < 0x80)
369 so->tx_gap = ktime_add_ns(so->tx_gap,
370 so->txfc.stmin * 1000000);
371 else
372 so->tx_gap = ktime_add_ns(so->tx_gap,
373 (so->txfc.stmin - 0xF0)
374 * 100000);
375 so->tx.state = ISOTP_WAIT_FC;
376 }
377
378 switch (cf->data[ae] & 0x0F) {
379 case ISOTP_FC_CTS:
380 so->tx.bs = 0;
381 so->tx.state = ISOTP_SENDING;
382
383 hrtimer_start(&so->txtimer, so->tx_gap,
384 HRTIMER_MODE_REL_SOFT);
385 break;
386
387 case ISOTP_FC_WT:
388
389 hrtimer_start(&so->txtimer, ktime_set(1, 0),
390 HRTIMER_MODE_REL_SOFT);
391 break;
392
393 case ISOTP_FC_OVFLW:
394
395 sk->sk_err = EMSGSIZE;
396 if (!sock_flag(sk, SOCK_DEAD))
397 sk_error_report(sk);
398 fallthrough;
399
400 default:
401
402 so->tx.state = ISOTP_IDLE;
403 wake_up_interruptible(&so->wait);
404 }
405 return 0;
406}
407
408static int isotp_rcv_sf(struct sock *sk, struct canfd_frame *cf, int pcilen,
409 struct sk_buff *skb, int len)
410{
411 struct isotp_sock *so = isotp_sk(sk);
412 struct sk_buff *nskb;
413
414 hrtimer_cancel(&so->rxtimer);
415 so->rx.state = ISOTP_IDLE;
416
417 if (!len || len > cf->len - pcilen)
418 return 1;
419
420 if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
421 check_pad(so, cf, pcilen + len, so->opt.rxpad_content)) {
422
423 sk->sk_err = EBADMSG;
424 if (!sock_flag(sk, SOCK_DEAD))
425 sk_error_report(sk);
426 return 1;
427 }
428
429 nskb = alloc_skb(len, gfp_any());
430 if (!nskb)
431 return 1;
432
433 memcpy(skb_put(nskb, len), &cf->data[pcilen], len);
434
435 nskb->tstamp = skb->tstamp;
436 nskb->dev = skb->dev;
437 isotp_rcv_skb(nskb, sk);
438 return 0;
439}
440
441static int isotp_rcv_ff(struct sock *sk, struct canfd_frame *cf, int ae)
442{
443 struct isotp_sock *so = isotp_sk(sk);
444 int i;
445 int off;
446 int ff_pci_sz;
447
448 hrtimer_cancel(&so->rxtimer);
449 so->rx.state = ISOTP_IDLE;
450
451
452 so->rx.ll_dl = padlen(cf->len);
453
454
455 if (cf->len != so->rx.ll_dl)
456 return 1;
457
458
459 so->rx.len = (cf->data[ae] & 0x0F) << 8;
460 so->rx.len += cf->data[ae + 1];
461
462
463 if (so->rx.len) {
464 ff_pci_sz = FF_PCI_SZ12;
465 } else {
466
467 so->rx.len = cf->data[ae + 2] << 24;
468 so->rx.len += cf->data[ae + 3] << 16;
469 so->rx.len += cf->data[ae + 4] << 8;
470 so->rx.len += cf->data[ae + 5];
471 ff_pci_sz = FF_PCI_SZ32;
472 }
473
474
475 off = (so->rx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
476
477 if (so->rx.len + ae + off + ff_pci_sz < so->rx.ll_dl)
478 return 1;
479
480 if (so->rx.len > MAX_MSG_LENGTH) {
481
482 isotp_send_fc(sk, ae, ISOTP_FC_OVFLW);
483 return 1;
484 }
485
486
487 so->rx.idx = 0;
488 for (i = ae + ff_pci_sz; i < so->rx.ll_dl; i++)
489 so->rx.buf[so->rx.idx++] = cf->data[i];
490
491
492 so->rx.sn = 1;
493 so->rx.state = ISOTP_WAIT_DATA;
494
495
496 if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
497 return 0;
498
499
500 isotp_send_fc(sk, ae, ISOTP_FC_CTS);
501 return 0;
502}
503
504static int isotp_rcv_cf(struct sock *sk, struct canfd_frame *cf, int ae,
505 struct sk_buff *skb)
506{
507 struct isotp_sock *so = isotp_sk(sk);
508 struct sk_buff *nskb;
509 int i;
510
511 if (so->rx.state != ISOTP_WAIT_DATA)
512 return 0;
513
514
515 if (so->opt.flags & CAN_ISOTP_FORCE_RXSTMIN) {
516 if (ktime_to_ns(ktime_sub(skb->tstamp, so->lastrxcf_tstamp)) <
517 so->force_rx_stmin)
518 return 0;
519
520 so->lastrxcf_tstamp = skb->tstamp;
521 }
522
523 hrtimer_cancel(&so->rxtimer);
524
525
526 if (cf->len > so->rx.ll_dl)
527 return 1;
528
529
530 if (cf->len < so->rx.ll_dl) {
531
532 if (so->rx.len - so->rx.idx > so->rx.ll_dl - ae - N_PCI_SZ)
533 return 1;
534 }
535
536 if ((cf->data[ae] & 0x0F) != so->rx.sn) {
537
538 sk->sk_err = EILSEQ;
539 if (!sock_flag(sk, SOCK_DEAD))
540 sk_error_report(sk);
541
542
543 so->rx.state = ISOTP_IDLE;
544 return 1;
545 }
546 so->rx.sn++;
547 so->rx.sn %= 16;
548
549 for (i = ae + N_PCI_SZ; i < cf->len; i++) {
550 so->rx.buf[so->rx.idx++] = cf->data[i];
551 if (so->rx.idx >= so->rx.len)
552 break;
553 }
554
555 if (so->rx.idx >= so->rx.len) {
556
557 so->rx.state = ISOTP_IDLE;
558
559 if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
560 check_pad(so, cf, i + 1, so->opt.rxpad_content)) {
561
562 sk->sk_err = EBADMSG;
563 if (!sock_flag(sk, SOCK_DEAD))
564 sk_error_report(sk);
565 return 1;
566 }
567
568 nskb = alloc_skb(so->rx.len, gfp_any());
569 if (!nskb)
570 return 1;
571
572 memcpy(skb_put(nskb, so->rx.len), so->rx.buf,
573 so->rx.len);
574
575 nskb->tstamp = skb->tstamp;
576 nskb->dev = skb->dev;
577 isotp_rcv_skb(nskb, sk);
578 return 0;
579 }
580
581
582 if (!so->rxfc.bs || ++so->rx.bs < so->rxfc.bs) {
583
584 hrtimer_start(&so->rxtimer, ktime_set(1, 0),
585 HRTIMER_MODE_REL_SOFT);
586 return 0;
587 }
588
589
590 if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
591 return 0;
592
593
594 isotp_send_fc(sk, ae, ISOTP_FC_CTS);
595 return 0;
596}
597
598static void isotp_rcv(struct sk_buff *skb, void *data)
599{
600 struct sock *sk = (struct sock *)data;
601 struct isotp_sock *so = isotp_sk(sk);
602 struct canfd_frame *cf;
603 int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
604 u8 n_pci_type, sf_dl;
605
606
607
608
609 if (skb->len != so->ll.mtu)
610 return;
611
612 cf = (struct canfd_frame *)skb->data;
613
614
615 if (ae && cf->data[0] != so->opt.rx_ext_address)
616 return;
617
618 n_pci_type = cf->data[ae] & 0xF0;
619
620
621
622
623
624 spin_lock(&so->rx_lock);
625
626 if (so->opt.flags & CAN_ISOTP_HALF_DUPLEX) {
627
628 if ((so->tx.state != ISOTP_IDLE && n_pci_type != N_PCI_FC) ||
629 (so->rx.state != ISOTP_IDLE && n_pci_type == N_PCI_FC))
630 goto out_unlock;
631 }
632
633 switch (n_pci_type) {
634 case N_PCI_FC:
635
636 isotp_rcv_fc(so, cf, ae);
637 break;
638
639 case N_PCI_SF:
640
641
642
643
644
645
646
647
648 sf_dl = cf->data[ae] & 0x0F;
649
650 if (cf->len <= CAN_MAX_DLEN) {
651 isotp_rcv_sf(sk, cf, SF_PCI_SZ4 + ae, skb, sf_dl);
652 } else {
653 if (skb->len == CANFD_MTU) {
654
655
656
657
658
659
660
661
662 if (sf_dl == 0)
663 isotp_rcv_sf(sk, cf, SF_PCI_SZ8 + ae, skb,
664 cf->data[SF_PCI_SZ4 + ae]);
665 }
666 }
667 break;
668
669 case N_PCI_FF:
670
671 isotp_rcv_ff(sk, cf, ae);
672 break;
673
674 case N_PCI_CF:
675
676 isotp_rcv_cf(sk, cf, ae, skb);
677 break;
678 }
679
680out_unlock:
681 spin_unlock(&so->rx_lock);
682}
683
684static void isotp_fill_dataframe(struct canfd_frame *cf, struct isotp_sock *so,
685 int ae, int off)
686{
687 int pcilen = N_PCI_SZ + ae + off;
688 int space = so->tx.ll_dl - pcilen;
689 int num = min_t(int, so->tx.len - so->tx.idx, space);
690 int i;
691
692 cf->can_id = so->txid;
693 cf->len = num + pcilen;
694
695 if (num < space) {
696 if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
697
698 cf->len = padlen(cf->len);
699 memset(cf->data, so->opt.txpad_content, cf->len);
700 } else if (cf->len > CAN_MAX_DLEN) {
701
702 cf->len = padlen(cf->len);
703 memset(cf->data, CAN_ISOTP_DEFAULT_PAD_CONTENT,
704 cf->len);
705 }
706 }
707
708 for (i = 0; i < num; i++)
709 cf->data[pcilen + i] = so->tx.buf[so->tx.idx++];
710
711 if (ae)
712 cf->data[0] = so->opt.ext_address;
713}
714
715static void isotp_create_fframe(struct canfd_frame *cf, struct isotp_sock *so,
716 int ae)
717{
718 int i;
719 int ff_pci_sz;
720
721 cf->can_id = so->txid;
722 cf->len = so->tx.ll_dl;
723 if (ae)
724 cf->data[0] = so->opt.ext_address;
725
726
727 if (so->tx.len > 4095) {
728
729 cf->data[ae] = N_PCI_FF;
730 cf->data[ae + 1] = 0;
731 cf->data[ae + 2] = (u8)(so->tx.len >> 24) & 0xFFU;
732 cf->data[ae + 3] = (u8)(so->tx.len >> 16) & 0xFFU;
733 cf->data[ae + 4] = (u8)(so->tx.len >> 8) & 0xFFU;
734 cf->data[ae + 5] = (u8)so->tx.len & 0xFFU;
735 ff_pci_sz = FF_PCI_SZ32;
736 } else {
737
738 cf->data[ae] = (u8)(so->tx.len >> 8) | N_PCI_FF;
739 cf->data[ae + 1] = (u8)so->tx.len & 0xFFU;
740 ff_pci_sz = FF_PCI_SZ12;
741 }
742
743
744 for (i = ae + ff_pci_sz; i < so->tx.ll_dl; i++)
745 cf->data[i] = so->tx.buf[so->tx.idx++];
746
747 so->tx.sn = 1;
748 so->tx.state = ISOTP_WAIT_FIRST_FC;
749}
750
751static enum hrtimer_restart isotp_tx_timer_handler(struct hrtimer *hrtimer)
752{
753 struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
754 txtimer);
755 struct sock *sk = &so->sk;
756 struct sk_buff *skb;
757 struct net_device *dev;
758 struct canfd_frame *cf;
759 enum hrtimer_restart restart = HRTIMER_NORESTART;
760 int can_send_ret;
761 int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
762
763 switch (so->tx.state) {
764 case ISOTP_WAIT_FC:
765 case ISOTP_WAIT_FIRST_FC:
766
767
768
769
770 sk->sk_err = ECOMM;
771 if (!sock_flag(sk, SOCK_DEAD))
772 sk_error_report(sk);
773
774
775 so->tx.state = ISOTP_IDLE;
776 wake_up_interruptible(&so->wait);
777 break;
778
779 case ISOTP_SENDING:
780
781
782 dev = dev_get_by_index(sock_net(sk), so->ifindex);
783 if (!dev)
784 break;
785
786isotp_tx_burst:
787 skb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv),
788 GFP_ATOMIC);
789 if (!skb) {
790 dev_put(dev);
791 break;
792 }
793
794 can_skb_reserve(skb);
795 can_skb_prv(skb)->ifindex = dev->ifindex;
796 can_skb_prv(skb)->skbcnt = 0;
797
798 cf = (struct canfd_frame *)skb->data;
799 skb_put_zero(skb, so->ll.mtu);
800
801
802 isotp_fill_dataframe(cf, so, ae, 0);
803
804
805 cf->data[ae] = N_PCI_CF | so->tx.sn++;
806 so->tx.sn %= 16;
807 so->tx.bs++;
808
809 cf->flags = so->ll.tx_flags;
810
811 skb->dev = dev;
812 can_skb_set_owner(skb, sk);
813
814 can_send_ret = can_send(skb, 1);
815 if (can_send_ret) {
816 pr_notice_once("can-isotp: %s: can_send_ret %pe\n",
817 __func__, ERR_PTR(can_send_ret));
818 if (can_send_ret == -ENOBUFS)
819 pr_notice_once("can-isotp: tx queue is full, increasing txqueuelen may prevent this error\n");
820 }
821 if (so->tx.idx >= so->tx.len) {
822
823 so->tx.state = ISOTP_IDLE;
824 dev_put(dev);
825 wake_up_interruptible(&so->wait);
826 break;
827 }
828
829 if (so->txfc.bs && so->tx.bs >= so->txfc.bs) {
830
831 so->tx.state = ISOTP_WAIT_FC;
832 dev_put(dev);
833 hrtimer_set_expires(&so->txtimer,
834 ktime_add(ktime_get(),
835 ktime_set(1, 0)));
836 restart = HRTIMER_RESTART;
837 break;
838 }
839
840
841 if (!so->tx_gap)
842 goto isotp_tx_burst;
843
844
845 dev_put(dev);
846 hrtimer_set_expires(&so->txtimer,
847 ktime_add(ktime_get(), so->tx_gap));
848 restart = HRTIMER_RESTART;
849 break;
850
851 default:
852 WARN_ON_ONCE(1);
853 }
854
855 return restart;
856}
857
858static int isotp_sendmsg(struct socket *sock, struct msghdr *msg, size_t size)
859{
860 struct sock *sk = sock->sk;
861 struct isotp_sock *so = isotp_sk(sk);
862 u32 old_state = so->tx.state;
863 struct sk_buff *skb;
864 struct net_device *dev;
865 struct canfd_frame *cf;
866 int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
867 int wait_tx_done = (so->opt.flags & CAN_ISOTP_WAIT_TX_DONE) ? 1 : 0;
868 int off;
869 int err;
870
871 if (!so->bound)
872 return -EADDRNOTAVAIL;
873
874
875 if (cmpxchg(&so->tx.state, ISOTP_IDLE, ISOTP_SENDING) != ISOTP_IDLE ||
876 wq_has_sleeper(&so->wait)) {
877 if (msg->msg_flags & MSG_DONTWAIT) {
878 err = -EAGAIN;
879 goto err_out;
880 }
881
882
883 err = wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
884 if (err)
885 goto err_out;
886 }
887
888 if (!size || size > MAX_MSG_LENGTH) {
889 err = -EINVAL;
890 goto err_out_drop;
891 }
892
893
894 off = (so->tx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
895
896
897 if ((so->opt.flags & CAN_ISOTP_SF_BROADCAST) &&
898 (size > so->tx.ll_dl - SF_PCI_SZ4 - ae - off)) {
899 err = -EINVAL;
900 goto err_out_drop;
901 }
902
903 err = memcpy_from_msg(so->tx.buf, msg, size);
904 if (err < 0)
905 goto err_out_drop;
906
907 dev = dev_get_by_index(sock_net(sk), so->ifindex);
908 if (!dev) {
909 err = -ENXIO;
910 goto err_out_drop;
911 }
912
913 skb = sock_alloc_send_skb(sk, so->ll.mtu + sizeof(struct can_skb_priv),
914 msg->msg_flags & MSG_DONTWAIT, &err);
915 if (!skb) {
916 dev_put(dev);
917 goto err_out_drop;
918 }
919
920 can_skb_reserve(skb);
921 can_skb_prv(skb)->ifindex = dev->ifindex;
922 can_skb_prv(skb)->skbcnt = 0;
923
924 so->tx.len = size;
925 so->tx.idx = 0;
926
927 cf = (struct canfd_frame *)skb->data;
928 skb_put_zero(skb, so->ll.mtu);
929
930
931 if (size <= so->tx.ll_dl - SF_PCI_SZ4 - ae - off) {
932
933
934
935
936
937
938
939
940
941 if (size <= CAN_MAX_DLEN - SF_PCI_SZ4 - ae)
942 off = 0;
943
944 isotp_fill_dataframe(cf, so, ae, off);
945
946
947 cf->data[ae] = N_PCI_SF;
948
949
950 if (off)
951 cf->data[SF_PCI_SZ4 + ae] = size;
952 else
953 cf->data[ae] |= size;
954
955 so->tx.state = ISOTP_IDLE;
956 wake_up_interruptible(&so->wait);
957
958
959 wait_tx_done = 0;
960 } else {
961
962
963 isotp_create_fframe(cf, so, ae);
964
965
966 hrtimer_start(&so->txtimer, ktime_set(1, 0), HRTIMER_MODE_REL_SOFT);
967 }
968
969
970 cf->flags = so->ll.tx_flags;
971
972 skb->dev = dev;
973 skb->sk = sk;
974 err = can_send(skb, 1);
975 dev_put(dev);
976 if (err) {
977 pr_notice_once("can-isotp: %s: can_send_ret %pe\n",
978 __func__, ERR_PTR(err));
979 goto err_out_drop;
980 }
981
982 if (wait_tx_done) {
983
984 wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
985
986 if (sk->sk_err)
987 return -sk->sk_err;
988 }
989
990 return size;
991
992err_out_drop:
993
994 old_state = ISOTP_IDLE;
995err_out:
996 so->tx.state = old_state;
997 if (so->tx.state == ISOTP_IDLE)
998 wake_up_interruptible(&so->wait);
999
1000 return err;
1001}
1002
1003static int isotp_recvmsg(struct socket *sock, struct msghdr *msg, size_t size,
1004 int flags)
1005{
1006 struct sock *sk = sock->sk;
1007 struct sk_buff *skb;
1008 int err = 0;
1009 int noblock;
1010
1011 noblock = flags & MSG_DONTWAIT;
1012 flags &= ~MSG_DONTWAIT;
1013
1014 skb = skb_recv_datagram(sk, flags, noblock, &err);
1015 if (!skb)
1016 return err;
1017
1018 if (size < skb->len)
1019 msg->msg_flags |= MSG_TRUNC;
1020 else
1021 size = skb->len;
1022
1023 err = memcpy_to_msg(msg, skb->data, size);
1024 if (err < 0) {
1025 skb_free_datagram(sk, skb);
1026 return err;
1027 }
1028
1029 sock_recv_timestamp(msg, sk, skb);
1030
1031 if (msg->msg_name) {
1032 __sockaddr_check_size(ISOTP_MIN_NAMELEN);
1033 msg->msg_namelen = ISOTP_MIN_NAMELEN;
1034 memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
1035 }
1036
1037 skb_free_datagram(sk, skb);
1038
1039 return size;
1040}
1041
1042static int isotp_release(struct socket *sock)
1043{
1044 struct sock *sk = sock->sk;
1045 struct isotp_sock *so;
1046 struct net *net;
1047
1048 if (!sk)
1049 return 0;
1050
1051 so = isotp_sk(sk);
1052 net = sock_net(sk);
1053
1054
1055 wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
1056
1057 spin_lock(&isotp_notifier_lock);
1058 while (isotp_busy_notifier == so) {
1059 spin_unlock(&isotp_notifier_lock);
1060 schedule_timeout_uninterruptible(1);
1061 spin_lock(&isotp_notifier_lock);
1062 }
1063 list_del(&so->notifier);
1064 spin_unlock(&isotp_notifier_lock);
1065
1066 lock_sock(sk);
1067
1068
1069 if (so->bound && (!(so->opt.flags & CAN_ISOTP_SF_BROADCAST))) {
1070 if (so->ifindex) {
1071 struct net_device *dev;
1072
1073 dev = dev_get_by_index(net, so->ifindex);
1074 if (dev) {
1075 can_rx_unregister(net, dev, so->rxid,
1076 SINGLE_MASK(so->rxid),
1077 isotp_rcv, sk);
1078 dev_put(dev);
1079 synchronize_rcu();
1080 }
1081 }
1082 }
1083
1084 hrtimer_cancel(&so->txtimer);
1085 hrtimer_cancel(&so->rxtimer);
1086
1087 so->ifindex = 0;
1088 so->bound = 0;
1089
1090 sock_orphan(sk);
1091 sock->sk = NULL;
1092
1093 release_sock(sk);
1094 sock_put(sk);
1095
1096 return 0;
1097}
1098
1099static int isotp_bind(struct socket *sock, struct sockaddr *uaddr, int len)
1100{
1101 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
1102 struct sock *sk = sock->sk;
1103 struct isotp_sock *so = isotp_sk(sk);
1104 struct net *net = sock_net(sk);
1105 int ifindex;
1106 struct net_device *dev;
1107 int err = 0;
1108 int notify_enetdown = 0;
1109 int do_rx_reg = 1;
1110
1111 if (len < ISOTP_MIN_NAMELEN)
1112 return -EINVAL;
1113
1114 if (addr->can_addr.tp.tx_id & (CAN_ERR_FLAG | CAN_RTR_FLAG))
1115 return -EADDRNOTAVAIL;
1116
1117 if (!addr->can_ifindex)
1118 return -ENODEV;
1119
1120 lock_sock(sk);
1121
1122
1123 if (so->opt.flags & CAN_ISOTP_SF_BROADCAST)
1124 do_rx_reg = 0;
1125
1126
1127 if (do_rx_reg) {
1128 if (addr->can_addr.tp.rx_id == addr->can_addr.tp.tx_id) {
1129 err = -EADDRNOTAVAIL;
1130 goto out;
1131 }
1132
1133 if (addr->can_addr.tp.rx_id & (CAN_ERR_FLAG | CAN_RTR_FLAG)) {
1134 err = -EADDRNOTAVAIL;
1135 goto out;
1136 }
1137 }
1138
1139 if (so->bound && addr->can_ifindex == so->ifindex &&
1140 addr->can_addr.tp.rx_id == so->rxid &&
1141 addr->can_addr.tp.tx_id == so->txid)
1142 goto out;
1143
1144 dev = dev_get_by_index(net, addr->can_ifindex);
1145 if (!dev) {
1146 err = -ENODEV;
1147 goto out;
1148 }
1149 if (dev->type != ARPHRD_CAN) {
1150 dev_put(dev);
1151 err = -ENODEV;
1152 goto out;
1153 }
1154 if (dev->mtu < so->ll.mtu) {
1155 dev_put(dev);
1156 err = -EINVAL;
1157 goto out;
1158 }
1159 if (!(dev->flags & IFF_UP))
1160 notify_enetdown = 1;
1161
1162 ifindex = dev->ifindex;
1163
1164 if (do_rx_reg)
1165 can_rx_register(net, dev, addr->can_addr.tp.rx_id,
1166 SINGLE_MASK(addr->can_addr.tp.rx_id),
1167 isotp_rcv, sk, "isotp", sk);
1168
1169 dev_put(dev);
1170
1171 if (so->bound && do_rx_reg) {
1172
1173 if (so->ifindex) {
1174 dev = dev_get_by_index(net, so->ifindex);
1175 if (dev) {
1176 can_rx_unregister(net, dev, so->rxid,
1177 SINGLE_MASK(so->rxid),
1178 isotp_rcv, sk);
1179 dev_put(dev);
1180 }
1181 }
1182 }
1183
1184
1185 so->ifindex = ifindex;
1186 so->rxid = addr->can_addr.tp.rx_id;
1187 so->txid = addr->can_addr.tp.tx_id;
1188 so->bound = 1;
1189
1190out:
1191 release_sock(sk);
1192
1193 if (notify_enetdown) {
1194 sk->sk_err = ENETDOWN;
1195 if (!sock_flag(sk, SOCK_DEAD))
1196 sk_error_report(sk);
1197 }
1198
1199 return err;
1200}
1201
1202static int isotp_getname(struct socket *sock, struct sockaddr *uaddr, int peer)
1203{
1204 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
1205 struct sock *sk = sock->sk;
1206 struct isotp_sock *so = isotp_sk(sk);
1207
1208 if (peer)
1209 return -EOPNOTSUPP;
1210
1211 memset(addr, 0, ISOTP_MIN_NAMELEN);
1212 addr->can_family = AF_CAN;
1213 addr->can_ifindex = so->ifindex;
1214 addr->can_addr.tp.rx_id = so->rxid;
1215 addr->can_addr.tp.tx_id = so->txid;
1216
1217 return ISOTP_MIN_NAMELEN;
1218}
1219
1220static int isotp_setsockopt_locked(struct socket *sock, int level, int optname,
1221 sockptr_t optval, unsigned int optlen)
1222{
1223 struct sock *sk = sock->sk;
1224 struct isotp_sock *so = isotp_sk(sk);
1225 int ret = 0;
1226
1227 if (so->bound)
1228 return -EISCONN;
1229
1230 switch (optname) {
1231 case CAN_ISOTP_OPTS:
1232 if (optlen != sizeof(struct can_isotp_options))
1233 return -EINVAL;
1234
1235 if (copy_from_sockptr(&so->opt, optval, optlen))
1236 return -EFAULT;
1237
1238
1239 if (!(so->opt.flags & CAN_ISOTP_RX_EXT_ADDR))
1240 so->opt.rx_ext_address = so->opt.ext_address;
1241 break;
1242
1243 case CAN_ISOTP_RECV_FC:
1244 if (optlen != sizeof(struct can_isotp_fc_options))
1245 return -EINVAL;
1246
1247 if (copy_from_sockptr(&so->rxfc, optval, optlen))
1248 return -EFAULT;
1249 break;
1250
1251 case CAN_ISOTP_TX_STMIN:
1252 if (optlen != sizeof(u32))
1253 return -EINVAL;
1254
1255 if (copy_from_sockptr(&so->force_tx_stmin, optval, optlen))
1256 return -EFAULT;
1257 break;
1258
1259 case CAN_ISOTP_RX_STMIN:
1260 if (optlen != sizeof(u32))
1261 return -EINVAL;
1262
1263 if (copy_from_sockptr(&so->force_rx_stmin, optval, optlen))
1264 return -EFAULT;
1265 break;
1266
1267 case CAN_ISOTP_LL_OPTS:
1268 if (optlen == sizeof(struct can_isotp_ll_options)) {
1269 struct can_isotp_ll_options ll;
1270
1271 if (copy_from_sockptr(&ll, optval, optlen))
1272 return -EFAULT;
1273
1274
1275 if (ll.tx_dl != padlen(ll.tx_dl))
1276 return -EINVAL;
1277
1278 if (ll.mtu != CAN_MTU && ll.mtu != CANFD_MTU)
1279 return -EINVAL;
1280
1281 if (ll.mtu == CAN_MTU &&
1282 (ll.tx_dl > CAN_MAX_DLEN || ll.tx_flags != 0))
1283 return -EINVAL;
1284
1285 memcpy(&so->ll, &ll, sizeof(ll));
1286
1287
1288 so->tx.ll_dl = ll.tx_dl;
1289 } else {
1290 return -EINVAL;
1291 }
1292 break;
1293
1294 default:
1295 ret = -ENOPROTOOPT;
1296 }
1297
1298 return ret;
1299}
1300
1301static int isotp_setsockopt(struct socket *sock, int level, int optname,
1302 sockptr_t optval, unsigned int optlen)
1303
1304{
1305 struct sock *sk = sock->sk;
1306 int ret;
1307
1308 if (level != SOL_CAN_ISOTP)
1309 return -EINVAL;
1310
1311 lock_sock(sk);
1312 ret = isotp_setsockopt_locked(sock, level, optname, optval, optlen);
1313 release_sock(sk);
1314 return ret;
1315}
1316
1317static int isotp_getsockopt(struct socket *sock, int level, int optname,
1318 char __user *optval, int __user *optlen)
1319{
1320 struct sock *sk = sock->sk;
1321 struct isotp_sock *so = isotp_sk(sk);
1322 int len;
1323 void *val;
1324
1325 if (level != SOL_CAN_ISOTP)
1326 return -EINVAL;
1327 if (get_user(len, optlen))
1328 return -EFAULT;
1329 if (len < 0)
1330 return -EINVAL;
1331
1332 switch (optname) {
1333 case CAN_ISOTP_OPTS:
1334 len = min_t(int, len, sizeof(struct can_isotp_options));
1335 val = &so->opt;
1336 break;
1337
1338 case CAN_ISOTP_RECV_FC:
1339 len = min_t(int, len, sizeof(struct can_isotp_fc_options));
1340 val = &so->rxfc;
1341 break;
1342
1343 case CAN_ISOTP_TX_STMIN:
1344 len = min_t(int, len, sizeof(u32));
1345 val = &so->force_tx_stmin;
1346 break;
1347
1348 case CAN_ISOTP_RX_STMIN:
1349 len = min_t(int, len, sizeof(u32));
1350 val = &so->force_rx_stmin;
1351 break;
1352
1353 case CAN_ISOTP_LL_OPTS:
1354 len = min_t(int, len, sizeof(struct can_isotp_ll_options));
1355 val = &so->ll;
1356 break;
1357
1358 default:
1359 return -ENOPROTOOPT;
1360 }
1361
1362 if (put_user(len, optlen))
1363 return -EFAULT;
1364 if (copy_to_user(optval, val, len))
1365 return -EFAULT;
1366 return 0;
1367}
1368
1369static void isotp_notify(struct isotp_sock *so, unsigned long msg,
1370 struct net_device *dev)
1371{
1372 struct sock *sk = &so->sk;
1373
1374 if (!net_eq(dev_net(dev), sock_net(sk)))
1375 return;
1376
1377 if (so->ifindex != dev->ifindex)
1378 return;
1379
1380 switch (msg) {
1381 case NETDEV_UNREGISTER:
1382 lock_sock(sk);
1383
1384 if (so->bound && (!(so->opt.flags & CAN_ISOTP_SF_BROADCAST)))
1385 can_rx_unregister(dev_net(dev), dev, so->rxid,
1386 SINGLE_MASK(so->rxid),
1387 isotp_rcv, sk);
1388
1389 so->ifindex = 0;
1390 so->bound = 0;
1391 release_sock(sk);
1392
1393 sk->sk_err = ENODEV;
1394 if (!sock_flag(sk, SOCK_DEAD))
1395 sk_error_report(sk);
1396 break;
1397
1398 case NETDEV_DOWN:
1399 sk->sk_err = ENETDOWN;
1400 if (!sock_flag(sk, SOCK_DEAD))
1401 sk_error_report(sk);
1402 break;
1403 }
1404}
1405
1406static int isotp_notifier(struct notifier_block *nb, unsigned long msg,
1407 void *ptr)
1408{
1409 struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1410
1411 if (dev->type != ARPHRD_CAN)
1412 return NOTIFY_DONE;
1413 if (msg != NETDEV_UNREGISTER && msg != NETDEV_DOWN)
1414 return NOTIFY_DONE;
1415 if (unlikely(isotp_busy_notifier))
1416 return NOTIFY_DONE;
1417
1418 spin_lock(&isotp_notifier_lock);
1419 list_for_each_entry(isotp_busy_notifier, &isotp_notifier_list, notifier) {
1420 spin_unlock(&isotp_notifier_lock);
1421 isotp_notify(isotp_busy_notifier, msg, dev);
1422 spin_lock(&isotp_notifier_lock);
1423 }
1424 isotp_busy_notifier = NULL;
1425 spin_unlock(&isotp_notifier_lock);
1426 return NOTIFY_DONE;
1427}
1428
1429static int isotp_init(struct sock *sk)
1430{
1431 struct isotp_sock *so = isotp_sk(sk);
1432
1433 so->ifindex = 0;
1434 so->bound = 0;
1435
1436 so->opt.flags = CAN_ISOTP_DEFAULT_FLAGS;
1437 so->opt.ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
1438 so->opt.rx_ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
1439 so->opt.rxpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
1440 so->opt.txpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
1441 so->opt.frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME;
1442 so->rxfc.bs = CAN_ISOTP_DEFAULT_RECV_BS;
1443 so->rxfc.stmin = CAN_ISOTP_DEFAULT_RECV_STMIN;
1444 so->rxfc.wftmax = CAN_ISOTP_DEFAULT_RECV_WFTMAX;
1445 so->ll.mtu = CAN_ISOTP_DEFAULT_LL_MTU;
1446 so->ll.tx_dl = CAN_ISOTP_DEFAULT_LL_TX_DL;
1447 so->ll.tx_flags = CAN_ISOTP_DEFAULT_LL_TX_FLAGS;
1448
1449
1450 so->tx.ll_dl = so->ll.tx_dl;
1451
1452 so->rx.state = ISOTP_IDLE;
1453 so->tx.state = ISOTP_IDLE;
1454
1455 hrtimer_init(&so->rxtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
1456 so->rxtimer.function = isotp_rx_timer_handler;
1457 hrtimer_init(&so->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
1458 so->txtimer.function = isotp_tx_timer_handler;
1459
1460 init_waitqueue_head(&so->wait);
1461 spin_lock_init(&so->rx_lock);
1462
1463 spin_lock(&isotp_notifier_lock);
1464 list_add_tail(&so->notifier, &isotp_notifier_list);
1465 spin_unlock(&isotp_notifier_lock);
1466
1467 return 0;
1468}
1469
1470static int isotp_sock_no_ioctlcmd(struct socket *sock, unsigned int cmd,
1471 unsigned long arg)
1472{
1473
1474 return -ENOIOCTLCMD;
1475}
1476
1477static const struct proto_ops isotp_ops = {
1478 .family = PF_CAN,
1479 .release = isotp_release,
1480 .bind = isotp_bind,
1481 .connect = sock_no_connect,
1482 .socketpair = sock_no_socketpair,
1483 .accept = sock_no_accept,
1484 .getname = isotp_getname,
1485 .poll = datagram_poll,
1486 .ioctl = isotp_sock_no_ioctlcmd,
1487 .gettstamp = sock_gettstamp,
1488 .listen = sock_no_listen,
1489 .shutdown = sock_no_shutdown,
1490 .setsockopt = isotp_setsockopt,
1491 .getsockopt = isotp_getsockopt,
1492 .sendmsg = isotp_sendmsg,
1493 .recvmsg = isotp_recvmsg,
1494 .mmap = sock_no_mmap,
1495 .sendpage = sock_no_sendpage,
1496};
1497
1498static struct proto isotp_proto __read_mostly = {
1499 .name = "CAN_ISOTP",
1500 .owner = THIS_MODULE,
1501 .obj_size = sizeof(struct isotp_sock),
1502 .init = isotp_init,
1503};
1504
1505static const struct can_proto isotp_can_proto = {
1506 .type = SOCK_DGRAM,
1507 .protocol = CAN_ISOTP,
1508 .ops = &isotp_ops,
1509 .prot = &isotp_proto,
1510};
1511
1512static struct notifier_block canisotp_notifier = {
1513 .notifier_call = isotp_notifier
1514};
1515
1516static __init int isotp_module_init(void)
1517{
1518 int err;
1519
1520 pr_info("can: isotp protocol\n");
1521
1522 err = can_proto_register(&isotp_can_proto);
1523 if (err < 0)
1524 pr_err("can: registration of isotp protocol failed %pe\n", ERR_PTR(err));
1525 else
1526 register_netdevice_notifier(&canisotp_notifier);
1527
1528 return err;
1529}
1530
1531static __exit void isotp_module_exit(void)
1532{
1533 can_proto_unregister(&isotp_can_proto);
1534 unregister_netdevice_notifier(&canisotp_notifier);
1535}
1536
1537module_init(isotp_module_init);
1538module_exit(isotp_module_exit);
1539