Merge pull request #13 from trsaunders/master

S6D1121 driver improvements

1 files changed, 271 insertions, 146 deletions

diff --git a/drivers/lcd/s6d1121_lld.c b/drivers/lcd/s6d1121_lld.c
index 2a591ee2..18125b55 100644
--- a/drivers/lcd/s6d1121_lld.c
+++ b/drivers/lcd/s6d1121_lld.c
@@ -1,12 +1,25 @@
+#include "ch.h"
+#include "hal.h"
+
 #include "s6d1121_lld.h"
+#include "chprintf.h"
 
 #ifdef LCD_USE_S6D1121
 
-static uint16_t buf[((SCREEN_HEIGHT > SCREEN_WIDTH ) ? SCREEN_HEIGHT : SCREEN_WIDTH)];
+static uint8_t orientation;
+extern uint16_t lcd_width, lcd_height;
 
+/* all interfaces use RST via GPIO */
+/* TODO: option to disable RST; assumes RST is tied high */
 #define LCD_RST_LOW		palClearPad(LCD_RST_GPIO, LCD_RST_PIN)
 #define LCD_RST_HIGH	palSetPad(LCD_RST_GPIO, LCD_RST_PIN)
 
+#define s6d1121_delay(n) 	halPolledDelay(MS2RTT(n));
+
+static uint16_t buf[((SCREEN_HEIGHT > SCREEN_WIDTH ) ? SCREEN_HEIGHT : SCREEN_WIDTH)];
+
+#if defined(LCD_USE_GPIO)
+
 #define LCD_CS_LOW		palClearPad(LCD_CS_GPIO, LCD_CS_PIN)
 #define LCD_CS_HIGH		palSetPad(LCD_CS_GPIO, LCD_CS_PIN)
 
