From c442a5787547c6407e1944c2b6b8b8558c69b5db Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Fri, 28 Dec 2018 23:57:16 +0000
Subject: [PATCH 01/11] Refactor code for locking access to MAX7456 to
 functions

Rename max7456Lock var to max7455Mutex. Add max7456Lock(),
max7456Unlock() and max7456TryLock().
---
 src/main/drivers/max7456.c | 39 +++++++++++++++++++++++++++-----------
 1 file changed, 28 insertions(+), 11 deletions(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 8fcd8386db..c6c6e545e8 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -174,10 +174,31 @@ static BITARRAY_DECLARE(screenIsDirty, VIDEO_BUFFER_CHARS_PAL);
 static bool     firstInit = true;
 static uint8_t  videoSignalCfg   = 0;
 static uint8_t  videoSignalReg   = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by default
-static bool  max7456Lock        = false;
+static bool max7456Mutex        = false;
 static bool fontIsLoading       = false;
 static busDevice_t * max7456dev = NULL;
 
+static void max7456Lock(void)
+{
+    while(max7456Mutex);
+
+    max7456Mutex = true;
+}
+
+static void max7456Unlock(void)
+{
+    max7456Mutex = false;
+}
+
+static bool max7456TryLock(void)
+{
+    if (!max7456Mutex) {
+        max7456Mutex = true;
+        return true;
+    }
+    return false;
+}
+
 static int max7456PrepareBuffer(uint8_t * buf, int bufPtr, uint8_t add, uint8_t data)
 {
     buf[bufPtr++] = add;
@@ -355,11 +376,9 @@ void max7456DrawScreenPartial(void)
         return;
     }
 
-    if (!max7456Lock && !fontIsLoading) {
+    if (!fontIsLoading && max7456TryLock()) {
         // (Re)Initialize MAX7456 at startup or stall is detected.
 
-        max7456Lock = true;
-
         // Stall check
         busRead(max7456dev, MAX7456ADD_VM0 | MAX7456ADD_READ, &stallCheck);
 
@@ -430,7 +449,7 @@ void max7456DrawScreenPartial(void)
             busTransfer(max7456dev, NULL, spiBuff, bufPtr);
         }
 
-        max7456Lock = false;
+        max7456Unlock();
     }
 }
 
@@ -447,9 +466,8 @@ void max7456RefreshAll(void)
         return;
     }
 
-    if (!max7456Lock) {
+    if (max7456TryLock()) {
         uint16_t xx;
-        max7456Lock = true;
 
         // Write characters. Start at character zero.
         bufPtr = 0;
@@ -499,7 +517,7 @@ void max7456RefreshAll(void)
 
         // All characters have been set to the MAX7456, none is dirty now.
         memset(screenIsDirty, 0, sizeof(screenIsDirty));
-        max7456Lock = false;
+        max7456Unlock();
     }
 }
 
@@ -513,8 +531,7 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
         return;
     }
 
-    while (max7456Lock);
-    max7456Lock = true;
+    max7456Lock();
 
     // disable display
     fontIsLoading = true;
@@ -547,7 +564,7 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
 #endif
     }
 
-    max7456Lock = false;
+    max7456Unlock();
 }
 
 

From 324a9940425b06ec9340de7b64d33d9dff56e69f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 1 Jan 2019 18:15:41 +0000
Subject: [PATCH 02/11] Add support for read/write MAX7456 NVM for chars > 256

Add max7456Character_t, which represents the data for character a
single MAX7456 character.
Use max7456Character_t to read and pass character data around.
Change max7456WriteNvm() to accept a 2-byte character address.
Change MSP handler for MSP_OSD_CHAR_WRITE to accept 2 byte character
addresses. To keep backwards compatibility, check wether the caller
sent 55 or 56 bytes.
Add max7456ReadNvm() to read a character from the MAX7456 NVM, which
will eventually be used for font metadata.
---
 src/main/drivers/max7456.c | 158 ++++++++++++++++++++++++++++---------
 src/main/drivers/max7456.h |   8 +-
 src/main/fc/fc_msp.c       |  15 ++--
 3 files changed, 137 insertions(+), 44 deletions(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index c6c6e545e8..9900051e5b 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -149,8 +149,10 @@
 #define MAX7456ADD_RB15         0x1f
 #define MAX7456ADD_OSDBL        0x6c
 #define MAX7456ADD_STAT         0xA0
+#define MAX7456ADD_CMDO         0xC0
 
 #define NVM_RAM_SIZE            54
+#define READ_NVR                0x50
 #define WRITE_NVR               0xA0
 
 #define CHARS_PER_LINE          30 // XXX Should be related to VIDEO_BUFFER_CHARS_*?
@@ -173,11 +175,59 @@ static BITARRAY_DECLARE(screenIsDirty, VIDEO_BUFFER_CHARS_PAL);
 
 static bool     firstInit = true;
 static uint8_t  videoSignalCfg   = 0;
-static uint8_t  videoSignalReg   = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by default
+// Buffer for VM0 register
+static uint8_t max7456VM0       = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by defaultstatic bool max7456Mutex        = false;
 static bool max7456Mutex        = false;
-static bool fontIsLoading       = false;
 static busDevice_t * max7456dev = NULL;
 
+static bool max7456ReadVM0(uint8_t *vm0)
+{
+    return busRead(max7456dev, MAX7456ADD_VM0 | MAX7456ADD_READ, vm0);
+}
+
+static bool max7456IsBusy(void)
+{
+    uint8_t status;
+
+    if (busRead(max7456dev, MAX7456ADD_STAT, &status)) {
+        return status & STAT_NVR_BUSY;
+    }
+    // Read failed or busy
+    return true;
+}
+
+// Wait until max7456IsBusy() returns false, toggling a LED on each iteration
+static void max7456WaitUntilNoBusy(void)
+{
+    while (1) {
+        if (!max7456IsBusy()) {
+            break;
+        }
+#ifdef LED0_TOGGLE
+        LED0_TOGGLE;
+#else
+        LED1_TOGGLE;
+#endif
+    }
+}
+
+static bool max7456OSDDisable(void)
+{
+    max7456VM0 &= ~OSD_ENABLE;
+    return busWrite(max7456dev, MAX7456ADD_VM0, max7456VM0);
+}
+
+static bool max7456OSDEnable(void)
+{
+    max7456VM0 |= OSD_ENABLE;
+    return busWrite(max7456dev, MAX7456ADD_VM0, max7456VM0);
+}
+
+static bool max7456OSDIsEnabled(void)
+{
+    return max7456VM0 & OSD_ENABLE;
+}
+
 static void max7456Lock(void)
 {
     while(max7456Mutex);
@@ -212,7 +262,7 @@ uint8_t max7456GetRowsCount(void)
         // Not initialized yet
         return 0;
     }
-    if (videoSignalReg & VIDEO_MODE_PAL) {
+    if (max7456VM0 & VIDEO_MODE_PAL) {
         return VIDEO_LINES_PAL;
     }
 
@@ -240,18 +290,18 @@ void max7456ReInit(void)
 
     switch (videoSignalCfg) {
         case PAL:
-            videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
+            max7456VM0 = VIDEO_MODE_PAL | OSD_ENABLE;
             break;
         case NTSC:
-            videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
+            max7456VM0 = VIDEO_MODE_NTSC | OSD_ENABLE;
             break;
         default:
             busRead(max7456dev, MAX7456ADD_STAT, &srdata);
             if ((0x02 & srdata) == 0x02)
-                videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
+                max7456VM0 = VIDEO_MODE_NTSC | OSD_ENABLE;
     }
 
-    if (videoSignalReg & VIDEO_MODE_PAL) { //PAL
+    if (max7456VM0 & VIDEO_MODE_PAL) { //PAL
         maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
         maxScreenRows = VIDEO_LINES_PAL;
     } else {              // NTSC
@@ -266,7 +316,7 @@ void max7456ReInit(void)
     }
 
     // make sure the Max7456 is enabled
-    bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, videoSignalReg);
+    bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, max7456VM0);
     bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM1, BLINK_DUTY_CYCLE_50_50 | BLINK_TIME_3 | BACKGROUND_BRIGHTNESS_28);
     bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_DMM, DMM_CLEAR_DISPLAY);
 
@@ -364,7 +414,7 @@ void max7456DrawScreenPartial(void)
     // in the MAX7456 is all bits to zero.
     static uint8_t setMode = 0;
 
-    uint8_t stallCheck;
+    uint8_t vm0;
     uint8_t videoSense;
     uint32_t nowMs;
     int pos;
@@ -376,15 +426,15 @@ void max7456DrawScreenPartial(void)
         return;
     }
 
-    if (!fontIsLoading && max7456TryLock()) {
+    if (max7456OSDIsEnabled() && max7456TryLock()) {
         // (Re)Initialize MAX7456 at startup or stall is detected.
 
         // Stall check
-        busRead(max7456dev, MAX7456ADD_VM0 | MAX7456ADD_READ, &stallCheck);
+        bool vm0Read = max7456ReadVM0(&vm0);
 
         nowMs = millis();
 
-        if (stallCheck != videoSignalReg) {
+        if (!vm0Read || vm0 != max7456VM0) {
             max7456ReInit();
         } else if ((videoSignalCfg == VIDEO_SYSTEM_AUTO)
                   && ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) {
@@ -395,8 +445,8 @@ void max7456DrawScreenPartial(void)
             if (videoSense & STAT_LOS) {
                 videoDetectTimeMs = 0;
             } else {
-                if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(videoSignalReg))
-                  || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(videoSignalReg))) {
+                if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(max7456VM0))
+                  || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(max7456VM0))) {
                     if (videoDetectTimeMs) {
                         if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
                             max7456ReInit();
@@ -521,9 +571,41 @@ void max7456RefreshAll(void)
     }
 }
 
