diff --git a/Makefile b/Makefile
index cb26d8e7c..a90b00f4d 100644
--- a/Makefile
+++ b/Makefile
@@ -94,6 +94,7 @@ OLIMEXINO_SRC	 = drv_spi.c \
 		   drv_l3g4200d.c \
 		   drv_pwm.c \
 		   drv_timer.c \
+		   drv_softserial.c \
 		   $(COMMON_SRC)
 
 # Search path for baseflight sources
diff --git a/src/board.h b/src/board.h
index b46ae3c2e..6bf30972b 100755
--- a/src/board.h
+++ b/src/board.h
@@ -199,6 +199,7 @@ typedef struct baro_t
 #include "drv_pwm.h"
 #include "drv_timer.h"
 #include "drv_uart.h"
+#include "drv_softserial.h"
 #else
 
  // AfroFlight32
@@ -217,6 +218,7 @@ typedef struct baro_t
 #include "drv_pwm.h"
 #include "drv_timer.h"
 #include "drv_uart.h"
+#include "drv_softserial.h"
 #include "drv_hcsr04.h"
 
 #endif
diff --git a/src/drv_pwm.c b/src/drv_pwm.c
index eeea201c1..fb4eb9604 100755
--- a/src/drv_pwm.c
+++ b/src/drv_pwm.c
@@ -1,11 +1,11 @@
 #include "board.h"
-
-#define PULSE_1MS       (1000) // 1ms pulse width
-
-/*
-    Configuration maps:
-
-    1) multirotor PPM input
+
+#define PULSE_1MS       (1000) // 1ms pulse width
+
+/*
+    Configuration maps:
+
+    1) multirotor PPM input
     PWM1 used for PPM
     PWM5..8 used for motors
     PWM9..10 used for servo or else motors
@@ -31,13 +31,13 @@
     PWM1 used for PPM
     PWM5..8 used for servos
     PWM9 used for motor throttle +PWM10 for 2nd motor
-    PWM11.14 used for servos
-*/
-
-typedef struct {
-    volatile uint16_t *ccr;
-    uint16_t period;
-
+    PWM11.14 used for servos
+*/
+
+typedef struct {
+    volatile uint16_t *ccr;
+    uint16_t period;
+
     // for input only
     uint8_t channel;
     uint8_t state;
@@ -137,21 +137,11 @@ static const uint8_t * const hardwareMaps[] = {
     airPPM,
 };
 
-static void pwmTimeBase(TIM_TypeDef *tim, uint32_t period)
-{
-    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+#define PWM_TIMER_MHZ 1
 
-    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
-    TIM_TimeBaseStructure.TIM_Period = period - 1;
-    TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 1000000) - 1; // all timers run at 1MHz
-    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-    TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);
-}
-
-static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
-{
-    TIM_OCInitTypeDef  TIM_OCInitStructure;
+static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
+{
+    TIM_OCInitTypeDef  TIM_OCInitStructure;
 
     TIM_OCStructInit(&TIM_OCInitStructure);
     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
@@ -181,7 +171,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
     }
 }
 
-static void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
+void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
 {
     TIM_ICInitTypeDef  TIM_ICInitStructure;
 
@@ -208,7 +198,7 @@ static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
 static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value)
 {
     pwmPortData_t *p = &pwmPorts[port];
-    pwmTimeBase(timerHardware[port].tim, period);
+    configTimeBase(timerHardware[port].tim, period, PWM_TIMER_MHZ);
     pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP);
     pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value);
     // Needed only on TIM1
@@ -230,26 +220,26 @@ static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value
             p->ccr = &timerHardware[port].tim->CCR4;
             break;
     }
