diff --git a/Makefile b/Makefile
index c6ccb2b20f..5696fd6bf6 100644
--- a/Makefile
+++ b/Makefile
@@ -485,6 +485,7 @@ COMMON_SRC = \
             drivers/bus_i2c_soft.c \
             drivers/bus_spi.c \
             drivers/bus_spi_soft.c \
+            drivers/display.c \
             drivers/exti.c \
             drivers/gyro_sync.c \
             drivers/io.c \
diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
new file mode 100644
index 0000000000..2587abb702
--- /dev/null
+++ b/src/main/drivers/display.c
@@ -0,0 +1,68 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "common/utils.h"
+
+#include "display.h"
+
+// XXX Why is this here? Something wrong?
+// XXX Something like Drawing Context that holds all state variables would be the way...
+int8_t lastCursorPos;
+
+void displayClear(displayPort_t *instance)
+{
+    instance->vTable->clear(instance);
+    instance->cleared = true;
+    instance->lastCursorPos = -1;
+}
+
+void displayBegin(displayPort_t *instance)
+{
+    instance->vTable->begin(instance);
+    instance->vTable->clear(instance);
+}
+
+void displayEnd(displayPort_t *instance)
+{
+    instance->vTable->end(instance);
+}
+
+int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s)
+{
+    return instance->vTable->write(instance, x, y, s);
+}
+
+void displayHeartbeat(displayPort_t *instance)
+{
+    instance->vTable->heartbeat(instance);
+}
+
+void displayResync(displayPort_t *instance)
+{
+    instance->vTable->resync(instance);
+}
+
+uint16_t displayTxBytesFree(displayPort_t *instance)
+{
+    return instance->vTable->txBytesFree(instance);
+}
+
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
new file mode 100644
index 0000000000..2f6f261659
--- /dev/null
+++ b/src/main/drivers/display.h
@@ -0,0 +1,49 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+struct displayPortVTable_s;
+typedef struct displayPort_s {
+    const struct displayPortVTable_s *vTable;
+    uint8_t rows;
+    uint8_t cols;
+    uint16_t buftime;
+    uint16_t bufsize;
+
+    // CMS state
+    bool cleared;
+    int8_t lastCursorPos;
+} displayPort_t;
+
+typedef struct displayPortVTable_s {
+    int (*begin)(displayPort_t *displayPort);
+    int (*end)(displayPort_t *displayPort);
+    int (*clear)(displayPort_t *displayPort);
+    int (*write)(displayPort_t *displayPort, uint8_t col, uint8_t row, char *text);
+    int (*heartbeat)(displayPort_t *displayPort);
+    void (*resync)(displayPort_t *displayPort);
+    uint32_t (*txBytesFree)(displayPort_t *displayPort);
+} displayPortVTable_t;
+
+void displayClear(displayPort_t *instance);
+void displayBegin(displayPort_t *instance);
+void displayEnd(displayPort_t *instance);
+int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s);
+void displayHeartbeat(displayPort_t *instance);
+void displayResync(displayPort_t *instance);
+uint16_t displayTxBytesFree(displayPort_t *instance);
diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c
index f2d0eeed15..bc76e09e4c 100644
--- a/src/main/io/canvas.c
+++ b/src/main/io/canvas.c
@@ -5,10 +5,10 @@
 
 #include "platform.h"
 
-#include "build/version.h"
-
 #ifdef CANVAS
 
+#include "build/version.h"
+
 #include "common/utils.h"
 
 #include "drivers/system.h"
@@ -16,46 +16,48 @@
 #include "io/cms.h"
 
 #include "fc/fc_msp.h"
+
 #include "msp/msp_protocol.h"
 #include "msp/msp_serial.h"
 
-int canvasOutput(uint8_t cmd, uint8_t *buf, int len)
+static int canvasOutput(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len)
 {
+    UNUSED(displayPort);
     mspSerialPush(cmd, buf, len);
 
     return 6 + len;
 }
 
