diff --git a/src/cli.c b/src/cli.c
index 7b70d90280..2d7c0a63f7 100644
--- a/src/cli.c
+++ b/src/cli.c
@@ -126,8 +126,9 @@ const clivalue_t valueTable[] = {
     { "flaps_speed", VAR_UINT8, &mcfg.flaps_speed, 0, 100 },
     { "fixedwing_althold_dir", VAR_INT8, &mcfg.fixedwing_althold_dir, -1, 1 },
     { "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 },
-    { "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 600, 19200 },
-    { "softserial_inverted", VAR_UINT8, &mcfg.softserial_inverted, 0, 1 },
+    { "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 1200, 19200 },
+    { "softserial_1_inverted", VAR_UINT8, &mcfg.softserial_1_inverted, 0, 1 },
+    { "softserial_2_inverted", VAR_UINT8, &mcfg.softserial_2_inverted, 0, 1 },
     { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 },
     { "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, -1, 4 },
     { "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 3 },
@@ -834,12 +835,12 @@ static void cliSave(char *cmdline)
 static void cliPrint(const char *str)
 {
     while (*str)
-        uartWrite(core.mainport, *(str++));
+        serialWrite(core.mainport, *(str++));
 }
 
 static void cliWrite(uint8_t ch)
 {
-    uartWrite(core.mainport, ch);
+    serialWrite(core.mainport, ch);
 }
 
 static void cliPrintVar(const clivalue_t *var, uint32_t full)
diff --git a/src/config.c b/src/config.c
index 75d17db4a7..9004002adc 100755
--- a/src/config.c
+++ b/src/config.c
@@ -13,7 +13,7 @@ master_t mcfg;  // master config struct with data independent from profiles
 config_t cfg;   // profile config struct
 const char rcChannelLetters[] = "AERT1234";
 
-static const uint8_t EEPROM_CONF_VERSION = 58;
+static const uint8_t EEPROM_CONF_VERSION = 59;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 
@@ -216,7 +216,8 @@ static void resetConf(void)
     // serial (USART1) baudrate
     mcfg.serial_baudrate = 115200;
     mcfg.softserial_baudrate = 19200;
-    mcfg.softserial_inverted = 0;
+    mcfg.softserial_1_inverted = 0;
+    mcfg.softserial_2_inverted = 0;
     mcfg.looptime = 3500;
     mcfg.rssi_aux_channel = 0;
 
diff --git a/src/drv_pwm.c b/src/drv_pwm.c
index 46c258461e..068ad2adbd 100755
--- a/src/drv_pwm.c
+++ b/src/drv_pwm.c
@@ -236,7 +236,7 @@ static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uin
     pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPD);
     pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 
-    timerInConfig(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ);
+    timerConfigure(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ);
     configureTimerCaptureCompareInterrupt(timerHardwarePtr, port, callback);
 
     return p;
diff --git a/src/drv_softserial.c b/src/drv_softserial.c
index cec19ee6e2..36eb4522d6 100644
--- a/src/drv_softserial.c
+++ b/src/drv_softserial.c
@@ -3,14 +3,18 @@
 #define SOFT_SERIAL_TIMER_MHZ 3
 #define SOFT_SERIAL_1_TIMER_RX_HARDWARE 4
 #define SOFT_SERIAL_1_TIMER_TX_HARDWARE 5
+#define SOFT_SERIAL_2_TIMER_RX_HARDWARE 6
+#define SOFT_SERIAL_2_TIMER_TX_HARDWARE 7
 
-#define RX_TOTAL_BITS 8
+#define RX_TOTAL_BITS 10
 #define TX_TOTAL_BITS 10
 
 #define MAX_SOFTSERIAL_PORTS 2
 softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
 
+
 void onSerialTimer(uint8_t portIndex, uint16_t capture);
+void onSerialRxPinChange(uint8_t portIndex, uint16_t capture);
 
 uint8_t readRxSignal(softSerial_t *softSerial)
 {
@@ -69,19 +73,17 @@ void serialInputPortConfig(const timerHardware_t *timerHardwarePtr)
     softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
 }
 
-#define TICKS_PER_BIT 3
-
 bool isTimerPeriodTooLarge(uint32_t timerPeriod)
 {
     return timerPeriod > 0xFFFF;
 }
 
-void serialTimerConfig(const timerHardware_t *timerHardwarePtr, uint32_t baud, uint8_t reference, timerCCCallbackPtr callback)
+void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud)
 {
     uint32_t clock = SystemCoreClock;
     uint32_t timerPeriod;
     do {
-        timerPeriod = clock / (baud * TICKS_PER_BIT);
+        timerPeriod = clock / baud;
         if (isTimerPeriodTooLarge(timerPeriod)) {
             if (clock > 1) {
                 clock = clock / 2;
@@ -93,8 +95,29 @@ void serialTimerConfig(const timerHardware_t *timerHardwarePtr, uint32_t baud, u
     } while (isTimerPeriodTooLarge(timerPeriod));
 
     uint8_t mhz = SystemCoreClock / 1000000;
-    timerInConfig(timerHardwarePtr, timerPeriod, mhz);
-    configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback);
+    timerConfigure(timerHardwarePtr, timerPeriod, mhz);
+    configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, onSerialTimer);
+}
+
+void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
+{
+    TIM_ICInitTypeDef  TIM_ICInitStructure;
+
+    TIM_ICStructInit(&TIM_ICInitStructure);
+    TIM_ICInitStructure.TIM_Channel = channel;
+    TIM_ICInitStructure.TIM_ICPolarity = polarity;
+    TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+    TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+    TIM_ICInitStructure.TIM_ICFilter = 0x0;
+
+    TIM_ICInit(tim, &TIM_ICInitStructure);
+}
+
+void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
+{
+    // start bit is a FALLING signal
+    serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
+    configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, onSerialRxPinChange);
 }
 
 void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
@@ -102,18 +125,10 @@ void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
     softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP);
 }
 
-void setupSoftSerial1(uint32_t baud, uint8_t inverted)
+void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t baud, uint8_t inverted)
 {
-    int portIndex = 0;
-    softSerial_t *softSerial = &(softSerialPorts[portIndex]);
-
     softSerial->port.vTable = softSerialVTable;
     softSerial->port.mode = MODE_RXTX;
-    softSerial->port.baudRate = baud;
-    softSerial->isInverted = inverted;
-
-    softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_RX_HARDWARE]);
-    softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_TX_HARDWARE]);
 
     softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE;
     softSerial->port.rxBuffer = softSerial->rxBuffer;
