From b6c1cdd6ec668b29a9d321ac9218b877556c7f3a Mon Sep 17 00:00:00 2001 From: garfieldG Date: Thu, 9 Aug 2018 15:44:06 +0300 Subject: [PATCH] [BACKPORT] Ethernet : changes were made to support ethernet -unicast -broadcast -multicast --- include/nuttx/net/ip.h | 70 +++++++----- libc/net/Make.defs | 3 +- libc/net/lib_inetaton.c | 125 +++++++++++++++++++++ net/arp/arp_out.c | 107 +++++++++++------- net/arp/arp_send.c | 11 ++ net/devif/ipv4_input.c | 108 ++++++++++-------- net/inet/inet_recvfrom.c | 4 +- net/inet/inet_sockif.c | 28 ++++- net/netdev/netdev_ioctl.c | 37 ++++++ net/udp/udp.h | 7 ++ net/udp/udp_conn.c | 229 +++++++++++++++++++++++++++----------- net/udp/udp_netpoll.c | 10 +- 12 files changed, 543 insertions(+), 196 deletions(-) create mode 100644 libc/net/lib_inetaton.c diff --git a/include/nuttx/net/ip.h b/include/nuttx/net/ip.h index 3a5c14212b7..2cdfeeb84b6 100644 --- a/include/nuttx/net/ip.h +++ b/include/nuttx/net/ip.h @@ -508,6 +508,46 @@ bool net_ipv6addr_maskcmp(const net_ipv6addr_t addr1, const net_ipv6addr_t mask); #endif +/**************************************************************************** + * Name: net_ipv4addr_broadcast + * + * Description: + * Mask out the network part of an IP address, given the address and + * the netmask. + * + * Example: + * + * in_addr_t ipaddr; + * in_addr_t netmask; + * bool isbroadcast; + * + * net_ipaddr(&netmask, 255,255,255,0); + * net_ipaddr(&ipaddr, 192,16,1,255); + * isbroadcast = net_ipv4addr_broadcast(ipaddr, netmask); + * + * Will return isboadcast == true. + * + * net_ipaddr(&ipaddr, 192,16,1,2); + * isbroadcast = net_ipv4addr_broadcast(ipaddr, netmask); + * + * Will return isboadcast == false. + * + * NOTES: + * 1. This function does not check for the broadcast address + * 255.255.255.255. That must be performed as a seperate check. + * 2. You must also separately check if the ipaddress lies on the sub-net + * using, perhaps, net_ipv4addr_maskcmp(). + * + * Input Parameters: + * addr - The IPv4 address to check + * mask - The network mask + * + ****************************************************************************/ + +#define net_ipv4addr_broadcast(addr, mask) \ + (((in_addr_t)(addr) & ~(in_addr_t)(mask)) == \ + ((in_addr_t)(0xffffffff) & ~(in_addr_t)(mask))) + /**************************************************************************** * Name: net_ipv6addr_prefixcmp * @@ -587,36 +627,6 @@ bool net_ipv6addr_maskcmp(const net_ipv6addr_t addr1, #define net_is_addr_linklocal(a) ((a)[0] == HTONS(0xfe80)) -/**************************************************************************** - * Name: net_ipaddr_mask - * - * Description: - * Mask out the network part of an IP address, given the address and - * the netmask. - * - * Example: - * - * in_addr_t ipaddr1, ipaddr2, netmask; - * - * net_ipaddr(&ipaddr1, 192,16,1,2); - * net_ipaddr(&netmask, 255,255,255,0); - * net_ipaddr_mask(&ipaddr2, &ipaddr1, &netmask); - * - * In the example above, the variable "ipaddr2" will contain the IP - * address 192.168.1.0. - * - * Input Parameters: - * dest Where the result is to be placed. - * src The IP address. - * mask The netmask. - * - ****************************************************************************/ - -#define net_ipaddr_mask(dest, src, mask) \ - do { \ - (in_addr_t)(dest) = (in_addr_t)(src) & (in_addr_t)(mask); \ - } while (0) - #undef EXTERN #ifdef __cplusplus } diff --git a/libc/net/Make.defs b/libc/net/Make.defs index 0d2a7fb4d42..0052bc440ac 100644 --- a/libc/net/Make.defs +++ b/libc/net/Make.defs @@ -36,7 +36,8 @@ # Add the networking C files to the build CSRCS += lib_addrconfig.c lib_etherntoa.c lib_htons.c lib_htonl.c -CSRCS += lib_inetaddr.c lib_inetntoa.c lib_inetntop.c lib_inetpton.c +CSRCS += lib_inetaddr.c lib_inetaton.c lib_inetntoa.c +CSRCS += lib_inetntop.c lib_inetpton.c ifeq ($(CONFIG_NET),y) CSRCS += lib_shutdown.c diff --git a/libc/net/lib_inetaton.c b/libc/net/lib_inetaton.c new file mode 100644 index 00000000000..1f0c052b02b --- /dev/null +++ b/libc/net/lib_inetaton.c @@ -0,0 +1,125 @@ +/**************************************************************************** + * libc/net/lib_inetaton + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Juha Niskanen + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: inet_aton + * + * Description: + * The inet_aton() function converts the string pointed to by cp, in the + * standard IPv4 dotted decimal notation, to an integer value suitable for + * use as an Internet address. + * + * Note: + * inet_aton() returns nonzero if the address is valid, zero if not. + * Therefore macros OK and ERROR are not used here. + * + ****************************************************************************/ + +int inet_aton(FAR const char *cp, FAR struct in_addr *inp) +{ + int dots = 0; + uint32_t num = 0; + uint32_t addr = 0; + int c; + + do + { + c = *cp; + + switch (c) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + num = 10 * num + (c - '0'); + break; + + case '.': + dots++; + if (dots > 3) + { + return 0; + } + + /* no break */ + + case '\0': + if (num > 255) + { + return 0; + } + + addr = addr << 8 | num; + num = 0; + break; + + default: + return 0; + } + } + while (*cp++); + + /* Normalize it */ + + if (dots < 3) + { + addr <<= 8 * (3 - dots); + } + + if (inp) + { + inp->s_addr = HTONL(addr); + } + + return 1; +} diff --git a/net/arp/arp_out.c b/net/arp/arp_out.c index 53853af7a2d..4c0ef478b96 100644 --- a/net/arp/arp_out.c +++ b/net/arp/arp_out.c @@ -169,6 +169,7 @@ void arp_out(FAR struct net_driver_s *dev) if (net_ipv4addr_hdrcmp(pip->eh_destipaddr, g_broadcast_ipaddr)) { memcpy(peth->dest, g_broadcast_ethaddr.ether_addr_octet, ETHER_ADDR_LEN); + goto finish_header; } #ifdef CONFIG_NET_IGMP @@ -181,74 +182,94 @@ void arp_out(FAR struct net_driver_s *dev) * addresses=0xff (ff00::/8.) */ - else if (NTOHS(pip->eh_destipaddr[0]) >= 0xe000 && - NTOHS(pip->eh_destipaddr[0]) <= 0xefff) + if (NTOHS(pip->eh_destipaddr[0]) >= 0xe000 && + NTOHS(pip->eh_destipaddr[0]) <= 0xefff) { /* Build the well-known IPv4 IGMP Ethernet address. The first * three bytes are fixed; the final three variable come from the - * last three bytes of the IP address. + * last three bytes of the IPv4 address (network order). + * + * Address range : 01:00:5e:00:00:00 to 01:00:5e:7f:ff:ff */ - FAR const uint8_t *ip = ((FAR uint8_t *)pip->eh_destipaddr) + 1; - memcpy(peth->dest, g_multicast_ethaddr, 3); - memcpy(&peth->dest[3], ip, 3); + FAR const uint8_t *ip = (FAR uint8_t *)pip->eh_destipaddr; + + peth->dest[0] = g_multicast_ethaddr[0]; + peth->dest[1] = g_multicast_ethaddr[1]; + peth->dest[2] = g_multicast_ethaddr[2]; + peth->dest[3] = ip[1] & 0x7f; + peth->dest[4] = ip[2]; + peth->dest[5] = ip[3]; + + goto finish_header; } #endif - else - { - /* Check if the destination address is on the local network. */ - destipaddr = net_ip4addr_conv32(pip->eh_destipaddr); - if (!net_ipv4addr_maskcmp(destipaddr, dev->d_ipaddr, dev->d_netmask)) - { - /* Destination address is not on the local network */ + /* Check if the destination address is on the local network. */ + + destipaddr = net_ip4addr_conv32(pip->eh_destipaddr); + if (!net_ipv4addr_maskcmp(destipaddr, dev->d_ipaddr, dev->d_netmask)) + { + /* Destination address is not on the local network */ #ifdef CONFIG_NET_ROUTE - /* We have a routing table.. find the correct router to use in - * this case (or, as a fall-back, use the device's default router - * address). We will use the router IP address instead of the - * destination address when determining the MAC address. - */ + /* We have a routing table.. find the correct router to use in + * this case (or, as a fall-back, use the device's default router + * address). We will use the router IP address instead of the + * destination address when determining the MAC address. + */ - netdev_ipv4_router(dev, destipaddr, &ipaddr); + netdev_ipv4_router(dev, destipaddr, &ipaddr); #else - /* Use the device's default router IP address instead of the - * destination address when determining the MAC address. - */ + /* Use the device's default router IP address instead of the + * destination address when determining the MAC address. + */ - net_ipv4addr_copy(ipaddr, dev->d_draddr); + net_ipv4addr_copy(ipaddr, dev->d_draddr); #endif - } - else - { - /* Else, we use the destination IP address. */ + } + + /* The destination address is on the local network. Check if it is + * the sub-net broadcast address. + */ - net_ipv4addr_copy(ipaddr, destipaddr); - } + else if (net_ipv4addr_broadcast(destipaddr, dev->d_netmask)) + { + /* Yes.. then we won't need to know the destination MAC address */ - /* Check if we already have this destination address in the ARP table */ + memcpy(peth->dest, g_broadcast_ethaddr.ether_addr_octet, ETHER_ADDR_LEN); + goto finish_header; + } + else + { + /* Else, we use the destination IP address. */ - tabptr = arp_find(ipaddr); - if (!tabptr) - { - ninfo("ARP request for IP %08lx\n", (unsigned long)ipaddr); + net_ipv4addr_copy(ipaddr, destipaddr); + } - /* The destination address was not in our ARP table, so we - * overwrite the IP packet with an ARP request. - */ + /* Check if we already have this destination address in the ARP table */ - arp_format(dev, ipaddr); - arp_dump(ARPBUF); - return; - } + tabptr = arp_find(ipaddr); + if (tabptr == NULL) + { + ninfo("ARP request for IP %08lx\n", (unsigned long)ipaddr); - /* Build an Ethernet header. */ + /* The destination address was not in our ARP table, so we overwrite + * the IP packet with an ARP request. + */ - memcpy(peth->dest, tabptr->at_ethaddr.ether_addr_octet, ETHER_ADDR_LEN); + arp_format(dev, ipaddr); + arp_dump(ARPBUF); + return; } + /* Build an Ethernet header. */ + + memcpy(peth->dest, tabptr->at_ethaddr.ether_addr_octet, ETHER_ADDR_LEN); + /* Finish populating the Ethernet header */ +finish_header: memcpy(peth->src, dev->d_mac.ether.ether_addr_octet, ETHER_ADDR_LEN); peth->type = HTONS(ETHTYPE_IP); dev->d_len += ETH_HDRLEN; diff --git a/net/arp/arp_send.c b/net/arp/arp_send.c index 9f3416cca33..94e99bcb57c 100644 --- a/net/arp/arp_send.c +++ b/net/arp/arp_send.c @@ -268,6 +268,17 @@ int arp_send(in_addr_t ipaddr) ipaddr = dripaddr; } + /* The destination address is on the local network. Check if it is + * the sub-net broadcast address. + */ + + else if (net_ipv4addr_broadcast(ipaddr, dev->d_netmask)) + { + /* Yes.. We don't need to send the ARP request */ + + return OK; + } + /* Allocate resources to receive a callback. This and the following * initialization is performed with the network lock because we don't * want anything to happen until we are ready. diff --git a/net/devif/ipv4_input.c b/net/devif/ipv4_input.c index f9dc601ec61..3e7a659a0ed 100644 --- a/net/devif/ipv4_input.c +++ b/net/devif/ipv4_input.c @@ -321,6 +321,7 @@ static uint8_t devif_reassembly(void) int ipv4_input(FAR struct net_driver_s *dev) { FAR struct ipv4_hdr_s *ipv4 = BUF; + in_addr_t destipaddr; uint16_t hdrlen; uint16_t iplen; @@ -395,6 +396,10 @@ int ipv4_input(FAR struct net_driver_s *dev) #endif /* CONFIG_NET_TCP_REASSEMBLY */ } + /* Get the destination IP address in a friendlier form */ + + destipaddr = net_ip4addr_conv32(ipv4->destipaddr); + #if defined(CONFIG_NET_BROADCAST) && defined(NET_UDP_HAVE_STACK) /* If IP broadcast support is configured, we check for a broadcast * UDP packet, which may be destined to us (even if there is no IP @@ -403,8 +408,7 @@ int ipv4_input(FAR struct net_driver_s *dev) */ if (ipv4->proto == IP_PROTO_UDP && - net_ipv4addr_cmp(net_ip4addr_conv32(ipv4->destipaddr), - INADDR_BROADCAST)) + net_ipv4addr_cmp(destipaddr, INADDR_BROADCAST)) { #ifdef CONFIG_NET_IPFORWARD_BROADCAST /* Forward broadcast packets */ @@ -413,74 +417,90 @@ int ipv4_input(FAR struct net_driver_s *dev) #endif return udp_ipv4_input(dev); } - - /* In other cases, the device must be assigned a non-zero IP address. */ - else #endif #ifdef CONFIG_NET_ICMP + /* In other cases, the device must be assigned a non-zero IP address. */ + if (net_ipv4addr_cmp(dev->d_ipaddr, INADDR_ANY)) { nwarn("WARNING: No IP address assigned\n"); goto drop; } - - /* Check if the packet is destined for our IP address */ else #endif +#if defined(CONFIG_NET_BROADCAST) && defined(NET_UDP_HAVE_STACK) + /* The address is not the broadcast address and we have been assigned a + * address. So there is also the possibility that the destination address + * is a sub-net broadcast address which we will need to handle just as for + * the broadcast address above. + */ + + if (ipv4->proto == IP_PROTO_UDP && + net_ipv4addr_maskcmp(destipaddr, dev->d_ipaddr, dev->d_netmask) && + net_ipv4addr_broadcast(destipaddr, dev->d_netmask)) { - /* Check if the packet is destined for our IP address. */ +#ifdef CONFIG_NET_IPFORWARD_BROADCAST + /* Forward broadcast packets */ - if (!net_ipv4addr_cmp(net_ip4addr_conv32(ipv4->destipaddr), - dev->d_ipaddr)) - { - /* Check for an IPv4 IGMP group address */ + ipv4_forward_broadcast(dev, ipv4); +#endif + return udp_ipv4_input(dev); + } + else +#endif + /* Check if the packet is destined for our IP address. */ + + if (!net_ipv4addr_cmp(destipaddr, dev->d_ipaddr)) + { + /* No.. This is not our IP address. Check for an IPv4 IGMP group + * address + */ #ifdef CONFIG_NET_IGMP - in_addr_t destip = net_ip4addr_conv32(ipv4->destipaddr); - if (igmp_grpfind(dev, &destip) != NULL) - { + in_addr_t destip = net_ip4addr_conv32(ipv4->destipaddr); + if (igmp_grpfind(dev, &destip) != NULL) + { #ifdef CONFIG_NET_IPFORWARD_BROADCAST - /* Forward multicast packets */ + /* Forward multicast packets */ - ipv4_forward_broadcast(dev, ipv4); + ipv4_forward_broadcast(dev, ipv4); #endif - } - else + } + else #endif - { - /* No.. The packet is not destined for us. */ + { + /* No.. The packet is not destined for us. */ #ifdef CONFIG_NET_IPFORWARD - /* Try to forward the packet */ + /* Try to forward the packet */ - int ret = ipv4_forward(dev, ipv4); - if (ret >= 0) - { - /* The packet was forwarded. Return success; d_len will - * be set appropriately by the forwarding logic: Cleared - * if the packet is forward via anoother device or non- - * zero if it will be forwarded by the same device that - * it was received on. - */ - - return OK; - } - else + int ret = ipv4_forward(dev, ipv4); + if (ret >= 0) + { + /* The packet was forwarded. Return success; d_len will + * be set appropriately by the forwarding logic: Cleared + * if the packet is forward via anoother device or non- + * zero if it will be forwarded by the same device that + * it was received on. + */ + + return OK; + } + else #endif - { - /* Not destined for us and not forwardable... Drop the - * packet. - */ + { + /* Not destined for us and not forwardable... Drop the + * packet. + */ - nwarn("WARNING: Not destined for us; not forwardable... " - "Dropping!\n"); + nwarn("WARNING: Not destined for us; not forwardable... " + "Dropping!\n"); #ifdef CONFIG_NET_STATISTICS - g_netstats.ipv4.drop++; + g_netstats.ipv4.drop++; #endif - goto drop; - } + goto drop; } } } diff --git a/net/inet/inet_recvfrom.c b/net/inet/inet_recvfrom.c index 7035d71a5f8..84eda3ca72e 100644 --- a/net/inet/inet_recvfrom.c +++ b/net/inet/inet_recvfrom.c @@ -1,7 +1,7 @@ /**************************************************************************** * net/inet/inet_recvfrom.c * - * Copyright (C) 2007-2009, 2011-2017 Gregory Nutt. All rights reserved. + * Copyright (C) 2007-2009, 2011-2018 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -1224,6 +1224,8 @@ static ssize_t inet_udp_recvfrom(FAR struct socket *psock, FAR void *buf, size_t #endif #ifdef CONFIG_NET_UDP_READAHEAD + /* Handle non-blocking UDP sockets */ + if (_SS_ISNONBLOCK(psock->s_flags)) { /* Return the number of bytes read from the read-ahead buffer if diff --git a/net/inet/inet_sockif.c b/net/inet/inet_sockif.c index 4ccedad5f91..e7f8513be00 100644 --- a/net/inet/inet_sockif.c +++ b/net/inet/inet_sockif.c @@ -1,7 +1,7 @@ /**************************************************************************** * net/inet/inet_sockif.c * - * Copyright (C) 2017 Gregory Nutt. All rights reserved. + * Copyright (C) 2017-2018 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -685,14 +685,36 @@ static int inet_connect(FAR struct socket *psock, #if defined(CONFIG_NET_UDP) && defined(NET_UDP_HAVE_STACK) case SOCK_DGRAM: { - int ret = udp_connect(psock->s_conn, addr); - if (ret < 0) + FAR struct udp_conn_s *conn; + int ret; + + /* We will accept connecting to a addr == NULL for disconnection. + * However, the correct way is to disconnect is to provide an + * address with sa_family == AF_UNSPEC. + */ + + if (addr != NULL && addr->sa_family == AF_UNSPEC) + { + addr = NULL; + } + + /* Perform the connect/disconnect operation */ + + conn = (FAR struct udp_conn_s *)psock->s_conn; + ret = udp_connect(conn, addr); + if (ret < 0 || addr == NULL) { + /* Failed to connect or explicitly disconnected */ + psock->s_flags &= ~_SF_CONNECTED; + conn->flags &= ~_UDP_FLAG_CONNECTMODE; } else { + /* Successfully connected */ + psock->s_flags |= _SF_CONNECTED; + conn->flags |= _UDP_FLAG_CONNECTMODE; } return ret; diff --git a/net/netdev/netdev_ioctl.c b/net/netdev/netdev_ioctl.c index 6870d4a4261..bd51a2d0ac4 100644 --- a/net/netdev/netdev_ioctl.c +++ b/net/netdev/netdev_ioctl.c @@ -264,6 +264,30 @@ static void ioctl_get_ipv4addr(FAR struct sockaddr *outaddr, } #endif +/**************************************************************************** + * Name: ioctl_get_ipv4broadcast + * + * Description: + * Return the sub-net broadcast address to user memory. + * + * Input Parameters: + * outaddr - Pointer to the user-provided memory to receive the address. + * inaddr - The source IP address in the device structure. + * netmask - The netmask address mask in the device structure. + * + ****************************************************************************/ + +#ifdef CONFIG_NET_IPv4 +static void inline ioctl_get_ipv4broadcast(FAR struct sockaddr *outaddr, + in_addr_t inaddr, in_addr_t netmask) +{ + FAR struct sockaddr_in *dest = (FAR struct sockaddr_in *)outaddr; + dest->sin_family = AF_INET; + dest->sin_port = 0; + dest->sin_addr.s_addr = net_ipv4addr_broadcast(inaddr, netmask); +} +#endif + /**************************************************************************** * Name: ioctl_get_ipv6addr * @@ -612,6 +636,19 @@ static int netdev_ifr_ioctl(FAR struct socket *psock, int cmd, #ifdef CONFIG_NET_IPv4 case SIOCGIFBRDADDR: /* Get broadcast IP address */ + { + dev = netdev_ifr_dev(req); + if (dev) + { + ioctl_get_ipv4broadcast(&req->ifr_broadaddr, dev->d_ipaddr, + dev->d_netmask); + ret = OK; + } + } + break; +#endif + +#ifdef CONFIG_NET_IPv4 case SIOCSIFBRDADDR: /* Set broadcast IP address */ { ret = -ENOSYS; diff --git a/net/udp/udp.h b/net/udp/udp.h index cb6ca378f5c..6de4248c95d 100644 --- a/net/udp/udp.h +++ b/net/udp/udp.h @@ -73,6 +73,12 @@ #define udp_callback_free(dev,conn,cb) \ devif_conn_callback_free((dev), (cb), &(conn)->list) +/* Definitions for the UDP connection struct flag field */ + +#define _UDP_FLAG_CONNECTMODE (1 << 0) /* Bit 0: UDP connection-mode */ + +#define _UDP_ISCONNECTMODE(f) (((f) & _UDP_FLAG_CONNECTMODE) != 0) + /**************************************************************************** * Public Type Definitions ****************************************************************************/ @@ -88,6 +94,7 @@ struct udp_conn_s union ip_binding_u u; /* IP address binding */ uint16_t lport; /* Bound local port number (network byte order) */ uint16_t rport; /* Remote port number (network byte order) */ + uint8_t flags; /* See _UDP_FLAG_* definitions */ uint8_t domain; /* IP domain: PF_INET or PF_INET6 */ uint8_t ttl; /* Default time-to-live */ uint8_t crefs; /* Reference counts on this instance */ diff --git a/net/udp/udp_conn.c b/net/udp/udp_conn.c index 210ce351e06..ddbcf26133e 100644 --- a/net/udp/udp_conn.c +++ b/net/udp/udp_conn.c @@ -271,35 +271,55 @@ static inline FAR struct udp_conn_s * /* If the local UDP port is non-zero, the connection is considered * to be used. If so, then the following checks are performed: * - * - The local port number is checked against the destination port - * number in the received packet. - * - The remote port number is checked if the connection is bound - * to a remote port. - * - If multiple network interfaces are supported, then the local - * IP address is available and we will insist that the - * destination IP matches the bound address (or the destination - * IP address is a broadcast address). If a socket is bound to - * INADDRY_ANY (laddr), then it should receive all packets - * directed to the port. - * - Finally, if the connection is bound to a remote IP address, - * the source IP address of the packet is checked. Broadcast - * addresses are also accepted. + * 1. The destination address is verified against the bound address + * of the connection. + * + * - The local port number is checked against the destination port + * number in the received packet. + * - If multiple network interfaces are supported, then the local + * IP address is available and we will insist that the + * destination IP matches the bound address (or the destination + * IP address is a broadcast address). If a socket is bound to + * INADDRY_ANY (laddr), then it should receive all packets + * directed to the port. + * + * 2. If this is a connection mode UDP socket, then the source address + * is verified against the connected remote address. + * + * - The remote port number is checked if the connection is bound + * to a remote port. + * - Finally, if the connection is bound to a remote IP address, + * the source IP address of the packet is checked. Broadcast + * addresses are also accepted. * * If all of the above are true then the newly received UDP packet * is destined for this UDP connection. * - * To send and receive broadcast packets, the application should: + * To send and receive multicast packets, the application should: + * + * - Bind socket to INADDR6_ANY (for the all-nodes multicast address) + * or to a specific + * - setsockopt to SO_BROADCAST (for all-nodes address) + * + * For connection-less UDP sockets: * - * - Bind socket to INADDR_ANY - * - setsockopt to SO_BROADCAST - * - call sendto with sendaddr.sin_addr.s_addr = - * - call recvfrom. + * - call sendto with sendaddr.sin_addr.s_addr = + * - call recvfrom. + * + * For connection-mode UDP sockets: + * + * - call connect() to connect the UDP socket to a specific remote + * address, then + * - Call send() with no address address information + * - call recv() (from address information should not be needed) * * REVIST: SO_BROADCAST flag is currently ignored. */ + /* Check that there is a local port number and this is matches + * the port number in the destination address. + */ if (conn->lport != 0 && udp->destport == conn->lport && - (conn->rport == 0 || udp->srcport == conn->rport) && /* Local port accepts any address on this port or there * is an exact match in destipaddr and the bound local @@ -308,22 +328,53 @@ static inline FAR struct udp_conn_s * */ (net_ipv4addr_cmp(conn->u.ipv4.laddr, INADDR_ANY) || - net_ipv4addr_hdrcmp(ip->destipaddr, &conn->u.ipv4.laddr)) && - - /* If not connected to a remote address, or a broadcast address - * destipaddr was received, or there is an exact match between the - * srcipaddr and the bound IP address, then accept the packet. + net_ipv4addr_hdrcmp(ip->destipaddr, &conn->u.ipv4.laddr))) + { + /* Check if the socket is connection mode. In this case, only + * packets with source addresses from the connected remote peer + * will be accepted. */ - (net_ipv4addr_cmp(conn->u.ipv4.raddr, INADDR_ANY) || + if (_UDP_ISCONNECTMODE(conn->flags)) + { + /* Check if the UDP connection is either (1) accepting packets + * from any port or (2) the packet srcport matches the local + * bound port number. + */ + + if ((conn->rport == 0 || udp->srcport == conn->rport) && + + /* If (1) not connected to a remote address, or (2) a all- + * nodes multicast destipaddr was received, or (3) there is an + * exact match between the srcipaddr and the bound remote IP + * address, then accept the packet. + */ + + + + (net_ipv4addr_cmp(conn->u.ipv4.raddr, INADDR_ANY) || #ifdef CONFIG_NET_BROADCAST - net_ipv4addr_hdrcmp(ip->destipaddr, &bcast) || + net_ipv4addr_hdrcmp(ip->destipaddr, &bcast) || #endif - net_ipv4addr_hdrcmp(ip->srcipaddr, &conn->u.ipv4.raddr))) - { - /* Matching connection found.. return a reference to it */ + net_ipv4addr_hdrcmp(ip->srcipaddr, &conn->u.ipv4.raddr))) + { + /* Matching connection found.. Break out of the loop and + * return this reference to it. + */ + + break; + } + } + else + { + /* This UDP socket is not connected. We need to match only + * the destination address with the bound socket address. + * Break out out of the loop and return this reference to + * the matching connection structure. + */ - break; + break; + } } /* Look at the next active connection */ @@ -355,64 +406,112 @@ static inline FAR struct udp_conn_s * FAR struct udp_conn_s *conn; conn = (FAR struct udp_conn_s *)g_active_udp_connections.head; - while (conn) + while (conn != NULL) { /* If the local UDP port is non-zero, the connection is considered * to be used. If so, then the following checks are performed: * - * - The local port number is checked against the destination port - * number in the received packet. - * - The remote port number is checked if the connection is bound - * to a remote port. - * - If multiple network interfaces are supported, then the local - * IP address is available and we will insist that the - * destination IP matches the bound address. If a socket is bound to - * INADDR6_ANY (laddr), then it should receive all packets directed - * to the port. REVISIT: Should also depend on SO_BROADCAST. - * - Finally, if the connection is bound to a remote IP address, - * the source IP address of the packet is checked. + * 1. The destination address is verified against the bound address + * of the connection. + * + * - The local port number is checked against the destination port + * number in the received packet. + * - If multiple network interfaces are supported, then the local + * IP address is available and we will insist that the + * destination IP matches the bound address. If a socket is bound + * to INADDR6_ANY (laddr), then it should receive all packets + * directed to the port. REVISIT: Should also depend on SO_BROADCAST. + * + * 2. If this is a connection mode UDP socket, then the source address + * is verified against the connected remote address. + * + * - The remote port number is checked if the connection is bound + * to a remote port. + * - Finally, if the connection is bound to a remote IP address, + * the source IP address of the packet is checked. * * If all of the above are true then the newly received UDP packet * is destined for this UDP connection. * * To send and receive multicast packets, the application should: * - * - Bind socket to INADDR6_ANY (for the all-nodes multicast address) - * or to a specific - * - setsockopt to SO_BROADCAST (for all-nodes address) - * - call sendto with sendaddr.sin_addr.s_addr = - * - call recvfrom. + * - Bind socket to INADDR6_ANY (for the all-nodes multicast address) + * or to a specific + * - setsockopt to SO_BROADCAST (for all-nodes address) + * + * For connection-less UDP sockets: + * + * - call sendto with sendaddr.sin_addr.s_addr = + * - call recvfrom. + * + * For connection-mode UDP sockets: + * + * - call connect() to connect the UDP socket to a specific remote + * address, then + * - Call send() with no address address information + * - call recv() (from address information should not be needed) * * REVIST: SO_BROADCAST flag is currently ignored. */ - if (conn->lport != 0 && udp->destport == conn->lport && - (conn->rport == 0 || udp->srcport == conn->rport) && + /* Check that there is a local port number and this is matches + * the port number in the destination address. + */ - /* Local port accepts any address on this port or there - * is an exact match in destipaddr and the bound local - * address. This catches the cast of the all nodes multicast - * when the socket is bound to INADDR6_ANY. + if ((conn->lport != 0 && udp->destport == conn->lport && + + /* Check if the local port accepts any address on this port or + * that there is an exact match between the destipaddr and the + * bound local address. This catches the case of the all nodes + * multicast when the socket is bound to INADDR6_ANY. */ (net_ipv6addr_cmp(conn->u.ipv6.laddr, g_ipv6_allzeroaddr) || - net_ipv6addr_hdrcmp(ip->destipaddr, conn->u.ipv6.laddr)) && - - /* If not connected to a remote address, or a all-nodes multicast - * destipaddr was received, or there is an exact match between the - * srcipaddr and the bound remote IP address, then accept the - * packet. + net_ipv6addr_hdrcmp(ip->destipaddr, conn->u.ipv6.laddr)))) + { + /* Check if the socket is connection mode. In this case, only + * packets with source addresses from the connected remote peer + * will be accepted. */ - (net_ipv6addr_cmp(conn->u.ipv6.raddr, g_ipv6_allzeroaddr) || + if (_UDP_ISCONNECTMODE(conn->flags)) + { + /* Check if the UDP connection is either (1) accepting packets + * from any port or (2) the packet srcport matches the local + * bound port number. + */ + + if ((conn->rport == 0 || udp->srcport == conn->rport) && + + /* If (1) not connected to a remote address, or (2) a all- + * nodes multicast destipaddr was received, or (3) there is an + * exact match between the srcipaddr and the bound remote IP + * address, then accept the packet. + */ + + (net_ipv6addr_cmp(conn->u.ipv6.raddr, g_ipv6_allzeroaddr) || #ifdef CONFIG_NET_BROADCAST - net_ipv6addr_hdrcmp(ip->destipaddr, g_ipv6_allnodes) || + net_ipv6addr_hdrcmp(ip->destipaddr, g_ipv6_allnodes) || #endif - net_ipv6addr_hdrcmp(ip->srcipaddr, conn->u.ipv6.raddr))) - { - /* Matching connection found.. return a reference to it */ + net_ipv6addr_hdrcmp(ip->srcipaddr, conn->u.ipv6.raddr))) + { + /* Matching connection found.. Break out of the loop and + * return this reference to it. + */ + + break; + } + } + else + { + /* This UDP socket is not connected. We need to match only + * the destination address with the bound socket address. + * Break out out of the loop and return this reference to + * the matching connection structure. + */ - break; + break; + } } /* Look at the next active connection */ diff --git a/net/udp/udp_netpoll.c b/net/udp/udp_netpoll.c index 20fdf587872..d589866f009 100644 --- a/net/udp/udp_netpoll.c +++ b/net/udp/udp_netpoll.c @@ -192,15 +192,7 @@ int udp_pollsetup(FAR struct socket *psock, FAR struct pollfd *fds) info->dev = udp_find_laddr_device(conn); - /* Setup the UDP remote connection */ - - ret = udp_connect(conn, NULL); - if (ret) - { - goto errout_with_lock; - } - - /* Allocate a TCP/IP callback structure */ + /* Allocate a UDP callback structure */ cb = udp_callback_alloc(info->dev, conn); if (!cb)