diff --git a/halext/drivers/gdispNokia6610/gdisp_lld.c b/halext/drivers/gdispNokia6610/gdisp_lld.c
index 1608c1f0..5ebe2177 100644
--- a/halext/drivers/gdispNokia6610/gdisp_lld.c
+++ b/halext/drivers/gdispNokia6610/gdisp_lld.c
@@ -32,19 +32,13 @@
 
 #if HAL_USE_GDISP || defined(__DOXYGEN__)
 
+/* Include the emulation code for things we don't support */
+#include "gdisp_emulation.c"
+
 /*===========================================================================*/
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
-#ifdef UNUSED
-#elif defined(__GNUC__)
-# define UNUSED(x) UNUSED_ ## x __attribute__((unused))
-#elif defined(__LCLINT__)
-# define UNUSED(x) /*@unused@*/ x
-#else
-# define UNUSED(x) x
-#endif
-
 /* Controller definitions */
 #if defined(LCD_USE_GE8)
 	#include "GE8.h"
@@ -58,10 +52,6 @@
 /* Driver exported variables.                                                */
 /*===========================================================================*/
 
-#if !defined(__DOXYGEN__)
-	GDISPDriver GDISP;
-#endif
-
 /*===========================================================================*/
 /* Driver local variables.                                                   */
 /*===========================================================================*/
@@ -79,8 +69,8 @@
 	#include "gdisp_lld_board.h"
 #endif
 
