Merge pull request #1 from Defragster/Defragster-patch-1

Replace micros for one using CycCnt

teensy4/EventResponder.cpp

@@ -336,8 +336,10 @@ void MillisTimer::runFromTimer()
 
 // TODO: this doesn't work for IMXRT - no longer using predefined names
 extern "C" volatile uint32_t systick_millis_count;
+extern "C" volatile uint32_t systick_cycle_count;
 extern "C" void systick_isr(void)
 {
+	systick_cycle_count += F_CPU_ACTUAL/1000;
 	systick_millis_count++;
 	MillisTimer::runFromTimer();
 }

teensy4/delay.c

@@ -3,6 +3,7 @@
 //volatile uint32_t F_CPU = 396000000;
 //volatile uint32_t F_BUS = 132000000;
 volatile uint32_t systick_millis_count = 0;
+volatile uint32_t systick_cycle_count = 0;
 
 // page 411 says "24 MHz XTALOSC can be the external clock source of SYSTICK"
 // Testing shows the frequency is actually 100 kHz - but how?  Did NXP really
@@ -62,6 +63,19 @@ void delay(uint32_t msec)
 }
 
 uint32_t micros(void)
+{
+	uint32_t ccdelta, usec, smc;
+	do {
+		smc = systick_millis_count;
+		ccdelta = ARM_DWT_CYCCNT - systick_cycle_count;
+	} while ( smc != systick_millis_count ); // repeat if systick_isr
+	usec = 1000*smc + (ccdelta/(F_CPU_ACTUAL/1000000));
+	return usec;
+}
+
+#if 0 // kept to compare test to cycle count micro()
+extern uint32_t otmicros(void);
+uint32_t otmicros(void)
 {
 	uint32_t msec, tick, elapsed, istatus, usec;
 	//static uint32_t prev_msec=0;
@@ -118,4 +132,4 @@ uint32_t micros(void)
 	prev_usec = usec;
 	return usec;
 }
-
+#endif

teensy4/startup.c

@@ -111,16 +111,28 @@ void ResetHandler(void)
 // the ARM clock to run at different speeds.
 #define SYSTICK_EXT_FREQ 100000
 
+extern volatile uint32_t systick_cycle_count;
+#define MAGIC_VAL 5 // empiric start value syncs signif. digits to millis()
+static void systick_isr_sync(void)
+{
+	// 1st[0] tick is sync baseline CycCnt for micros()
+	if ( 0 == systick_millis_count )
+		systick_cycle_count = ARM_DWT_CYCCNT - MAGIC_VAL*F_CPU_ACTUAL/1000;
+	else 
+		_VectorsRam[15] = systick_isr; // use CORE _isr
+	systick_millis_count++;
+}
 static void configure_systick(void)
 {
 	_VectorsRam[14] = pendablesrvreq_isr;
-	_VectorsRam[15] = systick_isr;
+	_VectorsRam[15] = systick_isr_sync; // Wait for CycleCounter to sync
 	SYST_RVR = (SYSTICK_EXT_FREQ / 1000) - 1;
 	SYST_CVR = 0;
 	SYST_CSR = SYST_CSR_TICKINT | SYST_CSR_ENABLE;
 	SCB_SHPR3 = 0x20000000;  // Systick = priority 32
 	ARM_DEMCR |= ARM_DEMCR_TRCENA;
 	ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA; // turn on cycle counter
+	systick_cycle_count = ARM_DWT_CYCCNT; // was 0, must sync w/_isr
 }
 
 
@@ -246,6 +258,7 @@ void usb_pll_start()
 //  R2
 //  R1
 //  R0
+__attribute__((weak))
 void unused_interrupt_vector(void)
 {
 	// TODO: polling Serial to complete buffered transmits