nuttx-commits mailing list archives

Site index · List index
Message view « Date » · « Thread »
Top « Date » · « Thread »
From gn...@apache.org
Subject [incubator-nuttx] 19/31: Added support for SO_TIMESTAMP in socketlayer and SocketCAN Cleanup FlexCAN driver driver Disabled workqueue based TX in FlexCAN
Date Sun, 12 Apr 2020 17:54:51 GMT
This is an automated email from the ASF dual-hosted git repository.

gnutt pushed a commit to branch SocketCAN
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git

commit 68d751692c98a192bda63dc4ebdb05fb2fb5bfee
Author: Peter van der Perk <peter.vanderperk@nxp.com>
AuthorDate: Wed Mar 11 16:13:18 2020 +0100

    Added support for SO_TIMESTAMP in socketlayer and SocketCAN
        Cleanup FlexCAN driver driver
        Disabled workqueue based TX in FlexCAN
---
 arch/arm/src/s32k1xx/s32k1xx_flexcan.c | 228 +++++------------------------
 include/nuttx/net/net.h                |   7 +
 libs/libc/net/lib_recvmsg.c            |   4 +-
 net/bluetooth/bluetooth_sockif.c       |   3 +
 net/can/can.h                          |  28 ++++
 net/can/can_callback.c                 |  16 +++
 net/can/can_recvfrom.c                 | 252 +++++++++++++++++++++++++++++++++
 net/can/can_sockif.c                   |  10 +-
 net/icmp/icmp_sockif.c                 |   3 +
 net/icmpv6/icmpv6_sockif.c             |   3 +
 net/ieee802154/ieee802154_sockif.c     |   3 +
 net/inet/inet_sockif.c                 |   3 +
 net/local/local_sockif.c               |   3 +
 net/netlink/netlink_sockif.c           |   3 +
 net/pkt/pkt_sockif.c                   |   3 +
 net/socket/Kconfig                     |  16 +++
 net/socket/Make.defs                   |   5 +
 net/socket/getsockopt.c                |  13 ++
 net/socket/setsockopt.c                |  24 ++++
 net/socket/socket.h                    |   2 +-
 20 files changed, 430 insertions(+), 199 deletions(-)

diff --git a/arch/arm/src/s32k1xx/s32k1xx_flexcan.c b/arch/arm/src/s32k1xx/s32k1xx_flexcan.c
index 2e73dc9..8e79833 100644
--- a/arch/arm/src/s32k1xx/s32k1xx_flexcan.c
+++ b/arch/arm/src/s32k1xx/s32k1xx_flexcan.c
@@ -59,9 +59,14 @@
  * is required.
  */
 
+
+/* FIXME A workqueue is required for enet but for FLEXCAN it increased the
+ * transmit latency by ~ 40us from 24 to 67us
+ * Therefore for now its configurable by the WORK_QUEUE define
+ * If we know for sure that a workqueue isn't required
+ * Then all WORK_QUEUE related code will be removed */
 #if !defined(CONFIG_SCHED_WORKQUEUE)
-#  error Work queue support is required
-   //FIXME maybe for enet not sure for FLEXCAN
+//#  error Work queue support is required
 #else
 
   /* Select work queue.  Always use the LP work queue if available.  If not,
@@ -72,28 +77,14 @@
    * processing that never suspends.  Suspending the high priority work queue
    * may bring the system to its knees!
    */
-
-#  define ETHWORK LPWORK
+//#  define WORK_QUEUE
+#  define CANWORK LPWORK
 #endif
 
 /* CONFIG_S32K1XX_FLEXCAN_NETHIFS determines the number of physical interfaces
  * that will be supported.
  */
 
-#if 0
-#if CONFIG_S32K1XX_FLEXCAN_NETHIFS != 1
-#  error "CONFIG_S32K1XX_FLEXCAN_NETHIFS must be one for now"
-#endif
-
-#if CONFIG_S32K1XX_FLEXCAN_NTXBUFFERS < 1
-#  error "Need at least one TX buffer"
-#endif
-
-#if CONFIG_S32K1XX_FLEXCAN_NRXBUFFERS < 1
-#  error "Need at least one RX buffer"
-#endif
-#endif
-
 #define MASKSTDID                   0x000007ff
 #define MASKEXTID                   0x1fffffff
 #define FLAGEFF                     (1 << 31) /* Extended frame format */
@@ -122,45 +113,13 @@
 
 static int peak_tx_mailbox_index_ = 0;
 
-/* Normally you would clean the cache after writing new values to the DMA
- * memory so assure that the dirty cache lines are flushed to memory
- * before the DMA occurs.  And you would invalid the cache after a data is
- * received via DMA so that you fetch the actual content of the data from
- * the cache.
- *
- * These conditions are not fully supported here.  If the write-throuch
- * D-Cache is enabled, however, then many of these issues go away:  The
- * cache clean operation does nothing (because there are not dirty cache
- * lines) and the cache invalid operation is innocuous (because there are
- * never dirty cache lines to be lost; valid data will always be reloaded).
- *
- * At present, we simply insist that write through cache be enabled.
- */
-
-#if defined(CONFIG_ARMV7M_DCACHE) && !defined(CONFIG_ARMV7M_DCACHE_WRITETHROUGH)
-#  error Write back D-Cache not yet supported
-#endif
-
+#ifdef WORK_QUEUE
 /* TX poll delay = 1 seconds. CLK_TCK is the number of clock ticks per
  * second.
  */
 
 #define S32K1XX_WDDELAY     (1*CLK_TCK)
