Convert S6D1121 driver to new format

7 files changed, 448 insertions, 626 deletions

diff --git a/drivers/gdisp/S6D1121/board_S6D1121_olimex_e407.h b/drivers/gdisp/S6D1121/board_S6D1121_olimex_e407.h
new file mode 100644
index 00000000..ec367183
--- /dev/null
+++ b/drivers/gdisp/S6D1121/board_S6D1121_olimex_e407.h
@@ -0,0 +1,91 @@
+/*
+ * This file is subject to the terms of the GFX License. If a copy of
+ * the license was not distributed with this file, you can obtain one at:
+ *
+ *              http://ugfx.org/license.html
+ */
+
+/**
+ * @file    drivers/gdisp/S6D1121/board_S6D1121_olimex_e407.h
+ * @brief   GDISP Graphic Driver subsystem board interface for the S6D1121 display
+ */
+
+#ifndef _GDISP_LLD_BOARD_H
+#define _GDISP_LLD_BOARD_H
+
+// For a multiple display configuration we would put all this in a structure and then
+//	set g->priv to that structure.
+#define GDISP_REG              (*((volatile uint16_t *) 0x60000000)) /* RS = 0 */
+#define GDISP_RAM              (*((volatile uint16_t *) 0x60020000)) /* RS = 1 */
+
+static inline void init_board(GDisplay *g) {
+
+	// As we are not using multiple displays we set g->priv to NULL as we don't use it.
+	g->priv = 0;
+
+	switch(g->controllerdisplay) {
+	case 0:											// Set up for Display 0
+		/* STM32F4 FSMC init */
+		rccEnableAHB3(RCC_AHB3ENR_FSMCEN, 0);
+
+		/* set pins to FSMC mode */
+		IOBus busD = {GPIOD, (1 << 0) | (1 << 1) | (1 << 4) | (1 << 5) | (1 << 7) | (1 << 8) |
+							(1 << 9) | (1 << 10) | (1 << 11) | (1 << 14) | (1 << 15), 0};
+
+		IOBus busE = {GPIOE, (1 << 7) | (1 << 8) | (1 << 9) | (1 << 10) | (1 << 11) | (1 << 12) |
+							(1 << 13) | (1 << 14) | (1 << 15), 0};
+
+		palSetBusMode(&busD, PAL_MODE_ALTERNATE(12));
+		palSetBusMode(&busE, PAL_MODE_ALTERNATE(12));
+
+		/* FSMC timing */
+		FSMC_Bank1->BTCR[0+1] = (6) | (10 << 8) | (10 << 16);
+
+		/* Bank1 NOR/SRAM control register configuration */
+		FSMC_Bank1->BTCR[0] = FSMC_BCR1_MWID_0 | FSMC_BCR1_WREN | FSMC_BCR1_MBKEN;
+		break;
+	}
+}
+
+static inline void setpin_reset(GDisplay *g, bool_t state) {
+	(void) g;
+	(void) state;
+}
+
+static inline void set_backlight(GDisplay *g, uint8_t percent) {
+	(void) g;
+	(void) percent;
+}
+
+static inline void acquire_bus(GDisplay *g) {
+	(void) g;
+}
+
+static inline void release_bus(GDisplay *g) {
+	(void) g;
+}
+
+static inline void write_index(GDisplay *g, uint16_t index) {
+	(void) g;
+	GDISP_REG = index;
+}
+
+static inline void write_data(GDisplay *g, uint16_t data) {
+	(void) g;
+	GDISP_RAM = data;
+}
+
+static inline void setreadmode(GDisplay *g) {
+	(void) g;
+}
+
+static inline void setwritemode(GDisplay *g) {
+	(void) g;
+}
+
+static inline uint16_t read_data(GDisplay *g) {
+	(void) g;
+	return GDISP_RAM;
+}
+
+#endif /* _GDISP_LLD_BOARD_H */
diff --git a/drivers/gdisp/S6D1121/board_S6D1121_template.h b/drivers/gdisp/S6D1121/board_S6D1121_template.h
new file mode 100644
index 00000000..41c8b199
--- /dev/null
+++ b/drivers/gdisp/S6D1121/board_S6D1121_template.h
@@ -0,0 +1,154 @@
+/*
+ * This file is subject to the terms of the GFX License. If a copy of
+ * the license was not distributed with this file, you can obtain one at:
+ *
+ *              http://ugfx.org/license.html
+ */
+
+/**
+ * @file    drivers/gdisp/S6D1121/board_S6D1121_template.h
+ * @brief   GDISP Graphic Driver subsystem board interface for the S6D1121 display.
+ *
+ * @addtogroup GDISP
+ * @{
+ */
+
+#ifndef _GDISP_LLD_BOARD_H
+#define _GDISP_LLD_BOARD_H
+
+/**
+ * @brief   Initialise the board for the display.
+ *
+ * @param[in] g			The GDisplay structure
+ *
+ * @note	Set the g->priv member to whatever is appropriate. For multiple
+ * 			displays this might be a pointer to the appropriate register set.
+ *
+ * @notapi
+ */
+static inline void init_board(GDisplay *g) {
+	(void) g;
+}
+
+/**
+ * @brief   After the initialisation.
+ *
+ * @param[in] g			The GDisplay structure
+ *
+ * @notapi
+ */
+static inline void post_init_board(GDisplay *g) {
+	(void) g;
+}
+
+/**
+ * @brief   Set or clear the lcd reset pin.
+ *
+ * @param[in] g			The GDisplay structure
+ * @param[in] state		TRUE = lcd in reset, FALSE = normal operation
+ * 
+ * @notapi
+ */
+static inline void setpin_reset(GDisplay *g, bool_t state) {
+	(void) g;
+	(void) state;
+}
+
+/**
+ * @brief   Set the lcd back-light level.
+ *
+ * @param[in] g				The GDisplay structure
+ * @param[in] percent		0 to 100%
+ * 
+ * @notapi
+ */
+static inline void set_backlight(GDisplay *g, uint8_t percent) {
+	(void) g;
+	(void) percent;
+}
+
+/**
+ * @brief   Take exclusive control of the bus
+ *
+ * @param[in] g				The GDisplay structure
+ *
+ * @notapi
+ */
+static inline void acquire_bus(GDisplay *g) {
+	(void) g;
+}
+
+/**
+ * @brief   Release exclusive control of the bus
+ *
+ * @param[in] g				The GDisplay structure
+ *
+ * @notapi
+ */
+static inline void release_bus(GDisplay *g) {
+	(void) g;
+}
+
+/**
+ * @brief   Send data to the index register.
+ *
+ * @param[in] g				The GDisplay structure
+ * @param[in] index			The index register to set
+ *
+ * @notapi
+ */
+static inline void write_index(GDisplay *g, uint16_t index) {
+	(void) g;
+	(void) index;
+}
+
+/**
+ * @brief   Send data to the lcd.
+ *
+ * @param[in] g				The GDisplay structure
+ * @param[in] data			The data to send
+ * 
+ * @notapi
+ */
+static inline void write_data(GDisplay *g, uint16_t data) {
+	(void) g;
+	(void) data;
+}
+
+/**
+ * @brief   Set the bus in read mode
+ *
+ * @param[in] g				The GDisplay structure
+ *
+ * @notapi
+ */
+static inline void setreadmode(GDisplay *g) {
+	(void) g;
+}
+
+/**
+ * @brief   Set the bus back into write mode
+ *
+ * @param[in] g				The GDisplay structure
+ *
+ * @notapi
+ */
+static inline void setwritemode(GDisplay *g) {
+	(void) g;
+}
+
+/**
+ * @brief   Read data from the lcd.
+ * @return	The data from the lcd
+ *
+ * @param[in] g				The GDisplay structure
+ *
+ * @notapi
+ */
+static inline uint16_t read_data(GDisplay *g) {
+	(void) g;
+	return 0;
+}
+
+#endif /* _GDISP_LLD_BOARD_H */
+/** @} */
diff --git a/drivers/gdisp/S6D1121/gdisp_lld.c b/drivers/gdisp/S6D1121/gdisp_lld.c
index e570c1e8..8b6a956c 100644
--- a/drivers/gdisp/S6D1121/gdisp_lld.c
+++ b/drivers/gdisp/S6D1121/gdisp_lld.c
@@ -17,13 +17,6 @@
 
 #if GFX_USE_GDISP /*|| defined(__DOXYGEN__)*/
 