-int canvasBegin(void)
+static int canvasBegin(displayPort_t *displayPort)
 {
     uint8_t subcmd[] = { 0 };
 
-    return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd));
+    return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd));
 }
 
-int canvasHeartBeat(void)
+static int canvasHeartBeat(displayPort_t *displayPort)
 {
-    return canvasBegin();
+    return canvasBegin(displayPort);
 }
 
-int canvasEnd(void)
+static int canvasEnd(displayPort_t *displayPort)
 {
     uint8_t subcmd[] = { 1 };
 
-    return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd));
+    return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd));
 }
 
-int canvasClear(void)
+static int canvasClear(displayPort_t *displayPort)
 {
     uint8_t subcmd[] = { 2 };
 
-    return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd));
+    return canvasOutput(displayPort, MSP_CANVAS, subcmd, sizeof(subcmd));
 }
 
-int canvasWrite(uint8_t col, uint8_t row, char *string)
+static int canvasWrite(displayPort_t *displayPort, uint8_t col, uint8_t row, char *string)
 {
     int len;
-    char buf[30 + 4];
+    uint8_t buf[30 + 4];
 
     if ((len = strlen(string)) >= 30)
         len = 30;
@@ -64,23 +66,24 @@ int canvasWrite(uint8_t col, uint8_t row, char *string)
     buf[1] = row;
     buf[2] = col;
     buf[3] = 0;
-    memcpy((char *)&buf[4], string, len);
+    memcpy(&buf[4], string, len);
 
-    return canvasOutput(MSP_CANVAS, (uint8_t *)buf, len + 4);
+    return canvasOutput(displayPort, MSP_CANVAS, buf, len + 4);
 }
 
-void canvasResync(displayPort_t *pPort)
+static void canvasResync(displayPort_t *displayPort)
 {
-    pPort->rows = 13; // XXX Will reflect NTSC/PAL in the future
-    pPort->rows = 30;
+    displayPort->rows = 13; // XXX Will reflect NTSC/PAL in the future
+    displayPort->rows = 30;
 }
 
-uint32_t canvasTxBytesFree(void)
+static uint32_t canvasTxBytesFree(displayPort_t *displayPort)
 {
+    UNUSED(displayPort);
     return mspSerialTxBytesFree();
 }
 
-displayPortVTable_t canvasVTable = {
+static const displayPortVTable_t canvasVTable = {
     canvasBegin,
     canvasEnd,
     canvasClear,
@@ -90,11 +93,10 @@ displayPortVTable_t canvasVTable = {
     canvasTxBytesFree,
 };
 
-void canvasCmsInit(displayPort_t *pPort)
+void canvasCmsInit(displayPort_t *displayPort)
 {
-    pPort->rows = 13; // XXX Will reflect NTSC/PAL in the future
-    pPort->cols = 30;
-    pPort->vTable = &canvasVTable;
+    displayPort->vTable = &canvasVTable;
+    canvasResync(displayPort);
 }
 
 void canvasInit()
diff --git a/src/main/io/cms.c b/src/main/io/cms.c
index 74fce20a91..2d1b8a6021 100644
--- a/src/main/io/cms.c
+++ b/src/main/io/cms.c
@@ -113,48 +113,6 @@ cmsDeviceInitFuncPtr cmsDeviceSelectNext(void)
 
 #define CMS_UPDATE_INTERVAL 50 // msec
 
-// XXX Why is this here? Something wrong?
-// XXX Something like Drawing Context that holds all state variables would be the way...
-int8_t lastCursorPos;
-
-void cmsScreenClear(displayPort_t *instance)
-{
-    instance->vTable->clear();
-    instance->cleared = true;
-    lastCursorPos = -1; // XXX Here
-}
-
-void cmsScreenBegin(displayPort_t *instance)
-{
-    instance->vTable->begin();
-    instance->vTable->clear();
-}
-
-void cmsScreenEnd(displayPort_t *instance)
-{
-    instance->vTable->end();
-}
-
-int cmsScreenWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s)
-{
-    return instance->vTable->write(x, y, s);
-}
-
-void cmsScreenHeartBeat(displayPort_t *instance)
-{
-    instance->vTable->heartbeat();
-}
-
-void cmsScreenResync(displayPort_t *instance)
-{
-    instance->vTable->resync(instance);
-}
-
-uint16_t cmsScreenTxBytesFree(displayPort_t *instance)
-{
-    return instance->vTable->txBytesFree();
-}
-
 void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc)
 {
     cmsDeviceInitFunc(pDisp);
@@ -277,22 +235,22 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
     switch (p->type) {
     case OME_String:
         if (IS_PRINTVALUE(p) && p->data) {
-            cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data);
+            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data);
             CLR_PRINTVALUE(p);
         }
         break;
     case OME_Submenu:
         if (IS_PRINTVALUE(p))  {
-            cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">");
+            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">");
             CLR_PRINTVALUE(p);
         }
         break;
     case OME_Bool:
         if (IS_PRINTVALUE(p) && p->data) {
             if (*((uint8_t *)(p->data))) {
-                cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
+                cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
             } else {
-                cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
+                cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
             }
             CLR_PRINTVALUE(p);
         }
