diff --git a/drivers/gdisp/Nokia6610GE12/gdisp_lld.c b/drivers/gdisp/Nokia6610GE12/gdisp_lld.c
index a991a9a6..441ac82d 100644
--- a/drivers/gdisp/Nokia6610GE12/gdisp_lld.c
+++ b/drivers/gdisp/Nokia6610GE12/gdisp_lld.c
@@ -17,17 +17,6 @@
 
 #if GFX_USE_GDISP /*|| defined(__DOXYGEN__)*/
 
-/* Include the emulation code for things we don't support */
-#include "gdisp/lld/emulation.c"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
-/* Controller definitions */
-#include "GE12.h"
-
-/* This controller is only ever used with a 132 x 132 display */
 #if defined(GDISP_SCREEN_HEIGHT)
 	#warning "GDISP: This low level driver does not support setting a screen size. It is being ignored."
 	#undef GDISP_SCREEN_HEIGHT
@@ -36,11 +25,43 @@
 	#warning "GDISP: This low level driver does not support setting a screen size. It is being ignored."
 	#undef GDISP_SCREEN_WIDTH
 #endif
-#define GDISP_SCREEN_HEIGHT		132
-#define GDISP_SCREEN_WIDTH		132
 
-#define GDISP_INITIAL_CONTRAST	38
-#define GDISP_INITIAL_BACKLIGHT	100
+#define GDISP_LLD_DECLARATIONS
+#include "gdisp/lld/gdisp_lld.h"
+#include "gdisp_lld_board.h"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+#include "GE12.h"
+
+#define GDISP_SCAN_LINES			132
+#define GDISP_SLEEP_SIZE			32		/* Sleep mode window lines - this must be 32 on this controller */
+
+// Set parameters if they are not already set
+#ifndef GDISP_SCREEN_HEIGHT
+	#define GDISP_SCREEN_HEIGHT		130
+#endif
+#ifndef GDISP_SCREEN_WIDTH
+	#define GDISP_SCREEN_WIDTH		130
+#endif
+#ifndef GDISP_RAM_X_OFFSET
+	#define GDISP_RAM_X_OFFSET		0		/* Offset in RAM of visible area */
+#endif
+#ifndef GDISP_RAM_Y_OFFSET
+	#define GDISP_RAM_Y_OFFSET		2		/* Offset in RAM of visible area */
+#endif
+#endif
+#ifndef GDISP_SLEEP_POS
+	#define GDISP_SLEEP_POS			((GDISP_SCAN_LINES-GDISP_SLEEP_SIZE)/2 & ~3)
+#endif
+#ifndef GDISP_INITIAL_CONTRAST
+	#define GDISP_INITIAL_CONTRAST	50
+#endif
+#ifndef GDISP_INITIAL_BACKLIGHT
+	#define GDISP_INITIAL_BACKLIGHT	100
+#endif
 
 /*===========================================================================*/
 /* Driver exported variables.                                                */
@@ -50,12 +71,14 @@
 /* Driver local variables.                                                   */
 /*===========================================================================*/
 
+static color_t savecolor;
+
+#define GDISP_FLG_ODDBYTE	(GDISP_FLG_DRIVER<<0)
+
 /*===========================================================================*/
 /* Driver local functions.                                                   */
 /*===========================================================================*/
 
-#include "gdisp_lld_board.h"
-
 // Some macros just to make reading the code easier
 #define delayms(ms)					gfxSleepMilliseconds(ms)
 #define write_data2(d1, d2)			{ write_data(d1); write_data(d2); }
@@ -64,33 +87,11 @@
 #define write_cmd2(cmd, d1, d2)		{ write_cmd(cmd); write_data2(d1, d2); }
 #define write_cmd3(cmd, d1, d2, d3)	{ write_cmd(cmd); write_data3(d1, d2, d3); }
 
-// A very common thing to do.
-// An inline function has been used here incase the parameters have side effects with the internal calculations.
-static inline void setviewport(coord_t x, coord_t y, coord_t cx, coord_t cy) {
-	write_cmd2(CASET, x, x+cx-1);			// Column address set
-	write_cmd2(PASET, y, y+cy-1);			// Page address set
-}
-
-/*===========================================================================*/
-/* Driver interrupt handlers.                                                */
-/*===========================================================================*/
-
 /*===========================================================================*/
 /* Driver exported functions.                                                */
 /*===========================================================================*/
 