-void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
+void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr)
 {
-    uint8_t spiBuff[(54 * 2 + 3) * 2];
+    // Check if device is available
+    if (max7456dev == NULL) {
+        return;
+    }
+
+    max7456Lock();
+    // OSD must be disabled to read or write to NVM
+    max7456OSDDisable();
+
+    busWrite(max7456dev, MAX7456ADD_CMAH, char_address);
+    if (char_address > 255) {
+        // AT7456E and AB7456 have 512 characters of NVM.
+        // To read/write to NVM they use CMAL[7:6] as the
+        // high bits of the character address.
+        uint8_t addr_h = char_address >> 8;
+        busWrite(max7456dev, MAX7456ADD_CMAL, addr_h << 6);
+    }
+    busWrite(max7456dev, MAX7456ADD_CMM, READ_NVR);
+
+    max7456WaitUntilNoBusy();
+
+    for (unsigned ii = 0; ii < sizeof(chr->data); ii++) {
+        busWrite(max7456dev, MAX7456ADD_CMAL, ii);
+        busRead(max7456dev, MAX7456ADD_CMDO, &chr->data[ii]);
+    }
+
+    max7456OSDEnable();
+    max7456Unlock();
+}
+
+void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr)
+{
+    uint8_t spiBuff[(sizeof(chr->data) * 2 + 2) * 2];
     int bufPtr = 0;
 
     // Check if device is available
@@ -532,16 +614,30 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
     }
 
     max7456Lock();
+    // OSD must be disabled to read or write to NVM
+    max7456OSDDisable();
 
-    // disable display
-    fontIsLoading = true;
-
-    bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_VM0, 0);
     bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address); // set start address high
 
-    for (int x = 0; x < 54; x++) {
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAL, x); //set start address low
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMDI, font_data[x]);
+    uint8_t or_val = 0;
+    if (char_address > 255) {
+        // AT7456E and AB7456 have 512 characters of NVM.
+        // To read/write to NVM they use CMAL[7:6] as the
+        // high bits of the character address.
+        //
+        // Instead of issuing an additional write to CMAL when
+        // we're done uploading to shadow RAM, we set the high
+        // bits of CMAL on every write since they have no side
+        // effects while writing from CMDI to RAM and when we
+        // issue the copy command to NVM, CMAL[7:6] is already
+        // set.
+        uint8_t addr_h = char_address >> 8;
+        or_val = addr_h << 6;
+    }
+
+    for (unsigned x = 0; x < sizeof(chr->data); x++) {
+        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAL, x | or_val); //set start address low
+        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMDI, chr->data[x]);
     }
 
     // transfer 54 bytes from shadow ram to NVM
@@ -549,21 +645,9 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
 
     busTransfer(max7456dev, NULL, spiBuff, bufPtr);
 
-    while (1) {
-        uint8_t status;
-
-        busRead(max7456dev, MAX7456ADD_STAT, &status);
-        if ((status & STAT_NVR_BUSY) == 0x00) {
-            break;
-        }
-
-#ifdef LED0_TOGGLE
-        LED0_TOGGLE;
-#else
-        LED1_TOGGLE;
-#endif
-    }
+    max7456WaitUntilNoBusy();
 
+    max7456OSDEnable();
     max7456Unlock();
 }
 
diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h
index 2227ec7483..5c6becb5c5 100644
--- a/src/main/drivers/max7456.h
+++ b/src/main/drivers/max7456.h
@@ -43,10 +43,14 @@ enum VIDEO_TYPES { AUTO = 0, PAL, NTSC };
 
 extern uint16_t maxScreenSize;
 
-struct vcdProfile_s;
+typedef struct max7456Character_s {
+    uint8_t data[54];
+} max7456Character_t;
+
 void    max7456Init(const videoSystem_e videoSystem);
 void    max7456DrawScreenPartial(void);
-void    max7456WriteNvm(uint8_t char_address, const uint8_t *font_data);
+void    max7456ReadNvm(uint16_t char_address, max7456Character_t *chr);
+void    max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr);
 uint8_t max7456GetRowsCount(void);
 void    max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode);
 void    max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 0878a8bf71..b3869a78ed 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -2140,13 +2140,18 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
     case MSP_OSD_CHAR_WRITE:
 #ifdef USE_MAX7456
         if (dataSize >= 55) {
-            uint8_t font_data[64];
-            const uint8_t addr = sbufReadU8(src);
-            for (int i = 0; i < 54; i++) {
-                font_data[i] = sbufReadU8(src);
+            max7456Character_t chr;
+            uint16_t addr;
+            if (dataSize >= 56) {
+                addr = sbufReadU16(src);
+            } else {
+                addr = sbufReadU8(src);
+            }
+            for (unsigned ii = 0; ii < sizeof(chr.data); ii++) {
+                chr.data[ii] = sbufReadU8(src);
             }
             // !!TODO - replace this with a device independent implementation
-            max7456WriteNvm(addr, font_data);
+            max7456WriteNvm(addr, &chr);
         } else
             return MSP_RESULT_ERROR;
 #endif // USE_MAX7456

From f34d57e333ca985b25379dae527fae528126ec9d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 1 Jan 2019 23:08:26 +0000
Subject: [PATCH 03/11] Add workaround for corrupted characters when uploading
 MAX7456 fonts

Restore the previous behavior of rebooting after uploading characters,
but add a comment explaining why it needs to be done.
---
 src/main/drivers/max7456.c | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 9900051e5b..d3ddf02746 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -647,7 +647,12 @@ void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr)
 
     max7456WaitUntilNoBusy();
 
-    max7456OSDEnable();
+    /* XXX: Don't call max7456OSDEnable(), it's intentionally ommited.
+     * If we continue drawing while characters are being uploaded, we
+     * get some corrupted characters from time to time. As a workaround,
+     * we require a reboot after characters have been uploaded to NVM.
+     */
+
     max7456Unlock();
 }
 

From e35d3435086a1255ae6cc83adeb4fa49f4123b6c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 1 Jan 2019 23:21:52 +0000
Subject: [PATCH 04/11] Add support for OSD characters > 255, refactor MAX7456

Change display(Read|Write)Char* functions to use an uint16_t
character. This lets callers which need characters > 255 use these
functions, while keeping most strings in ASCII to save memory (
the string based functions still take a cosnt char *).

Change display drivers to support 16 bit characters when writing
a single character. Note that some drivers might silently truncate
the character, it's the responsability of the caller to check
wether the display supports characters > 255 (API will follow in
the next commit).

Refactor and cleanup the MAX7456 driver:

- Move global state to a struct, for better encapsulation
- Make max7456DrawScreenPartial() static, expose instead the new
function max7456Update(), which calls max7456StallCheck() and
max7456DrawScreenPartial() instead of doing all the work on
a single function.
- Rewrite max7456RefreshAll() to use HW based screen clearing, then
leverage max7456DrawScreenPartial() to redraw only non-blank
characters. This should make full screen redraws faster while
reducing complexity and flash usage (we no longer have 2 paths
for drawing).
- Support writing characters > 255, switching to 8bit mode on
demand.
---
 src/main/drivers/display.c        |  19 +-
 src/main/drivers/display.h        |  10 +-
 src/main/drivers/max7456.c        | 417 ++++++++++++++++--------------
 src/main/drivers/max7456.h        |   6 +-
 src/main/io/displayport_max7456.c |   6 +-
 src/main/io/displayport_msp.c     |   2 +-
 src/main/io/displayport_oled.c    |   2 +-
 src/main/io/osd.c                 |   6 +-
 8 files changed, 250 insertions(+), 218 deletions(-)

diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index ca122435ee..5a6a7b80c8 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -54,12 +54,10 @@ static bool displayAttributesRequireEmulation(displayPort_t *instance, textAttri
 }
 
 static bool displayEmulateTextAttributes(displayPort_t *instance,
-                                        char *buf,
-                                        const char *s, size_t length,
+                                        char *buf, size_t length,
                                         textAttributes_t *attr)
 {
     UNUSED(instance);
-    UNUSED(s);
 
     // We only emulate blink for now, so there's no need to test
     // for it again.
@@ -148,7 +146,7 @@ int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const ch
         // We can't overwrite s, so we use an intermediate buffer if we need
         // text attribute emulation.
         size_t blockSize = length > sizeof(buf) ? sizeof(buf) : length;
-        if (displayEmulateTextAttributes(instance, buf, s, blockSize, &attr)) {
+        if (displayEmulateTextAttributes(instance, buf, blockSize, &attr)) {
             // Emulation required rewriting the string, use buf.
             s = buf;
         }
@@ -157,26 +155,29 @@ int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const ch
     return instance->vTable->writeString(instance, x, y, s, attr);
 }
 
-int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c)
+int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
 {
     instance->posX = x + 1;
     instance->posY = y;
     return instance->vTable->writeChar(instance, x, y, c, TEXT_ATTRIBUTES_NONE);
 }
 
-int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr)
+int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr)
 {
     if (displayAttributesRequireEmulation(instance, attr)) {
-        displayEmulateTextAttributes(instance, (char *)&c, (char *)&c, 1, &attr);
+        char ec;
+        if (displayEmulateTextAttributes(instance, &ec, 1, &attr)) {
+            c = ec;
+        }
     }
     instance->posX = x + 1;
     instance->posY = y;
     return instance->vTable->writeChar(instance, x, y, c, attr);
 }
 
-bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr)
+bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr)
 {
-    uint8_t dc;
+    uint16_t dc;
     textAttributes_t dattr;
 
     if (!instance->vTable->readChar) {
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index c061611792..a807df65a5 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -77,8 +77,8 @@ typedef struct displayPortVTable_s {
     int (*drawScreen)(displayPort_t *displayPort);
     int (*screenSize)(const displayPort_t *displayPort);
     int (*writeString)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text, textAttributes_t attr);
-    int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr);
-    bool (*readChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr);
+    int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr);
+    bool (*readChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr);
     bool (*isTransferInProgress)(const displayPort_t *displayPort);
     int (*heartbeat)(displayPort_t *displayPort);
     void (*resync)(displayPort_t *displayPort);
@@ -104,9 +104,9 @@ int displayScreenSize(const displayPort_t *instance);
 void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y);
 int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s);
 int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const char *s, textAttributes_t attr);
-int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c);
-int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr);
-bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr);
+int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c);
+int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr);
+bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr);
 bool displayIsTransferInProgress(const displayPort_t *instance);
 void displayHeartbeat(displayPort_t *instance);
 void displayResync(displayPort_t *instance);
diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index d3ddf02746..36e3ff1e51 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -108,12 +108,18 @@
 #define MAX7456_SIGNAL_CHECK_INTERVAL_MS 1000 // msec
 
 // DMM special bits
+#define DMM_8BIT_MODE (1 << 6)
 #define DMM_BLINK (1 << 4)
 #define DMM_INVERT_PIXEL_COLOR (1 << 3)
 #define DMM_CLEAR_DISPLAY (1 << 2)
 #define DMM_CLEAR_DISPLAY_VERT (DMM_CLEAR_DISPLAY | 1 << 1)
 #define DMM_AUTOINCREMENT (1 << 0)
 
+#define DMM_IS_8BIT_MODE(val) (val & DMM_8BIT_MODE)
+#define DMM_CHAR_MODE_MASK (MAX7456_MODE_INVERT | MAX7456_MODE_BLINK | MAX7456_MODE_SOLID_BG)
+
+#define DMAH_8_BIT_DMDI_IS_CHAR_ATTR (1 << 1)
+
 // Special address for terminating incremental write
 #define END_STRING 0xff
 
@@ -156,10 +162,14 @@
 #define WRITE_NVR               0xA0
 
 #define CHARS_PER_LINE          30 // XXX Should be related to VIDEO_BUFFER_CHARS_*?
-#define MAKE_CHAR_MODE(c, m)    ((((uint16_t)c) << 8) | m)
+#define MAKE_CHAR_MODE_U8(c, m) ((((uint16_t)c) << 8) | m)
+#define MAKE_CHAR_MODE(c, m)    (MAKE_CHAR_MODE_U8(c, m) | (c > 255 ? CHAR_MODE_EXT : 0))
 #define CHAR_BLANK              MAKE_CHAR_MODE(0x20, 0)
 #define CHAR_BYTE(x)            (x >> 8)
 #define MODE_BYTE(x)            (x & 0xFF)
+#define CHAR_IS_BLANK(x)        ((CHAR_BYTE(x) == 0x20 || CHAR_BYTE(x) == 0x00) && !CHAR_MODE_IS_EXT(MODE_BYTE(x)))
+#define CHAR_MODE_EXT           (1 << 2)
+#define CHAR_MODE_IS_EXT(m)     ((m) & CHAR_MODE_EXT)
 
 uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
 
@@ -171,25 +181,33 @@ static BITARRAY_DECLARE(screenIsDirty, VIDEO_BUFFER_CHARS_PAL);
 
 //max chars to update in one idle
 #define MAX_CHARS2UPDATE        10
-#define BYTES_PER_CHAR2UPDATE   8 // [3-4] spi regs + values for them
+#define BYTES_PER_CHAR2UPDATE   (7 * 2) // SPI regs + values for them
 
-static bool     firstInit = true;
-static uint8_t  videoSignalCfg   = 0;
-// Buffer for VM0 register
-static uint8_t max7456VM0       = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by defaultstatic bool max7456Mutex        = false;
-static bool max7456Mutex        = false;
-static busDevice_t * max7456dev = NULL;
+typedef struct max7456Registers_s {
+    uint8_t vm0;
+    uint8_t dmm;
+} max7456Registers_t;
+
+typedef struct max7456State_s {
+    busDevice_t *dev;
+    videoSystem_e videoSystem;
+    bool isInitialized;
+    bool mutex;
+    max7456Registers_t registers;
+} max7456State_t;
+
+static max7456State_t state;
 
 static bool max7456ReadVM0(uint8_t *vm0)
 {
-    return busRead(max7456dev, MAX7456ADD_VM0 | MAX7456ADD_READ, vm0);
+    return busRead(state.dev, MAX7456ADD_VM0 | MAX7456ADD_READ, vm0);
 }
 
 static bool max7456IsBusy(void)
 {
     uint8_t status;
 
-    if (busRead(max7456dev, MAX7456ADD_STAT, &status)) {
+    if (busRead(state.dev, MAX7456ADD_STAT, &status)) {
         return status & STAT_NVR_BUSY;
     }
     // Read failed or busy
@@ -213,37 +231,37 @@ static void max7456WaitUntilNoBusy(void)
 
 static bool max7456OSDDisable(void)
 {
-    max7456VM0 &= ~OSD_ENABLE;
-    return busWrite(max7456dev, MAX7456ADD_VM0, max7456VM0);
+    state.registers.vm0 &= ~OSD_ENABLE;
+    return busWrite(state.dev, MAX7456ADD_VM0, state.registers.vm0);
 }
 
 static bool max7456OSDEnable(void)
 {
-    max7456VM0 |= OSD_ENABLE;
-    return busWrite(max7456dev, MAX7456ADD_VM0, max7456VM0);
+    state.registers.vm0 |= OSD_ENABLE;
+    return busWrite(state.dev, MAX7456ADD_VM0, state.registers.vm0);
 }
 
 static bool max7456OSDIsEnabled(void)
 {
-    return max7456VM0 & OSD_ENABLE;
+    return state.registers.vm0 & OSD_ENABLE;
 }
 
 static void max7456Lock(void)
 {
-    while(max7456Mutex);
+    while(state.mutex);
 
-    max7456Mutex = true;
+    state.mutex = true;
 }
 
 static void max7456Unlock(void)
 {
-    max7456Mutex = false;
+    state.mutex = false;
 }
 
 static bool max7456TryLock(void)
 {
-    if (!max7456Mutex) {
-        max7456Mutex = true;
+    if (!state.mutex) {
+        state.mutex = true;
         return true;
     }
     return false;
@@ -258,11 +276,11 @@ static int max7456PrepareBuffer(uint8_t * buf, int bufPtr, uint8_t add, uint8_t
 
 uint8_t max7456GetRowsCount(void)
 {
-    if (firstInit) {
+    if (!state.isInitialized) {
         // Not initialized yet
         return 0;
     }
-    if (max7456VM0 & VIDEO_MODE_PAL) {
+    if (state.registers.vm0 & VIDEO_MODE_PAL) {
         return VIDEO_LINES_PAL;
     }
 
@@ -272,7 +290,7 @@ uint8_t max7456GetRowsCount(void)
 //because MAX7456 need some time to detect video system etc. we need to wait for a while to initialize it at startup
 //and in case of restart we need to reinitialize chip. Note that we can't touch screenBuffer here, since
 //it might already have some data by the first time this function is called.
-void max7456ReInit(void)
+static void max7456ReInit(void)
 {
     uint8_t buf[(VIDEO_BUFFER_CHARS_PAL + 3) * 2];
     int bufPtr;
@@ -280,28 +298,34 @@ void max7456ReInit(void)
     uint8_t srdata = 0;
 
     // Check if device is available
-    if (max7456dev == NULL) {
+    if (state.dev == NULL) {
         return;
     }
 
     //do not init MAX before camera power up correctly
-    if (millis() < 1500)
+    if (millis() < 1500) {
         return;
-
-    switch (videoSignalCfg) {
-        case PAL:
-            max7456VM0 = VIDEO_MODE_PAL | OSD_ENABLE;
-            break;
-        case NTSC:
-            max7456VM0 = VIDEO_MODE_NTSC | OSD_ENABLE;
-            break;
-        default:
-            busRead(max7456dev, MAX7456ADD_STAT, &srdata);
-            if ((0x02 & srdata) == 0x02)
-                max7456VM0 = VIDEO_MODE_NTSC | OSD_ENABLE;
     }
 
-    if (max7456VM0 & VIDEO_MODE_PAL) { //PAL
+    switch (state.videoSystem) {
+        case PAL:
+            state.registers.vm0 = VIDEO_MODE_PAL | OSD_ENABLE;
+            break;
+        case NTSC:
+            state.registers.vm0 = VIDEO_MODE_NTSC | OSD_ENABLE;
+            break;
+        default:
+            busRead(state.dev, MAX7456ADD_STAT, &srdata);
+            if ((0x02 & srdata) == 0x02) {
+                state.registers.vm0 = VIDEO_MODE_NTSC | OSD_ENABLE;
+                state.videoSystem = NTSC;
+            } else {
+                state.registers.vm0 = VIDEO_MODE_PAL | OSD_ENABLE;
+                state.videoSystem = PAL;
+            }
+    }
+
+    if (state.registers.vm0 & VIDEO_MODE_PAL) { //PAL
         maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
         maxScreenRows = VIDEO_LINES_PAL;
     } else {              // NTSC
@@ -316,18 +340,20 @@ void max7456ReInit(void)
     }
 
     // make sure the Max7456 is enabled
-    bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, max7456VM0);
+    bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, state.registers.vm0);
     bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM1, BLINK_DUTY_CYCLE_50_50 | BLINK_TIME_3 | BACKGROUND_BRIGHTNESS_28);
     bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_DMM, DMM_CLEAR_DISPLAY);
 
-    // Transfer data to SPI
-    busTransfer(max7456dev, NULL, buf, bufPtr);
+    state.registers.dmm = 0;
 
-    // force redrawing all screen in non-dma mode
-    memset(screenIsDirty, 0xFF, sizeof(screenIsDirty));
-    if (firstInit) {
+    // Transfer data to SPI
+    busTransfer(state.dev, NULL, buf, bufPtr);
+
+    // force redrawing all screen
+    BITARRAY_SET_ALL(screenIsDirty);
+    if (!state.isInitialized) {
         max7456RefreshAll();
-        firstInit = false;
+        state.isInitialized = true;
     }
 }
 
@@ -335,18 +361,18 @@ void max7456ReInit(void)
 //here we init only CS and try to init MAX for first time
 void max7456Init(const videoSystem_e videoSystem)
 {
-    max7456dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD);
+    state.dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD);
 
-    if (max7456dev == NULL) {
+    if (state.dev == NULL) {
         return;
     }
 
-    busSetSpeed(max7456dev, BUS_SPEED_STANDARD);
+    busSetSpeed(state.dev, BUS_SPEED_STANDARD);
 
     // force soft reset on Max7456
-    busWrite(max7456dev, MAX7456ADD_VM0, MAX7456_RESET);
+    busWrite(state.dev, MAX7456ADD_VM0, MAX7456_RESET);
 
-    videoSignalCfg = videoSystem;
+    state.videoSystem = videoSystem;
 
     // Set screenbuffer to all blanks
     for (uint_fast16_t ii = 0; ii < ARRAYLEN(screenBuffer); ii++) {
@@ -366,7 +392,7 @@ void max7456ClearScreen(void)
     }
 }
 
-void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode)
+void max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode)
 {
     unsigned pos = y * CHARS_PER_LINE + x;
     uint16_t val = MAKE_CHAR_MODE(c, mode);
@@ -376,13 +402,17 @@ void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode)
     }
 }
 
-bool max7456ReadChar(uint8_t x, uint8_t y, uint8_t *c, uint8_t *mode)
+bool max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode)
 {
     unsigned pos = y * CHARS_PER_LINE + x;
     if (pos < ARRAYLEN(screenBuffer)) {
         uint16_t val = screenBuffer[pos];
-        *c = val >> 8;
-        *mode = val & 0xFF;
+        *c = CHAR_BYTE(val);
+        *mode = MODE_BYTE(val);
+        if (CHAR_MODE_IS_EXT(*mode)) {
+            *c |= 1 << 8;
+            *mode &= ~CHAR_MODE_EXT;
+        }
         return true;
     }
     return false;
@@ -398,7 +428,7 @@ void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode)
         if (x + i >= CHARS_PER_LINE) {
             break;
         }
-        c = MAKE_CHAR_MODE(*buff, mode);
+        c = MAKE_CHAR_MODE_U8(*buff, mode);
         if (screenBuffer[pos] != c) {
             screenBuffer[pos] = c;
             bitArraySet(screenIsDirty, pos);
@@ -406,99 +436,126 @@ void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode)
     }
 }
 
