diff --git a/Makefile b/Makefile
index 9f9f4ea..c4d0260 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 delay.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
 
 ARCH = avr-
 CC = $(ARCH)gcc
diff --git a/README b/README
index f2f2561..df2abb8 100644
--- a/README
+++ b/README
@@ -176,7 +176,7 @@ The queue of moves received from the host.
 *** debug.[ch]
 Debugging aids
 
-*** delay.[ch]
+*** delay.h
 Delay functions
 
 *** Teacup.pde
diff --git a/delay.c b/delay.c
deleted file mode 100644
index b5840a2..0000000
--- a/delay.c
+++ /dev/null
@@ -1,58 +0,0 @@
-#include	"delay.h"
-
-/** \file
-	\brief Delay routines
-*/
-
-#include	"watchdog.h"
-
-/// delay microseconds
-/// \param delay time to wait in microseconds
-void delay(uint32_t delay) {
-	wd_reset();
-	while (delay > 65535) {
-		delayMicrosecondsInterruptible(65533);
-		delay -= 65535;
-		wd_reset();
-	}
-	delayMicrosecondsInterruptible(delay & 0xFFFF);
-	wd_reset();
-}
-
-/// delay milliseconds
-/// \param delay time to wait in milliseconds
-void delay_ms(uint32_t delay) {
-	wd_reset();
-	while (delay > 65) {
-		delayMicrosecondsInterruptible(64999);
-		delay -= 65;
-		wd_reset();
-	}
-	delayMicrosecondsInterruptible(delay * 1000);
-	wd_reset();
-}
-
-/// internal- wait for up to 65.5ms using a busy loop
-/// \param us time to wait in microseconds
-void delayMicrosecondsInterruptible(uint16_t us)
-{
-	// for a one-microsecond delay, simply return.  the overhead
-	// of the function call yields a delay of approximately 1 1/8 us.
-	if (--us == 0)
-		return;
-
-	// the following loop takes a quarter of a microsecond (4 cycles)
-	// per iteration, so execute it four times for each microsecond of
-	// delay requested.
-	us <<= 2;
-
-	// account for the time taken in the preceeding commands.
-	us -= 2;
-
-	// busy wait
-	__asm__ __volatile__ ("1: sbiw %0,1" "\n\t" // 2 cycles
-	"brne 1b" :
-	"=w" (us) :
-	"0" (us) // 2 cycles
-	);
-}
diff --git a/delay.h b/delay.h
index 83aaf36..e96dfc4 100644
--- a/delay.h
+++ b/delay.h
@@ -2,14 +2,84 @@
 #define	_DELAY_H
 
 #include	<stdint.h>
+#include 	<avr/builtins.h>
+#include	"watchdog.h"
+#include	<math.h>
 
-#define		WAITING_DELAY		10 MS
+// WAITING_DELAY is expressed in microseconds
+#define		WAITING_DELAY		10000
 
-void delay(uint32_t delay);
+// 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_ms(uint32_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.
 
-#define	delay_us(d) delayMicrosecondsInterruptible(d)
-void delayMicrosecondsInterruptible(unsigned int us);
+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);
+}
+
+// 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)
+{
+    wd_reset();
+    for( int 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 )
+{
+    wd_reset();
+    for( uint16_t i = 0; i < ms/65; i++ )
+    {
+        delay_us(65000);
+        wd_reset();
+    }
+    
+    if( ms%65 )
+    {
+        delay_us((ms%65536)*1000);
+        wd_reset();
+    }
+}
 
 #endif	/* _DELAY_H */