diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index 9b3810c41..753afacf7 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -29,18 +29,19 @@ void displayClear(displayPort_t *instance)
     instance->vTable->clear(instance);
     instance->cleared = true;
     instance->cursorRow = -1;
-    instance->inCMS = false;
 }
 
 void displayOpen(displayPort_t *instance)
 {
     instance->vTable->open(instance);
     instance->vTable->clear(instance);
+    instance->inCMS = true;
 }
 
 void displayClose(displayPort_t *instance)
 {
     instance->vTable->close(instance);
+    instance->inCMS = false;
 }
 
 int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s)
diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c
index bc76e09e4..f805c862c 100644
--- a/src/main/io/canvas.c
+++ b/src/main/io/canvas.c
@@ -20,6 +20,8 @@
 #include "msp/msp_protocol.h"
 #include "msp/msp_serial.h"
 
+static displayPort_t canvasDisplayPort;
+
 static int canvasOutput(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len)
 {
     UNUSED(displayPort);
@@ -74,7 +76,7 @@ static int canvasWrite(displayPort_t *displayPort, uint8_t col, uint8_t row, cha
 static void canvasResync(displayPort_t *displayPort)
 {
     displayPort->rows = 13; // XXX Will reflect NTSC/PAL in the future
-    displayPort->rows = 30;
+    displayPort->cols = 30;
 }
 
 static uint32_t canvasTxBytesFree(displayPort_t *displayPort)
@@ -93,14 +95,11 @@ static const displayPortVTable_t canvasVTable = {
     canvasTxBytesFree,
 };
 
-void canvasCmsInit(displayPort_t *displayPort)
-{
-    displayPort->vTable = &canvasVTable;
-    canvasResync(displayPort);
-}
-
 void canvasInit()
 {
-    cmsDeviceRegister(canvasCmsInit);
+    canvasDisplayPort.vTable = &canvasVTable;
+    canvasDisplayPort.inCMS = false;
+    canvasResync(&canvasDisplayPort);
+    cmsDisplayPortRegister(&canvasDisplayPort);
 }
 #endif
diff --git a/src/main/io/cms.c b/src/main/io/cms.c
index 21e27ae39..4fe5e5925 100644
--- a/src/main/io/cms.c
+++ b/src/main/io/cms.c
@@ -16,10 +16,12 @@
  */
 
 /*
- Created by Marcin Baliniak
+ Original OSD code created by Marcin Baliniak
  OSD-CMS separation by jflyper
+ CMS-displayPort separation by jflyper and martinbudden
  */
-#define CMS_MENU_DEBUG // For external menu content creators
+
+//#define CMS_MENU_DEBUG // For external menu content creators
 
 #include <stdbool.h>
 #include <stdint.h>
@@ -32,74 +34,54 @@
 
 #ifdef CMS
 
+#include "build/debug.h"
+
 #include "drivers/system.h"
 
 #include "common/typeconversion.h"
 
-#include "io/cms.h"
-#include "io/cms_types.h"
-
-#ifdef CANVAS
-#include "io/canvas.h"
-#endif
-
-#ifdef USE_FLASHFS
-#include "io/flashfs.h"
-#endif
-
-#ifdef OSD
-#include "io/osd.h"
-#endif
-
-#ifdef USE_DASHBOARD
-#include "io/dashboard.h"
-#endif
-
+// For 'ARM' related
 #include "fc/config.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "flight/pid.h"
-
+// For rcData, stopAllMotors, stopPwmAllMotors
 #include "config/config_profile.h"
 #include "config/config_master.h"
 #include "config/feature.h"
 
-#include "build/debug.h"
+// For VISIBLE* (Actually, included by config_master.h)
+#include "io/osd.h"
 
-// External menu contents
-#include "io/cms_imu.h"
-#include "io/cms_blackbox.h"
-#include "io/cms_vtx.h"
-#ifdef OSD
-#include "io/cms_osd.h"
-#endif
-#include "io/cms_ledstrip.h"
+#include "io/cms.h"
+#include "io/cms_types.h"
 
-// Forwards
-static long cmsx_InfoInit(void);
+// Menu contents
+#include "io/cms_builtin.h"
 
-// Device management
+// DisplayPort management
 
 #ifndef CMS_MAX_DEVICE
 #define CMS_MAX_DEVICE 4
 #endif
 
-static cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE];
+static displayPort_t *pCurrentDisplay;
+
+static displayPort_t *cmsDisplayPorts[CMS_MAX_DEVICE];
 static int cmsDeviceCount;
 static int cmsCurrentDevice = -1;
 
-bool cmsDeviceRegister(cmsDeviceInitFuncPtr func)
+bool cmsDisplayPortRegister(displayPort_t *pDisplay)
 {
     if (cmsDeviceCount == CMS_MAX_DEVICE)
         return false;
 
-    cmsDeviceInitFunc[cmsDeviceCount++] = func;
+    cmsDisplayPorts[cmsDeviceCount++] = pDisplay;
 
     return true;
 }
 
-static cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void)
+static displayPort_t *cmsDisplayPortSelectCurrent(void)
 {
     if (cmsDeviceCount == 0)
         return NULL;
@@ -107,28 +89,22 @@ static cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void)
     if (cmsCurrentDevice < 0)
         cmsCurrentDevice = 0;
 