-void max7456DrawScreenPartial(void)
+// Must be called with the lock held. Returns wether any new characters
+// were drawn.
+static bool max7456DrawScreenPartial(void)
+{
+    uint8_t spiBuff[MAX_CHARS2UPDATE * BYTES_PER_CHAR2UPDATE];
+    int bufPtr = 0;
+    int pos;
+    uint_fast16_t updatedCharCount;
+    uint8_t charMode;
+
+    for (pos = 0, updatedCharCount = 0;;) {
+        pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos);
+        if (pos < 0 || pos >= maxScreenSize) {
+            // No more dirty chars.
+            break;
+        }
+
+        // Found one dirty character to send
+        uint8_t ph = pos >> 8;
+        uint8_t pl = pos & 0xff;
+
+        charMode = MODE_BYTE(screenBuffer[pos]);
+        uint8_t chr = CHAR_BYTE(screenBuffer[pos]);
+        if (CHAR_MODE_IS_EXT(charMode)) {
+            if (!DMM_IS_8BIT_MODE(state.registers.dmm)) {
+                state.registers.dmm |= DMM_8BIT_MODE;
+                bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, state.registers.dmm);
+            }
+
+            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph | DMAH_8_BIT_DMDI_IS_CHAR_ATTR);
+            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl);
+            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, charMode << 2);
+
+            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph);
+            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl);
+            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, chr);
+
+        } else {
+            if (DMM_IS_8BIT_MODE(state.registers.dmm) || (DMM_CHAR_MODE_MASK & state.registers.dmm) != charMode) {
+                state.registers.dmm &= ~DMM_8BIT_MODE;
+                state.registers.dmm = (state.registers.dmm & ~DMM_CHAR_MODE_MASK) | charMode;
+                // Send the attributes for the character run. They
+                // will be applied to all characters until we change
+                // the DMM register.
+                bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, state.registers.dmm);
+            }
+
+            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph);
+            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl);
+            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, chr);
+        }
+
+        bitArrayClr(screenIsDirty, pos);
+        if (++updatedCharCount == MAX_CHARS2UPDATE) {
+            break;
+        }
+        // Start next search at next bit
+        pos++;
+    }
+
+    if (bufPtr) {
+        busTransfer(state.dev, NULL, spiBuff, bufPtr);
+        return true;
+    }
+    return false;
+}
+
+// Must be called with the lock held
+static void max7456StallCheck(void)
 {
     static uint32_t lastSigCheckMs = 0;
     static uint32_t videoDetectTimeMs = 0;
-    // Save this between updates. The default value
-    // in the MAX7456 is all bits to zero.
-    static uint8_t setMode = 0;
 
     uint8_t vm0;
     uint8_t videoSense;
-    uint32_t nowMs;
-    int pos;
-    uint_fast16_t updatedCharCount;
-    uint8_t currentMode;
+    uint32_t nowMs = millis();
 
+    if (!state.isInitialized || !max7456ReadVM0(&vm0) || vm0 != state.registers.vm0) {
+        max7456ReInit();
+        return;
+    }
+
+    if ((state.videoSystem == VIDEO_SYSTEM_AUTO)
+        && ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) {
+
+        // Adjust output format based on the current input format.
+        busRead(state.dev, MAX7456ADD_STAT, &videoSense);
+
+        if (videoSense & STAT_LOS) {
+            videoDetectTimeMs = 0;
+        } else {
+            if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(state.registers.vm0))
+                || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(state.registers.vm0))) {
+                if (videoDetectTimeMs) {
+                    if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
+                        max7456ReInit();
+                    }
+                } else {
+                    // Wait for signal to stabilize
+                    videoDetectTimeMs = millis();
+                }
+            }
+        }
+
+        lastSigCheckMs = nowMs;
+    }
+}
+
+void max7456Update(void)
+{
     // Check if device is available
-    if (max7456dev == NULL) {
+    if (state.dev == NULL) {
         return;
     }
 
     if (max7456OSDIsEnabled() && max7456TryLock()) {
         // (Re)Initialize MAX7456 at startup or stall is detected.
-
-        // Stall check
-        bool vm0Read = max7456ReadVM0(&vm0);
-
-        nowMs = millis();
-
-        if (!vm0Read || vm0 != max7456VM0) {
-            max7456ReInit();
-        } else if ((videoSignalCfg == VIDEO_SYSTEM_AUTO)
-                  && ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) {
-
-            // Adjust output format based on the current input format.
-            busRead(max7456dev, MAX7456ADD_STAT, &videoSense);
-
-            if (videoSense & STAT_LOS) {
-                videoDetectTimeMs = 0;
-            } else {
-                if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(max7456VM0))
-                  || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(max7456VM0))) {
-                    if (videoDetectTimeMs) {
-                        if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
-                            max7456ReInit();
-                        }
-                    } else {
-                        // Wait for signal to stabilize
-                        videoDetectTimeMs = millis();
-                    }
-                }
-            }
-
-            lastSigCheckMs = nowMs;
-        }
-
+        max7456StallCheck();
         //------------   end of (re)init-------------------------------------
-        uint8_t spiBuff[MAX_CHARS2UPDATE * BYTES_PER_CHAR2UPDATE];
-        int bufPtr = 0;
-
-        for (pos = 0, updatedCharCount = 0;;) {
-            pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos);
-            if (pos < 0 || pos >= maxScreenSize) {
-                // No more dirty chars.
-                break;
-            }
-
-            currentMode = MODE_BYTE(screenBuffer[pos]);
-
-            // Found one dirty character to send
-            if (setMode != currentMode) {
-                setMode = currentMode;
-                // Send the attributes for the character run. They
-                // will be applied to all characters until we change
-                // the DMM register.
-                bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, currentMode);
-            }
-
-            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, pos >> 8);
-            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pos & 0xff);
-            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[pos]));
-
-            bitArrayClr(screenIsDirty, pos);
-            if (++updatedCharCount == MAX_CHARS2UPDATE) {
-                break;
-            }
-            // Start next search at next bit
-            pos++;
-        }
-
-        if (bufPtr) {
-            busTransfer(max7456dev, NULL, spiBuff, bufPtr);
-        }
-
+        max7456DrawScreenPartial();
         max7456Unlock();
     }
 }
