1 files changed, 35 insertions, 35 deletions

diff --git a/src/Movement/StepTimer.cpp b/src/Movement/StepTimer.cpp
index 5edb2b00..e14a8859 100644
--- a/src/Movement/StepTimer.cpp
+++ b/src/Movement/StepTimer.cpp
@@ -220,9 +220,6 @@ bool StepTimer::ScheduleTimerInterrupt(uint32_t tim) noexcept
 	while (StepTc->SYNCBUSY.reg & TC_SYNCBUSY_CC0) { }
 	StepTc->INTFLAG.reg = TC_INTFLAG_MC0;							// clear any existing compare match
 	StepTc->INTENSET.reg = TC_INTFLAG_MC0;
-#elif defined(__LPC17xx__)
-	STEP_TC->MR[0] = tim;											// set MR0 compare register
-	STEP_TC->MCR |= (1u<<SBIT_MR0I);									// enable interrupt on MR0 match
 #else
 	STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_RB = tim;					// set up the compare register
 	(void)STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_SR;					// read the status register, which clears the status bits and any pending interrupt
@@ -353,7 +350,7 @@ void StepTimer::DisableTimerInterrupt() noexcept
 #endif
 
 // The guts of the ISR
-/*static*/ void StepTimer::Interrupt() noexcept
+/*static*/ inline void StepTimer::Interrupt() noexcept
 {
 	StepTimer * tmr = pendingList;
 	if (tmr != nullptr)
@@ -442,7 +439,8 @@ bool StepTimer::ScheduleCallbackFromIsr() noexcept
 	}
 
 	// Optimise the common case i.e. no other timer is pending
-	if (pendingList == nullptr)
+	StepTimer *tmr = pendingList;			// capture volatile variable
+	if (tmr == nullptr)
 	{
 		if (ScheduleTimerInterrupt(whenDue))
 		{
@@ -450,32 +448,33 @@ bool StepTimer::ScheduleCallbackFromIsr() noexcept
 		}
 		next = nullptr;
 		pendingList = this;
-		active = true;
-		return false;
 	}
-
-	// Another timer is already pending
-	const Ticks now = GetTimerTicks();
-	const int32_t howSoon = (int32_t)(whenDue - now);
-	StepTimer** ppst = const_cast<StepTimer**>(&pendingList);
-	if (howSoon < (int32_t)((*ppst)->whenDue - now))
+	else
 	{
-		// This one is due earlier than the first existing one
-		if (ScheduleTimerInterrupt(whenDue))
+		// Another timer is already pending
+		const Ticks now = GetTimerTicks();
+		const int32_t howSoon = (int32_t)(whenDue - now);
+		if (howSoon < (int32_t)(tmr->whenDue - now))
 		{
-			return true;
+			// This one is due earlier than the first existing one
+			if (ScheduleTimerInterrupt(whenDue))
+			{
+				return true;
+			}
+			next = tmr;
+			pendingList = this;
 		}
-	}
-	else
-	{
-		while (*ppst != nullptr && (int32_t)((*ppst)->whenDue - now) < howSoon)
+		else
 		{
-			ppst = &((*ppst)->next);
+			while (tmr->next != nullptr && (int32_t)(tmr->next->whenDue - now) < howSoon)
+			{
+				tmr = tmr->next;
+			}
+			next = tmr->next;
+			tmr->next = this;
 		}
 	}
 
-	next = *ppst;
-	*ppst = this;
 	active = true;
 	return false;
 }
@@ -515,16 +514,17 @@ extern "C" uint32_t StepTimerGetTimerTicks() noexcept
 	return StepTimer::GetTimerTicks();
 }
 
-#if SUPPORT_REMOTE_COMMANDS
-
-// Remote diagnostics
 /*static*/ void StepTimer::Diagnostics(const StringRef& reply) noexcept
 {
-	reply.lcatf("Peak sync jitter %" PRIi32 "/%" PRIi32 ", peak Rx sync delay %" PRIu32 ", resyncs %u/%u, ", peakNegJitter, peakPosJitter, peakReceiveDelay, numTimeoutResyncs, numJitterResyncs);
-	gotJitter = false;
-	numTimeoutResyncs = numJitterResyncs = 0;
-	peakReceiveDelay = 0;
-
+#if SUPPORT_REMOTE_COMMANDS
+	if (CanInterface::InExpansionMode())
+	{
+		reply.lcatf("Peak sync jitter %" PRIi32 "/%" PRIi32 ", peak Rx sync delay %" PRIu32 ", resyncs %u/%u, ", peakNegJitter, peakPosJitter, peakReceiveDelay, numTimeoutResyncs, numJitterResyncs);
+		gotJitter = false;
+		numTimeoutResyncs = numJitterResyncs = 0;
+		peakReceiveDelay = 0;
+	}
+#endif
 	StepTimer *pst = pendingList;
 	if (pst == nullptr)
 	{
@@ -536,13 +536,15 @@ extern "C" uint32_t StepTimerGetTimerTicks() noexcept
 					pst->whenDue - GetTimerTicks(),
 # if SAME5x
 					((StepTc->INTENSET.reg & TC_INTFLAG_MC0) == 0)
-# elif SAME70
+# elif SAME70 || SAM4E || SAM4S
 					((STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_IER & TC_IER_CPBS) == 0)
 # endif
 						? "disabled" : "enabled");
 # if SAME5x
 		if (StepTc->CC[0].reg != pst->whenDue)
-# elif SAME70
+# elif SAM4E
+		if (STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_RB != pst->whenDue)
+# elif SAME70 || SAM4S
 		if (STEP_TC->TC_CHANNEL[STEP_TC_CHAN].TC_RB != (uint16_t)pst->whenDue)
 # endif
 		{
@@ -551,6 +553,4 @@ extern "C" uint32_t StepTimerGetTimerTicks() noexcept
 	}
 }
 
-#endif
-
 // End