recvmsg: control msg support multi-attribute return

net/can/can_recvmsg.c

@@ -48,6 +48,7 @@
 
 #ifdef CONFIG_NET_TIMESTAMP
 #include <sys/time.h>
+#include <utils/utils.h>
 #endif
 
 /****************************************************************************
@@ -490,24 +491,14 @@ ssize_t can_recvmsg(FAR struct socket *psock, FAR struct msghdr *msg,
   state.pr_buffer = msg->msg_iov->iov_base;
 
 #ifdef CONFIG_NET_TIMESTAMP
-  if (conn->timestamp && msg->msg_controllen >=
-        (sizeof(struct cmsghdr) + sizeof(struct timeval)))
+  if (conn->timestamp)
     {
-      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;
-      msg->msg_controllen = sizeof(struct cmsghdr) + sizeof(struct timeval);
-    }
-  else
-    {
-      /* Expected behavior is that the msg_controllen becomes 0,
-       * otherwise CMSG_NXTHDR will go into a infinite loop
-       */
-
-      msg->msg_controllen = 0;
+      state.pr_msgbuf = cmsg_append(msg, SOL_SOCKET, SO_TIMESTAMP,
+                                    NULL, sizeof(struct timeval));
+      if (state.pr_msgbuf != NULL)
+        {
+          state.pr_msglen = sizeof(struct timeval);
+        }
     }
 #endif
 

net/local/local_recvmsg.c

@@ -40,6 +40,7 @@
 
 #include "socket/socket.h"
 #include "local/local.h"
+#include "utils/utils.h"
 
 /****************************************************************************
  * Private Functions
@@ -117,22 +118,12 @@ static void local_recvctl(FAR struct local_conn_s *conn,
                           FAR struct msghdr *msg, int flags)
 {
   FAR struct local_conn_s *peer;
-  struct cmsghdr *cmsg;
   int count;
   int *fds;
   int i;
 
   net_lock();
 
-  cmsg  = CMSG_FIRSTHDR(msg);
-  count = (cmsg->cmsg_len - sizeof(struct cmsghdr)) / sizeof(int);
-  cmsg->cmsg_len = 0;
-
-  if (count == 0)
-    {
-      goto out;
-    }
-
   if (conn->lc_peer == NULL)
     {
       peer = local_peerconn(conn);
@@ -151,10 +142,14 @@ static void local_recvctl(FAR struct local_conn_s *conn,
       goto out;
     }
 
-  fds = (int *)CMSG_DATA(cmsg);
+  fds = cmsg_append(msg, SOL_SOCKET, SCM_RIGHTS, NULL,
+                    sizeof(int) * peer->lc_cfpcount);
+  if (fds == NULL)
+    {
+      goto out;
+    }
 
-  count = count > peer->lc_cfpcount ?
-                  peer->lc_cfpcount : count;
+  count = peer->lc_cfpcount;
   for (i = 0; i < count; i++)
     {
       fds[i] = file_dup(peer->lc_cfps[i], 0, !!(flags & MSG_CMSG_CLOEXEC));
@@ -176,10 +171,6 @@ static void local_recvctl(FAR struct local_conn_s *conn,
           memmove(peer->lc_cfps[0], peer->lc_cfps[i],
                   sizeof(FAR void *) * peer->lc_cfpcount);
         }
-
-      cmsg->cmsg_len   = CMSG_LEN(sizeof(int) * i);
-      cmsg->cmsg_level = SOL_SOCKET;
-      cmsg->cmsg_type  = SCM_RIGHTS;
     }
 
 out:

net/socket/recvmsg.c

@@ -69,6 +69,10 @@
 ssize_t psock_recvmsg(FAR struct socket *psock, FAR struct msghdr *msg,
                        int flags)
 {
+  unsigned long msg_controllen;
+  FAR void *msg_control;
+  int ret;
+
   /* Verify that non-NULL pointers were passed */
 
   if (msg == NULL || msg->msg_iov == NULL || msg->msg_iov->iov_base == NULL)
