- properly set up MMU tables with caching and other fun stuff

- maps a regular ARM-style exception vector table instead of using the bootrom vector redirection

features a ton of bugs because I'm missing something, it actually manages to boot fb3DS v1.2 and BAX fine, but fails to boot itself

arm11/link.ld

@@ -1,15 +1,57 @@
 OUTPUT_FORMAT("elf32-littlearm", "elf32-bigarm", "elf32-littlearm")
 OUTPUT_ARCH(arm)
-
 ENTRY(__boot)
+
+MEMORY
+{
+    AXIWRAM (RWX) : ORIGIN = 0x1FF80000, LENGTH = 96K
+    HIGHRAM (RWX) : ORIGIN = 0xFFFF0000, LENGTH = 4K
+}
+
 SECTIONS
 {
-    . = 0x1FF80000;
+    .vector : ALIGN(4K)
+    {
+        __vector_pa = LOADADDR(.vector);
+        __vector_va = ABSOLUTE(.);
+        KEEP(*(.vector))
+        . = ALIGN(4K);
+        __vector_len = . - __vector_va;
+    } >HIGHRAM AT>AXIWRAM
 
-    .text   : ALIGN(4) { *(.text.boot) *(.text*); . = ALIGN(4); }
-    .rodata : ALIGN(4) { *(.rodata*); . = ALIGN(4); }
-    .data   : ALIGN(4) { *(.data*); . = ALIGN(8); }
-    .bss    : ALIGN(4) { __bss_start = .; *(.bss* COMMON); __bss_end = .; }
+    .text : ALIGN(4K)
+    {
+        __text_pa = LOADADDR(.text);
+        __text_va = ABSOLUTE(.);
+        *(.text*)
+        . = ALIGN(4K);
+        __text_len = . - __text_va;
+    } >AXIWRAM
 
-    . = ALIGN(4);
+    .data : ALIGN(4K)
+    {
+        __data_pa = LOADADDR(.data);
+        __data_va = ABSOLUTE(.);
+        *(.data*)
+        . = ALIGN(4K);
+        __data_len = . - __data_va;
+    } >AXIWRAM
+
+    .rodata : ALIGN(4K)
+    {
+        __rodata_pa = LOADADDR(.rodata);
+        __rodata_va = ABSOLUTE(.);
+        *(.rodata*)
+        . = ALIGN(4K);
+        __rodata_len = . - __rodata_va;
+    } >AXIWRAM
+
+    .bss (NOLOAD) : ALIGN(4K)
+    {
+        __bss_pa = LOADADDR(.bss);
+        __bss_va = ABSOLUTE(.);
+        *(.bss*)
+        . = ALIGN(4K);
+        __bss_len = . - __bss_va;
+    } >AXIWRAM
 }

arm11/source/arm/gic.c

@@ -41,7 +41,7 @@
 #define LOCAL_IRQ_OFF(c, n) (((c) * LOCAL_IRQS) + (n))
 #define GLOBAL_IRQ_OFF(n)   (((MAX_CPU-1) * LOCAL_IRQS) + (n))
 #define IRQ_TABLE_OFF(c, n) \
-    (IRQN_IS_LOCAL((n)) ? LOCAL_IRQ_OFF((c), (n)) : GLOBAL_IRQ_OFF((n)))
+    (IRQN_IS_LOCAL(n) ? LOCAL_IRQ_OFF((c), (n)) : GLOBAL_IRQ_OFF(n))
 
 static IRQ_Handler IRQ_Handlers[IRQ_TABLE_OFF(0, MAX_IRQ)];
 
@@ -117,7 +117,7 @@ void GIC_GlobalReset(void)
 
     // Reset all DIC priorities to lowest and clear target processor regs
     for (int i = 32; i < intn; i++) {
-        REG_DIC_PRIORITY[i] = 0xF0;
+        REG_DIC_PRIORITY[i] = 0;
         REG_DIC_TARGETPROC[i] = 0;
     }
 
