Merge pull request #551 from adafruit/fix-micros

micro() use DWT cyclecount if enabled else use rtos tick

Author: hathach

cores/nRF5/delay.c

@@ -30,11 +30,6 @@ uint32_t millis( void )
   return tick2ms(xTaskGetTickCount());
 }
 
-uint32_t micros( void )
-{
-  return tick2us(xTaskGetTickCount());
-}
-
 void delay( uint32_t ms )
 {
   uint32_t ticks = ms2tick(ms);

cores/nRF5/delay.h

@@ -26,6 +26,13 @@ extern "C" {
 #include <stdint.h>
 #include "nrfx.h"
 #include "variant.h"
+#include "rtos.h"
+
+static inline bool dwt_enabled(void) __attribute__((always_inline));
+static inline bool dwt_enabled(void)
+{
+  return (CoreDebug->DEMCR & CoreDebug_DEMCR_TRCENA_Msk) && (DWT->CTRL & DWT_CTRL_CYCCNTENA_Msk);
+}
 
 /**
  * \brief Returns the number of milliseconds since the Arduino board began running the current program.
@@ -46,7 +53,12 @@ extern uint32_t millis( void ) ;
  *
  * \note There are 1,000 microseconds in a millisecond and 1,000,000 microseconds in a second.
  */
-extern uint32_t micros( void ) ;
+static inline uint32_t micros( void ) __attribute__((always_inline));
+static inline uint32_t micros( void )
+{
+  // Use DWT cycle count if it is enabled, otherwise use rtos tick
+  return dwt_enabled() ? (DWT->CYCCNT / 64) : tick2us(xTaskGetTickCount());
+}
 
 /**
  * \brief Pauses the program for the amount of time (in miliseconds) specified as parameter.

cores/nRF5/freertos/portable/CMSIS/nrf52/port_cmsis_systick.c

@@ -239,6 +239,13 @@ void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime )
             if (diff > 1)
             {
                 vTaskStepTick(diff - 1);
+               
+                // If dwt cycle count is enable, adjust it as welll
+                if ( (CoreDebug->DEMCR & CoreDebug_DEMCR_TRCENA_Msk) && (DWT->CTRL & DWT_CTRL_CYCCNTENA_Msk) )
+                {
+                  DWT->CYCCNT += (((diff-1) * 1000000) / configTICK_RATE_HZ) * 64;
+                }
+              
                 switch_req = xTaskIncrementTick();
             }
             else if (diff == 1)