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