From 67a44c4636e73410b6418dba66fed649b0e8e470 Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Mon, 21 Aug 2017 11:23:09 +0900
Subject: [PATCH] Clock divisor for SPI2 and SPI3 must be halved

---
 src/main/drivers/bus_spi.c | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c
index 61f441ac75..4ff35aeb66 100644
--- a/src/main/drivers/bus_spi.c
+++ b/src/main/drivers/bus_spi.c
@@ -249,6 +249,8 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData,
     return true;
 }
 
+#include "build/debug.h"
+
 bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length)
 {
     IOLo(bus->busdev_u.spi.csnPin);
@@ -259,10 +261,17 @@ bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxDa
 
 void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
 {
-#define BR_CLEAR_MASK 0xFFC7
+#define BR_BITS ((BIT(5) | BIT(4) | BIT(3)))
+
+    // SPI2 and SPI3 are always on APB1/AHB1 which PCLK is half that of APB2/AHB2.
+
+    if (instance == SPI2 || instance == SPI3) {
+        divisor /= 2; // Safe for divisor == 0 or 1
+    }
+
     SPI_Cmd(instance, DISABLE);
 
-    const uint16_t tempRegister = (instance->CR1 & BR_CLEAR_MASK);
+    const uint16_t tempRegister = (instance->CR1 & ~BR_BITS);
     instance->CR1 = (tempRegister | ((ffs(divisor | 0x100) - 2) << 3));
 
     SPI_Cmd(instance, ENABLE);