M:Robe 500i: More LCD initialization, and beginnings of support for QVGA as well as VGA on the LCD. MPEGPlayer now works with reasonable performance on smaller videos, but YUV blitting persists after MPEGPlayer is left, some cleanup/changes to the initialization code. This should be functionally equivalent for the ZVM, but the #ifdef's may need to be added back for app.lds. Get the bootloader building again.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@20598 a1c6a512-1295-4272-9138-f99709370657

10 files changed, 239 insertions, 174 deletions

diff --git a/apps/plugins/plugin.lds b/apps/plugins/plugin.lds
index 69601d96a2..9c65f31779 100644
--- a/apps/plugins/plugin.lds
+++ b/apps/plugins/plugin.lds
@@ -36,7 +36,10 @@ OUTPUT_FORMAT(elf32-littlemips)
 #endif
 
 #if CONFIG_CPU==DM320 || CONFIG_CPU==S3C2440
-#include "cpu.h"
+#define LCD_BUFFER_SIZE  (LCD_WIDTH*LCD_HEIGHT*2)
+
+/* must be 16Kb (0x4000) aligned */
+#define TTB_SIZE         (0x4000) 
 #define DRAMSIZE (MEMORYSIZE * 0x100000) - STUBOFFSET - PLUGIN_BUFFER_SIZE - CODEC_SIZE - LCD_BUFFER_SIZE - TTB_SIZE
 #elif CONFIG_CPU==IMX31L
 #include "imx31l.h"
@@ -85,6 +88,7 @@ OUTPUT_FORMAT(elf32-littlemips)
 #elif CONFIG_CPU==DM320
 #define DRAMORIG 0x00900000 + STUBOFFSET
 #define IRAM DRAM
+/* The bit of IRAM that is available is used in the core */
 #define IRAMSIZE 0
 #elif defined(CPU_TCC780X) || defined(CPU_TCC77X)
 #define DRAMORIG 0x20000000
diff --git a/firmware/export/config-mrobe500.h b/firmware/export/config-mrobe500.h
index d5d31ffc41..25699bd005 100644
--- a/firmware/export/config-mrobe500.h
+++ b/firmware/export/config-mrobe500.h
@@ -41,6 +41,9 @@
 /* define this if you have a colour LCD */
 #define HAVE_LCD_COLOR
 
+/* define this if you want album art for this target */
+//#define HAVE_ALBUMART
+
 /* define this if you have access to the quickscreen */
 #define HAVE_QUICKSCREEN
 
@@ -58,14 +61,22 @@
 
 /* choose the lcd orientation. both work */
 /* #define CONFIG_ORIENTATION SCREEN_PORTRAIT */
-#define CONFIG_ORIENTATION SCREEN_LANDSCAPE
+#define CONFIG_ORIENTATION SCREEN_PORTRAIT
+
+#if 1
+#define NATIVE_MAX_WIDTH 480
+#define NATIVE_MAX_HEIGHT 640
+#else
+#define NATIVE_MAX_WIDTH 240
+#define NATIVE_MAX_HEIGHT 320
+#endif
 
 #if CONFIG_ORIENTATION == SCREEN_PORTRAIT
-#define LCD_WIDTH  480
-#define LCD_HEIGHT 640
+#define LCD_WIDTH  NATIVE_MAX_WIDTH
+#define LCD_HEIGHT NATIVE_MAX_HEIGHT
 #else
-#define LCD_WIDTH  640
-#define LCD_HEIGHT 480
+#define LCD_WIDTH  NATIVE_MAX_HEIGHT
+#define LCD_HEIGHT NATIVE_MAX_WIDTH
 #endif
 
 #define LCD_DEPTH  16   /* 65k colours */
@@ -80,10 +91,6 @@
    should be defined as well. */
 #define HAVE_LCD_SLEEP
 
-/* We don't use a setting but a fixed delay after the backlight has
- * turned off */
-#define LCD_SLEEP_TIMEOUT (5*HZ)
-
 /* remote LCD */
 //#define HAVE_REMOTE_LCD
 #define LCD_REMOTE_WIDTH  79
