New feature: send/recv message implementation added to network stack

connectivity/cellular/include/cellular/framework/AT/AT_CellularStack.h

@@ -88,6 +88,21 @@ class AT_CellularStack : public NetworkStack {
 
     virtual void socket_attach(nsapi_socket_t handle, void (*callback)(void *), void *data);
 
+    // FIXME
+    nsapi_size_or_error_t socket_sendmsg(nsapi_socket_t handle, const SocketAddress &address,
+                                         const void *data, nsapi_size_t size,
+                                         nsapi_msghdr_t *control, nsapi_size_t control_size) override
+    {
+        return NSAPI_ERROR_UNSUPPORTED;
+    }
+
+    nsapi_size_or_error_t socket_recvmsg(nsapi_socket_t handle, SocketAddress *address,
+                                         void *data, nsapi_size_t size,
+                                         nsapi_msghdr_t *control, nsapi_size_t control_size) override
+    {
+        return NSAPI_ERROR_UNSUPPORTED;
+    }
+
 protected:
     class CellularSocket {
     public:

connectivity/lwipstack/include/lwipstack/LWIPStack.h

@@ -449,6 +449,27 @@ class LWIP : public OnboardNetworkStack, private mbed::NonCopyable<LWIP> {
      */
     nsapi_size_or_error_t socket_send(nsapi_socket_t handle,
                                       const void *data, nsapi_size_t size) override;
+
+   /** Send a packet  with ancillary data over a UDP socket
+     *
+     *  Sends data to the specified address. Returns the number of bytes
+     *  sent from the buffer.
+     *
+     *  This call is non-blocking. If sendto would block,
+     *  NSAPI_ERROR_WOULD_BLOCK is returned immediately.
+     *
+     *  @param handle   Socket handle
+     *  @param address  The SocketAddress of the remote host
+     *  @param data     Buffer of data to send to the host
+     *  @param size     Size of the buffer in bytes
+     *  @param control     Ancillary data storage
+     *  @param control_size   Size of the Ancillary data in bytes
+     *  @return         Number of sent bytes on success, negative error
+     *                  code on failure
+     */
+    nsapi_size_or_error_t socket_sendmsg(nsapi_socket_t handle, const SocketAddress &address,
+                                             const void *data, nsapi_size_t size,
+                                             nsapi_msghdr_t* control, nsapi_size_t control_size) override;
 
     /** Receive data over a TCP socket
      *
@@ -493,6 +514,7 @@ class LWIP : public OnboardNetworkStack, private mbed::NonCopyable<LWIP> {
      *  This call is non-blocking. If recvfrom would block,
      *  NSAPI_ERROR_WOULD_BLOCK is returned immediately.
      *
+     *  It uses socket_recvmsg with  zero ancillary data.
      *  @param handle   Socket handle
      *  @param address  Destination for the source address or NULL
      *  @param buffer   Destination buffer for data received from the host
@@ -503,6 +525,27 @@ class LWIP : public OnboardNetworkStack, private mbed::NonCopyable<LWIP> {
     nsapi_size_or_error_t socket_recvfrom(nsapi_socket_t handle, SocketAddress *address,
                                           void *buffer, nsapi_size_t size) override;
 
+     /** Receive a packet with ancillary data over a UDP socket
+     *
+     *  Receives data and stores the source address in address if address
+     *  is not NULL. Returns the number of bytes received into the buffer.
+     *
+     *  This call is non-blocking. If recvfrom would block,
+     *  NSAPI_ERROR_WOULD_BLOCK is returned immediately.
+     *  
+     *  @param handle   Socket handle
+     *  @param address  Destination for the source address or NULL
+     *  @param buffer   Destination buffer for data received from the host
+     *  @param size     Size of the buffer in bytes
+     *  @param control     Ancillary data storage
+     *  @param control_size   Size of the Ancillary data in bytes
+     *  @return         Number of received bytes on success, negative error
+     *                  code on failure
+     */
+    nsapi_size_or_error_t socket_recvmsg(nsapi_socket_t handle, SocketAddress *address,
+                                                 void *data, nsapi_size_t size,
+                                                 nsapi_msghdr_t* control, nsapi_size_t control_size) override;
+
     /** Register a callback on state change of the socket
      *
      *  The specified callback will be called on state changes such as when

connectivity/lwipstack/include/lwipstack/lwipopts.h

@@ -311,6 +311,9 @@
 
 #endif
 
+// FIXME: Add compile time configuration and define conditionaly
+#define LWIP_NETBUF_RECVINFO            1
+
 // Make sure we default these to off, so
 // LWIP doesn't default to on
 #ifndef LWIP_ARP

connectivity/lwipstack/source/LWIPStack.cpp

@@ -15,10 +15,10 @@
  * limitations under the License.
  */
 #include "nsapi.h"
+#include "netsocket/MsgHeader.h"
 #include "mbed_interface.h"
 #include "mbed_assert.h"
 #include "Semaphore.h"
-#include <stdio.h>
 #include <stdbool.h>
 #include <string.h>
 
@@ -37,6 +37,7 @@
 #include "lwip/raw.h"
 #include "lwip/netif.h"
 #include "lwip/lwip_errno.h"
+#include "lwip/ip_addr.h"
 #include "lwip-sys/arch/sys_arch.h"
 
 #include "LWIPStack.h"
@@ -439,24 +440,109 @@ nsapi_size_or_error_t LWIP::socket_recv(nsapi_socket_t handle, void *data, nsapi
 }
 
 nsapi_size_or_error_t LWIP::socket_sendto(nsapi_socket_t handle, const SocketAddress &address, const void *data, nsapi_size_t size)
+{
+    return socket_sendmsg(handle, address, data, size, NULL, 0);
+}
+
+nsapi_size_or_error_t LWIP::socket_recvfrom(nsapi_socket_t handle, SocketAddress *address, void *data, nsapi_size_t size)
+{
+    return socket_recvmsg(handle, address, data, size, NULL, 0);
+
+}
+
+nsapi_size_or_error_t LWIP::socket_recvmsg(nsapi_socket_t handle, SocketAddress *address,
+                                           void *data, nsapi_size_t size,
+                                           nsapi_msghdr_t *control, nsapi_size_t control_size)
 {
     struct mbed_lwip_socket *s = (struct mbed_lwip_socket *)handle;
-    ip_addr_t ip_addr;
+    struct netbuf *buf;
+
+    err_t err = netconn_recv(s->conn, &buf);
+    if (err != ERR_OK) {
+        return err_remap(err);
+    }
+
+    if (address) {
+        nsapi_addr_t addr;
+        convert_lwip_addr_to_mbed(&addr, netbuf_fromaddr(buf));
+        address->set_addr(addr);
+        address->set_port(netbuf_fromport(buf));
+    }
+
+    if ((s->conn->flags & NETCONN_FLAG_PKTINFO) && control && control_size >= sizeof(nsapi_pktinfo_t)) {
+        nsapi_pktinfo_t *pkt_info = reinterpret_cast<nsapi_pktinfo *>(control);
+        // Not optimal but sufficient. It should help the caller in not iterating over
+        // the control data structure
+        control->len = control_size;
+        control->level = NSAPI_SOCKET;
+        control->type = NSAPI_PKTINFO;
+        // retrieve the destination
+        convert_lwip_addr_to_mbed(&pkt_info->ipi_addr, netbuf_destaddr(buf));
+        // retrieve the interface id
+        pkt_info->ipi_ifindex = buf->p->if_idx;
+    }
+
+    u16_t recv = netbuf_copy(buf, data, (u16_t)size);
+    netbuf_delete(buf);
+
+    return recv;
+}
+
+nsapi_size_or_error_t LWIP::socket_sendmsg(nsapi_socket_t handle, const SocketAddress &address,
+                                           const void *data, nsapi_size_t size,
+                                           nsapi_msghdr_t *control, nsapi_size_t control_size)
+{
+    struct mbed_lwip_socket *s = (struct mbed_lwip_socket *)handle;
+    ip_addr_t ip_addr = {};
+
+    // Used for backup the bound address if the packet must be sent from a specific address,
+    ip_addr_t bound_addr = {};
+    ip_addr_t src_addr = {};
+
+    nsapi_pktinfo_t *pkt_info = nullptr;
 
     nsapi_addr_t addr = address.get_addr();
     if (!convert_mbed_addr_to_lwip(&ip_addr, &addr)) {
         return NSAPI_ERROR_PARAMETER;
     }
-    struct netif *netif_ = netif_get_by_index(s->conn->pcb.ip->netif_idx);
+
+    // We try to extract the pktinfo from the header
+
+    if (control) {
+        MsgHeaderIterator it(control, control_size);
+        while (it.has_next()) {
+            auto *hdr = it.next();
+            if (hdr->level == NSAPI_SOCKET && hdr->type == NSAPI_PKTINFO) {
+                pkt_info = reinterpret_cast<nsapi_pktinfo_t *>(hdr);
+                break;
+            }
+        }
+    }
+
+    if (pkt_info) {
+        if (!convert_mbed_addr_to_lwip(&src_addr, &pkt_info->ipi_addr)) {
+            return NSAPI_ERROR_PARAMETER;
+        }
+    }
+
+    struct netif *netif_ = nullptr;
+
+    if (pkt_info) {
+        netif_ = netif_get_by_index(pkt_info->ipi_ifindex);
+    } else {
+        netif_ = netif_get_by_index(s->conn->pcb.ip->netif_idx);
+    }
     if (!netif_) {
         netif_ = &default_interface->netif;
     }
+
     if (netif_) {
         if ((addr.version == NSAPI_IPv4 && !get_ipv4_addr(netif_)) ||
                 (addr.version == NSAPI_IPv6 && !get_ipv6_addr(netif_) && !get_ipv6_link_local_addr(netif_))) {
             return NSAPI_ERROR_PARAMETER;
         }
     }
+
     struct netbuf *buf = netbuf_new();
 
     err_t err = netbuf_ref(buf, data, (u16_t)size);
@@ -465,36 +551,29 @@ nsapi_size_or_error_t LWIP::socket_sendto(nsapi_socket_t handle, const SocketAdd
         return err_remap(err);
     }
 
-    err = netconn_sendto(s->conn, buf, &ip_addr, address.get_port());
-    netbuf_delete(buf);
-    if (err != ERR_OK) {
-        return err_remap(err);
+    // handle src destination if required
+    if (pkt_info) {
+        // Backup the bound address
+        ip_addr_copy(bound_addr, s->conn->pcb.udp->local_ip);
+        // replace it with the source address
+        if (!ip_addr_isany(&src_addr)) {
+            ip_addr_copy(s->conn->pcb.udp->local_ip, src_addr);
+        }
     }
 
-    return size;
-}
+    err = netconn_sendto(s->conn, buf, &ip_addr, address.get_port());
 
-nsapi_size_or_error_t LWIP::socket_recvfrom(nsapi_socket_t handle, SocketAddress *address, void *data, nsapi_size_t size)
-{
-    struct mbed_lwip_socket *s = (struct mbed_lwip_socket *)handle;
-    struct netbuf *buf;
+    if (pkt_info) {
+        // restore bound address
+        ip_addr_copy(s->conn->pcb.udp->local_ip, bound_addr);
+    }
 
-    err_t err = netconn_recv(s->conn, &buf);
+    netbuf_delete(buf);
     if (err != ERR_OK) {
         return err_remap(err);
     }
 
-    if (address) {
-        nsapi_addr_t addr;
-        convert_lwip_addr_to_mbed(&addr, netbuf_fromaddr(buf));
-        address->set_addr(addr);
-        address->set_port(netbuf_fromport(buf));
-    }
-
-    u16_t recv = netbuf_copy(buf, data, (u16_t)size);
-    netbuf_delete(buf);
-
-    return recv;
+    return size;
 }
 
 int32_t LWIP::find_multicast_member(const struct mbed_lwip_socket *s, const nsapi_ip_mreq_t *imr)
@@ -687,6 +766,19 @@ nsapi_error_t LWIP::setsockopt(nsapi_socket_t handle, int level, int optname, co
             }
             s->conn->pcb.ip->tos = (u8_t)(*(const int *)optval);
             return 0;
+
+        case NSAPI_PKTINFO:
+            // FIXME: Turn NETCONN_FLAG_PKTINFO off by default
+            if (optlen != sizeof(int)) {
+                return NSAPI_ERROR_UNSUPPORTED;
+            }
+            if (*(const int *)optval) {
+                s->conn->flags |= NETCONN_FLAG_PKTINFO;
+            } else {
+                s->conn->flags &= ~NETCONN_FLAG_PKTINFO;
+            }
+            return 0;
+
         default:
             return NSAPI_ERROR_UNSUPPORTED;
     }

connectivity/nanostack/include/nanostack-interface/Nanostack.h

@@ -302,6 +302,21 @@ class Nanostack : public OnboardNetworkStack, private mbed::NonCopyable<Nanostac
      */
     nsapi_error_t getsockopt(void *handle, int level, int optname, void *optval, unsigned *optlen) override;
 
+    // FIXME: Implement
+    nsapi_size_or_error_t socket_sendmsg(nsapi_socket_t handle, const SocketAddress &address,
+                                         const void *data, nsapi_size_t size,
+                                         nsapi_msghdr_t *control, nsapi_size_t control_size) override
+    {
+        return NSAPI_ERROR_UNSUPPORTED;
+    }
+
+    nsapi_size_or_error_t socket_recvmsg(nsapi_socket_t handle, SocketAddress *address,
+                                         void *data, nsapi_size_t size,
+                                         nsapi_msghdr_t *control, nsapi_size_t control_size) override
+    {
+        return NSAPI_ERROR_UNSUPPORTED;
+    }
+
 private:
 
     /** Call in callback

connectivity/netsocket/include/netsocket/InternetDatagramSocket.h

@@ -36,6 +36,7 @@ class InternetDatagramSocket : public InternetSocket {
      *  nonblocking or times out, NSAPI_ERROR_WOULD_BLOCK is returned
      *  immediately.
      *
+     *  It uses sendmsg with zero ancillary data
      *  @param address  The SocketAddress of the remote host.
      *  @param data     Buffer of data to send to the host.
      *  @param size     Size of the buffer in bytes.
@@ -60,7 +61,7 @@ class InternetDatagramSocket : public InternetSocket {
      *  are accepted.
      *
      *  @note recvfrom() is allowed write to address and data buffers even if error occurs.
-     *
+     *  It uses recvmsg with zero ancillary data
      *  @param address  Destination for the source address or NULL.
      *  @param data     Destination buffer for RAW data to be received from the host.
      *  @param size     Size of the buffer in bytes.
@@ -74,6 +75,58 @@ class InternetDatagramSocket : public InternetSocket {
     nsapi_size_or_error_t recvfrom(SocketAddress *address,
                                    void *data, nsapi_size_t size) override;
 
+    /** Send datagram and ancillary data to the specified address.
+     *
+     *  By default, sendto blocks until data is sent. If socket is set to
+     *  nonblocking or times out, NSAPI_ERROR_WOULD_BLOCK is returned
+     *  immediately.
+     *
+     *  It uses sendmsg with zero ancillary data
+     *  @param address  The SocketAddress of the remote host.
+     *  @param data     Buffer of data to send to the host.
+     *  @param size     Size of the buffer in bytes.
+     *  @param control     Size of the buffer in bytes.
+     *  @param control_size     Size of the buffer in bytes.
+     *  @retval         NSAPI_ERROR_NO_SOCKET in case socket was not created correctly.
+     *  @retval         NSAPI_ERROR_WOULD_BLOCK in case non-blocking mode is enabled
+     *                  and send cannot be performed immediately.
+     *  @retval         int Other negative error codes for stack-related failures.
+     *                  See \ref NetworkStack::socket_send.
+     */
+    nsapi_size_or_error_t sendmsg(const SocketAddress &address,
+                                          const void *data, nsapi_size_t size, 
+                                          nsapi_msghdr_t* control, nsapi_size_t control_size) override;
+
+
+	 /** Receive a datagram with ancillary data and store the source address in address if it's not NULL.
+     *
+     *  By default, recvfrom blocks until a datagram is received. If socket is set to
+     *  nonblocking or times out with no datagram, NSAPI_ERROR_WOULD_BLOCK
+     *  is returned.
+     *  Ancillary data  is stored in msghdr struct
+     *  @note If the datagram is larger than the buffer, the excess data is silently discarded.
+     *
+     *  @note If socket is connected, only packets coming from connected peer address
+     *  are accepted.
+     *
+     *  @note recvfrom() is allowed write to address and data buffers even if error occurs.
+     *
+     *  @param address  Destination for the source address or NULL.
+     *  @param data     Destination buffer for RAW data to be received from the host.
+     *  @param size     Size of the buffer in bytes.
+     *  @param control     Size of the buffer in bytes.
+     *  @param control_size     Size of the buffer in bytes.
+     *  @retval         int Number of received bytes on success.
+     *  @retval         NSAPI_ERROR_NO_SOCKET in case socket was not created correctly.
+     *  @retval         NSAPI_ERROR_WOULD_BLOCK in case non-blocking mode is enabled
+     *                  and send cannot be performed immediately.
+     *  @retval         int Other negative error codes for stack-related failures.
+     *                  See \ref NetworkStack::socket_recv.
+     */
+    nsapi_size_or_error_t recvmsg(SocketAddress *address,
+                                  void *data, nsapi_size_t size,
+                                  nsapi_msghdr_t *control, nsapi_size_t control_size) override;
+
     /** Set the remote address for next send() call and filtering
      *  of incoming packets. To reset the address, zero initialized
      *  SocketAddress must be in the address parameter.