From b56ced249aadab731a5496b31ed3bfd450e89322 Mon Sep 17 00:00:00 2001 From: Kevin Gilbert Date: Fri, 21 Jul 2017 11:21:27 -0500 Subject: [PATCH] Add multicast implementation for UDPSocket. IPv4 and IPv6 both supported. --- .../FEATURE_LWIP/lwip-interface/lwip_stack.c | 159 ++++++++++++++++++ features/netsocket/Socket.cpp | 22 +++ features/netsocket/Socket.h | 15 ++ features/netsocket/TCPSocket.h | 5 + features/netsocket/UDPSocket.cpp | 1 + features/netsocket/nsapi_types.h | 36 ++-- 6 files changed, 225 insertions(+), 13 deletions(-) diff --git a/features/FEATURE_LWIP/lwip-interface/lwip_stack.c b/features/FEATURE_LWIP/lwip-interface/lwip_stack.c index 064612ba6f..7754cb414a 100644 --- a/features/FEATURE_LWIP/lwip-interface/lwip_stack.c +++ b/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,6 +62,12 @@ 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; @@ -63,6 +75,26 @@ 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; } diff --git a/features/netsocket/Socket.cpp b/features/netsocket/Socket.cpp index fd50083bc7..7317648b65 100644 --- a/features/netsocket/Socket.cpp +++ b/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 diff --git a/features/netsocket/Socket.h b/features/netsocket/Socket.h index 7edeeb8033..4040abe5be 100644 --- a/features/netsocket/Socket.h +++ b/features/netsocket/Socket.h @@ -62,6 +62,20 @@ public: */ 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 @@ protected: 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; diff --git a/features/netsocket/TCPSocket.h b/features/netsocket/TCPSocket.h index 8f7de42116..6005121484 100644 --- a/features/netsocket/TCPSocket.h +++ b/features/netsocket/TCPSocket.h @@ -57,6 +57,11 @@ public: */ 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 diff --git a/features/netsocket/UDPSocket.cpp b/features/netsocket/UDPSocket.cpp index 5b9f510d30..cb6fbd67ce 100644 --- a/features/netsocket/UDPSocket.cpp +++ b/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; diff --git a/features/netsocket/nsapi_types.h b/features/netsocket/nsapi_types.h index 162c408256..80663a80b3 100644 --- a/features/netsocket/nsapi_types.h +++ b/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;