-/* Include the emulation code for things we don't support */
-#include "gdisp/lld/emulation.c"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
 #if defined(GDISP_SCREEN_HEIGHT)
 	#warning "GDISP: This low level driver does not support setting a screen size. It is being ignored."
 	#undef GISP_SCREEN_HEIGHT
@@ -33,525 +26,303 @@
 	#undef GDISP_SCREEN_WIDTH
 #endif
 
-#define GDISP_SCREEN_HEIGHT		320
-#define GDISP_SCREEN_WIDTH		240
+#define GDISP_DRIVER_VMT			GDISPVMT_S6D1121
+#include "../drivers/gdisp/S6D1121/gdisp_lld_config.h"
+#include "gdisp/lld/gdisp_lld.h"
 
-#define GDISP_INITIAL_CONTRAST 		50
-#define GDISP_INITIAL_BACKLIGHT 	100
+#include "board_S6D1121.h"
 
 /*===========================================================================*/
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
-#include "gdisp_lld_board.h"
+#ifndef GDISP_SCREEN_HEIGHT
+	#define GDISP_SCREEN_HEIGHT		320
+#endif
+#ifndef GDISP_SCREEN_WIDTH
+	#define GDISP_SCREEN_WIDTH		240
+#endif
+#ifndef GDISP_INITIAL_CONTRAST
+	#define GDISP_INITIAL_CONTRAST	50
+#endif
+#ifndef GDISP_INITIAL_BACKLIGHT
+	#define GDISP_INITIAL_BACKLIGHT	100
+#endif
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
 
 /* Some common routines and macros */
-#define write_reg(reg, data)        { write_index(reg); write_data(data); }
-#define stream_start()              write_index(0x0022);
-#define stream_stop()
+#define dummy_read(g)				{ volatile uint16_t dummy; dummy = read_data(g); (void) dummy; }
+#define write_reg(g, reg, data)		{ write_index(g, reg); write_data(g, data); }
 #define delay(us)                   gfxSleepMicroseconds(us)
 #define delayms(ms)                 gfxSleepMilliseconds(ms)
 
