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

Author: tymoteuszblochmobica

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

@@ -163,6 +163,9 @@ class LWIP : public OnboardNetworkStack, private mbed::NonCopyable<LWIP> {
         static void netif_status_irq(struct netif *netif);
         static Interface *our_if_from_netif(struct netif *netif);
         static void delete_interface(OnboardNetworkStack::Interface **interface_out);
+        NetworkInterface *network_if_from_netif_id(int id);
+        int netif_id_from_network_if(NetworkInterface *userInterface);
+
 
 #if LWIP_ETHERNET
         static err_t emac_low_level_output(struct netif *netif, struct pbuf *p);
@@ -222,6 +225,8 @@ class LWIP : public OnboardNetworkStack, private mbed::NonCopyable<LWIP> {
             void *hw; /**< alternative implementation pointer - used for PPP */
         };
 
+        NetworkInterface *user_network_interface;
+
         mbed_rtos_storage_semaphore_t remove_interface_sem;
         osSemaphoreId_t remove_interface;
         mbed_rtos_storage_semaphore_t linked_sem;
@@ -265,7 +270,7 @@ class LWIP : public OnboardNetworkStack, private mbed::NonCopyable<LWIP> {
      * @param[out] interface_out    pointer to stack interface object controlling the EMAC
      * @return                      NSAPI_ERROR_OK on success, or error code
      */
-    nsapi_error_t add_ethernet_interface(EMAC &emac, bool default_if, OnboardNetworkStack::Interface **interface_out) override;
+    nsapi_error_t add_ethernet_interface(EMAC &emac, bool default_if, OnboardNetworkStack::Interface **interface_out, NetworkInterface *user_network_interface = NULL) override;
 
     /** Register a network interface with the IP stack
      *
@@ -450,6 +455,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
      *
      *  The socket must be connected to a remote host. Returns the number of
@@ -493,6 +519,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 +530,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            MBED_CONF_LWIP_NETBUF_RECVINFO_ENABLED
+
 // Make sure we default these to off, so
 // LWIP doesn't default to on
 #ifndef LWIP_ARP

connectivity/lwipstack/lwip/test/unit/lwipopts.h

@@ -46,7 +46,7 @@
 #define LWIP_NETCONN                    !NO_SYS
 #define LWIP_SOCKET                     !NO_SYS
 #define LWIP_NETCONN_FULLDUPLEX         LWIP_SOCKET
-#define LWIP_NETBUF_RECVINFO            1
+#define LWIP_NETBUF_RECVINFO            MBED_CONF_NETBUF_RECVINFO_ENABLED
 #define LWIP_HAVE_LOOPIF                1
 #define TCPIP_THREAD_TEST
 

connectivity/lwipstack/source/LWIPInterface.cpp

@@ -42,6 +42,28 @@
 
 LWIP::Interface *LWIP::Interface::list;
 
+NetworkInterface *LWIP::Interface::network_if_from_netif_id(int id)
+{
+    for (Interface *interface = list; interface; interface = interface->next) {
+        if (id == netif_get_index(&interface->netif)) {
+            return interface->user_network_interface;
+        }
+    }
+    return NULL;
+}
+
+int LWIP::Interface::netif_id_from_network_if(NetworkInterface *userInterface)
+{
+    if (userInterface != NULL) {
+        for (Interface *interface = list; interface; interface = interface->next) {
+            if (userInterface == interface->user_network_interface) {
+                return netif_get_index(&interface->netif);
+            }
+        }
+    }
+    return 0;
+}
+
 LWIP::Interface *LWIP::Interface::our_if_from_netif(struct netif *netif)
 {
     for (Interface *interface = list; interface; interface = interface->next) {
@@ -408,7 +430,7 @@ LWIP::Interface::Interface() :
     list = this;
 }
 
-nsapi_error_t LWIP::add_ethernet_interface(EMAC &emac, bool default_if, OnboardNetworkStack::Interface **interface_out)
+nsapi_error_t LWIP::add_ethernet_interface(EMAC &emac, bool default_if, OnboardNetworkStack::Interface **interface_out, NetworkInterface *user_network_interface)
 {
 #if LWIP_ETHERNET
     Interface *interface = new (std::nothrow) Interface();
@@ -431,7 +453,7 @@ nsapi_error_t LWIP::add_ethernet_interface(EMAC &emac, bool default_if, OnboardN
 #endif
 
     interface->netif.hwaddr_len = 6;
-
+    interface->user_network_interface = user_network_interface;
     if (!netif_add(&interface->netif,
 #if LWIP_IPV4
                    0, 0, 0,

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"
@@ -271,7 +272,9 @@ nsapi_error_t LWIP::socket_open(nsapi_socket_t *handle, nsapi_protocol_t proto)
         arena_dealloc(s);
         return NSAPI_ERROR_NO_SOCKET;
     }
-
+#if LWIP_NETBUF_RECVINFO
+    s->conn->flags &= ~NETCONN_FLAG_PKTINFO;
+#endif
     netconn_set_nonblocking(s->conn, true);
     *(struct mbed_lwip_socket **)handle = s;
     return 0;
@@ -439,24 +442,111 @@ 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 LWIP_NETBUF_RECVINFO
+    if ((s->conn->flags & NETCONN_FLAG_PKTINFO) && control && control_size >= sizeof(nsapi_pktinfo_t)) {
+        nsapi_pktinfo_t *pkt_info = reinterpret_cast<nsapi_pktinfo *>(control);
+        memset(control, 0, control_size);
+        // 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->network_interface = default_interface->network_if_from_netif_id(buf->p->if_idx);
+    }
+#endif
+    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) {
+        int index = default_interface->netif_id_from_network_if((NetworkInterface *)pkt_info->network_interface);
+        netif_ = netif_get_by_index(index);
+    } 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 +555,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 +770,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:
+#if LWIP_NETBUF_RECVINFO
+            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;
+#endif
         default:
             return NSAPI_ERROR_UNSUPPORTED;
     }