[PATCH v2 22/32] bsp/leon3: Simplify fatal error handling

[Sebastian Huber]

---
 bsps/sparc/leon3/include/bsp/leon3.h | 12 ++++
 bsps/sparc/leon3/start/bspclean.c    | 89 +++++++++++++++++++---------
 bsps/sparc/leon3/start/bspsmp.c      | 16 +----
 3 files changed, 77 insertions(+), 40 deletions(-)

diff --git a/bsps/sparc/leon3/include/bsp/leon3.h b/bsps/sparc/leon3/include/bsp/leon3.h
index 1394dd1c1c..476ed73647 100644
--- a/bsps/sparc/leon3/include/bsp/leon3.h
+++ b/bsps/sparc/leon3/include/bsp/leon3.h
@@ -161,6 +161,18 @@ static inline uint32_t leon3_get_data_cache_config_register( void )
   return leon3_get_system_register( 0xc );
 }
 
+/**
+ * @brief Gets the processor count.
+ *
+ * @param[in] regs is the IRQ(A)MP register block address.
+ *
+ * @return Returns the processor count.
+ */
+static inline uint32_t leon3_get_cpu_count( const irqamp *regs )
+{
+  return IRQAMP_MPSTAT_NCPU_GET( grlib_load_32( &regs->mpstat ) ) + 1;
+}
+
 /**
  * @brief This constant defines the index of the GPTIMER timer used by the
  *   clock driver.
diff --git a/bsps/sparc/leon3/start/bspclean.c b/bsps/sparc/leon3/start/bspclean.c
index acb2d6093c..4c9b385b43 100644
--- a/bsps/sparc/leon3/start/bspclean.c
+++ b/bsps/sparc/leon3/start/bspclean.c
@@ -35,14 +35,62 @@
 #include <bsp.h>
 #include <bsp/bootcard.h>
 #include <bsp/leon3.h>
+#include <rtems/score/cpuimpl.h>
 #include <rtems/score/smpimpl.h>
 
+#if defined(RTEMS_SMP)
+static void leon3_wait_for_power_down(irqamp *regs)
+{
+  uint32_t max_wait;
+  uint32_t cpu_self;
+  uint32_t cpu_count;
+  uint32_t halt_mask;
+  uint32_t i;
+
+  cpu_count = leon3_get_cpu_count(regs);
+
+  if (cpu_count > rtems_configuration_get_maximum_processors()) {
+    cpu_count = rtems_configuration_get_maximum_processors();
+  }
+
+  cpu_self = rtems_scheduler_get_processor();
+  halt_mask = 0;
+
+  for (i = 0; i < cpu_count; ++i) {
+    if (i != cpu_self && _SMP_Should_start_processor(i)) {
+      halt_mask |= UINT32_C(1) << i;
+    }
+  }
+
+  /*
+   * Wait some time for secondary processors to halt.
+   *
+   * The value was chosen to get something in the magnitude of 1ms on a 200MHz
+   * processor.
+   */
+
+  max_wait = 1234567;
+  i = 0;
+
+  while (
+    (grlib_load_32(&regs->mpstat) & halt_mask) != halt_mask && i < max_wait
+  ) {
+    ++i;
+  }
+}
+#endif
+
 void bsp_fatal_extension(
   rtems_fatal_source source,
   bool always_set_to_false,
   rtems_fatal_code code
 )
 {
+  rtems_interrupt_level level;
+
+  rtems_interrupt_local_disable(level);
+  (void) level;
+
 #if defined(RTEMS_SMP)
   /*
    * On SMP we must wait for all other CPUs not requesting a fatal halt, they
@@ -54,34 +102,18 @@ void bsp_fatal_extension(
       (code == SMP_FATAL_SHUTDOWN_RESPONSE)) {
     leon3_power_down_loop(); /* CPU didn't start shutdown sequence .. */
   } else {
-    irqamp *regs = LEON3_IrqCtrl_Regs;
+    irqamp *regs;
+
+    _SMP_Request_shutdown();
 
+    regs = LEON3_IrqCtrl_Regs;
+#if defined(LEON3_IRQAMP_BASE)
+    leon3_wait_for_power_down(regs);
+#else
     if (regs != NULL) {
-      /*
-       * Value was chosen to get something in the magnitude of 1ms on a 200MHz
-       * processor.
-       */
-      uint32_t max_wait = 1234567;
-      uint32_t self_cpu = rtems_scheduler_get_processor();
-      uint32_t cpu_count = rtems_scheduler_get_processor_maximum();
-      uint32_t halt_mask = 0;
-      uint32_t i;
-
-      for (i = 0; i < cpu_count; ++i) {
-        if ( (i != self_cpu) && _SMP_Should_start_processor( i ) ) {
-          halt_mask |= UINT32_C(1) << i;
-        }
-      }
-
-      /* Wait some time for secondary processors to halt */
-      i = 0;
-      while (
-        (grlib_load_32(&regs->mpstat) & halt_mask) != halt_mask &&
-        i < max_wait
-      ) {
-        ++i;
-      }
+      leon3_wait_for_power_down(regs);
     }
+#endif
   }
 #endif
 
@@ -92,7 +124,10 @@ void bsp_fatal_extension(
 #endif
 
 #if BSP_RESET_BOARD_AT_EXIT
-  /* If user wants to implement custom reset/reboot it can be done here */
-  bsp_reset();
+  /*
+   * Stop the system termination right now.  This skips the dynamically
+   * installed fatal error extensions and the generics shutdown procedure.
+   */
+  _CPU_Fatal_halt( source, code );
 #endif
 }
diff --git a/bsps/sparc/leon3/start/bspsmp.c b/bsps/sparc/leon3/start/bspsmp.c
index 7f8496289a..dc4065450a 100644
--- a/bsps/sparc/leon3/start/bspsmp.c
+++ b/bsps/sparc/leon3/start/bspsmp.c
@@ -39,14 +39,9 @@ static void bsp_inter_processor_interrupt( void *arg )
 
 void bsp_start_on_secondary_processor(Per_CPU_Control *cpu_self)
 {
-  /*
-   * If data cache snooping is not enabled we terminate using BSP_fatal_exit()
-   * instead of bsp_fatal().  This is done since the latter function tries to
-   * acquire a ticket lock, an operation which requires data cache snooping to
-   * be enabled.
-   */
-  if ( !leon3_data_cache_snooping_enabled() )
-    BSP_fatal_exit( LEON3_FATAL_INVALID_CACHE_CONFIG_SECONDARY_PROCESSOR );
+  if ( !leon3_data_cache_snooping_enabled() ) {
+    bsp_fatal( LEON3_FATAL_INVALID_CACHE_CONFIG_SECONDARY_PROCESSOR );
+  }
 
   _SMP_Start_multitasking_on_secondary_processor(cpu_self);
 }
@@ -75,11 +70,6 @@ static void leon3_install_inter_processor_interrupt( void )
   _Assert_Unused_variable_equals( sc, RTEMS_SUCCESSFUL );
 }
 
-static uint32_t leon3_get_cpu_count( const irqamp *regs )
-{
-  return IRQAMP_MPSTAT_NCPU_GET( grlib_load_32( &regs->mpstat ) ) + 1;
-}
-
 uint32_t _CPU_SMP_Initialize( void )
 {
   if ( !leon3_data_cache_snooping_enabled() )
-- 
2.35.3