icmpv6: add SOCK_RAW type support

The third-party library we are porting will send and receive ICMPV6 messages
(router_advert / router_solicit / neighbor_advert / neighbor_solicit etc.)
from the user mode itself, so we added the SOCK_RAW related implementation
for ICMPV6.

Signed-off-by: zhanghongyu <[email protected]>

Author: zhhyu7

net/icmpv6/icmpv6.h

@@ -30,6 +30,7 @@
 #include <sys/types.h>
 #include <stdint.h>
 #include <assert.h>
+#include <netinet/icmp6.h>
 
 #include <nuttx/mm/iob.h>
 #include <nuttx/net/ip.h>
@@ -84,7 +85,6 @@ struct icmpv6_conn_s
   /* ICMPv6-specific content follows */
 
   uint16_t   id;       /* ICMPv6 ECHO request ID */
-  uint8_t    nreqs;    /* Number of requests with no response received */
   uint8_t    crefs;    /* Reference counts on this instance */
 
   /* The device that the ICMPv6 request was sent on */
@@ -97,13 +97,19 @@ struct icmpv6_conn_s
    */
 
   struct iob_queue_s readahead;  /* Read-ahead buffering */
+  struct icmp6_filter filter;    /* ICMP6 type filter */
 
   /* The following is a list of poll structures of threads waiting for
    * socket events.
    */
 
   struct icmpv6_poll_s pollinfo[CONFIG_NET_ICMPv6_NPOLLWAITERS];
 };
+
+/* Callback from icmpv6_foreach() */
+
+typedef int (*icmpv6_callback_t)(FAR struct icmpv6_conn_s *conn,
+                                 FAR void *arg);
 #endif
 
 #ifdef CONFIG_NET_ICMPv6_NEIGHBOR
@@ -596,20 +602,20 @@ FAR struct icmpv6_conn_s *icmpv6_nextconn(FAR struct icmpv6_conn_s *conn);
 #endif
 
 /****************************************************************************
- * Name: icmpv6_findconn
+ * Name: icmpv6_foreach
  *
  * Description:
- *   Find an ICMPv6 connection structure that is expecting a ICMPv6 ECHO
- *   response with this ID from this device
+ *   Enumerate each ICMPv6 connection structure. This function will terminate
+ *   when either (1) all connection have been enumerated or (2) when a
+ *   callback returns any non-zero value.
  *
  * Assumptions:
  *   This function is called from network logic at with the network locked.
  *
  ****************************************************************************/
 
 #ifdef CONFIG_NET_ICMPv6_SOCKET
-FAR struct icmpv6_conn_s *icmpv6_findconn(FAR struct net_driver_s *dev,
-                                          uint16_t id);
+int icmpv6_foreach(icmpv6_callback_t callback, FAR void *arg);
 #endif
 
 /****************************************************************************

net/icmpv6/icmpv6_conn.c

@@ -252,31 +252,36 @@ FAR struct icmpv6_conn_s *icmpv6_nextconn(FAR struct icmpv6_conn_s *conn)
 }
 
 /****************************************************************************
- * Name: icmpv6_findconn
+ * Name: icmpv6_foreach
  *
  * Description:
- *   Find an ICMPv6 connection structure that is expecting a ICMPv6 ECHO
- *  response with this ID from this device
+ *   Enumerate each ICMPv6 connection structure. This function will terminate
+ *   when either (1) all connection have been enumerated or (2) when a
+ *   callback returns any non-zero value.
  *
  * Assumptions:
  *   This function is called from network logic at with the network locked.
  *
  ****************************************************************************/
 
-FAR struct icmpv6_conn_s *icmpv6_findconn(FAR struct net_driver_s *dev,
-                                          uint16_t id)
+int icmpv6_foreach(icmpv6_callback_t callback, FAR void *arg)
 {
   FAR struct icmpv6_conn_s *conn;
+  int ret = 0;
 
-  for (conn = icmpv6_nextconn(NULL); conn != NULL;
-       conn = icmpv6_nextconn(conn))
+  if (callback != NULL)
     {
-      if (conn->id == id && conn->dev == dev && conn->nreqs > 0)
+      for (conn = icmpv6_nextconn(NULL); conn != NULL;
+           conn = icmpv6_nextconn(conn))
         {
-          return conn;
+          ret = callback(conn, arg);
+          if (ret != 0)
+            {
+              break;
+            }
         }
     }
 
-  return conn;
+  return ret;
 }
 #endif /* CONFIG_NET_ICMP */

