From e920bd34a47402b36d150a25b95840584b2aa766 Mon Sep 17 00:00:00 2001 From: Wolfvak Date: Sat, 12 Aug 2017 16:04:20 -0300 Subject: [PATCH] Fixed the MPcore interrupt handler, added local ARM9 exception handler --- common/arm.h | 20 ++++++- common/brf.h | 19 +++++++ common/cpu.h | 4 +- link.ld | 1 + screeninit/source/boot.s | 4 +- screeninit/source/gic.c | 17 +++--- screeninit/source/gic.h | 2 +- screeninit/source/irq.s | 6 +- screeninit/source/main.c | 11 ++-- source/common/xrq.c | 100 ++++++++++++++++++++++++++++++++++ source/common/xrq_handler.s | 95 ++++++++++++++++++++++++++++++++ source/start.s | 106 ++++++++++++++++++++---------------- 12 files changed, 310 insertions(+), 75 deletions(-) create mode 100644 common/brf.h create mode 100644 source/common/xrq.c create mode 100644 source/common/xrq_handler.s diff --git a/common/arm.h b/common/arm.h index ced1d62..a12e61a 100644 --- a/common/arm.h +++ b/common/arm.h @@ -10,6 +10,20 @@ #define SR_SYS_MODE (0x1F) #define SR_PMODE_MASK (0x1F) -#define SR_THUMB (1 << 5) -#define SR_FIQ (1 << 6) -#define SR_IRQ (1 << 7) +#define SR_THUMB (1<<5) +#define SR_FIQ (1<<6) +#define SR_IRQ (1<<7) + +#ifdef ARM9 +#define CR_ENABLE_MPU (1<<0) +#define CR_ENABLE_BIGEND (1<<7) +#define CR_ENABLE_DCACHE (1<<2) +#define CR_ENABLE_ICACHE (1<<12) +#define CR_ENABLE_DTCM (1<<16) +#define CR_ENABLE_ITCM (1<<18) +#define CR_ALT_VECTORS (1<<13) +#define CR_CACHE_RROBIN (1<<14) +#define CR_DISABLE_TBIT (1<<15) +#define CR_DTCM_LMODE (1<<17) +#define CR_ITCM_LMODE (1<<19) +#endif diff --git a/common/brf.h b/common/brf.h new file mode 100644 index 0000000..fc6cc8d --- /dev/null +++ b/common/brf.h @@ -0,0 +1,19 @@ +#pragma once + +#ifdef ARM9 + +/* None of these functions require a working stack */ +/* Assume R0-R3, R12 are always clobbered */ +#define BRF_INVALIDATE_DCACHE (0xFFFF07F0) +#define BRF_INVALIDATE_DCACHE_RANGE (0xFFFF0868) +#define BRF_WRITEBACK_DCACHE (0xFFFF07FC) +#define BRF_WRITEBACK_DCACHE_RANGE (0xFFFF0884) +#define BRF_WB_INV_DCACHE (0xFFFF0830) +#define BRF_WB_INV_DCACHE_RANGE (0xFFFF08A8) +#define BRF_INVALIDATE_ICACHE (0xFFFF0AB4) +#define BRF_INVALIDATE_ICACHE_RANGE (0xFFFF0AC0) +#define BRF_RESETCP15 (0xFFFF0C58) + +#else + +#endif diff --git a/common/cpu.h b/common/cpu.h index 4661c1e..a8f73b4 100644 --- a/common/cpu.h +++ b/common/cpu.h @@ -37,7 +37,7 @@ static inline void CPU_WriteCR(u32 cr) static inline void CPU_DisableIRQ(void) { #ifdef ARM9 - CPU_WriteCPSR_c(CPU_ReadCPSR() | (0xC0)); + CPU_WriteCPSR_c(CPU_ReadCPSR() | (SR_IRQ | SR_FIQ)); #else asm("cpsid if\n\t"); #endif @@ -47,7 +47,7 @@ static inline void CPU_DisableIRQ(void) static inline void CPU_EnableIRQ(void) { #ifdef ARM9 - CPU_WriteCPSR_c(CPU_ReadCPSR() & ~(0xC0)); + CPU_WriteCPSR_c(CPU_ReadCPSR() & ~(SR_IRQ | SR_FIQ)); #else asm("cpsie if\n\t"); #endif diff --git a/link.ld b/link.ld index 3b08ee3..7abcdca 100644 --- a/link.ld +++ b/link.ld @@ -18,5 +18,6 @@ SECTIONS __end__ = ABSOLUTE(.); __stack_top = __start__; + __stack_abt = 0x8000; __code_size__ = __end__ - __start__; } diff --git a/screeninit/source/boot.s b/screeninit/source/boot.s index e8ec828..b25c999 100644 --- a/screeninit/source/boot.s +++ b/screeninit/source/boot.s @@ -8,7 +8,7 @@ .global __boot __boot: @ Disable interrupts and switch to IRQ - cpsid aif, #(SR_SVC_MODE) + cpsid if, #(SR_SVC_MODE) @ Writeback and invalidate caches mov r0, #0 @@ -32,8 +32,8 @@ __boot: ldr r1, =__bss_end mov r2, #0 .Lclearbss: - str r2, [r0], #4 cmp r0, r1 + strlt r2, [r0], #4 blt .Lclearbss bl main diff --git a/screeninit/source/gic.c b/screeninit/source/gic.c index d1f1a9c..7d32210 100644 --- a/screeninit/source/gic.c +++ b/screeninit/source/gic.c @@ -23,7 +23,7 @@ irq_handler GIC_AckIRQ(void) return ret; } -void GIC_Configure(u32 irq_id, irq_handler hndl) +void GIC_SetIRQ(u32 irq_id, irq_handler hndl) { handler_table[irq_id] = hndl; DIC_CLRENABLE[irq_id/32] |= BIT(irq_id & 0x1F); @@ -34,30 +34,27 @@ void GIC_Configure(u32 irq_id, irq_handler hndl) void GIC_Reset(void) { - *DIC_CONTROL = 0; - *GIC_CONTROL = 0; + *GIC_CONTROL = 1; + *DIC_CONTROL = 1; *GIC_PRIOMASK = ~0; - for (int i = 0; i < 0x80; i++) { + for (int i = 0; i < MAX_IRQ; i++) { + handler_table[i] = NULL; *GIC_IRQEND = i; } - for (int i = 0; i < (0x20/4); i++) { + for (int i = 0; i < (0x08); i++) { DIC_CLRENABLE[i] = ~0; DIC_PRIORITY[i] = 0; } while(*GIC_PENDING != SPURIOUS_IRQ) { - for (int i=0; i < (0x20/4); i++) { + for (int i=0; i < (0x08); i++) { DIC_CLRPENDING[i] = ~0; } } IRQ_BASE[1] = (u32)&main_irq_handler; IRQ_BASE[0] = 0xE51FF004; - - *GIC_CONTROL = 1; - *DIC_CONTROL = 1; - return; } diff --git a/screeninit/source/gic.h b/screeninit/source/gic.h index f0a5e8d..0892ab2 100644 --- a/screeninit/source/gic.h +++ b/screeninit/source/gic.h @@ -30,5 +30,5 @@ typedef void (*irq_handler)(void); #define DIC_PROCTGT ((vu8*) (DIC_BASE + 0x800)) #define DIC_CFGREG ((vu32*)(DIC_BASE + 0xC00)) -void GIC_Configure(u32 irq_id, irq_handler hndl); +void GIC_SetIRQ(u32 irq_id, irq_handler hndl); void GIC_Reset(void); diff --git a/screeninit/source/irq.s b/screeninit/source/irq.s index 265932c..3d53fc5 100644 --- a/screeninit/source/irq.s +++ b/screeninit/source/irq.s @@ -8,7 +8,7 @@ main_irq_handler: sub lr, lr, #4 @ Fix return address srsdb sp!, #(SR_SVC_MODE) @ Store IRQ mode LR and SPSR on the SVC stack - cps #(SR_SVC_MODE) @ Switch to SVC mode + cpsid i, #(SR_SVC_MODE) @ Switch to SVC mode push {r0-r3,r12} @ Preserve registers and r1, sp, #4 sub sp, sp, r1 @ Word-align stack @@ -18,9 +18,7 @@ main_irq_handler: cmp r0, #0 beq .Lskip_irq - cpsie i - blx r0 @ Branch to interrupt handler with IRQs enabled - cpsid i + blx r0 @ Branch to interrupt handler .Lskip_irq: pop {r1,lr} diff --git a/screeninit/source/main.c b/screeninit/source/main.c index 90f0bf6..418a366 100644 --- a/screeninit/source/main.c +++ b/screeninit/source/main.c @@ -2,6 +2,7 @@ // check there for license info // thanks go to AuroraWright #include +#include #include #include #include @@ -154,16 +155,16 @@ void main(void) PXI_Reset(); GIC_Reset(); + GIC_SetIRQ(IRQ_PXI_SYNC, pxi_interrupt_handler); screen_init(); + PXI_EnableIRQ(); + CPU_EnableIRQ(); + // Clear ARM11 entrypoint *arm11Entry = 0; - GIC_Configure(IRQ_PXI_SYNC, pxi_interrupt_handler); - PXI_EnableIRQ(); - CPU_EnableIRQ(); - - //Wait for the entrypoint to be set, then branch to it + // Wait for the entrypoint to be set, then branch to it while((entry=*arm11Entry) == 0); CPU_DisableIRQ(); diff --git a/source/common/xrq.c b/source/common/xrq.c new file mode 100644 index 0000000..d768c63 --- /dev/null +++ b/source/common/xrq.c @@ -0,0 +1,100 @@ +/* + Written by Wolfvak, specially sublicensed under the GPLv2 + Read LICENSE for more details +*/ + +#include "common.h" +#include "fsinit.h" +#include "fsutil.h" +#include "ui.h" + +#include + +/* Code will be dumped from PC-PC_DUMPRAD to PC+PC_DUMPRAD */ +#define PC_DUMPRAD (0x20) + +#define XRQ_DUMPDATAFUNC(type, size) \ +int XRQ_DumpData_##type(char *b, u32 s, u32 e) \ +{ \ + char *c = b; \ + while(s= 0x20000000 && sp <= st) { + wstr += sprintf(wstr, "\nStack dump:\n\n"); + wstr += XRQ_DumpData_u8(wstr, sp, st); + } + + pc = regs[15]; + wstr += sprintf(wstr, "\nCode dump:\n\n"); + if (regs[16] & SR_THUMB) { + wstr += XRQ_DumpData_u16(wstr, pc-PC_DUMPRAD, pc+PC_DUMPRAD); + } else { + wstr += XRQ_DumpData_u32(wstr, pc-PC_DUMPRAD, pc+PC_DUMPRAD); + } + + DrawStringF(MAIN_SCREEN, 10, y, COLOR_WHITE, COLOR_BLACK, + "Dumping state to SD..."); + y+=FONT_HEIGHT_EXT; + + FileSetData(OUTPUT_PATH"/dump.txt", dumpstr, wstr - dumpstr, 0, true); + DrawStringF(MAIN_SCREEN, 10, y, COLOR_WHITE, COLOR_BLACK, + "Done"); + return; +} diff --git a/source/common/xrq_handler.s b/source/common/xrq_handler.s new file mode 100644 index 0000000..b6c9b56 --- /dev/null +++ b/source/common/xrq_handler.s @@ -0,0 +1,95 @@ +/* + Written by Wolfvak, specially sublicensed under the GPLv2 + Read LICENSE for more details +*/ + +.section .text.xrqh +.arm + +#include +#include + +.macro XRQ_FATAL id=0 + adr sp, XRQ_Registers + stmia sp!, {r0-r12} + mov r11, #\id + b XRQ_MainHandler +.endm + +.global XRQ_Start +XRQ_Start: +XRQ_Vectors: + b XRQ_Reset + b XRQ_Undefined + b XRQ_SWI + b XRQ_PAbort + b XRQ_DAbort + b . @ Reserved exception vector + subs pc, lr, #4 @ IRQs are unhandled + b . @ FIQs are unused (except for debug?) + +XRQ_Registers: + .space (17*4) + +XRQ_Reset: + msr cpsr_c, #(SR_ABT_MODE | SR_IRQ | SR_FIQ) + XRQ_FATAL 0 + +XRQ_Undefined: + XRQ_FATAL 1 + +XRQ_SWI: + XRQ_FATAL 2 + +XRQ_PAbort: + XRQ_FATAL 3 + +XRQ_DAbort: + XRQ_FATAL 4 + +XRQ_MainHandler: + mrs r10, cpsr + mrs r9, spsr + mov r8, lr + + @ Disable interrupts + orr r0, r0, #(SR_IRQ | SR_FIQ) + msr cpsr, r0 + + @ Disable mpu / caches + ldr r4, =BRF_WB_INV_DCACHE + ldr r5, =BRF_INVALIDATE_ICACHE + ldr r6, =BRF_RESETCP15 + blx r4 + blx r5 + blx r6 + + @ Retrieve banked registers + and r0, r9, #(SR_PMODE_MASK) + cmp r0, #(SR_USR_MODE) + orreq r0, r9, #(SR_SYS_MODE) + orr r0, #(SR_IRQ | SR_FIQ) + + msr cpsr_c, r0 @ Switch to previous mode + mov r0, sp + mov r1, lr + msr cpsr, r10 @ Return to abort + + stmia sp!, {r0,r1,r8,r9} + + ldr sp, =__stack_abt + ldr r2, =XRQ_DumpRegisters + adr r1, XRQ_Registers + mov r0, r11 + blx r2 + + msr cpsr_c, #(SR_SVC_MODE | SR_IRQ | SR_FIQ) + mov r0, #0 + .LXRQ_WFI: + mcr p15, 0, r0, c7, c0, 4 + b .LXRQ_WFI + +.pool + +.global XRQ_End +XRQ_End: diff --git a/source/start.s b/source/start.s index a665b29..05137e3 100644 --- a/source/start.s +++ b/source/start.s @@ -3,8 +3,9 @@ .arm #include +#include -@ make sure not to clobber r0-r2 +@ Make sure to preserve r0-r2 .global _start _start: @ Switch to supervisor mode and disable interrupts @@ -33,7 +34,7 @@ _start: mov r5, r0 mov r6, r1 mov r7, r2 - ldr r3, =0xFFFF0830 @ Writeback & Invalidate DCache + ldr r3, =BRF_WB_INV_DCACHE blx r3 mov r0, r5 mov r1, r6 @@ -57,12 +58,13 @@ _start_gm: movne r9, #0 @ Disable caches / mpu - mrc p15, 0, r4, c1, c0, 0 @ read control register - bic r4, #(1<<16) @ - dtcm disable (mandated by the docs, before you change the dtcm's address) - bic r4, #(1<<12) @ - instruction cache disable - bic r4, #(1<<2) @ - data cache disable - bic r4, #(1<<0) @ - mpu disable - mcr p15, 0, r4, c1, c0, 0 @ write control register + ldr r1, =(CR_ENABLE_MPU | CR_ENABLE_DCACHE | CR_ENABLE_ICACHE | \ + CR_ENABLE_DTCM) + ldr r2, =(CR_ENABLE_ITCM | CR_CACHE_RROBIN) + mrc p15, 0, r0, c1, c0, 0 + bic r0, r1 + orr r0, r2 + mcr p15, 0, r0, c1, c0, 0 @ Clear bss ldr r0, =__bss_start @@ -84,53 +86,61 @@ _start_gm: mcr p15, 0, r5, c5, c0, 2 @ write data access mcr p15, 0, r5, c5, c0, 3 @ write instruction access - @ Sets MPU permissions and cache settings - ldr r0, =0xFFFF001F @ ffff0000 64k | bootrom (unprotected / protected) - ldr r1, =0x3000801B @ 30008000 16k | dtcm - ldr r2, =0x01FF801D @ 01ff8000 32k | itcm - ldr r3, =0x08000029 @ 08000000 2M | arm9 mem (O3DS / N3DS) - ldr r4, =0x10000029 @ 10000000 2M | io mem (ARM9 / first 2MB) - ldr r5, =0x20000037 @ 20000000 256M | fcram (O3DS / N3DS) - ldr r6, =0x1FF00027 @ 1FF00000 1M | dsp / axi wram - ldr r7, =0x1800002D @ 18000000 8M | vram (+ 2MB) - mov r8, #0b00101101 @ bootrom/itcm/arm9 mem and fcram are cacheable/bufferable - mcr p15, 0, r0, c6, c0, 0 - mcr p15, 0, r1, c6, c1, 0 - mcr p15, 0, r2, c6, c2, 0 - mcr p15, 0, r3, c6, c3, 0 - mcr p15, 0, r4, c6, c4, 0 - mcr p15, 0, r5, c6, c5, 0 - mcr p15, 0, r6, c6, c6, 0 - mcr p15, 0, r7, c6, c7, 0 - mcr p15, 0, r8, c3, c0, 0 @ Write bufferable 0, 2, 5 - mcr p15, 0, r8, c2, c0, 0 @ Data cacheable 0, 2, 5 - mcr p15, 0, r8, c2, c0, 1 @ Inst cacheable 0, 2, 5 + @ Sets MPU regions and cache settings + adr r0, __mpu_regions + ldmia r0, {r1-r8} + mov r0, #0b00101101 @ bootrom/itcm/arm9 mem and fcram are cacheable/bufferable + mcr p15, 0, r1, c6, c0, 0 + mcr p15, 0, r2, c6, c1, 0 + mcr p15, 0, r3, c6, c2, 0 + mcr p15, 0, r4, c6, c3, 0 + mcr p15, 0, r5, c6, c4, 0 + mcr p15, 0, r6, c6, c5, 0 + mcr p15, 0, r7, c6, c6, 0 + mcr p15, 0, r8, c6, c7, 0 + mcr p15, 0, r0, c3, c0, 0 @ Write bufferable 0, 2, 5 + mcr p15, 0, r0, c2, c0, 0 @ Data cacheable 0, 2, 5 + mcr p15, 0, r0, c2, c0, 1 @ Inst cacheable 0, 2, 5 @ Enable dctm - ldr r1, =0x3000800A @ set dtcm - mcr p15, 0, r1, c9, c1, 0 @ set the dtcm Region Register + ldr r0, =0x3000800A @ set dtcm + mcr p15, 0, r0, c9, c1, 0 @ set the dtcm Region Register - @ Wait for screen init - mov r0, #0x20000000 - .Lwaitforsi: - ldr r1, [r0, #-4] - cmp r1, #0 - bne .Lwaitforsi + @ Install exception handlers + ldr r0, =XRQ_Start + ldr r1, =XRQ_End + ldr r2, =0x00000000 + .LXRQ_Install: + cmp r0, r1 + ldrlt r3, [r0], #4 + strlt r3, [r2], #4 + blt .LXRQ_Install - @ Enable caches - mrc p15, 0, r4, c1, c0, 0 @ read control register - orr r4, r4, #(1<<18) @ - itcm enable - orr r4, r4, #(1<<16) @ - dtcm enable - orr r4, r4, #(1<<12) @ - instruction cache enable - orr r4, r4, #(1<<2) @ - data cache enable - orr r4, r4, #(1<<0) @ - mpu enable - mcr p15, 0, r4, c1, c0, 0 @ write control register + @ Enable caches / select low exception vectors + ldr r1, =(CR_ALT_VECTORS | CR_DISABLE_TBIT) + ldr r2, =(CR_ENABLE_MPU | CR_ENABLE_DCACHE | CR_ENABLE_ICACHE | \ + CR_ENABLE_DTCM) + mrc p15, 0, r0, c1, c0, 0 + bic r0, r1 + orr r0, r2 + mcr p15, 0, r0, c1, c0, 0 @ Fixes mounting of SDMC - ldr r0, =0x10000020 + ldr r0, =0x10000000 mov r1, #0x340 - str r1, [r0] + str r1, [r0, #0x20] mov r0, r9 mov r1, r10 - b main + + bl main + +__mpu_regions: + .word 0xFFFF001F @ ffff0000 64k | bootrom (unprotected / protected) + .word 0x3000801B @ 30008000 16k | dtcm + .word 0x00000035 @ 00000000 128M | itcm + .word 0x08000029 @ 08000000 2M | arm9 mem (O3DS / N3DS) + .word 0x10000029 @ 10000000 2M | io mem (ARM9 / first 2MB) + .word 0x20000037 @ 20000000 256M | fcram (O3DS / N3DS) + .word 0x1FF00027 @ 1FF00000 1M | dsp / axi wram + .word 0x1800002D @ 18000000 8M | vram (+ 2MB)