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