diff --git a/Makefile b/Makefile
index c4d0260..b1367c6 100644
--- a/Makefile
+++ b/Makefile
@@ -80,7 +80,7 @@ PROGID = arduino
 
 PROGRAM = mendel
 
-SOURCES = $(PROGRAM).c dda.c gcode_parse.c gcode_process.c timer.c temp.c sermsg.c dda_queue.c watchdog.c debug.c sersendf.c heater.c analog.c intercom.c pinio.c clock.c home.c crc.c
+SOURCES = $(PROGRAM).c dda.c gcode_parse.c gcode_process.c timer.c temp.c sermsg.c dda_queue.c watchdog.c debug.c sersendf.c heater.c analog.c intercom.c pinio.c clock.c home.c crc.c delay.c
 
 ARCH = avr-
 CC = $(ARCH)gcc
diff --git a/delay.c b/delay.c
new file mode 100644
index 0000000..0468c02
--- /dev/null
+++ b/delay.c
@@ -0,0 +1,35 @@
+#include	"delay.h"
+
+/** \file
+	\brief Delay routines
+*/
+
+#include <util/delay_basic.h>
+
+#include	"watchdog.h"
+
+/// delay microseconds
+/// \param delay time to wait in microseconds
+void delay_us(uint16_t delay) {
+	wd_reset();
+	while (delay > (65536L / (F_CPU / 4000000L))) {
+		_delay_loop_2(65534); // we use 65534 here to compensate for the time that the surrounding loop takes. TODO: exact figure needs tuning
+		delay -= (65536L / (F_CPU / 4000000L));
+		wd_reset();
+	}
+	_delay_loop_2(delay / (F_CPU / 4000000L));
+	wd_reset();
+}
+
+/// delay milliseconds
+/// \param delay time to wait in milliseconds
+void _delay_ms(uint32_t delay) {
+	wd_reset();
+	while (delay > 65) {
+		delay_us(64999);
+		delay -= 65;
+		wd_reset();
+	}
+	delay_us(delay * 1000);
+	wd_reset();
+}
diff --git a/delay.h b/delay.h
index 1b81f13..268c570 100644
--- a/delay.h
+++ b/delay.h
@@ -2,88 +2,29 @@
 #define	_DELAY_H
 
 #include	<stdint.h>
-#include 	<avr/builtins.h>
-#include	"watchdog.h"
-#include	<math.h>
+#include	<util/delay_basic.h>
 
-// WAITING_DELAY is expressed in microseconds
 #define		WAITING_DELAY		10000
 
-// Force delay functions to be inlined. 
-// Otherwise they will not work correctly. 
-static inline void delay_us(uint16_t us) __attribute__((always_inline));
-static inline void delay(uint32_t delay) __attribute__((always_inline));
-static inline void delay_ms( uint16_t delay ) __attribute__((always_inline));
+void delay_us(uint16_t delay);
 
-// Delay for a minimum of us microseconds. 
-// The parameter us MUST be a compile time constant.
-// A compiler error will be issued in the case that 
-// it is not.
+void _delay_ms(uint32_t delay);
 
-void delay_us(uint16_t us)
-{
-	// The floating point calculation will 
-	// be completed during compilation, so
-	// there is no runtime floating point
-	// code generated.
-    uint32_t cycles = ceil( (double)F_CPU * us / 1000000.0 );
-    __builtin_avr_delay_cycles(cycles);
+static void delay(uint32_t) __attribute__ ((always_inline));
+inline void delay(uint32_t d) {
+	if (d >= (65536L / (F_CPU / 4000000L))) {
+		delay_us(d);
+	}
+	else
+		_delay_loop_2(d * (F_CPU / 4000000L));
 }
 
-// Delay for a minimum of us microseconds.
-// If the watchdog functionality is enabled
-// this function will reset the timer before
-// and after the delay (and at least once every
-// 65536 microseconds).
-//
-// This function is forced (see declaration above) to be inlined.
-// The parameter us MUST be a compile time constant.
-// A compiler error will be issued in the case that 
-// it is not.
-void delay(uint32_t us)
-{
-    int i;
-
-    wd_reset();
-    for( i = 0; i < us/65536; i++ )
-    {
-        delay_us(65535);
-        delay_us(1);
-        wd_reset();
-    }
-    if( us%65536 )
-    {
-        delay_us(us%65536);
-        wd_reset();
-    }
-}
-
-// Delay for a minimum of ms milliseconds.
-// If the watchdog functionality is enabled
-// this function will reset the timer before
-// and after the delay (and at least once every
-// 65000 microseconds).
-//
-// This function is forced (see declaration above) to be inlined.
-// The parameter us MUST be a compile time constant.
-// A compiler error will be issued in the case that 
-// it is not.
-void delay_ms( uint16_t ms )
-{
-    uint16_t i;
-
-    wd_reset();
-    for( i = 0; i < ms/65; i++ )
-    {
-        delay_us(65000);
-        wd_reset();
-    }
-    
-    if( ms%65 )
-    {
-        delay_us((ms%65536)*1000);
-        wd_reset();
-    }
+static void delay_ms(uint32_t) __attribute__ ((always_inline));
+inline void delay_ms(uint32_t d) {
+	if (d > 65)
+		delay_ms(d);
+	else
+		delay_us(d * 1000);
 }
 
 #endif	/* _DELAY_H */