diff --git a/Cargo.toml b/Cargo.toml
index 41328b51..fe5c4703 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -74,6 +74,7 @@ embedded-sdmmc = "0.3.0"
 usb-device = { version = "0.3.2", features = ["defmt"] }
 usbd-serial = "0.2.2"
 rand = { version = "0.9", default-features = false }
+as5600 = "0.8.0"
 
 #TODO: Separate feature sets
 [features]
@@ -84,9 +85,23 @@ usb = ["dep:stm32-usbd"]
 stm32g431 = ["stm32g4/stm32g431", "cat2"]
 stm32g441 = ["stm32g4/stm32g441", "cat2"]
 stm32g473 = ["stm32g4/stm32g473", "cat3", "adc3", "adc4", "adc5"]
-stm32g474 = ["stm32g4/stm32g474", "cat3", "adc3", "adc4", "adc5", "stm32-hrtim/stm32g474"]
+stm32g474 = [
+    "stm32g4/stm32g474",
+    "cat3",
+    "adc3",
+    "adc4",
+    "adc5",
+    "stm32-hrtim/stm32g474",
+]
 stm32g483 = ["stm32g4/stm32g483", "cat3", "adc3", "adc4", "adc5"]
-stm32g484 = ["stm32g4/stm32g484", "cat3", "adc3", "adc4", "adc5", "stm32-hrtim/stm32g484"]
+stm32g484 = [
+    "stm32g4/stm32g484",
+    "cat3",
+    "adc3",
+    "adc4",
+    "adc5",
+    "stm32-hrtim/stm32g484",
+]
 stm32g491 = ["stm32g4/stm32g491", "cat4", "adc3"]
 stm32g4a1 = ["stm32g4/stm32g4a1", "cat4", "adc3"]
 
@@ -108,7 +123,7 @@ defmt = [
     "embedded-hal/defmt-03",
     "embedded-io/defmt-03",
     "embedded-test/defmt",
-    "stm32-hrtim?/defmt"
+    "stm32-hrtim?/defmt",
 ]
 cordic = ["dep:fixed"]
 adc3 = []
diff --git a/Embed.toml b/Embed.toml
new file mode 100644
index 00000000..74bc2415
--- /dev/null
+++ b/Embed.toml
@@ -0,0 +1,2 @@
+[default.rtt]
+enabled = true
diff --git a/examples/button.rs b/examples/button.rs
index c3530b90..a1c12a9d 100644
--- a/examples/button.rs
+++ b/examples/button.rs
@@ -5,14 +5,13 @@ use stm32g4xx_hal::{
     //delay::{DelayExt, SYSTDelayExt},
     gpio::{self, ExtiPin, GpioExt, Input, SignalEdge},
     rcc::RccExt,
-    stm32,
-    stm32::{interrupt, Interrupt},
+    stm32::{self, interrupt, Interrupt},
     syscfg::SysCfgExt,
 };
 
 use core::cell::RefCell;
 use core::sync::atomic::{AtomicBool, Ordering};
-use cortex_m::{asm::wfi, interrupt::Mutex};
+use cortex_m::interrupt::Mutex;
 use cortex_m_rt::entry;
 
 type ButtonPin = gpio::PC13<Input>;
