Add clocking function, fix LED bug

examples/mco.rs

 // #![deny(unsafe_code)]
-#![deny(warnings)]
+// #![deny(warnings)]
 #![feature(proc_macro)]
 #![no_std]
 
@@ -9,75 +9,44 @@ extern crate f4;
 
 use rtfm::app;
 use f4::led::{self, LED};
-use f4::frequency::apb1;
+use f4::clock;
 
 // TASKS & RESOURCES
 app! {
     device: f4::stm32f40x,
 }
 
-fn clk_init(p: &init::Peripherals) {
-    // setting up the flash memory latency
-    // RM0368 8.4.1 (register), 3.4 Table 6
-    // we assume 3.3 volt operation, thus 2 cycles for 84mHz
-    // apb1 will be at 42 MHz
-    ::apb1::set_frequency(42_000_000);
-    p.FLASH.acr.modify(|_, w| unsafe { w.latency().bits(2) });
-    println!("Flash latency! {:x}", p.FLASH.acr.read().latency().bits());
-
-    p.RCC
-        .cfgr
-        .modify(|_, w| w.sw0().clear_bit().sw1().clear_bit()); //Switch to HSI
-    p.RCC.cfgr.modify(|_, w| unsafe { w.ppre1().bits(4) }); //Configure apb1 prescaler = 2,
-    p.RCC.apb1enr.modify(|_, w| w.pwren().set_bit());
-    p.RCC.cr.write(|w| w.pllon().clear_bit());
-
-    //Enable PLL
-    //					   PP	  PLLN	    PLLM
-    // 0b0000 0000 0000 00 01 0 101010000 010000
-    // RM0368 6.3.2
-    // PP 01
-    p.RCC
-        .pllcfgr
-        .write(|w| unsafe { w.bits(0b00000000000000010101010000010000) }); //Configure PLL
-
-    p.RCC.cr.modify(|_, w| w.pllon().set_bit()); //Enable PLL
-
-    while p.RCC.cr.read().pllrdy().bit_is_clear() {}
-
-    p.RCC
-        .cfgr
-        .modify(|_, w| w.sw0().clear_bit().sw1().set_bit()); //Switch to PLL
-
-    // System configuration controller clock enable
-    p.RCC.apb2enr.modify(|_, w| w.syscfgen().set_bit());
-
-    p.RCC.ahb1enr.modify(|_, w| w.gpioaen().set_bit()); //Enable GPIOA clock
-    p.RCC.ahb1enr.modify(|_, w| w.gpioben().set_bit()); //Enable GPIOB clock
-}
-
 // INITIALIZATION PHASE
 fn init(p: init::Peripherals) {
+    // RM0368 6.2.10
+    // Configure PA8 as MCO_1 alternate function to output HSI clock
+    p.RCC.ahb1enr.modify(|_, w| w.gpioaen().set_bit()); //Enable GPIOA clock
+    p.GPIOA.ospeedr.modify(|_,w| w.ospeedr8().bits(0b11)); //Highest output speed
+    p.GPIOA.afrh.modify(|_,w| w.afrh8().bits(0)); //Alternate function AF0 MCO_1 on pin 8
+    p.GPIOA.moder.modify(|_,w| w.moder8().bits(0b10)); //Alternate function push-pull
+    p.RCC.cfgr.modify(|_, w| unsafe {w.mco1().bits(0b00)}); //HSI clock selected
+    p.RCC.cfgr.modify(|_, w| unsafe {w.mco1pre().bits(0)}); //No division
 
-    // Configure PC9 as MCO_2 alternate function to output SYSCLK
+    // Configure PC9 as MCO_2 alternate function to output System clock
     p.RCC.ahb1enr.modify(|_, w| w.gpiocen().set_bit()); //Enable GPIOC clock
     p.GPIOC.ospeedr.modify(|_,w| unsafe {w.ospeedr9().bits(0b11)}); //Highest output speed
-    p.GPIOC.afrh.modify(|_,w| unsafe {w.afrh9().bits(0)}); //Alternate function AF00 MCO_2 on pin 9
+    p.GPIOC.afrh.modify(|_,w| unsafe {w.afrh9().bits(0)}); //Alternate function AF0 MCO_2 on pin 9
     p.GPIOC.moder.modify(|_,w| unsafe {w.moder9().bits(0b10)}); //Alternate function push-pull
-    p.RCC.cfgr.modify(|_, w| unsafe {w.mco2().bits(0)}); //MCO2 SYSCLK clock selected
-    p.RCC.cfgr.modify(|_, w| unsafe {w.mco2pre().bits(0b111)}); //MCO2 SYSCLK clock selected
+    p.RCC.cfgr.modify(|_, w| unsafe {w.mco2().bits(0b00)}); //MCO2 SYSCLK clock selected
+    p.RCC.cfgr.modify(|_, w| unsafe {w.mco2pre().bits(0b110)}); //Divide SYSCLK by 4
 
-    clk_init(&p);
+    let sysclk = clock::set_100_mhz(&p.RCC, &p.FLASH);
+    println!("SYSCLK set to {}", sysclk);
+    // PC9 should now output sysclk/4 MHz and PA8 the frequency of the HSI RC
 
     led::init(&p.GPIOA, &p.RCC);
 }
 
 // IDLE LOOP
 fn idle() -> ! {
+    LED.on();
     // Sleep
     loop {
-        LED.on();
         rtfm::wfi();
-        LED.off();
     }
 }

