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

@@ -503,6 +503,15 @@ 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;
 
+    // TODO: Add doc
+    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;
+
+    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;
+
     /** 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

@@ -74,6 +74,15 @@ class InternetDatagramSocket : public InternetSocket {
     nsapi_size_or_error_t recvfrom(SocketAddress *address,
                                    void *data, nsapi_size_t size) override;
 
+    // TODO: documentation
+    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;
+
+    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.

connectivity/netsocket/include/netsocket/MsgHeader.h

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2021 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed 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.
+ */
+
+/* @file MeshInterface.h Mesh network interface */
+/** \addtogroup netinterface */
+/** @{*/
+
+#ifndef MSG_HEADER_H
+#define MSG_HEADER_H
+
+#include "netsocket/nsapi_types.h"
+
+struct MsgHeaderIterator {
+    MsgHeaderIterator(nsapi_msghdr_t *hdr, nsapi_size_t size) :
+        start(hdr),
+        current(nullptr),
+        size(size)
+    {}
+
+    bool has_next()
+    {
+        if (current == nullptr) {
+            if (start != nullptr) {
+                return true;
+            } else {
+                return false;
+            }
+        }
+
+        if (current->len < sizeof(*current)) {
+            return false;
+        }
+
+        if ((reinterpret_cast<uint8_t *>(current) + current->len) >= (reinterpret_cast<uint8_t *>(start) + size)) {
+            return false;
+        }
+
+        return true;
+    }
+
+    nsapi_msghdr_t *next()
+    {
+        if (!has_next()) {
+            return nullptr;
+        }
+
+        if (current == nullptr) {
+            current = start;
+        } else {
+            current = reinterpret_cast<nsapi_msghdr_t *>(reinterpret_cast<uint8_t *>(current) + current->len);
+        }
+
+        return current;
+    }
+
+private:
+    nsapi_msghdr_t *start;
+    nsapi_msghdr_t *current;
+    nsapi_size_t size;
+};
+
+
+#endif
+
+/** @}*/