Hopefully finally fix the corrupt LCD

The SPI bus is now selected and deselected before each set of commands.
Also speed up things by buffering many commands into a single batch.

2 files changed, 228 insertions, 211 deletions

diff --git a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/board_ST7565.h b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/board_ST7565.h
index abda0bd85..c2092b5e8 100644
--- a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/board_ST7565.h
+++ b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/board_ST7565.h
@@ -37,10 +37,14 @@
 // MSB First
 // CLK Low by default
 static const SPIConfig spi1config = {
-        NULL,
-        /* HW dependent part.*/
-        ST7565_GPIOPORT,
-    ST7565_SS_PIN,
+   // Operation complete callback or @p NULL.
+  .end_cb = NULL,
+   //The chip select line port - when not using pcs.
+  .ssport = ST7565_GPIOPORT,
+   // brief The chip select line pad number - when not using pcs.
+  .sspad=ST7565_SS_PIN,
+   // SPI initialization data.
+  .tar0 =
     SPIx_CTARn_FMSZ(7)
     | SPIx_CTARn_ASC(7)
     | SPIx_CTARn_DT(7)
@@ -50,13 +54,10 @@ static const SPIConfig spi1config = {
         //SPI_CR1_BR_0
 };
 
-static bool_t st7565_is_data_mode = 1;
-
 static GFXINLINE void init_board(GDisplay *g) {
     (void) g;
     palSetPadModeNamed(A0, PAL_MODE_OUTPUT_PUSHPULL);
     palSetPad(ST7565_GPIOPORT, ST7565_A0_PIN);
-    st7565_is_data_mode = 1;
     palSetPadModeNamed(RST, PAL_MODE_OUTPUT_PUSHPULL);
     palSetPad(ST7565_GPIOPORT, ST7565_RST_PIN);
     palSetPadModeRaw(MOSI, ST7565_SPI_MODE);
@@ -65,7 +66,6 @@ static GFXINLINE void init_board(GDisplay *g) {
 
     spiInit();
     spiStart(&SPID1, &spi1config);
-    spiSelect(&SPID1);
 }
 
 static GFXINLINE void post_init_board(GDisplay *g) {
@@ -86,39 +86,27 @@ static GFXINLINE void acquire_bus(GDisplay *g) {
     (void) g;
     // Only the LCD is using the SPI bus, so no need to acquire
     // spiAcquireBus(&SPID1);
+    spiSelect(&SPID1);
 }
 
 static GFXINLINE void release_bus(GDisplay *g) {
     (void) g;
     // Only the LCD is using the SPI bus, so no need to release
     //spiReleaseBus(&SPID1);
+    spiUnselect(&SPID1);
 }
 
-static GFXINLINE void write_cmd(GDisplay *g, uint8_t cmd) {
-        (void) g;
-        if (st7565_is_data_mode) {
-            // The sleeps need to be at lest 10 vs 25 ns respectively
-            // So let's sleep two ticks, one tick might not be enough
-            // if we are at the end of the tick
-            chThdSleep(2);
-        palClearPad(ST7565_GPIOPORT, ST7565_A0_PIN);
-        chThdSleep(2);
-        st7565_is_data_mode = 0;
-        }
-        spiSend(&SPID1, 1, &cmd);
+static GFXINLINE void enter_data_mode(GDisplay *g) {
+    palSetPad(ST7565_GPIOPORT, ST7565_A0_PIN);
+}
+
+static GFXINLINE void enter_cmd_mode(GDisplay *g) {
+    palClearPad(ST7565_GPIOPORT, ST7565_A0_PIN);
 }
 
+
 static GFXINLINE void write_data(GDisplay *g, uint8_t* data, uint16_t length) {
         (void) g;
-        if (!st7565_is_data_mode) {
-            // The sleeps need to be at lest 10 vs 25 ns respectively
-            // So let's sleep two ticks, one tick might not be enough
-            // if we are at the end of the tick
-            chThdSleep(2);
-        palSetPad(ST7565_GPIOPORT, ST7565_A0_PIN);
-            chThdSleep(2);
-        st7565_is_data_mode = 1;
-        }
         spiSend(&SPID1, length, data);
 }
 
diff --git a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_ST7565.c b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_ST7565.c
index c33aea81a..4547f1419 100644
--- a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_ST7565.c
+++ b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_ST7565.c
@@ -20,16 +20,16 @@
 /*===========================================================================*/
 
 #ifndef GDISP_SCREEN_HEIGHT
-        #define GDISP_SCREEN_HEIGHT             32
+#define GDISP_SCREEN_HEIGHT             32
 #endif
 #ifndef GDISP_SCREEN_WIDTH
-        #define GDISP_SCREEN_WIDTH              128
+#define GDISP_SCREEN_WIDTH              128
 #endif
 #ifndef GDISP_INITIAL_CONTRAST
-        #define GDISP_INITIAL_CONTRAST  0
+#define GDISP_INITIAL_CONTRAST  0
 #endif
 #ifndef GDISP_INITIAL_BACKLIGHT
-        #define GDISP_INITIAL_BACKLIGHT 100
+#define GDISP_INITIAL_BACKLIGHT 100
 #endif
 
 #define GDISP_FLG_NEEDFLUSH                     (GDISP_FLG_DRIVER<<0)
@@ -40,16 +40,16 @@
 /* Driver config defaults for backward compatibility.                        */
 /*===========================================================================*/
 #ifndef ST7565_LCD_BIAS
-  #define ST7565_LCD_BIAS         ST7565_LCD_BIAS_7
+#define ST7565_LCD_BIAS         ST7565_LCD_BIAS_7
 #endif
 #ifndef ST7565_ADC
-  #define ST7565_ADC              ST7565_ADC_NORMAL
+#define ST7565_ADC              ST7565_ADC_NORMAL
 #endif
 #ifndef ST7565_COM_SCAN
-  #define ST7565_COM_SCAN         ST7565_COM_SCAN_INC
+#define ST7565_COM_SCAN         ST7565_COM_SCAN_INC
 #endif
 #ifndef ST7565_PAGE_ORDER
-  #define ST7565_PAGE_ORDER       0,1,2,3
+#define ST7565_PAGE_ORDER       0,1,2,3
 #endif
 
 /*===========================================================================*/
@@ -58,12 +58,24 @@
 
 typedef struct{
     bool_t buffer2;
+    uint8_t data_pos;
+    uint8_t data[16];
     uint8_t ram[GDISP_SCREEN_HEIGHT * GDISP_SCREEN_WIDTH / 8];
 }PrivData;
 
 // Some common routines and macros
 #define PRIV(g)                         ((PrivData*)g->priv)
 #define RAM(g)                                                  (PRIV(g)->ram)
+
+static GFXINLINE void write_cmd(GDisplay* g, uint8_t cmd) {
+    PRIV(g)->data[PRIV(g)->data_pos++] = cmd;
+}
+
+static GFXINLINE void flush_cmd(GDisplay* g) {
+    write_data(g, PRIV(g)->data, PRIV(g)->data_pos);
+    PRIV(g)->data_pos = 0;
+}
+
 #define write_cmd2(g, cmd1, cmd2)               { write_cmd(g, cmd1); write_cmd(g, cmd2); }
 #define write_cmd3(g, cmd1, cmd2, cmd3) { write_cmd(g, cmd1); write_cmd(g, cmd2); write_cmd(g, cmd3); }
 
@@ -86,207 +98,224 @@ typedef struct{
  */
 
 LLDSPEC bool_t gdisp_lld_init(GDisplay *g) {
-        // The private area is the display surface.
-        g->priv = gfxAlloc(sizeof(PrivData));
-        PRIV(g)->buffer2 = false;
-
-        // Initialise the board interface
-        init_board(g);
-
-        // Hardware reset
-        setpin_reset(g, TRUE);
-        gfxSleepMilliseconds(20);
-        setpin_reset(g, FALSE);
-        gfxSleepMilliseconds(20);
-
-        acquire_bus(g);
+    // The private area is the display surface.
+    g->priv = gfxAlloc(sizeof(PrivData));
+    PRIV(g)->buffer2 = false;
+    PRIV(g)->data_pos = 0;
+
+    // Initialise the board interface
+    init_board(g);
+
+    // Hardware reset
+    setpin_reset(g, TRUE);
+    gfxSleepMilliseconds(20);
+    setpin_reset(g, FALSE);
+    gfxSleepMilliseconds(20);
+
+    acquire_bus(g);
+    enter_cmd_mode(g);
     write_cmd(g, ST7565_DISPLAY_OFF);
-        write_cmd(g, ST7565_LCD_BIAS);
+    write_cmd(g, ST7565_LCD_BIAS);
     write_cmd(g, ST7565_ADC);
     write_cmd(g, ST7565_COM_SCAN);
-    
+
     write_cmd(g, ST7565_START_LINE | 0);
 
-        write_cmd(g, ST7565_RESISTOR_RATIO | 0x6);
+    write_cmd(g, ST7565_RESISTOR_RATIO | 0x6);
 
-        // turn on voltage converter (VC=1, VR=0, VF=0)
-        write_cmd(g, ST7565_POWER_CONTROL | 0x04);
-        delay_ms(50);
+    // turn on voltage converter (VC=1, VR=0, VF=0)
+    write_cmd(g, ST7565_POWER_CONTROL | 0x04);
+    flush_cmd(g);
+    delay_ms(50);
 
-        // turn on voltage regulator (VC=1, VR=1, VF=0)
-        write_cmd(g, ST7565_POWER_CONTROL | 0x06);
-        delay_ms(50);
+    // turn on voltage regulator (VC=1, VR=1, VF=0)
+    write_cmd(g, ST7565_POWER_CONTROL | 0x06);
+    flush_cmd(g);
+    delay_ms(50);
 
-        // turn on voltage follower (VC=1, VR=1, VF=1)
-        write_cmd(g, ST7565_POWER_CONTROL | 0x07);
-        delay_ms(50);
+    // turn on voltage follower (VC=1, VR=1, VF=1)
+    write_cmd(g, ST7565_POWER_CONTROL | 0x07);
+    flush_cmd(g);
+    delay_ms(50);
 
-        write_cmd(g, 0xE2);
+    write_cmd(g, 0xE2);
     write_cmd(g, ST7565_COM_SCAN);
-        write_cmd2(g, ST7565_CONTRAST, GDISP_INITIAL_CONTRAST*64/101);
-        //write_cmd2(g, ST7565_CONTRAST, 0);
-        write_cmd(g, ST7565_DISPLAY_ON);
-        write_cmd(g, ST7565_ALLON_NORMAL);
-        write_cmd(g, ST7565_INVERT_DISPLAY);
+    write_cmd2(g, ST7565_CONTRAST, GDISP_INITIAL_CONTRAST*64/101);
+    //write_cmd2(g, ST7565_CONTRAST, 0);
+    write_cmd(g, ST7565_DISPLAY_ON);
+    write_cmd(g, ST7565_ALLON_NORMAL);
+    write_cmd(g, ST7565_INVERT_DISPLAY);
 
-        write_cmd(g, ST7565_RMW);
+    write_cmd(g, ST7565_RMW);
+    flush_cmd(g);
 
     // Finish Init
     post_init_board(g);
 
-        // Release the bus
-        release_bus(g);
-
-        /* Initialise the GDISP structure */
-        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;
+    // Release the bus
+    release_bus(g);
+
+    /* Initialise the GDISP structure */
+    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;
 }
 
 #if GDISP_HARDWARE_FLUSH
-        LLDSPEC void gdisp_lld_flush(GDisplay *g) {
-                unsigned        p;
-
-                // Don't flush if we don't need it.
-                if (!(g->flags & GDISP_FLG_NEEDFLUSH))
-                        return;
-
-                acquire_bus(g);
-                unsigned dstOffset = (PRIV(g)->buffer2 ? 4 : 0);
-                for (p = 0; p < 4; p++) {
-                        write_cmd(g, ST7565_PAGE | (p + dstOffset));
-                        write_cmd(g, ST7565_COLUMN_MSB | 0);
-                        write_cmd(g, ST7565_COLUMN_LSB | 0);
-                        write_cmd(g, ST7565_RMW);
-                        write_data(g, RAM(g) + (p*GDISP_SCREEN_WIDTH), GDISP_SCREEN_WIDTH);
-                }
-                unsigned line = (PRIV(g)->buffer2 ? 32 : 0);
-        write_cmd(g, ST7565_START_LINE | line);
-        PRIV(g)->buffer2 = !PRIV(g)->buffer2;
-                release_bus(g);
-
-                g->flags &= ~GDISP_FLG_NEEDFLUSH;
-        }
+LLDSPEC void gdisp_lld_flush(GDisplay *g) {
+    unsigned    p;
+
+    // Don't flush if we don't need it.
+    if (!(g->flags & GDISP_FLG_NEEDFLUSH))
+        return;
+
+    acquire_bus(g);
+    enter_cmd_mode(g);
+    unsigned dstOffset = (PRIV(g)->buffer2 ? 4 : 0);
+    for (p = 0; p < 4; p++) {
+        write_cmd(g, ST7565_PAGE | (p + dstOffset));
+        write_cmd(g, ST7565_COLUMN_MSB | 0);
+        write_cmd(g, ST7565_COLUMN_LSB | 0);
+        write_cmd(g, ST7565_RMW);
+        flush_cmd(g);
+        enter_data_mode(g);
+        write_data(g, RAM(g) + (p*GDISP_SCREEN_WIDTH), GDISP_SCREEN_WIDTH);
+        enter_cmd_mode(g);
+    }
+    unsigned line = (PRIV(g)->buffer2 ? 32 : 0);
+    write_cmd(g, ST7565_START_LINE | line);
+    flush_cmd(g);
+    PRIV(g)->buffer2 = !PRIV(g)->buffer2;
+    release_bus(g);
+
+    g->flags &= ~GDISP_FLG_NEEDFLUSH;
+}
 #endif
 
 #if GDISP_HARDWARE_DRAWPIXEL
-        LLDSPEC void gdisp_lld_draw_pixel(GDisplay *g) {
-                coord_t         x, y;
-
-                switch(g->g.Orientation) {
-                default:
-                case GDISP_ROTATE_0:
-                        x = g->p.x;
-                        y = g->p.y;
-                        break;
-                case GDISP_ROTATE_90:
-                        x = g->p.y;
-                        y = GDISP_SCREEN_HEIGHT-1 - g->p.x;
-                        break;
-                case GDISP_ROTATE_180:
-                        x = GDISP_SCREEN_WIDTH-1 - g->p.x;
-                        y = GDISP_SCREEN_HEIGHT-1 - g->p.y;
-                        break;
-                case GDISP_ROTATE_270:
-                        x = GDISP_SCREEN_HEIGHT-1 - g->p.y;
-                        y = g->p.x;
-                        break;
-                }
-                if (gdispColor2Native(g->p.color) != Black)
-                        RAM(g)[xyaddr(x, y)] |= xybit(y);
-                else
-                        RAM(g)[xyaddr(x, y)] &= ~xybit(y);
-                g->flags |= GDISP_FLG_NEEDFLUSH;
-        }
+LLDSPEC void gdisp_lld_draw_pixel(GDisplay *g) {
+    coord_t             x, y;
+
+    switch(g->g.Orientation) {
+    default:
+    case GDISP_ROTATE_0:
+        x = g->p.x;
+        y = g->p.y;
+        break;
+    case GDISP_ROTATE_90:
+        x = g->p.y;
+        y = GDISP_SCREEN_HEIGHT-1 - g->p.x;
+        break;
+    case GDISP_ROTATE_180:
+        x = GDISP_SCREEN_WIDTH-1 - g->p.x;
+        y = GDISP_SCREEN_HEIGHT-1 - g->p.y;
+        break;
+    case GDISP_ROTATE_270:
+        x = GDISP_SCREEN_HEIGHT-1 - g->p.y;
+        y = g->p.x;
+        break;
+    }
+    if (gdispColor2Native(g->p.color) != Black)
+        RAM(g)[xyaddr(x, y)] |= xybit(y);
+    else
+        RAM(g)[xyaddr(x, y)] &= ~xybit(y);
+    g->flags |= GDISP_FLG_NEEDFLUSH;
+}
 #endif
 
 #if GDISP_HARDWARE_PIXELREAD
-        LLDSPEC color_t gdisp_lld_get_pixel_color(GDisplay *g) {
-                coord_t         x, y;
-
-                switch(g->g.Orientation) {
-                default:
-                case GDISP_ROTATE_0:
-                        x = g->p.x;
-                        y = g->p.y;
-                        break;
-                case GDISP_ROTATE_90:
-                        x = g->p.y;
-                        y = GDISP_SCREEN_HEIGHT-1 - g->p.x;
-                        break;
-                case GDISP_ROTATE_180:
-                        x = GDISP_SCREEN_WIDTH-1 - g->p.x;
-                        y = GDISP_SCREEN_HEIGHT-1 - g->p.y;
-                        break;
-                case GDISP_ROTATE_270:
-                        x = GDISP_SCREEN_HEIGHT-1 - g->p.y;
-                        y = g->p.x;
-                        break;
-                }
-                return (RAM(g)[xyaddr(x, y)] & xybit(y)) ? White : Black;
-        }
+LLDSPEC color_t gdisp_lld_get_pixel_color(GDisplay *g) {
+    coord_t             x, y;
+
+    switch(g->g.Orientation) {
+    default:
+    case GDISP_ROTATE_0:
+        x = g->p.x;
+        y = g->p.y;
+        break;
+    case GDISP_ROTATE_90:
+        x = g->p.y;
+        y = GDISP_SCREEN_HEIGHT-1 - g->p.x;
+        break;
+    case GDISP_ROTATE_180:
+        x = GDISP_SCREEN_WIDTH-1 - g->p.x;
+        y = GDISP_SCREEN_HEIGHT-1 - g->p.y;
+        break;
+    case GDISP_ROTATE_270:
+        x = GDISP_SCREEN_HEIGHT-1 - g->p.y;
+        y = g->p.x;
+        break;
+    }
+    return (RAM(g)[xyaddr(x, y)] & xybit(y)) ? White : Black;
+}
 #endif
 
 #if GDISP_NEED_CONTROL && GDISP_HARDWARE_CONTROL
-        LLDSPEC void gdisp_lld_control(GDisplay *g) {
-                switch(g->p.x) {
-                case GDISP_CONTROL_POWER:
-                        if (g->g.Powermode == (powermode_t)g->p.ptr)
-                                return;
-                        switch((powermode_t)g->p.ptr) {
-                        case powerOff:
-                        case powerSleep:
-                        case powerDeepSleep:
-                                acquire_bus(g);
-                                write_cmd(g, ST7565_DISPLAY_OFF);
-                                release_bus(g);
-                                break;
-                        case powerOn:
-                                acquire_bus(g);
-                                write_cmd(g, ST7565_DISPLAY_ON);
-                                release_bus(g);
-                                break;
-                        default:
-                                return;
-                        }
-                        g->g.Powermode = (powermode_t)g->p.ptr;
-                        return;
-
-                case GDISP_CONTROL_ORIENTATION:
-                        if (g->g.Orientation == (orientation_t)g->p.ptr)
-                                return;
-                        switch((orientation_t)g->p.ptr) {
-                        /* Rotation is handled by the drawing routines */
-                        case GDISP_ROTATE_0:
-                        case GDISP_ROTATE_180:
-                                g->g.Height = GDISP_SCREEN_HEIGHT;
-                                g->g.Width = GDISP_SCREEN_WIDTH;
-                                break;
-                        case GDISP_ROTATE_90:
-                        case GDISP_ROTATE_270:
-                                g->g.Height = GDISP_SCREEN_WIDTH;
-                                g->g.Width = GDISP_SCREEN_HEIGHT;
-                                break;
-                        default:
-                                return;
-                        }
-                        g->g.Orientation = (orientation_t)g->p.ptr;
-                        return;
-
-                case GDISP_CONTROL_CONTRAST:
-            if ((unsigned)g->p.ptr > 100)
-                g->p.ptr = (void *)100;
-                        acquire_bus(g);
-                        write_cmd2(g, ST7565_CONTRAST, ((((unsigned)g->p.ptr)<<6)/101) & 0x3F);
-                        release_bus(g);
-            g->g.Contrast = (unsigned)g->p.ptr;
-                        return;
-                }
-        }
+LLDSPEC void gdisp_lld_control(GDisplay *g) {
+    switch(g->p.x) {
+    case GDISP_CONTROL_POWER:
+        if (g->g.Powermode == (powermode_t)g->p.ptr)
+            return;
+        switch((powermode_t)g->p.ptr) {
+        case powerOff:
+        case powerSleep:
+        case powerDeepSleep:
+            acquire_bus(g);
+            enter_cmd_mode(g);
+            write_cmd(g, ST7565_DISPLAY_OFF);
+            flush_cmd(g);
+            release_bus(g);
+            break;
+        case powerOn:
+            acquire_bus(g);
+            enter_cmd_mode(g);
+            write_cmd(g, ST7565_DISPLAY_ON);
+            flush_cmd(g);
+            release_bus(g);
+            break;
+        default:
+            return;
+        }
+        g->g.Powermode = (powermode_t)g->p.ptr;
+        return;
+
+        case GDISP_CONTROL_ORIENTATION:
+            if (g->g.Orientation == (orientation_t)g->p.ptr)
+                return;
+            switch((orientation_t)g->p.ptr) {
+            /* Rotation is handled by the drawing routines */
+            case GDISP_ROTATE_0:
+            case GDISP_ROTATE_180:
+                g->g.Height = GDISP_SCREEN_HEIGHT;
+                g->g.Width = GDISP_SCREEN_WIDTH;
+                break;
+            case GDISP_ROTATE_90:
+            case GDISP_ROTATE_270:
+                g->g.Height = GDISP_SCREEN_WIDTH;
+                g->g.Width = GDISP_SCREEN_HEIGHT;
+                break;
+            default:
+                return;
+            }
+            g->g.Orientation = (orientation_t)g->p.ptr;
+            return;
+
+            case GDISP_CONTROL_CONTRAST:
+                if ((unsigned)g->p.ptr > 100)
+                    g->p.ptr = (void *)100;
+                acquire_bus(g);
+                enter_cmd_mode(g);
+                write_cmd2(g, ST7565_CONTRAST, ((((unsigned)g->p.ptr)<<6)/101) & 0x3F);
+                flush_cmd(g);
+                release_bus(g);
+                g->g.Contrast = (unsigned)g->p.ptr;
+                return;
+    }
+}
 #endif // GDISP_NEED_CONTROL
 
 #endif // GFX_USE_GDISP