@@ -100,7 +104,19 @@ ssize_t psock_recvmsg(FAR struct socket *psock, FAR struct msghdr *msg,
   DEBUGASSERT(psock->s_sockif != NULL &&
               psock->s_sockif->si_recvmsg != NULL);
 
-  return psock->s_sockif->si_recvmsg(psock, msg, flags);
+  /* Save the original cmsg information */
+
+  msg_control         = msg->msg_control;
+  msg_controllen      = msg->msg_controllen;
+
+  ret = psock->s_sockif->si_recvmsg(psock, msg, flags);
+
+  /* Recover the pointer and calculate the cmsg's true data length */
+
+  msg->msg_control    = msg_control;
+  msg->msg_controllen = msg_controllen - msg->msg_controllen;
+
+  return ret;
 }
 
 /****************************************************************************

net/udp/udp_recvfrom.c

@@ -42,6 +42,7 @@
 #include "devif/devif.h"
 #include "udp/udp.h"
 #include "socket/socket.h"
+#include "utils/utils.h"
 
 /****************************************************************************
  * Private Types
@@ -64,59 +65,41 @@ struct udp_recvfrom_s
 static void udp_recvpktinfo(FAR struct udp_recvfrom_s *pstate,
                             FAR void *srcaddr, uint8_t ifindex)
 {
-  FAR struct msghdr     *msg      = pstate->ir_msg;
-  FAR struct udp_conn_s *conn     = pstate->ir_conn;
-  FAR struct cmsghdr    *control  = msg->msg_control;
-  size_t                 cmsg_len = 0;
+  FAR struct msghdr     *msg  = pstate->ir_msg;
+  FAR struct udp_conn_s *conn = pstate->ir_conn;
 
   if (!(conn->flags & _UDP_FLAG_PKTINFO))
     {
-      goto out;
+      return;
     }
 
 #ifdef CONFIG_NET_IPv4
   if (conn->domain == PF_INET)
     {
-      FAR struct sockaddr_in *infrom  = srcaddr;
-      FAR struct in_pktinfo *pkt_info = CMSG_DATA(control);
+      FAR struct sockaddr_in *infrom = srcaddr;
+      FAR struct in_pktinfo   pktinfo;
 
-      if (msg->msg_controllen < CMSG_LEN(sizeof(struct in_pktinfo)))
-        {
-          goto out;
-        }
+      pktinfo.ipi_ifindex         = ifindex;
+      pktinfo.ipi_addr.s_addr     = infrom->sin_addr.s_addr;
+      pktinfo.ipi_spec_dst.s_addr = conn->u.ipv4.laddr;
 
-      cmsg_len                      = CMSG_LEN(sizeof(struct in_pktinfo));
-      control->cmsg_level           = IPPROTO_IP;
-      control->cmsg_type            = IP_PKTINFO;
-      control->cmsg_len             = cmsg_len;
-      pkt_info->ipi_ifindex         = ifindex;
-      pkt_info->ipi_addr.s_addr     = infrom->sin_addr.s_addr;
-      pkt_info->ipi_spec_dst.s_addr = conn->u.ipv4.laddr;
+      cmsg_append(msg, IPPROTO_IP, IP_PKTINFO, &pktinfo, sizeof(pktinfo));
     }
 #endif
 
 #ifdef CONFIG_NET_IPv6
   if (conn->domain == PF_INET6)
     {
-      FAR struct sockaddr_in6 *infrom  = srcaddr;
-      FAR struct in6_pktinfo *pkt_info = CMSG_DATA(control);
+      FAR struct sockaddr_in6 *infrom = srcaddr;
+      FAR struct in6_pktinfo   pktinfo;
 
-      if (msg->msg_controllen < CMSG_LEN(sizeof(struct in6_pktinfo)))
-        {
-          goto out;
-        }
+      pktinfo.ipi6_ifindex = ifindex;
+      net_ipv6addr_copy(&pktinfo.ipi6_addr, infrom->sin6_addr.s6_addr);
 
-      cmsg_len               = CMSG_LEN(sizeof(struct in6_pktinfo));
-      control->cmsg_level    = IPPROTO_IPV6;
-      control->cmsg_type     = IPV6_PKTINFO;
-      control->cmsg_len      = cmsg_len;
-      pkt_info->ipi6_ifindex = ifindex;
-      net_ipv6addr_copy(&pkt_info->ipi6_addr, infrom->sin6_addr.s6_addr);
+      cmsg_append(msg, IPPROTO_IPV6, IPV6_PKTINFO, &pktinfo,
+                  sizeof(pktinfo));
     }
 #endif
-
-out:
-  msg->msg_controllen = cmsg_len;
 }
 
 /****************************************************************************

net/utils/Make.defs

@@ -22,6 +22,7 @@
 
 NET_CSRCS += net_dsec2tick.c net_dsec2timeval.c net_timeval2dsec.c
 NET_CSRCS += net_chksum.c net_ipchksum.c net_incr32.c net_lock.c net_snoop.c
+NET_CSRCS += net_cmsg.c
 
 # IPv6 utilities
 

net/utils/net_cmsg.c

@@ -0,0 +1,86 @@
+/****************************************************************************
+ * net/utils/net_cmsg.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/socket.h>
+
+#include "utils/utils.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: cmsg_append
+ *
+ * Description:
+ *   Append specified data into the control message, msg_control and
+ *   msg_controllen will be changed to the appropriate value when success
+ *
+ * Input Parameters:
+ *   msg       - Buffer to receive the message.
+ *   level     - The level of control message.
+ *   type      - The type of control message.
+ *   value     - If the value is not NULL, this interface copies the data
+ *               to the appropriate location in msg_control, and modify
+ *               msg_control and msg_controllen.
+ *               If the value is NULL, just modify the corresponding value
+ *               of msg.
+ *   value_len - The value length of control message.
+ *
+ * Returned Value:
+ *   On success, a pointer to the start address of control message data,
+ *               the caller can copy the data in.
+ *   On failure, return NULL, because of msg_controllen is not enough
+ *
+ ****************************************************************************/
+
+FAR void *cmsg_append(FAR struct msghdr *msg, int level, int type,
+                      FAR void *value, int value_len)
+{
+  FAR struct cmsghdr *cmsg;
+  unsigned long cmsgspace = CMSG_SPACE(value_len);
+  FAR void *cmsgdata;
+
+  if (msg->msg_controllen < cmsgspace)
+    {
+      return NULL;
+    }
+
+  cmsg              = CMSG_FIRSTHDR(msg);
+  cmsg->cmsg_level  = level;
+  cmsg->cmsg_type   = type;
+  cmsg->cmsg_len    = CMSG_LEN(value_len);
+  cmsgdata          = CMSG_DATA(cmsg);
+  if (value)
+    {
+      memcpy(cmsgdata, value, value_len);
+    }
+
+  msg->msg_control    += cmsgspace;
+  msg->msg_controllen -= cmsgspace;
+
+  return cmsgdata;
+}

