use embedded_hal::prelude::_embedded_hal_blocking_delay_DelayUs;
use libboard_zynq::timer::GlobalTimer;
use log::info;

use crate::pl::csr;

#[cfg(feature = "target_kasli_soc")]
const ADDRESS: u8 = 0x67;

const ADPLL_MAX: i32 = (950.0 / 0.0001164) as i32;

pub struct DividerConfig {
    pub hsdiv: u16,
    pub lsdiv: u8,
    pub fbdiv: u64,
}

pub struct FrequencySetting {
    pub main: DividerConfig,
    pub helper: DividerConfig,
}

mod i2c {
    use super::*;

    #[derive(Clone, Copy)]
    pub enum DCXO {
        Main,
        Helper,
    }

    fn half_period(timer: &mut GlobalTimer) {
        timer.delay_us(1)
    }

    fn sda_i(dcxo: DCXO) -> bool {
        match dcxo {
            DCXO::Main => unsafe { csr::wrpll::main_dcxo_sda_in_read() == 1 },
            DCXO::Helper => unsafe { csr::wrpll::helper_dcxo_sda_in_read() == 1 },
        }
    }

    fn sda_oe(dcxo: DCXO, oe: bool) {
        let val = if oe { 1 } else { 0 };
        match dcxo {
            DCXO::Main => unsafe { csr::wrpll::main_dcxo_sda_oe_write(val) },
            DCXO::Helper => unsafe { csr::wrpll::helper_dcxo_sda_oe_write(val) },
        };
    }

    fn sda_o(dcxo: DCXO, o: bool) {
        let val = if o { 1 } else { 0 };
        match dcxo {
            DCXO::Main => unsafe { csr::wrpll::main_dcxo_sda_out_write(val) },
            DCXO::Helper => unsafe { csr::wrpll::helper_dcxo_sda_out_write(val) },
        };
    }

    fn scl_oe(dcxo: DCXO, oe: bool) {
        let val = if oe { 1 } else { 0 };
        match dcxo {
            DCXO::Main => unsafe { csr::wrpll::main_dcxo_scl_oe_write(val) },
            DCXO::Helper => unsafe { csr::wrpll::helper_dcxo_scl_oe_write(val) },
        };
    }

    fn scl_o(dcxo: DCXO, o: bool) {
        let val = if o { 1 } else { 0 };
        match dcxo {
            DCXO::Main => unsafe { csr::wrpll::main_dcxo_scl_out_write(val) },
            DCXO::Helper => unsafe { csr::wrpll::helper_dcxo_scl_out_write(val) },
        };
    }

    pub fn init(dcxo: DCXO, timer: &mut GlobalTimer) -> Result<(), &'static str> {
        // Set SCL as output, and high level
        scl_o(dcxo, true);
        scl_oe(dcxo, true);
        // Prepare a zero level on SDA so that sda_oe pulls it down
        sda_o(dcxo, false);
        // Release SDA
        sda_oe(dcxo, false);

        // Check the I2C bus is ready
        half_period(timer);
        half_period(timer);
        if !sda_i(dcxo) {
            // Try toggling SCL a few times
            for _bit in 0..8 {
                scl_o(dcxo, false);
                half_period(timer);
                scl_o(dcxo, true);
                half_period(timer);
            }
        }

