5 files changed, 107 insertions, 12 deletions

diff --git a/keyboards/ergodox_infinity/config.h b/keyboards/ergodox_infinity/config.h
index a00c593ee..275d4e1dc 100644
--- a/keyboards/ergodox_infinity/config.h
+++ b/keyboards/ergodox_infinity/config.h
@@ -77,6 +77,26 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define I2C1_SCL 0
 #define I2C1_SDA 1
 
+#ifdef ST7565_ENABLE
+/* LCD driver */
+#    define ST7565_A0_PIN C7
+#    define ST7565_RST_PIN C8
+#    define ST7565_SS_PIN C4
+#    define ST7565_SPI_CLK_DIVISOR 2
+#    define ST7565_CONTRAST 22
+#    define ST7565_DISPLAY_WIDTH 128
+#    define ST7565_DISPLAY_HEIGHT 32
+
+/* SPI (for LCD) */
+#    define SPI_DRIVER SPID1
+#    define SPI_SCK_PIN C5
+#    define SPI_SCK_PAL_MODE PAL_MODE_ALTERNATIVE_2
+#    define SPI_MOSI_PIN C6
+#    define SPI_MOSI_PAL_MODE PAL_MODE_ALTERNATIVE_2
+#    define SPI_MISO_PIN A4  // Just an unused pin, the "normal" MISO pin (C7) is used for other things
+#    define SPI_MISO_PAL_MODE PAL_MODE_ALTERNATIVE_7  // Default for A4
+#endif
+
 /* define if matrix has ghost */
 //#define MATRIX_HAS_GHOST
 
diff --git a/keyboards/ergodox_infinity/ergodox_infinity.c b/keyboards/ergodox_infinity/ergodox_infinity.c
index da8ea311a..b802bf014 100644
--- a/keyboards/ergodox_infinity/ergodox_infinity.c
+++ b/keyboards/ergodox_infinity/ergodox_infinity.c
@@ -128,7 +128,11 @@ static uint16_t cie_lightness(uint16_t v) {
     return y * 65535.0f;
 }
 
