diff --git a/libs/bao1x-hal/src/clocks.rs b/libs/bao1x-hal/src/clocks.rs index d0e1295ad..890800638 100644 --- a/libs/bao1x-hal/src/clocks.rs +++ b/libs/bao1x-hal/src/clocks.rs @@ -1,5 +1,32 @@ +#[cfg(feature = "std")] +use bao1x_api::IoxHal; +use bitbybit::bitfield; use utralib::*; +#[cfg(feature = "std")] +use crate::i2c::I2c; + +#[bitfield(u32)] +#[derive(PartialEq, Eq, Debug)] +pub struct PmuControl { + #[bit(6, rw)] + pmu_2p5v_ena: bool, + #[bit(5, rw)] + pmu_0p8v_ana_ena: bool, + #[bit(4, rw)] + pmu_0p8v_dig_ena: bool, + #[bit(3, rw)] + iout_ref_ena: bool, + #[bit(2, rw)] + power_on_control: bool, + #[bit(1, rw)] + // when true sets 0.8v regulator to 0.9v + pmu_0p8v_ana_hv_ena: bool, + #[bit(0, rw)] + // when true sets 0.8v regulator to 0.9v + pmu_0p8v_dig_hv_ena: bool, +} + /// Requires: /// `freq_hz`: target frequency of PLL - generally it is 2x of the CPU clock frequency /// `daric_cgu`: Pointer to SYSCTL_BASE - for setting clocks @@ -20,14 +47,17 @@ use utralib::*; /// using direct offsets and a write_volatile(). This was done to ensure that the compiler is not /// optimizing out repeated writes. This could be cleaned up to use the UTRA abstractions, but because /// the code works and it was pretty hard to get it validated, there is an incentive not to touch it. -pub unsafe fn init_clock_asic( +pub unsafe fn init_clock_asic( freq_hz: u32, cgu_base: usize, ao_sysctl_base: usize, duart_base: Option, - delay_at_sysfreq: fn(usize, u32), + delay_at_sysfreq: F, fast_bio: bool, -) -> u32 { +) -> u32 +where + F: Fn(usize, u32), +{ use utra::sysctrl; let daric_cgu = cgu_base as *mut u32; let mut cgu = CSR::new(daric_cgu); @@ -62,14 +92,14 @@ pub unsafe fn init_clock_asic( // Hits a 16:8:4:2:1 ratio on fclk:aclk:hclk:iclk:pclk // Resulting in 800:400:200:100:50 MHz assuming 800MHz fclk if fast_bio { - daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_0.offset()).write_volatile(0x3fff); // fclk + daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_0.offset()).write_volatile(0x0700_01ff); // fclk } else { - daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_0.offset()).write_volatile(0x3f7f); // fclk + daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_0.offset()).write_volatile(0x0700_017f); // fclk } - daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_1.offset()).write_volatile(0x3f7f); // aclk - daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_2.offset()).write_volatile(0x1f3f); // hclk - daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_3.offset()).write_volatile(0x0f1f); // iclk - daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_4.offset()).write_volatile(0x070f); // pclk + daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_1.offset()).write_volatile(0x0f00_0f7f); // aclk + daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_2.offset()).write_volatile(0x1f01_073f); // hclk + daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_3.offset()).write_volatile(0x3f03_031f); // iclk + daric_cgu.add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_4.offset()).write_volatile(0x7f0f_010f); // pclk // calculate perclk divider. Target 100MHz. @@ -230,3 +260,536 @@ pub unsafe fn init_clock_asic( perclk } + +#[cfg(feature = "std")] +pub struct ClockManager { + pub vco_freq: u32, + pub fclk: u32, + pub aclk: u32, + pub hclk: u32, + pub iclk: u32, + pub pclk: u32, + pub perclk: u32, + sysctrl: CSR, + // this is a clone of what's in the kpc_aoint + ao_sysctrl: CSR, + i2c: I2c, + iox: IoxHal, +} + +/// maximum value representable in the fd counter +#[cfg(feature = "std")] +const FD_MAX: u32 = 256; + +#[cfg(feature = "std")] +impl ClockManager { + pub fn new() -> Result { + let sysctrl_mem = xous::map_memory( + xous::MemoryAddress::new(utra::sysctrl::HW_SYSCTRL_BASE), + None, + 4096, + xous::MemoryFlags::R | xous::MemoryFlags::W, + )?; + // this is actually double-mapped: once by us, and once by the KPC + let ao_mem = xous::map_memory( + xous::MemoryAddress::new(utra::ao_sysctrl::HW_AO_SYSCTRL_BASE), + None, + 4096, + xous::MemoryFlags::R | xous::MemoryFlags::W, + )?; + + const MHZ: u32 = 1_000_000; + // compute the actual frequency values by reading the PLL config + let sysctrl = CSR::new(sysctrl_mem.as_mut_ptr() as *mut u32); + let m = (sysctrl.r(utra::sysctrl::SFR_IPCPLLMN) >> 12) & 0xF; + let n = sysctrl.r(utra::sysctrl::SFR_IPCPLLMN) & 0xFFF; + let q1 = (sysctrl.r(utra::sysctrl::SFR_IPCPLLQ) >> 4) & 0x7; + let q0 = (sysctrl.r(utra::sysctrl::SFR_IPCPLLQ) >> 0) & 0x7; + let fracen = sysctrl.r(utra::sysctrl::SFR_IPCPLLF) & 0x100_0000 != 0; + let frac = sysctrl.r(utra::sysctrl::SFR_IPCPLLF) & 0xFF_FFFF; + let vco_freq = + if fracen { ((48 * n + (48 * frac) / (1 << 24)) / m) * MHZ } else { ((48 * n) / m) * MHZ }; + let pll0_freq = vco_freq / (1 + q0) / (1 + q1); + let fclk_fd = sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_0) & 0xFF; + let aclk_fd = sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_1) & 0xFF; + let hclk_fd = sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_2) & 0xFF; + let iclk_fd = sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_3) & 0xFF; + let pclk_fd = sysctrl.r(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_4) & 0xFF; + + let fclk = ClockManager::divide_by_fd(fclk_fd, pll0_freq); + let aclk = ClockManager::divide_by_fd(aclk_fd, pll0_freq); + let hclk = ClockManager::divide_by_fd(hclk_fd, pll0_freq); + let iclk = ClockManager::divide_by_fd(iclk_fd, pll0_freq); + let pclk = ClockManager::divide_by_fd(pclk_fd, pll0_freq); + // perclk has an extra /2 applied to it + log::info!("perclk: {:x}, pll0_freq {}", sysctrl.r(utra::sysctrl::SFR_CGUFDPER), pll0_freq); + let perclk_fd = sysctrl.r(utra::sysctrl::SFR_CGUFDPER) & 0xFF; + let perclk = ClockManager::divide_by_fd(perclk_fd, pll0_freq) / 2; + Ok(Self { + vco_freq, // 1_392_000_000, + fclk, // 348_000_000, + aclk, // 348_000_000, + hclk, + iclk, + pclk, + perclk, + sysctrl, + ao_sysctrl: CSR::new(ao_mem.as_ptr() as *mut u32), + i2c: I2c::new(), + iox: IoxHal::new(), + }) + } + + pub fn divide_by_fd(fd: u32, in_freq_hz: u32) -> u32 { + // shift by 1_000 to prevent overflow + let in_freq_khz = in_freq_hz / 1_000; + let out_freq_khz = (in_freq_khz * (fd + 1)) / FD_MAX; + // restore to Hz + out_freq_khz * 1_000 + } + + pub fn measured_freqs(&self) -> Vec<(String, u32)> { + let mut readings = Vec::new(); + let clk_desc: [(&'static str, u32, usize); 8] = [ + ("fclk", 16, 0x40 / size_of::()), + ("pke", 0, 0x40 / size_of::()), + ("ao", 16, 0x44 / size_of::()), + ("aoram", 0, 0x44 / size_of::()), + ("osc", 16, 0x48 / size_of::()), + ("xtal", 0, 0x48 / size_of::()), + ("pll0", 16, 0x4c / size_of::()), + ("pll1", 0, 0x4c / size_of::()), + ]; + let fsvalid = self.sysctrl.r(utra::sysctrl::SFR_CGUFSVLD); + for (i, &(name, shift, offset)) in clk_desc.iter().enumerate() { + if (1 << i as u32) & fsvalid != 0 { + let fsfreq = unsafe { (self.sysctrl.base().add(offset).read_volatile() >> shift) & 0xffff }; + readings.push((name.to_string(), fsfreq)); + } + } + readings + } + + pub fn request_freq(&mut self, cpu_freq_mhz: u32) -> Result { + if cpu_freq_mhz < 50 || cpu_freq_mhz > 800 { + return Err("Requested frequency out of range".into()); + } + // fclk top frequency is 2x of cpu frequency, in hz + let fclk_freq = cpu_freq_mhz * 1_000_000 * 2; + crate::println!("req_freq: {}", fclk_freq); + let perclk = unsafe { + init_clock_asic( + fclk_freq, + self.sysctrl.base() as usize, + self.ao_sysctrl.base() as usize, + None, + |ms: usize, _freq: u32| { + for i in 0..ms { + crate::println!("delay {}", i); + } + }, + false, + ) + }; + Ok(perclk) + } + + pub fn wfi(&mut self) { + crate::println!("entering wfi"); + + let (port, pin) = crate::board::setup_dcdc2_pin(&self.iox); + crate::println!("dcdc2 pin setup"); + let mut pmic = crate::axp2101::Axp2101::new(&mut self.i2c).unwrap(); + crate::println!("axp2101 handle"); + // let tt = xous_api_ticktimer::Ticktimer::new().unwrap(); + // crate::println!("tt handle"); + + /* + // high disconnects DCDC2 from the chip + self.iox.set_gpio_pin_value(port, pin, bao1x_api::IoxValue::High); + // we're now running on the LDO + crate::println!("DCDC2 D/C"); + tt.sleep_ms(2).ok(); + + // shut off the DCDC converter + pmic.set_dcdc(&mut self.i2c, None, crate::axp2101::WhichDcDc::Dcdc2).unwrap(); + crate::println!("DCDC2 *off*"); + */ + /* + crate::println!("disconnect LDOs"); + self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUCSR, 0); + self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUCRLP, 0); + self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUCRPD, 0x0); + self.sysctrl.wo(utra::sysctrl::SFR_IPCARIPFLOW, 0x57); + */ + + // switch to internal osc only + self.sysctrl.wo(utra::sysctrl::SFR_CGUSEL0, 0); + self.sysctrl.wo(utra::sysctrl::SFR_CGUSEL1, 1); + self.sysctrl.wo(utra::sysctrl::SFR_CGUSET, 0x32); + crate::println!("internal osc"); + + /* + unsafe { + self.sysctrl.base().add(utra::sysctrl::SFR_CGUFDAO.offset()).write_volatile(0x01010101); + self.sysctrl.base().add(utra::sysctrl::SFR_CGUFDAORAM.offset()).write_volatile(0x01010101); + self.sysctrl.base().add(utra::sysctrl::SFR_CGUFDPKE.offset()).write_volatile(0x01010101); + // commit dividers + self.sysctrl.base().add(utra::sysctrl::SFR_CGUSET.offset()).write_volatile(0x32); + } + */ + + // pmic.set_dcdc(&mut self.i2c, Some((0.70, true)), crate::axp2101::WhichDcDc::Dcdc2).unwrap(); + // crate::println!("DCDC2 to low voltage"); + + // pmic.set_dcdc(&mut self.i2c, Some((2.8, true)), crate::axp2101::WhichDcDc::Dcdc1).unwrap(); + // crate::println!("DCDC1 to low voltage"); + + // mbox on + /* + unsafe { + self.sysctrl.base().add(utra::sysctrl::SFR_ACLKGR.offset()).write_volatile(0xff); // mbox/qfc turned off + self.sysctrl.base().add(utra::sysctrl::SFR_CGUSET.offset()).write_volatile(0x32); + } + */ + // power down the PLL + self.sysctrl.wo(utra::sysctrl::SFR_IPCEN, self.sysctrl.r(utra::sysctrl::SFR_IPCEN) & !0x2); + self.sysctrl.wo(utra::sysctrl::SFR_IPCCR, 0x53); + self.sysctrl.wo(utra::sysctrl::SFR_IPCARIPFLOW, 0x32); + crate::println!("PLL off"); + + /* + unsafe { + self.sysctrl.base().add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_0.offset()).write_volatile(0x3f0f); // fclk + self.sysctrl.base().add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_1.offset()).write_volatile(0x3f0f); // aclk + self.sysctrl.base().add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_2.offset()).write_volatile(0x1f0f); // hclk + self.sysctrl.base().add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_3.offset()).write_volatile(0x0f0f); // iclk + self.sysctrl.base().add(utra::sysctrl::SFR_CGUFD_CFGFDCR_0_4_4.offset()).write_volatile(0x070f); // pclk + + self.sysctrl.base().add(utra::sysctrl::SFR_ACLKGR.offset()).write_volatile(0x00); // mbox/qfc turned off + self.sysctrl.base().add(utra::sysctrl::SFR_HCLKGR.offset()).write_volatile(0x00); // mdma off, sce on + self.sysctrl.base().add(utra::sysctrl::SFR_ICLKGR.offset()).write_volatile(0x00); // bio/udc enable + self.sysctrl.base().add(utra::sysctrl::SFR_PCLKGR.offset()).write_volatile(0x00); // enable mesh + // commit dividers + self.sysctrl.base().add(utra::sysctrl::SFR_CGUSET.offset()).write_volatile(0x32); + } + */ + + // lower core voltage to 0.7v + // self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUTRM0CSR, 0x08420002); + // self.sysctrl.wo(utra::sysctrl::SFR_IPCARIPFLOW, 0x57); + // crate::println!("0.7v"); + + self.sysctrl.wo(utra::sysctrl::SFR_CGULP, 0x3); + crate::println!("ULP"); + + // enter PD mode - cuts VDDCORE power - requires full reset to come out of this + // only RTC is kept (check this!). ~1.5-2mA @ 4.2V consumption in this mode. + self.ao_sysctrl.wo(utra::ao_sysctrl::CR_CR, 7); + crate::println!("CR"); + // setup wakeup mask + self.ao_sysctrl.wo(utra::ao_sysctrl::CR_WKUPMASK, 0x0001_003F); + self.ao_sysctrl.wo(utra::ao_sysctrl::CR_RSTCRMASK, 0x1f); + self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUDFTSR, 0); + self.ao_sysctrl + .wo(utra::ao_sysctrl::SFR_OSCCR, self.ao_sysctrl.r(utra::ao_sysctrl::SFR_OSCCR) & !0x0001_0000); + crate::println!("BEF AR1"); + self.sysctrl.wo(utra::sysctrl::SFR_IPCARIPFLOW, 0x32); + crate::println!("AFT AR1"); + self.sysctrl.wo(utra::sysctrl::SFR_CGUSET, 0x32); + self.sysctrl.wo(utra::sysctrl::SFR_IPCLPEN, 0x1f); + crate::println!("BEF AR2"); + self.sysctrl.wo(utra::sysctrl::SFR_IPCARIPFLOW, 0x32); + crate::println!("AFT AR2"); + // knock(); + // crate::println!("aft knock"); + // self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUPDAR, 0x5a); + // crate::println!("AFT PD"); + + /* + let pmuctrl = PmuControl::new_with_raw_value(0) + .with_iout_ref_ena(true) + .with_power_on_control(true) + .with_pmu_2p5v_ena(true); + // .with_pmu_0p8v_dig_ena(true); + crate::println!("pmuctl {:x}", pmuctrl.raw_value); + self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUCSR, pmuctrl.raw_value); + */ + // self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUCRLP, 0x4c); // does nothing? we can't enter LP + // self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUCRPD, 0x4c); // immediate shutdown + // self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUTRMLP0, 0x08420002); // 0.7v + // self.ao_sysctrl.wo(utra::ao_sysctrl::SFR_PMUPDAR, 0x5a); + crate::println!("PD -> WFI"); + + for _ in 0..10 { + unsafe { core::arch::asm!("wfi", "nop", "nop", "nop", "nop") }; + } + // unsafe { core::arch::asm!("wfi", "nop", "nop", "nop", "nop") }; + + crate::println!("out of WFI"); + // tt.sleep_ms(20).ok(); + + // high disconnects DCDC2 from the chip + self.iox.set_gpio_pin_value(port, pin, bao1x_api::IoxValue::High); + + // set the clock back to 350MHz CPU + self.request_freq(crate::board::DEFAULT_FCLK_FREQUENCY / 1_000_000 / 2) + .inspect_err(|e| crate::println!("req freq err {:?}", e)) + .ok(); + crate::println!("back to 350MHz"); + + crate::println!("setting DCDC"); + // pmic.set_dcdc(&mut self.i2c, Some((3.3, true)), crate::axp2101::WhichDcDc::Dcdc1).unwrap(); + // crate::println!("DCDC1 to 3.3v"); + pmic.set_dcdc( + &mut self.i2c, + Some(( + (crate::board::DEFAULT_CPU_VOLTAGE_MV + crate::board::VDD85_SWITCH_MARGIN_MV) as f32 / 1000.0, + true, + )), + crate::axp2101::WhichDcDc::Dcdc2, + ) + .inspect_err(|e| crate::println!("set dcdc err {:?}", e)) + .ok(); + + crate::println!("DCDC2 to normal voltage"); + + // low connects DCDC2 to the chip + self.iox.set_gpio_pin_value(port, pin, bao1x_api::IoxValue::Low); + + /* + // now re-enable the DCDC regulator for efficient power conversion + // set PWM mode on DCDC2. greatly reduces noise on the regulator line + pmic.set_pwm_mode(&mut self.i2c, crate::axp2101::WhichDcDc::Dcdc2, true).unwrap(); + // make sure the DCDC2 is set. Target 20mV above the acceptable run threshold because + // we have to take into account the transistor loss on the + // power switch. + crate::println!("dcdc2 on"); + pmic.set_dcdc( + &mut self.i2c, + Some(( + (crate::board::DEFAULT_CPU_VOLTAGE_MV + crate::board::VDD85_SWITCH_MARGIN_MV) as f32 / 1000.0, + true, + )), + crate::axp2101::WhichDcDc::Dcdc2, + ) + .unwrap(); + + crate::println!("dcdc2 connected"); + // low connects DCDC2 to the chip + self.iox.set_gpio_pin_value(port, pin, bao1x_api::IoxValue::Low); + */ + } +} + +use core::convert::TryFrom; +pub fn knock() { + let mut mbox = Mbox::new(); + + let test_data = [0xC0DE_0000u32, 0x0000_600Du32, 0, 0, 0, 0, 0, 0]; + let mut expected_result = 0; + for &d in test_data.iter() { + expected_result ^= d; + } + let test_pkt = + MboxToCm7Pkt { version: MBOX_PROTOCOL_REV, opcode: ToCm7Op::Knock, len: 2, data: test_data }; + // crate::println!("sending knock...\n"); + match mbox.try_send(test_pkt) { + Ok(_) => { + // crate::println!("Packet send Ok\n"); + let mut timeout = 0; + while mbox.poll_not_ready() { + timeout += 1; + if (timeout % 1_000) == 0 { + crate::println!("Waiting {}...", timeout); + } + if timeout >= 10_000 { + crate::println!("Mbox timed out"); + return; + } + } + // now receive the packet + // crate::println!("try_rx()..."); + match mbox.try_rx() { + Ok(rx_pkt) => { + crate::println!("Knock result: {:x}", rx_pkt.data[0]); + if rx_pkt.version != MBOX_PROTOCOL_REV { + crate::println!("Version mismatch {} != {}", rx_pkt.version, MBOX_PROTOCOL_REV); + } + if rx_pkt.opcode != ToRvOp::RetKnock { + crate::println!( + "Opcode mismatch {} != {}", + rx_pkt.opcode as u16, + ToRvOp::RetKnock as u16 + ); + } + if rx_pkt.len != 1 { + crate::println!("Expected length mismatch {} != {}", rx_pkt.len, 1); + } else { + if rx_pkt.data[0] != expected_result { + crate::println!( + "Expected data mismatch {:x} != {:x}", + rx_pkt.data[0], + expected_result + ); + } else { + crate::println!("Knock test PASS: {:x}", rx_pkt.data[0]); + } + } + } + Err(e) => { + crate::println!("Error while deserializing: {:?}\n", e); + } + } + } + Err(e) => { + crate::println!("Packet send error: {:?}\n", e); + } + }; +} +use utra::mailbox; + +/// This constraint is limited by the size of the memory on the CM7 side +const MAX_PKT_LEN: usize = 128; +const MBOX_PROTOCOL_REV: u32 = 0; +const TX_FIFO_DEPTH: u32 = 128; + +#[derive(Copy, Clone, PartialEq, Eq, Debug)] +#[allow(dead_code)] +pub enum MboxError { + None, + NotReady, + TxOverflow, + TxUnderflow, + RxOverflow, + RxUnderflow, + InvalidOpcode, + AbortFailed, +} + +#[repr(u16)] +#[derive(Copy, Clone, PartialEq, Eq, Debug)] +pub enum ToRvOp { + Invalid = 0, + + RetKnock = 128, + RetDct8x8 = 129, + RetClifford = 130, +} +impl TryFrom for ToRvOp { + type Error = MboxError; + + fn try_from(value: u16) -> Result { + match value { + 0 => Ok(ToRvOp::Invalid), + 128 => Ok(ToRvOp::RetKnock), + 129 => Ok(ToRvOp::RetDct8x8), + 130 => Ok(ToRvOp::RetClifford), + _ => Err(MboxError::InvalidOpcode), + } + } +} + +#[repr(u16)] +#[derive(Copy, Clone, PartialEq, Eq, Debug)] +#[allow(dead_code)] +pub enum ToCm7Op { + Invalid = 0, + + Knock = 1, + Dct8x8 = 2, + Clifford = 3, +} + +const STATIC_DATA_LEN: usize = 8; +pub struct MboxToCm7Pkt { + version: u32, + opcode: ToCm7Op, + len: usize, + data: [u32; STATIC_DATA_LEN], +} + +pub struct MboxToRvPkt { + version: u32, + opcode: ToRvOp, + len: usize, + data: [u32; STATIC_DATA_LEN], +} + +pub struct Mbox { + csr: CSR, +} +impl Mbox { + pub fn new() -> Mbox { + let mem = xous::map_memory( + xous::MemoryAddress::new(mailbox::HW_MAILBOX_BASE), + None, + 4096, + xous::MemoryFlags::R | xous::MemoryFlags::W, + ) + .unwrap(); + + Self { csr: CSR::new(mem.as_ptr() as *mut u32) } + } + + fn expect_tx(&mut self, val: u32) -> Result<(), MboxError> { + if (TX_FIFO_DEPTH - self.csr.rf(mailbox::STATUS_TX_WORDS)) == 0 { + return Err(MboxError::TxOverflow); + } else { + self.csr.wo(mailbox::WDATA, val); + Ok(()) + } + } + + pub fn try_send(&mut self, to_cm7: MboxToCm7Pkt) -> Result<(), MboxError> { + // clear any pending bits from previous transactions + self.csr.wo(mailbox::EV_PENDING, self.csr.r(mailbox::EV_PENDING)); + + if to_cm7.len > MAX_PKT_LEN { + Err(MboxError::TxOverflow) + } else { + self.expect_tx(to_cm7.version)?; + self.expect_tx(to_cm7.opcode as u32 | (to_cm7.len as u32) << 16)?; + for &d in to_cm7.data[..to_cm7.len].iter() { + self.expect_tx(d)?; + } + // trigger the send + self.csr.wfo(mailbox::DONE_DONE, 1); + Ok(()) + } + } + + fn expect_rx(&mut self) -> Result { + if self.csr.rf(mailbox::STATUS_RX_WORDS) == 0 { + Err(MboxError::RxUnderflow) + } else { + Ok(self.csr.r(mailbox::RDATA)) + } + } + + pub fn try_rx(&mut self) -> Result { + let version = self.expect_rx()?; + let op_and_len = self.expect_rx()?; + let opcode = ToRvOp::try_from((op_and_len & 0xFFFF) as u16)?; + let len = (op_and_len >> 16) as usize; + let mut data = [0u32; STATIC_DATA_LEN]; + for d in data[..len.min(STATIC_DATA_LEN)].iter_mut() { + *d = self.expect_rx()?; + } + Ok(MboxToRvPkt { version, opcode, len, data }) + } + + pub fn poll_not_ready(&self) -> bool { self.csr.rf(mailbox::EV_PENDING_AVAILABLE) == 0 } + + pub fn abort(&mut self) -> Result<(), MboxError> { + crate::println!("Initiating abort"); + self.csr.wfo(utra::mailbox::CONTROL_ABORT, 1); + const TIMEOUT: usize = 1000; + for _ in 0..TIMEOUT { + if self.csr.rf(utra::mailbox::STATUS_ABORT_IN_PROGRESS) == 0 { + return Ok(()); + } + } + return Err(MboxError::AbortFailed); + } +}