bsp/mpc55xx: Fix BSS initialization

2 files changed, 28 insertions, 5 deletions

diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/bspstart.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/bspstart.c
index d16f411fad..a9a0d70f73 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/bspstart.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/bspstart.c
@@ -80,11 +80,6 @@ void bsp_start(void)
 	null_pointer_protection();
 
 	/*
-	 * make sure BSS/SBSS is cleared
-	 */
-	memset(&bsp_section_bss_begin [0], 0, (size_t) bsp_section_bss_size);
-
-	/*
 	 * Get CPU identification dynamically. Note that the get_ppc_cpu_type()
 	 * function store the result in global variables so that it can be used
 	 * latter...
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c
index 11e35ec8c6..e1a0b3da09 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c
@@ -170,6 +170,33 @@ static BSP_START_TEXT_SECTION void mpc55xx_start_ebi(void)
   #endif
 }
 
+#ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+static BSP_START_TEXT_SECTION bool
+mpc55xx_start_is_in_internal_ram(const void *addr)
+{
+  return (size_t) addr - (size_t) bsp_ram_start < (size_t) bsp_ram_size;
+}
+#endif
+
+static BSP_START_TEXT_SECTION void mpc55xx_start_clear_bss(void)
+{
+  #ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+    if (!mpc55xx_start_is_in_internal_ram(bsp_section_sbss_begin)) {
+      bsp_start_zero(
+        bsp_section_sbss_begin,
+        (size_t) bsp_section_sbss_size
+      );
+    }
+
+    if (!mpc55xx_start_is_in_internal_ram(bsp_section_bss_begin)) {
+      bsp_start_zero(
+        bsp_section_bss_begin,
+        (size_t) bsp_section_bss_size
+      );
+    }
+  #endif
+}
+
 BSP_START_TEXT_SECTION void mpc55xx_start_early(void)
 {
   mpc55xx_start_watchdog();
@@ -185,4 +212,5 @@ BSP_START_TEXT_SECTION void mpc55xx_start_early(void)
   mpc55xx_start_siu();
   mpc55xx_start_ebi_chip_select();
   mpc55xx_start_ebi();
+  mpc55xx_start_clear_bss();
 }