@@ -126,20 +141,56 @@ void setupSoftSerial1(uint32_t baud, uint8_t inverted)
     softSerial->port.txBufferHead = 0;
 
     softSerial->isTransmittingData = false;
+
     softSerial->isSearchingForStartBit = true;
-    softSerial->isSearchingForStopBit = false;
+    softSerial->rxBitIndex = 0;
 
-    softSerial->timerRxCounter = 1;
-
-    serialInputPortConfig(softSerial->rxTimerHardware);
     serialOutputPortConfig(softSerial->txTimerHardware);
+    serialInputPortConfig(softSerial->rxTimerHardware);
 
-    setTxSignal(softSerial, 1);
+    setTxSignal(softSerial, ENABLE);
     delay(50);
 
-    serialTimerConfig(softSerial->rxTimerHardware, baud, portIndex, onSerialTimer);
+    serialTimerTxConfig(softSerial->txTimerHardware, portIndex, baud);
+    serialTimerRxConfig(softSerial->rxTimerHardware, portIndex);
 }
 
+typedef struct softSerialConfiguration_s {
+    uint32_t sharedBaudRate;
+    bool primaryPortInitialised;
+} softSerialConfiguration_t;
+
+softSerialConfiguration_t softSerialConfiguration = {
+    0,
+    false
+};
+
+void setupSoftSerialPrimary(uint32_t baud, uint8_t inverted)
+{
+    uint8_t portIndex = 0;
+    softSerial_t *softSerial = &(softSerialPorts[portIndex]);
+
+    softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_RX_HARDWARE]);
+    softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_TX_HARDWARE]);
+
+    initialiseSoftSerial(softSerial, portIndex, baud, inverted);
+
+    softSerialConfiguration.sharedBaudRate = baud;
+    softSerialConfiguration.primaryPortInitialised = true;
+}
+
+void setupSoftSerialSecondary(uint8_t inverted)
+{
+    int portIndex = 1;
+    softSerial_t *softSerial = &(softSerialPorts[portIndex]);
+
+    softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_2_TIMER_RX_HARDWARE]);
+    softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_2_TIMER_TX_HARDWARE]);
+
+    initialiseSoftSerial(softSerial, portIndex, softSerialConfiguration.sharedBaudRate, inverted);
+}
+
+
 void updateBufferIndex(softSerial_t *softSerial)
 {
     if (softSerial->port.rxBufferTail >= softSerial->port.rxBufferSize - 1) {
@@ -151,81 +202,6 @@ void updateBufferIndex(softSerial_t *softSerial)
 
 /*********************************************/
 
-void searchForStartBit(softSerial_t *softSerial)
-{
-    char rxSignal = readRxSignal(softSerial);
-    if (rxSignal == 1) {
-        // start bit not found
-        softSerial->timerRxCounter = 1; // process on next timer event
-        return;
-    }
-
-    // timer is aligned to falling signal of start bit.
-    // three ticks per bit.
-
-    softSerial->isSearchingForStartBit = false;
-    softSerial->internalRxBuffer = 0;
-    softSerial->timerRxCounter = TICKS_PER_BIT + 1; // align to middle of next bit
-    softSerial->bitsLeftToReceive = RX_TOTAL_BITS;
-    softSerial->rxBitSelectionMask = 1;
-}
-
-void searchForStopBit(softSerial_t *softSerial)
-{
-    char rxSignal;
-    softSerial->timerRxCounter = 1;
-
-    rxSignal = readRxSignal(softSerial);
-    if (rxSignal != 1) {
-        // not found
-        return;
-    }
-
-    softSerial->isSearchingForStopBit = false;
-    softSerial->isSearchingForStartBit = true;
-    softSerial->internalRxBuffer &= 0xFF;
-
-    softSerial->port.rxBuffer[softSerial->port.rxBufferTail] = softSerial->internalRxBuffer;
-    updateBufferIndex(softSerial);
-}
-
-void readDataBit(softSerial_t *softSerial)
-{
-    softSerial->timerRxCounter = TICKS_PER_BIT; // keep aligned to middle of bit
-
-    char rxSignal = readRxSignal(softSerial);
-    if (rxSignal) {
-        softSerial->internalRxBuffer |= softSerial->rxBitSelectionMask;
-    }
-    softSerial->rxBitSelectionMask <<= 1;
-    if (--softSerial->bitsLeftToReceive <= 0) {
-        softSerial->isSearchingForStopBit = true;
-        softSerial->timerRxCounter = 2;
-    }
-}
-
-void processRxState(softSerial_t *softSerial)
-{
-    //digitalToggle(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
-
-    if (--softSerial->timerRxCounter > 0) {
-        return;
-    }
-
-    if (softSerial->isSearchingForStartBit) {
-        searchForStartBit(softSerial);
-        return;
-    }
-
-    if (softSerial->isSearchingForStopBit) {
-        searchForStopBit(softSerial);
-        return;
-    }
-
-    readDataBit(softSerial);
-}
-
-
 void processTxState(softSerial_t *softSerial)
 {
     char mask;
@@ -245,26 +221,72 @@ void processTxState(softSerial_t *softSerial)
         // build internal buffer, start bit(1) + data bits + stop bit(0)
         softSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
         softSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
-
-        // start immediately
-        softSerial->timerTxCounter = 1;
         softSerial->isTransmittingData = true;
         return;
     }
 
-    if (--softSerial->timerTxCounter <= 0) {
-        mask = softSerial->internalTxBuffer & 1;
-        softSerial->internalTxBuffer >>= 1;
+    mask = softSerial->internalTxBuffer & 1;
+    softSerial->internalTxBuffer >>= 1;
 
-        setTxSignal(softSerial, mask);
+    setTxSignal(softSerial, mask);
 
-        softSerial->timerTxCounter = TICKS_PER_BIT;
-        if (--softSerial->bitsLeftToTransmit <= 0) {
-            softSerial->isTransmittingData = false;
+    if (--softSerial->bitsLeftToTransmit <= 0) {
+        softSerial->isTransmittingData = false;
+    }
+}
+
+enum {
+    FALLING,
+    RISING
+};
+
+void applyChangedBits(softSerial_t *softSerial)
+{
+    if (softSerial->rxPinMode == FALLING) {
+        uint8_t bitToSet;
+        for (bitToSet = softSerial->rxLastRiseAtBitIndex; bitToSet < softSerial->rxBitIndex ; bitToSet++) {
+            softSerial->internalRxBuffer |= 1 << bitToSet;
         }
     }
 }
 
+void prepareForNextRxByte(softSerial_t *softSerial)
+{
+    // prepare for next byte
+    softSerial->rxBitIndex = 0;
+    softSerial->isSearchingForStartBit = true;
+    if (softSerial->rxPinMode == RISING) {
+        softSerial->rxPinMode = FALLING;
+        serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling);
+    }
+}
+
+void extractAndStoreRxByte(softSerial_t *softSerial)
+{
+    uint8_t rxByte = (softSerial->internalRxBuffer >> 1) & 0xFF;
+    softSerial->port.rxBuffer[softSerial->port.rxBufferTail] = rxByte;
+    updateBufferIndex(softSerial);
+}
+
+void processRxState(softSerial_t *softSerial)
+{
+    if (softSerial->isSearchingForStartBit) {
+        return;
+    }
+
+    softSerial->rxBitIndex++;
+
+    if (softSerial->rxBitIndex == RX_TOTAL_BITS - 1) {
+        applyChangedBits(softSerial);
+        return;
+    }
+
+    if (softSerial->rxBitIndex == RX_TOTAL_BITS) {
+        extractAndStoreRxByte(softSerial);
+        prepareForNextRxByte(softSerial);
+    }
+}
+
 void onSerialTimer(uint8_t portIndex, uint16_t capture)
 {
     softSerial_t *softSerial = &(softSerialPorts[portIndex]);
@@ -273,6 +295,36 @@ void onSerialTimer(uint8_t portIndex, uint16_t capture)
     processRxState(softSerial);
 }
 
+void onSerialRxPinChange(uint8_t portIndex, uint16_t capture)
+{
+    softSerial_t *softSerial = &(softSerialPorts[portIndex]);
+
+    if (softSerial->isSearchingForStartBit) {
+        TIM_SetCounter(softSerial->rxTimerHardware->tim, 0); // synchronise bit counter
+        serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising);
+        softSerial->rxPinMode = RISING;
+
+        softSerial->rxBitIndex = 0;
+        softSerial->rxLastRiseAtBitIndex = 0;
+        softSerial->internalRxBuffer = 0;
+        softSerial->isSearchingForStartBit = false;
+        return;
+    }
+
+    if (softSerial->rxPinMode == RISING) {
+        softSerial->rxLastRiseAtBitIndex = softSerial->rxBitIndex;
+    }
+
+    applyChangedBits(softSerial);
+
+    if (softSerial->rxPinMode == FALLING) {
+        softSerial->rxPinMode = RISING;
+        serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising);
+    } else {
+        softSerial->rxPinMode = FALLING;
+        serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling);
+    }
+}
 
 uint8_t softSerialTotalBytesWaiting(serialPort_t *instance)
 {
diff --git a/src/drv_softserial.h b/src/drv_softserial.h
index c16135c4f1..dc1fa59eb0 100644
--- a/src/drv_softserial.h
+++ b/src/drv_softserial.h
@@ -18,16 +18,16 @@ typedef struct softSerial_s {
     const timerHardware_t *txTimerHardware;
     volatile uint8_t txBuffer[SOFT_SERIAL_BUFFER_SIZE];
     
-    uint8_t          isSearchingForStopBit;
-    uint8_t          rxBitSelectionMask;
     uint8_t          isSearchingForStartBit;
+    uint8_t          rxBitIndex;
+    uint8_t          rxLastRiseAtBitIndex;
+    uint8_t          rxPinMode;
+
     uint8_t          isTransmittingData;
-    uint8_t          timerRxCounter;
-    uint8_t          timerTxCounter;
-    uint8_t          bitsLeftToReceive;
     uint8_t          bitsLeftToTransmit;
-    uint16_t         internalRxBuffer;  // excluding start/stop bits
+
     uint16_t         internalTxBuffer;  // includes start and stop bits
+    uint16_t         internalRxBuffer;  // includes start and stop bits
 
     uint8_t          isInverted;
 } softSerial_t;
@@ -37,7 +37,8 @@ extern softSerial_t softSerialPorts[];
 
 extern const struct serialPortVTable softSerialVTable[];
 
-void setupSoftSerial1(uint32_t baud, uint8_t inverted);
+void setupSoftSerialPrimary(uint32_t baud, uint8_t inverted);
+void setupSoftSerialSecondary(uint8_t inverted);
 
 // serialPort API
 void softSerialWriteByte(serialPort_t *instance, uint8_t ch);
diff --git a/src/drv_timer.c b/src/drv_timer.c
index 2c2a884880..00958eb9fc 100644
--- a/src/drv_timer.c
+++ b/src/drv_timer.c
@@ -134,7 +134,7 @@ void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwareP
     configureTimerInputCaptureCompareChannel(timerHardwarePtr->tim, timerHardwarePtr->channel);
 }
 
