From 5acf25973eda67d37769abfdc4fc0df7bd4c9cbe Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 11 Sep 2024 11:38:14 +1000
Subject: [PATCH] AP_Networking: add connector loopback test for Ethernet

---
 libraries/AP_Networking/AP_Networking.h       |  2 +
 .../AP_Networking/AP_Networking_tests.cpp     | 73 +++++++++++++++++++
 2 files changed, 75 insertions(+)

diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h
index 71d323dc8b..5c7e8ebca1 100644
--- a/libraries/AP_Networking/AP_Networking.h
+++ b/libraries/AP_Networking/AP_Networking.h
@@ -289,12 +289,14 @@ private:
         TEST_TCP_CLIENT = (1U<<1),
         TEST_TCP_DISCARD = (1U<<2),
         TEST_TCP_REFLECT = (1U<<3),
+        TEST_CONNECTOR_LOOPBACK = (1U<<4),
     };
     void start_tests(void);
     void test_UDP_client(void);
     void test_TCP_client(void);
     void test_TCP_discard(void);
     void test_TCP_reflect(void);
+    void test_connector_loopback(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 8a87ed15ba..48af632282 100644
--- a/libraries/AP_Networking/AP_Networking_tests.cpp
+++ b/libraries/AP_Networking/AP_Networking_tests.cpp
@@ -38,6 +38,11 @@ void AP_Networking::start_tests(void)
                                      "TCP_reflect",
                                      8192, AP_HAL::Scheduler::PRIORITY_UART, -1);
     }
+    if (param.tests & TEST_CONNECTOR_LOOPBACK) {
+        hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::test_connector_loopback, void),
+                                     "connector_loopback",
+                                     8192, AP_HAL::Scheduler::PRIORITY_UART, -1);
+    }
 }
 
 /*
@@ -200,4 +205,72 @@ void AP_Networking::test_TCP_reflect(void)
     }
 }
 
+void AP_Networking::test_connector_loopback(void)
+{
+    startup_wait();
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Connector Loopback: starting");
+
+    // start tcp discard server
+    auto *listen_sock = new SocketAPM(false);
+    if (listen_sock == nullptr) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to create socket");
+        return;
+    }
+    listen_sock->reuseaddress();
+
+    // use netif ip address
+    char ipstr[16];
+    SocketAPM::inet_addr_to_str(get_ip_active(), ipstr, sizeof(ipstr));
+    if (!listen_sock->bind(ipstr, 9)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to bind");
+        return;
+    }
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "connector_loopback: bound");
+
+    if (!listen_sock->listen(1)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to listen");
+        return;
+    }
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "connector_loopback: listening");
+    // create discard client
+    auto *client = new SocketAPM(false);
+    if (client == nullptr) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to create client");
+        return;
+    }
+    while (!client->connect(ipstr, 9)) {
+        hal.scheduler->delay(10);
+    }
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "connector_loopback: connected");
+
+    // accept client
+    auto *sock = listen_sock->accept(100);
+    if (sock == nullptr) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to accept");
+        return;
+    }
+
+    uint8_t buf[1024];
+    uint32_t last_report_ms = AP_HAL::millis();
+    uint32_t total_rx = 0;
+    while (true) {
+        if ((param.tests & TEST_CONNECTOR_LOOPBACK) == 0) {
+            hal.scheduler->delay(1);
+            continue;
+        }
+        client->send(buf, 1024);
+        if (sock->pollin(0)) {
+            const ssize_t ret = sock->recv(buf, sizeof(buf), 0);
+            if (ret > 0) {
+                total_rx += ret;
+            }
+        }
+        if (AP_HAL::millis() - last_report_ms >= 1000) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "connector_loopback throughput %lu kbytes/sec", total_rx/1024);
+            total_rx = 0;
+            last_report_ms = AP_HAL::millis();
+        }
+    }
+}
+
 #endif // AP_NETWORKING_ENABLED && AP_NETWORKING_TESTS_ENABLED