Signed-off-by: Oleksij Rempel <o.rempel@xxxxxxxxxxxxxx>
---
include/uapi/linux/can/j1939.h | 6 +++++
net/can/j1939/j1939-priv.h | 2 ++
net/can/j1939/socket.c | 48 +++++++++++++++++++++++++++++++---
net/can/j1939/transport.c | 33 ++++++++++++++++++++---
4 files changed, 83 insertions(+), 6 deletions(-)
diff --git a/include/uapi/linux/can/j1939.h b/include/uapi/linux/can/j1939.h
index c7eb94d2ab10..77f8068bcc62 100644
--- a/include/uapi/linux/can/j1939.h
+++ b/include/uapi/linux/can/j1939.h
@@ -72,6 +72,8 @@ enum {
SCM_J1939_DEST_ADDR = 1,
SCM_J1939_DEST_NAME = 2,
SCM_J1939_PRIO = 3,
+ SCM_J1939_RECVERR = 4,
+ SCM_J1939_PKTINFO = 5,
};
struct j1939_filter {
@@ -83,6 +85,10 @@ struct j1939_filter {
__u8 addr_mask;
};
+struct j1939_pktinfo {
+ __u64 cookie;
+};
+
#define J1939_FILTER_MAX 512 /* maximum number of j1939_filter set via setsockopt() */
#endif /* !_UAPI_CAN_J1939_H_ */
diff --git a/net/can/j1939/j1939-priv.h b/net/can/j1939/j1939-priv.h
index 4cb2e63a86c4..cf42550de6d2 100644
--- a/net/can/j1939/j1939-priv.h
+++ b/net/can/j1939/j1939-priv.h
@@ -207,6 +207,8 @@ struct j1939_session {
bool transmission;
bool extd;
unsigned int total_message_size; /* Total message size, number of bytes */
+ int err;
+ u64 cookie;
/* Packets counters for a (extended) transfer session. The packet is
* maximal of 7 bytes. */
diff --git a/net/can/j1939/socket.c b/net/can/j1939/socket.c
index ea9ce6d99332..c88c67e93536 100644
--- a/net/can/j1939/socket.c
+++ b/net/can/j1939/socket.c
@@ -608,6 +608,10 @@ static int j1939_sk_recvmsg(struct socket *sock, struct msghdr *msg,
struct j1939_sk_buff_cb *skcb;
int ret = 0;
+ if (flags & MSG_ERRQUEUE)
+ return sock_recv_errqueue(sock->sk, msg, size, SOL_CAN_J1939,
+ SCM_J1939_RECVERR);
+
skb = skb_recv_datagram(sk, flags, 0, &ret);
if (!skb)
return ret;
@@ -721,8 +725,9 @@ void j1939_sk_send_multi_abort(struct j1939_priv *priv, struct sock *sk,
sk->sk_error_report(sk);
}
-static int j1939_sk_send_multi(struct j1939_priv *priv, struct sock *sk,
- struct msghdr *msg, size_t size)
+static int j1939_sk_send_multi(struct j1939_priv *priv, struct sock *sk,
+ struct msghdr *msg, size_t size,
+ struct j1939_pktinfo *info)
{
struct j1939_sock *jsk = j1939_sk(sk);
@@ -788,6 +793,7 @@ static int j1939_sk_send_multi(struct j1939_priv *priv, struct sock *sk,
ret = PTR_ERR(session);
goto kfree_skb;
}
+ session->cookie = info->cookie;
}
} else {
j1939_session_skb_queue(session, skb);
@@ -845,6 +851,35 @@ static int j1939_sk_send_one(struct j1939_priv *priv, struct sock *sk,
return ret ? ret : size;
}
+static int j1939_sk_cmsg_send(struct sock *sk, struct msghdr *msg,
+ struct j1939_pktinfo *info)
+{
+ struct cmsghdr *cmsg;
+
+ for_each_cmsghdr(cmsg, msg) {
+ if (!CMSG_OK(msg, cmsg))
+ return -EINVAL;
+
+ if (cmsg->cmsg_level != SOL_CAN_J1939)
+ continue;
+ switch (cmsg->cmsg_type) {
+ case SCM_J1939_PKTINFO:
+ {
+ struct j1939_pktinfo *tinfo;
+
+ if (cmsg->cmsg_len != CMSG_LEN(sizeof(struct j1939_pktinfo)))
+ return -EINVAL;
+ tinfo = (struct j1939_pktinfo *)CMSG_DATA(cmsg);
+ memcpy(info, tinfo, sizeof(*info));
+ break;
+ }
+ default:
+ return -EINVAL;
+ }
+ }
+ return 0;
+}
+
static int j1939_sk_sendmsg(struct socket *sock, struct msghdr *msg,
size_t size)
{
@@ -852,6 +887,7 @@ static int j1939_sk_sendmsg(struct socket *sock, struct msghdr *msg,
struct j1939_sock *jsk = j1939_sk(sk);
struct j1939_priv *priv;
struct net_device *ndev;
+ struct j1939_pktinfo info;
int ifindex;
int ret;
@@ -894,6 +930,12 @@ static int j1939_sk_sendmsg(struct socket *sock, struct msghdr *msg,
return -EACCES;
}
+ if (msg->msg_controllen) {
+ ret = j1939_sk_cmsg_send(sk, msg, &info);
+ if (unlikely(ret))
+ return ret;
+ }
+
ndev = dev_get_by_index(sock_net(sk), ifindex);
if (!ndev)
return -ENXIO;
@@ -904,7 +946,7 @@ static int j1939_sk_sendmsg(struct socket *sock, struct msghdr *msg,
if (size > 8)
/* re-route via transport protocol */
- ret = j1939_sk_send_multi(priv, sk, msg, size);
+ ret = j1939_sk_send_multi(priv, sk, msg, size, &info);
else
ret = j1939_sk_send_one(priv, sk, msg, size);
diff --git a/net/can/j1939/transport.c b/net/can/j1939/transport.c
index 734b0fd23078..330368c3a9a0 100644
--- a/net/can/j1939/transport.c
+++ b/net/can/j1939/transport.c
@@ -5,6 +5,7 @@
// Copyright (c) 2018 Protonic, Robin van der Gracht <robin@xxxxxxxxxxx>
#include <linux/can/skb.h>
+#include <linux/errqueue.h>
#include "j1939-priv.h"
@@ -247,6 +248,28 @@ void j1939_session_get(struct j1939_session *session)
kref_get(&session->kref);
}
+static void j1939_sk_err_queue(struct j1939_session *session)
+{
+ struct sock *sk = session->sk;
+ struct j1939_pktinfo info;
+ struct sock_exterr_skb *serr;
+ struct sk_buff *skb;
+
+ skb = alloc_skb(sizeof(info), GFP_ATOMIC);
+ if (!skb)
+ return;
+
+ info.cookie = session->cookie;
+ skb_put_data(skb, &info, sizeof(info));
+
+ serr = SKB_EXT_ERR(skb);
+ serr->ee.ee_errno = session->err;
+ serr->ee.ee_origin = SO_EE_ORIGIN_TXSTATUS;
+
+ if (sock_queue_err_skb(sk, skb))
+ kfree_skb(skb);
+};
+
/* session completion functions */
static void __j1939_session_drop(struct j1939_session *session)
{
@@ -261,6 +284,9 @@ static void __j1939_session_drop(struct j1939_session *session)
static void j1939_session_destroy(struct j1939_session *session)
{
+
+ if (session->sk)
+ j1939_sk_err_queue(session);
j1939_session_list_lock(session->priv);
j1939_session_list_del(session);
j1939_session_list_unlock(session->priv);
@@ -867,6 +893,7 @@ static void j1939_session_cancel(struct j1939_session *session,
WARN_ON_ONCE(!err);
+ session->err = j1939_xtp_abort_to_errno(priv, err);
/* do not send aborts on incoming broadcasts */
if (!j1939_cb_is_broadcast(&session->skcb))
j1939_xtp_tx_abort(priv, &session->skcb, session->extd,
@@ -874,8 +901,7 @@ static void j1939_session_cancel(struct j1939_session *session,
err, session->skcb.addr.pgn);
if (session->sk)
- j1939_sk_send_multi_abort(priv, session->sk,
- j1939_xtp_abort_to_errno(priv, err));
+ j1939_sk_send_multi_abort(priv, session->sk, session->err);
}
static enum hrtimer_restart j1939_tp_rxtimer(struct hrtimer *hrtimer)
@@ -950,9 +976,10 @@ static void j1939_xtp_rx_abort_one(struct j1939_priv *priv, struct sk_buff *skb,
u8 abort = skb->data[1];
j1939_session_timers_cancel(session);
+ session->err = j1939_xtp_abort_to_errno(priv, abort);
if (session->sk)
j1939_sk_send_multi_abort(priv, session->sk,
- j1939_xtp_abort_to_errno(priv, abort));
+ session->err);
}
/* TODO: maybe cancel current connection
--
2.20.1
