OLED Driver improvements (#20331)

Co-authored-by: Sergey Vlasov <sigprof@gmail.com>

2 files changed, 392 insertions, 55 deletions

diff --git a/drivers/oled/ssd1306_sh1106.c b/drivers/oled/oled_driver.c
index 342920572e..e50a881120 100644
--- a/drivers/oled/ssd1306_sh1106.c
+++ b/drivers/oled/oled_driver.c
@@ -14,20 +14,23 @@ GNU General Public License for more details.
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
-#include "i2c_master.h"
+
+#if defined(OLED_TRANSPORT_SPI)
+#    include "spi_master.h"
+#elif defined(OLED_TRANSPORT_I2C)
+#    include "i2c_master.h"
+#endif
 #include "oled_driver.h"
 #include OLED_FONT_H
 #include "timer.h"
 #include "print.h"
-
 #include <string.h>
-
 #include "progmem.h"
-
-#include "keyboard.h"
+#include "wait.h"
 
 // Used commands from spec sheet: https://cdn-shop.adafruit.com/datasheets/SSD1306.pdf
 // for SH1106: https://www.velleman.eu/downloads/29/infosheets/sh1106_datasheet.pdf
+// for SH1107: https://www.displayfuture.com/Display/datasheet/controller/SH1107.pdf
 
 // Fundamental Commands
 #define CONTRAST 0x81
@@ -82,6 +85,11 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 // Charge Pump Commands
 #define CHARGE_PUMP 0x8D
 
+// Commands specific to the SH1107 chip
+#define SH1107_DISPLAY_START_LINE 0xDC
+#define SH1107_MEMORY_MODE_PAGE 0x20
+#define SH1107_MEMORY_MODE_VERTICAL 0x21
+
 // Misc defines
 #ifndef OLED_BLOCK_COUNT
 #    define OLED_BLOCK_COUNT (sizeof(OLED_BLOCK_TYPE) * 8)
@@ -89,19 +97,43 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #ifndef OLED_BLOCK_SIZE
 #    define OLED_BLOCK_SIZE (OLED_MATRIX_SIZE / OLED_BLOCK_COUNT)
 #endif
+// Default display clock
+#if !defined(OLED_DISPLAY_CLOCK)
+#    define OLED_DISPLAY_CLOCK 0x80
+#endif
+// Default VCOMH deselect value
+#if !defined(OLED_VCOM_DETECT)
+#    define OLED_VCOM_DETECT 0x20
+#endif
+#if !defined(OLED_PRE_CHARGE_PERIOD)
+#    define OLED_PRE_CHARGE_PERIOD 0xF1
+#endif
+
 
 #define OLED_ALL_BLOCKS_MASK (((((OLED_BLOCK_TYPE)1 << (OLED_BLOCK_COUNT - 1)) - 1) << 1) | 1)
 
+#define OLED_IC_HAS_HORIZONTAL_MODE (OLED_IC == OLED_IC_SSD1306)
+#define OLED_IC_COM_PINS_ARE_COLUMNS (OLED_IC == OLED_IC_SH1107)
+
+#ifndef OLED_COM_PIN_COUNT
+#    if OLED_IC == OLED_IC_SSD1306
+#        define OLED_COM_PIN_COUNT 64
+#    elif OLED_IC == OLED_IC_SH1106
+#        define OLED_COM_PIN_COUNT 64
+#    elif OLED_IC == OLED_IC_SH1107
+#        define OLED_COM_PIN_COUNT 128
+#    else
+#        error Invalid OLED_IC value
+#    endif
+#endif
+
+#ifndef OLED_COM_PIN_OFFSET
+#    define OLED_COM_PIN_OFFSET 0
+#endif
+
 // i2c defines
 #define I2C_CMD 0x00
 #define I2C_DATA 0x40
-#if defined(__AVR__)
-#    define I2C_TRANSMIT_P(data) i2c_transmit_P((OLED_DISPLAY_ADDRESS << 1), &data[0], sizeof(data), OLED_I2C_TIMEOUT)
-#else // defined(__AVR__)
-#    define I2C_TRANSMIT_P(data) i2c_transmit((OLED_DISPLAY_ADDRESS << 1), &data[0], sizeof(data), OLED_I2C_TIMEOUT)
-#endif // defined(__AVR__)
-#define I2C_TRANSMIT(data) i2c_transmit((OLED_DISPLAY_ADDRESS << 1), &data[0], sizeof(data), OLED_I2C_TIMEOUT)
-#define I2C_WRITE_REG(mode, data, size) i2c_writeReg((OLED_DISPLAY_ADDRESS << 1), mode, data, size, OLED_I2C_TIMEOUT)
 
 #define HAS_FLAGS(bits, flags) ((bits & flags) == flags)
 
@@ -110,7 +142,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 // parts of the display unusable or don't get cleared correctly
 // and also allows for drawing & inverting
 uint8_t         oled_buffer[OLED_MATRIX_SIZE];
-uint8_t *       oled_cursor;
+uint8_t        *oled_cursor;
 OLED_BLOCK_TYPE oled_dirty          = 0;
 bool            oled_initialized    = false;
 bool            oled_active         = false;
@@ -132,24 +164,118 @@ uint32_t oled_scroll_timeout;
 uint16_t oled_update_timeout;
 #endif
 
-// Internal variables to reduce math instructions
+#if defined(OLED_TRANSPORT_SPI)
+#    ifndef OLED_DC_PIN
+#        error "The OLED driver in SPI needs a D/C pin defined"
+#    endif
+#    ifndef OLED_CS_PIN
+#        error "The OLED driver in SPI needs a CS pin defined"
+#    endif
+#    ifndef OLED_SPI_MODE
+#        define OLED_SPI_MODE 3
+#    endif
+#    ifndef OLED_SPI_DIVISOR
+#        define OLED_SPI_DIVISOR 2
+#    endif
+#elif defined(OLED_TRANSPORT_I2C)
+#    if !defined(OLED_DISPLAY_ADDRESS)
+#        define OLED_DISPLAY_ADDRESS 0x3C
+#    endif
+#endif
+
+// Transmit/Write Funcs.
+__attribute__((weak)) bool oled_send_cmd(const uint8_t *data, uint16_t size) {
+#if defined(OLED_TRANSPORT_SPI)
+    if (!spi_start(OLED_CS_PIN, false, OLED_SPI_MODE, OLED_SPI_DIVISOR)) {
+        return false;
+    }
+    // Command Mode
+    writePinLow(OLED_DC_PIN);
+    // Send the commands
+    if (spi_transmit(&data[1], size - 1) != SPI_STATUS_SUCCESS) {
+        spi_stop();
+        return false;
+    }
+    spi_stop();
+    return true;
+#elif defined(OLED_TRANSPORT_I2C)
+    i2c_status_t status = i2c_transmit((OLED_DISPLAY_ADDRESS << 1), data, size, OLED_I2C_TIMEOUT);
+
+    return (status == I2C_STATUS_SUCCESS);
+#endif
+}
 
+__attribute__((weak)) bool oled_send_cmd_P(const uint8_t *data, uint16_t size) {
 #if defined(__AVR__)
-// identical to i2c_transmit, but for PROGMEM since all initialization is in PROGMEM arrays currently
-// probably should move this into i2c_master...
-static i2c_status_t i2c_transmit_P(uint8_t address, const uint8_t *data, uint16_t length, uint16_t timeout) {
-    i2c_status_t status = i2c_start(address | I2C_WRITE, timeout);
+#    if defined(OLED_TRANSPORT_SPI)
+    if (!spi_start(OLED_CS_PIN, false, OLED_SPI_MODE, OLED_SPI_DIVISOR)) {
+        return false;
+    }
+    spi_status_t status = SPI_STATUS_SUCCESS;
+    // Command Mode
+    writePinLow(OLED_DC_PIN);
+    // Send the commands
+    for (uint16_t i = 1; i < size && status >= 0; i++) {
+        status = spi_write(pgm_read_byte((const char *)&data[i]));
+    }
+    spi_stop();
+    return (status >= 0);
+#    elif defined(OLED_TRANSPORT_I2C)
+    i2c_status_t status = i2c_start((OLED_DISPLAY_ADDRESS << 1) | I2C_WRITE, OLED_I2C_TIMEOUT);
 
-    for (uint16_t i = 0; i < length && status >= 0; i++) {
-        status = i2c_write(pgm_read_byte((const char *)data++), timeout);
-        if (status) break;
+    for (uint16_t i = 0; i < size && status >= 0; i++) {
+        status = i2c_write(pgm_read_byte((const char *)data++), OLED_I2C_TIMEOUT);
     }
 
     i2c_stop();
 
-    return status;
+    return (status == I2C_STATUS_SUCCESS);
+#    endif
+#else
+    return oled_send_cmd(data, size);
+#endif
+}
+
+__attribute__((weak)) bool oled_send_data(const uint8_t *data, uint16_t size) {
+#if defined(OLED_TRANSPORT_SPI)
+    if (!spi_start(OLED_CS_PIN, false, OLED_SPI_MODE, OLED_SPI_DIVISOR)) {
+        return false;
+    }
+    // Data Mode
+    writePinHigh(OLED_DC_PIN);
+    // Send the commands
+    if (spi_transmit(data, size) != SPI_STATUS_SUCCESS) {
+        spi_stop();
+        return false;
+    }
+    spi_stop();
+    return true;
+#elif defined(OLED_TRANSPORT_I2C)
+    i2c_status_t status = i2c_writeReg((OLED_DISPLAY_ADDRESS << 1), I2C_DATA, data, size, OLED_I2C_TIMEOUT);
+    return (status == I2C_STATUS_SUCCESS);
+#endif
 }
+
+__attribute__((weak)) void oled_driver_init(void) {
+#if defined(OLED_TRANSPORT_SPI)
+    spi_init();
+    setPinOutput(OLED_CS_PIN);
+    writePinHigh(OLED_CS_PIN);
+
+    setPinOutput(OLED_DC_PIN);
+    writePinLow(OLED_DC_PIN);
+#    ifdef OLED_RST_PIN
+    /* Reset device */
+    setPinOutput(OLED_RST_PIN);
+    writePinLow(OLED_RST_PIN);
+    wait_ms(20);
+    writePinHigh(OLED_RST_PIN);
+    wait_ms(20);
+#    endif
+#elif defined(OLED_TRANSPORT_I2C)
+    i2c_init();
 #endif
+}
 
 // Flips the rendering bits for a character at the current cursor position
 static void InvertCharacter(uint8_t *cursor) {
@@ -161,7 +287,7 @@ static void InvertCharacter(uint8_t *cursor) {
 }
 
 bool oled_init(oled_rotation_t rotation) {
-#if defined(USE_I2C) && defined(SPLIT_KEYBOARD)
+#if defined(USE_I2C) && defined(SPLIT_KEYBOARD) && defined(OLED_TRANSPORT_I2C)
     if (!is_keyboard_master()) {
         return true;
     }
@@ -173,47 +299,61 @@ bool oled_init(oled_rotation_t rotation) {
     } else {
         oled_rotation_width = OLED_DISPLAY_HEIGHT;
     }
-    i2c_init();
+    oled_driver_init();
 
     static const uint8_t PROGMEM display_setup1[] = {
         I2C_CMD,
         DISPLAY_OFF,
         DISPLAY_CLOCK,
-        0x80,
+        OLED_DISPLAY_CLOCK,
         MULTIPLEX_RATIO,
+#if OLED_IC_COM_PINS_ARE_COLUMNS
+        OLED_DISPLAY_WIDTH - 1,
+#else
         OLED_DISPLAY_HEIGHT - 1,
-        DISPLAY_OFFSET,
+#endif
+#if OLED_IC == OLED_IC_SH1107
+        SH1107_DISPLAY_START_LINE,
         0x00,
+#else
         DISPLAY_START_LINE | 0x00,
+#endif
         CHARGE_PUMP,
         0x14,
-#if (OLED_IC != OLED_IC_SH1106)
+#if OLED_IC_HAS_HORIZONTAL_MODE
         // MEMORY_MODE is unsupported on SH1106 (Page Addressing only)
         MEMORY_MODE,
         0x00, // Horizontal addressing mode
+#elif OLED_IC == OLED_IC_SH1107
+        // Page addressing mode
+        SH1107_MEMORY_MODE_PAGE,
 #endif
     };
-    if (I2C_TRANSMIT_P(display_setup1) != I2C_STATUS_SUCCESS) {
+    if (!oled_send_cmd_P(display_setup1, ARRAY_SIZE(display_setup1))) {
         print("oled_init cmd set 1 failed\n");
         return false;
     }
 
     if (!HAS_FLAGS(oled_rotation, OLED_ROTATION_180)) {
-        static const uint8_t PROGMEM display_normal[] = {I2C_CMD, SEGMENT_REMAP_INV, COM_SCAN_DEC};
-        if (I2C_TRANSMIT_P(display_normal) != I2C_STATUS_SUCCESS) {
+        static const uint8_t PROGMEM display_normal[] = {
+            I2C_CMD, SEGMENT_REMAP_INV, COM_SCAN_DEC, DISPLAY_OFFSET, OLED_COM_PIN_OFFSET,
+        };
+        if (!oled_send_cmd_P(display_normal, ARRAY_SIZE(display_normal))) {
             print("oled_init cmd normal rotation failed\n");
             return false;
         }
     } else {
-        static const uint8_t PROGMEM display_flipped[] = {I2C_CMD, SEGMENT_REMAP, COM_SCAN_INC};
-        if (I2C_TRANSMIT_P(display_flipped) != I2C_STATUS_SUCCESS) {
+        static const uint8_t PROGMEM display_flipped[] = {
+            I2C_CMD, SEGMENT_REMAP, COM_SCAN_INC, DISPLAY_OFFSET, (OLED_COM_PIN_COUNT - OLED_COM_PIN_OFFSET) % OLED_COM_PIN_COUNT,
+        };
+        if (!oled_send_cmd_P(display_flipped, ARRAY_SIZE(display_flipped))) {
             print("display_flipped failed\n");
             return false;
         }
     }
 
-    static const uint8_t PROGMEM display_setup2[] = {I2C_CMD, COM_PINS, OLED_COM_PINS, CONTRAST, OLED_BRIGHTNESS, PRE_CHARGE_PERIOD, 0xF1, VCOM_DETECT, 0x20, DISPLAY_ALL_ON_RESUME, NORMAL_DISPLAY, DEACTIVATE_SCROLL, DISPLAY_ON};
-    if (I2C_TRANSMIT_P(display_setup2) != I2C_STATUS_SUCCESS) {
+    static const uint8_t PROGMEM display_setup2[] = {I2C_CMD, COM_PINS, OLED_COM_PINS, CONTRAST, OLED_BRIGHTNESS, PRE_CHARGE_PERIOD, OLED_PRE_CHARGE_PERIOD, VCOM_DETECT, OLED_VCOM_DETECT, DISPLAY_ALL_ON_RESUME, NORMAL_DISPLAY, DEACTIVATE_SCROLL, DISPLAY_ON};
+    if (!oled_send_cmd_P(display_setup2, ARRAY_SIZE(display_setup2))) {
         print("display_setup2 failed\n");
         return false;
     }
@@ -249,30 +389,49 @@ static void calc_bounds(uint8_t update_start, uint8_t *cmd_array) {
     // Calculate commands to set memory addressing bounds.
     uint8_t start_page   = OLED_BLOCK_SIZE * update_start / OLED_DISPLAY_WIDTH;
     uint8_t start_column = OLED_BLOCK_SIZE * update_start % OLED_DISPLAY_WIDTH;
-#if (OLED_IC == OLED_IC_SH1106)
+#if !OLED_IC_HAS_HORIZONTAL_MODE
     // Commands for Page Addressing Mode. Sets starting page and column; has no end bound.
     // Column value must be split into high and low nybble and sent as two commands.
     cmd_array[0] = PAM_PAGE_ADDR | start_page;
     cmd_array[1] = PAM_SETCOLUMN_LSB | ((OLED_COLUMN_OFFSET + start_column) & 0x0f);
     cmd_array[2] = PAM_SETCOLUMN_MSB | ((OLED_COLUMN_OFFSET + start_column) >> 4 & 0x0f);
-    cmd_array[3] = NOP;
-    cmd_array[4] = NOP;
-    cmd_array[5] = NOP;
 #else
     // Commands for use in Horizontal Addressing mode.
-    cmd_array[1] = start_column;
+    cmd_array[1] = start_column + OLED_COLUMN_OFFSET;
     cmd_array[4] = start_page;
     cmd_array[2] = (OLED_BLOCK_SIZE + OLED_DISPLAY_WIDTH - 1) % OLED_DISPLAY_WIDTH + cmd_array[1];
-    cmd_array[5] = (OLED_BLOCK_SIZE + OLED_DISPLAY_WIDTH - 1) / OLED_DISPLAY_WIDTH - 1;
+    cmd_array[5] = (OLED_BLOCK_SIZE + OLED_DISPLAY_WIDTH - 1) / OLED_DISPLAY_WIDTH - 1 + cmd_array[4];
 #endif
 }
 
 static void calc_bounds_90(uint8_t update_start, uint8_t *cmd_array) {
-    cmd_array[1] = OLED_BLOCK_SIZE * update_start / OLED_DISPLAY_HEIGHT * 8;
-    cmd_array[4] = OLED_BLOCK_SIZE * update_start % OLED_DISPLAY_HEIGHT;
+    // Block numbering starts from the bottom left corner, going up and then to
+    // the right.  The controller needs the page and column numbers for the top
+    // left and bottom right corners of that block.
+
+    // Total number of pages across the screen height.
+    const uint8_t height_in_pages = OLED_DISPLAY_HEIGHT / 8;
+
+    // Difference of starting page numbers for adjacent blocks; may be 0 if
+    // blocks are large enough to occupy one or more whole 8px columns.
+    const uint8_t page_inc_per_block = OLED_BLOCK_SIZE % OLED_DISPLAY_HEIGHT / 8;
+
+    // Top page number for a block which is at the bottom edge of the screen.
+    const uint8_t bottom_block_top_page = (height_in_pages - page_inc_per_block) % height_in_pages;
+
+#if !OLED_IC_HAS_HORIZONTAL_MODE
+    // Only the Page Addressing Mode is supported
+    uint8_t start_page   = bottom_block_top_page - (OLED_BLOCK_SIZE * update_start % OLED_DISPLAY_HEIGHT / 8);
+    uint8_t start_column = OLED_BLOCK_SIZE * update_start / OLED_DISPLAY_HEIGHT * 8;
+    cmd_array[0]         = PAM_PAGE_ADDR | start_page;
+    cmd_array[1]         = PAM_SETCOLUMN_LSB | ((OLED_COLUMN_OFFSET + start_column) & 0x0f);
+    cmd_array[2]         = PAM_SETCOLUMN_MSB | ((OLED_COLUMN_OFFSET + start_column) >> 4 & 0x0f);
+#else
+    cmd_array[1] = OLED_BLOCK_SIZE * update_start / OLED_DISPLAY_HEIGHT * 8 + OLED_COLUMN_OFFSET;
+    cmd_array[4] = bottom_block_top_page - (OLED_BLOCK_SIZE * update_start % OLED_DISPLAY_HEIGHT / 8);
     cmd_array[2] = (OLED_BLOCK_SIZE + OLED_DISPLAY_HEIGHT - 1) / OLED_DISPLAY_HEIGHT * 8 - 1 + cmd_array[1];
-    ;
-    cmd_array[5] = (OLED_BLOCK_SIZE + OLED_DISPLAY_HEIGHT - 1) % OLED_DISPLAY_HEIGHT / 8;
+    cmd_array[5] = (OLED_BLOCK_SIZE + OLED_DISPLAY_HEIGHT - 1) % OLED_DISPLAY_HEIGHT / 8 + cmd_array[4];
+#endif
 }
 
 uint8_t crot(uint8_t a, int8_t n) {
@@ -309,7 +468,11 @@ void oled_render(void) {
         }
 
         // Set column & page position
+#if OLED_IC_HAS_HORIZONTAL_MODE
         static uint8_t display_start[] = {I2C_CMD, COLUMN_ADDR, 0, OLED_DISPLAY_WIDTH - 1, PAGE_ADDR, 0, OLED_DISPLAY_HEIGHT / 8 - 1};
+#else
+        static uint8_t display_start[] = {I2C_CMD, PAM_PAGE_ADDR, PAM_SETCOLUMN_LSB, PAM_SETCOLUMN_MSB};
+#endif
         if (!HAS_FLAGS(oled_rotation, OLED_ROTATION_90)) {
             calc_bounds(update_start, &display_start[1]); // Offset from I2C_CMD byte at the start
         } else {
@@ -317,14 +480,14 @@ void oled_render(void) {
         }
 
         // Send column & page position
-        if (I2C_TRANSMIT(display_start) != I2C_STATUS_SUCCESS) {
+        if (!oled_send_cmd(display_start, ARRAY_SIZE(display_start))) {
             print("oled_render offset command failed\n");
             return;
         }
 
         if (!HAS_FLAGS(oled_rotation, OLED_ROTATION_90)) {
             // Send render data chunk as is
-            if (I2C_WRITE_REG(I2C_DATA, &oled_buffer[OLED_BLOCK_SIZE * update_start], OLED_BLOCK_SIZE) != I2C_STATUS_SUCCESS) {
+            if (!oled_send_data(&oled_buffer[OLED_BLOCK_SIZE * update_start], OLED_BLOCK_SIZE)) {
                 print("oled_render data failed\n");
                 return;
             }
@@ -339,11 +502,32 @@ void oled_render(void) {
                 rotate_90(&oled_buffer[OLED_BLOCK_SIZE * update_start + source_map[i]], &temp_buffer[target_map[i]]);
             }
 
+#if OLED_IC_HAS_HORIZONTAL_MODE
             // Send render data chunk after rotating
-            if (I2C_WRITE_REG(I2C_DATA, &temp_buffer[0], OLED_BLOCK_SIZE) != I2C_STATUS_SUCCESS) {
+            if (!oled_send_data(&temp_buffer[0], OLED_BLOCK_SIZE)) {
                 print("oled_render90 data failed\n");
                 return;
             }
+#else
+            // For SH1106 or SH1107 the data chunk must be split into separate pieces for each page
+            const uint8_t columns_in_block = (OLED_BLOCK_SIZE + OLED_DISPLAY_HEIGHT - 1) / OLED_DISPLAY_HEIGHT * 8;
+            const uint8_t num_pages        = OLED_BLOCK_SIZE / columns_in_block;
+            for (uint8_t i = 0; i < num_pages; ++i) {
+                // Send column & page position for all pages except the first one
+                if (i > 0) {
+                    display_start[1]++;
+                    if (!oled_send_cmd(display_start, ARRAY_SIZE(display_start))) {
+                        print("oled_render offset command failed\n");
+                        return;
+                    }
+                }
+                // Send data for the page
+                if (!oled_send_data(&temp_buffer[columns_in_block * i], columns_in_block)) {
+                    print("oled_render90 data failed\n");
+                    return;
+                }
+            }
+#endif
         }
 
         // Clear dirty flag of just rendered block
@@ -568,7 +752,7 @@ bool oled_on(void) {
 #endif
 
     if (!oled_active) {
-        if (I2C_TRANSMIT_P(display_on) != I2C_STATUS_SUCCESS) {
+        if (!oled_send_cmd_P(display_on, ARRAY_SIZE(display_on))) {
             print("oled_on cmd failed\n");
             return oled_active;
         }
@@ -590,7 +774,7 @@ bool oled_off(void) {
 #endif
 
     if (oled_active) {
-        if (I2C_TRANSMIT_P(display_off) != I2C_STATUS_SUCCESS) {
+        if (!oled_send_cmd_P(display_off, ARRAY_SIZE(display_off))) {
             print("oled_off cmd failed\n");
             return oled_active;
         }
@@ -610,7 +794,7 @@ uint8_t oled_set_brightness(uint8_t level) {
 
     uint8_t set_contrast[] = {I2C_CMD, CONTRAST, level};
     if (oled_brightness != level) {
-        if (I2C_TRANSMIT(set_contrast) != I2C_STATUS_SUCCESS) {
+        if (!oled_send_cmd(set_contrast, ARRAY_SIZE(set_contrast))) {
             print("set_brightness cmd failed\n");
             return oled_brightness;
         }
@@ -657,7 +841,7 @@ bool oled_scroll_right(void) {
     // This prevents scrolling of bad data from starting the scroll too early after init
     if (!oled_dirty && !oled_scrolling) {
         uint8_t display_scroll_right[] = {I2C_CMD, SCROLL_RIGHT, 0x00, oled_scroll_start, oled_scroll_speed, oled_scroll_end, 0x00, 0xFF, ACTIVATE_SCROLL};
-        if (I2C_TRANSMIT(display_scroll_right) != I2C_STATUS_SUCCESS) {
+        if (!oled_send_cmd(display_scroll_right, ARRAY_SIZE(display_scroll_right))) {
             print("oled_scroll_right cmd failed\n");
             return oled_scrolling;
         }
@@ -675,7 +859,7 @@ bool oled_scroll_left(void) {
     // This prevents scrolling of bad data from starting the scroll too early after init
     if (!oled_dirty && !oled_scrolling) {
         uint8_t display_scroll_left[] = {I2C_CMD, SCROLL_LEFT, 0x00, oled_scroll_start, oled_scroll_speed, oled_scroll_end, 0x00, 0xFF, ACTIVATE_SCROLL};
-        if (I2C_TRANSMIT(display_scroll_left) != I2C_STATUS_SUCCESS) {
+        if (!oled_send_cmd(display_scroll_left, ARRAY_SIZE(display_scroll_left))) {
             print("oled_scroll_left cmd failed\n");
             return oled_scrolling;
         }
@@ -691,7 +875,7 @@ bool oled_scroll_off(void) {
 
     if (oled_scrolling) {
         static const uint8_t PROGMEM display_scroll_off[] = {I2C_CMD, DEACTIVATE_SCROLL};
-        if (I2C_TRANSMIT_P(display_scroll_off) != I2C_STATUS_SUCCESS) {
+        if (!oled_send_cmd_P(display_scroll_off, ARRAY_SIZE(display_scroll_off))) {
             print("oled_scroll_off cmd failed\n");
             return oled_scrolling;
         }
@@ -712,14 +896,14 @@ bool oled_invert(bool invert) {
 
     if (invert && !oled_inverted) {
         static const uint8_t PROGMEM display_inverted[] = {I2C_CMD, INVERT_DISPLAY};
-        if (I2C_TRANSMIT_P(display_inverted) != I2C_STATUS_SUCCESS) {
+        if (!oled_send_cmd_P(display_inverted, ARRAY_SIZE(display_inverted))) {
             print("oled_invert cmd failed\n");
             return oled_inverted;
         }
         oled_inverted = true;
     } else if (!invert && oled_inverted) {
         static const uint8_t PROGMEM display_normal[] = {I2C_CMD, NORMAL_DISPLAY};
-        if (I2C_TRANSMIT_P(display_normal) != I2C_STATUS_SUCCESS) {
+        if (!oled_send_cmd_P(display_normal, ARRAY_SIZE(display_normal))) {
             print("oled_invert cmd failed\n");
             return oled_inverted;
         }
diff --git a/drivers/oled/oled_driver.h b/drivers/oled/oled_driver.h
index 291049e36b..627a3da0ba 100644
--- a/drivers/oled/oled_driver.h
+++ b/drivers/oled/oled_driver.h
@@ -22,6 +22,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 // an enumeration of the chips this driver supports
 #define OLED_IC_SSD1306 0
 #define OLED_IC_SH1106 1
+#define OLED_IC_SH1107 2
 
 #if defined(OLED_DISPLAY_CUSTOM)
 // Expected user to implement the necessary defines
@@ -68,6 +69,152 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 // If OLED_BLOCK_TYPE is uint8_t, these tables would look like:
 // #define OLED_SOURCE_MAP { 0, 8, 16, 24, 32, 40, 48, 56, 64, 72, 80, 88, 96, 104, 112, 120 }
 // #define OLED_TARGET_MAP { 56, 120, 48, 112, 40, 104, 32, 96, 24, 88, 16, 80, 8, 72, 0, 64 }
+
+#elif defined(OLED_DISPLAY_64X32)
+#    ifndef OLED_DISPLAY_WIDTH
+#        define OLED_DISPLAY_WIDTH 64
+#    endif
+#    ifndef OLED_DISPLAY_HEIGHT
+#        define OLED_DISPLAY_HEIGHT 32
+#    endif
+#    ifndef OLED_COLUMN_OFFSET
+#        define OLED_COLUMN_OFFSET 32
+#    endif
+#    ifndef OLED_MATRIX_SIZE
+#        define OLED_MATRIX_SIZE (OLED_DISPLAY_HEIGHT / 8 * OLED_DISPLAY_WIDTH)
+#    endif
+#    ifndef OLED_BLOCK_TYPE
+#        define OLED_BLOCK_TYPE uint8_t
+#    endif
+#    ifndef OLED_BLOCK_COUNT
+#        define OLED_BLOCK_COUNT (sizeof(OLED_BLOCK_TYPE) * 8) // 8 (compile time mathed)
+#    endif
+#    ifndef OLED_BLOCK_SIZE
+#        define OLED_BLOCK_SIZE (OLED_MATRIX_SIZE / OLED_BLOCK_COUNT) // 32 (compile time mathed)
+#    endif
+#    ifndef OLED_COM_PINS
+#        define OLED_COM_PINS COM_PINS_ALT
+#    endif
+
+#    ifndef OLED_SOURCE_MAP
+#        define OLED_SOURCE_MAP \
+            { 0, 8, 16, 24 }
+#    endif
+#    ifndef OLED_TARGET_MAP
+#        define OLED_TARGET_MAP \
+            { 24, 16, 8, 0 }
+#    endif
+
+#elif defined(OLED_DISPLAY_64X48)
+#    ifndef OLED_DISPLAY_WIDTH
+#        define OLED_DISPLAY_WIDTH 64
+#    endif
+#    ifndef OLED_DISPLAY_HEIGHT
+#        define OLED_DISPLAY_HEIGHT 48
+#    endif
+#    ifndef OLED_COLUMN_OFFSET
+#        define OLED_COLUMN_OFFSET 32
+#    endif
+#    ifndef OLED_MATRIX_SIZE
+#        define OLED_MATRIX_SIZE (OLED_DISPLAY_HEIGHT / 8 * OLED_DISPLAY_WIDTH)
+#    endif
+#    ifndef OLED_BLOCK_TYPE
+#        define OLED_BLOCK_TYPE uint32_t
+#    endif
+#    ifndef OLED_BLOCK_COUNT
+#        define OLED_BLOCK_COUNT 24
+#    endif
+#    ifndef OLED_BLOCK_SIZE
+#        define OLED_BLOCK_SIZE (OLED_MATRIX_SIZE / OLED_BLOCK_COUNT)
+#    endif
+#    ifndef OLED_COM_PINS
+#        define OLED_COM_PINS COM_PINS_ALT
+#    endif
+
+#    ifndef OLED_SOURCE_MAP
+#        define OLED_SOURCE_MAP \
+            { 0, 8 }
+#    endif
+#    ifndef OLED_TARGET_MAP
+#        define OLED_TARGET_MAP \
+            { 8, 0 }
+#    endif
+
+#elif defined(OLED_DISPLAY_64X128)
+#    ifndef OLED_DISPLAY_WIDTH
+#        define OLED_DISPLAY_WIDTH 64
+#    endif
+#    ifndef OLED_DISPLAY_HEIGHT
+#        define OLED_DISPLAY_HEIGHT 128
+#    endif
+#    ifndef OLED_IC
+#        define OLED_IC OLED_IC_SH1107
+#    endif
+#    ifndef OLED_COM_PIN_OFFSET
+#        define OLED_COM_PIN_OFFSET 32
+#    endif
+#    ifndef OLED_MATRIX_SIZE
+#        define OLED_MATRIX_SIZE (OLED_DISPLAY_HEIGHT / 8 * OLED_DISPLAY_WIDTH)
+#    endif
+#    ifndef OLED_BLOCK_TYPE
+#        define OLED_BLOCK_TYPE uint16_t
+#    endif
+#    ifndef OLED_BLOCK_COUNT
+#        define OLED_BLOCK_COUNT (sizeof(OLED_BLOCK_TYPE) * 8)
+#    endif
+#    ifndef OLED_BLOCK_SIZE
+#        define OLED_BLOCK_SIZE (OLED_MATRIX_SIZE / OLED_BLOCK_COUNT)
+#    endif
+#    ifndef OLED_COM_PINS
+#        define OLED_COM_PINS COM_PINS_ALT
+#    endif
+
+#    ifndef OLED_SOURCE_MAP
+#        define OLED_SOURCE_MAP \
+            { 0, 8, 16, 24, 32, 40, 48, 56 }
+#    endif
+#    ifndef OLED_TARGET_MAP
+#        define OLED_TARGET_MAP \
+            { 56, 48, 40, 32, 24, 16, 8, 0 }
+#    endif
+
+#elif defined(OLED_DISPLAY_128X128)
+// Quad height 128x128
+#    ifndef OLED_DISPLAY_WIDTH
+#        define OLED_DISPLAY_WIDTH 128
+#    endif
+#    ifndef OLED_DISPLAY_HEIGHT
+#        define OLED_DISPLAY_HEIGHT 128
+#    endif
+#    ifndef OLED_IC
+#        define OLED_IC OLED_IC_SH1107
+#    endif
+#    ifndef OLED_MATRIX_SIZE
+#        define OLED_MATRIX_SIZE (OLED_DISPLAY_HEIGHT / 8 * OLED_DISPLAY_WIDTH) // 2048 (compile time mathed)
+#    endif
+#    ifndef OLED_BLOCK_TYPE
+#        define OLED_BLOCK_TYPE uint32_t
+#    endif
+#    ifndef OLED_BLOCK_COUNT
+#        define OLED_BLOCK_COUNT (sizeof(OLED_BLOCK_TYPE) * 8) // 32 (compile time mathed)
+#    endif
+#    ifndef OLED_BLOCK_SIZE
+#        define OLED_BLOCK_SIZE (OLED_MATRIX_SIZE / OLED_BLOCK_COUNT) // 64 (compile time mathed)
+#    endif
+#    ifndef OLED_COM_PINS
+#        define OLED_COM_PINS COM_PINS_ALT
+#    endif
+
+// For 90 degree rotation, we map our internal matrix to oled matrix using fixed arrays
+// The OLED writes to it's memory horizontally, starting top left, but our memory starts bottom left in this mode
+#    ifndef OLED_SOURCE_MAP
+#        define OLED_SOURCE_MAP \
+            { 0, 8, 16, 24, 32, 40, 48, 56 }
+#    endif
+#    ifndef OLED_TARGET_MAP
+#        define OLED_TARGET_MAP \
+            { 56, 48, 40, 32, 24, 16, 8, 0 }
+#    endif
 #else // defined(OLED_DISPLAY_128X64)
 // Default 128x32
 #    ifndef OLED_DISPLAY_WIDTH
@@ -191,6 +338,12 @@ typedef enum {
 // Returns true if the OLED was initialized successfully
 bool oled_init(oled_rotation_t rotation);
 
+// Send commands and data to screen
+bool oled_send_cmd(const uint8_t *data, uint16_t size);
+bool oled_send_cmd_P(const uint8_t *data, uint16_t size);
+bool oled_send_data(const uint8_t *data, uint16_t size);
+void oled_driver_init(void);
+
 // Called at the start of oled_init, weak function overridable by the user
 // rotation - the value passed into oled_init
 // Return new oled_rotation_t if you want to override default rotation