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

10 files changed, 312 insertions, 282 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;

+  }

 }

 

 /**

diff --git a/os/hal/platforms/SPC560BCxx/hal_lld.h b/os/hal/platforms/SPC560BCxx/hal_lld.h
index e8e3bcc15..701e2f4a7 100644
--- a/os/hal/platforms/SPC560BCxx/hal_lld.h
+++ b/os/hal/platforms/SPC560BCxx/hal_lld.h
@@ -237,6 +237,13 @@
 #endif

 

 /**

+ * @brief   Disables the watchdog on start.

+ */

+#if !defined(SPC5_DISABLE_WATCHDOG) || defined(__DOXYGEN__)

+#define SPC5_DISABLE_WATCHDOG               TRUE

+#endif

+

+/**

  * @brief   XOSC divider value.

  * @note    The allowed range is 1...32.

  */

@@ -589,6 +596,15 @@
 #define SPC5_PIT0_IRQ_PRIORITY              4

 #endif

 

+/**

+ * @brief   Clock initialization failure hook.

+ * @note    The default is to stop the system and let the RTC restart it.

+ * @note    The hook code must not return.

+ */

+#if !defined(SPC5_CLOCK_FAILURE_HOOK) || defined(__DOXYGEN__)

+#define SPC5_CLOCK_FAILURE_HOOK()           chSysHalt()

+#endif

+

 /*===========================================================================*/

 /* Derived constants and error checks.                                       */

 /*===========================================================================*/

diff --git a/os/hal/platforms/SPC560Pxx/hal_lld.c b/os/hal/platforms/SPC560Pxx/hal_lld.c
index f63573ad6..ef7c4806a 100644
--- a/os/hal/platforms/SPC560Pxx/hal_lld.c
+++ b/os/hal/platforms/SPC560Pxx/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,12 +113,42 @@ 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;

+

+  /* 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.*/

   CGM.OSC_CTL.B.OSCBYP = TRUE;

 #endif /* SPC5_OSC_BYPASS */

 

+  /* Enables the XOSC in order to check its functionality before proceeding

+     with the initialization.*/

+/*  ME.DRUN.R = SPC5_ME_MC_SYSCLK_IRC | SPC5_ME_MC_IRCON | SPC5_ME_MC_XOSC0ON |           \

+              SPC5_ME_MC_CFLAON_NORMAL | SPC5_ME_MC_CFLAON_NORMAL |

+              SPC5_ME_MC_MVRON;

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

+    SPC5_CLOCK_FAILURE_HOOK();

+  }*/

+

   /* Initialization of the FMPLLs settings.*/

   CGM.FMPLL[0].CR.R = SPC5_FMPLL0_ODF |

                       ((SPC5_FMPLL0_IDF_VALUE - 1) << 26) |

@@ -129,6 +160,7 @@ void spc_clock_init(void) {
   CGM.FMPLL[1].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.           */

@@ -139,6 +171,10 @@ void spc_clock_init(void) {
   ME.RUN[3].R       = SPC5_ME_RUN3_MC_BITS;     /* RUN0 run mode.           */

   ME.HALT0.R        = SPC5_ME_HALT0_MC_BITS;    /* HALT0 run mode.          */

   ME.STOP0.R        = SPC5_ME_STOP0_MC_BITS;    /* STOP0 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;

@@ -158,17 +194,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;

 

+  /* 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 */

 }

 

@@ -183,23 +219,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;

+  }

 }

 

 /**

diff --git a/os/hal/platforms/SPC560Pxx/hal_lld.h b/os/hal/platforms/SPC560Pxx/hal_lld.h
index 43bd1c781..2d652b90a 100644
--- a/os/hal/platforms/SPC560Pxx/hal_lld.h
+++ b/os/hal/platforms/SPC560Pxx/hal_lld.h
@@ -231,6 +231,13 @@
 #endif

 

 /**

+ * @brief   Disables the watchdog on start.

+ */

+#if !defined(SPC5_DISABLE_WATCHDOG) || defined(__DOXYGEN__)

+#define SPC5_DISABLE_WATCHDOG               TRUE

+#endif

+

+/**

  * @brief   FMPLL0 IDF divider value.

  * @note    The default value is calculated for XOSC=40MHz and PHI=64MHz.

  */

@@ -584,6 +591,15 @@
 #define SPC5_PIT0_IRQ_PRIORITY              4

 #endif

 

+/**

+ * @brief   Clock initialization failure hook.

+ * @note    The default is to stop the system and let the RTC restart it.

+ * @note    The hook code must not return.

+ */

