Fixed the MPcore interrupt handler, added local ARM9 exception handler

common/arm.h

@@ -10,6 +10,20 @@
 #define SR_SYS_MODE (0x1F)
 #define SR_PMODE_MASK (0x1F)
 
-#define SR_THUMB  (1 << 5)
+#define SR_THUMB  (1<<5)
-#define SR_FIQ    (1 << 6)
+#define SR_FIQ    (1<<6)
-#define SR_IRQ    (1 << 7)
+#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

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

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

link.ld

@@ -18,5 +18,6 @@ SECTIONS
     __end__ = ABSOLUTE(.);
 
     __stack_top = __start__;
+    __stack_abt = 0x8000;
     __code_size__ = __end__ - __start__;
 }

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

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 = 1;
-    *GIC_CONTROL = 0;
+    *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;
 }

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);

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
-    blx r0                     @ Branch to interrupt handler with IRQs enabled
-    cpsid i
 
     .Lskip_irq:
     pop {r1,lr}

screeninit/source/main.c

@@ -2,6 +2,7 @@
 // check there for license info
 // thanks go to AuroraWright
 #include <types.h>
+#include <arm.h>
 #include <cpu.h>
 #include <gic.h>
 #include <pxi.h>
@@ -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);
+    // Wait for the entrypoint to be set, then branch to it
-    PXI_EnableIRQ();
-    CPU_EnableIRQ();
-
-    //Wait for the entrypoint to be set, then branch to it
     while((entry=*arm11Entry) == 0);
 
     CPU_DisableIRQ();

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 <arm.h>
+
+/* 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<e) { \
+        b+=sprintf(b, "%08lX: ",s); \
+        type *dl = (type*)s; \
+        for (u32 i=0; i<(16/((size)/2)); i++) { \
+            b+=sprintf(b, "%0" #size "lX ", (u32)dl[i]); \
+        } \
+        b+=sprintf(b, "\n"); \
+        s+=16; \
+    } \
+    return (int)(b-c); \
+}
+XRQ_DUMPDATAFUNC(u8,  2)
+XRQ_DUMPDATAFUNC(u16, 4)
+XRQ_DUMPDATAFUNC(u32, 8)
+
+
+// Last 3 should never happen
+const char *XRQ_Name[] = {
+    "Reset", "Undefined", "SWI", "Prefetch Abort",
+    "Data Abort", "", "", ""
+};
+
+extern char __stack_top;
+
+void XRQ_DumpRegisters(u32 xrq, u32 *regs)
+{
+    int y;
+    u32 sp, st, pc;
+    char *wstr = (char*)TEMP_BUFFER, *dumpstr = wstr;
+
+    ClearScreen(MAIN_SCREEN, COLOR_BLACK);
+    wstr += sprintf(wstr, "Exception: %s (%lu)\n", XRQ_Name[xrq&7], xrq);
+
+    for (int i = 0; i < 8; i++) {
+        int i_ = i*2;
+        wstr += sprintf(wstr,
+        "R%02d: %08lX | R%02d: %08lX\n", i_, regs[i_], i_+1, regs[i_+1]);
+    }
+
+    wstr += sprintf(wstr, "CPSR: %08lX\n", regs[16]);
+
+    DrawStringF(MAIN_SCREEN, 10, 0, COLOR_WHITE, COLOR_BLACK,
+        dumpstr);
+
+    y = GetDrawStringHeight(dumpstr);
+    DrawStringF(MAIN_SCREEN, 10, y, COLOR_WHITE, COLOR_BLACK,
+        "Reinitializing SD subsystem...");
+    y+=FONT_HEIGHT_EXT;
+
+    while(!InitSDCardFS()) {
+        DeinitSDCardFS();
+    }
+
+    DrawStringF(MAIN_SCREEN, 10, y, COLOR_WHITE, COLOR_BLACK,
+        "Done");
+    y+=FONT_HEIGHT_EXT;
+
+    sp = regs[13] & ~0xF;
+    st = (u32)&__stack_top;
+    if (sp >= 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;
+}

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 <arm.h>
+#include <brf.h>
+
+.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:

source/start.s

@@ -3,8 +3,9 @@
 .arm
 
 #include <arm.h>
+#include <brf.h>
 
-@ 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
+    ldr r1, =(CR_ENABLE_MPU | CR_ENABLE_DCACHE | CR_ENABLE_ICACHE | \
-    bic r4, #(1<<16)           @ - dtcm disable (mandated by the docs, before you change the dtcm's address)
+              CR_ENABLE_DTCM)
-    bic r4, #(1<<12)           @ - instruction cache disable
+    ldr r2, =(CR_ENABLE_ITCM | CR_CACHE_RROBIN)
-    bic r4, #(1<<2)            @ - data cache disable
+    mrc p15, 0, r0, c1, c0, 0
-    bic r4, #(1<<0)            @ - mpu disable
+    bic r0, r1
-    mcr p15, 0, r4, c1, c0, 0  @ write control register
+    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
+    @ Sets MPU regions and cache settings
-    ldr r0, =0xFFFF001F	@ ffff0000 64k  | bootrom (unprotected / protected)
+    adr r0, __mpu_regions
-    ldr r1, =0x3000801B	@ 30008000 16k  | dtcm
+    ldmia r0, {r1-r8}
-    ldr r2, =0x01FF801D	@ 01ff8000 32k  | itcm
+    mov r0, #0b00101101 @ bootrom/itcm/arm9 mem and fcram are cacheable/bufferable
-    ldr r3, =0x08000029	@ 08000000 2M   | arm9 mem (O3DS / N3DS)
+    mcr p15, 0, r1, c6, c0, 0
-    ldr r4, =0x10000029	@ 10000000 2M   | io mem (ARM9 / first 2MB)
+    mcr p15, 0, r2, c6, c1, 0
-    ldr r5, =0x20000037	@ 20000000 256M | fcram (O3DS / N3DS)
+    mcr p15, 0, r3, c6, c2, 0
-    ldr r6, =0x1FF00027	@ 1FF00000 1M   | dsp / axi wram
+    mcr p15, 0, r4, c6, c3, 0
-    ldr r7, =0x1800002D	@ 18000000 8M   | vram (+ 2MB)
+    mcr p15, 0, r5, c6, c4, 0
-    mov r8, #0b00101101 @ bootrom/itcm/arm9 mem and fcram are cacheable/bufferable
+    mcr p15, 0, r6, c6, c5, 0
-    mcr p15, 0, r0, c6, c0, 0
+    mcr p15, 0, r7, c6, c6, 0
-    mcr p15, 0, r1, c6, c1, 0
+    mcr p15, 0, r8, c6, c7, 0
-    mcr p15, 0, r2, c6, c2, 0
+    mcr p15, 0, r0, c3, c0, 0	@ Write bufferable 0, 2, 5
-    mcr p15, 0, r3, c6, c3, 0
+    mcr p15, 0, r0, c2, c0, 0	@ Data cacheable 0, 2, 5
-    mcr p15, 0, r4, c6, c4, 0
+    mcr p15, 0, r0, c2, c0, 1	@ Inst cacheable 0, 2, 5
-    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
 
     @ Enable dctm
-    ldr r1, =0x3000800A        @ set dtcm
+    ldr r0, =0x3000800A        @ set dtcm
-    mcr p15, 0, r1, c9, c1, 0  @ set the dtcm Region Register
+    mcr p15, 0, r0, c9, c1, 0  @ set the dtcm Region Register
 
-    @ Wait for screen init
+    @ Install exception handlers
-    mov r0, #0x20000000
+    ldr r0, =XRQ_Start
-    .Lwaitforsi:
+    ldr r1, =XRQ_End
-        ldr r1, [r0, #-4]
+    ldr r2, =0x00000000
-        cmp r1, #0
+    .LXRQ_Install:
-        bne .Lwaitforsi
+        cmp r0, r1
+        ldrlt r3, [r0], #4
+        strlt r3, [r2], #4
+        blt .LXRQ_Install
 
-    @ Enable caches
+    @ Enable caches / select low exception vectors
-    mrc p15, 0, r4, c1, c0, 0  @ read control register
+    ldr r1, =(CR_ALT_VECTORS | CR_DISABLE_TBIT)
-    orr r4, r4, #(1<<18)       @ - itcm enable
+    ldr r2, =(CR_ENABLE_MPU | CR_ENABLE_DCACHE | CR_ENABLE_ICACHE | \
-    orr r4, r4, #(1<<16)       @ - dtcm enable
+              CR_ENABLE_DTCM)
-    orr r4, r4, #(1<<12)       @ - instruction cache enable
+    mrc p15, 0, r0, c1, c0, 0
-    orr r4, r4, #(1<<2)        @ - data cache enable
+    bic r0, r1
-    orr r4, r4, #(1<<0)        @ - mpu enable
+    orr r0, r2
-    mcr p15, 0, r4, c1, c0, 0  @ write control register
+    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)