From 26928f601f2072788e2a3340c25b2845e783b7f3 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Thu, 14 Mar 2013 11:47:48 +0000
Subject: [PATCH] git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@5427
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/hal/platforms/SPC563Mxx/hal_lld.h |  8 +++++---
 os/hal/platforms/SPC564Axx/hal_lld.c |  2 ++
 os/hal/platforms/SPC564Axx/hal_lld.h | 14 +++++++++-----
 os/ports/GCC/PPC/SPC564Axx/core.s    | 28 ++++++++++++++++++++++++++++
 4 files changed, 44 insertions(+), 8 deletions(-)

diff --git a/os/hal/platforms/SPC563Mxx/hal_lld.h b/os/hal/platforms/SPC563Mxx/hal_lld.h
index c0061556d..2d9952805 100644
--- a/os/hal/platforms/SPC563Mxx/hal_lld.h
+++ b/os/hal/platforms/SPC563Mxx/hal_lld.h
@@ -227,12 +227,14 @@
 /**
  * @brief   Flash wait states are a function of the system clock.
  */
-#if (SPC5_SYSCLK <= 30000000) || defined(__DOXYGEN__)
+#if (SPC5_SYSCLK <= 20000000) || defined(__DOXYGEN__)
 #define SPC5_FLASH_WS       (BIUCR_APC_0 | BIUCR_RWSC_0 | BIUCR_WWSC_1)
-#elif SPC5_SYSCLK <= 60000000
+#elif SPC5_SYSCLK <= 40000000
 #define SPC5_FLASH_WS       (BIUCR_APC_1 | BIUCR_RWSC_1 | BIUCR_WWSC_1)
-#else
+#elif SPC5_SYSCLK <= 64000000
 #define SPC5_FLASH_WS       (BIUCR_APC_2 | BIUCR_RWSC_2 | BIUCR_WWSC_1)
+#else
+#define SPC5_FLASH_WS       (BIUCR_APC_3 | BIUCR_RWSC_3 | BIUCR_WWSC_1)
 #endif
 
 /*===========================================================================*/
diff --git a/os/hal/platforms/SPC564Axx/hal_lld.c b/os/hal/platforms/SPC564Axx/hal_lld.c
index af0315af1..0f4dbc0cf 100644
--- a/os/hal/platforms/SPC564Axx/hal_lld.c
+++ b/os/hal/platforms/SPC564Axx/hal_lld.c
@@ -52,6 +52,7 @@ void hal_lld_init(void) {
   extern void _vectors(void);
   uint32_t n;
 
+#if 0
   /* FLASH wait states and prefetching setup.*/
   CFLASH0.BIUCR.R  = SPC5_FLASH_BIUCR | SPC5_FLASH_WS;
   CFLASH0.BIUCR2.R = 0;
@@ -73,6 +74,7 @@ void hal_lld_init(void) {
                                             Core Instructions (0): 1
                                             Undocumented (2):      2
                                             Core Data (4):         3        */
+#endif
 
   /* Downcounter timer initialized for system tick use, TB enabled for debug
      and measurements.*/
diff --git a/os/hal/platforms/SPC564Axx/hal_lld.h b/os/hal/platforms/SPC564Axx/hal_lld.h
index 7b26945a9..151e67d81 100644
--- a/os/hal/platforms/SPC564Axx/hal_lld.h
+++ b/os/hal/platforms/SPC564Axx/hal_lld.h
@@ -227,12 +227,16 @@
 /**
  * @brief   Flash wait states are a function of the system clock.
  */
-#if (SPC5_SYSCLK <= 30000000) || defined(__DOXYGEN__)
-#define SPC5_FLASH_WS       (BIUCR_APC_0 | BIUCR_RWSC_0 | BIUCR_WWSC_1)
-#elif SPC5_SYSCLK <= 60000000
-#define SPC5_FLASH_WS       (BIUCR_APC_1 | BIUCR_RWSC_1 | BIUCR_WWSC_1)
+#if (SPC5_SYSCLK <= 20000000) || defined(__DOXYGEN__)
+#define SPC5_FLASH_WS       (BIUCR_APC_0 | BIUCR_RWSC_0 | BIUCR_WWSC_3)
+#elif SPC5_SYSCLK <= 61000000
+#define SPC5_FLASH_WS       (BIUCR_APC_1 | BIUCR_RWSC_1 | BIUCR_WWSC_3)
+#elif SPC5_SYSCLK <= 90000000
+#define SPC5_FLASH_WS       (BIUCR_APC_2 | BIUCR_RWSC_2 | BIUCR_WWSC_3)
+#elif SPC5_SYSCLK <= 123000000
+#define SPC5_FLASH_WS       (BIUCR_APC_3 | BIUCR_RWSC_3 | BIUCR_WWSC_3)
 #else
-#define SPC5_FLASH_WS       (BIUCR_APC_2 | BIUCR_RWSC_2 | BIUCR_WWSC_1)
+#define SPC5_FLASH_WS       (BIUCR_APC_4 | BIUCR_RWSC_4 | BIUCR_WWSC_3)
 #endif
 
 /*===========================================================================*/
diff --git a/os/ports/GCC/PPC/SPC564Axx/core.s b/os/ports/GCC/PPC/SPC564Axx/core.s
index 36b5575d6..92232a9fa 100644
--- a/os/ports/GCC/PPC/SPC564Axx/core.s
+++ b/os/ports/GCC/PPC/SPC564Axx/core.s
@@ -34,6 +34,15 @@
 #define BUCSR_BALLOC_BFI        0x00000200
 /** @} */
 
+/**
+ * @name    LICSR1 registers definitions
+ * @{
+ */
+#define LICSR1_ICE              0x00000001
+#define LICSR1_ICINV            0x00000002
+#define LICSR1_ICORG            0x00000010
+/** @} */
+
 /**
  * @name    BUCSR default settings
  * @{
@@ -41,6 +50,13 @@
 #define BUCSR_DEFAULT           (BUCSR_BPEN | BUCSR_BALLOC_BFI)
 /** @} */
 
+/**
+ * @name    LICSR1 default settings
+ * @{
+ */
+#define LICSR1_DEFAULT          (LICSR1_ICE | LICSR1_ICORG)
+/** @} */
+
 /**
  * @name   MSR register definitions
  * @{
@@ -132,6 +148,18 @@ _coreinit:
         li          %r3, BUCSR_DEFAULT
         mtspr       1013, %r3       /* BUCSR */
 
+        /*
+         * Cache invalidated and then enabled.
+         */
+        li          %r3, LICSR1_ICINV
+        mtspr       1011, %r3       /* LICSR1 */
+.inv:   mfspr       %r3, 1011       /* LICSR1 */
+        andi.       %r3, %r3, LICSR1_ICINV
+        bne         .inv
+        lis         %r3, LICSR1_DEFAULT@h
+        ori         %r3, %r3, LICSR1_DEFAULT@l
+        mtspr       1011, %r3       /* LICSR1 */
+
         blr
 
         /*