From 303def49cb71e323ae82a86da44c0398f36ff75b Mon Sep 17 00:00:00 2001
From: David Buezas <dbuezas@users.noreply.github.com>
Date: Mon, 3 Oct 2022 15:14:31 +0200
Subject: [PATCH] [beken-72xx] Implement accurate delayMicroseconds() (#26)

* Implements accurate delayMicroseconds

* renamed to cammelCase, moved #defines and clang-formatted
---
 arduino/beken-72xx/cores/arduino/wiring.c | 47 ++++++++++++++++++-----
 1 file changed, 37 insertions(+), 10 deletions(-)

diff --git a/arduino/beken-72xx/cores/arduino/wiring.c b/arduino/beken-72xx/cores/arduino/wiring.c
index f8932aa..78cc2d1 100644
--- a/arduino/beken-72xx/cores/arduino/wiring.c
+++ b/arduino/beken-72xx/cores/arduino/wiring.c
@@ -7,17 +7,50 @@
 #include <rtos_pub.h>
 #include <sys_rtos.h>
 
+#define TICKS_PER_US	   (CFG_XTAL_FREQUENCE / 1000 / 1000)
+#define TIMER0_US_OVERFLOW (portTICK_PERIOD_MS * 1000)
+#define TICKS_PER_OVERFLOW (TICKS_PER_US * TIMER0_US_OVERFLOW)
+
 void delayMilliseconds(unsigned long ms) {
 	rtos_delay_milliseconds(ms);
 }
 
+static uint32_t getTicksCount() {
+	uint32_t timeout = 0;
+	REG_WRITE(TIMER0_2_READ_CTL, (BKTIMER0 << 2) | 1);
+	while (REG_READ(TIMER0_2_READ_CTL) & 1) {
+		timeout++;
+		if (timeout > (120 * 1000))
+			return 0;
+	}
+	return REG_READ(TIMER0_2_READ_VALUE);
+}
+
 void delayMicroseconds(unsigned int us) {
-	// TODO implement this properly
-	us /= 10;
+#if LT_MICROS_HIGH_RES
+	if (us == 0)
+		return;
+	us--;
+	uint32_t startTick = getTicksCount();
+	/* startTick2 accounts for the case where the timer counter overflows */
+	uint32_t startTick2 = startTick - TICKS_PER_OVERFLOW;
+	uint32_t delayTicks = TICKS_PER_US * us;
+	while (delayTicks > TICKS_PER_OVERFLOW) {
+		// The delay is longer than what the timer can count.
+		// Let it overflow until only a fraction of TICKS_PER_OVERFLOW remain.
+		while (getTicksCount() > startTick) {}
+		while (getTicksCount() < startTick) {}
+		delayTicks -= TICKS_PER_OVERFLOW;
+	}
+	while ((getTicksCount() - startTick < delayTicks) || // normal case
+		   (getTicksCount() - startTick2 < delayTicks)	 // counter overflow case
+	) {}
+#else
 	volatile uint32_t i, j;
 	for (i = 0; i < us; i++) {
-		for (j = 0; j < 100; j++) {}
+		for (j = 0; j < 6; j++) {}
 	}
+#endif
 }
 
 unsigned long millis() {
@@ -33,13 +66,7 @@ unsigned long micros() {
 #endif
 
 #if LT_MICROS_HIGH_RES
-	REG_WRITE(TIMER0_2_READ_CTL, (BKTIMER0 << 2) | 1);
-	while (REG_READ(TIMER0_2_READ_CTL) & 1) {
-		timeout++;
-		if (timeout > (120 * 1000))
-			return 0;
-	}
-	return millis() * 1000 + REG_READ(TIMER0_2_READ_VALUE) / (CFG_XTAL_FREQUENCE / 1000 / 1000);
+	return millis() * 1000 + getTicksCount() / (CFG_XTAL_FREQUENCE / 1000 / 1000);
 #else
 #if 0
 	REG_WRITE(TIMER3_5_READ_CTL, (BKTIMER3 << 2) | 1);