NSAPI: Add Nanostack multicast membership support

Add support for recently introduced NSAPI generic socket options to
control group membership. Previously applications using Nanostack would
have had to use Nanostack's native socket options.

Author: kjbracey

features/nanostack/FEATURE_NANOSTACK/nanostack-interface/NanostackInterface.cpp

@@ -128,6 +128,20 @@ static void convert_ns_addr_to_mbed(SocketAddress *s_addr, const ns_address_t *n
     s_addr->set_ip_bytes(ns_addr->address, NSAPI_IPv6);
 }
 
+static int8_t find_interface_by_address(const uint8_t target_addr[16])
+{
+    for (int if_id = 1; if_id <= 127; if_id++) {
+        int i = 0;
+        uint8_t if_addr[16];
+        while (arm_net_address_list_get_next(if_id, &i, if_addr) == 0) {
+            if (memcmp(target_addr, if_addr, 16) == 0) {
+                return if_id;
+            }
+        }
+    }
+    return -1;
+}
+
 void* NanostackSocket::operator new(std::size_t sz) {
     return MALLOC(sz);
 }
@@ -693,17 +707,50 @@ nsapi_error_t NanostackInterface::setsockopt(void *handle, int level, int optnam
         return NSAPI_ERROR_NO_SOCKET;
     }
 
-    nsapi_error_t ret;
-
     NanostackLockGuard lock;
 
+    ns_ipv6_mreq_t ns_mreq;
+
+    if (level == NSAPI_SOCKET) {
+        switch (optname) {
+            case NSAPI_ADD_MEMBERSHIP:
+            case NSAPI_DROP_MEMBERSHIP: {
+                if (optlen != sizeof(nsapi_ip_mreq_t)) {
+                    return NSAPI_ERROR_PARAMETER;
+                }
+                const nsapi_ip_mreq_t *imr = static_cast<const nsapi_ip_mreq_t *>(optval);
+
+                /* Check address types are IPv6, or unspecified for interface */
+                if (imr->imr_multiaddr.version != NSAPI_IPv6 ||
+                    (imr->imr_interface.version != NSAPI_UNSPEC && imr->imr_interface.version != NSAPI_IPv6)) {
+                    return NSAPI_ERROR_PARAMETER;
+                }
+
+                /* Convert all parameters to Nanostack native, and proceed with setsockopt */
+                memcpy(ns_mreq.ipv6mr_multiaddr, imr->imr_multiaddr.bytes, 16);
+                if (imr->imr_interface.version == NSAPI_UNSPEC || memcmp(imr->imr_interface.bytes, ns_in6addr_any, 16) == 0) {
+                    ns_mreq.ipv6mr_interface = 0;
+                } else {
+                    // If this fails, Nanostack will itself fault the invalid -1 interface ID
+                    ns_mreq.ipv6mr_interface = find_interface_by_address(imr->imr_interface.bytes);
+                }
+
+                level = SOCKET_IPPROTO_IPV6;
+                optname = optname == NSAPI_ADD_MEMBERSHIP ? SOCKET_IPV6_JOIN_GROUP : SOCKET_IPV6_LEAVE_GROUP;
+                optval = &ns_mreq;
+                optlen = sizeof ns_mreq;
+                break;
+            }
+            default:
+                return NSAPI_ERROR_PARAMETER;
+        }
+    }
+
     if (::socket_setsockopt(socket->socket_id, level, optname, optval, optlen) == 0) {
-        ret = NSAPI_ERROR_OK;
+        return NSAPI_ERROR_OK;
     } else {
-        ret = NSAPI_ERROR_PARAMETER;
+        return NSAPI_ERROR_PARAMETER;
     }
-
-    return ret;
 }
 
 nsapi_error_t NanostackInterface::getsockopt(void *handle, int level, int optname, void *optval, unsigned *optlen)