git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@5189 35acf78f-673a-0410-8e92-d51de3d6d3f4

1 files changed, 45 insertions, 23 deletions

diff --git a/os/hal/platforms/SPC560BCxx/hal_lld.c b/os/hal/platforms/SPC560BCxx/hal_lld.c
index bcc7e48b3..aa4c7f538 100644
--- a/os/hal/platforms/SPC560BCxx/hal_lld.c
+++ b/os/hal/platforms/SPC560BCxx/hal_lld.c
@@ -73,8 +73,9 @@ void hal_lld_init(void) {
 

   /* The system is switched to the RUN0 mode, the default for normal

      operations.*/

-  if (halSPCSetRunMode(SPC5_RUNMODE_RUN0) == CH_FAILED)

-    chSysHalt();

+  if (halSPCSetRunMode(SPC5_RUNMODE_RUN0) == CH_FAILED) {

+    SPC5_CLOCK_FAILURE_HOOK();

+  }

 

   /* INTC initialization, software vector mode, 4 bytes vectors, starting

      at priority 0.*/

@@ -112,10 +113,31 @@ void spc_clock_init(void) {
 

 #if !SPC5_NO_INIT

 

+#if SPC5_DISABLE_WATCHDOG

+  /* SWT disabled.*/

+  SWT.SR.R = 0xC520;

+  SWT.SR.R = 0xD928;

+  SWT.CR.R = 0xFF00000A;

+#endif

+

+  /* SSCM initialization. Setting up the most restrictive handling of

+     invalid accesses to peripherals.*/

+  SSCM.ERROR.R = 3;                             /* PAE and RAE bits.        */

+

+  /* RGM errors clearing.*/

+  RGM.FES.R         = 0xFFFF;

+  RGM.DES.R         = 0xFFFF;

+

   /* Oscillators dividers setup.*/

   CGM.FIRC_CTL.B.RCDIV   = SPC5_IRCDIV_VALUE - 1;

   CGM.FXOSC_CTL.B.OSCDIV = SPC5_XOSCDIV_VALUE - 1;

 

+  /* The system must be in DRUN mode on entry, if this is not the case then

+     it is considered a serious anomaly.*/

+  if (ME.GS.B.S_CURRENTMODE != SPC5_RUNMODE_DRUN) {

+    SPC5_CLOCK_FAILURE_HOOK();

+  }

+

 #if defined(SPC5_OSC_BYPASS)

   /* If the board is equipped with an oscillator instead of a xtal then the

      bypass must be activated.*/

@@ -129,6 +151,7 @@ void spc_clock_init(void) {
   CGM.FMPLL_MR.R = 0;                           /* TODO: Add a setting.     */

 

   /* Run modes initialization.*/

+  ME.IS.R           = 8;                        /* Resetting I_ICONF status.*/

   ME.MER.R          = SPC5_ME_ME_BITS;          /* Enabled run modes.       */

   ME.TEST.R         = SPC5_ME_TEST_MC_BITS;     /* TEST run mode.           */

   ME.SAFE.R         = SPC5_ME_SAFE_MC_BITS;     /* SAFE run mode.           */

@@ -140,6 +163,10 @@ void spc_clock_init(void) {
   ME.HALT0.R        = SPC5_ME_HALT0_MC_BITS;    /* HALT0 run mode.          */

   ME.STOP0.R        = SPC5_ME_STOP0_MC_BITS;    /* STOP0 run mode.          */

   ME.STANDBY0.R     = SPC5_ME_STANDBY0_MC_BITS; /* STANDBY0 run mode.       */

+  if (ME.IS.B.I_CONF) {

+    /* Configuration rejected.*/

+    SPC5_CLOCK_FAILURE_HOOK();

+  }

 

   /* Peripherals run and low power modes initialization.*/

   ME.RUNPC[0].R     = SPC5_ME_RUN_PC0_BITS;

@@ -159,20 +186,17 @@ void spc_clock_init(void) {
   ME.LPPC[6].R      = SPC5_ME_LP_PC6_BITS;

   ME.LPPC[7].R      = SPC5_ME_LP_PC7_BITS;

 

-  /* Switches again to DRUN mode (current mode) in order to update the

-     settings.*/

-  if (halSPCSetRunMode(SPC5_RUNMODE_DRUN) == CH_FAILED)

-    chSysHalt();

-

   /* CFLASH settings calculated for a maximum clock of 64MHz.*/

   CFLASH.PFCR0.B.BK0_APC  = 2;

   CFLASH.PFCR0.B.BK0_RWSC = 2;

   CFLASH.PFCR1.B.BK1_APC  = 2;

   CFLASH.PFCR1.B.BK1_RWSC = 2;

 

-  /* Initialization of e200z0 special registers.*/

-  port_mtspr(1013, 0x00000001);         /* BPEN=1.                          */

-

+  /* Switches again to DRUN mode (current mode) in order to update the

+     settings.*/

+  if (halSPCSetRunMode(SPC5_RUNMODE_DRUN) == CH_FAILED) {

+    SPC5_CLOCK_FAILURE_HOOK();

+  }

 #endif /* !SPC5_NO_INIT */

 }

 

@@ -187,23 +211,21 @@ void spc_clock_init(void) {
  */

 bool_t halSPCSetRunMode(spc5_runmode_t mode) {

 

+  /* Clearing status register bits I_IMODE(4) and I_IMTC(1).*/

+  ME.IS.R = 5;

+

   /* Starts a transition process.*/

   ME.MCTL.R = SPC5_ME_MCTL_MODE(mode) | SPC5_ME_MCTL_KEY;

   ME.MCTL.R = SPC5_ME_MCTL_MODE(mode) | SPC5_ME_MCTL_KEY_INV;

 

-  /* Waits the transition process to start.*/

-  while (!ME.GS.B.S_MTRANS)

-    ;

-

-  /* Waits the transition process to end.*/

-  while (ME.GS.B.S_MTRANS)

-    ;

-

-  /* Verifies that the mode has been effectively switched.*/

-  if (ME.GS.B.S_CURRENTMODE != mode)

-    return CH_FAILED;

-

-  return CH_SUCCESS;

+  /* Waits for the mode switch or an error condition.*/

+  while (TRUE) {

+    uint32_t r = ME.IS.R;

+    if (r & 1)

+      return CH_SUCCESS;

+    if (r & 4)

+      return CH_FAILED;

+  }

 }

 

 /**