net/utils/utils.h

@@ -311,6 +311,34 @@ uint16_t icmp_chksum_iob(FAR struct iob_s *iob);
 uint16_t icmpv6_chksum(FAR struct net_driver_s *dev, unsigned int iplen);
 #endif
 
+/****************************************************************************
+ * Name: cmsg_append
+ *
+ * Description:
+ *   Append specified data into the control message, msg_control and
+ *   msg_controllen will be changed to the appropriate value when success
+ *
+ * Input Parameters:
+ *   msg       - Buffer to receive the message.
+ *   level     - The level of control message.
+ *   type      - The type of control message.
+ *   value     - If the value is not NULL, this interface copies the data
+ *               to the appropriate location in msg_control, and modify
+ *               msg_control and msg_controllen.
+ *               If the value is NULL, just modify the corresponding value
+ *               of msg.
+ *   value_len - The value length of control message.
+ *
+ * Returned Value:
+ *   On success, a pointer to the start address of control message data,
+ *               the caller can copy the data in.
+ *   On failure, return NULL, because of msg_controllen is not enough
+ *
+ ****************************************************************************/
+
+FAR void *cmsg_append(FAR struct msghdr *msg, int level, int type,
+                      FAR void *value, int value_len);
+
 #undef EXTERN
 #ifdef __cplusplus
 }