Revision 9816:0561b171462a, committed 2017-01-03

Comitter:

Hasnain Virk

Date:

Tue Jan 03 18:05:42 2017 +0200

Parent:

9815:1d97cb0e78e4

Child:

9817:debcafb472c7

Commit message:

[ONME-2927] Socket adaptation layer for nanostack

As a result of major overhaul in Nanostack generally for socket API and especially
for TCP, the adaptation layer for mbed-OS is being upgraded.

Previously, as nanostack was not able to provide receive queue, adaptation layer had been
faking it. Now with Stream Socket by default Nanostack provides 2K receive queue and 2K send queue.
Receive queue size can be changed using setsockopt(). Batre metal nanostack would not provide with any
receive queues with Datagram Socket, however in this adaptation layer we introduce a 2K receive queue size
for the Datagram Socket as well.

Layer state machine handling is polished to ensure robustness.

::socket_connect() will can return 2 new error codes now. NSAPI_ERROR_ALREADY (like posix EALREADY) in case
if the connection is in progress or NSAPI_ERROR_IS_CONNECTED (like posix EISCONN) if already connected.
NSAPI_ERROR_WOULDBLOCK is now mapped directly to nanostack NS_WOULDBLOCK.

NanostackLockGaurd class is introduced which enables us to claim and release mutex using RAII style.

Changed in this revision

--- a/features/nanostack/FEATURE_NANOSTACK/nanostack-interface/NanostackInterface.cpp	Fri Feb 10 11:48:44 2017 -0600
+++ b/features/nanostack/FEATURE_NANOSTACK/nanostack-interface/NanostackInterface.cpp	Tue Jan 03 18:05:42 2017 +0200
@@ -17,11 +17,13 @@
 #include "mbed.h"
 #include "rtos.h"
 #include "NanostackInterface.h"
+#include "NanostackLockGuard.h"
 
 #include "ns_address.h"
 #include "nsdynmemLIB.h"
 #include "eventOS_scheduler.h"
 #include "randLIB.h"
+#include "ip6string.h"
 
 #include "mesh_system.h" // from inside mbed-mesh-api
 #include "socket_api.h"
@@ -33,28 +35,25 @@
 #define TRACE_GROUP "nsif"
 
 #define NS_INTERFACE_SOCKETS_MAX  16  //same as NanoStack SOCKET_MAX
-#define NANOSTACK_SOCKET_UDP 17 // same as nanostack SOCKET_UDP
-#define NANOSTACK_SOCKET_TCP 6  // same as nanostack SOCKET_TCP
 
 #define MALLOC  ns_dyn_mem_alloc
 #define FREE    ns_dyn_mem_free
 
+// Socket state progressions:
+// UDP: UNOPENED -> DATAGRAM
+// TCP client: UNOPENED -> OPENED -> CONNECTING -> STREAM -> CLOSED
+// TCP server: UNOPENED -> OPENED -> LISTENING
+// TCP accept: UNOPENED -> STREAM -> CLOSED
 enum socket_mode_t {
     SOCKET_MODE_UNOPENED,   // No socket ID
+    SOCKET_MODE_DATAGRAM,   // Socket is datagram type
     SOCKET_MODE_OPENED,     // Socket ID but no assigned use yet
     SOCKET_MODE_CONNECTING, // Socket is connecting but not open yet
-    SOCKET_MODE_DATAGRAM,   // Socket is bound to a port and listening for datagrams
     SOCKET_MODE_STREAM,     // Socket has an open stream
     SOCKET_MODE_CLOSED,     // Socket is closed and resources are freed
+    SOCKET_MODE_LISTENING,  // Socket is listening for connections
 };
 
-class NanostackBuffer {
-public:
-    NanostackBuffer *next;      /*<! next buffer */
-    ns_address_t ns_address;    /*<! address where data is received */
-    uint16_t length;            /*<! data length in this buffer */
-    uint8_t payload[1];          /*<! Trailing buffer data */
-};
 
 class NanostackSocket {
 public:
@@ -65,32 +64,29 @@
     NanostackSocket(int8_t protocol);
     ~NanostackSocket(void);
     bool open(void);
+    int accept(NanostackSocket *accepted_socket, ns_address_t *addr);
     void close(void);
     bool closed(void) {return SOCKET_MODE_CLOSED == mode;}
-    bool is_bound(void);
-    void set_bound(void);
     bool is_connecting(void);
     void set_connecting(ns_address_t *addr);
+    bool is_connected(void);
     void set_connected(void);
+    bool is_listening(void);
+    void set_listening(void);
 
     // Socket events from nanostack
     void event_data(socket_callback_t *sock_cb);
-    void event_bind_done(socket_callback_t *sock_cb);
-    void event_connnect_closed(socket_callback_t *sock_cb);
+    void event_connect_done(socket_callback_t *sock_cb);
+    void event_connect_fail(socket_callback_t *sock_cb);
+    void event_connect_closed(socket_callback_t *sock_cb);
+    void event_connection_reset(socket_callback_t *sock_cb);
     void event_tx_done(socket_callback_t *sock_cb);
+    void event_tx_fail(socket_callback_t *sock_cb);
+    void event_incoming_connection(socket_callback_t *sock_cb);
 
     // Run callback to signal the next layer of the NSAPI
     void signal_event(void);
 
-    // Add or remove a socket to the listening socket
-    void accept_list_add(NanostackSocket *socket);
-    NanostackSocket * accept_list_remove(void);
-
-    bool data_available(void);
-    size_t data_copy_and_free(void *dest, size_t len, SocketAddress *address, bool stream);
-    void data_free_all(void);
-    void data_attach(NanostackBuffer *data_buf);
-
     void (*callback)(void *);
     void *callback_data;
     int8_t socket_id;           /*!< allocated socket ID */
@@ -98,7 +94,7 @@
     bool addr_valid;
     ns_address_t ns_address;
 private:
-    NanostackBuffer *rxBufChain;    /*!< Receive buffers */
+    bool attach(int8_t socket_id);
     socket_mode_t mode;
 };
 