-    return p;
-}
-
-static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uint8_t channel)
-{
-    pwmPortData_t *p = &pwmPorts[port];
-    pwmTimeBase(timerHardware[port].tim, 0xFFFF);
-    pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_IPD);
-    pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising);
-    TIM_Cmd(timerHardware[port].tim, ENABLE);
-    timerNVICConfig(timerHardware[port].irq);
-
-    p->channel = channel;
-
-    configureTimerCaptureCompareInterrupt(&(timerHardware[port]), port, callback);
-
-    return p;
-}
-
-static void ppmCallback(uint8_t port, uint16_t capture)
+    return p;
+}
+
+static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uint8_t channel)
+{
+    pwmPortData_t *p = &pwmPorts[port];
+    const timerHardware_t *timerHardwarePtr = &(timerHardware[port]);
+
+    p->channel = channel;
+
+    pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPD);
+    pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
+
+    timerInConfig(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ);
+    configureTimerCaptureCompareInterrupt(timerHardwarePtr, port, callback);
+
+    return p;
+}
+
+static void ppmCallback(uint8_t port, uint16_t capture)
 {
     uint16_t diff;
     static uint16_t now;
@@ -320,19 +310,25 @@ bool pwmInit(drv_pwm_config_t *init)
         uint8_t port = setup[i] & 0x0F;
         uint8_t mask = setup[i] & 0xF0;
 
-        if (setup[i] == 0xFF) // terminator
-            break;
-
-#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER
-        // PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E
-        if (port == PWM2)
-            continue;
-#endif
+        if (setup[i] == 0xFF) // terminator
+            break;
+
+#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER
+        // PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E
+        if (port == PWM2)
+            continue;
+#endif
 
         // skip UART ports for GPS
         if (init->useUART && (port == PWM3 || port == PWM4))
             continue;
 
+#ifdef SOFTSERIAL_19200_LOOPBACK
+        // skip softSerial ports
+        if ((port == PWM5 || port == PWM6))
+            continue;
+#endif
+
         // skip ADC for powerMeter if configured
         if (init->adcChannel && (init->adcChannel == port))
             continue;
diff --git a/src/drv_pwm.h b/src/drv_pwm.h
index c5d5e289e..a56e281a2 100755
--- a/src/drv_pwm.h
+++ b/src/drv_pwm.h
@@ -33,12 +33,14 @@ enum {
     PWM12,
     PWM13,
     PWM14,
-    MAX_PORTS
-};
-
-bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not
-void pwmWriteMotor(uint8_t index, uint16_t value);
-void pwmWriteServo(uint8_t index, uint16_t value);
+    MAX_PORTS
+};
+
+void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
+
+bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not
+void pwmWriteMotor(uint8_t index, uint16_t value);
+void pwmWriteServo(uint8_t index, uint16_t value);
 uint16_t pwmRead(uint8_t channel);
 
 // void pwmWrite(uint8_t channel, uint16_t value);