-
-/* Align assuming that the D-Cache is enabled (probably 32-bytes).
- *
- * REVISIT: The size of descriptors and buffers must also be in even units
- * of the cache line size  That is because the operations to clean and
- * invalidate the cache will operate on a full 32-byte cache line.  If
- * CONFIG_FLEXCAN_ENHANCEDBD is selected, then the size of the descriptor is
- * 32-bytes (and probably already the correct size for the cache line);
- * otherwise, the size of the descriptors much smaller, only 8 bytes.
- */
-
-#define FLEXCAN_ALIGN        ARMV7M_DCACHE_LINESIZE
-#define FLEXCAN_ALIGN_MASK   (FLEXCAN_ALIGN - 1)
-#define FLEXCAN_ALIGN_UP(n)  (((n) + FLEXCAN_ALIGN_MASK) & ~FLEXCAN_ALIGN_MASK)
-
+#endif
 
 /****************************************************************************
  * Private Types
@@ -235,8 +194,10 @@ struct s32k1xx_driver_s
   uint8_t txhead;              /* The next TX descriptor to use */
   uint8_t rxtail;              /* The next RX descriptor to use */
   uint8_t phyaddr;             /* Selected PHY address */
+#ifdef WORK_QUEUE
   WDOG_ID txpoll;              /* TX poll timer */
   WDOG_ID txtimeout;           /* TX timeout timer */
+#endif
   struct work_s irqwork;       /* For deferring interrupt work to the work queue */
   struct work_s pollwork;      /* For deferring poll work to the work queue */
 #ifdef CAN_FD
@@ -276,21 +237,6 @@ static uint8_t g_rx_pool[sizeof(struct can_frame)*POOL_SIZE]
  * Private Function Prototypes
  ****************************************************************************/
 
-/* Utility functions */
-
-#ifndef S32K1XX_BUFFERS_SWAP
-#  define s32k1xx_swap32(value) (value)
-#  define s32k1xx_swap16(value) (value)
-#else
-#if 0 /* Use builtins if the compiler supports them */
-static inline uint32_t s32k1xx_swap32(uint32_t value);
-static inline uint16_t s32k1xx_swap16(uint16_t value);
-#else
-#  define s32k1xx_swap32 __builtin_bswap32
-#  define s32k1xx_swap16 __builtin_bswap16
-#endif
-#endif
-
 /****************************************************************************
  * Name: arm_clz
  *
@@ -328,21 +274,17 @@ static uint32_t s32k1xx_waitmcr_change(uint32_t mask,
 
 /* Interrupt handling */
 
-static void s32k1xx_dispatch(FAR struct s32k1xx_driver_s *priv);
 static void s32k1xx_receive(FAR struct s32k1xx_driver_s *priv, uint32_t flags);
 static void s32k1xx_txdone(FAR struct s32k1xx_driver_s *priv, uint32_t flags);
 
-static void s32k1xx_flexcan_interrupt_work(FAR void *arg);
 static int  s32k1xx_flexcan_interrupt(int irq, FAR void *context,
                                       FAR void *arg);
 
 /* Watchdog timer expirations */
-
-static void s32k1xx_txtimeout_work(FAR void *arg);
-static void s32k1xx_txtimeout_expiry(int argc, uint32_t arg, ...);
-
+#ifdef WORK_QUEUE
 static void s32k1xx_poll_work(FAR void *arg);
 static void s32k1xx_polltimer_expiry(int argc, uint32_t arg, ...);
+#endif
 
 /* NuttX callback functions */
 
@@ -359,7 +301,6 @@ static int  s32k1xx_ioctl(struct net_driver_s *dev, int cmd,
 
 /* Initialization */
 
-static void s32k1xx_initbuffers(struct s32k1xx_driver_s *priv);
 static int  s32k1xx_initialize(struct s32k1xx_driver_s *priv);
 static void s32k1xx_reset(struct s32k1xx_driver_s *priv);
 
@@ -632,28 +573,6 @@ static int s32k1xx_txpoll(struct net_driver_s *dev)
   return 0;
 }
 
-/****************************************************************************
- * Function: s32k1xx_dispatch
- *
- * Description:
- *   A new Rx packet was received; dispatch that packet to the network layer
- *   as necessary.
- *
- * Input Parameters:
- *   priv  - Reference to the driver state structure
- *
- * Returned Value:
- *   None
- *
- * Assumptions:
- *   Global interrupts are disabled by interrupt handling logic.
- *
- ****************************************************************************/
-
-static inline void s32k1xx_dispatch(FAR struct s32k1xx_driver_s *priv)
-{
-  #warning Missing logic
-}
 
 /****************************************************************************
  * Function: s32k1xx_receive
@@ -841,7 +760,9 @@ static void s32k1xx_txdone(FAR struct s32k1xx_driver_s *priv, uint32_t
flags)
    * canceled.
    */
 
+#ifdef WORK_QUEUE
   wd_cancel(priv->txtimeout);
+#endif
 
   /* FIXME process aborts */
 
