Draft: Dynamic Clock Speed

.gitignore

@@ -0,0 +1,5 @@
+Cargo.lock
+target/
+.cargo/
+.github/
+.idea/

Cargo.toml

@@ -5,12 +5,14 @@ categories = ["embedded", "no-std"]
 authors = [
 	"Thomas Campistron <[email protected]>",
 	"Debilausaure",
+	"Devin Brite <[email protected]>"
 ]
 edition = "2018"
 
 [dependencies]
 volatile = "*"
 bit_field = "*"
+r0 = "0.2.2"
 
 [features]
 default = []

examples/blink_dynamic_clocks.rs

@@ -0,0 +1,33 @@
+#![feature(stdsimd)]
+#![no_std]
+#![no_main]
+
+use teensy::mcg::{CpuFreq, Mcg};
+use teensy::sim::Sim;
+use teensy::*;
+
+define_panic! {blink}
+
+#[no_mangle]
+fn main() {
+    let mut led = unsafe { make_pin!(led).make_gpio().with_output() };
+
+    loop {
+        change_clocks(CpuFreq::High);
+        led.toggle();
+        sleep::delay(72_000_000 * 5); // 5s at 72MHz -> 3.75s at 96MHz
+        led.toggle();
+        sleep::delay(72_000_000 * 5); // 5s at 72MHz -> 3.75s at 96MHz
+
+        change_clocks(CpuFreq::Reduced);
+        led.toggle();
+        sleep::delay(72_000_000 * 5); // 5s at 72MHz -> 7.5s at 48MHz
+        led.toggle();
+        sleep::delay(72_000_000 * 5); // 5s at 72MHz -> 7.5s at 48MHz
+    }
+}
+
+fn change_clocks(freq: mcg::CpuFreq) {
+    let (sim, mcg): (&mut Sim, &mut Mcg) = unsafe { (sim::Sim::new(), mcg::Mcg::new()) };
+    mcg.set_clocks(freq, sim);
+}

layout.ld

