diff --git a/Makefile b/Makefile
index 5696fd6bf6..19a6ec1db9 100644
--- a/Makefile
+++ b/Makefile
@@ -562,6 +562,7 @@ HIGHEND_SRC = \
             flight/gps_conversion.c \
             io/gps.c \
             io/ledstrip.c \
+            io/displayport_oled.c \
             io/dashboard.c \
             sensors/sonar.c \
             sensors/barometer.c \
diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index 2d7ec79de3..9b3810c41b 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -29,6 +29,7 @@ void displayClear(displayPort_t *instance)
     instance->vTable->clear(instance);
     instance->cleared = true;
     instance->cursorRow = -1;
+    instance->inCMS = false;
 }
 
 void displayOpen(displayPort_t *instance)
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index 60028b7ebe..f0075ff264 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -26,6 +26,7 @@ typedef struct displayPort_s {
     // CMS state
     bool cleared;
     int8_t cursorRow;
+    bool inCMS;
 } displayPort_t;
 
 typedef struct displayPortVTable_s {
diff --git a/src/main/io/cms.c b/src/main/io/cms.c
index 85a2b5f9ae..610e2a3d9a 100644
--- a/src/main/io/cms.c
+++ b/src/main/io/cms.c
@@ -77,9 +77,11 @@
 #include "io/cms_ledstrip.h"
 
 // Forwards
-long cmsx_InfoInit(void);
+static long cmsx_InfoInit(void);
+#if 0
 long cmsx_FeatureRead(void);
 long cmsx_FeatureWriteback(void);
+#endif
 
 // Device management
 
@@ -87,10 +89,9 @@ long cmsx_FeatureWriteback(void);
 #define CMS_MAX_DEVICE 4
 #endif
 
-cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE];
-int cmsDeviceCount;
-int cmsCurrentDevice = -1;
-int cmsLastDevice = -1;
+static cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE];
+static int cmsDeviceCount;
+static int cmsCurrentDevice = -1;
 
 bool cmsDeviceRegister(cmsDeviceInitFuncPtr func)
 {
@@ -102,7 +103,7 @@ bool cmsDeviceRegister(cmsDeviceInitFuncPtr func)
     return true;
 }
 
-cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void)
+static cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void)
 {
     if (cmsDeviceCount == 0)
         return NULL;
@@ -113,7 +114,7 @@ cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void)
     return cmsDeviceInitFunc[cmsCurrentDevice];
 }
 
-cmsDeviceInitFuncPtr cmsDeviceSelectNext(void)
+static cmsDeviceInitFuncPtr cmsDeviceSelectNext(void)
 {
     if (cmsDeviceCount == 0)
         return NULL;
@@ -125,7 +126,7 @@ cmsDeviceInitFuncPtr cmsDeviceSelectNext(void)
 
 #define CMS_UPDATE_INTERVAL 50 // msec
 
-void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc)
+static void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc)
 {
     cmsDeviceInitFunc(pDisp);
 }
@@ -151,38 +152,37 @@ void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc)
 #define RIGHT_MENU_COLUMN(p) ((p)->cols - 8)
 #define MAX_MENU_ITEMS(p)    ((p)->rows - 2)
 
-displayPort_t currentDisplay;
+static displayPort_t currentDisplay;
 
-bool cmsInMenu = false;
+static bool cmsInMenu = false;
 
-CMS_Menu menuMain;
-CMS_Menu *currentMenu;          // Points to top entry of the current page
+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???
 
-CMS_Menu *menuStack[10];         // Stack to save menu transition
-uint8_t menuStackHistory[10];    // cursorRow in a stacked menu
-uint8_t menuStackIdx = 0;
+static CMS_Menu *menuStack[10];  // Stack to save menu transition
+static uint8_t menuStackHistory[10];// cursorRow in a stacked menu
+static uint8_t menuStackIdx = 0;
 
-OSD_Entry *pageTop;              // Points to top entry of the current page
-OSD_Entry *pageTopAlt;           // Only 2 pages are allowed (for now)
-uint8_t maxRow;                  // Max row in the current page
+static OSD_Entry *pageTop;       // Points to top entry of the current page
+static OSD_Entry *pageTopAlt;    // Only 2 pages are allowed (for now)
+static uint8_t maxRow;           // Max row in the current page
 