        if !sda_i(dcxo) {
            return Err("SDA is stuck low and doesn't get unstuck");
        }
        Ok(())
    }

    pub fn start(dcxo: DCXO, timer: &mut GlobalTimer) {
        // Set SCL high then SDA low
        scl_o(dcxo, true);
        half_period(timer);
        sda_oe(dcxo, true);
        half_period(timer);
    }

    pub fn stop(dcxo: DCXO, timer: &mut GlobalTimer) {
        // First, make sure SCL is low, so that the target releases the SDA line
        scl_o(dcxo, false);
        half_period(timer);
        // Set SCL high then SDA high
        sda_oe(dcxo, true);
        scl_o(dcxo, true);
        half_period(timer);
        sda_oe(dcxo, false);
        half_period(timer);
    }

    pub fn write(dcxo: DCXO, data: u8, timer: &mut GlobalTimer) -> bool {
        // MSB first
        for bit in (0..8).rev() {
            // Set SCL low and set our bit on SDA
            scl_o(dcxo, false);
            sda_oe(dcxo, data & (1 << bit) == 0);
            half_period(timer);
            // Set SCL high ; data is shifted on the rising edge of SCL
            scl_o(dcxo, true);
            half_period(timer);
        }
        // Check ack
        // Set SCL low, then release SDA so that the I2C target can respond
        scl_o(dcxo, false);
        half_period(timer);
        sda_oe(dcxo, false);
        // Set SCL high and check for ack
        scl_o(dcxo, true);
        half_period(timer);
        // returns true if acked (I2C target pulled SDA low)
        !sda_i(dcxo)
    }

    pub fn read(dcxo: DCXO, ack: bool, timer: &mut GlobalTimer) -> u8 {
        // Set SCL low first, otherwise setting SDA as input may cause a transition
        // on SDA with SCL high which will be interpreted as START/STOP condition.
        scl_o(dcxo, false);
        half_period(timer); // make sure SCL has settled low
        sda_oe(dcxo, false);

        let mut data: u8 = 0;

        // MSB first
        for bit in (0..8).rev() {
            scl_o(dcxo, false);
            half_period(timer);
            // Set SCL high and shift data
            scl_o(dcxo, true);
            half_period(timer);
            if sda_i(dcxo) {
                data |= 1 << bit
            }
        }
        // Send ack
        // Set SCL low and pull SDA low when acking
        scl_o(dcxo, false);
        if ack {
            sda_oe(dcxo, true)
        }
        half_period(timer);
        // then set SCL high
        scl_o(dcxo, true);
        half_period(timer);

        data
    }
}

fn write(dcxo: i2c::DCXO, reg: u8, val: u8, timer: &mut GlobalTimer) -> Result<(), &'static str> {
    i2c::start(dcxo, timer);
    if !i2c::write(dcxo, ADDRESS << 1, timer) {
        return Err("Si549 failed to ack write address");
    }
    if !i2c::write(dcxo, reg, timer) {
        return Err("Si549 failed to ack register");
    }
    if !i2c::write(dcxo, val, timer) {
        return Err("Si549 failed to ack value");
    }
    i2c::stop(dcxo, timer);
    Ok(())
}

fn read(dcxo: i2c::DCXO, reg: u8, timer: &mut GlobalTimer) -> Result<u8, &'static str> {
    i2c::start(dcxo, timer);
    if !i2c::write(dcxo, ADDRESS << 1, timer) {
        return Err("Si549 failed to ack write address");
    }
    if !i2c::write(dcxo, reg, timer) {
        return Err("Si549 failed to ack register");
    }
    i2c::stop(dcxo, timer);

    i2c::start(dcxo, timer);
    if !i2c::write(dcxo, (ADDRESS << 1) | 1, timer) {
        return Err("Si549 failed to ack read address");
    }
    let val = i2c::read(dcxo, false, timer);
    i2c::stop(dcxo, timer);
    Ok(val)
}