@@ -854,7 +775,7 @@ static void s32k1xx_txdone(FAR struct s32k1xx_driver_s *priv, uint32_t
flags)
         {
           putreg32(mb_bit, S32K1XX_CAN0_IFLAG1);
           flags &= ~mb_bit;
-#if 0
+#if 0 //FIXME TB ABORT SUPPORT
           const bool txok = priv->tx[mbi].cs.code != CAN_TXMB_ABORT;
           handleTxMailboxInterrupt(mbi, txok, utc_usec);
 #endif
@@ -872,27 +793,6 @@ static void s32k1xx_txdone(FAR struct s32k1xx_driver_s *priv, uint32_t
flags)
   devif_poll(&priv->dev, s32k1xx_txpoll);
 }
 
-/****************************************************************************
- * Function: s32k1xx_flexcan_interrupt_work
- *
- * Description:
- *   Perform interrupt related work from the worker thread
- *
- * Input Parameters:
- *   arg - The argument passed when work_queue() was called.
- *
- * Returned Value:
- *   OK on success
- *
- * Assumptions:
- *   The network is locked.
- *
- ****************************************************************************/
-
-static void s32k1xx_flexcan_interrupt_work(FAR void *arg)
-{
-  #warning Missing logic
-}
 
 /****************************************************************************
  * Function: s32k1xx_flexcan_interrupt
@@ -937,53 +837,6 @@ static int s32k1xx_flexcan_interrupt(int irq, FAR void *context, FAR
void *arg)
 }
 
 /****************************************************************************
- * Function: s32k1xx_txtimeout_work
- *
- * Description:
- *   Perform TX timeout related work from the worker thread
- *
- * Input Parameters:
- *   arg - The argument passed when work_queue() as called.
- *
- * Returned Value:
- *   OK on success
- *
- * Assumptions:
- *
- ****************************************************************************/
-
-static void s32k1xx_txtimeout_work(FAR void *arg)
-{
-  #warning Missing logic
-  ninfo("FLEXCAN: tx timeout work\r\n");
-}
-
-/****************************************************************************
- * Function: s32k1xx_txtimeout_expiry
- *
- * Description:
- *   Our TX watchdog timed out.  Called from the timer interrupt handler.
- *   The last TX never completed.  Reset the hardware and start again.
- *
- * Input Parameters:
- *   argc - The number of available arguments
- *   arg  - The first argument
- *
- * Returned Value:
- *   None
- *
- * Assumptions:
- *   Global interrupts are disabled by the watchdog logic.
- *
- ****************************************************************************/
-
-static void s32k1xx_txtimeout_expiry(int argc, uint32_t arg, ...)
-{
-  #warning Missing logic
-  ninfo("FLEXCAN: tx timeout expiry\r\n");
-}
-
-/****************************************************************************
  * Function: s32k1xx_poll_work
  *
  * Description:
@@ -999,7 +852,7 @@ static void s32k1xx_txtimeout_expiry(int argc, uint32_t arg, ...)
  *   The network is locked.
  *
  ****************************************************************************/
-
+#ifdef WORK_QUEUE
 static void s32k1xx_poll_work(FAR void *arg)
 {
   #warning Missing logic
@@ -1027,6 +880,7 @@ static void s32k1xx_poll_work(FAR void *arg)
            1, (wdparm_t)priv);
   net_unlock();
 }
+#endif
 
 /****************************************************************************
  * Function: s32k1xx_polltimer_expiry
@@ -1046,6 +900,7 @@ static void s32k1xx_poll_work(FAR void *arg)
  *
  ****************************************************************************/
 
+#ifdef WORK_QUEUE
 static void s32k1xx_polltimer_expiry(int argc, uint32_t arg, ...)
 {
   #warning Missing logic
@@ -1053,8 +908,9 @@ static void s32k1xx_polltimer_expiry(int argc, uint32_t arg, ...)
 
   /* Schedule to perform the poll processing on the worker thread. */
 
-  work_queue(ETHWORK, &priv->pollwork, s32k1xx_poll_work, priv, 0);
+  work_queue(CANWORK, &priv->pollwork, s32k1xx_poll_work, priv, 0);
 }
+#endif
 
 static void s32k1xx_setenable(uint32_t enable)
 {
@@ -1140,7 +996,6 @@ static int s32k1xx_ifup(struct net_driver_s *dev)
 {
   FAR struct s32k1xx_driver_s *priv =
     (FAR struct s32k1xx_driver_s *)dev->d_private;
-  uint32_t regval;
 
   #warning Missing logic
 
@@ -1150,10 +1005,12 @@ static int s32k1xx_ifup(struct net_driver_s *dev)
       return -1;
     }
 
-  /* Set and activate a timer process */
+#ifdef WORK_QUEUE
 
+  /* Set and activate a timer process */
   wd_start(priv->txpoll, S32K1XX_WDDELAY, s32k1xx_polltimer_expiry, 1,
            (wdparm_t)priv);
+#endif
 
   priv->bifup = true;
 
@@ -1270,10 +1127,10 @@ static int s32k1xx_txavail(struct net_driver_s *dev)
     {
       /* Schedule to serialize the poll on the worker thread. */
 
-#ifdef WORK_QUEUE_BYPASS
-      s32k1xx_txavail_work(priv);
+#ifdef WORK_QUEUE
+      work_queue(CANWORK, &priv->pollwork, s32k1xx_txavail_work, priv, 0);
 #else
-      work_queue(ETHWORK, &priv->pollwork, s32k1xx_txavail_work, priv, 0);
+      s32k1xx_txavail_work(priv);
 #endif
     }
 
@@ -1413,6 +1270,7 @@ static int s32k1xx_initialize(struct s32k1xx_driver_s *priv)
   for (i = TXMBCOUNT; i < TOTALMBCOUNT; i++)
     {
       priv->rx[i].id.w = 0x0;
+      //FIXME sometimes we get a hard fault here
     }
 
   putreg32(0x0, S32K1XX_CAN0_RXFGMASK);
@@ -1450,27 +1308,6 @@ static int s32k1xx_initialize(struct s32k1xx_driver_s *priv)
 }
 
 /****************************************************************************
- * Function: s32k1xx_initbuffers
- *
- * Description:
- *   Initialize FLEXCAN buffers and descriptors
- *
- * Input Parameters:
- *   priv - Reference to the private FLEXCAN driver state structure
- *
- * Returned Value:
- *   None
- *
- * Assumptions:
- *
- ****************************************************************************/
-
-static void s32k1xx_initbuffers(struct s32k1xx_driver_s *priv)
-{
-  #warning Missing logic
-}
-
-/****************************************************************************
  * Function: s32k1xx_reset
  *
  * Description:
@@ -1621,14 +1458,15 @@ int s32k1xx_netinitialize(int intf)
   priv->dev.d_ifdown  = s32k1xx_ifdown;    /* I/F down callback */
   priv->dev.d_txavail = s32k1xx_txavail;   /* New TX data callback */
 #ifdef CONFIG_NETDEV_IOCTL
