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