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