fn setup(dcxo: i2c::DCXO, config: &DividerConfig, timer: &mut GlobalTimer) -> Result<(), &'static str> {
    i2c::init(dcxo, timer)?;

    write(dcxo, 255, 0x00, timer)?; // PAGE
    write(dcxo, 69, 0x00, timer)?; // Disable FCAL override.
    write(dcxo, 17, 0x00, timer)?; // Synchronously disable output

    // The Si549 has no ID register, so we check that it responds correctly
    // by writing values to a RAM-like register and reading them back.
    for test_value in 0..255 {
        write(dcxo, 23, test_value, timer)?;
        let readback = read(dcxo, 23, timer)?;
        if readback != test_value {
            return Err("Si549 detection failed");
        }
    }

    write(dcxo, 23, config.hsdiv as u8, timer)?;
    write(dcxo, 24, (config.hsdiv >> 8) as u8 | (config.lsdiv << 4), timer)?;
    write(dcxo, 26, config.fbdiv as u8, timer)?;
    write(dcxo, 27, (config.fbdiv >> 8) as u8, timer)?;
    write(dcxo, 28, (config.fbdiv >> 16) as u8, timer)?;
    write(dcxo, 29, (config.fbdiv >> 24) as u8, timer)?;
    write(dcxo, 30, (config.fbdiv >> 32) as u8, timer)?;
    write(dcxo, 31, (config.fbdiv >> 40) as u8, timer)?;

    write(dcxo, 7, 0x08, timer)?; // Start FCAL
    timer.delay_us(30_000); // Internal FCAL VCO calibration
    write(dcxo, 17, 0x01, timer)?; // Synchronously enable output

    Ok(())
}

pub fn main_setup(timer: &mut GlobalTimer, settings: &FrequencySetting) -> Result<(), &'static str> {
    unsafe {
        csr::wrpll::main_dcxo_bitbang_enable_write(1);
        csr::wrpll::main_dcxo_i2c_address_write(ADDRESS);
    }

    setup(i2c::DCXO::Main, &settings.main, timer)?;

    // Si549 maximum settling time for large frequency change.
    timer.delay_us(40_000);

    unsafe {
        csr::wrpll::main_dcxo_bitbang_enable_write(0);
    }

    info!("Main Si549 started");
    Ok(())
}

pub fn helper_setup(timer: &mut GlobalTimer, settings: &FrequencySetting) -> Result<(), &'static str> {
    unsafe {
        csr::wrpll::helper_reset_write(1);
        csr::wrpll::helper_dcxo_bitbang_enable_write(1);
        csr::wrpll::helper_dcxo_i2c_address_write(ADDRESS);
    }

    setup(i2c::DCXO::Helper, &settings.helper, timer)?;

    // Si549 maximum settling time for large frequency change.
    timer.delay_us(40_000);

    unsafe {
        csr::wrpll::helper_reset_write(0);
        csr::wrpll::helper_dcxo_bitbang_enable_write(0);
    }
    info!("Helper Si549 started");
    Ok(())
}

fn set_adpll(dcxo: i2c::DCXO, adpll: i32) -> Result<(), &'static str> {
    if adpll.abs() > ADPLL_MAX {
        return Err("adpll is too large");
    }

    match dcxo {
        i2c::DCXO::Main => unsafe {
            if csr::wrpll::main_dcxo_bitbang_enable_read() == 1 {
                return Err("Main si549 bitbang mode is active when using gateware i2c");
            }

            while csr::wrpll::main_dcxo_adpll_busy_read() == 1 {}
            if csr::wrpll::main_dcxo_nack_read() == 1 {
                return Err("Main si549 failed to ack adpll write");
            }

            csr::wrpll::main_dcxo_i2c_address_write(ADDRESS);
            csr::wrpll::main_dcxo_adpll_write(adpll as u32);

            csr::wrpll::main_dcxo_adpll_stb_write(1);
        },
        i2c::DCXO::Helper => unsafe {
            if csr::wrpll::helper_dcxo_bitbang_enable_read() == 1 {
                return Err("Helper si549 bitbang mode is active when using gateware i2c");
            }

            while csr::wrpll::helper_dcxo_adpll_busy_read() == 1 {}
            if csr::wrpll::helper_dcxo_nack_read() == 1 {
                return Err("Helper si549 failed to ack adpll write");
            }

            csr::wrpll::helper_dcxo_i2c_address_write(ADDRESS);
            csr::wrpll::helper_dcxo_adpll_write(adpll as u32);

            csr::wrpll::helper_dcxo_adpll_stb_write(1);
        },
    };

    Ok(())
}

#[cfg(has_wrpll)]
pub mod wrpll {

    use super::*;

