Update README and build.sh (#59)

update build.sh to use rpi-4 directly; fix README

README: update build instruction

build.sh: fix missing package argument

Co-authored-by: Harry Ho <hh@m-labs.hk>

Cargo.lock

@@ -98,13 +98,13 @@ dependencies = [
  "libboard_zynq 0.0.0",
  "libcortex_a9 0.0.0",
  "libregister 0.0.0",
- "linked_list_allocator 0.8.3 (registry+https://github.com/rust-lang/crates.io-index)",
+ "linked_list_allocator 0.8.4 (registry+https://github.com/rust-lang/crates.io-index)",
  "r0 1.0.0 (registry+https://github.com/rust-lang/crates.io-index)",
 ]
 
 [[package]]
 name = "linked_list_allocator"
-version = "0.8.3"
+version = "0.8.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 
 [[package]]
@@ -170,7 +170,7 @@ dependencies = [
 "checksum cfg-if 0.1.10 (registry+https://github.com/rust-lang/crates.io-index)" = "4785bdd1c96b2a846b2bd7cc02e86b6b3dbf14e7e53446c4f54c92a361040822"
 "checksum compiler_builtins 0.1.27 (registry+https://github.com/rust-lang/crates.io-index)" = "38f18416546abfbf8d801c555a0e99524453e7214f9cc9107ad49de3d5948ccc"
 "checksum embedded-hal 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)" = "ee4908a155094da7723c2d60d617b820061e3b4efcc3d9e293d206a5a76c170b"
-"checksum linked_list_allocator 0.8.3 (registry+https://github.com/rust-lang/crates.io-index)" = "d6b60501dd4c850950bb43f970d544f6ce04e0ca021da2db2538fbe9d923f19e"
+"checksum linked_list_allocator 0.8.4 (registry+https://github.com/rust-lang/crates.io-index)" = "e70e46c13c0e8374c26cec5752e3347ca1087d9711de8f45aa513a7700efd73d"
 "checksum log 0.4.8 (registry+https://github.com/rust-lang/crates.io-index)" = "14b6052be84e6b71ab17edffc2eeabf5c2c3ae1fdb464aae35ac50c67a44e1f7"
 "checksum managed 0.7.1 (registry+https://github.com/rust-lang/crates.io-index)" = "fdcec5e97041c7f0f1c5b7d93f12e57293c831c646f4cc7a5db59460c7ea8de6"
 "checksum nb 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)" = "b1411551beb3c11dedfb0a90a0fa256b47d28b9ec2cdff34c25a2fa59e45dbdc"

README.md

@@ -1,7 +1,7 @@
 # Build
 
 ```shell
-nix-shell --command "cargo xbuild --release"
+nix-shell --command "cargo xbuild --release -p experiments"
 ```
 
 Currently the ELF output is placed at `target/armv7-none-eabihf/release/experiments`
@@ -41,7 +41,7 @@ Proceed using gdb with `load`, `c`
 ### Running on the ZC706
 
 ```shell
-nix-shell --command "cargo xbuild --release"
+nix-shell --command "cargo xbuild --release -p experiments"
 cd openocd
 openocd -f zc706.cfg
 ```
@@ -57,23 +57,26 @@ openocd -f cora-z7-10.cfg
 ### Loading a bitstream into volatile memory
 
 ```shell
-  openocd -f zc706.cfg -c "pld load 0 blinker_migen.bit; exit"
+openocd -f zc706.cfg -c "pld load 0 blinker_migen.bit; exit"
 ```
 
 ### Development Process
 
 Clone this repo onto your development/build machine and the raspberry pi that controls the Xilinx 7000 board
 
-On the dev machine, the below script builds zc706 and secure copies it to the target pi (in your pi $HOME directory)
+On the dev machine, the below script builds zc706 and secure copies it to the target pi (in your pi $HOME directory):
 ```shell
-cd ~/zc706
-./build.sh $your_user/ssh_id
+cd ~/zynq-rs
+./build.sh $your_user_or_ssh_id
 ```
 
 On the pi, we need an information rich environment that includes a relatively reliable `gdb` experience (that includes `ctrl-p` and `ctrl-n` command history that persists across `cgdb` executions), run:
 ```shell
 ssh pi4
-cd zc706
+cd zynq-rs
+# For ZC706, run:
+./tmux.sh 0
+# For Cora Z7, run:
 ./tmux.sh
 ```
 

build.sh

@@ -1 +1 @@
-nix-shell --command "cargo xbuild --release" && scp -P 2204 -C target/armv7-none-eabihf/release/zc706-experiments $1@nixbld.m-labs.hk:/home/$1/zc706/zc706.elf
+nix-shell --command "cargo xbuild --release -p experiments" && scp -C target/armv7-none-eabihf/release/experiments $1@rpi-4.m-labs.hk:/home/$1/zc706/zc706.elf

default.nix

@@ -49,7 +49,7 @@ let
     name = "${crate}";
     src = ./.;
     crateSubdir = crate;
-    cargoSha256 = "0xlynsr94dyv0g41qwk5490w3wnzd5g70msaih6mcbgr3v4s2q34";
+    cargoSha256 = "1c1qpg9by8bg93yhgllb5xs155g27qmh99pbrb681wazm8k7nwim";
     cargoFeatures = features;
     doCheck = false;
     dontFixup = true;

experiments/Cargo.toml

@@ -16,5 +16,5 @@ embedded-hal = "0.2"
 libregister = { path = "../libregister" }
 libcortex_a9 = { path = "../libcortex_a9" }
 libboard_zynq = { path = "../libboard_zynq" }
-libsupport_zynq = { path = "../libsupport_zynq" }
+libsupport_zynq = { path = "../libsupport_zynq", default-features = false, features = ["panic_handler"]}
 libasync = { path = "../libasync" }

experiments/link.x

@@ -44,6 +44,10 @@ SECTIONS
         __stack0_end = .;
         . = ORIGIN(OCM3) + LENGTH(OCM3) - 8;
         __stack0_start = .;
+
+        /* unused heap0 to prevent the linker from complaining*/
+        __heap0_start = .;
+        __heap0_end = .;
     } > OCM3
 
     /DISCARD/ :

experiments/src/main.rs

@@ -1,5 +1,7 @@
 #![no_std]
 #![no_main]
+#![feature(const_in_array_repeat_expressions)]
+#![feature(naked_functions)]
 
 extern crate alloc;
 
@@ -13,8 +15,9 @@ use libboard_zynq::{
     self as zynq,
     clocks::source::{ArmPll, ClockSource, IoPll},
     clocks::Clocks,
-    print, println,
-    sdio::sd_card::SdCard,
+    print, println, stdio,
+    mpcore,
+    gic,
     smoltcp::{
         iface::{EthernetInterfaceBuilder, NeighborCache, Routes},
         time::Instant,
@@ -22,24 +25,70 @@ use libboard_zynq::{
     },
     time::Milliseconds,
 };
-#[cfg(feature = "target_zc706")]
-use libboard_zynq::ps7_init;
 use libcortex_a9::{
     mutex::Mutex,
-    sync_channel::{self, sync_channel},
+    sync_channel::{Sender, Receiver},
+    sync_channel,
+    regs::{MPIDR, SP},
+    spin_lock_yield, notify_spin_lock,
+    asm
 };
-use libregister::RegisterR;
+use libregister::{RegisterR, RegisterW};
 use libsupport_zynq::{
     boot, ram,
 };
 use log::{info, warn};
+use core::sync::atomic::{AtomicBool, Ordering};
 
 const HWADDR: [u8; 6] = [0, 0x23, 0xde, 0xea, 0xbe, 0xef];
 
+static mut CORE1_REQ: (Sender<usize>, Receiver<usize>) = sync_channel!(usize, 10);
+static mut CORE1_RES: (Sender<usize>, Receiver<usize>) = sync_channel!(usize, 10);
+
+extern "C" {
+    static mut __stack1_start: u32;
+}
+
+static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+pub unsafe extern "C" fn IRQ() {
+    if MPIDR.read().cpu_id() == 1{
+        let mpcore = mpcore::RegisterBlock::new();
+        let mut gic = gic::InterruptController::new(mpcore);
+        let id = gic.get_interrupt_id();
+        if id.0 == 0 {
+            gic.end_interrupt(id);
+            asm::exit_irq();
+            SP.write(&mut __stack1_start as *mut _ as u32);
+            asm::enable_irq();
+            CORE1_RESTART.store(false, Ordering::Relaxed);
+            notify_spin_lock();
+            main_core1();
+        }
+    }
+    stdio::drop_uart();
+    println!("IRQ");
+    loop {}
+}
+
+pub fn restart_core1() {
+    let mut interrupt_controller = gic::InterruptController::new(mpcore::RegisterBlock::new());
+    CORE1_RESTART.store(true, Ordering::Relaxed);
+    interrupt_controller.send_sgi(gic::InterruptId(0), gic::CPUCore::Core1.into());
+    while CORE1_RESTART.load(Ordering::Relaxed) {
+        spin_lock_yield();
+    }
+}
+
 #[no_mangle]
 pub fn main_core0() {
     // zynq::clocks::CpuClocks::enable_io(1_250_000_000);
     println!("\nzc706 main");
+    let mut interrupt_controller = gic::InterruptController::new(mpcore::RegisterBlock::new());
+    interrupt_controller.enable_interrupts();
     // ps7_init::apply();
     libboard_zynq::stdio::drop_uart();
 
@@ -82,25 +131,6 @@ pub fn main_core0() {
         clocks.cpu_1x()
     );
 
-    let sd = libboard_zynq::sdio::SDIO::sdio0(true);
-    // only test SD card if it is inserted
-    if sd.is_card_inserted() {
-        let result = SdCard::from_sdio(sd);
-        match &result {
-            Ok(_) => info!("OK!"),
-            Err(a) => info!("{}", a),
-        };
-        const SIZE: usize = 512 * 2 + 1;
-        let mut sd_card = result.unwrap();
-        if false {
-            let buffer: [u8; SIZE] = [5; SIZE];
-            sd_card.write_block(0x0, 2, &buffer).unwrap();
-        }
-        let mut buffer: [u8; SIZE] = [0; SIZE];
-        sd_card.read_block(0 /*0x1*/, 2, &mut buffer[1..]).unwrap();
-        info!("buffer = {:?}", &buffer[..]);
-    }
-
     let mut flash = zynq::flash::Flash::new(200_000_000).linear_addressing_mode();
     let flash_ram: &[u8] = unsafe { core::slice::from_raw_parts(flash.ptr(), flash.size()) };
     for i in 0..=1 {
@@ -158,20 +188,54 @@ pub fn main_core0() {
         flash = flash_io.stop();
     }
 
-    let core1 = boot::Core1::start(false);
+    boot::Core1::start(false);
 
-    let (mut core1_req, rx) = sync_channel(10);
-    *CORE1_REQ.lock() = Some(rx);
-    let (tx, mut core1_res) = sync_channel(10);
-    *CORE1_RES.lock() = Some(tx);
+    let core1_req = unsafe { &mut CORE1_REQ.0 };
+    let core1_res = unsafe { &mut CORE1_RES.1 };
     task::block_on(async {
         for i in 0..10 {
+            restart_core1();
             core1_req.async_send(i).await;
             let j = core1_res.async_recv().await;
             println!("{} -> {}", i, j);
         }
     });
-    core1.disable();
+    unsafe {
+        core1_req.drop_elements();
+    }
+    
+    // Test I2C
+    #[cfg(feature = "target_zc706")]
+    {
+        let mut i2c = zynq::i2c::I2C::i2c();
+        i2c.init();
+        println!("I2C bit-banging enabled");
+        let mut eeprom = zynq::i2c::eeprom::EEPROM::new(&mut i2c, 16);
+        // Write to 0x00 and 0x08
+        let eeprom_buffer: [u8; 22] = [
+            0x66, 0x77, 0x88, 0x99, 0xaa, 0xbb, 
+            0x77, 0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 
+            0xef, 0xcd, 0xab, 0x89, 0x67, 0x45, 0x23, 0x01,
+        ];
+        eeprom.write(0x00, &eeprom_buffer[0..6]);
+        eeprom.write(0x08, &eeprom_buffer[6..22]);
+        println!("Data written to EEPROM");
+        let mut eeprom_buffer = [0u8; 24];
+        // Read from 0x00
+        eeprom.read(0x00, &mut eeprom_buffer);
+        print!("Data read from EEPROM @ 0x00: (hex) ");
+        for i in 0..6 {
+            print!("{:02x} ", eeprom_buffer[i]);
+        }
+        println!("");
+        // Read from 0x08
+        eeprom.read(0x08, &mut eeprom_buffer);
+        print!("Data read from EEPROM @ 0x08: (hex) ");
+        for i in 0..16 {
+            print!("{:02x} ", eeprom_buffer[i]);
+        }
+        println!("");
+    }
 
     let eth = zynq::eth::Eth::default(HWADDR.clone());
     println!("Eth on");
@@ -196,9 +260,6 @@ pub fn main_core0() {
         .neighbor_cache(neighbor_cache)
         .finalize();
 
-    #[cfg(feature = "target_zc706")]
-    ps7_init::report_differences();
-
     Sockets::init(32);
 
     const TCP_PORT: u16 = 19;
@@ -265,27 +326,18 @@ pub fn main_core0() {
     })
 }
 
-static CORE1_REQ: Mutex<Option<sync_channel::Receiver<usize>>> = Mutex::new(None);
-static CORE1_RES: Mutex<Option<sync_channel::Sender<usize>>> = Mutex::new(None);
 static DONE: Mutex<bool> = Mutex::new(false);
 
 #[no_mangle]
 pub fn main_core1() {
     println!("Hello from core1!");
-
-    let mut req = None;
-    while req.is_none() {
-        req = CORE1_REQ.lock().take();
-    }
-    let req = req.unwrap();
-    let mut res = None;
-    while res.is_none() {
-        res = CORE1_RES.lock().take();
-    }
-    let mut res = res.unwrap();
+    let mut interrupt_controller = gic::InterruptController::new(mpcore::RegisterBlock::new());
+    interrupt_controller.enable_interrupts();
+    let req = unsafe { &mut CORE1_REQ.1 };
+    let res = unsafe { &mut CORE1_RES.0 };
 
     for i in req {
-        res.send(*i * *i);
+        res.send(i * i);
     }
 
     println!("core1 done!");

libboard_zynq/src/ddr/mod.rs

@@ -56,14 +56,35 @@ impl DdrRam {
         clocks
     }
 
+    fn calculate_dci_divisors(clocks: &Clocks) -> (u8, u8) {
+        let target = (DCI_FREQ - 1 + clocks.ddr) / DCI_FREQ;
+
+        let mut best = None;
+        let mut best_error = 0;
+        for divisor0 in 1..63 {
+            for divisor1 in 1..63 {
+                let current = (divisor0 as u32) * (divisor1 as u32);
+                let error = if current > target {
+                    current - target
+                } else {
+                    target - current
+                };
+                if best.is_none() || best_error > error {
+                    best = Some((divisor0, divisor1));
+                    best_error = error;
+                }
+            }
+        }
+        best.unwrap()
+    }
+
     /// Zynq-7000 AP SoC Technical Reference Manual:
     /// 10.6.2 DDR IOB Impedance Calibration
     fn calibrate_iob_impedance(clocks: &Clocks) {
-        let divisor0 = ((DCI_FREQ - 1 + clocks.ddr) / DCI_FREQ)
-            .max(1).min(63) as u8;
-        let divisor1 = ((DCI_FREQ - 1 + clocks.ddr) / DCI_FREQ / u32::from(divisor0))
-            .max(1).min(63) as u8;
-        debug!("DDR DCI clock: {} Hz", clocks.ddr / u32::from(divisor0) / u32::from(divisor1));
+        let (divisor0, divisor1) = Self::calculate_dci_divisors(clocks);
+        debug!("DDR DCI clock: {} Hz (divisors={}*{})",
+               clocks.ddr / u32::from(divisor0) / u32::from(divisor1),
+               divisor0, divisor1);
 
         slcr::RegisterBlock::unlocked(|slcr| {
             // Step 1.

libboard_zynq/src/eth/mod.rs

@@ -1,5 +1,8 @@
-use core::ops::{Deref, DerefMut};
-use log::{error, info, warn};
+use core::{
+    marker::PhantomData,
+    ops::{Deref, DerefMut},
+};
+use log::{debug, info, warn, error};
 use libregister::*;
 use super::slcr;
 use super::clocks::Clocks;
@@ -42,14 +45,109 @@ impl DerefMut for Buffer {
     }
 }
 
-pub struct Eth<'r, RX, TX> {
+/// Gigabit Ethernet Peripheral
+pub trait Gem {
+    fn setup_clock(tx_clock: u32);
+    fn regs() -> &'static mut regs::RegisterBlock;
+}
+
+/// first Gigabit Ethernet peripheral
+pub struct Gem0;
+
+impl Gem for Gem0 {
+    fn setup_clock(tx_clock: u32) {
+        let (divisor0, divisor1) = calculate_tx_divisors(tx_clock);
+
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.gem0_clk_ctrl.write(
+                // 0x0050_0801: 8, 5: 100 Mb/s
+                // ...: 8, 1: 1000 Mb/s
+                slcr::GemClkCtrl::zeroed()
+                    .clkact(true)
+                    .srcsel(slcr::PllSource::IoPll)
+                    .divisor(divisor0 as u8)
+                    .divisor1(divisor1 as u8)
+            );
+            // Enable gem0 recv clock
+            slcr.gem0_rclk_ctrl.write(
+                // 0x0000_0801
+                slcr::RclkCtrl::zeroed()
+                    .clkact(true)
+            );
+        });
+    }
+
+    fn regs() -> &'static mut regs::RegisterBlock {
+        regs::RegisterBlock::gem0()
+    }
+}
+
+/// second Gigabit Ethernet peripheal
+pub struct Gem1;
+
+impl Gem for Gem1 {
+    fn setup_clock(tx_clock: u32) {
+        let (divisor0, divisor1) = calculate_tx_divisors(tx_clock);
+
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.gem1_clk_ctrl.write(
+                slcr::GemClkCtrl::zeroed()
+                    .clkact(true)
+                    .srcsel(slcr::PllSource::IoPll)
+                    .divisor(divisor0 as u8)
+                    .divisor1(divisor1 as u8)
+            );
+            // Enable gem1 recv clock
+            slcr.gem1_rclk_ctrl.write(
+                // 0x0000_0801
+                slcr::RclkCtrl::zeroed()
+                    .clkact(true)
+            );
+        });
+    }
+
+    fn regs() -> &'static mut regs::RegisterBlock {
+        regs::RegisterBlock::gem1()
+    }
+}
+
+fn calculate_tx_divisors(tx_clock: u32) -> (u8, u8) {
+    let io_pll = Clocks::get().io;
+    let target = (tx_clock - 1 + io_pll) / tx_clock;
+
+    let mut best = None;
+    let mut best_error = 0;
+    for divisor0 in 1..63 {
+        for divisor1 in 1..63 {
+            let current = (divisor0 as u32) * (divisor1 as u32);
+            let error = if current > target {
+                current - target
+            } else {
+                target - current
+            };
+            if best.is_none() || best_error > error {
+                best = Some((divisor0, divisor1));
+                best_error = error;
+            }
+        }
+    }
+    let result = best.unwrap();
+    debug!("Eth TX clock for {}: {} / {} / {} = {}",
+           tx_clock, io_pll,
+           result.0, result.1,
+           io_pll / result.0 as u32 / result.1 as u32
+    );
+    result
+}
+
+pub struct Eth<GEM: Gem, RX, TX> {
     rx: RX,
     tx: TX,
-    inner: EthInner<'r>,
+    inner: EthInner<GEM>,
     phy: Phy,
 }
 
