Aksal/nuttx
3
1
Fork 1
mirror of https://github.com/apache/nuttx.git synced 2026-10-03 04:07:55 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions 3

net/pkt: support SO_TIMESTAMPING and MSG_ERRQUEUE

Browse source 
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 <wenquan1@xiaomi.com>
This commit is contained in:
wenquan1 2025-11-26 09:20:13 +08:00 • committed by Xiang Xiao
parent 28402b9b35
commit be3446850a
13 changed files with 187 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

8
include/nuttx/mm/iob.h
View file

@ -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;

20
include/sys/socket.h
View file

@ -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 */

22
mm/iob/iob_alloc.c
View file

@ -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);

4
net/pkt/pkt.h
View file

@ -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

36
net/pkt/pkt_input.c
View file

@ -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.

18
net/pkt/pkt_netpoll.c
View file

@ -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.
*/

92
net/pkt/pkt_recvmsg.c
View file

@ -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)

7
net/pkt/pkt_sendmsg_buffered.c
View file

@ -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.
*/

8
net/pkt/pkt_sendmsg_unbuffered.c
View file

@ -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;

4
net/pkt/pkt_sockif.c
View file

@ -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. */

8
net/socket/Kconfig
View file

@ -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

3
net/socket/getsockopt.c
View file

@ -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;

3
net/socket/setsockopt.c
View file

@ -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;