    const BEATING_PERIOD: i32 = 0x8000;
    const BEATING_HALFPERIOD: i32 = 0x4000;
    const COUNTER_WIDTH: u32 = 24;
    const DIV_WIDTH: u32 = 2;

    const KP: i32 = 6;
    const KI: i32 = 2;
    // 4 ppm capture range
    const ADPLL_LIM: i32 = (4.0 / 0.0001164) as i32;

    static mut BASE_ADPLL: i32 = 0;
    static mut H_LAST_ADPLL: i32 = 0;
    static mut LAST_PERIOD_ERR: i32 = 0;
    static mut M_LAST_ADPLL: i32 = 0;
    static mut LAST_PHASE_ERR: i32 = 0;

    #[derive(Clone, Copy)]
    pub enum ISR {
        RefTag,
        MainTag,
    }

    mod tag_collector {
        use super::*;

        #[cfg(wrpll_ref_clk = "GT_CDR")]
        static mut TAG_OFFSET: u32 = 19050;
        #[cfg(wrpll_ref_clk = "SMA_CLKIN")]
        static mut TAG_OFFSET: u32 = 0;
        static mut REF_TAG: u32 = 0;
        static mut REF_TAG_READY: bool = false;
        static mut MAIN_TAG: u32 = 0;
        static mut MAIN_TAG_READY: bool = false;

        pub fn reset() {
            clear_phase_diff_ready();
            unsafe {
                REF_TAG = 0;
                MAIN_TAG = 0;
            }
        }

        pub fn clear_phase_diff_ready() {
            unsafe {
                REF_TAG_READY = false;
                MAIN_TAG_READY = false;
            }
        }

        pub fn collect_tags(interrupt: ISR) {
            match interrupt {
                ISR::RefTag => unsafe {
                    REF_TAG = csr::wrpll::ref_tag_read();
                    REF_TAG_READY = true;
                },
                ISR::MainTag => unsafe {
                    MAIN_TAG = csr::wrpll::main_tag_read();
                    MAIN_TAG_READY = true;
                },
            }
        }

        pub fn phase_diff_ready() -> bool {
            unsafe { REF_TAG_READY && MAIN_TAG_READY }
        }

        #[cfg(feature = "calibrate_wrpll_skew")]
        pub fn set_tag_offset(offset: u32) {
            unsafe {
                TAG_OFFSET = offset;
            }
        }

        #[cfg(feature = "calibrate_wrpll_skew")]
        pub fn get_tag_offset() -> u32 {
            unsafe { TAG_OFFSET }
        }

        pub fn get_period_error() -> i32 {
            // n * BEATING_PERIOD - REF_TAG(n) mod BEATING_PERIOD
            let mut period_error = unsafe { REF_TAG.overflowing_neg().0.rem_euclid(BEATING_PERIOD as u32) as i32 };
            // mapping tags from [0, 2π] -> [-π, π]
            if period_error > BEATING_HALFPERIOD {
                period_error -= BEATING_PERIOD
            }
            period_error
        }

        pub fn get_phase_error() -> i32 {
            // MAIN_TAG(n) - REF_TAG(n) - TAG_OFFSET mod BEATING_PERIOD
            let mut phase_error = unsafe {
                MAIN_TAG
                    .overflowing_sub(REF_TAG + TAG_OFFSET)
                    .0
                    .rem_euclid(BEATING_PERIOD as u32) as i32
            };

            // mapping tags from [0, 2π] -> [-π, π]
            if phase_error > BEATING_HALFPERIOD {
                phase_error -= BEATING_PERIOD
            }
            phase_error
        }
    }

    fn set_isr(en: bool) {
        let val = if en { 1 } else { 0 };
        unsafe {
            csr::wrpll::ref_tag_ev_enable_write(val);
            csr::wrpll::main_tag_ev_enable_write(val);
        }
    }

    fn set_base_adpll() -> Result<(), &'static str> {
        let count2adpll =
            |error: i32| ((error as f64 * 1e6) / (0.0001164 * (1 << (COUNTER_WIDTH - DIV_WIDTH)) as f64)) as i32;

