diff --git a/include/arm11/interrupt.h b/include/arm11/interrupt.h index 1b1e95f..03817f9 100644 --- a/include/arm11/interrupt.h +++ b/include/arm11/interrupt.h @@ -26,9 +26,36 @@ +/** + * @brief Initializes the generic interrupt controller. + */ void IRQ_init(void); + +/** + * @brief Registers a interrupt handler and enables the specified interrupt. + * + * @param[in] id The interrupt ID. Must be <128. + * @param[in] prio The priority. 0 = highest, 14 = lowest, 15 = disabled + * @param[in] cpuMask The CPU mask. Each of the 4 bits stands for 1 core. 0 means current CPU. + * @param[in] levHighActive Set to true to make the interrupt level high active. + * @param[in] irqHandler The interrupt handler to call. + */ void IRQ_registerHandler(Interrupt id, u8 prio, u8 cpuMask, bool levHighActive, void (*irqHandler)(void)); -//void IRQ_unregisterHandler(Interrupt id); + +/** + * @brief Unregisters the interrupt handler and disables the specified interrupt. + * + * @param[in] id The interrupt ID. Must be <128. + */ +void IRQ_unregisterHandler(Interrupt id); + +/** + * @brief Sets the priority of an interrupt. + * + * @param[in] id The interrupt ID. Must be <128. + * @param[in] prio The priority. 0 = highest, 14 = lowest, 15 = disabled + */ +void IRQ_setPriority(Interrupt id, u8 prio); /** * @brief Triggers a software interrupt for the specified CPUs. @@ -38,6 +65,7 @@ */ void softwareInterrupt(Interrupt id, u8 cpuMask); + static inline void waitForIrq(void) { __asm__ __volatile__("wfi" : :); diff --git a/include/arm11/start.h b/include/arm11/start.h index 27f3893..0bb5f17 100644 --- a/include/arm11/start.h +++ b/include/arm11/start.h @@ -6,3 +6,10 @@ void clearMem(u32 *adr, u32 size); void deinitCpu(void); + +static inline u32 getCpuId(void) +{ + u32 cpuId; + __asm__ __volatile__("mrc p15, 0, %0, c0, c0, 5" : "=r" (cpuId) : ); + return cpuId & 0xF; +} diff --git a/source/arm11/exception.s b/source/arm11/exception.s new file mode 100644 index 0000000..e3525a7 --- /dev/null +++ b/source/arm11/exception.s @@ -0,0 +1,49 @@ +#include "asmfunc.h" +#include "mem_map.h" + +.arm +.cpu mpcore +.fpu vfpv2 + +.extern deinitCpu +.extern privIrqHandlerTable +.extern irqHandlerTable + + + +ASM_FUNC tmpExceptionHandler + bl deinitCpu + ldr r0, =0x10202A04 + mov r1, #0xFF + orr r1, #0x1000000 + str r1, [r0] +tmpExceptionHandler_lp: + wfi + b tmpExceptionHandler_lp +.pool + + +ASM_FUNC irqHandler + sub lr, lr, #4 + srsfd sp!, #31 @ Store lr and spsr on system mode stack + cps #31 @ Switch to system mode + stmfd sp!, {r0-r3, r12, lr} + mov r0, #0x17000000 + orr r0, r0, #0xE00000 + ldr r1, [r0, #0x10C] @ REG_CPU_II_AKN + str r1, [r0, #0x110] @ REG_CPU_II_EOI + and r0, r1, #0x7F + cmp r0, #16 + ldrlo r2, =privIrqHandlerTable + addlo r2, r2, r1, lsr #8 + ldrhs r2, =irqHandlerTable + subhs r0, r0, #16 + ldr r3, [r2, r0, lsl #2] + cmp r3, #0 + beq irqHandler_skip_processing + cpsie i + blx r3 + cpsid i +irqHandler_skip_processing: + ldmfd sp!, {r0-r3, r12, lr} + rfefd sp! @ Restore lr (pc) and spsr (cpsr) diff --git a/source/arm11/interrupt.c b/source/arm11/interrupt.c index b304dfc..667dacf 100644 --- a/source/arm11/interrupt.c +++ b/source/arm11/interrupt.c @@ -1,6 +1,7 @@ #include "types.h" #include "mem_map.h" #include "arm11/interrupt.h" +#include "arm11/start.h" #define REG_CFG11_FIQ_CNT *((vu8* )(IO_MEM_ARM9_ARM11 + 0x40000 + 0x0104)) @@ -37,7 +38,8 @@ #define REG_GID_PRIME_CELL3 *((vu32*)(GID_REGS_BASE + 0xFFC)) -void (*irqHandlerTable[128])(void); // There are 96 external interrupts (total 128) +void (*privIrqHandlerTable[4][16])(void) = {0}; // Table for private MPCore interrupts +void (*irqHandlerTable[112])(void) = {0}; // There are 96 external interrupts (total 128) @@ -47,9 +49,7 @@ REG_CPU_II_CNT = 0; - u32 cpuId; - __asm__ __volatile__("mrc p15, 0, %0, c0, c0, 5" : "=r" (cpuId) : ); - if(!(cpuId & 0xF)) + if(!getCpuId()) { // Disable the global interrupt distributor REG_GID_CNT = 0; @@ -67,8 +67,7 @@ REGs_GID_PEN_CLR[3] = 0xFFFFFFFFu; // Set all 128 interrupts to lowest priority (disabled) - REGs_GID_IPRIO[0] = 0xF0F0E0F0u; // Needed by NATIVE_FIRM - for(u32 i = 1; i < 32; i++) REGs_GID_IPRIO[i] = 0xF0F0F0F0u; + for(u32 i = 0; i < 32; i++) REGs_GID_IPRIO[i] = 0xF0F0F0F0u; // Set all 128 interrupts to target no CPU. // Interrupt 0-31 can't be changed @@ -79,13 +78,6 @@ // Enable the global interrupt distributor REG_GID_CNT = 1; - - - for(u32 i = 0; i < 128; i += 2) - { - irqHandlerTable[i + 0] = (void (*)(void))NULL; - irqHandlerTable[i + 1] = (void (*)(void))NULL; - } } else { @@ -101,8 +93,8 @@ // Mask no interrupt REG_CPU_II_MASK = 0xF0; - // No pre-emption is performed - REG_CPU_II_BIN_POI = 7; + // All priority bits are compared for pre-emption + REG_CPU_II_BIN_POI = 3; // Enable the interrupt interface for this CPU REG_CPU_II_CNT = 1; @@ -114,14 +106,22 @@ REG_CPU_II_EOI = tmp; // End of interrupt } while(tmp != 1023); - //__asm__ __volatile__("cpsie i" : :); + __asm__ __volatile__("cpsie i" : :); } void IRQ_registerHandler(Interrupt id, u8 prio, u8 cpuMask, bool levHighActive, void (*irqHandler)(void)) { // TODO: Enter critical section - if(irqHandler) irqHandlerTable[id] = irqHandler; + const u32 cpuId = getCpuId(); + + if(!cpuMask) cpuMask = 1u<>2] & ~(0xFu<>5] = 1u<<(id % 32); + if(id < 16) privIrqHandlerTable[getCpuId()][id] = (void (*)(void))NULL; + else irqHandlerTable[id - 16] = (void (*)(void))NULL; // TODO: Leave critical section -}*/ +} + +void IRQ_setPriority(Interrupt id, u8 prio) +{ + // TODO: Enter critical section + + u32 shift = (id % 4 * 8) + 4; + u32 tmp = REGs_GID_IPRIO[id>>2] & ~(0xFu<>2] = tmp | (u32)prio<