ra-hal 0.3.0

use core::ops::RangeInclusive;

use cortex_m::asm;
use fugit::{HertzU32, HertzU64, MegahertzU32, RateExtU32 as _};
use ra_metapac::{
    fcache::vals::Flwt,
    system::{
        regs::{Pllccr, Pllccr2},
        vals::{Cpuck, Hcfrq0, Opcm, Scickdiv, Scicksel, Spickdiv, Spicksel},
    },
};

use crate::{
    clock::{CLOCK_STATUS, ClockConfig, ClockStatus, HocoFrequency, PllConfig, SystemClockSource},
    pac::{
        self,
        system::vals::{
            Bck, Fck, Hcstp, Ick, Pcka, Pckb, Pckc, Pckd, Pcke, Plidiv, Pllmul, Pllmulnf, Plodivp,
            Plodivq, Plodivr, Plsrcsel, SckscrCksel, Sodrv, Usbckdiv, Usbcksel,
        },
    },
    write_protect::ProtectedPeripheral as _,
};

struct PllLimits {
    /// Input clock range
    raw_input: RangeInclusive<HertzU32>,
    /// Input clock range after input division
    input: RangeInclusive<HertzU32>,
    /// Output range before division
    vco: RangeInclusive<HertzU32>,
    /// Output range for "clock P" after division
    output_p: RangeInclusive<HertzU32>,
    /// Output range for "clock Q" after division
    output_q: RangeInclusive<HertzU32>,
    /// Output range for "clock R" after division
    output_r: RangeInclusive<HertzU32>,
}

const PLL_LIMITS: PllLimits = PllLimits {
    raw_input: RangeInclusive::new(
        MegahertzU32::from_raw(8).convert(),
        MegahertzU32::from_raw(48).convert(),
    ),
    input: RangeInclusive::new(
        MegahertzU32::from_raw(6).convert(),
        MegahertzU32::from_raw(12).convert(),
    ),
    vco: RangeInclusive::new(
        MegahertzU32::from_raw(640).convert(),
        MegahertzU32::from_raw(1440).convert(),
    ),
    output_p: RangeInclusive::new(
        MegahertzU32::from_raw(40).convert(),
        MegahertzU32::from_raw(480).convert(),
    ),
    output_q: RangeInclusive::new(
        MegahertzU32::from_raw(71).convert(),
        MegahertzU32::from_raw(480).convert(),
    ),
    output_r: RangeInclusive::new(
        MegahertzU32::from_raw(71).convert(),
        MegahertzU32::from_raw(480).convert(),
    ),
};

const OUTPUT_FACTOR: u32 = 10;

#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Clone)]
pub enum SciClockSource {
    Hoco,
    Moco,
    Loco,
    Mosc,
    Sosc,
    PllP,
    PllQ,
    PllR,
    Pll2P,
    Pll2Q,
    Pll2R,
}

#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Clone)]
pub enum SpiClockSource {
    Hoco,
    Moco,
    Loco,
    Mosc,
    Sosc,
    PllP,
    PllQ,
    PllR,
    Pll2P,
    Pll2Q,
    Pll2R,
}

#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Clone)]
pub enum UsbClockSource {
    Hoco,
    Moco,
    Mosc,
    PllP,
    PllQ,
    PllR,
    Pll2P,
    Pll2Q,
    Pll2R,
}

#[allow(missing_docs)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Copy, Clone)]
pub enum PllInDiv {
    Div1,
    Div2,
    Div3,
    Div4,
}

#[allow(missing_docs)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Copy, Clone)]
pub enum PllInput {
    Hoco,
    Mosc,
}

#[allow(missing_docs)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Copy, Clone)]
#[repr(u8)]
pub enum PllPDiv {
    Div2 = 2,
    Div4 = 4,
    Div6 = 6,
    Div8 = 8,
    Div16 = 16,
}

#[allow(missing_docs)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Copy, Clone)]
#[repr(u8)]
pub enum PllQDiv {
    Div2 = 2,
    Div3 = 3,
    Div4 = 4,
    Div5 = 5,
    Div6 = 6,
    Div8 = 8,
    Div9 = 9,
}

#[allow(missing_docs)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Copy, Clone)]
#[repr(u8)]
pub enum PllRDiv {
    Div2 = 2,
    Div3 = 3,
    Div4 = 4,
    Div5 = 5,
    Div6 = 6,
    Div8 = 8,
    Div9 = 9,
}

// Should we just make this an integer and range check at runtime?
#[allow(missing_docs)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Copy, Clone)]
pub enum PllOutMul {
    Mul26_0 = 260,
    Mul27_0 = 270,
    Mul28_0 = 280,
    Mul40_0 = 400,
    Mul80_0 = 800,
    Mul89_0 = 890,
    Mul90_0 = 900,
    Mul91_0 = 910,
    Mul179_0 = 1790,
    Mul180_0 = 1800,
}

impl From<PllPDiv> for Plodivp {
    fn from(value: PllPDiv) -> Self {
        match value {
            PllPDiv::Div2 => Self::Div2,
            PllPDiv::Div4 => Self::Div4,
            PllPDiv::Div6 => Self::Div6,
            PllPDiv::Div8 => Self::Div8,
            PllPDiv::Div16 => Self::Div16,
        }
    }
}

