2 files changed, 78 insertions, 11 deletions

diff --git a/drivers/chibios/spi_master.c b/drivers/chibios/spi_master.c
index 4852a6eba..28ddcbb2b 100644
--- a/drivers/chibios/spi_master.c
+++ b/drivers/chibios/spi_master.c
@@ -18,8 +18,13 @@
 
 #include "timer.h"
 
-static pin_t     currentSlavePin = NO_PIN;
-static SPIConfig spiConfig       = {false, NULL, 0, 0, 0, 0};
+static pin_t currentSlavePin = NO_PIN;
+
+#if defined(K20x) || defined(KL2x)
+static SPIConfig spiConfig = {NULL, 0, 0, 0};
+#else
+static SPIConfig spiConfig = {false, NULL, 0, 0, 0, 0};
+#endif
 
 __attribute__((weak)) void spi_init(void) {
     static bool is_initialised = false;
@@ -27,15 +32,15 @@ __attribute__((weak)) void spi_init(void) {
         is_initialised = true;
 
         // Try releasing special pins for a short time
-        palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_INPUT);
-        palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_INPUT);
-        palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_INPUT);
+        setPinInput(SPI_SCK_PIN);
+        setPinInput(SPI_MOSI_PIN);
+        setPinInput(SPI_MISO_PIN);
 
         chThdSleepMilliseconds(10);
 #if defined(USE_GPIOV1)
-        palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_STM32_ALTERNATE_PUSHPULL);
-        palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_STM32_ALTERNATE_PUSHPULL);
-        palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_STM32_ALTERNATE_PUSHPULL);
+        palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), SPI_SCK_PAL_MODE);
+        palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), SPI_MOSI_PAL_MODE);
+        palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), SPI_MISO_PAL_MODE);
 #else
         palSetPadMode(PAL_PORT(SPI_SCK_PIN), PAL_PAD(SPI_SCK_PIN), PAL_MODE_ALTERNATE(SPI_SCK_PAL_MODE) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
         palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_ALTERNATE(SPI_MOSI_PAL_MODE) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
@@ -58,6 +63,54 @@ bool spi_start(pin_t slavePin, bool lsbFirst, uint8_t mode, uint16_t divisor) {
         return false;
     }
 
+#if defined(K20x) || defined(KL2x)
+    spiConfig.tar0 = SPIx_CTARn_FMSZ(7) | SPIx_CTARn_ASC(1);
+
+    if (lsbFirst) {
+        spiConfig.tar0 |= SPIx_CTARn_LSBFE;
+    }
+
+    switch (mode) {
+        case 0:
+            break;
+        case 1:
+            spiConfig.tar0 |= SPIx_CTARn_CPHA;
+            break;
+        case 2:
+            spiConfig.tar0 |= SPIx_CTARn_CPOL;
+            break;
+        case 3:
+            spiConfig.tar0 |= SPIx_CTARn_CPHA | SPIx_CTARn_CPOL;
+            break;
+    }
+
+    switch (roundedDivisor) {
+        case 2:
+            spiConfig.tar0 |= SPIx_CTARn_BR(0);
+            break;
+        case 4:
+            spiConfig.tar0 |= SPIx_CTARn_BR(1);
+            break;
+        case 8:
+            spiConfig.tar0 |= SPIx_CTARn_BR(3);
+            break;
+        case 16:
+            spiConfig.tar0 |= SPIx_CTARn_BR(4);
+            break;
+        case 32:
+            spiConfig.tar0 |= SPIx_CTARn_BR(5);
+            break;
+        case 64:
+            spiConfig.tar0 |= SPIx_CTARn_BR(6);
+            break;
+        case 128:
+            spiConfig.tar0 |= SPIx_CTARn_BR(7);
+            break;
+        case 256:
+            spiConfig.tar0 |= SPIx_CTARn_BR(8);
+            break;
+    }
+#else
     spiConfig.cr1 = 0;
 
     if (lsbFirst) {
@@ -103,6 +156,7 @@ bool spi_start(pin_t slavePin, bool lsbFirst, uint8_t mode, uint16_t divisor) {
             spiConfig.cr1 |= SPI_CR1_BR_2 | SPI_CR1_BR_1 | SPI_CR1_BR_0;
             break;
     }
+#endif
 
     currentSlavePin  = slavePin;
     spiConfig.ssport = PAL_PORT(slavePin);
diff --git a/drivers/chibios/spi_master.h b/drivers/chibios/spi_master.h
index e93580e31..b5a6ef143 100644
--- a/drivers/chibios/spi_master.h
+++ b/drivers/chibios/spi_master.h
@@ -21,6 +21,7 @@
 #include <stdbool.h>
 
 #include "gpio.h"
+#include "chibios_config.h"
 
 #ifndef SPI_DRIVER
 #    define SPI_DRIVER SPID2
@@ -31,7 +32,11 @@
 #endif
 
 #ifndef SPI_SCK_PAL_MODE
-#    define SPI_SCK_PAL_MODE 5
+#    if defined(USE_GPIOV1)
+#        define SPI_SCK_PAL_MODE PAL_MODE_STM32_ALTERNATE_PUSHPULL
+#    else
+#        define SPI_SCK_PAL_MODE 5
+#    endif
 #endif
 
 #ifndef SPI_MOSI_PIN
@@ -39,7 +44,11 @@
 #endif
 
 #ifndef SPI_MOSI_PAL_MODE
-#    define SPI_MOSI_PAL_MODE 5
+#    if defined(USE_GPIOV1)
+#        define SPI_MOSI_PAL_MODE PAL_MODE_STM32_ALTERNATE_PUSHPULL
+#    else
+#        define SPI_MOSI_PAL_MODE 5
+#    endif
 #endif
 
 #ifndef SPI_MISO_PIN
@@ -47,7 +56,11 @@
 #endif
 
 #ifndef SPI_MISO_PAL_MODE
-#    define SPI_MISO_PAL_MODE 5
+#    if defined(USE_GPIOV1)
+#        define SPI_MISO_PAL_MODE PAL_MODE_STM32_ALTERNATE_PUSHPULL
+#    else
+#        define SPI_MISO_PAL_MODE 5
+#    endif
 #endif
 
 typedef int16_t spi_status_t;