-void timerNVICConfig(uint8_t irq)
+void timerNVICConfigure(uint8_t irq)
 {
     NVIC_InitTypeDef NVIC_InitStructure;
 
@@ -162,11 +162,11 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
     TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);
 }
 
-void timerInConfig(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
+void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
 {
     configTimeBase(timerHardwarePtr->tim, period, mhz);
     TIM_Cmd(timerHardwarePtr->tim, ENABLE);
-    timerNVICConfig(timerHardwarePtr->irq);
+    timerNVICConfigure(timerHardwarePtr->irq);
 }
 
 
diff --git a/src/drv_timer.h b/src/drv_timer.h
index cae2e20a02..c198b7d227 100644
--- a/src/drv_timer.h
+++ b/src/drv_timer.h
@@ -14,8 +14,8 @@ typedef struct {
 extern const timerHardware_t timerHardware[];
 
 void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz);
-void timerInConfig(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz);
-void timerNVICConfig(uint8_t irq);
+void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz);
+void timerNVICConfigure(uint8_t irq);
 
 void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel);
 void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback);
diff --git a/src/main.c b/src/main.c
index ee6a1ab73f..70c98d37ee 100755
--- a/src/main.c
+++ b/src/main.c
@@ -30,8 +30,10 @@ int main(void)
     uint8_t i;
     drv_pwm_config_t pwm_params;
     drv_adc_config_t adc_params;
