From 42bc9adfb78d3922807de6c006be29441b608d8b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Dec 2020 17:44:19 +1100 Subject: [PATCH] HAL_ChibiOS: use ExpandingString for @SYS buffers --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 49 +++++++++--------- libraries/AP_HAL_ChibiOS/CANFDIface.h | 2 +- libraries/AP_HAL_ChibiOS/CANIface.h | 2 +- libraries/AP_HAL_ChibiOS/CanIface.cpp | 48 ++++++++---------- libraries/AP_HAL_ChibiOS/Util.cpp | 50 +++++++------------ libraries/AP_HAL_ChibiOS/Util.h | 4 +- .../AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat | 1 + libraries/AP_HAL_ChibiOS/shared_dma.cpp | 32 +++--------- libraries/AP_HAL_ChibiOS/shared_dma.h | 2 +- 9 files changed, 76 insertions(+), 114 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 29b04bac4e..9b122b62d7 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -46,6 +46,8 @@ #include # include #include +#include + # if defined(STM32H7XX) #include "CANFDIface.h" @@ -929,34 +931,29 @@ bool CANIface::select(bool &read, bool &write, } #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) -uint32_t CANIface::get_stats(char* data, uint32_t max_size) +void CANIface::get_stats(ExpandingString &str) { - if (data == nullptr) { - return 0; - } CriticalSectionLocker lock; - uint32_t ret = snprintf(data, max_size, - "tx_requests: %lu\n" - "tx_rejected: %lu\n" - "tx_success: %lu\n" - "tx_timedout: %lu\n" - "tx_abort: %lu\n" - "rx_received: %lu\n" - "rx_overflow: %lu\n" - "rx_errors: %lu\n" - "num_busoff_err: %lu\n" - "num_events: %lu\n", - stats.tx_requests, - stats.tx_rejected, - stats.tx_success, - stats.tx_timedout, - stats.tx_abort, - stats.rx_received, - stats.rx_overflow, - stats.rx_errors, - stats.num_busoff_err, - stats.num_events); - return ret; + str.printf("tx_requests: %lu\n" + "tx_rejected: %lu\n" + "tx_success: %lu\n" + "tx_timedout: %lu\n" + "tx_abort: %lu\n" + "rx_received: %lu\n" + "rx_overflow: %lu\n" + "rx_errors: %lu\n" + "num_busoff_err: %lu\n" + "num_events: %lu\n", + stats.tx_requests, + stats.tx_rejected, + stats.tx_success, + stats.tx_timedout, + stats.tx_abort, + stats.rx_received, + stats.rx_overflow, + stats.rx_errors, + stats.num_busoff_err, + stats.num_events); } #endif diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 3bad00138e..0edb08530c 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -219,7 +219,7 @@ public: // fetch stats text and return the size of the same, // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt - uint32_t get_stats(char* data, uint32_t max_size) override; + void get_stats(ExpandingString &str) override; #endif /************************************ * Methods used inside interrupt * diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h index 47b5a80ae7..a8bb1256f2 100644 --- a/libraries/AP_HAL_ChibiOS/CANIface.h +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -220,7 +220,7 @@ public: // fetch stats text and return the size of the same, // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt - uint32_t get_stats(char* data, uint32_t max_size) override; + void get_stats(ExpandingString &str) override; #endif /************************************ * Methods used inside interrupt * diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index d2d920cd23..76a579bf5b 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -46,6 +46,7 @@ #include # include #include +#include # if !defined(STM32H7XX) #include "CANIface.h" @@ -902,35 +903,30 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode) } #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) -uint32_t CANIface::get_stats(char* data, uint32_t max_size) +void CANIface::get_stats(ExpandingString &str) { - if (data == nullptr) { - return 0; - } CriticalSectionLocker lock; - uint32_t ret = snprintf(data, max_size, - "tx_requests: %lu\n" - "tx_rejected: %lu\n" - "tx_success: %lu\n" - "tx_timedout: %lu\n" - "tx_abort: %lu\n" - "rx_received: %lu\n" - "rx_overflow: %lu\n" - "rx_errors: %lu\n" - "num_busoff_err: %lu\n" - "num_events: %lu\n", - stats.tx_requests, - stats.tx_rejected, - stats.tx_success, - stats.tx_timedout, - stats.tx_abort, - stats.rx_received, - stats.rx_overflow, - stats.rx_errors, - stats.num_busoff_err, - stats.num_events); + str.printf("tx_requests: %lu\n" + "tx_rejected: %lu\n" + "tx_success: %lu\n" + "tx_timedout: %lu\n" + "tx_abort: %lu\n" + "rx_received: %lu\n" + "rx_overflow: %lu\n" + "rx_errors: %lu\n" + "num_busoff_err: %lu\n" + "num_events: %lu\n", + stats.tx_requests, + stats.tx_rejected, + stats.tx_success, + stats.tx_timedout, + stats.tx_abort, + stats.rx_received, + stats.rx_overflow, + stats.rx_errors, + stats.num_busoff_err, + stats.num_events); memset(&stats, 0, sizeof(stats)); - return ret; } #endif diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 5a87b891f7..a9b713154a 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -24,6 +24,7 @@ #include "hwdef/common/watchdog.h" #include "hwdef/common/flash.h" #include +#include #include "sdcard.h" #include "shared_dma.h" @@ -299,20 +300,14 @@ bool Util::was_watchdog_reset() const /* display stack usage as text buffer for @SYS/threads.txt */ -size_t Util::thread_info(char *buf, size_t bufsize) +void Util::thread_info(ExpandingString &str) { - size_t total = 0; - // a header to allow for machine parsers to determine format const uint32_t isr_stack_size = uint32_t((const uint8_t *)&__main_stack_end__ - (const uint8_t *)&__main_stack_base__); - int n = snprintf(buf, bufsize, "ThreadsV2\nISR PRI=255 sp=%p STACK=%u/%u\n", - &__main_stack_base__, stack_free(&__main_stack_base__), isr_stack_size); - if (n <= 0) { - return 0; - } - buf += n; - bufsize -= n; - total += n; + str.printf("ThreadsV2\nISR PRI=255 sp=%p STACK=%u/%u\n", + &__main_stack_base__, + unsigned(stack_free(&__main_stack_base__)), + unsigned(isr_stack_size)); for (thread_t *tp = chRegFirstThread(); tp; tp = chRegNextThread(tp)) { uint32_t total_stack; @@ -324,35 +319,26 @@ size_t Util::thread_info(char *buf, size_t bufsize) // above the stack top total_stack = uint32_t(tp) - uint32_t(tp->wabase); } - if (bufsize > 0) { #if HAL_ENABLE_THREAD_STATISTICS - n = snprintf(buf, bufsize, "%-13.13s PRI=%3u sp=%p STACK=%4u/%4u MIN=%4u AVG=%4u MAX=%4u\n", - tp->name, unsigned(tp->prio), tp->wabase, - stack_free(tp->wabase), total_stack, RTC2US(STM32_HSECLK, tp->stats.best), - RTC2US(STM32_HSECLK, uint32_t(tp->stats.cumulative / uint64_t(tp->stats.n))), - RTC2US(STM32_HSECLK, tp->stats.worst)); - chTMObjectInit(&tp->stats); // reset counters to zero + str.printf("%-13.13s PRI=%3u sp=%p STACK=%4u/%4u MIN=%4u AVG=%4u MAX=%4u\n", + tp->name, unsigned(tp->prio), tp->wabase, + stack_free(tp->wabase), total_stack, RTC2US(STM32_HSECLK, tp->stats.best), + RTC2US(STM32_HSECLK, uint32_t(tp->stats.cumulative / uint64_t(tp->stats.n))), + RTC2US(STM32_HSECLK, tp->stats.worst)); + chTMObjectInit(&tp->stats); // reset counters to zero #else - n = snprintf(buf, bufsize, "%-13.13s PRI=%3u sp=%p STACK=%u/%u\n", - tp->name, unsigned(tp->prio), tp->wabase, - stack_free(tp->wabase), total_stack); + str.printf("%-13.13s PRI=%3u sp=%p STACK=%u/%u\n", + tp->name, unsigned(tp->prio), tp->wabase, + unsigned(stack_free(tp->wabase)), unsigned(total_stack)); #endif - if (n > bufsize) { - n = bufsize; - } - buf += n; - bufsize -= n; - total += n; - } } - - return total; } #endif // CH_DBG_ENABLE_STACK_CHECK == TRUE #if CH_CFG_USE_SEMAPHORES // request information on dma contention -size_t Util::dma_info(char *buf, size_t bufsize) { - return ChibiOS::Shared_DMA::dma_info(buf, bufsize); +void Util::dma_info(ExpandingString &str) +{ + ChibiOS::Shared_DMA::dma_info(str); } #endif diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 130316302d..36bc95ac57 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -60,11 +60,11 @@ public: #if CH_DBG_ENABLE_STACK_CHECK == TRUE // request information on running threads - size_t thread_info(char *buf, size_t bufsize) override; + void thread_info(ExpandingString &str) override; #endif #if CH_CFG_USE_SEMAPHORES // request information on dma contention - size_t dma_info(char *buf, size_t bufsize) override; + void dma_info(ExpandingString &str) override; #endif private: diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat index 60f1f3701d..620979816e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat @@ -474,3 +474,4 @@ define STORAGE_FLASH_PAGE 1 # is "ROMFS ROMFS-filename source-filename". Paths are relative to the # ardupilot root ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin + diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.cpp b/libraries/AP_HAL_ChibiOS/shared_dma.cpp index a061ca5665..74d9177d3d 100644 --- a/libraries/AP_HAL_ChibiOS/shared_dma.cpp +++ b/libraries/AP_HAL_ChibiOS/shared_dma.cpp @@ -22,6 +22,8 @@ #if CH_CFG_USE_SEMAPHORES == TRUE +#include + using namespace ChibiOS; extern const AP_HAL::HAL& hal; @@ -228,26 +230,16 @@ void Shared_DMA::lock_all(void) } // display dma contention statistics as text buffer for @SYS/dma.txt -size_t Shared_DMA::dma_info(char *buf, size_t bufsize) +void Shared_DMA::dma_info(ExpandingString &str) { - size_t total = 0; - // no buffer allocated, start counting if (_contention_stats == nullptr) { _contention_stats = new dma_stats[SHARED_DMA_MAX_STREAM_ID+1]; - return 0; + return; } // a header to allow for machine parsers to determine format - int n = hal.util->snprintf(buf, bufsize, "DMAV1\n"); - - if (n <= 0) { - return 0; - } - - buf += n; - bufsize -= n; - total += n; + str.printf("DMAV1\n"); for (uint8_t i = 0; i < SHARED_DMA_MAX_STREAM_ID + 1; i++) { if (locks[i].obj == nullptr) { @@ -257,21 +249,11 @@ size_t Shared_DMA::dma_info(char *buf, size_t bufsize) const char* fmt = "DMA=%1u STRM=%1u ULCK=%8u CLCK=%8u CONT=%4.1f%%\n"; float cond_per = 100.0f * float(_contention_stats[i].contended_locks) / (1 + _contention_stats[i].contended_locks + _contention_stats[i].uncontended_locks); - n = hal.util->snprintf(buf, bufsize, fmt, i / 8 + 1, i % 8, - _contention_stats[i].uncontended_locks, _contention_stats[i].contended_locks, cond_per); - - if (n <= 0) { - break; - } - buf += n; - bufsize -= n; - total += n; - + str.printf(fmt, i / 8 + 1, i % 8, + _contention_stats[i].uncontended_locks, _contention_stats[i].contended_locks, cond_per); _contention_stats[i].contended_locks = 0; _contention_stats[i].uncontended_locks = 0; } - - return total; } #endif // CH_CFG_USE_SEMAPHORES diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.h b/libraries/AP_HAL_ChibiOS/shared_dma.h index 4cb236e75e..81b7006e5f 100644 --- a/libraries/AP_HAL_ChibiOS/shared_dma.h +++ b/libraries/AP_HAL_ChibiOS/shared_dma.h @@ -66,7 +66,7 @@ public: static void lock_all(void); // display dma contention statistics as text buffer for @SYS/dma.txt - static size_t dma_info(char *buf, size_t bufsize); + static void dma_info(ExpandingString &str); private: dma_allocate_fn_t allocate;