diff --git a/firmware/export/dm320.h b/firmware/export/dm320.h
index 3d932c69c1..d6599a6745 100644
--- a/firmware/export/dm320.h
+++ b/firmware/export/dm320.h
@@ -27,17 +27,14 @@
 #ifndef __DM320_H__
 #define __DM320_H__
 
-#define LCD_BUFFER_SIZE  (LCD_WIDTH*LCD_HEIGHT*2)
-#define TTB_SIZE         (0x4000)
-/* must be 16Kb (0x4000) aligned */
-#if 1
-#define MEM_END          0x00900000 + (MEM*0x00100000)
-#define TTB_BASE_ADDR    (MEM_END - TTB_SIZE)
-#else
-#define TTB_BASE_ADDR    (0x04900000 - TTB_SIZE)
+#if !defined(__ASSEMBLER__) && !defined(__LD__)
+/* These variables are created during linking (app/boot.lds) */
+extern unsigned long _lcdbuf;
+extern unsigned long _ttbstart;
 #endif
-#define TTB_BASE         ((unsigned long *)TTB_BASE_ADDR) /* End of memory */
-#define FRAME            ((short *) (TTB_BASE_ADDR - LCD_BUFFER_SIZE))  /* Right before TTB */
+
+#define TTB_BASE_ADDR    (_ttbstart) /* End of memory */
+#define FRAME            ((short *) (&_lcdbuf))  /* Right before TTB */
 
 #define PHY_IO_BASE      0x00030000
 #define DM320_REG(addr)  (*(volatile unsigned short *)(PHY_IO_BASE + (addr)))
diff --git a/firmware/rolo.c b/firmware/rolo.c
index 510720ed1a..2a352b4f72 100644
--- a/firmware/rolo.c
+++ b/firmware/rolo.c
@@ -172,7 +172,7 @@ void rolo_restart(const unsigned char* source, unsigned char* dest,
         "mov   pc, r0            \n"
     );
 
-#elif defined(CPU_TCC780X) || (CONFIG_CPU == S3C2440)
+#elif defined(CPU_TCC780X) || (CONFIG_CPU == S3C2440) || (CONFIG_CPU==DM320)
     /* Flush and invalidate caches */
     cpucache_invalidate();
 
diff --git a/firmware/target/arm/tms320dm320/app.lds b/firmware/target/arm/tms320dm320/app.lds
index 902e093633..bc00c3463b 100644
--- a/firmware/target/arm/tms320dm320/app.lds
+++ b/firmware/target/arm/tms320dm320/app.lds
@@ -15,7 +15,11 @@ STARTUP(target/arm/tms320dm320/crt0.o)
 #define STUBOFFSET 0
 #endif
 
-#include "cpu.h"
+#define LCD_BUFFER_SIZE  (LCD_WIDTH*LCD_HEIGHT*2)
+
+/* must be 16Kb (0x4000) aligned */
+#define TTB_SIZE         (0x4000) 
+
 #define DRAMSIZE (MEMORYSIZE * 0x100000) - STUBOFFSET - PLUGINSIZE - CODECSIZE - LCD_BUFFER_SIZE - TTB_SIZE
 
 #define DRAMORIG 0x00900000 + STUBOFFSET
@@ -28,6 +32,10 @@ STARTUP(target/arm/tms320dm320/crt0.o)
 /* Where the codec buffer ends, and the plugin buffer starts */
 #define ENDADDR (ENDAUDIOADDR + CODECSIZE)
 