@@ -52,8 +51,14 @@ fn main() -> ! {
     utils::logger::init();
 
     let mut dp = stm32::Peripherals::take().expect("cannot take peripherals");
+
     let mut rcc = dp.RCC.constrain();
-    let mut syscfg = dp.SYSCFG.constrain();
+
+    // Workaround for RTT when using wfi instruction
+    // Enable an AHB peripheral clock for debug probe with wfi
+    rcc.ahb1enr().modify(|_, w| w.dma1en().set_bit());
+
+    let mut syscfg = dp.SYSCFG.constrain(&mut rcc);
 
     println!("Led Init");
     // Configure PA5 pin to blink LED
@@ -80,7 +85,7 @@ fn main() -> ! {
 
     println!("Start Loop");
     loop {
-        wfi();
+        cortex_m::asm::wfi();
         println!("Check");
 
         if G_LED_ON.load(Ordering::Relaxed) {
diff --git a/examples/i2c-fmp-as5600.rs b/examples/i2c-fmp-as5600.rs
new file mode 100644
index 00000000..2031558e
--- /dev/null
+++ b/examples/i2c-fmp-as5600.rs
@@ -0,0 +1,92 @@
+//! I2C Fast Mode Plus example with an AS5600 magnetic angle sensor.
+//!
+//! This example expects the AS5600 to be connected to the I2C bus on PB7 (SDA) and PA15 (SCL),
+//! and a 24MHz HSE oscillator to configure the PLL for 168MHz system clock.
+//!
+//! The I2C bus is configured with Fast Mode Plus (FMP) enabled in SysCfg, and a 1MHz I2C clock rate.
+//!
+//! ```DEFMT_LOG=debug cargo run --release --example i2c-fmp-as5600 --features stm32g431,defmt -- --chip STM32G431KBTx```
+
+#![deny(warnings)]
+#![deny(unsafe_code)]
+#![no_main]
+#![no_std]
+
+use fugit::HertzU32 as Hertz;
+use hal::prelude::*;
+use hal::rcc::SysClockSrc;
+use hal::stm32;
+use hal::time::RateExtU32;
+use stm32g4xx_hal as hal;
+use stm32g4xx_hal::syscfg::SysCfgExt;
+
+use as5600::As5600;
+use cortex_m_rt::entry;
+
+#[macro_use]
+mod utils;
+use utils::logger::error;
+use utils::logger::info;
+
+#[entry]
+fn main() -> ! {
+    utils::logger::init();
+
+    let dp = stm32::Peripherals::take().expect("cannot take peripherals");
+    //let cp = cortex_m::Peripherals::take().expect("cannot take core peripherals");
+
+    let pwr_cfg = dp
+        .PWR
+        .constrain()
+        .vos(stm32g4xx_hal::pwr::VoltageScale::Range1 { enable_boost: true })
+        .freeze();
+
+    let pll_cfg = hal::rcc::PllConfig {
+        mux: hal::rcc::PllSrc::HSE(Hertz::MHz(24)),
+        m: hal::rcc::PllMDiv::DIV_2,
+        n: hal::rcc::PllNMul::MUL_28,
+        r: Some(hal::rcc::PllRDiv::DIV_2),
+        q: None,
+        p: None,
+    };
+
+    info!("Configuring PLL");
+    let rcc_cfg = hal::rcc::Config::new(SysClockSrc::PLL)
+        .boost(true)
+        .pll_cfg(pll_cfg);
+
+    let mut rcc = dp.RCC.freeze(rcc_cfg, pwr_cfg);
+    info!("System clock frequency: {}", rcc.clocks.sys_clk.to_Hz());
+
+    let gpioa = dp.GPIOA.split(&mut rcc);
+    let gpiob = dp.GPIOB.split(&mut rcc);
+
+    let sda = gpiob.pb7.into_alternate_open_drain();
+    let scl = gpioa.pa15.into_alternate_open_drain();
+
+    let mut syscfg = dp.SYSCFG.constrain(&mut rcc);
+
+    // Enable Fast Mode Plus for I2C1
+    syscfg.i2c_fmp_enable::<1>(true);
+
+    // Configure I2C for 1MHz
+    let i2c = dp.I2C1.i2c(sda, scl, 1.MHz(), &mut rcc);
+
+    let mut as5600 = As5600::new(i2c);
+
+    loop {
+        match as5600.angle() {
+            Ok(angle) => {
+                // Convert angle to degrees
+                let angle_degrees = angle as f32 * 360.0 / 4096.0;
+                info!("Angle: {}°", angle_degrees);
+            }
+            Err(e) => match e {
+                as5600::error::Error::Communication(_) => error!("I2C communication error"),
+                as5600::error::Error::Status(_error) => error!("AS5600 status error"),
+                as5600::error::Error::Configuration(_error) => error!("AS5600 configuration error"),
+                _ => error!("Other AS5600 error"),
+            },
+        }
+    }
+}
diff --git a/src/syscfg.rs b/src/syscfg.rs
index b13833bb..8da543a0 100644
--- a/src/syscfg.rs
+++ b/src/syscfg.rs
@@ -1,25 +1,17 @@
-use crate::bb;
-use crate::stm32::{RCC, SYSCFG};
+use crate::rcc::{Enable, Rcc, Reset};
+use crate::stm32::SYSCFG;
 use core::ops::Deref;
 
 /// Extension trait that constrains the `SYSCFG` peripheral
 pub trait SysCfgExt {
     /// Constrains the `SYSCFG` peripheral so it plays nicely with the other abstractions
-    fn constrain(self) -> SysCfg;
+    fn constrain(self, rcc: &mut Rcc) -> SysCfg;
 }
 
 impl SysCfgExt for SYSCFG {
-    fn constrain(self) -> SysCfg {
-        unsafe {
-            // NOTE(unsafe) this reference will only be used for atomic writes with no side effects.
-            let rcc = &(*RCC::ptr());
-
-            // Enable clock.
-            bb::set(&rcc.apb2enr(), 0);
-
-            // Stall the pipeline to work around erratum 2.1.13 (DM00037591)
-            cortex_m::asm::dsb();
-        }
+    fn constrain(self, rcc: &mut Rcc) -> SysCfg {
+        SYSCFG::enable(rcc);
+        SYSCFG::reset(rcc);
 
         SysCfg(self)
     }
@@ -35,3 +27,22 @@ impl Deref for SysCfg {
         &self.0
     }
 }
+
+impl SysCfg {
+    /// Enable/disable I2C fast mode plus on the I2C bus index provided as a const generic parameter.
+    ///
+    /// Pins that are configured as I2C alternate functions will be configured as fast mode plus.
+    /// The alternate function mode of the pin must be set before FMP is enabled in SysCfg.
+    ///
+    /// When FM+ mode is activated on a pin, the GPIO speed configuration (OSPEEDR) is ignored and overridden.
+    ///
+    pub fn i2c_fmp_enable<const BUS: u8>(&mut self, en: bool) {
+        match BUS {
+            1 => (*self).cfgr1().modify(|_, w| w.i2c1_fmp().bit(en)),
+            2 => (*self).cfgr1().modify(|_, w| w.i2c2_fmp().bit(en)),
+            3 => (*self).cfgr1().modify(|_, w| w.i2c3_fmp().bit(en)),
+            4 => (*self).cfgr1().modify(|_, w| w.i2c4_fmp().bit(en)),
+            _ => panic!("Invalid I2C bus"),
+        };
+    }
+}