@@ -149,14 +149,14 @@ void GIC_LocalReset(void)
     REG_DIC_CLRPENDING[0] = ~0;
 
     for (int i = 0; i < 32; i++) {
-        REG_DIC_PRIORITY[i] = 0xF0;
+        REG_DIC_PRIORITY[i] = 0;
         REG_DIC_TARGETPROC[i] = 0;
     }
 
     for (int i = 0; i < 2; i++)
         REG_DIC_CFGREG[i] = ~0;
 
-    // Wait for the spurious IRQ
+    // Acknowledge until it gets a spurious IRQ
     do {
         irq_s = REG_GIC_PENDING(GIC_THIS_CPU_ALIAS);
         REG_GIC_IRQEND(GIC_THIS_CPU_ALIAS) = irq_s;

arm11/source/arm/gic.h

@@ -14,7 +14,7 @@ enum {
 
 enum {
 	GIC_HIGHEST_PRIO = 0x0,
-	GIC_LOWEST_PRIO = 0xF,
+	GIC_LOWEST_PRIO = 0xE,
 };
 
 void GIC_GlobalReset(void);

arm11/source/arm/mmu.c

@@ -3,26 +3,325 @@
 
 #include "arm/mmu.h"
 
-static u32 __attribute__((aligned(16384))) MMU_TranslationTable[4096];
+/* Virtual Memory Mapper */
+#define L1_VA_IDX(v)	(((v) >> 20) & 0xFFF)
+#define L2_VA_IDX(v)	(((v) >> 12) & 0xFF)
 
-// Currently just does a super simple identity mapping
-// with all sections set to uncacheable/unbufferable
-// and no access limitations (RW and no NX bit set)
-void MMU_PopulateTranslationTable(void)
+#define SECT_ADDR_SHIFT		(20)
+#define COARSE_ADDR_SHIFT	(10)
+#define PAGE_ADDR_SHIFT		(12)
+#define LPAGE_ADDR_SHIFT	(16)
+
+#define SECT_SIZE	(BIT(SECT_ADDR_SHIFT))
+#define COARSE_SIZE	(BIT(COARSE_ADDR_SHIFT))
+#define PAGE_SIZE	(BIT(PAGE_ADDR_SHIFT))
+#define LPAGE_SIZE	(BIT(LPAGE_ADDR_SHIFT))
+
+#define SECT_MASK	(~(SECT_SIZE - 1))
+#define COARSE_MASK	(~(COARSE_SIZE - 1))
+#define PAGE_MASK	(~(PAGE_SIZE - 1))
+#define LPAGE_MASK	(~(LPAGE_SIZE - 1))
+
+#define DESCRIPTOR_L1_UNMAPPED	(0)
+#define DESCRIPTOR_L1_COARSE	(1)
+#define DESCRIPTOR_L1_SECTION	(2)
+#define DESCRIPTOR_L1_RESERVED	(3)
+
+#define DESCRIPTOR_L2_UNMAPPED	(0)
+#define DESCRIPTOR_L2_LARGEPAGE	(1)
+#define DESCRIPTOR_L2_PAGE_EXEC	(2)
+#define DESCRIPTOR_L2_PAGE_NX	(3)
+
+#define DESCRIPTOR_TYPE_MASK	(3)
+
+enum DescriptorType {
+	L1_UNMAPPED,
+	L1_COARSE,
+	L1_SECTION,
+	L1_RESERVED,
+
+	L2_UNMAPPED,
+	L2_LARGEPAGE,
+	L2_PAGE,
+};
+
+typedef struct {
+	u32 desc[4096];
+} __attribute__((aligned(16384))) MMU_Lvl1_Table;
+
+typedef struct {
+	u32 desc[256];
+} __attribute__((aligned(1024))) MMU_Lvl2_Table;
+
+static MMU_Lvl1_Table MMU_Lvl1_TT;
+
+/* function to allocate 2nd level page tables */
+#define MAX_SECOND_LEVEL	(4)
+static MMU_Lvl2_Table Lvl2_Tables[MAX_SECOND_LEVEL];
+static u32 Lvl2_Allocated = 0;
+static MMU_Lvl2_Table *Alloc_Lvl2(void)
 {
-	for (int i = 0; i < 4096; i++) {
-		MMU_TranslationTable[i] = (i << 20) | (3 << 10) | 2;
+	if (Lvl2_Allocated == MAX_SECOND_LEVEL)
+		return NULL;
+	return &Lvl2_Tables[Lvl2_Allocated++];
+}
+
+
+/* functions to convert from internal page flag format to ARM */
+static u32 MMU_GetTEX(u32 f)
+{
+	switch(MMU_FLAGS_TYPE(f)) {
+		default:
+		case STRONGLY_ORDERED:
+		case CACHED_WT:
+		case DEVICE_SHARED:
+			return 0;
+
+		case CACHED_WB:
+		case NON_CACHEABLE:
+		case CACHED_WB_ALLOC:
+			return 1;
+
+		case DEVICE_NONSHARED:
+			return 2;
 	}
 }
 
+static u32 MMU_GetCB(u32 f)
+{
+	switch(MMU_FLAGS_TYPE(f)) {
+		default:
+		case STRONGLY_ORDERED:
+		case NON_CACHEABLE:
+		case DEVICE_NONSHARED:
+			return 0;
+
+		case DEVICE_SHARED:
+			return 1;
+
+		case CACHED_WT:
+			return 2;
+
+		case CACHED_WB:
+		case CACHED_WB_ALLOC:
+			return 3;
+	}
+}
+
+static u32 MMU_GetAP(u32 f)
+{
+	switch(MMU_FLAGS_ACCESS(f)) {
+		default:
+		case NO_ACCESS:
+			return 0;
+
+		case READ_ONLY:
+			return 0x21;
+
+		case READ_WRITE:
+			return 0x01;
+	}
+}
+
+static u32 MMU_GetNX(u32 f)
+{
+	return MMU_FLAGS_NOEXEC(f) ? 1 : 0;
+}
+
+static u32 MMU_GetShared(u32 f)
+{
+	return MMU_FLAGS_SHARED(f) ? 1 : 0;
+}
+
+static enum DescriptorType MMU_WalkTT(u32 va)
+{
+	MMU_Lvl2_Table *coarsepd;
+	u32 desc = MMU_Lvl1_TT.desc[L1_VA_IDX(va)];
+
+	switch(desc & DESCRIPTOR_TYPE_MASK) {
+		case DESCRIPTOR_L1_UNMAPPED:
+			return L1_UNMAPPED;
+
+		case DESCRIPTOR_L1_COARSE:
+			break;
+
+		case DESCRIPTOR_L1_SECTION:
+			return L1_SECTION;
+
+		case DESCRIPTOR_L1_RESERVED:
+			return L1_RESERVED;
+	}
+
+	coarsepd = (MMU_Lvl2_Table*)(desc & COARSE_MASK);
+	desc = coarsepd->desc[L2_VA_IDX(va)];
+
+	switch(desc & DESCRIPTOR_TYPE_MASK) {
+		default:
+		case DESCRIPTOR_L2_UNMAPPED:
+			return L2_UNMAPPED;
+
+		case DESCRIPTOR_L2_LARGEPAGE:
+			return L2_LARGEPAGE;
+
+		case DESCRIPTOR_L2_PAGE_NX:
+		case DESCRIPTOR_L2_PAGE_EXEC:
+			return L2_PAGE;
+	}
+}
+
+static MMU_Lvl2_Table *MMU_CoarseFix(u32 va)
+{
+	enum DescriptorType type;
+	MMU_Lvl2_Table *coarsepd;
+
+	type = MMU_WalkTT(va);
+	switch(type) {
+		case L1_UNMAPPED:
+			coarsepd = Alloc_Lvl2();
+			if (coarsepd != NULL)
+				MMU_Lvl1_TT.desc[L1_VA_IDX(va)] = (u32)coarsepd | DESCRIPTOR_L1_COARSE;
+			break;
+
+		case L2_UNMAPPED:
+			coarsepd = (MMU_Lvl2_Table*)(MMU_Lvl1_TT.desc[L1_VA_IDX(va)] & COARSE_MASK);
+			break;
+
+		default:
+			coarsepd = NULL;
+			break;
+	}
+
+	return coarsepd;
+}
+
+
+/* Sections */
+static u32 MMU_SectionFlags(u32 f)
+{
+	return (MMU_GetShared(f) << 16) | (MMU_GetTEX(f) << 12) |
+		(MMU_GetAP(f) << 10) | (MMU_GetNX(f) << 4) |
+		(MMU_GetCB(f) << 2) | DESCRIPTOR_L1_SECTION;
+}
+
+static bool MMU_MapSection(u32 va, u32 pa, u32 flags)
+{
+	enum DescriptorType type = MMU_WalkTT(va);
+	if (type == L1_UNMAPPED) {
+		MMU_Lvl1_TT.desc[L1_VA_IDX(va)] = pa | MMU_SectionFlags(flags);
+		return true;
+	}
+
+	return false;
+}
+
+
+/* Large Pages */
+static u32 MMU_LargePageFlags(u32 f)
+{
+	return (MMU_GetNX(f) << 15) | (MMU_GetTEX(f) << 12) |
+		(MMU_GetShared(f) << 10) | (MMU_GetAP(f) << 4) |
+		(MMU_GetCB(f) << 2) | DESCRIPTOR_L2_LARGEPAGE;
+}
+
+static bool MMU_MapLargePage(u32 va, u32 pa, u32 flags)
+{
+	MMU_Lvl2_Table *l2 = MMU_CoarseFix(va);
+
+	if (l2 == NULL)
+		return false;
+
+	for (u32 i = va; i < (va + 0x10000); i += 0x1000)
+		l2->desc[L2_VA_IDX(i)] = pa | MMU_LargePageFlags(flags);
+
+	return true;
+}
+
+
+/* Pages */
+static u32 MMU_PageFlags(u32 f)
+{
+	return (MMU_GetShared(f) << 10) | (MMU_GetTEX(f) << 6) |
+		(MMU_GetAP(f) << 4) | (MMU_GetCB(f) << 2) |
+		(MMU_GetNX(f) ? DESCRIPTOR_L2_PAGE_NX : DESCRIPTOR_L2_PAGE_EXEC);
+}
+
+static bool MMU_MapPage(u32 va, u32 pa, u32 flags)
+{
+	MMU_Lvl2_Table *l2 = MMU_CoarseFix(va);
+
+	if (l2 == NULL)
+		return false;
+
+	l2->desc[L2_VA_IDX(va)] = pa | MMU_PageFlags(flags);
+	return true;
+}
+
+
+static bool MMU_MappingFits(u32 va, u32 pa, u32 len, u32 abits)
+{
+	return !((va | pa | len) & (BIT(abits) - 1));
+}
+
+u32 MMU_Map(u32 va, u32 pa, u32 size, u32 flags)
+{
+	static const struct {
+		u32 bits;
+		bool (*mapfn)(u32,u32,u32);
+	} VMappers[] = {
+		{
+			.bits = SECT_ADDR_SHIFT,
+			.mapfn = MMU_MapSection,
+		},
+		{
+			.bits = LPAGE_ADDR_SHIFT,
+			.mapfn = MMU_MapLargePage,
+		},
+		{
+			.bits = PAGE_ADDR_SHIFT,
+			.mapfn = MMU_MapPage,
+		},
+	};
+	static const size_t VMapperCount = sizeof(VMappers)/sizeof(*VMappers);
+
+	while(size > 0) {
+		size_t i = 0;
+		for (i = 0; i < VMapperCount; i++) {
+			u32 abits = VMappers[i].bits;
+
+			if (MMU_MappingFits(va, pa, size, abits)) {
+				bool mapped = (VMappers[i].mapfn)(va, pa, flags);
+				u32 offset = BIT(abits);
+
+				// no fun allowed
+				if (!mapped)
+					return size;
+
+				va += offset;
+				pa += offset;
+				size -= offset;
+				break;
+			}
+		}
+
+		if (i == VMapperCount)
+			return size;
+	}
+
+	return 0;
+}
+
 void MMU_Init(void)
 {
-	u32 ttbr0 = (u32)(&MMU_TranslationTable) | 0x12;
+	u32 ttbr0 = (u32)(&MMU_Lvl1_TT) | 0x12;
+
+	// Set up TTBR0/1 and the TTCR
 	ARM_MCR(p15, 0, ttbr0, c2, c0, 0);
 	ARM_MCR(p15, 0, 0, c2, c0, 1);
 	ARM_MCR(p15, 0, 0, c2, c0, 2);
 
+	// Set up the DACR
 	ARM_MCR(p15, 0, 0x55555555, c3, c0, 0);
 
+	// Invalidate the unified TLB
 	ARM_MCR(p15, 0, 0, c8, c7, 0);
 }

arm11/source/arm/mmu.h

@@ -2,5 +2,29 @@
 
 #include <types.h>
 
-void MMU_PopulateTranslationTable(void);
+enum MMU_MemoryType {
+	STRONGLY_ORDERED = 0,
+	NON_CACHEABLE,
+	DEVICE_SHARED,
+	DEVICE_NONSHARED,
+	CACHED_WT,
+	CACHED_WB,
+	CACHED_WB_ALLOC,
+};
+
+enum MMU_MemoryAccess {
+	NO_ACCESS = 0,
+	READ_ONLY,
+	READ_WRITE,
+};
+
+#define MMU_FLAGS(t, ap, nx, s)	((s) << 25 | (nx) << 24 | (ap) << 8 | (t))
+
+#define MMU_FLAGS_TYPE(f)	((f) & 0xFF)
+#define MMU_FLAGS_ACCESS(f)	(((f) >> 8) & 0xFF)
+
+#define MMU_FLAGS_NOEXEC(f)	((f) & BIT(24))
+#define MMU_FLAGS_SHARED(f)	((f) & BIT(25))
+
+u32 MMU_Map(u32 va, u32 pa, u32 size, u32 flags);
 void MMU_Init(void);

arm11/source/arm/timer.c

@@ -0,0 +1,20 @@
+#include <types.h>
+#include <arm.h>
+
+#include "arm/gic.h"
+#include "arm/timer.h"
+
+#define TIMER_INTERRUPT	(0x1E)
+
+#define REG_TIMER(c, n)	REG_ARM_PMR(0x700 + ((c) * 0x100) + (n), u32)
+#define TIMER_THIS_CPU	(-1)
+
+#define REG_TIMER_LOAD(c)	*REG_TIMER((c), 0x00)
+#define REG_TIMER_COUNT(c)	*REG_TIMER((c), 0x04)
+#define REG_TIMER_CNT(c)	*REG_TIMER((c), 0x08)
+#define REG_TIMER_IRQ(c)	*REG_TIMER((c), 0x0C)
+
+#define TIMER_CNT_SCALE(n)	((n) << 8)
+#define TIMER_CNT_INT_EN	BIT(2)
+#define TIMER_CNT_RELOAD	BIT(1)
+#define TIMER_CNT_ENABLE	BIT(0)

arm11/source/arm/timer.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <types.h>
+
+// The timer interval is calculated using the following equation:
+// T = [(PRESCALER_value + 1) * (Load_value + 1) * 2] / CPU_CLK
+// therefore
+// Load_value = [(CPU_CLK / 2) * (T / (PRESCALER_value + 1))] - 1
+
+#define BASE_CLKRATE	(268111856)
+
+#define CLK_FREQ_TO_INTERVAL(f, p)	((BASE_CLKRATE/2) * ((f) / ((p) + 1)) - 1)

arm11/source/arm/vectors.s

@@ -1,21 +1,42 @@
 #include <arm.h>
 
 .arm
-.section .text
 .align 2
 
-@ temporarily use dumb vectors redirected from the bootrom, rather
-@ than MMU-mapped pages
+.section .vector, "ax"
+vectors:
+    ldr pc, =XRQ_Reset
+    ldr pc, =XRQ_Reset
+    ldr pc, =XRQ_Reset
+    ldr pc, =XRQ_Reset
+    ldr pc, =XRQ_Reset
+    b . @ RESERVED
+    ldr pc, =XRQ_IRQ
+    ldr pc, =XRQ_Reset
+.pool
 
-.global irq_vector
-.type irq_vector, %function
-irq_vector:
+.section .text.XRQS, "ax"
+
+XRQ_Reset:
+    mov r0, #0x18000000
+    add r1, r0, #(6 << 20)
+    mov r2, #0xFFFFFFFF
+    1:
+        cmp r0, r1
+        strne r2, [r0], #4
+        bne 1b
+    2:
+        wfi
+        b 2b
+
+.global XRQ_IRQ
+XRQ_IRQ:
     sub lr, lr, #4             @ Fix return address
     srsfd sp!, #SR_SVC_MODE    @ Store IRQ mode LR and SPSR on the SVC stack
     cps #SR_SVC_MODE           @ Switch to SVC mode
     push {r0-r4, r12, lr}      @ Preserve registers
 
-    and r4, sp, #7              @ Fix SP to be 8byte aligned
+    and r4, sp, #7             @ Fix SP to be 8byte aligned
     sub sp, sp, r4
 
     bl GIC_MainHandler

arm11/source/boot.s

@@ -45,21 +45,15 @@ __boot:
     b 1b
 
 corezero_start:
-    ldr r0, =__bss_start
-    ldr r1, =__bss_end
+    ldr r0, =__bss_pa
+    ldr r1, =__bss_len
     mov r2, #0
+    add r1, r0, r1
     .Lclearbss:
         cmp r0, r1
         strlt r2, [r0], #4
         blt .Lclearbss
 
-    @ Set up IRQ vector
-    ldr r0, =0x1FFFFFA0
-    ldr r1, =0xE51FF004
-    ldr r2, =irq_vector
-
-    stmia r0, {r1, r2}
-
 coresmp_start:
     bl SYS_CoreInit
     bl MPCoreMain

arm11/source/hw/gpulcd.c

@@ -19,7 +19,6 @@ void LCD_Initialize(u8 brightness)
 	*REG_LCD(0xA40) = brightness;
 	*REG_LCD(0x244) = 0x1023E;
 	*REG_LCD(0xA44) = 0x1023E;
-	return;
 }
 
 void LCD_Deinitialize(void)
@@ -28,7 +27,6 @@ void LCD_Deinitialize(void)
 	*REG_LCD(0xA44) = 0;
 	*REG_LCD(0x00C) = 0;
 	*REG_LCD(0x014) = 0;
-	return;
 }
 
 /* GPU Control Registers */
@@ -88,7 +86,6 @@ void GPU_SetFramebuffers(const u32 *framebuffers)
 	*GPU_PDC(1, 0x6C) = framebuffers[5];
 	*GPU_PDC(0, 0x78) = 0;
 	*GPU_PDC(1, 0x78) = 0;
-	return;
 }
 
 void GPU_SetFramebufferMode(u32 screen, u8 mode)
