3 files changed, 33 insertions, 6 deletions

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 4547f1419..2c8a168e7 100644
--- a/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_ST7565.c
+++ b/keyboards/ergodox/infinity/drivers/gdisp/st7565ergodox/gdisp_lld_ST7565.c
@@ -254,6 +254,32 @@ LLDSPEC color_t gdisp_lld_get_pixel_color(GDisplay *g) {
 }
 #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++;
+        }
+    }
+}
+
 #if GDISP_NEED_CONTROL && GDISP_HARDWARE_CONTROL
 LLDSPEC void gdisp_lld_control(GDisplay *g) {
     switch(g->p.x) {
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 48587b9e1..2b66a877c 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/gfxconf.h b/keyboards/ergodox/infinity/gfxconf.h
index 8caa577b7..45b9f5858 100644
--- a/keyboards/ergodox/infinity/gfxconf.h
+++ b/keyboards/ergodox/infinity/gfxconf.h
@@ -143,7 +143,7 @@
         #define GDISP_HARDWARE_DRAWPIXEL             TRUE
         #define GDISP_HARDWARE_CLEARS                FALSE
         #define GDISP_HARDWARE_FILLS                 FALSE
-        #define GDISP_HARDWARE_BITFILLS              FALSE
+        //#define GDISP_HARDWARE_BITFILLS              FALSE
         #define GDISP_HARDWARE_SCROLL                FALSE
         #define GDISP_HARDWARE_PIXELREAD             TRUE
         #define GDISP_HARDWARE_CONTROL               TRUE