diff --git a/src/drv_softserial.c b/src/drv_softserial.c
new file mode 100644
index 000000000..bc99ad51f
--- /dev/null
+++ b/src/drv_softserial.c
@@ -0,0 +1,204 @@
+#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
+};
+
+#define MAX_SOFTSERIAL_PORTS 2
+softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
+
+
+softSerial_t* lookupSoftSerial(uint8_t reference)
+{
+    assert_param(reference >= 0 && reference <= MAX_SOFTSERIAL_PORTS);
+
+    return &(softSerialPorts[reference]);
+}
+
+
+void stopSerialTimer(softSerial_t *softSerial)
+{
+    TIM_Cmd(softSerial->timerHardware->tim, DISABLE);
+    TIM_SetCounter(softSerial->timerHardware->tim, 0);
+}
+
+void startSerialTimer(softSerial_t *softSerial)
+{
+    TIM_SetCounter(softSerial->timerHardware->tim, 0);
+    TIM_Cmd(softSerial->timerHardware->tim, ENABLE);
+}
+
+static void waitForFirstBit(softSerial_t *softSerial)
+{
+    softSerial->state = BIT_0;
+    startSerialTimer(softSerial);
+}
+
+static void onSerialPinChange(uint8_t reference, uint16_t capture)
+{
+    softSerial_t *softSerial = lookupSoftSerial(reference);
+    if (softSerial->state == WAITING_FOR_START_BIT) {
+        waitForFirstBit(softSerial);
+    }
+}
+
+uint8_t readSerialSignal(softSerial_t *softSerial)
+{
+    return GPIO_ReadInputDataBit(softSerial->timerHardware->gpio, softSerial->timerHardware->pin);
+}
+
+void mergeSignalWithCurrentByte(softSerial_t *softSerial, uint8_t serialSignal)
+{
+    softSerial->rxBuffer[softSerial->port.rxBufferTail] |= (serialSignal << softSerial->state);
+}
+
+inline void initialiseCurrentByteWithFirstSignal(softSerial_t *softSerial, uint8_t serialSignal)
+{
+    softSerial->rxBuffer[softSerial->port.rxBufferTail] = serialSignal;
+}
+
+inline void prepareForNextSignal(softSerial_t *softSerial) {
+    softSerial->state++;
+}
+
+void updateBufferIndex(softSerial_t *softSerial)
+{
+    if (softSerial->port.rxBufferTail >= softSerial->port.rxBufferSize - 1)
+    {
+        softSerial->port.rxBufferTail = 0; //cycling the buffer
+    } else {
+        softSerial->port.rxBufferTail++;
+    }
+}
+
+void prepareForNextByte(softSerial_t *softSerial)
+{
+    stopSerialTimer(softSerial);
+    softSerial->state = WAITING_FOR_START_BIT;
+    updateBufferIndex(softSerial);
+}
+
+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;
+    }
+}
+
+#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)
+{
+    if (softSerial->port.rxBufferTail == softSerial->port.rxBufferHead) {
+        return 0;
+    }
+
+    int availableBytes;
+    if (softSerial->port.rxBufferTail > softSerial->port.rxBufferHead) {
+        availableBytes = softSerial->port.rxBufferTail - softSerial->port.rxBufferHead;
+    } else {
+        availableBytes = softSerial->port.rxBufferTail + softSerial->port.rxBufferSize - softSerial->port.rxBufferHead;
+    }
+    return availableBytes;
+}
+
+static void moveHeadToNextByte(softSerial_t *softSerial)
+{
+    if (softSerial->port.rxBufferHead < softSerial->port.rxBufferSize - 1) {
+        softSerial->port.rxBufferHead++;
+    } else {
+        softSerial->port.rxBufferHead = 0;
+    }
+}
+
+uint8_t serialReadByte(softSerial_t *softSerial)
+{
+    if (serialAvailable(softSerial) == 0) {
+        return 0;
+    }
+
+    char b = softSerial->port.rxBuffer[softSerial->port.rxBufferHead];
+
+    moveHeadToNextByte(softSerial);
+    return b;
+}
diff --git a/src/drv_softserial.h b/src/drv_softserial.h
new file mode 100644
index 000000000..720d2fab7
--- /dev/null
+++ b/src/drv_softserial.h
@@ -0,0 +1,25 @@
+/*
+ * drv_softserial.h
+ *
+ *  Created on: 23 Aug 2013
+ *      Author: Hydra
+ */
+
+#pragma once
+
+#define SOFT_SERIAL_BUFFER_SIZE 256
+
+typedef struct softSerial_s {
+    const timerHardware_t *timerHardware;
+    uint8_t timerIndex;
+    serialPort_t port;
+    volatile int state;
+    volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE];
+} softSerial_t;
+void setupSoftSerial1(uint32_t baud);
+
+uint8_t serialReadByte(softSerial_t *softSerial);
+bool serialAvailable(softSerial_t *softSerial);
+
+extern timerHardware_t* serialTimerHardware;
+extern softSerial_t softSerialPorts[];
diff --git a/src/drv_timer.c b/src/drv_timer.c
index e778e2c65..5e33a9a80 100644
--- a/src/drv_timer.c
+++ b/src/drv_timer.c
@@ -71,9 +71,9 @@ static const uint8_t channels[CC_CHANNELS_PER_TIMER] = {
 };
 
 typedef struct timerConfig_s {
-    timerCCCallbackPtr *callback;
-    uint8_t channel;
     TIM_TypeDef *tim;
+    uint8_t channel;
+    timerCCCallbackPtr *callback;
     uint8_t reference;
 } timerConfig_t;
 
@@ -95,11 +95,81 @@ static uint8_t lookupChannelIndex(const uint8_t channel)
         channelIndex++;
     }
     return channelIndex;
-}
+}
+
+void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback)
+{
+    assert_param(IS_TIM_CHANNEL(channel));
+
+    uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel);
+
+    if (timerConfigIndex >= MAX_TIMERS * CC_CHANNELS_PER_TIMER) {
+        return;
+    }
+
+    timerConfig[timerConfigIndex].callback = callback;
+    timerConfig[timerConfigIndex].channel = channel;
+    timerConfig[timerConfigIndex].reference = reference;
+}
 
