2 files changed, 70 insertions, 29 deletions

diff --git a/firmware/export/s5l8700.h b/firmware/export/s5l8700.h
index 940f47f7f2..353690398a 100644
--- a/firmware/export/s5l8700.h
+++ b/firmware/export/s5l8700.h
@@ -459,6 +459,10 @@
 #define PDAT7                   (*(REG32_PTR_T)(0x3CF00074))     /* The data register for port 7 */
 #define PCON10                  (*(REG32_PTR_T)(0x3CF000A0))     /* Configures the pins of port 10 */
 #define PDAT10                  (*(REG32_PTR_T)(0x3CF000A4))     /* The data register for port 10 */
+#define PCON13                  (*(REG32_PTR_T)(0x3CF000D0))     /* Configures the pins of port 13 */
+#define PDAT13                  (*(REG32_PTR_T)(0x3CF000D4))     /* The data register for port 13 */
+#define PCON14                  (*(REG32_PTR_T)(0x3CF000E0))     /* Configures the pins of port 14 */
+#define PDAT14                  (*(REG32_PTR_T)(0x3CF000E4))     /* The data register for port 14 */
 #define PCON_ASRAM              (*(REG32_PTR_T)(0x3CF000F0))     /* Configures the pins of port nor flash */
 #define PCON_SDRAM              (*(REG32_PTR_T)(0x3CF000F4))     /* Configures the pins of port sdram */
 #define PCON11                  (*(REG32_PTR_T)(0x3CF000F8))     /* Configures the pins of port 11 */
diff --git a/firmware/target/arm/s5l8700/ipodnano2g/lcd-nano2g.c b/firmware/target/arm/s5l8700/ipodnano2g/lcd-nano2g.c
index 74f2fca383..ad2e356b5e 100644
--- a/firmware/target/arm/s5l8700/ipodnano2g/lcd-nano2g.c
+++ b/firmware/target/arm/s5l8700/ipodnano2g/lcd-nano2g.c
@@ -40,9 +40,9 @@ static void s5l_lcd_write_data(int data)
     while (LCD_STATUS&0x10);
 }
 
-
 /** globals **/
 
+static int lcd_type;
 static int xoffset; /* needed for flip */
 
 /*** hardware configuration ***/
@@ -89,6 +89,15 @@ void lcd_on(void)
 /* LCD init */
 void lcd_init_device(void)
 {
+    /* Detect lcd type */
+
+    PCON13 &= ~0xf;    /* Set pin 0 to input */
+    PCON14 &= ~0xf0;   /* Set pin 1 to input */
+
+    if (((PDAT13 & 1) == 0) && ((PDAT14 & 2) == 2))
+        lcd_type = 0;
+    else
+        lcd_type = 1;
 }
 
 
@@ -127,36 +136,64 @@ void lcd_update(void) ICODE_ATTR;
 void lcd_update(void)
 {
     int x,y;
-    fb_data* p;
+    fb_data* p = &lcd_framebuffer[0][0];
     fb_data pixel;
 
-    s5l_lcd_write_cmd(0x3a);
-    s5l_lcd_write_data(0x65);
-
-    s5l_lcd_write_cmd(0x2a);
-    s5l_lcd_write_data(0);
-    s5l_lcd_write_data(0);
-    s5l_lcd_write_data(0);
-    s5l_lcd_write_data(LCD_WIDTH-1);
-
-    s5l_lcd_write_cmd(0x2b);
-    s5l_lcd_write_data(0);
-    s5l_lcd_write_data(0);
-    s5l_lcd_write_data(0);
-    s5l_lcd_write_data(LCD_HEIGHT-1);
-
-    s5l_lcd_write_cmd(0x2c);
-
-    /* Copy display bitmap to hardware */
-
-    p = &lcd_framebuffer[0][0];
-    for (y = 0; y < LCD_HEIGHT; y++) {
-        for (x = 0; x < LCD_WIDTH; x++) {
-           pixel = *(p++);
-
-           while (LCD_STATUS&0x10);
-           LCD_WDATA = (pixel & 0xff00) >> 8;
-           LCD_WDATA = pixel & 0xff;
+    if (lcd_type==0) {
+        s5l_lcd_write_cmd(0x50);
+        s5l_lcd_write_data(0);            /* Start column */
+        s5l_lcd_write_cmd(0x51);
+        s5l_lcd_write_data(LCD_WIDTH-1);  /* End column */
+        s5l_lcd_write_cmd(0x52);
+        s5l_lcd_write_data(0);            /* Start row */
+        s5l_lcd_write_cmd(0x53);
+        s5l_lcd_write_data(LCD_HEIGHT-1); /* End row */
+
+        s5l_lcd_write_cmd(0x20);
+        s5l_lcd_write_data(0);
+        s5l_lcd_write_cmd(0x21);
+        s5l_lcd_write_data(0);
+        s5l_lcd_write_cmd(0x22);
+
+        /* Copy display bitmap to hardware */
+        for (y = 0; y < LCD_HEIGHT; y++) {
+            for (x = 0; x < LCD_WIDTH; x++) {
+                pixel = *(p++);
+
+                while (LCD_STATUS&0x10);
+
+                LCD_WDATA = pixel & 0xff;
+                LCD_WDATA = (pixel & 0xff00) >> 8;
+            }
+        }
+    } else {
+        s5l_lcd_write_cmd(0x3a);
+        s5l_lcd_write_data(0x65);
+
+        s5l_lcd_write_cmd(0x2a);
+        s5l_lcd_write_data(0);            /* Start column, high byte */
+        s5l_lcd_write_data(0);            /* Start column. low byte */
+        s5l_lcd_write_data(0);            /* End column, high byte */
+        s5l_lcd_write_data(LCD_WIDTH-1);  /* End column, low byte */
+
+        s5l_lcd_write_cmd(0x2b);
+        s5l_lcd_write_data(0);            /* Start row, high byte */
+        s5l_lcd_write_data(0);            /* Start row, low byte */
+        s5l_lcd_write_data(0);            /* End row, high byte */
+        s5l_lcd_write_data(LCD_HEIGHT-1); /* End row, low byte */
+
+        s5l_lcd_write_cmd(0x2c);
+
+        /* Copy display bitmap to hardware */
+        for (y = 0; y < LCD_HEIGHT; y++) {
+            for (x = 0; x < LCD_WIDTH; x++) {
+                pixel = *(p++);
+
+                while (LCD_STATUS&0x10);
+
+                LCD_WDATA = (pixel & 0xff00) >> 8;
+                LCD_WDATA = pixel & 0xff;
+            }
         }
     }