@@ -508,65 +565,39 @@ void max7456DrawScreenPartial(void)
 // when copter is armed.
 void max7456RefreshAll(void)
 {
-    uint8_t spiBuff[(VIDEO_BUFFER_CHARS_PAL + 4) * 2];
-    int bufPtr;
+    uint8_t dmm;
 
     // Check if device is available
-    if (max7456dev == NULL) {
+    if (state.dev == NULL) {
         return;
     }
 
     if (max7456TryLock()) {
-        uint16_t xx;
 
-        // Write characters. Start at character zero.
-        bufPtr = 0;
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, 0);
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, 0);
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, DMM_AUTOINCREMENT);
+        // Issue an OSD clear command
+        busRead(state.dev, MAX7456ADD_DMM | MAX7456ADD_READ, &dmm);
+        busWrite(state.dev, MAX7456ADD_DMM, state.registers.dmm | DMM_CLEAR_DISPLAY);
 
-        for (xx = 0; xx < maxScreenSize; ++xx) {
-            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[xx]));
+        // Wait for clear to complete (20us)
+        while (1) {
+             busRead(state.dev, MAX7456ADD_DMM | MAX7456ADD_READ, &dmm);
+             if (!(dmm & DMM_CLEAR_DISPLAY)) {
+                 state.registers.dmm = dmm;
+                 break;
+             }
         }
 
-        // Exit auto-increment mode by writing the 0xFF escape sequence to DMDI.
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF);
-
-        // Write chunk of data
-        busTransfer(max7456dev, NULL, spiBuff, bufPtr);
-
-        // Write character attributes. Start at zero, but
-        // set DMAH[1] = 1, to signal that we're sending
-        // attributes rather than characters. Process is the
-        // same as for the characters a few lines up.
-        bufPtr = 0;
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, 1<<1);
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, 0);
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, DMM_AUTOINCREMENT);
-
-        for (xx = 0; xx < maxScreenSize; ++xx) {
-            // Note that atttribute bits in DMDI are in different
-            // positions than in DMM (DMM is used for partial writes),
-            // and we store the attributes in the format expected by
-            // DMM.
-            //      | LBC | BLK | INV
-            // ----------------------
-            // DMDI:|  7  |  6  |  5
-            // DMM: |  5  |  4  |  3
-            //
-            // Thus, we need to shift the bits by 2 when writing character
-            // attributes to DMDI.
-            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, MODE_BYTE(screenBuffer[xx]) << 2);
+        // Mark non-blank characters as dirty
+        BITARRAY_CLR_ALL(screenIsDirty);
+        for (unsigned ii = 0; ii < ARRAYLEN(screenBuffer); ii++) {
+            if (!CHAR_IS_BLANK(screenBuffer[ii])) {
+                bitArraySet(screenIsDirty, ii);
+            }
         }
 
-        // Exit auto-increment mode by writing the 0xFF escape sequence to DMDI.
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF);
+        // Now perform partial writes until there are no dirty ones
+        while(max7456DrawScreenPartial());
 
-        // Write chunk of data
-        busTransfer(max7456dev, NULL, spiBuff, bufPtr);
-
-        // All characters have been set to the MAX7456, none is dirty now.
-        memset(screenIsDirty, 0, sizeof(screenIsDirty));
         max7456Unlock();
     }
 }
@@ -574,7 +605,7 @@ void max7456RefreshAll(void)
 void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr)
 {
     // Check if device is available
-    if (max7456dev == NULL) {
+    if (state.dev == NULL) {
         return;
     }
 
@@ -582,21 +613,21 @@ void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr)
     // OSD must be disabled to read or write to NVM
     max7456OSDDisable();
 
-    busWrite(max7456dev, MAX7456ADD_CMAH, char_address);
+    busWrite(state.dev, MAX7456ADD_CMAH, char_address);
     if (char_address > 255) {
         // AT7456E and AB7456 have 512 characters of NVM.
-        // To read/write to NVM they use CMAL[7:6] as the
+        // To read/write to NVM they use CMAL[6] as the
         // high bits of the character address.
         uint8_t addr_h = char_address >> 8;
-        busWrite(max7456dev, MAX7456ADD_CMAL, addr_h << 6);
+        busWrite(state.dev, MAX7456ADD_CMAL, addr_h << 6);
     }
-    busWrite(max7456dev, MAX7456ADD_CMM, READ_NVR);
+    busWrite(state.dev, MAX7456ADD_CMM, READ_NVR);
 
     max7456WaitUntilNoBusy();
 
     for (unsigned ii = 0; ii < sizeof(chr->data); ii++) {
-        busWrite(max7456dev, MAX7456ADD_CMAL, ii);
-        busRead(max7456dev, MAX7456ADD_CMDO, &chr->data[ii]);
+        busWrite(state.dev, MAX7456ADD_CMAL, ii);
+        busRead(state.dev, MAX7456ADD_CMDO, &chr->data[ii]);
     }
 
     max7456OSDEnable();
@@ -609,7 +640,7 @@ void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr)
     int bufPtr = 0;
 
     // Check if device is available
-    if (max7456dev == NULL) {
+    if (state.dev == NULL) {
         return;
     }
 
@@ -617,19 +648,19 @@ void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr)
     // OSD must be disabled to read or write to NVM
     max7456OSDDisable();
 
-    bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address); // set start address high
+    bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address & 0xFF); // set start address high
 
     uint8_t or_val = 0;
     if (char_address > 255) {
         // AT7456E and AB7456 have 512 characters of NVM.
-        // To read/write to NVM they use CMAL[7:6] as the
-        // high bits of the character address.
+        // To read/write to NVM they use CMAL[6] as the
+        // high bit of the character address.
         //
         // Instead of issuing an additional write to CMAL when
         // we're done uploading to shadow RAM, we set the high
         // bits of CMAL on every write since they have no side
         // effects while writing from CMDI to RAM and when we
-        // issue the copy command to NVM, CMAL[7:6] is already
+        // issue the copy command to NVM, CMAL[6] is already
         // set.
         uint8_t addr_h = char_address >> 8;
         or_val = addr_h << 6;
@@ -643,7 +674,7 @@ void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr)
     // transfer 54 bytes from shadow ram to NVM
     bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMM, WRITE_NVR);
 
-    busTransfer(max7456dev, NULL, spiBuff, bufPtr);
+    busTransfer(state.dev, NULL, spiBuff, bufPtr);
 
     max7456WaitUntilNoBusy();
 
diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h
index 5c6becb5c5..9910adcbef 100644
--- a/src/main/drivers/max7456.h
+++ b/src/main/drivers/max7456.h
@@ -48,13 +48,13 @@ typedef struct max7456Character_s {
 } max7456Character_t;
 
 void    max7456Init(const videoSystem_e videoSystem);
-void    max7456DrawScreenPartial(void);
+void    max7456Update(void);
 void    max7456ReadNvm(uint16_t char_address, max7456Character_t *chr);
 void    max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr);
 uint8_t max7456GetRowsCount(void);
 void    max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode);
-void    max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode);
-bool    max7456ReadChar(uint8_t x, uint8_t y, uint8_t *c, uint8_t *mode);
+void    max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode);
+bool    max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode);
 void    max7456ClearScreen(void);
 void    max7456RefreshAll(void);
 uint8_t* max7456GetScreenBuffer(void);
diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c
index d086b36f32..5dd1fcc30c 100644
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@@ -72,7 +72,7 @@ static int clearScreen(displayPort_t *displayPort)
 static int drawScreen(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
-    max7456DrawScreenPartial();
+    max7456Update();
 
     return 0;
 }
@@ -91,7 +91,7 @@ static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const c
     return 0;
 }
 
-static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr)
+static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr)
 {
     UNUSED(displayPort);
     max7456WriteChar(x, y, c, max7456Mode(attr));
@@ -99,7 +99,7 @@ static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c
     return 0;
 }
 
-static bool readChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr)
+static bool readChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr)
 {
     UNUSED(displayPort);
     return max7456ReadChar(x, y, c, attr);
diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c
index f12969c81b..27fc439e0a 100644
--- a/src/main/io/displayport_msp.c
+++ b/src/main/io/displayport_msp.c
@@ -113,7 +113,7 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con
     return output(displayPort, MSP_DISPLAYPORT, buf, len + 4);
 }
 
-static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t c, textAttributes_t attr)
+static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
 {
     char buf[2];
 
diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c
index 51989bdcfb..bc132e524b 100644
--- a/src/main/io/displayport_oled.c
+++ b/src/main/io/displayport_oled.c
@@ -66,7 +66,7 @@ static int oledWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, con
     return 0;
 }
 