-static timerConfig_t *findTimerConfig(TIM_TypeDef *tim, uint8_t channel)
+void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel)
 {
-    uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel);
+    switch (channel) {
+        case TIM_Channel_1:
+            TIM_ITConfig(tim, TIM_IT_CC1, ENABLE);
+            break;
+        case TIM_Channel_2:
+            TIM_ITConfig(tim, TIM_IT_CC2, ENABLE);
+            break;
+        case TIM_Channel_3:
+            TIM_ITConfig(tim, TIM_IT_CC3, ENABLE);
+            break;
+        case TIM_Channel_4:
+            TIM_ITConfig(tim, TIM_IT_CC4, ENABLE);
+            break;
+    }
+}
+
+void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback)
+{
+    configureTimerChannelCallback(timerHardwarePtr->tim, timerHardwarePtr->channel, reference, callback);
+    configureTimerInputCaptureCompareChannel(timerHardwarePtr->tim, timerHardwarePtr->channel);
+}
+
+void timerNVICConfig(uint8_t irq)
+{
+    NVIC_InitTypeDef NVIC_InitStructure;
+
+    NVIC_InitStructure.NVIC_IRQChannel = irq;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+}
+
+void configTimeBase(TIM_TypeDef *tim, uint32_t period, uint8_t mhz)
+{
+    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+
+    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
+    TIM_TimeBaseStructure.TIM_Period = period - 1;
+    TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1;
+    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+    TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);
+}
+
+void timerInConfig(const timerHardware_t *timerHardwarePtr, uint32_t period, uint8_t mhz)
+{
+    configTimeBase(timerHardwarePtr->tim, period, mhz);
+    TIM_Cmd(timerHardwarePtr->tim, ENABLE);
+    timerNVICConfig(timerHardwarePtr->irq);
+}
+
+
+timerConfig_t *findTimerConfig(TIM_TypeDef *tim, uint8_t channel)
+{
+    uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel);
     return &(timerConfig[timerConfigIndex]);
 }
 
@@ -128,6 +198,8 @@ static void timCCxHandler(TIM_TypeDef *tim)
 
         timerConfig = findTimerConfig(tim, TIM_Channel_4);
         capture = TIM_GetCapture4(tim);
+    } else {
+        return; // avoid uninitialised variable dereference
     }
 
     if (!timerConfig->callback) {
@@ -155,46 +227,3 @@ void TIM4_IRQHandler(void)
 {
     timCCxHandler(TIM4);
 }
-
-void timerNVICConfig(uint8_t irq)
-{
-    NVIC_InitTypeDef NVIC_InitStructure;
-
-    NVIC_InitStructure.NVIC_IRQChannel = irq;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
-}
-
-void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel)
-{
-    switch (channel) {
-        case TIM_Channel_1:
-            TIM_ITConfig(tim, TIM_IT_CC1, ENABLE);
-            break;
-        case TIM_Channel_2:
-            TIM_ITConfig(tim, TIM_IT_CC2, ENABLE);
-            break;
-        case TIM_Channel_3:
-            TIM_ITConfig(tim, TIM_IT_CC3, ENABLE);
-            break;
-        case TIM_Channel_4:
-            TIM_ITConfig(tim, TIM_IT_CC4, ENABLE);
-            break;
-    }
-}
-
-void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback)
-{
-    uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel);
-    timerConfig[timerConfigIndex].callback = callback;
-    timerConfig[timerConfigIndex].channel = channel;
-    timerConfig[timerConfigIndex].reference = reference;
-}
-
-void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback)
-{
-    configureTimerChannelCallback(timerHardwarePtr->tim, timerHardwarePtr->channel, reference, callback);
-    configureTimerInputCaptureCompareChannel(timerHardwarePtr->tim, timerHardwarePtr->channel);
-}
diff --git a/src/drv_timer.h b/src/drv_timer.h
index 471220646..ab99059aa 100644
--- a/src/drv_timer.h
+++ b/src/drv_timer.h
@@ -1,20 +1,22 @@
-#pragma once
-
-typedef void timerCCCallbackPtr(uint8_t port, uint16_t capture);
-
-typedef struct {
-    TIM_TypeDef *tim;
-    GPIO_TypeDef *gpio;
-    uint32_t pin;
-    uint8_t channel;
-    uint8_t irq;
-    uint8_t outputEnable;
-} timerHardware_t;
-
-extern const timerHardware_t timerHardware[];
-
-void timerNVICConfig(uint8_t irq);
-
-void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel);
-void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback);
-void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback);
+#pragma once
+
+typedef void timerCCCallbackPtr(uint8_t port, uint16_t capture);
+
+typedef struct {
+    TIM_TypeDef *tim;
+    GPIO_TypeDef *gpio;
+    uint32_t pin;
+    uint8_t channel;
+    uint8_t irq;
+    uint8_t outputEnable;
+} timerHardware_t;
+
+extern const timerHardware_t timerHardware[];
+
+void configTimeBase(TIM_TypeDef *tim, uint32_t period, uint8_t mhz);
+void timerInConfig(const timerHardware_t *timerHardwarePtr, uint32_t period, uint8_t mhz);
+void timerNVICConfig(uint8_t irq);
+
+void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel);
+void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback);
+void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback);
diff --git a/src/drv_uart.c b/src/drv_uart.c
index 702e05588..29bd9c040 100755
--- a/src/drv_uart.c
+++ b/src/drv_uart.c
@@ -241,6 +241,14 @@ void uartWrite(serialPort_t *s, uint8_t ch)
     }
 }
 