-    return cmsDeviceInitFunc[cmsCurrentDevice];
+    return cmsDisplayPorts[cmsCurrentDevice];
 }
 
-static cmsDeviceInitFuncPtr cmsDeviceSelectNext(void)
+static displayPort_t *cmsDisplayPortSelectNext(void)
 {
     if (cmsDeviceCount == 0)
         return NULL;
 
     cmsCurrentDevice = (cmsCurrentDevice + 1) % cmsDeviceCount; // -1 Okay
 
-    return cmsDeviceInitFunc[cmsCurrentDevice];
+    return cmsDisplayPorts[cmsCurrentDevice];
 }
 
 #define CMS_UPDATE_INTERVAL  50   // Interval of key scans (msec)
 #define CMS_POLL_INTERVAL   100   // Interval of polling dynamic values (msec)
 
-static void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc)
-{
-    cmsDeviceInitFunc(pDisp);
-}
-
-
 // XXX LEFT_MENU_COLUMN and RIGHT_MENU_COLUMN must be adjusted
 // dynamically depending on size of the active output device,
 // or statically to accomodate sizes of all supported devices.
@@ -149,11 +125,8 @@ static void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceIn
 #define RIGHT_MENU_COLUMN(p) ((p)->cols - 8)
 #define MAX_MENU_ITEMS(p)    ((p)->rows - 2)
 
-static displayPort_t currentDisplay;
-
 static bool cmsInMenu = false;
 
-static CMS_Menu menuMain;
 static CMS_Menu *currentMenu;    // Points to top entry of the current page
 
 // XXX Does menu backing support backing into second page???
@@ -168,7 +141,7 @@ static uint8_t maxRow;           // Max row in the current page
 
 static int8_t cursorRow;
 
-// Broken menu substitution
+#ifdef CMS_MENU_DEBUG // For external menu content creators
 
 static char menuErrLabel[21 + 1] = "RAMDOM DATA";
 
@@ -187,6 +160,7 @@ static CMS_Menu menuErr = {
     NULL,
     menuErrEntries,
 };
+#endif
 
 // Stick/key detection
 
@@ -500,6 +474,7 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr)
         }
 
         pageTop = currentMenu->entries;
+        pageTopAlt = NULL;
 
         displayClear(pDisplay);
         cmsUpdateMaxRow(pDisplay);
