From 79763a3bb891b3c0b0066932aced8eebe5a362aa Mon Sep 17 00:00:00 2001
From: Neil McKechnie <75813993+Neil-McK@users.noreply.github.com>
Date: Thu, 10 Jun 2021 16:24:44 +0100
Subject: [PATCH 01/18] SH1106 OLED Display Offset Fix (#169)

Correct display offset for SH1106.  The SH1106 supports 132 columns but most displays use 128 column OLED, connected to the middle columns 2-129.
---
 SSD1306Ascii.cpp | 4 ++--
 SSD1306init.h    | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/SSD1306Ascii.cpp b/SSD1306Ascii.cpp
index c99383a..6ddc8f4 100644
--- a/SSD1306Ascii.cpp
+++ b/SSD1306Ascii.cpp
@@ -74,8 +74,8 @@ void SSD1306AsciiWire::setCursor(uint8_t col, uint8_t row) {
     m_col = col + m_colOffset;
     I2CManager.write(m_i2cAddr, 4,
       0x00,    // Set to command mode
-      SSD1306_SETLOWCOLUMN | (col & 0XF), 
-      SSD1306_SETHIGHCOLUMN | (col >> 4),
+      SSD1306_SETLOWCOLUMN | (m_col & 0XF), 
+      SSD1306_SETHIGHCOLUMN | (m_col >> 4),
       SSD1306_SETSTARTPAGE | m_row);
   }
 }
