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