diff --git a/lib_m68k/time.c b/lib_m68k/time.c
index d45e470..3029949 100644
--- a/lib_m68k/time.c
+++ b/lib_m68k/time.c
@@ -141,6 +141,17 @@ void udelay(unsigned long usec)
 
 	timerp = (volatile unsigned short *) (CFG_MBAR + MCFTIMER_BASE3);
 
+#if CFG_CLK>=20000000
+	usec = (usec * (CFG_CLK / 1000000)) >> 1;
+#else
+	usec = usec * (2000000 / CFG_CLK);
+#endif
+	/* Set up TIMER 3 as timebase clock */
+	timerp[MCFTIMER_PCSR] = MCFTIMER_PCSR_OVW;
+	timerp[MCFTIMER_PMR] = 0;
+	/* set period to 2/Clock */
+	timerp[MCFTIMER_PCSR] = (0<<8) | MCFTIMER_PCSR_EN | MCFTIMER_PCSR_OVW |
+                             MCFTIMER_PCSR_PIF;
 	while (usec > 0) {
 		if (usec > 65000)
 			tmp = 65000;
@@ -148,18 +159,62 @@ void udelay(unsigned long usec)
 			tmp = usec;
 		usec = usec - tmp;
 
-		/* Set up TIMER 3 as timebase clock */
-		timerp[MCFTIMER_PCSR] = MCFTIMER_PCSR_OVW;
-		timerp[MCFTIMER_PMR] = 0;
-		/* set period to 1 us */
-		timerp[MCFTIMER_PCSR] =
-			(5 << 8) | MCFTIMER_PCSR_EN | MCFTIMER_PCSR_OVW;
 
 		timerp[MCFTIMER_PMR] = tmp;
-		while (timerp[MCFTIMER_PCNTR] > 0);
+		while ((timerp[MCFTIMER_PCSR] & MCFTIMER_PCSR_PIF) == 0);
+		timerp[MCFTIMER_PCSR] |= MCFTIMER_PCSR_PIF;
 	}
 }
 
+#if CFG_HZ == 1000
+
+void timer_isr (void * not_used)
+{
+	volatile unsigned short *timerp;
+	timerp = (volatile unsigned short *) (CFG_MBAR + MCFTIMER_BASE4);
+	timerp[MCFTIMER_PCSR] |= MCFTIMER_PCSR_PIF;
+	timestamp++;
+}
+
+void timer_init (void)
+{
+	volatile unsigned short *timerp;
+#if (CFG_CLK/2000) <65000
+	#define TI_PMR CFG_CLK/2000
+	#define TI_PRE 0
+#else
+	#define TI_PMR CFG_CLK/4000
+	#define TI_PRE 1
+#endif
+
+	timerp = (volatile unsigned short *) (CFG_MBAR + MCFTIMER_BASE4);
+	timestamp = 0;
+
+	irq_install_handler (MCFVECTOR_PIT3,timer_isr,0);
+
+	/* Set up TIMER 4 as 1ms interupt event */
+	timerp[MCFTIMER_PCSR] = MCFTIMER_PCSR_OVW;
+	timerp[MCFTIMER_PMR]  = TI_PMR;
+	timerp[MCFTIMER_PCSR] =	(TI_PRE << 8) | MCFTIMER_PCSR_RLD |
+                            MCFTIMER_PCSR_EN | MCFTIMER_PCSR_OVW |
+							MCFTIMER_PCSR_PIF | MCFTIMER_PCSR_PIE;
+
+	MCFICM_ICR_PIT3 = (3<<MCFICM_ICR_IL) | (3<<MCFICM_ICR_IP);
+
+	MCFICM_IMRH0 &= ~MCFICM_IMRH0_PIT3;
+}
+
+void set_timer (ulong t)
+{
+	timestamp = t;
+}
+
+ulong get_timer (ulong base)
+{
+	return timestamp - base;
+}
+
+#else
 void timer_init (void)
 {
 	volatile unsigned short *timerp;
@@ -202,8 +257,9 @@ void wait_ticks (unsigned long ticks)
 	set_timer (0);
 	while (get_timer (0) < ticks);
 }
-#endif
+#endif /* CFG_HZ=1000 */
 
+#endif /* defined(CONFIG_M5282) || defined(CONFIG_M5271) */
 
 #if defined(CONFIG_M5249)
 /*