Code:
// 1ms warten bei 16MHz
void waitms(uint16_t ms) {
	uint16_t i; 

	for(i=0; i<ms; i++) {
		//delaying 15996 cycles:
		asm volatile(
			"ldi R17, 0x1F	\n\t" \
			"L1:			\n\t" \
			"ldi R18, 0xAB	\n\t" \
			"L2:			\n\t" \
			"dec R18		\n\t" \
			"brne L2		\n\t" \
			"dec R17		\n\t" \
			"brne L1 		\n\t"
		);	
	}
}
Die funktion verbraucht ca. 16000 Takte was bei 16MHz eine ms sind...