diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index cb3ee98df..c8bb4b2ed 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -17,7 +17,6 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-#include <stdlib.h>
 #include <string.h>
 
 #include "platform.h"
@@ -26,8 +25,6 @@
 
 #include "build/debug.h"
 
-#include "common/printf.h"
-
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
 
@@ -41,7 +38,6 @@
 #include "drivers/time.h"
 #include "drivers/vcd.h"
 
-#include "fc/config.h" // For systemConfig()
 
 // DEBUG_MAX7456_SIGNAL
 #define DEBUG_MAX7456_SIGNAL_MODEREG       0
@@ -217,7 +213,7 @@ static uint8_t  displayMemoryModeReg = 0;
 static uint8_t  hosRegValue; // HOS (Horizontal offset register) value
 static uint8_t  vosRegValue; // VOS (Vertical offset register) value
 
-static bool  max7456Lock        = false;
+static bool max7456Lock         = false;
 static bool fontIsLoading       = false;
 static IO_t max7456CsPin        = IO_NONE;
 
@@ -366,26 +362,26 @@ void max7456ReInit(void)
     ENABLE_MAX7456;
 
     switch (videoSignalCfg) {
-        case VIDEO_SYSTEM_PAL:
-            videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
-            break;
+    case VIDEO_SYSTEM_PAL:
+        videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
+        break;
 
-        case VIDEO_SYSTEM_NTSC:
+    case VIDEO_SYSTEM_NTSC:
+        videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
+        break;
+
+    case VIDEO_SYSTEM_AUTO:
+        srdata = max7456Send(MAX7456ADD_STAT, 0x00);
+
+        if (VIN_IS_NTSC(srdata)) {
             videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
-            break;
-
-        case VIDEO_SYSTEM_AUTO:
-            srdata = max7456Send(MAX7456ADD_STAT, 0x00);
-
-            if (VIN_IS_NTSC(srdata)) {
-                videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
-            } else if (VIN_IS_PAL(srdata)) {
-                videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
-            } else {
-                // No valid input signal, fallback to default (XXX NTSC for now)
-                videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
-            }
-            break;
+        } else if (VIN_IS_PAL(srdata)) {
+            videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
+        } else {
+            // No valid input signal, fallback to default (XXX NTSC for now)
+            videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
+        }
+        break;
     }
 
     if (videoSignalReg & VIDEO_MODE_PAL) { //PAL
@@ -394,9 +390,9 @@ void max7456ReInit(void)
         maxScreenSize = VIDEO_BUFFER_CHARS_NTSC;
     }
 
-    /* Set all rows to same charactor black/white level. */
+    // Set all rows to same charactor black/white level
     max7456Brightness(0, 2);
-    /* Re-enable MAX7456 (last function call disables it) */
+    // Re-enable MAX7456 (last function call disables it)
     ENABLE_MAX7456;
 
     // Make sure the Max7456 is enabled
@@ -409,8 +405,7 @@ void max7456ReInit(void)
 
     // Clear shadow to force redraw all screen in non-dma mode.
     memset(shadowBuffer, 0, maxScreenSize);
-    if (firstInit)
-    {
+    if (firstInit) {
         max7456RefreshAll();
         firstInit = false;
     }
@@ -420,7 +415,7 @@ void max7456ReInit(void)
 // Here we init only CS and try to init MAX for first time.
 // Also detect device type (MAX v.s. AT)
 
-void max7456Init(const vcdProfile_t *pVcdProfile)
+void max7456Init(const vcdProfile_t *pVcdProfile, bool cpuOverclock)
 {
     max7456HardwareReset();
 
@@ -452,7 +447,7 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
         break;
 
     case MAX7456_CLOCK_CONFIG_OC:
-        max7456SpiClock = (systemConfig()->cpu_overclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? MAX7456_SPI_CLK * 2 : MAX7456_SPI_CLK;
+        max7456SpiClock = (cpuOverclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? MAX7456_SPI_CLK * 2 : MAX7456_SPI_CLK;
         break;
 
     case MAX7456_CLOCK_CONFIG_FULL:
@@ -460,9 +455,11 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
         break;
     }
 
-    DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_OVERCLOCK, systemConfig()->cpu_overclock);
+    DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_OVERCLOCK, cpuOverclock);
     DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DEVTYPE, max7456DeviceType);
     DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DIVISOR, max7456SpiClock);
+#else
+    UNUSED(cpuOverclock);
 #endif
 
     spiSetDivisor(MAX7456_SPI_INSTANCE, max7456SpiClock);