-/* ---- Required Routines ---- */
-/*
-	The following 2 routines are required.
-	All other routines are optional.
-*/
-
-/**
- * @brief   Low level GDISP driver initialization.
- *
- * @notapi
- */
-bool_t gdisp_lld_init(void) {
+LLDSPEC bool_t gdisp_lld_init(GDISPDriver *g) {
 	/* Initialise your display */
 	init_board();
 
@@ -100,402 +101,142 @@ bool_t gdisp_lld_init(void) {
 	setpin_reset(FALSE);
 	delayms(20);
 
-	// Get the bus for the following initialisation commands
 	acquire_bus();
-	
-	// UNTESTED
-	#if 1
-		write_cmd(SLEEPOUT);								// Sleep out
-		write_cmd(INVON);									// Inversion on: seems to be required for this controller
-		write_cmd1(COLMOD, 0x03);							// Color Interface Pixel Format - 0x03 = 12 bits-per-pixel
-		write_cmd1(MADCTL, 0xC8);							// Memory access controler - 0xC0 = mirror x and y, reverse rgb
-		write_cmd1(SETCON, GDISP_INITIAL_CONTRAST);			// Write contrast
-		delayms(20);
-		write_cmd(DISPON);									// Display On
-	#else
-		// Alternative
-		write_cmd(SOFTRST);								// Software Reset
-		delayms(20);
-		write_cmd(INITESC);								// Initial escape
-		delayms(20);
-		write_cmd1(REFSET, 0x00);						// Refresh set
-		write_cmd(DISPCTRL);							// Set Display control - really 7 bytes of data
-			write_data(128);								// Set the lenght of one selection term
-			write_data(128);								// Set N inversion -> no N inversion
-			write_data(134);								// Set frame frequence and bias rate -> 2 devision of frequency and 1/8 bias, 1/67 duty, 96x67 size
-			write_data(84);									// Set duty parameter
-			write_data(69);									// Set duty parameter
-			write_data(82);									// Set duty parameter
-			write_data(67);									// Set duty parameter
-		write_cmd(GRAYSCALE0);							// Grey scale 0 position set - really 15 bytes of data
-			write_data(1);									// GCP1 - gray lavel to be output when the RAM data is "0001"
-			write_data(2);									// GCP2 - gray lavel to be output when the RAM data is "0010"
-			write_data(4);									// GCP3 - gray lavel to be output when the RAM data is "0011"
-			write_data(8);									// GCP4 - gray lavel to be output when the RAM data is "0100"
-			write_data(16);									// GCP5 - gray lavel to be output when the RAM data is "0101"
-			write_data(30);									// GCP6 - gray lavel to be output when the RAM data is "0110"
-			write_data(40);									// GCP7 - gray lavel to be output when the RAM data is "0111"
-			write_data(50);									// GCP8 - gray lavel to be output when the RAM data is "1000"
-			write_data(60);									// GCP9 - gray lavel to be output when the RAM data is "1001"
-			write_data(70);									// GCP10 - gray lavel to be output when the RAM data is "1010"
-			write_data(80);									// GCP11 - gray lavel to be output when the RAM data is "1011"
-			write_data(90);									// GCP12 - gray lavel to be output when the RAM data is "1100"
-			write_data(100);								// GCP13 - gray lavel to be output when the RAM data is "1101"
-			write_data(110);								// GCP14 - gray lavel to be output when the RAM data is "1110"
-			write_data(127);								// GCP15 - gray lavel to be output when the RAM data is "1111"
-		write_cmd1(GAMMA, 0x01);						// Gamma curve set - select gray scale - GRAYSCALE 0 or GREYSCALE 1 - Select grey scale 0
-		write_cmd1(COMMONDRV, 0x00);					// Command driver output - Set COM1-COM41 side come first, normal mod
-		write_cmd(NORMALMODE);							// Set Normal mode (my)
-		// write_cmd(INVERSIONOFF);						// Inversion off
-		write_cmd2(COLADDRSET, 0, 131);					// Column address set
-		write_cmd2(PAGEADDRSET, 0, 131);				// Page address set
-		write_cmd1(ACCESSCTRL, 0x40);					// Memory access controler - 0x40 horizontal
-		// write_data(0x20);								// vertical
-		write_cmd1(PWRCTRL, 0x04);						// Power control - Internal resistance, V1OUT -> high power mode, oscilator devision rate
-		write_cmd(SLEEPOUT);							// Sleep out
-		write_cmd(VOLTCTRL);							// Voltage control - voltage control and write contrast define LCD electronic volume
-		// write_data(0x7f);								//  full voltage control
-		// write_data(0x03);								//  must be "1"
-		write_cmd1(CONTRAST, GDISP_INITIAL_CONTRAST);	// Write contrast
-		delayms(20);
-		write_cmd(TEMPGRADIENT);						// Temperature gradient - really 14 bytes of data
-		for(i=0; i<14; i++)
-			write_data(0);
-		write_cmd(BOOSTVON);							// Booster voltage ON
-		write_cmd(DISPLAYON);							// Finally - Display On
-	#endif
-
-	// Release the bus
+	write_cmd(SLEEPOUT);								// Sleep out
+	write_cmd1(COLMOD, 0x03);							// Color Interface Pixel Format - 0x03 = 12 bits-per-pixel
+	write_cmd1(MADCTL, 0x00);							// Memory access controller
+	write_cmd1(SETCON, 127*GDISP_INITIAL_CONTRAST/100-64);			// Write contrast
+	delayms(20);
 	release_bus();
 	
 	/* Turn on the back-light */
 	set_backlight(GDISP_INITIAL_BACKLIGHT);
 
 	/* Initialise the GDISP structure to match */
-	GDISP.Width = GDISP_SCREEN_WIDTH;
-	GDISP.Height = GDISP_SCREEN_HEIGHT;
-	GDISP.Orientation = GDISP_ROTATE_0;
-	GDISP.Powermode = powerOn;
-	GDISP.Backlight = GDISP_INITIAL_BACKLIGHT;
-	GDISP.Contrast = GDISP_INITIAL_CONTRAST;
-	#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-		GDISP.clipx0 = 0;
-		GDISP.clipy0 = 0;
-		GDISP.clipx1 = GDISP.Width;
-		GDISP.clipy1 = GDISP.Height;
-	#endif
+	g->g.Width = GDISP_SCREEN_WIDTH;
+	g->g.Height = GDISP_SCREEN_HEIGHT;
+	g->g.Orientation = GDISP_ROTATE_0;
+	g->g.Powermode = powerOn;
+	g->g.Backlight = GDISP_INITIAL_BACKLIGHT;
+	g->g.Contrast = GDISP_INITIAL_CONTRAST;
 	return TRUE;
 }
 
-/**
- * @brief   Draws a pixel on the display.
- *
- * @param[in] x        X location of the pixel
- * @param[in] y        Y location of the pixel
- * @param[in] color    The color of the pixel
- *
- * @notapi
- */
-void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
-	#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-		if (x < GDISP.clipx0 || y < GDISP.clipy0 || x >= GDISP.clipx1 || y >= GDISP.clipy1) return;
-	#endif
-	acquire_bus();
-	setviewport(x, y, 1, 1);
-	write_cmd3(RAMWR, 0, (color>>8) & 0x0F, color & 0xFF);
-	release_bus();
-}
-
-/* ---- Optional Routines ---- */
-
-#if GDISP_HARDWARE_FILLS || defined(__DOXYGEN__)
-	/**
-	 * @brief   Fill an area with a color.
-	 *
-	 * @param[in] x, y     The start filled area
-	 * @param[in] cx, cy   The width and height to be filled
-	 * @param[in] color    The color of the fill
-	 *
-	 * @notapi
-	 */
-	void gdisp_lld_fill_area(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
-		unsigned i, tuples;
-
-		#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-			if (x < GDISP.clipx0) { cx -= GDISP.clipx0 - x; x = GDISP.clipx0; }
-			if (y < GDISP.clipy0) { cy -= GDISP.clipy0 - y; y = GDISP.clipy0; }
-			if (cx <= 0 || cy <= 0 || x >= GDISP.clipx1 || y >= GDISP.clipy1) return;
-			if (x+cx > GDISP.clipx1)	cx = GDISP.clipx1 - x;
-			if (y+cy > GDISP.clipy1)	cy = GDISP.clipy1 - y;
-		#endif
-
-		tuples = (cx*cy+1)/2;				// With an odd sized area we over-print by one pixel.
-											// This extra pixel is ignored by the controller.
-
+#if GDISP_HARDWARE_STREAM_WRITE
+	LLDSPEC	void gdisp_lld_write_start(GDISPDriver *g) {
 		acquire_bus();
-		setviewport(x, y, cx, cy);
+		write_cmd2(CASET, GDISP_RAM_X_OFFSET+g->p.x, GDISP_RAM_X_OFFSET+g->p.x+g->p.cx-1);			// Column address set
+		write_cmd2(PASET, GDISP_RAM_Y_OFFSET+g->p.y, GDISP_RAM_Y_OFFSET+g->p.y+g->p.cy-1);			// Page address set
 		write_cmd(RAMWR);
-		for(i=0; i < tuples; i++)
-			write_data3(((color >> 4) & 0xFF), (((color << 4) & 0xF0)|((color >> 8) & 0x0F)), (color & 0xFF));
+		g->flags &= ~GDISP_FLG_ODDBYTE;
+	}
+	LLDSPEC	void gdisp_lld_write_color(GDISPDriver *g) {
+		if ((g->flags & GDISP_FLG_ODDBYTE)) {
+			// Write the pair of pixels to the display
+			write_data3(((savecolor >> 4) & 0xFF), (((savecolor << 4) & 0xF0)|((g->p.color >> 8) & 0x0F)), (g->p.color & 0xFF));
+			g->flags &= ~GDISP_FLG_ODDBYTE;
+		} else {
+			savecolor = g->p.color;
+			g->flags |= GDISP_FLG_ODDBYTE;
+		}
+	}
+	LLDSPEC	void gdisp_lld_write_stop(GDISPDriver *g) {
+		if ((g->flags & GDISP_FLG_ODDBYTE)) {
+			write_data2(((savecolor >> 4) & 0xFF), ((savecolor << 4) & 0xF0));
+			write_cmd(NOP);
+		}
 		release_bus();
 	}
 #endif
 
-#if GDISP_HARDWARE_BITFILLS || defined(__DOXYGEN__)
-	/**
-	 * @brief   Fill an area with a bitmap.
-	 *
-	 * @param[in] x, y     The start filled area
-	 * @param[in] cx, cy   The width and height to be filled
-	 * @param[in] srcx, srcy   The bitmap position to start the fill from
-	 * @param[in] srccx    The width of a line in the bitmap.
-	 * @param[in] buffer   The pixels to use to fill the area.
-	 *
-	 * @notapi
-	 */
-	void gdisp_lld_blit_area_ex(coord_t x, coord_t y, coord_t cx, coord_t cy, coord_t srcx, coord_t srcy, coord_t srccx, const pixel_t *buffer) {
-		coord_t endx, endy, lg;
-		color_t	c1, c2;
-		#if GDISP_PACKED_PIXELS
-			coord_t pos;
-			const uint8_t *p;
-		#endif
-
-		#if GDISP_NEED_VALIDATION || GDISP_NEED_CLIP
-			if (x < GDISP.clipx0) { cx -= GDISP.clipx0 - x; srcx += GDISP.clipx0 - x; x = GDISP.clipx0; }
-			if (y < GDISP.clipy0) { cy -= GDISP.clipy0 - y; srcy += GDISP.clipy0 - y; y = GDISP.clipy0; }
-			if (srcx+cx > srccx)		cx = srccx - srcx;
-			if (cx <= 0 || cy <= 0 || x >= GDISP.clipx1 || y >= GDISP.clipy1) return;
-			if (x+cx > GDISP.clipx1)	cx = GDISP.clipx1 - x;
-			if (y+cy > GDISP.clipy1)	cy = GDISP.clipy1 - y;
-		#endif
-
-		/* What are our end points */
-		endx = srcx + cx;
-		endy = y + cy;
-
-		acquire_bus();
-		setviewport(x, y, cx, cy);
-		write_cmd(RAMWR);
-
-		#if !GDISP_PACKED_PIXELS
-			// Although this controller uses packed pixels we support unpacked pixel
-			//  formats in this blit by packing the data as we feed it to the controller.
-			lg = srccx - cx;
-			buffer += srcy * srccx + srcx;
-			x = srcx;
-			while (1) {
-				/* Get a pixel */
-				c1 = *buffer++;
-				if (++x >= endx) {
-					if (++y >= endy) {
-						/* Odd pixel at end */
-						write_data3(0, ((c1 >> 8) & 0x0F), (c1 & 0xFF));
-						break;
-					}
-					x = srcx;
-					buffer += lg;
-				}
-				/* Get the next pixel */
-				c2 = *buffer++;
-				write_data3(((c1 >> 4) & 0xFF), (((c1 << 4) & 0xF0)|((c2 >> 8) & 0x0F)), (c2 & 0xFF));
-				if (++x >= endx) {
-					if (++y >= endy)
-						break;
-					x = srcx;
-					buffer += lg;
-				}
-			}
-
-		#else
-
-			// Although this controller uses packed pixels, we may have to feed it into
-			//  the controller with different packing to the source bitmap
-			#if !GDISP_PACKED_LINES
-				srccx = (srccx + 1) & ~1;
-			#endif
-			pos = srcy*srccx;
-			lg = (srccx - cx)/2*3;
-			p = ((const uint8_t *)buffer) + ((pos+srcx)/2 * 3);
-
-			x = srcx;
-			while (1) {
-				/* Get a pixel */
-				switch((pos+x)&1) {
-				case 0:		c1 = (((color_t)p[0]) << 4)|(((color_t)p[1])>>4);	break;
-				case 1:		c1 = (((color_t)p[1]&0x0F) << 8)|((color_t)p[1]);	break;
-				}
-				if (++x >= endx) {
-					if (++y >= endy) {
-						/* Odd pixel at end */
-						write_data3(0, ((c1 >> 8) & 0x0F), (c1 & 0xFF));
-						break;
-					}
-					x = srcx;
-					p += lg;
-					pos += srccx;
-				}
-				/* Get the next pixel */
-				switch((pos+x)&1) {
-				case 0:		c2 = (((color_t)p[0]) << 4)|(((color_t)p[1])>>4);	break;
-				case 1:		c2 = (((color_t)p[1]&0x0F) << 8)|((color_t)p[1]);	break;
-				}
-				write_data3(((c1 >> 4) & 0xFF), (((c1 << 4) & 0xF0)|((c2 >> 8) & 0x0F)), (c2 & 0xFF));
-				if (++x >= endx) {
-					if (++y >= endy)
-						break;
-					x = srcx;
-					p += lg;
-					pos += srccx;
-				}
-			}
-		#endif
-		release_bus();
-	}
-#endif
-
-#if (GDISP_NEED_PIXELREAD && GDISP_HARDWARE_PIXELREAD)
-	/**
-	 * @brief   Get the color of a particular pixel.
-	 * @note    If x,y is off the screen, the result is undefined.
-	 *
-	 * @param[in] x, y     The start of the text
-	 *
-	 * @notapi
-	 */
-	color_t gdisp_lld_get_pixel_color(coord_t x, coord_t y) {
-		/* NOT IMPLEMENTED */
-		/* Some board hardware might support this in the future.
-		 * The Olimex board doesn't.
-		 */
-	}
-#endif
-
-#if (GDISP_NEED_SCROLL && GDISP_HARDWARE_SCROLL)
-	/**
-	 * @brief   Scroll vertically a section of the screen.
-	 * @note    If x,y + cx,cy is off the screen, the result is undefined.
-	 * @note    If lines is >= cy, it is equivelent to a area fill with bgcolor.
-	 *
-	 * @param[in] x, y     The start of the area to be scrolled
-	 * @param[in] cx, cy   The size of the area to be scrolled
-	 * @param[in] lines    The number of lines to scroll (Can be positive or negative)
-	 * @param[in] bgcolor  The color to fill the newly exposed area.
-	 *
-	 * @notapi
-	 */
-	void gdisp_lld_vertical_scroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
-		/* NOT IMPLEMENTED */
-		/* The hardware seems capable of doing this.
-		 * It is just really complex so we leave it out for now.
-		 */
-	}
-#endif
-
-#if GDISP_HARDWARE_CONTROL || defined(__DOXYGEN__)
-	/**
-	 * @brief   Driver Control
-	 * @details	Unsupported control codes are ignored.
-	 * @note	The value parameter should always be typecast to (void *).
-	 * @note	There are some predefined and some specific to the low level driver.
-	 * @note	GDISP_CONTROL_POWER			- Takes a gdisp_powermode_t
-	 * 			GDISP_CONTROL_ORIENTATION	- Takes a gdisp_orientation_t
-	 * 			GDISP_CONTROL_BACKLIGHT -	 Takes an int from 0 to 100. For a driver
-	 * 											that only supports off/on anything other
-	 * 											than zero is on.
-	 * 			GDISP_CONTROL_CONTRAST		- Takes an int from 0 to 100.
-	 * 			GDISP_CONTROL_LLD			- Low level driver control constants start at
-	 * 											this value.
-	 *
-	 * @param[in] what		What to do.
-	 * @param[in] value		The value to use (always cast to a void *).
-	 *
-	 * @notapi
-	 */
-	void gdisp_lld_control(unsigned what, void *value) {
+#if GDISP_NEED_CONTROL && GDISP_HARDWARE_CONTROL
+	LLDSPEC void gdisp_lld_control(GDISPDriver *g) {
 		/* The hardware is capable of supporting...
-		 * 	GDISP_CONTROL_POWER				- not implemented yet
-		 * 	GDISP_CONTROL_ORIENTATION		- not implemented yet
-		 * 	GDISP_CONTROL_BACKLIGHT			- supported (the OlimexSAM7EX256 board.h currently only implements off/on although PWM is supported by the hardware)
+		 * 	GDISP_CONTROL_POWER				- supported
+		 * 	GDISP_CONTROL_ORIENTATION		- supported
+		 * 	GDISP_CONTROL_BACKLIGHT			- supported
 		 * 	GDISP_CONTROL_CONTRAST			- supported
 		 */
-		switch(what) {
-#if 0
-		// NOT IMPLEMENTED YET
+		switch(g->p.x) {
 		case GDISP_CONTROL_POWER:
-			if (GDISP.Powermode == (gdisp_powermode_t)value)
+			if (g->g.Powermode == (powermode_t)g->p.ptr)
+				return;
+			switch((powermode_t)g->p.ptr) {
+			case powerOff:
+				acquire_bus();
+				write_cmd(SLEEPIN);
+				release_bus();
+				break;
+			case powerOn:
+				acquire_bus();
+				write_cmd(SLEEPOUT);
+				delayms(20);
+				write_cmd(NORON);							// Set Normal mode (my)
+				release_bus();
+				break;
+			case powerSleep:
+				acquire_bus();
+				write_cmd(SLEEPOUT);
+				delayms(20);
+				write_cmd2(PTLAR, GDISP_SLEEP_POS, GDISP_SLEEP_POS+GDISP_SLEEP_SIZE)
+				write_cmd(PTLON);
+				release_bus();
+				break;
+			default:
 				return;
-			switch((gdisp_powermode_t)value) {
-				case powerOff:
-					// 	Code here
-					break;
-				case powerOn:
-					// 	Code here
-					/* You may need this ---
-					 *	if (GDISP.Powermode != powerSleep)
-					 *		gdisp_lld_init();
-					 */
-					break;
-				case powerSleep:
-					/* 	Code here */
-					break;
-				default:
-					return;
 			}
-			GDISP.Powermode = (gdisp_powermode_t)value;
+			g->g.Powermode = (powermode_t)g->p.ptr;
 			return;
-#endif
-#if 0
-		// NOT IMPLEMENTED YET
+
 		case GDISP_CONTROL_ORIENTATION:
-			if (GDISP.Orientation == (gdisp_orientation_t)value)
+			if (g->g.Orientation == (orientation_t)g->p.ptr)
+				return;
+			switch((orientation_t)g->p.ptr) {
+			case GDISP_ROTATE_0:
+				acquire_bus();
+				write_cmd1(MADCTL, 0x00);
+				release_bus();
+				g->g.Height = GDISP_SCREEN_HEIGHT;
+				g->g.Width = GDISP_SCREEN_WIDTH;
+				break;
+			case GDISP_ROTATE_90:
+				acquire_bus();
+				write_cmd1(MADCTL, 0xA0);					// MY, MX, V, LAO, RGB, X, X, X
+				release_bus();
+				g->g.Height = GDISP_SCREEN_WIDTH;
+				g->g.Width = GDISP_SCREEN_HEIGHT;
+				break;
+			case GDISP_ROTATE_180:
+				acquire_bus();
+				write_cmd1(MADCTL, 0xC0);
+				release_bus();
+				g->g.Height = GDISP_SCREEN_HEIGHT;
+				g->g.Width = GDISP_SCREEN_WIDTH;
+				break;
+			case GDISP_ROTATE_270:
+				acquire_bus();
+				write_cmd1(MADCTL, 0x60);
+				release_bus();
+				g->g.Height = GDISP_SCREEN_WIDTH;
+				g->g.Width = GDISP_SCREEN_HEIGHT;
+				break;
+			default:
 				return;
-	//		WriteSpiData(0x48); // no mirror Y (temporary to satisfy Olimex bmptoarray utility)
-	//		WriteSpiData(0xC8); // restore to (mirror x and y, reverse rgb)
-			switch((gdisp_orientation_t)value) {
-				case GDISP_ROTATE_0:
-					// 	Code here
-					GDISP.Height = GDISP_SCREEN_HEIGHT;
-					GDISP.Width = GDISP_SCREEN_WIDTH;
-					break;
-				case GDISP_ROTATE_90:
-					// 	Code here
-					GDISP.Height = GDISP_SCREEN_WIDTH;
-					GDISP.Width = GDISP_SCREEN_HEIGHT;
-					break;
-				case GDISP_ROTATE_180:
-					// 	Code here
-					GDISP.Height = GDISP_SCREEN_HEIGHT;
-					GDISP.Width = GDISP_SCREEN_WIDTH;
-					break;
-				case GDISP_ROTATE_270:
-					// 	Code here
-					GDISP.Height = GDISP_SCREEN_WIDTH;
-					GDISP.Width = GDISP_SCREEN_HEIGHT;
-					break;
-				default:
-					return;
 			}
-			#if GDISP_NEED_CLIP || GDISP_NEED_VALIDATION
-				GDISP.clipx0 = 0;
-				GDISP.clipy0 = 0;
-				GDISP.clipx1 = GDISP.Width;
-				GDISP.clipy1 = GDISP.Height;
-			#endif
-			GDISP.Orientation = (gdisp_orientation_t)value;
+			g->g.Orientation = (orientation_t)g->p.ptr;
 			return;
-#endif
+
 		case GDISP_CONTROL_BACKLIGHT:
-			if ((unsigned)value > 100) value = (void *)100;
-			set_backlight((unsigned)value);
-			GDISP.Backlight = (unsigned)value;
+			if ((unsigned)g->p.ptr > 100) g->p.ptr = (void *)100;
+			set_backlight((unsigned)g->p.ptr);
+			g->g.Backlight = (unsigned)g->p.ptr;
 			return;
 		case GDISP_CONTROL_CONTRAST:
-			if ((unsigned)value > 100) value = (void *)100;
+			if ((unsigned)g->p.ptr > 100) g->p.ptr = (void *)100;
 			acquire_bus();
-			write_cmd1(CONTRAST,(unsigned)value);
+			write_cmd1(CONTRAST,(unsigned)127*g->p.ptr/100-64);
 			release_bus();
-			GDISP.Contrast = (unsigned)value;
+			g->g.Contrast = (unsigned)g->p.ptr;
 			return;
 		}
 	}
diff --git a/drivers/gdisp/Nokia6610GE12/gdisp_lld_board_olimexsam7ex256.h b/drivers/gdisp/Nokia6610GE12/gdisp_lld_board_olimexsam7ex256.h
index ce0f7362..c8215c75 100644
--- a/drivers/gdisp/Nokia6610GE12/gdisp_lld_board_olimexsam7ex256.h
+++ b/drivers/gdisp/Nokia6610GE12/gdisp_lld_board_olimexsam7ex256.h
@@ -1,12 +1,12 @@
-/*
- * This file is subject to the terms of the GFX License. If a copy of
- * the license was not distributed with this file, you can obtain one at:
- *
- *              http://ugfx.org/license.html
- */
+/*
+ * This file is subject to the terms of the GFX License. If a copy of
+ * the license was not distributed with this file, you can obtain one at:
+ *
+ *              http://ugfx.org/license.html
+ */
 
 /**
- * @file    drivers/gdisp/Nokia6610GE12/gdisp_lld_board_olimexsam7ex256.h
+ * @file    drivers/gdisp/Nokia6610GE8/gdisp_lld_board_olimexsam7ex256.h
  * @brief   GDISP Graphic Driver subsystem board interface for the Olimex SAM7-EX256 board.
  *
  * @addtogroup GDISP
@@ -16,14 +16,46 @@
 #ifndef _GDISP_LLD_BOARD_H
 #define _GDISP_LLD_BOARD_H
 
+/*
+ * Set various display properties. These properties mostly depend on the exact controller chip you get.
+ * The defaults should work for most controllers.
+ */
+//#define GDISP_SCREEN_HEIGHT				130		// The visible display height
+//#define GDISP_SCREEN_WIDTH				130		// The visible display width
+//#define GDISP_RAM_X_OFFSET				0		// The x offset of the visible area
+//#define GDISP_RAM_Y_OFFSET				2		// The y offset of the visible area
+//#define GDISP_SLEEP_POS					50		// The position of the sleep mode partial display
+//#define GDISP_INITIAL_CONTRAST			50		// The initial contrast percentage
+//#define GDISP_INITIAL_BACKLIGHT			100		// The initial backlight percentage
+
 // ******************************************************
 // Pointers to AT91SAM7X256 peripheral data structures
 // ******************************************************
-volatile AT91PS_PIO pPIOA = AT91C_BASE_PIOA;
-volatile AT91PS_PIO pPIOB = AT91C_BASE_PIOB;
-volatile AT91PS_SPI pSPI = AT91C_BASE_SPI0;
-volatile AT91PS_PMC pPMC = AT91C_BASE_PMC;
-volatile AT91PS_PDC pPDC = AT91C_BASE_PDC_SPI0;
+static volatile AT91PS_PIO pPIOA = AT91C_BASE_PIOA;
+static volatile AT91PS_PIO pPIOB = AT91C_BASE_PIOB;
+static volatile AT91PS_SPI pSPI = AT91C_BASE_SPI0;
+static volatile AT91PS_PMC pPMC = AT91C_BASE_PMC;
+static volatile AT91PS_PDC pPDC = AT91C_BASE_PDC_SPI0;
+
+/* The PWM backlight control is non-linear on this board.
+ * We pick values here that make it look a bit more linear.
+ */
+#define PWM_TOP_VALUE		500
+#define PWM_BOTTOM_VALUE	200
+
+#define PWM_VALUE(x)	(PWM_BOTTOM_VALUE+(PWM_TOP_VALUE-PWM_BOTTOM_VALUE)*(x)/100)
+
+/* PWM configuration structure. The LCD Backlight is on PWM1/PB20 ie PWM2/PIN1 in ChibiOS speak */
+static const PWMConfig pwmcfg = {
+  1000000,		/* 1 MHz PWM clock frequency. Ignored as we are using PWM_MCK_DIV_n */
+  1000,			/* PWM period is 1000 cycles. */
+  NULL,
+  {
+   {PWM_MCK_DIV_1 | PWM_OUTPUT_ACTIVE_HIGH | PWM_OUTPUT_PIN1 | PWM_DISABLEPULLUP_PIN1, NULL},
+  },
+};
+
+static bool_t pwmRunning = FALSE;
 
 /**
  * @brief   Initialise the board for the display.
@@ -78,14 +110,14 @@ static inline void init_board(void) {
 	pPMC->PMC_PCER = 1 << AT91C_ID_SPI0;
 
 	// Fixed mode
-	pSPI->SPI_CR      = 0x81;               //SPI Enable, Sowtware reset
+	pSPI->SPI_CR      = 0x81;               //SPI Enable, Software reset
 	pSPI->SPI_CR      = 0x01;               //SPI Enable
+	pSPI->SPI_MR      = 0xE0011;			//Master mode, fixed select, disable decoder, PCS=1110
+	pSPI->SPI_CSR[0]  = 0x01010311;			//9bit, CPOL=1, ClockPhase=0, SCLK = 48Mhz/3 = 16MHz
 
-	//pSPI->SPI_MR      = 0xE0019;            //Master mode, fixed select, disable decoder, FDIV=1 (MCK), PCS=1110
-	pSPI->SPI_MR      = 0xE0011;          //Master mode, fixed select, disable decoder, FDIV=0 (MCK), PCS=1110
-
-	//pSPI->SPI_CSR[0]  = 0x01010C11;           //9bit, CPOL=1, ClockPhase=0, SCLK = 48Mhz/32*12 = 125kHz
-	pSPI->SPI_CSR[0]  = 0x01010311;        //9bit, CPOL=1, ClockPhase=0, SCLK = 48Mhz/8 = 6MHz if using commented MR line above
+	/* Display backlight control at 100% */
+	pwmRunning = FALSE;
+	palSetPad(IOPORT2, PIOB_LCD_BL);
 }
 
 /**
@@ -113,10 +145,28 @@ static inline void setpin_reset(bool_t state) {
  * @notapi
  */
 static inline void set_backlight(uint8_t percent) {
-	if (percent)
+	if (percent == 100) {
+		/* Turn the pin on - No PWM */
+		if (pwmRunning) {
+			pwmStop(&PWMD2);
+			pwmRunning = FALSE;
+		}
 		palSetPad(IOPORT2, PIOB_LCD_BL);
-	else
+	} else if (percent == 0) {
+		/* Turn the pin off - No PWM */
+		if (pwmRunning) {
+			pwmStop(&PWMD2);
+			pwmRunning = FALSE;
+		}
 		palClearPad(IOPORT2, PIOB_LCD_BL);
+	} else {
+		/* Use the PWM */
+		if (!pwmRunning) {
+			pwmStart(&PWMD2, &pwmcfg);
+			pwmRunning = TRUE;
+		}
+		pwmEnableChannel(&PWMD2, 0, PWM_VALUE(percent));
+	}
 }
 
 /**
diff --git a/drivers/gdisp/Nokia6610GE12/gdisp_lld_board_template.h b/drivers/gdisp/Nokia6610GE12/gdisp_lld_board_template.h
index a1fa6050..d3594434 100644
--- a/drivers/gdisp/Nokia6610GE12/gdisp_lld_board_template.h
+++ b/drivers/gdisp/Nokia6610GE12/gdisp_lld_board_template.h
@@ -6,8 +6,8 @@
  */
 
 /**
- * @file    drivers/gdisp/Nokia6610GE8/gdisp_lld_board_template.h
- * @brief   GDISP Graphic Driver subsystem board interface for the Nokia6610 GE8 display.
+ * @file    drivers/gdisp/Nokia6610GE12/gdisp_lld_board_template.h
+ * @brief   GDISP Graphic Driver subsystem board interface for the Nokia6610 GE12 display.
  *
  * @addtogroup GDISP
  * @{
@@ -16,6 +16,18 @@
 #ifndef _GDISP_LLD_BOARD_H
 #define _GDISP_LLD_BOARD_H
 
+/*
+ * Set various display properties. These properties mostly depend on the exact controller chip you get.
+ * The defaults should work for most controllers.
+ */
+//#define GDISP_SCREEN_HEIGHT				130		// The visible display height
+//#define GDISP_SCREEN_WIDTH				130		// The visible display width
+//#define GDISP_RAM_X_OFFSET				0		// The x offset of the visible area
+//#define GDISP_RAM_Y_OFFSET				2		// The y offset of the visible area
+//#define GDISP_SLEEP_POS					50		// The position of the sleep mode partial display
+//#define GDISP_INITIAL_CONTRAST			50		// The initial contrast percentage
+//#define GDISP_INITIAL_BACKLIGHT			100		// The initial backlight percentage
+
 /**
  * @brief   Initialise the board for the display.
  * @notes	Performs the following functions:
@@ -95,7 +107,7 @@ static inline void write_data(uint16_t data) {
 
 }
 
-#if GDISP_HARDWARE_READPIXEL || GDISP_HARDWARE_SCROLL || defined(__DOXYGEN__)
+#if GDISP_HARDWARE_READPIXEL || GDISP_HARDWARE_SCROLL
 /**
  * @brief   Read data from the lcd.
  *
diff --git a/drivers/gdisp/Nokia6610GE12/gdisp_lld_config.h b/drivers/gdisp/Nokia6610GE12/gdisp_lld_config.h
index 57b8b916..edbf3290 100644
--- a/drivers/gdisp/Nokia6610GE12/gdisp_lld_config.h
+++ b/drivers/gdisp/Nokia6610GE12/gdisp_lld_config.h
@@ -23,13 +23,10 @@
 /*===========================================================================*/
 
 #define GDISP_DRIVER_NAME				"Nokia6610GE12"
-
-#define GDISP_HARDWARE_FILLS			TRUE
-#define GDISP_HARDWARE_BITFILLS			TRUE
-#define GDISP_HARDWARE_CONTROL			TRUE
-
-#define GDISP_SOFTWARE_TEXTFILLDRAW		FALSE
-#define GDISP_SOFTWARE_TEXTBLITCOLUMN	FALSE
+#define GDISP_DRIVER_STRUCT				GDISP_Nokia6610GE12
+
+#define GDISP_HARDWARE_CONTROL			TRUE
+#define GDISP_HARDWARE_STREAM_WRITE		TRUE
 
 #define GDISP_PIXELFORMAT				GDISP_PIXELFORMAT_RGB444
 /* This driver supports both packed and unpacked pixel formats and line formats.
diff --git a/drivers/gdisp/Nokia6610GE8/gdisp_lld.c b/drivers/gdisp/Nokia6610GE8/gdisp_lld.c
index ce0400d1..3f535369 100644
--- a/drivers/gdisp/Nokia6610GE8/gdisp_lld.c
+++ b/drivers/gdisp/Nokia6610GE8/gdisp_lld.c
@@ -518,29 +518,29 @@ LLDSPEC bool_t gdisp_lld_init(GDISPDriver *g) {
 					return;
 				acquire_bus();
 				switch((orientation_t)g->p.ptr) {
-					case GDISP_ROTATE_0:
-						write_cmd3(DATCTL, 0x00, 0x00, 0x02);	// P1: page normal, column normal, scan in column direction
-						g->g.Height = GDISP_SCREEN_HEIGHT;
-						g->g.Width = GDISP_SCREEN_WIDTH;
-						break;
-					case GDISP_ROTATE_90:
-						write_cmd3(DATCTL, 0x05, 0x00, 0x02);	// P1: page reverse, column normal, scan in page direction
-						g->g.Height = GDISP_SCREEN_WIDTH;
-						g->g.Width = GDISP_SCREEN_HEIGHT;
-						break;
-					case GDISP_ROTATE_180:
-						write_cmd3(DATCTL, 0x03, 0x00, 0x02);	// P1: page reverse, column reverse, scan in column direction
-						g->g.Height = GDISP_SCREEN_HEIGHT;
-						g->g.Width = GDISP_SCREEN_WIDTH;
-						break;
-					case GDISP_ROTATE_270:
-						write_cmd3(DATCTL, 0x06, 0x00, 0x02);	// P1: page normal, column reverse, scan in page direction
-						g->g.Height = GDISP_SCREEN_WIDTH;
-						g->g.Width = GDISP_SCREEN_HEIGHT;
-						break;
-					default:
-						release_bus();
-						return;
+				case GDISP_ROTATE_0:
+					write_cmd3(DATCTL, 0x00, 0x00, 0x02);	// P1: page normal, column normal, scan in column direction
+					g->g.Height = GDISP_SCREEN_HEIGHT;
+					g->g.Width = GDISP_SCREEN_WIDTH;
+					break;
+				case GDISP_ROTATE_90:
+					write_cmd3(DATCTL, 0x05, 0x00, 0x02);	// P1: page reverse, column normal, scan in page direction
+					g->g.Height = GDISP_SCREEN_WIDTH;
+					g->g.Width = GDISP_SCREEN_HEIGHT;
+					break;
+				case GDISP_ROTATE_180:
+					write_cmd3(DATCTL, 0x03, 0x00, 0x02);	// P1: page reverse, column reverse, scan in column direction
+					g->g.Height = GDISP_SCREEN_HEIGHT;
+					g->g.Width = GDISP_SCREEN_WIDTH;
+					break;
+				case GDISP_ROTATE_270:
+					write_cmd3(DATCTL, 0x06, 0x00, 0x02);	// P1: page normal, column reverse, scan in page direction
+					g->g.Height = GDISP_SCREEN_WIDTH;
+					g->g.Width = GDISP_SCREEN_HEIGHT;
+					break;
+				default:
+					release_bus();
+					return;
 				}
 				release_bus();
 				g->g.Orientation = (orientation_t)g->p.ptr;