        let (ref_count, main_count) = get_freq_counts();
        unsafe {
            BASE_ADPLL = count2adpll(ref_count as i32 - main_count as i32);
            set_adpll(i2c::DCXO::Main, BASE_ADPLL)?;
            set_adpll(i2c::DCXO::Helper, BASE_ADPLL)?;
        }
        Ok(())
    }

    fn get_freq_counts() -> (u32, u32) {
        unsafe {
            csr::wrpll::frequency_counter_update_write(1);
            while csr::wrpll::frequency_counter_busy_read() == 1 {}
            #[cfg(wrpll_ref_clk = "GT_CDR")]
            let ref_count = csr::wrpll::frequency_counter_counter_rtio_rx0_read();
            #[cfg(wrpll_ref_clk = "SMA_CLKIN")]
            let ref_count = csr::wrpll::frequency_counter_counter_ref_read();
            let main_count = csr::wrpll::frequency_counter_counter_sys_read();

            (ref_count, main_count)
        }
    }

    fn reset_plls(timer: &mut GlobalTimer) -> Result<(), &'static str> {
        unsafe {
            H_LAST_ADPLL = 0;
            LAST_PERIOD_ERR = 0;
            M_LAST_ADPLL = 0;
            LAST_PHASE_ERR = 0;
        }
        set_adpll(i2c::DCXO::Main, 0)?;
        set_adpll(i2c::DCXO::Helper, 0)?;
        // wait for adpll to transfer and DCXO to settle
        timer.delay_us(200);
        Ok(())
    }

    fn clear_pending(interrupt: ISR) {
        match interrupt {
            ISR::RefTag => unsafe { csr::wrpll::ref_tag_ev_pending_write(1) },
            ISR::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) },
        };
    }

    fn is_pending(interrupt: ISR) -> bool {
        match interrupt {
            ISR::RefTag => unsafe { csr::wrpll::ref_tag_ev_pending_read() == 1 },
            ISR::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_read() == 1 },
        }
    }

    pub fn interrupt_handler() {
        if is_pending(ISR::RefTag) {
            tag_collector::collect_tags(ISR::RefTag);
            clear_pending(ISR::RefTag);
            helper_pll().expect("failed to run helper DCXO PLL");
        }

        if is_pending(ISR::MainTag) {
            tag_collector::collect_tags(ISR::MainTag);
            clear_pending(ISR::MainTag);
        }

        if tag_collector::phase_diff_ready() {
            main_pll().expect("failed to run main DCXO PLL");
            tag_collector::clear_phase_diff_ready();
        }
    }

    fn helper_pll() -> Result<(), &'static str> {
        let period_err = tag_collector::get_period_error();
        unsafe {
            // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw?view#Integral-wind-up-and-output-limiting
            let adpll = (H_LAST_ADPLL + (KP + KI) * period_err - KP * LAST_PERIOD_ERR).clamp(-ADPLL_LIM, ADPLL_LIM);
            set_adpll(i2c::DCXO::Helper, BASE_ADPLL + adpll)?;
            H_LAST_ADPLL = adpll;
            LAST_PERIOD_ERR = period_err;
        };
        Ok(())
    }

    fn main_pll() -> Result<(), &'static str> {
        let phase_err = tag_collector::get_phase_error();
        unsafe {
            // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw?view#Integral-wind-up-and-output-limiting
            let adpll = (M_LAST_ADPLL + (KP + KI) * phase_err - KP * LAST_PHASE_ERR).clamp(-ADPLL_LIM, ADPLL_LIM);
            set_adpll(i2c::DCXO::Main, BASE_ADPLL + adpll)?;
            M_LAST_ADPLL = adpll;
            LAST_PHASE_ERR = phase_err;
        };
        Ok(())
    }

    #[cfg(wrpll_ref_clk = "GT_CDR")]
    fn test_skew(timer: &mut GlobalTimer) -> Result<(), &'static str> {
        // wait for PLL to stabilize
        timer.delay_us(20_000);

        info!("testing the skew of SYS CLK...");
        if has_timing_error(timer) {
            return Err("the skew cannot satisfy setup/hold time constraint of RX synchronizer");
        }
        info!("the skew of SYS CLK met the timing constraint");
        Ok(())
    }

    #[cfg(wrpll_ref_clk = "GT_CDR")]
    fn has_timing_error(timer: &mut GlobalTimer) -> bool {
        unsafe {
            csr::wrpll_skewtester::error_write(1);
        }
        timer.delay_us(5_000);
        unsafe { csr::wrpll_skewtester::error_read() == 1 }
    }

    #[cfg(feature = "calibrate_wrpll_skew")]
    fn find_edge(target: bool, timer: &mut GlobalTimer) -> Result<u32, &'static str> {
        const STEP: u32 = 8;
        const STABLE_THRESHOLD: u32 = 10;

        enum FSM {
            Init,
            WaitEdge,
            GotEdge,
        }

        let mut state: FSM = FSM::Init;
        let mut offset: u32 = tag_collector::get_tag_offset();
        let mut median_edge: u32 = 0;
        let mut stable_counter: u32 = 0;

        for _ in 0..(BEATING_PERIOD as u32 / STEP) as usize {
            tag_collector::set_tag_offset(offset);
            offset += STEP;
            // wait for PLL to stabilize
            timer.delay_us(20_000);

            let error = has_timing_error(timer);
            // A median edge deglitcher
            match state {
                FSM::Init => {
                    if error != target {
                        stable_counter += 1;
                    } else {
                        stable_counter = 0;
                    }

                    if stable_counter >= STABLE_THRESHOLD {
                        state = FSM::WaitEdge;
                        stable_counter = 0;
                    }
                }
                FSM::WaitEdge => {
                    if error == target {
                        state = FSM::GotEdge;
                        median_edge = offset;
                    }
                }
                FSM::GotEdge => {
                    if error != target {
                        median_edge += STEP;
                        stable_counter = 0;
                    } else {
                        stable_counter += 1;
                    }

                    if stable_counter >= STABLE_THRESHOLD {
                        return Ok(median_edge);
                    }
                }
            }
        }
        return Err("failed to find timing error edge");
    }

    #[cfg(feature = "calibrate_wrpll_skew")]
    fn calibrate_skew(timer: &mut GlobalTimer) -> Result<(), &'static str> {
        info!("calibrating skew to meet timing constraint...");

        // clear calibrated value
        tag_collector::set_tag_offset(0);
        let rising = find_edge(true, timer)? as i32;
        let falling = find_edge(false, timer)? as i32;

        let width = BEATING_PERIOD - (falling - rising);
        let result = falling + width / 2;
        tag_collector::set_tag_offset(result as u32);

        info!(
            "calibration successful, error zone: {} -> {}, width: {} ({}deg), middle of working region: {}",
            rising,
            falling,
            width,
            360 * width / BEATING_PERIOD,
            result,
        );

        Ok(())
    }

    pub fn select_recovered_clock(rc: bool, timer: &mut GlobalTimer) {
        set_isr(false);

        if rc {
            tag_collector::reset();
            reset_plls(timer).expect("failed to reset main and helper PLL");

            // get within capture range
            set_base_adpll().expect("failed to set base adpll");

            // clear gateware pending flag
            clear_pending(ISR::RefTag);
            clear_pending(ISR::MainTag);

            // use nFIQ to avoid IRQ being disabled by mutex lock and mess up PLL
            set_isr(true);
            info!("WRPLL interrupt enabled");

            #[cfg(feature = "calibrate_wrpll_skew")]
            calibrate_skew(timer).expect("failed to set the correct skew");

            #[cfg(wrpll_ref_clk = "GT_CDR")]
            test_skew(timer).expect("skew test failed");
        }
    }
}

