QSPI boot works

zedboard-fsbl/src/main.rs

@@ -9,6 +9,7 @@ use embedded_io::Write as _;
 use log::{error, info};
 use zedboard_bsp::qspi_spansion::{self, QspiSpansionS25Fl256SLinearMode};
 use zynq_boot_image::DestinationDevice;
+use zynq7000_hal::priv_tim;
 use zynq7000_hal::{
     BootMode,
     clocks::{
@@ -91,17 +92,19 @@ pub fn main() -> ! {
     let uart_clk_config = ClockConfig::new_autocalc_with_error(clocks.io_clocks(), 115200)
         .unwrap()
         .0;
-    let mut uart = Uart::new_with_mio(
+    let mut logger_uart = Uart::new_with_mio(
         periphs.uart_1,
         Config::new_with_clk_config(uart_clk_config),
         (mio_pins.mio48, mio_pins.mio49),
     )
     .unwrap();
-    uart.write_all(b"-- Zedboard Rust FSBL --\n\r").unwrap();
+    logger_uart
+        .write_all(b"-- Zedboard Rust FSBL --\n\r")
+        .unwrap();
     // Safety: We are not multi-threaded yet.
     unsafe {
         zynq7000_hal::log::uart_blocking::init_unsafe_single_core(
-            uart,
+            logger_uart,
             log::LevelFilter::Trace,
             false,
         )
@@ -135,6 +138,8 @@ pub fn main() -> ! {
     l2_cache::init_with_defaults(&mut periphs.l2c);
     info!("L2 cache init done.");
 
+    let priv_tim = priv_tim::CpuPrivateTimer::take(clocks.arm_clocks()).unwrap();
+
     if PERFORM_DDR_MEMTEST {
         let ddr_base_addr = 0x100000;
         info!("performing DDR memory test..");
@@ -168,14 +173,14 @@ pub fn main() -> ! {
         let spansion_qspi = qspi_spansion::QspiSpansionS25Fl256SIoMode::new(qspi_io_mode, true);
         let spansion_lqspi =
             spansion_qspi.into_linear_addressed(qspi_spansion::QSPI_DEV_COMBINATION_REV_F.into());
-        qspi_boot(spansion_lqspi);
+        qspi_boot(spansion_lqspi, priv_tim);
     }
     loop {
         cortex_ar::asm::nop();
     }
 }
 
-fn qspi_boot(mut qspi: QspiSpansionS25Fl256SLinearMode) -> ! {
+fn qspi_boot(mut qspi: QspiSpansionS25Fl256SLinearMode, _priv_tim: priv_tim::CpuPrivateTimer) -> ! {
     let boot_bin_base_addr = ELF_BASE_ADDR + BOOT_BIN_STAGING_OFFSET;
     let mut boot_header_slice = unsafe {
         core::slice::from_raw_parts_mut(
@@ -214,49 +219,116 @@ fn qspi_boot(mut qspi: QspiSpansionS25Fl256SLinearMode) -> ! {
     let boot_header = zynq_boot_image::BootHeader::new_unchecked(boot_header_slice);
 
     let mut name_buf: [u8; 256] = [0; 256];
-    for image_header in boot_header.image_header_iterator().unwrap() {
+    let mut opt_jump_addr = None;
+    for (index, image_header) in boot_header.image_header_iterator().unwrap().enumerate() {
         let name = image_header.image_name(&mut name_buf).unwrap();
-        for partition in image_header
+        if index == 0 {
+            if !name.contains("fsbl") {
+                log::warn!("first image name did not contain FSBL string");
+            }
+            // Skip the FSBL. It is probably currently running, and we do not want to re-flash it,
+            // which would also lead to a self-overwrite.
+            log::info!("skipping FSBL image");
+            continue;
+        }
+
+        let _read_guard = qspi.read_guard();
+
+        for (partition_index, partition) in image_header
             .partition_header_iterator(boot_header_slice)
             .unwrap()
+            .enumerate()
         {
             let section_attrs = partition.section_attributes();
-            if let Ok(dest_dev) = section_attrs.destination_device() {
-                match dest_dev {
-                    DestinationDevice::Pl => {
-                        info!("Loading image '{name}' to PL (FPGA)..");
-                        // Load the bitstream directly from linear mapped QSPI memory.
-                        let boot_bin_slice = unsafe {
-                            core::slice::from_raw_parts(
-                                (QSPI_START_ADDRESS
-                                    + partition
-                                        .data_offset()
-                                        .expect("invalid PL partition data offset"))
-                                    as *const _,
-                                partition.total_partition_length().unwrap(),
-                            )
-                        };
-                        // The DMA will read from the linear mapped QSPI directly, so it
-                        // has to be configured for reads using the guard!
-                        let _read_guard = qspi.read_guard();
-                        devcfg::configure_bitstream_non_secure(true, boot_bin_slice)
-                            .expect("unexpected unaligned address");
-                        log::info!("loaded bitstream successfully");
+            if section_attrs.destination_device().is_err() {
+                log::error!(
+                    "invalid destination device ID {}",
+                    section_attrs.destination_device().unwrap_err()
+                );
+                continue;
+            }
+            let dest_dev = section_attrs.destination_device().unwrap();
+            match dest_dev {
+                DestinationDevice::Pl => {
+                    info!("Loading image '{name}' to PL (FPGA)..");
+                    // Load the bitstream directly from linear mapped QSPI memory.
+                    let boot_bin_slice = unsafe {
+                        core::slice::from_raw_parts(
+                            (QSPI_START_ADDRESS
+                                + partition
+                                    .data_offset()
+                                    .expect("invalid PL partition data offset"))
+                                as *const _,
+                            partition.total_partition_length().unwrap(),
+                        )
+                    };
+                    // The DMA will read from the linear mapped QSPI directly, so it
+                    // has to be configured for reads using the guard!
+                    devcfg::configure_bitstream_non_secure(true, boot_bin_slice)
+                        .expect("unexpected unaligned address");
+                    log::info!("loaded bitstream successfully");
+                }
+                DestinationDevice::Ps => {
+                    // Load the bitstream directly from linear mapped QSPI memory.
+                    let load_addr = partition.destination_load_address();
+                    if load_addr < 0x10_0000 {
+                        panic!("invalid load address which is not located in DDR memory");
                     }
-                    DestinationDevice::Ps => {
-                        // TODO: Load the binary into DDR. Jump at lowest load address after all
-                        // partitions were parsed.
-                    }
-                    _ => {
-                        error!("Unsupported destination device {dest_dev:?}");
-                        continue;
+                    log::info!(
+                        "Loading partition {partition_index} for '{name}' to PS with load address {load_addr}.."
+                    );
+
+                    let source_slice = unsafe {
+                        core::slice::from_raw_parts(
+                            (QSPI_START_ADDRESS
+                                + partition
+                                    .data_offset()
+                                    .expect("invalid PS partition data offset"))
+                                as *const _,
+                            partition.total_partition_length().unwrap(),
+                        )
+                    };
+                    let target_slice = unsafe {
+                        core::slice::from_raw_parts_mut(
+                            load_addr as *mut u8,
+                            partition.total_partition_length().unwrap(),
+                        )
+                    };
+
+                    // Copy from the linear mapped QSPI to DDR,
+                    target_slice.copy_from_slice(source_slice);
+
+                    match &mut opt_jump_addr {
+                        Some(current) => *current = core::cmp::min(*current, load_addr),
+                        None => opt_jump_addr = Some(load_addr),
                     }
+                    log::info!("load success");
+                }
+                _ => {
+                    error!("Unsupported destination device {dest_dev:?}");
+                    continue;
                 }
             }
         }
     }
 
-    panic!("did not find application elf to boot inside boot binary!");
+    match opt_jump_addr {
+        Some(jump_addr) => {
+            log::info!("jumping to address {}", jump_addr);
+            zynq7000_hal::log::uart_blocking::flush();
+
+            // Some clean up and preparation for jumping to the user application.
+            zynq7000_hal::cache::clean_and_invalidate_data_cache();
+            cortex_ar::register::TlbIAll::write();
+            cortex_ar::register::BpIAll::write();
+            cortex_ar::asm::dsb();
+            cortex_ar::asm::isb();
+
+            let jump_func: extern "C" fn() -> ! = unsafe { core::mem::transmute(jump_addr) };
+            jump_func();
+        }
+        None => panic!("did not find application elf to boot inside boot binary!"),
+    }
 }
 
 #[zynq7000_rt::exception(DataAbort)]

zynq7000-hal/src/log.rs

@@ -1,13 +1,23 @@
 //! # Simple logging providers.
 
+use core::sync::atomic::{AtomicBool, AtomicU8};
+
+static LOGGER_INIT_DONE: AtomicBool = AtomicBool::new(false);
+
+const LOG_SEL_LOCKED: u8 = 1;
+const LOG_SEL_UNSAFE_SINGLE_CORE: u8 = 2;
+
+static LOG_SEL: AtomicU8 = AtomicU8::new(0);
+
 /// Blocking UART loggers.
 pub mod uart_blocking {
+    use super::*;
     use core::cell::{Cell, RefCell, UnsafeCell};
     use embedded_io::Write as _;
 
     use cortex_ar::register::Cpsr;
     use critical_section::Mutex;
-    use log::{LevelFilter, set_logger, set_max_level};
+    use log::{LevelFilter, Log, set_logger, set_max_level};
 
     use crate::uart::Uart;
 
@@ -27,7 +37,10 @@ pub mod uart_blocking {
     /// For async applications, it is strongly recommended to use the asynchronous ring buffer
     /// logger instead.
     pub fn init_with_locks(uart: Uart, level: LevelFilter) {
-        // TODO: Impl debug for Uart
+        if LOGGER_INIT_DONE.swap(true, core::sync::atomic::Ordering::Relaxed) {
+            return;
+        }
+        LOG_SEL.swap(LOG_SEL_LOCKED, core::sync::atomic::Ordering::Relaxed);
         critical_section::with(|cs| {
             let inner = UART_LOGGER_BLOCKING.0.borrow(cs);
             inner.replace(Some(uart));
@@ -53,7 +66,16 @@ pub mod uart_blocking {
             })
         }
 
-        fn flush(&self) {}
+        fn flush(&self) {
+            critical_section::with(|cs| {
+                let mut opt_logger = self.0.borrow(cs).borrow_mut();
+                if opt_logger.is_none() {
+                    return;
+                }
+                let logger = opt_logger.as_mut().unwrap();
+                logger.flush().unwrap();
+            });
+        }
     }
 
     pub struct UartLoggerUnsafeSingleThread {
@@ -70,23 +92,6 @@ pub mod uart_blocking {
             uart: UnsafeCell::new(None),
         };
 
-    /// Initialize the logger with a blocking UART instance.
-    ///
-    /// For async applications, it is strongly recommended to use the asynchronous ring buffer
-    /// logger instead.
-    ///
-    /// # Safety
-    ///
-    /// This is a blocking logger which performs a write WITHOUT a critical section. This logger is
-    /// NOT thread-safe. Users must ensure that this logger is not used inside a pre-emptive
-    /// multi-threading context and interrupt handlers.
-    pub unsafe fn create_unsafe_single_thread_logger(uart: Uart) -> UartLoggerUnsafeSingleThread {
-        UartLoggerUnsafeSingleThread {
-            skip_in_isr: Cell::new(false),
-            uart: UnsafeCell::new(Some(uart)),
-        }
-    }
-
     /// Initialize the logger with a blocking UART instance which does not use locks.
     ///
     /// # Safety
@@ -95,6 +100,13 @@ pub mod uart_blocking {
     /// NOT thread-safe, which might lead to garbled output. Log output in ISRs can optionally be
     /// surpressed.
     pub unsafe fn init_unsafe_single_core(uart: Uart, level: LevelFilter, skip_in_isr: bool) {
+        if LOGGER_INIT_DONE.swap(true, core::sync::atomic::Ordering::Relaxed) {
+            return;
+        }
+        LOG_SEL.swap(
+            LOG_SEL_UNSAFE_SINGLE_CORE,
+            core::sync::atomic::Ordering::Relaxed,
+        );
         let opt_uart = unsafe { &mut *UART_LOGGER_UNSAFE_SINGLE_THREAD.uart.get() };
         opt_uart.replace(uart);
         UART_LOGGER_UNSAFE_SINGLE_THREAD
@@ -134,7 +146,22 @@ pub mod uart_blocking {
             .unwrap();
         }
 
-        fn flush(&self) {}
+        fn flush(&self) {
+            let uart_mut = unsafe { &mut *self.uart.get() }.as_mut();
+            if uart_mut.is_none() {
+                return;
+            }
+            uart_mut.unwrap().flush().unwrap();
+        }
+    }
+
+    // Flush the selected logger instance.
+    pub fn flush() {
+        match LOG_SEL.load(core::sync::atomic::Ordering::Relaxed) {
+            val if val == LOG_SEL_LOCKED => UART_LOGGER_BLOCKING.flush(),
+            val if val == LOG_SEL_UNSAFE_SINGLE_CORE => UART_LOGGER_UNSAFE_SINGLE_THREAD.flush(),
+            _ => (),
+        }
     }
 }
 
@@ -205,6 +232,9 @@ pub mod rb {
     }
 
     pub fn init(level: LevelFilter) {
+        if super::LOGGER_INIT_DONE.swap(true, core::sync::atomic::Ordering::Relaxed) {
+            return;
+        }
         critical_section::with(|cs| {
             let rb = StaticRb::<u8, 4096>::default();
             let rb_ref = LOGGER_RB.ring_buf.borrow(cs);

zynq7000-hal/src/uart/mod.rs

@@ -578,7 +578,7 @@ impl embedded_hal_nb::serial::Write for Uart {
     }
 
     fn flush(&mut self) -> nb::Result<(), Self::Error> {
-        self.tx.flush()
+        embedded_hal_nb::serial::Write::flush(&mut self.tx)
     }
 }
 
@@ -604,7 +604,8 @@ impl embedded_io::Write for Uart {
     }
 
     fn flush(&mut self) -> Result<(), Self::Error> {
-        self.tx.flush()
+        self.tx.flush();
+        Ok(())
     }
 }
 

zynq7000-hal/src/uart/tx.rs

@@ -78,6 +78,10 @@ impl Tx {
         }
     }
 
+    pub fn flush(&mut self) {
+        while !self.regs.read_sr().tx_empty() {}
+    }
+
     #[inline]
     pub fn write_fifo_unchecked(&mut self, word: u8) {
         self.regs.write_fifo(Fifo::new_with_raw_value(word as u32));
@@ -161,11 +165,10 @@ impl embedded_hal_nb::serial::Write for Tx {
     }
 
     fn flush(&mut self) -> nb::Result<(), Self::Error> {
-        loop {
-            if self.regs.read_sr().tx_empty() {
-                return Ok(());
-            }
+        if self.regs.read_sr().tx_empty() {
+            return Ok(());
         }
+        Err(nb::Error::WouldBlock)
     }
 }
 
@@ -195,7 +198,7 @@ impl embedded_io::Write for Tx {
     }
 
     fn flush(&mut self) -> Result<(), Self::Error> {
-        <Self as embedded_hal_nb::serial::Write<u8>>::flush(self).ok();
+        self.flush();
         Ok(())
     }
 }