diff --git a/SSD1306init.h b/SSD1306init.h
index b861b81..4acb2ce 100644
--- a/SSD1306init.h
+++ b/SSD1306init.h
@@ -182,6 +182,7 @@ static const uint8_t MEM_TYPE SH1106_128x64init[] = {
   0x00,                                  // Set to command mode
   SSD1306_DISPLAYOFF,
   SSD1306_SETSTARTPAGE | 0X0,            // set page address
+  SSD1306_SETSTARTLINE | 0x0,            // set start line
   SSD1306_SETCONTRAST, 0x80,             // 128
   SSD1306_SEGREMAP | 0X1,                // set segment remap
   SSD1306_NORMALDISPLAY,                 // normal / reverse

From 75eefc7677fd1791d00725573e93b768fd8c59c9 Mon Sep 17 00:00:00 2001
From: Neil McKechnie <75813993+Neil-McK@users.noreply.github.com>
Date: Thu, 10 Jun 2021 15:25:03 +0000
Subject: [PATCH 02/18] Committing a SHA

---
 GITHUB_SHA.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index a9f809f..7cf8145 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "90487d2"
+#define GITHUB_SHA "79763a3"

From dc4fa57dc6df41c1987970dc7b31ffb2101736f5 Mon Sep 17 00:00:00 2001
From: Fred <fndecker@gmail.com>
Date: Thu, 10 Jun 2021 14:06:59 -0400
Subject: [PATCH 03/18] Update version.h

---
 version.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/version.h b/version.h
index a72e345..fab097c 100644
--- a/version.h
+++ b/version.h
@@ -4,6 +4,7 @@
 #include "StringFormatter.h"
 
 #define VERSION "3.1.0"
+// 3.1.1 SH1106 OLED Display Offset Fix
 // 3.0.16 Ignore CV1 bit 7 read rejected by decoder when identifying loco id.  
 // 3.0.15 only send function commands once, not 4 times
 // 3.0.14 gap in ack tolerant fix,  prog track power management over join fix. 

From 55cce0ef99da18ebdba989eeb60f5d0430310550 Mon Sep 17 00:00:00 2001
From: Fred <fndecker@gmail.com>
Date: Thu, 10 Jun 2021 18:07:16 +0000
Subject: [PATCH 04/18] Committing a SHA

---
 GITHUB_SHA.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index 7cf8145..b64d702 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "79763a3"
+#define GITHUB_SHA "dc4fa57"

From 04403bbf5eafdf5198744dd8183fda6c527d5fcf Mon Sep 17 00:00:00 2001
From: Ash-4 <81280775+Ash-4@users.noreply.github.com>
Date: Thu, 10 Jun 2021 13:23:00 -0500
Subject: [PATCH 05/18] Update DCC.cpp (#165)

lines 906-910 added. avoid wait after write. use 1 PROG
---
 DCC.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/DCC.cpp b/DCC.cpp
index 5aaf5c1..226425b 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -904,6 +904,10 @@ void DCC::callback(int value) {
 
     switch (callbackState) {    
        case AFTER_WRITE:  // first attempt to callback after a write operation
+	    if (!ackManagerRejoin && !DCCWaveform::progTrack.autoPowerOff) {
+               callbackState=READY;
+               break;
+            }                              // lines 906-910 added. avoid wait after write. use 1 PROG
             callbackStart=millis();
             callbackState=WAITING_100;
             if (Diag::ACK) DIAG(F("Stable 100mS"));

From 7851e7099c93652e7ac810c5e8457cfc945580e5 Mon Sep 17 00:00:00 2001
From: Ash-4 <81280775+Ash-4@users.noreply.github.com>
Date: Thu, 10 Jun 2021 18:23:17 +0000
Subject: [PATCH 06/18] Committing a SHA

---
 GITHUB_SHA.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index b64d702..03e6a95 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "dc4fa57"
+#define GITHUB_SHA "04403bb"

From afd08dafc1295a21b5aee6b88036812b680b9ccd Mon Sep 17 00:00:00 2001
From: Fred <fndecker@gmail.com>
Date: Thu, 10 Jun 2021 14:23:57 -0400
Subject: [PATCH 07/18] Update version.h

---
 version.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/version.h b/version.h
index fab097c..ceec3fc 100644
--- a/version.h
+++ b/version.h
@@ -4,6 +4,7 @@
 #include "StringFormatter.h"
 
 #define VERSION "3.1.0"
+// 3.1.2 Eliminate wait after write when prog is joined or prog power is off
 // 3.1.1 SH1106 OLED Display Offset Fix
 // 3.0.16 Ignore CV1 bit 7 read rejected by decoder when identifying loco id.  
 // 3.0.15 only send function commands once, not 4 times

From 766f88166c8873b51121a870f2336e64118c283c Mon Sep 17 00:00:00 2001
From: Fred <fndecker@gmail.com>
Date: Thu, 10 Jun 2021 18:24:13 +0000
Subject: [PATCH 08/18] Committing a SHA

---
 GITHUB_SHA.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index 03e6a95..c073533 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "04403bb"
+#define GITHUB_SHA "afd08da"

From 6eeaacdf90d646c9c38568e3530edf7a2ec8be7d Mon Sep 17 00:00:00 2001
From: Fred <fndecker@gmail.com>
Date: Thu, 10 Jun 2021 19:02:20 -0400
Subject: [PATCH 09/18] Add loop to retry Ethernet cable connection (#173)

Implement PaulS's change to delay in a loop to give more time for sensing an Ethernet cable connection
---
 EthernetInterface.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/EthernetInterface.cpp b/EthernetInterface.cpp
index c0b43d3..22b61f3 100644
--- a/EthernetInterface.cpp
+++ b/EthernetInterface.cpp
@@ -68,6 +68,16 @@ EthernetInterface::EthernetInterface()
       DIAG(F("Ethernet shield not found"));
       return;
     }
+  
+    unsigned long startmilli = millis();
+    while ((millis() - startmilli) < 5500) // Loop to give time to check for cable connection
+    {
+        if (Ethernet.linkStatus() == LinkON)
+            break;
+        DIAG(F("Ethernet waiting for link (1sec) "));
+        delay(1000);
+    }
+
     if (Ethernet.linkStatus() == LinkOFF) {
       DIAG(F("Ethernet cable not connected"));
       return;

From 915af8d9817c56232ac823296ee2bbf3fec2a68b Mon Sep 17 00:00:00 2001
From: Fred <fndecker@gmail.com>
Date: Thu, 10 Jun 2021 23:02:35 +0000
Subject: [PATCH 10/18] Committing a SHA

---
 GITHUB_SHA.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index c073533..82402a7 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "afd08da"
+#define GITHUB_SHA "6eeaacd"

From a473f6ae5cdf70a3e3570ec0220b5d0414fbe626 Mon Sep 17 00:00:00 2001
From: Fred <fndecker@gmail.com>
Date: Thu, 10 Jun 2021 19:03:38 -0400
Subject: [PATCH 11/18] Update version.h

---
 version.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/version.h b/version.h
index ceec3fc..1577791 100644
--- a/version.h
+++ b/version.h
@@ -3,7 +3,8 @@
 
 #include "StringFormatter.h"
 
-#define VERSION "3.1.0"
+#define VERSION "3.1.3"
+// 3.1.3 Add a loop delay to give more time for sensing an Ethernet cable connection
 // 3.1.2 Eliminate wait after write when prog is joined or prog power is off
 // 3.1.1 SH1106 OLED Display Offset Fix
 // 3.0.16 Ignore CV1 bit 7 read rejected by decoder when identifying loco id.  

From 29a4ad0072a046b1b63a3cde4eafb91c499c0d4f Mon Sep 17 00:00:00 2001
From: Fred <fndecker@gmail.com>
Date: Thu, 10 Jun 2021 23:03:54 +0000
Subject: [PATCH 12/18] Committing a SHA

---
 GITHUB_SHA.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index 82402a7..7776728 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "6eeaacd"
+#define GITHUB_SHA "a473f6a"

From 38be1d61522b66a8a0be9e94c7c60a1196d009bf Mon Sep 17 00:00:00 2001
From: Neil McKechnie <75813993+Neil-McK@users.noreply.github.com>
Date: Tue, 13 Jul 2021 21:15:38 +0100
Subject: [PATCH 13/18] Refactor OLED and LCD drivers (#178)

* Refactor SSD and LCD drivers

Rework display drivers to use inheritance and to remove unused functionality.  The changes here were previously in neil-hal branch but have been separated out because of the amount of changes in neil-hal.

* Update version.h
---
 DisplayInterface.cpp  |  22 +++
 DisplayInterface.h    |  35 ++++
 FSH.h                 |   5 +
 LCDDisplay.cpp        |   3 +-
 LCDDisplay.h          |  30 ++--
 LCD_Implementation.h  |  32 ++--
 LiquidCrystal_I2C.cpp |  83 ++++-----
 LiquidCrystal_I2C.h   |  50 +++---
 SSD1306Ascii.cpp      | 389 ++++++++++++++++++++++++++++++++++++------
 SSD1306Ascii.h        | 124 +++++++-------
 SSD1306font.h         | 180 -------------------
 SSD1306init.h         | 208 ----------------------
 version.h             |   3 +-
 13 files changed, 564 insertions(+), 600 deletions(-)
 create mode 100644 DisplayInterface.cpp
 create mode 100644 DisplayInterface.h
 delete mode 100644 SSD1306font.h
 delete mode 100644 SSD1306init.h

diff --git a/DisplayInterface.cpp b/DisplayInterface.cpp
new file mode 100644
index 0000000..37e66f2
--- /dev/null
+++ b/DisplayInterface.cpp
@@ -0,0 +1,22 @@
+/*
+ *  © 2021, Chris Harlow, Neil McKechnie. All rights reserved.
+ *
+ *  This file is part of CommandStation-EX
+ *
+ *  This is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  It is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "DisplayInterface.h"
+
+DisplayInterface *DisplayInterface::lcdDisplay = 0;
\ No newline at end of file
diff --git a/DisplayInterface.h b/DisplayInterface.h
new file mode 100644
index 0000000..9e7c4d6
--- /dev/null
+++ b/DisplayInterface.h
@@ -0,0 +1,35 @@
+/*
+ *  © 2021, Chris Harlow, Neil McKechnie. All rights reserved.
+ *
+ *  This file is part of CommandStation-EX
+ *
+ *  This is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  It is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
+ */
+#ifndef DisplayInterface_h
+#define DisplayInterface_h
+
+#include <Arduino.h>
+
+// Definition of base class for displays.  The base class does nothing.
+class DisplayInterface : public Print {
+public:
+  virtual DisplayInterface* loop2(bool force) { (void)force; return NULL; };
+  virtual void setRow(byte line) { (void)line; };
+  virtual void clear() {};
+  virtual size_t write(uint8_t c) { (void)c; return 0; };
+
+  static DisplayInterface *lcdDisplay;
+};
+
+#endif
\ No newline at end of file
diff --git a/FSH.h b/FSH.h
index c6d787e..ec3fe7f 100644
--- a/FSH.h
+++ b/FSH.h
@@ -11,6 +11,7 @@
  *  __FlashStringHelper     Use FSH instead.
  *  PROGMEM                 use FLASH instead
  *  pgm_read_byte_near      use GETFLASH instead.
+ *  pgm_read_word_near      use GETFLASHW instead.
  *  
  */
 #include <Arduino.h>
@@ -21,10 +22,14 @@
 #define F(str) (str)
 typedef char FSH; 
 #define GETFLASH(addr) (*(const unsigned char *)(addr))
+#define GETFLASHW(addr) (*(const unsigned short *)(addr))
 #define FLASH
+#define strlen_P strlen
+#define strcpy_P strcpy
 #else 
 typedef __FlashStringHelper FSH;
 #define GETFLASH(addr) pgm_read_byte_near(addr)
+#define GETFLASHW(addr) pgm_read_word_near(addr)
 #define FLASH PROGMEM
 #endif
 #endif
diff --git a/LCDDisplay.cpp b/LCDDisplay.cpp
index 991db7d..cd5b10b 100644
--- a/LCDDisplay.cpp
+++ b/LCDDisplay.cpp
@@ -73,6 +73,7 @@ void LCDDisplay::loop() {
 
 LCDDisplay *LCDDisplay::loop2(bool force) {
   if (!lcdDisplay) return NULL;
+
   unsigned long currentMillis = millis();
 
   if (!force) {
@@ -159,4 +160,4 @@ void LCDDisplay::moveToNextRow() {
 void LCDDisplay::skipBlankRows() {
   while (!done && rowBuffer[rowNext][0] == 0)
     moveToNextRow();
-}
\ No newline at end of file
+}
diff --git a/LCDDisplay.h b/LCDDisplay.h
index e75fd31..4454532 100644
--- a/LCDDisplay.h
+++ b/LCDDisplay.h
@@ -19,6 +19,7 @@
 #ifndef LCDDisplay_h
 #define LCDDisplay_h
 #include <Arduino.h>
+#include "DisplayInterface.h"
 
 #if __has_include ( "config.h")
   #include "config.h"
@@ -29,43 +30,44 @@
 #define MAX_MSG_SIZE 16
 #endif
 
+// Set default scroll mode (overridable in config.h)
+#if !defined(SCROLLMODE) 
+#define SCROLLMODE 1
+#endif
+
 // This class is created in LCDisplay_Implementation.h
 
-class LCDDisplay : public Print {
+class LCDDisplay : public DisplayInterface {
  public:
   static const int MAX_LCD_ROWS = 8;
   static const int MAX_LCD_COLS = MAX_MSG_SIZE;
   static const long LCD_SCROLL_TIME = 3000;  // 3 seconds
 
-  static LCDDisplay* lcdDisplay;
-  LCDDisplay();
-  void interfake(int p1, int p2, int p3);
-
   // Internally handled functions
   static void loop();
   LCDDisplay* loop2(bool force);
   void setRow(byte line);
   void clear();
 
-  virtual size_t write(uint8_t b);
-  using Print::write;
+  size_t write(uint8_t b);
+
+protected:
+  uint8_t lcdRows;
+  uint8_t lcdCols;
 
  private:
   void moveToNextRow();
   void skipBlankRows();
 
-  // Relay functions to the live driver
-  void clearNative();
-  void displayNative();
-  void setRowNative(byte line);
-  void writeNative(char b);
+  // Relay functions to the live driver in the subclass
+  virtual void clearNative() = 0;
+  virtual void setRowNative(byte line) = 0;
+  virtual size_t writeNative(uint8_t b) = 0;
 
   unsigned long lastScrollTime = 0;
   int8_t hotRow = 0;
   int8_t hotCol = 0;
   int8_t topRow = 0;
-  uint8_t lcdRows;
-  uint8_t lcdCols;
   int8_t slot = 0;
   int8_t rowFirst = -1;
   int8_t rowNext = 0;
diff --git a/LCD_Implementation.h b/LCD_Implementation.h
index 9dcae24..95b0f81 100644
--- a/LCD_Implementation.h
+++ b/LCD_Implementation.h
@@ -27,29 +27,27 @@
 
 #ifndef LCD_Implementation_h
 #define LCD_Implementation_h
-#include <Wire.h>
 #include "LCDDisplay.h"
+#include "SSD1306Ascii.h"
+#include "LiquidCrystal_I2C.h"
   
-LCDDisplay * LCDDisplay::lcdDisplay=0;
 
 // Implement the LCDDisplay shim class as a singleton.
-// Notice that the LCDDisplay class declaration (LCDDisplay.h) is independent of the library
-// but the implementation is compiled here with dependencies on LCDDriver which is 
-// specific to the library in use.
-// Thats the workaround to the drivers not all implementing a common interface. 
- 
-#if defined(OLED_DRIVER) 
-  #include "LCD_OLED.h"
-  #define CONDITIONAL_LCD_START for (LCDDisplay * dummy=new LCDDisplay();dummy!=NULL; dummy=dummy->loop2(true)) 
+// The DisplayInterface class implements a displayy handler with no code (null device);
+// The LCDDisplay class sub-classes DisplayInterface to provide the common display code;
+// Then LCDDisplay class is subclassed to the specific device type classes:
+//    SSD1306AsciiWire for I2C OLED driver with SSD1306 or SH1106 controllers;
+//    LiquidCrystal_I2C for I2C LCD driver for HD44780 with PCF8574 'backpack'.
+
+#if defined(OLED_DRIVER)
+  #define CONDITIONAL_LCD_START for (DisplayInterface * dummy=new SSD1306AsciiWire(OLED_DRIVER);dummy!=NULL; dummy=dummy->loop2(true))
   
+#elif defined(LCD_DRIVER)
+  #define CONDITIONAL_LCD_START for (DisplayInterface * dummy=new LiquidCrystal_I2C(LCD_DRIVER);dummy!=NULL; dummy=dummy->loop2(true))
 
-#elif defined(LCD_DRIVER)  
-  #include "LCD_LCD.h"      
-  #define CONDITIONAL_LCD_START for (LCDDisplay * dummy=new LCDDisplay();dummy!=NULL; dummy=dummy->loop2(true))  
-
-#else 
-  #include "LCD_NONE.h"
-  #define CONDITIONAL_LCD_START if (true) /* NO LCD CONFIG, but do the LCD macros to get DIAGS */      
+#else
+  // Create null display handler just in case someone calls lcdDisplay->something without checking if lcdDisplay is NULL!
+  #define CONDITIONAL_LCD_START { new DisplayInterface(); }
 #endif
 
 #endif // LCD_Implementation_h
diff --git a/LiquidCrystal_I2C.cpp b/LiquidCrystal_I2C.cpp
index dbde4a0..30faad3 100644
--- a/LiquidCrystal_I2C.cpp
+++ b/LiquidCrystal_I2C.cpp
@@ -20,7 +20,7 @@
 
 #include <Arduino.h>
 #include "LiquidCrystal_I2C.h"
-#include "I2CManager.h"
+#include "DIAG.h"
 
 // When the display powers up, it is configured as follows:
 //
@@ -44,30 +44,30 @@
 LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t lcd_Addr, uint8_t lcd_cols,
                                      uint8_t lcd_rows) {
   _Addr = lcd_Addr;
-  _cols = lcd_cols;
-  _rows = lcd_rows;
-  _backlightval = LCD_NOBACKLIGHT;
-}
+  lcdRows = lcd_rows;
+  lcdCols = lcd_cols;
 
-void LiquidCrystal_I2C::init() { init_priv(); }
+  _backlightval &= ~LCD_BACKLIGHT;
 
-void LiquidCrystal_I2C::init_priv() {
   I2CManager.begin();
   I2CManager.setClock(100000L);    // PCF8574 is spec'd to 100kHz.
 
-  _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS;
-  begin(_cols, _rows);
+  if (I2CManager.exists(lcd_Addr)) {
+    DIAG(F("%dx%d LCD configured on I2C:x%x"), (int)lcd_cols, (int)lcd_rows, (int)lcd_Addr);
+    _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS;
+    begin();
+    backlight();
+    lcdDisplay = this;
+  }
 }
 
-void LiquidCrystal_I2C::begin(uint8_t cols, uint8_t lines) {
-  if (lines > 1) {
+void LiquidCrystal_I2C::begin() {
+  if (lcdRows > 1) {
     _displayfunction |= LCD_2LINE;
   }
-  _numlines = lines;
-  (void)cols; // Suppress compiler warning.
 
   // according to datasheet, we need at least 40ms after power rises above 2.7V
-  // before sending commands. Arduino can turn on way befer 4.5V so we'll allow
+  // before sending commands. Arduino can turn on way before 4.5V so we'll allow
   // 100 milliseconds after pulling both RS and R/W and backlight pin low
   expanderWrite(
       _backlightval);  // reset expander and turn backlight off (Bit 8 =1)
@@ -78,19 +78,19 @@ void LiquidCrystal_I2C::begin(uint8_t cols, uint8_t lines) {
   // figure 24, pg 46
 
   // we start in 8bit mode, try to set 4 bit mode
-  write4bits(0x03 << 4);
+  write4bits(0x03);
   delayMicroseconds(4500);  // wait min 4.1ms
 
   // second try
-  write4bits(0x03 << 4);
+  write4bits(0x03);
   delayMicroseconds(4500);  // wait min 4.1ms
 
   // third go!
-  write4bits(0x03 << 4);
+  write4bits(0x03);
   delayMicroseconds(150);
 
   // finally, set to 4-bit interface
-  write4bits(0x02 << 4);
+  write4bits(0x02);
 
   // set # lines, font size, etc.
   command(LCD_FUNCTIONSET | _displayfunction);
@@ -108,27 +108,21 @@ void LiquidCrystal_I2C::begin(uint8_t cols, uint8_t lines) {
   // set the entry mode
   command(LCD_ENTRYMODESET | _displaymode);
 
-  setCursor(0, 0);
+  setRowNative(0);
 }
 
 /********** high level commands, for the user! */
-void LiquidCrystal_I2C::clear() {
+void LiquidCrystal_I2C::clearNative() {
   command(LCD_CLEARDISPLAY);  // clear display, set cursor position to zero
   delayMicroseconds(2000);    // this command takes 1.52ms
 }
 
-void LiquidCrystal_I2C::setCursor(uint8_t col, uint8_t row) {
+void LiquidCrystal_I2C::setRowNative(byte row) {
   int row_offsets[] = {0x00, 0x40, 0x14, 0x54};
-  if (row > _numlines) {
-    row = _numlines - 1;  // we count rows starting w/0
+  if (row > lcdRows) {
+    row = lcdRows - 1;  // we count rows starting w/0
   }
-  command(LCD_SETDDRAMADDR | (col + row_offsets[row]));
-}
-
-// Turn the display on/off (quickly)
-void LiquidCrystal_I2C::noDisplay() {
-  _displaycontrol &= ~LCD_DISPLAYON;
-  command(LCD_DISPLAYCONTROL | _displaycontrol);
+  command(LCD_SETDDRAMADDR | (row_offsets[row]));
 }
 
 void LiquidCrystal_I2C::display() {
@@ -138,7 +132,7 @@ void LiquidCrystal_I2C::display() {
 
 // Turn the (optional) backlight off/on
 void LiquidCrystal_I2C::noBacklight(void) {
-  _backlightval = LCD_NOBACKLIGHT;
+  _backlightval &= ~LCD_BACKLIGHT;
   expanderWrite(0);
 }
 
@@ -147,7 +141,7 @@ void LiquidCrystal_I2C::backlight(void) {
   expanderWrite(0);
 }
 
-size_t LiquidCrystal_I2C::write(uint8_t value) {
+size_t LiquidCrystal_I2C::writeNative(uint8_t value) {
   send(value, Rs);
   return 1;
 }
@@ -194,25 +188,32 @@ inline void LiquidCrystal_I2C::command(uint8_t value) {
 //  a single I2C transmission. 
 void LiquidCrystal_I2C::send(uint8_t value, uint8_t mode) {
   mode |= _backlightval;
-  uint8_t highnib = (value & 0xf0) | mode;
-  uint8_t lownib = ((value << 4) & 0xf0) | mode;
+  uint8_t highnib = (((value >> 4) & 0x0f) << BACKPACK_DATA_BITS) | mode;
+  uint8_t lownib = ((value & 0x0f) << BACKPACK_DATA_BITS) | mode;
   // Send both nibbles
-  byte buffer[] = {(byte)(highnib|En), highnib, (byte)(lownib|En), lownib};
-  I2CManager.write(_Addr, buffer, sizeof(buffer));
+  uint8_t len = 0;
+  outputBuffer[len++] = highnib|En;
+  outputBuffer[len++] = highnib;
+  outputBuffer[len++] = lownib|En;
+  outputBuffer[len++] = lownib;
+  I2CManager.write(_Addr, outputBuffer, len);
 }
 
-// write 4 bits to the HD44780 LCD controller.
+// write 4 data bits to the HD44780 LCD controller.
 void LiquidCrystal_I2C::write4bits(uint8_t value) {
-  uint8_t _data = value | _backlightval;
+  uint8_t _data = ((value & 0x0f) << BACKPACK_DATA_BITS) | _backlightval;
   // Enable must be set/reset for at least 450ns.  This is well within the
   // I2C clock cycle time of 2.5us at 400kHz. Data is clocked in to the
   // HD44780 on the trailing edge of the Enable pin.
-  byte buffer[] = {(byte)(_data|En), _data};
-  I2CManager.write(_Addr, buffer, sizeof(buffer));
+  uint8_t len = 0;
+  outputBuffer[len++] = _data|En;
+  outputBuffer[len++] = _data;
+  I2CManager.write(_Addr, outputBuffer, len);
 }
 
 // write a byte to the PCF8574 I2C interface.  We don't need to set
 // the enable pin for this.
 void LiquidCrystal_I2C::expanderWrite(uint8_t value) {
-  I2CManager.write(_Addr, 1, value | _backlightval);
+  outputBuffer[0] = value | _backlightval;
+  I2CManager.write(_Addr, outputBuffer, 1);
 }
\ No newline at end of file
diff --git a/LiquidCrystal_I2C.h b/LiquidCrystal_I2C.h
index 9316385..0eb9eae 100644
--- a/LiquidCrystal_I2C.h
+++ b/LiquidCrystal_I2C.h
@@ -22,13 +22,13 @@
 #define LiquidCrystal_I2C_h
 
 #include <Arduino.h>
+#include "LCDDisplay.h"
+#include "I2CManager.h"
 
 // commands
 #define LCD_CLEARDISPLAY 0x01
-#define LCD_RETURNHOME 0x02
 #define LCD_ENTRYMODESET 0x04
 #define LCD_DISPLAYCONTROL 0x08
-#define LCD_CURSORSHIFT 0x10
 #define LCD_FUNCTIONSET 0x20
 #define LCD_SETCGRAMADDR 0x40
 #define LCD_SETDDRAMADDR 0x80
@@ -41,46 +41,39 @@
 
 // flags for display on/off control
 #define LCD_DISPLAYON 0x04
-#define LCD_DISPLAYOFF 0x00
-#define LCD_CURSORON 0x02
 #define LCD_CURSOROFF 0x00
-#define LCD_BLINKON 0x01
 #define LCD_BLINKOFF 0x00
 
-// flags for display/cursor shift
-#define LCD_DISPLAYMOVE 0x08
-#define LCD_CURSORMOVE 0x00
-#define LCD_MOVERIGHT 0x04
-#define LCD_MOVELEFT 0x00
-
 // flags for function set
-#define LCD_8BITMODE 0x10
 #define LCD_4BITMODE 0x00
 #define LCD_2LINE 0x08
 #define LCD_1LINE 0x00
-#define LCD_5x10DOTS 0x04
 #define LCD_5x8DOTS 0x00
 
-// flags for backlight control
-#define LCD_BACKLIGHT 0x08
-#define LCD_NOBACKLIGHT 0x00
+// Bit mapping onto PCF8574 port
+#define BACKPACK_Rs_BIT 0
+#define BACKPACK_Rw_BIT 1
+#define BACKPACK_En_BIT 2
+#define BACKPACK_BACKLIGHT_BIT 3
+#define BACKPACK_DATA_BITS 4 // Bits 4-7
+// Equivalent mask bits
+#define LCD_BACKLIGHT (1 << BACKPACK_BACKLIGHT_BIT)  // Backlight enable
+#define En (1 << BACKPACK_En_BIT)  // Enable bit
+#define Rw (1 << BACKPACK_Rw_BIT)  // Read/Write bit
+#define Rs (1 << BACKPACK_Rs_BIT)  // Register select bit
 
-#define En 0b00000100  // Enable bit
-#define Rw 0b00000010  // Read/Write bit
-#define Rs 0b00000001  // Register select bit
-
-class LiquidCrystal_I2C : public Print {
+class LiquidCrystal_I2C : public LCDDisplay {
 public:
   LiquidCrystal_I2C(uint8_t lcd_Addr,uint8_t lcd_cols,uint8_t lcd_rows);
-  void begin(uint8_t cols, uint8_t rows);
-  void clear();
-  void noDisplay();
+  void begin();
+  void clearNative();
+  void setRowNative(byte line);
+  size_t writeNative(uint8_t c);
+  
   void display();
   void noBacklight();
   void backlight();
   
-  void setCursor(uint8_t, uint8_t); 
-  virtual size_t write(uint8_t);
   void command(uint8_t);
   void init();
 
@@ -93,10 +86,9 @@ private:
   uint8_t _displayfunction;
   uint8_t _displaycontrol;
   uint8_t _displaymode;
-  uint8_t _numlines;
-  uint8_t _cols;
-  uint8_t _rows;
   uint8_t _backlightval;
+
+  uint8_t outputBuffer[4];
 };
 
 #endif
diff --git a/SSD1306Ascii.cpp b/SSD1306Ascii.cpp
index 6ddc8f4..f34a41b 100644
--- a/SSD1306Ascii.cpp
+++ b/SSD1306Ascii.cpp
@@ -19,90 +19,375 @@
 #include "I2CManager.h"
 #include "FSH.h"
 
+//==============================================================================
+// SSD1306/SSD1106 I2C command bytes
+//------------------------------------------------------------------------------
+/** Set Lower Column Start Address for Page Addressing Mode. */
+static const uint8_t SSD1306_SETLOWCOLUMN = 0x00;
+/** Set Higher Column Start Address for Page Addressing Mode. */
+static const uint8_t SSD1306_SETHIGHCOLUMN = 0x10;
+/** Set Memory Addressing Mode. */
+static const uint8_t SSD1306_MEMORYMODE = 0x20;
+/** Set display RAM display start line register from 0 - 63. */
+static const uint8_t SSD1306_SETSTARTLINE = 0x40;
+/** Set Display Contrast to one of 256 steps. */
+static const uint8_t SSD1306_SETCONTRAST = 0x81;
+/** Enable or disable charge pump.  Follow with 0X14 enable, 0X10 disable. */
+static const uint8_t SSD1306_CHARGEPUMP = 0x8D;
+/** Set Segment Re-map between data column and the segment driver. */
+static const uint8_t SSD1306_SEGREMAP = 0xA0;
+/** Resume display from GRAM content. */
+static const uint8_t SSD1306_DISPLAYALLON_RESUME = 0xA4;
+/** Force display on regardless of GRAM content. */
+static const uint8_t SSD1306_DISPLAYALLON = 0xA5;
+/** Set Normal Display. */
+static const uint8_t SSD1306_NORMALDISPLAY = 0xA6;
+/** Set Inverse Display. */
+static const uint8_t SSD1306_INVERTDISPLAY = 0xA7;
+/** Set Multiplex Ratio from 16 to 63. */
+static const uint8_t SSD1306_SETMULTIPLEX = 0xA8;
+/** Set Display off. */
+static const uint8_t SSD1306_DISPLAYOFF = 0xAE;
+/** Set Display on. */
+static const uint8_t SSD1306_DISPLAYON = 0xAF;
+/**Set GDDRAM Page Start Address. */
+static const uint8_t SSD1306_SETSTARTPAGE = 0xB0;
+/** Set COM output scan direction normal. */
+static const uint8_t SSD1306_COMSCANINC = 0xC0;
+/** Set COM output scan direction reversed. */
+static const uint8_t SSD1306_COMSCANDEC = 0xC8;
+/** Set Display Offset. */
+static const uint8_t SSD1306_SETDISPLAYOFFSET = 0xD3;
+/** Sets COM signals pin configuration to match the OLED panel layout. */
+static const uint8_t SSD1306_SETCOMPINS = 0xDA;
+/** This command adjusts the VCOMH regulator output. */
+static const uint8_t SSD1306_SETVCOMDETECT = 0xDB;
+/** Set Display Clock Divide Ratio/ Oscillator Frequency. */
+static const uint8_t SSD1306_SETDISPLAYCLOCKDIV = 0xD5;
+/** Set Pre-charge Period */
+static const uint8_t SSD1306_SETPRECHARGE = 0xD9;
+/** Deactivate scroll */
+static const uint8_t SSD1306_DEACTIVATE_SCROLL = 0x2E;
+/** No Operation Command. */
+static const uint8_t SSD1306_NOP = 0xE3;
+//------------------------------------------------------------------------------
+/** Set Pump voltage value: (30H~33H) 6.4, 7.4, 8.0 (POR), 9.0. */
+static const uint8_t SH1106_SET_PUMP_VOLTAGE = 0x30;
+/** First byte of set charge pump mode */
+static const uint8_t SH1106_SET_PUMP_MODE = 0xAD;
+/** Second byte charge pump on. */
+static const uint8_t SH1106_PUMP_ON = 0x8B;
+/** Second byte charge pump off. */
+static const uint8_t SH1106_PUMP_OFF = 0x8A;
+//------------------------------------------------------------------------------
 
-// Maximum number of bytes we can send per transmission is 32.
-const uint8_t SSD1306AsciiWire::blankPixels[16] = 
+// Sequence of blank pixels, to optimise clearing screen.
+// Send a maximum of 30 pixels per transmission.
+const uint8_t FLASH SSD1306AsciiWire::blankPixels[30] = 
   {0x40,        // First byte specifies data mode
-  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};  
+  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};  
 
 //==============================================================================
 // SSD1306AsciiWire Method Definitions
 //------------------------------------------------------------------------------
-void SSD1306AsciiWire::clear() {
-  clear(0, displayWidth() - 1, 0, displayRows() - 1);
+ 
+// Constructor
+SSD1306AsciiWire::SSD1306AsciiWire(int width, int height) {
+  // Set size in characters in base class
+  lcdRows = height / 8;
+  lcdCols = width / 6;
+
+  I2CManager.begin();
+  I2CManager.setClock(400000L);  // Set max supported I2C speed
+  for (byte address = 0x3c; address <= 0x3d; address++) {
+    if (I2CManager.exists(address)) {
+      // Device found
+      DIAG(F("%dx%d OLED display configured on I2C:x%x"), width, height, address);
+      if (width == 132)
+        begin(&SH1106_132x64, address);
+      else if (height == 32)
+        begin(&Adafruit128x32, address);
+      else
+        begin(&Adafruit128x64, address);
+      // Set singleton address
+      lcdDisplay = this;
+      clear();
+      return;
+    }
+  }
+  DIAG(F("OLED display not found"));
 }
-//------------------------------------------------------------------------------
-void SSD1306AsciiWire::clear(uint8_t columnStart, uint8_t columnEnd, 
-                             uint8_t rowStart, uint8_t rowEnd) {
+
+/* Clear screen by writing blank pixels. */
+void SSD1306AsciiWire::clearNative() {
   const int maxBytes = sizeof(blankPixels);  // max number of bytes sendable over Wire
-  // Ensure only rows on display will be cleared.
-  if (rowEnd >= displayRows()) rowEnd = displayRows() - 1;
-  for (uint8_t r = rowStart; r <= rowEnd; r++) {
-    setCursor(columnStart, r);   // Position at start of row to be erased
-    for (uint8_t c = columnStart; c <= columnEnd; c += maxBytes-1) {
-      uint8_t len = min((uint8_t)(columnEnd-c+1), maxBytes-1) + 1;
-      I2CManager.write(m_i2cAddr, blankPixels, len);  // Write up to 15 blank columns
+  for (uint8_t r = 0; r <= m_displayHeight/8 - 1; r++) {
+    setRowNative(r);   // Position at start of row to be erased
+    for (uint8_t c = 0; c <= m_displayWidth - 1; c += maxBytes-1) {
+      uint8_t len = min(m_displayWidth-c, maxBytes-1) + 1;
+      I2CManager.write_P(m_i2cAddr, blankPixels, len);  // Write a number of blank columns
     }
   }
 }
-//------------------------------------------------------------------------------
+
+// Initialise device
 void SSD1306AsciiWire::begin(const DevType* dev, uint8_t i2cAddr) {
   m_i2cAddr = i2cAddr;
   m_col = 0;
   m_row = 0;
-#ifdef __AVR__
-  const uint8_t* table = (const uint8_t*)pgm_read_word(&dev->initcmds);
-#else   // __AVR__
-  const uint8_t* table = dev->initcmds;
-#endif  // __AVR
-  uint8_t size = readFontByte(&dev->initSize);
-  m_displayWidth = readFontByte(&dev->lcdWidth);
-  m_displayHeight = readFontByte(&dev->lcdHeight);
-  m_colOffset = readFontByte(&dev->colOffset);
+  const uint8_t* table = (const uint8_t*)GETFLASHW(&dev->initcmds);
+  uint8_t size = GETFLASH(&dev->initSize);
+  m_displayWidth = GETFLASH(&dev->lcdWidth);
+  m_displayHeight = GETFLASH(&dev->lcdHeight);
+  m_colOffset = GETFLASH(&dev->colOffset);
   I2CManager.write_P(m_i2cAddr, table, size);
+  if (m_displayHeight == 32) 
+    I2CManager.write(m_i2cAddr, 5, 0, // Set command mode
+      SSD1306_SETMULTIPLEX, 0x1F,     // ratio 32
+      SSD1306_SETCOMPINS, 0x02);      // sequential COM pins, disable remap
 }
+
 //------------------------------------------------------------------------------
-void SSD1306AsciiWire::setContrast(uint8_t value) {
-  I2CManager.write(m_i2cAddr, 2, 
-    0x00,     // Set to command mode
-    SSD1306_SETCONTRAST, value);
-}
-//------------------------------------------------------------------------------
-void SSD1306AsciiWire::setCursor(uint8_t col, uint8_t row) {
-  if (row < displayRows() && col < m_displayWidth) {
+
+// Set cursor position (by text line)
+void SSD1306AsciiWire::setRowNative(uint8_t line) {
+  // Calculate pixel position from line number
+  uint8_t row = line*8;
+  if (row < m_displayHeight) {
     m_row = row;
-    m_col = col + m_colOffset;
-    I2CManager.write(m_i2cAddr, 4,
-      0x00,    // Set to command mode
-      SSD1306_SETLOWCOLUMN | (m_col & 0XF), 
-      SSD1306_SETHIGHCOLUMN | (m_col >> 4),
-      SSD1306_SETSTARTPAGE | m_row);
+    m_col = m_colOffset;
+    // Build output buffer for I2C
+    uint8_t len = 0;
+    outputBuffer[len++] = 0x00;  // Set to command mode
+    outputBuffer[len++] = SSD1306_SETLOWCOLUMN | (m_col & 0XF);
+    outputBuffer[len++] = SSD1306_SETHIGHCOLUMN | (m_col >> 4);
+    outputBuffer[len++] = SSD1306_SETSTARTPAGE | (m_row/8);
+    I2CManager.write(m_i2cAddr, outputBuffer, len);
   }
 }
 //------------------------------------------------------------------------------
-void SSD1306AsciiWire::setFont(const uint8_t* font) {
-  m_font = font;
-  m_fontFirstChar = readFontByte(m_font + FONT_FIRST_CHAR);
-  m_fontCharCount = readFontByte(m_font + FONT_CHAR_COUNT);
-}
-//------------------------------------------------------------------------------
-size_t SSD1306AsciiWire::write(uint8_t ch) {
-  const uint8_t* base = m_font + FONT_WIDTH_TABLE;
+
+// Write a character to the OLED
+size_t SSD1306AsciiWire::writeNative(uint8_t ch) {
+  const uint8_t* base = m_font;
 
   if (ch < m_fontFirstChar || ch >= (m_fontFirstChar + m_fontCharCount))
     return 0;
+  // Check if character would be partly or wholly off the display
+  if (m_col + fontWidth > m_displayWidth)
+    return 0;
+#if defined(NOLOWERCASE)
+  // Adjust if lowercase is missing
+  if (ch >= 'a') {
+    if (ch <= 'z')
+      ch = ch - 'a' + 'A';  // Capitalise
+    else
+      ch -= 26; // Allow for missing lowercase letters
+  }
+#endif
   ch -= m_fontFirstChar;
   base += fontWidth * ch;
-  uint8_t buffer[1+fontWidth+letterSpacing];
-  buffer[0] = 0x40;     // set SSD1306 controller to data mode
+  // Build output buffer for I2C
+  outputBuffer[0] = 0x40;     // set SSD1306 controller to data mode
   uint8_t bufferPos = 1;
   // Copy character pixel columns
   for (uint8_t i = 0; i < fontWidth; i++) 
-    buffer[bufferPos++] = readFontByte(base++);
+    outputBuffer[bufferPos++] = GETFLASH(base++);
   // Add blank pixels between letters
   for (uint8_t i = 0; i < letterSpacing; i++) 
-    buffer[bufferPos++] = 0;
+    outputBuffer[bufferPos++] = 0;
+
   // Write the data to I2C display
-  I2CManager.write(m_i2cAddr, buffer, bufferPos);
+  I2CManager.write(m_i2cAddr, outputBuffer, bufferPos);
+  m_col += fontWidth + letterSpacing;
   return 1;
 }
+
+//==============================================================================
+// this section is based on https://github.com/adafruit/Adafruit_SSD1306
+
+/** Initialization commands for a 128x32 or 128x64 SSD1306 oled display. */
+const uint8_t FLASH SSD1306AsciiWire::Adafruit128xXXinit[] = {
+    // Init sequence for Adafruit 128x32/64 OLED module
+    0x00,                              // Set to command mode
+    SSD1306_DISPLAYOFF,
+    SSD1306_SETDISPLAYCLOCKDIV, 0x80,  // the suggested ratio 0x80 
+    SSD1306_SETMULTIPLEX, 0x3F,        // ratio 64 (initially)
+    SSD1306_SETDISPLAYOFFSET, 0x0,     // no offset
+    SSD1306_SETSTARTLINE | 0x0,        // line #0
+    SSD1306_CHARGEPUMP, 0x14,          // internal vcc
+    SSD1306_MEMORYMODE, 0x02,          // page mode
+    SSD1306_SEGREMAP | 0x1,            // column 127 mapped to SEG0
+    SSD1306_COMSCANDEC,                // column scan direction reversed
+    SSD1306_SETCOMPINS, 0X12,          // set COM pins
+    SSD1306_SETCONTRAST, 0x7F,         // contrast level 127
+    SSD1306_SETPRECHARGE, 0xF1,        // pre-charge period (1, 15)
+    SSD1306_SETVCOMDETECT, 0x40,       // vcomh regulator level
+    SSD1306_DISPLAYALLON_RESUME,
+    SSD1306_NORMALDISPLAY,
+    SSD1306_DISPLAYON
+};
+
+/** Initialize a 128x32 SSD1306 oled display. */
+const DevType FLASH SSD1306AsciiWire::Adafruit128x32 = {
+  Adafruit128xXXinit,
+  sizeof(Adafruit128xXXinit),
+  128,
+  32,
+  0
+};
+
+/** Initialize a 128x64 oled display. */
+const DevType FLASH SSD1306AsciiWire::Adafruit128x64 = {
+  Adafruit128xXXinit,
+  sizeof(Adafruit128xXXinit),
+  128,
+  64,
+  0
+};
+//------------------------------------------------------------------------------
+// This section is based on https://github.com/stanleyhuangyc/MultiLCD
+
+/** Initialization commands for a 128x64 SH1106 oled display. */
+const uint8_t FLASH SSD1306AsciiWire::SH1106_132x64init[] = {
+  0x00,                                  // Set to command mode
+  SSD1306_DISPLAYOFF,
+  SSD1306_SETDISPLAYCLOCKDIV, 0X80,      // set osc division
+  SSD1306_SETMULTIPLEX, 0x3F,            // ratio 64
+  SSD1306_SETDISPLAYOFFSET, 0X00,        // set display offset
+  SSD1306_SETSTARTPAGE | 0X0,            // set page address
+  SSD1306_SETSTARTLINE | 0x0,            // set start line
+  SH1106_SET_PUMP_MODE, SH1106_PUMP_ON,  // set charge pump enable
+  SSD1306_SEGREMAP | 0X1,                // set segment remap
+  SSD1306_COMSCANDEC,                    // Com scan direction
+  SSD1306_SETCOMPINS, 0X12,              // set COM pins
+  SSD1306_SETCONTRAST, 0x80,             // 128
+  SSD1306_SETPRECHARGE, 0X1F,            // set pre-charge period
+  SSD1306_SETVCOMDETECT,  0x40,          // set vcomh
+  SH1106_SET_PUMP_VOLTAGE | 0X2,         // 8.0 volts
+  SSD1306_NORMALDISPLAY,                 // normal / reverse
+  SSD1306_DISPLAYON
+};
+
+/** Initialize a 132x64 oled SH1106 display. */
+const DevType FLASH SSD1306AsciiWire::SH1106_132x64 =  {
+  SH1106_132x64init,
+  sizeof(SH1106_132x64init),
+  128,
+  64,
+  2    // SH1106 is a 132x64 controller but most OLEDs are only attached
+       // to columns 2-129.
+};
+
+
+//------------------------------------------------------------------------------
+
+// Font characters, 5x7 pixels, 0x61 characters starting at 0x20.
+// Lower case characters optionally omitted.
+const uint8_t FLASH SSD1306AsciiWire::System5x7[] = {
+
+    // Fixed width; char width table not used !!!!
+    // or with lowercase character omitted.
+
+    // font data
+    0x00, 0x00, 0x00, 0x00, 0x00,  // (space)
+    0x00, 0x00, 0x5F, 0x00, 0x00,  // !
+    0x00, 0x07, 0x00, 0x07, 0x00,  // "
+    0x14, 0x7F, 0x14, 0x7F, 0x14,  // #
+    0x24, 0x2A, 0x7F, 0x2A, 0x12,  // $
+    0x23, 0x13, 0x08, 0x64, 0x62,  // %
+    0x36, 0x49, 0x55, 0x22, 0x50,  // &
+    0x00, 0x05, 0x03, 0x00, 0x00,  // '
+    0x00, 0x1C, 0x22, 0x41, 0x00,  // (
+    0x00, 0x41, 0x22, 0x1C, 0x00,  // )
+    0x08, 0x2A, 0x1C, 0x2A, 0x08,  // *
+    0x08, 0x08, 0x3E, 0x08, 0x08,  // +
+    0x00, 0x50, 0x30, 0x00, 0x00,  // ,
+    0x08, 0x08, 0x08, 0x08, 0x08,  // -
+    0x00, 0x60, 0x60, 0x00, 0x00,  // .
+    0x20, 0x10, 0x08, 0x04, 0x02,  // /
+    0x3E, 0x51, 0x49, 0x45, 0x3E,  // 0
+    0x00, 0x42, 0x7F, 0x40, 0x00,  // 1
+    0x42, 0x61, 0x51, 0x49, 0x46,  // 2
+    0x21, 0x41, 0x45, 0x4B, 0x31,  // 3
+    0x18, 0x14, 0x12, 0x7F, 0x10,  // 4
+    0x27, 0x45, 0x45, 0x45, 0x39,  // 5
+    0x3C, 0x4A, 0x49, 0x49, 0x30,  // 6
+    0x01, 0x71, 0x09, 0x05, 0x03,  // 7
+    0x36, 0x49, 0x49, 0x49, 0x36,  // 8
+    0x06, 0x49, 0x49, 0x29, 0x1E,  // 9
+    0x00, 0x36, 0x36, 0x00, 0x00,  // :
+    0x00, 0x56, 0x36, 0x00, 0x00,  // ;
+    0x00, 0x08, 0x14, 0x22, 0x41,  // <
+    0x14, 0x14, 0x14, 0x14, 0x14,  // =
+    0x41, 0x22, 0x14, 0x08, 0x00,  // >
+    0x02, 0x01, 0x51, 0x09, 0x06,  // ?
+    0x32, 0x49, 0x79, 0x41, 0x3E,  // @
+    0x7E, 0x11, 0x11, 0x11, 0x7E,  // A
+    0x7F, 0x49, 0x49, 0x49, 0x36,  // B
+    0x3E, 0x41, 0x41, 0x41, 0x22,  // C
+    0x7F, 0x41, 0x41, 0x22, 0x1C,  // D
+    0x7F, 0x49, 0x49, 0x49, 0x41,  // E
+    0x7F, 0x09, 0x09, 0x01, 0x01,  // F
+    0x3E, 0x41, 0x41, 0x51, 0x32,  // G
+    0x7F, 0x08, 0x08, 0x08, 0x7F,  // H
+    0x00, 0x41, 0x7F, 0x41, 0x00,  // I
+    0x20, 0x40, 0x41, 0x3F, 0x01,  // J
+    0x7F, 0x08, 0x14, 0x22, 0x41,  // K
+    0x7F, 0x40, 0x40, 0x40, 0x40,  // L
+    0x7F, 0x02, 0x04, 0x02, 0x7F,  // M
+    0x7F, 0x04, 0x08, 0x10, 0x7F,  // N
+    0x3E, 0x41, 0x41, 0x41, 0x3E,  // O
+    0x7F, 0x09, 0x09, 0x09, 0x06,  // P
+    0x3E, 0x41, 0x51, 0x21, 0x5E,  // Q
+    0x7F, 0x09, 0x19, 0x29, 0x46,  // R
+    0x46, 0x49, 0x49, 0x49, 0x31,  // S
+    0x01, 0x01, 0x7F, 0x01, 0x01,  // T
+    0x3F, 0x40, 0x40, 0x40, 0x3F,  // U
+    0x1F, 0x20, 0x40, 0x20, 0x1F,  // V
+    0x7F, 0x20, 0x18, 0x20, 0x7F,  // W
+    0x63, 0x14, 0x08, 0x14, 0x63,  // X
+    0x03, 0x04, 0x78, 0x04, 0x03,  // Y
+    0x61, 0x51, 0x49, 0x45, 0x43,  // Z
+    0x00, 0x00, 0x7F, 0x41, 0x41,  // [
+    0x02, 0x04, 0x08, 0x10, 0x20,  // "\"
+    0x41, 0x41, 0x7F, 0x00, 0x00,  // ]
+    0x04, 0x02, 0x01, 0x02, 0x04,  // ^
+    0x40, 0x40, 0x40, 0x40, 0x40,  // _
+    0x00, 0x01, 0x02, 0x04, 0x00,  // `
+#ifndef NOLOWERCASE
+    0x20, 0x54, 0x54, 0x54, 0x78,  // a
+    0x7F, 0x48, 0x44, 0x44, 0x38,  // b
+    0x38, 0x44, 0x44, 0x44, 0x20,  // c
+    0x38, 0x44, 0x44, 0x48, 0x7F,  // d
+    0x38, 0x54, 0x54, 0x54, 0x18,  // e
+    0x08, 0x7E, 0x09, 0x01, 0x02,  // f
+    0x08, 0x14, 0x54, 0x54, 0x3C,  // g
+    0x7F, 0x08, 0x04, 0x04, 0x78,  // h
+    0x00, 0x44, 0x7D, 0x40, 0x00,  // i
+    0x20, 0x40, 0x44, 0x3D, 0x00,  // j
+    0x00, 0x7F, 0x10, 0x28, 0x44,  // k
+    0x00, 0x41, 0x7F, 0x40, 0x00,  // l
+    0x7C, 0x04, 0x18, 0x04, 0x78,  // m
+    0x7C, 0x08, 0x04, 0x04, 0x78,  // n
+    0x38, 0x44, 0x44, 0x44, 0x38,  // o
+    0x7C, 0x14, 0x14, 0x14, 0x08,  // p
+    0x08, 0x14, 0x14, 0x18, 0x7C,  // q
+    0x7C, 0x08, 0x04, 0x04, 0x08,  // r
+    0x48, 0x54, 0x54, 0x54, 0x20,  // s
+    0x04, 0x3F, 0x44, 0x40, 0x20,  // t
+    0x3C, 0x40, 0x40, 0x20, 0x7C,  // u
+    0x1C, 0x20, 0x40, 0x20, 0x1C,  // v
+    0x3C, 0x40, 0x30, 0x40, 0x3C,  // w
+    0x44, 0x28, 0x10, 0x28, 0x44,  // x
+    0x0C, 0x50, 0x50, 0x50, 0x3C,  // y
+    0x44, 0x64, 0x54, 0x4C, 0x44,  // z
+#endif
+    0x00, 0x08, 0x36, 0x41, 0x00,  // {
+    0x00, 0x00, 0x7F, 0x00, 0x00,  // |
+    0x00, 0x41, 0x36, 0x08, 0x00,  // }
+    0x08, 0x08, 0x2A, 0x1C, 0x08,  // ->
+    0x08, 0x1C, 0x2A, 0x08, 0x08,  // <-
+    0x00, 0x06, 0x09, 0x09, 0x06   // degree symbol
+
+};
diff --git a/SSD1306Ascii.h b/SSD1306Ascii.h
index e06b4cf..3fa79f9 100644
--- a/SSD1306Ascii.h
+++ b/SSD1306Ascii.h
@@ -20,78 +20,88 @@
 #define SSD1306Ascii_h
 
 #include "Arduino.h"
-#include "SSD1306font.h"
-#include "SSD1306init.h"
+#include "FSH.h"
+#include "LCDDisplay.h"
 
-class SSD1306AsciiWire : public Print {
+#include "I2CManager.h"
+#include "DIAG.h"
+
+// Uncomment to remove lower-case letters to save 108 bytes of flash
+//#define NOLOWERCASE
+
+//------------------------------------------------------------------------------
+// Device initialization structure.
+
+struct DevType {
+  /* Pointer to initialization command bytes. */
+  const uint8_t* initcmds;
+  /* Number of initialization bytes */
+  const uint8_t initSize;
+  /* Width of the display in pixels */
+  const uint8_t lcdWidth;
+  /** Height of the display in pixels. */
+  const uint8_t lcdHeight;
+  /* Column offset RAM to display.  Used to pick start column of SH1106. */
+  const uint8_t colOffset;
+};
+
+// Constructor
+class SSD1306AsciiWire : public LCDDisplay {
  public:
-  using Print::write;
-  SSD1306AsciiWire() {}
+
+  // Constructor
+  SSD1306AsciiWire(int width, int height);
+
   // Initialize the display controller.
   void begin(const DevType* dev, uint8_t i2cAddr);
+
   // Clear the display and set the cursor to (0, 0).
-  void clear();
-  // Clear a region of the display.
-  void clear(uint8_t c0, uint8_t c1, uint8_t r0, uint8_t r1);
-  // The current column in pixels.
-  inline uint8_t col() const { return m_col; }
-  // The display hight in pixels.
-  inline uint8_t displayHeight() const { return m_displayHeight; }
-  // The display height in rows with eight pixels to a row.
-  inline uint8_t displayRows() const { return m_displayHeight / 8; }
-  // The display width in pixels.
-  inline uint8_t displayWidth() const { return m_displayWidth; }
-  // Set the cursor position to (0, 0).
-  inline void home() { setCursor(0, 0); }
+  void clearNative();
+
+  // Set cursor to start of specified text line
+  void setRowNative(byte line);
+
   // Initialize the display controller.
   void init(const DevType* dev);
-  // the current row number with eight pixels to a row.
-  inline uint8_t row() const { return m_row; }
-  /**
-   * @brief Set the display contrast.
-   *
-   * @param[in] value The contrast level in th range 0 to 255.
-   */
-  void setContrast(uint8_t value);
-  /**
-   * @brief Set the cursor position.
-   *
-   * @param[in] col The column number in pixels.
-   * @param[in] row the row number in eight pixel rows.
-   */
-  void setCursor(uint8_t col, uint8_t row);
-  /**
-   * @brief Set the current font.
-   *
-   * @param[in] font Pointer to a font table.
-   */
-  void setFont(const uint8_t* font);
-  /**
-   * @brief Display a character.
-   *
-   * @param[in] c The character to display.
-   * @return one for success else zero.
-   */
-  size_t write(uint8_t c);
+  
+  // Write one character to OLED
+  size_t writeNative(uint8_t c);
+
+  // Display characteristics / initialisation
+  static const DevType FLASH Adafruit128x32;
+  static const DevType FLASH Adafruit128x64;
+  static const DevType FLASH SH1106_132x64;
 
  private:
-  uint8_t m_col;                    // Cursor column.
-  uint8_t m_row;                    // Cursor RAM row.
-  uint8_t m_displayWidth;           // Display width.
-  uint8_t m_displayHeight;          // Display height.
-  uint8_t m_colOffset;              // Column offset RAM to SEG.
-  const uint8_t* m_font = NULL;     // Current font.
+  // Cursor column.
+  uint8_t m_col;
+  // Cursor RAM row.
+  uint8_t m_row;
+  // Display width.
+  uint8_t m_displayWidth;
+  // Display height.
+  uint8_t m_displayHeight;
+  // Column offset RAM to SEG.
+  uint8_t m_colOffset = 0;
+  // Current font.
+  const uint8_t* const m_font = System5x7;
 
   // Only fixed size 5x7 fonts in a 6x8 cell are supported.
-  const uint8_t fontWidth = 5;
-  const uint8_t fontHeight = 7;
-  const uint8_t letterSpacing = 1;
-  uint8_t m_fontFirstChar;
-  uint8_t m_fontCharCount;
+  static const uint8_t fontWidth = 5;
+  static const uint8_t fontHeight = 7;
+  static const uint8_t letterSpacing = 1;
+  static const uint8_t m_fontFirstChar = 0x20;
+  static const uint8_t m_fontCharCount = 0x61;
 
   uint8_t m_i2cAddr;
 
+  uint8_t outputBuffer[fontWidth+letterSpacing+1];
+
   static const uint8_t blankPixels[];
+
+  static const uint8_t System5x7[];
+  static const uint8_t FLASH Adafruit128xXXinit[];
+  static const uint8_t FLASH SH1106_132x64init[];
 };
 
 #endif  // SSD1306Ascii_h
diff --git a/SSD1306font.h b/SSD1306font.h
deleted file mode 100644
index fd98800..0000000
--- a/SSD1306font.h
+++ /dev/null
@@ -1,180 +0,0 @@
-/*
- *
- * System5x7
- *
- *
- * File Name           : System5x7.h
- * Date                : 28 Oct 2008
- * Font size in bytes  : 470
- * Font width          : 5
- * Font height         : 7
- * Font first char     : 32
- * Font last char      : 127
- * Font used chars     : 94
- *
- * The font data are defined as
- *
- * struct _FONT_ {
- *     uint16_t   font_Size_in_Bytes_over_all_included_Size_it_self;
- *     uint8_t    font_Width_in_Pixel_for_fixed_drawing;
- *     uint8_t    font_Height_in_Pixel_for_all_characters;
- *     unit8_t    font_First_Char;
- *     uint8_t    font_Char_Count;
- *
- *     uint8_t    font_Char_Widths[font_Last_Char - font_First_Char +1];
- *                  // for each character the separate width in pixels,
- *                  // characters < 128 have an implicit virtual right empty row
- *
- *     uint8_t    font_data[];
- *                  // bit field of all characters
- */
-
-#ifndef SSD1306font_H
-#define SSD1306font_H
-
-#define SYSTEM5x7_WIDTH 5
-#define SYSTEM5x7_HEIGHT 7
-
-#ifdef __AVR__
-#include <avr/pgmspace.h>
-/** declare a font for AVR. */
-#define GLCDFONTDECL(_n) static const uint8_t __attribute__((progmem)) _n[]
-#define readFontByte(addr) pgm_read_byte(addr)
-#else  // __AVR__
-/** declare a font. */
-#define GLCDFONTDECL(_n) static const uint8_t _n[]
-/** Fake read from flash. */
-#define readFontByte(addr) (*(const unsigned char *)(addr))
-#endif  // __AVR__
-//------------------------------------------------------------------------------
-// Font Indices
-/** No longer used Big Endian length field. Now indicates font type.
- *
- * 00 00 (fixed width font with 1 padding pixel on right and below)
- *
- * 00 01 (fixed width font with no padding pixels)
- */
-#define FONT_LENGTH 0
-/** Maximum character width. */
-#define FONT_WIDTH 2
-/** Font hight in pixels */
-#define FONT_HEIGHT 3
-/** Ascii value of first character */
-#define FONT_FIRST_CHAR 4
-/** count of characters in font. */
-#define FONT_CHAR_COUNT 5
-/** Offset to width table. */
-#define FONT_WIDTH_TABLE 6
-//------------------------------------------------------------------------------
-
-GLCDFONTDECL(System5x7) = {
-    0x0, 0x0,  // size of zero indicates fixed width font,
-    0x05,      // width
-    0x07,      // height
-    0x20,      // first char
-    0x61,      // char count
-
-    // Fixed width; char width table not used !!!!
-
-    // font data
-    0x00, 0x00, 0x00, 0x00, 0x00,  // (space)
-    0x00, 0x00, 0x5F, 0x00, 0x00,  // !
-    0x00, 0x07, 0x00, 0x07, 0x00,  // "
-    0x14, 0x7F, 0x14, 0x7F, 0x14,  // #
-    0x24, 0x2A, 0x7F, 0x2A, 0x12,  // $
-    0x23, 0x13, 0x08, 0x64, 0x62,  // %
-    0x36, 0x49, 0x55, 0x22, 0x50,  // &
-    0x00, 0x05, 0x03, 0x00, 0x00,  // '
-    0x00, 0x1C, 0x22, 0x41, 0x00,  // (
-    0x00, 0x41, 0x22, 0x1C, 0x00,  // )
-    0x08, 0x2A, 0x1C, 0x2A, 0x08,  // *
-    0x08, 0x08, 0x3E, 0x08, 0x08,  // +
-    0x00, 0x50, 0x30, 0x00, 0x00,  // ,
-    0x08, 0x08, 0x08, 0x08, 0x08,  // -
-    0x00, 0x60, 0x60, 0x00, 0x00,  // .
-    0x20, 0x10, 0x08, 0x04, 0x02,  // /
-    0x3E, 0x51, 0x49, 0x45, 0x3E,  // 0
-    0x00, 0x42, 0x7F, 0x40, 0x00,  // 1
-    0x42, 0x61, 0x51, 0x49, 0x46,  // 2
-    0x21, 0x41, 0x45, 0x4B, 0x31,  // 3
-    0x18, 0x14, 0x12, 0x7F, 0x10,  // 4
-    0x27, 0x45, 0x45, 0x45, 0x39,  // 5
-    0x3C, 0x4A, 0x49, 0x49, 0x30,  // 6
-    0x01, 0x71, 0x09, 0x05, 0x03,  // 7
-    0x36, 0x49, 0x49, 0x49, 0x36,  // 8
-    0x06, 0x49, 0x49, 0x29, 0x1E,  // 9
-    0x00, 0x36, 0x36, 0x00, 0x00,  // :
-    0x00, 0x56, 0x36, 0x00, 0x00,  // ;
-    0x00, 0x08, 0x14, 0x22, 0x41,  // <
-    0x14, 0x14, 0x14, 0x14, 0x14,  // =
-    0x41, 0x22, 0x14, 0x08, 0x00,  // >
-    0x02, 0x01, 0x51, 0x09, 0x06,  // ?
-    0x32, 0x49, 0x79, 0x41, 0x3E,  // @
-    0x7E, 0x11, 0x11, 0x11, 0x7E,  // A
-    0x7F, 0x49, 0x49, 0x49, 0x36,  // B
-    0x3E, 0x41, 0x41, 0x41, 0x22,  // C
-    0x7F, 0x41, 0x41, 0x22, 0x1C,  // D
-    0x7F, 0x49, 0x49, 0x49, 0x41,  // E
-    0x7F, 0x09, 0x09, 0x01, 0x01,  // F
-    0x3E, 0x41, 0x41, 0x51, 0x32,  // G
-    0x7F, 0x08, 0x08, 0x08, 0x7F,  // H
-    0x00, 0x41, 0x7F, 0x41, 0x00,  // I
-    0x20, 0x40, 0x41, 0x3F, 0x01,  // J
-    0x7F, 0x08, 0x14, 0x22, 0x41,  // K
-    0x7F, 0x40, 0x40, 0x40, 0x40,  // L
-    0x7F, 0x02, 0x04, 0x02, 0x7F,  // M
-    0x7F, 0x04, 0x08, 0x10, 0x7F,  // N
-    0x3E, 0x41, 0x41, 0x41, 0x3E,  // O
-    0x7F, 0x09, 0x09, 0x09, 0x06,  // P
-    0x3E, 0x41, 0x51, 0x21, 0x5E,  // Q
-    0x7F, 0x09, 0x19, 0x29, 0x46,  // R
-    0x46, 0x49, 0x49, 0x49, 0x31,  // S
-    0x01, 0x01, 0x7F, 0x01, 0x01,  // T
-    0x3F, 0x40, 0x40, 0x40, 0x3F,  // U
-    0x1F, 0x20, 0x40, 0x20, 0x1F,  // V
-    0x7F, 0x20, 0x18, 0x20, 0x7F,  // W
-    0x63, 0x14, 0x08, 0x14, 0x63,  // X
-    0x03, 0x04, 0x78, 0x04, 0x03,  // Y
-    0x61, 0x51, 0x49, 0x45, 0x43,  // Z
-    0x00, 0x00, 0x7F, 0x41, 0x41,  // [
-    0x02, 0x04, 0x08, 0x10, 0x20,  // "\"
-    0x41, 0x41, 0x7F, 0x00, 0x00,  // ]
-    0x04, 0x02, 0x01, 0x02, 0x04,  // ^
-    0x40, 0x40, 0x40, 0x40, 0x40,  // _
-    0x00, 0x01, 0x02, 0x04, 0x00,  // `
-    0x20, 0x54, 0x54, 0x54, 0x78,  // a
-    0x7F, 0x48, 0x44, 0x44, 0x38,  // b
-    0x38, 0x44, 0x44, 0x44, 0x20,  // c
-    0x38, 0x44, 0x44, 0x48, 0x7F,  // d
-    0x38, 0x54, 0x54, 0x54, 0x18,  // e
-    0x08, 0x7E, 0x09, 0x01, 0x02,  // f
-    0x08, 0x14, 0x54, 0x54, 0x3C,  // g
-    0x7F, 0x08, 0x04, 0x04, 0x78,  // h
-    0x00, 0x44, 0x7D, 0x40, 0x00,  // i
-    0x20, 0x40, 0x44, 0x3D, 0x00,  // j
-    0x00, 0x7F, 0x10, 0x28, 0x44,  // k
-    0x00, 0x41, 0x7F, 0x40, 0x00,  // l
-    0x7C, 0x04, 0x18, 0x04, 0x78,  // m
-    0x7C, 0x08, 0x04, 0x04, 0x78,  // n
-    0x38, 0x44, 0x44, 0x44, 0x38,  // o
-    0x7C, 0x14, 0x14, 0x14, 0x08,  // p
-    0x08, 0x14, 0x14, 0x18, 0x7C,  // q
-    0x7C, 0x08, 0x04, 0x04, 0x08,  // r
-    0x48, 0x54, 0x54, 0x54, 0x20,  // s
-    0x04, 0x3F, 0x44, 0x40, 0x20,  // t
-    0x3C, 0x40, 0x40, 0x20, 0x7C,  // u
-    0x1C, 0x20, 0x40, 0x20, 0x1C,  // v
-    0x3C, 0x40, 0x30, 0x40, 0x3C,  // w
-    0x44, 0x28, 0x10, 0x28, 0x44,  // x
-    0x0C, 0x50, 0x50, 0x50, 0x3C,  // y
-    0x44, 0x64, 0x54, 0x4C, 0x44,  // z
-    0x00, 0x08, 0x36, 0x41, 0x00,  // {
-    0x00, 0x00, 0x7F, 0x00, 0x00,  // |
-    0x00, 0x41, 0x36, 0x08, 0x00,  // }
-    0x08, 0x08, 0x2A, 0x1C, 0x08,  // ->
-    0x08, 0x1C, 0x2A, 0x08, 0x08,  // <-
-    0x00, 0x06, 0x09, 0x09, 0x06   // degree symbol
-
-};
-
-#endif
diff --git a/SSD1306init.h b/SSD1306init.h
deleted file mode 100644
index 4acb2ce..0000000
--- a/SSD1306init.h
+++ /dev/null
@@ -1,208 +0,0 @@
-/* Based on Arduino SSD1306Ascii Library, Copyright (C) 2015 by William Greiman
- * Modifications (C) 2021 Neil McKechnie
- *
- * This Library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This Library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the Arduino SSD1306Ascii Library.  If not, see
- * <http://www.gnu.org/licenses/>.
- */
-/**
- * @file SSD1306init.h
- * @brief Display controller initialization commands.
- */
-#ifndef SSD1306init_h
-#define SSD1306init_h
-//------------------------------------------------------------------------------
-#ifndef __AVR__
-/** Handle AVR flash addressing. */
-#define MEM_TYPE
-#else  // __AVR__
-#define MEM_TYPE __attribute__ ((progmem))
-#endif  // __AVR__
-//------------------------------------------------------------------------------
-/** Set Lower Column Start Address for Page Addressing Mode. */
-#define SSD1306_SETLOWCOLUMN 0x00
-/** Set Higher Column Start Address for Page Addressing Mode. */
-#define SSD1306_SETHIGHCOLUMN 0x10
-/** Set Memory Addressing Mode. */
-#define SSD1306_MEMORYMODE 0x20
-/** Set display RAM display start line register from 0 - 63. */
-#define SSD1306_SETSTARTLINE 0x40
-/** Set Display Contrast to one of 256 steps. */
-#define SSD1306_SETCONTRAST 0x81
-/** Enable or disable charge pump.  Follow with 0X14 enable, 0X10 disable. */
-#define SSD1306_CHARGEPUMP 0x8D
-/** Set Segment Re-map between data column and the segment driver. */
-#define SSD1306_SEGREMAP 0xA0
-/** Resume display from GRAM content. */
-#define SSD1306_DISPLAYALLON_RESUME 0xA4
-/** Force display on regardless of GRAM content. */
-#define SSD1306_DISPLAYALLON 0xA5
-/** Set Normal Display. */
-#define SSD1306_NORMALDISPLAY 0xA6
-/** Set Inverse Display. */
-#define SSD1306_INVERTDISPLAY 0xA7
-/** Set Multiplex Ratio from 16 to 63. */
-#define SSD1306_SETMULTIPLEX 0xA8
-/** Set Display off. */
-#define SSD1306_DISPLAYOFF 0xAE
-/** Set Display on. */
-#define SSD1306_DISPLAYON 0xAF
-/**Set GDDRAM Page Start Address. */
-#define SSD1306_SETSTARTPAGE 0XB0
-/** Set COM output scan direction normal. */
-#define SSD1306_COMSCANINC 0xC0
-/** Set COM output scan direction reversed. */
-#define SSD1306_COMSCANDEC 0xC8
-/** Set Display Offset. */
-#define SSD1306_SETDISPLAYOFFSET 0xD3
-/** Sets COM signals pin configuration to match the OLED panel layout. */
-#define SSD1306_SETCOMPINS 0xDA
-/** This command adjusts the VCOMH regulator output. */
-#define SSD1306_SETVCOMDETECT 0xDB
-/** Set Display Clock Divide Ratio/ Oscillator Frequency. */
-#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
-/** Set Pre-charge Period */
-#define SSD1306_SETPRECHARGE 0xD9
-/** Deactivate scroll */
-#define SSD1306_DEACTIVATE_SCROLL 0x2E
-/** No Operation Command. */
-#define SSD1306_NOP 0XE3
-//------------------------------------------------------------------------------
-/** Set Pump voltage value: (30H~33H) 6.4, 7.4, 8.0 (POR), 9.0. */
-#define SH1106_SET_PUMP_VOLTAGE 0X30
-/** First byte of set charge pump mode */
-#define SH1106_SET_PUMP_MODE 0XAD
-/** Second byte charge pump on. */
-#define SH1106_PUMP_ON 0X8B
-/** Second byte charge pump off. */
-#define SH1106_PUMP_OFF 0X8A
-//------------------------------------------------------------------------------
-/**
- * @struct DevType
- * @brief Device initialization structure.
- */
-struct DevType {
-  /**
-   * Pointer to initialization command bytes.
-   */
-  const uint8_t* initcmds;
-  /**
-   * Number of initialization bytes.
-   */
-  const uint8_t initSize;
-  /**
-   * Width of the diaplay in pixels.
-   */
-  const uint8_t lcdWidth;
-  /**
-   * Height of the display in pixels.
-   */
-  const uint8_t lcdHeight;
-  /**
-   * Column offset RAM to display.  Used to pick start column of SH1106.
-   */
-  const uint8_t colOffset;
-};
-//------------------------------------------------------------------------------
-// this section is based on https://github.com/adafruit/Adafruit_SSD1306
-/** Initialization commands for a 128x32 SSD1306 oled display. */
-static const uint8_t MEM_TYPE Adafruit128x32init[] = {
-    // Init sequence for Adafruit 128x32 OLED module
-    0x00,                              // Set to command mode
-    SSD1306_DISPLAYOFF,
-    SSD1306_SETDISPLAYCLOCKDIV, 0x80,  // the suggested ratio 0x80
-    SSD1306_SETMULTIPLEX, 0x1F,        // ratio 32
-    SSD1306_SETDISPLAYOFFSET, 0x0,     // no offset
-    SSD1306_SETSTARTLINE | 0x0,        // line #0
-    SSD1306_CHARGEPUMP, 0x14,          // internal vcc
-    SSD1306_MEMORYMODE, 0x02,          // page mode
-    SSD1306_SEGREMAP | 0x1,            // column 127 mapped to SEG0
-    SSD1306_COMSCANDEC,                // column scan direction reversed
-    SSD1306_SETCOMPINS, 0x02,          // sequential COM pins, disable remap
-    SSD1306_SETCONTRAST, 0x7F,         // contrast level 127
-    SSD1306_SETPRECHARGE, 0xF1,        // pre-charge period (1, 15)
-    SSD1306_SETVCOMDETECT, 0x40,       // vcomh regulator level
-    SSD1306_DISPLAYALLON_RESUME,
-    SSD1306_NORMALDISPLAY,
-    SSD1306_DISPLAYON
-};
-/** Initialize a 128x32 SSD1306 oled display. */
-static const DevType MEM_TYPE Adafruit128x32 = {
-  Adafruit128x32init,
-  sizeof(Adafruit128x32init),
-  128,
-  32,
-  0
-};
-//------------------------------------------------------------------------------
-// This section is based on https://github.com/adafruit/Adafruit_SSD1306
-/** Initialization commands for a 128x64 SSD1306 oled display. */
-static const uint8_t MEM_TYPE Adafruit128x64init[] = {
-    // Init sequence for Adafruit 128x64 OLED module
-    0x00,                              // Set to command mode
-    SSD1306_DISPLAYOFF,
-    SSD1306_SETDISPLAYCLOCKDIV, 0x80,  // the suggested ratio 0x80
-    SSD1306_SETMULTIPLEX, 0x3F,        // ratio 64
-    SSD1306_SETDISPLAYOFFSET, 0x0,     // no offset
-    SSD1306_SETSTARTLINE | 0x0,        // line #0
-    SSD1306_CHARGEPUMP, 0x14,          // internal vcc
-    SSD1306_MEMORYMODE, 0x02,          // page mode
-    SSD1306_SEGREMAP | 0x1,            // column 127 mapped to SEG0
-    SSD1306_COMSCANDEC,                // column scan direction reversed
-    SSD1306_SETCOMPINS, 0x12,          // alt COM pins, disable remap
-    SSD1306_SETCONTRAST, 0x7F,         // contrast level 127
-    SSD1306_SETPRECHARGE, 0xF1,        // pre-charge period (1, 15)
-    SSD1306_SETVCOMDETECT, 0x40,       // vcomh regulator level
-    SSD1306_DISPLAYALLON_RESUME,
-    SSD1306_NORMALDISPLAY,
-    SSD1306_DISPLAYON
-};
-/** Initialize a 128x64 oled display. */
-static const DevType MEM_TYPE Adafruit128x64 = {
-  Adafruit128x64init,
-  sizeof(Adafruit128x64init),
-  128,
-  64,
-  0
-};
-//------------------------------------------------------------------------------
-// This section is based on https://github.com/stanleyhuangyc/MultiLCD
-/** Initialization commands for a 128x64 SH1106 oled display. */
-static const uint8_t MEM_TYPE SH1106_128x64init[] = {
-  0x00,                                  // Set to command mode
-  SSD1306_DISPLAYOFF,
-  SSD1306_SETSTARTPAGE | 0X0,            // set page address
-  SSD1306_SETSTARTLINE | 0x0,            // set start line
-  SSD1306_SETCONTRAST, 0x80,             // 128
-  SSD1306_SEGREMAP | 0X1,                // set segment remap
-  SSD1306_NORMALDISPLAY,                 // normal / reverse
-  SSD1306_SETMULTIPLEX, 0x3F,            // ratio 64
-  SH1106_SET_PUMP_MODE, SH1106_PUMP_ON,  // set charge pump enable
-  SH1106_SET_PUMP_VOLTAGE | 0X2,         // 8.0 volts
-  SSD1306_COMSCANDEC,                    // Com scan direction
-  SSD1306_SETDISPLAYOFFSET, 0X00,        // set display offset
-  SSD1306_SETDISPLAYCLOCKDIV, 0X80,      // set osc division
-  SSD1306_SETPRECHARGE, 0X1F,            // set pre-charge period
-  SSD1306_SETCOMPINS, 0X12,              // set COM pins
-  SSD1306_SETVCOMDETECT,  0x40,          // set vcomh
-  SSD1306_DISPLAYON
-};
-/** Initialize a 128x64 oled SH1106 display. */
-static const DevType MEM_TYPE SH1106_128x64 =  {
-  SH1106_128x64init,
-  sizeof(SH1106_128x64init),
-  128,
-  64,
-  2    // SH1106 is a 132x64 controller.  Use middle 128 columns.
-};
-#endif  // SSD1306init_h
diff --git a/version.h b/version.h
index 1577791..9b926cd 100644
--- a/version.h
+++ b/version.h
@@ -3,7 +3,8 @@
 
 #include "StringFormatter.h"
 
-#define VERSION "3.1.3"
+#define VERSION "3.1.4"
+// 3.1.4 Refactor OLED and LCD drivers and remove unused code
 // 3.1.3 Add a loop delay to give more time for sensing an Ethernet cable connection
 // 3.1.2 Eliminate wait after write when prog is joined or prog power is off
 // 3.1.1 SH1106 OLED Display Offset Fix

From 88949358b419aa3546c625769d49bf8f89700aad Mon Sep 17 00:00:00 2001
From: Neil McKechnie <75813993+Neil-McK@users.noreply.github.com>
Date: Tue, 13 Jul 2021 20:15:56 +0000
Subject: [PATCH 14/18] Committing a SHA

---
 GITHUB_SHA.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index 7776728..cfb82e4 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "a473f6a"
+#define GITHUB_SHA "38be1d6"

From 52e8a939459d77336474756a2d60ad42a9e74460 Mon Sep 17 00:00:00 2001
From: Neil McKechnie <neilmck999@gmail.com>
Date: Thu, 22 Jul 2021 21:49:09 +0100
Subject: [PATCH 15/18] Fix LCD corruption on power-up.

LCD was not working correctly on power up, but was subsequently OK on reset.  Problem was caused by an uninitialised variable.
---
 LiquidCrystal_I2C.cpp | 2 +-
 version.h             | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/LiquidCrystal_I2C.cpp b/LiquidCrystal_I2C.cpp
index 30faad3..1697d70 100644
--- a/LiquidCrystal_I2C.cpp
+++ b/LiquidCrystal_I2C.cpp
@@ -47,7 +47,7 @@ LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t lcd_Addr, uint8_t lcd_cols,
   lcdRows = lcd_rows;
   lcdCols = lcd_cols;
 
-  _backlightval &= ~LCD_BACKLIGHT;
+  _backlightval = 0;
 
   I2CManager.begin();
   I2CManager.setClock(100000L);    // PCF8574 is spec'd to 100kHz.
diff --git a/version.h b/version.h
index 9b926cd..97b4ab8 100644
--- a/version.h
+++ b/version.h
@@ -3,7 +3,8 @@
 
 #include "StringFormatter.h"
 
-#define VERSION "3.1.4"
+#define VERSION "3.1.5"
+// 3.1.5 Fix LCD corruption on power-up
 // 3.1.4 Refactor OLED and LCD drivers and remove unused code
 // 3.1.3 Add a loop delay to give more time for sensing an Ethernet cable connection
 // 3.1.2 Eliminate wait after write when prog is joined or prog power is off

From 129d1c20396e15b66ec120ec32fe2b9e201c085a Mon Sep 17 00:00:00 2001
From: Neil McKechnie <neilmck999@gmail.com>
Date: Thu, 22 Jul 2021 20:49:33 +0000
Subject: [PATCH 16/18] Committing a SHA

---
 GITHUB_SHA.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index cfb82e4..2192045 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "38be1d6"
+#define GITHUB_SHA "52e8a93"

From 1941402c52e2471976fa249b29811ee3cf29d092 Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Sat, 24 Jul 2021 21:21:57 +0200
Subject: [PATCH 17/18] reserve version number

---
 version.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/version.h b/version.h
index 97b4ab8..074a09b 100644
--- a/version.h
+++ b/version.h
@@ -4,6 +4,7 @@
 #include "StringFormatter.h"
 
 #define VERSION "3.1.5"
+// 3.1.6 Make output ID two bytes (reserved version number for that merge/branch)
 // 3.1.5 Fix LCD corruption on power-up
 // 3.1.4 Refactor OLED and LCD drivers and remove unused code
 // 3.1.3 Add a loop delay to give more time for sensing an Ethernet cable connection

From d3ef5f53ae5c245a80a4609b466fb8cff9e61aab Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Sat, 24 Jul 2021 19:22:25 +0000
Subject: [PATCH 18/18] Committing a SHA

---
 GITHUB_SHA.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index 2192045..7d77b2b 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "52e8a93"
+#define GITHUB_SHA "1941402"