DSHOT - Use cycle counting instead of recording timestamp in dshot motor_DMA_IRQHandler.

It turns out that two calls to micros() and the calculation of
directionChangeDurationUs took 581 cycles, vs 396 cycles without the
calls to micros() and deferred calculation of the duration which is only
needed in the CLI.

This brings the time down from around 7 microseconds to 5.5 microseconds
on an F3 at 72Mhz.

This makes the difference between 100% invalid telemetry and 4% invalid
telemetry on the first motor on the F3.

Squashed commits:
* Remove the forward declaration for `pwmDshotSetDirectionInput` and make
it static.
* Remove unneeded forward declaration of `motor_DMA_IRQHandler`.
* Remove duplication in DMA IRQ Handler.
  Doesn't affect resulting code but improves readability.
* Use an inline function to read DWT->CYCCNT.
* Remove unneeded forward declarations from cli.c now that the correct
header is included.
* Update DWT unlock method.

src/main/cli/cli.c

@@ -66,6 +66,7 @@ bool cliMode = false;
 #include "drivers/dshot.h"
 #include "drivers/dshot_command.h"
 #include "drivers/dshot_dpwm.h"
+#include "drivers/pwm_output_dshot_shared.h"
 #include "drivers/camera_control.h"
 #include "drivers/compass/compass.h"
 #include "drivers/display.h"
@@ -274,12 +275,6 @@ static const char * const *sensorHardwareNames[] = {
 };
 #endif // USE_SENSOR_NAMES
 
