From c51ee0b83bd8ed3d424f49d7ce34481751c7b525 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 18 Nov 2023 14:09:42 +1100 Subject: [PATCH 1/4] Tools: allow networking in SITL periph --- Tools/autotest/sim_vehicle.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 9540454e5231e..9d66c1e147b0b 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -413,7 +413,7 @@ def do_build(opts, frame_options): if configure_target == 'sitl' and "--enable-networking" not in cmd_configure: cmd_configure.append("--enable-networking") - if opts.disable_networking or configure_target != "sitl": + if opts.disable_networking: cmd_configure.append("--disable-networking") if opts.enable_networking_tests: From ecb060ff2e0a0611230b6fcf612141f41ec1b7e2 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 16 Nov 2023 10:56:02 -0800 Subject: [PATCH 2/4] AP_Networking: add support for AP_Periph --- libraries/AP_Networking/AP_Networking.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 2dffb34dff855..c931fc2698246 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -27,6 +27,7 @@ class AP_Networking friend class AP_Networking_Backend; friend class AP_Networking_ChibiOS; friend class AP_Vehicle; + friend class Networking_Periph; AP_Networking(); From a3fcba4c7461e7051d3e7f0e99c6266246c1c0ce Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 22 Nov 2023 16:10:55 -0800 Subject: [PATCH 3/4] AP_Periph: add Networking-UART passthrough --- Tools/AP_Periph/AP_Periph.cpp | 4 +- Tools/AP_Periph/AP_Periph.h | 3 +- Tools/AP_Periph/Parameters.cpp | 4 +- Tools/AP_Periph/Parameters.h | 2 +- Tools/AP_Periph/networking.cpp | 261 +++++++++++++++++++++++++++++++++ Tools/AP_Periph/networking.h | 53 +++++++ 6 files changed, 321 insertions(+), 6 deletions(-) create mode 100644 Tools/AP_Periph/networking.cpp create mode 100644 Tools/AP_Periph/networking.h diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index b4e707d0b2e65..4d5cfdcbea155 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -101,7 +101,7 @@ void AP_Periph_FW::init() can_start(); #ifdef HAL_PERIPH_ENABLE_NETWORKING - networking.init(); + networking_periph.init(); #endif #if HAL_GCS_ENABLED @@ -502,7 +502,7 @@ void AP_Periph_FW::update() can_update(); #ifdef HAL_PERIPH_ENABLE_NETWORKING - networking.update(); + networking_periph.update(); #endif #if (defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) && HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY == 8) || defined(HAL_PERIPH_ENABLE_NOTIFY) diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 8ed77e88fc1fb..376e7245ba0e0 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -36,6 +36,7 @@ #include #include "rc_in.h" #include "batt_balance.h" +#include "networking.h" #include #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS)) @@ -356,7 +357,7 @@ class AP_Periph_FW { #endif #ifdef HAL_PERIPH_ENABLE_NETWORKING - AP_Networking networking; + Networking_Periph networking_periph; #endif #ifdef HAL_PERIPH_ENABLE_RTC diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 7e17cb8daed18..27c985a3bf621 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -588,8 +588,8 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #ifdef HAL_PERIPH_ENABLE_NETWORKING // @Group: NET_ - // @Path: ../libraries/AP_Networking/AP_Networking.cpp - GOBJECT(networking, "NET_", AP_Networking), + // @Path: networking.cpp + GOBJECT(networking_periph, "NET_", Networking_Periph), #endif #ifdef HAL_PERIPH_ENABLE_RPM diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index a2a439b31fcb9..ff64f16d1e950 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -76,7 +76,7 @@ class Parameters { k_param_esc_number1, k_param_pole_count1, k_param_esc_serial_port1, - k_param_networking, + k_param_networking_periph, k_param_rpm_sensor, k_param_g_rcin, k_param_sitl, diff --git a/Tools/AP_Periph/networking.cpp b/Tools/AP_Periph/networking.cpp new file mode 100644 index 0000000000000..cfce8930632d1 --- /dev/null +++ b/Tools/AP_Periph/networking.cpp @@ -0,0 +1,261 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_NETWORKING + +#include + +extern const AP_HAL::HAL &hal; + +const AP_Param::GroupInfo Networking_Periph::var_info[] { + // @Group: + // @Path: ../../libraries/AP_Networking/AP_Networking.cpp + AP_SUBGROUPINFO(networking_lib, "", 1, Networking_Periph, AP_Networking), + + /* + the NET_Pn_ parameters need to be here as otherwise we + are too deep in the parameter tree + */ + +#if AP_NETWORKING_NUM_PORTS > 0 + // @Group: P1_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[0], "P1_", 2, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 1 + // @Group: P2_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[1], "P2_", 3, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 2 + // @Group: P3_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[2], "P3_", 4, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 3 + // @Group: P4_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[3], "P4_", 5, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 4 + // @Group: P5_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[4], "P5_", 6, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 5 + // @Group: P6_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[5], "P6_", 7, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 6 + // @Group: P7_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[6], "P7_", 8, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 7 + // @Group: P8_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[7], "P8_", 9, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 8 + // @Group: P9_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[8], "P9_", 10, Networking_Periph, AP_Networking::Port), +#endif + + + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + // @Group: PASS1_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[0], "PASS1_", 11, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 1 + // @Group: PASS2_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[1], "PASS2_", 12, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 2 + // @Group: PASS3_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[2], "PASS3_", 13, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 3 + // @Group: PASS4_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[3], "PASS4_", 14, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 4 + // @Group: PASS5_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[4], "PASS5_", 15, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 5 + // @Group: PASS6_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[5], "PASS6_", 16, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 6 + // @Group: PASS7_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[6], "PASS7_", 17, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 7 + // @Group: PASS8_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[7], "PASS8_", 18, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 8 + // @Group: PASS9_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[8], "PASS9_", 19, Networking_Periph, Passthru), +#endif + + AP_GROUPEND +}; + + +const AP_Param::GroupInfo Networking_Periph::Passthru::var_info[] = { + // @Param: ENABLE + // @DisplayName: Enable Passthrough + // @Description: Enable Passthrough of any UART, Network, or CAN ports to any UART, Network, or CAN ports. + // @Values: 0:Disabled, 1:Enabled + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, Networking_Periph::Passthru, enabled, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: EP1 + // @DisplayName: Endpoint 1 + // @Description: Passthrough Endpoint 1. This can be a serial port UART, a Network port, or a CAN port. The selected port will route to Endport 2. + // @Values: -1:Disabled, 0:Serial0(usually USB), 1:Serial1, 2:Serial2, 3:Serial3, 4:Serial4, 5:Serial5, 6:Serial6, 7:Serial7, 8:Serial8, 9:Serial9, 21:Network Port1, 22:Network Port2, 23:Network Port3, 24:Network Port4, 25:Network Port5, 26:Network Port6, 27:Network Port7, 28:Network Port8, 29:Network Port9, 41:CAN1 Port1, 42:CAN1 Port2, 43:CAN1 Port3, 44:CAN1 Port4, 45:CAN1 Port5, 46:CAN1 Port6, 47:CAN1 Port7, 48:CAN1 Port8, 49:CAN1 Port9, 51:CAN2 Port1, 52:CAN2 Port2, 53:CAN2 Port3, 54:CAN2 Port4, 55:CAN2 Port5, 56:CAN2 Port6, 57:CAN2 Port7, 58:CAN2 Port8, 59:CAN2 Port9 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("EP1", 2, Networking_Periph::Passthru, ep1, -1), + + // @Param: EP2 + // @DisplayName: Endpoint 2 + // @Description: Passthrough Endpoint 2. This can be a serial port UART, a Network port, or a CAN port. The selected port will route to Endport 1. + // @CopyFieldsFrom: NET_PASS1_EP1 + AP_GROUPINFO("EP2", 3, Networking_Periph::Passthru, ep2, -1), + + // @Param: BAUD1 + // @DisplayName: Endpoint 1 Baud Rate + // @Description: The baud rate used for Endpoint 1. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_BAUD + AP_GROUPINFO("BAUD1", 4, Networking_Periph::Passthru, baud1, 115200), + + // @Param: BAUD2 + // @DisplayName: Endpoint 2 Baud Rate + // @Description: The baud rate used for Endpoint 2. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_BAUD + AP_GROUPINFO("BAUD2", 5, Networking_Periph::Passthru, baud2, 115200), + + // @Param: OPT1 + // @DisplayName: Serial Port Options EP1 + // @Description: Control over UART options for Endpoint 1. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_OPTIONS + AP_GROUPINFO("OPT1", 6, Networking_Periph::Passthru, options1, 0), + + // @Param: OPT2 + // @DisplayName: Serial Port Options EP2 + // @Description: Control over UART options for Endpoint 2. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_OPTIONS + AP_GROUPINFO("OPT2", 7, Networking_Periph::Passthru, options2, 0), + + AP_GROUPEND +}; + +void Networking_Periph::init(void) +{ + networking_lib.init(); + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + auto &serial_manager = AP::serialmanager(); + + for (auto &p : passthru) { + if (p.enabled != 0 && p.port1 == nullptr && p.port2 == nullptr && + p.ep1 != -1 && p.ep2 != -1 && p.ep1 != p.ep2) { + + p.port1 = serial_manager.get_serial_by_id(p.ep1); + p.port2 = serial_manager.get_serial_by_id(p.ep2); + + if (p.port1 != nullptr && p.port2 != nullptr) { + p.port1->set_options(p.options1); + p.port1->begin(p.baud1); + + p.port2->set_options(p.options2); + p.port2->begin(p.baud2); + } + } + } +#endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU +} + +void Networking_Periph::update(void) +{ + networking_lib.update(); + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + for (auto &p : passthru) { + if (p.enabled == 0 || p.port1 == nullptr || p.port2 == nullptr) { + continue; + } + uint8_t buf[1024]; + + // read from port1, and write to port2 + auto avail = p.port1->available(); + if (avail > 0) { + auto space = p.port2->txspace(); + const uint32_t n = MIN(space, sizeof(buf)); + const auto nbytes = p.port1->read(buf, n); + if (nbytes > 0) { + p.port2->write(buf, nbytes); + } + } + + // read from port2, and write to port1 + avail = p.port2->available(); + if (avail > 0) { + auto space = p.port1->txspace(); + const uint32_t n = MIN(space, sizeof(buf)); + const auto nbytes = p.port2->read(buf, n); + if (nbytes > 0) { + p.port1->write(buf, nbytes); + } + } + } +#endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU +} + +#endif // HAL_PERIPH_ENABLE_NETWORKING + diff --git a/Tools/AP_Periph/networking.h b/Tools/AP_Periph/networking.h new file mode 100644 index 0000000000000..0958977c2cbc6 --- /dev/null +++ b/Tools/AP_Periph/networking.h @@ -0,0 +1,53 @@ +#pragma once + +#ifdef HAL_PERIPH_ENABLE_NETWORKING + +#ifndef HAL_PERIPH_NETWORK_NUM_PASSTHRU +#define HAL_PERIPH_NETWORK_NUM_PASSTHRU 2 +#endif + +class Networking_Periph { +public: + Networking_Periph() { + AP_Param::setup_object_defaults(this, var_info); + } + + static const struct AP_Param::GroupInfo var_info[]; + + void init(); + void update(); + +private: + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + class Passthru { + public: + friend class Networking_Periph; + + CLASS_NO_COPY(Passthru); + + Passthru() { + AP_Param::setup_object_defaults(this, var_info); + } + + static const struct AP_Param::GroupInfo var_info[]; + + private: + AP_Int8 enabled; + AP_Int8 ep1; + AP_Int8 ep2; + AP_Int32 baud1; + AP_Int32 baud2; + AP_Int32 options1; + AP_Int32 options2; + + AP_HAL::UARTDriver *port1; + AP_HAL::UARTDriver *port2; + } passthru[HAL_PERIPH_NETWORK_NUM_PASSTHRU]; +#endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU + + AP_Networking networking_lib; +}; + +#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE + From 1aa88d6f331625c24d18b5a1ca25bd7e209a7929 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 22 Nov 2023 15:14:56 -0800 Subject: [PATCH 4/4] AP_Periph: move Network Passthrough to it's own file --- Tools/AP_Periph/networking.cpp | 102 +----------------- Tools/AP_Periph/networking.h | 5 + Tools/AP_Periph/networking_passthru.cpp | 135 ++++++++++++++++++++++++ 3 files changed, 144 insertions(+), 98 deletions(-) create mode 100644 Tools/AP_Periph/networking_passthru.cpp diff --git a/Tools/AP_Periph/networking.cpp b/Tools/AP_Periph/networking.cpp index cfce8930632d1..8ac256850cd9b 100644 --- a/Tools/AP_Periph/networking.cpp +++ b/Tools/AP_Periph/networking.cpp @@ -17,10 +17,6 @@ #ifdef HAL_PERIPH_ENABLE_NETWORKING -#include - -extern const AP_HAL::HAL &hal; - const AP_Param::GroupInfo Networking_Periph::var_info[] { // @Group: // @Path: ../../libraries/AP_Networking/AP_Networking.cpp @@ -145,80 +141,15 @@ const AP_Param::GroupInfo Networking_Periph::var_info[] { }; -const AP_Param::GroupInfo Networking_Periph::Passthru::var_info[] = { - // @Param: ENABLE - // @DisplayName: Enable Passthrough - // @Description: Enable Passthrough of any UART, Network, or CAN ports to any UART, Network, or CAN ports. - // @Values: 0:Disabled, 1:Enabled - // @RebootRequired: True - // @User: Advanced - AP_GROUPINFO_FLAGS("ENABLE", 1, Networking_Periph::Passthru, enabled, 0, AP_PARAM_FLAG_ENABLE), - - // @Param: EP1 - // @DisplayName: Endpoint 1 - // @Description: Passthrough Endpoint 1. This can be a serial port UART, a Network port, or a CAN port. The selected port will route to Endport 2. - // @Values: -1:Disabled, 0:Serial0(usually USB), 1:Serial1, 2:Serial2, 3:Serial3, 4:Serial4, 5:Serial5, 6:Serial6, 7:Serial7, 8:Serial8, 9:Serial9, 21:Network Port1, 22:Network Port2, 23:Network Port3, 24:Network Port4, 25:Network Port5, 26:Network Port6, 27:Network Port7, 28:Network Port8, 29:Network Port9, 41:CAN1 Port1, 42:CAN1 Port2, 43:CAN1 Port3, 44:CAN1 Port4, 45:CAN1 Port5, 46:CAN1 Port6, 47:CAN1 Port7, 48:CAN1 Port8, 49:CAN1 Port9, 51:CAN2 Port1, 52:CAN2 Port2, 53:CAN2 Port3, 54:CAN2 Port4, 55:CAN2 Port5, 56:CAN2 Port6, 57:CAN2 Port7, 58:CAN2 Port8, 59:CAN2 Port9 - // @RebootRequired: True - // @User: Advanced - AP_GROUPINFO("EP1", 2, Networking_Periph::Passthru, ep1, -1), - - // @Param: EP2 - // @DisplayName: Endpoint 2 - // @Description: Passthrough Endpoint 2. This can be a serial port UART, a Network port, or a CAN port. The selected port will route to Endport 1. - // @CopyFieldsFrom: NET_PASS1_EP1 - AP_GROUPINFO("EP2", 3, Networking_Periph::Passthru, ep2, -1), - - // @Param: BAUD1 - // @DisplayName: Endpoint 1 Baud Rate - // @Description: The baud rate used for Endpoint 1. Only applies to serial ports. - // @CopyFieldsFrom: SERIAL1_BAUD - AP_GROUPINFO("BAUD1", 4, Networking_Periph::Passthru, baud1, 115200), - - // @Param: BAUD2 - // @DisplayName: Endpoint 2 Baud Rate - // @Description: The baud rate used for Endpoint 2. Only applies to serial ports. - // @CopyFieldsFrom: SERIAL1_BAUD - AP_GROUPINFO("BAUD2", 5, Networking_Periph::Passthru, baud2, 115200), - - // @Param: OPT1 - // @DisplayName: Serial Port Options EP1 - // @Description: Control over UART options for Endpoint 1. Only applies to serial ports. - // @CopyFieldsFrom: SERIAL1_OPTIONS - AP_GROUPINFO("OPT1", 6, Networking_Periph::Passthru, options1, 0), - - // @Param: OPT2 - // @DisplayName: Serial Port Options EP2 - // @Description: Control over UART options for Endpoint 2. Only applies to serial ports. - // @CopyFieldsFrom: SERIAL1_OPTIONS - AP_GROUPINFO("OPT2", 7, Networking_Periph::Passthru, options2, 0), - - AP_GROUPEND -}; - void Networking_Periph::init(void) { networking_lib.init(); #if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 - auto &serial_manager = AP::serialmanager(); - for (auto &p : passthru) { - if (p.enabled != 0 && p.port1 == nullptr && p.port2 == nullptr && - p.ep1 != -1 && p.ep2 != -1 && p.ep1 != p.ep2) { - - p.port1 = serial_manager.get_serial_by_id(p.ep1); - p.port2 = serial_manager.get_serial_by_id(p.ep2); - - if (p.port1 != nullptr && p.port2 != nullptr) { - p.port1->set_options(p.options1); - p.port1->begin(p.baud1); - - p.port2->set_options(p.options2); - p.port2->begin(p.baud2); - } - } + p.init(); } -#endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU +#endif } void Networking_Periph::update(void) @@ -227,34 +158,9 @@ void Networking_Periph::update(void) #if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 for (auto &p : passthru) { - if (p.enabled == 0 || p.port1 == nullptr || p.port2 == nullptr) { - continue; - } - uint8_t buf[1024]; - - // read from port1, and write to port2 - auto avail = p.port1->available(); - if (avail > 0) { - auto space = p.port2->txspace(); - const uint32_t n = MIN(space, sizeof(buf)); - const auto nbytes = p.port1->read(buf, n); - if (nbytes > 0) { - p.port2->write(buf, nbytes); - } - } - - // read from port2, and write to port1 - avail = p.port2->available(); - if (avail > 0) { - auto space = p.port1->txspace(); - const uint32_t n = MIN(space, sizeof(buf)); - const auto nbytes = p.port2->read(buf, n); - if (nbytes > 0) { - p.port1->write(buf, nbytes); - } - } + p.update(); } -#endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU +#endif } #endif // HAL_PERIPH_ENABLE_NETWORKING diff --git a/Tools/AP_Periph/networking.h b/Tools/AP_Periph/networking.h index 0958977c2cbc6..d25dc5aa7aaa7 100644 --- a/Tools/AP_Periph/networking.h +++ b/Tools/AP_Periph/networking.h @@ -1,5 +1,7 @@ #pragma once +#include "AP_Periph.h" + #ifdef HAL_PERIPH_ENABLE_NETWORKING #ifndef HAL_PERIPH_NETWORK_NUM_PASSTHRU @@ -30,6 +32,9 @@ class Networking_Periph { AP_Param::setup_object_defaults(this, var_info); } + void init(); + void update(); + static const struct AP_Param::GroupInfo var_info[]; private: diff --git a/Tools/AP_Periph/networking_passthru.cpp b/Tools/AP_Periph/networking_passthru.cpp new file mode 100644 index 0000000000000..18ed58bf68bac --- /dev/null +++ b/Tools/AP_Periph/networking_passthru.cpp @@ -0,0 +1,135 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "networking.h" + +#if defined(HAL_PERIPH_ENABLE_NETWORKING) && HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + +#include + +const AP_Param::GroupInfo Networking_Periph::Passthru::var_info[] = { + // @Param: ENABLE + // @DisplayName: Enable Passthrough + // @Description: Enable Passthrough of any UART, Network, or CAN ports to any UART, Network, or CAN ports. + // @Values: 0:Disabled, 1:Enabled + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, Networking_Periph::Passthru, enabled, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: EP1 + // @DisplayName: Endpoint 1 + // @Description: Passthrough Endpoint 1. This can be a serial port UART, a Network port, or a CAN port. The selected port will route to Endport 2. + // @Values: -1:Disabled, 0:Serial0(usually USB), 1:Serial1, 2:Serial2, 3:Serial3, 4:Serial4, 5:Serial5, 6:Serial6, 7:Serial7, 8:Serial8, 9:Serial9, 21:Network Port1, 22:Network Port2, 23:Network Port3, 24:Network Port4, 25:Network Port5, 26:Network Port6, 27:Network Port7, 28:Network Port8, 29:Network Port9, 41:CAN1 Port1, 42:CAN1 Port2, 43:CAN1 Port3, 44:CAN1 Port4, 45:CAN1 Port5, 46:CAN1 Port6, 47:CAN1 Port7, 48:CAN1 Port8, 49:CAN1 Port9, 51:CAN2 Port1, 52:CAN2 Port2, 53:CAN2 Port3, 54:CAN2 Port4, 55:CAN2 Port5, 56:CAN2 Port6, 57:CAN2 Port7, 58:CAN2 Port8, 59:CAN2 Port9 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("EP1", 2, Networking_Periph::Passthru, ep1, -1), + + // @Param: EP2 + // @DisplayName: Endpoint 2 + // @Description: Passthrough Endpoint 2. This can be a serial port UART, a Network port, or a CAN port. The selected port will route to Endport 1. + // @CopyFieldsFrom: NET_PASS1_EP1 + AP_GROUPINFO("EP2", 3, Networking_Periph::Passthru, ep2, -1), + + // @Param: BAUD1 + // @DisplayName: Endpoint 1 Baud Rate + // @Description: The baud rate used for Endpoint 1. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_BAUD + AP_GROUPINFO("BAUD1", 4, Networking_Periph::Passthru, baud1, 115200), + + // @Param: BAUD2 + // @DisplayName: Endpoint 2 Baud Rate + // @Description: The baud rate used for Endpoint 2. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_BAUD + AP_GROUPINFO("BAUD2", 5, Networking_Periph::Passthru, baud2, 115200), + + // @Param: OPT1 + // @DisplayName: Serial Port Options EP1 + // @Description: Control over UART options for Endpoint 1. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_OPTIONS + AP_GROUPINFO("OPT1", 6, Networking_Periph::Passthru, options1, 0), + + // @Param: OPT2 + // @DisplayName: Serial Port Options EP2 + // @Description: Control over UART options for Endpoint 2. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_OPTIONS + AP_GROUPINFO("OPT2", 7, Networking_Periph::Passthru, options2, 0), + + AP_GROUPEND +}; + +void Networking_Periph::Passthru::init() +{ + if (enabled == 0) { + // Feature is disabled + return; + } + + if (port1 != nullptr || port2 != nullptr) { + // The ports have already been initialized, nothing to do. + return; + } + + if (ep1 <= -1 || ep2 <= -1 || ep1 == ep2) { + // end points are not set or are the same. Can't route to self + return; + } + + port1 = AP::serialmanager().get_serial_by_id(ep1); + port2 = AP::serialmanager().get_serial_by_id(ep2); + + if (port1 != nullptr && port2 != nullptr) { + port1->set_options(options1); + port1->begin(baud1); + + port2->set_options(options2); + port2->begin(baud2); + } +} + +void Networking_Periph::Passthru::update() +{ + if (enabled == 0 || port1 == nullptr || port2 == nullptr) { + return; + } + + // Fastest possible connection is 3Mbps serial port, which is roughly 300KB/s payload and we service this at <= 1kHz + // Raising this any higher just causes excess stack usage which never gets used. + uint8_t buf[300]; + + // read from port1, and write to port2 + auto avail = port1->available(); + if (avail > 0) { + auto space = port2->txspace(); + const uint32_t n = MIN(space, sizeof(buf)); + const auto nbytes = port1->read(buf, n); + if (nbytes > 0) { + port2->write(buf, nbytes); + } + } + + // read from port2, and write to port1 + avail = port2->available(); + if (avail > 0) { + auto space = port1->txspace(); + const uint32_t n = MIN(space, sizeof(buf)); + const auto nbytes = port2->read(buf, n); + if (nbytes > 0) { + port1->write(buf, nbytes); + } + } +} + +#endif // defined(HAL_PERIPH_ENABLE_NETWORKING) && HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 +