1
2
3
4
5
6
7
8
9
10
11#include <linux/can/skb.h>
12
13#include "j1939-priv.h"
14
15#define J1939_XTP_TX_RETRY_LIMIT 100
16
17#define J1939_ETP_PGN_CTL 0xc800
18#define J1939_ETP_PGN_DAT 0xc700
19#define J1939_TP_PGN_CTL 0xec00
20#define J1939_TP_PGN_DAT 0xeb00
21
22#define J1939_TP_CMD_RTS 0x10
23#define J1939_TP_CMD_CTS 0x11
24#define J1939_TP_CMD_EOMA 0x13
25#define J1939_TP_CMD_BAM 0x20
26#define J1939_TP_CMD_ABORT 0xff
27
28#define J1939_ETP_CMD_RTS 0x14
29#define J1939_ETP_CMD_CTS 0x15
30#define J1939_ETP_CMD_DPO 0x16
31#define J1939_ETP_CMD_EOMA 0x17
32#define J1939_ETP_CMD_ABORT 0xff
33
34enum j1939_xtp_abort {
35 J1939_XTP_NO_ABORT = 0,
36 J1939_XTP_ABORT_BUSY = 1,
37
38
39
40
41
42
43
44 J1939_XTP_ABORT_RESOURCE = 2,
45
46
47
48
49
50
51
52
53
54 J1939_XTP_ABORT_TIMEOUT = 3,
55
56
57
58
59
60
61
62
63 J1939_XTP_ABORT_GENERIC = 4,
64
65
66
67
68
69
70 J1939_XTP_ABORT_FAULT = 5,
71
72
73
74
75
76
77 J1939_XTP_ABORT_UNEXPECTED_DATA = 6,
78
79
80
81
82
83
84 J1939_XTP_ABORT_BAD_SEQ = 7,
85
86
87
88
89
90
91 J1939_XTP_ABORT_DUP_SEQ = 8,
92
93
94
95
96 J1939_XTP_ABORT_EDPO_UNEXPECTED = 9,
97
98
99
100
101 J1939_XTP_ABORT_BAD_EDPO_PGN = 10,
102
103
104 J1939_XTP_ABORT_EDPO_OUTOF_CTS = 11,
105
106
107 J1939_XTP_ABORT_BAD_EDPO_OFFSET = 12,
108
109
110 J1939_XTP_ABORT_OTHER_DEPRECATED = 13,
111
112
113 J1939_XTP_ABORT_ECTS_UNXPECTED_PGN = 14,
114
115
116 J1939_XTP_ABORT_ECTS_TOO_BIG = 15,
117
118
119 J1939_XTP_ABORT_OTHER = 250,
120
121
122
123};
124
125static unsigned int j1939_tp_block = 255;
126static unsigned int j1939_tp_packet_delay;
127static unsigned int j1939_tp_padding = 1;
128
129
130static const char *j1939_xtp_abort_to_str(enum j1939_xtp_abort abort)
131{
132 switch (abort) {
133 case J1939_XTP_ABORT_BUSY:
134 return "Already in one or more connection managed sessions and cannot support another.";
135 case J1939_XTP_ABORT_RESOURCE:
136 return "System resources were needed for another task so this connection managed session was terminated.";
137 case J1939_XTP_ABORT_TIMEOUT:
138 return "A timeout occurred and this is the connection abort to close the session.";
139 case J1939_XTP_ABORT_GENERIC:
140 return "CTS messages received when data transfer is in progress";
141 case J1939_XTP_ABORT_FAULT:
142 return "Maximal retransmit request limit reached";
143 case J1939_XTP_ABORT_UNEXPECTED_DATA:
144 return "Unexpected data transfer packet";
145 case J1939_XTP_ABORT_BAD_SEQ:
146 return "Bad sequence number (and software is not able to recover)";
147 case J1939_XTP_ABORT_DUP_SEQ:
148 return "Duplicate sequence number (and software is not able to recover)";
149 case J1939_XTP_ABORT_EDPO_UNEXPECTED:
150 return "Unexpected EDPO packet (ETP) or Message size > 1785 bytes (TP)";
151 case J1939_XTP_ABORT_BAD_EDPO_PGN:
152 return "Unexpected EDPO PGN (PGN in EDPO is bad)";
153 case J1939_XTP_ABORT_EDPO_OUTOF_CTS:
154 return "EDPO number of packets is greater than CTS";
155 case J1939_XTP_ABORT_BAD_EDPO_OFFSET:
156 return "Bad EDPO offset";
157 case J1939_XTP_ABORT_OTHER_DEPRECATED:
158 return "Deprecated. Use 250 instead (Any other reason)";
159 case J1939_XTP_ABORT_ECTS_UNXPECTED_PGN:
160 return "Unexpected ECTS PGN (PGN in ECTS is bad)";
161 case J1939_XTP_ABORT_ECTS_TOO_BIG:
162 return "ECTS requested packets exceeds message size";
163 case J1939_XTP_ABORT_OTHER:
164 return "Any other reason (if a Connection Abort reason is identified that is not listed in the table use code 250)";
165 default:
166 return "<unknown>";
167 }
168}
169
170static int j1939_xtp_abort_to_errno(struct j1939_priv *priv,
171 enum j1939_xtp_abort abort)
172{
173 int err;
174
175 switch (abort) {
176 case J1939_XTP_NO_ABORT:
177 WARN_ON_ONCE(abort == J1939_XTP_NO_ABORT);
178 err = 0;
179 break;
180 case J1939_XTP_ABORT_BUSY:
181 err = EALREADY;
182 break;
183 case J1939_XTP_ABORT_RESOURCE:
184 err = EMSGSIZE;
185 break;
186 case J1939_XTP_ABORT_TIMEOUT:
187 err = EHOSTUNREACH;
188 break;
189 case J1939_XTP_ABORT_GENERIC:
190 err = EBADMSG;
191 break;
192 case J1939_XTP_ABORT_FAULT:
193 err = ENOTRECOVERABLE;
194 break;
195 case J1939_XTP_ABORT_UNEXPECTED_DATA:
196 err = ENOTCONN;
197 break;
198 case J1939_XTP_ABORT_BAD_SEQ:
199 err = EILSEQ;
200 break;
201 case J1939_XTP_ABORT_DUP_SEQ:
202 err = EPROTO;
203 break;
204 case J1939_XTP_ABORT_EDPO_UNEXPECTED:
205 err = EPROTO;
206 break;
207 case J1939_XTP_ABORT_BAD_EDPO_PGN:
208 err = EPROTO;
209 break;
210 case J1939_XTP_ABORT_EDPO_OUTOF_CTS:
211 err = EPROTO;
212 break;
213 case J1939_XTP_ABORT_BAD_EDPO_OFFSET:
214 err = EPROTO;
215 break;
216 case J1939_XTP_ABORT_OTHER_DEPRECATED:
217 err = EPROTO;
218 break;
219 case J1939_XTP_ABORT_ECTS_UNXPECTED_PGN:
220 err = EPROTO;
221 break;
222 case J1939_XTP_ABORT_ECTS_TOO_BIG:
223 err = EPROTO;
224 break;
225 case J1939_XTP_ABORT_OTHER:
226 err = EPROTO;
227 break;
228 default:
229 netdev_warn(priv->ndev, "Unknown abort code %i", abort);
230 err = EPROTO;
231 }
232
233 return err;
234}
235
236static inline void j1939_session_list_lock(struct j1939_priv *priv)
237{
238 spin_lock_bh(&priv->active_session_list_lock);
239}
240
241static inline void j1939_session_list_unlock(struct j1939_priv *priv)
242{
243 spin_unlock_bh(&priv->active_session_list_lock);
244}
245
246void j1939_session_get(struct j1939_session *session)
247{
248 kref_get(&session->kref);
249}
250
251
252static void __j1939_session_drop(struct j1939_session *session)
253{
254 if (!session->transmission)
255 return;
256
257 j1939_sock_pending_del(session->sk);
258 sock_put(session->sk);
259}
260
261static void j1939_session_destroy(struct j1939_session *session)
262{
263 if (session->err)
264 j1939_sk_errqueue(session, J1939_ERRQUEUE_ABORT);
265 else
266 j1939_sk_errqueue(session, J1939_ERRQUEUE_ACK);
267
268 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
269
270 WARN_ON_ONCE(!list_empty(&session->sk_session_queue_entry));
271 WARN_ON_ONCE(!list_empty(&session->active_session_list_entry));
272
273 skb_queue_purge(&session->skb_queue);
274 __j1939_session_drop(session);
275 j1939_priv_put(session->priv);
276 kfree(session);
277}
278
279static void __j1939_session_release(struct kref *kref)
280{
281 struct j1939_session *session = container_of(kref, struct j1939_session,
282 kref);
283
284 j1939_session_destroy(session);
285}
286
287void j1939_session_put(struct j1939_session *session)
288{
289 kref_put(&session->kref, __j1939_session_release);
290}
291
292static void j1939_session_txtimer_cancel(struct j1939_session *session)
293{
294 if (hrtimer_cancel(&session->txtimer))
295 j1939_session_put(session);
296}
297
298static void j1939_session_rxtimer_cancel(struct j1939_session *session)
299{
300 if (hrtimer_cancel(&session->rxtimer))
301 j1939_session_put(session);
302}
303
304void j1939_session_timers_cancel(struct j1939_session *session)
305{
306 j1939_session_txtimer_cancel(session);
307 j1939_session_rxtimer_cancel(session);
308}
309
310static inline bool j1939_cb_is_broadcast(const struct j1939_sk_buff_cb *skcb)
311{
312 return (!skcb->addr.dst_name && (skcb->addr.da == 0xff));
313}
314
315static void j1939_session_skb_drop_old(struct j1939_session *session)
316{
317 struct sk_buff *do_skb;
318 struct j1939_sk_buff_cb *do_skcb;
319 unsigned int offset_start;
320 unsigned long flags;
321
322 if (skb_queue_len(&session->skb_queue) < 2)
323 return;
324
325 offset_start = session->pkt.tx_acked * 7;
326
327 spin_lock_irqsave(&session->skb_queue.lock, flags);
328 do_skb = skb_peek(&session->skb_queue);
329 do_skcb = j1939_skb_to_cb(do_skb);
330
331 if ((do_skcb->offset + do_skb->len) < offset_start) {
332 __skb_unlink(do_skb, &session->skb_queue);
333 kfree_skb(do_skb);
334 }
335 spin_unlock_irqrestore(&session->skb_queue.lock, flags);
336}
337
338void j1939_session_skb_queue(struct j1939_session *session,
339 struct sk_buff *skb)
340{
341 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
342 struct j1939_priv *priv = session->priv;
343
344 j1939_ac_fixup(priv, skb);
345
346 if (j1939_address_is_unicast(skcb->addr.da) &&
347 priv->ents[skcb->addr.da].nusers)
348 skcb->flags |= J1939_ECU_LOCAL_DST;
349
350 skcb->flags |= J1939_ECU_LOCAL_SRC;
351
352 skb_queue_tail(&session->skb_queue, skb);
353}
354
355static struct
356sk_buff *j1939_session_skb_find_by_offset(struct j1939_session *session,
357 unsigned int offset_start)
358{
359 struct j1939_priv *priv = session->priv;
360 struct j1939_sk_buff_cb *do_skcb;
361 struct sk_buff *skb = NULL;
362 struct sk_buff *do_skb;
363 unsigned long flags;
364
365 spin_lock_irqsave(&session->skb_queue.lock, flags);
366 skb_queue_walk(&session->skb_queue, do_skb) {
367 do_skcb = j1939_skb_to_cb(do_skb);
368
369 if (offset_start >= do_skcb->offset &&
370 offset_start < (do_skcb->offset + do_skb->len)) {
371 skb = do_skb;
372 }
373 }
374 spin_unlock_irqrestore(&session->skb_queue.lock, flags);
375
376 if (!skb)
377 netdev_dbg(priv->ndev, "%s: 0x%p: no skb found for start: %i, queue size: %i\n",
378 __func__, session, offset_start,
379 skb_queue_len(&session->skb_queue));
380
381 return skb;
382}
383
384static struct sk_buff *j1939_session_skb_find(struct j1939_session *session)
385{
386 unsigned int offset_start;
387
388 offset_start = session->pkt.dpo * 7;
389 return j1939_session_skb_find_by_offset(session, offset_start);
390}
391
392
393
394
395static inline int j1939_tp_im_receiver(const struct j1939_sk_buff_cb *skcb)
396{
397 return skcb->flags & J1939_ECU_LOCAL_DST;
398}
399
400
401static inline int j1939_tp_im_transmitter(const struct j1939_sk_buff_cb *skcb)
402{
403 return skcb->flags & J1939_ECU_LOCAL_SRC;
404}
405
406
407static int j1939_tp_im_involved(const struct j1939_sk_buff_cb *skcb, bool swap)
408{
409 if (swap)
410 return j1939_tp_im_receiver(skcb);
411 else
412 return j1939_tp_im_transmitter(skcb);
413}
414
415static int j1939_tp_im_involved_anydir(struct j1939_sk_buff_cb *skcb)
416{
417 return skcb->flags & (J1939_ECU_LOCAL_SRC | J1939_ECU_LOCAL_DST);
418}
419
420
421static inline pgn_t j1939_xtp_ctl_to_pgn(const u8 *dat)
422{
423 pgn_t pgn;
424
425 pgn = (dat[7] << 16) | (dat[6] << 8) | (dat[5] << 0);
426 if (j1939_pgn_is_pdu1(pgn))
427 pgn &= 0xffff00;
428 return pgn;
429}
430
431static inline unsigned int j1939_tp_ctl_to_size(const u8 *dat)
432{
433 return (dat[2] << 8) + (dat[1] << 0);
434}
435
436static inline unsigned int j1939_etp_ctl_to_packet(const u8 *dat)
437{
438 return (dat[4] << 16) | (dat[3] << 8) | (dat[2] << 0);
439}
440
441static inline unsigned int j1939_etp_ctl_to_size(const u8 *dat)
442{
443 return (dat[4] << 24) | (dat[3] << 16) |
444 (dat[2] << 8) | (dat[1] << 0);
445}
446
447
448
449
450
451
452
453static bool j1939_session_match(struct j1939_addr *se_addr,
454 struct j1939_addr *sk_addr, bool reverse)
455{
456 if (se_addr->type != sk_addr->type)
457 return false;
458
459 if (reverse) {
460 if (se_addr->src_name) {
461 if (se_addr->src_name != sk_addr->dst_name)
462 return false;
463 } else if (se_addr->sa != sk_addr->da) {
464 return false;
465 }
466
467 if (se_addr->dst_name) {
468 if (se_addr->dst_name != sk_addr->src_name)
469 return false;
470 } else if (se_addr->da != sk_addr->sa) {
471 return false;
472 }
473 } else {
474 if (se_addr->src_name) {
475 if (se_addr->src_name != sk_addr->src_name)
476 return false;
477 } else if (se_addr->sa != sk_addr->sa) {
478 return false;
479 }
480
481 if (se_addr->dst_name) {
482 if (se_addr->dst_name != sk_addr->dst_name)
483 return false;
484 } else if (se_addr->da != sk_addr->da) {
485 return false;
486 }
487 }
488
489 return true;
490}
491
492static struct
493j1939_session *j1939_session_get_by_addr_locked(struct j1939_priv *priv,
494 struct list_head *root,
495 struct j1939_addr *addr,
496 bool reverse, bool transmitter)
497{
498 struct j1939_session *session;
499
500 lockdep_assert_held(&priv->active_session_list_lock);
501
502 list_for_each_entry(session, root, active_session_list_entry) {
503 j1939_session_get(session);
504 if (j1939_session_match(&session->skcb.addr, addr, reverse) &&
505 session->transmission == transmitter)
506 return session;
507 j1939_session_put(session);
508 }
509
510 return NULL;
511}
512
513static struct
514j1939_session *j1939_session_get_simple(struct j1939_priv *priv,
515 struct sk_buff *skb)
516{
517 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
518 struct j1939_session *session;
519
520 lockdep_assert_held(&priv->active_session_list_lock);
521
522 list_for_each_entry(session, &priv->active_session_list,
523 active_session_list_entry) {
524 j1939_session_get(session);
525 if (session->skcb.addr.type == J1939_SIMPLE &&
526 session->tskey == skcb->tskey && session->sk == skb->sk)
527 return session;
528 j1939_session_put(session);
529 }
530
531 return NULL;
532}
533
534static struct
535j1939_session *j1939_session_get_by_addr(struct j1939_priv *priv,
536 struct j1939_addr *addr,
537 bool reverse, bool transmitter)
538{
539 struct j1939_session *session;
540
541 j1939_session_list_lock(priv);
542 session = j1939_session_get_by_addr_locked(priv,
543 &priv->active_session_list,
544 addr, reverse, transmitter);
545 j1939_session_list_unlock(priv);
546
547 return session;
548}
549
550static void j1939_skbcb_swap(struct j1939_sk_buff_cb *skcb)
551{
552 u8 tmp = 0;
553
554 swap(skcb->addr.dst_name, skcb->addr.src_name);
555 swap(skcb->addr.da, skcb->addr.sa);
556
557
558 if (skcb->flags & J1939_ECU_LOCAL_SRC)
559 tmp |= J1939_ECU_LOCAL_DST;
560 if (skcb->flags & J1939_ECU_LOCAL_DST)
561 tmp |= J1939_ECU_LOCAL_SRC;
562 skcb->flags &= ~(J1939_ECU_LOCAL_SRC | J1939_ECU_LOCAL_DST);
563 skcb->flags |= tmp;
564}
565
566static struct
567sk_buff *j1939_tp_tx_dat_new(struct j1939_priv *priv,
568 const struct j1939_sk_buff_cb *re_skcb,
569 bool ctl,
570 bool swap_src_dst)
571{
572 struct sk_buff *skb;
573 struct j1939_sk_buff_cb *skcb;
574
575 skb = alloc_skb(sizeof(struct can_frame) + sizeof(struct can_skb_priv),
576 GFP_ATOMIC);
577 if (unlikely(!skb))
578 return ERR_PTR(-ENOMEM);
579
580 skb->dev = priv->ndev;
581 can_skb_reserve(skb);
582 can_skb_prv(skb)->ifindex = priv->ndev->ifindex;
583
584 skb_reserve(skb, offsetof(struct can_frame, data));
585
586 memcpy(skb->cb, re_skcb, sizeof(skb->cb));
587 skcb = j1939_skb_to_cb(skb);
588 if (swap_src_dst)
589 j1939_skbcb_swap(skcb);
590
591 if (ctl) {
592 if (skcb->addr.type == J1939_ETP)
593 skcb->addr.pgn = J1939_ETP_PGN_CTL;
594 else
595 skcb->addr.pgn = J1939_TP_PGN_CTL;
596 } else {
597 if (skcb->addr.type == J1939_ETP)
598 skcb->addr.pgn = J1939_ETP_PGN_DAT;
599 else
600 skcb->addr.pgn = J1939_TP_PGN_DAT;
601 }
602
603 return skb;
604}
605
606
607static int j1939_tp_tx_dat(struct j1939_session *session,
608 const u8 *dat, int len)
609{
610 struct j1939_priv *priv = session->priv;
611 struct sk_buff *skb;
612
613 skb = j1939_tp_tx_dat_new(priv, &session->skcb,
614 false, false);
615 if (IS_ERR(skb))
616 return PTR_ERR(skb);
617
618 skb_put_data(skb, dat, len);
619 if (j1939_tp_padding && len < 8)
620 memset(skb_put(skb, 8 - len), 0xff, 8 - len);
621
622 return j1939_send_one(priv, skb);
623}
624
625static int j1939_xtp_do_tx_ctl(struct j1939_priv *priv,
626 const struct j1939_sk_buff_cb *re_skcb,
627 bool swap_src_dst, pgn_t pgn, const u8 *dat)
628{
629 struct sk_buff *skb;
630 u8 *skdat;
631
632 if (!j1939_tp_im_involved(re_skcb, swap_src_dst))
633 return 0;
634
635 skb = j1939_tp_tx_dat_new(priv, re_skcb, true, swap_src_dst);
636 if (IS_ERR(skb))
637 return PTR_ERR(skb);
638
639 skdat = skb_put(skb, 8);
640 memcpy(skdat, dat, 5);
641 skdat[5] = (pgn >> 0);
642 skdat[6] = (pgn >> 8);
643 skdat[7] = (pgn >> 16);
644
645 return j1939_send_one(priv, skb);
646}
647
648static inline int j1939_tp_tx_ctl(struct j1939_session *session,
649 bool swap_src_dst, const u8 *dat)
650{
651 struct j1939_priv *priv = session->priv;
652
653 return j1939_xtp_do_tx_ctl(priv, &session->skcb,
654 swap_src_dst,
655 session->skcb.addr.pgn, dat);
656}
657
658static int j1939_xtp_tx_abort(struct j1939_priv *priv,
659 const struct j1939_sk_buff_cb *re_skcb,
660 bool swap_src_dst,
661 enum j1939_xtp_abort err,
662 pgn_t pgn)
663{
664 u8 dat[5];
665
666 if (!j1939_tp_im_involved(re_skcb, swap_src_dst))
667 return 0;
668
669 memset(dat, 0xff, sizeof(dat));
670 dat[0] = J1939_TP_CMD_ABORT;
671 dat[1] = err;
672 return j1939_xtp_do_tx_ctl(priv, re_skcb, swap_src_dst, pgn, dat);
673}
674
675void j1939_tp_schedule_txtimer(struct j1939_session *session, int msec)
676{
677 j1939_session_get(session);
678 hrtimer_start(&session->txtimer, ms_to_ktime(msec),
679 HRTIMER_MODE_REL_SOFT);
680}
681
682static inline void j1939_tp_set_rxtimeout(struct j1939_session *session,
683 int msec)
684{
685 j1939_session_rxtimer_cancel(session);
686 j1939_session_get(session);
687 hrtimer_start(&session->rxtimer, ms_to_ktime(msec),
688 HRTIMER_MODE_REL_SOFT);
689}
690
691static int j1939_session_tx_rts(struct j1939_session *session)
692{
693 u8 dat[8];
694 int ret;
695
696 memset(dat, 0xff, sizeof(dat));
697
698 dat[1] = (session->total_message_size >> 0);
699 dat[2] = (session->total_message_size >> 8);
700 dat[3] = session->pkt.total;
701
702 if (session->skcb.addr.type == J1939_ETP) {
703 dat[0] = J1939_ETP_CMD_RTS;
704 dat[1] = (session->total_message_size >> 0);
705 dat[2] = (session->total_message_size >> 8);
706 dat[3] = (session->total_message_size >> 16);
707 dat[4] = (session->total_message_size >> 24);
708 } else if (j1939_cb_is_broadcast(&session->skcb)) {
709 dat[0] = J1939_TP_CMD_BAM;
710
711 session->pkt.tx = 0;
712 } else {
713 dat[0] = J1939_TP_CMD_RTS;
714 dat[4] = dat[3];
715 }
716
717 if (dat[0] == session->last_txcmd)
718
719 return 0;
720
721 ret = j1939_tp_tx_ctl(session, false, dat);
722 if (ret < 0)
723 return ret;
724
725 session->last_txcmd = dat[0];
726 if (dat[0] == J1939_TP_CMD_BAM) {
727 j1939_tp_schedule_txtimer(session, 50);
728 j1939_tp_set_rxtimeout(session, 250);
729 } else {
730 j1939_tp_set_rxtimeout(session, 1250);
731 }
732
733 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
734
735 return 0;
736}
737
738static int j1939_session_tx_dpo(struct j1939_session *session)
739{
740 unsigned int pkt;
741 u8 dat[8];
742 int ret;
743
744 memset(dat, 0xff, sizeof(dat));
745
746 dat[0] = J1939_ETP_CMD_DPO;
747 session->pkt.dpo = session->pkt.tx_acked;
748 pkt = session->pkt.dpo;
749 dat[1] = session->pkt.last - session->pkt.tx_acked;
750 dat[2] = (pkt >> 0);
751 dat[3] = (pkt >> 8);
752 dat[4] = (pkt >> 16);
753
754 ret = j1939_tp_tx_ctl(session, false, dat);
755 if (ret < 0)
756 return ret;
757
758 session->last_txcmd = dat[0];
759 j1939_tp_set_rxtimeout(session, 1250);
760 session->pkt.tx = session->pkt.tx_acked;
761
762 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
763
764 return 0;
765}
766
767static int j1939_session_tx_dat(struct j1939_session *session)
768{
769 struct j1939_priv *priv = session->priv;
770 struct j1939_sk_buff_cb *skcb;
771 int offset, pkt_done, pkt_end;
772 unsigned int len, pdelay;
773 struct sk_buff *se_skb;
774 const u8 *tpdat;
775 int ret = 0;
776 u8 dat[8];
777
778 se_skb = j1939_session_skb_find_by_offset(session, session->pkt.tx * 7);
779 if (!se_skb)
780 return -ENOBUFS;
781
782 skcb = j1939_skb_to_cb(se_skb);
783 tpdat = se_skb->data;
784 ret = 0;
785 pkt_done = 0;
786 if (session->skcb.addr.type != J1939_ETP &&
787 j1939_cb_is_broadcast(&session->skcb))
788 pkt_end = session->pkt.total;
789 else
790 pkt_end = session->pkt.last;
791
792 while (session->pkt.tx < pkt_end) {
793 dat[0] = session->pkt.tx - session->pkt.dpo + 1;
794 offset = (session->pkt.tx * 7) - skcb->offset;
795 len = se_skb->len - offset;
796 if (len > 7)
797 len = 7;
798
799 if (offset + len > se_skb->len) {
800 netdev_err_once(priv->ndev,
801 "%s: 0x%p: requested data outside of queued buffer: offset %i, len %i, pkt.tx: %i\n",
802 __func__, session, skcb->offset, se_skb->len , session->pkt.tx);
803 return -EOVERFLOW;
804 }
805
806 if (!len) {
807 ret = -ENOBUFS;
808 break;
809 }
810
811 memcpy(&dat[1], &tpdat[offset], len);
812 ret = j1939_tp_tx_dat(session, dat, len + 1);
813 if (ret < 0) {
814
815 if (ret != -ENOBUFS)
816 netdev_alert(priv->ndev,
817 "%s: 0x%p: queue data error: %i\n",
818 __func__, session, ret);
819 break;
820 }
821
822 session->last_txcmd = 0xff;
823 pkt_done++;
824 session->pkt.tx++;
825 pdelay = j1939_cb_is_broadcast(&session->skcb) ? 50 :
826 j1939_tp_packet_delay;
827
828 if (session->pkt.tx < session->pkt.total && pdelay) {
829 j1939_tp_schedule_txtimer(session, pdelay);
830 break;
831 }
832 }
833
834 if (pkt_done)
835 j1939_tp_set_rxtimeout(session, 250);
836
837 return ret;
838}
839
840static int j1939_xtp_txnext_transmiter(struct j1939_session *session)
841{
842 struct j1939_priv *priv = session->priv;
843 int ret = 0;
844
845 if (!j1939_tp_im_transmitter(&session->skcb)) {
846 netdev_alert(priv->ndev, "%s: 0x%p: called by not transmitter!\n",
847 __func__, session);
848 return -EINVAL;
849 }
850
851 switch (session->last_cmd) {
852 case 0:
853 ret = j1939_session_tx_rts(session);
854 break;
855
856 case J1939_ETP_CMD_CTS:
857 if (session->last_txcmd != J1939_ETP_CMD_DPO) {
858 ret = j1939_session_tx_dpo(session);
859 if (ret)
860 return ret;
861 }
862
863 fallthrough;
864 case J1939_TP_CMD_CTS:
865 case 0xff:
866 case J1939_ETP_CMD_DPO:
867 case J1939_TP_CMD_BAM:
868 ret = j1939_session_tx_dat(session);
869
870 break;
871 default:
872 netdev_alert(priv->ndev, "%s: 0x%p: unexpected last_cmd: %x\n",
873 __func__, session, session->last_cmd);
874 }
875
876 return ret;
877}
878
879static int j1939_session_tx_cts(struct j1939_session *session)
880{
881 struct j1939_priv *priv = session->priv;
882 unsigned int pkt, len;
883 int ret;
884 u8 dat[8];
885
886 if (!j1939_sk_recv_match(priv, &session->skcb))
887 return -ENOENT;
888
889 len = session->pkt.total - session->pkt.rx;
890 len = min3(len, session->pkt.block, j1939_tp_block ?: 255);
891 memset(dat, 0xff, sizeof(dat));
892
893 if (session->skcb.addr.type == J1939_ETP) {
894 pkt = session->pkt.rx + 1;
895 dat[0] = J1939_ETP_CMD_CTS;
896 dat[1] = len;
897 dat[2] = (pkt >> 0);
898 dat[3] = (pkt >> 8);
899 dat[4] = (pkt >> 16);
900 } else {
901 dat[0] = J1939_TP_CMD_CTS;
902 dat[1] = len;
903 dat[2] = session->pkt.rx + 1;
904 }
905
906 if (dat[0] == session->last_txcmd)
907
908 return 0;
909
910 ret = j1939_tp_tx_ctl(session, true, dat);
911 if (ret < 0)
912 return ret;
913
914 if (len)
915
916 session->last_txcmd = dat[0];
917 j1939_tp_set_rxtimeout(session, 1250);
918
919 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
920
921 return 0;
922}
923
924static int j1939_session_tx_eoma(struct j1939_session *session)
925{
926 struct j1939_priv *priv = session->priv;
927 u8 dat[8];
928 int ret;
929
930 if (!j1939_sk_recv_match(priv, &session->skcb))
931 return -ENOENT;
932
933 memset(dat, 0xff, sizeof(dat));
934
935 if (session->skcb.addr.type == J1939_ETP) {
936 dat[0] = J1939_ETP_CMD_EOMA;
937 dat[1] = session->total_message_size >> 0;
938 dat[2] = session->total_message_size >> 8;
939 dat[3] = session->total_message_size >> 16;
940 dat[4] = session->total_message_size >> 24;
941 } else {
942 dat[0] = J1939_TP_CMD_EOMA;
943 dat[1] = session->total_message_size;
944 dat[2] = session->total_message_size >> 8;
945 dat[3] = session->pkt.total;
946 }
947
948 if (dat[0] == session->last_txcmd)
949
950 return 0;
951
952 ret = j1939_tp_tx_ctl(session, true, dat);
953 if (ret < 0)
954 return ret;
955
956 session->last_txcmd = dat[0];
957
958
959 j1939_tp_set_rxtimeout(session, 1250);
960
961 netdev_dbg(session->priv->ndev, "%p: 0x%p\n", __func__, session);
962
963 return 0;
964}
965
966static int j1939_xtp_txnext_receiver(struct j1939_session *session)
967{
968 struct j1939_priv *priv = session->priv;
969 int ret = 0;
970
971 if (!j1939_tp_im_receiver(&session->skcb)) {
972 netdev_alert(priv->ndev, "%s: 0x%p: called by not receiver!\n",
973 __func__, session);
974 return -EINVAL;
975 }
976
977 switch (session->last_cmd) {
978 case J1939_TP_CMD_RTS:
979 case J1939_ETP_CMD_RTS:
980 ret = j1939_session_tx_cts(session);
981 break;
982
983 case J1939_ETP_CMD_CTS:
984 case J1939_TP_CMD_CTS:
985 case 0xff:
986 case J1939_ETP_CMD_DPO:
987 if ((session->skcb.addr.type == J1939_TP &&
988 j1939_cb_is_broadcast(&session->skcb)))
989 break;
990
991 if (session->pkt.rx >= session->pkt.total) {
992 ret = j1939_session_tx_eoma(session);
993 } else if (session->pkt.rx >= session->pkt.last) {
994 session->last_txcmd = 0;
995 ret = j1939_session_tx_cts(session);
996 }
997 break;
998 default:
999 netdev_alert(priv->ndev, "%s: 0x%p: unexpected last_cmd: %x\n",
1000 __func__, session, session->last_cmd);
1001 }
1002
1003 return ret;
1004}
1005
1006static int j1939_simple_txnext(struct j1939_session *session)
1007{
1008 struct j1939_priv *priv = session->priv;
1009 struct sk_buff *se_skb = j1939_session_skb_find(session);
1010 struct sk_buff *skb;
1011 int ret;
1012
1013 if (!se_skb)
1014 return 0;
1015
1016 skb = skb_clone(se_skb, GFP_ATOMIC);
1017 if (!skb)
1018 return -ENOMEM;
1019
1020 can_skb_set_owner(skb, se_skb->sk);
1021
1022 j1939_tp_set_rxtimeout(session, J1939_SIMPLE_ECHO_TIMEOUT_MS);
1023
1024 ret = j1939_send_one(priv, skb);
1025 if (ret)
1026 return ret;
1027
1028 j1939_sk_errqueue(session, J1939_ERRQUEUE_SCHED);
1029 j1939_sk_queue_activate_next(session);
1030
1031 return 0;
1032}
1033
1034static bool j1939_session_deactivate_locked(struct j1939_session *session)
1035{
1036 bool active = false;
1037
1038 lockdep_assert_held(&session->priv->active_session_list_lock);
1039
1040 if (session->state >= J1939_SESSION_ACTIVE &&
1041 session->state < J1939_SESSION_ACTIVE_MAX) {
1042 active = true;
1043
1044 list_del_init(&session->active_session_list_entry);
1045 session->state = J1939_SESSION_DONE;
1046 j1939_session_put(session);
1047 }
1048
1049 return active;
1050}
1051
1052static bool j1939_session_deactivate(struct j1939_session *session)
1053{
1054 bool active;
1055
1056 j1939_session_list_lock(session->priv);
1057 active = j1939_session_deactivate_locked(session);
1058 j1939_session_list_unlock(session->priv);
1059
1060 return active;
1061}
1062
1063static void
1064j1939_session_deactivate_activate_next(struct j1939_session *session)
1065{
1066 if (j1939_session_deactivate(session))
1067 j1939_sk_queue_activate_next(session);
1068}
1069
1070static void __j1939_session_cancel(struct j1939_session *session,
1071 enum j1939_xtp_abort err)
1072{
1073 struct j1939_priv *priv = session->priv;
1074
1075 WARN_ON_ONCE(!err);
1076 lockdep_assert_held(&session->priv->active_session_list_lock);
1077
1078 session->err = j1939_xtp_abort_to_errno(priv, err);
1079 session->state = J1939_SESSION_WAITING_ABORT;
1080
1081 if (!j1939_cb_is_broadcast(&session->skcb)) {
1082 j1939_xtp_tx_abort(priv, &session->skcb,
1083 !session->transmission,
1084 err, session->skcb.addr.pgn);
1085 }
1086
1087 if (session->sk)
1088 j1939_sk_send_loop_abort(session->sk, session->err);
1089}
1090
1091static void j1939_session_cancel(struct j1939_session *session,
1092 enum j1939_xtp_abort err)
1093{
1094 j1939_session_list_lock(session->priv);
1095
1096 if (session->state >= J1939_SESSION_ACTIVE &&
1097 session->state < J1939_SESSION_WAITING_ABORT) {
1098 j1939_tp_set_rxtimeout(session, J1939_XTP_ABORT_TIMEOUT_MS);
1099 __j1939_session_cancel(session, err);
1100 }
1101
1102 j1939_session_list_unlock(session->priv);
1103}
1104
1105static enum hrtimer_restart j1939_tp_txtimer(struct hrtimer *hrtimer)
1106{
1107 struct j1939_session *session =
1108 container_of(hrtimer, struct j1939_session, txtimer);
1109 struct j1939_priv *priv = session->priv;
1110 int ret = 0;
1111
1112 if (session->skcb.addr.type == J1939_SIMPLE) {
1113 ret = j1939_simple_txnext(session);
1114 } else {
1115 if (session->transmission)
1116 ret = j1939_xtp_txnext_transmiter(session);
1117 else
1118 ret = j1939_xtp_txnext_receiver(session);
1119 }
1120
1121 switch (ret) {
1122 case -ENOBUFS:
1123
1124 if (session->tx_retry < J1939_XTP_TX_RETRY_LIMIT) {
1125 session->tx_retry++;
1126 j1939_tp_schedule_txtimer(session,
1127 10 + prandom_u32_max(16));
1128 } else {
1129 netdev_alert(priv->ndev, "%s: 0x%p: tx retry count reached\n",
1130 __func__, session);
1131 session->err = -ENETUNREACH;
1132 j1939_session_rxtimer_cancel(session);
1133 j1939_session_deactivate_activate_next(session);
1134 }
1135 break;
1136 case -ENETDOWN:
1137
1138
1139
1140
1141
1142
1143 break;
1144 case -EOVERFLOW:
1145 j1939_session_cancel(session, J1939_XTP_ABORT_ECTS_TOO_BIG);
1146 break;
1147 case 0:
1148 session->tx_retry = 0;
1149 break;
1150 default:
1151 netdev_alert(priv->ndev, "%s: 0x%p: tx aborted with unknown reason: %i\n",
1152 __func__, session, ret);
1153 if (session->skcb.addr.type != J1939_SIMPLE) {
1154 j1939_session_cancel(session, J1939_XTP_ABORT_OTHER);
1155 } else {
1156 session->err = ret;
1157 j1939_session_rxtimer_cancel(session);
1158 j1939_session_deactivate_activate_next(session);
1159 }
1160 }
1161
1162 j1939_session_put(session);
1163
1164 return HRTIMER_NORESTART;
1165}
1166
1167static void j1939_session_completed(struct j1939_session *session)
1168{
1169 struct sk_buff *skb;
1170
1171 if (!session->transmission) {
1172 skb = j1939_session_skb_find(session);
1173
1174 j1939_sk_recv(session->priv, skb);
1175 }
1176
1177 j1939_session_deactivate_activate_next(session);
1178}
1179
1180static enum hrtimer_restart j1939_tp_rxtimer(struct hrtimer *hrtimer)
1181{
1182 struct j1939_session *session = container_of(hrtimer,
1183 struct j1939_session,
1184 rxtimer);
1185 struct j1939_priv *priv = session->priv;
1186
1187 if (session->state == J1939_SESSION_WAITING_ABORT) {
1188 netdev_alert(priv->ndev, "%s: 0x%p: abort rx timeout. Force session deactivation\n",
1189 __func__, session);
1190
1191 j1939_session_deactivate_activate_next(session);
1192
1193 } else if (session->skcb.addr.type == J1939_SIMPLE) {
1194 netdev_alert(priv->ndev, "%s: 0x%p: Timeout. Failed to send simple message.\n",
1195 __func__, session);
1196
1197
1198
1199
1200 session->err = -ETIME;
1201 j1939_session_deactivate(session);
1202 } else {
1203 netdev_alert(priv->ndev, "%s: 0x%p: rx timeout, send abort\n",
1204 __func__, session);
1205
1206 j1939_session_list_lock(session->priv);
1207 if (session->state >= J1939_SESSION_ACTIVE &&
1208 session->state < J1939_SESSION_ACTIVE_MAX) {
1209 j1939_session_get(session);
1210 hrtimer_start(&session->rxtimer,
1211 ms_to_ktime(J1939_XTP_ABORT_TIMEOUT_MS),
1212 HRTIMER_MODE_REL_SOFT);
1213 __j1939_session_cancel(session, J1939_XTP_ABORT_TIMEOUT);
1214 }
1215 j1939_session_list_unlock(session->priv);
1216 }
1217
1218 j1939_session_put(session);
1219
1220 return HRTIMER_NORESTART;
1221}
1222
1223static bool j1939_xtp_rx_cmd_bad_pgn(struct j1939_session *session,
1224 const struct sk_buff *skb)
1225{
1226 const struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1227 pgn_t pgn = j1939_xtp_ctl_to_pgn(skb->data);
1228 struct j1939_priv *priv = session->priv;
1229 enum j1939_xtp_abort abort = J1939_XTP_NO_ABORT;
1230 u8 cmd = skb->data[0];
1231
1232 if (session->skcb.addr.pgn == pgn)
1233 return false;
1234
1235 switch (cmd) {
1236 case J1939_TP_CMD_BAM:
1237 abort = J1939_XTP_NO_ABORT;
1238 break;
1239
1240 case J1939_ETP_CMD_RTS:
1241 case J1939_TP_CMD_RTS:
1242 abort = J1939_XTP_ABORT_BUSY;
1243 break;
1244
1245 case J1939_ETP_CMD_CTS:
1246 case J1939_TP_CMD_CTS:
1247 abort = J1939_XTP_ABORT_ECTS_UNXPECTED_PGN;
1248 break;
1249
1250 case J1939_ETP_CMD_DPO:
1251 abort = J1939_XTP_ABORT_BAD_EDPO_PGN;
1252 break;
1253
1254 case J1939_ETP_CMD_EOMA:
1255 case J1939_TP_CMD_EOMA:
1256 abort = J1939_XTP_ABORT_OTHER;
1257 break;
1258
1259 case J1939_ETP_CMD_ABORT:
1260 abort = J1939_XTP_NO_ABORT;
1261 break;
1262
1263 default:
1264 WARN_ON_ONCE(1);
1265 break;
1266 }
1267
1268 netdev_warn(priv->ndev, "%s: 0x%p: CMD 0x%02x with PGN 0x%05x for running session with different PGN 0x%05x.\n",
1269 __func__, session, cmd, pgn, session->skcb.addr.pgn);
1270 if (abort != J1939_XTP_NO_ABORT)
1271 j1939_xtp_tx_abort(priv, skcb, true, abort, pgn);
1272
1273 return true;
1274}
1275
1276static void j1939_xtp_rx_abort_one(struct j1939_priv *priv, struct sk_buff *skb,
1277 bool reverse, bool transmitter)
1278{
1279 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1280 struct j1939_session *session;
1281 u8 abort = skb->data[1];
1282
1283 session = j1939_session_get_by_addr(priv, &skcb->addr, reverse,
1284 transmitter);
1285 if (!session)
1286 return;
1287
1288 if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1289 goto abort_put;
1290
1291 netdev_info(priv->ndev, "%s: 0x%p: 0x%05x: (%u) %s\n", __func__,
1292 session, j1939_xtp_ctl_to_pgn(skb->data), abort,
1293 j1939_xtp_abort_to_str(abort));
1294
1295 j1939_session_timers_cancel(session);
1296 session->err = j1939_xtp_abort_to_errno(priv, abort);
1297 if (session->sk)
1298 j1939_sk_send_loop_abort(session->sk, session->err);
1299 j1939_session_deactivate_activate_next(session);
1300
1301abort_put:
1302 j1939_session_put(session);
1303}
1304
1305
1306static void
1307j1939_xtp_rx_abort(struct j1939_priv *priv, struct sk_buff *skb,
1308 bool transmitter)
1309{
1310 j1939_xtp_rx_abort_one(priv, skb, false, transmitter);
1311 j1939_xtp_rx_abort_one(priv, skb, true, transmitter);
1312}
1313
1314static void
1315j1939_xtp_rx_eoma_one(struct j1939_session *session, struct sk_buff *skb)
1316{
1317 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1318 const u8 *dat;
1319 int len;
1320
1321 if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1322 return;
1323
1324 dat = skb->data;
1325
1326 if (skcb->addr.type == J1939_ETP)
1327 len = j1939_etp_ctl_to_size(dat);
1328 else
1329 len = j1939_tp_ctl_to_size(dat);
1330
1331 if (session->total_message_size != len) {
1332 netdev_warn_once(session->priv->ndev,
1333 "%s: 0x%p: Incorrect size. Expected: %i; got: %i.\n",
1334 __func__, session, session->total_message_size,
1335 len);
1336 }
1337
1338 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
1339
1340 session->pkt.tx_acked = session->pkt.total;
1341 j1939_session_timers_cancel(session);
1342
1343 j1939_session_completed(session);
1344}
1345
1346static void
1347j1939_xtp_rx_eoma(struct j1939_priv *priv, struct sk_buff *skb,
1348 bool transmitter)
1349{
1350 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1351 struct j1939_session *session;
1352
1353 session = j1939_session_get_by_addr(priv, &skcb->addr, true,
1354 transmitter);
1355 if (!session)
1356 return;
1357
1358 j1939_xtp_rx_eoma_one(session, skb);
1359 j1939_session_put(session);
1360}
1361
1362static void
1363j1939_xtp_rx_cts_one(struct j1939_session *session, struct sk_buff *skb)
1364{
1365 enum j1939_xtp_abort err = J1939_XTP_ABORT_FAULT;
1366 unsigned int pkt;
1367 const u8 *dat;
1368
1369 dat = skb->data;
1370
1371 if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1372 return;
1373
1374 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
1375
1376 if (session->last_cmd == dat[0]) {
1377 err = J1939_XTP_ABORT_DUP_SEQ;
1378 goto out_session_cancel;
1379 }
1380
1381 if (session->skcb.addr.type == J1939_ETP)
1382 pkt = j1939_etp_ctl_to_packet(dat);
1383 else
1384 pkt = dat[2];
1385
1386 if (!pkt)
1387 goto out_session_cancel;
1388 else if (dat[1] > session->pkt.block )
1389 goto out_session_cancel;
1390
1391
1392 session->pkt.tx_acked = pkt - 1;
1393 j1939_session_skb_drop_old(session);
1394 session->pkt.last = session->pkt.tx_acked + dat[1];
1395 if (session->pkt.last > session->pkt.total)
1396
1397 session->pkt.last = session->pkt.total;
1398
1399 session->pkt.tx = session->pkt.tx_acked;
1400
1401 session->last_cmd = dat[0];
1402 if (dat[1]) {
1403 j1939_tp_set_rxtimeout(session, 1250);
1404 if (session->transmission) {
1405 if (session->pkt.tx_acked)
1406 j1939_sk_errqueue(session,
1407 J1939_ERRQUEUE_SCHED);
1408 j1939_session_txtimer_cancel(session);
1409 j1939_tp_schedule_txtimer(session, 0);
1410 }
1411 } else {
1412
1413 j1939_tp_set_rxtimeout(session, 550);
1414 }
1415 return;
1416
1417 out_session_cancel:
1418 j1939_session_timers_cancel(session);
1419 j1939_session_cancel(session, err);
1420}
1421
1422static void
1423j1939_xtp_rx_cts(struct j1939_priv *priv, struct sk_buff *skb, bool transmitter)
1424{
1425 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1426 struct j1939_session *session;
1427
1428 session = j1939_session_get_by_addr(priv, &skcb->addr, true,
1429 transmitter);
1430 if (!session)
1431 return;
1432 j1939_xtp_rx_cts_one(session, skb);
1433 j1939_session_put(session);
1434}
1435
1436static struct j1939_session *j1939_session_new(struct j1939_priv *priv,
1437 struct sk_buff *skb, size_t size)
1438{
1439 struct j1939_session *session;
1440 struct j1939_sk_buff_cb *skcb;
1441
1442 session = kzalloc(sizeof(*session), gfp_any());
1443 if (!session)
1444 return NULL;
1445
1446 INIT_LIST_HEAD(&session->active_session_list_entry);
1447 INIT_LIST_HEAD(&session->sk_session_queue_entry);
1448 kref_init(&session->kref);
1449
1450 j1939_priv_get(priv);
1451 session->priv = priv;
1452 session->total_message_size = size;
1453 session->state = J1939_SESSION_NEW;
1454
1455 skb_queue_head_init(&session->skb_queue);
1456 skb_queue_tail(&session->skb_queue, skb);
1457
1458 skcb = j1939_skb_to_cb(skb);
1459 memcpy(&session->skcb, skcb, sizeof(session->skcb));
1460
1461 hrtimer_init(&session->txtimer, CLOCK_MONOTONIC,
1462 HRTIMER_MODE_REL_SOFT);
1463 session->txtimer.function = j1939_tp_txtimer;
1464 hrtimer_init(&session->rxtimer, CLOCK_MONOTONIC,
1465 HRTIMER_MODE_REL_SOFT);
1466 session->rxtimer.function = j1939_tp_rxtimer;
1467
1468 netdev_dbg(priv->ndev, "%s: 0x%p: sa: %02x, da: %02x\n",
1469 __func__, session, skcb->addr.sa, skcb->addr.da);
1470
1471 return session;
1472}
1473
1474static struct
1475j1939_session *j1939_session_fresh_new(struct j1939_priv *priv,
1476 int size,
1477 const struct j1939_sk_buff_cb *rel_skcb)
1478{
1479 struct sk_buff *skb;
1480 struct j1939_sk_buff_cb *skcb;
1481 struct j1939_session *session;
1482
1483 skb = alloc_skb(size + sizeof(struct can_skb_priv), GFP_ATOMIC);
1484 if (unlikely(!skb))
1485 return NULL;
1486
1487 skb->dev = priv->ndev;
1488 can_skb_reserve(skb);
1489 can_skb_prv(skb)->ifindex = priv->ndev->ifindex;
1490 skcb = j1939_skb_to_cb(skb);
1491 memcpy(skcb, rel_skcb, sizeof(*skcb));
1492
1493 session = j1939_session_new(priv, skb, size);
1494 if (!session) {
1495 kfree_skb(skb);
1496 return NULL;
1497 }
1498
1499
1500 skb_put(skb, size);
1501
1502 return session;
1503}
1504
1505int j1939_session_activate(struct j1939_session *session)
1506{
1507 struct j1939_priv *priv = session->priv;
1508 struct j1939_session *active = NULL;
1509 int ret = 0;
1510
1511 j1939_session_list_lock(priv);
1512 if (session->skcb.addr.type != J1939_SIMPLE)
1513 active = j1939_session_get_by_addr_locked(priv,
1514 &priv->active_session_list,
1515 &session->skcb.addr, false,
1516 session->transmission);
1517 if (active) {
1518 j1939_session_put(active);
1519 ret = -EAGAIN;
1520 } else {
1521 WARN_ON_ONCE(session->state != J1939_SESSION_NEW);
1522 list_add_tail(&session->active_session_list_entry,
1523 &priv->active_session_list);
1524 j1939_session_get(session);
1525 session->state = J1939_SESSION_ACTIVE;
1526
1527 netdev_dbg(session->priv->ndev, "%s: 0x%p\n",
1528 __func__, session);
1529 }
1530 j1939_session_list_unlock(priv);
1531
1532 return ret;
1533}
1534
1535static struct
1536j1939_session *j1939_xtp_rx_rts_session_new(struct j1939_priv *priv,
1537 struct sk_buff *skb)
1538{
1539 enum j1939_xtp_abort abort = J1939_XTP_NO_ABORT;
1540 struct j1939_sk_buff_cb skcb = *j1939_skb_to_cb(skb);
1541 struct j1939_session *session;
1542 const u8 *dat;
1543 pgn_t pgn;
1544 int len;
1545
1546 netdev_dbg(priv->ndev, "%s\n", __func__);
1547
1548 dat = skb->data;
1549 pgn = j1939_xtp_ctl_to_pgn(dat);
1550 skcb.addr.pgn = pgn;
1551
1552 if (!j1939_sk_recv_match(priv, &skcb))
1553 return NULL;
1554
1555 if (skcb.addr.type == J1939_ETP) {
1556 len = j1939_etp_ctl_to_size(dat);
1557 if (len > J1939_MAX_ETP_PACKET_SIZE)
1558 abort = J1939_XTP_ABORT_FAULT;
1559 else if (len > priv->tp_max_packet_size)
1560 abort = J1939_XTP_ABORT_RESOURCE;
1561 else if (len <= J1939_MAX_TP_PACKET_SIZE)
1562 abort = J1939_XTP_ABORT_FAULT;
1563 } else {
1564 len = j1939_tp_ctl_to_size(dat);
1565 if (len > J1939_MAX_TP_PACKET_SIZE)
1566 abort = J1939_XTP_ABORT_FAULT;
1567 else if (len > priv->tp_max_packet_size)
1568 abort = J1939_XTP_ABORT_RESOURCE;
1569 }
1570
1571 if (abort != J1939_XTP_NO_ABORT) {
1572 j1939_xtp_tx_abort(priv, &skcb, true, abort, pgn);
1573 return NULL;
1574 }
1575
1576 session = j1939_session_fresh_new(priv, len, &skcb);
1577 if (!session) {
1578 j1939_xtp_tx_abort(priv, &skcb, true,
1579 J1939_XTP_ABORT_RESOURCE, pgn);
1580 return NULL;
1581 }
1582
1583
1584 session->pkt.total = (len + 6) / 7;
1585 session->pkt.block = 0xff;
1586 if (skcb.addr.type != J1939_ETP) {
1587 if (dat[3] != session->pkt.total)
1588 netdev_alert(priv->ndev, "%s: 0x%p: strange total, %u != %u\n",
1589 __func__, session, session->pkt.total,
1590 dat[3]);
1591 session->pkt.total = dat[3];
1592 session->pkt.block = min(dat[3], dat[4]);
1593 }
1594
1595 session->pkt.rx = 0;
1596 session->pkt.tx = 0;
1597
1598 WARN_ON_ONCE(j1939_session_activate(session));
1599
1600 return session;
1601}
1602
1603static int j1939_xtp_rx_rts_session_active(struct j1939_session *session,
1604 struct sk_buff *skb)
1605{
1606 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1607 struct j1939_priv *priv = session->priv;
1608
1609 if (!session->transmission) {
1610 if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1611 return -EBUSY;
1612
1613
1614 j1939_session_timers_cancel(session);
1615 j1939_session_cancel(session, J1939_XTP_ABORT_BUSY);
1616 }
1617
1618 if (session->last_cmd != 0) {
1619
1620 netdev_alert(priv->ndev, "%s: 0x%p: connection exists (%02x %02x). last cmd: %x\n",
1621 __func__, session, skcb->addr.sa, skcb->addr.da,
1622 session->last_cmd);
1623
1624 j1939_session_timers_cancel(session);
1625 j1939_session_cancel(session, J1939_XTP_ABORT_BUSY);
1626
1627 return -EBUSY;
1628 }
1629
1630 if (session->skcb.addr.sa != skcb->addr.sa ||
1631 session->skcb.addr.da != skcb->addr.da)
1632 netdev_warn(priv->ndev, "%s: 0x%p: session->skcb.addr.sa=0x%02x skcb->addr.sa=0x%02x session->skcb.addr.da=0x%02x skcb->addr.da=0x%02x\n",
1633 __func__, session,
1634 session->skcb.addr.sa, skcb->addr.sa,
1635 session->skcb.addr.da, skcb->addr.da);
1636
1637
1638
1639
1640 session->skcb.addr.sa = skcb->addr.sa;
1641 session->skcb.addr.da = skcb->addr.da;
1642
1643 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
1644
1645 return 0;
1646}
1647
1648static void j1939_xtp_rx_rts(struct j1939_priv *priv, struct sk_buff *skb,
1649 bool transmitter)
1650{
1651 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1652 struct j1939_session *session;
1653 u8 cmd = skb->data[0];
1654
1655 session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1656 transmitter);
1657
1658 if (!session) {
1659 if (transmitter) {
1660
1661
1662
1663
1664
1665
1666
1667
1668
1669
1670
1671
1672
1673
1674
1675 return;
1676 }
1677 session = j1939_xtp_rx_rts_session_new(priv, skb);
1678 if (!session) {
1679 if (cmd == J1939_TP_CMD_BAM && j1939_sk_recv_match(priv, skcb))
1680 netdev_info(priv->ndev, "%s: failed to create TP BAM session\n",
1681 __func__);
1682 return;
1683 }
1684 } else {
1685 if (j1939_xtp_rx_rts_session_active(session, skb)) {
1686 j1939_session_put(session);
1687 return;
1688 }
1689 }
1690 session->last_cmd = cmd;
1691
1692 if (cmd == J1939_TP_CMD_BAM) {
1693 if (!session->transmission)
1694 j1939_tp_set_rxtimeout(session, 750);
1695 } else {
1696 if (!session->transmission) {
1697 j1939_session_txtimer_cancel(session);
1698 j1939_tp_schedule_txtimer(session, 0);
1699 }
1700 j1939_tp_set_rxtimeout(session, 1250);
1701 }
1702
1703 j1939_session_put(session);
1704}
1705
1706static void j1939_xtp_rx_dpo_one(struct j1939_session *session,
1707 struct sk_buff *skb)
1708{
1709 const u8 *dat = skb->data;
1710
1711 if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1712 return;
1713
1714 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
1715
1716
1717 session->pkt.dpo = j1939_etp_ctl_to_packet(skb->data);
1718 session->last_cmd = dat[0];
1719 j1939_tp_set_rxtimeout(session, 750);
1720}
1721
1722static void j1939_xtp_rx_dpo(struct j1939_priv *priv, struct sk_buff *skb,
1723 bool transmitter)
1724{
1725 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1726 struct j1939_session *session;
1727
1728 session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1729 transmitter);
1730 if (!session) {
1731 netdev_info(priv->ndev,
1732 "%s: no connection found\n", __func__);
1733 return;
1734 }
1735
1736 j1939_xtp_rx_dpo_one(session, skb);
1737 j1939_session_put(session);
1738}
1739
1740static void j1939_xtp_rx_dat_one(struct j1939_session *session,
1741 struct sk_buff *skb)
1742{
1743 struct j1939_priv *priv = session->priv;
1744 struct j1939_sk_buff_cb *skcb;
1745 struct sk_buff *se_skb;
1746 const u8 *dat;
1747 u8 *tpdat;
1748 int offset;
1749 int nbytes;
1750 bool final = false;
1751 bool remain = false;
1752 bool do_cts_eoma = false;
1753 int packet;
1754
1755 skcb = j1939_skb_to_cb(skb);
1756 dat = skb->data;
1757 if (skb->len <= 1)
1758
1759 goto out_session_cancel;
1760
1761 switch (session->last_cmd) {
1762 case 0xff:
1763 break;
1764 case J1939_ETP_CMD_DPO:
1765 if (skcb->addr.type == J1939_ETP)
1766 break;
1767 fallthrough;
1768 case J1939_TP_CMD_BAM:
1769 case J1939_TP_CMD_CTS:
1770 if (skcb->addr.type != J1939_ETP)
1771 break;
1772 fallthrough;
1773 default:
1774 netdev_info(priv->ndev, "%s: 0x%p: last %02x\n", __func__,
1775 session, session->last_cmd);
1776 goto out_session_cancel;
1777 }
1778
1779 packet = (dat[0] - 1 + session->pkt.dpo);
1780 if (packet > session->pkt.total ||
1781 (session->pkt.rx + 1) > session->pkt.total) {
1782 netdev_info(priv->ndev, "%s: 0x%p: should have been completed\n",
1783 __func__, session);
1784 goto out_session_cancel;
1785 }
1786
1787 se_skb = j1939_session_skb_find_by_offset(session, packet * 7);
1788 if (!se_skb) {
1789 netdev_warn(priv->ndev, "%s: 0x%p: no skb found\n", __func__,
1790 session);
1791 goto out_session_cancel;
1792 }
1793
1794 skcb = j1939_skb_to_cb(se_skb);
1795 offset = packet * 7 - skcb->offset;
1796 nbytes = se_skb->len - offset;
1797 if (nbytes > 7)
1798 nbytes = 7;
1799 if (nbytes <= 0 || (nbytes + 1) > skb->len) {
1800 netdev_info(priv->ndev, "%s: 0x%p: nbytes %i, len %i\n",
1801 __func__, session, nbytes, skb->len);
1802 goto out_session_cancel;
1803 }
1804
1805 tpdat = se_skb->data;
1806 if (!session->transmission) {
1807 memcpy(&tpdat[offset], &dat[1], nbytes);
1808 } else {
1809 int err;
1810
1811 err = memcmp(&tpdat[offset], &dat[1], nbytes);
1812 if (err)
1813 netdev_err_once(priv->ndev,
1814 "%s: 0x%p: Data of RX-looped back packet (%*ph) doesn't match TX data (%*ph)!\n",
1815 __func__, session,
1816 nbytes, &dat[1],
1817 nbytes, &tpdat[offset]);
1818 }
1819
1820 if (packet == session->pkt.rx)
1821 session->pkt.rx++;
1822
1823 if (skcb->addr.type != J1939_ETP &&
1824 j1939_cb_is_broadcast(&session->skcb)) {
1825 if (session->pkt.rx >= session->pkt.total)
1826 final = true;
1827 else
1828 remain = true;
1829 } else {
1830
1831 if (session->pkt.rx >= session->pkt.last)
1832 do_cts_eoma = true;
1833 }
1834
1835 if (final) {
1836 j1939_session_timers_cancel(session);
1837 j1939_session_completed(session);
1838 } else if (remain) {
1839 if (!session->transmission)
1840 j1939_tp_set_rxtimeout(session, 750);
1841 } else if (do_cts_eoma) {
1842 j1939_tp_set_rxtimeout(session, 1250);
1843 if (!session->transmission)
1844 j1939_tp_schedule_txtimer(session, 0);
1845 } else {
1846 j1939_tp_set_rxtimeout(session, 250);
1847 }
1848 session->last_cmd = 0xff;
1849 j1939_session_put(session);
1850
1851 return;
1852
1853 out_session_cancel:
1854 j1939_session_timers_cancel(session);
1855 j1939_session_cancel(session, J1939_XTP_ABORT_FAULT);
1856 j1939_session_put(session);
1857}
1858
1859static void j1939_xtp_rx_dat(struct j1939_priv *priv, struct sk_buff *skb)
1860{
1861 struct j1939_sk_buff_cb *skcb;
1862 struct j1939_session *session;
1863
1864 skcb = j1939_skb_to_cb(skb);
1865
1866 if (j1939_tp_im_transmitter(skcb)) {
1867 session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1868 true);
1869 if (!session)
1870 netdev_info(priv->ndev, "%s: no tx connection found\n",
1871 __func__);
1872 else
1873 j1939_xtp_rx_dat_one(session, skb);
1874 }
1875
1876 if (j1939_tp_im_receiver(skcb)) {
1877 session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1878 false);
1879 if (!session)
1880 netdev_info(priv->ndev, "%s: no rx connection found\n",
1881 __func__);
1882 else
1883 j1939_xtp_rx_dat_one(session, skb);
1884 }
1885
1886 if (j1939_cb_is_broadcast(skcb)) {
1887 session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1888 false);
1889 if (session)
1890 j1939_xtp_rx_dat_one(session, skb);
1891 }
1892}
1893
1894
1895struct j1939_session *j1939_tp_send(struct j1939_priv *priv,
1896 struct sk_buff *skb, size_t size)
1897{
1898 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1899 struct j1939_session *session;
1900 int ret;
1901
1902 if (skcb->addr.pgn == J1939_TP_PGN_DAT ||
1903 skcb->addr.pgn == J1939_TP_PGN_CTL ||
1904 skcb->addr.pgn == J1939_ETP_PGN_DAT ||
1905 skcb->addr.pgn == J1939_ETP_PGN_CTL)
1906
1907 return ERR_PTR(-EDOM);
1908
1909 if (size > priv->tp_max_packet_size)
1910 return ERR_PTR(-EMSGSIZE);
1911
1912 if (size <= 8)
1913 skcb->addr.type = J1939_SIMPLE;
1914 else if (size > J1939_MAX_TP_PACKET_SIZE)
1915 skcb->addr.type = J1939_ETP;
1916 else
1917 skcb->addr.type = J1939_TP;
1918
1919 if (skcb->addr.type == J1939_ETP &&
1920 j1939_cb_is_broadcast(skcb))
1921 return ERR_PTR(-EDESTADDRREQ);
1922
1923
1924 ret = j1939_ac_fixup(priv, skb);
1925 if (unlikely(ret))
1926 return ERR_PTR(ret);
1927
1928
1929 if (j1939_address_is_unicast(skcb->addr.da) &&
1930 priv->ents[skcb->addr.da].nusers)
1931 skcb->flags |= J1939_ECU_LOCAL_DST;
1932
1933
1934 skcb->flags |= J1939_ECU_LOCAL_SRC;
1935
1936
1937 session = j1939_session_new(priv, skb, size);
1938 if (!session)
1939 return ERR_PTR(-ENOMEM);
1940
1941
1942 sock_hold(skb->sk);
1943 session->sk = skb->sk;
1944 session->transmission = true;
1945 session->pkt.total = (size + 6) / 7;
1946 session->pkt.block = skcb->addr.type == J1939_ETP ? 255 :
1947 min(j1939_tp_block ?: 255, session->pkt.total);
1948
1949 if (j1939_cb_is_broadcast(&session->skcb))
1950
1951 session->pkt.last = session->pkt.total;
1952
1953 skcb->tskey = session->sk->sk_tskey++;
1954 session->tskey = skcb->tskey;
1955
1956 return session;
1957}
1958
1959static void j1939_tp_cmd_recv(struct j1939_priv *priv, struct sk_buff *skb)
1960{
1961 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1962 int extd = J1939_TP;
1963 u8 cmd = skb->data[0];
1964
1965 switch (cmd) {
1966 case J1939_ETP_CMD_RTS:
1967 extd = J1939_ETP;
1968 fallthrough;
1969 case J1939_TP_CMD_BAM:
1970 case J1939_TP_CMD_RTS:
1971 if (skcb->addr.type != extd)
1972 return;
1973
1974 if (cmd == J1939_TP_CMD_RTS && j1939_cb_is_broadcast(skcb)) {
1975 netdev_alert(priv->ndev, "%s: rts without destination (%02x)\n",
1976 __func__, skcb->addr.sa);
1977 return;
1978 }
1979
1980 if (j1939_tp_im_transmitter(skcb))
1981 j1939_xtp_rx_rts(priv, skb, true);
1982
1983 if (j1939_tp_im_receiver(skcb) || j1939_cb_is_broadcast(skcb))
1984 j1939_xtp_rx_rts(priv, skb, false);
1985
1986 break;
1987
1988 case J1939_ETP_CMD_CTS:
1989 extd = J1939_ETP;
1990 fallthrough;
1991 case J1939_TP_CMD_CTS:
1992 if (skcb->addr.type != extd)
1993 return;
1994
1995 if (j1939_tp_im_transmitter(skcb))
1996 j1939_xtp_rx_cts(priv, skb, false);
1997
1998 if (j1939_tp_im_receiver(skcb))
1999 j1939_xtp_rx_cts(priv, skb, true);
2000
2001 break;
2002
2003 case J1939_ETP_CMD_DPO:
2004 if (skcb->addr.type != J1939_ETP)
2005 return;
2006
2007 if (j1939_tp_im_transmitter(skcb))
2008 j1939_xtp_rx_dpo(priv, skb, true);
2009
2010 if (j1939_tp_im_receiver(skcb))
2011 j1939_xtp_rx_dpo(priv, skb, false);
2012
2013 break;
2014
2015 case J1939_ETP_CMD_EOMA:
2016 extd = J1939_ETP;
2017 fallthrough;
2018 case J1939_TP_CMD_EOMA:
2019 if (skcb->addr.type != extd)
2020 return;
2021
2022 if (j1939_tp_im_transmitter(skcb))
2023 j1939_xtp_rx_eoma(priv, skb, false);
2024
2025 if (j1939_tp_im_receiver(skcb))
2026 j1939_xtp_rx_eoma(priv, skb, true);
2027
2028 break;
2029
2030 case J1939_ETP_CMD_ABORT:
2031 if (j1939_tp_im_transmitter(skcb))
2032 j1939_xtp_rx_abort(priv, skb, true);
2033
2034 if (j1939_tp_im_receiver(skcb))
2035 j1939_xtp_rx_abort(priv, skb, false);
2036
2037 break;
2038 default:
2039 return;
2040 }
2041}
2042
2043int j1939_tp_recv(struct j1939_priv *priv, struct sk_buff *skb)
2044{
2045 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
2046
2047 if (!j1939_tp_im_involved_anydir(skcb) && !j1939_cb_is_broadcast(skcb))
2048 return 0;
2049
2050 switch (skcb->addr.pgn) {
2051 case J1939_ETP_PGN_DAT:
2052 skcb->addr.type = J1939_ETP;
2053 fallthrough;
2054 case J1939_TP_PGN_DAT:
2055 j1939_xtp_rx_dat(priv, skb);
2056 break;
2057
2058 case J1939_ETP_PGN_CTL:
2059 skcb->addr.type = J1939_ETP;
2060 fallthrough;
2061 case J1939_TP_PGN_CTL:
2062 if (skb->len < 8)
2063 return 0;
2064
2065 j1939_tp_cmd_recv(priv, skb);
2066 break;
2067 default:
2068 return 0;
2069 }
2070 return 1;
2071}
2072
2073void j1939_simple_recv(struct j1939_priv *priv, struct sk_buff *skb)
2074{
2075 struct j1939_session *session;
2076
2077 if (!skb->sk)
2078 return;
2079
2080 if (skb->sk->sk_family != AF_CAN ||
2081 skb->sk->sk_protocol != CAN_J1939)
2082 return;
2083
2084 j1939_session_list_lock(priv);
2085 session = j1939_session_get_simple(priv, skb);
2086 j1939_session_list_unlock(priv);
2087 if (!session) {
2088 netdev_warn(priv->ndev,
2089 "%s: Received already invalidated message\n",
2090 __func__);
2091 return;
2092 }
2093
2094 j1939_session_timers_cancel(session);
2095 j1939_session_deactivate(session);
2096 j1939_session_put(session);
2097}
2098
2099int j1939_cancel_active_session(struct j1939_priv *priv, struct sock *sk)
2100{
2101 struct j1939_session *session, *saved;
2102
2103 netdev_dbg(priv->ndev, "%s, sk: %p\n", __func__, sk);
2104 j1939_session_list_lock(priv);
2105 list_for_each_entry_safe(session, saved,
2106 &priv->active_session_list,
2107 active_session_list_entry) {
2108 if (!sk || sk == session->sk) {
2109 if (hrtimer_try_to_cancel(&session->txtimer) == 1)
2110 j1939_session_put(session);
2111 if (hrtimer_try_to_cancel(&session->rxtimer) == 1)
2112 j1939_session_put(session);
2113
2114 session->err = ESHUTDOWN;
2115 j1939_session_deactivate_locked(session);
2116 }
2117 }
2118 j1939_session_list_unlock(priv);
2119 return NOTIFY_DONE;
2120}
2121
2122void j1939_tp_init(struct j1939_priv *priv)
2123{
2124 spin_lock_init(&priv->active_session_list_lock);
2125 INIT_LIST_HEAD(&priv->active_session_list);
2126 priv->tp_max_packet_size = J1939_MAX_ETP_PACKET_SIZE;
2127}
2128