-#define gdisp_lld_write_command(cmd)		gdisp_lld_write_spi((cmd) & ~0x0100)
-#define gdisp_lld_write_data(data)			gdisp_lld_write_spi((data) | 0x0100)
+#define gdisp_lld_write_command(cmd)		GDISP_LLD(write_spi)((cmd) & ~0x0100)
+#define gdisp_lld_write_data(data)			GDISP_LLD(write_spi)((data) | 0x0100)
 
 static __inline void gdisp_lld_setviewport(coord_t x, coord_t y, coord_t cx, coord_t cy) {
 	gdisp_lld_write_command(CASET);			// Column address set
@@ -91,12 +81,6 @@ static __inline void gdisp_lld_setviewport(coord_t x, coord_t y, coord_t cx, coo
 	gdisp_lld_write_data(y+cy-1);
 }
 
-void Delay (unsigned long a) {
-	chThdSleepMilliseconds(a/100);
-//	volatile unsigned long d;
-//	for(d=a; d; d--);
-}
-
 /*===========================================================================*/
 /* Driver interrupt handlers.                                                */
 /*===========================================================================*/
@@ -116,15 +100,15 @@ void Delay (unsigned long a) {
  *
  * @notapi
  */
-void gdisp_lld_init(void) {
+bool_t GDISP_LLD(init)(void) {
 	/* Initialise your display */
-	gdisp_lld_init_board();
+	GDISP_LLD(init_board)();
 
 	// Hardware reset
-	gdisp_lld_setpin_reset(TRUE);
-	Delay(20000);
-	gdisp_lld_setpin_reset(FALSE);
-	Delay(20000);
+	GDISP_LLD(setpin_reset)(TRUE);
+	chThdSleepMilliseconds(20);
+	GDISP_LLD(setpin_reset)(FALSE);
+	chThdSleepMilliseconds(20);
 
 	#if defined(LCD_USE_GE8)
 		#if 1
@@ -147,7 +131,7 @@ void gdisp_lld_init(void) {
 			gdisp_lld_write_command(VOLCTR);		// Voltage control (contrast setting)
 			gdisp_lld_write_data(32);					// P1 = 32 volume value (experiment with this value to get the best contrast)
 			gdisp_lld_write_data(3);					// P2 = 3 resistance ratio (only value that works)
-			Delay(100000);							// allow power supply to stabilize
+			chThdSleepMilliseconds(100);			// allow power supply to stabilize
 			gdisp_lld_write_command(DISON);			// Turn on the display
 		#else
 			// Alternative
@@ -158,7 +142,7 @@ void gdisp_lld_init(void) {
 			gdisp_lld_write_command(COMSCN);		// COM scan
 			gdisp_lld_write_data(0x00);					// Scan 1-80
 			gdisp_lld_write_command(OSCON);			// Internal oscilator ON
-			Delay(10000);							// wait aproximetly 100ms
+			chThdSleepMilliseconds(100);			// wait aproximetly 100ms
 			gdisp_lld_write_command(SLPOUT);		// Sleep out
 			gdisp_lld_write_command(VOLCTR);		// Voltage control
 			gdisp_lld_write_data(0x1F);					// middle value of V1
@@ -200,15 +184,15 @@ void gdisp_lld_init(void) {
 			gdisp_lld_write_data(0xC8);				// 0xC0 = mirror x and y, reverse rgb
 			gdisp_lld_write_command(SETCON);		// Write contrast
 			gdisp_lld_write_data(0x30);				// contrast - experiental value
-			Delay(2000);
+			chThdSleepMilliseconds(20);
 			gdisp_lld_write_command(DISPON);		// Display On
 		#else
 			// Alternative
 			// Hardware reset commented out
 			gdisp_lld_write_command(SOFTRST);		// Software Reset
-			Delay(2000);
+			chThdSleepMilliseconds(20);
 			gdisp_lld_write_command(INITESC);		// Initial escape
-			Delay(2000);
+			chThdSleepMilliseconds(20);
 			gdisp_lld_write_command(REFSET);		// Refresh set
 			gdisp_lld_write_data(0);
 			gdisp_lld_write_command(DISPCTRL);		// Set Display control
@@ -258,7 +242,7 @@ void gdisp_lld_init(void) {
 			//gdisp_lld_write_data(0x03);					//  must be "1"
 			gdisp_lld_write_command(CONTRAST);			// Write contrast
 			gdisp_lld_write_data(0x3b);						// contrast
-			Delay(2000);
+			chThdSleepMilliseconds(20);
 			gdisp_lld_write_command(TEMPGRADIENT);		// Temperature gradient
 			for(i=0; i<14; i++) gdisp_lld_write_data(0);
 			gdisp_lld_write_command(BOOSTVON);			// Booster voltage ON
@@ -267,13 +251,16 @@ void gdisp_lld_init(void) {
 	#endif
 
 	/* Turn on the back-light */
-	gdisp_lld_setpin_backlight(TRUE);
+	GDISP_LLD(setpin_backlight)(TRUE);
 
 	/* Initialise the GDISP structure to match */
 	GDISP.Width = 132;
 	GDISP.Height = 132;
 	GDISP.Orientation = portrait;
 	GDISP.Powermode = powerOn;
+	GDISP.Backlight = 100;
+	GDISP.Contrast = 50;
+	return TRUE;
 }
 
 /**
@@ -285,7 +272,7 @@ void gdisp_lld_init(void) {
  *
  * @notapi
  */
-void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
+void GDISP_LLD(drawpixel)(coord_t x, coord_t y, color_t color) {
 	#if GDISP_NEED_VALIDATION
 		if (x >= GDISP.Width || y >= GDISP.Height) return;
 	#endif
@@ -306,11 +293,11 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	Don't bother coding for obvious similar routines if
 	there is no performance penalty as the emulation software
 	makes a good job of using similar routines.
-		eg. If gdisp_lld_fillarea() is defined there is little
-			point in defining gdisp_lld_clear() unless the
+		eg. If fillarea() is defined there is little
+			point in defining clear() unless the
 			performance bonus is significant.
 	For good performance it is suggested to implement
-		gdisp_lld_fillarea() and gdisp_lld_blitarea().
+		fillarea() and blitarea().
 */
 
 #if GDISP_HARDWARE_CLEARS || defined(__DOXYGEN__)
@@ -322,7 +309,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_clear(color_t color) {
+	void GDISP_LLD(clear(color_t color) {
 		/* NOT IMPLEMENTED */
 		/* Nothing to be gained by implementing this
 		 * as fillarea is just as fast.
@@ -341,7 +328,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawline(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
+	void GDISP_LLD(drawline)(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -357,7 +344,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillarea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
+	void GDISP_LLD(fillarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
 		unsigned i, tuples;
 
 		#if GDISP_NEED_VALIDATION
@@ -390,7 +377,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_blitarea(coord_t x, coord_t y, coord_t cx, coord_t cy, pixel_t *buffer) {
+	void GDISP_LLD(blitarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer) {
 		unsigned i, area, tuples;
 		#ifndef GDISP_PACKED_PIXELS
 			color_t	c1, c2;
@@ -451,7 +438,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(drawcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -469,7 +456,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(fillcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -487,7 +474,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(drawellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -505,7 +492,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(fillellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -525,7 +512,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawchar(coord_t x, coord_t y, char c, font_t font, color_t color) {
+	void GDISP_LLD(drawchar)(coord_t x, coord_t y, char c, font_t font, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -542,7 +529,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillchar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
+	void GDISP_LLD(fillchar)(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -557,7 +544,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	color_t gdisp_lld_getpixelcolor(coord_t x, coord_t y) {
+	color_t GDISP_LLD(getpixelcolor)(coord_t x, coord_t y) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -576,7 +563,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_verticalscroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
+	void GDISP_LLD(verticalscroll)(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -601,7 +588,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_control(int what, void *value) {
+	void GDISP_LLD(control)(int what, void *value) {
 		/* NOT IMPLEMENTED YET */
 		switch(what) {
 		case GDISP_CONTROL_POWER:
@@ -615,7 +602,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 					/* 	Code here */
 					/* You may need this ---
 						if (GDISP.Powermode != powerSleep)
-							gdisp_lld_init();
+							GDISP_LLD(init();
 					*/
 					break;
 				case powerSleep:
@@ -665,5 +652,37 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	}
 #endif
 
+#if (GDISP_NEED_QUERY && GDISP_HARDWARE_QUERY) || defined(__DOXYGEN__)
+/**
+ * @brief   Query a driver value.
+ * @detail	Typecase the result to the type you want.
+ * @note	GDISP_QUERY_WIDTH			- (coord_t)	Gets the width of the screen
+ * 			GDISP_QUERY_HEIGHT			- (coord_t)	Gets the height of the screen
+ * 			GDISP_QUERY_POWER			- (gdisp_powermode_t) Get the current powermode
+ * 			GDISP_QUERY_ORIENTATION		- (gdisp_orientation_t) Get the current screen orientation
+ * 			GDISP_QUERY_BACKLIGHT		- (coord_t) Get the backlight state (0 to 100)
+ * 			GDISP_QUERY_CONTRAST		- (coord_t) Get the contrast (0 to 100).
+ * 			GDISP_QUERY_LLD				- Low level driver control constants start at
+ * 											this value.
+ *
+ * @param[in] what     What to Query
+ *
+ * @notapi
+ */
+void *GDISP_LLD(query)(unsigned what) {
+	switch(what) {
+	case GDISP_QUERY_WIDTH:			return (void *)(unsigned)GDISP.Width;
+	case GDISP_QUERY_HEIGHT:		return (void *)(unsigned)GDISP.Height;
+	case GDISP_QUERY_POWER:			return (void *)(unsigned)GDISP.Powermode;
+	case GDISP_QUERY_ORIENTATION:	return (void *)(unsigned)GDISP.Orientation;
+	case GDISP_QUERY_BACKLIGHT:		return (void *)(unsigned)GDISP.Backlight;
+	case GDISP_QUERY_CONTRAST:		return (void *)(unsigned)GDISP.Contrast;
+	case GDISP_QUERY_LLD+0:
+		/* Code here */
+	default:						return (void *)-1;
+	}
+}
+#endif
+
 #endif /* HAL_USE_GDISP */
 /** @} */
diff --git a/halext/drivers/gdispNokia6610/gdisp_lld_board_example.h b/halext/drivers/gdispNokia6610/gdisp_lld_board_example.h
index 984e1aae..0e2b806e 100644
--- a/halext/drivers/gdispNokia6610/gdisp_lld_board_example.h
+++ b/halext/drivers/gdispNokia6610/gdisp_lld_board_example.h
@@ -39,7 +39,7 @@
  *
  * @notapi
  */
-static __inline void gdisp_lld_init_board(void) {
+static __inline void GDISP_LLD(init_board)(void) {
 	/* Code here */
 }
 
@@ -50,7 +50,7 @@ static __inline void gdisp_lld_init_board(void) {
  * 
  * @notapi
  */
-static __inline void gdisp_lld_setpin_reset(bool_t state) {
+static __inline void GDISP_LLD(setpin_reset)(bool_t state) {
 	/* Code here */
 }
 
@@ -61,7 +61,7 @@ static __inline void gdisp_lld_setpin_reset(bool_t state) {
  * 
  * @notapi
  */
-static __inline void gdisp_lld_setpin_backlight(bool_t state) {
+static __inline void GDISP_LLD(setpin_backlight)(bool_t state) {
 	/* Code here */
 }
 
@@ -74,7 +74,7 @@ static __inline void gdisp_lld_setpin_backlight(bool_t state) {
  * 
  * @notapi
  */
-static __inline void gdisp_lld_write_spi(uint16_t data) {
+static __inline void GDISP_LLD(write_spi)(uint16_t data) {
 	/* Code here */
 }
 
@@ -88,7 +88,7 @@ static __inline void gdisp_lld_write_spi(uint16_t data) {
  * 
  * @notapi
  */
-static __inline uint16_t gdisp_lld_read_spi(void) {
+static __inline uint16_t GDISP_LLD(read_spi)(void) {
 	/* Code here */
 }
 #endif
diff --git a/halext/drivers/gdispNokia6610/gdisp_lld_board_olimexsam7ex256.h b/halext/drivers/gdispNokia6610/gdisp_lld_board_olimexsam7ex256.h
index e2a8c1d7..b5b4b1d5 100644
--- a/halext/drivers/gdispNokia6610/gdisp_lld_board_olimexsam7ex256.h
+++ b/halext/drivers/gdispNokia6610/gdisp_lld_board_olimexsam7ex256.h
@@ -82,7 +82,7 @@ volatile AT91PS_PDC pPDC = AT91C_BASE_PDC_SPI0;
  *
  * @notapi
  */
-static __inline void gdisp_lld_init_board(void) {
+static __inline void GDISP_LLD(init_board)(void) {
 	// *********************************************************************************************
 	// InitSpi( )
 	//
@@ -142,7 +142,7 @@ static __inline void gdisp_lld_init_board(void) {
  * 
  * @notapi
  */
-static __inline void gdisp_lld_setpin_reset(bool_t state) {
+static __inline void GDISP_LLD(setpin_reset)(bool_t state) {
 	if (state)
 		palClearPad(IOPORT1, PIOA_LCD_RESET);
 //		pPIOA->PIO_CODR = PIOA_LCD_RESET_MASK;
@@ -158,7 +158,7 @@ static __inline void gdisp_lld_setpin_reset(bool_t state) {
  * 
  * @notapi
  */
-static __inline void gdisp_lld_setpin_backlight(bool_t state) {
+static __inline void GDISP_LLD(setpin_backlight)(bool_t state) {
 	if (state)
 		palSetPad(IOPORT2, PIOB_LCD_BL);
 //		pPIOB->PIO_SODR = PIOB_LCD_BL_MASK;
@@ -174,7 +174,7 @@ static __inline void gdisp_lld_setpin_backlight(bool_t state) {
  * 
  * @notapi
  */
-static __inline void gdisp_lld_write_spi(uint16_t data) {
+static __inline void GDISP_LLD(write_spi)(uint16_t data) {
 	// wait for the previous transfer to complete
 	while((pSPI->SPI_SR & AT91C_SPI_TXEMPTY) == 0);
 	// send the data
@@ -189,7 +189,7 @@ static __inline void gdisp_lld_write_spi(uint16_t data) {
  * 
  * @notapi
  */
-static __inline uint16_t gdisp_lld_read_spi(void) {
+static __inline uint16_t GDISP_LLD(read_spi)(void) {
 	#error "gdispNokia6610: GDISP_HARDWARE_READPIXEL and GDISP_HARDWARE_SCROLL are not supported on this board"
 	return 0;
 }
diff --git a/halext/drivers/gdispNokia6610/gdisp_lld_config.h b/halext/drivers/gdispNokia6610/gdisp_lld_config.h
index fafda6c3..9e2d1258 100644
--- a/halext/drivers/gdispNokia6610/gdisp_lld_config.h
+++ b/halext/drivers/gdispNokia6610/gdisp_lld_config.h
@@ -35,6 +35,9 @@
 /* Driver hardware support.                                                  */
 /*===========================================================================*/
 
+#define GDISP_DRIVER_NAME				"Nokia6610"
+#define GDISP_LLD(x)					gdisp_lld_##x##_Nokia6610
+
 #define GDISP_HARDWARE_LINES			FALSE
 #define GDISP_HARDWARE_CLEARS			FALSE
 #define GDISP_HARDWARE_FILLS			TRUE
@@ -48,22 +51,15 @@
 #define GDISP_HARDWARE_SCROLL			FALSE
 #define GDISP_HARDWARE_PIXELREAD		FALSE
 #define GDISP_HARDWARE_CONTROL			FALSE
+#define GDISP_HARDWARE_QUERY			FALSE
 
 #define GDISP_SOFTWARE_TEXTFILLDRAW		TRUE
 #define GDISP_SOFTWARE_TEXTBLITCOLUMN	FALSE
 
-#define GDISP_PIXELFORMAT_RGB444
+#define GDISP_PIXELFORMAT				GDISP_PIXELFORMAT_RGB444
 #define GDISP_PACKED_PIXELS				FALSE
 #define GDISP_PACKED_LINES				FALSE
 
-/*===========================================================================*/
-/* Extra fields for the GDISPDriver structure                                 */
-/*===========================================================================*/
-
-/*
-#define GDISP_DRIVER_EXT_FIELDS		int abc; int def;
-*/
-
 #endif	/* HAL_USE_GDISP */
 
 #endif	/* _GDISP_LLD_CONFIG_H */
diff --git a/halext/drivers/gdispS6d1121/gdisp_lld.c b/halext/drivers/gdispS6d1121/gdisp_lld.c
index 4e83fc82..32e0fa32 100644
--- a/halext/drivers/gdispS6d1121/gdisp_lld.c
+++ b/halext/drivers/gdispS6d1121/gdisp_lld.c
@@ -32,27 +32,17 @@
 
 #if HAL_USE_GDISP || defined(__DOXYGEN__)
 
+/* Include the emulation code for things we don't support */
+#include "gdisp_emulation.c"
+
 /*===========================================================================*/
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
-#ifdef UNUSED
-#elif defined(__GNUC__)
-# define UNUSED(x) UNUSED_ ## x __attribute__((unused))
-#elif defined(__LCLINT__)
-# define UNUSED(x) /*@unused@*/ x
-#else
-# define UNUSED(x) x
-#endif
-
 /*===========================================================================*/
 /* Driver exported variables.                                                */
 /*===========================================================================*/
 
-#if !defined(__DOXYGEN__)
-	GDISPDriver GDISP;
-#endif
-
 /*===========================================================================*/
 /* Driver local variables.                                                   */
 /*===========================================================================*/
@@ -82,7 +72,7 @@
  *
  * @notapi
  */
-void gdisp_lld_init(void) {
+bool_t GDISP_LLD(init)(void) {
 	palSetPadMode(LCD_RST_GPIO, LCD_RST_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
 	// A Good idea to reset the module before using
 	LCD_RST_LOW;
@@ -208,6 +198,9 @@ void gdisp_lld_init(void) {
 	GDISP.Height = SCREEN_HEIGHT;
 	GDISP.Orientation = portrait;
 	GDISP.Powermode = powerOn;
+	GDISP.Backlight = 100;
+	GDISP.Contrast = 50;
+	return TRUE;
 }
 
 /**
@@ -219,7 +212,7 @@ void gdisp_lld_init(void) {
  *
  * @notapi
  */
-void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
+void GDISP_LLD(drawpixel)(coord_t x, coord_t y, color_t color) {
 	#if GDISP_NEED_VALIDATION
 		if (x >= GDISP.Width || y >= GDISP.Height) return;
 	#endif
@@ -237,11 +230,11 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	Don't bother coding for obvious similar routines if
 	there is no performance penalty as the emulation software
 	makes a good job of using similar routines.
-		eg. If gdisp_lld_fillarea() is defined there is little
-			point in defining gdisp_lld_clear() unless the
+		eg. If fillarea() is defined there is little
+			point in defining clear() unless the
 			performance bonus is significant.
 	For good performance it is suggested to implement
-		gdisp_lld_fillarea() and gdisp_lld_blitarea().
+		fillarea() and blitarea().
 */
 
 #if GDISP_HARDWARE_CLEARS || defined(__DOXYGEN__)
@@ -253,7 +246,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_clear(color_t color) {
+	void GDISP_LLD(clear)(color_t color) {
 	    unsigned i;
 
 	    lld_lcdSetCursor(0, 0);
@@ -277,7 +270,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawline(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
+	void GDISP_LLD(drawline)(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Need to clip to screen */
 		#endif
@@ -296,7 +289,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillarea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
+	void GDISP_LLD(fillarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			if (cx < 1 || cy < 1 || x >= GDISP.Width || y >= GDISP.Height) return;
 			if (x+cx > GDISP.Width)	cx = GDISP.Width - x;
@@ -326,7 +319,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_blitarea(coord_t x, coord_t y, coord_t cx, coord_t cy, pixel_t *buffer) {
+	void GDISP_LLD(blitarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer) {
 		unsigned i, area;
 
 		#if GDISP_NEED_VALIDATION
@@ -359,7 +352,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(drawcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -380,7 +373,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(fillcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -401,7 +394,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(drawellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -422,7 +415,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(fillellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -445,7 +438,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawchar(coord_t x, coord_t y, char c, font_t font, color_t color) {
+	void GDISP_LLD(drawchar)(coord_t x, coord_t y, char c, font_t font, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -465,7 +458,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillchar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
+	void GDISP_LLD(fillchar)(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -483,7 +476,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	color_t gdisp_lld_getpixelcolor(coord_t x, coord_t y) {
+	color_t GDISP_LLD(getpixelcolor)(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.
@@ -520,7 +513,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_verticalscroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
+	void GDISP_LLD(verticalscroll)(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.
 		 */
@@ -568,7 +561,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 		lld_lcdSetViewPort(x, lines > 0 ? (y+gap) : y, cx, abslines);
 		lld_lcdWriteStreamStart();
 		gap = cx*abslines;
-		for(i = 0; i < gap; i++) lld_lcdWriteData(bgcolor);
+		for(i = 0; i < gap; i++) lld_lcdWriteData(color);
 		lld_lcdWriteStreamStop();
 		lld_lcdResetViewPort();
 	}
@@ -594,7 +587,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_control(int what, void *value) {
+	void GDISP_LLD(control)(unsigned what, void *value) {
 		switch(what) {
 		case GDISP_CONTROL_POWER:
 			if (GDISP.Powermode == (gdisp_powermode_t)value)
@@ -607,7 +600,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 					/* 	Code here */
 					/* You may need this ---
 						if (GDISP.Powermode != powerSleep)
-							gdisp_lld_init();
+							GDISP_LLD(init();
 					*/
 					/* break; */
 				case powerSleep:
@@ -659,5 +652,37 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	}
 #endif
 
+#if (GDISP_NEED_QUERY && GDISP_HARDWARE_QUERY) || defined(__DOXYGEN__)
+/**
+ * @brief   Query a driver value.
+ * @detail	Typecase the result to the type you want.
+ * @note	GDISP_QUERY_WIDTH			- (coord_t)	Gets the width of the screen
+ * 			GDISP_QUERY_HEIGHT			- (coord_t)	Gets the height of the screen
+ * 			GDISP_QUERY_POWER			- (gdisp_powermode_t) Get the current powermode
+ * 			GDISP_QUERY_ORIENTATION		- (gdisp_orientation_t) Get the current screen orientation
+ * 			GDISP_QUERY_BACKLIGHT		- (coord_t) Get the backlight state (0 to 100)
+ * 			GDISP_QUERY_CONTRAST		- (coord_t) Get the contrast (0 to 100).
+ * 			GDISP_QUERY_LLD				- Low level driver control constants start at
+ * 											this value.
+ *
+ * @param[in] what     What to Query
+ *
+ * @notapi
+ */
+void *GDISP_LLD(query)(unsigned what) {
+	switch(what) {
+	case GDISP_QUERY_WIDTH:			return (void *)(unsigned)GDISP.Width;
+	case GDISP_QUERY_HEIGHT:		return (void *)(unsigned)GDISP.Height;
+	case GDISP_QUERY_POWER:			return (void *)(unsigned)GDISP.Powermode;
+	case GDISP_QUERY_ORIENTATION:	return (void *)(unsigned)GDISP.Orientation;
+	case GDISP_QUERY_BACKLIGHT:		return (void *)(unsigned)GDISP.Backlight;
+	case GDISP_QUERY_CONTRAST:		return (void *)(unsigned)GDISP.Contrast;
+	case GDISP_QUERY_LLD+0:
+		/* Code here */
+	default:						return (void *)-1;
+	}
+}
+#endif
+
 #endif /* HAL_USE_GDISP */
 /** @} */
diff --git a/halext/drivers/gdispS6d1121/gdisp_lld_config.h b/halext/drivers/gdispS6d1121/gdisp_lld_config.h
index 9e01b901..57aaf692 100644
--- a/halext/drivers/gdispS6d1121/gdisp_lld_config.h
+++ b/halext/drivers/gdispS6d1121/gdisp_lld_config.h
@@ -29,12 +29,15 @@
 #ifndef _GDISP_LLD_CONFIG_H
 #define _GDISP_LLD_CONFIG_H
 
-#if HAL_USE_GDISP || defined(__DOXYGEN__)
+#if HAL_USE_GDISP
 
 /*===========================================================================*/
 /* Driver hardware support.                                                  */
 /*===========================================================================*/
 
+#define GDISP_DRIVER_NAME				"S6d1121"
+#define GDISP_LLD(x)					gdisp_lld_##x##_S6d1121
+
 #define GDISP_HARDWARE_LINES			FALSE
 #define GDISP_HARDWARE_CLEARS			TRUE
 #define GDISP_HARDWARE_FILLS			TRUE
@@ -45,25 +48,18 @@
 #define GDISP_HARDWARE_ELLIPSEFILLS		FALSE
 #define GDISP_HARDWARE_TEXT				FALSE
 #define GDISP_HARDWARE_TEXTFILLS		FALSE
-#define GDISP_HARDWARE_SCROLL			TRUE
+#define GDISP_HARDWARE_SCROLL			FALSE
 #define GDISP_HARDWARE_PIXELREAD		FALSE
 #define GDISP_HARDWARE_CONTROL			TRUE
+#define GDISP_HARDWARE_QUERY			FALSE
 
 #define GDISP_SOFTWARE_TEXTFILLDRAW		FALSE
 #define GDISP_SOFTWARE_TEXTBLITCOLUMN	FALSE
 
-#define GDISP_PIXELFORMAT_RGB565
+#define GDISP_PIXELFORMAT				GDISP_PIXELFORMAT_RGB565
 #define GDISP_PACKED_PIXELS				FALSE
 #define GDISP_PACKED_LINES				FALSE
 
-/*===========================================================================*/
-/* Extra fields for the GDISPDriver structure                                 */
-/*===========================================================================*/
-
-/*
-#define GDISP_DRIVER_EXT_FIELDS		int abc; int def;
-*/
-
 #endif	/* HAL_USE_GDISP */
 
 #endif	/* _GDISP_LLD_CONFIG_H */
diff --git a/halext/drivers/gdispSsd1289/gdisp_lld.c b/halext/drivers/gdispSsd1289/gdisp_lld.c
index f5c94bf1..7dfb83a7 100644
--- a/halext/drivers/gdispSsd1289/gdisp_lld.c
+++ b/halext/drivers/gdispSsd1289/gdisp_lld.c
@@ -32,27 +32,17 @@
 
 #if HAL_USE_GDISP || defined(__DOXYGEN__)
 
+/* Include the emulation code for things we don't support */
+#include "gdisp_emulation.c"
+
 /*===========================================================================*/
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
-#ifdef UNUSED
-#elif defined(__GNUC__)
-# define UNUSED(x) UNUSED_ ## x __attribute__((unused))
-#elif defined(__LCLINT__)
-# define UNUSED(x) /*@unused@*/ x
-#else
-# define UNUSED(x) x
-#endif
-
 /*===========================================================================*/
 /* Driver exported variables.                                                */
 /*===========================================================================*/
 
-#if !defined(__DOXYGEN__)
-	GDISPDriver GDISP;
-#endif
-
 /*===========================================================================*/
 /* Driver local variables.                                                   */
 /*===========================================================================*/
@@ -82,7 +72,7 @@
  *
  * @notapi
  */
-void gdisp_lld_init(void) {
+bool_t GDISP_LLD(init)(void) {
 	uint16_t	deviceCode;
 
 	#ifdef LCD_USE_FSMC
@@ -153,6 +143,9 @@ void gdisp_lld_init(void) {
 	GDISP.Height = SCREEN_HEIGHT;
 	GDISP.Orientation = portrait;
 	GDISP.Powermode = powerOn;
+	GDISP.Backlight = 100;
+	GDISP.Contrast = 50;
+	return TRUE;
 }
 
 /**
@@ -164,7 +157,7 @@ void gdisp_lld_init(void) {
  *
  * @notapi
  */
-void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
+void GDISP_LLD(drawpixel)(coord_t x, coord_t y, color_t color) {
 	#if GDISP_NEED_VALIDATION
 		if (x >= GDISP.Width || y >= GDISP.Height) return;
 	#endif
@@ -182,11 +175,11 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	Don't bother coding for obvious similar routines if
 	there is no performance penalty as the emulation software
 	makes a good job of using similar routines.
-		eg. If gdisp_lld_fillarea() is defined there is little
-			point in defining gdisp_lld_clear() unless the
+		eg. If gfillarea() is defined there is little
+			point in defining clear() unless the
 			performance bonus is significant.
 	For good performance it is suggested to implement
-		gdisp_lld_fillarea() and gdisp_lld_blitarea().
+		fillarea() and blitarea().
 */
 
 #if GDISP_HARDWARE_CLEARS || defined(__DOXYGEN__)
@@ -198,7 +191,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_clear(color_t color) {
+	void GDISP_LLD(clear)(color_t color) {
 	    unsigned i;
 
 	    lld_lcdSetCursor(0, 0);
@@ -222,7 +215,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawline(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
+	void GDISP_LLD(drawline)(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -238,7 +231,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillarea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
+	void GDISP_LLD(fillarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			if (cx < 1 || cy < 1 || x >= GDISP.Width || y >= GDISP.Height) return;
 			if (x+cx > GDISP.Width)	cx = GDISP.Width - x;
@@ -268,7 +261,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_blitarea(coord_t x, coord_t y, coord_t cx, coord_t cy, pixel_t *buffer) {
+	void GDISP_LLD(blitarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer) {
 		unsigned i, area;
 
 		#if GDISP_NEED_VALIDATION
@@ -301,7 +294,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(drawcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -319,7 +312,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(fillcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -337,7 +330,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(drawellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -355,7 +348,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(fillellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -375,7 +368,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawchar(coord_t x, coord_t y, char c, font_t font, color_t color) {
+	void GDISP_LLD(drawchar)(coord_t x, coord_t y, char c, font_t font, color_t color) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -392,7 +385,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillchar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
+	void GDISP_LLD(fillchar)(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
 		/* NOT IMPLEMENTED */
 	}
 #endif
@@ -407,7 +400,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	color_t gdisp_lld_getpixelcolor(coord_t x, coord_t y) {
+	color_t GDISP_LLD(getpixelcolor)(coord_t x, coord_t y) {
 		color_t color;
 
 		#if GDISP_NEED_VALIDATION
@@ -440,7 +433,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_verticalscroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
+	void GDISP_LLD(verticalscroll)(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
 		static color_t buf[((SCREEN_HEIGHT > SCREEN_WIDTH ) ? SCREEN_HEIGHT : SCREEN_WIDTH)];
 		coord_t row0, row1;
 		unsigned i, gap, abslines;
@@ -511,7 +504,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_control(int what, void *value) {
+	void GDISP_LLD(control)(unsigned what, void *value) {
 		switch(what) {
 		case GDISP_CONTROL_POWER:
 			if (GDISP.Powermode == (gdisp_powermode_t)value)
@@ -526,7 +519,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 			case powerOn:
 				lld_lcdWriteReg(0x0010, 0x0000);	// leave sleep mode
 				if (GDISP.Powermode != powerSleep)
-					gdisp_lld_init();
+					GDISP_LLD(init)();
 				break;
 			case powerSleep:
 				lld_lcdWriteReg(0x0010, 0x0001);	// enter sleep mode
@@ -581,5 +574,37 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	}
 #endif
 
+#if (GDISP_NEED_QUERY && GDISP_HARDWARE_QUERY) || defined(__DOXYGEN__)
+/**
+ * @brief   Query a driver value.
+ * @detail	Typecase the result to the type you want.
+ * @note	GDISP_QUERY_WIDTH			- (coord_t)	Gets the width of the screen
+ * 			GDISP_QUERY_HEIGHT			- (coord_t)	Gets the height of the screen
+ * 			GDISP_QUERY_POWER			- (gdisp_powermode_t) Get the current powermode
+ * 			GDISP_QUERY_ORIENTATION		- (gdisp_orientation_t) Get the current screen orientation
+ * 			GDISP_QUERY_BACKLIGHT		- (coord_t) Get the backlight state (0 to 100)
+ * 			GDISP_QUERY_CONTRAST		- (coord_t) Get the contrast (0 to 100).
+ * 			GDISP_QUERY_LLD				- Low level driver control constants start at
+ * 											this value.
+ *
+ * @param[in] what     What to Query
+ *
+ * @notapi
+ */
+void *GDISP_LLD(query)(unsigned what) {
+	switch(what) {
+	case GDISP_QUERY_WIDTH:			return (void *)(unsigned)GDISP.Width;
+	case GDISP_QUERY_HEIGHT:		return (void *)(unsigned)GDISP.Height;
+	case GDISP_QUERY_POWER:			return (void *)(unsigned)GDISP.Powermode;
+	case GDISP_QUERY_ORIENTATION:	return (void *)(unsigned)GDISP.Orientation;
+	case GDISP_QUERY_BACKLIGHT:		return (void *)(unsigned)GDISP.Backlight;
+	case GDISP_QUERY_CONTRAST:		return (void *)(unsigned)GDISP.Contrast;
+	case GDISP_QUERY_LLD+0:
+		/* Code here */
+	default:						return (void *)-1;
+	}
+}
+#endif
+
 #endif /* HAL_USE_GDISP */
 /** @} */
diff --git a/halext/drivers/gdispSsd1289/gdisp_lld_config.h b/halext/drivers/gdispSsd1289/gdisp_lld_config.h
index 60be3017..123ce875 100644
--- a/halext/drivers/gdispSsd1289/gdisp_lld_config.h
+++ b/halext/drivers/gdispSsd1289/gdisp_lld_config.h
@@ -29,12 +29,15 @@
 #ifndef _GDISP_LLD_CONFIG_H
 #define _GDISP_LLD_CONFIG_H
 
-#if HAL_USE_GDISP || defined(__DOXYGEN__)
+#if HAL_USE_GDISP
 
 /*===========================================================================*/
 /* Driver hardware support.                                                  */
 /*===========================================================================*/
 
+#define GDISP_DRIVER_NAME				"SSD1289"
+#define GDISP_LLD(x)					gdisp_lld_##x##_SSD1289
+
 #define GDISP_HARDWARE_LINES			FALSE
 #define GDISP_HARDWARE_CLEARS			TRUE
 #define GDISP_HARDWARE_FILLS			TRUE
@@ -48,22 +51,15 @@
 #define GDISP_HARDWARE_SCROLL			TRUE
 #define GDISP_HARDWARE_PIXELREAD		TRUE
 #define GDISP_HARDWARE_CONTROL			TRUE
+#define GDISP_HARDWARE_QUERY			FALSE
 
 #define GDISP_SOFTWARE_TEXTFILLDRAW		FALSE
 #define GDISP_SOFTWARE_TEXTBLITCOLUMN	FALSE
 
-#define GDISP_PIXELFORMAT_RGB565
+#define GDISP_PIXELFORMAT				GDISP_PIXELFORMAT_RGB565
 #define GDISP_PACKED_PIXELS				FALSE
 #define GDISP_PACKED_LINES				FALSE
 
-/*===========================================================================*/
-/* Extra fields for the GDISPDriver structure                                 */
-/*===========================================================================*/
-
-/*
-#define GDISP_DRIVER_EXT_FIELDS		int abc; int def;
-*/
-
 #endif	/* HAL_USE_GDISP */
 
 #endif	/* _GDISP_LLD_CONFIG_H */
diff --git a/halext/drivers/gdispTestStub/gdisp_lld.c b/halext/drivers/gdispTestStub/gdisp_lld.c
index 44431e30..fd8410fb 100644
--- a/halext/drivers/gdispTestStub/gdisp_lld.c
+++ b/halext/drivers/gdispTestStub/gdisp_lld.c
@@ -32,27 +32,17 @@
 
 #if HAL_USE_GDISP || defined(__DOXYGEN__)
 
+/* Include the emulation code for things we don't support */
+#include "gdisp_emulation.c"
+
 /*===========================================================================*/
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
-#ifdef UNUSED
-#elif defined(__GNUC__)
-# define UNUSED(x) UNUSED_ ## x __attribute__((unused))
-#elif defined(__LCLINT__)
-# define UNUSED(x) /*@unused@*/ x
-#else
-# define UNUSED(x) x
-#endif
-
 /*===========================================================================*/
 /* Driver exported variables.                                                */
 /*===========================================================================*/
 
-#if !defined(__DOXYGEN__)
-	GDISPDriver GDISP;
-#endif
-
 /*===========================================================================*/
 /* Driver local variables.                                                   */
 /*===========================================================================*/
@@ -80,12 +70,15 @@
  *
  * @notapi
  */
-void gdisp_lld_init(void) {
+bool_t GDISP_LLD(init)(void) {
 	/* Initialise the GDISP structure */
 	GDISP.Width = 128;
 	GDISP.Height = 128;
 	GDISP.Orientation = portrait;
 	GDISP.Powermode = powerOff;
+	GDISP.Backlight = 100;
+	GDISP.Contrast = 50;
+	return TRUE;
 }
 
 /**
@@ -97,7 +90,7 @@ void gdisp_lld_init(void) {
  *
  * @notapi
  */
-void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(color)) {
+void GDISP_LLD(drawpixel)(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(color)) {
 }
 
 /* ---- Optional Routines ---- */
@@ -110,11 +103,11 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	Don't bother coding for obvious similar routines if
 	there is no performance penalty as the emulation software
 	makes a good job of using similar routines.
-		eg. If gdisp_lld_fillarea() is defined there is little
-			point in defining gdisp_lld_clear() unless the
+		eg. If fillarea() is defined there is little
+			point in defining clear() unless the
 			performance bonus is significant.
 	For good performance it is suggested to implement
-		gdisp_lld_fillarea() and gdisp_lld_blitarea().
+		fillarea() and blitarea().
 */
 
 #if GDISP_HARDWARE_CLEARS || defined(__DOXYGEN__)
@@ -126,7 +119,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_clear(color_t UNUSED(color)) {
+	void GDISP_LLD(clear)(color_t UNUSED(color)) {
 	}
 #endif
 
@@ -141,7 +134,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawline(coord_t UNUSED(x0), coord_t UNUSED(y0), coord_t UNUSED(x1), coord_t UNUSED(y1), color_t UNUSED(color)) {
+	void GDISP_LLD(drawline)(coord_t UNUSED(x0), coord_t UNUSED(y0), coord_t UNUSED(x1), coord_t UNUSED(y1), color_t UNUSED(color)) {
 	}
 #endif
 
@@ -156,7 +149,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillarea(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(cx), coord_t UNUSED(cy), color_t UNUSED(color)) {
+	void GDISP_LLD(fillarea)(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(cx), coord_t UNUSED(cy), color_t UNUSED(color)) {
 	}
 #endif
 
@@ -171,7 +164,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_blitarea(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(cx), coord_t UNUSED(cy), pixel_t *UNUSED(buffer)) {
+	void GDISP_LLD(blitarea)(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(cx), coord_t UNUSED(cy), const pixel_t *UNUSED(buffer)) {
 	}
 #endif
 
@@ -189,7 +182,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawcircle(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(radius), color_t UNUSED(color)) {
+	void GDISP_LLD(drawcircle)(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(radius), color_t UNUSED(color)) {
 	}
 #endif
 
@@ -206,7 +199,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillcircle(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(radius), color_t UNUSED(color)) {
+	void GDISP_LLD(fillcircle)(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(radius), color_t UNUSED(color)) {
 	}
 #endif
 
@@ -223,7 +216,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawellipse(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(a), coord_t UNUSED(b), color_t UNUSED(color)) {
+	void GDISP_LLD(drawellipse)(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(a), coord_t UNUSED(b), color_t UNUSED(color)) {
 	}
 #endif
 
@@ -240,7 +233,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillellipse(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(a), coord_t UNUSED(b), color_t UNUSED(color)) {
+	void GDISP_LLD(fillellipse)(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(a), coord_t UNUSED(b), color_t UNUSED(color)) {
 	}
 #endif
 
@@ -255,7 +248,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawchar(coord_t UNUSED(x), coord_t UNUSED(y), char UNUSED(c), font_t UNUSED(font), color_t UNUSED(color)) {
+	void GDISP_LLD(drawchar)(coord_t UNUSED(x), coord_t UNUSED(y), char UNUSED(c), font_t UNUSED(font), color_t UNUSED(color)) {
 	}
 #endif
 
@@ -271,7 +264,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillchar(coord_t UNUSED(x), coord_t UNUSED(y), char UNUSED(c), font_t UNUSED(font), color_t UNUSED(color), color_t UNUSED(bgcolor)) {
+	void GDISP_LLD(fillchar)(coord_t UNUSED(x), coord_t UNUSED(y), char UNUSED(c), font_t UNUSED(font), color_t UNUSED(color), color_t UNUSED(bgcolor)) {
 	}
 #endif
 
@@ -285,7 +278,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	color_t gdisp_lld_getpixelcolor(coord_t UNUSED(x), coord_t UNUSED(y)) {
+	color_t GDISP_LLD(getpixelcolor)(coord_t UNUSED(x), coord_t UNUSED(y)) {
 		return 0;
 	}
 #endif
@@ -304,7 +297,7 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_verticalscroll(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(cx), coord_t UNUSED(cy), int UNUSED(lines), color_t UNUSED(bgcolor)) {
+	void GDISP_LLD(verticalscroll)(coord_t UNUSED(x), coord_t UNUSED(y), coord_t UNUSED(cx), coord_t UNUSED(cy), int UNUSED(lines), color_t UNUSED(bgcolor)) {
 	}
 #endif
 
@@ -328,9 +321,41 @@ void gdisp_lld_drawpixel(coord_t UNUSED(x), coord_t UNUSED(y), color_t UNUSED(co
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_control(int UNUSED(what), void *UNUSED(value)) {
+	void GDISP_LLD(control)(unsigned UNUSED(what), void *UNUSED(value)) {
 	}
 #endif
 
+#if (GDISP_NEED_QUERY && GDISP_HARDWARE_QUERY) || defined(__DOXYGEN__)
+/**
+ * @brief   Query a driver value.
+ * @detail	Typecase the result to the type you want.
+ * @note	GDISP_QUERY_WIDTH			- (coord_t)	Gets the width of the screen
+ * 			GDISP_QUERY_HEIGHT			- (coord_t)	Gets the height of the screen
+ * 			GDISP_QUERY_POWER			- (gdisp_powermode_t) Get the current powermode
+ * 			GDISP_QUERY_ORIENTATION		- (gdisp_orientation_t) Get the current screen orientation
+ * 			GDISP_QUERY_BACKLIGHT		- (coord_t) Get the backlight state (0 to 100)
+ * 			GDISP_QUERY_CONTRAST		- (coord_t) Get the contrast (0 to 100).
+ * 			GDISP_QUERY_LLD				- Low level driver control constants start at
+ * 											this value.
+ *
+ * @param[in] what     What to Query
+ *
+ * @notapi
+ */
+void *GDISP_LLD(query)(unsigned what) {
+	switch(what) {
+	case GDISP_QUERY_WIDTH:			return (void *)(unsigned)GDISP.Width;
+	case GDISP_QUERY_HEIGHT:		return (void *)(unsigned)GDISP.Height;
+	case GDISP_QUERY_POWER:			return (void *)(unsigned)GDISP.Powermode;
+	case GDISP_QUERY_ORIENTATION:	return (void *)(unsigned)GDISP.Orientation;
+	case GDISP_QUERY_BACKLIGHT:		return (void *)(unsigned)GDISP.Backlight;
+	case GDISP_QUERY_CONTRAST:		return (void *)(unsigned)GDISP.Contrast;
+	case GDISP_QUERY_LLD+0:
+		/* Code here */
+	default:						return (void *)-1;
+	}
+}
+#endif
+
 #endif /* HAL_USE_GDISP */
 /** @} */
diff --git a/halext/drivers/gdispTestStub/gdisp_lld_config.h b/halext/drivers/gdispTestStub/gdisp_lld_config.h
index f58ca878..d66e050f 100644
--- a/halext/drivers/gdispTestStub/gdisp_lld_config.h
+++ b/halext/drivers/gdispTestStub/gdisp_lld_config.h
@@ -29,12 +29,15 @@
 #ifndef _GDISP_LLD_CONFIG_H
 #define _GDISP_LLD_CONFIG_H
 
-#if HAL_USE_GDISP || defined(__DOXYGEN__)
+#if HAL_USE_GDISP
 
 /*===========================================================================*/
 /* Driver hardware support.                                                  */
 /*===========================================================================*/
 
+#define GDISP_DRIVER_NAME				"TestStub"
+#define GDISP_LLD(x)					gdisp_lld_##x##_TestStub
+
 #define GDISP_HARDWARE_LINES			FALSE
 #define GDISP_HARDWARE_CLEARS			FALSE
 #define GDISP_HARDWARE_FILLS			FALSE
@@ -45,25 +48,18 @@
 #define GDISP_HARDWARE_ELLIPSEFILLS		FALSE
 #define GDISP_HARDWARE_TEXT				FALSE
 #define GDISP_HARDWARE_TEXTFILLS		FALSE
-#define GDISP_HARDWARE_SCROLL			FALSE
-#define GDISP_HARDWARE_PIXELREAD		FALSE
+#define GDISP_HARDWARE_SCROLL			GDISP_NEED_SCROLL
+#define GDISP_HARDWARE_PIXELREAD		GDISP_NEED_PIXELREAD
 #define GDISP_HARDWARE_CONTROL			FALSE
+#define GDISP_HARDWARE_QUERY			FALSE
 
 #define GDISP_SOFTWARE_TEXTFILLDRAW		FALSE
 #define GDISP_SOFTWARE_TEXTBLITCOLUMN	FALSE
 
-#define GDISP_PIXELFORMAT_RGB565
+#define GDISP_PIXELFORMAT				GDISP_PIXELFORMAT_RGB565
 #define GDISP_PACKED_PIXELS				FALSE
 #define GDISP_PACKED_LINES				FALSE
 
-/*===========================================================================*/
-/* Extra fields for the GDISPDriver structure                                 */
-/*===========================================================================*/
-
-/*
-#define GDISP_DRIVER_EXT_FIELDS		int abc; int def;
-*/
-
 #endif	/* HAL_USE_GDISP */
 
 #endif	/* _GDISP_LLD_CONFIG_H */
diff --git a/halext/drivers/gdispVMT/gdisp_lld.c b/halext/drivers/gdispVMT/gdisp_lld.c
new file mode 100644
index 00000000..1528f470
--- /dev/null
+++ b/halext/drivers/gdispVMT/gdisp_lld.c
@@ -0,0 +1,249 @@
+/*
+    ChibiOS/RT - Copyright (C) 2012
+                 Joel Bodenmann aka Tectu <joel@unormal.org>
+
+    This file is part of ChibiOS-LCD-Driver.
+
+    ChibiOS-LCD-Driver is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS-LCD-Driver is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    gdispVMT/gdisp_lld.c
+ * @brief   GDISP Graphics Driver subsystem low level driver source for VMT.
+ *
+ * @addtogroup GDISP
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+#include "gdisp.h"
+
+#if HAL_USE_GDISP || defined(__DOXYGEN__)
+
+#define GDISP_LLD_NO_STRUCT
+
+/* Include the emulation code for things we don't support */
+#include "gdisp_emulation.c"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+#define GDISP_LLD1(x)	GDISP_VMT_NAME1(gdisp_lld_##x##_)
+#define GDISP_LLD2(x)	GDISP_VMT_NAME2(gdisp_lld_##x##_)
+
+/* Prototypes for lld driver functions */
+bool_t GDISP_LLD1(init)(void);
+void GDISP_LLD1(clear)(color_t color);
+void GDISP_LLD1(drawpixel)(coord_t x, coord_t y, color_t color);
+void GDISP_LLD1(fillarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color);
+void GDISP_LLD1(blitarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer);
+void GDISP_LLD1(drawline)(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color);
+#if GDISP_NEED_CIRCLE
+	void GDISP_LLD1(drawcircle)(coord_t x, coord_t y, coord_t radius, color_t color);
+	void GDISP_LLD1(fillcircle)(coord_t x, coord_t y, coord_t radius, color_t color);
+#endif
+#if GDISP_NEED_ELLIPSE
+	void GDISP_LLD1(drawellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
+	void GDISP_LLD1(fillellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
+#endif
+#if GDISP_NEED_TEXT
+	void GDISP_LLD1(drawchar)(coord_t x, coord_t y, char c, font_t font, color_t color);
+	void GDISP_LLD1(fillchar)(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor);
+#endif
+#if GDISP_NEED_PIXELREAD
+	color_t GDISP_LLD1(getpixelcolor)(coord_t x, coord_t y);
+#endif
+#if GDISP_NEED_SCROLL
+	void GDISP_LLD1(verticalscroll)(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor);
+#endif
+#if GDISP_NEED_CONTROL
+	void GDISP_LLD1(control)(unsigned what, void *value);
+#endif
+#if GDISP_NEED_QUERY
+	void *GDISP_LLD1(query)(unsigned what);
+#endif
+
+bool_t GDISP_LLD2(init)(void);
+void GDISP_LLD2(clear)(color_t color);
+void GDISP_LLD2(drawpixel)(coord_t x, coord_t y, color_t color);
+void GDISP_LLD2(fillarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color);
+void GDISP_LLD2(blitarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer);
+void GDISP_LLD2(drawline)(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color);
+#if GDISP_NEED_CIRCLE
+	void GDISP_LLD2(drawcircle)(coord_t x, coord_t y, coord_t radius, color_t color);
+	void GDISP_LLD2(fillcircle)(coord_t x, coord_t y, coord_t radius, color_t color);
+#endif
+#if GDISP_NEED_ELLIPSE
+	void GDISP_LLD2(drawellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
+	void GDISP_LLD2(fillellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
+#endif
+#if GDISP_NEED_TEXT
+	void GDISP_LLD2(drawchar)(coord_t x, coord_t y, char c, font_t font, color_t color);
+	void GDISP_LLD2(fillchar)(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor);
+#endif
+#if GDISP_NEED_PIXELREAD
+	color_t GDISP_LLD2(getpixelcolor)(coord_t x, coord_t y);
+#endif
+#if GDISP_NEED_SCROLL
+	void GDISP_LLD2(verticalscroll)(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor);
+#endif
+#if GDISP_NEED_CONTROL
+	void GDISP_LLD2(control)(unsigned what, void *value);
+#endif
+#if GDISP_NEED_QUERY
+	void *GDISP_LLD2(query)(unsigned what);
+#endif
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/* Our VMT table variables */
+void GDISP_LLD_VMT(clear)(color_t color);
+void GDISP_LLD_VMT(drawpixel)(coord_t x, coord_t y, color_t color);
+void GDISP_LLD_VMT(fillarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color);
+void GDISP_LLD_VMT(blitarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer);
+void GDISP_LLD_VMT(drawline)(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color);
+
+#if GDISP_NEED_CIRCLE
+void GDISP_LLD_VMT(drawcircle)(coord_t x, coord_t y, coord_t radius, color_t color);
+void GDISP_LLD_VMT(fillcircle)(coord_t x, coord_t y, coord_t radius, color_t color);
+#endif
+
+#if GDISP_NEED_ELLIPSE
+void GDISP_LLD_VMT(drawellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
+void GDISP_LLD_VMT(fillellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
+#endif
+
+/* Text Rendering Functions */
+#if GDISP_NEED_TEXT
+void GDISP_LLD_VMT(drawchar)(coord_t x, coord_t y, char c, font_t font, color_t color);
+void GDISP_LLD_VMT(fillchar)(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor);
+#endif
+
+/* Pixel readback */
+#if GDISP_NEED_PIXELREAD
+color_t GDISP_LLD_VMT(getpixelcolor)(coord_t x, coord_t y);
+#endif
+
+/* Scrolling Function - clears the area scrolled out */
+#if GDISP_NEED_SCROLL
+void GDISP_LLD_VMT(verticalscroll)(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor);
+#endif
+
+/* Set driver specific control */
+#if GDISP_NEED_CONTROL
+void GDISP_LLD_VMT(control)(unsigned what, void *value);
+#endif
+/* Set driver specific control */
+#if GDISP_NEED_QUERY
+void *GDISP_LLD_VMT(query)(unsigned what);
+#endif
+
+
+/*===========================================================================*/
+/* Driver local variables.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver interrupt handlers.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+bool_t	gdisp_lld_init_VMT(void) {
+	if (GDISP_VMT_NAME1(gdisp_lld_init_)()) {
+		gdisp_lld_clear_VMT					= GDISP_VMT_NAME1(gdisp_lld_clear_);
+		gdisp_lld_drawpixel_VMT				= GDISP_VMT_NAME1(gdisp_lld_drawpixel_);
+		gdisp_lld_fillarea_VMT				= GDISP_VMT_NAME1(gdisp_lld_fillarea_);
+		gdisp_lld_blitarea_VMT				= GDISP_VMT_NAME1(gdisp_lld_blitarea_);
+		gdisp_lld_drawline_VMT				= GDISP_VMT_NAME1(gdisp_lld_drawline_);
+		#if GDISP_NEED_CIRCLE
+			gdisp_lld_drawcircle_VMT		= GDISP_VMT_NAME1(gdisp_lld_drawcircle_);
+			gdisp_lld_fillcircle_VMT		= GDISP_VMT_NAME1(gdisp_lld_fillcircle_);
+		#endif
+		#if GDISP_NEED_ELLIPSE
+			gdisp_lld_drawellipse_VMT		= GDISP_VMT_NAME1(gdisp_lld_drawellipse_);
+			gdisp_lld_fillellipse_VMT		= GDISP_VMT_NAME1(gdisp_lld_fillellipse_);
+		#endif
+		#if GDISP_NEED_TEXT
+			gdisp_lld_drawchar_VMT			= GDISP_VMT_NAME1(gdisp_lld_drawchar_);
+			gdisp_lld_fillchar_VMT			= GDISP_VMT_NAME1(gdisp_lld_fillchar_);
+		#endif
+		#if GDISP_NEED_PIXELREAD
+			gdisp_lld_getpixelcolor_VMT		= GDISP_VMT_NAME1(gdisp_lld_pixelread_);
+		#endif
+		#if GDISP_NEED_SCROLL
+			gdisp_lld_verticalscroll_VMT	= GDISP_VMT_NAME1(gdisp_lld_scroll_);
+		#endif
+		#if GDISP_NEED_CONTROL
+			gdisp_lld_control_VMT			= GDISP_VMT_NAME1(gdisp_lld_control_);
+		#endif
+		#if GDISP_NEED_QUERY
+			gdisp_lld_query_VMT				= GDISP_VMT_NAME1(gdisp_lld_query_);
+		#endif
+
+		return TRUE;
+	}
+
+	if (GDISP_VMT_NAME2(gdisp_lld_init_)()) {
+		gdisp_lld_clear_VMT					= GDISP_VMT_NAME2(gdisp_lld_clear_);
+		gdisp_lld_drawpixel_VMT				= GDISP_VMT_NAME2(gdisp_lld_drawpixel_);
+		gdisp_lld_fillarea_VMT				= GDISP_VMT_NAME2(gdisp_lld_fillarea_);
+		gdisp_lld_blitarea_VMT				= GDISP_VMT_NAME2(gdisp_lld_blitarea_);
+		gdisp_lld_drawline_VMT				= GDISP_VMT_NAME2(gdisp_lld_drawline_);
+		#if GDISP_NEED_CIRCLE
+			gdisp_lld_drawcircle_VMT		= GDISP_VMT_NAME2(gdisp_lld_drawcircle_);
+			gdisp_lld_fillcircle_VMT		= GDISP_VMT_NAME2(gdisp_lld_fillcircle_);
+		#endif
+		#if GDISP_NEED_ELLIPSE
+			gdisp_lld_drawellipse_VMT		= GDISP_VMT_NAME2(gdisp_lld_drawellipse_);
+			gdisp_lld_fillellipse_VMT		= GDISP_VMT_NAME2(gdisp_lld_fillellipse_);
+		#endif
+		#if GDISP_NEED_TEXT
+			gdisp_lld_drawchar_VMT			= GDISP_VMT_NAME2(gdisp_lld_drawchar_);
+			gdisp_lld_fillchar_VMT			= GDISP_VMT_NAME2(gdisp_lld_fillchar_);
+		#endif
+		#if GDISP_NEED_PIXELREAD
+			gdisp_lld_getpixelcolor_VMT		= GDISP_VMT_NAME2(gdisp_lld_pixelread_);
+		#endif
+		#if GDISP_NEED_SCROLL
+			gdisp_lld_verticalscroll_VMT	= GDISP_VMT_NAME2(gdisp_lld_scroll_);
+		#endif
+		#if GDISP_NEED_CONTROL
+			gdisp_lld_control_VMT			= GDISP_VMT_NAME2(gdisp_lld_control_);
+		#endif
+		#if GDISP_NEED_QUERY
+			gdisp_lld_query_VMT				= GDISP_VMT_NAME2(gdisp_lld_query_);
+		#endif
+
+		return TRUE;
+	}
+	return FALSE;
+}
+
+#endif /* HAL_USE_GDISP */
+/** @} */
diff --git a/halext/drivers/gdispVMT/gdisp_lld.mk b/halext/drivers/gdispVMT/gdisp_lld.mk
new file mode 100644
index 00000000..ab935370
--- /dev/null
+++ b/halext/drivers/gdispVMT/gdisp_lld.mk
@@ -0,0 +1,7 @@
+# List the required driver.
+HALSRC += ${CHIBIOS}/os/halext/drivers/gdispVMT/gdisp_lld.c \
+			${CHIBIOS}/os/halext/drivers/gdispVMT/gdisp_lld_driver1.c \
+			${CHIBIOS}/os/halext/drivers/gdispVMT/gdisp_lld_driver2.c
+
+# Required include directories
+HALINC += ${CHIBIOS}/os/halext/drivers/gdispVMT
diff --git a/halext/drivers/gdispVMT/gdisp_lld_config.h b/halext/drivers/gdispVMT/gdisp_lld_config.h
new file mode 100644
index 00000000..6c6f7c80
--- /dev/null
+++ b/halext/drivers/gdispVMT/gdisp_lld_config.h
@@ -0,0 +1,67 @@
+/*
+    ChibiOS/RT - Copyright (C) 2012
+                 Joel Bodenmann aka Tectu <joel@unormal.org>
+
+    This file is part of ChibiOS-LCD-Driver.
+
+    ChibiOS-LCD-Driver is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS-LCD-Driver is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    gdispVMT/gdisp_lld_config.h
+ * @brief   GDISP Graphic Driver subsystem low level driver header template.
+ *
+ * @addtogroup GDISP
+ * @{
+ */
+
+#ifndef _GDISP_LLD_CONFIG_H
+#define _GDISP_LLD_CONFIG_H
+
+#if HAL_USE_GDISP
+
+/*===========================================================================*/
+/* Driver hardware support.                                                  */
+/*===========================================================================*/
+
+#define GDISP_DRIVER_NAME				"VMT"
+#define GDISP_LLD(x)					gdisp_lld_##x##_VMT
+#define GDISP_LLD_VMT(x)				(*GDISP_LLD(x))
+
+#define GDISP_HARDWARE_LINES			TRUE
+#define GDISP_HARDWARE_CLEARS			TRUE
+#define GDISP_HARDWARE_FILLS			TRUE
+#define GDISP_HARDWARE_BITFILLS			TRUE
+#define GDISP_HARDWARE_CIRCLES			TRUE
+#define GDISP_HARDWARE_CIRCLEFILLS		TRUE
+#define GDISP_HARDWARE_ELLIPSES			TRUE
+#define GDISP_HARDWARE_ELLIPSEFILLS		TRUE
+#define GDISP_HARDWARE_TEXT				TRUE
+#define GDISP_HARDWARE_TEXTFILLS		TRUE
+#define GDISP_HARDWARE_SCROLL			TRUE
+#define GDISP_HARDWARE_PIXELREAD		TRUE
+#define GDISP_HARDWARE_CONTROL			TRUE
+#define GDISP_HARDWARE_QUERY			TRUE
+
+#define GDISP_SOFTWARE_TEXTFILLDRAW		FALSE
+#define GDISP_SOFTWARE_TEXTBLITCOLUMN	FALSE
+
+#define GDISP_PIXELFORMAT				GDISP_PIXELFORMAT_RGB565
+#define GDISP_PACKED_PIXELS				FALSE
+#define GDISP_PACKED_LINES				FALSE
+
+#endif	/* HAL_USE_GDISP */
+
+#endif	/* _GDISP_LLD_CONFIG_H */
+/** @} */
diff --git a/halext/drivers/gdispVMT/gdisp_lld_driver1.c b/halext/drivers/gdispVMT/gdisp_lld_driver1.c
new file mode 100644
index 00000000..f088623e
--- /dev/null
+++ b/halext/drivers/gdispVMT/gdisp_lld_driver1.c
@@ -0,0 +1,51 @@
+/*
+    ChibiOS/RT - Copyright (C) 2012
+                 Joel Bodenmann aka Tectu <joel@unormal.org>
+
+    This file is part of ChibiOS-LCD-Driver.
+
+    ChibiOS-LCD-Driver is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS-LCD-Driver is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    gdispVMT/gdisp_lld.c
+ * @brief   GDISP Graphics Driver subsystem low level driver source for VMT.
+ *
+ * @addtogroup GDISP
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#if HAL_USE_GDISP || defined(__DOXYGEN__)
+
+#define CONFIGFILE()	<../GDISP_VMT_NAME1(gdisp)/gdisp_lld_config.h>
+#define DRIVERFILE()	<../GDISP_VMT_NAME1(gdisp)/gdisp_lld.c>
+
+/* We don't need these in our VMT referenced driver */
+#undef GDISP_NEED_MSGAPI
+#define GDISP_NEED_MSGAPI	FALSE
+
+/* Include the specific config file we want */
+#include CONFIGFILE()
+
+/* Bring in our API */
+#include "gdisp.h"
+
+/* Add the low level driver */
+#include DRIVERFILE()
+
+#endif /* HAL_USE_GDISP */
+/** @} */
diff --git a/halext/drivers/gdispVMT/gdisp_lld_driver2.c b/halext/drivers/gdispVMT/gdisp_lld_driver2.c
new file mode 100644
index 00000000..325a53e6
--- /dev/null
+++ b/halext/drivers/gdispVMT/gdisp_lld_driver2.c
@@ -0,0 +1,51 @@
+/*
+    ChibiOS/RT - Copyright (C) 2012
+                 Joel Bodenmann aka Tectu <joel@unormal.org>
+
+    This file is part of ChibiOS-LCD-Driver.
+
+    ChibiOS-LCD-Driver is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS-LCD-Driver is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    gdispVMT/gdisp_lld.c
+ * @brief   GDISP Graphics Driver subsystem low level driver source for VMT.
+ *
+ * @addtogroup GDISP
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#if HAL_USE_GDISP || defined(__DOXYGEN__)
+
+#define CONFIGFILE()	<../GDISP_VMT_NAME2(gdisp)/gdisp_lld_config.h>
+#define DRIVERFILE()	<../GDISP_VMT_NAME2(gdisp)/gdisp_lld.c>
+
+/* We don't need these in our VMT referenced driver */
+#undef GDISP_NEED_MSGAPI
+#define GDISP_NEED_MSGAPI	FALSE
+
+/* Include the specific config file we want */
+#include CONFIGFILE()
+
+/* Bring in our API */
+#include "gdisp.h"
+
+/* Add the low level driver */
+#include DRIVERFILE()
+
+#endif /* HAL_USE_GDISP */
+/** @} */
diff --git a/halext/drivers/gdispVMT/readme.txt b/halext/drivers/gdispVMT/readme.txt
new file mode 100644
index 00000000..91d3f58c
--- /dev/null
+++ b/halext/drivers/gdispVMT/readme.txt
@@ -0,0 +1,23 @@
+This driver enables you to have two underlying drivers handling different hardware.
+A choice is made at run-time of which driver to call based on which driver succeeds
+to initialise first (init returns TRUE).
+
+To use this driver:
+
+1. Add in your halconf.h:
+	a) #define HAL_USE_GDISP	TRUE
+	b) Any optional high level driver defines (see gdisp.h) eg: GDISP_NEED_MULTITHREAD
+	c) Define these:
+		#define GDISP_VMT_NAME1(x)	x##YourDriver1
+		#define GDISP_VMT_NAME2(x)	x##YourDriver2
+		Note YourDriver1 & 2 are the basenames of the directories containing the driver.
+		Note that both drivers must be the same pixel format which is
+			GDISP_PIXELFORMAT_RGB565 by default. Alter gdispVMT/gdisp_lld_config.h if your
+			pixel format is different on both drivers.
+	d) Any driver specific defines. If both drivers use the same defines then they must
+		accept the same values for the define.
+
+2. To your makefile add the following lines:
+	include $(CHIBIOS)/os/halext/halext.mk
+	include $(CHIBIOS)/os/halext/drivers/gdispVMT/gdisp_lld.mk
+	
\ No newline at end of file
diff --git a/halext/halext.mk b/halext/halext.mk
index 80ddf666..045d864f 100644
--- a/halext/halext.mk
+++ b/halext/halext.mk
@@ -3,7 +3,6 @@
 
 HALSRC +=	$(LCDLIB)/halext/src/gdisp.c \
 			$(LCDLIB)/halext/src/gdisp_fonts.c \
-			$(LCDLIB)/halext/src/gdisp_emulation.c \
 			$(LCDLIB)/halext/src/touchpad.c
 
 # Required include directories
diff --git a/halext/include/gdisp.h b/halext/include/gdisp.h
index ddb12137..4963043b 100644
--- a/halext/include/gdisp.h
+++ b/halext/include/gdisp.h
@@ -67,80 +67,42 @@
  * @name    GDISP more complex functionality to be compiled
  * @{
  */
-	/**
-	 * @brief   Should all operations be clipped to the screen and colors validated.
-	 * @details	Defaults to TRUE.
-	 * @note    If this is FALSE, any operations that extend beyond the
-	 *          edge of the screen will have undefined results. Any
-	 *			out-of-range colors will produce undefined results.
-	 * @note	If defined then all low level and high level driver routines
-	 *			must check the validity of inputs and do something sensible
-	 *			if they are out of range. It doesn't have to be efficient,
-	 *			just valid.
-	 */
-	#ifndef GDISP_NEED_VALIDATION
-		#define GDISP_NEED_VALIDATION	TRUE
-	#endif
-
-	/**
-	 * @brief   Are circle functions needed.
-	 * @details	Defaults to TRUE
-	 */
-	#ifndef GDISP_NEED_CIRCLE
-		#define GDISP_NEED_CIRCLE		TRUE
-	#endif
-
-	/**
-	 * @brief   Are ellipse functions needed.
-	 * @details	Defaults to TRUE
-	 */
-	#ifndef GDISP_NEED_ELLIPSE
-		#define GDISP_NEED_ELLIPSE		TRUE
-	#endif
-
-	/**
-	 * @brief   Are text functions needed.
-	 * @details	Defaults to TRUE
-	 */
-	#ifndef GDISP_NEED_TEXT
-		#define GDISP_NEED_TEXT			TRUE
-	#endif
-
-	/**
-	 * @brief   Is scrolling needed.
-	 * @details	Defaults to FALSE
-	 */
-	#ifndef GDISP_NEED_SCROLL
-		#define GDISP_NEED_SCROLL		FALSE
-	#endif
-
-	/**
-	 * @brief   Is the capability to read pixels back needed.
-	 * @details	Defaults to FALSE
-	 */
-	#ifndef GDISP_NEED_PIXELREAD
-		#define GDISP_NEED_PIXELREAD	FALSE
-	#endif
-
-	/**
-	 * @brief   Control some aspect of the drivers operation.
-	 * @details	Defaults to FALSE
-	 */
-	#ifndef GDISP_NEED_CONTROL
-		#define GDISP_NEED_CONTROL		FALSE
-	#endif
-
 	/**
 	 * @brief   Do the drawing functions need to be thread-safe.
 	 * @details	Defaults to FALSE
-	 * @note	Turning this on adds two context switches per transaction
-	 *			so it can significantly slow graphics drawing.
+	 * @note	Both GDISP_NEED_MULTITHREAD and GDISP_NEED_ASYNC make
+	 * 			the gdisp API thread-safe.
+	 * @note	This is more efficient than GDISP_NEED_ASYNC as it only
+	 * 			requires a context switch if something else is already
+	 * 			drawing.
 	 */
 	#ifndef GDISP_NEED_MULTITHREAD
 		#define GDISP_NEED_MULTITHREAD	FALSE
 	#endif
+
+	/**
+	 * @brief   Use asynchronous calls (multi-thread safe).
+	 * @details	Defaults to FALSE
+	 * @note	Both GDISP_NEED_MULTITHREAD and GDISP_NEED_ASYNC make
+	 * 			the gdisp API thread-safe.
+	 * @note	Turning this on adds two context switches per transaction
+	 *			so it can significantly slow graphics drawing.
+	 */
+	#ifndef GDISP_NEED_ASYNC
+		#define GDISP_NEED_ASYNC	FALSE
+	#endif
 /** @} */
 
+#if GDISP_NEED_MULTITHREAD && GDISP_NEED_ASYNC
+	#error "GDISP: Only one of GDISP_NEED_MULTITHREAD and GDISP_NEED_ASYNC should be defined."
+#endif
+
+#if GDISP_NEED_ASYNC
+	/* Messaging API is required for Async Multi-Thread */
+	#undef GDISP_NEED_MSGAPI
+	#define GDISP_NEED_MSGAPI	TRUE
+#endif
+
 /*===========================================================================*/
 /* Low Level Driver details and error checks.                                */
 /*===========================================================================*/
@@ -190,17 +152,18 @@ extern const struct font fontLargeNumbersNarrow;
 extern "C" {
 #endif
 
-#if GDISP_NEED_MULTITHREAD
+#if GDISP_NEED_MULTITHREAD || GDISP_NEED_ASYNC
 
 	/* Base Functions */
-	void gdispInit(GDISPDriver *gdisp);
+	bool_t gdispInit(GDISPDriver *gdisp);
+	bool_t gdispIsBusy(void);
 
 	/* Drawing Functions */
 	void gdispClear(color_t color);
 	void gdispDrawPixel(coord_t x, coord_t y, color_t color);
 	void gdispDrawLine(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color);
 	void gdispFillArea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color);
-	void gdispBlitArea(coord_t x, coord_t y, coord_t cx, coord_t cy, pixel_t *buffer);
+	void gdispBlitArea(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer);
 
 	/* Circle Functions */
 	#if GDISP_NEED_CIRCLE
@@ -218,7 +181,6 @@ extern "C" {
 	#if GDISP_NEED_TEXT
 	void gdispDrawChar(coord_t x, coord_t y, char c, font_t font, color_t color);
 	void gdispFillChar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor);
-	void gdispDrawString(coord_t x, coord_t y, const char *str, font_t font, color_t color);
 	#endif
 	
 	/* Read a pixel Function */
@@ -233,27 +195,34 @@ extern "C" {
 
 	/* Set driver specific control */
 	#if GDISP_NEED_CONTROL
-	void gdispControl(int what, void *value);
+	void gdispControl(unsigned what, void *value);
+	#endif
+
+	/* Query driver specific data */
+	#if GDISP_NEED_CONTROL
+	void *gdispQuery(unsigned what);
 	#endif
 
 #else
 
 	/* The same as above but use the low level driver directly if no multi-thread support is needed */
-	#define gdispInit(gdisp)									gdisp_lld_init()
-	#define gdispClear(color)									gdisp_lld_clear(color)
-	#define gdispDrawPixel(x, y, color)							gdisp_lld_drawpixel(x, y, color)
-	#define gdispDrawLine(x0, y0, x1, y1, color)				gdisp_lld_drawline(x0, y0, x1, y1, color)
-	#define gdispFillArea(x, y, cx, cy, color)					gdisp_lld_fillarea(x, y, cx, cy, color)
-	#define gdispBlitArea(x, y, cx, cy, buffer)					gdisp_lld_blitarea(x, y, cx, cy, buffer)
-	#define gdispDrawCircle(x, y, radius, color)				gdisp_lld_drawcircle(x, y, radius, color)
-	#define gdispFillCircle(x, y, radius, color)				gdisp_lld_fillcircle(x, y, radius, color)
-	#define gdispDrawEllipse(x, y, a, b, color)					gdisp_lld_drawellipse(x, y, a, b, color)
-	#define gdispFillEllipse(x, y, a, b, color)					gdisp_lld_fillellipse(x, y, a, b, color)
-	#define gdispDrawChar(x, y, c, font, color)					gdisp_lld_drawchar(x, y, c, font, color)
-	#define gdispFillChar(x, y, c, font, color, bgcolor)		gdisp_lld_fillchar(x, y, c, font, color, bgcolor)
-	#define gdispGetPixelColor(x, y)							gdisp_lld_getpixelcolor(x, y)
-	#define gdispVerticalScroll(x, y, cx, cy, lines, bgcolor)	gdisp_lld_verticalscroll(x, y, cx, cy, lines, bgcolor)
-	#define gdispControl(what, value)							gdisp_lld_control(what, value)
+	#define gdispInit(gdisp)									GDISP_LLD(init)()
+	#define gdispIsBusy()										FALSE
+	#define gdispClear(color)									GDISP_LLD(clear)(color)
+	#define gdispDrawPixel(x, y, color)							GDISP_LLD(drawpixel)(x, y, color)
+	#define gdispDrawLine(x0, y0, x1, y1, color)				GDISP_LLD(drawline)(x0, y0, x1, y1, color)
+	#define gdispFillArea(x, y, cx, cy, color)					GDISP_LLD(fillarea)(x, y, cx, cy, color)
+	#define gdispBlitArea(x, y, cx, cy, buffer)					GDISP_LLD(blitarea)(x, y, cx, cy, buffer)
+	#define gdispDrawCircle(x, y, radius, color)				GDISP_LLD(drawcircle)(x, y, radius, color)
+	#define gdispFillCircle(x, y, radius, color)				GDISP_LLD(fillcircle)(x, y, radius, color)
+	#define gdispDrawEllipse(x, y, a, b, color)					GDISP_LLD(drawellipse)(x, y, a, b, color)
+	#define gdispFillEllipse(x, y, a, b, color)					GDISP_LLD(fillellipse)(x, y, a, b, color)
+	#define gdispDrawChar(x, y, c, font, color)					GDISP_LLD(drawchar)(x, y, c, font, color)
+	#define gdispFillChar(x, y, c, font, color, bgcolor)		GDISP_LLD(fillchar)(x, y, c, font, color, bgcolor)
+	#define gdispGetPixelColor(x, y)							GDISP_LLD(getpixelcolor)(x, y)
+	#define gdispVerticalScroll(x, y, cx, cy, lines, bgcolor)	GDISP_LLD(verticalscroll)(x, y, cx, cy, lines, bgcolor)
+	#define gdispControl(what, value)							GDISP_LLD(control)(what, value)
+	#define gdispQuery(what)									GDISP_LLD(query)(what)
 
 #endif
 
diff --git a/halext/src/gdisp_emulation.c b/halext/include/gdisp_emulation.c
similarity index 55%
rename from halext/src/gdisp_emulation.c
rename to halext/include/gdisp_emulation.c
index 26562e92..cb3773a7 100644
--- a/halext/src/gdisp_emulation.c
+++ b/halext/include/gdisp_emulation.c
@@ -28,9 +28,8 @@
 	we call a real low level driver routine and if validation is
 	required - it will do it.
 */
-#include "ch.h"
-#include "hal.h"
-#include "gdisp.h"
+#ifndef GDISP_EMULATION_C
+#define GDISP_EMULATION_C
 
 #if HAL_USE_GDISP || defined(__DOXYGEN__)
 
@@ -43,24 +42,25 @@
 # define UNUSED(x) x
 #endif
 
-#if !GDISP_HARDWARE_POWERCONTROL
-	void gdisp_lld_setpowermode(gdisp_powermode_t UNUSED(powerMode)) {
-	}
-#endif
-
-#if !GDISP_HARDWARE_ORIENTATION
-	void gdisp_lld_setorientation(gdisp_orientation_t UNUSED(newOrientation)) {
-	}
+#ifndef GDISP_LLD_NO_STRUCT
+	static struct GDISPDriver {
+		coord_t				Width;
+		coord_t				Height;
+		gdisp_orientation_t	Orientation;
+		gdisp_powermode_t	Powermode;
+		coord_t				Backlight;
+		coord_t				Contrast;
+		} GDISP;
 #endif
 
 #if !GDISP_HARDWARE_CLEARS 
-	void gdisp_lld_clear(color_t color) {
-		gdisp_lld_fillarea(0, 0, GDISP.Width, GDISP.Height, color);
+	void GDISP_LLD(clear)(color_t color) {
+		GDISP_LLD(fillarea)(0, 0, GDISP.Width, GDISP.Height, color);
 	}
 #endif
 
 #if !GDISP_HARDWARE_LINES 
-	void gdisp_lld_drawline(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
+	void GDISP_LLD(drawline)(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
 		int16_t dy, dx;
 		int16_t addx, addy;
 		int16_t P, diff, i;
@@ -69,16 +69,16 @@
 		// speed improvement if vertical or horizontal
 		if (x0 == x1) {
 			if (y1 > y0)
-				gdisp_lld_fillarea(x0, y0, 1, y1-y0+1, color);
+				GDISP_LLD(fillarea)(x0, y0, 1, y1-y0+1, color);
 			else
-				gdisp_lld_fillarea(x0, y1, 1, y0-y1+1, color);
+				GDISP_LLD(fillarea)(x0, y1, 1, y0-y1+1, color);
 			return;
 		}
 		if (y0 == y1) {
 			if (x1 > x0)
-				gdisp_lld_fillarea(x0, y0, x1-x0+1, 1, color);
+				GDISP_LLD(fillarea)(x0, y0, x1-x0+1, 1, color);
 			else
-				gdisp_lld_fillarea(x0, y1, x0-x1+1, 1, color);
+				GDISP_LLD(fillarea)(x0, y1, x0-x1+1, 1, color);
 			return;
 		}
 		#endif
@@ -104,7 +104,7 @@
 			diff = P - dx;
 
 			for(i=0; i<=dx; ++i) {
-				gdisp_lld_drawpixel(x0, y0, color);
+				GDISP_LLD(drawpixel)(x0, y0, color);
 				if (P < 0) {
 					P  += dy;
 					x0 += addx;
@@ -120,7 +120,7 @@
 			diff = P - dy;
 
 			for(i=0; i<=dy; ++i) {
-				gdisp_lld_drawpixel(x0, y0, color);
+				GDISP_LLD(drawpixel)(x0, y0, color);
 				if (P < 0) {
 					P  += dx;
 					y0 += addy;
@@ -135,16 +135,16 @@
 #endif
 
 #if !GDISP_HARDWARE_FILLS
-	void gdisp_lld_fillarea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
+	void GDISP_LLD(fillarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
 		#if GDISP_HARDWARE_SCROLL
-			gdisp_lld_verticalscroll(x, y, cx, cy, cy, color);
+			GDISP_LLD(verticalscroll)(x, y, cx, cy, cy, color);
 		#elif GDISP_HARDWARE_LINES
 			coord_t x1, y1;
 			
 			x1 = x + cx - 1;
 			y1 = y + cy;
 			for(; y < y1; y++)
-				gdisp_lld_drawline(x, y, x1, y, color);
+				GDISP_LLD(drawline)(x, y, x1, y, color);
 		#else
 			coord_t x0, x1, y1;
 			
@@ -153,13 +153,13 @@
 			y1 = y + cy;
 			for(; y < y1; y++)
 				for(x = x0; x < x1; x++)
-					gdisp_lld_drawpixel(x, y, color);
+					GDISP_LLD(drawpixel)(x, y, color);
 		#endif
 	}
 #endif
 
 #if !GDISP_HARDWARE_BITFILLS
-	void gdisp_lld_blitarea(coord_t x, coord_t y, coord_t cx, coord_t cy, pixel_t *buffer) {
+	void GDISP_LLD(blitarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer) {
 			coord_t x0, x1, y1;
 			
 			x0 = x;
@@ -167,12 +167,12 @@
 			y1 = y + cy;
 			for(; y < y1; y++)
 				for(x = x0; x < x1; x++)
-					gdisp_lld_drawpixel(x, y, *buffer++);
+					GDISP_LLD(drawpixel)(x, y, *buffer++);
 	}
 #endif
 
 #if GDISP_NEED_CIRCLE && !GDISP_HARDWARE_CIRCLES
-	void gdisp_lld_drawcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(drawcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		int16_t a, b, P;
 
 		a = 0;
@@ -180,14 +180,14 @@
 		P = 1 - radius;
 
 		do {
-			gdisp_lld_drawpixel(a+x, b+y, color);
-			gdisp_lld_drawpixel(b+x, a+y, color);
-			gdisp_lld_drawpixel(x-a, b+y, color);
-			gdisp_lld_drawpixel(x-b, a+y, color);
-			gdisp_lld_drawpixel(b+x, y-a, color);
-			gdisp_lld_drawpixel(a+x, y-b, color);
-			gdisp_lld_drawpixel(x-a, y-b, color);
-			gdisp_lld_drawpixel(x-b, y-a, color);
+			GDISP_LLD(drawpixel)(a+x, b+y, color);
+			GDISP_LLD(drawpixel)(b+x, a+y, color);
+			GDISP_LLD(drawpixel)(x-a, b+y, color);
+			GDISP_LLD(drawpixel)(x-b, a+y, color);
+			GDISP_LLD(drawpixel)(b+x, y-a, color);
+			GDISP_LLD(drawpixel)(a+x, y-b, color);
+			GDISP_LLD(drawpixel)(x-a, y-b, color);
+			GDISP_LLD(drawpixel)(x-b, y-a, color);
 			if (P < 0)
 				P += 3 + 2*a++;
 			else
@@ -197,7 +197,7 @@
 #endif
 
 #if GDISP_NEED_CIRCLE && !GDISP_HARDWARE_CIRCLEFILLS
-	void gdisp_lld_fillcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(fillcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		int16_t a, b, P;
 		
 		a = 0;
@@ -205,10 +205,10 @@
 		P = 1 - radius;
 
 		do {
-			gdisp_lld_drawline(x-a, y+b, x+a, y+b, color);
-			gdisp_lld_drawline(x-a, y-b, x+a, y-b, color);
-			gdisp_lld_drawline(x-b, y+a, x+b, y+a, color);
-			gdisp_lld_drawline(x-b, y-a, x+b, y-a, color);
+			GDISP_LLD(drawline)(x-a, y+b, x+a, y+b, color);
+			GDISP_LLD(drawline)(x-a, y-b, x+a, y-b, color);
+			GDISP_LLD(drawline)(x-b, y+a, x+b, y+a, color);
+			GDISP_LLD(drawline)(x-b, y-a, x+b, y-a, color);
 			if (P < 0)
 				P += 3 + 2*a++;
 			else
@@ -218,16 +218,16 @@
 #endif
 
 #if GDISP_NEED_ELLIPSE && !GDISP_HARDWARE_ELLIPSES
-	void gdisp_lld_drawellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(drawellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		int  dx = 0, dy = b; /* im I. Quadranten von links oben nach rechts unten */
 		long a2 = a*a, b2 = b*b;
 		long err = b2-(2*b-1)*a2, e2; /* Fehler im 1. Schritt */
 
 		do {
-			gdisp_lld_drawpixel(x+dx, y+dy, color); /* I. Quadrant */
-			gdisp_lld_drawpixel(x-dx, y+dy, color); /* II. Quadrant */
-			gdisp_lld_drawpixel(x-dx, y-dy, color); /* III. Quadrant */
-			gdisp_lld_drawpixel(x+dx, y-dy, color); /* IV. Quadrant */
+			GDISP_LLD(drawpixel)(x+dx, y+dy, color); /* I. Quadrant */
+			GDISP_LLD(drawpixel)(x-dx, y+dy, color); /* II. Quadrant */
+			GDISP_LLD(drawpixel)(x-dx, y-dy, color); /* III. Quadrant */
+			GDISP_LLD(drawpixel)(x+dx, y-dy, color); /* IV. Quadrant */
 
 			e2 = 2*err;
 			if(e2 <  (2*dx+1)*b2) {
@@ -241,21 +241,21 @@
 		} while(dy >= 0); 
 
 		while(dx++ < a) { /* fehlerhafter Abbruch bei flachen Ellipsen (b=1) */
-			gdisp_lld_drawpixel(x+dx, y, color); /* -> Spitze der Ellipse vollenden */
-			gdisp_lld_drawpixel(x-dx, y, color);
+			GDISP_LLD(drawpixel)(x+dx, y, color); /* -> Spitze der Ellipse vollenden */
+			GDISP_LLD(drawpixel)(x-dx, y, color);
 	   }   
 	}
 #endif
 
 #if GDISP_NEED_ELLIPSE && !GDISP_HARDWARE_ELLIPSEFILLS
-	void gdisp_lld_fillellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(fillellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		int  dx = 0, dy = b; /* im I. Quadranten von links oben nach rechts unten */
 		long a2 = a*a, b2 = b*b;
 		long err = b2-(2*b-1)*a2, e2; /* Fehler im 1. Schritt */
 
 		do {
-			gdisp_lld_drawline(x-dx,y+dy,x+dx,y+dy, color);
-			gdisp_lld_drawline(x-dx,y-dy,x+dx,y-dy, color);
+			GDISP_LLD(drawline)(x-dx,y+dy,x+dx,y+dy, color);
+			GDISP_LLD(drawline)(x-dx,y-dy,x+dx,y-dy, color);
 
 			e2 = 2*err;
 			if(e2 <  (2*dx+1)*b2) {
@@ -269,8 +269,8 @@
 		} while(dy >= 0); 
 
 		while(dx++ < a) { /* fehlerhafter Abbruch bei flachen Ellipsen (b=1) */
-			gdisp_lld_drawpixel(x+dx, y, color); /* -> Spitze der Ellipse vollenden */
-			gdisp_lld_drawpixel(x-dx, y, color);
+			GDISP_LLD(drawpixel)(x+dx, y, color); /* -> Spitze der Ellipse vollenden */
+			GDISP_LLD(drawpixel)(x-dx, y, color);
 	   }   
 	}
 #endif
@@ -280,7 +280,7 @@
 #endif
 
 #if GDISP_NEED_TEXT && !GDISP_HARDWARE_TEXT
-	void gdisp_lld_drawchar(coord_t x, coord_t y, char c, font_t font, color_t color) {
+	void GDISP_LLD(drawchar)(coord_t x, coord_t y, char c, font_t font, color_t color) {
 		const fontcolumn_t	*ptr;
 		fontcolumn_t		column;
 		coord_t				width, height, xscale, yscale;
@@ -307,7 +307,7 @@
 				if (column & 0x01) {
 					for(xs=0; xs < xscale; xs++)
 						for(ys=0; ys < yscale; ys++)
-							gdisp_lld_drawpixel(x+i+xs, y+j+ys, color);
+							GDISP_LLD(drawpixel)(x+i+xs, y+j+ys, color);
 				}
 			}
 		}
@@ -315,7 +315,7 @@
 #endif
 
 #if GDISP_NEED_TEXT && !GDISP_HARDWARE_TEXTFILLS
-	void gdisp_lld_fillchar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
+	void GDISP_LLD(fillchar)(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
 		coord_t			width, height;
 		coord_t			xscale, yscale;
 		
@@ -332,10 +332,10 @@
 		#if GDISP_HARDWARE_TEXT || GDISP_SOFTWARE_TEXTFILLDRAW
 			
 			/* Fill the area */
-			gdisp_lld_fillarea(x, y, width, height, bgcolor);
+			GDISP_LLD(fillarea)(x, y, width, height, bgcolor);
 			
 			/* Draw the text */
-			gdisp_lld_drawchar(x, y, c, font, color);
+			GDISP_LLD(drawchar)(x, y, c, font, color);
 
 		/* Method 2: Create a single column bitmap and then blit it */
 		#elif GDISP_HARDWARE_BITFILLS && GDISP_SOFTWARE_TEXTBLITCOLUMN
@@ -374,7 +374,7 @@
 				}
 
 				for(xs=0; xs < xscale; xs++)
-					gdisp_lld_blitarea(x+i+xs, y, 1, height, buf);
+					GDISP_LLD(blitarea)(x+i+xs, y, 1, height, buf);
 			}
 		}
 
@@ -393,7 +393,7 @@
 
 			#if GDISP_NEED_VALIDATION
 				/* Check our buffer is big enough */
-				if (width * height > sizeof(buf)/sizeof(buf[0]))	return;
+				if ((unsigned)(width * height) > sizeof(buf)/sizeof(buf[0]))	return;
 			#endif
 
 			ptr = _getCharData(font, c);
@@ -418,7 +418,7 @@
 			}
 
 			/* [Patch by Badger] Write all in one stroke */
-			gdisp_lld_blitarea(x, y, width, height, buf);
+			GDISP_LLD(blitarea)(x, y, width, height, buf);
 		}
 
 		/* Method 4: Draw pixel by pixel */
@@ -440,11 +440,11 @@
 					if (column & 0x01) {
 						for(xs=0; xs < xscale; xs++)
 							for(ys=0; ys < yscale; ys++)
-								gdisp_lld_drawpixel(x+i, y+j, color);
+								GDISP_LLD(drawpixel)(x+i, y+j, color);
 					} else {
 						for(xs=0; xs < xscale; xs++)
 							for(ys=0; ys < yscale; ys++)
-								gdisp_lld_drawpixel(x+i, y+j, bgcolor);
+								GDISP_LLD(drawpixel)(x+i, y+j, bgcolor);
 					}
 				}
 			}
@@ -455,9 +455,95 @@
 
 
 #if GDISP_NEED_CONTROL && !GDISP_HARDWARE_CONTROL
-	void gdisp_lld_control(int UNUSED(what), void *UNUSED(value)) {
+	void GDISP_LLD(control)(unsigned UNUSED(what), void *UNUSED(value)) {
 		/* Ignore everything */
 	}
 #endif
 
+#if GDISP_NEED_QUERY && !GDISP_HARDWARE_QUERY
+void *GDISP_LLD(query)(unsigned what) {
+	switch(what) {
+	case GDISP_QUERY_WIDTH:			return (void *)(unsigned)GDISP.Width;
+	case GDISP_QUERY_HEIGHT:		return (void *)(unsigned)GDISP.Height;
+	case GDISP_QUERY_POWER:			return (void *)(unsigned)GDISP.Powermode;
+	case GDISP_QUERY_ORIENTATION:	return (void *)(unsigned)GDISP.Orientation;
+	case GDISP_QUERY_BACKLIGHT:		return (void *)(unsigned)GDISP.Backlight;
+	case GDISP_QUERY_CONTRAST:		return (void *)(unsigned)GDISP.Contrast;
+	default:						return (void *)-1;
+	}
+}
+#endif
+
+#if GDISP_NEED_MSGAPI
+	void GDISP_LLD(msgdispatch)(gdisp_lld_msg_t *msg) {
+		switch(msg->action) {
+		case GDISP_LLD_MSG_NOP:
+			break;
+		case GDISP_LLD_MSG_INIT:
+			GDISP_LLD(init)();
+			break;
+		case GDISP_LLD_MSG_CLEAR:
+			GDISP_LLD(clear)(msg->clear.color);
+			break;
+		case GDISP_LLD_MSG_DRAWPIXEL:
+			GDISP_LLD(drawpixel)(msg->drawpixel.x, msg->drawpixel.y, msg->drawpixel.color);
+			break;
+		case GDISP_LLD_MSG_FILLAREA:
+			GDISP_LLD(fillarea)(msg->fillarea.x, msg->fillarea.y, msg->fillarea.cx, msg->fillarea.cy, msg->fillarea.color);
+			break;
+		case GDISP_LLD_MSG_BLITAREA:
+			GDISP_LLD(blitarea)(msg->blitarea.x, msg->blitarea.y, msg->blitarea.cx, msg->blitarea.cy, msg->blitarea.buffer);
+			break;
+		case GDISP_LLD_MSG_DRAWLINE:
+			GDISP_LLD(drawline)(msg->drawline.x0, msg->drawline.y0, msg->drawline.x1, msg->drawline.y1, msg->drawline.color);
+			break;
+		#if GDISP_NEED_CIRCLE
+			case GDISP_LLD_MSG_DRAWCIRCLE:
+				GDISP_LLD(drawcircle)(msg->drawcircle.x, msg->drawcircle.y, msg->drawcircle.radius, msg->drawcircle.color);
+				break;
+			case GDISP_LLD_MSG_FILLCIRCLE:
+				GDISP_LLD(fillcircle)(msg->fillcircle.x, msg->fillcircle.y, msg->fillcircle.radius, msg->fillcircle.color);
+				break;
+		#endif
+		#if GDISP_NEED_ELLIPSE
+			case GDISP_LLD_MSG_DRAWELLIPSE:
+				GDISP_LLD(drawellipse)(msg->drawellipse.x, msg->drawellipse.y, msg->drawellipse.a, msg->drawellipse.b, msg->drawellipse.color);
+				break;
+			case GDISP_LLD_MSG_FILLELLIPSE:
+				GDISP_LLD(fillellipse)(msg->fillellipse.x, msg->fillellipse.y, msg->fillellipse.a, msg->fillellipse.b, msg->fillellipse.color);
+				break;
+		#endif
+		#if GDISP_NEED_TEXT
+			case GDISP_LLD_MSG_DRAWCHAR:
+				GDISP_LLD(drawchar)(msg->drawchar.x, msg->drawchar.y, msg->drawchar.c, msg->drawchar.font, msg->drawchar.color);
+				break;
+			case GDISP_LLD_MSG_FILLCHAR:
+				GDISP_LLD(fillchar)(msg->fillchar.x, msg->fillchar.y, msg->fillchar.c, msg->fillchar.font, msg->fillchar.color, msg->fillchar.bgcolor);
+				break;
+		#endif
+		#if GDISP_NEED_PIXELREAD
+			case GDISP_LLD_MSG_GETPIXELCOLOR:
+				msg->getpixelcolor.result = GDISP_LLD(getpixelcolor)(msg->getpixelcolor.x, msg->getpixelcolor.y);
+				break;
+		#endif
+		#if GDISP_NEED_SCROLL
+			case GDISP_LLD_MSG_VERTICALSCROLL:
+				GDISP_LLD(verticalscroll)(msg->verticalscroll.x, msg->verticalscroll.y, msg->verticalscroll.cx, msg->verticalscroll.cy, msg->verticalscroll.lines, msg->verticalscroll.bgcolor);
+				break;
+		#endif
+		#if GDISP_NEED_CONTROL
+			case GDISP_LLD_MSG_CONTROL:
+				GDISP_LLD(control)(msg->control.what, msg->control.value);
+				break;
+		#endif
+		#if GDISP_NEED_QUERY
+			case GDISP_LLD_MSG_QUERY:
+				msg->query.result = GDISP_LLD(query)(msg->query.what);
+				break;
+		#endif
+		}
+	}
+#endif
+
 #endif  /* HAL_USE_GDISP */
+#endif	/* GDISP_EMULATION_C */
diff --git a/halext/include/gdisp_lld.h b/halext/include/gdisp_lld.h
index e9edbe96..bf083a4f 100644
--- a/halext/include/gdisp_lld.h
+++ b/halext/include/gdisp_lld.h
@@ -30,6 +30,94 @@
 
 #if HAL_USE_GDISP || defined(__DOXYGEN__)
 
+/*===========================================================================*/
+/* Low level driver configuration needs					                     */
+/*===========================================================================*/
+
+/**
+ * @name    GDISP low level driver more complex functionality to be compiled
+ * @{
+ */
+	/**
+	 * @brief   Should all operations be clipped to the screen and colors validated.
+	 * @details	Defaults to TRUE.
+	 * @note    If this is FALSE, any operations that extend beyond the
+	 *          edge of the screen will have undefined results. Any
+	 *			out-of-range colors will produce undefined results.
+	 * @note	If defined then all low level and high level driver routines
+	 *			must check the validity of inputs and do something sensible
+	 *			if they are out of range. It doesn't have to be efficient,
+	 *			just valid.
+	 */
+	#ifndef GDISP_NEED_VALIDATION
+		#define GDISP_NEED_VALIDATION	TRUE
+	#endif
+
+	/**
+	 * @brief   Are circle functions needed.
+	 * @details	Defaults to TRUE
+	 */
+	#ifndef GDISP_NEED_CIRCLE
+		#define GDISP_NEED_CIRCLE		TRUE
+	#endif
+
+	/**
+	 * @brief   Are ellipse functions needed.
+	 * @details	Defaults to TRUE
+	 */
+	#ifndef GDISP_NEED_ELLIPSE
+		#define GDISP_NEED_ELLIPSE		TRUE
+	#endif
+
+	/**
+	 * @brief   Are text functions needed.
+	 * @details	Defaults to TRUE
+	 */
+	#ifndef GDISP_NEED_TEXT
+		#define GDISP_NEED_TEXT			TRUE
+	#endif
+
+	/**
+	 * @brief   Is scrolling needed.
+	 * @details	Defaults to FALSE
+	 */
+	#ifndef GDISP_NEED_SCROLL
+		#define GDISP_NEED_SCROLL		FALSE
+	#endif
+
+	/**
+	 * @brief   Is the capability to read pixels back needed.
+	 * @details	Defaults to FALSE
+	 */
+	#ifndef GDISP_NEED_PIXELREAD
+		#define GDISP_NEED_PIXELREAD	FALSE
+	#endif
+
+	/**
+	 * @brief   Control some aspect of the drivers operation.
+	 * @details	Defaults to FALSE
+	 */
+	#ifndef GDISP_NEED_CONTROL
+		#define GDISP_NEED_CONTROL		FALSE
+	#endif
+
+	/**
+	 * @brief   Query some aspect of the drivers operation.
+	 * @details	Defaults to TRUE
+	 */
+	#ifndef GDISP_NEED_QUERY
+		#define GDISP_NEED_QUERY		TRUE
+	#endif
+
+	/**
+	 * @brief   Is the messaging api interface required.
+	 * @details	Defaults to FALSE
+	 */
+	#ifndef GDISP_NEED_MSGAPI
+		#define GDISP_NEED_MSGAPI	FALSE
+	#endif
+/** @} */
+
 /*===========================================================================*/
 /* Include the low level driver configuration information                    */
 /*===========================================================================*/
@@ -60,6 +148,39 @@
 #define GDISP_CONTROL_CONTRAST		3
 #define GDISP_CONTROL_LLD			1000
 
+/**
+ * @brief   Driver Query Constants
+ * @detail	Unsupported query codes return (void *)-1.
+ * @note	There are some predefined and some specific to the low level driver.
+ * @note	The result should be typecast the required type.
+ * @note	GDISP_QUERY_WIDTH			- Gets the width of the screen
+ * 			GDISP_QUERY_HEIGHT			- Gets the height of the screen
+ * 			GDISP_QUERY_POWER			- Get the current powermode
+ * 			GDISP_QUERY_ORIENTATION		- Get the current orientation
+ * 			GDISP_QUERY_BACKLIGHT		- Get the backlight state (0 to 100)
+ * 			GDISP_QUERY_CONTRAST		- Get the contrast.
+ * 			GDISP_QUERY_LLD				- Low level driver control constants start at
+ * 											this value.
+ */
+#define GDISP_QUERY_WIDTH			0
+#define GDISP_QUERY_HEIGHT			1
+#define GDISP_QUERY_POWER			2
+#define GDISP_QUERY_ORIENTATION		3
+#define GDISP_QUERY_BACKLIGHT		4
+#define GDISP_QUERY_CONTRAST		5
+#define GDISP_QUERY_LLD				1000
+
+/**
+ * @brief   Driver Pixel Format Constants
+ */
+#define GDISP_PIXELFORMAT_RGB565	565
+#define GDISP_PIXELFORMAT_RGB888	888
+#define GDISP_PIXELFORMAT_RGB444	444
+#define GDISP_PIXELFORMAT_RGB332	332
+#define GDISP_PIXELFORMAT_RGB666	666
+#define GDISP_PIXELFORMAT_CUSTOM	99999
+#define GDISP_PIXELFORMAT_ERROR		88888
+
 /*===========================================================================*/
 /* Error checks.                                                             */
 /*===========================================================================*/
@@ -171,6 +292,14 @@
 	#ifndef GDISP_HARDWARE_CONTROL
 		#define GDISP_HARDWARE_CONTROL			FALSE
 	#endif
+
+	/**
+	 * @brief   The driver supports a non-standard query.
+	 * @details If set to @p FALSE there is no support for non-standard queries.
+	 */
+	#ifndef GDISP_HARDWARE_QUERY
+		#define GDISP_HARDWARE_QUERY			FALSE
+	#endif
 /** @} */
 
 /**
@@ -205,19 +334,21 @@
  */
 	/**
 	 * @brief   The native pixel format for this device
-	 * @note	One of the following should be defined:
+	 * @note	Should be set to one of the following:
 	 *				GDISP_PIXELFORMAT_RGB565
 	 *				GDISP_PIXELFORMAT_RGB888
 	 *				GDISP_PIXELFORMAT_RGB444
 	 *				GDISP_PIXELFORMAT_RGB332
 	 *				GDISP_PIXELFORMAT_RGB666
 	 *				GDISP_PIXELFORMAT_CUSTOM
-	 * @note	If you define GDISP_PIXELFORMAT_CUSTOM you need to also define
+	 * @note	If you set GDISP_PIXELFORMAT_CUSTOM you need to also define
 	 *				color_t, RGB2COLOR(r,g,b), HTML2COLOR(h),
 	 *              RED_OF(c), GREEN_OF(c), BLUE_OF(c),
 	 *              COLOR(c) and MASKCOLOR.
 	 */
-	#define GDISP_PIXELFORMAT_XXXXXX
+	#ifndef GDISP_PIXELFORMAT
+		#define GDISP_PIXELFORMAT	GDISP_PIXELFORMAT_ERROR
+	#endif
 
 	/**
 	 * @brief   Do pixels require packing for a blit
@@ -287,7 +418,7 @@
 	 */
 	#define BLUE_OF(c)			(((c)&0x001F)<<3)
 
-#elif defined(GDISP_PIXELFORMAT_RGB565)
+#elif GDISP_PIXELFORMAT == GDISP_PIXELFORMAT_RGB565
 	typedef uint16_t color_t;
 	#define COLOR(c)			((color_t)(c))
 	#define MASKCOLOR			FALSE
@@ -299,7 +430,7 @@
 	#define RGB565CONVERT(red, green, blue) (uint16_t)( (( red   >> 3 ) << 11 ) | (( green >> 2 ) << 5  ) | ( blue  >> 3 ))
 
 
-#elif defined(GDISP_PIXELFORMAT_RGB888)
+#elif GDISP_PIXELFORMAT == GDISP_PIXELFORMAT_RGB888
 	typedef uint32_t color_t;
 	#define COLOR(c)			((color_t)(((c) & 0xFFFFFF)))
 	#define MASKCOLOR			TRUE
@@ -309,7 +440,7 @@
 	#define GREEN_OF(c)			(((c)&0x00FF00)>>8)
 	#define BLUE_OF(c)			((c)&0x0000FF)
 
-#elif defined(GDISP_PIXELFORMAT_RGB444)
+#elif GDISP_PIXELFORMAT == GDISP_PIXELFORMAT_RGB444
 	typedef uint16_t color_t;
 	#define COLOR(c)			((color_t)(((c) & 0x0FFF)))
 	#define MASKCOLOR			TRUE
@@ -319,7 +450,7 @@
 	#define GREEN_OF(c)			((c)&0x00F0)
 	#define BLUE_OF(c)			(((c)&0x000F)<<4)
 
-#elif defined(GDISP_PIXELFORMAT_RGB332)
+#elif GDISP_PIXELFORMAT == GDISP_PIXELFORMAT_RGB332
 	typedef uint8_t color_t;
 	#define COLOR(c)			((color_t)(c))
 	#define MASKCOLOR			FALSE
@@ -329,7 +460,7 @@
 	#define GREEN_OF(c)			(((c)&0x1C)<<3)
 	#define BLUE_OF(c)			(((c)&0x03)<<6)
 
-#elif defined(GDISP_PIXELFORMAT_RGB666)
+#elif GDISP_PIXELFORMAT == GDISP_PIXELFORMAT_RGB666
 	typedef uint32_t color_t;
 	#define COLOR(c)			((color_t)(((c) & 0x03FFFF)))
 	#define MASKCOLOR			TRUE
@@ -339,7 +470,7 @@
 	#define GREEN_OF(c)			(((c)&0x00FC00)>>8)
 	#define BLUE_OF(c)			(((c)&0x00003F)<<2)
 
-#elif !defined(GDISP_PIXELFORMAT_CUSTOM)
+#elif GDISP_PIXELFORMAT != GDISP_PIXELFORMAT_CUSTOM
 	#error "GDISP: No supported pixel format has been specified."
 #endif
 
@@ -348,10 +479,10 @@
 	#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 !defined(GDISP_PIXELFORMAT_RGB888) \
-		&& !defined(GDISP_PIXELFORMAT_RGB444) \
-		&& !defined(GDISP_PIXELFORMAT_RGB666) \
-		&& !defined(GDISP_PIXELFORMAT_CUSTOM)
+#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
 
@@ -392,44 +523,13 @@ typedef enum orientation {portrait, landscape, portraitInv, landscapeInv} gdisp_
  */
 typedef enum powermode {powerOff, powerSleep, powerOn} gdisp_powermode_t;
 
-/**
- * @brief   Structure representing a GDISP driver.
- * @note    Implementations may extend this structure to contain more,
- *          architecture dependent, fields by defining GDISP_DRIVER_EXT_FIELDS
- */
-struct GDISPDriver {
-	/**
-	 * @brief   Width of the screen.
-	 * @note    Read-only.
-	 */
-	coord_t				Width;
-	/**
-	 * @brief   Height of the screen.
-	 * @note    Read-only.
-	 */
-	coord_t				Height;
-	/**
-	 * @brief   Current orientation of the screen.
-	 * @note    Read-only.
-	 */
-	gdisp_orientation_t	Orientation;
-	/**
-	 * @brief   Current power mode of the screen.
-	 * @note    Read-only.
-	 */
-	gdisp_powermode_t	Powermode;
-
-	#if defined(GDISP_DRIVER_EXT_FIELDS)
-	GDISP_DRIVER_EXT_FIELDS
-	#endif
-};
-
 /*===========================================================================*/
 /* External declarations.                                                    */
 /*===========================================================================*/
 
-#if !defined(__DOXYGEN__)
-	extern GDISPDriver GDISP;
+#ifndef GDISP_LLD_VMT
+	/* Special magic stuff for the VMT driver */
+	#define GDISP_LLD_VMT(x) GDISP_LLD(x)
 #endif
 
 #ifdef __cplusplus
@@ -437,49 +537,60 @@ extern "C" {
 #endif
 
 	/* Core functions */
-	void gdisp_lld_init(void);
+	extern bool_t GDISP_LLD(init)(void);
 
 	/* Some of these functions will be implemented in software by the high level driver
 	   depending on the GDISP_HARDWARE_XXX macros defined in gdisp_lld_config.h.
 	 */
 
 	/* Drawing functions */
-	void gdisp_lld_clear(color_t color);
-	void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color);
-	void gdisp_lld_fillarea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color);
-	void gdisp_lld_blitarea(coord_t x, coord_t y, coord_t cx, coord_t cy, pixel_t *buffer);
-	void gdisp_lld_drawline(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color);
+	extern void GDISP_LLD_VMT(clear)(color_t color);
+	extern void GDISP_LLD_VMT(drawpixel)(coord_t x, coord_t y, color_t color);
+	extern void GDISP_LLD_VMT(fillarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color);
+	extern void GDISP_LLD_VMT(blitarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer);
+	extern void GDISP_LLD_VMT(drawline)(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color);
 
 	/* Circular Drawing Functions */
 	#if GDISP_NEED_CIRCLE
-	void gdisp_lld_drawcircle(coord_t x, coord_t y, coord_t radius, color_t color);
-	void gdisp_lld_fillcircle(coord_t x, coord_t y, coord_t radius, color_t color);
+	extern void GDISP_LLD_VMT(drawcircle)(coord_t x, coord_t y, coord_t radius, color_t color);
+	extern void GDISP_LLD_VMT(fillcircle)(coord_t x, coord_t y, coord_t radius, color_t color);
 	#endif
 
 	#if GDISP_NEED_ELLIPSE
-	void gdisp_lld_drawellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
-	void gdisp_lld_fillellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
+	extern void GDISP_LLD_VMT(drawellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
+	extern void GDISP_LLD_VMT(fillellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color);
 	#endif
 
 	/* Text Rendering Functions */
 	#if GDISP_NEED_TEXT
-	void gdisp_lld_drawchar(coord_t x, coord_t y, char c, font_t font, color_t color);
-	void gdisp_lld_fillchar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor);
+	extern void GDISP_LLD_VMT(drawchar)(coord_t x, coord_t y, char c, font_t font, color_t color);
+	extern void GDISP_LLD_VMT(fillchar)(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor);
 	#endif
 
 	/* Pixel readback */
 	#if GDISP_NEED_PIXELREAD
-	color_t gdisp_lld_getpixelcolor(coord_t x, coord_t y);
+	extern color_t GDISP_LLD_VMT(getpixelcolor)(coord_t x, coord_t y);
 	#endif
 
 	/* Scrolling Function - clears the area scrolled out */
 	#if GDISP_NEED_SCROLL
-	void gdisp_lld_verticalscroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor);
+	extern void GDISP_LLD_VMT(verticalscroll)(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor);
 	#endif
 
 	/* Set driver specific control */
 	#if GDISP_NEED_CONTROL
-	void gdisp_lld_control(int what, void *value);
+	extern void GDISP_LLD_VMT(control)(unsigned what, void *value);
+	#endif
+
+	/* Query driver specific data */
+	#if GDISP_NEED_QUERY
+	extern void *GDISP_LLD_VMT(query)(unsigned what);
+	#endif
+
+	/* Messaging API */
+	#if GDISP_NEED_MSGAPI
+	#include "gdisp_lld_msgs.h"
+	extern void GDISP_LLD(msgdispatch)(gdisp_lld_msg_t *msg);
 	#endif
 
 #ifdef __cplusplus
diff --git a/halext/include/gdisp_lld_msgs.h b/halext/include/gdisp_lld_msgs.h
new file mode 100644
index 00000000..28a6707e
--- /dev/null
+++ b/halext/include/gdisp_lld_msgs.h
@@ -0,0 +1,165 @@
+/*
+    ChibiOS/RT - Copyright (C) 2012
+                 Joel Bodenmann aka Tectu <joel@unormal.org>
+
+    This file is part of ChibiOS-LCD-Driver.
+
+    ChibiOS-LCD-Driver is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS-LCD-Driver is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+/**
+ * @file    gdisp_lld_msgs.h
+ * @brief   GDISP Graphic Driver subsystem low level driver message structures.
+ *
+ * @addtogroup GDISP
+ * @{
+ */
+
+#ifndef _GDISP_LLD_MSGS_H
+#define _GDISP_LLD_MSGS_H
+
+/* This file describes the message API for gdisp_lld */
+#if HAL_USE_GDISP && GDISP_NEED_MSGAPI
+
+typedef enum gdisp_msgaction {
+	GDISP_LLD_MSG_NOP,
+	GDISP_LLD_MSG_INIT,
+	GDISP_LLD_MSG_CLEAR,
+	GDISP_LLD_MSG_DRAWPIXEL,
+	GDISP_LLD_MSG_FILLAREA,
+	GDISP_LLD_MSG_BLITAREA,
+	GDISP_LLD_MSG_DRAWLINE,
+	#if GDISP_NEED_CIRCLE
+		GDISP_LLD_MSG_DRAWCIRCLE,
+		GDISP_LLD_MSG_FILLCIRCLE,
+	#endif
+	#if GDISP_NEED_ELLIPSE
+		GDISP_LLD_MSG_DRAWELLIPSE,
+		GDISP_LLD_MSG_FILLELLIPSE,
+	#endif
+	#if GDISP_NEED_TEXT
+		GDISP_LLD_MSG_DRAWCHAR,
+		GDISP_LLD_MSG_FILLCHAR,
+	#endif
+	#if GDISP_NEED_PIXELREAD
+		GDISP_LLD_MSG_GETPIXELCOLOR,
+	#endif
+	#if GDISP_NEED_SCROLL
+		GDISP_LLD_MSG_VERTICALSCROLL,
+	#endif
+	#if GDISP_NEED_CONTROL
+		GDISP_LLD_MSG_CONTROL,
+	#endif
+	#if GDISP_NEED_QUERY
+		GDISP_LLD_MSG_QUERY,
+	#endif
+} gdisp_msgaction_t;
+
+typedef union gdisp_lld_msg {
+	gdisp_msgaction_t	action;
+	struct gdisp_lld_msg_init {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_INIT
+	} init;
+	struct gdisp_lld_msg_clear {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_CLEAR
+		color_t				color;
+	} clear;
+	struct gdisp_lld_msg_drawpixel {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_DRAWPIXEL
+		coord_t				x, y;
+		color_t				color;
+	} drawpixel;
+	struct gdisp_lld_msg_fillarea {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_FILLAREA
+		coord_t				x, y;
+		coord_t				cx, cy;
+		color_t				color;
+	} fillarea;
+	struct gdisp_lld_msg_blitarea {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_BLITAREA
+		coord_t				x, y;
+		coord_t				cx, cy;
+		const pixel_t		*buffer;
+	} blitarea;
+	struct gdisp_lld_msg_drawline {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_DRAWLINE
+		coord_t				x0, y0;
+		coord_t				x1, y1;
+		color_t				color;
+	} drawline;
+	struct gdisp_lld_msg_drawcircle {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_DRAWCIRCLE
+		coord_t				x, y;
+		coord_t				radius;
+		color_t				color;
+	} drawcircle;
+	struct gdisp_lld_msg_fillcircle {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_FILLCIRCLE
+		coord_t				x, y;
+		coord_t				radius;
+		color_t				color;
+	} fillcircle;
+	struct gdisp_lld_msg_drawellipse {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_DRAWELLIPSE
+		coord_t				x, y;
+		coord_t				a, b;
+		color_t				color;
+	} drawellipse;
+	struct gdisp_lld_msg_fillellipse {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_FILLELLIPSE
+		coord_t				x, y;
+		coord_t				a, b;
+		color_t				color;
+	} fillellipse;
+	struct gdisp_lld_msg_drawchar {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_DRAWCHAR
+		coord_t				x, y;
+		char				c;
+		font_t				font;
+		color_t				color;
+	} drawchar;
+	struct gdisp_lld_msg_fillchar {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_FILLCHAR
+		coord_t				x, y;
+		char				c;
+		font_t				font;
+		color_t				color;
+		color_t				bgcolor;
+	} fillchar;
+	struct gdisp_lld_msg_getpixelcolor {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_GETPIXELCOLOR
+		coord_t				x, y;
+		color_t				result;
+	} getpixelcolor;
+	struct gdisp_lld_msg_verticalscroll {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_VERTICALSCROLL
+		coord_t				x, y;
+		coord_t				cx, cy;
+		int					lines;
+		color_t				bgcolor;
+	} verticalscroll;
+	struct gdisp_lld_msg_control {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_CONTROL
+		int					what;
+		void *				value;
+	} control;
+	struct gdisp_lld_msg_query {
+		gdisp_msgaction_t	action;			// GDISP_LLD_MSG_QUERY
+		int					what;
+		void *				result;
+	} query;
+} gdisp_lld_msg_t;
+
+#endif	/* HAL_USE_GDISP */
+#endif	/* _GDISP_LLD_MSGS_H */
+/** @} */
diff --git a/halext/include/glcd.h b/halext/include/glcd.h
index 54c5158d..670d160e 100644
--- a/halext/include/glcd.h
+++ b/halext/include/glcd.h
@@ -24,6 +24,8 @@
 	some code changes may be necessary.
 	Note it does not replicate the GLCD low level driver, just
 	the high level interface.
+	You may also need to define the various GDISP_NEED_XXX in your
+	halconf.h in order to turn on the functionality you need.
 */
 
 #ifndef GLCD_H
@@ -38,10 +40,8 @@
 
 #define RGB565CONVERT(r, g, b)	RGB2COLOR(r,g,b)
 
-enum orientation {portrait, landscape, portraitInv, landscapeInv};
 enum filled {frame, filled};
 enum transparency {solid, transparent};
-enum powermode {powerOff, powerOn, sleepOn, sleepOff};
 #define sleepOn		powerSleep
 #define sleepOff	powerOn
 
@@ -70,14 +70,33 @@ typedef enum glcd_result glcd_result_t;
 
 /* Core functions */
 #define lcdInit(dvr)							gdispInit(dvr)
-#define lcdClear(color)							(gdispClear(color), GLCD_DONE)
-#define lcdSetOrientation(newO)					(gdispControl(GDISP_CONTROL_ORIENTATION, (void *)(int)newO), (GDISP1.Orientation == (newO) ? GLCD_DONE : GLCD_FAILED))
-#define lcdFillArea(x0,y0,x1,y1,c)				(gdispFillArea((x0),(y0),(x1)-(x0)+1,(y1)-(y0)+1,(c)), GLCD_DONE)
-#define lcdWriteArea(x0,y0,x1,y1,b,n)			(gdispBlitArea((x0),(y0),(x1)-(x0)+1,(y1)-(y0)+1,(b)), GLCD_DONE)
-#define lcdSetPowerMode(pm)						(gdispControl(GDISP_CONTROL_POWER, (void *)(int)pm), (GDISP1.Powermode == (pm) ? GLCD_DONE : GLCD_FAILED))
+static __inline glcd_result_t lcdClear(color_t color) {
+	gdispClear(color);
+	return GLCD_DONE;
+}
+static __inline glcd_result_t lcdSetOrientation(enum orientation newO) {
+	gdispControl(GDISP_CONTROL_ORIENTATION, (void *)(int)newO);
+	return ((enum orientation)(unsigned)gdispQuery(GDISP_QUERY_ORIENTATION)) == (newO) ? GLCD_DONE : GLCD_FAILED;
+}
+static __inline glcd_result_t lcdFillArea(coord_t x0, coord_t y0, coord_t x1, coord_t y1,color_t c) {
+	gdispFillArea((x0),(y0),(x1)-(x0)+1,(y1)-(y0)+1,(c));
+	return GLCD_DONE;
+}
+static __inline glcd_result_t lcdWriteArea(coord_t x0, coord_t y0, coord_t x1, coord_t y1, const pixel_t *b, coord_t n) {
+		(void)n;
+		gdispBlitArea((x0),(y0),(x1)-(x0)+1,(y1)-(y0)+1,(b));
+		return GLCD_DONE;
+}
+static __inline glcd_result_t lcdSetPowerMode(enum powermode pm) {
+	gdispControl(GDISP_CONTROL_POWER, (void *)(int)pm);
+	return ((enum powermode)(unsigned)gdispQuery(GDISP_QUERY_POWER)) == (pm) ? GLCD_DONE : GLCD_FAILED;
+}
 
 /* Drawing functions */
-#define lcdDrawPixel(x,y,c)						(gdispDrawPixel((x),(y),(c)), GLCD_DONE)
+static __inline glcd_result_t lcdDrawPixel(coord_t x, coord_t y, color_t c) {
+	gdispDrawPixel((x),(y),(c));
+	return GLCD_DONE;
+}
 #define lcdDrawLine(x0,y0,x1,y1,c)				gdispDrawLine((x0),(y0),(x1),(y1),(c))
 #define lcdDrawRect(x0,y0,x1,y1,f,c)			{if(f) gdispFillArea((x0),(y0),(x1)-(x0)+1,(y1)-(y0)+1,(c)); else gdispDrawBox((x0),(y0),(x1)-(x0)+1,(y1)-(y0)+1,(c));}
 #define lcdDrawRectString(x0,y0,x1,y1,s,f,c,b)	gdispFillStringBox((x0),(y0),(x1)-(x0)+1,(y1)-(y0)+1,(s),(f),(c),(b),justifyLeft)
@@ -85,18 +104,30 @@ typedef enum glcd_result glcd_result_t;
 #define lcdDrawEllipse(x,y,a,b,f,c)				{if(f) gdispFillEllipse((x),(y),(a),(b),(c)); else gdispDrawEllipse((x),(y),(a),(b),(c));}
 
 /* Text Rendering Functions */
-#define lcdDrawChar(x,y,h,f,c,b,t)				({if(t) gdispDrawChar((x),(y),(h),(f),(c)); else gdispFillChar((x),(y),(h),(f),(c),(b));}, (gdispGetCharWidth((h),(f))+(f)->charPadding))
-#define lcdDrawString(x,y,s,f,c,b,t)			({if(t) gdispDrawString((x),(y),(s),(f),(c)); else gdispFillString((x),(y),(s),(f),(c),(b));}, (gdispGetStringWidth((s),(f))+(f)->charPadding))
+static __inline coord_t lcdDrawChar(coord_t x, coord_t y, char h, font_t f, color_t c, color_t b, bool_t t) {
+	if (t)
+		gdispDrawChar((x),(y),(h),(f),(c));
+	else
+		gdispFillChar((x),(y),(h),(f),(c),(b));
+	return gdispGetCharWidth((h),(f))+gdispGetFontMetric((f), fontCharPadding);
+}
+static __inline coord_t lcdDrawString(coord_t x, coord_t y, const char *s, font_t f, color_t c, color_t b, bool_t t) {
+	if (t)
+		gdispDrawString((x),(y),(s),(f),(c));
+	else
+		gdispFillString((x),(y),(s),(f),(c),(b));
+	return gdispGetStringWidth((s),(f))+gdispGetFontMetric((f), fontCharPadding);
+}
 
 /* Character measuring functions */
-#define lcdMeasureChar(h,f)						(gdispGetCharWidth((h),(f))+(f)->charPadding)
-#define lcdMeasureString(s,f)					(gdispGetStringWidth((s),(f))+(f)->charPadding)
+#define lcdMeasureChar(h,f)						(gdispGetCharWidth((h),(f))+gdispGetFontMetric((f), fontCharPadding))
+#define lcdMeasureString(s,f)					(gdispGetStringWidth((s),(f))+gdispGetFontMetric((f), fontCharPadding))
 #define lcdGetFontHeight(f)						gdispGetFontMetric((f), fontHeight)
 
 /* Size and orientation related */
-#define lcdGetHeight()							(GDISP.Height)
-#define lcdGetWidth()							(GDISP.Width)
-#define lcdGetOrientation()						(GDISP.Orientation)
+#define lcdGetHeight()							((coord_t)(unsigned)gdispQuery(GDISP_QUERY_HEIGHT))
+#define lcdGetWidth()							((coord_t)(unsigned)gdispQuery(GDISP_QUERY_WIDTH))
+#define lcdGetOrientation()						((enum orientation)(unsigned)gdispQuery(GDISP_QUERY_ORIENTATION))
 
 /* BGR->RGB and pixel readback */
 #define lcdBGR2RGB(c)							RGB2COLOR(BLUE_OF(c),GREEN_OF(c),RED_OF(c))
diff --git a/halext/readme.txt b/halext/readme.txt
index 1123e827..907c7eb4 100644
--- a/halext/readme.txt
+++ b/halext/readme.txt
@@ -7,6 +7,7 @@ To include any of these functions/drivers in your project...
 
 	3/ In your project Makefile add the makefiles for any specific drivers you want e.g
 		include $(LCDLIB)/halext/drivers/touchpad/touchpadXPT2046/touchpad_lld.mk
+		include $(LCDLIB)/halext/drivers/gdispNokia6610/gdisp_lld.mk
 
 	4/ In your project halconf.h turn on the support you want eg.
 		/**
@@ -15,6 +16,13 @@ To include any of these functions/drivers in your project...
 		#if !defined(HAL_USE_TOUCHPAD) || defined(__DOXYGEN__)
 			#define HAL_USE_TOUCHPAD           TRUE
 		#endif
-
+		/**
+		 * @brief   Enables the GDISP subsystem.
+		 */
+		#if !defined(HAL_USE_GDISP) || defined(__DOXYGEN__)
+			#define HAL_USE_GDISP           TRUE
+			/* Any driver specific defines required go here. The below line is an example. */
+			#define GDISP_NEED_MULTITHREAD	TRUE
+		#endif
 	5/ Do a make clean.
 
diff --git a/halext/src/gdisp-readme.txt b/halext/src/gdisp-readme.txt
index 28b86077..12b00e2c 100644
--- a/halext/src/gdisp-readme.txt
+++ b/halext/src/gdisp-readme.txt
@@ -1,10 +1,10 @@
-The new GDISP driver is an architecture independent rewrite of the GLCD interface.
-This new architecture independence should allow many new low level drivers to be easily added.
+The new GDISP driver is an architecture independant rewrite of the GLCD interface.
+This new architecture independance should allow many new low level drivers to be easily added.
 
 GDISP allows low-level driver hardware accelerated drawing routines while providing a software emulation
 if the low level driver can not provide it. A basic low level driver now only requires 2 routines to be written.
 
-A glcd.h compatibility file has been included that allow applications written to use the existing GLCD driver to
+A glcd.h compatability file has been included that allow applications written to use the existing GLCD driver to
 use the GDISP driver with little or no change.
 
 It is written in the ChibiOS style with ChibiOS style includes and documentation.
diff --git a/halext/src/gdisp.c b/halext/src/gdisp.c
index de3761cd..90b4582d 100644
--- a/halext/src/gdisp.c
+++ b/halext/src/gdisp.c
@@ -29,19 +29,15 @@
 #include "hal.h"
 #include "gdisp.h"
 
+#ifndef _GDISP_C
+#define _GDISP_C
+
 #if HAL_USE_GDISP || defined(__DOXYGEN__)
 
 #ifdef GDISP_NEED_TEXT
 	#include "gdisp_fonts.h"
 #endif
 
-#if GDISP_NEED_MULTITHREAD
-	#warning "GDISP: Multithread support not complete"
-	#define MUTEX_INIT		/* Not defined yet */
-	#define MUTEX_ENTER		/* Not defined yet */
-	#define MUTEX_EXIT		/* Not defined yet */
-#endif
-
 /*===========================================================================*/
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
@@ -55,6 +51,18 @@
 # define UNUSED(x) x
 #endif
 
+#if GDISP_NEED_MULTITHREAD
+	#if !CH_USE_MUTEXES
+		#error "GDISP: CH_USE_MUTEXES must be defined in chconf.h because GDISP_NEED_MULTITHREAD is defined"
+	#endif
+#endif
+
+#if GDISP_NEED_ASYNC
+	#if !CH_USE_MAILBOXES || !CH_USE_MUTEXES || !CH_USE_SEMAPHORES
+		#error "GDISP: CH_USE_MAILBOXES, CH_USE_SEMAPHORES and CH_USE_MUTEXES must be defined in chconf.h because GDISP_NEED_ASYNC is defined"
+	#endif
+#endif
+
 /*===========================================================================*/
 /* Driver exported variables.                                                */
 /*===========================================================================*/
@@ -63,10 +71,77 @@
 /* Driver local variables.                                                   */
 /*===========================================================================*/
 
+#if GDISP_NEED_MULTITHREAD || GDISP_NEED_ASYNC
+	static Mutex			gdispMutex;
+#endif
+
+#if GDISP_NEED_ASYNC
+	#define GDISP_THREAD_STACK_SIZE	512		/* Just a number - not yet a reflection of actual use */
+	#define GDISP_QUEUE_SIZE		8		/* We only allow a short queue */
+
+	static Thread *			lldThread;
+	static Mailbox			gdispMailbox;
+	static msg_t 			gdispMailboxQueue[GDISP_QUEUE_SIZE];
+	static Semaphore		gdispMsgsSem;
+	static Mutex			gdispMsgsMutex;
+	static gdisp_lld_msg_t	gdispMsgs[GDISP_QUEUE_SIZE];
+	static WORKING_AREA(waGDISPThread, GDISP_THREAD_STACK_SIZE);
+#endif
+
 /*===========================================================================*/
 /* Driver local functions.                                                   */
 /*===========================================================================*/
 
+#if GDISP_NEED_ASYNC
+	static msg_t GDISPThreadHandler(void *UNUSED(arg)) {
+		gdisp_lld_msg_t	*pmsg;
+
+		#if CH_USE_REGISTRY
+			chRegSetThreadName("GDISPAsyncAPI");
+		#endif
+
+		while(1) {
+			/* Wait for msg with work to do. */
+			chMBFetch(&gdispMailbox, (msg_t *)&pmsg, TIME_INFINITE);
+
+			/* OK - we need to obtain the mutex in case a synchronous operation is occurring */
+			chMtxLock(&gdispMutex);
+			GDISP_LLD(msgdispatch)(pmsg);
+			chMtxUnlock();
+
+			/* Mark the message as free */
+			pmsg->action = GDISP_LLD_MSG_NOP;
+			chSemSignal(&gdispMsgsSem);
+		}
+		return 0;
+	}
+
+	static gdisp_lld_msg_t *gdispAllocMsg(gdisp_msgaction_t action) {
+		gdisp_lld_msg_t	*p;
+
+		while(1) {		/* To be sure, to be sure */
+
+			/* Wait for a slot */
+			chSemWait(&gdispMsgsSem);
+
+			/* Find the slot */
+			chMtxLock(&gdispMsgsMutex);
+			for(p=gdispMsgs; p < &gdispMsgs[GDISP_QUEUE_SIZE]; p++) {
+				if (p->action == GDISP_LLD_MSG_NOP) {
+					/* Allocate it */
+					p->action = action;
+					chMtxUnlock();
+					return p;
+				}
+			}
+			chMtxUnlock();
+
+			/* Oops - none found, try again */
+			chSemSignal(&gdispMsgsSem);
+		}
+	}
+#endif
+
 /*===========================================================================*/
 /* Driver exported functions.                                                */
 /*===========================================================================*/
@@ -79,14 +154,62 @@
 	 *
 	 * @init
 	 */
-	void gdispInit(GDISPDriver * UNUSED(gdisp)) {
+	bool_t gdispInit(GDISPDriver * UNUSED(gdisp)) {
+		bool_t	res;
+
 		/* Initialise Mutex */
-		MUTEX_INIT
+		chMtxInit(&gdispMutex);
 
 		/* Initialise driver */
-		MUTEX_ENTER
-		gdisp_lld_init();
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		res = GDISP_LLD(init)();
+		chMtxUnlock();
+
+		return res;
+	}
+#elif GDISP_NEED_ASYNC
+	bool_t gdispInit(GDISPDriver * UNUSED(gdisp)) {
+		bool_t		res;
+		unsigned	i;
+
+		/* Mark all the Messages as free */
+		for(i=0; i < GDISP_QUEUE_SIZE; i++)
+			gdispMsgs[i].action = GDISP_LLD_MSG_NOP;
+
+		/* Initialise our Mailbox, Mutex's and Counting Semaphore.
+		 * 	A Mutex is required as well as the Mailbox and Thread because some calls have to be synchronous.
+		 *	Synchronous calls get handled by the calling thread, asynchronous by our worker thread.
+		 */
+		chMBInit(&gdispMailbox, gdispMailboxQueue, sizeof(gdispMailboxQueue)/sizeof(gdispMailboxQueue[0]));
+		chMtxInit(&gdispMutex);
+		chMtxInit(&gdispMsgsMutex);
+		chSemInit(&gdispMsgsSem, GDISP_QUEUE_SIZE);
+
+		lldThread = chThdCreateStatic(waGDISPThread, sizeof(waGDISPThread), NORMALPRIO, GDISPThreadHandler, NULL);
+
+		/* Initialise driver - synchronous */
+		chMtxLock(&gdispMutex);
+		res = GDISP_LLD(init)();
+		chMtxUnlock();
+
+		return res;
+	}
+#endif
+
+#if GDISP_NEED_MULTITHREAD || defined(__DOXYGEN__)
+	/**
+	 * @brief   Test if the GDISP engine is currently drawing.
+	 * @note    This function will always return FALSE if
+	 * 			 GDISP_NEED_ASYNC is not defined.
+	 *
+	 * @init
+	 */
+	bool_t gdispIsBusy(void) {
+		return FALSE;
+	}
+#elif GDISP_NEED_ASYNC
+	bool_t gdispIsBusy(void) {
+		return chMBGetUsedCountI(&gdispMailbox) != FALSE;
 	}
 #endif
 
@@ -100,9 +223,15 @@
 	 * @api
 	 */
 	void gdispClear(color_t color) {
-		MUTEX_ENTER
-		gdisp_lld_clear(color);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(clear)(color);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_ASYNC
+	void gdispClear(color_t color) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_CLEAR);
+		p->clear.color = color;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 
@@ -117,9 +246,17 @@
 	 * @api
 	 */
 	void gdispDrawPixel(coord_t x, coord_t y, color_t color) {
-		MUTEX_ENTER
-		gdisp_lld_drawpixel(x, y, color);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(drawpixel)(x, y, color);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_ASYNC
+	void gdispDrawPixel(coord_t x, coord_t y, color_t color) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_DRAWPIXEL);
+		p->drawpixel.x = x;
+		p->drawpixel.y = y;
+		p->drawpixel.color = color;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 	
@@ -135,9 +272,19 @@
 	 * @api
 	 */
 	void gdispDrawLine(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
-		MUTEX_ENTER
-		gdisp_lld_drawline(x0, y0, x1, y1, color);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(drawline)(x0, y0, x1, y1, color);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_ASYNC
+	void gdispDrawLine(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_DRAWLINE);
+		p->drawline.x0 = x0;
+		p->drawline.y0 = y0;
+		p->drawline.x1 = x1;
+		p->drawline.y1 = y1;
+		p->drawline.color = color;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 	
@@ -153,9 +300,19 @@
 	 * @api
 	 */
 	void gdispFillArea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
-		MUTEX_ENTER
-		gdisp_lld_fillarea(x, y, cx, cy, color);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(fillarea)(x, y, cx, cy, color);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_ASYNC
+	void gdispFillArea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLAREA);
+		p->fillarea.x = x;
+		p->fillarea.y = y;
+		p->fillarea.cx = cx;
+		p->fillarea.cy = cy;
+		p->fillarea.color = color;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 	
@@ -167,6 +324,9 @@
 	 * @note	If a packed pixel format is used and the width doesn't
 	 *			match a whole number of bytes, the next line will start on a
 	 *			non-byte boundary (no end-of-line padding).
+	 * @note	If GDISP_NEED_ASYNC is defined then the buffer must be static
+	 * 			or at least retained until this call has finished the blit. You can
+	 * 			tell when all graphics drawing is finished by @p gdispIsBusy() going FALSE.
 	 *
 	 * @param[in] x0,y0   The start position
 	 * @param[in] cx,cy   The size of the filled area
@@ -174,10 +334,20 @@
 	 *
 	 * @api
 	 */
-	void gdispBlitArea(coord_t x, coord_t y, coord_t cx, coord_t cy, pixel_t *buffer) {
-		MUTEX_ENTER
-		gdisp_lld_blitarea(x, y, cx, cy, buffer);
-		MUTEX_EXIT
+	void gdispBlitArea(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer) {
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(blitarea)(x, y, cx, cy, buffer);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_ASYNC
+	void gdispBlitArea(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_BLITAREA);
+		p->blitarea.x = x;
+		p->blitarea.y = y;
+		p->blitarea.cx = cx;
+		p->blitarea.cy = cy;
+		p->blitarea.buffer = buffer;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 	
@@ -193,9 +363,18 @@
 	 * @api
 	 */
 	void gdispDrawCircle(coord_t x, coord_t y, coord_t radius, color_t color) {
-		MUTEX_ENTER
-		gdisp_lld_drawcircle(x, y, radius, color);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(drawcircle)(x, y, radius, color);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_CIRCLE && GDISP_NEED_ASYNC
+	void gdispDrawCircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_DRAWCIRCLE);
+		p->drawcircle.x = x;
+		p->drawcircle.y = y;
+		p->drawcircle.radius = radius;
+		p->drawcircle.color = color;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 	
@@ -211,9 +390,18 @@
 	 * @api
 	 */
 	void gdispFillCircle(coord_t x, coord_t y, coord_t radius, color_t color) {
-		MUTEX_ENTER
-		gdisp_lld_fillcircle(x, y, radius, color);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(fillcircle)(x, y, radius, color);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_CIRCLE && GDISP_NEED_ASYNC
+	void gdispFillCircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLCIRCLE);
+		p->fillcircle.x = x;
+		p->fillcircle.y = y;
+		p->fillcircle.radius = radius;
+		p->fillcircle.color = color;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 
@@ -229,9 +417,19 @@
 	 * @api
 	 */
 	void gdispDrawEllipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
-		MUTEX_ENTER
-		gdisp_lld_drawellipse(x, y, a, b, color);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(drawellipse)(x, y, a, b, color);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_ELLIPSE && GDISP_NEED_ASYNC
+	void gdispDrawEllipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_DRAWELLIPSE);
+		p->drawellipse.x = x;
+		p->drawellipse.y = y;
+		p->drawellipse.a = a;
+		p->drawellipse.b = b;
+		p->drawellipse.color = color;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 	
@@ -247,9 +445,19 @@
 	 * @api
 	 */
 	void gdispFillEllipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
-		MUTEX_ENTER
-		gdisp_lld_fillellipse(x, y, a, b, color);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(fillellipse)(x, y, a, b, color);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_ELLIPSE && GDISP_NEED_ASYNC
+	void gdispFillEllipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLELLIPSE);
+		p->fillellipse.x = x;
+		p->fillellipse.y = y;
+		p->fillellipse.a = a;
+		p->fillellipse.b = b;
+		p->fillellipse.color = color;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 
@@ -265,13 +473,53 @@
 	 * @api
 	 */
 	void gdispDrawChar(coord_t x, coord_t y, char c, font_t font, color_t color) {
-		MUTEX_ENTER
-		gdisp_lld_drawchar(x, y, c, font, color);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(drawchar)(x, y, c, font, color);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_TEXT && GDISP_NEED_ASYNC
+	void gdispDrawChar(coord_t x, coord_t y, char c, font_t font, color_t color) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_DRAWCHAR);
+		p->drawchar.x = x;
+		p->drawchar.y = y;
+		p->drawchar.c = c;
+		p->drawchar.font = font;
+		p->drawchar.color = color;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
+	}
+#endif
+
+#if (GDISP_NEED_TEXT && GDISP_NEED_MULTITHREAD) || defined(__DOXYGEN__)
+	/**
+	 * @brief   Draw a text character with a filled background.
+	 * @pre     The GDISP unit must be in powerOn or powerSleep mode.
+	 *
+	 * @param[in] x,y     The position for the text
+	 * @param[in] c       The character to draw
+	 * @param[in] color   The color to use
+	 * @param[in] bgcolor The background color to use
+	 *
+	 * @api
+	 */
+	void gdispFillChar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(fillchar)(x, y, c, font, color, bgcolor);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_TEXT && GDISP_NEED_ASYNC
+	void gdispFillChar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLCHAR);
+		p->fillchar.x = x;
+		p->fillchar.y = y;
+		p->fillchar.c = c;
+		p->fillchar.font = font;
+		p->fillchar.color = color;
+		p->fillchar.bgcolor = bgcolor;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 	
-#if (GDISP_NEED_PIXELREAD && GDISP_NEED_MULTITHREAD) || defined(__DOXYGEN__)
+#if (GDISP_NEED_PIXELREAD && (GDISP_NEED_MULTITHREAD || GDISP_NEED_ASYNC)) || defined(__DOXYGEN__)
 	/**
 	 * @brief   Get the color of a pixel.
 	 * @return  The color of the pixel.
@@ -283,9 +531,10 @@
 	color_t gdispGetPixelColor(coord_t x, coord_t y) {
 		color_t		c;
 
-		MUTEX_ENTER
-		c = gdisp_lld_getpixelcolor(x, y);
-		MUTEX_EXIT
+		/* Always synchronous as it must return a value */
+		chMtxLock(&gdispMutex);
+		c = GDISP_LLD(getpixelcolor)(x, y);
+		chMtxUnlock();
 
 		return c;
 	}
@@ -305,47 +554,66 @@
 	 * @api
 	 */
 	void gdispVerticalScroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
-		MUTEX_ENTER
-		gdisp_lld_verticalscroll(x, y, cx, cy, lines, bgcolor);
-		MUTEX_EXIT
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(verticalscroll)(x, y, cx, cy, lines, bgcolor);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_SCROLL && GDISP_NEED_ASYNC
+	void gdispVerticalScroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_VERTICALSCROLL);
+		p->verticalscroll.x = x;
+		p->verticalscroll.y = y;
+		p->verticalscroll.cx = cx;
+		p->verticalscroll.cy = cy;
+		p->verticalscroll.lines = lines;
+		p->verticalscroll.bgcolor = bgcolor;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
 	}
 #endif
 
-#if (GDISP_NEED_TEXT && GDISP_NEED_MULTITHREAD) || defined(__DOXYGEN__)
-	/**
-	 * @brief   Draw a text character with a filled background.
-	 * @pre     The GDISP unit must be in powerOn or powerSleep mode.
-	 *
-	 * @param[in] x,y     The position for the text
-	 * @param[in] c       The character to draw
-	 * @param[in] color   The color to use
-	 * @param[in] bgcolor The background color to use
-	 *
-	 * @api
-	 */
-	void gdispFillChar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
-		MUTEX_ENTER
-		gdisp_lld_fillchar(x, y, c, font, color, bgcolor);
-		MUTEX_EXIT
-	}
-#endif
-	
 #if (GDISP_NEED_CONTROL && GDISP_NEED_MULTITHREAD) || defined(__DOXYGEN__)
 	/**
 	 * @brief   Set the power mode for the display.
 	 * @pre     The GDISP unit must have been initialised using @p gdispInit().
 	 * @note    Depending on the hardware implementation this function may not
-	 *          support powerSleep. If not powerSleep is treated the same as powerOn.
-	 *          (sleep allows drawing to the display without the display updating).
+	 *          support some codes. They will be ignored.
 	 *
 	 * @param[in] powerMode The power mode to use
 	 *
 	 * @api
 	 */
-	void gdispControl(int what, void *value) {
-		MUTEX_ENTER
-		gdisp_lld_control(what, value);
-		MUTEX_EXIT
+	void gdispControl(unsigned what, void *value) {
+		chMtxLock(&gdispMutex);
+		GDISP_LLD(control)(what, value);
+		chMtxUnlock();
+	}
+#elif GDISP_NEED_CONTROL && GDISP_NEED_ASYNC
+	void gdispControl(unsigned what, void *value) {
+		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_CONTROL);
+		p->control.what = what;
+		p->control.value = value;
+		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
+	}
+#endif
+
+#if (GDISP_NEED_QUERY && (GDISP_NEED_MULTITHREAD || GDISP_NEED_ASYNC)) || defined(__DOXYGEN__)
+	/**
+	 * @brief   Query a property of the display.
+	 * @pre     The GDISP unit must have been initialised using @p gdispInit().
+	 * @note    The result must be typecast to the correct type.
+	 * @note    An uunsupported query will return (void *)-1.
+	 *
+	 * @param[in] what		What to query
+	 *
+	 * @api
+	 */
+	void *gdispQuery(unsigned what) {
+		void *res;
+
+		chMtxLock(&gdispMutex);
+		res = GDISP_LLD(query)(what);
+		chMtxUnlock();
+		return res;
 	}
 #endif
 
@@ -353,44 +621,42 @@
 /* High Level Driver Routines.                                               */
 /*===========================================================================*/
 
-#if GDISP_NEED_MULTITHREAD || defined(__DOXYGEN__)
-	/**
-	 * @brief   Draw a rectangular box.
-	 * @pre     The GDISP unit must be in powerOn or powerSleep mode.
-	 *
-	 * @param[in] x0,y0   The start position
-	 * @param[in] cx,cy   The size of the box (outside dimensions)
-	 * @param[in] color   The color to use
-	 * @param[in] filled  Should the box should be filled
-	 *
-	 * @api
-	 */
-	void gdispDrawBox(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
-		/* No mutex required as we only call high level functions which have their own mutex */
-		coord_t	x1, y1;
+/**
+ * @brief   Draw a rectangular box.
+ * @pre     The GDISP unit must be in powerOn or powerSleep mode.
+ *
+ * @param[in] x0,y0   The start position
+ * @param[in] cx,cy   The size of the box (outside dimensions)
+ * @param[in] color   The color to use
+ * @param[in] filled  Should the box should be filled
+ *
+ * @api
+ */
+void gdispDrawBox(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
+	/* No mutex required as we only call high level functions which have their own mutex */
+	coord_t	x1, y1;
 
-		x1 = x+cx-1;
-		y1 = y+cy-1;
+	x1 = x+cx-1;
+	y1 = y+cy-1;
 
-		if (cx > 2) {
-			if (cy >= 1) {
-				gdisp_lld_drawline(x, y, x1, y, color);
-				if (cy >= 2) {
-					gdisp_lld_drawline(x, y1, x1, y1, color);
-					if (cy > 2) {
-						gdisp_lld_drawline(x, y+1, x, y1-1, color);
-						gdisp_lld_drawline(x1, y+1, x1, y1-1, color);
-					}
+	if (cx > 2) {
+		if (cy >= 1) {
+			gdispDrawLine(x, y, x1, y, color);
+			if (cy >= 2) {
+				gdispDrawLine(x, y1, x1, y1, color);
+				if (cy > 2) {
+					gdispDrawLine(x, y+1, x, y1-1, color);
+					gdispDrawLine(x1, y+1, x1, y1-1, color);
 				}
 			}
-		} else if (cx == 2) {
-			gdisp_lld_drawline(x, y, x, y1, color);
-			gdisp_lld_drawline(x1, y, x1, y1, color);
-		} else if (cx == 1) {
-			gdisp_lld_drawline(x, y, x, y1, color);
 		}
+	} else if (cx == 2) {
+		gdispDrawLine(x, y, x, y1, color);
+		gdispDrawLine(x1, y, x1, y1, color);
+	} else if (cx == 1) {
+		gdispDrawLine(x, y, x, y1, color);
 	}
-#endif
+}
 
 #if GDISP_NEED_TEXT || defined(__DOXYGEN__)
 	/**
@@ -726,4 +992,6 @@
 #endif
 
 #endif /* HAL_USE_GDISP */
+
+#endif /* _GDISP_C */
 /** @} */
diff --git a/halext/templates/gdispXXXXX/gdisp_lld.c b/halext/templates/gdispXXXXX/gdisp_lld.c
index cb465915..81e96894 100644
--- a/halext/templates/gdispXXXXX/gdisp_lld.c
+++ b/halext/templates/gdispXXXXX/gdisp_lld.c
@@ -32,27 +32,17 @@
 
 #if HAL_USE_GDISP || defined(__DOXYGEN__)
 
+/* Include the emulation code for things we don't support */
+#include "gdisp_emulation.c"
+
 /*===========================================================================*/
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
-#ifdef UNUSED
-#elif defined(__GNUC__)
-# define UNUSED(x) UNUSED_ ## x __attribute__((unused))
-#elif defined(__LCLINT__)
-# define UNUSED(x) /*@unused@*/ x
-#else
-# define UNUSED(x) x
-#endif
-
 /*===========================================================================*/
 /* Driver exported variables.                                                */
 /*===========================================================================*/
 
-#if !defined(__DOXYGEN__)
-	GDISPDriver GDISP;
-#endif
-
 /*===========================================================================*/
 /* Driver local variables.                                                   */
 /*===========================================================================*/
@@ -61,7 +51,9 @@
 /* Driver local functions.                                                   */
 /*===========================================================================*/
 
-#include "gdisp_fonts.h"
+#if GDISP_NEED_TEXT
+	#include "gdisp_fonts.h"
+#endif
 
 /* All the board specific code should go in these include file so the driver
  * can be ported to another board just by creating a suitable file.
@@ -88,11 +80,12 @@
 */
 
 /**
- * @brief   Low level GDISP driver initialization.
+ * @brief   Low level GDISP driver initialisation.
+ * @return	TRUE if successful, FALSE on error.
  *
  * @notapi
  */
-void gdisp_lld_init(void) {
+bool_t GDISP_LLD(init)(void) {
 	/* Initialise your display */
 
 	/* Initialise the GDISP structure to match */
@@ -100,6 +93,9 @@ void gdisp_lld_init(void) {
 	GDISP.Height = SCREEN_HEIGHT;
 	GDISP.Orientation = portrait;
 	GDISP.Powermode = powerOn;
+	GDISP.Backlight = 100;
+	GDISP.Contrast = 50;
+	return TRUE;
 }
 
 /**
@@ -111,7 +107,7 @@ void gdisp_lld_init(void) {
  *
  * @notapi
  */
-void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
+void GDISP_LLD(drawpixel)(coord_t x, coord_t y, color_t color) {
 	#if GDISP_NEED_VALIDATION
 		if (x >= GDISP.Width || y >= GDISP.Height) return;
 	#endif
@@ -128,11 +124,11 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	Don't bother coding for obvious similar routines if
 	there is no performance penalty as the emulation software
 	makes a good job of using similar routines.
-		eg. If gdisp_lld_fillarea() is defined there is little
-			point in defining gdisp_lld_clear() unless the
+		eg. If fillarea() is defined there is little
+			point in defining clear() unless the
 			performance bonus is significant.
 	For good performance it is suggested to implement
-		gdisp_lld_fillarea() and gdisp_lld_blitarea().
+		fillarea() and blitarea().
 */
 
 #if GDISP_HARDWARE_CLEARS || defined(__DOXYGEN__)
@@ -144,7 +140,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_clear(color_t color) {
+	void GDISP_LLD(clear)(color_t color) {
 		/* Code here */
 	}
 #endif
@@ -160,7 +156,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawline(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
+	void GDISP_LLD(drawline)(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Need to clip to screen */
 		#endif
@@ -179,7 +175,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillarea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
+	void GDISP_LLD(fillarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			if (cx < 1 || cy < 1 || x >= GDISP.Width || y >= GDISP.Height) return;
 			if (x+cx > GDISP.Width)	cx = GDISP.Width - x;
@@ -200,7 +196,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_blitarea(coord_t x, coord_t y, coord_t cx, coord_t cy, pixel_t *buffer) {
+	void GDISP_LLD(blitarea)(coord_t x, coord_t y, coord_t cx, coord_t cy, const pixel_t *buffer) {
 		#if GDISP_NEED_VALIDATION
 			if (cx < 1 || cy < 1 || x >= GDISP.Width || y >= GDISP.Height) return;
 			if (x+cx > GDISP.Width) return;
@@ -224,7 +220,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(drawcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -245,7 +241,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillcircle(coord_t x, coord_t y, coord_t radius, color_t color) {
+	void GDISP_LLD(fillcircle)(coord_t x, coord_t y, coord_t radius, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -266,7 +262,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(drawellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -287,7 +283,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillellipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
+	void GDISP_LLD(fillellipse)(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -306,7 +302,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_drawchar(coord_t x, coord_t y, char c, font_t font, color_t color) {
+	void GDISP_LLD(drawchar)(coord_t x, coord_t y, char c, font_t font, color_t color) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -326,7 +322,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_fillchar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
+	void GDISP_LLD(fillchar)(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
 		#if GDISP_NEED_VALIDATION
 			/* Code here */
 		#endif
@@ -339,12 +335,13 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 * @brief   Get the color of a particular pixel.
 	 * @note    Optional.
 	 * @note    If x,y is off the screen, the result is undefined.
+	 * @return	The color of the specified pixel.
 	 *
 	 * @param[in] x, y     The start of the text
 	 *
 	 * @notapi
 	 */
-	color_t gdisp_lld_getpixelcolor(coord_t x, coord_t y) {
+	color_t GDISP_LLD(getpixelcolor)(coord_t x, coord_t y) {
 		#if GDISP_NEED_VALIDATION
 			if (x >= GDISP.Width || y >= GDISP.Height) return 0;
 		#endif
@@ -366,7 +363,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_verticalscroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
+	void GDISP_LLD(verticalscroll)(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
 		#if GDISP_NEED_VALIDATION
 			if (cx < 1 || cy < 1 || x >= GDISP.Width || y >= GDISP.Height) return;
 			if (x+cx > GDISP.Width)	cx = GDISP.Width - x;
@@ -376,7 +373,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	}
 #endif
 
-#if GDISP_HARDWARE_CONTROL || defined(__DOXYGEN__)
+#if (GDISP_NEED_CONTROL && GDISP_HARDWARE_CONTROL) || defined(__DOXYGEN__)
 	/**
 	 * @brief   Driver Control
 	 * @detail	Unsupported control codes are ignored.
@@ -396,7 +393,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	 *
 	 * @notapi
 	 */
-	void gdisp_lld_control(int what, void *value) {
+	void GDISP_LLD(control)(unsigned what, void *value) {
 		switch(what) {
 		case GDISP_CONTROL_POWER:
 			if (GDISP.Powermode == (gdisp_powermode_t)value)
@@ -409,7 +406,7 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 					/* 	Code here */
 					/* You may need this ---
 						if (GDISP.Powermode != powerSleep)
-							gdisp_lld_init();
+							GDISP_LLD(init)();
 					*/
 					break;
 				case powerSleep:
@@ -457,5 +454,37 @@ void gdisp_lld_drawpixel(coord_t x, coord_t y, color_t color) {
 	}
 #endif
 
+#if (GDISP_NEED_QUERY && GDISP_HARDWARE_QUERY) || defined(__DOXYGEN__)
+/**
+ * @brief   Query a driver value.
+ * @detail	Typecase the result to the type you want.
+ * @note	GDISP_QUERY_WIDTH			- (coord_t)	Gets the width of the screen
+ * 			GDISP_QUERY_HEIGHT			- (coord_t)	Gets the height of the screen
+ * 			GDISP_QUERY_POWER			- (gdisp_powermode_t) Get the current powermode
+ * 			GDISP_QUERY_ORIENTATION		- (gdisp_orientation_t) Get the current screen orientation
+ * 			GDISP_QUERY_BACKLIGHT		- (coord_t) Get the backlight state (0 to 100)
+ * 			GDISP_QUERY_CONTRAST		- (coord_t) Get the contrast (0 to 100).
+ * 			GDISP_QUERY_LLD				- Low level driver control constants start at
+ * 											this value.
+ *
+ * @param[in] what     What to Query
+ *
+ * @notapi
+ */
+void *GDISP_LLD(query)(unsigned what) {
+	switch(what) {
+	case GDISP_QUERY_WIDTH:			return (void *)(unsigned)GDISP.Width;
+	case GDISP_QUERY_HEIGHT:		return (void *)(unsigned)GDISP.Height;
+	case GDISP_QUERY_POWER:			return (void *)(unsigned)GDISP.Powermode;
+	case GDISP_QUERY_ORIENTATION:	return (void *)(unsigned)GDISP.Orientation;
+	case GDISP_QUERY_BACKLIGHT:		return (void *)(unsigned)GDISP.Backlight;
+	case GDISP_QUERY_CONTRAST:		return (void *)(unsigned)GDISP.Contrast;
+	case GDISP_QUERY_LLD+0:
+		/* Code here */
+	default:						return (void *)-1;
+	}
+}
+#endif
+
 #endif /* HAL_USE_GDISP */
 /** @} */
diff --git a/halext/templates/gdispXXXXX/gdisp_lld_config.h b/halext/templates/gdispXXXXX/gdisp_lld_config.h
index 0caf3f4c..81a6826f 100644
--- a/halext/templates/gdispXXXXX/gdisp_lld_config.h
+++ b/halext/templates/gdispXXXXX/gdisp_lld_config.h
@@ -35,6 +35,9 @@
 /* Driver hardware support.                                                  */
 /*===========================================================================*/
 
+#define GDISP_DRIVER_NAME				"YourDriverName"
+#define GDISP_LLD(x)					gdisp_lld_##x##_YourDriverName
+
 #define GDISP_HARDWARE_LINES			FALSE
 #define GDISP_HARDWARE_CLEARS			FALSE
 #define GDISP_HARDWARE_FILLS			FALSE
@@ -48,22 +51,15 @@
 #define GDISP_HARDWARE_SCROLL			FALSE
 #define GDISP_HARDWARE_PIXELREAD		FALSE
 #define GDISP_HARDWARE_CONTROL			FALSE
+#define GDISP_HARDWARE_QUERY			FALSE
 
 #define GDISP_SOFTWARE_TEXTFILLDRAW		FALSE
 #define GDISP_SOFTWARE_TEXTBLITCOLUMN	FALSE
 
-#define GDISP_PIXELFORMAT_RGB565
+#define GDISP_PIXELFORMAT				GDISP_PIXELFORMAT_RGB565
 #define GDISP_PACKED_PIXELS				FALSE
 #define GDISP_PACKED_LINES				FALSE
 
-/*===========================================================================*/
-/* Extra fields for the GDISPDriver structure                                 */
-/*===========================================================================*/
-
-/*
-#define GDISP_DRIVER_EXT_FIELDS		int abc; int def;
-*/
-
 #endif	/* HAL_USE_GDISP */
 
 #endif	/* _GDISP_LLD_CONFIG_H */