diff --git a/src/drv_serial.c b/src/drv_serial.c
index ee55de4b35..93f9c23996 100644
--- a/src/drv_serial.c
+++ b/src/drv_serial.c
@@ -38,3 +38,8 @@ inline bool isSerialTransmitBufferEmpty(serialPort_t *instance)
     return instance->vTable->isSerialTransmitBufferEmpty(instance);
 }
 
+inline void serialSetMode(serialPort_t *instance, portMode_t mode)
+{
+    instance->vTable->setMode(instance, mode);
+}
+
diff --git a/src/drv_serial.h b/src/drv_serial.h
index 6d83b56c79..22adee4ce2 100644
--- a/src/drv_serial.h
+++ b/src/drv_serial.h
@@ -38,12 +38,15 @@ struct serialPortVTable {
     void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate);
     
     bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance);
+
+    void (*setMode)(serialPort_t *instance, portMode_t mode);
 };
 
 inline void serialWrite(serialPort_t *instance, uint8_t ch);
 inline uint8_t serialTotalBytesWaiting(serialPort_t *instance);
 inline uint8_t serialRead(serialPort_t *instance);
 inline void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate);
+void serialSetMode(serialPort_t *instance, portMode_t mode);
 inline bool isSerialTransmitBufferEmpty(serialPort_t *instance);
 void serialPrint(serialPort_t *instance, const char *str);
 uint32_t serialGetBaudRate(serialPort_t *instance);
diff --git a/src/drv_softserial.c b/src/drv_softserial.c
index 848cb7d1e7..d38395d985 100644
--- a/src/drv_softserial.c
+++ b/src/drv_softserial.c
@@ -120,11 +120,8 @@ void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
     softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP);
 }
 