-    serialPort_t* loopbackPort = NULL;
-
+#ifdef SOFTSERIAL_LOOPBACK
+    serialPort_t* loopbackPort1 = NULL;
+    serialPort_t* loopbackPort2 = NULL;
+#endif
     systemInit();
 #ifdef USE_LAME_PRINTF
     init_printf(NULL, _putc);
@@ -148,11 +150,19 @@ int main(void)
     serialInit(mcfg.serial_baudrate);
 
     if (feature(FEATURE_SOFTSERIAL)) {
-      setupSoftSerial1(mcfg.softserial_baudrate, mcfg.softserial_inverted);
+        //mcfg.softserial_baudrate = 19200; // Uncomment to override config value
+
+        setupSoftSerialPrimary(mcfg.softserial_baudrate, mcfg.softserial_1_inverted);
+        setupSoftSerialSecondary(mcfg.softserial_2_inverted);
+
 #ifdef SOFTSERIAL_LOOPBACK
-      loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
-      serialPrint(loopbackPort, "LOOPBACK ENABLED\r\n");
+        loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]);
+        serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n");
+
+        loopbackPort2 = (serialPort_t*)&(softSerialPorts[1]);
+        serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n");
 #endif
+        //core.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial.
     }
 
     if (feature(FEATURE_TELEMETRY))
