17 files changed, 641 insertions, 208 deletions

diff --git a/apps/lang/english.lang b/apps/lang/english.lang
index 61d16d22f7..a723c1cc0e 100644
--- a/apps/lang/english.lang
+++ b/apps/lang/english.lang
@@ -8805,6 +8805,62 @@
   </dest>
   <voice>
     *: "(Vol- : Re-enable)"
+<phrase>
+  id: LANG_BACKLIGHT_ON_BUTTON_HOLD_NORMAL
+  desc: in lcd settings
+  user:
+  <source>
+    *: "Normal"
+  </source>
+  <dest>
+    *: "Normal"
+  </dest>
+  <voice>
+    *: "Normal"
+  </voice>
+</phrase>
+<phrase>
+  id: LANG_BACKLIGHT_ON_BUTTON_HOLD
+  desc: in lcd settings
+  user:
+  <source>
+    *: "Backlight (On Hold Key)"
+  </source>
+  <dest>
+    *: "Backlight (On Hold Key)"
+  </dest>
+  <voice>
+    *: "Backlight on hold key"
+  </voice>
+</phrase>
+<phrase>
+  id: LANG_NEVER
+  desc: in lcd settings
+  user:
+  <source>
+    *: "Never"
+  </source>
+  <dest>
+    *: "Never"
+  </dest>
+  <voice>
+    *: "Never"
+  </voice>
+</phrase>
+<phrase>
+  id: LANG_LCD_SLEEP_AFTER_BACKLIGHT_OFF
+  desc: In display settings, time to switch LCD chip into power saving state
+  user:
+  <source>
+    *: "Sleep (After Backlight Off)"
+  </source>
+  <dest>
+    *: "Sleep (After Backlight Off)"
+  </dest>
+  <voice>
+    *: "Sleep after backlight off"
+  </voice>
+</phrase>
   </voice>
 </phrase>
 <phrase>
diff --git a/apps/misc.c b/apps/misc.c
index b0c315af98..ffacf7ed82 100644
--- a/apps/misc.c
+++ b/apps/misc.c
@@ -491,6 +491,9 @@ static bool clean_shutdown(void (*callback)(void *), void *parameter)
     {
         FOR_NB_SCREENS(i)
             screens[i].clear_display();
+#ifdef X5_BACKLIGHT_SHUTDOWN
+        x5_backlight_shutdown();
+#endif
         gui_syncsplash(0, true, str(LANG_SHUTTINGDOWN));
         
         if (!tagcache_prepare_shutdown())
diff --git a/apps/settings.c b/apps/settings.c
index 673b8fe246..3c036d8594 100644
--- a/apps/settings.c
+++ b/apps/settings.c
@@ -24,11 +24,11 @@
 #include "config.h"
 #include "kernel.h"
 #include "thread.h"
+#include "button.h"
 #include "settings.h"
 #include "disk.h"
 #include "panic.h"
 #include "debug.h"
-#include "button.h"
 #include "usb.h"
 #include "backlight.h"
 #include "lcd.h"
@@ -40,7 +40,6 @@
 #include "ata.h"
 #include "fat.h"
 #include "power.h"
-#include "backlight.h"
 #include "powermgmt.h"
 #include "status.h"
 #include "atoi.h"
@@ -244,7 +243,7 @@ static const struct bit_entry rtc_bits[] =
     {6, S_O(contrast), DEFAULT_CONTRAST_SETTING, "contrast", NULL },
 #endif
 #ifdef CONFIG_BACKLIGHT
-    {5, S_O(backlight_timeout), 5, "backlight timeout", backlight_times_conf },
+    {5, S_O(backlight_timeout), 6, "backlight timeout", backlight_times_conf },
 #ifdef CONFIG_CHARGING
     {5, S_O(backlight_timeout_plugged), 11, "backlight timeout plugged",
         backlight_times_conf },
@@ -296,7 +295,7 @@ static const struct bit_entry rtc_bits[] =
     {6, S_O(remote_contrast), 42, "remote contrast", NULL },
     {1, S_O(remote_invert), false, "remote invert", off_on },
     {1, S_O(remote_flip_display), false, "remote flip display", off_on },
-    {5, S_O(remote_backlight_timeout), 5, "remote backlight timeout",
+    {5, S_O(remote_backlight_timeout), 6, "remote backlight timeout",
         backlight_times_conf },
 #ifdef CONFIG_CHARGING
     {5, S_O(remote_backlight_timeout_plugged), 11,
@@ -319,8 +318,7 @@ static const struct bit_entry rtc_bits[] =
     {1, S_O(remote_bl_filter_first_keypress), false,
             "backlight filters first remote keypress", off_on },
 #endif
-
-#endif
+#endif /* CONFIG_BACKLIGHT */
 
     /* new stuff to be added here */
     /* If values are just added to the end, no need to bump the version. */
@@ -574,6 +572,20 @@ static const struct bit_entry hd_bits[] =
     {1, S_O(warnon_erase_dynplaylist), false,
         "warn when erasing dynamic playlist", off_on },
 
+#ifdef CONFIG_BACKLIGHT
+#ifdef HAS_BUTTON_HOLD
+    {2, S_O(backlight_on_button_hold), 0, "backlight on button hold",
+        "normal,off,on" },
+#endif
+
+#ifdef HAVE_LCD_SLEEP
+    {4, S_O(lcd_sleep_after_backlight_off), 3,
+        "lcd sleep after backlight off",
+        "always,never,5,10,15,20,30,45,60,90" },
+#endif
+#endif /* CONFIG_BACKLIGHT */
+
+
     
     /* If values are just added to the end, no need to bump the version. */
     /* new stuff to be added at the end */
@@ -1183,7 +1195,13 @@ void settings_apply(void)
 #ifdef HAVE_REMOTE_LCD
     set_remote_backlight_filter_keypress(global_settings.remote_bl_filter_first_keypress);
 #endif
+#ifdef HAS_BUTTON_HOLD
+    backlight_set_on_button_hold(global_settings.backlight_on_button_hold);
+#endif
+#ifdef HAVE_LCD_SLEEP
+    lcd_set_sleep_after_backlight_off(global_settings.lcd_sleep_after_backlight_off);
 #endif
+#endif /* CONFIG_BACKLIGHT */
 }
 
 
diff --git a/apps/settings.h b/apps/settings.h
index 4ab290914a..369e1ebd6b 100644
--- a/apps/settings.h
+++ b/apps/settings.h
@@ -531,7 +531,13 @@ struct user_settings
 #ifdef HAVE_REMOTE_LCD
     bool remote_bl_filter_first_keypress; /* filter first remote keypress when remote dark? */
 #endif
+#ifdef HAS_BUTTON_HOLD
+    int backlight_on_button_hold;
 #endif
+#ifdef HAVE_LCD_SLEEP
+    int lcd_sleep_after_backlight_off;
+#endif
+#endif /* CONFIG_BACKLIGHT */
 
 #ifdef HAVE_LCD_BITMAP
     unsigned char kbd_file[MAX_FILENAME+1]; /* last keyboard */
diff --git a/apps/settings_menu.c b/apps/settings_menu.c
index 20235f5a72..93c809d007 100644
--- a/apps/settings_menu.c
+++ b/apps/settings_menu.c
@@ -196,6 +196,21 @@ static bool backlight_timer(void)
                       backlight_set_timeout );
 }
 
