bsp/mpc55xx: Add MPC55XX_NEEDS_LOW_LEVEL_INIT

8 files changed, 169 insertions, 132 deletions

diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/configure.ac b/c/src/lib/libbsp/powerpc/mpc55xxevb/configure.ac
index cd7c066aa0..ccfa1050cf 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/configure.ac
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/configure.ac
@@ -139,6 +139,9 @@ RTEMS_BSPOPTS_HELP([RTEMS_BSP_I2C_EEPROM_DEVICE_NAME],[EEPROM name for LibI2C])
 RTEMS_BSPOPTS_SET([RTEMS_BSP_I2C_EEPROM_DEVICE_PATH],[gwlcfm],['"/dev/i2c1.eeprom"'])
 RTEMS_BSPOPTS_HELP([RTEMS_BSP_I2C_EEPROM_DEVICE_PATH],[EEPROM device file path])
 
+RTEMS_BSPOPTS_SET([MPC55XX_NEEDS_LOW_LEVEL_INIT],[*],[1])
+RTEMS_BSPOPTS_HELP([MPC55XX_NEEDS_LOW_LEVEL_INIT],[if defined, do low level initialization])
+
 AC_CONFIG_FILES([Makefile
 include/bspopts.h])
 
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/bspopts.h.in b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/bspopts.h.in
index 958b5d396c..124a22247d 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/include/bspopts.h.in
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/include/bspopts.h.in
@@ -73,6 +73,9 @@
 /* Must be defined to be the PLL predivider factor for clock generation */
 #undef MPC55XX_FMPLL_PREDIV
 
+/* if defined, do low level initialization */
+#undef MPC55XX_NEEDS_LOW_LEVEL_INIT
+
 /* Must be defined to be the external reference clock (in Hz) for clock
    generation */
 #undef MPC55XX_REFERENCE_CLOCK
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-cache.S b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-cache.S
index 0434fc2068..ae2bb00995 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-cache.S
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-cache.S
@@ -30,6 +30,8 @@
 
 mpc55xx_start_cache:
 
+#ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+
 	/* Load zero, CINV, and CABT) */
 	li	r0, 0
 	li	r3, 0x2
@@ -106,5 +108,7 @@ get_data_cache_invalidation_status:
 
 #endif
 
+#endif /* MPC55XX_NEEDS_LOW_LEVEL_INIT */
+
 	/* Return */
 	blr
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-clock.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-clock.c
index 6b8ff1eb77..90d392e4bd 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-clock.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-clock.c
@@ -25,75 +25,79 @@
 #include <bsp/bootcard.h>
 #include <bsp/mpc55xx-config.h>
 
