1 files changed, 32 insertions, 6 deletions

diff --git a/fw/fe310/eos/dev/lcd.c b/fw/fe310/eos/dev/lcd.c
index 01d86e9..9fb8767 100644
--- a/fw/fe310/eos/dev/lcd.c
+++ b/fw/fe310/eos/dev/lcd.c
@@ -22,8 +22,6 @@
 static int lcd_enable(void) {
     int rv;
 
-    if (eos_i2s_running() || !eos_egpio_get_val(EGPIO_PIN_DISP_SEL)) return EOS_ERR_BUSY;
-
     rv = eos_egpio_set_val(EGPIO_PIN_LCD_EN, 1);
     return rv;
 }
@@ -31,8 +29,6 @@ static int lcd_enable(void) {
 static int lcd_disable(void) {
     int rv;
 
-    if (eos_i2s_running() || !eos_egpio_get_val(EGPIO_PIN_DISP_SEL)) return EOS_ERR_BUSY;
-
     rv = eos_egpio_set_val(EGPIO_PIN_LCD_EN, 0);
     return rv;
 }
@@ -44,7 +40,7 @@ static int lcd_enabled(void) {
 static int lcd_select(void) {
     int rv;
 
-    if (eos_i2s_running() || !eos_egpio_get_val(EGPIO_PIN_DISP_SEL)) return EOS_ERR_BUSY;
+    if (eos_i2s_running()) return EOS_ERR_BUSY;
 
     GPIO_REG(GPIO_OUTPUT_XOR)   |=  (1 << SPI_CSPIN_LCD);
     GPIO_REG(GPIO_OUTPUT_VAL)   |=  (1 << SPI_CSPIN_LCD);
@@ -85,13 +81,32 @@ static int lcd_init(void) {
     }
 
     rv = ili9806e_init();
-    if (rv == EOS_ERR_NOTFOUND) eve_lcd_absent();
+    if (rv == EOS_ERR_NOTFOUND) eve_lcd_set_absent(1);
     lcd_deselect();
 
     if (rv) lcd_disable();
     return rv;
 }
 
+static int lcd_reset(void) {
+    int rv;
+
+    if (!lcd_enabled()) return EOS_ERR;
+
+    rv = lcd_disable();
+    if (rv) return rv;
+
+    eos_sleep(10);
+
+    rv = lcd_init();
+    if (rv) {
+        lcd_enable();
+        return rv;
+    }
+
+    return EOS_OK;
+}
+
 static int lcd_sleep(void) {
     int rv;
 
@@ -109,6 +124,9 @@ int eos_lcd_init(void) {
     uint8_t wakeup_cause;
     int rv, rst;
 
+    if (!eos_egpio_get_val(EGPIO_PIN_DISP_SEL)) return EOS_ERR_BUSY;
+    if (lcd_enabled()) return EOS_ERR;
+
     wakeup_cause = eos_pwr_wakeup_cause();
     rst = (wakeup_cause == EOS_PWR_WAKE_RST);
     if (rst) {
@@ -137,3 +155,11 @@ int eos_lcd_wake(void) {
     rv = lcd_init();
     return rv;
 }
+
+int eos_lcd_take(void) {
+    return lcd_reset();
+}
+
+int eos_lcd_give(void) {
+    return lcd_reset();
+}