From 28eeb6a86fb96a2673a84491758c0e1f3e98a902 Mon Sep 17 00:00:00 2001
From: Pavel Pisa <pisa@cmp.felk.cvut.cz>
Date: Sat, 3 Sep 2016 01:59:21 +0200
Subject: bsps/arm: reorganize CP15 code to allow clean and invalidate ARMv7
 cache by level.

New function arm_cp15_cache_invalidate_level and arm_cp15_cache_clean_level
can be used to maintain single cache level (instruction or data).

Updates #2782
Updates #2783
---
 c/src/lib/libcpu/arm/shared/include/arm-cp15.h | 108 ++++++++++++++-----------
 1 file changed, 60 insertions(+), 48 deletions(-)

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 4e4cac430d..7b27c1bb06 100644
--- a/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
+++ b/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
@@ -1082,6 +1082,35 @@ arm_cp15_data_cache_invalidate_line_by_set_and_way(uint32_t set_and_way)
   );
 }
 
+ARM_CP15_TEXT_SECTION static inline void
+arm_cp15_cache_invalidate_level(uint32_t level, uint32_t inst_data_fl)
+{
+  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) | inst_data_fl);
+
+  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_invalidate_line_by_set_and_way(set_way);
+    }
+  }
+}
+
 ARM_CP15_TEXT_SECTION static inline void
 arm_cp15_data_cache_invalidate_all_levels(void)
 {
@@ -1094,30 +1123,7 @@ arm_cp15_data_cache_invalidate_all_levels(void)
 
     /* 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_invalidate_line_by_set_and_way(set_way);
-        }
-      }
+      arm_cp15_cache_invalidate_level(level, 0);
     }
   }
 }
@@ -1154,6 +1160,35 @@ 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_level(uint32_t level)
+{
+  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_clean_all_levels(void)
 {
@@ -1166,30 +1201,7 @@ arm_cp15_data_cache_clean_all_levels(void)
 
     /* 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_data_cache_clean_level(level);
     }
   }
 }
-- 
cgit v1.2.3