#[cfg(has_wrpll_refclk)]
pub mod wrpll_refclk {
    use super::*;

    pub struct MmcmSetting {
        pub clkout0_reg1: u16,  //0x08
        pub clkout0_reg2: u16,  //0x09
        pub clkfbout_reg1: u16, //0x14
        pub clkfbout_reg2: u16, //0x15
        pub div_reg: u16,       //0x16
        pub lock_reg1: u16,     //0x18
        pub lock_reg2: u16,     //0x19
        pub lock_reg3: u16,     //0x1A
        pub power_reg: u16,     //0x28
        pub filt_reg1: u16,     //0x4E
        pub filt_reg2: u16,     //0x4F
    }

    fn one_clock_cycle() {
        unsafe {
            csr::wrpll_refclk::mmcm_dclk_write(1);
            csr::wrpll_refclk::mmcm_dclk_write(0);
        }
    }

    fn set_addr(address: u8) {
        unsafe {
            csr::wrpll_refclk::mmcm_daddr_write(address);
        }
    }

    fn set_data(value: u16) {
        unsafe {
            csr::wrpll_refclk::mmcm_din_write(value);
        }
    }

    fn set_enable(en: bool) {
        unsafe {
            let val = if en { 1 } else { 0 };
            csr::wrpll_refclk::mmcm_den_write(val);
        }
    }

