3 files changed, 41 insertions, 19 deletions
diff --git a/common/timer.c b/common/avr/timer.c
index e0dec6cefc..292b41c3a6 100644
--- a/common/timer.c
+++ b/common/avr/timer.c
@@ -18,6 +18,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <stdint.h>
+#include "timer_avr.h"
 #include "timer.h"
 
 
diff --git a/common/mbed/timer.c b/common/mbed/timer.c
new file mode 100644
index 0000000000..a64a77239c
--- /dev/null
+++ b/common/mbed/timer.c
@@ -0,0 +1,40 @@
+#include "cmsis.h"
+#include "timer.h"
+
+/* Mill second tick count */
+volatile uint32_t timer_count = 0;
+
+/* Timer interrupt handler */
+void SysTick_Handler(void)  {
+    timer_count++;
+}
+
+void timer_init(void)
+{
+    SysTick_Config(SystemCoreClock / 1000); /* 1ms tick */
+}
+
+void timer_clear(void)
+{
+    timer_count = 0;
+}
+
+uint16_t timer_read(void)
+{
+    return (uint16_t)(timer_count & 0xFFFF);
+}
+
+uint32_t timer_read32(void)
+{
+    return timer_count;
+}
+
+uint16_t timer_elapsed(uint16_t last)
+{
+    return TIMER_DIFF_16(timer_read(), last);
+}
+
+uint32_t timer_elapsed32(uint32_t last)
+{
+    return TIMER_DIFF_32(timer_read32(), last);
+}
diff --git a/common/timer.h b/common/timer.h
index 6437473ff7..f0c5ffc98a 100644
--- a/common/timer.h
+++ b/common/timer.h
@@ -20,25 +20,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <stdint.h>
 
-#ifndef TIMER_PRESCALER
-#   if F_CPU > 16000000
-#       define TIMER_PRESCALER      256
-#   elif F_CPU > 2000000
-#       define TIMER_PRESCALER      64
-#   elif F_CPU > 250000
-#       define TIMER_PRESCALER      8
-#   else
-#       define TIMER_PRESCALER      1
-#   endif
-#endif
-#define TIMER_RAW_FREQ      (F_CPU/TIMER_PRESCALER)
-#define TIMER_RAW           TCNT0
-#define TIMER_RAW_TOP       (TIMER_RAW_FREQ/1000)
-
-#if (TIMER_RAW_TOP > 255)
-#   error "Timer0 can't count 1ms at this clock freq. Use larger prescaler."
-#endif
-
 #define TIMER_DIFF(a, b, max)   ((a) >= (b) ?  (a) - (b) : (max) - (b) + (a))
 #define TIMER_DIFF_8(a, b)      TIMER_DIFF(a, b, UINT8_MAX)
 #define TIMER_DIFF_16(a, b)     TIMER_DIFF(a, b, UINT16_MAX)