From 5a795ccba065d948b89d32325455a6cab3db528b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 17 Mar 2024 16:42:42 +1100
Subject: [PATCH] AP_Networking: added TCP reflection test

TCP to an echo server, testing bi-directional transfer
---
 libraries/AP_Networking/AP_Networking.cpp     |  2 +-
 libraries/AP_Networking/AP_Networking.h       |  2 +
 .../AP_Networking/AP_Networking_tests.cpp     | 60 +++++++++++++++++++
 3 files changed, 63 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp
index 63bdb7235a..1d2edb1c98 100644
--- a/libraries/AP_Networking/AP_Networking.cpp
+++ b/libraries/AP_Networking/AP_Networking.cpp
@@ -76,7 +76,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = {
     // @Param: TESTS
     // @DisplayName: Test enable flags
     // @Description: Enable/Disable networking tests
-    // @Bitmask: 0:UDP echo test,1:TCP echo test, 2:TCP discard test
+    // @Bitmask: 0:UDP echo test,1:TCP echo test, 2:TCP discard test, 3:TCP reflect test
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("TESTS", 7,  AP_Networking,    param.tests,   0),
diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h
index eaa51ffc8e..ea94afe8e6 100644
--- a/libraries/AP_Networking/AP_Networking.h
+++ b/libraries/AP_Networking/AP_Networking.h
@@ -283,11 +283,13 @@ private:
         TEST_UDP_CLIENT = (1U<<0),
         TEST_TCP_CLIENT = (1U<<1),
         TEST_TCP_DISCARD = (1U<<2),
+        TEST_TCP_REFLECT = (1U<<3),
     };
     void start_tests(void);
     void test_UDP_client(void);
     void test_TCP_client(void);
     void test_TCP_discard(void);
+    void test_TCP_reflect(void);
 #endif // AP_NETWORKING_TESTS_ENABLED
 
 #if AP_NETWORKING_REGISTER_PORT_ENABLED
diff --git a/libraries/AP_Networking/AP_Networking_tests.cpp b/libraries/AP_Networking/AP_Networking_tests.cpp
index 4747b0ce88..8a87ed15ba 100644
--- a/libraries/AP_Networking/AP_Networking_tests.cpp
+++ b/libraries/AP_Networking/AP_Networking_tests.cpp
@@ -33,6 +33,11 @@ void AP_Networking::start_tests(void)
                                      "TCP_discard",
                                      8192, AP_HAL::Scheduler::PRIORITY_UART, -1);
     }
+    if (param.tests & TEST_TCP_REFLECT) {
+        hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::test_TCP_reflect, void),
+                                     "TCP_reflect",
+                                     8192, AP_HAL::Scheduler::PRIORITY_UART, -1);
+    }
 }
 
 /*
@@ -140,4 +145,59 @@ void AP_Networking::test_TCP_discard(void)
     }
 }
 
+/*
+  start TCP reflect (TCP echo throughput) test
+ */
+void AP_Networking::test_TCP_reflect(void)
+{
+    startup_wait();
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_reflect: starting");
+    const char *dest = param.test_ipaddr.get_str();
+    auto *sock = new SocketAPM(false);
+    if (sock == nullptr) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TCP_reflect: failed to create socket");
+        return;
+    }
+    // connect to the echo service, which is port 7
+    while (!sock->connect(dest, 7)) {
+        hal.scheduler->delay(10);
+    }
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_reflect: connected");
+    const uint32_t bufsize = 4096;
+    uint8_t *buf = (uint8_t*)malloc(bufsize);
+    for (uint32_t i=0; i<bufsize; i++) {
+        buf[i] = i & 0xFF;
+    }
+    uint32_t last_report_ms = AP_HAL::millis();
+    uint32_t total_sent = 0;
+    uint32_t total_recv = 0;
+    uint32_t last_recv = 0;
+    const uint32_t max_disparity = 100*1024;
+    while (true) {
+        if ((param.tests & TEST_TCP_REFLECT) == 0) {
+            hal.scheduler->delay(1);
+            continue;
+        }
+        const bool will_send = total_sent < total_recv + max_disparity;
+        if (will_send) {
+            total_sent += sock->send(buf, bufsize);
+        }
+        if (sock->pollin(0)) {
+            uint32_t n = sock->recv(buf, bufsize, 0);
+            if (n == 0 && !will_send) {
+                hal.scheduler->delay_microseconds(100);
+            }
+            total_recv += n;
+        }
+        const uint32_t now = AP_HAL::millis();
+
+        if (now - last_report_ms >= 1000) {
+            float dt = (now - last_report_ms)*0.001;
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reflect throughput %.3f kbyte/sec (disparity %u)", ((total_recv-last_recv)/dt)*1.0e-3, unsigned(total_sent-total_recv));
+            last_recv = total_recv;
+            last_report_ms = now;
+        }
+    }
+}
+
 #endif // AP_NETWORKING_ENABLED && AP_NETWORKING_TESTS_ENABLED