From 205c5dc009e35c9f72cd052ccabda2db26dfcf22 Mon Sep 17 00:00:00 2001
From: Max SCHMELLER <max.schmeller@tier4.jp>
Date: Fri, 22 Nov 2024 14:06:30 +0900
Subject: [PATCH] fix(udp): return correctly truncated buffer when oversized
 packet is received

Signed-off-by: Max SCHMELLER <max.schmeller@tier4.jp>
---
 .../connections/udp.hpp                          | 11 +++++++----
 nebula_hw_interfaces/test/common/test_udp.cpp    | 16 +++++++++++++++-
 .../test/common/test_udp/utils.hpp               |  1 +
 3 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp
index 0261e069..23cb4b92 100644
--- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp
+++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp
@@ -22,6 +22,7 @@
 #include <sys/socket.h>
 #include <unistd.h>
 
+#include <algorithm>
 #include <atomic>
 #include <cassert>
 #include <cerrno>
@@ -286,15 +287,17 @@ class UdpSocket
         buffer.resize(buffer_size_);
         auto msg_header = make_msg_header(buffer);
 
-        ssize_t received = recvmsg(sock_fd_, &msg_header.msg, MSG_TRUNC);
-        if (received < 0) throw SocketError(errno);
+        ssize_t recv_result = recvmsg(sock_fd_, &msg_header.msg, MSG_TRUNC);
+        if (recv_result < 0) throw SocketError(errno);
+        size_t untruncated_packet_length = recv_result;
+
         if (!is_accepted_sender(msg_header.sender_addr)) continue;
 
         ReceiveMetadata metadata;
         get_receive_metadata(msg_header.msg, metadata);
-        metadata.truncated = static_cast<size_t>(received) > buffer_size_;
+        metadata.truncated = untruncated_packet_length > buffer_size_;
 
-        buffer.resize(received);
+        buffer.resize(std::min(buffer_size_, untruncated_packet_length));
         callback_(buffer, metadata);
       }
     });
diff --git a/nebula_hw_interfaces/test/common/test_udp.cpp b/nebula_hw_interfaces/test/common/test_udp.cpp
index eb5f68cc..67407724 100644
--- a/nebula_hw_interfaces/test/common/test_udp.cpp
+++ b/nebula_hw_interfaces/test/common/test_udp.cpp
@@ -159,7 +159,7 @@ TEST(test_udp, test_receiving_oversized)
 {
   const size_t mtu = 1500;
   std::vector<uint8_t> payload;
-  payload.resize(mtu + 1);
+  payload.resize(mtu + 1, 0x42);
   UdpSocket sock{};
   sock.init(localhost_ip, host_port).set_mtu(mtu).bind();
 
@@ -170,11 +170,25 @@ TEST(test_udp, test_receiving_oversized)
 
   ASSERT_TRUE(result_opt.has_value());
   auto const & [recv_payload, metadata] = result_opt.value();
+  ASSERT_EQ(recv_payload.size(), mtu);
   ASSERT_TRUE(std::equal(recv_payload.begin(), recv_payload.end(), payload.begin()));
   ASSERT_TRUE(metadata.truncated);
   ASSERT_EQ(metadata.drops_since_last_receive, 0);
 }
 
+TEST(test_udp, test_filtering_sender)
+{
+  std::vector<uint8_t> payload{1, 2, 3};
+  UdpSocket sock{};
+  sock.init(localhost_ip, host_port).bind().limit_to_sender(sender_ip, sender_port);
+
+  auto err_no_opt = udp_send(localhost_ip, host_port, payload);
+  if (err_no_opt.has_value()) GTEST_SKIP() << strerror(err_no_opt.value());
+
+  auto result_opt = receive_once(sock, send_receive_timeout);
+  ASSERT_FALSE(result_opt.has_value());
+}
+
 }  // namespace nebula::drivers::connections
 
 int main(int argc, char * argv[])
diff --git a/nebula_hw_interfaces/test/common/test_udp/utils.hpp b/nebula_hw_interfaces/test/common/test_udp/utils.hpp
index 6b949219..dc9a08c0 100644
--- a/nebula_hw_interfaces/test/common/test_udp/utils.hpp
+++ b/nebula_hw_interfaces/test/common/test_udp/utils.hpp
@@ -26,6 +26,7 @@
 #include <cstdint>
 #include <cstring>
 #include <optional>
+#include <utility>
 #include <vector>
 
 std::optional<int> udp_send(