5 files changed, 371 insertions, 2 deletions

diff --git a/embassy-stm32/build.rs b/embassy-stm32/build.rs
index 3780c5a40..481fec677 100644
--- a/embassy-stm32/build.rs
+++ b/embassy-stm32/build.rs
@@ -50,10 +50,13 @@ fn main() {
                 // We *shouldn't* have singletons for these, but the HAL currently requires
                 // singletons, for using with RccPeripheral to enable/disable clocks to them.
                 "rcc" => {
-                    if r.version.starts_with("h7") {
+                    if r.version.starts_with("h7") || r.version.starts_with("f4") {
                         singletons.push("MCO1".to_string());
                         singletons.push("MCO2".to_string());
                     }
+                    if r.version.starts_with("l4") {
+                        singletons.push("MCO".to_string());
+                    }
                     singletons.push(p.name.to_string());
                 }
                 //"dbgmcu" => {}
@@ -258,6 +261,7 @@ fn main() {
         (("i2c", "SCL"), quote!(crate::i2c::SclPin)),
         (("rcc", "MCO_1"), quote!(crate::rcc::McoPin)),
         (("rcc", "MCO_2"), quote!(crate::rcc::McoPin)),
+        (("rcc", "MCO"), quote!(crate::rcc::McoPin)),
         (("dcmi", "D0"), quote!(crate::dcmi::D0Pin)),
         (("dcmi", "D1"), quote!(crate::dcmi::D1Pin)),
         (("dcmi", "D2"), quote!(crate::dcmi::D2Pin)),
@@ -447,13 +451,22 @@ fn main() {
                     // MCO is special
                     if pin.signal.starts_with("MCO_") {
                         // Supported in H7 only for now
-                        if regs.version.starts_with("h7") {
+                        if regs.version.starts_with("h7") || regs.version.starts_with("f4") {
                             peri = format_ident!("{}", pin.signal.replace("_", ""));
                         } else {
                             continue;
                         }
                     }
 
+                    if pin.signal == "MCO" {
+                        // Supported in H7 only for now
+                        if regs.version.starts_with("l4") {
+                            peri = format_ident!("MCO");
+                        } else {
+                            continue;
+                        }
+                    }
+
                     g.extend(quote! {
                         pin_trait_impl!(#tr, #peri, #pin_name, #af);
                     })
diff --git a/embassy-stm32/src/rcc/f4.rs b/embassy-stm32/src/rcc/f4.rs
index 200bcce9c..d0e0d585a 100644
--- a/embassy-stm32/src/rcc/f4.rs
+++ b/embassy-stm32/src/rcc/f4.rs
@@ -1,8 +1,16 @@
+use core::marker::PhantomData;
+
+use embassy_hal_common::into_ref;
+use stm32_metapac::rcc::vals::{Mco1, Mco2, Mcopre};
+
 use super::sealed::RccPeripheral;
+use crate::gpio::sealed::AFType;
+use crate::gpio::Speed;
 use crate::pac::rcc::vals::{Hpre, Ppre, Sw};
 use crate::pac::{FLASH, PWR, RCC};
 use crate::rcc::{set_freqs, Clocks};
 use crate::time::Hertz;
+use crate::{peripherals, Peripheral};
 
 /// HSI speed
 pub const HSI_FREQ: Hertz = Hertz(16_000_000);
@@ -96,6 +104,163 @@ unsafe fn setup_pll(pllsrcclk: u32, use_hse: bool, pllsysclk: Option<u32>, pll48
     }
 }
 
+pub enum McoClock {
+    DIV1,
+    DIV2,
+    DIV3,
+    DIV4,
+    DIV5,
+}
+
+impl McoClock {
+    fn into_raw(&self) -> Mcopre {
+        match self {
+            McoClock::DIV1 => Mcopre::DIV1,
+            McoClock::DIV2 => Mcopre::DIV2,
+            McoClock::DIV3 => Mcopre::DIV3,
+            McoClock::DIV4 => Mcopre::DIV4,
+            McoClock::DIV5 => Mcopre::DIV5,
+        }
+    }
+}
+
+#[derive(Copy, Clone)]
+pub enum Mco1Source {
+    Hsi,
+    Lse,
+    Hse,
+    Pll,
+}
+
+impl Default for Mco1Source {
+    fn default() -> Self {
+        Self::Hsi
+    }
+}
+
+pub trait McoSource {
+    type Raw;
+
+    fn into_raw(&self) -> Self::Raw;
+}
+
+impl McoSource for Mco1Source {
+    type Raw = Mco1;
+    fn into_raw(&self) -> Self::Raw {
+        match self {
+            Mco1Source::Hsi => Mco1::HSI,
+            Mco1Source::Lse => Mco1::LSE,
+            Mco1Source::Hse => Mco1::HSE,
+            Mco1Source::Pll => Mco1::PLL,
+        }
+    }
+}
+
+#[derive(Copy, Clone)]
+pub enum Mco2Source {
+    SysClk,
+    Plli2s,
+    Hse,
+    Pll,
+}
+
+impl Default for Mco2Source {
+    fn default() -> Self {
+        Self::SysClk
+    }
+}
+
+impl McoSource for Mco2Source {
+    type Raw = Mco2;
+    fn into_raw(&self) -> Self::Raw {
+        match self {
+            Mco2Source::SysClk => Mco2::SYSCLK,
+            Mco2Source::Plli2s => Mco2::PLLI2S,
+            Mco2Source::Hse => Mco2::HSE,
+            Mco2Source::Pll => Mco2::PLL,
+        }
+    }
+}
+
+pub(crate) mod sealed {
+    use stm32_metapac::rcc::vals::Mcopre;
+    pub trait McoInstance {
+        type Source;
+        unsafe fn apply_clock_settings(source: Self::Source, prescaler: Mcopre);
+    }
+}
+
+pub trait McoInstance: sealed::McoInstance + 'static {}
+
+pin_trait!(McoPin, McoInstance);
+
+impl sealed::McoInstance for peripherals::MCO1 {
+    type Source = Mco1;
+    unsafe fn apply_clock_settings(source: Self::Source, prescaler: Mcopre) {
+        RCC.cfgr().modify(|w| {
+            w.set_mco1(source);
+            w.set_mco1pre(prescaler);
+        });
+        match source {
+            Mco1::PLL => {
+                RCC.cr().modify(|w| w.set_pllon(true));
+                while !RCC.cr().read().pllrdy() {}
+            }
+            Mco1::HSI => {
+                RCC.cr().modify(|w| w.set_hsion(true));
+                while !RCC.cr().read().hsirdy() {}
+            }
+            _ => {}
+        }
+    }
+}
+impl McoInstance for peripherals::MCO1 {}
+
+impl sealed::McoInstance for peripherals::MCO2 {
+    type Source = Mco2;
+    unsafe fn apply_clock_settings(source: Self::Source, prescaler: Mcopre) {
+        RCC.cfgr().modify(|w| {
+            w.set_mco2(source);
+            w.set_mco2pre(prescaler);
+        });
+        match source {
+            Mco2::PLL => {
+                RCC.cr().modify(|w| w.set_pllon(true));
+                while !RCC.cr().read().pllrdy() {}
+            }
+            Mco2::PLLI2S => {
+                RCC.cr().modify(|w| w.set_plli2son(true));
+                while !RCC.cr().read().plli2srdy() {}
+            }
+            _ => {}
+        }
+    }
+}
+impl McoInstance for peripherals::MCO2 {}
+
+pub struct Mco<'d, T: McoInstance> {
+    phantom: PhantomData<&'d mut T>,
+}
+
+impl<'d, T: McoInstance> Mco<'d, T> {
+    pub fn new(
+        _peri: impl Peripheral<P = T> + 'd,
+        pin: impl Peripheral<P = impl McoPin<T>> + 'd,
+        source: impl McoSource<Raw = T::Source>,
+        prescaler: McoClock,
+    ) -> Self {
+        into_ref!(pin);
+
+        critical_section::with(|_| unsafe {
+            T::apply_clock_settings(source.into_raw(), prescaler.into_raw());
+            pin.set_as_af(pin.af_num(), AFType::OutputPushPull);
+            pin.set_speed(Speed::VeryHigh);
+        });
+
+        Self { phantom: PhantomData }
+    }
+}
+
 unsafe fn flash_setup(sysclk: u32) {
     use crate::pac::flash::vals::Latency;
 
diff --git a/embassy-stm32/src/rcc/l4.rs b/embassy-stm32/src/rcc/l4.rs
index e650490fe..c1bf7d0cd 100644
--- a/embassy-stm32/src/rcc/l4.rs
+++ b/embassy-stm32/src/rcc/l4.rs
@@ -1,7 +1,15 @@
+use core::marker::PhantomData;
+
+use embassy_hal_common::into_ref;
+use stm32_metapac::rcc::vals::{Mcopre, Mcosel};
+
+use crate::gpio::sealed::AFType;
+use crate::gpio::Speed;
 use crate::pac::rcc::vals::{Hpre, Msirange, Pllsrc, Ppre, Sw};
 use crate::pac::{FLASH, RCC};
 use crate::rcc::{set_freqs, Clocks};
 use crate::time::Hertz;
+use crate::{peripherals, Peripheral};
 
 /// HSI speed
 pub const HSI_FREQ: Hertz = Hertz(16_000_000);
@@ -298,6 +306,131 @@ impl Default for Config {
     }
 }
 
+pub enum McoClock {
+    DIV1,
+    DIV2,
+    DIV4,
+    DIV8,
+    DIV16,
+}
+
+impl McoClock {
+    fn into_raw(&self) -> Mcopre {
+        match self {
+            McoClock::DIV1 => Mcopre::DIV1,
+            McoClock::DIV2 => Mcopre::DIV2,
+            McoClock::DIV4 => Mcopre::DIV4,
+            McoClock::DIV8 => Mcopre::DIV8,
+            McoClock::DIV16 => Mcopre::DIV16,
+        }
+    }
+}
+
+#[derive(Copy, Clone)]
+pub enum Mco1Source {
+    Disabled,
+    Lse,
+    Lsi,
+    Hse,
+    Hsi16,
+    PllClk,
+    SysClk,
+    Msi,
+    #[cfg(not(any(stm32l471, stm32l475, stm32l476, stm32l486)))]
+    Hsi48,
+}
+
+impl Default for Mco1Source {
+    fn default() -> Self {
+        Self::Hsi16
+    }
+}
+
+pub trait McoSource {
+    type Raw;
+
+    fn into_raw(&self) -> Self::Raw;
+}
+
+impl McoSource for Mco1Source {
+    type Raw = Mcosel;
+    fn into_raw(&self) -> Self::Raw {
+        match self {
+            Mco1Source::Disabled => Mcosel::NOCLOCK,
+            Mco1Source::Lse => Mcosel::LSE,
+            Mco1Source::Lsi => Mcosel::LSI,
+            Mco1Source::Hse => Mcosel::HSE,
+            Mco1Source::Hsi16 => Mcosel::HSI16,
+            Mco1Source::PllClk => Mcosel::PLL,
+            Mco1Source::SysClk => Mcosel::SYSCLK,
+            Mco1Source::Msi => Mcosel::MSI,
+            #[cfg(not(any(stm32l471, stm32l475, stm32l476, stm32l486)))]
+            Mco1Source::Hsi48 => Mcosel::HSI48,
+        }
+    }
+}
+
+pub(crate) mod sealed {
+    use stm32_metapac::rcc::vals::Mcopre;
+    pub trait McoInstance {
+        type Source;
+        unsafe fn apply_clock_settings(source: Self::Source, prescaler: Mcopre);
+    }
+}
+
+pub trait McoInstance: sealed::McoInstance + 'static {}
+
+pin_trait!(McoPin, McoInstance);
+
+impl sealed::McoInstance for peripherals::MCO {
+    type Source = Mcosel;
+
+    unsafe fn apply_clock_settings(source: Self::Source, prescaler: Mcopre) {
+        RCC.cfgr().modify(|w| {
+            w.set_mcosel(source);
+            w.set_mcopre(prescaler);
+        });
+
+        match source {
+            Mcosel::HSI16 => {
+                RCC.cr().modify(|w| w.set_hsion(true));
+                while !RCC.cr().read().hsirdy() {}
+            }
+            #[cfg(not(any(stm32l471, stm32l475, stm32l476, stm32l486)))]
+            Mcosel::HSI48 => {
+                RCC.crrcr().modify(|w| w.set_hsi48on(true));
+                while !RCC.crrcr().read().hsi48rdy() {}
+            }
+            _ => {}
+        }
+    }
+}
+
+impl McoInstance for peripherals::MCO {}
+
+pub struct Mco<'d, T: McoInstance> {
+    phantom: PhantomData<&'d mut T>,
+}
+
+impl<'d, T: McoInstance> Mco<'d, T> {
+    pub fn new(
+        _peri: impl Peripheral<P = T> + 'd,
+        pin: impl Peripheral<P = impl McoPin<T>> + 'd,
+        source: impl McoSource<Raw = T::Source>,
+        prescaler: McoClock,
+    ) -> Self {
+        into_ref!(pin);
+
+        critical_section::with(|_| unsafe {
+            T::apply_clock_settings(source.into_raw(), prescaler.into_raw());
+            pin.set_as_af(pin.af_num(), AFType::OutputPushPull);
+            pin.set_speed(Speed::VeryHigh);
+        });
+
+        Self { phantom: PhantomData }
+    }
+}
+
 pub(crate) unsafe fn init(config: Config) {
     let (sys_clk, sw) = match config.mux {
         ClockSrc::MSI(range) => {
diff --git a/examples/stm32f4/src/bin/mco.rs b/examples/stm32f4/src/bin/mco.rs
new file mode 100644
index 000000000..5d780f94d
--- /dev/null
+++ b/examples/stm32f4/src/bin/mco.rs
@@ -0,0 +1,31 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_stm32::gpio::{Level, Output, Speed};
+use embassy_stm32::rcc::{Mco, Mco1Source, Mco2Source, McoClock};
+use embassy_time::{Duration, Timer};
+use {defmt_rtt as _, panic_probe as _};
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let p = embassy_stm32::init(Default::default());
+    info!("Hello World!");
+
+
+    let _mco1 = Mco::new(p.MCO1, p.PA8, Mco1Source::Hsi, McoClock::DIV1);
+    let _mco2 = Mco::new(p.MCO2, p.PC9, Mco2Source::Pll, McoClock::DIV4);
+    let mut led = Output::new(p.PB7, Level::High, Speed::Low);
+
+    loop {
+        info!("high");
+        led.set_high();
+        Timer::after(Duration::from_millis(300)).await;
+
+        info!("low");
+        led.set_low();
+        Timer::after(Duration::from_millis(300)).await;
+    }
+}
diff --git a/examples/stm32l4/src/bin/mco.rs b/examples/stm32l4/src/bin/mco.rs
new file mode 100644
index 000000000..dea0c66e0
--- /dev/null
+++ b/examples/stm32l4/src/bin/mco.rs
@@ -0,0 +1,27 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_stm32::gpio::{Level, Output, Speed};
+use embassy_stm32::rcc::{Mco, Mco1Source, McoClock};
+use embassy_time::{Duration, Timer};
+use {defmt_rtt as _, panic_probe as _};
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let p = embassy_stm32::init(Default::default());
+    info!("Hello World!");
+
+    let _mco = Mco::new(p.MCO, p.PA8, Mco1Source::Hsi16, McoClock::DIV1);
+
+    let mut led = Output::new(p.PB14, Level::High, Speed::Low);
+
+    loop {
+        led.set_high();
+        Timer::after(Duration::from_millis(300)).await;
+        led.set_low();
+        Timer::after(Duration::from_millis(300)).await;
+    }
+}