Support multiple timers (currently 0 and 1)

[model-railway-devices.git] / firmware / delay.h

diff --git a/firmware/delay.h b/firmware/delay.h
new file mode 100644 (file)
index 0000000..1798cec
--- /dev/null
+++ b/firmware/delay.h
@@ -0,0 +1,52 @@
+/* $Id$
+
+This file is part of the MSP Märklin suite
+Copyright © 2010  Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#ifndef DELAY_H_
+#define DELAY_H_
+
+static inline void __attribute__((always_inline)) delay_loop8(uint8_t count)
+{
+       __asm__ volatile (
+               "1: dec %0" "\n\t"
+               "brne 1b"
+               : "=r" (count)
+               : "0" (count)
+       );
+}
+
+static inline void __attribute__((always_inline)) delay_loop16(uint16_t count)
+{
+       __asm__ volatile (
+               "1: sbiw %0, 1" "\n\t"
+               "brne 1b"
+               : "=r" (count)
+               : "0" (count)
+       );
+}
+
+static inline void __attribute__((always_inline)) delay_us(uint16_t us)
+{
+       uint16_t clocks = F_CPU/1000000*us;
+       if(clocks<768)
+               delay_loop8(clocks/3);
+       else
+               delay_loop16(clocks/4);
+}
+
+static inline void __attribute__((always_inline)) delay_ms(uint16_t ms)
+{
+       if(ms<0x40000/(F_CPU/1000))
+               delay_loop16(F_CPU/1000*ms/4);
+       else
+       {
+               uint16_t i = ms*10;
+               while(--i)
+                       delay_loop16(F_CPU/40000);
+       }
+}
+
+#endif