Added wait

3 files changed, 10 insertions, 6 deletions

diff --git a/include/util/time.h b/include/util/time.h
index dc51e00..e5e18b1 100644
--- a/include/util/time.h
+++ b/include/util/time.h
@@ -10,6 +10,7 @@ unsigned long read_cntv_tval(void);
 void write_cntv_tval(unsigned long val);
 unsigned long read_cntfrq(void);
 unsigned long long get_time(void);
+void wait_msec(unsigned int n);
 
 static inline void enablecntv(void)
 {
diff --git a/later/sd.c b/later/sd.c
index ec07699..aa8a1fa 100644
--- a/later/sd.c
+++ b/later/sd.c
@@ -1,7 +1,9 @@
+// Adapted from bztsrc
 #include <drivers/uart.h>
 #include <drivers/sd.h>
 #include <symbols.h>
 #include <sys/core.h>
+#include <util/time.h>
 
 #define EMMC_ARG2           ((volatile unsigned int*)(MMIO_BASE+0x00300000))
 #define EMMC_BLKSIZECNT     ((volatile unsigned int*)(MMIO_BASE+0x00300004))
@@ -91,12 +93,6 @@
 
 unsigned long sd_scr[2], sd_ocr, sd_rca, sd_err, sd_hv;
 
-void wait_msec(unsigned int n)
-{
-	for(unsigned long i = 0; i < n*100; i++)
-		asm volatile("nop");
-}
-
 /**
  * Wait for data or command ready
  */
diff --git a/src/util/time.c b/src/util/time.c
index 0ae3ccf..abb9c8d 100644
--- a/src/util/time.c
+++ b/src/util/time.c
@@ -67,3 +67,10 @@ unsigned long long get_time(void)
 	t.tvalb.high = *(unsigned long*)SYS_TIMER_CHI;
 	return t.tval;
 }
+
+void wait_msec(unsigned int n)
+{
+	unsigned long start = *(volatile unsigned long*)SYS_TIMER_CHI;
+	while (*(volatile unsigned long*)SYS_TIMER_CHI - start < n)
+		asm volatile("nop");
+}