@@ -300,8 +258,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
     case OME_TAB: {
         if (IS_PRINTVALUE(p)) {
             OSD_TAB_t *ptr = p->data;
-            //cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 5, row, (char *)ptr->names[*ptr->val]);
-            cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, (char *)ptr->names[*ptr->val]);
+            //cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 5, row, (char *)ptr->names[*ptr->val]);
+            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, (char *)ptr->names[*ptr->val]);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -315,9 +273,9 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
             val = (uint16_t *)address;
 
             if (VISIBLE(*val)) {
-                cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
+                cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
             } else {
-                cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
+                cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
             }
             CLR_PRINTVALUE(p);
         }
@@ -328,8 +286,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
             OSD_UINT8_t *ptr = p->data;
             itoa(*ptr->val, buff, 10);
             cmsPad(buff, 5);
-            //cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "     ");
-            cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+            //cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "     ");
+            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -338,7 +296,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
             OSD_INT8_t *ptr = p->data;
             itoa(*ptr->val, buff, 10);
             cmsPad(buff, 5);
-            cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -347,7 +305,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
             OSD_UINT16_t *ptr = p->data;
             itoa(*ptr->val, buff, 10);
             cmsPad(buff, 5);
-            cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -356,7 +314,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
             OSD_UINT16_t *ptr = p->data;
             itoa(*ptr->val, buff, 10);
             cmsPad(buff, 5);
-            cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             CLR_PRINTVALUE(p);
         }
         break;
@@ -365,7 +323,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
             OSD_UINT16_t *ptr = p->data;
             itoa(*ptr->val, buff, 10);
             cmsPad(buff, 5);
-            cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
+            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
             // PRINTVALUE not cleared on purpose
         }
         break;
@@ -374,7 +332,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
             OSD_FLOAT_t *ptr = p->data;
             cmsFormatFloat(*ptr->val * ptr->multipler, buff);
             cmsPad(buff, 5);
-            cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ???
+            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ???
             CLR_PRINTVALUE(p);
         }
         break;
@@ -399,7 +357,7 @@ void cmsDrawMenu(displayPort_t *pDisplay)
     static uint8_t pollDenom = 0;
     bool drawPolled = (++pollDenom % 8 == 0);
 
-    uint32_t room = cmsScreenTxBytesFree(pDisplay);
+    uint32_t room = displayTxBytesFree(pDisplay);
 
     if (!currentMenu)
         return;
@@ -425,16 +383,16 @@ void cmsDrawMenu(displayPort_t *pDisplay)
     while ((currentMenu + currentCursorPos)->type == OME_Label) // skip label
         currentCursorPos++;
 
