diff --git a/arch/arm64/include/imx9/chip.h b/arch/arm64/include/imx9/chip.h
index 9f005a12054..0f5419de9da 100644
--- a/arch/arm64/include/imx9/chip.h
+++ b/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
 
diff --git a/arch/arm64/include/imx9/imx93_irq.h b/arch/arm64/include/imx9/imx93_irq.h
index b703622e2c9..a08d267e484 100644
--- a/arch/arm64/include/imx9/imx93_irq.h
+++ b/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 */
diff --git a/arch/arm64/src/imx9/imx9_boot.c b/arch/arm64/src/imx9/imx9_boot.c
index 9e9865302b7..4e1c643dd5d 100644
--- a/arch/arm64/src/imx9/imx9_boot.c
+++ b/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
  *