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