bsps/arm: Clear SCTLR[M, I, A, C] in start.S

Initialize the data and unified cache levels.  Invalidate the
instruction cache levels.

Update #4202.

5 files changed, 43 insertions, 119 deletions

diff --git a/bsps/arm/altera-cyclone-v/start/bspstarthooks.c b/bsps/arm/altera-cyclone-v/start/bspstarthooks.c
index d3ba05626b..d1d21350f0 100644
--- a/bsps/arm/altera-cyclone-v/start/bspstarthooks.c
+++ b/bsps/arm/altera-cyclone-v/start/bspstarthooks.c
@@ -35,8 +35,6 @@
 
 BSP_START_TEXT_SECTION void bsp_start_hook_0( void )
 {
-  arm_cp15_instruction_cache_invalidate();
-  arm_cp15_data_cache_invalidate_all_levels();
   arm_a9mpcore_start_hook_0();
 }
 
diff --git a/bsps/arm/raspberrypi/start/bspstarthooks.c b/bsps/arm/raspberrypi/start/bspstarthooks.c
index b050cb695e..60ff098be7 100644
--- a/bsps/arm/raspberrypi/start/bspstarthooks.c
+++ b/bsps/arm/raspberrypi/start/bspstarthooks.c
@@ -67,57 +67,14 @@ raspberrypi_mmu_config_table[] = {
 
 void BSP_START_TEXT_SECTION bsp_start_hook_0(void)
 {
-  uint32_t sctlr_val;
-#ifdef RTEMS_SMP
-  uint32_t cpu_index_self = _SMP_Get_current_processor();
-#endif /* RTEMS_SMP */
-
-  sctlr_val = arm_cp15_get_control();
-
-  /*
-   * Current U-boot loader seems to start kernel image
-   * with I and D caches on and MMU enabled.
-   * If RTEMS application image finds that cache is on
-   * during startup then disable caches.
-   */
-  if (sctlr_val & (ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M)) {
-    if (sctlr_val & (ARM_CP15_CTRL_C | ARM_CP15_CTRL_M)) {
-      /*
-       * If the data cache is on then ensure that it is clean
-       * before switching off to be extra carefull.
-       */
-#ifdef RTEMS_SMP
-      if (cpu_index_self != 0) {
-        arm_cp15_data_cache_clean_level(0);
-        arm_cp15_cache_invalidate_level(0, 0);
-      } else
-#endif /* RTEMS_SMP */
-      {
-        rtems_cache_flush_entire_data();
-        rtems_cache_invalidate_entire_data();
-      }
-    }
-    arm_cp15_flush_prefetch_buffer();
-    sctlr_val &= ~(ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M | ARM_CP15_CTRL_A);
-    arm_cp15_set_control(sctlr_val);
-  }
-#ifdef RTEMS_SMP
-  if (cpu_index_self != 0) {
-    arm_cp15_cache_invalidate_level(0, 0);
-  } else
-#endif /* RTEMS_SMP */
-  {
-    rtems_cache_invalidate_entire_data();
-  }
-  rtems_cache_invalidate_entire_instruction();
   arm_cp15_tlb_invalidate();
-  arm_cp15_flush_prefetch_buffer();
+  _ARM_Instruction_synchronization_barrier();
 
   /* Clear Translation Table Base Control Register */
   arm_cp15_set_translation_table_base_control_register(0);
 
 #ifdef RTEMS_SMP
-  if (cpu_index_self == 0) {
+  if (_SMP_Get_current_processor() == 0) {
     rpi_ipi_initialize();
   } else {
     rpi_start_rtems_on_secondary_processor();
diff --git a/bsps/arm/shared/start/start.S b/bsps/arm/shared/start/start.S
index 92b11ee45e..0a3d4288cd 100644
--- a/bsps/arm/shared/start/start.S
+++ b/bsps/arm/shared/start/start.S
@@ -31,6 +31,8 @@
 #include <bspopts.h>
 #include <bsp/irq.h>
 
+#include <dev/cache/arm-data-cache-loop-set-way.h>
+
 	/* Global symbols */
 	.globl	_start
 	.globl	bsp_start_hook_0_done
@@ -390,15 +392,46 @@ _start:
 #if (__ARM_ARCH >= 7 && __ARM_ARCH_PROFILE == 'A') || __ARM_ARCH >= 8
 	/*
 	 * Set VBAR to the vector table in the start section and make sure
-	 * SCTLR[V] is cleared.  Afterwards, exceptions are handled by RTEMS.
+	 * SCTLR[M, I, A, C, V] are cleared.  Afterwards, exceptions are
+	 * handled by RTEMS.
 	 */
 	ldr	r0, =bsp_start_vector_table_begin
 	dsb
 	mcr	p15, 0, r0, c12, c0, 0
 	mrc	p15, 0, r0, c1, c0, 0
-	bic	r1, r0, #0x2000
+	bic	r1, r0, #0x2800
+	bic	r1, r1, #0x7
 	mcr	p15, 0, r1, c1, c0, 0
 	isb
+
+	/* Check previous SCTLR[C] and initialize data caches */
+	tst	r0, #0x4
+	bne	.Lclean_invalidate_data_caches
+
+	/*
+	 * Invalidate the sets and ways of all data or unified cache levels
+	 * using DCISW (Data Cache line Invalidate by Set/Way).
+	 */
+	ARM_DATA_CACHE_LOOP_SET_WAY c6
+	b	.Ldata_caches_initialized
+
+.Lclean_invalidate_data_caches:
+
+	/*
+	 * Clean and invalidate the sets and ways of all data or unified cache
+	 * levels using DCCISW (Data Cache line Clean and Invalidate by
+	 * Set/Way).
+	 */
+	ARM_DATA_CACHE_LOOP_SET_WAY c14
+
+.Ldata_caches_initialized:
+
+	/*
+	 * Invalidate the instruction cache levels using ICIALLU (Instruction
+	 * Cache Invalidate All to PoU).
+	 */
+	mov	r0, #0
+	mcr	p15, 0, r0, c7, c5, 0
 #endif
 
 	/*
diff --git a/bsps/arm/xilinx-zynq/start/bspstarthooks.c b/bsps/arm/xilinx-zynq/start/bspstarthooks.c
index 0fc2a2590d..25cf96abdb 100644
--- a/bsps/arm/xilinx-zynq/start/bspstarthooks.c
+++ b/bsps/arm/xilinx-zynq/start/bspstarthooks.c
@@ -29,45 +29,14 @@
 
 #include <bsp.h>
 #include <bsp/start.h>
-#include <bsp/arm-cp15-start.h>
 #include <bsp/arm-a9mpcore-start.h>
 
+#include <libcpu/arm-cp15.h>
+
 BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
 {
-  uint32_t sctlr_val;
-
-  sctlr_val = arm_cp15_get_control();
-
-  /*
-   * Current U-boot loader seems to start kernel image
-   * with I and D caches on and MMU enabled.
-   * If RTEMS application image finds that cache is on
-   * during startup then disable caches.
-   */
-  if ( sctlr_val & (ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M ) ) {
-    if ( sctlr_val & (ARM_CP15_CTRL_C | ARM_CP15_CTRL_M ) ) {
-      /*
-       * If the data cache is on then ensure that it is clean
-       * before switching off to be extra carefull.
-       */
-      arm_cp15_data_cache_clean_all_levels();
-    }
-    arm_cp15_flush_prefetch_buffer();
-    sctlr_val &= ~ ( ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M | ARM_CP15_CTRL_A );
-    arm_cp15_set_control( sctlr_val );
-  }
-  arm_cp15_instruction_cache_invalidate();
-  /*
-   * The care should be taken there that no shared levels
-   * are invalidated by secondary CPUs in SMP case.
-   * It is not problem on Zynq because level of coherency
-   * is L1 only and higher level is not maintained and seen
-   * by CP15. So no special care to limit levels on the secondary
-   * are required there.
-   */
-  arm_cp15_data_cache_invalidate_all_levels();
   arm_cp15_tlb_invalidate();
-  arm_cp15_flush_prefetch_buffer();
+  _ARM_Instruction_synchronization_barrier()
   arm_a9mpcore_start_hook_0();
 }
 
diff --git a/bsps/arm/xilinx-zynqmp/start/bspstarthooks.c b/bsps/arm/xilinx-zynqmp/start/bspstarthooks.c
index bc3f0fbe5e..0e844bc105 100644
--- a/bsps/arm/xilinx-zynqmp/start/bspstarthooks.c
+++ b/bsps/arm/xilinx-zynqmp/start/bspstarthooks.c
@@ -34,46 +34,13 @@
 
 #include <bsp.h>
 #include <bsp/start.h>
-#include <bsp/arm-cp15-start.h>
+
+#include <libcpu/arm-cp15.h>
 
 BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
 {
-  uint32_t sctlr_val;
-
-  sctlr_val = arm_cp15_get_control();
-  sctlr_val |= ARM_CP15_CTRL_CP15BEN;
-  arm_cp15_set_control( sctlr_val );
-
-  /*
-   * Current U-boot loader seems to start kernel image
-   * with I and D caches on and MMU enabled.
-   * If RTEMS application image finds that cache is on
-   * during startup then disable caches.
-   */
-  if ( sctlr_val & (ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M ) ) {
-    if ( sctlr_val & (ARM_CP15_CTRL_C | ARM_CP15_CTRL_M ) ) {
-      /*
-       * If the data cache is on then ensure that it is clean
-       * before switching off to be extra carefull.
-       */
-      arm_cp15_data_cache_clean_all_levels();
-    }
-    arm_cp15_flush_prefetch_buffer();
-    sctlr_val &= ~ ( ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M | ARM_CP15_CTRL_A );
-    arm_cp15_set_control( sctlr_val );
-  }
-  arm_cp15_instruction_cache_invalidate();
-  /*
-   * The care should be taken there that no shared levels
-   * are invalidated by secondary CPUs in SMP case.
-   * It is not problem on Zynq because level of coherency
-   * is L1 only and higher level is not maintained and seen
-   * by CP15. So no special care to limit levels on the secondary
-   * are required there.
-   */
-  arm_cp15_data_cache_invalidate_all_levels();
   arm_cp15_tlb_invalidate();
-  arm_cp15_flush_prefetch_buffer();
+  _ARM_Instruction_synchronization_barrier();
 }
 
 BSP_START_TEXT_SECTION void bsp_start_hook_1(void)