diff --git a/src/startup/startup.s b/src/startup/startup.s
index eb43edd..8a07d0a 100755
--- a/src/startup/startup.s
+++ b/src/startup/startup.s
@@ -180,7 +180,7 @@ Reset_Handler:
 	@ Enable FPU (enable CP10/CP11, i.e. CPACR[23:20] = 1111)
 	LDR   R0, =0xE000ED88
 	LDR   R1, [R0]
-	ORR   R1, R1, #(0xF << 20)
+	ORR   R1, #(0xF << 20)
 	STR   R1, [R0]
 
 	@ FPU enabled: sync barrier, flush pipeline
@@ -189,34 +189,32 @@ Reset_Handler:
 #endif
 
 	@ Copy .data to RAM. Symbols defined by linker script:
-	@ Data_Start_ROM: start of .data section in ROM
-	@ Data_Start_RAM: start of .data section in RAM
-	@ Data_Size     : size of .data section
-	LDR   R0, =Data_Start_ROM
-	LDR   R1, =Data_Start_RAM
-	LDR   R2, =Data_Size
-	B     Data_Copy_Check
+	@ Data_Size      : size of .data section
+	@ Data_Start_ROM : start of .data section in ROM
+	@ Data_Start_RAM : start of .data section in RAM
+	LDR   R0, =Data_Size
+	CBZ   R0, Data_Copy_End
+	LDR   R1, =Data_Start_ROM
+	LDR   R2, =Data_Start_RAM
 Data_Copy_Loop:
-	LDMIA R0!, {R3}
-	STMIA R1!, {R3}
-	SUBS  R2, R2, #4
-Data_Copy_Check:
-	CMP   R2, #0
+	LDR   R3, [R1], #4
+	STR   R3, [R2], #4
+	SUBS  R0, #4
 	BNE   Data_Copy_Loop
+Data_Copy_End:
 
 	@ Zero out BSS. Symbols defined by linker script:
-	@ BSS_Start: start of .bss section in RAM
-	@ BSS_Size : size of .bss section
-	LDR   R0, =BSS_Start
+	@ BSS_Size  : size of .bss section
+	@ BSS_Start : start of .bss section in RAM
+	@ R0 is already zeroed from the .data copy.
 	LDR   R1, =BSS_Size
-	MOVS  R2, #0
-	B     BSS_Zero_Check
+	CBZ   R1, BSS_Zero_End
+	LDR   R2, =BSS_Start
 BSS_Zero_Loop:
-	STMIA R0!, {R2}
-	SUBS  R1, R1, #4
-BSS_Zero_Check:
-	CMP   R1, #0
+	STR   R0, [R2], #4
+	SUBS  R1, #4
 	BNE   BSS_Zero_Loop
+BSS_Zero_End:
 
 	@ Call Sys_Init
 	LDR   R0, =Sys_Init