-#if defined(MPC55XX_HAS_FMPLL) || defined(MPC55XX_HAS_FMPLL_ENHANCED)
-  static BSP_START_TEXT_SECTION void fmpll_wait_for_lock(void)
-  {
-    int i = 0;
-    bool lock = false;
-
-    while (!lock && i < 6000) {
-      lock = FMPLL.SYNSR.B.LOCK != 0;
-      ++i;
+#ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+  #if defined(MPC55XX_HAS_FMPLL) || defined(MPC55XX_HAS_FMPLL_ENHANCED)
+    static BSP_START_TEXT_SECTION void fmpll_wait_for_lock(void)
+    {
+      int i = 0;
+      bool lock = false;
+
+      while (!lock && i < 6000) {
+        lock = FMPLL.SYNSR.B.LOCK != 0;
+        ++i;
+      }
+
+      if (!lock) {
+        bsp_reset();
+      }
     }
-
-    if (!lock) {
-      bsp_reset();
-    }
-  }
+  #endif
 #endif
 
 BSP_START_TEXT_SECTION void mpc55xx_start_clock(void)
 {
-  const mpc55xx_clock_config *cfg = mpc55xx_start_config_clock;
+  #ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+    const mpc55xx_clock_config *cfg = mpc55xx_start_config_clock;
 
-  #ifdef MPC55XX_HAS_FMPLL
-    volatile struct FMPLL_tag *fmpll = &FMPLL;
+    #ifdef MPC55XX_HAS_FMPLL
+      volatile struct FMPLL_tag *fmpll = &FMPLL;
 
-    fmpll->SYNCR.R = cfg->syncr_tmp.R;
-    fmpll->SYNCR.R;
-    fmpll_wait_for_lock();
+      fmpll->SYNCR.R = cfg->syncr_tmp.R;
+      fmpll->SYNCR.R;
+      fmpll_wait_for_lock();
 
-    fmpll->SYNCR.R = cfg->syncr_final.R;
-    fmpll->SYNCR.R;
-    fmpll_wait_for_lock();
-  #endif
+      fmpll->SYNCR.R = cfg->syncr_final.R;
+      fmpll->SYNCR.R;
+      fmpll_wait_for_lock();
+    #endif
 
-  #ifdef MPC55XX_HAS_FMPLL_ENHANCED
-    volatile struct FMPLL_tag *fmpll = &FMPLL;
+    #ifdef MPC55XX_HAS_FMPLL_ENHANCED
+      volatile struct FMPLL_tag *fmpll = &FMPLL;
 
-    fmpll->ESYNCR2.R = cfg->esyncr2_tmp.R;
-    fmpll->ESYNCR2.R;
-    fmpll->ESYNCR1.R = cfg->esyncr1_final.R;
-    fmpll->ESYNCR1.R;
-    fmpll_wait_for_lock();
+      fmpll->ESYNCR2.R = cfg->esyncr2_tmp.R;
+      fmpll->ESYNCR2.R;
+      fmpll->ESYNCR1.R = cfg->esyncr1_final.R;
+      fmpll->ESYNCR1.R;
+      fmpll_wait_for_lock();
 
-    fmpll->ESYNCR2.R = cfg->esyncr2_final.R;
-    fmpll->ESYNCR2.R;
-    fmpll_wait_for_lock();
+      fmpll->ESYNCR2.R = cfg->esyncr2_final.R;
+      fmpll->ESYNCR2.R;
+      fmpll_wait_for_lock();
 
-    #if MPC55XX_CHIP_TYPE / 10 == 551
-      /* System clock supplied by PLL */
-      SIU.SYSCLK.B.SYSCLKSEL = 2;
+      #if MPC55XX_CHIP_TYPE / 10 == 551
+        /* System clock supplied by PLL */
+        SIU.SYSCLK.B.SYSCLKSEL = 2;
+      #endif
     #endif
-  #endif
 
-  #ifdef MPC55XX_HAS_MODE_CONTROL
-    volatile CGM_tag *cgm = &CGM;
-    size_t fmpll_count = sizeof(cfg->fmpll) / sizeof(cfg->fmpll [0]);
-    size_t auxclk_count = sizeof(cfg->auxclk) / sizeof(cfg->auxclk [0]);
-    size_t i = 0;
+    #ifdef MPC55XX_HAS_MODE_CONTROL
+      volatile CGM_tag *cgm = &CGM;
+      size_t fmpll_count = sizeof(cfg->fmpll) / sizeof(cfg->fmpll [0]);
+      size_t auxclk_count = sizeof(cfg->auxclk) / sizeof(cfg->auxclk [0]);
+      size_t i = 0;
 
-    for (i = 0; i < auxclk_count; ++i) {
-      cgm->AUXCLK [i].AC_SC.R = cfg->auxclk [i].AC_SC.R;
-      cgm->AUXCLK [i].AC_DC0_3.R = cfg->auxclk [i].AC_DC0_3.R;
-    }
+      for (i = 0; i < auxclk_count; ++i) {
+        cgm->AUXCLK [i].AC_SC.R = cfg->auxclk [i].AC_SC.R;
+        cgm->AUXCLK [i].AC_DC0_3.R = cfg->auxclk [i].AC_DC0_3.R;
+      }
 
-    for (i = 0; i < fmpll_count; ++i) {
-      cgm->FMPLL [i].CR.R = cfg->fmpll [i].cr.R;
-      cgm->FMPLL [i].MR.R = cfg->fmpll [i].mr.R;
-    }
+      for (i = 0; i < fmpll_count; ++i) {
+        cgm->FMPLL [i].CR.R = cfg->fmpll [i].cr.R;
+        cgm->FMPLL [i].MR.R = cfg->fmpll [i].mr.R;
+      }
 
-    cgm->OC_EN.R = cfg->oc_en.R;
-    cgm->OCDS_SC.R = cfg->ocds_sc.R;
+      cgm->OC_EN.R = cfg->oc_en.R;
+      cgm->OCDS_SC.R = cfg->ocds_sc.R;
+    #endif
   #endif
 }
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 7d17e94615..6f168a72db 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-early.c
@@ -51,13 +51,24 @@ static BSP_START_TEXT_SECTION void mpc55xx_start_mmu(void)
 
 static BSP_START_TEXT_SECTION void mpc55xx_start_internal_ram(void)
 {
-  /* Initialize internal SRAM to zero (ECC) */
-  bsp_start_zero(
-    (char *) bsp_ram_start + MPC55XX_EARLY_STACK_SIZE,
-    (size_t) bsp_ram_size - MPC55XX_EARLY_STACK_SIZE
-  );
-  #ifdef MPC55XX_HAS_SECOND_INTERNAL_RAM_AREA
-    bsp_start_zero(&bsp_ram_1_start [0], (size_t) bsp_ram_1_size);
+  #ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+    /* Initialize internal SRAM to zero (ECC) */
+    bsp_start_zero(
+      (char *) bsp_ram_start + MPC55XX_EARLY_STACK_SIZE,
+      (size_t) bsp_ram_size - MPC55XX_EARLY_STACK_SIZE
+    );
+    #ifdef MPC55XX_HAS_SECOND_INTERNAL_RAM_AREA
+      bsp_start_zero(&bsp_ram_1_start [0], (size_t) bsp_ram_1_size);
+    #endif
+  #else
+    bsp_start_zero(
+      bsp_section_sbss_begin,
+      (size_t) bsp_section_sbss_size
+    );
+    bsp_start_zero(
+      bsp_section_bss_begin,
+      (size_t) bsp_section_bss_size
+    );
   #endif
 }
 
@@ -76,40 +87,42 @@ static BSP_START_TEXT_SECTION void mpc55xx_start_load_nocache_section(void)
 
 static BSP_START_TEXT_SECTION void mpc55xx_start_mode_change(void)
 {
-  #ifdef MPC55XX_HAS_MODE_CONTROL
-    uint32_t mctl_key1 = 0x5af0;
-    uint32_t mctl_key2 = 0xa50f;
-    int i = 0;
-
-    /* Clear any pending RGM status */
-    RGM.FES.R = 0xffff;
-    RGM.DES.R = 0xffff;
-
-    /* Make sure XOSC and PLLs are on in RUN0 state */
-    ME.DRUN_MC.R = 0x001f0074;
-    ME.RUN_MC [0].R = 0x001f0074;
-
-    /*
-     * Make sure all peripherals are active in DRUN and RUN0 state.
-     *
-     * FIXME: This might be optimized to reduce power consumtion.
-     */
-    for (i = 0; i < 8; ++i) {
-      ME_RUN_PC_32B_tag run_pc = { .R = ME.RUN_PC [i].R };
-
-      run_pc.B.DRUN = 1;
-      run_pc.B.RUN0 = 1;
-
-      ME.RUN_PC [i].R = run_pc.R;
-    }
-
-    /* Switch to RUN0 state */
-    ME.MCTL.R = 0x40000000 | mctl_key1;
-    ME.MCTL.R = 0x40000000 | mctl_key2;
-
-    while (ME.GS.B.S_MTRANS) {
-      /* Wait for mode switch to be completed */
-    }
+  #ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+    #ifdef MPC55XX_HAS_MODE_CONTROL
+      uint32_t mctl_key1 = 0x5af0;
+      uint32_t mctl_key2 = 0xa50f;
+      int i = 0;
+
+      /* Clear any pending RGM status */
+      RGM.FES.R = 0xffff;
+      RGM.DES.R = 0xffff;
+
+      /* Make sure XOSC and PLLs are on in RUN0 state */
+      ME.DRUN_MC.R = 0x001f0074;
+      ME.RUN_MC [0].R = 0x001f0074;
+
+      /*
+       * Make sure all peripherals are active in DRUN and RUN0 state.
+       *
+       * FIXME: This might be optimized to reduce power consumtion.
+       */
+      for (i = 0; i < 8; ++i) {
+	ME_RUN_PC_32B_tag run_pc = { .R = ME.RUN_PC [i].R };
+
+	run_pc.B.DRUN = 1;
+	run_pc.B.RUN0 = 1;
+
+	ME.RUN_PC [i].R = run_pc.R;
+      }
+
+      /* Switch to RUN0 state */
+      ME.MCTL.R = 0x40000000 | mctl_key1;
+      ME.MCTL.R = 0x40000000 | mctl_key2;
+
+      while (ME.GS.B.S_MTRANS) {
+	/* Wait for mode switch to be completed */
+      }
+    #endif
   #endif
 }
 
@@ -134,44 +147,48 @@ static BSP_START_TEXT_SECTION void mpc55xx_start_siu(void)
 
 static BSP_START_TEXT_SECTION void mpc55xx_start_ebi_chip_select(void)
 {
-  #ifdef MPC55XX_HAS_EBI
-    size_t i = 0;
+  #ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+    #ifdef MPC55XX_HAS_EBI
+      size_t i = 0;
 
-    for (i = 0; i < mpc55xx_start_config_ebi_cs_count [0]; ++i) {
-       EBI.CS [i] = mpc55xx_start_config_ebi_cs [i];
-    }
+      for (i = 0; i < mpc55xx_start_config_ebi_cs_count [0]; ++i) {
+	 EBI.CS [i] = mpc55xx_start_config_ebi_cs [i];
+      }
 
-    for (i = 0; i < mpc55xx_start_config_ebi_cal_cs_count [0]; ++i) {
-       EBI.CAL_CS [i] = mpc55xx_start_config_ebi_cal_cs [i];
-    }
+      for (i = 0; i < mpc55xx_start_config_ebi_cal_cs_count [0]; ++i) {
+	 EBI.CAL_CS [i] = mpc55xx_start_config_ebi_cal_cs [i];
+      }
+    #endif
   #endif
 }
 
 static BSP_START_TEXT_SECTION void mpc55xx_start_ebi(void)
 {
-  #if defined(MPC55XX_BOARD_GWLCFM)
-    /*
-     * init EBI for Muxed AD bus
-     */
-    EBI.MCR.B.DBM = 1;
-    EBI.MCR.B.AD_MUX = 1; /* use multiplexed bus */
-    EBI.MCR.B.D16_31 = 1; /* use lower AD bus    */
-
-    SIU.ECCR.B.EBDF = 3;  /* use CLK/4 as bus clock */
-  #elif defined(MPC55XX_BOARD_MPC5674FEVB)
-    struct EBI_tag ebi = {
-      .MCR = {
-        .B = {
-          .ACGE = 0,
-          .MDIS = 0,
-          .D16_31 = 0,
-          .AD_MUX = 0,
-          .DBM = 0
-        }
-      }
-    };
-
-    EBI.MCR.R = ebi.MCR.R;
+  #ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+    #if defined(MPC55XX_BOARD_GWLCFM)
+      /*
+       * init EBI for Muxed AD bus
+       */
+      EBI.MCR.B.DBM = 1;
+      EBI.MCR.B.AD_MUX = 1; /* use multiplexed bus */
+      EBI.MCR.B.D16_31 = 1; /* use lower AD bus    */
+
+      SIU.ECCR.B.EBDF = 3;  /* use CLK/4 as bus clock */
+    #elif defined(MPC55XX_BOARD_MPC5674FEVB)
+      struct EBI_tag ebi = {
+	.MCR = {
+	  .B = {
+	    .ACGE = 0,
+	    .MDIS = 0,
+	    .D16_31 = 0,
+	    .AD_MUX = 0,
+	    .DBM = 0
+	  }
+	}
+      };
+
+      EBI.MCR.R = ebi.MCR.R;
+    #endif
   #endif
 }
 
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-flash.S b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-flash.S
index 9a304d5c89..61df507b13 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-flash.S
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-flash.S
@@ -38,7 +38,7 @@
  * @warning Code will be copied and executed on the stack.
  */
 GLOBAL_FUNCTION mpc55xx_start_flash
-#if MPC55XX_CHIP_TYPE / 10 == 564
+#if !defined(MPC55XX_NEEDS_LOW_LEVEL_INIT) || MPC55XX_CHIP_TYPE / 10 == 564
 	blr
 #else
 	.equ	stack_size, 20
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-watchdog.c b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-watchdog.c
index 209405df0c..153a8095e9 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-watchdog.c
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start-watchdog.c
@@ -26,12 +26,14 @@
 
 BSP_START_TEXT_SECTION void mpc55xx_start_watchdog(void)
 {
-  #ifdef MPC55XX_HAS_SWT
-    /* Write keys to clear soft lock bit */
-    SWT.SR.R = 0x0000c520;
-    SWT.SR.R = 0x0000d928;
+  #ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+    #ifdef MPC55XX_HAS_SWT
+      /* Write keys to clear soft lock bit */
+      SWT.SR.R = 0x0000c520;
+      SWT.SR.R = 0x0000d928;
 
-    /* Clear watchdog enable (WEN) */
-    SWT.CR.R = 0x8000010A;
+      /* Clear watchdog enable (WEN) */
+      SWT.CR.R = 0x8000010A;
+    #endif
   #endif
 }
diff --git a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start.S b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start.S
index 0b54b75734..b5e31463e0 100644
--- a/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start.S
+++ b/c/src/lib/libbsp/powerpc/mpc55xxevb/startup/start.S
@@ -80,6 +80,8 @@ mpc55xx_bootflag_1:
 
 _start:
 
+#ifdef MPC55XX_NEEDS_LOW_LEVEL_INIT
+
 	/* Enable SPE */
 #ifdef HAS_SPE
 	mfmsr	r3
@@ -204,6 +206,8 @@ zero_intermediate_stack_loop:
 	bne	cr7, zero_intermediate_stack_loop
 	subi	r1, r3, 16
 
+#endif /* MPC55XX_NEEDS_LOW_LEVEL_INIT */
+
 	/* Next steps in C */
 	bl	mpc55xx_start_early