GDRIVER now working for GDISP including multiple displays.

Still uses old GDISP driver model for now
Untested with uGFXnet.
Still to come: Input drivers etc

9 files changed, 728 insertions, 639 deletions

diff --git a/gfx.h b/gfx.h
index de6047fc..dc7605e8 100644
--- a/gfx.h
+++ b/gfx.h
@@ -59,6 +59,14 @@
  * @{
  */
 	/**
+	 * @brief   GFX Driver API
+	 * @details	Defaults to TRUE
+	 * @note	Not much useful can be done without a driver
+	 */
+	#ifndef GFX_USE_GDRIVER
+		#define GFX_USE_GDRIVER	TRUE
+	#endif
+	/**
 	 * @brief   GFX Graphics Display Basic API
 	 * @details	Defaults to FALSE
 	 * @note	Also add the specific hardware driver to your makefile.
@@ -154,6 +162,7 @@
  *
  */
 #include "src/gos/sys_options.h"
+#include "src/gdriver/sys_options.h"
 #include "src/gfile/sys_options.h"
 #include "src/gmisc/sys_options.h"
 #include "src/gqueue/sys_options.h"
@@ -183,12 +192,14 @@
 #include "src/gqueue/sys_rules.h"
 #include "src/gmisc/sys_rules.h"
 #include "src/gfile/sys_rules.h"
+#include "src/gdriver/sys_rules.h"
 #include "src/gos/sys_rules.h"
 
 /**
  *  Include the sub-system header files
  */
 #include "src/gos/sys_defs.h"
+//#include "src/gdriver/sys_defs.h"			// This module is only included by source that needs it.
 #include "src/gfile/sys_defs.h"
 #include "src/gmisc/sys_defs.h"
 #include "src/gqueue/sys_defs.h"
diff --git a/gfx.mk b/gfx.mk
index 8a2faa2b..d3dbdbed 100644
--- a/gfx.mk
+++ b/gfx.mk
@@ -2,6 +2,7 @@ GFXINC +=   $(GFXLIB)
 GFXSRC +=	$(GFXLIB)/src/gfx.c

 

 include $(GFXLIB)/src/gos/sys_make.mk

+include $(GFXLIB)/src/gdriver/sys_make.mk

 include $(GFXLIB)/src/gqueue/sys_make.mk

 include $(GFXLIB)/src/gdisp/sys_make.mk

 include $(GFXLIB)/src/gevent/sys_make.mk

diff --git a/src/gdisp/driver.h b/src/gdisp/driver.h
index f45b3f87..82435589 100644
--- a/src/gdisp/driver.h
+++ b/src/gdisp/driver.h
@@ -16,17 +16,19 @@
 #ifndef _GDISP_LLD_H
 #define _GDISP_LLD_H
 
-#if GFX_USE_GDISP // || defined(__DOXYGEN__)
+#if GFX_USE_GDISP
 
-/*===========================================================================*/
-/* Error checks.                                                             */
-/*===========================================================================*/
+// Include the GDRIVER infrastructure
+#include "src/gdriver/sys_defs.h"
+
+// Our special auto-detect hardware code which uses the VMT.
+#define HARDWARE_AUTODETECT		2
 
 #if GDISP_TOTAL_CONTROLLERS > 1 && !defined(GDISP_DRIVER_VMT)
-	#define HARDWARE_AUTODETECT		2
+	// Multiple controllers the default is to hardware detect
 	#define HARDWARE_DEFAULT		HARDWARE_AUTODETECT
 #else
-	#define HARDWARE_AUTODETECT		2
+	// The default is to call the routines directly
 	#define HARDWARE_DEFAULT		FALSE
 #endif
 
@@ -182,13 +184,47 @@
 /* External declarations.                                                    */
 /*===========================================================================*/
 
-struct GDisplay {
-	// The public GDISP stuff - must be the first element
-	GDISPControl				g;
+/* Verify information for packed pixels and define a non-packed pixel macro */
+#if !GDISP_PACKED_PIXELS
+	#define gdispPackPixels(buf,cx,x,y,c)	{ ((color_t *)(buf))[(y)*(cx)+(x)] = (c); }
+#elif !GDISP_HARDWARE_BITFILLS
+	#error "GDISP: packed pixel formats are only supported for hardware accelerated drivers."
+#elif GDISP_PIXELFORMAT != GDISP_PIXELFORMAT_RGB888 \
+		&& GDISP_PIXELFORMAT != GDISP_PIXELFORMAT_RGB444 \
+		&& GDISP_PIXELFORMAT != GDISP_PIXELFORMAT_RGB666 \
+		&& GDISP_PIXELFORMAT != GDISP_PIXELFORMAT_CUSTOM
+	#error "GDISP: A packed pixel format has been specified for an unsupported pixel format."
+#endif
 
-	#if GDISP_TOTAL_CONTROLLERS > 1
-		const struct GDISPVMT const *	vmt;		// The Virtual Method Table
-	#endif
+/* Support routine for packed pixel formats */
+#if !defined(gdispPackPixels) || defined(__DOXYGEN__)
+	/**
+	 * @brief   Pack a pixel into a pixel buffer.
+	 * @note    This function performs no buffer boundary checking
+	 *			regardless of whether GDISP_NEED_CLIP has been specified.
+	 *
+	 * @param[in] buf		The buffer to put the pixel in
+	 * @param[in] cx		The width of a pixel line
+	 * @param[in] x, y		The location of the pixel to place
+	 * @param[in] color		The color to put into the buffer
+	 *
+	 * @api
+	 */
+	void gdispPackPixels(const pixel_t *buf, coord_t cx, coord_t x, coord_t y, color_t color);
+#endif
+
+struct GDisplay {
+	struct GDriver				d;					// This must be the first element
+		#define gvmt(g)		((const GDISPVMT const *)(g)->d.vmt)	// For ease of access to the vmt member
+
+	struct GDISPControl {
+		coord_t					Width;
+		coord_t					Height;
+		orientation_t			Orientation;
+		powermode_t				Powermode;
+		uint8_t					Backlight;
+		uint8_t					Contrast;
+	} g;
 
 	void *						priv;				// A private area just for the drivers use.
 	void *						board;				// A private area just for the board interfaces use.
@@ -237,24 +273,47 @@ struct GDisplay {
 		// A pixel line buffer
 		color_t		linebuf[GDISP_LINEBUF_SIZE];
 	#endif
-
 };
 
-#if GDISP_TOTAL_CONTROLLERS == 1 || defined(GDISP_DRIVER_VMT) || defined(__DOXYGEN__)
-	#if GDISP_TOTAL_CONTROLLERS > 1
-		#define LLDSPEC			static
-	#else
-		#define LLDSPEC
-	#endif
-
+typedef struct GDISPVMT {
+	GDriverVMT	vmtdriver;
+	bool_t (*init)(GDisplay *g);
+	void (*writestart)(GDisplay *g);				// Uses p.x,p.y  p.cx,p.cy
+	void (*writepos)(GDisplay *g);					// Uses p.x,p.y
+	void (*writecolor)(GDisplay *g);				// Uses p.color
+	void (*writestop)(GDisplay *g);					// Uses no parameters
+	void (*readstart)(GDisplay *g);					// Uses p.x,p.y  p.cx,p.cy
+	color_t (*readcolor)(GDisplay *g);				// Uses no parameters
+	void (*readstop)(GDisplay *g);					// Uses no parameters
+	void (*pixel)(GDisplay *g);						// Uses p.x,p.y  p.color
+	void (*clear)(GDisplay *g);						// Uses p.color
+	void (*fill)(GDisplay *g);						// Uses p.x,p.y  p.cx,p.cy  p.color
+	void (*blit)(GDisplay *g);						// Uses p.x,p.y  p.cx,p.cy  p.x1,p.y1 (=srcx,srcy)  p.x2 (=srccx), p.ptr (=buffer)
+	color_t (*get)(GDisplay *g);					// Uses p.x,p.y
+	void (*vscroll)(GDisplay *g);					// Uses p.x,p.y  p.cx,p.cy, p.y1 (=lines) p.color
+	void (*control)(GDisplay *g);					// Uses p.x (=what)  p.ptr (=value)
+	void *(*query)(GDisplay *g);					// Uses p.x (=what);
+	void (*setclip)(GDisplay *g);					// Uses p.x,p.y  p.cx,p.cy
+	void (*flush)(GDisplay *g);						// Uses no parameters
+} GDISPVMT;
+
+// Do we need function definitions or macro's (via the VMT)
+#if GDISP_TOTAL_CONTROLLERS <= 1 || defined(GDISP_DRIVER_VMT) || defined(__DOXYGEN__)
 	#ifdef __cplusplus
 	extern "C" {
 	#endif
 
+	// Should the driver routines should be static or not
+	#if GDISP_TOTAL_CONTROLLERS <= 1
+		#define LLDSPEC
+	#else
+		#define LLDSPEC			static
+	#endif
+
 	/**
 	 * @brief   Initialize the driver.
 	 * @return	TRUE if successful.
-	 * @param[in]	g		The driver structure
+	 * @param[in]	g				The driver structure
 	 * @param[out]	g->g	The driver must fill in the GDISPControl structure
 	 */
 	LLDSPEC	bool_t gdisp_lld_init(GDisplay *g);
@@ -497,374 +556,368 @@ struct GDisplay {
 	#ifdef __cplusplus
 	}
 	#endif
-#endif  // GDISP_TOTAL_CONTROLLERS == 1 || defined(GDISP_DRIVER_VMT)
-
-
-#if GDISP_TOTAL_CONTROLLERS > 1
-
-	typedef struct GDISPVMT {
-		bool_t (*init)(GDisplay *g);
-		void (*writestart)(GDisplay *g);				// Uses p.x,p.y  p.cx,p.cy
-		void (*writepos)(GDisplay *g);					// Uses p.x,p.y
-		void (*writecolor)(GDisplay *g);				// Uses p.color
-		void (*writestop)(GDisplay *g);					// Uses no parameters
-		void (*readstart)(GDisplay *g);					// Uses p.x,p.y  p.cx,p.cy
-		color_t (*readcolor)(GDisplay *g);				// Uses no parameters
-		void (*readstop)(GDisplay *g);					// Uses no parameters
-		void (*pixel)(GDisplay *g);						// Uses p.x,p.y  p.color
-		void (*clear)(GDisplay *g);						// Uses p.color
-		void (*fill)(GDisplay *g);						// Uses p.x,p.y  p.cx,p.cy  p.color
-		void (*blit)(GDisplay *g);						// Uses p.x,p.y  p.cx,p.cy  p.x1,p.y1 (=srcx,srcy)  p.x2 (=srccx), p.ptr (=buffer)
-		color_t (*get)(GDisplay *g);					// Uses p.x,p.y
-		void (*vscroll)(GDisplay *g);					// Uses p.x,p.y  p.cx,p.cy, p.y1 (=lines) p.color
-		void (*control)(GDisplay *g);					// Uses p.x (=what)  p.ptr (=value)
-		void *(*query)(GDisplay *g);					// Uses p.x (=what);
-		void (*setclip)(GDisplay *g);					// Uses p.x,p.y  p.cx,p.cy
-		void (*flush)(GDisplay *g);						// Uses no parameters
-	} GDISPVMT;
-
-	#if defined(GDISP_DRIVER_VMT)
-		#if !GDISP_HARDWARE_STREAM_WRITE && !GDISP_HARDWARE_DRAWPIXEL
-			#error "GDISP Driver: Either GDISP_HARDWARE_STREAM_WRITE or GDISP_HARDWARE_DRAWPIXEL must be TRUE"
-		#endif
-		const GDISPVMT const GDISP_DRIVER_VMT[1] = {{
-			gdisp_lld_init,
-			#if GDISP_HARDWARE_FLUSH
-				gdisp_lld_flush,
-			#else
-				0,
-			#endif
-			#if GDISP_HARDWARE_STREAM_WRITE
-				gdisp_lld_write_start,
-				#if GDISP_HARDWARE_STREAM_POS
-					gdisp_lld_write_pos,
-				#else
-					0,
-				#endif
-				gdisp_lld_write_color,
-				gdisp_lld_write_stop,
-			#else
-				0, 0, 0, 0,
-			#endif
-			#if GDISP_HARDWARE_STREAM_READ
-				gdisp_lld_read_start,
-				gdisp_lld_read_color,
-				gdisp_lld_read_stop,
-			#else
-				0, 0, 0,
-			#endif
-			#if GDISP_HARDWARE_DRAWPIXEL
-				gdisp_lld_draw_pixel,
-			#else
-				0,
-			#endif
-			#if GDISP_HARDWARE_CLEARS
-				gdisp_lld_clear,
-			#else
-				0,
-			#endif
-			#if GDISP_HARDWARE_FILLS
-				gdisp_lld_fill_area,
-			#else
-				0,
-			#endif
-			#if GDISP_HARDWARE_BITFILLS
-				gdisp_lld_blit_area,
-			#else
-				0,
-			#endif
-			#if GDISP_HARDWARE_PIXELREAD
-				gdisp_lld_get_pixel_color,
-			#else
-				0,
-			#endif
-			#if GDISP_HARDWARE_SCROLL && GDISP_NEED_SCROLL
-				gdisp_lld_vertical_scroll,
-			#else
-				0,
-			#endif
-			#if GDISP_HARDWARE_CONTROL && GDISP_NEED_CONTROL
-				gdisp_lld_control,
-			#else
-				0,
-			#endif
-			#if GDISP_HARDWARE_QUERY && GDISP_NEED_QUERY
-				gdisp_lld_query,
-			#else
-				0,
-			#endif
-			#if GDISP_HARDWARE_CLIP && (GDISP_NEED_CLIP || GDISP_NEED_VALIDATION)
-				gdisp_lld_set_clip,
-			#else
-				0,
-			#endif
-		}};
 
-	#else
-		#define gdisp_lld_init(g)				g->vmt->init(g)
-		#define gdisp_lld_flush(g)				g->vmt->flush(g)
-		#define gdisp_lld_write_start(g)		g->vmt->writestart(g)
-		#define gdisp_lld_write_pos(g)			g->vmt->writepos(g)
-		#define gdisp_lld_write_color(g)		g->vmt->writecolor(g)
-		#define gdisp_lld_write_stop(g)			g->vmt->writestop(g)
-		#define gdisp_lld_read_start(g)			g->vmt->readstart(g)
-		#define gdisp_lld_read_color(g)			g->vmt->readcolor(g)
-		#define gdisp_lld_read_stop(g)			g->vmt->readstop(g)
-		#define gdisp_lld_draw_pixel(g)			g->vmt->pixel(g)
-		#define gdisp_lld_clear(g)				g->vmt->clear(g)
-		#define gdisp_lld_fill_area(g)			g->vmt->fill(g)
-		#define gdisp_lld_blit_area(g)			g->vmt->blit(g)
-		#define gdisp_lld_get_pixel_color(g)	g->vmt->get(g)
-		#define gdisp_lld_vertical_scroll(g)	g->vmt->vscroll(g)
-		#define gdisp_lld_control(g)			g->vmt->control(g)
-		#define gdisp_lld_query(g)				g->vmt->query(g)
-		#define gdisp_lld_set_clip(g)			g->vmt->setclip(g)
-	#endif	// GDISP_LLD_DECLARATIONS
-
-#endif		// GDISP_TOTAL_CONTROLLERS > 1
-
-/* Verify information for packed pixels and define a non-packed pixel macro */
-#if !GDISP_PACKED_PIXELS
-	#define gdispPackPixels(buf,cx,x,y,c)	{ ((color_t *)(buf))[(y)*(cx)+(x)] = (c); }
-#elif !GDISP_HARDWARE_BITFILLS
-	#error "GDISP: packed pixel formats are only supported for hardware accelerated drivers."
-#elif GDISP_PIXELFORMAT != GDISP_PIXELFORMAT_RGB888 \
-		&& GDISP_PIXELFORMAT != GDISP_PIXELFORMAT_RGB444 \
-		&& GDISP_PIXELFORMAT != GDISP_PIXELFORMAT_RGB666 \
-		&& GDISP_PIXELFORMAT != GDISP_PIXELFORMAT_CUSTOM
-	#error "GDISP: A packed pixel format has been specified for an unsupported pixel format."
+#else
+	#define gdisp_lld_init(g)				gvmt(g)->init(g)
+	#define gdisp_lld_flush(g)				gvmt(g)->flush(g)
+	#define gdisp_lld_write_start(g)		gvmt(g)->writestart(g)
+	#define gdisp_lld_write_pos(g)			gvmt(g)->writepos(g)
+	#define gdisp_lld_write_color(g)		gvmt(g)->writecolor(g)
+	#define gdisp_lld_write_stop(g)			gvmt(g)->writestop(g)
+	#define gdisp_lld_read_start(g)			gvmt(g)->readstart(g)
+	#define gdisp_lld_read_color(g)			gvmt(g)->readcolor(g)
+	#define gdisp_lld_read_stop(g)			gvmt(g)->readstop(g)
+	#define gdisp_lld_draw_pixel(g)			gvmt(g)->pixel(g)
+	#define gdisp_lld_clear(g)				gvmt(g)->clear(g)
+	#define gdisp_lld_fill_area(g)			gvmt(g)->fill(g)
+	#define gdisp_lld_blit_area(g)			gvmt(g)->blit(g)
+	#define gdisp_lld_get_pixel_color(g)	gvmt(g)->get(g)
+	#define gdisp_lld_vertical_scroll(g)	gvmt(g)->vscroll(g)
+	#define gdisp_lld_control(g)			gvmt(g)->control(g)
+	#define gdisp_lld_query(g)				gvmt(g)->query(g)
+	#define gdisp_lld_set_clip(g)			gvmt(g)->setclip(g)
 #endif
 
-/* Support routine for packed pixel formats */
-#if !defined(gdispPackPixels) || defined(__DOXYGEN__)
-	/**
-	 * @brief   Pack a pixel into a pixel buffer.
-	 * @note    This function performs no buffer boundary checking
-	 *			regardless of whether GDISP_NEED_CLIP has been specified.
-	 *
-	 * @param[in] buf		The buffer to put the pixel in
-	 * @param[in] cx		The width of a pixel line
-	 * @param[in] x, y		The location of the pixel to place
-	 * @param[in] color		The color to put into the buffer
-	 *
-	 * @api
-	 */
-	void gdispPackPixels(const pixel_t *buf, coord_t cx, coord_t x, coord_t y, color_t color);
-#endif
+// If compiling the driver then build the VMT and set the low level driver color macros.
+#ifdef GDISP_DRIVER_VMT
 
-/* Low level driver pixel format information */
-//-------------------------
-//	True-Color color system
-//-------------------------
-#if GDISP_LLD_PIXELFORMAT & GDISP_COLORSYSTEM_TRUECOLOR
-	#define LLDCOLOR_SYSTEM			GDISP_COLORSYSTEM_TRUECOLOR
-
-	// Calculate the number of bits
-	#define LLDCOLOR_BITS_R			((GDISP_LLD_PIXELFORMAT>>8) & 0x0F)
-	#define LLDCOLOR_BITS_G			((GDISP_LLD_PIXELFORMAT>>4) & 0x0F)
-	#define LLDCOLOR_BITS_B			((GDISP_LLD_PIXELFORMAT>>0) & 0x0F)
-	#define LLDCOLOR_BITS			(LLDCOLOR_BITS_R + LLDCOLOR_BITS_G + LLDCOLOR_BITS_B)
-
-	// From the number of bits determine COLOR_TYPE, COLOR_TYPE_BITS and masking
-	#if LLDCOLOR_BITS <= 8
-		#define LLDCOLOR_TYPE			uint8_t
-		#define LLDCOLOR_TYPE_BITS		8
-	#elif LLDCOLOR_BITS <= 16
-		#define LLDCOLOR_TYPE			uint16_t
-		#define LLDCOLOR_TYPE_BITS		16
-	#elif LLDCOLOR_BITS <= 32
-		#define LLDCOLOR_TYPE			uint32_t
-		#define LLDCOLOR_TYPE_BITS		32
-	#else
-		#error "GDISP: Cannot define low level driver color types with more than 32 bits"
+	// Make sure the driver has a valid model
+	#if !GDISP_HARDWARE_STREAM_WRITE && !GDISP_HARDWARE_DRAWPIXEL
+		#error "GDISP Driver: Either GDISP_HARDWARE_STREAM_WRITE or GDISP_HARDWARE_DRAWPIXEL must be TRUE"
 	#endif
-	#if LLDCOLOR_TYPE_BITS == LLDCOLOR_BITS
-		#define LLDCOLOR_NEEDS_MASK	FALSE
-	#else
-		#define LLDCOLOR_NEEDS_MASK	TRUE
+
+	// If we are not using multiple displays then hard-code the VMT name
+	#ifndef GDISP_CONTROLLER_LIST
+		#undef GDISP_DRIVER_VMT
+		#define GDISP_DRIVER_VMT		GDISPVMT_OnlyOne
 	#endif
-	#define LLDCOLOR_MASK()			((1 << LLDCOLOR_BITS)-1)
 
-	// Calculate the component bit shifts
-	#if (GDISP_LLD_PIXELFORMAT & GDISP_COLORSYSTEM_MASK) == GDISP_COLORSYSTEM_RGB
-		#define LLDCOLOR_SHIFT_R		(LLDCOLOR_BITS_B+LLDCOLOR_BITS_G)
-		#define LLDCOLOR_SHIFT_G		LLDCOLOR_BITS_B
-		#define LLDCOLOR_SHIFT_B		0
-	#else
-		#define LLDCOLOR_SHIFT_B		(LLDCOLOR_BITS_R+LLDCOLOR_BITS_G)
-		#define LLDCOLOR_SHIFT_G		LLDCOLOR_BITS_R
+	// Routines needed by the general driver VMT
+	bool_t _gdispInitDriver(GDriver *g, int driverinstance, int systeminstance);
+	void _gdispDeinitDriver(GDriver *g);
+
+	// Build the VMT
+	const GDISPVMT const GDISP_DRIVER_VMT[1] = {{
+		{ GDRIVER_TYPE_DISPLAY, 0, sizeof(GDisplay), _gdispInitDriver, _gdispDeinitDriver },
+		gdisp_lld_init,
+		#if GDISP_HARDWARE_STREAM_WRITE
+			gdisp_lld_write_start,
+			#if GDISP_HARDWARE_STREAM_POS
+				gdisp_lld_write_pos,
+			#else
+				0,
+			#endif
+			gdisp_lld_write_color,
+			gdisp_lld_write_stop,
+		#else
+			0, 0, 0, 0,
+		#endif
+		#if GDISP_HARDWARE_STREAM_READ
+			gdisp_lld_read_start,
+			gdisp_lld_read_color,
+			gdisp_lld_read_stop,
+		#else
+			0, 0, 0,
+		#endif
+		#if GDISP_HARDWARE_DRAWPIXEL
+			gdisp_lld_draw_pixel,
+		#else
+			0,
+		#endif
+		#if GDISP_HARDWARE_CLEARS
+			gdisp_lld_clear,
+		#else
+			0,
+		#endif
+		#if GDISP_HARDWARE_FILLS
+			gdisp_lld_fill_area,
+		#else
+			0,
+		#endif
+		#if GDISP_HARDWARE_BITFILLS
+			gdisp_lld_blit_area,
+		#else
+			0,
+		#endif
+		#if GDISP_HARDWARE_PIXELREAD
+			gdisp_lld_get_pixel_color,
+		#else
+			0,
+		#endif
+		#if GDISP_HARDWARE_SCROLL && GDISP_NEED_SCROLL
+			gdisp_lld_vertical_scroll,
+		#else
+			0,
+		#endif
+		#if GDISP_HARDWARE_CONTROL && GDISP_NEED_CONTROL
+			gdisp_lld_control,
+		#else
+			0,
+		#endif
+		#if GDISP_HARDWARE_QUERY && GDISP_NEED_QUERY
+			gdisp_lld_query,
+		#else
+			0,
+		#endif
+		#if GDISP_HARDWARE_CLIP && (GDISP_NEED_CLIP || GDISP_NEED_VALIDATION)
+			gdisp_lld_set_clip,
+		#else
+			0,
+		#endif
+		#if GDISP_HARDWARE_FLUSH
+			gdisp_lld_flush,
+		#else
+			0,
+		#endif
+	}};
+
+	/* Low level driver pixel format information */
+	//-------------------------
+	//	True-Color color system
+	//-------------------------
+	#if GDISP_LLD_PIXELFORMAT & GDISP_COLORSYSTEM_TRUECOLOR
+		#define LLDCOLOR_SYSTEM			GDISP_COLORSYSTEM_TRUECOLOR
+
+		// Calculate the number of bits
+		#define LLDCOLOR_BITS_R			((GDISP_LLD_PIXELFORMAT>>8) & 0x0F)
+		#define LLDCOLOR_BITS_G			((GDISP_LLD_PIXELFORMAT>>4) & 0x0F)
+		#define LLDCOLOR_BITS_B			((GDISP_LLD_PIXELFORMAT>>0) & 0x0F)
+		#define LLDCOLOR_BITS			(LLDCOLOR_BITS_R + LLDCOLOR_BITS_G + LLDCOLOR_BITS_B)
+
+		// From the number of bits determine COLOR_TYPE, COLOR_TYPE_BITS and masking
+		#if LLDCOLOR_BITS <= 8
+			#define LLDCOLOR_TYPE			uint8_t
+			#define LLDCOLOR_TYPE_BITS		8
+		#elif LLDCOLOR_BITS <= 16
+			#define LLDCOLOR_TYPE			uint16_t
+			#define LLDCOLOR_TYPE_BITS		16
+		#elif LLDCOLOR_BITS <= 32
+			#define LLDCOLOR_TYPE			uint32_t
+			#define LLDCOLOR_TYPE_BITS		32
+		#else
+			#error "GDISP: Cannot define low level driver color types with more than 32 bits"
+		#endif
+		#if LLDCOLOR_TYPE_BITS == LLDCOLOR_BITS
+			#define LLDCOLOR_NEEDS_MASK	FALSE
+		#else
+			#define LLDCOLOR_NEEDS_MASK	TRUE
+		#endif
+		#define LLDCOLOR_MASK()			((1 << LLDCOLOR_BITS)-1)
+
+		// Calculate the component bit shifts
+		#if (GDISP_LLD_PIXELFORMAT & GDISP_COLORSYSTEM_MASK) == GDISP_COLORSYSTEM_RGB
+			#define LLDCOLOR_SHIFT_R		(LLDCOLOR_BITS_B+LLDCOLOR_BITS_G)
+			#define LLDCOLOR_SHIFT_G		LLDCOLOR_BITS_B
+			#define LLDCOLOR_SHIFT_B		0
+		#else
+			#define LLDCOLOR_SHIFT_B		(LLDCOLOR_BITS_R+LLDCOLOR_BITS_G)
+			#define LLDCOLOR_SHIFT_G		LLDCOLOR_BITS_R
+			#define LLDCOLOR_SHIFT_R		0
+		#endif
+
+		// Calculate LLDRED_OF, LLDGREEN_OF, LLDBLUE_OF and LLDRGB2COLOR
+		#if LLDCOLOR_BITS_R + LLDCOLOR_SHIFT_R == 8
+			#define LLDRED_OF(c)			((c) & (((1<<LLDCOLOR_BITS_R)-1) << LLDCOLOR_SHIFT_R))
+			#define LLDRGB2COLOR_R(r)		((LLDCOLOR_TYPE)((r) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1))))
+		#elif LLDCOLOR_BITS_R + LLDCOLOR_SHIFT_R > 8
+			#define LLDRED_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_R)-1) << LLDCOLOR_SHIFT_R)) >> (LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R-8))
+			#define LLDRGB2COLOR_R(r)		(((LLDCOLOR_TYPE)((r) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1)))) << (LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R-8))
+		#else // LLDCOLOR_BITS_R + LLDCOLOR_SHIFT_R < 8
+			#define LLDRED_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_R)-1) << LLDCOLOR_SHIFT_R)) << (8-(LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R)))
+			#define LLDRGB2COLOR_R(r)		(((LLDCOLOR_TYPE)((r) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1)))) >> (8-(LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R)))
+		#endif
+		#if LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G == 8
+			#define LLDGREEN_OF(c)			((c) & (((1<<LLDCOLOR_BITS_G)-1) << LLDCOLOR_SHIFT_G))
+			#define LLDRGB2COLOR_G(g)		((LLDCOLOR_TYPE)((g) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1))))
+		#elif LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G > 8
+			#define LLDGREEN_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_G)-1) << LLDCOLOR_SHIFT_G)) >> (LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G-8))
+			#define LLDRGB2COLOR_G(g)		(((LLDCOLOR_TYPE)((g) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1)))) << (LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G-8))
+		#else // LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G < 8
+			#define LLDGREEN_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_G)-1) << LLDCOLOR_SHIFT_G)) << (8-(LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G)))
+			#define LLDRGB2COLOR_G(g)		(((LLDCOLOR_TYPE)((g) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1)))) >> (8-(LLDCOLOR_BITS_LLDG+COLOR_SHIFT_G)))
+		#endif
+		#if LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B == 8
+			#define LLDBLUE_OF(c)			((c) & (((1<<LLDCOLOR_BITS_B)-1) << LLDCOLOR_SHIFT_B))
+			#define LLDRGB2COLOR_B(b)		((LLDCOLOR_TYPE)((b) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1))))
+		#elif LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B > 8
+			#define LLDBLUE_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_B)-1) << LLDCOLOR_SHIFT_B)) >> (LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B-8))
+			#define LLDRGB2COLOR_B(b)		(((LLDCOLOR_TYPE)((b) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1)))) << (LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B-8))
+		#else // LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B < 8
+			#define LLDBLUE_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_B)-1) << LLDCOLOR_SHIFT_B)) << (8-(LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B)))
+			#define LLDRGB2COLOR_B(b)		(((COLOR_TYPE)((b) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1)))) >> (8-(LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B)))
+		#endif
+		#define LLDLUMA_OF(c)				((LLDRED_OF(c)+((uint16_t)LLDGREEN_OF(c)<<1)+LLDBLUE_OF(c))>>2)
+		#define LLDEXACT_RED_OF(c)			(((uint16_t)(((c)>>LLDCOLOR_SHIFT_R)&((1<<LLDCOLOR_BITS_R)-1))*255)/((1<<LLDCOLOR_BITS_R)-1))
+		#define LLDEXACT_GREEN_OF(c)		(((uint16_t)(((c)>>LLDCOLOR_SHIFT_G)&((1<<LLDCOLOR_BITS_G)-1))*255)/((1<<LLDCOLOR_BITS_G)-1))
+		#define LLDEXACT_BLUE_OF(c)			(((uint16_t)(((c)>>LLDCOLOR_SHIFT_B)&((1<<LLDCOLOR_BITS_B)-1))*255)/((1<<LLDCOLOR_BITS_B)-1))
+		#define LLDEXACT_LUMA_OF(c)			((LLDEXACT_RED_OF(c)+((uint16_t)LLDEXACT_GREEN_OF(c)<<1)+LLDEXACT_BLUE_OF(c))>>2)
+		#define LLDLUMA2COLOR(l)			(LLDRGB2COLOR_R(l) | LLDRGB2COLOR_G(l) | LLDRGB2COLOR_B(l))
+		#define LLDRGB2COLOR(r,g,b)			(LLDRGB2COLOR_R(r) | LLDRGB2COLOR_G(g) | LLDRGB2COLOR_B(b))
+
+		// Calculate LLDHTML2COLOR
+		#if LLDCOLOR_BITS_R + LLDCOLOR_SHIFT_R == 24
+			#define LLDHTML2COLOR_R(h)		((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1))<<16))
+		#elif COLOR_BITS_R + COLOR_SHIFT_R > 24
+			#define LLDHTML2COLOR_R(h)		(((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1))<<16)) << (LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R-24))
+		#else // COLOR_BITS_R + COLOR_SHIFT_R < 24
+			#define LLDHTML2COLOR_R(h)		(((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1))<<16)) >> (24-(LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R)))
+		#endif
+		#if LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G == 16
+			#define LLDHTML2COLOR_G(h)		((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1))<<8))
+		#elif LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G > 16
+			#define LLDHTML2COLOR_G(h)		(((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1))<<8)) << (LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G-16))
+		#else // LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G < 16
+			#define LLDHTML2COLOR_G(h)		(((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1))<<8)) >> (16-(LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G)))
+		#endif
+		#if LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B == 8
+			#define LLDHTML2COLOR_B(h)		((h) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1)))
+		#elif LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B > 8
+			#define LLDHTML2COLOR_B(h)		(((h) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1))) << (LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B-8))
+		#else // LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B < 8
+			#define LLDHTML2COLOR_B(h)		(((h) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1))) >> (8-(LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B)))
+		#endif
+		#define LLDHTML2COLOR(h)		((LLDCOLOR_TYPE)(LLDHTML2COLOR_R(h) | LLDHTML2COLOR_G(h) | LLDHTML2COLOR_B(h)))
+
+	//-------------------------
+	//	Gray-scale color system
+	//-------------------------
+	#elif (GDISP_LLD_PIXELFORMAT & GDISP_COLORSYSTEM_MASK) == GDISP_COLORSYSTEM_GRAYSCALE
+		#define LLDCOLOR_SYSTEM			GDISP_COLORSYSTEM_GRAYSCALE
+
+		// Calculate the number of bits and shifts
+		#define LLDCOLOR_BITS			(GDISP_LLD_PIXELFORMAT & 0xFF)
+		#define LLDCOLOR_BITS_R			LLDCOLOR_BITS
+		#define LLDCOLOR_BITS_G			LLDCOLOR_BITS
+		#define LLDCOLOR_BITS_B			LLDCOLOR_BITS
 		#define LLDCOLOR_SHIFT_R		0
-	#endif
+		#define LLDCOLOR_SHIFT_G		0
+		#define LLDCOLOR_SHIFT_B		0
 
-	// Calculate LLDRED_OF, LLDGREEN_OF, LLDBLUE_OF and LLDRGB2COLOR
-	#if LLDCOLOR_BITS_R + LLDCOLOR_SHIFT_R == 8
-		#define LLDRED_OF(c)			((c) & (((1<<LLDCOLOR_BITS_R)-1) << LLDCOLOR_SHIFT_R))
-		#define LLDRGB2COLOR_R(r)		((LLDCOLOR_TYPE)((r) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1))))
-	#elif LLDCOLOR_BITS_R + LLDCOLOR_SHIFT_R > 8
-		#define LLDRED_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_R)-1) << LLDCOLOR_SHIFT_R)) >> (LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R-8))
-		#define LLDRGB2COLOR_R(r)		(((LLDCOLOR_TYPE)((r) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1)))) << (LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R-8))
-	#else // LLDCOLOR_BITS_R + LLDCOLOR_SHIFT_R < 8
-		#define LLDRED_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_R)-1) << LLDCOLOR_SHIFT_R)) << (8-(LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R)))
-		#define LLDRGB2COLOR_R(r)		(((LLDCOLOR_TYPE)((r) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1)))) >> (8-(LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R)))
-	#endif
-	#if LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G == 8
-		#define LLDGREEN_OF(c)			((c) & (((1<<LLDCOLOR_BITS_G)-1) << LLDCOLOR_SHIFT_G))
-		#define LLDRGB2COLOR_G(g)		((LLDCOLOR_TYPE)((g) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1))))
-	#elif LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G > 8
-		#define LLDGREEN_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_G)-1) << LLDCOLOR_SHIFT_G)) >> (LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G-8))
-		#define LLDRGB2COLOR_G(g)		(((LLDCOLOR_TYPE)((g) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1)))) << (LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G-8))
-	#else // LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G < 8
-		#define LLDGREEN_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_G)-1) << LLDCOLOR_SHIFT_G)) << (8-(LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G)))
-		#define LLDRGB2COLOR_G(g)		(((LLDCOLOR_TYPE)((g) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1)))) >> (8-(LLDCOLOR_BITS_LLDG+COLOR_SHIFT_G)))
-	#endif
-	#if LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B == 8
-		#define LLDBLUE_OF(c)			((c) & (((1<<LLDCOLOR_BITS_B)-1) << LLDCOLOR_SHIFT_B))
-		#define LLDRGB2COLOR_B(b)		((LLDCOLOR_TYPE)((b) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1))))
-	#elif LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B > 8
-		#define LLDBLUE_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_B)-1) << LLDCOLOR_SHIFT_B)) >> (LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B-8))
-		#define LLDRGB2COLOR_B(b)		(((LLDCOLOR_TYPE)((b) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1)))) << (LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B-8))
-	#else // LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B < 8
-		#define LLDBLUE_OF(c)			(((c) & (((1<<LLDCOLOR_BITS_B)-1) << LLDCOLOR_SHIFT_B)) << (8-(LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B)))
-		#define LLDRGB2COLOR_B(b)		(((COLOR_TYPE)((b) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1)))) >> (8-(LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B)))
-	#endif
-	#define LLDLUMA_OF(c)				((LLDRED_OF(c)+((uint16_t)LLDGREEN_OF(c)<<1)+LLDBLUE_OF(c))>>2)
-	#define LLDEXACT_RED_OF(c)			(((uint16_t)(((c)>>LLDCOLOR_SHIFT_R)&((1<<LLDCOLOR_BITS_R)-1))*255)/((1<<LLDCOLOR_BITS_R)-1))
-	#define LLDEXACT_GREEN_OF(c)		(((uint16_t)(((c)>>LLDCOLOR_SHIFT_G)&((1<<LLDCOLOR_BITS_G)-1))*255)/((1<<LLDCOLOR_BITS_G)-1))
-	#define LLDEXACT_BLUE_OF(c)			(((uint16_t)(((c)>>LLDCOLOR_SHIFT_B)&((1<<LLDCOLOR_BITS_B)-1))*255)/((1<<LLDCOLOR_BITS_B)-1))
-	#define LLDEXACT_LUMA_OF(c)			((LLDEXACT_RED_OF(c)+((uint16_t)LLDEXACT_GREEN_OF(c)<<1)+LLDEXACT_BLUE_OF(c))>>2)
-	#define LLDLUMA2COLOR(l)			(LLDRGB2COLOR_R(l) | LLDRGB2COLOR_G(l) | LLDRGB2COLOR_B(l))
-	#define LLDRGB2COLOR(r,g,b)			(LLDRGB2COLOR_R(r) | LLDRGB2COLOR_G(g) | LLDRGB2COLOR_B(b))
-
-	// Calculate LLDHTML2COLOR
-	#if LLDCOLOR_BITS_R + LLDCOLOR_SHIFT_R == 24
-		#define LLDHTML2COLOR_R(h)		((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1))<<16))
-	#elif COLOR_BITS_R + COLOR_SHIFT_R > 24
-		#define LLDHTML2COLOR_R(h)		(((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1))<<16)) << (LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R-24))
-	#else // COLOR_BITS_R + COLOR_SHIFT_R < 24
-		#define LLDHTML2COLOR_R(h)		(((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_R))-1))<<16)) >> (24-(LLDCOLOR_BITS_R+LLDCOLOR_SHIFT_R)))
-	#endif
-	#if LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G == 16
-		#define LLDHTML2COLOR_G(h)		((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1))<<8))
-	#elif LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G > 16
-		#define LLDHTML2COLOR_G(h)		(((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1))<<8)) << (LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G-16))
-	#else // LLDCOLOR_BITS_G + LLDCOLOR_SHIFT_G < 16
-		#define LLDHTML2COLOR_G(h)		(((h) & ((0xFF & ~((1<<(8-LLDCOLOR_BITS_G))-1))<<8)) >> (16-(LLDCOLOR_BITS_G+LLDCOLOR_SHIFT_G)))
-	#endif
-	#if LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B == 8
-		#define LLDHTML2COLOR_B(h)		((h) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1)))
-	#elif LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B > 8
-		#define LLDHTML2COLOR_B(h)		(((h) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1))) << (LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B-8))
-	#else // LLDCOLOR_BITS_B + LLDCOLOR_SHIFT_B < 8
-		#define LLDHTML2COLOR_B(h)		(((h) & (0xFF & ~((1<<(8-LLDCOLOR_BITS_B))-1))) >> (8-(LLDCOLOR_BITS_B+LLDCOLOR_SHIFT_B)))
-	#endif
-	#define LLDHTML2COLOR(h)		((LLDCOLOR_TYPE)(LLDHTML2COLOR_R(h) | LLDHTML2COLOR_G(h) | LLDHTML2COLOR_B(h)))
-
-//-------------------------
-//	Gray-scale color system
-//-------------------------
-#elif (GDISP_LLD_PIXELFORMAT & GDISP_COLORSYSTEM_MASK) == GDISP_COLORSYSTEM_GRAYSCALE
-	#define LLDCOLOR_SYSTEM			GDISP_COLORSYSTEM_GRAYSCALE
-
-	// Calculate the number of bits and shifts
-	#define LLDCOLOR_BITS			(GDISP_LLD_PIXELFORMAT & 0xFF)
-	#define LLDCOLOR_BITS_R			LLDCOLOR_BITS
-	#define LLDCOLOR_BITS_G			LLDCOLOR_BITS
-	#define LLDCOLOR_BITS_B			LLDCOLOR_BITS
-	#define LLDCOLOR_SHIFT_R		0
-	#define LLDCOLOR_SHIFT_G		0
-	#define LLDCOLOR_SHIFT_B		0
-
-	// From the number of bits determine COLOR_TYPE, COLOR_TYPE_BITS and masking
-	#if LLDCOLOR_BITS <= 8
-		#define LLDCOLOR_TYPE		uint8_t
-		#define LLDCOLOR_TYPE_BITS	8
-	#else
-		#error "GDISP: Cannot define gray-scale low level driver color types with more than 8 bits"
-	#endif
-	#if LLDCOLOR_TYPE_BITS == LLDCOLOR_BITS
-		#define LLDCOLOR_NEEDS_MASK	FALSE
+		// From the number of bits determine COLOR_TYPE, COLOR_TYPE_BITS and masking
+		#if LLDCOLOR_BITS <= 8
+			#define LLDCOLOR_TYPE		uint8_t
+			#define LLDCOLOR_TYPE_BITS	8
+		#else
+			#error "GDISP: Cannot define gray-scale low level driver color types with more than 8 bits"
+		#endif
+		#if LLDCOLOR_TYPE_BITS == LLDCOLOR_BITS
+			#define LLDCOLOR_NEEDS_MASK	FALSE
+		#else
+			#define LLDCOLOR_NEEDS_MASK	TRUE
+		#endif
+		#define LLDCOLOR_MASK()			((1 << LLDCOLOR_BITS)-1)
+
+		#if COLOR_BITS == 1
+			#define LLDRGB2COLOR(r,g,b)		(((r)|(g)|(b)) ? 1 : 0)
+			#define LLDLUMA2COLOR(l)		((l) ? 1 : 0)
+			#define LLDHTML2COLOR(h)		((h) ? 1 : 0)
+			#define LLDLUMA_OF(c)			((c) ? 255 : 0)
+			#define LLDEXACT_LUMA_OF(c)		LLDLUMA_OF(c)
+		#else
+			// They eye is more sensitive to green
+			#define LLDRGB2COLOR(r,g,b)		((LLDCOLOR_TYPE)(((uint16_t)(r)+(g)+(g)+(b)) >> (10-LLDCOLOR_BITS)))
+			#define LLDLUMA2COLOR(l)		((LLDCOLOR_TYPE)((l)>>(8-LLDCOLOR_BITS)))
+			#define LLDHTML2COLOR(h)		((LLDCOLOR_TYPE)(((((h)&0xFF0000)>>16)+(((h)&0x00FF00)>>7)+((h)&0x0000FF)) >> (10-LLDCOLOR_BITS)))
+			#define LLDLUMA_OF(c)			(((c) & ((1<<LLDCOLOR_BITS)-1)) << (8-LLDCOLOR_BITS))
+			#define LLDEXACT_LUMA_OF(c)		((((uint16_t)(c) & ((1<<LLDCOLOR_BITS)-1))*255)/((1<<LLDCOLOR_BITS)-1))
+		#endif
+
+		#define LLDRED_OF(c)			LLDLUMA_OF(c)
+		#define LLDGREEN_OF(c)			LLDLUMA_OF(c)
+		#define LLDBLUE_OF(c)			LLDLUMA_OF(c)
+		#define LLDEXACT_RED_OF(c)		LLDEXACT_LUMA_OF(c)
+		#define LLDEXACT_GREEN_OF(c)	LLDEXACT_LUMA_OF(c)
+		#define LLDEXACT_BLUE_OF(c)		LLDEXACT_LUMA_OF(c)
+
+	//-------------------------
+	//	Palette color system
+	//-------------------------
+	#elif (GDISP_LLD_PIXELFORMAT & GDISP_COLORSYSTEM_MASK) == GDISP_COLORSYSTEM_PALETTE
+		#define LLDCOLOR_SYSTEM			GDISP_COLORSYSTEM_PALETTE
+
+		#error "GDISP: A palette color system for low level drivers is not currently supported"
+
+	//-------------------------
+	//	Some other color system
+	//-------------------------
 	#else
-		#define LLDCOLOR_NEEDS_MASK	TRUE
+		#error "GDISP: Unsupported color system for low level drivers"
 	#endif
-	#define LLDCOLOR_MASK()			((1 << LLDCOLOR_BITS)-1)
 
-	#if COLOR_BITS == 1
-		#define LLDRGB2COLOR(r,g,b)		(((r)|(g)|(b)) ? 1 : 0)
-		#define LLDLUMA2COLOR(l)		((l) ? 1 : 0)
-		#define LLDHTML2COLOR(h)		((h) ? 1 : 0)
-		#define LLDLUMA_OF(c)			((c) ? 255 : 0)
-		#define LLDEXACT_LUMA_OF(c)		LLDLUMA_OF(c)
+	/* Which is the larger color type */
+	#if COLOR_BITS > LLDCOLOR_BITS
+		#define LARGER_COLOR_BITS	COLOR_BITS
+		#define LARGER_COLOR_TYPE	COLOR_TYPE
 	#else
-		// They eye is more sensitive to green
-		#define LLDRGB2COLOR(r,g,b)		((LLDCOLOR_TYPE)(((uint16_t)(r)+(g)+(g)+(b)) >> (10-LLDCOLOR_BITS)))
-		#define LLDLUMA2COLOR(l)		((LLDCOLOR_TYPE)((l)>>(8-LLDCOLOR_BITS)))
-		#define LLDHTML2COLOR(h)		((LLDCOLOR_TYPE)(((((h)&0xFF0000)>>16)+(((h)&0x00FF00)>>7)+((h)&0x0000FF)) >> (10-LLDCOLOR_BITS)))
-		#define LLDLUMA_OF(c)			(((c) & ((1<<LLDCOLOR_BITS)-1)) << (8-LLDCOLOR_BITS))
-		#define LLDEXACT_LUMA_OF(c)		((((uint16_t)(c) & ((1<<LLDCOLOR_BITS)-1))*255)/((1<<LLDCOLOR_BITS)-1))
-	#endif
-
-	#define LLDRED_OF(c)			LLDLUMA_OF(c)
-	#define LLDGREEN_OF(c)			LLDLUMA_OF(c)
-	#define LLDBLUE_OF(c)			LLDLUMA_OF(c)
-	#define LLDEXACT_RED_OF(c)		LLDEXACT_LUMA_OF(c)
-	#define LLDEXACT_GREEN_OF(c)	LLDEXACT_LUMA_OF(c)
-	#define LLDEXACT_BLUE_OF(c)		LLDEXACT_LUMA_OF(c)
-
-//-------------------------
-//	Palette color system
-//-------------------------
-#elif (GDISP_LLD_PIXELFORMAT & GDISP_COLORSYSTEM_MASK) == GDISP_COLORSYSTEM_PALETTE
-	#define LLDCOLOR_SYSTEM			GDISP_COLORSYSTEM_PALETTE
-
-	#error "GDISP: A palette color system for low level drivers is not currently supported"
-
-//-------------------------
-//	Some other color system
-//-------------------------
-#else
-	#error "GDISP: Unsupported color system for low level drivers"
-#endif
+		#define LARGER_COLOR_BITS	LLDCOLOR_BITS
+		#define LARGER_COLOR_TYPE	LLDCOLOR_TYPE
+	#endif
 
-/* Which is the larger color type */
-#if COLOR_BITS > LLDCOLOR_BITS
-	#define LARGER_COLOR_BITS	COLOR_BITS
-	#define LARGER_COLOR_TYPE	COLOR_TYPE
-#else
-	#define LARGER_COLOR_BITS	LLDCOLOR_BITS
-	#define LARGER_COLOR_TYPE	LLDCOLOR_TYPE
-#endif
+	/**
+	 * @brief	Controls color conversion accuracy for a low level driver
+	 * @details	Should higher precision be used when converting colors.
+	 * @note	Color conversion is only necessary if GDISP_PIXELFORMAT != GDISP_LLD_PIXELFORMAT
+	 * @note	It only makes sense to turn this on if you have a high bit depth display but
+	 * 			are running the application in low bit depths.
+	 * @note	To achieve higher color accuracy bit shifting is replaced with multiplies and divides.
+	 */
+	#ifndef GDISP_HARDWARE_USE_EXACT_COLOR
+		#if LLDCOLOR_BITS_R - COLOR_BITS_R >= LLDCOLOR_BITS_R/2 || LLDCOLOR_BITS_G - COLOR_BITS_G >= LLDCOLOR_BITS_G/2 || LLDCOLOR_BITS_B - COLOR_BITS_B >= LLDCOLOR_BITS_B/2
+			#define GDISP_HARDWARE_USE_EXACT_COLOR	TRUE
+		#else
+			#define GDISP_HARDWARE_USE_EXACT_COLOR	FALSE
+		#endif
+	#endif
 
-/**
- * @brief	Controls color conversion accuracy for a low level driver
- * @details	Should higher precision be used when converting colors.
- * @note	Color conversion is only necessary if GDISP_PIXELFORMAT != GDISP_LLD_PIXELFORMAT
- * @note	It only makes sense to turn this on if you have a high bit depth display but
- * 			are running the application in low bit depths.
- * @note	To achieve higher color accuracy bit shifting is replaced with multiplies and divides.
- */
-#ifndef GDISP_HARDWARE_USE_EXACT_COLOR
-	#if LLDCOLOR_BITS_R - COLOR_BITS_R >= LLDCOLOR_BITS_R/2 || LLDCOLOR_BITS_G - COLOR_BITS_G >= LLDCOLOR_BITS_G/2 || LLDCOLOR_BITS_B - COLOR_BITS_B >= LLDCOLOR_BITS_B/2
-		#define GDISP_HARDWARE_USE_EXACT_COLOR	TRUE
+	/* Low level driver pixel format conversion functions */
+	#if GDISP_PIXELFORMAT == GDISP_LLD_PIXELFORMAT || defined(__DOXYGEN__)
+		/**
+		 * @brief	Convert from a standard color format to the low level driver pixel format
+		 * @note	For use only by low level drivers
+		 */
+		#define gdispColor2Native(c)	(c)
+		/**
+		 * @brief	Convert from a low level driver pixel format to the standard color format
+		 * @note	For use only by low level drivers
+		 */
+		#define gdispNative2Color(c)	(c)
 	#else
-		#define GDISP_HARDWARE_USE_EXACT_COLOR	FALSE
+		static LLDCOLOR_TYPE gdispColor2Native(color_t c) {
+			#if COLOR_SYSTEM == GDISP_COLORSYSTEM_GRAYSCALE || LLDCOLOR_SYSTEM == GDISP_COLORSYSTEM_GRAYSCALE
+				#if GDISP_HARDWARE_USE_EXACT_COLOR
+					return LLDLUMA2COLOR(EXACT_LUMA_OF(c));
+				#else
+					return LLDLUMA2COLOR(LUMA_OF(c));
+				#endif
+			#elif COLOR_SYSTEM == GDISP_COLORSYSTEM_TRUECOLOR && LLDCOLOR_SYSTEM == GDISP_COLORSYSTEM_TRUECOLOR
+				#if GDISP_HARDWARE_USE_EXACT_COLOR
+					return LLDRGB2COLOR(EXACT_RED_OF(c), EXACT_GREEN_OF(c), EXACT_BLUE_OF(c));
+				#else
+					return LLDRGB2COLOR(RED_OF(c), GREEN_OF(c), BLUE_OF(c));
+				#endif
+			#else
+				#error "GDISP: This pixel format conversion is not supported yet"
+			#endif
+		}
+		static color_t gdispNative2Color(LLDCOLOR_TYPE c) {
+			#if COLOR_SYSTEM == GDISP_COLORSYSTEM_GRAYSCALE || LLDCOLOR_SYSTEM == GDISP_COLORSYSTEM_GRAYSCALE
+				#if GDISP_HARDWARE_USE_EXACT_COLOR
+					return LUMA2COLOR(LLDEXACT_LUMA_OF(c));
+				#else
+					return LUMA2COLOR(LLDLUMA_OF(c));
+				#endif
+			#elif COLOR_SYSTEM == GDISP_COLORSYSTEM_TRUECOLOR && LLDCOLOR_SYSTEM == GDISP_COLORSYSTEM_TRUECOLOR
+				#if GDISP_HARDWARE_USE_EXACT_COLOR
+					return RGB2COLOR(LLDEXACT_RED_OF(c), LLDEXACT_GREEN_OF(c), LLDEXACT_BLUE_OF(c));
+				#else
+					return RGB2COLOR(LLDRED_OF(c), LLDGREEN_OF(c), LLDBLUE_OF(c));
+				#endif
+			#else
+				#error "GDISP: This pixel format conversion is not supported yet"
+			#endif
+		}
 	#endif
-#endif
 
-/* Low level driver pixel format conversion functions */
-#if GDISP_PIXELFORMAT == GDISP_LLD_PIXELFORMAT || defined(__DOXYGEN__)
-	/**
-	 * @brief	Convert from a standard color format to the low level driver pixel format
-	 * @note	For use only by low level drivers
-	 */
-	#define gdispColor2Native(c)	(c)
-	/**
-	 * @brief	Convert from a low level driver pixel format to the standard color format
-	 * @note	For use only by low level drivers
-	 */
-	#define gdispNative2Color(c)	(c)
-#else
-	LLDCOLOR_TYPE gdispColor2Native(color_t c);
-	color_t gdispNative2Color(LLDCOLOR_TYPE c);
 #endif
 
 #endif	/* GFX_USE_GDISP */
diff --git a/src/gdisp/gdisp_gdisp.c b/src/gdisp/gdisp_gdisp.c
index 02ee4711..5b99da6c 100644
--- a/src/gdisp/gdisp_gdisp.c
+++ b/src/gdisp/gdisp_gdisp.c
@@ -45,20 +45,11 @@
 /* Driver local variables.                                                   */
 /*===========================================================================*/
 
-// The controller array, the display array and the default display
-#if GDISP_TOTAL_CONTROLLERS > 1
-	typedef const struct GDISPVMT const	VMTEL[1];
-	extern VMTEL			GDISP_CONTROLLER_LIST;
-	static const struct GDISPVMT const *	ControllerList[GDISP_TOTAL_CONTROLLERS] = {GDISP_CONTROLLER_LIST};
-	static const unsigned					DisplayCountList[GDISP_TOTAL_CONTROLLERS] = {GDISP_CONTROLLER_DISPLAYS};
-#endif
-
 #if GDISP_NEED_TIMERFLUSH
 	static GTimer	FlushTimer;
 #endif
 
-static GDisplay GDisplayArray[GDISP_TOTAL_DISPLAYS];
-GDisplay	*GDISP = GDisplayArray;
+GDisplay	*GDISP;
 
 #if GDISP_NEED_MULTITHREAD
 	#define MUTEX_INIT(g)		gfxMutexInit(&(g)->mutex)
@@ -76,7 +67,7 @@ GDisplay	*GDISP = GDisplayArray;
 	#define TEST_CLIP_AREA(g)
 #elif GDISP_HARDWARE_CLIP == HARDWARE_AUTODETECT
 	#define TEST_CLIP_AREA(g)																					\
-			if (!g->vmt->setclip) {																				\
+			if (!gvmt(g)->setclip) {																				\
 				if ((g)->p.x < (g)->clipx0) { (g)->p.cx -= (g)->clipx0 - (g)->p.x; (g)->p.x = (g)->clipx0; }	\
 				if ((g)->p.y < (g)->clipy0) { (g)->p.cy -= (g)->clipy0 - (g)->p.y; (g)->p.y = (g)->clipy0; }	\
 				if ((g)->p.x + (g)->p.cx > (g)->clipx1)	(g)->p.cx = (g)->clipx1 - (g)->p.x;						\
@@ -109,7 +100,7 @@ GDisplay	*GDISP = GDisplayArray;
 #endif
 
 #if GDISP_NEED_AUTOFLUSH && GDISP_HARDWARE_FLUSH == HARDWARE_AUTODETECT
-	#define autoflush_stopdone(g)	if (g->vmt->flush) gdisp_lld_flush(g)
+	#define autoflush_stopdone(g)	if (gvmt(g)->flush) gdisp_lld_flush(g)
 #elif GDISP_NEED_AUTOFLUSH && GDISP_HARDWARE_FLUSH
 	#define autoflush_stopdone(g)	gdisp_lld_flush(g)
 #else
@@ -138,7 +129,7 @@ static INLINE void drawpixel(GDisplay *g) {
 	// Best is hardware accelerated pixel draw
 	#if GDISP_HARDWARE_DRAWPIXEL
 		#if GDISP_HARDWARE_DRAWPIXEL == HARDWARE_AUTODETECT
-			if (g->vmt->pixel)
+			if (gvmt(g)->pixel)
 		#endif
 		{
 			gdisp_lld_draw_pixel(g);
@@ -149,7 +140,7 @@ static INLINE void drawpixel(GDisplay *g) {
 	// Next best is cursor based streaming
 	#if GDISP_HARDWARE_DRAWPIXEL != TRUE && GDISP_HARDWARE_STREAM_POS && GDISP_HARDWARE_STREAM_WRITE
 		#if GDISP_HARDWARE_STREAM_POS == HARDWARE_AUTODETECT
-			if (g->vmt->writepos)
+			if (gvmt(g)->writepos)
 		#endif
 		{
 			if (!(g->flags & GDISP_FLG_SCRSTREAM))
@@ -164,7 +155,7 @@ static INLINE void drawpixel(GDisplay *g) {
 	#if GDISP_HARDWARE_DRAWPIXEL != TRUE && GDISP_HARDWARE_STREAM_POS != TRUE && GDISP_HARDWARE_STREAM_WRITE
 		// The following test is unneeded because we are guaranteed to have streaming if we don't have drawpixel
 		//#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-		//	if (g->vmt->writestart)
+		//	if (gvmt(g)->writestart)
 		//#endif
 		{
 			g->p.cx = g->p.cy = 1;
@@ -182,7 +173,7 @@ static INLINE void drawpixel(GDisplay *g) {
 #if NEED_CLIPPING
 	static INLINE void drawpixel_clip(GDisplay *g) {
 		#if GDISP_HARDWARE_CLIP == HARDWARE_AUTODETECT
-			if (!g->vmt->setclip)
+			if (!gvmt(g)->setclip)
 		#endif
 		{
 			if (g->p.x < g->clipx0 || g->p.x >= g->clipx1 || g->p.y < g->clipy0 || g->p.y >= g->clipy1)
@@ -204,7 +195,7 @@ static INLINE void fillarea(GDisplay *g) {
 	// Best is hardware accelerated area fill
 	#if GDISP_HARDWARE_FILLS
 		#if GDISP_HARDWARE_FILLS == HARDWARE_AUTODETECT
-			if (g->vmt->fill)
+			if (gvmt(g)->fill)
 		#endif
 		{
 			gdisp_lld_fill_area(g);
@@ -215,7 +206,7 @@ static INLINE void fillarea(GDisplay *g) {
 	// Next best is hardware streaming
 	#if GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE
 		#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-			if (g->vmt->writestart)
+			if (gvmt(g)->writestart)
 		#endif
 		{
 			uint32_t	area;
@@ -231,7 +222,7 @@ static INLINE void fillarea(GDisplay *g) {
 			gdisp_lld_write_start(g);
 			#if GDISP_HARDWARE_STREAM_POS
 				#if GDISP_HARDWARE_STREAM_POS == HARDWARE_AUTODETECT
-					if (g->vmt->writepos)
+					if (gvmt(g)->writepos)
 				#endif
 				gdisp_lld_write_pos(g);
 			#endif
@@ -246,7 +237,7 @@ static INLINE void fillarea(GDisplay *g) {
 	#if GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_HARDWARE_DRAWPIXEL
 		// The following test is unneeded because we are guaranteed to have draw pixel if we don't have streaming
 		//#if GDISP_HARDWARE_DRAWPIXEL == HARDWARE_AUTODETECT
-		//	if (g->vmt->pixel)
+		//	if (gvmt(g)->pixel)
 		//#endif
 		{
 			coord_t x0, y0, x1, y1;
@@ -277,7 +268,7 @@ static void hline_clip(GDisplay *g) {
 	// Clipping
 	#if NEED_CLIPPING
 		#if GDISP_HARDWARE_CLIP == HARDWARE_AUTODETECT
-			if (!g->vmt->setclip)
+			if (!gvmt(g)->setclip)
 		#endif
 		{
 			if (g->p.y < g->clipy0 || g->p.y >= g->clipy1) return;
@@ -300,7 +291,7 @@ static void hline_clip(GDisplay *g) {
 	// Best is hardware accelerated area fill
 	#if GDISP_HARDWARE_FILLS
 		#if GDISP_HARDWARE_FILLS == HARDWARE_AUTODETECT
-			if (g->vmt->fill)
+			if (gvmt(g)->fill)
 		#endif
 		{
 			g->p.cx = g->p.x1 - g->p.x + 1;
@@ -313,7 +304,7 @@ static void hline_clip(GDisplay *g) {
 	// Next best is cursor based streaming
 	#if GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_STREAM_POS && GDISP_HARDWARE_STREAM_WRITE
 		#if GDISP_HARDWARE_STREAM_POS == HARDWARE_AUTODETECT
-			if (g->vmt->writepos)
+			if (gvmt(g)->writepos)
 		#endif
 		{
 			if (!(g->flags & GDISP_FLG_SCRSTREAM))
@@ -328,7 +319,7 @@ static void hline_clip(GDisplay *g) {
 	// Next best is streaming
 	#if GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_STREAM_POS != TRUE && GDISP_HARDWARE_STREAM_WRITE
 		#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-			if (g->vmt->writestart)
+			if (gvmt(g)->writestart)
 		#endif
 		{
 			g->p.cx = g->p.x1 - g->p.x + 1;
@@ -344,7 +335,7 @@ static void hline_clip(GDisplay *g) {
 	#if GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_HARDWARE_DRAWPIXEL
 		// The following test is unneeded because we are guaranteed to have draw pixel if we don't have streaming
 		//#if GDISP_HARDWARE_DRAWPIXEL == HARDWARE_AUTODETECT
-		//	if (g->vmt->pixel)
+		//	if (gvmt(g)->pixel)
 		//#endif
 		{
 			for(; g->p.x <= g->p.x1; g->p.x++)
@@ -365,7 +356,7 @@ static void vline_clip(GDisplay *g) {
 	// Clipping
 	#if NEED_CLIPPING
 		#if GDISP_HARDWARE_CLIP == HARDWARE_AUTODETECT
-			if (!g->vmt->setclip)
+			if (!gvmt(g)->setclip)
 		#endif
 		{
 			if (g->p.x < g->clipx0 || g->p.x >= g->clipx1) return;
@@ -388,7 +379,7 @@ static void vline_clip(GDisplay *g) {
 	// Best is hardware accelerated area fill
 	#if GDISP_HARDWARE_FILLS
 		#if GDISP_HARDWARE_FILLS == HARDWARE_AUTODETECT
-			if (g->vmt->fill)
+			if (gvmt(g)->fill)
 		#endif
 		{
 			g->p.cy = g->p.y1 - g->p.y + 1;
@@ -401,7 +392,7 @@ static void vline_clip(GDisplay *g) {
 	// Next best is streaming
 	#if GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE
 		#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-			if (g->vmt->writestart)
+			if (gvmt(g)->writestart)
 		#endif
 		{
 			#if GDISP_HARDWARE_STREAM_POS
@@ -415,7 +406,7 @@ static void vline_clip(GDisplay *g) {
 			gdisp_lld_write_start(g);
 			#if GDISP_HARDWARE_STREAM_POS
 				#if GDISP_HARDWARE_STREAM_POS == HARDWARE_AUTODETECT
-					if (g->vmt->writepos)
+					if (gvmt(g)->writepos)
 				#endif
 				gdisp_lld_write_pos(g);
 			#endif
@@ -429,7 +420,7 @@ static void vline_clip(GDisplay *g) {
 	#if GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_HARDWARE_DRAWPIXEL
 		// The following test is unneeded because we are guaranteed to have draw pixel if we don't have streaming
 		//#if GDISP_HARDWARE_DRAWPIXEL == HARDWARE_AUTODETECT
-		//	if (g->vmt->pixel)
+		//	if (gvmt(g)->pixel)
 		//#endif
 		{
 			for(; g->p.y <= g->p.y1; g->p.y++)
@@ -517,6 +508,7 @@ static void line_clip(GDisplay *g) {
 }
 
 #if GDISP_STARTUP_LOGO_TIMEOUT > 0
+	static bool_t	initDone;
 	static void StartupLogoDisplay(GDisplay *g) {
 		coord_t			x, y, w;
 		const coord_t *	p;
@@ -561,7 +553,7 @@ static void line_clip(GDisplay *g) {
 		GDisplay *	g;
 		(void)		param;
 
-		for(g = GDisplayArray; g < &GDisplayArray[GDISP_TOTAL_DISPLAYS]; g++)
+		for(g = (GDisplay *)gdriverGetNext(GDRIVER_TYPE_DISPLAY, 0); g; g = (GDisplay *)gdriverGetNext(GDRIVER_TYPE_DISPLAY, (GDriver *)g))
 			gdispGFlush(g);
 	}
 #endif
@@ -572,51 +564,14 @@ static void line_clip(GDisplay *g) {
 
 void _gdispInit(void)
 {
-	GDisplay		*g;
-	uint16_t		i;
-
-	/* Initialise all controllers */
-	#if GDISP_TOTAL_CONTROLLERS > 1
-		uint16_t	j;
-
-		for(g = GDisplayArray, j=0; j < GDISP_TOTAL_CONTROLLERS; j++)
-			for(i = 0; i < DisplayCountList[j]; g++, i++) {
-				g->vmt = ControllerList[j];
-				g->systemdisplay = j*GDISP_TOTAL_CONTROLLERS+i;
-				g->controllerdisplay = i;
-	#else
-		for(g = GDisplayArray, i = 0; i < GDISP_TOTAL_DISPLAYS; g++, i++) {
-			g->systemdisplay = i;
-			g->controllerdisplay = i;
-	#endif
-			MUTEX_INIT(g);
-			MUTEX_ENTER(g);
-			g->flags = 0;
-			gdisp_lld_init(g);
-			MUTEX_EXIT(g);
-	}
-
-	// Set the orientation, the clipping area, clear all the displays (and add the logo if required)
-	for(g = GDisplayArray, i = 0; i < GDISP_TOTAL_DISPLAYS; g++, i++) {
-		#if defined(GDISP_DEFAULT_ORIENTATION) && GDISP_NEED_CONTROL && GDISP_HARDWARE_CONTROL
-			gdispGControl(g, GDISP_CONTROL_ORIENTATION, (void *)GDISP_DEFAULT_ORIENTATION);
-		#endif
-		#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-			gdispGSetClip(g, 0, 0, g->g.Width, g->g.Height);
-		#endif
-		gdispGClear(g, GDISP_STARTUP_COLOR);
-		#if GDISP_STARTUP_LOGO_TIMEOUT > 0
-			StartupLogoDisplay(g);
-		#endif
-		#if GDISP_HARDWARE_FLUSH
-			gdispGFlush(g);
-		#endif
-	}
-
 	// Re-clear the display after the timeout if we added the logo
 	#if GDISP_STARTUP_LOGO_TIMEOUT > 0
+		GDisplay	*g;
+
 		gfxSleepMilliseconds(GDISP_STARTUP_LOGO_TIMEOUT);
-		for(g = GDisplayArray, i = 0; i < GDISP_TOTAL_DISPLAYS; g++, i++) {
+		initDone = TRUE;
+
+		for(g = (GDisplay *)gdriverGetNext(GDRIVER_TYPE_DISPLAY, 0); g; g = (GDisplay *)gdriverGetNext(GDRIVER_TYPE_DISPLAY, (GDriver *)g)) {
 			gdispGClear(g, GDISP_STARTUP_COLOR);
 			#if GDISP_HARDWARE_FLUSH
 				gdispGFlush(g);
@@ -636,20 +591,73 @@ void _gdispDeinit(void)
 	/* ToDo */
 }
 
+bool_t _gdispInitDriver(GDriver *g, int driverinstance, int systeminstance) {
+	#define		gd		((GDisplay *)g)
+	bool_t		ret;
+
+	// Intialise fields
+	gd->systemdisplay = systeminstance;
+	gd->controllerdisplay = driverinstance;
+	gd->flags = 0;
+	MUTEX_INIT(gd);
+
+	// Call the driver init
+	MUTEX_ENTER(gd);
+	ret = gdisp_lld_init(gd);
+	MUTEX_EXIT(gd);
+	if (!ret) return FALSE;
+
+	// Set orientation, clip, blankscreen, startup logo and then flush
+	#if defined(GDISP_DEFAULT_ORIENTATION) && GDISP_NEED_CONTROL && GDISP_HARDWARE_CONTROL
+		gdispGControl(gd, GDISP_CONTROL_ORIENTATION, (void *)GDISP_DEFAULT_ORIENTATION);
+	#endif
+	#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
+		gdispGSetClip(gd, 0, 0, gd->g.Width, gd->g.Height);
+	#endif
+	gdispGClear(gd, GDISP_STARTUP_COLOR);
+
+	#if GDISP_STARTUP_LOGO_TIMEOUT > 0
+		if (!initDone)
+			StartupLogoDisplay(gd);
+	#endif
+
+	#if GDISP_HARDWARE_FLUSH
+		gdispGFlush(gd);
+	#endif
+
+	// If this is the first driver set GDISP
+	if (!GDISP)
+		GDISP = gd;
+
+	return TRUE;
+
+	#undef gd
+}
+
+void _gdispDeinitDriver(GDriver *g) {
+	(void) g;
+	// For now do nothing
+}
+
 GDisplay *gdispGetDisplay(unsigned display) {
-	if (display >= GDISP_TOTAL_DISPLAYS)
-		return 0;
-	return &GDisplayArray[display];
+	return (GDisplay *)gdriverGetInstance(GDRIVER_TYPE_DISPLAY, display);
 }
 
 void gdispSetDisplay(GDisplay *g) {
 	if (g) GDISP = g;
 }
 
+coord_t gdispGGetWidth(GDisplay *g)				{ return g->g.Width; }
+coord_t gdispGGetHeight(GDisplay *g)			{ return g->g.Height; }
+powermode_t gdispGGetPowerMode(GDisplay *g)		{ return g->g.Powermode; }
+orientation_t gdispGGetOrientation(GDisplay *g)	{ return g->g.Orientation; }
+uint8_t gdispGGetBacklight(GDisplay *g)			{ return g->g.Backlight; }
+uint8_t gdispGGetContrast(GDisplay *g)			{ return g->g.Contrast; }
+
 void gdispGFlush(GDisplay *g) {
 	#if GDISP_HARDWARE_FLUSH
 		#if GDISP_HARDWARE_FLUSH == HARDWARE_AUTODETECT
-			if (g->vmt->flush)
+			if (gvmt(g)->flush)
 		#endif
 		{
 			MUTEX_ENTER(g);
@@ -667,7 +675,7 @@ void gdispGFlush(GDisplay *g) {
 
 		#if NEED_CLIPPING
 			#if GDISP_HARDWARE_CLIP == HARDWARE_AUTODETECT
-				if (!g->vmt->setclip)
+				if (!gvmt(g)->setclip)
 			#endif
 			// Test if the area is valid - if not then exit
 			if (x < g->clipx0 || x+cx > g->clipx1 || y < g->clipy0 || y+cy > g->clipy1) {
@@ -681,7 +689,7 @@ void gdispGFlush(GDisplay *g) {
 		// Best is hardware streaming
 		#if GDISP_HARDWARE_STREAM_WRITE
 			#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-				if (g->vmt->writestart)
+				if (gvmt(g)->writestart)
 			#endif
 			{
 				g->p.x = x;
@@ -691,7 +699,7 @@ void gdispGFlush(GDisplay *g) {
 				gdisp_lld_write_start(g);
 				#if GDISP_HARDWARE_STREAM_POS
 					#if GDISP_HARDWARE_STREAM_POS == HARDWARE_AUTODETECT
-						if (g->vmt->writepos)
+						if (gvmt(g)->writepos)
 					#endif
 					gdisp_lld_write_pos(g);
 				#endif
@@ -703,7 +711,7 @@ void gdispGFlush(GDisplay *g) {
 		#if GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_HARDWARE_DRAWPIXEL
 			// The following test is unneeded because we are guaranteed to have draw pixel if we don't have streaming
 			//#if GDISP_HARDWARE_DRAWPIXEL == HARDWARE_AUTODETECT
-			//	if (g->vmt->pixel)
+			//	if (gvmt(g)->pixel)
 			//#endif
 			{
 				// Use x,y as the current position, x1,y1 as the save position and x2,y2 as the end position, cx = bufpos
@@ -736,7 +744,7 @@ void gdispGFlush(GDisplay *g) {
 		// Best is hardware streaming
 		#if GDISP_HARDWARE_STREAM_WRITE
 			#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-				if (g->vmt->writestart)
+				if (gvmt(g)->writestart)
 			#endif
 			{
 				g->p.color = color;
@@ -748,7 +756,7 @@ void gdispGFlush(GDisplay *g) {
 		// Next best is to use bitfills with our line buffer
 		#if GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_LINEBUF_SIZE != 0 && GDISP_HARDWARE_BITFILLS
 			#if GDISP_HARDWARE_BITFILLS == HARDWARE_AUTODETECT
-				if (g->vmt->blit)
+				if (gvmt(g)->blit)
 			#endif
 			{
 				g->linebuf[g->p.cx++] = color;
@@ -789,7 +797,7 @@ void gdispGFlush(GDisplay *g) {
 		#if GDISP_HARDWARE_STREAM_WRITE != TRUE && (GDISP_LINEBUF_SIZE == 0 || GDISP_HARDWARE_BITFILLS != TRUE) && GDISP_HARDWARE_FILLS
 			// We don't need to test for auto-detect on drawpixel as we know we have it because we don't have streaming.
 			#if GDISP_HARDWARE_FILLS == HARDWARE_AUTODETECT
-				if (g->vmt->fill)
+				if (gvmt(g)->fill)
 			#endif
 			{
 				if (!g->p.cx || g->p.color == color) {
@@ -825,7 +833,7 @@ void gdispGFlush(GDisplay *g) {
 		#if GDISP_HARDWARE_STREAM_WRITE != TRUE && (GDISP_LINEBUF_SIZE == 0 || GDISP_HARDWARE_BITFILLS != TRUE) && GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_DRAWPIXEL
 			// The following test is unneeded because we are guaranteed to have draw pixel if we don't have streaming
 			//#if GDISP_HARDWARE_DRAWPIXEL == HARDWARE_AUTODETECT
-			//	if (g->vmt->pixel)
+			//	if (gvmt(g)->pixel)
 			//#endif
 			{
 				g->p.color = color;
@@ -854,7 +862,7 @@ void gdispGFlush(GDisplay *g) {
 
 		#if GDISP_HARDWARE_STREAM_WRITE
 			#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-				if (g->vmt->writestart)
+				if (gvmt(g)->writestart)
 			#endif
 			{
 					gdisp_lld_write_stop(g);
@@ -866,7 +874,7 @@ void gdispGFlush(GDisplay *g) {
 
 		#if GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_LINEBUF_SIZE != 0 && GDISP_HARDWARE_BITFILLS
 			#if GDISP_HARDWARE_BITFILLS == HARDWARE_AUTODETECT
-				if (g->vmt->blit)
+				if (gvmt(g)->blit)
 			#endif
 			{
 				if (g->p.cx) {
@@ -884,7 +892,7 @@ void gdispGFlush(GDisplay *g) {
 		#if GDISP_HARDWARE_STREAM_WRITE != TRUE && (GDISP_LINEBUF_SIZE == 0 || GDISP_HARDWARE_BITFILLS != TRUE) && GDISP_HARDWARE_FILLS
 			// We don't need to test for auto-detect on drawpixel as we know we have it because we don't have streaming.
 			#if GDISP_HARDWARE_FILLS == HARDWARE_AUTODETECT
-				if (g->vmt->fill)
+				if (gvmt(g)->fill)
 			#endif
 			{
 				if (g->p.cx) {
@@ -937,7 +945,7 @@ void gdispGClear(GDisplay *g, color_t color) {
 	// Best is hardware accelerated clear
 	#if GDISP_HARDWARE_CLEARS
 		#if GDISP_HARDWARE_CLEARS == HARDWARE_AUTODETECT
-			if (g->vmt->clear)
+			if (gvmt(g)->clear)
 		#endif
 		{
 			g->p.color = color;
@@ -951,7 +959,7 @@ void gdispGClear(GDisplay *g, color_t color) {
 	// Next best is hardware accelerated area fill
 	#if GDISP_HARDWARE_CLEARS != TRUE && GDISP_HARDWARE_FILLS
 		#if GDISP_HARDWARE_FILLS == HARDWARE_AUTODETECT
-			if (g->vmt->fill)
+			if (gvmt(g)->fill)
 		#endif
 		{
 			g->p.x = g->p.y = 0;
@@ -968,7 +976,7 @@ void gdispGClear(GDisplay *g, color_t color) {
 	// Next best is streaming
 	#if GDISP_HARDWARE_CLEARS != TRUE && GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE
 		#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-			if (g->vmt->writestart)
+			if (gvmt(g)->writestart)
 		#endif
 		{
 			uint32_t	area;
@@ -982,7 +990,7 @@ void gdispGClear(GDisplay *g, color_t color) {
 			gdisp_lld_write_start(g);
 			#if GDISP_HARDWARE_STREAM_POS
 				#if GDISP_HARDWARE_STREAM_POS == HARDWARE_AUTODETECT
-					if (g->vmt->writepos)
+					if (gvmt(g)->writepos)
 				#endif
 				gdisp_lld_write_pos(g);
 			#endif
@@ -999,7 +1007,7 @@ void gdispGClear(GDisplay *g, color_t color) {
 	#if GDISP_HARDWARE_CLEARS != TRUE && GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_HARDWARE_DRAWPIXEL
 		// The following test is unneeded because we are guaranteed to have draw pixel if we don't have streaming
 		//#if GDISP_HARDWARE_DRAWPIXEL == HARDWARE_AUTODETECT
-		//	if (g->vmt->pixel)
+		//	if (gvmt(g)->pixel)
 		//#endif
 		{
 			g->p.color = color;
@@ -1032,7 +1040,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 
 	#if NEED_CLIPPING
 		#if GDISP_HARDWARE_CLIP == HARDWARE_AUTODETECT
-			if (!g->vmt->setclip)
+			if (!gvmt(g)->setclip)
 		#endif
 		{
 			// This is a different clipping to fillarea(g) as it needs to take into account srcx,srcy
@@ -1048,7 +1056,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 	// Best is hardware bitfills
 	#if GDISP_HARDWARE_BITFILLS
 		#if GDISP_HARDWARE_BITFILLS == HARDWARE_AUTODETECT
-			if (g->vmt->blit)
+			if (gvmt(g)->blit)
 		#endif
 		{
 			g->p.x = x;
@@ -1069,7 +1077,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 	// Next best is hardware streaming
 	#if GDISP_HARDWARE_BITFILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE
 		#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-			if (g->vmt->writestart)
+			if (gvmt(g)->writestart)
 		#endif
 		{
 			// Translate buffer to the real image data, use srcx,srcy as the end point, srccx as the buffer line gap
@@ -1085,7 +1093,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 			gdisp_lld_write_start(g);
 			#if GDISP_HARDWARE_STREAM_POS
 				#if GDISP_HARDWARE_STREAM_POS == HARDWARE_AUTODETECT
-					if (g->vmt->writepos)
+					if (gvmt(g)->writepos)
 				#endif
 				gdisp_lld_write_pos(g);
 			#endif
@@ -1106,7 +1114,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 	#if GDISP_HARDWARE_BITFILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_HARDWARE_FILLS
 		// We don't need to test for auto-detect on drawpixel as we know we have it because we don't have streaming.
 		#if GDISP_HARDWARE_FILLS == HARDWARE_AUTODETECT
-			if (g->vmt->fill)
+			if (gvmt(g)->fill)
 		#endif
 		{
 			// Translate buffer to the real image data, use srcx,srcy as the end point, srccx as the buffer line gap
@@ -1141,7 +1149,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 	#if GDISP_HARDWARE_BITFILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_DRAWPIXEL
 		// The following test is unneeded because we are guaranteed to have draw pixel if we don't have streaming
 		//#if GDISP_HARDWARE_DRAWPIXEL == HARDWARE_AUTODETECT
-		//	if (g->vmt->pixel)
+		//	if (gvmt(g)->pixel)
 		//#endif
 		{
 			// Translate buffer to the real image data, use srcx,srcy as the end point, srccx as the buffer line gap
@@ -1170,7 +1178,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 		// Best is using hardware clipping
 		#if GDISP_HARDWARE_CLIP
 			#if GDISP_HARDWARE_CLIP == HARDWARE_AUTODETECT
-				if (g->vmt->setclip)
+				if (gvmt(g)->setclip)
 			#endif
 			{
 				g->p.x = x;
@@ -2083,7 +2091,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 		MUTEX_ENTER(g);
 		#if GDISP_HARDWARE_PIXELREAD
 			#if GDISP_HARDWARE_PIXELREAD == HARDWARE_AUTODETECT
-				if (g->vmt->get)
+				if (gvmt(g)->get)
 			#endif
 			{
 				// Best is direct pixel read
@@ -2096,7 +2104,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 		#endif
 		#if GDISP_HARDWARE_PIXELREAD != TRUE && GDISP_HARDWARE_STREAM_READ
 			#if GDISP_HARDWARE_STREAM_READ == HARDWARE_AUTODETECT
-				if (g->vmt->readcolor)
+				if (gvmt(g)->readcolor)
 			#endif
 			{
 				// Next best is hardware streaming
@@ -2132,7 +2140,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 		MUTEX_ENTER(g);
 		#if NEED_CLIPPING
 			#if GDISP_HARDWARE_CLIP == HARDWARE_AUTODETECT
-				if (!g->vmt->setclip)
+				if (!gvmt(g)->setclip)
 			#endif
 			{
 				if (x < g->clipx0) { cx -= g->clipx0 - x; x = g->clipx0; }
@@ -2151,7 +2159,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 			// Best is hardware scroll
 			#if GDISP_HARDWARE_SCROLL
 				#if GDISP_HARDWARE_SCROLL == HARDWARE_AUTODETECT
-					if (g->vmt->vscroll)
+					if (gvmt(g)->vscroll)
 				#endif
 				{
 					g->p.x = x;
@@ -2197,7 +2205,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 							// Best line read is hardware streaming
 							#if GDISP_HARDWARE_STREAM_READ
 								#if GDISP_HARDWARE_STREAM_READ == HARDWARE_AUTODETECT
-									if (g->vmt->readstart)
+									if (gvmt(g)->readstart)
 								#endif
 								{
 									g->p.x = x+ix;
@@ -2217,7 +2225,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 							// Next best line read is single pixel reads
 							#if GDISP_HARDWARE_STREAM_READ != TRUE && GDISP_HARDWARE_PIXELREAD
 								#if GDISP_HARDWARE_PIXELREAD == HARDWARE_AUTODETECT
-									if (g->vmt->get)
+									if (gvmt(g)->get)
 								#endif
 								{
 									for(j=0; j < fx; j++) {
@@ -2245,7 +2253,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 							// Best line write is hardware bitfills
 							#if GDISP_HARDWARE_BITFILLS
 								#if GDISP_HARDWARE_BITFILLS == HARDWARE_AUTODETECT
-									if (g->vmt->blit)
+									if (gvmt(g)->blit)
 								#endif
 								{
 									g->p.x = x+ix;
@@ -2266,7 +2274,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 							// Next best line write is hardware streaming
 							#if GDISP_HARDWARE_BITFILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE
 								#if GDISP_HARDWARE_STREAM_WRITE == HARDWARE_AUTODETECT
-									if (g->vmt->writestart)
+									if (gvmt(g)->writestart)
 								#endif
 								{
 									g->p.x = x+ix;
@@ -2292,7 +2300,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 							#if GDISP_HARDWARE_BITFILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_HARDWARE_FILLS && GDISP_HARDWARE_DRAWPIXEL
 								// We don't need to test for auto-detect on drawpixel as we know we have it because we don't have streaming.
 								#if GDISP_HARDWARE_FILLS == HARDWARE_AUTODETECT
-									if (g->vmt->fill)
+									if (gvmt(g)->fill)
 								#endif
 								{
 									g->p.y = fy;
@@ -2324,7 +2332,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 							#if GDISP_HARDWARE_BITFILLS != TRUE && GDISP_HARDWARE_STREAM_WRITE != TRUE && GDISP_HARDWARE_FILLS != TRUE && GDISP_HARDWARE_DRAWPIXEL
 								// The following test is unneeded because we are guaranteed to have draw pixel if we don't have streaming
 								//#if GDISP_HARDWARE_DRAWPIXEL == HARDWARE_AUTODETECT
-								//	if (g->vmt->pixel)
+								//	if (gvmt(g)->pixel)
 								//#endif
 								{
 									g->p.y = fy;
@@ -2356,7 +2364,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 	#if GDISP_HARDWARE_CONTROL
 		void gdispGControl(GDisplay *g, unsigned what, void *value) {
 			#if GDISP_HARDWARE_CONTROL == HARDWARE_AUTODETECT
-				if (!g->vmt->control)
+				if (!gvmt(g)->control)
 					return;
 			#endif
 			MUTEX_ENTER(g);
@@ -2380,7 +2388,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 					// Best is hardware clipping
 					#if GDISP_HARDWARE_CLIP
 						#if GDISP_HARDWARE_CLIP == HARDWARE_AUTODETECT
-							if (g->vmt->setclip)
+							if (gvmt(g)->setclip)
 						#endif
 						{
 							g->p.x = 0;
@@ -2423,7 +2431,7 @@ void gdispGBlitArea(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, c
 			void *res;
 
 			#if GDISP_HARDWARE_QUERY == HARDWARE_AUTODETECT
-				if (!g->vmt->query)
+				if (!gvmt(g)->query)
 					return -1;
 			#endif
 			MUTEX_ENTER(g);
@@ -3056,45 +3064,5 @@ color_t gdispContrastColor(color_t color) {
 	}
 #endif
 
-#if GDISP_PIXELFORMAT != GDISP_LLD_PIXELFORMAT
-	LLDCOLOR_TYPE gdispColor2Native(color_t c) {
-		#if COLOR_SYSTEM == GDISP_COLORSYSTEM_GRAYSCALE || LLDCOLOR_SYSTEM == GDISP_COLORSYSTEM_GRAYSCALE
-			#if GDISP_HARDWARE_USE_EXACT_COLOR
-				return LLDLUMA2COLOR(EXACT_LUMA_OF(c));
-			#else
-				return LLDLUMA2COLOR(LUMA_OF(c));
-			#endif
-		#elif COLOR_SYSTEM == GDISP_COLORSYSTEM_TRUECOLOR && LLDCOLOR_SYSTEM == GDISP_COLORSYSTEM_TRUECOLOR
-			#if GDISP_HARDWARE_USE_EXACT_COLOR
-				return LLDRGB2COLOR(EXACT_RED_OF(c), EXACT_GREEN_OF(c), EXACT_BLUE_OF(c));
-			#else
-				return LLDRGB2COLOR(RED_OF(c), GREEN_OF(c), BLUE_OF(c));
-			#endif
-		#else
-			#error "GDISP: This pixel format conversion is not supported yet"
-		#endif
-	}
-#endif
-
-#if GDISP_PIXELFORMAT != GDISP_LLD_PIXELFORMAT
-	color_t gdispNative2Color(LLDCOLOR_TYPE c) {
-		#if COLOR_SYSTEM == GDISP_COLORSYSTEM_GRAYSCALE || LLDCOLOR_SYSTEM == GDISP_COLORSYSTEM_GRAYSCALE
-			#if GDISP_HARDWARE_USE_EXACT_COLOR
-				return LUMA2COLOR(LLDEXACT_LUMA_OF(c));
-			#else
-				return LUMA2COLOR(LLDLUMA_OF(c));
-			#endif
-		#elif COLOR_SYSTEM == GDISP_COLORSYSTEM_TRUECOLOR && LLDCOLOR_SYSTEM == GDISP_COLORSYSTEM_TRUECOLOR
-			#if GDISP_HARDWARE_USE_EXACT_COLOR
-				return RGB2COLOR(LLDEXACT_RED_OF(c), LLDEXACT_GREEN_OF(c), LLDEXACT_BLUE_OF(c));
-			#else
-				return RGB2COLOR(LLDRED_OF(c), LLDGREEN_OF(c), LLDBLUE_OF(c));
-			#endif
-		#else
-			#error "GDISP: This pixel format conversion is not supported yet"
-		#endif
-	}
-#endif
-
 #endif /* GFX_USE_GDISP */
 /** @} */
diff --git a/src/gdisp/sys_defs.h b/src/gdisp/sys_defs.h
index 253de49e..d67fdcee 100644
--- a/src/gdisp/sys_defs.h
+++ b/src/gdisp/sys_defs.h
@@ -72,22 +72,7 @@ typedef enum orientation { GDISP_ROTATE_0=0, GDISP_ROTATE_90=90, GDISP_ROTATE_18
 typedef enum powermode { powerOff, powerSleep, powerDeepSleep, powerOn } powermode_t;
 
 /*
- * This is not documented in Doxygen as it is meant to be a black-box.
- * Applications should always use the routines and macros defined
- * below to access it in case the implementation ever changed.
- */
-typedef struct GDISPControl {
-	coord_t				Width;
-	coord_t				Height;
-	orientation_t		Orientation;
-	powermode_t			Powermode;
-	uint8_t				Backlight;
-	uint8_t				Contrast;
-} GDISPControl;
-
-/*
- * Our black box display structure. We know only one thing about it...
- * The first member is a GDISPControl structure.
+ * Our black box display structure.
  */
 typedef struct GDisplay		GDisplay;
 
@@ -126,7 +111,7 @@ extern GDisplay	*GDISP;
 /* Defines relating to the display hardware									 */
 /*===========================================================================*/
 
-#if !defined(GDISP_TOTAL_CONTROLLERS) || GDISP_TOTAL_CONTROLLERS == 1
+#if GDISP_TOTAL_CONTROLLERS <= 1
 	// Pull in the default hardware configuration for a single controller.
 	// If we have multiple controllers the settings must be set in the
 	// users gfxconf.h file.
@@ -149,7 +134,7 @@ extern GDisplay	*GDISP;
 	 * @note	This doesn't need to match the hardware pixel format.
 	 * 			It is definitely more efficient when it does.
 	 * @note	When GDISP_TOTAL_CONTROLLERS > 1, this must
-	 * 			be explicitly defined and should ensure the best match
+	 * 			be explicitly defined and you should ensure the best match
 	 * 			with your hardware across all devices.
 	 */
 	#ifndef GDISP_PIXELFORMAT
@@ -196,7 +181,7 @@ typedef color_t		pixel_t;
 extern "C" {
 #endif
 
-/* Base Functions */
+/* Color Utility Functions */
 
 /**
  * @brief   Blend 2 colors according to the alpha
@@ -220,6 +205,8 @@ color_t gdispBlendColor(color_t fg, color_t bg, uint8_t alpha);
  */
 color_t gdispContrastColor(color_t color);
 
+/* Base Functions */
+
 /**
  * @brief   Get the specified display
  * @return	The pointer to the display or NULL if the display doesn't exist
@@ -248,6 +235,68 @@ GDisplay *gdispGetDisplay(unsigned display);
  */
 void gdispSetDisplay(GDisplay *g);
 
+/* Property Functions */
+
+/**
+ * @brief   Get the display width in pixels.
+ *
+ * @param[in] g 		The display to use
+ *
+ * @api
+ */
+coord_t gdispGGetWidth(GDisplay *g);
+#define gdispGetWidth()								gdispGGetWidth(GDISP)
+
+/**
+ * @brief   Get the display height in pixels.
+ *
+ * @param[in] g 		The display to use
+ *
+ * @api
+ */
+coord_t gdispGGetHeight(GDisplay *g);
+#define gdispGetHeight()							gdispGGetHeight(GDISP)
+
+/**
+ * @brief   Get the current display power mode.
+ *
+ * @param[in] g 		The display to use
+ *
+ * @api
+ */
+powermode_t gdispGGetPowerMode(GDisplay *g);
+#define gdispGetPowerMode()							gdispGGetPowerMode(GDISP)
+
+/**
+ * @brief   Get the current display orientation.
+ *
+ * @param[in] g 		The display to use
+ *
+ * @api
+ */
+orientation_t gdispGGetOrientation(GDisplay *g);
+#define gdispGetOrientation()						gdispGGetOrientation(GDISP)
+
+/**
+ * @brief   Get the current display backlight brightness.
+ *
+ * @param[in] g 		The display to use
+ *
+ * @api
+ */
+uint8_t gdispGGetBacklight(GDisplay *g);
+#define gdispGetBacklight()							gdispGGetBacklight(GDISP)
+
+/**
+ * @brief   Get the current display contrast.
+ *
+ * @param[in] g 		The display to use
+ *
+ * @api
+ */
+uint8_t gdispGGetContrast(GDisplay *g);
+#define gdispGetContrast()							gdispGGetContrast(GDISP)
+
 /* Drawing Functions */
 
 /**
@@ -918,66 +967,6 @@ void gdispGDrawBox(GDisplay *g, coord_t x, coord_t y, coord_t cx, coord_t cy, co
 #define gdispGSetContrast(g, percent)				gdispGControl((g), GDISP_CONTROL_CONTRAST, (void *)(unsigned)(percent))
 #define gdispSetContrast(percent)					gdispGControl(GDISP, GDISP_CONTROL_CONTRAST, (void *)(unsigned)(percent))
 
-/**
- * @brief   Get the display width in pixels.
- *
- * @param[in] g 		The display to use
- *
- * @api
- */
-#define gdispGGetWidth(g)							(((GDISPControl *)(g))->Width)
-#define gdispGetWidth()								gdispGGetWidth(GDISP)
-
-/**
- * @brief   Get the display height in pixels.
- *
- * @param[in] g 		The display to use
- *
- * @api
- */
-#define gdispGGetHeight(g)							(((GDISPControl *)(g))->Height)
-#define gdispGetHeight()							gdispGGetHeight(GDISP)
-
-/**
- * @brief   Get the current display power mode.
- *
- * @param[in] g 		The display to use
- *
- * @api
- */
-#define gdispGGetPowerMode(g)						(((GDISPControl *)(g))->Powermode)
-#define gdispGetPowerMode()							gdispGGetPowerMode(GDISP)
-
-/**
- * @brief   Get the current display orientation.
- *
- * @param[in] g 		The display to use
- *
- * @api
- */
-#define gdispGGetOrientation(g)						(((GDISPControl *)(g))->Orientation)
-#define gdispGetOrientation()						gdispGGetOrientation(GDISP)
-
-/**
- * @brief   Get the current display backlight brightness.
- *
- * @param[in] g 		The display to use
- *
- * @api
- */
-#define gdispGGetBacklight(g)						(((GDISPControl *)(g))->Backlight)
-#define gdispGetBacklight()							gdispGGetBacklight(GDISP)
-
-/**
- * @brief   Get the current display contrast.
- *
- * @param[in] g 		The display to use
- *
- * @api
- */
-#define gdispGGetContrast(g)						(((GDISPControl *)(g))->Contrast)
-#define gdispGetContrast()							gdispGGetContrast(GDISP)
-
 /* More interesting macro's */
 
 /**
diff --git a/src/gdisp/sys_rules.h b/src/gdisp/sys_rules.h
index 83c95216..eca55ce3 100644
--- a/src/gdisp/sys_rules.h
+++ b/src/gdisp/sys_rules.h
@@ -17,6 +17,13 @@
 #define _GDISP_RULES_H
 
 #if GFX_USE_GDISP
+	#if !GFX_USE_GDRIVER
+		#if GFX_DISPLAY_RULE_WARNINGS
+			#warning "GDISP: GFX_USE_GDRIVER is required. GFX_USE_GDRIVER has turned on for you."
+		#endif
+		#undef GFX_USE_GDRIVER
+		#define GFX_USE_GDRIVER				TRUE
+	#endif
 	#if GDISP_TOTAL_CONTROLLERS > 1
 		#ifndef GDISP_CONTROLLER_LIST
 			#error "GDISP Multiple Controllers: You must specify a value for GDISP_CONTROLLER_LIST"
diff --git a/src/gdriver/gdriver_gdriver.c b/src/gdriver/gdriver_gdriver.c
index a6eb3e98..2c73719e 100644
--- a/src/gdriver/gdriver_gdriver.c
+++ b/src/gdriver/gdriver_gdriver.c
@@ -7,42 +7,72 @@
 
 #include "gfx.h"
 
-#if GFX_NEED_GDRIVER
+#if GFX_USE_GDRIVER
 
-// Some HACKS as these aren't supported yet
-#ifndef GINPUT_NEED_STRING
-	#define GINPUT_NEED_STRING	FALSE
-#endif
-#ifndef GFX_USE_GBLOCK
-	#define GFX_USE_GBLOCK	FALSE
-#endif
+#include "sys_defs.h"
 
 // Define the tables to hold the driver instances.
 static GDriver *dhead;
 
+// Definition that allows getting addresses of structures
+typedef const struct GDriverVMT const	VMT_EL[1];
+
 // The system initialization.
 void _gdriverInit(void) {
-	const GDriverAutoStart const *pa;
-	int		cnt;
-
 	#if GFX_USE_GDISP
-		#ifdef GDISP_DRIVER_LIST
-			{
-				static const struct {
-					const struct GDISPVMT const *	vmt;
-					int								instances;
-				} drivers[] = {GDISP_DRIVER_LIST};
-
-				for(pa = drivers; pa < &drivers[sizeof(drivers)/sizeof(drivers[0])]) {
-					for(cnt = pa->instances; cnt; cnt--)
-						gdriverRegister(pa->vmt);
-				}
+	{
+		// Both GDISP_CONTROLLER_LIST and GDISP_CONTROLLER_DISPLAYS are defined - create the required numbers of each controller
+		#if defined(GDISP_CONTROLLER_LIST) && defined(GDISP_CONTROLLER_DISPLAYS)
+			int		i, cnt;
+
+
+			extern VMT_EL							GDISP_CONTROLLER_LIST;
+			static const struct GDriverVMT const *	dclist[GDISP_TOTAL_CONTROLLERS] = {GDISP_CONTROLLER_LIST};
+			static const unsigned					dnlist[GDISP_TOTAL_CONTROLLERS] = {GDISP_CONTROLLER_DISPLAYS};
+			for(i = 0; i < GDISP_TOTAL_CONTROLLERS; i++) {
+				for(cnt = dnlist[i]; cnt; cnt--)
+					gdriverRegister(dclist[i]);
 			}
+
+		// Only GDISP_CONTROLLER_LIST is defined - create one of each controller
+		#elif defined(GDISP_CONTROLLER_LIST)
+			int		i;
+
+
+			extern VMT_EL							GDISP_CONTROLLER_LIST;
+			static const struct GDriverVMT const *	dclist[GDISP_TOTAL_CONTROLLERS] = {GDISP_CONTROLLER_LIST};
+			for(i = 0; i < GDISP_TOTAL_CONTROLLERS; i++)
+				gdriverRegister(dclist[i]);
+
+		// Only GDISP_TOTAL_DISPLAYS is defined - create the required number of the one controller
+		#elif GDISP_TOTAL_DISPLAYS > 1
+			int		cnt;
+
+			extern VMT_EL GDISPVMT_OnlyOne;
+			for(cnt = 0; cnt < GDISP_TOTAL_DISPLAYS; cnt++)
+				gdriverRegister(GDISPVMT_OnlyOne);
+
+		// One and only one display
 		#else
-			extern struct GDISPVMT GDISP_VMT;
-			gdriverRegister((GDriver *)&GDISP_VMT);
+			extern VMT_EL GDISPVMT_OnlyOne;
+			gdriverRegister(GDISPVMT_OnlyOne);
 		#endif
+	}
 	#endif
+
+	// Drivers not loaded yet
+	// GINPUT_NEED_MOUSE
+	// GINPUT_NEED_DIAL
+	// GINPUT_NEED_TOGGLE
+	// GINPUT_NEED_KEYBOARD
+	// GINPUT_NEED_STRING
+	// GFX_USE_GBLOCK
+}
+
+// The system de-initialization.
+void _gdriverDeinit(void) {
+	while(dhead)
+		gdriverUnRegister(dhead);
 }
 
 
@@ -76,6 +106,8 @@ GDriver *gdriverRegister(const GDriverVMT *vmt) {
 		dtail->driverchain = pd;
 	else
 		dhead = pd;
+
+	return pd;
 }
 
 void gdriverUnRegister(GDriver *driver) {
@@ -99,7 +131,7 @@ void gdriverUnRegister(GDriver *driver) {
 
 	// Call the deinit()
 	if (driver->vmt->deinit)
-		driver->vmt->dinit(driver);
+		driver->vmt->deinit(driver);
 
 	// Cleanup
 	gfxFree(driver);
@@ -134,4 +166,13 @@ int gdriverInstanceCount(uint16_t type) {
 	return sinstance;
 }
 
-#endif /* GFX_NEED_GDRIVER */
+GDriver *gdriverGetNext(uint16_t type, GDriver *driver) {
+	driver = driver ? driver->driverchain : dhead;
+
+	while(driver && driver->vmt->type != type)
+		driver = driver->driverchain;
+
+	return driver;
+}
+
+#endif /* GFX_USE_GDRIVER */
diff --git a/src/gdriver/sys_defs.h b/src/gdriver/sys_defs.h
index 3392f73a..c5ab9862 100644
--- a/src/gdriver/sys_defs.h
+++ b/src/gdriver/sys_defs.h
@@ -34,7 +34,7 @@
 #ifndef _GDRIVER_H
 #define _GDRIVER_H
 
-#if GFX_NEED_GDRIVER || defined(__DOXYGEN__)
+#if GFX_USE_GDRIVER || defined(__DOXYGEN__)
 
 /*===========================================================================*/
 /* Type definitions                                                          */
@@ -56,7 +56,7 @@
  * 			dynamically by the system for each driver instance.
  */
 typedef struct GDriver {
-	GDriver *					driverchain;
+	struct GDriver *			driverchain;
 	const struct GDriverVMT *	vmt;
 } GDriver;
 
@@ -64,13 +64,13 @@ typedef struct GDriver {
  * @brief	All driver VMT's start with this structure.
  */
 typedef struct GDriverVMT {
-	uint16_t	type;															// @< What type of driver this is
-	uint16_t	flags;															// @< Flags for the driver. Meaning is specific to each driver type.
-	uint32_t	objsize;														// @< How big the runtime driver structure is
-	bool_t		(*init)(void *driver, int driverinstance, int systeminstance);	// @< Initialise the driver.
-																				//		driverinstance is the instance 0..n of this driver.
-																				//		systeminstance is the instance 0..n of this type of device.
-	void		(*deinit)(void *driver);										// @< De-initialise the driver
+	uint16_t	type;																// @< What type of driver this is
+	uint16_t	flags;																// @< Flags for the driver. Meaning is specific to each driver type.
+	uint32_t	objsize;															// @< How big the runtime driver structure is
+	bool_t		(*init)(GDriver *driver, int driverinstance, int systeminstance);	// @< Initialise the driver.
+																					//		driverinstance is the instance 0..n of this driver.
+																					//		systeminstance is the instance 0..n of this type of device.
+	void		(*deinit)(GDriver *driver);											// @< De-initialise the driver
 } GDriverVMT;
 
 /*===========================================================================*/
@@ -115,11 +115,20 @@ extern "C" {
 	 */
 	int gdriverInstanceCount(uint16_t type);
 
+	/**
+	 * @brief	Get the next driver for a type of device
+	 * @return	The runtime driver structure or NULL if there are no more.
+	 *
+	 * @param[in]	type		The type of driver to find
+	 * @param[in]	driver		The last driver returned or NULL to start again
+	 */
+	GDriver *gdriverGetNext(uint16_t type, GDriver *driver);
+
 #ifdef __cplusplus
 }
 #endif
 
-#endif /* GFX_NEED_GDRIVER */
+#endif /* GFX_USE_GDRIVER */
 
 #endif /* _GDRIVER_H */
 /** @} */
diff --git a/src/gfx.c b/src/gfx.c
index b291a736..0803f59a 100644
--- a/src/gfx.c
+++ b/src/gfx.c
@@ -20,6 +20,10 @@ static bool_t initDone = FALSE;
 /* These init functions are defined by each module but not published */
 extern void _gosInit(void);
 extern void _gosDeinit(void);
+#if GFX_USE_GDRIVER
+	extern void _gdriverInit(void);
+	extern void _gdriverDeinit(void);
+#endif
 #if GFX_USE_GDISP
 	extern void _gdispInit(void);
 	extern void _gdispDeinit(void);
@@ -79,6 +83,9 @@ void gfxInit(void)
 	#if GFX_USE_GTIMER
 		_gtimerInit();
 	#endif
+	#if GFX_USE_GDRIVER
+		_gdriverInit();
+	#endif
 	#if GFX_USE_GDISP
 		_gdispInit();
 	#endif
@@ -118,6 +125,9 @@ void gfxDeinit(void)
 	#if GFX_USE_GDISP
 		_gdispDeinit();
 	#endif
+	#if GFX_USE_GDRIVER
+		_gdriverDeinit();
+	#endif
 	#if GFX_USE_GTIMER
 		_gtimerDeinit();
 	#endif