@@ -143,7 +139,6 @@
     callback = NULL;
     callback_data = NULL;
     socket_id = -1;
-    rxBufChain = NULL;
     proto = protocol;
     addr_valid = false;
     memset(&ns_address, 0, sizeof(ns_address));
@@ -157,7 +152,6 @@
     if (mode != SOCKET_MODE_CLOSED) {
         close();
     }
-    data_free_all();
 }
 
 bool NanostackSocket::open(void)
@@ -171,6 +165,38 @@
         tr_error("NanostackSocket::open() failed");
         return false;
     }
+
+    if (proto == SOCKET_TCP) {
+        /* Receive and send buffers enabled by default */
+        mode = SOCKET_MODE_OPENED;
+    } else {
+        static const int32_t rcvbuf_size = 2048;
+        socket_setsockopt(temp_socket, SOCKET_SOL_SOCKET, SOCKET_SO_RCVBUF, &rcvbuf_size, sizeof rcvbuf_size);
+        mode = SOCKET_MODE_DATAGRAM;
+    }
+
+    return attach(temp_socket);
+}
+
+int NanostackSocket::accept(NanostackSocket *accepted_socket, ns_address_t *addr)
+{
+    nanostack_assert_locked();
+    MBED_ASSERT(SOCKET_MODE_LISTENING == mode && SOCKET_MODE_UNOPENED == accepted_socket->mode);
+
+    int temp_socket = socket_accept(socket_id, addr, socket_callback);
+    if (temp_socket < 0) {
+        tr_error("NanostackSocket::accept() failed");
+        return temp_socket;
+    }
+    if (!accepted_socket->attach(temp_socket)) {
+        return -1;
+    }
+    accepted_socket->mode = SOCKET_MODE_STREAM;
+    return temp_socket;
+}
+
+bool NanostackSocket::attach(int8_t temp_socket)
+{
     if (temp_socket >= NS_INTERFACE_SOCKETS_MAX) {
         MBED_ASSERT(false);
         return false;
@@ -181,9 +207,7 @@
     }
     socket_id = temp_socket;
     socket_tbl[socket_id] = this;
-    mode = SOCKET_MODE_OPENED;
     return true;
-
 }
 
 void NanostackSocket::close()
@@ -201,26 +225,10 @@
         MBED_ASSERT(SOCKET_MODE_UNOPENED == mode);
     }
 
-    data_free_all();
-
     mode = SOCKET_MODE_CLOSED;
     signal_event();
 }
 
-bool NanostackSocket::is_bound()
-{
-    return SOCKET_MODE_DATAGRAM == mode;
-}
-
-void NanostackSocket::set_bound()
-{
-    nanostack_assert_locked();
-    MBED_ASSERT(SOCKET_MODE_OPENED == mode);
-    if (SOCKET_UDP == proto) {
-        mode = SOCKET_MODE_DATAGRAM;
-    }
-}
-
 bool NanostackSocket::is_connecting()
 {
     return SOCKET_MODE_CONNECTING == mode;
@@ -235,6 +243,11 @@
     mode = SOCKET_MODE_CONNECTING;
 }
 
+bool NanostackSocket::is_connected()
+{
+    return SOCKET_MODE_STREAM == mode;
+}
+
 void NanostackSocket::set_connected()
 {
     nanostack_assert_locked();
@@ -243,6 +256,19 @@
     mode = SOCKET_MODE_STREAM;
 }
 
