diff --git a/sys/include/gpanel.h b/sys/include/gpanel.h
index 96b1f00..405642a 100644
--- a/sys/include/gpanel.h
+++ b/sys/include/gpanel.h
@@ -138,6 +138,8 @@ extern void gpanel_cs_idle(void);
 extern void gpanel_rs_command(void);
 extern void gpanel_rs_data(void);
 extern void gpanel_wr_strobe(void);
+extern void gpanel_read_dir(void);
+extern void gpanel_write_dir(void);
 
 /*
  * Descriptor for access to the hardware-level driver.
diff --git a/sys/pic32/files.kconf b/sys/pic32/files.kconf
index d22476c..5c0a909 100644
--- a/sys/pic32/files.kconf
+++ b/sys/pic32/files.kconf
@@ -73,7 +73,7 @@ pic32/glcd.c                    optional glcd
 pic32/gpanel.c                  optional gpanel
 pic32/gpanel-ili9341.c          optional gpanel
 pic32/gpanel-nt35702.c          optional gpanel
-pic32/gpanel-s6d04h0.c          optional gpanel
+#pic32/gpanel-s6d04h0.c          optional gpanel
 pic32/gpanel-st7781.c           optional gpanel
 pic32/gpio.c                    optional gpio
 pic32/hx8357.c                  optional hxtft
diff --git a/sys/pic32/gpanel-ili9341.c b/sys/pic32/gpanel-ili9341.c
index 220d844..34a2597 100644
--- a/sys/pic32/gpanel-ili9341.c
+++ b/sys/pic32/gpanel-ili9341.c
@@ -386,9 +386,16 @@ static void ili9341_draw_glyph(const struct gpanel_font_t *font,
  */
 void ili9341_init_display(struct gpanel_hw *h)
 {
+    /* Use a few NOPs to synchronize after the hard Reset. */
     gpanel_cs_active();
+    write_command(ILI9341_No_Operation);
+    write_command(ILI9341_No_Operation);
+    write_command(ILI9341_No_Operation);
+    write_command(ILI9341_No_Operation);
+
+    /* Need at least 200msec to restore after the soft Reset. */
     write_command(ILI9341_Software_Reset);
-    udelay(50000);
+    udelay(200000);
 
     write_command(ILI9341_Display_OFF);
 
@@ -405,7 +412,7 @@ void ili9341_init_display(struct gpanel_hw *h)
     write_command(ILI9341_VCOM_Control_2);
     write_data(0xC0);
 
-    set_rotation(1);                /* Landscape */
+    set_rotation(3);                /* Landscape */
 
     write_command(ILI9341_Pixel_Format_Set);
     write_data(0x55);
@@ -418,12 +425,9 @@ void ili9341_init_display(struct gpanel_hw *h)
     write_data(0x07);
 
     write_command(ILI9341_Sleep_OUT);
-    write_data(0);
     udelay(150000);
 
     write_command(ILI9341_Display_ON);
-    write_data(0);
-    udelay(500000);
 
     set_window(0, 0, _width-1, _height-1);
     gpanel_cs_idle();
diff --git a/sys/pic32/gpanel-s6d04h0.c b/sys/pic32/gpanel-s6d04h0.c
index cf43019..ba7ddd7 100644
--- a/sys/pic32/gpanel-s6d04h0.c
+++ b/sys/pic32/gpanel-s6d04h0.c
@@ -122,8 +122,10 @@ static int _width, _height;
  */
 static void write_command(int cmd)
 {
+    gpanel_cs_active();
     gpanel_rs_command();
     gpanel_write_byte(cmd);
+    gpanel_cs_idle();
 }
 
 /*
@@ -131,8 +133,10 @@ static void write_command(int cmd)
  */
 static void write_data(int cmd)
 {
+    gpanel_cs_active();
     gpanel_rs_data();
     gpanel_write_byte(cmd);
+    gpanel_cs_idle();
 }
 
 /*
@@ -162,11 +166,9 @@ static void s6d04h0_set_pixel(int x, int y, int color)
 {
     if (x < 0 || x >= _width || y < 0 || y >= _height)
         return;
-    gpanel_cs_active();
     set_window(x, y, x, y);
     write_data(color >> 8);
     write_data(color);
-    gpanel_cs_idle();
 }
 
 /*
@@ -260,8 +262,6 @@ static void set_rotation(int rotation)
 
 static void s6d04h0_clear(struct gpanel_hw *h, int color, int width, int height)
 {
-    gpanel_cs_active();
-
     /* Switch screen orientaation. */
     if (width > height)
         set_rotation(1);        /* Landscape */
@@ -271,7 +271,6 @@ static void s6d04h0_clear(struct gpanel_hw *h, int color, int width, int height)
     /* Fill the screen with a color. */
     set_window(0, 0, _width-1, _height-1);
     flood(color, _width * _height);
-    gpanel_cs_idle();
 }
 
 /*
@@ -298,10 +297,8 @@ static void s6d04h0_fill_rectangle(int x0, int y0, int x1, int y1, int color)
         y0 = y1;
         y1 = t;
     }
-    gpanel_cs_active();
     set_window(x0, y0, x1, y1);
     flood(color, (x1 - x0 + 1) * (y1 - y0 + 1));
-    gpanel_cs_idle();
 }
 
 /*
@@ -313,7 +310,6 @@ static void s6d04h0_draw_image(int x, int y, int width, int height,
     unsigned cnt = width * height;
     int color;
 
-    gpanel_cs_active();
     set_window(x, y, x + width - 1, y + height - 1);
     gpanel_rs_data();
     while (cnt--) {
@@ -321,7 +317,6 @@ static void s6d04h0_draw_image(int x, int y, int width, int height,
         gpanel_write_byte(color >> 8);
         gpanel_write_byte(color);
     }
-    gpanel_cs_idle();
 }
 
 /*
@@ -338,8 +333,8 @@ static void s6d04h0_draw_glyph(const struct gpanel_font_t *font,
         /*
          * Clear background.
          */
