diff --git a/src/drv_pwm.c b/src/drv_pwm.c
index fb4eb9604..77dfc20aa 100755
--- a/src/drv_pwm.c
+++ b/src/drv_pwm.c
@@ -325,7 +325,7 @@ bool pwmInit(drv_pwm_config_t *init)
 
 #ifdef SOFTSERIAL_19200_LOOPBACK
         // skip softSerial ports
-        if ((port == PWM5 || port == PWM6))
+        if ((port == PWM5 || port == PWM6 || port == PWM7 || port == PWM8))
             continue;
 #endif
 
diff --git a/src/drv_softserial.c b/src/drv_softserial.c
index 7ddf08ca6..b8a4cbf97 100644
--- a/src/drv_softserial.c
+++ b/src/drv_softserial.c
@@ -1,12 +1,34 @@
 #include "board.h"
 
-enum serialBitStatus {
-    WAITING_FOR_START_BIT = -1, BIT_0, BIT_1, BIT_2, BIT_3, BIT_4, BIT_5, BIT_6, BIT_7, WAITING_FOR_STOP_BIT
-};
+// There needs to be a table of timerMhz per baud rate so the system can set the timer correctly.
+// See http://www.wormfood.net/avrbaudcalc.php?postbitrate=19200&postclock=72
+// Currently defaulting to 3Mhz to support 19200.
+#define SOFT_SERIAL_TIMER_MHZ 3
+#define SOFT_SERIAL_1_TIMER_RX_HARDWARE 4
+#define SOFT_SERIAL_1_TIMER_TX_HARDWARE 5
+
+#define RX_TOTAL_BITS 8
+#define TX_TOTAL_BITS 10
 
 #define MAX_SOFTSERIAL_PORTS 2
 softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
 
+void onSerialTimer(uint8_t portIndex, uint16_t capture);
+
+uint8_t readRxSignal(softSerial_t *softSerial)
+{
+    return GPIO_ReadInputDataBit(softSerial->rxTimerHardware->gpio, softSerial->rxTimerHardware->pin);
+}
+
+void setTxSignal(softSerial_t *softSerial, uint8_t state)
+{
+    if (state) {
+        digitalHi(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
+    } else {
+        digitalLo(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
+    }
+}
+
 softSerial_t* lookupSoftSerial(uint8_t reference)
 {
     assert_param(reference >= 0 && reference <= MAX_SOFTSERIAL_PORTS);
@@ -14,50 +36,90 @@ softSerial_t* lookupSoftSerial(uint8_t reference)
     return &(softSerialPorts[reference]);
 }
 
+void resetSerialTimer(softSerial_t *softSerial)
+{
+    //uint16_t counter = TIM_GetCounter(softSerial->rxTimerHardware->tim);
+    TIM_SetCounter(softSerial->rxTimerHardware->tim, 0);
+    //counter = TIM_GetCounter(softSerial->rxTimerHardware->tim);
+}
 
 void stopSerialTimer(softSerial_t *softSerial)
 {
-    TIM_Cmd(softSerial->timerHardware->tim, DISABLE);
-    TIM_SetCounter(softSerial->timerHardware->tim, 0);
+    TIM_Cmd(softSerial->rxTimerHardware->tim, DISABLE);
 }
 
 void startSerialTimer(softSerial_t *softSerial)
 {
-    TIM_SetCounter(softSerial->timerHardware->tim, 0);
-    TIM_Cmd(softSerial->timerHardware->tim, ENABLE);
+    TIM_Cmd(softSerial->rxTimerHardware->tim, ENABLE);
 }
 
-static void waitForFirstBit(softSerial_t *softSerial)
+static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
 {
-    softSerial->state = BIT_0;
-    startSerialTimer(softSerial);
+    gpio_config_t cfg;
+
+    cfg.pin = pin;
+    cfg.mode = mode;
+    cfg.speed = Speed_2MHz;
+    gpioInit(gpio, &cfg);
 }
 
-static void onSerialPinChange(uint8_t reference, uint16_t capture)
+void serialInputPortConfig(const timerHardware_t *timerHardwarePtr)
 {
-    softSerial_t *softSerial = lookupSoftSerial(reference);
-    if (softSerial->state == WAITING_FOR_START_BIT) {
-        waitForFirstBit(softSerial);
-    }
+    softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
 }
 
-uint8_t readSerialSignal(softSerial_t *softSerial)
+#define TICKS_PER_BIT 3
+
+void serialTimerConfig(const timerHardware_t *timerHardwarePtr, uint32_t baud, uint8_t reference, timerCCCallbackPtr callback)
 {
-    return GPIO_ReadInputDataBit(softSerial->timerHardware->gpio, softSerial->timerHardware->pin);
+    uint16_t timerPeriod = (SOFT_SERIAL_TIMER_MHZ * 1000000) / (baud * TICKS_PER_BIT);
+    timerInConfig(timerHardwarePtr, timerPeriod, SOFT_SERIAL_TIMER_MHZ);
+    configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback);
 }
 
-void mergeSignalWithCurrentByte(softSerial_t *softSerial, uint8_t serialSignal)
+void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
 {
-    softSerial->rxBuffer[softSerial->port.rxBufferTail] |= (serialSignal << softSerial->state);
+    softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP);
 }
 