-static inline void set_cursor(coord_t x, coord_t y) {
+static inline void set_cursor(GDisplay *g) {
     /* R20h - 8 bit
      * R21h - 9 bit
      */
-    switch(GDISP.Orientation) {
+    switch(g->g.Orientation) {
         case GDISP_ROTATE_0:
-            write_reg(0x0020, x & 0x00FF);
-            write_reg(0x0021, y & 0x01FF);
+            write_reg(g, 0x20, g->p.x & 0x00FF);
+            write_reg(g, 0x21, g->p.y & 0x01FF);
             break;
         case GDISP_ROTATE_90:
-            /* Note X has already been mirrored, so we do it directly */
-            write_reg(0x0020, y & 0x00FF);
-            write_reg(0x0021, x & 0x01FF);
+            write_reg(g, 0x20, g->p.y & 0x00FF);
+            write_reg(g, 0x21, (GDISP_SCREEN_HEIGHT - 1 - g->p.x) & 0x01FF);
             break;
         case GDISP_ROTATE_180:
-            write_reg(0x0020, (GDISP_SCREEN_WIDTH - 1 - x) & 0x00FF);
-            write_reg(0x0021, (GDISP_SCREEN_HEIGHT - 1 - y) & 0x01FF);
+            write_reg(g, 0x20, (GDISP_SCREEN_WIDTH - 1 - g->p.x) & 0x00FF);
+            write_reg(g, 0x21, (GDISP_SCREEN_HEIGHT - 1 - g->p.y) & 0x01FF);
             break;
         case GDISP_ROTATE_270:
-            write_reg(0x0020, (GDISP_SCREEN_WIDTH - 1 - y) & 0x00FF);
-            write_reg(0x0021, (GDISP_SCREEN_HEIGHT - 1 - x) & 0x01FF);
+            write_reg(g, 0x20, (GDISP_SCREEN_WIDTH - 1 - g->p.y) & 0x00FF);
+            write_reg(g, 0x21, g->p.x & 0x01FF);
             break;
-    } 
+    }
+    write_index(g, 0x22);
 }
 
-static inline void set_viewport(coord_t x, coord_t y, coord_t cx, coord_t cy) {
+static inline void set_viewport(GDisplay *g) {
     /* HSA / HEA are 8 bit
      * VSA / VEA are 9 bit
      * use masks 0x00FF and 0x01FF to enforce this
      */
 
-    switch(GDISP.Orientation) {
+    switch(g->g.Orientation) {
         case GDISP_ROTATE_0:
-            write_reg(0x46, (((x + cx - 1) << 8) & 0xFF00 ) | 
-                                      (x & 0x00FF));
-
-            write_reg(0x48, y & 0x01FF);
-            write_reg(0x47, (y + cy - 1) & 0x01FF);
+            write_reg(g, 0x46, (((g->p.x + g->p.cx - 1) << 8) & 0xFF00 ) | (g->p.x & 0x00FF));
+            write_reg(g, 0x48, g->p.y & 0x01FF);
+            write_reg(g, 0x47, (g->p.y + g->p.cy - 1) & 0x01FF);
             break;
         case GDISP_ROTATE_90:
-            write_reg(0x46, (((y + cy - 1) << 8) & 0xFF00) |
-                                      (y & 0x00FF));
-
-            write_reg(0x48, x & 0x01FF);
-            write_reg(0x47, (x + cx - 1) & 0x01FF);
+            write_reg(g, 0x46, (((g->p.y + g->p.cy - 1) << 8) & 0xFF00) | (g->p.y & 0x00FF));
+            write_reg(g, 0x48, (GDISP_SCREEN_HEIGHT - (g->p.x + g->p.cx)) & 0x01FF);
+            write_reg(g, 0x47, (GDISP_SCREEN_HEIGHT-1 - g->p.x) & 0x01FF);
             break;
         case GDISP_ROTATE_180:
-            write_reg(0x46, (((GDISP_SCREEN_WIDTH - x - 1) & 0x00FF) << 8) |
-                                      ((GDISP_SCREEN_WIDTH - (x + cx)) & 0x00FF));
-            write_reg(0x48, (GDISP_SCREEN_HEIGHT - (y + cy)) & 0x01FF);
-            write_reg(0x47, (GDISP_SCREEN_HEIGHT- y - 1) & 0x01FF);
+            write_reg(g, 0x46, (((GDISP_SCREEN_WIDTH-1 - g->p.x) & 0x00FF) << 8) |
+                                      ((GDISP_SCREEN_WIDTH - (g->p.x + g->p.cx)) & 0x00FF));
+            write_reg(g, 0x48, (GDISP_SCREEN_HEIGHT - (g->p.y + g->p.cy)) & 0x01FF);
+            write_reg(g, 0x47, (GDISP_SCREEN_HEIGHT-1 - g->p.y) & 0x01FF);
             break;
         case GDISP_ROTATE_270:
-            write_reg(0x46, (((GDISP_SCREEN_WIDTH - y - 1) & 0x00FF) << 8) |
-                                      ((GDISP_SCREEN_WIDTH - (y + cy)) & 0x00FF));
-            write_reg(0x48, (GDISP_SCREEN_HEIGHT - (x + cx)) & 0x01FF);
-            write_reg(0x47, (GDISP_SCREEN_HEIGHT - x - 1) & 0x01FF);
+            write_reg(g, 0x46, (((GDISP_SCREEN_WIDTH-1 - g->p.y) & 0x00FF) << 8) |
+                                      ((GDISP_SCREEN_WIDTH - (g->p.y + g->p.cy)) & 0x00FF));
+            write_reg(g, 0x48, g->p.x & 0x01FF);
+            write_reg(g, 0x47, (g->p.x + g->p.cx - 1) & 0x01FF);
             break;
     }   
-
-    set_cursor(x, y);
 }
 
-static inline void reset_viewport(void) {
-	switch(GDISP.Orientation) {
-		case GDISP_ROTATE_0:
-		case GDISP_ROTATE_180:
-			set_viewport(0, 0, GDISP_SCREEN_WIDTH, GDISP_SCREEN_HEIGHT);
-			break;
-		case GDISP_ROTATE_90:
-		case GDISP_ROTATE_270:
-			set_viewport(0, 0, GDISP_SCREEN_HEIGHT, GDISP_SCREEN_WIDTH);
-			break;
-	}
-}
-
-bool_t gdisp_lld_init(void) {
+LLDSPEC bool_t gdisp_lld_init(GDisplay *g) {
 	/* initialize the hardware */
-	init_board();
+	init_board(g);
 
 	/* Hardware reset */
-	setpin_reset(TRUE);
+	setpin_reset(g, TRUE);
 	delayms(20);
-	setpin_reset(TRUE);
+	setpin_reset(g, TRUE);
 	delayms(20);
 
 	/* Get the bus for the following initialisation commands */
-	acquire_bus();
+	acquire_bus(g);
 
-	write_reg(0x11,0x2004);
-	write_reg(0x13,0xCC00);
-	write_reg(0x15,0x2600);
-	write_reg(0x14,0x252A);
-	write_reg(0x12,0x0033);
-	write_reg(0x13,0xCC04);
+	write_reg(g, 0x11, 0x2004);
+	write_reg(g, 0x13, 0xCC00);
+	write_reg(g, 0x15, 0x2600);
+	write_reg(g, 0x14, 0x252A);
+	write_reg(g, 0x12, 0x0033);
+	write_reg(g, 0x13, 0xCC04);
 
 	delayms(1);
 
-	write_reg(0x13,0xCC06);
+	write_reg(g, 0x13, 0xCC06);
 
 	delayms(1);
 
-	write_reg(0x13,0xCC4F);
+	write_reg(g, 0x13, 0xCC4F);
 
 	delayms(1);
 
-	write_reg(0x13,0x674F);
-	write_reg(0x11,0x2003);
+	write_reg(g, 0x13, 0x674F);
+	write_reg(g, 0x11, 0x2003);
 
 	delayms(1);
 
 	// Gamma Setting
-	write_reg(0x30,0x2609);
-	write_reg(0x31,0x242C);
-	write_reg(0x32,0x1F23);
-	write_reg(0x33,0x2425);
-	write_reg(0x34,0x2226);
-	write_reg(0x35,0x2523);
-	write_reg(0x36,0x1C1A);
-	write_reg(0x37,0x131D);
-	write_reg(0x38,0x0B11);
-	write_reg(0x39,0x1210);
-	write_reg(0x3A,0x1315);
-	write_reg(0x3B,0x3619);
-	write_reg(0x3C,0x0D00);
-	write_reg(0x3D,0x000D);
-
-	write_reg(0x16,0x0007);
-	write_reg(0x02,0x0013);
-	write_reg(0x03,0x0003);
-	write_reg(0x01,0x0127);
+	write_reg(g, 0x30, 0x2609);
+	write_reg(g, 0x31, 0x242C);
+	write_reg(g, 0x32, 0x1F23);
+	write_reg(g, 0x33, 0x2425);
+	write_reg(g, 0x34, 0x2226);
+	write_reg(g, 0x35, 0x2523);
+	write_reg(g, 0x36, 0x1C1A);
+	write_reg(g, 0x37, 0x131D);
+	write_reg(g, 0x38, 0x0B11);
+	write_reg(g, 0x39, 0x1210);
+	write_reg(g, 0x3A, 0x1315);
+	write_reg(g, 0x3B, 0x3619);
+	write_reg(g, 0x3C, 0x0D00);
+	write_reg(g, 0x3D, 0x000D);
+
+	write_reg(g, 0x16, 0x0007);
+	write_reg(g, 0x02, 0x0013);
+	write_reg(g, 0x03, 0x0003);
+	write_reg(g, 0x01, 0x0127);
 
 	delayms(1);
 
-	write_reg(0x08,0x0303);
-	write_reg(0x0A,0x000B);
-	write_reg(0x0B,0x0003);
-	write_reg(0x0C,0x0000);
-	write_reg(0x41,0x0000);
-	write_reg(0x50,0x0000);
-	write_reg(0x60,0x0005);
-	write_reg(0x70,0x000B);
-	write_reg(0x71,0x0000);
-	write_reg(0x78,0x0000);
-	write_reg(0x7A,0x0000);
-	write_reg(0x79,0x0007);
-	write_reg(0x07,0x0051);
+	write_reg(g, 0x08, 0x0303);
+	write_reg(g, 0x0A, 0x000B);
+	write_reg(g, 0x0B, 0x0003);
+	write_reg(g, 0x0C, 0x0000);
+	write_reg(g, 0x41, 0x0000);
+	write_reg(g, 0x50, 0x0000);
+	write_reg(g, 0x60, 0x0005);
+	write_reg(g, 0x70, 0x000B);
+	write_reg(g, 0x71, 0x0000);
+	write_reg(g, 0x78, 0x0000);
+	write_reg(g, 0x7A, 0x0000);
+	write_reg(g, 0x79, 0x0007);
+	write_reg(g, 0x07, 0x0051);
 
 	delayms(1);
 
-	write_reg(0x07,0x0053);
-	write_reg(0x79,0x0000);
-
-	reset_viewport();
-	set_backlight(GDISP_INITIAL_BACKLIGHT);
-
-	/* Now initialise the GDISP structure */
-	GDISP.Width = GDISP_SCREEN_WIDTH;
-	GDISP.Height = GDISP_SCREEN_HEIGHT;
-	GDISP.Orientation = GDISP_ROTATE_0;
-	GDISP.Powermode = powerOn;
-	GDISP.Backlight = 100;
-	GDISP.Contrast = 50;
-	#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-		GDISP.clipx0 = 0;
-		GDISP.clipy0 = 0;
-		GDISP.clipx1 = GDISP.Width;
-		GDISP.clipy1 = GDISP.Height;
-	#endif
-	return TRUE;
-}
+	write_reg(g, 0x07,0x0053);
+	write_reg(g, 0x79,0x0000);
 
-/**
- * @brief   Draws a pixel on the display.
- *
- * @param[in] x        X location of the pixel
- * @param[in] y        Y location of the pixel
- * @param[in] color    The color of the pixel
- *
- * @notapi
- */
-void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
-	#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-		if (x < GDISP.clipx0 || y < GDISP.clipy0 || x >= GDISP.clipx1 || y >= GDISP.clipy1) return;
-	#endif
-
-	acquire_bus();
-	set_cursor(x, y);
-	write_reg(0x0022, color);
-	release_bus();
-}
+    // Finish Init
+    post_init_board(g);
 
-/* ---- Optional Routines ---- */
-
-#if GDISP_HARDWARE_CLEARS || defined(__DOXYGEN__)
-	/**
-	 * @brief   Clear the display.
-	 * @note    Optional - The high level driver can emulate using software.
-	 *
-	 * @param[in] color    The color of the pixel
-	 *
-	 * @notapi
-	 */
-	void gdisp_lld_clear(color_t color) {
-	    unsigned i;
-
-		acquire_bus();
-	    set_cursor(0, 0);
-	    stream_start();
-
-	    for(i = 0; i < GDISP_SCREEN_WIDTH * GDISP_SCREEN_HEIGHT; i++)
-	    	write_data(color);
-
-	    stream_stop();
-		release_bus();
-	}
-#endif
-
-#if GDISP_HARDWARE_FILLS || defined(__DOXYGEN__)
-	/**
-	 * @brief   Fill an area with a color.
-	 * @note    Optional - The high level driver can emulate using software.
-	 *
-	 * @param[in] x, y     The start filled area
-	 * @param[in] cx, cy   The width and height to be filled
-	 * @param[in] color    The color of the fill
-	 *
-	 * @notapi
-	 */
-	void gdisp_lld_fill_area(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
-		unsigned i, area;
-
-		#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-			if (x < GDISP.clipx0) { cx -= GDISP.clipx0 - x; x = GDISP.clipx0; }
-			if (y < GDISP.clipy0) { cy -= GDISP.clipy0 - y; y = GDISP.clipy0; }
-			if (cx <= 0 || cy <= 0 || x >= GDISP.clipx1 || y >= GDISP.clipy1) return;
-			if (x+cx > GDISP.clipx1)	cx = GDISP.clipx1 - x;
-			if (y+cy > GDISP.clipy1)	cy = GDISP.clipy1 - y;
-		#endif
+ 	// Release the bus
+	release_bus(g);
 
-		area = cx*cy;
-		acquire_bus();
-		set_viewport(x, y, cx, cy);
-		stream_start();
-		for(i = 0; i < area; i++)
-			write_data(color);
-		stream_stop();
-		reset_viewport();
-		release_bus();
-	}
-#endif
+	/* Turn on the back-light */
+	set_backlight(g, GDISP_INITIAL_BACKLIGHT);
 
-#if GDISP_HARDWARE_BITFILLS || defined(__DOXYGEN__)
-	/**
-	 * @brief   Fill an area with a bitmap.
-	 * @note    Optional - The high level driver can emulate using software.
-	 *
-	 * @param[in] x, y     The start filled area
-	 * @param[in] cx, cy   The width and height to be filled
-	 * @param[in] srcx, srcy   The bitmap position to start the fill from
-	 * @param[in] srccx    The width of a line in the bitmap.
-	 * @param[in] buffer   The pixels to use to fill the area.
-	 *
-	 * @notapi
-	 */
-	void gdisp_lld_blit_area_ex(coord_t x, coord_t y, coord_t cx, coord_t cy, coord_t srcx, coord_t srcy, coord_t srccx, const pixel_t *buffer) {
-		coord_t endx, endy;
-		unsigned lg;
-
-		#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-			if (x < GDISP.clipx0) { cx -= GDISP.clipx0 - x; srcx += GDISP.clipx0 - x; x = GDISP.clipx0; }
-			if (y < GDISP.clipy0) { cy -= GDISP.clipy0 - y; srcy += GDISP.clipy0 - y; y = GDISP.clipy0; }
-			if (srcx+cx > srccx)		cx = srccx - srcx;
-			if (cx <= 0 || cy <= 0 || x >= GDISP.clipx1 || y >= GDISP.clipy1) return;
-			if (x+cx > GDISP.clipx1)	cx = GDISP.clipx1 - x;
-			if (y+cy > GDISP.clipy1)	cy = GDISP.clipy1 - y;
-		#endif
+	/* Initialise the GDISP structure */
+	g->g.Width = GDISP_SCREEN_WIDTH;
+	g->g.Height = GDISP_SCREEN_HEIGHT;
+	g->g.Orientation = GDISP_ROTATE_0;
+	g->g.Powermode = powerOn;
+	g->g.Backlight = GDISP_INITIAL_BACKLIGHT;
+	g->g.Contrast = GDISP_INITIAL_CONTRAST;
+	return TRUE;
+}
 
-		acquire_bus();
-		set_viewport(x, y, cx, cy);
-		stream_start();
-
-		endx = srcx + cx;
-		endy = y + cy;
-		lg = srccx - cx;
-		buffer += srcx + srcy * srccx;
-		for(; y < endy; y++, buffer += lg)
-			for(x=srcx; x < endx; x++)
-				write_data(*buffer++);
-		stream_stop();
-		reset_viewport();
-		release_bus();
+#if GDISP_HARDWARE_STREAM_WRITE
+	LLDSPEC	void gdisp_lld_write_start(GDisplay *g) {
+		acquire_bus(g);
+		set_viewport(g);
 	}
-#endif
-
-#if (GDISP_NEED_PIXELREAD && GDISP_HARDWARE_PIXELREAD) || defined(__DOXYGEN__)
-	/**
-	 * @brief   Get the color of a particular pixel.
-	 * @note    Optional.
-	 * @note    If x,y is off the screen, the result is undefined.
-	 *
-	 * @param[in] x, y     The start of the text
-	 *
-	 * @notapi
-	 */
-	color_t gdisp_lld_get_pixel_color(coord_t x, coord_t y) {
-		/* This routine is marked "DO NOT USE" in the original
-		 *  GLCD driver. We just keep our GDISP_HARDWARE_READPIXEL
-		 *  turned off for now.
-		 */
-		color_t color;
-
-		#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-			if (x < 0 || x >= GDISP.Width || y < 0 || y >= GDISP.Height) return 0;
-		#endif
-
-		aquire_bus();
-		set_cursor(x, y);
-		stream_start();
-
-		color = lld_lcdReadData();
-		color = lld_lcdReadData();
-
-		stream_stop();
-		release_bus();
-
-		return color;
+	LLDSPEC	void gdisp_lld_write_color(GDisplay *g) {
+		write_data(g, g->p.color);
+	}
+	LLDSPEC	void gdisp_lld_write_stop(GDisplay *g) {
+		release_bus(g);
+	}
+	LLDSPEC void gdisp_lld_write_pos(GDisplay *g) {
+		set_cursor(g);
 	}
 #endif
 
-#if (GDISP_NEED_SCROLL && GDISP_HARDWARE_SCROLL) || defined(__DOXYGEN__)
-	/**
-	 * @brief   Scroll vertically a section of the screen.
-	 * @note    Optional.
-	 * @note    If x,y + cx,cy is off the screen, the result is undefined.
-	 * @note    If lines is >= cy, it is equivelent to a area fill with bgcolor.
-	 *
-	 * @param[in] x, y     The start of the area to be scrolled
-	 * @param[in] cx, cy   The size of the area to be scrolled
-	 * @param[in] lines    The number of lines to scroll (Can be positive or negative)
-	 * @param[in] bgcolor  The color to fill the newly exposed area.
-	 *
-	 * @notapi
-	 */
-	void gdisp_lld_vertical_scroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
-		/* This is marked as "TODO: Test this" in the original GLCD driver.
-		 * For now we just leave the GDISP_HARDWARE_SCROLL off.
-		 */
-		static color_t buf[((GDISP_SCREEN_HEIGHT > GDISP_SCREEN_WIDTH ) ? GDISP_SCREEN_HEIGHT : GDISP_SCREEN_WIDTH)];
-		coord_t row0, row1;
-		unsigned i, gap, abslines;
-
-		#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-			if (x < GDISP.clipx0) { cx -= GDISP.clipx0 - x; x = GDISP.clipx0; }
-			if (y < GDISP.clipy0) { cy -= GDISP.clipy0 - y; y = GDISP.clipy0; }
-			if (!lines || cx <= 0 || cy <= 0 || x >= GDISP.clipx1 || y >= GDISP.clipy1) return;
-			if (x+cx > GDISP.clipx1)	cx = GDISP.clipx1 - x;
-			if (y+cy > GDISP.clipy1)	cy = GDISP.clipy1 - y;
-		#endif
-
-		abslines = lines < 0 ? -lines : lines;
-
-		acquire_bus();
-		if (abslines >= cy) {
-			abslines = cy;
-			gap = 0;
-		} else {
-			gap = cy - abslines;
-			for(i = 0; i < gap; i++) {
-				if(lines > 0) {
-					row0 = y + i + lines;
-					row1 = y + i;
-				} else {
-					row0 = (y - i - 1) + lines;
-					row1 = (y - i - 1);
-				}
-
-				/* read row0 into the buffer and then write at row1*/
-				set_viewport(x, row0, cx, 1);
-				lld_lcdReadStreamStart();
-				lld_lcdReadStream(buf, cx);
-				lld_lcdReadStreamStop();
-
-				set_viewport(x, row1, cx, 1);
-				stream_start();
-				write_data(buf, cx);
-				stream_stop();
-			}
-		}
-
-		/* fill the remaining gap */
-		set_viewport(x, lines > 0 ? (y+gap) : y, cx, abslines);
-		stream_start();
-		gap = cx*abslines;
-		for(i = 0; i < gap; i++) write_data(bgcolor);
-		stream_stop();
-		reset_viewport();
-		release_bus();
+#if GDISP_HARDWARE_STREAM_READ
+	LLDSPEC	void gdisp_lld_read_start(GDisplay *g) {
+		acquire_bus(g);
+		set_viewport(g);
+		set_cursor(g);
+		setreadmode(g);
+		dummy_read(g);
+	}
+	LLDSPEC	color_t gdisp_lld_read_color(GDisplay *g) {
+		return read_data(g);
+	}
+	LLDSPEC	void gdisp_lld_read_stop(GDisplay *g) {
+		setwritemode(g);
+		release_bus(g);
 	}
 #endif
 
-#if GDISP_HARDWARE_CONTROL || defined(__DOXYGEN__)
-	/**
-	 * @brief   Driver Control
-	 * @details	Unsupported control codes are ignored.
-	 * @note	The value parameter should always be typecast to (void *).
-	 * @note	There are some predefined and some specific to the low level driver.
-	 * @note	GDISP_CONTROL_POWER			- Takes a gdisp_powermode_t
-	 * 			GDISP_CONTROL_ORIENTATION	- Takes a gdisp_orientation_t
-	 * 			GDISP_CONTROL_BACKLIGHT -	 Takes an int from 0 to 100. For a driver
-	 * 											that only supports off/on anything other
-	 * 											than zero is on.
-	 * 			GDISP_CONTROL_CONTRAST		- Takes an int from 0 to 100.
-	 * 			GDISP_CONTROL_LLD			- Low level driver control constants start at
-	 * 											this value.
-	 *
-	 * @param[in] what		What to do.
-	 * @param[in] value		The value to use (always cast to a void *).
-	 *
-	 * @notapi
-	 */
-	void gdisp_lld_control(unsigned what, void *value) {
-		switch(what) {
-		case GDISP_CONTROL_POWER:
-			if (GDISP.Powermode == (gdisp_powermode_t)value)
-				return;
-			switch((gdisp_powermode_t)value) {
+#if GDISP_NEED_CONTROL && GDISP_HARDWARE_CONTROL
+	LLDSPEC void gdisp_lld_control(GDisplay *g) {
+		switch(g->p.x) {
+		#if 0
+			case GDISP_CONTROL_POWER:
+				if (g->g.Powermode == (powermode_t)g->p.ptr)
+					return;
+				switch((powermode_t)g->p.ptr) {
 				case powerOff:
-					/* 	Code here */
-					/* break; */
+					acquire_bus(g);
+					// TODO
+					release_bus(g);
+					break;
 				case powerOn:
-					/* 	Code here */
-					/* You may need this ---
-						if (GDISP.Powermode != powerSleep)
-							gdisp_lld_init();
-					*/
-					/* break; */
+					acquire_bus(g);
+					// TODO
+					release_bus(g);
+					break;
 				case powerSleep:
-					/* 	Code here */
-					/* break; */
+					acquire_bus(g);
+					// TODO
+					release_bus(g);
+					break;
 				default:
 					return;
-			}
-			GDISP.Powermode = (gdisp_powermode_t)value;
-			return;
+				}
+				g->g.Powermode = (powermode_t)g->p.ptr;
+				return;
+		#endif
+
 		case GDISP_CONTROL_ORIENTATION:
-			if (GDISP.Orientation == (gdisp_orientation_t)value)
+			if (g->g.Orientation == (orientation_t)g->p.ptr)
 				return;
-			switch((gdisp_orientation_t)value) {
+			switch((orientation_t)g->p.ptr) {
 			case GDISP_ROTATE_0:
-				acquire_bus();
-				write_reg(0x0001,0x0127);
-				write_reg(0x03, 0b0011);
-				release_bus();
-
-				GDISP.Height = GDISP_SCREEN_HEIGHT;
-				GDISP.Width = GDISP_SCREEN_WIDTH;
+				acquire_bus(g);
+				write_reg(g, 0x03, 0b0011);
+				release_bus(g);
+				g->g.Height = GDISP_SCREEN_HEIGHT;
+				g->g.Width = GDISP_SCREEN_WIDTH;
 				break;
 			case GDISP_ROTATE_90:
-				acquire_bus();
-				write_reg(0x0001,0x0027);
-				write_reg(0x0003, 0b1011);
-				release_bus();
-
-				GDISP.Height = GDISP_SCREEN_WIDTH;
-				GDISP.Width = GDISP_SCREEN_HEIGHT;
+				acquire_bus(g);
+				write_reg(g, 0x03, 0b1001);
+				release_bus(g);
+				g->g.Height = GDISP_SCREEN_WIDTH;
+				g->g.Width = GDISP_SCREEN_HEIGHT;
 				break;
 			case GDISP_ROTATE_180:
-				acquire_bus();
-				write_reg(0x0001,0x0127);
-				write_reg(0x0003, 0b0000);
-				release_bus();
-
-				GDISP.Height = GDISP_SCREEN_HEIGHT;
-				GDISP.Width = GDISP_SCREEN_WIDTH;
+				acquire_bus(g);
+				write_reg(g, 0x03, 0b0000);
+				release_bus(g);
+				g->g.Height = GDISP_SCREEN_HEIGHT;
+				g->g.Width = GDISP_SCREEN_WIDTH;
 				break;
 			case GDISP_ROTATE_270:
-				acquire_bus();
-				write_reg(0x0001,0x0027);
-				write_reg(0x0003, 0b1000);
-				release_bus();
-
-				GDISP.Height = GDISP_SCREEN_WIDTH;
-				GDISP.Width = GDISP_SCREEN_HEIGHT;
+				acquire_bus(g);
+				write_reg(g, 0x03, 0b1010);
+				release_bus(g);
+				g->g.Height = GDISP_SCREEN_WIDTH;
+				g->g.Width = GDISP_SCREEN_HEIGHT;
 				break;
 			default:
 				return;
 			}
-			#if GDISP_NEED_CLIP || GDISP_NEED_VALIDATION
-				GDISP.clipx0 = 0;
-				GDISP.clipy0 = 0;
-				GDISP.clipx1 = GDISP.Width;
-				GDISP.clipy1 = GDISP.Height;
-			#endif
-			GDISP.Orientation = (gdisp_orientation_t)value;
+			g->g.Orientation = (orientation_t)value;
 			return;
-/*
-		case GDISP_CONTROL_BACKLIGHT:
-		case GDISP_CONTROL_CONTRAST:
-*/
+
+        case GDISP_CONTROL_BACKLIGHT:
+            if ((unsigned)g->p.ptr > 100)
+            	g->p.ptr = (void *)100;
+            set_backlight(g, (unsigned)g->p.ptr);
+            g->g.Backlight = (unsigned)g->p.ptr;
+            return;
+
+		//case GDISP_CONTROL_CONTRAST:
+        default:
+            return;
 		}
 	}
 #endif
diff --git a/drivers/gdisp/S6D1121/gdisp_lld.mk b/drivers/gdisp/S6D1121/gdisp_lld.mk
index 07f9c380..f5f067b7 100644
--- a/drivers/gdisp/S6D1121/gdisp_lld.mk
+++ b/drivers/gdisp/S6D1121/gdisp_lld.mk
@@ -1,5 +1,2 @@
-# List the required driver.
-GFXSRC += $(GFXLIB)/drivers/gdisp/S6D1121/gdisp_lld.c
-
-# Required include directories
 GFXINC += $(GFXLIB)/drivers/gdisp/S6D1121
+GFXSRC += $(GFXLIB)/drivers/gdisp/S6D1121/gdisp_lld.c
diff --git a/drivers/gdisp/S6D1121/gdisp_lld_board_olimex_e407.h b/drivers/gdisp/S6D1121/gdisp_lld_board_olimex_e407.h
deleted file mode 100644
index 0ce843b4..00000000
--- a/drivers/gdisp/S6D1121/gdisp_lld_board_olimex_e407.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- * This file is subject to the terms of the GFX License. If a copy of
- * the license was not distributed with this file, you can obtain one at:
- *
- *              http://ugfx.org/license.html
- */
-
-/**
- * @file    drivers/gdisp/S6D1121/gdisp_lld_board_olimex_e407.h
- * @brief   GDISP Graphic Driver subsystem board interface for the S6D1121 display
- *
- * @addtogroup GDISP
- * @{
- */
-
-#ifndef _GDISP_LLD_BOARD_H
-#define _GDISP_LLD_BOARD_H
-
-#define GDISP_REG              (*((volatile uint16_t *) 0x60000000)) /* RS = 0 */
-#define GDISP_RAM              (*((volatile uint16_t *) 0x60020000)) /* RS = 1 */
-
-static inline void init_board(void) {
-	int FSMC_Bank = 0;
-
-	/* STM32F4 FSMC init */
-	rccEnableAHB3(RCC_AHB3ENR_FSMCEN, 0);
-
-	/* set pins to FSMC mode */
-	IOBus busD = {GPIOD, (1 << 0) | (1 << 1) | (1 << 4) | (1 << 5) | (1 << 7) | (1 << 8) |
- 						(1 << 9) | (1 << 10) | (1 << 11) | (1 << 14) | (1 << 15), 0};
-
-	IOBus busE = {GPIOE, (1 << 7) | (1 << 8) | (1 << 9) | (1 << 10) | (1 << 11) | (1 << 12) |
-						(1 << 13) | (1 << 14) | (1 << 15), 0};
-
-	palSetBusMode(&busD, PAL_MODE_ALTERNATE(12));
-	palSetBusMode(&busE, PAL_MODE_ALTERNATE(12));
-
-	/* FSMC timing */
-	FSMC_Bank1->BTCR[FSMC_Bank+1] = (6) | (10 << 8) | (10 << 16);
-
-	/* Bank1 NOR/SRAM control register configuration */
-	FSMC_Bank1->BTCR[FSMC_Bank] = FSMC_BCR1_MWID_0 | FSMC_BCR1_WREN | FSMC_BCR1_MBKEN;	
-}
-
-static inline void setpin_reset(bool_t state) {
-	(void)state;
-
-	/* Nothing to do here */
-}
-
-static inline void set_backlight(uint8_t percent) {
-	(void)percent;
-
-	/* Nothing to do here */
-}
-
-static inline void acquire_bus(void) {
-	/* Nothing to do here */
-}
-
-static inline void release_bus(void) {
-	/* Nothing to do here */
-}
-
-static inline void write_index(uint16_t index) {
-	GDISP_REG = index;
-}
-
-static inline void write_data(uint16_t data) {
-	GDISP_RAM = data;
-}
-
-#if GDISP_HARDWARE_READPIXEL || GDISP_HARDWARE_SCROLL || defined(__DOXYGEN__)
-static inline uint16_t read_data(void) {
-	return GDISP_RAM;
-}
-#endif
-
-#endif /* _GDISP_LLD_BOARD_H */
-/** @} */
-
diff --git a/drivers/gdisp/S6D1121/gdisp_lld_board_template.h b/drivers/gdisp/S6D1121/gdisp_lld_board_template.h
deleted file mode 100644
index f123bd06..00000000
--- a/drivers/gdisp/S6D1121/gdisp_lld_board_template.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/*
- * This file is subject to the terms of the GFX License. If a copy of
- * the license was not distributed with this file, you can obtain one at:
- *
- *              http://ugfx.org/license.html
- */
-
-/**
- * @file    drivers/gdisp/S6D1121/gdisp_lld_board_template.h
- * @brief   GDISP Graphic Driver subsystem board interface for the S6D1121 display.
- *
- * @addtogroup GDISP
- * @{
- */
-
-#ifndef _GDISP_LLD_BOARD_H
-#define _GDISP_LLD_BOARD_H
-
-/**
- * @brief   Initialise the board for the display.
- *
- * @notapi
- */
-static inline void init_board(void) {
-
-}
-
-/**
- * @brief   Set or clear the lcd reset pin.
- *
- * @param[in] state		TRUE = lcd in reset, FALSE = normal operation
- * 
- * @notapi
- */
-static inline void setpin_reset(bool_t state) {
-
-}
-
-/**
- * @brief   Set the lcd back-light level.
- *
- * @param[in] percent		0 to 100%
- * 
- * @notapi
- */
-static inline void set_backlight(uint8_t percent) {
-
-}
-
-/**
- * @brief   Take exclusive control of the bus
- *
- * @notapi
- */
-static inline void acquire_bus(void) {
-
-}
-
-/**
- * @brief   Release exclusive control of the bus
- *
- * @notapi
- */
-static inline void release_bus(void) {
-
-}
-
-/**
- * @brief   Send data to the index register.
- *
- * @param[in] index		The index register to set
- *
- * @notapi
- */
-static inline void write_index(uint16_t index) {
-
-}
-
-/**
- * @brief   Send data to the lcd.
- *
- * @param[in] data		The data to send
- * 
- * @notapi
- */
-static inline void write_data(uint16_t data) {
-
-}
-
-#if GDISP_HARDWARE_READPIXEL || GDISP_HARDWARE_SCROLL || defined(__DOXYGEN__)
-/**
- * @brief   Read data from the lcd.
- *
- * @return	The data from the lcd
- * @note	The chip select may need to be asserted/de-asserted
- * 			around the actual spi read
- * 
- * @notapi
- */
-static inline uint16_t read_data(void) {
-
-}
-#endif
-
-#endif /* _GDISP_LLD_BOARD_H */
-/** @} */
diff --git a/drivers/gdisp/S6D1121/gdisp_lld_config.h b/drivers/gdisp/S6D1121/gdisp_lld_config.h
index bace096c..839bbf9d 100644
--- a/drivers/gdisp/S6D1121/gdisp_lld_config.h
+++ b/drivers/gdisp/S6D1121/gdisp_lld_config.h
@@ -22,13 +22,9 @@
 /* Driver hardware support.                                                  */
 /*===========================================================================*/
 
-#define GDISP_DRIVER_NAME				"S6D1121"
-
-#define GDISP_HARDWARE_CLEARS			TRUE
-#define GDISP_HARDWARE_FILLS			TRUE
-#define GDISP_HARDWARE_BITFILLS			TRUE
-#define GDISP_HARDWARE_SCROLL			TRUE
-#define GDISP_HARDWARE_PIXELREAD		FALSE
+#define GDISP_HARDWARE_STREAM_WRITE		TRUE
+#define GDISP_HARDWARE_STREAM_READ		TRUE
+#define GDISP_HARDWARE_STREAM_POS		TRUE
 #define GDISP_HARDWARE_CONTROL			TRUE
 
 #define GDISP_PIXELFORMAT				GDISP_PIXELFORMAT_RGB565