diff --git a/Makefile b/Makefile
index b9fce24af..f804d2bb2 100644
--- a/Makefile
+++ b/Makefile
@@ -8,7 +8,7 @@
 # Makefile for building the cleanflight firmware.
 #
 # Invoke this with 'make help' to see the list of supported targets.
-# 
+#
 
 ###############################################################################
 # Things that the user might override on the commandline
@@ -48,8 +48,8 @@ SRC_DIR		 = $(ROOT)/src/main
 OBJECT_DIR	 = $(ROOT)/obj/main
 BIN_DIR		 = $(ROOT)/obj
 CMSIS_DIR	 = $(ROOT)/lib/main/CMSIS
-INCLUDE_DIRS = $(SRC_DIR)
-LINKER_DIR   = $(ROOT)/src/main/target
+INCLUDE_DIRS	 = $(SRC_DIR)
+LINKER_DIR	 = $(ROOT)/src/main/target
 
 # Search path for sources
 VPATH		:= $(SRC_DIR):$(SRC_DIR)/startup
@@ -57,12 +57,12 @@ VPATH		:= $(SRC_DIR):$(SRC_DIR)/startup
 ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO MASSIVEF3))
 
 STDPERIPH_DIR	= $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver
-USBFS_DIR		= $(ROOT)/lib/main/STM32_USB-FS-Device_Driver
+USBFS_DIR	= $(ROOT)/lib/main/STM32_USB-FS-Device_Driver
 
 USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c))
 STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c))
 
-EXCLUDES = stm32f30x_crc.c \
+EXCLUDES	= stm32f30x_crc.c \
 		stm32f30x_can.c
 
 STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
@@ -244,6 +244,7 @@ NAZE_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/sound_beeper_stm32f10x.c \
 		   drivers/system_stm32f10x.c \
 		   drivers/timer.c \
+		   drivers/timer_stm32f10x.c \
 		   hardware_revision.c \
 		   $(HIGHEND_SRC) \
 		   $(COMMON_SRC)
@@ -280,6 +281,7 @@ EUSTM32F103RC_SRC	 = startup_stm32f10x_hd_gcc.S \
 		   drivers/sound_beeper_stm32f10x.c \
 		   drivers/system_stm32f10x.c \
 		   drivers/timer.c \
+		   drivers/timer_stm32f10x.c \
 		   $(HIGHEND_SRC) \
 		   $(COMMON_SRC)
 
@@ -307,6 +309,7 @@ OLIMEXINO_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/sound_beeper_stm32f10x.c \
 		   drivers/system_stm32f10x.c \
 		   drivers/timer.c \
+		   drivers/timer_stm32f10x.c \
 		   $(HIGHEND_SRC) \
 		   $(COMMON_SRC)
 
@@ -339,6 +342,7 @@ CJMCU_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/sound_beeper_stm32f10x.c \
 		   drivers/system_stm32f10x.c \
 		   drivers/timer.c \
+		   drivers/timer_stm32f10x.c \
 		   $(COMMON_SRC)
 
 CC3D_SRC	 = startup_stm32f10x_md_gcc.S \
@@ -360,6 +364,7 @@ CC3D_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/sound_beeper_stm32f10x.c \
 		   drivers/system_stm32f10x.c \
 		   drivers/timer.c \
+		   drivers/timer_stm32f10x.c \
 		   $(HIGHEND_SRC) \
 		   $(COMMON_SRC)
 
@@ -381,6 +386,7 @@ STM32F30x_COMMON_SRC	 = startup_stm32f30x_md_gcc.S \
 		   drivers/sound_beeper_stm32f30x.c \
 		   drivers/system_stm32f30x.c \
 		   drivers/timer.c \
+		   drivers/timer_stm32f30x.c \
 		   vcp/hw_config.c \
 		   vcp/stm32_it.c \
 		   vcp/usb_desc.c \
@@ -527,7 +533,7 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.s
 $(OBJECT_DIR)/$(TARGET)/%.o: %.S
 	@mkdir -p $(dir $@)
 	@echo %% $(notdir $<)
-	@$(CC) -c -o $@ $(ASFLAGS) $< 
+	@$(CC) -c -o $@ $(ASFLAGS) $<
 
 clean:
 	rm -f $(TARGET_BIN) $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP)
diff --git a/src/main/common/atomic.h b/src/main/common/atomic.h
new file mode 100644
index 000000000..6fc63a804
--- /dev/null
+++ b/src/main/common/atomic.h
@@ -0,0 +1,110 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+// only set_BASEPRI is implemented in device library. It does always create memory barrirer
+// missing versions are implemented here
+
+// set BASEPRI and BASEPRI_MAX register, but do not create memory barrier
+__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_nb(uint32_t basePri)
+{
+   __ASM volatile ("\tMSR basepri, %0\n" : : "r" (basePri) );
+}
+
+__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint32_t basePri)
+{
+   __ASM volatile ("\tMSR basepri_max\n, %0" : : "r" (basePri) );
+}
+
+__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX(uint32_t basePri)
+{
+    __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) : "memory" );
+}
+
+// cleanup BASEPRI restore function, with global memory barrier
+static inline void __basepriRestoreMem(uint8_t *val)
+{
+    __set_BASEPRI(*val);
+}
+
+// set BASEPRI_MAX function, with global memory barrier, returns true
+static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
+{
+    __set_BASEPRI_MAX(prio);
+    return 1;
+}
+
+// cleanup BASEPRI restore function, no memory barrier
+static inline void __basepriRestore(uint8_t *val)
+{
+    __set_BASEPRI_nb(*val);
+}
+
+// set BASEPRI_MAX function, no memory barrier, returns true
+static inline uint8_t __basepriSetRetVal(uint8_t prio)
+{
+    __set_BASEPRI_MAX_nb(prio);
+    return 1;
+}
+
+// Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled
+// Full memory barrier is placed at start and exit of block
+#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
+                                     __ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 )
+
+// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier.
+// Be carefull when using this, you must use some method to prevent optimizer form breaking things
+// - lto is used for baseflight compillation, so function call is not memory barrier
+// - use ATOMIC_BARRIER or propper volatile to protect used variables
+// - gcc 4.8.4 does write all values in registes to memory before 'asm volatile', so this optimization does not help much
+//    but that can change in future versions
+#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
+                                    __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
+
+// ATOMIC_BARRIER
+// Create memory barrier
+// - at the beginning (all data must be reread from memory)
+// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use)
+// ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier
+// this macro can be used only ONCE PER LINE, but multiple uses per block are fine
+
+#if (__GNUC__ > 4 || (__GNUC__ == 4 && (__GNUC_MINOR__ > 8)))
+# warn "Please verify that ATOMIC_BARRIER works as intended"
+// increment version number is BARRIER works
+// TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead
+// you should check that local variable scope with cleanup spans entire block
+#endif
+
+#ifndef __UNIQL
+# define __UNIQL_CONCAT2(x,y) x ## y
+# define __UNIQL_CONCAT(x,y) __UNIQL_CONCAT2(x,y)
+# define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__)
+#endif
+
+// this macro uses local function for cleanup. CLang block can be substituded
+#define ATOMIC_BARRIER(data)                                            \
+    __extension__ void  __UNIQL(__barrierEnd)(typeof(data) **__d) {     \
+        __asm__ volatile ("\t# barier(" #data ")  end\n" : : "m" (**__d));                          \
+    }                                                                   \
+    typeof(data)  __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \
+    __asm__ volatile ("\t# barier (" #data ") start\n" : "=m" (*__UNIQL(__barrier)))
+
+
+// define these wrappers for atomic operations, use gcc buildins
+#define ATOMIC_OR(ptr, val) __sync_fetch_and_or(ptr, val)
+#define ATOMIC_AND(ptr, val) __sync_fetch_and_and(ptr, val)
diff --git a/src/main/common/utils.h b/src/main/common/utils.h
new file mode 100644
index 000000000..d9f22c01e
--- /dev/null
+++ b/src/main/common/utils.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stddef.h>
+
+#define ARRAYLEN(x) (sizeof(x) / sizeof((x)[0]))
+
+/*
+http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
+*/
+#define BITCOUNT(x) (((BX_(x)+(BX_(x)>>4)) & 0x0F0F0F0F) % 255)
+#define BX_(x) ((x) - (((x)>>1)&0x77777777) - (((x)>>2)&0x33333333) - (((x)>>3)&0x11111111))
+
+#define UNUSED(x) (void)(x)
+
+#if 0
+// ISO C version, but no type checking
+#define container_of(ptr, type, member) \
+                      ((type *) ((char *)(ptr) - offsetof(type, member)))
+#else
+// non ISO variant from linux kernel; checks ptr type, but triggers 'ISO C forbids braced-groups within expressions [-Wpedantic]'
+//  __extension__ is here to disable this warning
+#define container_of(ptr, type, member)  __extension__ ({       \
+        const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
+        (type *)( (char *)__mptr - offsetof(type,member) );})
+
+
+#endif
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 1fbcfad79..904e79f19 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -34,13 +34,7 @@
 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
 
 typedef struct {
-#ifdef STM32F303xC
-    volatile uint32_t *ccr;
-#endif
-
-#ifdef STM32F10X
-    volatile uint16_t *ccr;
-#endif
+    volatile timCCR_t *ccr;
     uint16_t period;
     pwmWriteFuncPtr pwmWritePtr;
 } pwmOutputPort_t;
diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c
index 5a7547bef..2ecd4aa64 100644
--- a/src/main/drivers/pwm_rx.c
+++ b/src/main/drivers/pwm_rx.c
@@ -23,6 +23,9 @@
 #include "platform.h"
 #include "build_config.h"
 
+#include "common/utils.h"
+#include "system.h"
+
 #include "nvic.h"
 #include "gpio.h"
 #include "timer.h"
@@ -64,14 +67,16 @@ typedef struct {
     uint8_t missedEvents;
 
     const timerHardware_t *timerHardware;
+    timerCCHandlerRec_t edgeCb;
+    timerOvrHandlerRec_t overflowCb;
 } pwmInputPort_t;
 
 static pwmInputPort_t pwmInputPorts[PWM_INPUT_PORT_COUNT];
 
 static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT];
 
-#define PPM_TIMER_PERIOD 0xFFFF
-#define PWM_TIMER_PERIOD 0xFFFF
+#define PPM_TIMER_PERIOD 0x10000
+#define PWM_TIMER_PERIOD 0x10000
 
 static uint8_t ppmFrameCount = 0;
 static uint8_t lastPPMFrameCount = 0;
@@ -132,15 +137,15 @@ static void ppmInit(void)
     ppmDev.tracking     = false;
 }
 
-static void ppmOverflowCallback(uint8_t port, captureCompare_t capture)
+static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
 {
-    UNUSED(port);
-    ppmDev.largeCounter += capture;
+    UNUSED(cbRec);
+    ppmDev.largeCounter += capture + 1;
 }
 
-static void ppmEdgeCallback(uint8_t port, captureCompare_t capture)
+static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture)
 {
-    UNUSED(port);
+    UNUSED(cbRec);
     int32_t i;
 
     /* Shift the last measurement out */
@@ -218,10 +223,10 @@ static void ppmEdgeCallback(uint8_t port, captureCompare_t capture)
 
 #define MAX_MISSED_PWM_EVENTS 10
 
-static void pwmOverflowCallback(uint8_t port, captureCompare_t capture)
+static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
 {
     UNUSED(capture);
-    pwmInputPort_t *pwmInputPort = &pwmInputPorts[port];
+    pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
 
     if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
         if (pwmInputPort->state == 0) {
@@ -231,9 +236,9 @@ static void pwmOverflowCallback(uint8_t port, captureCompare_t capture)
     }
 }
 
-static void pwmEdgeCallback(uint8_t port, captureCompare_t capture)
+static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 {
-    pwmInputPort_t *pwmInputPort = &pwmInputPorts[port];
+    pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, edgeCb);
     const timerHardware_t *timerHardwarePtr = pwmInputPort->timerHardware;
 
     if (pwmInputPort->state == 0) {
@@ -285,27 +290,22 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
 
 void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel)
 {
-    pwmInputPort_t *p = &pwmInputPorts[channel];
+    pwmInputPort_t *self = &pwmInputPorts[channel];
 
-    p->state = 0;
-    p->missedEvents = 0;
-    p->channel = channel;
-    p->mode = INPUT_MODE_PWM;
-    p->timerHardware = timerHardwarePtr;
+    self->state = 0;
+    self->missedEvents = 0;
+    self->channel = channel;
+    self->mode = INPUT_MODE_PWM;
+    self->timerHardware = timerHardwarePtr;
 
     pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
     pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 
-    timerConfigure(timerHardwarePtr, PWM_TIMER_PERIOD, PWM_TIMER_MHZ);
+    timerConfigure(timerHardwarePtr, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ);
 
-#ifdef STM32F303xC
-    // If overflow monitoring is enabled on STM32F3 then the IRQ handler TIM1_UP_TIM16_IRQHandler is continually called.
-    if (timerHardwarePtr->tim == TIM1) {
-        configureTimerCaptureCompareInterrupt(timerHardwarePtr, channel, pwmEdgeCallback, NULL);
-        return;
-    }
-#endif
-    configureTimerCaptureCompareInterrupt(timerHardwarePtr, channel, pwmEdgeCallback, pwmOverflowCallback);
+    timerChCCHandlerInit(&self->edgeCb, pwmEdgeCallback);
+    timerChOvrHandlerInit(&self->overflowCb, pwmOverflowCallback);
+    timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
 }
 
 #define UNUSED_PPM_TIMER_REFERENCE 0
@@ -315,16 +315,19 @@ void ppmInConfig(const timerHardware_t *timerHardwarePtr)
 {
     ppmInit();
 
-    pwmInputPort_t *p = &pwmInputPorts[FIRST_PWM_PORT];
+    pwmInputPort_t *self = &pwmInputPorts[FIRST_PWM_PORT];
 
-    p->mode = INPUT_MODE_PPM;
-    p->timerHardware = timerHardwarePtr;
+    self->mode = INPUT_MODE_PPM;
+    self->timerHardware = timerHardwarePtr;
 
     pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
     pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 
-    timerConfigure(timerHardwarePtr, PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
-    configureTimerCaptureCompareInterrupt(timerHardwarePtr, UNUSED_PPM_TIMER_REFERENCE, ppmEdgeCallback, ppmOverflowCallback);
+    timerConfigure(timerHardwarePtr, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
+
+    timerChCCHandlerInit(&self->edgeCb, ppmEdgeCallback);
+    timerChOvrHandlerInit(&self->overflowCb, ppmOverflowCallback);
+    timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
 }
 
 uint16_t pwmRead(uint8_t channel)
diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c
index f588bb14f..82a0eb468 100644
--- a/src/main/drivers/serial_softserial.c
+++ b/src/main/drivers/serial_softserial.c
@@ -25,6 +25,9 @@
 
 #include "build_config.h"
 
+#include "common/utils.h"
+#include "common/atomic.h"
+
 #include "nvic.h"
 #include "system.h"
 #include "gpio.h"
@@ -44,8 +47,8 @@
 
 softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
 
-void onSerialTimer(uint8_t portIndex, captureCompare_t capture);
-void onSerialRxPinChange(uint8_t portIndex, captureCompare_t capture);
+void onSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
+void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
 
 void setTxSignal(softSerial_t *softSerial, uint8_t state)
 {
@@ -88,7 +91,7 @@ static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t
         timerPeriod = clock / baud;
         if (isTimerPeriodTooLarge(timerPeriod)) {
             if (clock > 1) {
-                clock = clock / 2;
+                clock = clock / 2;   // this is wrong - mhz stays the same ... This will double baudrate until ok (but minimum baudrate is < 1200)
             } else {
                 // TODO unable to continue, unable to determine clock and timerPeriods for the given baud
             }
@@ -98,7 +101,8 @@ static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t
 
     uint8_t mhz = SystemCoreClock / 1000000;
     timerConfigure(timerHardwarePtr, timerPeriod, mhz);
-    configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, onSerialTimer, NULL);
+    timerChCCHandlerInit(&softSerialPorts[reference].timerCb, onSerialTimer);
+    timerChConfigCallbacks(timerHardwarePtr, &softSerialPorts[reference].timerCb, NULL);
 }
 
 static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
@@ -119,7 +123,8 @@ static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t
 {
     // start bit is usually a FALLING signal
     serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, inversion == SERIAL_INVERTED ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
-    configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, onSerialRxPinChange, NULL);
+    timerChCCHandlerInit(&softSerialPorts[reference].edgeCb, onSerialRxPinChange);
+    timerChConfigCallbacks(timerHardwarePtr, &softSerialPorts[reference].edgeCb, NULL);
 }
 
 static void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
@@ -309,20 +314,20 @@ void processRxState(softSerial_t *softSerial)
     }
 }
 