@@ -22,8 +35,6 @@ static uint16_t buf[((SCREEN_HEIGHT > SCREEN_WIDTH ) ? SCREEN_HEIGHT : SCREEN_WI
 #define LCD_BL_LOW		palClearPad(LCD_BL_GPIO, LCD_BL_PIN)
 #define LCD_BL_HIGH		palSetPad(LCD_BL_GPIO, LCD_BL_PIN)
 
-static uint8_t orientation;
-extern uint16_t lcd_width, lcd_height;
 
 static inline void lld_lcddelay(void)
 {
@@ -56,9 +67,6 @@ static __inline uint16_t lld_lcdReadData(void) {
 
 #endif
 
-//    value = palReadPort(LCD_DATA_PORT); // dummy
-//    value = palReadPort(LCD_DATA_PORT);
-
 #ifndef STM32F4XX
     // change pin mode back to digital output
     LCD_DATA_PORT->CRH = 0x33333333;
@@ -96,7 +104,7 @@ void lld_lcdWriteData(uint16_t lcdData) {
 	lld_lcdwrite(lcdData);
 }
 
-void lcdWriteReg(uint16_t lcdReg, uint16_t lcdRegValue) {
+void lld_lcdWriteReg(uint16_t lcdReg, uint16_t lcdRegValue) {
 	LCD_CS_LOW;
 
 	lld_lcdWriteIndex(lcdReg);
@@ -105,151 +113,244 @@ void lcdWriteReg(uint16_t lcdReg, uint16_t lcdRegValue) {
 	LCD_CS_HIGH;
 }
 
-void lld_lcdInit(void) {
-	// ChibiOS HAL has a nice and precise delay function,
-	// halPolledDelay(US2RTT(x)); or halPolledDelay(MS2RTT(x));
-	// Why Not use it?
-	// IO Default Configurations
-	palSetPadMode(LCD_CS_GPIO, LCD_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(LCD_WR_GPIO, LCD_WR_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(LCD_RD_GPIO, LCD_RD_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(LCD_RST_GPIO, LCD_RST_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(LCD_RS_GPIO, LCD_RS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(LCD_BL_GPIO, LCD_BL_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-
-	palSetGroupMode(LCD_D0_GPIO, 0x0000000F, 0, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetGroupMode(LCD_D4_GPIO, 0x0000FFF0, 0, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+static __inline void lld_lcdWriteStreamStart(void) {
+	LCD_CS_LOW;
+	lld_lcdWriteIndex(0x0022);
+}
 
+static __inline void lld_lcdWriteStreamStop(void) {
 	LCD_CS_HIGH;
-	LCD_RST_HIGH;
-	LCD_RD_HIGH;
-	LCD_WR_HIGH;
-	LCD_BL_LOW;
+}
 
-	// A Good idea to reset the module before using
-	LCD_RST_LOW;
-	halPolledDelay(MS2RTT(2));
-	LCD_RST_HIGH;         // Hardware Reset
-	halPolledDelay(MS2RTT(2));
+__inline void lld_lcdWriteStream(uint16_t *buffer, uint16_t size) {
+	uint16_t i;
 
-	lcdWriteReg(0x11,0x2004);
-	lcdWriteReg(0x13,0xCC00);
-	lcdWriteReg(0x15,0x2600);
-	lcdWriteReg(0x14,0x252A);
-	lcdWriteReg(0x12,0x0033);
-	lcdWriteReg(0x13,0xCC04);
+	for(i = 0; i < size; i++) {
+		lld_lcdwrite(buffer[i]);
+	}
+}
 
-	halPolledDelay(MS2RTT(1));
+__inline void lld_lcdReadStreamStart(void) {
+	/* TODO */
+}
 
-	lcdWriteReg(0x13,0xCC06);
+__inline void lld_lcdReadStreamStop(void) {
+	/* TODO */
+}
 
-	halPolledDelay(MS2RTT(1));
+__inline void lld_lcdReadStream(uint16_t *buffer, size_t size) {
+	/* TODO */
+}
 
-	lcdWriteReg(0x13,0xCC4F);
+#elif defined(LCD_USE_FSMC)
 
-	halPolledDelay(MS2RTT(1));
+#define LCD_REG              (*((volatile uint16_t *) 0x60000000)) /* RS = 0 */
+#define LCD_RAM              (*((volatile uint16_t *) 0x60020000)) /* RS = 1 */
 
-	lcdWriteReg(0x13,0x674F);
-	lcdWriteReg(0x11,0x2003);
+static __inline void lld_lcdWriteIndex(uint16_t index) {
+	LCD_REG = index;
+}
 
-	halPolledDelay(MS2RTT(1));
+static __inline void lld_lcdWriteData(uint16_t data) {
+	LCD_RAM = data;
+}
 
-	// Gamma Setting
-	lcdWriteReg(0x30,0x2609);
-	lcdWriteReg(0x31,0x242C);
-	lcdWriteReg(0x32,0x1F23);
-	lcdWriteReg(0x33,0x2425);
-	lcdWriteReg(0x34,0x2226);
-	lcdWriteReg(0x35,0x2523);
-	lcdWriteReg(0x36,0x1C1A);
-	lcdWriteReg(0x37,0x131D);
-	lcdWriteReg(0x38,0x0B11);
-	lcdWriteReg(0x39,0x1210);
-	lcdWriteReg(0x3A,0x1315);
-	lcdWriteReg(0x3B,0x3619);
-	lcdWriteReg(0x3C,0x0D00);
-	lcdWriteReg(0x3D,0x000D);
-
-	lcdWriteReg(0x16,0x0007);
-	lcdWriteReg(0x02,0x0013);
-	lcdWriteReg(0x03,0x0003);
-	lcdWriteReg(0x01,0x0127);
-
-	halPolledDelay(MS2RTT(1));
-
-	lcdWriteReg(0x08,0x0303);
-	lcdWriteReg(0x0A,0x000B);
-	lcdWriteReg(0x0B,0x0003);
-	lcdWriteReg(0x0C,0x0000);
-	lcdWriteReg(0x41,0x0000);
-	lcdWriteReg(0x50,0x0000);
-	lcdWriteReg(0x60,0x0005);
-	lcdWriteReg(0x70,0x000B);
-	lcdWriteReg(0x71,0x0000);
-	lcdWriteReg(0x78,0x0000);
-	lcdWriteReg(0x7A,0x0000);
-	lcdWriteReg(0x79,0x0007);
-	lcdWriteReg(0x07,0x0051);
-
-	halPolledDelay(MS2RTT(1));
-
-	lcdWriteReg(0x07,0x0053);
-	lcdWriteReg(0x79,0x0000);
+static __inline void lld_lcdWriteReg(uint16_t lcdReg,uint16_t lcdRegValue) {
+	LCD_REG = lcdReg;
+	LCD_RAM = lcdRegValue;
 }
 
-void lld_lcdSetCursor(uint16_t x, uint16_t y) {
-	if(PORTRAIT) {
-		lcdWriteReg(0x0020, x);
-		lcdWriteReg(0x0021, y);
-	} else if(LANDSCAPE) {
-		lcdWriteReg(0x0020, y);
-		lcdWriteReg(0x0021, x);
-	}
+static __inline uint16_t lld_lcdReadData(void) {
+	return (LCD_RAM);
 }
 
-static __inline void lld_lcdWriteStreamStart(void) {
-	#ifdef LCD_USE_GPIO
-		LCD_CS_LOW;
-		lld_lcdWriteIndex(0x0022);
-	#endif
+static __inline uint16_t lld_lcdReadReg(uint16_t lcdReg) {
+	LCD_REG = lcdReg;
+	return LCD_RAM;
+}
 
-	#ifdef LCD_USE_SPI
-	#endif
+__inline void lld_lcdWriteStreamStart(void) {
+	LCD_REG = 0x0022;
+}
+
+__inline void lld_lcdWriteStreamStop(void) {
 
-	#ifdef LCD_USE_FSMC
-	#endif
 }
 
-static __inline void lld_lcdWriteStreamStop(void) {
-	#ifdef LCD_USE_GPIO
-		LCD_CS_HIGH;
-	#endif
+__inline void lld_lcdWriteStream(uint16_t *buffer, uint16_t size) {
+	uint16_t i;
+	for(i = 0; i < size; i++)
+		LCD_RAM = buffer[i];
+}
+
+__inline void lld_lcdReadStreamStart(void) {
+	LCD_REG = 0x0022;
+}
 
-	#ifdef LCD_USE_SPI
-	#endif
+__inline void lld_lcdReadStreamStop(void) {
 
-	#ifdef LCD_USE_FSMC
-	#endif
 }
 
-__inline void lld_lcdWriteStream(uint16_t *buffer, uint16_t size) {
+__inline void lld_lcdReadStream(uint16_t *buffer, size_t size) {
 	uint16_t i;
+	/* throw away first value read */
+	volatile uint16_t dummy = LCD_RAM;
 
 	for(i = 0; i < size; i++) {
-		lld_lcdwrite(buffer[i]);
+		buffer[i] = LCD_RAM;
 	}
 }
 
-__inline void lld_lcdReadStreamStart(void) {
-	/* TODO */
+#endif
+
+void lld_lcdSetPowerMode(uint8_t powerMode) {
+	/* TODO: implement */
 }
 
-__inline void lld_lcdReadStreamStop(void) {
-	/* TODO */
+void lld_lcdInit(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;
+	s6d1121_delay(2);
+	LCD_RST_HIGH;         // Hardware Reset
+	s6d1121_delay(2);
+
+#ifdef LCD_USE_GPIO
+	// IO Default Configurations
+	palSetPadMode(LCD_CS_GPIO, LCD_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(LCD_WR_GPIO, LCD_WR_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(LCD_RD_GPIO, LCD_RD_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(LCD_RS_GPIO, LCD_RS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(LCD_BL_GPIO, LCD_BL_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+
+	palSetGroupMode(LCD_D0_GPIO, 0x0000000F, 0, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetGroupMode(LCD_D4_GPIO, 0x0000FFF0, 0, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+
+	LCD_CS_HIGH;
+	LCD_RD_HIGH;
+	LCD_WR_HIGH;
+	LCD_BL_LOW;
+
+
+#elif defined(LCD_USE_FSMC)
+#if defined(STM32F1XX)
+	/* FSMC setup. TODO: this only works for STM32F1 */
+	rccEnableAHB(RCC_AHBENR_FSMCEN, 0);
+
+	/* TODO: pin setup */
+#elif defined(STM32F4XX)
+	/* STM32F4 FSMC init */
+	rccEnableAHB3(RCC_AHB3ENR_FSMCEN, 0);
+
+	/* set pins to FSMC mode */
+	IOBus busD = {GPIOD, (1 << 0) | (1 << 1) | (1 << 4) | (1 << 5) | (1 << 7) | (1 << 8) |
+							(1 << 9) | (1 << 10) | (1 << 11) | (1 << 14) | (1 << 15), 0};
+
+	IOBus busE = {GPIOE, (1 << 7) | (1 << 8) | (1 << 9) | (1 << 10) | (1 << 11) | (1 << 12) |
+						(1 << 13) | (1 << 14) | (1 << 15), 0};
+
+	palSetBusMode(&busD, PAL_MODE_ALTERNATE(12));
+	palSetBusMode(&busE, PAL_MODE_ALTERNATE(12));
+#else
+#error "FSMC not implemented for this device"
+#endif
+	int FSMC_Bank = 0;
+	/* FSMC timing */
+	FSMC_Bank1->BTCR[FSMC_Bank+1] = (10) | (10 << 8) | (10 << 16);
+
+	/* Bank1 NOR/SRAM control register configuration */
+	FSMC_Bank1->BTCR[FSMC_Bank] = FSMC_BCR1_MWID_0 | FSMC_BCR1_WREN | FSMC_BCR1_MBKEN;
+#endif
+
+	lld_lcdWriteReg(0x11,0x2004);
+	lld_lcdWriteReg(0x13,0xCC00);
+	lld_lcdWriteReg(0x15,0x2600);
+	lld_lcdWriteReg(0x14,0x252A);
+	lld_lcdWriteReg(0x12,0x0033);
+	lld_lcdWriteReg(0x13,0xCC04);
+
+	s6d1121_delay(1);
+
+	lld_lcdWriteReg(0x13,0xCC06);
+
+	s6d1121_delay(1);
+
+	lld_lcdWriteReg(0x13,0xCC4F);
+
+	s6d1121_delay(1);
+
+	lld_lcdWriteReg(0x13,0x674F);
+	lld_lcdWriteReg(0x11,0x2003);
+
+	s6d1121_delay(1);
+
+	// Gamma Setting
+	lld_lcdWriteReg(0x30,0x2609);
+	lld_lcdWriteReg(0x31,0x242C);
+	lld_lcdWriteReg(0x32,0x1F23);
+	lld_lcdWriteReg(0x33,0x2425);
+	lld_lcdWriteReg(0x34,0x2226);
+	lld_lcdWriteReg(0x35,0x2523);
+	lld_lcdWriteReg(0x36,0x1C1A);
+	lld_lcdWriteReg(0x37,0x131D);
+	lld_lcdWriteReg(0x38,0x0B11);
+	lld_lcdWriteReg(0x39,0x1210);
+	lld_lcdWriteReg(0x3A,0x1315);
+	lld_lcdWriteReg(0x3B,0x3619);
+	lld_lcdWriteReg(0x3C,0x0D00);
+	lld_lcdWriteReg(0x3D,0x000D);
+
+	lld_lcdWriteReg(0x16,0x0007);
+	lld_lcdWriteReg(0x02,0x0013);
+	lld_lcdWriteReg(0x03,0x0003);
+	lld_lcdWriteReg(0x01,0x0127);
+
+	s6d1121_delay(1);
+
+	lld_lcdWriteReg(0x08,0x0303);
+	lld_lcdWriteReg(0x0A,0x000B);
+	lld_lcdWriteReg(0x0B,0x0003);
+	lld_lcdWriteReg(0x0C,0x0000);
+	lld_lcdWriteReg(0x41,0x0000);
+	lld_lcdWriteReg(0x50,0x0000);
+	lld_lcdWriteReg(0x60,0x0005);
+	lld_lcdWriteReg(0x70,0x000B);
+	lld_lcdWriteReg(0x71,0x0000);
+	lld_lcdWriteReg(0x78,0x0000);
+	lld_lcdWriteReg(0x7A,0x0000);
+	lld_lcdWriteReg(0x79,0x0007);
+	lld_lcdWriteReg(0x07,0x0051);
+
+	s6d1121_delay(1);
+
+	lld_lcdWriteReg(0x07,0x0053);
+	lld_lcdWriteReg(0x79,0x0000);
+
+	lld_lcdResetWindow();
 }
 
-__inline void lld_lcdReadStream(uint16_t *buffer, size_t size) {
-	/* TODO */
+void lld_lcdSetCursor(uint16_t x, uint16_t y) {
+	/* R20h - 8 bit
+	 * R21h - 9 bit
+	 */
+	switch(lcdGetOrientation()) {
+		case portraitInv:
+			lld_lcdWriteReg(0x0020, (SCREEN_WIDTH-1-x) & 0x00FF);
+			lld_lcdWriteReg(0x0021, (SCREEN_HEIGHT-1-y) & 0x01FF);
+			break;
+		case portrait:
+			lld_lcdWriteReg(0x0020, x & 0x00FF);
+			lld_lcdWriteReg(0x0021, y & 0x01FF);
+			break;
+		case landscape:
+			lld_lcdWriteReg(0x0020, y & 0x00FF);
+			lld_lcdWriteReg(0x0021, x & 0x01FF);
+			break;
+		case landscapeInv:
+			lld_lcdWriteReg(0x0020, (SCREEN_WIDTH - y - 1) & 0x00FF);
+			lld_lcdWriteReg(0x0021, (SCREEN_HEIGHT - x - 1) & 0x01FF);
+			break;
+	}
 }
 
 void lld_lcdFillArea(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t color) {
@@ -265,6 +366,8 @@ void lld_lcdFillArea(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_
         lld_lcdWriteData(color);
 
     lld_lcdWriteStreamStop();
+
+    lld_lcdResetWindow();
 }
 
 // Do not use now, will be fixed in future
@@ -273,64 +376,85 @@ void lld_lcdSetOrientation(uint8_t newOrientation) {
 
     switch(orientation) {
         case portrait:
-            lcdWriteReg(0x03, 0x03);
+        	lld_lcdWriteReg(0x0001,0x0127);
+            lld_lcdWriteReg(0x03, 0b0011);
             lcd_height = SCREEN_HEIGHT;
             lcd_width = SCREEN_WIDTH;
             break;
         case landscape:
-        	// Not implemented yet
+        	lld_lcdWriteReg(0x0001,0x0027);
+        	lld_lcdWriteReg(0x0003, 0b1011);
             lcd_height = SCREEN_WIDTH;
             lcd_width = SCREEN_HEIGHT;
             break;
         case portraitInv:
-        	// Not implemented yet
+        	lld_lcdWriteReg(0x0001,0x0127);
+        	lld_lcdWriteReg(0x0003, 0b0000);
             lcd_height = SCREEN_HEIGHT;
             lcd_width = SCREEN_WIDTH;
             break;
         case landscapeInv:
-        	// Not implemented yet
+        	lld_lcdWriteReg(0x0001,0x0027);
+        	lld_lcdWriteReg(0x0003, 0b1000);
             lcd_height = SCREEN_WIDTH;
             lcd_width = SCREEN_HEIGHT;
             break;
     }
 }
 
+void lld_lcdResetWindow(void) {
+	switch(lcdGetOrientation()) {
+        case portrait:
+        case portraitInv:
+        	lld_lcdSetWindow(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT);
+            break;
+        case landscape:
+        case landscapeInv:
+        	lld_lcdSetWindow(0, 0, SCREEN_HEIGHT, SCREEN_WIDTH);
+            break;
+    }
+}
+
 void lld_lcdSetWindow(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1) {
+	/* HSA / HEA are 8 bit
+	 * VSA / VEA are 9 bit
+	 * use masks 0x00FF and 0x01FF to enforce this
+	 */
 	switch(lcdGetOrientation()) {
         case portrait:
-            lcdWriteReg(0x46, ((x1-1) << 8) | x0);
-            lcdWriteReg(0x48, y0);
-            lcdWriteReg(0x47, y1-1);
+            lld_lcdWriteReg(0x46, (((x1-1) & 0x00FF) << 8) | (x0 & 0x00FF));
+            lld_lcdWriteReg(0x48, y0 & 0x01FF);
+            lld_lcdWriteReg(0x47, (y1-1) & 0x01FF);
             break;
         case landscape:
-            lcdWriteReg(0x46, ((x0-1) << 8) | x1);
-            lcdWriteReg(0x48, x0);
-            lcdWriteReg(0x47, x1-1);
+            lld_lcdWriteReg(0x46, (((y1-1) & 0x00FF) << 8) | (y1 & 0x00FF));
+            lld_lcdWriteReg(0x48, x0 & 0x01FF);
+            lld_lcdWriteReg(0x47, (x1-1) & 0x01FF);
             break;
         case portraitInv:
-            lcdWriteReg(0x46, ((x1-1) << 8) | x0);
-            lcdWriteReg(0x48, y0);
-            lcdWriteReg(0x47, y1-1);
+            lld_lcdWriteReg(0x46, (((SCREEN_WIDTH-x0-1) & 0x00FF) << 8) | ((SCREEN_WIDTH - x1) & 0x00FF));
+            lld_lcdWriteReg(0x48, (SCREEN_HEIGHT-y1) & 0x01FF);
+            lld_lcdWriteReg(0x47, (SCREEN_HEIGHT-y0-1) & 0x01FF);
             break;
         case landscapeInv:
-            lcdWriteReg(0x46, ((y0-1) << 8) | y1);
-            lcdWriteReg(0x48, x0);
-            lcdWriteReg(0x47, x1-1);
+            lld_lcdWriteReg(0x46, (((SCREEN_WIDTH - y0 - 1) & 0x00FF) << 8) | ((SCREEN_WIDTH - y1) & 0x00FF));
+            lld_lcdWriteReg(0x48, (SCREEN_HEIGHT - x1) & 0x01FF);
+            lld_lcdWriteReg(0x47, (SCREEN_HEIGHT - x0 - 1) & 0x01FF);
             break;
     }
 
-	 lld_lcdSetCursor(x0, y0);
+	lld_lcdSetCursor(x0, y0);
 }
 
 void lld_lcdClear(uint16_t color) {
     uint32_t index = 0;
+    lld_lcdSetCursor(0, 0);
+    lld_lcdWriteStreamStart();
 
-    lld_lcdSetCursor(0,0);
-   	LCD_CS_LOW;
-    lld_lcdWriteIndex(0x0022);
     for(index = 0; index < SCREEN_WIDTH * SCREEN_HEIGHT; index++)
-        lld_lcdWriteData(color);
-    LCD_CS_HIGH;
+    	lld_lcdWriteData(color);
+
+    lld_lcdWriteStreamStop();
 }
 
 // Do not use!
@@ -338,18 +462,19 @@ uint16_t lld_lcdGetPixelColor(uint16_t x, uint16_t y) {
     uint16_t dummy;
 
     lld_lcdSetCursor(x,y);
-    LCD_CS_LOW;
-    lld_lcdWriteIndex(0x0022);
+    lld_lcdWriteStreamStart();
+
     dummy = lld_lcdReadData();
     dummy = lld_lcdReadData();
-    LCD_CS_HIGH;
+
+    lld_lcdWriteStreamStop();
 
 	return dummy;
 }
 
 void lld_lcdDrawPixel(uint16_t x, uint16_t y, uint16_t color) {
     lld_lcdSetCursor(x, y);
-    lcdWriteReg(0x0022, color);
+    lld_lcdWriteReg(0x0022, color);
 }
 
 uint16_t lld_lcdGetOrientation(void) {
@@ -369,7 +494,6 @@ uint16_t lld_lcdGetWidth(void) {
 void lld_lcdVerticalScroll(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, int16_t lines) {
 	uint16_t row0, row1;
 	uint16_t i;
-	lld_lcdSetWindow(x0, y0, x1, y1);
 
 	for(i = 0; i < ((y1-y0) - abs(lines)); i++) {
 		if(lines > 0) {
@@ -391,8 +515,9 @@ void lld_lcdVerticalScroll(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, i
 		lld_lcdWriteStream(buf, x1-x0);
 		lld_lcdWriteStreamStop();
 	}
+
+	lld_lcdResetWindow();
 }
 
 
 #endif
-