-void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t baud, uint8_t inverted)
+void resetBuffers(softSerial_t *softSerial)
 {
-    softSerial->port.vTable = softSerialVTable;
-    softSerial->port.mode = MODE_RXTX;
-
     softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE;
     softSerial->port.rxBuffer = softSerial->rxBuffer;
     softSerial->port.rxBufferTail = 0;
@@ -134,6 +131,14 @@ void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t
     softSerial->port.txBufferSize = SOFT_SERIAL_BUFFER_SIZE,
     softSerial->port.txBufferTail = 0;
     softSerial->port.txBufferHead = 0;
+}
+
+void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t baud, uint8_t inverted)
+{
+    softSerial->port.vTable = softSerialVTable;
+    softSerial->port.mode = MODE_RXTX;
+
+    resetBuffers(softSerial);
 
     softSerial->isTransmittingData = false;
 
@@ -198,39 +203,50 @@ void updateBufferIndex(softSerial_t *softSerial)
 
 /*********************************************/
 
+void prepareToSendNextByte(softSerial_t *softSerial)
+{
+    char byteToSend;
+
+    if ((softSerial->port.mode & MODE_TX) == 0) {
+        return;
+    }
+
+    if (isSoftSerialTransmitBufferEmpty((serialPort_t *)softSerial)) {
+        return;
+    }
+
+    // choose data to send and update pointer to next byte
+    byteToSend = softSerial->port.txBuffer[softSerial->port.txBufferTail++];
+    if (softSerial->port.txBufferTail >= softSerial->port.txBufferSize) {
+        softSerial->port.txBufferTail = 0;
+    }
+
+    // build internal buffer, MSB = Stop Bit (1) + data bits (MSB to LSB) + start bit(0) LSB
+    softSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
+    softSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
+    softSerial->isTransmittingData = true;
+}
+
 void processTxState(softSerial_t *softSerial)
 {
     uint8_t mask;
 
-    if (!softSerial->isTransmittingData) {
-        char byteToSend;
-        if (isSoftSerialTransmitBufferEmpty((serialPort_t *)softSerial)) {
-            return;
-        }
+    if (softSerial->isTransmittingData) {
+        mask = softSerial->internalTxBuffer & 1;
+        softSerial->internalTxBuffer >>= 1;
 
-        // data to send
-        byteToSend = softSerial->port.txBuffer[softSerial->port.txBufferTail++];
-        if (softSerial->port.txBufferTail >= softSerial->port.txBufferSize) {
-            softSerial->port.txBufferTail = 0;
-        }
+        setTxSignal(softSerial, mask);
 
-        // build internal buffer, MSB = Stop Bit (1) + data bits (MSB to LSB) + start bit(0) LSB
-        softSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
-        softSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
-        softSerial->isTransmittingData = true;
+        if (--softSerial->bitsLeftToTransmit <= 0) {
+            softSerial->isTransmittingData = false;
+        }
         return;
     }
 
-    mask = softSerial->internalTxBuffer & 1;
-    softSerial->internalTxBuffer >>= 1;
-
-    setTxSignal(softSerial, mask);
-
-    if (--softSerial->bitsLeftToTransmit <= 0) {
-        softSerial->isTransmittingData = false;
-    }
+    prepareToSendNextByte(softSerial);
 }
 
+
 enum {
     TRAILING,
     LEADING
@@ -259,6 +275,10 @@ void prepareForNextRxByte(softSerial_t *softSerial)
 
 void extractAndStoreRxByte(softSerial_t *softSerial)
 {
+    if ((softSerial->port.mode & MODE_RX) == 0) {
+        return;
+    }
+
     uint8_t rxByte = (softSerial->internalRxBuffer >> 1) & 0xFF;
     softSerial->port.rxBuffer[softSerial->port.rxBufferTail] = rxByte;
     updateBufferIndex(softSerial);
@@ -295,6 +315,10 @@ void onSerialRxPinChange(uint8_t portIndex, uint16_t capture)
 {
     softSerial_t *softSerial = &(softSerialPorts[portIndex]);
 
+    if ((softSerial->port.mode & MODE_RX) == 0) {
+        return;
+    }
+
     if (softSerial->isSearchingForStartBit) {
         TIM_SetCounter(softSerial->rxTimerHardware->tim, 0); // synchronise bit counter
         serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->isInverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
@@ -324,6 +348,10 @@ void onSerialRxPinChange(uint8_t portIndex, uint16_t capture)
 
 uint8_t softSerialTotalBytesWaiting(serialPort_t *instance)
 {
+    if ((instance->mode & MODE_RX) == 0) {
+        return 0;
+    }
+
     int availableBytes;
     softSerial_t *softSerial = (softSerial_t *)instance;
     if (softSerial->port.rxBufferTail == softSerial->port.rxBufferHead) {
@@ -350,6 +378,11 @@ static void moveHeadToNextByte(softSerial_t *softSerial)
 uint8_t softSerialReadByte(serialPort_t *instance)
 {
     char b;
+
+    if ((instance->mode & MODE_RX) == 0) {
+        return 0;
+    }
+
     if (softSerialTotalBytesWaiting(instance) == 0) {
         return 0;
     }
@@ -362,6 +395,10 @@ uint8_t softSerialReadByte(serialPort_t *instance)
 
 void softSerialWriteByte(serialPort_t *s, uint8_t ch)
 {
+    if ((s->mode & MODE_TX) == 0) {
+        return;
+    }
+
     s->txBuffer[s->txBufferHead] = ch;
     s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
 }
@@ -371,6 +408,11 @@ void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
     // not implemented.
 }
 
+void softSerialSetMode(serialPort_t *instance, portMode_t mode)
+{
+    instance->mode = mode;
+}
+
 bool isSoftSerialTransmitBufferEmpty(serialPort_t *instance)
 {
     return instance->txBufferHead == instance->txBufferTail;
@@ -378,10 +420,11 @@ bool isSoftSerialTransmitBufferEmpty(serialPort_t *instance)
 
 const struct serialPortVTable softSerialVTable[] = {
     {
-        softSerialWriteByte, 
+        softSerialWriteByte,
         softSerialTotalBytesWaiting,
         softSerialReadByte,
         softSerialSetBaudRate,
-        isSoftSerialTransmitBufferEmpty
+        isSoftSerialTransmitBufferEmpty,
+        softSerialSetMode,
     }
 };
diff --git a/src/drv_uart.c b/src/drv_uart.c
index 7a1727f431..0bdc6253b2 100755
--- a/src/drv_uart.c
+++ b/src/drv_uart.c
@@ -202,6 +202,12 @@ void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate)
     s->port.baudRate = baudRate;
 }
 
+void uartSetMode(serialPort_t *s, portMode_t mode)
+{
+    // not implemented.
+}
+
+
 static void uartStartTxDMA(uartPort_t *s)
 {
     s->txDMAChannel->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail];
@@ -273,7 +279,8 @@ const struct serialPortVTable uartVTable[] = {
         uartTotalBytesWaiting,
         uartRead,
         uartSetBaudRate,
-        isUartTransmitBufferEmpty
+        isUartTransmitBufferEmpty,
+        uartSetMode,
     }
 };
 
diff --git a/src/main.c b/src/main.c
index 86f18bdfac..b4cfee5972 100755
--- a/src/main.c
+++ b/src/main.c
@@ -189,7 +189,7 @@ int main(void)
         if (loopbackPort2) {
             while (serialTotalBytesWaiting(loopbackPort2)) {
                 uint8_t b = serialRead(loopbackPort2);
-                serialWrite(loopbackPort1, b);
+                serialWrite(loopbackPort2, b);
                 //serialWrite(core.mainport, 0x02);
                 //serialWrite(core.mainport, b);
             };
diff --git a/src/mw.c b/src/mw.c
index 7214c36abc..4460bb68ec 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -182,7 +182,7 @@ void annexCode(void)
             LED0_ON;
         // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes. With telemetry_softserial>0 telemetry is always enabled, also see updateTelemetryState()
         if (feature(FEATURE_TELEMETRY))
-            updateTelemetryState();
+            checkTelemetryState();
     }
 
 #ifdef LEDRING
diff --git a/src/telemetry_common.c b/src/telemetry_common.c
index a595378367..5c2fee999b 100644
--- a/src/telemetry_common.c
+++ b/src/telemetry_common.c
@@ -4,6 +4,16 @@
 #include "telemetry_frsky.h"
 #include "telemetry_hott.h"
 
+bool isTelemetryProviderFrSky(void)
+{
+    return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY;
+}
+
+bool isTelemetryProviderHoTT(void)
+{
+    return mcfg.telemetry_provider == TELEMETRY_PROVIDER_HOTT;
+}
+
 void initTelemetry(void)
 {
     // Sanity check for softserial vs. telemetry port
@@ -18,9 +28,7 @@ void initTelemetry(void)
         core.telemport = core.mainport;
 }
 
-void updateTelemetryState(void) {
-    updateFrSkyTelemetryState();
-}
+static bool telemetryEnabled = false;
 
 bool isTelemetryEnabled(void)
 {
@@ -36,14 +44,45 @@ bool isTelemetryEnabled(void)
     return telemetryCurrentlyEnabled;
 }
 
-bool isFrSkyTelemetryEnabled(void)
+bool shouldChangeTelemetryStateNow(bool telemetryCurrentlyEnabled)
 {
-    return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY;
+    return telemetryCurrentlyEnabled != telemetryEnabled;
 }
 
-bool isHoTTTelemetryEnabled(void)
+void configureTelemetryPort(void) {
+    if (isTelemetryProviderFrSky()) {
+        configureFrSkyTelemetryPort();
+    }
+
+    if (isTelemetryProviderHoTT()) {
+        configureHoTTTelemetryPort();
+    }
+}
+
+void freeTelemetryPort(void) {
+    if (isTelemetryProviderFrSky()) {
+        freeFrSkyTelemetryPort();
+    }
+
+    if (isTelemetryProviderHoTT()) {
+        freeHoTTTelemetryPort();
+    }
+}
+
+void checkTelemetryState(void)
 {
-    return mcfg.telemetry_provider == TELEMETRY_PROVIDER_HOTT;
+    bool telemetryCurrentlyEnabled = isTelemetryEnabled();
+
+    if (!shouldChangeTelemetryStateNow(telemetryCurrentlyEnabled)) {
+        return;
+    }
+
+    if (telemetryCurrentlyEnabled)
+        configureTelemetryPort();
+    else
+        freeTelemetryPort();
+
+    telemetryEnabled = telemetryCurrentlyEnabled;
 }
 
 void handleTelemetry(void)
@@ -51,11 +90,11 @@ void handleTelemetry(void)
     if (!isTelemetryEnabled())
         return;
 
-    if (isFrSkyTelemetryEnabled()) {
+    if (isTelemetryProviderFrSky()) {
         handleFrSkyTelemetry();
     }
 
-    if (isHoTTTelemetryEnabled()) {
+    if (isTelemetryProviderHoTT()) {
         handleHoTTTelemetry();
     }
 }
diff --git a/src/telemetry_common.h b/src/telemetry_common.h
index defd43320a..5462132b3f 100644
--- a/src/telemetry_common.h
+++ b/src/telemetry_common.h
@@ -10,7 +10,7 @@
 
 // telemetry
 void initTelemetry(void);
-void updateTelemetryState(void);
+void checkTelemetryState(void);
 void handleTelemetry(void);
 bool isTelemetryEnabled(void);
 
diff --git a/src/telemetry_frsky.c b/src/telemetry_frsky.c
index a551ecde09..8d737d9f42 100644
--- a/src/telemetry_frsky.c
+++ b/src/telemetry_frsky.c
@@ -215,28 +215,18 @@ static void sendHeading(void)
     serialize16(0);
 }
 
-static bool frSkyTelemetryEnabled = false;
-
-bool shouldChangeTelemetryStateNow(bool frSkyTelemetryCurrentlyEnabled)
+void freeFrSkyTelemetryPort(void)
 {
-    return frSkyTelemetryCurrentlyEnabled != frSkyTelemetryEnabled;
+    if (mcfg.telemetry_port == TELEMETRY_PORT_UART) {
+        serialInit(mcfg.serial_baudrate);
+    }
 }
 
-void updateFrSkyTelemetryState(void)
+void configureFrSkyTelemetryPort(void)
 {
-    bool frSkyTelemetryCurrentlyEnabled = isTelemetryEnabled();
-
-    if (!shouldChangeTelemetryStateNow(frSkyTelemetryCurrentlyEnabled)) {
-        return;
+    if (mcfg.telemetry_port == TELEMETRY_PORT_UART) {
+        serialInit(9600);
     }
-
-    if (mcfg.telemetry_port == TELEMETRY_PORT_UART && mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY) {
-        if (frSkyTelemetryCurrentlyEnabled)
-            serialInit(9600);
-        else
-            serialInit(mcfg.serial_baudrate);
-    }
-    frSkyTelemetryEnabled = frSkyTelemetryCurrentlyEnabled;
 }
 
 static uint32_t lastCycleTime = 0;
diff --git a/src/telemetry_frsky.h b/src/telemetry_frsky.h
index 550b929324..086632253a 100644
--- a/src/telemetry_frsky.h
+++ b/src/telemetry_frsky.h
@@ -9,6 +9,9 @@
 #define TELEMETRY_FRSKY_H_
 
 void handleFrSkyTelemetry(void);
-void updateFrSkyTelemetryState(void);
+void checkFrSkyTelemetryState(void);
+
+void configureFrSkyTelemetryPort(void);
+void freeFrSkyTelemetryPort(void);
 
 #endif /* TELEMETRY_FRSKY_H_ */
diff --git a/src/telemetry_hott.c b/src/telemetry_hott.c
index 8bb8b03216..d02cee9712 100644
--- a/src/telemetry_hott.c
+++ b/src/telemetry_hott.c
@@ -216,9 +216,7 @@ void hottV4FormatAndSendEAMResponse(void) {
 
 static void hottV4Respond(uint8_t *data, uint8_t size) {
 
-    if (serialTotalBytesWaiting(core.telemport) != 0 ) {
-        return; // cannot respond since another request came in.
-    }
+    serialSetMode(core.telemport, MODE_TX);
 
     uint16_t crc = 0;
     uint8_t i;
@@ -235,16 +233,29 @@ static void hottV4Respond(uint8_t *data, uint8_t size) {
     hottV4SerialWrite(crc & 0xFF);
 
     delayMicroseconds(HOTTV4_TX_DELAY);
+
+    serialSetMode(core.telemport, MODE_RX);
 }
 
 static void hottV4SerialWrite(uint8_t c) {
   serialWrite(core.telemport, c);
 }
 
+void configureHoTTTelemetryPort(void) {
+    // TODO set speed here to 19200
+    serialSetMode(core.telemport, MODE_RX);
+}
+
+void freeHoTTTelemetryPort(void) {
+    serialSetMode(core.telemport, MODE_RXTX);
+}
+
 void handleHoTTTelemetry(void)
 {
-    while (serialTotalBytesWaiting(core.telemport)) {
-      uint8_t c = serialRead(core.telemport);
+    uint8_t c;
+
+    while (serialTotalBytesWaiting(core.telemport) > 0) {
+      c = serialRead(core.telemport);
 
       // Protocol specific waiting time
       // to avoid collisions
diff --git a/src/telemetry_hott.h b/src/telemetry_hott.h
index 352452cb9f..fef7546860 100644
--- a/src/telemetry_hott.h
+++ b/src/telemetry_hott.h
@@ -198,5 +198,9 @@ struct {
 } HoTTV4ElectricAirModule;
 
 void handleHoTTTelemetry(void);
+void checkTelemetryState(void);
+
+void configureHoTTTelemetryPort(void);
+void freeHoTTTelemetryPort(void);
 
 #endif /* TELEMETRY_HOTT_H_ */