update dependencies

flake output hashes
up cargo dependencies
2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00
123 changed files with 16658 additions and 2823 deletions
--- a/.cargo/config
+++ b/.cargo/config
@ -1,6 +1,4 @@
 [target.armv7-none-eabihf]
 runner = "./runner.sh"
 linker = "arm-none-eabihf-gcc"
 rustflags = [
  "-C", "link-arg=-Tlink.x",
  "-C", "target-feature=a9,armv7-a,neon",
--- a/.gitignore
+++ b/.gitignore
@ -0,0 +1,2 @@
 /target
 result*
--- a/Cargo.lock
+++ b/Cargo.lock
@ -1,76 +1,240 @@
 # This file is automatically @generated by Cargo.
 # It is not intended for manual editing.
 version = 3
 [[package]]
 name = "bit_field"
-version = "0.10.0"
+version = "0.10.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "dcb6dd1c2376d2e096796e234a70e17e94cc2d5d54ff8ce42b28cef1d0d359a4"
 [[package]]
 name = "bitflags"
-version = "1.1.0"
+version = "1.3.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
 [[package]]
 name = "byteorder"
-version = "1.3.2"
+version = "1.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "60f0b0d4c0a382d2734228fd12b5a6b5dac185c60e938026fd31b265b94f9bd2"
 [[package]]
 name = "cc"
 version = "1.0.73"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "2fff2a6927b3bb87f9595d67196a70493f627687a71d87a0d692242c33f58c11"
 [[package]]
 name = "cfg-if"
 version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
 [[package]]
 name = "compiler_builtins"
-version = "0.1.19"
+version = "0.1.49"
-source = "git+https://github.com/rust-lang-nursery/compiler-builtins#36da64f20e96206ac279f700586817c8abe3bdf8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "20b1438ef42c655665a8ab2c1c6d605a305f031d38d9be689ddfef41a20f3aa2"
 [[package]]
 name = "core_io"
 version = "0.1.0"
 source = "git+https://git.m-labs.hk/M-Labs/rs-core_io.git?rev=e9d3edf027#e9d3edf0272502b0dd6c26e8a4869c2912657615"
 [[package]]
 name = "embedded-hal"
 version = "0.2.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff"
 dependencies = [
 "nb 0.1.3",
 "void",
 ]
 [[package]]
 name = "experiments"
 version = "0.0.0"
 dependencies = [
 "embedded-hal",
 "libasync",
 "libboard_zynq",
 "libcortex_a9",
 "libregister",
 "libsupport_zynq",
 "log",
 ]
 [[package]]
 name = "fatfs"
 version = "0.3.6"
 source = "git+https://git.m-labs.hk/M-Labs/rust-fatfs.git?rev=4b5e420084#4b5e420084fd1c4a9c105680b687523909b6469c"
 dependencies = [
 "bitflags",
 "byteorder",
 "core_io",
 "log",
 ]
 [[package]]
 name = "libasync"
 version = "0.0.0"
 dependencies = [
 "embedded-hal",
 "libcortex_a9",
 "nb 1.0.0",
 "pin-utils",
 "smoltcp",
 ]
 [[package]]
 name = "libboard_zynq"
 version = "0.0.0"
 dependencies = [
 "bit_field",
 "embedded-hal",
 "libasync",
 "libcortex_a9",
 "libregister",
 "log",
 "nb 0.1.3",
 "smoltcp",
 "void",
 "volatile-register",
 ]
 [[package]]
 name = "libconfig"
 version = "0.1.0"
 dependencies = [
 "core_io",
 "fatfs",
 "libboard_zynq",
 "log",
 ]
 [[package]]
 name = "libcortex_a9"
 version = "0.0.0"
 dependencies = [
 "bit_field",
 "libregister",
 "volatile-register",
 ]
 [[package]]
 name = "libregister"
 version = "0.0.0"
 dependencies = [
 "bit_field",
 "vcell",
 "volatile-register",
 ]
 [[package]]
 name = "libsupport_zynq"
 version = "0.0.0"
 dependencies = [
 "cc",
 "compiler_builtins",
 "libboard_zynq",
 "libcortex_a9",
 "libregister",
 "linked_list_allocator",
 "r0",
 ]
 [[package]]
 name = "linked_list_allocator"
 version = "0.8.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "822add9edb1860698b79522510da17bef885171f75aa395cff099d770c609c24"
 [[package]]
 name = "log"
 version = "0.4.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "51b9bbe6c47d51fc3e1a9b945965946b4c44142ab8792c50835a980d362c2710"
 dependencies = [
 "cfg-if",
 ]
 [[package]]
 name = "managed"
-version = "0.7.1"
+version = "0.7.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c75de51135344a4f8ed3cfe2720dc27736f7711989703a0b43aadf3753c55577"
 [[package]]
 name = "nb"
 version = "0.1.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f"
 dependencies = [
 "nb 1.0.0",
 ]
 [[package]]
 name = "nb"
 version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae"
 [[package]]
 name = "pin-utils"
 version = "0.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
 [[package]]
 name = "r0"
-version = "0.2.2"
+version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "bd7a31eed1591dcbc95d92ad7161908e72f4677f8fabf2a32ca49b4237cbf211"
 [[package]]
 name = "smoltcp"
-version = "0.5.0"
+version = "0.7.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3e4a069bef843d170df47e7c0a8bf8d037f217d9f5b325865acc3e466ffe40d3"
 dependencies = [
- "bitflags 1.1.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "bitflags",
- "byteorder 1.3.2 (registry+https://github.com/rust-lang/crates.io-index)",
+ "byteorder",
- "managed 0.7.1 (registry+https://github.com/rust-lang/crates.io-index)",
+ "managed",
 ]
 [[package]]
 name = "szl"
 version = "0.1.0"
 dependencies = [
 "byteorder",
 "core_io",
 "libboard_zynq",
 "libconfig",
 "libcortex_a9",
 "libregister",
 "libsupport_zynq",
 "log",
 ]
 [[package]]
 name = "vcell"
-version = "0.1.0"
+version = "0.1.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002"
 [[package]]
 name = "void"
 version = "1.0.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d"
 [[package]]
 name = "volatile-register"
-version = "0.2.0"
+version = "0.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "9ee8f19f9d74293faf70901bc20ad067dc1ad390d2cbf1e3f75f721ffee908b6"
 dependencies = [
- "vcell 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "vcell",
 ]
 [[package]]
 name = "zc706"
 version = "0.0.0"
 dependencies = [
 "bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)",
 "compiler_builtins 0.1.19 (git+https://github.com/rust-lang-nursery/compiler-builtins)",
 "r0 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)",
 "smoltcp 0.5.0 (registry+https://github.com/rust-lang/crates.io-index)",
 "volatile-register 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)",
 ]
 [metadata]
 "checksum bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)" = "a165d606cf084741d4ac3a28fb6e9b1eb0bd31f6cd999098cfddb0b2ab381dc0"
 "checksum bitflags 1.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "3d155346769a6855b86399e9bc3814ab343cd3d62c7e985113d46a0ec3c281fd"
 "checksum byteorder 1.3.2 (registry+https://github.com/rust-lang/crates.io-index)" = "a7c3dd8985a7111efc5c80b44e23ecdd8c007de8ade3b96595387e812b957cf5"
 "checksum compiler_builtins 0.1.19 (git+https://github.com/rust-lang-nursery/compiler-builtins)" = "<none>"
 "checksum managed 0.7.1 (registry+https://github.com/rust-lang/crates.io-index)" = "fdcec5e97041c7f0f1c5b7d93f12e57293c831c646f4cc7a5db59460c7ea8de6"
 "checksum r0 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)" = "e2a38df5b15c8d5c7e8654189744d8e396bddc18ad48041a500ce52d6948941f"
 "checksum smoltcp 0.5.0 (registry+https://github.com/rust-lang/crates.io-index)" = "fef582369edb298c6c41319a544ca9c4e83622f226055ccfcb35974fbb55ed34"
 "checksum vcell 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "45c297f0afb6928cd08ab1ff9d95e99392595ea25ae1b5ecf822ff8764e57a0d"
 "checksum volatile-register 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "0d67cb4616d99b940db1d6bd28844ff97108b498a6ca850e5b6191a532063286"
--- a/Cargo.toml
+++ b/Cargo.toml
@ -1,25 +1,20 @@
-[package]
+[workspace]
-name = "zc706"
+members = [
-version = "0.0.0"
+  "libregister",
-authors = ["Astro <astro@spaceboyz.net>"]
+  "libcortex_a9",
-edition = "2018"
+  "libboard_zynq",
-
+  "libsupport_zynq",
-[profile.dev]
+  "libasync",
-panic = "abort"
+  "libconfig",
-lto = false
+  "experiments",
  "szl",
 ]
 [profile.release]
 panic = "abort"
 debug = true
-
+codegen-units = 1
-[features]
+opt-level = 's'
-target_zc706 = []
+lto = true
-target_cora_z7_10 = []
+debug-assertions = false
-default = ["target_zc706"]
+overflow-checks = false
 [dependencies]
 r0 = "0.2"
 volatile-register = "0.2"
 bit_field = "0.10"
 compiler_builtins = { git = "https://github.com/rust-lang-nursery/compiler-builtins", no-default-features = true, features = ["mem", "no-lang-items"]}
 smoltcp = { version = "0.5", default-features = false, features = ["proto-ipv4", "socket-tcp"] }
--- a/165
+++ b/165
@ -0,0 +1,165 @@
                   GNU LESSER GENERAL PUBLIC LICENSE
                       Version 3, 29 June 2007
 Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
 Everyone is permitted to copy and distribute verbatim copies
 of this license document, but changing it is not allowed.
  This version of the GNU Lesser General Public License incorporates
 the terms and conditions of version 3 of the GNU General Public
 License, supplemented by the additional permissions listed below.
  0. Additional Definitions.
  As used herein, "this License" refers to version 3 of the GNU Lesser
 General Public License, and the "GNU GPL" refers to version 3 of the GNU
 General Public License.
  "The Library" refers to a covered work governed by this License,
 other than an Application or a Combined Work as defined below.
  An "Application" is any work that makes use of an interface provided
 by the Library, but which is not otherwise based on the Library.
 Defining a subclass of a class defined by the Library is deemed a mode
 of using an interface provided by the Library.
  A "Combined Work" is a work produced by combining or linking an
 Application with the Library.  The particular version of the Library
 with which the Combined Work was made is also called the "Linked
 Version".
  The "Minimal Corresponding Source" for a Combined Work means the
 Corresponding Source for the Combined Work, excluding any source code
 for portions of the Combined Work that, considered in isolation, are
 based on the Application, and not on the Linked Version.
  The "Corresponding Application Code" for a Combined Work means the
 object code and/or source code for the Application, including any data
 and utility programs needed for reproducing the Combined Work from the
 Application, but excluding the System Libraries of the Combined Work.
  1. Exception to Section 3 of the GNU GPL.
  You may convey a covered work under sections 3 and 4 of this License
 without being bound by section 3 of the GNU GPL.
  2. Conveying Modified Versions.
  If you modify a copy of the Library, and, in your modifications, a
 facility refers to a function or data to be supplied by an Application
 that uses the facility (other than as an argument passed when the
 facility is invoked), then you may convey a copy of the modified
 version:
   a) under this License, provided that you make a good faith effort to
   ensure that, in the event an Application does not supply the
   function or data, the facility still operates, and performs
   whatever part of its purpose remains meaningful, or
   b) under the GNU GPL, with none of the additional permissions of
   this License applicable to that copy.
  3. Object Code Incorporating Material from Library Header Files.
  The object code form of an Application may incorporate material from
 a header file that is part of the Library.  You may convey such object
 code under terms of your choice, provided that, if the incorporated
 material is not limited to numerical parameters, data structure
 layouts and accessors, or small macros, inline functions and templates
 (ten or fewer lines in length), you do both of the following:
   a) Give prominent notice with each copy of the object code that the
   Library is used in it and that the Library and its use are
   covered by this License.
   b) Accompany the object code with a copy of the GNU GPL and this license
   document.
  4. Combined Works.
  You may convey a Combined Work under terms of your choice that,
 taken together, effectively do not restrict modification of the
 portions of the Library contained in the Combined Work and reverse
 engineering for debugging such modifications, if you also do each of
 the following:
   a) Give prominent notice with each copy of the Combined Work that
   the Library is used in it and that the Library and its use are
   covered by this License.
   b) Accompany the Combined Work with a copy of the GNU GPL and this license
   document.
   c) For a Combined Work that displays copyright notices during
   execution, include the copyright notice for the Library among
   these notices, as well as a reference directing the user to the
   copies of the GNU GPL and this license document.
   d) Do one of the following:
       0) Convey the Minimal Corresponding Source under the terms of this
       License, and the Corresponding Application Code in a form
       suitable for, and under terms that permit, the user to
       recombine or relink the Application with a modified version of
       the Linked Version to produce a modified Combined Work, in the
       manner specified by section 6 of the GNU GPL for conveying
       Corresponding Source.
       1) Use a suitable shared library mechanism for linking with the
       Library.  A suitable mechanism is one that (a) uses at run time
       a copy of the Library already present on the user's computer
       system, and (b) will operate properly with a modified version
       of the Library that is interface-compatible with the Linked
       Version.
   e) Provide Installation Information, but only if you would otherwise
   be required to provide such information under section 6 of the
   GNU GPL, and only to the extent that such information is
   necessary to install and execute a modified version of the
   Combined Work produced by recombining or relinking the
   Application with a modified version of the Linked Version. (If
   you use option 4d0, the Installation Information must accompany
   the Minimal Corresponding Source and Corresponding Application
   Code. If you use option 4d1, you must provide the Installation
   Information in the manner specified by section 6 of the GNU GPL
   for conveying Corresponding Source.)
  5. Combined Libraries.
  You may place library facilities that are a work based on the
 Library side by side in a single library together with other library
 facilities that are not Applications and are not covered by this
 License, and convey such a combined library under terms of your
 choice, if you do both of the following:
   a) Accompany the combined library with a copy of the same work based
   on the Library, uncombined with any other library facilities,
   conveyed under the terms of this License.
   b) Give prominent notice with the combined library that part of it
   is a work based on the Library, and explaining where to find the
   accompanying uncombined form of the same work.
  6. Revised Versions of the GNU Lesser General Public License.
  The Free Software Foundation may publish revised and/or new versions
 of the GNU Lesser General Public License from time to time. Such new
 versions will be similar in spirit to the present version, but may
 differ in detail to address new problems or concerns.
  Each version is given a distinguishing version number. If the
 Library as you received it specifies that a certain numbered version
 of the GNU Lesser General Public License "or any later version"
 applies to it, you have the option of following the terms and
 conditions either of that published version or of any later version
 published by the Free Software Foundation. If the Library as you
 received it does not specify a version number of the GNU Lesser
 General Public License, you may choose any version of the GNU Lesser
 General Public License ever published by the Free Software Foundation.
  If the Library as you received it specifies that a proxy can decide
 whether future versions of the GNU Lesser General Public License shall
 apply, that proxy's public statement of acceptance of any version is
 permanent authorization for you to choose that version for the
 Library.
--- a/README.md
+++ b/README.md
@ -1,45 +1,49 @@
-# Build
+# Bare-metal Rust on Zynq-7000
 Supported features:
 * Clocking setup
 * UART
 * SDRAM setup
 * Ethernet with smoltcp and async-await on TCP sockets
 * SD card
 * PL programming and startup
 * Pure Rust SZL first-stage bootloader, with SD boot and netboot
 * Control of second CPU core and message passing, with async-await support
 Supported boards:
 * Kasli-SoC
 * ZC706
 * Red Pitaya
 * Cora Z7-10 (seems to also run on Cora Z7-07S, including dual-core support)
 ## Build
 Zynq-rs is packaged using the [Nix](https://nixos.org) Flakes system. Install Nix 2.4+ and enable flakes by adding ``experimental-features = nix-command flakes`` to ``nix.conf`` (e.g. ``~/.config/nix/nix.conf``). 
 You can build SZL or experiments crate for the platform of your choice by using ``nix build`` command, e.g.
 ```shell
-nix-shell --command "cargo xbuild --release"
+nix build .#coraz7-experiments
 ```
-# Debug
+Alternatively, you can still use ``cargo xbuild`` within ``nix develop`` shell.
 ## Using the Xilinx toolchain
 Tested with the ZC706 board.
 Run the Xilinx Microprocessor Debugger:
 ```shell
-/opt/Xilinx/14.7/ISE_DS/EDK/bin/lin64/xmd
+nix develop
 cargo xbuild --release -p experiments
 ```
-Connect to target (given it is connected and you have permissions):
+Currently the ELF output is placed at `target/armv7-none-eabihf/release/experiments`, or `result/experiments.elf` for Nix Flakes build.
 ```tcl
 connect arm hw
 ```
-Leave xmd running.
+## Debug
 Start the Xilinx version of the GNU debugger with your latest build:
 ```shell
 /opt/Xilinx/14.7/ISE_DS/EDK/gnu/arm/lin/bin/arm-xilinx-linux-gnueabi-gdb zc706
 ```
 Connect the debugger to xmd over TCP on localhost:
 ```gdb
 target remote :1234
 ```
 Proceed using gdb with `load`, `c`
 ## Using OpenOCD
 ### Running on the ZC706
 ```shell
-nix-shell --command "cargo xbuild --release"
+nix develop
 cargo xbuild --release -p experiments
 cd openocd
 openocd -f zc706.cfg
 ```
@ -47,7 +51,19 @@ openocd -f zc706.cfg
 ### Running on the Cora Z7-10
 ```shell
-nix-shell --command "cargo xbuild --release --no-default-features --features=target_cora_z7_10"
+nix develop
 cargo xbuild --release -p experiments --no-default-features --features=target_coraz7
 cd openocd
 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"
 ```
 ## License
 Copyright (C) 2019-2022 M-Labs Limited.
 Released under the GNU LGPL v3. See the LICENSE file for details.
--- a/armv7-none-eabihf.json
+++ b/armv7-none-eabihf.json
@ -1,12 +1,4 @@
 {
  "abi-blacklist": [
    "stdcall",
    "fastcall",
    "vectorcall",
    "thiscall",
    "win64",
    "sysv64"
  ],
  "arch": "arm",
  "data-layout": "e-m:e-p:32:32-Fi8-i64:64-v128:64:128-a:0:32-n32-S64",
  "emit-debug-gdb-scripts": false,
--- a/default.nix
+++ b/default.nix
@ -1,27 +0,0 @@
 { # Use master branch of the overlay by default
  mozillaOverlay ? import (builtins.fetchTarball https://github.com/mozilla/nixpkgs-mozilla/archive/master.tar.gz),
 }:
 let
  pkgs = import <nixpkgs> { overlays = [ mozillaOverlay ]; };
 in
 with pkgs;
 let
  rustcSrc = fetchgit {
    url = https://github.com/rust-lang/rust.git;
    # master of 2019-08-18
    rev = "ea52be482ab4945fda63cb65b6a198309a041e3c";
    sha256 = "1spifrkvyyrh1gazqrby29fjqsdbwvajv9k9f6mk2ldrdghlsd21";
    fetchSubmodules = true;
  };
  targets = [
  ];
  rust =
    rustChannelOfTargets "nightly" null targets;
  rustPlatform = recurseIntoAttrs (makeRustPlatform {
    rustc = rust // { src = rustcSrc; };
    cargo = rust;
  });
 in {
  inherit pkgs rustPlatform rustcSrc;
 }
--- a/experiments/Cargo.toml
+++ b/experiments/Cargo.toml
@ -0,0 +1,23 @@
 [package]
 name = "experiments"
 description = "Developing bare-metal Rust on Zynq"
 version = "0.0.0"
 authors = ["M-Labs"]
 edition = "2018"
 [features]
 target_zc706 = ["libboard_zynq/target_zc706", "libsupport_zynq/target_zc706"]
 target_coraz7 = ["libboard_zynq/target_coraz7", "libsupport_zynq/target_coraz7"]
 target_ebaz4205 = ["libboard_zynq/target_ebaz4205", "libsupport_zynq/target_ebaz4205"]
 target_redpitaya = ["libboard_zynq/target_redpitaya", "libsupport_zynq/target_redpitaya"]
 target_kasli_soc = ["libboard_zynq/target_kasli_soc", "libsupport_zynq/target_kasli_soc"]
 default = ["target_zc706"]
 [dependencies]
 log = "0.4"
 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", "dummy_fiq_handler"]}
 libasync = { path = "../libasync" }
--- a/experiments/build.rs
+++ b/experiments/build.rs
@ -12,7 +12,7 @@ fn main() {
        .unwrap();
    println!("cargo:rustc-link-search={}", out.display());
-    // Only re-run the build script when memory.x is changed,
+    // Only re-run the build script when link.x is changed,
    // instead of when any part of the source code changes.
    println!("cargo:rerun-if-changed=link.x");
 }
--- a/experiments/link.x
+++ b/experiments/link.x
@ -0,0 +1,76 @@
 ENTRY(Reset);
 MEMORY
 {
    /* 256 kB On-Chip Memory */
    OCM : ORIGIN = 0, LENGTH = 0x30000
    OCM3 : ORIGIN = 0xFFFF0000, LENGTH = 0x10000
 }
 SECTIONS
 {
    .text :
    {
        KEEP(*(.text.exceptions));
        *(.text.boot);
        *(.text .text.*);
    } > OCM
    .rodata : ALIGN(4)
    {
        *(.rodata .rodata.*);
    } > OCM
    .data : ALIGN(4)
    {
        *(.data .data.*);
    } > OCM
    .bss (NOLOAD) : ALIGN(4)
    {
        __bss_start = .;
        *(.bss .bss.*);
        . = ALIGN(4);
        __bss_end = .;
    } > OCM3
    .irq_stack1 (NOLOAD) : ALIGN(8)
    {
        __irq_stack1_end = .;
        . += 0x100;
        __irq_stack1_start = .;
    } > OCM3
    .irq_stack0 (NOLOAD) : ALIGN(8)
    {
        __irq_stack0_end = .;
        . += 0x100;
        __irq_stack0_start = .;
    } > OCM3
    .stack1 (NOLOAD) : ALIGN(8) {
        __stack1_end = .;
        . += 0x200;
        __stack1_start = .;
    } > OCM3
    .stack0 (NOLOAD) : ALIGN(8) {
        __stack0_end = .;
        . = ORIGIN(OCM3) + LENGTH(OCM3) - 8;
        __stack0_start = .;
        /* unused heap0 to prevent the linker from complaining*/
        __heap0_start = .;
        __heap0_end = .;
    } > OCM3
    /DISCARD/ :
    {
        /* Unused exception related info that only wastes space */
        *(.ARM.exidx);
        *(.ARM.exidx.*);
        *(.ARM.extab.*);
    }
 }
 ASSERT(SIZEOF(.stack0) >= 0x1000, "less than 4 KB left for stack");
--- a/experiments/src/main.rs
+++ b/experiments/src/main.rs
@ -0,0 +1,325 @@
 #![no_std]
 #![no_main]
 #![allow(incomplete_features)]
 #![feature(naked_functions)]
 #![feature(asm)]
 #![feature(inline_const)]
 extern crate alloc;
 use alloc::collections::BTreeMap;
 use core::arch::asm;
 use libasync::{
    delay,
    smoltcp::{Sockets, TcpStream},
    task,
 };
 use libboard_zynq::{
    self as zynq,
    clocks::source::{ArmPll, ClockSource, IoPll},
    clocks::Clocks,
    println, stdio,
    mpcore,
    gic,
    smoltcp::{
        iface::{EthernetInterfaceBuilder, NeighborCache, Routes},
        time::Instant,
        wire::{EthernetAddress, IpAddress, IpCidr},
    },
    time::Milliseconds,
 };
 #[cfg(feature = "target_zc706")]
 use libboard_zynq::print;
 use libcortex_a9::{
    mutex::Mutex,
    l2c::enable_l2_cache,
    sync_channel::{Sender, Receiver},
    sync_channel,
    regs::{MPIDR, SP},
    spin_lock_yield, notify_spin_lock,
    asm, interrupt_handler
 };
 use libregister::{RegisterR, RegisterW};
 use libsupport_zynq::{
    boot, exception_vectors, 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);
 interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
    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");
    loop {}
 });
 pub fn restart_core1() {
    let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());
    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() {
    exception_vectors::set_vector_table(0x0);
    // zynq::clocks::CpuClocks::enable_io(1_250_000_000);
    enable_l2_cache(0x8);
    println!("\nZynq experiments");
    let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());
    interrupt_controller.enable_interrupts();
    libboard_zynq::logger::init().unwrap();
    log::set_max_level(log::LevelFilter::Trace);
    info!(
        "Boot mode: {:?}",
        zynq::slcr::RegisterBlock::slcr()
            .boot_mode
            .read()
            .boot_mode_pins()
    );
    #[cfg(any(
        feature = "target_zc706",
        feature = "target_ebaz4205",
        feature = "target_redpitaya",
        feature = "target_kasli_soc",
    ))]
    const CPU_FREQ: u32 = 800_000_000;
    #[cfg(feature = "target_coraz7")]
    const CPU_FREQ: u32 = 650_000_000;
    info!("Setup clock sources...");
    ArmPll::setup(2 * CPU_FREQ);
    Clocks::set_cpu_freq(CPU_FREQ);
    IoPll::setup(1_000_000_000);
    libboard_zynq::stdio::drop_uart();
    info!("PLLs set up");
    let clocks = zynq::clocks::Clocks::get();
    info!(
        "CPU Clocks: {}/{}/{}/{}",
        clocks.cpu_6x4x(),
        clocks.cpu_3x2x(),
        clocks.cpu_2x(),
        clocks.cpu_1x()
    );
    let timer = libboard_zynq::timer::GlobalTimer::start();
    let mut ddr = zynq::ddr::DdrRam::ddrram();
    #[cfg(not(feature = "target_zc706"))]
    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 };
    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);
        }
    });
    unsafe {
        core1_req.drop_elements();
    }
    // Test I2C
    #[cfg(feature = "target_zc706")]
    {
        let mut i2c = zynq::i2c::I2c::i2c0();
        i2c.init().unwrap();
        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]).unwrap();
        eeprom.write(0x08, &eeprom_buffer[6..22]).unwrap();
        println!("Data written to EEPROM");
        let mut eeprom_buffer = [0u8; 24];
        // Read from 0x00
        eeprom.read(0x00, &mut eeprom_buffer).unwrap();
        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).unwrap();
        print!("Data read from EEPROM @ 0x08: (hex) ");
        for i in 0..16 {
            print!("{:02x} ", eeprom_buffer[i]);
        }
        println!("");
    }
    #[cfg(feature = "target_kasli_soc")]
    {
        let mut err_cdwn = timer.countdown();
        let mut err_state = true;
        let mut led = zynq::error_led::ErrorLED::error_led();
        task::spawn( async move { 
            loop {
                led.toggle(err_state);
                err_state = !err_state;
                delay(&mut err_cdwn, Milliseconds(1000)).await;
            }
        });
    }
    let eth = zynq::eth::Eth::eth0(HWADDR.clone());
    println!("Eth on");
    const RX_LEN: usize = 4096;
    // Number of transmission buffers (minimum is two because with
    // one, duplicate packet transmission occurs)
    const TX_LEN: usize = 4096;
    let eth = eth.start_rx(RX_LEN);
    let mut eth = eth.start_tx(TX_LEN);
    let ethernet_addr = EthernetAddress(HWADDR);
    // IP stack
    let local_addr = IpAddress::v4(192, 168, 1, 51);
    let mut ip_addrs = [IpCidr::new(local_addr, 24)];
    let routes = Routes::new(BTreeMap::new());
    let neighbor_cache = NeighborCache::new(BTreeMap::new());
    let mut iface = EthernetInterfaceBuilder::new(&mut eth)
        .ethernet_addr(ethernet_addr)
        .ip_addrs(&mut ip_addrs[..])
        .routes(routes)
        .neighbor_cache(neighbor_cache)
        .finalize();
    Sockets::init(32);
    const TCP_PORT: u16 = 19;
    // (rx, tx)
    let stats = alloc::rc::Rc::new(core::cell::RefCell::new((0, 0)));
    let stats_tx = stats.clone();
    task::spawn(async move {
        while let Ok(stream) = TcpStream::accept(TCP_PORT, 0x10_0000, 0x10_0000).await {
            let stats_tx = stats_tx.clone();
            task::spawn(async move {
                let tx_data = (0..=255).cycle().take(4096).collect::<alloc::vec::Vec<u8>>();
                loop {
                    // const CHUNK_SIZE: usize = 65536;
                    // match stream.send((0..=255).cycle().take(CHUNK_SIZE)).await {
                    match stream.send_slice(&tx_data[..]).await {
                        Ok(_len) => stats_tx.borrow_mut().1 += tx_data.len(), //CHUNK_SIZE,
                        Err(e) => {
                            warn!("tx: {:?}", e);
                            break
                        }
                    }
                }
            });
        }
    });
    let stats_rx = stats.clone();
    task::spawn(async move {
        while let Ok(stream) = TcpStream::accept(TCP_PORT+1, 0x10_0000, 0x10_0000).await {
            let stats_rx = stats_rx.clone();
            task::spawn(async move {
                loop {
                    match stream.recv(|buf| (buf.len(), buf.len())).await {
                        Ok(len) => stats_rx.borrow_mut().0 += len,
                        Err(e) => {
                            warn!("rx: {:?}", e);
                            break
                        }
                    }
                }
            });
        }
    });
    let mut countdown = timer.countdown();
    task::spawn(async move {
        loop {
            delay(&mut countdown, Milliseconds(1000)).await;
            let timestamp = timer.get_us().0;
            let seconds = timestamp / 1_000_000;
            let micros = timestamp % 1_000_000;
            let (rx, tx) = {
                let mut stats = stats.borrow_mut();
                let result = *stats;
                *stats = (0, 0);
                result
            };
            info!("time: {:6}.{:06}s, rx: {}k/s, tx: {}k/s", seconds, micros, rx / 1024, tx / 1024);
        }
    });
    Sockets::run(&mut iface, || {
        Instant::from_millis(timer.get_time().0 as i64)
    })
 }
 static DONE: Mutex<bool> = Mutex::new(false);
 #[no_mangle]
 pub fn main_core1() {
    println!("Hello from core1!");
    let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());
    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);
    }
    println!("core1 done!");
    *DONE.lock() = true;
    loop {}
 }
--- a/flake.lock
+++ b/flake.lock
@ -0,0 +1,49 @@
 {
  "nodes": {
    "nixpkgs": {
      "locked": {
        "lastModified": 1734529975,
        "narHash": "sha256-ze3IJksru9dN0keqUxY0WNf8xrwfs8Ty/z9v/keyBbg=",
        "owner": "NixOS",
        "repo": "nixpkgs",
        "rev": "72d11d40b9878a67c38f003c240c2d2e1811e72a",
        "type": "github"
      },
      "original": {
        "owner": "NixOS",
        "ref": "nixos-24.05",
        "repo": "nixpkgs",
        "type": "github"
      }
    },
    "root": {
      "inputs": {
        "nixpkgs": "nixpkgs",
        "rust-overlay": "rust-overlay"
      }
    },
    "rust-overlay": {
      "inputs": {
        "nixpkgs": [
          "nixpkgs"
        ]
      },
      "locked": {
        "lastModified": 1719454714,
        "narHash": "sha256-MojqG0lyUINkEk0b3kM2drsU5vyaF8DFZe/FAlZVOGs=",
        "owner": "oxalica",
        "repo": "rust-overlay",
        "rev": "d1c527659cf076ecc4b96a91c702d080b213801e",
        "type": "github"
      },
      "original": {
        "owner": "oxalica",
        "ref": "snapshot/2024-08-01",
        "repo": "rust-overlay",
        "type": "github"
      }
    }
  },
  "root": "root",
  "version": 7
 }
--- a/flake.nix
+++ b/flake.nix
@ -0,0 +1,177 @@
 {
  description = "Bare-metal Rust on Zynq-7000";
  inputs.nixpkgs.url = github:NixOS/nixpkgs/nixos-24.05;
  inputs.rust-overlay = {
    url = "github:oxalica/rust-overlay?ref=snapshot/2024-08-01";
    inputs.nixpkgs.follows = "nixpkgs";
  };
  outputs = { self, nixpkgs, rust-overlay }:
    let
      pkgs = import nixpkgs { system = "x86_64-linux"; overlays = [ (import rust-overlay) crosspkgs-overlay ]; };
      rust = pkgs.rust-bin.nightly."2021-09-01".default.override {
        extensions = [ "rust-src" ];
        targets = [ ];
      };
      rustPlatform = pkgs.makeRustPlatform {
        rustc = rust // {
          # https://github.com/oxalica/rust-overlay/commit/c48c2d76b68dd9ede0815fec53479375c61af857
          targetPlatforms = pkgs.lib.platforms.all;
          tier1TargetPlatforms = pkgs.lib.platforms.all;
          badTargetPlatforms = [ ];
        };
        cargo = rust;
      };
      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";
            };
          };
        };
      });
      mkbootimage = pkgs.stdenv.mkDerivation {
        pname = "mkbootimage";
        version = "2.3dev";
        src = pkgs.fetchFromGitHub {
          owner = "antmicro";
          repo = "zynq-mkbootimage";
          rev = "872363ce32c249f8278cf107bc6d3bdeb38d849f";
          sha256 = "sha256-5FPyAhUWZDwHbqmp9J2ZXTmjaXPz+dzrJMolaNwADHs=";
        };
        propagatedBuildInputs = [ pkgs.libelf pkgs.pcre ];
        patchPhase =
        ''
          substituteInPlace Makefile --replace "git rev-parse --short HEAD" "echo nix"
        '';
        installPhase =
        ''
          mkdir -p $out/bin
          cp mkbootimage $out/bin
        '';
        hardeningDisable = [ "fortify" ];
      };
      fsbl = { board ? "zc706" }: pkgs.stdenv.mkDerivation {
        name = "${board}-fsbl";
        src = pkgs.fetchFromGitHub {
          owner = "Xilinx";
          repo = "embeddedsw";
          rev = "xilinx_v2022.2";
          sha256 = "sha256-UDz9KK/Hw3qM1BAeKif30rE8Bi6C2uvuZlvyvtJCMfw=";
        };
        nativeBuildInputs = [
          pkgs.pkgsCross.zynq-baremetal.buildPackages.binutils
          pkgs.pkgsCross.zynq-baremetal.buildPackages.gcc
        ];
        patchPhase = ''
          patchShebangs lib/sw_apps/zynq_fsbl/misc/copy_bsp.sh
          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
          make BOARD=${board} "CFLAGS=-DFSBL_DEBUG_INFO -g"
        '';
        installPhase = ''
          mkdir $out
          cp fsbl.elf $out
        '';
        doCheck = false;
        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}";
        src = builtins.filterSource (path: type:
          baseNameOf path != "target"
        ) ./.;
        cargoLock = { 
          lockFile = ./Cargo.lock;
          outputHashes = {
            "core_io-0.1.0" = "sha256-0HINFWRiJx8pjMgUOL/CS336ih7SENSRh3Kah9LPRrw="; 
            "fatfs-0.3.6" = "sha256-Nz9hCq/1YgSXF8ltJ5ZawV0Hc8WV44KNK0tJdVnNb4U=";
          };
        };
        nativeBuildInputs = [ cargo-xbuild pkgs.llvmPackages_13.clang-unwrapped ];
        buildPhase = ''
          export XARGO_RUST_SRC="${rust}/lib/rustlib/src/rust/library"
          export CARGO_HOME=$(mktemp -d cargo-home.XXX)
          pushd ${crate}
          cargo xbuild --release --frozen \
            --no-default-features \
            --features=${features}
          popd
        '';
        installPhase = ''
          mkdir -p $out $out/nix-support
          cp target/armv7-none-eabihf/release/${name} $out/${name}.elf
          echo file binary-dist $out/${name}.elf >> $out/nix-support/hydra-build-products
        '';
        doCheck = false;
        dontFixup = true;
        auditable = false;
      };
      targetCrates = target: {
        "${target}-experiments" = build-crate "${target}-experiments" "experiments" "target_${target}";
        "${target}-szl" = build-crate "${target}-szl" "szl" "target_${target}";
      };
      targets = ["zc706" "coraz7" "redpitaya" "kasli_soc" "ebaz4205"];
      allTargetCrates = (builtins.foldl' (results: target:
        results // targetCrates target
      ) {} targets);
      szl = pkgs.runCommand "szl" {} (builtins.foldl' (commands: target:
        let
          szlResult = builtins.getAttr "${target}-szl" allTargetCrates;
        in
          commands + "ln -s ${szlResult}/szl.elf $out/szl-${target}.elf\n"
      ) "mkdir $out\n" targets);
    in rec {
      packages.x86_64-linux = { 
        inherit cargo-xbuild szl mkbootimage;
        zc706-fsbl = fsbl { board = "zc706"; };
      } // allTargetCrates ;
      hydraJobs = packages.x86_64-linux;
      inherit rust rustPlatform;
      devShell.x86_64-linux = pkgs.mkShell {
        name = "zynq-rs-dev-shell";
        buildInputs = [
          rust
          cargo-xbuild
          mkbootimage
          pkgs.openocd pkgs.gdb
          pkgs.openssh pkgs.rsync
          pkgs.llvmPackages_13.clang-unwrapped
          (pkgs.python3.withPackages(ps: [ ps.pyftdi ]))
        ];
      };
    };
 }
--- a/kasli_soc_por.py
+++ b/kasli_soc_por.py
@ -0,0 +1,17 @@
 from time import sleep
 from pyftdi.ftdi import Ftdi
 POR = 1 << 7
 def main():
    dev = Ftdi()
    dev.open_bitbang_from_url("ftdi://ftdi:4232h/0")
    dev.set_bitmode(POR, Ftdi.BitMode.BITBANG)
    dev.write_data(bytes([0]))
    sleep(0.1)
    dev.write_data(bytes([POR]))
    sleep(0.1)
    dev.close()
 if __name__ == "__main__":
    main()
--- a/libasync/Cargo.toml
+++ b/libasync/Cargo.toml
@ -0,0 +1,18 @@
 [package]
 name = "libasync"
 description = "low-level async support"
 version = "0.0.0"
 authors = ["M-Labs"]
 edition = "2018"
 [dependencies]
 #futures = { version = "0.3", default-features = false }
 pin-utils = "0.1.0-alpha.4"
 embedded-hal = "0.2"
 nb = "1.0"
 libcortex_a9 = { path = "../libcortex_a9" }
 [dependencies.smoltcp]
 version = "0.7"
 default-features = false
 features = ["alloc"]
--- a/libasync/src/delay.rs
+++ b/libasync/src/delay.rs
@ -0,0 +1,7 @@
 use embedded_hal::timer::CountDown;
 use crate::block_async;
 pub async fn delay<T: CountDown<Time=C>, C>(timer: &mut T, count: C) {
    timer.start(count);
    let _ = block_async!(timer.wait()).await;
 }
--- a/libasync/src/executor.rs
+++ b/libasync/src/executor.rs
@ -0,0 +1,157 @@
 use core::{
    cell::{RefCell, UnsafeCell},
    future::Future,
    mem::MaybeUninit,
    pin::Pin,
    sync::atomic::{AtomicBool, Ordering},
    task::{Context, Poll, RawWaker, RawWakerVTable, Waker},
 };
 use alloc::{boxed::Box, vec::Vec};
 //use futures::future::FutureExt;
 use pin_utils::pin_mut;
 // NOTE `*const ()` is &AtomicBool
 static VTABLE: RawWakerVTable = {
    unsafe fn clone(p: *const ()) -> RawWaker {
        RawWaker::new(p, &VTABLE)
    }
    unsafe fn wake(p: *const ()) {
        wake_by_ref(p)
    }
    unsafe fn wake_by_ref(p: *const ()) {
        (*(p as *const AtomicBool)).store(true, Ordering::Relaxed)
    }
    unsafe fn drop(_: *const ()) {
        // no-op
    }
    RawWakerVTable::new(clone, wake, wake_by_ref, drop)
 };
 /// ready should not move as long as this waker references it. That is
 /// the reason for keeping Tasks in a pinned box.
 fn wrap_waker(ready: &AtomicBool) -> Waker {
    unsafe { Waker::from_raw(RawWaker::new(ready as *const _ as *const (), &VTABLE)) }
 }
 /// A single-threaded executor
 ///
 /// This is a singleton
 pub struct Executor {
    // Entered block_on() already?
    in_block_on: RefCell<bool>,
    /// Tasks reside on the heap, so that we just queue pointers. They
    /// must also be pinned in memory because our RawWaker is a pointer
    /// to their `ready` field.
    tasks: RefCell<Vec<Pin<Box<Task>>>>,
 }
 impl Executor {
    /// Creates a new instance of the executor
    pub fn new() -> Self {
        Self {
            in_block_on: RefCell::new(false),
            tasks: RefCell::new(Vec::new()),
        }
    }
    pub fn block_on<T>(&self, f: impl Future<Output = T>) -> T {
        // we want to avoid reentering `block_on` because then all the code
        // below has to become more complex. It's also likely that the
        // application will only call `block_on` once on an infinite task
        // (`Future<Output = !>`)
        {
            let mut in_block_on = self.in_block_on.borrow_mut();
            if *in_block_on {
                panic!("nested `block_on`");
            }
            *in_block_on = true;
        }
        pin_mut!(f);
        let ready = AtomicBool::new(true);
        let waker = wrap_waker(&ready);
        let mut backup = Vec::new();
        let val = loop {
            // advance the main task
            if ready.load(Ordering::Relaxed) {
                ready.store(false, Ordering::Relaxed);
                // println!("run block_on");
                let mut cx = Context::from_waker(&waker);
                if let Poll::Ready(val) = f.as_mut().poll(&mut cx) {
                    break val;
                }
                // println!("ran block_on");
            }
            // advance all tasks
            core::mem::swap(&mut *self.tasks.borrow_mut(), &mut backup);
            for mut task in backup.drain(..) {
                // NOTE we don't need a CAS operation here because `wake` invocations that come from
                // interrupt handlers (the only source of 'race conditions' (!= data races)) are
                // "oneshot": they'll issue a `wake` and then disable themselves to not run again
                // until the woken task has made more work
                if task.ready.load(Ordering::Relaxed) {
                    // we are about to service the task so switch the `ready` flag to `false`
                    task.ready.store(false, Ordering::Relaxed);
                    let waker = wrap_waker(&task.ready);
                    let mut cx = Context::from_waker(&waker);
                    let ready = task.f.as_mut().poll(&mut cx).is_ready();
                    if ready {
                        // Task is finished, do not requeue
                        continue;
                    }
                }
                // Requeue
                self.tasks.borrow_mut().push(task);
            }
            // // try to sleep; this will be a no-op if any of the previous tasks generated a SEV or an
            // // interrupt ran (regardless of whether it generated a wake-up or not)
            // asm::wfe();
        };
        self.in_block_on.replace(false);
        val
    }
    pub fn spawn(&self, f: impl Future + 'static) {
        let task = Box::pin(Task::new(f));
        self.tasks.borrow_mut().push(task);
    }
 }
 pub struct Task {
    ready: AtomicBool,
    f: Pin<Box<dyn Future<Output = ()>>>,
 }
 impl Task {
    fn new(f: impl Future + 'static) -> Self {
        Task {
            ready: AtomicBool::new(true),
            f: Box::pin(async { f.await; }),
        }
    }
 }
 /// Returns a handle to the executor singleton
 ///
 /// This lazily initializes the executor and allocator when first called
 pub(crate) fn current() -> &'static Executor {
    static INIT: AtomicBool = AtomicBool::new(false);
    static mut EXECUTOR: UnsafeCell<MaybeUninit<Executor>> = UnsafeCell::new(MaybeUninit::uninit());
    if INIT.load(Ordering::Relaxed) {
        unsafe { &*(EXECUTOR.get() as *const Executor) }
    } else {
        unsafe {
            let executorp = EXECUTOR.get() as *mut Executor;
            executorp.write(Executor::new());
            INIT.store(true, Ordering::Relaxed);
            &*executorp
        }
    }
 }
--- a/libasync/src/lib.rs
+++ b/libasync/src/lib.rs
@ -0,0 +1,36 @@
 #![no_std]
 extern crate alloc;
 pub mod task;
 pub mod executor;
 mod delay;
 pub use delay::delay;
 pub mod smoltcp;
 /// Reexport for macro use
 pub use nb;
 /// The `nb` crate's `block!` macro adapted for async fns
 ///
 /// Call `.await` on the result!
 #[macro_export]
 macro_rules! block_async {
    ($e:expr) => {
        async {
            loop {
                #[allow(unreachable_patterns)]
                match $e {
                    Err($crate::nb::Error::Other(e)) => {
                        #[allow(unreachable_code)]
                        break Err(e)
                    },
                    Err($crate::nb::Error::WouldBlock) =>
                        $crate::task::r#yield().await,
                    Ok(x) => break Ok(x),
                }
            }
        }
    }
 }
--- a/libasync/src/smoltcp/mod.rs
+++ b/libasync/src/smoltcp/mod.rs
@ -0,0 +1,110 @@
 use core::{
    cell::RefCell,
    task::Waker,
 };
 use alloc::vec::Vec;
 use smoltcp::{
    iface::EthernetInterface,
    phy::Device,
    socket::SocketSet,
    time::{Duration, Instant},
 };
 use crate::task;
 mod tcp_stream;
 pub use tcp_stream::TcpStream;
 pub trait LinkCheck {
    type Link;
    fn is_idle(&self) -> bool;
    fn check_link_change(&mut self) -> Option<Self::Link>;
 }
 static mut SOCKETS: Option<Sockets> = None;
 pub struct Sockets {
    sockets: RefCell<SocketSet<'static>>,
    wakers: RefCell<Vec<Waker>>,
 }
 impl Sockets {
    pub fn init(max_sockets: usize) {
        let mut sockets_storage = Vec::with_capacity(max_sockets);
        for _ in 0..max_sockets {
            sockets_storage.push(None);
        }
        let sockets = RefCell::new(SocketSet::new(sockets_storage));
        let wakers = RefCell::new(Vec::new());
        let instance = Sockets {
            sockets,
            wakers,
        };
        // println!("sockets initialized");
        unsafe { SOCKETS = Some(instance); }
    }
    /// Block and run executor indefinitely while polling the smoltcp
    /// iface
    pub fn run<'b, D: for<'d> Device<'d> + LinkCheck>(
        iface: &mut EthernetInterface<'b, D>,
        mut get_time: impl FnMut() -> Instant,
    ) -> ! {
        task::block_on(async {
            let mut last_link_check = Instant::from_millis(0);
            const LINK_CHECK_INTERVAL: u64 = 500;
            loop {
                let instant = get_time();
                Self::instance().poll(iface, instant);
                let dev = iface.device_mut();
                if dev.is_idle() && instant >= last_link_check + Duration::from_millis(LINK_CHECK_INTERVAL) {
                    dev.check_link_change();
                    last_link_check = instant;
                }
                task::r#yield().await;
            }
        })
    }
    pub(crate) fn instance() -> &'static Self {
        unsafe { SOCKETS.as_ref().expect("Sockets") }
    }
    fn poll<'b, D: for<'d> Device<'d>>(
        &self,
        iface: &mut EthernetInterface<'b, D>,
        instant: Instant
    ) {
        let processed = {
            let mut sockets = self.sockets.borrow_mut();
            match iface.poll(&mut sockets, instant) {
                Ok(processed) => processed,
                Err(_) => true,
            }
        };
        if processed {
            let mut wakers = self.wakers.borrow_mut();
            for waker in wakers.drain(..) {
                waker.wake();
            }
        }
    }
    /// TODO: this was called through eg. TcpStream, another poll()
    /// might want to send packets before sleeping for an interrupt.
    pub(crate) fn register_waker(waker: Waker) {
        let mut wakers = Self::instance().wakers.borrow_mut();
        for (i, w) in wakers.iter().enumerate() {
            if w.will_wake(&waker) {
                let last = wakers.len() - 1;
                wakers.swap(i, last);
                return;
            }
        }
        wakers.push(waker);
    }
 }
--- a/libasync/src/smoltcp/tcp_stream.rs
+++ b/libasync/src/smoltcp/tcp_stream.rs
@ -0,0 +1,291 @@
 //! async TCP interface
 //!
 //! TODO: implement futures AsyncRead/AsyncWrite/Stream/Sink interfaces
 use core::{
    future::Future,
    pin::Pin,
    task::{Context, Poll},
 };
 use alloc::vec::Vec;
 use smoltcp::{
    Error, Result,
    socket::{
        SocketHandle, SocketRef,
        TcpSocketBuffer, TcpSocket, TcpState,
    },
    time::Duration,
 };
 use crate::task;
 use super::Sockets;
 /// References a smoltcp TcpSocket
 pub struct TcpStream {
    handle: SocketHandle,
 }
 /// Wait while letting `$f()` poll a stream's socket
 macro_rules! poll_stream {
    ($stream: expr, $output: ty, $f: expr) => (async {
        struct Adhoc<'a> {
            stream: &'a TcpStream,
        }
        impl<'a> Future for Adhoc<'a> {
            type Output = $output;
            fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
                let result = self.stream.with_socket($f);
                if !result.is_ready() {
                    Sockets::register_waker(cx.waker().clone());
                }
                result
            }
        }
        Adhoc { stream: $stream }.await
    })
 }
 impl TcpStream {
    /// Allocates sockets and its buffers, registers it in the
    /// SocketSet.
    ///
    /// Not `pub` as the result can not yet be used. Use `listen()` or
    /// `connect()` to obtain a valid TcpStream.
    fn new(rx_bufsize: usize, tx_bufsize: usize) -> Self {
        fn uninit_vec<T>(size: usize) -> Vec<T> {
            let mut result = Vec::with_capacity(size);
            unsafe {
                result.set_len(size);
            }
            result
        }
        let rx_buffer = TcpSocketBuffer::new(uninit_vec(rx_bufsize));
        let tx_buffer = TcpSocketBuffer::new(uninit_vec(tx_bufsize));
        let socket = TcpSocket::new(rx_buffer, tx_buffer);
        let handle = Sockets::instance().sockets.borrow_mut()
            .add(socket);
        TcpStream { handle }
    }
    /// Operate on the referenced TCP socket
    fn with_socket<F, R>(&self, f: F) -> R
    where
        F: FnOnce(SocketRef<TcpSocket>) -> R,
    {
        let mut sockets = Sockets::instance().sockets.borrow_mut();
        let socket_ref = sockets.get::<TcpSocket>(self.handle);
        f(socket_ref)
    }
    /// Listen for the next incoming connection on a TCP
    /// port. Succeeds on connection attempt.
    ///
    /// Calling this serially in a loop will cause slow/botched
    /// connection attempts stall any more new connections. Use
    /// `listen()` with a backlog instead.
    pub async fn accept(port: u16, rx_bufsize: usize, tx_bufsize: usize) -> Result<Self> {
        let stream = Self::new(rx_bufsize, tx_bufsize);
        // Set socket to listen
        stream.with_socket(|mut s| s.listen(port))?;
        // Wait for a connection
        poll_stream!(&stream, (), |socket| {
            if socket.state() != TcpState::Listen {
                Poll::Ready(())
            } else {
                Poll::Pending
            }
        }).await;
        Ok(stream)
    }
    /// Probe the receive buffer
    ///
    /// Your callback will only be called when there is some data available,
    /// and it must consume at least one byte. It returns a tuple with the
    /// number of bytes it consumed, and a user-defined return value of type R.
    pub async fn recv<F, R>(&self, f: F) -> Result<R>
    where
        F: Fn(&[u8]) -> (usize, R),
    {
        struct Recv<'a, F: FnOnce(&[u8]) -> (usize, R), R> {
            stream: &'a TcpStream,
            f: F,
        }
        impl<'a, F: Fn(&[u8]) -> (usize, R), R> Future for Recv<'a, F, R> {
            type Output = Result<R>;
            fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
                let result = self.stream.with_socket(|mut socket| {
                    if socket_is_handhshaking(&socket) {
                        return Ok(Poll::Pending);
                    }
                    socket.recv(|buf| {
                        if buf.len() > 0 {
                            let (amount, result) = (self.f)(buf);
                            assert!(amount > 0);
                            (amount, Poll::Ready(Ok(result)))
                        } else {
                            (0, Poll::Pending)
                        }
                    })
                });
                match result {
                    Ok(Poll::Pending) => {
                        Sockets::register_waker(cx.waker().clone());
                        Poll::Pending
                    }
                    Ok(result) => {
                        result
                    }
                    Err(e) =>
                        Poll::Ready(Err(e)),
                }
            }
        }
        Recv {
            stream: self,
            f,
        }.await
    }
    /// Wait until there is any space in the socket's send queue
    async fn wait_can_send(&self) -> Result<()> {
        poll_stream!(self, Result<()>, |socket| {
            if socket_is_handhshaking(&socket) {
                Poll::Pending
            } else if socket.can_send() {
                Poll::Ready(Ok(()))
            } else if ! socket.may_send() {
                Poll::Ready(Err(Error::Truncated))
            } else {
                Poll::Pending
            }
        }).await
    }
    /// Yields to wait for more buffer space
    pub async fn send<I: IntoIterator<Item = u8>>(&self, data: I) -> Result<()> {
        let mut data = data.into_iter();
        let mut done = false;
        while !done {
            self.wait_can_send().await?;
            self.with_socket(|mut socket| {
                socket.send(|buf| {
                    for i in 0..buf.len() {
                        if let Some(byte) = data.next() {
                            buf[i] = byte;
                        } else {
                            done = true;
                            return (i, ())
                        }
                    }
                    (buf.len(), ())
                })
            })?;
        }
        Ok(())
    }
    /// Yields to wait for more buffer space
    pub async fn send_slice(&self, mut data: &'_ [u8]) -> Result<()> {
        while data.len() > 0 {
            self.wait_can_send().await?;
            data = self.with_socket(|mut socket| {
                socket.send(|buf| {
                    let len = buf.len().min(data.len());
                    buf[..len].copy_from_slice(&data[..len]);
                    data = &data[len..];
                    (len, data)
                })
            })?;
        }
        Ok(())
    }
    /// Wait for all queued data to be sent and ACKed
    ///
    /// **Warning:** this may not work as immediately as expected! The
    /// other side may wait until it sends packets to you for
    /// piggybacking the ACKs.
    pub async fn flush(&self) -> Result<()> {
        poll_stream!(self, Result<()>, |socket| {
            if socket_is_handhshaking(&socket) {
                Poll::Pending
            } else if socket.may_send() && socket.send_queue() > 0 {
                Poll::Pending
            } else if socket.may_send() {
                Poll::Ready(Ok(()))
            } else {
                Poll::Ready(Err(Error::Truncated))
            }
        }).await
    }
    /// Close the transmit half of the connection
    pub async fn close(&self) {
        self.with_socket(|mut socket| socket.close());
        // Yield for one iface.poll() to send the packet
        task::r#yield().await;
    }
    /// Destroy the socket, sending the RST
    pub async fn abort(self) {
        self.with_socket(|mut socket| socket.abort());
        // Yield for one iface.poll() to send the packet
        task::r#yield().await;
    }
    pub fn keep_alive(&self) -> Option<Duration> {
        self.with_socket(|socket| socket.keep_alive())
    }
    pub fn set_keep_alive(&mut self, interval: Option<Duration>) {
        self.with_socket(|mut socket| socket.set_keep_alive(interval));
    }
    pub fn timeout(&self) -> Option<Duration> {
        self.with_socket(|socket| socket.timeout())
    }
    pub fn set_timeout(&mut self, duration: Option<Duration>) {
        self.with_socket(|mut socket| socket.set_timeout(duration));
    }
    pub fn ack_delay(&self) -> Option<Duration> {
        self.with_socket(|socket| socket.ack_delay())
    }
    pub fn set_ack_delay(&mut self, duration: Option<Duration>) {
        self.with_socket(|mut socket| socket.set_ack_delay(duration));
    }
 }
 impl Drop for TcpStream {
    /// Free item in the socket set, which leads to deallocation of
    /// the rx/tx buffers associated with this socket.
    fn drop(&mut self) {
        Sockets::instance().sockets.borrow_mut()
            .remove(self.handle);
    }
 }
 fn socket_is_handhshaking(socket: &SocketRef<TcpSocket>) -> bool {
    match socket.state() {
        TcpState::SynSent | TcpState::SynReceived =>
            true,
        _ =>
            false,
    }
 }
--- a/libasync/src/task.rs
+++ b/libasync/src/task.rs
@ -0,0 +1,47 @@
 //! Asynchronous tasks
 use core::{
    future::Future,
    pin::Pin,
    task::{Context, Poll},
 };
 use super::executor;
 /// Drives the future `f` to completion
 ///
 /// This also makes any previously `spawn`-ed future make progress
 pub fn block_on<T>(f: impl Future<Output = T>) -> T {
    executor::current().block_on(f)
 }
 /// Spawns a task onto the executor
 ///
 /// The spawned task will not make any progress until `block_on` is called.
 pub fn spawn(f: impl Future + 'static) {
    executor::current().spawn(f)
 }
 /// Use `r#yield.await` to suspend the execution of a task
 pub async fn r#yield() {
    struct Yield {
        yielded: bool,
    }
    impl Future for Yield {
        type Output = ();
        fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
            if self.yielded {
                Poll::Ready(())
            } else {
                self.yielded = true;
                // wake ourselves
                cx.waker().wake_by_ref();
                //asm::sev();
                Poll::Pending
            }
        }
    }
    Yield { yielded: false }.await
 }
--- a/libboard_zynq/Cargo.toml
+++ b/libboard_zynq/Cargo.toml
@ -0,0 +1,30 @@
 [package]
 name = "libboard_zynq"
 description = "Drivers for peripherals in the Zynq PS"
 version = "0.0.0"
 authors = ["M-Labs"]
 edition = "2018"
 [features]
 target_zc706 = []
 target_coraz7 = []
 target_ebaz4205 = []
 target_redpitaya = []
 target_kasli_soc = []
 ipv6 = [ "smoltcp/proto-ipv6" ]
 [dependencies]
 volatile-register = "0.2"
 bit_field = "0.10"
 embedded-hal = "0.2"
 nb = "0.1"
 void = { version = "1", default-features = false }
 log = "0.4"
 libregister = { path = "../libregister" }
 libcortex_a9 = { path = "../libcortex_a9" }
 libasync = { path = "../libasync" }
 [dependencies.smoltcp]
 version = "0.7"
 features = ["ethernet", "proto-ipv4", "socket-tcp"]
 default-features = false
--- a/libboard_zynq/src/axi_gp.rs
+++ b/libboard_zynq/src/axi_gp.rs
@ -0,0 +1,2 @@
 pub const M_AXI_GP0: usize = 0x4000_0000;
 pub const M_AXI_GP1: usize = 0x8000_0000;
--- a/libboard_zynq/src/axi_hp.rs
+++ b/libboard_zynq/src/axi_hp.rs
@ -0,0 +1,59 @@
 //! AXI_HP Interface (AFI)
 use volatile_register::RW;
 use libregister::{register, register_bit, register_bits};
 pub unsafe fn axi_hp0() -> &'static RegisterBlock {
    &*(0xF8008000 as *const _)
 }
 pub unsafe fn axi_hp1() -> &'static RegisterBlock {
    &*(0xF8009000 as *const _)
 }
 pub unsafe fn axi_hp2() -> &'static RegisterBlock {
    &*(0xF800A000 as *const _)
 }
 pub unsafe fn axi_hp3() -> &'static RegisterBlock {
    &*(0xF800B000 as *const _)
 }
 #[repr(C)]
 pub struct RegisterBlock {
    /// Read Channel Control Register
    pub rdchan_ctrl: RdchanCtrl,
    /// Read Issuing Capability Register
    pub rdchan_issuingcap: RW<u32>,
    /// QOS Read Channel Register
    pub rdqos: RW<u32>,
    /// Read Data FIFO Level Register
    pub rddatafifo_level: RW<u32>,
    /// Read Channel Debug Register
    pub rddebug: RW<u32>,
    /// Write Channel Control Register
    pub wrchan_ctrl: WrchanCtrl,
    /// Write Issuing Capability Register
    pub wrchan_issuingcap: RW<u32>,
    /// QOS Write Channel Register
    pub wrqos: RW<u32>,
    /// Write Data FIFO Level Register
    pub wrdatafifo_level: RW<u32>,
    /// Write Channel Debug Register
    pub wrdebug: RW<u32>,
 }
 register!(rdchan_ctrl, RdchanCtrl, RW, u32);
 register_bit!(rdchan_ctrl, en_32bit, 0);
 register_bit!(rdchan_ctrl, fabric_qos_en, 1);
 register_bit!(rdchan_ctrl, fabric_out_cmd_en, 2);
 register_bit!(rdchan_ctrl, qos_head_of_cmd_q_en, 3);
 register!(wrchan_ctrl, WrchanCtrl, RW, u32);
 register_bit!(wrchan_ctrl, en_32bit, 0);
 register_bit!(wrchan_ctrl, fabric_qos_en, 1);
 register_bit!(wrchan_ctrl, fabric_out_cmd_en, 2);
 register_bit!(wrchan_ctrl, qos_head_of_cmd_q_en, 3);
 register_bits!(wrchan_ctrl, wr_cmd_release_mode, u8, 4, 5);
 register_bits!(wrchan_ctrl, wr_data_threshold, u8, 8, 11);
--- a/libboard_zynq/src/clocks/mod.rs
+++ b/libboard_zynq/src/clocks/mod.rs
@ -1,10 +1,11 @@
-use crate::slcr;
+use core::unimplemented;
 use crate::regs::RegisterR;
-#[cfg(feature = "target_zc706")]
+use libregister::{RegisterR, RegisterRW};
-const PS_CLK: u32 = 33_333_333;
+use super::slcr;
-#[cfg(feature = "target_cora_z7_10")]
+pub use slcr::ArmPllSource;
-const PS_CLK: u32 = 50_000_000;
+
 pub mod source;
 use source::*;
 enum CpuClockMode {
    /// Clocks run in 4:2:2:1 mode
@ -15,7 +16,7 @@ enum CpuClockMode {
 impl CpuClockMode {
    pub fn get() -> Self {
-        let regs = slcr::RegisterBlock::new();
+        let regs = slcr::RegisterBlock::slcr();
        if regs.clk_621_true.read().clk_621_true() {
            CpuClockMode::C621
        } else {
@ -25,7 +26,7 @@ impl CpuClockMode {
 }
 #[derive(Debug, Clone)]
-pub struct CpuClocks {
+pub struct Clocks {
    /// ARM PLL: Recommended clock source for the CPUs and the interconnect
    pub arm: u32,
    /// DDR PLL: Recommended clock for the DDR DRAM controller and AXI_HP interfaces
@ -34,22 +35,38 @@ pub struct CpuClocks {
    pub io: u32,
 }
-impl CpuClocks {
+impl Clocks {
    pub fn get() -> Self {
-        let regs = slcr::RegisterBlock::new();
+        Clocks {
-        let arm = u32::from(regs.arm_pll_ctrl.read().pll_fdiv()) * PS_CLK;
+            arm: ArmPll::freq(),
-        let ddr = u32::from(regs.ddr_pll_ctrl.read().pll_fdiv()) * PS_CLK;
+            ddr: DdrPll::freq(),
-        let io = u32::from(regs.io_pll_ctrl.read().pll_fdiv()) * PS_CLK;
+            io: IoPll::freq(),
-        CpuClocks { arm, ddr, io }
+        }
    }
    pub fn set_cpu_freq(target_freq: u32) {
        let arm_pll = ArmPll::freq();
        // 1 and 3 cannot be used
        let mut div = 2u8;
        while div == 3 || (div < 63 && arm_pll / u32::from(div) > target_freq) {
            div += 1;
        }
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.arm_clk_ctrl.modify(|_, w| w
                .srcsel(ArmPllSource::ArmPll)
                .divisor(div)
            );
        })
    }
    pub fn cpu_6x4x(&self) -> u32 {
-        let regs = slcr::RegisterBlock::new();
+        let slcr = slcr::RegisterBlock::slcr();
-        let arm_clk_ctrl = regs.arm_clk_ctrl.read();
+        let arm_clk_ctrl = slcr.arm_clk_ctrl.read();
        let pll = match arm_clk_ctrl.srcsel() {
-            slcr::ArmPllSource::ArmPll => self.arm,
+            ArmPllSource::ArmPll => self.arm,
-            slcr::ArmPllSource::DdrPll => self.ddr,
+            ArmPllSource::DdrPll => self.ddr,
-            slcr::ArmPllSource::IoPll => self.io,
+            ArmPllSource::IoPll => self.io,
        };
        pll / u32::from(arm_clk_ctrl.divisor())
    }
@ -77,7 +94,7 @@ impl CpuClocks {
    }
    pub fn uart_ref_clk(&self) -> u32 {
-        let regs = slcr::RegisterBlock::new();
+        let regs = slcr::RegisterBlock::slcr();
        let uart_clk_ctrl = regs.uart_clk_ctrl.read();
        let pll = match uart_clk_ctrl.srcsel() {
            slcr::PllSource::ArmPll =>
@ -86,7 +103,25 @@ impl CpuClocks {
                self.ddr,
            slcr::PllSource::IoPll =>
                self.io,
            slcr::PllSource::Emio =>
                unimplemented!(),
        };
        pll / u32::from(uart_clk_ctrl.divisor())
    }
    pub fn sdio_ref_clk(&self) -> u32 {
        let regs = slcr::RegisterBlock::slcr();
        let sdio_clk_ctrl = regs.sdio_clk_ctrl.read();
        let pll = match sdio_clk_ctrl.srcsel() {
            slcr::PllSource::ArmPll =>
                self.arm,
            slcr::PllSource::DdrPll =>
                self.ddr,
            slcr::PllSource::IoPll =>
                self.io,
            slcr::PllSource::Emio =>
                unimplemented!(),
        };
        pll / u32::from(sdio_clk_ctrl.divisor())
    }
 }
--- a/libboard_zynq/src/clocks/source.rs
+++ b/libboard_zynq/src/clocks/source.rs
@ -0,0 +1,178 @@
 use log::debug;
 use libregister::{RegisterR, RegisterW, RegisterRW};
 use super::slcr;
 #[cfg(feature = "target_zc706")]
 pub const PS_CLK: u32 = 33_333_333;
 #[cfg(feature = "target_coraz7")]
 pub const PS_CLK: u32 = 50_000_000;
 #[cfg(feature = "target_ebaz4205")]
 pub const PS_CLK: u32 = 33_333_333;
 #[cfg(feature = "target_redpitaya")]
 pub const PS_CLK: u32 = 33_333_333;
 #[cfg(feature = "target_kasli_soc")]
 pub const PS_CLK: u32 = 33_333_333;
 /// (pll_fdiv_max, (pll_cp, pll_res, lock_cnt))
 const PLL_FDIV_LOCK_PARAM: &[(u16, (u8, u8, u16))] = &[
    (13, (2, 6, 750)),
    (14, (2, 6, 700)),
    (15, (2, 6, 650)),
    (16, (2, 10, 625)),
    (17, (2, 10, 575)),
    (18, (2, 10, 550)),
    (19, (2, 10, 525)),
    (20, (2, 12, 500)),
    (21, (2, 12, 475)),
    (22, (2, 12, 450)),
    (23, (2, 12, 425)),
    (25, (2, 12, 400)),
    (26, (2, 12, 375)),
    (28, (2, 12, 350)),
    (30, (2, 12, 325)),
    (33, (2, 2, 300)),
    (36, (2, 2, 275)),
    (40, (2, 2, 250)),
    (47, (3, 12, 250)),
    (66, (2, 4, 250)),
 ];
 pub trait ClockSource {
    /// picks this ClockSource's registers from the SLCR block
    fn pll_regs(slcr: &mut crate::slcr::RegisterBlock)
                -> (&mut crate::slcr::PllCtrl,
                    &mut crate::slcr::PllCfg,
                    &mut crate::slcr::PllStatus
                   );
    /// query PLL lock status
    fn pll_locked(pll_status: &mut crate::slcr::PllStatus) -> bool;
    /// get configured frequency
    fn freq() -> u32 {
        let mut slcr = slcr::RegisterBlock::slcr();
        let (pll_ctrl, _, _) = Self::pll_regs(&mut slcr);
        u32::from(pll_ctrl.read().pll_fdiv()) * PS_CLK
    }
    fn name() -> &'static str;
    /// Zynq-7000 AP SoC Technical Reference Manual:
    /// 25.10.4 PLLs
    fn setup(target_freq: u32) {
        let fdiv = (target_freq / PS_CLK).min(66) as u16;
        let (pll_cp, pll_res, lock_cnt) = PLL_FDIV_LOCK_PARAM.iter()
            .filter(|(fdiv_max, _)| fdiv <= *fdiv_max)
            .nth(0)
            .expect("PLL_FDIV_LOCK_PARAM")
            .1.clone();
        debug!("Set {} to {} Hz", Self::name(), target_freq);
        slcr::RegisterBlock::unlocked(|slcr| {
            let (pll_ctrl, pll_cfg, pll_status) = Self::pll_regs(slcr);
            // Bypass
            pll_ctrl.modify(|_, w| w
                            .pll_pwrdwn(false)
                            .pll_bypass_force(true)
                            .pll_fdiv(fdiv)
            );
            // Configure
            pll_cfg.write(
                slcr::PllCfg::zeroed()
                    .pll_res(pll_res)
                    .pll_cp(pll_cp)
                    .lock_cnt(lock_cnt)
            );
            // Reset
            pll_ctrl.modify(|_, w| w.pll_reset(true));
            pll_ctrl.modify(|_, w| w.pll_reset(false));
            // Wait for PLL lock
            while ! Self::pll_locked(pll_status) {}
            // Remove bypass
            pll_ctrl.modify(|_, w| w
                            .pll_bypass_force(false)
                            .pll_bypass_qual(false)
            );
        });
    }
 }
 /// ARM PLL: Recommended clock source for the CPUs and the interconnect
 pub struct ArmPll;
 impl ClockSource for ArmPll {
    #[inline]
    fn pll_regs(slcr: &mut crate::slcr::RegisterBlock)
                -> (&mut crate::slcr::PllCtrl,
                    &mut crate::slcr::PllCfg,
                    &mut crate::slcr::PllStatus
    ) {
        (&mut slcr.arm_pll_ctrl,
         &mut slcr.arm_pll_cfg,
         &mut slcr.pll_status
        )
    }
    #[inline]
    fn pll_locked(pll_status: &mut crate::slcr::PllStatus) -> bool {
        pll_status.read().arm_pll_lock()
    }
    fn name() -> &'static str {
        &"ARM_PLL"
    }
 }
 /// DDR PLL: Recommended clock for the DDR DRAM controller and AXI_HP interfaces
 pub struct DdrPll;
 impl ClockSource for DdrPll {
    #[inline]
    fn pll_regs(slcr: &mut crate::slcr::RegisterBlock)
                -> (&mut crate::slcr::PllCtrl,
                    &mut crate::slcr::PllCfg,
                    &mut crate::slcr::PllStatus
    ) {
        (&mut slcr.ddr_pll_ctrl,
         &mut slcr.ddr_pll_cfg,
         &mut slcr.pll_status
        )
    }
    #[inline]
    fn pll_locked(pll_status: &mut crate::slcr::PllStatus) -> bool {
        pll_status.read().ddr_pll_lock()
    }
    fn name() -> &'static str {
        &"DDR_PLL"
    }
 }
 /// I/O PLL: Recommended clock for I/O peripherals
 pub struct IoPll;
 impl ClockSource for IoPll {
    #[inline]
    fn pll_regs(slcr: &mut crate::slcr::RegisterBlock)
                -> (&mut crate::slcr::PllCtrl,
                    &mut crate::slcr::PllCfg,
                    &mut crate::slcr::PllStatus
    ) {
        (&mut slcr.io_pll_ctrl,
         &mut slcr.io_pll_cfg,
         &mut slcr.pll_status
        )
    }
    #[inline]
    fn pll_locked(pll_status: &mut crate::slcr::PllStatus) -> bool {
        pll_status.read().io_pll_lock()
    }
    fn name() -> &'static str {
        &"IO_PLL"
    }
 }
--- a/libboard_zynq/src/ddr/mod.rs
+++ b/libboard_zynq/src/ddr/mod.rs
@ -0,0 +1,520 @@
 use libregister::{RegisterR, RegisterW, RegisterRW};
 use log::{debug, info, error};
 use crate::{print, println};
 use super::slcr;
 #[cfg(feature = "target_zc706")]
 use super::slcr::DdriobVrefSel;
 use super::clocks::{Clocks, source::{DdrPll, ClockSource}};
 mod regs;
 #[cfg(feature = "target_zc706")]
 /// Micron MT41J256M8HX-15E: 667 MHz DDR3
 const DDR_FREQ: u32 = 666_666_666;
 #[cfg(feature = "target_coraz7")]
 /// Micron MT41K256M16HA-125: 800 MHz DDR3L, max supported 533 MHz
 const DDR_FREQ: u32 = 525_000_000;
 #[cfg(feature = "target_ebaz4205")]
 /// EtronTech Memory EM6GD16EWKG-12H: 800 MHz DDR3 at 533 MHz
 const DDR_FREQ: u32 = 533_333_333;
 #[cfg(feature = "target_redpitaya")]
 /// Alliance Memory AS4C256M16D3B: 800 MHz DDR3 at 533 MHz
 const DDR_FREQ: u32 = 533_333_333;
 #[cfg(feature = "target_kasli_soc")]
 /// MT41K256M16HA-125:E: 800 MHz DDR3L at 533 MHz
 const DDR_FREQ: u32 = 533_333_333;
 const DCI_MAX_FREQ: u32 = 10_000_000;
 pub struct DdrRam {
    regs: &'static mut regs::RegisterBlock,
 }
 impl DdrRam {
    pub fn ddrram() -> Self {
        let clocks = Self::clock_setup();
        Self::configure_iob();
        Self::calibrate_iob_impedance(&clocks);
        let regs = regs::RegisterBlock::ddrc();
        let mut ddr = DdrRam { regs };
        ddr.reset_ddrc(|ddr| ddr.configure());
        ddr
    }
    /// Zynq-7000 AP SoC Technical Reference Manual:
    /// 10.6.1 DDR Clock Initialization
    fn clock_setup() -> Clocks {
        DdrPll::setup(2 * DDR_FREQ);
        let clocks = Clocks::get();
        let ddr3x_clk_divisor = 2;
        let ddr2x_clk_divisor = 3;
        debug!("DDR 3x/2x clocks: {}/{}", clocks.ddr / u32::from(ddr3x_clk_divisor), clocks.ddr / u32::from(ddr2x_clk_divisor));
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.ddr_clk_ctrl.write(
                slcr::DdrClkCtrl::zeroed()
                    .ddr_2xclkact(true)
                    .ddr_3xclkact(true)
                    .ddr_2xclk_divisor(ddr2x_clk_divisor)
                    .ddr_3xclk_divisor(ddr3x_clk_divisor)
            );
        });
        clocks
    }
    fn calculate_dci_divisors(clocks: &Clocks) -> (u8, u8) {
        let target = (DCI_MAX_FREQ - 1 + clocks.ddr) / DCI_MAX_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, 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.
            slcr.dci_clk_ctrl.write(
                slcr::DciClkCtrl::zeroed()
                    .clkact(true)
                    .divisor0(divisor0)
                    .divisor1(divisor1)
            );
            // Step 2.a.
            slcr.ddriob_dci_ctrl.modify(|_, w|
                w.reset(false)
            );
            slcr.ddriob_dci_ctrl.modify(|_, w|
                w.reset(true)
            );
            // Step 3.b. for DDR3/DDR3L
            slcr.ddriob_dci_ctrl.modify(|_, w|
                w.nref_opt1(0)
                 .nref_opt2(0)
                 .nref_opt4(1)
                 .pref_opt1(0)
                 .pref_opt2(0)
            );
            // Step 2.c.
            slcr.ddriob_dci_ctrl.modify(|_, w|
                w.update_control(false)
            );
            // Step 2.d.
            slcr.ddriob_dci_ctrl.modify(|_, w|
                w.enable(true)
            );
            // Step 2.e.
            while ! slcr.ddriob_dci_status.read().done() {}
        });
    }
    /// Zynq-7000 AP SoC Technical Reference Manual:
    /// 10.6.3 DDR IOB Configuration
    fn configure_iob() {
        slcr::RegisterBlock::unlocked(|slcr| {
            let addr_config = slcr::DdriobConfig::zeroed()
                .output_en(slcr::DdriobOutputEn::Obuf);
            slcr.ddriob_addr0.write(addr_config.clone());
            slcr.ddriob_addr1.write(addr_config);
            #[cfg(feature = "target_zc706")]
            let data0_config = slcr::DdriobConfig::zeroed()
                .inp_type(slcr::DdriobInputType::VrefDifferential)
                .term_en(true)
                .dci_type(slcr::DdriobDciType::Termination)
                .output_en(slcr::DdriobOutputEn::Obuf);
            #[cfg(feature = "target_zc706")]
            let data1_config = data0_config.clone();
            #[cfg(any(
                feature = "target_coraz7",
                feature = "target_ebaz4205",
                feature = "target_redpitaya",
                feature = "target_kasli_soc",
            ))]
            let data0_config = slcr::DdriobConfig::zeroed()
                .inp_type(slcr::DdriobInputType::VrefDifferential)
                .term_en(true)
                .dci_type(slcr::DdriobDciType::Termination)
                .output_en(slcr::DdriobOutputEn::Obuf);
            #[cfg(any(
                feature = "target_coraz7",
                feature = "target_ebaz4205",
                feature = "target_redpitaya",
                feature = "target_kasli_soc",
            ))]
            let data1_config = slcr::DdriobConfig::zeroed()
                .pullup_en(true);
            slcr.ddriob_data0.write(data0_config);
            slcr.ddriob_data1.write(data1_config);
            #[cfg(feature = "target_zc706")]
            let diff0_config = slcr::DdriobConfig::zeroed()
                .inp_type(slcr::DdriobInputType::Differential)
                .term_en(true)
                .dci_type(slcr::DdriobDciType::Termination)
                .output_en(slcr::DdriobOutputEn::Obuf);
            #[cfg(feature = "target_zc706")]
            let diff1_config = diff0_config.clone();
            #[cfg(any(
                feature = "target_coraz7",
                feature = "target_ebaz4205",
                feature = "target_redpitaya",
                feature = "target_kasli_soc",
            ))]
            let diff0_config = slcr::DdriobConfig::zeroed()
                .inp_type(slcr::DdriobInputType::Differential)
                .term_en(true)
                .dci_type(slcr::DdriobDciType::Termination)
                .output_en(slcr::DdriobOutputEn::Obuf);
            #[cfg(any(
                feature = "target_coraz7",
                feature = "target_ebaz4205",
                feature = "target_redpitaya",
                feature = "target_kasli_soc",
            ))]
            let diff1_config = slcr::DdriobConfig::zeroed()
                .pullup_en(true);
            slcr.ddriob_diff0.write(diff0_config);
            slcr.ddriob_diff1.write(diff1_config);
            slcr.ddriob_clock.write(
                slcr::DdriobConfig::zeroed()
                    .output_en(slcr::DdriobOutputEn::Obuf)
            );
            unsafe {
                // Not documented in Technical Reference Manual
                slcr.ddriob_drive_slew_addr.write(0x0018C61C);
                slcr.ddriob_drive_slew_data.write(0x00F9861C);
                slcr.ddriob_drive_slew_diff.write(0x00F9861C);
                slcr.ddriob_drive_slew_clock.write(0x00F9861C);
            }
            #[cfg(any(
                feature = "target_coraz7",
                feature = "target_ebaz4205",
                feature = "target_redpitaya",
                feature = "target_kasli_soc",
            ))]
            slcr.ddriob_ddr_ctrl.modify(|_, w| w
                    .vref_int_en(false)
                    .vref_ext_en_lower(true)
                    .vref_ext_en_upper(false)
                    .refio_en(true)
            );
            #[cfg(feature = "target_zc706")]
            slcr.ddriob_ddr_ctrl.modify(|_, w| w
                    .vref_int_en(true)
                    .vref_sel(DdriobVrefSel::Vref0_75V)
                    .vref_ext_en_lower(false)
                    .vref_ext_en_upper(false)
            );
        });
    }
    fn configure(&mut self) {
        #[cfg(any(feature = "target_coraz7", feature = "target_kasli_soc"))]
        self.regs.dram_param0.write(
            regs::DramParam0::zeroed()
                .t_rc(0x1a)
                .t_rfc_min(0x9e)
                .post_selfref_gap_x32(0x10)
        );
        #[cfg(feature = "target_ebaz4205")]
        self.regs.dram_param0.write(
            regs::DramParam0::zeroed()
                .t_rc(0x1a)
                .t_rfc_min(0x56)
                .post_selfref_gap_x32(0x10)
        );
        #[cfg(feature = "target_redpitaya")]
        self.regs.dram_param0.write(
            regs::DramParam0::zeroed()
                .t_rc(0x1b)
                .t_rfc_min(0xa0)
                .post_selfref_gap_x32(0x10)
        );
        #[cfg(feature = "target_zc706")]
        self.regs.dram_param0.write(
            regs::DramParam0::zeroed()
                .t_rc(0x1b)
                .t_rfc_min(0x56)
                .post_selfref_gap_x32(0x10)
        );
        #[cfg(feature = "target_ebaz4205")]
        self.regs.dram_param1.modify(
            |_, w| w
                .t_faw(0x16)
                .t_ras_min(0x13)
        );
        #[cfg(feature = "target_redpitaya")]
        self.regs.dram_param1.modify(
            |_, w| w
                .wr2pre(0x12)
                .powerdown_to_x32(6)
                .t_faw(0x16)
                .t_ras_max(0x24)
                .t_ras_min(0x13)
                .t_cke(4)
        );
        self.regs.dram_param2.write(
            regs::DramParam2::zeroed()
                .write_latency(0x5)
                .rd2wr(0x7)
                .wr2rd(0xe)
                .t_xp(0x4)
                .pad_pd(0x0)
                .rd2pre(0x4)
                .t_rcd(0x7)
        );
        #[cfg(feature = "target_ebaz4205")]
        self.regs.dram_param3.modify(
            |_, w| w
                .t_rp(7)
        );
        #[cfg(feature = "target_redpitaya")]
        self.regs.dram_param3.modify(
            |_, w| w
                .t_ccd(4)
                .t_rrd(6)
                .refresh_margin(2)
                .t_rp(7)
                .refresh_to_x32(8)
                .mobile(false)
                .dfi_dram_clk_disable(false)
                .read_latency(7)
                .mode_ddr1_ddr2(true)
                .dis_pad_pd(false)
        );
        self.regs.dram_emr_mr.write(
            regs::DramEmrMr::zeroed()
                .mr(0x930)
                .emr(0x4)
        );
        #[cfg(any(
            feature = "target_coraz7",
            feature = "target_ebaz4205",
            feature = "target_redpitaya",
            feature = "target_kasli_soc",
        ))]
        self.regs.phy_configs[2].modify(
            |_, w| w.data_slice_in_use(false)
        );
        #[cfg(any(
            feature = "target_coraz7",
            feature = "target_ebaz4205",
            feature = "target_redpitaya",
            feature = "target_kasli_soc",
        ))]
        self.regs.phy_configs[3].modify(
            |_, w| w.data_slice_in_use(false)
        );
        self.regs.phy_cmd_timeout_rddata_cpt.modify(
            |_, w| w
                .rd_cmd_to_data(0x0)
                .wr_cmd_to_data(0x0)
                .we_to_re_delay(0x8)
                .rdc_fifo_rst_disable(false)
                .use_fixed_re(true)
                .rdc_fifo_rst_err_cnt_clr(false)
                .dis_phy_ctrl_rstn(false)
                .clk_stall_level(false)
                .gatelvl_num_of_dq0(0x7)
                .wrlvl_num_of_dq0(0x7)
        );
        self.regs.reg_2c.write(
            regs::Reg2C::zeroed()
                .wrlvl_max_x1024(0xfff)
                .rdlvl_max_x1024(0xfff)
                .twrlvl_max_error(false)
                .trdlvl_max_error(false)
                .dfi_wr_level_en(true)
                .dfi_rd_dqs_gate_level(true)
                .dfi_rd_data_eye_train(true)
        );
        self.regs.dfi_timing.write(
            regs::DfiTiming::zeroed()
                .rddata_en(0x6)
                .ctrlup_min(0x3)
                .ctrlup_max(0x40)
        );
        #[cfg(feature = "target_zc706")]
        self.regs.phy_init_ratios[3].write(
            regs::PhyInitRatio::zeroed()
                .wrlvl_init_ratio(0x21)
                .gatelvl_init_ratio(0xee)
        );
        #[cfg(any(
            feature = "target_coraz7",
            feature = "target_ebaz4205",
            feature = "target_kasli_soc"),
        )]
        self.regs.reg_64.modify(
            |_, w| w
                .phy_ctrl_slave_ratio(0x100)
                .phy_invert_clkout(true)
        );
        #[cfg(feature = "target_redpitaya")]
        self.regs.reg_64.modify(
            |_, w| w
                .phy_bl2(false)
                .phy_invert_clkout(true)
                .phy_sel_logic(false)
                .phy_ctrl_slave_ratio(0x100)
                .phy_ctrl_slave_force(false)
                .phy_ctrl_slave_delay(0)
                .phy_lpddr(false)
                .phy_cmd_latency(false)
        );
        self.regs.reg_65.write(
            regs::Reg65::zeroed()
                .wr_rl_delay(0x2)
                .rd_rl_delay(0x4)
                .dll_lock_diff(0xf)
                .use_wr_level(true)
                .use_rd_dqs_gate_level(true)
                .use_rd_data_eye_level(true)
                .dis_calib_rst(false)
                .ctrl_slave_delay(0x0)
        );
    }
    /// Reset DDR controller
    fn reset_ddrc<F: FnMut(&mut Self)>(&mut self, mut f: F) {
        #[cfg(feature = "target_zc706")]
        let width = regs::DataBusWidth::Width32bit;
        #[cfg(any(
            feature = "target_coraz7",
            feature = "target_ebaz4205",
            feature = "target_redpitaya",
            feature = "target_kasli_soc",
        ))]
        let width = regs::DataBusWidth::Width16bit;
        self.regs.ddrc_ctrl.modify(|_, w| w
            .soft_rstb(false)
            .powerdown_en(false)
            .data_bus_width(width)
        );
        f(self);
        #[cfg(feature = "target_zc706")]
        unsafe {
            // row/column address bits
            self.regs.dram_addr_map_bank.write(0x00000777);
            self.regs.dram_addr_map_col.write(0xFFF00000);
            self.regs.dram_addr_map_row.write(0x0F666666);
        }
        #[cfg(any(
            feature = "target_coraz7",
            feature = "target_ebaz4205",
            feature = "target_redpitaya",
            feature = "target_kasli_soc",
        ))]
        unsafe {
            // row/column address bits
            self.regs.dram_addr_map_bank.write(0x00000666);
            self.regs.dram_addr_map_col.write(0xFFFF0000);
            self.regs.dram_addr_map_row.write(0x0F555555);
        }
        self.regs.ddrc_ctrl.modify(|_, w| w
            .soft_rstb(true)
            .powerdown_en(false)
            .data_bus_width(width)
        );
        while self.status() == regs::ControllerStatus::Init {}
    }
    pub fn status(&self) -> regs::ControllerStatus {
        self.regs.mode_sts.read().operating_mode()
    }
    pub fn ptr<T>(&mut self) -> *mut T {
        0x0010_0000 as *mut _
    }
    /// actually there's 1 MB more but starting at 0x0000_0000
    /// overlaps with OCM.
    pub fn size(&self) -> usize {
        // DDR range ends at 0x3FFF_FFFF in the default SCU address
        // filtering address map
        #[cfg(feature = "target_zc706")]
        let megabytes = 1023;
        #[cfg(any(
            feature = "target_coraz7",
            feature = "target_redpitaya",
            feature = "target_kasli_soc",
        ))]
        let megabytes = 512;
        #[cfg(feature = "target_ebaz4205")]
        let megabytes = 256;
        megabytes * 1024 * 1024
    }
    pub fn memtest(&mut self) {
        let slice = unsafe {
            core::slice::from_raw_parts_mut(self.ptr(), self.size())
        };
        let patterns: &'static [u32] = &[0xffff_ffff, 0x5555_5555, 0xaaaa_aaaa, 0];
        let mut expected = None;
        for (i, pattern) in patterns.iter().enumerate() {
            info!("memtest phase {} (status: {:?})", i, self.status());
            for megabyte in 0..slice.len() / (1024 * 1024) {
                let start = megabyte * 1024 * 1024 / 4;
                let end = (megabyte + 1) * 1024 * 1024 / 4;
                for b in slice[start..end].iter_mut() {
                    expected.map(|expected| {
                        let read: u32 = *b;
                        if read != expected {
                            error!("{:08X}: expected {:08X}, read {:08X}", b as *mut _ as usize, expected, read);
                        }
                    });
                    *b = *pattern;
                }
                print!("\r{} MB", megabyte);
            }
            println!(" Ok");
            expected = Some(*pattern);
        }
    }
 }
--- a/libboard_zynq/src/ddr/regs.rs
+++ b/libboard_zynq/src/ddr/regs.rs
@ -0,0 +1,279 @@
 use volatile_register::{RO, RW};
 use libregister::{register, register_at, register_bit, register_bits, register_bits_typed};
 #[allow(unused)]
 #[derive(Clone, Copy)]
 #[repr(u8)]
 pub enum DataBusWidth {
    Width32bit = 0b00,
    Width16bit = 0b01,
 }
 #[derive(Debug, Clone, PartialEq)]
 #[repr(u8)]
 pub enum ControllerStatus {
    Init = 0,
    Normal = 1,
    Powerdown = 2,
    SelfRefresh = 3,
    Powerdown1 = 4,
    Powerdown2 = 5,
    Powerdown3 = 6,
    Powerdown4 = 7,
 }
 #[repr(C)]
 pub struct RegisterBlock {
    pub ddrc_ctrl: DdrcCtrl,
    pub two_rank_cfg: RW<u32>,
    pub hpr: RW<u32>,
    pub lpr: RW<u32>,
    pub wr: RW<u32>,
    pub dram_param0: DramParam0,
    pub dram_param1: DramParam1,
    pub dram_param2: DramParam2,
    pub dram_param3: DramParam3,
    pub dram_param4: RW<u32>,
    pub dram_init_param: RW<u32>,
    pub dram_emr: RW<u32>,
    pub dram_emr_mr: DramEmrMr,
    pub dram_burst8_rdwr: Burst8Rdwr,
    pub dram_disable_dq: RW<u32>,
    pub dram_addr_map_bank: RW<u32>,
    pub dram_addr_map_col: RW<u32>,
    pub dram_addr_map_row: RW<u32>,
    pub dram_odt: RW<u32>,
    pub phy_dbg: RW<u32>,
    pub phy_cmd_timeout_rddata_cpt: PhyCmdTimeoutRddataCpt,
    pub mode_sts: ModeStsReg,
    pub dll_calib: RW<u32>,
    pub odt_delay_hold: RW<u32>,
    pub ctrl1: RW<u32>,
    pub ctrl2: RW<u32>,
    pub ctrl3: RW<u32>,
    pub ctrl4: RW<u32>,
    _unused0: [RO<u32>; 2],
    pub ctrl5: RW<u32>,
    pub ctrl6: RW<u32>,
    _unused1: [RO<u32>; 8],
    pub che_refresh_timer01: RW<u32>,
    pub che_t_zq: CheTZq,
    pub che_t_zq_short_interval: RW<u32>,
    pub deep_pwrdwn: RW<u32>,
    pub reg_2c: Reg2C,
    pub reg_2d: RW<u32>,
    pub dfi_timing: DfiTiming,
    _unused2: [RO<u32>; 2],
    pub che_ecc_control_offset: RW<u32>,
    pub che_corr_ecc_log_offset: RW<u32>,
    pub che_corr_ecc_addr_offset: RW<u32>,
    pub che_corr_ecc_data_31_0_offset: RW<u32>,
    pub che_corr_ecc_data_63_32_offset: RW<u32>,
    pub che_corr_ecc_data_71_64_offset: RW<u32>,
    pub che_uncorr_ecc_log_offset: RW<u32>,
    pub che_uncorr_ecc_addr_offset: RW<u32>,
    pub che_uncorr_ecc_data_31_0_offset: RW<u32>,
    pub che_uncorr_ecc_data_63_32_offset: RW<u32>,
    pub che_uncorr_ecc_data_71_64_offset: RW<u32>,
    pub che_ecc_stats_offset: RW<u32>,
    pub ecc_scrub: RW<u32>,
    pub che_ecc_corr_bit_mask_31_0_offset: RW<u32>,
    pub che_ecc_corr_bit_mask_63_32_offset: RW<u32>,
    _unused3: [RO<u32>; 5],
    pub phy_rcvr_enable: RW<u32>,
    pub phy_configs: [PhyConfig; 4],
    _unused4: RO<u32>,
    pub phy_init_ratios: [PhyInitRatio; 4],
    _unused5: RO<u32>,
    pub phy_rd_dqs_cfg0: RW<u32>,
    pub phy_rd_dqs_cfg1: RW<u32>,
    pub phy_rd_dqs_cfg2: RW<u32>,
    pub phy_rd_dqs_cfg3: RW<u32>,
    _unused6: RO<u32>,
    pub phy_wr_dqs_cfg0: RW<u32>,
    pub phy_wr_dqs_cfg1: RW<u32>,
    pub phy_wr_dqs_cfg2: RW<u32>,
    pub phy_wr_dqs_cfg3: RW<u32>,
    _unused7: RO<u32>,
    pub phy_we_cfg0: RW<u32>,
    pub phy_we_cfg1: RW<u32>,
    pub phy_we_cfg2: RW<u32>,
    pub phy_we_cfg3: RW<u32>,
    _unused8: RO<u32>,
    pub wr_data_slv0: RW<u32>,
    pub wr_data_slv1: RW<u32>,
    pub wr_data_slv2: RW<u32>,
    pub wr_data_slv3: RW<u32>,
    _unused9: RO<u32>,
    pub reg_64: Reg64,
    pub reg_65: Reg65,
    _unused10: [RO<u32>; 3],
    pub reg69_6a0: RW<u32>,
    pub reg69_6a1: RW<u32>,
    _unused11: RO<u32>,
    pub reg6c_6d2: RW<u32>,
    pub reg6c_6d3: RW<u32>,
    pub reg6e_710: RW<u32>,
    pub reg6e_711: RW<u32>,
    pub reg6e_712: RW<u32>,
    pub reg6e_713: RW<u32>,
    pub phy_dll_sts0: RW<u32>,
    _unused12: RO<u32>,
    pub phy_dll_sts1: RW<u32>,
    pub phy_dll_sts2: RW<u32>,
    pub phy_dll_sts3: RW<u32>,
    _unused13: RO<u32>,
    pub dll_lock_sts: RW<u32>,
    pub phy_ctrl_sts: RW<u32>,
    pub phy_ctrl_sts2: RW<u32>,
    _unused14: [RO<u32>; 5],
    pub axi_id: RW<u32>,
    pub page_mask: RW<u32>,
    pub axi_priority_wr_ports: [RW<u32>; 4],
    pub axi_priority_rd_ports: [AxiPriorityRd; 4],
    _unused15: [RO<u32>; 27],
    pub excl_access_cfg0: RW<u32>,
    pub excl_access_cfg1: RW<u32>,
    pub excl_access_cfg2: RW<u32>,
    pub excl_access_cfg3: RW<u32>,
    pub mode_reg_read: RW<u32>,
    pub lpddr_ctrl0: RW<u32>,
    pub lpddr_ctrl1: RW<u32>,
    pub lpddr_ctrl2: RW<u32>,
    pub lpddr_ctrl3: RW<u32>,
 }
 register_at!(RegisterBlock, 0xF8006000, ddrc);
 register!(ddrc_ctrl, DdrcCtrl, RW, u32);
 register_bit!(ddrc_ctrl,
              /// `false` resets controller, `true` continues
              soft_rstb, 0);
 register_bit!(ddrc_ctrl, powerdown_en, 1);
 register_bits_typed!(ddrc_ctrl, data_bus_width, u8, DataBusWidth, 2, 3);
 // (ddrc_ctrl) ...
 register!(dram_param0, DramParam0, RW, u32);
 register_bits!(dram_param0, t_rc, u8, 0, 5);
 register_bits!(dram_param0, t_rfc_min, u8, 6, 13);
 register_bits!(dram_param0, post_selfref_gap_x32, u8, 14, 20);
 register!(dram_param1, DramParam1, RW, u32);
 register_bits!(dram_param1, wr2pre, u8, 0, 4);
 register_bits!(dram_param1, powerdown_to_x32, u8, 5, 9);
 register_bits!(dram_param1, t_faw, u8, 10, 15);
 register_bits!(dram_param1, t_ras_max, u8, 16, 21);
 register_bits!(dram_param1, t_ras_min, u8, 22, 26);
 register_bits!(dram_param1, t_cke, u8, 28, 31);
 register!(dram_param2, DramParam2, RW, u32);
 register_bits!(dram_param2, write_latency, u8, 0, 4);
 register_bits!(dram_param2, rd2wr, u8, 5, 9);
 register_bits!(dram_param2, wr2rd, u8, 10, 14);
 register_bits!(dram_param2, t_xp, u8, 15, 19);
 register_bits!(dram_param2, pad_pd, u8, 20, 22);
 register_bits!(dram_param2, rd2pre, u8, 23, 27);
 register_bits!(dram_param2, t_rcd, u8, 28, 31);
 register!(dram_param3, DramParam3, RW, u32);
 register_bits!(dram_param3, t_ccd, u8, 2, 4);
 register_bits!(dram_param3, t_rrd, u8, 5, 7);
 register_bits!(dram_param3, refresh_margin, u8, 8, 11);
 register_bits!(dram_param3, t_rp, u8, 12, 15);
 register_bits!(dram_param3, refresh_to_x32, u8, 16, 20);
 register_bit!(dram_param3, sdram, 21);
 register_bit!(dram_param3, mobile, 22);
 register_bit!(dram_param3, dfi_dram_clk_disable, 23);
 register_bits!(dram_param3, read_latency, u8, 24, 28);
 register_bit!(dram_param3, mode_ddr1_ddr2, 29);
 register_bit!(dram_param3, dis_pad_pd, 30);
 register!(dram_emr_mr, DramEmrMr, RW, u32);
 register_bits!(dram_emr_mr, mr, u16, 0, 15);
 register_bits!(dram_emr_mr, emr, u16, 16, 31);
 register!(burst8_rdwr, Burst8Rdwr, RW, u32);
 register_bits!(burst8_rdwr, burst_rdwr, u8, 0, 3);
 register_bits!(burst8_rdwr, pre_cke_x1024, u16, 4, 13);
 register_bits!(burst8_rdwr, post_cke_x1024, u16, 16, 25);
 register_bit!(burst8_rdwr, burstchop, 28);
 register!(phy_cmd_timeout_rddata_cpt, PhyCmdTimeoutRddataCpt, RW, u32);
 register_bits!(phy_cmd_timeout_rddata_cpt, rd_cmd_to_data, u8, 0, 3);
 register_bits!(phy_cmd_timeout_rddata_cpt, wr_cmd_to_data, u8, 4, 7);
 register_bits!(phy_cmd_timeout_rddata_cpt, we_to_re_delay, u8, 8, 11);
 register_bit!(phy_cmd_timeout_rddata_cpt, rdc_fifo_rst_disable, 15);
 register_bit!(phy_cmd_timeout_rddata_cpt, use_fixed_re, 16);
 register_bit!(phy_cmd_timeout_rddata_cpt, rdc_fifo_rst_err_cnt_clr, 17);
 register_bit!(phy_cmd_timeout_rddata_cpt, dis_phy_ctrl_rstn, 18);
 register_bit!(phy_cmd_timeout_rddata_cpt, clk_stall_level, 19);
 register_bits!(phy_cmd_timeout_rddata_cpt, gatelvl_num_of_dq0, u8, 24, 27);
 register_bits!(phy_cmd_timeout_rddata_cpt, wrlvl_num_of_dq0, u8, 28, 31);
 register!(che_t_zq, CheTZq, RW, u32);
 register_bit!(che_t_zq, dis_auto_zq, 0);
 register_bit!(che_t_zq, ddr3, 1);
 register_bits!(che_t_zq, t_mod, u8, 2, 11);
 register_bits!(che_t_zq, t_zq_long_nop, u16, 12, 21);
 register_bits!(che_t_zq, t_zq_short_nop, u16, 22, 31);
 register!(reg_2c, Reg2C, RW, u32);
 register_bits!(reg_2c, wrlvl_max_x1024, u16, 0, 11);
 register_bits!(reg_2c, rdlvl_max_x1024, u16, 12, 23);
 register_bit!(reg_2c, twrlvl_max_error, 24);
 register_bit!(reg_2c, trdlvl_max_error, 25);
 register_bit!(reg_2c, dfi_wr_level_en, 26);
 register_bit!(reg_2c, dfi_rd_dqs_gate_level, 27);
 register_bit!(reg_2c, dfi_rd_data_eye_train, 28);
 register!(dfi_timing, DfiTiming, RW, u32);
 register_bits!(dfi_timing, rddata_en, u8, 0, 4);
 register_bits!(dfi_timing, ctrlup_min, u16, 5, 14);
 register_bits!(dfi_timing, ctrlup_max, u16, 15, 24);
 register!(phy_config, PhyConfig, RW, u32);
 register_bit!(phy_config, data_slice_in_use, 0);
 register_bit!(phy_config, rdlvl_inc_mode, 1);
 register_bit!(phy_config, gatelvl_inc_mode, 2);
 register_bit!(phy_config, wrlvl_inc_mode, 3);
 register_bits!(phy_config, dq_offset, u8, 24, 30);
 register!(phy_init_ratio, PhyInitRatio, RW, u32);
 register_bits!(phy_init_ratio, wrlvl_init_ratio, u16, 0, 9);
 register_bits!(phy_init_ratio, gatelvl_init_ratio, u16, 10, 19);
 register!(reg_64, Reg64, RW, u32);
 register_bit!(reg_64, phy_bl2, 1);
 register_bit!(reg_64, phy_invert_clkout, 7);
 register_bit!(reg_64, phy_sel_logic, 9);
 register_bits!(reg_64, phy_ctrl_slave_ratio, u16, 10, 19);
 register_bit!(reg_64, phy_ctrl_slave_force, 20);
 register_bits!(reg_64, phy_ctrl_slave_delay, u8, 21, 27);
 register_bit!(reg_64, phy_lpddr, 29);
 register_bit!(reg_64, phy_cmd_latency, 30);
 register!(reg_65, Reg65, RW, u32);
 register_bits!(reg_65, wr_rl_delay, u8, 0, 4);
 register_bits!(reg_65, rd_rl_delay, u8, 5, 9);
 register_bits!(reg_65, dll_lock_diff, u8, 10, 13);
 register_bit!(reg_65, use_wr_level, 14);
 register_bit!(reg_65, use_rd_dqs_gate_level, 15);
 register_bit!(reg_65, use_rd_data_eye_level, 16);
 register_bit!(reg_65, dis_calib_rst, 17);
 register_bits!(reg_65, ctrl_slave_delay, u8, 18, 19);
 // Controller operation mode status
 register!(mode_sts_reg,
          ModeStsReg, RO, u32);
 register_bits_typed!(mode_sts_reg, operating_mode, u8, ControllerStatus, 0, 2);
 // (mode_sts_reg) ...
 register!(axi_priority_rd, AxiPriorityRd, RW, u32);
 register_bits!(axi_priority_rd, arb_pri_rd_portn, u16, 0, 9);
 register_bit!(axi_priority_rd, arb_disable_aging_rd_portn, 16);
 register_bit!(axi_priority_rd, arb_disable_urgent_rd_portn, 17);
 register_bit!(axi_priority_rd, arb_disable_page_match_rd_portn, 18);
 register_bit!(axi_priority_rd, arb_set_hpr_rd_portn, 19);
--- a/libboard_zynq/src/devc/mod.rs
+++ b/libboard_zynq/src/devc/mod.rs
@ -0,0 +1,341 @@
 use super::time::Milliseconds;
 use crate::slcr;
 use embedded_hal::timer::CountDown;
 use libcortex_a9::cache;
 use libregister::*;
 use log::{debug, trace};
 mod regs;
 pub struct DevC {
    regs: &'static mut regs::RegisterBlock,
    enabled: bool,
    count_down: super::timer::global::CountDown<Milliseconds>,
    timeout_ms: Milliseconds,
 }
 /// DMA transfer type for PCAP
 /// All insecure, we do not implement encrypted transfer
 #[derive(PartialEq, Clone, Copy)]
 pub enum TransferType {
    PcapWrite,
    PcapReadback,
    ConcurrentReadWrite,
 }
 pub enum TransferTarget<'a> {
    /// From/To PL, with length in bytes.
    PL(u32),
    /// Source target, immutable.
    SliceSrc(&'a [u8]),
    /// Last source target, immutable.
    SliceSrcLast(&'a [u8]),
    /// Destination target, mutable.
    SliceDest(&'a mut [u8]),
    /// Last destination target, mutable.
    SliceDestLast(&'a mut [u8]),
 }
 #[derive(PartialEq, Clone, Copy, Debug)]
 pub enum DevcError {
    NotInitialized,
    ResetTimeout,
    DmaBusy,
    DmaTimeout,
    DoneTimeout,
    Unknown(u32),
 }
 impl core::fmt::Display for DevcError {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        use DevcError::*;
        match self {
            NotInitialized => write!(f, "DevC driver not initialized properly."),
            ResetTimeout => write!(f, "DevC driver reset timeout."),
            DmaBusy => write!(f, "DevC driver DMA busy."),
            DmaTimeout => write!(f, "DevC driver DMA timeout."),
            DoneTimeout => write!(
                f,
                "FPGA DONE signal timeout. Check if the bitstream is correct."
            ),
            Unknown(reg) => write!(f, "Unknown error, interrupt status register = 0x{:0X}", reg),
        }
    }
 }
 impl DevC {
    /// Create a new DevC peripheral handle with default timeout = 500ms.
    pub fn new() -> Self {
        Self::new_timeout(Milliseconds(500))
    }
    /// Create a new DevC peripheral handle.
    /// `timeout_ms`: timeout for operations like initialize and DMA transfer.
    pub fn new_timeout(timeout_ms: Milliseconds) -> Self {
        DevC {
            regs: regs::RegisterBlock::devc(),
            enabled: false,
            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown(),
            timeout_ms,
        }
    }
    /// Enable the devc driver, must be called before `program` or
    /// `start_dma_transaction`.
    pub fn enable(&mut self) {
        const UNLOCK_PATTERN: u32 = 0x757BDF0D;
        unsafe {
            // unlock register with magic pattern
            self.regs.unlock.write(UNLOCK_PATTERN);
        }
        self.regs
            .control
            .modify(|_, w| w.pcap_mode(true).pcap_pr(true));
        self.regs
            .int_mask
            .write(self::regs::int_mask::Write { inner: 0xFFFFFFFF });
        self.clear_interrupts();
        self.enabled = true;
    }
    /// Disable the devc driver.
    /// `enable` has to be called before further `program` or
    /// `start_dma_transaction`.
    pub fn disable(&mut self) {
        self.regs
            .control
            .modify(|_, w| w.pcap_mode(false).pcap_pr(false));
        self.enabled = false;
    }
    /// Check if the FPGA programming is done.
    pub fn is_done(&self) -> bool {
        // Note: contrary to what the TRM says, this appears to be simply the
        // state of the DONE signal.
        self.regs.int_sts.read().ixr_pcfg_done()
    }
    /// Wait on a certain condition with hardcoded timeout.
    fn wait_condition<F: Fn(&mut Self) -> bool>(
        &mut self,
        fun: F,
        err: DevcError,
    ) -> Result<(), DevcError> {
        self.count_down.start(self.timeout_ms);
        while let Err(nb::Error::WouldBlock) = self.count_down.wait() {
            if fun(self) {
                return Ok(());
            } else if self.has_error() {
                return Err(DevcError::Unknown(self.regs.int_sts.read().inner));
            }
        }
        Err(err)
    }
    /// Program the FPGA.
    /// Note that the user should make sure that the bitstream loaded is
    /// correct.
    pub fn program(&mut self, src: &[u8]) -> Result<(), DevcError> {
        if !self.enabled {
            panic!("Attempting to use devc when it is not enabled");
        }
        self.clear_interrupts();
        debug!("Invalidate DCache for bitstream buffer");
        cache::dcci_slice(src);
        debug!("Init preload FPGA");
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.init_preload_fpga();
        });
        debug!("Toggling PROG_B");
        // set PCFG_PROG_B to high low high
        self.regs.control.modify(|_, w| w.pcfg_prog_b(true));
        self.regs.control.modify(|_, w| w.pcfg_prog_b(false));
        // wait until init is false
        self.wait_condition(
            |s| !s.regs.status.read().pcfg_init(),
            DevcError::ResetTimeout,
        )?;
        self.regs.control.modify(|_, w| w.pcfg_prog_b(true));
        // wait until init is true
        self.wait_condition(
            |s| s.regs.status.read().pcfg_init(),
            DevcError::ResetTimeout,
        )?;
        self.regs.int_sts.write(
            self::regs::IntSts::zeroed()
                .pss_cfg_reset_b_int(true)
                .ixr_pcfg_cfg_rst(true),
        );
        self.dma_transfer(
            TransferTarget::SliceSrcLast(src),
            TransferTarget::PL(src.len() as u32),
            TransferType::PcapWrite,
        )?;
        debug!("Waiting for done");
        self.wait_condition(|s| s.is_done(), DevcError::DoneTimeout)?;
        debug!("Init postload FPGA");
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.init_postload_fpga();
        });
        Ok(())
    }
    /// Initiate DMA transaction
    /// This function only sets the src and dest registers, and should not be used directly.
    fn initiate_dma(&mut self, src: TransferTarget, dest: TransferTarget) {
        use TransferTarget::*;
        const INVALID_ADDR: u32 = 0xFFFFFFFF;
        if let (PL(_), PL(_)) = (&src, &dest) {
            panic!("Only one of src/dest can be PL");
        }
        let (src_addr, src_len): (u32, u32) = match src {
            PL(l) => (INVALID_ADDR, l / 4),
            SliceSrc(s) => (s.as_ptr() as u32, s.len() as u32 / 4),
            SliceDest(s) => (s.as_ptr() as u32, s.len() as u32 / 4),
            SliceSrcLast(s) => ((s.as_ptr() as u32) | 0x01, s.len() as u32 / 4),
            SliceDestLast(s) => ((s.as_ptr() as u32) | 0x01, s.len() as u32 / 4),
        };
        let (dest_addr, dest_len): (u32, u32) = match dest {
            PL(l) => (INVALID_ADDR, l / 4),
            SliceDest(s) => (s.as_ptr() as u32, s.len() as u32 / 4),
            SliceDestLast(s) => ((s.as_ptr() as u32) | 0x01, s.len() as u32 / 4),
            SliceSrc(_) | SliceSrcLast(_) => {
                panic!("Destination cannot be SliceSrc/SliceSrcLast, it must be mutable.")
            }
        };
        self.regs.dma_src_addr.modify(|_, w| w.src_addr(src_addr));
        self.regs
            .dma_dest_addr
            .modify(|_, w| w.dest_addr(dest_addr));
        self.regs.dma_src_len.modify(|_, w| w.dma_len(src_len));
        self.regs.dma_dest_len.modify(|_, w| w.dma_len(dest_len));
    }
    /// Blocking DMA transfer
    /// ## Note
    /// This is blocking because there seems to be no other way to guarantee
    /// safety, and I don't think requiring static is a solution here due to the
    /// large buffer size.
    /// See https://docs.rust-embedded.org/embedonomicon/dma.html for details.
    ///
    /// The following checks are implemented in runtime (panic).
    /// * Dest would *NOT* accept src type, as the slices are immutable.
    /// * At most one of src and dest can be PL type.
    pub fn dma_transfer(
        &mut self,
        src: TransferTarget,
        dest: TransferTarget,
        transfer_type: TransferType,
    ) -> Result<(), DevcError> {
        if !self.enabled {
            panic!("Attempting to use devc when it is not enabled");
        }
        if self.regs.status.read().dma_cmd_q_f() {
            return Err(DevcError::DmaBusy);
        }
        if transfer_type != TransferType::ConcurrentReadWrite
            && !self.regs.status.read().pcfg_init()
        {
            return Err(DevcError::NotInitialized);
        }
        match &transfer_type {
            TransferType::PcapReadback => {
                // clear internal PCAP loopback
                self.regs.mctrl.modify(|_, w| w.pcap_lpbk(false));
                // send READ frame command
                self.initiate_dma(src, TransferTarget::PL(0));
                // wait until DMA done
                self.wait_dma_transfer_complete()?;
                // initiate the DMA write
                self.initiate_dma(TransferTarget::PL(0), dest);
            }
            TransferType::PcapWrite | TransferType::ConcurrentReadWrite => {
                self.regs
                    .mctrl
                    .modify(|_, w| w.pcap_lpbk(transfer_type == TransferType::ConcurrentReadWrite));
                // PCAP data transmitted every clock
                self.regs.control.modify(|_, w| w.pcap_rate_en(false));
                self.initiate_dma(src, dest);
            }
        }
        self.wait_dma_transfer_complete()?;
        Ok(())
    }
    fn wait_dma_transfer_complete(&mut self) -> Result<(), DevcError> {
        trace!("Wait for DMA done");
        self.wait_condition(
            |s| s.regs.int_sts.read().ixr_dma_done(),
            DevcError::DmaTimeout,
        )?;
        self.regs
            .int_sts
            .write(self::regs::IntSts::zeroed().ixr_dma_done(true));
        Ok(())
    }
    /// Dump useful registers for devc block.
    pub fn dump_registers(&self) {
        debug!("Mctrl: 0x{:0X}", self.regs.mctrl.read().inner);
        debug!("Control: 0x{:0X}", self.regs.control.read().inner);
        debug!("Status: 0x{:0X}", self.regs.status.read().inner);
        debug!("INT STS: 0x{:0X}", self.regs.int_sts.read().inner);
    }
    /// Clear interrupt status for devc.
    pub fn clear_interrupts(&mut self) {
        self.regs.int_sts.modify(|_, w| {
            w.pss_gts_usr_b_int(true)
                .pss_fst_cfg_b_int(true)
                .pss_gpwrdwn_b_int(true)
                .pss_gts_cfg_b_int(true)
                .pss_cfg_reset_b_int(true)
                .ixr_axi_wto(true)
                .ixr_axi_werr(true)
                .ixr_axi_rto(true)
                .ixr_axi_rerr(true)
                .ixr_rx_fifo_ov(true)
                .ixr_wr_fifo_lvl(true)
                .ixr_rd_fifo_lvl(true)
                .ixr_dma_cmd_err(true)
                .ixr_dma_q_ov(true)
                .ixr_dma_done(true)
                .ixr_d_p_done(true)
                .ixr_p2d_len_err(true)
                .ixr_pcfg_hmac_err(true)
                .ixr_pcfg_seu_err(true)
                .ixr_pcfg_por_b(true)
                .ixr_pcfg_cfg_rst(true)
                .ixr_pcfg_done(true)
                .ixr_pcfg_init_pe(true)
                .ixr_pcfg_init_ne(true)
        })
    }
    fn has_error(&self) -> bool {
        let status = self.regs.int_sts.read();
        status.ixr_axi_wto()
            || status.ixr_axi_werr()
            || status.ixr_axi_rto()
            || status.ixr_axi_rerr()
            || status.ixr_rx_fifo_ov()
            || status.ixr_dma_cmd_err()
            || status.ixr_dma_q_ov()
            || status.ixr_p2d_len_err()
    }
 }
--- a/libboard_zynq/src/devc/regs.rs
+++ b/libboard_zynq/src/devc/regs.rs
@ -0,0 +1,213 @@
 use libregister::{
    register, register_at,
    register_bit, register_bits, register_bits_typed,
 };
 use volatile_register::WO;
 #[repr(C)]
 pub struct RegisterBlock {
    pub control: Control,
    pub lock: Lock,
    pub cfg: Cfg,
    pub int_sts: IntSts,
    pub int_mask: IntMask,
    pub status: Status,
    pub dma_src_addr: DmaSrcAddr,
    pub dma_dest_addr: DmaDestAddr,
    pub dma_src_len: DmaSrcLen,
    pub dma_dest_len: DmaDestLen,
    unused0: u32,
    pub multiboot_addr: MultibootAddr,
    unused1: u32,
    pub unlock: WO<u32>,
    unused2: [u32; 18],
    pub mctrl: MCtrl,
    unused3: [u32; 31],
    pub xadcif_cfg: XADCIfCfg,
    pub xadcif_int_sts: XADCIfIntSts,
    pub xadcif_int_mask: XADCIfIntMask,
    pub xadcif_msts: XADCIf_Msts,
    pub xadcif_cmdfifo: XADCIf_CmdFIFO,
    pub xadcif_rdfifo: XADCIf_RdFIFO,
    pub xadcif_mctl: XADCIf_MCtl,
 }
 register_at!(RegisterBlock, 0xF8007000, devc);
 register!(control, Control, RW, u32);
 register_bit!(control,  force_rst, 31);
 register_bit!(control,  pcfg_prog_b, 30);
 register_bit!(control,  pcfg_pro_cnt_4k, 29);
 register_bit!(control,  pcap_pr, 27);
 register_bit!(control,  pcap_mode, 26);
 register_bit!(control,  pcap_rate_en, 25);
 register_bit!(control,  multiboot_en, 24);
 register_bit!(control,  jtag_chain_dis, 23);
 register_bit!(control,  pcfg_aes_fuse, 12);
 register_bits!(control, pcfg_aes_en, u8, 9, 11);
 register_bit!(control,  seu_en, 8);
 register_bit!(control,  sec_en, 7);
 register_bit!(control,  spniden, 6);
 register_bit!(control,  spiden, 5);
 register_bit!(control,  niden, 4);
 register_bit!(control,  dbgen, 3);
 register_bits!(control, dap_en, u8, 0, 2);
 register!(lock, Lock, RW, u32);
 register_bit!(lock, aes_fuse_lock, 4);
 register_bit!(lock, aes_en, 3);
 register_bit!(lock, seu, 2);
 register_bit!(lock, sec, 1);
 register_bit!(lock, dbg, 0);
 register!(cfg, Cfg, RW, u32);
 #[allow(unused)]
 #[repr(u8)]
 pub enum RFifoTh {
    OneFourthFull    = 0b00, // One fourth full for read
    HalfFull         = 0b01, // Half full for read
    ThreeFourthFull  = 0b10, // Three fourth full for read
    Full             = 0b11, // Full for read
 }
 register_bits_typed!(cfg, rfifo_th, u8, RFifoTh, 10, 11);
 #[allow(unused)]
 #[repr(u8)]
 pub enum WFifoTh {
    OneFourthEmpty   = 0b00, // One fourth empty for write
    HalfEmpty        = 0b01, // Half empty for write
    ThreeFourthEmpty = 0b10, // Three fourth empty for write
    Empty            = 0b11, // Empty for write
 }
 register_bits_typed!(cfg, wfifo_th, u8, WFifoTh, 8, 9);
 register_bit!(cfg, rclk_edge, 7);
 register_bit!(cfg, wclk_edge, 6);
 register_bit!(cfg, disable_src_inc, 5);
 register_bit!(cfg, disable_dst_inc, 4);
 register!(int_sts, IntSts, RW, u32);
 register_bit!(int_sts, pss_gts_usr_b_int, 31);
 register_bit!(int_sts, pss_fst_cfg_b_int, 30);
 register_bit!(int_sts, pss_gpwrdwn_b_int, 29);
 register_bit!(int_sts, pss_gts_cfg_b_int, 28);
 register_bit!(int_sts, pss_cfg_reset_b_int, 27);
 register_bit!(int_sts, ixr_axi_wto, 23);
 register_bit!(int_sts, ixr_axi_werr, 22);
 register_bit!(int_sts, ixr_axi_rto, 21);
 register_bit!(int_sts, ixr_axi_rerr, 20);
 register_bit!(int_sts, ixr_rx_fifo_ov, 18);
 register_bit!(int_sts, ixr_wr_fifo_lvl, 17);
 register_bit!(int_sts, ixr_rd_fifo_lvl, 16);
 register_bit!(int_sts, ixr_dma_cmd_err, 15);
 register_bit!(int_sts, ixr_dma_q_ov, 14);
 register_bit!(int_sts, ixr_dma_done, 13);
 register_bit!(int_sts, ixr_d_p_done, 12);
 register_bit!(int_sts, ixr_p2d_len_err, 11);
 register_bit!(int_sts, ixr_pcfg_hmac_err, 6);
 register_bit!(int_sts, ixr_pcfg_seu_err, 5);
 register_bit!(int_sts, ixr_pcfg_por_b, 4);
 register_bit!(int_sts, ixr_pcfg_cfg_rst, 3);
 register_bit!(int_sts, ixr_pcfg_done, 2);
 register_bit!(int_sts, ixr_pcfg_init_pe, 1);
 register_bit!(int_sts, ixr_pcfg_init_ne, 0);
 register!(int_mask, IntMask, RW, u32);
 register_bit!(int_mask, m_pss_gts_usr_b_int, 31);
 register_bit!(int_mask, m_pss_fst_cfg_b_int, 30);
 register_bit!(int_mask, m_pss_gpwrdwn_b_int, 29);
 register_bit!(int_mask, m_pss_gts_cfg_b_int, 28);
 register_bit!(int_mask, m_pss_cfg_reset_b_int, 27);
 register_bit!(int_mask, ixr_axi_wto, 23);
 register_bit!(int_mask, ixr_axi_werr, 22);
 register_bit!(int_mask, ixr_axi_rto, 21);
 register_bit!(int_mask, ixr_axi_rerr, 20);
 register_bit!(int_mask, ixr_rx_fifo_ov, 18);
 register_bit!(int_mask, ixr_wr_fifo_lvl, 17);
 register_bit!(int_mask, ixr_rd_fifo_lvl, 16);
 register_bit!(int_mask, ixr_dma_cmd_err, 15);
 register_bit!(int_mask, ixr_dma_q_ov, 14);
 register_bit!(int_mask, ixr_dma_done, 13);
 register_bit!(int_mask, ixr_d_p_done, 12);
 register_bit!(int_mask, ixr_p2d_len_err, 11);
 register_bit!(int_mask, ixr_pcfg_hmac_err, 6);
 register_bit!(int_mask, ixr_pcfg_seu_err, 5);
 register_bit!(int_mask, ixr_pcfg_por_b, 4);
 register_bit!(int_mask, ixr_pcfg_cfg_rst, 3);
 register_bit!(int_mask, ixr_pcfg_done, 2);
 register_bit!(int_mask, ixr_pcfg_init_pe, 1);
 register_bit!(int_mask, ixr_pcfg_init_ne, 0);
 register!(status, Status, RO, u32);
 register_bit!(status, dma_cmd_q_f, 31);
 register_bit!(status, dma_cmd_q_e, 30);
 register_bits!(status, dma_done_cnt, u8, 28, 29);
 register_bits!(status, rx_fifo_lvl, u8, 20, 24);
 register_bits!(status, tx_fifo_lvl, u8, 12, 18);
 register_bit!(status, pss_gts_usr_b, 11);
 register_bit!(status, pss_fst_cfg_b, 10);
 register_bit!(status, pss_gpwrdwn_b, 9);
 register_bit!(status, pss_gts_cfg_b, 8);
 register_bit!(status, secure_rst, 7);
 register_bit!(status, illegal_apb_access , 6);
 register_bit!(status, pss_cfg_reset_b, 5);
 register_bit!(status, pcfg_init, 4);
 register_bit!(status, efuse_sw_reserve, 3);
 register_bit!(status, efuse_sec_en, 2);
 register_bit!(status, efuse_jtag_dis, 1);
 register!(dma_src_addr, DmaSrcAddr, RW, u32);
 register_bits!(dma_src_addr, src_addr, u32, 0, 31);
 register!(dma_dest_addr, DmaDestAddr, RW, u32);
 register_bits!(dma_dest_addr, dest_addr, u32, 0, 31);
 register!(dma_src_len, DmaSrcLen, RW, u32);
 register_bits!(dma_src_len, dma_len, u32, 0, 26);
 register!(dma_dest_len, DmaDestLen, RW, u32);
 register_bits!(dma_dest_len, dma_len, u32, 0, 26);
 register!(multiboot_addr, MultibootAddr, RW, u32);
 register_bits!(multiboot_addr, multiboot_addr, u8, 0, 12);
 register!(mctrl, MCtrl, RW, u32);
 register_bits!(mctrl, ps_version, u8, 28, 31);
 register_bit!(mctrl, pcfg_por_b, 8);
 register_bit!(mctrl, pcap_lpbk, 4);
 register!(xadcif_cfg, XADCIfCfg, RW, u32);
 register_bit!(xadcif_cfg, enable, 31);
 register_bits!(xadcif_cfg, cfifoth, u8, 20, 23);
 register_bits!(xadcif_cfg, dfifoth, u8, 16, 19);
 register_bit!(xadcif_cfg, wedge, 13);
 register_bit!(xadcif_cfg, redge, 13);
 register_bits!(xadcif_cfg, tckrate, u8, 8, 9);
 register_bits!(xadcif_cfg, igap, u8, 0, 4);
 register!(xadcif_int_sts, XADCIfIntSts, RW, u32);
 register_bit!(xadcif_int_sts, cfifo_lth, 9);
 register_bit!(xadcif_int_sts, dfifo_gth, 8);
 register_bit!(xadcif_int_sts, ot, 7);
 register_bits!(xadcif_int_sts, alm, u8, 0, 6);
 register!(xadcif_int_mask, XADCIfIntMask, RW, u32);
 register_bit!(xadcif_int_mask, m_cfifo_lth, 9);
 register_bit!(xadcif_int_mask, m_dfifo_gth, 8);
 register_bit!(xadcif_int_mask, m_ot, 7);
 register_bits!(xadcif_int_mask, m_alm, u8, 0, 6);
 register!(xadcif_msts, XADCIf_Msts, RO, u32);
 register_bits!(xadcif_msts, cfifo_lvl, u8, 16, 19);
 register_bits!(xadcif_msts, dfifo_lvl, u8, 12, 15);
 register_bit!(xadcif_msts, cfifof, 11);
 register_bit!(xadcif_msts, cfifoe, 10);
 register_bit!(xadcif_msts, dfifof, 9);
 register_bit!(xadcif_msts, dfifoe, 8);
 register_bit!(xadcif_msts, ot, 7);
 register_bits!(xadcif_msts, alm, u8, 0, 6);
 register!(xadcif_cmdfifo, XADCIf_CmdFIFO, WO, u32);
 register_bits!(xadcif_cmdfifo, cmd, u8, 0, 31);
 register!(xadcif_rdfifo, XADCIf_RdFIFO, RO, u32);
 register_bits!(xadcif_rdfifo, rddata, u8, 0, 31);
 register!(xadcif_mctl, XADCIf_MCtl, RW, u32);
 register_bit!(xadcif_mctl, reset, 4);
--- a/libboard_zynq/src/error_led.rs
+++ b/libboard_zynq/src/error_led.rs
@ -0,0 +1,114 @@
 use libregister::{RegisterRW, RegisterW};
 use libregister::{register, register_at, register_bit, register_bits};
 use super::slcr;
 pub struct ErrorLED {
    regs: RegisterBlock,
 }
 impl ErrorLED {
    #[cfg(feature = "target_kasli_soc")]
    pub fn error_led() -> Self {
        slcr::RegisterBlock::unlocked(|slcr| {
            // Error LED at MIO pin 37
            slcr.mio_pin_37.write(
                slcr::MioPin37::zeroed()
                    .l3_sel(0b000)
                    .io_type(slcr::IoBufferType::Lvcmos25)
                    .pullup(true)
                    .disable_rcvr(true)
            );
        });
        Self::error_led_common(0xFFFF - 0x0080)
    }
    fn error_led_common(gpio_output_mask: u16) -> Self {
        // Setup register block
        let self_ = Self {
            regs: RegisterBlock::error_led(),
        };
        // Setup GPIO output mask
        self_.regs.gpio_output_mask.modify(|_, w| {
            w.mask(gpio_output_mask)
        });
        self_.regs.gpio_direction.modify(|_, w| {
            w.lederr(true)
        });
        self_
    }
    fn led_oe(&mut self, oe: bool) {
        self.regs.gpio_output_enable.modify(|_, w| {
             w.lederr(oe)
        })
    }
    fn led_o(&mut self, o: bool) {
        self.regs.gpio_output_mask.modify(|_, w| {
             w.lederr_o(o)
        })
    }
    pub fn toggle(&mut self, state: bool) {
        self.led_o(state);
        self.led_oe(state);
    }
 }
 pub struct RegisterBlock {
    pub gpio_output_mask: &'static mut GPIOOutputMask,
    pub gpio_direction: &'static mut GPIODirection,
    pub gpio_output_enable: &'static mut GPIOOutputEnable,
 }
 impl RegisterBlock {
    pub fn error_led() -> Self {
        Self {
            gpio_output_mask: GPIOOutputMask::new(),
            gpio_direction: GPIODirection::new(),
            gpio_output_enable: GPIOOutputEnable::new()
        }
    }
 }
 register!(gpio_output_mask,
            /// MASK_DATA_1_LSW:
            /// Maskable output data for MIO[47:32]
            GPIOOutputMask, RW, u32);
 #[cfg(feature = "target_kasli_soc")]
 register_at!(GPIOOutputMask, 0xE000A008, new);
 #[cfg(feature = "target_kasli_soc")]
 register_bit!(gpio_output_mask,
              /// Output for LED_ERR (MIO[37])
              lederr_o, 5);
 #[cfg(feature = "target_kasli_soc")]
 register_bits!(gpio_output_mask,
               mask, u16, 16, 31);
 register!(gpio_direction,
          /// DIRM_1:
          /// Direction mode for MIO[53:32]; 0/1 = in/out
          GPIODirection, RW, u32);
 #[cfg(feature = "target_kasli_soc")]
 register_at!(GPIODirection, 0xE000A244, new);
 #[cfg(feature = "target_kasli_soc")]
 register_bit!(gpio_direction,
              /// Direction for LED_ERR
              lederr, 5);
 register!(gpio_output_enable,
          /// OEN_1:
          /// Output enable for MIO[53:32]
          GPIOOutputEnable, RW, u32);
 #[cfg(feature = "target_kasli_soc")]
 register_at!(GPIOOutputEnable, 0xE000A248, new);
 #[cfg(feature = "target_kasli_soc")]
 register_bit!(gpio_output_enable,
              /// Output enable for LED_ERR
              lederr, 5);
--- a/libboard_zynq/src/eth/mod.rs
+++ b/libboard_zynq/src/eth/mod.rs
@ -1,28 +1,174 @@
-use crate::regs::*;
+use core::{
-use crate::slcr;
+    marker::PhantomData,
-use crate::println;
+    ops::{Deref, DerefMut},
-use crate::clocks::CpuClocks;
+};
 use log::{debug, info, warn, error};
 use libregister::*;
 use super::slcr;
 use super::clocks::Clocks;
 pub mod phy;
 use phy::{Phy, PhyAccess};
 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
 const MAX_MDC: u32 = 2_500_000;
 const TX_10: u32 = 10_000_000;
 const TX_100: u32 = 25_000_000;
 /// Clock for GbE
 const TX_1000: u32 = 125_000_000;
-pub struct Eth<'r, RX, TX> {
+#[derive(Clone)]
-    regs: &'r mut regs::RegisterBlock,
+#[repr(C, align(0x20))]
-    rx: RX,
+pub struct Buffer(pub [u8; MTU]);
-    tx: TX,
+
 impl Buffer {
    pub const fn new() -> Self {
        Buffer([0; MTU])
    }
 }
-impl<'r> Eth<'r, (), ()> {
+impl Deref for Buffer {
-    pub fn default(macaddr: [u8; 6]) -> Self {
+    type Target = [u8];
    fn deref(&self) -> &Self::Target {
        &self.0
    }
 }
 impl DerefMut for Buffer {
    fn deref_mut(&mut self) -> &mut <Self as Deref>::Target {
        &mut self.0
    }
 }
 /// 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
                #[cfg(not(feature = "target_ebaz4205"))]
                slcr::GemClkCtrl::zeroed()
                    .clkact(true)
                    .srcsel(slcr::PllSource::IoPll)
                    .divisor(divisor0 as u8)
                    .divisor1(divisor1 as u8),
                // ebaz4205 -- EMIO
                #[cfg(feature = "target_ebaz4205")]
                slcr::GemClkCtrl::zeroed()
                    .clkact(true)
                    .srcsel(slcr::PllSource::Emio)
                    .divisor(divisor0 as u8)
                    .divisor1(divisor1 as u8)
            );
            // Enable gem0 recv clock
            slcr.gem0_rclk_ctrl.write(
                // 0x0000_0801
                #[cfg(not(feature = "target_ebaz4205"))]
                slcr::RclkCtrl::zeroed()
                    .clkact(true),
                // ebaz4205 -- EMIO
                #[cfg(feature = "target_ebaz4205")]
                slcr::RclkCtrl::zeroed()
                    .clkact(true)
                    .srcsel(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<GEM>,
    phy: Phy,
    /// keep track of RX path occupation to avoid needless `check_link_change()`
    idle: bool,
 }
 impl Eth<Gem0, (), ()> {
    pub fn eth0(macaddr: [u8; 6]) -> Self {
        #[cfg(not(feature = "target_ebaz4205"))]
        slcr::RegisterBlock::unlocked(|slcr| {
            // Manual example: 0x0000_1280
            // MDIO
@ -98,48 +244,48 @@ impl<'r> Eth<'r, (), ()> {
            // RX_CLK
            slcr.mio_pin_22.write(
                slcr::MioPin22::zeroed()
                    .tri_enable(true)
                    .l0_sel(true)
                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RX_CTRL
            slcr.mio_pin_27.write(
                slcr::MioPin27::zeroed()
                    .tri_enable(true)
                    .l0_sel(true)
                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RXD3
            slcr.mio_pin_26.write(
                slcr::MioPin26::zeroed()
                    .tri_enable(true)
                    .l0_sel(true)
                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RXD2
            slcr.mio_pin_25.write(
                slcr::MioPin25::zeroed()
                    .tri_enable(true)
                    .l0_sel(true)
                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RXD1
            slcr.mio_pin_24.write(
                slcr::MioPin24::zeroed()
                    .tri_enable(true)
                    .l0_sel(true)
                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RXD0
            slcr.mio_pin_23.write(
                slcr::MioPin23::zeroed()
                    .tri_enable(true)
                    .l0_sel(true)
                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
@ -154,90 +300,296 @@ impl<'r> Eth<'r, (), ()> {
    }
    pub fn gem0(macaddr: [u8; 6]) -> Self {
-        Self::setup_gem0_clock(TX_1000);
+        Self::gem_common(macaddr)
        let regs = regs::RegisterBlock::gem0();
        Self::from_regs(regs, macaddr)
    }
 }
 impl Eth<Gem1, (), ()> {
    // TODO: Add a `eth1()`
    pub fn gem1(macaddr: [u8; 6]) -> Self {
-        Self::setup_gem1_clock(TX_1000);
+        Self::gem_common(macaddr)
-
+    }
        let regs = regs::RegisterBlock::gem1();
        Self::from_regs(regs, macaddr)
 }
-    fn from_regs(regs: &'r mut regs::RegisterBlock, macaddr: [u8; 6]) -> Self {
+
-        let mut eth = Eth {
+impl<GEM: Gem> Eth<GEM, (), ()> {
-            regs,
+    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");
        phy.reset(&mut inner);
        phy.restart_autoneg(&mut inner);
        #[cfg(feature="target_kasli_soc")]
        phy.set_leds(&mut inner);
        Eth {
            rx: (),
            tx: (),
-        }.init();
+            inner,
-        eth.configure(macaddr);
+            phy,
-        eth
+            idle: true,
        }
    }
 }
-impl<'r, RX, TX> Eth<'r, RX, TX> {
+impl<GEM: Gem, RX, TX> Eth<GEM, RX, TX> {
-    pub fn setup_gem0_clock(tx_clock: u32) {
+    pub fn start_rx(self, rx_size: usize) -> Eth<GEM, rx::DescList, TX> {
-        let io_pll = CpuClocks::get().io;
+        let new_self = Eth {
-        let d0 = (io_pll / tx_clock).min(63);
+            rx: rx::DescList::new(rx_size),
-        let d1 = (io_pll / tx_clock / d0).min(63);
+            tx: self.tx,
-
+            inner: self.inner,
-        slcr::RegisterBlock::unlocked(|slcr| {
+            phy: self.phy,
-            slcr.gem0_clk_ctrl.write(
+            idle: self.idle,
-                // 0x0050_0801: 8, 5: 100 Mb/s
+        };
-                // ...: 8, 1: 1000 Mb/s
+        let list_addr = new_self.rx.list_addr();
-                slcr::GemClkCtrl::zeroed()
+        assert!(list_addr & 0b11 == 0);
-                    .clkact(true)
+        GEM::regs().rx_qbar.write(
-                    .srcsel(slcr::PllSource::IoPll)
+            regs::RxQbar::zeroed()
-                    .divisor(d0 as u8)
+                .rx_q_baseaddr(list_addr >> 2)
                    .divisor1(d1 as u8)
        );
-            // Enable gem0 recv clock
+        GEM::regs().net_ctrl.modify(|_, w|
-            slcr.gem0_rclk_ctrl.write(
+            w.rx_en(true)
-                // 0x0000_0801
+        );
-                slcr::RclkCtrl::zeroed()
+        new_self
-                    .clkact(true)
+    }
    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),
            inner: self.inner,
            phy: self.phy,
            idle: self.idle,
        };
        let list_addr = &new_self.tx.list_addr();
        assert!(list_addr & 0b11 == 0);
        GEM::regs().tx_qbar.write(
            regs::TxQbar::zeroed()
                .tx_q_baseaddr(list_addr >> 2)
        );
        GEM::regs().net_ctrl.modify(|_, w|
            w.tx_en(true)
        );
        new_self
    }
 }
 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 = GEM::regs().rx_status.read();
        if status.hresp_not_ok() {
            // Clear
            GEM::regs().rx_status.write(
                regs::RxStatus::zeroed()
                    .hresp_not_ok(true)
            );
            return Err(rx::Error::HrespNotOk);
        }
        if status.rx_overrun() {
            // Clear
            GEM::regs().rx_status.write(
                regs::RxStatus::zeroed()
                    .rx_overrun(true)
            );
            return Err(rx::Error::RxOverrun);
        }
        if status.buffer_not_avail() {
            // Clear
            GEM::regs().rx_status.write(
                regs::RxStatus::zeroed()
                    .buffer_not_avail(true)
            );
            return Err(rx::Error::BufferNotAvail);
        }
        if status.frame_recd() {
            let result = self.rx.recv_next();
            match result {
                Ok(None) => {
                    // No packet, clear status bit
                    GEM::regs().rx_status.write(
                        regs::RxStatus::zeroed()
                            .frame_recd(true)
                    );
                    self.idle = true;
                }
                _ =>
                    self.idle = false,
            }
            result
        } else {
            self.idle = true;
            Ok(None)
        }
    }
 }
 impl<GEM: Gem, TX> libasync::smoltcp::LinkCheck for &mut Eth<GEM, rx::DescList, TX> {
    type Link = Option<phy::Link>;
    fn check_link_change(&mut self) -> Option<Self::Link> {
        self.inner.check_link_change(&self.phy)
    }
    fn is_idle(&self) -> bool {
        self.idle
    }
 }
 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(GEM::regs(), length)
    }
 }
 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>;
    fn capabilities(&self) -> smoltcp::phy::DeviceCapabilities {
        use smoltcp::phy::{DeviceCapabilities, ChecksumCapabilities, Checksum};
        let mut checksum_caps = ChecksumCapabilities::default();
        checksum_caps.ipv4 = Checksum::Both;
        checksum_caps.tcp = Checksum::Both;
        checksum_caps.udp = Checksum::Both;
        let mut caps = DeviceCapabilities::default();
        caps.max_transmission_unit = MTU;
        caps.max_burst_size = Some(self.rx.len().min(self.tx.len()));
        caps.checksum = checksum_caps;
        caps
    }
    fn receive(&'a mut self) -> Option<(Self::RxToken, Self::TxToken)> {
        match self.rx.recv_next() {
            Ok(Some(pktref)) => {
                let tx_token = tx::Token {
                    regs: GEM::regs(),
                    desc_list: &mut self.tx,
                };
                self.idle = false;
                Some((pktref, tx_token))
            }
            Ok(None) => {
                self.idle = true;
                None
            }
            Err(e) => {
                error!("eth recv error: {:?}", e);
                None
            }
        }
    }
    fn transmit(&'a mut self) -> Option<Self::TxToken> {
        Some(tx::Token {
            regs: GEM::regs(),
            desc_list: &mut self.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)
    }
-    pub fn setup_gem1_clock(tx_clock: u32) {
+    fn delay_ms(&mut self, ms: u64) {
-        let io_pll = CpuClocks::get().io;
+        self.count_down.start(Milliseconds(ms));
-        let d0 = (io_pll / tx_clock).min(63);
+        nb::block!(self.count_down.wait()).unwrap();
-        let d1 = (io_pll / tx_clock / d0).min(63);
+    }
-        slcr::RegisterBlock::unlocked(|slcr| {
+    fn eth_reset_common(gpio_output_mask: u16) -> Self {
-            slcr.gem1_clk_ctrl.write(
+        let self_ = Self {
-                slcr::GemClkCtrl::zeroed()
+            regs: regs::GpioRegisterBlock::regs(),
-                    .clkact(true)
+            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown(),
-                    .srcsel(slcr::PllSource::IoPll)
+        };
-                    .divisor(d0 as u8)
+
-                    .divisor1(d1 as u8)
+        // Setup GPIO output mask
-            );
+        self_.regs.gpio_output_mask.modify(|_, w| {
-            // Enable gem1 recv clock
+            w.mask(gpio_output_mask)
            slcr.gem1_rclk_ctrl.write(
                // 0x0000_0801
                slcr::RclkCtrl::zeroed()
                    .clkact(true)
            );
        });
        self_.regs.gpio_direction.modify(|_, w| {
            w.phy_rst(true)
        });
        self_
    }
-    fn init(self) -> 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>,
    link: Option<phy::Link>,
 }
 impl<GEM: Gem> EthInner<GEM> {
    fn init(&mut self) {
        // Clear the Network Control register.
-        self.regs.net_ctrl.write(regs::NetCtrl::zeroed());
+        GEM::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().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)
@ -251,7 +603,7 @@ impl<'r, RX, TX> Eth<'r, RX, TX> {
                .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)
@ -281,54 +633,58 @@ impl<'r, RX, TX> Eth<'r, RX, TX> {
                .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()
        );
        self
    }
    fn configure(&mut self, macaddr: [u8; 6]) {
-        let clocks = CpuClocks::get();
+        let clocks = Clocks::get();
-        let mut mdc_clk_div = (clocks.cpu_1x() / MAX_MDC) + 1;
+        let mut mdc_clk_div = clocks.cpu_1x() / MAX_MDC;
        if clocks.cpu_1x() % MAX_MDC > 0 {
            mdc_clk_div += 1;
        }
-        self.regs.net_cfg.write(
+        GEM::regs().net_cfg.write(
            regs::NetCfg::zeroed()
                .full_duplex(true)
                .gige_en(true)
                .speed(true)
                .no_broadcast(false)
                .multi_hash_en(true)
-                // Promiscuous mode (TODO?)
+                .rx_1536_byte_frames(true)
                .copy_all(true)
                // Remove 4-byte Frame CheckSum
                .fcs_remove(true)
                .dis_cp_pause_frame(true)
                // RX checksum offload
                .rx_chksum_offld_en(true)
                // One of the slower speeds
                .mdc_clk_div((mdc_clk_div >> 4).min(0b111) as u8)
        );
        let macaddr_msbs =
-            (u16::from(macaddr[0]) << 8) |
+            (u16::from(macaddr[5]) << 8) |
-            u16::from(macaddr[1]);
+            u16::from(macaddr[4]);
        let macaddr_lsbs =
-            (u32::from(macaddr[2]) << 24) |
+            (u32::from(macaddr[3]) << 24) |
-            (u32::from(macaddr[3]) << 16) |
+            (u32::from(macaddr[2]) << 16) |
-            (u32::from(macaddr[4]) << 8) |
+            (u32::from(macaddr[1]) << 8) |
-            u32::from(macaddr[5]);
+            u32::from(macaddr[0]);
-        self.regs.spec_addr1_top.write(
+        // writing to bot would disable the specific address
-            regs::SpecAddrTop::zeroed()
+        GEM::regs().spec_addr1_bot.write(
                .addr_msbs(macaddr_msbs)
        );
        self.regs.spec_addr1_bot.write(
            regs::SpecAddrBot::zeroed()
                .addr_lsbs(macaddr_lsbs)
        );
        // writing to top would enable it again
        GEM::regs().spec_addr1_top.write(
            regs::SpecAddrTop::zeroed()
                .addr_msbs(macaddr_msbs)
        );
-
+        GEM::regs().dma_cfg.write(
        self.regs.dma_cfg.write(
            regs::DmaCfg::zeroed()
                // 1536 bytes
                .ahb_mem_rx_buf_size((MTU >> 6) as u8)
@ -336,6 +692,7 @@ impl<'r, RX, TX> Eth<'r, RX, TX> {
                .rx_pktbuf_memsz_sel(0x3)
                // 4 KB
                .tx_pktbuf_memsz_sel(true)
                // TX checksum offload
                .csum_gen_offload_en(true)
                // Little-endian
                .ahb_endian_swp_mgmt_en(false)
@ -343,135 +700,61 @@ impl<'r, RX, TX> Eth<'r, RX, TX> {
                .ahb_fixed_burst_len(0x10)
        );
-        self.regs.net_ctrl.write(
+        GEM::regs().net_ctrl.write(
            regs::NetCtrl::zeroed()
                .mgmt_port_en(true)
        );
    }
    pub fn start_rx<'rx>(self, rx_list: &'rx mut [rx::DescEntry], rx_buffers: &'rx mut [[u8; MTU]]) -> Eth<'r, rx::DescList<'rx>, TX> {
        let new_self = Eth {
            regs: self.regs,
            rx: rx::DescList::new(rx_list, rx_buffers),
            tx: self.tx,
        };
        let list_addr = new_self.rx.list_addr();
        assert!(list_addr & 0b11 == 0);
        new_self.regs.rx_qbar.write(
            regs::RxQbar::zeroed()
                .rx_q_baseaddr(list_addr >> 2)
        );
        new_self.regs.net_ctrl.modify(|_, w|
            w.rx_en(true)
        );
        new_self
    }
    pub fn start_tx<'tx>(self, tx_list: &'tx mut [tx::DescEntry], tx_buffers: &'tx mut [[u8; MTU]]) -> Eth<'r, RX, tx::DescList<'tx>> {
        let new_self = Eth {
            regs: self.regs,
            rx: self.rx,
            tx: tx::DescList::new(tx_list, tx_buffers),
        };
        let list_addr = &new_self.tx.list_addr();
        assert!(list_addr & 0b11 == 0);
        new_self.regs.tx_qbar.write(
            regs::TxQbar::zeroed()
                .tx_q_baseaddr(list_addr >> 2)
        );
        new_self.regs.net_ctrl.modify(|_, w|
            w.tx_en(true)
        );
        new_self
    }
    fn wait_phy_idle(&self) {
-        while !self.regs.net_status.read().phy_mgmt_idle() {}
+        while !GEM::regs().net_status.read().phy_mgmt_idle() {}
    }
-    pub fn reset_phy(&mut self) -> bool {
+
-        match phy::Phy::find(self) {
+    fn check_link_change(&mut self, phy: &Phy) -> Option<Option<phy::Link>> {
-            Some(phy) => {
+        let link = phy.get_link(self);
-                println!("eth: Found PHY at {}: {}", phy.addr, phy.name());
+
-                phy.modify_control(self, |control|
+        // Check link state transition
-                    control.set_reset(true)
+        if self.link != link {
            match &link {
                Some(link) => {
                    info!("eth: got {:?}", link);
                    use phy::{LinkDuplex::Full, LinkSpeed::*};
                    let txclock = match link.speed {
                        S10 => TX_10,
                        S100 => TX_100,
                        S1000 => TX_1000,
                    };
                    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)
                    );
                while phy.get_control(self).reset() {
                    println!("eth: Wait for PHY reset");
                }
                None => {
                    warn!("eth: link lost");
                    phy.modify_control(self, |control|
                                       control.set_autoneg_enable(true)
                                       .set_restart_autoneg(true)
                    );
                println!("eth: Wait for link");
                while !phy.get_status(self).link_status() {}
                println!("eth: Got link, setting clock for gigabit");
                Self::setup_gem0_clock(TX_1000);
                true
            }
            None => false
        }
                }
            }
-impl<'r, 'rx, TX> Eth<'r, rx::DescList<'rx>, TX> {
+            self.link = link;
-    pub fn recv_next<'s: 'p, 'p>(&'s mut self) -> Result<Option<rx::PktRef<'p>>, rx::Error> {
+            Some(link)
        let status = self.regs.rx_status.read();
        if status.hresp_not_ok() {
            // Clear
            self.regs.rx_status.write(
                regs::RxStatus::zeroed()
                    .hresp_not_ok(true)
            );
            return Err(rx::Error::HrespNotOk);
        }
        if status.rx_overrun() {
            // Clear
            self.regs.rx_status.write(
                regs::RxStatus::zeroed()
                    .rx_overrun(true)
            );
            return Err(rx::Error::RxOverrun);
        }
        if status.buffer_not_avail() {
            // Clear
            self.regs.rx_status.write(
                regs::RxStatus::zeroed()
                    .buffer_not_avail(true)
            );
            return Err(rx::Error::BufferNotAvail);
        }
        if status.frame_recd() {
            let result = self.rx.recv_next();
            match result {
                Ok(None) => {
                    // No packet, clear status bit
                    self.regs.rx_status.write(
                        regs::RxStatus::zeroed()
                            .frame_recd(true)
                    );
                }
                _ => {}
            }
            result
        } else {
-            Ok(None)
+            None
        }
    }
 }
-impl<'r, 'tx, RX> Eth<'r, RX, tx::DescList<'tx>> {
+impl<GEM: Gem> PhyAccess for EthInner<GEM> {
    pub fn send<'s: 'p, 'p>(&'s mut self, length: usize) -> Option<tx::PktRef<'p>> {
        self.tx.send(self.regs, length)
    }
 }
 impl<'r, RX, TX> phy::PhyAccess for Eth<'r, RX, TX> {
    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)
@ -480,12 +763,12 @@ impl<'r, RX, TX> phy::PhyAccess for Eth<'r, RX, TX> {
                .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)
@ -498,39 +781,4 @@ impl<'r, RX, TX> phy::PhyAccess for Eth<'r, RX, TX> {
    }
 }
 impl<'r, 'rx, 'tx: 'a, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescList<'rx>, tx::DescList<'tx>> {
    type RxToken = rx::PktRef<'a>;
    type TxToken = tx::Token<'a, 'tx>;
    fn capabilities(&self) -> smoltcp::phy::DeviceCapabilities {
        let mut caps = smoltcp::phy::DeviceCapabilities::default();
        caps.max_transmission_unit = MTU;
        caps
    }
    fn receive(&'a mut self) -> Option<(Self::RxToken, Self::TxToken)> {
        match self.rx.recv_next() {
            Ok(Some(mut pktref)) => {
                let tx_token = tx::Token {
                    regs: self.regs,
                    desc_list: &mut self.tx,
                };
                Some((pktref, tx_token))
            }
            Ok(None) =>
                None,
            Err(e) => {
                println!("eth recv error: {:?}", e);
                None
            }
        }
    }
    fn transmit(&'a mut self) -> Option<Self::TxToken> {
        Some(tx::Token {
            regs: self.regs,
            desc_list: &mut self.tx,
        })
    }
 }
--- a/libboard_zynq/src/eth/phy/control.rs
+++ b/libboard_zynq/src/eth/phy/control.rs
@ -82,6 +82,10 @@ impl PhyRegister for Control {
    fn addr() -> u8 {
        0
    }
    fn page() -> u8 {
        0
    }
 }
 impl From<u16> for Control {
--- a/libboard_zynq/src/eth/phy/id.rs
+++ b/libboard_zynq/src/eth/phy/id.rs
@ -11,6 +11,9 @@ pub struct PhyIdentifier {
 }
 pub fn identify_phy<PA: PhyAccess>(pa: &mut PA, addr: u8) -> Option<PhyIdentifier> {
    #[cfg(feature = "target_kasli_soc")]
    pa.write_phy(addr, 0x16, 0); //reset page
    let id1 = pa.read_phy(addr, 2);
    let id2 = pa.read_phy(addr, 3);
    if id1 != 0xFFFF || id2 != 0xFFFF {
--- a/libboard_zynq/src/eth/phy/leds.rs
+++ b/libboard_zynq/src/eth/phy/leds.rs
@ -0,0 +1,79 @@
 use bit_field::BitField;
 use super::{PhyRegister, Led0Control, Led1Control};
 #[derive(Clone, Copy, Debug)]
 /// LED Control Register
 pub struct Leds(pub u16);
 impl Leds {
    pub fn led0(&self) -> Led0Control {
        match self.0.get_bits(0..=3) {
            0b0000 => Led0Control::OnLinkOffNoLink,
            0b0001 => Led0Control::OnLinkBlinkActivityOffNoLink,
            0b0010 => Led0Control::BlinkDependingOnLink,
            0b0011 => Led0Control::OnActivityOffNoActivity,
            0b0100 => Led0Control::BlinkActivityOffNoActivity,
            0b0101 => Led0Control::OnTransmitOffNoTransmit,
            0b0110 => Led0Control::OnCopperLinkOffElse,
            0b0111 => Led0Control::On1000LinkOffElse,
            0b1000 => Led0Control::ForceOff,
            0b1001 => Led0Control::ForceOn,
            0b1010 => Led0Control::ForceHiZ,
            0b1011 => Led0Control::ForceBlink,
            0b1100 => Led0Control::Mode1,
            0b1101 => Led0Control::Mode2,
            0b1110 => Led0Control::Mode3,
            0b1111 => Led0Control::Mode4,
            _ => unreachable!()
        }
    }
    pub fn led1(&self) -> Led1Control {
        match self.0.get_bits(4..=7) {
            0b0000 => Led1Control::OnReceiveOffNoReceive,
            0b0001 => Led1Control::OnLinkBlinkActivityOffNoLink,
            0b0010 => Led1Control::OnLinkBlinkReceiveOffNoLink,
            0b0011 => Led1Control::OnActivityOffNoActivity,
            0b0100 => Led1Control::BlinkActivityOffNoActivity,
            0b0101 => Led1Control::On100OrFiberOffElse,
            0b0110 => Led1Control::On1001000LinkOffElse,
            0b0111 => Led1Control::On100LinkOffElse,
            0b1000 => Led1Control::ForceOff,
            0b1001 => Led1Control::ForceOn,
            0b1010 => Led1Control::ForceHiZ,
            0b1011 => Led1Control::ForceBlink,
            _ => unreachable!()
        }
    }
    pub fn set_led0(mut self, setting: Led0Control) -> Self {
        self.0.set_bits(0..=3, setting as u16);
        self
    }
    pub fn set_led1(mut self, setting: Led1Control) -> Self {
        self.0.set_bits(4..=7, setting as u16);
        self
    }
 }
 impl PhyRegister for Leds {
    fn addr() -> u8 {
        0x10
    }
    fn page() -> u8 {
        3
    }
 }
 impl From<u16> for Leds {
    fn from(value: u16) -> Self {
        Leds(value)
    }
 }
 impl Into<u16> for Leds {
    fn into(self) -> u16 {
        self.0
    }
 }
--- a/libboard_zynq/src/eth/phy/mod.rs
+++ b/libboard_zynq/src/eth/phy/mod.rs
@ -0,0 +1,214 @@
 pub mod id;
 use id::{identify_phy, PhyIdentifier};
 mod status;
 pub use status::Status;
 mod control;
 pub use control::Control;
 mod pssr;
 pub use pssr::PSSR;
 mod leds;
 pub use leds::Leds;
 #[derive(Copy, Clone, Debug, PartialEq)]
 pub struct Link {
    pub speed: LinkSpeed,
    pub duplex: LinkDuplex,
 }
 #[derive(Copy, Clone, Debug, PartialEq)]
 pub enum LinkSpeed {
    S10,
    S100,
    S1000,
 }
 #[derive(Copy, Clone, Debug, PartialEq)]
 pub enum LinkDuplex {
    Half,
    Full,
 }
 #[derive(Copy, Clone, Debug, PartialEq)]
 pub enum Led0Control {
    OnLinkOffNoLink = 0b0000,
    OnLinkBlinkActivityOffNoLink = 0b0001,
    BlinkDependingOnLink = 0b0010,
    OnActivityOffNoActivity = 0b0011,
    BlinkActivityOffNoActivity = 0b0100,
    OnTransmitOffNoTransmit = 0b0101,
    OnCopperLinkOffElse = 0b0110,
    On1000LinkOffElse = 0b0111,
    ForceOff = 0b1000,
    ForceOn = 0b1001,
    ForceHiZ = 0b1010,
    ForceBlink = 0b1011,
    Mode1 = 0b1100,
    Mode2 = 0b1101,
    Mode3 = 0b1110,
    Mode4 = 0b1111
 }
 #[derive(Copy, Clone, Debug, PartialEq)]
 pub enum Led1Control {
    OnReceiveOffNoReceive = 0b0000,
    OnLinkBlinkActivityOffNoLink = 0b0001,
    OnLinkBlinkReceiveOffNoLink = 0b0010,
    OnActivityOffNoActivity = 0b0011,
    BlinkActivityOffNoActivity = 0b0100,
    On100OrFiberOffElse = 0b0101,
    On1001000LinkOffElse = 0b0110,
    On100LinkOffElse = 0b0111,
    ForceOff = 0b1000,
    ForceOn = 0b1001,
    ForceHiZ = 0b1010,
    ForceBlink = 0b1011,
 }
 pub trait PhyAccess {
    fn read_phy(&mut self, addr: u8, reg: u8) -> u16;
    fn write_phy(&mut self, addr: u8, reg: u8, data: u16);
 }
 pub trait PhyRegister {
    fn addr() -> u8;
    fn page() -> u8;
 }
 #[derive(Clone)]
 pub struct Phy {
    pub addr: u8,
 }
 const OUI_MARVELL: u32 = 0x005043;
 const OUI_REALTEK: u32 = 0x000732;
 const OUI_LANTIQ : u32 = 0x355969;
 const OUI_ICPLUS : u32 = 0x0090c3;
 //only change pages on Kasli-SoC's Marvel 88E11xx
 #[cfg(feature="target_kasli_soc")]
 const PAGE_REGISTER: u8 = 0x16;
 impl Phy {
    /// Probe all addresses on MDIO for a known PHY
    pub fn find<PA: PhyAccess>(pa: &mut PA) -> Option<Phy> {
        (0..32).find(|addr| {
            match identify_phy(pa, *addr) {
                Some(PhyIdentifier {
                    oui: OUI_MARVELL,
                    // Marvell 88E1116R
                    model: 36,
                    ..
                }) => true,
                Some(PhyIdentifier {
                    oui: OUI_MARVELL,
                    // Marvell 88E1512
                    model: 29,
                    ..
                }) => true,
                Some(PhyIdentifier {
                    oui: OUI_REALTEK,
                    // RTL 8211E
                    model: 0b010001,
                    rev: 0b0101,
                }) => true,
                Some(PhyIdentifier {
                    oui: OUI_LANTIQ,
                    // Intel XWAY PHY11G (PEF 7071/PEF 7072) v1.5 / v1.6
                    model: 0,
                    ..
                }) => true,
                Some(PhyIdentifier {
                    oui: OUI_ICPLUS,
                    // IP101G-DS-R01
                    model: 5,
                    rev: 4,
                }) => true,
                _ => false,
            }
        }).map(|addr| Phy { addr })
    }
    pub fn read_reg<PA, PR>(&self, pa: &mut PA) -> PR
    where
        PA: PhyAccess,
        PR: PhyRegister + From<u16>,
    {
        #[cfg(feature="target_kasli_soc")]
        pa.write_phy(self.addr, PAGE_REGISTER, PR::page().into());
        pa.read_phy(self.addr, PR::addr()).into()
    }
    pub fn modify_reg<PA, PR, F>(&self, pa: &mut PA, mut f: F)
    where
        PA: PhyAccess,
        PR: PhyRegister + From<u16> + Into<u16>,
        F: FnMut(PR) -> PR,
    {
        #[cfg(feature="target_kasli_soc")]
        pa.write_phy(self.addr, PAGE_REGISTER, PR::page().into());
        let reg = pa.read_phy(self.addr, PR::addr()).into();
        let reg = f(reg);
        pa.write_phy(self.addr, PR::addr(), reg.into())
    }
    pub fn modify_control<PA, F>(&self, pa: &mut PA, f: F)
    where
        PA: PhyAccess,
        F: FnMut(Control) -> Control,
    {
        self.modify_reg(pa, f)
    }
    pub fn modify_leds<PA, F>(&self, pa: &mut PA, f: F)
    where
        PA: PhyAccess,
        F: FnMut(Leds) -> Leds,
    {
        self.modify_reg(pa, f)   
    }
    pub fn get_control<PA: PhyAccess>(&self, pa: &mut PA) -> Control {
        self.read_reg(pa)
    }
    pub fn get_status<PA: PhyAccess>(&self, pa: &mut PA) -> Status {
        self.read_reg(pa)
    }
    pub fn get_link<PA: PhyAccess>(&self, pa: &mut PA) -> Option<Link> {
        let status = self.get_status(pa);
        if !status.link_status() {
            None
        } else if status.cap_1000base_t_extended_status() {
            let phy_status: PSSR = self.read_reg(pa);
            phy_status.get_link()
        } else {
            status.get_link()
        }
    }
    pub fn reset<PA: PhyAccess>(&self, pa: &mut PA) {
        self.modify_control(pa, |control|
            control.set_reset(true)
        );
        while self.get_control(pa).reset() {}
    }
    pub fn restart_autoneg<PA: PhyAccess>(&self, pa: &mut PA) {
        self.modify_control(pa, |control|
            control.set_autoneg_enable(true)
                .set_restart_autoneg(true)
        );
    }
    #[cfg(feature="target_kasli_soc")]
    pub fn set_leds<PA: PhyAccess>(&self, pa: &mut PA) {
        self.modify_leds(pa, |leds|
            leds.set_led0(Led0Control::OnCopperLinkOffElse)
                .set_led1(Led1Control::BlinkActivityOffNoActivity)
        );
    }
 }
--- a/libboard_zynq/src/eth/phy/pssr.rs
+++ b/libboard_zynq/src/eth/phy/pssr.rs
@ -0,0 +1,56 @@
 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
    }
    fn page() -> u8 {
        0
    }
 }
 impl From<u16> for PSSR {
    fn from(value: u16) -> Self {
        PSSR(value)
    }
 }
--- a/libboard_zynq/src/eth/phy/status.rs
+++ b/libboard_zynq/src/eth/phy/status.rs
@ -1,5 +1,5 @@
 use bit_field::BitField;
-use super::PhyRegister;
+use super::{PhyRegister, Link, LinkDuplex, LinkSpeed};
 #[derive(Clone, Copy, Debug)]
 /// Basic Mode Status Register
@ -51,12 +51,59 @@ impl Status {
    pub fn cap_100base_t4(&self) -> bool {
        self.0.get_bit(15)
    }
    pub fn get_link(&self) -> Option<Link> {
        if ! self.link_status() {
            None
        } else if self.cap_100base_tx_full() {
            Some(Link {
                speed: LinkSpeed::S100,
                duplex: LinkDuplex::Full,
            })
        } else if self.cap_100base_tx_half() {
            Some(Link {
                speed: LinkSpeed::S100,
                duplex: LinkDuplex::Half,
            })
        } else if self.cap_100base_t4() {
            Some(Link {
                speed: LinkSpeed::S100,
                duplex: LinkDuplex::Half,
            })
        } else if self.cap_10base_t2_full() {
            Some(Link {
                speed: LinkSpeed::S10,
                duplex: LinkDuplex::Full,
            })
        } else if self.cap_10base_t2_half() {
            Some(Link {
                speed: LinkSpeed::S10,
                duplex: LinkDuplex::Half,
            })
        } else if self.cap_10base_t_full() {
            Some(Link {
                speed: LinkSpeed::S10,
                duplex: LinkDuplex::Full,
            })
        } else if self.cap_10base_t_half() {
            Some(Link {
                speed: LinkSpeed::S10,
                duplex: LinkDuplex::Half,
            })
        } else {
            None
        }
    }
 }
 impl PhyRegister for Status {
    fn addr() -> u8 {
        1
    }
    fn page() -> u8 {
        0
    }
 }
 impl From<u16> for Status {
--- a/libboard_zynq/src/eth/regs.rs
+++ b/libboard_zynq/src/eth/regs.rs
@ -1,6 +1,6 @@
 use volatile_register::{RO, WO, RW};
-use crate::{register, register_bit, register_bits, register_bits_typed};
+use libregister::{register, register_at, register_bit, register_bits, register_bits_typed};
 #[repr(C)]
 pub struct RegisterBlock {
@ -110,18 +110,51 @@ pub struct RegisterBlock {
    pub design_cfg5: RO<u32>,
 }
-impl RegisterBlock {
+pub struct GpioRegisterBlock {
-    const GEM0: *mut Self = 0xE000B000 as *mut _;
+    pub gpio_output_mask: &'static mut OutputMask,
-    const GEM1: *mut Self = 0xE000C000 as *mut _;
+    pub gpio_direction: &'static mut Direction,
-
+    pub gpio_output_enable: &'static mut OutputEnable,
    pub fn gem0() -> &'static mut Self {
        unsafe { &mut *Self::GEM0 }
 }
-    pub fn gem1() -> &'static mut Self {
+impl GpioRegisterBlock {
-        unsafe { &mut *Self::GEM1 }
+    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);
 register!(net_ctrl, NetCtrl, RW, u32);
 register_bit!(net_ctrl, loopback_local, 1);
--- a/libboard_zynq/src/eth/rx.rs
+++ b/libboard_zynq/src/eth/rx.rs
@ -1,6 +1,8 @@
 use core::ops::Deref;
-use crate::{register, register_bit, register_bits, regs::*};
+use alloc::{vec, vec::Vec};
-use super::MTU;
+use libcortex_a9::{asm::*, cache::*, UncachedSlice};
 use libregister::*;
 use super::Buffer;
 #[derive(Debug)]
 pub enum Error {
@ -11,13 +13,22 @@ pub enum Error {
 }
 /// Descriptor entry
-#[repr(C)]
+#[repr(C, align(0x08))]
 pub struct DescEntry {
    word0: DescWord0,
    word1: DescWord1,
 }
-register!(desc_word0, DescWord0, RW, u32);
+impl DescEntry {
    pub fn zeroed() -> Self {
        DescEntry {
            word0: DescWord0 { inner: VolatileCell::new(0) },
            word1: DescWord1 { inner: VolatileCell::new(0) },
        }
    }
 }
 register!(desc_word0, DescWord0, VolatileCell, u32);
 register_bit!(desc_word0,
              /// true if owned by software, false if owned by hardware
              used, 0);
@ -26,7 +37,7 @@ register_bit!(desc_word0,
              wrap, 1);
 register_bits!(desc_word0, address, u32, 2, 31);
-register!(desc_word1, DescWord1, RW, u32);
+register!(desc_word1, DescWord1, VolatileCell, u32);
 register_bits!(desc_word1, frame_length_lsbs, u16, 0, 12);
 register_bit!(desc_word1, bad_fcs, 13);
 register_bit!(desc_word1, start_of_frame, 14);
@ -44,18 +55,22 @@ register_bit!(desc_word1, multi_hash_match, 30);
 register_bit!(desc_word1, global_broadcast, 31);
 #[repr(C)]
-pub struct DescList<'a> {
+pub struct DescList {
-    list: &'a mut [DescEntry],
+    list: UncachedSlice<DescEntry>,
-    buffers: &'a mut [[u8; MTU]],
+    buffers: Vec<Buffer>,
    next: usize,
 }
-impl<'a> DescList<'a> {
+impl DescList {
-    pub fn new(list: &'a mut [DescEntry], buffers: &'a mut [[u8; MTU]]) -> Self {
+    pub fn new(size: usize) -> Self {
        let mut list = UncachedSlice::new(size, || DescEntry::zeroed())
            .unwrap();
        let mut buffers = vec![Buffer::new(); size];
        let last = list.len().min(buffers.len()) - 1;
        for (i, (entry, buffer)) in list.iter_mut().zip(buffers.iter_mut()).enumerate() {
            let is_last = i == last;
-            let buffer_addr = &mut buffer[0] as *mut _ as u32;
+            let buffer_addr = &mut buffer.0[0] as *mut _ as u32;
            assert!(buffer_addr & 0b11 == 0);
            entry.word0.write(
                DescWord0::zeroed()
@ -66,6 +81,9 @@ impl<'a> DescList<'a> {
            entry.word1.write(
                DescWord1::zeroed()
            );
            // Flush buffer from cache, to be filled by the peripheral
            // before next read
            dcci_slice(&buffer[..]);
        }
        DescList {
@ -75,6 +93,10 @@ impl<'a> DescList<'a> {
        }
    }
    pub fn len(&self) -> usize {
        self.list.len().min(self.buffers.len())
    }
    pub fn list_addr(&self) -> u32 {
        &self.list[0] as *const _ as u32
    }
@ -82,10 +104,25 @@ impl<'a> DescList<'a> {
    pub fn recv_next<'s: 'p, 'p>(&'s mut self) -> Result<Option<PktRef<'p>>, Error> {
        let list_len = self.list.len();
        let entry = &mut self.list[self.next];
        dmb();
        if entry.word0.read().used() {
            let word1 = entry.word1.read();
            let len = word1.frame_length_lsbs().into();
-            let buffer = &self.buffers[self.next][0..len];
+            let padding = {
                let diff = len % 0x20;
                if diff == 0 {
                    0
                } else {
                    0x20 - diff
                }
            };
            unsafe {
                // invalidate the buffer
                // we cannot do it in the drop function, as L2 cache data prefetch would prefetch
                // the data, and there is no way for us to prevent that unless changing MMU table.
                dci_slice(&mut self.buffers[self.next][0..len + padding]);
            }
            let buffer = &mut self.buffers[self.next][0..len];
            self.next += 1;
            if self.next >= list_len {
@ -107,12 +144,13 @@ impl<'a> DescList<'a> {
 /// Releases a buffer back to the HW upon Drop
 pub struct PktRef<'a> {
    entry: &'a mut DescEntry,
-    buffer: &'a [u8],
+    buffer: &'a mut [u8],
 }
 impl<'a> Drop for PktRef<'a> {
    fn drop(&mut self) {
        self.entry.word0.modify(|_, w| w.used(false));
        dmb();
    }
 }
@ -124,9 +162,9 @@ impl<'a> Deref for PktRef<'a> {
 }
 impl<'a> smoltcp::phy::RxToken for PktRef<'a> {
-    fn consume<R, F>(mut self, _timestamp: smoltcp::time::Instant, f: F) -> smoltcp::Result<R>
+    fn consume<R, F>(self, _timestamp: smoltcp::time::Instant, f: F) -> smoltcp::Result<R>
    where
-        F: FnOnce(&[u8]) -> smoltcp::Result<R>
+        F: FnOnce(&mut [u8]) -> smoltcp::Result<R>
    {
        f(self.buffer)
    }
--- a/libboard_zynq/src/eth/tx.rs
+++ b/libboard_zynq/src/eth/tx.rs
@ -1,18 +1,20 @@
 use core::ops::{Deref, DerefMut};
-use crate::{register, register_bit, register_bits, regs::*};
+use alloc::{vec, vec::Vec};
-use crate::println;
+use libcortex_a9::{cache::dcc_slice, UncachedSlice};
-use super::{MTU, regs};
+use libregister::*;
 use super::{Buffer, regs};
 /// Descriptor entry
 #[repr(C, align(0x08))]
 pub struct DescEntry {
    word0: DescWord0,
    word1: DescWord1,
 }
-register!(desc_word0, DescWord0, RW, u32);
+register!(desc_word0, DescWord0, VolatileCell, u32);
 register_bits!(desc_word0, address, u32, 0, 31);
-register!(desc_word1, DescWord1, RW, u32);
+register!(desc_word1, DescWord1, VolatileCell, u32);
 register_bits!(desc_word1, length, u16, 0, 13);
 register_bit!(desc_word1, last_buffer, 15);
 register_bit!(desc_word1, no_crc_append, 16);
@ -27,22 +29,41 @@ register_bit!(desc_word1,
              /// true if owned by software, false if owned by hardware
              used, 31);
 impl DescEntry {
    pub fn zeroed() -> Self {
        DescEntry {
            word0: DescWord0 { inner: VolatileCell::new(0) },
            word1: DescWord1 { inner: VolatileCell::new(0) },
        }
    }
 }
 /// Number of descriptors
 pub const DESCS: usize = 8;
 #[repr(C)]
-pub struct DescList<'a> {
+pub struct DescList {
-    list: &'a mut [DescEntry],
+    list: UncachedSlice<DescEntry>,
-    buffers: &'a mut [[u8; MTU]],
+    buffers: Vec<Buffer>,
    next: usize,
 }
-impl<'a> DescList<'a> {
+impl DescList {
-    pub fn new(list: &'a mut [DescEntry], buffers: &'a mut [[u8; MTU]]) -> Self {
+    pub fn new(size: usize) -> Self {
        let mut list = UncachedSlice::new(size, || DescEntry::zeroed())
            .unwrap();
        let mut buffers = vec![Buffer::new(); size];
        let last = list.len().min(buffers.len()) - 1;
        // Sending seems to not work properly with only one packet
        // buffer (two duplicates get send with every packet), so
        // check that at least 2 are allocated, i.e. that the index of
        // the last one is at least one.
        assert!(last > 0);
        for (i, (entry, buffer)) in list.iter_mut().zip(buffers.iter_mut()).enumerate() {
            let is_last = i == last;
-            let buffer_addr = &mut buffer[0] as *mut _ as u32;
+            let buffer_addr = &mut buffer.0[0] as *mut _ as u32;
            assert!(buffer_addr & 0b11 == 0);
            entry.word0.write(
                DescWord0::zeroed()
@ -64,6 +85,10 @@ impl<'a> DescList<'a> {
        }
    }
    pub fn len(&self) -> usize {
        self.list.len().min(self.buffers.len())
    }
    pub fn list_addr(&self) -> u32 {
        &self.list[0] as *const _ as u32
    }
@ -72,8 +97,13 @@ impl<'a> DescList<'a> {
        let list_len = self.list.len();
        let entry = &mut self.list[self.next];
        if entry.word1.read().used() {
            entry.word1.modify(|_, w| w.length(length as u16));
            let buffer = &mut self.buffers[self.next][0..length];
            entry.word1.write(DescWord1::zeroed()
                .length(length as u16)
                .last_buffer(true)
                .wrap(self.next >= list_len - 1)
                .used(true)
            );
            self.next += 1;
            if self.next >= list_len {
@ -98,13 +128,12 @@ pub struct PktRef<'a> {
 impl<'a> Drop for PktRef<'a> {
    fn drop(&mut self) {
        // Write back all dirty cachelines of this buffer
        dcc_slice(self.buffer);
        self.entry.word1.modify(|_, w| w.used(false));
-        if ! self.regs.tx_status.read().tx_go() {
+        // Start the TX engine
-            println!("tx start_tx");
+        self.regs.net_ctrl.modify(|_, w| w.start_tx(true));
            self.regs.net_ctrl.modify(|_, w|
                                      w.start_tx(true)
            );
        }
    }
 }
@ -122,12 +151,12 @@ impl<'a> DerefMut for PktRef<'a> {
 }
 /// TxToken for smoltcp support
-pub struct Token<'a, 'tx: 'a> {
+pub struct Token<'a> {
    pub regs: &'a mut regs::RegisterBlock,
-    pub desc_list: &'a mut DescList<'tx>,
+    pub desc_list: &'a mut DescList,
 }
-impl<'a, 'tx: 'a> smoltcp::phy::TxToken for Token<'a, 'tx> {
+impl<'a> smoltcp::phy::TxToken for Token<'a> {
    fn consume<R, F>(self, _timestamp: smoltcp::time::Instant, len: usize, f: F) -> smoltcp::Result<R>
        where F: FnOnce(&mut [u8]) -> smoltcp::Result<R>
    {
@ -135,10 +164,7 @@ impl<'a, 'tx: 'a> smoltcp::phy::TxToken for Token<'a, 'tx> {
            None =>
                Err(smoltcp::Error::Exhausted),
            Some(mut pktref) => {
-                let result = f(pktref.deref_mut());
+                f(pktref.deref_mut())
                // TODO: on result.is_err() don;t send
                drop(pktref);
                result
            }
        }
    }
--- a/libboard_zynq/src/gic.rs
+++ b/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 gic(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));
        }
        // 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)
    }
 }
--- a/libboard_zynq/src/i2c/eeprom.rs
+++ b/libboard_zynq/src/i2c/eeprom.rs
@ -0,0 +1,130 @@
 use super::I2c;
 use crate::time::Milliseconds;
 use embedded_hal::timer::CountDown;
 pub struct EEPROM<'a> {
    i2c: &'a mut I2c,
    #[cfg(not(feature = "target_ebaz4205"))]
    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_kasli_soc")]
    pub fn new(i2c: &'a mut I2c, page_size: u8) -> Self {
        EEPROM {
            i2c: i2c,
            port: 3,
            address: 0x57,
            page_size: page_size,
            count_down: unsafe { crate::timer::GlobalTimer::get() }.countdown()
        }
    }
    #[cfg(feature = "target_zc706")]
    fn select(&mut self) -> Result<(), &'static str> {
        self.i2c.pca954x_select(0b1110100, Some(self.port))?;
        Ok(())
    }
    #[cfg(feature = "target_kasli_soc")]
    fn select(&mut self) -> Result<(), &'static str> {
        // tca9548 is compatible with pca9548
        self.i2c.pca954x_select(0b1110001, Some(self.port))?;
        Ok(())
    }
    #[cfg(feature = "target_ebaz4205")]
    fn select(&mut self) -> Result<(), &'static str> {
        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)
    }
 }
--- a/libboard_zynq/src/i2c/mod.rs
+++ b/libboard_zynq/src/i2c/mod.rs
@ -0,0 +1,336 @@
 //! I2C Bit-banging Controller
 mod regs;
 pub mod eeprom;
 #[cfg(not(feature = "target_ebaz4205"))]
 use super::slcr;
 use super::time::Microseconds;
 use embedded_hal::timer::CountDown;
 use libregister::{RegisterR, RegisterRW};
 #[cfg(not(feature = "target_ebaz4205"))]
 use libregister::RegisterW;
 #[cfg(feature = "target_kasli_soc")]
 use log::info;
 pub enum I2cMultiplexer {
    PCA9548 = 0,
    #[cfg(feature = "target_kasli_soc")]
    PCA9547 = 1,
 }
 pub struct I2c {
    regs: regs::RegisterBlock,
    count_down: super::timer::global::CountDown<Microseconds>,
    pca_type: I2cMultiplexer
 }
 impl I2c {
    #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
    pub fn i2c0() -> Self {
        // Route I2C 0 SCL / SDA Signals to MIO Pins 50 / 51
        #[cfg(not(feature = "target_ebaz4205"))]
        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)
            );
            // On Kasli-SoC prototype, leakage through the unconfigured I2C_SW_RESET
            // MIO pin develops enough voltage on the T21 gate to assert the reset.
            // Configure the pin to avoid this problem.
            #[cfg(feature = "target_kasli_soc")]
            slcr.mio_pin_33.write(
                slcr::MioPin33::zeroed()
                    .l3_sel(0b000)
                    .io_type(slcr::IoBufferType::Lvcmos33)
                    .pullup(false)
                    .disable_rcvr(true)
            );
        });
        Self::i2c_common(0xFFFF - 0x000C, 0xFFFF - 0x0002)
    }
    fn i2c_common(gpio_output_mask: u16, _gpio_output_mask_lower: u16) -> Self {
        // Setup register block
        let self_ = Self {
            regs: regs::RegisterBlock::i2c(),
            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown(),
            pca_type: I2cMultiplexer::PCA9548 //default for zc706
        };
        // 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)
        });
        //Kasli-SoC only: I2C_SW_RESET configuration
        #[cfg(feature = "target_kasli_soc")]
        {
            self_.regs.gpio_output_mask_lower.modify(|_, w| {
                w.mask(_gpio_output_mask_lower)
            });
            self_.regs.gpio_direction.modify(|_, w| {
                w.i2cswr(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 unit_delay(&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)
        })
    }
    #[cfg(feature = "target_kasli_soc")]
    fn i2cswr_oe(&mut self, oe: bool) {
        self.regs.gpio_output_enable.modify(|_, w| {
             w.i2cswr(oe)
        })
    }
    #[cfg(feature = "target_kasli_soc")]
    fn i2cswr_o(&mut self, o: bool) {
        self.regs.gpio_output_mask_lower.modify(|_, w| {
             w.i2cswr_o(o)
        })
    }
    #[cfg(feature = "target_kasli_soc")]
    fn pca_autodetect(&mut self) -> Result<I2cMultiplexer, &'static str> {
        // start with resetting the PCA954X
        // SDA must be clear (before start)
        // reset time is 500ns, unit_delay (100us) to account for propagation
        self.i2cswr_o(true);
        self.unit_delay();
        self.i2cswr_o(false);
        self.unit_delay();
        let pca954x_read_addr = (0x71 << 1) | 0x01;
        self.start()?;
        // read the config register
        if !self.write(pca954x_read_addr)? {
            return Err("PCA954X failed to ack read address");
        }
        let config = self.read(false)?;
        let pca = match config {
            0x00 => { info!("PCA9548 detected"); I2cMultiplexer::PCA9548 },
            0x08 => { info!("PCA9547 detected"); I2cMultiplexer::PCA9547 },
            _ => { return Err("Unknown response for PCA954X autodetect")},
        };
        self.stop()?;
        Ok(pca)
    }
    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.unit_delay();
        self.unit_delay();
        if !self.sda_i() {
            // Try toggling SCL a few times
            for _bit in 0..8 {
                self.scl_oe(true);
                self.unit_delay();
                self.scl_oe(false);
                self.unit_delay();
            }
        }
        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");
        }
        // postcondition: SCL and SDA high
        #[cfg(feature = "target_kasli_soc")]
        {
            self.i2cswr_oe(true);
            self.pca_type = self.pca_autodetect()?;
        }
        Ok(())
    }
    pub fn start(&mut self) -> Result<(), &'static str> {
        // precondition: SCL and SDA high
        if !self.scl_i() {
            return Err("SCL is stuck low");
        }
        if !self.sda_i() {
            return Err("SDA arbitration lost");
        }
        self.sda_oe(true);
        self.unit_delay();
        self.scl_oe(true);
        self.unit_delay();
        // postcondition: SCL and SDA low
        Ok(())
    }
    pub fn restart(&mut self) -> Result<(), &'static str> {
        // precondition SCL and SDA low
        self.sda_oe(false);
        self.unit_delay();
        self.scl_oe(false);
        self.unit_delay();
        self.start()?;
        // postcondition: SCL and SDA low
        Ok(())
    }
    pub fn stop(&mut self) -> Result<(), &'static str> {
        // precondition: SCL and SDA low
        self.unit_delay();
        self.scl_oe(false);
        self.unit_delay();
        self.sda_oe(false);
        self.unit_delay();
        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.unit_delay();
            self.scl_oe(false);
            self.unit_delay();
            self.scl_oe(true);
            self.unit_delay();
        }
        self.sda_oe(false);
        self.unit_delay();
        self.scl_oe(false);
        self.unit_delay();
        // Read ack/nack
        let ack = !self.sda_i();
        self.scl_oe(true);
        self.unit_delay();
        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.unit_delay();
            self.scl_oe(false);
            self.unit_delay();
            if self.sda_i() { data |= 1 << bit }
            self.scl_oe(true);
        }
        // Send ack/nack (true = nack, false = ack)
        self.sda_oe(ack);
        self.unit_delay();
        self.scl_oe(false);
        self.unit_delay();
        self.scl_oe(true);
        self.sda_oe(true);
        // postcondition: SCL and SDA low
        Ok(data)
    }
    pub fn pca954x_select(&mut self, address: u8, channel: Option<u8>) -> Result<(), &'static str> {
        self.start()?;
        // PCA9547 supports only one channel at a time
        // for compatibility, PCA9548 is treated as such too
        // channel - Some(x) - # of the channel [0,7], or None for all disabled
        let setting = match self.pca_type {
            I2cMultiplexer::PCA9548 => { 
                match channel {
                    Some(ch) => 1 << ch,
                    None => 0,
                }
            },
            #[cfg(feature = "target_kasli_soc")]
            I2cMultiplexer::PCA9547 => {
                match channel {
                    Some(ch) => ch | 0x08,
                    None => 0,
                }
            }
        };
        if !self.write(address << 1)? {
            return Err("PCA954X failed to ack write address")
        }
        if !self.write(setting)? {
            return Err("PCA954X failed to ack control word")
        }
        self.stop()?;
        Ok(())
    }
 }
--- a/libboard_zynq/src/i2c/regs.rs
+++ b/libboard_zynq/src/i2c/regs.rs
@ -0,0 +1,135 @@
 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
 // kasli_soc: GPIO 50, 51 == SCL, SDA; GPIO 33 == I2C_SW_RESET
 // ebaz4205: GPIO (EMIO)
 pub struct RegisterBlock {
    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,
    #[cfg(feature = "target_kasli_soc")]
    pub gpio_output_mask_lower: &'static mut GPIOOutputMaskLower,
 }
 impl RegisterBlock {
    pub fn i2c() -> Self {
        Self {
            gpio_output_mask: GPIOOutputMask::new(),
            gpio_input: GPIOInput::new(),
            gpio_direction: GPIODirection::new(),
            gpio_output_enable: GPIOOutputEnable::new(),
            #[cfg(feature = "target_kasli_soc")]
            gpio_output_mask_lower: GPIOOutputMaskLower::new(),
        }
    }
 }
 register!(gpio_output_mask,
          /// MASK_DATA_1_MSW:
          /// Maskable output data for MIO[53:48]
          GPIOOutputMask, RW, u32);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_at!(GPIOOutputMask, 0xE000A00C, new);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_bit!(gpio_output_mask,
              /// Output for SCL
              scl_o, 2);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_bit!(gpio_output_mask,
              /// Output for SDA
              sda_o, 3);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_bits!(gpio_output_mask,
               /// Mask for keeping bits except SCL and SDA unchanged
               mask, u16, 16, 31);
 register!(gpio_output_mask_lower,
            /// MASK_DATA_1_LSW:
            /// Maskable output data for MIO[47:32]
            GPIOOutputMaskLower, RW, u32);
 #[cfg(feature = "target_kasli_soc")]
 register_at!(GPIOOutputMaskLower, 0xE000A008, new);
 #[cfg(feature = "target_kasli_soc")]
 register_bit!(gpio_output_mask_lower,
              /// Output for I2C_SW_RESET (MIO[33])
              i2cswr_o, 1);
 #[cfg(feature = "target_kasli_soc")]
 register_bits!(gpio_output_mask_lower,
               mask, u16, 16, 31);
 register!(gpio_input,
          /// DATA_1_RO:
          /// Input data for MIO[53:32]
          GPIOInput, RO, u32);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_at!(GPIOInput, 0xE000A064, new);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_bit!(gpio_input,
              /// Input for SCL
              scl, 18);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_bit!(gpio_input,
              /// Input for SDA
              sda, 19);
 register!(gpio_direction,
          /// DIRM_1:
          /// Direction mode for MIO[53:32]; 0/1 = in/out
          GPIODirection, RW, u32);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_at!(GPIODirection, 0xE000A244, new);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_bit!(gpio_direction,
              /// Direction for SCL
              scl, 18);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_bit!(gpio_direction,
              /// Direction for SDA
              sda, 19);
 #[cfg(feature = "target_kasli_soc")]
 register_bit!(gpio_direction,
              /// Direction for I2C_SW_RESET
              i2cswr, 1);
 register!(gpio_output_enable,
          /// OEN_1:
          /// Output enable for MIO[53:32]
          GPIOOutputEnable, RW, u32);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_at!(GPIOOutputEnable, 0xE000A248, new);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_bit!(gpio_output_enable,
              /// Output enable for SCL
              scl, 18);
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 register_bit!(gpio_output_enable,
              /// Output enable for SDA
              sda, 19);
 #[cfg(feature = "target_kasli_soc")]
 register_bit!(gpio_output_enable,
              /// Output enable for I2C_SW_RESET
              i2cswr, 1);
--- a/libboard_zynq/src/lib.rs
+++ b/libboard_zynq/src/lib.rs
@ -0,0 +1,27 @@
 #![no_std]
 extern crate alloc;
 /// Re-export so that dependents can always use the same version
 pub use smoltcp;
 pub mod slcr;
 pub mod clocks;
 pub mod uart;
 pub mod devc;
 pub mod stdio;
 pub mod eth;
 pub mod axi_hp;
 pub mod axi_gp;
 pub mod ddr;
 pub mod mpcore;
 pub mod gic;
 pub mod time;
 pub mod timer;
 pub mod sdio;
 #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
 pub mod i2c;
 pub mod logger;
 pub mod ps7_init;
 #[cfg(feature="target_kasli_soc")]
 pub mod error_led;
--- a/libboard_zynq/src/logger.rs
+++ b/libboard_zynq/src/logger.rs
@ -0,0 +1,34 @@
 //! A logger for the `log` crate
 use crate::{println, stdio, timer::GlobalTimer};
 pub static LOGGER: Logger = Logger;
 pub struct Logger;
 pub fn init() -> Result<(), log::SetLoggerError> {
    log::set_logger(&LOGGER)
 }
 impl log::Log for Logger {
    fn enabled(&self, metadata: &log::Metadata) -> bool {
        metadata.level() <= log::Level::Trace
    }
    fn log(&self, record: &log::Record) {
        if self.enabled(record.metadata()) {
            let timestamp = unsafe {
                GlobalTimer::get()
            }.get_us().0;
            let seconds   = timestamp / 1_000_000;
            let micros    = timestamp % 1_000_000;
            println!("[{:6}.{:06}s] {:>5}({}): {}",
                     seconds, micros, record.level(), record.target(), record.args());
        }
    }
    fn flush(&self) {
        let uart = stdio::get_uart();
        while !uart.tx_idle() {}
    }
 }
--- a/libboard_zynq/src/mpcore.rs
+++ b/libboard_zynq/src/mpcore.rs
@ -0,0 +1,321 @@
 ///! Register definitions for Application Processing Unit (mpcore)
 use volatile_register::{RO, RW};
 use libregister::{
    register, register_at, register_bit, register_bits,
    RegisterW, RegisterRW,
 };
 #[repr(C)]
 pub struct RegisterBlock {
    /// SCU Control Register
    pub scu_control: ScuControl,
    /// 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,
    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,
    /// Global Timer Interrupt Status Register
    pub global_timer_interrupt_status: GlobalTimerInterruptStatusRegister,
    /// Comparator Value Register_0
    pub comparator_value0: ValueRegister,
    pub comparator_value1: ValueRegister,
    /// 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, mpcore);
 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);
 register_bit!(scu_control, force_to_port0_enable, 4);
 register_bit!(scu_control, scu_speculative_linefill_enable, 3);
 register_bit!(scu_control, scu_rams_parity_enable, 2);
 register_bit!(scu_control, address_filtering_enable, 1);
 register_bit!(scu_control, enable, 0);
 impl ScuControl {
    pub fn start(&mut self) {
        self.modify(|_, w| w.enable(true).scu_speculative_linefill_enable(true));
    }
 }
 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);
 register_bits!(scu_invalidate, cpu2_ways, u8, 8, 11);
 register_bits!(scu_invalidate, cpu3_ways, u8, 12, 15);
 impl ScuInvalidate {
    pub fn invalidate_all_cores(&mut self) {
        self.write(ScuInvalidate::zeroed()
            .cpu0_ways(0xf)
            .cpu1_ways(0xf)
            .cpu2_ways(0xf)
            .cpu3_ways(0xf)
        );
    }
    pub fn invalidate_core1(&mut self) {
        self.write(ScuInvalidate::zeroed()
            .cpu1_ways(0xf)
        );
    }
 }
 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);
 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);
--- a/libboard_zynq/src/ps7_init/mod.rs
+++ b/libboard_zynq/src/ps7_init/mod.rs
@ -0,0 +1,108 @@
 use crate::println;
 #[cfg(feature = "target_zc706")]
 mod zc706;
 #[cfg(not(feature = "target_zc706"))]
 mod none;
 #[cfg(feature = "target_zc706")]
 use zc706 as target;
 #[cfg(not(feature = "target_zc706"))]
 use none as target;
 pub fn report_differences() {
    for (i, op) in target::INIT_DATA.iter().enumerate() {
        let address = op.address();
        let overwritten_later = target::INIT_DATA[(i + 1)..].iter()
            .any(|later_op| later_op.address() == address);
        if !overwritten_later {
            op.report_difference();
        }
    }
 }
 pub fn apply() {
    for op in target::INIT_DATA {
        op.apply();
    }
 }
 #[derive(Clone, Debug)]
 pub enum InitOp {
    MaskWrite(usize, usize, usize),
    MaskPoll(usize, usize),
    MaskDelay(usize, usize),
 }
 impl InitOp {
    fn address(&self) -> usize {
        match self {
            InitOp::MaskWrite(address, _, _) => *address,
            InitOp::MaskPoll(address, _) => *address,
            InitOp::MaskDelay(address, _) => *address,
        }
    }
    fn read(&self) -> usize {
        unsafe { *(self.address() as *const usize) }
    }
    fn difference(&self) -> Option<(usize, usize)> {
        let expected = match self {
            InitOp::MaskWrite(_, mask, expected) =>
                Some((*mask, *expected)),
            InitOp::MaskPoll(_, mask) =>
                Some((*mask, *mask)),
            _ => None,
        };
        match expected {
            Some((mask, expected)) => {
                let actual = self.read();
                if actual & mask == expected {
                    None
                } else {
                    Some((actual & mask, expected))
                }
            }
            None =>
                None
        }
    }
    pub fn report_difference(&self) {
        if let Some((actual, expected)) = self.difference() {
            println!(
                "Register {:08X} is {:08X}&={:08X} != {:08X} expected",
                self.address(),
                self.read(),
                actual,
                expected
            );
        }
    }
    pub fn apply(&self) {
        let reg = self.address() as *mut usize;
        println!("apply {:?}", self);
        match self {
            InitOp::MaskWrite(_, mask, val) =>
                unsafe {
                    *reg = (val & mask) | (*reg & !mask);
                },
            InitOp::MaskPoll(_, mask) =>
                while unsafe { *reg } & mask == 0 {},
            InitOp::MaskDelay(_, mask) => {
                let delay = get_number_of_cycles_for_delay(*mask);
                while unsafe { *reg } < delay {
                    println!("W");
                }
            }
        }
    }
 }
 fn get_number_of_cycles_for_delay(delay: usize) -> usize {
    const APU_FREQ: usize = 666666687;
    APU_FREQ * delay/ (2 * 1000)
 }
--- a/libboard_zynq/src/ps7_init/none.rs
+++ b/libboard_zynq/src/ps7_init/none.rs
@ -0,0 +1,4 @@
 use super::InitOp;
 pub const INIT_DATA: &'static [InitOp] = &[
 ];
--- a/libboard_zynq/src/ps7_init/zc706.rs
+++ b/libboard_zynq/src/ps7_init/zc706.rs
--- a/libboard_zynq/src/sdio/adma.rs
+++ b/libboard_zynq/src/sdio/adma.rs
@ -0,0 +1,76 @@
 /// ADMA library
 use core::mem::MaybeUninit;
 use super::Sdio;
 use libcortex_a9::cache;
 use libregister::{
    register, register_bit,
    RegisterR, RegisterW, RegisterRW, VolatileCell,
 };
 #[repr(C, align(4))]
 pub struct Adma2Desc32 {
    attribute: Desc32Attribute,
    length: VolatileCell<u16>,
    address: VolatileCell<u32>,
 }
 const DESC_MAX_LENGTH: u32 = 65536;
 register!(desc32_attribute, Desc32Attribute, VolatileCell, u16);
 register_bit!(desc32_attribute, trans, 5);
 register_bit!(desc32_attribute, int, 2);
 register_bit!(desc32_attribute, end, 1);
 register_bit!(desc32_attribute, valid, 0);
 pub struct Adma2DescTable([Adma2Desc32; 32]);
 impl Adma2DescTable {
    pub fn new() -> Self {
        let table = MaybeUninit::zeroed();
        let table = unsafe { table.assume_init() };
        Adma2DescTable(table)
    }
    /// Initialize the table and setup `adma_system_address`
    pub fn setup(&mut self, sdio: &mut Sdio, blk_cnt: u32, buffer: &[u8]) {
        let descr_table = &mut self.0;
        let blk_size = sdio
            .regs
            .block_size_block_count
            .read()
            .transfer_block_size() as u32;
        let total_desc_lines = if blk_size * blk_cnt < DESC_MAX_LENGTH {
            1
        } else {
            blk_size * blk_cnt / DESC_MAX_LENGTH
                + if (blk_size * blk_cnt) % DESC_MAX_LENGTH == 0 {
                    0
                } else {
                    1
                }
        } as usize;
        let ptr = buffer.as_ptr() as u32;
        for desc_num in 0..total_desc_lines {
            descr_table[desc_num].address.set(ptr + (desc_num as u32) * DESC_MAX_LENGTH);
            descr_table[desc_num].attribute.write(
                Desc32Attribute::zeroed()
                    .trans(true)
                    .valid(true)
            );
            // 0 is the max length (65536)
            descr_table[desc_num].length.set(0);
        }
        descr_table[total_desc_lines - 1].attribute.modify(|_, w| w.end(true));
        descr_table[total_desc_lines - 1].length.set(
            (blk_cnt * blk_size - ((total_desc_lines as u32) - 1) * DESC_MAX_LENGTH) as u16,
        );
        unsafe {
            sdio.regs
                .adma_system_address
                .write(descr_table.as_ptr() as u32);
        }
        cache::dcci_slice(descr_table);
    }
 }
--- a/libboard_zynq/src/sdio/cmd.rs
+++ b/libboard_zynq/src/sdio/cmd.rs
@ -0,0 +1,133 @@
 use super::regs;
 const APP_CMD_PREFIX: u8 = 0x80;
 #[allow(unused)]
 pub mod args {
    pub const CMD8_VOL_PATTERN: u32 = 0x1AA;
    pub const RESPOCR_READY: u32 = 0x80000000;
    pub const ACMD41_HCS: u32 = 0x40000000;
    pub const ACMD41_3V3: u32 = 0x00300000;
    pub const CMD1_HIGH_VOL: u32 = 0x00FF8000;
    pub const OCR_S18: u32 = 1 << 24;
 }
 #[allow(unused)]
 #[repr(u8)]
 #[derive(Copy, Clone, PartialEq, Debug)]
 pub enum SdCmd {
    CMD0 = 0x00,
    CMD1 = 0x01,
    CMD2 = 0x02,
    CMD3 = 0x03,
    CMD4 = 0x04,
    CMD5 = 0x05,
    CMD6 = 0x06,
    ACMD6 = APP_CMD_PREFIX + 0x06,
    CMD7 = 0x07,
    CMD8 = 0x08,
    CMD9 = 0x09,
    CMD10 = 0x0A,
    CMD11 = 0x0B,
    CMD12 = 0x0C,
    ACMD13 = APP_CMD_PREFIX + 0x0D,
    CMD16 = 0x10,
    CMD17 = 0x11,
    CMD18 = 0x12,
    CMD19 = 0x13,
    CMD21 = 0x15,
    CMD23 = 0x17,
    ACMD23 = APP_CMD_PREFIX + 0x17,
    CMD24 = 0x18,
    CMD25 = 0x19,
    CMD41 = 0x29,
    ACMD41 = APP_CMD_PREFIX + 0x29,
    ACMD42 = APP_CMD_PREFIX + 0x2A,
    ACMD51 = APP_CMD_PREFIX + 0x33,
    CMD52 = 0x34,
    CMD55 = 0x37,
    CMD58 = 0x3A,
 }
 pub fn require_dat(cmd: SdCmd, is_sd_card: bool) -> bool {
    use SdCmd::*;
    match cmd {
        CMD6 => is_sd_card,
        CMD8 => !is_sd_card,
        ACMD13 | CMD17 | CMD18 | CMD19 | CMD21 | CMD23 | ACMD23 | CMD24 | CMD25 | ACMD51 => true,
        _ => false,
    }
 }
 type CmdReg = regs::transfer_mode_command::Write;
 fn resp_r1(w: CmdReg) -> CmdReg {
    w.response_type_select(regs::ResponseTypeSelect::Length48)
        .crc_check_en(true)
        .index_check_en(true)
 }
 fn resp_r1b(w: CmdReg) -> CmdReg {
    w.response_type_select(regs::ResponseTypeSelect::Legnth48Check)
        .crc_check_en(true)
        .index_check_en(true)
 }
 fn resp_r2(w: CmdReg) -> CmdReg {
    w.response_type_select(regs::ResponseTypeSelect::Length136)
        .crc_check_en(true)
 }
 fn resp_r3(w: CmdReg) -> CmdReg {
    w.response_type_select(regs::ResponseTypeSelect::Length48)
 }
 fn resp_r6(w: CmdReg) -> CmdReg {
    w.response_type_select(regs::ResponseTypeSelect::Legnth48Check)
        .crc_check_en(true)
        .index_check_en(true)
 }
 pub fn set_cmd_reg(cmd: SdCmd, is_sd_card: bool, w: CmdReg) -> CmdReg {
    use SdCmd::*;
    let w = w.command_index(cmd as u8 & 0x3F);
    match cmd {
        CMD1 => resp_r3(w),
        CMD2 => resp_r2(w),
        CMD3 => {
            if is_sd_card {
                resp_r6(w)
            } else {
                resp_r1(w)
            }
        }
        CMD5 => resp_r1b(w),
        CMD6 => {
            if is_sd_card {
                resp_r1(w).data_present_select(true)
            } else {
                resp_r1b(w)
            }
        }
        ACMD6 => resp_r1(w),
        CMD7 => resp_r1(w),
        CMD8 => {
            if is_sd_card {
                resp_r1(w)
            } else {
                resp_r1(w).data_present_select(true)
            }
        }
        CMD9 => resp_r2(w),
        CMD10 | CMD11 | CMD12 => resp_r1(w),
        ACMD13 => resp_r1(w).data_present_select(true),
        CMD16 => resp_r1(w),
        CMD17 | CMD18 | CMD19 | CMD21 | CMD23 | ACMD23 | CMD24 | CMD25 => {
            resp_r1(w).data_present_select(true)
        }
        ACMD41 => resp_r3(w),
        ACMD42 => resp_r1(w),
        ACMD51 => resp_r1(w).data_present_select(true),
        CMD52 | CMD55 => resp_r1(w),
        _ => w,
    }
 }
--- a/libboard_zynq/src/sdio/mod.rs
+++ b/libboard_zynq/src/sdio/mod.rs
@ -0,0 +1,475 @@
 pub mod sd_card;
 mod adma;
 mod cmd;
 mod regs;
 use super::clocks::Clocks;
 use super::slcr;
 use super::time::Milliseconds;
 use embedded_hal::timer::CountDown;
 use libregister::{RegisterR, RegisterRW, RegisterW};
 use log::{trace, debug};
 use nb;
 /// Basic SDIO Struct with common low-level functions.
 pub struct Sdio {
    regs: &'static mut regs::RegisterBlock,
    count_down: super::timer::global::CountDown<Milliseconds>,
    input_clk_hz: u32,
    card_type: CardType,
    card_detect: bool,
 }
 #[derive(Debug)]
 pub enum CmdTransferError {
    CmdInhibited,
    DatLineInhibited,
    CmdTimeout,
    Other(regs::interrupt_status::Read),
 }
 impl core::fmt::Display for CmdTransferError {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        use CmdTransferError::*;
        write!(f, "Command transfer error: ")?;
        match self {
            CmdInhibited => write!(f, "Command line inhibited."),
            DatLineInhibited => write!(f, "Data line inhibited, possibly due to ongonging data transfer."),
            CmdTimeout => write!(f, "Command timeout, check if the card is inserted properly."),
            Other(x) => write!(f, "Unknown Error, interrupt status = 0x{:0X}", x.inner),
        }
    }
 }
 #[derive(PartialEq, Debug, Clone, Copy)]
 pub enum CardType {
    CardNone,
    CardSd,
    CardMmc,
 }
 impl Sdio {
    /// Initialize SDIO0
    /// card_detect means if we would use the card detect pin,
    /// false to disable card detection (assume there is card inserted)
    pub fn sdio0(card_detect: bool) -> Self {
        // initialization according to ps7_init.c
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.mio_pin_40.write(
                slcr::MioPin40::zeroed()
                    .l3_sel(0b100)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .speed(true),
            );
            slcr.mio_pin_41.write(
                slcr::MioPin41::zeroed()
                    .l3_sel(0b100)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .speed(true),
            );
            slcr.mio_pin_42.write(
                slcr::MioPin42::zeroed()
                    .l3_sel(0b100)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .speed(true),
            );
            slcr.mio_pin_43.write(
                slcr::MioPin43::zeroed()
                    .l3_sel(0b100)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .speed(true),
            );
            slcr.mio_pin_44.write(
                slcr::MioPin44::zeroed()
                    .l3_sel(0b100)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .speed(true),
            );
            slcr.mio_pin_45.write(
                slcr::MioPin45::zeroed()
                    .l3_sel(0b100)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .speed(true),
            );
            // zc706 card detect pin
            #[cfg(feature = "target_zc706")]
            {
                unsafe {
                    slcr.sd0_wp_cd_sel.write(0x000E000F);
                }
                slcr.mio_pin_14.write(
                    slcr::MioPin14::zeroed()
                        .io_type(slcr::IoBufferType::Lvcmos18)
                        .pullup(true)
                        .tri_enable(true),
                );
            }
            // cora card detect pin
            #[cfg(feature = "target_coraz7")]
            {
                unsafe {
                    slcr.sd0_wp_cd_sel.write(47 << 16);
                }
                slcr.mio_pin_47.write(
                    slcr::MioPin47::zeroed()
                        .io_type(slcr::IoBufferType::Lvcmos18)
                        .speed(true),
                );
            }
            // kasli_soc and redpitaya card detect pin
            #[cfg(any(feature = "target_kasli_soc", feature = "target_redpitaya"))]
            {
                unsafe {
                    slcr.sd0_wp_cd_sel.write(46 << 16);
                }
                slcr.mio_pin_46.write(
                    slcr::MioPin46::zeroed()
                        .io_type(slcr::IoBufferType::Lvcmos25)
                        .speed(true),
                );
            }
            // ebaz4205 card detect pin
            #[cfg(feature = "target_ebaz4205")]
            {
                unsafe {
                    slcr.sd0_wp_cd_sel.write(34 << 16);
                }
                slcr.mio_pin_34.write(
                    slcr::MioPin34::zeroed()
                        .io_type(slcr::IoBufferType::Lvcmos33)
                        .pullup(true)
                        .speed(true),
                );
            }
            slcr.sdio_rst_ctrl.reset_sdio0();
            slcr.aper_clk_ctrl.enable_sdio0();
            slcr.sdio_clk_ctrl.enable_sdio0();
        });
        let clocks = Clocks::get();
        let mut self_ = Sdio {
            regs: regs::RegisterBlock::sdio0(),
            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown(),
            input_clk_hz: clocks.sdio_ref_clk(),
            card_type: CardType::CardNone,
            card_detect,
        };
        self_.init();
        self_
    }
    /// Change clock frequency to the value less than or equal to the given value.
    /// From XSdPs_Change_ClkFreq in xsdps_options.c. SPEC_V3 related code is removed as
    /// our board would only be V1 or V2.
    fn change_clk_freq(&mut self, freq: u32) {
        debug!("Changing clock frequency to {}", freq);
        self.regs
            .clock_control
            .modify(|_, w| w.sd_clk_en(false).internal_clk_en(false));
        const XSDPS_CC_MAX_DIV_CNT: u32 = 256;
        // calculate clock divisor
        let mut div_cnt: u32 = 0x1;
        let mut divisor = 0;
        while div_cnt <= XSDPS_CC_MAX_DIV_CNT {
            if (self.input_clk_hz / div_cnt) <= freq {
                divisor = div_cnt / 2;
                break;
            }
            div_cnt <<= 1;
        }
        if div_cnt > XSDPS_CC_MAX_DIV_CNT {
            panic!("No valid divisor!");
        }
        // enable internal clock
        self.regs
            .clock_control
            .modify(|_, w| w.sdclk_freq_divisor(divisor as u8).internal_clk_en(true));
        while !self.regs.clock_control.read().internal_clk_stable() {}
        // enable SD clock
        self.regs.clock_control.modify(|_, w| w.sd_clk_en(true));
    }
    /// Initialization based on XSdPs_CfgInitialize function in xsdps.c
    fn init(&mut self) {
        // poweroff
        self.regs
            .control
            .modify(|_, w| w.bus_voltage(regs::BusVoltage::V0).bus_power(false));
        if self.regs.misc_reg.read().spec_ver() == regs::SpecificationVersion::V3 {
            // The documentation said the field can only be V1 or V2,
            // so the code is written for V1 and V2. V3 requires special handling
            // which is currently not implemented.
            // I hope that this would never trigger but it is safer to put a check here.
            panic!("The code written is for V1 and V2");
        }
        // delay to poweroff card
        self.delay(1);
        // reset all
        debug!("Reset SDIO!");
        self.regs
            .clock_control
            .modify(|_, w| w.software_reset_all(true));
        while self.regs.clock_control.read().software_reset_all() {}
        // set power to 3.3V
        self.regs
            .control
            .modify(|_, w| w.bus_voltage(regs::BusVoltage::V33).bus_power(true));
        // set clock frequency
        self.change_clk_freq(400_000);
        // select voltage
        let capabilities = self.regs.capabilities.read();
        let voltage = if capabilities.voltage_3_3() {
            regs::BusVoltage::V33
        } else if capabilities.voltage_3_0() {
            regs::BusVoltage::V30
        } else if capabilities.voltage_1_8() {
            regs::BusVoltage::V18
        } else {
            regs::BusVoltage::V0
        };
        self.regs.control.modify(|_, w| w.bus_voltage(voltage));
        self.regs
            .control
            .modify(|_, w| w.dma_select(regs::DmaSelect::ADMA2_32));
        // enable all interrupt status except card interrupt
        self.regs.interrupt_status_en.write(
            (regs::interrupt_status_en::Write { inner: 0xFFFFFFFF })
                .card_interrupt_status_en(false),
        );
        // disable all interrupt signals
        self.regs
            .interrupt_signal_en
            .write(regs::InterruptSignalEn::zeroed());
        // set block size to 512 by default
        self.regs
            .block_size_block_count
            .modify(|_, w| w.transfer_block_size(512));
    }
    /// Delay for SDIO operations, simple wrapper for nb.
    pub fn delay(&mut self, ms: u64) {
        self.count_down.start(Milliseconds(ms));
        nb::block!(self.count_down.wait()).unwrap();
    }
    /// Send SD command. Basically `cmd_transfer_with_mode` with mode
    /// `regs::TransferModeCommand::zeroed()`.
    /// Return: Ok if success, Err(status) if failed.
    fn cmd_transfer(
        &mut self,
        cmd: cmd::SdCmd,
        arg: u32,
        block_cnt: u16,
    ) -> Result<(), CmdTransferError> {
        self.cmd_transfer_with_mode(cmd, arg, block_cnt, regs::TransferModeCommand::zeroed())
    }
    /// Send SD Command with additional transfer mode.
    /// This function would block until response is ready.
    /// Return: Ok if success, Err(status) if failed.
    fn cmd_transfer_with_mode(
        &mut self,
        cmd: cmd::SdCmd,
        arg: u32,
        block_cnt: u16,
        transfer_mode: regs::transfer_mode_command::Write,
    ) -> Result<(), CmdTransferError> {
        trace!("Send Cmd {:?}", cmd);
        let state = self.regs.present_state.read();
        if state.command_inhibit_cmd() {
            return Err(CmdTransferError::CmdInhibited);
        }
        self.regs
            .block_size_block_count
            .modify(|_, w| w.blocks_count(block_cnt));
        self.regs
            .clock_control
            .modify(|_, w| w.timeout_counter_value(0xE));
        unsafe {
            self.regs.argument.write(arg);
        }
        self.regs
            .interrupt_status_en
            .write(regs::interrupt_status_en::Write { inner: 0xFFFFFFFF });
        let is_sd_card = self.card_type == CardType::CardSd;
        // Check DAT Line
        if cmd != cmd::SdCmd::CMD21 && cmd != cmd::SdCmd::CMD19 {
            if self.regs.present_state.read().command_inhibit_dat()
                && cmd::require_dat(cmd, is_sd_card)
            {
                return Err(CmdTransferError::DatLineInhibited);
            }
        }
        // Set the command registers.
        self.regs
            .transfer_mode_command
            .write(cmd::set_cmd_reg(cmd, is_sd_card, transfer_mode));
        // polling for response
        loop {
            let status = self.regs.interrupt_status.read();
            if cmd == cmd::SdCmd::CMD21 || cmd == cmd::SdCmd::CMD19 {
                if status.buffer_read_ready() {
                    self.regs
                        .interrupt_status
                        .modify(|_, w| w.buffer_read_ready());
                    break;
                }
            }
            if status.command_complete() {
                break;
            }
            self.check_error(&status)?;
        }
        // wait for command complete
        while !self.regs.interrupt_status.read().command_complete() {}
        self.regs
            .interrupt_status
            .modify(|_, w| w.command_complete());
        Ok(())
    }
    /// Check if card is inserted.
    pub fn is_card_inserted(&self) -> bool {
        !self.card_detect || self.regs.present_state.read().card_inserted()
    }
    /// Switch voltage from 3.3V to 1.8V.
    fn switch_voltage(&mut self) -> Result<(), CmdTransferError> {
        use cmd::SdCmd::*;
        // send switch voltage command
        self.cmd_transfer(CMD11, 0, 0)?;
        // wait for the lines to go low
        let mut state = self.regs.present_state.read();
        while state.cmd_line_level()
            || state.dat0_level()
            || state.dat1_level()
            || state.dat2_level()
            || state.dat3_level()
        {
            state = self.regs.present_state.read();
        }
        // stop the clock
        self.regs
            .clock_control
            .modify(|_, w| w.sd_clk_en(false).internal_clk_en(false));
        // enabling 1.8v in controller
        self.regs
            .control
            .modify(|_, w| w.bus_voltage(regs::BusVoltage::V18));
        // wait minimum 5ms
        self.delay(5);
        if self.regs.control.read().bus_voltage() != regs::BusVoltage::V18 {
            // I should not wrap the error of this function into another type later.
            // actually this is not correct.
            return Err(CmdTransferError::CmdTimeout);
        }
        // wait for internal clock to stabilize
        self.regs
            .clock_control
            .modify(|_, w| w.internal_clk_en(true));
        while !self.regs.clock_control.read().internal_clk_stable() {}
        // enable SD clock
        self.regs.clock_control.modify(|_, w| w.sd_clk_en(true));
        // wait for 1ms
        self.delay(1);
        // wait for CMD and DATA line to go high
        state = self.regs.present_state.read();
        while !state.cmd_line_level()
            || !state.dat0_level()
            || !state.dat1_level()
            || !state.dat2_level()
            || !state.dat3_level()
        {
            state = self.regs.present_state.read();
        }
        Ok(())
    }
    /// Detect inserted card type, and set the corresponding field.
    /// Return Ok(CardType) on success, Err(CmdTransferError) when failed to identify.
    pub fn identify_card(&mut self) -> Result<CardType, CmdTransferError> {
        use cmd::{args::*, SdCmd::*};
        // actually the delay for this one is unclear in the xilinx code.
        self.delay(10);
        self.cmd_transfer(CMD0, 0, 0)?;
        self.card_type = match self.cmd_transfer(CMD1, ACMD41_HCS | CMD1_HIGH_VOL, 0) {
            Ok(()) => CardType::CardMmc,
            Err(_) => CardType::CardSd,
        };
        // clear all status
        self.regs
            .interrupt_status
            .write(regs::interrupt_status::Write { inner: 0xF3FFFFFF });
        self.regs
            .clock_control
            .modify(|_, w| w.software_reset_cmd(true));
        // wait for reset completion
        while self.regs.clock_control.read().software_reset_cmd() {}
        Ok(self.card_type)
    }
    /// Modify transfer block size.
    fn set_block_size(&mut self, block_size: u16) -> Result<(), CmdTransferError> {
        use cmd::SdCmd::*;
        let state = self.regs.present_state.read();
        if state.command_inhibit_cmd()
            || state.command_inhibit_dat()
            || state.write_transfer_active()
            || state.read_transfer_active()
        {
            return Err(CmdTransferError::CmdInhibited);
        }
        debug!("Set block size to {}", block_size);
        // send block write command
        self.cmd_transfer(CMD16, block_size as u32, 0)?;
        // set block size
        self.regs
            .block_size_block_count
            .modify(|_, w| w.transfer_block_size(block_size));
        Ok(())
    }
    /// Check if error occured, and reset the error status.
    /// Return Err(CmdTransferError) if error occured, Ok(()) otherwise.
    fn check_error(
        &mut self,
        status: &regs::interrupt_status::Read,
    ) -> Result<(), CmdTransferError> {
        if status.error_interrupt() {
            let err_status = if status.inner & 0xFFFE0000 == 0 {
                CmdTransferError::CmdTimeout
            } else {
                CmdTransferError::Other(regs::interrupt_status::Read {
                    inner: status.inner,
                })
            };
            // reset all error status
            self.regs
                .interrupt_status
                .write(regs::interrupt_status::Write { inner: 0xF3FF0000 });
            return Err(err_status);
        }
        Ok(())
    }
 }
--- a/libboard_zynq/src/sdio/regs.rs
+++ b/libboard_zynq/src/sdio/regs.rs
@ -0,0 +1,548 @@
 use core::fmt;
 use libregister::{register, register_at, register_bit, register_bits, register_bits_typed};
 use volatile_register::{RO, RW};
 #[allow(unused)]
 #[repr(C)]
 pub struct RegisterBlock {
    pub sdma_system_address: RW<u32>,
    pub block_size_block_count: BlockSizeBlockCount,
    pub argument: RW<u32>,
    pub transfer_mode_command: TransferModeCommand,
    pub responses: [RO<u32>; 4],
    pub buffer: RW<u32>,
    pub present_state: PresentState,
    /// Host. power, block gap, wakeup control
    pub control: Control,
    /// Clock and timeout control, and software reset register.
    pub clock_control: ClockControl,
    pub interrupt_status: InterruptStatus,
    pub interrupt_status_en: InterruptStatusEn,
    pub interrupt_signal_en: InterruptSignalEn,
    pub auto_cmd12_error_status: AutoCmd12ErrorStatus,
    pub capabilities: Capabilities,
    pub unused0: RO<u32>,
    pub max_current_capabilities: MaxCurrentCapabilities,
    pub unused1: RO<u32>,
    pub force_event: ForceEvent,
    pub adma_error_status: AdmaErrorStatus,
    pub adma_system_address: RW<u32>,
    pub unused2: RO<u32>,
    pub boot_data_timeout_counter: RW<u32>,
    pub debug_selection: DebugSelection,
    pub unused3: [RO<u32>; 34],
    pub spi_interrupt_support: SpiInterruptSupport,
    pub unused4: [RO<u32>; 2],
    pub misc_reg: MiscReg,
 }
 #[allow(unused)]
 #[repr(u8)]
 pub enum CommandType {
    Normal = 0b00,
    Suspend = 0b01,
    Resume = 0b10,
    Abort = 0b11,
 }
 #[allow(unused)]
 #[repr(u8)]
 pub enum ResponseTypeSelect {
    NoResponse = 0b00,
    Length136 = 0b01,
    Length48 = 0b10,
    Legnth48Check = 0b11,
 }
 #[allow(unused)]
 #[repr(u8)]
 #[derive(PartialEq, Debug)]
 pub enum BusVoltage {
    /// 3.3V
    V33 = 0b111,
    /// 3.0V, typ.
    V30 = 0b110,
    /// 1.8V, typ.
    V18 = 0b101,
    /// No power,
    V0 = 0b000,
 }
 #[allow(unused)]
 #[repr(u8)]
 pub enum DmaSelect {
    SDMA = 0b00,
    ADMA1 = 0b01,
    ADMA2_32 = 0b10,
    ADMA2_64 = 0b11,
 }
 #[allow(unused)]
 #[repr(u8)]
 pub enum AdmaErrorState {
    StStop = 0b00,
    StFds = 0b01,
    StTfr = 0b11,
 }
 #[allow(unused)]
 #[repr(u8)]
 #[derive(PartialEq)]
 pub enum SpecificationVersion {
    V1 = 0,
    V2 = 1,
    V3 = 2,
 }
 register_at!(RegisterBlock, 0xE0100000, sdio0);
 register_at!(RegisterBlock, 0xE0101000, sdio1);
 register!(block_size_block_count, BlockSizeBlockCount, RW, u32);
 register_bits!(
    block_size_block_count,
    /// Current transfer block count.
    blocks_count,
    u16,
    16,
    31
 );
 register_bits!(
    block_size_block_count,
    /// Host SDMA Buffer Size, size = 2^(val + 2) KB.
    dma_buffer_size,
    u8,
    12,
    14
 );
 register_bits!(
    block_size_block_count,
    /// Block size for data transfer. Unit: byte.
    transfer_block_size,
    u16,
    0,
    11
 );
 register!(transfer_mode_command, TransferModeCommand, RW, u32);
 register_bits!(
    transfer_mode_command,
    /// Command Number.
    command_index,
    u8,
    24,
    29
 );
 register_bits_typed!(
    transfer_mode_command,
    /// Command type register.
    command_type,
    u8,
    CommandType,
    22,
    23
 );
 register_bit!(
    transfer_mode_command,
    /// 1 if data is present and shall be transferred using the DAT line.
    data_present_select,
    21
 );
 register_bit!(
    transfer_mode_command,
    /// If the index field shall be checked.
    index_check_en,
    20
 );
 register_bit!(
    transfer_mode_command,
    /// If CRC shall be checked.
    crc_check_en,
    19
 );
 register_bits_typed!(
    transfer_mode_command,
    /// Different type of response.
    response_type_select,
    u8,
    ResponseTypeSelect,
    16,
    17
 );
 register_bit!(
    transfer_mode_command,
    /// Enables the multi block DAT line data transfer.
    multi_block_en,
    5
 );
 register_bit!(
    transfer_mode_command,
    /// 1 if read (card to host), 0 if write (host to card).
    direction_select,
    4
 );
 register_bit!(
    transfer_mode_command,
    /// If CMD12 shall be issued automatically when last block transfer is completed.
    auto_cmd12_en,
    2
 );
 register_bit!(
    transfer_mode_command,
    /// Enable the block count register.
    block_count_en,
    1
 );
 register_bit!(
    transfer_mode_command,
    /// Enable DMA,
    dma_en,
    0
 );
 register!(present_state, PresentState, RO, u32);
 register_bit!(
    present_state,
    /// CMD Line Signal Level.
    cmd_line_level,
    24
 );
 register_bit!(
    present_state,
    /// Signal level in DAT[3]
    dat3_level,
    23
 );
 register_bit!(
    present_state,
    /// Signal level in DAT[2]
    dat2_level,
    22
 );
 register_bit!(
    present_state,
    /// Signal level in DAT[1]
    dat1_level,
    21
 );
 register_bit!(
    present_state,
    /// Signal level in DAT[0]
    dat0_level,
    20
 );
 register_bit!(
    present_state,
    /// Write enabled and inverse of SDx_WP pin level.
    write_enabled,
    19
 );
 register_bit!(
    present_state,
    /// Card detected and inverse of SDx_CDn pin level.
    card_detected,
    18
 );
 register_bit!(present_state, card_state_stable, 17);
 register_bit!(present_state, card_inserted, 16);
 register_bit!(present_state, buffer_read_en, 11);
 register_bit!(present_state, buffer_write_en, 10);
 register_bit!(present_state, read_transfer_active, 9);
 register_bit!(present_state, write_transfer_active, 8);
 register_bit!(present_state, dat_line_active, 2);
 register_bit!(present_state, command_inhibit_dat, 1);
 register_bit!(present_state, command_inhibit_cmd, 0);
 register!(control, Control, RW, u32);
 register_bit!(
    control,
    /// Enable wakeup event via SD card removal assertion.
    wakeup_on_removal,
    26
 );
 register_bit!(
    control,
    /// Enable wakeup event via SD card insertion assertion.
    wakeup_on_insertion,
    25
 );
 register_bit!(
    control,
    /// Enable wakeup event via card interrupt assertion.
    wakeup_on_interrupt,
    24
 );
 register_bit!(
    control,
    ///Enable interrupt detection at the block gap for a multiple block transfer.
    interrupt_at_block_gap,
    19
 );
 register_bit!(
    control,
    /// Enable the use of the read wait protocol.
    read_wait_control,
    18
 );
 register_bit!(
    control,
    /// Restart a trasaction which was stopped using the stop at block gap request.
    continue_req,
    17
 );
 register_bit!(
    control,
    /// Stop executing a transaction at the next block gap.
    stop_at_block_gap_req,
    16
 );
 register_bits_typed!(control, bus_voltage, u8, BusVoltage, 9, 11);
 register_bit!(control, bus_power, 8);
 register_bit!(
    control,
    /// Selects source for card detection. 0 for SDCD#, 1 for card detect test level.
    card_detect_signal,
    7
 );
 register_bit!(
    control,
    /// Indicates card inserted or not. Enabled when card detect signal is 1.
    card_detect_test_level,
    6
 );
 register_bits_typed!(control, dma_select, u8, DmaSelect, 3, 4);
 register_bit!(control, high_speed_en, 2);
 register_bit!(
    control,
    /// Select the data width of the HC. 1 for 4-bit, 0 for 1-bit.
    data_width_select,
    1
 );
 register_bit!(
    control,
    /// 1 for LED on, 0 for LED off.
    led_control,
    0
 );
 register!(clock_control, ClockControl, RW, u32);
 register_bit!(
    clock_control,
    /// Software reset for DAT line.
    software_reset_dat,
    26
 );
 register_bit!(
    clock_control,
    /// Software reset for CMD line.
    software_reset_cmd,
    25
 );
 register_bit!(
    clock_control,
    /// Software reset for ALL.
    software_reset_all,
    24
 );
 register_bits!(
    clock_control,
    /// Determines the interval by which DAT line time-outs are detected.
    /// Interval = TMCLK * 2^(13 + val)
    /// Note: 0b1111 is reserved.
    timeout_counter_value,
    u8,
    16,
    19
 );
 register_bits!(
    clock_control,
    /// Selects the frequency divisor, thus the clock frequency for SDCLK.
    /// Choose the smallest possible divisor which results in a clock frequency
    /// that is less than or equal to the target frequency.
    sdclk_freq_divisor,
    u8,
    8,
    15
 );
 register_bit!(clock_control, sd_clk_en, 2);
 register_bit!(
    clock_control,
    /// 1 when SD clock is stable.
    /// Note that this field is read-only.
    internal_clk_stable,
    1,
    RO
 );
 register_bit!(clock_control, internal_clk_en, 0);
 register!(interrupt_status, InterruptStatus, RW, u32, 1 << 15 | 1 << 8);
 register_bit!(interrupt_status, ceata_error, 29, WTC);
 register_bit!(interrupt_status, target_response_error, 28, WTC);
 register_bit!(interrupt_status, adma_error, 25, WTC);
 register_bit!(interrupt_status, auto_cmd12_error, 24, WTC);
 register_bit!(interrupt_status, current_limit_error, 23, WTC);
 register_bit!(interrupt_status, data_end_bit_error, 22, WTC);
 register_bit!(interrupt_status, data_crc_error, 21, WTC);
 register_bit!(interrupt_status, data_timeout_error, 20, WTC);
 register_bit!(interrupt_status, command_index_error, 19, WTC);
 register_bit!(interrupt_status, command_end_bit_error, 18, WTC);
 register_bit!(interrupt_status, command_crc_error, 17, WTC);
 register_bit!(interrupt_status, command_timeout_error, 16, WTC);
 register_bit!(interrupt_status, error_interrupt, 15, RO);
 register_bit!(interrupt_status, boot_terminate_interrupt, 10, WTC);
 register_bit!(interrupt_status, boot_ack_rcv, 9, WTC);
 register_bit!(interrupt_status, card_interrupt, 8, RO);
 register_bit!(interrupt_status, card_removal, 7, WTC);
 register_bit!(interrupt_status, card_insertion, 6, WTC);
 register_bit!(interrupt_status, buffer_read_ready, 5, WTC);
 register_bit!(interrupt_status, buffer_write_ready, 4, WTC);
 register_bit!(interrupt_status, dma_interrupt, 3, WTC);
 register_bit!(interrupt_status, block_gap_event, 2, WTC);
 register_bit!(interrupt_status, transfer_complete, 1, WTC);
 register_bit!(interrupt_status, command_complete, 0, WTC);
 register!(interrupt_status_en, InterruptStatusEn, RW, u32);
 register_bit!(interrupt_status_en, ceata_error_status_en, 29);
 register_bit!(interrupt_status_en, target_response_error_status_en, 28);
 register_bit!(interrupt_status_en, adma_error_status_en, 25);
 register_bit!(interrupt_status_en, auto_cmd12_error_status_en, 24);
 register_bit!(interrupt_status_en, current_limit_error_status_en, 23);
 register_bit!(interrupt_status_en, data_end_bit_error_status_en, 22);
 register_bit!(interrupt_status_en, data_crc_error_status_en, 21);
 register_bit!(interrupt_status_en, data_timeout_error_status_en, 20);
 register_bit!(interrupt_status_en, cmd_index_error_status_en, 19);
 register_bit!(interrupt_status_en, cmd_end_bit_error_status_en, 18);
 register_bit!(interrupt_status_en, cmd_crc_error_status_en, 17);
 register_bit!(interrupt_status_en, cmd_timeout_error_status_en, 16);
 register_bit!(interrupt_status_en, fixed_to_0, 15, RO);
 register_bit!(interrupt_status_en, boot_terminate_interrupt_en, 10);
 register_bit!(interrupt_status_en, boot_ack_rcv_en, 9);
 register_bit!(interrupt_status_en, card_interrupt_status_en, 8);
 register_bit!(interrupt_status_en, card_removal_status_en, 7);
 register_bit!(interrupt_status_en, card_insertion_status_en, 6);
 register_bit!(interrupt_status_en, buffer_read_ready_status_en, 5);
 register_bit!(interrupt_status_en, buffer_write_ready_status_en, 4);
 register_bit!(interrupt_status_en, dma_interrupt_status_en, 3);
 register_bit!(interrupt_status_en, block_gap_evt_status_en, 2);
 register_bit!(interrupt_status_en, transfer_complete_status_en, 1);
 register_bit!(interrupt_status_en, cmd_complete_status_en, 0);
 register!(interrupt_signal_en, InterruptSignalEn, RW, u32);
 register_bit!(interrupt_signal_en, ceata_error_signal_en, 29);
 register_bit!(interrupt_signal_en, target_response_error_signal_en, 28);
 register_bit!(interrupt_signal_en, adma_error_signal_en, 25);
 register_bit!(interrupt_signal_en, auto_cmd12_error_signal_en, 24);
 register_bit!(interrupt_signal_en, current_limit_error_signal_en, 23);
 register_bit!(interrupt_signal_en, data_end_bit_error_signal_en, 22);
 register_bit!(interrupt_signal_en, data_crc_error_signal_en, 21);
 register_bit!(interrupt_signal_en, data_timeout_error_signal_en, 20);
 register_bit!(interrupt_signal_en, cmd_index_error_signal_en, 19);
 register_bit!(interrupt_signal_en, cmd_end_bit_error_signal_en, 18);
 register_bit!(interrupt_signal_en, cmd_crc_error_signal_en, 17);
 register_bit!(interrupt_signal_en, cmd_timeout_error_signal_en, 16);
 register_bit!(interrupt_signal_en, fixed_to_0, 15, RO);
 register_bit!(interrupt_signal_en, boot_terminate_interrupt_signal_en, 10);
 register_bit!(interrupt_signal_en, boot_ack_rcv_signal_en, 9);
 register_bit!(interrupt_signal_en, card_interrupt_signal_en, 8);
 register_bit!(interrupt_signal_en, card_removal_signal_en, 7);
 register_bit!(interrupt_signal_en, card_insertion_signal_en, 6);
 register_bit!(interrupt_signal_en, buffer_read_ready_signal_en, 5);
 register_bit!(interrupt_signal_en, buffer_write_ready_signal_en, 4);
 register_bit!(interrupt_signal_en, dma_interrupt_signal_en, 3);
 register_bit!(interrupt_signal_en, block_gap_evt_signal_en, 2);
 register_bit!(interrupt_signal_en, transfer_complete_signal_en, 1);
 register_bit!(interrupt_signal_en, cmd_complete_signal_en, 0);
 register!(auto_cmd12_error_status, AutoCmd12ErrorStatus, RO, u32);
 register_bit!(
    auto_cmd12_error_status,
    cmd_not_issued_by_auto_cmd12_error,
    7
 );
 register_bit!(auto_cmd12_error_status, index_error, 4);
 register_bit!(auto_cmd12_error_status, end_bit_error, 3);
 register_bit!(auto_cmd12_error_status, crc_error, 2);
 register_bit!(auto_cmd12_error_status, timeout_error, 1);
 register_bit!(auto_cmd12_error_status, not_executed, 0);
 register!(capabilities, Capabilities, RO, u32);
 register_bit!(capabilities, spi_block_mode, 30);
 register_bit!(capabilities, spi_mode, 29);
 register_bit!(capabilities, support_64bit, 28);
 register_bit!(capabilities, interrupt_mode, 27);
 register_bit!(capabilities, voltage_1_8, 26);
 register_bit!(capabilities, voltage_3_0, 25);
 register_bit!(capabilities, voltage_3_3, 24);
 register_bit!(capabilities, suspend_resume, 23);
 register_bit!(capabilities, sdma, 22);
 register_bit!(capabilities, hgih_speed, 21);
 register_bit!(capabilities, adma2, 19);
 register_bit!(capabilities, extended_media_bus, 18);
 register_bits!(
    capabilities,
    /// Length = 2^(9 + v) bytes.
    max_block_len,
    u8,
    16,
    17
 );
 register_bit!(capabilities, timeout_clock_unit, 7);
 register!(max_current_capabilities, MaxCurrentCapabilities, RO, u32);
 register_bits!(max_current_capabilities, max_current_1_8v, u8, 16, 23);
 register_bits!(max_current_capabilities, max_current_3_0v, u8, 8, 15);
 register_bits!(max_current_capabilities, max_current_3_3v, u8, 0, 7);
 register!(force_event, ForceEvent, WO, u32);
 register_bit!(force_event, ceata_error, 29);
 register_bit!(force_event, target_response_error, 28);
 register_bit!(force_event, adma_error, 25);
 register_bit!(force_event, auto_cmd12_error, 24);
 register_bit!(force_event, current_limit_error, 23);
 register_bit!(force_event, data_end_bit_error, 22);
 register_bit!(force_event, data_crc_error, 21);
 register_bit!(force_event, data_timeout_error, 20);
 register_bit!(force_event, cmd_index_error, 19);
 register_bit!(force_event, cmd_end_bit_error, 18);
 register_bit!(force_event, cmd_crc_error, 17);
 register_bit!(force_event, cmd_timeout_error, 16);
 register_bit!(force_event, cmd_not_issued_by_auto_cmd12_error, 7);
 register_bit!(force_event, auto_cmd12_index_error, 4);
 register_bit!(force_event, auto_cmd12_end_bit_error, 3);
 register_bit!(force_event, auto_cmd12_crc_error, 2);
 register_bit!(force_event, auto_cmd12_timeout_error, 1);
 register_bit!(force_event, auto_cmd12_not_executed, 0);
 register!(adma_error_status, AdmaErrorStatus, RW, u32, 0b11);
 register_bit!(adma_error_status, length_mismatch_error, 2, WTC);
 register_bits_typed!(adma_error_status, error_state, u8, AdmaErrorState, 0, 1);
 register!(debug_selection, DebugSelection, WO, u32);
 register_bit!(debug_selection, debug_select, 0);
 register!(spi_interrupt_support, SpiInterruptSupport, RW, u32);
 register_bits!(
    spi_interrupt_support,
    /// There should be a problem with the documentation of this field.
    spi_int_support,
    u8,
    0,
    7
 );
 register!(misc_reg, MiscReg, RO, u32);
 register_bits!(misc_reg, vendor_version_num, u8, 24, 31);
 register_bits_typed!(misc_reg, spec_ver, u8, SpecificationVersion, 16, 23);
 register_bits!(
    misc_reg,
    /// Logical OR of interrupt signal and wakeup signal for each slot.
    slot_interrupt_signal,
    u8,
    0,
    7
 );
 impl fmt::Debug for interrupt_status::Read {
    fn fmt(&self, fmt: &mut fmt::Formatter) -> fmt::Result {
        fmt.write_fmt(format_args!("status: {:0X}", self.inner))
    }
 }
--- a/libboard_zynq/src/sdio/sd_card.rs
+++ b/libboard_zynq/src/sdio/sd_card.rs
@ -0,0 +1,379 @@
 use super::{adma::Adma2DescTable, cmd, CardType, CmdTransferError, Sdio};
 use libcortex_a9::cache;
 use libregister::{RegisterR, RegisterRW, RegisterW};
 use log::{trace, debug};
 #[derive(Debug)]
 pub enum CardInitializationError {
    AlreadyInitialized,
    NoCardInserted,
    InitializationFailedOther,
    InitializationFailedCmd(CmdTransferError),
 }
 impl core::fmt::Display for CardInitializationError {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        use CardInitializationError::*;
        write!(f, "Card initialization error: ")?;
        match self {
            AlreadyInitialized => write!(f, "Card already initialized."),
            NoCardInserted => write!(f, "No card inserted, check if the card is inserted properly."),
            InitializationFailedOther => write!(f, "Unknown error. Please check the debug messages."),
            InitializationFailedCmd(x) => write!(f, "{}", x)
        }
    }
 }
 impl From<CmdTransferError> for CardInitializationError {
    fn from(error: CmdTransferError) -> Self {
        CardInitializationError::InitializationFailedCmd(error)
    }
 }
 #[derive(Debug)]
 enum CardVersion {
    SdVer1,
    SdVer2,
 }
 pub struct SdCard {
    sdio: Sdio,
    adma2_desc_table: Adma2DescTable,
    card_version: CardVersion,
    hcs: bool,
    card_id: [u32; 4],
    rel_card_addr: u32,
    sector_cnt: u32,
    switch_1v8: bool,
    width_4_bit: bool,
 }
 const BLK_SIZE_MASK: u16 = 0x00000FFF;
 impl core::fmt::Display for SdCard {
    fn fmt(&self, f: &mut core::fmt::Formatter) -> core::fmt::Result {
        write!(f, "SdCard: \n  card version: {:?}\n  hcs: {}\n  card id: {:?}\n rel card addr: {}\n sector count: {}",
            self.card_version, self.hcs, self.card_id, self.rel_card_addr, self.sector_cnt)
    }
 }
 impl SdCard {
    fn sd_card_initialize(&mut self) -> Result<(), CardInitializationError> {
        use cmd::{args::*, SdCmd::*};
        if !self.sdio.is_card_inserted() {
            return Err(CardInitializationError::NoCardInserted);
        }
        // CMD0
        self.sdio.cmd_transfer(CMD0, 0, 0)?;
        match self.sdio.cmd_transfer(CMD8, CMD8_VOL_PATTERN, 0) {
            Err(CmdTransferError::CmdTimeout) => {
                // reset
                self.sdio
                    .regs
                    .clock_control
                    .modify(|_, w| w.software_reset_cmd(true));
                // wait until reset is completed
                while self.sdio.regs.clock_control.read().software_reset_cmd() {}
            }
            // for other error, return initialization failed
            Err(e) => return Err(CardInitializationError::from(e)),
            _ => (),
        }
        self.card_version = if self.sdio.regs.responses[0].read() != CMD8_VOL_PATTERN {
            CardVersion::SdVer1
        } else {
            CardVersion::SdVer2
        };
        // send ACMD41 while card is still busy with power up
        loop {
            self.sdio.cmd_transfer(CMD55, 0, 0)?;
            self.sdio
                .cmd_transfer(ACMD41, ACMD41_HCS | ACMD41_3V3 | (0x1FF << 15), 0)?;
            if (self.sdio.regs.responses[0].read() & RESPOCR_READY) != 0 {
                break;
            }
        }
        let response = self.sdio.regs.responses[0].read();
        // update HCS support flag
        self.hcs = (response & ACMD41_HCS) != 0;
        if (response & OCR_S18) != 0 {
            self.switch_1v8 = true;
            self.sdio.switch_voltage()?;
        }
        self.sdio.cmd_transfer(CMD2, 0, 0)?;
        for i in 0..=3 {
            self.card_id[i] = self.sdio.regs.responses[i].read();
        }
        self.rel_card_addr = 0;
        while self.rel_card_addr == 0 {
            self.sdio.cmd_transfer(CMD3, 0, 0)?;
            self.rel_card_addr = self.sdio.regs.responses[0].read() & 0xFFFF0000;
        }
        self.sdio.cmd_transfer(CMD9, self.rel_card_addr, 0)?;
        self.sdio
            .regs
            .interrupt_status
            .modify(|_, w| w.transfer_complete());
        let mut csd: [u32; 4] = [0, 0, 0, 0];
        for i in 0..=3 {
            csd[i] = self.sdio.regs.responses[i].read();
            trace!("CSD[{}] = {:0X}", i, csd[i]);
        }
        const CSD_STRUCT_MSK: u32 = 0x00C00000;
        const C_SIZE_MULT_MASK: u32 = 0x00000380;
        const C_SIZE_LOWER_MASK: u32 = 0xFFC00000;
        const C_SIZE_UPPER_MASK: u32 = 0x00000003;
        const READ_BLK_LEN_MASK: u32 = 0x00000F00;
        const CSD_V2_C_SIZE_MASK: u32 = 0x3FFFFF00;
        const XSDPS_BLK_SIZE_512_MASK: u32 = 0x200;
        if ((csd[3] & CSD_STRUCT_MSK) >> 22) == 0 {
            let blk_len = 1 << ((csd[2] & READ_BLK_LEN_MASK) >> 8);
            let mult = 1 << (((csd[1] & C_SIZE_MULT_MASK) >> 7) + 2);
            let mut device_size = (csd[1] & C_SIZE_LOWER_MASK) >> 22;
            device_size |= (csd[2] & C_SIZE_UPPER_MASK) << 10;
            device_size = (device_size + 1) * mult;
            device_size = device_size * blk_len;
            self.sector_cnt = device_size / XSDPS_BLK_SIZE_512_MASK;
        } else if ((csd[3] & CSD_STRUCT_MSK) >> 22) == 1 {
            self.sector_cnt = (((csd[1] & CSD_V2_C_SIZE_MASK) >> 8) + 1) * 1024;
        } else {
            return Err(CardInitializationError::InitializationFailedOther);
        }
        self.sdio.change_clk_freq(25_000_000);
        // CMD7: select card
        self.sdio.cmd_transfer(CMD7, self.rel_card_addr, 0)?;
        // pull up
        self.sdio.cmd_transfer(CMD55, self.rel_card_addr, 0)?;
        self.sdio.cmd_transfer(ACMD42, 0, 0)?;
        let mut scr: [u8; 32] = [0; 32];
        self.get_bus_width(&mut scr)?;
        trace!("SCR={:?}", scr);
        if scr[1] & 0x4 != 0 {
            // 4bit support
            debug!("4 bit support");
            self.change_bus_width()?;
        }
        self.sdio.set_block_size(512)?;
        Ok(())
    }
    /// Convert Sdio into SdCard struct, error if no card inserted or it is not an SD card.
    pub fn from_sdio(mut sdio: Sdio) -> Result<Self, CardInitializationError> {
        match sdio.identify_card()? {
            CardType::CardSd => (),
            _ => return Err(CardInitializationError::NoCardInserted),
        };
        let mut _self = SdCard {
            sdio,
            adma2_desc_table: Adma2DescTable::new(),
            card_version: CardVersion::SdVer1,
            hcs: false,
            card_id: [0, 0, 0, 0],
            rel_card_addr: 0,
            sector_cnt: 0,
            switch_1v8: false,
            width_4_bit: false,
        };
        _self.sd_card_initialize()?;
        Ok(_self)
    }
    /// Convert SdCard struct back to Sdio struct.
    pub fn to_sdio(self) -> Sdio {
        self.sdio
    }
    /// read blocks starting from an address. Each block has length 512 byte.
    /// Note that the address is block address, i.e. 0 for 0~512, 1 for 512~1024, etc.
    pub fn read_block(
        &mut self,
        address: u32,
        block_cnt: u16,
        buffer: &mut [u8],
    ) -> Result<(), CmdTransferError> {
        assert!(buffer.len() >= (block_cnt as usize) * 512);
        // set block size if not set already
        if self
            .sdio
            .regs
            .block_size_block_count
            .read()
            .transfer_block_size()
            != 512
        {
            self.sdio.set_block_size(512)?;
        }
        let real_addr = if self.hcs {
            address
        } else {
            // standard capacity card uses byte address
            address * 0x200
        };
        self.adma2_desc_table.setup(&mut self.sdio, block_cnt as u32, buffer);
        // invalidate D cache, required for ZC706, not sure for Cora Z7 10
        cache::dcci_slice(buffer);
        let cmd = if block_cnt == 1 {
            cmd::SdCmd::CMD17
        } else {
            cmd::SdCmd::CMD18
        };
        let mode = if block_cnt == 1 {
            super::regs::TransferModeCommand::zeroed()
                .block_count_en(true)
                .direction_select(true)
                .dma_en(true)
        } else {
            super::regs::TransferModeCommand::zeroed()
                .auto_cmd12_en(true)
                .block_count_en(true)
                .direction_select(true)
                .multi_block_en(true)
                .dma_en(true)
        };
        self.sdio
            .cmd_transfer_with_mode(cmd, real_addr, block_cnt, mode)?;
        self.wait_transfer_complete()?;
        cache::dcci_slice(buffer);
        Ok(())
    }
    /// write blocks starting from an address. Each block has length 512 byte.
    /// Note that the address is block address, i.e. 0 for 0~512, 1 for 512~1024, etc.
    pub fn write_block(
        &mut self,
        address: u32,
        block_cnt: u16,
        buffer: &[u8],
    ) -> Result<(), CmdTransferError> {
        assert!(buffer.len() >= (block_cnt as usize) * 512);
        // set block size if not set already
        if self
            .sdio
            .regs
            .block_size_block_count
            .read()
            .transfer_block_size()
            != 512
        {
            self.sdio.set_block_size(512)?;
        }
        let real_addr = if self.hcs {
            address
        } else {
            // standard capacity card uses byte address
            address * 0x200
        };
        self.adma2_desc_table.setup(&mut self.sdio, block_cnt as u32, buffer);
        // invalidate D cache, required for ZC706, not sure for Cora Z7 10
        cache::dcci_slice(buffer);
        let cmd = if block_cnt == 1 {
            cmd::SdCmd::CMD24
        } else {
            cmd::SdCmd::CMD25
        };
        let mode = if block_cnt == 1 {
            super::regs::TransferModeCommand::zeroed()
                .block_count_en(true)
                .dma_en(true)
        } else {
            super::regs::TransferModeCommand::zeroed()
                .auto_cmd12_en(true)
                .block_count_en(true)
                .multi_block_en(true)
                .dma_en(true)
        };
        self.sdio
            .cmd_transfer_with_mode(cmd, real_addr, block_cnt, mode)?;
        // wait for transfer complete interrupt
        self.wait_transfer_complete()?;
        cache::dcci_slice(buffer);
        Ok(())
    }
    fn get_bus_width(&mut self, buf: &mut [u8]) -> Result<(), CmdTransferError> {
        use cmd::SdCmd::*;
        debug!("Getting bus width");
        for i in 0..8 {
            buf[i] = 0;
        }
        // send block write command
        self.sdio.cmd_transfer(CMD55, self.rel_card_addr, 0)?;
        let blk_cnt: u16 = 1;
        let blk_size: u16 = 8 & BLK_SIZE_MASK;
        self.sdio
            .regs
            .block_size_block_count
            .modify(|_, w| w.transfer_block_size(blk_size));
        self.adma2_desc_table.setup(&mut self.sdio, blk_cnt as u32, buf);
        cache::dcci_slice(buf);
        self.sdio.cmd_transfer_with_mode(
            ACMD51,
            0,
            blk_cnt,
            super::regs::TransferModeCommand::zeroed()
                .dma_en(true)
                .direction_select(true),
        )?;
        self.wait_transfer_complete()?;
        cache::dcci_slice(buf);
        Ok(())
    }
    fn change_bus_width(&mut self) -> Result<(), CmdTransferError> {
        use cmd::SdCmd::*;
        debug!("Changing bus width");
        self.sdio.cmd_transfer(CMD55, self.rel_card_addr, 0)?;
        self.width_4_bit = true;
        self.sdio.cmd_transfer(ACMD6, 0x2, 0)?;
        self.sdio.delay(1);
        self.sdio
            .regs
            .control
            .modify(|_, w| w.data_width_select(true));
        Ok(())
    }
    fn wait_transfer_complete(&mut self) -> Result<(), CmdTransferError> {
        trace!("Wait for transfer complete");
        let mut status = self.sdio.regs.interrupt_status.read();
        while !status.transfer_complete() {
            self.sdio.check_error(&status)?;
            status = self.sdio.regs.interrupt_status.read();
        }
        trace!("Clearing transfer complete");
        self.sdio
            .regs
            .interrupt_status
            .modify(|_, w| w.transfer_complete());
        Ok(())
    }
 }
--- a/libboard_zynq/src/slcr.rs
+++ b/libboard_zynq/src/slcr.rs
@ -1,15 +1,19 @@
 ///! Register definitions for System Level Control
 use volatile_register::{RO, RW};
-use crate::{register, register_at,
+use libregister::{
    register, register_at,
    register_bit, register_bits, register_bits_typed,
-            regs::RegisterW, regs::RegisterRW};
+    RegisterW, RegisterRW,
 };
 #[repr(u8)]
 pub enum PllSource {
-    IoPll  = 0b00,
+    IoPll  = 0b000,
-    ArmPll = 0b10,
+    ArmPll = 0b010,
-    DdrPll = 0b11,
+    DdrPll = 0b011,
    // Ethernet controller 0 EMIO clock
    Emio   = 0b100,
 }
 #[repr(u8)]
@ -19,6 +23,52 @@ pub enum ArmPllSource {
    IoPll  = 0b11,
 }
 #[repr(u8)]
 pub enum DdriobInputType {
    Off = 0b00,
    /// For SSTL, HSTL
    VrefDifferential = 0b01,
    Differential = 0b10,
    Lvcmos = 0b11,
 }
 #[repr(u8)]
 pub enum DdriobDciType {
    /// DDR2/3L Addr and Clock
    Disabled = 0b00,
    /// LPDDR2
    Drive = 0b01,
    /// DDR2/3/3L Data and Diff
    Termination = 0b11,
 }
 #[repr(u8)]
 pub enum DdriobOutputEn {
    Ibuf = 0b00,
    Obuf = 0b11,
 }
 #[repr(u8)]
 pub enum DdriobVrefSel {
    /// For LPDDR2 with 1.2V IO
    Vref0_6V = 0b0001,
    /// For DDR3L with 1.35V IO
    Vref0_675V = 0b0010,
    /// For DDR3 with 1.5V IO
    Vref0_75V = 0b0100,
    /// For DDR2 with 1.8V IO
    Vref0_9V = 0b1000,
 }
 #[repr(u8)]
 pub enum LevelShifterEnable {
    DisableAll = 0x0,
    EnablePsToPl = 0xA,
    EnableAll = 0xF,
 }
 #[repr(C)]
 pub struct RegisterBlock {
    pub scl: RW<u32>,
@ -29,14 +79,14 @@ pub struct RegisterBlock {
    pub arm_pll_ctrl: PllCtrl,
    pub ddr_pll_ctrl: PllCtrl,
    pub io_pll_ctrl: PllCtrl,
-    pub pll_status: RO<u32>,
+    pub pll_status: PllStatus,
-    pub arm_pll_cfg: RW<u32>,
+    pub arm_pll_cfg: PllCfg,
-    pub ddr_pll_cfg: RW<u32>,
+    pub ddr_pll_cfg: PllCfg,
-    pub io_pll_cfg: RW<u32>,
+    pub io_pll_cfg: PllCfg,
    reserved1: [u32; 1],
    pub arm_clk_ctrl: ArmClkCtrl,
-    pub ddr_clk_ctrl: RW<u32>,
+    pub ddr_clk_ctrl: DdrClkCtrl,
-    pub dci_clk_ctrl: RW<u32>,
+    pub dci_clk_ctrl: DciClkCtrl,
    pub aper_clk_ctrl: AperClkCtrl,
    pub usb0_clk_ctrl: RW<u32>,
    pub usb1_clk_ctrl: RW<u32>,
@ -45,8 +95,8 @@ pub struct RegisterBlock {
    pub gem0_clk_ctrl: GemClkCtrl,
    pub gem1_clk_ctrl: GemClkCtrl,
    pub smc_clk_ctrl: RW<u32>,
-    pub lqspi_clk_ctrl: RW<u32>,
+    pub lqspi_clk_ctrl: LqspiClkCtrl,
-    pub sdio_clk_ctrl: RW<u32>,
+    pub sdio_clk_ctrl: SdioClkCtrl,
    pub uart_clk_ctrl: UartClkCtrl,
    pub spi_clk_ctrl: RW<u32>,
    pub can_clk_ctrl: RW<u32>,
@ -54,19 +104,19 @@ pub struct RegisterBlock {
    pub dbg_clk_ctrl: RW<u32>,
    pub pcap_clk_ctrl: RW<u32>,
    pub topsw_clk_ctrl: RW<u32>,
-    pub fpga0_clk_ctrl: RW<u32>,
+    pub fpga0_clk_ctrl: Fpga0ClkCtrl,
    pub fpga0_thr_ctrl: RW<u32>,
    pub fpga0_thr_cnt: RW<u32>,
    pub fpga0_thr_sta: RO<u32>,
-    pub fpga1_clk_ctrl: RW<u32>,
+    pub fpga1_clk_ctrl: Fpga1ClkCtrl,
    pub fpga1_thr_ctrl: RW<u32>,
    pub fpga1_thr_cnt: RW<u32>,
    pub fpga1_thr_sta: RO<u32>,
-    pub fpga2_clk_ctrl: RW<u32>,
+    pub fpga2_clk_ctrl: Fpga2ClkCtrl,
    pub fpga2_thr_ctrl: RW<u32>,
    pub fpga2_thr_cnt: RW<u32>,
    pub fpga2_thr_sta: RO<u32>,
-    pub fpga3_clk_ctrl: RW<u32>,
+    pub fpga3_clk_ctrl: Fpga3ClkCtrl,
    pub fpga3_thr_ctrl: RW<u32>,
    pub fpga3_thr_cnt: RW<u32>,
    pub fpga3_thr_sta: RO<u32>,
@ -79,23 +129,23 @@ pub struct RegisterBlock {
    pub dmac_rst_ctrl: RW<u32>,
    pub usb_rst_ctrl: RW<u32>,
    pub gem_rst_ctrl: RW<u32>,
-    pub sdio_rst_ctrl: RW<u32>,
+    pub sdio_rst_ctrl: SdioRstCtrl,
    pub spi_rst_ctrl: RW<u32>,
    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: RW<u32>,
+    pub lqspi_rst_ctrl: LqspiRstCtrl,
    pub smc_rst_ctrl: RW<u32>,
    pub ocm_rst_ctrl: RW<u32>,
    reserved4: [u32; 1],
-    pub fpga_rst_ctrl: RW<u32>,
+    pub fpga_rst_ctrl: FpgaRstCtrl,
    pub a9_cpu_rst_ctrl: A9CpuRstCtrl,
    reserved5: [u32; 1],
    pub rs_awdt_ctrl: RW<u32>,
    reserved6: [u32; 2],
    pub reboot_status: RW<u32>,
-    pub boot_mode: RW<u32>,
+    pub boot_mode: BootMode,
    reserved7: [u32; 40],
    pub apu_ctrl: RW<u32>,
    pub wdt_clk_sel: RW<u32>,
@ -178,7 +228,7 @@ pub struct RegisterBlock {
    pub sd0_wp_cd_sel: RW<u32>,
    pub sd1_wp_cd_sel: RW<u32>,
    reserved17: [u32; 50],
-    pub lvl_shftr_en: RW<u32>,
+    pub lvl_shftr_en: LvlShftr,
    reserved18: [u32; 3],
    pub ocm_cfg: RW<u32>,
    reserved19: [u32; 123],
@ -190,36 +240,62 @@ pub struct RegisterBlock {
    pub gpiob_cfg_hstl: RW<u32>,
    pub gpiob_drvr_bias_ctrl: RW<u32>,
    reserved21: [u32; 9],
-    pub ddriob_addr1: RW<u32>,
+    pub ddriob_addr0: DdriobConfig,
-    pub ddriob_data0: RW<u32>,
+    pub ddriob_addr1: DdriobConfig,
-    pub ddriob_data1: RW<u32>,
+    pub ddriob_data0: DdriobConfig,
-    pub ddriob_diff0: RW<u32>,
+    pub ddriob_data1: DdriobConfig,
-    pub ddriob_diff1: RW<u32>,
+    pub ddriob_diff0: DdriobConfig,
-    pub ddriob_clock: RW<u32>,
+    pub ddriob_diff1: DdriobConfig,
-    pub w_addr: RW<u32>,
+    pub ddriob_clock: DdriobConfig,
-    pub w_data: RW<u32>,
+    pub ddriob_drive_slew_addr: RW<u32>,
-    pub w_diff: RW<u32>,
+    pub ddriob_drive_slew_data: RW<u32>,
-    pub w_clock: RW<u32>,
+    pub ddriob_drive_slew_diff: RW<u32>,
-    pub ddriob_ddr_ctrl: RW<u32>,
+    pub ddriob_drive_slew_clock: RW<u32>,
-    pub ddriob_dci_ctrl: RW<u32>,
+    pub ddriob_ddr_ctrl: DdriobDdrCtrl,
-    pub ddriob_dci_status: RW<u32>,
+    pub ddriob_dci_ctrl: DdriobDciCtrl,
    pub ddriob_dci_status: DdriobDciStatus,
 }
-register_at!(RegisterBlock, 0xF8000000, new);
+register_at!(RegisterBlock, 0xF8000000, slcr);
 impl RegisterBlock {
    /// Required to modify any sclr register
    pub fn unlocked<F: FnMut(&mut Self) -> R, R>(mut f: F) -> R {
-        let mut self_ = Self::new();
+        let mut self_ = Self::slcr();
        self_.slcr_unlock.unlock();
        let r = f(&mut self_);
        self_.slcr_lock.lock();
        r
    }
-    /// Perform a soft reset
+    pub fn init_preload_fpga(&mut self) {
-    pub fn soft_reset(&mut self) {
+        // Assert FPGA top level output resets
-        self.pss_rst_ctrl.write(
+        self.fpga_rst_ctrl.write(
-            PssRstCtrl::zeroed()
+            FpgaRstCtrl::zeroed()
-                .soft_rst(true)
+                .fpga0_out_rst(true)
                .fpga1_out_rst(true)
                .fpga2_out_rst(true)
                .fpga3_out_rst(true)
        );
        // Disable level shifters
        self.lvl_shftr_en.write(
            LvlShftr::zeroed()
        );
        // Enable output level shifters
        self.lvl_shftr_en.write(
            LvlShftr::zeroed()
                .user_lvl_shftr_en(LevelShifterEnable::EnablePsToPl)
        );
    }
    pub fn init_postload_fpga(&mut self) {
        // Enable level shifters
        self.lvl_shftr_en.write(
            LvlShftr::zeroed()
                .user_lvl_shftr_en(LevelShifterEnable::EnableAll)
        );
        // Deassert AXI interface resets
        self.fpga_rst_ctrl.write(
            FpgaRstCtrl::zeroed()
        );
    }
 }
@ -247,12 +323,38 @@ impl SlcrUnlock {
 }
 register!(pll_ctrl, PllCtrl, RW, u32);
-register_bits!(pll_ctrl, pll_fdiv, u8, 12, 18);
+register_bits!(pll_ctrl, pll_fdiv, u16, 12, 18);
 register_bit!(pll_ctrl, pll_bypass_force, 4);
 register_bit!(pll_ctrl, pll_bypass_qual, 3);
 register_bit!(pll_ctrl, pll_pwrdwn, 1);
 register_bit!(pll_ctrl, pll_reset, 0);
 register!(pll_status, PllStatus, RO, u32);
 register_bit!(pll_status, arm_pll_lock, 0);
 register_bit!(pll_status, ddr_pll_lock, 1);
 register_bit!(pll_status, io_pll_lock, 2);
 register_bit!(pll_status, arm_pll_stable, 3);
 register_bit!(pll_status, ddr_pll_stable, 4);
 register_bit!(pll_status, io_pll_stable, 5);
 impl core::fmt::Display for pll_status::Read {
    fn fmt(&self, fmt: &mut core::fmt::Formatter) -> Result<(), core::fmt::Error> {
        write!(fmt, "ARM: {}/{} DDR: {}/{} IO: {}/{}",
               if self.arm_pll_lock() { "locked" } else { "NOT locked" },
               if self.arm_pll_stable() { "stable" } else { "UNSTABLE" },
               if self.ddr_pll_lock() { "locked" } else { "NOT locked" },
               if self.ddr_pll_stable() { "stable" } else { "UNSTABLE" },
               if self.io_pll_lock() { "locked" } else { "NOT locked" },
               if self.io_pll_stable() { "stable" } else { "UNSTABLE" },
        )
    }
 }
 register!(pll_cfg, PllCfg, RW, u32);
 register_bits!(pll_cfg, pll_res, u8, 4, 7);
 register_bits!(pll_cfg, pll_cp, u8, 8, 11);
 register_bits!(pll_cfg, lock_cnt, u16, 12, 21);
 register!(arm_clk_ctrl, ArmClkCtrl, RW, u32);
 register_bit!(arm_clk_ctrl,
              /// Clock active
@ -261,8 +363,21 @@ register_bit!(arm_clk_ctrl, cpu_1xclkact, 27);
 register_bit!(arm_clk_ctrl, cpu_2xclkact, 26);
 register_bit!(arm_clk_ctrl, cpu_3or2xclkact, 25);
 register_bit!(arm_clk_ctrl, cpu_6or4xclkact, 24);
-register_bits!(arm_clk_ctrl, divisor, u8, 8, 13);
+register_bits!(arm_clk_ctrl,
-register_bits_typed!(arm_clk_ctrl, srcsel, u8, ArmPllSource, 8, 13);
+               /// should be divisible by 2 (see TRM: 25.2 CPU Clock)
               divisor, u8, 8, 13);
 register_bits_typed!(arm_clk_ctrl, srcsel, u8, ArmPllSource, 4, 5);
 register!(ddr_clk_ctrl, DdrClkCtrl, RW, u32);
 register_bit!(ddr_clk_ctrl, ddr_3xclkact, 0);
 register_bit!(ddr_clk_ctrl, ddr_2xclkact, 1);
 register_bits!(ddr_clk_ctrl, ddr_3xclk_divisor, u8, 20, 25);
 register_bits!(ddr_clk_ctrl, ddr_2xclk_divisor, u8, 26, 31);
 register!(dci_clk_ctrl, DciClkCtrl, RW, u32);
 register_bit!(dci_clk_ctrl, clkact, 0);
 register_bits!(dci_clk_ctrl, divisor0, u8, 8, 13);
 register_bits!(dci_clk_ctrl, divisor1, u8, 20, 25);
 register!(clk_621_true, Clk621True, RW, u32);
 register_bit!(clk_621_true, clk_621_true, 0);
@ -270,6 +385,8 @@ register_bit!(clk_621_true, clk_621_true, 0);
 register!(aper_clk_ctrl, AperClkCtrl, RW, u32);
 register_bit!(aper_clk_ctrl, uart1_cpu_1xclkact, 21);
 register_bit!(aper_clk_ctrl, uart0_cpu_1xclkact, 20);
 register_bit!(aper_clk_ctrl, sdio1_cpu_1xclkact, 11);
 register_bit!(aper_clk_ctrl, sdio0_cpu_1xclkact, 10);
 impl AperClkCtrl {
    pub fn enable_uart0(&mut self) {
        self.modify(|_, w| w.uart0_cpu_1xclkact(true));
@ -278,6 +395,14 @@ impl AperClkCtrl {
    pub fn enable_uart1(&mut self) {
        self.modify(|_, w| w.uart1_cpu_1xclkact(true));
    }
    pub fn enable_sdio0(&mut self) {
        self.modify(|_, w| w.sdio0_cpu_1xclkact(true));
    }
    pub fn enable_sdio1(&mut self) {
        self.modify(|_, w| w.sdio1_cpu_1xclkact(true));
    }
 }
 register!(rclk_ctrl, RclkCtrl, RW, u32);
@ -297,11 +422,29 @@ register_bits!(gem_clk_ctrl,
               divisor, u8, 8, 13);
 register_bits_typed!(gem_clk_ctrl,
                     /// Source to generate the ref clock
-                     srcsel, u8, PllSource, 4, 5);
+                     srcsel, u8, PllSource, 4, 6);
 register_bit!(gem_clk_ctrl,
              /// SMC reference clock control
              clkact, 0);
 register!(sdio_clk_ctrl, SdioClkCtrl, RW, u32);
 register_bit!(sdio_clk_ctrl, clkact0, 0);
 register_bit!(sdio_clk_ctrl, clkact1, 1);
 register_bits!(sdio_clk_ctrl, divisor, u8, 8, 13);
 register_bits_typed!(sdio_clk_ctrl, srcsel, u8, PllSource, 4, 5);
 impl SdioClkCtrl {
    pub fn enable_sdio0(&mut self) {
        self.modify(|_, w| {
            w.divisor(0x14).srcsel(PllSource::IoPll).clkact0(true)
        })
    }
    pub fn enable_sdio1(&mut self) {
        self.modify(|_, w| {
            w.divisor(0x14).srcsel(PllSource::IoPll).clkact1(true)
        })
    }
 }
 register!(uart_clk_ctrl, UartClkCtrl, RW, u32);
 register_bit!(uart_clk_ctrl, clkact0, 0);
 register_bit!(uart_clk_ctrl, clkact1, 1);
@ -332,6 +475,34 @@ impl UartClkCtrl {
    }
 }
 register!(sdio_rst_ctrl, SdioRstCtrl, RW, u32);
 register_bit!(sdio_rst_ctrl, sdio1_ref_rst, 5);
 register_bit!(sdio_rst_ctrl, sdio0_ref_rst, 4);
 register_bit!(sdio_rst_ctrl, sdio1_cpu1x_rst, 1);
 register_bit!(sdio_rst_ctrl, sdio0_cpu1x_rst, 0);
 impl SdioRstCtrl {
    pub fn reset_sdio0(&mut self) {
        self.modify(|_, w|
            w.sdio0_ref_rst(true)
             .sdio0_cpu1x_rst(true)
        );
        self.modify(|_, w|
            w.sdio0_ref_rst(false)
             .sdio0_cpu1x_rst(false)
        );
    }
    pub fn reset_sdio1(&mut self) {
        self.modify(|_, w|
            w.sdio1_ref_rst(true)
             .sdio1_cpu1x_rst(true)
        );
        self.modify(|_, w|
            w.sdio1_ref_rst(false)
             .sdio1_cpu1x_rst(false)
        );
    }
 }
 register!(uart_rst_ctrl, UartRstCtrl, RW, u32);
 register_bit!(uart_rst_ctrl, uart0_ref_rst, 3);
 register_bit!(uart_rst_ctrl, uart1_ref_rst, 2);
@ -362,8 +533,54 @@ impl UartRstCtrl {
    }
 }
-register!(pss_rst_ctrl, PssRstCtrl, RW, u32);
+register!(gpio_rst_ctrl, GpioRstCtrl, RW, u32);
-register_bit!(pss_rst_ctrl, soft_rst, 1);
+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);
 register_bits!(lqspi_clk_ctrl, divisor, u8, 8, 13);
 register!(lqspi_rst_ctrl, LqspiRstCtrl, RW, u32);
 register_bit!(lqspi_rst_ctrl, ref_rst, 1);
 register_bit!(lqspi_rst_ctrl, cpu1x_rst, 0);
 register!(fpga0_clk_ctrl, Fpga0ClkCtrl, RW, u32);
 register_bits!(fpga0_clk_ctrl, divisor1, u8, 20, 25);
 register_bits!(fpga0_clk_ctrl, divisor0, u8, 8, 13);
 register_bits_typed!(fpga0_clk_ctrl, src_sel, u8, PllSource, 4, 5);
 register!(fpga1_clk_ctrl, Fpga1ClkCtrl, RW, u32);
 register_bits!(fpga1_clk_ctrl, divisor1, u8, 20, 25);
 register_bits!(fpga1_clk_ctrl, divisor0, u8, 8, 13);
 register_bits_typed!(fpga1_clk_ctrl, src_sel, u8, PllSource, 4, 5);
 register!(fpga2_clk_ctrl, Fpga2ClkCtrl, RW, u32);
 register_bits!(fpga2_clk_ctrl, divisor1, u8, 20, 25);
 register_bits!(fpga2_clk_ctrl, divisor0, u8, 8, 13);
 register_bits_typed!(fpga2_clk_ctrl, src_sel, u8, PllSource, 4, 5);
 register!(fpga3_clk_ctrl, Fpga3ClkCtrl, RW, u32);
 register_bits!(fpga3_clk_ctrl, divisor1, u8, 20, 25);
 register_bits!(fpga3_clk_ctrl, divisor0, u8, 8, 13);
 register_bits_typed!(fpga3_clk_ctrl, src_sel, u8, PllSource, 4, 5);
 register!(fpga_rst_ctrl, FpgaRstCtrl, RW, u32);
 register_bit!(fpga_rst_ctrl, fpga0_out_rst, 0);
 register_bit!(fpga_rst_ctrl, fpga1_out_rst, 1);
 register_bit!(fpga_rst_ctrl, fpga2_out_rst, 2);
 register_bit!(fpga_rst_ctrl, fpga3_out_rst, 3);
 register!(a9_cpu_rst_ctrl, A9CpuRstCtrl, RW, u32);
 register_bit!(a9_cpu_rst_ctrl, peri_rst, 8);
@ -372,6 +589,37 @@ register_bit!(a9_cpu_rst_ctrl, a9_clkstop0, 4);
 register_bit!(a9_cpu_rst_ctrl, a9_rst1, 1);
 register_bit!(a9_cpu_rst_ctrl, a9_rst0, 0);
 pub fn reboot() {
    RegisterBlock::unlocked(|slcr| {
        unsafe {
            let reboot = slcr.reboot_status.read();
            slcr.reboot_status.write(reboot & 0xF0FFFFFF);
            slcr.pss_rst_ctrl.modify(|_, w| w.soft_rst(true));
        }
    });
 }
 #[derive(Clone, Copy, Debug, PartialEq)]
 #[repr(u8)]
 pub enum BootModePins {
    // CAUTION!
    // The BOOT_MODE bits table 6-4 in UG585 are *out of order*.
    Jtag = 0b000,
    Nor = 0b010,
    Nand = 0b100,
    QuadSpi = 0b001,
    SdCard = 0b101,
 }
 register!(boot_mode, BootMode, RO, u32);
 register_bit!(boot_mode, pll_bypass, 4);
 register_bit!(boot_mode, jtag_routing, 3);
 register_bits_typed!(boot_mode, boot_mode_pins, u8, BootModePins, 0, 2);
 register!(pss_rst_ctrl, PssRstCtrl, RW, u32);
 register_bit!(pss_rst_ctrl, soft_rst, 0);
 /// Used for MioPin*.io_type
 #[repr(u8)]
 pub enum IoBufferType {
@ -451,5 +699,39 @@ mio_pin_register!(mio_pin_51, MioPin51);
 mio_pin_register!(mio_pin_52, MioPin52);
 mio_pin_register!(mio_pin_53, MioPin53);
 register!(lvl_shftr, LvlShftr, RW, u32);
 register_bits_typed!(lvl_shftr, user_lvl_shftr_en, u8, LevelShifterEnable, 0, 3);
 register!(gpiob_ctrl, GpiobCtrl, RW, u32);
 register_bit!(gpiob_ctrl, vref_en, 0);
 register!(ddriob_config, DdriobConfig, RW, u32);
 register_bits_typed!(ddriob_config, inp_type, u8, DdriobInputType, 1, 2);
 register_bit!(ddriob_config, dci_update_b, 3);
 register_bit!(ddriob_config, term_en, 4);
 register_bits_typed!(ddriob_config, dci_type, u8, DdriobDciType, 5, 6);
 register_bit!(ddriob_config, ibuf_disable_mode, 7);
 register_bit!(ddriob_config, term_disable_mode, 8);
 register_bits_typed!(ddriob_config, output_en, u8, DdriobOutputEn, 9, 10);
 register_bit!(ddriob_config, pullup_en, 11);
 register!(ddriob_ddr_ctrl, DdriobDdrCtrl, RW, u32);
 register_bit!(ddriob_ddr_ctrl, vref_int_en, 0);
 register_bits_typed!(ddriob_ddr_ctrl, vref_sel, u8, DdriobVrefSel, 1, 4);
 register_bit!(ddriob_ddr_ctrl, vref_ext_en_lower, 5);
 register_bit!(ddriob_ddr_ctrl, vref_ext_en_upper, 6);
 register_bit!(ddriob_ddr_ctrl, refio_en, 9);
 register!(ddriob_dci_ctrl, DdriobDciCtrl, RW, u32);
 register_bit!(ddriob_dci_ctrl, reset, 0);
 register_bit!(ddriob_dci_ctrl, enable, 1);
 register_bits!(ddriob_dci_ctrl, nref_opt1, u8, 6, 7);
 register_bits!(ddriob_dci_ctrl, nref_opt2, u8, 8, 10);
 register_bits!(ddriob_dci_ctrl, nref_opt4, u8, 11, 13);
 register_bits!(ddriob_dci_ctrl, pref_opt1, u8, 14, 15);
 register_bits!(ddriob_dci_ctrl, pref_opt2, u8, 17, 19);
 register_bit!(ddriob_dci_ctrl, update_control, 20);
 register!(ddriob_dci_status, DdriobDciStatus, RW, u32);
 register_bit!(ddriob_dci_status, done, 0);
 register_bit!(ddriob_dci_status, lock, 13);
--- a/libboard_zynq/src/stdio.rs
+++ b/libboard_zynq/src/stdio.rs
@ -0,0 +1,84 @@
 use core::ops::{Deref, DerefMut};
 use libcortex_a9::{asm, mutex::{Mutex, MutexGuard}};
 use crate::uart::Uart;
 const UART_RATE: u32 = 115_200;
 static mut UART: Mutex<LazyUart> = Mutex::new(LazyUart::Uninitialized);
 #[doc(hidden)]
 pub fn get_uart<'a>() -> MutexGuard<'a, LazyUart> {
    unsafe { UART.lock() }
 }
 /// Deinitialize so that the Uart will be reinitialized on next
 /// output.
 ///
 /// Delays so that an outstanding transmission can finish.
 pub fn drop_uart() {
    for _ in 0..1_000_000 {
        asm::nop();
    }
    unsafe { UART = Mutex::new(LazyUart::Uninitialized); }
 }
 /// Initializes the UART on first use through `.deref_mut()` for debug
 /// output through the `print!` and `println!` macros.
 pub enum LazyUart {
    Uninitialized,
    Initialized(Uart),
 }
 impl Deref for LazyUart {
    type Target = Uart;
    fn deref(&self) -> &Uart {
        match self {
            LazyUart::Uninitialized =>
                panic!("stdio not initialized!"),
            LazyUart::Initialized(uart) =>
                uart,
        }
    }
 }
 impl DerefMut for LazyUart {
    fn deref_mut(&mut self) -> &mut Uart {
        match self {
            LazyUart::Uninitialized => {
                #[cfg(any(feature = "target_coraz7", feature = "target_redpitaya"))]
                let uart = Uart::uart0(UART_RATE);
                #[cfg(any(
                    feature = "target_zc706",
                    feature = "target_ebaz4205",
                    feature = "target_kasli_soc",
                ))]
                let uart = Uart::uart1(UART_RATE);
                *self = LazyUart::Initialized(uart);
                self
            }
            LazyUart::Initialized(uart) =>
                uart,
        }
    }
 }
 #[macro_export]
 macro_rules! print {
    ($($arg:tt)*) => ({
        use core::fmt::Write;
        let mut uart = $crate::stdio::get_uart();
        let _ = write!(uart, $($arg)*);
    })
 }
 #[macro_export]
 macro_rules! println {
    ($($arg:tt)*) => ({
        use core::fmt::Write;
        let mut uart = $crate::stdio::get_uart();
        let _ = write!(uart, $($arg)*);
        let _ = write!(uart, "\n");
        // flush after the newline
        while !uart.tx_idle() {}
    })
 }
--- a/libboard_zynq/src/time.rs
+++ b/libboard_zynq/src/time.rs
@ -0,0 +1,25 @@
 #[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
 pub struct Milliseconds(pub u64);
 impl core::ops::Add for Milliseconds {
    type Output = Self;
    fn add(self, rhs: Self) -> Self::Output {
        Milliseconds(self.0 + rhs.0)
    }
 }
 #[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
 pub struct Microseconds(pub u64);
 impl core::ops::Add for Microseconds {
    type Output = Self;
    fn add(self, rhs: Self) -> Self::Output {
        Microseconds(self.0 + rhs.0)
    }
 }
 pub trait TimeSource<U> {
    fn now(&self) -> U;
 }
--- a/libboard_zynq/src/timer/global.rs
+++ b/libboard_zynq/src/timer/global.rs
@ -0,0 +1,170 @@
 use core::ops::Add;
 use void::Void;
 use libregister::{RegisterR, RegisterW};
 use crate::{
    clocks::Clocks,
    mpcore,
    time::{Milliseconds, Microseconds, TimeSource},
 };
 /// "uptime"
 #[derive(Clone, Copy)]
 pub struct GlobalTimer {
    regs: &'static mpcore::RegisterBlock,
 }
 impl GlobalTimer {
    /// Get the potentially uninitialized timer
    pub unsafe fn get() -> GlobalTimer {
        let regs = mpcore::RegisterBlock::mpcore();
        GlobalTimer { regs }
    }
    /// Get the timer with a reset
    pub fn start() -> GlobalTimer {
        let mut regs = mpcore::RegisterBlock::mpcore();
        Self::reset(&mut regs);
        GlobalTimer { regs }
    }
    fn reset(regs: &mut mpcore::RegisterBlock) {
        // Disable
        regs.global_timer_control.write(
            mpcore::GlobalTimerControl::zeroed()
        );
        // Reset counters
        regs.global_timer_counter0.write(
            mpcore::ValueRegister::zeroed()
        );
        regs.global_timer_counter1.write(
            mpcore::ValueRegister::zeroed()
        );
        // find a prescaler value that matches CPU speed / 2 to us
        let clocks = Clocks::get();
        let mut prescaler = clocks.cpu_3x2x() / 1_000_000;
        while prescaler > 256 {
            prescaler /= 2;
        }
        // Start
        regs.global_timer_control.write(
            mpcore::GlobalTimerControl::zeroed()
                .prescaler((prescaler - 1) as u8)
                .auto_increment_mode(true)
                .timer_enable(true)
        );
    }
    /// read the raw counter value
    pub fn get_counter(&self) -> u64 {
        loop {
            let c1_pre = self.regs.global_timer_counter1.read().value();
            let c0 = self.regs.global_timer_counter0.read().value();
            let c1_post = self.regs.global_timer_counter1.read().value();
            if c1_pre == c1_post {
                return ((c1_pre as u64) << 32) | (c0 as u64);
            }
            // retry if c0 has wrapped while reading.
        }
    }
    /// read and convert to time
    pub fn get_time(&self) -> Milliseconds {
        let prescaler = self.regs.global_timer_control.read().prescaler() as u64;
        let clocks = Clocks::get();
        Milliseconds(self.get_counter() * (prescaler + 1) / (clocks.cpu_3x2x() as u64 / 1000))
    }
    /// read with high precision
    pub fn get_us(&self) -> Microseconds {
        let prescaler = self.regs.global_timer_control.read().prescaler() as u64;
        let clocks = Clocks::get();
        Microseconds(1_000_000 * self.get_counter() * (prescaler + 1) / clocks.cpu_3x2x() as u64)
    }
    /// return a handle that has implements
    /// `embedded_hal::timer::CountDown`
    pub fn countdown<U>(&self) -> CountDown<U>
    where
        Self: TimeSource<U>,
    {
        CountDown {
            timer: self.clone(),
            timeout: self.now(),
        }
    }
 }
 impl TimeSource<Milliseconds> for GlobalTimer {
    fn now(&self) -> Milliseconds {
        self.get_time()
    }
 }
 impl TimeSource<Microseconds> for GlobalTimer {
    fn now(&self) -> Microseconds {
        self.get_us()
    }
 }
 #[derive(Clone)]
 pub struct CountDown<U> {
    timer: GlobalTimer,
    timeout: U,
 }
 /// embedded-hal async API
 impl<U: Add<Output=U> + PartialOrd> embedded_hal::timer::CountDown for CountDown<U>
 where
    GlobalTimer: TimeSource<U>,
 {
    type Time = U;
    fn start<T: Into<Self::Time>>(&mut self, count: T) {
        self.timeout = self.timer.now() + count.into();
    }
    fn wait(&mut self) -> nb::Result<(), Void> {
        if self.timer.now() <= self.timeout {
            Err(nb::Error::WouldBlock)
        } else {
            Ok(())
        }
    }
 }
 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) {
        use embedded_hal::timer::CountDown;
        let mut countdown = self.countdown::<Milliseconds>();
        countdown.start(Milliseconds(ms));
        nb::block!(countdown.wait()).unwrap();
    }
 }
 /// embedded-hal sync API
 impl embedded_hal::blocking::delay::DelayUs<u64> for GlobalTimer {
    fn delay_us(&mut self, us: u64) {
        use embedded_hal::timer::CountDown;
        let mut countdown = self.countdown::<Microseconds>();
        countdown.start(Microseconds(us));
        nb::block!(countdown.wait()).unwrap();
    }
 }
--- a/libboard_zynq/src/timer/mod.rs
+++ b/libboard_zynq/src/timer/mod.rs
@ -0,0 +1,2 @@
 pub mod global;
 pub use global::GlobalTimer;
--- a/libboard_zynq/src/uart/baud_rate_gen.rs
+++ b/libboard_zynq/src/uart/baud_rate_gen.rs
@ -1,4 +1,4 @@
-use crate::regs::*;
+use libregister::*;
 use super::regs::{RegisterBlock, BaudRateGen, BaudRateDiv};
 const BDIV_MIN: u32 = 4;
--- a/libboard_zynq/src/uart/mod.rs
+++ b/libboard_zynq/src/uart/mod.rs
@ -1,8 +1,9 @@
 use core::fmt;
 use void::Void;
-use crate::regs::*;
+use libregister::*;
-use crate::slcr;
+use super::slcr;
-use crate::clocks::CpuClocks;
+use super::clocks::Clocks;
 mod regs;
 mod baud_rate_gen;
@ -12,31 +13,8 @@ pub struct Uart {
 }
 impl Uart {
-    #[cfg(feature = "target_zc706")]
+    #[cfg(any(feature = "target_coraz7", feature = "target_redpitaya"))]
-    pub fn serial(baudrate: u32) -> Self {
+    pub fn uart0(baudrate: u32) -> Self {
        slcr::RegisterBlock::unlocked(|slcr| {
            // Route UART 1 RxD/TxD Signals to MIO Pins
            // TX pin
            slcr.mio_pin_48.write(
                slcr::MioPin48::zeroed()
                    .l3_sel(0b111)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .pullup(true)
            );
            // RX pin
            slcr.mio_pin_49.write(
                slcr::MioPin49::zeroed()
                    .tri_enable(true)
                    .l3_sel(0b111)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .pullup(true)
            );
        });
        Self::uart1(baudrate)
    }
    #[cfg(feature = "target_cora_z7_10")]
    pub fn serial(baudrate: u32) -> Self {
        slcr::RegisterBlock::unlocked(|slcr| {
            // Route UART 0 RxD/TxD Signals to MIO Pins
            // TX pin
@ -55,10 +33,7 @@ impl Uart {
                    .pullup(true)
            );
        });
        Self::uart0(baudrate)
    }
    pub fn uart0(baudrate: u32) -> Self {
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.uart_rst_ctrl.reset_uart0();
            slcr.aper_clk_ctrl.enable_uart0();
@ -71,7 +46,60 @@ impl Uart {
        self_
    }
    #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc"))]
    pub fn uart1(baudrate: u32) -> Self {
        slcr::RegisterBlock::unlocked(|slcr| {
            // Route UART 1 RxD/TxD Signals to MIO Pins
            // TX pin
            slcr.mio_pin_48.write(
                slcr::MioPin48::zeroed()
                    .l3_sel(0b111)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .pullup(true)
            );
            // RX pin
            slcr.mio_pin_49.write(
                slcr::MioPin49::zeroed()
                    .tri_enable(true)
                    .l3_sel(0b111)
                    .io_type(slcr::IoBufferType::Lvcmos18)
                    .pullup(true)
            );
        });
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.uart_rst_ctrl.reset_uart1();
            slcr.aper_clk_ctrl.enable_uart1();
            slcr.uart_clk_ctrl.enable_uart1();
        });
        let mut self_ = Uart {
            regs: regs::RegisterBlock::uart1(),
        };
        self_.configure(baudrate);
        self_
    }
    #[cfg(feature = "target_ebaz4205")]
    pub fn uart1(baudrate: u32) -> Self {
        slcr::RegisterBlock::unlocked(|slcr| {
            // Route UART 1 RxD/TxD Signals to MIO Pins
            // TX pin
            slcr.mio_pin_24.write(
                slcr::MioPin24::zeroed()
                    .l3_sel(0b111)
                    .io_type(slcr::IoBufferType::Lvcmos33)
                    .pullup(true)
            );
            // RX pin
            slcr.mio_pin_25.write(
                slcr::MioPin25::zeroed()
                    .tri_enable(true)
                    .l3_sel(0b111)
                    .io_type(slcr::IoBufferType::Lvcmos33)
                    .pullup(true)
            );
        });
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.uart_rst_ctrl.reset_uart1();
            slcr.aper_clk_ctrl.enable_uart1();
@ -110,7 +138,7 @@ impl Uart {
        self.disable_rx();
        self.disable_tx();
-        let clocks = CpuClocks::get();
+        let clocks = Clocks::get();
        baud_rate_gen::configure(self.regs, clocks.uart_ref_clk(), baudrate);
        // Enable controller
@ -191,18 +219,42 @@ impl Uart {
        self.regs.channel_sts.read().txfull()
    }
-    pub fn tx_fifo_empty(&self) -> bool {
+    pub fn tx_idle(&self) -> bool {
-        self.regs.channel_sts.read().txempty()
+        let status = self.regs.channel_sts.read();
        status.txempty() && !status.tactive()
    }
 }
 impl fmt::Write for Uart {
    fn write_str(&mut self, s: &str) -> Result<(), fmt::Error> {
        while !self.tx_fifo_empty() {}
        for b in s.bytes() {
            self.write_byte(b);
        }
        Ok(())
    }
 }
 /// embedded_hal async API
 impl embedded_hal::serial::Write<u8> for Uart {
    type Error = Void;
    fn write(&mut self, b: u8) -> nb::Result<(), Void> {
        if self.tx_fifo_full() {
            Err(nb::Error::WouldBlock)
        } else {
            self.write_byte(b);
            Ok(())
        }
    }
    fn flush(&mut self) -> nb::Result<(), Void> {
        if self.tx_idle() {
            Ok(())
        } else {
            Err(nb::Error::WouldBlock)
        }
    }
 }
 /// embedded_hal sync API
 impl embedded_hal::blocking::serial::write::Default<u8> for Uart {}
--- a/libboard_zynq/src/uart/regs.rs
+++ b/libboard_zynq/src/uart/regs.rs
@ -1,7 +1,11 @@
 use volatile_register::{RO, WO, RW};
-use crate::{register, register_bit, register_bits, register_bits_typed, register_at};
+use libregister::{
    register, register_at,
    register_bit, register_bits, register_bits_typed,
 };
 #[allow(unused)]
 #[repr(u8)]
 pub enum ChannelMode {
    Normal         = 0b00,
@ -10,6 +14,7 @@ pub enum ChannelMode {
    RemoteLoopback = 0b11,
 }
 #[allow(unused)]
 #[repr(u8)]
 pub enum ParityMode {
    EvenParity = 0b000,
@ -19,6 +24,7 @@ pub enum ParityMode {
    None       = 0b100,
 }
 #[allow(unused)]
 #[repr(u8)]
 pub enum StopBits {
    One        = 0b00,
--- a/libconfig/Cargo.toml
+++ b/libconfig/Cargo.toml
@ -0,0 +1,29 @@
 [package]
 name = "libconfig"
 version = "0.1.0"
 authors = ["M-Labs"]
 edition = "2018"
 [dependencies]
 libboard_zynq = { path = "../libboard_zynq" }
 log = "0.4"
 [dependencies.core_io]
 git = "https://git.m-labs.hk/M-Labs/rs-core_io.git"
 rev = "e9d3edf027"
 features = ["collections"]
 [dependencies.fatfs]
 git = "https://git.m-labs.hk/M-Labs/rust-fatfs.git"
 rev = "4b5e420084"
 default-features = false
 features = ["core_io"]
 [features]
 target_zc706 = []
 target_coraz7 = []
 target_ebaz4205 = []
 target_redpitaya = []
 target_kasli_soc = []
 ipv6 = []
 fat_lfn = [ "fatfs/alloc" ]
--- a/libconfig/src/bootgen.rs
+++ b/libconfig/src/bootgen.rs
@ -0,0 +1,181 @@
 use alloc::vec::Vec;
 use core_io::{Error, Read, Seek, SeekFrom};
 use libboard_zynq::devc;
 use log::debug;
 #[derive(Debug)]
 pub enum BootgenLoadingError {
    InvalidBootImageHeader,
    MissingPartition,
    EncryptedBitstream,
    IoError(Error),
    DevcError(devc::DevcError),
 }
 impl From<Error> for BootgenLoadingError {
    fn from(error: Error) -> Self {
        BootgenLoadingError::IoError(error)
    }
 }
 impl From<devc::DevcError> for BootgenLoadingError {
    fn from(error: devc::DevcError) -> Self {
        BootgenLoadingError::DevcError(error)
    }
 }
 impl core::fmt::Display for BootgenLoadingError {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        use BootgenLoadingError::*;
        match self {
            InvalidBootImageHeader => write!(
                f,
                "Invalid boot image header. Check if the file is correct."
            ),
            MissingPartition => write!(f, "Partition not found. Check your compile configuration."),
            EncryptedBitstream => write!(f, "Encrypted bitstream is not supported."),
            IoError(e) => write!(f, "Error while reading: {}", e),
            DevcError(e) => write!(f, "PCAP interface error: {}", e),
        }
    }
 }
 #[repr(C)]
 struct PartitionHeader {
    pub encrypted_length: u32,
    pub unencrypted_length: u32,
    pub word_length: u32,
    pub dest_load_addr: u32,
    pub dest_exec_addr: u32,
    pub data_offset: u32,
    pub attribute_bits: u32,
    pub section_count: u32,
    pub checksum_offset: u32,
    pub header_offset: u32,
    pub cert_offset: u32,
    pub reserved: [u32; 4],
    pub checksum: u32,
 }
 /// Read a u32 word from the reader.
 fn read_u32<Reader: Read>(reader: &mut Reader) -> Result<u32, BootgenLoadingError> {
    let mut buffer: [u8; 4] = [0; 4];
    reader.read_exact(&mut buffer)?;
    let mut result: u32 = 0;
    for i in 0..4 {
        result |= (buffer[i] as u32) << (i * 8);
    }
    Ok(result)
 }
 /// Load PL partition header.
 fn load_pl_header<File: Read + Seek>(
    file: &mut File,
 ) -> Result<Option<PartitionHeader>, BootgenLoadingError> {
    let mut buffer: [u8; 0x40] = [0; 0x40];
    file.read_exact(&mut buffer)?;
    let header = unsafe { core::mem::transmute::<_, PartitionHeader>(buffer) };
    if header.attribute_bits & (2 << 4) != 0 {
        Ok(Some(header))
    } else {
        Ok(None)
    }
 }
 fn load_ps_header<File: Read + Seek>(
    file: &mut File,
 ) -> Result<Option<PartitionHeader>, BootgenLoadingError> {
    let mut buffer: [u8; 0x40] = [0; 0x40];
    file.read_exact(&mut buffer)?;
    let header = unsafe { core::mem::transmute::<_, PartitionHeader>(buffer) };
    if header.attribute_bits & (1 << 4) != 0 {
        Ok(Some(header))
    } else {
        Ok(None)
    }
 }
 /// Locate the partition from the image, and return the size (in bytes) of the partition if successful.
 /// This function would seek the file to the location of the partition.
 fn locate<
    File: Read + Seek,
    F: Fn(&mut File) -> Result<Option<PartitionHeader>, BootgenLoadingError>,
 >(
    file: &mut File,
    f: F,
 ) -> Result<usize, BootgenLoadingError> {
    file.seek(SeekFrom::Start(0))?;
    const BOOT_HEADER_SIGN: u32 = 0x584C4E58;
    // read boot header signature
    file.seek(SeekFrom::Start(0x24))?;
    if read_u32(file)? != BOOT_HEADER_SIGN {
        return Err(BootgenLoadingError::InvalidBootImageHeader);
    }
    // find fsbl offset
    file.seek(SeekFrom::Start(0x30))?;
    // the length is in bytes, we have to convert it to words to compare with the partition offset
    // later
    let fsbl = read_u32(file)? / 4;
    // read partition header offset
    file.seek(SeekFrom::Start(0x9C))?;
    let ptr = read_u32(file)?;
    debug!("Partition header pointer = {:0X}", ptr);
    file.seek(SeekFrom::Start(ptr as u64))?;
    // at most 3 partition headers
    for _ in 0..3 {
        if let Some(header) = f(file)? {
            let encrypted_length = header.encrypted_length;
            let unencrypted_length = header.unencrypted_length;
            debug!("Unencrypted length = {:0X}", unencrypted_length);
            if encrypted_length != unencrypted_length {
                return Err(BootgenLoadingError::EncryptedBitstream);
            }
            let start_addr = header.data_offset;
            // skip fsbl
            if start_addr == fsbl {
                continue;
            }
            debug!("Partition start address: {:0X}", start_addr);
            file.seek(SeekFrom::Start(start_addr as u64 * 4))?;
            return Ok(unencrypted_length as usize * 4);
        }
    }
    Err(BootgenLoadingError::MissingPartition)
 }
 /// Load bitstream from bootgen file.
 /// This function parses the file, locate the bitstream and load it through the PCAP driver.
 /// It requires a large buffer, please enable the DDR RAM before using it.
 pub fn load_bitstream<File: Read + Seek>(file: &mut File) -> Result<(), BootgenLoadingError> {
    let size = locate(file, load_pl_header)?;
    unsafe {
        // align to 64 bytes
        let ptr = alloc::alloc::alloc(alloc::alloc::Layout::from_size_align(size, 64).unwrap());
        let buffer = core::slice::from_raw_parts_mut(ptr, size);
        file.read_exact(buffer).map_err(|e| {
            core::ptr::drop_in_place(ptr);
            e
        })?;
        let mut devcfg = devc::DevC::new();
        devcfg.enable();
        devcfg.program(&buffer).map_err(|e| {
            core::ptr::drop_in_place(ptr);
            e
        })?;
        core::ptr::drop_in_place(ptr);
        Ok(())
    }
 }
 pub fn get_runtime<File: Read + Seek>(file: &mut File) -> Result<Vec<u8>, BootgenLoadingError> {
    let size = locate(file, load_ps_header)?;
    let mut buffer = Vec::with_capacity(size);
    unsafe {
        buffer.set_len(size);
    }
    file.read_exact(&mut buffer)?;
    Ok(buffer)
 }
--- a/libconfig/src/lib.rs
+++ b/libconfig/src/lib.rs
@ -0,0 +1,174 @@
 #![no_std]
 extern crate alloc;
 use core::fmt;
 use alloc::{string::FromUtf8Error, string::String, vec::Vec, rc::Rc};
 use core_io::{self as io, BufRead, BufReader, Read, Write, Seek, SeekFrom};
 use libboard_zynq::sdio;
 pub mod sd_reader;
 pub mod net_settings;
 pub mod bootgen;
 #[derive(Debug)]
 pub enum Error<'a> {
    SdError(sdio::sd_card::CardInitializationError),
    IoError(io::Error),
    Utf8Error(FromUtf8Error),
    KeyNotFoundError(&'a str),
    NoConfig,
 }
 pub type Result<'a, T> = core::result::Result<T, Error<'a>>;
 impl<'a> fmt::Display for Error<'a> {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        match self {
            Error::SdError(error) => write!(f, "SD error: {}", error),
            Error::IoError(error) => write!(f, "I/O error: {}", error),
            Error::Utf8Error(error) => write!(f, "UTF-8 error: {}", error),
            Error::KeyNotFoundError(name) => write!(f, "Configuration key `{}` not found", name),
            Error::NoConfig => write!(f, "Configuration not present"),
        }
    }
 }
 impl<'a> From<sdio::sd_card::CardInitializationError> for Error<'a> {
    fn from(error: sdio::sd_card::CardInitializationError) -> Self {
        Error::SdError(error)
    }
 }
 impl<'a> From<io::Error> for Error<'a> {
    fn from(error: io::Error) -> Self {
        Error::IoError(error)
    }
 }
 impl<'a> From<FromUtf8Error> for Error<'a> {
    fn from(error: FromUtf8Error) -> Self {
        Error::Utf8Error(error)
    }
 }
 fn parse_config<'a>(
    key: &'a str,
    buffer: &mut Vec<u8>,
    file: fatfs::File<sd_reader::SdReader>,
 ) -> Result<'a, ()> {
    let prefix = [key, "="].concat().to_ascii_lowercase();
    for line in BufReader::new(file).lines() {
        let line = line?.to_ascii_lowercase();
        if line.starts_with(&prefix) {
            buffer.extend(line[prefix.len()..].as_bytes());
            return Ok(());
        }
    }
    Err(Error::KeyNotFoundError(key))
 }
 pub struct Config {
    fs: Option<Rc<fatfs::FileSystem<sd_reader::SdReader>>>,
 }
 const NEWLINE: &[u8] = b"\n";
 impl Config {
    pub fn new() -> Result<'static, Self> {
        let sdio = sdio::Sdio::sdio0(true);
        if !sdio.is_card_inserted() {
            Err(sdio::sd_card::CardInitializationError::NoCardInserted)?;
        }
        let sd = sdio::sd_card::SdCard::from_sdio(sdio)?;
        let reader = sd_reader::SdReader::new(sd);
        let fs = reader.mount_fatfs(sd_reader::PartitionEntry::Entry1)?;
        Ok(Config { fs: Some(Rc::new(fs)) })
    }
    pub fn from_fs(fs: Option<Rc<fatfs::FileSystem<sd_reader::SdReader>>>) -> Self {
        Config { fs }
    }
    pub fn new_dummy() -> Self {
        Config { fs: None }
    }
    pub fn read<'b>(&self, key: &'b str) -> Result<'b, Vec<u8>> {
        if let Some(fs) = &self.fs {
            let root_dir = fs.root_dir();
            let mut buffer: Vec<u8> = Vec::new();
            match root_dir.open_file(&["/CONFIG/", key, ".BIN"].concat()) {
                Ok(mut f) => f.read_to_end(&mut buffer).map(|_| ())?,
                Err(_) => match root_dir.open_file("/CONFIG.TXT") {
                    Ok(f) => parse_config(key, &mut buffer, f)?,
                    Err(_) => return Err(Error::KeyNotFoundError(key)),
                },
            };
            Ok(buffer)
        } else {
            Err(Error::NoConfig)
        }
    }
    pub fn read_str<'b>(&self, key: &'b str) -> Result<'b, String> {
        Ok(String::from_utf8(self.read(key)?)?)
    }
    pub fn remove<'b>(&self, key: &'b str) -> Result<'b, ()> {
        if let Some(fs) = &self.fs {
            let root_dir = fs.root_dir();
            match root_dir.remove(&["/CONFIG/", key, ".BIN"].concat()) {
                Ok(()) => Ok(()),
                Err(_) => {
                    let prefix = [key, "="].concat().to_ascii_lowercase();
                    match root_dir.create_file("/CONFIG.TXT") {
                        Ok(mut f) => {
                            let mut buffer = String::new();
                            f.read_to_string(&mut buffer)?;
                            f.seek(SeekFrom::Start(0))?;
                            f.truncate()?;
                            for line in buffer.lines() {
                                if line.len() > 0 && !line.to_ascii_lowercase().starts_with(&prefix) {
                                    f.write(line.as_bytes())?;
                                    f.write(NEWLINE)?;
                                }
                            }
                            Ok(())
                        },
                        Err(_) => Err(Error::KeyNotFoundError(key))
                    }
                }
            }
        } else {
            Err(Error::NoConfig)
        }
    }
    pub fn write<'b>(&self, key: &'b str, value: Vec<u8>) -> Result<'b, ()> {
        if self.fs.is_none() {
            return Err(Error::NoConfig);
        }
        let fs = self.fs.as_ref().unwrap();
        let root_dir = fs.root_dir();
        let is_str = value.len() <= 100 && value.is_ascii() && !value.contains(&b'\n');
        if key == "boot" {
            let mut f = root_dir.create_file("/BOOT.BIN")?;
            f.truncate()?;
            f.write_all(&value)?;
            drop(f);
        } else {
            let _ = self.remove(key);
            if is_str {
                let mut f = root_dir.create_file("/CONFIG.TXT")?;
                f.seek(SeekFrom::End(0))?;
                write!(f, "{}={}\n", key, String::from_utf8(value).unwrap())?;
            } else {
                let dir = root_dir.create_dir("/CONFIG")?;
                let mut f = dir.create_file(&[key, ".BIN"].concat())?;
                f.write_all(&value)?;
            }
        }
        Ok(())
    }
 }
--- a/libconfig/src/net_settings.rs
+++ b/libconfig/src/net_settings.rs
@ -0,0 +1,92 @@
 use core::fmt;
 use libboard_zynq::smoltcp::wire::{EthernetAddress, IpAddress};
 use super::Config;
 pub struct NetAddresses {
    pub hardware_addr: EthernetAddress,
    pub ipv4_addr: IpAddress,
    #[cfg(feature = "ipv6")]
    pub ipv6_ll_addr: IpAddress,
    #[cfg(feature = "ipv6")]
    pub ipv6_addr: Option<IpAddress>
 }
 impl fmt::Display for NetAddresses {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "MAC={} IPv4={} ",
            self.hardware_addr, self.ipv4_addr)?;
        #[cfg(feature = "ipv6")]
        {
            write!(f, "IPv6-LL={}", self.ipv6_ll_addr)?;
            match self.ipv6_addr {
                Some(addr) => write!(f, " {}", addr)?,
                None => write!(f, " IPv6: no configured address")?
            }
        }
        Ok(())
    }
 }
 #[cfg(feature = "target_kasli_soc")]
 fn get_address_from_eeprom() -> EthernetAddress {
    use libboard_zynq::i2c::{I2c, eeprom};
    let mut i2c = I2c::i2c0();
    i2c.init().unwrap();
    let mut eeprom = eeprom::EEPROM::new(&mut i2c, 16);
    let address = eeprom.read_eui48().unwrap_or([0x02, 0x00, 0x00, 0x00, 0x00, 0x56]);
    EthernetAddress(address)
 }
 pub fn get_addresses(cfg: &Config) -> NetAddresses {
    #[cfg(feature = "target_zc706")]
    let mut hardware_addr = EthernetAddress([0x02, 0x00, 0x00, 0x00, 0x00, 0x52]);
    #[cfg(feature = "target_zc706")]
    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 52);
    #[cfg(feature = "target_coraz7")]
    let mut hardware_addr = EthernetAddress([0x02, 0x00, 0x00, 0x00, 0x00, 0x54]);
    #[cfg(feature = "target_coraz7")]
    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 54);
    #[cfg(feature = "target_redpitaya")]
    let mut hardware_addr = EthernetAddress([0x02, 0x00, 0x00, 0x00, 0x00, 0x55]);
    #[cfg(feature = "target_redpitaya")]
    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 55);
    #[cfg(feature = "target_kasli_soc")]
    let mut hardware_addr = get_address_from_eeprom();
    #[cfg(feature = "target_kasli_soc")]
    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 56);
    #[cfg(feature = "target_ebaz4205")]
    let mut hardware_addr = EthernetAddress([0x02, 0x00, 0x00, 0x00, 0x00, 0x57]);
    #[cfg(feature = "target_ebaz4205")]
    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 57);
    if let Ok(Ok(addr)) = cfg.read_str("mac").map(|s| s.parse()) {
        hardware_addr = addr;
    }
    if let Ok(Ok(addr)) = cfg.read_str("ip").map(|s| s.parse()) {
        ipv4_addr = addr;
    }
    #[cfg(feature = "ipv6")]
    let ipv6_addr = cfg.read_str("ip6").ok().and_then(|s| s.parse().ok());
    #[cfg(feature = "ipv6")]
    let ipv6_ll_addr = IpAddress::v6(
        0xfe80, 0x0000, 0x0000, 0x0000,
        (((hardware_addr.0[0] ^ 0x02) as u16) << 8) | (hardware_addr.0[1] as u16),
        ((hardware_addr.0[2] as u16) << 8) | 0x00ff,
        0xfe00 | (hardware_addr.0[3] as u16),
        ((hardware_addr.0[4] as u16) << 8) | (hardware_addr.0[5] as u16));
    NetAddresses {
        hardware_addr,
        ipv4_addr,
        #[cfg(feature = "ipv6")]
        ipv6_ll_addr,
        #[cfg(feature = "ipv6")]
        ipv6_addr
    }
 }
--- a/libconfig/src/sd_reader.rs
+++ b/libconfig/src/sd_reader.rs
@ -0,0 +1,304 @@
 use core_io::{BufRead, Error, ErrorKind, Read, Result as IoResult, Seek, SeekFrom, Write};
 use fatfs;
 use libboard_zynq::sdio::{sd_card::SdCard, CmdTransferError};
 use log::debug;
 use alloc::vec::Vec;
 const MBR_SIGNATURE: [u8; 2] = [0x55, 0xAA];
 const PARTID_FAT12: u8 = 0x01;
 const PARTID_FAT16_LESS32M: u8 = 0x04;
 const PARTID_FAT16: u8 = 0x06;
 const PARTID_FAT32: u8 = 0x0B;
 const PARTID_FAT32_LBA: u8 = 0x0C;
 const PARTID_FAT16_LBA: u8 = 0x0E;
 fn cmd_error_to_io_error(_: CmdTransferError) -> Error {
    Error::new(ErrorKind::Other, "Command transfer error")
 }
 const BLOCK_SIZE: usize = 512;
 /// SdReader struct implementing `Read + BufRead + Write + Seek` traits for `core_io`.
 /// Used as an adaptor for fatfs crate, but could be used directly for raw data access.
 ///
 /// Implementation: all read/writes would be split into unaligned and block-aligned parts,
 /// unaligned read/writes would do a buffered read/write using a block-sized internal buffer,
 /// while aligned transactions would be sent to the SD card directly for performance reason.
 pub struct SdReader {
    /// Internal SdCard handle.
    sd: SdCard,
    /// Read buffer with the size of 1 block.
    buffer: Vec<u8>,
    /// Address for the next byte.
    byte_addr: u32,
    /// Internal index for the next byte.
    /// Normally in range `[0, BLOCK_SIZE - 1]`.
    ///
    /// `index = BLOCK_SIZE` means that the `buffer` is invalid for the current `byte_addr`,
    /// the next `fill_buf` call would fill the buffer.
    index: usize,
    /// Dirty flag indicating the content has to be flushed.
    dirty: bool,
    /// Base offset for translation from logical address to physical address.
    offset: u32,
 }
 #[derive(Copy, Clone)]
 #[allow(unused)]
 // Partition entry enum, normally we would use entry1.
 pub enum PartitionEntry {
    Entry1 = 0x1BE,
    Entry2 = 0x1CE,
    Entry3 = 0x1DE,
    Entry4 = 0x1EE,
 }
 impl SdReader {
    /// Create SdReader from SdCard
    pub fn new(sd: SdCard) -> SdReader {
        let mut vec: Vec<u8> = Vec::with_capacity(BLOCK_SIZE);
        unsafe {
            vec.set_len(vec.capacity());
        }
        SdReader {
            sd,
            buffer: vec,
            byte_addr: 0,
            index: BLOCK_SIZE,
            dirty: false,
            offset: 0,
        }
    }
    /// Internal read function for unaligned read.
    /// The read must not cross block boundary.
    fn read_unaligned(&mut self, buf: &mut [u8]) -> IoResult<usize> {
        if buf.len() == 0 {
            return Ok(0);
        }
        let filled_buffer = self.fill_buf()?;
        for (dest, src) in buf.iter_mut().zip(filled_buffer.iter()) {
            *dest = *src;
        }
        self.consume(buf.len());
        Ok(buf.len())
    }
    /// Internal write function for unaligned write.
    /// The write must not cross block boundary.
    fn write_unaligned(&mut self, buf: &[u8]) -> IoResult<usize> {
        if buf.len() == 0 {
            return Ok(0);
        }
        // update buffer if needed, as we will flush the entire block later.
        self.fill_buf()?;
        self.dirty = true;
        let dest_buffer = &mut self.buffer[self.index..];
        for (src, dest) in buf.iter().zip(dest_buffer.iter_mut()) {
            *dest = *src;
        }
        self.consume(buf.len());
        Ok(buf.len())
    }
    /// Split the slice into three segments, with the middle block-aligned.
    /// Alignment depends on the current `self.byte_addr` instead of the slice pointer address
    fn block_align<'b>(&self, buf: &'b [u8]) -> (&'b [u8], &'b [u8], &'b [u8]) {
        let head_len = BLOCK_SIZE - (self.byte_addr as usize % BLOCK_SIZE);
        if head_len > buf.len() {
            (buf, &[], &[])
        } else {
            let remaining_length = buf.len() - head_len;
            let mid_length = remaining_length - remaining_length % BLOCK_SIZE;
            let (head, remaining) = buf.split_at(head_len);
            let (mid, tail) = remaining.split_at(mid_length);
            (head, mid, tail)
        }
    }
    /// Split the mutable slice into three segments, with the middle block-aligned.
    /// Alignment depends on the current `self.byte_addr` instead of the slice pointer address
    fn block_align_mut<'b>(&self, buf: &'b mut [u8]) -> (&'b mut [u8], &'b mut [u8], &'b mut [u8]) {
        let head_len = BLOCK_SIZE - (self.byte_addr as usize % BLOCK_SIZE);
        if head_len > buf.len() {
            (buf, &mut [], &mut [])
        } else {
            let remaining_length = buf.len() - head_len;
            let mid_length = remaining_length - remaining_length % BLOCK_SIZE;
            let (head, remaining) = buf.split_at_mut(head_len);
            let (mid, tail) = remaining.split_at_mut(mid_length);
            (head, mid, tail)
        }
    }
    /// Invalidate the buffer, so later unaligned read/write would reload the buffer from SD card.
    fn invalidate_buffer(&mut self) {
        self.index = BLOCK_SIZE;
    }
    /// Set the base offset of the SD card, to transform from physical address to logical address.
    fn set_base_offset(&mut self, offset: u32) -> IoResult<u64> {
        self.offset = offset;
        self.seek(SeekFrom::Start(0))
    }
    /// Mount fatfs from partition entry, and return the fatfs object if success.
    /// This takes the ownership of self, so currently there is no way to recover from an error,
    /// except creating a new SD card instance.
    pub fn mount_fatfs(mut self, entry: PartitionEntry) -> IoResult<fatfs::FileSystem<Self>> {
        let mut buffer: [u8; 4] = [0; 4];
        self.seek(SeekFrom::Start(0x1FE))?;
        self.read_exact(&mut buffer[..2])?;
        // check MBR signature
        if buffer[..2] != MBR_SIGNATURE {
            return Err(Error::new(
                ErrorKind::InvalidData,
                "Incorrect signature for MBR sector.",
            ));
        }
        // Read partition ID.
        self.seek(SeekFrom::Start(entry as u64 + 0x4))?;
        self.read_exact(&mut buffer[..1])?;
        debug!("Partition ID: {:0X}", buffer[0]);
        match buffer[0] {
            PARTID_FAT12 | PARTID_FAT16_LESS32M | PARTID_FAT16 |
            PARTID_FAT16_LBA | PARTID_FAT32 | PARTID_FAT32_LBA => {}
            _ => {
                return Err(Error::new(
                    ErrorKind::InvalidData,
                    "No FAT partition found for the specified entry.",
                ));
            }
        }
        // Read LBA
        self.seek(SeekFrom::Current(0x3))?;
        self.read_exact(&mut buffer)?;
        let mut lba: u32 = 0;
        // Little endian
        for i in 0..4 {
            lba |= (buffer[i] as u32) << (i * 8);
        }
        // Set to logical address
        self.set_base_offset(lba * BLOCK_SIZE as u32)?;
        // setup fatfs
        fatfs::FileSystem::new(self, fatfs::FsOptions::new())
    }
 }
 impl Read for SdReader {
    fn read(&mut self, buf: &mut [u8]) -> IoResult<usize> {
        let total_length = buf.len();
        let (a, b, c) = self.block_align_mut(buf);
        self.read_unaligned(a)?;
        if b.len() > 0 {
            // invalidate internal buffer
            self.invalidate_buffer();
            if let Err(_) = self.sd.read_block(
                self.byte_addr / BLOCK_SIZE as u32,
                (b.len() / BLOCK_SIZE) as u16,
                b,
            ) {
                // we have to allow partial read, as per the trait required
                return Ok(a.len());
            }
            self.byte_addr += b.len() as u32;
        }
        if let Err(_) = self.read_unaligned(c) {
            // we have to allow partial read, as per the trait required
            return Ok(a.len() + b.len());
        }
        Ok(total_length)
    }
 }
 impl BufRead for SdReader {
    fn fill_buf(&mut self) -> IoResult<&[u8]> {
        if self.index == BLOCK_SIZE {
            // flush the buffer if it is dirty before overwriting it with new data
            if self.dirty {
                self.flush()?;
            }
            // reload buffer
            self.sd
                .read_block(self.byte_addr / (BLOCK_SIZE as u32), 1, &mut self.buffer)
                .map_err(cmd_error_to_io_error)?;
            self.index = (self.byte_addr as usize) % BLOCK_SIZE;
        }
        Ok(&self.buffer[self.index..])
    }
    fn consume(&mut self, amt: usize) {
        self.index += amt;
        self.byte_addr += amt as u32;
    }
 }
 impl Write for SdReader {
    fn write(&mut self, buf: &[u8]) -> IoResult<usize> {
        let (a, b, c) = self.block_align(buf);
        self.write_unaligned(a)?;
        if b.len() > 0 {
            self.flush()?;
            self.invalidate_buffer();
            if let Err(_) = self.sd.write_block(
                self.byte_addr / BLOCK_SIZE as u32,
                (b.len() / BLOCK_SIZE) as u16,
                b,
            ) {
                return Ok(a.len());
            }
            self.byte_addr += b.len() as u32;
        }
        if let Err(_) = self.write_unaligned(c) {
            return Ok(a.len() + b.len());
        }
        Ok(buf.len())
    }
    fn flush(&mut self) -> IoResult<()> {
        if self.dirty {
            let block_addr = (self.byte_addr - self.index as u32) / (BLOCK_SIZE as u32);
            self.sd
                .write_block(block_addr, 1, &self.buffer)
                .map_err(cmd_error_to_io_error)?;
            self.dirty = false;
        }
        Ok(())
    }
 }
 impl Seek for SdReader {
    fn seek(&mut self, pos: SeekFrom) -> IoResult<u64> {
        let raw_target = match pos {
            SeekFrom::Start(x) => self.offset as i64 + x as i64,
            SeekFrom::Current(x) => self.byte_addr as i64 + x,
            SeekFrom::End(_) => panic!("SD card does not support seek from end"),
        };
        if raw_target < self.offset as i64 || raw_target > core::u32::MAX as i64 {
            return Err(Error::new(ErrorKind::InvalidInput, "Invalid address"));
        }
        let target_byte_addr = raw_target as u32;
        let address_same_block =
            self.byte_addr / (BLOCK_SIZE as u32) == target_byte_addr / (BLOCK_SIZE as u32);
        // if the buffer was invalidated, we consider seek as different block
        let same_block = address_same_block && self.index != BLOCK_SIZE;
        if !same_block {
            self.flush()?;
        }
        self.byte_addr = target_byte_addr;
        self.index = if same_block {
            target_byte_addr as usize % BLOCK_SIZE
        } else {
            // invalidate the buffer as we moved to a different block
            BLOCK_SIZE
        };
        Ok((self.byte_addr - self.offset) as u64)
    }
 }
 impl Drop for SdReader {
    fn drop(&mut self) {
        // just try to flush it, ignore error if any
        self.flush().unwrap_or(());
    }
 }
--- a/libcortex_a9/Cargo.toml
+++ b/libcortex_a9/Cargo.toml
@ -0,0 +1,14 @@
 [package]
 name = "libcortex_a9"
 version = "0.0.0"
 authors = ["M-Labs"]
 edition = "2018"
 [features]
 power_saving = []
 default = []
 [dependencies]
 bit_field = "0.10"
 volatile-register = "0.2"
 libregister = { path = "../libregister" }
--- a/libcortex_a9/src/asm.rs
+++ b/libcortex_a9/src/asm.rs
@ -0,0 +1,83 @@
 use core::arch::asm;
 /// The classic no-op
 #[inline]
 pub fn nop() {
    unsafe { asm!("nop") }
 }
 /// Wait For Event
 #[inline]
 pub fn wfe() {
    unsafe { asm!("wfe") }
 }
 /// Send Event
 #[inline]
 pub fn sev() {
    unsafe { asm!("sev") }
 }
 /// Data Memory Barrier
 #[inline]
 pub fn dmb() {
    unsafe { asm!("dmb") }
 }
 /// Data Synchronization Barrier
 #[inline]
 pub fn dsb() {
    unsafe { asm!("dsb") }
 }
 /// Instruction Synchronization Barrier
 #[inline]
 pub fn isb() {
    unsafe { asm!("isb") }
 }
 /// Enable FIQ
 #[inline]
 pub unsafe fn enable_fiq() {
    asm!("cpsie f");
 }
 /// Enable IRQ
 #[inline]
 pub unsafe fn enable_irq() {
    asm!("cpsie i");
 }
 /// Disable IRQ, return if IRQ was originally enabled.
 #[inline]
 pub unsafe fn enter_critical() -> bool {
    let mut cpsr: u32;
    asm!(
        "mrs {}, cpsr
         cpsid i", lateout(reg) cpsr);
    (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
    };
    asm!(
        "mrs r1, cpsr
         bic r1, r1, {}
         msr cpsr_c, r1"
         , in(reg) mask, out("r1") _);
 }
 /// Exiting IRQ
 #[inline]
 pub unsafe fn exit_irq() {
    asm!("
        mrs r0, SPSR
        msr CPSR, r0
        ", out("r0") _);
 }
--- a/libcortex_a9/src/cache.rs
+++ b/libcortex_a9/src/cache.rs
@ -0,0 +1,279 @@
 use super::asm::{dmb, dsb};
 use super::l2c::*;
 use core::arch::asm;
 /// Invalidate TLBs
 #[inline(always)]
 pub fn tlbiall() {
    unsafe {
        asm!("mcr p15, 0, {}, c8, c7, 0", in(reg) 0);
    }
 }
 /// Invalidate I-Cache
 #[inline(always)]
 pub fn iciallu() {
    unsafe {
        asm!("mcr p15, 0, {}, c7, c5, 0", in(reg) 0);
    }
 }
 /// Invalidate Branch Predictor Array
 #[inline(always)]
 pub fn bpiall() {
    unsafe {
        asm!("mcr p15, 0, {}, c7, c5, 6", in(reg) 0);
    }
 }
 /// Data cache clean by set/way
 #[inline(always)]
 pub fn dccsw(setway: u32) {
    unsafe {
        asm!("mcr p15, 0, {}, c7, c10, 2", in(reg) setway);
    }
 }
 /// Data cache invalidate by set/way
 #[inline(always)]
 pub fn dcisw(setway: u32) {
    unsafe {
        // acc. to ARM Architecture Reference Manual, Figure B3-32;
        // also see example code (for DCCISW, but DCISW will be
        // analogous) "Example code for cache maintenance operations"
        // on pages B2-1286 and B2-1287.
        asm!("mcr p15, 0, {}, c7, c6, 2", in(reg) setway);
    }
 }
 /// Data cache clean by set/way
 #[inline(always)]
 pub fn dccisw(setway: u32) {
    unsafe {
        asm!("mcr p15, 0, {}, c7, c14, 2", in(reg) setway);
    }
 }
 /// A made-up "instruction": invalidate all of the L1 D-Cache
 #[inline(always)]
 pub fn dciall_l1() {
    // the cache associativity could be read from a register, but will
    // always be 4 in L1 data cache of a cortex a9
    let ways = 4;
    let bit_pos_of_way = 30; // 32 - log2(ways)
    // the cache sets could be read from a register, but are always
    // 256 for the cores in the zync-7000; in general, 128 or 512 are
    // also possible.
    let sets = 256;
    let bit_pos_of_set = 5; // for a line size of 8 words = 2^5 bytes
    // select L1 data cache
    unsafe {
        asm!("mcr p15, 2, {}, c0, c0, 0", in(reg) 0);
    }
    // Invalidate entire D-Cache by iterating every set and every way
    for set in 0..sets {
        for way in 0..ways {
            dcisw((set << bit_pos_of_set) | (way << bit_pos_of_way));
        }
    }
 }
 /// A made-up "instruction": invalidate all of the L1 L2 D-Cache
 #[inline(always)]
 pub fn dciall() {
    dmb();
    l2_cache_invalidate_all();
    dciall_l1();
 }
 /// A made-up "instruction": flush and invalidate all of the L1 D-Cache
 #[inline(always)]
 pub fn dcciall_l1() {
    // the cache associativity could be read from a register, but will
    // always be 4 in L1 data cache of a cortex a9
    let ways = 4;
    let bit_pos_of_way = 30; // 32 - log2(ways)
    // the cache sets could be read from a register, but are always
    // 256 for the cores in the zync-7000; in general, 128 or 512 are
    // also possible.
    let sets = 256;
    let bit_pos_of_set = 5; // for a line size of 8 words = 2^5 bytes
    // select L1 data cache
    unsafe {
        asm!("mcr p15, 2, {}, c0, c0, 0", in(reg) 0);
    }
    // Invalidate entire D-Cache by iterating every set and every way
    for set in 0..sets {
        for way in 0..ways {
            dccisw((set << bit_pos_of_set) | (way << bit_pos_of_way));
        }
    }
 }
 #[inline(always)]
 pub  fn dcciall() {
    dmb();
    dcciall_l1();
    dsb();
    l2_cache_clean_invalidate_all();
    dcciall_l1();
    dsb();
 }
 const CACHE_LINE: usize = 0x20;
 const CACHE_LINE_MASK: usize = CACHE_LINE - 1;
 #[inline]
 fn cache_line_addrs(first_addr: usize, beyond_addr: usize) -> impl Iterator<Item = usize> {
    let first_addr = first_addr & !CACHE_LINE_MASK;
    let beyond_addr = (beyond_addr | CACHE_LINE_MASK) + 1;
    (first_addr..beyond_addr).step_by(CACHE_LINE)
 }
 fn object_cache_line_addrs<T>(object: &T) -> impl Iterator<Item = usize> {
    let first_addr = object as *const _ as usize;
    let beyond_addr = (object as *const _ as usize) + core::mem::size_of_val(object);
    cache_line_addrs(first_addr, beyond_addr)
 }
 fn slice_cache_line_addrs<T>(slice: &[T]) -> impl Iterator<Item = usize> {
    let first_addr = &slice[0] as *const _ as usize;
    let beyond_addr = (&slice[slice.len() - 1] as *const _ as usize) +
        core::mem::size_of_val(&slice[slice.len() - 1]);
    cache_line_addrs(first_addr, beyond_addr)
 }
 /// Data cache clean and invalidate by memory virtual address. This
 /// flushes data out to the point of coherency, and invalidates the
 /// corresponding cache line (as appropriate when DMA is meant to be
 /// writing into it).
 #[inline(always)]
 pub fn dccimvac(addr: usize) {
    unsafe {
        asm!("mcr p15, 0, {}, c7, c14, 1", in(reg) addr);
    }
 }
 /// Data cache clean and invalidate for an object.
 pub fn dcci<T>(object: &T) {
    // ref: L2C310 TRM 3.3.10
    dmb();
    for addr in object_cache_line_addrs(object) {
        dccmvac(addr);
    }
    dsb();
    for addr in object_cache_line_addrs(object) {
        l2_cache_clean_invalidate(addr);
    }
    l2_cache_sync();
    for addr in object_cache_line_addrs(object) {
        dccimvac(addr);
    }
    dsb();
 }
 pub fn dcci_slice<T>(slice: &[T]) {
    dmb();
    for addr in slice_cache_line_addrs(slice) {
        dccmvac(addr);
    }
    dsb();
    for addr in slice_cache_line_addrs(slice) {
        l2_cache_clean_invalidate(addr);
    }
    l2_cache_sync();
    for addr in slice_cache_line_addrs(slice) {
        dccimvac(addr);
    }
    dsb();
 }
 /// Data cache clean by memory virtual address.
 #[inline(always)]
 pub fn dccmvac(addr: usize) {
    unsafe {
        asm!("mcr p15, 0, {}, c7, c10, 1", in(reg) addr);
    }
 }
 /// Data cache clean for an object.
 pub fn dcc<T>(object: &T) {
    dmb();
    for addr in object_cache_line_addrs(object) {
        dccmvac(addr);
    }
    dsb();
    for addr in object_cache_line_addrs(object) {
        l2_cache_clean(addr);
    }
    l2_cache_sync();
 }
 /// Data cache clean for an object. Panics if not properly
 /// aligned and properly sized to be contained in an exact number of
 /// cache lines.
 pub fn dcc_slice<T>(slice: &[T]) {
    if slice.len() == 0 {
        return;
    }
    dmb();
    for addr in slice_cache_line_addrs(slice) {
        dccmvac(addr);
    }
    dsb();
    for addr in slice_cache_line_addrs(slice) {
        l2_cache_clean(addr);
    }
    l2_cache_sync();
 }
 /// Data cache invalidate by memory virtual address. This and
 /// invalidates the cache line containing the given address. Super
 /// unsafe, as this discards a write-back cache line, potentially
 /// affecting more data than intended.
 #[inline(always)]
 pub unsafe fn dcimvac(addr: usize) {
    asm!("mcr p15, 0, {}, c7, c6, 1", in(reg) addr);
 }
 /// Data cache clean and invalidate for an object.
 pub unsafe fn dci<T>(object: &mut T) {
    let first_addr = object as *const _ as usize;
    let beyond_addr = (object as *const _ as usize) + core::mem::size_of_val(object);
    assert_eq!(first_addr & CACHE_LINE_MASK, 0, "dci object first_addr must be aligned");
    assert_eq!(beyond_addr & CACHE_LINE_MASK, 0, "dci object beyond_addr must be aligned");
    dmb();
    for addr in (first_addr..beyond_addr).step_by(CACHE_LINE) {
        l2_cache_invalidate(addr);
    }
    l2_cache_sync();
    for addr in (first_addr..beyond_addr).step_by(CACHE_LINE) {
        dcimvac(addr);
    }
    dsb();
 }
 pub unsafe fn dci_slice<T>(slice: &mut [T]) {
    let first_addr = &slice[0] as *const _ as usize;
    let beyond_addr = (&slice[slice.len() - 1] as *const _ as usize) +
        core::mem::size_of_val(&slice[slice.len() - 1]);
    assert_eq!(first_addr & CACHE_LINE_MASK, 0, "dci slice first_addr must be aligned");
    assert_eq!(beyond_addr & CACHE_LINE_MASK, 0, "dci slice beyond_addr must be aligned");
    dmb();
    for addr in (first_addr..beyond_addr).step_by(CACHE_LINE) {
        l2_cache_invalidate(addr);
    }
    l2_cache_sync();
    for addr in (first_addr..beyond_addr).step_by(CACHE_LINE) {
        dcimvac(addr);
    }
    dsb();
 }
--- a/libcortex_a9/src/exceptions.s
+++ b/libcortex_a9/src/exceptions.s
--- a/libcortex_a9/src/fpu.rs
+++ b/libcortex_a9/src/fpu.rs
@ -0,0 +1,15 @@
 use core::arch::asm;
 /// Enable FPU in the current core.
 pub fn enable_fpu() {
    unsafe {
        asm!("
            mrc p15, 0, r1, c1, c0, 2
            orr r1, r1, (0b1111<<20)
            mcr p15, 0, r1, c1, c0, 2
            vmrs r1, fpexc
            orr r1, r1, (1<<30)
            vmsr fpexc, r1
            ", out("r1") _);
    }
 }
--- a/libcortex_a9/src/l2c.rs
+++ b/libcortex_a9/src/l2c.rs
@ -0,0 +1,333 @@
 use libregister::{register, register_at, register_bit, register_bits, RegisterRW, RegisterR, RegisterW};
 use super::asm::dmb;
 use volatile_register::RW;
 /// enable L2 cache with specific prefetch offset
 /// prefetch offset requires manual tuning, it seems that 8 is good for ZC706 current settings
 pub fn enable_l2_cache(offset: u8) {
    dmb();
    let regs = RegisterBlock::new();
    // disable L2 cache
    regs.reg1_control.modify(|_, w| w.l2_enable(false));
    regs.reg15_prefetch_ctrl.modify(|_, w|
        w.instr_prefetch_en(true)
            .data_prefetch_en(true)
            .double_linefill_en(true)
            .incr_double_linefill_en(true)
            .pref_drop_en(true)
            .prefetch_offset(offset)
    );
    regs.reg1_aux_control.modify(|_, w| {
        w.early_bresp_en(true)
            .instr_prefetch_en(true)
            .data_prefetch_en(true)
            .cache_replace_policy(true)
            .way_size(3)
    });
    regs.reg1_tag_ram_control.modify(|_, w| w.ram_wr_access_lat(1).ram_rd_access_lat(1).ram_setup_lat(1));
    regs.reg1_data_ram_control.modify(|_, w| w.ram_wr_access_lat(1).ram_rd_access_lat(2).ram_setup_lat(1));
    // invalidate L2 ways
    unsafe {
        regs.reg7_inv_way.write(0xFFFF);
    }
    // poll for completion
    while regs.reg7_cache_sync.read().c() {}
    // write to a magic memory location with a magic sequence
    // required in UG585 Section 3.4.10 Initialization Sequence
    unsafe {
        core::ptr::write_volatile(0xF8000008usize as *mut u32, 0xDF0D);
        core::ptr::write_volatile(0xF8000A1Cusize as *mut u32, 0x020202);
        core::ptr::write_volatile(0xF8000004usize as *mut u32, 0x767B);
    }
    regs.reg1_control.modify(|_, w| w.l2_enable(true));
    dmb();
 }
 #[inline(always)]
 pub fn l2_cache_invalidate_all()  {
    let regs = RegisterBlock::new();
    unsafe {
        regs.reg7_inv_way.write(0xFFFF);
    }
    // poll for completion
    while regs.reg7_cache_sync.read().c() {}
 }
 #[inline(always)]
 pub fn l2_cache_clean_all()  {
    let regs = RegisterBlock::new();
    unsafe  {
        regs.reg7_clean_way.write(0xFFFF);
    }
    // poll for completion
    while regs.reg7_cache_sync.read().c() {}
 }
 #[inline(always)]
 pub fn l2_cache_clean_invalidate_all()  {
    let regs = RegisterBlock::new();
    unsafe {
        regs.reg7_clean_inv_way.write(0xFFFF);
    }
    // poll for completion
    while regs.reg7_cache_sync.read().c() {}
 }
 /// L2 cache sync, similar to dsb for L1 cache
 #[inline(always)]
 pub fn l2_cache_sync() {
    let regs = RegisterBlock::new();
    regs.reg7_cache_sync.write(Reg7CacheSync::zeroed().c(false));
 }
 #[inline(always)]
 pub fn l2_cache_clean(addr: usize) {
    let regs = RegisterBlock::new();
    unsafe {
        regs.reg7_clean_pa.write(addr as u32);
    }
 }
 #[inline(always)]
 pub fn l2_cache_invalidate(addr: usize) {
    let regs = RegisterBlock::new();
    unsafe {
        regs.reg7_inv_pa.write(addr as u32);
    }
 }
 #[inline(always)]
 pub fn l2_cache_clean_invalidate(addr: usize) {
    let regs = RegisterBlock::new();
    unsafe {
        regs.reg7_clean_inv_pa.write(addr as u32);
    }
 }
 #[repr(C)]
 struct RegisterBlock {
    /// cache ID register, Returns the 32-bit device ID code it reads off the CACHEID input bus.
    /// The value is specified by the system integrator. Reset value: 0x410000c8
    pub reg0_cache_id: Reg0CacheId,
    /// cache type register, Returns the 32-bit cache type. Reset value: 0x1c100100
    pub reg0_cache_type: Reg0CacheType,
    unused0: [u32; 62],
    /// control register, reset value: 0x0
    pub reg1_control: Reg1Control,
    /// auxilary control register, reset value: 0x02020000
    pub reg1_aux_control: Reg1AuxControl,
    /// Configures Tag RAM latencies
    pub reg1_tag_ram_control: Reg1TagRamControl,
    /// configures data RAM latencies
    pub reg1_data_ram_control: Reg1DataRamControl,
    unused1: [u32; 60],
    /// Permits the event counters to be enabled and reset.
    pub reg2_ev_counter_ctrl: Reg2EvCounterCtrl,
    /// Enables event counter 1 to be driven by a specific event. Counter 1 increments when the
    /// event occurs.
    pub reg2_ev_counter1_cfg: Reg2EvCounter1Cfg,
    /// Enables event counter 0 to be driven by a specific event. Counter 0 increments when the
    /// event occurs.
    pub reg2_ev_counter0_cfg: Reg2EvCounter0Cfg,
    /// Enable the programmer to read off the counter value. The counter counts an event as
    /// specified by the Counter Configuration Registers. The counter can be preloaded if counting
    /// is disabled and reset by the Event Counter Control Register.
    pub reg2_ev_counter1: RW<u32>,
    /// Enable the programmer to read off the counter value. The counter counts an event as
    /// specified by the Counter Configuration Registers. The counter can be preloaded if counting
    /// is disabled and reset by the Event Counter Control Register.
    pub reg2_ev_counter0: RW<u32>,
    /// This register enables or masks interrupts from being triggered on the external pins of the
    /// cache controller. Figure 3-8 on page 3-17 shows the register bit assignments. The bit
    /// assignments enables the masking of the interrupts on both their individual outputs and the
    /// combined L2CCINTR line. Clearing a bit by writing a 0, disables the interrupt triggering on
    /// that pin. All bits are cleared by a reset. You must write to the register bits with a 1 to
    /// enable the generation of interrupts. 1 = Enabled. 0 = Masked. This is the default.
    pub reg2_int_mask: Reg2IntMask,
    /// This register is a read-only.It returns the masked interrupt status. This register can be
    /// accessed by secure and non-secure operations. The register gives an AND function of the raw
    /// interrupt status with the values of the interrupt mask register. All the bits are cleared
    /// by a reset. A write to this register is ignored. Bits read can be HIGH or LOW: HIGH If the
    /// bits read HIGH, they reflect the status of the input lines triggering an interrupt. LOW If
    /// the bits read LOW, either no interrupt has been generated, or the interrupt is masked.
    pub reg2_int_mask_status: Reg2IntMaskStatus,
    /// The Raw Interrupt Status Register enables the interrupt status that excludes the masking
    /// logic. Bits read can be HIGH or LOW: HIGH If the bits read HIGH, they reflect the status of
    /// the input lines triggering an interrupt. LOW If the bits read LOW, no interrupt has been
    /// generated.
    pub reg2_int_raw_status: Reg2IntRawStatus,
    /// Clears the Raw Interrupt Status Register bits. When a bit is written as 1, it clears the
    /// corresponding bit in the Raw Interrupt Status Register. When a bit is written as 0, it has
    /// no effect
    pub reg2_int_clear: Reg2IntClear,
    unused2: [u32; 323],
    /// Drain the STB. Operation complete when all buffers, LRB, LFB, STB, and EB, are empty
    pub reg7_cache_sync: Reg7CacheSync,
    unused3: [u32; 15],
    /// Invalidate Line by PA: Specific L2 cache line is marked as not valid
    pub reg7_inv_pa: RW<u32>,
    unused4: [u32; 2],
    /// Invalidate by Way Invalidate all data in specified ways, including dirty data. An
    /// Invalidate by way while selecting all cache ways is equivalent to invalidating all cache
    /// entries. Completes as a background task with the way, or ways, locked, preventing
    /// allocation.
    pub reg7_inv_way: RW<u32>,
    unused5: [u32; 12],
    /// Clean Line by PA Write the specific L2 cache line to L3 main memory if the line is marked
    /// as valid and dirty. The line is marked as not dirty. The valid bit is unchanged
    pub reg7_clean_pa: RW<u32>,
    unused6: [u32; 1],
    /// Clean Line by Set/Way Write the specific L2 cache line within the specified way to L3 main
    /// memory if the line is marked as valid and dirty. The line is marked as not dirty. The valid
    /// bit is unchanged
    pub reg7_clean_index: Reg7CleanIndex,
    /// Clean by Way Writes each line of the specified L2 cache ways to L3 main memory if the line
    /// is marked as valid and dirty. The lines are marked as not dirty. The valid bits are
    /// unchanged. Completes as a background task with the way, or ways, locked, preventing
    /// allocation.
    pub reg7_clean_way: RW<u32>,
    unused7: [u32; 12],
    /// Clean and Invalidate Line by PA Write the specific L2 cache line to L3 main memory if the
    /// line is marked as valid and dirty. The line is marked as not valid
    pub reg7_clean_inv_pa: RW<u32>,
    unused8: [u32; 1],
    /// Clean and Invalidate Line by Set/Way Write the specific L2 cache line within the specified
    /// way to L3 main memory if the line is marked as valid and dirty. The line is marked as not
    /// valid
    pub reg7_clean_inv_index: Reg7CleanInvIndex,
    /// Clean and Invalidate by Way Writes each line of the specified L2 cache ways to L3 main
    /// memory if the line is marked as valid and dirty. The lines are marked as not valid.
    /// Completes as a background task with the way, or ways, locked, preventing allocation.
    pub reg7_clean_inv_way: RW<u32>,
    unused9: [u32; 0x1D8],
    pub reg15_prefetch_ctrl: Reg15PrefetechCtrl,
 }
 register_at!(RegisterBlock, 0xF8F02000, new);
 register!(reg0_cache_id, Reg0CacheId, RW, u32);
 register_bits!(reg0_cache_id, implementer, u8, 24, 31);
 register_bits!(reg0_cache_id, cache_id, u8, 10, 15);
 register_bits!(reg0_cache_id, part_num, u8, 6, 9);
 register_bits!(reg0_cache_id, rtl_release, u8, 0, 5);
 register!(reg0_cache_type, Reg0CacheType, RW, u32);
 register_bit!(reg0_cache_type, data_banking, 31);
 register_bits!(reg0_cache_type, ctype, u8, 25, 28);
 register_bit!(reg0_cache_type, h, 24);
 register_bits!(reg0_cache_type, dsize_middsize_19, u8, 20, 22);
 register_bit!(reg0_cache_type, l2_assoc_d, 18);
 register_bits!(reg0_cache_type, l2cache_line_len_disize_11, u8, 12, 13);
 register_bits!(reg0_cache_type, isize_midisize_7, u8, 8, 10);
 register_bit!(reg0_cache_type, l2_assoc_i, 6);
 register_bits!(reg0_cache_type, l2cache_line_len_i, u8, 0, 1);
 register!(reg1_control, Reg1Control, RW, u32);
 register_bit!(reg1_control, l2_enable, 0);
 register!(reg1_aux_control, Reg1AuxControl, RW, u32);
 register_bit!(reg1_aux_control, early_bresp_en, 30);
 register_bit!(reg1_aux_control, instr_prefetch_en, 29);
 register_bit!(reg1_aux_control, data_prefetch_en, 28);
 register_bit!(reg1_aux_control, nonsec_inte_access_ctrl, 27);
 register_bit!(reg1_aux_control, nonsec_lockdown_en, 26);
 register_bit!(reg1_aux_control, cache_replace_policy, 25);
 register_bits!(reg1_aux_control, force_write_alloc, u8, 23, 24);
 register_bit!(reg1_aux_control, shared_attr_override_en, 22);
 register_bit!(reg1_aux_control, parity_en, 21);
 register_bit!(reg1_aux_control, event_mon_bus_en, 20);
 register_bits!(reg1_aux_control, way_size, u8, 17, 19);
 register_bit!(reg1_aux_control, associativity, 16);
 register_bit!(reg1_aux_control, shared_attr_inva_en, 13);
 register_bit!(reg1_aux_control, ex_cache_config, 12);
 register_bit!(reg1_aux_control, store_buff_dev_lim_en, 11);
 register_bit!(reg1_aux_control, high_pr_so_dev_rd_en, 10);
 register_bit!(reg1_aux_control, full_line_zero_enable, 0);
 register!(reg1_tag_ram_control, Reg1TagRamControl, RW, u32);
 register_bits!(reg1_tag_ram_control, ram_wr_access_lat, u8, 8, 10);
 register_bits!(reg1_tag_ram_control, ram_rd_access_lat, u8, 4, 6);
 register_bits!(reg1_tag_ram_control, ram_setup_lat, u8, 0, 2);
 register!(reg1_data_ram_control, Reg1DataRamControl, RW, u32);
 register_bits!(reg1_data_ram_control, ram_wr_access_lat, u8, 8, 10);
 register_bits!(reg1_data_ram_control, ram_rd_access_lat, u8, 4, 6);
 register_bits!(reg1_data_ram_control, ram_setup_lat, u8, 0, 2);
 register!(reg2_ev_counter_ctrl, Reg2EvCounterCtrl, RW, u32);
 register_bit!(reg2_ev_counter_ctrl, ev_ctr_en, 0);
 register!(reg2_ev_counter1_cfg, Reg2EvCounter1Cfg, RW, u32);
 register_bits!(reg2_ev_counter1_cfg, ctr_ev_src, u8, 2, 5);
 register_bits!(reg2_ev_counter1_cfg, ev_ctr_intr_gen, u8, 0, 1);
 register!(reg2_ev_counter0_cfg, Reg2EvCounter0Cfg, RW, u32);
 register_bits!(reg2_ev_counter0_cfg, ctr_ev_src, u8, 2, 5);
 register_bits!(reg2_ev_counter0_cfg, ev_ctr_intr_gen, u8, 0, 1);
 register!(reg2_int_mask, Reg2IntMask, RW, u32);
 register_bit!(reg2_int_mask, decerr, 8);
 register_bit!(reg2_int_mask, slverr, 7);
 register_bit!(reg2_int_mask, errrd, 6);
 register_bit!(reg2_int_mask, errrt, 5);
 register_bit!(reg2_int_mask, errwd, 4);
 register_bit!(reg2_int_mask, errwt, 3);
 register_bit!(reg2_int_mask, parrd, 2);
 register_bit!(reg2_int_mask, parrt, 1);
 register_bit!(reg2_int_mask, ecntr, 0);
 register!(reg2_int_mask_status, Reg2IntMaskStatus, RW, u32);
 register_bit!(reg2_int_mask_status, decerr, 8);
 register_bit!(reg2_int_mask_status, slverr, 7);
 register_bit!(reg2_int_mask_status, errrd, 6);
 register_bit!(reg2_int_mask_status, errrt, 5);
 register_bit!(reg2_int_mask_status, errwd, 4);
 register_bit!(reg2_int_mask_status, errwt, 3);
 register_bit!(reg2_int_mask_status, parrd, 2);
 register_bit!(reg2_int_mask_status, parrt, 1);
 register_bit!(reg2_int_mask_status, ecntr, 0);
 register!(reg2_int_raw_status, Reg2IntRawStatus, RW, u32);
 register_bit!(reg2_int_raw_status, decerr, 8);
 register_bit!(reg2_int_raw_status, slverr, 7);
 register_bit!(reg2_int_raw_status, errrd, 6);
 register_bit!(reg2_int_raw_status, errrt, 5);
 register_bit!(reg2_int_raw_status, errwd, 4);
 register_bit!(reg2_int_raw_status, errwt, 3);
 register_bit!(reg2_int_raw_status, parrd, 2);
 register_bit!(reg2_int_raw_status, parrt, 1);
 register_bit!(reg2_int_raw_status, ecntr, 0);
 register!(reg2_int_clear, Reg2IntClear, RW, u32, 0);
 register_bit!(reg2_int_clear, decerr, 8, WTC);
 register_bit!(reg2_int_clear, slverr, 7, WTC);
 register_bit!(reg2_int_clear, errrd, 6, WTC);
 register_bit!(reg2_int_clear, errrt, 5, WTC);
 register_bit!(reg2_int_clear, errwd, 4, WTC);
 register_bit!(reg2_int_clear, errwt, 3, WTC);
 register_bit!(reg2_int_clear, parrd, 2, WTC);
 register_bit!(reg2_int_clear, parrt, 1, WTC);
 register_bit!(reg2_int_clear, ecntr, 0, WTC);
 register!(reg7_cache_sync, Reg7CacheSync, RW, u32);
 register_bit!(reg7_cache_sync, c, 0);
 register!(reg7_clean_index, Reg7CleanIndex, RW, u32);
 register_bits!(reg7_clean_index, way, u8, 28, 30);
 register_bits!(reg7_clean_index, index, u8, 5, 11);
 register_bit!(reg7_clean_index, c, 0);
 register!(reg7_clean_inv_index, Reg7CleanInvIndex, RW, u32);
 register_bits!(reg7_clean_inv_index, way, u8, 28, 30);
 register_bits!(reg7_clean_inv_index, index, u8, 5, 11);
 register_bit!(reg7_clean_inv_index, c, 0);
 register!(reg15_prefetch_ctrl, Reg15PrefetechCtrl, RW, u32);
 register_bit!(reg15_prefetch_ctrl, double_linefill_en, 30);
 register_bit!(reg15_prefetch_ctrl, instr_prefetch_en, 29);
 register_bit!(reg15_prefetch_ctrl, data_prefetch_en, 28);
 register_bit!(reg15_prefetch_ctrl, pref_drop_en, 24);
 register_bit!(reg15_prefetch_ctrl, incr_double_linefill_en, 23);
 register_bits!(reg15_prefetch_ctrl, prefetch_offset, u8, 0, 4);
--- a/libcortex_a9/src/lib.rs
+++ b/libcortex_a9/src/lib.rs
@ -0,0 +1,85 @@
 #![no_std]
 #![feature(never_type)]
 #![feature(global_asm)]
 #![feature(asm)]
 #![allow(incomplete_features)]
 #![feature(inline_const)]
 #![feature(const_fn_trait_bound)]
 extern crate alloc;
 pub mod asm;
 pub mod cache;
 mod fpu;
 pub mod l2c;
 pub mod mmu;
 pub mod mutex;
 pub mod regs;
 pub mod semaphore;
 pub mod sync_channel;
 mod uncached;
 pub use fpu::enable_fpu;
 pub use uncached::UncachedSlice;
 use core::arch::global_asm;
 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();
    }
 }
 #[macro_export]
 /// 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.
 /// - `stack1` is the stack for the interrupt handler when called from core1.
 /// - `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.
 macro_rules! interrupt_handler {
    ($name:ident, $name2:ident, $stack0:ident, $stack1:ident, $body:block) => {
        #[link_section = ".text.boot"]
        #[no_mangle]
        #[naked]
        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
    };
 }
--- a/libcortex_a9/src/mmu.rs
+++ b/libcortex_a9/src/mmu.rs
@ -1,6 +1,6 @@
 use bit_field::BitField;
-use super::{regs::*, asm};
+use super::{regs::*, asm::*, cache::*};
-use crate::regs::RegisterW;
+use libregister::RegisterW;
 #[derive(Copy, Clone)]
 #[repr(u8)]
@ -44,6 +44,12 @@ pub enum AccessPermissions {
 }
 impl AccessPermissions {
    fn new(ap: u8, apx: bool) -> Self {
        unsafe {
            core::mem::transmute(if apx { 0b100 } else { 0 } | ap)
        }
    }
    fn ap(&self) -> u8 {
        (*self as u8) & 0b11
    }
@ -65,45 +71,64 @@ pub struct L1Section {
    pub bufferable: bool,
 }
 const ENTRY_TYPE_SECTION: u32 = 0b10;
 pub const L1_PAGE_SIZE: usize = 0x100000;
 #[repr(C)]
 #[derive(Clone, Copy)]
 pub struct L1Entry(u32);
 impl L1Entry {
    #[inline(always)]
-    pub fn section(phys_base: u32, section: L1Section) -> Self {
+    pub fn from_section(phys_base: u32, section: L1Section) -> Self {
        // Must be aligned to 1 MB
        assert!(phys_base & 0x000f_ffff == 0);
        let mut entry = L1Entry(phys_base);
-        entry.0.set_bits(0..=1, 0b10);
+        entry.set_section(section);
        entry.0.set_bit(2, section.bufferable);
        entry.0.set_bit(3, section.cacheable);
        entry.0.set_bit(4, !section.exec);
        assert!(section.domain < 16);
        entry.0.set_bits(5..=8, section.domain.into());
        entry.0.set_bits(10..=11, section.access.ap().into());
        assert!(section.tex < 8);
        entry.0.set_bits(12..=14, section.tex.into());
        entry.0.set_bit(15, section.access.apx());
        entry.0.set_bit(16, section.shareable);
        entry.0.set_bit(17, !section.global);
        entry
    }
    pub fn get_section(&mut self) -> L1Section {
        assert_eq!(self.0.get_bits(0..=1), ENTRY_TYPE_SECTION);
        let access = AccessPermissions::new(
            self.0.get_bits(10..=11) as u8,
            self.0.get_bit(15)
        );
        L1Section {
            global: !self.0.get_bit(17),
            shareable: self.0.get_bit(16),
            access,
            tex: self.0.get_bits(12..=14) as u8,
            domain: self.0.get_bits(5..=8) as u8,
            exec: !self.0.get_bit(4),
            cacheable: self.0.get_bit(3),
            bufferable: self.0.get_bit(2),
        }
    }
    pub fn set_section(&mut self, section: L1Section) {
        self.0.set_bits(0..=1, ENTRY_TYPE_SECTION);
        self.0.set_bit(2, section.bufferable);
        self.0.set_bit(3, section.cacheable);
        self.0.set_bit(4, !section.exec);
        assert!(section.domain < 16);
        self.0.set_bits(5..=8, section.domain.into());
        self.0.set_bits(10..=11, section.access.ap().into());
        assert!(section.tex < 8);
        self.0.set_bits(12..=14, section.tex.into());
        self.0.set_bit(15, section.access.apx());
        self.0.set_bit(16, section.shareable);
        self.0.set_bit(17, !section.global);
    }
 }
 const L1_TABLE_SIZE: usize = 4096;
-#[doc(hidden)]
+static mut L1_TABLE: L1Table = L1Table {
 #[link_section = ".bss.l1_table"]
 #[no_mangle]
 pub static mut l1_table: L1Table = L1Table {
    table: [L1Entry(0); L1_TABLE_SIZE]
 };
-/// The `#[repr(align(16384))]` is unfortunately ineffective. Hence we
+#[repr(C, align(16384))]
 /// require explicit linking to a region defined in the linker script.
 #[repr(align(16384))]
 pub struct L1Table {
    table: [L1Entry; L1_TABLE_SIZE]
 }
@ -111,7 +136,7 @@ pub struct L1Table {
 impl L1Table {
    pub fn get() -> &'static mut Self {
        unsafe {
-            &mut l1_table
+            &mut L1_TABLE
        }
    }
@ -124,37 +149,22 @@ impl L1Table {
            tex: 0b101,
            domain: 0b1111,
            exec: true,
-            // TODO: temporarily turn on cache for SMP testing;
+            cacheable: true,
            //       consider turning it off again for production
            cacheable: !false, 
            bufferable: true,
        });
        /* (DDR cacheable) */
-        for ddr in 1..=0x1ff {
+        for ddr in 1..=0x3ff {
            self.direct_mapped_section(ddr, L1Section {
                global: true,
                shareable: true,
                access: AccessPermissions::FullAccess,
-                tex: 0b101,
+                tex: 0b0,
                domain: 0b1111,
                exec: true,
                cacheable: true,
                bufferable: true,
            });
        }
        /* (unassigned/reserved). */
        for undef in 0x1ff..=0x3ff {
            self.direct_mapped_section(undef, L1Section {
                global: false,
                shareable: false,
                access: AccessPermissions::PermissionFault,
                tex: 0,
                domain: 0,
                exec: false,
                cacheable: false,
                bufferable: false,
            });
        }
        /* 0x40000000 - 0x7fffffff (FPGA slave0) */
        for fpga_slave in 0x400..=0x7ff {
            self.direct_mapped_section(fpga_slave, L1Section {
@ -203,7 +213,7 @@ impl L1Table {
                access: AccessPermissions::FullAccess,
                tex: 0,
                domain: 0,
-                exec: true,
+                exec: false,
                cacheable: false,
                bufferable: true,
            });
@ -328,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,
@ -345,7 +355,34 @@ impl L1Table {
        assert!(index < L1_TABLE_SIZE);
        let base = (index as u32) << 20;
-        self.table[index] = L1Entry::section(base, section);
+        self.table[index] = L1Entry::from_section(base, section);
    }
    pub fn update<T, F, R>(&mut self, ptr: *const T, f: F) -> R
    where
        F: FnOnce(&'_ mut L1Section) -> R,
    {
        let index = (ptr as usize) >> 20;
        let entry = &mut self.table[index];
        let mut section = entry.get_section();
        let result = f(&mut section);
        entry.set_section(section);
        // Flush L1Dcache
        dcciall();
        // // TODO: L2?
        // Invalidate TLB
        tlbiall();
        // Invalidate all branch predictors
        bpiall();
        // ensure completion of the BP and TLB invalidation
        dsb();
        // synchronize context on this processor
        isb();
        result
    }
 }
@ -373,14 +410,15 @@ pub fn with_mmu<F: FnMut() -> !>(l1table: &L1Table, mut f: F) -> ! {
            .a(false)
            .c(true)
            .i(true)
            .z(true)
            .unaligned(true)
    );
    // Synchronization barriers
    // Allows MMU to start
-    asm::dsb();
+    dsb();
    // Flushes pre-fetch buffer
-    asm::isb();
+    isb();
    f();
 }
--- a/libcortex_a9/src/mutex.rs
+++ b/libcortex_a9/src/mutex.rs
@ -0,0 +1,112 @@
 use core::ops::{Deref, DerefMut};
 use core::sync::atomic::{AtomicU32, Ordering};
 use core::cell::UnsafeCell;
 use core::task::{Context, Poll};
 use core::pin::Pin;
 use core::future::Future;
 use super::{
    spin_lock_yield, notify_spin_lock,
    asm::{enter_critical, exit_critical}
 };
 const LOCKED: u32 = 1;
 const UNLOCKED: u32 = 0;
 /// Mutex implementation for Cortex-A9
 ///
 /// [ARM Synchronization Primitives Development Article: Implementing a mutex](http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dht0008a/ch01s03s02.html)
 pub struct Mutex<T> {
    locked: AtomicU32,
    inner: UnsafeCell<T>,
 }
 unsafe impl<T: Send> Sync for Mutex<T> {}
 unsafe impl<T: Send> Send for Mutex<T> {}
 struct Fut<'a, T>(&'a Mutex<T>);
 impl<'a, T> Future for Fut<'a, T> {
    type Output = MutexGuard<'a, T>;
    fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
        let irq = unsafe { enter_critical() };
        if self.0.locked.compare_exchange_weak(UNLOCKED, LOCKED, Ordering::AcqRel, Ordering::Relaxed).is_err() {
            unsafe { exit_critical(irq) };
            cx.waker().wake_by_ref();
            Poll::Pending
        }
        else {
            Poll::Ready(MutexGuard { mutex: self.0, irq })
        }
    }
 }
 impl<T> Mutex<T> {
    /// Constructor, const-fn
    pub const fn new(inner: T) -> Self {
        Mutex{
            locked: AtomicU32::new(UNLOCKED),
            inner: UnsafeCell::new(inner),
        }
    }
    /// Lock the Mutex, blocks when already locked
    pub fn lock(&self) -> MutexGuard<T> {
        let mut irq = unsafe { enter_critical() };
        while self.locked.compare_exchange_weak(UNLOCKED, LOCKED, Ordering::AcqRel, Ordering::Relaxed).is_err() {
            unsafe {
                exit_critical(irq);
                spin_lock_yield();
                irq = enter_critical();
            }
        }
        MutexGuard { mutex: self, irq }
    }
    pub async fn async_lock(&self) -> MutexGuard<'_, T> {
        Fut(&self).await
    }
    pub fn try_lock(&self) -> Option<MutexGuard<T>> {
        let irq = unsafe { enter_critical() };
        if self.locked.compare_exchange_weak(UNLOCKED, LOCKED, Ordering::AcqRel, Ordering::Relaxed).is_err() {
            unsafe { exit_critical(irq) };
            None
        } else {
            Some(MutexGuard { mutex: self, irq })
        }
    }
    fn unlock(&self) {
        self.locked.store(UNLOCKED, Ordering::Release);
        notify_spin_lock();
    }
 }
 /// Returned by `Mutex.lock()`, allows access to data via
 /// `Deref`/`DerefMutx`
 pub struct MutexGuard<'a, T> {
    mutex: &'a Mutex<T>,
    irq: bool,
 }
 impl<'a, T> Deref for MutexGuard<'a, T> {
    type Target = T;
    fn deref(&self) -> &T {
        unsafe { &*self.mutex.inner.get() }
    }
 }
 impl<'a, T> DerefMut for MutexGuard<'a, T> {
    fn deref_mut(&mut self) -> &mut T {
        unsafe { &mut *self.mutex.inner.get() }
    }
 }
 /// Automatically `Mutex.unlock()` when this reference is dropped
 impl<'a, T> Drop for MutexGuard<'a, T> {
    fn drop(&mut self) {
        self.mutex.unlock();
        unsafe { exit_critical(self.irq) };
    }
 }
--- a/libcortex_a9/src/regs.rs
+++ b/libcortex_a9/src/regs.rs
@ -0,0 +1,205 @@
 use libregister::{
    register_bit, register_bits,
    RegisterR, RegisterW, RegisterRW,
 };
 use core::arch::asm;
 macro_rules! def_reg_r {
    ($name:tt, $type: ty, $asm_instr:tt) => {
        impl RegisterR for $name {
            type R = $type;
            #[inline]
            fn read(&self) -> Self::R {
                let mut value: u32;
                unsafe { asm!($asm_instr, lateout(reg) value) }
                value.into()
            }
        }
    }
 }
 macro_rules! def_reg_w {
    ($name:ty, $type:ty, $asm_instr:tt) => {
        impl RegisterW for $name {
            type W = $type;
            #[inline]
            fn write(&mut self, value: Self::W) {
                let value: u32 = value.into();
                unsafe { asm!($asm_instr, in(reg) value) }
            }
            #[inline]
            fn zeroed() -> Self::W {
                0u32.into()
            }
        }
    }
 }
 macro_rules! wrap_reg {
    ($mod_name: ident) => {
        pub mod $mod_name {
            pub struct Read {
                pub inner: u32,
            }
            impl From<u32> for Read {
                #[inline]
                fn from(value: u32) -> Self {
                    Read { inner: value }
                }
            }
            pub struct Write {
                pub inner: u32,
            }
            impl From<u32> for Write {
                #[inline]
                fn from(value: u32) -> Self {
                    Write { inner: value }
                }
            }
            impl Into<u32> for Write {
                #[inline]
                fn into(self) -> u32 {
                    self.inner
                }
            }
        }
    }
 }
 /// Stack Pointer
 pub struct SP;
 def_reg_r!(SP, u32, "mov {}, sp");
 def_reg_w!(SP, u32, "mov sp, {}");
 /// Link register (function call return address)
 pub struct LR;
 def_reg_r!(LR, u32, "mov {}, lr");
 def_reg_w!(LR, u32, "mov lr, {}");
 pub struct VBAR;
 def_reg_r!(VBAR, u32, "mrc p15, 0, {}, c12, c0, 0");
 def_reg_w!(VBAR, u32, "mcr p15, 0, {}, c12, c0, 0");
 pub struct MVBAR;
 def_reg_r!(MVBAR, u32, "mrc p15, 0, {}, c12, c0, 1");
 def_reg_w!(MVBAR, u32, "mcr p15, 0, {}, c12, c0, 1");
 pub struct HVBAR;
 def_reg_r!(HVBAR, u32, "mrc p15, 4, {}, c12, c0, 0");
 def_reg_w!(HVBAR, u32, "mcr p15, 4, {}, c12, c0, 0");
 /// Multiprocess Affinity Register
 pub struct MPIDR;
 def_reg_r!(MPIDR, mpidr::Read, "mrc p15, 0, {}, c0, c0, 5");
 wrap_reg!(mpidr);
 register_bits!(mpidr,
               /// CPU core index
               cpu_id, u8, 0, 1);
 register_bits!(mpidr,
               /// Processor index in "multi-socket" systems
               cluster_id, u8, 8, 11);
 register_bit!(mpidr,
              /// true if part of uniprocessor system
              u, 30);
 pub struct DFAR;
 def_reg_r!(DFAR, u32, "mrc p15, 0, {}, c6, c0, 0");
 pub struct DFSR;
 def_reg_r!(DFSR, u32, "mrc p15, 0, {}, c5, c0, 0");
 pub struct SCTLR;
 wrap_reg!(sctlr);
 def_reg_r!(SCTLR, sctlr::Read, "mrc p15, 0, {}, c1, c0, 0");
 def_reg_w!(SCTLR, sctlr::Write, "mcr p15, 0, {}, c1, c0, 0");
 register_bit!(sctlr,
              /// Enables MMU
              m, 0);
 register_bit!(sctlr,
              /// Strict Alignment Checking
              a, 1);
 register_bit!(sctlr,
              /// Data Caching
              c, 2);
 register_bit!(sctlr, sw, 10);
 register_bit!(sctlr, z, 11);
 register_bit!(sctlr, i, 12);
 register_bit!(sctlr, v, 13);
 register_bit!(sctlr, ha, 17);
 register_bit!(sctlr, ee, 25);
 register_bit!(sctlr,
              /// (read-only)
              nmfi, 27);
 register_bit!(sctlr, unaligned, 22);
 register_bit!(sctlr,
              /// TEX Remap Enable
              tre, 28);
 register_bit!(sctlr,
              /// Access Flag Enable
              afe, 29);
 register_bit!(sctlr,
              /// Thumb Exception Enable
              te, 30);
 /// Auxiliary Control Register
 pub struct ACTLR;
 wrap_reg!(actlr);
 def_reg_r!(ACTLR, actlr::Read, "mrc p15, 0, {}, c1, c0, 1");
 def_reg_w!(ACTLR, actlr::Write, "mcr p15, 0, {}, c1, c0, 1");
 // SMP bit
 register_bit!(actlr, parity_on, 9);
 register_bit!(actlr, alloc_one_way, 8);
 register_bit!(actlr, excl, 7);
 register_bit!(actlr, smp, 6);
 register_bit!(actlr, write_full_line_of_zeros, 3);
 register_bit!(actlr, l1_prefetch_enable, 2);
 // L2 cache prefetch hint, in UG585 section 3.4.8
 register_bit!(actlr, l2_prefetch_enable, 1);
 // Cache/TLB maintenance broadcast
 register_bit!(actlr, fw, 0);
 impl RegisterRW for ACTLR {
    #[inline]
    fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
        let r = self.read();
        let w = actlr::Write { inner: r.inner };
        let w = f(r, w);
        self.write(w);
    }
 }
 impl ACTLR {
    pub fn enable_smp(&mut self) {
        self.modify(|_, w| w.smp(true).fw(true).alloc_one_way(true));
    }
    pub fn enable_prefetch(&mut self) {
        self.modify(|_, w| w.l1_prefetch_enable(true).l2_prefetch_enable(true))
    }
 }
 /// Domain Access Control Register
 pub struct DACR;
 def_reg_r!(DACR, u32, "mrc p15, 0, {}, c3, c0, 0");
 def_reg_w!(DACR, u32, "mcr p15, 0, {}, c3, c0, 0");
 /// Translation Table Base Register 0
 pub struct TTBR0;
 /// Translation Table Base Register 1
 pub struct TTBR1;
 def_reg_r!(TTBR0, ttbr::Read, "mrc p15, 0, {}, c2, c0, 0");
 def_reg_w!(TTBR0, ttbr::Write, "mcr p15, 0, {}, c2, c0, 0");
 def_reg_r!(TTBR1, ttbr::Read, "mrc p15, 0, {}, c2, c0, 1");
 def_reg_w!(TTBR1, ttbr::Write, "mcr p15, 0, {}, c2, c0, 1");
 wrap_reg!(ttbr);
 register_bits!(ttbr, table_base, u32, 14, 31);
 register_bit!(ttbr, irgn0, 6);
 register_bits!(ttbr, rgn, u8, 3, 4);
 register_bit!(ttbr,
              /// Translation table walk to shared memory?
              s, 1);
 register_bit!(ttbr, irgn1, 0);
--- a/libcortex_a9/src/semaphore.rs
+++ b/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 const 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_exchange_weak(value, value - 1, Ordering::SeqCst, Ordering::Relaxed).is_ok() {
                    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_exchange_weak(value, value + 1, Ordering::SeqCst, Ordering::Relaxed).is_ok() {
                    notify_spin_lock();
                    return;
                }
            } else {
                return;
            }
        }
    }
 }
--- a/libcortex_a9/src/sync_channel.rs
+++ b/libcortex_a9/src/sync_channel.rs
@ -0,0 +1,189 @@
 use core::{
    pin::Pin,
    future::Future,
    sync::atomic::{AtomicPtr, AtomicUsize, Ordering},
    task::{Context, Poll},
 };
 use alloc::boxed::Box;
 use super::{spin_lock_yield, notify_spin_lock};
 pub struct Sender<'a, T> where T: Clone {
    list: &'a [AtomicPtr<T>],
    write: &'a AtomicUsize,
    read: &'a AtomicUsize,
 }
 pub struct Receiver<'a, T> where T: Clone {
    list: &'a [AtomicPtr<T>],
    write: &'a AtomicUsize,
    read: &'a AtomicUsize,
 }
 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}
    }
    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 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 {
                    Box::from_raw(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, 'b, T> where T: Clone, 'b: 'a {
            sender: &'a mut Sender<'b, T>,
            content: Result<(), Box<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 core::mem::replace(&mut self.content, Ok(())) {
                    Err(content) => {
                        if let Err(content) = self.sender.try_send(content) {
                            // failure
                            self.content = Err(content);
                            cx.waker().wake_by_ref();
                            Poll::Pending
                        } else {
                            // success
                            Poll::Ready(())
                        }
                    }
                    Ok(_) => panic!("Send future polled after success"),
                }
            }
        }
        Send {
            sender: self,
            content: Err(content.into()),
        }.await
    }
    /// 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() {
                Box::from_raw(original);
            }
        }
    }
    /// 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);
        }
    }
 }
 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}
    }
    pub fn try_recv(&mut self) -> Result<T, ()> {
        let read = self.read.load(Ordering::Relaxed);
        if read == self.write.load(Ordering::Acquire) {
            Err(())
        } else {
            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 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> where T: Clone {
            type Output = T;
            fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
                if let Ok(content) = self.receiver.try_recv() {
                    Poll::Ready(content)
                } else {
                    cx.waker().wake_by_ref();
                    Poll::Pending
                }
            }
        }
        Recv {
            receiver: self,
        }.await
    }
 }
 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.
 macro_rules! sync_channel {
    ($t: ty, $cap: expr) => {
        {
            use core::sync::atomic::{AtomicUsize, AtomicPtr};
            use $crate::sync_channel::{Sender, Receiver};
            const fn new_atomic() -> AtomicPtr<$t> {
                AtomicPtr::new(core::ptr::null_mut())
            }
            static LIST: [AtomicPtr<$t>; $cap + 1] = [const { new_atomic() }; $cap + 1];
            static WRITE: AtomicUsize = AtomicUsize::new(0);
            static READ: AtomicUsize = AtomicUsize::new(0);
            (Sender::new(&LIST, &WRITE, &READ), Receiver::new(&LIST, &WRITE, &READ))
        }
    };
 }
--- a/libcortex_a9/src/uncached.rs
+++ b/libcortex_a9/src/uncached.rs
@ -0,0 +1,67 @@
 use core::{
    ops::{Deref, DerefMut},
    mem::{align_of, size_of},
 };
 use alloc::alloc::{dealloc, Layout, LayoutError};
 use crate::mmu::{L1_PAGE_SIZE, L1Table};
 pub struct UncachedSlice<T: 'static> {
    layout: Layout,
    slice: &'static mut [T],
 }
 impl<T> UncachedSlice<T> {
    /// allocates in chunks of 1 MB
    pub fn new<F: Fn() -> T>(len: usize, default: F) -> Result<Self, LayoutError> {
        // round to full pages
        let size = ((len * size_of::<T>() - 1) | (L1_PAGE_SIZE - 1)) + 1;
        let align = align_of::<T>()
            .max(L1_PAGE_SIZE);
        let layout = Layout::from_size_align(size, align)?;
        let ptr = unsafe { alloc::alloc::alloc(layout).cast::<T>() };
        let start = ptr as usize;
        assert_eq!(start & (L1_PAGE_SIZE - 1), 0);
        for page_start in (start..(start + size)).step_by(L1_PAGE_SIZE) {
            // non-shareable device
            L1Table::get()
                .update(page_start as *const (), |l1_section| {
                    l1_section.tex = 0b10;
                    l1_section.cacheable = true;
                    l1_section.bufferable = false;
                });
        }
        let slice = unsafe { core::slice::from_raw_parts_mut(ptr, len) };
        // verify size
        assert!(unsafe { slice.get_unchecked(len) } as *const _ as usize <= start + size);
        // initialize
        for e in slice.iter_mut() {
            *e = default();
        }
        Ok(UncachedSlice { layout, slice })
    }
 }
 /// Does not yet mark the pages cachable again
 impl<T> Drop for UncachedSlice<T> {
    fn drop(&mut self) {
        unsafe {
            dealloc(self.slice.as_mut_ptr() as *mut _ as *mut u8, self.layout);
        }
    }
 }
 impl<T> Deref for UncachedSlice<T> {
    type Target = [T];
    fn deref(&self) -> &Self::Target {
        self.slice
    }
 }
 impl<T> DerefMut for UncachedSlice<T> {
    fn deref_mut(&mut self) -> &mut Self::Target {
        self.slice
    }
 }
--- a/libregister/Cargo.toml
+++ b/libregister/Cargo.toml
@ -0,0 +1,10 @@
 [package]
 name = "libregister"
 version = "0.0.0"
 authors = ["M-Labs"]
 edition = "2018"
 [dependencies]
 vcell = "0.1"
 volatile-register = "0.2"
 bit_field = "0.10"
--- a/libregister/src/lib.rs
+++ b/libregister/src/lib.rs
@ -1,9 +1,11 @@
 //! Type-safe interface to peripheral registers akin to the code that
 //! svd2rust generates.
 #![allow(unused)]
-use volatile_register::{RO, WO, RW};
+#![no_std]
-use bit_field::BitField;
+
 pub use vcell::VolatileCell;
 pub use volatile_register::{RO, WO, RW};
 pub use bit_field::BitField;
 /// A readable register
 pub trait RegisterR {
@ -28,16 +30,19 @@ pub trait RegisterRW: RegisterR + RegisterW {
 #[doc(hidden)]
 #[macro_export]
 macro_rules! register_common {
-    ($mod_name: ident, $struct_name: ident, $access: ty, $inner: ty) => (
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, $access: ty, $inner: ty) => (
        #[repr(C)]
        $(#[$outer])*
        pub struct $struct_name {
            inner: $access,
        }
        pub mod $mod_name {
            #[derive(Clone)]
            pub struct Read {
                pub inner: $inner,
            }
            #[derive(Clone)]
            pub struct Write {
                pub inner: $inner,
            }
@ -48,9 +53,10 @@ macro_rules! register_common {
 #[macro_export]
 macro_rules! register_r {
    ($mod_name: ident, $struct_name: ident) => (
-        impl crate::regs::RegisterR for $struct_name {
+        impl $crate::RegisterR for $struct_name {
            type R = $mod_name::Read;
            #[inline]
            fn read(&self) -> Self::R {
                let inner = self.inner.read();
                $mod_name::Read { inner }
@ -62,13 +68,15 @@ macro_rules! register_r {
 #[macro_export]
 macro_rules! register_w {
    ($mod_name: ident, $struct_name: ident) => (
-         impl crate::regs::RegisterW for $struct_name {
+        impl $crate::RegisterW for $struct_name {
            type W = $mod_name::Write;
            #[inline]
            fn zeroed() -> $mod_name::Write {
                $mod_name::Write { inner: 0 }
            }
            #[inline]
            fn write(&mut self, w: Self::W) {
                unsafe {
                    self.inner.write(w.inner);
@ -81,7 +89,8 @@ macro_rules! register_w {
 #[macro_export]
 macro_rules! register_rw {
    ($mod_name: ident, $struct_name: ident) => (
-        impl crate::regs::RegisterRW for $struct_name {
+        impl $crate::RegisterRW for $struct_name {
            #[inline]
            fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
                unsafe {
                    self.inner.modify(|inner| {
@ -92,29 +101,94 @@ macro_rules! register_rw {
            }
        }
    );
    ($mod_name: ident, $struct_name: ident, $mask: expr) => (
        impl $crate::RegisterRW for $struct_name {
            #[inline]
            fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
                unsafe {
                    self.inner.modify(|inner| {
                        f($mod_name::Read { inner }, $mod_name::Write { inner: inner & ($mask) })
                            .inner
                    });
                }
            }
        }
    );
 }
 #[doc(hidden)]
 #[macro_export]
 macro_rules! register_vcell {
    ($mod_name: ident, $struct_name: ident) => (
        impl $crate::RegisterR for $struct_name {
            type R = $mod_name::Read;
            #[inline]
            fn read(&self) -> Self::R {
                let inner = self.inner.get();
                $mod_name::Read { inner }
            }
        }
        impl $crate::RegisterW for $struct_name {
            type W = $mod_name::Write;
            #[inline]
            fn zeroed() -> $mod_name::Write {
                $mod_name::Write { inner: 0 }
            }
            #[inline]
            fn write(&mut self, w: Self::W) {
                self.inner.set(w.inner);
            }
        }
        impl $crate::RegisterRW for $struct_name {
            #[inline]
            fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
                let r = self.read();
                let w = $mod_name::Write { inner: r.inner };
                let w = f(r, w);
                self.write(w);
            }
        }
    );
 }
 /// Main macro for register definition
 #[macro_export]
 macro_rules! register {
    // Define read-only register
-    ($mod_name: ident, $struct_name: ident, RO, $inner: ty) => (
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, RO, $inner: ty) => (
-        crate::register_common!($mod_name, $struct_name, volatile_register::RO<$inner>, $inner);
+        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, $crate::RO<$inner>, $inner);
-        crate::register_r!($mod_name, $struct_name);
+        $crate::register_r!($mod_name, $struct_name);
    );
    // Define write-only register
-    ($mod_name: ident, $struct_name: ident, WO, $inner: ty) => (
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, WO, $inner: ty) => (
-        crate::register_common!($mod_name, $struct_name, volatile_register::WO<$inner>, $inner);
+        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, volatile_register::WO<$inner>, $inner);
-        crate::register_w!($mod_name, $struct_name);
+        $crate::register_w!($mod_name, $struct_name);
    );
    // Define read-write register
-    ($mod_name: ident, $struct_name: ident, RW, $inner: ty) => (
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, RW, $inner: ty) => (
-        crate::register_common!($mod_name, $struct_name, volatile_register::RW<$inner>, $inner);
+        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, volatile_register::RW<$inner>, $inner);
-        crate::register_r!($mod_name, $struct_name);
+        $crate::register_r!($mod_name, $struct_name);
-        crate::register_w!($mod_name, $struct_name);
+        $crate::register_w!($mod_name, $struct_name);
-        crate::register_rw!($mod_name, $struct_name);
+        $crate::register_rw!($mod_name, $struct_name);
    );
    // Define read-write register
    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, VolatileCell, $inner: ty) => (
        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, VolatileCell<$inner>, $inner);
        $crate::register_vcell!($mod_name, $struct_name);
    );
    // Define read-write register with mask on write (for WTC mixed access.)
    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, RW, $inner: ty, $mask: expr) => (
        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, volatile_register::RW<$inner>, $inner);
        $crate::register_r!($mod_name, $struct_name);
        $crate::register_w!($mod_name, $struct_name);
        $crate::register_rw!($mod_name, $struct_name, $mask);
    );
 }
@ -125,6 +199,7 @@ macro_rules! register_bit {
        $(#[$outer])*
        impl $mod_name::Read {
            #[allow(unused)]
            #[inline]
            pub fn $name(&self) -> bool {
                use bit_field::BitField;
@ -135,6 +210,7 @@ macro_rules! register_bit {
        $(#[$outer])*
        impl $mod_name::Write {
            #[allow(unused)]
            #[inline]
            pub fn $name(mut self, value: bool) -> Self {
                use bit_field::BitField;
@ -143,6 +219,47 @@ macro_rules! register_bit {
            }
        }
    );
    // Single bit read-only
    ($mod_name: ident, $(#[$outer:meta])* $name: ident, $bit: expr, RO) => (
        $(#[$outer])*
        impl $mod_name::Read {
            #[allow(unused)]
            #[inline]
            pub fn $name(&self) -> bool {
                use bit_field::BitField;
                self.inner.get_bit($bit)
            }
        }
    );
    // Single bit write to clear. Note that this must be used with WTC register.
    ($mod_name: ident, $(#[$outer:meta])* $name: ident, $bit: expr, WTC) => (
        $(#[$outer])*
        impl $mod_name::Read {
            #[allow(unused)]
            #[inline]
            pub fn $name(&self) -> bool {
                use bit_field::BitField;
                self.inner.get_bit($bit)
            }
        }
        $(#[$outer])*
        impl $mod_name::Write {
            /// Clear bit field. (WTC)
            #[allow(unused)]
            #[inline]
            pub fn $name(mut self) -> Self {
                use bit_field::BitField;
                self.inner.set_bit($bit, true);
                self
            }
        }
    );
 }
 /// Define a multi-bit field of a register
@ -151,6 +268,7 @@ macro_rules! register_bits {
    ($mod_name: ident, $(#[$outer:meta])* $name: ident, $type: ty, $bit_begin: expr, $bit_end: expr) => (
        impl $mod_name::Read {
            #[allow(unused)]
            #[inline]
            $(#[$outer])*
            pub fn $name(&self) -> $type {
                use bit_field::BitField;
@ -163,6 +281,7 @@ macro_rules! register_bits {
        $(#[$outer])*
        impl $mod_name::Write {
            #[allow(unused)]
            #[inline]
            pub fn $name(mut self, value: $type) -> Self {
                use bit_field::BitField;
@ -182,6 +301,7 @@ macro_rules! register_bits_typed {
    ($mod_name: ident, $(#[$outer:meta])* $name: ident, $bit_type: ty, $type: ty, $bit_begin: expr, $bit_end: expr) => (
        impl $mod_name::Read {
            #[allow(unused)]
            #[inline]
            $(#[$outer])*
            pub fn $name(&self) -> $type {
                use bit_field::BitField;
@ -193,6 +313,7 @@ macro_rules! register_bits_typed {
        impl $mod_name::Write {
            #[allow(unused)]
            #[inline]
            $(#[$outer])*
            pub fn $name(mut self, value: $type) -> Self {
                use bit_field::BitField;
@ -210,6 +331,7 @@ macro_rules! register_at {
    ($name: ident, $addr: expr, $ctor: ident) => (
        impl $name {
            #[allow(unused)]
            #[inline]
            pub fn $ctor() -> &'static mut Self {
                let addr = $addr as *mut Self;
                unsafe { &mut *addr }
--- a/libsupport_zynq/Cargo.toml
+++ b/libsupport_zynq/Cargo.toml
@ -0,0 +1,30 @@
 [package]
 name = "libsupport_zynq"
 description = "Software support for running in the Zynq PS"
 version = "0.0.0"
 authors = ["M-Labs"]
 edition = "2018"
 [features]
 target_zc706 = ["libboard_zynq/target_zc706"]
 target_coraz7 = ["libboard_zynq/target_coraz7"]
 target_ebaz4205 = ["libboard_zynq/target_ebaz4205"]
 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", "dummy_fiq_handler"]
 [dependencies]
 r0 = "1"
 compiler_builtins = "=0.1.49"
 linked_list_allocator = { version = "0.8", default-features = false, features = ["const_mut_refs"] }
 libregister = { path = "../libregister" }
 libcortex_a9 = { path = "../libcortex_a9" }
 libboard_zynq = { path = "../libboard_zynq" }
 [build-dependencies]
 cc = { version = "1.0" }
--- a/libsupport_zynq/build.rs
+++ b/libsupport_zynq/build.rs
@ -0,0 +1,24 @@
 fn main() {
    println!("cargo:rerun-if-changed=build.rs");
    compile_memcpy();
 }
 fn compile_memcpy() {
    use std::path::Path;
    extern crate cc;
    let cfg = &mut cc::Build::new();
    cfg.compiler("clang");
    cfg.no_default_flags(true);
    cfg.warnings(false);
    cfg.flag("--target=armv7-none-eabihf");
    let sources = vec![
        "memcpy.S",
    ];
    let root = Path::new("src/asm");
    for src in sources {
        println!("cargo:rerun-if-changed={}", src);
        cfg.file(root.join(src));
    }
    cfg.compile("memcpy");
 }
--- a/libsupport_zynq/src/asm/memcpy.S
+++ b/libsupport_zynq/src/asm/memcpy.S
@ -0,0 +1,626 @@
 /* Copyright (c) 2013, Linaro Limited
   All rights reserved.
   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions
   are met:
      * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
      * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
      * Neither the name of Linaro Limited nor the names of its
      contributors may be used to endorse or promote products derived
      from this software without specific prior written permission.
   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
   HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   This memcpy routine is optimised for Cortex-A15 cores and takes advantage
   of VFP or NEON when built with the appropriate flags.
   Assumptions:
    ARMv6 (ARMv7-a if using Neon)
    ARM state
    Unaligned accesses
    LDRD/STRD support unaligned word accesses
   If compiled with GCC, this file should be enclosed within following
   pre-processing check:
   if defined (__ARM_ARCH_7A__) && defined (__ARM_FEATURE_UNALIGNED)
 */
 	.syntax unified
 	/* This implementation requires ARM state.  */
 	.arm
 #ifdef __ARM_NEON__
 	.fpu	neon
 	.arch	armv7-a
 # define FRAME_SIZE	4
 # define USE_VFP
 # define USE_NEON
 #elif !defined (__SOFTFP__)
 	.arch	armv6
 	.fpu	vfpv2
 # define FRAME_SIZE	32
 # define USE_VFP
 #else
 	.arch	armv6
 # define FRAME_SIZE    32
 #endif
 /* Old versions of GAS incorrectly implement the NEON align semantics.  */
 #ifdef BROKEN_ASM_NEON_ALIGN
 #define ALIGN(addr, align) addr,:align
 #else
 #define ALIGN(addr, align) addr:align
 #endif
 #define PC_OFFSET	8	/* PC pipeline compensation.  */
 #define INSN_SIZE	4
 /* Call parameters.  */
 #define dstin	r0
 #define src	r1
 #define count	r2
 /* Locals.  */
 #define tmp1	r3
 #define dst	ip
 #define tmp2	r10
 #ifndef USE_NEON
 /* For bulk copies using GP registers.  */
 #define	A_l	r2		/* Call-clobbered.  */
 #define	A_h	r3		/* Call-clobbered.  */
 #define	B_l	r4
 #define	B_h	r5
 #define	C_l	r6
 #define	C_h	r7
 #define	D_l	r8
 #define	D_h	r9
 #endif
 /* Number of lines ahead to pre-fetch data.  If you change this the code
   below will need adjustment to compensate.  */
 #define prefetch_lines	5
 #ifdef USE_VFP
 	.macro	cpy_line_vfp vreg, base
 	vstr	\vreg, [dst, #\base]
 	vldr	\vreg, [src, #\base]
 	vstr	d0, [dst, #\base + 8]
 	vldr	d0, [src, #\base + 8]
 	vstr	d1, [dst, #\base + 16]
 	vldr	d1, [src, #\base + 16]
 	vstr	d2, [dst, #\base + 24]
 	vldr	d2, [src, #\base + 24]
 	vstr	\vreg, [dst, #\base + 32]
 	vldr	\vreg, [src, #\base + prefetch_lines * 64 - 32]
 	vstr	d0, [dst, #\base + 40]
 	vldr	d0, [src, #\base + 40]
 	vstr	d1, [dst, #\base + 48]
 	vldr	d1, [src, #\base + 48]
 	vstr	d2, [dst, #\base + 56]
 	vldr	d2, [src, #\base + 56]
 	.endm
 	.macro	cpy_tail_vfp vreg, base
 	vstr	\vreg, [dst, #\base]
 	vldr	\vreg, [src, #\base]
 	vstr	d0, [dst, #\base + 8]
 	vldr	d0, [src, #\base + 8]
 	vstr	d1, [dst, #\base + 16]
 	vldr	d1, [src, #\base + 16]
 	vstr	d2, [dst, #\base + 24]
 	vldr	d2, [src, #\base + 24]
 	vstr	\vreg, [dst, #\base + 32]
 	vstr	d0, [dst, #\base + 40]
 	vldr	d0, [src, #\base + 40]
 	vstr	d1, [dst, #\base + 48]
 	vldr	d1, [src, #\base + 48]
 	vstr	d2, [dst, #\base + 56]
 	vldr	d2, [src, #\base + 56]
 	.endm
 #endif
 	.macro def_fn f p2align=0
 	.text
 	.p2align \p2align
 	.global \f
 	.type \f, %function
 \f:
 	.endm
 	.global __aeabi_memcpy
 	.global __aeabi_memcpy4
 	.global __aeabi_memcpy8
 	.set __aeabi_memcpy, fast_memcpy
 	.set __aeabi_memcpy4, fast_memcpy
 	.set __aeabi_memcpy8, fast_memcpy
 def_fn fast_memcpy p2align=6
 	mov	dst, dstin	/* Preserve dstin, we need to return it.  */
 	cmp	count, #64
 	bge	.Lcpy_not_short
 	/* Deal with small copies quickly by dropping straight into the
 	   exit block.  */
 .Ltail63unaligned:
 #ifdef USE_NEON
 	and	tmp1, count, #0x38
 	rsb	tmp1, tmp1, #(56 - PC_OFFSET + INSN_SIZE)
 	add	pc, pc, tmp1
 	vld1.8	{d0}, [src]!	/* 14 words to go.  */
 	vst1.8	{d0}, [dst]!
 	vld1.8	{d0}, [src]!	/* 12 words to go.  */
 	vst1.8	{d0}, [dst]!
 	vld1.8	{d0}, [src]!	/* 10 words to go.  */
 	vst1.8	{d0}, [dst]!
 	vld1.8	{d0}, [src]!	/* 8 words to go.  */
 	vst1.8	{d0}, [dst]!
 	vld1.8	{d0}, [src]!	/* 6 words to go.  */
 	vst1.8	{d0}, [dst]!
 	vld1.8	{d0}, [src]!	/* 4 words to go.  */
 	vst1.8	{d0}, [dst]!
 	vld1.8	{d0}, [src]!	/* 2 words to go.  */
 	vst1.8	{d0}, [dst]!
 	tst	count, #4
 	ldrne	tmp1, [src], #4
 	strne	tmp1, [dst], #4
 #else
 	/* Copy up to 15 full words of data.  May not be aligned.  */
 	/* Cannot use VFP for unaligned data.  */
 	and	tmp1, count, #0x3c
 	add	dst, dst, tmp1
 	add	src, src, tmp1
 	rsb	tmp1, tmp1, #(60 - PC_OFFSET/2 + INSN_SIZE/2)
 	/* Jump directly into the sequence below at the correct offset.  */
 	add	pc, pc, tmp1, lsl #1
 	ldr	tmp1, [src, #-60]	/* 15 words to go.  */
 	str	tmp1, [dst, #-60]
 	ldr	tmp1, [src, #-56]	/* 14 words to go.  */
 	str	tmp1, [dst, #-56]
 	ldr	tmp1, [src, #-52]
 	str	tmp1, [dst, #-52]
 	ldr	tmp1, [src, #-48]	/* 12 words to go.  */
 	str	tmp1, [dst, #-48]
 	ldr	tmp1, [src, #-44]
 	str	tmp1, [dst, #-44]
 	ldr	tmp1, [src, #-40]	/* 10 words to go.  */
 	str	tmp1, [dst, #-40]
 	ldr	tmp1, [src, #-36]
 	str	tmp1, [dst, #-36]
 	ldr	tmp1, [src, #-32]	/* 8 words to go.  */
 	str	tmp1, [dst, #-32]
 	ldr	tmp1, [src, #-28]
 	str	tmp1, [dst, #-28]
 	ldr	tmp1, [src, #-24]	/* 6 words to go.  */
 	str	tmp1, [dst, #-24]
 	ldr	tmp1, [src, #-20]
 	str	tmp1, [dst, #-20]
 	ldr	tmp1, [src, #-16]	/* 4 words to go.  */
 	str	tmp1, [dst, #-16]
 	ldr	tmp1, [src, #-12]
 	str	tmp1, [dst, #-12]
 	ldr	tmp1, [src, #-8]	/* 2 words to go.  */
 	str	tmp1, [dst, #-8]
 	ldr	tmp1, [src, #-4]
 	str	tmp1, [dst, #-4]
 #endif
 	lsls	count, count, #31
 	ldrhcs	tmp1, [src], #2
 	ldrbne	src, [src]		/* Src is dead, use as a scratch.  */
 	strhcs	tmp1, [dst], #2
 	strbne	src, [dst]
 	bx	lr
 .Lcpy_not_short:
 	/* At least 64 bytes to copy, but don't know the alignment yet.  */
 	str	tmp2, [sp, #-FRAME_SIZE]!
 	and	tmp2, src, #7
 	and	tmp1, dst, #7
 	cmp	tmp1, tmp2
 	bne	.Lcpy_notaligned
 #ifdef USE_VFP
 	/* Magic dust alert!  Force VFP on Cortex-A9.  Experiments show
 	   that the FP pipeline is much better at streaming loads and
 	   stores.  This is outside the critical loop.  */
 	vmov.f32	s0, s0
 #endif
 	/* SRC and DST have the same mutual 32-bit alignment, but we may
 	   still need to pre-copy some bytes to get to natural alignment.
 	   We bring DST into full 64-bit alignment.  */
 	lsls	tmp2, dst, #29
 	beq	1f
 	rsbs	tmp2, tmp2, #0
 	sub	count, count, tmp2, lsr #29
 	ldrmi	tmp1, [src], #4
 	strmi	tmp1, [dst], #4
 	lsls	tmp2, tmp2, #2
 	ldrhcs	tmp1, [src], #2
 	ldrbne	tmp2, [src], #1
 	strhcs	tmp1, [dst], #2
 	strbne	tmp2, [dst], #1
 1:
 	subs	tmp2, count, #64	/* Use tmp2 for count.  */
 	blt	.Ltail63aligned
 	cmp	tmp2, #512
 	bge	.Lcpy_body_long
 .Lcpy_body_medium:			/* Count in tmp2.  */
 #ifdef USE_VFP
 1:
 	vldr	d0, [src, #0]
 	subs	tmp2, tmp2, #64
 	vldr	d1, [src, #8]
 	vstr	d0, [dst, #0]
 	vldr	d0, [src, #16]
 	vstr	d1, [dst, #8]
 	vldr	d1, [src, #24]
 	vstr	d0, [dst, #16]
 	vldr	d0, [src, #32]
 	vstr	d1, [dst, #24]
 	vldr	d1, [src, #40]
 	vstr	d0, [dst, #32]
 	vldr	d0, [src, #48]
 	vstr	d1, [dst, #40]
 	vldr	d1, [src, #56]
 	vstr	d0, [dst, #48]
 	add	src, src, #64
 	vstr	d1, [dst, #56]
 	add	dst, dst, #64
 	bge	1b
 	tst	tmp2, #0x3f
 	beq	.Ldone
 .Ltail63aligned:			/* Count in tmp2.  */
 	and	tmp1, tmp2, #0x38
 	add	dst, dst, tmp1
 	add	src, src, tmp1
 	rsb	tmp1, tmp1, #(56 - PC_OFFSET + INSN_SIZE)
 	add	pc, pc, tmp1
 	vldr	d0, [src, #-56]	/* 14 words to go.  */
 	vstr	d0, [dst, #-56]
 	vldr	d0, [src, #-48]	/* 12 words to go.  */
 	vstr	d0, [dst, #-48]
 	vldr	d0, [src, #-40]	/* 10 words to go.  */
 	vstr	d0, [dst, #-40]
 	vldr	d0, [src, #-32]	/* 8 words to go.  */
 	vstr	d0, [dst, #-32]
 	vldr	d0, [src, #-24]	/* 6 words to go.  */
 	vstr	d0, [dst, #-24]
 	vldr	d0, [src, #-16]	/* 4 words to go.  */
 	vstr	d0, [dst, #-16]
 	vldr	d0, [src, #-8]	/* 2 words to go.  */
 	vstr	d0, [dst, #-8]
 #else
 	sub	src, src, #8
 	sub	dst, dst, #8
 1:
 	ldrd	A_l, A_h, [src, #8]
 	strd	A_l, A_h, [dst, #8]
 	ldrd	A_l, A_h, [src, #16]
 	strd	A_l, A_h, [dst, #16]
 	ldrd	A_l, A_h, [src, #24]
 	strd	A_l, A_h, [dst, #24]
 	ldrd	A_l, A_h, [src, #32]
 	strd	A_l, A_h, [dst, #32]
 	ldrd	A_l, A_h, [src, #40]
 	strd	A_l, A_h, [dst, #40]
 	ldrd	A_l, A_h, [src, #48]
 	strd	A_l, A_h, [dst, #48]
 	ldrd	A_l, A_h, [src, #56]
 	strd	A_l, A_h, [dst, #56]
 	ldrd	A_l, A_h, [src, #64]!
 	strd	A_l, A_h, [dst, #64]!
 	subs	tmp2, tmp2, #64
 	bge	1b
 	tst	tmp2, #0x3f
 	bne	1f
 	ldr	tmp2,[sp], #FRAME_SIZE
 	bx	lr
 1:
 	add	src, src, #8
 	add	dst, dst, #8
 .Ltail63aligned:			/* Count in tmp2.  */
 	/* Copy up to 7 d-words of data.  Similar to Ltail63unaligned, but
 	   we know that the src and dest are 32-bit aligned so we can use
 	   LDRD/STRD to improve efficiency.  */
 	/* TMP2 is now negative, but we don't care about that.  The bottom
 	   six bits still tell us how many bytes are left to copy.  */
 	and	tmp1, tmp2, #0x38
 	add	dst, dst, tmp1
 	add	src, src, tmp1
 	rsb	tmp1, tmp1, #(56 - PC_OFFSET + INSN_SIZE)
 	add	pc, pc, tmp1
 	ldrd	A_l, A_h, [src, #-56]	/* 14 words to go.  */
 	strd	A_l, A_h, [dst, #-56]
 	ldrd	A_l, A_h, [src, #-48]	/* 12 words to go.  */
 	strd	A_l, A_h, [dst, #-48]
 	ldrd	A_l, A_h, [src, #-40]	/* 10 words to go.  */
 	strd	A_l, A_h, [dst, #-40]
 	ldrd	A_l, A_h, [src, #-32]	/* 8 words to go.  */
 	strd	A_l, A_h, [dst, #-32]
 	ldrd	A_l, A_h, [src, #-24]	/* 6 words to go.  */
 	strd	A_l, A_h, [dst, #-24]
 	ldrd	A_l, A_h, [src, #-16]	/* 4 words to go.  */
 	strd	A_l, A_h, [dst, #-16]
 	ldrd	A_l, A_h, [src, #-8]	/* 2 words to go.  */
 	strd	A_l, A_h, [dst, #-8]
 #endif
 	tst	tmp2, #4
 	ldrne	tmp1, [src], #4
 	strne	tmp1, [dst], #4
 	lsls	tmp2, tmp2, #31		/* Count (tmp2) now dead. */
 	ldrhcs	tmp1, [src], #2
 	ldrbne	tmp2, [src]
 	strhcs	tmp1, [dst], #2
 	strbne	tmp2, [dst]
 .Ldone:
 	ldr	tmp2, [sp], #FRAME_SIZE
 	bx	lr
 .Lcpy_body_long:			/* Count in tmp2.  */
 	/* Long copy.  We know that there's at least (prefetch_lines * 64)
 	   bytes to go.  */
 #ifdef USE_VFP
 	/* Don't use PLD.  Instead, read some data in advance of the current
 	   copy position into a register.  This should act like a PLD
 	   operation but we won't have to repeat the transfer.  */
 	vldr	d3, [src, #0]
 	vldr	d4, [src, #64]
 	vldr	d5, [src, #128]
 	vldr	d6, [src, #192]
 	vldr	d7, [src, #256]
 	vldr	d0, [src, #8]
 	vldr	d1, [src, #16]
 	vldr	d2, [src, #24]
 	add	src, src, #32
 	subs	tmp2, tmp2, #prefetch_lines * 64 * 2
 	blt	2f
 1:
 	cpy_line_vfp	d3, 0
 	cpy_line_vfp	d4, 64
 	cpy_line_vfp	d5, 128
 	add	dst, dst, #3 * 64
 	add	src, src, #3 * 64
 	cpy_line_vfp	d6, 0
 	cpy_line_vfp	d7, 64
 	add	dst, dst, #2 * 64
 	add	src, src, #2 * 64
 	subs	tmp2, tmp2, #prefetch_lines * 64
 	bge	1b
 2:
 	cpy_tail_vfp	d3, 0
 	cpy_tail_vfp	d4, 64
 	cpy_tail_vfp	d5, 128
 	add	src, src, #3 * 64
 	add	dst, dst, #3 * 64
 	cpy_tail_vfp	d6, 0
 	vstr	d7, [dst, #64]
 	vldr	d7, [src, #64]
 	vstr	d0, [dst, #64 + 8]
 	vldr	d0, [src, #64 + 8]
 	vstr	d1, [dst, #64 + 16]
 	vldr	d1, [src, #64 + 16]
 	vstr	d2, [dst, #64 + 24]
 	vldr	d2, [src, #64 + 24]
 	vstr	d7, [dst, #64 + 32]
 	add	src, src, #96
 	vstr	d0, [dst, #64 + 40]
 	vstr	d1, [dst, #64 + 48]
 	vstr	d2, [dst, #64 + 56]
 	add	dst, dst, #128
 	add	tmp2, tmp2, #prefetch_lines * 64
 	b	.Lcpy_body_medium
 #else
 	/* Long copy.  Use an SMS style loop to maximize the I/O
 	   bandwidth of the core.  We don't have enough spare registers
 	   to synthesise prefetching, so use PLD operations.  */
 	/* Pre-bias src and dst.  */
 	sub	src, src, #8
 	sub	dst, dst, #8
 	pld	[src, #8]
 	pld	[src, #72]
 	subs	tmp2, tmp2, #64
 	pld	[src, #136]
 	ldrd	A_l, A_h, [src, #8]
 	strd	B_l, B_h, [sp, #8]
 	ldrd	B_l, B_h, [src, #16]
 	strd	C_l, C_h, [sp, #16]
 	ldrd	C_l, C_h, [src, #24]
 	strd	D_l, D_h, [sp, #24]
 	pld	[src, #200]
 	ldrd	D_l, D_h, [src, #32]!
 	b	1f
 	.p2align	6
 2:
 	pld	[src, #232]
 	strd	A_l, A_h, [dst, #40]
 	ldrd	A_l, A_h, [src, #40]
 	strd	B_l, B_h, [dst, #48]
 	ldrd	B_l, B_h, [src, #48]
 	strd	C_l, C_h, [dst, #56]
 	ldrd	C_l, C_h, [src, #56]
 	strd	D_l, D_h, [dst, #64]!
 	ldrd	D_l, D_h, [src, #64]!
 	subs	tmp2, tmp2, #64
 1:
 	strd	A_l, A_h, [dst, #8]
 	ldrd	A_l, A_h, [src, #8]
 	strd	B_l, B_h, [dst, #16]
 	ldrd	B_l, B_h, [src, #16]
 	strd	C_l, C_h, [dst, #24]
 	ldrd	C_l, C_h, [src, #24]
 	strd	D_l, D_h, [dst, #32]
 	ldrd	D_l, D_h, [src, #32]
 	bcs	2b
 	/* Save the remaining bytes and restore the callee-saved regs.  */
 	strd	A_l, A_h, [dst, #40]
 	add	src, src, #40
 	strd	B_l, B_h, [dst, #48]
 	ldrd	B_l, B_h, [sp, #8]
 	strd	C_l, C_h, [dst, #56]
 	ldrd	C_l, C_h, [sp, #16]
 	strd	D_l, D_h, [dst, #64]
 	ldrd	D_l, D_h, [sp, #24]
 	add	dst, dst, #72
 	tst	tmp2, #0x3f
 	bne	.Ltail63aligned
 	ldr	tmp2, [sp], #FRAME_SIZE
 	bx	lr
 #endif
 .Lcpy_notaligned:
 	pld	[src]
 	pld	[src, #64]
 	/* There's at least 64 bytes to copy, but there is no mutual
 	   alignment.  */
 	/* Bring DST to 64-bit alignment.  */
 	lsls	tmp2, dst, #29
 	pld	[src, #(2 * 64)]
 	beq	1f
 	rsbs	tmp2, tmp2, #0
 	sub	count, count, tmp2, lsr #29
 	ldrmi	tmp1, [src], #4
 	strmi	tmp1, [dst], #4
 	lsls	tmp2, tmp2, #2
 	ldrbne	tmp1, [src], #1
 	ldrhcs	tmp2, [src], #2
 	strbne	tmp1, [dst], #1
 	strhcs	tmp2, [dst], #2
 1:
 	pld	[src, #(3 * 64)]
 	subs	count, count, #64
 	ldrmi	tmp2, [sp], #FRAME_SIZE
 	bmi	.Ltail63unaligned
 	pld	[src, #(4 * 64)]
 #ifdef USE_NEON
 	vld1.8	{d0-d3}, [src]!
 	vld1.8	{d4-d7}, [src]!
 	subs	count, count, #64
 	bmi	2f
 1:
 	pld	[src, #(4 * 64)]
 	vst1.8	{d0-d3}, [ALIGN (dst, 64)]!
 	vld1.8	{d0-d3}, [src]!
 	vst1.8	{d4-d7}, [ALIGN (dst, 64)]!
 	vld1.8	{d4-d7}, [src]!
 	subs	count, count, #64
 	bpl	1b
 2:
 	vst1.8	{d0-d3}, [ALIGN (dst, 64)]!
 	vst1.8	{d4-d7}, [ALIGN (dst, 64)]!
 	ands	count, count, #0x3f
 #else
 	/* Use an SMS style loop to maximize the I/O bandwidth.  */
 	sub	src, src, #4
 	sub	dst, dst, #8
 	subs	tmp2, count, #64	/* Use tmp2 for count.  */
 	ldr	A_l, [src, #4]
 	ldr	A_h, [src, #8]
 	strd	B_l, B_h, [sp, #8]
 	ldr	B_l, [src, #12]
 	ldr	B_h, [src, #16]
 	strd	C_l, C_h, [sp, #16]
 	ldr	C_l, [src, #20]
 	ldr	C_h, [src, #24]
 	strd	D_l, D_h, [sp, #24]
 	ldr	D_l, [src, #28]
 	ldr	D_h, [src, #32]!
 	b	1f
 	.p2align	6
 2:
 	pld	[src, #(5 * 64) - (32 - 4)]
 	strd	A_l, A_h, [dst, #40]
 	ldr	A_l, [src, #36]
 	ldr	A_h, [src, #40]
 	strd	B_l, B_h, [dst, #48]
 	ldr	B_l, [src, #44]
 	ldr	B_h, [src, #48]
 	strd	C_l, C_h, [dst, #56]
 	ldr	C_l, [src, #52]
 	ldr	C_h, [src, #56]
 	strd	D_l, D_h, [dst, #64]!
 	ldr	D_l, [src, #60]
 	ldr	D_h, [src, #64]!
 	subs	tmp2, tmp2, #64
 1:
 	strd	A_l, A_h, [dst, #8]
 	ldr	A_l, [src, #4]
 	ldr	A_h, [src, #8]
 	strd	B_l, B_h, [dst, #16]
 	ldr	B_l, [src, #12]
 	ldr	B_h, [src, #16]
 	strd	C_l, C_h, [dst, #24]
 	ldr	C_l, [src, #20]
 	ldr	C_h, [src, #24]
 	strd	D_l, D_h, [dst, #32]
 	ldr	D_l, [src, #28]
 	ldr	D_h, [src, #32]
 	bcs	2b
 	/* Save the remaining bytes and restore the callee-saved regs.  */
 	strd	A_l, A_h, [dst, #40]
 	add	src, src, #36
 	strd	B_l, B_h, [dst, #48]
 	ldrd	B_l, B_h, [sp, #8]
 	strd	C_l, C_h, [dst, #56]
 	ldrd	C_l, C_h, [sp, #16]
 	strd	D_l, D_h, [dst, #64]
 	ldrd	D_l, D_h, [sp, #24]
 	add	dst, dst, #72
 	ands	count, tmp2, #0x3f
 #endif
 	ldr	tmp2, [sp], #FRAME_SIZE
 	bne	.Ltail63unaligned
 	bx	lr
 	.size	memcpy, . - memcpy
--- a/libsupport_zynq/src/boot.rs
+++ b/libsupport_zynq/src/boot.rs
@ -0,0 +1,170 @@
 use r0::zero_bss;
 use core::ptr::write_volatile;
 use core::arch::asm;
 use libregister::{
    VolatileCell,
    RegisterR, RegisterRW,
 };
 use libcortex_a9::{asm, l2c, regs::*, cache, mmu, spin_lock_yield, notify_spin_lock, enable_fpu, interrupt_handler};
 use libboard_zynq::{slcr, mpcore};
 extern "C" {
    static mut __bss_start: u32;
    static mut __bss_end: u32;
    static mut __stack0_start: u32;
    static mut __stack1_start: u32;
    fn main_core0();
    fn main_core1();
 }
 static mut CORE1_ENABLED: VolatileCell<bool> = VolatileCell::new(false);
 interrupt_handler!(Reset, reset_irq, __stack0_start, __stack1_start, {
    // no need to setup stack here, as we already did when entering the handler
    match MPIDR.read().cpu_id() {
        0 => {
            boot_core0();
        }
        1 => {
            while !CORE1_ENABLED.get() {
                spin_lock_yield();
            }
            boot_core1();
        }
        _ => unreachable!(),
    }
 });
 #[inline(never)]
 unsafe extern "C" fn boot_core0() -> ! {
    l1_cache_init();
    enable_fpu();
    let mpcore = mpcore::RegisterBlock::mpcore();
    mpcore.scu_invalidate.invalidate_all_cores();
    zero_bss(&mut __bss_start, &mut __bss_end);
    let mmu_table = mmu::L1Table::get()
        .setup_flat_layout();
    mmu::with_mmu(mmu_table, || {
        mpcore.scu_control.start();
        ACTLR.enable_smp();
        ACTLR.enable_prefetch();
        // TODO: Barriers reqd when core1 is not yet starting?
        asm::dmb();
        asm::dsb();
        asm::enable_fiq();
        asm::enable_irq();
        main_core0();
        panic!("return from main");
    });
 }
 #[inline(never)]
 unsafe extern "C" fn boot_core1() -> ! {
    l1_cache_init();
    let mpcore = mpcore::RegisterBlock::mpcore();
    mpcore.scu_invalidate.invalidate_core1();
    let mmu_table = mmu::L1Table::get();
    mmu::with_mmu(mmu_table, || {
        ACTLR.enable_smp();
        ACTLR.enable_prefetch();
        // TODO: Barriers reqd when core1 is not yet starting?
        asm::dmb();
        asm::dsb();
        asm::enable_fiq();
        asm::enable_irq();
        main_core1();
        panic!("return from main_core1");
    });
 }
 fn l1_cache_init() {
    use libcortex_a9::cache::*;
    // Invalidate TLBs
    tlbiall();
    // Invalidate I-Cache
    iciallu();
    // Invalidate Branch Predictor Array
    bpiall();
    // Invalidate D-Cache
    //
    // NOTE: It is both faster and correct to only invalidate instead
    //       of also flush the cache (as was done before with
    //       `dccisw()`) and it is correct to perform this operation
    //       for all of the L1 data cache rather than a (previously
    //       unspecified) combination of one cache set and one cache
    //       way.
    dciall_l1();
 }
 pub struct Core1 {
 }
 impl Core1 {
    /// Reset and start core1
    pub fn start(sdram: bool) -> Self {
        // reset and stop (safe to repeat)
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(true));
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(true));
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
        });
        if sdram {
            // Cores always start from OCM no matter what you do.
            // Make up a vector table there that just jumps to SDRAM.
            for i in 0..8 {
                unsafe {
                    // this is the ARM instruction "b +0x00100000"
                    write_volatile((i*4) as *mut u32, 0xea03fffe);
                }
            }
        }
        unsafe {
            CORE1_ENABLED.set(true);
        }
        // Flush cache-line
        cache::dcc(unsafe { &CORE1_ENABLED });
        if sdram {
            cache::dccmvac(0);
            asm::dsb();
            l2c::l2_cache_clean(0);
            l2c::l2_cache_sync();
        }
        // wake up core1
        slcr::RegisterBlock::unlocked(|slcr| {
            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 {}
    }
    pub fn disable(&self) {
        unsafe {
            CORE1_ENABLED.set(false);
            cache::dccmvac(&CORE1_ENABLED  as *const _ as usize);
            asm::dsb();
        }
        self.restart();
    }
    pub fn restart(&self) {
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(true));
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(true));
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(false));
        });
    }
 }
--- a/libsupport_zynq/src/exception_vectors.rs
+++ b/libsupport_zynq/src/exception_vectors.rs
@ -0,0 +1,55 @@
 use libregister::{RegisterR, RegisterW};
 use libcortex_a9::{regs::{DFSR, MPIDR, VBAR}, interrupt_handler};
 use libboard_zynq::{println, stdio}; 
 use core::arch::asm;
 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();
    println!("UndefinedInstruction");
    loop {}
 });
 interrupt_handler!(SoftwareInterrupt, software_interrupt, __irq_stack0_start, __irq_stack1_start, {
    stdio::drop_uart();
    println!("SoftwareInterrupt");
    loop {}
 });
 interrupt_handler!(PrefetchAbort, prefetch_abort, __irq_stack0_start, __irq_stack1_start, {
    stdio::drop_uart();
    println!("PrefetchAbort");
    loop {}
 });
 interrupt_handler!(DataAbort, data_abort, __irq_stack0_start, __irq_stack1_start, {
    stdio::drop_uart();
    println!("DataAbort on core {}", MPIDR.read().cpu_id());
    println!("DFSR: {:03X}", DFSR.read());
    loop {}
 });
 interrupt_handler!(ReservedException, reserved_exception, __irq_stack0_start, __irq_stack1_start, {
    stdio::drop_uart();
    println!("ReservedException");
    loop {}
 });
 #[cfg(feature = "dummy_irq_handler")]
 interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
    stdio::drop_uart();
    println!("IRQ");
    loop {}
 });
 #[cfg(feature = "dummy_fiq_handler")]
 interrupt_handler!(FIQ, fiq, __irq_stack0_start, __irq_stack1_start, {
    stdio::drop_uart();
    println!("FIQ");
    loop {}
 });
--- a/libsupport_zynq/src/lib.rs
+++ b/libsupport_zynq/src/lib.rs
@ -0,0 +1,17 @@
 #![no_std]
 #![feature(alloc_error_handler)]
 #![feature(panic_info_message)]
 #![feature(naked_functions)]
 #![feature(global_asm)]
 #![feature(asm)]
 pub extern crate alloc;
 pub extern crate compiler_builtins;
 pub mod boot;
 pub mod exception_vectors;
 #[cfg(feature = "panic_handler")]
 mod panic;
 pub mod ram;
--- a/libsupport_zynq/src/panic.rs
+++ b/libsupport_zynq/src/panic.rs
@ -0,0 +1,24 @@
 use libboard_zynq::{print, println};
 #[cfg(feature = "target_kasli_soc")]
 use libboard_zynq::error_led::ErrorLED;
 #[panic_handler]
 fn panic(info: &core::panic::PanicInfo) -> ! {
    print!("panic at ");
    if let Some(location) = info.location() {
        print!("{}:{}:{}", location.file(), location.line(), location.column());
    } else {
        print!("unknown location");
    }
    if let Some(message) = info.message() {
        println!(": {}", message);
    } else {
        println!("");
    }
    #[cfg(feature = "target_kasli_soc")]
    {
        let mut err_led = ErrorLED::error_led();
        err_led.toggle(true);
    }
    loop {}
 }
--- a/libsupport_zynq/src/ram.rs
+++ b/libsupport_zynq/src/ram.rs
@ -0,0 +1,100 @@
 use alloc::alloc::Layout;
 use core::alloc::GlobalAlloc;
 use core::ptr::NonNull;
 use libcortex_a9::{
    mutex::Mutex,
    regs::MPIDR
 };
 use libregister::RegisterR;
 use linked_list_allocator::Heap;
 #[cfg(not(feature = "alloc_core"))]
 use libboard_zynq::ddr::DdrRam;
 #[global_allocator]
 static ALLOCATOR: CortexA9Alloc = CortexA9Alloc(
    Mutex::new(Heap::empty()),
    Mutex::new(Heap::empty()),
 );
 struct CortexA9Alloc(Mutex<Heap>, Mutex<Heap>);
 unsafe impl Sync for CortexA9Alloc {}
 unsafe impl GlobalAlloc for CortexA9Alloc {
    unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
        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) {
        if cfg!(not(feature = "alloc_core"))
            || ((&__heap0_start as *const usize as usize <= ptr as usize)
                && ((ptr as usize) < &__heap0_end as *const usize as usize))
        {
            &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()
            .init(ddr.ptr::<u8>() as usize, ddr.size());
    }
 }
 extern "C" {
    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_core0() {
    unsafe {
        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(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
    );
 }
--- a/link.x
+++ b/link.x
@ -1,73 +0,0 @@
 ENTRY(_boot_cores);
 /* Size of stack for core 0 in bytes */
 STACK_SIZE = 0x8000;
 /* Provide some defaults */
 PROVIDE(Reset = _boot_cores);
 PROVIDE(UndefinedInstruction = Reset);
 PROVIDE(SoftwareInterrupt = Reset);
 PROVIDE(PrefetchAbort = Reset);
 PROVIDE(DataAbort = Reset);
 PROVIDE(ReservedException = Reset);
 PROVIDE(IRQ = Reset);
 PROVIDE(FIQ = Reset);
 MEMORY
 {
  /* 256 kB On-Chip Memory */
  OCM : ORIGIN = 0, LENGTH = 0x30000
  OCM3 : ORIGIN = 0xFFFF0000, LENGTH = 0x10000
 }
 SECTIONS
 {
    .exceptions ORIGIN(OCM) :
    {
        KEEP(*(.text.exceptions));
    } > OCM
    .__fill (NOLOAD) : {
          . = ORIGIN(OCM) + 0x8000;
    } > OCM
    .text (ORIGIN(OCM) + 0x8000) :
    {
        *(.text.boot);
        *(.text .text.*);
    } > OCM
    .rodata : ALIGN(4)
    {
        *(.rodata .rodata.*);
    } > OCM
    .data : ALIGN(4)
    {
        *(.data .data.*);
    } > OCM
    .bss (NOLOAD) : ALIGN(0x4000)
    {
        /* Aligned to 16 kB */
        KEEP(*(.bss.l1_table));
        *(.bss .bss.*);
        . = ALIGN(4);
    } > OCM
    __bss_start = ADDR(.bss);
    __bss_end = ADDR(.bss) + SIZEOF(.bss);
    .stack (NOLOAD) : ALIGN(0x1000) {
      . += STACK_SIZE;
    } > OCM
    __stack_end = ADDR(.stack);
    __stack_start = ADDR(.stack) + SIZEOF(.stack) - 4;
  /DISCARD/ :
  {
    /* Unused exception related info that only wastes space */
    *(.ARM.exidx);
    *(.ARM.exidx.*);
    *(.ARM.extab.*);
  }
 }
--- a/openocd/common.cfg
+++ b/openocd/common.cfg
@ -0,0 +1,20 @@
 set XC7_JSHUTDOWN 0x0d
 set XC7_JPROGRAM 0x0b
 set XC7_JSTART 0x0c
 set XC7_BYPASS 0x3f
 proc xc7_program {tap} {
    global XC7_JSHUTDOWN XC7_JPROGRAM XC7_JSTART XC7_BYPASS
    irscan $tap $XC7_JSHUTDOWN
    irscan $tap $XC7_JPROGRAM
    runtest 60000
    #JSTART prevents this from working...
    #irscan $tap $XC7_JSTART
    runtest 2000
    irscan $tap $XC7_BYPASS
    runtest 2000
 }
 pld device virtex2 zynq.tap 1
 init
 xc7_program zynq.tap
--- a/openocd/cora-z7-10.cfg
+++ b/openocd/cora-z7-10.cfg
@ -1,41 +1,19 @@
 source [find interface/ftdi/digilent-hs1.cfg]
 adapter_khz 10000
-set PL_TAPID 0x03722093
+set PL_TAPID 0x13722093
 set SMP 1
 source ./zynq-7000.cfg
 source ./xilinx-tcl.cfg
 source ./ps7_init.tcl
 reset_config srst_only srst_push_pull
-set XC7_JSHUTDOWN 0x0d
+source ./common.cfg
 set XC7_JPROGRAM 0x0b
 set XC7_JSTART 0x0c
 set XC7_BYPASS 0x3f
-proc xc7_program {tap} {
+reset halt
    global XC7_JSHUTDOWN XC7_JPROGRAM XC7_JSTART XC7_BYPASS
    irscan $tap $XC7_JSHUTDOWN
    irscan $tap $XC7_JPROGRAM
    runtest 60000
    #JSTART prevents this from working...
    #irscan $tap $XC7_JSTART
    runtest 2000
    irscan $tap $XC7_BYPASS
    runtest 2000
 }
 pld device virtex2 zynq.tap 1
 init
 xc7_program zynq.tap
 xilinx_ps7_init
 # Disable MMU
 targets $_TARGETNAME_1
-arm mcr 15 0 1 0 0 [expr [arm mrc 15 0 1 0 0] & ~0xd]
+arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
 targets $_TARGETNAME_0
-arm mcr 15 0 1 0 0 [expr [arm mrc 15 0 1 0 0] & ~0xd]
+arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
--- a/openocd/digilent-hs2.cfg
+++ b/openocd/digilent-hs2.cfg
@ -0,0 +1,12 @@
 # this is the original file from OpenOCD, but with ftdi_device_desc
 # removed because some cables don't have it programmed.
 # this supports JTAG-HS2 (and apparently Nexys4 as well)
 adapter driver ftdi
 ftdi_vid_pid 0x0403 0x6014
 ftdi_channel 0
 ftdi_layout_init 0x00e8 0x60eb
 reset_config none
--- a/openocd/digilent_jtag_smt2_nc.cfg
+++ b/openocd/digilent_jtag_smt2_nc.cfg
@ -1,19 +0,0 @@
 #
 # Digilent JTAG-SMT2-NC
 #
 # http://store.digilentinc.com/jtag-smt2-nc-surface-mount-programming-module/
 # https://reference.digilentinc.com/_media/jtag_smt2nc/jtag-smt2-nc_rm.pdf
 #
 # Based on reference sheet (above) and Xilinx KCU105 schematics
 # https://www.xilinx.com/products/boards-and-kits/kcu105.html#documentation
 #
 # Note that the digilent_jtag_smt2 layout does not work and hangs while
 # the ftdi_device_desc from digilent_hs2 is wrong.
 interface ftdi
 ftdi_device_desc "Digilent USB Device"
 ftdi_vid_pid 0x0403 0x6014
 ftdi_channel 0
 ftdi_layout_init 0x00e8 0x60eb
 reset_config none
--- a/openocd/ebaz4205.cfg
+++ b/openocd/ebaz4205.cfg
@ -0,0 +1,33 @@
 # The contents of this file are partially dependend on
 # the adapter that you have. Please modify accordingly.
 adapter driver ftdi
 ftdi vid_pid 0x0403 0x6010
 ftdi channel 0
 # Every pin set as high impedance except TCK, TDI, TDO and TMS
 ftdi layout_init 0x0088 0x008b
 # nSRST defined on pin CN2-13 of the MiniModule (pin ADBUS5 [AD5] on the FT2232H chip)
 # This choice is arbitrary. Use other GPIO pin if desired.
 ftdi layout_signal nSRST -data 0x0020 -oe 0x0020
 transport select jtag
 adapter speed 10000
 set PL_TAPID 0x13722093
 set SMP 1
 source ./zynq-7000.cfg
 reset_config srst_only srst_open_drain
 adapter srst pulse_width 250
 adapter srst delay 400
 source ./common.cfg
 reset halt
 # Disable MMU
 targets $_TARGETNAME_1
 arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
 targets $_TARGETNAME_0
 arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
--- a/openocd/kasli_soc.cfg
+++ b/openocd/kasli_soc.cfg
@ -0,0 +1,28 @@
 adapter driver ftdi
 ftdi_device_desc "Quad RS232-HS"
 ftdi_vid_pid 0x0403 0x6011
 ftdi_channel 0
 # some GPIOs need to be set, otherwise the FTDI chip craps out for some reason.
 ftdi_layout_init 0x0098 0x008b
 transport select jtag
 adapter speed 1000
 set PL_TAPID 0x1372c093
 set SMP 1
 source ./zynq-7000.cfg
 ftdi_layout_signal nSRST -oe 0x0004
 reset_config srst_only srst_open_drain
 adapter srst pulse_width 250
 adapter srst delay 400
 source ./common.cfg
 reset halt
 # Disable MMU
 targets $_TARGETNAME_1
 arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
 targets $_TARGETNAME_0
 arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
--- a/Show More
+++ b/Show More
		`@ -0,0 +1,2 @@`
							`pub const M_AXI_GP0: usize = 0x4000_0000;`
							`pub const M_AXI_GP1: usize = 0x8000_0000;`
		`@ -0,0 +1,2 @@`
							`pub mod global;`
							`pub use global::GlobalTimer;`