Add delayNanoseconds for Teensy 2.0

PaulStoffregen · Nov 14, 2020 · cf5d643 · cf5d643
1 parent 26edab7
commit cf5d643
Showing 1 changed file with 88 additions and 0 deletions.
diff --git a/teensy/core_pins.h b/teensy/core_pins.h
@@ -2445,6 +2445,94 @@ static inline void delayMicroseconds(uint16_t usec)
 }
 
 
+static inline void delayNanoseconds(uint16_t) __attribute__((always_inline, unused));
+static inline void delayNanoseconds(uint16_t nsec)
+{
+	if (__builtin_constant_p(nsec)) {
+		// use NOPs for the common usage of a constexpr input and short delay
+		if (nsec == 0) return;
+		if (nsec <= 1000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop");
+			return;
+		}
+		if (nsec <= 2000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop");
+			return;
+		}
+		if (nsec <= 3000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop");
+			return;
+		}
+		if (nsec <= 4000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop");
+			return;
+		}
+		if (nsec <= 5000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			return;
+		}
+		if (nsec <= 6000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop");
+			return;
+		}
+		if (nsec <= 7000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop");
+			return;
+		}
+		if (nsec <= 8000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop");
+			return;
+		}
+		if (nsec <= 9000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop\n nop");
+			return;
+		}
+		if (nsec <= 10000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			return;
+		}
+		if (nsec <= 11000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop");
+			return;
+		}
+		if (nsec <= 12000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop");
+			return;
+		}
+		if (nsec <= 13000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop");
+			return;
+		}
+		if (nsec <= 14000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop\n nop");
+			return;
+		}
+		if (nsec <= 15000 / (F_CPU / 1000000)) {
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			__asm__ volatile("nop\n nop\n nop\n nop\n nop");
+			return;
+		}
+	}
+	// if we can't use NOPs, just round up to the nearest microsecond...
+	// not ideal, but AVR is too slow to do anything more complex
+	delayMicroseconds((nsec >> 10) + 1);
+}
+
+
 
 
 #ifdef __cplusplus