-        gpanel_cs_active();
         set_window(x, y, x + width - 1, y + font->height - 1);
+        gpanel_cs_active();
         gpanel_rs_data();
 
         /* Loop on each glyph row. */
@@ -383,27 +378,223 @@ static void s6d04h0_draw_glyph(const struct gpanel_font_t *font,
  */
 void s6d04h0_init_display(struct gpanel_hw *h)
 {
-    gpanel_cs_active();
-    write_command(S6D04H0_Software_Reset);
-    udelay(50000);
+    write_command(S6D04H0_PASSWD1);
+    write_data(0x5A);
+    write_data(0x5A);
+
+    write_command(0xFC);
+    write_data(0x5A);
+    write_data(0x5A);
+
+    write_command(0xFD);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x10);
+    write_data(0x14);
+    write_data(0x12);
+    write_data(0x00);
+    write_data(0x04);
+    write_data(0x48);
+    write_data(0x40);
+    write_data(0x16);
+    write_data(0x16);
+
+    write_command(S6D04H0_Tearing_Effect_Line_On);
 
     write_command(S6D04H0_Display_Off);
 
-    set_rotation(1);                /* Landscape */
+    set_rotation(3);
 
     write_command(S6D04H0_Interface_Pixel_Format);
     write_data(0x55);
 
+    write_command(S6D04H0_DISCTL);
+    write_data(0x28);
+    write_data(0x5B);
+    write_data(0x7F);
+    write_data(0x08);
+    write_data(0x08);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x15);
+    write_data(0x48);
+    write_data(0x04);
+    write_data(0x07);
+    write_data(0x01);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x63);
+    write_data(0x08);
+    write_data(0x08);
+
+    write_command(S6D04H0_IFCTL);
+    write_data(0x01);
+    write_data(0x00);
+    write_data(0x10);
+    write_data(0x00);
+
+    write_command(S6D04H0_PANELCTL);
+    write_data(0x33);
+    write_data(0x00);
+    write_data(0x00);
+
+    write_command(S6D04H0_SRCCTL);
+    write_data(0x01);
+    write_data(0x01);
+    write_data(0x07);
+    write_data(0x00);
+    write_data(0x01);
+    write_data(0x0C);
+    write_data(0x03);
+    write_data(0x0C);
+    write_data(0x03);
+
+    write_command(S6D04H0_VCMCTL);
+    write_data(0x00);
+    write_data(0x2E);
+    write_data(0x40);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x01);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x0D);
+    write_data(0x0D);
+    write_data(0x00);
+    write_data(0x00);
+
+    write_command(S6D04H0_PWRCTL);
+    write_data(0x07);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x22);
+    write_data(0x64);
+    write_data(0x01);
+    write_data(0x02);
+    write_data(0x2A);
+    write_data(0x4D);
+    write_data(0x06);
+    write_data(0x2A);
+    write_data(0x00);
+    write_data(0x06);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x00);
+    write_data(0x00);
+
+    write_command(S6D04H0_MANPWRSEQ);
+    write_data(0x01);
+
+    write_command(S6D04H0_GAMMASEL);
+    write_data(0x04);
+
+    write_command(S6D04H0_PGAMMACTL);
+    write_data(0x0A);
+    write_data(0x04);
+    write_data(0x0C);
+    write_data(0x19);
+    write_data(0x25);
+    write_data(0x33);
+    write_data(0x2D);
+    write_data(0x27);
+    write_data(0x22);
+    write_data(0x1E);
+    write_data(0x1A);
+    write_data(0x00);
+
+    write_command(S6D04H0_NGAMMACTL);
+    write_data(0x0C);
+    write_data(0x04);
+    write_data(0x19);
+    write_data(0x1E);
+    write_data(0x20);
+    write_data(0x23);
+    write_data(0x18);
+    write_data(0x3D);
+    write_data(0x25);
+    write_data(0x19);
+    write_data(0x0B);
+    write_data(0x00);
+
+    write_command(S6D04H0_GAMMASEL);
+    write_data(0x02);
+
+    write_command(S6D04H0_PGAMMACTL);
+    write_data(0x0A);
+    write_data(0x04);
+    write_data(0x0C);
+    write_data(0x19);
+    write_data(0x25);
+    write_data(0x33);
+    write_data(0x2D);
+    write_data(0x27);
+    write_data(0x22);
+    write_data(0x1E);
+    write_data(0x1A);
+    write_data(0x00);
+
+    write_command(S6D04H0_NGAMMACTL);
+    write_data(0x0C);
+    write_data(0x04);
+    write_data(0x19);
+    write_data(0x1E);
+    write_data(0x20);
+    write_data(0x23);
+    write_data(0x18);
+    write_data(0x3D);
+    write_data(0x25);
+    write_data(0x19);
+    write_data(0x0B);
+    write_data(0x00);
+
+    write_command(S6D04H0_GAMMASEL);
+    write_data(0x01);
+
+    write_command(S6D04H0_PGAMMACTL);
+    write_data(0x0A);
+    write_data(0x04);
+    write_data(0x0C);
+    write_data(0x19);
+    write_data(0x25);
+    write_data(0x33);
+    write_data(0x2D);
+    write_data(0x27);
+    write_data(0x22);
+    write_data(0x1E);
+    write_data(0x1A);
+    write_data(0x00);
+
+    write_command(S6D04H0_NGAMMACTL);
+    write_data(0x0C);
+    write_data(0x04);
+    write_data(0x19);
+    write_data(0x1E);
+    write_data(0x20);
+    write_data(0x23);
+    write_data(0x18);
+    write_data(0x3D);
+    write_data(0x25);
+    write_data(0x19);
+    write_data(0x0B);
+    write_data(0x00);
+
     write_command(S6D04H0_Sleep_Out);