+#if !defined(SPC5_CLOCK_FAILURE_HOOK) || defined(__DOXYGEN__)

+#define SPC5_CLOCK_FAILURE_HOOK()           chSysHalt()

+#endif

+

 /*===========================================================================*/

 /* Derived constants and error checks.                                       */

 /*===========================================================================*/

diff --git a/os/ports/GCC/PPC/SPC560Pxx/bam.s b/os/ports/GCC/PPC/SPC560Pxx/bam.s
index 8fbc0b53c..b5a2a6af0 100644
--- a/os/ports/GCC/PPC/SPC560Pxx/bam.s
+++ b/os/ports/GCC/PPC/SPC560Pxx/bam.s
@@ -28,10 +28,16 @@
 

 #if !defined(__DOXYGEN__)

 

-        /* BAM info, SWT off, WTE off, VLE from settings.*/

+        /* BAM record.*/

         .section    .bam, "ax"

         .long       0x015A0000

-        .long       _boot_address

+        .long       .init

+

+.init:

+        bl          _coreinit

+        bl          _ivinit

+

+        b           _boot_address

 

 #endif /* !defined(__DOXYGEN__) */

 

diff --git a/os/ports/GCC/PPC/SPC560Pxx/core.s b/os/ports/GCC/PPC/SPC560Pxx/core.s
new file mode 100644
index 000000000..246c39739
--- /dev/null
+++ b/os/ports/GCC/PPC/SPC560Pxx/core.s
@@ -0,0 +1,167 @@
+/*

+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,

+                 2011,2012,2013 Giovanni Di Sirio.

+

+    This file is part of ChibiOS/RT.

+

+    ChibiOS/RT is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS/RT is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+*/

+

+/**

+ * @file    SPC560Pxx/core.s

+ * @brief   e200z0 core configuration.

+ *

+ * @addtogroup PPC_CORE

+ * @{

+ */

+

+/**

+ * @name    BUCSR registers definitions

+ * @{

+ */

+#define BUCSR_BPEN              0x00000001

+#define BUCSR_BALLOC_BFI        0x00000200

+/** @} */

+

+/**

+ * @name    BUCSR default settings

+ * @{

+ */

+#define BUCSR_DEFAULT           (BUCSR_BPEN | BUCSR_BALLOC_BFI)

+/** @} */

+

+/**

+ * @name   MSR register definitions

+ * @{

+ */

+#define MSR_WE                  0x00040000

+#define MSR_CE                  0x00020000

+#define MSR_EE                  0x00008000

+#define MSR_PR                  0x00004000

+#define MSR_ME                  0x00001000

+#define MSR_DE                  0x00000200

+#define MSR_IS                  0x00000020

+#define MSR_DS                  0x00000010

+#define MSR_RI                  0x00000002

+/** @} */

+

+/**

+ * @name   MSR default settings

+ * @{

+ */

+#define MSR_DEFAULT             (MSR_WE | MSR_CE | MSR_ME)

+/** @} */

+

+#if !defined(__DOXYGEN__)

+

+        .section    .coreinit, "ax"

+

+        .align      2

+        .globl      _coreinit

+        .type       _coreinit, @function

+_coreinit:

+        /*

+         * Branch prediction enabled.

+         */

+        li          %r3, BUCSR_DEFAULT

+        mtspr       1013, %r3       /* BUCSR */

+

+        blr

+

+        /*

+         * Exception vectors initialization.

+         */

+        .global     _ivinit

+        .type       _ivinit, @function

+_ivinit:

+        /* MSR initialization.*/

+        lis         %r3, MSR_DEFAULT@h

+        ori         %r3, %r3, MSR_DEFAULT@l

+        mtMSR       %r3

+

+        /* IVPR initialization.*/

+        lis         %r3, __ivpr_base__@h

+        ori         %r3, %r3, __ivpr_base__@l

+        mtIVPR      %r3

+

+        blr

+

+        .section    .handlers, "ax"

+

+        .globl      IVORS

+IVORS:

+IVOR0:  b           IVOR0

+        .align      4

+IVOR1:  b           _IVOR1

+        .align      4

+IVOR2:  b           _IVOR2

+        .align      4

+IVOR3:  b           _IVOR3

+        .align      4

+IVOR4:  b           _IVOR4

+        .align      4

+IVOR5:  b           _IVOR5

+        .align      4

+IVOR6:  b           _IVOR6

+        .align      4

