9 files changed, 234 insertions, 173 deletions

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);