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