use nix format for arm gnu toolchain

cfg: add dummy FIQ handler cfg
abort: gate dummy FIQ handler using cfg

experiments/Cargo.toml

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

experiments/src/main.rs

@@ -39,7 +39,7 @@ use libcortex_a9::{
 };
 use libregister::{RegisterR, RegisterW};
 use libsupport_zynq::{
-    boot, ram,
+    boot, exception_vectors, ram,
 };
 use log::{info, warn};
 use core::sync::atomic::{AtomicBool, Ordering};
@@ -56,19 +56,29 @@ extern "C" {
 static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
 
 interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
-    if MPIDR.read().cpu_id() == 1{
-        let mpcore = mpcore::RegisterBlock::mpcore();
-        let mut gic = gic::InterruptController::gic(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();
-        }
+    let mpcore = mpcore::RegisterBlock::mpcore();
+    let mut gic = gic::InterruptController::gic(mpcore);
+    let id = gic.get_interrupt_id();
+    match MPIDR.read().cpu_id(){
+        0 => {
+            if id.0 == 0 {
+                println!("Interrupting core0...");
+                gic.end_interrupt(id);
+                return;
+            }
+        },
+        1 => {
+            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");
@@ -86,6 +96,7 @@ pub fn restart_core1() {
 
 #[no_mangle]
 pub fn main_core0() {
+    exception_vectors::set_vector_table(0x0);
     // zynq::clocks::CpuClocks::enable_io(1_250_000_000);
     enable_l2_cache(0x8);
     println!("\nZynq experiments");
@@ -134,6 +145,10 @@ pub fn main_core0() {
     ddr.memtest();
     ram::init_alloc_ddr(&mut ddr);
 
+    info!("Send software interrupt to core0");
+    interrupt_controller.send_sgi(gic::InterruptId(0), gic::CPUCore::Core0.into());
+    info!("Core0 returned from interrupt");
+
     boot::Core1::start(false);
 
     let core1_req = unsafe { &mut CORE1_REQ.0 };

flake.lock

@@ -3,11 +3,11 @@
     "mozilla-overlay": {
       "flake": false,
       "locked": {
-        "lastModified": 1650459918,
-        "narHash": "sha256-sroCK+QJTmoXtcRkwZyKOP9iAYOPID2Bwdxn4GkG16w=",
+        "lastModified": 1695805681,
+        "narHash": "sha256-1ElPLD8eFfnuIk0G52HGGpRtQZ4QPCjChRlEOfkZ5ro=",
         "owner": "mozilla",
         "repo": "nixpkgs-mozilla",
-        "rev": "e1f7540fc0a8b989fb8cf701dc4fd7fc76bcf168",
+        "rev": "6eabade97bc28d707a8b9d82ad13ef143836736e",
         "type": "github"
       },
       "original": {
@@ -18,16 +18,16 @@
     },
     "nixpkgs": {
       "locked": {
-        "lastModified": 1653920503,
-        "narHash": "sha256-BBeCZwZImtjP3oYy4WogkQYy5OxNyfNciVSc1AfZgLQ=",
+        "lastModified": 1701389149,
+        "narHash": "sha256-rU1suTIEd5DGCaAXKW6yHoCfR1mnYjOXQFOaH7M23js=",
         "owner": "NixOS",
         "repo": "nixpkgs",
-        "rev": "a634c8f6c1fbf9b9730e01764999666f3436f10a",
+        "rev": "5de0b32be6e85dc1a9404c75131316e4ffbc634c",
         "type": "github"
       },
       "original": {
         "owner": "NixOS",
-        "ref": "nixos-22.05",
+        "ref": "nixos-23.11",
         "repo": "nixpkgs",
         "type": "github"
       }

flake.nix

@@ -1,12 +1,12 @@
 {
   description = "Bare-metal Rust on Zynq-7000";
 
-  inputs.nixpkgs.url = github:NixOS/nixpkgs/nixos-22.05;
+  inputs.nixpkgs.url = github:NixOS/nixpkgs/nixos-23.11;
   inputs.mozilla-overlay = { url = github:mozilla/nixpkgs-mozilla; flake = false; };
 
   outputs = { self, nixpkgs, mozilla-overlay }:
     let
-      pkgs = import nixpkgs { system = "x86_64-linux"; overlays = [ (import mozilla-overlay) ]; };
+      pkgs = import nixpkgs { system = "x86_64-linux"; overlays = [ (import mozilla-overlay) crosspkgs-overlay ]; };
       
       rustManifest = pkgs.fetchurl {
         url = "https://static.rust-lang.org/dist/2021-01-29/channel-rust-nightly.toml";
@@ -26,148 +26,29 @@
         cargo = rust;
       });
 
-      gnu-platform = "arm-none-eabi";
-
-      binutils-pkg = { zlib, extraConfigureFlags ? [] }: pkgs.stdenv.mkDerivation rec {
-        basename = "binutils";
-        version = "2.30";
-        name = "${basename}-${gnu-platform}-${version}";
-        src = pkgs.fetchurl {
-          url = "https://ftp.gnu.org/gnu/binutils/binutils-${version}.tar.bz2";
-          sha256 = "028cklfqaab24glva1ks2aqa1zxa6w6xmc8q34zs1sb7h22dxspg";
+      crosspkgs-overlay = (self: super: {
+        pkgsCross = super.pkgsCross // {
+          zynq-baremetal = import super.path {
+            system = "x86_64-linux";
+            crossSystem = {
+              config = "arm-none-eabihf";
+              libc = "newlib";
+              gcc.cpu = "cortex-a9";
+              gcc.fpu = "vfpv3";
+            };
+          };
         };
-        configureFlags = [
-          "--enable-deterministic-archives"
-          "--target=${gnu-platform}"
-          "--with-cpu=cortex-a9"
-          "--with-fpu=vfpv3"
-          "--with-float=hard"
-          "--with-mode=thumb"
-        ] ++ extraConfigureFlags;
-        outputs = [ "out" "info" "man" ];
-        depsBuildBuild = [ pkgs.buildPackages.stdenv.cc ];
-        buildInputs = [ zlib ];
-        enableParallelBuilding = true;
-        meta = {
-          description = "Tools for manipulating binaries (linker, assembler, etc.)";
-          longDescription = ''
-            The GNU Binutils are a collection of binary tools.  The main
-            ones are `ld' (the GNU linker) and `as' (the GNU assembler).
-            They also include the BFD (Binary File Descriptor) library,
-            `gprof', `nm', `strip', etc.
-          '';
-          homepage = http://www.gnu.org/software/binutils/;
-          license = pkgs.lib.licenses.gpl3Plus;
-          /* Give binutils a lower priority than gcc-wrapper to prevent a
-            collision due to the ld/as wrappers/symlinks in the latter. */
-          priority = "10";
-        };
-      };
-
-      gcc-pkg = { gmp, mpfr, libmpc, platform-binutils, extraConfigureFlags ? [] }: pkgs.stdenv.mkDerivation rec {
-        basename = "gcc";
-        version = "9.1.0";
-        name = "${basename}-${gnu-platform}-${version}";
-        src = pkgs.fetchurl {
-          url = "https://ftp.gnu.org/gnu/gcc/gcc-${version}/gcc-${version}.tar.xz";
-          sha256 = "1817nc2bqdc251k0lpc51cimna7v68xjrnvqzvc50q3ax4s6i9kr";
-        };
-        preConfigure = ''
-          mkdir build
-          cd build
-        '';
-        configureScript = "../configure";
-        configureFlags = [ 
-          "--target=${gnu-platform}"
-          "--with-arch=armv7-a"
-          "--with-tune=cortex-a9"
-          "--with-fpu=vfpv3"
-          "--with-float=hard"
-          "--disable-libssp"
-          "--enable-languages=c"
-          "--with-as=${platform-binutils}/bin/${gnu-platform}-as"
-          "--with-ld=${platform-binutils}/bin/${gnu-platform}-ld" ] ++ extraConfigureFlags;
-        outputs = [ "out" "info" "man" ];
-        hardeningDisable = [ "format" "pie" ];
-        propagatedBuildInputs = [ gmp mpfr libmpc platform-binutils ];
-        enableParallelBuilding = true;
-        dontFixup = true;
-      };
-
-      newlib-pkg = { platform-binutils, platform-gcc }: pkgs.stdenv.mkDerivation rec {
-        pname = "newlib";
-        version = "3.1.0";
-        src = pkgs.fetchurl {
-          url = "ftp://sourceware.org/pub/newlib/newlib-${version}.tar.gz";
-          sha256 = "0ahh3n079zjp7d9wynggwrnrs27440aac04340chf1p9476a2kzv";
-        };
-        nativeBuildInputs = [ platform-binutils platform-gcc ];
-        configureFlags = [
-          "--target=${gnu-platform}"
-
-          "--with-cpu=cortex-a9"
-          "--with-fpu=vfpv3"
-          "--with-float=hard"
-          "--with-mode=thumb"
-          "--enable-interwork"
-          "--disable-multilib"
-
-          "--disable-newlib-supplied-syscalls"
-          "--with-gnu-ld"
-          "--with-gnu-as"
-          "--disable-newlib-io-float"
-          "--disable-werror"
-        ];
-        dontFixup = true;
-      };
-      gnutoolchain = rec {
-        binutils-bootstrap = pkgs.callPackage binutils-pkg { };
-        gcc-bootstrap = pkgs.callPackage gcc-pkg {
-          platform-binutils = binutils-bootstrap;
-          extraConfigureFlags = [ "--disable-libgcc" ];
-        };
-        newlib = pkgs.callPackage newlib-pkg {
-          platform-binutils = binutils-bootstrap;
-          platform-gcc = gcc-bootstrap;
-        };
-        binutils = pkgs.callPackage binutils-pkg {
-          extraConfigureFlags = [ "--with-lib-path=${newlib}/arm-none-eabi/lib" ];
-        };
-        gcc = pkgs.callPackage gcc-pkg {
-          platform-binutils = binutils;
-          extraConfigureFlags = [ "--enable-newlib" "--with-headers=${newlib}/arm-none-eabi/include" ];
-        };
-      };
-
-      cargo-xbuild = rustPlatform.buildRustPackage rec {
-        pname = "cargo-xbuild";
-        version = "0.6.5";
-
-        src = pkgs.fetchFromGitHub {
-          owner = "rust-osdev";
-          repo = pname;
-          rev = "v${version}";
-          sha256 = "18djvygq9v8rmfchvi2hfj0i6fhn36m716vqndqnj56fiqviwxvf";
-        };
-        cargoSha256 = "13sj9j9kl6js75h9xq0yidxy63vixxm9q3f8jil6ymarml5wkhx8";
-
-        meta = with pkgs.lib; {
-          description = "Automatically cross-compiles the sysroot crates core, compiler_builtins, and alloc";
-          homepage = "https://github.com/rust-osdev/cargo-xbuild";
-          license = with licenses; [ mit asl20 ];
-          maintainers = with maintainers; [ johntitor xrelkd ];
-        };
-      };
+      });
 
       mkbootimage = pkgs.stdenv.mkDerivation {
         pname = "mkbootimage";
-        version = "2.2";
+        version = "2.3dev";
 
         src = pkgs.fetchFromGitHub {
           owner = "antmicro";
           repo = "zynq-mkbootimage";
-          rev = "4ee42d782a9ba65725ed165a4916853224a8edf7";
-          sha256 = "1k1mbsngqadqihzjgvwvsrkvryxy5ladpxd9yh9iqn2s7fxqwqa9";
+          rev = "872363ce32c249f8278cf107bc6d3bdeb38d849f";
+          sha256 = "sha256-5FPyAhUWZDwHbqmp9J2ZXTmjaXPz+dzrJMolaNwADHs=";
         };
 
         propagatedBuildInputs = [ pkgs.libelf pkgs.pcre ];
@@ -180,6 +61,7 @@
           mkdir -p $out/bin
           cp mkbootimage $out/bin
         '';
+        hardeningDisable = [ "fortify" ];
       };
 
       fsbl = { board ? "zc706" }: pkgs.stdenv.mkDerivation {
@@ -187,19 +69,20 @@
         src = pkgs.fetchFromGitHub {
           owner = "Xilinx";
           repo = "embeddedsw";
-          rev = "65c849ed46c88c67457e1fc742744f96db968ff1";
-          sha256 = "1rvl06ha40dzd6s9aa4sylmksh4xb9dqaxq462lffv1fdk342pda";
+          rev = "xilinx_v2022.2";
+          sha256 = "sha256-UDz9KK/Hw3qM1BAeKif30rE8Bi6C2uvuZlvyvtJCMfw=";
         };
-        patches = [ ./fsbl.patch ];
         nativeBuildInputs = [
-          pkgs.gnumake
-          gnutoolchain.binutils
-          gnutoolchain.gcc
+          pkgs.pkgsCross.zynq-baremetal.buildPackages.binutils
+          pkgs.pkgsCross.zynq-baremetal.buildPackages.gcc
         ];
         patchPhase = ''
-          patch -p1 -i ${./fsbl.patch}
           patchShebangs lib/sw_apps/zynq_fsbl/misc/copy_bsp.sh
-          echo 'SEARCH_DIR("${gnutoolchain.newlib}/arm-none-eabi/lib");' >> lib/sw_apps/zynq_fsbl/src/lscript.ld
+
+          for x in lib/sw_apps/zynq_fsbl/src/Makefile lib/sw_apps/zynq_fsbl/misc/copy_bsp.sh lib/bsp/standalone/src/arm/cortexa9/gcc/Makefile; do
+            substituteInPlace $x \
+              --replace "arm-none-eabi-" "arm-none-eabihf-"
+          done
         '';
         buildPhase = ''
           cd lib/sw_apps/zynq_fsbl/src
@@ -213,6 +96,10 @@
         dontFixup = true;
       };
 
+      cargo-xbuild = pkgs.cargo-xbuild.overrideAttrs(oa: {
+        postPatch = "substituteInPlace src/sysroot.rs --replace 2021 2018";
+      });
+
       build-crate = name: crate: features: rustPlatform.buildRustPackage rec {
         name = "${crate}";
 
@@ -221,9 +108,9 @@
         ) ./.;
         cargoLock = { lockFile = ./Cargo.lock; };
 
-        nativeBuildInputs = [ cargo-xbuild pkgs.llvmPackages_9.clang-unwrapped ];
+        nativeBuildInputs = [ cargo-xbuild pkgs.llvmPackages_14.clang-unwrapped ];
         buildPhase = ''
-          export XARGO_RUST_SRC="${rustPlatform.rust.rustc}/lib/rustlib/src/rust/library"
+          export XARGO_RUST_SRC="${rust}/lib/rustlib/src/rust/library"
           export CARGO_HOME=$(mktemp -d cargo-home.XXX)
           pushd ${crate}
           cargo xbuild --release --frozen \
@@ -240,6 +127,7 @@
 
         doCheck = false;
         dontFixup = true;
+        auditable = false;
       };
 
       targetCrates = target: {
@@ -265,21 +153,20 @@
 
       hydraJobs = packages.x86_64-linux;
 
-      inherit rustPlatform;
+      inherit rust rustPlatform;
 
       devShell.x86_64-linux = pkgs.mkShell {
         name = "zynq-rs-dev-shell";
-        buildInputs = with pkgs; [
-          rustPlatform.rust.rustc
-          rustPlatform.rust.cargo
-          cacert
+        buildInputs = [
+          rust
           cargo-xbuild
+          mkbootimage
 
-          openocd gdb
-          openssh rsync
-          llvmPackages_9.clang-unwrapped
-          (python3.withPackages(ps: [ ps.pyftdi ]))
-          mkbootimage ];
-        };
+          pkgs.openocd pkgs.gdb
+          pkgs.openssh pkgs.rsync
+          pkgs.llvmPackages_14.clang-unwrapped
+          (pkgs.python3.withPackages(ps: [ ps.pyftdi ]))
+        ];
+      };
     };
-}
+}

fsbl.patch

@@ -1,31 +0,0 @@
-diff --git a/lib/sw_apps/zynq_fsbl/src/Makefile b/lib/sw_apps/zynq_fsbl/src/Makefile
-index 0e3ccdf1c5..a5b02f386e 100644
---- a/lib/sw_apps/zynq_fsbl/src/Makefile
-+++ b/lib/sw_apps/zynq_fsbl/src/Makefile
-@@ -71,11 +71,14 @@ endif
- all: $(EXEC)
- 
- $(EXEC): $(LIBS) $(OBJS) $(INCLUDES)
--	cp $(BSP_DIR)/$(BOARD)/ps7_init.* .
- 	$(LINKER) $(LD1FLAGS)  -o $@ $(OBJS) $(LDFLAGS)
- 	rm -rf $(OBJS)
--	
--	
-+
-+.PHONY: ps7_init
-+
-+ps7_init:
-+	cp $(BSP_DIR)/$(BOARD)/ps7_init.* .
-+
- $(LIBS): 
- 	echo "Copying BSP files"
- 	$(BSP_DIR)/copy_bsp.sh $(BOARD) $(CC)
-@@ -86,7 +89,7 @@ $(LIBS):
-         make -C $(BSP_DIR) -k all "CC=armcc" "AR=armar" "C_FLAGS= -O2 -c" "EC_FLAGS=--debug --wchar32"; \
- 	fi;
- 
--%.o:%.c
-+%.o:%.c ps7_init
- 	$(CC) $(CC_FLAGS) $(CFLAGS) $(ECFLAGS) -c $< -o $@ $(INCLUDEPATH)
- 
- %.o:%.S

libboard_zynq/src/error_led.rs

@@ -18,8 +18,6 @@ impl ErrorLED {
                     .pullup(true)
                     .disable_rcvr(true)
             );
-            // reset
-            slcr.gpio_rst_ctrl.reset_gpio();
         });
 
         Self::error_led_common(0xFFFF - 0x0080)

libboard_zynq/src/eth/mod.rs

@@ -13,6 +13,9 @@ mod regs;
 pub mod rx;
 pub mod tx;
 
+use super::time::Milliseconds;
+use embedded_hal::timer::CountDown;
+
 /// Size of all the buffers
 pub const MTU: usize = 1536;
 /// Maximum MDC clock
@@ -300,11 +303,18 @@ impl<GEM: Gem> Eth<GEM, (), ()> {
     fn gem_common(macaddr: [u8; 6]) -> Self {
         GEM::setup_clock(TX_1000);
 
+        #[cfg(feature="target_kasli_soc")]
+        {
+            let mut eth_reset_pin = PhyRst::rst_pin();
+            eth_reset_pin.reset();
+        }
+
         let mut inner = EthInner {
             gem: PhantomData,
             link: None,
         };
         inner.init();
+
         inner.configure(macaddr);
 
         let phy = Phy::find(&mut inner).expect("phy");
@@ -482,6 +492,69 @@ impl<'a, GEM: Gem> smoltcp::phy::Device<'a> for &mut Eth<GEM, rx::DescList, tx::
     }
 }
 
+pub struct PhyRst {
+    regs: regs::GpioRegisterBlock,
+    count_down: super::timer::global::CountDown<Milliseconds>,
+}
+
+impl PhyRst {
+    pub fn rst_pin() -> Self {
+        slcr::RegisterBlock::unlocked(|slcr| {
+            // Hardware Reset for PHY
+            slcr.mio_pin_47.write(
+                slcr::MioPin47::zeroed()
+                    .l3_sel(0b000)
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+                    .disable_rcvr(true)
+            );
+        });
+        Self::eth_reset_common(0xFFFF - 0x8000)
+    }
+    
+    fn delay_ms(&mut self, ms: u64) {
+        self.count_down.start(Milliseconds(ms));
+        nb::block!(self.count_down.wait()).unwrap();
+    }
+
+    fn eth_reset_common(gpio_output_mask: u16) -> Self {
+        let self_ = Self {
+            regs: regs::GpioRegisterBlock::regs(),
+            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown(),
+        };
+
+        // Setup GPIO output mask
+        self_.regs.gpio_output_mask.modify(|_, w| {
+            w.mask(gpio_output_mask)
+        });
+
+        self_.regs.gpio_direction.modify(|_, w| {
+            w.phy_rst(true)
+        });
+        
+        self_
+    }
+
+    fn oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+            w.phy_rst(oe)
+        })
+    }
+
+    fn toggle(&mut self, o: bool) {
+        self.regs.gpio_output_mask.modify(|_, w| {
+            w.phy_rst(o)
+        })
+    }
+
+    pub fn reset(&mut self) {
+        self.toggle(false); // drive phy_rst (active LOW) pin low
+        self.oe(true);         // enable pin's output
+        self.delay_ms(10);
+        self.toggle(true);
+    }
+}
+
 
 struct EthInner<GEM: Gem> {
     gem: PhantomData<GEM>,

libboard_zynq/src/eth/regs.rs

@@ -110,6 +110,49 @@ pub struct RegisterBlock {
     pub design_cfg5: RO<u32>,
 }
 
+pub struct GpioRegisterBlock {
+    pub gpio_output_mask: &'static mut OutputMask,
+    pub gpio_direction: &'static mut Direction,
+    pub gpio_output_enable: &'static mut OutputEnable,
+}
+
+impl GpioRegisterBlock {
+    pub fn regs() -> Self {
+        Self {
+            gpio_output_mask: OutputMask::new(),
+            gpio_direction: Direction::new(),
+            gpio_output_enable: OutputEnable::new(),
+        }
+    }
+}
+
+register!(gpio_output_mask, 
+            /// MASK_DATA_1_SW:
+            /// Maskable output data for MIO[47:32]
+            OutputMask, RW, u32);
+register_at!(OutputMask, 0xE000A008, new);
+register_bit!(gpio_output_mask, 
+            /// Output for PHY_RST (MIO[47])
+            phy_rst, 15);
+register_bits!(gpio_output_mask,
+            mask, u16, 16, 31);
+register!(gpio_direction, 
+            /// DIRM_1:
+            /// Direction mode for MIO[53:32]; 0/1 = in/out
+            Direction, RW, u32);
+register_at!(Direction, 0xE000A244, new);
+register_bit!(gpio_direction,
+            /// Direction for PHY_RST
+            phy_rst, 15);
+register!(gpio_output_enable,
+            /// OEN_1:
+            /// Output enable for MIO[53:32]
+            OutputEnable, RW, u32);
+register_at!(OutputEnable, 0xE000A248, new);
+register_bit!(gpio_output_enable,
+            /// Output enable for PHY_RST
+            phy_rst, 15);
+    
 register_at!(RegisterBlock, 0xE000B000, gem0);
 register_at!(RegisterBlock, 0xE000C000, gem1);
 

libboard_zynq/src/gic.rs

@@ -115,7 +115,7 @@ impl InterruptController {
         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));