impl From<PllQDiv> for Plodivq {
    fn from(value: PllQDiv) -> Self {
        match value {
            PllQDiv::Div2 => Self::Div2,
            PllQDiv::Div3 => Self::Div3,
            PllQDiv::Div4 => Self::Div4,
            PllQDiv::Div5 => Self::Div5,
            PllQDiv::Div6 => Self::Div6,
            PllQDiv::Div8 => Self::Div8,
            PllQDiv::Div9 => Self::Div9,
        }
    }
}

impl From<PllRDiv> for Plodivr {
    fn from(value: PllRDiv) -> Self {
        match value {
            PllRDiv::Div2 => Self::Div2,
            PllRDiv::Div3 => Self::Div3,
            PllRDiv::Div4 => Self::Div4,
            PllRDiv::Div5 => Self::Div5,
            PllRDiv::Div6 => Self::Div6,
            PllRDiv::Div8 => Self::Div8,
            PllRDiv::Div9 => Self::Div9,
        }
    }
}

impl From<PllInput> for Plsrcsel {
    fn from(value: PllInput) -> Self {
        match value {
            PllInput::Hoco => Self::Hoco,
            PllInput::Mosc => Self::Mosc,
        }
    }
}

impl From<PllInDiv> for Plidiv {
    fn from(value: PllInDiv) -> Self {
        match value {
            PllInDiv::Div1 => Self::Div1,
            PllInDiv::Div2 => Self::Div2,
            PllInDiv::Div3 => Self::Div3,
            PllInDiv::Div4 => Self::Div4,
        }
    }
}

impl From<PllOutMul> for Pllmul {
    fn from(value: PllOutMul) -> Self {
        match value {
            PllOutMul::Mul26_0 => Self::Mul26,
            PllOutMul::Mul27_0 => Self::Mul27,
            PllOutMul::Mul28_0 => Self::Mul28,
            PllOutMul::Mul40_0 => Self::Mul40,
            PllOutMul::Mul80_0 => Self::Mul80,
            PllOutMul::Mul89_0 => Self::Mul89,
            PllOutMul::Mul90_0 => Self::Mul90,
            PllOutMul::Mul91_0 => Self::Mul91,
            PllOutMul::Mul179_0 => Self::Mul179,
            PllOutMul::Mul180_0 => Self::Mul180,
        }
    }
}

/// Returns the output frequencies of the PLL clocks based on reading the registers.
fn pll_status(
    running: bool,
    hoco: HertzU32,
    mosc: Option<HertzU32>,
    pllccr: Pllccr,
    pllccr2: Pllccr2,
) -> Option<(HertzU32, HertzU32, HertzU32)> {
    match running {
        true => {
            let input_frequency = match pllccr.plsrcsel() {
                Plsrcsel::Mosc => mosc.unwrap(),
                Plsrcsel::Hoco => hoco,
            };
            let output_mul: u32 = match pllccr.pllmul() {
                Pllmul::Mul26 => 26,
                Pllmul::Mul27 => 27,
                Pllmul::Mul28 => 28,
                Pllmul::Mul40 => 40,
                Pllmul::Mul80 => 80,
                Pllmul::Mul89 => 89,
                Pllmul::Mul90 => 90,
                Pllmul::Mul91 => 91,
                Pllmul::Mul180 => 180,
                _ => unimplemented!(),
            };

            let input_div: u32 = match pllccr.plidiv() {
                Plidiv::Div1 => 1,
                Plidiv::Div2 => 2,
                Plidiv::Div3 => 3,
                Plidiv::Div4 => 4,
            };

            let output_factor = 10;

            let vco = (((input_frequency * 10) / input_div) / output_factor) * output_mul;

            // Explicitly specifying the reserved values is intentional, to catch any missed valid values.

            let pll_p = vco
                / match pllccr2.plodivp() {
                    Plodivp::Div1 => 1,
                    Plodivp::Div2 => 2,
                    Plodivp::Div4 => 4,
                    Plodivp::Div6 => 6,
                    Plodivp::Div8 => 8,
                    Plodivp::Div16 => 16,
                    Plodivp::_RESERVED_2
                    | Plodivp::_RESERVED_4
                    | Plodivp::_RESERVED_6
                    | Plodivp::_RESERVED_8
                    | Plodivp::_RESERVED_9
                    | Plodivp::_RESERVED_a
                    | Plodivp::_RESERVED_b
                    | Plodivp::_RESERVED_c
                    | Plodivp::_RESERVED_d
                    | Plodivp::_RESERVED_e => unimplemented!(),
                };

            let pll_q = vco
                / match pllccr2.plodivq() {
                    Plodivq::Div2 => 2,
                    Plodivq::Div3 => 3,
                    Plodivq::Div4 => 4,
                    Plodivq::Div5 => 5,
                    Plodivq::Div6 => 6,
                    Plodivq::Div8 => 8,
                    Plodivq::Div9 => 9,
                    Plodivq::_RESERVED_0
                    | Plodivq::_RESERVED_6
                    | Plodivq::_RESERVED_9
                    | Plodivq::_RESERVED_a
                    | Plodivq::_RESERVED_b
                    | Plodivq::_RESERVED_c
                    | Plodivq::_RESERVED_d
                    | Plodivq::_RESERVED_e
                    | Plodivq::_RESERVED_f => unimplemented!(),
                };

            let pll_r = vco
                / match pllccr2.plodivr() {
                    Plodivr::Div2 => 2,
                    Plodivr::Div3 => 3,
                    Plodivr::Div4 => 4,
                    Plodivr::Div5 => 5,
                    Plodivr::Div6 => 6,
                    Plodivr::Div8 => 8,
                    Plodivr::Div9 => 9,
                    Plodivr::_RESERVED_0
                    | Plodivr::_RESERVED_6
                    | Plodivr::_RESERVED_9
                    | Plodivr::_RESERVED_a
                    | Plodivr::_RESERVED_b
                    | Plodivr::_RESERVED_c
                    | Plodivr::_RESERVED_d
                    | Plodivr::_RESERVED_e
                    | Plodivr::_RESERVED_f => unimplemented!(),
                };

            Some((pll_p, pll_q, pll_r))
        }
        false => None,
    }
}