-inline void initialiseCurrentByteWithFirstSignal(softSerial_t *softSerial, uint8_t serialSignal)
+void setupSoftSerial1(uint32_t baud)
 {
-    softSerial->rxBuffer[softSerial->port.rxBufferTail] = serialSignal;
-}
+    int portIndex = 0;
+    softSerial_t *softSerial = &(softSerialPorts[portIndex]);
 
-inline void prepareForNextSignal(softSerial_t *softSerial) {
-    softSerial->state++;
+    softSerial->port.mode = MODE_RXTX;
+    softSerial->port.baudRate = baud;
+
+    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;
+    softSerial->port.rxBufferTail = 0;
+    softSerial->port.rxBufferHead = 0;
+
+    softSerial->port.txBuffer = softSerial->txBuffer;
+    softSerial->port.txBufferSize = SOFT_SERIAL_BUFFER_SIZE,
+    softSerial->port.txBufferTail = 0;
+    softSerial->port.txBufferHead = 0;
+
+    // FIXME this uart specific initialisation doesn't belong really here
+    softSerial->port.txDMAChannel = 0;
+    softSerial->port.txDMAChannel = 0;
+
+    softSerial->isTransmittingData = false;
+    softSerial->isSearchingForStartBit = true;
+    softSerial->isSearchingForStopBit = false;
+
+    softSerial->timerRxCounter = 1;
+
+    serialInputPortConfig(softSerial->rxTimerHardware);
+    serialOutputPortConfig(softSerial->txTimerHardware);
+
+    setTxSignal(softSerial, 1);
+    delay(50);
+
+    serialTimerConfig(softSerial->rxTimerHardware, baud, portIndex, onSerialTimer);
 }
 
 void updateBufferIndex(softSerial_t *softSerial)
@@ -70,103 +132,133 @@ void updateBufferIndex(softSerial_t *softSerial)
     }
 }
 
-void prepareForNextByte(softSerial_t *softSerial)
+/*********************************************/
+
+void searchForStartBit(softSerial_t *softSerial)
 {
-    stopSerialTimer(softSerial);
-    softSerial->state = WAITING_FOR_START_BIT;
+    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)
+{
+    softSerial->timerRxCounter = 1;
+
+    char 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;
+
+    if (!softSerial->isTransmittingData) {
+
+        if (softSerial->port.txBufferHead == softSerial->port.txBufferTail) {
+            return;
+        }
+
+        // data to send
+
+        char byteToSend = softSerial->port.txBuffer[softSerial->port.txBufferTail++];
+        if (softSerial->port.txBufferTail >= softSerial->port.txBufferSize) {
+    softSerial->port.txBufferTail = 0;
+        }
+
+        // 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;
+
+        setTxSignal(softSerial, mask);
+
+        softSerial->timerTxCounter = TICKS_PER_BIT;
+        if (--softSerial->bitsLeftToTransmit <= 0) {
+            softSerial->isTransmittingData = false;
+        }
+    }
+}
+
 void onSerialTimer(uint8_t portIndex, uint16_t capture)
 {
     softSerial_t *softSerial = &(softSerialPorts[portIndex]);
 
-    uint8_t serialSignal = readSerialSignal(softSerial);
-
-    switch (softSerial->state) {
-    case BIT_0:
-        initialiseCurrentByteWithFirstSignal(softSerial, serialSignal);
-        prepareForNextSignal(softSerial);
-        break;
-
-    case BIT_1:
-    case BIT_2:
-    case BIT_3:
-    case BIT_4:
-    case BIT_5:
-    case BIT_6:
-    case BIT_7:
-        mergeSignalWithCurrentByte(softSerial, serialSignal);
-
-        prepareForNextSignal(softSerial);
-        break;
-
-    case WAITING_FOR_STOP_BIT:
-        prepareForNextByte(softSerial);
-        break;
-    }
+    processTxState(softSerial);
+    processRxState(softSerial);
 }
 
-#define SOFT_SERIAL_TIMER_MHZ 1
-#define SOFT_SERIAL_1_TIMER_HARDWARE 4
 