+            self.mpcore.icdiptr[m].modify(|mut icdiptr| *icdiptr.set_bits(n..=n+1, target_cpu as u32));
         }
 
         // sensitivity

libboard_zynq/src/i2c/mod.rs

@@ -53,8 +53,6 @@ impl I2c {
                     .pullup(false)
                     .disable_rcvr(true)
             );
-            // Reset
-            slcr.gpio_rst_ctrl.reset_gpio();
         });
 
         Self::i2c_common(0xFFFF - 0x000C, 0xFFFF - 0x0002)

libcortex_a9/src/asm.rs

@@ -34,6 +34,12 @@ pub fn isb() {
     unsafe { llvm_asm!("isb" :::: "volatile") }
 }
 
+/// Enable FIQ
+#[inline]
+pub unsafe fn enable_fiq() {
+    llvm_asm!("cpsie f":::: "volatile");
+}
+
 /// Enable IRQ
 #[inline]
 pub unsafe fn enable_irq() {

libcortex_a9/src/lib.rs

@@ -36,7 +36,9 @@ pub fn notify_spin_lock() {
 }
 
 #[macro_export]
-/// Interrupt handler, which setup the stack and jump to actual interrupt handler.
+/// Interrupt handler, which setup the stack and preserve registers before jumping to actual interrupt handler.
+/// Registers r0-r12, PC, SP and CPSR are restored after the actual handler.
+/// 
 /// - `name` is the name of the interrupt, should be the same as the one defined in vector table.
 /// - `name2` is the name for the actual handler, should be different from name.
 /// - `stack0` is the stack for the interrupt handler when called from core0.
@@ -44,8 +46,7 @@ pub fn notify_spin_lock() {
 /// - `body` is the body of the actual interrupt handler, should be a normal unsafe rust function
 /// body.
 ///
-/// Note that the interrupt handler would use the same stack as normal programs by default, so
-/// interrupt handlers should not return to normal program or it may corrupt the stack.
+/// Note that the interrupt handler would use the same stack as normal programs by default.
 macro_rules! interrupt_handler {
     ($name:ident, $name2:ident, $stack0:ident, $stack1:ident, $body:block) => {
         #[link_section = ".text.boot"]
@@ -54,19 +55,27 @@ macro_rules! interrupt_handler {
         pub unsafe extern "C" fn $name() -> ! {
             asm!(
                 // setup SP, depending on CPU 0 or 1
+                // and preserve registers
+                "sub lr, lr, #4",
+                "stmfd sp!, {{r0-r12, lr}}",
                 "mrc p15, #0, r0, c0, c0, #5",
                 concat!("movw r1, :lower16:", stringify!($stack0)),
                 concat!("movt r1, :upper16:", stringify!($stack0)),
                 "tst r0, #3",
                 concat!("movwne r1, :lower16:", stringify!($stack1)),
                 concat!("movtne r1, :upper16:", stringify!($stack1)),
+                "mov r0, sp",
                 "mov sp, r1",
+                "push {{r0, r1}}",                                      // 2 registers are pushed to maintain 8 byte stack alignment 
                 concat!("bl ", stringify!($name2)),
+                "pop {{r0, r1}}",
+                "mov sp, r0",
+                "ldmfd sp!, {{r0-r12, pc}}^",                           // caret ^ : copy SPSR to the CPSR  
                 options(noreturn)
             );
         }
 
         #[no_mangle]
-        pub unsafe extern "C" fn $name2() -> ! $body
+        pub unsafe extern "C" fn $name2() $body
     };
 }

libsupport_zynq/Cargo.toml

@@ -12,9 +12,10 @@ target_redpitaya = ["libboard_zynq/target_redpitaya"]
 target_kasli_soc = ["libboard_zynq/target_kasli_soc"]
 panic_handler = []
 dummy_irq_handler = []
+dummy_fiq_handler = []
 alloc_core = []
 
-default = ["panic_handler", "dummy_irq_handler"]
+default = ["panic_handler", "dummy_irq_handler", "dummy_fiq_handler"]
 
 [dependencies]
 r0 = "1"

libsupport_zynq/src/boot.rs

@@ -54,6 +54,7 @@ unsafe extern "C" fn boot_core0() -> ! {
         asm::dmb();
         asm::dsb();
 
+        asm::enable_fiq();
         asm::enable_irq();
         main_core0();
         panic!("return from main");
@@ -75,6 +76,7 @@ unsafe extern "C" fn boot_core1() -> ! {
         asm::dmb();
         asm::dsb();
 
+        asm::enable_fiq();
         asm::enable_irq();
         main_core1();
         panic!("return from main_core1");

libsupport_zynq/src/exception_vectors.rs

@@ -1,6 +1,10 @@
-use libregister::RegisterR;
-use libcortex_a9::{regs::{DFSR, MPIDR}, interrupt_handler};
-use libboard_zynq::{println, stdio};
+use libregister::{RegisterR, RegisterW};
+use libcortex_a9::{regs::{DFSR, MPIDR, VBAR}, interrupt_handler};
+use libboard_zynq::{println, stdio}; 
+
+pub fn set_vector_table(base_addr: u32){
+    VBAR.write(base_addr);
+}
 
 interrupt_handler!(UndefinedInstruction, undefined_instruction, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
@@ -42,6 +46,7 @@ interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
     loop {}
 });
 
+#[cfg(feature = "dummy_fiq_handler")]
 interrupt_handler!(FIQ, fiq, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("FIQ");

libsupport_zynq/src/lib.rs

@@ -9,7 +9,7 @@ pub extern crate alloc;
 pub extern crate compiler_builtins;
 
 pub mod boot;
-mod abort;
+pub mod exception_vectors;
 #[cfg(feature = "panic_handler")]
 mod panic;
 pub mod ram;