Skip to content

Commit

Permalink
HAL_ChibiOS: replace volatile bools with mutexes
Browse files Browse the repository at this point in the history
this replaces the two booleans used to mediate TX and RX buffer
protection with mutexes.

The booleans were a hangover from the very early HAL_ChibiOS code, and
can lead to a deadlock. The sequence is as follows:

 - a very high CAN bus bandwidth usage, triggered by MissionPlanner
   requesting CAN_FORWARD on a CAN serial port. That causes a
   "infinite" number of CAN_FRAME messages which saturates the bus,
   and leads to the DroneCAN thread looping with no pause

 - a serial port configured as GPS type AUTO, auto-probing for a GPS
   that isn't there. This calls begin() periodically

 - the UART TX thread assocated with that UART not making progress as
   the TX thread priority is below the DroneCAN thread priority

 - this causes the begin() in main thread waiting for _in_tx_timer to
   loop forever, which triggers a watchdog
  • Loading branch information
tridge committed Sep 27, 2024
1 parent eb4224b commit a605b36
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 22 deletions.
30 changes: 10 additions & 20 deletions libraries/AP_HAL_ChibiOS/UARTDriver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -295,9 +295,7 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
thrashing of the heap once we are up. The ttyACM0 driver may not
connect for some time after boot
*/
while (_in_rx_timer) {
hal.scheduler->delay(1);
}
WITH_SEMAPHORE(rx_sem);
if (rxS != _readbuf.get_size()) {
_rx_initialised = false;
_readbuf.set_size_best(rxS);
Expand Down Expand Up @@ -359,9 +357,7 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
/*
allocate the write buffer
*/
while (_in_tx_timer) {
hal.scheduler->delay(1);
}
WITH_SEMAPHORE(tx_sem);
if (txS != _writebuf.get_size()) {
_tx_initialised = false;
_writebuf.set_size_best(txS);
Expand Down Expand Up @@ -640,9 +636,9 @@ __RAMFUNC__ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags)

void UARTDriver::_end()
{
while (_in_rx_timer) hal.scheduler->delay(1);
WITH_SEMAPHORE(rx_sem);
WITH_SEMAPHORE(tx_sem);
_rx_initialised = false;
while (_in_tx_timer) hal.scheduler->delay(1);
_tx_initialised = false;

if (sdef.is_usb) {
Expand Down Expand Up @@ -1112,7 +1108,7 @@ void UARTDriver::_rx_timer_tick(void)
return;
}

_in_rx_timer = true;
WITH_SEMAPHORE(rx_sem);

#if HAL_UART_STATS_ENABLED && CH_CFG_USE_EVENTS == TRUE
if (!sdef.is_usb) {
Expand Down Expand Up @@ -1165,7 +1161,6 @@ void UARTDriver::_rx_timer_tick(void)
if (sdef.is_usb) {
#ifdef HAVE_USB_SERIAL
if (((SerialUSBDriver*)sdef.serial)->config->usbp->state != USB_ACTIVE) {
_in_rx_timer = false;
return;
}
#endif
Expand All @@ -1189,7 +1184,6 @@ void UARTDriver::_rx_timer_tick(void)
fwd_otg2_serial();
}
#endif
_in_rx_timer = false;
}

// forward data from a serial port to the USB
Expand Down Expand Up @@ -1308,14 +1302,13 @@ void UARTDriver::_tx_timer_tick(void)
return;
}

_in_tx_timer = true;

if (hd_tx_active) {
WITH_SEMAPHORE(tx_sem);
hd_tx_active &= ~chEvtGetAndClearFlags(&hd_listener);
if (!hd_tx_active) {
/*
half-duplex transmit has finished. We now re-enable the
HDSEL bit for receive
half-duplex transmit has finished. We now re-enable the
HDSEL bit for receive
*/
SerialDriver *sd = (SerialDriver*)(sdef.serial);
sdStop(sd);
Expand All @@ -1328,7 +1321,6 @@ void UARTDriver::_tx_timer_tick(void)
if (sdef.is_usb) {
#ifdef HAVE_USB_SERIAL
if (((SerialUSBDriver*)sdef.serial)->config->usbp->state != USB_ACTIVE) {
_in_tx_timer = false;
return;
}
#endif
Expand All @@ -1341,18 +1333,16 @@ void UARTDriver::_tx_timer_tick(void)

// half duplex we do reads in the write thread
if (half_duplex) {
_in_rx_timer = true;
WITH_SEMAPHORE(rx_sem);
read_bytes_NODMA();
if (_wait.thread_ctx && _readbuf.available() >= _wait.n) {
chEvtSignal(_wait.thread_ctx, EVT_DATA);
}
_in_rx_timer = false;
}

// now do the write
WITH_SEMAPHORE(tx_sem);
write_pending_bytes();

_in_tx_timer = false;
}

/*
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_HAL_ChibiOS/UARTDriver.h
Original file line number Diff line number Diff line change
Expand Up @@ -196,8 +196,8 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver {
const stm32_dma_stream_t* rxdma;
const stm32_dma_stream_t* txdma;
#endif
volatile bool _in_rx_timer;
volatile bool _in_tx_timer;
HAL_Semaphore tx_sem;
HAL_Semaphore rx_sem;
volatile bool _rx_initialised;
volatile bool _tx_initialised;
volatile bool _device_initialised;
Expand Down

0 comments on commit a605b36

Please sign in to comment.