3 files changed, 23 insertions, 1 deletions
diff --git a/os/hal/platforms/SPC560BCxx/hal_lld.c b/os/hal/platforms/SPC560BCxx/hal_lld.c
index b7e157dc5..0be4cfb57 100644
--- a/os/hal/platforms/SPC560BCxx/hal_lld.c
+++ b/os/hal/platforms/SPC560BCxx/hal_lld.c
@@ -176,6 +176,9 @@ void spc_clock_init(void) {
   CFLASH.PFCR1.B.BK1_APC  = 2;
   CFLASH.PFCR1.B.BK1_RWSC = 2;
 
+  /* Initialization of e200z0 special registers.*/
+  port_mtspr(1013, 0x00000001);         /* BPEN=1.                          */
+
 #endif /* !SPC5_NO_INIT */
 }
 
diff --git a/os/hal/platforms/SPC560BCxx/spc560bc_registry.h b/os/hal/platforms/SPC560BCxx/spc560bc_registry.h
index 6fe68be87..92158b107 100644
--- a/os/hal/platforms/SPC560BCxx/spc560bc_registry.h
+++ b/os/hal/platforms/SPC560BCxx/spc560bc_registry.h
@@ -81,6 +81,7 @@
 #define SPC5_SIU_NUM_PORTS                  8
 #define SPC5_SIU_NUM_PCRS                   123
 #define SPC5_SIU_NUM_PADSELS                32
+#define SPC5_SIU_SYSTEM_PINS                32,33,121,122
 /** @} */
 
 #endif /* _SPC560BC_REGISTRY_H_ */
diff --git a/os/hal/platforms/SPC5xx/SIUL_v1/pal_lld.c b/os/hal/platforms/SPC5xx/SIUL_v1/pal_lld.c
index e098391ce..ec0b576e9 100644
--- a/os/hal/platforms/SPC5xx/SIUL_v1/pal_lld.c
+++ b/os/hal/platforms/SPC5xx/SIUL_v1/pal_lld.c
@@ -43,6 +43,10 @@
 /* Driver local variables.                                                   */
 /*===========================================================================*/
 
+#if defined(SPC5_SIU_SYSTEM_PINS)
+static const unsigned system_pins[] = {SPC5_SIU_SYSTEM_PINS};
+#endif
+
 /*===========================================================================*/
 /* Driver local functions.                                                   */
 /*===========================================================================*/
@@ -72,8 +76,22 @@ void _pal_lld_init(const PALConfig *config) {
 #endif
 
   /* Initialize PCR registers for undefined pads.*/
-  for (i = 0; i < SPC5_SIU_NUM_PCRS; i++)
+  for (i = 0; i < SPC5_SIU_NUM_PCRS; i++) {
+#if defined(SPC5_SIU_SYSTEM_PINS)
+    /* Handling the case where some SIU pins are not meant to be reprogrammed,
+       for example JTAG pins.*/
+    unsigned j;
+    for (j = 0; j < sizeof system_pins; j++) {
+      if (i == system_pins[j])
+        goto skip;
+    }
+    SIU.PCR[i].R = config->default_mode;
+skip:
+    ;
+#else
     SIU.PCR[i].R = config->default_mode;
+#endif
+  }
 
   /* Initialize PADSEL registers.*/
   for (i = 0; i < SPC5_SIU_NUM_PADSELS; i++)