-static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c,  textAttributes_t attr)
+static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c,  textAttributes_t attr)
 {
     UNUSED(displayPort);
     UNUSED(attr);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index aaffa1a93c..87f8841458 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1021,7 +1021,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
                 }
             } else {
 
-                uint8_t c;
+                uint16_t c;
                 if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) {
                     // Something else written here, increase scale. If the display doesn't support reading
                     // back characters, we assume there's nothing.
@@ -1673,7 +1673,7 @@ static bool osdDrawSingleElement(uint8_t item)
                     float fy = dx * (ky / kx) + pitchAngle * pitch_rad_to_char + 0.49f;
                     int8_t dy = floorf(fy);
                     const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
-                    uint8_t c;
+                    uint16_t c;
 
                     if ((dy >= -AH_HEIGHT / 2) && (dy <= AH_HEIGHT / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
                         c = SYM_AH_H_START + ((AH_H_SYM_COUNT - 1) - (uint8_t)((fy - dy) * AH_H_SYM_COUNT));
@@ -1690,7 +1690,7 @@ static bool osdDrawSingleElement(uint8_t item)
                     const float fx = (dy - pitchAngle * pitch_rad_to_char) * (kx / ky) + 0.5f;
                     const int8_t dx = floorf(fx);
                     const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
-                    uint8_t c;
+                    uint16_t c;
 
                     if ((dx >= -AH_WIDTH / 2) && (dx <= AH_WIDTH / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
                         c = SYM_AH_V_START + (fx - dx) * AH_V_SYM_COUNT;

From 8949efa24bc0084582e19528aea2b050bb1a60b7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 2 Jan 2019 13:50:33 +0000
Subject: [PATCH 05/11] Add support for font metadata, use it for detecting
 extended fonts

Also, add boot logo on OSD when the font supports it. If the font
is invalid or out of date, show an "INVALID FONT" message.
---
 src/main/drivers/display.c         | 22 ++++++++++++++++++++
 src/main/drivers/display.h         |  8 ++++++++
 src/main/drivers/max7456_symbols.h |  4 ++++
 src/main/io/displayport_max7456.c  | 33 ++++++++++++++++++++++++++++++
 src/main/io/osd.c                  | 32 ++++++++++++++++++++++++++++-
 5 files changed, 98 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index 5a6a7b80c8..f6a624fb9a 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -157,6 +157,9 @@ int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const ch
 
 int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
 {
+    if (c > instance->maxChar) {
+        return -1;
+    }
     instance->posX = x + 1;
     instance->posY = y;
     return instance->vTable->writeChar(instance, x, y, c, TEXT_ATTRIBUTES_NONE);
@@ -164,6 +167,9 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
 
 int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr)
 {
+    if (c > instance->maxChar) {
+        return -1;
+    }
     if (displayAttributesRequireEmulation(instance, attr)) {
         char ec;
         if (displayEmulateTextAttributes(instance, &ec, 1, &attr)) {
@@ -215,6 +221,14 @@ uint16_t displayTxBytesFree(const displayPort_t *instance)
     return instance->vTable->txBytesFree(instance);
 }
 
+bool displayFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance)
+{
+    if (instance->vTable->fontMetadata) {
+        return instance->vTable->fontMetadata(metadata, instance);
+    }
+    return false;
+}
+
 void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable)
 {
     instance->vTable = vTable;
@@ -229,5 +243,13 @@ void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable)
     if (displayConfig()->force_sw_blink) {
         TEXT_ATTRIBUTES_REMOVE_BLINK(instance->cachedSupportedTextAttributes);
     }
+
+    displayFontMetadata_t metadata;
+    if (displayFontMetadata(&metadata, instance)) {
+        instance->maxChar = metadata.charCount - 1;
+    } else {
+        // Assume 8-bit character implementation
+        instance->maxChar = 255;
+    }
 }
 
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index a807df65a5..c2d769ab0b 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -54,6 +54,11 @@ typedef uint8_t textAttributes_t;
 
 static inline void TEXT_ATTRIBUTES_COPY(textAttributes_t *dst, textAttributes_t *src) { *dst = *src; }
 
+typedef struct displayFontMetadata_s {
+    uint8_t version;
+    uint16_t charCount;
+} displayFontMetadata_t;
+
 struct displayPortVTable_s;
 typedef struct displayPort_s {
     const struct displayPortVTable_s *vTable;
@@ -68,6 +73,7 @@ typedef struct displayPort_s {
     int8_t cursorRow;
     int8_t grabCount;
     textAttributes_t cachedSupportedTextAttributes;
+    uint16_t maxChar;
 } displayPort_t;
 
 typedef struct displayPortVTable_s {
@@ -84,6 +90,7 @@ typedef struct displayPortVTable_s {
     void (*resync)(displayPort_t *displayPort);
     uint32_t (*txBytesFree)(const displayPort_t *displayPort);
     textAttributes_t (*supportedTextAttributes)(const displayPort_t *displayPort);
+    bool (*fontMetadata)(displayFontMetadata_t *metadata, const displayPort_t *displayPort);
 } displayPortVTable_t;
 
 typedef struct displayPortProfile_s {
@@ -111,4 +118,5 @@ bool displayIsTransferInProgress(const displayPort_t *instance);
 void displayHeartbeat(displayPort_t *instance);
 void displayResync(displayPort_t *instance);
 uint16_t displayTxBytesFree(const displayPort_t *instance);
+bool displayFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance);
 void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable);
diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h
index 477d9973db..7cb39c64d5 100644
--- a/src/main/drivers/max7456_symbols.h
+++ b/src/main/drivers/max7456_symbols.h
@@ -196,6 +196,10 @@
 
 #define SYM_AH_V_START            0xE0 // 224 to 229 Vertical AHI
 
+#define SYM_LOGO_START            0x101 // 257 to 280, INAV logo
+#define SYM_LOGO_WIDTH            6
+#define SYM_LOGO_HEIGHT           4
+
 #define SYM_CURSOR SYM_AH_LEFT // Menu cursor
 
 #endif // USE_MAX7456
diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c
index 5dd1fcc30c..d3d2b12297 100644
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@@ -33,6 +33,13 @@
 
 #include "io/displayport_max7456.h"
 
+// 'I', 'N', 'A', 'V', 1
+#define CHR_IS_METADATA(chr) ((chr)->data[0] == 'I' && \
+    (chr)->data[1] == 'N' && \
+    (chr)->data[2] == 'A' && \
+    (chr)->data[3] == 'V' && \
+    (chr)->data[4] == 1)
+
 displayPort_t max7456DisplayPort;
 
 static uint8_t max7456Mode(textAttributes_t attr)
@@ -142,6 +149,31 @@ static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort
     return attr;
 }
 
+static bool fontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+
+    max7456Character_t chr;
+
+    max7456ReadNvm(255, &chr);
+
+    if (CHR_IS_METADATA(&chr)) {
+        metadata->version = chr.data[5];
+        // Not all MAX7456 chips support 512 characters. To detect this,
+        // we place metadata in both characters 255 and 256. This way we
+        // can find out how many characters the font in NVM has.
+        max7456ReadNvm(256, &chr);
+        if (CHR_IS_METADATA(&chr)) {
+            metadata->charCount = 512;
+        } else {
+            metadata->charCount = 256;
+        }
+        return true;
+    }
+
+    return false;
+}
+
 static const displayPortVTable_t max7456VTable = {
     .grab = grab,
     .release = release,
@@ -156,6 +188,7 @@ static const displayPortVTable_t max7456VTable = {
     .resync = resync,
     .txBytesFree = txBytesFree,
     .supportedTextAttributes = supportedTextAttributes,
+    .fontMetadata = fontMetadata,
 };
 
 displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem)
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 87f8841458..48fa06748d 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -124,8 +124,16 @@
     x; \
 })
 
+#define OSD_CHR_IS_NUM(c) (c >= '0' && c <= '9')
+
+#define OSD_CENTER_LEN(x) ((osdDisplayPort->cols - x) / 2)
+#define OSD_CENTER_S(s) OSD_CENTER_LEN(strlen(s))
+
+#define OSD_MIN_FONT_VERSION 1
+
 static unsigned currentLayout = 0;
 static int layoutOverride = -1;
+static bool hasExtendedFont = false; // Wether the font supports characters > 256
 
 typedef struct statistic_s {
     uint16_t max_speed;
@@ -2545,7 +2553,29 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
 
     displayClearScreen(osdDisplayPort);
 
-    uint8_t y = 4;
+    uint8_t y = 1;
+    displayFontMetadata_t metadata;
+    bool fontHasMetadata = displayFontMetadata(&metadata, osdDisplayPort);
+
+    if (fontHasMetadata && metadata.charCount > 256) {
+        hasExtendedFont = true;
+        unsigned logo_c = SYM_LOGO_START;
+        unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH);
+        for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) {
+            for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) {
+                displayWriteChar(osdDisplayPort, logo_x + jj, y, logo_c++);
+            }
+            y++;
+        }
+        y++;
+    } else {
+        if (!fontHasMetadata || metadata.version < OSD_MIN_FONT_VERSION) {
+            const char *m = "INVALID FONT";
+            displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m);
+        }
+        y = 4;
+    }
+
     char string_buffer[30];
     tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING);
     displayWrite(osdDisplayPort, 5, y++, string_buffer);

From 04358bd3d65d509f4934980553e82deb26cbb979 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 2 Jan 2019 15:13:03 +0000
Subject: [PATCH 06/11] Call max7456Init() before displayInit()

Otherwise the display initialization might call methods on the
max7456 driver on an uninitialized device
---
 src/main/io/displayport_max7456.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c
index d3d2b12297..d597332509 100644
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@@ -193,8 +193,8 @@ static const displayPortVTable_t max7456VTable = {
 
 displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem)
 {
-    displayInit(&max7456DisplayPort, &max7456VTable);
     max7456Init(videoSystem);
+    displayInit(&max7456DisplayPort, &max7456VTable);
     resync(&max7456DisplayPort);
     return &max7456DisplayPort;
 }

From 8b9312c408d7613d57c56a3160ac5f29af42e14a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 2 Jan 2019 15:13:52 +0000
Subject: [PATCH 07/11] Ensure the MAX7456 detects a valid signal before
 accepting it

Code was not checking for the Loss-of-Sync bit, causing it to
detect PAL too early in some cases.
---
 src/main/drivers/max7456.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 36e3ff1e51..22dc058e58 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -316,12 +316,12 @@ static void max7456ReInit(void)
             break;
         default:
             busRead(state.dev, MAX7456ADD_STAT, &srdata);
-            if ((0x02 & srdata) == 0x02) {
-                state.registers.vm0 = VIDEO_MODE_NTSC | OSD_ENABLE;
-                state.videoSystem = NTSC;
-            } else {
+            if (VIN_IS_PAL(srdata)) {
                 state.registers.vm0 = VIDEO_MODE_PAL | OSD_ENABLE;
                 state.videoSystem = PAL;
+            } else if (VIN_IS_NTSC_alt(srdata)) {
+                state.registers.vm0 = VIDEO_MODE_NTSC | OSD_ENABLE;
+                state.videoSystem = NTSC;
             }
     }
 