-    if (lastCursorPos >= 0 && currentCursorPos != lastCursorPos) {
-        room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, lastCursorPos + top, "  ");
+    if (pDisplay->lastCursorPos >= 0 && currentCursorPos != pDisplay->lastCursorPos) {
+        room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->lastCursorPos + top, "  ");
     }
 
     if (room < 30)
         return;
 
-    if (lastCursorPos != currentCursorPos) {
-        room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >");
-        lastCursorPos = currentCursorPos;
+    if (pDisplay->lastCursorPos != currentCursorPos) {
+        room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >");
+        pDisplay->lastCursorPos = currentCursorPos;
     }
 
     if (room < 30)
@@ -443,7 +401,7 @@ void cmsDrawMenu(displayPort_t *pDisplay)
     // Print text labels
     for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
         if (IS_PRINTLABEL(p)) {
-            room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text);
+            room -= displayWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text);
             CLR_PRINTLABEL(p);
             if (room < 30)
                 return;
@@ -483,7 +441,7 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr)
             currentMenu = (OSD_Entry *)ptr;
             currentCursorPos = 0;
         }
-        cmsScreenClear(pDisplay);
+        displayClear(pDisplay);
         cmsUpdateMaxRows(pDisplay);
     }
 
@@ -502,7 +460,7 @@ long cmsMenuBack(displayPort_t *pDisplay)
         cmsx_RateExpoWriteback();
 
     if (menuStackIdx) {
-        cmsScreenClear(pDisplay);
+        displayClear(pDisplay);
         menuStackIdx--;
         nextPage = NULL;
         currentMenu = menuStack[menuStackIdx];
@@ -527,7 +485,7 @@ void cmsMenuOpen(void)
         currentMenu = &menuMain[0];
     } else {
         // Switch display
-        cmsScreenEnd(&currentDisplay);
+        displayEnd(&currentDisplay);
         initfunc = cmsDeviceSelectNext();
     }
 
@@ -535,17 +493,17 @@ void cmsMenuOpen(void)
         return;
 
     cmsScreenInit(&currentDisplay, initfunc);