+#define LCDBEGIN (ENDADDR + PLUGINSIZE)
+
+#define TTBBEGIN (LCDBEGIN + LCD_BUFFER_SIZE)
+
 MEMORY
 {
     DRAM : ORIGIN = DRAMORIG, LENGTH = DRAMSIZE
@@ -40,7 +48,6 @@ SECTIONS
     {
         loadaddress = .;
         _loadaddress = .;
-        . = ALIGN(0x200);
         *(.init.text)
         *(.text*)
         *(.glue_7)
@@ -55,20 +62,22 @@ SECTIONS
         *(.rodata.str1.1)
         *(.rodata.str1.4)
         . = ALIGN(0x4);
-
-        /* Pseudo-allocate the copies of the data sections */
-        _datacopy = .;
     } > DRAM
 
-    /* TRICK ALERT! For RAM execution, we put the .data section at the
-       same load address as the copy. Thus, we don't waste extra RAM
-       when we don't actually need the copy.  */
-    .data : AT ( _datacopy )
+    .data : 
     {
-        _datastart = .;
         *(.data*)
         . = ALIGN(0x4);
-        _dataend  = .;
+    } > DRAM
+    
+    .bss (NOLOAD) :
+    {
+        _edata = .;
+        *(.bss*)
+        *(.ibss*)
+        *(COMMON)
+        . = ALIGN(0x4);
+        _end = .;
     } > DRAM
 
     /DISCARD/ :
@@ -81,30 +90,22 @@ SECTIONS
         _vectorsstart = .;
         *(.vectors);
         _vectorsend = .;
-#ifndef CREATIVE_ZVx
     } > IRAM AT> DRAM
 
     _vectorscopy = LOADADDR(.vectors);
-#else
-    } > IRAM
-#endif
 
     .iram :
     {
         _iramstart = .;
         *(.icode)
-        *(.irodata)
+        *(.irodata*)
         *(.idata)
         . = ALIGN(0x4);
         _iramend = .;
-#ifndef CREATIVE_ZVx
     } > IRAM AT> DRAM
 
     _iramcopy = LOADADDR(.iram);
-#else
-    } > IRAM
-#endif
-
+    
     .ibss (NOLOAD) :
     {
         _iedata = .;
@@ -113,49 +114,49 @@ SECTIONS
         _iend = .;
     } > IRAM
 
-    .stack :
+    .stack (NOLOAD) :
     {
        *(.stack)
        stackbegin = .;
        . += 0x2000;
        stackend = .;
     } > IRAM
+    
+    . = ADDR(.bss) + SIZEOF(.bss) + SIZEOF(.vectors) + SIZEOF(.iram);
 
-#ifndef CREATIVE_ZVx
-    .bss ADDR(.data) + SIZEOF(.data) + SIZEOF(.iram) + SIZEOF(.vectors):
-#else
-    .bss ADDR(.data) + SIZEOF(.data):
-#endif
-    {
-       _edata = .;
-        *(.bss*)
-        *(COMMON)
-        . = ALIGN(0x4);
-       _end = .;
-    } > DRAM
-
-    .audiobuf ALIGN(4) :
+    .audiobuf (NOLOAD) :
     {
+        . = ALIGN(4);
         _audiobuffer = .;
         audiobuffer = .;
     } > DRAM
 
-    .audiobufend ENDAUDIOADDR:
+    .audiobufend ENDAUDIOADDR (NOLOAD) :
     {
         audiobufend = .;
         _audiobufend = .;
     } > DRAM
 
-    .codec ENDAUDIOADDR:
+    .codec ENDAUDIOADDR (NOLOAD) :
     {
         codecbuf = .;
         _codecbuf = .;
     }
 
-    .plugin ENDADDR:
+    .plugin ENDADDR (NOLOAD) :
     {
         _pluginbuf = .;
         pluginbuf = .;
     }
+    
+    .lcdbuffer LCDBEGIN (NOLOAD) :
+    {
+        _lcdbuf = .;
+    }
+    
+    .ttbtable TTBBEGIN (NOLOAD) :
+    {
+        _ttbstart = .;
+    }
 }
 