@@ -489,10 +486,11 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
  */
 void max7456Invert(bool invert)
 {
-    if (invert)
+    if (invert) {
         displayMemoryModeReg |= INVERT_PIXEL_COLOR;
-    else
+    } else {
         displayMemoryModeReg &= ~INVERT_PIXEL_COLOR;
+    }
 
     ENABLE_MAX7456;
     max7456Send(MAX7456ADD_DMM, displayMemoryModeReg);
@@ -507,7 +505,7 @@ void max7456Invert(bool invert)
  */
 void max7456Brightness(uint8_t black, uint8_t white)
 {
-    uint8_t reg = (black << 2) | (3 - white);
+    const uint8_t reg = (black << 2) | (3 - white);
 
     ENABLE_MAX7456;
     for (int i = MAX7456ADD_RB0; i <= MAX7456ADD_RB15; i++) {
@@ -519,13 +517,11 @@ void max7456Brightness(uint8_t black, uint8_t white)
 //just fill with spaces with some tricks
 void max7456ClearScreen(void)
 {
-    uint16_t x;
-    uint32_t *p = (uint32_t*)&screenBuffer[0];
-    for (x = 0; x < VIDEO_BUFFER_CHARS_PAL/4; x++)
-        p[x] = 0x20202020;
+    memset(screenBuffer, 0x20, VIDEO_BUFFER_CHARS_PAL);
 }
 
-uint8_t* max7456GetScreenBuffer(void) {
+uint8_t* max7456GetScreenBuffer(void)
+{
     return screenBuffer;
 }
 
@@ -536,10 +532,11 @@ void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c)
 
 void max7456Write(uint8_t x, uint8_t y, const char *buff)
 {
-    uint8_t i = 0;
-    for (i = 0; *(buff+i); i++)
-        if (x+i < CHARS_PER_LINE) // Do not write over screen
+    for (int i = 0; *(buff+i); i++) {
+        if (x+i < CHARS_PER_LINE) {// Do not write over screen
             screenBuffer[y*CHARS_PER_LINE+x+i] = *(buff+i);
+        }
+    }
 }
 
 bool max7456DmaInProgress(void)
@@ -553,13 +550,9 @@ bool max7456DmaInProgress(void)
 
 void max7456DrawScreen(void)
 {
-    uint8_t stallCheck;
-    uint8_t videoSense;
     static uint32_t lastSigCheckMs = 0;
-    uint32_t nowMs;
     static uint32_t videoDetectTimeMs = 0;
     static uint16_t pos = 0;
-    int k = 0, buff_len=0;
 
     static uint16_t reInitCount = 0;
 
@@ -569,10 +562,10 @@ void max7456DrawScreen(void)
 
         max7456Lock = true;
         ENABLE_MAX7456;
-        stallCheck = max7456Send(MAX7456ADD_VM0|MAX7456ADD_READ, 0x00);
+        const uint8_t stallCheck = max7456Send(MAX7456ADD_VM0|MAX7456ADD_READ, 0x00);
         DISABLE_MAX7456;
 
-        nowMs = millis();
+        const timeMs_t nowMs = millis();
 
         if (stallCheck != videoSignalReg) {
             max7456ReInit();
@@ -583,7 +576,7 @@ void max7456DrawScreen(void)
             // Adjust output format based on the current input format.
 
             ENABLE_MAX7456;
-            videoSense = max7456Send(MAX7456ADD_STAT, 0x00);
+            const uint8_t videoSense = max7456Send(MAX7456ADD_STAT, 0x00);
             DISABLE_MAX7456;
 
             DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_MODEREG, videoSignalReg & VIDEO_MODE_MASK);
@@ -612,7 +605,8 @@ void max7456DrawScreen(void)
 
         //------------   end of (re)init-------------------------------------
 
-        for (k=0; k< MAX_CHARS2UPDATE; k++) {
+        int buff_len = 0;
+        for (int k = 0; k < MAX_CHARS2UPDATE; k++) {
             if (screenBuffer[pos] != shadowBuffer[pos]) {
                 spiBuff[buff_len++] = MAX7456ADD_DMAH;
                 spiBuff[buff_len++] = pos >> 8;
@@ -631,15 +625,13 @@ void max7456DrawScreen(void)
         }
 
         if (buff_len) {
-            #ifdef MAX7456_DMA_CHANNEL_TX
-            if (buff_len > 0)
-                max7456SendDma(spiBuff, NULL, buff_len);
-            #else
+#ifdef MAX7456_DMA_CHANNEL_TX
+            max7456SendDma(spiBuff, NULL, buff_len);
+#else
             ENABLE_MAX7456;
-            for (k=0; k < buff_len; k++)
-                spiTransferByte(MAX7456_SPI_INSTANCE, spiBuff[k]);
+            spiTransfer(MAX7456_SPI_INSTANCE, spiBuff, NULL, buff_len);
             DISABLE_MAX7456;
-            #endif // MAX7456_DMA_CHANNEL_TX
+#endif // MAX7456_DMA_CHANNEL_TX
         }
         max7456Lock = false;
     }
@@ -651,17 +643,15 @@ void max7456RefreshAll(void)
 {
     if (!max7456Lock) {
 #ifdef MAX7456_DMA_CHANNEL_TX
-    while (dmaTransactionInProgress);
+        while (dmaTransactionInProgress);
 #endif
-        uint16_t xx;
         max7456Lock = true;
         ENABLE_MAX7456;
         max7456Send(MAX7456ADD_DMAH, 0);
         max7456Send(MAX7456ADD_DMAL, 0);
         max7456Send(MAX7456ADD_DMM, displayMemoryModeReg | 1);
 
-        for (xx = 0; xx < maxScreenSize; ++xx)
-        {
+        for (int xx = 0; xx < maxScreenSize; ++xx) {
             max7456Send(MAX7456ADD_DMDI, screenBuffer[xx]);
             shadowBuffer[xx] = screenBuffer[xx];
         }
@@ -675,8 +665,6 @@ void max7456RefreshAll(void)
 
 void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
 {
-    uint8_t x;
-
 #ifdef MAX7456_DMA_CHANNEL_TX
     while (dmaTransactionInProgress);
 #endif
@@ -690,7 +678,7 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
 
     max7456Send(MAX7456ADD_CMAH, char_address); // set start address high
 
-    for (x = 0; x < 54; x++) {
+    for (int x = 0; x < 54; x++) {
         max7456Send(MAX7456ADD_CMAL, x); //set start address low
         max7456Send(MAX7456ADD_CMDI, font_data[x]);
 #ifdef LED0_TOGGLE
@@ -734,4 +722,4 @@ void max7456HardwareReset(void)
 #endif
 }
 
-#endif
+#endif // USE_MAX7456
diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h
index b3a7b8296..ad4b3af01 100755
--- a/src/main/drivers/max7456.h
+++ b/src/main/drivers/max7456.h
@@ -27,7 +27,7 @@ extern uint16_t maxScreenSize;
 
 struct vcdProfile_s;
 void    max7456HardwareReset(void);
-void    max7456Init(const struct vcdProfile_s *vcdProfile);
+void    max7456Init(const struct vcdProfile_s *vcdProfile, bool cpuOverclock);
 void    max7456Invert(bool invert);
 void    max7456Brightness(uint8_t black, uint8_t white);
 void    max7456DrawScreen(void);
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 2a045948e..3112ccdae 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -68,6 +68,7 @@
 #include "io/statusindicator.h"
 #include "io/transponder_ir.h"
 #include "io/vtx_control.h"
+#include "io/vtx_rtc6705.h"
 
 #include "rx/rx.h"
 
@@ -86,13 +87,6 @@
 
 // June 2013     V2.2-dev
 
-#ifdef VTX_RTC6705
-bool canUpdateVTX(void);
-#define VTX_IF_READY if (canUpdateVTX())
-#else
-#define VTX_IF_READY
-#endif
-
 enum {
     ALIGN_GYRO = 0,
     ALIGN_ACCEL = 1,
@@ -381,6 +375,16 @@ void updateMagHold(void)
 }
 #endif
 
+#ifdef VTX_CONTROL
+static bool canUpdateVTX(void)
+{
+#ifdef VTX_RTC6705
+    return vtxRTC6705CanUpdate();
+#endif
+    return true;
+}
+#endif
+
 /*
  * processRx called from taskUpdateRxMain
  */
@@ -586,7 +590,7 @@ void processRx(timeUs_t currentTimeUs)
 #ifdef VTX_CONTROL
     vtxUpdateActivatedChannel();
 
-    VTX_IF_READY {
+    if (canUpdateVTX()) {
         handleVTXControlButton();
     }
 #endif
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 41e06cb13..73b30f146 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -65,7 +65,6 @@
 #include "drivers/usb_io.h"
 #include "drivers/transponder_ir.h"
 #include "drivers/exti.h"
-#include "drivers/max7456.h"
 #include "drivers/vtx_rtc6705.h"
 #include "drivers/vtx_common.h"
 #include "drivers/camera_control.h"
@@ -163,18 +162,6 @@ void processLoopback(void)
 #endif
 }
 
-#ifdef VTX_RTC6705
-bool canUpdateVTX(void)
-{
-#if defined(MAX7456_SPI_INSTANCE) && defined(RTC6705_SPI_INSTANCE) && defined(SPI_SHARED_MAX7456_AND_RTC6705)
-    if (feature(FEATURE_OSD)) {
-        return !max7456DmaInProgress();
-    }
-#endif
-    return true;
-}
-#endif
-
 #ifdef BUS_SWITCH_PIN
 void busSwitchInit(void)
 {
diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c
index 7a6292338..4d5e17d81 100755
--- a/src/main/interface/cli.c
+++ b/src/main/interface/cli.c
@@ -107,8 +107,6 @@ extern uint8_t __config_end;
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
 #include "io/flashfs.h"
-#include "io/displayport_max7456.h"
-#include "io/displayport_msp.h"
 #include "io/gimbal.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c
index 05d40ca3a..2c24e751b 100644
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@@ -31,6 +31,8 @@
 #include "drivers/max7456.h"
 #include "drivers/vcd.h"
 
+#include "fc/config.h"
+
 #include "io/displayport_max7456.h"
 #include "io/osd.h"
 #include "io/osd_slave.h"
@@ -154,7 +156,11 @@ static const displayPortVTable_t max7456VTable = {
 displayPort_t *max7456DisplayPortInit(const vcdProfile_t *vcdProfile)
 {
     displayInit(&max7456DisplayPort, &max7456VTable);
-    max7456Init(vcdProfile);
+#ifdef USE_OSD_SLAVE
+    max7456Init(vcdProfile, false);
+#else
+    max7456Init(vcdProfile, systemConfig()->cpu_overclock);
+#endif
     resync(&max7456DisplayPort);
     return &max7456DisplayPort;
 }
diff --git a/src/main/io/vtx_rtc6705.c b/src/main/io/vtx_rtc6705.c
index 5dae83b65..0110af3a5 100644
--- a/src/main/io/vtx_rtc6705.c
+++ b/src/main/io/vtx_rtc6705.c
@@ -33,14 +33,17 @@
 #include "common/maths.h"
 #include "common/utils.h"
 
+#include "config/feature.h"
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
 
+#include "drivers/max7456.h"
 #include "drivers/system.h"
 #include "drivers/time.h"
 #include "drivers/vtx_rtc6705.h"
 #include "drivers/vtx_common.h"
 
+#include "fc/config.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
@@ -48,10 +51,6 @@
 #include "io/vtx_rtc6705.h"
 #include "io/vtx_string.h"
 
-bool canUpdateVTX(void);
-
-#define WAIT_FOR_VTX while (!canUpdateVTX()) {}
-
 #if defined(USE_CMS) || defined(VTX_COMMON)
 const char * const rtc6705PowerNames[VTX_RTC6705_POWER_COUNT] = {
     "---", "25 ", "200",
@@ -84,10 +83,20 @@ void vtxRTC6705Configure(void)
     rtc6705SetBandAndChannel(vtxRTC6705.band - 1, vtxRTC6705.channel - 1);
 }
 
+bool vtxRTC6705CanUpdate(void)
+{
+#if defined(MAX7456_SPI_INSTANCE) && defined(RTC6705_SPI_INSTANCE) && defined(SPI_SHARED_MAX7456_AND_RTC6705)
+    if (feature(FEATURE_OSD)) {
+        return !max7456DmaInProgress();
+    }
+#endif
+    return true;
+}
+
 #ifdef RTC6705_POWER_PIN
 static void vtxRTC6705EnableAndConfigure(void)
 {
-    WAIT_FOR_VTX;
+    while (!vtxRTC6705CanUpdate());
 
     rtc6705Enable();
 
@@ -117,7 +126,7 @@ bool vtxRTC6705IsReady(void)
 
 void vtxRTC6705SetBandAndChannel(uint8_t band, uint8_t channel)
 {
-    WAIT_FOR_VTX;
+    while (!vtxRTC6705CanUpdate());
 
     if (band && channel) {
         if (vtxRTC6705.powerIndex > 0) {
@@ -131,7 +140,7 @@ void vtxRTC6705SetBandAndChannel(uint8_t band, uint8_t channel)
 
 void vtxRTC6705SetPowerByIndex(uint8_t index)
 {
-    WAIT_FOR_VTX;
+    while (!vtxRTC6705CanUpdate());
 
 #ifdef RTC6705_POWER_PIN
     if (index == 0) {
diff --git a/src/main/io/vtx_rtc6705.h b/src/main/io/vtx_rtc6705.h
index 00bc26917..25b14c255 100644
--- a/src/main/io/vtx_rtc6705.h
+++ b/src/main/io/vtx_rtc6705.h
@@ -35,4 +35,5 @@
 extern const char * const rtc6705PowerNames[VTX_RTC6705_POWER_COUNT];
 
 void vtxRTC6705Configure(void);
+bool vtxRTC6705CanUpdate(void);
 bool vtxRTC6705Init(void);