@@ -123,7 +120,6 @@ void GPU_SetFramebufferMode(u32 screen, u8 mode)
 
 	*fbcfg_reg = cfg;
 	*fbstr_reg = stride;
-	return;
 }
 
 void GPU_Init(void)

arm11/source/hw/gpulcd.h

@@ -13,4 +13,4 @@ void GPU_PSCFill(u32 start, u32 end, u32 fv);
 #define PDC_RGBA4   (4<<0)
 void GPU_SetFramebufferMode(u32 screen, u8 mode);
 void GPU_SetFramebuffers(const u32 *framebuffers);
-void GPU_Init();
+void GPU_Init(void);

arm11/source/main.c

@@ -8,8 +8,8 @@
 #include "hw/i2c.h"
 #include "hw/mcu.h"
 
-#define LEGACY_BOOT_ENTRY	((vu32*)0x1FFFFFFC)
-#define LEGACY_BOOT_MAGIC	(0xDEADDEAD)
+static bool legacy;
+void SYS_CoreShutdown(void);
 
 void PXI_RX_Handler(u32 __attribute__((unused)) irqn)
 {
@@ -29,7 +29,8 @@ void PXI_RX_Handler(u32 __attribute__((unused)) irqn)
 	switch (cmd) {
 		case PXI_LEGACY_MODE:
 		{
-			*LEGACY_BOOT_ENTRY = LEGACY_BOOT_MAGIC;
+			// TODO: If SMP is enabled, an IPI should be sent here (with a DSB)
+			legacy = true;
 			ret = 0;
 			break;
 		}
@@ -65,11 +66,15 @@ void PXI_RX_Handler(u32 __attribute__((unused)) irqn)
 		case PXI_I2C_READ:
 		{
 			ret = I2C_readRegBuf(args[0], args[1], (u8*)args[2], args[3]);
+			ARM_WbDC_Range((void*)args[2], args[3]);
+			ARM_DSB();
 			break;
 		}
 
 		case PXI_I2C_WRITE:
 		{
+			ARM_InvDC_Range((void*)args[2], args[3]);
+			ARM_DSB();
 			ret = I2C_writeRegBuf(args[0], args[1], (u8*)args[2], args[3]);
 			break;
 		}
@@ -95,14 +100,8 @@ void PXI_RX_Handler(u32 __attribute__((unused)) irqn)
 
 void MPCoreMain(void)
 {
-	u32 entry;
-
+	legacy = false;
 	GIC_Enable(IRQ_PXI_RX, BIT(0), GIC_HIGHEST_PRIO, PXI_RX_Handler);
-	*LEGACY_BOOT_ENTRY = 0;
-
-	PXI_Reset();
-	I2C_init();
-	//MCU_init();
 
 	PXI_Barrier(ARM11_READY_BARRIER);
 	ARM_EnableInterrupts();
@@ -110,16 +109,10 @@ void MPCoreMain(void)
 	// Process IRQs until the ARM9 tells us it's time to boot something else
 	do {
 		ARM_WFI();
-	} while(*LEGACY_BOOT_ENTRY != LEGACY_BOOT_MAGIC);
+	} while(!legacy);
 
 	// Perform any needed deinit stuff
 	ARM_DisableInterrupts();
-	GIC_GlobalReset();
-	GIC_LocalReset();
 
-	do {
-		entry = *LEGACY_BOOT_ENTRY;
-	} while(entry == LEGACY_BOOT_MAGIC);
-
-	((void (*)())(entry))();
+	SYS_CoreShutdown();
 }

arm11/source/sections.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#define DEF_SECT_(n)	extern u32 __##n##_pa, __##n##_va, __##n##_len; static const u32 n##_pa = (u32)&__##n##_pa, n##_va = (u32)&__##n##_va;
+DEF_SECT_(vector)
+DEF_SECT_(text)
+DEF_SECT_(data)
+DEF_SECT_(rodata)
+DEF_SECT_(bss)
+#undef DEF_SECT_
+
+#define SECTION_VA(n)	n##_va
+#define SECTION_PA(n)	n##_pa
+#define SECTION_LEN(n)	((u32)&__##n##_len)
+
+#define SECTION_TRI(n)	SECTION_VA(n), SECTION_PA(n), SECTION_LEN(n)

arm11/source/sys.c

@@ -1,12 +1,21 @@
 #include <types.h>
 #include <arm.h>
+#include <pxi.h>
 
 #include "arm/gic.h"
 #include "arm/scu.h"
 #include "arm/mmu.h"
 
+#include "hw/i2c.h"
+
 #define CFG11_MPCORE_CLKCNT	((vu16*)(0x10141300))
-#define CFG11_SOCINFO	((vu16*)(0x10140FFC))
+#define CFG11_SOCINFO		((vu16*)(0x10140FFC))
+
+#define LEGACY_BOOT_ENTRY	((vu32*)0x1FFFFFFC)
+
+#define INIT_DONE	(0xDDEEFFAA)
+
+static volatile u32 sys_init_state = 0;
 
 static bool SYS_IsNewConsole(void)
 {
@@ -36,25 +45,60 @@ static void SYS_EnableClkMult(void)
 	}
 }
 
