diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp index 662593fa06..ace3169582 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "Util.h" #include "Scheduler.h" #include "Semaphores.h" @@ -166,14 +167,15 @@ void SPIDevice::set_slowdown(uint8_t slowdown) /* low level transfer function */ -void SPIDevice::do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len) +bool SPIDevice::do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len) { bool old_cs_forced = cs_forced; if (!set_chip_select(true)) { - return; + return false; } + bool ret = true; #if defined(HAL_SPI_USE_POLLED) for (uint16_t i=0; ithread, TIME_MS2I(timeout_us)); + osalSysUnlock(); + if (msg == MSG_TIMEOUT) { + ret = false; + AP::internalerror().error(AP_InternalError::error_t::spi_fail); + spiAbort(spi_devices[device_desc.bus].driver); } bus.bouncebuffer_finish(send, recv, len); #endif - set_chip_select(old_cs_forced); + return ret; } bool SPIDevice::clock_pulse(uint32_t n) @@ -252,8 +267,7 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, } if ((send_len == recv_len && send == recv) || !send || !recv) { // simplest cases, needed for DMA - do_transfer(send, recv, recv_len?recv_len:send_len); - return true; + return do_transfer(send, recv, recv_len?recv_len:send_len); } uint8_t buf[send_len+recv_len]; if (send_len > 0) { @@ -262,11 +276,11 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, if (recv_len > 0) { memset(&buf[send_len], 0, recv_len); } - do_transfer(buf, buf, send_len+recv_len); - if (recv_len > 0) { + bool ret = do_transfer(buf, buf, send_len+recv_len); + if (ret && recv_len > 0) { memcpy(recv, &buf[send_len], recv_len); } - return true; + return ret; } bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) @@ -274,9 +288,11 @@ bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t bus.semaphore.assert_owner(); uint8_t buf[len]; memcpy(buf, send, len); - do_transfer(buf, buf, len); - memcpy(recv, buf, len); - return true; + bool ret = do_transfer(buf, buf, len); + if (ret) { + memcpy(recv, buf, len); + } + return ret; } AP_HAL::Semaphore *SPIDevice::get_semaphore() diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.h b/libraries/AP_HAL_ChibiOS/SPIDevice.h index 59f02021cc..af597a4033 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.h +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.h @@ -74,9 +74,6 @@ public: /* See AP_HAL::Device::set_speed() */ bool set_speed(AP_HAL::Device::Speed speed) override; - // low level transfer function - void do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len); - /* See AP_HAL::Device::transfer() */ bool transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) override; @@ -127,6 +124,8 @@ private: static void *spi_thread(void *arg); static uint32_t derive_freq_flag_bus(uint8_t busid, uint32_t _frequency); uint32_t derive_freq_flag(uint32_t _frequency); + // low level transfer function + bool do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len); }; class SPIDeviceManager : public AP_HAL::SPIDeviceManager {