From 3c95544566a689312051e1ad31cb3d1fadd658af Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 2 Jan 2019 19:00:17 +0000
Subject: [PATCH 08/11] Refactor the rest of the MAX7456 driver

- Cleanup init sequence to perform as much initialization as
possible in max7456Init().
- Initialize the OSD as soon as video sync is detected. If no
video sync is detected at 1.5s, start as PAL but then continue
probing until we find a valid video signal.
- Wait for the software reset to finish during initialization.
- Remove extern variable for maxScreenSize, replace it with
max7456GetScreenSize().
- Make sure that when disabling the OSD for reading/writing to
NVM we only enable it again if it was enabled before.
- Rewrite prototypes in the header file to use the same style and
formatting as the rest of the code.
- Delay call to millis() in max7456StallCheck() to avoid calling
it when it's not strictly needed.
---
 src/main/drivers/max7456.c        | 168 +++++++++++++++++-------------
 src/main/drivers/max7456.h        |  21 ++--
 src/main/io/displayport_max7456.c |   2 +-
 3 files changed, 109 insertions(+), 82 deletions(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 22dc058e58..f34349f63c 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -161,6 +161,14 @@
 #define READ_NVR                0x50
 #define WRITE_NVR               0xA0
 
+// Maximum time to wait for video sync. After this we
+// default to PAL
+#define MAX_SYNC_WAIT_MS        1500
+
+// Maximum time to wait for a software reset to complete
+// Under normal conditions, it should take 20us
+#define MAX_RESET_TIMEOUT_MS    50
+
 #define CHARS_PER_LINE          30 // XXX Should be related to VIDEO_BUFFER_CHARS_*?
 #define MAKE_CHAR_MODE_U8(c, m) ((((uint16_t)c) << 8) | m)
 #define MAKE_CHAR_MODE(c, m)    (MAKE_CHAR_MODE_U8(c, m) | (c > 255 ? CHAR_MODE_EXT : 0))
@@ -171,8 +179,6 @@
 #define CHAR_MODE_EXT           (1 << 2)
 #define CHAR_MODE_IS_EXT(m)     ((m) & CHAR_MODE_EXT)
 
-uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
-
 // we write everything in screenBuffer and set a dirty bit
 // in screenIsDirty to upgrade only changed chars this solution
 // is faster than redrawing the whole screen on each frame
@@ -229,15 +235,18 @@ static void max7456WaitUntilNoBusy(void)
     }
 }
 
-static bool max7456OSDDisable(void)
+// Sets wether the OSD drawing is enabled. Returns true iff
+// changes were succesfully performed.
+static bool max7456OSDSetEnabled(bool enabled)
 {
-    state.registers.vm0 &= ~OSD_ENABLE;
-    return busWrite(state.dev, MAX7456ADD_VM0, state.registers.vm0);
-}
-
-static bool max7456OSDEnable(void)
-{
-    state.registers.vm0 |= OSD_ENABLE;
+    if (enabled && !(state.registers.vm0 | OSD_ENABLE)) {
+        state.registers.vm0 |= OSD_ENABLE;
+    } else if (!enabled && (state.registers.vm0 | OSD_ENABLE)) {
+        state.registers.vm0 &= ~OSD_ENABLE;
+    } else {
+        // No changes needed
+        return false;
+    }
     return busWrite(state.dev, MAX7456ADD_VM0, state.registers.vm0);
 }
 
@@ -274,6 +283,19 @@ static int max7456PrepareBuffer(uint8_t * buf, int bufPtr, uint8_t add, uint8_t
     return bufPtr;
 }
 
+uint16_t max7456GetScreenSize(void)
+{
+    // Default to PAL while the display is not yet initialized. This
+    // was the initial behavior and not all callers might be able to
+    // deal with a zero returned from here.
+    // TODO: Inspect all callers, make sure they can handle zero and
+    // change this function to return zero before initialization.
+    if (state.isInitialized && (state.registers.vm0 & VIDEO_LINES_NTSC)) {
+        return VIDEO_BUFFER_CHARS_NTSC;
+    }
+    return VIDEO_BUFFER_CHARS_PAL;
+}
+
 uint8_t max7456GetRowsCount(void)
 {
     if (!state.isInitialized) {
@@ -292,60 +314,43 @@ uint8_t max7456GetRowsCount(void)
 //it might already have some data by the first time this function is called.
 static void max7456ReInit(void)
 {
-    uint8_t buf[(VIDEO_BUFFER_CHARS_PAL + 3) * 2];
+    uint8_t buf[2 * 2];
     int bufPtr;
-    uint8_t maxScreenRows;
-    uint8_t srdata = 0;
+    uint8_t statVal;
+
 
     // Check if device is available
     if (state.dev == NULL) {
         return;
     }
 
-    //do not init MAX before camera power up correctly
-    if (millis() < 1500) {
-        return;
-    }
+    uint8_t vm0Mode;
 
     switch (state.videoSystem) {
         case PAL:
-            state.registers.vm0 = VIDEO_MODE_PAL | OSD_ENABLE;
+            vm0Mode = VIDEO_MODE_PAL;
             break;
         case NTSC:
-            state.registers.vm0 = VIDEO_MODE_NTSC | OSD_ENABLE;
+            vm0Mode = VIDEO_MODE_NTSC;
             break;
         default:
-            busRead(state.dev, MAX7456ADD_STAT, &srdata);
-            if (VIN_IS_PAL(srdata)) {
-                state.registers.vm0 = VIDEO_MODE_PAL | OSD_ENABLE;
-                state.videoSystem = PAL;
-            } else if (VIN_IS_NTSC_alt(srdata)) {
-                state.registers.vm0 = VIDEO_MODE_NTSC | OSD_ENABLE;
-                state.videoSystem = NTSC;
+            busRead(state.dev, MAX7456ADD_STAT, &statVal);
+            if (VIN_IS_PAL(statVal) || millis() > MAX_SYNC_WAIT_MS) {
+                vm0Mode = VIDEO_MODE_PAL;
+            } else if (VIN_IS_NTSC_alt(statVal)) {
+                vm0Mode = VIDEO_MODE_NTSC;
+            } else {
+                // No signal detected yet, wait for detection timeout
+                return;
             }
     }
 
-    if (state.registers.vm0 & VIDEO_MODE_PAL) { //PAL
-        maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
-        maxScreenRows = VIDEO_LINES_PAL;
-    } else {              // NTSC
-        maxScreenSize = VIDEO_BUFFER_CHARS_NTSC;
-        maxScreenRows = VIDEO_LINES_NTSC;
-    }
+    state.registers.vm0 = vm0Mode | OSD_ENABLE;
 
-    // set all rows to same charactor black/white level
-    bufPtr = 0;
-    for (int x = 0; x < maxScreenRows; x++) {
-        bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + x, BWBRIGHTNESS);
-    }
-
-    // make sure the Max7456 is enabled
+    // Enable OSD drawing and clear the display
     bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, state.registers.vm0);
-    bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM1, BLINK_DUTY_CYCLE_50_50 | BLINK_TIME_3 | BACKGROUND_BRIGHTNESS_28);
     bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_DMM, DMM_CLEAR_DISPLAY);
 
-    state.registers.dmm = 0;
-
     // Transfer data to SPI
     busTransfer(state.dev, NULL, buf, bufPtr);
 
@@ -361,6 +366,8 @@ static void max7456ReInit(void)
 //here we init only CS and try to init MAX for first time
 void max7456Init(const videoSystem_e videoSystem)
 {
+    uint8_t buf[(VIDEO_LINES_PAL + 1) * 2];
+    int bufPtr;
     state.dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD);
 
     if (state.dev == NULL) {
@@ -372,6 +379,8 @@ void max7456Init(const videoSystem_e videoSystem)
     // force soft reset on Max7456
     busWrite(state.dev, MAX7456ADD_VM0, MAX7456_RESET);
 
+    // DMM defaults to all zeroes on reset
+    state.registers.dmm = 0;
     state.videoSystem = videoSystem;
 
     // Set screenbuffer to all blanks
@@ -379,7 +388,24 @@ void max7456Init(const videoSystem_e videoSystem)
         screenBuffer[ii] = CHAR_BLANK;
     }
 
-    //real init will be made letter when driver idle detect
+    // Wait for software reset to finish
+    timeMs_t timeout = millis() + MAX_RESET_TIMEOUT_MS;
+    while(max7456ReadVM0(&state.registers.vm0) &&
+        (state.registers.vm0 | MAX7456_RESET) &&
+        millis() < timeout);
+
+    // Set all rows to same charactor black/white level. We
+    // can do this without finding wether the display is PAL
+    // NTSC because all the brightness registers can be written
+    // regardless of the video mode.
+    bufPtr = 0;
+    for (int ii = 0; ii < VIDEO_LINES_PAL; ii++) {
+        bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + ii, BWBRIGHTNESS);
+    }
+
+    // Set the blink duty cycle
+    bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM1, BLINK_DUTY_CYCLE_50_50 | BLINK_TIME_3 | BACKGROUND_BRIGHTNESS_28);
+    busTransfer(state.dev, NULL, buf, bufPtr);
 }
 
 void max7456ClearScreen(void)
@@ -448,7 +474,7 @@ static bool max7456DrawScreenPartial(void)
 
     for (pos = 0, updatedCharCount = 0;;) {
         pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos);
