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