1 files changed, 17 insertions, 5 deletions

diff --git a/drivers/oled/oled_driver.c b/drivers/oled/oled_driver.c
index ba11db1d2..53bb8ca3f 100644
--- a/drivers/oled/oled_driver.c
+++ b/drivers/oled/oled_driver.c
@@ -119,6 +119,9 @@ uint32_t oled_timeout;
 #if OLED_SCROLL_TIMEOUT > 0
 uint32_t oled_scroll_timeout;
 #endif
+#if OLED_UPDATE_INTERVAL > 0
+uint16_t oled_update_timeout;
+#endif
 
 // Internal variables to reduce math instructions
 
@@ -468,8 +471,9 @@ void oled_write_raw_byte(const char data, uint16_t index) {
 }
 
 void oled_write_raw(const char *data, uint16_t size) {
-    if (size > OLED_MATRIX_SIZE) size = OLED_MATRIX_SIZE;
-    for (uint16_t i = 0; i < size; i++) {
+    uint16_t cursor_start_index = oled_cursor - &oled_buffer[0];
+    if ((size + cursor_start_index) > OLED_MATRIX_SIZE) size = OLED_MATRIX_SIZE - cursor_start_index;
+    for (uint16_t i = cursor_start_index; i < cursor_start_index + size; i++) {
         if (oled_buffer[i] == data[i]) continue;
         oled_buffer[i] = data[i];
         oled_dirty |= ((OLED_BLOCK_TYPE)1 << (i / OLED_BLOCK_SIZE));
@@ -511,8 +515,9 @@ void oled_write_ln_P(const char *data, bool invert) {
 }
 
 void oled_write_raw_P(const char *data, uint16_t size) {
-    if (size > OLED_MATRIX_SIZE) size = OLED_MATRIX_SIZE;
-    for (uint16_t i = 0; i < size; i++) {
+    uint16_t cursor_start_index = oled_cursor - &oled_buffer[0];
+    if ((size + cursor_start_index) > OLED_MATRIX_SIZE) size = OLED_MATRIX_SIZE - cursor_start_index;
+    for (uint16_t i = cursor_start_index; i < cursor_start_index + size; i++) {
         uint8_t c = pgm_read_byte(data++);
         if (oled_buffer[i] == c) continue;
         oled_buffer[i] = c;
@@ -650,9 +655,16 @@ void oled_task(void) {
         return;
     }
 
+#if OLED_UPDATE_INTERVAL > 0
+    if (timer_elapsed(oled_update_timeout) >= OLED_UPDATE_INTERVAL) {
+        oled_update_timeout = timer_read();
+        oled_set_cursor(0, 0);
+        oled_task_user();
+    }
+#else
     oled_set_cursor(0, 0);
-
     oled_task_user();
+#endif
 
 #if OLED_SCROLL_TIMEOUT > 0
     if (oled_dirty && oled_scrolling) {