diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 3be7a5539b575..9d19fa4423959 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -277,6 +277,13 @@ class AP_Networking bool init_buffers(const uint32_t size_rx, const uint32_t size_tx); void thread_create(AP_HAL::MemberProc); + // true if addr (host byte order, as returned by AP_Networking_IPV4::get_uint32() + // or SocketAPM::last_recv_address()) is the broadcast or a multicast address + static bool is_broadcast_or_multicast(uint32_t addr) { + const uint8_t first_octet = (addr >> 24) & 0xFF; + return addr == 0xFFFFFFFF || (first_octet >= 224 && first_octet <= 239); + } + uint32_t txspace() override; void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override; size_t _write(const uint8_t *buffer, size_t size) override; @@ -299,6 +306,7 @@ class AP_Networking uint32_t last_size_rx; bool packetise; bool connected; + bool is_udp_client_unicast; // only meaningful when type == UDP_CLIENT, set in udp_client_loop() uint32_t last_udp_connect_address; uint16_t last_udp_connect_port; bool have_received; diff --git a/libraries/AP_Networking/AP_Networking_port.cpp b/libraries/AP_Networking/AP_Networking_port.cpp index 18bc64592b607..fb58f5754e1b4 100644 --- a/libraries/AP_Networking/AP_Networking_port.cpp +++ b/libraries/AP_Networking/AP_Networking_port.cpp @@ -181,11 +181,26 @@ void AP_Networking::Port::udp_client_loop(void) AP::network().startup_wait(); const char *dest = ip.get_str(); - if (!sock->connect(dest, port.get())) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to connect to %s", (unsigned)state.idx, dest); - delete sock; - sock = nullptr; - return; + is_udp_client_unicast = !is_broadcast_or_multicast(ip.get_uint32()); + + if (is_udp_client_unicast) { + // deliberately not calling sock->connect(): a connect()'d UDP socket + // has its incoming packets filtered by the kernel to the exact + // address *and port* it connected to, but some devices reply from a + // different source port than the one they were queried on. + // send_receive() uses sendto() for our fixed destination instead, + // and checks the source IP itself (ignoring port) on receive + } else { + // broadcast/multicast: connect() also joins the multicast group + // (IP_ADD_MEMBERSHIP) when the destination is one, which we must + // not skip - and neither of these targets are point-to-point, so + // they don't have the mismatched-reply-port problem above + if (!sock->connect(dest, port.get())) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to connect to %s", (unsigned)state.idx, dest); + delete sock; + sock = nullptr; + return; + } } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", (unsigned)state.idx, dest, unsigned(port.get())); @@ -339,15 +354,28 @@ bool AP_Networking::Port::send_receive(void) return false; } if (ret > 0) { - WITH_SEMAPHORE(sem); - readbuffer->write(buf, ret); + bool accept = true; + if (type == NetworkPortType::UDP_CLIENT && is_udp_client_unicast) { + // our socket isn't connect()'d for a unicast destination (see + // udp_client_loop()), so the kernel doesn't filter incoming + // packets for us - check the source IP ourselves. Deliberately + // not checking the source port: some devices reply from a + // different port than the one they were queried on + uint32_t src_addr = 0; + uint16_t src_port = 0; + accept = sock->last_recv_address(src_addr, src_port) && (src_addr == ip.get_uint32()); + } + if (accept) { + WITH_SEMAPHORE(sem); + readbuffer->write(buf, ret); - // Cant track dropped read packets because we only read in what there is space for - // The socket buffer becomes full and data is lost there - rx_stats_bytes += ret; + // Cant track dropped read packets because we only read in what there is space for + // The socket buffer becomes full and data is lost there + rx_stats_bytes += ret; - active = true; - have_received = true; + active = true; + have_received = true; + } } } @@ -410,8 +438,14 @@ bool AP_Networking::Port::send_receive(void) if(last_udp_connect_address != 0 && last_udp_connect_port != 0) { ret = sock->sendto(buf, n, last_udp_connect_address, last_udp_connect_port); } + } else if (type == NetworkPortType::UDP_CLIENT && is_udp_client_unicast) { + // a unicast UDP Client also uses sendto rather than a connect()'d + // send() - see udp_client_loop() and the receive-side comment + // above for why + ret = sock->sendto(buf, n, ip.get_uint32(), port.get()); } else { - // TCP Server and Client and UDP Client use send + // TCP Server and Client, and a broadcast/multicast UDP Client + // (which is connect()'d - see udp_client_loop()), use send() ret = sock->send(buf, n); }