-    write_data(0);
     udelay(150000);
 
+    write_command(S6D04H0_PASSWD1);
+    write_data(0xA5);
+    write_data(0xA5);
+
+    write_command(0xFC);
+    write_data(0xA5);
+    write_data(0xA5);
+
     write_command(S6D04H0_Display_On);
-    write_data(0);
-    udelay(500000);
 
     set_window(0, 0, _width-1, _height-1);
-    gpanel_cs_idle();
 
     /*
      * Fill the gpanel_hw descriptor.
diff --git a/sys/pic32/gpanel.c b/sys/pic32/gpanel.c
index f87ed44..2317154 100644
--- a/sys/pic32/gpanel.c
+++ b/sys/pic32/gpanel.c
@@ -96,7 +96,7 @@ static int _chip_id;
 /*
  * Set direction of data bus as output.
  */
-static void set_write_dir()
+void gpanel_write_dir()
 {
     TRIS_CLR(LCD_D0_PORT) = 1 << LCD_D0_PIN;
     TRIS_CLR(LCD_D1_PORT) = 1 << LCD_D1_PIN;
@@ -111,7 +111,7 @@ static void set_write_dir()
 /*
  * Set direction of data bus as input.
  */
-static void set_read_dir()
+void gpanel_read_dir()
 {
     TRIS_SET(LCD_D0_PORT) = 1 << LCD_D0_PIN;
     TRIS_SET(LCD_D1_PORT) = 1 << LCD_D1_PIN;
@@ -228,11 +228,11 @@ static int read_reg16(int reg)
     RS_COMMAND();
     //gpanel_write_byte(reg >> 8);
     gpanel_write_byte(reg);
-    set_read_dir();         // Switch data bus to input
+    gpanel_read_dir();         // Switch data bus to input
     RS_DATA();
     value = gpanel_read_byte() << 8;
     value |= gpanel_read_byte();
-    set_write_dir();        // Restore data bus as output
+    gpanel_write_dir();        // Restore data bus as output
     CS_IDLE();
     return value;
 }
@@ -247,13 +247,13 @@ static int read_reg32(int reg)
     CS_ACTIVE();
     RS_COMMAND();
     gpanel_write_byte(reg);
-    set_read_dir();         // Switch data bus to input
+    gpanel_read_dir();         // Switch data bus to input
     RS_DATA();
     value = gpanel_read_byte() << 24;
     value |= gpanel_read_byte() << 16;
     value |= gpanel_read_byte() << 8;
     value |= gpanel_read_byte();
-    set_write_dir();        // Restore data bus as output
+    gpanel_write_dir();        // Restore data bus as output
     CS_IDLE();
     return value;
 }