-impl<'r> Eth<'r, (), ()> {
+impl Eth<Gem0, (), ()> {
     pub fn default(macaddr: [u8; 6]) -> Self {
         slcr::RegisterBlock::unlocked(|slcr| {
             // Manual example: 0x0000_1280
@@ -182,22 +280,23 @@ impl<'r> Eth<'r, (), ()> {
     }
 
     pub fn gem0(macaddr: [u8; 6]) -> Self {
-        Self::setup_gem0_clock(TX_1000);
-
-        let regs = regs::RegisterBlock::gem0();
-        Self::from_regs(regs, macaddr)
+        Self::new(macaddr)
     }
+}
 
+
+impl Eth<Gem1, (), ()> {
     pub fn gem1(macaddr: [u8; 6]) -> Self {
-        Self::setup_gem1_clock(TX_1000);
-
-        let regs = regs::RegisterBlock::gem1();
-        Self::from_regs(regs, macaddr)
+        Self::new(macaddr)
     }
+}
+
+impl<GEM: Gem> Eth<GEM, (), ()> {
+    fn new(macaddr: [u8; 6]) -> Self {
+        GEM::setup_clock(TX_1000);
 
-    fn from_regs(regs: &'r mut regs::RegisterBlock, macaddr: [u8; 6]) -> Self {
         let mut inner = EthInner {
-            regs,
+            gem: PhantomData,
             link: None,
         };
         inner.init();
@@ -216,54 +315,8 @@ impl<'r> Eth<'r, (), ()> {
     }
 }
 
-impl<'r, RX, TX> Eth<'r, RX, TX> {
-    pub fn setup_gem0_clock(tx_clock: u32) {
-        let io_pll = Clocks::get().io;
-        let d0 = ((tx_clock - 1 + io_pll) / tx_clock).max(1).min(63);
-        let d1 = (io_pll / tx_clock / d0).max(1).min(63);
-
-        slcr::RegisterBlock::unlocked(|slcr| {
-            slcr.gem0_clk_ctrl.write(
-                // 0x0050_0801: 8, 5: 100 Mb/s
-                // ...: 8, 1: 1000 Mb/s
-                slcr::GemClkCtrl::zeroed()
-                    .clkact(true)
-                    .srcsel(slcr::PllSource::IoPll)
-                    .divisor(d0 as u8)
-                    .divisor1(d1 as u8)
-            );
-            // Enable gem0 recv clock
-            slcr.gem0_rclk_ctrl.write(
-                // 0x0000_0801
-                slcr::RclkCtrl::zeroed()
-                    .clkact(true)
-            );
-        });
-    }
-
-    pub fn setup_gem1_clock(tx_clock: u32) {
-        let io_pll = Clocks::get().io;
-        let d0 = ((tx_clock - 1 + io_pll) / tx_clock).max(1).min(63);
-        let d1 = (io_pll / tx_clock / d0).max(1).min(63);
-
-        slcr::RegisterBlock::unlocked(|slcr| {
-            slcr.gem1_clk_ctrl.write(
-                slcr::GemClkCtrl::zeroed()
-                    .clkact(true)
-                    .srcsel(slcr::PllSource::IoPll)
-                    .divisor(d0 as u8)
-                    .divisor1(d1 as u8)
-            );
-            // Enable gem1 recv clock
-            slcr.gem1_rclk_ctrl.write(
-                // 0x0000_0801
-                slcr::RclkCtrl::zeroed()
-                    .clkact(true)
-            );
-        });
-    }
-
-    pub fn start_rx(self, rx_size: usize) -> Eth<'r, rx::DescList, TX> {
+impl<GEM: Gem, RX, TX> Eth<GEM, RX, TX> {
+    pub fn start_rx(self, rx_size: usize) -> Eth<GEM, rx::DescList, TX> {
         let new_self = Eth {
             rx: rx::DescList::new(rx_size),
             tx: self.tx,
@@ -272,17 +325,17 @@ impl<'r, RX, TX> Eth<'r, RX, TX> {
         };
         let list_addr = new_self.rx.list_addr();
         assert!(list_addr & 0b11 == 0);
-        new_self.inner.regs.rx_qbar.write(
+        GEM::regs().rx_qbar.write(
             regs::RxQbar::zeroed()
                 .rx_q_baseaddr(list_addr >> 2)
         );
-        new_self.inner.regs.net_ctrl.modify(|_, w|
+        GEM::regs().net_ctrl.modify(|_, w|
             w.rx_en(true)
         );
         new_self
     }
 
-    pub fn start_tx(self, tx_size: usize) -> Eth<'r, RX, tx::DescList> {
+    pub fn start_tx(self, tx_size: usize) -> Eth<GEM, RX, tx::DescList> {
         let new_self = Eth {
             rx: self.rx,
             tx: tx::DescList::new(tx_size),
@@ -291,23 +344,23 @@ impl<'r, RX, TX> Eth<'r, RX, TX> {
         };
         let list_addr = &new_self.tx.list_addr();
         assert!(list_addr & 0b11 == 0);
-        new_self.inner.regs.tx_qbar.write(
+        GEM::regs().tx_qbar.write(
             regs::TxQbar::zeroed()
                 .tx_q_baseaddr(list_addr >> 2)
         );
-        new_self.inner.regs.net_ctrl.modify(|_, w|
+        GEM::regs().net_ctrl.modify(|_, w|
             w.tx_en(true)
         );
         new_self
     }
 }
 
-impl<'r, TX> Eth<'r, rx::DescList, TX> {
+impl<GEM: Gem, TX> Eth<GEM, rx::DescList, TX> {
     pub fn recv_next<'s: 'p, 'p>(&'s mut self) -> Result<Option<rx::PktRef<'p>>, rx::Error> {
-        let status = self.inner.regs.rx_status.read();
+        let status = GEM::regs().rx_status.read();
         if status.hresp_not_ok() {
             // Clear
-            self.inner.regs.rx_status.write(
+            GEM::regs().rx_status.write(
                 regs::RxStatus::zeroed()
                     .hresp_not_ok(true)
             );
@@ -315,7 +368,7 @@ impl<'r, TX> Eth<'r, rx::DescList, TX> {
         }
         if status.rx_overrun() {
             // Clear
-            self.inner.regs.rx_status.write(
+            GEM::regs().rx_status.write(
                 regs::RxStatus::zeroed()
                     .rx_overrun(true)
             );
@@ -323,7 +376,7 @@ impl<'r, TX> Eth<'r, rx::DescList, TX> {
         }
         if status.buffer_not_avail() {
             // Clear
-            self.inner.regs.rx_status.write(
+            GEM::regs().rx_status.write(
                 regs::RxStatus::zeroed()
                     .buffer_not_avail(true)
             );
@@ -335,7 +388,7 @@ impl<'r, TX> Eth<'r, rx::DescList, TX> {
             match result {
                 Ok(None) => {
                     // No packet, clear status bit
-                    self.inner.regs.rx_status.write(
+                    GEM::regs().rx_status.write(
                         regs::RxStatus::zeroed()
                             .frame_recd(true)
                     );
@@ -350,13 +403,13 @@ impl<'r, TX> Eth<'r, rx::DescList, TX> {
     }
 }
 
-impl<'r, RX> Eth<'r, RX, tx::DescList> {
+impl<GEM: Gem, RX> Eth<GEM, RX, tx::DescList> {
     pub fn send<'s: 'p, 'p>(&'s mut self, length: usize) -> Option<tx::PktRef<'p>> {
-        self.tx.send(self.inner.regs, length)
+        self.tx.send(GEM::regs(), length)
     }
 }
 
-impl<'r, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescList, tx::DescList> {
+impl<'a, GEM: Gem> smoltcp::phy::Device<'a> for &mut Eth<GEM, rx::DescList, tx::DescList> {
     type RxToken = rx::PktRef<'a>;
     type TxToken = tx::Token<'a>;
 
@@ -380,7 +433,7 @@ impl<'r, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescList, tx::DescLis
         match self.rx.recv_next() {
             Ok(Some(pktref)) => {
                 let tx_token = tx::Token {
-                    regs: self.inner.regs,
+                    regs: GEM::regs(),
                     desc_list: &mut self.tx,
                 };
                 Some((pktref, tx_token))
@@ -398,33 +451,32 @@ impl<'r, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescList, tx::DescLis
 
     fn transmit(&'a mut self) -> Option<Self::TxToken> {
         Some(tx::Token {
-            regs: self.inner.regs,
+            regs: GEM::regs(),
             desc_list: &mut self.tx,
         })
     }
-
 }
 
 
-struct EthInner<'r> {
-    regs: &'r mut regs::RegisterBlock,
+struct EthInner<GEM: Gem> {
+    gem: PhantomData<GEM>,
     link: Option<phy::Link>,
 }
 
-impl<'r> EthInner<'r> {
+impl<GEM: Gem> EthInner<GEM> {
     fn init(&mut self) {
         // Clear the Network Control register.
-        self.regs.net_ctrl.write(regs::NetCtrl::zeroed());
-        self.regs.net_ctrl.write(regs::NetCtrl::zeroed().clear_stat_regs(true));
+        GEM::regs().net_ctrl.write(regs::NetCtrl::zeroed());
+        GEM::regs().net_ctrl.write(regs::NetCtrl::zeroed().clear_stat_regs(true));
         // Clear the Status registers.
-        self.regs.rx_status.write(
+        GEM::regs().rx_status.write(
             regs::RxStatus::zeroed()
                 .buffer_not_avail(true)
                 .frame_recd(true)
                 .rx_overrun(true)
                 .hresp_not_ok(true)
         );
-        self.regs.tx_status.write(
+        GEM::regs().tx_status.write(
             regs::TxStatus::zeroed()
                 .used_bit_read(true)
                 .collision(true)
@@ -438,7 +490,7 @@ impl<'r> EthInner<'r> {
                 .hresp_not_ok(true)
         );
         // Disable all interrupts.
-        self.regs.intr_dis.write(
+        GEM::regs().intr_dis.write(
             regs::IntrDis::zeroed()
                 .mgmt_done(true)
                 .rx_complete(true)
@@ -468,10 +520,10 @@ impl<'r> EthInner<'r> {
                 .tsu_sec_incr(true)
         );
         // Clear the buffer queues.
-        self.regs.rx_qbar.write(
+        GEM::regs().rx_qbar.write(
             regs::RxQbar::zeroed()
         );
-        self.regs.tx_qbar.write(
+        GEM::regs().tx_qbar.write(
             regs::TxQbar::zeroed()
         );
     }
@@ -480,7 +532,7 @@ impl<'r> EthInner<'r> {
         let clocks = Clocks::get();
         let mdc_clk_div = (clocks.cpu_1x() / MAX_MDC) + 1;
 
-        self.regs.net_cfg.write(
+        GEM::regs().net_cfg.write(
             regs::NetCfg::zeroed()
                 .full_duplex(true)
                 .gige_en(true)
@@ -505,17 +557,17 @@ impl<'r> EthInner<'r> {
             (u32::from(macaddr[3]) << 16) |
             (u32::from(macaddr[4]) << 8) |
             u32::from(macaddr[5]);
-        self.regs.spec_addr1_top.write(
+        GEM::regs().spec_addr1_top.write(
             regs::SpecAddrTop::zeroed()
                 .addr_msbs(macaddr_msbs)
         );
-        self.regs.spec_addr1_bot.write(
+        GEM::regs().spec_addr1_bot.write(
             regs::SpecAddrBot::zeroed()
                 .addr_lsbs(macaddr_lsbs)
         );
 
 
-        self.regs.dma_cfg.write(
+        GEM::regs().dma_cfg.write(
             regs::DmaCfg::zeroed()
                 // 1536 bytes
                 .ahb_mem_rx_buf_size((MTU >> 6) as u8)
@@ -531,7 +583,7 @@ impl<'r> EthInner<'r> {
                 .ahb_fixed_burst_len(0x10)
         );
 
-        self.regs.net_ctrl.write(
+        GEM::regs().net_ctrl.write(
             regs::NetCtrl::zeroed()
                 .mgmt_port_en(true)
         );
@@ -539,7 +591,7 @@ impl<'r> EthInner<'r> {
 
 
     fn wait_phy_idle(&self) {
-        while !self.regs.net_status.read().phy_mgmt_idle() {}
+        while !GEM::regs().net_status.read().phy_mgmt_idle() {}
     }
 
 
@@ -558,17 +610,15 @@ impl<'r> EthInner<'r> {
                 Some(link) => {
                     info!("eth: got {:?}", link);
 
-                    use phy::LinkSpeed::*;
+                    use phy::{LinkDuplex::Full, LinkSpeed::*};
                     let txclock = match link.speed {
                         S10 => TX_10,
                         S100 => TX_100,
                         S1000 => TX_1000,
                     };
-                    Eth::<(), ()>::setup_gem0_clock(txclock);
-                    /* .full_duplex(false) doesn't work even if
-                       half duplex has been negotiated. */
-                    self.regs.net_cfg.modify(|_, w| w
-                        .full_duplex(true)
+                    GEM::setup_clock(txclock);
+                    GEM::regs().net_cfg.modify(|_, w| w
+                        .full_duplex(link.duplex == Full)
                         .gige_en(link.speed == S1000)
                         .speed(link.speed != S10)
                     );
@@ -587,10 +637,10 @@ impl<'r> EthInner<'r> {
     }
 }
 
-impl<'r> PhyAccess for EthInner<'r> {
+impl<GEM: Gem> PhyAccess for EthInner<GEM> {
     fn read_phy(&mut self, addr: u8, reg: u8) -> u16 {
         self.wait_phy_idle();
-        self.regs.phy_maint.write(
+        GEM::regs().phy_maint.write(
             regs::PhyMaint::zeroed()
                 .clause_22(true)
                 .operation(regs::PhyOperation::Read)
@@ -599,12 +649,12 @@ impl<'r> PhyAccess for EthInner<'r> {
                 .must_10(0b10)
         );
         self.wait_phy_idle();
-        self.regs.phy_maint.read().data()
+        GEM::regs().phy_maint.read().data()
     }
 
     fn write_phy(&mut self, addr: u8, reg: u8, data: u16) {
         self.wait_phy_idle();
-        self.regs.phy_maint.write(
+        GEM::regs().phy_maint.write(
             regs::PhyMaint::zeroed()
                 .clause_22(true)
                 .operation(regs::PhyOperation::Write)

libboard_zynq/src/eth/phy/extended_status.rs

@@ -1,59 +0,0 @@
-use bit_field::BitField;
-use super::{PhyRegister, Link, LinkDuplex, LinkSpeed};
-
-#[derive(Clone, Copy, Debug)]
-/// 1000Base-T Extended Status Register
-pub struct ExtendedStatus(pub u16);
-
-impl ExtendedStatus {
-    pub fn cap_1000base_t_half(&self) -> bool {
-        self.0.get_bit(12)
-    }
-    pub fn cap_1000base_t_full(&self) -> bool {
-        self.0.get_bit(13)
-    }
-    pub fn cap_1000base_x_half(&self) -> bool {
-        self.0.get_bit(14)
-    }
-    pub fn cap_1000base_x_full(&self) -> bool {
-        self.0.get_bit(12)
-    }
-
-    pub fn get_link(&self) -> Option<Link> {
-        if self.cap_1000base_t_half() {
-            Some(Link {
-                speed: LinkSpeed::S1000,
-                duplex: LinkDuplex::Half,
-            })
-        } else if self.cap_1000base_t_full() {
-            Some(Link {
-                speed: LinkSpeed::S1000,
-                duplex: LinkDuplex::Full,
-            })
-        } else if self.cap_1000base_x_half() {
-            Some(Link {
-                speed: LinkSpeed::S1000,
-                duplex: LinkDuplex::Half,
-            })
-        } else if self.cap_1000base_x_full() {
-            Some(Link {
-                speed: LinkSpeed::S1000,
-                duplex: LinkDuplex::Full,
-            })
-        } else {
-            None
-        }
-    }
-}
-
-impl PhyRegister for ExtendedStatus {
-    fn addr() -> u8 {
-        0xF
-    }
-}
-
-impl From<u16> for ExtendedStatus {
-    fn from(value: u16) -> Self {
-        ExtendedStatus(value)
-    }
-}

libboard_zynq/src/eth/phy/mod.rs

@@ -2,10 +2,10 @@ pub mod id;
 use id::{identify_phy, PhyIdentifier};
 mod status;
 pub use status::Status;
-mod extended_status;
-pub use extended_status::ExtendedStatus;
 mod control;
 pub use control::Control;
+mod pssr;
+pub use pssr::PSSR;
 
 #[derive(Clone, Debug, PartialEq)]
 pub struct Link {
@@ -39,43 +39,36 @@ pub struct Phy {
 
 #[derive(Clone, Copy)]
 pub enum PhyDevice {
-    Marvel88E1116R,
+    Marvell88E1116R,
     Rtl8211E,
 }
 
-const OUI_MARVEL: u32 = 0x005043;
+const OUI_MARVELL: u32 = 0x005043;
 const OUI_REALTEK: u32 = 0x000732;
 
 impl Phy {
     /// Probe all addresses on MDIO for a known PHY
     pub fn find<PA: PhyAccess>(pa: &mut PA) -> Option<Phy> {
-        for addr in 1..32 {
-            let device = match identify_phy(pa, addr) {
+        (1..32).filter_map(|addr| {
+            match identify_phy(pa, addr) {
                 Some(PhyIdentifier {
-                    oui: OUI_MARVEL,
+                    oui: OUI_MARVELL,
                     model: 36,
                     ..
-                }) => Some(PhyDevice::Marvel88E1116R),
+                }) => Some(PhyDevice::Marvell88E1116R),
                 Some(PhyIdentifier {
                     oui: OUI_REALTEK,
                     model: 0b010001,
                     rev: 0b0101,
                 }) => Some(PhyDevice::Rtl8211E),
                 _ => None,
-            };
-            match device {
-                Some(device) =>
-                    return Some(Phy { addr, device }),
-                None => {}
-            }
-        }
-
-        None
+            }.map(|device| Phy { addr, device })
+        }).next()
     }
 
     pub fn name(&self) -> &'static str {
         match self.device {
-            PhyDevice::Marvel88E1116R => &"Marvel 88E1116R",
+            PhyDevice::Marvell88E1116R => &"Marvell 88E1116R",
             PhyDevice::Rtl8211E => &"RTL8211E",
         }
     }
@@ -120,12 +113,8 @@ impl Phy {
         if !status.link_status() {
             None
         } else if status.cap_1000base_t_extended_status() {
-            let ext_status: ExtendedStatus = self.read_reg(pa);
-            if let Some(link) = ext_status.get_link() {
-                Some(link)
-            } else {
-                status.get_link()
-            }
+            let phy_status: PSSR = self.read_reg(pa);
+            phy_status.get_link()
         } else {
             status.get_link()
         }

libboard_zynq/src/eth/phy/pssr.rs

@@ -0,0 +1,52 @@
+use bit_field::BitField;
+use super::{PhyRegister, Link, LinkDuplex, LinkSpeed};
+
+#[derive(Clone, Copy, Debug)]
+/// PHY-Specific Status Register
+pub struct PSSR(pub u16);
+
+impl PSSR {
+    pub fn link(&self) -> bool {
+        self.0.get_bit(10)
+    }
+
+    pub fn duplex(&self) -> LinkDuplex {
+        if self.0.get_bit(13) {
+            LinkDuplex::Full
+        } else {
+            LinkDuplex::Half
+        }
+    }
+
+    pub fn speed(&self) -> Option<LinkSpeed> {
+        match self.0.get_bits(14..=15) {
+            0b00 => Some(LinkSpeed::S10),
+            0b01 => Some(LinkSpeed::S100),
+            0b10 => Some(LinkSpeed::S1000),
+            _ => None,
+        }
+    }
+
+    pub fn get_link(&self) -> Option<Link> {
+        if self.link() {
+            Some(Link {
+                speed: self.speed()?,
+                duplex: self.duplex(),
+            })
+        } else {
+            None
+        }
+    }
+}
+
+impl PhyRegister for PSSR {
+    fn addr() -> u8 {
+        0x11
+    }
+}
+
+impl From<u16> for PSSR {
+    fn from(value: u16) -> Self {
+        PSSR(value)
+    }
+}

libboard_zynq/src/gic.rs

@@ -0,0 +1,150 @@
+//! ARM Generic Interrupt Controller
+
+use bit_field::BitField;
+use libregister::{RegisterW, RegisterRW, RegisterR};
+use super::mpcore;
+
+#[derive(Debug, Clone, Copy)]
+pub struct InterruptId(pub u8);
+
+#[derive(Debug, Clone, Copy)]
+#[repr(u8)]
+pub enum CPUCore {
+    Core0 = 0b01,
+    Core1 = 0b10
+}
+
+#[derive(Debug, Clone, Copy)]
+pub struct TargetCPU(u8);
+
+impl TargetCPU {
+    pub const fn none() -> TargetCPU {
+        TargetCPU(0)
+    }
+
+    pub const fn and(self, other: TargetCPU) -> TargetCPU {
+        TargetCPU(self.0 | other.0)
+    }
+}
+
+impl From<CPUCore> for TargetCPU {
+    fn from(core: CPUCore) -> Self {
+        TargetCPU(core as u8)
+    }
+}
+
+pub enum TargetList {
+    CPUList(TargetCPU),
+    Others,
+    This
+}
+
+impl From<CPUCore> for TargetList {
+    fn from(core: CPUCore) -> Self {
+        TargetList::CPUList(TargetCPU(core as u8))
+    }
+}
+
+impl From<TargetCPU> for TargetList {
+    fn from(cpu: TargetCPU) -> Self {
+        TargetList::CPUList(cpu)
+    }
+}
+
+#[derive(Debug, Clone, Copy)]
+pub enum InterruptSensitivity {
+    Level,
+    Edge,
+}
+
+pub struct InterruptController {
+    mpcore: &'static mut mpcore::RegisterBlock,
+}
+
+impl InterruptController {
+    pub fn new(mpcore: &'static mut mpcore::RegisterBlock) -> Self {
+        InterruptController { mpcore }
+    }
+
+    pub fn disable_interrupts(&mut self) {
+        self.mpcore.iccicr.modify(|_, w| w.enable_ns(false)
+                                          .enable_s(false));
+        // FIXME: Should we disable the distributor globally when we disable interrupt (for a single
+        // core)?
+        // self.mpcore.icddcr.modify(|_, w| w.enable_secure(false)
+        //                                   .enable_non_secure(false));
+    }
+
+    /// enable interrupt signaling
+    pub fn enable_interrupts(&mut self) {
+        self.mpcore.iccicr.modify(|_, w| w.enable_ns(true)
+                                          .enable_s(true));
+        self.mpcore.icddcr.modify(|_, w| w.enable_secure(true));
+
+        // Enable all interrupts except those of the lowest priority.
+        self.mpcore.iccpmr.write(mpcore::ICCPMR::zeroed().priority(0xFF));
+    }
+
+    /// send software generated interrupt
+    pub fn send_sgi(&mut self, id: InterruptId, targets: TargetList) {
+        assert!(id.0 < 16);
+        self.mpcore.icdsgir.modify(|_, w| match targets {
+            TargetList::CPUList(list) => w.target_list_filter(0).cpu_target_list(list.0),
+            TargetList::Others => w.target_list_filter(0b01),
+            TargetList::This => w.target_list_filter(0b10)
+        }.sgiintid(id.0).satt(false));
+    }
+
+    /// enable the interrupt *for this core*.
+    /// Not needed for SGI.
+    pub fn enable(&mut self, id: InterruptId, target_cpu: CPUCore, sensitivity: InterruptSensitivity, priority: u8) {
+        // only 5 bits of the priority is useful
+        assert!(priority < 32);
+
+        self.disable_interrupts();
+
+        // enable
+        let m = (id.0 >> 5) as usize;
+        let n = (id.0 & 0x1F) as usize;
+        assert!(m < 3);
+        unsafe {
+            self.mpcore.icdiser[m].modify(|mut icdiser| *icdiser.set_bit(n, true));
+        }
+
+        // target cpu
+        let m = (id.0 >> 2) as usize;
+        let n = (8 * (id.0 & 3)) as usize;
+        unsafe {
+            self.mpcore.icdiptr[m].modify(|mut icdiptr| *icdiptr.set_bits(n..=n+1, target_cpu as u32 + 1));
+        }
+
+        // sensitivity
+        let m = (id.0 >> 4) as usize;
+        let n = (2 * (id.0 & 0xF)) as usize;
+        unsafe {
+            self.mpcore.icdicfr[m].modify(|mut icdicfr| *icdicfr.set_bits(n..=n+1, match sensitivity {
+                InterruptSensitivity::Level => 0b00,
+                InterruptSensitivity::Edge  => 0b10,
+            }));
+        }
+
+        // priority
+        let offset = (id.0 % 4) * 8;
+        let priority: u32 = (priority as u32) << (offset + 3);
+        let mask: u32 = 0xFFFFFFFF ^ (0xFF << offset);
+        unsafe {
+            self.mpcore.icdipr[id.0 as usize / 4].modify(|v| (v & mask) | priority);
+        }
+
+        self.enable_interrupts();
+    }
+
+    pub fn end_interrupt(&mut self, id: InterruptId) {
+        self.mpcore.icceoir.modify(|_, w| w.eoiintid(id.0 as u32));
+    }
+
+    pub fn get_interrupt_id(&self) -> InterruptId {
+        InterruptId(self.mpcore.icciar.read().ackintid() as u8)
+    }
+
+}

libboard_zynq/src/i2c/eeprom.rs

@@ -0,0 +1,107 @@
+use super::I2C;
+use crate::time::Milliseconds;
+use embedded_hal::timer::CountDown;
+
+pub struct EEPROM<'a> {
+    i2c: &'a mut I2C,
+    port: u8,
+    address: u8,
+    page_size: u8,
+    count_down: crate::timer::global::CountDown<Milliseconds>
+}
+
+impl<'a> EEPROM<'a> {
+    #[cfg(feature = "target_zc706")]
+    pub fn new(i2c: &'a mut I2C, page_size: u8) -> Self {
+        EEPROM {
+            i2c: i2c,
+            port: 2,
+            address: 0b1010100,
+            page_size: page_size,
+            count_down: unsafe { crate::timer::GlobalTimer::get() }.countdown()
+        }
+    }
+
+    #[cfg(feature = "target_zc706")]
+    fn select(&mut self) -> Result<(), &'static str> {
+        let mask: u16 = 1 << self.port;
+        self.i2c.pca9548_select(0b1110100, mask as u8)?;
+        Ok(())
+    }
+
+    /// Random read
+    pub fn read<'r>(&mut self, addr: u8, buf: &'r mut [u8]) -> Result<(), &'static str> {
+        self.select()?;
+
+        self.i2c.start()?;
+        self.i2c.write(self.address << 1)?;
+        self.i2c.write(addr)?;
+
+        self.i2c.restart()?;
+        self.i2c.write((self.address << 1) | 1)?;
+        let buf_len = buf.len();
+        for (i, byte) in buf.iter_mut().enumerate() {
+            *byte = self.i2c.read(i < buf_len - 1)?;
+        }
+
+        self.i2c.stop()?;
+
+        Ok(())
+    }
+
+    /// Smart multi-page writing
+    /// Using the "Page Write" function of an EEPROM, the memory region for each transaction
+    /// (i.e. from byte `addr` to byte `addr+buf.len()`) should fit under each page 
+    /// (i.e. `addr+buf.len()` < `addr/self.page_size+1`); otherwise, a roll-oever occurs, 
+    /// where bytes beyond the page end. This smart function takes care of the scenario to avoid
+    /// any roll-over when writing ambiguous memory regions.
+    pub fn write(&mut self, addr: u8, buf: &[u8]) -> Result<(), &'static str> {
+        self.select()?;
+
+        let buf_len = buf.len();
+        let mut pb: u8 = addr % self.page_size;
+        for (i, byte) in buf.iter().enumerate() {
+            if (i == 0) || (pb == 0) {
+                self.i2c.start()?;
+                self.i2c.write(self.address << 1)?;
+                self.i2c.write(addr + (i as u8))?;
+            }
+            self.i2c.write(*byte)?;
+            pb += 1;
+
+            if (i == buf_len-1) || (pb == self.page_size) {
+                self.i2c.stop()?;
+                self.poll(1_000)?;
+                pb = 0;
+            }
+        }
+
+        Ok(())
+    }
+
+    /// Poll
+    pub fn poll(&mut self, timeout_ms: u64) -> Result<(), &'static str> {
+        self.select()?;
+
+        self.count_down.start(Milliseconds(timeout_ms));
+        loop {
+            self.i2c.start()?;
+            let ack = self.i2c.write(self.address << 1)?;
+            self.i2c.stop()?;
+            if ack {
+                break
+            };
+            if !self.count_down.waiting() {
+                return Err("I2C polling timeout")
+            }
+        }
+
+        Ok(())
+    }
+
+    pub fn read_eui48<'r>(&mut self) -> Result<[u8; 6], &'static str> {
+        let mut buffer = [0u8; 6];
+        self.read(0xFA, &mut buffer)?;
+        Ok(buffer)
+    }
+}

libboard_zynq/src/i2c/mod.rs

@@ -0,0 +1,235 @@
+//! I2C Bit-banging Controller
+
+mod regs;
+pub mod eeprom;
+use super::clocks::Clocks;
+use super::slcr;
+use super::time::Microseconds;
+use embedded_hal::timer::CountDown;
+use libregister::{RegisterR, RegisterRW, RegisterW};
+
+const INVALID_BUS: &'static str = "Invalid I2C bus";
+
+pub struct I2C {
+    regs: regs::RegisterWrapper,
+    count_down: super::timer::global::CountDown<Microseconds>
+}
+
+impl I2C {
+    #[cfg(feature = "target_zc706")]
+    pub fn i2c() -> Self {
+        // Route I2C 0 SCL / SDA Signals to MIO Pins 50 / 51
+        slcr::RegisterBlock::unlocked(|slcr| {
+            // SCL
+            slcr.mio_pin_50.write(
+                slcr::MioPin50::zeroed()
+                    .l3_sel(0b000)  // as GPIO 50
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+                    .disable_rcvr(true)
+            );
+            // SDA
+            slcr.mio_pin_51.write(
+                slcr::MioPin51::zeroed()
+                    .l3_sel(0b000)  // as GPIO 51
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+                    .disable_rcvr(true)
+            );
+            // Reset
+            slcr.gpio_rst_ctrl.reset_gpio();
+        });
+
+        Self::ctor_common(0xFFFF - 0x000C)
+    }
+
+    fn ctor_common(gpio_output_mask: u16) -> Self {
+        // Setup register block
+        let clocks = Clocks::get();
+        let self_ = Self {
+            regs: regs::RegisterWrapper::new(),
+            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown()
+        };
+
+        // Setup GPIO output mask
+        self_.regs.gpio_output_mask.modify(|_, w| {
+            w.mask(gpio_output_mask)
+        });
+        // Setup GPIO driver direction
+        self_.regs.gpio_direction.modify(|_, w| {
+            w.scl(true).sda(true)
+        });
+
+        self_
+    }
+
+    /// Delay for I2C operations, simple wrapper for nb.
+    fn delay_us(&mut self, us: u64) {
+        self.count_down.start(Microseconds(us));
+        nb::block!(self.count_down.wait()).unwrap();
+    }
+
+    fn half_period(&mut self) { self.delay_us(100) }
+
+    fn sda_i(&mut self) -> bool {
+        self.regs.gpio_input.read().sda()
+    }
+
+    fn scl_i(&mut self) -> bool {
+        self.regs.gpio_input.read().scl()
+    }
+
+    fn sda_oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+             w.sda(oe)
+        })
+    }
+
+    fn sda_o(&mut self, o: bool) {
+        self.regs.gpio_output_mask.modify(|_, w| {
+             w.sda_o(o)
+        })
+    }
+
+    fn scl_oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+             w.scl(oe)
+        })
+    }
+
+    fn scl_o(&mut self, o: bool) {
+        self.regs.gpio_output_mask.modify(|_, w| {
+             w.scl_o(o)
+        })
+    }
+
+    pub fn init(&mut self) -> Result<(), &'static str> {
+        self.scl_oe(false);
+        self.sda_oe(false);
+        self.scl_o(false);
+        self.sda_o(false);
+
+        // Check the I2C bus is ready
+        self.half_period();
+        self.half_period();
+        if !self.sda_i() {
+            // Try toggling SCL a few times
+            for _bit in 0..8 {
+                self.scl_oe(true);
+                self.half_period();
+                self.scl_oe(false);
+                self.half_period();
+            }
+        }
+
+        if !self.sda_i() {
+            return Err("SDA is stuck low and doesn't get unstuck");
+        }
+        if !self.scl_i() {
+            return Err("SCL is stuck low and doesn't get unstuck");
+        }
+        // postcondition: SCL and SDA high
+        Ok(())
+    }
+
+    pub fn start(&mut self) -> Result<(), &'static str> {
+        // precondition: SCL and SDA high
+        if !self.scl_i() {
+            return Err("SCL is stuck low and doesn't get unstuck");
+        }
+        if !self.sda_i() {
+            return Err("SDA arbitration lost");
+        }
+        self.sda_oe(true);
+        self.half_period();
+        self.scl_oe(true);
+        // postcondition: SCL and SDA low
+        Ok(())
+    }
+
+    pub fn restart(&mut self) -> Result<(), &'static str> {
+        // precondition SCL and SDA low
+        self.sda_oe(false);
+        self.half_period();
+        self.scl_oe(false);
+        self.half_period();
+        self.start()?;
+        // postcondition: SCL and SDA low
+        Ok(())
+    }
+
+    pub fn stop(&mut self) -> Result<(), &'static str> {
+        // precondition: SCL and SDA low
+        self.half_period();
+        self.scl_oe(false);
+        self.half_period();
+        self.sda_oe(false);
+        self.half_period();
+        if !self.sda_i() {
+            return Err("SDA arbitration lost");
+        }
+        // postcondition: SCL and SDA high
+        Ok(())
+    }
+
+    pub fn write(&mut self, data: u8) -> Result<bool, &'static str> {
+        // precondition: SCL and SDA low
+        // MSB first
+        for bit in (0..8).rev() {
+            self.sda_oe(data & (1 << bit) == 0);
+            self.half_period();
+            self.scl_oe(false);
+            self.half_period();
+            self.scl_oe(true);
+        }
+        self.sda_oe(false);
+        self.half_period();
+        self.scl_oe(false);
+        self.half_period();
+        // Read ack/nack
+        let ack = !self.sda_i();
+        self.scl_oe(true);
+        self.sda_oe(true);
+        // postcondition: SCL and SDA low
+
+        Ok(ack)
+    }
+
+    pub fn read(&mut self, ack: bool) -> Result<u8, &'static str> {
+        // precondition: SCL and SDA low
+        self.sda_oe(false);
+
+        let mut data: u8 = 0;
+
+        // MSB first
+        for bit in (0..8).rev() {
+            self.half_period();
+            self.scl_oe(false);
+            self.half_period();
+            if self.sda_i() { data |= 1 << bit }
+            self.scl_oe(true);
+        }
+        // Send ack/nack
+        self.sda_oe(ack);
+        self.half_period();
+        self.scl_oe(false);
+        self.half_period();
+        self.scl_oe(true);
+        self.sda_oe(true);
+        // postcondition: SCL and SDA low
+
+        Ok(data)
+    }
+
+    pub fn pca9548_select(&mut self, address: u8, channels: u8) -> Result<(), &'static str> {
+        self.start()?;
+        if !self.write(address << 1)? {
+            return Err("PCA9548 failed to ack write address")
+        }
+        if !self.write(channels)? {
+            return Err("PCA9548 failed to ack control word")
+        }
+        self.stop()?;
+        Ok(())
+    }
+}

libboard_zynq/src/i2c/regs.rs

@@ -0,0 +1,93 @@
+use volatile_register::{RO, WO, RW};
+
+use libregister::{
+    register, register_at,
+    register_bit, register_bits
+};
+
+// With reference to:
+//
+// artiq:artiq/gateware/targets/kasli.py:
+// self.submodules.i2c = gpio.GPIOTristate([i2c.scl, i2c.sda])
+//
+// misoc:misoc/cores/gpio.py:
+// class GPIOTristate(Module, AutoCSR):
+//    def __init__(self, signals, reset_out=0, reset_oe=0):
+//        l = len(signals)
+//        self._in = CSRStatus(l)
+//        self._out = CSRStorage(l, reset=reset_out)
+//        self._oe = CSRStorage(l, reset=reset_oe)
+// 
+// Hence, using GPIOs as SCL and SDA GPIOs respectively.
+//
+// Current compatibility:
+// zc706: GPIO 50, 51 == SCL, SDA
+
+pub struct RegisterWrapper {
+    pub gpio_output_mask: &'static mut GPIOOutputMask,
+    pub gpio_input: &'static mut GPIOInput,
+    pub gpio_direction: &'static mut GPIODirection,
+    pub gpio_output_enable: &'static mut GPIOOutputEnable,
+}
+
+impl RegisterWrapper {
+    pub fn new() -> Self {
+        Self {
+            gpio_output_mask: GPIOOutputMask::new(),
+            gpio_input: GPIOInput::new(),
+            gpio_direction: GPIODirection::new(),
+            gpio_output_enable: GPIOOutputEnable::new()
+        }
+    }
+}
+
+// MASK_DATA_1_MSW:
+// Maskable output data for MIO[53:48]
+register!(gpio_output_mask, GPIOOutputMask, RW, u32);
+#[cfg(feature = "target_zc706")]
+register_at!(GPIOOutputMask, 0xE000A00C, new);
+// Output for SCL
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_output_mask, scl_o, 2);
+// Output for SDA
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_output_mask, sda_o, 3);
+// Mask for keeping bits except SCL and SDA unchanged
+#[cfg(feature = "target_zc706")]
+register_bits!(gpio_output_mask, mask, u16, 16, 31);
+
+// DATA_1_RO:
+// Input data for MIO[53:32]
+register!(gpio_input, GPIOInput, RO, u32);
+#[cfg(feature = "target_zc706")]
+register_at!(GPIOInput, 0xE000A064, new);
+// Input for SCL
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_input, scl, 18);
+// Input for SDA
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_input, sda, 19);
+
+// DIRM_1:
+// Direction mode for MIO[53:32]; 0/1 = in/out
+register!(gpio_direction, GPIODirection, RW, u32);
+#[cfg(feature = "target_zc706")]
+register_at!(GPIODirection, 0xE000A244, new);
+// Direction for SCL
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_direction, scl, 18);
+// Direction for SDA
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_direction, sda, 19);
+
+// OEN_1:
+// Output enable for MIO[53:32]
+register!(gpio_output_enable, GPIOOutputEnable, RW, u32);
+#[cfg(feature = "target_zc706")]
+register_at!(GPIOOutputEnable, 0xE000A248, new);
+// Output enable for SCL
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_output_enable, scl, 18);
+// Output enable for SDA
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_output_enable, sda, 19);

libboard_zynq/src/lib.rs

@@ -15,9 +15,12 @@ pub mod axi_hp;
 pub mod axi_gp;
 pub mod ddr;
 pub mod mpcore;
+pub mod gic;
 pub mod flash;
 pub mod time;
 pub mod timer;
 pub mod sdio;
+#[cfg(feature = "target_zc706")]
+pub mod i2c;
 pub mod logger;
 pub mod ps7_init;

libboard_zynq/src/mpcore.rs

@@ -8,48 +8,141 @@ use libregister::{
 
 #[repr(C)]
 pub struct RegisterBlock {
+    /// SCU Control Register
     pub scu_control: ScuControl,
-    pub scu_config: RO<u32>,
-    pub scu_cpu_power: RW<u32>,
+    /// SCU Configuration Register
+    pub scu_config: ScuConfig,
+    /// SCU CPU Power Status Register
+    pub scu_cpu_power_status: SCUCPUPowerStatusRegister,
+    /// SCU Invalidate All Registers in Secure State
     pub scu_invalidate: ScuInvalidate,
-    reserved0: [u32; 12],
-    pub filter_start: RW<u32>,
-    pub filter_end: RW<u32>,
-    reserved1: [u32; 2],
-    pub scu_access_control: RW<u32>,
-    pub scu_non_secure_access_control: RW<u32>,
-    reserved2: [u32; 42],
-    pub iccicr: RW<u32>,
-    pub iccpmw: RW<u32>,
-    pub iccbpr: RW<u32>,
-    pub icciar: RW<u32>,
-    pub icceoir: RW<u32>,
-    pub iccrpr: RW<u32>,
-    pub icchpir: RW<u32>,
-    pub iccabpr: RW<u32>,
-    reserved3: [u32; 55],
-    pub iccidr: RW<u32>,
+    unused0: [u32; 12],
+    /// Filtering Start Address Register
+    pub filtering_start_address: FilteringStartAddressRegister,
+    /// Defined by FILTEREND input
+    pub filtering_end_address: FilteringEndAddressRegister,
+    unused1: [u32; 2],
+    /// SCU Access Control (SAC) Register
+    pub scu_access_control_sac: SCUAccessControlRegisterSAC,
+    /// SCU Non-secure Access Control Register SNSAC
+    pub scu_non_secure_access_control: SCUNonSecureAccessControlRegister,
+    unused2: [u32; 42],
+    /// CPU Interface Control Register
+    pub iccicr: ICCICR,
+    /// Interrupt Priority Mask Register
+    pub iccpmr: ICCPMR,
+    /// Binary Point Register
+    pub iccbpr: ICCBPR,
+    /// Interrupt Acknowledge Register
+    pub icciar: ICCIAR,
+    /// End Of Interrupt Register
+    pub icceoir: ICCEOIR,
+    /// Running Priority Register
+    pub iccrpr: ICCRPR,
+    /// Highest Pending Interrupt Register
+    pub icchpir: ICCHPIR,
+    /// Aliased Non-secure Binary Point Register
+    pub iccabpr: ICCABPR,
+    unused3: [u32; 55],
+    /// CPU Interface Implementer Identification Register
+    pub iccidr: ICCIDR,
+    /// Global Timer Counter Register 0
     pub global_timer_counter0: ValueRegister,
     pub global_timer_counter1: ValueRegister,
+    /// Global Timer Control Register
     pub global_timer_control: GlobalTimerControl,
-    pub global_timer_interrupt_status: RW<u32>,
+    /// Global Timer Interrupt Status Register
+    pub global_timer_interrupt_status: GlobalTimerInterruptStatusRegister,
+    /// Comparator Value Register_0
     pub comparator_value0: ValueRegister,
     pub comparator_value1: ValueRegister,
-    pub auto_increment: ValueRegister,
-    reserved4: [u32; 249],
-    pub private_timer_load: ValueRegister,
-    pub private_timer_counter: ValueRegister,
-    pub private_timer_control: RW<u32>,
-    pub private_timer_interrupt_status: RW<u32>,
-    reserved5: [u32; 4],
-    pub watchdog_load: ValueRegister,
-    pub watchdog_counter: ValueRegister,
-    pub watchdog_control: RW<u32>,
-    pub watchdog_interrupt_status: RW<u32>,
-    // there is plenty more (unimplemented)
+    /// Auto-increment Register
+    pub auto_increment: RW<u32>,
+    unused4: [u32; 249],
+    /// Private Timer Load Register
+    pub private_timer_load: RW<u32>,
+    /// Private Timer Counter Register
+    pub private_timer_counter: RW<u32>,
+    /// Private Timer Control Register
+    pub private_timer_control: PrivateTimerControlRegister,
+    /// Private Timer Interrupt Status Register
+    pub private_timer_interrupt_status: PrivateTimerInterruptStatusRegister,
+    unused5: [u32; 4],
+    /// Watchdog Load Register
+    pub watchdog_load: RW<u32>,
+    /// Watchdog Counter Register
+    pub watchdog_counter: RW<u32>,
+    /// Watchdog Control Register
+    pub watchdog_control: WatchdogControlRegister,
+    /// Watchdog Interrupt Status Register
+    pub watchdog_interrupt_status: WatchdogInterruptStatusRegister,
+    /// Watchdog Reset Status Register
+    pub watchdog_reset_status: WatchdogResetStatusRegister,
+    /// Watchdog Disable Register
+    pub watchdog_disable: RW<u32>,
+    unused6: [u32; 626],
+    /// Distributor Control Register
+    pub icddcr: ICDDCR,
+    /// Interrupt Controller Type Register
+    pub icdictr: ICDICTR,
+    /// Distributor Implementer Identification Register
+    pub icdiidr: ICDIIDR,
+    unused7: [u32; 29],
+    /// Interrupt Security Register
+    pub icdisr0: RW<u32>,
+    pub icdisr1: RW<u32>,
+    pub icdisr2: RW<u32>,
+    unused8: [u32; 29],
+    /// Interrupt Set-enable Registers
+    pub icdiser: [RW<u32>; 3],
+    unused9: [u32; 29],
+    /// Interrupt Clear-Enable Register 0
+    pub icdicer0: RW<u32>,
+    /// Interrupt Clear-Enable Register 1
+    pub icdicer1: RW<u32>,
+    /// Interrupt Clear-Enable Register 2
+    pub icdicer2: RW<u32>,
+    unused10: [u32; 29],
+    /// Interrupt Set-pending Register
+    pub icdispr0: RW<u32>,
+    pub icdispr1: RW<u32>,
+    pub icdispr2: RW<u32>,
+    unused11: [u32; 29],
+    /// Interrupt Clear-Pending Register
+    pub icdicpr0: RW<u32>,
+    pub icdicpr1: RW<u32>,
+    pub icdicpr2: RW<u32>,
+    unused12: [u32; 29],
+    /// Active Bit register
+    pub icdabr0: RW<u32>,
+    pub icdabr1: RW<u32>,
+    pub icdabr2: RW<u32>,
+    unused13: [u32; 61],
+    /// Interrupt Priority Register
+    pub icdipr: [RW<u32>; 24],
+    unused14: [u32; 232],
+    /// Interrupt Processor Targets Registers
+    pub icdiptr: [RW<u32>; 24],
+    unused15: [u32; 232],
+    /// Interrupt Configuration Registers
+    pub icdicfr: [RW<u32>; 6],
+    unused16: [u32; 58],
+    /// PPI Status Register
+    pub ppi_status: PpiStatus,
+    /// SPI Status Register 0
+    pub spi_status_0: RO<u32>,
+    /// SPI Status Register 1
+    pub spi_status_1: RO<u32>,
+    unused17: [u32; 125],
+    /// Software Generated Interrupt Register
+    pub icdsgir: ICDSGIR,
 }
+
 register_at!(RegisterBlock, 0xF8F00000, new);
 
+register!(value_register, ValueRegister, RW, u32);
+register_bits!(value_register, value, u32, 0, 31);
+
 register!(scu_control, ScuControl, RW, u32);
 register_bit!(scu_control, ic_standby_enable, 6);
 register_bit!(scu_control, scu_standby_enable, 5);
@@ -65,6 +158,17 @@ impl ScuControl {
     }
 }
 
+register!(scu_config, ScuConfig, RO, u32);
+register_bits!(scu_config, tag_ram_sizes, u8, 8, 15);
+register_bits!(scu_config, cpus_smp, u8, 4, 7);
+register_bits!(scu_config, cpu_number, u8, 0, 1);
+
+register!(scu_cpu_power_status, SCUCPUPowerStatusRegister, RW, u32);
+register_bits!(scu_cpu_power_status, cpu3_status, u8, 24, 25);
+register_bits!(scu_cpu_power_status, cpu2_status, u8, 16, 17);
+register_bits!(scu_cpu_power_status, cpu1_status, u8, 8, 9);
+register_bits!(scu_cpu_power_status, cpu0_status, u8, 0, 1);
+
 register!(scu_invalidate, ScuInvalidate, WO, u32);
 register_bits!(scu_invalidate, cpu0_ways, u8, 0, 3);
 register_bits!(scu_invalidate, cpu1_ways, u8, 4, 7);
@@ -88,8 +192,71 @@ impl ScuInvalidate {
     }
 }
 
-register!(value_register, ValueRegister, RW, u32);
-register_bits!(value_register, value, u32, 0, 31);
+register!(filtering_start_address, FilteringStartAddressRegister, RW, u32);
+register_bits!(filtering_start_address, filtering_start_address, u32, 20, 31);
+register_bits!(filtering_start_address, sbz, u32, 0, 19);
+
+register!(filtering_end_address, FilteringEndAddressRegister, RW, u32);
+register_bits!(filtering_end_address, filtering_end_address, u32, 20, 31);
+register_bits!(filtering_end_address, sbz, u32, 0, 19);
+
+register!(scu_access_control_sac, SCUAccessControlRegisterSAC, RW, u32);
+register_bit!(scu_access_control_sac, cp_u3, 3);
+register_bit!(scu_access_control_sac, cp_u2, 2);
+register_bit!(scu_access_control_sac, cp_u1, 1);
+register_bit!(scu_access_control_sac, cp_u0, 0);
+
+register!(scu_non_secure_access_control, SCUNonSecureAccessControlRegister, RO, u32);
+register_bits!(scu_non_secure_access_control, sbz, u32, 12, 31);
+register_bit!(scu_non_secure_access_control, cpu3_global_timer, 11);
+register_bit!(scu_non_secure_access_control, cpu2_global_timer, 10);
+register_bit!(scu_non_secure_access_control, cpu1_global_timer, 9);
+register_bit!(scu_non_secure_access_control, cpu0_global_timer, 8);
+register_bit!(scu_non_secure_access_control, private_timers_for_cpu3, 7);
+register_bit!(scu_non_secure_access_control, private_timers_for_cpu2, 6);
+register_bit!(scu_non_secure_access_control, private_timers_for_cpu1, 5);
+register_bit!(scu_non_secure_access_control, private_timers_for_cpu0, 4);
+register_bit!(scu_non_secure_access_control, component_access_for_cpu3, 3);
+register_bit!(scu_non_secure_access_control, component_access_for_cpu2, 2);
+register_bit!(scu_non_secure_access_control, component_access_for_cpu1, 1);
+register_bit!(scu_non_secure_access_control, component_access_for_cpu0, 0);
+
+register!(iccicr, ICCICR, RW, u32);
+register_bit!(iccicr, sbpr, 4);
+register_bit!(iccicr, fiq_en, 3);
+register_bit!(iccicr, ack_ctl, 2);
+register_bit!(iccicr, enable_ns, 1);
+register_bit!(iccicr, enable_s, 0);
+
+register!(iccpmr, ICCPMR, RW, u32);
+register_bits!(iccpmr, priority, u8, 0, 7);
+
+register!(iccbpr, ICCBPR, RW, u32);
+register_bits!(iccbpr, binary_point, u8, 0, 2);
+
+register!(icciar, ICCIAR, RW, u32);
+register_bits!(icciar, cpuid, u8, 10, 12);
+register_bits!(icciar, ackintid, u32, 0, 9);
+
+register!(icceoir, ICCEOIR, RW, u32);
+register_bits!(icceoir, cpuid, u8, 10, 12);
+register_bits!(icceoir, eoiintid, u32, 0, 9);
+
+register!(iccrpr, ICCRPR, RW, u32);
+register_bits!(iccrpr, priority, u8, 0, 7);
+
+register!(icchpir, ICCHPIR, RW, u32);
+register_bits!(icchpir, cpuid, u8, 10, 12);
+register_bits!(icchpir, pendintid, u32, 0, 9);
+
+register!(iccabpr, ICCABPR, RW, u32);
+register_bits!(iccabpr, binary_point, u8, 0, 2);
+
+register!(iccidr, ICCIDR, RO, u32);
+register_bits!(iccidr, part_number, u32, 20, 31);
+register_bits!(iccidr, architecture_version, u8, 16, 19);
+register_bits!(iccidr, revision_number, u8, 12, 15);
+register_bits!(iccidr, implementer, u32, 0, 11);
 
 register!(global_timer_control, GlobalTimerControl, RW, u32);
 register_bits!(global_timer_control, prescaler, u8, 8, 15);
@@ -97,3 +264,58 @@ register_bit!(global_timer_control, auto_increment_mode, 3);
 register_bit!(global_timer_control, irq_enable, 2);
 register_bit!(global_timer_control, comp_enablea, 1);
 register_bit!(global_timer_control, timer_enable, 0);
+
+register!(global_timer_interrupt_status, GlobalTimerInterruptStatusRegister, RW, u32);
+register_bit!(global_timer_interrupt_status, event_flag, 0);
+
+register!(private_timer_control, PrivateTimerControlRegister, RW, u32);
+register_bits!(private_timer_control, sbzp, u32, 16, 31);
+register_bits!(private_timer_control, prescaler, u8, 8, 15);
+register_bits!(private_timer_control, unk_sbzp, u8, 3, 7);
+register_bit!(private_timer_control, irq_enable, 2);
+register_bit!(private_timer_control, auto_reload, 1);
+register_bit!(private_timer_control, timer_enable, 0);
+
+register!(private_timer_interrupt_status, PrivateTimerInterruptStatusRegister, RW, u32);
+register_bits!(private_timer_interrupt_status, unk_sbzp, u32, 1, 31);
+
+register!(watchdog_control, WatchdogControlRegister, RW, u32);
+register_bits!(watchdog_control, prescaler, u8, 8, 15);
+register_bit!(watchdog_control, watchdog_mode, 3);
+register_bit!(watchdog_control, it_enable, 2);
+register_bit!(watchdog_control, auto_reload, 1);
+register_bit!(watchdog_control, watchdog_enable, 0);
+
+register!(watchdog_interrupt_status, WatchdogInterruptStatusRegister, RW, u32);
+register_bit!(watchdog_interrupt_status, event_flag, 0);
+
+register!(watchdog_reset_status, WatchdogResetStatusRegister, RW, u32);
+register_bit!(watchdog_reset_status, reset_flag, 0);
+
+register!(icddcr, ICDDCR, RW, u32);
+register_bit!(icddcr, enable_non_secure, 1);
+register_bit!(icddcr, enable_secure, 0);
+
+register!(icdictr, ICDICTR, RO, u32);
+register_bits!(icdictr, lspi, u8, 11, 15);
+register_bit!(icdictr, security_extn, 10);
+register_bits!(icdictr, sbz, u8, 8, 9);
+register_bits!(icdictr, cpu_number, u8, 5, 7);
+register_bits!(icdictr, it_lines_number, u8, 0, 4);
+
+register!(icdiidr, ICDIIDR, RO, u32);
+register_bits!(icdiidr, implementation_version, u8, 24, 31);
+register_bits!(icdiidr, revision_number, u32, 12, 23);
+register_bits!(icdiidr, implementer, u32, 0, 11);
+
+register!(ppi_status, PpiStatus, RO, u32);
+register_bits!(ppi_status, ppi_status, u8, 11, 15);
+register_bits!(ppi_status, sbz, u32, 0, 10);
+
+register!(icdsgir, ICDSGIR, RW, u32);
+register_bits!(icdsgir, target_list_filter, u8, 24, 25);
+register_bits!(icdsgir, cpu_target_list, u8, 16, 23);
+register_bit!(icdsgir, satt, 15);
+register_bits!(icdsgir, sbz, u32, 4, 14);
+register_bits!(icdsgir, sgiintid, u8, 0, 3);
+

libboard_zynq/src/slcr.rs

@@ -132,7 +132,7 @@ pub struct RegisterBlock {
     pub can_rst_ctrl: RW<u32>,
     pub i2c_rst_ctrl: RW<u32>,
     pub uart_rst_ctrl: UartRstCtrl,
-    pub gpio_rst_ctrl: RW<u32>,
+    pub gpio_rst_ctrl: GpioRstCtrl,
     pub lqspi_rst_ctrl: LqspiRstCtrl,
     pub smc_rst_ctrl: RW<u32>,
     pub ocm_rst_ctrl: RW<u32>,
@@ -531,6 +531,20 @@ impl UartRstCtrl {
     }
 }
 
+register!(gpio_rst_ctrl, GpioRstCtrl, RW, u32);
+register_bit!(gpio_rst_ctrl, gpio_cpu1x_rst, 0);
+register_at!(GpioRstCtrl, 0xF800022C, new);
+impl GpioRstCtrl {
+    pub fn reset_gpio(&mut self) {
+        self.modify(|_, w|
+            w.gpio_cpu1x_rst(true)
+        );
+        self.modify(|_, w|
+            w.gpio_cpu1x_rst(false)
+        );
+    }
+}
+
 register!(lqspi_clk_ctrl, LqspiClkCtrl, RW, u32);
 register_bit!(lqspi_clk_ctrl, clkact, 0);
 register_bits_typed!(lqspi_clk_ctrl, src_sel, u8, PllSource, 4, 5);

libboard_zynq/src/timer/global.rs

@@ -138,6 +138,15 @@ where
     }
 }
 
+impl<U: PartialOrd> CountDown<U>
+where
+    GlobalTimer: TimeSource<U>,
+{
+    pub fn waiting(&self) -> bool {
+        self.timer.now() <= self.timeout
+    }
+}
+
 /// embedded-hal sync API
 impl embedded_hal::blocking::delay::DelayMs<u64> for GlobalTimer {
     fn delay_ms(&mut self, ms: u64) {

libcortex_a9/Cargo.toml

@@ -7,6 +7,7 @@ edition = "2018"
 [features]
 target_zc706 = []
 target_cora_z7_10 = []
+power_saving = []
 default = ["target_zc706"]
 
 [dependencies]

libcortex_a9/src/asm.rs

@@ -33,3 +33,44 @@ pub fn dsb() {
 pub fn isb() {
     unsafe { llvm_asm!("isb" :::: "volatile") }
 }
+
+/// Enable IRQ
+#[inline]
+pub unsafe fn enable_irq() {
+    llvm_asm!("cpsie i":::: "volatile");
+}
+
+/// Disable IRQ, return if IRQ was originally enabled.
+#[inline]
+pub unsafe fn enter_critical() -> bool {
+    let mut cpsr: u32;
+    llvm_asm!(
+        "mrs $0, cpsr
+         cpsid i"
+         : "=r"(cpsr) ::: "volatile");
+    (cpsr & (1 << 7)) == 0
+}
+
+#[inline]
+pub unsafe fn exit_critical(enable: bool) {
+    // https://stackoverflow.com/questions/40019929/temporarily-disable-interrupts-on-arm
+    let mask: u32 = if enable {
+        1 << 7
+    } else {
+        0
+    };
+    llvm_asm!(
+        "mrs r1, cpsr
+         bic r1, r1, $0
+         msr cpsr_c, r1"
+         :: "r"(mask) : "r1");
+}
+
+/// Exiting IRQ
+#[inline]
+pub unsafe fn exit_irq() {
+    llvm_asm!("
+        mrs r0, SPSR
+        msr CPSR, r0
+        " ::: "r0");
+}

libcortex_a9/src/lib.rs

@@ -1,6 +1,7 @@
 #![no_std]
 #![feature(llvm_asm, global_asm)]
 #![feature(never_type)]
+#![feature(const_fn)]
 
 extern crate alloc;
 
@@ -10,9 +11,26 @@ pub mod cache;
 pub mod mmu;
 pub mod mutex;
 pub mod sync_channel;
+pub mod semaphore;
 mod uncached;
 mod fpu;
 pub use uncached::UncachedSlice;
 pub use fpu::enable_fpu;
 
 global_asm!(include_str!("exceptions.s"));
+
+#[inline]
+pub fn spin_lock_yield() {
+    #[cfg(feature = "power_saving")]
+    asm::wfe();
+}
+
+#[inline]
+pub fn notify_spin_lock() {
+    #[cfg(feature = "power_saving")]
+    {
+        asm::dsb();
+        asm::sev();
+    }
+}
+

libcortex_a9/src/mmu.rs

@@ -338,7 +338,7 @@ impl L1Table {
         /* 0xfff00000 - 0xffffffff (256K OCM when mapped to high address space) */
         self.direct_mapped_section(0xfff, L1Section {
             global: true,
-            shareable: false,
+            shareable: true,
             access: AccessPermissions::FullAccess,
             tex: 0b100,
             domain: 0,

libcortex_a9/src/mutex.rs

@@ -1,20 +1,10 @@
 use core::ops::{Deref, DerefMut};
 use core::sync::atomic::{AtomicU32, Ordering};
 use core::cell::UnsafeCell;
-use super::asm::*;
-
-/// [Power-saving features](http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dht0008a/ch01s03s02.html)
-#[inline]
-fn wait_for_update() {
-    wfe();
-}
-
-/// [Power-saving features](http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dht0008a/ch01s03s02.html)
-#[inline]
-fn signal_update() {
-    dsb();
-    sev();
-}
+use super::{
+    spin_lock_yield, notify_spin_lock,
+    asm::{dmb, enter_critical, exit_critical}
+};
 
 const LOCKED: u32 = 1;
 const UNLOCKED: u32 = 0;
@@ -41,19 +31,26 @@ impl<T> Mutex<T> {
 
     /// Lock the Mutex, blocks when already locked
     pub fn lock(&self) -> MutexGuard<T> {
+        let mut irq = unsafe { enter_critical() };
         while self.locked.compare_and_swap(UNLOCKED, LOCKED, Ordering::Acquire) != UNLOCKED {
-            wait_for_update();
+            unsafe {
+                exit_critical(irq);
+                spin_lock_yield();
+                irq = enter_critical();
+            }
         }
         dmb();
-        MutexGuard { mutex: self }
+        MutexGuard { mutex: self, irq }
     }
 
     pub fn try_lock(&self) -> Option<MutexGuard<T>> {
+        let irq = unsafe { enter_critical() };
         if self.locked.compare_and_swap(UNLOCKED, LOCKED, Ordering::Acquire) != UNLOCKED {
+            unsafe { exit_critical(irq) };
             None
         } else {
             dmb();
-            Some(MutexGuard { mutex: self })
+            Some(MutexGuard { mutex: self, irq })
         }
     }
 
@@ -61,7 +58,7 @@ impl<T> Mutex<T> {
         dmb();
         self.locked.store(UNLOCKED, Ordering::Release);
 
-        signal_update();
+        notify_spin_lock();
     }
 }
 
@@ -69,6 +66,7 @@ impl<T> Mutex<T> {
 /// `Deref`/`DerefMutx`
 pub struct MutexGuard<'a, T> {
     mutex: &'a Mutex<T>,
+    irq: bool,
 }
 
 impl<'a, T> Deref for MutexGuard<'a, T> {
@@ -88,5 +86,6 @@ impl<'a, T> DerefMut for MutexGuard<'a, T> {
 impl<'a, T> Drop for MutexGuard<'a, T> {
     fn drop(&mut self) {
         self.mutex.unlock();
+        unsafe { exit_critical(self.irq) };
     }
 }

libcortex_a9/src/semaphore.rs

@@ -0,0 +1,71 @@
+use super::{spin_lock_yield, notify_spin_lock};
+use core::{
+    task::{Context, Poll},
+    pin::Pin,
+    future::Future,
+    sync::atomic::{AtomicI32, Ordering}
+};
+
+pub struct Semaphore {
+    value: AtomicI32,
+    max: i32
+}
+
+impl Semaphore {
+    pub fn new(value: i32, max: i32) -> Self {
+        Semaphore { value: AtomicI32::new(value), max}
+    }
+
+    pub fn try_wait(&self) -> Option<()> {
+        loop {
+            let value = self.value.load(Ordering::Relaxed);
+            if value > 0 {
+                if self.value.compare_and_swap(value, value - 1, Ordering::SeqCst) == value {
+                    return Some(());
+                }
+            } else {
+                return None;
+            }
+        }
+    }
+
+    pub fn wait(&self) {
+        while self.try_wait().is_none() {
+            spin_lock_yield();
+        }
+    }
+
+    pub async fn async_wait(&self) {
+        struct Fut<'a>(&'a Semaphore);
+
+        impl Future for Fut<'_> {
+            type Output = ();
+            fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
+                match self.0.try_wait() {
+                    Some(_) => Poll::Ready(()),
+                    None => {
+                        cx.waker().wake_by_ref();
+                        Poll::Pending
+                    }
+                }
+            }
+        }
+
+        Fut(&self).await
+    }
+
+    pub fn signal(&self) {
+        loop {
+            let value = self.value.load(Ordering::Relaxed);
+            if value < self.max {
+                if self.value.compare_and_swap(value, value + 1, Ordering::SeqCst) == value {
+                    notify_spin_lock();
+                    return;
+                }
+            } else {
+                return;
+            }
+        }
+    }
+}
+

libcortex_a9/src/sync_channel.rs

@@ -1,115 +1,73 @@
 use core::{
-    future::Future,
     pin::Pin,
-    ptr::null_mut,
-    sync::atomic::{AtomicPtr, Ordering},
+    future::Future,
+    ptr::drop_in_place,
+    sync::atomic::{AtomicPtr, AtomicUsize, Ordering},
     task::{Context, Poll},
 };
-use alloc::{
-    boxed::Box,
-    sync::Arc,
-    vec::Vec,
-};
-use super::asm::*;
+use alloc::boxed::Box;
+use super::{spin_lock_yield, notify_spin_lock};
 
-
-type Channel<T> = Vec<AtomicPtr<T>>;
-
-/// Create a bounded channel
-///
-/// Returns `(tx, rx)` where one should be used one the local core,
-/// and the other is to be shared with another core.
-pub fn sync_channel<T>(bound: usize) -> (Sender<T>, Receiver<T>) {
-    // allow for bound=0
-    let len = bound + 1;
-    let mut channel = Vec::with_capacity(len);
-    for _ in 0..len {
-        channel.push(AtomicPtr::default());
-    }
-
-    let channel = Arc::new(channel);
-    let sender = Sender {
-        channel: channel.clone(),
-        pos: 0,
-    };
-    let receiver = Receiver {
-        channel: channel,
-        pos: 0,
-    };
-    (sender, receiver)
+pub struct Sender<'a, T> where T: Clone {
+    list: &'a [AtomicPtr<T>],
+    write: &'a AtomicUsize,
+    read: &'a AtomicUsize,
 }
 
-/// Sending half of a channel
-pub struct Sender<T> {
-    channel: Arc<Channel<T>>,
-    pos: usize,
+pub struct Receiver<'a, T> where T: Clone {
+    list: &'a [AtomicPtr<T>],
+    write: &'a AtomicUsize,
+    read: &'a AtomicUsize,
 }
 
-impl<T> Sender<T> {
-    /// Blocking send
-    pub fn send<B: Into<Box<T>>>(&mut self, content: B) {
-        let ptr = Box::into_raw(content.into());
-        let entry = &self.channel[self.pos];
-        // try to write the new pointer if the current pointer is
-        // NULL, retrying while it is not NULL
-        while entry.compare_and_swap(null_mut(), ptr, Ordering::Acquire) != null_mut() {
-            // power-saving
-            wfe();
-        }
-        dsb();
-        // wake power-saving receivers
-        sev();
-
-        // advance
-        self.pos += 1;
-        // wrap
-        if self.pos >= self.channel.len() {
-            self.pos = 0;
-        }
+impl<'a, T> Sender<'a, T> where T: Clone {
+    pub const fn new(list: &'static [AtomicPtr<T>], write: &'static AtomicUsize, read: &'static AtomicUsize) -> Self {
+        Sender {list, write, read}
     }
 
-    /// Non-blocking send, handing you back ownership of the content on **failure**
-    pub fn try_send<B: Into<Box<T>>>(&mut self, content: B) -> Option<Box<T>> {
-        let ptr = Box::into_raw(content.into());
-        let entry = &self.channel[self.pos];
-        // try to write the new pointer if the current pointer is
-        // NULL
-        if entry.compare_and_swap(null_mut(), ptr, Ordering::Acquire) == null_mut() {
-            dsb();
-            // wake power-saving receivers
-            sev();
-
-            // advance
-            self.pos += 1;
-            // wrap
-            if self.pos >= self.channel.len() {
-                self.pos = 0;
-            }
-
-            // success
-            None
+    pub fn try_send<B: Into<Box<T>>>(&mut self, content: B) -> Result<(), B> {
+        let write = self.write.load(Ordering::Relaxed);
+        if (write + 1) % self.list.len() == self.read.load(Ordering::Acquire) {
+            Err(content)
         } else {
-            let content = unsafe { Box::from_raw(ptr) };
-            // failure
-            Some(content)
+            let ptr = Box::into_raw(content.into());
+            let entry = &self.list[write];
+            let prev = entry.swap(ptr, Ordering::Relaxed);
+            // we allow other end get it first
+            self.write.store((write + 1) % self.list.len(), Ordering::Release);
+            notify_spin_lock();
+            if !prev.is_null() {
+                unsafe {
+                    drop_in_place(prev);
+                }
+            }
+            Ok(())
+        }
+    }
+
+    pub fn send<B: Into<Box<T>>>(&mut self, content: B) {
+        let mut content = content;
+        while let Err(back) = self.try_send(content) {
+            content = back;
+            spin_lock_yield();
         }
     }
 
     pub async fn async_send<B: Into<Box<T>>>(&mut self, content: B) {
-        struct Send<'a, T> {
-            sender: &'a mut Sender<T>,
-            content: Option<Box<T>>,
+        struct Send<'a, 'b, T> where T: Clone, 'b: 'a {
+            sender: &'a mut Sender<'b, T>,
+            content: Result<(), Box<T>>,
         }
 
-        impl<T> Future for Send<'_, T> {
+        impl<T> Future for Send<'_, '_, T> where T: Clone {
             type Output = ();
 
             fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
-                match self.content.take() {
-                    Some(content) => {
-                        if let Some(content) = self.sender.try_send(content) {
+                match core::mem::replace(&mut self.content, Ok(())) {
+                    Err(content) => {
+                        if let Err(content) = self.sender.try_send(content) {
                             // failure
-                            self.content = Some(content);
+                            self.content = Err(content);
                             cx.waker().wake_by_ref();
                             Poll::Pending
                         } else {
@@ -117,93 +75,80 @@ impl<T> Sender<T> {
                             Poll::Ready(())
                         }
                     }
-                    None => panic!("Send future polled after success"),
+                    Ok(_) => panic!("Send future polled after success"),
                 }
             }
         }
 
         Send {
             sender: self,
-            content: Some(content.into()),
+            content: Err(content.into()),
         }.await
     }
-}
 
-
-
-
-/// Receiving half of a channel
-pub struct Receiver<T> {
-    channel: Arc<Channel<T>>,
-    pos: usize,
-}
-
-impl<T> Receiver<T> {
-    /// Blocking receive
-    pub fn recv(&mut self) -> Box<T> {
-        let entry = &self.channel[self.pos];
-
-        loop {
-            dmb();
-            let ptr = entry.swap(null_mut(), Ordering::Release);
-            if ptr != null_mut() {
-                dsb();
-                // wake power-saving senders
-                sev();
-
-                let content = unsafe { Box::from_raw(ptr) };
-
-                // advance
-                self.pos += 1;
-                // wrap
-                if self.pos >= self.channel.len() {
-                    self.pos = 0;
-                }
-
-                return content;
+    /// free all items in the queue. It is the user's responsibility to
+    /// ensure no reader is trying to copy the data.
+    pub unsafe fn drop_elements(&mut self) {
+        for v in self.list.iter() {
+            let original = v.swap(core::ptr::null_mut(), Ordering::Relaxed);
+            if !original.is_null() {
+                drop_in_place(original);
             }
-
-            // power-saving
-            wfe();
         }
     }
 
-    /// Non-blocking receive
-    pub fn try_recv(&mut self) -> Option<Box<T>> {
-        let entry = &self.channel[self.pos];
+    /// Reset the `sync_channel`, *forget* all items in the queue. Affects both the sender and
+    /// receiver.
+    pub unsafe fn reset(&mut self) {
+        self.write.store(0, Ordering::Relaxed);
+        self.read.store(0, Ordering::Relaxed);
+        for v in self.list.iter() {
+            v.store(core::ptr::null_mut(), Ordering::Relaxed);
+        }
+    }
+}
 
-        dmb();
-        let ptr = entry.swap(null_mut(), Ordering::Release);
-        if ptr != null_mut() {
-            dsb();
-            // wake power-saving senders
-            sev();
+impl<'a, T> Receiver<'a, T> where T: Clone {
+    pub const fn new(list: &'static [AtomicPtr<T>], write: &'static AtomicUsize, read: &'static AtomicUsize) -> Self {
+        Receiver {list, write, read}
+    }
 
-            let content = unsafe { Box::from_raw(ptr) };
-
-            // advance
-            self.pos += 1;
-            // wrap
-            if self.pos >= self.channel.len() {
-                self.pos = 0;
-            }
-
-            Some(content)
+    pub fn try_recv(&mut self) -> Result<T, ()> {
+        let read = self.read.load(Ordering::Relaxed);
+        if read == self.write.load(Ordering::Acquire) {
+            Err(())
         } else {
-            None
+            let entry = &self.list[read];
+            let data = unsafe {
+                // we cannot deallocate the box
+                Box::leak(Box::from_raw(entry.load(Ordering::Relaxed)))
+            };
+            let result = data.clone();
+            self.read.store((read + 1) % self.list.len(), Ordering::Release);
+            notify_spin_lock();
+            Ok(result)
         }
     }
 
-    pub async fn async_recv(&mut self) -> Box<T> {
-        struct Recv<'a, T> {
-            receiver: &'a mut Receiver<T>,
+    pub fn recv(&mut self) -> T {
+        loop {
+            if let Ok(data) = self.try_recv() {
+                return data;
+            }
+            spin_lock_yield();
+        }
+    }
+
+    pub async fn async_recv(&mut self) -> T {
+        struct Recv<'a, 'b, T> where T: Clone, 'b: 'a {
+            receiver: &'a mut Receiver<'b, T>,
         }
 
-        impl<T> Future for Recv<'_, T> {
-            type Output = Box<T>;
+        impl<T> Future for Recv<'_, '_, T> where T: Clone {
+            type Output = T;
 
             fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
-                if let Some(content) = self.receiver.try_recv() {
+                if let Ok(content) = self.receiver.try_recv() {
                     Poll::Ready(content)
                 } else {
                     cx.waker().wake_by_ref();
@@ -218,10 +163,26 @@ impl<T> Receiver<T> {
     }
 }
 
-impl<T> Iterator for Receiver<T> {
-    type Item = Box<T>;
+impl<'a, T> Iterator for Receiver<'a, T> where T: Clone {
+    type Item = T;
 
     fn next(&mut self) -> Option<Self::Item> {
         Some(self.recv())
     }
 }
+
+#[macro_export]
+/// Macro for initializing the sync_channel with static buffer and indexes.
+/// Note that this requires `#![feature(const_in_array_repeat_expressions)]`
+macro_rules! sync_channel {
+    ($t: ty, $cap: expr) => {
+        {
+            use core::sync::atomic::{AtomicUsize, AtomicPtr};
+            use $crate::sync_channel::{Sender, Receiver};
+            static LIST: [AtomicPtr<$t>; $cap + 1] = [AtomicPtr::new(core::ptr::null_mut()); $cap + 1];
+            static WRITE: AtomicUsize = AtomicUsize::new(0);
+            static READ: AtomicUsize = AtomicUsize::new(0);
+            (Sender::new(&LIST, &WRITE, &READ), Receiver::new(&LIST, &WRITE, &READ))
+        }
+    };
+}

libsupport_zynq/Cargo.toml

@@ -9,8 +9,10 @@ edition = "2018"
 target_zc706 = ["libboard_zynq/target_zc706"]
 target_cora_z7_10 = ["libboard_zynq/target_cora_z7_10"]
 panic_handler = []
+dummy_irq_handler = []
+alloc_core = []
 
-default = ["panic_handler"]
+default = ["panic_handler", "dummy_irq_handler"]
 
 [dependencies]
 r0 = "1"

libsupport_zynq/src/abort.rs

@@ -53,6 +53,7 @@ pub unsafe extern "C" fn ReservedException() {
 #[link_section = ".text.boot"]
 #[no_mangle]
 #[naked]
+#[cfg(feature = "dummy_irq_handler")]
 pub unsafe extern "C" fn IRQ() {
     stdio::drop_uart();
     println!("IRQ");

libsupport_zynq/src/boot.rs

@@ -4,7 +4,7 @@ use libregister::{
     VolatileCell,
     RegisterR, RegisterW, RegisterRW,
 };
-use libcortex_a9::{asm, regs::*, cache, mmu};
+use libcortex_a9::{asm, regs::*, cache, mmu, spin_lock_yield, notify_spin_lock};
 use libboard_zynq::{slcr, mpcore};
 
 extern "C" {
@@ -29,7 +29,7 @@ pub unsafe extern "C" fn Reset() -> ! {
         }
         1 => {
             while !CORE1_ENABLED.get() {
-                asm::wfe();
+                spin_lock_yield();
             }
             SP.write(&mut __stack1_start as *mut _ as u32);
             boot_core1();
@@ -57,6 +57,7 @@ unsafe fn boot_core0() -> ! {
         asm::dmb();
         asm::dsb();
 
+        asm::enable_irq();
         main_core0();
         panic!("return from main");
     });
@@ -77,6 +78,7 @@ unsafe fn boot_core1() -> ! {
         asm::dmb();
         asm::dsb();
 
+        asm::enable_irq();
         main_core1();
         panic!("return from main_core1");
     });
@@ -142,6 +144,7 @@ impl Core1 {
             slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
             slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(false));
         });
+        notify_spin_lock();
 
         Core1 {}
     }
@@ -149,6 +152,8 @@ impl Core1 {
     pub fn disable(&self) {
         unsafe {
             CORE1_ENABLED.set(false);
+            cache::dccmvac(&CORE1_ENABLED  as *const _ as usize);
+            asm::dsb();
         }
         self.restart();
     }

libsupport_zynq/src/ram.rs

@@ -1,55 +1,97 @@
+use alloc::alloc::Layout;
 use core::alloc::GlobalAlloc;
 use core::ptr::NonNull;
-use alloc::alloc::Layout;
+use libcortex_a9::{
+    mutex::Mutex,
+    regs::MPIDR
+};
+use libregister::RegisterR;
 use linked_list_allocator::Heap;
-use libcortex_a9::mutex::Mutex;
+#[cfg(not(feature = "alloc_core"))]
 use libboard_zynq::ddr::DdrRam;
 
 #[global_allocator]
-static ALLOCATOR: CortexA9Alloc = CortexA9Alloc(Mutex::new(Heap::empty()));
+static ALLOCATOR: CortexA9Alloc = CortexA9Alloc(
+    Mutex::new(Heap::empty()),
+    Mutex::new(Heap::empty()),
+);
 
-/// LockedHeap doesn't lock properly
-struct CortexA9Alloc(Mutex<Heap>);
+struct CortexA9Alloc(Mutex<Heap>, Mutex<Heap>);
 
 unsafe impl Sync for CortexA9Alloc {}
 
 unsafe impl GlobalAlloc for CortexA9Alloc {
     unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
-        self.0.lock()
-            .allocate_first_fit(layout)
-            .ok()
-            .map_or(0 as *mut u8, |allocation| allocation.as_ptr())
+        if cfg!(not(feature = "alloc_core")) || MPIDR.read().cpu_id() == 0 {
+            &self.0
+        } else {
+            &self.1
+        }
+        .lock()
+        .allocate_first_fit(layout)
+        .ok()
+        .map_or(0 as *mut u8, |allocation| allocation.as_ptr())
     }
 
     unsafe fn dealloc(&self, ptr: *mut u8, layout: Layout) {
-        self.0.lock()
-            .deallocate(NonNull::new_unchecked(ptr), layout)
+        if cfg!(not(feature = "alloc_core")) || MPIDR.read().cpu_id() == 0 {
+            &self.0
+        } else {
+            &self.1
+        }
+        .lock()
+        .deallocate(NonNull::new_unchecked(ptr), layout)
     }
 }
 
+#[cfg(not(feature = "alloc_core"))]
 pub fn init_alloc_ddr(ddr: &mut DdrRam) {
     unsafe {
-        ALLOCATOR.0.lock()
+        ALLOCATOR
+            .0
+            .lock()
             .init(ddr.ptr::<u8>() as usize, ddr.size());
     }
 }
 
 extern "C" {
-    static __heap_start: usize;
-    static __heap_end: usize;
+    static __heap0_start: usize;
+    static __heap0_end: usize;
+    #[cfg(feature = "alloc_core")]
+    static __heap1_start: usize;
+    #[cfg(feature = "alloc_core")]
+    static __heap1_end: usize;
 }
 
-pub fn init_alloc_linker() {
+pub fn init_alloc_core0() {
     unsafe {
-        let start = &__heap_start as *const usize as usize;
-        let end = &__heap_end as *const usize as usize;
-        ALLOCATOR.0.lock()
-            .init(start, end - start);
+        let start = &__heap0_start as *const usize as usize;
+        let end = &__heap0_end as *const usize as usize;
+        ALLOCATOR.0.lock().init(start, end - start);
     }
 }
 
+#[cfg(feature = "alloc_core")]
+pub fn init_alloc_core1() {
+    unsafe {
+        let start = &__heap1_start as *const usize as usize;
+        let end = &__heap1_end as *const usize as usize;
+        ALLOCATOR.1.lock().init(start, end - start);
+    }
+}
 
 #[alloc_error_handler]
-fn alloc_error(_: core::alloc::Layout) -> ! {
-    panic!("alloc_error")
+fn alloc_error(layout: core::alloc::Layout) -> ! {
+    let id = MPIDR.read().cpu_id();
+    let used = if cfg!(not(feature = "alloc_core")) || id == 0 {
+        ALLOCATOR.0.lock().used()
+    } else {
+        ALLOCATOR.1.lock().used()
+    };
+    panic!(
+        "Core {} alloc_error, layout: {:?}, used memory: {}",
+        id,
+        layout,
+        used
+    );
 }

shell.nix

@@ -19,6 +19,6 @@ stdenv.mkDerivation {
   XARGO_RUST_SRC = "${rustcSrc}/src";
 
   shellHook = ''
-    echo "Run 'cargo xbuild --release' to build."
+    echo "Run 'cargo xbuild --release -p experiments' to build."
   '';
 }