1 files changed, 31 insertions, 17 deletions

diff --git a/drivers/oled/oled_driver.c b/drivers/oled/oled_driver.c
index 0e15c3c4b..0b24a987d 100644
--- a/drivers/oled/oled_driver.c
+++ b/drivers/oled/oled_driver.c
@@ -75,8 +75,14 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define CHARGE_PUMP 0x8D
 
 // Misc defines
-#define OLED_BLOCK_COUNT (sizeof(OLED_BLOCK_TYPE) * 8)
-#define OLED_BLOCK_SIZE (OLED_MATRIX_SIZE / OLED_BLOCK_COUNT)
+#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
+
+#define OLED_ALL_BLOCKS_MASK (((((OLED_BLOCK_TYPE)1 << (OLED_BLOCK_COUNT - 1)) - 1) << 1) | 1)
 
 // i2c defines
 #define I2C_CMD 0x00
@@ -212,7 +218,7 @@ __attribute__((weak)) oled_rotation_t oled_init_user(oled_rotation_t rotation) {
 void oled_clear(void) {
     memset(oled_buffer, 0, sizeof(oled_buffer));
     oled_cursor = &oled_buffer[0];
-    oled_dirty  = -1;  // -1 will be max value as long as display_dirty is unsigned type
+    oled_dirty  = OLED_ALL_BLOCKS_MASK;
 }
 
 static void calc_bounds(uint8_t update_start, uint8_t *cmd_array) {
@@ -262,13 +268,14 @@ static void rotate_90(const uint8_t *src, uint8_t *dest) {
 
 void oled_render(void) {
     // Do we have work to do?
+    oled_dirty &= OLED_ALL_BLOCKS_MASK;
     if (!oled_dirty || oled_scrolling) {
         return;
     }
 
     // Find first dirty block
     uint8_t update_start = 0;
-    while (!(oled_dirty & (1 << update_start))) {
+    while (!(oled_dirty & ((OLED_BLOCK_TYPE)1 << update_start))) {
         ++update_start;
     }
 
@@ -314,7 +321,7 @@ void oled_render(void) {
     oled_on();
 
     // Clear dirty flag
-    oled_dirty &= ~(1 << update_start);
+    oled_dirty &= ~((OLED_BLOCK_TYPE)1 << update_start);
 }
 
 void oled_set_cursor(uint8_t col, uint8_t line) {
@@ -404,9 +411,9 @@ void oled_write_char(const char data, bool invert) {
     // Dirty check
     if (memcmp(&oled_temp_buffer, oled_cursor, OLED_FONT_WIDTH)) {
         uint16_t index = oled_cursor - &oled_buffer[0];
-        oled_dirty |= (1 << (index / OLED_BLOCK_SIZE));
+        oled_dirty |= ((OLED_BLOCK_TYPE)1 << (index / OLED_BLOCK_SIZE));
         // Edgecase check if the written data spans the 2 chunks
-        oled_dirty |= (1 << ((index + OLED_FONT_WIDTH) / OLED_BLOCK_SIZE));
+        oled_dirty |= ((OLED_BLOCK_TYPE)1 << ((index + OLED_FONT_WIDTH - 1) / OLED_BLOCK_SIZE));
     }
 
     // Finally move to the next char
@@ -441,7 +448,7 @@ void oled_pan(bool left) {
             }
         }
     }
-    oled_dirty = ~((OLED_BLOCK_TYPE)0);
+    oled_dirty = OLED_ALL_BLOCKS_MASK;
 }
 
 oled_buffer_reader_t oled_read_raw(uint16_t start_index) {
@@ -456,7 +463,7 @@ void oled_write_raw_byte(const char data, uint16_t index) {
     if (index > OLED_MATRIX_SIZE) index = OLED_MATRIX_SIZE;
     if (oled_buffer[index] == data) return;
     oled_buffer[index] = data;
-    oled_dirty |= (1 << (index / OLED_BLOCK_SIZE));
+    oled_dirty |= ((OLED_BLOCK_TYPE)1 << (index / OLED_BLOCK_SIZE));
 }
 
 void oled_write_raw(const char *data, uint16_t size) {
@@ -464,21 +471,28 @@ void oled_write_raw(const char *data, uint16_t size) {
     for (uint16_t i = 0; i < size; i++) {
         if (oled_buffer[i] == data[i]) continue;
         oled_buffer[i] = data[i];
-        oled_dirty |= (1 << (i / OLED_BLOCK_SIZE));
+        oled_dirty |= ((OLED_BLOCK_TYPE)1 << (i / OLED_BLOCK_SIZE));
     }
 }
 
 void oled_write_pixel(uint8_t x, uint8_t y, bool on) {
-    if (x >= OLED_DISPLAY_WIDTH || y >= OLED_DISPLAY_HEIGHT) {
+    if (x >= oled_rotation_width) {
         return;
     }
-    uint16_t index = x + (y / 8) * OLED_DISPLAY_WIDTH;
+    uint16_t index = x + (y / 8) * oled_rotation_width;
+    if (index >= OLED_MATRIX_SIZE) {
+        return;
+    }
+    uint8_t data = oled_buffer[index];
     if (on) {
-        oled_buffer[index] |= (1 << (y % 8));
+        data |= (1 << (y % 8));
     } else {
-        oled_buffer[index] &= ~(1 << (y % 8));
+        data &= ~(1 << (y % 8));
+    }
+    if (oled_buffer[index] != data) {
+        oled_buffer[index] = data;
+        oled_dirty |= ((OLED_BLOCK_TYPE)1 << (index / OLED_BLOCK_SIZE));
     }
-    oled_dirty |= (1 << (index / OLED_BLOCK_SIZE));
 }
 
 #if defined(__AVR__)
@@ -501,7 +515,7 @@ void oled_write_raw_P(const char *data, uint16_t size) {
         uint8_t c = pgm_read_byte(data++);
         if (oled_buffer[i] == c) continue;
         oled_buffer[i] = c;
-        oled_dirty |= (1 << (i / OLED_BLOCK_SIZE));
+        oled_dirty |= ((OLED_BLOCK_TYPE)1 << (i / OLED_BLOCK_SIZE));
     }
 }
 #endif  // defined(__AVR__)
@@ -597,7 +611,7 @@ bool oled_scroll_off(void) {
             return oled_scrolling;
         }
         oled_scrolling = false;
-        oled_dirty     = -1;
+        oled_dirty     = OLED_ALL_BLOCKS_MASK;
     }
     return !oled_scrolling;
 }