[PATCH 3/6] arm/bsps: CP15 and basic cache support entire cache clean for more architecture variants now.

[pisa at cmp.felk.cvut.cz]

From: Pavel Pisa <pisa at cmp.felk.cvut.cz>

Next cache operations should work on most of cores now

  rtems_cache_flush_entire_data()
  rtems_cache_invalidate_entire_data()
  rtems_cache_invalidate_entire_instruction()

Instruction cache invalidate works on the first level for now only.
Data cacache operations are extended to ensure flush/invalidate
on all cache levels.

The CP15 arm_cp15_data_cache_clean_all_levels() function extended
to continue through unified levels too (ctype = 4).
---
 .../arm/shared/armv467ar-basic-cache/cache_.h      | 22 +++++++++--
 c/src/lib/libbsp/arm/shared/include/arm-cache-l1.h |  6 ++-
 c/src/lib/libcpu/arm/shared/include/arm-cp15.h     | 44 +++++++++++++++++++++-
 3 files changed, 66 insertions(+), 6 deletions(-)

diff --git a/c/src/lib/libbsp/arm/shared/armv467ar-basic-cache/cache_.h b/c/src/lib/libbsp/arm/shared/armv467ar-basic-cache/cache_.h
index c6e1f83..2ff1ae1 100644
--- a/c/src/lib/libbsp/arm/shared/armv467ar-basic-cache/cache_.h
+++ b/c/src/lib/libbsp/arm/shared/armv467ar-basic-cache/cache_.h
@@ -49,11 +49,14 @@ _CPU_cache_flush_data_range(
 )
 {
   _ARM_Data_synchronization_barrier();
-  arm_cp15_drain_write_buffer();
   arm_cache_l1_flush_data_range(
     d_addr,
     n_bytes
   );
+  #if !defined(__ARM_ARCH_7A__)
+  arm_cp15_drain_write_buffer();
+  #endif
+ _ARM_Data_synchronization_barrier();
 }
 
 static inline void _CPU_cache_invalidate_1_data_line(const void *d_addr)
@@ -92,6 +95,7 @@ static inline void
 _CPU_cache_invalidate_instruction_range( const void *i_addr, size_t n_bytes)
 {
   arm_cache_l1_invalidate_instruction_range( i_addr, n_bytes );
+  _ARM_Instruction_synchronization_barrier();
 }
 
 static inline void _CPU_cache_freeze_instruction(void)
@@ -106,12 +110,23 @@ static inline void _CPU_cache_unfreeze_instruction(void)
 
 static inline void _CPU_cache_flush_entire_data(void)
 {
-  arm_cp15_data_cache_test_and_clean();
+  _ARM_Data_synchronization_barrier();
+  #if defined(__ARM_ARCH_7A__)
+  arm_cp15_data_cache_clean_all_levels();
+  #else
+  arm_cp15_data_cache_clean_and_invalidate();
+  arm_cp15_drain_write_buffer();
+  #endif
+  _ARM_Data_synchronization_barrier();
 }
 
 static inline void _CPU_cache_invalidate_entire_data(void)
 {
+  #if defined(__ARM_ARCH_7A__)
+  arm_cp15_data_cache_invalidate_all_levels();
+  #else
   arm_cp15_data_cache_invalidate();
+  #endif
 }
 
 static inline void _CPU_cache_enable_data(void)
@@ -141,7 +156,8 @@ static inline void _CPU_cache_disable_data(void)
 
 static inline void _CPU_cache_invalidate_entire_instruction(void)
 {
-  arm_cp15_instruction_cache_invalidate();
+  arm_cache_l1_invalidate_entire_instruction();
+  _ARM_Instruction_synchronization_barrier();
 }
 
 static inline void _CPU_cache_enable_instruction(void)
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-cache-l1.h b/c/src/lib/libbsp/arm/shared/include/arm-cache-l1.h
index 74a65c5..9caa268 100644
--- a/c/src/lib/libbsp/arm/shared/include/arm-cache-l1.h
+++ b/c/src/lib/libbsp/arm/shared/include/arm-cache-l1.h
@@ -329,9 +329,13 @@ static inline void arm_cache_l1_invalidate_entire_instruction( void )
   arm_cp15_instruction_cache_invalidate();
   #endif /* RTEMS_SMP */
 
-  if ( ( ctrl & ARM_CP15_CTRL_Z ) == 0 ) {
+  if ( ( ctrl & ARM_CP15_CTRL_Z ) != 0 ) {
+    #if defined(__ARM_ARCH_7A__)
     arm_cp15_branch_predictor_inner_shareable_invalidate_all();
+    #endif
+    #if defined(__ARM_ARCH_6KZ__) || defined(__ARM_ARCH_7A__)
     arm_cp15_branch_predictor_invalidate_all();
+    #endif
   }
 }
 
diff --git a/c/src/lib/libcpu/arm/shared/include/arm-cp15.h b/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
index 52cca49..a27e50d 100644
--- a/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
+++ b/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
@@ -1071,8 +1071,8 @@ arm_cp15_data_cache_invalidate_all_levels(void)
   for (level = 0; level < loc; ++level) {
     uint32_t ctype = arm_clidr_get_cache_type(clidr, level);
 
-    /* Check if this level has a data cache */
-    if ((ctype & 0x2) != 0) {
+    /* Check if this level has a data cache or unified cache */
+    if (((ctype & (0x6)) == 2) || (ctype == 4)) {
       uint32_t ccsidr;
       uint32_t line_power;
       uint32_t associativity;
@@ -1134,6 +1134,46 @@ arm_cp15_data_cache_clean_line_by_set_and_way(uint32_t set_and_way)
 }
 
 ARM_CP15_TEXT_SECTION static inline void
+arm_cp15_data_cache_clean_all_levels(void)
+{
+  uint32_t clidr = arm_cp15_get_cache_level_id();
+  uint32_t loc = arm_clidr_get_level_of_coherency(clidr);
+  uint32_t level = 0;
+
+  for (level = 0; level < loc; ++level) {
+    uint32_t ctype = arm_clidr_get_cache_type(clidr, level);
+
+    /* Check if this level has a data cache or unified cache */
+    if (((ctype & (0x6)) == 2) || (ctype == 4)) {
+      uint32_t ccsidr;
+      uint32_t line_power;
+      uint32_t associativity;
+      uint32_t way;
+      uint32_t way_shift;
+
+      ccsidr = arm_cp15_get_cache_size_id_for_level(level << 1);
+
+      line_power = arm_ccsidr_get_line_power(ccsidr);
+      associativity = arm_ccsidr_get_associativity(ccsidr);
+      way_shift = __builtin_clz(associativity - 1);
+
+      for (way = 0; way < associativity; ++way) {
+        uint32_t num_sets = arm_ccsidr_get_num_sets(ccsidr);
+        uint32_t set;
+
+        for (set = 0; set < num_sets; ++set) {
+          uint32_t set_way = (way << way_shift)
+            | (set << line_power)
+            | (level << 1);
+
+          arm_cp15_data_cache_clean_line_by_set_and_way(set_way);
+        }
+      }
+    }
+  }
+}
+
+ARM_CP15_TEXT_SECTION static inline void
 arm_cp15_data_cache_test_and_clean(void)
 {
   ARM_SWITCH_REGISTERS;
-- 
1.9.1