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/dashboard.c b/src/main/io/dashboard.c
index 7fc7ab9a30..6447bd8eaa 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,9 @@
 #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;
 
 #ifdef GPS
 #include "io/gps.h"
@@ -585,17 +585,14 @@ 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 (displayPort && displayPort->inCMS) {
         return;
+    }
 #endif
 
     const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L;
@@ -703,6 +700,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 +752,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..2e158a3c20
--- /dev/null
+++ b/src/main/io/displayport_oled.c
@@ -0,0 +1,92 @@
+/*
+ * 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"
+
+static int oledOpen(displayPort_t *displayPort)
+{
+    displayPort->inCMS = true;
+    return 0;
+}
+
+static int oledClose(displayPort_t *displayPort)
+{
+    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 f1b3871e70..820f99d72e 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"