Add delayNanoseconds for Teensy 3.x & LC

teensy3/core_pins.h

@@ -2424,9 +2424,103 @@ static inline void delayMicroseconds(uint32_t usec)
 }
 #endif
 
-
-
-
+static inline void delayNanoseconds(uint32_t) __attribute__((always_inline, unused));
+static inline void delayNanoseconds(uint32_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;
+		}
+	}
+	uint32_t n = nsec * (F_CPU / 45776) >> 16;
+	if (n == 0) return;
+	__asm__ volatile(
+		"L_%=_delayNanoseconds:"		"\n\t"
+#ifdef KINETISL
+		"sub    %0, #1"				"\n\t"
+		"bne    L_%=_delayNanoseconds"		"\n"
+		: "+l" (n) :
+#else
+		"subs   %0, #1"				"\n\t"
+		"bne    L_%=_delayNanoseconds"		"\n"
+		: "+r" (n) :
+#endif
+	);
+}