-  priv->dev.d_ioctl   = s32k1xx_ioctl;     /* Support PHY ioctl() calls */
+  priv->dev.d_ioctl   = s32k1xx_ioctl;     /* Support CAN ioctl() calls */
 #endif
   priv->dev.d_private = (void *)g_flexcan; /* Used to recover private state from dev */
 
+#ifdef WORK_QUEUE
   /* Create a watchdog for timing polling for and timing of transmissions */
-
   priv->txpoll        = wd_create();       /* Create periodic poll timer */
   priv->txtimeout     = wd_create();       /* Create TX timeout timer */
+#endif
   priv->rx            = (struct mb_s *)(S32K1XX_CAN0_MB);
   priv->tx            = (struct mb_s *)(S32K1XX_CAN0_MB +
                           (sizeof(struct mb_s) * RXMBCOUNT));
diff --git a/include/nuttx/net/net.h b/include/nuttx/net/net.h
index 02f7ac2..20da67a 100644
--- a/include/nuttx/net/net.h
+++ b/include/nuttx/net/net.h
@@ -212,6 +212,10 @@ struct sock_intf_s
   CODE ssize_t    (*si_recvfrom)(FAR struct socket *psock, FAR void *buf,
                     size_t len, int flags, FAR struct sockaddr *from,
                     FAR socklen_t *fromlen);
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  CODE ssize_t    (*si_recvmsg)(FAR struct socket *psock,
+		            FAR struct msghdr *msg, int flags);
+#endif
   CODE int        (*si_close)(FAR struct socket *psock);
 #ifdef CONFIG_NET_USRSOCK
   CODE int        (*si_ioctl)(FAR struct socket *psock, int cmd,
@@ -268,6 +272,9 @@ struct socket
 #ifdef CONFIG_NET_SOLINGER
   socktimeo_t   s_linger;    /* Linger timeout value (in deciseconds) */
 #endif
+#ifdef CONFIG_NET_TIMESTAMP
+  int32_t       s_timestamp; /* Socket timestamp enabled/disabled */
+#endif
 #endif
 
   FAR void     *s_conn;      /* Connection inherits from struct socket_conn_s */
diff --git a/libs/libc/net/lib_recvmsg.c b/libs/libc/net/lib_recvmsg.c
index 984a16f..2513dfa 100644
--- a/libs/libc/net/lib_recvmsg.c
+++ b/libs/libc/net/lib_recvmsg.c
@@ -39,7 +39,7 @@
 
 #include <nuttx/config.h>
 
-#ifdef CONFIG_NET
+#if defined(CONFIG_NET) && !defined(CONFIG_NET_RECVMSG_CMSG)
 
 #include <sys/types.h>
 #include <sys/socket.h>
@@ -86,4 +86,4 @@ ssize_t recvmsg(int sockfd, FAR struct msghdr *msg, int flags)
     }
 }
 
-#endif /* CONFIG_NET */
+#endif /* CONFIG_NET && !CONFIG_NET_RECVMSG_CMSG */
diff --git a/net/bluetooth/bluetooth_sockif.c b/net/bluetooth/bluetooth_sockif.c
index 41bcafa..4f4b936 100644
--- a/net/bluetooth/bluetooth_sockif.c
+++ b/net/bluetooth/bluetooth_sockif.c
@@ -109,6 +109,9 @@ const struct sock_intf_s g_bluetooth_sockif =
   NULL,                   /* si_sendfile */
 #endif
   bluetooth_recvfrom,    /* si_recvfrom */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  NULL,                  /* si_recvmsg */
+#endif
   bluetooth_close        /* si_close */
 };
 
diff --git a/net/can/can.h b/net/can/can.h
index e5d45f0..c74fca4 100644
--- a/net/can/can.h
+++ b/net/can/can.h
@@ -115,6 +115,10 @@ struct can_conn_s
   /* TODO add filter support */
 #endif
   
+#ifdef CONFIG_NET_TIMESTAMP
+  FAR struct socket *psock; /* Needed to get SO_TIMESTAMP value */
+#endif
+
   
 };
 
