pios_delay.c

Go to the documentation of this file.

/* 
 * This program is free software; you can redistribute it and/or modify 
 * it under the terms of the GNU General Public License as published by 
 * the Free Software Foundation; either version 3 of the License, or 
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but 
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
 * for more details.
 * 
 * You should have received a copy of the GNU General Public License along 
 * with this program; if not, see <http://www.gnu.org/licenses/>
 */

/* Project Includes */
#include <pios.h>

/* cycles per microsecond */
static uint32_t us_ticks;
static uint32_t us_modulo;

int32_t PIOS_DELAY_Init(void)
{
        RCC_ClocksTypeDef       clocks;

        /* compute the number of system clocks per microsecond */
        RCC_GetClocksFreq(&clocks);
        us_ticks = clocks.SYSCLK_Frequency / 1000000;
        PIOS_DEBUG_Assert(us_ticks > 1);

        // Split this into two steps to avoid 64bit maths
        us_modulo = 0xffffffff / us_ticks;
        us_modulo += ((0xffffffff % us_ticks) + 1) / us_ticks;

        // ensure that the us_module is smaller than half of uint32_t max to make modulo operation possible
        PIOS_Assert(us_modulo < 0x80000000);

        /* turn on access to the DWT registers */
        CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;

        /* enable the CPU cycle counter */
        DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;

        return 0;
}

int32_t PIOS_DELAY_WaituS(uint32_t uS)
{
        uint32_t elapsed = 0;
        uint32_t last_count;

        /* turn on access to the DWT registers */
        CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
        last_count = DWT->CYCCNT;
        
        for (;;) {
                uint32_t current_count;
                uint32_t elapsed_uS;

                /* turn on access to the DWT registers */
                CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
                current_count = DWT->CYCCNT;

                /* measure the time elapsed since the last time we checked */
                elapsed += current_count - last_count;
                last_count = current_count;

                /* convert to microseconds */
                elapsed_uS = elapsed / us_ticks;
                if (elapsed_uS >= uS)
                        break;

                /* reduce the delay by the elapsed time */
                uS -= elapsed_uS;

                /* keep fractional microseconds for the next iteration */
                elapsed %= us_ticks;
        }

        /* No error */
        return 0;
}

int32_t PIOS_DELAY_WaitmS(uint32_t mS)
{
        while (mS--) {
                PIOS_DELAY_WaituS(1000);
        }

        /* No error */
        return 0;
}

uint32_t PIOS_DELAY_GetuS()
{
        /* turn on access to the DWT registers */
        CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
        return DWT->CYCCNT / us_ticks;
}

uint32_t PIOS_DELAY_GetuSSince(uint32_t t)
{
        return (PIOS_DELAY_GetuS() + us_modulo - t) % us_modulo;
}

uint32_t PIOS_DELAY_GetuSExpired(uint32_t t)
{
        uint32_t interval = PIOS_DELAY_GetuSSince(t);

        /* Use a windowed approach with the maximum timeval */
        if (interval < (us_modulo / 2)) {
                return true;
        }

        return false;
}

uint32_t PIOS_DELAY_GetRaw()
{
        /* turn on access to the DWT registers */
        CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
        return DWT->CYCCNT;
}

uint32_t PIOS_DELAY_DiffuS(uint32_t raw)
{
        /* turn on access to the DWT registers */
        CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;

        return PIOS_DELAY_DiffuS2(raw, DWT->CYCCNT);
}

uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later) {
        uint32_t diff = later - raw;
        return diff / us_ticks;
}