-void onSerialTimer(uint8_t portIndex, captureCompare_t capture)
+void onSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 {
     UNUSED(capture);
-    softSerial_t *softSerial = &(softSerialPorts[portIndex]);
+    softSerial_t *softSerial = container_of(cbRec, softSerial_t, timerCb);
 
     processTxState(softSerial);
     processRxState(softSerial);
 }
 
-void onSerialRxPinChange(uint8_t portIndex, captureCompare_t capture)
+void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 {
     UNUSED(capture);
 
-    softSerial_t *softSerial = &(softSerialPorts[portIndex]);
+    softSerial_t *softSerial = container_of(cbRec, softSerial_t, edgeCb);
 
     if ((softSerial->port.mode & MODE_RX) == 0) {
         return;
@@ -332,7 +337,7 @@ void onSerialRxPinChange(uint8_t portIndex, captureCompare_t capture)
         // synchronise bit counter
         // FIXME this reduces functionality somewhat as receiving breaks concurrent transmission on all ports because
         // the next callback to the onSerialTimer will happen too early causing transmission errors.
-        TIM_SetCounter(softSerial->rxTimerHardware->tim, 0);
+        TIM_SetCounter(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->tim->ARR / 2);
         if (softSerial->isTransmittingData) {
             softSerial->transmissionErrors++;
         }
diff --git a/src/main/drivers/serial_softserial.h b/src/main/drivers/serial_softserial.h
index b5725e7dd..89eda56c3 100644
--- a/src/main/drivers/serial_softserial.h
+++ b/src/main/drivers/serial_softserial.h
@@ -32,7 +32,7 @@ typedef struct softSerial_s {
 
     const timerHardware_t *txTimerHardware;
     volatile uint8_t txBuffer[SOFTSERIAL_BUFFER_SIZE];
-    
+
     uint8_t          isSearchingForStartBit;
     uint8_t          rxBitIndex;
     uint8_t          rxLastLeadingEdgeAtBitIndex;
@@ -48,6 +48,9 @@ typedef struct softSerial_s {
     uint16_t         receiveErrors;
 
     uint8_t          softSerialPortIndex;
+
+    timerCCHandlerRec_t timerCb;
+    timerCCHandlerRec_t edgeCb;
 } softSerial_t;
 
 extern timerHardware_t* serialTimerHardware;
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index 9af5f5af0..04dbbf85d 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -28,6 +28,7 @@
 
 #include "build_config.h"
 
+#include "common/utils.h"
 #include "gpio.h"
 #include "inverter.h"
 
diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c
index 14cf26720..c7b01d204 100644
--- a/src/main/drivers/timer.c
+++ b/src/main/drivers/timer.c
@@ -21,12 +21,18 @@
 #include <string.h>
 
 #include "platform.h"
+#include "common/utils.h"
+#include "common/atomic.h"
+
 #include "nvic.h"
 
 #include "gpio.h"
 #include "system.h"
 
 #include "timer.h"
+#include "timer_impl.h"
+
+#define TIM_N(n) (1 << (n))
 
 /* FreeFlight/Naze32 timer layout
     TIM2_CH1    RC1             PWM1
@@ -86,11 +92,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, Mode_IPD}           // PWM14
 };
 
-#define MAX_TIMERS 4 // TIM1..TIM4
-
-static const TIM_TypeDef const *timers[MAX_TIMERS] = {
-    TIM1, TIM2, TIM3, TIM4
-};
+#define USED_TIMERS         (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4))
 
 #define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4)
 #define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB)
@@ -113,11 +115,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 1, GPIO_Mode_AF_PP},    // S6_OUT
 };
 
-#define MAX_TIMERS 4 // TIM1..TIM4
-
-static const TIM_TypeDef const *timers[MAX_TIMERS] = {
-    TIM1, TIM2, TIM3, TIM4
-};
+#define USED_TIMERS         (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4))
 
 #define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4)
 #define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB)
@@ -141,11 +139,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_1}                    // PWM14 - PA2
 };
 
-#define MAX_TIMERS 7
-
-static const TIM_TypeDef const *timers[MAX_TIMERS] = {
-    TIM1, TIM2, TIM3, TIM4, TIM8, TIM16, TIM17
-};
+#define USED_TIMERS  (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17))
 
 #define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4)
 #define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17)
@@ -178,11 +172,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     { TIM3,  GPIOA, Pin_4,  TIM_Channel_2, TIM3_IRQn,               0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}     // PWM18 - PA4
 };
 
-#define MAX_TIMERS 8
-
-static const TIM_TypeDef const *timers[MAX_TIMERS] = {
-    TIM1, TIM2, TIM3, TIM4, TIM8, TIM15, TIM16, TIM17
-};
+#define USED_TIMERS  (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17))
 
 #define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4)
 #define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17)
@@ -209,11 +199,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     { TIM17, GPIOA, Pin_7,  TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1}, // PA7 - untested
 };
 
-#define MAX_TIMERS 7
-
-static const TIM_TypeDef const *timers[MAX_TIMERS] = {
-    TIM1, TIM2, TIM3, TIM4, TIM15, TIM16, TIM17
-};
+#define USED_TIMERS  (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17))
 
 #define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4)
 #define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17)
@@ -221,91 +207,102 @@ static const TIM_TypeDef const *timers[MAX_TIMERS] = {
 
 #endif
 
+#define USED_TIMER_COUNT BITCOUNT(USED_TIMERS)
+#define CC_CHANNELS_PER_TIMER 4              // TIM_Channel_1..4
 
-#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4
-static const uint16_t const channels[CC_CHANNELS_PER_TIMER] = {
-    TIM_Channel_1, TIM_Channel_2, TIM_Channel_3, TIM_Channel_4
-};
+#define TIM_IT_CCx(ch) (TIM_IT_CC1 << ((ch) / 4))
 
 typedef struct timerConfig_s {
-    TIM_TypeDef *tim;
-    uint8_t channel;
-    timerCCCallbackPtr *edgeCallback;
-    timerCCCallbackPtr *overflowCallback;
-    uint8_t reference;
+    timerCCHandlerRec_t *edgeCallback[CC_CHANNELS_PER_TIMER];
+    timerOvrHandlerRec_t *overflowCallback[CC_CHANNELS_PER_TIMER];
+    timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linkded list of active overflow callbacks
 } timerConfig_t;
+timerConfig_t timerConfig[USED_TIMER_COUNT];
 
-static timerConfig_t timerConfig[MAX_TIMERS * CC_CHANNELS_PER_TIMER];
+typedef struct {
+    channelType_t type;
+} timerChannelInfo_t;
+timerChannelInfo_t timerChannelInfo[USABLE_TIMER_CHANNEL_COUNT];
+
+typedef struct {
+    uint8_t priority;
+} timerInfo_t;
+timerInfo_t timerInfo[USED_TIMER_COUNT];
+
+// return index of timer in timer table. Lowest timer has index 0
+#define TIMER_INDEX(i) BITCOUNT((TIM_N(i) - 1) & USED_TIMERS)
 
 static uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
 {
-    uint8_t timerIndex = 0;
-    while (timers[timerIndex] != tim) {
-        timerIndex++;
+#define _CASE_SHF 10           // amount we can safely shift timer address to the right. gcc will throw error if some timers overlap
+#define _CASE_(tim, index) case ((unsigned)tim >> _CASE_SHF): return index; break
+#define _CASE(i) _CASE_(TIM##i##_BASE, TIMER_INDEX(i))
+
+// let gcc do the work, switch should be quite optimized
+    switch((unsigned)tim >> _CASE_SHF) {
+#if USED_TIMERS & TIM_N(1)
+        _CASE(1);
+#endif
+#if USED_TIMERS & TIM_N(2)
+        _CASE(2);
+#endif
+#if USED_TIMERS & TIM_N(3)
+        _CASE(3);
+#endif
+#if USED_TIMERS & TIM_N(4)
+        _CASE(4);
+#endif
+#if USED_TIMERS & TIM_N(8)
+        _CASE(8);
+#endif
+#if USED_TIMERS & TIM_N(15)
+        _CASE(15);
+#endif
+#if USED_TIMERS & TIM_N(16)
+        _CASE(16);
+#endif
+#if USED_TIMERS & TIM_N(17)
+        _CASE(17);
+#endif
+    default:  return ~1;  // make sure final index is out of range
     }
-    return timerIndex;
+#undef _CASE
+#undef _CASE_
 }
 
-static uint8_t lookupChannelIndex(const uint16_t channel)
+TIM_TypeDef * const usedTimers[USED_TIMER_COUNT] = {
+#define _DEF(i) TIM##i
+
+#if USED_TIMERS & TIM_N(1)
+    _DEF(1),
+#endif
+#if USED_TIMERS & TIM_N(2)
+    _DEF(2),
+#endif
+#if USED_TIMERS & TIM_N(3)
+    _DEF(3),
+#endif
+#if USED_TIMERS & TIM_N(4)
+    _DEF(4),
+#endif
+#if USED_TIMERS & TIM_N(8)
+    _DEF(8),
+#endif
+#if USED_TIMERS & TIM_N(15)
+    _DEF(15),
+#endif
+#if USED_TIMERS & TIM_N(16)
+    _DEF(16),
+#endif
+#if USED_TIMERS & TIM_N(17)
+    _DEF(17),
+#endif
+#undef _DEF
+};
+
+static inline uint8_t lookupChannelIndex(const uint16_t channel)
 {
-    uint8_t channelIndex = 0;
-    while (channels[channelIndex] != channel) {
-        channelIndex++;
-    }
-    return channelIndex;
-}
-
-static uint8_t lookupTimerConfigIndex(TIM_TypeDef *tim, const uint16_t channel)
-{
-    return lookupTimerIndex(tim) + (MAX_TIMERS * lookupChannelIndex(channel));
-}
-
-void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *edgeCallback)
-{
-    configureTimerChannelCallbacks(tim, channel, reference, edgeCallback, NULL);
-}
-
-void configureTimerChannelCallbacks(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *edgeCallback, timerCCCallbackPtr *overflowCallback)
-{
-    assert_param(IS_TIM_CHANNEL(channel));
-
-    uint8_t timerConfigIndex = lookupTimerConfigIndex(tim, channel);
-
-    if (timerConfigIndex >= MAX_TIMERS * CC_CHANNELS_PER_TIMER) {
-        return;
-    }
-
-    timerConfig[timerConfigIndex].edgeCallback = edgeCallback;
-    timerConfig[timerConfigIndex].overflowCallback = overflowCallback;
-    timerConfig[timerConfigIndex].channel = channel;
-    timerConfig[timerConfigIndex].reference = reference;
-}
-
-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 configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *edgeCallback, timerCCCallbackPtr *overflowCallback)
-{
-    configureTimerChannelCallbacks(timerHardwarePtr->tim, timerHardwarePtr->channel, reference, edgeCallback, overflowCallback);
-    configureTimerInputCaptureCompareChannel(timerHardwarePtr->tim, timerHardwarePtr->channel);
-    if (overflowCallback) {
-        TIM_ITConfig(timerHardwarePtr->tim, TIM_IT_Update, ENABLE);
-    }
+    return channel >> 2;
 }
 
 void timerNVICConfigure(uint8_t irq)
@@ -330,12 +327,12 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
     // Thus for 1Mhz: 72000000 / 1000000 = 72, 72 - 1 = 71 = TIM_Prescaler
     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);
 }
 
+// old interface for PWM inputs. It should be replaced
 void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
 {
     configTimeBase(timerHardwarePtr->tim, period, mhz);
@@ -343,108 +340,382 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui
     timerNVICConfigure(timerHardwarePtr->irq);
 }
 
-timerConfig_t *findTimerConfig(TIM_TypeDef *tim, uint16_t channel)
+// allocate and configure timer channel. Timer priority is set to highest priority of its channels
+void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority)
 {
-    uint8_t timerConfigIndex = lookupTimerConfigIndex(tim, channel);
-    return &(timerConfig[timerConfigIndex]);
+    unsigned channel = timHw - timerHardware;
+    if(channel >= USABLE_TIMER_CHANNEL_COUNT)
+        return;
+
+    timerChannelInfo[channel].type = type;
+    unsigned timer = lookupTimerIndex(timHw->tim);
+    if(timer >= USED_TIMER_COUNT)
+        return;
+    if(irqPriority < timerInfo[timer].priority) {
+        // it would be better to set priority in the end, but current startup sequence is not ready
+        configTimeBase(usedTimers[timer], 0, 1);
+        TIM_Cmd(usedTimers[timer],  ENABLE);
+
+        NVIC_InitTypeDef NVIC_InitStructure;
+
+        NVIC_InitStructure.NVIC_IRQChannel = timHw->irq;
+        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority);
+        NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority);
+        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+        NVIC_Init(&NVIC_InitStructure);
+
+        timerInfo[timer].priority = irqPriority;
+    }
 }
 
-static void timCCxHandler(TIM_TypeDef *tim)
+void timerChCCHandlerInit(timerCCHandlerRec_t *self, timerCCHandlerCallback *fn)
 {
-    captureCompare_t capture;
-    timerConfig_t *timerConfig;
-    uint8_t channel;
-    uint8_t channelIndex;
+    self->fn = fn;
+}
 
-    if (TIM_GetITStatus(tim, TIM_IT_Update) == SET) {
-        TIM_ClearITPendingBit(tim, TIM_IT_Update);
-        capture = tim->ARR;
+void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *fn)
+{
+    self->fn = fn;
+    self->next = NULL;
+}
 
-        for (channelIndex = 0; channelIndex < CC_CHANNELS_PER_TIMER; channelIndex++) {
-            channel = channels[channelIndex];
-            timerConfig = findTimerConfig(tim, channel);
-
-            if (!timerConfig->overflowCallback) {
-                continue;
+// update overflow callback list
+// some synchronization mechanism is neccesary to avoid disturbing other channels (BASEPRI used now)
+static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef* tim) {
+    timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive;
+    ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+        for(int i = 0; i < CC_CHANNELS_PER_TIMER; i++)
+            if(cfg->overflowCallback[i]) {
+                *chain = cfg->overflowCallback[i];
+                chain = &cfg->overflowCallback[i]->next;
             }
-            timerConfig->overflowCallback(timerConfig->reference, capture);
-        }
+        *chain = NULL;
+    }
+    // enable or disable IRQ
+    TIM_ITConfig(tim, TIM_IT_Update, cfg->overflowCallbackActive ? ENABLE : DISABLE);
+}
+
+// config edge and overflow callback for channel. Try to avoid overflowCallback, it is a bit expensive
+void timerChConfigCallbacks(const timerHardware_t* timHw, timerCCHandlerRec_t *edgeCallback, timerOvrHandlerRec_t *overflowCallback)
+{
+    uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+    if (timerIndex >= USED_TIMER_COUNT) {
+        return;
+    }
+    uint8_t channelIndex = lookupChannelIndex(timHw->channel);
+    if(edgeCallback == NULL)   // disable irq before changing callback to NULL
+        TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), DISABLE);
+    // setup callback info
+    timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback;
+    timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback;
+    // enable channel IRQ
+    if(edgeCallback)
+        TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), ENABLE);
+
+    timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
+}
+
+// configure callbacks for pair of channels (1+2 or 3+4).
+// Hi(2,4) and Lo(1,3) callbacks are specified, it is not important which timHw channel is used.
+// This is intended for dual capture mode (each channel handles one transition)
+void timerChConfigCallbacksDual(const timerHardware_t* timHw, timerCCHandlerRec_t *edgeCallbackLo, timerCCHandlerRec_t *edgeCallbackHi, timerOvrHandlerRec_t *overflowCallback)
+{
+    uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+    if (timerIndex >= USED_TIMER_COUNT) {
+        return;
+    }
+    uint16_t chLo = timHw->channel & ~TIM_Channel_2;   // lower channel
+    uint16_t chHi = timHw->channel | TIM_Channel_2;    // upper channel
+    uint8_t channelIndex = lookupChannelIndex(chLo);   // get index of lower channel
+
+    if(edgeCallbackLo == NULL)   // disable irq before changing setting callback to NULL
+        TIM_ITConfig(timHw->tim, TIM_IT_CCx(chLo), DISABLE);
+    if(edgeCallbackHi == NULL)   // disable irq before changing setting callback to NULL
+        TIM_ITConfig(timHw->tim, TIM_IT_CCx(chHi), DISABLE);
+
+    // setup callback info
+    timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallbackLo;
+    timerConfig[timerIndex].edgeCallback[channelIndex + 1] = edgeCallbackHi;
+    timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback;
+    timerConfig[timerIndex].overflowCallback[channelIndex + 1] = NULL;
+
+    // enable channel IRQs
+    if(edgeCallbackLo) {
+        TIM_ClearFlag(timHw->tim, TIM_IT_CCx(chLo));
+        TIM_ITConfig(timHw->tim, TIM_IT_CCx(chLo), ENABLE);
+    }
+    if(edgeCallbackHi) {
+        TIM_ClearFlag(timHw->tim, TIM_IT_CCx(chHi));
+        TIM_ITConfig(timHw->tim, TIM_IT_CCx(chHi), ENABLE);
     }
 
-    for (channelIndex = 0; channelIndex < CC_CHANNELS_PER_TIMER; channelIndex++) {
-        channel = channels[channelIndex];
+    timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
+}
 
-        if (channel == TIM_Channel_1 && TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
-            TIM_ClearITPendingBit(tim, TIM_IT_CC1);
+// enable/disable IRQ for low channel in dual configuration
+void timerChITConfigDualLo(const timerHardware_t* timHw, FunctionalState newState) {
+    TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel&~TIM_Channel_2), newState);
+}
 
-            timerConfig = findTimerConfig(tim, TIM_Channel_1);
-            capture = TIM_GetCapture1(tim);
-        } else if (channel == TIM_Channel_2 && TIM_GetITStatus(tim, TIM_IT_CC2) == SET) {
-            TIM_ClearITPendingBit(tim, TIM_IT_CC2);
+// enable or disable IRQ
+void timerChITConfig(const timerHardware_t* timHw, FunctionalState newState)
+{
+    TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), newState);
+}
 
-            timerConfig = findTimerConfig(tim, TIM_Channel_2);
-            capture = TIM_GetCapture2(tim);
-        } else if (channel == TIM_Channel_3 && TIM_GetITStatus(tim, TIM_IT_CC3) == SET) {
-            TIM_ClearITPendingBit(tim, TIM_IT_CC3);
+// clear Compare/Capture flag for channel
+void timerChClearCCFlag(const timerHardware_t* timHw)
+{
+    TIM_ClearFlag(timHw->tim, TIM_IT_CCx(timHw->channel));
+}
 
-            timerConfig = findTimerConfig(tim, TIM_Channel_3);
-            capture = TIM_GetCapture3(tim);
-        } else if (channel == TIM_Channel_4 && TIM_GetITStatus(tim, TIM_IT_CC4) == SET) {
-            TIM_ClearITPendingBit(tim, TIM_IT_CC4);
+// configure timer channel GPIO mode
+void timerChConfigGPIO(const timerHardware_t* timHw, GPIO_Mode mode)
+{
+    gpio_config_t cfg;
 
-            timerConfig = findTimerConfig(tim, TIM_Channel_4);
-            capture = TIM_GetCapture4(tim);
-        } else {
-            continue; // avoid uninitialised variable dereference
-        }
+    cfg.pin = timHw->pin;
+    cfg.mode = mode;
+    cfg.speed = Speed_2MHz;
+    gpioInit(timHw->gpio, &cfg);
+}
 
-        if (!timerConfig->edgeCallback) {
-            continue;
-        }
-        timerConfig->edgeCallback(timerConfig->reference, capture);
+// calculate input filter constant
+// TODO - we should probably setup DTS to higher value to allow reasonable input filtering
+//   - notice that prescaler[0] does use DTS for sampling - the sequence won't be monotonous anymore
+static unsigned getFilter(unsigned ticks)
+{
+    static const unsigned ftab[16] = {
+        1*1,                 // fDTS !
+        1*2, 1*4, 1*8,       // fCK_INT
+        2*6, 2*8,            // fDTS/2
+        4*6, 4*8,
+        8*6, 8*8,
+        16*5, 16*6, 16*8,
+        32*5, 32*6, 32*8
+    };
+    for(unsigned i = 1; i < ARRAYLEN(ftab); i++)
+        if(ftab[i] > ticks)
+            return i - 1;
+    return 0x0f;
+}
+
+// Configure input captupre
+void timerChConfigIC(const timerHardware_t* timHw, bool polarityRising, unsigned inputFilterTicks)
+{
+    TIM_ICInitTypeDef TIM_ICInitStructure;
+
+    TIM_ICStructInit(&TIM_ICInitStructure);
+    TIM_ICInitStructure.TIM_Channel = timHw->channel;
+    TIM_ICInitStructure.TIM_ICPolarity = polarityRising ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling;
+    TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+    TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+    TIM_ICInitStructure.TIM_ICFilter = getFilter(inputFilterTicks);
+
+    TIM_ICInit(timHw->tim, &TIM_ICInitStructure);
+}
+
+// configure dual channel input channel for capture
+// polarity is for Low channel (capture order is always Lo - Hi)
+void timerChConfigICDual(const timerHardware_t* timHw, bool polarityRising, unsigned inputFilterTicks)
+{
+    TIM_ICInitTypeDef TIM_ICInitStructure;
+    bool directRising = (timHw->channel & TIM_Channel_2) ? !polarityRising : polarityRising;
+    // configure direct channel
+    TIM_ICStructInit(&TIM_ICInitStructure);
+
+    TIM_ICInitStructure.TIM_Channel = timHw->channel;
+    TIM_ICInitStructure.TIM_ICPolarity = directRising ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling;
+    TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+    TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+    TIM_ICInitStructure.TIM_ICFilter = getFilter(inputFilterTicks);
+    TIM_ICInit(timHw->tim, &TIM_ICInitStructure);
+    // configure indirect channel
+    TIM_ICInitStructure.TIM_Channel = timHw->channel ^ TIM_Channel_2;   // get opposite channel no
+    TIM_ICInitStructure.TIM_ICPolarity = directRising ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising;
+    TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_IndirectTI;
+    TIM_ICInit(timHw->tim, &TIM_ICInitStructure);
+}
+
+
+
+void timerChICPolarity(const timerHardware_t* timHw, bool polarityRising)
+{
+    timCCER_t tmpccer = timHw->tim->CCER;
+    tmpccer &= ~(TIM_CCER_CC1P << timHw->channel);
+    tmpccer |= polarityRising ? (TIM_ICPolarity_Rising << timHw->channel) : (TIM_ICPolarity_Falling << timHw->channel);
+    timHw->tim->CCER = tmpccer;
+}
+
+volatile timCCR_t* timerChCCRHi(const timerHardware_t* timHw)
+{
+    return (volatile timCCR_t*)((volatile char*)&timHw->tim->CCR1 + (timHw->channel | TIM_Channel_2));
+}
+
+volatile timCCR_t* timerChCCRLo(const timerHardware_t* timHw)
+{
+    return (volatile timCCR_t*)((volatile char*)&timHw->tim->CCR1 + (timHw->channel & ~TIM_Channel_2));
+}
+
+
+
+volatile timCCR_t* timerChCCR(const timerHardware_t* timHw)
+{
+    return (volatile timCCR_t*)((volatile char*)&timHw->tim->CCR1 + timHw->channel);
+}
+
+void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh)
+{
+    TIM_OCInitTypeDef  TIM_OCInitStructure;
+
+    TIM_OCStructInit(&TIM_OCInitStructure);
+    if(outEnable) {
+        TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive;
+        TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+        TIM_OCInitStructure.TIM_OCPolarity = stateHigh ? TIM_OCPolarity_High : TIM_OCPolarity_Low;
+    } else {
+        TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
+    }
+
+    switch (timHw->channel) {
+    case TIM_Channel_1:
+        TIM_OC1Init(timHw->tim, &TIM_OCInitStructure);
+        TIM_OC1PreloadConfig(timHw->tim, TIM_OCPreload_Disable);
+        break;
+    case TIM_Channel_2:
+        TIM_OC2Init(timHw->tim, &TIM_OCInitStructure);
+        TIM_OC2PreloadConfig(timHw->tim, TIM_OCPreload_Disable);
+        break;
+    case TIM_Channel_3:
+        TIM_OC3Init(timHw->tim, &TIM_OCInitStructure);
+        TIM_OC3PreloadConfig(timHw->tim, TIM_OCPreload_Disable);
+        break;
+    case TIM_Channel_4:
+        TIM_OC4Init(timHw->tim, &TIM_OCInitStructure);
+        TIM_OC4PreloadConfig(timHw->tim, TIM_OCPreload_Disable);
+        break;
     }
 }
 
-void TIM1_CC_IRQHandler(void)
+
+
+static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t* timerConfig)
 {
-    timCCxHandler(TIM1);
+    uint16_t capture;
+    unsigned tim_status;
+    tim_status = tim->SR & tim->DIER;
+#if 1
+    while(tim_status) {
+        // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly
+        // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway)
+        unsigned bit = __builtin_clz(tim_status);
+        unsigned mask = ~(0x80000000 >> bit);
+        tim->SR = mask;
+        tim_status &= mask;
+        switch(bit) {
+        case __builtin_clz(TIM_IT_Update):
+            capture = tim->ARR;
+            timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
+            while(cb) {
+                cb->fn(cb, capture);
+                cb = cb->next;
+            }
+            break;
+        case __builtin_clz(TIM_IT_CC1):
+            timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CCR1);
+            break;
+        case __builtin_clz(TIM_IT_CC2):
+            timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], tim->CCR2);
+            break;
+        case __builtin_clz(TIM_IT_CC3):
+            timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CCR3);
+            break;
+        case __builtin_clz(TIM_IT_CC4):
+            timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CCR4);
+            break;
+        }
+    }
+#else
+    if (tim_status & (int)TIM_IT_Update) {
+        tim->SR = ~TIM_IT_Update;
+        capture = tim->ARR;
+        timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
+        while(cb) {
+            cb->fn(cb, capture);
+            cb = cb->next;
+        }
+    }
+    if (tim_status & (int)TIM_IT_CC1) {
+        tim->SR = ~TIM_IT_CC1;
+        timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CCR1);
+    }
+    if (tim_status & (int)TIM_IT_CC2) {
+        tim->SR = ~TIM_IT_CC2;
+        timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[1], tim->CCR2);
+    }
+    if (tim_status & (int)TIM_IT_CC3) {
+        tim->SR = ~TIM_IT_CC3;
+        timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CCR3);
+    }
+    if (tim_status & (int)TIM_IT_CC4) {
+        tim->SR = ~TIM_IT_CC4;
+        timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CCR4);
+    }
+#endif
 }
 
-void TIM2_IRQHandler(void)
-{
-    timCCxHandler(TIM2);
-}
+// handler for shared interrupts when both timers need to check status bits
+#define _TIM_IRQ_HANDLER2(name, i, j)                                   \
+    void name(void)                                                     \
+    {                                                                   \
+        timCCxHandler(TIM ## i, &timerConfig[TIMER_INDEX(i)]);          \
+        timCCxHandler(TIM ## j, &timerConfig[TIMER_INDEX(j)]);          \
+    } struct dummy
 
-void TIM3_IRQHandler(void)
-{
-    timCCxHandler(TIM3);
-}
+#define _TIM_IRQ_HANDLER(name, i)                                       \
+    void name(void)                                                     \
+    {                                                                   \
+        timCCxHandler(TIM ## i, &timerConfig[TIMER_INDEX(i)]);          \
+    } struct dummy
 
-void TIM4_IRQHandler(void)
-{
-    timCCxHandler(TIM4);
-}
-
-#if defined(STM32F303) || defined(STM32F3DISCOVERY)
-void TIM8_CC_IRQHandler(void)
-{
-    timCCxHandler(TIM8);
-}
-
-void TIM1_BRK_TIM15_IRQHandler(void)
-{
-    timCCxHandler(TIM15);
-}
-
-void TIM1_UP_TIM16_IRQHandler(void)
-{
-    timCCxHandler(TIM16);
-}
-
-void TIM1_TRG_COM_TIM17_IRQHandler(void)
-{
-    timCCxHandler(TIM17);
-}
+#if USED_TIMERS & TIM_N(1)
+_TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
+# if defined(STM32F10X)
+_TIM_IRQ_HANDLER(TIM1_UP_IRQHandler, 1);       // timer can't be shared
+# endif
+# ifdef STM32F303xC
+#  if USED_TIMERS & TIM_N(16)
+_TIM_IRQ_HANDLER2(TIM1_UP_TIM16_IRQHandler, 1, 16);  // both timers are in use
+#  else
+_TIM_IRQ_HANDLER(TIM1_UP_TIM16_IRQHandler, 1);       // timer16 is not used
+#  endif
+# endif
+#endif
+#if USED_TIMERS & TIM_N(2)
+_TIM_IRQ_HANDLER(TIM2_IRQHandler, 2);
+#endif
+#if USED_TIMERS & TIM_N(3)
+_TIM_IRQ_HANDLER(TIM3_IRQHandler, 3);
+#endif
+#if USED_TIMERS & TIM_N(4)
+_TIM_IRQ_HANDLER(TIM4_IRQHandler, 4);
+#endif
+#if USED_TIMERS & TIM_N(8)
+_TIM_IRQ_HANDLER(TIM8_CC_IRQHandler, 8);
+# if defined(STM32F10X_XL)
+_TIM_IRQ_HANDLER(TIM8_UP_TIM13_IRQHandler, 8);
+# else  // f10x_hd, f30x
+_TIM_IRQ_HANDLER(TIM8_UP_IRQHandler, 8);
+# endif
+#endif
+#if USED_TIMERS & TIM_N(15)
+_TIM_IRQ_HANDLER(TIM1_BRK_TIM15_IRQHandler, 15);
+#endif
+#if defined(STM32F303xC) && ((USED_TIMERS & (TIM_N(1)|TIM_N(16))) == (TIM_N(16)))
+_TIM_IRQ_HANDLER(TIM1_UP_TIM16_IRQHandler, 16);    // only timer16 is used, not timer1
+#endif
+#if USED_TIMERS & TIM_N(17)
+_TIM_IRQ_HANDLER(TIM1_TRG_COM_TIM17_IRQHandler, 17);
 #endif
 
 void timerInit(void)
@@ -482,4 +753,41 @@ void timerInit(void)
 #endif
 #endif
 
+// initialize timer channel structures
+    for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+        timerChannelInfo[i].type = TYPE_FREE;
+    }
+    for(int i = 0; i < USED_TIMER_COUNT; i++) {
+        timerInfo[i].priority = ~0;
+    }
+}
+
+// finish configuring timers after allocation phase
+// start timers
+// TODO - Work in progress - initialization routine must be modified/verified to start correctly without timers
+void timerStart(void)
+{
+#if 0
+    for(unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) {
+        int priority = -1;
+        int irq = -1;
+        for(unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++)
+            if((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) {
+                // TODO - move IRQ to timer info
+                irq = timerHardware[hwc].irq;
+            }
+        // TODO - aggregate required timer paramaters
+        configTimeBase(usedTimers[timer], 0, 1);
+        TIM_Cmd(usedTimers[timer],  ENABLE);
+        if(priority >= 0) {  // maybe none of the channels was configured
+            NVIC_InitTypeDef NVIC_InitStructure;
+
+            NVIC_InitStructure.NVIC_IRQChannel = irq;
+            NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SPLIT_PRIORITY_BASE(priority);
+            NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_SPLIT_PRIORITY_SUB(priority);
+            NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+            NVIC_Init(&NVIC_InitStructure);
+        }
+    }
+#endif
 }
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index 2c7693b66..08fbab597 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -29,14 +29,36 @@
 #define USABLE_TIMER_CHANNEL_COUNT 14
 #endif
 
-#ifdef STM32F303xC
-typedef uint32_t captureCompare_t;
-#endif
-#ifdef STM32F10X
-typedef uint16_t captureCompare_t;
+typedef uint16_t captureCompare_t;        // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t)
+
+#if defined(STM32F303)
+typedef uint32_t timCCR_t;
+typedef uint32_t timCCER_t;
+typedef uint32_t timSR_t;
+typedef uint32_t timCNT_t;
+#elif defined(STM32F10X)
+typedef uint16_t timCCR_t;
+typedef uint16_t timCCER_t;
+typedef uint16_t timSR_t;
+typedef uint16_t timCNT_t;
+#else
+# error "Unknown CPU defined"
 #endif
 
-typedef void timerCCCallbackPtr(uint8_t port, captureCompare_t capture);
+// use different types from capture and overflow - multiple overflow handlers are implemented as linked list
+struct timerCCHandlerRec_s;
+struct timerOvrHandlerRec_s;
+typedef void timerCCHandlerCallback(struct timerCCHandlerRec_s* self, uint16_t capture);
+typedef void timerOvrHandlerCallback(struct timerOvrHandlerRec_s* self, uint16_t capture);
+
+typedef struct timerCCHandlerRec_s {
+    timerCCHandlerCallback* fn;
+} timerCCHandlerRec_t;
+
+typedef struct timerOvrHandlerRec_s {
+    timerOvrHandlerCallback* fn;
+    struct timerOvrHandlerRec_s* next;
+} timerOvrHandlerRec_t;
 
 typedef struct {
     TIM_TypeDef *tim;
@@ -47,18 +69,53 @@ typedef struct {
     uint8_t outputEnable;
     GPIO_Mode gpioInputMode;
 #ifdef STM32F303
-    uint8_t gpioPinSource;
+    uint8_t gpioPinSource;             // TODO - this can be removed and pinSource calculated from pin
     uint8_t alternateFunction;
 #endif
 } timerHardware_t;
 
 extern const timerHardware_t timerHardware[];
 
-void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz);
-void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz);
-void timerNVICConfigure(uint8_t irq);
+typedef enum {
+    TYPE_FREE,
+    TYPE_PWMINPUT,
+    TYPE_PPMINPUT,
+    TYPE_PWMOUTPUT_MOTOR,
+    TYPE_PWMOUTPUT_FAST,
+    TYPE_PWMOUTPUT_SERVO,
+    TYPE_SOFTSERIAL_RX,
+    TYPE_SOFTSERIAL_TX,
+    TYPE_SOFTSERIAL_RXTX,        // bidirectional pin for softserial
+    TYPE_SOFTSERIAL_AUXTIMER,    // timer channel is used for softserial. No IO function on pin
+    TYPE_ADC,
+    TYPE_SERIAL_RX,
+    TYPE_SERIAL_TX,
+    TYPE_SERIAL_RXTX,
+    TYPE_TIMER
+} channelType_t;
 
-void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel);
-void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *edgeCallback, timerCCCallbackPtr *overflowCallback);
-void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *edgeCallback);
-void configureTimerChannelCallbacks(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *edgeCallback, timerCCCallbackPtr *overflowCallback);
+void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint8_t mhz);  // This interface should be replaced.
+
+void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterSamples);
+void timerChConfigICDual(const timerHardware_t* timHw, bool polarityRising, unsigned inputFilterSamples);
+void timerChICPolarity(const timerHardware_t *timHw, bool polarityRising);
+volatile timCCR_t* timerChCCR(const timerHardware_t* timHw);
+volatile timCCR_t* timerChCCRLo(const timerHardware_t* timHw);
+volatile timCCR_t* timerChCCRHi(const timerHardware_t* timHw);
+void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh);
+void timerChConfigGPIO(const timerHardware_t* timHw, GPIO_Mode mode);
+
+void timerChCCHandlerInit(timerCCHandlerRec_t *self, timerCCHandlerCallback *fn);
+void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *fn);
+void timerChConfigCallbacks(const timerHardware_t *channel, timerCCHandlerRec_t *edgeCallback, timerOvrHandlerRec_t *overflowCallback);
+void timerChConfigCallbacksDual(const timerHardware_t *channel, timerCCHandlerRec_t *edgeCallbackLo, timerCCHandlerRec_t *edgeCallbackHi, timerOvrHandlerRec_t *overflowCallback);
+void timerChITConfigDualLo(const timerHardware_t* timHw, FunctionalState newState);
+void timerChITConfig(const timerHardware_t* timHw, FunctionalState newState);
+void timerChClearCCFlag(const timerHardware_t* timHw);
+
+void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority);
+
+void timerInit(void);
+void timerStart(void);
+
+void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz);  // TODO - just for migration
diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h
new file mode 100644
index 000000000..5a8868b37
--- /dev/null
+++ b/src/main/drivers/timer_impl.h
@@ -0,0 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+void timerInitTarget(void);
diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c
new file mode 100644
index 000000000..f8016950f
--- /dev/null
+++ b/src/main/drivers/timer_stm32f10x.c
@@ -0,0 +1,67 @@
+/*
+  modified version of StdPeriph function is located here.
+  TODO - what license does apply here?
+  original file was lincesed under MCD-ST Liberty SW License Agreement V2
+  http://www.st.com/software_license_agreement_liberty_v2
+*/
+
+#include "stm32f10x.h"
+
+/**
+  * @brief  Selects the TIM Output Compare Mode.
+  * @note   This function does NOT disable the selected channel before changing the Output
+  *         Compare Mode.
+  * @param  TIMx: where x can be 1 to 17 except 6 and 7 to select the TIM peripheral.
+  * @param  TIM_Channel: specifies the TIM Channel
+  *   This parameter can be one of the following values:
+  *     @arg TIM_Channel_1: TIM Channel 1
+  *     @arg TIM_Channel_2: TIM Channel 2
+  *     @arg TIM_Channel_3: TIM Channel 3
+  *     @arg TIM_Channel_4: TIM Channel 4
+  * @param  TIM_OCMode: specifies the TIM Output Compare Mode.
+  *   This parameter can be one of the following values:
+  *     @arg TIM_OCMode_Timing
+  *     @arg TIM_OCMode_Active
+  *     @arg TIM_OCMode_Toggle
+  *     @arg TIM_OCMode_PWM1
+  *     @arg TIM_OCMode_PWM2
+  *     @arg TIM_ForcedAction_Active
+  *     @arg TIM_ForcedAction_InActive
+  * @retval None
+  */
+
+#define CCMR_Offset                 ((uint16_t)0x0018)
+
+void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
+{
+  uint32_t tmp = 0;
+
+  /* Check the parameters */
+  assert_param(IS_TIM_LIST8_PERIPH(TIMx));
+  assert_param(IS_TIM_CHANNEL(TIM_Channel));
+  assert_param(IS_TIM_OCM(TIM_OCMode));
+
+  tmp = (uint32_t) TIMx;
+  tmp += CCMR_Offset;
+
+  if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3))
+  {
+    tmp += (TIM_Channel>>1);
+
+    /* Reset the OCxM bits in the CCMRx register */
+    *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
+
+    /* Configure the OCxM bits in the CCMRx register */
+    *(__IO uint32_t *) tmp |= TIM_OCMode;
+  }
+  else
+  {
+    tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1;
+
+    /* Reset the OCxM bits in the CCMRx register */
+    *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
+
+    /* Configure the OCxM bits in the CCMRx register */
+    *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
+  }
+}
diff --git a/src/main/drivers/timer_stm32f10x.h b/src/main/drivers/timer_stm32f10x.h
new file mode 100644
index 000000000..be46e4f37
--- /dev/null
+++ b/src/main/drivers/timer_stm32f10x.h
@@ -0,0 +1,6 @@
+
+#pragma once
+
+#include "stm32f10x.h"
+
+void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);
diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c
new file mode 100644
index 000000000..458ef824f
--- /dev/null
+++ b/src/main/drivers/timer_stm32f30x.c
@@ -0,0 +1,75 @@
+/*
+  modified version of StdPeriph function is located here.
+  TODO - what license does apply here?
+  original file was lincesed under MCD-ST Liberty SW License Agreement V2
+  http://www.st.com/software_license_agreement_liberty_v2
+*/
+
+#include "stm32f30x.h"
+
+/**
+  * @brief  Selects the TIM Output Compare Mode.
+  * @note   This function does NOT disable the selected channel before changing the Output
+  *         Compare Mode. If needed, user has to enable this channel using
+  *         TIM_CCxCmd() and TIM_CCxNCmd() functions.
+  * @param  TIMx: where x can be 1, 2, 3, 4, 8, 15, 16 or 17 to select the TIM peripheral.
+  * @param  TIM_Channel: specifies the TIM Channel
+  *          This parameter can be one of the following values:
+  *            @arg TIM_Channel_1: TIM Channel 1
+  *            @arg TIM_Channel_2: TIM Channel 2
+  *            @arg TIM_Channel_3: TIM Channel 3
+  *            @arg TIM_Channel_4: TIM Channel 4
+  * @param  TIM_OCMode: specifies the TIM Output Compare Mode.
+  *           This parameter can be one of the following values:
+  *            @arg TIM_OCMode_Timing
+  *            @arg TIM_OCMode_Active
+  *            @arg TIM_OCMode_Toggle
+  *            @arg TIM_OCMode_PWM1
+  *            @arg TIM_OCMode_PWM2
+  *            @arg TIM_ForcedAction_Active
+  *            @arg TIM_ForcedAction_InActive
+  *            @arg TIM_OCMode_Retrigerrable_OPM1
+  *            @arg TIM_OCMode_Retrigerrable_OPM2
+  *            @arg TIM_OCMode_Combined_PWM1
+  *            @arg TIM_OCMode_Combined_PWM2
+  *            @arg TIM_OCMode_Asymmetric_PWM1
+  *            @arg TIM_OCMode_Asymmetric_PWM2
+  * @retval None
+  */
+#define CCMR_OFFSET        ((uint16_t)0x0018)
+#define CCMR_OC13M_MASK    ((uint32_t)0xFFFEFF8F)
+#define CCMR_OC24M_MASK    ((uint32_t)0xFEFF8FFF) 
+
+void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode)
+{
+  uint32_t tmp = 0;
+
+  /* Check the parameters */
+  assert_param(IS_TIM_LIST1_PERIPH(TIMx));
+  assert_param(IS_TIM_CHANNEL(TIM_Channel));
+  assert_param(IS_TIM_OCM(TIM_OCMode));
+
+  tmp = (uint32_t) TIMx;
+  tmp += CCMR_OFFSET;
+
+  if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3))
+  {
+    tmp += (TIM_Channel>>1);
+
+    /* Reset the OCxM bits in the CCMRx register */
+    *(__IO uint32_t *) tmp &= CCMR_OC13M_MASK;
+
+    /* Configure the OCxM bits in the CCMRx register */
+    *(__IO uint32_t *) tmp |= TIM_OCMode;
+  }
+  else
+  {
+    tmp += (uint32_t)(TIM_Channel - (uint32_t)4)>> (uint32_t)1;
+
+    /* Reset the OCxM bits in the CCMRx register */
+    *(__IO uint32_t *) tmp &= CCMR_OC24M_MASK;
+
+    /* Configure the OCxM bits in the CCMRx register */
+    *(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8);
+  }
+}
diff --git a/src/main/drivers/timer_stm32f30x.h b/src/main/drivers/timer_stm32f30x.h
new file mode 100644
index 000000000..781385875
--- /dev/null
+++ b/src/main/drivers/timer_stm32f30x.h
@@ -0,0 +1,6 @@
+
+#pragma once
+
+#include "stm32f30x.h"
+
+void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);
diff --git a/src/main/main.c b/src/main/main.c
index 6608e010e..a7d0d3153 100755
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -24,6 +24,8 @@
 
 #include "common/axis.h"
 #include "common/color.h"