+void uartPrint(serialPort_t *s, const char *str)
+{
+    uint8_t ch;
+    while ((ch = *(str++))) {
+        uartWrite(s, ch);
+    }
+}
+
 // Handlers
 
 // USART1 Tx DMA Handler
diff --git a/src/drv_uart.h b/src/drv_uart.h
index df6369a11..ec65bbc79 100755
--- a/src/drv_uart.h
+++ b/src/drv_uart.h
@@ -15,27 +15,34 @@ typedef enum portmode_t {
     MODE_RXTX = 3
 } portmode_t;
 
+// FIXME this is a uart_t really.  Move the generic properties into a separate structure (serialPort_t) and update the code to use it
 typedef struct {
+    portmode_t mode;
     uint32_t baudRate;
+
     uint32_t rxBufferSize;
     uint32_t txBufferSize;
     volatile uint8_t *rxBuffer;
     volatile uint8_t *txBuffer;
-    uint32_t rxDMAPos;
     uint32_t rxBufferHead;
     uint32_t rxBufferTail;
     uint32_t txBufferHead;
     uint32_t txBufferTail;
 
+    // FIXME rename callback type so something more generic
+    uartReceiveCallbackPtr callback;
+
+    // FIXME these are uart specific and do not belong in here
     DMA_Channel_TypeDef *rxDMAChannel;
     DMA_Channel_TypeDef *txDMAChannel;
+
     uint32_t rxDMAIrq;
     uint32_t txDMAIrq;
-    bool txDMAEmpty;
-    USART_TypeDef *USARTx;
 
-    uartReceiveCallbackPtr callback;
-    portmode_t mode;
+    uint32_t rxDMAPos;
+    bool txDMAEmpty;
+
+    USART_TypeDef *USARTx;
 } serialPort_t;
 
 serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode);
diff --git a/src/main.c b/src/main.c
index a2eb82fab..a360a9d92 100755
--- a/src/main.c
+++ b/src/main.c
@@ -122,7 +122,14 @@ int main(void)
     if (feature(FEATURE_VBAT))
         batteryInit();
 
-    serialInit(mcfg.serial_baudrate);
+#ifdef SOFTSERIAL_19200_LOOPBACK
+
+    serialInit(19200);
+    setupSoftSerial1(19200);
+    uartPrint(core.mainport, "LOOPBACK 19200 ENABLED");
+#else
+    serialInit(mcfg.serial_baudrate);
+#endif
 
     previousTime = micros();
     if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
@@ -134,6 +141,13 @@ int main(void)
     // loopy
     while (1) {
         loop();
+#ifdef SOFTSERIAL_19200_LOOPBACK
+        while (serialAvailable(&softSerialPorts[0])) {
+
+            uint8_t b = serialReadByte(&softSerialPorts[0]);
+            uartWrite(core.mainport, b);
+        };
+#endif
     }
 }
 
diff --git a/src/sensors.c b/src/sensors.c
index be3439e68..5daaa43f2 100755
--- a/src/sensors.c
+++ b/src/sensors.c
@@ -67,8 +67,8 @@ retry:
                     break;
             }
             ; // fallthrough
-        case 3: // MMA8452
 #ifndef OLIMEXINO
+        case 3: // MMA8452
             if (mma8452Detect(&acc)) {
                 accHardware = ACC_MMA8452;
                 if (mcfg.acc_hardware == ACC_MMA8452)