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