-#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
-extern uint32_t readDoneCount;
-extern uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
-extern uint32_t setDirectionMicros;
-#endif
-
 typedef enum dumpFlags_e {
     DUMP_MASTER = (1 << 0),
     DUMP_PROFILE = (1 << 1),
@@ -5840,8 +5835,10 @@ static void cliDshotTelemetryInfo(char *cmdline)
     if (useDshotTelemetry) {
         cliPrintLinef("Dshot reads: %u", readDoneCount);
         cliPrintLinef("Dshot invalid pkts: %u", dshotInvalidPacketCount);
-        extern uint32_t setDirectionMicros;
+        uint32_t directionChangeCycles = dshotDMAHandlerCycleCounters.changeDirectionCompletedAt - dshotDMAHandlerCycleCounters.irqAt;
-        cliPrintLinef("Dshot irq micros: %u", setDirectionMicros);
+        uint32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles);
+        cliPrintLinef("Dshot directionChange cycles: %u, micros: %u", directionChangeCycles, directionChangeDurationUs);
+        cliPrintLinef("Dshot packet decode micros: %u", decodePacketDurationUs);
         cliPrintLinefeed();
 
 #ifdef USE_DSHOT_TELEMETRY_STATS

src/main/drivers/dma.h

@@ -108,8 +108,9 @@ typedef enum {
 
 #define DEFINE_DMA_IRQ_HANDLER(d, s, i) void DMA ## d ## _Stream ## s ## _IRQHandler(void) {\
                                                                 const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \
-                                                                if (dmaDescriptors[index].irqHandlerCallback)\
+                                                                dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \
-                                                                    dmaDescriptors[index].irqHandlerCallback(&dmaDescriptors[index]);\
+                                                                if (handler) \
+                                                                    handler(&dmaDescriptors[index]); \
                                                             }
 
 #define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift)
@@ -169,8 +170,9 @@ typedef enum {
 
 #define DEFINE_DMA_IRQ_HANDLER(d, c, i) void DMA ## d ## _Channel ## c ## _IRQHandler(void) {\
                                                                         const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \
-                                                                        if (dmaDescriptors[index].irqHandlerCallback)\
+                                                                        dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \
-                                                                            dmaDescriptors[index].irqHandlerCallback(&dmaDescriptors[index]);\
+                                                                        if (handler) \
+                                                                            handler(&dmaDescriptors[index]); \
                                                                     }
 
 #define DMA_CLEAR_FLAG(d, flag) d->dma->IFCR = (flag << d->flagsShift)

src/main/drivers/pwm_output_dshot.c

@@ -35,6 +35,7 @@
 #include "drivers/rcc.h"
 #include "drivers/time.h"
 #include "drivers/timer.h"
+#include "drivers/system.h"
 #if defined(STM32F4)
 #include "stm32f4xx.h"
 #elif defined(STM32F3)
@@ -63,8 +64,6 @@ void dshotEnableChannels(uint8_t motorCount)
 
 #endif
 
-static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor);
-
 FAST_CODE void pwmDshotSetDirectionOutput(
     motorDmaOutput_t * const motor
 #ifndef USE_DSHOT_TELEMETRY
@@ -121,7 +120,7 @@ FAST_CODE void pwmDshotSetDirectionOutput(
 
 
 #ifdef USE_DSHOT_TELEMETRY
-FAST_CODE void pwmDshotSetDirectionInput(
+FAST_CODE static void pwmDshotSetDirectionInput(
     motorDmaOutput_t * const motor
 )
 {
@@ -189,7 +188,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
 #ifdef USE_DSHOT_TELEMETRY
-        uint32_t irqStart = micros();
+        dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
 #endif
 #ifdef USE_DSHOT_DMAR
         if (useBurstDshot) {
@@ -208,7 +207,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
             xDMA_SetCurrDataCounter(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
             xDMA_Cmd(motor->dmaRef, ENABLE);
             TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, ENABLE);
-            setDirectionMicros = micros() - irqStart;
+            dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
         }
 #endif
         DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);

src/main/drivers/pwm_output_dshot_hal.c

@@ -41,6 +41,7 @@
 #include "drivers/rcc.h"
 #include "drivers/time.h"
 #include "drivers/timer.h"
+#include "drivers/system.h"
 
 #include "pwm_output.h"
 
@@ -66,8 +67,6 @@ void dshotEnableChannels(uint8_t motorCount)
 
 #endif
 
-static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor);
-
 void pwmDshotSetDirectionOutput(
     motorDmaOutput_t * const motor
 #ifndef USE_DSHOT_TELEMETRY
@@ -99,7 +98,7 @@ void pwmDshotSetDirectionOutput(
 }
 
 #ifdef USE_DSHOT_TELEMETRY
-void pwmDshotSetDirectionInput(
+static void pwmDshotSetDirectionInput(
     motorDmaOutput_t * const motor
 )
 {
@@ -161,7 +160,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
         if (!motor->isInput) {
 #ifdef USE_DSHOT_TELEMETRY
-            uint32_t irqStartUs = micros();
+            dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
 #endif
 #ifdef USE_DSHOT_DMAR
             if (useBurstDshot) {
@@ -180,7 +179,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
                 xLL_EX_DMA_SetDataLength(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
                 xLL_EX_DMA_EnableResource(motor->dmaRef);
                 LL_EX_TIM_EnableIT(motor->timerHardware->tim, motor->timerDmaSource);
-                setDirectionMicros = micros() - irqStartUs;
+                dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
             }
 #endif
         }

src/main/drivers/pwm_output_dshot_shared.c

@@ -80,8 +80,10 @@ uint32_t readDoneCount;
 // TODO remove once debugging no longer needed
 FAST_RAM_ZERO_INIT uint32_t dshotInvalidPacketCount;
 FAST_RAM_ZERO_INIT uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
-FAST_RAM_ZERO_INIT uint32_t setDirectionMicros;
+FAST_RAM_ZERO_INIT uint32_t decodePacketDurationUs;
 FAST_RAM_ZERO_INIT uint32_t inputStampUs;
+
+FAST_RAM_ZERO_INIT dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters;
 #endif
 
 motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
@@ -194,13 +196,13 @@ static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
     csum = csum ^ (csum >> 4); // xor nibbles
 
     if ((csum & 0xf) != 0xf) {
-        setDirectionMicros = micros() - start;
+        decodePacketDurationUs = micros() - start;
         return 0xffff;
     }
     decodedValue >>= 4;
 
     if (decodedValue == 0x0fff) {
-        setDirectionMicros = micros() - start;
+        decodePacketDurationUs = micros() - start;
         return 0;
     }
     decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9);
@@ -208,7 +210,7 @@ static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
         return 0xffff;
     }
     uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue;
-    setDirectionMicros = micros() - start;
+    decodePacketDurationUs = micros() - start;
     return ret;
 }
 

src/main/drivers/pwm_output_dshot_shared.h

@@ -46,8 +46,16 @@ extern uint32_t readDoneCount;
 // TODO remove once debugging no longer needed
 FAST_RAM_ZERO_INIT extern uint32_t dshotInvalidPacketCount;
 FAST_RAM_ZERO_INIT extern uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
-FAST_RAM_ZERO_INIT extern uint32_t setDirectionMicros;
+FAST_RAM_ZERO_INIT extern uint32_t decodePacketDurationUs;
 FAST_RAM_ZERO_INIT extern uint32_t inputStampUs;
+
+typedef struct dshotDMAHandlerCycleCounters_s {
+    uint32_t irqAt;
+    uint32_t changeDirectionCompletedAt;
+} dshotDMAHandlerCycleCounters_t;
+
+FAST_RAM_ZERO_INIT extern dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters;
+
 #endif
 
 uint8_t getTimerIndex(TIM_TypeDef *timer);
@@ -67,10 +75,6 @@ FAST_CODE void pwmDshotSetDirectionOutput(
 #endif
 );
 
-#ifdef USE_DSHOT_TELEMETRY
-FAST_CODE void pwmDshotSetDirectionInput(motorDmaOutput_t * const motor);
-#endif
-
 bool pwmStartDshotMotorUpdate(void);
 
 #endif

src/main/drivers/system.c

@@ -32,6 +32,15 @@
 
 #include "system.h"
 
+#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
+// See "RM CoreSight Architecture Specification"
+// B2.3.10  "LSR and LAR, Software Lock Status Register and Software Lock Access Register"
+// "E1.2.11  LAR, Lock Access Register"
+
+#define DWT_LAR_UNLOCK_VALUE 0xC5ACCE55
+
+#endif
+
 // cycles per microsecond
 static uint32_t usTicks = 0;
 // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
@@ -39,16 +48,34 @@ static volatile uint32_t sysTickUptime = 0;
 static volatile uint32_t sysTickValStamp = 0;
 // cached value of RCC->CSR
 uint32_t cachedRccCsrValue;
+static uint32_t cpuClockFrequency = 0;
 
 void cycleCounterInit(void)
 {
 #if defined(USE_HAL_DRIVER)
-    usTicks = HAL_RCC_GetSysClockFreq() / 1000000;
+    cpuClockFrequency = HAL_RCC_GetSysClockFreq();
 #else
     RCC_ClocksTypeDef clocks;
     RCC_GetClocksFreq(&clocks);
-    usTicks = clocks.SYSCLK_Frequency / 1000000;
+    cpuClockFrequency = clocks.SYSCLK_Frequency;
 #endif
+    usTicks = cpuClockFrequency / 1000000;
+
+    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+
+#if defined(DWT_LAR_UNLOCK_VALUE)
+#if defined(STM32F7) || defined(STM32H7)
+    DWT->LAR = DWT_LAR_UNLOCK_VALUE;
+#elif defined(STM32F3) || defined(STM32F4)
+    // Note: DWT_Type does not contain LAR member.
+#define DWT_LAR
+    __O uint32_t *DWTLAR = (uint32_t *)(DWT_BASE + 0x0FB0);
+    *(DWTLAR) = DWT_LAR_UNLOCK_VALUE;
+#endif
+#endif
+
+    DWT->CYCCNT = 0;
+    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
 }
 
 // SysTick
@@ -116,6 +143,16 @@ uint32_t micros(void)
     return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks;
 }
 
+inline uint32_t getCycleCounter(void)
+{
+    return DWT->CYCCNT;
+}
+
+uint32_t clockCyclesToMicros(uint32_t clockCycles)
+{
+    return clockCycles / usTicks;
+}
+
 // Return system uptime in milliseconds (rollover in 49 days)
 uint32_t millis(void)
 {

src/main/drivers/system.h

@@ -64,10 +64,13 @@ void systemReset(void);
 void systemResetToBootloader(bootloaderRequestType_e requestType);
 bool isMPUSoftReset(void);
 void cycleCounterInit(void);
+uint32_t clockCyclesToMicros(uint32_t clockCycles);
+uint32_t getCycleCounter(void);
 #if defined(STM32H7)
 void systemCheckResetReason(void);
 #endif
 
+
 void initialiseMemorySections(void);
 
 void enableGPIOPowerUsageAndNoiseReductions(void);