mirror of https://github.com/ArduPilot/ardupilot
HAL_ChibiOS: improved stack checking
This commit is contained in:
parent
2d459dccc3
commit
84eac7642b
|
@ -473,6 +473,9 @@ void Scheduler::_io_thread(void* arg)
|
||||||
}
|
}
|
||||||
#ifndef HAL_NO_LOGGING
|
#ifndef HAL_NO_LOGGING
|
||||||
uint32_t last_sd_start_ms = AP_HAL::millis();
|
uint32_t last_sd_start_ms = AP_HAL::millis();
|
||||||
|
#endif
|
||||||
|
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
|
||||||
|
uint32_t last_stack_check_ms = 0;
|
||||||
#endif
|
#endif
|
||||||
while (true) {
|
while (true) {
|
||||||
sched->delay_microseconds(1000);
|
sched->delay_microseconds(1000);
|
||||||
|
@ -480,16 +483,25 @@ void Scheduler::_io_thread(void* arg)
|
||||||
// run registered IO processes
|
// run registered IO processes
|
||||||
sched->_run_io();
|
sched->_run_io();
|
||||||
|
|
||||||
|
#if !defined(HAL_NO_LOGGING) || CH_DBG_ENABLE_STACK_CHECK == TRUE
|
||||||
|
uint32_t now = AP_HAL::millis();
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef HAL_NO_LOGGING
|
#ifndef HAL_NO_LOGGING
|
||||||
if (!hal.util->get_soft_armed()) {
|
if (!hal.util->get_soft_armed()) {
|
||||||
// if sdcard hasn't mounted then retry it every 3s in the IO
|
// if sdcard hasn't mounted then retry it every 3s in the IO
|
||||||
// thread when disarmed
|
// thread when disarmed
|
||||||
uint32_t now = AP_HAL::millis();
|
|
||||||
if (now - last_sd_start_ms > 3000) {
|
if (now - last_sd_start_ms > 3000) {
|
||||||
last_sd_start_ms = now;
|
last_sd_start_ms = now;
|
||||||
AP::FS().retry_mount();
|
AP::FS().retry_mount();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
|
||||||
|
if (now - last_stack_check_ms > 1000) {
|
||||||
|
last_stack_check_ms = now;
|
||||||
|
check_stack_free();
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -693,4 +705,28 @@ void Scheduler::watchdog_pat(void)
|
||||||
last_watchdog_pat_ms = AP_HAL::millis();
|
last_watchdog_pat_ms = AP_HAL::millis();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
|
||||||
|
/*
|
||||||
|
check we have enough stack free on all threads and the ISR stack
|
||||||
|
*/
|
||||||
|
void Scheduler::check_stack_free(void)
|
||||||
|
{
|
||||||
|
const uint32_t min_stack = 32;
|
||||||
|
bool ok = false;
|
||||||
|
|
||||||
|
if (stack_free(__main_stack_base__) < min_stack) {
|
||||||
|
AP::internalerror().error(error_number, 0xFFFF);
|
||||||
|
}
|
||||||
|
|
||||||
|
thread_t *tp = chRegFirstThread();
|
||||||
|
do {
|
||||||
|
if (stack_free(tp->wabase) < min_stack) {
|
||||||
|
AP::internalerror().error(error_number, tp->prio);
|
||||||
|
}
|
||||||
|
tp = chRegNextThread(tp);
|
||||||
|
} while (tp != NULL);
|
||||||
|
}
|
||||||
|
#endif // CH_DBG_ENABLE_STACK_CHECK == TRUE
|
||||||
|
|
||||||
|
|
||||||
#endif // CH_CFG_USE_DYNAMIC
|
#endif // CH_CFG_USE_DYNAMIC
|
||||||
|
|
|
@ -176,5 +176,8 @@ private:
|
||||||
#if defined STM32H7
|
#if defined STM32H7
|
||||||
void check_low_memory_is_zero();
|
void check_low_memory_is_zero();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// check for free stack space
|
||||||
|
void check_stack_free(void);
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -304,7 +304,8 @@ size_t Util::thread_info(char *buf, size_t bufsize)
|
||||||
size_t total = 0;
|
size_t total = 0;
|
||||||
|
|
||||||
// a header to allow for machine parsers to determine format
|
// a header to allow for machine parsers to determine format
|
||||||
int n = snprintf(buf, bufsize, "ThreadsV1\n");
|
int n = snprintf(buf, bufsize, "ThreadsV2\nISR PRI=255 sp=%p STACK_FREE=%u/%u\n",
|
||||||
|
__main_stack_base__, stack_free(__main_stack_base__), __main_stack_size__);
|
||||||
if (n <= 0) {
|
if (n <= 0) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -315,13 +316,10 @@ size_t Util::thread_info(char *buf, size_t bufsize)
|
||||||
tp = chRegFirstThread();
|
tp = chRegFirstThread();
|
||||||
|
|
||||||
do {
|
do {
|
||||||
uint32_t stklimit = (uint32_t)tp->wabase;
|
const uint32_t total_stack = uint32_t(tp) - uint32_t(tp->wabase);
|
||||||
uint8_t *p = (uint8_t *)tp->wabase;
|
n = snprintf(buf, bufsize, "%-13.13s PRI=%3u sp=%p STACK_LEFT=%u/%u\n",
|
||||||
while (*p == CH_DBG_STACK_FILL_VALUE) {
|
tp->name, unsigned(tp->prio), tp->wabase,
|
||||||
p++;
|
stack_free(tp->wabase), total_stack);
|
||||||
}
|
|
||||||
uint32_t stack_left = ((uint32_t)p) - stklimit;
|
|
||||||
n = snprintf(buf, bufsize, "%-13.13s PRI=%3u STACK_LEFT=%u\n", tp->name, unsigned(tp->prio), unsigned(stack_left));
|
|
||||||
if (n <= 0) {
|
if (n <= 0) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -449,3 +449,19 @@ void stack_overflow(thread_t *tp)
|
||||||
(void)tp;
|
(void)tp;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
|
||||||
|
/*
|
||||||
|
check how much stack is free given a stack base. Assumes the fill
|
||||||
|
byte is 0x55
|
||||||
|
*/
|
||||||
|
uint32_t stack_free(void *stack_base)
|
||||||
|
{
|
||||||
|
const uint32_t *p = (uint32_t *)stack_base;
|
||||||
|
const uint32_t canary_word = 0x55555555;
|
||||||
|
while (*p == canary_word) {
|
||||||
|
p++;
|
||||||
|
}
|
||||||
|
return uint32_t(p) - uint32_t(stack_base);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
|
@ -110,6 +110,16 @@ void system_halt_hook(void);
|
||||||
// hook for stack overflow
|
// hook for stack overflow
|
||||||
void stack_overflow(thread_t *tp);
|
void stack_overflow(thread_t *tp);
|
||||||
|
|
||||||
|
/*
|
||||||
|
check how much stack is free given a stack base. Assumes the fill
|
||||||
|
byte is 0x55
|
||||||
|
*/
|
||||||
|
uint32_t stack_free(void *stack_base);
|
||||||
|
|
||||||
|
// allow stack view code to show free ISR stack
|
||||||
|
extern void *__main_stack_base__;
|
||||||
|
extern uint32_t __main_stack_size__;
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue