From 29a2bac4691dae81a613af31e5e99660e61edc3d Mon Sep 17 00:00:00 2001
From: Nick Brassel <nick@tzarc.org>
Date: Wed, 6 Jul 2022 06:41:35 +1000
Subject: [PATCH] Fixup SPI. (#17534)

---
 drivers/eeprom/eeprom_spi.c            | 42 ++++++++++++--------------
 platforms/chibios/drivers/spi_master.c |  3 ++
 2 files changed, 22 insertions(+), 23 deletions(-)

diff --git a/drivers/eeprom/eeprom_spi.c b/drivers/eeprom/eeprom_spi.c
index 25955498c46..51ba25deced 100644
--- a/drivers/eeprom/eeprom_spi.c
+++ b/drivers/eeprom/eeprom_spi.c
@@ -58,14 +58,20 @@ static bool spi_eeprom_start(void) {
 
 static spi_status_t spi_eeprom_wait_while_busy(int timeout) {
     uint32_t     deadline = timer_read32() + timeout;
-    spi_status_t response;
-    do {
+    spi_status_t response = SR_WIP;
+    while (response & SR_WIP) {
+        if (!spi_eeprom_start()) {
+            return SPI_STATUS_ERROR;
+        }
+
         spi_write(CMD_RDSR);
         response = spi_read();
+        spi_stop();
+
         if (timer_read32() >= deadline) {
             return SPI_STATUS_TIMEOUT;
         }
-    } while (response & SR_WIP);
+    }
     return SPI_STATUS_SUCCESS;
 }
 
@@ -105,27 +111,21 @@ void eeprom_driver_erase(void) {
 void eeprom_read_block(void *buf, const void *addr, size_t len) {
     //-------------------------------------------------
     // Wait for the write-in-progress bit to be cleared
-    bool res = spi_eeprom_start();
-    if (!res) {
-        dprint("failed to start SPI for WIP check\n");
-        memset(buf, 0, len);
-        return;
-    }
-
     spi_status_t response = spi_eeprom_wait_while_busy(EXTERNAL_EEPROM_SPI_TIMEOUT);
-    spi_stop();
-    if (response == SPI_STATUS_TIMEOUT) {
-        dprint("SPI timeout for WIP check\n");
+    if (response != SPI_STATUS_SUCCESS) {
+        spi_stop();
         memset(buf, 0, len);
+        dprint("SPI timeout for WIP check\n");
         return;
     }
 
     //-------------------------------------------------
     // Perform read
-    res = spi_eeprom_start();
+    bool res = spi_eeprom_start();
     if (!res) {
-        dprint("failed to start SPI for read\n");
+        spi_stop();
         memset(buf, 0, len);
+        dprint("failed to start SPI for read\n");
         return;
     }
 
@@ -158,15 +158,9 @@ void eeprom_write_block(const void *buf, void *addr, size_t len) {
 
         //-------------------------------------------------
         // Wait for the write-in-progress bit to be cleared
-        res = spi_eeprom_start();
-        if (!res) {
-            dprint("failed to start SPI for WIP check\n");
-            return;
-        }
-
         spi_status_t response = spi_eeprom_wait_while_busy(EXTERNAL_EEPROM_SPI_TIMEOUT);
-        spi_stop();
-        if (response == SPI_STATUS_TIMEOUT) {
+        if (response != SPI_STATUS_SUCCESS) {
+            spi_stop();
             dprint("SPI timeout for WIP check\n");
             return;
         }
@@ -175,6 +169,7 @@ void eeprom_write_block(const void *buf, void *addr, size_t len) {
         // Enable writes
         res = spi_eeprom_start();
         if (!res) {
+            spi_stop();
             dprint("failed to start SPI for write-enable\n");
             return;
         }
@@ -186,6 +181,7 @@ void eeprom_write_block(const void *buf, void *addr, size_t len) {
         // Perform the write
         res = spi_eeprom_start();
         if (!res) {
+            spi_stop();
             dprint("failed to start SPI for write\n");
             return;
         }
diff --git a/platforms/chibios/drivers/spi_master.c b/platforms/chibios/drivers/spi_master.c
index f9974d9f6bc..223d8a403ef 100644
--- a/platforms/chibios/drivers/spi_master.c
+++ b/platforms/chibios/drivers/spi_master.c
@@ -46,6 +46,9 @@ __attribute__((weak)) void spi_init(void) {
         palSetPadMode(PAL_PORT(SPI_MOSI_PIN), PAL_PAD(SPI_MOSI_PIN), PAL_MODE_ALTERNATE(SPI_MOSI_PAL_MODE) | PAL_OUTPUT_TYPE_PUSHPULL | PAL_OUTPUT_SPEED_HIGHEST);
         palSetPadMode(PAL_PORT(SPI_MISO_PIN), PAL_PAD(SPI_MISO_PIN), PAL_MODE_ALTERNATE(SPI_MISO_PAL_MODE) | PAL_OUTPUT_TYPE_PUSHPULL | PAL_OUTPUT_SPEED_HIGHEST);
 #endif
+        spiUnselect(&SPI_DRIVER);
+        spiStop(&SPI_DRIVER);
+        currentSlavePin = NO_PIN;
     }
 }