From 307c3d6fade53980dc43640c74046b1102264fbc Mon Sep 17 00:00:00 2001
From: Guangyao Ma <Guangyao.Ma@outlook.com>
Date: Fri, 12 Mar 2021 15:36:47 +0800
Subject: [PATCH] fix:start up optimized

Change-Id: Ia6907482000a06aada11feb4c52347e8f93e73b5
---
 arch/arm/arm/src/startup/reset_vector_mp.S | 18 ++++++++++++++++--
 arch/arm/arm/src/startup/reset_vector_up.S | 19 ++++++++++++++++---
 2 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/arch/arm/arm/src/startup/reset_vector_mp.S b/arch/arm/arm/src/startup/reset_vector_mp.S
index cec87786..209c2a56 100755
--- a/arch/arm/arm/src/startup/reset_vector_mp.S
+++ b/arch/arm/arm/src/startup/reset_vector_mp.S
@@ -65,7 +65,9 @@
 
     .equ MPIDR_CPUID_MASK, 0xffU
 
-    .fpu vfpv4
+    .fpu neon-vfpv4
+    .syntax unified
+    .arch armv7-a
     .arm
 
 /* param0 is stack bottom, param1 is stack size, r12 hold cpu id */
@@ -187,7 +189,7 @@ reloc_img_to_bottom_done:
     mov     r0, r4
     mov     r1, #0
     mov     r2, #MMU_DESCRIPTOR_L1_SMALL_ENTRY_NUMBERS
-    bl      memset
+    bl      memset_optimized                    /* optimized memset since r0 is 64-byte aligned */
 
     PAGE_TABLE_SET SYS_MEM_BASE, KERNEL_VMM_BASE, KERNEL_VMM_SIZE, MMU_DESCRIPTOR_KERNEL_L1_PTE_FLAGS
     PAGE_TABLE_SET SYS_MEM_BASE, UNCACHED_VMM_BASE, UNCACHED_VMM_SIZE, MMU_INITIAL_MAP_NORMAL_NOCACHE
@@ -463,6 +465,18 @@ _bootaddr_setup:
 
     bx      lr
 
+memset_optimized:
+    mov     r3, r0
+    vdup.8  q0, r1
+    vmov    q1, q0
+    vmov    q2, q0
+    vmov    q3, q0
+memset_optimized_loop:
+    subs    r2, #64
+    vstmia  r3!, {d0 - d7}
+    bge     memset_optimized_loop
+    bx      lr
+
 init_done:
     .long  0xDEADB00B
 
diff --git a/arch/arm/arm/src/startup/reset_vector_up.S b/arch/arm/arm/src/startup/reset_vector_up.S
index 63ac5764..521b7c88 100755
--- a/arch/arm/arm/src/startup/reset_vector_up.S
+++ b/arch/arm/arm/src/startup/reset_vector_up.S
@@ -65,7 +65,9 @@
 
     .equ MPIDR_CPUID_MASK, 0xffU
 
-    .fpu vfpv4
+    .fpu neon-vfpv4
+    .syntax unified
+    .arch armv7-a
     .arm
 
 /* param0 is stack bottom, param1 is stack size, r11 hold cpu id */
@@ -164,8 +166,7 @@ reloc_img_to_bottom_done:
     mov     r0, r4
     mov     r1, #0
     mov     r2, #MMU_DESCRIPTOR_L1_SMALL_ENTRY_NUMBERS
-    bl      memset
-
+    bl      memset_optimized                    /* optimized memset since r0 is 64-byte aligned */
 
     PAGE_TABLE_SET SYS_MEM_BASE, KERNEL_VMM_BASE, KERNEL_VMM_SIZE, MMU_DESCRIPTOR_KERNEL_L1_PTE_FLAGS
     PAGE_TABLE_SET SYS_MEM_BASE, UNCACHED_VMM_BASE, UNCACHED_VMM_SIZE, MMU_INITIAL_MAP_NORMAL_NOCACHE
@@ -431,6 +432,18 @@ _bootaddr_setup:
 
     bx      lr
 
+memset_optimized:
+    mov     r3, r0
+    vdup.8  q0, r1
+    vmov    q1, q0
+    vmov    q2, q0
+    vmov    q3, q0
+memset_optimized_loop:
+    subs    r2, #64
+    vstmia  r3!, {d0 - d7}
+    bge     memset_optimized_loop
+    bx      lr
+
 init_done:
     .long  0xDEADB00B