diff --git a/firmware/target/arm/tms320dm320/boot.lds b/firmware/target/arm/tms320dm320/boot.lds
index 97274a2c04..e391526dc5 100644
--- a/firmware/target/arm/tms320dm320/boot.lds
+++ b/firmware/target/arm/tms320dm320/boot.lds
@@ -1,11 +1,17 @@
 #include "config.h"
 
 ENTRY(start)
+
 OUTPUT_FORMAT(elf32-littlearm)
 OUTPUT_ARCH(arm)
 STARTUP(target/arm/tms320dm320/crt0.o)
 
-#define DRAMSIZE (MEMORYSIZE * 0x100000)
+#define LCD_BUFFER_SIZE  (LCD_WIDTH*LCD_HEIGHT*2)
+
+/* must be 16Kb (0x4000) aligned */
+#define TTB_SIZE         (0x4000)
+
+#define DRAMSIZE (MEMORYSIZE * 0x100000) - TTB_SIZE - LCD_BUFFER_SIZE
 
 #define DRAMORIG 0x01900000 /* actually it's 0x00900000 */
 #define IRAMORIG 0x00000000
@@ -13,6 +19,12 @@ STARTUP(target/arm/tms320dm320/crt0.o)
 #define FLASHORIG 0x00100000
 #define FLASHSIZE 8M
 
+/* Now we have the LCD buffer */
+#define LCDBEGIN (DRAMSIZE+0x00900000)
+
+/* Finally the TTB table */
+#define TTBBEGIN (LCDBEGIN + LCD_BUFFER_SIZE)
+
 MEMORY
 {
    DRAM : ORIGIN = DRAMORIG, LENGTH = DRAMSIZE
@@ -28,6 +40,7 @@ SECTIONS
         loadaddress = .;
         _loadaddress = .;
         *(.init.text)
+        *(.icode)
         *(.text*)
         *(.glue_7)
         *(.glue_7t)
@@ -40,6 +53,7 @@ SECTIONS
         *(.rodata*)
         *(.rodata.str1.1)
         *(.rodata.str1.4)
+        *(.irodata)
         . = ALIGN(0x4);
 
         /* Pseudo-allocate the copies of the data sections */
@@ -47,8 +61,6 @@ SECTIONS
     } > DRAM
 
     .data : {
-        *(.icode)
-        *(.irodata)
         *(.idata)
         *(.data*)
         . = ALIGN(0x4);
@@ -84,4 +96,14 @@ SECTIONS
         _vectorsend = .;
     } AT > DRAM
     _vectorscopy = LOADADDR(.vectors);
+    
+    .lcdbuffer LCDBEGIN (NOLOAD) :
+    {
+        _lcdbuf = .;
+    }
+    
+    .ttbtable TTBBEGIN (NOLOAD) :
+    {
+        _ttbstart = .;
+    }
 }
diff --git a/firmware/target/arm/tms320dm320/crt0.S b/firmware/target/arm/tms320dm320/crt0.S
index 9375de7d0d..a5a965b9c2 100755
--- a/firmware/target/arm/tms320dm320/crt0.S
+++ b/firmware/target/arm/tms320dm320/crt0.S
@@ -30,20 +30,6 @@
 
     .global    start
 start:
-    .equ    INTC_IRQ0,    0x00030508
-    .equ    INTC_IRQ1,    0x0003050A
-    .equ    INTC_IRQ2,    0x0003050C
-    .equ    INTC_FIQ0,    0x00030500
-    .equ    INTC_FIQ1,    0x00030502
-    .equ    INTC_FIQ2,    0x00030504
-    .equ    INTC_EINT0,   0x00030528
-    .equ    INTC_EINT1,   0x0003052A
-    .equ    INTC_EINT2,   0x0003052C
-    .equ    INTC_FISEL0,  0x00030520
-    .equ    INTC_FISEL1,  0x00030522
-    .equ    INTC_FISEL2,  0x00030524
-    .equ    INTC_MASK,    0xFFFFFFFF
-
     msr    cpsr, #0xd3 /* enter supervisor mode, disable IRQ/FIQ */
 
 #ifndef CREATIVE_ZVx
