diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 71d323dc8be768..5c7e8ebca18d62 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -289,12 +289,14 @@ class AP_Networking 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 8a87ed15ba388d..57a3b9f4e57851 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,73 @@ 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]; + memset(buf, 0xab, sizeof(buf)); + 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