@@ -553,6 +553,37 @@ int gpanel_ioctl(dev_t dev, register u_int cmd, caddr_t addr, int flag)
     return 0;
 }
 
+/*
+ * Read the the chip ID register.
+ * Some controllers have a register #0
+ * programmed with unique chip ID.
+ */
+static int read_id()
+{
+    int id, retry;
+
+    /* Some controllers have a register #0
+     * programmed with unique chip ID. */
+    id = read_reg16(0);
+    if (id != 0)
+        return id;
+
+    /* Try ID from register #4. */
+    id = read_reg32(4) & 0xffffff;
+    if (id != 0)
+        return id;
+
+    /* Try ID from register #D3.
+     * Might need to wait until the register becomes alive after Reset. */
+    for (retry=0; retry<5; retry++) {
+        id = read_reg32(0xD3) & 0xffffff;
+        if (id != 0)
+            return id;
+        udelay(50000);
+    }
+    return 0;
+}
+
 /*
  * Detect the type of the LCD controller, and initialize it.
  * Return true if found and initialized ok.
@@ -575,7 +606,7 @@ static int probe(config)
     TRIS_CLR(LCD_WR_PORT) = 1 << LCD_WR_PIN;
     TRIS_CLR(LCD_RD_PORT) = 1 << LCD_RD_PIN;
     TRIS_CLR(LCD_RST_PORT) = 1 << LCD_RST_PIN;
-    set_write_dir();
+    gpanel_write_dir();
 
     /* Reset the chip. */
     RST_ACTIVE();
@@ -586,10 +617,10 @@ static int probe(config)
     /* Read the the chip ID register.
      * Some controllers have a register #0
      * programmed with unique chip ID. */
-    _chip_id = read_reg16(0);
+    _chip_id = read_id();
     switch (_chip_id) {
     default:
-        printf("gpanel0: Unknown chip ID0 = 0x%04x\n", _chip_id);
+        printf("gpanel0: Unknown chip ID = 0x%04x\n", _chip_id);
         goto failed;
 
     case 0x7783:
@@ -597,40 +628,14 @@ static int probe(config)
         st7781_init_display(&hw);
         break;
 
-    case 0:
-        /* Try ID from register #4. */
-        _chip_id = read_reg32(4) & 0xffffff;
-        switch (_chip_id) {
-        default:
-            printf("gpanel0: Unknown chip ID4 = 0x%06x\n", _chip_id);
-            goto failed;
+    case 0x009341:
+        /* Ilitek ILI9341. */
+        ili9341_init_display(&hw);
+        break;
 
-        case 0x009341:
-            /* Ilitek ILI9341. */
-            ili9341_init_display(&hw);
-            break;
-
-        case 0x388000:
-            /* Novatek NT35702. */
-            nt35702_init_display(&hw);
-            break;
-
-        case 0:
-            /* Try ID from register #D3.
-             * Samsung controller needs 120us to finish the reset. */
-            udelay(120000);
-            _chip_id = read_reg32(0xD3) & 0xffffff;
-            switch (_chip_id) {
-            default:
-                printf("gpanel0: Unknown chip ID_D3 = 0x%06x\n", _chip_id);
-                goto failed;
-
-            case 0x009341:
-                /* Samsung S6D04H0. */
-                s6d04h0_init_display(&hw);
-                break;
-            }
-        }
+    case 0x388000:
+        /* Novatek NT35702. */
+        nt35702_init_display(&hw);
         break;
     }
     printf("gpanel0: <%s> display %ux%u\n", hw.name, hw.width, hw.height);
