X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=common%2Fdelay.h;fp=common%2Fdelay.h;h=725d22b5b41fe6ce6bcf00eff5373ae847b533e3;hb=9c37d18b9c70fdb70dfec453398c4649e9e57586;hp=0000000000000000000000000000000000000000;hpb=49b6b6ad84ec47b4f9eb9ef131975cc5b72372a2;p=model-railway-devices.git

diff --git a/common/delay.h b/common/delay.h
new file mode 100644
index 0000000..725d22b
--- /dev/null
+++ b/common/delay.h
@@ -0,0 +1,45 @@
+#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