mirror of
https://github.com/Gericom/libtwl.git
synced 2025-06-19 11:35:37 -04:00
84 lines
1.6 KiB
C
84 lines
1.6 KiB
C
#include <nds.h>
|
|
#include "libtwl/rtos/rtosState.h"
|
|
#include "libtwl/math/mathUtil.h"
|
|
#include "libtwl/rtos/rtosIrq.h"
|
|
|
|
#ifdef LIBTWL_ARM7
|
|
|
|
#define ARM7_MAGIC_MUL 0x04D7651F
|
|
#define ARM7_MAGIC_SHIFT 27
|
|
|
|
#endif
|
|
|
|
static void irqDummyFunc(u32 irqMask)
|
|
{
|
|
|
|
}
|
|
|
|
void rtos_initIrq(void)
|
|
{
|
|
REG_IME = 0;
|
|
rtos_disableIrqs();
|
|
|
|
rtos_disableIrqMask(~0);
|
|
rtos_ackIrqMask(~0);
|
|
#ifdef LIBTWL_ARM7
|
|
rtos_disableIrq2Mask(~0);
|
|
rtos_ackIrq2Mask(~0);
|
|
#endif
|
|
|
|
for (int i = 0; i < 32; i++)
|
|
gRtosState.irqTable[i] = irqDummyFunc;
|
|
|
|
#ifdef LIBTWL_ARM7
|
|
for (int i = 0; i < 32; i++)
|
|
gRtosState.irq2Table[i] = irqDummyFunc;
|
|
#endif
|
|
|
|
gRtosState.irqDepth = 0;
|
|
|
|
#ifdef LIBTWL_ARM9
|
|
((vu32*)&SystemVectors)[5] = (u32)&rtos_irqVector;
|
|
setVectorBase(0);
|
|
#endif
|
|
#ifdef LIBTWL_ARM7
|
|
BIOS_IRQ_HANDLER = (u32)&rtos_irqVector;
|
|
#endif
|
|
|
|
rtos_enableIrqs();
|
|
REG_IME = 1;
|
|
}
|
|
|
|
void rtos_setIrqFunc(u32 mask, rtos_irq_func_t func)
|
|
{
|
|
if (!func)
|
|
func = irqDummyFunc;
|
|
while (mask)
|
|
{
|
|
u32 bit = (-mask) & mask;
|
|
mask &= ~bit;
|
|
#ifdef LIBTWL_ARM9
|
|
int idx = math_clz(bit);
|
|
#endif
|
|
#ifdef LIBTWL_ARM7
|
|
int idx = (bit * ARM7_MAGIC_MUL) >> ARM7_MAGIC_SHIFT;
|
|
#endif
|
|
gRtosState.irqTable[idx] = func;
|
|
}
|
|
}
|
|
|
|
#ifdef LIBTWL_ARM7
|
|
void rtos_setIrq2Func(u32 mask, rtos_irq_func_t func)
|
|
{
|
|
if (!func)
|
|
func = irqDummyFunc;
|
|
while (mask)
|
|
{
|
|
u32 bit = (-mask) & mask;
|
|
mask &= ~bit;
|
|
int idx = (bit * ARM7_MAGIC_MUL) >> ARM7_MAGIC_SHIFT;
|
|
gRtosState.irq2Table[idx] = func;
|
|
}
|
|
}
|
|
#endif
|