@@ -169,11 +179,19 @@ int main(void)
     while (1) {
         loop();
 #ifdef SOFTSERIAL_LOOPBACK
-        if (loopbackPort) {
-            while (serialTotalBytesWaiting(loopbackPort)) {
-    
-                uint8_t b = serialRead(loopbackPort);
-                serialWrite(loopbackPort, b);
+        if (loopbackPort1) {
+            while (serialTotalBytesWaiting(loopbackPort1)) {
+                uint8_t b = serialRead(loopbackPort1);
+                serialWrite(loopbackPort1, b);
+                //serialWrite(core.mainport, 0x01);
+                //serialWrite(core.mainport, b);
+            };
+        }
+        if (loopbackPort2) {
+            while (serialTotalBytesWaiting(loopbackPort2)) {
+                uint8_t b = serialRead(loopbackPort2);
+                serialWrite(loopbackPort1, b);
+                //serialWrite(core.mainport, 0x02);
                 //serialWrite(core.mainport, b);
             };
         }
diff --git a/src/mw.h b/src/mw.h
index 42f2cf4f10..d1f833d64c 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -276,8 +276,9 @@ typedef struct master_t {
 
     uint32_t serial_baudrate;
 
-    uint32_t softserial_baudrate;
-    uint8_t softserial_inverted;            // use inverted softserial input and output signals
+    uint32_t softserial_baudrate;             // shared by both soft serial ports
+    uint8_t softserial_1_inverted;            // use inverted softserial input and output signals on port 1
+    uint8_t softserial_2_inverted;            // use inverted softserial input and output signals on port 2
 
     uint8_t telemetry_softserial;           // Serial to use for Telemetry. 0:USART1, 1:SoftSerial1 (Enable FEATURE_SOFTSERIAL first)
     uint8_t telemetry_switch;               // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.