+#ifdef VISUALIZER_ENABLE
 void lcd_backlight_hal_color(uint16_t r, uint16_t g, uint16_t b) {
+#else
+void ergodox_infinity_lcd_color(uint16_t r, uint16_t g, uint16_t b) {
+#endif
     CHANNEL_RED.CnV   = cie_lightness(r);
     CHANNEL_GREEN.CnV = cie_lightness(g);
     CHANNEL_BLUE.CnV  = cie_lightness(b);
@@ -145,6 +149,13 @@ void keyboard_pre_init_kb() {
     setPinOutput(B16);
     writePinHigh(B16);
 #endif
+#ifndef VISUALIZER_ENABLE
+    // The backlight always has to be initialized, otherwise it will stay lit
+    lcd_backlight_hal_init();
+#    ifdef ST7565_ENABLE
+    ergodox_infinity_lcd_color(UINT16_MAX / 2, UINT16_MAX / 2, UINT16_MAX / 2);
+#    endif
+#endif
     keyboard_pre_init_user();
 }
 
@@ -165,10 +176,6 @@ void matrix_init_kb(void) {
 #endif
 
     matrix_init_user();
-    // The backlight always has to be initialized, otherwise it will stay lit
-#ifndef VISUALIZER_ENABLE
-    lcd_backlight_hal_init();
-#endif
 #if (defined(LED_MATRIX_ENABLE) || defined(WPM_ENABLE))
     add_remote_objects(remote_objects, sizeof(remote_objects) / sizeof(remote_object_t *));
 #endif
@@ -404,3 +411,64 @@ led_config_t g_led_config = {
     }
 };
 #endif
+
+#ifdef ST7565_ENABLE
+__attribute__((weak)) void st7565_on_user(void) {
+    ergodox_infinity_lcd_color(UINT16_MAX / 2, UINT16_MAX / 2, UINT16_MAX / 2);
+}
+
+__attribute__((weak)) void st7565_off_user(void) {
+    ergodox_infinity_lcd_color(0, 0, 0);
+}
+
+static void format_layer_bitmap_string(char* buffer, uint8_t offset) {
+    for (int i = 0; i < 16 && i + offset < MAX_LAYER; i++) {
+        if (i == 0 || i == 4 || i == 8 || i == 12) {
+            *buffer = ' ';
+            ++buffer;
+        }
+
+        uint8_t layer = i + offset;
+        if (layer_state_cmp(default_layer_state, layer)) {
+            *buffer = 'D';
+        } else if (layer_state_is(layer)) {
+            *buffer = '1';
+        } else {
+            *buffer = '_';
+        }
+        ++buffer;
+    }
+    *buffer = 0;
+}
+
+__attribute__((weak)) void st7565_task_user(void) {
+    if (is_keyboard_master()) {
+        // Draw led and layer status
+        led_t leds = host_keyboard_led_state();
+        if(leds.num_lock) { st7565_write("Num ", false); }
+        if(leds.caps_lock) { st7565_write("Cap ", false); }
+        if(leds.scroll_lock) { st7565_write("Scrl ", false); }
+        if(leds.compose) { st7565_write("Com ", false); }
+        if(leds.kana) { st7565_write("Kana", false); }
+        st7565_advance_page(true);
+
+        char layer_buffer[16 + 5];  // 3 spaces and one null terminator
+        st7565_set_cursor(0, 1);
+        format_layer_bitmap_string(layer_buffer, 0);
+        st7565_write_ln(layer_buffer, false);
+        format_layer_bitmap_string(layer_buffer, 16);
+        st7565_write_ln(layer_buffer, false);
+        st7565_write_ln("  1=On    D=Default", false);
+    } else {
+        // Draw logo
+        static const char qmk_logo[] = {
+            0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x8B, 0x8C, 0x8D, 0x8E, 0x8F, 0x90, 0x91, 0x92, 0x93, 0x94,
+            0xA0, 0xA1, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8, 0xA9, 0xAA, 0xAB, 0xAC, 0xAD, 0xAE, 0xAF, 0xB0, 0xB1, 0xB2, 0xB3, 0xB4,
+            0xC0, 0xC1, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xCB, 0xCC, 0xCD, 0xCE, 0xCF, 0xD0, 0xD1, 0xD2, 0xD3, 0xD4, 0x00
+        };
+
+        st7565_write(qmk_logo, false);
+        st7565_write("  Infinity  Ergodox  ", false);
+    }
+}
+#endif
diff --git a/keyboards/ergodox_infinity/ergodox_infinity.h b/keyboards/ergodox_infinity/ergodox_infinity.h
index 79f01b184..81ac1f338 100644
--- a/keyboards/ergodox_infinity/ergodox_infinity.h
+++ b/keyboards/ergodox_infinity/ergodox_infinity.h
@@ -80,6 +80,10 @@ inline void ergodox_led_all_set(uint8_t n) {
     ergodox_right_led_3_set(n);
 }
 
+#ifndef VISUALIZER_ENABLE
+void ergodox_infinity_lcd_color(uint16_t r, uint16_t g, uint16_t b);
+#endif
+
 /*
  *   LEFT HAND: LINES 88-95
  *  RIGHT HAND: LINES 97-104
diff --git a/keyboards/ergodox_infinity/keymaps/default/keymap.c b/keyboards/ergodox_infinity/keymaps/default/keymap.c
index cd9b476bb..ce8a3661e 100644
--- a/keyboards/ergodox_infinity/keymaps/default/keymap.c
+++ b/keyboards/ergodox_infinity/keymaps/default/keymap.c
@@ -167,7 +167,7 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
 // Runs just one time when the keyboard initializes.
 void matrix_init_user(void) {
 
-};
+}
 
 
 // Runs constantly in the background, in a loop.
@@ -192,4 +192,4 @@ void matrix_scan_user(void) {
             break;
     }
 
-};
+}
diff --git a/keyboards/ergodox_infinity/rules.mk b/keyboards/ergodox_infinity/rules.mk
index 70bcabe80..c7ff07d4e 100644
--- a/keyboards/ergodox_infinity/rules.mk
+++ b/keyboards/ergodox_infinity/rules.mk
@@ -20,19 +20,22 @@ SWAP_HANDS_ENABLE= yes # Allow swapping hands of keyboard
 
 CUSTOM_MATRIX = yes # Custom matrix file
 SERIAL_LINK_ENABLE = yes
-VISUALIZER_ENABLE = yes
-LCD_ENABLE = yes
-LCD_BACKLIGHT_ENABLE = yes
+
 MIDI_ENABLE = no
 RGBLIGHT_ENABLE = no
 
-LCD_DRIVER = st7565
-LCD_WIDTH = 128
-LCD_HEIGHT = 32
+ST7565_ENABLE = yes
 
 LED_MATRIX_ENABLE = yes
 LED_MATRIX_DRIVER = IS31FL3731
 
+# Config for Visualizer (set VISUALIZER_ENABLE = yes and ST7565_ENABLE = no to use)
+LCD_ENABLE = yes
+LCD_BACKLIGHT_ENABLE = yes
+LCD_DRIVER = st7565
+LCD_WIDTH = 128
+LCD_HEIGHT = 32
+
 # project specific files
 SRC = matrix.c \
       led.c