@@ -254,11 +258,35 @@ uint16_t can_datahandler(FAR struct can_conn_s *conn, FAR uint8_t *buffer,
  *
  ****************************************************************************/
 
+
 ssize_t can_recvfrom(FAR struct socket *psock, FAR void *buf, size_t len,
                      int flags, FAR struct sockaddr *from,
                      FAR socklen_t *fromlen);
 
 /****************************************************************************
+ * Name: can_recvmsg
+ *
+ * Description:
+ *   recvmsg() receives messages from a socket, and may be used to receive
+ *   data on a socket whether or not it is connection-oriented.
+ *
+ *   If from is not NULL, and the underlying protocol provides the source
+ *   address, this source address is filled in. The argument 'fromlen'
+ *   initialized to the size of the buffer associated with from, and modified
+ *   on return to indicate the actual size of the address stored there.
+ *
+ * Input Parameters:
+ *   psock    A pointer to a NuttX-specific, internal socket structure
+ *   msg      Buffer to receive msg
+ *   flags    Receive flags (ignored)
+ *
+ ****************************************************************************/
+#ifdef CONFIG_NET_RECVMSG_CMSG
+ssize_t can_recvmsg(FAR struct socket *psock, FAR struct msghdr *msg,
+                    size_t len, int flags);
+#endif
+
+/****************************************************************************
  * Name: can_poll
  *
  * Description:
diff --git a/net/can/can_callback.c b/net/can/can_callback.c
index 7b923d3..f8bff86 100644
--- a/net/can/can_callback.c
+++ b/net/can/can_callback.c
@@ -35,6 +35,10 @@
 #include "devif/devif.h"
 #include "can/can.h"
 
+#ifdef CONFIG_NET_TIMESTAMP
+#include <sys/time.h>
+#endif
+
 /****************************************************************************
  * Private Functions
  ****************************************************************************/
@@ -114,6 +118,18 @@ uint16_t can_callback(FAR struct net_driver_s *dev,
 
   if (conn)
     {
+#ifdef CONFIG_NET_TIMESTAMP
+  /* TIMESTAMP sockopt is activated, create timestamp and copy to iob */
+	  if(conn->psock->s_timestamp)
+	    {
+		  struct timespec *ts = (struct timespec*)&dev->d_appdata[dev->d_len];
+		  struct timeval *tv = (struct timeval*)&dev->d_appdata[dev->d_len];
+		  dev->d_len += sizeof(struct timeval);
+		  clock_systimespec(ts);
+		  tv->tv_usec = ts->tv_nsec / 1000;
+	    }
+#endif
+
       /* Perform the callback */
 
       flags = devif_conn_event(dev, conn, flags, conn->list);
diff --git a/net/can/can_recvfrom.c b/net/can/can_recvfrom.c
index 06b133c..98a0b2a 100644
--- a/net/can/can_recvfrom.c
+++ b/net/can/can_recvfrom.c
@@ -46,6 +46,11 @@
 #include "socket/socket.h"
 #include <netpacket/packet.h>
 
+#ifdef CONFIG_NET_TIMESTAMP
+#include <sys/time.h>
+#endif
+
+
 /****************************************************************************
  * Private Types
  ****************************************************************************/
@@ -58,6 +63,10 @@ struct can_recvfrom_s
   size_t       pr_buflen;              /* Length of receive buffer */
   FAR uint8_t *pr_buffer;              /* Pointer to receive buffer */
   ssize_t      pr_recvlen;             /* The received length */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  size_t       pr_msglen;              /* Length of msg buffer */
+  FAR uint8_t *pr_msgbuf;              /* Pointer to msg buffer */
+#endif
   int          pr_result;              /* Success:OK, failure:negated errno */
 };
 
@@ -294,6 +303,79 @@ static inline int can_readahead(struct can_recvfrom_s *pstate)
   return 0;
 }
 
+/****************************************************************************
+ * Name: can_readahead
+ *
+ * Description:
+ *   Copy the read-ahead data from the packet
+ *
+ * Input Parameters:
+ *   pstate   recvfrom state structure
+ *
+ * Returned Value:
+ *   None
+ *
+ * Assumptions:
+ *   The network is locked.
+ *
+ ****************************************************************************/
+#ifdef CONFIG_NET_TIMESTAMP
+static inline int can_readahead_timestamp(struct can_conn_s *conn, FAR uint8_t *buffer)
+{
+  FAR struct iob_s *iob;
+  int recvlen;
+
+
+  if ((iob = iob_peek_queue(&conn->readahead)) != NULL)
+    {
+      DEBUGASSERT(iob->io_pktlen > 0);
+
+      /* Transfer that buffered data from the I/O buffer chain into
+       * the user buffer.
+       */
+
+      recvlen = iob_copyout(buffer, iob, sizeof(struct timeval), 0);
+
+      /* If we took all of the data from the I/O buffer chain is empty, then
+       * release it.  If there is still data available in the I/O buffer
+       * chain, then just trim the data that we have taken from the
+       * beginning of the I/O buffer chain.
+       */
+
+      if (recvlen >= iob->io_pktlen)
+        {
+          FAR struct iob_s *tmp;
+
+          /* Remove the I/O buffer chain from the head of the read-ahead
+           * buffer queue.
+           */
+
+          tmp = iob_remove_queue(&conn->readahead);
+          DEBUGASSERT(tmp == iob);
+          UNUSED(tmp);
+
+          /* And free the I/O buffer chain */
+
+          iob_free_chain(iob, IOBUSER_NET_CAN_READAHEAD);
+        }
+      else
+        {
+          /* The bytes that we have received from the head of the I/O
+           * buffer chain (probably changing the head of the I/O
+           * buffer queue).
+           */
+
+          iob_trimhead_queue(&conn->readahead, recvlen,
+                             IOBUSER_NET_CAN_READAHEAD);
+        }
+
+      return recvlen;
+    }
+
+  return 0;
+}
+#endif
+
 #ifdef CONFIG_NET_CANPROTO_OPTIONS
 static int can_recv_filter(struct can_conn_s *conn, canid_t id)
 {
@@ -360,6 +442,22 @@ static uint16_t can_recvfrom_eventhandler(FAR struct net_driver_s *dev,
 
           can_newdata(dev, pstate);
 
+#ifdef CONFIG_NET_TIMESTAMP
+		  if(pstate->pr_sock->s_timestamp)
+			{
+			  if(pstate->pr_msglen == sizeof(struct timeval))
+			    {
+				  can_readahead_timestamp(conn, pstate->pr_msgbuf);
+			    }
+			  else
+			    {
+				/* We still have to consume the data otherwise IOB gets full */
+				  uint8_t dummy_buf[sizeof(struct timeval)];
+				  can_readahead_timestamp(conn, &dummy_buf);
+			    }
+			}
+#endif
+
           /* We are finished. */
 
           /* Don't allow any further call backs. */
@@ -559,4 +657,158 @@ errout_with_state:
   return ret;
 }
 
+/****************************************************************************
+ * Name: can_recvmsg
+ *
+ * Description:
+ *   recvmsg() receives messages from a socket, and may be used to receive
+ *   data on a socket whether or not it is connection-oriented.
+ *
+ *   If from is not NULL, and the underlying protocol provides the source
+ *   address, this source address is filled in. The argument 'fromlen'
+ *   initialized to the size of the buffer associated with from, and modified
+ *   on return to indicate the actual size of the address stored there.
+ *
+ * Input Parameters:
+ *   psock    A pointer to a NuttX-specific, internal socket structure
+ *   msg      Buffer to receive msg
+ *   flags    Receive flags (ignored)
+ *
+ ****************************************************************************/
+#ifdef CONFIG_NET_RECVMSG_CMSG
+ssize_t can_recvmsg(FAR struct socket *psock, FAR struct msghdr *msg,
+                    size_t len, int flags)
+{
+  FAR struct can_conn_s *conn;
+  FAR struct net_driver_s *dev;
+  struct can_recvfrom_s state;
+  int ret;
+
+  DEBUGASSERT(psock != NULL && psock->s_conn != NULL && msg != NULL);
+
+  conn = (FAR struct can_conn_s *)psock->s_conn;
+
+  if (psock->s_type != SOCK_RAW)
+    {
+      nerr("ERROR: Unsupported socket type: %d\n", psock->s_type);
+      ret = -ENOSYS;
+    }
+
+  net_lock();
+
+  /* Initialize the state structure. */
+
+  memset(&state, 0, sizeof(struct can_recvfrom_s));
+
+  /* This semaphore is used for signaling and, hence, should not have
+   * priority inheritance enabled.
+   */
+
+  nxsem_init(&state.pr_sem, 0, 0); /* Doesn't really fail */
+  nxsem_setprotocol(&state.pr_sem, SEM_PRIO_NONE);
+
+
+  state.pr_buflen = msg->msg_iov->iov_len;
+  state.pr_buffer = msg->msg_iov->iov_base;
+#ifdef CONFIG_NET_TIMESTAMP
+  if(psock->s_timestamp && msg->msg_controllen == (sizeof(struct cmsghdr) +
sizeof(struct timeval)))
+    {
+	  struct cmsghdr* cmsg = CMSG_FIRSTHDR(msg);
+	  state.pr_msglen = sizeof(struct timeval);
+	  state.pr_msgbuf = CMSG_DATA(cmsg);
+	  cmsg->cmsg_level = SOL_SOCKET;
+	  cmsg->cmsg_type = SO_TIMESTAMP;
+	  cmsg->cmsg_len = state.pr_msglen;
+    }
+#endif
+  state.pr_sock   = psock;
+
+  /* Handle any any CAN data already buffered in a read-ahead buffer.  NOTE
+   * that there may be read-ahead data to be retrieved even after the
+   * socket has been disconnected.
+   */
+
+  ret = can_readahead(&state);
+  if (ret > 0)
+    {
+#ifdef CONFIG_NET_TIMESTAMP
+	  if(psock->s_timestamp)
+	    {
+		  if(state.pr_msglen == sizeof(struct timeval))
+		    {
+			  can_readahead_timestamp(conn, state.pr_msgbuf);
+		    }
+		  else
+		    {
+			/* We still have to consume the data otherwise IOB gets full */
+		    uint8_t dummy_buf[sizeof(struct timeval)];
+			can_readahead_timestamp(conn, &dummy_buf);
+		    }
+	    }
+#endif
+      goto errout_with_state;
+    }
+
+  ret = state.pr_recvlen;
+
+  /* Handle non-blocking CAN sockets */
+
+  if (_SS_ISNONBLOCK(psock->s_flags) || (flags & MSG_DONTWAIT) != 0)
+    {
+      /* Return the number of bytes read from the read-ahead buffer if
+       * something was received (already in 'ret'); EAGAIN if not.
+       */
+
+      if (ret < 0)
+        {
+          /* Nothing was received */
+
+          ret = -EAGAIN;
+          goto errout_with_state;
+        }
+    }
+
+  /* Get the device driver that will service this transfer */
+
+  dev  = conn->dev;
+  if (dev == NULL)
+    {
+      ret = -ENODEV;
+      goto errout_with_state;
+    }
+
+  /* Set up the callback in the connection */
+
+  state.pr_cb = can_callback_alloc(dev, conn);
+  if (state.pr_cb)
+    {
+      state.pr_cb->flags  = (CAN_NEWDATA | CAN_POLL);
+      state.pr_cb->priv   = (FAR void *)&state;
+      state.pr_cb->event  = can_recvfrom_eventhandler;
+
+      /* Wait for either the receive to complete or for an error/timeout to
+       * occur. NOTES:  (1) net_lockedwait will also terminate if a signal
+       * is received, (2) the network is locked!  It will be un-locked while
+       * the task sleeps and automatically re-locked when the task restarts.
+       */
+
+      ret = net_lockedwait(&state.pr_sem);
+
+      /* Make sure that no further events are processed */
+
+      can_callback_free(dev, conn, state.pr_cb);
+      ret = can_recvfrom_result(ret, &state);
+    }
+  else
+    {
+      ret = -EBUSY;
+    }
+
+errout_with_state:
+  net_unlock();
+  nxsem_destroy(&state.pr_sem);
+  return ret;
+}
+#endif
+
 #endif /* CONFIG_NET_CAN */
diff --git a/net/can/can_sockif.c b/net/can/can_sockif.c
index 4deef14..844ac3b 100644
--- a/net/can/can_sockif.c
+++ b/net/can/can_sockif.c
@@ -93,6 +93,9 @@ const struct sock_intf_s g_can_sockif =
   NULL,             /* si_sendfile */
 #endif
   can_recvfrom,     /* si_recvfrom */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  can_recvmsg,      /* si_recvmsg */
+#endif
   can_close         /* si_close */
 };
 
@@ -105,7 +108,7 @@ const struct sock_intf_s g_can_sockif =
  *
  * Description:
  *   This function is called to perform the actual CAN receive operation
- *   via the device interface layer.
+ *   via the device interface layer. from can_input()
  *
  * Input Parameters:
  *   dev      The structure of the network driver that caused the event
@@ -229,6 +232,11 @@ static int can_setup(FAR struct socket *psock, int protocol)
           return -ENOMEM;
         }
 