+#ifdef HAS_BUTTON_HOLD
+static bool backlight_on_button_hold(void)
+{
+    static const struct opt_items names[3] = {
+        { STR(LANG_BACKLIGHT_ON_BUTTON_HOLD_NORMAL) },
+        { STR(LANG_OFF) },
+        { STR(LANG_ON) },
+    };
+    return set_option(str(LANG_BACKLIGHT_ON_BUTTON_HOLD),
+                      &global_settings.backlight_on_button_hold,
+                      INT, names, 3,
+                      backlight_set_on_button_hold);
+}
+#endif /* HAS_BUTTON_HOLD */
+
 #if defined(HAVE_BACKLIGHT_PWM_FADING) && !defined(SIMULATOR)
 static bool backlight_fade_in(void)
 {
@@ -227,6 +242,29 @@ static bool backlight_fade_out(void)
                       INT, names, 8, backlight_set_fade_out );
 }
 #endif
+
+#ifdef HAVE_LCD_SLEEP
+static bool lcd_sleep_after_backlight_off(void)
+{
+    static const struct opt_items names[] = {
+        { STR(LANG_ALWAYS) },
+        { STR(LANG_NEVER) },
+        { (unsigned char *)"5s", TALK_ID(5, UNIT_SEC) },
+        { (unsigned char *)"10s", TALK_ID(10, UNIT_SEC) },
+        { (unsigned char *)"15s", TALK_ID(15, UNIT_SEC) },
+        { (unsigned char *)"20s", TALK_ID(20, UNIT_SEC) },
+        { (unsigned char *)"30s", TALK_ID(30, UNIT_SEC) },
+        { (unsigned char *)"45s", TALK_ID(45, UNIT_SEC) },
+        { (unsigned char *)"60s", TALK_ID(60, UNIT_SEC) },
+        { (unsigned char *)"90s", TALK_ID(90, UNIT_SEC) },
+    };
+
+    return set_option(str(LANG_LCD_SLEEP_AFTER_BACKLIGHT_OFF),
+                      &global_settings.lcd_sleep_after_backlight_off,
+                      INT, names, 10,
+                      lcd_set_sleep_after_backlight_off );
+}
+#endif /* HAVE_LCD_SLEEP */
 #endif /* CONFIG_BACKLIGHT */
 
 #ifdef HAVE_BACKLIGHT_BRIGHTNESS
@@ -257,7 +295,7 @@ static bool remote_backlight_timer_plugged(void)
                       INT, backlight_timeouts, 19,
                       remote_backlight_set_timeout_plugged );
 }
-#endif
+#endif /* HAVE_REMOTE_LCD */
 
 static bool remote_caption_backlight(void)
 {
@@ -1771,12 +1809,18 @@ static bool lcd_settings_menu(void)
 #ifdef CONFIG_CHARGING
         { ID2P(LANG_BACKLIGHT_ON_WHEN_CHARGING), backlight_timer_plugged },
 #endif
+#ifdef HAS_BUTTON_HOLD
+        { ID2P(LANG_BACKLIGHT_ON_BUTTON_HOLD), backlight_on_button_hold },
+#endif
         { ID2P(LANG_CAPTION_BACKLIGHT), caption_backlight },
 #if defined(HAVE_BACKLIGHT_PWM_FADING) && !defined(SIMULATOR)
         { ID2P(LANG_BACKLIGHT_FADE_IN), backlight_fade_in },
         { ID2P(LANG_BACKLIGHT_FADE_OUT), backlight_fade_out },
 #endif
         { ID2P(LANG_BACKLIGHT_FILTER_FIRST_KEYPRESS), set_bl_filter_first_keypress },
+#ifdef HAVE_LCD_SLEEP
+        { ID2P(LANG_LCD_SLEEP_AFTER_BACKLIGHT_OFF), lcd_sleep_after_backlight_off },
+#endif
 #ifdef HAVE_BACKLIGHT_BRIGHTNESS
         { ID2P(LANG_BRIGHTNESS),      brightness },
 #endif
diff --git a/firmware/backlight.c b/firmware/backlight.c
index cd1fbaf1ba..f35e87a81d 100644
--- a/firmware/backlight.c
+++ b/firmware/backlight.c
@@ -27,15 +27,16 @@
 #include "usb.h"
 #include "power.h"
 #include "system.h"
+#include "button.h"
 #include "timer.h"
 #include "backlight.h"
 
 #ifdef HAVE_BACKLIGHT_BRIGHTNESS
-#include "pcf50606.h" /* iRiver brightness */
+#include "pcf50606.h" /* iRiver, iAudio X5 brightness */
 #endif
- 
-#if (CONFIG_BACKLIGHT == BL_IRIVER_H300)
-#include "lcd.h" /* for lcd_enable() */
+
+#if defined(HAVE_LCD_ENABLE) || defined(HAVE_LCD_SLEEP)
+#include "lcd.h" /* for lcd_enable() and lcd_sleep() */
 #endif
 #ifdef HAVE_REMOTE_LCD
 #include "lcd-remote.h"
@@ -61,11 +62,11 @@ static inline void __backlight_off(void)
 static inline void __backlight_on(void)
 {
 #if CONFIG_BACKLIGHT == BL_IRIVER_H100
-    and_l(~0x00020000, &GPIO1_OUT);
+    and_l(~0x00020000, &GPIO1_OUT);    
 #elif CONFIG_BACKLIGHT == BL_IRIVER_H300
     lcd_enable(true);
     sleep(HZ/100); /* lcd needs time - avoid flashing for dark screens */
-    or_l(0x00020000, &GPIO1_OUT);    
+    or_l(0x00020000, &GPIO1_OUT);
 #elif CONFIG_BACKLIGHT == BL_RTC
     /* Enable square wave */
     rtc_write(0x0a, rtc_read(0x0a) | 0x40);
@@ -148,9 +149,17 @@ const signed char backlight_timeout_value[19] =
 #define REMOTE_BACKLIGHT_ON 3
 #define REMOTE_BACKLIGHT_OFF 4
 #define BACKLIGHT_UNBOOST_CPU 5
+#ifdef HAVE_LCD_SLEEP
+#define LCD_SLEEP 6
+#endif
 
 static void backlight_thread(void);
 static long backlight_stack[DEFAULT_STACK_SIZE/sizeof(long)];
+#ifdef X5_BACKLIGHT_SHUTDOWN
+#define BACKLIGHT_QUIT 256
+/* Need to save this for x5 shutdown */
+static int backlight_thread_id;
+#endif
 static const char backlight_thread_name[] = "backlight";
 static struct event_queue backlight_queue;
 
@@ -159,6 +168,9 @@ static int backlight_timeout = 5*HZ;
 #ifdef CONFIG_CHARGING
 static int backlight_timeout_plugged = 5*HZ;
 #endif
+#ifdef HAS_BUTTON_HOLD
+static int backlight_on_button_hold = 0;
+#endif
 
 #ifdef HAVE_REMOTE_LCD
 static int remote_backlight_timer;
@@ -168,6 +180,15 @@ static int remote_backlight_timeout_plugged = 5*HZ;
 #endif
 #endif
 
+#ifdef HAVE_LCD_SLEEP
+const signed char lcd_sleep_timeout_value[10] =
+{
+    -1, 0, 5, 10, 15, 20, 30, 45, 60, 90
+};
+static int lcd_sleep_timer;
+static int lcd_sleep_timeout = 10*HZ;
+#endif
+
 #if defined(HAVE_BACKLIGHT_PWM_FADING) && !defined(SIMULATOR)
 /* backlight fading */
 #define BL_PWM_INTERVAL 5000  /* Cycle interval in µs */
@@ -318,6 +339,9 @@ static void _backlight_on(void)
 #else
     __backlight_on();
 #endif
+#ifdef HAVE_LCD_SLEEP
+    lcd_sleep_timer = 0; /* LCD should be awake already */
+#endif
 }
 
 static void _backlight_off(void)
@@ -333,6 +357,17 @@ static void _backlight_off(void)
 #else
     __backlight_off();
 #endif
+
+#ifdef HAVE_LCD_SLEEP
+    /* Start LCD sleep countdown */
+    if (lcd_sleep_timeout < 0)
+    {
+        lcd_sleep_timer = 0; /* Setting == Always */
+        lcd_sleep();
+    }
+    else
+        lcd_sleep_timer = lcd_sleep_timeout;
+#endif
 }
 
 #ifdef HAVE_REMOTE_LCD
