Merge pull request #4142 from iNavFlight/agh_max7456_extra_chars

Add support for 512 character fonts on MAX7456, add boot logo

src/main/drivers/display.c

@@ -54,12 +54,10 @@ static bool displayAttributesRequireEmulation(displayPort_t *instance, textAttri
 }
 
 static bool displayEmulateTextAttributes(displayPort_t *instance,
-                                        char *buf,
+                                        char *buf, size_t length,
-                                        const char *s, 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,35 @@ 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)
 {
+    if (c > instance->maxChar) {
+        return -1;
+    }
     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 (c > instance->maxChar) {
+        return -1;
+    }
     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) {
@@ -214,6 +221,14 @@ uint16_t displayTxBytesFree(const displayPort_t *instance)
     return instance->vTable->txBytesFree(instance);
 }
 
+bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance)
+{
+    if (instance->vTable->getFontMetadata) {
+        return instance->vTable->getFontMetadata(metadata, instance);
+    }
+    return false;
+}
+
 void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable)
 {
     instance->vTable = vTable;
@@ -228,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 (displayGetFontMetadata(&metadata, instance)) {
+        instance->maxChar = metadata.charCount - 1;
+    } else {
+        // Assume 8-bit character implementation
+        instance->maxChar = 255;
+    }
 }
 

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 {
@@ -77,13 +83,14 @@ 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);
+    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, uint8_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);
     uint32_t (*txBytesFree)(const displayPort_t *displayPort);
     textAttributes_t (*supportedTextAttributes)(const displayPort_t *displayPort);
+    bool (*getFontMetadata)(displayFontMetadata_t *metadata, const displayPort_t *displayPort);
 } displayPortVTable_t;
 
 typedef struct displayPortProfile_s {
@@ -104,11 +111,12 @@ 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 displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c);
-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);
-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);
 bool displayIsTransferInProgress(const displayPort_t *instance);
 void displayHeartbeat(displayPort_t *instance);
 void displayResync(displayPort_t *instance);
 uint16_t displayTxBytesFree(const displayPort_t *instance);
+bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance);
 void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable);

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
 
@@ -149,17 +155,29 @@
 #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
 
+// 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(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)))
-uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
+#define CHAR_MODE_EXT           (1 << 2)
+#define CHAR_MODE_IS_EXT(m)     ((m) & CHAR_MODE_EXT)
 
 // we write everything in screenBuffer and set a dirty bit
 // in screenIsDirty to upgrade only changed chars this solution