+#ifdef CONFIG_NET_TIMESTAMP
+      /* Store psock in conn se we can read the SO_TIMESTAMP value */
+      conn->psock = psock;
+#endif
+
       /* Initialize the connection instance */
 
       conn->protocol = (uint8_t)protocol;
diff --git a/net/icmp/icmp_sockif.c b/net/icmp/icmp_sockif.c
index 983eb71..c81592e 100644
--- a/net/icmp/icmp_sockif.c
+++ b/net/icmp/icmp_sockif.c
@@ -102,6 +102,9 @@ const struct sock_intf_s g_icmp_sockif =
   NULL,             /* si_sendfile */
 #endif
   icmp_recvfrom,    /* si_recvfrom */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  NULL,             /* si_recvmsg */
+#endif
   icmp_close        /* si_close */
 };
 
diff --git a/net/icmpv6/icmpv6_sockif.c b/net/icmpv6/icmpv6_sockif.c
index c99d184..29e5e47 100644
--- a/net/icmpv6/icmpv6_sockif.c
+++ b/net/icmpv6/icmpv6_sockif.c
@@ -102,6 +102,9 @@ const struct sock_intf_s g_icmpv6_sockif =
   NULL,               /* si_sendfile */
 #endif
   icmpv6_recvfrom,    /* si_recvfrom */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  NULL,               /* si_recvmsg */
