src/main/drivers/time.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  *
  24  */
  25
  26 #include <stdint.h>
  27
  28 #include "platform.h"
  29
  30
  31 #include "build/atomic.h"
  32
  33 #include "drivers/nvic.h"
  34 #include "drivers/time.h"
  35
  36 // cycles per microsecond, this is deliberately uint32_t to avoid type conversions
  37 // This is not static so system.c can set it up for us.
  38 uint32_t usTicks = 0;
  39
  40 // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
  41 STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0;
  42 STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
  43
  44 // Return system uptime in milliseconds (rollover in 49 days)
  45 timeMs_t millis(void)
  46 {
  47     return sysTickUptime;
  48 }
  49
  50 // SysTick
  51
  52 static volatile int sysTickPending = 0;
  53
  54 void SysTick_Handler(void)
  55 {
  56     ATOMIC_BLOCK(NVIC_PRIO_MAX) {
  57         sysTickUptime++;
  58         sysTickValStamp = SysTick->VAL;
  59         sysTickPending = 0;
  60         (void)(SysTick->CTRL);
  61     }
  62 #ifdef USE_HAL_DRIVER
  63     // used by the HAL for some timekeeping and timeouts, should always be 1ms
  64     HAL_IncTick();
  65 #endif
  66 }
  67
  68 uint32_t ticks(void)
  69 {
  70 #ifdef UNIT_TEST
  71     return 0;
  72 #else
  73     return DWT->CYCCNT;
  74 #endif
  75 }
  76
  77 void delayNanos(timeDelta_t ns)
  78 {
  79     const uint32_t startTicks = ticks();
  80     const uint32_t ticksToWait = (ns * usTicks) / 1000;
  81     while (ticks() - startTicks <= ticksToWait);
  82 }
  83
  84 // Return system uptime in microseconds
  85 timeUs_t microsISR(void)
  86 {
  87     register uint32_t ms, pending, cycle_cnt;
  88
  89     ATOMIC_BLOCK(NVIC_PRIO_MAX) {
  90         cycle_cnt = SysTick->VAL;
  91
  92         if (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) {
  93             // Update pending.
  94             // Record it for multiple calls within the same rollover period
  95             // (Will be cleared when serviced).
  96             // Note that multiple rollovers are not considered.
  97
  98             sysTickPending = 1;
  99
 100             // Read VAL again to ensure the value is read after the rollover.
 101
 102             cycle_cnt = SysTick->VAL;
 103         }
 104
 105         ms = sysTickUptime;
 106         pending = sysTickPending;
 107     }
 108
 109     // XXX: Be careful to not trigger 64 bit division
 110     const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
 111     return ((timeUs_t)(ms + pending) * 1000LL) + ((timeUs_t)partial);
 112 }
 113
 114 timeUs_t micros(void)
 115 {
 116     register uint32_t ms, cycle_cnt;
 117
 118     // Call microsISR() in interrupt and elevated (non-zero) BASEPRI context
 119
 120 #ifndef UNIT_TEST
 121     if ((SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) || (__get_BASEPRI())) {
 122         return microsISR();
 123     }
 124 #endif
 125
 126     do {
 127         ms = sysTickUptime;
 128         cycle_cnt = SysTick->VAL;
 129     } while (ms != sysTickUptime || cycle_cnt > sysTickValStamp);
 130
 131     // XXX: Be careful to not trigger 64 bit division
 132     const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
 133     return ((timeUs_t)ms * 1000LL) + ((timeUs_t)partial);
 134 }
 135
 136 #if 1
 137 void delayMicroseconds(timeUs_t us)
 138 {
 139     timeUs_t now = micros();
 140     while (micros() - now < us);
 141 }
 142 #else
 143 void delayMicroseconds(timeUs_t us)
 144 {
 145     uint32_t elapsed = 0;
 146     uint32_t lastCount = SysTick->VAL;
 147
 148     for (;;) {
 149         register uint32_t current_count = SysTick->VAL;
 150         timeUs_t elapsed_us;
 151
 152         // measure the time elapsed since the last time we checked
 153         elapsed += current_count - lastCount;
 154         lastCount = current_count;
 155
 156         // convert to microseconds
 157         elapsed_us = elapsed / usTicks;
 158         if (elapsed_us >= us)
 159             break;
 160
 161         // reduce the delay by the elapsed time
 162         us -= elapsed_us;
 163
 164         // keep fractional microseconds for the next iteration
 165         elapsed %= usTicks;
 166     }
 167 }
 168 #endif
 169
 170 void delay(timeMs_t ms)
 171 {
 172     while (ms--)
 173         delayMicroseconds(1000);
 174 }