@@ -541,26 +516,26 @@ static long cmsMenuBack(displayPort_t *pDisplay)
 
 static void cmsMenuOpen(void)
 {
-    cmsDeviceInitFuncPtr initfunc;
-
     if (!cmsInMenu) {
         // New open
+        pCurrentDisplay = cmsDisplayPortSelectCurrent(); 
+        if (!pCurrentDisplay)
+            return;
         cmsInMenu = true;
-        DISABLE_ARMING_FLAG(OK_TO_ARM);
-        initfunc = cmsDeviceSelectCurrent(); 
         currentMenu = &menuMain;
+        DISABLE_ARMING_FLAG(OK_TO_ARM);
     } else {
         // Switch display
-        displayClose(&currentDisplay);
-        initfunc = cmsDeviceSelectNext();
+        displayPort_t *pNextDisplay = cmsDisplayPortSelectNext();
+        if (pNextDisplay != pCurrentDisplay) {
+            displayClose(pCurrentDisplay);
+            pCurrentDisplay = pNextDisplay;
+        } else {
+            return;
+        }
     }
-
-    if (!initfunc)
-        return;
-
-    cmsScreenInit(&currentDisplay, initfunc);
-    displayOpen(&currentDisplay);
-    cmsMenuChange(&currentDisplay, currentMenu);
+    displayOpen(pCurrentDisplay);
+    cmsMenuChange(pCurrentDisplay, currentMenu);
 }
 
 static void cmsTraverseGlobalExit(CMS_Menu *pMenu)
@@ -575,7 +550,7 @@ static void cmsTraverseGlobalExit(CMS_Menu *pMenu)
         pMenu->onGlobalExit();
 }
 
-static long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
+long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
 {
     if (ptr) {
         displayClear(pDisplay);
@@ -829,7 +804,7 @@ static void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
         //lastCalled = currentTime;
 
         if (key) {
-            rcDelay = cmsHandleKey(&currentDisplay, key);
+            rcDelay = cmsHandleKey(pCurrentDisplay, key);
             return;
         }
 
@@ -838,7 +813,7 @@ static void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
         if (currentTime > lastCmsHeartBeat + 500) {
             // Heart beat for external CMS display device @ 500msec
             // (Timeout @ 1000msec)
-            displayHeartbeat(&currentDisplay);
+            displayHeartbeat(pCurrentDisplay);
             lastCmsHeartBeat = currentTime;
         }
     }
@@ -854,107 +829,15 @@ void cmsHandler(uint32_t currentTime)
     const uint32_t now = currentTime / 1000;
 
     if (now - lastCalled >= CMS_UPDATE_INTERVAL) {
-        cmsUpdate(&currentDisplay, now);
+        cmsUpdate(pCurrentDisplay, now);
         lastCalled = now;
     }
 }
 
+// Will initializing with menuMain be better?
+// Can it be done with the current main()?
 void cmsInit(void)
 {
 }
 
-//
-// Built-in menu contents and support functions
-//
-
-// Info
-
-static char infoGitRev[GIT_SHORT_REVISION_LENGTH];
-static char infoTargetName[] = __TARGET__;
-
-#include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere
-
-static OSD_Entry menuInfoEntries[] = {
-    { "--- INFO ---", OME_Label, NULL, NULL, 0 },
-    { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 },
-    { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 },
-    { "GITREV", OME_String, NULL, infoGitRev, 0 },
-    { "TARGET", OME_String, NULL, infoTargetName, 0 },
-    { "BACK", OME_Back, NULL, NULL, 0 },
-    { NULL, OME_END, NULL, NULL, 0 }
-};
-
-static CMS_Menu menuInfo = {
-    "MENUINFO",
-    OME_MENU,
-    cmsx_InfoInit,
-    NULL,
-    NULL,
-    menuInfoEntries,
-};
-
-static long cmsx_InfoInit(void)
-{
-    for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
-        if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f')
-            infoGitRev[i] = shortGitRevision[i] - 'a' + 'A';
-        else
-            infoGitRev[i] = shortGitRevision[i];
-    }
-
-    return 0;
-}
-
-// Features
-
-static OSD_Entry menuFeaturesEntries[] =
-{
-    {"--- FEATURES ---", OME_Label, NULL, NULL, 0},
-    {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0},
-#if defined(VTX) || defined(USE_RTC6705)
-    {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0},
-#endif // VTX || USE_RTC6705
-#ifdef LED_STRIP
-    {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0},
-#endif // LED_STRIP
-    {"BACK", OME_Back, NULL, NULL, 0},
-    {NULL, OME_END, NULL, NULL, 0}
-};
-
-static CMS_Menu menuFeatures = {
-    "MENUFEATURES",
-    OME_MENU,
-    NULL,
-    NULL,
-    NULL,
-    menuFeaturesEntries,
-};
-
-// Main
-
-static OSD_Entry menuMainEntries[] =
-{
-    {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0},
-    {"CFG&IMU", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0},
-    {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0},
-#ifdef OSD
-    {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0},
-    {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0},
-#endif
-    {"ERR DEMO", OME_Submenu, cmsMenuChange, &menuErr, 0},
-    {"REAL ERR", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0},
-    {"FC&FW INFO2", OME_Submenu, cmsMenuChange, &menuInfo, 0},
-    {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0},
-    {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0},
-    {NULL,OME_END, NULL, NULL, 0}
-};
-
-static CMS_Menu menuMain = {
-    "MENUMAIN",
-    OME_MENU,
-    NULL,
-    NULL,
-    NULL,
-    menuMainEntries,
-};
 #endif // CMS