@@ -169,14 +187,94 @@ 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;
+typedef struct max7456Registers_s {
-static uint8_t  videoSignalCfg   = 0;
+    uint8_t vm0;
-static uint8_t  videoSignalReg   = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by default
+    uint8_t dmm;
-static bool  max7456Lock        = false;
+} max7456Registers_t;
-static bool fontIsLoading       = false;
+
-static busDevice_t * max7456dev = NULL;
+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(state.dev, MAX7456ADD_VM0 | MAX7456ADD_READ, vm0);
+}
+
+static bool max7456IsBusy(void)
+{
+    uint8_t status;
+
+    if (busRead(state.dev, 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
+    }
+}
+
+// Sets wether the OSD drawing is enabled. Returns true iff
+// changes were succesfully performed.
+static bool max7456OSDSetEnabled(bool enabled)
+{
+    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);
+}
+
+static bool max7456OSDIsEnabled(void)
+{
+    return state.registers.vm0 & OSD_ENABLE;
+}
+
+static void max7456Lock(void)
+{
+    while(state.mutex);
+
+    state.mutex = true;
+}
+
+static void max7456Unlock(void)
+{
+    state.mutex = false;
+}
+
+static bool max7456TryLock(void)
+{
+    if (!state.mutex) {
+        state.mutex = true;
+        return true;
+    }
+    return false;
+}
 
 static int max7456PrepareBuffer(uint8_t * buf, int bufPtr, uint8_t add, uint8_t data)
 {
@@ -185,13 +283,26 @@ 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 (firstInit) {
+    if (!state.isInitialized) {
         // Not initialized yet
         return 0;
     }
-    if (videoSignalReg & VIDEO_MODE_PAL) {
+    if (state.registers.vm0 & VIDEO_MODE_PAL) {
         return VIDEO_LINES_PAL;
     }
 
@@ -201,88 +312,99 @@ 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];
+    uint8_t buf[2 * 2];
     int bufPtr;
-    uint8_t maxScreenRows;
+    uint8_t statVal;
-    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
+    uint8_t vm0Mode;
-    if (millis() < 1500)
-        return;
 
-    switch (videoSignalCfg) {
+    switch (state.videoSystem) {
         case PAL:
-            videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
+            vm0Mode = VIDEO_MODE_PAL;
             break;
         case NTSC:
-            videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
+            vm0Mode = VIDEO_MODE_NTSC;
             break;
         default:
-            busRead(max7456dev, MAX7456ADD_STAT, &srdata);
+            busRead(state.dev, MAX7456ADD_STAT, &statVal);
-            if ((0x02 & srdata) == 0x02)
+            if (VIN_IS_PAL(statVal) || millis() > MAX_SYNC_WAIT_MS) {
-                videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
+                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 (videoSignalReg & VIDEO_MODE_PAL) { //PAL
+    state.registers.vm0 = vm0Mode | OSD_ENABLE;
-        maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
-        maxScreenRows = VIDEO_LINES_PAL;
-    } else {              // NTSC
-        maxScreenSize = VIDEO_BUFFER_CHARS_NTSC;
-        maxScreenRows = VIDEO_LINES_NTSC;
-    }
 
-    // set all rows to same charactor black/white level
+    // Enable OSD drawing and clear the display
-    bufPtr = 0;
+    bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, state.registers.vm0);
-    for (int x = 0; x < maxScreenRows; x++) {
-        bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + x, BWBRIGHTNESS);
-    }
-
-    // make sure the Max7456 is enabled
-    bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, videoSignalReg);
-    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);
+    busTransfer(state.dev, NULL, buf, bufPtr);
 
-    // force redrawing all screen in non-dma mode
+    // force redrawing all screen
-    memset(screenIsDirty, 0xFF, sizeof(screenIsDirty));
+    BITARRAY_SET_ALL(screenIsDirty);
-    if (firstInit) {
+    if (!state.isInitialized) {
         max7456RefreshAll();
-        firstInit = false;
+        state.isInitialized = true;
     }
 }
 
-
 //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);
+    uint8_t buf[(VIDEO_LINES_PAL + 1) * 2];
+    int bufPtr;
+    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;
+    // DMM defaults to all zeroes on reset
+    state.registers.dmm = 0;
+    state.videoSystem = videoSystem;
 
     // Set screenbuffer to all blanks
     for (uint_fast16_t ii = 0; ii < ARRAYLEN(screenBuffer); ii++) {
         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)
@@ -295,7 +417,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);
@@ -305,13 +427,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;
+        *c = CHAR_BYTE(val);
-        *mode = val & 0xFF;
+        *mode = MODE_BYTE(val);
+        if (CHAR_MODE_IS_EXT(*mode)) {
+            *c |= 1 << 8;
+            *mode &= ~CHAR_MODE_EXT;
+        }
         return true;
     }
     return false;
@@ -327,7 +453,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);
@@ -335,102 +461,134 @@ 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) {
+            // 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);
+            // 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);
+            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 stallCheck;
+    uint8_t vm0;
-    uint8_t videoSense;
+    uint8_t statReg;
-    uint32_t nowMs;
-    int pos;
-    uint_fast16_t updatedCharCount;
-    uint8_t currentMode;
 
-    // Check if device is available
+    if (!state.isInitialized || !max7456ReadVM0(&vm0) || vm0 != state.registers.vm0) {
-    if (max7456dev == NULL) {
+        max7456ReInit();
         return;
     }
 
-    if (!max7456Lock && !fontIsLoading) {
+    if (state.videoSystem == VIDEO_SYSTEM_AUTO) {
-        // (Re)Initialize MAX7456 at startup or stall is detected.
+        timeMs_t nowMs = millis();
-
+        if ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS) {
-        max7456Lock = true;
-
-        // Stall check
-        busRead(max7456dev, MAX7456ADD_VM0 | MAX7456ADD_READ, &stallCheck);
-
-        nowMs = millis();
-
-        if (stallCheck != videoSignalReg) {
-            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);
+            busRead(state.dev, MAX7456ADD_STAT, &statReg);
 
-            if (videoSense & STAT_LOS) {
+            if (statReg & STAT_LOS) {
                 videoDetectTimeMs = 0;
             } else {
-                if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(videoSignalReg))
+                if ((VIN_IS_PAL(statReg) && VIDEO_MODE_IS_NTSC(state.registers.vm0))
-                  || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(videoSignalReg))) {
+                    || (VIN_IS_NTSC_alt(statReg) && VIDEO_MODE_IS_PAL(state.registers.vm0))) {
                     if (videoDetectTimeMs) {
-                        if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
+                        if (nowMs - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
                             max7456ReInit();
                         }
                     } else {
                         // Wait for signal to stabilize
-                        videoDetectTimeMs = millis();
+                        videoDetectTimeMs = nowMs;
                     }
                 }
             }
 
             lastSigCheckMs = nowMs;
         }
+    }
+}
 