+#include "common/atomic.h"
+#include "drivers/nvic.h"
 
 #include "drivers/system.h"
 #include "drivers/gpio.h"
@@ -153,6 +155,8 @@ void init(void)
 
     delay(100);
 
+    timerInit();  // timer must be initialized before any channel is allocated
+
     ledInit();
 
 #ifdef BEEPER
@@ -250,8 +254,6 @@ void init(void)
         compassInit();
 #endif
 
-    timerInit();
-
     serialInit(&masterConfig.serialConfig);
 
     memset(&pwm_params, 0, sizeof(pwm_params));
@@ -333,6 +335,10 @@ void init(void)
     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 #endif
 
+    // start all timers
+    // TODO - not implemented yet
+    timerStart();
+
     ENABLE_STATE(SMALL_ANGLE);
     DISABLE_ARMING_FLAG(PREVENT_ARMING);
 
diff --git a/src/main/startup/startup_stm32f30x_md_gcc.S b/src/main/startup/startup_stm32f30x_md_gcc.S
index be696fd0b..9c2af124c 100644
--- a/src/main/startup/startup_stm32f30x_md_gcc.S
+++ b/src/main/startup/startup_stm32f30x_md_gcc.S
@@ -4,7 +4,7 @@
   * @author    MCD Application Team
   * @version   V1.0.0
   * @date      04-Spetember-2012
-  * @brief     STM32F30x Devices vector table for RIDE7 toolchain. 
+  * @brief     STM32F30x Devices vector table for RIDE7 toolchain.
   *            This module performs:
   *                - Set the initial SP
   *                - Set the initial PC == Reset_Handler,