Merge tag '0.5.43' into coderkun_neo2

5 files changed, 270 insertions, 271 deletions

diff --git a/keyboards/ergodox/infinity/drivers/gdisp/IS31FL3731C/gdisp_IS31FL3731C.c b/keyboards/ergodox/infinity/drivers/gdisp/IS31FL3731C/gdisp_IS31FL3731C.c
index 1d21f0c498..ea09c4bb0c 100644
--- a/keyboards/ergodox/infinity/drivers/gdisp/IS31FL3731C/gdisp_IS31FL3731C.c
+++ b/keyboards/ergodox/infinity/drivers/gdisp/IS31FL3731C/gdisp_IS31FL3731C.c
@@ -25,6 +25,10 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include "board_IS31FL3731C.h"
 
+
+// Can't include led_tables from here
+extern const uint8_t CIE1931_CURVE[];
+
 /*===========================================================================*/
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
@@ -100,37 +104,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #define IS31
 
-//Generated by http://jared.geek.nz/2013/feb/linear-led-pwm
-const unsigned char cie[256] = {
-    0, 0, 0, 0, 0, 1, 1, 1, 1, 1,
-    1, 1, 1, 1, 2, 2, 2, 2, 2, 2,
-    2, 2, 2, 3, 3, 3, 3, 3, 3, 3,
-    3, 4, 4, 4, 4, 4, 4, 5, 5, 5,
-    5, 5, 6, 6, 6, 6, 6, 7, 7, 7,
-    7, 8, 8, 8, 8, 9, 9, 9, 10, 10,
-    10, 10, 11, 11, 11, 12, 12, 12, 13, 13,
-    13, 14, 14, 15, 15, 15, 16, 16, 17, 17,
-    17, 18, 18, 19, 19, 20, 20, 21, 21, 22,
-    22, 23, 23, 24, 24, 25, 25, 26, 26, 27,
-    28, 28, 29, 29, 30, 31, 31, 32, 32, 33,
-    34, 34, 35, 36, 37, 37, 38, 39, 39, 40,
-    41, 42, 43, 43, 44, 45, 46, 47, 47, 48,
-    49, 50, 51, 52, 53, 54, 54, 55, 56, 57,
-    58, 59, 60, 61, 62, 63, 64, 65, 66, 67,
-    68, 70, 71, 72, 73, 74, 75, 76, 77, 79,
-    80, 81, 82, 83, 85, 86, 87, 88, 90, 91,
-    92, 94, 95, 96, 98, 99, 100, 102, 103, 105,
-    106, 108, 109, 110, 112, 113, 115, 116, 118, 120,
-    121, 123, 124, 126, 128, 129, 131, 132, 134, 136,
-    138, 139, 141, 143, 145, 146, 148, 150, 152, 154,
-    155, 157, 159, 161, 163, 165, 167, 169, 171, 173,
-    175, 177, 179, 181, 183, 185, 187, 189, 191, 193,
-    196, 198, 200, 202, 204, 207, 209, 211, 214, 216,
-    218, 220, 223, 225, 228, 230, 232, 235, 237, 240,
-    242, 245, 247, 250, 252, 255,
-};
-
-
 /*===========================================================================*/
 /* Driver local functions.                                                   */
 /*===========================================================================*/
