/* $Id$

This file is part of R²C²
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