@@ -366,13 +401,79 @@ static void __remote_backlight_off(void)
 #endif
 }
 #endif /* TARGET_TREE */
-#endif /* HAVE_REMOTE_LCD */
 #endif /* SIMULATOR */
+#endif /* HAVE_REMOTE_LCD */
+
+/* Update state of backlight according to timeout setting */
+static void backlight_update_state(void)
+{
+#ifdef CONFIG_CHARGING
+    if (charger_inserted()
+#ifdef HAVE_USB_POWER
+            || usb_powered()
+#endif
+            )
+        backlight_timer = backlight_timeout_plugged;
+    else
+#endif
+        backlight_timer = backlight_timeout;
+
+    /* Backlight == OFF in the setting? */
+    if (backlight_timer < 0)
+    {
+        backlight_timer = 0; /* Disable the timeout */
+#ifdef HAS_BUTTON_HOLD
+        if (backlight_on_button_hold == 2 && button_hold())
+            return; /* Keep on if "On" */
+#endif
+        _backlight_off();
+    }
+    else
+    {
+#ifdef HAS_BUTTON_HOLD
+        if (backlight_on_button_hold == 1 && button_hold())
+        {
+            /* Keep off if "Off". */
+            backlight_timer = 0; /* Disable the timeout */
+            return;
+        }
+#endif
+        _backlight_on();
+    }
+}
+
+#ifdef HAVE_REMOTE_LCD
+/* Update state of remote backlight according to timeout setting */
+static void remote_backlight_update_state(void)
+{
+#ifdef CONFIG_CHARGING
+    if (charger_inserted()
+#ifdef HAVE_USB_POWER
+            || usb_powered()
+#endif
+            )
+        remote_backlight_timer = remote_backlight_timeout_plugged;
+    else
+#endif
+        remote_backlight_timer = remote_backlight_timeout;
+
+    /* Backlight == OFF in the setting? */
+    if (remote_backlight_timer < 0)
+    {
+        remote_backlight_timer = 0; /* Disable the timeout */
+        __remote_backlight_off();
+    }
+    else
+    {
+        __remote_backlight_on();
+    }
+}
+#endif /* HAVE_REMOTE_LCD */
 
 void backlight_thread(void)
 {
     struct event ev;
-    
+
     while(1)
     {
         queue_wait(&backlight_queue, &ev);
@@ -380,61 +481,39 @@ void backlight_thread(void)
         {
 #ifdef HAVE_REMOTE_LCD
             case REMOTE_BACKLIGHT_ON:
-#ifdef CONFIG_CHARGING
-                if (charger_inserted()
-#ifdef HAVE_USB_POWER
-                        || usb_powered()
-#endif
-                        )
-                    remote_backlight_timer = remote_backlight_timeout_plugged;
-                else
-#endif
-                    remote_backlight_timer = remote_backlight_timeout;
-
-                /* Backlight == OFF in the setting? */
-                if (remote_backlight_timer < 0)
-                {
-                    remote_backlight_timer = 0; /* Disable the timeout */
-                    __remote_backlight_off();
-                }
-                else 
-                {
-                    __remote_backlight_on();
-                }
+                remote_backlight_update_state();
                 break;
 
             case REMOTE_BACKLIGHT_OFF:
+                remote_backlight_timer = 0; /* Disable the timeout */
                 __remote_backlight_off();
                 break;
-                
-#endif /* HAVE_REMOTE_LCD */
+#endif
             case BACKLIGHT_ON:
-#ifdef CONFIG_CHARGING
-                if (charger_inserted()
-#ifdef HAVE_USB_POWER
-                        || usb_powered()
-#endif
-                        )
-                    backlight_timer = backlight_timeout_plugged;
-                else
-#endif
-                    backlight_timer = backlight_timeout;
-
-                if (backlight_timer < 0) /* Backlight == OFF in the setting? */
-                {
-                    backlight_timer = 0; /* Disable the timeout */
-                    _backlight_off();
-                }
-                else 
-                {
-                    _backlight_on();
-                }
+                backlight_update_state();
                 break;
-                
+
             case BACKLIGHT_OFF:
+                backlight_timer = 0; /* Disable the timeout */
+#ifdef HAS_BUTTON_HOLD
+                if (backlight_on_button_hold == 2 && button_hold())
+                    break; /* Keep on if "On" */
+#endif
                 _backlight_off();
                 break;
 
+#ifdef HAVE_LCD_SLEEP
+            case LCD_SLEEP:
+                lcd_sleep();
+                break;
+#endif
+
+#ifdef X5_BACKLIGHT_SHUTDOWN
+            case BACKLIGHT_QUIT:
+                remove_thread(backlight_thread_id);
+                break;
+#endif
+
 #if defined(HAVE_BACKLIGHT_PWM_FADING) && defined(CPU_COLDFIRE) \
     && !defined(SIMULATOR)
             case BACKLIGHT_UNBOOST_CPU:
@@ -451,8 +530,8 @@ void backlight_thread(void)
             case SYS_USB_DISCONNECTED:
                 usb_acknowledge(SYS_USB_DISCONNECTED_ACK);
                 break;
-        }
-    }
+        } /* end switch */
+    } /* end while */
 }
 
 static void backlight_tick(void)
@@ -483,6 +562,18 @@ static void backlight_tick(void)
             backlight_off();
         }
     }
+#ifdef HAVE_LCD_SLEEP
+    else if(lcd_sleep_timer)
+    {
+        lcd_sleep_timer--;
+        if(lcd_sleep_timer == 0)
+        {
+            /* Queue on bl thread or freeze! */
+            queue_post(&backlight_queue, LCD_SLEEP, NULL);
+        }
+    }
+#endif /* HAVE_LCD_SLEEP */
+
 #ifdef HAVE_REMOTE_LCD
     if(remote_backlight_timer)
     {
@@ -492,12 +583,15 @@ static void backlight_tick(void)
             remote_backlight_off();
         }
     }
