1 files changed, 77 insertions, 45 deletions

diff --git a/fw/fe310/eos/dev/spi.c b/fw/fe310/eos/dev/spi.c
index 12549fc..fef00e1 100644
--- a/fw/fe310/eos/dev/spi.c
+++ b/fw/fe310/eos/dev/spi.c
@@ -12,72 +12,112 @@
 
 #include "soc/interrupt.h"
 #include "soc/spi.h"
+#include "soc/spi_priv.h"
 
 #include "net.h"
+#include "egpio.h"
 
 #include "spi.h"
 #include "spi_cfg.h"
 
-static uint8_t spi_dev;
-static uint8_t spi_lock;
+#ifdef EOS_DEBUG
+#include <stdio.h>
+#endif
+
+static unsigned char spi_dstack[EOS_SPI_MAX_DSTACK];
+static unsigned char spi_dstack_len;
 static uint16_t spi_div[EOS_SPI_MAX_DEV];
 
-int eos_spi_dev_init(uint8_t wakeup_cause) {
+static uint8_t spi_dev(void) {
+    return spi_dstack_len ? spi_dstack[spi_dstack_len - 1] : EOS_SPI_DEV_NET;
+}
+
+static void spi_stop(unsigned char dev) {
+    if (dev == EOS_SPI_DEV_NET) {
+        eos_net_stop();
+    } else if (spi_cfg[dev].flags & SPI_DEV_FLAG_9BIT) {
+        eos_spi_enable();
+    } else {
+        eos_spi_stop();
+    }
+}
+
+static void spi_start(unsigned char dev) {
+    if (dev == EOS_SPI_DEV_NET) {
+        eos_net_start();
+    } else if (spi_cfg[dev].flags & SPI_DEV_FLAG_9BIT) {
+        eos_spi_configure(spi_div[dev], spi_cfg[dev].csid, spi_cfg[dev].cspin, spi_cfg[dev].evt);
+        eos_spi_disable();
+    } else {
+        eos_spi_start(spi_div[dev], spi_cfg[dev].csid, spi_cfg[dev].cspin, spi_cfg[dev].evt);
+    }
+}
+
+int eos_spi_dev_init(void) {
     int i;
 
     for (i=0; i<EOS_SPI_MAX_DEV; i++) {
         spi_div[i] = spi_cfg[i].div;
-        if (!(spi_cfg[i].flags & SPI_DEV_FLAG_CSFLOAT) && (spi_cfg[i].cspin != -1)) {
-            GPIO_REG(GPIO_OUTPUT_VAL)   |=  (1 << spi_cfg[i].cspin);
-            GPIO_REG(GPIO_OUTPUT_EN)    |=  (1 << spi_cfg[i].cspin);
-        }
+    }
+
+    for (i=0; i<EOS_SPI_MAX_DSTACK; i++) {
+        spi_dstack[i] = 0xff;
     }
 
     return EOS_OK;
 }
 
 int eos_spi_select(unsigned char dev) {
-    if (spi_lock) return EOS_ERR_BUSY;
+    int rv;
+    int dsel;
 
-    if (spi_cfg[spi_dev].flags & SPI_DEV_FLAG_9BIT) {
-        eos_spi_enable();
-    } else {
-        if (spi_dev == EOS_SPI_DEV_NET) {
-            eos_net_stop();
-        } else {
-            eos_spi_stop();
-        }
+    if (eos_spi_cs_get()) rv = EOS_ERR_BUSY;
+    if (!rv && (spi_dstack_len == EOS_SPI_MAX_DSTACK)) rv = EOS_ERR_FULL;
+
+    if (rv) {
+#ifdef EOS_DEBUG
+        printf("SPI SELECT DEV:%d ERR:%d\n", dev, rv);
+#endif
+        return rv;
     }
 
-    spi_dev = dev;
-    if (spi_cfg[dev].flags & SPI_DEV_FLAG_9BIT) {
-        eos_spi_configure(spi_div[dev], spi_cfg[dev].csid, spi_cfg[dev].cspin, spi_cfg[dev].evt);
-        eos_spi_disable();
-    } else {
-        if (dev == EOS_SPI_DEV_NET) {
-            eos_net_start();
-        } else {
-            eos_spi_start(spi_div[dev], spi_cfg[dev].csid, spi_cfg[dev].cspin, spi_cfg[dev].evt);
-        }
+    dsel = 1;
+    if (spi_dev() == dev) {
+        dev |= EOS_SPI_DEV_FLAG_NDSEL;
+        dsel = 0;
     }
 
+    if (dsel) spi_stop(spi_dev());
+
+    spi_dstack[spi_dstack_len] = dev;
+    spi_dstack_len++;
+
+    if (dsel) spi_start(dev);
+
     return EOS_OK;
 }
 
-int eos_spi_deselect(void) {
-    if (spi_lock) return EOS_ERR_BUSY;
-    if (spi_dev == EOS_SPI_DEV_NET) return EOS_ERR;
+void eos_spi_deselect(void) {
+    int rv;
+    int dsel;
 
-    if (spi_cfg[spi_dev].flags & SPI_DEV_FLAG_9BIT) {
-        eos_spi_enable();
-    } else {
-        eos_spi_stop();
+    if (eos_spi_cs_get()) rv = EOS_ERR_BUSY;
+    if (!rv && (spi_dstack_len == 0)) rv = EOS_ERR_EMPTY;
+
+    if (rv) {
+#ifdef EOS_DEBUG
+        printf("SPI DESELECT ERR:%d\n", rv);
+#endif
+        return;
     }
 
-    spi_dev = EOS_SPI_DEV_NET;
-    eos_net_start();
+    dsel = !(spi_dev() & EOS_SPI_DEV_FLAG_NDSEL);
+    if (dsel) spi_stop(spi_dev());
 
-    return EOS_OK;
+    spi_dstack_len--;
+    spi_dstack[spi_dstack_len] = 0xff;
+
+    if (dsel) spi_start(spi_dev());
 }
 
 void eos_spi_dev_configure(unsigned char dev) {
@@ -85,7 +125,7 @@ void eos_spi_dev_configure(unsigned char dev) {
 }
 
 uint8_t eos_spi_dev(void) {
-    return spi_dev;
+    return spi_dev();
 }
 
 uint16_t eos_spi_div(unsigned char dev) {
@@ -100,14 +140,6 @@ uint8_t eos_spi_cspin(unsigned char dev) {
     return spi_cfg[dev].cspin;
 }
 
-void eos_spi_lock(void) {
-    spi_lock = 1;
-}
-
-void eos_spi_unlock(void) {
-    spi_lock = 0;
-}
-
 void eos_spi_set_div(unsigned char dev, uint16_t div) {
     spi_div[dev] = div;
 }