-    cmsScreenBegin(&currentDisplay);
+    displayBegin(&currentDisplay);
     cmsMenuChange(&currentDisplay, currentMenu);
 }
 
 long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
 {
     if (ptr) {
-        cmsScreenClear(pDisplay);
+        displayClear(pDisplay);
 
-        cmsScreenWrite(pDisplay, 5, 3, "REBOOTING...");
-        cmsScreenResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
+        displayWrite(pDisplay, 5, 3, "REBOOTING...");
+        displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
 
         stopMotors();
         stopPwmAllMotors();
@@ -557,7 +515,7 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
 
     cmsInMenu = false;
 
-    cmsScreenEnd(pDisplay);
+    displayEnd(pDisplay);
     currentMenu = NULL;
 
     if (ptr)
@@ -585,9 +543,8 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
         if (currentCursorPos < currentMenuIdx) {
             currentCursorPos++;
         } else {
-            if (nextPage) // we have more pages
-            {
-                cmsScreenClear(pDisplay);
+            if (nextPage) { // we have more pages
+                displayClear(pDisplay);
                 p = nextPage;
                 nextPage = currentMenu;
                 currentMenu = (OSD_Entry *)p;
@@ -607,7 +564,7 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
 
         if (currentCursorPos == -1 || (currentMenu + currentCursorPos)->type == OME_Label) {
             if (nextPage) {
-                cmsScreenClear(pDisplay);
+                displayClear(pDisplay);
                 p = nextPage;
                 nextPage = currentMenu;
                 currentMenu = (OSD_Entry *)p;
@@ -803,7 +760,7 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
         if (currentTime > lastCmsHeartBeat + 500) {
             // Heart beat for external CMS display device @ 500msec
             // (Timeout @ 1000msec)
-            cmsScreenHeartBeat(&currentDisplay);
+            displayHeartbeat(&currentDisplay);
             lastCmsHeartBeat = currentTime;
         }
     }
diff --git a/src/main/io/cms.h b/src/main/io/cms.h
index a7d0d21e08..5e3be53e80 100644
--- a/src/main/io/cms.h
+++ b/src/main/io/cms.h
@@ -1,27 +1,6 @@
 #pragma once
 
-struct displayPort_s;
-
-typedef struct displayPortVTable_s {
-    int (*begin)(void);
-    int (*end)(void);
-    int (*clear)(void);
-    int (*write)(uint8_t col, uint8_t row, char *text);
-    int (*heartbeat)(void);
-    void (*resync)(struct displayPort_s *pPort);
-    uint32_t (*txBytesFree)(void);
-} displayPortVTable_t;
-
-typedef struct displayPort_s {
-    displayPortVTable_t *vTable;
-    uint8_t rows;
-    uint8_t cols;
-    uint16_t buftime;
-    uint16_t bufsize;
-
-    // CMS state
-    bool cleared;
-} displayPort_t;
+#include "drivers/display.h"
 
 // Device management
 typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *pPort);
@@ -31,11 +10,6 @@ bool cmsDeviceRegister(cmsDeviceInitFuncPtr);
 void cmsInit(void);
 void cmsHandler(uint32_t currentTime);
 
-// Required for external CMS tables
-void cmsScreenClear(displayPort_t *pPort);
-void cmsScreenResync(displayPort_t *pPort);
-int cmsScreenWrite(displayPort_t *pPort, uint8_t x, uint8_t y, char *s);
-
 long cmsMenuChange(displayPort_t *pPort, void *ptr);
 long cmsMenuExit(displayPort_t *pPort, void *ptr);
 
diff --git a/src/main/io/cms_blackbox.c b/src/main/io/cms_blackbox.c
index 3f118f87ee..696b1231c4 100644
--- a/src/main/io/cms_blackbox.c
+++ b/src/main/io/cms_blackbox.c
@@ -30,17 +30,17 @@ long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr)
 {
     UNUSED(ptr);
 
-    cmsScreenClear(pDisplay);
-    cmsScreenWrite(pDisplay, 5, 3, "ERASING FLASH...");
-    cmsScreenResync(pDisplay); // Was max7456RefreshAll(); Why at this timing?
+    displayClear(pDisplay);
+    displayWrite(pDisplay, 5, 3, "ERASING FLASH...");
+    displayResync(pDisplay); // Was max7456RefreshAll(); Why at this timing?
 
     flashfsEraseCompletely();
     while (!flashfsIsReady()) {
         delay(100);
     }
 
-    cmsScreenClear(pDisplay);
-    cmsScreenResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI?
+    displayClear(pDisplay);
+    displayResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI?
 
     return 0;
 }
diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index c63b6d90c0..7fc7ab9a30 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -751,68 +751,74 @@ void dashboardDisablePageCycling(void)
 }
 
 #ifdef OLEDCMS
-#include "io/cms.h"
+#include "drivers/display.h"
 
-int dashboardCmsBegin(void)
+static int dashboardBegin(displayPort_t *displayPort)
 {
+    UNUSED(displayPort);
     dashboardInCMS = true;
 
     return 0;
 }
 
-int dashboardCmsEnd(void)
+static int dashboardEnd(displayPort_t *displayPort)
 {
+    UNUSED(displayPort);
     dashboardInCMS = false;
 
     return 0;
 }
 
-int dashboardCmsClear(void)
+static int dashboardClear(displayPort_t *displayPort)
 {
+    UNUSED(displayPort);
     i2c_OLED_clear_display_quick();
 
     return 0;
 }
 
-int dashboardCmsWrite(uint8_t x, uint8_t y, char *s)
+static int dashboardWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s)
 {
+    UNUSED(displayPort);
     i2c_OLED_set_xy(x, y);
     i2c_OLED_send_string(s);
 
     return 0;
 }
 
-int dashboardCmsHeartbeat(void)
+static int dashboardHeartbeat(displayPort_t *displayPort)
 {
+    UNUSED(displayPort);
     return 0;
 }
 
-void dashboardCmsResync(displayPort_t *pPort)
+static void dashboardResync(displayPort_t *displayPort)
 {
-    UNUSED(pPort);
+    UNUSED(displayPort);
 }
 
-uint32_t dashboardCmsTxBytesFree(void)
+static uint32_t dashboardTxBytesFree(displayPort_t *displayPort)
 {
+    UNUSED(displayPort);
     return UINT32_MAX;
 }
 
-displayPortVTable_t dashboardCmsVTable = {
-    dashboardCmsBegin,
-    dashboardCmsEnd,
-    dashboardCmsClear,
-    dashboardCmsWrite,
-    dashboardCmsHeartbeat,
-    dashboardCmsResync,
-    dashboardCmsTxBytesFree,
+static const displayPortVTable_t dashboardVTable = {
+    dashboardBegin,
+    dashboardEnd,
+    dashboardClear,
+    dashboardWrite,
+    dashboardHeartbeat,
+    dashboardResync,
+    dashboardTxBytesFree,
 };
 
-void dashboardCmsInit(displayPort_t *pPort)
+void dashboardCmsInit(displayPort_t *displayPort)
 {
-    pPort->rows = SCREEN_CHARACTER_ROW_COUNT;
-    pPort->cols = SCREEN_CHARACTER_COLUMN_COUNT;
-    pPort->vTable = &dashboardCmsVTable;
+    displayPort->rows = SCREEN_CHARACTER_ROW_COUNT;
+    displayPort->cols = SCREEN_CHARACTER_COLUMN_COUNT;
+    displayPort->vTable = &dashboardVTable;
 }
 #endif // OLEDCMS
 
-#endif
+#endif // USE_DASHBOARD
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index c271d4339f..f1b3871e70 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -39,16 +39,15 @@
 
 #include "io/cms.h"
 #include "io/cms_types.h"
-
+#include "io/cms_osd.h"
 #include "io/flashfs.h"
 #include "io/osd.h"
+#include "io/osd_max7456.h"
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-//#include "flight/pid.h"
-
 #include "config/config_profile.h"
 #include "config/config_master.h"
 #include "config/feature.h"
@@ -401,7 +400,7 @@ void osdInit(void)
     refreshTimeout = 4 * REFRESH_1S;
 
 #ifdef CMS
-    cmsDeviceRegister(osdCmsInit);
+    cmsDeviceRegister(osdMax7456Init);
 #endif
 }
 
@@ -633,60 +632,6 @@ void osdUpdate(uint32_t currentTime)
     }
 }
 