net/icmpv6/icmpv6_input.c

@@ -59,10 +59,32 @@
 #define MLDREPORT_V2     ((FAR struct mld_mcast_listen_report_v2_s *)icmpv6)
 #define MLDDONE          ((FAR struct mld_mcast_listen_done_s *)icmpv6)
 
+#ifdef CONFIG_NET_ICMPv6_SOCKET
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+struct icmpv6_deliver_s
+{
+  FAR struct net_driver_s *dev; /* Current network device */
+  unsigned int iplen;           /* The size of the IPv6 header */
+  bool delivered;               /* Whether the message is delivered */
+};
+
 /****************************************************************************
  * Private Functions
  ****************************************************************************/
 
+static bool icmpv6_filter(FAR const uint32_t *data, uint8_t type)
+{
+  /* We require only the four bytes of the ICMPv6 header. */
+
+  DEBUGASSERT(data != NULL);
+
+  return (data[type >> 5] & (1u << (type & 31))) != 0;
+}
+
 /****************************************************************************
  * Name: icmpv6_datahandler
  *
@@ -82,7 +104,6 @@
  *
  ****************************************************************************/
 
-#ifdef CONFIG_NET_ICMPv6_SOCKET
 static uint16_t icmpv6_datahandler(FAR struct net_driver_s *dev,
                                    FAR struct icmpv6_conn_s *conn,
                                    unsigned int iplen)