@@ -68,37 +54,6 @@ start:
     orr    r0, r0, #0x00000002
     mcr    p15, 0, r0, c1, c0, 0
 
-#if 0
-    /* mask interrupts */
-    ldr    r1, =INTC_MASK
-    ldr    r2, =INTC_IRQ0
-    strh   r1, [r2]
-    ldr    r2, =INTC_IRQ1
-    strh   r1, [r2]
-    ldr    r2, =INTC_IRQ2
-    strh   r1, [r2]
-    ldr    r2, =INTC_FIQ0
-    strh   r1, [r2]
-    ldr    r2, =INTC_FIQ1
-    strh   r1, [r2]
-    ldr    r2, =INTC_FIQ2
-    strh   r1, [r2]
-
-    mov    r1, #0
-    ldr    r2, =INTC_EINT0
-    strh   r1, [r2]
-    ldr    r2, =INTC_EINT1
-    strh   r1, [r2]
-    ldr    r2, =INTC_EINT2
-    strh   r1, [r2]
-    ldr    r2, =INTC_FISEL0
-    strh   r1, [r2]
-    ldr    r2, =INTC_FISEL1
-    strh   r1, [r2]
-    ldr    r2, =INTC_FISEL2
-    strh   r1, [r2]
-#endif
-
 #if !defined(BOOTLOADER) && !defined(STUB)
     /* Zero out IBSS */
     ldr    r2, =_iedata
@@ -122,25 +77,25 @@ start:
 #endif
 #endif /* !BOOTLOADER,!STUB */
 
-    /* Initialise bss section to zero */
-    ldr    r2, =_edata
-    ldr    r3, =_end
-    mov    r4, #0
-1:
-    cmp    r3, r2
-    strhi  r4, [r2], #4
-    bhi    1b
 
-    /* Load stack munge value */
-    ldr    r4, =0xdeadbeef
+    /* Initialise bss section to zero */
+    ldr     r2, =_edata
+    ldr     r3, =_end
+    mov     r4, #0
+bsszero:
+    cmp     r3, r2
+    strhi   r4, [r2], #4
+    bhi     bsszero
 
     /* Set up some stack and munge it with 0xdeadbeef */
-    ldr    r2, =stackbegin
-    ldr    r3, =stackend
-1:
-    cmp    r3, r2
-    strhi  r4, [r2], #4
-    bhi    1b
+    ldr     sp, =stackend
+    mov     r3, sp
+    ldr     r2, =stackbegin
+    ldr     r4, =0xdeadbeef
+stackmunge:
+    cmp     r3, r2
+    strhi   r4, [r2], #4
+    bhi     stackmunge
 
     /* Set up stack for IRQ mode */
     msr    cpsr_c, #0xd2 /* IRQ disabled, FIQ enabled */
@@ -185,26 +140,14 @@ start_loc:
 
 /* Exception handlers. Will be copied to address 0 after memory remapping */
     .section .vectors,"aw"
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-
-    /* Exception vectors */
-    .global vectors
-vectors:
-    .word  start 
-    .word  undef_instr_handler
-    .word  software_int_handler
-    .word  prefetch_abort_handler
-    .word  data_abort_handler
-    .word  reserved_handler
-    .word  irq_handler
-    .word  fiq_handler
+        b       start
+        b       undef_instr_handler
+    b   software_int_handler
+    b   prefetch_abort_handler
+    b   data_abort_handler
+    b   reserved_handler
+    b   irq_handler
+    b   fiq_handler
 
     .text
 
