ports: stm32h7: implement spiCalcClockDiv()

Author: dron0gus

firmware/hw_layer/hardware.h

@@ -13,6 +13,19 @@ void stopHardware();
 
 #if HAL_USE_SPI
 
+#ifdef STM32H7XX
+
+#define SPI_BaudRatePrescaler_2         (0x0UL << SPI_CFG1_MBR_Pos)
+#define SPI_BaudRatePrescaler_4         (0x1UL << SPI_CFG1_MBR_Pos)
+#define SPI_BaudRatePrescaler_8         (0x2UL << SPI_CFG1_MBR_Pos)
+#define SPI_BaudRatePrescaler_16        (0x3UL << SPI_CFG1_MBR_Pos)
+#define SPI_BaudRatePrescaler_32        (0x4UL << SPI_CFG1_MBR_Pos)
+#define SPI_BaudRatePrescaler_64        (0x5UL << SPI_CFG1_MBR_Pos)
+#define SPI_BaudRatePrescaler_128       (0x6UL << SPI_CFG1_MBR_Pos)
+#define SPI_BaudRatePrescaler_256       (0x7UL << SPI_CFG1_MBR_Pos)
+
+#else /* not STM32H7XX */
+
 // Peripherial Clock 42MHz SPI2 SPI3
 // Peripherial Clock 84MHz SPI1                                SPI1        SPI2/3
 //  42 MHz      21 MHZ
@@ -32,6 +45,8 @@ void stopHardware();
 //  328.125 KHz 164.06 KHz
 #define SPI_BaudRatePrescaler_256       ((uint16_t)0x0038)
 
+#endif
+
 SPIDriver * getSpiDevice(spi_device_e spiDevice);
 void turnOnSpi(spi_device_e device);
 void lockSpi(spi_device_e device);

firmware/hw_layer/ports/stm32/stm32_spi.cpp

@@ -182,6 +182,83 @@ void initSpiCs(SPIConfig *spiConfig, brain_pin_e csPin) {
 	efiSetPadMode("chip select", csPin, PAL_STM32_MODE_OUTPUT);
 }
 
+#ifdef STM32H7XX
+
+int spiGetBaseClock(SPIDriver *spip)
+{
+#if STM32_SPI_USE_SPI1
+	if (spip == &SPID1) {
+		return STM32_SPI1CLK;
+	}
+#endif
+#if STM32_SPI_USE_SPI2
+	if (spip == &SPID2) {
+		return STM32_SPI2CLK;
+	}
+#endif
+#if STM32_SPI_USE_SPI3
+	if (spip == &SPID3) {
+		return STM32_SPI3CLK;
+	}
+#endif
+#if STM32_SPI_USE_SPI4
+	if (spip == &SPID4) {
+		return STM32_SPI4CLK;
+	}
+#endif
+#if STM32_SPI_USE_SPI5
+	if (spip == &SPID5) {
+		return STM32_SPI5CLK;
+	}
+#endif
+#if STM32_SPI_USE_SPI6
+	if (spip == &SPID6) {
+		return STM32_SPI6CLK;
+	}
+#endif
+
+	return 0;
+}
+
+int spiCalcClockDiv(SPIDriver *spip, SPIConfig *spiConfig, unsigned int clk)
+{
+	if (clk == 0) {
+		return -1;
+	}
+
+	unsigned int baseClock = spiGetBaseClock(spip);
+
+	if (baseClock == 0) {
+		return -1;
+	}
+
+	// round down
+	int div = (baseClock + clk - 1) / clk;
+
+	spiConfig->cfg1 &= ~SPI_CFG1_MBR_Msk;
+	if (div <= 2) {
+		spiConfig->cfg1 |= SPI_BaudRatePrescaler_2;
+	} else if (div <= 4) {
+		spiConfig->cfg1 |= SPI_BaudRatePrescaler_4;
+	} else if (div <= 8) {
+		spiConfig->cfg1 |= SPI_BaudRatePrescaler_8;
+	} else if (div <= 16) {
+		spiConfig->cfg1 |= SPI_BaudRatePrescaler_16;
+	} else if (div <= 32) {
+		spiConfig->cfg1 |= SPI_BaudRatePrescaler_32;
+	} else if (div <= 64) {
+		spiConfig->cfg1 |= SPI_BaudRatePrescaler_64;
+	} else if (div <= 128) {
+		spiConfig->cfg1 |= SPI_BaudRatePrescaler_128;
+	} else {
+		spiConfig->cfg1 |= SPI_BaudRatePrescaler_256;
+	}
+
+	return 0;
+}
+
+#else
+
 int spiGetBaseClock(SPIDriver *spip)
 {
 #if STM32_SPI_USE_SPI1
@@ -224,16 +301,6 @@ int spiGetBaseClock(SPIDriver *spip)
 	return 0;
 }
 
-#ifdef STM32H7XX
-
-int spiCalcClockDiv(SPIDriver*, SPIConfig*, unsigned int)
-{
-	// TODO: implement
-	return -1;
-}
-
-#else
-
 int spiCalcClockDiv(SPIDriver *spip, SPIConfig *spiConfig, unsigned int clk)
 {
 	if (clk == 0) {