@@ -231,7 +204,7 @@ LLDSPEC bool_t gdisp_lld_init(GDisplay *g) {
 		uint8_t* src = PRIV(g)->frame_buffer;
 		for (int y=0;y<GDISP_SCREEN_HEIGHT;y++) {
 		    for (int x=0;x<GDISP_SCREEN_WIDTH;x++) {
-		        PRIV(g)->write_buffer[get_led_address(g, x, y)]=cie[*src];
+		        PRIV(g)->write_buffer[get_led_address(g, x, y)]=CIE1931_CURVE[*src];
 		        ++src;
 		    }
 		}
diff --git a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/board_ST7565.h b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/board_ST7565.h
index 290571ce5b..9650ffb440 100644
--- a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/board_ST7565.h
+++ b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/board_ST7565.h
@@ -8,8 +8,6 @@
 #ifndef _GDISP_LLD_BOARD_H
 #define _GDISP_LLD_BOARD_H
 
-#include "print.h"
-
 #define ST7565_LCD_BIAS         ST7565_LCD_BIAS_9 // actually 6
 #define ST7565_ADC              ST7565_ADC_NORMAL
 #define ST7565_COM_SCAN         ST7565_COM_SCAN_DEC
@@ -39,35 +37,49 @@
 // MSB First
 // CLK Low by default
 static const SPIConfig spi1config = {
-	NULL,
-	/* HW dependent part.*/
-	ST7565_GPIOPORT,
-    ST7565_SS_PIN,
-    SPIx_CTARn_FMSZ(7)
-    | SPIx_CTARn_ASC(7)
-    | SPIx_CTARn_DT(7)
-    | SPIx_CTARn_CSSCK(7)
-    | SPIx_CTARn_PBR(0)
-    | SPIx_CTARn_BR(7)
-	//SPI_CR1_BR_0
+   // 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) // Frame size = 8 bytes
+    | SPIx_CTARn_ASC(1) // After SCK Delay Scaler (min 50 ns) = 55.56ns
+    | SPIx_CTARn_DT(0) // Delay After Transfer Scaler (no minimum)= 27.78ns
+    | SPIx_CTARn_CSSCK(0) // PCS to SCK Delay Scaler (min 20 ns) = 27.78ns
+    | SPIx_CTARn_PBR(0) // Baud Rate Prescaler = 2
+    | SPIx_CTARn_BR(0) // Baud rate (min 50ns) = 55.56ns
 };
 
-static bool_t st7565_is_data_mode = 1;
+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 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);
     palSetPadModeRaw(SLCK, ST7565_SPI_MODE);
-    palSetPadModeRaw(SS, ST7565_SPI_MODE);
+    palSetPadModeNamed(SS, PAL_MODE_OUTPUT_PUSHPULL);
 
     spiInit();
     spiStart(&SPID1, &spi1config);
-    spiSelect(&SPID1);
+    release_bus(g);
 }
 
 static GFXINLINE void post_init_board(GDisplay *g) {
@@ -84,43 +96,17 @@ static GFXINLINE void setpin_reset(GDisplay *g, bool_t state) {
     }
 }
 
