3 files changed, 32 insertions, 12 deletions

diff --git a/firmware/export/config-gigabeat-s.h b/firmware/export/config-gigabeat-s.h
index e5654fe0a2..f55a4fd382 100644
--- a/firmware/export/config-gigabeat-s.h
+++ b/firmware/export/config-gigabeat-s.h
@@ -104,11 +104,10 @@
  * turned off */
 #define LCD_SLEEP_TIMEOUT (2*HZ)
 
-#ifndef BOOTLOADER
-#if 0
 /* Define this if your LCD can be enabled/disabled */
 #define HAVE_LCD_ENABLE
-#endif
+
+#ifndef BOOTLOADER
 
 #define HAVE_BACKLIGHT_BRIGHTNESS
 
diff --git a/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c b/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
index 8b4459c3a9..8f75b72f58 100644
--- a/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
@@ -23,6 +23,7 @@
 #include "backlight.h"
 #include "mc13783.h"
 #include "backlight-target.h"
+#include "lcd.h"
 
 #ifdef HAVE_BACKLIGHT_BRIGHTNESS
 /* Table that uses combinations of current level and pwm fraction to get
@@ -129,6 +130,9 @@ void _backlight_on(void)
 #ifdef HAVE_LCD_SLEEP
     backlight_lcd_sleep_countdown(false); /* stop counter */
 #endif
+#ifdef HAVE_LCD_ENABLE
+    lcd_enable(true);
+#endif
 
     /* Set/clear LEDRAMPUP bit, clear LEDRAMPDOWN bit,
      * Ensure LED supply is on. */
diff --git a/firmware/target/arm/imx31/gigabeat-s/lcd-imx31.c b/firmware/target/arm/imx31/gigabeat-s/lcd-imx31.c
index c353380221..47834b3b19 100644
--- a/firmware/target/arm/imx31/gigabeat-s/lcd-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/lcd-imx31.c
@@ -30,8 +30,8 @@
 #define MAIN_LCD_IDMAC_CHANNEL 14
 #define LCDADDR(x, y) (&lcd_framebuffer[(y)][(x)])
 
-static volatile bool lcd_on = true;
-volatile bool lcd_poweroff = false;
+static bool lcd_on = true;
+static bool lcd_powered = true;
 static unsigned lcd_yuv_options = 0;
 /*
 ** This is imported from lcd-16bit.c
@@ -98,24 +98,41 @@ void lcd_update_rect(int x, int y, int width, int height)
     }
 }
 
-#ifdef HAVE_LCD_SLEEP
 void lcd_sleep(void)
 {
-    _backlight_lcd_sleep();
+    if (lcd_powered)
+    {
+        lcd_enable(false);
+        lcd_powered = false;
+        IPU_IDMAC_CHA_EN &= ~(1ul << MAIN_LCD_IDMAC_CHANNEL);
+        _backlight_lcd_sleep();
+    }
 }
-#endif /* HAVE_LCD_SLEEP */
 
-#if 0
 void lcd_enable(bool state)
 {
-    (void)state;
+    if (state == lcd_on)
+        return;
+
+    if (state)
+    {
+        IPU_IDMAC_CHA_EN |= 1ul << MAIN_LCD_IDMAC_CHANNEL;
+        sleep(HZ/50);
+        lcd_powered = true;
+        lcd_on = true;
+        lcd_update();
+        lcd_call_enable_hook();
+    }
+    else
+    {
+        lcd_on = false;
+    }
 }
 
 bool lcd_enabled(void)
 {
-    return true;
+    return lcd_on;
 }
-#endif
 
 /* Update the display.
    This must be called after all other LCD functions that change the display. */