+#include "sections.h"
+
+#define	MMU_FLAGS_DEF	MMU_FLAGS(STRONGLY_ORDERED, READ_WRITE, 0, 1)
+
 static void SYS_CoreZeroInit(void)
 {
 	GIC_GlobalReset();
+	GIC_LocalReset();
+
+	*LEGACY_BOOT_ENTRY = 0;
+
 	SYS_EnableClkMult();
 
 	SCU_Init();
 
-	// Init MMU tables here
-	MMU_PopulateTranslationTable();
+	// Map all sections here
+	MMU_Map(SECTION_TRI(vector), MMU_FLAGS(CACHED_WT, READ_ONLY, 0, 0));
+	MMU_Map(SECTION_TRI(text), MMU_FLAGS(CACHED_WT, READ_ONLY, 0, 1));
+	MMU_Map(SECTION_TRI(data), MMU_FLAGS(CACHED_WB_ALLOC, READ_WRITE, 1, 1));
+	MMU_Map(SECTION_TRI(rodata), MMU_FLAGS(CACHED_WT, READ_ONLY, 1, 1));
+	MMU_Map(SECTION_TRI(bss), MMU_FLAGS(CACHED_WB_ALLOC, READ_WRITE, 1, 1));
+
+	// IO Registers
+	MMU_Map(0x10100000, 0x10100000, 4UL << 20, MMU_FLAGS(DEVICE_SHARED, READ_WRITE, 1, 1));
+
+	// MPCore Private Memory Region
+	MMU_Map(0x17E00000, 0x17E00000, 8UL << 10, MMU_FLAGS(DEVICE_SHARED, READ_WRITE, 1, 1));
+
+	// VRAM
+	MMU_Map(0x18000000, 0x18000000, 6UL << 20, MMU_FLAGS(CACHED_WT, READ_WRITE, 1, 1));
+
+	// FCRAM
+	MMU_Map(0x20000000, 0x20000000, 128UL << 20, MMU_FLAGS(CACHED_WB, READ_WRITE, 1, 1));
+}
+
+static void SYS_InitPeripherals(void)
+{
+	PXI_Reset();
+	I2C_init();
+	//MCU_init();
 }
 
 void SYS_CoreInit(void)
 {
 	if (!ARM_CoreID()) {
 		SYS_CoreZeroInit();
+	} else {
+		while(sys_init_state != INIT_DONE)
+			ARM_WFE();
+
+		GIC_LocalReset();
 	}
 
-	GIC_LocalReset();
-
+	// set up MMU registers
 	MMU_Init();
 
 	// enable fancy ARM11 stuff
@@ -65,4 +109,50 @@ void SYS_CoreInit(void)
 		CR_MMU | CR_CACHES | CR_FLOWPRED | CR_HIGHVEC | CR_DSUBPAGE);
 
 	ARM_DSB();