+bool NanostackSocket::is_listening()
+{
+    return SOCKET_MODE_LISTENING == mode;
+}
+
+void NanostackSocket::set_listening()
+{
+    nanostack_assert_locked();
+    MBED_ASSERT(SOCKET_MODE_OPENED == mode);
+
+    mode = SOCKET_MODE_LISTENING;
+}
+
 void NanostackSocket::signal_event()
 {
     nanostack_assert_locked();
@@ -267,148 +293,60 @@
             tr_debug("SOCKET_DATA, sock=%d, bytes=%d", sock_cb->socket_id, sock_cb->d_len);
             socket->event_data(sock_cb);
             break;
-        case SOCKET_BIND_DONE:
-            tr_debug("SOCKET_BIND_DONE");
-            socket->event_bind_done(sock_cb);
+        case SOCKET_CONNECT_DONE:
+            tr_debug("SOCKET_CONNECT_DONE");
+            socket->event_connect_done(sock_cb);
             break;
-        case SOCKET_BIND_FAIL:
-            tr_debug("SOCKET_BIND_FAIL");
+        case SOCKET_CONNECT_FAIL:
+            tr_debug("SOCKET_CONNECT_FAIL");
+            socket->event_connect_fail(sock_cb);
             break;
-        case SOCKET_BIND_AUTH_FAIL:
-            tr_debug("SOCKET_BIND_AUTH_FAIL");
+        case SOCKET_CONNECT_AUTH_FAIL:
+            tr_debug("SOCKET_CONNECT_AUTH_FAIL");
             break;
         case SOCKET_INCOMING_CONNECTION:
             tr_debug("SOCKET_INCOMING_CONNECTION");
+            socket->event_incoming_connection(sock_cb);
             break;
         case SOCKET_TX_FAIL:
             tr_debug("SOCKET_TX_FAIL");
+            socket->event_tx_fail(sock_cb);
             break;
         case SOCKET_CONNECT_CLOSED:
             tr_debug("SOCKET_CONNECT_CLOSED");
-            socket->event_connnect_closed(sock_cb);
+            socket->event_connect_closed(sock_cb);
             break;
         case SOCKET_CONNECTION_RESET:
             tr_debug("SOCKET_CONNECTION_RESET");
+            socket->event_connection_reset(sock_cb);
             break;
         case SOCKET_NO_ROUTE:
             tr_debug("SOCKET_NO_ROUTE");
+            socket->event_tx_fail(sock_cb);
             break;
         case SOCKET_TX_DONE:
-            tr_debug("SOCKET_TX_DONE, %d bytes sent", sock_cb->d_len);
             socket->event_tx_done(sock_cb);
             break;
+        case SOCKET_NO_RAM:
+            tr_debug("SOCKET_NO_RAM");
+            socket->event_tx_fail(sock_cb);
+            break;
+        case SOCKET_CONNECTION_PROBLEM:
+            tr_debug("SOCKET_CONNECTION_PROBLEM");
+            break;
         default:
-            // SOCKET_NO_RAM, error case for SOCKET_TX_DONE
             break;
     }
 }
 
 
-bool NanostackSocket::data_available()
-{
-    nanostack_assert_locked();
-    MBED_ASSERT((SOCKET_MODE_DATAGRAM == mode) ||
-                (SOCKET_MODE_CONNECTING == mode) ||
-                (SOCKET_MODE_STREAM == mode));
-
-    return (NULL == rxBufChain) ? false : true;
-}
-
-size_t NanostackSocket::data_copy_and_free(void *dest, size_t len,
-                                                  SocketAddress *address, bool stream)
-{
-    nanostack_assert_locked();
-    MBED_ASSERT((SOCKET_MODE_DATAGRAM == mode) ||
-                (mode == SOCKET_MODE_STREAM));
-
-    NanostackBuffer *data_buf = rxBufChain;
-    if (NULL == data_buf) {
-        // No data
-        return 0;
-    }
-
-    if (address) {
-        convert_ns_addr_to_mbed(address, &data_buf->ns_address);
-    }
-
-    size_t copy_size = (len > data_buf->length) ? data_buf->length : len;
-    memcpy(dest, data_buf->payload, copy_size);
-
-    if (stream && (copy_size < data_buf->length)) {
-        // Update the size in the buffer
-        size_t new_buf_size = data_buf->length - copy_size;
-        memmove(data_buf->payload, data_buf->payload + copy_size, new_buf_size);
-        data_buf->length = new_buf_size;
-    } else {
-        // Entire packet used so free it
-        rxBufChain = data_buf->next;
-        FREE(data_buf);
-    }
-
-    return copy_size;
-}
-
-void NanostackSocket::data_free_all(void)
-{
-    nanostack_assert_locked();
-    // No mode requirement
-
-    NanostackBuffer *buffer = rxBufChain;
-    rxBufChain = NULL;
-    while (buffer != NULL) {
-        NanostackBuffer *next_buffer = buffer->next;
-        FREE(buffer);
-        buffer = next_buffer;
-    }
-}
-
-void NanostackSocket::data_attach(NanostackBuffer *data_buf)
-{
-    nanostack_assert_locked();
-    MBED_ASSERT((SOCKET_MODE_DATAGRAM == mode) ||
-                (SOCKET_MODE_STREAM == mode));
-
-    // Add to linked list
-    tr_debug("data_attach socket=%p", this);
-    if (NULL == rxBufChain) {
-        rxBufChain = data_buf;
-    } else {
-        NanostackBuffer *buf_tmp = rxBufChain;
-        while (NULL != buf_tmp->next) {
-            buf_tmp = buf_tmp->next;
-        }
-        buf_tmp->next = data_buf;
-    }
-    signal_event();
-}
-
 void NanostackSocket::event_data(socket_callback_t *sock_cb)
 {
     nanostack_assert_locked();
-    MBED_ASSERT((SOCKET_MODE_DATAGRAM == mode) ||
-                (SOCKET_MODE_STREAM == mode));
-
-    // Allocate buffer
-    NanostackBuffer *recv_buff = (NanostackBuffer *) MALLOC(
-                                 sizeof(NanostackBuffer) + sock_cb->d_len);
-    if (NULL == recv_buff) {
-        tr_error("alloc failed!");
-        return;
-    }
-    recv_buff->next = NULL;
+    MBED_ASSERT((SOCKET_MODE_STREAM == mode) ||
+                (SOCKET_MODE_DATAGRAM == mode));
 
-    // Write data to buffer
-    int16_t length = socket_read(sock_cb->socket_id,
-                                 &recv_buff->ns_address, recv_buff->payload,
-                                 sock_cb->d_len);
-    if (length < 0) {
-        tr_error("socket_read failed!");
-        FREE(recv_buff);
-        return;
-    }
-    recv_buff->length = length;
-
-    data_attach(recv_buff);
+    signal_event();
 }
 
 void NanostackSocket::event_tx_done(socket_callback_t *sock_cb)
