diff --git a/drivers/gdisp/ILI9320/gdisp_lld.c b/drivers/gdisp/ILI9320/gdisp_lld.c
index 0cb5542f..f4ff05d5 100644
--- a/drivers/gdisp/ILI9320/gdisp_lld.c
+++ b/drivers/gdisp/ILI9320/gdisp_lld.c
@@ -460,16 +460,20 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 					return;
 				switch((gdisp_powermode_t)value) {
 					case powerOff:
+						acquire_bus();
 						lld_lcdWriteReg(0x0007, 0x0000);
 						lld_lcdWriteReg(0x0010, 0x0000);
 						lld_lcdWriteReg(0x0011, 0x0000);
 						lld_lcdWriteReg(0x0012, 0x0000);
 						lld_lcdWriteReg(0x0013, 0x0000);
+						release_bus();
+
 						gdisp_lld_backlight(0);
 						break;
 			
 					case powerOn:
 						//*************Power On sequence ******************//
+						acquire_bus();
 						lld_lcdWriteReg(0x0010, 0x0000); /* SAP, BT[3:0], AP, DSTB, SLP, STB */
 						lld_lcdWriteReg(0x0011, 0x0000); /* DC1[2:0], DC0[2:0], VC[2:0] */
 						lld_lcdWriteReg(0x0012, 0x0000); /* VREG1OUT voltage */
@@ -483,13 +487,16 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 						lld_lcdWriteReg(0x0013, 0x0E00); /* VDV[4:0] for VCOM amplitude */
 						lld_lcdWriteReg(0x0029, 0x0009); /* VCM[4:0] for VCOMH */
 						lld_lcdDelay(500);
-						lld_lcdWriteReg(0x0007, 0x0173); /* 262K color and display ON */	
+						lld_lcdWriteReg(0x0007, 0x0173); /* 262K color and display ON */
+						release_bus();
+
 						gdisp_lld_backlight(GDISP.Backlight);
 						if(GDISP.Powermode != powerSleep || GDISP.Powermode != powerDeepSleep)
 							gdisp_lld_init();
 						break;
 	
 					case powerSleep:
+						acquire_bus();
 	             		lld_lcdWriteReg(0x0007, 0x0000); /* display OFF */
 	                   	lld_lcdWriteReg(0x0010, 0x0000); /* SAP, BT[3:0], APE, AP, DSTB, SLP */
 	                  	lld_lcdWriteReg(0x0011, 0x0000); /* DC1[2:0], DC0[2:0], VC[2:0] */
@@ -497,10 +504,13 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 	                  	lld_lcdWriteReg(0x0013, 0x0000); /* VDV[4:0] for VCOM amplitude */
 	                  	lld_lcdDelay(2000); /* Dis-charge capacitor power voltage */
 	                   	lld_lcdWriteReg(0x0010, 0x0002); /* SAP, BT[3:0], APE, AP, DSTB, SLP */				
+						release_bus();
+
 						gdisp_lld_backlight(0);
 						break;
 
 					case powerDeepSleep:
+						acquire_bus();
 					    lld_lcdWriteReg(0x0007, 0x0000); /* display OFF */
 					    lld_lcdWriteReg(0x0010, 0x0000); /* SAP, BT[3:0], APE, AP, DSTB, SLP */
 					    lld_lcdWriteReg(0x0011, 0x0000); /* DC1[2:0], DC0[2:0], VC[2:0] */
@@ -508,6 +518,8 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 	   					lld_lcdWriteReg(0x0013, 0x0000); /* VDV[4:0] for VCOM amplitude */
 	   					lld_lcdDelay(2000); /* Dis-charge capacitor power voltage */
 	  					lld_lcdWriteReg(0x0010, 0x0004); /* SAP, BT[3:0], APE, AP, DSTB, SLP */
+	  					release_bus();
+
 						gdisp_lld_backlight(0);
 						break;
 
@@ -522,33 +534,45 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 					return;
 				switch((gdisp_orientation_t)value) {
 					case GDISP_ROTATE_0:
+						acquire_bus();
 						lld_lcdWriteReg(0x0001, 0x0100);
 						lld_lcdWriteReg(0x0003, 0x1038);
 						lld_lcdWriteReg(0x0060, 0x2700);
+						release_bus();
+
 						GDISP.Height = GDISP_SCREEN_HEIGHT;
 						GDISP.Width = GDISP_SCREEN_WIDTH;
 						break;
 
 					case GDISP_ROTATE_90:
+						acquire_bus();
 						lld_lcdWriteReg(0x0001, 0x0100);
 						lld_lcdWriteReg(0x0003, 0x1030);
 						lld_lcdWriteReg(0x0060, 0x2700);
+						release_bus();
+
 						GDISP.Height = GDISP_SCREEN_WIDTH;
 						GDISP.Width = GDISP_SCREEN_HEIGHT;
 						break;
 			
 					case GDISP_ROTATE_180:
+						acquire_bus();
 						lld_lcdWriteReg(0x0001, 0x0000);
 						lld_lcdWriteReg(0x0003, 0x1030);
 						lld_lcdWriteReg(0x0060, 0x2700);
+						release_bus();
+
 						GDISP.Height = GDISP_SCREEN_HEIGHT;
 						GDISP.Width = GDISP_SCREEN_WIDTH;
 						break;
 		
 					case GDISP_ROTATE_270:
+						acquire_bus();
 						lld_lcdWriteReg(0x0001, 0x0000);
 						lld_lcdWriteReg(0x0003, 0x1038);
 						lld_lcdWriteReg(0x0060, 0xA700);
+						release_bus();
+
 						GDISP.Height = GDISP_SCREEN_WIDTH;
 						GDISP.Width = GDISP_SCREEN_HEIGHT;
 						break;
diff --git a/drivers/gdisp/ILI9325/gdisp_lld.c b/drivers/gdisp/ILI9325/gdisp_lld.c
index 62508d87..88dd4ade 100644
--- a/drivers/gdisp/ILI9325/gdisp_lld.c
+++ b/drivers/gdisp/ILI9325/gdisp_lld.c
@@ -456,16 +456,20 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 					return;
 				switch((gdisp_powermode_t)value) {
 					case powerOff:
+						acquire_bus();
 						lld_lcdWriteReg(0x0007, 0x0000);
 						lld_lcdWriteReg(0x0010, 0x0000);
 						lld_lcdWriteReg(0x0011, 0x0000);
 						lld_lcdWriteReg(0x0012, 0x0000);
 						lld_lcdWriteReg(0x0013, 0x0000);
+						release_bus();
+
 						gdisp_lld_backlight(0);
 						break;
 			
 					case powerOn:
 						//*************Power On sequence ******************//
+						acquire_bus();
 						lld_lcdWriteReg(0x0010, 0x0000); /* SAP, BT[3:0], AP, DSTB, SLP, STB */
 						lld_lcdWriteReg(0x0011, 0x0000); /* DC1[2:0], DC0[2:0], VC[2:0] */
 						lld_lcdWriteReg(0x0012, 0x0000); /* VREG1OUT voltage */
@@ -479,24 +483,30 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 						lld_lcdWriteReg(0x0013, 0x0E00); /* VDV[4:0] for VCOM amplitude */
 						lld_lcdWriteReg(0x0029, 0x0009); /* VCM[4:0] for VCOMH */
 						lld_lcdDelay(500);
-						lld_lcdWriteReg(0x0007, 0x0173); /* 262K color and display ON */	
+						lld_lcdWriteReg(0x0007, 0x0173); /* 262K color and display ON */
+						release_bus();
+
 						gdisp_lld_backlight(GDISP.Backlight);
 						if(GDISP.Powermode != powerSleep || GDISP.Powermode != powerDeepSleep)
 							gdisp_lld_init();
 						break;
 	
 					case powerSleep:
+						acquire_bus();
 	             		lld_lcdWriteReg(0x0007, 0x0000); /* display OFF */
 	                   	lld_lcdWriteReg(0x0010, 0x0000); /* SAP, BT[3:0], APE, AP, DSTB, SLP */
 	                  	lld_lcdWriteReg(0x0011, 0x0000); /* DC1[2:0], DC0[2:0], VC[2:0] */
 	                  	lld_lcdWriteReg(0x0012, 0x0000); /* VREG1OUT voltage */
 	                  	lld_lcdWriteReg(0x0013, 0x0000); /* VDV[4:0] for VCOM amplitude */
 	                  	lld_lcdDelay(2000); /* Dis-charge capacitor power voltage */
-	                   	lld_lcdWriteReg(0x0010, 0x0002); /* SAP, BT[3:0], APE, AP, DSTB, SLP */				
+	                   	lld_lcdWriteReg(0x0010, 0x0002); /* SAP, BT[3:0], APE, AP, DSTB, SLP */
+	                  	release_bus();
+
 						gdisp_lld_backlight(0);
 						break;
 
 					case powerDeepSleep:
+						acquire_bus();
 					    lld_lcdWriteReg(0x0007, 0x0000); /* display OFF */
 					    lld_lcdWriteReg(0x0010, 0x0000); /* SAP, BT[3:0], APE, AP, DSTB, SLP */
 					    lld_lcdWriteReg(0x0011, 0x0000); /* DC1[2:0], DC0[2:0], VC[2:0] */
@@ -504,6 +514,8 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 	   					lld_lcdWriteReg(0x0013, 0x0000); /* VDV[4:0] for VCOM amplitude */
 	   					lld_lcdDelay(2000); /* Dis-charge capacitor power voltage */
 	  					lld_lcdWriteReg(0x0010, 0x0004); /* SAP, BT[3:0], APE, AP, DSTB, SLP */
+	  					release_bus();
+
 						gdisp_lld_backlight(0);
 						break;
 
@@ -518,33 +530,45 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 					return;
 				switch((gdisp_orientation_t)value) {
 					case GDISP_ROTATE_0:
+						acquire_bus();
 						lld_lcdWriteReg(0x0001, 0x0100);
 						lld_lcdWriteReg(0x0003, 0x1038);
 						lld_lcdWriteReg(0x0060, 0x2700);
+						release_bus();
+
 						GDISP.Height = GDISP_SCREEN_HEIGHT;
 						GDISP.Width = GDISP_SCREEN_WIDTH;
 						break;
 
 					case GDISP_ROTATE_90:
+						acquire_bus();
 						lld_lcdWriteReg(0x0001, 0x0000);
 						lld_lcdWriteReg(0x0003, 0x1030);
 						lld_lcdWriteReg(0x0060, 0x2700);
+						release_bus();
+
 						GDISP.Height = GDISP_SCREEN_WIDTH;
 						GDISP.Width = GDISP_SCREEN_HEIGHT;
 						break;
 			
 					case GDISP_ROTATE_180:
+						acquire_bus();
 						lld_lcdWriteReg(0x0001, 0x0000);
 						lld_lcdWriteReg(0x0003, 0x1038);
 						lld_lcdWriteReg(0x0060, 0xa700);
+						release_bus();
+
 						GDISP.Height = GDISP_SCREEN_HEIGHT;
 						GDISP.Width = GDISP_SCREEN_WIDTH;
 						break;
 		
 					case GDISP_ROTATE_270:
+						acquire_bus();
 						lld_lcdWriteReg(0x0001, 0x0100);
 						lld_lcdWriteReg(0x0003, 0x1030);
 						lld_lcdWriteReg(0x0060, 0xA700);
+						release_bus();
+
 						GDISP.Height = GDISP_SCREEN_WIDTH;
 						GDISP.Width = GDISP_SCREEN_HEIGHT;
 						break;
diff --git a/drivers/gdisp/S6D1121/gdisp_lld.c b/drivers/gdisp/S6D1121/gdisp_lld.c
index 982e788b..671b8f86 100644
--- a/drivers/gdisp/S6D1121/gdisp_lld.c
+++ b/drivers/gdisp/S6D1121/gdisp_lld.c
@@ -524,26 +524,38 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 				return;
 			switch((gdisp_orientation_t)value) {
 			case GDISP_ROTATE_0:
+				acquire_bus();
 				write_reg(0x0001,0x0127);
 				write_reg(0x03, 0b0011);
+				release_bus();
+
 				GDISP.Height = GDISP_SCREEN_HEIGHT;
 				GDISP.Width = GDISP_SCREEN_WIDTH;
 				break;
 			case GDISP_ROTATE_90:
+				acquire_bus();
 				write_reg(0x0001,0x0027);
 				write_reg(0x0003, 0b1011);
+				release_bus();
+
 				GDISP.Height = GDISP_SCREEN_WIDTH;
 				GDISP.Width = GDISP_SCREEN_HEIGHT;
 				break;
 			case GDISP_ROTATE_180:
+				acquire_bus();
 				write_reg(0x0001,0x0127);
 				write_reg(0x0003, 0b0000);
+				release_bus();
+
 				GDISP.Height = GDISP_SCREEN_HEIGHT;
 				GDISP.Width = GDISP_SCREEN_WIDTH;
 				break;
 			case GDISP_ROTATE_270:
+				acquire_bus();
 				write_reg(0x0001,0x0027);
 				write_reg(0x0003, 0b1000);
+				release_bus();
+
 				GDISP.Height = GDISP_SCREEN_WIDTH;
 				GDISP.Width = GDISP_SCREEN_HEIGHT;
 				break;
diff --git a/drivers/gdisp/SSD1289/gdisp_lld.c b/drivers/gdisp/SSD1289/gdisp_lld.c
index e2dd79e0..f6531f2c 100644
--- a/drivers/gdisp/SSD1289/gdisp_lld.c
+++ b/drivers/gdisp/SSD1289/gdisp_lld.c
@@ -429,7 +429,7 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 		abslines = lines < 0 ? -lines : lines;
 
 		acquire_bus();
-		if (abslines >= cy) {
+		if ((coord_t)abslines >= cy) {
 			abslines = cy;
 			gap = 0;
 		} else {
@@ -447,20 +447,20 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 				set_viewport(x, row0, cx, 1);
 				stream_start();
 				j = read_data();			// dummy read
-				for (j = 0; j < cx; j++)
+				for (j = 0; (coord_t)j < cx; j++)
 					buf[j] = read_data();
 				stream_stop();
 
 				set_viewport(x, row1, cx, 1);
 				stream_start();
-				for (j = 0; j < cx; j++)
+				for (j = 0; (coord_t)j < cx; j++)
 					write_data(buf[j]);
 				stream_stop();
 			}
 		}
 
 		/* fill the remaining gap */
-		set_viewport(x, lines > 0 ? (y+gap) : y, cx, abslines);
+		set_viewport(x, lines > 0 ? (y+(coord_t)gap) : y, cx, abslines);
 		stream_start();
 		gap = cx*abslines;
 		for(i = 0; i < gap; i++) write_data(bgcolor);
diff --git a/drivers/gdisp/SSD2119/gdisp_lld.c b/drivers/gdisp/SSD2119/gdisp_lld.c
index b8884e0f..a7ad7b1a 100644
--- a/drivers/gdisp/SSD2119/gdisp_lld.c
+++ b/drivers/gdisp/SSD2119/gdisp_lld.c
@@ -472,7 +472,7 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 		abslines = lines < 0 ? -lines : lines;
 
 		acquire_bus();
-		if (abslines >= cy) {
+		if ((coord_t)abslines >= cy) {
 			abslines = cy;
 			gap = 0;
 		} else {
@@ -490,20 +490,20 @@ void gdisp_lld_draw_pixel(coord_t x, coord_t y, color_t color) {
 				set_viewport(x, row0, cx, 1);
 				stream_start();
 				j = read_data();			// dummy read
-				for (j = 0; j < cx; j++)
+				for (j = 0; (coord_t)j < cx; j++)
 					buf[j] = read_data();
 				stream_stop();
 
 				set_viewport(x, row1, cx, 1);
 				stream_start();
-				for (j = 0; j < cx; j++)
+				for (j = 0; (coord_t)j < cx; j++)
 					write_data(buf[j]);
 				stream_stop();
 			}
 		}
 
 		/* fill the remaining gap */
-		set_viewport(x, lines > 0 ? (y+gap) : y, cx, abslines);
+		set_viewport(x, lines > 0 ? (y+(coord_t)gap) : y, cx, abslines);
 		stream_start();
 		gap = cx*abslines;
 		for(i = 0; i < gap; i++) write_data(bgcolor);
diff --git a/drivers/ginput/touch/STMPE811/ginput_lld_mouse.c b/drivers/ginput/touch/STMPE811/ginput_lld_mouse.c
index bacb1f1f..696795cc 100644
--- a/drivers/ginput/touch/STMPE811/ginput_lld_mouse.c
+++ b/drivers/ginput/touch/STMPE811/ginput_lld_mouse.c
@@ -1,144 +1,183 @@
-/*
-    ChibiOS/GFX - Copyright (C) 2012, 2013
-                 Joel Bodenmann aka Tectu <joel@unormal.org>
-
-    This file is part of ChibiOS/GFX.
-
-    ChibiOS/GFX 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/GFX 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    drivers/ginput/touch/STMPE811/ginput_lld_mouse.c
- * @brief   GINPUT Touch low level driver source for the STMPE811.
- *
- * @defgroup Mouse Mouse
- * @ingroup GINPUT
- * @{
- */
-
-#include "ch.h"
-#include "hal.h"
-#include "gfx.h"
-
-#include "stmpe811.h"
-
-#if (GFX_USE_GINPUT && GINPUT_NEED_MOUSE) /*|| defined(__DOXYGEN__)*/
-
-#include "ginput/lld/mouse.h"
-
-#if defined(GINPUT_MOUSE_USE_CUSTOM_BOARD) && GINPUT_MOUSE_USE_CUSTOM_BOARD
-	#include "ginput_lld_mouse_board.h"
-#elif defined(BOARD_EMBEST_DMSTF4BB)
-	#include "ginput_lld_mouse_board_embest_dmstf4bb.h"
-#else
-	#include "ginput_lld_mouse_board_example.h"
-#endif
-
-static uint16_t sampleBuf[7];
-static coord_t	lastx, lasty;
-
-/**
- * @brief   7-point median filtering code for touch samples
- *
- * @note    This is an internally used routine only.
- *
- * @notapi
- */
-static void filter(void) {
-	uint16_t temp;
-	int i,j;
-
-	for(i = 0; i < 4; i++) {
-		for(j = i; j < 7; j++) {
-			if(sampleBuf[i] > sampleBuf[j]) {
-				/* Swap the values */
-				temp = sampleBuf[i];
-				sampleBuf[i] = sampleBuf[j];
-				sampleBuf[j] = temp;
-			}
-		}
-	}
-}
-
-/**
- * @brief   Initialise the mouse/touch.
- *
- * @notapi
- */
-void ginput_lld_mouse_init(void) {
-	init_board();
-}
-
-/**
- * @brief   Read the mouse/touch position.
- *
- * @param[in] pt	A pointer to the structure to fill
- *
- * @note			For drivers that don't support returning a position
- *					when the touch is up (most touch devices), it should
- *					return the previous position with the new Z value.
- *					The z value is the pressure for those touch devices
- *					that support it (-100 to 100 where > 0 is touched)
- *					or, 0 or 100 for those drivers that don't.
- *
- * @notapi
- */
-void ginput_lld_mouse_get_reading(MouseReading *pt) {
-	uint16_t i;
-
-	// If touch-off return the previous results
-	if (!getpin_pressed()) {
-		pt->x = lastx;
-		pt->y = lasty;
-		pt->z = 0;
-		pt->buttons = 0;
-		return;
-	}
-	
-	// Read the port to get the touch settings
-	aquire_bus();
-
-	/* Get the X value
-	 * Discard the first conversion - very noisy and keep the ADC on hereafter
-	 * till we are done with the sampling. Note that PENIRQ is disabled while reading.
-	 * Finally switch on PENIRQ once again - perform a dummy read.
-	 * Once we have the readings, find the medium using our filter function
- 	 */
-	read_value(0xD1);
-	for(i = 0; i < 7; i++)
-		sampleBuf[i] = read_value(0xD1);
-	read_value(0xD0);
-	filter();
-	lastx = (coord_t)sampleBuf[3];
-
-	/* Get the Y value using the same process as above */
-	read_value(0x91);
-	for(i = 0; i < 7; i++)
-		sampleBuf[i] = read_value(0x91);
-	read_value(0x90);
-	filter();
-	lasty = (coord_t)sampleBuf[3];
-
-	// Release the bus
-	release_bus();
-	
-	// Return the results
-	pt->x = lastx;
-	pt->y = lasty;
-	pt->z = 100;
-	pt->buttons = GINPUT_TOUCH_PRESSED;
-}
-
-#endif /* GFX_USE_GINPUT && GINPUT_NEED_MOUSE */
-/** @} */
+/*
+    ChibiOS/GFX - Copyright (C) 2012, 2013
+                 Joel Bodenmann aka Tectu <joel@unormal.org>
+
+    This file is part of ChibiOS/GFX.
+
+    ChibiOS/GFX 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/GFX 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    drivers/ginput/touch/STMPE811/ginput_lld_mouse.c
+ * @brief   GINPUT Touch low level driver source for the STMPE811.
+ *
+ * @defgroup Mouse Mouse
+ * @ingroup GINPUT
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+#include "gfx.h"
+
+#include "stmpe811.h"
+
+#if (GFX_USE_GINPUT && GINPUT_NEED_MOUSE) /*|| defined(__DOXYGEN__)*/
+
+#include "ginput/lld/mouse.h"
+
+#if defined(GINPUT_MOUSE_USE_CUSTOM_BOARD) && GINPUT_MOUSE_USE_CUSTOM_BOARD
+	#include "ginput_lld_mouse_board.h"
+#elif defined(BOARD_EMBEST_DMSTF4BB)
+	#include "ginput_lld_mouse_board_embest_dmstf4bb.h"
+#else
+	#include "ginput_lld_mouse_board_example.h"
+#endif
+
+#ifndef STMP811_NO_GPIO_IRQPIN
+	#define STMP811_NO_GPIO_IRQPIN	FALSE
+#endif
+#ifndef STMP811_SLOW_CPU
+	#define STMP811_SLOW_CPU	FALSE
+#endif
+
+static coord_t x, y, z;
+static uint8_t touched;
+
+/* set the active window of the stmpe811. bl is bottom left, tr is top right */
+static void setActiveWindow(uint16_t bl_x, uint16_t bl_y, uint16_t tr_x, uint16_t tr_y)
+{
+	write_reg(STMPE811_REG_WDW_TR_X, 2, tr_x);
+	write_reg(STMPE811_REG_WDW_TR_Y, 2, tr_y);
+	write_reg(STMPE811_REG_WDW_BL_X, 2, bl_x);
+	write_reg(STMPE811_REG_WDW_BL_Y, 2, bl_y);
+}
+
+/**
+ * @brief   Initialise the mouse/touch.
+ *
+ * @notapi
+ */
+void ginput_lld_mouse_init(void)
+{
+	init_board();
+
+	write_reg(STMPE811_REG_SYS_CTRL1,		1, 0x02);	// Software chip reset
+	chThdSleepMilliseconds(10);
+
+	write_reg(STMPE811_REG_SYS_CTRL2,		1, 0x0C);	// Temperature sensor clock off, GPIO clock off, touch clock on, ADC clock on
+#if STMP811_NO_GPIO_IRQPIN
+	write_reg(STMPE811_REG_INT_EN,			1, 0x00);	// Interrupt on INT pin when touch is detected
+#else
+	write_reg(STMPE811_REG_INT_EN,			1, 0x01);	// Interrupt on INT pin when touch is detected
+#endif
+	write_reg(STMPE811_REG_ADC_CTRL1,		1, 0x48);	// ADC conversion time = 80 clock ticks, 12-bit ADC, internal voltage refernce
+	chThdSleepMilliseconds(2);
+
+	write_reg(STMPE811_REG_ADC_CTRL2,		1, 0x01);	// ADC speed 3.25MHz
+	write_reg(STMPE811_REG_GPIO_AF,			1, 0x00);	// GPIO alternate function - OFF
+	write_reg(STMPE811_REG_TSC_CFG,			1, 0x9A);	// Averaging 4, touch detect delay 500 us, panel driver settling time 500 us
+	write_reg(STMPE811_REG_FIFO_TH,			1, 0x40);	// FIFO threshold = 64
+	write_reg(STMPE811_REG_FIFO_STA,		1, 0x01);	// FIFO reset enable
+	write_reg(STMPE811_REG_FIFO_STA,		1, 0x00);	// FIFO reset disable
+	write_reg(STMPE811_REG_TSC_FRACT_XYZ,	1, 0x07);	// Z axis data format
+	write_reg(STMPE811_REG_TSC_I_DRIVE,		1, 0x01);	// 50mA touchscreen line current
+	write_reg(STMPE811_REG_TSC_CTRL,		1, 0x00);	// X&Y&Z
+	write_reg(STMPE811_REG_TSC_CTRL,		1, 0x01);	// X&Y&Z, TSC enable
+	write_reg(STMPE811_REG_INT_STA,			1, 0xFF);	// Clear all interrupts
+#if !STMP811_NO_GPIO_IRQPIN
+	touched = (uint8_t)read_reg(STMPE811_REG_TSC_CTRL, 1) & 0x80;
+#endif
+	write_reg(STMPE811_REG_INT_CTRL,		1, 0x01);	// Level interrupt, enable intrrupts
+}
+
+/**
+ * @brief   Read the mouse/touch position.
+ *
+ * @param[in] pt	A pointer to the structure to fill
+ *
+ * @note			For drivers that don't support returning a position
+ *					when the touch is up (most touch devices), it should
+ *					return the previous position with the new Z value.
+ *					The z value is the pressure for those touch devices
+ *					that support it (-100 to 100 where > 0 is touched)
+ *					or, 0 or 100 for those drivers that don't.
+ *
+ * @notapi
+ */
+void ginput_lld_mouse_get_reading(MouseReading *pt)
+{
+	bool_t	clearfifo;		// Do we need to clear the FIFO
+
+#if STMP811_NO_GPIO_IRQPIN
+	// Poll to get the touched status
+	uint8_t		last_touched;
+	
+	last_touched = touched;
+	touched = (uint8_t)read_reg(STMPE811_REG_TSC_CTRL, 1) & 0x80;
+	clearfifo = (touched != last_touched);
+#else
+	// Check if the touch controller IRQ pin has gone off
+	clearfifo = false;
+	if(getpin_irq()) {							// please rename this to getpin_irq
+		write_reg(STMPE811_REG_INT_STA, 1, 0xFF);	// clear all interrupts
+		touched = (uint8_t)read_reg(STMPE811_REG_TSC_CTRL, 1) & 0x80;	// set the new touched status
+		clearfifo = true;							// only take the last FIFO reading
+	}
+#endif
+
+	// If not touched, return the previous results
+	if (!touched) {
+		pt->x = x;
+		pt->y = y;
+		pt->z = 0;
+		pt->buttons = 0;
+		return;
+	}
+
+#if !STMP811_SLOW_CPU
+	if (!clearfifo && (read_reg(STMPE811_REG_FIFO_STA, 1) & 0xD0))
+#endif
+		clearfifo = true;
+
+	do {
+		/* Get the X, Y, Z values */
+		/* This could be done in a single 4 byte read to STMPE811_REG_TSC_DATA_XYZ (incr or non-incr) */
+		x = (coord_t)read_reg(STMPE811_REG_TSC_DATA_X, 2);
+		y = (coord_t)read_reg(STMPE811_REG_TSC_DATA_Y, 2);
+		z = (coord_t)read_reg(STMPE811_REG_TSC_DATA_Z, 1);
+	} while(clearfifo && !(read_reg(STMPE811_REG_FIFO_STA, 1) & 0x20));
+
+	// Rescale X,Y,Z - X & Y don't need scaling when you are using calibration!
+#if !GINPUT_MOUSE_NEED_CALIBRATION
+	x = gdispGetWidth() - x / (4096/gdispGetWidth());
+	y = y / (4096/gdispGetHeight());
+#endif
+	z = (((z&0xFF) * 100)>>8) + 1;
+	
+	// Return the results. ADC gives values from 0 to 2^12 (4096)
+	pt->x = x;
+	pt->y = y;
+	pt->z = z;
+	pt->buttons = GINPUT_TOUCH_PRESSED;
+
+	/* Force another read if we have more results */
+	if (!clearfifo && !(read_reg(STMPE811_REG_FIFO_STA, 1) & 0x20))
+		ginputMouseWakeup();
+
+}
+
+#endif /* GFX_USE_GINPUT && GINPUT_NEED_MOUSE */
+/** @} */
+
diff --git a/drivers/ginput/touch/STMPE811/ginput_lld_mouse_board_embest_dmstf4bb.h b/drivers/ginput/touch/STMPE811/ginput_lld_mouse_board_embest_dmstf4bb.h
index fa43f4ac..a0d5a0de 100644
--- a/drivers/ginput/touch/STMPE811/ginput_lld_mouse_board_embest_dmstf4bb.h
+++ b/drivers/ginput/touch/STMPE811/ginput_lld_mouse_board_embest_dmstf4bb.h
@@ -30,7 +30,7 @@
 #ifndef _GINPUT_LLD_MOUSE_BOARD_H
 #define _GINPUT_LLD_MOUSE_BOARD_H
 
-static const I2CConfig i2ccfg2 = {
+static const I2CConfig i2ccfg = {
 	OPMODE_I2C,
 	400000,
 	FAST_DUTY_CYCLE_2,
@@ -41,52 +41,98 @@ static const I2CConfig i2ccfg2 = {
  *
  * @notapi
  */
-static inline void init_board(void) {
-	palSetPadMode(GPIOC, 13, PAL_MODE_INPUT);			/* TP IRQ */
-	palSetPadMode(GPIOB, 10, PAL_MODE_ALTERNATE(4));	/* I2C2 SCL */
-	palSetPadMode(GPIOB, 11, PAL_MODE_ALTERNATE(4));	/* I2C2 SDA */
+static void init_board(void)
+{
+	palSetPadMode(GPIOC, 13, PAL_MODE_INPUT | PAL_STM32_PUDR_FLOATING);			/* TP IRQ */
+	palSetPadMode(GPIOB, 8, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);	/* SCL */
+	palSetPadMode(GPIOB, 9, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);	/* SDA */
 
-	i2cStart(&I2CD2, &i2ccfg2);
+	i2cStart(&I2CD1, &i2ccfg);
 }
 
 /**
- * @brief   Check whether the surface is currently touched
- * @return	TRUE if the surface is currently touched
+ * @brief   Check whether an interrupt is raised
+ * @return	TRUE if there is an interrupt signal present
  *
  * @notapi
  */
-static inline bool_t getpin_pressed(void) {
+static inline bool_t getpin_irq(void) {
 	return (!(palReadPad(GPIOC, 13)));
 }
 
 /**
- * @brief   Aquire the bus ready for readings
+ * @brief	Write a value into a certain register
+ *
+ * @param[in] reg	The register address
+ * @param[in] n		The amount of bytes (one or two)
+ * @param[in] val	The value
  *
  * @notapi
  */
-static inline void aquire_bus(void) {
-	i2cAcquireBus(&I2CD2);
+static void write_reg(uint8_t reg, uint8_t n, uint16_t val)
+{
+	uint8_t txbuf[3];
+
+	i2cAcquireBus(&I2CD1);
+
+	txbuf[0] = reg;
+
+	if (n == 1) {
+		txbuf[1] = val;
+		i2cMasterTransmitTimeout(&I2CD1, STMPE811_ADDR, txbuf, 2, NULL, 0, MS2ST(STMPE811_TIMEOUT));
+	} else if (n == 2) {
+		txbuf[1] = ((val & 0xFF00) >> 8);
+		txbuf[2] = (val & 0x00FF);
+		i2cMasterTransmitTimeout(&I2CD1, STMPE811_ADDR, txbuf, 3, NULL, 0, MS2ST(STMPE811_TIMEOUT));
+	}
+	
+	i2cReleaseBus(&I2CD1);
+}
+ 
+/**
+ * @brief	Read the value of a certain register
+ *
+ * @param[in] reg	The register address
+ * @param[in] n		The amount of bytes (one or two)
+ *
+ * @return Data read from device (one byte or two depending on n param)
+ *
+ * @notapi
+ */
+static uint16_t read_reg(uint8_t reg, uint8_t n)
+{
+	uint8_t txbuf[1], rxbuf[2];
+	uint16_t ret;
+
+	rxbuf[0] = 0;
+	rxbuf[1] = 0;
+
+	i2cAcquireBus(&I2CD1);
+
+	txbuf[0] = reg;
+	i2cMasterTransmitTimeout(&I2CD1, STMPE811_ADDR, txbuf, 1, rxbuf, n, MS2ST(STMPE811_TIMEOUT));
+
+	if (n == 1) {
+		ret = rxbuf[0];
+	} else if (n == 2) {
+		ret = ((rxbuf[0] << 8) | (rxbuf[1] & 0xFF));
+	}
+
+	i2cReleaseBus(&I2CD1);
+
+	return ret;
 }
 
-/**
- * @brief   Release the bus after readings
- *
- * @notapi
- */
-static inline void release_bus(void) {
-	i2cReleaseBus(&I2CD2);
-}
+static void read_reg_n(uint8_t reg, uint8_t n, uint8_t *rxbuf)
+{
+	uint8_t txbuf[1];
 
-/**
- * @brief   Read a value from touch controller
- * @return	The value read from the controller
- *
- * params[in] port	The controller port to read.
- *
- * @notapi
- */
-static inline uint16_t read_value(uint16_t port) {
-	 #error "STMPE811: Implement this driver first!"
+	i2cAcquireBus(&I2CD1);
+
+	txbuf[0] = reg;
+	i2cMasterTransmitTimeout(&I2CD1, STMPE811_ADDR, txbuf, 1, rxbuf, n, MS2ST(STMPE811_TIMEOUT));
+
+	i2cReleaseBus(&I2CD1);
 }
 
 #endif /* _GINPUT_LLD_MOUSE_BOARD_H */
diff --git a/drivers/ginput/touch/STMPE811/ginput_lld_mouse_board_example.h b/drivers/ginput/touch/STMPE811/ginput_lld_mouse_board_example.h
index fbedcb60..453924b0 100644
--- a/drivers/ginput/touch/STMPE811/ginput_lld_mouse_board_example.h
+++ b/drivers/ginput/touch/STMPE811/ginput_lld_mouse_board_example.h
@@ -35,53 +35,53 @@
  *
  * @notapi
  */
-static inline void init_board(void) {
+static void init_board(void)
+{
 	/* Code here */
 	#error "ginputSTMPE811: You must supply a definition for init_board for your board"
 }
 
 /**
- * @brief   Check whether the surface is currently touched
- * @return	TRUE if the surface is currently touched
+ * @brief   Check whether an interrupt is raised
+ * @return	TRUE if there is an interrupt signal present
  *
  * @notapi
  */
-static inline bool_t getpin_pressed(void) {
+static inline bool_t getpin_irq(void)
+{
 	/* Code here */
-	#error "ginputSTMPE811: You must supply a definition for getpin_pressed for your board"
+	#error "ginputSTMPE811: You must supply a definition for getpin_irq for your board"
 }
 
 /**
- * @brief   Aquire the bus ready for readings
+ * @brief	Write a value into a certain register
+ *
+ * @param[in] reg	The register address
+ * @param[in] n		The amount of bytes (one or two)
+ * @param[in] val	The value
  *
  * @notapi
  */
-static inline void aquire_bus(void) {
+static void write_reg(uint8_t reg, uint8_t n, uint16_t val)
+{
 	/* Code here */
-	#error "ginputSTMPE811: You must supply a definition for aquire_bus for your board"
+	#error "ginputSTMPE811: You must supply a definition for write_reg for your board"
 }
 
 /**
- * @brief   Release the bus after readings
+ * @brief	Read the value of a certain register
+ *
+ * @param[in] reg	The register address
+ * @param[in] n		The amount of bytes (one or two)
+ *
+ * @return Data read from device (one byte or two depending on n param)
  *
  * @notapi
  */
-static inline void release_bus(void) {
+static uint16_t read_reg(uint8_t reg, uint8_t n)
+{
 	/* Code here */
-	#error "ginputSTMPE811: You must supply a definition for release_bus for your board"
-}
-
-/**
- * @brief   Read a value from touch controller
- * @return	The value read from the controller
- *
- * params[in] port	The controller port to read.
- *
- * @notapi
- */
-static inline uint16_t read_value(uint16_t port) {
-	/* Code here */
-	#error "ginputSTMPE811: You must supply a definition for read_value for your board"
+	#error "ginputSTMPE811: You must supply a definition for read_reg for your board"
 }
 
 #endif /* _GINPUT_LLD_MOUSE_BOARD_H */
diff --git a/drivers/ginput/touch/STMPE811/ginput_lld_mouse_config.h b/drivers/ginput/touch/STMPE811/ginput_lld_mouse_config.h
index 76224198..871a6c2e 100644
--- a/drivers/ginput/touch/STMPE811/ginput_lld_mouse_config.h
+++ b/drivers/ginput/touch/STMPE811/ginput_lld_mouse_config.h
@@ -34,12 +34,15 @@
 #define GINPUT_MOUSE_NEED_CALIBRATION			TRUE
 #define GINPUT_MOUSE_LLD_CALIBRATION_LOADSAVE	FALSE
 #define GINPUT_MOUSE_MAX_CALIBRATION_ERROR		5
-#define GINPUT_MOUSE_READ_CYCLES				4
+#define GINPUT_MOUSE_READ_CYCLES				1
 #define GINPUT_MOUSE_POLL_PERIOD				25
 #define GINPUT_MOUSE_MAX_CLICK_JITTER			10
-#define GINPUT_MOUSE_MAX_MOVE_JITTER			2
-#define GINPUT_MOUSE_CLICK_TIME					500
+#define GINPUT_MOUSE_MAX_MOVE_JITTER			5
+#define GINPUT_MOUSE_CLICK_TIME					450
+
+/* default values - over write these in your boad files */
+#define STMP811_SLOWER_RESPONSE 				FALSE
+#define STMP811_NO_GPIO_IRQPIN 					FALSE
 
 #endif /* _LLD_GINPUT_MOUSE_CONFIG_H */
 /** @} */
-
diff --git a/drivers/ginput/touch/STMPE811/readme.txt b/drivers/ginput/touch/STMPE811/readme.txt
index 1d7a8b4f..065840d8 100644
--- a/drivers/ginput/touch/STMPE811/readme.txt
+++ b/drivers/ginput/touch/STMPE811/readme.txt
@@ -1,9 +1,20 @@
-To use this driver:
-
-1. Add in your halconf.h:
-	a) #define GFX_USE_GINPUT			TRUE
-	b) #define GINPUT_NEED_MOUSE		TRUE
-
-2. To your makefile add the following lines:
-	include $(GFXLIB)/drivers/ginput/touch/STMPE811/ginput_lld.mk
-
+The STMPE811 driver comes with two different #defines to perfectly fit
+your application:
+
+
+STMPE811_NO_GPIO_IRQPIN
+This Macro is meant to be set in your board file. When you set this macro to
+TRUE, the GINPUT module will not use the IRQ lane which might be connected
+to a GPIO pin to recognize interrupts by the STMPE811 controller. This
+costs a few more I2C calls.
+When the interrupt IRQ pin is connected to a GPIO of your MCU, set this
+macro to FALSE.
+
+
+STMP811_SLOW_CPU
+If you have a slow CPU and you need to take care of your resources, you can
+set this macro TRUE. This will save some IRQs and therefore a few I2C calls.
+The disadvantage is a little higher response time.
+If you don't want to draw continious lines on your display, it's recommended
+to set this to TRUE anyways.
+
diff --git a/drivers/ginput/touch/STMPE811/stmpe811.h b/drivers/ginput/touch/STMPE811/stmpe811.h
index 8396a844..36101cd4 100644
--- a/drivers/ginput/touch/STMPE811/stmpe811.h
+++ b/drivers/ginput/touch/STMPE811/stmpe811.h
@@ -29,7 +29,11 @@
 #ifndef _STMPE811_H
 #define _STMPE811_H
 
-/* STMPE811 registers */
+// Slave address
+#define STMPE811_ADDR				(0x82 >> 1)
+
+// Maximum timeout
+#define STMPE811_TIMEOUT			0x3000
 
 // Identification registers
 #define STMPE811_REG_CHP_ID         0x00 // 16-bit
@@ -88,6 +92,7 @@
 #define STMPE811_REG_TSC_DATA_XYZ   0x52 // 32-bit
 #define STMPE811_REG_TSC_FRACT_XYZ  0x56
 #define STMPE811_REG_TSC_DATA       0x57
+#define STMPE811_REG_TSC_DATA_AI    0xD7
 #define STMPE811_REG_TSC_I_DRIVE    0x58
 #define STMPE811_REG_TSC_SHIELD     0x59
 
@@ -98,4 +103,3 @@
 
 #endif /* _STMPE811_H */
 /** @} */
-
diff --git a/include/gwin/button.h b/include/gwin/button.h
index fb9a2b76..2df540da 100644
--- a/include/gwin/button.h
+++ b/include/gwin/button.h
@@ -251,3 +251,4 @@ void gwinButtonDraw_Square(GHandle gh, bool_t isdown, const char *txt, const GBu
 
 #endif /* _GWIN_BUTTON_H */
 /** @} */
+
diff --git a/src/gwin/button.c b/src/gwin/button.c
index 519dd1fe..bec738b4 100644
--- a/src/gwin/button.c
+++ b/src/gwin/button.c
@@ -48,12 +48,13 @@ static const GButtonDrawStyle GButtonDefaultStyleUp = {
 	HTML2COLOR(0x404040),		// color_up_edge;
 	HTML2COLOR(0xE0E0E0),		// color_up_fill;
 	HTML2COLOR(0x000000),		// color_up_txt;
-	};
+};
+
 static const GButtonDrawStyle GButtonDefaultStyleDown = {
 	HTML2COLOR(0x404040),		// color_dn_edge;
 	HTML2COLOR(0x808080),		// color_dn_fill;
 	HTML2COLOR(0x404040),		// color_dn_txt;
-	};
+};
 
 // Process an event callback
 static void gwinButtonCallback(void *param, GEvent *pe) {