    fn set_write_enable(en: bool) {
        unsafe {
            let val = if en { 1 } else { 0 };
            csr::wrpll_refclk::mmcm_dwen_write(val);
        }
    }

    fn get_data() -> u16 {
        unsafe { csr::wrpll_refclk::mmcm_dout_read() }
    }

    fn drp_ready() -> bool {
        unsafe { csr::wrpll_refclk::mmcm_dready_read() == 1 }
    }

    #[allow(dead_code)]
    fn read(address: u8) -> u16 {
        set_addr(address);
        set_enable(true);
        // Set DADDR on the mmcm and assert DEN for one clock cycle
        one_clock_cycle();

        set_enable(false);
        while !drp_ready() {
            // keep the clock signal until data is ready
            one_clock_cycle();
        }
        get_data()
    }

    fn write(address: u8, value: u16) {
        set_addr(address);
        set_data(value);
        set_write_enable(true);
        set_enable(true);
        // Set DADDR, DI on the mmcm and assert DWE, DEN for one clock cycle
        one_clock_cycle();

        set_write_enable(false);
        set_enable(false);
        while !drp_ready() {
            // keep the clock signal until write is finished
            one_clock_cycle();
        }
    }

    fn reset(rst: bool) {
        unsafe {
            let val = if rst { 1 } else { 0 };
            csr::wrpll_refclk::mmcm_reset_write(val)
        }
    }

    pub fn setup(timer: &mut GlobalTimer, settings: MmcmSetting, mmcm_bypass: bool) -> Result<(), &'static str> {
        unsafe {
            csr::wrpll_refclk::refclk_reset_write(1);
        }

        if mmcm_bypass {
            info!("Bypassing mmcm");
            unsafe {
                csr::wrpll_refclk::mmcm_bypass_write(1);
            }
        } else {
            // Based on "DRP State Machine" from XAPP888
            // hold reset HIGH during mmcm config
            reset(true);
            write(0x08, settings.clkout0_reg1);
            write(0x09, settings.clkout0_reg2);
            write(0x14, settings.clkfbout_reg1);
            write(0x15, settings.clkfbout_reg2);
            write(0x16, settings.div_reg);
            write(0x18, settings.lock_reg1);
            write(0x19, settings.lock_reg2);
            write(0x1A, settings.lock_reg3);
            write(0x28, settings.power_reg);
            write(0x4E, settings.filt_reg1);
            write(0x4F, settings.filt_reg2);
            reset(false);

            // wait for the mmcm to lock
            timer.delay_us(100);

            let locked = unsafe { csr::wrpll_refclk::mmcm_locked_read() == 1 };
            if !locked {
                return Err("mmcm failed to generate 125MHz ref clock from SMA CLKIN");
            }
        }

        unsafe {
            csr::wrpll_refclk::refclk_reset_write(0);
        }

        Ok(())
    }
}