@@ -638,7 +643,7 @@ static int probe(config)
 
 failed:
     /* Disable outputs. */
-    set_read_dir();
+    gpanel_read_dir();
     TRIS_SET(LCD_CS_PORT) = 1 << LCD_CS_PIN;
     TRIS_SET(LCD_RS_PORT) = 1 << LCD_RS_PIN;
     TRIS_SET(LCD_WR_PORT) = 1 << LCD_WR_PIN;
diff --git a/sys/pic32/wf32/Makefile b/sys/pic32/wf32/Makefile
index fe394e7..fdb15ba 100644
--- a/sys/pic32/wf32/Makefile
+++ b/sys/pic32/wf32/Makefile
@@ -79,8 +79,8 @@ OBJS = exec_aout.o exec_conf.o exec_elf.o exec_script.o exec_subr.o \
 	ufs_namei.o ufs_subr.o ufs_syscalls.o ufs_syscalls2.o \
 	vfs_vnops.o vm_sched.o vm_swap.o vm_swp.o clock.o cons.o devsw.o \
 	exception.o machdep.o mem.o signal.o swap.o sysctl.o adc.o \
-	gpanel.o gpanel-ili9341.o gpanel-nt35702.o gpanel-s6d04h0.o \
-	gpanel-st7781.o gpio.o pwm.o sd.o spi.o spi_bus.o uart.o 
+	gpanel.o gpanel-ili9341.o gpanel-nt35702.o gpanel-st7781.o \
+	gpio.o pwm.o sd.o spi.o spi_bus.o uart.o 
 
 CFILES = $S/kernel/exec_aout.c $S/kernel/exec_conf.c $S/kernel/exec_elf.c \
 	$S/kernel/exec_script.c $S/kernel/exec_subr.c \
@@ -107,9 +107,9 @@ CFILES = $S/kernel/exec_aout.c $S/kernel/exec_conf.c $S/kernel/exec_elf.c \
 	$S/pic32/mem.c $S/pic32/signal.c $S/pic32/swap.c \
 	$S/pic32/sysctl.c $S/pic32/adc.c $S/pic32/gpanel.c \
 	$S/pic32/gpanel-ili9341.c $S/pic32/gpanel-nt35702.c \
-	$S/pic32/gpanel-s6d04h0.c $S/pic32/gpanel-st7781.c \
-	$S/pic32/gpio.c $S/pic32/pwm.c $S/pic32/sd.c $S/pic32/spi.c \
-	$S/pic32/spi_bus.c $S/pic32/uart.c swapunix.c 
+	$S/pic32/gpanel-st7781.c $S/pic32/gpio.c $S/pic32/pwm.c \
+	$S/pic32/sd.c $S/pic32/spi.c $S/pic32/spi_bus.c $S/pic32/uart.c \
+	swapunix.c 
 
 # load lines for config "xxx" will be emitted as:
 # xxx: ${SYSTEM_DEP} swapxxx.o
@@ -353,9 +353,6 @@ gpanel-ili9341.o: $S/pic32/gpanel-ili9341.c
 gpanel-nt35702.o: $S/pic32/gpanel-nt35702.c
 	${COMPILE_C}
 
-gpanel-s6d04h0.o: $S/pic32/gpanel-s6d04h0.c
-	${COMPILE_C}
-
 gpanel-st7781.o: $S/pic32/gpanel-st7781.c
 	${COMPILE_C}