3 files changed, 230 insertions, 8 deletions

diff --git a/embassy-stm32/build.rs b/embassy-stm32/build.rs
index 32115f9a8..517ee57ba 100644
--- a/embassy-stm32/build.rs
+++ b/embassy-stm32/build.rs
@@ -1,3 +1,4 @@
+use std::collections::HashMap;
 use std::env;
 use std::fs;
 use std::path::PathBuf;
@@ -14,14 +15,25 @@ fn main() {
     struct Peripheral {
         kind: String,
         name: String,
+        version: String,
     }
 
+    let mut peripheral_version_mapping = HashMap::<String, String>::new();
+    stm32_metapac::peripheral_versions!(
+        ($peri:ident, $version:ident) => {
+            peripheral_version_mapping.insert(stringify!($peri).to_string(), stringify!($version).to_string());
+            println!("cargo:rustc-cfg={}", stringify!($peri));
+            println!("cargo:rustc-cfg={}_{}", stringify!($peri), stringify!($version));
+        };
+    );
+
     let mut peripherals: Vec<Peripheral> = Vec::new();
     stm32_metapac::peripherals!(
         ($kind:ident, $name:ident) => {
             peripherals.push(Peripheral{
                 kind: stringify!($kind).to_string(),
                 name: stringify!($name).to_string(),
+                version: peripheral_version_mapping[&stringify!($kind).to_ascii_lowercase()].clone()
             });
         };
     );
@@ -43,7 +55,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" => {}
+            "rcc" => {
+                if p.version == "h7" {
+                    singletons.push("MCO1".to_string());
+                    singletons.push("MCO2".to_string());
+                }
+                singletons.push(p.name.clone());
+            }
             //"dbgmcu" => {}
             //"syscfg" => {}
             //"dma" => {}
@@ -78,13 +96,6 @@ fn main() {
     )
     .unwrap();
 
-    stm32_metapac::peripheral_versions!(
-        ($peri:ident, $version:ident) => {
-            println!("cargo:rustc-cfg={}", stringify!($peri));
-            println!("cargo:rustc-cfg={}_{}", stringify!($peri), stringify!($version));
-        };
-    );
-
     let mut s = chip_name.split('_');
     let mut chip_name: String = s.next().unwrap().to_string();
     let core_name = if let Some(c) = s.next() {
diff --git a/embassy-stm32/src/rcc/h7/mod.rs b/embassy-stm32/src/rcc/h7/mod.rs
index 5567a478b..dc458a8a3 100644
--- a/embassy-stm32/src/rcc/h7/mod.rs
+++ b/embassy-stm32/src/rcc/h7/mod.rs
@@ -1,7 +1,11 @@
 use core::marker::PhantomData;
 
 use embassy::util::Unborrow;
+use embassy_hal_common::unborrow;
+use stm32_metapac::rcc::vals::{Mco1, Mco2};
 
+use crate::gpio::sealed::Pin as __GpioPin;
+use crate::gpio::Pin;
 use crate::pac::rcc::vals::Timpre;
 use crate::pac::{RCC, SYSCFG};
 use crate::peripherals;
@@ -508,6 +512,181 @@ impl<'d> Rcc<'d> {
         }
     }
 }
