diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index e0b187f002f..c44ebec8cff 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -84,6 +84,21 @@ config ARCH_HAVE_EL3
 		runing at EL3 is not necessary and system register for EL3
 		is not accessible
 
+config ARCH_SET_VMPIDR_EL2
+	bool "Set VMPIDR_EL2 at EL2 stage"
+	help
+		VMPIDR_EL2 holds the value of the Virtualization Multiprocessor ID.
+		From architecture manual of AArch64, the behave is:
+		  -reading register MPIDR_EL1 in EL2, it's return real MPIDR_EL1
+		  -reading register MPIDR_EL1 in EL1, it's return VMPIDR_EL2
+		So since NuttX for SMP is running at EL1 to read MPIDR_EL1 for
+		identify CPU id, it's need to set VMPIDR_EL2 to MPIDR_EL1 for
+		every CPU at boot EL2 stage.
+		For some platform, the bootloader or hypervisor will do that at
+		the EL2 stage, but not all.
+		ARM FVP and VDK should set it since these platform will boot
+		without BootLoader.
+
 config ARCH_EARLY_PRINT
 	bool "arch early print support"
 	default n
diff --git a/arch/arm64/src/common/arm64_boot.c b/arch/arm64/src/common/arm64_boot.c
index 1d4013733af..74ba8632edc 100644
--- a/arch/arm64/src/common/arm64_boot.c
+++ b/arch/arm64/src/common/arm64_boot.c
@@ -130,6 +130,11 @@ void arm64_boot_el2_init(void)
   zero_sysreg(cnthp_ctl_el2);
 #endif
 
+#ifdef CONFIG_ARCH_SET_VMPIDR_EL2
+  reg = read_sysreg(mpidr_el1);
+  write_sysreg(reg, vmpidr_el2);
+#endif
+
   /* Enable this if/when we use the hypervisor timer.
    * write_cnthp_cval_el2(~(uint64_t)0);
    */
diff --git a/arch/arm64/src/common/arm64_cpustart.c b/arch/arm64/src/common/arm64_cpustart.c
index 1eac831cf0a..808799c8b2a 100644
--- a/arch/arm64/src/common/arm64_cpustart.c
+++ b/arch/arm64/src/common/arm64_cpustart.c
@@ -149,7 +149,6 @@ static void arm64_start_cpu(int cpu_num, char *stack, int stack_sz,
 {
   uint64_t cpu_mpid = cpu_num;
 
-
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 
   /* Notify of the start event */