diff --git a/connectivity/cellular/include/cellular/framework/AT/AT_CellularStack.h b/connectivity/cellular/include/cellular/framework/AT/AT_CellularStack.h
index 0a96b9b1b9..8fbb9e5100 100644
--- a/connectivity/cellular/include/cellular/framework/AT/AT_CellularStack.h
+++ b/connectivity/cellular/include/cellular/framework/AT/AT_CellularStack.h
@@ -89,16 +89,16 @@ protected: // 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
+    nsapi_size_or_error_t socket_sendto_control(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
+    nsapi_size_or_error_t socket_recvfrom_control(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;
     }
diff --git a/connectivity/lwipstack/include/lwipstack/LWIPStack.h b/connectivity/lwipstack/include/lwipstack/LWIPStack.h
index df47d5f065..2707c52ef7 100644
--- a/connectivity/lwipstack/include/lwipstack/LWIPStack.h
+++ b/connectivity/lwipstack/include/lwipstack/LWIPStack.h
@@ -472,9 +472,9 @@ protected:
       *  @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;
+    nsapi_size_or_error_t socket_sendto_control(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
      *
@@ -519,7 +519,7 @@ protected:
      *  This call is non-blocking. If recvfrom would block,
      *  NSAPI_ERROR_WOULD_BLOCK is returned immediately.
      *
-     *  It uses socket_recvmsg with  zero ancillary data.
+     *  It uses socket_recvfrom_control 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
@@ -547,9 +547,9 @@ protected:
     *  @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;
+    nsapi_size_or_error_t socket_recvfrom_control(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
      *
diff --git a/connectivity/lwipstack/source/LWIPStack.cpp b/connectivity/lwipstack/source/LWIPStack.cpp
index 0645ae00c1..c67887e62d 100644
--- a/connectivity/lwipstack/source/LWIPStack.cpp
+++ b/connectivity/lwipstack/source/LWIPStack.cpp
@@ -443,18 +443,18 @@ 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);
+    return socket_sendto_control(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);
+    return socket_recvfrom_control(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)
+nsapi_size_or_error_t LWIP::socket_recvfrom_control(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;
     struct netbuf *buf;
@@ -491,9 +491,9 @@ nsapi_size_or_error_t LWIP::socket_recvmsg(nsapi_socket_t handle, SocketAddress
     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)
+nsapi_size_or_error_t LWIP::socket_sendto_control(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 = {};
diff --git a/connectivity/nanostack/include/nanostack-interface/Nanostack.h b/connectivity/nanostack/include/nanostack-interface/Nanostack.h
index b9cbee4f63..09ffc51f1e 100644
--- a/connectivity/nanostack/include/nanostack-interface/Nanostack.h
+++ b/connectivity/nanostack/include/nanostack-interface/Nanostack.h
@@ -302,17 +302,16 @@ protected:
      */
     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
+    nsapi_size_or_error_t socket_sendto_control(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
+    nsapi_size_or_error_t socket_recvfrom_control(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;
     }
diff --git a/connectivity/netsocket/include/netsocket/CellularNonIPSocket.h b/connectivity/netsocket/include/netsocket/CellularNonIPSocket.h
index c4c76c59ec..0d206aec3c 100644
--- a/connectivity/netsocket/include/netsocket/CellularNonIPSocket.h
+++ b/connectivity/netsocket/include/netsocket/CellularNonIPSocket.h
@@ -133,13 +133,13 @@ public:
     nsapi_size_or_error_t recvfrom(SocketAddress *address,
                                    void *data, nsapi_size_t size) override;
     /// NOT APPLICABLE
-    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 sendto_control(const SocketAddress &address,
+                                         const void *data, nsapi_size_t size,
+                                         nsapi_msghdr_t *control, nsapi_size_t control_size) override;
     /// NOT APPLICABLE
-    nsapi_size_or_error_t recvmsg(SocketAddress *address,
-                                  void *data, nsapi_size_t size,
-                                  nsapi_msghdr_t *control, nsapi_size_t control_size) override;
+    nsapi_size_or_error_t recvfrom_control(SocketAddress *address,
+                                           void *data, nsapi_size_t size,
+                                           nsapi_msghdr_t *control, nsapi_size_t control_size) override;
     /// NOT APPLICABLE
     nsapi_error_t bind(const SocketAddress &address) override;
 
diff --git a/connectivity/netsocket/include/netsocket/InternetDatagramSocket.h b/connectivity/netsocket/include/netsocket/InternetDatagramSocket.h
index 7cbf85a611..ceac79f4db 100644
--- a/connectivity/netsocket/include/netsocket/InternetDatagramSocket.h
+++ b/connectivity/netsocket/include/netsocket/InternetDatagramSocket.h
@@ -36,7 +36,7 @@ public:
      *  nonblocking or times out, NSAPI_ERROR_WOULD_BLOCK is returned
      *  immediately.
      *
-     *  It uses sendmsg with zero ancillary data
+     *  It uses sendto_control 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.
@@ -61,7 +61,7 @@ public:
      *  are accepted.
      *
      *  @note recvfrom() is allowed write to address and data buffers even if error occurs.
-     *  It uses recvmsg with zero ancillary data
+     *  It uses recvfrom_control 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.
@@ -81,7 +81,7 @@ public:
      *  nonblocking or times out, NSAPI_ERROR_WOULD_BLOCK is returned
      *  immediately.
      *
-     *  It uses sendmsg with zero ancillary data
+     *  It uses sendto_control 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.
@@ -93,9 +93,9 @@ public:
      *  @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;
+    nsapi_size_or_error_t sendto_control(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.
@@ -109,7 +109,7 @@ public:
     *  @note If socket is connected, only packets coming from connected peer address
     *  are accepted.
     *
-    *  @note recvmsg() is allowed write to address and data buffers even if error occurs.
+    *  @note recvfrom_control() 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.
@@ -123,9 +123,9 @@ public:
     *  @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;
+    nsapi_size_or_error_t recvfrom_control(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
diff --git a/connectivity/netsocket/include/netsocket/MsgHeader.h b/connectivity/netsocket/include/netsocket/MsgHeader.h
index fccf05370b..eebf1a63ed 100644
--- a/connectivity/netsocket/include/netsocket/MsgHeader.h
+++ b/connectivity/netsocket/include/netsocket/MsgHeader.h
@@ -20,17 +20,24 @@
 
 #include "netsocket/nsapi_types.h"
 
+/**
+ * Allows iteration through the list of message headers received in the control parameter of the
+ * socket_sendto_control / socket_recvfrom_control methods.
+ */
+
 struct MsgHeaderIterator {
+    // Constructor takes pointer to the first header element and size of the whole list.
     MsgHeaderIterator(nsapi_msghdr_t *hdr, nsapi_size_t size) :
         start(hdr),
         current(nullptr),
         size(size)
     {}
 
+    // Checks if the next address of the iterator is a valid list member.
     bool has_next()
     {
         if (current == nullptr) {
-            if (start != nullptr) {
+            if (start != nullptr && start->len <= size && start->len >= sizeof(*start)) {
                 return true;
             } else {
                 return false;
@@ -41,13 +48,15 @@ struct MsgHeaderIterator {
             return false;
         }
 
-        if ((reinterpret_cast<uint8_t *>(current) + current->len) >= (reinterpret_cast<uint8_t *>(start) + size)) {
+        if (get_next_aligned_addr() >= (reinterpret_cast<uint8_t *>(start) + size)) {
             return false;
         }
 
         return true;
     }
 
+    // Returns pointer to the next member of the list.
+    // If next member doesn't exist nullptr is returned.
     nsapi_msghdr_t *next()
     {
         if (!has_next()) {
@@ -57,13 +66,29 @@ struct MsgHeaderIterator {
         if (current == nullptr) {
             current = start;
         } else {
-            current = reinterpret_cast<nsapi_msghdr_t *>(reinterpret_cast<uint8_t *>(current) + current->len);
+            current = reinterpret_cast<nsapi_msghdr *>(get_next_aligned_addr());
         }
 
         return current;
     }
 
 private:
+    // Get address of the next member aligned to the size of msghdr_t.
+    void *get_next_aligned_addr()
+    {
+        size_t remaining_size = size - (reinterpret_cast<uintptr_t>(current) - reinterpret_cast<uintptr_t>(start));
+        void *next = reinterpret_cast<void *>(reinterpret_cast<uint8_t *>(current) + current->len);
+
+        next = std::align(
+                   alignof(nsapi_msghdr_t),
+                   sizeof(nsapi_msghdr_t),
+                   next,
+                   remaining_size
+               );
+
+        return next;
+    }
+
     nsapi_msghdr_t *start;
     nsapi_msghdr_t *current;
     nsapi_size_t size;
diff --git a/connectivity/netsocket/include/netsocket/NetworkStack.h b/connectivity/netsocket/include/netsocket/NetworkStack.h
index 8118372095..6d67566367 100644
--- a/connectivity/netsocket/include/netsocket/NetworkStack.h
+++ b/connectivity/netsocket/include/netsocket/NetworkStack.h
@@ -412,9 +412,16 @@ protected:
      *  @return         Number of sent bytes on success, negative error
      *                  code on failure
      */
-    virtual 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) = 0;
+    virtual nsapi_size_or_error_t socket_sendto_control(nsapi_socket_t handle, const SocketAddress &address,
+                                                        const void *data, nsapi_size_t size,
+                                                        nsapi_msghdr_t *control, nsapi_size_t control_size)
+    {
+        if (control != NULL) {
+            return NSAPI_ERROR_UNSUPPORTED;
+        }
+
+        return socket_sendto(handle, address, data, size);
+    }
 
     /** Receive a packet with ancillary data over a UDP socket
      *
@@ -436,9 +443,16 @@ protected:
      *  @return         Number of received bytes on success, negative error
      *                  code on failure
      */
-    virtual 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) = 0;
+    virtual nsapi_size_or_error_t socket_recvfrom_control(nsapi_socket_t handle, SocketAddress *address,
+                                                          void *data, nsapi_size_t size,
+                                                          nsapi_msghdr_t *control, nsapi_size_t control_size)
+    {
+        if (control != NULL) {
+            return NSAPI_ERROR_UNSUPPORTED;
+        }
+
+        return socket_recvfrom(handle, address, data, size);
+    }
 
     /** Register a callback on state change of the socket
      *
diff --git a/connectivity/netsocket/include/netsocket/Socket.h b/connectivity/netsocket/include/netsocket/Socket.h
index 5d10fdcbd2..6ef5635ed2 100644
--- a/connectivity/netsocket/include/netsocket/Socket.h
+++ b/connectivity/netsocket/include/netsocket/Socket.h
@@ -159,7 +159,7 @@ public:
 
     /** Send a message on a socket.
      *
-     * The sendmsg() function sends a message through a connection-mode or connectionless-mode socket.
+     * The sendto_control() function sends a message through a connection-mode or connectionless-mode socket.
      * If the socket is a connectionless-mode socket, the message is sent to the address specified.
      * If the socket is a connected-mode socket, address is ignored.
      *
@@ -175,9 +175,9 @@ public:
      *  @return         Number of sent bytes on success, negative subclass-dependent error
      *                  code on failure
      */
-    virtual 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) = 0;
+    virtual nsapi_size_or_error_t sendto_control(const SocketAddress &address,
+                                                 const void *data, nsapi_size_t size,
+                                                 nsapi_msghdr_t *control, nsapi_size_t control_size) = 0;
 
 
     /** Receive a data from a socket
@@ -190,7 +190,7 @@ public:
      *
      * Additional information related to the message can be retrieved with the control data.
      *
-     *  @note recvmsg() is allowed write to address and data buffers even if error occurs.
+     *  @note recvfrom_control() is allowed write to address and data buffers even if error occurs.
      *
      *  By default, recvfrom blocks until a datagram is received. If socket is set to
      *  non-blocking or times out with no data, NSAPI_ERROR_WOULD_BLOCK
@@ -202,9 +202,9 @@ public:
      *  @return         Number of received bytes on success, negative subclass-dependent
      *                  error code on failure
      */
-    virtual nsapi_size_or_error_t recvmsg(SocketAddress *address,
-                                          void *data, nsapi_size_t size,
-                                          nsapi_msghdr_t *control, nsapi_size_t control_size) = 0;
+    virtual nsapi_size_or_error_t recvfrom_control(SocketAddress *address,
+                                                   void *data, nsapi_size_t size,
+                                                   nsapi_msghdr_t *control, nsapi_size_t control_size) = 0;
 
     /** Bind a specific address to a socket.
      *
diff --git a/connectivity/netsocket/include/netsocket/TCPSocket.h b/connectivity/netsocket/include/netsocket/TCPSocket.h
index 877bfe26cb..05f1d11da0 100644
--- a/connectivity/netsocket/include/netsocket/TCPSocket.h
+++ b/connectivity/netsocket/include/netsocket/TCPSocket.h
@@ -149,7 +149,7 @@ public:
     *
     * TCP socket is connection oriented protocol, so address is ignored.
     *
-    * By default, sendmsg blocks until data is sent. If socket is set to
+    * By default, sendto_control blocks until data is sent. If socket is set to
     * non-blocking or times out, NSAPI_ERROR_WOULD_BLOCK is returned
     * immediately.
     *
@@ -163,16 +163,16 @@ public:
     *  @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;
+    nsapi_size_or_error_t sendto_control(const SocketAddress &address,
+                                         const void *data, nsapi_size_t size,
+                                         nsapi_msghdr_t *control, nsapi_size_t control_size) override;
 
     /** Receive a packet with ancillary data from a socket
      *
      *  Receives a data and stores the source address in address if address
      *  is not NULL. Returns the number of bytes written into the buffer.
      *
-     *  By default, recvmsg blocks until a data is received. If socket is set to
+     *  By default, recvfrom_control blocks until a data is received. If socket is set to
      *  non-blocking or times out with no datagram, NSAPI_ERROR_WOULD_BLOCK
      *  is returned.
      *
@@ -188,9 +188,9 @@ public:
      *  @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;
+    nsapi_size_or_error_t recvfrom_control(SocketAddress *address,
+                                           void *data, nsapi_size_t size,
+                                           nsapi_msghdr_t *control, nsapi_size_t control_size) override;
 
     /** Accepts a connection on a socket.
      *
diff --git a/connectivity/netsocket/include/netsocket/TLSSocketWrapper.h b/connectivity/netsocket/include/netsocket/TLSSocketWrapper.h
index bcf2c335ee..1ea28d8f61 100644
--- a/connectivity/netsocket/include/netsocket/TLSSocketWrapper.h
+++ b/connectivity/netsocket/include/netsocket/TLSSocketWrapper.h
@@ -186,12 +186,12 @@ public:
     nsapi_size_or_error_t recvfrom(SocketAddress *address,
                                    void *data, nsapi_size_t size) override;
 
-    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;
+    nsapi_size_or_error_t sendto_control(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 recvfrom_control(SocketAddress *address,
+                                           void *data, nsapi_size_t size,
+                                           nsapi_msghdr_t *control, nsapi_size_t control_size) override;
 
     nsapi_error_t bind(const SocketAddress &address) override;
     void set_blocking(bool blocking) override;
diff --git a/connectivity/netsocket/include/netsocket/nsapi_types.h b/connectivity/netsocket/include/netsocket/nsapi_types.h
index 77ae8cd487..e4a8ab09c6 100644
--- a/connectivity/netsocket/include/netsocket/nsapi_types.h
+++ b/connectivity/netsocket/include/netsocket/nsapi_types.h
@@ -22,6 +22,7 @@
 #define NSAPI_TYPES_H
 
 #include <stdint.h>
+#include "mbed_toolchain.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -319,7 +320,7 @@ typedef enum nsapi_socket_option {
     NSAPI_STAGGER,           /*!< Read estimated stagger value to destination */
     NSAPI_IPTOS,             /*!< Set IP type of service to set specific precedence */
     NSAPI_BROADCAST,         /*!< Set broadcast flag for UDP socket */
-    NSAPI_PKTINFO            /*!< Get additional information when using sendmsg/recvmsg */
+    NSAPI_PKTINFO            /*!< Get additional information when using sendto_control/recvfrom_control */
 } nsapi_socket_option_t;
 
 typedef enum nsapi_tlssocket_level {
@@ -409,7 +410,7 @@ typedef struct nsapi_stagger_req {
 
 /** nsapi_msghdr
  */
-typedef struct nsapi_msghdr {
+typedef struct MBED_ALIGN(double) nsapi_msghdr {
     nsapi_size_t len;    /* Data byte count, including header */
     int          level;  /* Originating protocol */
     int          type;   /* Protocol-specific type */
@@ -663,15 +664,15 @@ typedef struct nsapi_stack_api {
                                              nsapi_addr_t *addr, uint16_t *port, void *buffer, nsapi_size_t size);
 
     // TODO: Documentation
-    nsapi_size_or_error_t (*socket_sendmsg)(nsapi_stack_t *stack, nsapi_socket_t socket,
-                                            nsapi_addr_t addr, uint16_t port,
-                                            const void *data, nsapi_size_t size,
-                                            const nsapi_msghdr_t *control, nsapi_size_t control_size);
+    nsapi_size_or_error_t (*socket_sendto_control)(nsapi_stack_t *stack, nsapi_socket_t socket,
+                                                   nsapi_addr_t addr, uint16_t port,
+                                                   const void *data, nsapi_size_t size,
+                                                   const nsapi_msghdr_t *control, nsapi_size_t control_size);
 
-    nsapi_size_or_error_t (*socket_recvmsg)(nsapi_stack_t *stack, nsapi_socket_t socket,
-                                            nsapi_addr_t *addr, uint16_t *port,
-                                            void *data, nsapi_size_t size,
-                                            nsapi_msghdr_t *control, nsapi_size_t control_size);
+    nsapi_size_or_error_t (*socket_recvfrom_control)(nsapi_stack_t *stack, nsapi_socket_t socket,
+                                                     nsapi_addr_t *addr, uint16_t *port,
+                                                     void *data, nsapi_size_t size,
+                                                     nsapi_msghdr_t *control, nsapi_size_t control_size);
 
     /** Register a callback on state change of the socket
      *
diff --git a/connectivity/netsocket/source/CellularNonIPSocket.cpp b/connectivity/netsocket/source/CellularNonIPSocket.cpp
index 622acc1a30..11a0786a3e 100644
--- a/connectivity/netsocket/source/CellularNonIPSocket.cpp
+++ b/connectivity/netsocket/source/CellularNonIPSocket.cpp
@@ -244,9 +244,9 @@ nsapi_size_or_error_t CellularNonIPSocket::sendto(const SocketAddress &address,
 {
     return NSAPI_ERROR_UNSUPPORTED;
 }
-nsapi_size_or_error_t CellularNonIPSocket::sendmsg(const SocketAddress &address,
-                                                   const void *data, nsapi_size_t size,
-                                                   nsapi_msghdr_t *control, nsapi_size_t control_size)
+nsapi_size_or_error_t CellularNonIPSocket::sendto_control(const SocketAddress &address,
+                                                          const void *data, nsapi_size_t size,
+                                                          nsapi_msghdr_t *control, nsapi_size_t control_size)
 {
     return NSAPI_ERROR_UNSUPPORTED;
 }
@@ -257,9 +257,9 @@ nsapi_size_or_error_t CellularNonIPSocket::recvfrom(SocketAddress *address,
     return NSAPI_ERROR_UNSUPPORTED;
 }
 
-nsapi_size_or_error_t CellularNonIPSocket::recvmsg(SocketAddress *address,
-                                                   void *data, nsapi_size_t size,
-                                                   nsapi_msghdr_t *control, nsapi_size_t control_size)
+nsapi_size_or_error_t CellularNonIPSocket::recvfrom_control(SocketAddress *address,
+                                                            void *data, nsapi_size_t size,
+                                                            nsapi_msghdr_t *control, nsapi_size_t control_size)
 {
     return NSAPI_ERROR_UNSUPPORTED;
 }
diff --git a/connectivity/netsocket/source/InternetDatagramSocket.cpp b/connectivity/netsocket/source/InternetDatagramSocket.cpp
index 3794b85113..fdf5ad8df1 100644
--- a/connectivity/netsocket/source/InternetDatagramSocket.cpp
+++ b/connectivity/netsocket/source/InternetDatagramSocket.cpp
@@ -27,7 +27,9 @@ nsapi_error_t InternetDatagramSocket::connect(const SocketAddress &address)
     return NSAPI_ERROR_OK;
 }
 
-nsapi_size_or_error_t InternetDatagramSocket::sendmsg(const SocketAddress &address, const void *data, nsapi_size_t size, nsapi_msghdr_t *control, nsapi_size_t control_size)
+nsapi_size_or_error_t InternetDatagramSocket::sendto_control(const SocketAddress &address, const void *data,
+                                                             nsapi_size_t size, nsapi_msghdr_t *control,
+                                                             nsapi_size_t control_size)
 {
     _lock.lock();
     nsapi_size_or_error_t ret;
@@ -44,7 +46,8 @@ nsapi_size_or_error_t InternetDatagramSocket::sendmsg(const SocketAddress &addre
         }
 
         core_util_atomic_flag_clear(&_pending);
-        nsapi_size_or_error_t sent = _stack->socket_sendmsg(_socket, address, data, size, control, control_size);
+        nsapi_size_or_error_t sent = _stack->socket_sendto_control(_socket, address, data, size, control,
+                                                                   control_size);
         if ((0 == _timeout) || (NSAPI_ERROR_WOULD_BLOCK != sent)) {
             _socket_stats.stats_update_sent_bytes(this, sent);
             ret = sent;
@@ -77,7 +80,7 @@ nsapi_size_or_error_t InternetDatagramSocket::sendmsg(const SocketAddress &addre
 nsapi_size_or_error_t InternetDatagramSocket::sendto(const SocketAddress &address, const void *data, nsapi_size_t size)
 {
 
-    return sendmsg(address, data, size, NULL, 0);
+    return sendto_control(address, data, size, NULL, 0);
 }
 
 nsapi_size_or_error_t InternetDatagramSocket::send(const void *data, nsapi_size_t size)
@@ -88,8 +91,8 @@ nsapi_size_or_error_t InternetDatagramSocket::send(const void *data, nsapi_size_
     return sendto(_remote_peer, data, size);
 }
 
-
-nsapi_size_or_error_t InternetDatagramSocket::recvmsg(SocketAddress *address, void *buffer, nsapi_size_t size, nsapi_msghdr_t *control, nsapi_size_t control_size)
+nsapi_size_or_error_t InternetDatagramSocket::recvfrom_control(SocketAddress *address, void *buffer, nsapi_size_t size,
+                                                               nsapi_msghdr_t *control, nsapi_size_t control_size)
 {
     _lock.lock();
     nsapi_size_or_error_t ret;
@@ -111,7 +114,8 @@ nsapi_size_or_error_t InternetDatagramSocket::recvmsg(SocketAddress *address, vo
         }
 
         core_util_atomic_flag_clear(&_pending);
-        nsapi_size_or_error_t recv = _stack->socket_recvmsg(_socket, address, buffer, size, control, control_size);
+        nsapi_size_or_error_t recv = _stack->socket_recvfrom_control(_socket, address, buffer, size, control,
+                                                                     control_size);
 
         // Filter incomming packets using connected peer address
         if (recv >= 0 && _remote_peer && _remote_peer != *address) {
@@ -152,7 +156,7 @@ nsapi_size_or_error_t InternetDatagramSocket::recvmsg(SocketAddress *address, vo
 
 nsapi_size_or_error_t InternetDatagramSocket::recvfrom(SocketAddress *address, void *buffer, nsapi_size_t size)
 {
-    return recvmsg(address, buffer, size, NULL, 0);
+    return recvfrom_control(address, buffer, size, NULL, 0);
 }
 
 nsapi_size_or_error_t InternetDatagramSocket::recv(void *buffer, nsapi_size_t size)
diff --git a/connectivity/netsocket/source/NetworkStack.cpp b/connectivity/netsocket/source/NetworkStack.cpp
index 83949f0055..7ece7b6e5f 100644
--- a/connectivity/netsocket/source/NetworkStack.cpp
+++ b/connectivity/netsocket/source/NetworkStack.cpp
@@ -399,33 +399,39 @@ protected:
         return err;
     }
 
-    nsapi_size_or_error_t socket_sendmsg(nsapi_socket_t socket, 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 socket_sendto_control(nsapi_socket_t socket, const SocketAddress &address, const void *data,
+                                                nsapi_size_t size, nsapi_msghdr_t *control,
+                                                nsapi_size_t control_size) override
     {
         if (control != NULL) {
             return NSAPI_ERROR_UNSUPPORTED;
         }
 
-        if (!_stack_api()->socket_sendmsg) {
+        if (!_stack_api()->socket_sendto_control) {
             return NSAPI_ERROR_UNSUPPORTED;
         }
 
-        return _stack_api()->socket_sendmsg(_stack(), socket, address.get_addr(), address.get_port(), data, size, control, control_size);
+        return _stack_api()->socket_sendto_control(_stack(), socket, address.get_addr(), address.get_port(), data,
+                                                   size, control, control_size);
     }
 
-    nsapi_size_or_error_t socket_recvmsg(nsapi_socket_t socket, SocketAddress *address, void *data, nsapi_size_t size, nsapi_msghdr_t *control, nsapi_size_t control_size) override
+    nsapi_size_or_error_t socket_recvfrom_control(nsapi_socket_t socket, SocketAddress *address, void *data,
+                                                  nsapi_size_t size, nsapi_msghdr_t *control,
+                                                  nsapi_size_t control_size) override
     {
         if (control != NULL) {
             return NSAPI_ERROR_UNSUPPORTED;
         }
 
-        if (!_stack_api()->socket_recvmsg) {
+        if (!_stack_api()->socket_recvfrom_control) {
             return NSAPI_ERROR_UNSUPPORTED;
         }
 
         nsapi_addr_t addr = {NSAPI_IPv4, 0};
         uint16_t port = 0;
 
-        nsapi_size_or_error_t err = _stack_api()->socket_recvmsg(_stack(), socket, &addr, &port, data, size, control, control_size);
+        nsapi_size_or_error_t err = _stack_api()->socket_recvfrom_control(_stack(), socket, &addr, &port, data, size,
+                                                                          control, control_size);
 
         if (address) {
             address->set_addr(addr);
diff --git a/connectivity/netsocket/source/TCPSocket.cpp b/connectivity/netsocket/source/TCPSocket.cpp
index f749aff97f..bf75e589e3 100644
--- a/connectivity/netsocket/source/TCPSocket.cpp
+++ b/connectivity/netsocket/source/TCPSocket.cpp
@@ -173,9 +173,8 @@ nsapi_size_or_error_t TCPSocket::sendto(const SocketAddress &address, const void
     return send(data, size);
 }
 
-nsapi_size_or_error_t TCPSocket::sendmsg(const SocketAddress &address,
-                                         const void *data, nsapi_size_t size,
-                                         nsapi_msghdr_t *control, nsapi_size_t control_size)
+nsapi_size_or_error_t TCPSocket::sendto_control(const SocketAddress &address, const void *data, nsapi_size_t size,
+                                                nsapi_msghdr_t *control, nsapi_size_t control_size)
 {
     // FIXME: Implement
     if (control) {
@@ -240,9 +239,8 @@ nsapi_size_or_error_t TCPSocket::recvfrom(SocketAddress *address, void *data, ns
     return recv(data, size);
 }
 
-nsapi_size_or_error_t TCPSocket::recvmsg(SocketAddress *address,
-                                         void *data, nsapi_size_t size,
-                                         nsapi_msghdr_t *control, nsapi_size_t control_size)
+nsapi_size_or_error_t TCPSocket::recvfrom_control(SocketAddress *address, void *data, nsapi_size_t size,
+                                                  nsapi_msghdr_t *control, nsapi_size_t control_size)
 {
     // FIXME: Implement
     if (control) {
diff --git a/connectivity/netsocket/source/TLSSocketWrapper.cpp b/connectivity/netsocket/source/TLSSocketWrapper.cpp
index 44af1feaf6..dfb7cf0e79 100644
--- a/connectivity/netsocket/source/TLSSocketWrapper.cpp
+++ b/connectivity/netsocket/source/TLSSocketWrapper.cpp
@@ -385,9 +385,9 @@ nsapi_size_or_error_t TLSSocketWrapper::sendto(const SocketAddress &, const void
     return send(data, size);
 }
 
-nsapi_size_or_error_t TLSSocketWrapper::sendmsg(const SocketAddress &address,
-                                                const void *data, nsapi_size_t size,
-                                                nsapi_msghdr_t *control, nsapi_size_t control_size)
+nsapi_size_or_error_t TLSSocketWrapper::sendto_control(const SocketAddress &address, const void *data,
+                                                       nsapi_size_t size, nsapi_msghdr_t *control,
+                                                       nsapi_size_t control_size)
 {
     return sendto(address, data, size);
 }
@@ -452,7 +452,8 @@ nsapi_size_or_error_t TLSSocketWrapper::recvfrom(SocketAddress *address, void *d
     return recv(data, size);
 }
 
-nsapi_size_or_error_t TLSSocketWrapper::recvmsg(SocketAddress *address, void *data, nsapi_size_t size, nsapi_msghdr_t *control, nsapi_size_t control_size)
+nsapi_size_or_error_t TLSSocketWrapper::recvfrom_control(SocketAddress *address, void *data, nsapi_size_t size,
+                                                         nsapi_msghdr_t *control, nsapi_size_t control_size)
 {
     return recvfrom(address, data, size);
 }
diff --git a/connectivity/netsocket/tests/UNITTESTS/doubles/NetworkStack_stub.h b/connectivity/netsocket/tests/UNITTESTS/doubles/NetworkStack_stub.h
index 356e3bea4e..0c2d9bc7db 100644
--- a/connectivity/netsocket/tests/UNITTESTS/doubles/NetworkStack_stub.h
+++ b/connectivity/netsocket/tests/UNITTESTS/doubles/NetworkStack_stub.h
@@ -136,9 +136,9 @@ protected:
     {
         return return_value;
     };
-    virtual 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)
+    virtual nsapi_size_or_error_t socket_sendto_control(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 return_value;
     };
@@ -155,11 +155,15 @@ protected:
         }
         return return_value;
     };
-    virtual 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)
+    virtual nsapi_size_or_error_t socket_recvfrom_control(nsapi_socket_t handle, SocketAddress *address,
+                                                          void *data, nsapi_size_t size,
+                                                          nsapi_msghdr_t *control, nsapi_size_t control_size) override
     {
-        return return_value;
+        if (control != NULL) {
+            return NSAPI_ERROR_UNSUPPORTED;
+        }
+
+        return socket_recvfrom(handle, address, data, size);
     };
     virtual void socket_attach(nsapi_socket_t handle, void (*callback)(void *), void *data) {};
 
diff --git a/connectivity/netsocket/tests/UNITTESTS/doubles/OnboardNetworkStack_mock.h b/connectivity/netsocket/tests/UNITTESTS/doubles/OnboardNetworkStack_mock.h
index c6fe3882d1..79c3b3ffe1 100644
--- a/connectivity/netsocket/tests/UNITTESTS/doubles/OnboardNetworkStack_mock.h
+++ b/connectivity/netsocket/tests/UNITTESTS/doubles/OnboardNetworkStack_mock.h
@@ -46,12 +46,19 @@ public:
     MOCK_METHOD4(socket_recvfrom, nsapi_error_t(nsapi_socket_t handle, SocketAddress *address, void *data, nsapi_size_t size));
     MOCK_METHOD5(setsockopt, nsapi_error_t(nsapi_socket_t handle, int level, int optname, const void *optval, unsigned optlen));
     MOCK_METHOD5(getsockopt, nsapi_error_t(nsapi_socket_t handle, int level, int optname, const void *optval, unsigned *optlen));
-    MOCK_METHOD6(socket_sendmsg, nsapi_error_t(nsapi_socket_t handle, SocketAddress *address, void *data, nsapi_size_t size, nsapi_msghdr_t *control, nsapi_size_t control_size));
-    MOCK_METHOD6(socket_recvmsg, nsapi_error_t(nsapi_socket_t handle, SocketAddress *address, void *data, nsapi_size_t size, nsapi_msghdr_t *control, nsapi_size_t control_size));
 //    MOCK_METHOD3(socket_attach, void(nsapi_socket_t handle, void (*callback)(void *), void *data));
-    MOCK_METHOD3(add_ethernet_interface, nsapi_error_t(EMAC &emac, bool default_if, OnboardNetworkStack::Interface **interface_out));
     MOCK_METHOD3(add_ppp_interface, nsapi_error_t(PPP &ppp, bool default_if, OnboardNetworkStack::Interface **interface_out));
     MOCK_METHOD1(set_default_interface, void (OnboardNetworkStack::Interface *interface));
+    MOCK_METHOD4(add_ethernet_interface_mock, nsapi_error_t(EMAC &emac, bool default_if,
+                                                            OnboardNetworkStack::Interface **interface_out,
+                                                            NetworkInterface *user_network_interface));
+
+    // Wrapper written to handle function with the default argument in the gmock.
+    nsapi_error_t add_ethernet_interface(EMAC &emac, bool default_if, Interface **interface_out, NetworkInterface *user_network_interface = NULL)
+    {
+        return add_ethernet_interface_mock(emac, default_if, interface_out, user_network_interface);
+    }
+
     void *socket_cb;
     // No need to mock socket_attach really.
     void socket_attach(nsapi_socket_t handle, void (*callback)(void *), void *data)
@@ -59,7 +66,6 @@ public:
         socket_cb = data;
     };
 
-
     static OnboardNetworkStackMock &get_instance()
     {
         static OnboardNetworkStackMock stackMock1;
diff --git a/connectivity/netsocket/tests/UNITTESTS/netsocket/EthernetInterface/test_EthernetInterface.cpp b/connectivity/netsocket/tests/UNITTESTS/netsocket/EthernetInterface/test_EthernetInterface.cpp
index 8db41166ab..1b0851f439 100644
--- a/connectivity/netsocket/tests/UNITTESTS/netsocket/EthernetInterface/test_EthernetInterface.cpp
+++ b/connectivity/netsocket/tests/UNITTESTS/netsocket/EthernetInterface/test_EthernetInterface.cpp
@@ -68,7 +68,7 @@ protected:
     /* Enclose the heavily-used connection procedure to improve code redability */
     void doConnect(bool dhcp = true, bool blocking = true)
     {
-        EXPECT_CALL(*stackMock, add_ethernet_interface(testing::Ref(*emacMock), true, _))
+        EXPECT_CALL(*stackMock, add_ethernet_interface_mock(testing::Ref(*emacMock), true, _, _))
         .Times(1)
         .WillOnce(DoAll(SetArgPointee<2>(netStackIface), Return(NSAPI_ERROR_OK)));
         EXPECT_CALL(*netStackIface, attach(_))
@@ -113,7 +113,7 @@ TEST_F(TestEthernetInterface, connect)
 
 TEST_F(TestEthernetInterface, connect_failure)
 {
-    EXPECT_CALL(*stackMock, add_ethernet_interface(testing::Ref(*emacMock), true, _))
+    EXPECT_CALL(*stackMock, add_ethernet_interface_mock(testing::Ref(*emacMock), true, _, _))
     .Times(1)
     .WillOnce(DoAll(SetArgPointee<2>(netStackIface), Return(NSAPI_ERROR_NO_MEMORY)));
     EXPECT_EQ(NSAPI_ERROR_NO_MEMORY, iface->connect());
@@ -158,7 +158,7 @@ TEST_F(TestEthernetInterface, set_network)
     EXPECT_EQ(NSAPI_ERROR_OK, iface->set_network(ipAddress, netmask, gateway));
 
     // Now the bringup should have different arguments. We can't use doConnect method.
-    EXPECT_CALL(*stackMock, add_ethernet_interface(testing::Ref(*emacMock), true, _))
+    EXPECT_CALL(*stackMock, add_ethernet_interface_mock(testing::Ref(*emacMock), true, _, _))
     .Times(1)
     .WillOnce(DoAll(SetArgPointee<2>(netStackIface), Return(NSAPI_ERROR_OK)));
     EXPECT_CALL(*netStackIface, attach(_))
diff --git a/connectivity/netsocket/tests/UNITTESTS/netsocket/InternetSocket/test_InternetSocket.cpp b/connectivity/netsocket/tests/UNITTESTS/netsocket/InternetSocket/test_InternetSocket.cpp
index 9b6e10edcb..54ef26ac11 100644
--- a/connectivity/netsocket/tests/UNITTESTS/netsocket/InternetSocket/test_InternetSocket.cpp
+++ b/connectivity/netsocket/tests/UNITTESTS/netsocket/InternetSocket/test_InternetSocket.cpp
@@ -56,13 +56,13 @@ public:
     {
         return return_value;
     }
-    virtual 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)
+    virtual nsapi_size_or_error_t sendto_control(const SocketAddress &address, const void *data, nsapi_size_t size,
+                                                 nsapi_msghdr_t *control, nsapi_size_t control_size)
     {
         return return_value;
     }
-    virtual nsapi_size_or_error_t recvmsg(SocketAddress *address,
-                                          void *data, nsapi_size_t size, nsapi_msghdr_t *control, nsapi_size_t control_size)
+    virtual nsapi_size_or_error_t recvfrom_control(SocketAddress *address, void *data, nsapi_size_t size,
+                                                   nsapi_msghdr_t *control, nsapi_size_t control_size)
     {
         return return_value;
     }
diff --git a/connectivity/netsocket/tests/UNITTESTS/netsocket/NetworkStack/CMakeLists.txt b/connectivity/netsocket/tests/UNITTESTS/netsocket/NetworkStack/CMakeLists.txt
index dd5dd50653..83c805e500 100644
--- a/connectivity/netsocket/tests/UNITTESTS/netsocket/NetworkStack/CMakeLists.txt
+++ b/connectivity/netsocket/tests/UNITTESTS/netsocket/NetworkStack/CMakeLists.txt
@@ -22,6 +22,7 @@ target_sources(${TEST_NAME}
         ${mbed-os_SOURCE_DIR}/connectivity/libraries/nanostack-libservice/source/libip6string/stoip6.c
         ${mbed-os_SOURCE_DIR}/connectivity/libraries/nanostack-libservice/source/libBits/common_functions.c
         test_NetworkStack.cpp
+        test_MsgHeaderIterator.cpp
 )
 
 target_link_libraries(${TEST_NAME}
diff --git a/connectivity/netsocket/tests/UNITTESTS/netsocket/NetworkStack/test_MsgHeaderIterator.cpp b/connectivity/netsocket/tests/UNITTESTS/netsocket/NetworkStack/test_MsgHeaderIterator.cpp
new file mode 100644
index 0000000000..7ff3cede46
--- /dev/null
+++ b/connectivity/netsocket/tests/UNITTESTS/netsocket/NetworkStack/test_MsgHeaderIterator.cpp
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2021, Arm Limited and affiliates
+ * 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.
+ */
+
+#include "gtest/gtest.h"
+#include "netsocket/MsgHeader.h"
+#include "netsocket/nsapi_types.h"
+
+struct custom_type_t {
+    nsapi_msghdr_t hdr;
+    uint64_t custom_field1;
+    uint64_t custom_field2;
+    uint64_t custom_field3;
+    uint64_t custom_field4;
+};
+
+struct pktinfo_buffer_t {
+    pktinfo_buffer_t()
+    {
+        pkt_info_1.hdr.len = sizeof(nsapi_pktinfo_t);
+        pkt_info_2.hdr.len = sizeof(nsapi_pktinfo_t);
+    }
+    nsapi_pktinfo_t pkt_info_1;
+    nsapi_pktinfo_t pkt_info_2;
+};
+
+struct custom_buffer_t {
+    custom_buffer_t()
+    {
+        pkt_info.hdr.len = sizeof(nsapi_pktinfo_t);
+        msg_hdr.len = sizeof(nsapi_msghdr_t);
+        custom.hdr.len = sizeof(custom_type_t);
+    }
+    nsapi_pktinfo_t pkt_info;
+    nsapi_msghdr_t msg_hdr;
+    custom_type_t custom;
+};
+
+class TestMsgHeaderIterator : public testing::Test {
+protected:
+    pktinfo_buffer_t p_buff;
+    custom_buffer_t c_buff;
+
+    virtual void SetUp() {}
+
+    virtual void TearDown() {}
+};
+
+TEST_F(TestMsgHeaderIterator, pktinfo_list)
+{
+    nsapi_msghdr_t *hdr = reinterpret_cast<nsapi_msghdr_t *>(&p_buff);
+    MsgHeaderIterator it(hdr, sizeof(p_buff));
+
+    EXPECT_EQ(it.next(), &p_buff.pkt_info_1.hdr);
+    EXPECT_EQ(it.next(), &p_buff.pkt_info_2.hdr);
+    EXPECT_TRUE(it.next() == nullptr);
+}
+
+TEST_F(TestMsgHeaderIterator, custom_list)
+{
+    nsapi_msghdr_t *hdr_c = reinterpret_cast<nsapi_msghdr_t *>(&c_buff);
+    MsgHeaderIterator it(hdr_c, sizeof(c_buff));
+
+    EXPECT_EQ(it.next(), &c_buff.pkt_info.hdr);
+    EXPECT_EQ(it.next(), &c_buff.msg_hdr);
+    EXPECT_EQ(it.next(), &c_buff.custom.hdr);
+    EXPECT_TRUE(it.next() == nullptr);
+}
+
+TEST_F(TestMsgHeaderIterator, null_list)
+{
+    MsgHeaderIterator it(nullptr, sizeof(p_buff));
+
+    EXPECT_TRUE(it.next() == nullptr);
+}
+
+TEST_F(TestMsgHeaderIterator, wrong_list_size)
+{
+    nsapi_msghdr_t *hdr = reinterpret_cast<nsapi_msghdr_t *>(&p_buff);
+    MsgHeaderIterator it(hdr, 0);
+
+    EXPECT_TRUE(it.next() == nullptr);
+}
+
+TEST_F(TestMsgHeaderIterator, wrong_member_len)
+{
+    nsapi_msghdr_t *hdr = reinterpret_cast<nsapi_msghdr_t *>(&p_buff);
+    MsgHeaderIterator it(hdr, sizeof(p_buff));
+    p_buff.pkt_info_1.hdr.len = 0;
+
+    EXPECT_TRUE(it.next() == nullptr);
+}
\ No newline at end of file