From f339189b11a9d229162d7d31b73ca400ce93782d Mon Sep 17 00:00:00 2001
From: Michael Stapelberg <stapelberg@google.com>
Date: Wed, 3 Jun 2020 20:47:11 +0200
Subject: [PATCH] hal_lld: make HSRUN mode work for e.g. 180 MHz Teensy 3.6

---
 os/hal/ports/KINETIS/MK66F18/hal_lld.c | 91 ++++++++++++++++++++++++++
 1 file changed, 91 insertions(+)

diff --git a/os/hal/ports/KINETIS/MK66F18/hal_lld.c b/os/hal/ports/KINETIS/MK66F18/hal_lld.c
index addd23f1..9782cc4f 100644
--- a/os/hal/ports/KINETIS/MK66F18/hal_lld.c
+++ b/os/hal/ports/KINETIS/MK66F18/hal_lld.c
@@ -12,6 +12,34 @@
     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     See the License for the specific language governing permissions and
     limitations under the License.
+
+    Portions Copyright (C) 2017 PJRC.COM, LLC.
+
+    Permission is hereby granted, free of charge, to any person obtaining
+    a copy of this software and associated documentation files (the
+    "Software"), to deal in the Software without restriction, including
+    without limitation the rights to use, copy, modify, merge, publish,
+    distribute, sublicense, and/or sell copies of the Software, and to
+    permit persons to whom the Software is furnished to do so, subject to
+    the following conditions:
+    
+    1. The above copyright notice and this permission notice shall be
+    included in all copies or substantial portions of the Software.
+    
+    2. If the Software is incorporated into a build system that allows
+    selection among a list of target devices, then similar target
+    devices manufactured by PJRC.COM must be included in the list of
+    target devices and selectable in the same manner.
+    
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+    BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+    ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+    CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+
 */
 
 /**
@@ -112,6 +140,12 @@ void MK66F18_clock_init(void) {
                 SIM_SCGC5_PORTD |
                 SIM_SCGC5_PORTE;
 
+  /* Ported from the Teensyduino Core Library at
+     https://github.com/PaulStoffregen/cores/blob/master/teensy3/mk20dx128.c
+   */    
+  /* Allow the MCU to enter High Speed Run mode (HSRUN) */
+  SMC->PMPROT = SMC_PMPROT_AHSRUN_SET(1) | SMC_PMPROT_AVLP_SET(1) | SMC_PMPROT_ALLS_SET(1) | SMC_PMPROT_AVLLS_SET(1);
+
 #if KINETIS_MCG_MODE == KINETIS_MCG_MODE_FEI
   /* This is the default mode at reset. */
 
@@ -182,6 +216,62 @@ void MK66F18_clock_init(void) {
   /* Wait for the MCGOUTCLK to use the oscillator */
   while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST_SET(2));
 
+  /* Ported from the Teensyduino Core Library at
+     https://github.com/PaulStoffregen/cores/blob/master/teensy3/mk20dx128.c
+   */    
+#define F_CPU KINETIS_SYSCLK_FREQUENCY
+#define MCG_C5 MCG->C5
+#undef MCG_C5_PRDIV0
+#define MCG_C5_PRDIV0 MCG_C5_PRDIV0_SET
+#define MCG_C6 MCG->C6
+#undef MCG_C6_VDIV0
+#define MCG_C6_VDIV0 MCG_C6_VDIV0_SET
+#if 1 /* PJRC_HSRUN */
+  // if we need faster than the crystal, turn on the PLL
+
+  SMC->PMCTRL = SMC_PMCTRL_RUNM_SET(3); // enter HSRUN mode
+  #define SMC_PMSTAT_HSRUN		((uint8_t)0x80)
+  while (SMC->PMSTAT != SMC_PMSTAT_HSRUN)
+    ; // wait for HSRUN
+
+                #if F_CPU == 256000000
+        //See table in 27.4.6 MCG Control 6 Register (MCG_C6)
+        //16 -> Multiply factor 32. 32*8MHz =256MHz
+        MCG_C5 = MCG_C5_PRDIV0(0);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(16);
+    #elif F_CPU == 240000000
+        MCG_C5 = MCG_C5_PRDIV0(0);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(14);
+    #elif F_CPU == 216000000
+        MCG_C5 = MCG_C5_PRDIV0(0);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(11);
+    #elif F_CPU == 192000000
+        MCG_C5 = MCG_C5_PRDIV0(0);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(8);
+    #elif F_CPU == 180000000
+        MCG_C5 = MCG_C5_PRDIV0(1);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(29);
+    #elif F_CPU == 168000000
+        MCG_C5 = MCG_C5_PRDIV0(0);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(5);
+    #elif F_CPU == 144000000
+        MCG_C5 = MCG_C5_PRDIV0(0);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(2);
+    #elif F_CPU == 120000000
+        MCG_C5 = MCG_C5_PRDIV0(1);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(14);
+    #elif F_CPU == 96000000 || F_CPU == 48000000 || F_CPU == 24000000
+        MCG_C5 = MCG_C5_PRDIV0(1);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(8);
+    #elif F_CPU == 72000000
+        MCG_C5 = MCG_C5_PRDIV0(1);
+        MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0(2);
+    #elif F_CPU > 16000000
+    #error "MK66FX1M0 does not support this clock speed yet...."
+    #endif
+
+#else /* PJRC_HSRUN */
+
   /*
    * Now in FBE mode
    */
@@ -208,6 +298,7 @@ void MK66F18_clock_init(void) {
 
   if(i>=56)  /* Config PLL for 96 MHz output as default setting */
     MCG->C6 = MCG_C6_PLLS | MCG_C6_VDIV0_SET(0);
+#endif /* PJRC_HSRUN */
 
   /* Wait for PLL to start using crystal as its input, and to lock */
   while ((MCG->S & (MCG_S_PLLST|MCG_S_LOCK0))!=(MCG_S_PLLST|MCG_S_LOCK0));