+
+	if (!ARM_CoreID()) {
+		SYS_InitPeripherals();
+
+		sys_init_state = INIT_DONE;
+		ARM_DSB();
+		ARM_SEV();
+	}
+}
+
+// assumes all cores have been initialized
+void SYS_CoreShutdown(void)
+{
+	u32 core = ARM_CoreID();
+
+	if (!core) {
+		// wait for the other cores to do their thing
+		while(sys_init_state != (INIT_DONE - MAX_CPU + 1)) {
+			ARM_WFE();
+			ARM_DSB();
+		}
+
+		GIC_GlobalReset();
+	} else {
+		__atomic_sub_fetch(&sys_init_state, 1, __ATOMIC_SEQ_CST);
+		ARM_SEV();
+	}
+
+	GIC_LocalReset();
+
+	ARM_WbInvDC();
+	ARM_InvIC();
+	ARM_DSB();
+
+	ARM_SetCR(ARM_GetCR() & ~(CR_MMU | CR_CACHES | CR_FLOWPRED));
+	ARM_SetACR(ARM_GetACR() &
+		~(ACR_RETSTK | ACR_DBPRED | ACR_SBPRED | ACR_FOLDING | ACR_SMP));
+
+	if (!core) {
+		while(*LEGACY_BOOT_ENTRY == 0);
+		((void (*)(void))(*LEGACY_BOOT_ENTRY))();
+	} else {
+		// Branch to bootrom function that does SMP reinit magic
+		// (waits for IPI + branches to word @ 0x1FFFFFDC)
+		((void (*)(void))0x0001004C)();
+	}
 }

common/arm.h

@@ -58,8 +58,8 @@
 	#define ICACHE_SZ	(16384)
 	#define DCACHE_SZ	(16384)
 
-	#define MAX_IRQ	(96)
-	#define MAX_CPU	(2)
+	#define MAX_IRQ	(224)
+	#define MAX_CPU	(1)
 #endif
 
 #define CR_CACHES	(CR_DCACHE | CR_ICACHE)