@@ -24,28 +24,41 @@ SECTIONS
 {
     PROVIDE(_stack_top = ORIGIN(RAM) + LENGTH(RAM));
 
-    .vector_table ORIGIN(FLASH) : {
+	.text ORIGIN(FLASH) : {
         /* Initial stack pointer */
         LONG(_stack_top);
 
+        /* TODO: add other items from vector_table before interrupts */
         /* Interrupts */
         KEEP(*(.vector_table.interrupts));
-    } > FLASH
-
-
-
-	.text : {
 		. = 0x400;
 		KEEP(*(.flashconfig*))
 		. = ALIGN(4);
 		*(.text*)
-    } > FLASH = 0xFF
+    } > FLASH
 
     .rodata : ALIGN(4){
 	    *(.rodata .rodata.*);
 	    . = ALIGN(4);
     } > FLASH
 
+    /* uninitialized static vars */
+    .bss : ALIGN(4)
+    {
+        _sbss = .;
+        *(.bss.*);
+        _ebss = ALIGN(4);
+    } > RAM
+
+    /* initialized static vars */
+    .data : ALIGN(4)
+    {
+        _sdata = .;
+        *(.data.*);
+        _edata = ALIGN(4);
+    } > RAM AT > FLASH
+    _sidata = LOADADDR(.data);
+
     /DISCARD/ : {
 	    *(.ARM.*)
     }

src/boot.rs

@@ -1,10 +1,12 @@
+#![no_builtins]
 //! Here is the most important file of this crate.
 //! When you add this crate as a dependency it will move the bootloader to the good section. Enable
 //! **all** the port, the clock at 72MHz and disable the watchdog and **then** call your `main`.
 //! Right now if you don't want something you'll need to either disable it in your `main` or edit
 //! this crate.
 
 use crate::*;
+use r0::{init_data, zero_bss};
 
 /// The first function to be executed by the teensy
 /// Enable all the clocks:
@@ -15,8 +17,18 @@ use crate::*;
 /// use all the ports.
 /// Disable the watchdog.
 #[no_mangle]
-extern "C" fn __boot() {
+extern "C" fn __reset() {
+    extern "C" {
+        static mut _sbss: u32;
+        static mut _ebss: u32;
+        static mut _sdata: u32;
+        static mut _edata: u32;
+        static _sidata: u32;
+    }
+
     unsafe {
+        zero_bss(&mut _sbss, &mut _ebss);
+        init_data(&mut _sdata, &mut _edata, &_sidata);
         init();
         main();
     }
@@ -26,17 +38,17 @@ extern "C" fn __boot() {
 #[cfg(not(feature = "manual_init"))]
 #[no_mangle]
 fn init() {
-    let (wdog, sim, mcg, osc) = unsafe {
+    let (sim, mcg, osc) = unsafe {
+
+        watchdog::Watchdog::new().disable();
+
         (
-            watchdog::Watchdog::new(),
             sim::Sim::new(),
             mcg::Mcg::new(),
             osc::Osc::new(),
         )
     };
 
-    wdog.disable();
-
     // Enable the crystal oscillator with 10pf of capacitance
     osc.enable(10);
     // Turn on all the port clock gate
@@ -46,33 +58,14 @@ fn init() {
     sim.enable_clock(sim::Clock::PortD);
     sim.enable_clock(sim::Clock::PortE);
 
-    // Set our clocks:
-    // core: 72Mhz
-    // peripheral: 36MHz
-    // flash: 24MHz
-    sim.set_dividers(1, 2, 3);
-    // We would also set the USB divider here if we wanted to use it.
-
-    // Now we can start setting up the MCG for our needs.
-    if let mcg::Clock::Fei(mut fei) = mcg.clock() {
-        // Our 16MHz xtal is "very fast", and needs to be divided
-        // by 512 to be in the acceptable FLL range.
-        fei.enable_xtal(mcg::OscRange::VeryHigh);
-        let fbe = fei.use_external(512);
-
-        // PLL is 27/6 * xtal == 72MHz
-        let pbe = fbe.enable_pll(27, 6);
-        pbe.use_pll();
-    } else {
-        panic!("Somehow the clock wasn't in FEI mode");
-    }
+    mcg.set_clocks(mcg::CpuFreq::Default, sim);
 }
 
 /// This is the Interrupt Descriptor Table
 #[link_section = ".vector_table.interrupts"]
 #[no_mangle]
 pub static _INTERRUPTS: [unsafe extern "C" fn(); 111] = [
-    __boot, // TODO: Move this to a different vector?
+    __reset, // TODO: Move this to a different vector?
     interrupts::isr_non_maskable,
     interrupts::isr_hard_fault,
     interrupts::isr_memmanage_fault,

src/mcg.rs

@@ -1,10 +1,10 @@
 use core::mem;
 
+use crate::sim;
 use bit_field::BitField;
 use volatile::Volatile;
 
-/// TODO This value should not be hardcoded and should change depending on the choosen frequency
-pub const F_CPU: u32 = 72_000_000;
+pub static mut F_CPU: u32 = 72_000_000;
 
 #[repr(C, packed)]
 pub struct Mcg {
@@ -42,6 +42,14 @@ enum OscSource {
     External = 2,
 }
 
+pub enum CpuFreq {
+    /// Core/Bus/Flash speeds
+    High,    // 96/48/24 (96MHz PLL)
+    Default, // 72/36/24 (72MHz PLL)
+    Reduced, // 48/48/24 (96MHz PLL)
+    Low,     // 24/24/24 (96MHz PLL)
+}
+
 pub struct Fei {
     mcg: &'static mut Mcg,
 }
@@ -107,6 +115,8 @@ pub struct Fbe {
 }
 
 impl Fbe {
+    // numerator / denominator * xtal (16MHz) = clock speed??
+    // enable phase-locked loop
     pub fn enable_pll(self, numerator: u8, denominator: u8) -> Pbe {
         if numerator < 24 || numerator > 55 {
             panic!("Invalid PLL VCO divide factor: {}", numerator);
@@ -117,11 +127,15 @@ impl Fbe {
         }
 
         self.mcg.c5.update(|c5| {
+            // set PLL external reference divide factor
             c5.set_bits(0..5, denominator - 1);
+            // Subtract 1 here to make math easier
         });
 
         self.mcg.c6.update(|c6| {
+            // set VCO divider
             c6.set_bits(0..5, numerator - 24);
+            // select PLL
             c6.set_bit(6, true);
         });
 
@@ -139,7 +153,7 @@ pub struct Pbe {
 }
 
 impl Pbe {
-    pub fn use_pll(self) {
+    pub fn use_pll(self) -> Pee {
         self.mcg.c1.update(|c1| {
             c1.set_bits(6..8, OscSource::LockedLoop as u8);
         });
@@ -151,13 +165,50 @@ impl Pbe {
         // which would be invalid to set, we just check for the known
         // value "3" here.
         while self.mcg.s.read().get_bits(2..4) != 3 {}
+        Pee { mcg: self.mcg }
+    }
+
+    pub fn disable_pll(self) -> Fbe {
+        self.mcg.c6.update(|c6| {
+            // select FLL
+            c6.set_bit(6, false);
+        });
+
+        // Wait for PLL to be disabled
+        while self.mcg.s.read().get_bit(5) {}
+        // Wait for the PLL to be "unlocked"
+        while self.mcg.s.read().get_bit(6) {}
+
+        Fbe { mcg: self.mcg }
+    }
+}
+
+pub struct Pee {
+    mcg: &'static mut Mcg,
+}
+
+impl Pee {
+    fn use_external(self) -> Pbe {
+        self.mcg.c1.update(|c1| {
+            c1.set_bits(6..8, OscSource::External as u8);
+        });
+
+        // mcg.c1 and mcg.s have slightly different behaviors.  In c1,
+        // we use one value to indicate "Use whichever LL is
+        // enabled". In s, it is differentiated between the FLL at 0,
+        // and the PLL at 3. Instead of adding a value to OscSource
+        // which would be invalid to set, we just check for the known
+        // value "0" here.
+        while self.mcg.s.read().get_bits(2..4) != 2 {}
+        return Pbe { mcg: self.mcg };
     }
 }
 
 pub enum Clock {
     Fei(Fei),
     Fbe(Fbe),
     Pbe(Pbe),
+    Pee(Pee),
 }
 
 impl Mcg {
@@ -166,11 +217,82 @@ impl Mcg {
         let fll_internal = self.c1.read().get_bit(2);
         let pll_enabled = self.c6.read().get_bit(6);
 
+        // TODO: match all possible MCG clock modes before panic
         match (fll_internal, pll_enabled, source) {
             (true, false, OscSource::LockedLoop) => Clock::Fei(Fei { mcg: self }),
             (false, false, OscSource::External) => Clock::Fbe(Fbe { mcg: self }),
             (_, true, OscSource::External) => Clock::Pbe(Pbe { mcg: self }),
+            (_, true, OscSource::LockedLoop) => Clock::Pee(Pee { mcg: self }),
             _ => panic!("The current clock mode cannot be represented as a known struct"),
         }
     }
+
+    pub fn set_clocks(&'static mut self, clock: CpuFreq, sim: &mut sim::Sim) -> Pee {
+        match clock {
+            CpuFreq::High => {
+                unsafe {
+                    F_CPU = 96_000_000;
+                }
+                // Set our clocks: 96/48/24
+                sim.set_dividers(1, 2, 4);
+                // We would also set the USB divider here if we wanted to use it.
+                let fbe = self.mode_to_fbe();
+                let pbe = fbe.enable_pll(24, 4); // 16MHz / 4 * 24 = 96MHz
+                pbe.use_pll()
+            }
+            CpuFreq::Default => {
+                unsafe {
+                    F_CPU = 72_000_000;
+                }
+                // Set our clocks: 72/36/24
+                sim.set_dividers(1, 2, 3);
+                // We would also set the USB divider here if we wanted to use it.
+                let fbe = self.mode_to_fbe();
+                let pbe = fbe.enable_pll(27, 6); // 16MHz / 6 * 27 = 72MHz
+                pbe.use_pll()
+            }
+            CpuFreq::Reduced => {
+                unsafe {
+                    F_CPU = 48_000_000;
+                }
+                // Set our clocks: 48/48/24
+                sim.set_dividers(2, 2, 4);
+                // We would also set the USB divider here if we wanted to use it.
+                let fbe = self.mode_to_fbe();
+                let pbe = fbe.enable_pll(24, 4); // 16MHz / 4 * 24 = 96MHz
+                pbe.use_pll()
+            }
+            CpuFreq::Low => {
+                unsafe {
+                    F_CPU = 24_000_000;
+                }
+                // Set our clocks: 24/24/24
+                sim.set_dividers(4, 4, 4);
+                // We would also set the USB divider here if we wanted to use it.
+                let fbe = self.mode_to_fbe();
+                let pbe = fbe.enable_pll(24, 4); // 16MHz / 4 * 24 = 96MHz
+                pbe.use_pll()
+            }
+        }
+    }
+
+    pub fn mode_to_fbe(&'static mut self) -> Fbe {
+        return match self.clock() {
+            Clock::Fei(mut fei) => {
+                // Our 16MHz xtal is "very fast", and needs to be divided
+                // by 512 to be in the acceptable FLL range.
+                // 31.25 kHz to 39.0625 kHz
+                fei.enable_xtal(OscRange::VeryHigh);
+                // (literally the only valid divisor on teensy)
+                fei.use_external(512) // 16MHz/512 = 31.25KHz
+            }
+            Clock::Fbe(fbe) => fbe,
+            Clock::Pbe(pbe) => pbe.disable_pll(),
+            Clock::Pee(pee) => {
+                let pbe = pee.use_external();
+                let fbe = pbe.disable_pll();
+                return fbe;
+            }
+        };
+    }
 }

src/panic.rs

@@ -62,7 +62,7 @@ macro_rules! blink_panic {
 
             loop {
                 led.toggle();
-                sleep::sleep_ms(500);
+                sleep::sleep_ms(50);
             }
         }
     };