+IVOR7:  b           _IVOR7

+        .align      4

+IVOR8:  b           _IVOR8

+        .align      4

+IVOR9:  b           _IVOR9

+        .align      4

+IVOR10: b           _IVOR10

+        .align      4

+IVOR11: b           _IVOR11

+        .align      4

+IVOR12: b           _IVOR12

+        .align      4

+IVOR13: b           _IVOR13

+        .align      4

+IVOR14: b           _IVOR14

+        .align      4

+IVOR15: b           _IVOR15

+

+        /*

+         * Unhandled exceptions handler.

+         */

+        .weak       _IVOR0,  _IVOR1,  _IVOR2,  _IVOR3,  _IVOR4,  _IVOR5

+        .weak       _IVOR6,  _IVOR7,  _IVOR8,  _IVOR9,  _IVOR10, _IVOR11

+        .weak       _IVOR12, _IVOR13, _IVOR14, _IVOR15, _IVOR32, _IVOR33

+        .weak       _IVOR34

+        .weak       _unhandled_exception

+_IVOR0:

+_IVOR1:

+_IVOR2:

+_IVOR3:

+_IVOR5:

+_IVOR6:

+_IVOR7:

+_IVOR8:

+_IVOR9:

+_IVOR11:

+_IVOR12:

+_IVOR13:

+_IVOR14:

+_IVOR15:

+_IVOR32:

+_IVOR33:

+_IVOR34:

+        .type       _unhandled_exception, @function

+_unhandled_exception:

+        b           _unhandled_exception

+

+#endif /* !defined(__DOXYGEN__) */

+

+/** @} */

diff --git a/os/ports/GCC/PPC/SPC560Pxx/ivor.s b/os/ports/GCC/PPC/SPC560Pxx/ivor.s
deleted file mode 100644
index df84fc636..000000000
--- a/os/ports/GCC/PPC/SPC560Pxx/ivor.s
+++ /dev/null
@@ -1,234 +0,0 @@
-/*

-    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,

-                 2011,2012,2013 Giovanni Di Sirio.

-

-    This file is part of ChibiOS/RT.

-

-    ChibiOS/RT is free software; you can redistribute it and/or modify

-    it under the terms of the GNU General Public License as published by

-    the Free Software Foundation; either version 3 of the License, or

-    (at your option) any later version.

-

-    ChibiOS/RT is distributed in the hope that it will be useful,

-    but WITHOUT ANY WARRANTY; without even the implied warranty of

-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

-    GNU General Public License for more details.

-

-    You should have received a copy of the GNU General Public License

-    along with this program.  If not, see <http://www.gnu.org/licenses/>.

-*/

-

-/**

- * @file    SPC560Pxx/ivor.s

- * @brief   SPC560Pxx IVORx handlers.

- *

- * @addtogroup PPC_CORE

- * @{

- */

-

-/*

- * Imports the PPC configuration headers.

- */

-#define _FROM_ASM_

-#include "chconf.h"

-#include "chcore.h"

-

-#if !defined(__DOXYGEN__)

-        /*

-         * INTC registers address.

-         */

-        .equ  INTC_IACKR, 0xfff48010

-        .equ  INTC_EOIR,  0xfff48018

-

-        .section    .handlers, "ax"

-

-        /*

-         * Fixed IVOR offset table.

-         */

-        .globl      IVORS

-IVORS:

-IVOR0:  b           IVOR0

-        .align      4

-IVOR1:  b           _IVOR1

-        .align      4

-IVOR2:  b           _IVOR2

-        .align      4

-IVOR3:  b           _IVOR3

-        .align      4

-IVOR4:  b           _IVOR4

-        .align      4

-IVOR5:  b           _IVOR5

-        .align      4

-IVOR6:  b           _IVOR6

-        .align      4

-IVOR7:  b           _IVOR7

-        .align      4

-IVOR8:  b           _IVOR8

-        .align      4

-IVOR9:  b           _IVOR9

-        .align      4

-IVOR10: b           _IVOR10

-        .align      4

-IVOR11: b           _IVOR11

-        .align      4

-IVOR12: b           _IVOR12

-        .align      4

-IVOR13: b           _IVOR13

-        .align      4

-IVOR14: b           _IVOR14

-        .align      4

-IVOR15: b           _IVOR15

-

-        /*

-         * Unhandled exceptions handler.

-         */

-        .weak       _IVOR0

-_IVOR0:

-        .weak       _IVOR1

-_IVOR1:

-        .weak       _IVOR2

