diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c index 99c4db8191..573a29c61c 100644 --- a/src/main/drivers/bus_spi_ll.c +++ b/src/main/drivers/bus_spi_ll.c @@ -27,6 +27,7 @@ #if defined(USE_SPI) #include "common/utils.h" +#include "common/maths.h" #include "drivers/bus.h" #include "drivers/bus_spi.h" @@ -226,8 +227,10 @@ void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) } #endif + divisor = constrain(divisor, 2, 256); + LL_SPI_Disable(instance); - LL_SPI_SetBaudRatePrescaler(instance, divisor ? (ffs(divisor | 0x100) - 2) << SPI_CR1_BR_Pos : 0); + LL_SPI_SetBaudRatePrescaler(instance, (ffs(divisor) - 2) << SPI_CR1_BR_Pos); LL_SPI_Enable(instance); } #endif diff --git a/src/main/drivers/bus_spi_stdperiph.c b/src/main/drivers/bus_spi_stdperiph.c index f2626a8543..de7911ea51 100644 --- a/src/main/drivers/bus_spi_stdperiph.c +++ b/src/main/drivers/bus_spi_stdperiph.c @@ -26,6 +26,7 @@ #ifdef USE_SPI +#include "common/maths.h" #include "drivers/bus.h" #include "drivers/bus_spi.h" #include "drivers/bus_spi_impl.h" @@ -186,10 +187,12 @@ void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) } #endif + divisor = constrain(divisor, 2, 256); + SPI_Cmd(instance, DISABLE); const uint16_t tempRegister = (instance->CR1 & ~BR_BITS); - instance->CR1 = tempRegister | (divisor ? ((ffs(divisor | 0x100) - 2) << 3) : 0); + instance->CR1 = tempRegister | ((ffs(divisor) - 2) << 3); SPI_Cmd(instance, ENABLE);