-//
-// OSD specific CMS functions
-//
-#include "io/cms_osd.h"
-
-uint8_t shiftdown;
-
-int osdMenuBegin(void)
-{
-    osdResetAlarms();
-    osdInMenu = true;
-    refreshTimeout = 0;
-
-    return 0;
-}
-
-int osdMenuEnd(void)
-{
-    osdInMenu = false;
-
-    return 0;
-}
-
-int osdClearScreen(void)
-{
-    max7456ClearScreen();
-
-    return 0;
-}
-
-int osdWrite(uint8_t x, uint8_t y, char *s)
-{
-    max7456Write(x, y + shiftdown, s);
-
-    return 0;
-}
-
-void osdResync(displayPort_t *pPort)
-{
-    max7456RefreshAll();
-    pPort->rows = max7456GetRowsCount() - masterConfig.osdProfile.row_shiftdown;
-    pPort->cols = 30;
-}
-
-int osdHeartbeat(void)
-{
-    return 0;
-}
-
-uint32_t osdTxBytesFree(void)
-{
-    return UINT32_MAX;
-}
-
 #ifdef EDIT_ELEMENT_SUPPORT
 void osdEditElement(void *ptr)
 {
@@ -713,22 +658,6 @@ void osdDrawElementPositioningHelp(void)
 }
 #endif
 