@@ -417,10 +355,16 @@
     MBED_ASSERT((SOCKET_MODE_STREAM == mode) ||
                 (SOCKET_MODE_DATAGRAM == mode));
 
+    if (mode == SOCKET_MODE_DATAGRAM) {
+        tr_debug("SOCKET_TX_DONE, %d bytes sent", sock_cb->d_len);
+    } else if (mode == SOCKET_MODE_STREAM) {
+        tr_debug("SOCKET_TX_DONE, %d bytes remaining", sock_cb->d_len);
+    }
+
     signal_event();
 }
 
-void NanostackSocket::event_bind_done(socket_callback_t *sock_cb)
+void NanostackSocket::event_connect_done(socket_callback_t *sock_cb)
 {
     nanostack_assert_locked();
     MBED_ASSERT(SOCKET_MODE_CONNECTING == mode);
@@ -429,7 +373,49 @@
     signal_event();
 }
 
-void NanostackSocket::event_connnect_closed(socket_callback_t *sock_cb)
+void NanostackSocket::event_connect_fail(socket_callback_t *sock_cb)
+{
+    nanostack_assert_locked();
+    MBED_ASSERT(mode == SOCKET_MODE_CONNECTING);
+    close();
+}
+
+void NanostackSocket::event_incoming_connection(socket_callback_t *sock_cb)
+{
+    MBED_ASSERT(mode == SOCKET_MODE_LISTENING);
+    signal_event();
+}
+
+void NanostackSocket::event_connect_closed(socket_callback_t *sock_cb)
+{
+    nanostack_assert_locked();
+
+    // Can happen if we have an orderly close()
+    // Might never happen as we have not implemented shutdown() in abstraction layer.
+    MBED_ASSERT(mode == SOCKET_MODE_STREAM);
+    close();
+}
+
+void NanostackSocket::event_tx_fail(socket_callback_t *sock_cb)
+{
+    nanostack_assert_locked();
+
+    switch (mode) {
+        case SOCKET_MODE_CONNECTING:
+        case SOCKET_MODE_STREAM:
+            // TX_FAIL is fatal for stream sockets
+            close();
+            break;
+        case SOCKET_MODE_DATAGRAM:
+            // TX_FAIL is non-fatal for datagram sockets
+            break;
+        default:
+            MBED_ASSERT(false);
+            break;
+    }
+}
+
+void NanostackSocket::event_connection_reset(socket_callback_t *sock_cb)
 {
     nanostack_assert_locked();
 
@@ -439,9 +425,9 @@
     close();
 }
 
-NanostackInterface * NanostackInterface::_ns_interface;
+NanostackInterface *NanostackInterface::_ns_interface;
 
-NanostackInterface * NanostackInterface::get_stack()
+NanostackInterface *NanostackInterface::get_stack()
 {
     nanostack_lock();
 
@@ -454,13 +440,21 @@
     return _ns_interface;
 }
 
-
 const char * NanostackInterface::get_ip_address()
 {
+    NanostackLockGuard lock;
+
+    for (int if_id = 1; if_id <= 127; if_id++) {
+        uint8_t address[16];
+        int ret = arm_net_address_get(if_id, ADDR_IPV6_GP, address);
+        if (ret == 0) {
+            ip6tos(address, text_ip_address);
+            return text_ip_address;
+        }
+    }
     // Must result a valid IPv6 address
     // For gethostbyname() to detect IP version.
-    static const char localhost[] = "::";
-    return localhost;
+    return "::";
 }
 
 nsapi_error_t NanostackInterface::socket_open(void **handle, nsapi_protocol_t protocol)
@@ -481,10 +475,10 @@
     }
     *handle = (void*)NULL;
 
-    nanostack_lock();
+    NanostackLockGuard lock;
 
     NanostackSocket * socket = new NanostackSocket(ns_proto);
-    if (NULL == socket) {
+    if (socket == NULL) {
         nanostack_unlock();
         tr_debug("socket_open() ret=%i", NSAPI_ERROR_NO_MEMORY);
         return NSAPI_ERROR_NO_MEMORY;
@@ -497,11 +491,9 @@
     }
     *handle = (void*)socket;
 
-    nanostack_unlock();
-
     tr_debug("socket_open() socket=%p, sock_id=%d, ret=0", socket, socket->socket_id);
 
-    return 0;
+    return NSAPI_ERROR_OK;
 }
 
 nsapi_error_t NanostackInterface::socket_close(void *handle)
@@ -524,95 +516,132 @@
 
 }
 