src/capture.rs

@@ -36,7 +36,7 @@
 use core::any::{Any, TypeId};
 use core::u32;
 
-use cast::{u16, u32};
+use cast::{u32};
 use hal;
 use nb;
 use stm32f40x::{TIM1, TIM2, TIM3, TIM4, GPIOA, GPIOB, GPIOC, RCC};

src/clock.rs

+//! System clocking
+
+use stm32f40x::{FLASH, RCC};
+
+const HSI_FREQ: u32 = 16_000_000;
+
+fn calculate_pll(m: u8, n: u16, p: u8) -> (u32, u32) {
+    // RM0368 6.3.2
+
+    let pllm_output = match m {
+        2...63 => match HSI_FREQ / m as u32 {
+            950_000...2_100_000 => HSI_FREQ / m as u32,
+            _ => panic!("Invalid PLLM output frequency"),
+        },
+        _ => panic!("Invalid PLLM divisor"),
+    };
+    let vco_clock = match n {
+        50...432 => match pllm_output * n as u32 {
+            100_000_000...432_000_000 => pllm_output * n as u32,
+            _ => panic!("Invalid PLLN output frequency"),
+        },
+        _ => panic!("Invalid PLLN multiplier"),
+    };
+    let log2p = match p {
+        2 => 0b00,
+        4 => 0b01,
+        6 => 0b10,
+        8 => 0b11,
+        _ => panic!("Invalid PLLP divisor"),
+    };
+    let pll_output = match vco_clock / p as u32 {
+        24_000_000...100_000_000 => vco_clock / p as u32,
+        _ => panic!("Invalid PLLP output frequency"),
+    };
+    let pll_bitmask = ((log2p as u32) << 16) | ((n as u32) << 6) | (m as u32);
+
+    (pll_bitmask, pll_output)
+}
+
+
+
+/// Set system clock
+pub fn set(rcc: &RCC, flash: &FLASH, m: u8, n: u16, p: u8) -> u32 {
+    let (pll_bitmask, sysclk) = calculate_pll(m, n, p);
+    // let ahb prescaler = 1, then
+    let hclk = sysclk;
+
+    // setting up the flash memory latency
+    // RM0368 8.4.1 (register), 3.4 Table 6
+    // apb1 will be at 42 MHz
+    rcc.cfgr.modify(|_, w| unsafe { w.ppre1().bits(4) }); //Configure apb1 prescaler = 2,
+    ::apb1::set_frequency(hclk / 2);
+
+    // we assume 3.3 volt operation, thus 2 cycles for 84MHz
+    flash.acr.modify(|_, w| unsafe {
+        w.latency().bits(match hclk {
+            0...30_000_000 => 0,
+            30_000_000...64_000_000 => 1,
+            64_000_000...90_000_000 => 2,
+            90_000_000...100_000_000 => 3,
+            _ => panic!("Invalid HCLK frequency"),
+        })
+    });
+    // println!("Flash latency! {:x}", p.FLASH.acr.read().latency().bits());
+
+    rcc.cfgr
+        .modify(|_, w| w.sw0().clear_bit().sw1().clear_bit()); //Switch to HSI
+    rcc.apb1enr.modify(|_, w| w.pwren().set_bit());
+    rcc.cr.write(|w| w.pllon().clear_bit());
+
+    //Enable PLL
+    rcc.pllcfgr.write(|w| unsafe { w.bits(pll_bitmask) }); //Configure PLL
+
+    rcc.cr.modify(|_, w| w.pllon().set_bit()); //Enable PLL
+
+    while rcc.cr.read().pllrdy().bit_is_clear() {}
+
+    rcc.cfgr.modify(|_, w| w.sw0().clear_bit().sw1().set_bit()); //Switch to PLL
+
+    // System configuration controller clock enable
+    rcc.apb2enr.modify(|_, w| w.syscfgen().set_bit());
+
+    rcc.ahb1enr.modify(|_, w| w.gpioaen().set_bit()); //Enable GPIOA clock
+    rcc.ahb1enr.modify(|_, w| w.gpioben().set_bit()); //Enable GPIOB clock
+    hclk
+}
+
+/// Set system clock to 100 MHz
+pub fn set_100_mhz(rcc: &RCC, flash: &FLASH) -> u32 {
+    set(rcc, flash, 16, 400, 4)
+}
+/// Set system clock to 84 MHz
+pub fn set_84_mhz(rcc: &RCC, flash: &FLASH) -> u32 {
+    set(rcc, flash, 16, 336, 4)
+}