-_IVOR2:

-        .weak       _IVOR3

-_IVOR3:

-        .weak       _IVOR5

-_IVOR5:

-        .weak       _IVOR6

-_IVOR6:

-        .weak       _IVOR7

-_IVOR7:

-        .weak       _IVOR8

-_IVOR8:

-        .weak       _IVOR9

-_IVOR9:

-        .weak       _IVOR10

-_IVOR10:

-        .weak       _IVOR11

-_IVOR11:

-        .weak       _IVOR12

-_IVOR12:

-        .weak       _IVOR13

-_IVOR13:

-        .weak       _IVOR14

-_IVOR14:

-        .weak       _IVOR15

-_IVOR15:

-        .weak       _unhandled_exception

-        .type       _unhandled_exception, @function

-_unhandled_exception:

-        b           _unhandled_exception

-

-        /*

-         * IVOR4 handler (Book-E external interrupt).

-         */

-        .align		4

-        .globl      _IVOR4

-        .type       _IVOR4, @function

-_IVOR4:

-        /* Creation of the external stack frame (extctx structure).*/

-        stwu        %sp, -80(%sp)           /* Size of the extctx structure.*/

-#if PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI

-        e_stmvsrrw  8(%sp)                  /* Saves PC, MSR.               */

-        e_stmvsprw  16(%sp)                 /* Saves CR, LR, CTR, XER.      */

-        e_stmvgprw  32(%sp)                 /* Saves GPR0, GPR3...GPR12.    */

-#else /* !(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */

-        stw         %r0, 32(%sp)            /* Saves GPR0.                  */

-        mfSRR0      %r0

-        stw         %r0, 8(%sp)             /* Saves PC.                    */

-        mfSRR1      %r0

-        stw         %r0, 12(%sp)            /* Saves MSR.                   */

-        mfCR        %r0

-        stw         %r0, 16(%sp)            /* Saves CR.                    */

-        mfLR        %r0

-        stw         %r0, 20(%sp)            /* Saves LR.                    */

-        mfCTR       %r0

-        stw         %r0, 24(%sp)            /* Saves CTR.                   */

-        mfXER       %r0

-        stw         %r0, 28(%sp)            /* Saves XER.                   */

-        stw         %r3, 36(%sp)            /* Saves GPR3...GPR12.          */

-        stw         %r4, 40(%sp)

-        stw         %r5, 44(%sp)

-        stw         %r6, 48(%sp)

-        stw         %r7, 52(%sp)

-        stw         %r8, 56(%sp)

-        stw         %r9, 60(%sp)

-        stw         %r10, 64(%sp)

-        stw         %r11, 68(%sp)

-        stw         %r12, 72(%sp)

-#endif /* !(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */

-

-        /* Software vector address from the INTC register.*/

-        lis         %r3, INTC_IACKR@h

-        ori         %r3, %r3, INTC_IACKR@l  /* IACKR register address.      */

-        lwz         %r3, 0(%r3)             /* IACKR register value.        */

-        lwz         %r3, 0(%r3)

-        mtCTR       %r3                     /* Software handler address.    */

-

-#if PPC_USE_IRQ_PREEMPTION

-        /* Allows preemption while executing the software handler.*/

-        wrteei      1

-#endif

-

-        /* Exectes the software handler.*/

-        bctrl

-

-#if PPC_USE_IRQ_PREEMPTION

-        /* Prevents preemption again.*/

-        wrteei      0

-#endif

-

-        /* Informs the INTC that the interrupt has been served.*/

-        mbar        0

-        lis         %r3, INTC_EOIR@h

-        ori         %r3, %r3, INTC_EOIR@l

-        stw         %r3, 0(%r3)             /* Writing any value should do. */

-

-        /* Verifies if a reschedule is required.*/

-#if CH_DBG_SYSTEM_STATE_CHECK

-        bl          dbg_check_lock

-#endif

-        bl          chSchIsPreemptionRequired

-        cmpli       cr0, %r3, 0

-        beq         cr0, _ivor_exit

-        bl          chSchDoReschedule

-

-        /* Context restore.*/

-        .globl      _ivor_exit

-_ivor_exit:

-#if CH_DBG_SYSTEM_STATE_CHECK

-        bl          dbg_check_unlock

-#endif

-#if PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI

-        e_lmvgprw  32(%sp)                  /* Restores GPR0, GPR3...GPR12. */

-        e_lmvsprw  16(%sp)                  /* Restores CR, LR, CTR, XER.   */

