From 90ad93c41fa86534479f7006ec2c3956da9eac3c Mon Sep 17 00:00:00 2001
From: inmarket <andrewh@inmarket.com.au>
Date: Thu, 24 Oct 2013 11:59:41 +1000
Subject: [PATCH] Updates to SSD1306 driver to raise thread priority during
 flushing.

---
 drivers/gdisp/SSD1306/board_SSD1306_i2c.h | 15 +++++++++++++--
 drivers/gdisp/SSD1306/board_SSD1306_spi.h | 13 ++++++++++++-
 drivers/gdisp/SSD1306/gdisp_lld.c         |  2 +-
 3 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/drivers/gdisp/SSD1306/board_SSD1306_i2c.h b/drivers/gdisp/SSD1306/board_SSD1306_i2c.h
index c89562e0..69c054f5 100644
--- a/drivers/gdisp/SSD1306/board_SSD1306_i2c.h
+++ b/drivers/gdisp/SSD1306/board_SSD1306_i2c.h
@@ -41,6 +41,10 @@
 // I2C configuration structure.
 static I2CConfig i2cconfig;
 
+#if GFX_USE_OS_CHIBIOS
+	static int32_t thdPriority = 0;
+#endif
+
 static inline void init_board(GDisplay *g) {
 
 	// As we are not using multiple displays we set g->board to NULL as we don't use it.
@@ -62,10 +66,10 @@ static inline void init_board(GDisplay *g) {
 		 * 0x0030020A;		// 400kHz Fast Mode
 		 * 0x00100002;		// 800kHz Fast Mode +
 		 */
-		i2cconfig.timingr = 0x00100002;		// 800kHz Fast Mode+
-		i2cInit();
 		palSetPadMode(SSD1306_SCL_PORT, SSD1306_SCL_PIN, PAL_MODE_ALTERNATE(1));
 		palSetPadMode(SSD1306_SDA_PORT, SSD1306_SDA_PIN, PAL_MODE_ALTERNATE(1));
+		i2cconfig.timingr = 0x00100002;		// 800kHz Fast Mode+
+		i2cInit();
 		break;
 	}
 }
@@ -84,11 +88,18 @@ static inline void setpin_reset(GDisplay *g, bool_t state) {
 
 static inline void acquire_bus(GDisplay *g) {
 	(void) g;
+	#if GFX_USE_OS_CHIBIOS
+		thdPriority = (int32_t)chThdGetPriority();
+		chThdSetPriority(HIGHPRIO);
+	#endif
 	i2cAcquireBus(&I2CD1);
 }
 
 static inline void release_bus(GDisplay *g) {
 	(void) g;
+	#if GFX_USE_OS_CHIBIOS
+		chThdSetPriority(thdPriority);
+	#endif
 	i2cReleaseBus(&I2CD1);
 }
 
diff --git a/drivers/gdisp/SSD1306/board_SSD1306_spi.h b/drivers/gdisp/SSD1306/board_SSD1306_spi.h
index e206a517..476c51bf 100644
--- a/drivers/gdisp/SSD1306/board_SSD1306_spi.h
+++ b/drivers/gdisp/SSD1306/board_SSD1306_spi.h
@@ -44,6 +44,10 @@ static const SPIConfig spi1config = {
 	//SPI_CR1_BR_0
 };
 
+#if GFX_USE_OS_CHIBIOS
+	static int32_t thdPriority = 0;
+#endif
+
 static inline void init_board(GDisplay *g) {
 
 	// As we are not using multiple displays we set g->board to NULL as we don't use it.
@@ -54,7 +58,6 @@ static inline void init_board(GDisplay *g) {
 		// RESET pin.
 		palSetPadMode(SSD1306_RESET_PORT, SSD1306_RESET_PIN, PAL_MODE_OUTPUT_PUSHPULL);
 
-		spiInit();
 		palSetPadMode(SSD1306_MISO_PORT, SSD1306_MISO_PIN, 	PAL_MODE_ALTERNATE(1)|
 															PAL_STM32_OSPEED_HIGHEST);
 		palSetPadMode(SSD1306_MOSI_PORT, SSD1306_MOSI_PIN, 	PAL_MODE_ALTERNATE(1)|
@@ -64,6 +67,7 @@ static inline void init_board(GDisplay *g) {
 		palSetPad(SSD1306_CS_PORT, SSD1306_CS_PIN);
 		palSetPadMode(SSD1306_CS_PORT,   SSD1306_CS_PIN,   	PAL_MODE_ALTERNATE(1)|
 															PAL_STM32_OSPEED_HIGHEST);
+		spiInit();
 		break;
 	}
 }
@@ -82,11 +86,18 @@ static inline void setpin_reset(GDisplay *g, bool_t state) {
 
 static inline void acquire_bus(GDisplay *g) {
 	(void) g;
+	#if GFX_USE_OS_CHIBIOS
+		thdPriority = (int32_t)chThdGetPriority();
+		chThdSetPriority(HIGHPRIO);
+	#endif
 	spiAcquireBus(&SPID1);
 }
 
 static inline void release_bus(GDisplay *g) {
 	(void) g;
+	#if GFX_USE_OS_CHIBIOS
+		chThdSetPriority(thdPriority);
+	#endif
 	spiReleaseBus(&SPID1);
 }
 
diff --git a/drivers/gdisp/SSD1306/gdisp_lld.c b/drivers/gdisp/SSD1306/gdisp_lld.c
index 079c9256..81b3b692 100644
--- a/drivers/gdisp/SSD1306/gdisp_lld.c
+++ b/drivers/gdisp/SSD1306/gdisp_lld.c
@@ -12,7 +12,7 @@
 
 #include "gfx.h"
 
-#if GFX_USE_GDISP || defined(__DOXYGEN__)
+#if GFX_USE_GDISP
 
 #define GDISP_DRIVER_VMT			GDISPVMT_SSD1306
 #include "../drivers/gdisp/SSD1306/gdisp_lld_config.h"