-static GFXINLINE void acquire_bus(GDisplay *g) {
-    (void) g;
-    // Only the LCD is using the SPI bus, so no need to acquire
-    // spiAcquireBus(&SPID1);
+static GFXINLINE void enter_data_mode(GDisplay *g) {
+    palSetPad(ST7565_GPIOPORT, ST7565_A0_PIN);
 }
 
-static GFXINLINE void release_bus(GDisplay *g) {
-    (void) g;
-    // Only the LCD is using the SPI bus, so no need to release
-    //spiReleaseBus(&SPID1);
+static GFXINLINE void enter_cmd_mode(GDisplay *g) {
+    palClearPad(ST7565_GPIOPORT, ST7565_A0_PIN);
 }
 
-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 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 c33aea81a8..b04ad02935 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	35
 #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,232 @@ 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);
-    write_cmd(g, ST7565_DISPLAY_OFF);
-	write_cmd(g, ST7565_LCD_BIAS);
+    // 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_RESET);
+    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);
-
-	// turn on voltage converter (VC=1, VR=0, VF=0)
-	write_cmd(g, ST7565_POWER_CONTROL | 0x04);
-	delay_ms(50);
 
-	// turn on voltage regulator (VC=1, VR=1, VF=0)
-	write_cmd(g, ST7565_POWER_CONTROL | 0x06);
-	delay_ms(50);
+    write_cmd(g, ST7565_RESISTOR_RATIO | 0x1);
+    write_cmd2(g, ST7565_CONTRAST, GDISP_INITIAL_CONTRAST);
 
-	// turn on voltage follower (VC=1, VR=1, VF=1)
-	write_cmd(g, ST7565_POWER_CONTROL | 0x07);
-	delay_ms(50);
+    // turn on internal power supply (VC=1, VR=1, VF=1)
+    write_cmd(g, ST7565_POWER_CONTROL | 0x07);
 
-	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_cmd(g, ST7565_INVERT_DISPLAY);
+    write_cmd(g, ST7565_ALLON_NORMAL);
 
-	write_cmd(g, ST7565_RMW);
+    write_cmd(g, ST7565_START_LINE | 0);
+    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 = powerOff;
+    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
 
+LLDSPEC void gdisp_lld_blit_area(GDisplay *g) {
+    uint8_t* buffer = (uint8_t*)g->p.ptr;
+    int linelength = g->p.cx;
+    for (int i = 0; i < g->p.cy; i++) {
+        unsigned dstx = g->p.x;
+        unsigned dsty = g->p.y + i;
+        unsigned srcx = g->p.x1;
+        unsigned srcy = g->p.y1 + i;
+        unsigned srcbit = srcy * g->p.x2 + srcx;
+        for(int j=0; j < linelength; j++) {
+            uint8_t src = buffer[srcbit / 8];
+            uint8_t bit = 7-(srcbit % 8);
+            uint8_t bitset = (src >> bit) & 1;
+            uint8_t* dst = &(RAM(g)[xyaddr(dstx, dsty)]);
+            if (bitset) {
+                *dst |= xybit(dsty);
+            }
+            else {
+                *dst &= ~xybit(dsty);
+            }
+			dstx++;
+            srcbit++;
+        }
+    }
+    g->flags |= GDISP_FLG_NEEDFLUSH;
+}
+
 #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:
+                g->g.Contrast = (unsigned)g->p.ptr & 63;
+                acquire_bus(g);
+                enter_cmd_mode(g);
+                write_cmd2(g, ST7565_CONTRAST, g->g.Contrast);
+                flush_cmd(g);
+                release_bus(g);
+                return;
+    }
+}
 #endif // GDISP_NEED_CONTROL
 
 #endif // GFX_USE_GDISP
diff --git a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_config.h b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_config.h
index 48587b9e14..2b66a877c8 100644
--- a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_config.h
+++ b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_config.h
@@ -14,12 +14,13 @@
 /* Driver hardware support.                                                  */
 /*===========================================================================*/
 
-#define GDISP_HARDWARE_FLUSH			TRUE		// This controller requires flushing
-#define GDISP_HARDWARE_DRAWPIXEL		TRUE
-#define GDISP_HARDWARE_PIXELREAD		TRUE
-#define GDISP_HARDWARE_CONTROL			TRUE
+#define GDISP_HARDWARE_FLUSH            TRUE		// This controller requires flushing
+#define GDISP_HARDWARE_DRAWPIXEL        TRUE
+#define GDISP_HARDWARE_PIXELREAD        TRUE
+#define GDISP_HARDWARE_CONTROL          TRUE
+#define GDISP_HARDWARE_BITFILLS         TRUE
 
-#define GDISP_LLD_PIXELFORMAT			GDISP_PIXELFORMAT_MONO
+#define GDISP_LLD_PIXELFORMAT           GDISP_PIXELFORMAT_MONO
 
 #endif	/* GFX_USE_GDISP */
 
diff --git a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/st7565.h b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/st7565.h
index 48636b33d6..24924ff050 100644
--- a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/st7565.h
+++ b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/st7565.h
@@ -34,4 +34,6 @@
 #define ST7565_RESISTOR_RATIO       0x20
 #define ST7565_POWER_CONTROL        0x28
 
+#define ST7565_RESET                0xE2
+
 #endif /* _ST7565_H */