From 0c2041d4155055c3e8993b244f84937010fafe61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Apr 2022 09:54:20 +1000 Subject: [PATCH] HAL_ChibiOS: constrain more timer timeouts --- libraries/AP_HAL_ChibiOS/EventSource.cpp | 3 ++- libraries/AP_HAL_ChibiOS/GPIO.cpp | 11 +++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/EventSource.cpp b/libraries/AP_HAL_ChibiOS/EventSource.cpp index dfb21919b6..4977a46be1 100644 --- a/libraries/AP_HAL_ChibiOS/EventSource.cpp +++ b/libraries/AP_HAL_ChibiOS/EventSource.cpp @@ -1,4 +1,5 @@ #include "EventSource.h" +#include using namespace ChibiOS; @@ -13,7 +14,7 @@ bool EventSource::wait(uint64_t duration, AP_HAL::EventHandle *evt_handle) if (duration == 0) { ret = chEvtWaitAnyTimeout(evt_mask, TIME_IMMEDIATE); } else { - ret = chEvtWaitAnyTimeout(evt_mask, chTimeUS2I(duration)); + ret = chEvtWaitAnyTimeout(evt_mask, MAX(CH_CFG_ST_TIMEDELTA, chTimeUS2I(duration))); } ch_evt_src_.unregister(&evt_listener); return ret == MSG_OK; diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index 1bb06ab9a0..e9df940765 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -28,6 +28,7 @@ #include #endif #include +#include using namespace ChibiOS; @@ -462,8 +463,7 @@ static void pal_interrupt_wait(void *arg) } /* - block waiting for a pin to change. A timeout of 0 means wait - forever. Return true on pin change, false on timeout + block waiting for a pin to change. Return true on pin change, false on timeout */ bool GPIO::wait_pin(uint8_t pin, INTERRUPT_TRIGGER_TYPE mode, uint32_t timeout_us) { @@ -486,8 +486,11 @@ bool GPIO::wait_pin(uint8_t pin, INTERRUPT_TRIGGER_TYPE mode, uint32_t timeout_u osalSysUnlock(); return false; } - - msg_t msg = osalThreadSuspendTimeoutS(&g->thd_wait, TIME_US2I(timeout_us)); + + // don't allow for very long timeouts, or below the delta + timeout_us = constrain_uint32(TIME_US2I(timeout_us), CH_CFG_ST_TIMEDELTA, TIME_US2I(30000U)); + + msg_t msg = osalThreadSuspendTimeoutS(&g->thd_wait, timeout_us); _attach_interruptI(g->pal_line, palcallback_t(nullptr), nullptr,