+pub enum McoClock {
+    Disabled,
+    Bypassed,
+    Divided(u8),
+}
+
+impl McoClock {
+    fn into_raw(&self) -> u8 {
+        match self {
+            McoClock::Disabled => 0,
+            McoClock::Bypassed => 1,
+            McoClock::Divided(divisor) => {
+                if *divisor > 15 {
+                    panic!("Mco divisor must be less than 15. Refer to the reference manual for more information.")
+                }
+                *divisor
+            }
+        }
+    }
+}
+
+#[derive(Copy, Clone)]
+pub enum Mco1Source {
+    Hsi,
+    Lse,
+    Hse,
+    Pll1Q,
+    Hsi48,
+}
+
+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::Pll1Q => Mco1::PLL1_Q,
+            Mco1Source::Hsi48 => Mco1::HSI48,
+        }
+    }
+}
+
+#[derive(Copy, Clone)]
+pub enum Mco2Source {
+    SysClk,
+    Pll2Q,
+    Hse,
+    Pll1Q,
+    Csi,
+    Lsi,
+}
+
+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::Pll2Q => Mco2::PLL2_P,
+            Mco2Source::Hse => Mco2::HSE,
+            Mco2Source::Pll1Q => Mco2::PLL1_P,
+            Mco2Source::Csi => Mco2::CSI,
+            Mco2Source::Lsi => Mco2::LSI,
+        }
+    }
+}
+
+pub(crate) mod sealed {
+    use super::*;
+
+    pub trait McoInstance {
+        type Source;
+        unsafe fn apply_clock_settings(source: Self::Source, prescaler: u8);
+    }
+
+    pub trait McoPin<T: McoInstance>: Pin {
+        fn configure(&mut self);
+    }
+}
+
+pub trait McoInstance: sealed::McoInstance + 'static {}
+
+pub trait McoPin<T: McoInstance>: sealed::McoPin<T> + 'static {}
+
+macro_rules! impl_peri {
+    ($peri:ident, $source:ident, $set_source:ident, $set_prescaler:ident) => {
+        impl sealed::McoInstance for peripherals::$peri {
+            type Source = $source;
+
+            unsafe fn apply_clock_settings(source: Self::Source, prescaler: u8) {
+                RCC.cfgr().modify(|w| {
+                    w.$set_source(source);
+                    w.$set_prescaler(prescaler);
+                });
+            }
+        }
+
+        impl McoInstance for peripherals::$peri {}
+    };
+}
+
+impl_peri!(MCO1, Mco1, set_mco1, set_mco1pre);
+impl_peri!(MCO2, Mco2, set_mco2, set_mco2pre);
+
+macro_rules! impl_pin {
+    ($peri:ident, $pin:ident, $af:expr) => {
+        impl McoPin<peripherals::$peri> for peripherals::$pin {}
+
+        impl sealed::McoPin<peripherals::$peri> for peripherals::$pin {
+            fn configure(&mut self) {
+                critical_section::with(|_| unsafe {
+                    self.set_as_af($af, crate::gpio::sealed::AFType::OutputPushPull);
+                    self.block().ospeedr().modify(|w| {
+                        w.set_ospeedr(
+                            self.pin() as usize,
+                            crate::pac::gpio::vals::Ospeedr::VERYHIGHSPEED,
+                        )
+                    });
+                })
+            }
+        }
+    };
+}
+
+crate::pac::peripheral_pins!(
+    ($inst:ident, rcc, RCC, $pin:ident, MCO_1, $af:expr) => {
+        impl_pin!(MCO1, $pin, $af);
+    };
+    ($inst:ident, rcc, RCC, $pin:ident, MCO_2, $af:expr) => {
+        impl_pin!(MCO2, $pin, $af);
+    };
+);
+
+pub struct Mco<'d, T: McoInstance> {
+    phantom: PhantomData<&'d mut T>,
+}
+
+impl<'d, T: McoInstance> Mco<'d, T> {
+    pub fn new(
+        _peri: impl Unborrow<Target = T> + 'd,
+        pin: impl Unborrow<Target = impl McoPin<T>> + 'd,
+        source: impl McoSource<Raw = T::Source>,
+        prescaler: McoClock,
+    ) -> Self {
+        unborrow!(pin);
+
+        unsafe {
+            T::apply_clock_settings(source.into_raw(), prescaler.into_raw());
+        }
+
+        pin.configure();
+
+        Self {
+            phantom: PhantomData,
+        }
+    }
+}
 
 pub unsafe fn init(config: Config) {
     let mut power = Power::new(<peripherals::PWR as embassy::util::Steal>::steal(), false);
diff --git a/examples/stm32h7/src/bin/mco.rs b/examples/stm32h7/src/bin/mco.rs
new file mode 100644
index 000000000..4cecd9b04
--- /dev/null
+++ b/examples/stm32h7/src/bin/mco.rs
@@ -0,0 +1,32 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+#[path = "../example_common.rs"]
+mod example_common;
+use embassy::executor::Spawner;
+use embassy::time::{Duration, Timer};
+use embassy_stm32::gpio::{Level, Output, Speed};
+use embassy_stm32::rcc::{Mco, Mco1Source, McoClock};
+use embassy_stm32::Peripherals;
+use embedded_hal::digital::v2::OutputPin;
+use example_common::*;
+
+#[embassy::main]
+async fn main(_spawner: Spawner, p: Peripherals) {
+    info!("Hello World!");
+
+    let mut led = Output::new(p.PB14, Level::High, Speed::Low);
+
+    let _mco = Mco::new(p.MCO1, p.PA8, Mco1Source::Hsi, McoClock::Divided(8));
+
+    loop {
+        info!("high");
+        unwrap!(led.set_high());
+        Timer::after(Duration::from_millis(500)).await;
+
+        info!("low");
+        unwrap!(led.set_low());
+        Timer::after(Duration::from_millis(500)).await;
+    }
+}