diff --git a/firmware/target/arm/tms320dm320/mrobe-500/backlight-mr500.c b/firmware/target/arm/tms320dm320/mrobe-500/backlight-mr500.c
index 855f10ef35..9df857ec31 100644
--- a/firmware/target/arm/tms320dm320/mrobe-500/backlight-mr500.c
+++ b/firmware/target/arm/tms320dm320/mrobe-500/backlight-mr500.c
@@ -30,7 +30,7 @@
 
 void _backlight_on(void)
 {
-#ifdef HAVE_LCD_SLEEP
+#if defined(HAVE_LCD_SLEEP) && !defined(BOOTLOADER)
     backlight_lcd_sleep_countdown(false); /* stop counter */
 #endif
 #ifdef HAVE_LCD_ENABLE
@@ -42,7 +42,7 @@ void _backlight_on(void)
 void _backlight_off(void)
 {
     _backlight_set_brightness(0);
-#ifdef HAVE_LCD_SLEEP
+#if defined(HAVE_LCD_SLEEP) && !defined(BOOTLOADER)
     /* Disable lcd after fade completes (when lcd_sleep timeout expires) */
     backlight_lcd_sleep_countdown(true); /* start countdown */
 #endif
diff --git a/firmware/target/arm/tms320dm320/mrobe-500/lcd-mr500.c b/firmware/target/arm/tms320dm320/mrobe-500/lcd-mr500.c
index 1126f35149..0357b469d9 100644
--- a/firmware/target/arm/tms320dm320/mrobe-500/lcd-mr500.c
+++ b/firmware/target/arm/tms320dm320/mrobe-500/lcd-mr500.c
@@ -43,7 +43,6 @@ static bool lcd_on = true;
 static bool lcd_powered = true;
 #endif
 
-volatile bool lcd_poweroff = false;
 /*
 ** These are imported from lcd-16bit.c
 */
@@ -65,6 +64,14 @@ void lcd_sleep()
         /* "not powered" implies "disabled" */
         if (lcd_on)
             lcd_enable(false);
+            
+        /* Disabling these saves another ~15mA */
+        IO_OSD_OSDWINMD0&=~(0x01);
+                IO_VID_ENC_VMOD&=~(0x01);
+        
+        sleep(HZ/5);
+        
+        /* Disabling the LCD saves ~50mA */
         IO_GIO_BITCLR2=1<<4;
         lcd_powered=false;
     }
@@ -83,6 +90,11 @@ void lcd_enable(bool state)
         if (!lcd_powered)
         {
                 lcd_powered=true;
+                
+                IO_OSD_OSDWINMD0|=0x01;
+                        IO_VID_ENC_VMOD|=0x01;
+        
+                sleep(2);
             IO_GIO_BITSET2=1<<4;
             /* Wait long enough for a frame to be written - yes, it
              * takes awhile. */
@@ -110,6 +122,41 @@ void lcd_init_device(void)
     /* Clear the Frame */
     memset16(FRAME, 0x0000, LCD_WIDTH*LCD_HEIGHT);
 
+        /* Setup the LCD controller */
+        IO_VID_ENC_VMOD=0x2015;
+        IO_VID_ENC_VDCTL=0x2000;
+        IO_VID_ENC_VDPRO=0x0000;
+        IO_VID_ENC_SYNCTL=0x100E;
+        IO_VID_ENC_HSPLS=1; /* HSYNC pulse width */
+        IO_VID_ENC_VSPLS=1; /* VSYNC pulse width */
+        
+        /* These calculations support 640x480 and 320x240  */
+        IO_VID_ENC_HINT=NATIVE_MAX_WIDTH+NATIVE_MAX_WIDTH/3;
+        IO_VID_ENC_HSTART=NATIVE_MAX_WIDTH/6; /* Front porch */
+        IO_VID_ENC_HVALID=NATIVE_MAX_WIDTH; /* Data valid */
+        IO_VID_ENC_VINT=NATIVE_MAX_HEIGHT+7;
+        IO_VID_ENC_VSTART=3;
+        IO_VID_ENC_VVALID=NATIVE_MAX_HEIGHT;
+        
+        IO_VID_ENC_HSDLY=0x0000;
+        IO_VID_ENC_VSDLY=0x0000;
+        IO_VID_ENC_YCCTL=0x0000;
+        IO_VID_ENC_RGBCTL=0x0000;
+        IO_VID_ENC_RGBCLP=0xFF00;
+        IO_VID_ENC_LNECTL=0x0000;
+        IO_VID_ENC_CULLLNE=0x0000;
+        IO_VID_ENC_LCDOUT=0x0000;
+        IO_VID_ENC_BRTS=0x0000;
+        IO_VID_ENC_BRTW=0x0000;
+        IO_VID_ENC_ACCTL=0x0000;
+        IO_VID_ENC_PWMP=0x0000;
+        IO_VID_ENC_PWMW=0x0000;
+        
+        IO_VID_ENC_DCLKCTL=0x0800;
+        IO_VID_ENC_DCLKPTN0=0x0001;
+
+
+        /* Setup the display */
     IO_OSD_MODE=0x00ff;
     IO_OSD_VIDWINMD=0x0002;
     IO_OSD_OSDWINMD0=0x2001;
@@ -117,21 +164,34 @@ void lcd_init_device(void)
     IO_OSD_ATRMD=0x0000;
     IO_OSD_RECTCUR=0x0000;
 
-    IO_OSD_OSDWIN0OFST=(480*2) / 32;
+    IO_OSD_OSDWIN0OFST=(NATIVE_MAX_WIDTH*2) / 32;
+    
     addr = ((int)FRAME-CONFIG_SDRAM_START) / 32;
     IO_OSD_OSDWINADH=addr >> 16;
     IO_OSD_OSDWIN0ADL=addr & 0xFFFF;
+    
+    IO_OSD_VIDWINADH=addr >> 16;
+    IO_OSD_VIDWIN0ADL=addr & 0xFFFF;
 
-    IO_OSD_BASEPX=80;
-    IO_OSD_BASEPY=2;
+    IO_OSD_BASEPX=IO_VID_ENC_HSTART;
+    IO_OSD_BASEPY=IO_VID_ENC_VSTART;
 
     IO_OSD_OSDWIN0XP=0;
     IO_OSD_OSDWIN0YP=0;
-    IO_OSD_OSDWIN0XL=480;
-    IO_OSD_OSDWIN0YL=640;
+  
+    IO_OSD_OSDWIN0XL=NATIVE_MAX_WIDTH;
+    IO_OSD_OSDWIN0YL=NATIVE_MAX_HEIGHT;
+
+    /* Set pin 36 and 35 (LCD POWER and LCD RESOLUTION) to an output */
+    IO_GIO_DIR2&=!(3<<3);
     
-    /* Set pin 36 to an output */
-    IO_GIO_DIR2&=!(1<<4);
+#if NATIVE_MAX_HEIGHT > 320
+        /* Set LCD resolution to VGA */
+    IO_GIO_BITSET2=1<<3;
+#else
+        /* Set LCD resolution to QVGA */
+        IO_GIO_BITCLR2=1<<3;
+#endif
 }
 
 /* Update a fraction of the display. */
@@ -204,50 +264,79 @@ void lcd_update(void)
 #endif
 }
 