-        //------------   end of (re)init-------------------------------------
+void max7456Update(void)
-        uint8_t spiBuff[MAX_CHARS2UPDATE * BYTES_PER_CHAR2UPDATE];
+{
-        int bufPtr = 0;
+    // Check if device is available
+    if (state.dev == NULL) {
+        return;
+    }
 
-        for (pos = 0, updatedCharCount = 0;;) {
+    if ((max7456OSDIsEnabled() && max7456TryLock()) || !state.isInitialized) {
-            pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos);
+        // (Re)Initialize MAX7456 at startup or stall is detected.
-            if (pos < 0 || pos >= maxScreenSize) {
+        max7456StallCheck();
-                // No more dirty chars.
+        if (state.isInitialized) {
-                break;
+            max7456DrawScreenPartial();
-            }
-
-            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++;
         }
-
+        max7456Unlock();
-        if (bufPtr) {
-            busTransfer(max7456dev, NULL, spiBuff, bufPtr);
-        }
-
-        max7456Lock = false;
     }
 }
 
@@ -439,115 +597,126 @@ void max7456DrawScreenPartial(void)
 // when copter is armed.
 void max7456RefreshAll(void)
 {
-    uint8_t spiBuff[(VIDEO_BUFFER_CHARS_PAL + 4) * 2];
+    uint8_t dmm;
-    int bufPtr;
 
     // Check if device is available
-    if (max7456dev == NULL) {
+    if (state.dev == NULL) {
         return;
     }
 
-    if (!max7456Lock) {
+    if (max7456TryLock()) {
-        uint16_t xx;
-        max7456Lock = true;
 
-        // Write characters. Start at character zero.
+        // Issue an OSD clear command
-        bufPtr = 0;
+        busRead(state.dev, MAX7456ADD_DMM | MAX7456ADD_READ, &dmm);
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, 0);
+        busWrite(state.dev, MAX7456ADD_DMM, state.registers.dmm | DMM_CLEAR_DISPLAY);
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, 0);
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, DMM_AUTOINCREMENT);
 
-        for (xx = 0; xx < maxScreenSize; ++xx) {
+        // Wait for clear to complete (20us)
-            bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[xx]));
+        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.
+        // Mark non-blank characters as dirty
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF);
+        BITARRAY_CLR_ALL(screenIsDirty);
-
+        for (unsigned ii = 0; ii < ARRAYLEN(screenBuffer); ii++) {
-        // Write chunk of data
+            if (!CHAR_IS_BLANK(screenBuffer[ii])) {
-        busTransfer(max7456dev, NULL, spiBuff, bufPtr);
+                bitArraySet(screenIsDirty, ii);
-
+            }
-        // 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);
         }
 
-        // Exit auto-increment mode by writing the 0xFF escape sequence to DMDI.
+        // Now perform partial writes until there are no dirty ones
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF);
+        while(max7456DrawScreenPartial());
 
-        // Write chunk of data
+        max7456Unlock();
-        busTransfer(max7456dev, NULL, spiBuff, bufPtr);
-
-        // All characters have been set to the MAX7456, none is dirty now.
-        memset(screenIsDirty, 0, sizeof(screenIsDirty));
-        max7456Lock = false;
     }
 }
 
-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];
-    int bufPtr = 0;
-
     // Check if device is available