+#endif
   icmpv6_close        /* si_close */
 };
 
diff --git a/net/ieee802154/ieee802154_sockif.c b/net/ieee802154/ieee802154_sockif.c
index 216264a..d1ac29b 100644
--- a/net/ieee802154/ieee802154_sockif.c
+++ b/net/ieee802154/ieee802154_sockif.c
@@ -107,6 +107,9 @@ const struct sock_intf_s g_ieee802154_sockif =
   NULL,                   /* si_sendfile */
 #endif
   ieee802154_recvfrom,    /* si_recvfrom */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  NULL,                   /* si_recvmsg */
+#endif
   ieee802154_close        /* si_close */
 };
 
diff --git a/net/inet/inet_sockif.c b/net/inet/inet_sockif.c
index 7a71ff9..0980989 100644
--- a/net/inet/inet_sockif.c
+++ b/net/inet/inet_sockif.c
@@ -117,6 +117,9 @@ static const struct sock_intf_s g_inet_sockif =
   inet_sendfile,    /* si_sendfile */
 #endif
   inet_recvfrom,    /* si_recvfrom */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  NULL,             /* si_recvmsg */
+#endif
   inet_close        /* si_close */
 };
 
diff --git a/net/local/local_sockif.c b/net/local/local_sockif.c
index f2bbcb1..62d56b5 100644
--- a/net/local/local_sockif.c
+++ b/net/local/local_sockif.c
@@ -110,6 +110,9 @@ const struct sock_intf_s g_local_sockif =
   NULL,              /* si_sendfile */
 #endif
   local_recvfrom,    /* si_recvfrom */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  NULL,              /* si_recvmsg */
