121 lines
2.5 KiB
C
121 lines
2.5 KiB
C
#include "Timer.h"
|
|
#include "bcm2835int.h"
|
|
#include "rpiHardware.h"
|
|
|
|
// Support functions for uspi
|
|
// Based on env\include\uspienv\timer.h
|
|
|
|
// The number of 1MHz ticks for 10ms
|
|
#define TEN_MILLISECS (CLOCKHZ / HZ)
|
|
|
|
static unsigned Ticks = 0;
|
|
|
|
static volatile TKernelTimer m_KernelTimer[KERNEL_TIMERS]; // TODO: should be linked list
|
|
|
|
static void TimerPollKernelTimers()
|
|
{
|
|
//EnterCritical();
|
|
|
|
for (unsigned hTimer = 0; hTimer < KERNEL_TIMERS; hTimer++)
|
|
{
|
|
volatile TKernelTimer* pTimer = &m_KernelTimer[hTimer];
|
|
|
|
TKernelTimerHandler* pHandler = pTimer->m_pHandler;
|
|
if (pHandler != 0)
|
|
{
|
|
if ((int)(pTimer->m_nElapsesAt - Ticks) <= 0)
|
|
{
|
|
pTimer->m_pHandler = 0;
|
|
|
|
(*pHandler)(hTimer + 1, pTimer->m_pParam, pTimer->m_pContext);
|
|
}
|
|
}
|
|
}
|
|
|
|
//LeaveCritical();
|
|
}
|
|
|
|
static void TimerInterruptHandler(void* pParam)
|
|
{
|
|
DataMemBarrier();
|
|
|
|
//assert(read32(ARM_SYSTIMER_CS) & (1 << 3));
|
|
|
|
u32 nCompare = read32(ARM_SYSTIMER_C3) + TEN_MILLISECS;
|
|
write32(ARM_SYSTIMER_C3, nCompare);
|
|
if (nCompare < read32(ARM_SYSTIMER_CLO)) // time may drift
|
|
{
|
|
nCompare = read32(ARM_SYSTIMER_CLO) + TEN_MILLISECS;
|
|
write32(ARM_SYSTIMER_C3, nCompare);
|
|
}
|
|
|
|
write32(ARM_SYSTIMER_CS, 1 << 3);
|
|
|
|
DataMemBarrier();
|
|
|
|
++Ticks;
|
|
|
|
TimerPollKernelTimers();
|
|
}
|
|
|
|
void TimerSystemInitialize()
|
|
{
|
|
InterruptSystemConnectIRQ(ARM_IRQ_TIMER3, TimerInterruptHandler, 0);
|
|
|
|
DataMemBarrier();
|
|
|
|
write32(ARM_SYSTIMER_CLO, -(30 * CLOCKHZ)); // timer wraps soon, to check for problems
|
|
|
|
// Interrupt every 10ms
|
|
write32(ARM_SYSTIMER_C3, read32(ARM_SYSTIMER_CLO) + TEN_MILLISECS);
|
|
|
|
DataMemBarrier();
|
|
|
|
for (unsigned hTimer = 0; hTimer < KERNEL_TIMERS; hTimer++)
|
|
{
|
|
m_KernelTimer[hTimer].m_pHandler = 0;
|
|
}
|
|
|
|
}
|
|
|
|
unsigned TimerStartKernelTimer(unsigned nDelay, TKernelTimerHandler* pHandler, void* pParam, void* pContext)
|
|
{
|
|
//EnterCritical();
|
|
|
|
// DEBUG_LOG("Timer started\r\n");
|
|
|
|
unsigned hTimer;
|
|
for (hTimer = 0; hTimer < KERNEL_TIMERS; hTimer++)
|
|
{
|
|
if (m_KernelTimer[hTimer].m_pHandler == 0)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (hTimer >= KERNEL_TIMERS)
|
|
{
|
|
//LeaveCritical();
|
|
|
|
DEBUG_LOG("System limit of kernel timers exceeded\r\n");
|
|
|
|
return 0;
|
|
}
|
|
|
|
//assert(pHandler != 0);
|
|
m_KernelTimer[hTimer].m_pHandler = pHandler;
|
|
m_KernelTimer[hTimer].m_nElapsesAt = Ticks+nDelay;
|
|
m_KernelTimer[hTimer].m_pParam = pParam;
|
|
m_KernelTimer[hTimer].m_pContext = pContext;
|
|
|
|
//LeaveCritical();
|
|
|
|
return hTimer+1;
|
|
}
|
|
|
|
void TimerCancelKernelTimer(unsigned hTimer)
|
|
{
|
|
//assert(1 <= hTimer && hTimer <= KERNEL_TIMERS);
|
|
m_KernelTimer[hTimer-1].m_pHandler = 0;
|
|
}
|
|
|