-nsapi_size_or_error_t NanostackInterface::socket_sendto(void *handle, const SocketAddress &address, const void *data, nsapi_size_t size)
+nsapi_size_or_error_t NanostackInterface::do_sendto(void *handle, const ns_address_t *address, const void *data, nsapi_size_t size)
 {
     // Validate parameters
     NanostackSocket * socket = static_cast<NanostackSocket *>(handle);
-    if (NULL == handle) {
+    if (handle == NULL) {
         MBED_ASSERT(false);
         return NSAPI_ERROR_NO_SOCKET;
     }
 
-    if (address.get_ip_version() != NSAPI_IPv6) {
-        return NSAPI_ERROR_UNSUPPORTED;
+    nsapi_size_or_error_t ret;
+
+    NanostackLockGuard lock;
+
+    if (socket->closed() || (!address && !socket->is_connected())) {
+        ret = NSAPI_ERROR_NO_CONNECTION;
+        goto out;
+    }
+
+    if (address && socket->proto == SOCKET_TCP) {
+        tr_error("socket_sendto() not supported with TCP!");
+        ret = NSAPI_ERROR_IS_CONNECTED;
+        goto out;
     }
 
-    nanostack_lock();
+    int retcode;
+#if 0
+    retcode = ::socket_sendto(socket->socket_id, address,
+                                            data, size);
+#else
+    // Use sendmsg purely to get the new return style
+    // of returning data written rather than 0 on success,
+    // which means TCP can do partial writes. (Sadly,
+    // it's the only call which takes flags so we can
+    // leave the NS_MSG_LEGACY0 flag clear).
+    ns_msghdr_t msg;
+    ns_iovec_t iov;
+    iov.iov_base = const_cast<void *>(data);
+    iov.iov_len = size;
+    msg.msg_name = const_cast<ns_address_t *>(address);
+    msg.msg_namelen = address ? sizeof *address : 0;
+    msg.msg_iov = &iov;
+    msg.msg_iovlen = 1;
+    msg.msg_control = NULL;
+    msg.msg_controllen = 0;
+    retcode = ::socket_sendmsg(socket->socket_id, &msg, 0);
+#endif
 
-    nsapi_size_or_error_t ret;
-    if (socket->closed()) {
-        ret = NSAPI_ERROR_NO_CONNECTION;
-    } else if (NANOSTACK_SOCKET_TCP == socket->proto) {
-        tr_error("socket_sendto() not supported with SOCKET_STREAM!");
-        ret = NSAPI_ERROR_UNSUPPORTED;
+    /*
+     * \return length if entire amount written (which could be 0)
+     * \return value >0 and <length if partial amount written (stream only)
+     * \return NS_EWOULDBLOCK if nothing written due to lack of queue space.
+     * \return -1 Invalid socket ID or message structure.
+     * \return -2 Socket memory allocation fail.
+     * \return -3 TCP state not established or address scope not defined .
+     * \return -4 Socket TX process busy or unknown interface.
+     * \return -5 Socket not connected
+     * \return -6 Packet too short (ICMP raw socket error).
+     * */
+    if (retcode == -2 || retcode == NS_EWOULDBLOCK) {
+        ret = NSAPI_ERROR_WOULD_BLOCK;
+    } else if (retcode < 0) {
+        tr_error("socket_sendmsg: error=%d", retcode);
+        ret = NSAPI_ERROR_DEVICE_ERROR;
     } else {
-        ns_address_t ns_address;
-        convert_mbed_addr_to_ns(&ns_address, &address);
-        if (!socket->is_bound()) {
-            socket->set_bound();
-        }
-        int8_t send_to_status = ::socket_sendto(socket->socket_id, &ns_address,
-                                       (uint8_t *)data, size);
-        /*
-         * \return 0 on success.
-         * \return -1 invalid socket id.
-         * \return -2 Socket memory allocation fail.
-         * \return -3 TCP state not established.
-         * \return -4 Socket tx process busy.
-         * \return -5 TLS authentication not ready.
-         * \return -6 Packet too short.
-         * */
-        if (-4 == send_to_status) {
-            ret = NSAPI_ERROR_WOULD_BLOCK;
-        } else if (0 != send_to_status) {
-            tr_error("socket_sendto: error=%d", send_to_status);
-            ret = NSAPI_ERROR_DEVICE_ERROR;
-        } else {
-            ret = size;
-        }
+        ret = retcode;
     }
 
-    nanostack_unlock();
-
+out:
     tr_debug("socket_sendto(socket=%p) sock_id=%d, ret=%i", socket, socket->socket_id, ret);
 
     return ret;
 }
 
+nsapi_size_or_error_t NanostackInterface::socket_sendto(void *handle, const SocketAddress &address, const void *data, nsapi_size_t size)
+{
+    if (address.get_ip_version() != NSAPI_IPv6) {
+        return NSAPI_ERROR_UNSUPPORTED;
+    }
+
+    ns_address_t ns_address;
+    convert_mbed_addr_to_ns(&ns_address, &address);
+    /*No lock gaurd needed here as do_sendto() will handle locks.*/
+    return do_sendto(handle, &ns_address, data, size);
+}
+
 nsapi_size_or_error_t NanostackInterface::socket_recvfrom(void *handle, SocketAddress *address, void *buffer, nsapi_size_t size)
 {
     // Validate parameters
-    NanostackSocket * socket = static_cast<NanostackSocket *>(handle);
-    if (NULL == handle) {
+    NanostackSocket *socket = static_cast<NanostackSocket *>(handle);
+    if (handle == NULL) {
         MBED_ASSERT(false);
         return NSAPI_ERROR_NO_SOCKET;
     }
-    if (NULL == buffer) {
-        MBED_ASSERT(false);
-        return NSAPI_ERROR_PARAMETER;
-    }
-    if (0 == size) {
-        MBED_ASSERT(false);
-        return NSAPI_ERROR_PARAMETER;
-    }
-
-    nanostack_lock();
 
     nsapi_size_or_error_t ret;
+
+    NanostackLockGuard lock;
+
     if (socket->closed()) {
         ret = NSAPI_ERROR_NO_CONNECTION;
-    } else if (NANOSTACK_SOCKET_TCP == socket->proto) {
-        tr_error("recv_from() not supported with SOCKET_STREAM!");
-        ret = NSAPI_ERROR_UNSUPPORTED;
-    } else if (!socket->data_available()) {
-        ret = NSAPI_ERROR_WOULD_BLOCK;
-    } else {
-        ret = socket->data_copy_and_free(buffer, size, address, false);
+        goto out;
     }
 
-    nanostack_unlock();
+    ns_address_t ns_address;
+
+    int retcode;
+    retcode = ::socket_recvfrom(socket->socket_id, buffer, size, 0, &ns_address);
 
-    tr_debug("socket_recvfrom(socket=%p) sock_id=%d, ret=%i", socket, socket->socket_id, ret);
+    if (retcode == NS_EWOULDBLOCK) {
+        ret = NSAPI_ERROR_WOULD_BLOCK;
+    } else if (retcode < 0) {
+        ret = NSAPI_ERROR_PARAMETER;
+    } else {
+        ret = retcode;
+        if (address != NULL) {
+            convert_ns_addr_to_mbed(address, &ns_address);
+        }
+    }
+
+out:
+    if (address) {
+        tr_debug("socket_recvfrom(socket=%p) sock_id=%d, ret=%i, addr=[%s]:%i", socket, socket->socket_id, ret,
+                 trace_ipv6(address->get_ip_bytes()), address->get_port());
+    } else {
+        tr_debug("socket_recv(socket=%p) sock_id=%d, ret=%i", socket, socket->socket_id, ret);
+    }
 
     return ret;
 }
@@ -620,8 +649,8 @@
 nsapi_error_t NanostackInterface::socket_bind(void *handle, const SocketAddress &address)
 {
     // Validate parameters
-    NanostackSocket * socket = static_cast<NanostackSocket *>(handle);
-    if (NULL == handle) {
+    NanostackSocket *socket = static_cast<NanostackSocket *>(handle);
+    if (handle == NULL) {
         MBED_ASSERT(false);
         return NSAPI_ERROR_NO_SOCKET;
     }
@@ -638,68 +667,139 @@
             return NSAPI_ERROR_UNSUPPORTED;
     }
 
-    nanostack_lock();
+    NanostackLockGuard lock;
 
     ns_address_t ns_address;
     ns_address.type = ADDRESS_IPV6;
     memcpy(ns_address.address, addr_field, sizeof ns_address.address);
     ns_address.identifier = address.get_port();
-    nsapi_error_t ret = NSAPI_ERROR_DEVICE_ERROR;
-    if (0 == ::socket_bind(socket->socket_id, &ns_address)) {
-        socket->set_bound();
-        ret = 0;
+    nsapi_error_t ret;
+    int retcode = ::socket_bind(socket->socket_id, &ns_address);
+
+    if (retcode == 0) {
+        ret = NSAPI_ERROR_OK;
+    } else {
+        ret = NSAPI_ERROR_PARAMETER;
     }
 
-    nanostack_unlock();
-
-    tr_debug("socket_bind(socket=%p) sock_id=%d, ret=%i", socket, socket->socket_id, ret);
+    tr_debug("socket_bind(socket=%p) sock_id=%d, retcode=%i, ret=%i", socket, socket->socket_id, retcode, ret);
 
     return ret;
 }
 
 nsapi_error_t NanostackInterface::setsockopt(void *handle, int level, int optname, const void *optval, unsigned optlen)
 {
-    return NSAPI_ERROR_UNSUPPORTED;
+    NanostackSocket *socket = static_cast<NanostackSocket *>(handle);
+    if (handle == NULL) {
+        MBED_ASSERT(false);
+        return NSAPI_ERROR_NO_SOCKET;
+    }
+
+    nsapi_error_t ret;
+
+    NanostackLockGuard lock;
+
+    if (::socket_setsockopt(socket->socket_id, level, optname, optval, optlen) == 0) {
+        ret = NSAPI_ERROR_OK;
+    } else {
+        ret = NSAPI_ERROR_PARAMETER;
+    }
+
+    return ret;
 }
 
 nsapi_error_t NanostackInterface::getsockopt(void *handle, int level, int optname, void *optval, unsigned *optlen)
 {
-    return NSAPI_ERROR_UNSUPPORTED;
+    NanostackSocket *socket = static_cast<NanostackSocket *>(handle);
+    if (handle == NULL) {
+        MBED_ASSERT(false);
+        return NSAPI_ERROR_NO_SOCKET;
+    }
+
+    nsapi_error_t ret;
+
+    NanostackLockGuard lock;
+
+    uint16_t optlen16 = *optlen;
+    if (::socket_getsockopt(socket->socket_id, level, optname, optval, &optlen16) == 0) {
+        ret = NSAPI_ERROR_OK;
+        *optlen = optlen16;
+    } else {
+        ret = NSAPI_ERROR_PARAMETER;
+    }
+
+    return ret;
 }
 
 nsapi_error_t NanostackInterface::socket_listen(void *handle, int backlog)
 {
-    return NSAPI_ERROR_UNSUPPORTED;
+    //Check if socket exists
+    NanostackSocket *socket = static_cast<NanostackSocket *>(handle);
+    if (handle == NULL) {
+        MBED_ASSERT(false);
+        return NSAPI_ERROR_NO_SOCKET;
+    }
+
+    nsapi_error_t ret = NSAPI_ERROR_OK;
+
+    NanostackLockGuard lock;
+
+    if(::socket_listen(socket->socket_id, backlog) < 0) {
+        ret = NSAPI_ERROR_PARAMETER;
+    } else {
+        socket->set_listening();
+    }
+
+    return ret;
 }
 
 nsapi_error_t NanostackInterface::socket_connect(void *handle, const SocketAddress &addr)
 {
     // Validate parameters
-    NanostackSocket * socket = static_cast<NanostackSocket *>(handle);
-    if (NULL == handle) {
+    NanostackSocket *socket = static_cast<NanostackSocket *>(handle);
+    nsapi_error_t ret;
+    if (handle == NULL) {
         MBED_ASSERT(false);
         return NSAPI_ERROR_NO_SOCKET;
     }
 
+    NanostackLockGuard lock;
+
     if (addr.get_ip_version() != NSAPI_IPv6) {
-        return NSAPI_ERROR_UNSUPPORTED;
+        ret = NSAPI_ERROR_UNSUPPORTED;
+        goto out;
+    }
+
+    if (socket->closed()) {
+        ret = NSAPI_ERROR_NO_CONNECTION;
+        goto out;
     }
 
-    nanostack_lock();
+    if (socket->is_connecting()) {
+        ret = NSAPI_ERROR_ALREADY;
+        goto out;
+    }
 
-    nsapi_error_t ret;
+    if (socket->is_connected()) {
+        ret = NSAPI_ERROR_IS_CONNECTED;
+        goto out;
+    }
+
     ns_address_t ns_addr;
-    int random_port = socket->is_bound() ? 0 : 1;
+
     convert_mbed_addr_to_ns(&ns_addr, &addr);
-    if (0 == ::socket_connect(socket->socket_id, &ns_addr, random_port)) {
-        socket->set_connecting(&ns_addr);
-        ret = 0;
+    if (::socket_connect(socket->socket_id, &ns_addr, 0) == 0) {
+        if (socket->proto == SOCKET_TCP) {
+            socket->set_connecting(&ns_addr);
+            ret = NSAPI_ERROR_IN_PROGRESS;
+        } else {
+            ret = NSAPI_ERROR_OK;
+        }
     } else {
         ret = NSAPI_ERROR_DEVICE_ERROR;
     }
 
-    nanostack_unlock();
-
+out:
     tr_debug("socket_connect(socket=%p) sock_id=%d, ret=%i", socket, socket->socket_id, ret);
 
     return ret;
@@ -707,95 +807,77 @@
 
 nsapi_error_t NanostackInterface::socket_accept(void *server, void **handle, SocketAddress *address)
 {
-    return NSAPI_ERROR_UNSUPPORTED;
-}
+    NanostackSocket * socket = static_cast<NanostackSocket *>(server);
+    NanostackSocket *accepted_sock = NULL;
+    nsapi_error_t ret;
 
-nsapi_size_or_error_t NanostackInterface::socket_send(void *handle, const void *p, nsapi_size_t size)
-{
-    // Validate parameters
-    NanostackSocket * socket = static_cast<NanostackSocket *>(handle);
-    if (NULL == handle) {
+    if (handle == NULL) {
         MBED_ASSERT(false);
         return NSAPI_ERROR_NO_SOCKET;
     }
 
-    nanostack_lock();
+    NanostackLockGuard lock;
 
-    nsapi_size_or_error_t ret;
-    if (socket->closed()) {
-        ret = NSAPI_ERROR_NO_CONNECTION;
-    } else if (socket->is_connecting()) {
-        ret = NSAPI_ERROR_WOULD_BLOCK;
-    } else {
-        ret = ::socket_sendto(socket->socket_id,  &socket->ns_address, (uint8_t*)p, size);
-        /*
-         * \return 0 on success.
-         * \return -1 invalid socket id.
-         * \return -2 Socket memory allocation fail.
-         * \return -3 TCP state not established.
-         * \return -4 Socket tx process busy.
-         * \return -5 TLS authentication not ready.
-         * \return -6 Packet too short.
-         * */
-        if (-4 == ret) {
-            ret = NSAPI_ERROR_WOULD_BLOCK;
-        } else if (ret != 0) {
-            tr_warning("socket_sendto ret %i, socket_id %i", ret, socket->socket_id);
-            ret = NSAPI_ERROR_DEVICE_ERROR;
-        } else {
-            ret = size;
-        }
+    if (!socket->is_listening()) {
+        ret = NSAPI_ERROR_PARAMETER;
+        goto out;
+    }
+
+    accepted_sock = new NanostackSocket(socket->proto);
+    if (accepted_sock == NULL) {
+        ret = NSAPI_ERROR_NO_MEMORY;
+        goto out;
     }
 
-    nanostack_unlock();
+    ns_address_t ns_addr;
+    int retcode;
+    retcode = socket->accept(accepted_sock, &ns_addr);
+    if (retcode < 0) {
+        delete accepted_sock;
+        if (retcode == NS_EWOULDBLOCK) {
+            ret = NSAPI_ERROR_WOULD_BLOCK;
+        } else {
+            ret = NSAPI_ERROR_DEVICE_ERROR;
+        }
+        goto out;
+    }
+    ret = NSAPI_ERROR_OK;
 
-    tr_debug("socket_send(socket=%p) sock_id=%d, ret=%i", socket, socket->socket_id, ret);
+    if (address) {
+        convert_ns_addr_to_mbed(address, &ns_addr);
+    }
+
+    *handle = accepted_sock;
+
+out:
+    tr_debug("socket_accept() socket=%p, sock_id=%d, ret=%i", accepted_sock, accepted_sock ? accepted_sock->socket_id : -1, ret);
 
     return ret;
 }
 
+nsapi_size_or_error_t NanostackInterface::socket_send(void *handle, const void *data, nsapi_size_t size)
+{
+    return do_sendto(handle, NULL, data, size);
+}
+
 nsapi_size_or_error_t NanostackInterface::socket_recv(void *handle, void *data, nsapi_size_t size)
 {
-    // Validate parameters
-    NanostackSocket * socket = static_cast<NanostackSocket *>(handle);
-    if (NULL == handle) {
-        MBED_ASSERT(false);
-        return NSAPI_ERROR_NO_SOCKET;
-    }
-
-    nanostack_lock();
-
-    nsapi_size_or_error_t ret;
-    if (socket->closed()) {
-        ret = NSAPI_ERROR_NO_CONNECTION;
-    } else if (socket->data_available()) {
-        ret = socket->data_copy_and_free(data, size, NULL, true);
-    } else {
-        ret = NSAPI_ERROR_WOULD_BLOCK;
-    }
-
-    nanostack_unlock();
-
-    tr_debug("socket_recv(socket=%p) sock_id=%d, ret=%i", socket, socket->socket_id, ret);
-
-    return ret;
+    return socket_recvfrom(handle, NULL, data, size);
 }
 
 void NanostackInterface::socket_attach(void *handle, void (*callback)(void *), void *id)
 {
     // Validate parameters
     NanostackSocket * socket = static_cast<NanostackSocket *>(handle);
-    if (NULL == handle) {
+    if (handle == NULL) {
         MBED_ASSERT(false);
         return;
     }
 
-    nanostack_lock();
+    NanostackLockGuard lock;
 
     socket->callback = callback;
     socket->callback_data = id;
 
-    nanostack_unlock();
-
     tr_debug("socket_attach(socket=%p) sock_id=%d", socket, socket->socket_id);
 }

--- a/features/nanostack/FEATURE_NANOSTACK/nanostack-interface/NanostackInterface.h	Fri Feb 10 11:48:44 2017 -0600
+++ b/features/nanostack/FEATURE_NANOSTACK/nanostack-interface/NanostackInterface.h	Tue Jan 03 18:05:42 2017 +0200
@@ -26,6 +26,8 @@
 #include "NanostackEthernetInterface.h"
 #include "MeshInterfaceNanostack.h"
 
+struct ns_address;
+
 class NanostackInterface : public NetworkStack {
 public:
     static NanostackInterface *get_stack();
@@ -229,6 +231,8 @@
     virtual nsapi_error_t getsockopt(void *handle, int level, int optname, void *optval, unsigned *optlen);
 
 private:
+    nsapi_size_or_error_t do_sendto(void *handle, const struct ns_address *address, const void *data, nsapi_size_t size);
+    char text_ip_address[40];
     static NanostackInterface * _ns_interface;
 };
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/features/nanostack/FEATURE_NANOSTACK/nanostack-interface/NanostackLockGuard.h	Tue Jan 03 18:05:42 2017 +0200
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2017 ARM Limited. All rights reserved.
+ * 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.
+ */
+
+#ifndef NANOSTACK_LOCK_GUARD_H_
+#define NANOSTACK_LOCK_GUARD_H_
+
+#include "eventOS_scheduler.h"
+
+/**
+ * RAII style Nanostack mutex acquisition.
+ * Mutex is held until object leaves scope.
+ */
+
+class NanostackLockGuard {
+public:
+    NanostackLockGuard() {
+        eventOS_scheduler_mutex_wait();
+    }
+
+    ~NanostackLockGuard() {
+        eventOS_scheduler_mutex_release();
+    }
+
+private:
+    NanostackLockGuard(const NanostackLockGuard&);
+    NanostackLockGuard& operator=(const NanostackLockGuard&);
+};
+
+#endif /* NANOSTACK_LOCK_GUARD_H_ */