Add IPv4 and IPv6 multicast implementation for UDPSocket

features/FEATURE_LWIP/lwip-interface/lwip_stack.c

@@ -32,8 +32,10 @@
 #include "lwip/tcp.h"
 #include "lwip/ip.h"
 #include "lwip/mld6.h"
+#include "lwip/igmp.h"
 #include "lwip/dns.h"
 #include "lwip/udp.h"
+#include "lwip_errno.h"
 #include "netif/lwip_ethernet.h"
 #include "emac_api.h"
 #include "ppp_lwip.h"
@@ -46,6 +48,10 @@ static nsapi_error_t mbed_lwip_err_remap(err_t err);
     #define MBED_NETIF_INIT_FN eth_arch_enetif_init
 #endif
 
+#ifndef LWIP_SOCKET_MAX_MEMBERSHIPS
+    #define LWIP_SOCKET_MAX_MEMBERSHIPS 4
+#endif
+
 /* Static arena of sockets */
 static struct lwip_socket {
     bool in_use;
@@ -56,13 +62,39 @@ static struct lwip_socket {
 
     void (*cb)(void *);
     void *data;
+
+    // Track multicast addresses subscribed to by this socket
+    nsapi_ip_mreq_t *multicast_memberships;
+    uint32_t         multicast_memberships_count;
+    uint32_t         multicast_memberships_registry;
+
 } lwip_arena[MEMP_NUM_NETCONN];
 
 static bool lwip_inited = false;
 static bool lwip_connected = false;
 static bool netif_inited = false;
 static bool netif_is_ppp = false;
 
+static nsapi_error_t mbed_lwip_setsockopt(nsapi_stack_t *stack, nsapi_socket_t handle, int level, int optname, const void *optval, unsigned optlen);
+
+static inline uint32_t next_registered_multicast_member(const struct lwip_socket *s, uint32_t index) {
+    while (!(s->multicast_memberships_registry & (0x0001 << index))) { index++; }
+    return index;
+}
+
+static inline uint32_t next_free_multicast_member(const struct lwip_socket *s, uint32_t index) {
+    while ((s->multicast_memberships_registry & (0x0001 << index))) { index++; }
+    return index;
+}
+
+static inline void set_multicast_member_registry_bit(struct lwip_socket *s, uint32_t index) {
+    s->multicast_memberships_registry |= (0x0001 << index);
+}
+
+static inline void clear_multicast_member_registry_bit(struct lwip_socket *s, uint32_t index) {
+    s->multicast_memberships_registry &= ~(0x0001 << index);
+}
+
 static struct lwip_socket *mbed_lwip_arena_alloc(void)
 {
     sys_prot_t prot = sys_arch_protect();
@@ -84,6 +116,18 @@ static struct lwip_socket *mbed_lwip_arena_alloc(void)
 static void mbed_lwip_arena_dealloc(struct lwip_socket *s)
 {
     s->in_use = false;
+
+    while (s->multicast_memberships_count > 0) {
+        uint32_t index = 0;
+        index = next_registered_multicast_member(s, index);
+
+        mbed_lwip_setsockopt(NULL, s, NSAPI_SOCKET, NSAPI_DROP_MEMBERSHIP, &s->multicast_memberships[index],
+            sizeof(s->multicast_memberships[index]));
+        index++;
+    }
+
+    free(s->multicast_memberships);
+    s->multicast_memberships = NULL;
 }
 
 static void mbed_lwip_socket_callback(struct netconn *nc, enum netconn_evt eh, u16_t len)
@@ -1081,6 +1125,24 @@ static nsapi_size_or_error_t mbed_lwip_socket_recvfrom(nsapi_stack_t *stack, nsa
     return recv;
 }
 
+static int32_t find_multicast_member(const struct lwip_socket *s, const nsapi_ip_mreq_t *imr) {
+    uint32_t count = 0;
+    uint32_t index = 0;
+    // Set upper limit on while loop, should break out when the membership pair is found
+    while (count < s->multicast_memberships_count) {
+        index = next_registered_multicast_member(s, index);
+
+        if (memcmp(&s->multicast_memberships[index].imr_multiaddr, &imr->imr_multiaddr, sizeof(nsapi_addr_t)) == 0 &&
+           memcmp(&s->multicast_memberships[index].imr_interface, &imr->imr_interface, sizeof(nsapi_addr_t)) == 0) {
+            return index;
+        }
+        count++;
+        index++;
+    }
+
+    return -1;
+}
+
 static nsapi_error_t mbed_lwip_setsockopt(nsapi_stack_t *stack, nsapi_socket_t handle, int level, int optname, const void *optval, unsigned optlen)
 {
     struct lwip_socket *s = (struct lwip_socket *)handle;
@@ -1124,6 +1186,103 @@ static nsapi_error_t mbed_lwip_setsockopt(nsapi_stack_t *stack, nsapi_socket_t h
             }
             return 0;
 
+        case NSAPI_ADD_MEMBERSHIP:
+        case NSAPI_DROP_MEMBERSHIP: {
+            if (optlen != sizeof(nsapi_ip_mreq_t)) {
+                return NSAPI_ERROR_PARAMETER;
+            }
+            err_t igmp_err;
+            const nsapi_ip_mreq_t *imr = optval;
+
+            /* Check interface address type matches group, or is unspecified */
+            if (imr->imr_interface.version != NSAPI_UNSPEC && imr->imr_interface.version != imr->imr_multiaddr.version) {
+                return NSAPI_ERROR_PARAMETER;
+            }
+
+            ip_addr_t if_addr;
+            ip_addr_t multi_addr;
+
+            /* Convert the group address */
+            if (!convert_mbed_addr_to_lwip(&multi_addr, &imr->imr_multiaddr)) {
+                return NSAPI_ERROR_PARAMETER;
+            }
+
+            /* Convert the interface address, or make sure it's the correct sort of "any" */
+            if (imr->imr_interface.version != NSAPI_UNSPEC) {
+                if (!convert_mbed_addr_to_lwip(&if_addr, &imr->imr_interface)) {
+                    return NSAPI_ERROR_PARAMETER;
+                }
+            } else {
+                ip_addr_set_any(IP_IS_V6(&if_addr), &if_addr);
+            }
+
+            igmp_err = ERR_USE; // Maps to NSAPI_ERROR_UNSUPPORTED
+            int32_t member_pair_index = find_multicast_member(s, imr);
+
+            if (optname == NSAPI_ADD_MEMBERSHIP) {
+                if (!s->multicast_memberships) {
+                    // First multicast join on this socket, allocate space for membership tracking
+                    s->multicast_memberships = malloc(sizeof(nsapi_ip_mreq_t) * LWIP_SOCKET_MAX_MEMBERSHIPS);
+                    if (!s->multicast_memberships) {
+                        return NSAPI_ERROR_NO_MEMORY;
+                    }
+                } else if(s->multicast_memberships_count == LWIP_SOCKET_MAX_MEMBERSHIPS) {
+                    return NSAPI_ERROR_NO_MEMORY;
+                }
+
+                if (member_pair_index != -1) {
+                    return NSAPI_ERROR_ADDRESS_IN_USE;
+                }
+
+                member_pair_index = next_free_multicast_member(s, 0);
+
+                sys_prot_t prot = sys_arch_protect();
+
+                #if LWIP_IPV4
+                if (IP_IS_V4(&if_addr)) {
+                    igmp_err = igmp_joingroup(ip_2_ip4(&if_addr), ip_2_ip4(&multi_addr));
+                }
+                #endif
+                #if LWIP_IPV6
+                if (IP_IS_V6(&if_addr)) {
+                    igmp_err = mld6_joingroup(ip_2_ip6(&if_addr), ip_2_ip6(&multi_addr));
+                }
+                #endif
+
+                sys_arch_unprotect(prot);
+
+                if (igmp_err == ERR_OK) {
+                    set_multicast_member_registry_bit(s, member_pair_index);
+                    s->multicast_memberships[member_pair_index] = *imr;
+                    s->multicast_memberships_count++;
+                }
+            } else {
+                if (member_pair_index == -1) {
+                    return NSAPI_ERROR_NO_ADDRESS;
+                }
+
+                clear_multicast_member_registry_bit(s, member_pair_index);
+                s->multicast_memberships_count--;
+
+                sys_prot_t prot = sys_arch_protect();
+
+                #if LWIP_IPV4
+                if (IP_IS_V4(&if_addr)) {
+                    igmp_err = igmp_leavegroup(ip_2_ip4(&if_addr), ip_2_ip4(&multi_addr));
+                }
+                #endif
+                #if LWIP_IPV6
+                if (IP_IS_V6(&if_addr)) {
+                    igmp_err = mld6_leavegroup(ip_2_ip6(&if_addr), ip_2_ip6(&multi_addr));
+                }
+                #endif
+
+                sys_arch_unprotect(prot);
+            }
+
+            return mbed_lwip_err_remap(igmp_err);
+         }
+
         default:
             return NSAPI_ERROR_UNSUPPORTED;
     }

features/netsocket/Socket.cpp

@@ -70,6 +70,28 @@ nsapi_error_t Socket::close()
     return ret;
 }
 
+int Socket::modify_multicast_group(const SocketAddress &address, nsapi_socket_option_t socketopt)
+{
+    nsapi_ip_mreq_t mreq;
+
+    // Set up group address
+    mreq.imr_multiaddr = address.get_addr();
+    mreq.imr_interface = nsapi_addr_t();   // Default address, NSAPI_UNSPEC
+
+    return this->setsockopt(NSAPI_SOCKET, socketopt, &mreq, sizeof(mreq));
+}
+
+int Socket::join_multicast_group(const SocketAddress &address)
+{
+    return modify_multicast_group(address, NSAPI_ADD_MEMBERSHIP);
+}
+
+int Socket::leave_multicast_group(const SocketAddress &address)
+{
+    return modify_multicast_group(address, NSAPI_DROP_MEMBERSHIP);
+}
+
+
 nsapi_error_t Socket::bind(uint16_t port)
 {
     // Underlying bind is thread safe

features/netsocket/Socket.h

@@ -62,6 +62,20 @@ class Socket {
      */
     nsapi_error_t close();
 
+    /** Subscribes to an IP multicast group
+     *
+     * @param address   Multicast group IP address
+     * @return          Negative error code on failure
+     */
+    int join_multicast_group(const SocketAddress &address);
+
+    /** Leave an IP multicast group
+     *
+     * @param address   Multicast group IP address
+     * @return          Negative error code on failure
+     */
+    int leave_multicast_group(const SocketAddress &address);
+
     /** Bind a specific address to a socket
      *
      *  Binding a socket specifies the address and port on which to recieve
@@ -203,6 +217,7 @@ class Socket {
     Socket();
     virtual nsapi_protocol_t get_proto() = 0;
     virtual void event() = 0;
+    int modify_multicast_group(const SocketAddress &address, nsapi_socket_option_t socketopt);
 
     NetworkStack *_stack;
     nsapi_socket_t _socket;

features/netsocket/TCPSocket.h

@@ -57,6 +57,11 @@ class TCPSocket : public Socket {
      */
     virtual ~TCPSocket();
 
+   /** Override multicast functions to return error for TCP
+    *
+    */
+    int join_multicast_group(const SocketAddress &address) { return NSAPI_ERROR_UNSUPPORTED; }
+
     /** Connects TCP socket to a remote host
      *
      *  Initiates a connection to a remote server specified by either

features/netsocket/UDPSocket.cpp

@@ -37,6 +37,7 @@ nsapi_protocol_t UDPSocket::get_proto()
     return NSAPI_UDP;
 }
 
+
 nsapi_size_or_error_t UDPSocket::sendto(const char *host, uint16_t port, const void *data, nsapi_size_t size)
 {
     SocketAddress address;

features/netsocket/nsapi_types.h

@@ -53,6 +53,7 @@ enum nsapi_error {
     NSAPI_ERROR_IS_CONNECTED        = -3015,     /*!< socket is already connected */
     NSAPI_ERROR_CONNECTION_LOST     = -3016,     /*!< connection lost */
     NSAPI_ERROR_CONNECTION_TIMEOUT  = -3017,     /*!< connection timed out */
+    NSAPI_ERROR_ADDRESS_IN_USE      = -3018,     /*!< Address already in use */
 };
 
 /** Type used to represent error codes
@@ -158,7 +159,7 @@ typedef void *nsapi_socket_t;
 /** Enum of socket protocols
  *
  *  The socket protocol specifies a particular protocol to
- *  be used with a newly created socket. 
+ *  be used with a newly created socket.
  *
  *  @enum nsapi_protocol
  */
@@ -207,13 +208,15 @@ typedef enum nsapi_socket_level {
  *  @enum nsapi_socket_option
  */
 typedef enum nsapi_socket_option {
-    NSAPI_REUSEADDR, /*!< Allow bind to reuse local addresses */
-    NSAPI_KEEPALIVE, /*!< Enables sending of keepalive messages */
-    NSAPI_KEEPIDLE,  /*!< Sets timeout value to initiate keepalive */
-    NSAPI_KEEPINTVL, /*!< Sets timeout value for keepalive */
-    NSAPI_LINGER,    /*!< Keeps close from returning until queues empty */
-    NSAPI_SNDBUF,    /*!< Sets send buffer size */
-    NSAPI_RCVBUF,    /*!< Sets recv buffer size */
+    NSAPI_REUSEADDR,         /*!< Allow bind to reuse local addresses */
+    NSAPI_KEEPALIVE,         /*!< Enables sending of keepalive messages */
+    NSAPI_KEEPIDLE,          /*!< Sets timeout value to initiate keepalive */
+    NSAPI_KEEPINTVL,         /*!< Sets timeout value for keepalive */
+    NSAPI_LINGER,            /*!< Keeps close from returning until queues empty */
+    NSAPI_SNDBUF,            /*!< Sets send buffer size */
+    NSAPI_RCVBUF,            /*!< Sets recv buffer size */
+    NSAPI_ADD_MEMBERSHIP,    /*!< Add membership to multicast address */
+    NSAPI_DROP_MEMBERSHIP,   /*!< Drop membership to multicast address */
 } nsapi_socket_option_t;
 
 /** Supported IP protocol versions of IP stack
@@ -265,6 +268,13 @@ typedef struct nsapi_stack {
     unsigned _stack_buffer[16];
 } nsapi_stack_t;
 
+/** nsapi_ip_mreq structure
+ */
+typedef struct nsapi_ip_mreq {
+    nsapi_addr_t imr_multiaddr; /* IP multicast address of group */
+    nsapi_addr_t imr_interface; /* local IP address of interface */
+} nsapi_ip_mreq_t;
+
 /** nsapi_stack_api structure
  *
  *  Common api structure for network stack operations. A network stack
@@ -286,9 +296,9 @@ typedef struct nsapi_stack_api
      *
      *  The hostname may be either a domain name or an IP address. If the
      *  hostname is an IP address, no network transactions will be performed.
-     *  
+     *
      *  If no stack-specific DNS resolution is provided, the hostname
-     *  will be resolve using a UDP socket on the stack. 
+     *  will be resolve using a UDP socket on the stack.
      *
      *  @param stack    Stack handle
      *  @param addr     Destination for the host IP address
@@ -333,7 +343,7 @@ typedef struct nsapi_stack_api
      *  @param optval   Destination for option value
      *  @param optlen   Length of the option value
      *  @return         0 on success, negative error code on failure
-     */    
+     */
     nsapi_error_t (*getstackopt)(nsapi_stack_t *stack, int level,
             int optname, void *optval, unsigned *optlen);
 
@@ -534,7 +544,7 @@ typedef struct nsapi_stack_api
      *  @param optval   Option value
      *  @param optlen   Length of the option value
      *  @return         0 on success, negative error code on failure
-     */    
+     */
     nsapi_error_t (*setsockopt)(nsapi_stack_t *stack, nsapi_socket_t socket, int level,
             int optname, const void *optval, unsigned optlen);
 
@@ -551,7 +561,7 @@ typedef struct nsapi_stack_api
      *  @param optval   Destination for option value
      *  @param optlen   Length of the option value
      *  @return         0 on success, negative error code on failure
-     */    
+     */
     nsapi_error_t (*getsockopt)(nsapi_stack_t *stack, nsapi_socket_t socket, int level,
             int optname, void *optval, unsigned *optlen);
 } nsapi_stack_api_t;