@@ -93,9 +114,14 @@ static uint16_t icmpv6_datahandler(FAR struct net_driver_s *dev,
   uint16_t buflen;
   int ret;
 
+  iob = iob_tryalloc(false);
+  if (iob == NULL)
+    {
+      return -ENOMEM;
+    }
+
   /* Put the IPv6 address at the beginning of the read-ahead buffer */
 
-  iob                = dev->d_iob;
   ipv6               = IPv6BUF;
   inaddr.sin6_family = AF_INET6;
   inaddr.sin6_port   = 0;
@@ -107,13 +133,19 @@ static uint16_t icmpv6_datahandler(FAR struct net_driver_s *dev,
 
   memcpy(iob->io_data, &inaddr, sizeof(struct sockaddr_in6));
 
-  /* Copy the new ICMPv6 reply into the I/O buffer chain (without waiting) */
+  iob_reserve(iob, sizeof(struct sockaddr_in6));
 
-  buflen = ICMPv6SIZE;
+  /* Copy the ICMPv6 message into the I/O buffer chain (without waiting) */
 
-  /* Trim l3 header */
+  ret = iob_clone_partial(dev->d_iob, dev->d_iob->io_pktlen,
+                          iplen, iob, 0, true, false);
+  if (ret < 0)
+    {
+      iob_free_chain(iob);
+      return ret;
+    }
 
-  iob = iob_trimhead(iob, iplen);
+  buflen = ICMPv6SIZE;
 
   /* Add the new I/O buffer chain to the tail of the read-ahead queue (again
    * without waiting).
@@ -130,12 +162,81 @@ static uint16_t icmpv6_datahandler(FAR struct net_driver_s *dev,
       ninfo("Buffered %d bytes\n", buflen);
     }
 
-  /* Device buffer must be enqueue or freed, clear the handle */
-
-  netdev_iob_clear(dev);
   return buflen;
 }
-#endif
+
+/****************************************************************************
+ * Name: icmpv6_delivery_callback
+ *
+ * Description:
+ *   Copy the icmpv6 package to the application according to the filter
+ *   conditions, but ICMPv6_ECHO_REPLY is a special message type, if there
+ *   is an application waiting, it will also copy.
+ *
+ * Input Parameters:
+ *   conn - A pointer to the ICMPv6 connection structure.
+ *   arg - The context information
+ *
+ ****************************************************************************/
+
+static int icmpv6_delivery_callback(FAR struct icmpv6_conn_s *conn,
+                                    FAR void *arg)
+{
+  FAR struct icmpv6_deliver_s *info   = arg;
+  FAR struct net_driver_s     *dev    = info->dev;
+  FAR struct icmpv6_hdr_s     *icmpv6 = IPBUF(info->iplen);
+
+  if (icmpv6_filter(conn->filter.icmp6_filt, icmpv6->type) &&
+      (icmpv6->type != ICMPv6_ECHO_REPLY || conn->id != ICMPv6REPLY->id ||
+       conn->dev != dev))
+    {
+      return 0;
+    }
+
+  info->delivered = true;
+  if (devif_conn_event(dev, ICMPv6_NEWDATA, conn->sconn.list) !=
+      ICMPv6_NEWDATA)
+    {
+      dev->d_len = dev->d_iob->io_pktlen;
+    }
+  else
+    {
+      icmpv6_datahandler(dev, conn, info->iplen);
+    }
+
+  return 0;
+}
+
+/****************************************************************************
+ * Name: icmpv6_deliver
+ *
+ * Description:
+ *   Copy the icmpv6 package to the application according to the filter
+ *   conditions, but ICMPv6_ECHO_REPLY is a special message type, if there
+ *   is an application waiting, it will also copy.
+ *
+ * Input Parameters:
+ *   dev - Reference to a device driver structure.
+ *   iplen - The size of the IPv6 header.  This may be larger than
+ *           IPv6_HDRLEN the IPv6 header if IPv6 extension headers are
+ *           present.
+ *
+ ****************************************************************************/
+
+static bool icmpv6_deliver(FAR struct net_driver_s *dev, unsigned int iplen)
+{
+  struct icmpv6_deliver_s info;
+
+  info.dev       = dev;
+  info.iplen     = iplen;
+  info.delivered = false;
+
+  icmpv6_foreach(icmpv6_delivery_callback, &info);
+
+  return info.delivered;
+}
+
+#endif /* CONFIG_NET_ICMPv6_SOCKET */
 
 /****************************************************************************
  * Public Functions
@@ -166,6 +267,9 @@ void icmpv6_input(FAR struct net_driver_s *dev, unsigned int iplen)
 {
   FAR struct ipv6_hdr_s *ipv6 = IPv6BUF;
   FAR struct icmpv6_hdr_s *icmpv6 = IPBUF(iplen);
+#ifdef CONFIG_NET_ICMPv6_SOCKET
+  bool delivered = icmpv6_deliver(dev, iplen);
+#endif
 
 #ifdef CONFIG_NET_STATISTICS
   g_netstats.icmpv6.recv++;
@@ -443,61 +547,6 @@ void icmpv6_input(FAR struct net_driver_s *dev, unsigned int iplen)
       }
       break;
 
-#ifdef CONFIG_NET_ICMPv6_SOCKET
-    /* If an ICMPv6 echo reply is received then there should also be
-     * a thread waiting to received the echo response.
-     */
-
-    case ICMPv6_ECHO_REPLY:
-      {
-        FAR struct icmpv6_echo_reply_s *reply;
-        FAR struct icmpv6_conn_s *conn;
-        uint16_t flags = ICMPv6_NEWDATA;
-
-        /* Nothing consumed the ICMP reply.  That might be because this is
-         * an old, invalid reply or simply because the ping application
-         * has not yet put its poll or recv in place.
-         */
-
-        /* Is there any connection that might expect this reply? */
-
-        reply = ICMPv6REPLY;
-        conn = icmpv6_findconn(dev, reply->id);
-        if (conn == NULL)
-          {
-            /* No.. drop the packet */
-
-            goto icmpv6_drop_packet;
-          }
-
-        /* Dispatch the ECHO reply to the waiting thread */
-
-        flags = devif_conn_event(dev, flags, conn->sconn.list);
-
-        /* Was the ECHO reply consumed by any waiting thread? */
-
-        if ((flags & ICMPv6_NEWDATA) != 0)
-          {
-            uint16_t nbuffered;
-
-            /* Yes.. Add the ICMP echo reply to the IPPROTO_ICMP socket read
-             * ahead buffer.
-             */
-
-            nbuffered = icmpv6_datahandler(dev, conn, iplen);
-            if (nbuffered == 0)
-              {
-                /* Could not buffer the data.. drop the packet */
-
-                goto icmpv6_drop_packet;
-              }
-          }
-
-          goto icmpv6_send_nothing;
-      }
-      break;
-#endif
-
 #ifdef CONFIG_NET_MLD
     /* Dispatch received Multicast Listener Discovery (MLD) packets. */
 
@@ -556,6 +605,13 @@ void icmpv6_input(FAR struct net_driver_s *dev, unsigned int iplen)
 
     default:
       {
+#ifdef CONFIG_NET_ICMPv6_SOCKET
+        if (delivered)
+          {
+            goto icmpv6_send_nothing;
+          }
+#endif
+
         nwarn("WARNING: Unknown ICMPv6 type: %d\n", icmpv6->type);
         goto icmpv6_type_error;
       }