-static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
-{
-    gpio_config_t cfg;
-
-    cfg.pin = pin;
-    cfg.mode = mode;
-    cfg.speed = Speed_2MHz;
-    gpioInit(gpio, &cfg);
-}
-
-void serialInputPortConfig(const timerHardware_t *timerHardwarePtr, uint16_t baud, uint8_t reference, timerCCCallbackPtr callback)
-{
-    softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
-
-    pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
-
-    int oneBitPeriod = (SOFT_SERIAL_TIMER_MHZ * 1000000) / baud;
-
-    timerInConfig(timerHardwarePtr, oneBitPeriod, SOFT_SERIAL_TIMER_MHZ);
-    configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback);
-}
-
-void setupSoftSerial1(uint32_t baud)
-{
-    int portIndex = 0;
-    softSerial_t *softSerial = &(softSerialPorts[portIndex]);
-
-    softSerial->timerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_HARDWARE]);
-    softSerial->timerIndex = SOFT_SERIAL_1_TIMER_HARDWARE;
-    softSerial->state = WAITING_FOR_START_BIT;
-
-    softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE;
-    softSerial->port.rxBuffer = softSerial->rxBuffer;
-    softSerial->port.rxBufferTail = 0;
-    softSerial->port.rxBufferHead = 0;
-
-    softSerial->port.txBuffer = 0;
-    softSerial->port.txBufferSize = 0;
-    softSerial->port.txBufferTail = 0;
-    softSerial->port.txBufferHead = 0;
-
-    softSerial->port.baudRate = baud;
-    softSerial->port.mode = MODE_RX;
-
-    // FIXME this uart specific initialisation doesn't belong really here
-    softSerial->port.txDMAChannel = 0;
-    softSerial->port.txDMAChannel = 0;
-
-    configureTimerChannelCallback(softSerial->timerHardware->tim, TIM_Channel_2, portIndex, onSerialTimer);
-
-    TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE);
-    stopSerialTimer(softSerial);
-
-    serialInputPortConfig(softSerial->timerHardware, baud, portIndex, onSerialPinChange);
-}
-
-bool serialAvailable(softSerial_t *softSerial)
+uint8_t serialAvailable(softSerial_t *softSerial)
 {
     if (softSerial->port.rxBufferTail == softSerial->port.rxBufferHead) {
         return 0;
@@ -201,3 +293,19 @@ uint8_t serialReadByte(softSerial_t *softSerial)
     moveHeadToNextByte(softSerial);
     return b;
 }
+void serialWriteByte(softSerial_t *softSerial, uint8_t ch)
+{
+    serialPort_t *s = &(softSerial->port);
+
+    s->txBuffer[s->txBufferHead] = ch;
+    s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
+
+}
+
+void serialPrint(softSerial_t *softSerial, const char *str)
+{
+    uint8_t ch;
+    while ((ch = *(str++))) {
+        serialWriteByte(softSerial, ch);
+    }
+}
diff --git a/src/drv_softserial.h b/src/drv_softserial.h
index 720d2fab7..fb7d55bf5 100644
--- a/src/drv_softserial.h
+++ b/src/drv_softserial.h
@@ -10,16 +10,33 @@
 #define SOFT_SERIAL_BUFFER_SIZE 256
 
 typedef struct softSerial_s {
-    const timerHardware_t *timerHardware;
-    uint8_t timerIndex;
     serialPort_t port;
-    volatile int state;
+
+    const timerHardware_t *rxTimerHardware;
     volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE];
+
+    const timerHardware_t *txTimerHardware;
+    volatile uint8_t txBuffer[SOFT_SERIAL_BUFFER_SIZE];
+    
+    uint8_t          isSearchingForStopBit;
+    uint8_t          rxBitSelectionMask;
+    uint8_t          isSearchingForStartBit;
+    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
+
 } softSerial_t;
 void setupSoftSerial1(uint32_t baud);
 
 uint8_t serialReadByte(softSerial_t *softSerial);
-bool serialAvailable(softSerial_t *softSerial);
+uint8_t serialAvailable(softSerial_t *softSerial);
+
+void serialWriteByte(softSerial_t *softSerial, uint8_t ch);
+void serialPrint(softSerial_t *softSerial, const char *str);
 
 extern timerHardware_t* serialTimerHardware;
 extern softSerial_t softSerialPorts[];
diff --git a/src/main.c b/src/main.c
index 01233d511..b5d77316b 100755
--- a/src/main.c
+++ b/src/main.c
@@ -129,13 +129,10 @@ int main(void)
     if (feature(FEATURE_VBAT))
         batteryInit();
 
-#ifdef SOFTSERIAL_19200_LOOPBACK
-
-    serialInit(19200);
-    setupSoftSerial1(19200);
-    uartPrint(core.mainport, "LOOPBACK 19200 ENABLED");
-#else
     serialInit(mcfg.serial_baudrate);
+#ifdef SOFTSERIAL_19200_LOOPBACK
+    setupSoftSerial1(19200);
+    serialPrint(&(softSerialPorts[0]), "LOOPBACK 19200 ENABLED\r\n");
 #endif
 
     previousTime = micros();
@@ -152,7 +149,8 @@ int main(void)
         while (serialAvailable(&softSerialPorts[0])) {
 
             uint8_t b = serialReadByte(&softSerialPorts[0]);
-            uartWrite(core.mainport, b);
+            serialWriteByte(&softSerialPorts[0], b);
+            //uartWrite(core.mainport, b);
         };
 #endif
     }