diff --git a/FluidNC/src/OLED.cpp b/FluidNC/src/OLED.cpp
index 8285e3a632..58d393c565 100644
--- a/FluidNC/src/OLED.cpp
+++ b/FluidNC/src/OLED.cpp
@@ -69,7 +69,7 @@ void OLED::init() {
         return;
     }
     log_info("OLED I2C address: " << to_hex(_address) << " width: " << _width << " height: " << _height);
-    _oled = new SSD1306_I2C(_address, _geometry, config->_i2c[_i2c_num], 400000);
+    _oled = new SH1106_I2C(_address, _geometry, config->_i2c[_i2c_num], 700000);
     _oled->init();
 
     if (_flip) {
diff --git a/FluidNC/src/OLED.h b/FluidNC/src/OLED.h
index 542af394d1..492a5bf24e 100644
--- a/FluidNC/src/OLED.h
+++ b/FluidNC/src/OLED.h
@@ -6,7 +6,7 @@
 
 #include "src/Channel.h"
 #include "src/Module.h"
-#include "SSD1306_I2C.h"
+#include "SH1106_I2C.h"
 
 typedef const uint8_t* font_t;
 
diff --git a/FluidNC/src/SH1106_I2C.h b/FluidNC/src/SH1106_I2C.h
new file mode 100644
index 0000000000..5113bc0464
--- /dev/null
+++ b/FluidNC/src/SH1106_I2C.h
@@ -0,0 +1,116 @@
+
+#pragma once
+
+#include <OLEDDisplay.h>
+#include "Machine/I2CBus.h"
+#include <algorithm>
+
+using namespace Machine;
+
+class SH1106_I2C : public OLEDDisplay {
+private:
+    uint8_t _address;
+    I2CBus* _i2c;
+    int     _frequency;
+    bool    _error = false;
+
+public:
+    SH1106_I2C(uint8_t address, OLEDDISPLAY_GEOMETRY g, I2CBus* i2c, int frequency) :
+        _address(address), _i2c(i2c), _frequency(frequency), _error(false) {
+        setGeometry(g);
+    }
+
+    bool connect() {
+#if 1
+        if (this->_frequency != -1) {
+            _i2c->_frequency = this->_frequency;
+        }
+#endif
+        return true;
+    }
+
+    void display(void) {
+        if (_error) {
+            return;
+        }
+        const int x_offset = 2;  // SH1106 has 132 pixel RAM, display starts at column 2
+#ifdef OLEDDISPLAY_DOUBLE_BUFFER
+        uint8_t minBoundY = UINT8_MAX;
+        uint8_t maxBoundY = 0;
+
+        uint8_t minBoundX = UINT8_MAX;
+        uint8_t maxBoundX = 0;
+        uint8_t x, y;
+
+        // Calculate the Y bounding box of changes
+        // and copy buffer[pos] to buffer_back[pos];
+        for (y = 0; y < (this->height() / 8); y++) {
+            for (x = 0; x < this->width(); x++) {
+                uint16_t pos = x + y * this->width();
+                if (buffer[pos] != buffer_back[pos]) {
+                    minBoundY = std::min(minBoundY, y);
+                    maxBoundY = std::max(maxBoundY, y);
+                    minBoundX = std::min(minBoundX, x);
+                    maxBoundX = std::max(maxBoundX, x);
+                }
+                buffer_back[pos] = buffer[pos];
+            }
+            yield();
+        }
+
+        // If the minBoundY wasn't updated
+        // we can savely assume that buffer_back[pos] == buffer[pos]
+        // holdes true for all values of pos
+
+        if (minBoundY == UINT8_MAX)
+            return;
+
+        // SH1106 doesn't support COLUMNADDR/PAGEADDR commands
+        // We need to set page and column for each row
+        for (y = minBoundY; y <= maxBoundY; y++) {
+            uint8_t col_start = x_offset + minBoundX;
+            sendCommand(0xB0 + y);                      // Set page address
+            sendCommand(0x00 + (col_start & 0x0F));     // Set lower column address
+            sendCommand(0x10 + ((col_start >> 4) & 0x0F)); // Set higher column address
+
+            uint8_t* start = &buffer[(minBoundX + y * this->width()) - 1];
+            uint8_t  save  = *start;
+
+            *start = 0x40;  // control
+            _i2c->write(_address, start, (maxBoundX - minBoundX) + 1 + 1);
+            *start = save;
+        }
+#else
+        // SH1106 doesn't support COLUMNADDR/PAGEADDR commands
+        // We need to set page and column for each row
+        uint8_t pages = (this->height() / 8);
+        for (uint8_t page = 0; page < pages; page++) {
+            sendCommand(0xB0 + page);                   // Set page address (0xB0-0xB7)
+            sendCommand(0x00 + (x_offset & 0x0F));      // Set lower column address
+            sendCommand(0x10 + ((x_offset >> 4) & 0x0F)); // Set higher column address
+
+            buffer[-1] = 0x40;  // control byte for data
+            _i2c->write(_address, (uint8_t*)&buffer[-1], this->width() + 1);
+            buffer += this->width();
+        }
+        // Reset buffer pointer
+        buffer -= pages * this->width();
+#endif
+    }
+
+private:
+    int getBufferOffset(void) { return 0; }
+
+    inline void sendCommand(uint8_t command) __attribute__((always_inline)) {
+        if (_error) {
+            return;
+        }
+        uint8_t _data[2];
+        _data[0] = 0x80;  // control
+        _data[1] = command;
+        if (_i2c->write(_address, _data, sizeof(_data)) < 0) {
+            log_error("OLED is not responding");
+            _error = true;
+        }
+    }
+};
diff --git a/README.md b/README.md
index a22932e80f..7b05ec715e 100644
--- a/README.md
+++ b/README.md
@@ -41,6 +41,13 @@ The Wifi and WebUI is based on [this project.](https://github.com/luc-github/ESP
 
 We have a Discord server for the development this project. Ask for an invite
 
+## Display fix
+
+Current fork includes some fixes to make SH1106-based OLED screens work without issues.
+The original code had a bug: the screen is initialized, the charge pump works (there is a visible image on the screen).
+There is a small horizontal rectangular window, around 10 pixels high, displaying part of the image. However, the majority of the screen is filled with garbage, mostly white with some black dots on it. 
+
+Tested on a random Chinese module with 128*64 OLED screen, which -- after some testing with [U8g2](https://github.com/olikraus/u8g2) library worked perfectly with SH1106 init sequence.
 
 ## Donations