-displayPortVTable_t osdVTable = {
-    osdMenuBegin,
-    osdMenuEnd,
-    osdClearScreen,
-    osdWrite,
-    osdHeartbeat,
-    osdResync,
-    osdTxBytesFree,
-};
-
-void osdCmsInit(displayPort_t *pPort)
-{
-    osdResync(pPort);
-    pPort->vTable = &osdVTable;
-}
-
 OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5};
 OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50};
 OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1};
diff --git a/src/main/io/osd_max7456.c b/src/main/io/osd_max7456.c
new file mode 100644
index 0000000000..7471910c6e
--- /dev/null
+++ b/src/main/io/osd_max7456.c
@@ -0,0 +1,90 @@
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef OSD
+
+#include "common/utils.h"
+
+#include "config/config_master.h"
+
+#include "drivers/display.h"
+#include "drivers/max7456.h"
+
+extern bool osdInMenu;
+extern uint16_t refreshTimeout;
+void osdResetAlarms(void);
+
+uint8_t shiftdown;
+
+static int osdMenuBegin(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    osdResetAlarms();
+    osdInMenu = true;
+    refreshTimeout = 0;
+
+    return 0;
+}
+
+static int osdMenuEnd(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    osdInMenu = false;
+
+    return 0;
+}
+
+static int osdClearScreen(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    max7456ClearScreen();
+
+    return 0;
+}
+
+static int osdWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s)
+{
+    UNUSED(displayPort);
+    max7456Write(x, y + shiftdown, s);
+
+    return 0;
+}
+
+static void osdResync(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    max7456RefreshAll();
+    displayPort->rows = max7456GetRowsCount() - masterConfig.osdProfile.row_shiftdown;
+    displayPort->cols = 30;
+}
+
+static int osdHeartbeat(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static uint32_t osdTxBytesFree(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return UINT32_MAX;
+}
+
+displayPortVTable_t osdVTable = {
+    osdMenuBegin,
+    osdMenuEnd,
+    osdClearScreen,
+    osdWrite,
+    osdHeartbeat,
+    osdResync,
+    osdTxBytesFree,
+};
+
+void osdMax7456Init(displayPort_t *displayPort)
+{
+    displayPort->vTable = &osdVTable;
+    osdResync(displayPort);
+}
+#endif // OSD
diff --git a/src/main/io/osd_max7456.h b/src/main/io/osd_max7456.h
new file mode 100644
index 0000000000..26d696f304
--- /dev/null
+++ b/src/main/io/osd_max7456.h
@@ -0,0 +1,3 @@
+#pragma once
+
+void osdMax7456Init(displayPort_t *displayPort);
diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk
index 5a7e1a2382..e30c6530a8 100644
--- a/src/main/target/OMNIBUS/target.mk
+++ b/src/main/target/OMNIBUS/target.mk
@@ -15,6 +15,7 @@ TARGET_SRC = \
             io/transponder_ir.c \
             drivers/max7456.c \
             io/osd.c \
+            io/osd_max7456.c \
             io/canvas.c \
             io/cms.c \
             io/cms_imu.c \
diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk
index c7cf0ad933..5cefb51d21 100644
--- a/src/main/target/OMNIBUSF4/target.mk
+++ b/src/main/target/OMNIBUSF4/target.mk
@@ -7,6 +7,10 @@ TARGET_SRC = \
             drivers/compass_hmc5883l.c \
             drivers/max7456.c \
             io/osd.c \
+            io/osd_max7456.c \
             io/cms.c \
+            io/cms_imu.c \
+            io/cms_blackbox.c \
+            io/cms_ledstrip.c \
             io/canvas.c