This is an automated email from the ASF dual-hosted git repository. xiaoxiang781216 pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/nuttx.git
commit be3446850a545c0d7eafd89c8f0be86548efd22e Author: wenquan1 <[email protected]> AuthorDate: Wed Nov 26 09:20:13 2025 +0800 net/pkt: support SO_TIMESTAMPING and MSG_ERRQUEUE Add SO_TIMESTAMPING TX path for PKT sockets. Tagged TX packets loop back through the driver with io_conn set, are routed into conn->errahead, and delivered to userspace via recvmsg(MSG_ERRQUEUE) with SO_TIMESTAMPING cmsg. Add poll(POLLPRI) notification when errahead is non-empty. Signed-off-by: wenquan1 <[email protected]> --- include/nuttx/mm/iob.h | 8 +++- include/sys/socket.h | 20 +++++++-- mm/iob/iob_alloc.c | 22 ++++------ net/pkt/pkt.h | 4 ++ net/pkt/pkt_input.c | 36 +++++++++++++--- net/pkt/pkt_netpoll.c | 18 +++++++- net/pkt/pkt_recvmsg.c | 92 +++++++++++++++++++++++++++++++--------- net/pkt/pkt_sendmsg_buffered.c | 7 +++ net/pkt/pkt_sendmsg_unbuffered.c | 8 ++++ net/pkt/pkt_sockif.c | 4 ++ net/socket/Kconfig | 8 +++- net/socket/getsockopt.c | 3 ++ net/socket/setsockopt.c | 3 ++ 13 files changed, 187 insertions(+), 46 deletions(-) diff --git a/include/nuttx/mm/iob.h b/include/nuttx/mm/iob.h index 6985cc3dcf2..9a1265835e5 100644 --- a/include/nuttx/mm/iob.h +++ b/include/nuttx/mm/iob.h @@ -37,7 +37,7 @@ # include <nuttx/wqueue.h> #endif -#ifdef CONFIG_NET_TIMESTAMP +#if defined(CONFIG_NET_TIMESTAMP) || defined(CONFIG_NET_TIMESTAMPING) # include <sys/time.h> #endif @@ -130,7 +130,7 @@ struct iob_s #endif unsigned int io_pktlen; /* Total length of the packet */ -#ifdef CONFIG_NET_TIMESTAMP +#if defined(CONFIG_NET_TIMESTAMP) || defined(CONFIG_NET_TIMESTAMPING) /* timestamp of the packet. * d_features is the member of net_driver_s struct, if the NETDEV_RX_STAMP * bit of d_features is set, the timestamp is provided by hardware driver. @@ -140,6 +140,10 @@ struct iob_s struct timespec io_time; #endif +#endif +#ifdef CONFIG_NET_TIMESTAMPING + FAR struct socket_conn_s *io_conn; +#endif #ifdef CONFIG_IOB_ALLOC iob_free_cb_t io_free; /* Custom free callback */ FAR uint8_t *io_data; diff --git a/include/sys/socket.h b/include/sys/socket.h index 3cdb3ce75b8..fddc049959f 100644 --- a/include/sys/socket.h +++ b/include/sys/socket.h @@ -223,6 +223,22 @@ #define SO_TIMESTAMPNS 20 /* Generates a timestamp in ns for each incoming packet * arg: integer value */ +#define SO_TIMESTAMPING 21 /* Generates timestamp for each output packet + */ + +/* Protocol-level socket options may begin with this value */ + +#define __SO_PROTOCOL 16 + +/* Timestamp generation */ + +#define SOF_TIMESTAMPING_TX_HARDWARE (1 << SO_TIMESTAMPING) +#define SOF_TIMESTAMPING_TX_SOFTWARE SOF_TIMESTAMPING_TX_HARDWARE + +/* Timestamp reporting */ + +#define SOF_TIMESTAMPING_SOFTWARE SOF_TIMESTAMPING_TX_SOFTWARE +#define SOF_TIMESTAMPING_RAW_HARDWARE SOF_TIMESTAMPING_TX_HARDWARE /* The options are unsupported but included for compatibility * and portability @@ -249,10 +265,6 @@ #define SOL_PACKET 19 -/* Protocol-level socket options may begin with this value */ - -#define __SO_PROTOCOL 16 - /* Values for the 'how' argument of shutdown() */ #define SHUT_RD 1 /* Bit 0: Disables further receive operations */ diff --git a/mm/iob/iob_alloc.c b/mm/iob/iob_alloc.c index 27575084c03..9eb822b9867 100644 --- a/mm/iob/iob_alloc.c +++ b/mm/iob/iob_alloc.c @@ -96,6 +96,9 @@ static FAR struct iob_s *iob_alloc_committed(void) iob->io_len = 0; /* Length of the data in the entry */ iob->io_offset = 0; /* Offset to the beginning of data */ iob->io_pktlen = 0; /* Total length of the packet */ +#ifdef CONFIG_NET_TIMESTAMPING + iob->io_conn = NULL; +#endif } spin_unlock_irqrestore(&g_iob_lock, flags); @@ -135,6 +138,9 @@ static FAR struct iob_s *iob_tryalloc_internal(bool throttled) iob->io_len = 0; /* Length of the data in the entry */ iob->io_offset = 0; /* Offset to the beginning of data */ iob->io_pktlen = 0; /* Total length of the packet */ +#ifdef CONFIG_NET_TIMESTAMPING + iob->io_conn = NULL; +#endif return iob; } } @@ -340,11 +346,8 @@ FAR struct iob_s *iob_alloc_dynamic(uint16_t size) iob = kmm_memalign(IOB_ALIGNMENT, alignsize); if (iob) { - iob->io_flink = NULL; /* Not in a chain */ - iob->io_len = 0; /* Length of the data in the entry */ - iob->io_offset = 0; /* Offset to the beginning of data */ + memset(iob, 0, offsetof(struct iob_s, io_data)); iob->io_bufsize = size; /* Total length of the iob buffer */ - iob->io_pktlen = 0; /* Total length of the packet */ iob->io_free = iob_free_dynamic; /* Customer free callback */ iob->io_data = (FAR uint8_t *)ALIGN_UP((uintptr_t)(iob + 1), IOB_ALIGNMENT); @@ -383,14 +386,10 @@ FAR struct iob_s *iob_alloc_with_data(FAR void *data, uint16_t size, DEBUGASSERT(free_cb != NULL); - iob = kmm_malloc(sizeof(struct iob_s)); + iob = kmm_zalloc(sizeof(struct iob_s)); if (iob) { - iob->io_flink = NULL; /* Not in a chain */ - iob->io_len = 0; /* Length of the data in the entry */ - iob->io_offset = 0; /* Offset to the beginning of data */ iob->io_bufsize = size; /* Total length of the iob buffer */ - iob->io_pktlen = 0; /* Total length of the packet */ iob->io_free = free_cb; /* Customer free callback */ iob->io_data = data; } @@ -426,10 +425,7 @@ FAR struct iob_s *iob_init_with_data(FAR void *data, uint16_t size, { FAR struct iob_s *iob = (FAR struct iob_s *)data; - iob->io_flink = NULL; /* Not in a chain */ - iob->io_len = 0; /* Length of the data in the entry */ - iob->io_offset = 0; /* Offset to the beginning of data */ - iob->io_pktlen = 0; /* Total length of the packet */ + memset(iob, 0, offsetof(struct iob_s, io_data)); iob->io_free = free_cb; /* Customer free callback */ iob->io_data = (FAR uint8_t *)ALIGN_UP((uintptr_t)(iob + 1), IOB_ALIGNMENT); diff --git a/net/pkt/pkt.h b/net/pkt/pkt.h index 63d946dafa2..2b2a3685334 100644 --- a/net/pkt/pkt.h +++ b/net/pkt/pkt.h @@ -104,6 +104,10 @@ struct pkt_conn_s struct iob_queue_s readahead; /* Read-ahead buffering */ +#ifdef CONFIG_NET_TIMESTAMPING + struct iob_queue_s errahead; /* Error-ahead buffering */ +#endif + FAR struct iob_s *pendiob; /* The iob currently being sent */ /* The following is a list of poll structures of threads waiting for diff --git a/net/pkt/pkt_input.c b/net/pkt/pkt_input.c index fa3b88e0e2d..8fc36e0e4d2 100644 --- a/net/pkt/pkt_input.c +++ b/net/pkt/pkt_input.c @@ -52,6 +52,7 @@ * Input Parameters: * dev - Device instance only the input packet in d_buf, length = d_len; * conn - A pointer to the PKT connection structure + * iobq - A pointer to the buffer queue * * Returned Value: * The number of bytes actually buffered is returned. This will be either @@ -60,7 +61,8 @@ ****************************************************************************/ static uint16_t pkt_datahandler(FAR struct net_driver_s *dev, - FAR struct pkt_conn_s *conn) + FAR struct pkt_conn_s *conn, + FAR struct iob_queue_s *iobq) { FAR struct iob_s *iob = iob_tryalloc(true); int ret; @@ -88,7 +90,7 @@ static uint16_t pkt_datahandler(FAR struct net_driver_s *dev, */ conn_lock(&conn->sconn); - ret = iob_tryadd_queue(iob, &conn->readahead); + ret = iob_tryadd_queue(iob, iobq); conn_unlock(&conn->sconn); if (ret < 0) @@ -157,10 +159,32 @@ static int pkt_in(FAR struct net_driver_s *dev) return OK; } +#ifdef CONFIG_NET_TIMESTAMPING + + /* Handle hardware timestamp */ + + if (dev->d_iob->io_conn == &conn->sconn) + { + if (pkt_datahandler(dev, conn, &conn->errahead) > 0) + { + pkt_callback(dev, conn, PKT_NEWDATA); + } + + pkt_conn_list_unlock(); + return OK; + } + + if (dev->d_iob->io_conn != NULL) + { + /* Skip no related pkt conn */ + + pkt_conn_list_unlock(); + return OK; + } +#endif + #ifdef CONFIG_NET_TIMESTAMP - if ((dev->d_features & NETDEV_RX_STAMP) == 0 && - (_SO_GETOPT(conn->sconn.s_options, SO_TIMESTAMP) || - _SO_GETOPT(conn->sconn.s_options, SO_TIMESTAMPNS))) + if ((dev->d_features & NETDEV_RX_STAMP) == 0) { /* Storing reception timestamp provided by realtime * if timestamp no provided by hardware. @@ -187,7 +211,7 @@ static int pkt_in(FAR struct net_driver_s *dev) { /* Add the PKT to the socket read-ahead buffer. */ - if (pkt_datahandler(dev, conn) == 0) + if (pkt_datahandler(dev, conn, &conn->readahead) == 0) { /* No.. the packet was not processed now. Return -EAGAIN so * that the driver may retry again later. diff --git a/net/pkt/pkt_netpoll.c b/net/pkt/pkt_netpoll.c index 991db490aa5..e04e74b23f0 100644 --- a/net/pkt/pkt_netpoll.c +++ b/net/pkt/pkt_netpoll.c @@ -124,7 +124,7 @@ static uint32_t pkt_poll_eventhandler(FAR struct net_driver_s *dev, if ((flags & NETDEV_DOWN) != 0) { - eventset |= (POLLHUP | POLLERR); + eventset |= POLLHUP | POLLERR; } /* A poll is a sign that we are free to send data. */ @@ -134,6 +134,15 @@ static uint32_t pkt_poll_eventhandler(FAR struct net_driver_s *dev, eventset |= POLLOUT; } +#ifdef CONFIG_NET_TIMESTAMPING + /* Check for timestamping data */ + + if (!IOB_QEMPTY(&info->conn->errahead)) + { + eventset |= POLLPRI | POLLERR; + } +#endif + /* Awaken the caller of poll() is requested event occurred. */ poll_notify(&info->fds, 1, eventset); @@ -237,6 +246,13 @@ int pkt_pollsetup(FAR struct socket *psock, FAR struct pollfd *fds) cb->flags |= PKT_NEWDATA; } +#ifdef CONFIG_NET_TIMESTAMPING + if ((fds->events & POLLPRI) != 0) + { + cb->flags |= PKT_NEWDATA; + } +#endif + /* Save the reference in the poll info structure as fds private as well * for use during poll teardown as well. */ diff --git a/net/pkt/pkt_recvmsg.c b/net/pkt/pkt_recvmsg.c index f3f675eddbe..c2c0bf02d77 100644 --- a/net/pkt/pkt_recvmsg.c +++ b/net/pkt/pkt_recvmsg.c @@ -185,6 +185,18 @@ static uint32_t pkt_recvfrom_eventhandler(FAR struct net_driver_s *dev, { /* If a new packet is available, then complete the read action. */ +#ifdef CONFIG_NET_TIMESTAMPING + if ((flags & PKT_NEWDATA) != 0 && dev->d_iob->io_conn != NULL) + { + pstate->pr_cb->flags = 0; + pstate->pr_cb->priv = NULL; + pstate->pr_cb->event = NULL; + pstate->pr_result = -EAGAIN; + nxsem_post(&pstate->pr_sem); + } + else +#endif + if ((flags & PKT_NEWDATA) != 0) { /* Copy the packet */ @@ -197,9 +209,9 @@ static uint32_t pkt_recvfrom_eventhandler(FAR struct net_driver_s *dev, /* Don't allow any further call backs. */ - pstate->pr_cb->flags = 0; - pstate->pr_cb->priv = NULL; - pstate->pr_cb->event = NULL; + pstate->pr_cb->flags = 0; + pstate->pr_cb->priv = NULL; + pstate->pr_cb->event = NULL; /* Save the sender's address in the caller's 'from' location */ @@ -308,23 +320,56 @@ static ssize_t pkt_recvfrom_result(int result, } /**************************************************************************** - * Name: pkt_readahead + * Name: pkt_readdata * * Description: - * Copy the buffered read-ahead data to the user buffer. + * Copy the buffered data to the user buffer based on the flag errmsg. * * Input Parameters: * pstate The state structure of the recv operation * * Returned Value: - * None + * copy length or -ENODATA * * Assumptions: * The network is locked. * ****************************************************************************/ -static inline void pkt_readahead(FAR struct pkt_recvfrom_s *pstate) +static void append_timestamp(FAR struct pkt_recvfrom_s *pstate, + FAR struct iob_s *iob) +{ +#ifdef CONFIG_NET_TIMESTAMP + FAR struct pkt_conn_s *conn = pstate->pr_conn; + cmsg_store_timestamp(pstate->pr_msg, &iob->io_time, + conn->sconn.s_options); +#endif +} + +#ifdef CONFIG_NET_TIMESTAMPING +static void append_timestamping(FAR struct pkt_recvfrom_s *pstate, + FAR struct iob_s *iob) +{ + FAR struct pkt_conn_s *conn = pstate->pr_conn; + struct timespec ts[3]; + + memset(&ts, 0, sizeof(ts)); + + ts[0].tv_sec = iob->io_time.tv_sec; + ts[0].tv_nsec = iob->io_time.tv_nsec; + ts[2].tv_sec = iob->io_time.tv_sec; + ts[2].tv_nsec = iob->io_time.tv_nsec; + + cmsg_append(pstate->pr_msg, SOL_SOCKET, SO_TIMESTAMPING, &ts, + sizeof(ts)); + pstate->pr_msg->msg_flags |= MSG_ERRQUEUE; +} +#endif + +static inline int pkt_readdata(FAR struct pkt_recvfrom_s *pstate, + FAR struct iob_queue_s *iobq, + CODE void (*tsfunc)(FAR struct pkt_recvfrom_s *, + FAR struct iob_s *)) { FAR struct pkt_conn_s *conn = pstate->pr_conn; FAR struct iob_s *iob; @@ -335,14 +380,11 @@ static inline void pkt_readahead(FAR struct pkt_recvfrom_s *pstate) pstate->pr_recvlen = -ENODATA; - if ((iob = iob_peek_queue(&conn->readahead)) != NULL) + if ((iob = iob_remove_queue(iobq)) != NULL) { DEBUGASSERT(iob->io_pktlen > 0); -#ifdef CONFIG_NET_TIMESTAMP - cmsg_store_timestamp(pstate->pr_msg, &iob->io_time, - conn->sconn.s_options); -#endif + /* Copy to user */ @@ -370,16 +412,14 @@ static inline void pkt_readahead(FAR struct pkt_recvfrom_s *pstate) ninfo("Received %d bytes (of %u)\n", recvlen, iob->io_pktlen); - /* Remove the I/O buffer chain from the head of the read-ahead - * buffer queue. - */ - - iob_remove_queue(&conn->readahead); + tsfunc(pstate, iob); /* And free the I/O buffer chain */ iob_free_chain(iob); } + + return pstate->pr_recvlen; } /**************************************************************************** @@ -462,14 +502,28 @@ ssize_t pkt_recvmsg(FAR struct socket *psock, FAR struct msghdr *msg, conn_dev_lock(&conn->sconn, dev); +#ifdef CONFIG_NET_TIMESTAMPING + if (flags & MSG_ERRQUEUE) + { + if (!IOB_QEMPTY(&conn->errahead)) + { + ret = pkt_readdata(&state, &conn->errahead, append_timestamping); + } + else + { + ret = -EAGAIN; + } + } + else +#endif + /* Check if there is buffered read-ahead data for this socket. We may have * already received the response to previous command. */ if (!IOB_QEMPTY(&conn->readahead)) { - pkt_readahead(&state); - ret = pkt_recvfrom_result(ret, &state); + ret = pkt_readdata(&state, &conn->readahead, append_timestamp); } else if (_SS_ISNONBLOCK(conn->sconn.s_flags) || (flags & MSG_DONTWAIT) != 0) diff --git a/net/pkt/pkt_sendmsg_buffered.c b/net/pkt/pkt_sendmsg_buffered.c index 5a237e11f74..3743f7ffda1 100644 --- a/net/pkt/pkt_sendmsg_buffered.c +++ b/net/pkt/pkt_sendmsg_buffered.c @@ -294,6 +294,13 @@ ssize_t pkt_sendmsg(FAR struct socket *psock, FAR const struct msghdr *msg, iob_reserve(iob, CONFIG_NET_LL_GUARDSIZE); iob_update_pktlen(iob, 0, false); +#ifdef CONFIG_NET_TIMESTAMPING + if (_SO_GETOPT(conn->sconn.s_options, SO_TIMESTAMPING)) + { + iob->io_conn = &conn->sconn; + } +#endif + /* Copy the user data into the write buffer. We cannot wait for * buffer space if the socket was opened non-blocking. */ diff --git a/net/pkt/pkt_sendmsg_unbuffered.c b/net/pkt/pkt_sendmsg_unbuffered.c index 349440e5d4b..62d35e8af92 100644 --- a/net/pkt/pkt_sendmsg_unbuffered.c +++ b/net/pkt/pkt_sendmsg_unbuffered.c @@ -131,6 +131,14 @@ static uint32_t psock_send_eventhandler(FAR struct net_driver_s *dev, pstate->snd_sent = pstate->snd_buflen; pstate->snd_conn->pendiob = dev->d_iob; +#ifdef CONFIG_NET_TIMESTAMPING + if (_SO_GETOPT(pstate->snd_conn->sconn.s_options, + SO_TIMESTAMPING)) + { + dev->d_iob->io_conn = &pstate->snd_conn->sconn; + } +#endif + if (pstate->snd_sock->s_type == SOCK_DGRAM) { FAR struct eth_hdr_s *ethhdr = NETLLBUF; diff --git a/net/pkt/pkt_sockif.c b/net/pkt/pkt_sockif.c index 1efca0a898c..7da08a0dd85 100644 --- a/net/pkt/pkt_sockif.c +++ b/net/pkt/pkt_sockif.c @@ -368,6 +368,10 @@ static int pkt_close(FAR struct socket *psock) iob_free_queue(&conn->readahead); +#ifdef CONFIG_NET_TIMESTAMPING + iob_free_queue(&conn->errahead); +#endif + #ifdef CONFIG_NET_PKT_WRITE_BUFFERS /* Free write buffer callback. */ diff --git a/net/socket/Kconfig b/net/socket/Kconfig index eb438e91e87..663860b923a 100644 --- a/net/socket/Kconfig +++ b/net/socket/Kconfig @@ -80,11 +80,17 @@ config NET_SOLINGER config NET_TIMESTAMP bool "SO_TIMESTAMP socket option" default n - depends on NET_CAN || NET_ETHERNET ---help--- Enable or disable support for the SO_TIMESTAMP socket option. Supported on SocketCAN and Ethernet/UDP. +config NET_TIMESTAMPING + bool "SO_TIMESTAMPING socket option" + default n + ---help--- + Enable or disable support for the SO_TIMESTAMPING socket option. + Supported on Ethernet/PKT. + config NET_BINDTODEVICE bool "SO_BINDTODEVICE socket option Bind-to-device support" default n diff --git a/net/socket/getsockopt.c b/net/socket/getsockopt.c index 8f258ae1be4..e2d5bdd85a7 100644 --- a/net/socket/getsockopt.c +++ b/net/socket/getsockopt.c @@ -150,6 +150,9 @@ static int psock_socketlevel_option(FAR struct socket *psock, int option, #ifdef CONFIG_NET_TIMESTAMP case SO_TIMESTAMP: /* Generates a timestamp in us for each incoming packet */ case SO_TIMESTAMPNS: /* Generates a timestamp in ns for each incoming packet */ +#endif +#ifdef CONFIG_NET_TIMESTAMPING + case SO_TIMESTAMPING: /* Timestamping options */ #endif { sockopt_t optionset; diff --git a/net/socket/setsockopt.c b/net/socket/setsockopt.c index a5368be6b92..c22efce8f3b 100644 --- a/net/socket/setsockopt.c +++ b/net/socket/setsockopt.c @@ -140,6 +140,9 @@ static int psock_socketlevel_option(FAR struct socket *psock, int option, #ifdef CONFIG_NET_TIMESTAMP case SO_TIMESTAMP: /* Generates a timestamp in us for each incoming packet */ case SO_TIMESTAMPNS: /* Generates a timestamp in ns for each incoming packet */ +#endif +#ifdef CONFIG_NET_TIMESTAMPING + case SO_TIMESTAMPING: /* Timestamp all packets */ #endif { int setting;