-        if (pos < 0 || pos >= maxScreenSize) {
+        if (pos < 0) {
             // No more dirty chars.
             break;
         }
@@ -510,37 +536,38 @@ static void max7456StallCheck(void)
     static uint32_t videoDetectTimeMs = 0;
 
     uint8_t vm0;
-    uint8_t videoSense;
-    uint32_t nowMs = millis();
+    uint8_t statReg;
 
     if (!state.isInitialized || !max7456ReadVM0(&vm0) || vm0 != state.registers.vm0) {
         max7456ReInit();
         return;
     }
 
-    if ((state.videoSystem == VIDEO_SYSTEM_AUTO)
-        && ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) {
+    if (state.videoSystem == VIDEO_SYSTEM_AUTO) {
+        timeMs_t nowMs = millis();
+        if ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS) {
 
-        // Adjust output format based on the current input format.
-        busRead(state.dev, MAX7456ADD_STAT, &videoSense);
+            // Adjust output format based on the current input format.
+            busRead(state.dev, MAX7456ADD_STAT, &statReg);
 
-        if (videoSense & STAT_LOS) {
-            videoDetectTimeMs = 0;
-        } else {
-            if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(state.registers.vm0))
-                || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(state.registers.vm0))) {
-                if (videoDetectTimeMs) {
-                    if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
-                        max7456ReInit();
+            if (statReg & STAT_LOS) {
+                videoDetectTimeMs = 0;
+            } else {
+                if ((VIN_IS_PAL(statReg) && VIDEO_MODE_IS_NTSC(state.registers.vm0))
+                    || (VIN_IS_NTSC_alt(statReg) && VIDEO_MODE_IS_PAL(state.registers.vm0))) {
+                    if (videoDetectTimeMs) {
+                        if (nowMs - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
+                            max7456ReInit();
+                        }
+                    } else {
+                        // Wait for signal to stabilize
+                        videoDetectTimeMs = nowMs;
                     }
-                } else {
-                    // Wait for signal to stabilize
-                    videoDetectTimeMs = millis();
                 }
             }
-        }
 
-        lastSigCheckMs = nowMs;
+            lastSigCheckMs = nowMs;
+        }
     }
 }
 
@@ -551,11 +578,12 @@ void max7456Update(void)
         return;
     }
 
-    if (max7456OSDIsEnabled() && max7456TryLock()) {
+    if ((max7456OSDIsEnabled() && max7456TryLock()) || !state.isInitialized) {
         // (Re)Initialize MAX7456 at startup or stall is detected.
         max7456StallCheck();
-        //------------   end of (re)init-------------------------------------
-        max7456DrawScreenPartial();
+        if (state.isInitialized) {
+            max7456DrawScreenPartial();
+        }
         max7456Unlock();
     }
 }
@@ -611,7 +639,7 @@ void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr)
 
     max7456Lock();
     // OSD must be disabled to read or write to NVM
-    max7456OSDDisable();
+    bool enabled = max7456OSDSetEnabled(false);
 
     busWrite(state.dev, MAX7456ADD_CMAH, char_address);
     if (char_address > 255) {
@@ -630,7 +658,7 @@ void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr)
         busRead(state.dev, MAX7456ADD_CMDO, &chr->data[ii]);
     }
 
-    max7456OSDEnable();
+    max7456OSDSetEnabled(enabled);
     max7456Unlock();
 }
 
@@ -646,7 +674,7 @@ void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr)
 
     max7456Lock();
     // OSD must be disabled to read or write to NVM
-    max7456OSDDisable();
+    max7456OSDSetEnabled(false);
 
     bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address & 0xFF); // set start address high
 
diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h
index 9910adcbef..be894adde1 100644
--- a/src/main/drivers/max7456.h
+++ b/src/main/drivers/max7456.h
@@ -41,20 +41,19 @@ enum VIDEO_TYPES { AUTO = 0, PAL, NTSC };
 #define MAX7456_MODE_BLINK    (1 << 4)
 #define MAX7456_MODE_SOLID_BG (1 << 5)
 
-extern uint16_t maxScreenSize;
-
 typedef struct max7456Character_s {
     uint8_t data[54];
 } max7456Character_t;
 
-void    max7456Init(const videoSystem_e videoSystem);
-void    max7456Update(void);
-void    max7456ReadNvm(uint16_t char_address, max7456Character_t *chr);
-void    max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr);
+void max7456Init(const videoSystem_e videoSystem);
+void max7456Update(void);
+void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr);
+void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr);
+uint16_t max7456GetScreenSize(void);
 uint8_t max7456GetRowsCount(void);
-void    max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode);
-void    max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode);
-bool    max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode);
-void    max7456ClearScreen(void);
-void    max7456RefreshAll(void);
+void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode);
+void max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode);
+bool max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode);
+void max7456ClearScreen(void);
+void max7456RefreshAll(void);
 uint8_t* max7456GetScreenBuffer(void);
diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c
index d597332509..2700eacc97 100644
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@@ -87,7 +87,7 @@ static int drawScreen(displayPort_t *displayPort)
 static int screenSize(const displayPort_t *displayPort)
 {
     UNUSED(displayPort);
-    return maxScreenSize;
+    return max7456GetScreenSize();
 }
 
 static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s, textAttributes_t attr)

From ac3f9984b620d5a38498e62ad39da6f120ea5317 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 2 Jan 2019 19:09:00 +0000
Subject: [PATCH 09/11] Rename displayFontMetadata() to
 displayGetFontMetadata()

---
 src/main/drivers/display.c | 4 ++--
 src/main/drivers/display.h | 2 +-
 src/main/io/osd.c          | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index f6a624fb9a..685c98c63e 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -221,7 +221,7 @@ uint16_t displayTxBytesFree(const displayPort_t *instance)
     return instance->vTable->txBytesFree(instance);
 }
 
-bool displayFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance)
+bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance)
 {
     if (instance->vTable->fontMetadata) {
         return instance->vTable->fontMetadata(metadata, instance);
@@ -245,7 +245,7 @@ void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable)
     }
 
     displayFontMetadata_t metadata;
-    if (displayFontMetadata(&metadata, instance)) {
+    if (displayGetFontMetadata(&metadata, instance)) {
         instance->maxChar = metadata.charCount - 1;
     } else {
         // Assume 8-bit character implementation
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index c2d769ab0b..7da3199d52 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -118,5 +118,5 @@ bool displayIsTransferInProgress(const displayPort_t *instance);
 void displayHeartbeat(displayPort_t *instance);
 void displayResync(displayPort_t *instance);
 uint16_t displayTxBytesFree(const displayPort_t *instance);
-bool displayFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance);
+bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance);
 void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 48fa06748d..f9a382bc61 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2555,7 +2555,7 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
 
     uint8_t y = 1;
     displayFontMetadata_t metadata;
-    bool fontHasMetadata = displayFontMetadata(&metadata, osdDisplayPort);
+    bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort);
 
     if (fontHasMetadata && metadata.charCount > 256) {
         hasExtendedFont = true;

From 66f38ded64d7794ae21fd61a151dc6a0b586ac14 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 2 Jan 2019 19:13:06 +0000
Subject: [PATCH 10/11] Add comment explaining the shift by 2 bits when writing
 to DMDI

---
 src/main/drivers/max7456.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index f34349f63c..af1213845d 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -362,7 +362,6 @@ static void max7456ReInit(void)
     }
 }
 
-
 //here we init only CS and try to init MAX for first time
 void max7456Init(const videoSystem_e videoSystem)
 {
@@ -493,6 +492,11 @@ static bool max7456DrawScreenPartial(void)
 
             bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph | DMAH_8_BIT_DMDI_IS_CHAR_ATTR);
             bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl);
+            // Attribute bit positions on DMDI are 2 bits up relative to DMM.
+            // DMM uses [5:3] while DMDI uses [7:4] - one bit more for referencing
+            // characters in the [256, 511] range (which is not possible via DMM).
+            // Since we write mostly to DMM, the internal representation uses
+            // the format of the former and we shift it up here.
             bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, charMode << 2);
 
             bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph);

From 34ef222dd369381f9c5b451259ad559d15ef84a3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 2 Jan 2019 19:15:35 +0000
Subject: [PATCH 11/11] Rename fontMetadata in display driver's vTable to
 getFontMetadata

---
 src/main/drivers/display.c        | 4 ++--
 src/main/drivers/display.h        | 2 +-
 src/main/io/displayport_max7456.c | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index 685c98c63e..b26838225a 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -223,8 +223,8 @@ uint16_t displayTxBytesFree(const displayPort_t *instance)
 
 bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance)
 {
-    if (instance->vTable->fontMetadata) {
-        return instance->vTable->fontMetadata(metadata, instance);
+    if (instance->vTable->getFontMetadata) {
+        return instance->vTable->getFontMetadata(metadata, instance);
     }
     return false;
 }
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index 7da3199d52..4b76a6349a 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -90,7 +90,7 @@ typedef struct displayPortVTable_s {
     void (*resync)(displayPort_t *displayPort);
     uint32_t (*txBytesFree)(const displayPort_t *displayPort);
     textAttributes_t (*supportedTextAttributes)(const displayPort_t *displayPort);
-    bool (*fontMetadata)(displayFontMetadata_t *metadata, const displayPort_t *displayPort);
+    bool (*getFontMetadata)(displayFontMetadata_t *metadata, const displayPort_t *displayPort);
 } displayPortVTable_t;
 
 typedef struct displayPortProfile_s {
diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c
index 2700eacc97..cba52cbac1 100644
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@@ -149,7 +149,7 @@ static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort
     return attr;
 }
 
-static bool fontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
+static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
 {
     UNUSED(displayPort);
 
@@ -188,7 +188,7 @@ static const displayPortVTable_t max7456VTable = {
     .resync = resync,
     .txBytesFree = txBytesFree,
     .supportedTextAttributes = supportedTextAttributes,
-    .fontMetadata = fontMetadata,
+    .getFontMetadata = getFontMetadata,
 };
 
 displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem)