From c61195bcc9bed2ba3da5e1836e33e7163c066408 Mon Sep 17 00:00:00 2001
From: chenrun1 <chenrun1@xiaomi.com>
Date: Fri, 23 Dec 2022 15:42:36 +0800
Subject: [PATCH] arch/armv7-a & armv7-r:Add invalidate icache behavior

Signed-off-by: chenrun1 <chenrun1@xiaomi.com>
---
 arch/arm/src/armv7-a/arm_cache.c     | 23 ++++++++++++++++++++++-
 arch/arm/src/armv7-a/cp15_cacheops.c | 20 +++++++++++++++++++-
 arch/arm/src/armv7-a/cp15_cacheops.h |  2 +-
 arch/arm/src/armv7-r/arm_cache.c     | 23 ++++++++++++++++++++++-
 arch/arm/src/armv7-r/cp15_cacheops.c | 20 +++++++++++++++++++-
 arch/arm/src/armv7-r/cp15_cacheops.h |  2 +-
 6 files changed, 84 insertions(+), 6 deletions(-)

diff --git a/arch/arm/src/armv7-a/arm_cache.c b/arch/arm/src/armv7-a/arm_cache.c
index 7b8479c78c..e5ae5ca323 100644
--- a/arch/arm/src/armv7-a/arm_cache.c
+++ b/arch/arm/src/armv7-a/arm_cache.c
@@ -53,7 +53,28 @@
 
 void up_invalidate_icache_all(void)
 {
-  cp15_invalidate_icache();
+  cp15_invalidate_icache_all();
+}
+
+/****************************************************************************
+ * Name: up_invalidate_icache
+ *
+ * Description:
+ *   Validate the specified range instruction cache as PoU,
+ *   and flush the branch target cache
+ *
+ * Input Parameters:
+ *   start - virtual start address of region
+ *   end   - virtual end address of region + 1
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void up_invalidate_icache(uintptr_t start, uintptr_t end)
+{
+  cp15_invalidate_icache(start, end);
 }
 
 /****************************************************************************
diff --git a/arch/arm/src/armv7-a/cp15_cacheops.c b/arch/arm/src/armv7-a/cp15_cacheops.c
index 1561e6734c..9a4de3947d 100644
--- a/arch/arm/src/armv7-a/cp15_cacheops.c
+++ b/arch/arm/src/armv7-a/cp15_cacheops.c
@@ -189,10 +189,28 @@ void cp15_dcache_op_level(uint32_t level, int op)
   ARM_ISB();
 }
 
+void cp15_invalidate_icache(uintptr_t start, uintptr_t end)
+{
+  uint32_t line;
+
+  line = cp15_cache_get_info(NULL, NULL);
+  start &= ~(line - 1);
+
+  ARM_DSB();
+
+  while (start < end)
+    {
+      cp15_invalidate_icache_bymva(start);
+      start += line;
+    }
+
+  ARM_ISB();
+}
+
 void cp15_coherent_dcache(uintptr_t start, uintptr_t end)
 {
   cp15_dcache_op_mva(start, end, CP15_CACHE_CLEANINVALIDATE);
-  cp15_invalidate_icache();
+  cp15_invalidate_icache_all();
 }
 
 void cp15_invalidate_dcache(uintptr_t start, uintptr_t end)
diff --git a/arch/arm/src/armv7-a/cp15_cacheops.h b/arch/arm/src/armv7-a/cp15_cacheops.h
index 23bd0c4642..2ef9279fee 100644
--- a/arch/arm/src/armv7-a/cp15_cacheops.h
+++ b/arch/arm/src/armv7-a/cp15_cacheops.h
@@ -681,7 +681,7 @@ static inline void cp15_invalidate_btb_inner_sharable(void)
  *
  ****************************************************************************/
 
-static inline void cp15_invalidate_icache(void)
+static inline void cp15_invalidate_icache_all(void)
 {
   CP15_SET(ICIALLU, 0);
   ARM_ISB();
diff --git a/arch/arm/src/armv7-r/arm_cache.c b/arch/arm/src/armv7-r/arm_cache.c
index ef35e50306..91e5a241c1 100644
--- a/arch/arm/src/armv7-r/arm_cache.c
+++ b/arch/arm/src/armv7-r/arm_cache.c
@@ -53,7 +53,28 @@
 
 void up_invalidate_icache_all(void)
 {
-  cp15_invalidate_icache();
+  cp15_invalidate_icache_all();
+}
+
+/****************************************************************************
+ * Name: up_invalidate_icache
+ *
+ * Description:
+ *   Validate the specified range instruction cache as PoU,
+ *   and flush the branch target cache
+ *
+ * Input Parameters:
+ *   start - virtual start address of region
+ *   end   - virtual end address of region + 1
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void up_invalidate_icache(uintptr_t start, uintptr_t end)
+{
+  cp15_invalidate_icache(start, end);
 }
 
 /****************************************************************************
diff --git a/arch/arm/src/armv7-r/cp15_cacheops.c b/arch/arm/src/armv7-r/cp15_cacheops.c
index f6e87d49ba..955bf7a16e 100644
--- a/arch/arm/src/armv7-r/cp15_cacheops.c
+++ b/arch/arm/src/armv7-r/cp15_cacheops.c
@@ -189,10 +189,28 @@ void cp15_dcache_op_level(uint32_t level, int op)
   ARM_ISB();
 }
 
+void cp15_invalidate_icache(uintptr_t start, uintptr_t end)
+{
+  uint32_t line;
+
+  line = cp15_cache_get_info(NULL, NULL);
+  start &= ~(line - 1);
+
+  ARM_DSB();
+
+  while (start < end)
+    {
+      cp15_invalidate_icache_bymva(start);
+      start += line;
+    }
+
+  ARM_ISB();
+}
+
 void cp15_coherent_dcache(uintptr_t start, uintptr_t end)
 {
   cp15_dcache_op_mva(start, end, CP15_CACHE_CLEANINVALIDATE);
-  cp15_invalidate_icache();
+  cp15_invalidate_icache_all();
 }
 
 void cp15_invalidate_dcache(uintptr_t start, uintptr_t end)
diff --git a/arch/arm/src/armv7-r/cp15_cacheops.h b/arch/arm/src/armv7-r/cp15_cacheops.h
index 6053675dbb..4130111803 100644
--- a/arch/arm/src/armv7-r/cp15_cacheops.h
+++ b/arch/arm/src/armv7-r/cp15_cacheops.h
@@ -688,7 +688,7 @@ static inline void cp15_invalidate_btb_inner_sharable(void)
  *
  ****************************************************************************/
 
-static inline void cp15_invalidate_icache(void)
+static inline void cp15_invalidate_icache_all(void)
 {
   CP15_SET(ICIALLU, 0);
   ARM_ISB();