diff --git a/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp b/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp index 059799b7..b7e13fe3 100644 --- a/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp +++ b/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp @@ -18,6 +18,14 @@ bool equals(const SPI_InitTypeDef& lhs, const SPI_InitTypeDef& rhs) { && (lhs.CRCPolynomial == rhs.CRCPolynomial); } +bool Stm32SpiArbiter::acquire_task(SpiTask* task) { + return !__atomic_exchange_n(&task->is_in_use, true, __ATOMIC_SEQ_CST); +} + +void Stm32SpiArbiter::release_task(SpiTask* task) { + task->is_in_use = false; +} + bool Stm32SpiArbiter::start() { if (!task_list_) { return false; @@ -63,7 +71,7 @@ void Stm32SpiArbiter::transfer_async(SpiTask* task) { if (ptr == &task_list_) { if (!start()) { if (task->on_complete) { - (*task->on_complete)(task->cb_ctx, false); + (*task->on_complete)(task->on_complete_ctx, false); } } } @@ -80,7 +88,7 @@ bool Stm32SpiArbiter::transfer(SPI_InitTypeDef config, Stm32Gpio ncs_gpio, const .rx_buf = rx_buf, .length = length, .on_complete = [](void* ctx, bool success) { *(volatile uint8_t*)ctx = success ? 1 : 0; }, - .cb_ctx = (void*)&result, + .on_complete_ctx = (void*)&result, .next = nullptr }; @@ -101,7 +109,7 @@ void Stm32SpiArbiter::on_complete() { // Wrap up transfer task_list_->ncs_gpio.write(true); if (task_list_->on_complete) { - (*task_list_->on_complete)(task_list_->cb_ctx, true); + (*task_list_->on_complete)(task_list_->on_complete_ctx, true); } // Start next task if any diff --git a/Firmware/Drivers/STM32/stm32_spi_arbiter.hpp b/Firmware/Drivers/STM32/stm32_spi_arbiter.hpp index 52963e95..79d54c3c 100644 --- a/Firmware/Drivers/STM32/stm32_spi_arbiter.hpp +++ b/Firmware/Drivers/STM32/stm32_spi_arbiter.hpp @@ -14,12 +14,36 @@ public: uint8_t* rx_buf; size_t length; void (*on_complete)(void*, bool); - void* cb_ctx; + void* on_complete_ctx; + bool is_in_use = false; struct SpiTask* next; }; Stm32SpiArbiter(SPI_HandleTypeDef* hspi): hspi_(hspi) {} + /** + * Reserves the task for the caller if it's not in use currently. + * + * This can be used by the caller to ensure that the task structure is not + * overwritten while it's in use in a preceding transfer. + * + * Example: + * + * if (acquire_task(&task)) { + * transfer_async(&task) + * } + * + * A call to release_task() makes the task available for use again. + */ + static bool acquire_task(SpiTask* task); + + /** + * Releases the task so that the next call to `acquire_task()` returns true. + * This should usually be called inside the on_complete() callback after + * the rx buffer has been processed. + */ + static void release_task(SpiTask* task); + /** * @brief Enqueues a non-blocking transfer. * diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 9e6709e8..38fce2c6 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -371,24 +371,20 @@ void Encoder::decode_hall_samples() { bool Encoder::abs_spi_start_transaction(){ if (mode_ & MODE_FLAG_ABS){ axis_->motor_.log_timing(TIMING_LOG_SPI_START); - - if (spi_busy_) { + + if (Stm32SpiArbiter::acquire_task(&spi_task_)) { + spi_task_.ncs_gpio = abs_spi_cs_gpio_; + spi_task_.tx_buf = (uint8_t*)abs_spi_dma_tx_; + spi_task_.rx_buf = (uint8_t*)abs_spi_dma_rx_; + spi_task_.length = 1; + spi_task_.on_complete = [](void* ctx, bool success) { ((Encoder*)ctx)->abs_spi_cb(success); }; + spi_task_.on_complete_ctx = this; + spi_task_.next = nullptr; + + spi_arbiter_->transfer_async(&spi_task_); + } else { return false; } - - spi_task_.ncs_gpio = abs_spi_cs_gpio_; - spi_task_.tx_buf = (uint8_t*)abs_spi_dma_tx_; - spi_task_.rx_buf = (uint8_t*)abs_spi_dma_rx_; - spi_task_.length = 1; - spi_task_.on_complete = [](void* ctx, bool success) { - ((Encoder*)ctx)->spi_busy_ = false; - if (success) - ((Encoder*)ctx)->abs_spi_cb(); - }; - spi_task_.cb_ctx = this; - spi_task_.next = nullptr; - - spi_arbiter_->transfer_async(&spi_task_); } return true; } @@ -408,17 +404,21 @@ uint8_t cui_parity(uint16_t v) { return ~v & 3; } -void Encoder::abs_spi_cb() { - axis_->motor_.log_timing(TIMING_LOG_SPI_END); - +void Encoder::abs_spi_cb(bool success) { uint16_t pos; + if (!success) { + goto done; + } + + axis_->motor_.log_timing(TIMING_LOG_SPI_END); + switch (mode_) { case MODE_SPI_ABS_AMS: { uint16_t rawVal = abs_spi_dma_rx_[0]; // check if parity is correct (even) and error flag clear if (ams_parity(rawVal) || ((rawVal >> 14) & 1)) { - return; + goto done; } pos = rawVal & 0x3fff; } break; @@ -427,14 +427,14 @@ void Encoder::abs_spi_cb() { uint16_t rawVal = abs_spi_dma_rx_[0]; // check if parity is correct if (cui_parity(rawVal)) { - return; + goto done; } pos = rawVal & 0x3fff; } break; default: { set_error(ERROR_UNSUPPORTED_ENCODER_MODE); - return; + goto done; } break; } @@ -443,6 +443,9 @@ void Encoder::abs_spi_cb() { if (config_.pre_calibrated) { is_ready_ = true; } + +done: + Stm32SpiArbiter::release_task(&spi_task_); } void Encoder::abs_spi_cs_pin_init(){ diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 793083e5..41ab751b 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -107,16 +107,15 @@ public: float sincos_sample_c_ = 0.0f; bool abs_spi_start_transaction(); - void abs_spi_cb(); + void abs_spi_cb(bool success); void abs_spi_cs_pin_init(); - uint16_t abs_spi_dma_tx_[1] = {0xFFFF}; - uint16_t abs_spi_dma_rx_[1]; bool abs_spi_pos_updated_ = false; Mode mode_ = MODE_INCREMENTAL; Stm32Gpio abs_spi_cs_gpio_; uint32_t abs_spi_cr1; uint32_t abs_spi_cr2; - bool spi_busy_ = false; + uint16_t abs_spi_dma_tx_[1] = {0xFFFF}; + uint16_t abs_spi_dma_rx_[1]; Stm32SpiArbiter::SpiTask spi_task_; constexpr float getCoggingRatio(){