-#endif
+#endif /* HAVE_REMOVE_LCD */
 }
 
 void backlight_init(void)
 {
     queue_init(&backlight_queue);
+#ifdef X5_BACKLIGHT_SHUTDOWN
+    backlight_thread_id =
+#endif
     create_thread(backlight_thread, backlight_stack,
                   sizeof(backlight_stack), backlight_thread_name);
     tick_add_task(backlight_tick);
@@ -516,13 +610,26 @@ void backlight_init(void)
     or_b(0x40, &PAIORH); /* ..and output */
 #elif CONFIG_BACKLIGHT == BL_GMINI
     P1CON |= 0x10; /* P1.4 C-MOS output mode */
-#endif    
+#endif
     backlight_on();
 #ifdef HAVE_REMOTE_LCD
     remote_backlight_on();
 #endif
 }
 
+#ifdef X5_BACKLIGHT_SHUTDOWN
+void x5_backlight_shutdown(void)
+{
+    /* Turn on the screen and don't let anyone else mess with it. Called
+       from clean_shutdown in misc.c. */
+    queue_empty(&backlight_queue);
+    tick_remove_task(backlight_tick);
+    /* Next time the thread runs, if at all, it will just remove itself. */
+    queue_post(&backlight_queue, BACKLIGHT_QUIT, NULL);
+    __backlight_on();
+}
+#endif /* X5_BACKLIGHT_SHUTDOWN */
+
 void backlight_on(void)
 {
     queue_post(&backlight_queue, BACKLIGHT_ON, NULL);
@@ -562,22 +669,72 @@ int backlight_get_current_timeout(void)
 void backlight_set_timeout(int index)
 {
     if((unsigned)index >= sizeof(backlight_timeout_value))
-        /* if given a weird value, use 0 */
-        index=0;
+        /* if given a weird value, use default */
+        index = 6;
     backlight_timeout = HZ * backlight_timeout_value[index];
-    backlight_on();
+    backlight_update_state();
 }
 
 #ifdef CONFIG_CHARGING
 void backlight_set_timeout_plugged(int index)
 {
     if((unsigned)index >= sizeof(backlight_timeout_value))
-        /* if given a weird value, use 0 */
-        index=0;
+        /* if given a weird value, use default */
+        index = 6;
     backlight_timeout_plugged = HZ * backlight_timeout_value[index];
-    backlight_on();
+    backlight_update_state();
 }
-#endif
+#endif /* CONFIG_CHARGING */
+
+#ifdef HAS_BUTTON_HOLD
+/* Hold button change event handler. */
+void backlight_hold_changed(bool hold_button)
+{
+    /* Hold switch overrides all backlight behavior except when
+       set to "Normal" */
+    /* Queue or freeze */
+    if (hold_button && backlight_on_button_hold == 1)
+    {
+        backlight_off(); /* setting == Off */
+    }
+    else  /* setting == On, Normal, no hold button, or anything else */
+        backlight_on();
+}
+
+void backlight_set_on_button_hold(int index)
+{
+    if ((unsigned)index >= 3)
+        /* if given a weird value, use default */
+        index = 0;
+
+    if (index == backlight_on_button_hold)
+        return;
+
+    backlight_on_button_hold = index;
+    backlight_hold_changed(button_hold());
+}
+#endif /* HAS_BUTTON_HOLD */
+
+#ifdef HAVE_LCD_SLEEP
+void lcd_set_sleep_after_backlight_off(int index)
+{
+    if ((unsigned)index >= sizeof(lcd_sleep_timeout_value))
+        /* if given a weird value, use default */
+        index = 3;
+
+    lcd_sleep_timeout = HZ * lcd_sleep_timeout_value[index];
+
+    if (backlight_timer > 0 || backlight_get_current_timeout() == 0)
+        /* Timer will be set when bl turns off or bl set to on. */
+        return;
+
+    /* Backlight is Off */
+    if (lcd_sleep_timeout < 0)
+        lcd_sleep_timer = 1; /* Always - sleep next tick */
+    else
+        lcd_sleep_timer = lcd_sleep_timeout; /* Never, other */
+}
+#endif /* HAVE_LCD_SLEEP */
 
 #ifdef HAVE_REMOTE_LCD
 void remote_backlight_on(void)
@@ -593,22 +750,22 @@ void remote_backlight_off(void)
 void remote_backlight_set_timeout(int index)
 {
     if((unsigned)index >= sizeof(backlight_timeout_value))
-        /* if given a weird value, use 0 */
-        index=0;
+        /* if given a weird value, use default */
+        index=6;
     remote_backlight_timeout = HZ * backlight_timeout_value[index];
-    remote_backlight_on();
+    remote_backlight_update_state();
 }
 
 #ifdef CONFIG_CHARGING
 void remote_backlight_set_timeout_plugged(int index)
 {
     if((unsigned)index >= sizeof(backlight_timeout_value))
-        /* if given a weird value, use 0 */
-        index=0;
+        /* if given a weird value, use default */
+        index=6;
     remote_backlight_timeout_plugged = HZ * backlight_timeout_value[index];
-    remote_backlight_on();
+    remote_backlight_update_state();
 }
-#endif
+#endif /* CONFIG_CHARGING */
 
 /* return value in ticks; 0 means always on, <0 means always off */
 int remote_backlight_get_current_timeout(void)
@@ -639,7 +796,8 @@ bool is_remote_backlight_on(void)
 
 #endif /* HAVE_REMOTE_LCD */
 
-#else /* no backlight, empty dummy functions */
+#else /* !defined(CONFIG_BACKLIGHT) || defined(BOOTLOADER)
+    -- no backlight, empty dummy functions */
 
 #if defined(BOOTLOADER) && defined(CONFIG_BACKLIGHT)
 void backlight_init(void)
@@ -662,8 +820,10 @@ void remote_backlight_off(void) {}
 void remote_backlight_set_timeout(int index) {(void)index;}
 bool is_remote_backlight_on(void) {return true;}
 #endif
-#endif /* #ifdef CONFIG_BACKLIGHT */
+#endif /* defined(CONFIG_BACKLIGHT) && !defined(BOOTLOADER) */
 
+/* TODO: Move low level code to target/ tree. Create
+   __backlight_set_brightness and call from here. */
 #ifdef HAVE_BACKLIGHT_BRIGHTNESS
 #if defined(IRIVER_H300_SERIES) || defined(IAUDIO_X5)
 void backlight_set_brightness(int val)
@@ -692,5 +852,5 @@ void backlight_set_brightness(int val)
 #endif
 }
 #endif
-#endif
+#endif /* HAVE_BACKLIGHT_BRIGHTNESS */
 
diff --git a/firmware/drivers/button.c b/firmware/drivers/button.c
index d9f9ae2a6d..9ff0206a2b 100644
--- a/firmware/drivers/button.c
+++ b/firmware/drivers/button.c
@@ -844,21 +844,15 @@ static int button_read(void)
     static bool remote_hold_button = false;
     static int prev_data = 0xff;
     static int last_valid = 0xff;
+    bool hold_button_old;
 
-    /* light handling */
-    if (hold_button && !button_hold())
-    {
-        backlight_on();
-    }
-    if (remote_hold_button && !remote_button_hold_only())
-    {
-        remote_backlight_on();
-    }
-
+    /* normal buttons */
+    hold_button_old = hold_button;
     hold_button = button_hold();
-    remote_hold_button = remote_button_hold_only();
 
-    /* normal buttons */
+    if (hold_button != hold_button_old)
+        backlight_hold_changed(hold_button);
+
     if (!hold_button)
     {
         data = adc_scan(ADC_BUTTONS);
@@ -920,6 +914,13 @@ static int button_read(void)
     }
 
     /* remote buttons */
