From 1d9d599bb051958e93abfe9930e0768e6db87e90 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Dec 2023 06:27:13 +1100 Subject: [PATCH] AP_Networking: allow for PPP on ChibiOS --- .../AP_Networking/AP_Networking_Config.h | 11 +++-- libraries/AP_Networking/AP_Networking_PPP.cpp | 41 +++++++++++++++---- .../AP_Networking/AP_Networking_SLIP.cpp | 1 - .../AP_Networking/AP_Networking_address.cpp | 4 +- 4 files changed, 41 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h index d356770955..dcecd2be3a 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -21,22 +21,25 @@ // Backends // --------------------------- #ifndef AP_NETWORKING_BACKEND_CHIBIOS -#define AP_NETWORKING_BACKEND_CHIBIOS (AP_NETWORKING_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS)) +#ifndef HAL_USE_MAC +#define HAL_USE_MAC 0 +#endif +#define AP_NETWORKING_BACKEND_CHIBIOS (AP_NETWORKING_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS) && HAL_USE_MAC) #endif #ifndef AP_NETWORKING_BACKEND_PPP -#define AP_NETWORKING_BACKEND_PPP (AP_NETWORKING_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS)) +#define AP_NETWORKING_BACKEND_PPP (AP_NETWORKING_BACKEND_DEFAULT_ENABLED) #endif #ifndef AP_NETWORKING_BACKEND_SLIP -#define AP_NETWORKING_BACKEND_SLIP (AP_NETWORKING_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS)) +#define AP_NETWORKING_BACKEND_SLIP 0 #endif #ifndef AP_NETWORKING_BACKEND_SITL #define AP_NETWORKING_BACKEND_SITL (AP_NETWORKING_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)) #endif -#define AP_NETWORKING_SOCKETS_ENABLED (HAL_OS_SOCKETS || AP_NETWORKING_BACKEND_CHIBIOS) +#define AP_NETWORKING_SOCKETS_ENABLED AP_NETWORKING_ENABLED // --------------------------- // IP Features diff --git a/libraries/AP_Networking/AP_Networking_PPP.cpp b/libraries/AP_Networking/AP_Networking_PPP.cpp index ef4128871f..8e296a1abb 100644 --- a/libraries/AP_Networking/AP_Networking_PPP.cpp +++ b/libraries/AP_Networking/AP_Networking_PPP.cpp @@ -6,7 +6,6 @@ #include "AP_Networking_PPP.h" #include -#include #include #include #include @@ -17,6 +16,7 @@ extern const AP_HAL::HAL& hal; +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS /* uint32_t timestamp in smallest available units */ @@ -24,6 +24,15 @@ uint32_t sys_jiffies(void) { return AP_HAL::micros(); } +#endif + +#if LWIP_TCPIP_CORE_LOCKING +#define LWIP_TCPIP_LOCK() sys_lock_tcpip_core() +#define LWIP_TCPIP_UNLOCK() sys_unlock_tcpip_core() +#else +#define LWIP_TCPIP_LOCK() +#define LWIP_TCPIP_UNLOCK() +#endif /* output some data to the uart @@ -39,8 +48,6 @@ uint32_t AP_Networking_PPP::ppp_output_cb(ppp_pcb *pcb, const void *data, uint32 if (n > 0) { remaining -= n; ptr += n; - } else { - hal.scheduler->delay_microseconds(100); } } return len; @@ -137,24 +144,26 @@ bool AP_Networking_PPP::init() } // initialise TCP/IP thread + LWIP_TCPIP_LOCK(); tcpip_init(NULL, NULL); + LWIP_TCPIP_UNLOCK(); + hal.scheduler->delay(100); // create ppp connection + LWIP_TCPIP_LOCK(); + ppp = pppos_create(pppif, ppp_output_cb, ppp_status_callback, this); if (ppp == nullptr) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: failed to create link"); return false; } - - // connect and set as default route - ppp_connect(ppp, 0); - netif_set_default(pppif); + LWIP_TCPIP_UNLOCK(); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: started"); hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking_PPP::ppp_loop, void), "ppp", - 8192, AP_HAL::Scheduler::PRIORITY_UART, 0); + 2048, AP_HAL::Scheduler::PRIORITY_UART, 0); return true; } @@ -164,6 +173,17 @@ bool AP_Networking_PPP::init() */ void AP_Networking_PPP::ppp_loop(void) { + while (!hal.scheduler->is_system_initialized()) { + hal.scheduler->delay_microseconds(1000); + } + + // connect and set as default route + LWIP_TCPIP_LOCK(); + ppp_connect(ppp, 0); + + netif_set_default(pppif); + LWIP_TCPIP_UNLOCK(); + // ensure this thread owns the uart uart->begin(0); @@ -171,9 +191,12 @@ void AP_Networking_PPP::ppp_loop(void) uint8_t buf[1024]; auto n = uart->read(buf, sizeof(buf)); if (n > 0) { + LWIP_TCPIP_LOCK(); pppos_input(ppp, buf, n); - } else { + LWIP_TCPIP_UNLOCK(); hal.scheduler->delay_microseconds(100); + } else { + hal.scheduler->delay_microseconds(1000); } } } diff --git a/libraries/AP_Networking/AP_Networking_SLIP.cpp b/libraries/AP_Networking/AP_Networking_SLIP.cpp index 088bd024c8..b1e5085b5d 100644 --- a/libraries/AP_Networking/AP_Networking_SLIP.cpp +++ b/libraries/AP_Networking/AP_Networking_SLIP.cpp @@ -6,7 +6,6 @@ #include "AP_Networking_SLIP.h" #include -#include #include #include #include diff --git a/libraries/AP_Networking/AP_Networking_address.cpp b/libraries/AP_Networking/AP_Networking_address.cpp index 28900e387b..a1c1cdc59c 100644 --- a/libraries/AP_Networking/AP_Networking_address.cpp +++ b/libraries/AP_Networking/AP_Networking_address.cpp @@ -8,6 +8,7 @@ #include #include "AP_Networking.h" +#include const AP_Param::GroupInfo AP_Networking_IPV4::var_info[] = { // @Param: 0 @@ -72,8 +73,7 @@ void AP_Networking_IPV4::set_default_uint32(uint32_t v) const char* AP_Networking_IPV4::get_str() { const auto ip = ntohl(get_uint32()); - inet_ntop(AF_INET, &ip, strbuf, sizeof(strbuf)); - return strbuf; + return SocketAPM::inet_addr_to_str(&ip, strbuf, sizeof(strbuf)); } #endif // AP_NETWORKING_ENABLED