diff --git a/libraries/AP_HAL_ChibiOS/Device.cpp b/libraries/AP_HAL_ChibiOS/Device.cpp index 4d35af5183..e14f1efb9e 100644 --- a/libraries/AP_HAL_ChibiOS/Device.cpp +++ b/libraries/AP_HAL_ChibiOS/Device.cpp @@ -164,15 +164,23 @@ bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_u /* setup to use DMA-safe bouncebuffers for device transfers */ -void DeviceBus::bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len, +bool DeviceBus::bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len, uint8_t *&buf_rx, uint16_t rx_len) { if (buf_rx) { - bouncebuffer_setup_read(bounce_buffer_rx, &buf_rx, rx_len); + if (!bouncebuffer_setup_read(bounce_buffer_rx, &buf_rx, rx_len)) { + return false; + } } if (buf_tx) { - bouncebuffer_setup_write(bounce_buffer_tx, &buf_tx, tx_len); + if (!bouncebuffer_setup_write(bounce_buffer_tx, &buf_tx, tx_len)) { + if (buf_rx) { + bouncebuffer_abort(bounce_buffer_rx); + } + return false; + } } + return true; } /* diff --git a/libraries/AP_HAL_ChibiOS/Device.h b/libraries/AP_HAL_ChibiOS/Device.h index febd7ff663..c1b62f7a71 100644 --- a/libraries/AP_HAL_ChibiOS/Device.h +++ b/libraries/AP_HAL_ChibiOS/Device.h @@ -39,8 +39,8 @@ public: bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec); static void bus_thread(void *arg); - void bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len, - uint8_t *&buf_rx, uint16_t rx_len); + bool bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len, + uint8_t *&buf_rx, uint16_t rx_len) WARN_IF_UNUSED; void bouncebuffer_finish(const uint8_t *buf_tx, uint8_t *buf_rx, uint16_t rx_len); private: diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp index a838cc4125..5b3c447dc0 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp @@ -258,7 +258,10 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len, { i2cAcquireBus(I2CD[bus.busnum].i2c); - bus.bouncebuffer_setup(send, send_len, recv, recv_len); + if (!bus.bouncebuffer_setup(send, send_len, recv, recv_len)) { + i2cReleaseBus(I2CD[bus.busnum].i2c); + return false; + } for(uint8_t i=0 ; i <= _retries; i++) { int ret; diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp index b71f63b9fe..338d0ccdfb 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -185,7 +185,10 @@ bool SPIDevice::do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len) } } #else - bus.bouncebuffer_setup(send, len, recv, len); + if (!bus.bouncebuffer_setup(send, len, recv, len)) { + set_chip_select(old_cs_forced); + return false; + } osalSysLock(); hal.util->persistent_data.spi_count++; if (send == nullptr) { diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c index c7fc0b4855..7b6fce4086 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c @@ -45,8 +45,9 @@ void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_b (*bouncebuffer)->is_sdcard = sdcard; if (prealloc_bytes) { (*bouncebuffer)->dma_buf = sdcard?malloc_sdcard_dma(prealloc_bytes):malloc_dma(prealloc_bytes); - osalDbgAssert(((*bouncebuffer)->dma_buf != NULL), "bouncebuffer preallocate"); - (*bouncebuffer)->size = prealloc_bytes; + if ((*bouncebuffer)->dma_buf) { + (*bouncebuffer)->size = prealloc_bytes; + } } } @@ -55,11 +56,11 @@ void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_b Note that *buf can be NULL, in which case we allocate DMA capable memory, but don't copy to it in bouncebuffer_finish_read(). This avoids DMA failures in dummyrx in the SPI LLD */ -void bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf, uint32_t size) +bool bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf, uint32_t size) { if (!bouncebuffer || IS_DMA_SAFE(*buf)) { // nothing needs to be done - return; + return true; } osalDbgAssert((bouncebuffer->busy == false), "bouncebuffer read"); bouncebuffer->orig_buf = *buf; @@ -68,7 +69,10 @@ void bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf, free(bouncebuffer->dma_buf); } bouncebuffer->dma_buf = bouncebuffer->is_sdcard?malloc_sdcard_dma(size):malloc_dma(size); - osalDbgAssert((bouncebuffer->dma_buf != NULL), "bouncebuffer read allocate"); + if (!bouncebuffer->dma_buf) { + bouncebuffer->size = 0; + return false; + } bouncebuffer->size = size; } *buf = bouncebuffer->dma_buf; @@ -77,6 +81,7 @@ void bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf, stm32_cacheBufferInvalidate(*buf, (size+31)&~31); #endif bouncebuffer->busy = true; + return true; } /* @@ -97,11 +102,11 @@ void bouncebuffer_finish_read(struct bouncebuffer_t *bouncebuffer, const uint8_t /* setup for reading from memory to a device, allocating a bouncebuffer if needed */ -void bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t **buf, uint32_t size) +bool bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t **buf, uint32_t size) { if (!bouncebuffer || IS_DMA_SAFE(*buf)) { // nothing needs to be done - return; + return true; } osalDbgAssert((bouncebuffer->busy == false), "bouncebuffer write"); if (bouncebuffer->size < size) { @@ -109,7 +114,10 @@ void bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t free(bouncebuffer->dma_buf); } bouncebuffer->dma_buf = bouncebuffer->is_sdcard?malloc_sdcard_dma(size):malloc_dma(size); - osalDbgAssert((bouncebuffer->dma_buf != NULL), "bouncebuffer write allocate"); + if (!bouncebuffer->dma_buf) { + bouncebuffer->size = 0; + return false; + } bouncebuffer->size = size; } if (*buf) { @@ -121,6 +129,7 @@ void bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t stm32_cacheBufferFlush(*buf, (size+31)&~31); #endif bouncebuffer->busy = true; + return true; } @@ -134,3 +143,13 @@ void bouncebuffer_finish_write(struct bouncebuffer_t *bouncebuffer, const uint8_ bouncebuffer->busy = false; } } + +/* + abort an operation + */ +void bouncebuffer_abort(struct bouncebuffer_t *bouncebuffer) +{ + if (bouncebuffer) { + bouncebuffer->busy = false; + } +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.h b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.h index 67b29bd6c5..89732db576 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.h @@ -27,7 +27,7 @@ void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_b /* setup for reading from a device into memory, allocating a bouncebuffer if needed */ -void bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf, uint32_t size); +bool bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf, uint32_t size); /* finish a read operation @@ -37,13 +37,18 @@ void bouncebuffer_finish_read(struct bouncebuffer_t *bouncebuffer, const uint8_t /* setup for reading from memory to a device, allocating a bouncebuffer if needed */ -void bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t **buf, uint32_t size); +bool bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t **buf, uint32_t size); /* finish a write operation */ void bouncebuffer_finish_write(struct bouncebuffer_t *bouncebuffer, const uint8_t *buf); +/* + abort an operation + */ +void bouncebuffer_abort(struct bouncebuffer_t *bouncebuffer); + #ifdef __cplusplus } #endif