/// Returns and validates the intended PLL clock frequencies based on the `PllConfig` and `ClockConfig` structs.
/// Panics if any of the frequencies are out of range.
fn calc_pll_frequency(
    pll_config: &PllConfig,
    config: &ClockConfig,
    limits: &PllLimits,
) -> (HertzU32, HertzU32, HertzU32) {
    let output_mul: u32 = pll_config.mul as u32;

    let input_div: u32 = match pll_config.div {
        PllInDiv::Div1 => 1,
        PllInDiv::Div2 => 2,
        PllInDiv::Div3 => 3,
        PllInDiv::Div4 => 4,
    };

    let raw_input_frequency = match pll_config.input {
        PllInput::Hoco => config.hoco.into(),
        PllInput::Mosc => config.mosc.unwrap(),
    };
    let raw_input_frequency: HertzU64 = raw_input_frequency.into();

    let pll_input_frequency = raw_input_frequency / input_div;

    assert!(
        pll_input_frequency >= *limits.input.start(),
        "Invalid PLL configuration, input too slow {pll_input_frequency} < {}",
        limits.input.start()
    );
    assert!(
        pll_input_frequency <= *limits.input.end(),
        "Invalid PLL configuration, input too fast {pll_input_frequency} > {}",
        limits.input.end()
    );

    let pll_output = (pll_input_frequency * output_mul) / OUTPUT_FACTOR;

    assert!(
        pll_output >= *limits.vco.start(),
        "Invalid PLL configuration, output too slow {pll_output} < {}",
        limits.vco.start()
    );
    assert!(
        pll_output <= *limits.vco.end(),
        "Invalid PLL configuration, output too fast {pll_output} > {}",
        limits.vco.end()
    );

    let pll_output: HertzU32 = HertzU32::from_raw(pll_output.to_Hz() as u32);

    (
        (pll_output / pll_config.div_p as u32),
        (pll_output / pll_config.div_q as u32),
        (pll_output / pll_config.div_r as u32),
    )
}

