imx9/smp: Add boiler plate code to enable SMP with iMX9.X

This adds SMP support for iMX9.X CPUs

Signed-off-by: Ville Juven <ville.juven@unikie.com>

arch/arm64/include/imx9/chip.h

@@ -84,6 +84,7 @@
 
 .macro  get_cpu_id xreg0
   mrs    \xreg0, mpidr_el1
+  lsr    \xreg0, \xreg0, 8
   ubfx   \xreg0, \xreg0, #0, #8
 .endm
 

arch/arm64/include/imx9/imx93_irq.h

@@ -297,4 +297,8 @@
 
 #define NR_IRQS                         (301)
 
+/* Cores are at 100h offset from each other (affinity 1) */
+
+#define MPID_TO_CORE(mpid)              (((mpid) >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK)
+
 #endif /* __ARCH_ARM64_INCLUDE_IMX9_IMX93_IRQ_H */

arch/arm64/src/imx9/imx9_boot.c

@@ -87,6 +87,43 @@ const struct arm_mmu_config g_mmu_config =
  * Public Functions
  ****************************************************************************/
 
+#ifdef CONFIG_SMP
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: arm64_get_mpid
+ *
+ * Description:
+ *   The function from cpu index to get cpu mpid which is reading
+ * from mpidr_el1 register. Different ARM64 Core will use different
+ * Affn define, the mpidr_el1 value is not CPU number, So we need
+ * to change CPU number to mpid and vice versa
+ *
+ ****************************************************************************/
+
+uint64_t arm64_get_mpid(int cpu)
+{
+  return CORE_TO_MPID(cpu, 1);
+}
+
+/****************************************************************************
+ * Name: arm64_get_cpuid
+ *
+ * Description:
+ *   The function from mpid to get cpu id
+ *
+ ****************************************************************************/
+
+int arm64_get_cpuid(uint64_t mpid)
+{
+  return MPID_TO_CORE(mpid);
+}
+
+#endif /* CONFIG_SMP */
+
 /****************************************************************************
  * Name: arm64_el_init
  *