+    remote_hold_button = remote_button_hold_only();
+
+    if (remote_hold_button && !remote_button_hold_only())
+    {
+        remote_backlight_on();
+    }
+
     if (!remote_hold_button)
     {
         data = adc_scan(ADC_REMOTE);
@@ -1038,15 +1039,15 @@ static int button_read(void)
    
 #elif CONFIG_KEYPAD == IRIVER_IFP7XX_PAD
     static bool hold_button = false;
+    bool hold_button_old;
 
-    /* light handling */
-    if (hold_button && !button_hold())
-    {
-        backlight_on();
-    }
+    /* normal buttons */
+    hold_button_old = hold_button;
     hold_button = button_hold();
 
-    /* normal buttons */
+    if (hold_button != hold_button_old)
+        backlight_hold_changed(hold_button);
+
     if (!button_hold())
     {          
         data = adc_read(ADC_BUTTONS);
@@ -1217,28 +1218,30 @@ static int button_read(void)
 
 #elif (CONFIG_KEYPAD == IPOD_4G_PAD)
     static bool hold_button = false;
+    bool hold_button_old;
 
-    /* light handling */
-    if (hold_button && !button_hold())
-    {
-        backlight_on();
-    }
+    /* normal buttons */
+    hold_button_old = hold_button;
     hold_button = button_hold();
 
+    if (hold_button != hold_button_old)
+        backlight_hold_changed(hold_button);
+
     (void)data;
     /* The int_btn variable is set in the button interrupt handler */
     btn = int_btn;
 
 #elif (CONFIG_KEYPAD == IPOD_3G_PAD)
     static bool hold_button = false;
+    bool hold_button_old;
 
-    /* light handling */
-    if (hold_button && !button_hold())
-    {
-        backlight_on();
-    }
+    /* normal buttons */
+    hold_button_old = hold_button;
     hold_button = button_hold();
 
+    if (hold_button != hold_button_old)
+        backlight_hold_changed(hold_button);
+
     (void)data;
     btn = ipod_3g_button_read();
 
diff --git a/firmware/export/backlight.h b/firmware/export/backlight.h
index 09960b7537..62f024816f 100644
--- a/firmware/export/backlight.h
+++ b/firmware/export/backlight.h
@@ -27,6 +27,10 @@ void backlight_off(void);
 void backlight_set_timeout(int index);
 #ifdef CONFIG_BACKLIGHT
 void backlight_init(void);
+#if defined(IAUDIO_X5) && !defined(SIMULATOR)
+#define X5_BACKLIGHT_SHUTDOWN
+void x5_backlight_shutdown(void);
+#endif
 int  backlight_get_current_timeout(void);
 #ifdef HAVE_BACKLIGHT_PWM_FADING
 void backlight_set_fade_in(int index);
@@ -34,9 +38,17 @@ void backlight_set_fade_out(int index);
 #endif
 void backlight_set_timeout_plugged(int index);
 extern const signed char backlight_timeout_value[];
-#else
-#define backlight_init()
+#ifdef HAS_BUTTON_HOLD
+void backlight_set_on_button_hold(int index);
+void backlight_hold_changed(bool hold_button);
 #endif
+#ifdef HAVE_LCD_SLEEP
+void lcd_set_sleep_after_backlight_off(int index);
+extern const signed char lcd_sleep_timeout_value[];
+#endif
+#else /* ndef CONFIG_BACKLIGHT */
+#define backlight_init()
+#endif /* CONFIG_BACKLIGHT */
 
 #ifdef HAVE_REMOTE_LCD
 void remote_backlight_on(void);
diff --git a/firmware/export/config-h300.h b/firmware/export/config-h300.h
index e98b60c9fc..6d5fbabbdf 100644
--- a/firmware/export/config-h300.h
+++ b/firmware/export/config-h300.h
@@ -70,6 +70,9 @@
 
 #ifndef SIMULATOR
 
+/* Define this if your LCD can be enabled/disabled */
+#define HAVE_LCD_ENABLE
+
 /* Define this if you have a Motorola SCF5249 */
 #define CONFIG_CPU MCF5249
 
@@ -126,4 +129,4 @@
 /* define this if the unit can be powered or charged via USB */
 #define HAVE_USB_POWER
 
-#endif
+#endif /* SIMULATOR */
diff --git a/firmware/export/config-iaudiox5.h b/firmware/export/config-iaudiox5.h
index a828ca1ec9..afb5074662 100644
--- a/firmware/export/config-iaudiox5.h
+++ b/firmware/export/config-iaudiox5.h
@@ -23,7 +23,6 @@
 #define LCD_HEIGHT 128
 #define LCD_DEPTH  16   /* pseudo 262.144 colors */
 #define LCD_PIXELFORMAT RGB565 /* rgb565 */
-#define HAVE_LCD_CONTRAST
 
 /* remote LCD */
 #define LCD_REMOTE_WIDTH  128
@@ -47,7 +46,7 @@
 #define DEFAULT_CONTRAST_SETTING    19 /* Match boot contrast */
 
 /* Define this for LCD backlight available */
-#define CONFIG_BACKLIGHT BL_IRIVER_H100 /* port controlled !?!? */ 
+#define CONFIG_BACKLIGHT BL_X5 /* PCF50606 I2C */
 #define HAVE_BACKLIGHT_BRIGHTNESS
 
 /* Define this if you have a software controlled poweroff */
@@ -70,6 +69,16 @@
 
 #ifndef SIMULATOR
 
+/* Define this if your LCD can set contrast */
+#define HAVE_LCD_CONTRAST
+
+/* Define this if your LCD can be enabled/disabled */
+#define HAVE_LCD_ENABLE
+
+/* Define this if your LCD can be put to sleep. HAVE_LCD_ENABLE
+   should be defined as well. */
+#define HAVE_LCD_SLEEP
+
 /* Define this if you have a Motorola SCF5250 */
 #define CONFIG_CPU MCF5250
 
@@ -122,4 +131,4 @@
 #define BOOTFILE_EXT "iaudio"
 #define BOOTFILE "rockbox." BOOTFILE_EXT
 
-#endif
+#endif /* SIMULATOR */
diff --git a/firmware/export/config-ipod3g.h b/firmware/export/config-ipod3g.h
index d0d2d0664c..831ff40c1d 100644
--- a/firmware/export/config-ipod3g.h
+++ b/firmware/export/config-ipod3g.h
@@ -54,6 +54,9 @@
 
 #ifndef SIMULATOR
 
+/* Define this if your LCD can be enabled/disabled */
+#define HAVE_LCD_ENABLE
+
 /* Define this if you have a PortalPlayer PP5002 */
 #define CONFIG_CPU PP5002
 
@@ -103,4 +106,4 @@
 #define BOOTFILE_EXT "ipod"
 #define BOOTFILE "rockbox." BOOTFILE_EXT
 
-#endif
+#endif /* SIMULATOR */
diff --git a/firmware/export/config.h b/firmware/export/config.h
index 0dd1d25637..f00ac6498c 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -116,6 +116,7 @@
 #define BL_IPODMINI      10 /* Apple iPod Mini */
 #define BL_GIGABEAT      11 /* Toshiba Gigabeat */
 #define BL_IRIVER_H10    12 /* iriver H10 */
+#define BL_X5            13 /* iAudio X5 PCF50606 I2C */
 
 /* CONFIG_I2C */
 #define I2C_PLAYREC  0 /* Archos Player/Recorder style */
diff --git a/firmware/export/lcd.h b/firmware/export/lcd.h
index c3ad4f97d9..5c362c2b53 100644
--- a/firmware/export/lcd.h
+++ b/firmware/export/lcd.h
@@ -227,10 +227,17 @@ extern fb_data lcd_framebuffer[LCD_HEIGHT][LCD_WIDTH];
 #define LCD_FBWIDTH LCD_WIDTH
 #endif
 
-#if (CONFIG_BACKLIGHT==BL_IRIVER_H300) || (CONFIG_BACKLIGHT==BL_IPOD3G)
+/** Port-specific functions. Enable in port config file. **/
+#ifdef HAVE_LCD_ENABLE
+/* Enable/disable the main display. */
 extern void lcd_enable(bool on);
 #endif
 
+#ifdef HAVE_LCD_SLEEP
+/* Put the LCD into a power saving state deeper than lcd_enable(false). */
+extern void lcd_sleep(void);
+#endif
+
 /* Bitmap formats */
 enum
 {
diff --git a/firmware/powermgmt.c b/firmware/powermgmt.c
index a8e641e1ed..1be361a01d 100644
--- a/firmware/powermgmt.c
+++ b/firmware/powermgmt.c
@@ -1032,10 +1032,12 @@ void shutdown_hw(void)
 #elif defined(HAVE_WM8758) || defined(HAVE_WM8975)
     wmcodec_close();
 #endif
+#ifndef IAUDIO_X5
 #if defined(HAVE_BACKLIGHT_PWM_FADING) && !defined(SIMULATOR)
     backlight_set_fade_out(0);
 #endif
     backlight_off();
+#endif /* IAUDIO_X5 */
 #if defined(IPOD_ARCH) && defined(HAVE_LCD_COLOR)
     /* Clear the screen and backdrop to
     remove ghosting effect on shutdown */
diff --git a/firmware/target/coldfire/iaudio/x5/backlight-x5.c b/firmware/target/coldfire/iaudio/x5/backlight-x5.c
index 91c7224267..70a5d957b6 100755
--- a/firmware/target/coldfire/iaudio/x5/backlight-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/backlight-x5.c
@@ -21,10 +21,13 @@
 #include "system.h"
 #include "backlight.h"
 #include "pcf50606.h"
+#include "lcd.h"
 
 void __backlight_on(void)
 {
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level;
+    lcd_enable(true);
+    level = set_irq_level(HIGHEST_IRQ_LEVEL);
     pcf50606_write(0x38, 0xb0); /* Backlight ON, GPO1INV=1, GPO1ACT=011 */
     set_irq_level(level);
 }
@@ -34,6 +37,7 @@ void __backlight_off(void)
     int level = set_irq_level(HIGHEST_IRQ_LEVEL);
     pcf50606_write(0x38, 0x80); /* Backlight OFF, GPO1INV=1, GPO1ACT=000 */
     set_irq_level(level);
+    lcd_enable(false);
 }
 
 void __remote_backlight_on(void)
diff --git a/firmware/target/coldfire/iaudio/x5/button-x5.c b/firmware/target/coldfire/iaudio/x5/button-x5.c
index 07bc7bf3f7..b44909a975 100755
--- a/firmware/target/coldfire/iaudio/x5/button-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/button-x5.c
@@ -50,18 +50,16 @@ int button_read_device(void)
     int btn = BUTTON_NONE;
     static bool hold_button = false;
     static bool remote_hold_button = false;
+    bool hold_button_old;
 
-    /* backlight handling */
-    if (hold_button && !button_hold())
-    {
-        backlight_on();
-    }
-    /* TODO: add light handling for the remote */
-
+    /* normal buttons */
+    hold_button_old = hold_button;
     hold_button = button_hold();
-    remote_hold_button = remote_button_hold();
 
-    /* normal buttons */
+    /* give BL notice if HB state chaged */
+    if (hold_button != hold_button_old)
+        backlight_hold_changed(hold_button);
+
     if (!hold_button)
     {
         data = adc_scan(ADC_BUTTONS);
@@ -90,6 +88,11 @@ int button_read_device(void)
     }
 
     /* remote buttons */
+
+    /* TODO: add light handling for the remote */
+
+    remote_hold_button = remote_button_hold();
+
     data = adc_scan(ADC_REMOTE);
     if(data < 0x17)
         remote_hold_button = true;
diff --git a/firmware/target/coldfire/iaudio/x5/lcd-x5.c b/firmware/target/coldfire/iaudio/x5/lcd-x5.c
index 2e3e785d73..f66b8801dc 100755
--- a/firmware/target/coldfire/iaudio/x5/lcd-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/lcd-x5.c
@@ -30,14 +30,20 @@
 #include "font.h"
 #include "bidi.h"
 
+/** Initialized in lcd_init_device() **/
+/* Is the power turned on? */
+static bool power_on;
 /* Is the display turned on? */
-static bool display_on = false;
-
-/* Amount of vertical offset. Used for offset correction when flipped. */
-static int y_offset = 0;
-
+static bool display_on;
+/* Amount of vertical offset. Used for flip offset correction/detection. */
+static int y_offset;
 /* Amount of roll offset (0-127). */
-static int roll_offset = 0;
+static int roll_offset;
+/* Reverse flag. Must be remembered when display is turned off. */
+static unsigned short disp_control_rev;
+
+/* Forward declarations */
+static void lcd_display_off(void);
 
 /* A15(0x8000) && CS1->CS, A1(0x0002)->RS */
 #define LCD_CMD  *(volatile unsigned short *)0xf0008000
@@ -79,16 +85,16 @@ static int roll_offset = 0;
 #define R_GAMMA_AMP_ADJ_NEG     0x3b
 
 /* called very frequently - inline! */
-inline void lcd_write_reg(int reg, int val)
+static inline void lcd_write_reg(int reg, int val)
 {
     LCD_CMD = 0x0000; /* MSB is ~always~ 0 */
     LCD_CMD = reg << 1;
     LCD_DATA = (val >> 8) << 1;
-    LCD_DATA = (val & 0xff) << 1;
+    LCD_DATA = val << 1;
 }
 
 /* called very frequently - inline! */
-inline void lcd_begin_write_gram(void)
+static inline void lcd_begin_write_gram(void)
 {
     LCD_CMD = 0x0000;
     LCD_CMD = R_WRITE_DATA_2_GRAM << 1;
@@ -112,7 +118,7 @@ static inline void lcd_write_two(unsigned long px2)
     LCD_DATA = px2sr + (px2sr & 0x1F8);
     LCD_DATA = px2 << 1;
 }
- 
+
 /*** hardware configuration ***/
 
 int lcd_default_contrast(void)
@@ -135,29 +141,43 @@ void lcd_set_contrast(int val)
 
 void lcd_set_invert_display(bool yesno)
 {
+    if (yesno == (disp_control_rev == 0x0000))
+        return;
+
+    disp_control_rev = yesno ? 0x0000 : 0x0004;
+
+    if (!display_on)
+        return;
+
     /* PT1-0=00, VLE2-1=00, SPT=0, GON=1, DTE=1, REV=x, D1-0=11 */
-    lcd_write_reg(R_DISP_CONTROL, yesno ? 0x0033 : 0x0037);
+    lcd_write_reg(R_DISP_CONTROL, 0x0033 | disp_control_rev);
 }
 
 /* turn the display upside down (call lcd_update() afterwards) */
 void lcd_set_flip(bool yesno)
 {
+    if (yesno == (y_offset != 0))
+        return;
+
     y_offset = yesno ? 4 : 0;
-    /* SCN4-0=000x0 (G160) */
+
+    if (!power_on)
+        return;
+
+    /* SCN4-0=000x0 (G1/G160) */
     lcd_write_reg(R_GATE_SCAN_START_POS, yesno ? 0x0000 : 0x0002);
     /* SM=0, GS=x, SS=x, NL4-0=10011 (G1-G160)*/
     lcd_write_reg(R_DRV_OUTPUT_CONTROL, yesno ? 0x0013 : 0x0313);
-    /* Vertical stripe */
-    /* HEA7-0=0xxx, HSA7-0=0xxx */  
-    lcd_write_reg(R_HORIZ_RAM_ADDR_POS, 0x7f00 + (y_offset << 8) + y_offset);
+    /* HEA7-0=0xxx, HSA7-0=0xxx */
+    lcd_write_reg(R_HORIZ_RAM_ADDR_POS, yesno ? 0x8304 : 0x7f00);
 }
 
 /* Rolls up the lcd display by the specified amount of lines.
  * Lines that are rolled out over the top of the screen are
- * rolled in from the bottom again. This is a hardware 
+ * rolled in from the bottom again. This is a hardware
  * remapping only and all operations on the lcd are affected.
- * -> 
- * @param int lines - The number of lines that are rolled. 
+ * ->
+ * @param int lines - The number of lines that are rolled.
  *  The value must be 0 <= pixels < LCD_HEIGHT.
  * Call lcd_update() afterwards */
 void lcd_roll(int lines)
@@ -171,17 +191,11 @@ void lcd_roll(int lines)
     roll_offset = lines;
 }
 
-/* LCD init */
-void lcd_init_device(void)
+static void lcd_power_on(void)
 {
-    /* LCD Reset */
-    and_l(~0x00000010, &GPIO1_OUT);
-    or_l(0x00000010, &GPIO1_ENABLE);
-    or_l(0x00000010, &GPIO1_FUNCTION);
-    sleep(HZ/100);
-    or_l(0x00000010, &GPIO1_OUT);
-
-    sleep(HZ/100);
+    /* Be sure standby bit is clear. */
+    /* BT2-0=000, DC2-0=000, AP2-0=000, SLP=0, STB=0 */
+    lcd_write_reg(R_POWER_CONTROL1, 0x0000);
 
     /** Power ON Sequence **/
     /* Per datasheet Rev.1.10, Jun.21.2003, p. 99 */
@@ -212,13 +226,13 @@ void lcd_init_device(void)
     /* Instruction (4) for power setting; PON = "1" */
     /* VRL3-0=0100, PON=1, VRH3-0=0001 */
     lcd_write_reg(R_POWER_CONTROL4, 0x0411);
-    
+
     /* 40ms or more; time for step-up circuit 4 to stabilize */
     sleep(HZ/25);
 
-    /* Instructions for other mode settings (in register order). */    
-    /* SM=0, GS=1, SS=1, NL4-0=10011 (G1-G160)*/
-    lcd_write_reg(R_DRV_OUTPUT_CONTROL, 0x313);
+    /* Instructions for other mode settings (in register order). */
+    /* SM=0, GS=x, SS=x, NL4-0=10011 (G1-G160)*/
+    lcd_write_reg(R_DRV_OUTPUT_CONTROL, y_offset ? 0x0013 : 0x0313);
     /* FLD1-0=01 (1 field), B/C=1, EOR=1 (C-pat), NW5-0=000000 (1 row) */
     lcd_write_reg(R_DRV_AC_CONTROL, 0x0700);
     /* DIT=1, BGR=1, HWM=0, I/D1-0=11, AM=1, LG2-0=000 */
@@ -227,16 +241,16 @@ void lcd_init_device(void)
     lcd_write_reg(R_COMPARE_REG, 0x0000);
     /* NO1-0=01, SDT1-0=00, EQ1-0=00, DIV1-0=00, RTN3-00000 */
     lcd_write_reg(R_FRAME_CYCLE_CONTROL, 0x4000);
-    /* SCN4-0=00010 (G160) */
-    lcd_write_reg(R_GATE_SCAN_START_POS, 0x0002);
+    /* SCN4-0=000x0 (G1/G160) */
+    lcd_write_reg(R_GATE_SCAN_START_POS, y_offset ? 0x0000 : 0x0002);
     /* VL7-0=0x00 */
     lcd_write_reg(R_VERT_SCROLL_CONTROL, 0x0000);
     /* SE17-10(End)=0x9f (159), SS17-10(Start)=0x00 */
     lcd_write_reg(R_1ST_SCR_DRV_POS, 0x9f00);
     /* SE27-20(End)=0x5c (92), SS27-20(Start)=0x00 */
     lcd_write_reg(R_2ND_SCR_DRV_POS, 0x5c00);
-    /* HEA7-0=0x7f, HSA7-0=0x00 */
-    lcd_write_reg(R_HORIZ_RAM_ADDR_POS, 0x7f00); /* Vertical stripe */
+    /* HEA7-0=0xxx, HSA7-0=0xxx */
+    lcd_write_reg(R_HORIZ_RAM_ADDR_POS, y_offset ? 0x8304 : 0x7f00);
     /* PKP12-10=0x0, PKP02-00=0x0 */
     lcd_write_reg(R_GAMMA_FINE_ADJ_POS1, 0x0003);
     /* PKP32-30=0x4, PKP22-20=0x0 */
@@ -261,32 +275,135 @@ void lcd_init_device(void)
     /* 100ms or more; time for step-up circuits to stabilize */
     sleep(HZ/10);
 
+    power_on = true;
+}
+
+static void lcd_power_off(void)
+{
+    /* Display must be off first */
+    if (display_on)
+        lcd_display_off();
+
+    power_on = false;
+
+    /** Power OFF sequence **/
+    /* Per datasheet Rev.1.10, Jun.21.2003, p. 99 */
+
+    /* Step-up1 halt setting bit */
+    /* BT2-0=110, DC2-0=001, AP2-0=011, SLP=0, STB=0 */
+    lcd_write_reg(R_POWER_CONTROL1, 0x062c);
+    /* Step-up3,4 halt setting bit */
+    /* VRL3-0=0100, PON=0, VRH3-0=0001 */
+    lcd_write_reg(R_POWER_CONTROL4, 0x0401);
+    /* VCOMG=0, VDV4-0=10011, VCM4-0=11000 */
+    lcd_write_reg(R_POWER_CONTROL5, 0x1318);
+
+    /* Wait 100ms or more */
+    sleep(HZ/10);
+
+    /* Step-up2,amp halt setting bit */
+    /* BT2-0=000, DC2-0=000, AP2-0=000, SLP=0, STB=0 */
+    lcd_write_reg(R_POWER_CONTROL1, 0x0000);
+}
+
+static void lcd_display_on(void)
+{
+    /* Be sure power is on first */
+    if (!power_on)
+        lcd_power_on();
+
     /** Display ON Sequence **/
     /* Per datasheet Rev.1.10, Jun.21.2003, p. 97 */
 
-    /* PT1-0=00, VLE2-1=00, SPT=0, GON=0, DTE=0, REV=1, D1-0=01 */
-    lcd_write_reg(R_DISP_CONTROL, 0x0005);
+    /* PT1-0=00, VLE2-1=00, SPT=0, GON=0, DTE=0, REV=0, D1-0=01 */
+    lcd_write_reg(R_DISP_CONTROL, 0x0001);
 
     sleep(HZ/25); /* Wait 2 frames or more */
 
-    /* PT1-0=00, VLE2-1=00, SPT=0, GON=1, DTE=0, REV=1, D1-0=01 */
-    lcd_write_reg(R_DISP_CONTROL, 0x0025);
-    /* PT1-0=00, VLE2-1=00, SPT=0, GON=1, DTE=0, REV=1, D1-0=11 */
-    lcd_write_reg(R_DISP_CONTROL, 0x0027);
+    /* PT1-0=00, VLE2-1=00, SPT=0, GON=1, DTE=0, REV=x, D1-0=01 */
+    lcd_write_reg(R_DISP_CONTROL, 0x0021 | disp_control_rev);
+    /* PT1-0=00, VLE2-1=00, SPT=0, GON=1, DTE=0, REV=x, D1-0=11 */
+    lcd_write_reg(R_DISP_CONTROL, 0x0023 | disp_control_rev);
 
     sleep(HZ/25); /* Wait 2 frames or more */
 
-    /* PT1-0=00, VLE2-1=00, SPT=0, GON=1, DTE=1, REV=1, D1-0=11 */
-    lcd_write_reg(R_DISP_CONTROL, 0x0037);
+    /* PT1-0=00, VLE2-1=00, SPT=0, GON=1, DTE=1, REV=x, D1-0=11 */
+    lcd_write_reg(R_DISP_CONTROL, 0x0033 | disp_control_rev);
 
     display_on = true;
+}
+
+static void lcd_display_off(void)
+{
+    display_on = false;
+
+    /** Display OFF sequence **/
+    /* Per datasheet Rev.1.10, Jun.21.2003, p. 97 */
+
+    /* EQ1-0=00 already */
+
+    /* PT1-0=00, VLE2-1=00, SPT=0, GON=1, DTE=1, REV=x, D1-0=10 */
+    lcd_write_reg(R_DISP_CONTROL, 0x0032 | disp_control_rev);
+
+    sleep(HZ/25); /* Wait 2 frames or more */
+
+    /* PT1-0=00, VLE2-1=00, SPT=0, GON=1, DTE=0, REV=x, D1-0=10 */
+    lcd_write_reg(R_DISP_CONTROL, 0x0022 | disp_control_rev);
+
+    sleep(HZ/25); /* Wait 2 frames or more */
+
+    /* PT1-0=00, VLE2-1=00, SPT=0, GON=0, DTE=0, REV=0, D1-0=00 */
+    lcd_write_reg(R_DISP_CONTROL, 0x0000);
+}
+
+/* LCD init */
+void lcd_init_device(void)
+{
+    /* Reset settings */
+    power_on = false;
+    display_on = false;
     y_offset = 0;
     roll_offset = 0;
+    disp_control_rev = 0x0004;
+
+    /* LCD Reset */
+    and_l(~0x00000010, &GPIO1_OUT);
+    or_l(0x00000010, &GPIO1_ENABLE);
+    or_l(0x00000010, &GPIO1_FUNCTION);
+    sleep(HZ/100);
+    or_l(0x00000010, &GPIO1_OUT);
+
+    sleep(HZ/100);
+
+    lcd_display_on();
 }
 
 void lcd_enable(bool on)
 {
-    display_on = on;
+    if (on == display_on)
+        return;
+
+    if (on)
+    {
+        lcd_display_on();
+        /* Probably out of sync and we don't wanna pepper the code with
+           lcd_update() calls for this. */
+        lcd_update();
+    }
+    else
+    {
+        lcd_display_off();
+    }
+}
+
+void lcd_sleep(void)
+{
+    if (power_on)
+        lcd_power_off();
+
+    /* Set standby mode */
+    /* BT2-0=000, DC2-0=000, AP2-0=000, SLP=0, STB=1 */
+    lcd_write_reg(R_POWER_CONTROL1, 0x0001);
 }
 
 /*** update functions ***/
@@ -303,7 +420,7 @@ void lcd_blit(const fb_data* data, int x, int by, int width,
     (void)width;
     (void)bheight;
     (void)stride;
-    /*if(display_on)*/ 
+    /*if(display_on)*/
 }
 
 
@@ -327,7 +444,7 @@ void lcd_update(void)
     lcd_begin_write_gram();
 
     ptr = (unsigned long *)lcd_framebuffer;
-    ptr_end = ptr + (LCD_WIDTH*LCD_HEIGHT >> 1);
+    ptr_end = ptr + LCD_WIDTH*LCD_HEIGHT/2;
 
     do
     {
@@ -375,8 +492,6 @@ void lcd_update_rect(int x, int y, int width, int height)
     if (y >= y_end)
         return; /* nothing left to do */
 
-    ptr = (unsigned long *)&lcd_framebuffer[y][x];
-
     /* Set start position and window */
     lcd_write_reg(R_RAM_ADDR_SET, (x << 8) |
         (((y + roll_offset) & 127) + y_offset));
@@ -384,44 +499,28 @@ void lcd_update_rect(int x, int y, int width, int height)
 
     lcd_begin_write_gram();
 
+    ptr = (unsigned long *)&lcd_framebuffer[y][x];
+
     /* Aligning source reads to long boundaries helps 2% - 3% with IRAM
        buffer. DK with DRAM. */
+    odd_lead = x & 1;
 
-    /* special case widths 1 and 2. */
-    switch (width)
+    if (odd_lead)
     {
-    case 1:
-        odd_lead = 1; /* odd_lead case writes pixels */
-        odd_trail = 0;
-        duff = 0; /* Squelch compiler warning. */
-        duff_end = ptr;
-        break;
-    case 2: /* Just read as long */
-        odd_lead = 0;
-        odd_trail = 0;
-        duff = 1;
-        duff_end = ptr + 1;
-        break;
-    default:
-        odd_lead = x & 1;
-
-        if (odd_lead)
-        {
-            duff = width - 1;
-            odd_trail = duff & 1;
-            duff >>= 1;
-        }
-        else
-        {
-            duff = width >> 1;
-            odd_trail = width & 1;
-        }
+        duff = width - 1;
+        odd_trail = duff & 1;
+        duff >>= 1;
+    }
+    else
+    {
+        duff = width >> 1;
+        odd_trail = width & 1;
+    }
 
-        duff_end = ptr + duff;
-#ifndef BOOTLOADER        
-        duff &= 7;
+    duff_end = ptr + duff;
+#ifndef BOOTLOADER
+    duff &= 7;
 #endif
-    } /* end switch */
 
     stride = LCD_WIDTH - width + odd_trail; /* See odd_trail below */
 
@@ -440,7 +539,7 @@ void lcd_update_rect(int x, int y, int width, int height)
             do
                 lcd_write_two(*ptr);
             while (++ptr < duff_end);
-#else           
+#else
             switch (duff)
             {
             do