From 194a2efdf4b65531c3149f5a3bb0154b35514abd Mon Sep 17 00:00:00 2001
From: wangqinglin <53550140+HelloByeAll@users.noreply.github.com>
Date: Sun, 11 Jun 2023 07:06:16 +0800
Subject: [PATCH] =?UTF-8?q?AMP-step3:=20=E6=B7=BB=E5=8A=A0conifg=20ARCH=5F?=
 =?UTF-8?q?ARM=5FCORTEX=5FA55=EF=BC=8C=20rt=5Fhw=5Fcpu=5Fid=5Fset=20?=
 =?UTF-8?q?=E9=80=82=E9=85=8DA55=E6=9E=B6=E6=9E=84,=20=E6=B7=BB=E5=8A=A0aa?=
 =?UTF-8?q?rch64=20rt=5F=E2=80=A6=20(#7628)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 libcpu/Kconfig                      |  4 ++++
 libcpu/aarch64/common/context_gcc.S |  3 +++
 libcpu/aarch64/common/cpu.c         | 16 ++++++++++++++++
 3 files changed, 23 insertions(+)

diff --git a/libcpu/Kconfig b/libcpu/Kconfig
index 164520c4d4..c50571eda2 100644
--- a/libcpu/Kconfig
+++ b/libcpu/Kconfig
@@ -152,6 +152,10 @@ config ARCH_ARM_CORTEX_A9
     bool
     select ARCH_ARM_CORTEX_A
 
+config ARCH_ARM_CORTEX_A55
+    bool
+    select ARCH_ARM_CORTEX_A
+
 config ARCH_ARM_SECURE_MODE
     bool "Running in secure mode [ARM Cortex-A]"
     default n
diff --git a/libcpu/aarch64/common/context_gcc.S b/libcpu/aarch64/common/context_gcc.S
index 11e54bfa7b..dadd5cffc1 100644
--- a/libcpu/aarch64/common/context_gcc.S
+++ b/libcpu/aarch64/common/context_gcc.S
@@ -18,6 +18,9 @@
 .type rt_hw_cpu_id_set, @function
 rt_hw_cpu_id_set:
     mrs x0, mpidr_el1           /* MPIDR_EL1: Multi-Processor Affinity Register */
+#ifdef ARCH_ARM_CORTEX_A55
+    lsr x0, x0, #8
+#endif
     and x0, x0, #15
     msr tpidr_el1, x0
     ret
diff --git a/libcpu/aarch64/common/cpu.c b/libcpu/aarch64/common/cpu.c
index 8f7f2843a9..8953b64556 100644
--- a/libcpu/aarch64/common/cpu.c
+++ b/libcpu/aarch64/common/cpu.c
@@ -374,4 +374,20 @@ rt_weak void rt_hw_cpu_shutdown()
 }
 MSH_CMD_EXPORT_ALIAS(rt_hw_cpu_shutdown, shutdown, shutdown machine);
 
+#ifdef RT_USING_CPU_FFS
+/**
+ * This function finds the first bit set (beginning with the least significant bit)
+ * in value and return the index of that bit.
+ *
+ * Bits are numbered starting at 1 (the least significant bit).  A return value of
+ * zero from any of these functions means that the argument was zero.
+ *
+ * @return return the index of the first bit set. If value is 0, then this function
+ * shall return 0.
+ */
+int __rt_ffs(int value)
+{
+    return __builtin_ffs(value);
+}
+#endif
 /*@}*/