diff --git a/source/arm9/firm.c b/source/arm9/firm.c
index 8a33384..04c8198 100644
--- a/source/arm9/firm.c
+++ b/source/arm9/firm.c
@@ -5,6 +5,7 @@
 #include "arm9/firm.h"
 #include "mem_map.h"
 #include "arm9/crypto.h"
+#include "cache.h"
 #include "arm9/ndma.h"
 #include "arm9/mpu.h"
 
@@ -85,6 +86,7 @@
 	while(*((vu32*)CORE_SYNC_ID) != 0x4F4B4F4B);
 	
 	//printf("Relocating FIRM launch stub...\n");
+	flushDCacheRange((void*)A9_STUB_ENTRY, 0x200);
 	NDMA_copy((u32*)A9_STUB_ENTRY, (u32*)firmLaunchStub, 0x200>>2);
 
 	//printf("Starting firm launch...\n");
diff --git a/source/arm9/ndma.c b/source/arm9/ndma.c
index 405d084..bfed977 100644
--- a/source/arm9/ndma.c
+++ b/source/arm9/ndma.c
@@ -4,16 +4,12 @@
  */
 
 #include "types.h"
-#include "cache.h"
 #include "arm9/ndma.h"
 
 
 
 void NDMA_copy(u32 *dest, const u32 *source, u32 num)
 {
-	flushDCacheRange(source, num<<2); // Make sure possible writes finished before DMAing.
-	flushDCacheRange(dest, num<<2);
-
 	REG_NDMA0_SRC_ADDR = (u32)source;
 	REG_NDMA0_DST_ADDR = (u32)dest;
 	REG_NDMA0_WRITE_CNT = num;
@@ -21,14 +17,10 @@
 	REG_NDMA0_CNT = NDMA_DST_UPDATE_INC | NDMA_SRC_UPDATE_INC | NDMA_IMMEDIATE_MODE | NDMA_ENABLE;
 
 	while(REG_NDMA0_CNT & NDMA_ENABLE);
-
-	invalidateDCacheRange(dest, num<<2); // Make sure no old data is in cache.
 }
 
 void NDMA_fill(u32 *dest, u32 value, u32 num)
 {
-	flushDCacheRange(dest, num<<2);
-
 	REG_NDMA0_DST_ADDR = (u32)dest;
 	REG_NDMA0_WRITE_CNT = num;
 	REG_NDMA0_BLOCK_CNT = NDMA_BLOCK_SYS_FREQ;
@@ -36,6 +28,4 @@
 	REG_NDMA0_CNT = NDMA_DST_UPDATE_INC | NDMA_SRC_UPDATE_FILL | NDMA_IMMEDIATE_MODE | NDMA_ENABLE;
 
 	while(REG_NDMA0_CNT & NDMA_ENABLE);
-
-	invalidateDCacheRange(dest, num<<2);
 }
diff --git a/source/arm9/start.s b/source/arm9/start.s
index 722e832..f92da0a 100644
--- a/source/arm9/start.s
+++ b/source/arm9/start.s
@@ -6,16 +6,17 @@
 .fpu softvfp
 
 .global _start
+.global clearMem
 .global _init
 .global firmLaunchEntry9
 .global firmLaunchEntry11
 
 .type _start STT_FUNC
+.type clearMem STT_FUNC
 .type _init STT_FUNC
 
 .extern setupSystem
 .extern __libc_init_array
-.extern heap_init
 .extern main
 .extern firm_launch
 .extern __bss_start__
@@ -40,19 +41,18 @@
 
 skip_pool:
 	msr cpsr_c, #0xDF           @ Disable all interrupts, system mode
-	ldr sp, =A9_STUB_ENTRY
-
 	bl setupSystem
 
+	@ Set sp and clear heap + stack
+	ldr sp, =A9_STUB_ENTRY
+	ldr r0, =A9_HEAP_START
+	mov r1, sp
+	bl clearMem
+
 	@ Clear bss section
 	ldr r0, =__bss_start__
 	ldr r1, =__bss_end__
-	sub r1, r1, r0
-	mov r2, #0
-	loop_bss_clear:
-		str r2, [r0], #4
-		subs r1, r1, #4
-		bne loop_bss_clear
+	bl clearMem
 
 	@ Setup newlib heap
 	ldr r0, =A9_HEAP_START
@@ -69,6 +69,17 @@
 	b .
 
 
+@ void clearMem(void *start, void *end)
+clearMem:
+	sub r1, r1, r0
+	mov r2, #0
+	loop_clear:
+		str r2, [r0], #4
+		subs r1, r1, #4
+		bne loop_clear
+	bx lr
+
+
 @ needed by libc
 _init:
 	bx lr