pub(crate) fn init(config: ClockConfig) -> Result<(), ()> {
    let system = pac::SYSTEM;
    debug!("HOCO: status={}", system.hococr().read());

    if config.pll.is_none() && config.system == SystemClockSource::Pll1P {
        panic!("PLL selected as root clock, but not enabled/configured.");
    }

    if !config.sosc && config.system == SystemClockSource::Pll1P {
        panic!("SOSC required to use PLL as root clock, but SOSC not enabled.");
    }

    // Sanity check PLL and PLL2 configs
    let pll_frequency = match config.pll {
        Some(ref pll_config) => Some(calc_pll_frequency(pll_config, &config, &PLL_LIMITS)),
        None => None,
    };

    let pll2_frequency = match config.pll2 {
        Some(ref pll_config) => Some(calc_pll_frequency(pll_config, &config, &PLL_LIMITS)),
        None => None,
    };

    system.protected_write(|| {
        // § 10.2.10 high speed mode needed for PLL operation
        // § 8.11.1.1 generally suggested as part of the clock init
        trace!("Setting high speed mode");
        system.opccr().modify(|r| r.set_opcm(Opcm::High));
        while system.opccr().read().opcmtsf() {}

        if config.sosc {
            trace!("Enabling SCOSC");
            system.somcr().modify(|r| r.set_sodrv(Sodrv::Normal));
            system.sosccr().modify(|r| r.set_sostp(false));
            // TODO: Set oscillator stabilization time.
            while system.sosccr().read().sostp() {}
        }

        let fll_magic = match config.hoco {
            HocoFrequency::_16mhz => crate::constants::FLL_16MHZ,
            HocoFrequency::_18mhz => crate::constants::FLL_18MHZ,
            HocoFrequency::_20mhz => crate::constants::FLL_20MHZ,
            HocoFrequency::_32mhz => crate::constants::FLL_32MHZ,
            HocoFrequency::_48mhz => crate::constants::FLL_48MHZ,
        };
        system.fllcr2().modify(|r| r.set_fllcntl(fll_magic));
        system.fllcr1().modify(|r| r.set_fllen(true));

        system.hococr2().write(|r| r.set_hcfrq0(config.hoco.into()));
        system.hococr().write(|r| r.set_hcstp(Hcstp::Start));
        while !system.oscsf().read().hocosf() {}

        system.pllcr().write(|r| r.set_pllstp(true));
        while system.oscsf().read().pllsf() {}

        if let Some(pll_config) = config.pll.as_ref() {
            system.pllccr().modify(|r| {
                r.set_plsrcsel(pll_config.input.into());
                r.set_plidiv(pll_config.div.into());
                r.set_pllmul(pll_config.mul.into());
                r.set_pllmulnf(Pllmulnf::_00);
            });
            system.pllccr2().modify(|r| {
                r.set_plodivp(pll_config.div_p.into());
                r.set_plodivq(pll_config.div_q.into());
                r.set_plodivr(pll_config.div_r.into());
            });
            system.pllcr().write(|r| r.set_pllstp(false));
            while !system.oscsf().read().pllsf() {}
            trace!("PLL1 stabilized");
        }

        let root_frequency: HertzU32;
        match config.system {
            SystemClockSource::Mosc => todo!(),
            SystemClockSource::Sosc => todo!(),
            SystemClockSource::Hoco => todo!(),
            SystemClockSource::Moco => todo!(),
            SystemClockSource::Pll1P => root_frequency = 480_u32.MHz(),
        }

        let (pckb_div, pckc_div, fck_div, pcka_div, pckd_div, ick_div, pcke_div, bck_div) = {
            let _60mhz = root_frequency.to_Hz().div_ceil(60_000_000);
            let _100mhz = root_frequency.to_Hz().div_ceil(100_000_000);
            let _120mhz = root_frequency.to_Hz().div_ceil(120_000_000);
            let _240mhz = root_frequency.to_Hz().div_ceil(240_000_000);

            let (pckb_div, pckc_div, fck_div) = match _60mhz {
                1 => (Pckb::Div1, Pckc::Div1, Fck::Div1),
                2 => (Pckb::Div2, Pckc::Div2, Fck::Div2),
                3 => (Pckb::Div3, Pckc::Div3, Fck::Div3),
                4 => (Pckb::Div4, Pckc::Div4, Fck::Div4),
                5..=6 => (Pckb::Div6, Pckc::Div6, Fck::Div6),
                7..=8 => (Pckb::Div8, Pckc::Div8, Fck::Div8),
                9..=12 => (Pckb::Div12, Pckc::Div12, Fck::Div12),
                13..=16 => (Pckb::Div16, Pckc::Div16, Fck::Div16),
                17..=32 => (Pckb::Div32, Pckc::Div32, Fck::Div32),
                33..=64 => (Pckb::Div64, Pckc::Div64, Fck::Div64),
                _ => unimplemented!(),
            };

            let (pcka_div, pckd_div, bck_div) = match _120mhz {
                1 => (Pcka::Div1, Pckd::Div1, Bck::Div1),
                2 => (Pcka::Div2, Pckd::Div2, Bck::Div2),
                3 => (Pcka::Div3, Pckd::Div3, Bck::Div3),
                4 => (Pcka::Div4, Pckd::Div4, Bck::Div4),
                5..=6 => (Pcka::Div6, Pckd::Div6, Bck::Div6),
                7..=8 => (Pcka::Div8, Pckd::Div8, Bck::Div8),
                9..=12 => (Pcka::Div12, Pckd::Div12, Bck::Div12),
                13..=16 => (Pcka::Div16, Pckd::Div16, Bck::Div16),
                17..=32 => (Pcka::Div32, Pckd::Div32, Bck::Div32),
                33..=64 => (Pcka::Div64, Pckd::Div64, Bck::Div64),
                _ => unimplemented!(),
            };

            let (ick_div, pcke_div) = match _240mhz {
                1 => (Ick::Div1, Pcke::Div1),
                2 => (Ick::Div2, Pcke::Div2),
                3 => (Ick::Div3, Pcke::Div3),
                4 => (Ick::Div4, Pcke::Div4),
                5..=6 => (Ick::Div6, Pcke::Div6),
                7..=8 => (Ick::Div8, Pcke::Div8),
                9..=12 => (Ick::Div12, Pcke::Div12),
                13..=16 => (Ick::Div16, Pcke::Div16),
                17..=32 => (Ick::Div32, Pcke::Div32),
                33..=64 => (Ick::Div64, Pcke::Div64),
                _ => unimplemented!(),
            };

            (
                pckb_div, pckc_div, fck_div, pcka_div, pckd_div, ick_div, pcke_div, bck_div,
            )
        };

        // TODO: Make sure that the dividers are all multiples of 2 or all multiples of 3

        let ick_frq = match ick_div {
            Ick::Div1 => root_frequency / 1,
            Ick::Div2 => root_frequency / 2,
            Ick::Div3 => root_frequency / 3,
            Ick::Div4 => root_frequency / 4,
            Ick::Div6 => root_frequency / 6,
            Ick::Div8 => root_frequency / 8,
            Ick::Div12 => root_frequency / 12,
            Ick::Div16 => root_frequency / 16,
            Ick::Div32 => root_frequency / 32,
            Ick::Div64 => root_frequency / 64,
            Ick::_RESERVED_7
            | Ick::_RESERVED_b
            | Ick::_RESERVED_c
            | Ick::_RESERVED_d
            | Ick::_RESERVED_e
            | Ick::_RESERVED_f => unimplemented!(),
        };

        // Needed for ƒICLK >= 120 MHz
        let sram = pac::SRAM;
        sram.sramwtsc().write(|r| r.set_wten(true));

        let fcache = pac::FCACHE;
        // TODO: Set this depending on ƒICLK
        fcache.flwt().modify(|r| r.set_flwt(Flwt::_4));

        fcache.fcacheiv().write(|r| r.set_fcacheiv(true));
        while fcache.fcacheiv().read().fcacheiv() {}
        // § 52.4.1 It is prohibited to disable FCACHE after enabling.
        fcache.fcachee().write(|r| r.set_fcacheen(true));

        system.sckdivcr().write(|r| {
            r.set_ick(ick_div);
            r.set_fck(fck_div);
            r.set_bck(bck_div);
            r.set_pcka(pcka_div);
            r.set_pckb(pckb_div);
            r.set_pckc(pckc_div);
            r.set_pckd(pckd_div);
            r.set_pcke(pcke_div);
        });
        while system.sckdivcr().read().pcke() != pcke_div {
            error!("If stuck here, clock didn't set correctly.");
        }
        system.sckdivcr2().modify(|r| r.set_cpuck(Cpuck::Div1));

        match config.system {
            SystemClockSource::Mosc => todo!(),
            SystemClockSource::Sosc => {
                assert_eq!(config.sosc, true);
                system.sckscr().modify(|r| r.set_cksel(SckscrCksel::Sosc));
            }
            SystemClockSource::Hoco => {
                system.sckscr().modify(|r| r.set_cksel(SckscrCksel::Hoco));
            }
            SystemClockSource::Moco => todo!(),
            SystemClockSource::Pll1P => {
                // RA6M5: § 8.2.3 When changing the value from a non-PLL clock source to the PLL, wait at least
                // 250 ns after changing the value, before starting subsequent processing.
                system.sckscr().modify(|r| r.set_cksel(SckscrCksel::Pll1P));
                for _ in 0..100 {
                    asm::nop();
                }
            }
        }

        const SCICLK_MAX: MegahertzU32 = MegahertzU32::from_raw(120);
        let sci_clock = config.sci.as_ref().map(|(sci_source, freq)| {
            assert!(freq <= &SCICLK_MAX);
            match sci_source {
                SciClockSource::PllP => {
                    let (pll_p_frequency, _, _) = pll_frequency.unwrap();
                    (Scicksel::Pll1P, pll_p_frequency / *freq)
                }
                SciClockSource::PllQ => {
                    let (_, pll_q_frequency, _) = pll_frequency.unwrap();
                    (Scicksel::Pll1Q, pll_q_frequency / *freq)
                }
                SciClockSource::PllR => {
                    let (_, _, pll_r_frequency) = pll_frequency.unwrap();
                    (Scicksel::Pll1R, pll_r_frequency / *freq)
                }
                SciClockSource::Pll2P => {
                    let (pll2_p_frequency, _, _) = pll2_frequency.unwrap();
                    (Scicksel::Pll2P, pll2_p_frequency / *freq)
                }
                SciClockSource::Pll2Q => {
                    let (_, pll2_q_frequency, _) = pll2_frequency.unwrap();
                    (Scicksel::Pll2Q, pll2_q_frequency / *freq)
                }
                SciClockSource::Pll2R => {
                    let (_, _, pll2_r_frequency) = pll2_frequency.unwrap();
                    (Scicksel::Pll2R, pll2_r_frequency / *freq)
                }
                _ => todo!(),
            }
        });

        match sci_clock {
            Some((source, divider)) => {
                let divider = match divider {
                    1 => Scickdiv::Div1,
                    2 => Scickdiv::Div2,
                    3 => Scickdiv::Div3,
                    4 => Scickdiv::Div4,
                    5 => Scickdiv::Div5,
                    6 => Scickdiv::Div6,
                    8 => Scickdiv::Div8,
                    div => unimplemented!("SCI div={} not supported", div),
                };
                system.scickcr().modify(|r| r.set_cksreq(true));
                while !system.scickcr().read().cksrdy() {}
                system.scickdivcr().modify(|r| r.set_ckdiv(divider));
                system.scickcr().modify(|r| r.set_cksel(source));
                system.scickcr().modify(|r| r.set_cksreq(false));
                trace!("Waiting for SCI clock to switch");
                while system.scickcr().read().cksrdy() {}
            }
            None => {}
        };

        const SPICLK_MAX: MegahertzU32 = MegahertzU32::from_raw(120);
        let spi_clock = config.spi.as_ref().map(|(spi_source, freq)| {
            assert!(freq <= &SPICLK_MAX);
            match spi_source {
                SpiClockSource::PllP => {
                    let (pll_p_frequency, _, _) = pll_frequency.unwrap();
                    (Spicksel::Pll1P, pll_p_frequency / *freq)
                }
                SpiClockSource::PllQ => {
                    let (_, pll_q_frequency, _) = pll_frequency.unwrap();
                    (Spicksel::Pll1Q, pll_q_frequency / *freq)
                }
                SpiClockSource::PllR => {
                    let (_, _, pll_r_frequency) = pll_frequency.unwrap();
                    (Spicksel::Pll1R, pll_r_frequency / *freq)
                }
                SpiClockSource::Pll2P => {
                    let (pll2_p_frequency, _, _) = pll2_frequency.unwrap();
                    (Spicksel::Pll2P, pll2_p_frequency / *freq)
                }
                SpiClockSource::Pll2Q => {
                    let (_, pll2_q_frequency, _) = pll2_frequency.unwrap();
                    (Spicksel::Pll2Q, pll2_q_frequency / *freq)
                }
                SpiClockSource::Pll2R => {
                    let (_, _, pll2_r_frequency) = pll2_frequency.unwrap();
                    (Spicksel::Pll2R, pll2_r_frequency / *freq)
                }
                _ => todo!(),
            }
        });

        match spi_clock {
            Some((source, divider)) => {
                let divider = match divider {
                    1 => Spickdiv::Div1,
                    2 => Spickdiv::Div2,
                    3 => Spickdiv::Div3,
                    4 => Spickdiv::Div4,
                    5 => Spickdiv::Div5,
                    6 => Spickdiv::Div6,
                    8 => Spickdiv::Div8,
                    div => unimplemented!("SPI div={} not supported", div),
                };
                system.spickcr().modify(|r| r.set_cksreq(true));
                while !system.spickcr().read().cksrdy() {}
                system.spickdivcr().modify(|r| r.set_ckdiv(divider));
                system.spickcr().modify(|r| r.set_cksel(source));
                system.spickcr().modify(|r| r.set_cksreq(false));
                trace!("Waiting for SPI clock to switch");
                while system.spickcr().read().cksrdy() {}
            }
            None => {}
        };

        let usb_clock = config.usb.as_ref().map(|usb_source| match usb_source {
            UsbClockSource::PllQ => {
                let (_, pll_q_frequency, _) = pll_frequency.unwrap();
                match (pll_q_frequency / 48).to_MHz() {
                    1 => (Usbcksel::Pll1Q, Usbckdiv::Div1),
                    2 => (Usbcksel::Pll1Q, Usbckdiv::Div2),
                    3 => (Usbcksel::Pll1Q, Usbckdiv::Div3),
                    4 => (Usbcksel::Pll1Q, Usbckdiv::Div4),
                    5 => (Usbcksel::Pll1Q, Usbckdiv::Div5),
                    6 => (Usbcksel::Pll1Q, Usbckdiv::Div6),
                    8 => (Usbcksel::Pll1Q, Usbckdiv::Div8),
                    div => unimplemented!(
                        "USB clock input must be 1x, 2x, 3x, 4x, 5x, 6x, or 8x 48 MHz. It's {}x.",
                        div
                    ),
                }
            }
            _ => todo!(),
        });

        match usb_clock {
            Some((source, divider)) => {
                system.usbckcr().modify(|r| r.set_usbcksreq(true));
                while !system.usbckcr().read().usbcksrdy() {}
                system.usbckdivcr().modify(|r| r.set_usbckdiv(divider));
                system.usbckcr().modify(|r| r.set_usbcksel(source));
                system.usbckcr().modify(|r| r.set_usbcksreq(false));
                trace!("Waiting for USB clock to switch");
                while system.usbckcr().read().usbcksrdy() {}
            }
            None => {}
        };
    });

    {
        let system = pac::SYSTEM;
        let hoco = system.hococr2().read().hcfrq0();
        let hoco: HertzU32 = match hoco {
            Hcfrq0::_16mhz => 16.MHz(),
            Hcfrq0::_18mhz => 18.MHz(),
            Hcfrq0::_20mhz => 20.MHz(),
            Hcfrq0::_32mhz => 32.MHz(),
            Hcfrq0::_48mhz => 48.MHz(),
            Hcfrq0::_RESERVED_3 | Hcfrq0::_RESERVED_5 | Hcfrq0::_RESERVED_6 => unimplemented!(),
        };

        let mosc = match config.mosc {
            Some(mosc) => match system.mosccr().read().mostp() {
                true => None,
                false => Some(mosc),
            },
            None => None,
        };

        let pll_running = !system.pllcr().read().pllstp();
        let pll = pll_status(
            pll_running,
            hoco,
            config.mosc,
            system.pllccr().read(),
            system.pllccr2().read(),
        );

        let pll2_running = !system.pll2cr().read().pllstp();
        let pll2 = pll_status(
            pll2_running,
            hoco,
            config.mosc,
            system.pll2ccr().read(),
            system.pll2ccr2().read(),
        );

        let cksel = system.sckscr().read().cksel();
        let root = match cksel {
            SckscrCksel::Hoco => hoco,
            SckscrCksel::Moco => 8_u32.MHz(),
            SckscrCksel::Mosc => mosc.expect("MOSC selected as clock source but not configured"),
            SckscrCksel::Sosc => 32_768_u32.Hz(),
            SckscrCksel::Pll1P => {
                pll.expect("PLL1.P selected as clock source but not running")
                    .0
            }
            SckscrCksel::_RESERVED_2 | SckscrCksel::_RESERVED_6 | SckscrCksel::_RESERVED_7 => {
                unimplemented!("Reserved value")
            }
        };

        let prescaler = system.sckdivcr().read();

        let iclk = match prescaler.ick() {
            Ick::Div1 => root,
            Ick::Div2 => root / 2,
            Ick::Div3 => root / 3,
            Ick::Div4 => root / 4,
            Ick::Div6 => root / 6,
            Ick::Div8 => root / 8,
            Ick::Div12 => root / 12,
            Ick::Div16 => root / 16,
            Ick::Div32 => root / 32,
            Ick::Div64 => root / 64,
            Ick::_RESERVED_7
            | Ick::_RESERVED_b
            | Ick::_RESERVED_c
            | Ick::_RESERVED_d
            | Ick::_RESERVED_e
            | Ick::_RESERVED_f => unimplemented!("Invalid sckdivcr.ick"),
        };

        let flash = match prescaler.fck() {
            Fck::Div1 => root,
            Fck::Div2 => root / 2,
            Fck::Div3 => root / 3,
            Fck::Div4 => root / 4,
            Fck::Div6 => root / 6,
            Fck::Div8 => root / 8,
            Fck::Div12 => root / 12,
            Fck::Div16 => root / 16,
            Fck::Div32 => root / 32,
            Fck::Div64 => root / 64,
            Fck::_RESERVED_7
            | Fck::_RESERVED_b
            | Fck::_RESERVED_c
            | Fck::_RESERVED_d
            | Fck::_RESERVED_e
            | Fck::_RESERVED_f => unimplemented!("Invalid sckdivcr.fck"),
        };

        let peripheral_a = match prescaler.pcka() {
            Pcka::Div1 => root,
            Pcka::Div2 => root / 2,
            Pcka::Div3 => root / 3,
            Pcka::Div4 => root / 4,
            Pcka::Div6 => root / 6,
            Pcka::Div8 => root / 8,
            Pcka::Div12 => root / 12,
            Pcka::Div16 => root / 16,
            Pcka::Div32 => root / 32,
            Pcka::Div64 => root / 64,
            Pcka::_RESERVED_7
            | Pcka::_RESERVED_b
            | Pcka::_RESERVED_c
            | Pcka::_RESERVED_d
            | Pcka::_RESERVED_e
            | Pcka::_RESERVED_f => unimplemented!("Invalid sckdivcr.pcka"),
        };

        let peripheral_b = match prescaler.pckb() {
            Pckb::Div1 => root,
            Pckb::Div2 => root / 2,
            Pckb::Div3 => root / 3,
            Pckb::Div4 => root / 4,
            Pckb::Div6 => root / 6,
            Pckb::Div8 => root / 8,
            Pckb::Div12 => root / 12,
            Pckb::Div16 => root / 16,
            Pckb::Div32 => root / 32,
            Pckb::Div64 => root / 64,
            Pckb::_RESERVED_7
            | Pckb::_RESERVED_b
            | Pckb::_RESERVED_c
            | Pckb::_RESERVED_d
            | Pckb::_RESERVED_e
            | Pckb::_RESERVED_f => unimplemented!("Invalid sckdivcr.pckb"),
        };

        let peripheral_c = match prescaler.pckc() {
            Pckc::Div1 => root,
            Pckc::Div2 => root / 2,
            Pckc::Div3 => root / 3,
            Pckc::Div4 => root / 4,
            Pckc::Div6 => root / 6,
            Pckc::Div8 => root / 8,
            Pckc::Div12 => root / 12,
            Pckc::Div16 => root / 16,
            Pckc::Div32 => root / 32,
            Pckc::Div64 => root / 64,
            Pckc::_RESERVED_7
            | Pckc::_RESERVED_b
            | Pckc::_RESERVED_c
            | Pckc::_RESERVED_d
            | Pckc::_RESERVED_e
            | Pckc::_RESERVED_f => unimplemented!("Invalid sckdivcr.pckc"),
        };

        let peripheral_d = match prescaler.pckd() {
            Pckd::Div1 => root,
            Pckd::Div2 => root / 2,
            Pckd::Div3 => root / 3,
            Pckd::Div4 => root / 4,
            Pckd::Div6 => root / 6,
            Pckd::Div8 => root / 8,
            Pckd::Div12 => root / 12,
            Pckd::Div16 => root / 16,
            Pckd::Div32 => root / 32,
            Pckd::Div64 => root / 64,
            Pckd::_RESERVED_7
            | Pckd::_RESERVED_b
            | Pckd::_RESERVED_c
            | Pckd::_RESERVED_d
            | Pckd::_RESERVED_e
            | Pckd::_RESERVED_f => unimplemented!("Invalid sckdivcr.pckd"),
        };

        let peripheral_e = match prescaler.pcke() {
            Pcke::Div1 => root,
            Pcke::Div2 => root / 2,
            Pcke::Div3 => root / 3,
            Pcke::Div4 => root / 4,
            Pcke::Div6 => root / 6,
            Pcke::Div8 => root / 8,
            Pcke::Div12 => root / 12,
            Pcke::Div16 => root / 16,
            Pcke::Div32 => root / 32,
            Pcke::Div64 => root / 64,
            Pcke::_RESERVED_7
            | Pcke::_RESERVED_b
            | Pcke::_RESERVED_c
            | Pcke::_RESERVED_d
            | Pcke::_RESERVED_e
            | Pcke::_RESERVED_f => unimplemented!("Invalid sckdivcr.pcke"),
        };

        let bus_clock = match prescaler.bck() {
            Bck::Div1 => root,
            Bck::Div2 => root / 2,
            Bck::Div3 => root / 3,
            Bck::Div4 => root / 4,
            Bck::Div6 => root / 6,
            Bck::Div8 => root / 8,
            Bck::Div12 => root / 12,
            Bck::Div16 => root / 16,
            Bck::Div32 => root / 32,
            Bck::Div64 => root / 64,
            Bck::_RESERVED_7
            | Bck::_RESERVED_b
            | Bck::_RESERVED_c
            | Bck::_RESERVED_d
            | Bck::_RESERVED_e
            | Bck::_RESERVED_f => unimplemented!("Invalid sckdivcr.bck"),
        };

        let usb = {
            let div = system.usbckdivcr().read().usbckdiv().to_divider();
            match config.usb {
                Some(UsbClockSource::PllP) => pll_frequency.map(|(pll_p, _, _)| pll_p / div),
                Some(UsbClockSource::PllQ) => pll_frequency.map(|(_, pll_q, _)| pll_q / div),
                Some(UsbClockSource::PllR) => pll_frequency.map(|(_, _, pll_r)| pll_r / div),
                Some(UsbClockSource::Pll2P) => pll2_frequency.map(|(pll_p, _, _)| pll_p / div),
                Some(UsbClockSource::Pll2Q) => pll2_frequency.map(|(_, pll_q, _)| pll_q / div),
                Some(UsbClockSource::Pll2R) => pll2_frequency.map(|(_, _, pll_r)| pll_r / div),
                Some(UsbClockSource::Hoco)
                | Some(UsbClockSource::Moco)
                | Some(UsbClockSource::Mosc) => todo!(),
                None => None,
            }
        };

        let sci = {
            let div = system.scickdivcr().read().ckdiv().to_divider();
            match config.sci {
                Some((SciClockSource::PllP, _)) => pll_frequency.map(|(pll_p, _, _)| pll_p / div),
                Some((SciClockSource::PllQ, _)) => pll_frequency.map(|(_, pll_q, _)| pll_q / div),
                Some((SciClockSource::PllR, _)) => pll_frequency.map(|(_, _, pll_r)| pll_r / div),
                Some((SciClockSource::Pll2P, _)) => pll2_frequency.map(|(pll_p, _, _)| pll_p / div),
                Some((SciClockSource::Pll2Q, _)) => pll2_frequency.map(|(_, pll_q, _)| pll_q / div),
                Some((SciClockSource::Pll2R, _)) => pll2_frequency.map(|(_, _, pll_r)| pll_r / div),
                Some((SciClockSource::Hoco, _))
                | Some((SciClockSource::Moco, _))
                | Some((SciClockSource::Loco, _))
                | Some((SciClockSource::Sosc, _))
                | Some((SciClockSource::Mosc, _)) => todo!(),
                None => None,
            }
        };

        let spi = {
            let div = system.spickdivcr().read().ckdiv().to_divider();
            match config.spi {
                Some((SpiClockSource::PllP, _)) => pll_frequency.map(|(pll_p, _, _)| pll_p / div),
                Some((SpiClockSource::PllQ, _)) => pll_frequency.map(|(_, pll_q, _)| pll_q / div),
                Some((SpiClockSource::PllR, _)) => pll_frequency.map(|(_, _, pll_r)| pll_r / div),
                Some((SpiClockSource::Pll2P, _)) => pll2_frequency.map(|(pll_p, _, _)| pll_p / div),
                Some((SpiClockSource::Pll2Q, _)) => pll2_frequency.map(|(_, pll_q, _)| pll_q / div),
                Some((SpiClockSource::Pll2R, _)) => pll2_frequency.map(|(_, _, pll_r)| pll_r / div),
                Some((SpiClockSource::Hoco, _))
                | Some((SpiClockSource::Moco, _))
                | Some((SpiClockSource::Loco, _))
                | Some((SpiClockSource::Sosc, _))
                | Some((SpiClockSource::Mosc, _)) => todo!(),
                None => None,
            }
        };

        CLOCK_STATUS
            .init(ClockStatus {
                master: root,
                mosc: config.mosc,
                sosc: config.sosc,
                hoco,
                pll,
                pll2,
                system: iclk,
                flash,
                peripheral_a,
                peripheral_b,
                peripheral_c,
                peripheral_d,
                peripheral_e,
                bus_clock,
                usb,
                sci,
                spi,
            })
            .or(Err(()))
    }
}

