[PATCH RESEND 2/2] can: j1939: make sure that sent CTL frames are marked as TX

[Devid Antonio Filoni <devid.filoni@xxxxxxxxxxxxxxxxxxxxx>]

Fixes: 9d71dd0 ("can: add support of SAE J1939 protocol")
Signed-off-by: Devid Antonio Filoni <devid.filoni@xxxxxxxxxxxxxxxxxxxxx>
---
 net/can/j1939/transport.c | 55 +++++++++++++++++++++++++++++++--------
 1 file changed, 44 insertions(+), 11 deletions(-)

diff --git a/net/can/j1939/transport.c b/net/can/j1939/transport.c
index 030f5fe901e1..b8368f9c78c2 100644
--- a/net/can/j1939/transport.c
+++ b/net/can/j1939/transport.c
@@ -648,6 +648,7 @@ static int j1939_tp_tx_dat(struct j1939_session *session,
 }
 
 static int j1939_xtp_do_tx_ctl(struct j1939_priv *priv,
+			       struct sock *re_sk,
 			       const struct j1939_sk_buff_cb *re_skcb,
 			       bool swap_src_dst, pgn_t pgn, const u8 *dat)
 {
@@ -661,6 +662,8 @@ static int j1939_xtp_do_tx_ctl(struct j1939_priv *priv,
 	if (IS_ERR(skb))
 		return PTR_ERR(skb);
 
+	can_skb_set_owner(skb, re_sk);
+
 	skdat = skb_put(skb, 8);
 	memcpy(skdat, dat, 5);
 	skdat[5] = (pgn >> 0);
@@ -674,13 +677,26 @@ static inline int j1939_tp_tx_ctl(struct j1939_session *session,
 				  bool swap_src_dst, const u8 *dat)
 {
 	struct j1939_priv *priv = session->priv;
+	struct sk_buff *se_skb = j1939_session_skb_get(session);
+	struct sock *se_skb_sk = NULL;
+	int ret;
+
+	if (se_skb)
+		se_skb_sk = se_skb->sk;
 
-	return j1939_xtp_do_tx_ctl(priv, &session->skcb,
-				   swap_src_dst,
-				   session->skcb.addr.pgn, dat);
+	ret = j1939_xtp_do_tx_ctl(priv, se_skb_sk, &session->skcb,
+				  swap_src_dst,
+				  session->skcb.addr.pgn, dat);
+
+	if (ret)
+		kfree_skb(se_skb);
+	else
+		consume_skb(se_skb);
+	return ret;
 }
 
 static int j1939_xtp_tx_abort(struct j1939_priv *priv,
+			      struct sock *re_sk,
 			      const struct j1939_sk_buff_cb *re_skcb,
 			      bool swap_src_dst,
 			      enum j1939_xtp_abort err,
@@ -694,7 +710,7 @@ static int j1939_xtp_tx_abort(struct j1939_priv *priv,
 	memset(dat, 0xff, sizeof(dat));
 	dat[0] = J1939_TP_CMD_ABORT;
 	dat[1] = err;
-	return j1939_xtp_do_tx_ctl(priv, re_skcb, swap_src_dst, pgn, dat);
+	return j1939_xtp_do_tx_ctl(priv, re_sk, re_skcb, swap_src_dst, pgn, dat);
 }
 
 void j1939_tp_schedule_txtimer(struct j1939_session *session, int msec)
@@ -1117,6 +1133,8 @@ static void __j1939_session_cancel(struct j1939_session *session,
 				   enum j1939_xtp_abort err)
 {
 	struct j1939_priv *priv = session->priv;
+	struct sk_buff *se_skb;
+	struct sock *se_skb_sk = NULL;
 
 	WARN_ON_ONCE(!err);
 	lockdep_assert_held(&session->priv->active_session_list_lock);
@@ -1125,9 +1143,15 @@ static void __j1939_session_cancel(struct j1939_session *session,
 	session->state = J1939_SESSION_WAITING_ABORT;
 	/* do not send aborts on incoming broadcasts */
 	if (!j1939_cb_is_broadcast(&session->skcb)) {
-		j1939_xtp_tx_abort(priv, &session->skcb,
-				   !session->transmission,
-				   err, session->skcb.addr.pgn);
+		se_skb = j1939_session_skb_get(session);
+		if (se_skb)
+			se_skb_sk = se_skb->sk;
+		if (j1939_xtp_tx_abort(priv, se_skb_sk, &session->skcb,
+				       !session->transmission,
+				       err, session->skcb.addr.pgn))
+			kfree_skb(se_skb);
+		else
+			consume_skb(se_skb);
 	}
 
 	if (session->sk)
@@ -1274,6 +1298,8 @@ static bool j1939_xtp_rx_cmd_bad_pgn(struct j1939_session *session,
 	const struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
 	pgn_t pgn = j1939_xtp_ctl_to_pgn(skb->data);
 	struct j1939_priv *priv = session->priv;
+	struct sk_buff *se_skb;
+	struct sock *se_skb_sk = NULL;
 	enum j1939_xtp_abort abort = J1939_XTP_NO_ABORT;
 	u8 cmd = skb->data[0];
 
@@ -1318,8 +1344,15 @@ static bool j1939_xtp_rx_cmd_bad_pgn(struct j1939_session *session,
 
 	netdev_warn(priv->ndev, "%s: 0x%p: CMD 0x%02x with PGN 0x%05x for running session with different PGN 0x%05x.\n",
 		    __func__, session, cmd, pgn, session->skcb.addr.pgn);
-	if (abort != J1939_XTP_NO_ABORT)
-		j1939_xtp_tx_abort(priv, skcb, true, abort, pgn);
+	if (abort != J1939_XTP_NO_ABORT) {
+		se_skb = j1939_session_skb_get(session);
+		if (se_skb)
+			se_skb_sk = se_skb->sk;
+		if (j1939_xtp_tx_abort(priv, se_skb_sk, skcb, true, abort, pgn))
+			kfree_skb(se_skb);
+		else
+			consume_skb(se_skb);
+	}
 
 	return true;
 }
@@ -1625,13 +1658,13 @@ j1939_session *j1939_xtp_rx_rts_session_new(struct j1939_priv *priv,
 	}
 
 	if (abort != J1939_XTP_NO_ABORT) {
-		j1939_xtp_tx_abort(priv, &skcb, true, abort, pgn);
+		j1939_xtp_tx_abort(priv, skb->sk, &skcb, true, abort, pgn);
 		return NULL;
 	}
 
 	session = j1939_session_fresh_new(priv, len, &skcb);
 	if (!session) {
-		j1939_xtp_tx_abort(priv, &skcb, true,
+		j1939_xtp_tx_abort(priv, skb->sk, &skcb, true,
 				   J1939_XTP_ABORT_RESOURCE, pgn);
 		return NULL;
 	}
-- 
2.25.1