-        e_lmvsrrw  8(%sp)                   /* Restores PC, MSR.            */

-#else /*!(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */

-        lwz         %r3, 36(%sp)            /* Restores GPR3...GPR12.       */

-        lwz         %r4, 40(%sp)

-        lwz         %r5, 44(%sp)

-        lwz         %r6, 48(%sp)

-        lwz         %r7, 52(%sp)

-        lwz         %r8, 56(%sp)

-        lwz         %r9, 60(%sp)

-        lwz         %r10, 64(%sp)

-        lwz         %r11, 68(%sp)

-        lwz         %r12, 72(%sp)

-        lwz         %r0, 8(%sp)

-        mtSRR0      %r0                     /* Restores PC.                 */

-        lwz         %r0, 12(%sp)

-        mtSRR1      %r0                     /* Restores MSR.                */

-        lwz         %r0, 16(%sp)

-        mtCR        %r0                     /* Restores CR.                 */

-        lwz         %r0, 20(%sp)

-        mtLR        %r0                     /* Restores LR.                 */

-        lwz         %r0, 24(%sp)

-        mtCTR       %r0                     /* Restores CTR.                */

-        lwz         %r0, 28(%sp)

-        mtXER       %r0                     /* Restores XER.                */

-        lwz         %r0, 32(%sp)            /* Restores GPR0.               */

-#endif /* !(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */

-        addi        %sp, %sp, 80            /* Back to the previous frame.  */

-        rfi

-

-#endif /* !defined(__DOXYGEN__) */

-

-/** @} */

diff --git a/os/ports/GCC/PPC/SPC560Pxx/ld/SPC560P44.ld b/os/ports/GCC/PPC/SPC560Pxx/ld/SPC560P44.ld
index 675445fbe..075aae5e9 100644
--- a/os/ports/GCC/PPC/SPC560Pxx/ld/SPC560P44.ld
+++ b/os/ports/GCC/PPC/SPC560Pxx/ld/SPC560P44.ld
@@ -48,6 +48,7 @@ SECTIONS
     .boot : ALIGN(16) SUBALIGN(16)

     {

         KEEP(*(.bam))

+        KEEP(*(.coreinit))

         KEEP(*(.crt0))

         . = ALIGN(0x00000800);

         KEEP(*(.vectors))

diff --git a/os/ports/GCC/PPC/SPC560Pxx/ld/SPC560P50.ld b/os/ports/GCC/PPC/SPC560Pxx/ld/SPC560P50.ld
index 1d9f85e60..64e10aeb4 100644
--- a/os/ports/GCC/PPC/SPC560Pxx/ld/SPC560P50.ld
+++ b/os/ports/GCC/PPC/SPC560Pxx/ld/SPC560P50.ld
@@ -48,6 +48,7 @@ SECTIONS
     .boot : ALIGN(16) SUBALIGN(16)

     {

         KEEP(*(.bam))

+        KEEP(*(.coreinit))

         KEEP(*(.crt0))

         . = ALIGN(0x00000800);

         KEEP(*(.vectors))

diff --git a/os/ports/GCC/PPC/SPC560Pxx/port.mk b/os/ports/GCC/PPC/SPC560Pxx/port.mk
index d1c9dcd8e..112728112 100644
--- a/os/ports/GCC/PPC/SPC560Pxx/port.mk
+++ b/os/ports/GCC/PPC/SPC560Pxx/port.mk
@@ -2,9 +2,10 @@
 PORTSRC = ${CHIBIOS}/os/ports/GCC/PPC/chcore.c

 

 PORTASM = ${CHIBIOS}/os/ports/GCC/PPC/SPC560Pxx/bam.s \

-          ${CHIBIOS}/os/ports/GCC/PPC/crt0.s \

-          ${CHIBIOS}/os/ports/GCC/PPC/SPC560Pxx/ivor.s \

-          ${CHIBIOS}/os/ports/GCC/PPC/SPC560Pxx/vectors.s

+          ${CHIBIOS}/os/ports/GCC/PPC/SPC560Pxx/core.s \

+          ${CHIBIOS}/os/ports/GCC/PPC/SPC560Pxx/vectors.s \

+          ${CHIBIOS}/os/ports/GCC/PPC/ivor.s \

+          ${CHIBIOS}/os/ports/GCC/PPC/crt0.s

 

 PORTINC = ${CHIBIOS}/os/ports/GCC/PPC \

           ${CHIBIOS}/os/ports/GCC/PPC/SPC560Pxx