//! Deterministic STM32F722 PLL clock-source selection.

use stm32f7::stm32f722;

const CLOCK_SWITCH_POLL_LIMIT: u16 = 1_024;

/// Failure to establish the 216 MHz PLL clock profile.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum ClockProfileError {
    HsiNotReady,
    SystemClockNotHsi,
    PllNotStopped,
    PllNotReady,
    OverDriveNotReady,
    OverDriveSwitchNotReady,
    SystemClockNotPll,
}

/// Selects the board's HSI PLL profile.
///
/// SYSCLK/HCLK run at 216 MHz; APB1 is 54 MHz (timers run at 108 MHz), and
/// APB2 is 108 MHz. HSI is the PLL source, so no external-crystal assumption
/// is introduced.
pub fn select_pll216_profile(
    rcc: &stm32f722::RCC,
    pwr: &stm32f722::PWR,
    flash: &stm32f722::FLASH,
) -> Result<(), ClockProfileError> {
    rcc.cr().modify(|_, write| write.hsion().on());
    if !wait_until(|| rcc.cr().read().hsirdy().is_ready()) {
        return Err(ClockProfileError::HsiNotReady);
    }

    // Return to HSI before changing a PLL left configured by an earlier boot stage.
    rcc.cfgr().modify(|_, write| write.sw().hsi());
    if !wait_until(|| rcc.cfgr().read().sws().is_hsi()) {
        return Err(ClockProfileError::SystemClockNotHsi);
    }
    rcc.cr().modify(|_, write| write.pllon().off());
    if !wait_until(|| !rcc.cr().read().pllrdy().is_ready()) {
        return Err(ClockProfileError::PllNotStopped);
    }

    // Scale 1, over-drive and seven flash wait states are required at 216 MHz.
    rcc.apb1enr().modify(|_, write| write.pwren().set_bit());
    pwr.cr1().modify(|read, write| unsafe {
        write.bits((read.bits() & !(0b11 << 14)) | (0b11 << 14) | (1 << 16))
    });
    if !wait_until(|| pwr.csr1().read().odrdy().bit_is_set()) {
        return Err(ClockProfileError::OverDriveNotReady);
    }
    pwr.cr1().modify(|_, write| write.odswen().set_bit());
    if !wait_until(|| pwr.csr1().read().odswrdy().bit_is_set()) {
        return Err(ClockProfileError::OverDriveSwitchNotReady);
    }
    flash.acr().modify(|read, write| unsafe {
        write.bits((read.bits() & !0x0f) | 7 | (1 << 8) | (1 << 9))
    });

    // HSI / 16 * 432 / 2 = 216 MHz; Q=9 supplies a 48 MHz domain.
    rcc.pllcfgr()
        .write(|write| unsafe { write.bits(16 | (432 << 6) | (9 << 24)) });
    rcc.cr().modify(|_, write| write.pllon().on());
    if !wait_until(|| rcc.cr().read().pllrdy().is_ready()) {
        return Err(ClockProfileError::PllNotReady);
    }
    rcc.cfgr().modify(|read, write| unsafe {
        // AHB /1, APB1 /4, APB2 /2, SYSCLK = PLL.
        write.bits(
            (read.bits() & !((0x0f << 4) | (0x07 << 10) | (0x07 << 13) | 0x03))
                | (0b101 << 10)
                | (0b100 << 13)
                | 0b10,
        )
    });
    if !wait_until(|| rcc.cfgr().read().sws().is_pll()) {
        return Err(ClockProfileError::SystemClockNotPll);
    }
    Ok(())
}

fn wait_until(mut ready: impl FnMut() -> bool) -> bool {
    for _ in 0..CLOCK_SWITCH_POLL_LIMIT {
        if ready() {
            return true;
        }
    }
    false
}