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>

/* This will result in a value of 32000 -- about 15 bits */
#define SYSTICK_HZ 1250
#define RTC_DIVIDER 2   /* Must be power of 2; this results in 625Hz "RTC" */

#define PIOS_RTC_MAX_CALLBACKS 3

struct rtc_callback_entry {
        void (*fn)(uintptr_t);
        uintptr_t data;
};

static struct rtc_callback_entry rtc_callback_list[PIOS_RTC_MAX_CALLBACKS];

static void PIOS_DELAY_Systick_Handler(void) __attribute__((isr));
static void PIOS_RTC_Tick();

const void *_systick_vector __attribute((section(".systick_vector"))) = PIOS_DELAY_Systick_Handler;

static volatile uint32_t systick_cnt;

static void PIOS_DELAY_Systick_Handler(void)
{
        systick_cnt++;

        if ((systick_cnt & (RTC_DIVIDER - 1)) == 0) {
                PIOS_RTC_Tick();
        }
}

int32_t PIOS_DELAY_Init(void)
{
        /* Count up at HCLK frequency .. */
        SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK);
        /* Request ~1ms tickiness -- .977ms actually */

        SysTick_Config(SYSCLK_FREQ / SYSTICK_HZ);

        return 0;
}

int32_t PIOS_DELAY_WaituS(uint32_t us)
{
        PIOS_Assert(us < 0x80000000);

        uint32_t initial = PIOS_DELAY_GetuS();
        uint32_t expires = initial + us;
        uint32_t offset = 0;

        if (initial > expires) {
                offset = 0x80000000;
                expires += offset;
        }

        while ((PIOS_DELAY_GetuS() + offset) < expires);

        /* No error */
        return 0;
}

int32_t PIOS_DELAY_WaitmS(uint32_t ms)
{
        // ~ 2147 seconds is a long time to busywait.
        PIOS_Assert(ms < (0x8000000 / 1000));

        return PIOS_DELAY_WaituS(ms * 1000);
}

uint32_t PIOS_DELAY_GetuS()
{
        /* XXX fixup to be more precise */
        return systick_cnt * SYSTICK_HZ;
}

uint32_t PIOS_DELAY_GetuSSince(uint32_t t)
{
        return PIOS_DELAY_GetuS() - t;
}

uint32_t PIOS_DELAY_GetRaw()
{
        return PIOS_DELAY_GetuS();
}

uint32_t PIOS_DELAY_DiffuS(uint32_t raw)
{
        return PIOS_DELAY_GetuSSince(raw);
}

uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later) {
        return later - raw;
}

float PIOS_RTC_Rate()
{
        return ((float) SYSTICK_HZ) / RTC_DIVIDER;
}

float PIOS_RTC_MsPerTick()
{
        return 1000 / ((float) SYSTICK_HZ) * RTC_DIVIDER;
}

bool PIOS_RTC_RegisterTickCallback(void (*fn)(uintptr_t id), uintptr_t data)
{
        for (int i = 0; i < PIOS_RTC_MAX_CALLBACKS; i++) {
                if (!rtc_callback_list[i].fn) {
                        struct rtc_callback_entry *cb;

                        cb = &rtc_callback_list[i];

                        cb->fn = fn;
                        cb->data = data;

                        return true;
                }
        }

        return false;
}

static void PIOS_RTC_Tick()
{
        for (int i = 0; i < PIOS_RTC_MAX_CALLBACKS; i++) {
                if (rtc_callback_list[i].fn) {
                        rtc_callback_list[i].fn(rtc_callback_list[i].data);
                }
        }
}