impl Default for ClockConfig {
    /// Config assumes EK-RA8M1.
    fn default() -> Self {
        Self {
            system: SystemClockSource::Pll1P,
            hoco: HocoFrequency::_48mhz,
            mosc: Some(20.MHz()),
            sosc: true,
            pll: Some(PllConfig {
                input: PllInput::Hoco,
                div: PllInDiv::Div4,
                mul: PllOutMul::Mul80_0,
                div_p: PllPDiv::Div2,
                div_q: PllQDiv::Div4,
                div_r: PllRDiv::Div2,
            }),
            pll2: None,
            usb: Some(UsbClockSource::PllQ),
            sci: Some((SciClockSource::PllQ, 30.MHz())),
            spi: Some((SpiClockSource::PllQ, 120.MHz())),
        }
    }
}

trait CkDivExt {
    fn to_divider(&self) -> u32;
}
impl CkDivExt for Scickdiv {
    fn to_divider(&self) -> u32 {
        match self {
            Self::Div1 => 1,
            Self::Div2 => 2,
            Self::Div3 => 3,
            Self::Div4 => 4,
            Self::Div5 => 5,
            Self::Div6 => 6,
            Self::Div8 => 8,
            Self::_RESERVED_7 => unimplemented!("Invalid sckdivcr.scickdiv"),
        }
    }
}

impl CkDivExt for Spickdiv {
    fn to_divider(&self) -> u32 {
        match self {
            Self::Div1 => 1,
            Self::Div2 => 2,
            Self::Div3 => 3,
            Self::Div4 => 4,
            Self::Div5 => 5,
            Self::Div6 => 6,
            Self::Div8 => 8,
            Self::_RESERVED_7 => unimplemented!("Invalid sckdivcr.spickdiv"),
        }
    }
}

impl CkDivExt for Usbckdiv {
    fn to_divider(&self) -> u32 {
        match self {
            Self::Div1 => 1,
            Self::Div2 => 2,
            Self::Div3 => 3,
            Self::Div4 => 4,
            Self::Div5 => 5,
            Self::Div6 => 6,
            Self::Div8 => 8,
            Self::_RESERVED_7 => unimplemented!("Invalid sckdivcr.usbckdiv"),
        }
    }
}