-int8_t entryPos;                 // Absolute position of the cursor
-int8_t cursorRow;                // Position of the cursor relative to pageTop
+static int8_t cursorRow;
 
 // Broken menu substitution
 
-char menuErrLabel[21 + 1];
+static char menuErrLabel[21 + 1];
 
-OSD_Entry menuErrEntries[] = {
+static OSD_Entry menuErrEntries[] = {
     { "BROKEN MENU", OME_Label, NULL, NULL, 0 },
     { menuErrLabel, OME_String, NULL, NULL, 0 },
     { "BACK", OME_Back, NULL, NULL, 0 },
     { NULL, OME_END, NULL, NULL, 0 }
 };
 
-CMS_Menu menuErr = {
+static CMS_Menu menuErr = {
     "MENU CONTENT BROKEN",
     OME_MENU,
     NULL,
@@ -208,7 +208,7 @@ CMS_Menu menuErr = {
 #define BUTTON_TIME   250 // msec
 #define BUTTON_PAUSE  500 // msec
 
-void cmsUpdateMaxRow(displayPort_t *instance)
+static void cmsUpdateMaxRow(displayPort_t *instance)
 {
     OSD_Entry *ptr;
 
@@ -249,7 +249,7 @@ static void cmsFormatFloat(int32_t value, char *floatString)
         floatString[0] = ' ';
 }
 
-void cmsPadToSize(char *buf, int size)
+static void cmsPadToSize(char *buf, int size)
 {
     int i;
 
@@ -265,7 +265,7 @@ void cmsPadToSize(char *buf, int size)
     buf[size] = 0;
 }
 
-int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool drawPolled)
+static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool drawPolled)
 {
     char buff[10];
     int cnt = 0;
@@ -389,7 +389,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
     return cnt;
 }
 
-void cmsDrawMenu(displayPort_t *pDisplay)
+static void cmsDrawMenu(displayPort_t *pDisplay)
 {
     uint8_t i;
     OSD_Entry *p;
@@ -507,7 +507,7 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr)
     return 0;
 }
 
-long cmsMenuBack(displayPort_t *pDisplay)
+static long cmsMenuBack(displayPort_t *pDisplay)
 {
     if (currentMenu->onExit)
         currentMenu->onExit();
@@ -530,7 +530,7 @@ long cmsMenuBack(displayPort_t *pDisplay)
     return 0;
 }
 
-void cmsMenuOpen(void)
+static void cmsMenuOpen(void)
 {
     cmsDeviceInitFuncPtr initfunc;
 
@@ -554,7 +554,7 @@ void cmsMenuOpen(void)
     cmsMenuChange(&currentDisplay, currentMenu);
 }
 
-void cmsTraverseGlobalExit(CMS_Menu *pMenu)
+static void cmsTraverseGlobalExit(CMS_Menu *pMenu)
 {
     OSD_Entry *p;
 
@@ -566,7 +566,7 @@ void cmsTraverseGlobalExit(CMS_Menu *pMenu)
         pMenu->onGlobalExit();
 }
 
-long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
+static long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
 {
     if (ptr) {
         displayClear(pDisplay);
@@ -597,7 +597,7 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
     return 0;
 }
 
-uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
+static uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
 {
     uint16_t res = BUTTON_TIME;
     OSD_Entry *p;
@@ -773,7 +773,7 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
     return res;
 }
 
-void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
+static void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
 {
     static int16_t rcDelay = BUTTON_TIME;
     static uint32_t lastCalled = 0;
@@ -853,7 +853,6 @@ void cmsHandler(uint32_t currentTime)
 
 void cmsInit(void)
 {
-    //cmsx_InfoInit();
 }
 
 //
@@ -867,7 +866,7 @@ static char infoTargetName[] = __TARGET__;
 
 #include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere
 
-OSD_Entry menuInfoEntries[] = {
+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 },
@@ -877,7 +876,7 @@ OSD_Entry menuInfoEntries[] = {
     { NULL, OME_END, NULL, NULL, 0 }
 };
 
-CMS_Menu menuInfo = {
+static CMS_Menu menuInfo = {
     "MENUINFO",
     OME_MENU,
     cmsx_InfoInit,
@@ -886,7 +885,7 @@ CMS_Menu menuInfo = {
     menuInfoEntries,
 };
 
-long cmsx_InfoInit(void)
+static long cmsx_InfoInit(void)
 {
     for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
         if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f')
@@ -900,7 +899,7 @@ long cmsx_InfoInit(void)
 
 // Features
 
-OSD_Entry menuFeaturesEntries[] =
+static OSD_Entry menuFeaturesEntries[] =
 {
     {"--- FEATURES ---", OME_Label, NULL, NULL, 0},
     {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0},
@@ -914,7 +913,7 @@ OSD_Entry menuFeaturesEntries[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
-CMS_Menu menuFeatures = {
+static CMS_Menu menuFeatures = {
     "MENUFEATURES",
     OME_MENU,
     NULL,
@@ -925,7 +924,7 @@ CMS_Menu menuFeatures = {
 
 // Main
 
-OSD_Entry menuMainEntries[] =
+static OSD_Entry menuMainEntries[] =
 {
     {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0},
     {"CFG&IMU", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0},
@@ -942,7 +941,7 @@ OSD_Entry menuMainEntries[] =
     {NULL,OME_END, NULL, NULL, 0}
 };
 
-CMS_Menu menuMain = {
+static CMS_Menu menuMain = {
     "MENUMAIN",
     OME_MENU,
     NULL,
@@ -950,42 +949,4 @@ CMS_Menu menuMain = {
     NULL,
     menuMainEntries,
 };
-
-#if 0
-long cmsx_FeatureRead(void)
-{
-    cmsx_Blackbox_FeatureRead();
-
-#ifdef LED_STRIP
-    cmsx_Ledstrip_FeatureRead();
-    cmsx_Ledstrip_ConfigRead();
-#endif
-
-#if defined(VTX) || defined(USE_RTC6705)
-    cmsx_Vtx_FeatureRead();
-    cmsx_Vtx_ConfigRead();
-#endif // VTX || USE_RTC6705
-
-    return 0;
-}
-
-long cmsx_FeatureWriteback(void)
-{
-    cmsx_Blackbox_FeatureWriteback();
-
-#ifdef LED_STRIP
-    cmsx_Ledstrip_FeatureWriteback();
-#endif
-
-#if defined(VTX) || defined(USE_RTC6705)
-    cmsx_Vtx_FeatureWriteback();
-    cmsx_Vtx_ConfigWriteback();
-#endif // VTX || USE_RTC6705
-
-    saveConfigAndNotify();
-
-    return 0;
-}
-#endif
-
 #endif // CMS
diff --git a/src/main/io/cms.h b/src/main/io/cms.h
index 5e3be53e80..e1a1c3be6b 100644
--- a/src/main/io/cms.h
+++ b/src/main/io/cms.h
@@ -11,7 +11,7 @@ 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/dashboard.c b/src/main/io/dashboard.c
index 7fc7ab9a30..b12b451264 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -31,6 +31,7 @@
 #include "build/build_config.h"
 
 #include "drivers/system.h"
+#include "drivers/display.h"
 #include "drivers/display_ug2864hsweg01.h"
 
 #include "common/printf.h"
@@ -53,10 +54,10 @@
 #include "flight/failsafe.h"
 
 #include "io/cms.h"
+#include "io/displayport_oled.h"
 
-#ifdef CMS
-void dashboardCmsInit(displayPort_t *pPort); // Forward
-#endif
+displayPort_t *displayPort;
+bool dashboardInCMS = false; // temporary
 
 #ifdef GPS
 #include "io/gps.h"
@@ -585,17 +586,15 @@ void showDebugPage(void)
 }
 #endif
 
-#ifdef OLEDCMS
-static bool dashboardInCMS = false;
-#endif
-
 void dashboardUpdate(uint32_t currentTime)
 {
     static uint8_t previousArmedState = 0;
 
 #ifdef OLEDCMS
-    if (dashboardInCMS)
+    if (dashboardInCMS) return;
+    if (displayPort && displayPort->inCMS) {
         return;
+    }
 #endif
 
     const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L;
@@ -703,6 +702,12 @@ 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);
@@ -749,76 +754,4 @@ void dashboardDisablePageCycling(void)
 {
     pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED;
 }
-
-#ifdef OLEDCMS
-#include "drivers/display.h"
-
-static int dashboardBegin(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    dashboardInCMS = true;
-
-    return 0;
-}
-
-static int dashboardEnd(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    dashboardInCMS = false;
-
-    return 0;
-}
-
-static int dashboardClear(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    i2c_OLED_clear_display_quick();
-
-    return 0;
-}
-
-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;
-}
-
-static int dashboardHeartbeat(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
-static void dashboardResync(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-}
-
-static uint32_t dashboardTxBytesFree(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return UINT32_MAX;
-}
-
-static const displayPortVTable_t dashboardVTable = {
-    dashboardBegin,
-    dashboardEnd,
-    dashboardClear,
-    dashboardWrite,
-    dashboardHeartbeat,
-    dashboardResync,
-    dashboardTxBytesFree,
-};
-
-void dashboardCmsInit(displayPort_t *displayPort)
-{
-    displayPort->rows = SCREEN_CHARACTER_ROW_COUNT;
-    displayPort->cols = SCREEN_CHARACTER_COLUMN_COUNT;
-    displayPort->vTable = &dashboardVTable;
-}
-#endif // OLEDCMS
-
 #endif // USE_DASHBOARD
diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h
index 5b98b4c282..cca02effdf 100644
--- a/src/main/io/dashboard.h
+++ b/src/main/io/dashboard.h
@@ -45,11 +45,3 @@ void dashboardEnablePageCycling(void);
 void dashboardDisablePageCycling(void);
 void dashboardResetPageCycling(void);
 void dashboardSetNextPageChangeAt(uint32_t futureMicros);
-void displayEnablePageCycling(void);
-void displayDisablePageCycling(void);
-void displayResetPageCycling(void);
-void displaySetNextPageChangeAt(uint32_t futureMicros);
-
-#ifdef CMS
-//void dashboardCmsInit(displayPort_t *pPort);
-#endif
diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c
new file mode 100644
index 0000000000..a8689ecaa9
--- /dev/null
+++ b/src/main/io/displayport_oled.c
@@ -0,0 +1,96 @@
+/*
+ * 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"
+
+#ifdef OLEDCMS
+
+#include "common/utils.h"
+
+#include "drivers/display.h"
+#include "drivers/display_ug2864hsweg01.h"
+
+#include "io/displayport_oled.h"
+
+extern bool dashboardInCMS; // temporary
+
+static int oledOpen(displayPort_t *displayPort)
+{
+dashboardInCMS = true;
+    displayPort->inCMS = true;
+    return 0;
+}
+
+static int oledClose(displayPort_t *displayPort)
+{
+dashboardInCMS = false;
+    displayPort->inCMS = false;
+    return 0;
+}
+
+static int oledClear(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    i2c_OLED_clear_display_quick();
+    return 0;
+}
+
+static int oledWrite(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;
+}
+
+static int oledHeartbeat(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static void oledResync(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+}
+
+static uint32_t oledTxBytesFree(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return UINT32_MAX;
+}
+
+static const displayPortVTable_t oledVTable = {
+    .open = oledOpen,
+    .close = oledClose,
+    .clear = oledClear,
+    .write = oledWrite,
+    .heartbeat = oledHeartbeat,
+    .resync = oledResync,
+    .txBytesFree = oledTxBytesFree
+};
+
+void displayPortOledInit(displayPort_t *displayPort)
+{
+    displayPort->vTable = &oledVTable;
+    displayPort->rows = SCREEN_CHARACTER_ROW_COUNT;
+    displayPort->cols = SCREEN_CHARACTER_COLUMN_COUNT;
+}
+#endif // OLEDCMS
diff --git a/src/main/io/displayport_oled.h b/src/main/io/displayport_oled.h
new file mode 100644
index 0000000000..4bd02cf652
--- /dev/null
+++ b/src/main/io/displayport_oled.h
@@ -0,0 +1,21 @@
+/*
+ * 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 displayPort_s;
+void displayPortOledInit(struct displayPort_s *displayPort);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 59041a3e69..ed220fa2d2 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -29,10 +29,10 @@
 
 #include "platform.h"
 
-#include "build/version.h"
-
 #ifdef OSD
 
+#include "build/version.h"
+
 #include "common/utils.h"
 
 #include "drivers/system.h"