src/led.rs

-//! User LEDs
-//!
-//! - PA5
+//! User LED PA5
 
 use stm32f40x::{GPIOA, RCC};
 
-/// LED connected to pin PC13
+/// LED connected to pin PA5
 pub const LED: PA5 = PA5;
 
 /// Pin PA5. There's an LED connected to this pin
@@ -12,25 +10,25 @@ pub struct PA5;
 
 /// Initializes the user LED
 pub fn init(gpioa: &GPIOA, rcc: &RCC) {
-    // power on GPIOC
+    // power on GPIOA
     rcc.ahb1enr.modify(|_, w| w.gpioaen().set_bit());
 
-    // configure PC13 as output
-    gpioa.moder.write(|w| w.moder5().bits(1));
+    // configure PA5 as output
+    gpioa.moder.modify(|_,w| w.moder5().bits(1));
 }
 
 impl PA5 {
     /// Turns the LED on
     pub fn on(&self) {
         unsafe {
-            (*GPIOA.get()).odr.write(|w| w.odr5().bit(false));
+            (*GPIOA.get()).odr.modify(|_,w| w.odr5().bit(true));
         }
     }
 
     /// Turns the LED off
     pub fn off(&self) {
         unsafe {
-            (*GPIOA.get()).odr.write(|w| w.odr5().bit(true));
+            (*GPIOA.get()).odr.modify(|_,w| w.odr5().bit(false));
         }
     }
 }

src/lib.rs

@@ -43,6 +43,7 @@ pub mod timer;
 pub mod time;
 pub mod pwm;
 pub mod capture;
+pub mod clock;
 
 pub mod frequency;
 use frequency::*;