From 5f257392863a807a4c1b70f836d99cb656c931b7 Mon Sep 17 00:00:00 2001
From: profi200 <fd3194@gmx.de>
Date: Sun, 22 Nov 2020 01:47:58 +0100
Subject: [PATCH] Enabled more debugging techniques by default in debug builds.
 Added more Arm intrinsics. ARM7 stub now waits for REG_VCOUNT == 126 (same as
 after BIOS intro). Enable ROM mirroring for all 8 Mbit games instead of just
 Classic NES Series. Other misc. changes.

---
 arm11/Makefile                          |  15 ++--
 include/arm.h                           |  69 +++++++++++++++
 include/arm11/debug.h                   |   3 +-
 include/arm9/arm7_stub.h                |   9 +-
 include/arm_intrinsic.h                 |   2 +
 include/ipc_handler.h                   |   2 +-
 kernel/include/internal/contextswitch.h |   2 +-
 kernel/source/contextswitch.s           |   2 +-
 kernel/source/kernel.c                  |   4 +
 source/arm11/debug.c                    | 106 +++++++++++++++---------
 source/arm11/hardware/exception.s       |  12 ++-
 source/arm11/hardware/gfx.c             |   1 -
 source/arm11/main.c                     |  45 ++++------
 source/arm9/arm7_stub.s                 |  63 +++++++-------
 source/arm9/hardware/lgy.c              |  19 +----
 source/arm9/ipc_handler.c               |   1 +
 source/fsutil.c                         |   7 +-
 17 files changed, 229 insertions(+), 133 deletions(-)

diff --git a/arm11/Makefile b/arm11/Makefile
index c2ae063..b6aeddf 100644
--- a/arm11/Makefile
+++ b/arm11/Makefile
@@ -36,18 +36,24 @@ endif
 #---------------------------------------------------------------------------------
 ARCH	:=	-march=armv6k+vfpv2 -mtune=mpcore -mfloat-abi=hard -mtp=soft -marm -mthumb-interwork
 
-CFLAGS	:=	$(ARCH) -std=gnu17 -O2 -g -flto -mword-relocations -ffunction-sections \
-			-fno-math-errno -Wall -Wextra -Wno-unused-function
+CFLAGS	:=	$(ARCH) -std=c17 -O2 -g -flto -mword-relocations \
+			-ffunction-sections -fno-math-errno -Wall -Wextra
 CFLAGS	+=	$(INCLUDE) $(DEFINES)
 
 CXXFLAGS	:=	$(ARCH) -std=c++17 -O2 -g -flto -fno-rtti -fno-exceptions \
-				-mword-relocations -ffunction-sections -fno-math-errno -Wall -Wextra \
-				-Wno-unused-function
+				-mword-relocations -ffunction-sections -fno-math-errno -Wall -Wextra
 CXXFLAGS	+=	$(INCLUDE) $(DEFINES)
 
 ASFLAGS	:=	$(ARCH) -g -flto $(INCLUDE) $(DEFINES)
 LDFLAGS	=	$(ARCH) -g -flto -specs=../arm11.specs -Wl,-Map,$(notdir $*.map) -nostartfiles
 
+ifeq ($(strip $(NO_DEBUG)),)
+	CFLAGS	:=	$(subst -flto,,$(CFLAGS)) -fstack-protector-strong -fno-inline
+	CXXFLAGS	:=	$(subst -flto,,$(CXXFLAGS)) -fstack-protector-strong -fno-inline
+	ASFLAGS	:=	$(subst -flto,,$(ASFLAGS))
+	LDFLAGS	:=	$(subst -flto,,$(LDFLAGS)) -fstack-protector-strong -fno-inline -Wl,-wrap=malloc,-wrap=calloc,-wrap=free
+endif
+
 LIBS	:=	-lm
 
 #---------------------------------------------------------------------------------
@@ -188,4 +194,3 @@ endef
 
 #---------------------------------------------------------------------------------------
 endif
-#---------------------------------------------------------------------------------------
\ No newline at end of file
diff --git a/include/arm.h b/include/arm.h
index 6c71949..be98ca7 100644
--- a/include/arm.h
+++ b/include/arm.h
@@ -197,6 +197,59 @@ ALWAYS_INLINE u32 __strex(vu32 *addr, u32 val)
 
 MAKE_INTR_NO_INOUT(1, clrex, "memory")
 