-    if (max7456dev == NULL) {
+    if (state.dev == NULL) {
         return;
     }
 
-    while (max7456Lock);
+    max7456Lock();
-    max7456Lock = true;
+    // OSD must be disabled to read or write to NVM
+    bool enabled = max7456OSDSetEnabled(false);
 
-    // disable display
+    busWrite(state.dev, MAX7456ADD_CMAH, char_address);
-    fontIsLoading = true;
+    if (char_address > 255) {
+        // AT7456E and AB7456 have 512 characters of NVM.
+        // 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(state.dev, MAX7456ADD_CMAL, addr_h << 6);
+    }
+    busWrite(state.dev, MAX7456ADD_CMM, READ_NVR);
 
-    bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_VM0, 0);
+    max7456WaitUntilNoBusy();
-    bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address); // set start address high
 
-    for (int x = 0; x < 54; x++) {
+    for (unsigned ii = 0; ii < sizeof(chr->data); ii++) {
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAL, x); //set start address low
+        busWrite(state.dev, MAX7456ADD_CMAL, ii);
-        bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMDI, font_data[x]);
+        busRead(state.dev, MAX7456ADD_CMDO, &chr->data[ii]);
+    }
+
+    max7456OSDSetEnabled(enabled);
+    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
+    if (state.dev == NULL) {
+        return;
+    }
+
+    max7456Lock();
+    // OSD must be disabled to read or write to NVM
+    max7456OSDSetEnabled(false);
+
+    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[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[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
     bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMM, WRITE_NVR);
 
-    busTransfer(max7456dev, NULL, spiBuff, bufPtr);
+    busTransfer(state.dev, NULL, spiBuff, bufPtr);
 
-    while (1) {
+    max7456WaitUntilNoBusy();
-        uint8_t status;
 
-        busRead(max7456dev, MAX7456ADD_STAT, &status);
+    /* XXX: Don't call max7456OSDEnable(), it's intentionally ommited.
-        if ((status & STAT_NVR_BUSY) == 0x00) {
+     * If we continue drawing while characters are being uploaded, we
-            break;
+     * get some corrupted characters from time to time. As a workaround,
-        }
+     * we require a reboot after characters have been uploaded to NVM.
+     */
 
-#ifdef LED0_TOGGLE
+    max7456Unlock();
-        LED0_TOGGLE;
-#else
-        LED1_TOGGLE;
-#endif
-    }
-
-    max7456Lock = false;
 }
 
 

src/main/drivers/max7456.h

@@ -41,16 +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;
 
-struct vcdProfile_s;
+void max7456Init(const videoSystem_e videoSystem);
-void    max7456Init(const videoSystem_e videoSystem);
+void max7456Update(void);
-void    max7456DrawScreenPartial(void);
+void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr);
-void    max7456WriteNvm(uint8_t char_address, const uint8_t *font_data);
+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 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);
+void max7456WriteChar(uint8_t x, uint8_t y, uint16_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);
-void    max7456ClearScreen(void);
+void max7456ClearScreen(void);
-void    max7456RefreshAll(void);
+void max7456RefreshAll(void);
 uint8_t* max7456GetScreenBuffer(void);

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

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];
+            max7456Character_t chr;
-            const uint8_t addr = sbufReadU8(src);
+            uint16_t addr;
-            for (int i = 0; i < 54; i++) {
+            if (dataSize >= 56) {
-                font_data[i] = sbufReadU8(src);
+                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

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)
@@ -72,7 +79,7 @@ static int clearScreen(displayPort_t *displayPort)
 static int drawScreen(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
-    max7456DrawScreenPartial();
+    max7456Update();
 
     return 0;
 }
@@ -80,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)
@@ -91,7 +98,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 +106,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);
@@ -142,6 +149,31 @@ static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort
     return attr;
 }
 
+static bool getFontMetadata(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,12 +188,13 @@ static const displayPortVTable_t max7456VTable = {
     .resync = resync,
     .txBytesFree = txBytesFree,
     .supportedTextAttributes = supportedTextAttributes,
+    .getFontMetadata = getFontMetadata,
 };
 
 displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem)
 {
-    displayInit(&max7456DisplayPort, &max7456VTable);
     max7456Init(videoSystem);
+    displayInit(&max7456DisplayPort, &max7456VTable);
     resync(&max7456DisplayPort);
     return &max7456DisplayPort;
 }

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];
 

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);

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;
@@ -1021,7 +1029,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 +1681,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 +1698,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;
@@ -2545,7 +2553,29 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
 
     displayClearScreen(osdDisplayPort);
 
-    uint8_t y = 4;
+    uint8_t y = 1;
+    displayFontMetadata_t metadata;
+    bool fontHasMetadata = displayGetFontMetadata(&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);