diff --git a/src/main/io/cms.h b/src/main/io/cms.h
index e1a1c3be6..ff3fa46bf 100644
--- a/src/main/io/cms.h
+++ b/src/main/io/cms.h
@@ -3,15 +3,14 @@
 #include "drivers/display.h"
 
 // Device management
-typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *pPort);
-bool cmsDeviceRegister(cmsDeviceInitFuncPtr);
+bool cmsDisplayPortRegister(displayPort_t *pDisplay);
 
 // For main.c and scheduler
 void cmsInit(void);
 void cmsHandler(uint32_t currentTime);
 
 long cmsMenuChange(displayPort_t *pPort, void *ptr);
-//long cmsMenuExit(displayPort_t *pPort, void *ptr);
+long cmsMenuExit(displayPort_t *pPort, void *ptr);
 
 #define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID"
 #define CMS_STARTUP_HELP_TEXT2     "+ YAW LEFT"
diff --git a/src/main/io/cms_builtin.c b/src/main/io/cms_builtin.c
new file mode 100644
index 000000000..518c283fb
--- /dev/null
+++ b/src/main/io/cms_builtin.c
@@ -0,0 +1,125 @@
+//
+// Built-in menu contents and support functions
+//
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <ctype.h>
+
+#include "platform.h"
+
+#include "build/version.h"
+
+#ifdef CMS
+
+#include "drivers/system.h"
+
+#include "io/cms.h"
+#include "io/cms_types.h"
+#include "io/cms_imu.h"
+
+// Sub menus
+
+#include "io/cms_imu.h"
+#include "io/cms_blackbox.h"
+#include "io/cms_vtx.h"
+#ifdef OSD
+#include "io/cms_osd.h"
+#endif
+#include "io/cms_ledstrip.h"
+
+
+// Info
+
+static char infoGitRev[GIT_SHORT_REVISION_LENGTH];
+static char infoTargetName[] = __TARGET__;
+
+#include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere
+
+static long cmsx_InfoInit(void)
+{
+    for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
+        if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f')
+            infoGitRev[i] = shortGitRevision[i] - 'a' + 'A';
+        else
+            infoGitRev[i] = shortGitRevision[i];
+    }
+
+    return 0;
+}
+
+static OSD_Entry menuInfoEntries[] = {
+    { "--- INFO ---", OME_Label, NULL, NULL, 0 },
+    { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 },
+    { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 },
+    { "GITREV", OME_String, NULL, infoGitRev, 0 },
+    { "TARGET", OME_String, NULL, infoTargetName, 0 },
+    { "BACK", OME_Back, NULL, NULL, 0 },
+    { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu menuInfo = {
+    "MENUINFO",
+    OME_MENU,
+    cmsx_InfoInit,
+    NULL,
+    NULL,
+    menuInfoEntries,
+};
+
+// Features
+
+static OSD_Entry menuFeaturesEntries[] =
+{
+    {"--- FEATURES ---", OME_Label, NULL, NULL, 0},
+    {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0},
+#if defined(VTX) || defined(USE_RTC6705)
+    {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0},
+#endif // VTX || USE_RTC6705
+#ifdef LED_STRIP
+    {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0},
+#endif // LED_STRIP
+    {"BACK", OME_Back, NULL, NULL, 0},
+    {NULL, OME_END, NULL, NULL, 0}
+};
+
+static CMS_Menu menuFeatures = {
+    "MENUFEATURES",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    menuFeaturesEntries,
+};
+
+// Main
+
+static OSD_Entry menuMainEntries[] =
+{
+    {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0},
+    {"CFG&IMU", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0},
+    {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0},
+#ifdef OSD
+    {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0},
+    {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0},
+#endif
+    {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0},
+    {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0},
+    {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0},
+#ifdef CMS_MENU_DEBUG
+    {"ERR SAMPLE", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0},
+#endif
+    {NULL,OME_END, NULL, NULL, 0}
+};
+
+CMS_Menu menuMain = {
+    "MENUMAIN",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    menuMainEntries,
+};
+
+#endif
diff --git a/src/main/io/cms_builtin.h b/src/main/io/cms_builtin.h
new file mode 100644
index 000000000..f5ef1e0fc
--- /dev/null
+++ b/src/main/io/cms_builtin.h
@@ -0,0 +1 @@
+extern CMS_Menu menuMain;
diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index b12b45126..5800f216f 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -56,9 +56,6 @@
 #include "io/cms.h"
 #include "io/displayport_oled.h"
 
-displayPort_t *displayPort;
-bool dashboardInCMS = false; // temporary
-
 #ifdef GPS
 #include "io/gps.h"
 #include "flight/navigation.h"
@@ -591,8 +588,7 @@ void dashboardUpdate(uint32_t currentTime)
     static uint8_t previousArmedState = 0;
 
 #ifdef OLEDCMS
-    if (dashboardInCMS) return;
-    if (displayPort && displayPort->inCMS) {
+    if (oledDisplayPort.inCMS) {
         return;
     }
 #endif
@@ -702,12 +698,6 @@ void dashboardSetPage(pageId_e pageId)
     pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 }
 
-void dashboardCmsInit(displayPort_t *displayPortToUse)
-{
-    displayPort = displayPortToUse;
-    displayPortOledInit(displayPort);
-}
-
 void dashboardInit(rxConfig_t *rxConfigToUse)
 {
     delay(200);
@@ -715,7 +705,7 @@ void dashboardInit(rxConfig_t *rxConfigToUse)
     delay(200);
 
 #if defined(CMS) && defined(OLEDCMS)
-    cmsDeviceRegister(dashboardCmsInit);
+    displayPortOledInit();
 #endif
 
     rxConfig = rxConfigToUse;
diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c
index a8689ecaa..e77abb5ce 100644
--- a/src/main/io/displayport_oled.c
+++ b/src/main/io/displayport_oled.c
@@ -27,21 +27,21 @@
 #include "drivers/display.h"
 #include "drivers/display_ug2864hsweg01.h"
 
+#include "io/cms.h"
 #include "io/displayport_oled.h"
 
-extern bool dashboardInCMS; // temporary
+// Exported
+displayPort_t oledDisplayPort;
 
 static int oledOpen(displayPort_t *displayPort)
 {
-dashboardInCMS = true;
-    displayPort->inCMS = true;
+    UNUSED(displayPort);
     return 0;
 }
 
 static int oledClose(displayPort_t *displayPort)
 {
-dashboardInCMS = false;
-    displayPort->inCMS = false;
+    UNUSED(displayPort);
     return 0;
 }
 
@@ -87,10 +87,13 @@ static const displayPortVTable_t oledVTable = {
     .txBytesFree = oledTxBytesFree
 };
 
-void displayPortOledInit(displayPort_t *displayPort)
+void displayPortOledInit()
 {
-    displayPort->vTable = &oledVTable;
-    displayPort->rows = SCREEN_CHARACTER_ROW_COUNT;
-    displayPort->cols = SCREEN_CHARACTER_COLUMN_COUNT;
+    oledDisplayPort.vTable = &oledVTable;
+    oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT;
+    oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT;
+    oledDisplayPort.inCMS = false;
+
+    cmsDisplayPortRegister(&oledDisplayPort);
 }
 #endif // OLEDCMS
diff --git a/src/main/io/displayport_oled.h b/src/main/io/displayport_oled.h
index 4bd02cf65..2d0bc2050 100644
--- a/src/main/io/displayport_oled.h
+++ b/src/main/io/displayport_oled.h
@@ -17,5 +17,6 @@
 
 #pragma once
 
-struct displayPort_s;
-void displayPortOledInit(struct displayPort_s *displayPort);
+void displayPortOledInit(void);
+
+extern displayPort_t oledDisplayPort;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index ed220fa2d..c6aa8dde4 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -92,8 +92,6 @@ void osdEditElement(void *ptr);
 void osdDrawElements(void);
 void osdDrawSingleElement(uint8_t item);
 
-bool osdInMenu = false;
-
 #define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees
 #define AH_MAX_ROLL 400  // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees
 #define AH_SIDEBAR_WIDTH_POS 7
@@ -110,7 +108,7 @@ void osdDrawElements(void)
     if (false)
         ;
 #endif
-    else if (sensors(SENSOR_ACC) || osdInMenu)
+    else if (sensors(SENSOR_ACC) || osd7456DisplayPort.inCMS)
     {
         osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
         osdDrawSingleElement(OSD_CROSSHAIRS);
@@ -129,7 +127,7 @@ void osdDrawElements(void)
     osdDrawSingleElement(OSD_ALTITUDE);
 
 #ifdef GPS
-    if (sensors(SENSOR_GPS) || osdInMenu) {
+    if (sensors(SENSOR_GPS) || osd7456DisplayPort.inCMS) {
         osdDrawSingleElement(OSD_GPS_SATS);
         osdDrawSingleElement(OSD_GPS_SPEED);
     }
@@ -400,7 +398,7 @@ void osdInit(void)
     refreshTimeout = 4 * REFRESH_1S;
 
 #ifdef CMS
-    cmsDeviceRegister(osdMax7456Init);
+    osd7456DisplayPortInit();
 #endif
 }
 
@@ -576,7 +574,7 @@ void updateOsd(uint32_t currentTime)
         max7456DrawScreen();
 
     // do not allow ARM if we are in menu
-    if (osdInMenu)
+    if (osd7456DisplayPort.inCMS)
         DISABLE_ARMING_FLAG(OK_TO_ARM);
 }
 
@@ -622,7 +620,7 @@ void osdUpdate(uint32_t currentTime)
 
     blinkState = (millis() / 200) % 2;
 
-    if (!osdInMenu) {
+    if (!osd7456DisplayPort.inCMS) {
         osdUpdateAlarms();
         osdDrawElements();
 #ifdef OSD_CALLS_CMS
diff --git a/src/main/io/osd_max7456.c b/src/main/io/osd_max7456.c
index 7471910c6..66d89c1e4 100644
--- a/src/main/io/osd_max7456.c
+++ b/src/main/io/osd_max7456.c
@@ -12,17 +12,16 @@
 #include "drivers/display.h"
 #include "drivers/max7456.h"
 
-extern bool osdInMenu;
+displayPort_t osd7456DisplayPort;
+
 extern uint16_t refreshTimeout;
 void osdResetAlarms(void);
 
-uint8_t shiftdown;
-
 static int osdMenuBegin(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
     osdResetAlarms();
-    osdInMenu = true;
+    displayPort->inCMS = true;
     refreshTimeout = 0;
 
     return 0;
@@ -31,7 +30,7 @@ static int osdMenuBegin(displayPort_t *displayPort)
 static int osdMenuEnd(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
-    osdInMenu = false;
+    displayPort->inCMS = false;
 
     return 0;
 }
@@ -47,7 +46,7 @@ static int osdClearScreen(displayPort_t *displayPort)
 static int osdWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, char *s)
 {
     UNUSED(displayPort);
-    max7456Write(x, y + shiftdown, s);
+    max7456Write(x, y, s);
 
     return 0;
 }
@@ -56,7 +55,7 @@ static void osdResync(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
     max7456RefreshAll();
-    displayPort->rows = max7456GetRowsCount() - masterConfig.osdProfile.row_shiftdown;
+    displayPort->rows = max7456GetRowsCount();
     displayPort->cols = 30;
 }
 
@@ -82,9 +81,11 @@ displayPortVTable_t osdVTable = {
     osdTxBytesFree,
 };
 
-void osdMax7456Init(displayPort_t *displayPort)
+void osd7456DisplayPortInit(void)
 {
-    displayPort->vTable = &osdVTable;
-    osdResync(displayPort);
+    osd7456DisplayPort.vTable = &osdVTable;
+    osd7456DisplayPort.inCMS = false;
+    osdResync(&osd7456DisplayPort);
+    cmsDisplayPortRegister(&osd7456DisplayPort);
 }
 #endif // OSD
diff --git a/src/main/io/osd_max7456.h b/src/main/io/osd_max7456.h
index 26d696f30..22c64faa7 100644
--- a/src/main/io/osd_max7456.h
+++ b/src/main/io/osd_max7456.h
@@ -1,3 +1,5 @@
 #pragma once
 
-void osdMax7456Init(displayPort_t *displayPort);
+extern displayPort_t osd7456DisplayPort;
+
+void osd7456DisplayPortInit(void);
diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk
index e30c6530a..756617182 100644
--- a/src/main/target/OMNIBUS/target.mk
+++ b/src/main/target/OMNIBUS/target.mk
@@ -18,6 +18,7 @@ TARGET_SRC = \
             io/osd_max7456.c \
             io/canvas.c \
             io/cms.c \
+            io/cms_builtin.c \
             io/cms_imu.c \
             io/cms_blackbox.c \
             io/cms_vtx.c \
diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk
index 5cefb51d2..d805edc7f 100644
--- a/src/main/target/OMNIBUSF4/target.mk
+++ b/src/main/target/OMNIBUSF4/target.mk
@@ -9,6 +9,7 @@ TARGET_SRC = \
             io/osd.c \
             io/osd_max7456.c \
             io/cms.c \
+            io/cms_builtin.c \
             io/cms_imu.c \
             io/cms_blackbox.c \
             io/cms_ledstrip.c \
diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk
index 038c7da7d..f12379591 100644
--- a/src/main/target/SIRINFPV/target.mk
+++ b/src/main/target/SIRINFPV/target.mk
@@ -16,6 +16,7 @@ TARGET_SRC = \
             io/osd_max7456.c \
             io/canvas.c \
             io/cms.c \
+            io/cms_builtin.c \
             io/cms_imu.c \
             io/cms_blackbox.c \
             io/cms_vtx.c \
diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk
index f2a3df744..b37d1b2b0 100644
--- a/src/main/target/SPRACINGF3/target.mk
+++ b/src/main/target/SPRACINGF3/target.mk
@@ -11,6 +11,7 @@ TARGET_SRC = \
             drivers/compass_hmc5883l.c \
             io/canvas.c \
             io/cms.c \
+            io/cms_builtin.c \
             io/cms_imu.c \
             io/cms_blackbox.c \
             io/cms_vtx.c \
diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk
index 706b68492..8189f15f4 100644
--- a/src/main/target/STM32F3DISCOVERY/target.mk
+++ b/src/main/target/STM32F3DISCOVERY/target.mk
@@ -30,6 +30,7 @@ TARGET_SRC = \
             io/osd_max7456.c \
             io/canvas.c \
             io/cms.c \
+            io/cms_builtin.c \
             io/cms_imu.c \
             io/cms_blackbox.c \
             io/cms_vtx.c \