From 7e2ddedc8a1682d4b9afc06a36abce8223fd30a5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 31 Mar 2026 19:57:11 +0200 Subject: [PATCH] improve FSBL --- firmware/zedboard-fsbl/src/main.rs | 33 +++++++++++ firmware/zynq7000-hal/src/clocks/mod.rs | 73 +++++++++++++++++++++++-- firmware/zynq7000-hal/src/l2_cache.rs | 7 +++ firmware/zynq7000/Cargo.toml | 3 +- firmware/zynq7000/src/slcr/clocks.rs | 17 ++++-- 5 files changed, 122 insertions(+), 11 deletions(-) diff --git a/firmware/zedboard-fsbl/src/main.rs b/firmware/zedboard-fsbl/src/main.rs index 4666654..5661e52 100644 --- a/firmware/zedboard-fsbl/src/main.rs +++ b/firmware/zedboard-fsbl/src/main.rs @@ -16,6 +16,7 @@ use embedded_io::Write as _; use log::{error, info}; use zedboard_bsp::qspi_spansion::{self, QspiSpansionS25Fl256SLinearMode}; use zynq7000_boot_image::DestinationDevice; +use zynq7000_hal::clocks::ArmClocks; use zynq7000_hal::priv_tim; use zynq7000_hal::{ BootMode, @@ -75,6 +76,16 @@ fn main() -> ! { ); let mut periphs = zynq7000::Peripherals::take().unwrap(); + l2_cache::disable(); + + // Initialize the ARM clock. Safety: We only run this once. + unsafe { + ArmClocks::new_with_cpu_clock_init( + ARM_CLK, + zynq7000_hal::clocks::CpuClockRatio::SixToTwoToOne, + u6::new(2), + ); + } // Clock was already initialized by PS7 Init TCL script or FSBL, we just read it. let clocks = Clocks::new_from_regs(PS_CLK).unwrap(); @@ -102,6 +113,7 @@ fn main() -> ! { false, ) }; + //log::info!("clocks: {:?}", clocks); // Set up the global interrupt controller. let mut gic = gic::GicConfigurator::new_with_init(periphs.gicc, periphs.gicd); @@ -323,6 +335,7 @@ fn qspi_boot(mut qspi: QspiSpansionS25Fl256SLinearMode, _priv_tim: priv_tim::Cpu zynq7000_hal::cache::clean_and_invalidate_data_cache(); aarch32_cpu::register::TlbIAll::write(); aarch32_cpu::register::BpIAll::write(); + l2_cache::disable(); aarch32_cpu::asm::dsb(); aarch32_cpu::asm::isb(); @@ -333,6 +346,26 @@ fn qspi_boot(mut qspi: QspiSpansionS25Fl256SLinearMode, _priv_tim: priv_tim::Cpu } } +#[zynq7000_rt::irq] +fn interrupt_handler() { + let mut gic_helper = gic::GicInterruptHelper::new(); + let irq_info = gic_helper.acknowledge_interrupt(); + match irq_info.interrupt() { + gic::Interrupt::Sgi(_) => (), + gic::Interrupt::Ppi(ppi_interrupt) => { + log::warn!("unexpected PPI interrupt: {:?}", ppi_interrupt); + } + gic::Interrupt::Spi(spi_interrupt) => { + log::warn!("unexpected SPI interrupt: {:?}", spi_interrupt); + } + gic::Interrupt::Invalid(_) => (), + gic::Interrupt::Spurious => { + log::warn!("spurious interrupt"); + }, + } + gic_helper.end_of_interrupt(irq_info); +} + #[zynq7000_rt::exception(DataAbort)] fn data_abort_handler(_faulting_addr: usize) -> ! { loop { diff --git a/firmware/zynq7000-hal/src/clocks/mod.rs b/firmware/zynq7000-hal/src/clocks/mod.rs index 67c2ff7..1236dd9 100644 --- a/firmware/zynq7000-hal/src/clocks/mod.rs +++ b/firmware/zynq7000-hal/src/clocks/mod.rs @@ -3,11 +3,12 @@ use arbitrary_int::{prelude::*, u6}; pub mod pll; +pub use zynq7000::slcr::clocks::CpuClockRatio; use zynq7000::slcr::{ ClockControlRegisters, clocks::{ - ClockkRatioSelect, DualCommonPeriphIoClockControl, FpgaClockControl, GigEthClockControl, - SingleCommonPeriphIoClockControl, + ArmClockControl, ClockRatioSelectReg, DualCommonPeriphIoClockControl, FpgaClockControl, + GigEthClockControl, SingleCommonPeriphIoClockControl, }, }; @@ -17,6 +18,7 @@ use super::time::Hertz; #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct ArmClocks { ref_clk: Hertz, + ratio: CpuClockRatio, cpu_1x_clk: Hertz, cpu_2x_clk: Hertz, cpu_3x2x_clk: Hertz, @@ -24,23 +26,82 @@ pub struct ArmClocks { } impl ArmClocks { + /// Configure the ARM clocks based on the ARM PLL input clock. + /// + /// # Safety + /// + /// This changes the CPU clock frequency. You must pass the ARM PLL clock frequency and + /// you must ensure that this is only run once during system initialization, for example + /// in the first-stage bootloader. + pub unsafe fn new_with_cpu_clock_init( + arm_pll_clk: Hertz, + clock_ratio: CpuClockRatio, + divisor: u6, + ) -> Self { + unsafe { + crate::slcr::Slcr::with(|slcr| { + slcr.clk_ctrl().write_clk_ratio_select( + ClockRatioSelectReg::builder().with_sel(clock_ratio).build(), + ); + slcr.clk_ctrl().write_arm_clk_ctrl( + ArmClockControl::builder() + .with_cpu_peri_clk_act(true) + .with_cpu_1x_clk_act(true) + .with_cpu_2x_clk_act(true) + .with_cpu_3or2x_clk_act(true) + .with_cpu_6or4x_clk_act(true) + .with_divisor(divisor) + .with_srcsel(zynq7000::slcr::clocks::SrcSelArm::ArmPll) + .build(), + ); + }); + } + let cpu_6x4x_clk = arm_pll_clk / divisor.as_u32(); + let cpu_1x_clk = match clock_ratio { + CpuClockRatio::FourToTwoToOne => cpu_6x4x_clk / 4, + CpuClockRatio::SixToTwoToOne => cpu_6x4x_clk / 6, + }; + + Self { + ref_clk: arm_pll_clk, + ratio: clock_ratio, + cpu_1x_clk, + cpu_2x_clk: cpu_1x_clk * 2, + cpu_3x2x_clk: match clock_ratio { + CpuClockRatio::SixToTwoToOne => cpu_1x_clk * 3, + CpuClockRatio::FourToTwoToOne => cpu_1x_clk * 2, + }, + cpu_6x4x_clk, + } + } + + #[inline] + pub const fn ratio(&self) -> CpuClockRatio { + self.ratio + } + /// Reference clock provided by ARM PLL which is used to calculate all other clock frequencies. + #[inline] pub const fn ref_clk(&self) -> Hertz { self.ref_clk } + #[inline] pub const fn cpu_1x_clk(&self) -> Hertz { self.cpu_1x_clk } + #[inline] pub const fn cpu_2x_clk(&self) -> Hertz { self.cpu_2x_clk } + #[inline] pub const fn cpu_3x2x_clk(&self) -> Hertz { self.cpu_3x2x_clk } + #[inline] pub const fn cpu_6x4x_clk(&self) -> Hertz { self.cpu_6x4x_clk } @@ -278,25 +339,27 @@ impl Clocks { zynq7000::slcr::clocks::SrcSelArm::DdrPll => ddr_pll_out, zynq7000::slcr::clocks::SrcSelArm::IoPll => io_pll_out, }; - let clk_sel = clk_regs.read_clk_621_true(); + let clk_sel = clk_regs.read_clk_ratio_select(); if arm_clk_ctrl.divisor().as_u32() == 0 { return Err(ClockReadError::DivisorZero(DivisorZero(ClockModuleId::Arm))); } let arm_clk_divided = arm_base_clk / arm_clk_ctrl.divisor().as_u32(); let arm_clks = match clk_sel.sel() { - ClockkRatioSelect::FourToTwoToOne => ArmClocks { + CpuClockRatio::FourToTwoToOne => ArmClocks { ref_clk: arm_pll_out, cpu_1x_clk: arm_clk_divided / 4, cpu_2x_clk: arm_clk_divided / 2, cpu_3x2x_clk: arm_clk_divided / 2, cpu_6x4x_clk: arm_clk_divided, + ratio: clk_sel.sel(), }, - ClockkRatioSelect::SixToTwoToOne => ArmClocks { + CpuClockRatio::SixToTwoToOne => ArmClocks { ref_clk: arm_pll_out, cpu_1x_clk: arm_clk_divided / 6, cpu_2x_clk: arm_clk_divided / 3, cpu_3x2x_clk: arm_clk_divided / 2, cpu_6x4x_clk: arm_clk_divided, + ratio: clk_sel.sel(), }, }; diff --git a/firmware/zynq7000-hal/src/l2_cache.rs b/firmware/zynq7000-hal/src/l2_cache.rs index e350a6e..5155932 100644 --- a/firmware/zynq7000-hal/src/l2_cache.rs +++ b/firmware/zynq7000-hal/src/l2_cache.rs @@ -81,3 +81,10 @@ pub fn init( } l2c_mmio.write_control(Control::new_enabled()); } + +/// Disable the L2 cache. +#[inline] +pub fn disable() { + let mut l2c_mmio = unsafe { zynq7000::l2_cache::Registers::new_mmio_fixed() }; + l2c_mmio.write_control(Control::new_disabled()); +} diff --git a/firmware/zynq7000/Cargo.toml b/firmware/zynq7000/Cargo.toml index a093cae..1fbbf76 100644 --- a/firmware/zynq7000/Cargo.toml +++ b/firmware/zynq7000/Cargo.toml @@ -13,11 +13,12 @@ categories = ["embedded", "no-std", "hardware-support"] [dependencies] static_assertions = "1.1" derive-mmio = { version = "0.6", default-features = false } -bitbybit = "1.4" +bitbybit = "2" arbitrary-int = "2" rustversion = "1" thiserror = { version = "2", default-features = false } once_cell = { version = "1", default-features = false, features = ["critical-section"] } +defmt = { version = "1", optional = true } [dev-dependencies] approx = "0.5" diff --git a/firmware/zynq7000/src/slcr/clocks.rs b/firmware/zynq7000/src/slcr/clocks.rs index 9d2322e..1a6017b 100644 --- a/firmware/zynq7000/src/slcr/clocks.rs +++ b/firmware/zynq7000/src/slcr/clocks.rs @@ -128,7 +128,7 @@ pub enum SrcSelArm { IoPll = 0b11, } -#[bitbybit::bitfield(u32, debug)] +#[bitbybit::bitfield(u32, debug, default = 0x0)] pub struct ArmClockControl { #[bit(28, rw)] cpu_peri_clk_act: bool, @@ -178,19 +178,26 @@ pub struct DciClockControl { clk_act: bool, } -#[bitbybit::bitfield(u32, debug)] +#[bitbybit::bitfield(u32, debug, default = 0x0)] pub struct ClockRatioSelectReg { /// Reset value: 0x1 (6:2:1 clock) #[bit(0, rw)] - sel: ClockkRatioSelect, + sel: CpuClockRatio, } #[bitbybit::bitenum(u1, exhaustive = true)] #[derive(Debug)] -pub enum ClockkRatioSelect { +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum CpuClockRatio { /// 4:2:1 clock ratio, which is an abbreviation for 4:2:2:1. + /// + /// The 4x clock is calculated by dividing the reference clock + /// by the divisor, the rest by dividing by 2, 2 and 4 respectively. FourToTwoToOne = 0b0, /// 6:2:1 clock ratio, which is an abbreviation for 6:3:2:1. + /// + /// The 6x clock is calculated by dividing the reference clock + /// by the divisor, the rest by dividing by 2, 3 and 6 respectively. SixToTwoToOne = 0b1, } @@ -399,7 +406,7 @@ pub struct ClockControlRegisters { #[mmio(Inner)] fpga_3_clk_ctrl: FpgaClockControlRegisters, _gap1: [u32; 5], - clk_621_true: ClockRatioSelectReg, + clk_ratio_select: ClockRatioSelectReg, } impl ClockControlRegisters {