diff --git a/drivers/ginput/touch/STMPE811/ginput_lld_mouse.c b/drivers/ginput/touch/STMPE811/ginput_lld_mouse.c
index b2f7ba76..bdae310e 100644
--- a/drivers/ginput/touch/STMPE811/ginput_lld_mouse.c
+++ b/drivers/ginput/touch/STMPE811/ginput_lld_mouse.c
@@ -57,24 +57,25 @@ static void setActiveWindow(uint16_t bl_x, uint16_t bl_y, uint16_t tr_x, uint16_
 void ginput_lld_mouse_init(void) {
 	init_board();
 
-	write_reg(STMPE811_REG_SYS_CTRL1, 		1, 0x02);	// software chip reset
-	write_reg(STMPE811_REG_SYS_CTRL2, 		1, 0x04);	// temp. sensor clock on, GPIO clock off, touch clock on, ADC clock on
-	write_reg(STMPE811_REG_INT_EN, 			1, 0x03);  //0x03
-	write_reg(STMPE811_REG_ADC_CTRL1, 		1, 0x49);  //ADC conversion time = 80 clock ticks, 12-bit ADC, internacl voltage refernce
+	write_reg(STMPE811_REG_SYS_CTRL1,		1, 0x02);	// Software chip reset
+	write_reg(STMPE811_REG_SYS_CTRL2,		1, 0x0C);	// Temp. sensor clock off, GPIO clock off, touch clock on, ADC clock on
+	write_reg(STMPE811_REG_INT_EN,			1, 0x03);	// Interrupt on INT pin when FIFO is equal or above threshold value OR touch is detected
+	write_reg(STMPE811_REG_ADC_CTRL1,		1, 0x48);	// ADC conversion time = 80 clock ticks, 12-bit ADC, internaal voltage refernce
 
-	chThdSleepMicroseconds(1000);
+	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, 0xA3);  //avaraging 4, Touch detect delay 1ms, Panel driver settling time 1ms
-	write_reg(STMPE811_REG_FIFO_TH,   		1, 0x01);  //FIFO trashold =1
-	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, 0x03);  //X&Y only, TSC enable
-	write_reg(STMPE811_REG_INT_STA,   		1, 0xFF);  //clear all interrupts
-	write_reg(STMPE811_REG_INT_CTRL,  		1, 0x01);  //level interrupt, enable intrrupts 
+	write_reg(STMPE811_REG_ADC_CTRL2,		1, 0x01);	// ADC speed 3.25MHz
+	write_reg(STMPE811_REG_GPIO_AF,			1, 0x00);	// GPIO alternate function - OFF
+	// @TODO: decide which value should STMPE811_REG_TSC_CFG have - either 0x9A or from comment;)
+	write_reg(STMPE811_REG_TSC_CFG,			1, 0x9A);	// Averaging 4, Touch detect delay 1ms, Panel driver settling time 1ms
+	write_reg(STMPE811_REG_FIFO_TH,			1, 0x01);	// FIFO threshold =1
+	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, 0x03);	// X&Y only, TSC enable
+	write_reg(STMPE811_REG_INT_STA,			1, 0xFF);	// Clear all interrupts
+	write_reg(STMPE811_REG_INT_CTRL,		1, 0x01);	// Level interrupt, enable intrrupts
 }
 
 /**
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 490b8dca..6808196d 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
@@ -69,7 +69,7 @@ static inline bool_t getpin_pressed(void) {
  * @notapi
  */
 static void write_reg(uint8_t reg, uint8_t n, uint16_t val) {
-	uint8_t txbuf[1];
+	uint8_t txbuf[2];
 
 	i2cAcquireBus(&I2CD1);
 	
@@ -79,7 +79,7 @@ static void write_reg(uint8_t reg, uint8_t n, uint16_t val) {
 	if(n == 1) {
 		txbuf[0] = val;
 		i2cMasterTransmitTimeout(&I2CD1, STMPE811_ADDR, txbuf, 1, NULL, 0, MS2ST(STMPE811_TIMEOUT));
-	} else if(n == 3) {
+	} else if(n == 2) {
 		txbuf[0] = ((val & 0xFF00) >> 8);
 		txbuf[1] = (val & 0x00FF);
 		i2cMasterTransmitTimeout(&I2CD1, STMPE811_ADDR, txbuf, 2, NULL, 0, MS2ST(STMPE811_TIMEOUT));
@@ -100,21 +100,26 @@ 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, 0x82 >> 1, txbuf, 1, rxbuf, 2, MS2ST(STMPE811_TIMEOUT));
-		
-	i2cReleaseBus(&I2CD1);
+	i2cMasterTransmitTimeout(&I2CD1, STMPE811_ADDR, txbuf, 1, rxbuf, 0, MS2ST(STMPE811_TIMEOUT));
 
-	if(n == 1)
+	if(n == 1) {
+		i2cMasterReceiveTimeout(&I2CD1, STMPE811_ADDR, rxbuf, 1, MS2ST(STMPE811_TIMEOUT));
 		ret = rxbuf[0];
-	else if (n == 2)
+	} else if(n == 2) {
+		i2cMasterReceiveTimeout(&I2CD1, STMPE811_ADDR, rxbuf, 2, MS2ST(STMPE811_TIMEOUT));
 		ret = ((rxbuf[0] << 8) | (rxbuf[1] & 0xFF));
+	}
+
+	i2cReleaseBus(&I2CD1);
 
 	return ret;
 }
 
 #endif /* _GINPUT_LLD_MOUSE_BOARD_H */
 /** @} */
-