diff --git a/irq_asm.S b/irq_asm.S
index 7825a48..a168be6 100644
--- a/irq_asm.S
+++ b/irq_asm.S
@@ -11,16 +11,13 @@
 irq_setup_stack:
 	@ Switch to IRQ mode
 	mrs	r0, cpsr
-	bic	r1, r0, #0x1f
-	orr	r1, #0x12
-	msr cpsr, r1
+	msr cpsr_c, #0xd2
 	
 	@ Setup interrupt stack
-	ldr	sp, =__stack_addr
-	add	sp, r4
+	ldr	sp, =__irqstack_addr
 
 	@ Restore mode
-	msr cpsr, r0
+	msr cpsr_c, r0
 	bx	lr
 
 v_irq:
@@ -35,12 +32,12 @@ irq_kill:
 	mrs		r1, cpsr
 	and		r0, r1, #(CPSR_IRQDIS|CPSR_FIQDIS)
 	orr		r1, r1, #(CPSR_IRQDIS|CPSR_FIQDIS)
-	msr		cpsr, r1
+	msr		cpsr_c, r1
 	bx		lr
 
 irq_restore:
 	mrs		r1, cpsr
 	bic		r1, r1, #(CPSR_IRQDIS|CPSR_FIQDIS)
 	orr		r1, r1, r0
-	msr		cpsr, r1
+	msr		cpsr_c, r1
 	bx		lr
diff --git a/start.S b/start.S
index 5a91d89..db033cd 100644
--- a/start.S
+++ b/start.S
@@ -29,6 +29,9 @@ _start:
 .pool
 
 v_reset:
+	@ Switch to System mode
+	msr cpsr_c, #0xdf
+
 	@ Get loader base from ELF loader
 	mov	r4, r0
 
@@ -38,13 +41,10 @@ v_reset:
 	
 	@ Set up a stack
 	ldr	sp, =__stack_addr
-	add	sp, r4
 
 	@ clear BSS
 	ldr	r1, =__bss_start
-	add	r1, r4
 	ldr	r2, =__bss_end
-	add	r2, r4
 	mov	r3, #0
 bss_loop:
 	@ check for the end
@@ -58,9 +58,7 @@ bss_loop:
 done_bss:
 	@ clear BSS2
 	ldr	r1, =__bss2_start
-	add	r1, r4
 	ldr	r2, =__bss2_end
-	add	r2, r4
 	mov	r3, #0
 bss2_loop:
 	@ check for the end