diff --git a/sys/arm/versatile/sp804.c b/sys/arm/versatile/sp804.c
index 894415ba11c..ecc3b7cba8f 100644
--- a/sys/arm/versatile/sp804.c
+++ b/sys/arm/versatile/sp804.c
@@ -42,6 +42,10 @@ __FBSDID("$FreeBSD$");
 #include <machine/cpu.h>
 #include <machine/intr.h>
 
+#ifdef MULTIDELAY
+#include <machine/machdep.h> /* For arm_set_delay */
+#endif
+
 #include <dev/ofw/openfirm.h>
 #include <dev/ofw/ofw_bus.h>
 #include <dev/ofw/ofw_bus_subr.h>
@@ -109,6 +113,7 @@ struct sp804_timer_softc {
 	bus_space_write_4(sc->bst, sc->bsh, reg, val)
 
 static unsigned sp804_timer_tc_get_timecount(struct timecounter *);
+static void sp804_timer_delay(int, void *);
 
 static unsigned
 sp804_timer_tc_get_timecount(struct timecounter *tc)
@@ -287,6 +292,10 @@ sp804_timer_attach(device_t dev)
 		     (sp804_timer_tc_read_4(SP804_PRIMECELL_ID0 + i*4) & 0xff);
 	}
 
+#ifdef MULTIDELAY
+	arm_set_delay(sp804_timer_delay, sc);
+#endif
+
 	device_printf(dev, "PrimeCell ID: %08x\n", id);
 
 	sc->timer_initialized = 1;
@@ -310,11 +319,36 @@ static devclass_t sp804_timer_devclass;
 
 DRIVER_MODULE(sp804_timer, simplebus, sp804_timer_driver, sp804_timer_devclass, 0, 0);
 
+static void
+sp804_timer_delay(int usec, void *arg)
+{
+	struct sp804_timer_softc *sc = arg;
+	int32_t counts;
+	uint32_t first, last;
+
+	/* Get the number of times to count */
+	counts = usec * ((sc->tc.tc_frequency / 1000000) + 1);
+
+	first = sp804_timer_tc_get_timecount(&sc->tc);
+
+	while (counts > 0) {
+		last = sp804_timer_tc_get_timecount(&sc->tc);
+		if (last == first)
+			continue;
+		if (last > first) {
+			counts -= (int32_t)(last - first);
+		} else {
+			counts -= (int32_t)((0xFFFFFFFF - first) + last);
+		}
+		first = last;
+	}
+}
+
+#ifndef MULTIDELAY
 void
 DELAY(int usec)
 {
 	int32_t counts;
-	uint32_t first, last;
 	device_t timer_dev;
 	struct sp804_timer_softc *sc;
 	int timer_initialized = 0;
@@ -336,23 +370,8 @@ DELAY(int usec)
 			for (counts = 200; counts > 0; counts--)
 				/* Prevent gcc from optimizing  out the loop */
 				cpufunc_nullop();
-		return;
-	}
-
-	/* Get the number of times to count */
-	counts = usec * ((sc->tc.tc_frequency / 1000000) + 1);
-
-	first = sp804_timer_tc_get_timecount(&sc->tc);
-
-	while (counts > 0) {
-		last = sp804_timer_tc_get_timecount(&sc->tc);
-		if (last == first)
-			continue;
-		if (last>first) {
-			counts -= (int32_t)(last - first);
-		} else {
-			counts -= (int32_t)((0xFFFFFFFF - first) + last);
-		}
-		first = last;
+	} else {
+		sp804_timer_delay(usec, sc);
 	}
 }
+#endif