+// Debug ID Register
+MAKE_INTR_GET_REG(0, getDidr, "mrc p14, 0, %0, c0, c0, 0")
+
+// Debug Status and Control Register
+MAKE_INTR_GET_REG(1, getDscr, "mrc p14, 0, %0, c0, c1, 0")
+MAKE_INTR_SET_REG(1, setDscr, "mcr p14, 0, %0, c0, c1, 0", "memory")
+
+// Data Transfer Register
+
+// Vector Catch Register
+MAKE_INTR_GET_REG(1, getVcr, "mrc p14, 0, %0, c0, c7, 0")
+MAKE_INTR_SET_REG(1, setVcr, "mcr p14, 0, %0, c0, c7, 0", "memory")
+
+// Breakpoint Value Register 0-5
+MAKE_INTR_GET_REG(1, getBvr0, "mrc p14, 0, %0, c0, c0, 4")
+MAKE_INTR_SET_REG(1, setBvr0, "mcr p14, 0, %0, c0, c0, 4", "memory")
+MAKE_INTR_GET_REG(1, getBvr1, "mrc p14, 0, %0, c0, c1, 4")
+MAKE_INTR_SET_REG(1, setBvr1, "mcr p14, 0, %0, c0, c1, 4", "memory")
+MAKE_INTR_GET_REG(1, getBvr2, "mrc p14, 0, %0, c0, c2, 4")
+MAKE_INTR_SET_REG(1, setBvr2, "mcr p14, 0, %0, c0, c2, 4", "memory")
+MAKE_INTR_GET_REG(1, getBvr3, "mrc p14, 0, %0, c0, c3, 4")
+MAKE_INTR_SET_REG(1, setBvr3, "mcr p14, 0, %0, c0, c3, 4", "memory")
+MAKE_INTR_GET_REG(1, getBvr4, "mrc p14, 0, %0, c0, c4, 4")
+MAKE_INTR_SET_REG(1, setBvr4, "mcr p14, 0, %0, c0, c4, 4", "memory")
+MAKE_INTR_GET_REG(1, getBvr5, "mrc p14, 0, %0, c0, c5, 4")
+MAKE_INTR_SET_REG(1, setBvr5, "mcr p14, 0, %0, c0, c5, 4", "memory")
+
+// Breakpoint Control Register 0-5
+MAKE_INTR_GET_REG(1, getBcr0, "mrc p14, 0, %0, c0, c0, 5")
+MAKE_INTR_SET_REG(1, setBcr0, "mcr p14, 0, %0, c0, c0, 5", "memory")
+MAKE_INTR_GET_REG(1, getBcr1, "mrc p14, 0, %0, c0, c1, 5")
+MAKE_INTR_SET_REG(1, setBcr1, "mcr p14, 0, %0, c0, c1, 5", "memory")
+MAKE_INTR_GET_REG(1, getBcr2, "mrc p14, 0, %0, c0, c2, 5")
+MAKE_INTR_SET_REG(1, setBcr2, "mcr p14, 0, %0, c0, c2, 5", "memory")
+MAKE_INTR_GET_REG(1, getBcr3, "mrc p14, 0, %0, c0, c3, 5")
+MAKE_INTR_SET_REG(1, setBcr3, "mcr p14, 0, %0, c0, c3, 5", "memory")
+MAKE_INTR_GET_REG(1, getBcr4, "mrc p14, 0, %0, c0, c4, 5")
+MAKE_INTR_SET_REG(1, setBcr4, "mcr p14, 0, %0, c0, c4, 5", "memory")
+MAKE_INTR_GET_REG(1, getBcr5, "mrc p14, 0, %0, c0, c5, 5")
+MAKE_INTR_SET_REG(1, setBcr5, "mcr p14, 0, %0, c0, c5, 5", "memory")
+
+// Watchpoint Value Register 0-1
+MAKE_INTR_GET_REG(1, getWvr0, "mrc p14, 0, %0, c0, c0, 6")
+MAKE_INTR_SET_REG(1, setWvr0, "mcr p14, 0, %0, c0, c0, 6", "memory")
+MAKE_INTR_GET_REG(1, getWvr1, "mrc p14, 0, %0, c0, c1, 6")
+MAKE_INTR_SET_REG(1, setWvr1, "mcr p14, 0, %0, c0, c1, 6", "memory")
+
+// Watchpoint Control Register 0-1
+MAKE_INTR_GET_REG(1, getWcr0, "mrc p14, 0, %0, c0, c0, 7")
+MAKE_INTR_SET_REG(1, setWcr0, "mcr p14, 0, %0, c0, c0, 7", "memory")
+MAKE_INTR_GET_REG(1, getWcr1, "mrc p14, 0, %0, c0, c1, 7")
+MAKE_INTR_SET_REG(1, setWcr1, "mcr p14, 0, %0, c0, c1, 7", "memory")
+
 ALWAYS_INLINE u32 __getCpuId(void)
 {
 	u32 cpuId;
@@ -224,6 +277,22 @@ MAKE_INTR_SET_REG(1, setTtbcr, "mcr p15, 0, %0, c2, c0, 2", "memory")
 MAKE_INTR_GET_REG(1, getDacr, "mrc p15, 0, %0, c3, c0, 0")
 MAKE_INTR_SET_REG(1, setDacr, "mcr p15, 0, %0, c3, c0, 0", "memory")
 
+// Data Fault Status Register
+MAKE_INTR_GET_REG(1, getDfsr, "mrc p15, 0, %0, c5, c0, 0")
+MAKE_INTR_SET_REG(1, setDfsr, "mcr p15, 0, %0, c5, c0, 0", "memory")
+
+// Instruction Fault Status Register
+MAKE_INTR_GET_REG(1, getIfsr, "mrc p15, 0, %0, c5, c0, 1")
+MAKE_INTR_SET_REG(1, setIfsr, "mcr p15, 0, %0, c5, c0, 1", "memory")
+
+// Fault Address Register
+MAKE_INTR_GET_REG(1, getFar, "mrc p15, 0, %0, c6, c0, 0")
+MAKE_INTR_SET_REG(1, setFar, "mcr p15, 0, %0, c6, c0, 0", "memory")
+
+// Watchpoint Fault Address Register
+MAKE_INTR_GET_REG(1, getWfar, "mrc p15, 0, %0, c6, c0, 1")
+MAKE_INTR_SET_REG(1, setWfar, "mcr p15, 0, %0, c6, c0, 1", "memory")
+
 // Flush Prefetch Buffer
 // Data Synchronization Barrier
 // Data Memory Barrier
diff --git a/include/arm11/debug.h b/include/arm11/debug.h
index aec7e4a..67f5261 100644
--- a/include/arm11/debug.h
+++ b/include/arm11/debug.h
@@ -22,9 +22,8 @@
 
 
 
-noreturn void panic();
+noreturn void panic(void);
 noreturn void panicMsg(const char *msg);
-//void debugMemdump(const char *filepath, void *mem, size_t size);
 
 // Exception tests
 /*static inline regTest(void)
diff --git a/include/arm9/arm7_stub.h b/include/arm9/arm7_stub.h
index cfee4c8..f4eb409 100644
--- a/include/arm9/arm7_stub.h
+++ b/include/arm9/arm7_stub.h
@@ -4,6 +4,9 @@
 
 
 
-extern const u32 _arm7_stub_start[];
-extern const u32 _arm7_stub_swi[]; // Final ARM9 mem location.
-extern const u32 _arm7_stub_size[];
+noreturn void _a7_overlay_stub(void);
+extern const u32 _a7_overlay_stub_size[];
+
+noreturn void _a7_stub_start(void);
+extern u16 _a7_stub9_swi[]; // Final ARM9 mem location.
+extern const u32 _a7_stub_size[];
diff --git a/include/arm_intrinsic.h b/include/arm_intrinsic.h
index 07a4226..ebada1c 100644
--- a/include/arm_intrinsic.h
+++ b/include/arm_intrinsic.h
@@ -111,6 +111,7 @@ ALWAYS_INLINE u64 __##inst(u32 op1, u32 op2, u64 acc)
 
 
 
+// Pack Halfword Bottom Top.
 #define __pkhbt(op1, op2, sh)                                                               \
 ({                                                                                          \
 	u32 __res;                                                                              \
@@ -118,6 +119,7 @@ ALWAYS_INLINE u64 __##inst(u32 op1, u32 op2, u64 acc)
 	__res;                                                                                  \
 })
 
+// Pack Halfword Top Bottom.
 #define __pkhtb(op1, op2, sh)                                                                     \
 ({                                                                                                \
 	u32 __res, __sh = (sh);                                                                       \
diff --git a/include/ipc_handler.h b/include/ipc_handler.h
index 6f1713a..b95d69b 100644
--- a/include/ipc_handler.h
+++ b/include/ipc_handler.h
@@ -64,7 +64,7 @@ typedef enum
 	IPC_CMD9_BACKUP_GBA_SAVE   = MAKE_CMD9(0, 0, 0),
 
 	// Miscellaneous API.
-	IPC_CMD9_PREPARE_POWER     = MAKE_CMD9(0, 0, 0)
+	IPC_CMD9_PREPARE_POWER     = MAKE_CMD9(0, 0, 0)  // Also used for panic() and guruMeditation().
 } IpcCmd9;
 
 enum {_CMD11_C_BASE = __COUNTER__ + 1}; // Start at 0.
diff --git a/kernel/include/internal/contextswitch.h b/kernel/include/internal/contextswitch.h
index b91c33e..236b4ac 100644
--- a/kernel/include/internal/contextswitch.h
+++ b/kernel/include/internal/contextswitch.h
@@ -42,7 +42,7 @@ typedef struct
 
 
 
-KRes switchContext(KRes res, void *oldSp, uintptr_t newSp);
+KRes switchContext(KRes res, uintptr_t *oldSp, uintptr_t newSp);
 
 #ifdef __cplusplus
 }
diff --git a/kernel/source/contextswitch.s b/kernel/source/contextswitch.s
index 58beb13..78c4085 100644
--- a/kernel/source/contextswitch.s
+++ b/kernel/source/contextswitch.s
@@ -54,7 +54,7 @@ switchContextAllRegs_end:
 	cpsie i
 	bx lr*/
 
-@ KRes switchContext(KRes res, void *oldSp, uintptr_t newSp);
+@ KRes switchContext(KRes res, uintptr_t *oldSp, uintptr_t newSp);
 BEGIN_ASM_FUNC switchContext
 	stmfd sp!, {r4-r11, lr}
 	str sp, [r1]
diff --git a/kernel/source/kernel.c b/kernel/source/kernel.c
index 47d7682..3916f37 100644
--- a/kernel/source/kernel.c
+++ b/kernel/source/kernel.c
@@ -195,6 +195,10 @@ bool waitQueueWakeN(ListNode *waitQueue, u32 wakeCount, KRes res, bool reschedul
 
 static KRes scheduler(TaskState curTaskState)
 {
+#ifndef NDEBUG
+	if((__getCpsr() & PSR_MODE_MASK) != PSR_SYS_MODE) panic();
+#endif
+
 	TaskCb *const curDeadTask = g_curDeadTask;
 	// TODO: Get rid of this and find a better way.
 	if(UNLIKELY(curDeadTask != NULL))
diff --git a/source/arm11/debug.c b/source/arm11/debug.c
index 3fe6efd..211bef9 100644
--- a/source/arm11/debug.c
+++ b/source/arm11/debug.c
@@ -17,6 +17,7 @@
  */
 
 #include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
 #include "types.h"
 #include "mem_map.h"
@@ -24,7 +25,6 @@
 #include "arm11/fmt.h"
 #include "hardware/pxi.h"
 #include "ipc_handler.h"
-#include "hardware/gfx.h"
 #include "arm11/hardware/interrupt.h"
 #include "arm.h"
 #include "arm11/hardware/mcu.h"
@@ -32,20 +32,17 @@
 
 
 
-noreturn void panic()
+noreturn void panic(void)
 {
 	enterCriticalSection();
 
 	consoleInit(SCREEN_BOT, NULL);
 	ee_printf("\x1b[41m\x1b[0J\x1b[15C****PANIC!!!****\n");
 
-	//PXI_sendPanicCmd(IPC_CMD9_PANIC);
+	PXI_sendPanicCmd(IPC_CMD9_PREPARE_POWER);
 
 	// Wait for A/B/X or Y
-	do
-	{
-		hidScanInput();
-	} while(!(hidKeysDown() & (KEY_A | KEY_B | KEY_X | KEY_Y)));
+	while(!(REG_HID_PAD & (KEY_A | KEY_B | KEY_X | KEY_Y)));
 
 	MCU_powerOffSys();
 	while(1) __wfi();
@@ -59,32 +56,21 @@ noreturn void panicMsg(const char *msg)
 	ee_printf("\x1b[41m\x1b[0J\x1b[15C****PANIC!!!****\n\n");
 	ee_printf("\nERROR MESSAGE:\n%s\n", msg);
 
-	//PXI_sendPanicCmd(IPC_CMD9_PANIC);
+	PXI_sendPanicCmd(IPC_CMD9_PREPARE_POWER);
 
 	// Wait for A/B/X or Y
-	do
-	{
-		hidScanInput();
-	} while(!(hidKeysDown() & (KEY_A | KEY_B | KEY_X | KEY_Y)));
+	while(!(REG_HID_PAD & (KEY_A | KEY_B | KEY_X | KEY_Y)));
 
 	MCU_powerOffSys();
 	while(1) __wfi();
 }
 
 // Expects the registers in the exception stack to be in the following order:
-// r0-r14, pc (unmodified), cpsr
+// r0-r14, pc (unmodified), CPSR, DFSR, IFSR, FAR, WFAR
 noreturn void guruMeditation(u8 type, const u32 *excStack)
 {
 	const char *const typeStr[3] = {"Undefined instruction", "Prefetch abort", "Data abort"};
 	u32 realPc, instSize = 4;
-	//bool codeChanged = false;
-
-
-	// verify text and rodata
-	/*u32 prevHash = debugHash;
-	debugHashCodeRoData();
-	if(prevHash != debugHash)
-		codeChanged = true;*/
 
 	consoleInit(SCREEN_BOT, NULL);
 
@@ -123,32 +109,72 @@ noreturn void guruMeditation(u8 type, const u32 *excStack)
 		}
 	}
 
-	//if(codeChanged) ee_printf("Attention: RO section data changed!!");
-
-	//PXI_sendPanicCmd(IPC_CMD9_EXCEPTION);
+	PXI_sendPanicCmd(IPC_CMD9_PREPARE_POWER);
 
 	// Wait for A/B/X or Y
-	do
-	{
-		hidScanInput();
-	} while(!(hidKeysDown() & (KEY_A | KEY_B | KEY_X | KEY_Y)));
+	while(!(REG_HID_PAD & (KEY_A | KEY_B | KEY_X | KEY_Y)));
 
 	MCU_powerOffSys();
 	while(1) __wfi();
 }
 
-/*void debugMemdump(const char *filepath, void *mem, size_t size)
+#ifndef NDEBUG
+// Needs to be marked as used to work with LTO.
+// The used attribute also overrides the newlib symbol.
+// This is for debugging purposes only. For security this value needs to be random!
+__attribute__((used)) uintptr_t __stack_chk_guard = 0xC724B66D;
+
+// Needs to be marked as noinline and used to work with LTO.
+// The used attribute also overrides the newlib symbol.
+// Combine -fstack-protector-all with -fno-inline to get the most effective detection.
+__attribute__((noinline, used)) noreturn void __stack_chk_fail(void)
 {
-	s32 file;
+	panicMsg("Stack smash!");
+}
 
-	if((file = fOpen(filepath, FS_CREATE_ALWAYS | FS_OPEN_WRITE)) < 0)
-	{
-		return;
-	}
-		
-	fWrite(file, mem, size);
-	
-	fSync(file);
 
-	fClose(file);
-}*/
+// Add "-Wl,-wrap=malloc,-wrap=calloc,-wrap=free" to LDFLAGS to enable the heap check.
+static const u32 __heap_chk_guard[4] = {0x9240A724, 0x6A6594A0, 0x976F0392, 0xB3A669AB};
+
+void* __real_malloc(size_t size);
+void __real_free(void *ptr);
+
+void* __wrap_malloc(size_t size)
+ {
+	void *const buf = __real_malloc(size + 32);
+	if(buf == NULL) return NULL;
+
+	memcpy(buf, &size, sizeof(size_t));
+	memcpy(buf + sizeof(size_t), (u8*)__heap_chk_guard + sizeof(size_t), 16 - sizeof(size_t));
+	memcpy(buf + 16 + size, __heap_chk_guard, 16);
+
+	return buf + 16;
+}
+
+void* __wrap_calloc(size_t num, size_t size)
+{
+	void *const buf = __wrap_malloc(num * size);
+	if(buf == NULL) return NULL;
+
+	memset(buf, 0, num * size);
+
+	return buf;
+}
+
+void __wrap_free(void *ptr)
+{
+	if(ptr == NULL) return;
+
+	if(memcmp(ptr - (16 - sizeof(size_t)), (u8*)__heap_chk_guard + sizeof(size_t), 16 - sizeof(size_t)) != 0)
+		panicMsg("Heap underflow!");
+	size_t size;
+	memcpy(&size, ptr - 16, sizeof(size_t));
+
+	// Important! Adjust the size check if needed.
+	// 1024u * 512 is roughly ok for AXIWRAM.
+	if(size > (1024u * 512) || memcmp(ptr + size, __heap_chk_guard, 16) != 0)
+		panicMsg("Heap overflow!");
+
+	__real_free(ptr - 16);
+}
+#endif
diff --git a/source/arm11/hardware/exception.s b/source/arm11/hardware/exception.s
index 1f66b90..7d1a460 100644
--- a/source/arm11/hardware/exception.s
+++ b/source/arm11/hardware/exception.s
@@ -36,7 +36,7 @@ EXCEPTION_ENTRY undefInstrHandler, 0<<29
 EXCEPTION_ENTRY prefetchAbortHandler, 1<<29
 EXCEPTION_ENTRY dataAbortHandler, 2<<29
 BEGIN_ASM_FUNC exceptionHandler
-	sub sp, #68
+	sub sp, #84
 	stmia sp, {r0-r14}^            @ Save all user/system mode regs except pc
 	mrs r2, spsr                   @ Get saved cpsr
 	mrs r3, cpsr
@@ -51,13 +51,21 @@ BEGIN_ASM_FUNC exceptionHandler
 exceptionHandler_skip_other_mode:
 	str lr, [sp, #60]              @ Save lr (pc) on exception stack
 	str r2, [sp, #64]              @ Save spsr (cpsr) on exception stack
+	mrc p15, 0, r3, c5, c0, 0
+	str r3, [sp, #68]              @ DFSR
+	mrc p15, 0, r3, c5, c0, 1
+	str r3, [sp, #72]              @ IFSR
+	mrc p15, 0, r3, c6, c0, 0
+	str r3, [sp, #76]              @ FAR
+	mrc p15, 0, r3, c6, c0, 1
+	str r3, [sp, #80]              @ WFAR
 	mov r4, r0
 	mov r5, sp
 	bl deinitCpu
 	mov r0, r4
 	mov sp, r5
 	mov r1, r5
-	b guruMeditation               @ r0 = exception type, r1 = reg dump ptr {r0-r14, pc (unmodified), cpsr}
+	b guruMeditation               @ r0 = exception type, r1 = reg dump ptr {r0-r14, pc (unmodified), CPSR, DFSR, IFSR, FAR, WFAR}
 END_ASM_FUNC
 
 
diff --git a/source/arm11/hardware/gfx.c b/source/arm11/hardware/gfx.c
index b5c46a1..606d1da 100644
--- a/source/arm11/hardware/gfx.c
+++ b/source/arm11/hardware/gfx.c
@@ -57,7 +57,6 @@ static u8 fmt2PixSize(GfxFbFmt fmt);
 static void setupFramebufs(GfxFbFmt fmtTop, GfxFbFmt fmtBot);
 static void deallocFramebufs(void);
 static void setupDislayController(u8 lcd);
-static void gfxIrqHandler(u32 intSource);
 
 void GFX_init(GfxFbFmt fmtTop, GfxFbFmt fmtBot)
 {
diff --git a/source/arm11/main.c b/source/arm11/main.c
index 20fa25c..3ac8e49 100644
--- a/source/arm11/main.c
+++ b/source/arm11/main.c
@@ -54,7 +54,7 @@ static u32 padRomArea(u32 romFileSize)
 	if(romSize < 0x100000u) romSize = 0x100000u;
 	memset((void*)(ROM_LOC + romFileSize), 0xFFFFFFFFu, romSize - romFileSize);
 
-	if((*(u32*)(ROM_LOC + 0xAC) & 0xFFu) != 'F')
+	if(romSize > 0x100000u)
 	{
 		// Fake "open bus" padding.
 		u32 padding = (ROM_LOC + romSize) / 2;
@@ -62,13 +62,13 @@ static u32 padRomArea(u32 romFileSize)
 		for(uintptr_t i = ROM_LOC + romSize; i < ROM_LOC + MAX_ROM_SIZE; i += 4)
 		{
 			*(u32*)i = padding;
-			padding = __uadd16(padding, 0x00020002u); // 0xBA028B
+			padding = __uadd16(padding, 0x00020002u); // Unsigned parallel halfword-wise addition.
 		}
 	}
 	else
 	{
-		// Classic NES Series ROM mirroring.
-		// Mirror ROM area across the entire 32 MiB range.
+		// ROM mirroring (Classic NES Series/possibly others with 8 Mbit ROM).
+		// Mirror ROM across the entire 32 MiB area.
 		for(uintptr_t i = ROM_LOC + romSize; i < ROM_LOC + MAX_ROM_SIZE; i += romSize)
 		{
 			//memcpy((void*)i, (void*)(i - romSize), romSize); // 0x23A15DD
@@ -398,39 +398,30 @@ static void adjustGammaTableForGba(void)
 	}
 }
 
-static void dumpFrameTex(void)
+static Result dumpFrameTex(void)
 {
 	// 512x-512 (hight negative to flip vertically).
-	alignas(4) static const u8 bmpHeader[122] =
+	// Pixels at offset 0x40.
+	alignas(4) static const u8 bmpHeader[54] =
 	{
-		0x42, 0x4D, 0x7A, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7A, 0x00,
-		0x00, 0x00, 0x6C, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0xFE,
-		0xFF, 0xFF, 0x01, 0x00, 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		0x0C, 0x00, 0x13, 0x0B, 0x00, 0x00, 0x13, 0x0B, 0x00, 0x00, 0x00, 0x00,
-		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x47, 0x52, 0x73, 0x00, 0x00,
-		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00,
-		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		0x00, 0x00
+		0x42, 0x4D, 0x40, 0x00, 0x0C, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x28, 0x00,
+		0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0xFE,
+		0xFF, 0xFF, 0x01, 0x00, 0x18, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x13, 0x0B,
+		0x00, 0x00, 0x13, 0x0B, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00
 	};
 
 	/*GX_displayTransfer((u32*)0x18200000, 160u<<16 | 256u, (u32*)0x18400000, 160u<<16 | 256u, 1u<<12 | 1u<<8);
 	GFX_waitForPPF();
 	//fsQuickWrite("sdmc:/lgyfb_dbg_frame.bgr", (void*)0x18400000, 256 * 160 * 3);*/
-	GX_displayTransfer((u32*)0x18200000, 240u<<16 | 512u, (u32*)0x18400000, 240u<<16 | 512u, 1u<<12 | 1u<<8);
+	GX_displayTransfer((u32*)0x18200000, 240u<<16 | 512u, (u32*)0x18400040, 240u<<16 | 512u, 1u<<12 | 1u<<8);
 	GFX_waitForPPF();
 
-	FHandle f;
-	if(fOpen(&f, "sdmc:/texture_dump.bmp", FA_CREATE_ALWAYS | FA_WRITE) == RES_OK)
-	{
-		fLseek(f, 0x0C007A);
-		fLseek(f, 0);
-		fWrite(f, bmpHeader, sizeof(bmpHeader), NULL);
-		fWrite(f, (void*)0x18400000, 512 * 512 * 3, NULL);
-		fClose(f);
-	}
+	memcpy((void*)0x18400000, bmpHeader, sizeof(bmpHeader));
+
+	return fsQuickWrite("sdmc:/texture_dump.bmp", (void*)0x18400000, 0x40 + 512 * 512 * 3);
 }
 
 static void gbaGfxHandler(void *args)
diff --git a/source/arm9/arm7_stub.s b/source/arm9/arm7_stub.s
index 3fd28e5..e9fe786 100644
--- a/source/arm9/arm7_stub.s
+++ b/source/arm9/arm7_stub.s
@@ -6,48 +6,55 @@
 
 
 
+BEGIN_ASM_FUNC _a7_overlay_stub
+	mov  r0, #1
+	mov  r1, #0x4000000
+	strb r0, [r1, #0x300]  @ "POSTFLG"
+	ldr  pc, =0x3007E00
+
+.pool
+.global _a7_overlay_stub_size
+_a7_overlay_stub_size = . - _a7_overlay_stub
+END_ASM_FUNC
+
 @ Must be located at 0x3007E00.
-BEGIN_ASM_FUNC _arm7_stub_start
-	mov r0, #PSR_INT_OFF | PSR_SVC_MODE
-	adr r1, _arm7_stub_start + 0x200  @ 0x3008000
-	msr CPSR_cxsf, r0
-	@mov r0, #PSR_INT_OFF | PSR_IRQ_MODE
-	mov sp, r1
-	@msr CPSR_cxsf, r0
-	mov r0, #PSR_INT_OFF | PSR_SYS_MODE
-	@sub sp, r1, #0x60  @ 0x3007FA0
-	msr CPSR_cxsf, r0
+BEGIN_ASM_FUNC _a7_stub_start
+	adr r1, _a7_stub_thumb + 1  @ 0x3007E1D
+	msr CPSR_fsxc, #PSR_INT_OFF | PSR_SVC_MODE
+	add sp, r1, #0x6B  @ 0x3007E88
+	msr CPSR_fsxc, #PSR_INT_OFF | PSR_SYS_MODE
+	add sp, r1, #0x5B  @ 0x3007E78
 	mov r3, #0x4700000
-	adr r2, _arm7_stub_16 + 1
-	sub sp, r1, #0x80  @ 0x3007F80
-	bx  r2
+	bx  r1
 
 .thumb
-_arm7_stub_16:
+_a7_stub_thumb:
 	mov r0, #1
 	str r0, [r3]  @ Disable BIOS overlay.
 	@ The original ARM7 stub waits 256 cycles here (for the BIOS overlay disable?).
 	@ The original ARM7 stub waits 1677800 cycles (100 ms) here for LCD/LgyFb sync.
+	@ The original ARM7 stub waits for REG_VCOUNT = 160 here.
 
-	lsl  r3, r0, #26   @ 0x4000000
-wait_vcount_160_lp:
-	ldrb r0, [r3, #6]  @ REG_VCOUNT
-	cmp  r0, #160      @ Wait for REG_VCOUNT == 160.
-	bne  wait_vcount_160_lp
+	lsl  r4, r0, #26  @ 0x4000000 Needed for "function" call 0xBC below.
+	mov  r0, #0xFF    @ Clear WRAM, iWRAM, palette RAM, VRAM, OAM
+	                  @ + reset SIO, sound and all other registers.
 
-	mov  r4, r3     @ Needed for "function" call 0xBC below.
-	mov  r0, #0xFF  @ Clear WRAM, iWRAM, palette RAM, VRAM, OAM
-	                @ + reset SIO, sound and all other registers.
-
-.global _arm7_stub_swi
-_arm7_stub_swi = . - _arm7_stub_start + 0x80BFE00 @ Final ARM9 mem location.
+.global _a7_stub9_swi
+_a7_stub9_swi = . - _a7_stub_start + 0x80BFE00 @ Final ARM9 mem location.
 	swi  0x01       @ RegisterRamReset
+
 	mov  r0, #0xBC  @ SoftReset (0xB4) but skipping r2 & r4 loading.
 	mov  r2, #0
+
+	@ REG_VCOUNT should be 126 at ROM entry like after BIOS intro.
+_a7_stub_vcount_lp:
+	ldrb r1, [r4, #6]  @ REG_VCOUNT
+	cmp  r1, #126      @ Loop until REG_VCOUNT == 126.
+	bne  _a7_stub_vcount_lp
+
 	bx   r0
 
-.pool
 .align 2
-.global _arm7_stub_size
-_arm7_stub_size = . - _arm7_stub_start
+.global _a7_stub_size
+_a7_stub_size = . - _a7_stub_start
 END_ASM_FUNC
diff --git a/source/arm9/hardware/lgy.c b/source/arm9/hardware/lgy.c
index 181e80c..7ad86e2 100644
--- a/source/arm9/hardware/lgy.c
+++ b/source/arm9/hardware/lgy.c
@@ -31,28 +31,15 @@ static char g_savePath[512] = {0};
 
 
 
-#define STRINGIFY(s) #s
-#define STR(s) STRINGIFY(s)
-NAKED static void _overlay_stub(void)
-{
-	__asm__("mov r0, #0x4000000\n\t"
-	        "mov r1, #1\n\t"
-	        "strb r1, [r0, #0x300]\n\t"   // "POSTFLG"
-	        "ldr pc, _overlay_stub_jmp\n\t"
-	        "_overlay_stub_jmp: .4byte " STR(ARM7_STUB_LOC) "\n\t"
-	        "_overlay_stub_size = . - _overlay_stub\n\t" : : : );
-}
-extern const u32 _overlay_stub_size[];
-
 static void setupBiosOverlay(bool biosIntro)
 {
-	iomemcpy(REGs_LGY_A7_VECTOR, (u32*)_overlay_stub, (u32)_overlay_stub_size);
+	iomemcpy(REGs_LGY_A7_VECTOR, (u32*)_a7_overlay_stub, (u32)_a7_overlay_stub_size);
 	//static const u32 biosVectors[8] = {0xEA000018, 0xEA000004, 0xEA00004C, 0xEA000002,
 	//                                   0xEA000001, 0xEA000000, 0xEA000042, 0xE59FD1A0};
 	//iomemcpy(REGs_LGY_A7_VECTOR, biosVectors, 32);
 
-	NDMA_copy((u32*)ARM7_STUB_LOC9, _arm7_stub_start, (u32)_arm7_stub_size);
-	if(biosIntro) *((vu8*)_arm7_stub_swi) = 0x26; // Patch swi 0x01 (RegisterRamReset) to swi 0x26 (HardReset).
+	NDMA_copy((u32*)ARM7_STUB_LOC9, (u32*)_a7_stub_start, (u32)_a7_stub_size);
+	if(biosIntro) *((vu8*)_a7_stub9_swi) = 0x26; // Patch swi 0x01 (RegisterRamReset) to swi 0x26 (HardReset).
 }
 
 static u32 setupSaveType(u16 saveType)
diff --git a/source/arm9/ipc_handler.c b/source/arm9/ipc_handler.c
index 440abd9..c779f1a 100644
--- a/source/arm9/ipc_handler.c
+++ b/source/arm9/ipc_handler.c
@@ -108,6 +108,7 @@ u32 IPC_handleCmd(u8 cmdId, u32 inBufs, u32 outBufs, const u32 *const buf)
 
 		// Miscellaneous API.
 		case IPC_CMD_ID_MASK(IPC_CMD9_PREPARE_POWER):
+			fsDeinit();
 			break;
 		default:
 			panic();
diff --git a/source/fsutil.c b/source/fsutil.c
index eb68fec..7814159 100644
--- a/source/fsutil.c
+++ b/source/fsutil.c
@@ -43,12 +43,7 @@ Result fsQuickWrite(const char *const path, const void *const buf, u32 size)
 	FHandle f;
 	if((res = fOpen(&f, path, FA_OPEN_ALWAYS | FA_WRITE)) == RES_OK)
 	{
-		if((res = fLseek(f, size)) == RES_OK && fTell(f) == size)
-		{
-			fLseek(f, 0);
-			res = fWrite(f, buf, size, NULL);
-		}
-		else if(res == RES_OK) res = RES_DISK_FULL; // Seek pre-allocation fail.
+		res = fWrite(f, buf, size, NULL);
 
 		fClose(f);
 	}