-/* Line write helper function for lcd_yuv_blit. Write two lines of yuv420. */
-extern void lcd_write_yuv420_lines(fb_data *dst,
-                                   unsigned char chroma_buf[LCD_HEIGHT/2*3],
-                                   unsigned char const * const src[3],
-                                   int width,
-                                   int stride);
+void lcd_blit_yuv(unsigned char * const src[3],
+                  int src_x, int src_y, int stride,
+                  int x, int y, int width, 
+                  int height) __attribute__ ((section(".icode")));
+                                           
 /* Performance function to blit a YUV bitmap directly to the LCD */
-/* For the Gigabeat - show it rotated */
-/* So the LCD_WIDTH is now the height */
+/* Show it rotated so the LCD_WIDTH is now the height */
 void lcd_blit_yuv(unsigned char * const src[3],
                   int src_x, int src_y, int stride,
                   int x, int y, int width, int height)
 {
     /* Caches for chroma data so it only need be recaculated every other
        line */
-    unsigned char chroma_buf[LCD_HEIGHT/2*3]; /* 480 bytes */
     unsigned char const * yuv_src[3];
     off_t z;
 
+        /* Turn off the RGB buffer and enable the YUV buffer */
+        IO_OSD_OSDWINMD0&=~(0x01);
+        IO_OSD_VIDWINMD|=0x01;
+
     if (!lcd_on)
         return;
+        
+    /* y has to be at multiple of 2 or else it will mess up the HW (interleaving) */
+    y &= ~1;
 
     /* Sorry, but width and height must be >= 2 or else */
     width &= ~1;
-    height >>= 1;
+    height>>=1;
 
-    fb_data *dst = (fb_data*)FRAME + x * LCD_WIDTH + (LCD_WIDTH - y) - 1;
+    fb_data *dst = (fb_data*)FRAME + LCD_WIDTH*LCD_HEIGHT - x * LCD_WIDTH + y;
 
     z = stride*src_y;
     yuv_src[0] = src[0] + z + src_x;
     yuv_src[1] = src[1] + (z >> 2) + (src_x >> 1);
     yuv_src[2] = src[2] + (yuv_src[1] - src[1]);
 
-    do
     {
-        lcd_write_yuv420_lines(dst, chroma_buf, yuv_src, width,
-                               stride);
-
-        yuv_src[0] += stride << 1; /* Skip down two luma lines */
-        yuv_src[1] += stride >> 1; /* Skip down one chroma line */
-        yuv_src[2] += stride >> 1;
-        dst -= 2;
+        do
+        {
+                register fb_data *c_dst=dst;
+                register int c_width=width;
+                unsigned char const * c_yuv_src[3];
+                c_yuv_src[0] = yuv_src[0];
+                c_yuv_src[1] = yuv_src[1];
+                c_yuv_src[2] = yuv_src[2];
+
+                do
+                {
+                /* This needs to be done in a block of 4 pixels */
+                        *c_dst=*c_yuv_src[0]<<8 | *c_yuv_src[1];
+                        *(c_dst+1)=*(c_yuv_src[0]+stride)<<8 | *c_yuv_src[2];
+                        c_dst-=LCD_WIDTH;
+                        c_yuv_src[0]++;
+                        *c_dst=*c_yuv_src[0]<<8 | *c_yuv_src[1];
+                        *(c_dst+1)=*(c_yuv_src[0]+stride)<<8 | *c_yuv_src[2];
+                        c_dst-=LCD_WIDTH;
+                    c_yuv_src[0]++;
+                    
+                c_yuv_src[1]++;
+                c_yuv_src[2]++;
+                
+                        c_width -= 2;
+                }
+                while (c_width > 0);
+                
+                yuv_src[0] += stride << 1; /* Skip down two luma lines */
+            yuv_src[1] += stride >> 1; /* Skip down one chroma line */
+            yuv_src[2] += stride >> 1;
+            dst+=2;
+        }
+        while (--height > 0);
     }
-    while (--height > 0);
 }
 
 void lcd_set_contrast(int val) {
diff --git a/firmware/target/arm/tms320dm320/mrobe-500/power-mr500.c b/firmware/target/arm/tms320dm320/mrobe-500/power-mr500.c
index 48a989a736..9fd976ec10 100644
--- a/firmware/target/arm/tms320dm320/mrobe-500/power-mr500.c
+++ b/firmware/target/arm/tms320dm320/mrobe-500/power-mr500.c
@@ -25,6 +25,7 @@
 #include "kernel.h"
 #include "system.h"
 #include "power.h"
+#include "lcd.h"
 #include "pcf50606.h"
 #include "backlight.h"
 #include "backlight-target.h"
@@ -71,6 +72,7 @@ void power_off(void)
 {
     /* turn off backlight and wait for 1 second */
     _backlight_off();
+    lcd_sleep();
     sleep(HZ);
     /* Hard shutdown */
     IO_GIO_DIR1&=~(1<<10);