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