arm64: ARMv8-r(Cortex-R82) support(add ARCH_SET_VMPIDR_EL2 option)

Summary

   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.

Signed-off-by: qinwei1 <qinwei1@xiaomi.com>

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

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);
    */

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 */