Merge branch 'dev' into config-serializer

# Conflicts:
#	test/mocks/Arduino.h
#	test/mocks/Stream.h

src/helpers/NRF52Board.cpp

@@ -311,17 +311,41 @@ float NRF52Board::getMCUTemperature() {
   return temp * 0.25f; // Convert to *C
 }
 
-void NRF52Board::powerOff() {
+void NRF52Board::shutdownPeripherals() {
   // Power off the display if any
 #ifdef DISPLAY_CLASS
-  display.turnOff();
+  if (display.isOn()) {
+    display.turnOff();
+  }
 #endif
-
+  // Prep LoRa radio for power down
+  #ifdef P_LORA_RESET
+    digitalWrite(P_LORA_RESET, HIGH);  // preload OUT latch so pinMode can't glitch NRESET low
+    pinMode(P_LORA_RESET, OUTPUT);
+    digitalWrite(P_LORA_RESET, LOW);   // deliberate hardware reset (datasheet: >=100us)
+    delayMicroseconds(200);
+    digitalWrite(P_LORA_RESET, HIGH);
+  #endif
+  #if defined(P_LORA_SCLK) && defined(P_LORA_MISO) && defined(P_LORA_MOSI)
+    SPI.setPins(P_LORA_MISO, P_LORA_SCLK, P_LORA_MOSI);
+    SPI.begin(); // SPI may not be started on some shutdown paths, need it to shut down radio
+  #endif
+  #ifdef P_LORA_BUSY
+    pinMode(P_LORA_BUSY, INPUT);
+    uint32_t started_at = millis();
+    while (digitalRead(P_LORA_BUSY) && millis() - started_at < 10) {} //wait for radio to be ready
+  #endif
+  #ifdef P_LORA_NSS
+    pinMode(P_LORA_NSS, OUTPUT);
+    digitalWrite(P_LORA_NSS, HIGH);
+  #endif
   // Power off LoRa
   radio_driver.powerOff();
 
   // Keep LoRa inactive during deepsleep
-  digitalWrite(P_LORA_NSS, HIGH);
+  #ifdef P_LORA_NSS
+    digitalWrite(P_LORA_NSS, HIGH);
+  #endif
 
   // Power off GPS if any
   if(sensors.getLocationProvider() != NULL) {
@@ -331,6 +355,10 @@ void NRF52Board::powerOff() {
   // Flush serial buffers
   Serial.flush();
   delay(100);
+}
+
+void NRF52Board::powerOff() {
+  shutdownPeripherals();
 
   // Enter SYSTEMOFF
   uint8_t sd_enabled = 0;

src/helpers/NRF52Board.h

@@ -50,6 +50,7 @@ public:
   virtual uint8_t getStartupReason() const override { return startup_reason; }
   virtual float getMCUTemperature() override;
   virtual void reboot() override { NVIC_SystemReset(); }
+  virtual void shutdownPeripherals();
   virtual void powerOff() override;
   virtual bool getBootloaderVersion(char* version, size_t max_len) override;
   virtual bool startOTAUpdate(const char *id, char reply[]) override;

src/helpers/esp32/SerialBLEInterface.cpp

@@ -103,10 +103,9 @@ void SerialBLEInterface::onMtuChanged(BLEServer* pServer, esp_ble_gatts_cb_param
 
 void SerialBLEInterface::onDisconnect(BLEServer* pServer) {
   BLE_DEBUG_PRINTLN("onDisconnect()");
+  deviceConnected = false;
   if (_isEnabled) {
     adv_restart_time = millis() + ADVERT_RESTART_DELAY;
-
-    // loop() will detect this on next loop, and set deviceConnected to false
   }
 }
 
@@ -118,17 +117,24 @@ void SerialBLEInterface::onWrite(BLECharacteristic* pCharacteristic, esp_ble_gat
 
   if (len > MAX_FRAME_SIZE) {
     BLE_DEBUG_PRINTLN("ERROR: onWrite(), frame too big, len=%d", len);
-  } else if (recv_queue_len >= FRAME_QUEUE_SIZE) {
-    BLE_DEBUG_PRINTLN("ERROR: onWrite(), recv_queue is full!");
   } else {
-    recv_queue[recv_queue_len].len = len;
-    memcpy(recv_queue[recv_queue_len].buf, rxValue, len);
-    recv_queue_len++;
+    Frame frame = {};
+    frame.len = len;
+    memcpy(frame.buf, rxValue, len);
+
+    if (xQueueSend(recv_queue, &frame, 0) != pdTRUE) {
+      BLE_DEBUG_PRINTLN("ERROR: onWrite(), recv_queue is full!");
+    }
   }
 }
 
 // ---------- public methods
 
+void SerialBLEInterface::clearBuffers() {
+  xQueueReset(recv_queue);
+  send_queue_len = 0;
+}
+
 void SerialBLEInterface::enable() { 
   if (_isEnabled) return;
 
@@ -202,21 +208,13 @@ size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) {
     }
   }
 
-  if (recv_queue_len > 0) {   // check recv queue
-    size_t len = recv_queue[0].len;   // take from top of queue
-    memcpy(dest, recv_queue[0].buf, len);
-
-    BLE_DEBUG_PRINTLN("readBytes: sz=%d, hdr=%d", len, (uint32_t) dest[0]);
-
-    recv_queue_len--;
-    for (int i = 0; i < recv_queue_len; i++) {   // delete top item from queue
-      recv_queue[i] = recv_queue[i + 1];
-    }
-    return len;
+  Frame frame;
+  if (xQueueReceive(recv_queue, &frame, 0) == pdTRUE) {
+    memcpy(dest, frame.buf, frame.len);
+    BLE_DEBUG_PRINTLN("readBytes: sz=%d, hdr=%d", (uint32_t) frame.len, (uint32_t) dest[0]);
+    return frame.len;
   }
 
-  if (pServer->getConnectedCount() == 0)  deviceConnected = false;
-
   if (deviceConnected != oldDeviceConnected) {
     if (!deviceConnected) {    // disconnecting
       clearBuffers();

src/helpers/esp32/SerialBLEInterface.h

@@ -5,6 +5,8 @@
 #include <BLEServer.h>
 #include <BLEUtils.h>
 #include <BLE2902.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/queue.h>
 
 class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLEServerCallbacks, BLECharacteristicCallbacks {
   BLEServer *pServer;
@@ -24,12 +26,13 @@ class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLE
   };
 
   #define FRAME_QUEUE_SIZE  4
-  int recv_queue_len;
-  Frame recv_queue[FRAME_QUEUE_SIZE];
+  StaticQueue_t recv_queue_state;
+  uint8_t recv_queue_storage[FRAME_QUEUE_SIZE * sizeof(Frame)];
+  QueueHandle_t recv_queue;
   int send_queue_len;
   Frame send_queue[FRAME_QUEUE_SIZE];
 
-  void clearBuffers() { recv_queue_len = 0; send_queue_len = 0; }
+  void clearBuffers();
 
 protected:
   // BLESecurityCallbacks methods
@@ -58,7 +61,10 @@ public:
     _isEnabled = false;
     _last_write = 0;
     last_conn_id = 0;
-    send_queue_len = recv_queue_len = 0;
+    recv_queue = xQueueCreateStatic(
+      FRAME_QUEUE_SIZE, sizeof(Frame), recv_queue_storage, &recv_queue_state
+    );
+    send_queue_len = 0;
   }
 
   /**

src/helpers/esp32/TBeamBoard.cpp

@@ -16,6 +16,16 @@ void TBeamBoard::begin() {
     
     ESP32Board::begin();
 
+#ifdef TBEAM_SUPREME_SX1262
+    // On the T-Beam S3 Supreme the PMU + RTC sit on Wire1 (GPIO 42/41, brought
+    // up by XPowersLib), while the SH1106 OLED and the BME280/QMC6310 sensors
+    // sit on the primary bus, Wire (GPIO PIN_BOARD_SDA/PIN_BOARD_SCL). Nothing
+    // else initialises Wire on this board, so the display driver would
+    // otherwise talk on the wrong (default) pins and display.begin() fails,
+    // leaving the screen blank. Bring the OLED bus up here.
+    Wire.begin(PIN_BOARD_SDA, PIN_BOARD_SCL);
+#endif
+
     power_init();
 
     //Configure user button

src/helpers/nrf52/SerialEthernetInterface.cpp

@@ -1,3 +1,5 @@
+#ifdef ETHERNET_ENABLED
+
 #include "SerialEthernetInterface.h"
 #include "EthernetMac.h"
 #include <SPI.h>
@@ -262,3 +264,5 @@ bool SerialEthernetInterface::isConnected() const {
 void SerialEthernetInterface::loop() {
   Ethernet.maintain();
 }
+
+#endif // ETHERNET_ENABLED

src/helpers/nrf52/SerialEthernetInterface.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#ifdef ETHERNET_ENABLED
+
 #include "helpers/BaseSerialInterface.h"
 #include <SPI.h>
 #include <RAK13800_W5100S.h>
@@ -76,3 +78,5 @@ class SerialEthernetInterface : public BaseSerialInterface {
   #define ETHERNET_DEBUG_PRINTLN(...) {}
   #define ETHERNET_DEBUG_PRINT_IP(...) {}
 #endif
+
+#endif // ETHERNET_ENABLED

src/helpers/radiolib/CustomLR1110.h

@@ -4,6 +4,10 @@
 #include "MeshCore.h"
 
 class CustomLR1110 : public LR1110 {
+  uint32_t _preambleMillis = 66;
+  uint32_t _maxPayloadMillis = 3934;
+  uint32_t _activityAt = 0;
+  bool _headerSeen = false;
   bool _rx_boosted = false;
 
   public:
@@ -32,9 +36,49 @@ class CustomLR1110 : public LR1110 {
     bool getRxBoostedGainMode() const { return _rx_boosted; }
 
     bool isReceiving() {
-      uint16_t irq = getIrqStatus();
-      bool detected = ((irq & RADIOLIB_LR11X0_IRQ_SYNC_WORD_HEADER_VALID) || (irq & RADIOLIB_LR11X0_IRQ_PREAMBLE_DETECTED));
-      return detected;
+      uint32_t irq = getIrqStatus();
+      bool preamble = irq & RADIOLIB_LR11X0_IRQ_PREAMBLE_DETECTED;      // bit 4
+      bool header   = irq & RADIOLIB_LR11X0_IRQ_SYNC_WORD_HEADER_VALID; // bit 5
+      bool hdrErr   = irq & RADIOLIB_LR11X0_IRQ_HEADER_ERR;             // bit 6
+      uint32_t now  = millis();
+      if (hdrErr) {
+        clearIrqState(RADIOLIB_LR11X0_IRQ_PREAMBLE_DETECTED | RADIOLIB_LR11X0_IRQ_SYNC_WORD_HEADER_VALID | RADIOLIB_LR11X0_IRQ_HEADER_ERR);
+        _activityAt = 0;
+        _headerSeen = false;
+        return false;
+      }
+      if (header) {
+        if (!_headerSeen) { _headerSeen = true; _activityAt = now; };
+        if (now - _activityAt > _maxPayloadMillis) {
+          MESH_DEBUG_PRINTLN("Clearing header IRQ after %ums", _maxPayloadMillis);
+          clearIrqState(RADIOLIB_LR11X0_IRQ_PREAMBLE_DETECTED | RADIOLIB_LR11X0_IRQ_SYNC_WORD_HEADER_VALID | RADIOLIB_LR11X0_IRQ_HEADER_ERR);
+          _activityAt = 0; _headerSeen = false;
+          return false;
+        }
+        return true;
+      }
+      if (preamble) {
+        if (_activityAt == 0) _activityAt = now;
+        if (now - _activityAt > _preambleMillis) {
+          clearIrqState(RADIOLIB_LR11X0_IRQ_PREAMBLE_DETECTED);
+          _activityAt = 0;
+          MESH_DEBUG_PRINTLN("Clearing preamble IRQ after %ums", _preambleMillis);
+
+          return false;
+        }
+        return true;
+      }
+      _activityAt = 0; _headerSeen = false;
+      return false;
+    }
+    
+    void setPreambleMillis(uint32_t preambleMillis) {
+      _preambleMillis = preambleMillis;
+      MESH_DEBUG_PRINTLN("Set _preambleMillis=%u", _preambleMillis);
+    }
+    void setMaxPayloadMillis(uint32_t payloadMillis) {
+      _maxPayloadMillis = payloadMillis;
+      MESH_DEBUG_PRINTLN("Set _maxPayloadMillis=%u", _maxPayloadMillis);
     }
 
     uint8_t getSpreadingFactor() const { return spreadingFactor; }

src/helpers/radiolib/CustomLR1110Wrapper.h

@@ -14,6 +14,9 @@ public:
     ((CustomLR1110 *)_radio)->setBandwidth(bw);
     ((CustomLR1110 *)_radio)->setCodingRate(cr);
     updatePreamble(sf);
+    PacketMillis pm = calcMaxPacketMillis(sf, bw, cr, preambleLengthForSF(sf));
+    ((CustomLR1110 *)_radio)->setPreambleMillis(pm.preambleMillis);
+    ((CustomLR1110 *)_radio)->setMaxPayloadMillis(pm.payloadMillis);
   }
 
   void doResetAGC() override { lr11x0ResetAGC((LR11x0 *)_radio, ((CustomLR1110 *)_radio)->getFreqMHz()); }

src/helpers/radiolib/CustomSX1262.h

@@ -3,10 +3,12 @@
 #include <RadioLib.h>
 #include "MeshCore.h"
 
-#define SX126X_IRQ_HEADER_VALID                0b0000010000  //  4     4     valid LoRa header received
-#define SX126X_IRQ_PREAMBLE_DETECTED           0x04
-
 class CustomSX1262 : public SX1262 {
+  uint32_t _preambleMillis = 66;
+  uint32_t _maxPayloadMillis = 3934;
+  uint32_t _activityAt = 0;
+  bool _headerSeen = false;
+
   public:
     CustomSX1262(Module *mod) : SX1262(mod) { }
 
@@ -99,9 +101,49 @@ class CustomSX1262 : public SX1262 {
     }
 
     bool isReceiving() {
-      uint16_t irq = getIrqFlags();
-      bool detected = (irq & SX126X_IRQ_HEADER_VALID) || (irq & SX126X_IRQ_PREAMBLE_DETECTED);
-      return detected;
+      uint32_t irq = getIrqFlags();
+      bool preamble = irq & RADIOLIB_SX126X_IRQ_PREAMBLE_DETECTED; // bit 2
+      bool header   = irq & RADIOLIB_SX126X_IRQ_HEADER_VALID;      // bit 4
+      bool hdrErr   = irq & RADIOLIB_SX126X_IRQ_HEADER_ERR;        // bit 5
+      uint32_t now  = millis();
+      if (hdrErr) {
+        clearIrqFlags(RADIOLIB_SX126X_IRQ_PREAMBLE_DETECTED | RADIOLIB_SX126X_IRQ_HEADER_VALID | RADIOLIB_SX126X_IRQ_HEADER_ERR | RADIOLIB_SX126X_IRQ_SYNC_WORD_VALID);
+        _activityAt = 0;
+        _headerSeen = false;
+        return false;
+      }
+      if (header) {
+        if (!_headerSeen) { _headerSeen = true; _activityAt = now; };
+        if (now - _activityAt > _maxPayloadMillis) {
+          MESH_DEBUG_PRINTLN("Clearing header IRQ after %ums", _maxPayloadMillis);
+          clearIrqFlags(RADIOLIB_SX126X_IRQ_PREAMBLE_DETECTED | RADIOLIB_SX126X_IRQ_HEADER_VALID | RADIOLIB_SX126X_IRQ_HEADER_ERR | RADIOLIB_SX126X_IRQ_SYNC_WORD_VALID);
+          _activityAt = 0; _headerSeen = false;
+          return false;
+        }
+        return true;
+      }
+      if (preamble) {
+        if (_activityAt == 0) _activityAt = now;
+        if (now - _activityAt > _preambleMillis) {
+          clearIrqFlags(RADIOLIB_SX126X_IRQ_PREAMBLE_DETECTED);
+          _activityAt = 0;
+          MESH_DEBUG_PRINTLN("Clearing preamble IRQ after %ums", _preambleMillis);
+
+          return false;
+        }
+        return true;
+      }
+      _activityAt = 0; _headerSeen = false;
+      return false;
+    }
+
+    void setPreambleMillis(uint32_t preambleMillis) {
+      _preambleMillis = preambleMillis;
+      MESH_DEBUG_PRINTLN("Set _preambleMillis=%u", _preambleMillis);
+    }
+    void setMaxPayloadMillis(uint32_t payloadMillis) {
+      _maxPayloadMillis = payloadMillis;
+      MESH_DEBUG_PRINTLN("Set _maxPayloadMillis=%u", _maxPayloadMillis);
     }
 
     bool getRxBoostedGainMode() {

src/helpers/radiolib/CustomSX1262Wrapper.h

@@ -18,6 +18,9 @@ public:
     ((CustomSX1262 *)_radio)->setBandwidth(bw);
     ((CustomSX1262 *)_radio)->setCodingRate(cr);
     updatePreamble(sf);
+    PacketMillis pm = calcMaxPacketMillis(sf, bw, cr, preambleLengthForSF(sf));
+    ((CustomSX1262 *)_radio)->setPreambleMillis(pm.preambleMillis);
+    ((CustomSX1262 *)_radio)->setMaxPayloadMillis(pm.payloadMillis);
   }
 
   bool isReceivingPacket() override { 

src/helpers/radiolib/RadioLibWrappers.cpp

@@ -228,3 +228,21 @@ float RadioLibWrapper::packetScoreInt(float snr, int sf, int packet_len) {
 
   return max(0.0, min(1.0, success_rate_based_on_snr * collision_penalty));
 }
+
+PacketMillis RadioLibWrapper::calcMaxPacketMillis(uint8_t sf, float bw, uint8_t cr, uint8_t preambleSymbols) {
+  // based on RadioLib's calculateTimeOnAir()
+  uint32_t tsym_us = ((uint32_t)10000 << sf) / (bw * 10);
+  uint32_t sfCoeff1_x4 = (sf == 5 || sf == 6) ? 25 : 17; // 6.25 : 4.25, semtech magic numbers to account for sync word + sfd
+
+  // preamble + syncword + sfd + header
+  uint32_t preamble_us = (((preambleSymbols + 8) * 4 + sfCoeff1_x4) * tsym_us) / 4;
+  
+  // airtime for max packet at current radio settings
+  uint32_t total_us   = _radio->getTimeOnAir(MAX_TRANS_UNIT);
+  // airtime for payload only (no preamble, header or SOF)
+  uint32_t payload_us = total_us > preamble_us ? total_us - preamble_us : 4000 - preamble_us; // fallback to 4 secs at worst case
+  // rescale payload_us for max possible CR
+  if (cr >= 5 && cr < 8) { payload_us = (payload_us * 8) / cr; }
+
+  return PacketMillis {(preamble_us + 999) / 1000, (payload_us + 999) / 1000};
+}

src/helpers/radiolib/RadioLibWrappers.h

@@ -3,6 +3,11 @@
 #include <Mesh.h>
 #include <RadioLib.h>
 
+struct PacketMillis {
+  uint32_t preambleMillis;  // preamble-detect -> header-valid deadline
+  uint32_t payloadMillis;   // header-valid   -> rx-done deadline
+};
+
 class RadioLibWrapper : public mesh::Radio {
 protected:
   PhysicalLayer* _radio;
@@ -47,6 +52,7 @@ public:
   virtual uint8_t getSpreadingFactor() const { return LORA_SF; }
   static uint16_t preambleLengthForSF(uint8_t sf) { return sf <= 8 ? 32 : 16; }
   void updatePreamble(uint8_t sf) { _preamble_sf = sf; _radio->setPreambleLength(preambleLengthForSF(sf)); }
+  PacketMillis calcMaxPacketMillis(uint8_t sf, float bw, uint8_t cr, uint8_t preambleSymbols);
   virtual int16_t performChannelScan();
 
   int getNoiseFloor() const override { return _noise_floor; }

src/helpers/sensors/MicroNMEALocationProvider.h

@@ -13,8 +13,12 @@
     #endif
 #endif
 
-#ifndef PIN_GPS_EN_ACTIVE
-    #define PIN_GPS_EN_ACTIVE HIGH
+#ifndef GPS_EN_ACTIVE
+    #ifdef PIN_GPS_EN_ACTIVE
+        #define GPS_EN_ACTIVE PIN_GPS_EN_ACTIVE
+    #else
+        #define GPS_EN_ACTIVE HIGH
+    #endif
 #endif
 
 #ifndef GPS_RESET
@@ -25,11 +29,11 @@
     #endif
 #endif
 
-#ifndef GPS_RESET_FORCE
+#ifndef GPS_RESET_ACTIVE
     #ifdef PIN_GPS_RESET_ACTIVE
-        #define GPS_RESET_FORCE PIN_GPS_RESET_ACTIVE
+        #define GPS_RESET_ACTIVE PIN_GPS_RESET_ACTIVE
     #else
-        #define GPS_RESET_FORCE LOW
+        #define GPS_RESET_ACTIVE LOW
     #endif
 #endif
 
@@ -52,11 +56,11 @@ public :
     nmea(_nmeaBuffer, sizeof(_nmeaBuffer)), _clock(clock), _gps_serial(&ser), _peripher_power(peripher_power), _pin_reset(pin_reset), _pin_en(pin_en) {
         if (_pin_reset != -1) {
             pinMode(_pin_reset, OUTPUT);
-            digitalWrite(_pin_reset, GPS_RESET_FORCE);
+            digitalWrite(_pin_reset, GPS_RESET_ACTIVE);
         }
         if (_pin_en != -1) {
             pinMode(_pin_en, OUTPUT);
-            digitalWrite(_pin_en, LOW);
+            digitalWrite(_pin_en, !GPS_EN_ACTIVE);
         }
     }
 
@@ -74,27 +78,27 @@ public :
     void begin() override {
         claim();
         if (_pin_en != -1) {
-            digitalWrite(_pin_en, PIN_GPS_EN_ACTIVE);
+            digitalWrite(_pin_en, GPS_EN_ACTIVE);
         }
         if (_pin_reset != -1) {
-            digitalWrite(_pin_reset, !GPS_RESET_FORCE);
+            digitalWrite(_pin_reset, !GPS_RESET_ACTIVE);
         }
     }
 
     void reset() override {
         if (_pin_reset != -1) {
-            digitalWrite(_pin_reset, GPS_RESET_FORCE);
+            digitalWrite(_pin_reset, GPS_RESET_ACTIVE);
             delay(10);
-            digitalWrite(_pin_reset, !GPS_RESET_FORCE);
+            digitalWrite(_pin_reset, !GPS_RESET_ACTIVE);
         }
     }
 
     void stop() override {
         if (_pin_en != -1) {
-            digitalWrite(_pin_en, !PIN_GPS_EN_ACTIVE);
+            digitalWrite(_pin_en, !GPS_EN_ACTIVE);
         }
         if (_pin_reset != -1) {
-            digitalWrite(_pin_reset, GPS_RESET_FORCE);
+            digitalWrite(_pin_reset, GPS_RESET_ACTIVE);
         }
         release();
     }
@@ -103,7 +107,7 @@ public :
         // directly read the enable pin if present as gps can be
         // activated/deactivated outside of here ...
         if (_pin_en != -1) {
-            return digitalRead(_pin_en) == PIN_GPS_EN_ACTIVE;
+            return digitalRead(_pin_en) == GPS_EN_ACTIVE;
         } else {
             return true; // no enable so must be active
         }

src/helpers/ui/NV3001BDisplay.cpp

@@ -0,0 +1,553 @@
+#include "NV3001BDisplay.h"
+#include <Arduino.h>
+#include <string.h>
+
+#ifndef SPI_FREQUENCY
+  #define SPI_FREQUENCY 8000000
+#endif
+
+#ifndef PIN_TFT_SCL
+  #error "PIN_TFT_SCL must be defined"
+#endif
+
+#ifndef PIN_TFT_SDA
+  #error "PIN_TFT_SDA must be defined"
+#endif
+
+#ifndef PIN_TFT_CS
+  #error "PIN_TFT_CS must be defined"
+#endif
+
+#ifndef PIN_TFT_DC
+  #error "PIN_TFT_DC must be defined"
+#endif
+
+#ifndef PIN_TFT_MISO
+  #define PIN_TFT_MISO -1
+#endif
+
+#ifndef PIN_TFT_RST
+  #define PIN_TFT_RST -1
+#endif
+
+#ifndef PIN_TFT_EN
+  #define PIN_TFT_EN -1
+#endif
+
+#ifndef PIN_TFT_BL
+  #define PIN_TFT_BL -1
+#endif
+
+#ifndef PIN_TFT_EN_ACTIVE
+  #define PIN_TFT_EN_ACTIVE LOW
+#endif
+
+#ifndef PIN_TFT_BL_ACTIVE
+  #define PIN_TFT_BL_ACTIVE HIGH
+#endif
+
+#ifndef DISPLAY_ROTATION
+  #define DISPLAY_ROTATION 0
+#endif
+
+#ifndef NV3001B_SCREEN_WIDTH
+  #define NV3001B_SCREEN_WIDTH 220
+#endif
+
+#ifndef NV3001B_SCREEN_HEIGHT
+  #define NV3001B_SCREEN_HEIGHT 128
+#endif
+
+#ifndef DISPLAY_SCALE_X
+  #define DISPLAY_SCALE_X ((float)NV3001B_SCREEN_WIDTH / NV3001B_LOGICAL_WIDTH)
+#endif
+
+#ifndef DISPLAY_SCALE_Y
+  #define DISPLAY_SCALE_Y ((float)NV3001B_SCREEN_HEIGHT / NV3001B_LOGICAL_HEIGHT)
+#endif
+
+#define NV3001B_SWRESET 0x01
+#define NV3001B_SLPOUT  0x11
+#define NV3001B_DISPON  0x29
+#define NV3001B_CASET   0x2A
+#define NV3001B_RASET   0x2B
+#define NV3001B_RAMWR   0x2C
+#define NV3001B_MADCTL  0x36
+#define NV3001B_COLMOD  0x3A
+
+#define NV3001B_MADCTL_MY  0x80
+#define NV3001B_MADCTL_MX  0x40
+#define NV3001B_MADCTL_MV  0x20
+#define NV3001B_MADCTL_RGB 0x00
+
+#ifndef NV3001B_TEXT_SIZE1_SCALE_X
+  #define NV3001B_TEXT_SIZE1_SCALE_X 1
+#endif
+
+#ifndef NV3001B_TEXT_SIZE1_SCALE_Y
+  #define NV3001B_TEXT_SIZE1_SCALE_Y 2
+#endif
+
+#ifndef NV3001B_TEXT_SIZE2_SCALE_X
+  #define NV3001B_TEXT_SIZE2_SCALE_X 2
+#endif
+
+#ifndef NV3001B_TEXT_SIZE2_SCALE_Y
+  #define NV3001B_TEXT_SIZE2_SCALE_Y 3
+#endif
+
+static uint16_t mapColor(DisplayDriver::Color c) {
+  switch (c) {
+    case DisplayDriver::DARK: return 0x0000;
+    case DisplayDriver::LIGHT: return 0xffff;
+    case DisplayDriver::RED: return 0xf800;
+    case DisplayDriver::GREEN: return 0x07e0;
+    case DisplayDriver::BLUE: return 0x001f;
+    case DisplayDriver::YELLOW: return 0xffe0;
+    case DisplayDriver::ORANGE: return 0xfd20;
+    default: return 0xffff;
+  }
+}
+
+static int scaleX(int x) {
+  return (int)(x * DISPLAY_SCALE_X);
+}
+
+static int scaleY(int y) {
+  return (int)(y * DISPLAY_SCALE_Y);
+}
+
+static int scaleWidth(int x, int w) {
+  if (w <= 0) return 0;
+  int scaled = scaleX(x + w) - scaleX(x);
+  return scaled > 0 ? scaled : 1;
+}
+
+static int scaleHeight(int y, int h) {
+  if (h <= 0) return 0;
+  int scaled = scaleY(y + h) - scaleY(y);
+  return scaled > 0 ? scaled : 1;
+}
+
+static uint8_t nv3001bMADCTL(uint8_t rotation) {
+  uint8_t madctl;
+  switch (rotation & 3) {
+    case 0:
+      madctl = NV3001B_MADCTL_MY | NV3001B_MADCTL_MV | NV3001B_MADCTL_RGB;
+      break;
+    case 1:
+      madctl = NV3001B_MADCTL_MY | NV3001B_MADCTL_MX | NV3001B_MADCTL_RGB;
+      break;
+    case 2:
+      madctl = NV3001B_MADCTL_RGB;
+      break;
+    default:
+      madctl = NV3001B_MADCTL_MX | NV3001B_MADCTL_MV | NV3001B_MADCTL_RGB;
+      break;
+  }
+  return madctl;
+}
+
+static const uint8_t font5x7[] PROGMEM = {
+  0x00, 0x00, 0x00, 0x00, 0x00, 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, 0x14, 0x08, 0x3e, 0x08, 0x14, 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, 0x00, 0x42, 0x7f, 0x40, 0x00, 0x42, 0x61, 0x51, 0x49, 0x46, 0x21,
+  0x41, 0x45, 0x4b, 0x31, 0x18, 0x14, 0x12, 0x7f, 0x10, 0x27, 0x45, 0x45, 0x45, 0x39, 0x3c, 0x4a,
+  0x49, 0x49, 0x30, 0x01, 0x71, 0x09, 0x05, 0x03, 0x36, 0x49, 0x49, 0x49, 0x36, 0x06, 0x49, 0x49,
+  0x29, 0x1e, 0x00, 0x36, 0x36, 0x00, 0x00, 0x00, 0x56, 0x36, 0x00, 0x00, 0x08, 0x14, 0x22, 0x41,
+  0x00, 0x14, 0x14, 0x14, 0x14, 0x14, 0x00, 0x41, 0x22, 0x14, 0x08, 0x02, 0x01, 0x51, 0x09, 0x06,
+  0x32, 0x49, 0x79, 0x41, 0x3e, 0x7e, 0x11, 0x11, 0x11, 0x7e, 0x7f, 0x49, 0x49, 0x49, 0x36, 0x3e,
+  0x41, 0x41, 0x41, 0x22, 0x7f, 0x41, 0x41, 0x22, 0x1c, 0x7f, 0x49, 0x49, 0x49, 0x41, 0x7f, 0x09,
+  0x09, 0x09, 0x01, 0x3e, 0x41, 0x49, 0x49, 0x7a, 0x7f, 0x08, 0x08, 0x08, 0x7f, 0x00, 0x41, 0x7f,
+  0x41, 0x00, 0x20, 0x40, 0x41, 0x3f, 0x01, 0x7f, 0x08, 0x14, 0x22, 0x41, 0x7f, 0x40, 0x40, 0x40,
+  0x40, 0x7f, 0x02, 0x0c, 0x02, 0x7f, 0x7f, 0x04, 0x08, 0x10, 0x7f, 0x3e, 0x41, 0x41, 0x41, 0x3e,
+  0x7f, 0x09, 0x09, 0x09, 0x06, 0x3e, 0x41, 0x51, 0x21, 0x5e, 0x7f, 0x09, 0x19, 0x29, 0x46, 0x46,
+  0x49, 0x49, 0x49, 0x31, 0x01, 0x01, 0x7f, 0x01, 0x01, 0x3f, 0x40, 0x40, 0x40, 0x3f, 0x1f, 0x20,
+  0x40, 0x20, 0x1f, 0x3f, 0x40, 0x38, 0x40, 0x3f, 0x63, 0x14, 0x08, 0x14, 0x63, 0x07, 0x08, 0x70,
+  0x08, 0x07, 0x61, 0x51, 0x49, 0x45, 0x43, 0x00, 0x7f, 0x41, 0x41, 0x00, 0x02, 0x04, 0x08, 0x10,
+  0x20, 0x00, 0x41, 0x41, 0x7f, 0x00, 0x04, 0x02, 0x01, 0x02, 0x04, 0x40, 0x40, 0x40, 0x40, 0x40,
+  0x00, 0x01, 0x02, 0x04, 0x00, 0x20, 0x54, 0x54, 0x54, 0x78, 0x7f, 0x48, 0x44, 0x44, 0x38, 0x38,
+  0x44, 0x44, 0x44, 0x20, 0x38, 0x44, 0x44, 0x48, 0x7f, 0x38, 0x54, 0x54, 0x54, 0x18, 0x08, 0x7e,
+  0x09, 0x01, 0x02, 0x0c, 0x52, 0x52, 0x52, 0x3e, 0x7f, 0x08, 0x04, 0x04, 0x78, 0x00, 0x44, 0x7d,
+  0x40, 0x00, 0x20, 0x40, 0x44, 0x3d, 0x00, 0x7f, 0x10, 0x28, 0x44, 0x00, 0x00, 0x41, 0x7f, 0x40,
+  0x00, 0x7c, 0x04, 0x18, 0x04, 0x78, 0x7c, 0x08, 0x04, 0x04, 0x78, 0x38, 0x44, 0x44, 0x44, 0x38,
+  0x7c, 0x14, 0x14, 0x14, 0x08, 0x08, 0x14, 0x14, 0x18, 0x7c, 0x7c, 0x08, 0x04, 0x04, 0x08, 0x48,
+  0x54, 0x54, 0x54, 0x20, 0x04, 0x3f, 0x44, 0x40, 0x20, 0x3c, 0x40, 0x40, 0x20, 0x7c, 0x1c, 0x20,
+  0x40, 0x20, 0x1c, 0x3c, 0x40, 0x30, 0x40, 0x3c, 0x44, 0x28, 0x10, 0x28, 0x44, 0x0c, 0x50, 0x50,
+  0x50, 0x3c, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 0x08, 0x36, 0x41, 0x00, 0x00, 0x00, 0x7f, 0x00,
+  0x00, 0x00, 0x41, 0x36, 0x08, 0x00, 0x08, 0x08, 0x2a, 0x1c, 0x08, 0x00, 0x06, 0x09, 0x09, 0x06
+};
+
+static int textPixelScaleX(uint8_t size) {
+  return size <= 1 ? NV3001B_TEXT_SIZE1_SCALE_X : NV3001B_TEXT_SIZE2_SCALE_X;
+}
+
+static int textPixelScaleY(uint8_t size) {
+  return size <= 1 ? NV3001B_TEXT_SIZE1_SCALE_Y : NV3001B_TEXT_SIZE2_SCALE_Y;
+}
+
+static void setupOptionalOutput(int pin, int level) {
+  if (pin < 0) return;
+
+  pinMode(pin, OUTPUT);
+  digitalWrite(pin, level);
+}
+
+static void writeOptionalPin(int pin, int level) {
+  if (pin < 0) return;
+
+  digitalWrite(pin, level);
+}
+
+void NV3001BDisplay::writeCommand(uint8_t cmd) {
+  spi.beginTransaction(SPISettings(SPI_FREQUENCY, MSBFIRST, SPI_MODE0));
+  digitalWrite(PIN_TFT_DC, LOW);
+  digitalWrite(PIN_TFT_CS, LOW);
+  spi.transfer(cmd);
+  digitalWrite(PIN_TFT_CS, HIGH);
+  spi.endTransaction();
+}
+
+void NV3001BDisplay::writeBytes(const uint8_t* data, size_t len) {
+  if (!data || len == 0) return;
+
+  spi.beginTransaction(SPISettings(SPI_FREQUENCY, MSBFIRST, SPI_MODE0));
+  digitalWrite(PIN_TFT_DC, HIGH);
+  digitalWrite(PIN_TFT_CS, LOW);
+  for (size_t i = 0; i < len; i++) {
+    spi.transfer(data[i]);
+  }
+  digitalWrite(PIN_TFT_CS, HIGH);
+  spi.endTransaction();
+}
+
+void NV3001BDisplay::writeCommandData(uint8_t cmd, const uint8_t* data, size_t len) {
+  writeCommand(cmd);
+  writeBytes(data, len);
+}
+
+void NV3001BDisplay::setAddrWindow(uint16_t x, uint16_t y, uint16_t w, uint16_t h) {
+  uint16_t x2 = x + w - 1;
+  uint16_t y2 = y + h - 1;
+  uint8_t data[4];
+
+  data[0] = x >> 8;
+  data[1] = x & 0xff;
+  data[2] = x2 >> 8;
+  data[3] = x2 & 0xff;
+  writeCommandData(NV3001B_CASET, data, sizeof(data));
+
+  data[0] = y >> 8;
+  data[1] = y & 0xff;
+  data[2] = y2 >> 8;
+  data[3] = y2 & 0xff;
+  writeCommandData(NV3001B_RASET, data, sizeof(data));
+
+  writeCommand(NV3001B_RAMWR);
+}
+
+void NV3001BDisplay::writeColor(uint16_t rgb, uint32_t count) {
+  uint8_t hi = rgb >> 8;
+  uint8_t lo = rgb & 0xff;
+
+  spi.beginTransaction(SPISettings(SPI_FREQUENCY, MSBFIRST, SPI_MODE0));
+  digitalWrite(PIN_TFT_DC, HIGH);
+  digitalWrite(PIN_TFT_CS, LOW);
+  while (count--) {
+    spi.transfer(hi);
+    spi.transfer(lo);
+  }
+  digitalWrite(PIN_TFT_CS, HIGH);
+  spi.endTransaction();
+}
+
+void NV3001BDisplay::initPanel() {
+#define CMD0(C) do { writeCommand(C); } while (0)
+#define CMD1(C, A) do { const uint8_t d[] = { A }; writeCommandData(C, d, sizeof(d)); } while (0)
+#define CMD2(C, A, B) do { const uint8_t d[] = { A, B }; writeCommandData(C, d, sizeof(d)); } while (0)
+
+  CMD0(NV3001B_SWRESET);
+  delay(120);
+  CMD1(0xFF, 0xA5);
+  CMD1(0x41, 0x00);
+  CMD1(0x50, 0x02);
+  CMD1(0x52, 0x6E);
+  CMD1(0x57, 0x02);
+  CMD1(0x46, 0x11);
+  CMD2(0x47, 0x00, 0x01);
+  CMD2(0x8F, 0x22, 0x03);
+  CMD1(0x9A, 0x78);
+  CMD1(0x9B, 0x78);
+  CMD1(0x9C, 0xA0);
+  CMD1(0x9D, 0x17);
+  CMD1(0x9E, 0xC1);
+  CMD1(0x83, 0x5A);
+  CMD1(0x84, 0xB6);
+  CMD1(0xFF, 0xA5);
+  CMD1(0x85, 0x5F);
+  CMD1(0x6E, 0x0F);
+  CMD1(0x7E, 0x0F);
+  CMD1(0x60, 0x00);
+  CMD1(0x70, 0x00);
+  CMD1(0x6D, 0x33);
+  CMD1(0x7D, 0x37);
+  CMD1(0x61, 0x09);
+  CMD1(0x71, 0x0A);
+  CMD1(0x6C, 0x2A);
+  CMD1(0x7C, 0x36);
+  CMD1(0x62, 0x11);
+  CMD1(0x72, 0x10);
+  CMD1(0x68, 0x4E);
+  CMD1(0x78, 0x4E);
+  CMD1(0x66, 0x36);
+  CMD1(0x76, 0x3C);
+  CMD1(0x1A, 0x1C);
+  CMD1(0x7B, 0x14);
+  CMD1(0x63, 0x0D);
+  CMD1(0x73, 0x0A);
+  CMD1(0x6A, 0x16);
+  CMD1(0x7A, 0x12);
+  CMD1(0x64, 0x0B);
+  CMD1(0x74, 0x0A);
+  CMD1(0x69, 0x08);
+  CMD1(0x79, 0x0A);
+  CMD1(0x65, 0x06);
+  CMD1(0x75, 0x07);
+  CMD1(0x67, 0x23);
+  CMD1(0x77, 0x44);
+  CMD1(0xE0, 0x00);
+  CMD1(0xE9, 0x30);
+  CMD1(0xEB, 0xB7);
+  CMD1(0xEC, 0x00);
+  CMD1(0xED, 0x11);
+  CMD1(0xF0, 0xB7);
+  CMD1(0x53, 0x04);
+  CMD1(0x54, 0x04);
+  CMD1(0x55, 0x40);
+  CMD1(0x56, 0x40);
+  CMD2(0xA0, 0x60, 0x01);
+  CMD1(0xA1, 0x84);
+  CMD1(0xA2, 0x85);
+  CMD2(0xAB, 0x00, 0x02);
+  CMD2(0xAC, 0x00, 0x06);
+  CMD2(0xAD, 0x00, 0x03);
+  CMD2(0xAE, 0x00, 0x07);
+  CMD1(0xC7, 0x01);
+  CMD1(0xB9, 0x82);
+  CMD1(0xBA, 0x83);
+  CMD1(0xBB, 0x00);
+  CMD1(0xBC, 0x81);
+  CMD1(0xBD, 0x02);
+  CMD1(0xBE, 0x01);
+  CMD1(0xBF, 0x04);
+  CMD1(0xC0, 0x03);
+  CMD1(0xC8, 0x55);
+  CMD1(0xC9, 0xC9);
+  CMD1(0xCA, 0xC8);
+  CMD1(0xCB, 0xCB);
+  CMD1(0xCC, 0xCA);
+  CMD1(0xCD, 0x55);
+  CMD1(0xCE, 0xCE);
+  CMD1(0xCF, 0xCD);
+  CMD1(0xD0, 0xD0);
+  CMD1(0xD1, 0xCF);
+  CMD1(0xF2, 0x46);
+  CMD1(0xA8, 0x04);
+  CMD1(0xA9, 0xB0);
+  CMD1(0xAA, 0xA3);
+  CMD1(0xB6, 0x00);
+  CMD1(0xB7, 0xB0);
+  CMD1(0xB8, 0xA3);
+  CMD1(0xC4, 0x03);
+  CMD1(0xC5, 0xB0);
+  CMD1(0xC6, 0xA3);
+  CMD1(0x80, 0x10);
+  CMD1(0xFF, 0x00);
+  CMD1(0x35, 0x00);
+  CMD0(NV3001B_SLPOUT);
+  delay(120);
+  CMD1(NV3001B_COLMOD, 0x05);
+  CMD1(NV3001B_MADCTL, nv3001bMADCTL(DISPLAY_ROTATION));
+  CMD0(NV3001B_DISPON);
+  delay(10);
+
+#undef CMD0
+#undef CMD1
+#undef CMD2
+}
+
+void NV3001BDisplay::fillPhysicalRect(int x, int y, int w, int h) {
+  if (!is_on || w <= 0 || h <= 0) return;
+
+  if (x < 0) {
+    w += x;
+    x = 0;
+  }
+  if (y < 0) {
+    h += y;
+    y = 0;
+  }
+  if (x + w > NV3001B_SCREEN_WIDTH) w = NV3001B_SCREEN_WIDTH - x;
+  if (y + h > NV3001B_SCREEN_HEIGHT) h = NV3001B_SCREEN_HEIGHT - y;
+  if (w <= 0 || h <= 0) return;
+
+  setAddrWindow(x, y, w, h);
+  writeColor(color, (uint32_t)w * h);
+}
+
+void NV3001BDisplay::drawChar(int x, int y, char ch) {
+  if (ch < 32 || ch > 127) ch = '?';
+
+  uint16_t index = (uint16_t)(ch - 32) * 5;
+  int scale_x = textPixelScaleX(text_size);
+  int scale_y = textPixelScaleY(text_size);
+  for (int col = 0; col < 5; col++) {
+    uint8_t line = pgm_read_byte(font5x7 + index + col);
+    for (int row = 0; row < 7; row++) {
+      if (line & (1 << row)) {
+        fillPhysicalRect(x + col * scale_x, y + row * scale_y, scale_x, scale_y);
+      }
+    }
+  }
+}
+
+bool NV3001BDisplay::begin() {
+  if (is_on) return true;
+
+  if (periph_power) periph_power->claim();
+
+  setupOptionalOutput(PIN_TFT_EN, PIN_TFT_EN_ACTIVE);
+  setupOptionalOutput(PIN_TFT_BL, !PIN_TFT_BL_ACTIVE);
+  pinMode(PIN_TFT_CS, OUTPUT);
+  pinMode(PIN_TFT_DC, OUTPUT);
+  digitalWrite(PIN_TFT_CS, HIGH);
+  digitalWrite(PIN_TFT_DC, HIGH);
+  delay(20);
+
+  spi.begin(PIN_TFT_SCL, PIN_TFT_MISO, PIN_TFT_SDA, PIN_TFT_CS);
+  if (PIN_TFT_RST >= 0) {
+    pinMode(PIN_TFT_RST, OUTPUT);
+    digitalWrite(PIN_TFT_RST, HIGH);
+    delay(10);
+    digitalWrite(PIN_TFT_RST, LOW);
+    delay(20);
+    digitalWrite(PIN_TFT_RST, HIGH);
+    delay(120);
+  }
+
+  initPanel();
+  is_on = true;
+  color = 0x0000;
+  fillPhysicalRect(0, 0, NV3001B_SCREEN_WIDTH, NV3001B_SCREEN_HEIGHT);
+  color = 0xffff;
+  text_size = 1;
+  cursor_x = 0;
+  cursor_y = 0;
+  writeOptionalPin(PIN_TFT_BL, PIN_TFT_BL_ACTIVE);
+  return true;
+}
+
+void NV3001BDisplay::turnOn() {
+  begin();
+}
+
+void NV3001BDisplay::turnOff() {
+  if (!is_on) return;
+
+  writeOptionalPin(PIN_TFT_BL, !PIN_TFT_BL_ACTIVE);
+  writeOptionalPin(PIN_TFT_EN, !PIN_TFT_EN_ACTIVE);
+  is_on = false;
+  if (periph_power) periph_power->release();
+}
+
+void NV3001BDisplay::clear() {
+  uint16_t saved = color;
+  color = 0x0000;
+  fillPhysicalRect(0, 0, NV3001B_SCREEN_WIDTH, NV3001B_SCREEN_HEIGHT);
+  color = saved;
+}
+
+void NV3001BDisplay::startFrame(Color bkg) {
+  color = mapColor(bkg);
+  fillPhysicalRect(0, 0, NV3001B_SCREEN_WIDTH, NV3001B_SCREEN_HEIGHT);
+  color = 0xffff;
+  text_size = 1;
+  cursor_x = 0;
+  cursor_y = 0;
+}
+
+void NV3001BDisplay::setTextSize(int sz) {
+  text_size = sz < 1 ? 1 : sz;
+}
+
+void NV3001BDisplay::setColor(Color c) {
+  color = mapColor(c);
+}
+
+void NV3001BDisplay::setCursor(int x, int y) {
+  cursor_x = scaleX(x);
+  cursor_y = scaleY(y);
+}
+
+void NV3001BDisplay::print(const char* str) {
+  if (!str || !is_on) return;
+
+  int scale_x = textPixelScaleX(text_size);
+  int scale_y = textPixelScaleY(text_size);
+  while (*str) {
+    if (*str == '\n') {
+      cursor_x = 0;
+      cursor_y += 8 * scale_y;
+    } else if (*str == '\r') {
+      cursor_x = 0;
+    } else {
+      drawChar(cursor_x, cursor_y, *str);
+      cursor_x += 6 * scale_x;
+    }
+    str++;
+  }
+}
+
+void NV3001BDisplay::fillRect(int x, int y, int w, int h) {
+  fillPhysicalRect(scaleX(x), scaleY(y), scaleWidth(x, w), scaleHeight(y, h));
+}
+
+void NV3001BDisplay::drawRect(int x, int y, int w, int h) {
+  int x1 = scaleX(x);
+  int y1 = scaleY(y);
+  int sw = scaleWidth(x, w);
+  int sh = scaleHeight(y, h);
+
+  fillPhysicalRect(x1, y1, sw, 1);
+  fillPhysicalRect(x1, y1 + sh - 1, sw, 1);
+  fillPhysicalRect(x1, y1, 1, sh);
+  fillPhysicalRect(x1 + sw - 1, y1, 1, sh);
+}
+
+void NV3001BDisplay::drawXbm(int x, int y, const uint8_t* bits, int w, int h) {
+  if (!bits || !is_on) return;
+
+  int byte_width = (w + 7) / 8;
+  for (int j = 0; j < h; j++) {
+    for (int i = 0; i < w; i++) {
+      uint8_t byte = pgm_read_byte(bits + j * byte_width + i / 8);
+      if (byte & (0x80 >> (i & 7))) {
+        fillPhysicalRect(scaleX(x + i), scaleY(y + j), scaleWidth(x + i, 1), scaleHeight(y + j, 1));
+      }
+    }
+  }
+}
+
+uint16_t NV3001BDisplay::getTextWidth(const char* str) {
+  if (!str) return 0;
+
+  uint16_t len = 0;
+  while (str[len] && str[len] != '\n' && str[len] != '\r') len++;
+  return (uint16_t)((len * 6 * textPixelScaleX(text_size)) / DISPLAY_SCALE_X);
+}
+
+void NV3001BDisplay::endFrame() {
+}

src/helpers/ui/NV3001BDisplay.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include "DisplayDriver.h"
+#include <SPI.h>
+#include <helpers/RefCountedDigitalPin.h>
+
+#ifndef NV3001B_LOGICAL_WIDTH
+  #define NV3001B_LOGICAL_WIDTH 128
+#endif
+
+#ifndef NV3001B_LOGICAL_HEIGHT
+  #define NV3001B_LOGICAL_HEIGHT 64
+#endif
+
+#ifndef NV3001B_PANEL_WIDTH
+  #define NV3001B_PANEL_WIDTH 128
+#endif
+
+#ifndef NV3001B_PANEL_HEIGHT
+  #define NV3001B_PANEL_HEIGHT 220
+#endif
+
+#ifndef NV3001B_SPI_HOST
+  #define NV3001B_SPI_HOST HSPI
+#endif
+
+class NV3001BDisplay : public DisplayDriver {
+  SPIClass spi;
+  RefCountedDigitalPin* periph_power;
+  bool is_on = false;
+  uint16_t color = 0xffff;
+  uint8_t text_size = 1;
+  int cursor_x = 0;
+  int cursor_y = 0;
+
+  void writeCommand(uint8_t cmd);
+  void writeBytes(const uint8_t* data, size_t len);
+  void writeCommandData(uint8_t cmd, const uint8_t* data, size_t len);
+  void setAddrWindow(uint16_t x, uint16_t y, uint16_t w, uint16_t h);
+  void writeColor(uint16_t rgb, uint32_t count);
+  void fillPhysicalRect(int x, int y, int w, int h);
+  void initPanel();
+  void drawChar(int x, int y, char ch);
+
+public:
+  NV3001BDisplay(RefCountedDigitalPin* power = nullptr) :
+      DisplayDriver(NV3001B_LOGICAL_WIDTH, NV3001B_LOGICAL_HEIGHT), spi(NV3001B_SPI_HOST), periph_power(power) { }
+
+  bool begin();
+  static const char* driverName() { return "NV3001B"; }
+  static uint16_t physicalWidth() { return NV3001B_PANEL_WIDTH; }
+  static uint16_t physicalHeight() { return NV3001B_PANEL_HEIGHT; }
+
+  bool isOn() override { return is_on; }
+  void turnOn() override;
+  void turnOff() override;
+  void clear() override;
+  void startFrame(Color bkg = DARK) override;
+  void setTextSize(int sz) override;
+  void setColor(Color c) override;
+  void setCursor(int x, int y) override;
+  void print(const char* str) override;
+  void fillRect(int x, int y, int w, int h) override;
+  void drawRect(int x, int y, int w, int h) override;
+  void drawXbm(int x, int y, const uint8_t* bits, int w, int h) override;
+  uint16_t getTextWidth(const char* str) override;
+  void endFrame() override;
+};

src/helpers/ui/RotaryInput.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <Arduino.h>
+
+enum class RotaryInputEvent : uint8_t {
+  None,
+  Next,
+  Prev,
+};
+
+class RotaryInput {
+public:
+  virtual ~RotaryInput() = default;
+
+  virtual bool begin() = 0;
+  virtual RotaryInputEvent poll() = 0;
+  virtual bool isReady() const = 0;
+};

src/helpers/ui/SH1106Display.cpp

@@ -11,7 +11,10 @@ bool SH1106Display::i2c_probe(TwoWire &wire, uint8_t addr)
 
 bool SH1106Display::begin()
 {
-  return display.begin(DISPLAY_ADDRESS, true) && i2c_probe(Wire, DISPLAY_ADDRESS);
+  // Wire must already be initialised by board.begin() before this is called.
+  // Boards with non-standard SH1106 addresses should define DISPLAY_ADDRESS
+  // in their variant/platformio configuration.
+  return i2c_probe(Wire, DISPLAY_ADDRESS) && display.begin(DISPLAY_ADDRESS, true);
 }
 
 void SH1106Display::turnOn()

src/helpers/ui/ST7789LCDDisplay.cpp

@@ -1,5 +1,9 @@
 #include "ST7789LCDDisplay.h"
 
+#ifndef PIN_TFT_MISO
+  #define PIN_TFT_MISO -1
+#endif
+
 #ifndef DISPLAY_ROTATION
   #define DISPLAY_ROTATION 3
 #endif
@@ -29,8 +33,8 @@ bool ST7789LCDDisplay::begin() {
     }
 
     // Im not sure if this is just a t-deck problem or not, if your display is slow try this.
-    #if defined(LILYGO_TDECK) || defined(HELTEC_LORA_V4_TFT)
-      displaySPI.begin(PIN_TFT_SCL, -1, PIN_TFT_SDA, PIN_TFT_CS);
+    #if defined(LILYGO_TDECK) || defined(HELTEC_LORA_V4_TFT) || defined(HELTEC_V4_R8_TFT)
+      displaySPI.begin(PIN_TFT_SCL, PIN_TFT_MISO, PIN_TFT_SDA, PIN_TFT_CS);
     #endif
 
     display.init(DISPLAY_WIDTH, DISPLAY_HEIGHT);

src/helpers/ui/ST7789LCDDisplay.h

@@ -8,7 +8,7 @@
 #include <helpers/RefCountedDigitalPin.h>
 
 class ST7789LCDDisplay : public DisplayDriver {
-  #if defined(LILYGO_TDECK) || defined(HELTEC_LORA_V4_TFT)
+  #if defined(LILYGO_TDECK) || defined(HELTEC_LORA_V4_TFT) || defined(HELTEC_V4_R8_TFT)
     SPIClass displaySPI;
   #endif
   Adafruit_ST7789 display;
@@ -25,7 +25,7 @@ public:
   {
     _isOn = false;
   }
-#elif defined(LILYGO_TDECK) || defined(HELTEC_LORA_V4_TFT)
+#elif defined(LILYGO_TDECK) || defined(HELTEC_LORA_V4_TFT) || defined(HELTEC_V4_R8_TFT)
   ST7789LCDDisplay(RefCountedDigitalPin* peripher_power=NULL) : DisplayDriver(128, 64),
       displaySPI(HSPI),
       display(&displaySPI, PIN_TFT_CS, PIN_TFT_DC, PIN_TFT_RST),