+#endif
   local_close        /* si_close */
 };
 
diff --git a/net/netlink/netlink_sockif.c b/net/netlink/netlink_sockif.c
index 9295901..1c0c815 100644
--- a/net/netlink/netlink_sockif.c
+++ b/net/netlink/netlink_sockif.c
@@ -111,6 +111,9 @@ const struct sock_intf_s g_netlink_sockif =
   NULL,                 /* si_sendfile */
 #endif
   netlink_recvfrom,     /* si_recvfrom */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  NULL,                 /* si_recvmsg */
+#endif
   netlink_close         /* si_close */
 };
 
diff --git a/net/pkt/pkt_sockif.c b/net/pkt/pkt_sockif.c
index e189ddc..bbabf42 100644
--- a/net/pkt/pkt_sockif.c
+++ b/net/pkt/pkt_sockif.c
@@ -108,6 +108,9 @@ const struct sock_intf_s g_pkt_sockif =
   NULL,            /* si_sendfile */
 #endif
   pkt_recvfrom,    /* si_recvfrom */
+#ifdef CONFIG_NET_RECVMSG_CMSG
+  NULL,            /* si_recvmsg */
+#endif
   pkt_close        /* si_close */
 };
 
diff --git a/net/socket/Kconfig b/net/socket/Kconfig
index 8b71ade..f4c9408 100644
--- a/net/socket/Kconfig
+++ b/net/socket/Kconfig
@@ -55,5 +55,21 @@ config NET_SOLINGER
 		Enable or disable support for the SO_LINGER socket option.  Requires
 		write buffer support.
 
+config NET_TIMESTAMP
+	bool "SO_TIMESTAMP socket option"
+	default n
+	depends on NET_CAN && NET_RECVMSG_CMSG
+	---help---
+		Enable or disable support for the SO_TIMESTAMP socket option. Currently only tested &
implemented in SocketCAN but should work on all sockets
+
 endif # NET_SOCKOPTS
+
+config NET_RECVMSG_CMSG
+	bool "recvmsg() control messages (CMSG) support"
+	default n
+	---help---
+		Enable or disable support for control messages in the recvmsg function.
+		Control messages (also defined in POSIX 1003.1g as ancillary data object information).
+		Includes additional information on the packet received.
+
 endmenu # Socket Support
diff --git a/net/socket/Make.defs b/net/socket/Make.defs
index 47fb4d2..2be9dbf 100644
--- a/net/socket/Make.defs
+++ b/net/socket/Make.defs
@@ -74,3 +74,8 @@ endif
 
 DEPPATH += --dep-path socket
 VPATH += :socket
+
+# Support for recvmsg() with control messages (CMSG)
+ifeq ($(CONFIG_NET_RECVMSG_CMSG),y)
+SOCK_CSRCS += recvmsg.c
+endif
diff --git a/net/socket/getsockopt.c b/net/socket/getsockopt.c
index 47fd82f..9c2658b 100644
--- a/net/socket/getsockopt.c
+++ b/net/socket/getsockopt.c
@@ -277,6 +277,19 @@ static int psock_socketlevel_option(FAR struct socket *psock, int option,
         }
         break;
 
+#ifdef CONFIG_NET_TIMESTAMP
+      case SO_TIMESTAMP:
+        {
+          if (*value_len != sizeof(int))
+            {
+              return -EINVAL;
+            }
+
+          *(FAR int *)value = (int)psock->s_timestamp;
+        }
+        break;
+#endif
+
       /* The following are not yet implemented (return values other than {0,1) */
 
       case SO_LINGER:     /* Lingers on a close() if data is present */
diff --git a/net/socket/setsockopt.c b/net/socket/setsockopt.c
index 4c1802c..3608152 100644
--- a/net/socket/setsockopt.c
+++ b/net/socket/setsockopt.c
@@ -276,6 +276,30 @@ static int psock_socketlevel_option(FAR struct socket *psock, int option,
         }
         break;
 #endif
+
+#ifdef CONFIG_NET_TIMESTAMP
+      case SO_TIMESTAMP:  /* Generates a timestamp for each incoming packet */
+        {
+
+          /* Verify that option is at least the size of an integer. */
+
+          if (value_len < sizeof(FAR int32_t))
+            {
+              return -EINVAL;
+            }
+
+          /* Lock the network so that we have exclusive access to the socket
+           * options.
+           */
+
+          net_lock();
+
+          psock->s_timestamp = *((FAR int32_t*)value);
+
+          net_unlock();
+        }
+        break;
+#endif
       /* The following are not yet implemented */
 
       case SO_RCVBUF:     /* Sets receive buffer size */
diff --git a/net/socket/socket.h b/net/socket/socket.h
index 0f809ac..80279da 100644
--- a/net/socket/socket.h
+++ b/net/socket/socket.h
@@ -80,7 +80,7 @@
 
 /* This is the largest option value.  REVISIT: belongs in sys/socket.h */
 
-#define _SO_MAXOPT       (15)
+#define _SO_MAXOPT       (29)
 
 /* Macros to set, test, clear options */
 


Mime
View raw message