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