WIP: Fix mutability

WIP: Fix return statement
WIP: fix borrow
2020-09-25 15:55:08 +08:00 · 2020-09-25 15:53:46 +08:00 · 2020-09-22 17:28:30 +08:00 · 2020-09-22 17:19:54 +08:00 · 2020-09-18 17:30:16 +08:00 · 2020-09-11 16:35:20 +08:00
122 changed files with 22258 additions and 5591 deletions
--- a/.cargo/config
+++ b/.cargo/config
@ -1,5 +1,4 @@
 [target.armv7-none-eabihf]
-runner = "./runner.sh"
 rustflags = [
  "-C", "link-arg=-Tlink.x",
  "-C", "target-feature=a9,armv7-a,neon",
--- a/Cargo.lock
+++ b/Cargo.lock
@ -2,71 +2,111 @@
 # It is not intended for manual editing.
 [[package]]
 name = "bit_field"
-version = "0.10.0"
+version = "0.10.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"

 [[package]]
 name = "bitflags"
-version = "1.1.0"
+version = "1.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"

 [[package]]
 name = "byteorder"
-version = "1.3.2"
+version = "1.3.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+
+[[package]]
+name = "cfg-if"
+version = "0.1.10"
 source = "registry+https://github.com/rust-lang/crates.io-index"

 [[package]]
 name = "compiler_builtins"
-version = "0.1.24"
+version = "0.1.35"
 source = "registry+https://github.com/rust-lang/crates.io-index"

+[[package]]
+name = "core_io"
+version = "0.1.20200410"
+dependencies = [
+ "memchr 2.3.3 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "embedded-hal"
+version = "0.2.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "nb 0.1.3 (registry+https://github.com/rust-lang/crates.io-index)",
+ "void 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
 [[package]]
 name = "experiments"
 version = "0.0.0"
 dependencies = [
- "libboard_zc706 0.0.0",
+ "embedded-hal 0.2.4 (registry+https://github.com/rust-lang/crates.io-index)",
+ "libasync 0.0.0",
 "libboard_zynq 0.0.0",
 "libcortex_a9 0.0.0",
 "libregister 0.0.0",
- "smoltcp 0.5.0 (git+https://github.com/m-labs/smoltcp.git?rev=8eb01aca364aefe5f823d68d552d62c76c9be4a3)",
+ "libsupport_zynq 0.0.0",
+ "log 0.4.11 (registry+https://github.com/rust-lang/crates.io-index)",
 ]

 [[package]]
-name = "libboard_zc706"
+name = "fatfs"
+version = "0.3.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "bitflags 1.2.1 (registry+https://github.com/rust-lang/crates.io-index)",
+ "byteorder 1.3.4 (registry+https://github.com/rust-lang/crates.io-index)",
+ "core_io 0.1.20200410",
+ "log 0.4.11 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "libasync"
 version = "0.0.0"
 dependencies = [
- "compiler_builtins 0.1.24 (registry+https://github.com/rust-lang/crates.io-index)",
- "libboard_zynq 0.0.0",
+ "embedded-hal 0.2.4 (registry+https://github.com/rust-lang/crates.io-index)",
 "libcortex_a9 0.0.0",
- "libregister 0.0.0",
- "linked_list_allocator 0.6.4 (registry+https://github.com/rust-lang/crates.io-index)",
- "r0 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)",
- "smoltcp 0.5.0 (git+https://github.com/m-labs/smoltcp.git?rev=8eb01aca364aefe5f823d68d552d62c76c9be4a3)",
+ "nb 0.1.3 (registry+https://github.com/rust-lang/crates.io-index)",
+ "pin-utils 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "smoltcp 0.6.0 (registry+https://github.com/rust-lang/crates.io-index)",
 ]

 [[package]]
 name = "libboard_zynq"
 version = "0.0.0"
 dependencies = [
- "bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "bit_field 0.10.1 (registry+https://github.com/rust-lang/crates.io-index)",
+ "embedded-hal 0.2.4 (registry+https://github.com/rust-lang/crates.io-index)",
 "libcortex_a9 0.0.0",
 "libregister 0.0.0",
- "linked_list_allocator 0.6.4 (registry+https://github.com/rust-lang/crates.io-index)",
- "r0 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)",
- "smoltcp 0.5.0 (git+https://github.com/m-labs/smoltcp.git?rev=8eb01aca364aefe5f823d68d552d62c76c9be4a3)",
- "vcell 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)",
+ "log 0.4.11 (registry+https://github.com/rust-lang/crates.io-index)",
+ "nb 0.1.3 (registry+https://github.com/rust-lang/crates.io-index)",
+ "smoltcp 0.6.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "void 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)",
 "volatile-register 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)",
 ]

+[[package]]
+name = "libconfig"
+version = "0.1.0"
+dependencies = [
+ "core_io 0.1.20200410",
+ "fatfs 0.3.4 (registry+https://github.com/rust-lang/crates.io-index)",
+ "libboard_zynq 0.0.0",
+ "log 0.4.11 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
 [[package]]
 name = "libcortex_a9"
 version = "0.0.0"
 dependencies = [
- "bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "bit_field 0.10.1 (registry+https://github.com/rust-lang/crates.io-index)",
 "libregister 0.0.0",
- "r0 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)",
- "smoltcp 0.5.0 (git+https://github.com/m-labs/smoltcp.git?rev=8eb01aca364aefe5f823d68d552d62c76c9be4a3)",
- "vcell 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)",
 "volatile-register 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)",
 ]

@ -74,34 +114,91 @@ dependencies = [
 name = "libregister"
 version = "0.0.0"
 dependencies = [
- "bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "bit_field 0.10.1 (registry+https://github.com/rust-lang/crates.io-index)",
 "vcell 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)",
 "volatile-register 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)",
 ]

+[[package]]
+name = "libsupport_zynq"
+version = "0.0.0"
+dependencies = [
+ "compiler_builtins 0.1.35 (registry+https://github.com/rust-lang/crates.io-index)",
+ "libboard_zynq 0.0.0",
+ "libcortex_a9 0.0.0",
+ "libregister 0.0.0",
+ "linked_list_allocator 0.8.5 (registry+https://github.com/rust-lang/crates.io-index)",
+ "r0 1.0.0 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
 [[package]]
 name = "linked_list_allocator"
-version = "0.6.4"
+version = "0.8.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"

+[[package]]
+name = "log"
+version = "0.4.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "cfg-if 0.1.10 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
 [[package]]
 name = "managed"
-version = "0.7.1"
+version = "0.7.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+
+[[package]]
+name = "memchr"
+version = "2.3.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+
+[[package]]
+name = "nb"
+version = "0.1.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "nb 1.0.0 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "nb"
+version = "1.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+
+[[package]]
+name = "pin-utils"
+version = "0.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"

 [[package]]
 name = "r0"
-version = "0.2.2"
+version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"

 [[package]]
 name = "smoltcp"
-version = "0.5.0"
-source = "git+https://github.com/m-labs/smoltcp.git?rev=8eb01aca364aefe5f823d68d552d62c76c9be4a3#8eb01aca364aefe5f823d68d552d62c76c9be4a3"
+version = "0.6.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
 dependencies = [
- "bitflags 1.1.0 (registry+https://github.com/rust-lang/crates.io-index)",
- "byteorder 1.3.2 (registry+https://github.com/rust-lang/crates.io-index)",
- "managed 0.7.1 (registry+https://github.com/rust-lang/crates.io-index)",
+ "bitflags 1.2.1 (registry+https://github.com/rust-lang/crates.io-index)",
+ "byteorder 1.3.4 (registry+https://github.com/rust-lang/crates.io-index)",
+ "managed 0.7.2 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "szl"
+version = "0.1.0"
+dependencies = [
+ "byteorder 1.3.4 (registry+https://github.com/rust-lang/crates.io-index)",
+ "core_io 0.1.20200410",
+ "libboard_zynq 0.0.0",
+ "libconfig 0.1.0",
+ "libcortex_a9 0.0.0",
+ "libregister 0.0.0",
+ "libsupport_zynq 0.0.0",
+ "log 0.4.11 (registry+https://github.com/rust-lang/crates.io-index)",
 ]

 [[package]]
@ -109,6 +206,11 @@ name = "vcell"
 version = "0.1.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"

+[[package]]
+name = "void"
+version = "1.0.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+
 [[package]]
 name = "volatile-register"
 version = "0.2.0"
@ -118,13 +220,22 @@ dependencies = [
 ]

 [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.24 (registry+https://github.com/rust-lang/crates.io-index)" = "b9975aefa63997ef75ca9cf013ff1bb81487aaa0b622c21053afd3b92979a7af"
-"checksum linked_list_allocator 0.6.4 (registry+https://github.com/rust-lang/crates.io-index)" = "47314ec1d29aa869ee7cb5a5be57be9b1055c56567d59c3fb6689926743e0bea"
-"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 (git+https://github.com/m-labs/smoltcp.git?rev=8eb01aca364aefe5f823d68d552d62c76c9be4a3)" = "<none>"
+"checksum bit_field 0.10.1 (registry+https://github.com/rust-lang/crates.io-index)" = "dcb6dd1c2376d2e096796e234a70e17e94cc2d5d54ff8ce42b28cef1d0d359a4"
+"checksum bitflags 1.2.1 (registry+https://github.com/rust-lang/crates.io-index)" = "cf1de2fe8c75bc145a2f577add951f8134889b4795d47466a54a5c846d691693"
+"checksum byteorder 1.3.4 (registry+https://github.com/rust-lang/crates.io-index)" = "08c48aae112d48ed9f069b33538ea9e3e90aa263cfa3d1c24309612b1f7472de"
+"checksum cfg-if 0.1.10 (registry+https://github.com/rust-lang/crates.io-index)" = "4785bdd1c96b2a846b2bd7cc02e86b6b3dbf14e7e53446c4f54c92a361040822"
+"checksum compiler_builtins 0.1.35 (registry+https://github.com/rust-lang/crates.io-index)" = "e3fcd8aba10d17504c87ef12d4f62ef404c6a4703d16682a9eb5543e6cf24455"
+"checksum embedded-hal 0.2.4 (registry+https://github.com/rust-lang/crates.io-index)" = "fa998ce59ec9765d15216393af37a58961ddcefb14c753b4816ba2191d865fcb"
+"checksum fatfs 0.3.4 (registry+https://github.com/rust-lang/crates.io-index)" = "93079df23039e52059e1f03b4c29fb0c72da2c792aad91bb2236c9fb81d3592e"
+"checksum linked_list_allocator 0.8.5 (registry+https://github.com/rust-lang/crates.io-index)" = "660b26e6156a7d00eefb19052fe1943cf5ab2f353a723a577fad6ba2f99d1f90"
+"checksum log 0.4.11 (registry+https://github.com/rust-lang/crates.io-index)" = "4fabed175da42fed1fa0746b0ea71f412aa9d35e76e95e59b192c64b9dc2bf8b"
+"checksum managed 0.7.2 (registry+https://github.com/rust-lang/crates.io-index)" = "c75de51135344a4f8ed3cfe2720dc27736f7711989703a0b43aadf3753c55577"
+"checksum memchr 2.3.3 (registry+https://github.com/rust-lang/crates.io-index)" = "3728d817d99e5ac407411fa471ff9800a778d88a24685968b36824eaf4bee400"
+"checksum nb 0.1.3 (registry+https://github.com/rust-lang/crates.io-index)" = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f"
+"checksum nb 1.0.0 (registry+https://github.com/rust-lang/crates.io-index)" = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae"
+"checksum pin-utils 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
+"checksum r0 1.0.0 (registry+https://github.com/rust-lang/crates.io-index)" = "bd7a31eed1591dcbc95d92ad7161908e72f4677f8fabf2a32ca49b4237cbf211"
+"checksum smoltcp 0.6.0 (registry+https://github.com/rust-lang/crates.io-index)" = "0fe46639fd2ec79eadf8fe719f237a7a0bd4dac5d957f1ca5bbdbc1c3c39e53a"
 "checksum vcell 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)" = "876e32dcadfe563a4289e994f7cb391197f362b6315dc45e8ba4aa6f564a4b3c"
+"checksum void 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)" = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d"
 "checksum volatile-register 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "0d67cb4616d99b940db1d6bd28844ff97108b498a6ca850e5b6191a532063286"
--- a/Cargo.toml
+++ b/Cargo.toml
@ -1,16 +1,24 @@
 [workspace]
 members = [
-  "libregister", "libcortex_a9",
-  "libboard_zynq", "libboard_zc706",
+  "libregister",
+  "libcortex_a9",
+  "libboard_zynq",
+  "libsupport_zynq",
+  "libasync",
+  "libconfig",
+  "libcoreio",
  "experiments",
+  "szl",
 ]

-[profile.dev]
-panic = "abort"
-lto = false
-
 [profile.release]
 panic = "abort"
 debug = true
-lto = true  # Link-Time Optimization
-opt-level = 'z'  # Optimize for size.
+codegen-units = 1
+opt-level = 'z'
+lto = true
+debug-assertions = false
+overflow-checks = false
+
+[patch.crates-io]
+core_io = { path = "./libcoreio" }
--- a/README.md
+++ b/README.md
@ -1,52 +1,22 @@
 # Build

 ```shell
-nix-shell --command "cargo xbuild --release"
+nix-shell --command "cargo xbuild --release -p experiments"
 ```

 Currently the ELF output is placed at `target/armv7-none-eabihf/release/experiments`

 # Debug

-## 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
-```
-
-Connect to target (given it is connected and you have permissions):
-```tcl
-connect arm hw
-```
-
-Leave xmd running.
-
-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
+## Running on the ZC706

 ```shell
-nix-shell --command "cargo xbuild --release"
+nix-shell --command "cargo xbuild --release -p experiments"
 cd openocd
 openocd -f zc706.cfg
 ```

-### Running on the Cora Z7-10
+## Running on the Cora Z7-10

 ```shell
 nix-shell --command "cd experiments && cargo xbuild --release --no-default-features --features=target_cora_z7_10"
@ -54,46 +24,8 @@ cd openocd
 openocd -f cora-z7-10.cfg
 ```

-### Loading a bitstream into volatile memory
+## Loading a bitstream into volatile memory

 ```shell
-  openocd -f zc706.cfg -c "pld load 0 blinker_migen.bit; exit"
+openocd -f zc706.cfg -c "pld load 0 blinker_migen.bit; exit"
 ```
-
-### Development Process
-
-Clone this repo onto your development/build machine and the raspberry pi that controls the Xilinx 7000 board
-
-On the dev machine, the below script builds zc706 and secure copies it to the target pi (in your pi $HOME directory)
-```shell
-cd ~/zc706
-./build.sh $your_user/ssh_id
-```
-
-On the pi, we need an information rich environment that includes a relatively reliable `gdb` experience (that includes `ctrl-p` and `ctrl-n` command history that persists across `cgdb` executions), run:
-```shell
-ssh pi4
-cd zc706
-./tmux.sh
-```
-
-Time to run your code with:
-```shell
-zynq-connect
-zynq-restart
-c
-```
-or, for a more succinct experience, (identical to above)
-```shell
-dc
-dr
-c
-```
-
-After every build on your dev machine, simply run:
-```shell
-dr
-c
-```
-Sometimes you might need to type `load` after `dr`.
-
--- a/build.sh
+++ b/build.sh
@ -1 +0,0 @@
-nix-shell --command "cargo xbuild --release" && scp -P 2204 -C target/armv7-none-eabihf/release/zc706-experiments $1@nixbld.m-labs.hk:/home/$1/zc706/zc706.elf
--- a/default.nix
+++ b/default.nix
@ -1,62 +1,38 @@
-{ # Use master branch of the overlay by default
-  mozillaOverlay ? import (builtins.fetchTarball https://github.com/mozilla/nixpkgs-mozilla/archive/master.tar.gz),
-  rustManifest ? ./channel-rust-nightly.toml,
-}:
-
 let
-  pkgs = import <nixpkgs> { overlays = [ mozillaOverlay ]; };
-  rustcSrc = pkgs.fetchgit {
-    url = https://github.com/rust-lang/rust.git;
-    # master of 2020-01-25
-    rev = "c2d141df59703393c0c683abc259f9a8c3be041a";
-    sha256 = "0v23ia4sp436yjksbq5m5vdarj481w2z8q3px51kidabdd2282yr";
-    fetchSubmodules = true;
-  };
-  targets = [];
-  rustChannelOfTargets = _channel: _date: targets:
-    (pkgs.lib.rustLib.fromManifestFile rustManifest {
-      inherit (pkgs) stdenv fetchurl patchelf;
-    }).rust.override { inherit targets; };
-  rust =
-    rustChannelOfTargets "nightly" null targets;
-  rustPlatform = pkgs.recurseIntoAttrs (pkgs.makeRustPlatform {
-    rustc = rust // { src = rustcSrc; };
-    cargo = rust;
-  });
-  gcc = pkgs.pkgsCross.armv7l-hf-multiplatform.buildPackages.gcc;
-  xbuildRustPackage = { cargoFeatures, crateSubdir, ... } @ attrs:
-    let
-      buildPkg = rustPlatform.buildRustPackage attrs;
-    in
-    buildPkg.overrideAttrs ({ name, nativeBuildInputs, ... }: {
-      nativeBuildInputs =
-        nativeBuildInputs ++ [ pkgs.cargo-xbuild ];
+  pkgs = import <nixpkgs> { overlays = [ (import ./nix/mozilla-overlay.nix) ]; };
+  rustPlatform = (import ./nix/rust-platform.nix { inherit pkgs; });
+  build-crate = name: crate: features:
+    rustPlatform.buildRustPackage rec {
+      name = "${crate}";
+
+      src = ./.;
+      cargoSha256 = "1f2psa1g41pl2j8n60hhik2s2pqdfjhr5capimvajf81kxrnn2ck";
+
+      nativeBuildInputs = [ pkgs.cargo-xbuild ];
      buildPhase = ''
-        pushd ${crateSubdir}
+        export XARGO_RUST_SRC="${rustPlatform.rust.rustc.src}/src"
+        export CARGO_HOME=$(mktemp -d cargo-home.XXX)
+        pushd ${crate}
        cargo xbuild --release --frozen \
          --no-default-features \
-          --features=${cargoFeatures}
+          --features=${features}
        popd
      '';
-      XARGO_RUST_SRC = "${rustcSrc}/src";
+
      installPhase = ''
-        mkdir $out
-        ls -la target/armv7-none-eabihf/release/
+        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
      '';
-    });
-  xbuildCrate = name: crate: features: xbuildRustPackage rec {
-    name = "${crate}";
-    src = ./.;
-    crateSubdir = crate;
-    cargoSha256 = "1wvj585vylbjlab7cxbkr4f60km5y7s8knxxvcixmqywdldnh7g2";
-    cargoFeatures = features;
-    doCheck = false;
-  };
-in {
-  inherit pkgs rustPlatform rustcSrc gcc;
-  zc706 = {
-    experiments-zc706 = xbuildCrate "experiments-zc706" "experiments" "target_zc706";
-    experiments-cora = xbuildCrate "experiments-cora" "experiments" "target_cora_z7_10";
-  };
-}
+
+      doCheck = false;
+      dontFixup = true;
+    };
+in
+  {
+    zc706-experiments = build-crate "zc706-experiments" "experiments" "target_zc706";
+    cora-experiments = build-crate "cora-experiments" "experiments" "target_cora_z7_10";
+    redpitaya-experiments = build-crate "redpitaya-experiments" "experiments" "target_redpitaya";
+    zc706-fsbl = (import ./nix/fsbl.nix { inherit pkgs; });
+    zc706-szl = build-crate "zc706-szl" "szl" "target_zc706";
+  }
--- a/experiments/Cargo.toml
+++ b/experiments/Cargo.toml
@ -2,22 +2,20 @@
 name = "experiments"
 description = "Developing bare-metal Rust on Zynq"
 version = "0.0.0"
-authors = ["Astro <astro@spaceboyz.net>"]
+authors = ["M-Labs"]
 edition = "2018"

 [features]
-target_zc706 = ["libboard_zynq/target_zc706", "libboard_zc706/target_zc706"]
-target_cora_z7_10 = ["libboard_zynq/target_cora_z7_10", "libboard_zc706/target_cora_z7_10"]
+target_zc706 = ["libboard_zynq/target_zc706", "libsupport_zynq/target_zc706"]
+target_cora_z7_10 = ["libboard_zynq/target_cora_z7_10", "libsupport_zynq/target_cora_z7_10"]
+target_redpitaya = ["libboard_zynq/target_redpitaya", "libsupport_zynq/target_redpitaya"]
 default = ["target_zc706"]

 [dependencies]
+log = "0.4"
+embedded-hal = "0.2"
 libregister = { path = "../libregister" }
 libcortex_a9 = { path = "../libcortex_a9" }
 libboard_zynq = { path = "../libboard_zynq" }
-libboard_zc706 = { path = "../libboard_zc706" }
-
-[dependencies.smoltcp]
-git = "https://github.com/m-labs/smoltcp.git"
-rev = "8eb01aca364aefe5f823d68d552d62c76c9be4a3"
-features = ["ethernet", "proto-ipv4", "socket-tcp"]
-default-features = false
+libsupport_zynq = { path = "../libsupport_zynq", default-features = false, features = ["panic_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,62 @@
+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
+
+    .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
@ -1,220 +1,306 @@
 #![no_std]
 #![no_main]
+#![feature(const_in_array_repeat_expressions)]
+#![feature(naked_functions)]

-use core::mem::transmute;
-use libcortex_a9::mutex::Mutex;
-use libboard_zynq::{print, println, self as zynq, clocks::Clocks, clocks::source::{ClockSource, ArmPll, IoPll}};
-use libboard_zc706::{
-    ram, alloc::{vec, vec::Vec},
-    boot,
-    smoltcp::wire::{EthernetAddress, IpAddress, IpCidr},
-    smoltcp::iface::{NeighborCache, EthernetInterfaceBuilder},
-    smoltcp::time::Instant,
-    smoltcp::socket::SocketSet,
-    smoltcp::socket::{TcpSocket, TcpSocketBuffer},
+extern crate alloc;
+
+use alloc::collections::BTreeMap;
+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
+};
+use libregister::{RegisterR, RegisterW};
+use libsupport_zynq::{
+    boot, ram,
+};
+use log::{info, warn};
+use core::sync::atomic::{AtomicBool, Ordering};

 const HWADDR: [u8; 6] = [0, 0x23, 0xde, 0xea, 0xbe, 0xef];

-static mut STACK_CORE1: [u32; 512] = [0; 512];
+static mut CORE1_REQ: (Sender<usize>, Receiver<usize>) = sync_channel!(usize, 10);
+static mut CORE1_RES: (Sender<usize>, Receiver<usize>) = sync_channel!(usize, 10);
+
+extern "C" {
+    static mut __stack1_start: u32;
+}
+
+static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+pub unsafe extern "C" fn IRQ() {
+    if MPIDR.read().cpu_id() == 1{
+        let mpcore = mpcore::RegisterBlock::mpcore();
+        let mut gic = gic::InterruptController::gic(mpcore);
+        let id = gic.get_interrupt_id();
+        if id.0 == 0 {
+            gic.end_interrupt(id);
+            asm::exit_irq();
+            SP.write(&mut __stack1_start as *mut _ as u32);
+            asm::enable_irq();
+            CORE1_RESTART.store(false, Ordering::Relaxed);
+            notify_spin_lock();
+            main_core1();
+        }
+    }
+    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() {
    // zynq::clocks::CpuClocks::enable_io(1_250_000_000);
-    println!("\nzc706 main");
-    {
-        use libregister::RegisterR;
-        println!("Boot mode: {:?}", zynq::slcr::RegisterBlock::new().boot_mode.read().boot_mode_pins());
-    }
+    enable_l2_cache();
+    println!("\nZynq experiments");
+    let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());
+    interrupt_controller.enable_interrupts();
+    // ps7_init::apply();
+    libboard_zynq::stdio::drop_uart();
+
+    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(feature = "target_zc706")]
    const CPU_FREQ: u32 = 800_000_000;
    #[cfg(feature = "target_cora_z7_10")]
    const CPU_FREQ: u32 = 650_000_000;
+    #[cfg(feature = "target_redpitaya")]
+    const CPU_FREQ: u32 = 800_000_000;

-    println!("Setup clock sources...");
+    info!("Setup clock sources...");
    ArmPll::setup(2 * CPU_FREQ);
    Clocks::set_cpu_freq(CPU_FREQ);
-    IoPll::setup(700_000_000);
-    libboard_zynq::stdio::drop_uart();
-    println!("PLLs set up");
-    let clocks = zynq::clocks::Clocks::get();
-    println!("CPU Clocks: {}/{}/{}/{}", clocks.cpu_6x4x(), clocks.cpu_3x2x(), clocks.cpu_2x(), clocks.cpu_1x());
-
-    let mut flash = zynq::flash::Flash::new(200_000_000).linear_addressing_mode();
-    let flash_ram: &[u8] = unsafe { core::slice::from_raw_parts(flash.ptr(), flash.size()) };
-    for i in 0..=1 {
-        print!("Flash {}:", i);
-        for b in &flash_ram[(i * 16 * 1024 * 1024)..][..128] {
-            print!(" {:02X}", *b);
-        }
-        println!("");
+    #[cfg(feature = "target_zc706")]
+    {
+        IoPll::setup(1_000_000_000);
+        libboard_zynq::stdio::drop_uart();
    }
-    let mut flash = flash.stop();
+    #[cfg(feature = "target_cora_z7_10")]
+    {
+        IoPll::setup(1_000_000_000);
+        libboard_zynq::stdio::drop_uart();
+    }
+    #[cfg(feature = "target_redpitaya")]
+    {
+        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 mut ddr = zynq::ddr::DdrRam::new();
+    let timer = libboard_zynq::timer::GlobalTimer::start();
+
+    let mut ddr = zynq::ddr::DdrRam::ddrram();
    #[cfg(not(feature = "target_zc706"))]
    ddr.memtest();
-    ram::init_alloc(&mut ddr);
+    ram::init_alloc_ddr(&mut ddr);

-    for i in 0..=1 {
-        let mut flash_io = flash.manual_mode(i);
-        // println!("rdcr={:02X}", flash_io.rdcr());
-        print!("Flash {} ID:", i);
-        for b in flash_io.rdid() {
-            print!(" {:02X}", b);
+    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!("");
-        print!("Flash {} I/O:", i);
-        for o in 0..8 {
-            const CHUNK: u32 = 8;
-            for b in flash_io.read(CHUNK * o, CHUNK as usize) {
-                print!(" {:02X}", b);
-            }
+        // 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!("");
-
-        flash_io.dump("Read cr1", 0x35);
-        flash_io.dump("Read Autoboot", 0x14);
-        flash_io.dump("Read Bank", 0x16);
-        flash_io.dump("DLP Bank", 0x16);
-        flash_io.dump("Read ESig", 0xAB);
-        flash_io.dump("OTP Read", 0x4B);
-        flash_io.dump("DYB Read", 0xE0);
-        flash_io.dump("PPB Read", 0xE2);
-        flash_io.dump("ASP Read", 0x2B);
-        flash_io.dump("Password Read", 0xE7);
-
-        flash_io.write_enabled(|flash_io| {
-            flash_io.erase(0);
-        });
-        flash_io.write_enabled(|flash_io| {
-            flash_io.program(0, [0x23054223; (0x100 >> 2)].iter().cloned());
-        });
-
-        flash = flash_io.stop();
    }

-    let core1_stack = unsafe { &mut STACK_CORE1[..] };
-    println!("{} bytes stack for core1", core1_stack.len());
-    let core1 = boot::Core1::start(core1_stack);
-
-    for _ in 0..0x1000000 {
-        let mut l = SHARED.lock();
-        *l += 1;
-    }
-    while !*DONE.lock() {
-        let x = { *SHARED.lock() };
-        println!("shared: {:08X}", x);
-    }
-    let x = { *SHARED.lock() };
-    println!("done shared: {:08X}", x);
-
-    core1.reset();
-
-    libcortex_a9::asm::dsb();
-    print!("Core1 stack [{:08X}..{:08X}]:", &core1.stack[0] as *const _ as u32, &core1.stack[core1.stack.len() - 1] as *const _ as u32);
-    for w in core1.stack {
-        print!(" {:08X}", w);
-    }
-    println!(".");
-
-    let eth = zynq::eth::Eth::default(HWADDR.clone());
+    let eth = zynq::eth::Eth::eth0(HWADDR.clone());
    println!("Eth on");

-    const RX_LEN: usize = 8;
-    let mut rx_descs = (0..RX_LEN)
-        .map(|_| zynq::eth::rx::DescEntry::zeroed())
-        .collect::<Vec<_>>();
-    let mut rx_buffers = vec![[0u8; zynq::eth::MTU]; RX_LEN];
+    const RX_LEN: usize = 4096;
    // Number of transmission buffers (minimum is two because with
    // one, duplicate packet transmission occurs)
-    const TX_LEN: usize = 8;
-    let mut tx_descs = (0..TX_LEN)
-        .map(|_| zynq::eth::tx::DescEntry::zeroed())
-        .collect::<Vec<_>>();
-    let mut tx_buffers = vec![[0u8; zynq::eth::MTU]; TX_LEN];
-    let eth = eth.start_rx(&mut rx_descs, &mut rx_buffers);
-    //let mut eth = eth.start_tx(&mut tx_descs, &mut tx_buffers);
-    let mut eth = eth.start_tx(
-        // HACK
-        unsafe { transmute(tx_descs.as_mut_slice()) },
-        unsafe { transmute(tx_buffers.as_mut_slice()) },
-    );
+    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 mut neighbor_storage = vec![None; 256];
-    let neighbor_cache = NeighborCache::new(&mut neighbor_storage[..]);
+    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();
-    let mut sockets_storage = [
-        None, None, None, None,
-        None, None, None, None
-    ];
-    let mut sockets = SocketSet::new(&mut sockets_storage[..]);

-    // taken from example code for smoltcp
-    let mut tcp_server_rx_data = vec![0; 512 * 1024];
-    let mut tcp_server_tx_data = vec![0; 512 * 1024];
-    let tcp_rx_buffer = TcpSocketBuffer::new(&mut tcp_server_rx_data[..]);
-    let tcp_tx_buffer = TcpSocketBuffer::new(&mut tcp_server_tx_data[..]);
-    let tcp_socket = TcpSocket::new(tcp_rx_buffer, tcp_tx_buffer);
-    let tcp_handle = sockets.add(tcp_socket);
-    /// `chargen`
+    Sockets::init(32);
+
    const TCP_PORT: u16 = 19;
-
-    let mut time = 0u32;
-    loop {
-        time += 1;
-        let timestamp = Instant::from_millis(time);
-
-        match iface.poll(&mut sockets, timestamp) {
-            Ok(_) => {},
-            Err(e) => {
-                println!("poll error: {}", e);
-            }
+    // (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).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
+                        }
+                    }
+                }
+            });
        }
-
-        // (mostly) taken from smoltcp example: TCP echo server
-        let mut socket = sockets.get::<TcpSocket>(tcp_handle);
-        if !socket.is_open() {
-            socket.listen(TCP_PORT).unwrap()
+    });
+    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
+                        }
+                    }
+                }
+            });
        }
-        if socket.may_recv() && socket.can_send() {
-            socket.recv(|buf| {
-                let len = buf.len().min(4096);
-                let buffer = buf[..len].iter().cloned().collect::<Vec<_>>();
-                (len, buffer)
-            })
-                .and_then(|buffer| socket.send_slice(&buffer[..]))
-                .map(|_| {})
-                .unwrap_or_else(|e| println!("tcp: {:?}", e));
+    });

+    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);
        }
-    }
+    });

-    // #[allow(unreachable_code)]
-    // drop(tx_descs);
-    // #[allow(unreachable_code)]
-    // drop(tx_buffers);
+    Sockets::run(&mut iface, || {
+        Instant::from_millis(timer.get_time().0 as i64)
+    })
 }

-static SHARED: Mutex<u32> = Mutex::new(0);
 static DONE: Mutex<bool> = Mutex::new(false);

 #[no_mangle]
 pub fn main_core1() {
    println!("Hello from core1!");
-    for _ in 0..0x1000000 {
-        let mut l = SHARED.lock();
-        *l += 1;
+    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;

--- 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 = "0.1"
+libcortex_a9 = { path = "../libcortex_a9" }
+
+[dependencies.smoltcp]
+version = "0.6"
+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,94 @@
+use core::{
+    cell::RefCell,
+    task::Waker,
+};
+use alloc::vec::Vec;
+use smoltcp::{
+    iface::EthernetInterface,
+    phy::Device,
+    socket::SocketSet,
+    time::Instant,
+};
+use crate::task;
+
+mod tcp_stream;
+pub use tcp_stream::TcpStream;
+
+static mut SOCKETS: Option<Sockets> = None;
+
+pub struct Sockets {
+    sockets: RefCell<SocketSet<'static, 'static, '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, 'c, 'e, D: for<'d> Device<'d>>(
+        iface: &mut EthernetInterface<'b, 'c, 'e, D>,
+        mut get_time: impl FnMut() -> Instant,
+    ) -> ! {
+        task::block_on(async {
+            loop {
+                let instant = get_time();
+                Self::instance().poll(iface, instant);
+                task::r#yield().await;
+            }
+        })
+    }
+
+    pub(crate) fn instance() -> &'static Self {
+        unsafe { SOCKETS.as_ref().expect("Sockets") }
+    }
+
+    fn poll<'b, 'c, 'e, D: for<'d> Device<'d>>(
+        &self,
+        iface: &mut EthernetInterface<'b, 'c, 'e, 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,283 @@
+//! 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));
+    }
+}
+
+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_zc706/Cargo.toml
+++ b/libboard_zc706/Cargo.toml
@ -1,24 +0,0 @@
-[package]
-name = "libboard_zc706"
-description = "Software support for running on devboards"
-version = "0.0.0"
-authors = ["Astro <astro@spaceboyz.net>"]
-edition = "2018"
-
-[features]
-target_zc706 = ["libboard_zynq/target_zc706"]
-target_cora_z7_10 = ["libboard_zynq/target_cora_z7_10"]
-
-[dependencies]
-r0 = "0.2"
-compiler_builtins = "0.1"
-linked_list_allocator = { version = "0.6", default-features = false }
-libregister = { path = "../libregister" }
-libcortex_a9 = { path = "../libcortex_a9" }
-libboard_zynq = { path = "../libboard_zynq" }
-
-[dependencies.smoltcp]
-git = "https://github.com/m-labs/smoltcp.git"
-rev = "8eb01aca364aefe5f823d68d552d62c76c9be4a3"
-features = ["ethernet", "proto-ipv4", "socket-tcp"]
-default-features = false
--- a/libboard_zc706/src/abort.rs
+++ b/libboard_zc706/src/abort.rs
@ -1,21 +0,0 @@
-use libboard_zynq::{println, slcr, stdio};
-
-#[no_mangle]
-pub unsafe extern "C" fn PrefetchAbort() {
-    stdio::drop_uart();
-
-    println!("PrefetchAbort");
-
-    slcr::RegisterBlock::unlocked(|slcr| slcr.soft_reset());
-    loop {}
-}
-
-#[no_mangle]
-pub unsafe extern "C" fn DataAbort() {
-    stdio::drop_uart();
-
-    println!("DataAbort");
-
-    slcr::RegisterBlock::unlocked(|slcr| slcr.soft_reset());
-    loop {}
-}
--- a/libboard_zc706/src/ram.rs
+++ b/libboard_zc706/src/ram.rs
@ -1,41 +0,0 @@
-use core::alloc::GlobalAlloc;
-use core::ptr::NonNull;
-use alloc::alloc::Layout;
-use linked_list_allocator::Heap;
-use libcortex_a9::mutex::Mutex;
-use libboard_zynq::ddr::DdrRam;
-
-#[global_allocator]
-static ALLOCATOR: CortexA9Alloc = CortexA9Alloc(Mutex::new(Heap::empty()));
-
-/// LockedHeap doesn't locking properly
-struct CortexA9Alloc(Mutex<Heap>);
-
-unsafe impl Sync for CortexA9Alloc {}
-
-unsafe impl GlobalAlloc for CortexA9Alloc {
-    unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
-        self.0.lock()
-            .allocate_first_fit(layout)
-            .ok()
-            .map_or(0 as *mut u8, |allocation| allocation.as_ptr())
-    }
-
-    unsafe fn dealloc(&self, ptr: *mut u8, layout: Layout) {
-        self.0.lock()
-            .deallocate(NonNull::new_unchecked(ptr), layout)
-    }
-}
-
-pub fn init_alloc(ddr: &mut DdrRam) {
-    unsafe {
-        ALLOCATOR.0.lock()
-            .init(ddr.ptr::<u8>() as usize, ddr.size());
-    }
-}
-
-
-#[alloc_error_handler]
-fn alloc_error(_: core::alloc::Layout) -> ! {
-    panic!("alloc_error")
-}
--- a/libboard_zynq/Cargo.toml
+++ b/libboard_zynq/Cargo.toml
@ -2,24 +2,26 @@
 name = "libboard_zynq"
 description = "Drivers for peripherals in the Zynq PS"
 version = "0.0.0"
-authors = ["Astro <astro@spaceboyz.net>"]
+authors = ["M-Labs"]
 edition = "2018"

 [features]
 target_zc706 = []
 target_cora_z7_10 = []
+target_redpitaya = []
+ipv6 = [ "smoltcp/proto-ipv6" ]

 [dependencies]
-r0 = "0.2"
-vcell = "0.1"
 volatile-register = "0.2"
 bit_field = "0.10"
-linked_list_allocator = { version = "0.6", default-features = false }
+embedded-hal = "0.2"
+nb = "0.1"
+void = { version = "1", default-features = false }
+log = "0.4"
 libregister = { path = "../libregister" }
 libcortex_a9 = { path = "../libcortex_a9" }

 [dependencies.smoltcp]
-git = "https://github.com/m-labs/smoltcp.git"
-rev = "8eb01aca364aefe5f823d68d552d62c76c9be4a3"
+version = "0.6"
 features = ["ethernet", "proto-ipv4", "socket-tcp"]
 default-features = false
--- a/libboard_zynq/src/clocks/mod.rs
+++ b/libboard_zynq/src/clocks/mod.rs
@ -1,4 +1,4 @@
-use libregister::{RegisterR, RegisterW, RegisterRW};
+use libregister::{RegisterR, RegisterRW};
 use super::slcr;
 pub use slcr::ArmPllSource;

@ -14,7 +14,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 {
@ -59,7 +59,7 @@ impl Clocks {
    }

    pub fn cpu_6x4x(&self) -> u32 {
-        let slcr = slcr::RegisterBlock::new();
+        let slcr = slcr::RegisterBlock::slcr();
        let arm_clk_ctrl = slcr.arm_clk_ctrl.read();
        let pll = match arm_clk_ctrl.srcsel() {
            ArmPllSource::ArmPll => self.arm,
@ -92,7 +92,7 @@ impl Clocks {
    }

    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 =>
@ -104,4 +104,18 @@ impl Clocks {
        };
        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,
+        };
+        pll / u32::from(sdio_clk_ctrl.divisor())
+    }
 }
--- a/libboard_zynq/src/clocks/source.rs
+++ b/libboard_zynq/src/clocks/source.rs
@ -1,3 +1,4 @@
+use log::debug;
 use libregister::{RegisterR, RegisterW, RegisterRW};
 use super::slcr;

@ -5,6 +6,8 @@ use super::slcr;
 pub const PS_CLK: u32 = 33_333_333;
 #[cfg(feature = "target_cora_z7_10")]
 pub const PS_CLK: u32 = 50_000_000;
+#[cfg(feature = "target_redpitaya")]
+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))] = &[
@ -43,11 +46,13 @@ pub trait ClockSource {

    /// get configured frequency
    fn freq() -> u32 {
-        let mut slcr = slcr::RegisterBlock::new();
+        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) {
@ -58,6 +63,7 @@ pub trait ClockSource {
            .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);

@ -108,6 +114,10 @@ impl ClockSource for ArmPll {
    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
@ -130,6 +140,10 @@ impl ClockSource for DdrPll {
    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
@ -153,4 +167,8 @@ impl ClockSource for IoPll {
    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
@ -1,6 +1,7 @@
 use libregister::{RegisterR, RegisterW, RegisterRW};
+use log::{debug, info, error};
 use crate::{print, println};
-use super::slcr;
+use super::slcr::{self, DdriobVrefSel};
 use super::clocks::{Clocks, source::{DdrPll, ClockSource}};

 mod regs;
@ -13,6 +14,10 @@ const DDR_FREQ: u32 = 666_666_666;
 /// Micron MT41K256M16HA-125: 800 MHz DDR3L, max supported 533 MHz
 const DDR_FREQ: u32 = 525_000_000;

+#[cfg(feature = "target_redpitaya")]
+/// Alliance Memory AS4C256M16D3B: 800 MHz DDR3
+const DDR_FREQ: u32 = 800_000_000;
+
 /// MT41K256M16HA-125
 const DCI_FREQ: u32 = 10_000_000;

@ -21,13 +26,14 @@ pub struct DdrRam {
 }

 impl DdrRam {
-    pub fn new() -> Self {
+    pub fn ddrram() -> Self {
        let clocks = Self::clock_setup();
        Self::calibrate_iob_impedance(&clocks);
        Self::configure_iob();

-        let regs = unsafe { regs::RegisterBlock::new() };
+        let regs = regs::RegisterBlock::ddrc();
        let mut ddr = DdrRam { regs };
+        ddr.configure();
        ddr.reset_ddrc();
        ddr
    }
@ -38,11 +44,9 @@ impl DdrRam {
        DdrPll::setup(2 * DDR_FREQ);

        let clocks = Clocks::get();
-        println!("Clocks: {:?}", clocks);
-
        let ddr3x_clk_divisor = 2;
        let ddr2x_clk_divisor = 3;
-        println!("DDR 3x/2x clocks: {}/{}", clocks.ddr / u32::from(ddr3x_clk_divisor), clocks.ddr / u32::from(ddr2x_clk_divisor));
+        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(
@ -56,14 +60,35 @@ impl DdrRam {
        clocks
    }

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

        slcr::RegisterBlock::unlocked(|slcr| {
            // Step 1.
@ -111,21 +136,63 @@ impl DdrRam {
            slcr.ddriob_addr0.write(addr_config.clone());
            slcr.ddriob_addr1.write(addr_config);

-            let data_config = slcr::DdriobConfig::zeroed()
+            #[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);
-            slcr.ddriob_data0.write(data_config.clone());
-            slcr.ddriob_data1.write(data_config);
+            #[cfg(feature = "target_zc706")]
+            let data1_config = data0_config.clone();
+            #[cfg(feature = "target_cora_z7_10")]
+            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_cora_z7_10")]
+            let data1_config = slcr::DdriobConfig::zeroed()
+                .pullup_en(true);
+            #[cfg(feature = "target_redpitaya")]
+            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_redpitaya")]
+            let data1_config = slcr::DdriobConfig::zeroed()
+                .pullup_en(true);
+            slcr.ddriob_data0.write(data0_config);
+            slcr.ddriob_data1.write(data1_config);

-            let diff_config = slcr::DdriobConfig::zeroed()
+            #[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);
-            slcr.ddriob_diff0.write(diff_config.clone());
-            slcr.ddriob_diff1.write(diff_config);
+            #[cfg(feature = "target_zc706")]
+            let diff1_config = diff0_config.clone();
+            #[cfg(feature = "target_cora_z7_10")]
+            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_cora_z7_10")]
+            let diff1_config = slcr::DdriobConfig::zeroed()
+                .pullup_en(true);
+            #[cfg(feature = "target_redpitaya")]
+            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_redpitaya")]
+            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()
@ -140,37 +207,126 @@ impl DdrRam {
                slcr.ddriob_drive_slew_clock.write(0x00F9861C);
            }

-            #[cfg(feature = "target_zc706")]
-            let vref_sel = slcr::DdriobVrefSel::Vref0_75V;
-            #[cfg(feature = "target_cora_z7_10")]
-            let vref_sel = slcr::DdriobVrefSel::Vref0_675V;
-
-            // // Enable internal V[REF]
-            // slcr.ddriob_ddr_ctrl.modify(|_, w| w
-            //         .vref_ext_en_lower(false)
-            //         .vref_ext_en_upper(false)
-            //         .vref_sel(vref_sel)
-            //         .vref_int_en(true)
-            // );
            // Enable external V[REF]
+            #[cfg(feature = "target_cora_z7_10")]
            slcr.ddriob_ddr_ctrl.modify(|_, w| w
-                    .vref_ext_en_lower(true)
-                    .vref_ext_en_upper(true)
-                    .vref_sel(vref_sel)
                    .vref_int_en(false)
+                    .vref_ext_en_lower(true)
+                    .vref_ext_en_upper(false)
+            );
+            #[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)
+            );
+            #[cfg(feature = "target_redpitaya")]
+            slcr.ddriob_ddr_ctrl.modify(|_, w| w
+                    .vref_int_en(false)
+                    .vref_ext_en_lower(true)
+                    .vref_ext_en_upper(false)
            );
        });
    }

+    fn configure(&mut self) {
+        self.regs.dram_param0.write(
+            regs::DramParam0::zeroed()
+                .t_rc(0x1b)
+                .t_rfc_min(0x56)
+                .post_selfref_gap_x32(0x10)
+        );
+
+        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)
+        );
+
+        self.regs.dram_emr_mr.write(
+            regs::DramEmrMr::zeroed()
+                .mr(0x930)
+                .emr(0x4)
+        );
+
+        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)
+        );
+
+        self.regs.phy_init_ratio3.write(
+            regs::PhyInitRatio::zeroed()
+                .wrlvl_init_ratio(0x21)
+                .gatelvl_init_ratio(0xee)
+        );
+
+        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(&mut self) {
-        self.regs.ddrc_ctrl.modify(|_, w| w
-            .soft_rstb(false)
-        );
+        #[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(feature = "target_zc706")]
        let width = regs::DataBusWidth::Width32bit;
        #[cfg(feature = "target_cora_z7_10")]
        let width = regs::DataBusWidth::Width16bit;
+        #[cfg(feature = "target_redpitaya")]
+        let width = regs::DataBusWidth::Width16bit;
+        self.regs.ddrc_ctrl.modify(|_, w| w
+            .soft_rstb(false)
+            .powerdown_en(false)
+            .data_bus_width(width)
+        );
        self.regs.ddrc_ctrl.modify(|_, w| w
            .soft_rstb(true)
            .powerdown_en(false)
@ -181,18 +337,22 @@ impl DdrRam {
    }

    pub fn status(&self) -> regs::ControllerStatus {
-        self.regs.mode_sts_reg.read().operating_mode()
+        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 {
        #[cfg(feature = "target_zc706")]
-        let megabytes = 511;
+        let megabytes = 1023;
        #[cfg(feature = "target_cora_z7_10")]
        let megabytes = 511;
+        #[cfg(feature = "target_redpitaya")]
+        let megabytes = 511;

        megabytes * 1024 * 1024
    }
@ -204,16 +364,16 @@ impl DdrRam {
        let patterns: &'static [u32] = &[0xffff_ffff, 0x5555_5555, 0xaaaa_aaaa, 0];
        let mut expected = None;
        for (i, pattern) in patterns.iter().enumerate() {
-            println!("memtest phase {} (status: {:?})", i, self.status());
+            info!("memtest phase {} (status: {:?})", i, self.status());

-            for megabyte in 0..=(slice.len() / (1024 * 1024)) {
+            for megabyte in 0..slice.len() / (1024 * 1024) {
                let start = megabyte * 1024 * 1024 / 4;
-                let end = ((megabyte + 1) * 1024 * 1024 / 4).min(slice.len());
+                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 {
-                            println!("{:08X}: expected {:08X}, read {:08X}", b as *mut _ as usize, expected, read);
+                            error!("{:08X}: expected {:08X}, read {:08X}", b as *mut _ as usize, expected, read);
                        }
                    });
                    *b = *pattern;
--- a/libboard_zynq/src/ddr/regs.rs
+++ b/libboard_zynq/src/ddr/regs.rs
@ -1,8 +1,9 @@
 use volatile_register::{RO, RW};

-use libregister::{register, register_bit, register_bits_typed};
+use libregister::{register, register_at, register_bit, register_bits, register_bits_typed};

 #[allow(unused)]
+#[derive(Clone, Copy)]
 #[repr(u8)]
 pub enum DataBusWidth {
    Width32bit = 0b00,
@ -28,59 +29,59 @@ pub enum ControllerStatus {
 pub struct RegisterBlock {
    pub ddrc_ctrl: DdrcCtrl,
    pub two_rank_cfg: RW<u32>,
-    pub hpr_reg: RW<u32>,
-    pub lpr_reg: RW<u32>,
-    pub wr_reg: RW<u32>,
-    pub dram_param_reg0: RW<u32>,
-    pub dram_param_reg1: RW<u32>,
-    pub dram_param_reg2: RW<u32>,
-    pub dram_param_reg3: RW<u32>,
-    pub dram_param_reg4: RW<u32>,
+    pub hpr: RW<u32>,
+    pub lpr: RW<u32>,
+    pub wr: RW<u32>,
+    pub dram_param0: DramParam0,
+    pub dram_param1: RW<u32>,
+    pub dram_param2: DramParam2,
+    pub dram_param3: RW<u32>,
+    pub dram_param4: RW<u32>,
    pub dram_init_param: RW<u32>,
-    pub dram_emr_reg: RW<u32>,
-    pub dram_emr_mr_reg: RW<u32>,
+    pub dram_emr: RW<u32>,
+    pub dram_emr_mr: DramEmrMr,
    pub dram_burst8_rdwr: RW<u32>,
    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_reg: RW<u32>,
-    pub phy_dbg_reg: RW<u32>,
-    pub phy_cmd_timeout_rddata_cpt: RW<u32>,
-    pub mode_sts_reg: ModeStsReg,
+    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 ctrl_reg1: RW<u32>,
-    pub ctrl_reg2: RW<u32>,
-    pub ctrl_reg3: RW<u32>,
-    pub ctrl_reg4: RW<u32>,
+    pub ctrl1: RW<u32>,
+    pub ctrl2: RW<u32>,
+    pub ctrl3: RW<u32>,
+    pub ctrl4: RW<u32>,
    _unused0: [RO<u32>; 2],
-    pub ctrl_reg5: RW<u32>,
-    pub ctrl_reg6: RW<u32>,
+    pub ctrl5: RW<u32>,
+    pub ctrl6: RW<u32>,
    _unused1: [RO<u32>; 8],
    pub che_refresh_timer01: RW<u32>,
    pub che_t_zq: RW<u32>,
-    pub che_t_zq_short_interval_reg: RW<u32>,
-    pub deep_pwrdwn_reg: RW<u32>,
-    pub reg_2c: RW<u32>,
+    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: RW<u32>,
+    pub dfi_timing: DfiTiming,
    _unused2: [RO<u32>; 2],
-    pub che_ecc_control_reg_offset: RW<u32>,
-    pub che_corr_ecc_log_reg_offset: RW<u32>,
-    pub che_corr_ecc_addr_reg_offset: RW<u32>,
-    pub che_corr_ecc_data_31_0_reg_offset: RW<u32>,
-    pub che_corr_ecc_data_63_32_reg_offset: RW<u32>,
-    pub che_corr_ecc_data_71_64_reg_offset: RW<u32>,
-    pub che_uncorr_ecc_log_reg_offset: RW<u32>,
-    pub che_uncorr_ecc_addr_reg_offset: RW<u32>,
-    pub che_uncorr_ecc_data_31_0_reg_offset: RW<u32>,
-    pub che_uncorr_ecc_data_63_32_reg_offset: RW<u32>,
-    pub che_uncorr_ecc_data_71_64_reg_offset: RW<u32>,
-    pub che_ecc_stats_reg_offset: RW<u32>,
+    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_reg_offset: RW<u32>,
-    pub che_ecc_corr_bit_mask_63_32_reg_offset: 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_config0: RW<u32>,
@ -88,10 +89,10 @@ pub struct RegisterBlock {
    pub phy_config2: RW<u32>,
    pub phy_config3: RW<u32>,
    _unused4: RO<u32>,
-    pub phy_init_ratio0: RW<u32>,
-    pub phy_init_ratio1: RW<u32>,
-    pub phy_init_ratio2: RW<u32>,
-    pub phy_init_ratio3: RW<u32>,
+    pub phy_init_ratio0: PhyInitRatio,
+    pub phy_init_ratio1: PhyInitRatio,
+    pub phy_init_ratio2: PhyInitRatio,
+    pub phy_init_ratio3: PhyInitRatio,
    _unused5: RO<u32>,
    pub phy_rd_dqs_cfg0: RW<u32>,
    pub phy_rd_dqs_cfg1: RW<u32>,
@ -114,7 +115,7 @@ pub struct RegisterBlock {
    pub wr_data_slv3: RW<u32>,
    _unused9: RO<u32>,
    pub reg_64: RW<u32>,
-    pub reg_65: RW<u32>,
+    pub reg_65: Reg65,
    _unused10: [RO<u32>; 3],
    pub reg69_6a0: RW<u32>,
    pub reg69_6a1: RW<u32>,
@ -133,7 +134,7 @@ pub struct RegisterBlock {
    _unused13: RO<u32>,
    pub dll_lock_sts: RW<u32>,
    pub phy_ctrl_sts: RW<u32>,
-    pub phy_ctrl_sts_reg2: RW<u32>,
+    pub phy_ctrl_sts2: RW<u32>,
    _unused14: [RO<u32>; 5],
    pub axi_id: RW<u32>,
    pub page_mask: RW<u32>,
@ -157,11 +158,7 @@ pub struct RegisterBlock {
    pub lpddr_ctrl3: RW<u32>,
 }

-impl RegisterBlock {
-    pub unsafe fn new() -> &'static mut Self {
-        &mut *(0xF8006000 as *mut _)
-    }
-}
+register_at!(RegisterBlock, 0xF8006000, ddrc);

 register!(ddrc_ctrl, DdrcCtrl, RW, u32);
 register_bit!(ddrc_ctrl,
@ -171,6 +168,64 @@ 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_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_emr_mr, DramEmrMr, RW, u32);
+register_bits!(dram_emr_mr, mr, u16, 0, 15);
+register_bits!(dram_emr_mr, emr, u16, 16, 31);
+
+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!(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_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_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);
--- a/libboard_zynq/src/devc/mod.rs
+++ b/libboard_zynq/src/devc/mod.rs
@ -1,29 +1,341 @@
-use core::fmt;
-
+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) {
-        self.regs.control.modify(|_, w| {
-            w.pcap_mode(true)
-            .pcap_pr(true)
+        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)
        })
    }
-    pub fn disable(&mut self) {
-        self.regs.control.modify(|_, w| {
-            w.pcap_mode(false)
-            .pcap_pr(false)
-        })
+
+    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
@ -1,15 +1,14 @@
-use volatile_register::{RO, WO, RW};
-
 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 cfg: Cfg,
    pub lock: Lock,
+    pub cfg: Cfg,
    pub int_sts: IntSts,
    pub int_mask: IntMask,
    pub status: Status,
@ -17,9 +16,13 @@ pub struct RegisterBlock {
    pub dma_dest_addr: DmaDestAddr,
    pub dma_src_len: DmaSrcLen,
    pub dma_dest_len: DmaDestLen,
+    unused0: u32,
    pub multiboot_addr: MultibootAddr,
-    pub unlock: Unlock,
+    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,
@ -73,18 +76,18 @@ pub enum WFifoTh {
    ThreeFourthEmpty = 0b10, // Three fourth empty for write
    Empty            = 0b11, // Empty for write
 }
-register_bits_typed!(cfg, wfifo_th, u8, WFifoTh, 10, 11);
+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, pps_gts_usr_b_int, 31);
-register_bit!(int_sts, pps_fst_cfg_b_int, 30);
-register_bit!(int_sts, pps_gpwrdwn_b_int, 29);
-register_bit!(int_sts, pps_gts_cfg_b_int, 27);
-register_bit!(int_sts, pps_cfg_reset_b_int, 26);
+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);
@ -150,23 +153,20 @@ 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, u8, 0, 31);
+register_bits!(dma_src_addr, src_addr, u32, 0, 31);

 register!(dma_dest_addr, DmaDestAddr, RW, u32);
-register_bits!(dma_dest_addr, dest_addr, u8, 0, 31);
+register_bits!(dma_dest_addr, dest_addr, u32, 0, 31);

 register!(dma_src_len, DmaSrcLen, RW, u32);
-register_bits!(dma_src_len, dma_len, u8, 0, 26);
+register_bits!(dma_src_len, dma_len, u32, 0, 26);

 register!(dma_dest_len, DmaDestLen, RW, u32);
-register_bits!(dma_dest_len, dma_len, u8, 0, 26);
+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!(unlock, Unlock, RW, u32);
-register_bits!(unlock, unlock, u8, 0, 31);
-
 register!(mctrl, MCtrl, RW, u32);
 register_bits!(mctrl, ps_version, u8, 28, 31);
 register_bit!(mctrl, pcfg_por_b, 8);
--- a/libboard_zynq/src/dmac/mod.rs
+++ b/libboard_zynq/src/dmac/mod.rs
@ -1 +0,0 @@
-mod regs;
--- a/libboard_zynq/src/dmac/regs.rs
+++ b/libboard_zynq/src/dmac/regs.rs
@ -1,651 +0,0 @@
-use volatile_register::{RO, WO, RW};
-
-use libregister::{
-    register, register_at,
-    register_bit, register_bits, register_bits_typed,
-};
-
-#[allow(unused)]
-#[repr(C)]
-pub struct RegisterBlock {
-    pub ds: Ds,
-    pub dpc: DPc,
-    pub inten: Inten,
-    pub es: Es,
-    pub intstatus: IntStatus,
-    pub intclr: IntClr,
-    pub fsm: Fsm,
-    pub fsc: Fsc,
-    pub ftm: Ftm,
-    pub ftc0: Ftc0,
-    pub xdmaps_ftcn_offset_1: XDmaPsFtcnOffset1,
-    pub xdmaps_ftcn_offset_2: XDmaPsFtcnOffset2,
-    pub xdmaps_ftcn_offset_3: XDmaPsFtcnOffset3,
-    pub xdmaps_ftcn_offset_4: XDmaPsFtcnOffset4,
-    pub xdmaps_ftcn_offset_5: XDmaPsFtcnOffset5,
-    pub xdmaps_ftcn_offset_6: XDmaPsFtcnOffset6,
-    pub xdmaps_ftcn_offset_7: XDmaPsFtcnOffset7,
-    pub cs0: Cs0,
-    pub cpc0: Cpc0,
-    pub xdmaps_csn_offset_1:  XDmaPsCSnOffset1,
-    pub xdmaps_cpcn_offset_1: XDmaPsCPCnOffset1,
-    pub xdmaps_csn_offset_2:  XDmaPsCSnOffset2,
-    pub xdmaps_cpcn_offset_2: XDmaPsCPCnOffset2,
-    pub xdmaps_csn_offset_3:  XDmaPsCSnOffset3,
-    pub xdmaps_cpcn_offset_3: XDmaPsCPCnOffset3,
-    pub xdmaps_csn_offset_4:  XDmaPsCSnOffset4,
-    pub xdmaps_cpcn_offset_4: XDmaPsCPCnOffset4,
-    pub xdmaps_csn_offset_5:  XDmaPsCSnOffset5,
-    pub xdmaps_cpcn_offset_5: XDmaPsCPCnOffset5,
-    pub xdmaps_csn_offset_6:  XDmaPsCSnOffset6,
-    pub xdmaps_cpcn_offset_6: XDmaPsCPCnOffset6,
-    pub xdmaps_csn_offset_7:  XDmaPsCSnOffset7,
-    pub xdmaps_cpcn_offset_7: XDmaPsCPCnOffset7,
-    pub sa_0: Sa0,
-    pub da_0: Da0,
-    pub cc_0: Cc0,
-    pub lc0_0: Lc00,
-    pub lc1_0: Lc10,
-    pub xdmaps_sa_n_offset_1:  XDmaPsSaNOffset1,
-    pub xdmaps_da_n_offset_1:  XDmaPsDaNOffset1,
-    pub xdmaps_cc_n_offset_1:  XDmaPsCcNOffset1,
-    pub xdmaps_lc0_n_offset_1: XDmaPsLc0NOffset1,
-    pub xdmaps_lc1_n_offset_1: XDmaPsLc1NOffset1,
-    pub xdmaps_sa_n_offset_2:  XDmaPsSaNOffset2,
-    pub xdmaps_da_n_offset_2:  XDmaPsDaNOffset2,
-    pub xdmaps_cc_n_offset_2:  XDmaPsCcNOffset2,
-    pub xdmaps_lc0_n_offset_2: XDmaPsLc0NOffset2,
-    pub xdmaps_lc1_n_offset_2: XDmaPsLc1NOffset2,
-    pub xdmaps_sa_n_offset_3:  XDmaPsSaNOffset3,
-    pub xdmaps_da_n_offset_3:  XDmaPsDaNOffset3,
-    pub xdmaps_cc_n_offset_3:  XDmaPsCcNOffset3,
-    pub xdmaps_lc0_n_offset_3: XDmaPsLc0NOffset3,
-    pub xdmaps_lc1_n_offset_3: XDmaPsLc1NOffset3,
-    pub xdmaps_sa_n_offset_4:  XDmaPsSaNOffset4,
-    pub xdmaps_da_n_offset_4:  XDmaPsDaNOffset4,
-    pub xdmaps_cc_n_offset_4:  XDmaPsCcNOffset4,
-    pub xdmaps_lc0_n_offset_4: XDmaPsLc0NOffset4,
-    pub xdmaps_lc1_n_offset_4: XDmaPsLc1NOffset4,
-    pub xdmaps_sa_n_offset_5:  XDmaPsSaNOffset5,
-    pub xdmaps_da_n_offset_5:  XDmaPsDaNOffset5,
-    pub xdmaps_cc_n_offset_5:  XDmaPsCcNOffset5,
-    pub xdmaps_lc0_n_offset_5: XDmaPsLc0NOffset5,
-    pub xdmaps_lc1_n_offset_5: XDmaPsLc1NOffset5,
-    pub xdmaps_sa_n_offset_6:  XDmaPsSaNOffset6,
-    pub xdmaps_da_n_offset_6:  XDmaPsDaNOffset6,
-    pub xdmaps_cc_n_offset_6:  XDmaPsCcNOffset6,
-    pub xdmaps_lc0_n_offset_6: XDmaPsLc0NOffset6,
-    pub xdmaps_lc1_n_offset_6: XDmaPsLc1NOffset6,
-    pub xdmaps_sa_n_offset_7:  XDmaPsSaNOffset7,
-    pub xdmaps_da_n_offset_7:  XDmaPsDaNOffset7,
-    pub xdmaps_cc_n_offset_7:  XDmaPsCcNOffset7,
-    pub xdmaps_lc0_n_offset_7: XDmaPsLc0NOffset7,
-    pub xdmaps_lc1_n_offset_7: XDmaPsLc1NOffset7,
-    pub dbgstatus: DbgStatus,
-    pub dbgcmd: DbgCmd,
-    pub dbginst0: DbgInst0,
-    pub dbginst1: DbgInst1,
-    pub cr0: Cr0,
-    pub cr1: Cr1,
-    pub cr2: Cr2,
-    pub cr3: Cr3,
-    pub cr4: Cr4,
-    pub crdn: Crdn,
-    pub wd: Wd,
-    pub periph_id_0: PeriphId0,
-    pub periph_id_1: PeriphId1,
-    pub periph_id_2: PeriphId2,
-    pub periph_id_3: PeriphId3,
-    pub pcell_id_0: PCellId0,
-    pub pcell_id_1: PCellId1,
-    pub pcell_id_2: PCellId2,
-    pub pcell_id_3: PCellId3,
-
-}
-
-register_at!(RegisterBlock, 0xF8004000, dmac0_ns);
-register_at!(RegisterBlock, 0xF8003000, dmac0_s);
-
-#[allow(unused)]
-#[repr(u8)]
-pub enum WakeUpEvent{
-    // @missing: there's a binary prefix ahead of this as per TRM 1173 Wakeup_event
-    Event0  = 0b0000,
-    Event1  = 0b0001,
-    Event2  = 0b0010,
-    Event3  = 0b0011,
-    Event4  = 0b0100,
-    Event5  = 0b0101,
-    Event6  = 0b0110,
-    Event7  = 0b0111,
-    Event8  = 0b1000,
-    Event9  = 0b1001,
-    Event10 = 0b1010,
-    Event11 = 0b1011,
-    Event12 = 0b1100,
-    Event13 = 0b1101,
-    Event14 = 0b1110,
-    Event15 = 0b1111,
-}
-#[allow(unused)]
-#[repr(u8)]
-pub enum DMAStatus{
-    Stopped         = 0b0000,
-    Executing       = 0b0001,
-    CacheMiss       = 0b0010,
-    UpdatingPc      = 0b0011,
-    WaitingForEvent = 0b0100,
-    Reserved0       = 0b0101,
-    Reserved1       = 0b0110,
-    Reserved2       = 0b0111,
-    Reserved3       = 0b1000,
-    Reserved4       = 0b1001,
-    Reserved5       = 0b1010,
-    Reserved6       = 0b1011,
-    Reserved7       = 0b1100,
-    Reserved8       = 0b1101,
-    Reserved9       = 0b1110,
-    Faulting        = 0b1111,
-}
-
-register!(ds, Ds, RW, u32);
-register_bit!(ds, dns, 9);
-register_bits_typed!(ds, wakeup_event, u8, WakeUpEvent, 4, 8);
-register_bits_typed!(ds, dma_status, u8, DMAStatus, 0, 3);
-
-register!(dpc, DPc, RW, u32);
-register_bits!(dpc, pc_mgr, u8, 0, 31);
-
-register!(inten, Inten, RW, u32);
-register_bits!(inten, event_irq_select, u8, 0, 31);
-
-register!(es, Es, RW, u32);
-register_bits!(es, dmasev_active, u8, 0, 31);
-
-register!(intstatus, IntStatus, RW, u32);
-register_bits!(intstatus, irq_status, u8, 0, 31);
-
-register!(intclr, IntClr, RW, u32);
-register_bits!(intstatus, irq_clr, u8, 0, 31);
-
-register!(fsm, Fsm, RW, u32);
-register_bit!(fsm, fs_mgr, 0);
-
-register!(fsc, Fsc, RW, u32);
-register_bits!(fsc, fault_status, u8, 0, 7);
-
-register!(ftm, Ftm, RW, u32);
-register_bit!(ftm, dbg_instr, 30);
-register_bit!(ftm, instr_fetch_err, 16);
-register_bit!(ftm, mgr_evnt_err, 5);
-register_bit!(ftm, dmago_err, 4);
-register_bit!(ftm, operand_invalid, 1);
-register_bit!(ftm, undef_instr, 0);
-
-register!(ftc0, Ftc0, RW, u32);
-register_bit!(ftc0, lockup_err, 31);
-register_bit!(ftc0, dbg_instr, 30);
-register_bit!(ftc0, data_read_err, 18);
-register_bit!(ftc0, data_write_err, 17);
-register_bit!(ftc0, instr_fetch_err, 16);
-register_bit!(ftc0, st_data_unavailable, 13);
-register_bit!(ftc0, mfifo_err, 12);
-register_bit!(ftc0, ch_rdwr_err, 7);
-register_bit!(ftc0, ch_periph_err, 6);
-register_bit!(ftc0, ch_evnt_err, 5);
-register_bit!(ftc0, operand_invalid, 1);
-register_bit!(ftc0, undef_instr, 0);
-
-register!(xdmaps_ftcn_offset_1, XDmaPsFtcnOffset1, RW, u32);
-register_bit!(xdmaps_ftcn_offset_1, lockup_err, 31);
-register_bit!(xdmaps_ftcn_offset_1, dbg_instr, 30);
-register_bit!(xdmaps_ftcn_offset_1, data_read_err, 18);
-register_bit!(xdmaps_ftcn_offset_1, data_write_err, 17);
-register_bit!(xdmaps_ftcn_offset_1, instr_fetch_err, 16);
-register_bit!(xdmaps_ftcn_offset_1, st_data_unavailable, 13);
-register_bit!(xdmaps_ftcn_offset_1, mfifo_err, 12);
-register_bit!(xdmaps_ftcn_offset_1, ch_rdwr_err, 7);
-register_bit!(xdmaps_ftcn_offset_1, ch_periph_err, 6);
-register_bit!(xdmaps_ftcn_offset_1, ch_evnt_err, 5);
-register_bit!(xdmaps_ftcn_offset_1, operand_invalid, 1);
-register_bit!(xdmaps_ftcn_offset_1, undef_instr, 0);
-
-register!(xdmaps_ftcn_offset_2, XDmaPsFtcnOffset2, RW, u32);
-register_bit!(xdmaps_ftcn_offset_2, lockup_err, 31);
-register_bit!(xdmaps_ftcn_offset_2, dbg_instr, 30);
-register_bit!(xdmaps_ftcn_offset_2, data_read_err, 18);
-register_bit!(xdmaps_ftcn_offset_2, data_write_err, 17);
-register_bit!(xdmaps_ftcn_offset_2, instr_fetch_err, 16);
-register_bit!(xdmaps_ftcn_offset_2, st_data_unavailable, 13);
-register_bit!(xdmaps_ftcn_offset_2, mfifo_err, 12);
-register_bit!(xdmaps_ftcn_offset_2, ch_rdwr_err, 7);
-register_bit!(xdmaps_ftcn_offset_2, ch_periph_err, 6);
-register_bit!(xdmaps_ftcn_offset_2, ch_evnt_err, 5);
-register_bit!(xdmaps_ftcn_offset_2, operand_invalid, 1);
-register_bit!(xdmaps_ftcn_offset_2, undef_instr, 0);
-
-register!(xdmaps_ftcn_offset_3, XDmaPsFtcnOffset3, RW, u32);
-register_bit!(xdmaps_ftcn_offset_3, lockup_err, 31);
-register_bit!(xdmaps_ftcn_offset_3, dbg_instr, 30);
-register_bit!(xdmaps_ftcn_offset_3, data_read_err, 18);
-register_bit!(xdmaps_ftcn_offset_3, data_write_err, 17);
-register_bit!(xdmaps_ftcn_offset_3, instr_fetch_err, 16);
-register_bit!(xdmaps_ftcn_offset_3, st_data_unavailable, 13);
-register_bit!(xdmaps_ftcn_offset_3, mfifo_err, 12);
-register_bit!(xdmaps_ftcn_offset_3, ch_rdwr_err, 7);
-register_bit!(xdmaps_ftcn_offset_3, ch_periph_err, 6);
-register_bit!(xdmaps_ftcn_offset_3, ch_evnt_err, 5);
-register_bit!(xdmaps_ftcn_offset_3, operand_invalid, 1);
-register_bit!(xdmaps_ftcn_offset_3, undef_instr, 0);
-
-register!(xdmaps_ftcn_offset_4, XDmaPsFtcnOffset4, RW, u32);
-register_bit!(xdmaps_ftcn_offset_4, lockup_err, 31);
-register_bit!(xdmaps_ftcn_offset_4, dbg_instr, 30);
-register_bit!(xdmaps_ftcn_offset_4, data_read_err, 18);
-register_bit!(xdmaps_ftcn_offset_4, data_write_err, 17);
-register_bit!(xdmaps_ftcn_offset_4, instr_fetch_err, 16);
-register_bit!(xdmaps_ftcn_offset_4, st_data_unavailable, 13);
-register_bit!(xdmaps_ftcn_offset_4, mfifo_err, 12);
-register_bit!(xdmaps_ftcn_offset_4, ch_rdwr_err, 7);
-register_bit!(xdmaps_ftcn_offset_4, ch_periph_err, 6);
-register_bit!(xdmaps_ftcn_offset_4, ch_evnt_err, 5);
-register_bit!(xdmaps_ftcn_offset_4, operand_invalid, 1);
-register_bit!(xdmaps_ftcn_offset_4, undef_instr, 0);
-
-register!(xdmaps_ftcn_offset_5, XDmaPsFtcnOffset5, RW, u32);
-register_bit!(xdmaps_ftcn_offset_5, lockup_err, 31);
-register_bit!(xdmaps_ftcn_offset_5, dbg_instr, 30);
-register_bit!(xdmaps_ftcn_offset_5, data_read_err, 18);
-register_bit!(xdmaps_ftcn_offset_5, data_write_err, 17);
-register_bit!(xdmaps_ftcn_offset_5, instr_fetch_err, 16);
-register_bit!(xdmaps_ftcn_offset_5, st_data_unavailable, 13);
-register_bit!(xdmaps_ftcn_offset_5, mfifo_err, 12);
-register_bit!(xdmaps_ftcn_offset_5, ch_rdwr_err, 7);
-register_bit!(xdmaps_ftcn_offset_5, ch_periph_err, 6);
-register_bit!(xdmaps_ftcn_offset_5, ch_evnt_err, 5);
-register_bit!(xdmaps_ftcn_offset_5, operand_invalid, 1);
-register_bit!(xdmaps_ftcn_offset_5, undef_instr, 0);
-
-register!(xdmaps_ftcn_offset_6, XDmaPsFtcnOffset6, RW, u32);
-register_bit!(xdmaps_ftcn_offset_6, lockup_err, 31);
-register_bit!(xdmaps_ftcn_offset_6, dbg_instr, 30);
-register_bit!(xdmaps_ftcn_offset_6, data_read_err, 18);
-register_bit!(xdmaps_ftcn_offset_6, data_write_err, 17);
-register_bit!(xdmaps_ftcn_offset_6, instr_fetch_err, 16);
-register_bit!(xdmaps_ftcn_offset_6, st_data_unavailable, 13);
-register_bit!(xdmaps_ftcn_offset_6, mfifo_err, 12);
-register_bit!(xdmaps_ftcn_offset_6, ch_rdwr_err, 7);
-register_bit!(xdmaps_ftcn_offset_6, ch_periph_err, 6);
-register_bit!(xdmaps_ftcn_offset_6, ch_evnt_err, 5);
-register_bit!(xdmaps_ftcn_offset_6, operand_invalid, 1);
-register_bit!(xdmaps_ftcn_offset_6, undef_instr, 0);
-
-register!(xdmaps_ftcn_offset_7, XDmaPsFtcnOffset7, RW, u32);
-register_bit!(xdmaps_ftcn_offset_7, lockup_err, 31);
-register_bit!(xdmaps_ftcn_offset_7, dbg_instr, 30);
-register_bit!(xdmaps_ftcn_offset_7, data_read_err, 18);
-register_bit!(xdmaps_ftcn_offset_7, data_write_err, 17);
-register_bit!(xdmaps_ftcn_offset_7, instr_fetch_err, 16);
-register_bit!(xdmaps_ftcn_offset_7, st_data_unavailable, 13);
-register_bit!(xdmaps_ftcn_offset_7, mfifo_err, 12);
-register_bit!(xdmaps_ftcn_offset_7, ch_rdwr_err, 7);
-register_bit!(xdmaps_ftcn_offset_7, ch_periph_err, 6);
-register_bit!(xdmaps_ftcn_offset_7, ch_evnt_err, 5);
-register_bit!(xdmaps_ftcn_offset_7, operand_invalid, 1);
-register_bit!(xdmaps_ftcn_offset_7, undef_instr, 0);
-
-register!(cs0, Cs0, RW, u32);
-register_bit!(cs0, cns, 21);
-register_bit!(cs0, dmawfp_periph, 15);
-register_bit!(cs0, dmawfp_b_ns, 14);
-register_bits!(cs0, wakeup_num, u8, 4, 8);
-register_bits!(cs0, channel_status, u8, 0, 3);
-
-register!(cpc0, Cpc0, RW, u32);
-register_bits!(cpc0, pc_chnl, u8, 0, 31);
-
-register!(xdmaps_csn_offset_1, XDmaPsCSnOffset1, RW, u32);
-register_bit!(xdmaps_csn_offset_1, cns, 21);
-register_bit!(xdmaps_csn_offset_1, dmawfp_periph, 15);
-register_bit!(xdmaps_csn_offset_1, dmawfp_b_ns, 14);
-register_bits!(xdmaps_csn_offset_1, wakeup_num, u8, 4, 8);
-register_bits!(xdmaps_csn_offset_1, channel_status, u8, 0, 3);
-
-register!(xdmaps_cpcn_offset_1, XDmaPsCPCnOffset1, RW, u32);
-register_bits!(xdmaps_cpcn_offset_1, pc_chnl, u8, 0, 31);
-
-register!(xdmaps_csn_offset_2, XDmaPsCSnOffset2, RW, u32);
-register_bit!(xdmaps_csn_offset_2, cns, 21);
-register_bit!(xdmaps_csn_offset_2, dmawfp_periph, 15);
-register_bit!(xdmaps_csn_offset_2, dmawfp_b_ns, 14);
-register_bits!(xdmaps_csn_offset_2, wakeup_num, u8, 4, 8);
-register_bits!(xdmaps_csn_offset_2, channel_status, u8, 0, 3);
-
-register!(xdmaps_cpcn_offset_2, XDmaPsCPCnOffset2, RW, u32);
-register_bits!(xdmaps_cpcn_offset_2, pc_chnl, u8, 0, 31);
-
-register!(xdmaps_csn_offset_3, XDmaPsCSnOffset3, RW, u32);
-register_bit!(xdmaps_csn_offset_3, cns, 21);
-register_bit!(xdmaps_csn_offset_3, dmawfp_periph, 15);
-register_bit!(xdmaps_csn_offset_3, dmawfp_b_ns, 14);
-register_bits!(xdmaps_csn_offset_3, wakeup_num, u8, 4, 8);
-register_bits!(xdmaps_csn_offset_3, channel_status, u8, 0, 3);
-
-register!(xdmaps_cpcn_offset_3, XDmaPsCPCnOffset3, RW, u32);
-register_bits!(xdmaps_cpcn_offset_3, pc_chnl, u8, 0, 31);
-
-register!(xdmaps_csn_offset_4, XDmaPsCSnOffset4, RW, u32);
-register_bit!(xdmaps_csn_offset_4, cns, 21);
-register_bit!(xdmaps_csn_offset_4, dmawfp_periph, 15);
-register_bit!(xdmaps_csn_offset_4, dmawfp_b_ns, 14);
-register_bits!(xdmaps_csn_offset_4, wakeup_num, u8, 4, 8);
-register_bits!(xdmaps_csn_offset_4, channel_status, u8, 0, 3);
-
-register!(xdmaps_cpcn_offset_4, XDmaPsCPCnOffset4, RW, u32);
-register_bits!(xdmaps_cpcn_offset_4, pc_chnl, u8, 0, 31);
-
-register!(xdmaps_csn_offset_5, XDmaPsCSnOffset5, RW, u32);
-register_bit!(xdmaps_csn_offset_5, cns, 21);
-register_bit!(xdmaps_csn_offset_5, dmawfp_periph, 15);
-register_bit!(xdmaps_csn_offset_5, dmawfp_b_ns, 14);
-register_bits!(xdmaps_csn_offset_5, wakeup_num, u8, 4, 8);
-register_bits!(xdmaps_csn_offset_5, channel_status, u8, 0, 3);
-
-register!(xdmaps_cpcn_offset_5, XDmaPsCPCnOffset5, RW, u32);
-register_bits!(xdmaps_cpcn_offset_5, pc_chnl, u8, 0, 31);
-
-register!(xdmaps_csn_offset_6, XDmaPsCSnOffset6, RW, u32);
-register_bit!(xdmaps_csn_offset_6, cns, 21);
-register_bit!(xdmaps_csn_offset_6, dmawfp_periph, 15);
-register_bit!(xdmaps_csn_offset_6, dmawfp_b_ns, 14);
-register_bits!(xdmaps_csn_offset_6, wakeup_num, u8, 4, 8);
-register_bits!(xdmaps_csn_offset_6, channel_status, u8, 0, 3);
-
-register!(xdmaps_cpcn_offset_6, XDmaPsCPCnOffset6, RW, u32);
-register_bits!(xdmaps_cpcn_offset_6, pc_chnl, u8, 0, 31);
-
-register!(xdmaps_csn_offset_7, XDmaPsCSnOffset7, RW, u32);
-register_bit!(xdmaps_csn_offset_7, cns, 21);
-register_bit!(xdmaps_csn_offset_7, dmawfp_periph, 15);
-register_bit!(xdmaps_csn_offset_7, dmawfp_b_ns, 14);
-register_bits!(xdmaps_csn_offset_7, wakeup_num, u8, 4, 8);
-register_bits!(xdmaps_csn_offset_7, channel_status, u8, 0, 3);
-
-register!(xdmaps_cpcn_offset_7, XDmaPsCPCnOffset7, RW, u32);
-register_bits!(xdmaps_cpcn_offset_7, pc_chnl, u8, 0, 31);
-
-register!(sa_0, Sa0, RW, u32);
-register_bits!(sa_0, src_addr, u8, 0, 31);
-
-register!(da_0, Da0, RW, u32);
-register_bits!(da_0, dest_addr, u8, 0, 31);
-
-register!(cc_0, Cc0, RW, u32);
-register_bits!(cc_0, endian_swap_size, u8, 28, 30);
-register_bits!(cc_0, dst_cache_ctrl, u8, 25, 27);
-register_bits!(cc_0, dst_prot_ctrl, u8, 22, 24);
-register_bits!(cc_0, dst_burst_len, u8, 18, 21);
-register_bits!(cc_0, dst_burst_size, u8, 15, 17);
-register_bit!(cc_0, dst_inc, 14);
-register_bits!(cc_0, src_cache_ctrl, u8, 11, 13);
-register_bits!(cc_0, src_prot_ctrl, u8, 8, 10);
-register_bits!(cc_0, src_burst_len, u8, 4, 7);
-register_bits!(cc_0, src_burst_size, u8, 1, 3);
-register_bit!(cc_0, src_inc, 0);
-
-register!(lc0_0, Lc00, RW, u32);
-register_bits!(lc0_0, loop_counter_iteration, u8, 0, 7);
-
-register!(lc1_0, Lc10, RW, u32);
-register_bits!(lc1_0, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_sa_n_offset_1, XDmaPsSaNOffset1, RW, u32);
-register_bits!(xdmaps_sa_n_offset_1, src_addr, u8, 0, 31);
-
-register!(xdmaps_da_n_offset_1, XDmaPsDaNOffset1, RW, u32);
-register_bits!(xdmaps_da_n_offset_1, dest_addr, u8, 0, 31);
-
-register!(xdmaps_cc_n_offset_1, XDmaPsCcNOffset1, RW, u32);
-register_bits!(xdmaps_cc_n_offset_1, endian_swap_size, u8, 28, 30);
-register_bits!(xdmaps_cc_n_offset_1, dst_cache_ctrl, u8, 25, 27);
-register_bits!(xdmaps_cc_n_offset_1, dst_prot_ctrl, u8, 22, 24);
-register_bits!(xdmaps_cc_n_offset_1, dst_burst_len, u8, 18, 21);
-register_bits!(xdmaps_cc_n_offset_1, dst_burst_size, u8, 15, 17);
-register_bit!(xdmaps_cc_n_offset_1,  dst_inc, 14);
-register_bits!(xdmaps_cc_n_offset_1, src_cache_ctrl, u8, 11, 13);
-register_bits!(xdmaps_cc_n_offset_1, src_prot_ctrl, u8, 8, 10);
-register_bits!(xdmaps_cc_n_offset_1, src_burst_len, u8, 4, 7);
-register_bits!(xdmaps_cc_n_offset_1, src_burst_size, u8, 1, 3);
-register_bit!(xdmaps_cc_n_offset_1,  src_inc, 0);
-
-register!(xdmaps_lc0_n_offset_1, XDmaPsLc0NOffset1, RW, u32);
-register_bits!(xdmaps_lc0_n_offset_1, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_lc1_n_offset_1, XDmaPsLc1NOffset1, RW, u32);
-register_bits!(xdmaps_lc1_n_offset_1, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_sa_n_offset_2, XDmaPsSaNOffset2, RW, u32);
-register_bits!(xdmaps_sa_n_offset_2, src_addr, u8, 0, 31);
-
-register!(xdmaps_da_n_offset_2, XDmaPsDaNOffset2, RW, u32);
-register_bits!(xdmaps_da_n_offset_2, dest_addr, u8, 0, 31);
-
-register!(xdmaps_cc_n_offset_2, XDmaPsCcNOffset2, RW, u32);
-register_bits!(xdmaps_cc_n_offset_2, endian_swap_size, u8, 28, 30);
-register_bits!(xdmaps_cc_n_offset_2, dst_cache_ctrl, u8, 25, 27);
-register_bits!(xdmaps_cc_n_offset_2, dst_prot_ctrl, u8, 22, 24);
-register_bits!(xdmaps_cc_n_offset_2, dst_burst_len, u8, 18, 21);
-register_bits!(xdmaps_cc_n_offset_2, dst_burst_size, u8, 15, 17);
-register_bit!(xdmaps_cc_n_offset_2,  dst_inc, 14);
-register_bits!(xdmaps_cc_n_offset_2, src_cache_ctrl, u8, 11, 13);
-register_bits!(xdmaps_cc_n_offset_2, src_prot_ctrl, u8, 8, 10);
-register_bits!(xdmaps_cc_n_offset_2, src_burst_len, u8, 4, 7);
-register_bits!(xdmaps_cc_n_offset_2, src_burst_size, u8, 1, 3);
-register_bit!(xdmaps_cc_n_offset_2,  src_inc, 0);
-
-register!(xdmaps_lc0_n_offset_2, XDmaPsLc0NOffset2, RW, u32);
-register_bits!(xdmaps_lc0_n_offset_2, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_lc1_n_offset_2, XDmaPsLc1NOffset2, RW, u32);
-register_bits!(xdmaps_lc1_n_offset_2, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_sa_n_offset_3, XDmaPsSaNOffset3, RW, u32);
-register_bits!(xdmaps_sa_n_offset_3, src_addr, u8, 0, 31);
-
-register!(xdmaps_da_n_offset_3, XDmaPsDaNOffset3, RW, u32);
-register_bits!(xdmaps_da_n_offset_3, dest_addr, u8, 0, 31);
-
-register!(xdmaps_cc_n_offset_3, XDmaPsCcNOffset3, RW, u32);
-register_bits!(xdmaps_cc_n_offset_3, endian_swap_size, u8, 28, 30);
-register_bits!(xdmaps_cc_n_offset_3, dst_cache_ctrl, u8, 25, 27);
-register_bits!(xdmaps_cc_n_offset_3, dst_prot_ctrl, u8, 22, 24);
-register_bits!(xdmaps_cc_n_offset_3, dst_burst_len, u8, 18, 21);
-register_bits!(xdmaps_cc_n_offset_3, dst_burst_size, u8, 15, 17);
-register_bit!(xdmaps_cc_n_offset_3,  dst_inc, 14);
-register_bits!(xdmaps_cc_n_offset_3, src_cache_ctrl, u8, 11, 13);
-register_bits!(xdmaps_cc_n_offset_3, src_prot_ctrl, u8, 8, 10);
-register_bits!(xdmaps_cc_n_offset_3, src_burst_len, u8, 4, 7);
-register_bits!(xdmaps_cc_n_offset_3, src_burst_size, u8, 1, 3);
-register_bit!(xdmaps_cc_n_offset_3,  src_inc, 0);
-
-register!(xdmaps_lc0_n_offset_3, XDmaPsLc0NOffset3, RW, u32);
-register_bits!(xdmaps_lc0_n_offset_3, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_lc1_n_offset_3, XDmaPsLc1NOffset3, RW, u32);
-register_bits!(xdmaps_lc1_n_offset_3, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_sa_n_offset_4, XDmaPsSaNOffset4, RW, u32);
-register_bits!(xdmaps_sa_n_offset_4, src_addr, u8, 0, 31);
-
-register!(xdmaps_da_n_offset_4, XDmaPsDaNOffset4, RW, u32);
-register_bits!(xdmaps_da_n_offset_4, dest_addr, u8, 0, 31);
-
-register!(xdmaps_cc_n_offset_4, XDmaPsCcNOffset4, RW, u32);
-register_bits!(xdmaps_cc_n_offset_4, endian_swap_size, u8, 28, 30);
-register_bits!(xdmaps_cc_n_offset_4, dst_cache_ctrl, u8, 25, 27);
-register_bits!(xdmaps_cc_n_offset_4, dst_prot_ctrl, u8, 22, 24);
-register_bits!(xdmaps_cc_n_offset_4, dst_burst_len, u8, 18, 21);
-register_bits!(xdmaps_cc_n_offset_4, dst_burst_size, u8, 15, 17);
-register_bit!(xdmaps_cc_n_offset_4,  dst_inc, 14);
-register_bits!(xdmaps_cc_n_offset_4, src_cache_ctrl, u8, 11, 13);
-register_bits!(xdmaps_cc_n_offset_4, src_prot_ctrl, u8, 8, 10);
-register_bits!(xdmaps_cc_n_offset_4, src_burst_len, u8, 4, 7);
-register_bits!(xdmaps_cc_n_offset_4, src_burst_size, u8, 1, 3);
-register_bit!(xdmaps_cc_n_offset_4,  src_inc, 0);
-
-register!(xdmaps_lc0_n_offset_4, XDmaPsLc0NOffset4, RW, u32);
-register_bits!(xdmaps_lc0_n_offset_4, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_lc1_n_offset_4, XDmaPsLc1NOffset4, RW, u32);
-register_bits!(xdmaps_lc1_n_offset_4, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_sa_n_offset_5, XDmaPsSaNOffset5, RW, u32);
-register_bits!(xdmaps_sa_n_offset_5, src_addr, u8, 0, 31);
-
-register!(xdmaps_da_n_offset_5, XDmaPsDaNOffset5, RW, u32);
-register_bits!(xdmaps_da_n_offset_5, dest_addr, u8, 0, 31);
-
-register!(xdmaps_cc_n_offset_5, XDmaPsCcNOffset5, RW, u32);
-register_bits!(xdmaps_cc_n_offset_5, endian_swap_size, u8, 28, 30);
-register_bits!(xdmaps_cc_n_offset_5, dst_cache_ctrl, u8, 25, 27);
-register_bits!(xdmaps_cc_n_offset_5, dst_prot_ctrl, u8, 22, 24);
-register_bits!(xdmaps_cc_n_offset_5, dst_burst_len, u8, 18, 21);
-register_bits!(xdmaps_cc_n_offset_5, dst_burst_size, u8, 15, 17);
-register_bit!(xdmaps_cc_n_offset_5,  dst_inc, 14);
-register_bits!(xdmaps_cc_n_offset_5, src_cache_ctrl, u8, 11, 13);
-register_bits!(xdmaps_cc_n_offset_5, src_prot_ctrl, u8, 8, 10);
-register_bits!(xdmaps_cc_n_offset_5, src_burst_len, u8, 4, 7);
-register_bits!(xdmaps_cc_n_offset_5, src_burst_size, u8, 1, 3);
-register_bit!(xdmaps_cc_n_offset_5,  src_inc, 0);
-
-register!(xdmaps_lc0_n_offset_5, XDmaPsLc0NOffset5, RW, u32);
-register_bits!(xdmaps_lc0_n_offset_5, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_lc1_n_offset_5, XDmaPsLc1NOffset5, RW, u32);
-register_bits!(xdmaps_lc1_n_offset_5, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_sa_n_offset_6, XDmaPsSaNOffset6, RW, u32);
-register_bits!(xdmaps_sa_n_offset_6, src_addr, u8, 0, 31);
-
-register!(xdmaps_da_n_offset_6, XDmaPsDaNOffset6, RW, u32);
-register_bits!(xdmaps_da_n_offset_6, dest_addr, u8, 0, 31);
-
-register!(xdmaps_cc_n_offset_6, XDmaPsCcNOffset6, RW, u32);
-register_bits!(xdmaps_cc_n_offset_6, endian_swap_size, u8, 28, 30);
-register_bits!(xdmaps_cc_n_offset_6, dst_cache_ctrl, u8, 25, 27);
-register_bits!(xdmaps_cc_n_offset_6, dst_prot_ctrl, u8, 22, 24);
-register_bits!(xdmaps_cc_n_offset_6, dst_burst_len, u8, 18, 21);
-register_bits!(xdmaps_cc_n_offset_6, dst_burst_size, u8, 15, 17);
-register_bit!(xdmaps_cc_n_offset_6,  dst_inc, 14);
-register_bits!(xdmaps_cc_n_offset_6, src_cache_ctrl, u8, 11, 13);
-register_bits!(xdmaps_cc_n_offset_6, src_prot_ctrl, u8, 8, 10);
-register_bits!(xdmaps_cc_n_offset_6, src_burst_len, u8, 4, 7);
-register_bits!(xdmaps_cc_n_offset_6, src_burst_size, u8, 1, 3);
-register_bit!(xdmaps_cc_n_offset_6,  src_inc, 0);
-
-register!(xdmaps_lc0_n_offset_6, XDmaPsLc0NOffset6, RW, u32);
-register_bits!(xdmaps_lc0_n_offset_6, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_lc1_n_offset_6, XDmaPsLc1NOffset6, RW, u32);
-register_bits!(xdmaps_lc1_n_offset_6, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_sa_n_offset_7, XDmaPsSaNOffset7, RW, u32);
-register_bits!(xdmaps_sa_n_offset_7, src_addr, u8, 0, 31);
-
-register!(xdmaps_da_n_offset_7, XDmaPsDaNOffset7, RW, u32);
-register_bits!(xdmaps_da_n_offset_7, dest_addr, u8, 0, 31);
-
-register!(xdmaps_cc_n_offset_7, XDmaPsCcNOffset7, RW, u32);
-register_bits!(xdmaps_cc_n_offset_7, endian_swap_size, u8, 28, 30);
-register_bits!(xdmaps_cc_n_offset_7, dst_cache_ctrl, u8, 25, 27);
-register_bits!(xdmaps_cc_n_offset_7, dst_prot_ctrl, u8, 22, 24);
-register_bits!(xdmaps_cc_n_offset_7, dst_burst_len, u8, 18, 21);
-register_bits!(xdmaps_cc_n_offset_7, dst_burst_size, u8, 15, 17);
-register_bit!(xdmaps_cc_n_offset_7,  dst_inc, 14);
-register_bits!(xdmaps_cc_n_offset_7, src_cache_ctrl, u8, 11, 13);
-register_bits!(xdmaps_cc_n_offset_7, src_prot_ctrl, u8, 8, 10);
-register_bits!(xdmaps_cc_n_offset_7, src_burst_len, u8, 4, 7);
-register_bits!(xdmaps_cc_n_offset_7, src_burst_size, u8, 1, 3);
-register_bit!(xdmaps_cc_n_offset_7,  src_inc, 0);
-
-register!(xdmaps_lc0_n_offset_7, XDmaPsLc0NOffset7, RW, u32);
-register_bits!(xdmaps_lc0_n_offset_7, loop_counter_iteration, u8, 0, 7);
-
-register!(xdmaps_lc1_n_offset_7, XDmaPsLc1NOffset7, RW, u32);
-register_bits!(xdmaps_lc1_n_offset_7, loop_counter_iteration, u8, 0, 7);
-
-register!(dbgstatus, DbgStatus, RW, u32);
-register_bit!(dbgstatus, dbgstatus, 0);
-
-register!(dbgcmd, DbgCmd, RW, u32);
-register_bits!(dbgcmd, dbgcmd, u8, 0, 1);
-
-register!(dbginst0, DbgInst0, RW, u32);
-register_bits!(dbginst0, instruction_byte1, u8, 24, 31);
-register_bits!(dbginst0, instruction_byte0, u8, 16, 23);
-register_bits!(dbginst0, channel_num, u8, 8, 10);
-register_bit!(dbginst0, debug_thread, 0);
-
-register!(dbginst1, DbgInst1, RW, u32);
-register_bits!(dbginst1, instruction_byte5, u8, 24, 31);
-register_bits!(dbginst1, instruction_byte4, u8, 16, 23);
-register_bits!(dbginst1, instruction_byte3, u8, 8, 10);
-register_bits!(dbginst1, instruction_byte2, u8, 0, 7);
-
-register!(cr0, Cr0, RW, u32);
-register_bits!(cr0, num_events, u8, 17, 21);
-register_bits!(cr0, num_periph_req, u8, 12, 16);
-register_bits!(cr0, num_chnls, u8, 4, 6);
-register_bit!(cr0,  mgr_ns_at_rst, 2);
-register_bit!(cr0,  boot_en, 1);
-register_bit!(cr0,  periph_req, 0);
-
-register!(cr1, Cr1, RW, u32);
-register_bits!(cr1, num_icache_lines, u8, 4, 7);
-register_bits!(cr1, icache_len, u8, 0, 2);
-
-register!(cr2, Cr2, RW, u32);
-register_bits!(cr2, boot_addr, u8, 0, 31);
-
-register!(cr3, Cr3, RW, u32);
-register_bits!(cr3, ins, u8, 0, 31);
-
-register!(cr4, Cr4, RW, u32);
-register_bits!(cr4, ins, u8, 0, 31);
-
-register!(crdn, Crdn, RW, u32);
-register_bits!(crdn, data_buffer_dep, u8, 20, 29);
-register_bits!(crdn, rd_q_dep, u8, 16, 19);
-register_bits!(crdn, rd_cap, u8, 12, 14);
-register_bits!(crdn, wr_q_dep, u8, 8, 11);
-register_bits!(crdn, wr_cap, u8, 4, 6);
-register_bits!(crdn, data_width, u8, 0, 2);
-
-register!(wd, Wd, RW, u32);
-register_bit!(wd, wd_irq_only, 0);
-
-register!(periph_id_0, PeriphId0, RW, u32);
-register_bits!(periph_id_0, part_number_0, u8, 0, 7);
-
-register!(periph_id_1, PeriphId1, RW, u32);
-register_bits!(periph_id_1, designer_0, u8, 4, 7);
-register_bits!(periph_id_1, part_number_1, u8, 0, 3);
-
-register!(periph_id_2, PeriphId2, RW, u32);
-register_bits!(periph_id_2, revision, u8, 4, 7);
-register_bits!(periph_id_2, designer_1, u8, 0, 3);
-
-register!(periph_id_3, PeriphId3, RW, u32);
-register_bit!(periph_id_3, integration_cfg, 0);
-
-register!(pcell_id_0, PCellId0, RW, u32);
-register_bits!(pcell_id_0, pcell_id_0, u8, 0, 7);
-
-register!(pcell_id_1, PCellId1, RW, u32);
-register_bits!(pcell_id_1, pcell_id_1, u8, 0, 7);
-
-register!(pcell_id_2, PCellId2, RW, u32);
-register_bits!(pcell_id_2, pcell_id_2, u8, 0, 7);
-
-register!(pcell_id_3, PCellId3, RW, u32);
-register_bits!(pcell_id_3, pcell_id_3, u8, 0, 7);
--- a/libboard_zynq/src/eth/mod.rs
+++ b/libboard_zynq/src/eth/mod.rs
@ -1,5 +1,9 @@
+use core::{
+    marker::PhantomData,
+    ops::{Deref, DerefMut},
+};
+use log::{debug, info, warn, error};
 use libregister::*;
-use crate::println;
 use super::slcr;
 use super::clocks::Clocks;

@ -18,15 +22,133 @@ 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)]
+#[repr(C, align(0x20))]
+pub struct Buffer(pub [u8; MTU]);
+
+impl Buffer {
+    pub const fn new() -> Self {
+        Buffer([0; MTU])
+    }
+}
+
+impl Deref for Buffer {
+    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
+                slcr::GemClkCtrl::zeroed()
+                    .clkact(true)
+                    .srcsel(slcr::PllSource::IoPll)
+                    .divisor(divisor0 as u8)
+                    .divisor1(divisor1 as u8)
+            );
+            // Enable gem0 recv clock
+            slcr.gem0_rclk_ctrl.write(
+                // 0x0000_0801
+                slcr::RclkCtrl::zeroed()
+                    .clkact(true)
+            );
+        });
+    }
+
+    fn regs() -> &'static mut regs::RegisterBlock {
+        regs::RegisterBlock::gem0()
+    }
+}
+
+/// second Gigabit Ethernet peripheal
+pub struct Gem1;
+
+impl Gem for Gem1 {
+    fn setup_clock(tx_clock: u32) {
+        let (divisor0, divisor1) = calculate_tx_divisors(tx_clock);
+
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.gem1_clk_ctrl.write(
+                slcr::GemClkCtrl::zeroed()
+                    .clkact(true)
+                    .srcsel(slcr::PllSource::IoPll)
+                    .divisor(divisor0 as u8)
+                    .divisor1(divisor1 as u8)
+            );
+            // Enable gem1 recv clock
+            slcr.gem1_rclk_ctrl.write(
+                // 0x0000_0801
+                slcr::RclkCtrl::zeroed()
+                    .clkact(true)
+            );
+        });
+    }
+
+    fn regs() -> &'static mut regs::RegisterBlock {
+        regs::RegisterBlock::gem1()
+    }
+}
+
+fn calculate_tx_divisors(tx_clock: u32) -> (u8, u8) {
+    let io_pll = Clocks::get().io;
+    let target = (tx_clock - 1 + io_pll) / tx_clock;
+
+    let mut best = None;
+    let mut best_error = 0;
+    for divisor0 in 1..63 {
+        for divisor1 in 1..63 {
+            let current = (divisor0 as u32) * (divisor1 as u32);
+            let error = if current > target {
+                current - target
+            } else {
+                target - current
+            };
+            if best.is_none() || best_error > error {
+                best = Some((divisor0, divisor1));
+                best_error = error;
+            }
+        }
+    }
+    let result = best.unwrap();
+    debug!("Eth TX clock for {}: {} / {} / {} = {}",
+           tx_clock, io_pll,
+           result.0, result.1,
+           io_pll / result.0 as u32 / result.1 as u32
+    );
+    result
+}
+
+pub struct Eth<GEM: Gem, RX, TX> {
    rx: RX,
    tx: TX,
-    inner: EthInner<'r>,
+    inner: EthInner<GEM>,
    phy: Phy,
 }

-impl<'r> Eth<'r, (), ()> {
-    pub fn default(macaddr: [u8; 6]) -> Self {
+impl Eth<Gem0, (), ()> {
+    pub fn eth0(macaddr: [u8; 6]) -> Self {
        slcr::RegisterBlock::unlocked(|slcr| {
            // Manual example: 0x0000_1280
            // MDIO
@ -102,48 +224,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)
            );
@ -158,22 +280,26 @@ impl<'r> Eth<'r, (), ()> {
    }

    pub fn gem0(macaddr: [u8; 6]) -> Self {
-        Self::setup_gem0_clock(TX_1000);
-
-        let regs = regs::RegisterBlock::gem0();
-        Self::from_regs(regs, macaddr)
+        Self::gem_common(macaddr)
    }
+}
+
+
+impl Eth<Gem1, (), ()> {
+    // TODO: Add a `eth1()`

    pub fn gem1(macaddr: [u8; 6]) -> Self {
-        Self::setup_gem1_clock(TX_1000);
-
-        let regs = regs::RegisterBlock::gem1();
-        Self::from_regs(regs, macaddr)
+        Self::gem_common(macaddr)
    }
+}
+
+
+impl<GEM: Gem> Eth<GEM, (), ()> {
+    fn gem_common(macaddr: [u8; 6]) -> Self {
+        GEM::setup_clock(TX_1000);

-    fn from_regs(regs: &'r mut regs::RegisterBlock, macaddr: [u8; 6]) -> Self {
        let mut inner = EthInner {
-            regs,
+            gem: PhantomData,
            link: None,
        };
        inner.init();
@ -192,98 +318,52 @@ impl<'r> Eth<'r, (), ()> {
    }
 }

-impl<'r, RX, TX> Eth<'r, RX, TX> {
-    pub fn setup_gem0_clock(tx_clock: u32) {
-        let io_pll = Clocks::get().io;
-        let d0 = ((tx_clock - 1 + io_pll) / tx_clock).max(1).min(63);
-        let d1 = (io_pll / tx_clock / d0).max(1).min(63);
-
-        slcr::RegisterBlock::unlocked(|slcr| {
-            slcr.gem0_clk_ctrl.write(
-                // 0x0050_0801: 8, 5: 100 Mb/s
-                // ...: 8, 1: 1000 Mb/s
-                slcr::GemClkCtrl::zeroed()
-                    .clkact(true)
-                    .srcsel(slcr::PllSource::IoPll)
-                    .divisor(d0 as u8)
-                    .divisor1(d1 as u8)
-            );
-            // Enable gem0 recv clock
-            slcr.gem0_rclk_ctrl.write(
-                // 0x0000_0801
-                slcr::RclkCtrl::zeroed()
-                    .clkact(true)
-            );
-        });
-    }
-
-    pub fn setup_gem1_clock(tx_clock: u32) {
-        let io_pll = Clocks::get().io;
-        let d0 = ((tx_clock - 1 + io_pll) / tx_clock).max(1).min(63);
-        let d1 = (io_pll / tx_clock / d0).max(1).min(63);
-
-        slcr::RegisterBlock::unlocked(|slcr| {
-            slcr.gem1_clk_ctrl.write(
-                slcr::GemClkCtrl::zeroed()
-                    .clkact(true)
-                    .srcsel(slcr::PllSource::IoPll)
-                    .divisor(d0 as u8)
-                    .divisor1(d1 as u8)
-            );
-            // Enable gem1 recv clock
-            slcr.gem1_rclk_ctrl.write(
-                // 0x0000_0801
-                slcr::RclkCtrl::zeroed()
-                    .clkact(true)
-            );
-        });
-    }
-
-    pub fn start_rx<'rx>(self, rx_list: &'rx mut [rx::DescEntry], rx_buffers: &'rx mut [[u8; MTU]]) -> Eth<'r, rx::DescList<'rx>, TX> {
+impl<GEM: Gem, RX, TX> Eth<GEM, RX, TX> {
+    pub fn start_rx(self, rx_size: usize) -> Eth<GEM, rx::DescList, TX> {
        let new_self = Eth {
-            rx: rx::DescList::new(rx_list, rx_buffers),
+            rx: rx::DescList::new(rx_size),
            tx: self.tx,
            inner: self.inner,
            phy: self.phy,
        };
        let list_addr = new_self.rx.list_addr();
        assert!(list_addr & 0b11 == 0);
-        new_self.inner.regs.rx_qbar.write(
+        GEM::regs().rx_qbar.write(
            regs::RxQbar::zeroed()
                .rx_q_baseaddr(list_addr >> 2)
        );
-        new_self.inner.regs.net_ctrl.modify(|_, w|
+        GEM::regs().net_ctrl.modify(|_, w|
            w.rx_en(true)
        );
        new_self
    }

-    pub fn start_tx<'tx>(self, tx_list: &'tx mut [tx::DescEntry], tx_buffers: &'tx mut [[u8; MTU]]) -> Eth<'r, RX, tx::DescList<'tx>> {
+    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_list, tx_buffers),
+            tx: tx::DescList::new(tx_size),
            inner: self.inner,
            phy: self.phy,
        };
        let list_addr = &new_self.tx.list_addr();
        assert!(list_addr & 0b11 == 0);
-        new_self.inner.regs.tx_qbar.write(
+        GEM::regs().tx_qbar.write(
            regs::TxQbar::zeroed()
                .tx_q_baseaddr(list_addr >> 2)
        );
-        new_self.inner.regs.net_ctrl.modify(|_, w|
+        GEM::regs().net_ctrl.modify(|_, w|
            w.tx_en(true)
        );
        new_self
    }
 }

-impl<'r, 'rx, TX> Eth<'r, rx::DescList<'rx>, TX> {
+impl<GEM: Gem, TX> Eth<GEM, rx::DescList, TX> {
    pub fn recv_next<'s: 'p, 'p>(&'s mut self) -> Result<Option<rx::PktRef<'p>>, rx::Error> {
-        let status = self.inner.regs.rx_status.read();
+        let status = GEM::regs().rx_status.read();
        if status.hresp_not_ok() {
            // Clear
-            self.inner.regs.rx_status.write(
+            GEM::regs().rx_status.write(
                regs::RxStatus::zeroed()
                    .hresp_not_ok(true)
            );
@ -291,7 +371,7 @@ impl<'r, 'rx, TX> Eth<'r, rx::DescList<'rx>, TX> {
        }
        if status.rx_overrun() {
            // Clear
-            self.inner.regs.rx_status.write(
+            GEM::regs().rx_status.write(
                regs::RxStatus::zeroed()
                    .rx_overrun(true)
            );
@ -299,7 +379,7 @@ impl<'r, 'rx, TX> Eth<'r, rx::DescList<'rx>, TX> {
        }
        if status.buffer_not_avail() {
            // Clear
-            self.inner.regs.rx_status.write(
+            GEM::regs().rx_status.write(
                regs::RxStatus::zeroed()
                    .buffer_not_avail(true)
            );
@ -311,7 +391,7 @@ impl<'r, 'rx, TX> Eth<'r, rx::DescList<'rx>, TX> {
            match result {
                Ok(None) => {
                    // No packet, clear status bit
-                    self.inner.regs.rx_status.write(
+                    GEM::regs().rx_status.write(
                        regs::RxStatus::zeroed()
                            .frame_recd(true)
                    );
@ -326,15 +406,15 @@ impl<'r, 'rx, TX> Eth<'r, rx::DescList<'rx>, TX> {
    }
 }

-impl<'r, 'tx, RX> Eth<'r, RX, tx::DescList<'tx>> {
+impl<GEM: Gem, RX> Eth<GEM, RX, tx::DescList> {
    pub fn send<'s: 'p, 'p>(&'s mut self, length: usize) -> Option<tx::PktRef<'p>> {
-        self.tx.send(self.inner.regs, length)
+        self.tx.send(GEM::regs(), length)
    }
 }

-impl<'r, 'rx, 'tx: 'a, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescList<'rx>, tx::DescList<'tx>> {
+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, 'tx>;
+    type TxToken = tx::Token<'a>;

    fn capabilities(&self) -> smoltcp::phy::DeviceCapabilities {
        use smoltcp::phy::{DeviceCapabilities, ChecksumCapabilities, Checksum};
@ -346,6 +426,7 @@ impl<'r, 'rx, 'tx: 'a, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescLis

        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
@ -355,7 +436,7 @@ impl<'r, 'rx, 'tx: 'a, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescLis
        match self.rx.recv_next() {
            Ok(Some(pktref)) => {
                let tx_token = tx::Token {
-                    regs: self.inner.regs,
+                    regs: GEM::regs(),
                    desc_list: &mut self.tx,
                };
                Some((pktref, tx_token))
@ -365,7 +446,7 @@ impl<'r, 'rx, 'tx: 'a, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescLis
                None
            }
            Err(e) => {
-                println!("eth recv error: {:?}", e);
+                error!("eth recv error: {:?}", e);
                None
            }
        }
@ -373,33 +454,32 @@ impl<'r, 'rx, 'tx: 'a, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescLis

    fn transmit(&'a mut self) -> Option<Self::TxToken> {
        Some(tx::Token {
-            regs: self.inner.regs,
+            regs: GEM::regs(),
            desc_list: &mut self.tx,
        })
    }
-
 }


-struct EthInner<'r> {
-    regs: &'r mut regs::RegisterBlock,
+struct EthInner<GEM: Gem> {
+    gem: PhantomData<GEM>,
    link: Option<phy::Link>,
 }

-impl<'r> EthInner<'r> {
+impl<GEM: Gem> EthInner<GEM> {
    fn init(&mut self) {
        // Clear the Network Control register.
-        self.regs.net_ctrl.write(regs::NetCtrl::zeroed());
-        self.regs.net_ctrl.write(regs::NetCtrl::zeroed().clear_stat_regs(true));
+        GEM::regs().net_ctrl.write(regs::NetCtrl::zeroed());
+        GEM::regs().net_ctrl.write(regs::NetCtrl::zeroed().clear_stat_regs(true));
        // Clear the Status registers.
-        self.regs.rx_status.write(
+        GEM::regs().rx_status.write(
            regs::RxStatus::zeroed()
                .buffer_not_avail(true)
                .frame_recd(true)
                .rx_overrun(true)
                .hresp_not_ok(true)
        );
-        self.regs.tx_status.write(
+        GEM::regs().tx_status.write(
            regs::TxStatus::zeroed()
                .used_bit_read(true)
                .collision(true)
@ -413,7 +493,7 @@ impl<'r> EthInner<'r> {
                .hresp_not_ok(true)
        );
        // Disable all interrupts.
-        self.regs.intr_dis.write(
+        GEM::regs().intr_dis.write(
            regs::IntrDis::zeroed()
                .mgmt_done(true)
                .rx_complete(true)
@ -443,29 +523,32 @@ impl<'r> EthInner<'r> {
                .tsu_sec_incr(true)
        );
        // Clear the buffer queues.
-        self.regs.rx_qbar.write(
+        GEM::regs().rx_qbar.write(
            regs::RxQbar::zeroed()
        );
-        self.regs.tx_qbar.write(
+        GEM::regs().tx_qbar.write(
            regs::TxQbar::zeroed()
        );
    }

    fn configure(&mut self, macaddr: [u8; 6]) {
        let clocks = Clocks::get();
-        let 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?)
-                .copy_all(true)
+                .rx_1536_byte_frames(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
@ -473,24 +556,25 @@ impl<'r> EthInner<'r> {
        );

        let macaddr_msbs =
-            (u16::from(macaddr[0]) << 8) |
-            u16::from(macaddr[1]);
+            (u16::from(macaddr[5]) << 8) |
+            u16::from(macaddr[4]);
        let macaddr_lsbs =
-            (u32::from(macaddr[2]) << 24) |
-            (u32::from(macaddr[3]) << 16) |
-            (u32::from(macaddr[4]) << 8) |
-            u32::from(macaddr[5]);
-        self.regs.spec_addr1_top.write(
-            regs::SpecAddrTop::zeroed()
-                .addr_msbs(macaddr_msbs)
-        );
-        self.regs.spec_addr1_bot.write(
+            (u32::from(macaddr[3]) << 24) |
+            (u32::from(macaddr[2]) << 16) |
+            (u32::from(macaddr[1]) << 8) |
+            u32::from(macaddr[0]);
+        // writing to bot would disable the specific address
+        GEM::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)
+        );

-
-        self.regs.dma_cfg.write(
+        GEM::regs().dma_cfg.write(
            regs::DmaCfg::zeroed()
                // 1536 bytes
                .ahb_mem_rx_buf_size((MTU >> 6) as u8)
@ -506,7 +590,7 @@ impl<'r> EthInner<'r> {
                .ahb_fixed_burst_len(0x10)
        );

-        self.regs.net_ctrl.write(
+        GEM::regs().net_ctrl.write(
            regs::NetCtrl::zeroed()
                .mgmt_port_en(true)
        );
@ -514,7 +598,7 @@ impl<'r> EthInner<'r> {


    fn wait_phy_idle(&self) {
-        while !self.regs.net_status.read().phy_mgmt_idle() {}
+        while !GEM::regs().net_status.read().phy_mgmt_idle() {}
    }


@ -531,25 +615,23 @@ impl<'r> EthInner<'r> {
        if self.link != link {
            match &link {
                Some(link) => {
-                    println!("eth: got {:?}", link);
+                    info!("eth: got {:?}", link);

-                    use phy::LinkSpeed::*;
+                    use phy::{LinkDuplex::Full, LinkSpeed::*};
                    let txclock = match link.speed {
                        S10 => TX_10,
                        S100 => TX_100,
                        S1000 => TX_1000,
                    };
-                    Eth::<(), ()>::setup_gem0_clock(txclock);
-                    /* .full_duplex(false) doesn't work even if
-                       half duplex has been negotiated. */
-                    self.regs.net_cfg.modify(|_, w| w
-                        .full_duplex(true)
+                    GEM::setup_clock(txclock);
+                    GEM::regs().net_cfg.modify(|_, w| w
+                        .full_duplex(link.duplex == Full)
                        .gige_en(link.speed == S1000)
                        .speed(link.speed != S10)
                    );
                }
                None => {
-                    println!("eth: link lost");
+                    warn!("eth: link lost");
                    phy.modify_control(self, |control|
                                       control.set_autoneg_enable(true)
                                       .set_restart_autoneg(true)
@ -562,10 +644,10 @@ impl<'r> EthInner<'r> {
    }
 }

-impl<'r> PhyAccess for EthInner<'r> {
+impl<GEM: Gem> PhyAccess for EthInner<GEM> {
    fn read_phy(&mut self, addr: u8, reg: u8) -> u16 {
        self.wait_phy_idle();
-        self.regs.phy_maint.write(
+        GEM::regs().phy_maint.write(
            regs::PhyMaint::zeroed()
                .clause_22(true)
                .operation(regs::PhyOperation::Read)
@ -574,12 +656,12 @@ impl<'r> PhyAccess for EthInner<'r> {
                .must_10(0b10)
        );
        self.wait_phy_idle();
-        self.regs.phy_maint.read().data()
+        GEM::regs().phy_maint.read().data()
    }

    fn write_phy(&mut self, addr: u8, reg: u8, data: u16) {
        self.wait_phy_idle();
-        self.regs.phy_maint.write(
+        GEM::regs().phy_maint.write(
            regs::PhyMaint::zeroed()
                .clause_22(true)
                .operation(regs::PhyOperation::Write)
--- a/libboard_zynq/src/eth/phy/extended_status.rs
+++ b/libboard_zynq/src/eth/phy/extended_status.rs
@ -1,59 +0,0 @@
-use bit_field::BitField;
-use super::{PhyRegister, Link, LinkDuplex, LinkSpeed};
-
-#[derive(Clone, Copy, Debug)]
-/// 1000Base-T Extended Status Register
-pub struct ExtendedStatus(pub u16);
-
-impl ExtendedStatus {
-    pub fn cap_1000base_t_half(&self) -> bool {
-        self.0.get_bit(12)
-    }
-    pub fn cap_1000base_t_full(&self) -> bool {
-        self.0.get_bit(13)
-    }
-    pub fn cap_1000base_x_half(&self) -> bool {
-        self.0.get_bit(14)
-    }
-    pub fn cap_1000base_x_full(&self) -> bool {
-        self.0.get_bit(12)
-    }
-
-    pub fn get_link(&self) -> Option<Link> {
-        if self.cap_1000base_t_half() {
-            Some(Link {
-                speed: LinkSpeed::S1000,
-                duplex: LinkDuplex::Half,
-            })
-        } else if self.cap_1000base_t_full() {
-            Some(Link {
-                speed: LinkSpeed::S1000,
-                duplex: LinkDuplex::Full,
-            })
-        } else if self.cap_1000base_x_half() {
-            Some(Link {
-                speed: LinkSpeed::S1000,
-                duplex: LinkDuplex::Half,
-            })
-        } else if self.cap_1000base_x_full() {
-            Some(Link {
-                speed: LinkSpeed::S1000,
-                duplex: LinkDuplex::Full,
-            })
-        } else {
-            None
-        }
-    }
-}
-
-impl PhyRegister for ExtendedStatus {
-    fn addr() -> u8 {
-        0xF
-    }
-}
-
-impl From<u16> for ExtendedStatus {
-    fn from(value: u16) -> Self {
-        ExtendedStatus(value)
-    }
-}
--- a/libboard_zynq/src/eth/phy/mod.rs
+++ b/libboard_zynq/src/eth/phy/mod.rs
@ -2,10 +2,10 @@ pub mod id;
 use id::{identify_phy, PhyIdentifier};
 mod status;
 pub use status::Status;
-mod extended_status;
-pub use extended_status::ExtendedStatus;
 mod control;
 pub use control::Control;
+mod pssr;
+pub use pssr::PSSR;

 #[derive(Clone, Debug, PartialEq)]
 pub struct Link {
@ -39,43 +39,36 @@ pub struct Phy {

 #[derive(Clone, Copy)]
 pub enum PhyDevice {
-    Marvel88E1116R,
+    Marvell88E1116R,
    Rtl8211E,
 }

-const OUI_MARVEL: u32 = 0x005043;
+const OUI_MARVELL: u32 = 0x005043;
 const OUI_REALTEK: u32 = 0x000732;

 impl Phy {
    /// Probe all addresses on MDIO for a known PHY
    pub fn find<PA: PhyAccess>(pa: &mut PA) -> Option<Phy> {
-        for addr in 1..32 {
-            let device = match identify_phy(pa, addr) {
+        (1..32).filter_map(|addr| {
+            match identify_phy(pa, addr) {
                Some(PhyIdentifier {
-                    oui: OUI_MARVEL,
+                    oui: OUI_MARVELL,
                    model: 36,
                    ..
-                }) => Some(PhyDevice::Marvel88E1116R),
+                }) => Some(PhyDevice::Marvell88E1116R),
                Some(PhyIdentifier {
                    oui: OUI_REALTEK,
                    model: 0b010001,
                    rev: 0b0101,
                }) => Some(PhyDevice::Rtl8211E),
                _ => None,
-            };
-            match device {
-                Some(device) =>
-                    return Some(Phy { addr, device }),
-                None => {}
-            }
-        }
-
-        None
+            }.map(|device| Phy { addr, device })
+        }).next()
    }

    pub fn name(&self) -> &'static str {
        match self.device {
-            PhyDevice::Marvel88E1116R => &"Marvel 88E1116R",
+            PhyDevice::Marvell88E1116R => &"Marvell 88E1116R",
            PhyDevice::Rtl8211E => &"RTL8211E",
        }
    }
@ -120,12 +113,8 @@ impl Phy {
        if !status.link_status() {
            None
        } else if status.cap_1000base_t_extended_status() {
-            let ext_status: ExtendedStatus = self.read_reg(pa);
-            if let Some(link) = ext_status.get_link() {
-                Some(link)
-            } else {
-                status.get_link()
-            }
+            let phy_status: PSSR = self.read_reg(pa);
+            phy_status.get_link()
        } else {
            status.get_link()
        }
--- a/libboard_zynq/src/eth/phy/pssr.rs
+++ b/libboard_zynq/src/eth/phy/pssr.rs
@ -0,0 +1,52 @@
+use bit_field::BitField;
+use super::{PhyRegister, Link, LinkDuplex, LinkSpeed};
+
+#[derive(Clone, Copy, Debug)]
+/// PHY-Specific Status Register
+pub struct PSSR(pub u16);
+
+impl PSSR {
+    pub fn link(&self) -> bool {
+        self.0.get_bit(10)
+    }
+
+    pub fn duplex(&self) -> LinkDuplex {
+        if self.0.get_bit(13) {
+            LinkDuplex::Full
+        } else {
+            LinkDuplex::Half
+        }
+    }
+
+    pub fn speed(&self) -> Option<LinkSpeed> {
+        match self.0.get_bits(14..=15) {
+            0b00 => Some(LinkSpeed::S10),
+            0b01 => Some(LinkSpeed::S100),
+            0b10 => Some(LinkSpeed::S1000),
+            _ => None,
+        }
+    }
+
+    pub fn get_link(&self) -> Option<Link> {
+        if self.link() {
+            Some(Link {
+                speed: self.speed()?,
+                duplex: self.duplex(),
+            })
+        } else {
+            None
+        }
+    }
+}
+
+impl PhyRegister for PSSR {
+    fn addr() -> u8 {
+        0x11
+    }
+}
+
+impl From<u16> for PSSR {
+    fn from(value: u16) -> Self {
+        PSSR(value)
+    }
+}
--- 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 libregister::{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,8 @@ pub struct RegisterBlock {
    pub design_cfg5: RO<u32>,
 }

-impl RegisterBlock {
-    const GEM0: *mut Self = 0xE000B000 as *mut _;
-    const GEM1: *mut Self = 0xE000C000 as *mut _;
-
-    pub fn gem0() -> &'static mut Self {
-        unsafe { &mut *Self::GEM0 }
-    }
-
-    pub fn gem1() -> &'static mut Self {
-        unsafe { &mut *Self::GEM1 }
-    }
-}
+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 alloc::{vec, vec::Vec};
+use libcortex_a9::{asm::*, cache::*, UncachedSlice};
 use libregister::*;
-use super::MTU;
+use super::Buffer;

 #[derive(Debug)]
 pub enum Error {
@ -53,18 +55,22 @@ register_bit!(desc_word1, multi_hash_match, 30);
 register_bit!(desc_word1, global_broadcast, 31);

 #[repr(C)]
-pub struct DescList<'a> {
-    list: &'a mut [DescEntry],
-    buffers: &'a mut [[u8; MTU]],
+pub struct DescList {
+    list: UncachedSlice<DescEntry>,
+    buffers: Vec<Buffer>,
    next: usize,
 }

-impl<'a> DescList<'a> {
-    pub fn new(list: &'a mut [DescEntry], buffers: &'a mut [[u8; MTU]]) -> Self {
+impl DescList {
+    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()
@ -75,16 +81,22 @@ 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 {
-            // Shorten the list of descriptors to the required number.
-            list: &mut list[0..=last],
+            list,
            buffers,
            next: 0,
        }
    }

+    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
    }
@ -92,9 +104,24 @@ 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 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;
@ -123,6 +150,7 @@ pub struct PktRef<'a> {
 impl<'a> Drop for PktRef<'a> {
    fn drop(&mut self) {
        self.entry.word0.modify(|_, w| w.used(false));
+        dmb();
    }
 }

--- a/libboard_zynq/src/eth/tx.rs
+++ b/libboard_zynq/src/eth/tx.rs
@ -1,6 +1,8 @@
 use core::ops::{Deref, DerefMut};
+use alloc::{vec, vec::Vec};
+use libcortex_a9::{cache::dcc_slice, UncachedSlice};
 use libregister::*;
-use super::{MTU, regs};
+use super::{Buffer, regs};

 /// Descriptor entry
 #[repr(C, align(0x08))]
@ -40,14 +42,18 @@ impl DescEntry {
 pub const DESCS: usize = 8;

 #[repr(C)]
-pub struct DescList<'a> {
-    list: &'a mut [DescEntry],
-    buffers: &'a mut [[u8; MTU]],
+pub struct DescList {
+    list: UncachedSlice<DescEntry>,
+    buffers: Vec<Buffer>,
    next: usize,
 }

-impl<'a> DescList<'a> {
-    pub fn new(list: &'a mut [DescEntry], buffers: &'a mut [[u8; MTU]]) -> Self {
+impl DescList {
+    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
@ -57,7 +63,7 @@ impl<'a> DescList<'a> {

        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()
@ -73,13 +79,16 @@ impl<'a> DescList<'a> {
        }

        DescList {
-            // Shorten the list of descriptors to the required number.
-            list: &mut list[0..=last],
+            list,
            buffers,
            next: 0,
        }
    }

+    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
    }
@ -119,12 +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() {
-            self.regs.net_ctrl.modify(|_, w|
-                                      w.start_tx(true)
-            );
-        }
+        // Start the TX engine
+        self.regs.net_ctrl.modify(|_, w| w.start_tx(true));
    }
 }

@ -142,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>
    {
@ -155,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());
-                // TODO: on result.is_err() don;t send
-                drop(pktref);
-                result
+                f(pktref.deref_mut())
            }
        }
    }
--- a/libboard_zynq/src/flash/bytes.rs
+++ b/libboard_zynq/src/flash/bytes.rs
@ -1,41 +0,0 @@
-pub trait BytesTransferExt: Sized {
-    // Turn u32 into u8
-    fn bytes_transfer(self) -> BytesTransfer<Self>
-    where
-        Self: Iterator<Item = u32>;
-}
-
-impl<I: Iterator<Item = u32>> BytesTransferExt for I {
-    // Turn u32 into u8
-    fn bytes_transfer(self) -> BytesTransfer<Self> {
-        BytesTransfer {
-            iter: self,
-            shift: 0,
-            word: 0,
-        }
-    }
-}
-
-pub struct BytesTransfer<I: Iterator<Item = u32> + Sized> {
-    iter: I,
-    shift: u8,
-    word: u32,
-}
-
-impl<I: Iterator<Item = u32> + Sized> Iterator for BytesTransfer<I> {
-    type Item = u8;
-
-    fn next(&mut self) -> Option<u8> {
-        if self.shift > 0 {
-            self.shift -= 8;
-            Some((self.word >> self.shift) as u8)
-        } else {
-            self.iter.next()
-                .and_then(|word| {
-                    self.shift = 32;
-                    self.word = word;
-                    self.next()
-                })
-        }
-    }
-}
--- a/libboard_zynq/src/flash/mod.rs
+++ b/libboard_zynq/src/flash/mod.rs
@ -1,507 +0,0 @@
-//! Quad-SPI Flash Controller
-
-use crate::{print, println};
-use core::marker::PhantomData;
-use libregister::{RegisterR, RegisterW, RegisterRW};
-use super::slcr;
-use super::clocks::source::{IoPll, ClockSource};
-
-mod regs;
-mod bytes;
-pub use bytes::{BytesTransferExt, BytesTransfer};
-mod spi_flash_register;
-use spi_flash_register::*;
-mod transfer;
-use transfer::Transfer;
-
-const FLASH_BAUD_RATE: u32 = 50_000_000;
-/// 16 MB
-pub const SINGLE_CAPACITY: u32 = 0x1000000;
-pub const SECTOR_SIZE: u32 = 0x10000;
-pub const PAGE_SIZE: u32 = 0x100;
-
-/// Instruction: Read Identification
-const INST_RDID: u8 = 0x9F;
-const INST_READ: u8 = 0x03;
-/// Instruction: Write Disable
-const INST_WRDI: u8 = 0x04;
-/// Instruction: Write Enable
-const INST_WREN: u8 = 0x06;
-/// Instruction: Program page
-const INST_PP: u8 = 0x02;
-/// Instruction: Sector Erase
-const INST_SE: u8 = 0xD8;
-/// Instruction: Erase 4K Block
-const INST_BE_4K: u8 = 0x20;
-
-#[derive(Clone)]
-pub enum SpiWord {
-    W8(u8),
-    W16(u16),
-    W24(u32),
-    W32(u32),
-}
-
-impl From<u8> for SpiWord {
-    fn from(x: u8) -> Self {
-        SpiWord::W8(x)
-    }
-}
-
-impl From<u16> for SpiWord {
-    fn from(x: u16) -> Self {
-        SpiWord::W16(x)
-    }
-}
-
-impl From<u32> for SpiWord {
-    fn from(x: u32) -> Self {
-        SpiWord::W32(x)
-    }
-}
-
-/// Memory-mapped mode
-pub struct LinearAddressing;
-/// Manual I/O mode
-pub struct Manual;
-
-/// Flash Interface Driver
-///
-/// For 2x Spansion S25FL128SAGMFIR01
-pub struct Flash<MODE> {
-    regs: &'static mut regs::RegisterBlock,
-    _mode: PhantomData<MODE>,
-}
-
-impl<MODE> Flash<MODE> {
-    fn transition<TO>(self) -> Flash<TO> {
-        Flash {
-            regs: self.regs,
-            _mode: PhantomData,
-        }
-    }
-
-    fn disable_interrupts(&mut self) {
-        self.regs.intr_dis.write(
-            regs::IntrDis::zeroed()
-                .rx_overflow(true)
-                .tx_fifo_not_full(true)
-                .tx_fifo_full(true)
-                .rx_fifo_not_empty(true)
-                .rx_fifo_full(true)
-                .tx_fifo_underflow(true)
-        );
-    }
-
-    fn enable_interrupts(&mut self) {
-        self.regs.intr_en.write(
-            regs::IntrEn::zeroed()
-                .rx_overflow(true)
-                .tx_fifo_not_full(true)
-                .tx_fifo_full(true)
-                .rx_fifo_not_empty(true)
-                .rx_fifo_full(true)
-                .tx_fifo_underflow(true)
-        );
-    }
-
-    fn clear_rx_fifo(&self) {
-        while self.regs.intr_status.read().rx_fifo_not_empty() {
-            let _ = self.regs.rx_data.read();
-        }
-    }
-
-    fn clear_interrupt_status(&mut self) {
-        self.regs.intr_status.write(
-            regs::IntrStatus::zeroed()
-                .rx_overflow(true)
-                .tx_fifo_underflow(true)
-        );
-    }
-
-    fn wait_tx_fifo_flush(&mut self) {
-        self.regs.config.modify(|_, w| w.man_start_com(true));
-        while !self.regs.intr_status.read().tx_fifo_not_full() {}
-    }
-}
-
-impl Flash<()> {
-    pub fn new(clock: u32) -> Self {
-        Self::enable_clocks(clock);
-        Self::setup_signals();
-        Self::reset();
-
-        let regs = regs::RegisterBlock::qspi();
-        let mut flash = Flash { regs, _mode: PhantomData };
-        flash.configure((FLASH_BAUD_RATE - 1 + clock) / FLASH_BAUD_RATE);
-        flash
-    }
-
-    /// typical: `200_000_000` Hz
-    fn enable_clocks(clock: u32) {
-        let io_pll = IoPll::freq();
-        let divisor = ((clock - 1 + io_pll) / clock)
-            .max(1).min(63) as u8;
-
-        slcr::RegisterBlock::unlocked(|slcr| {
-            slcr.lqspi_clk_ctrl.write(
-                slcr::LqspiClkCtrl::zeroed()
-                    .src_sel(slcr::PllSource::IoPll)
-                    .divisor(divisor)
-                    .clkact(true)
-            );
-        });
-    }
-
-    fn setup_signals() {
-        slcr::RegisterBlock::unlocked(|slcr| {
-            // 1. Configure MIO pin 1 for chip select 0 output.
-            slcr.mio_pin_01.write(
-                slcr::MioPin01::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-                    .pullup(true)
-            );
-
-            // Configure MIO pins 2 through 5 for I/O.
-            slcr.mio_pin_02.write(
-                slcr::MioPin02::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-            slcr.mio_pin_03.write(
-                slcr::MioPin03::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-            slcr.mio_pin_04.write(
-                slcr::MioPin04::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-            slcr.mio_pin_05.write(
-                slcr::MioPin05::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-
-            // 3. Configure MIO pin 6 for serial clock 0 output.
-            slcr.mio_pin_06.write(
-                slcr::MioPin06::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-
-            // Option: Add Second Device Chip Select
-            // 4. Configure MIO pin 0 for chip select 1 output.
-            slcr.mio_pin_00.write(
-                slcr::MioPin00::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-                    .pullup(true)
-            );
-
-            // Option: Add Second Serial Clock
-            // 5. Configure MIO pin 9 for serial clock 1 output.
-            slcr.mio_pin_09.write(
-                slcr::MioPin09::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-
-            // Option: Add 4-bit Data
-            // 6. Configure MIO pins 10 through 13 for I/O.
-            slcr.mio_pin_10.write(
-                slcr::MioPin10::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-            slcr.mio_pin_11.write(
-                slcr::MioPin11::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-            slcr.mio_pin_12.write(
-                slcr::MioPin12::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-            slcr.mio_pin_13.write(
-                slcr::MioPin13::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-
-            // Option: Add Feedback Output Clock
-            // 7. Configure MIO pin 8 for feedback clock.
-            slcr.mio_pin_08.write(
-                slcr::MioPin08::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-            );
-        });
-    }
-
-    fn reset() {
-        slcr::RegisterBlock::unlocked(|slcr| {
-            slcr.lqspi_rst_ctrl.write(
-                slcr::LqspiRstCtrl::zeroed()
-                    .ref_rst(true)
-                    .cpu1x_rst(true)
-            );
-            slcr.lqspi_rst_ctrl.write(
-                slcr::LqspiRstCtrl::zeroed()
-            );
-        });
-    }
-
-    fn configure(&mut self, divider: u32) {
-        // Disable
-        self.regs.enable.write(
-            regs::Enable::zeroed()
-        );
-        self.disable_interrupts();
-        self.regs.lqspi_cfg.write(
-            regs::LqspiCfg::zeroed()
-        );
-        self.clear_rx_fifo();
-        self.clear_interrupt_status();
-
-        // for a baud_rate_div=1 LPBK_DLY_ADJ would be required
-        let mut baud_rate_div = 2u32;
-        while baud_rate_div < 7 && 2u32.pow(1 + baud_rate_div) < divider {
-            baud_rate_div += 1;
-        }
-
-        self.regs.config.write(regs::Config::zeroed()
-            .baud_rate_div(baud_rate_div as u8)
-            .mode_sel(true)
-            .leg_flsh(true)
-            .holdb_dr(true)
-            // 32 bits TX FIFO width
-            .fifo_width(0b11)
-        );
-
-        // Initialize RX/TX pipes thresholds
-        unsafe {
-            self.regs.rx_thres.write(1);
-            self.regs.tx_thres.write(1);
-        }
-    }
-
-    pub fn linear_addressing_mode(self) -> Flash<LinearAddressing> {
-        // Set manual start enable to auto mode.
-        // Assert the chip select.
-        self.regs.config.modify(|_, w| w
-            .man_start_en(false)
-            .pcs(false)
-            .manual_cs(false)
-        );
-
-        self.regs.lqspi_cfg.write(regs::LqspiCfg::zeroed()
-            // Quad I/O Fast Read
-            .inst_code(0xEB)
-            .mode_bits(0xFF)
-            .dummy_byte(0x2)
-            .mode_en(true)
-            // 2 devices
-            .two_mem(true)
-            .u_page(false)
-            // Linear Addressing Mode
-            .lq_mode(true)
-        );
-
-        self.regs.enable.write(
-            regs::Enable::zeroed()
-                .spi_en(true)
-        );
-
-        self.transition()
-    }
-
-    pub fn manual_mode(self, chip_index: usize) -> Flash<Manual> {
-        self.regs.config.modify(|_, w| w
-            .man_start_en(true)
-            .manual_cs(true)
-            .endian(true)
-        );
-
-        self.regs.lqspi_cfg.write(regs::LqspiCfg::zeroed()
-            .mode_bits(0xFF)
-            .dummy_byte(0x2)
-            .mode_en(true)
-            // 2 devices
-            .two_mem(true)
-            .sep_bus(true)
-            .u_page(chip_index != 0)
-            // Manual I/O mode
-            .lq_mode(false)
-        );
-
-        self.transition()
-    }
-}
-
-impl Flash<LinearAddressing> {
-    /// Stop linear addressing mode
-    pub fn stop(self) -> Flash<()> {
-        self.regs.enable.modify(|_, w| w.spi_en(false));
-        // De-assert chip select.
-        self.regs.config.modify(|_, w| w.pcs(true));
-
-        self.transition()
-    }
-
-    pub fn ptr<T>(&mut self) -> *mut T {
-        0xFC00_0000 as *mut _
-    }
-
-    pub fn size(&self) -> usize {
-        2 * (SINGLE_CAPACITY as usize)
-    }
-}
-
-impl Flash<Manual> {
-    pub fn stop(self) -> Flash<()> {
-        self.transition()
-    }
-
-    pub fn read_reg<R: SpiFlashRegister>(&mut self) -> R {
-        let args = Some(R::inst_code());
-        let transfer = self.transfer(args.into_iter(), 2)
-            .bytes_transfer();
-        R::new(transfer.skip(1).next().unwrap())
-    }
-
-    pub fn read_reg_until<R, F, A>(&mut self, f: F) -> A
-    where
-        R: SpiFlashRegister,
-        F: Fn(R) -> Option<A>,
-    {
-        let mut result = None;
-        while result.is_none() {
-            let args = Some(R::inst_code());
-            for b in self.transfer(args.into_iter(), 32)
-                .bytes_transfer().skip(1) {
-                    result = f(R::new(b));
-                    
-                    if result.is_none() {
-                        break;
-                    }
-                }
-        }
-        result.unwrap()
-    }
-
-    /// Status Register-1 remains `0x00` immediately after invoking a command.
-    fn wait_while_sr1_zeroed(&mut self) -> SR1 {
-        self.read_reg_until::<SR1, _, SR1>(|sr1|
-            if sr1.is_zeroed() {
-                None
-            } else {
-                Some(sr1)
-            }
-        )
-    }
-
-    /// Read Identification
-    pub fn rdid(&mut self) -> core::iter::Skip<BytesTransfer<Transfer<core::option::IntoIter<u32>, u32>>> {
-        let args = Some((INST_RDID as u32) << 24);
-        self.transfer(args.into_iter(), 0x44)
-            .bytes_transfer().skip(1)
-    }
-
-    /// Read flash data
-    pub fn read(&mut self, offset: u32, len: usize
-    ) -> core::iter::Take<core::iter::Skip<BytesTransfer<Transfer<core::option::IntoIter<u32>, u32>>>>
-    {
-        let args = Some(((INST_READ as u32) << 24) | (offset as u32));
-        self.transfer(args.into_iter(), len + 6)
-            .bytes_transfer().skip(6).take(len)
-    }
-
-    pub fn erase(&mut self, offset: u32) {
-        let args = Some(((INST_BE_4K as u32) << 24) | (offset as u32));
-        self.transfer(args.into_iter(), 4);
-
-        let sr1 = self.wait_while_sr1_zeroed();
-
-        if sr1.e_err() {
-            println!("E_ERR");
-        } else if sr1.p_err() {
-            println!("P_ERR");
-        } else if sr1.wip() {
-            print!("Erase in progress");
-            while self.read_reg::<SR1>().wip() {
-                print!(".");
-            }
-            println!("");
-        } else {
-            println!("erased? sr1={:02X}", sr1.inner);
-        }
-    }
-
-    pub fn program<I: Iterator<Item=u32>>(&mut self, offset: u32, data: I) {
-        {
-            let len = 4 + 4 * data.size_hint().0;
-            let args = Some(SpiWord::W32(((INST_PP as u32) << 24) | (offset as u32))).into_iter()
-                .chain(data.map(SpiWord::W32));
-            self.transfer(args, len);
-        }
-
-        // let sr1 = self.wait_while_sr1_zeroed();
-        let sr1 = self.read_reg::<SR1>();
-
-        if sr1.e_err() {
-            println!("E_ERR");
-        } else if sr1.p_err() {
-            println!("P_ERR");
-        } else if sr1.wip() {
-            println!("Program in progress");
-            while self.read_reg::<SR1>().wip() {
-                print!(".");
-            }
-            println!("");
-        } else {
-            println!("programmed? sr1={:02X}", sr1.inner);
-        }
-    }
-
-    pub fn write_enabled<F: Fn(&mut Self) -> R, R>(&mut self, f: F) -> R {
-        // Write Enable
-        let args = Some(INST_WREN);
-        self.transfer(args.into_iter(), 1);
-        self.regs.gpio.modify(|_, w| w.wp_n(true));
-        let sr1 = self.wait_while_sr1_zeroed();
-        if !sr1.wel() {
-            panic!("Cannot write-enable flash");
-        }
-
-        let result = f(self);
-
-        // Write Disable
-        let args = Some(INST_WRDI);
-        self.transfer(args.into_iter(), 1);
-        self.regs.gpio.modify(|_, w| w.wp_n(false));
-
-        result
-    }
-
-    pub fn transfer<'s: 't, 't, Args, W>(&'s mut self, args: Args, len: usize) -> Transfer<'t, Args, W>
-    where
-        Args: Iterator<Item = W>,
-        W: Into<SpiWord>,
-    {
-        Transfer::new(self, args, len)
-    }
-
-    pub fn dump(&mut self, label: &'_ str, inst_code: u8) {
-        print!("{}:", label);
-
-        let args = Some(u32::from(inst_code) << 24);
-        for b in self.transfer(args.into_iter(), 32).bytes_transfer() {
-            print!(" {:02X}", b);
-        }
-        println!("");
-    }
-}
--- a/libboard_zynq/src/flash/regs.rs
+++ b/libboard_zynq/src/flash/regs.rs
@ -1,126 +0,0 @@
-use volatile_register::{RO, WO, RW};
-
-use libregister::{register, register_bit, register_bits};
-
-#[repr(C)]
-pub struct RegisterBlock {
-    pub config: Config,
-    pub intr_status: IntrStatus,
-    pub intr_en: IntrEn,
-    pub intr_dis: IntrDis,
-    pub intr_mask: RO<u32>,
-    pub enable: Enable,
-    pub delay: RW<u32>,
-    pub txd0: WO<u32>,
-    pub rx_data: RO<u32>,
-    pub slave_idle_count: RW<u32>,
-    pub tx_thres: RW<u32>,
-    pub rx_thres: RW<u32>,
-    pub gpio: QspiGpio,
-    pub _unused1: RO<u32>,
-    pub lpbk_dly_adj: RW<u32>,
-    pub _unused2: [RO<u32>; 17],
-    pub txd1: WO<u32>,
-    pub txd2: WO<u32>,
-    pub txd3: WO<u32>,
-    pub _unused3: [RO<u32>; 5],
-    pub lqspi_cfg: LqspiCfg,
-    pub lqspi_sts: RW<u32>,
-    pub _unused4: [RO<u32>; 21],
-    pub mod_id: RW<u32>,
-}
-
-impl RegisterBlock {
-    const BASE_ADDRESS: *mut Self = 0xE000D000 as *mut _;
-
-    pub fn qspi() -> &'static mut Self {
-        unsafe { &mut *Self::BASE_ADDRESS }
-    }
-}
-
-register!(config, Config, RW, u32);
-register_bit!(config,
-              /// Enables master mode
-              mode_sel, 0);
-register_bit!(config,
-              /// Clock polarity low/high
-              clk_pol, 1);
-register_bit!(config,
-              /// Clock phase
-              clk_ph, 2);
-register_bits!(config,
-               /// divider = 2 ** (1 + baud_rate_div)
-               baud_rate_div, u8, 3, 5);
-register_bits!(config,
-               /// Must be set to 0b11
-               fifo_width, u8, 6, 7);
-register_bit!(config,
-              /// Must be 0
-              ref_clk, 8);
-register_bit!(config,
-              /// Peripheral Chip Select Line
-              pcs, 10);
-register_bit!(config,
-              /// false: auto mode, true: manual CS mode
-              manual_cs, 14);
-register_bit!(config,
-              /// false: auto mode, true: enables manual start enable
-              man_start_en, 15);
-register_bit!(config,
-              /// false: auto mode, true: enables manual start command
-              man_start_com, 16);
-register_bit!(config, holdb_dr, 19);
-register_bit!(config,
-              /// false: little, true: endian
-              endian, 26);
-register_bit!(config,
-              /// false: legacy SPI mode, true: Flash memory interface mode
-              leg_flsh, 31);
-
-register!(intr_status, IntrStatus, RW, u32);
-register_bit!(intr_status, rx_overflow, 0);
-register_bit!(intr_status,
-              /// < tx_thres
-              tx_fifo_not_full, 2);
-register_bit!(intr_status, tx_fifo_full, 3);
-register_bit!(intr_status,
-              /// >= rx_thres
-              rx_fifo_not_empty, 4);
-register_bit!(intr_status, rx_fifo_full, 5);
-register_bit!(intr_status, tx_fifo_underflow, 6);
-
-register!(intr_en, IntrEn, WO, u32);
-register_bit!(intr_en, rx_overflow, 0);
-register_bit!(intr_en, tx_fifo_not_full, 2);
-register_bit!(intr_en, tx_fifo_full, 3);
-register_bit!(intr_en, rx_fifo_not_empty, 4);
-register_bit!(intr_en, rx_fifo_full, 5);
-register_bit!(intr_en, tx_fifo_underflow, 6);
-
-register!(intr_dis, IntrDis, WO, u32);
-register_bit!(intr_dis, rx_overflow, 0);
-register_bit!(intr_dis, tx_fifo_not_full, 2);
-register_bit!(intr_dis, tx_fifo_full, 3);
-register_bit!(intr_dis, rx_fifo_not_empty, 4);
-register_bit!(intr_dis, rx_fifo_full, 5);
-register_bit!(intr_dis, tx_fifo_underflow, 6);
-
-register!(enable, Enable, RW, u32);
-register_bit!(enable, spi_en, 0);
-
-// named to avoid confusion with normal gpio
-register!(qspi_gpio, QspiGpio, RW, u32);
-register_bit!(qspi_gpio,
-              /// Write protect pin (inverted)
-              wp_n, 0);
-
-register!(lqspi_cfg, LqspiCfg, RW, u32);
-register_bits!(lqspi_cfg, inst_code, u8, 0, 7);
-register_bits!(lqspi_cfg, dummy_byte, u8, 8, 10);
-register_bits!(lqspi_cfg, mode_bits, u8, 16, 23);
-register_bit!(lqspi_cfg, mode_on, 24);
-register_bit!(lqspi_cfg, mode_en, 25);
-register_bit!(lqspi_cfg, u_page, 28);
-register_bit!(lqspi_cfg, sep_bus, 29);
-register_bit!(lqspi_cfg, two_mem, 30);
-register_bit!(lqspi_cfg, lq_mode, 31);
--- a/libboard_zynq/src/flash/spi_flash_register.rs
+++ b/libboard_zynq/src/flash/spi_flash_register.rs
@ -1,62 +0,0 @@
-use bit_field::BitField;
-
-pub trait SpiFlashRegister {
-    fn inst_code() -> u8;
-    fn new(src: u8) -> Self;
-}
-
-macro_rules! u8_register {
-    ($name: ident, $doc: tt, $inst_code: expr) => {
-        #[derive(Clone)]
-        #[doc=$doc]
-        pub struct $name {
-            pub inner: u8,
-        }
-
-        impl SpiFlashRegister for $name {
-            fn inst_code() -> u8 {
-                $inst_code
-            }
-
-            fn new(src: u8) -> Self {
-                $name {
-                    inner: src,
-                }
-            }
-        }
-
-        impl $name {
-            #[allow(unused)]
-            pub fn is_zeroed(&self) -> bool {
-                self.inner == 0
-            }
-        }
-    };
-}
-
-u8_register!(CR, "Configuration Register", 0x35);
-u8_register!(SR1, "Status Register-1", 0x05);
-impl SR1 {
-    /// Write In Progress
-    pub fn wip(&self) -> bool {
-        self.inner.get_bit(0)
-    }
-
-    /// Write Enable Latch
-    pub fn wel(&self) -> bool {
-        self.inner.get_bit(1)
-    }
-
-    /// Erase Error Occurred
-    pub fn e_err(&self) -> bool {
-        self.inner.get_bit(5)
-    }
-
-    /// Programming Error Occurred
-    pub fn p_err(&self) -> bool {
-        self.inner.get_bit(6)
-    }
-}
-
-u8_register!(SR2, "Status Register-2", 0x07);
-u8_register!(BA, "Bank Address Register", 0xB9);
--- a/libboard_zynq/src/flash/transfer.rs
+++ b/libboard_zynq/src/flash/transfer.rs
@ -1,125 +0,0 @@
-use libregister::{RegisterR, RegisterW, RegisterRW};
-use super::regs;
-use super::{SpiWord, Flash, Manual};
-
-pub struct Transfer<'a, Args: Iterator<Item = W>, W: Into<SpiWord>> {
-    flash: &'a mut Flash<Manual>,
-    args: Args,
-    sent: usize,
-    received: usize,
-    len: usize,
-}
-
-impl<'a, Args: Iterator<Item = W>, W: Into<SpiWord>> Transfer<'a, Args, W> {
-    pub fn new(flash: &'a mut Flash<Manual>, args: Args, len: usize) -> Self {
-        flash.regs.config.modify(|_, w| w.pcs(false));
-        flash.regs.enable.write(
-            regs::Enable::zeroed()
-                .spi_en(true)
-        );
-
-        let mut xfer = Transfer {
-            flash,
-            args,
-            sent: 0,
-            received: 0,
-            len,
-        };
-        xfer.fill_tx_fifo();
-        xfer.flash.regs.config.modify(|_, w| w.man_start_com(true));
-        xfer
-    }
-
-    fn fill_tx_fifo(&mut self) {
-        while self.sent < self.len && !self.flash.regs.intr_status.read().tx_fifo_full() {
-            let arg = self.args.next()
-                .map(|n| n.into())
-                .unwrap_or(SpiWord::W32(0));
-            match arg {
-                SpiWord::W32(w) => {
-                    // println!("txd0 {:08X}", w);
-                    unsafe {
-                        self.flash.regs.txd0.write(w);
-                    }
-                    self.sent += 4;
-                }
-                // Only txd0 can be used without flushing
-                _ => {
-                    if !self.flash.regs.intr_status.read().tx_fifo_not_full() {
-                        // Flush if necessary
-                        self.flash.wait_tx_fifo_flush();
-                    }
-
-                    match arg {
-                        SpiWord::W8(w) => {
-                            // println!("txd1 {:02X}", w);
-                            unsafe {
-                                self.flash.regs.txd1.write(u32::from(w) << 24);
-                            }
-                            self.sent += 1;
-                        }
-                        SpiWord::W16(w) => {
-                            unsafe {
-                                self.flash.regs.txd2.write(u32::from(w) << 16);
-                            }
-                            self.sent += 2;
-                        }
-                        SpiWord::W24(w) => {
-                            unsafe {
-                                self.flash.regs.txd3.write(w << 8);
-                            }
-                            self.sent += 3;
-                        }
-                        SpiWord::W32(_) => unreachable!(),
-                    }
-
-                    self.flash.wait_tx_fifo_flush();
-                }
-            }
-        }
-    }
-
-    fn can_read(&mut self) -> bool {
-        self.flash.regs.intr_status.read().rx_fifo_not_empty()
-    }
-
-    fn read(&mut self) -> u32 {
-        let rx = self.flash.regs.rx_data.read();
-        self.received += 4;
-        rx
-    }
-}
-
-impl<'a, Args: Iterator<Item = W>, W: Into<SpiWord>> Drop for Transfer<'a, Args, W> {
-    fn drop(&mut self) {
-        // Discard remaining rx_data
-        while self.can_read() {
-            self.read();
-        }
-
-        // Stop
-        self.flash.regs.enable.write(
-            regs::Enable::zeroed()
-                .spi_en(false)
-        );
-        self.flash.regs.config.modify(|_, w| w
-            .pcs(true)
-            .man_start_com(false)
-        );
-    }
-}
-
-impl<'a, Args: Iterator<Item = W>, W: Into<SpiWord>> Iterator for Transfer<'a, Args, W> {
-    type Item = u32;
-
-    fn next<'s>(&'s mut self) -> Option<u32> {
-        if self.received >= self.len {
-            return None;
-        }
-
-        self.fill_tx_fifo();
-
-        while !self.can_read() {}
-        Some(self.read())
-    }
-}
--- 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 + 1));
+        }
+
+        // sensitivity
+        let m = (id.0 >> 4) as usize;
+        let n = (2 * (id.0 & 0xF)) as usize;
+        unsafe {
+            self.mpcore.icdicfr[m].modify(|mut icdicfr| *icdicfr.set_bits(n..=n+1, match sensitivity {
+                InterruptSensitivity::Level => 0b00,
+                InterruptSensitivity::Edge  => 0b10,
+            }));
+        }
+
+        // priority
+        let offset = (id.0 % 4) * 8;
+        let priority: u32 = (priority as u32) << (offset + 3);
+        let mask: u32 = 0xFFFFFFFF ^ (0xFF << offset);
+        unsafe {
+            self.mpcore.icdipr[id.0 as usize / 4].modify(|v| (v & mask) | priority);
+        }
+
+        self.enable_interrupts();
+    }
+
+    pub fn end_interrupt(&mut self, id: InterruptId) {
+        self.mpcore.icceoir.modify(|_, w| w.eoiintid(id.0 as u32));
+    }
+
+    pub fn get_interrupt_id(&self) -> InterruptId {
+        InterruptId(self.mpcore.icciar.read().ackintid() as u8)
+    }
+
+}
--- a/libboard_zynq/src/i2c/eeprom.rs
+++ b/libboard_zynq/src/i2c/eeprom.rs
@ -0,0 +1,107 @@
+use super::I2c;
+use crate::time::Milliseconds;
+use embedded_hal::timer::CountDown;
+
+pub struct EEPROM<'a> {
+    i2c: &'a mut I2c,
+    port: u8,
+    address: u8,
+    page_size: u8,
+    count_down: crate::timer::global::CountDown<Milliseconds>
+}
+
+impl<'a> EEPROM<'a> {
+    #[cfg(feature = "target_zc706")]
+    pub fn new(i2c: &'a mut I2c, page_size: u8) -> Self {
+        EEPROM {
+            i2c: i2c,
+            port: 2,
+            address: 0b1010100,
+            page_size: page_size,
+            count_down: unsafe { crate::timer::GlobalTimer::get() }.countdown()
+        }
+    }
+
+    #[cfg(feature = "target_zc706")]
+    fn select(&mut self) -> Result<(), &'static str> {
+        let mask: u16 = 1 << self.port;
+        self.i2c.pca9548_select(0b1110100, mask as u8)?;
+        Ok(())
+    }
+
+    /// Random read
+    pub fn read<'r>(&mut self, addr: u8, buf: &'r mut [u8]) -> Result<(), &'static str> {
+        self.select()?;
+
+        self.i2c.start()?;
+        self.i2c.write(self.address << 1)?;
+        self.i2c.write(addr)?;
+
+        self.i2c.restart()?;
+        self.i2c.write((self.address << 1) | 1)?;
+        let buf_len = buf.len();
+        for (i, byte) in buf.iter_mut().enumerate() {
+            *byte = self.i2c.read(i < buf_len - 1)?;
+        }
+
+        self.i2c.stop()?;
+
+        Ok(())
+    }
+
+    /// Smart multi-page writing
+    /// Using the "Page Write" function of an EEPROM, the memory region for each transaction
+    /// (i.e. from byte `addr` to byte `addr+buf.len()`) should fit under each page 
+    /// (i.e. `addr+buf.len()` < `addr/self.page_size+1`); otherwise, a roll-oever occurs, 
+    /// where bytes beyond the page end. This smart function takes care of the scenario to avoid
+    /// any roll-over when writing ambiguous memory regions.
+    pub fn write(&mut self, addr: u8, buf: &[u8]) -> Result<(), &'static str> {
+        self.select()?;
+
+        let buf_len = buf.len();
+        let mut pb: u8 = addr % self.page_size;
+        for (i, byte) in buf.iter().enumerate() {
+            if (i == 0) || (pb == 0) {
+                self.i2c.start()?;
+                self.i2c.write(self.address << 1)?;
+                self.i2c.write(addr + (i as u8))?;
+            }
+            self.i2c.write(*byte)?;
+            pb += 1;
+
+            if (i == buf_len-1) || (pb == self.page_size) {
+                self.i2c.stop()?;
+                self.poll(1_000)?;
+                pb = 0;
+            }
+        }
+
+        Ok(())
+    }
+
+    /// Poll
+    pub fn poll(&mut self, timeout_ms: u64) -> Result<(), &'static str> {
+        self.select()?;
+
+        self.count_down.start(Milliseconds(timeout_ms));
+        loop {
+            self.i2c.start()?;
+            let ack = self.i2c.write(self.address << 1)?;
+            self.i2c.stop()?;
+            if ack {
+                break
+            };
+            if !self.count_down.waiting() {
+                return Err("I2C polling timeout")
+            }
+        }
+
+        Ok(())
+    }
+
+    pub fn read_eui48<'r>(&mut self) -> Result<[u8; 6], &'static str> {
+        let mut buffer = [0u8; 6];
+        self.read(0xFA, &mut buffer)?;
+        Ok(buffer)
+    }
+}
--- a/libboard_zynq/src/i2c/mod.rs
+++ b/libboard_zynq/src/i2c/mod.rs
@ -0,0 +1,231 @@
+//! I2C Bit-banging Controller
+
+mod regs;
+pub mod eeprom;
+use super::slcr;
+use super::time::Microseconds;
+use embedded_hal::timer::CountDown;
+use libregister::{RegisterR, RegisterRW, RegisterW};
+
+pub struct I2c {
+    regs: regs::RegisterBlock,
+    count_down: super::timer::global::CountDown<Microseconds>
+}
+
+impl I2c {
+    #[cfg(feature = "target_zc706")]
+    pub fn i2c0() -> Self {
+        // Route I2C 0 SCL / SDA Signals to MIO Pins 50 / 51
+        slcr::RegisterBlock::unlocked(|slcr| {
+            // SCL
+            slcr.mio_pin_50.write(
+                slcr::MioPin50::zeroed()
+                    .l3_sel(0b000)  // as GPIO 50
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+                    .disable_rcvr(true)
+            );
+            // SDA
+            slcr.mio_pin_51.write(
+                slcr::MioPin51::zeroed()
+                    .l3_sel(0b000)  // as GPIO 51
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+                    .disable_rcvr(true)
+            );
+            // Reset
+            slcr.gpio_rst_ctrl.reset_gpio();
+        });
+
+        Self::i2c_common(0xFFFF - 0x000C)
+    }
+
+    fn i2c_common(gpio_output_mask: u16) -> Self {
+        // Setup register block
+        let self_ = Self {
+            regs: regs::RegisterBlock::i2c(),
+            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown()
+        };
+
+        // Setup GPIO output mask
+        self_.regs.gpio_output_mask.modify(|_, w| {
+            w.mask(gpio_output_mask)
+        });
+        // Setup GPIO driver direction
+        self_.regs.gpio_direction.modify(|_, w| {
+            w.scl(true).sda(true)
+        });
+
+        self_
+    }
+
+    /// Delay for I2C operations, simple wrapper for nb.
+    fn delay_us(&mut self, us: u64) {
+        self.count_down.start(Microseconds(us));
+        nb::block!(self.count_down.wait()).unwrap();
+    }
+
+    fn half_period(&mut self) { self.delay_us(100) }
+
+    fn sda_i(&mut self) -> bool {
+        self.regs.gpio_input.read().sda()
+    }
+
+    fn scl_i(&mut self) -> bool {
+        self.regs.gpio_input.read().scl()
+    }
+
+    fn sda_oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+             w.sda(oe)
+        })
+    }
+
+    fn sda_o(&mut self, o: bool) {
+        self.regs.gpio_output_mask.modify(|_, w| {
+             w.sda_o(o)
+        })
+    }
+
+    fn scl_oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+             w.scl(oe)
+        })
+    }
+
+    fn scl_o(&mut self, o: bool) {
+        self.regs.gpio_output_mask.modify(|_, w| {
+             w.scl_o(o)
+        })
+    }
+
+    pub fn init(&mut self) -> Result<(), &'static str> {
+        self.scl_oe(false);
+        self.sda_oe(false);
+        self.scl_o(false);
+        self.sda_o(false);
+
+        // Check the I2C bus is ready
+        self.half_period();
+        self.half_period();
+        if !self.sda_i() {
+            // Try toggling SCL a few times
+            for _bit in 0..8 {
+                self.scl_oe(true);
+                self.half_period();
+                self.scl_oe(false);
+                self.half_period();
+            }
+        }
+
+        if !self.sda_i() {
+            return Err("SDA is stuck low and doesn't get unstuck");
+        }
+        if !self.scl_i() {
+            return Err("SCL is stuck low and doesn't get unstuck");
+        }
+        // postcondition: SCL and SDA high
+        Ok(())
+    }
+
+    pub fn start(&mut self) -> Result<(), &'static str> {
+        // precondition: SCL and SDA high
+        if !self.scl_i() {
+            return Err("SCL is stuck low and doesn't get unstuck");
+        }
+        if !self.sda_i() {
+            return Err("SDA arbitration lost");
+        }
+        self.sda_oe(true);
+        self.half_period();
+        self.scl_oe(true);
+        // postcondition: SCL and SDA low
+        Ok(())
+    }
+
+    pub fn restart(&mut self) -> Result<(), &'static str> {
+        // precondition SCL and SDA low
+        self.sda_oe(false);
+        self.half_period();
+        self.scl_oe(false);
+        self.half_period();
+        self.start()?;
+        // postcondition: SCL and SDA low
+        Ok(())
+    }
+
+    pub fn stop(&mut self) -> Result<(), &'static str> {
+        // precondition: SCL and SDA low
+        self.half_period();
+        self.scl_oe(false);
+        self.half_period();
+        self.sda_oe(false);
+        self.half_period();
+        if !self.sda_i() {
+            return Err("SDA arbitration lost");
+        }
+        // postcondition: SCL and SDA high
+        Ok(())
+    }
+
+    pub fn write(&mut self, data: u8) -> Result<bool, &'static str> {
+        // precondition: SCL and SDA low
+        // MSB first
+        for bit in (0..8).rev() {
+            self.sda_oe(data & (1 << bit) == 0);
+            self.half_period();
+            self.scl_oe(false);
+            self.half_period();
+            self.scl_oe(true);
+        }
+        self.sda_oe(false);
+        self.half_period();
+        self.scl_oe(false);
+        self.half_period();
+        // Read ack/nack
+        let ack = !self.sda_i();
+        self.scl_oe(true);
+        self.sda_oe(true);
+        // postcondition: SCL and SDA low
+
+        Ok(ack)
+    }
+
+    pub fn read(&mut self, ack: bool) -> Result<u8, &'static str> {
+        // precondition: SCL and SDA low
+        self.sda_oe(false);
+
+        let mut data: u8 = 0;
+
+        // MSB first
+        for bit in (0..8).rev() {
+            self.half_period();
+            self.scl_oe(false);
+            self.half_period();
+            if self.sda_i() { data |= 1 << bit }
+            self.scl_oe(true);
+        }
+        // Send ack/nack
+        self.sda_oe(ack);
+        self.half_period();
+        self.scl_oe(false);
+        self.half_period();
+        self.scl_oe(true);
+        self.sda_oe(true);
+        // postcondition: SCL and SDA low
+
+        Ok(data)
+    }
+
+    pub fn pca9548_select(&mut self, address: u8, channels: u8) -> Result<(), &'static str> {
+        self.start()?;
+        if !self.write(address << 1)? {
+            return Err("PCA9548 failed to ack write address")
+        }
+        if !self.write(channels)? {
+            return Err("PCA9548 failed to ack control word")
+        }
+        self.stop()?;
+        Ok(())
+    }
+}
--- a/libboard_zynq/src/i2c/regs.rs
+++ b/libboard_zynq/src/i2c/regs.rs
@ -0,0 +1,91 @@
+use libregister::{
+    register, register_at,
+    register_bit, register_bits
+};
+
+// With reference to:
+//
+// artiq:artiq/gateware/targets/kasli.py:
+// self.submodules.i2c = gpio.GPIOTristate([i2c.scl, i2c.sda])
+//
+// misoc:misoc/cores/gpio.py:
+// class GPIOTristate(Module, AutoCSR):
+//    def __init__(self, signals, reset_out=0, reset_oe=0):
+//        l = len(signals)
+//        self._in = CSRStatus(l)
+//        self._out = CSRStorage(l, reset=reset_out)
+//        self._oe = CSRStorage(l, reset=reset_oe)
+// 
+// Hence, using GPIOs as SCL and SDA GPIOs respectively.
+//
+// Current compatibility:
+// zc706: GPIO 50, 51 == SCL, SDA
+
+pub struct 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,
+}
+
+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()
+        }
+    }
+}
+
+// MASK_DATA_1_MSW:
+// Maskable output data for MIO[53:48]
+register!(gpio_output_mask, GPIOOutputMask, RW, u32);
+#[cfg(feature = "target_zc706")]
+register_at!(GPIOOutputMask, 0xE000A00C, new);
+// Output for SCL
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_output_mask, scl_o, 2);
+// Output for SDA
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_output_mask, sda_o, 3);
+// Mask for keeping bits except SCL and SDA unchanged
+#[cfg(feature = "target_zc706")]
+register_bits!(gpio_output_mask, mask, u16, 16, 31);
+
+// DATA_1_RO:
+// Input data for MIO[53:32]
+register!(gpio_input, GPIOInput, RO, u32);
+#[cfg(feature = "target_zc706")]
+register_at!(GPIOInput, 0xE000A064, new);
+// Input for SCL
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_input, scl, 18);
+// Input for SDA
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_input, sda, 19);
+
+// DIRM_1:
+// Direction mode for MIO[53:32]; 0/1 = in/out
+register!(gpio_direction, GPIODirection, RW, u32);
+#[cfg(feature = "target_zc706")]
+register_at!(GPIODirection, 0xE000A244, new);
+// Direction for SCL
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_direction, scl, 18);
+// Direction for SDA
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_direction, sda, 19);
+
+// OEN_1:
+// Output enable for MIO[53:32]
+register!(gpio_output_enable, GPIOOutputEnable, RW, u32);
+#[cfg(feature = "target_zc706")]
+register_at!(GPIOOutputEnable, 0xE000A248, new);
+// Output enable for SCL
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_output_enable, scl, 18);
+// Output enable for SDA
+#[cfg(feature = "target_zc706")]
+register_bit!(gpio_output_enable, sda, 19);
--- a/libboard_zynq/src/lib.rs
+++ b/libboard_zynq/src/lib.rs
@ -1,5 +1,10 @@
 #![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;
@ -10,4 +15,11 @@ pub mod axi_hp;
 pub mod axi_gp;
 pub mod ddr;
 pub mod mpcore;
-pub mod flash;
+pub mod gic;
+pub mod time;
+pub mod timer;
+pub mod sdio;
+#[cfg(feature = "target_zc706")]
+pub mod i2c;
+pub mod logger;
+pub mod ps7_init;
--- 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
@ -8,19 +8,140 @@ use libregister::{

 #[repr(C)]
 pub struct RegisterBlock {
+    /// SCU Control Register
    pub scu_control: ScuControl,
-    pub scu_config: RO<u32>,
-    pub scu_cpu_power: RW<u32>,
+    /// SCU Configuration Register
+    pub scu_config: ScuConfig,
+    /// SCU CPU Power Status Register
+    pub scu_cpu_power_status: SCUCPUPowerStatusRegister,
+    /// SCU Invalidate All Registers in Secure State
    pub scu_invalidate: ScuInvalidate,
-    reserved0: [u32; 12],
-    pub filter_start: RW<u32>,
-    pub filter_end: RW<u32>,
-    reserved1: [u32; 2],
-    pub scu_access_control: RW<u32>,
-    pub scu_non_secure_access_control: RW<u32>,
-    // there is plenty more (unimplemented)
+    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, new);
+
+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);
@ -33,10 +154,21 @@ register_bit!(scu_control, enable, 0);

 impl ScuControl {
    pub fn start(&mut self) {
-        self.modify(|_, w| w.enable(true));
+        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);
@ -59,3 +191,131 @@ impl ScuInvalidate {
        );
    }
 }
+
+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 @@
+#![cfg(feature = "target_zc706")]
+
+use crate::println;
+
+mod zc706;
+// mod cora_z7_10;
+
+#[cfg(feature = "target_zc706")]
+use zc706 as target;
+// #[cfg(feature = "target_cora_z7_10")]
+// use cora_z7_10 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/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,461 @@
+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_cora_z7_10")]
+            {
+                unsafe {
+                    slcr.sd0_wp_cd_sel.write(47 << 16);
+                }
+                slcr.mio_pin_47.write(
+                    slcr::MioPin47::zeroed()
+                        .io_type(slcr::IoBufferType::Lvcmos18)
+                        .speed(true),
+                );
+            }
+            // redpitaya card detect pin
+            #[cfg(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),
+                );
+            }
+            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
@ -50,13 +50,20 @@ pub enum DdriobOutputEn {
 #[repr(u8)]
 pub enum DdriobVrefSel {
    /// For LPDDR2 with 1.2V IO
-    Vref0_6V,
+    Vref0_6V = 0b0001,
    /// For DDR3L with 1.35V IO
-    Vref0_675V,
+    Vref0_675V = 0b0010,
    /// For DDR3 with 1.5V IO
-    Vref0_75V,
+    Vref0_75V = 0b0100,
    /// For DDR2 with 1.8V IO
-    Vref0_9V,
+    Vref0_9V = 0b1000,
+}
+
+#[repr(u8)]
+pub enum LevelShifterEnable {
+    DisableAll = 0x0,
+    EnablePsToPl = 0xA,
+    EnableAll = 0xF,
 }


@ -87,7 +94,7 @@ pub struct RegisterBlock {
    pub gem1_clk_ctrl: GemClkCtrl,
    pub smc_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>,
@ -95,19 +102,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>,
@ -120,17 +127,17 @@ 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: 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>,
@ -219,7 +226,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],
@ -246,23 +253,47 @@ pub struct RegisterBlock {
    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 soft_reset(&mut self) {
-        self.pss_rst_ctrl.write(
-            PssRstCtrl::zeroed()
-                .soft_rst(true)
+    pub fn init_preload_fpga(&mut self) {
+        // Assert FPGA top level output resets
+        self.fpga_rst_ctrl.write(
+            FpgaRstCtrl::zeroed()
+                .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()
        );
    }
 }
@ -352,6 +383,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));
@ -360,6 +393,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);
@ -384,6 +425,24 @@ 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);
@ -414,6 +473,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);
@ -444,6 +531,20 @@ impl UartRstCtrl {
    }
 }

+register!(gpio_rst_ctrl, GpioRstCtrl, RW, u32);
+register_bit!(gpio_rst_ctrl, gpio_cpu1x_rst, 0);
+register_at!(GpioRstCtrl, 0xF800022C, new);
+impl GpioRstCtrl {
+    pub fn reset_gpio(&mut self) {
+        self.modify(|_, w|
+            w.gpio_cpu1x_rst(true)
+        );
+        self.modify(|_, w|
+            w.gpio_cpu1x_rst(false)
+        );
+    }
+}
+
 register!(lqspi_clk_ctrl, LqspiClkCtrl, RW, u32);
 register_bit!(lqspi_clk_ctrl, clkact, 0);
 register_bits_typed!(lqspi_clk_ctrl, src_sel, u8, PllSource, 4, 5);
@ -453,6 +554,32 @@ 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);
 register_bit!(a9_cpu_rst_ctrl, a9_clkstop1, 5);
@ -463,16 +590,19 @@ register_bit!(a9_cpu_rst_ctrl, a9_rst0, 0);
 #[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 = 0b001,
-    Nand = 0b010,
-    QuadSpi = 0b100,
-    SdCard = 0b110,
+    Nor = 0b010,
+    Nand = 0b100,
+    QuadSpi = 0b001,
+    SdCard = 0b101,
 }

 register!(boot_mode, BootMode, RO, u32);
 register_bit!(boot_mode, pll_bypass, 4);
-register_bits_typed!(boot_mode, boot_mode_pins, u8, BootModePins, 0, 3);
+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, 1);
@ -556,6 +686,9 @@ 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);

@ -570,7 +703,7 @@ 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, 1);
+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);
@ -578,7 +711,7 @@ 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, 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);
--- a/libboard_zynq/src/stdio.rs
+++ b/libboard_zynq/src/stdio.rs
@ -1,5 +1,5 @@
 use core::ops::{Deref, DerefMut};
-use libcortex_a9::mutex::{Mutex, MutexGuard};
+use libcortex_a9::{asm, mutex::{Mutex, MutexGuard}};
 use crate::uart::Uart;

 const UART_RATE: u32 = 115_200;
@ -10,7 +10,15 @@ 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); }
 }

@ -37,7 +45,10 @@ impl DerefMut for LazyUart {
    fn deref_mut(&mut self) -> &mut Uart {
        match self {
            LazyUart::Uninitialized => {
-                let uart = Uart::serial(UART_RATE);
+                #[cfg(any(feature = "target_cora_z7_10", feature = "target_redpitaya"))]
+                let uart = Uart::uart0(UART_RATE);
+                #[cfg(feature = "target_zc706")]
+                let uart = Uart::uart1(UART_RATE);
                *self = LazyUart::Initialized(uart);
                self
            }
@ -62,7 +73,8 @@ macro_rules! println {
        use core::fmt::Write;
        let mut uart = $crate::stdio::get_uart();
        let _ = write!(uart, $($arg)*);
-        let _ = write!(uart, "\r\n");
-        while !uart.tx_fifo_empty() {}
+        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/mod.rs
+++ b/libboard_zynq/src/uart/mod.rs
@ -1,4 +1,5 @@
 use core::fmt;
+use void::Void;

 use libregister::*;
 use super::slcr;
@ -12,31 +13,8 @@ pub struct Uart {
 }

 impl Uart {
-    #[cfg(feature = "target_zc706")]
-    pub fn serial(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 {
+    #[cfg(any(feature = "target_cora_z7_10", feature = "target_redpitaya"))]
+    pub fn uart0(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,27 @@ impl Uart {
        self_
    }

+    #[cfg(feature = "target_zc706")]
    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();
@ -191,18 +186,42 @@ impl Uart {
        self.regs.channel_sts.read().txfull()
    }

-    pub fn tx_fifo_empty(&self) -> bool {
-        self.regs.channel_sts.read().txempty()
+    pub fn tx_idle(&self) -> bool {
+        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/libconfig/Cargo.toml
+++ b/libconfig/Cargo.toml
@ -0,0 +1,15 @@
+[package]
+name = "libconfig"
+version = "0.1.0"
+authors = ["M-Labs"]
+edition = "2018"
+
+[dependencies]
+libboard_zynq = { path = "../libboard_zynq" }
+core_io = { version = "0.1", features = ["collections"] }
+fatfs = { version = "0.3", features = ["core_io"], default-features = false }
+log = "0.4"
+
+[features]
+ipv6 = []
+
--- 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,287 @@
+#![no_std]
+extern crate alloc;
+
+use core::fmt;
+use alloc::{string::FromUtf8Error, string::String, vec::Vec, rc::Rc, str};
+use core_io::{self as io, BufRead, BufReader, Read, Write, Seek, ErrorKind, 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();
+    for line in BufReader::new(file).lines() {
+        let line = line?;
+        if line.starts_with(&prefix) {
+            buffer.extend(line[prefix.len()..].as_bytes());
+            return Ok(());
+        }
+    }
+    Err(Error::KeyNotFoundError(key))
+}
+
+fn delete_old_entry<'a>(
+    key: &str,
+    file: fatfs::File<sd_reader::SdReader>,
+    mut file_tmp: fatfs::File<sd_reader::SdReader>,
+) -> Result<'a, ()> {
+    let prefix = [key, "="].concat();
+    let buf_reader = BufReader::new(file);
+
+    for line in buf_reader.lines() {
+        let line = line?;
+        if !line.starts_with(&prefix) {
+            file_tmp.write_all(&[line.as_str(), "\n"].concat().as_bytes())?;
+        }
+    }
+    Ok(())
+}
+
+fn rename_file<'a>(dir: &fatfs::Dir<sd_reader::SdReader>, old_file_name: &str, new_file_name: &str) -> Result<'a, ()>{
+    {
+        let old_file = dir.open_file(old_file_name)?;
+        let mut new_file = dir.create_file(new_file_name)?;
+        new_file.truncate()?;
+
+        for line in BufReader::new(old_file).lines() {
+            let line = line?;
+            new_file.write_all(&[line.as_str(), "\n"].concat().as_bytes())?;
+        }
+    }
+
+    dir.remove(old_file_name)?;
+
+    Ok(())
+}
+
+pub struct Config {
+    fs: Option<Rc<fatfs::FileSystem<sd_reader::SdReader>>>,
+}
+
+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 erase<'b>(&mut self) -> Result<'b, ()>{
+        if let Some(fs) = &self.fs {
+            let root_dir = fs.root_dir();
+
+            match root_dir.create_file("/CONFIG.TXT") {
+                Ok(mut file) => {
+                    file.truncate()?;
+                },
+                Err(e) => {
+                    return Err(Error::IoError(e));
+                }
+            };
+
+            let dir = root_dir.create_dir("/CONFIG")?;
+            for r in dir.iter() {
+                let entry = r?;
+                if entry.is_file() {
+                    dir.remove(str::from_utf8(entry.short_file_name_as_bytes()).unwrap())?;
+                }
+            }
+
+            Ok(())
+        } else {
+            Err(Error::NoConfig)
+        }
+    }
+    pub fn remove_config_txt<'b>(&mut self, key: &str) -> Result<'b, ()>{
+        if let Some(fs) = &self.fs {
+            let root_dir = fs.root_dir();
+
+            let config_txt_tmp = "/CONFIG.TMP";
+
+            let mut need_to_rename = false;
+            match root_dir.open_file("/CONFIG.TXT") {
+                Ok(file) => {
+                    need_to_rename = true;
+                    let mut file_tmp = root_dir.create_file(config_txt_tmp)?;
+                    file_tmp.truncate()?;
+                    delete_old_entry(key, file, file_tmp)?;
+                },
+                Err(e) => match e.kind() {
+                    ErrorKind::NotFound => {},
+                    _ => {
+                        return Err(Error::IoError(e));
+                    }
+                }
+            };
+            if need_to_rename {
+                rename_file(&root_dir, config_txt_tmp, "/CONFIG.TXT")?;
+            }
+
+            Ok(())
+        } else {
+            Err(Error::NoConfig)
+        }
+    }
+    pub fn remove_config_key_bin<'b>(&mut self, key: &str) -> Result<'b, ()>{
+        if let Some(fs) = &self.fs {
+            let root_dir = fs.root_dir();
+
+            let config_key_bin = &["/CONFIG/", key, ".BIN"].concat();
+
+            match root_dir.remove(config_key_bin) {
+                Ok(_) => {},
+                Err(e) => match e.kind() {
+                    ErrorKind::NotFound => {},
+                    _ => {
+                        return Err(Error::IoError(e));
+                    }
+                }
+            };
+
+            Ok(())
+        } else {
+            Err(Error::NoConfig)
+        }
+    }
+    pub fn remove<'b>(&mut self, key: &str) -> Result<'b, ()>{
+        self.remove_config_txt(key)?;
+        self.remove_config_key_bin(key)?;
+        Ok(())
+    }
+    pub fn write_str<'b>(&mut self, key: &str, data: &str) -> Result<'b, ()>{
+        let mut call_remove_config_key_bin = false;
+        let mut call_remove_config_txt = false;
+
+        if let Some(fs) = &self.fs {
+            let root_dir = fs.root_dir();
+
+            let config_key_bin = &["/CONFIG/", key, ".BIN"].concat();
+            let config_txt_tmp = "/CONFIG.TMP";
+
+            if data.is_ascii() & (data.len() <= 100) {
+                match root_dir.create_file("/CONFIG.TXT") {
+                    Ok(file) => {
+                        let mut file_tmp = root_dir.create_file(config_txt_tmp)?;
+                        file_tmp.truncate()?;
+                        delete_old_entry(key, file, file_tmp)?;
+                    },
+                    Err(e) => {
+                        return Err(Error::IoError(e));
+                    }
+                };
+
+                rename_file(&root_dir, config_txt_tmp, "/CONFIG.TXT")?;
+
+                let mut file = root_dir.open_file("/CONFIG.TXT")?;
+                file.seek(SeekFrom::End(0))?;
+                file.write_all(&["\n", key, "=", data, "\n"].concat().as_bytes())?;
+
+                call_remove_config_key_bin = true;
+            } else {
+                root_dir.create_dir("/CONFIG")?;
+                match root_dir.create_file(config_key_bin) {
+                    Ok(mut file) => {
+                        file.truncate()?;
+                        file.write_all(&[data, "\n"].concat().as_bytes())?;
+                    },
+                    Err(e) => {
+                        return Err(Error::IoError(e));
+                    }
+                };
+
+                call_remove_config_txt = true;
+            }
+        } else {
+            return Err(Error::NoConfig);
+        }
+
+        if call_remove_config_key_bin {
+            self.remove_config_key_bin(key)?;
+        }
+        if call_remove_config_txt {
+            self.remove_config_txt(key)?;
+        }
+        Ok(())
+    }
+}
--- a/libconfig/src/net_settings.rs
+++ b/libconfig/src/net_settings.rs
@ -0,0 +1,62 @@
+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(())
+    }
+}
+
+pub fn get_adresses(cfg: &Config) -> NetAddresses {
+    let mut hardware_addr = EthernetAddress([0x02, 0x00, 0x00, 0x00, 0x00, 0x52]);
+    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 52);
+
+    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("ipv6").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,303 @@
+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;
+
+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_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/libcoreio/Cargo.toml
+++ b/libcoreio/Cargo.toml
@ -0,0 +1,14 @@
+[package]
+authors = ["M-Labs"]
+name = "core_io"
+version = "0.1.20200410"
+
+[lib]
+name = "core_io"
+
+[dependencies]
+memchr = { version = "2", default-features = false, optional = true }
+
+[features]
+alloc = []
+collections = ["alloc", "memchr"]
--- a/libcoreio/src/io/buffered.rs
+++ b/libcoreio/src/io/buffered.rs
--- a/libcoreio/src/io/cursor.rs
+++ b/libcoreio/src/io/cursor.rs
@ -0,0 +1,896 @@
+use crate::io::prelude::*;
+
+use core::cmp;
+use crate::io::{self, Error, ErrorKind, Initializer, SeekFrom};
+
+#[cfg(feature = "collections")]
+use core::convert::TryInto;
+
+#[cfg(feature="collections")] 
+use collections::vec::Vec;
+
+#[cfg(feature = "alloc")]
+use alloc::boxed::Box;
+
+/// A `Cursor` wraps an in-memory buffer and provides it with a
+/// [`Seek`] implementation.
+///
+/// `Cursor`s are used with in-memory buffers, anything implementing
+/// `AsRef<[u8]>`, to allow them to implement [`Read`] and/or [`Write`],
+/// allowing these buffers to be used anywhere you might use a reader or writer
+/// that does actual I/O.
+///
+/// The standard library implements some I/O traits on various types which
+/// are commonly used as a buffer, like `Cursor<`[`Vec`]`<u8>>` and
+/// `Cursor<`[`&[u8]`][bytes]`>`.
+///
+/// # Examples
+///
+/// We may want to write bytes to a [`File`] in our production
+/// code, but use an in-memory buffer in our tests. We can do this with
+/// `Cursor`:
+///
+/// [`Seek`]: trait.Seek.html
+/// [`Read`]: ../../std/io/trait.Read.html
+/// [`Write`]: ../../std/io/trait.Write.html
+/// [`Vec`]: ../../std/vec/struct.Vec.html
+/// [bytes]: ../../std/primitive.slice.html
+/// [`File`]: ../fs/struct.File.html
+///
+/// ```no_run
+/// use std::io::prelude::*;
+/// use std::io::{self, SeekFrom};
+/// use std::fs::File;
+///
+/// // a library function we've written
+/// fn write_ten_bytes_at_end<W: Write + Seek>(writer: &mut W) -> io::Result<()> {
+///     writer.seek(SeekFrom::End(-10))?;
+///
+///     for i in 0..10 {
+///         writer.write(&[i])?;
+///     }
+///
+///     // all went well
+///     Ok(())
+/// }
+///
+/// # fn foo() -> io::Result<()> {
+/// // Here's some code that uses this library function.
+/// //
+/// // We might want to use a BufReader here for efficiency, but let's
+/// // keep this example focused.
+/// let mut file = File::create("foo.txt")?;
+///
+/// write_ten_bytes_at_end(&mut file)?;
+/// # Ok(())
+/// # }
+///
+/// // now let's write a test
+/// #[test]
+/// fn test_writes_bytes() {
+///     // setting up a real File is much slower than an in-memory buffer,
+///     // let's use a cursor instead
+///     use std::io::Cursor;
+///     let mut buff = Cursor::new(vec![0; 15]);
+///
+///     write_ten_bytes_at_end(&mut buff).unwrap();
+///
+///     assert_eq!(&buff.get_ref()[5..15], &[0, 1, 2, 3, 4, 5, 6, 7, 8, 9]);
+/// }
+/// ```
+#[derive(Clone, Debug, Default, Eq, PartialEq)]
+pub struct Cursor<T> {
+    inner: T,
+    pos: u64,
+}
+
+impl<T> Cursor<T> {
+    /// Creates a new cursor wrapping the provided underlying in-memory buffer.
+    ///
+    /// Cursor initial position is `0` even if underlying buffer (e.g., `Vec`)
+    /// is not empty. So writing to cursor starts with overwriting `Vec`
+    /// content, not with appending to it.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::Cursor;
+    ///
+    /// let buff = Cursor::new(Vec::new());
+    /// # fn force_inference(_: &Cursor<Vec<u8>>) {}
+    /// # force_inference(&buff);
+    /// ```
+    pub fn new(inner: T) -> Cursor<T> {
+        Cursor { pos: 0, inner }
+    }
+
+    /// Consumes this cursor, returning the underlying value.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::Cursor;
+    ///
+    /// let buff = Cursor::new(Vec::new());
+    /// # fn force_inference(_: &Cursor<Vec<u8>>) {}
+    /// # force_inference(&buff);
+    ///
+    /// let vec = buff.into_inner();
+    /// ```
+    pub fn into_inner(self) -> T {
+        self.inner
+    }
+
+    /// Gets a reference to the underlying value in this cursor.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::Cursor;
+    ///
+    /// let buff = Cursor::new(Vec::new());
+    /// # fn force_inference(_: &Cursor<Vec<u8>>) {}
+    /// # force_inference(&buff);
+    ///
+    /// let reference = buff.get_ref();
+    /// ```
+    pub fn get_ref(&self) -> &T {
+        &self.inner
+    }
+
+    /// Gets a mutable reference to the underlying value in this cursor.
+    ///
+    /// Care should be taken to avoid modifying the internal I/O state of the
+    /// underlying value as it may corrupt this cursor's position.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::Cursor;
+    ///
+    /// let mut buff = Cursor::new(Vec::new());
+    /// # fn force_inference(_: &Cursor<Vec<u8>>) {}
+    /// # force_inference(&buff);
+    ///
+    /// let reference = buff.get_mut();
+    /// ```
+    pub fn get_mut(&mut self) -> &mut T {
+        &mut self.inner
+    }
+
+    /// Returns the current position of this cursor.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::Cursor;
+    /// use std::io::prelude::*;
+    /// use std::io::SeekFrom;
+    ///
+    /// let mut buff = Cursor::new(vec![1, 2, 3, 4, 5]);
+    ///
+    /// assert_eq!(buff.position(), 0);
+    ///
+    /// buff.seek(SeekFrom::Current(2)).unwrap();
+    /// assert_eq!(buff.position(), 2);
+    ///
+    /// buff.seek(SeekFrom::Current(-1)).unwrap();
+    /// assert_eq!(buff.position(), 1);
+    /// ```
+    pub fn position(&self) -> u64 {
+        self.pos
+    }
+
+    /// Sets the position of this cursor.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::Cursor;
+    ///
+    /// let mut buff = Cursor::new(vec![1, 2, 3, 4, 5]);
+    ///
+    /// assert_eq!(buff.position(), 0);
+    ///
+    /// buff.set_position(2);
+    /// assert_eq!(buff.position(), 2);
+    ///
+    /// buff.set_position(4);
+    /// assert_eq!(buff.position(), 4);
+    /// ```
+    pub fn set_position(&mut self, pos: u64) {
+        self.pos = pos;
+    }
+}
+
+impl<T> io::Seek for Cursor<T>
+where
+    T: AsRef<[u8]>,
+{
+    fn seek(&mut self, style: SeekFrom) -> io::Result<u64> {
+        let (base_pos, offset) = match style {
+            SeekFrom::Start(n) => {
+                self.pos = n;
+                return Ok(n);
+            }
+            SeekFrom::End(n) => (self.inner.as_ref().len() as u64, n),
+            SeekFrom::Current(n) => (self.pos, n),
+        };
+        let new_pos = if offset >= 0 {
+            base_pos.checked_add(offset as u64)
+        } else {
+            base_pos.checked_sub((offset.wrapping_neg()) as u64)
+        };
+        match new_pos {
+            Some(n) => {
+                self.pos = n;
+                Ok(self.pos)
+            }
+            None => Err(Error::new(
+                ErrorKind::InvalidInput,
+                "invalid seek to a negative or overflowing position",
+            )),
+        }
+    }
+
+    fn stream_len(&mut self) -> io::Result<u64> {
+        Ok(self.inner.as_ref().len() as u64)
+    }
+
+    fn stream_position(&mut self) -> io::Result<u64> {
+        Ok(self.pos)
+    }
+}
+
+impl<T> Read for Cursor<T>
+where
+    T: AsRef<[u8]>,
+{
+    fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
+        let n = Read::read(&mut self.get_ref().as_ref(), buf)?;
+        self.pos += n as u64;
+        Ok(n)
+    }
+
+    fn read_exact(&mut self, buf: &mut [u8]) -> io::Result<()> {
+        let n = buf.len();
+        Read::read_exact(&mut self.get_ref().as_ref(), buf)?;
+        self.pos += n as u64;
+        Ok(())
+    }
+
+    #[inline]
+    unsafe fn initializer(&self) -> Initializer {
+        Initializer::nop()
+    }
+}
+
+#[cfg(feature = "collections")]
+impl<T> BufRead for Cursor<T>
+where
+    T: AsRef<[u8]>,
+{
+    fn fill_buf(&mut self) -> io::Result<&[u8]> {
+        let amt = cmp::min(self.pos, self.inner.as_ref().len() as u64);
+        Ok(&self.inner.as_ref()[(amt as usize)..])
+    }
+    fn consume(&mut self, amt: usize) {
+        self.pos += amt as u64;
+    }
+}
+
+// Non-resizing write implementation
+#[inline]
+fn slice_write(pos_mut: &mut u64, slice: &mut [u8], buf: &[u8]) -> io::Result<usize> {
+    let pos = cmp::min(*pos_mut, slice.len() as u64);
+    let amt = (&mut slice[(pos as usize)..]).write(buf)?;
+    *pos_mut += amt as u64;
+    Ok(amt)
+}
+
+// Resizing write implementation
+#[cfg(feature = "collections")]
+fn vec_write(pos_mut: &mut u64, vec: &mut Vec<u8>, buf: &[u8]) -> io::Result<usize> {
+    let pos: usize = (*pos_mut).try_into().map_err(|_| {
+        Error::new(
+            ErrorKind::InvalidInput,
+            "cursor position exceeds maximum possible vector length",
+        )
+    })?;
+    // Make sure the internal buffer is as least as big as where we
+    // currently are
+    let len = vec.len();
+    if len < pos {
+        // use `resize` so that the zero filling is as efficient as possible
+        vec.resize(pos, 0);
+    }
+    // Figure out what bytes will be used to overwrite what's currently
+    // there (left), and what will be appended on the end (right)
+    {
+        let space = vec.len() - pos;
+        let (left, right) = buf.split_at(cmp::min(space, buf.len()));
+        vec[pos..pos + left.len()].copy_from_slice(left);
+        vec.extend_from_slice(right);
+    }
+
+    // Bump us forward
+    *pos_mut = (pos + buf.len()) as u64;
+    Ok(buf.len())
+}
+
+impl Write for Cursor<&mut [u8]> {
+    #[inline]
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        slice_write(&mut self.pos, self.inner, buf)
+    }
+
+    #[inline]
+    fn flush(&mut self) -> io::Result<()> {
+        Ok(())
+    }
+}
+
+#[cfg(feature = "collections")]
+impl Write for Cursor<&mut Vec<u8>> {
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        vec_write(&mut self.pos, self.inner, buf)
+    }
+
+    #[inline]
+    fn flush(&mut self) -> io::Result<()> {
+        Ok(())
+    }
+}
+
+#[cfg(feature = "collections")]
+impl Write for Cursor<Vec<u8>> {
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        vec_write(&mut self.pos, &mut self.inner, buf)
+    }
+
+    #[inline]
+    fn flush(&mut self) -> io::Result<()> {
+        Ok(())
+    }
+}
+
+#[cfg(feature = "alloc")]
+impl Write for Cursor<Box<[u8]>> {
+    #[inline]
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        slice_write(&mut self.pos, &mut self.inner, buf)
+    }
+
+    #[inline]
+    fn flush(&mut self) -> io::Result<()> {
+        Ok(())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::io::prelude::*;
+    use crate::io::{Cursor, IoSlice, IoSliceMut, SeekFrom};
+
+    #[test]
+    fn test_vec_writer() {
+        let mut writer = Vec::new();
+        assert_eq!(writer.write(&[0]).unwrap(), 1);
+        assert_eq!(writer.write(&[1, 2, 3]).unwrap(), 3);
+        assert_eq!(writer.write(&[4, 5, 6, 7]).unwrap(), 4);
+        assert_eq!(
+            writer
+                .write_vectored(&[IoSlice::new(&[]), IoSlice::new(&[8, 9]), IoSlice::new(&[10])],)
+                .unwrap(),
+            3
+        );
+        let b: &[_] = &[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10];
+        assert_eq!(writer, b);
+    }
+
+    #[test]
+    fn test_mem_writer() {
+        let mut writer = Cursor::new(Vec::new());
+        assert_eq!(writer.write(&[0]).unwrap(), 1);
+        assert_eq!(writer.write(&[1, 2, 3]).unwrap(), 3);
+        assert_eq!(writer.write(&[4, 5, 6, 7]).unwrap(), 4);
+        assert_eq!(
+            writer
+                .write_vectored(&[IoSlice::new(&[]), IoSlice::new(&[8, 9]), IoSlice::new(&[10])],)
+                .unwrap(),
+            3
+        );
+        let b: &[_] = &[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10];
+        assert_eq!(&writer.get_ref()[..], b);
+    }
+
+    #[test]
+    fn test_mem_mut_writer() {
+        let mut vec = Vec::new();
+        let mut writer = Cursor::new(&mut vec);
+        assert_eq!(writer.write(&[0]).unwrap(), 1);
+        assert_eq!(writer.write(&[1, 2, 3]).unwrap(), 3);
+        assert_eq!(writer.write(&[4, 5, 6, 7]).unwrap(), 4);
+        assert_eq!(
+            writer
+                .write_vectored(&[IoSlice::new(&[]), IoSlice::new(&[8, 9]), IoSlice::new(&[10])],)
+                .unwrap(),
+            3
+        );
+        let b: &[_] = &[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10];
+        assert_eq!(&writer.get_ref()[..], b);
+    }
+
+    #[test]
+    fn test_box_slice_writer() {
+        let mut writer = Cursor::new(vec![0u8; 9].into_boxed_slice());
+        assert_eq!(writer.position(), 0);
+        assert_eq!(writer.write(&[0]).unwrap(), 1);
+        assert_eq!(writer.position(), 1);
+        assert_eq!(writer.write(&[1, 2, 3]).unwrap(), 3);
+        assert_eq!(writer.write(&[4, 5, 6, 7]).unwrap(), 4);
+        assert_eq!(writer.position(), 8);
+        assert_eq!(writer.write(&[]).unwrap(), 0);
+        assert_eq!(writer.position(), 8);
+
+        assert_eq!(writer.write(&[8, 9]).unwrap(), 1);
+        assert_eq!(writer.write(&[10]).unwrap(), 0);
+        let b: &[_] = &[0, 1, 2, 3, 4, 5, 6, 7, 8];
+        assert_eq!(&**writer.get_ref(), b);
+    }
+
+    #[test]
+    fn test_box_slice_writer_vectored() {
+        let mut writer = Cursor::new(vec![0u8; 9].into_boxed_slice());
+        assert_eq!(writer.position(), 0);
+        assert_eq!(writer.write_vectored(&[IoSlice::new(&[0])]).unwrap(), 1);
+        assert_eq!(writer.position(), 1);
+        assert_eq!(
+            writer
+                .write_vectored(&[IoSlice::new(&[1, 2, 3]), IoSlice::new(&[4, 5, 6, 7]),])
+                .unwrap(),
+            7,
+        );
+        assert_eq!(writer.position(), 8);
+        assert_eq!(writer.write_vectored(&[]).unwrap(), 0);
+        assert_eq!(writer.position(), 8);
+
+        assert_eq!(writer.write_vectored(&[IoSlice::new(&[8, 9])]).unwrap(), 1);
+        assert_eq!(writer.write_vectored(&[IoSlice::new(&[10])]).unwrap(), 0);
+        let b: &[_] = &[0, 1, 2, 3, 4, 5, 6, 7, 8];
+        assert_eq!(&**writer.get_ref(), b);
+    }
+
+    #[test]
+    fn test_buf_writer() {
+        let mut buf = [0 as u8; 9];
+        {
+            let mut writer = Cursor::new(&mut buf[..]);
+            assert_eq!(writer.position(), 0);
+            assert_eq!(writer.write(&[0]).unwrap(), 1);
+            assert_eq!(writer.position(), 1);
+            assert_eq!(writer.write(&[1, 2, 3]).unwrap(), 3);
+            assert_eq!(writer.write(&[4, 5, 6, 7]).unwrap(), 4);
+            assert_eq!(writer.position(), 8);
+            assert_eq!(writer.write(&[]).unwrap(), 0);
+            assert_eq!(writer.position(), 8);
+
+            assert_eq!(writer.write(&[8, 9]).unwrap(), 1);
+            assert_eq!(writer.write(&[10]).unwrap(), 0);
+        }
+        let b: &[_] = &[0, 1, 2, 3, 4, 5, 6, 7, 8];
+        assert_eq!(buf, b);
+    }
+
+    #[test]
+    fn test_buf_writer_vectored() {
+        let mut buf = [0 as u8; 9];
+        {
+            let mut writer = Cursor::new(&mut buf[..]);
+            assert_eq!(writer.position(), 0);
+            assert_eq!(writer.write_vectored(&[IoSlice::new(&[0])]).unwrap(), 1);
+            assert_eq!(writer.position(), 1);
+            assert_eq!(
+                writer
+                    .write_vectored(&[IoSlice::new(&[1, 2, 3]), IoSlice::new(&[4, 5, 6, 7])],)
+                    .unwrap(),
+                7,
+            );
+            assert_eq!(writer.position(), 8);
+            assert_eq!(writer.write_vectored(&[]).unwrap(), 0);
+            assert_eq!(writer.position(), 8);
+
+            assert_eq!(writer.write_vectored(&[IoSlice::new(&[8, 9])]).unwrap(), 1);
+            assert_eq!(writer.write_vectored(&[IoSlice::new(&[10])]).unwrap(), 0);
+        }
+        let b: &[_] = &[0, 1, 2, 3, 4, 5, 6, 7, 8];
+        assert_eq!(buf, b);
+    }
+
+    #[test]
+    fn test_buf_writer_seek() {
+        let mut buf = [0 as u8; 8];
+        {
+            let mut writer = Cursor::new(&mut buf[..]);
+            assert_eq!(writer.position(), 0);
+            assert_eq!(writer.write(&[1]).unwrap(), 1);
+            assert_eq!(writer.position(), 1);
+
+            assert_eq!(writer.seek(SeekFrom::Start(2)).unwrap(), 2);
+            assert_eq!(writer.position(), 2);
+            assert_eq!(writer.write(&[2]).unwrap(), 1);
+            assert_eq!(writer.position(), 3);
+
+            assert_eq!(writer.seek(SeekFrom::Current(-2)).unwrap(), 1);
+            assert_eq!(writer.position(), 1);
+            assert_eq!(writer.write(&[3]).unwrap(), 1);
+            assert_eq!(writer.position(), 2);
+
+            assert_eq!(writer.seek(SeekFrom::End(-1)).unwrap(), 7);
+            assert_eq!(writer.position(), 7);
+            assert_eq!(writer.write(&[4]).unwrap(), 1);
+            assert_eq!(writer.position(), 8);
+        }
+        let b: &[_] = &[1, 3, 2, 0, 0, 0, 0, 4];
+        assert_eq!(buf, b);
+    }
+
+    #[test]
+    fn test_buf_writer_error() {
+        let mut buf = [0 as u8; 2];
+        let mut writer = Cursor::new(&mut buf[..]);
+        assert_eq!(writer.write(&[0]).unwrap(), 1);
+        assert_eq!(writer.write(&[0, 0]).unwrap(), 1);
+        assert_eq!(writer.write(&[0, 0]).unwrap(), 0);
+    }
+
+    #[test]
+    fn test_mem_reader() {
+        let mut reader = Cursor::new(vec![0, 1, 2, 3, 4, 5, 6, 7]);
+        let mut buf = [];
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+        assert_eq!(reader.position(), 0);
+        let mut buf = [0];
+        assert_eq!(reader.read(&mut buf).unwrap(), 1);
+        assert_eq!(reader.position(), 1);
+        let b: &[_] = &[0];
+        assert_eq!(buf, b);
+        let mut buf = [0; 4];
+        assert_eq!(reader.read(&mut buf).unwrap(), 4);
+        assert_eq!(reader.position(), 5);
+        let b: &[_] = &[1, 2, 3, 4];
+        assert_eq!(buf, b);
+        assert_eq!(reader.read(&mut buf).unwrap(), 3);
+        let b: &[_] = &[5, 6, 7];
+        assert_eq!(&buf[..3], b);
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+    }
+
+    #[test]
+    fn test_mem_reader_vectored() {
+        let mut reader = Cursor::new(vec![0, 1, 2, 3, 4, 5, 6, 7]);
+        let mut buf = [];
+        assert_eq!(reader.read_vectored(&mut [IoSliceMut::new(&mut buf)]).unwrap(), 0);
+        assert_eq!(reader.position(), 0);
+        let mut buf = [0];
+        assert_eq!(
+            reader
+                .read_vectored(&mut [IoSliceMut::new(&mut []), IoSliceMut::new(&mut buf),])
+                .unwrap(),
+            1,
+        );
+        assert_eq!(reader.position(), 1);
+        let b: &[_] = &[0];
+        assert_eq!(buf, b);
+        let mut buf1 = [0; 4];
+        let mut buf2 = [0; 4];
+        assert_eq!(
+            reader
+                .read_vectored(&mut [IoSliceMut::new(&mut buf1), IoSliceMut::new(&mut buf2),])
+                .unwrap(),
+            7,
+        );
+        let b1: &[_] = &[1, 2, 3, 4];
+        let b2: &[_] = &[5, 6, 7];
+        assert_eq!(buf1, b1);
+        assert_eq!(&buf2[..3], b2);
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+    }
+
+    #[test]
+    fn test_boxed_slice_reader() {
+        let mut reader = Cursor::new(vec![0, 1, 2, 3, 4, 5, 6, 7].into_boxed_slice());
+        let mut buf = [];
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+        assert_eq!(reader.position(), 0);
+        let mut buf = [0];
+        assert_eq!(reader.read(&mut buf).unwrap(), 1);
+        assert_eq!(reader.position(), 1);
+        let b: &[_] = &[0];
+        assert_eq!(buf, b);
+        let mut buf = [0; 4];
+        assert_eq!(reader.read(&mut buf).unwrap(), 4);
+        assert_eq!(reader.position(), 5);
+        let b: &[_] = &[1, 2, 3, 4];
+        assert_eq!(buf, b);
+        assert_eq!(reader.read(&mut buf).unwrap(), 3);
+        let b: &[_] = &[5, 6, 7];
+        assert_eq!(&buf[..3], b);
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+    }
+
+    #[test]
+    fn test_boxed_slice_reader_vectored() {
+        let mut reader = Cursor::new(vec![0, 1, 2, 3, 4, 5, 6, 7].into_boxed_slice());
+        let mut buf = [];
+        assert_eq!(reader.read_vectored(&mut [IoSliceMut::new(&mut buf)]).unwrap(), 0);
+        assert_eq!(reader.position(), 0);
+        let mut buf = [0];
+        assert_eq!(
+            reader
+                .read_vectored(&mut [IoSliceMut::new(&mut []), IoSliceMut::new(&mut buf),])
+                .unwrap(),
+            1,
+        );
+        assert_eq!(reader.position(), 1);
+        let b: &[_] = &[0];
+        assert_eq!(buf, b);
+        let mut buf1 = [0; 4];
+        let mut buf2 = [0; 4];
+        assert_eq!(
+            reader
+                .read_vectored(&mut [IoSliceMut::new(&mut buf1), IoSliceMut::new(&mut buf2)],)
+                .unwrap(),
+            7,
+        );
+        let b1: &[_] = &[1, 2, 3, 4];
+        let b2: &[_] = &[5, 6, 7];
+        assert_eq!(buf1, b1);
+        assert_eq!(&buf2[..3], b2);
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+    }
+
+    #[test]
+    fn read_to_end() {
+        let mut reader = Cursor::new(vec![0, 1, 2, 3, 4, 5, 6, 7]);
+        let mut v = Vec::new();
+        reader.read_to_end(&mut v).unwrap();
+        assert_eq!(v, [0, 1, 2, 3, 4, 5, 6, 7]);
+    }
+
+    #[test]
+    fn test_slice_reader() {
+        let in_buf = vec![0, 1, 2, 3, 4, 5, 6, 7];
+        let reader = &mut &in_buf[..];
+        let mut buf = [];
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+        let mut buf = [0];
+        assert_eq!(reader.read(&mut buf).unwrap(), 1);
+        assert_eq!(reader.len(), 7);
+        let b: &[_] = &[0];
+        assert_eq!(&buf[..], b);
+        let mut buf = [0; 4];
+        assert_eq!(reader.read(&mut buf).unwrap(), 4);
+        assert_eq!(reader.len(), 3);
+        let b: &[_] = &[1, 2, 3, 4];
+        assert_eq!(&buf[..], b);
+        assert_eq!(reader.read(&mut buf).unwrap(), 3);
+        let b: &[_] = &[5, 6, 7];
+        assert_eq!(&buf[..3], b);
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+    }
+
+    #[test]
+    fn test_slice_reader_vectored() {
+        let in_buf = vec![0, 1, 2, 3, 4, 5, 6, 7];
+        let reader = &mut &in_buf[..];
+        let mut buf = [];
+        assert_eq!(reader.read_vectored(&mut [IoSliceMut::new(&mut buf)]).unwrap(), 0);
+        let mut buf = [0];
+        assert_eq!(
+            reader
+                .read_vectored(&mut [IoSliceMut::new(&mut []), IoSliceMut::new(&mut buf),])
+                .unwrap(),
+            1,
+        );
+        assert_eq!(reader.len(), 7);
+        let b: &[_] = &[0];
+        assert_eq!(buf, b);
+        let mut buf1 = [0; 4];
+        let mut buf2 = [0; 4];
+        assert_eq!(
+            reader
+                .read_vectored(&mut [IoSliceMut::new(&mut buf1), IoSliceMut::new(&mut buf2)],)
+                .unwrap(),
+            7,
+        );
+        let b1: &[_] = &[1, 2, 3, 4];
+        let b2: &[_] = &[5, 6, 7];
+        assert_eq!(buf1, b1);
+        assert_eq!(&buf2[..3], b2);
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+    }
+
+    #[test]
+    fn test_read_exact() {
+        let in_buf = vec![0, 1, 2, 3, 4, 5, 6, 7];
+        let reader = &mut &in_buf[..];
+        let mut buf = [];
+        assert!(reader.read_exact(&mut buf).is_ok());
+        let mut buf = [8];
+        assert!(reader.read_exact(&mut buf).is_ok());
+        assert_eq!(buf[0], 0);
+        assert_eq!(reader.len(), 7);
+        let mut buf = [0, 0, 0, 0, 0, 0, 0];
+        assert!(reader.read_exact(&mut buf).is_ok());
+        assert_eq!(buf, [1, 2, 3, 4, 5, 6, 7]);
+        assert_eq!(reader.len(), 0);
+        let mut buf = [0];
+        assert!(reader.read_exact(&mut buf).is_err());
+    }
+
+    #[test]
+    fn test_buf_reader() {
+        let in_buf = vec![0, 1, 2, 3, 4, 5, 6, 7];
+        let mut reader = Cursor::new(&in_buf[..]);
+        let mut buf = [];
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+        assert_eq!(reader.position(), 0);
+        let mut buf = [0];
+        assert_eq!(reader.read(&mut buf).unwrap(), 1);
+        assert_eq!(reader.position(), 1);
+        let b: &[_] = &[0];
+        assert_eq!(buf, b);
+        let mut buf = [0; 4];
+        assert_eq!(reader.read(&mut buf).unwrap(), 4);
+        assert_eq!(reader.position(), 5);
+        let b: &[_] = &[1, 2, 3, 4];
+        assert_eq!(buf, b);
+        assert_eq!(reader.read(&mut buf).unwrap(), 3);
+        let b: &[_] = &[5, 6, 7];
+        assert_eq!(&buf[..3], b);
+        assert_eq!(reader.read(&mut buf).unwrap(), 0);
+    }
+
+    #[test]
+    fn seek_past_end() {
+        let buf = [0xff];
+        let mut r = Cursor::new(&buf[..]);
+        assert_eq!(r.seek(SeekFrom::Start(10)).unwrap(), 10);
+        assert_eq!(r.read(&mut [0]).unwrap(), 0);
+
+        let mut r = Cursor::new(vec![10]);
+        assert_eq!(r.seek(SeekFrom::Start(10)).unwrap(), 10);
+        assert_eq!(r.read(&mut [0]).unwrap(), 0);
+
+        let mut buf = [0];
+        let mut r = Cursor::new(&mut buf[..]);
+        assert_eq!(r.seek(SeekFrom::Start(10)).unwrap(), 10);
+        assert_eq!(r.write(&[3]).unwrap(), 0);
+
+        let mut r = Cursor::new(vec![10].into_boxed_slice());
+        assert_eq!(r.seek(SeekFrom::Start(10)).unwrap(), 10);
+        assert_eq!(r.write(&[3]).unwrap(), 0);
+    }
+
+    #[test]
+    fn seek_past_i64() {
+        let buf = [0xff];
+        let mut r = Cursor::new(&buf[..]);
+        assert_eq!(r.seek(SeekFrom::Start(6)).unwrap(), 6);
+        assert_eq!(r.seek(SeekFrom::Current(0x7ffffffffffffff0)).unwrap(), 0x7ffffffffffffff6);
+        assert_eq!(r.seek(SeekFrom::Current(0x10)).unwrap(), 0x8000000000000006);
+        assert_eq!(r.seek(SeekFrom::Current(0)).unwrap(), 0x8000000000000006);
+        assert!(r.seek(SeekFrom::Current(0x7ffffffffffffffd)).is_err());
+        assert_eq!(r.seek(SeekFrom::Current(-0x8000000000000000)).unwrap(), 6);
+
+        let mut r = Cursor::new(vec![10]);
+        assert_eq!(r.seek(SeekFrom::Start(6)).unwrap(), 6);
+        assert_eq!(r.seek(SeekFrom::Current(0x7ffffffffffffff0)).unwrap(), 0x7ffffffffffffff6);
+        assert_eq!(r.seek(SeekFrom::Current(0x10)).unwrap(), 0x8000000000000006);
+        assert_eq!(r.seek(SeekFrom::Current(0)).unwrap(), 0x8000000000000006);
+        assert!(r.seek(SeekFrom::Current(0x7ffffffffffffffd)).is_err());
+        assert_eq!(r.seek(SeekFrom::Current(-0x8000000000000000)).unwrap(), 6);
+
+        let mut buf = [0];
+        let mut r = Cursor::new(&mut buf[..]);
+        assert_eq!(r.seek(SeekFrom::Start(6)).unwrap(), 6);
+        assert_eq!(r.seek(SeekFrom::Current(0x7ffffffffffffff0)).unwrap(), 0x7ffffffffffffff6);
+        assert_eq!(r.seek(SeekFrom::Current(0x10)).unwrap(), 0x8000000000000006);
+        assert_eq!(r.seek(SeekFrom::Current(0)).unwrap(), 0x8000000000000006);
+        assert!(r.seek(SeekFrom::Current(0x7ffffffffffffffd)).is_err());
+        assert_eq!(r.seek(SeekFrom::Current(-0x8000000000000000)).unwrap(), 6);
+
+        let mut r = Cursor::new(vec![10].into_boxed_slice());
+        assert_eq!(r.seek(SeekFrom::Start(6)).unwrap(), 6);
+        assert_eq!(r.seek(SeekFrom::Current(0x7ffffffffffffff0)).unwrap(), 0x7ffffffffffffff6);
+        assert_eq!(r.seek(SeekFrom::Current(0x10)).unwrap(), 0x8000000000000006);
+        assert_eq!(r.seek(SeekFrom::Current(0)).unwrap(), 0x8000000000000006);
+        assert!(r.seek(SeekFrom::Current(0x7ffffffffffffffd)).is_err());
+        assert_eq!(r.seek(SeekFrom::Current(-0x8000000000000000)).unwrap(), 6);
+    }
+
+    #[test]
+    fn seek_before_0() {
+        let buf = [0xff];
+        let mut r = Cursor::new(&buf[..]);
+        assert!(r.seek(SeekFrom::End(-2)).is_err());
+
+        let mut r = Cursor::new(vec![10]);
+        assert!(r.seek(SeekFrom::End(-2)).is_err());
+
+        let mut buf = [0];
+        let mut r = Cursor::new(&mut buf[..]);
+        assert!(r.seek(SeekFrom::End(-2)).is_err());
+
+        let mut r = Cursor::new(vec![10].into_boxed_slice());
+        assert!(r.seek(SeekFrom::End(-2)).is_err());
+    }
+
+    #[test]
+    fn test_seekable_mem_writer() {
+        let mut writer = Cursor::new(Vec::<u8>::new());
+        assert_eq!(writer.position(), 0);
+        assert_eq!(writer.write(&[0]).unwrap(), 1);
+        assert_eq!(writer.position(), 1);
+        assert_eq!(writer.write(&[1, 2, 3]).unwrap(), 3);
+        assert_eq!(writer.write(&[4, 5, 6, 7]).unwrap(), 4);
+        assert_eq!(writer.position(), 8);
+        let b: &[_] = &[0, 1, 2, 3, 4, 5, 6, 7];
+        assert_eq!(&writer.get_ref()[..], b);
+
+        assert_eq!(writer.seek(SeekFrom::Start(0)).unwrap(), 0);
+        assert_eq!(writer.position(), 0);
+        assert_eq!(writer.write(&[3, 4]).unwrap(), 2);
+        let b: &[_] = &[3, 4, 2, 3, 4, 5, 6, 7];
+        assert_eq!(&writer.get_ref()[..], b);
+
+        assert_eq!(writer.seek(SeekFrom::Current(1)).unwrap(), 3);
+        assert_eq!(writer.write(&[0, 1]).unwrap(), 2);
+        let b: &[_] = &[3, 4, 2, 0, 1, 5, 6, 7];
+        assert_eq!(&writer.get_ref()[..], b);
+
+        assert_eq!(writer.seek(SeekFrom::End(-1)).unwrap(), 7);
+        assert_eq!(writer.write(&[1, 2]).unwrap(), 2);
+        let b: &[_] = &[3, 4, 2, 0, 1, 5, 6, 1, 2];
+        assert_eq!(&writer.get_ref()[..], b);
+
+        assert_eq!(writer.seek(SeekFrom::End(1)).unwrap(), 10);
+        assert_eq!(writer.write(&[1]).unwrap(), 1);
+        let b: &[_] = &[3, 4, 2, 0, 1, 5, 6, 1, 2, 0, 1];
+        assert_eq!(&writer.get_ref()[..], b);
+    }
+
+    #[test]
+    fn vec_seek_past_end() {
+        let mut r = Cursor::new(Vec::new());
+        assert_eq!(r.seek(SeekFrom::Start(10)).unwrap(), 10);
+        assert_eq!(r.write(&[3]).unwrap(), 1);
+    }
+
+    #[test]
+    fn vec_seek_before_0() {
+        let mut r = Cursor::new(Vec::new());
+        assert!(r.seek(SeekFrom::End(-2)).is_err());
+    }
+
+    #[test]
+    #[cfg(target_pointer_width = "32")]
+    fn vec_seek_and_write_past_usize_max() {
+        let mut c = Cursor::new(Vec::new());
+        c.set_position(<usize>::max_value() as u64 + 1);
+        assert!(c.write_all(&[1, 2, 3]).is_err());
+    }
+
+    #[test]
+    fn test_partial_eq() {
+        assert_eq!(Cursor::new(Vec::<u8>::new()), Cursor::new(Vec::<u8>::new()));
+    }
+
+    #[test]
+    fn test_eq() {
+        struct AssertEq<T: Eq>(pub T);
+
+        let _: AssertEq<Cursor<Vec<u8>>> = AssertEq(Cursor::new(Vec::new()));
+    }
+}
--- a/libcoreio/src/io/error.rs
+++ b/libcoreio/src/io/error.rs
@ -0,0 +1,551 @@
+#[cfg(feature="alloc")] use alloc::boxed::Box;
+#[cfg(not(feature="alloc"))] use ::FakeBox as Box;
+use core::convert::Into;
+use core::fmt;
+use core::marker::{Send, Sync};
+use core::option::Option::{self, Some, None};
+use core::result;
+#[cfg(feature="collections")] use collections::string::String;
+#[cfg(not(feature="collections"))] use ::ErrorString as String;
+use core::convert::From;
+
+/// A specialized [`Result`](../result/enum.Result.html) type for I/O
+/// operations.
+///
+/// This type is broadly used across [`std::io`] for any operation which may
+/// produce an error.
+///
+/// This typedef is generally used to avoid writing out [`io::Error`] directly and
+/// is otherwise a direct mapping to [`Result`].
+///
+/// While usual Rust style is to import types directly, aliases of [`Result`]
+/// often are not, to make it easier to distinguish between them. [`Result`] is
+/// generally assumed to be [`std::result::Result`][`Result`], and so users of this alias
+/// will generally use `io::Result` instead of shadowing the prelude's import
+/// of [`std::result::Result`][`Result`].
+///
+/// [`std::io`]: ../io/index.html
+/// [`io::Error`]: ../io/struct.Error.html
+/// [`Result`]: ../result/enum.Result.html
+///
+/// # Examples
+///
+/// A convenience function that bubbles an `io::Result` to its caller:
+///
+/// ```
+/// use std::io;
+///
+/// fn get_string() -> io::Result<String> {
+///     let mut buffer = String::new();
+///
+///     io::stdin().read_line(&mut buffer)?;
+///
+///     Ok(buffer)
+/// }
+/// ```
+pub type Result<T> = result::Result<T, Error>;
+
+/// The error type for I/O operations of the [`Read`], [`Write`], [`Seek`], and
+/// associated traits.
+///
+/// Errors mostly originate from the underlying OS, but custom instances of
+/// `Error` can be created with crafted error messages and a particular value of
+/// [`ErrorKind`].
+///
+/// [`Read`]: ../io/trait.Read.html
+/// [`Write`]: ../io/trait.Write.html
+/// [`Seek`]: ../io/trait.Seek.html
+/// [`ErrorKind`]: enum.ErrorKind.html
+pub struct Error {
+    repr: Repr,
+}
+
+impl fmt::Debug for Error {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        fmt::Debug::fmt(&self.repr, f)
+    }
+}
+
+enum Repr {
+    Os(i32),
+    Simple(ErrorKind),
+    #[cfg(feature="alloc")]
+    Custom(Box<Custom>),
+    #[cfg(not(feature="alloc"))]
+    Custom(Custom),
+}
+
+#[derive(Debug)]
+struct Custom {
+    kind: ErrorKind,
+    error: String,
+}
+
+/// A list specifying general categories of I/O error.
+///
+/// This list is intended to grow over time and it is not recommended to
+/// exhaustively match against it.
+///
+/// It is used with the [`io::Error`] type.
+///
+/// [`io::Error`]: struct.Error.html
+#[derive(Clone, Copy, Debug, Eq, Hash, Ord, PartialEq, PartialOrd)]
+#[allow(deprecated)]
+#[non_exhaustive]
+pub enum ErrorKind {
+    /// An entity was not found, often a file.
+    NotFound,
+    /// The operation lacked the necessary privileges to complete.
+    PermissionDenied,
+    /// The connection was refused by the remote server.
+    ConnectionRefused,
+    /// The connection was reset by the remote server.
+    ConnectionReset,
+    /// The connection was aborted (terminated) by the remote server.
+    ConnectionAborted,
+    /// The network operation failed because it was not connected yet.
+    NotConnected,
+    /// A socket address could not be bound because the address is already in
+    /// use elsewhere.
+    AddrInUse,
+    /// A nonexistent interface was requested or the requested address was not
+    /// local.
+    AddrNotAvailable,
+    /// The operation failed because a pipe was closed.
+    BrokenPipe,
+    /// An entity already exists, often a file.
+    AlreadyExists,
+    /// The operation needs to block to complete, but the blocking operation was
+    /// requested to not occur.
+    WouldBlock,
+    /// A parameter was incorrect.
+    InvalidInput,
+    /// Data not valid for the operation were encountered.
+    ///
+    /// Unlike [`InvalidInput`], this typically means that the operation
+    /// parameters were valid, however the error was caused by malformed
+    /// input data.
+    ///
+    /// For example, a function that reads a file into a string will error with
+    /// `InvalidData` if the file's contents are not valid UTF-8.
+    ///
+    /// [`InvalidInput`]: #variant.InvalidInput
+    InvalidData,
+    /// The I/O operation's timeout expired, causing it to be canceled.
+    TimedOut,
+    /// An error returned when an operation could not be completed because a
+    /// call to [`write`] returned [`Ok(0)`].
+    ///
+    /// This typically means that an operation could only succeed if it wrote a
+    /// particular number of bytes but only a smaller number of bytes could be
+    /// written.
+    ///
+    /// [`write`]: ../../std/io/trait.Write.html#tymethod.write
+    /// [`Ok(0)`]: ../../std/io/type.Result.html
+    WriteZero,
+    /// This operation was interrupted.
+    ///
+    /// Interrupted operations can typically be retried.
+    Interrupted,
+    /// Any I/O error not part of this list.
+    Other,
+
+    /// An error returned when an operation could not be completed because an
+    /// "end of file" was reached prematurely.
+    ///
+    /// This typically means that an operation could only succeed if it read a
+    /// particular number of bytes but only a smaller number of bytes could be
+    /// read.
+    UnexpectedEof,
+}
+
+impl ErrorKind {
+    pub(crate) fn as_str(&self) -> &'static str {
+        match *self {
+            ErrorKind::NotFound => "entity not found",
+            ErrorKind::PermissionDenied => "permission denied",
+            ErrorKind::ConnectionRefused => "connection refused",
+            ErrorKind::ConnectionReset => "connection reset",
+            ErrorKind::ConnectionAborted => "connection aborted",
+            ErrorKind::NotConnected => "not connected",
+            ErrorKind::AddrInUse => "address in use",
+            ErrorKind::AddrNotAvailable => "address not available",
+            ErrorKind::BrokenPipe => "broken pipe",
+            ErrorKind::AlreadyExists => "entity already exists",
+            ErrorKind::WouldBlock => "operation would block",
+            ErrorKind::InvalidInput => "invalid input parameter",
+            ErrorKind::InvalidData => "invalid data",
+            ErrorKind::TimedOut => "timed out",
+            ErrorKind::WriteZero => "write zero",
+            ErrorKind::Interrupted => "operation interrupted",
+            ErrorKind::Other => "other os error",
+            ErrorKind::UnexpectedEof => "unexpected end of file",
+        }
+    }
+}
+
+/// Intended for use for errors not exposed to the user, where allocating onto
+/// the heap (for normal construction via Error::new) is too costly.
+impl From<ErrorKind> for Error {
+    /// Converts an [`ErrorKind`] into an [`Error`].
+    ///
+    /// This conversion allocates a new error with a simple representation of error kind.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::{Error, ErrorKind};
+    ///
+    /// let not_found = ErrorKind::NotFound;
+    /// let error = Error::from(not_found);
+    /// assert_eq!("entity not found", format!("{}", error));
+    /// ```
+    ///
+    /// [`ErrorKind`]: ../../std/io/enum.ErrorKind.html
+    /// [`Error`]: ../../std/io/struct.Error.html
+    #[inline]
+    fn from(kind: ErrorKind) -> Error {
+        Error { repr: Repr::Simple(kind) }
+    }
+}
+
+impl Error {
+    /// Creates a new I/O error from a known kind of error as well as an
+    /// arbitrary error payload.
+    ///
+    /// This function is used to generically create I/O errors which do not
+    /// originate from the OS itself. The `error` argument is an arbitrary
+    /// payload which will be contained in this `Error`.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::{Error, ErrorKind};
+    ///
+    /// // errors can be created from strings
+    /// let custom_error = Error::new(ErrorKind::Other, "oh no!");
+    ///
+    /// // errors can also be created from other errors
+    /// let custom_error2 = Error::new(ErrorKind::Interrupted, custom_error);
+    /// ```
+    pub fn new<E>(kind: ErrorKind, error: E) -> Error
+    where
+        E: Into<String>,
+    {
+        Self::_new(kind, error.into())
+    }
+
+    fn _new(kind: ErrorKind, error: String) -> Error {
+        Error { repr: Repr::Custom(Box::new(Custom { kind, error })) }
+    }
+
+    /// Creates a new instance of an `Error` from a particular OS error code.
+    ///
+    /// # Examples
+    ///
+    /// On Linux:
+    ///
+    /// ```
+    /// # if cfg!(target_os = "linux") {
+    /// use std::io;
+    ///
+    /// let error = io::Error::from_raw_os_error(22);
+    /// assert_eq!(error.kind(), io::ErrorKind::InvalidInput);
+    /// # }
+    /// ```
+    ///
+    /// On Windows:
+    ///
+    /// ```
+    /// # if cfg!(windows) {
+    /// use std::io;
+    ///
+    /// let error = io::Error::from_raw_os_error(10022);
+    /// assert_eq!(error.kind(), io::ErrorKind::InvalidInput);
+    /// # }
+    /// ```
+    pub fn from_raw_os_error(code: i32) -> Error {
+        Error { repr: Repr::Os(code) }
+    }
+
+    /// Returns the OS error that this error represents (if any).
+    ///
+    /// If this `Error` was constructed via `last_os_error` or
+    /// `from_raw_os_error`, then this function will return `Some`, otherwise
+    /// it will return `None`.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::{Error, ErrorKind};
+    ///
+    /// fn print_os_error(err: &Error) {
+    ///     if let Some(raw_os_err) = err.raw_os_error() {
+    ///         println!("raw OS error: {:?}", raw_os_err);
+    ///     } else {
+    ///         println!("Not an OS error");
+    ///     }
+    /// }
+    ///
+    /// fn main() {
+    ///     // Will print "raw OS error: ...".
+    ///     print_os_error(&Error::last_os_error());
+    ///     // Will print "Not an OS error".
+    ///     print_os_error(&Error::new(ErrorKind::Other, "oh no!"));
+    /// }
+    /// ```
+    pub fn raw_os_error(&self) -> Option<i32> {
+        match self.repr {
+            Repr::Os(i) => Some(i),
+            Repr::Custom(..) => None,
+            Repr::Simple(..) => None,
+        }
+    }
+
+    /// Returns a reference to the inner error wrapped by this error (if any).
+    ///
+    /// If this `Error` was constructed via `new` then this function will
+    /// return `Some`, otherwise it will return `None`.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::{Error, ErrorKind};
+    ///
+    /// fn print_error(err: &Error) {
+    ///     if let Some(inner_err) = err.get_ref() {
+    ///         println!("Inner error: {:?}", inner_err);
+    ///     } else {
+    ///         println!("No inner error");
+    ///     }
+    /// }
+    ///
+    /// fn main() {
+    ///     // Will print "No inner error".
+    ///     print_error(&Error::last_os_error());
+    ///     // Will print "Inner error: ...".
+    ///     print_error(&Error::new(ErrorKind::Other, "oh no!"));
+    /// }
+    /// ```
+    pub fn get_ref(&self) -> Option<&String> {
+        match self.repr {
+            Repr::Os(..) => None,
+            Repr::Simple(..) => None,
+            Repr::Custom(ref c) => Some(&c.error),
+        }
+    }
+
+    /// Returns a mutable reference to the inner error wrapped by this error
+    /// (if any).
+    ///
+    /// If this `Error` was constructed via `new` then this function will
+    /// return `Some`, otherwise it will return `None`.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::{Error, ErrorKind};
+    /// use std::{error, fmt};
+    /// use std::fmt::Display;
+    ///
+    /// #[derive(Debug)]
+    /// struct MyError {
+    ///     v: String,
+    /// }
+    ///
+    /// impl MyError {
+    ///     fn new() -> MyError {
+    ///         MyError {
+    ///             v: "oh no!".to_string()
+    ///         }
+    ///     }
+    ///
+    ///     fn change_message(&mut self, new_message: &str) {
+    ///         self.v = new_message.to_string();
+    ///     }
+    /// }
+    ///
+    /// impl error::Error for MyError {}
+    ///
+    /// impl Display for MyError {
+    ///     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+    ///         write!(f, "MyError: {}", &self.v)
+    ///     }
+    /// }
+    ///
+    /// fn change_error(mut err: Error) -> Error {
+    ///     if let Some(inner_err) = err.get_mut() {
+    ///         inner_err.downcast_mut::<MyError>().unwrap().change_message("I've been changed!");
+    ///     }
+    ///     err
+    /// }
+    ///
+    /// fn print_error(err: &Error) {
+    ///     if let Some(inner_err) = err.get_ref() {
+    ///         println!("Inner error: {}", inner_err);
+    ///     } else {
+    ///         println!("No inner error");
+    ///     }
+    /// }
+    ///
+    /// fn main() {
+    ///     // Will print "No inner error".
+    ///     print_error(&change_error(Error::last_os_error()));
+    ///     // Will print "Inner error: ...".
+    ///     print_error(&change_error(Error::new(ErrorKind::Other, MyError::new())));
+    /// }
+    /// ```
+    pub fn get_mut(&mut self) -> Option<&mut String> {
+        match self.repr {
+            Repr::Os(..) => None,
+            Repr::Simple(..) => None,
+            Repr::Custom(ref mut c) => Some(&mut c.error),
+        }
+    }
+
+    /// Consumes the `Error`, returning its inner error (if any).
+    ///
+    /// If this `Error` was constructed via `new` then this function will
+    /// return `Some`, otherwise it will return `None`.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::{Error, ErrorKind};
+    ///
+    /// fn print_error(err: Error) {
+    ///     if let Some(inner_err) = err.into_inner() {
+    ///         println!("Inner error: {}", inner_err);
+    ///     } else {
+    ///         println!("No inner error");
+    ///     }
+    /// }
+    ///
+    /// fn main() {
+    ///     // Will print "No inner error".
+    ///     print_error(Error::last_os_error());
+    ///     // Will print "Inner error: ...".
+    ///     print_error(Error::new(ErrorKind::Other, "oh no!"));
+    /// }
+    /// ```
+    pub fn into_inner(self) -> Option<String> {
+        match self.repr {
+            Repr::Os(..) => None,
+            Repr::Simple(..) => None,
+            Repr::Custom(c) => Some(c.error),
+        }
+    }
+
+    /// Returns the corresponding `ErrorKind` for this error.
+    ///
+    /// # Examples
+    ///
+    /// ```
+    /// use std::io::{Error, ErrorKind};
+    ///
+    /// fn print_error(err: Error) {
+    ///     println!("{:?}", err.kind());
+    /// }
+    ///
+    /// fn main() {
+    ///     // Will print "No inner error".
+    ///     print_error(Error::last_os_error());
+    ///     // Will print "Inner error: ...".
+    ///     print_error(Error::new(ErrorKind::AddrInUse, "oh no!"));
+    /// }
+    /// ```
+    pub fn kind(&self) -> ErrorKind {
+        match self.repr {
+            Repr::Os(_code) => ErrorKind::Other,
+            Repr::Custom(ref c) => c.kind,
+            Repr::Simple(kind) => kind,
+        }
+    }
+}
+
+impl fmt::Debug for Repr {
+    fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
+        match *self {
+            Repr::Os(code) => fmt
+                .debug_struct("Os")
+                .field("code", &code)
+                .finish(),
+            Repr::Custom(ref c) => fmt::Debug::fmt(&c, fmt),
+            Repr::Simple(kind) => fmt.debug_tuple("Kind").field(&kind).finish(),
+        }
+    }
+}
+
+impl fmt::Display for Error {
+    fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
+        match self.repr {
+            Repr::Os(code) => {
+                write!(fmt, "os error {}", code)
+            }
+            Repr::Custom(ref c) => c.error.fmt(fmt),
+            Repr::Simple(kind) => write!(fmt, "{}", kind.as_str()),
+        }
+    }
+}
+
+fn _assert_error_is_sync_send() {
+    fn _is_sync_send<T: Sync + Send>() {}
+    _is_sync_send::<Error>();
+}
+
+#[cfg(test)]
+mod test {
+    use super::{Custom, Error, ErrorKind, Repr};
+    use crate::error;
+    use crate::fmt;
+    use crate::sys::decode_error_kind;
+    use crate::sys::os::error_string;
+
+    #[test]
+    fn test_debug_error() {
+        let code = 6;
+        let msg = error_string(code);
+        let kind = decode_error_kind(code);
+        let err = Error {
+            repr: Repr::Custom(box Custom {
+                kind: ErrorKind::InvalidInput,
+                error: box Error { repr: super::Repr::Os(code) },
+            }),
+        };
+        let expected = format!(
+            "Custom {{ \
+             kind: InvalidInput, \
+             error: Os {{ \
+             code: {:?}, \
+             kind: {:?}, \
+             message: {:?} \
+             }} \
+             }}",
+            code, kind, msg
+        );
+        assert_eq!(format!("{:?}", err), expected);
+    }
+
+    #[test]
+    fn test_downcasting() {
+        #[derive(Debug)]
+        struct TestError;
+
+        impl fmt::Display for TestError {
+            fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+                f.write_str("asdf")
+            }
+        }
+
+        impl error::Error for TestError {}
+
+        // we have to call all of these UFCS style right now since method
+        // resolution won't implicitly drop the Send+Sync bounds
+        let mut err = Error::new(ErrorKind::Other, TestError);
+        assert!(err.get_ref().unwrap().is::<TestError>());
+        assert_eq!("asdf", err.get_ref().unwrap().to_string());
+        assert!(err.get_mut().unwrap().is::<TestError>());
+        let extracted = err.into_inner().unwrap();
+        extracted.downcast::<TestError>().unwrap();
+    }
+}
--- a/libcoreio/src/io/impls.rs
+++ b/libcoreio/src/io/impls.rs
@ -0,0 +1,378 @@
+use core::cmp;
+use core::fmt;
+use crate::io::{
+    self, Error, ErrorKind, Initializer, Read, Seek, SeekFrom, Write,
+};
+#[cfg(feature = "collections")] use crate::io::BufRead;
+use core::mem;
+
+#[cfg(feature="collections")] 
+use collections::{
+    vec::Vec,
+    string::String,
+};
+#[cfg(feature = "alloc")]
+use alloc::boxed::Box;
+
+// =============================================================================
+// Forwarding implementations
+
+impl<R: Read + ?Sized> Read for &mut R {
+    #[inline]
+    fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
+        (**self).read(buf)
+    }
+
+    #[inline]
+    unsafe fn initializer(&self) -> Initializer {
+        (**self).initializer()
+    }
+
+    #[cfg(feature="collections")]
+    #[inline]
+    fn read_to_end(&mut self, buf: &mut Vec<u8>) -> io::Result<usize> {
+        (**self).read_to_end(buf)
+    }
+
+    #[cfg(feature="collections")]
+    #[inline]
+    fn read_to_string(&mut self, buf: &mut String) -> io::Result<usize> {
+        (**self).read_to_string(buf)
+    }
+
+    #[inline]
+    fn read_exact(&mut self, buf: &mut [u8]) -> io::Result<()> {
+        (**self).read_exact(buf)
+    }
+}
+impl<W: Write + ?Sized> Write for &mut W {
+    #[inline]
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        (**self).write(buf)
+    }
+
+    #[inline]
+    fn flush(&mut self) -> io::Result<()> {
+        (**self).flush()
+    }
+
+    #[inline]
+    fn write_all(&mut self, buf: &[u8]) -> io::Result<()> {
+        (**self).write_all(buf)
+    }
+
+    #[inline]
+    fn write_fmt(&mut self, fmt: fmt::Arguments<'_>) -> io::Result<()> {
+        (**self).write_fmt(fmt)
+    }
+}
+impl<S: Seek + ?Sized> Seek for &mut S {
+    #[inline]
+    fn seek(&mut self, pos: SeekFrom) -> io::Result<u64> {
+        (**self).seek(pos)
+    }
+}
+#[cfg(feature = "collections")]
+impl<B: BufRead + ?Sized> BufRead for &mut B {
+    #[inline]
+    fn fill_buf(&mut self) -> io::Result<&[u8]> {
+        (**self).fill_buf()
+    }
+
+    #[inline]
+    fn consume(&mut self, amt: usize) {
+        (**self).consume(amt)
+    }
+
+    #[cfg(feature="collections")]
+    #[inline]
+    fn read_until(&mut self, byte: u8, buf: &mut Vec<u8>) -> io::Result<usize> {
+        (**self).read_until(byte, buf)
+    }
+
+    #[cfg(feature="collections")]
+    #[inline]
+    fn read_line(&mut self, buf: &mut String) -> io::Result<usize> {
+        (**self).read_line(buf)
+    }
+}
+
+#[cfg(feature="alloc")]
+#[cfg(feature="collections")]
+impl<R: Read + ?Sized> Read for Box<R> {
+    #[inline]
+    fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
+        (**self).read(buf)
+    }
+
+    #[cfg(feature="collections")]
+    #[inline]
+    fn read_to_end(&mut self, buf: &mut Vec<u8>) -> io::Result<usize> {
+        (**self).read_to_end(buf)
+    }
+
+    #[cfg(feature="collections")]
+    #[inline]
+    fn read_to_string(&mut self, buf: &mut String) -> io::Result<usize> {
+        (**self).read_to_string(buf)
+    }
+
+    #[inline]
+    fn read_exact(&mut self, buf: &mut [u8]) -> io::Result<()> {
+        (**self).read_exact(buf)
+    }
+}
+#[cfg(feature="alloc")]
+#[cfg(feature="collections")]
+impl<W: Write + ?Sized> Write for Box<W> {
+    #[inline]
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        (**self).write(buf)
+    }
+
+    #[inline]
+    fn flush(&mut self) -> io::Result<()> {
+        (**self).flush()
+    }
+
+    #[inline]
+    fn write_all(&mut self, buf: &[u8]) -> io::Result<()> {
+        (**self).write_all(buf)
+    }
+
+    #[inline]
+    fn write_fmt(&mut self, fmt: fmt::Arguments<'_>) -> io::Result<()> {
+        (**self).write_fmt(fmt)
+    }
+}
+#[cfg(feature="collections")]
+impl<S: Seek + ?Sized> Seek for Box<S> {
+    #[inline]
+    fn seek(&mut self, pos: SeekFrom) -> io::Result<u64> {
+        (**self).seek(pos)
+    }
+}
+#[cfg(feature="collections")]
+impl<B: BufRead + ?Sized> BufRead for Box<B> {
+    #[inline]
+    fn fill_buf(&mut self) -> io::Result<&[u8]> {
+        (**self).fill_buf()
+    }
+
+    #[inline]
+    fn consume(&mut self, amt: usize) {
+        (**self).consume(amt)
+    }
+
+    #[inline]
+    fn read_until(&mut self, byte: u8, buf: &mut Vec<u8>) -> io::Result<usize> {
+        (**self).read_until(byte, buf)
+    }
+
+    #[inline]
+    fn read_line(&mut self, buf: &mut String) -> io::Result<usize> {
+        (**self).read_line(buf)
+    }
+}
+
+// Used by panicking::default_hook
+#[cfg(test)]
+/// This impl is only used by printing logic, so any error returned is always
+/// of kind `Other`, and should be ignored.
+#[cfg(feature="collections")]
+impl Write for Box<dyn (::realstd::io::Write) + Send> {
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        (**self).write(buf).map_err(|_| ErrorKind::Other.into())
+    }
+
+    fn flush(&mut self) -> io::Result<()> {
+        (**self).flush().map_err(|_| ErrorKind::Other.into())
+    }
+}
+
+// =============================================================================
+// In-memory buffer implementations
+
+/// Read is implemented for `&[u8]` by copying from the slice.
+///
+/// Note that reading updates the slice to point to the yet unread part.
+/// The slice will be empty when EOF is reached.
+impl Read for &[u8] {
+    #[inline]
+    fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
+        let amt = cmp::min(buf.len(), self.len());
+        let (a, b) = self.split_at(amt);
+
+        // First check if the amount of bytes we want to read is small:
+        // `copy_from_slice` will generally expand to a call to `memcpy`, and
+        // for a single byte the overhead is significant.
+        if amt == 1 {
+            buf[0] = a[0];
+        } else {
+            buf[..amt].copy_from_slice(a);
+        }
+
+        *self = b;
+        Ok(amt)
+    }
+
+    #[inline]
+    unsafe fn initializer(&self) -> Initializer {
+        Initializer::nop()
+    }
+
+    #[inline]
+    fn read_exact(&mut self, buf: &mut [u8]) -> io::Result<()> {
+        if buf.len() > self.len() {
+            return Err(Error::new(ErrorKind::UnexpectedEof, "failed to fill whole buffer"));
+        }
+        let (a, b) = self.split_at(buf.len());
+
+        // First check if the amount of bytes we want to read is small:
+        // `copy_from_slice` will generally expand to a call to `memcpy`, and
+        // for a single byte the overhead is significant.
+        if buf.len() == 1 {
+            buf[0] = a[0];
+        } else {
+            buf.copy_from_slice(a);
+        }
+
+        *self = b;
+        Ok(())
+    }
+
+    #[cfg(feature="collections")]
+    #[inline]
+    fn read_to_end(&mut self, buf: &mut Vec<u8>) -> io::Result<usize> {
+        buf.extend_from_slice(*self);
+        let len = self.len();
+        *self = &self[len..];
+        Ok(len)
+    }
+}
+
+#[cfg(feature="collections")]
+impl BufRead for &[u8] {
+    #[inline]
+    fn fill_buf(&mut self) -> io::Result<&[u8]> {
+        Ok(*self)
+    }
+
+    #[inline]
+    fn consume(&mut self, amt: usize) {
+        *self = &self[amt..];
+    }
+}
+
+/// Write is implemented for `&mut [u8]` by copying into the slice, overwriting
+/// its data.
+///
+/// Note that writing updates the slice to point to the yet unwritten part.
+/// The slice will be empty when it has been completely overwritten.
+impl Write for &mut [u8] {
+    #[inline]
+    fn write(&mut self, data: &[u8]) -> io::Result<usize> {
+        let amt = cmp::min(data.len(), self.len());
+        let (a, b) = mem::replace(self, &mut []).split_at_mut(amt);
+        a.copy_from_slice(&data[..amt]);
+        *self = b;
+        Ok(amt)
+    }
+
+    #[inline]
+    fn write_all(&mut self, data: &[u8]) -> io::Result<()> {
+        if self.write(data)? == data.len() {
+            Ok(())
+        } else {
+            Err(Error::new(ErrorKind::WriteZero, "failed to write whole buffer"))
+        }
+    }
+
+    #[inline]
+    fn flush(&mut self) -> io::Result<()> {
+        Ok(())
+    }
+}
+
+/// Write is implemented for `Vec<u8>` by appending to the vector.
+/// The vector will grow as needed.
+#[cfg(feature="collections")]
+impl Write for Vec<u8> {
+    #[inline]
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        self.extend_from_slice(buf);
+        Ok(buf.len())
+    }
+
+    #[inline]
+    fn write_all(&mut self, buf: &[u8]) -> io::Result<()> {
+        self.extend_from_slice(buf);
+        Ok(())
+    }
+
+    #[inline]
+    fn flush(&mut self) -> io::Result<()> {
+        Ok(())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::io::prelude::*;
+
+    #[bench]
+    fn bench_read_slice(b: &mut test::Bencher) {
+        let buf = [5; 1024];
+        let mut dst = [0; 128];
+
+        b.iter(|| {
+            let mut rd = &buf[..];
+            for _ in 0..8 {
+                let _ = rd.read(&mut dst);
+                test::black_box(&dst);
+            }
+        })
+    }
+
+    #[bench]
+    fn bench_write_slice(b: &mut test::Bencher) {
+        let mut buf = [0; 1024];
+        let src = [5; 128];
+
+        b.iter(|| {
+            let mut wr = &mut buf[..];
+            for _ in 0..8 {
+                let _ = wr.write_all(&src);
+                test::black_box(&wr);
+            }
+        })
+    }
+
+    #[bench]
+    fn bench_read_vec(b: &mut test::Bencher) {
+        let buf = vec![5; 1024];
+        let mut dst = [0; 128];
+
+        b.iter(|| {
+            let mut rd = &buf[..];
+            for _ in 0..8 {
+                let _ = rd.read(&mut dst);
+                test::black_box(&dst);
+            }
+        })
+    }
+
+    #[bench]
+    fn bench_write_vec(b: &mut test::Bencher) {
+        let mut buf = Vec::with_capacity(1024);
+        let src = [5; 128];
+
+        b.iter(|| {
+            let mut wr = &mut buf[..];
+            for _ in 0..8 {
+                let _ = wr.write_all(&src);
+                test::black_box(&wr);
+            }
+        })
+    }
+}
--- a/libcoreio/src/io/mod.rs
+++ b/libcoreio/src/io/mod.rs
--- a/libcoreio/src/io/prelude.rs
+++ b/libcoreio/src/io/prelude.rs
@ -0,0 +1,13 @@
+//! The I/O Prelude
+//!
+//! The purpose of this module is to alleviate imports of many common I/O traits
+//! by adding a glob import to the top of I/O heavy modules:
+//!
+//! ```
+//! # #![allow(unused_imports)]
+//! use std::io::prelude::*;
+//! ```
+
+
+pub use super::{Read, Seek, Write};
+#[cfg(feature = "collections")] pub use super::BufRead;
--- a/libcoreio/src/io/util.rs
+++ b/libcoreio/src/io/util.rs
@ -0,0 +1,269 @@
+#![allow(missing_copy_implementations)]
+
+use core::fmt;
+use core::mem;
+use crate::io::{self, ErrorKind, Initializer, Read, Write};
+#[cfg(feature = "collections")] use crate::io::BufRead;
+
+/// Copies the entire contents of a reader into a writer.
+///
+/// This function will continuously read data from `reader` and then
+/// write it into `writer` in a streaming fashion until `reader`
+/// returns EOF.
+///
+/// On success, the total number of bytes that were copied from
+/// `reader` to `writer` is returned.
+///
+/// If you’re wanting to copy the contents of one file to another and you’re
+/// working with filesystem paths, see the [`fs::copy`] function.
+///
+/// [`fs::copy`]: ../fs/fn.copy.html
+///
+/// # Errors
+///
+/// This function will return an error immediately if any call to `read` or
+/// `write` returns an error. All instances of `ErrorKind::Interrupted` are
+/// handled by this function and the underlying operation is retried.
+///
+/// # Examples
+///
+/// ```
+/// use std::io;
+///
+/// fn main() -> io::Result<()> {
+///     let mut reader: &[u8] = b"hello";
+///     let mut writer: Vec<u8> = vec![];
+///
+///     io::copy(&mut reader, &mut writer)?;
+///
+///     assert_eq!(&b"hello"[..], &writer[..]);
+///     Ok(())
+/// }
+/// ```
+pub fn copy<R: ?Sized, W: ?Sized>(reader: &mut R, writer: &mut W) -> io::Result<u64>
+where
+    R: Read,
+    W: Write,
+{
+    let mut buf = unsafe {
+        #[allow(deprecated)]
+        let mut buf: [u8; super::DEFAULT_BUF_SIZE] = mem::uninitialized();
+        reader.initializer().initialize(&mut buf);
+        buf
+    };
+
+    let mut written = 0;
+    loop {
+        let len = match reader.read(&mut buf) {
+            Ok(0) => return Ok(written),
+            Ok(len) => len,
+            Err(ref e) if e.kind() == ErrorKind::Interrupted => continue,
+            Err(e) => return Err(e),
+        };
+        writer.write_all(&buf[..len])?;
+        written += len as u64;
+    }
+}
+
+/// A reader which is always at EOF.
+///
+/// This struct is generally created by calling [`empty`]. Please see
+/// the documentation of [`empty()`][`empty`] for more details.
+///
+/// [`empty`]: fn.empty.html
+pub struct Empty {
+    _priv: (),
+}
+
+/// Constructs a new handle to an empty reader.
+///
+/// All reads from the returned reader will return [`Ok`]`(0)`.
+///
+/// [`Ok`]: ../result/enum.Result.html#variant.Ok
+///
+/// # Examples
+///
+/// A slightly sad example of not reading anything into a buffer:
+///
+/// ```
+/// use std::io::{self, Read};
+///
+/// let mut buffer = String::new();
+/// io::empty().read_to_string(&mut buffer).unwrap();
+/// assert!(buffer.is_empty());
+/// ```
+pub fn empty() -> Empty {
+    Empty { _priv: () }
+}
+
+impl Read for Empty {
+    #[inline]
+    fn read(&mut self, _buf: &mut [u8]) -> io::Result<usize> {
+        Ok(0)
+    }
+
+    #[inline]
+    unsafe fn initializer(&self) -> Initializer {
+        Initializer::nop()
+    }
+}
+
+#[cfg(feature="collections")]
+impl BufRead for Empty {
+    #[inline]
+    fn fill_buf(&mut self) -> io::Result<&[u8]> {
+        Ok(&[])
+    }
+    #[inline]
+    fn consume(&mut self, _n: usize) {}
+}
+
+impl fmt::Debug for Empty {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        f.pad("Empty { .. }")
+    }
+}
+
+/// A reader which yields one byte over and over and over and over and over and...
+///
+/// This struct is generally created by calling [`repeat`][repeat]. Please
+/// see the documentation of `repeat()` for more details.
+///
+/// [repeat]: fn.repeat.html
+pub struct Repeat {
+    byte: u8,
+}
+
+/// Creates an instance of a reader that infinitely repeats one byte.
+///
+/// All reads from this reader will succeed by filling the specified buffer with
+/// the given byte.
+///
+/// # Examples
+///
+/// ```
+/// use std::io::{self, Read};
+///
+/// let mut buffer = [0; 3];
+/// io::repeat(0b101).read_exact(&mut buffer).unwrap();
+/// assert_eq!(buffer, [0b101, 0b101, 0b101]);
+/// ```
+pub fn repeat(byte: u8) -> Repeat {
+    Repeat { byte }
+}
+
+impl Read for Repeat {
+    #[inline]
+    fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
+        for slot in &mut *buf {
+            *slot = self.byte;
+        }
+        Ok(buf.len())
+    }
+
+    #[inline]
+    unsafe fn initializer(&self) -> Initializer {
+        Initializer::nop()
+    }
+}
+
+impl fmt::Debug for Repeat {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        f.pad("Repeat { .. }")
+    }
+}
+
+/// A writer which will move data into the void.
+///
+/// This struct is generally created by calling [`sink`][sink]. Please
+/// see the documentation of `sink()` for more details.
+///
+/// [sink]: fn.sink.html
+pub struct Sink {
+    _priv: (),
+}
+
+/// Creates an instance of a writer which will successfully consume all data.
+///
+/// All calls to `write` on the returned instance will return `Ok(buf.len())`
+/// and the contents of the buffer will not be inspected.
+///
+/// # Examples
+///
+/// ```rust
+/// use std::io::{self, Write};
+///
+/// let buffer = vec![1, 2, 3, 5, 8];
+/// let num_bytes = io::sink().write(&buffer).unwrap();
+/// assert_eq!(num_bytes, 5);
+/// ```
+pub fn sink() -> Sink {
+    Sink { _priv: () }
+}
+
+impl Write for Sink {
+    #[inline]
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        Ok(buf.len())
+    }
+
+    #[inline]
+    fn flush(&mut self) -> io::Result<()> {
+        Ok(())
+    }
+}
+
+impl fmt::Debug for Sink {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        f.pad("Sink { .. }")
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::io::prelude::*;
+    use crate::io::{copy, empty, repeat, sink};
+
+    #[test]
+    fn copy_copies() {
+        let mut r = repeat(0).take(4);
+        let mut w = sink();
+        assert_eq!(copy(&mut r, &mut w).unwrap(), 4);
+
+        let mut r = repeat(0).take(1 << 17);
+        assert_eq!(copy(&mut r as &mut dyn Read, &mut w as &mut dyn Write).unwrap(), 1 << 17);
+    }
+
+    #[test]
+    fn sink_sinks() {
+        let mut s = sink();
+        assert_eq!(s.write(&[]).unwrap(), 0);
+        assert_eq!(s.write(&[0]).unwrap(), 1);
+        assert_eq!(s.write(&[0; 1024]).unwrap(), 1024);
+        assert_eq!(s.by_ref().write(&[0; 1024]).unwrap(), 1024);
+    }
+
+    #[test]
+    fn empty_reads() {
+        let mut e = empty();
+        assert_eq!(e.read(&mut []).unwrap(), 0);
+        assert_eq!(e.read(&mut [0]).unwrap(), 0);
+        assert_eq!(e.read(&mut [0; 1024]).unwrap(), 0);
+        assert_eq!(e.by_ref().read(&mut [0; 1024]).unwrap(), 0);
+    }
+
+    #[test]
+    fn repeat_repeats() {
+        let mut r = repeat(4);
+        let mut b = [0; 1024];
+        assert_eq!(r.read(&mut b).unwrap(), 1024);
+        assert!(b.iter().all(|b| *b == 4));
+    }
+
+    #[test]
+    fn take_some_bytes() {
+        assert_eq!(repeat(4).take(100).bytes().count(), 100);
+        assert_eq!(repeat(4).take(100).bytes().next().unwrap().unwrap(), 4);
+        assert_eq!(repeat(1).take(10).chain(repeat(2).take(10)).bytes().count(), 20);
+    }
+}
--- a/libcoreio/src/lib.rs
+++ b/libcoreio/src/lib.rs
@ -0,0 +1,51 @@
+//! <p id="core_io-show-docblock"></p>
+//! This is just a listing of the functionality available in this crate. See
+//! the [std documentation](https://doc.rust-lang.org/nightly/std/io/index.html)
+//! for a full description of the functionality.
+#![allow(stable_features,unused_features)]
+#![feature(question_mark,const_fn,copy_from_slice,try_from,str_internals,align_offset,slice_internals)]
+#![cfg_attr(any(feature="alloc",feature="collections"),feature(alloc))]
+#![cfg_attr(pattern_guards,feature(bind_by_move_pattern_guards,nll))]
+#![cfg_attr(non_exhaustive,feature(non_exhaustive))]
+#![cfg_attr(unicode,feature(str_char))]
+#![cfg_attr(unicode,feature(unicode))]
+#![no_std]
+
+#[cfg_attr(feature="collections",macro_use)]
+#[cfg_attr(feature="collections",allow(unused_imports))]
+#[cfg(feature="collections")] extern crate alloc as collections;
+#[cfg(feature="alloc")] extern crate alloc;
+#[cfg(rustc_unicode)]
+extern crate rustc_unicode;
+#[cfg(std_unicode)]
+extern crate std_unicode;
+
+#[cfg(not(feature="collections"))]
+pub type ErrorString = &'static str;
+
+// Provide Box::new wrapper
+#[cfg(not(feature="alloc"))]
+struct FakeBox<T>(core::marker::PhantomData<T>);
+#[cfg(not(feature="alloc"))]
+impl<T> FakeBox<T> {
+	fn new(val: T) -> T {
+		val
+	}
+}
+
+// Needed for older compilers, to ignore vec!/format! macros in tests
+#[cfg(not(feature="collections"))]
+#[allow(unused)]
+macro_rules! vec (
+	( $ elem : expr ; $ n : expr ) => { () };
+	( $ ( $ x : expr ) , * ) => { () };
+	( $ ( $ x : expr , ) * ) => { () };
+);
+#[cfg(not(feature="collections"))]
+#[allow(unused)]
+macro_rules! format {
+	( $ ( $ arg : tt ) * ) => { () };
+}
+
+mod io;
+pub use io::*;
--- a/libcortex_a9/Cargo.toml
+++ b/libcortex_a9/Cargo.toml
@ -1,23 +1,17 @@
 [package]
 name = "libcortex_a9"
 version = "0.0.0"
-authors = ["Astro <astro@spaceboyz.net>"]
+authors = ["M-Labs"]
 edition = "2018"

 [features]
 target_zc706 = []
 target_cora_z7_10 = []
+target_redpitaya = []
+power_saving = []
 default = ["target_zc706"]

 [dependencies]
-r0 = "0.2"
-vcell = "0.1"
-volatile-register = "0.2"
 bit_field = "0.10"
+volatile-register = "0.2"
 libregister = { path = "../libregister" }
-
-[dependencies.smoltcp]
-git = "https://github.com/m-labs/smoltcp.git"
-rev = "8eb01aca364aefe5f823d68d552d62c76c9be4a3"
-features = ["ethernet", "proto-ipv4", "socket-tcp"]
-default-features = false
--- a/libcortex_a9/src/asm.rs
+++ b/libcortex_a9/src/asm.rs
@ -1,35 +1,76 @@
 /// The classic no-op
 #[inline]
 pub fn nop() {
-    unsafe { asm!("nop" :::: "volatile") }
+    unsafe { llvm_asm!("nop" :::: "volatile") }
 }

 /// Wait For Event
 #[inline]
 pub fn wfe() {
-    unsafe { asm!("wfe" :::: "volatile") }
+    unsafe { llvm_asm!("wfe" :::: "volatile") }
 }

 /// Send Event
 #[inline]
 pub fn sev() {
-    unsafe { asm!("sev" :::: "volatile") }
+    unsafe { llvm_asm!("sev" :::: "volatile") }
 }

 /// Data Memory Barrier
 #[inline]
 pub fn dmb() {
-    unsafe { asm!("dmb" :::: "volatile") }
+    unsafe { llvm_asm!("dmb" :::: "volatile") }
 }

 /// Data Synchronization Barrier
 #[inline]
 pub fn dsb() {
-    unsafe { asm!("dsb" :::: "volatile") }
+    unsafe { llvm_asm!("dsb" :::: "volatile") }
 }

 /// Instruction Synchronization Barrier
 #[inline]
 pub fn isb() {
-    unsafe { asm!("isb" :::: "volatile") }
+    unsafe { llvm_asm!("isb" :::: "volatile") }
+}
+
+/// Enable IRQ
+#[inline]
+pub unsafe fn enable_irq() {
+    llvm_asm!("cpsie i":::: "volatile");
+}
+
+/// Disable IRQ, return if IRQ was originally enabled.
+#[inline]
+pub unsafe fn enter_critical() -> bool {
+    let mut cpsr: u32;
+    llvm_asm!(
+        "mrs $0, cpsr
+         cpsid i"
+         : "=r"(cpsr) ::: "volatile");
+    (cpsr & (1 << 7)) == 0
+}
+
+#[inline]
+pub unsafe fn exit_critical(enable: bool) {
+    // https://stackoverflow.com/questions/40019929/temporarily-disable-interrupts-on-arm
+    let mask: u32 = if enable {
+        1 << 7
+    } else {
+        0
+    };
+    llvm_asm!(
+        "mrs r1, cpsr
+         bic r1, r1, $0
+         msr cpsr_c, r1"
+         :: "r"(mask) : "r1");
+}
+
+/// Exiting IRQ
+#[inline]
+pub unsafe fn exit_irq() {
+    llvm_asm!("
+        mrs r0, SPSR
+        msr CPSR, r0
+        " ::: "r0");
 }
--- a/libcortex_a9/src/cache.rs
+++ b/libcortex_a9/src/cache.rs
@ -1,8 +1,11 @@
+use super::asm::{dmb, dsb};
+use super::l2c::*;
+
 /// Invalidate TLBs
 #[inline(always)]
 pub fn tlbiall() {
    unsafe {
-        asm!("mcr p15, 0, $0, c8, c7, 0" :: "r" (0) :: "volatile");
+        llvm_asm!("mcr p15, 0, $0, c8, c7, 0" :: "r" (0) :: "volatile");
    }
 }

@ -10,7 +13,7 @@ pub fn tlbiall() {
 #[inline(always)]
 pub fn iciallu() {
    unsafe {
-        asm!("mcr p15, 0, $0, c7, c5, 0" :: "r" (0) :: "volatile");
+        llvm_asm!("mcr p15, 0, $0, c7, c5, 0" :: "r" (0) :: "volatile");
    }
 }

@ -18,10 +21,19 @@ pub fn iciallu() {
 #[inline(always)]
 pub fn bpiall() {
    unsafe {
-        asm!("mcr p15, 0, $0, c7, c5, 6" :: "r" (0) :: "volatile");
+        llvm_asm!("mcr p15, 0, $0, c7, c5, 6" :: "r" (0) :: "volatile");
    }
 }

+/// Data cache clean by set/way
+#[inline(always)]
+pub fn dccsw(setway: u32) {
+    unsafe {
+        llvm_asm!("mcr p15, 0, $0, c7, c10, 2" :: "r" (setway) :: "volatile");
+    }
+}
+
+/// Data cache invalidate by set/way
 #[inline(always)]
 pub fn dcisw(setway: u32) {
    unsafe {
@ -29,13 +41,21 @@ pub fn dcisw(setway: u32) {
        // 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, $0, c7, c6, 2" :: "r" (setway) :: "volatile");
+        llvm_asm!("mcr p15, 0, $0, c7, c6, 2" :: "r" (setway) :: "volatile");
+    }
+}
+
+/// Data cache clean by set/way
+#[inline(always)]
+pub fn dccisw(setway: u32) {
+    unsafe {
+        llvm_asm!("mcr p15, 0, $0, c7, c14, 2" :: "r" (setway) :: "volatile");
    }
 }

 /// A made-up "instruction": invalidate all of the L1 D-Cache
 #[inline(always)]
-pub fn dciall() {
+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;
@ -49,7 +69,7 @@ pub fn dciall() {

    // select L1 data cache
    unsafe {
-        asm!("mcr p15, 2, $0, c0, c0, 0" :: "r" (0) :: "volatile");
+        llvm_asm!("mcr p15, 2, $0, c0, c0, 0" :: "r" (0) :: "volatile");
    }

    // Invalidate entire D-Cache by iterating every set and every way
@ -60,75 +80,157 @@ pub fn dciall() {
    }
 }

-/// Data cache clear and invalidate by memory virtual address. This
+/// 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 {
+        llvm_asm!("mcr p15, 2, $0, c0, c0, 0" :: "r" (0) :: "volatile");
+    }
+
+    // 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 dccimva(addr: usize) {
+pub fn dccimvac(addr: usize) {
    unsafe {
-        asm!("mcr p15, 0, $0, c7, c14, 1" :: "r" (addr) :: "volatile");
+        llvm_asm!("mcr p15, 0, $0, c7, c14, 1" :: "r" (addr) :: "volatile");
    }
 }

-/// clear cache line by virtual address to point of coherency (DCCMVAC)
-#[inline]
-pub fn dccmvac(addr: u32) {
+/// 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, $0, c7, c10, 1" :: "r" (addr) :: "volatile");
+        llvm_asm!("mcr p15, 0, $0, c7, c10, 1" :: "r" (addr) :: "volatile");
    }
 }

-/// The DCCIVMA (data cache clear and invalidate) applied to the
-/// region of memory occupied by the argument. This does not modify
-/// the argument, but due to the invalidate part (only ever needed if
-/// external write access is to be granted, e.g. by DMA) it only makes
-/// sense if the caller has exclusive access to it as otherwise other
-/// accesses might just bring it back into the data cache.
-pub fn dcci<T>(object: &mut T) {
-    let cache_line = 0x20;
-    let first_addr =
-        (object as *mut _ as *const _ as usize) & !(cache_line - 1);
-    let beyond_addr = (
-        (object as *mut _ as *const _ as usize)
-            + core::mem::size_of_val(object)
-            + (cache_line - 1)
-    ) & !(cache_line - 1);
-    for addr in (first_addr..beyond_addr).step_by(cache_line) {
-        dccimva(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();
 }

-pub fn dcci_slice_content<T>(slice: &mut [T]) {
+/// 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;
    }
-    let cache_line = 0x20;
-    let first_addr =
-        (&slice[0] as *const _ as usize) & !(cache_line - 1);
-    let beyond_addr = (
-        (&slice[slice.len() - 1] as *const _ as usize)
-            + (cache_line - 1)
-    ) & !(cache_line - 1);
-    for addr in (first_addr..beyond_addr).step_by(cache_line) {
-        dccimva(addr);
+    dmb();
+    for addr in slice_cache_line_addrs(slice) {
+        dccmvac(addr);
    }
-}
-
-pub fn dcci_slice_content_unmut<T>(slice: &[T]) {
-    if slice.len() == 0 {
-        return;
-    }
-    let cache_line = 0x20;
-    let first_addr =
-        (&slice[0] as *const _ as usize) & !(cache_line - 1);
-    let beyond_addr = (
-        (&slice[slice.len() - 1] as *const _ as usize)
-            + (cache_line - 1)
-    ) & !(cache_line - 1);
-    for addr in (first_addr..beyond_addr).step_by(cache_line) {
-        dccimva(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
@ -136,79 +238,42 @@ pub fn dcci_slice_content_unmut<T>(slice: &[T]) {
 /// unsafe, as this discards a write-back cache line, potentially
 /// affecting more data than intended.
 #[inline(always)]
-pub unsafe fn dcimva(addr: usize) {
-    asm!("mcr p15, 0, $0, c7, c6, 1" :: "r" (addr) :: "volatile");
+pub unsafe fn dcimvac(addr: usize) {
+    llvm_asm!("mcr p15, 0, $0, c7, c6, 1" :: "r" (addr) :: "volatile");
 }

-/// Data cache invalidate for an object. Panics if not properly
-/// aligned and properly sized to be contained in an exact number of
-/// cache lines.
-pub fn dci<T>(object: &mut T) {
-    let cache_line = 0x20;
-    let first_addr = object as *mut _ as *const _ as usize;
-    let beyond_addr = (object as *mut _ as *const _ as usize) +
-        core::mem::size_of_val(object);
-    assert_eq!((first_addr & (cache_line - 1)), 0x00);
-    assert_eq!((beyond_addr & (cache_line - 1)), 0x00);
-    for addr in (first_addr..beyond_addr).step_by(cache_line) {
-        unsafe {
-            dcimva(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();
 }

-/// Data cache invalidate for the contents of a slice. Panics if not
-/// properly aligned and properly sized to be contained in an exact
-/// number of cache lines.
-pub fn dci_slice_content<T>(slice: &mut [T]) {
-    if slice.len() == 0 {
-        return;
-    }
-    let cache_line = 0x20;
+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::<T>();
-    assert_eq!((first_addr & (cache_line - 1)), 0x00);
-    assert_eq!((beyond_addr & (cache_line - 1)), 0x00);
-    for addr in (first_addr..beyond_addr).step_by(cache_line) {
-        unsafe {
-            dcimva(addr);
-        }
-    }
-}
+    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");

-pub unsafe fn dci_more_than_slice_content<T>(slice: &mut [T]) {
-    if slice.len() == 0 {
-        return;
+    dmb();
+    for addr in (first_addr..beyond_addr).step_by(CACHE_LINE) {
+        l2_cache_invalidate(addr);
    }
-    let cache_line = 0x20;
-    let first_addr =
-        (&slice[0] as *const _ as usize) & !(cache_line - 1);
-    let beyond_addr = (
-        (&slice[slice.len() - 1] as *const _ as usize)
-            + (cache_line - 1)
-    ) & !(cache_line - 1);
-    assert_eq!((first_addr & (cache_line - 1)), 0x00);
-    assert_eq!((beyond_addr & (cache_line - 1)), 0x00);
-    for addr in (first_addr..beyond_addr).step_by(cache_line) {
-        dcimva(addr);
-    }
-}
-
-pub unsafe fn dci_more_than_slice_content_nonmut<T>(slice: &[T]) {
-    if slice.len() == 0 {
-        return;
-    }
-    let cache_line = 0x20;
-    let first_addr =
-        (&slice[0] as *const _ as usize) & !(cache_line - 1);
-    let beyond_addr = (
-        (&slice[slice.len() - 1] as *const _ as usize)
-            + (cache_line - 1)
-    ) & !(cache_line - 1);
-    assert_eq!((first_addr & (cache_line - 1)), 0x00);
-    assert_eq!((beyond_addr & (cache_line - 1)), 0x00);
-    for addr in (first_addr..beyond_addr).step_by(cache_line) {
-        dcimva(addr);
+    l2_cache_sync();
+    for addr in (first_addr..beyond_addr).step_by(CACHE_LINE) {
+        dcimvac(addr);
    }
+    dsb();
 }
--- a/libcortex_a9/src/fpu.rs
+++ b/libcortex_a9/src/fpu.rs
@ -0,0 +1,14 @@
+/// Enable FPU in the current core.
+pub fn enable_fpu() {
+    unsafe {
+        llvm_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
+            ":::"r1");
+    }
+}
--- a/libcortex_a9/src/l2c.rs
+++ b/libcortex_a9/src/l2c.rs
@ -0,0 +1,328 @@
+use libregister::{register, register_at, register_bit, register_bits, RegisterRW, RegisterR, RegisterW};
+use super::asm::dmb;
+use volatile_register::RW;
+
+pub fn enable_l2_cache() {
+    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)
+    );
+    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);
--- a/libcortex_a9/src/lib.rs
+++ b/libcortex_a9/src/lib.rs
@ -1,11 +1,37 @@
 #![no_std]
-#![feature(asm, global_asm)]
+#![feature(llvm_asm, global_asm)]
 #![feature(never_type)]
+#![feature(const_fn)]
+
+extern crate alloc;

 pub mod asm;
 pub mod regs;
 pub mod cache;
 pub mod mmu;
 pub mod mutex;
+pub mod sync_channel;
+pub mod semaphore;
+pub mod l2c;
+mod uncached;
+mod fpu;
+pub use uncached::UncachedSlice;
+pub use fpu::enable_fpu;

 global_asm!(include_str!("exceptions.s"));
+
+#[inline]
+pub fn spin_lock_yield() {
+    #[cfg(feature = "power_saving")]
+    asm::wfe();
+}
+
+#[inline]
+pub fn notify_spin_lock() {
+    #[cfg(feature = "power_saving")]
+    {
+        asm::dsb();
+        asm::sev();
+    }
+}
+
--- a/libcortex_a9/src/mmu.rs
+++ b/libcortex_a9/src/mmu.rs
@ -1,5 +1,5 @@
 use bit_field::BitField;
-use super::{regs::*, asm};
+use super::{regs::*, asm::*, cache::*};
 use libregister::RegisterW;

 #[derive(Copy, Clone)]
@ -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.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.set_section(section);
        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)]
-#[link_section = ".bss.l1_table"]
-#[no_mangle]
-pub static mut l1_table: L1Table = L1Table {
+static mut L1_TABLE: L1Table = L1Table {
    table: [L1Entry(0); L1_TABLE_SIZE]
 };

-/// The `#[repr(align(16384))]` is unfortunately ineffective. Hence we
-/// require explicit linking to a region defined in the linker script.
-#[repr(align(16384))]
+#[repr(C, 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
        }
    }

@ -128,31 +153,18 @@ impl L1Table {
            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 {
@ -201,7 +213,7 @@ impl L1Table {
                access: AccessPermissions::FullAccess,
                tex: 0,
                domain: 0,
-                exec: true,
+                exec: false,
                cacheable: false,
                bufferable: true,
            });
@ -326,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,
@ -343,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
    }
 }

@ -371,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
@ -1,20 +1,10 @@
 use core::ops::{Deref, DerefMut};
 use core::sync::atomic::{AtomicU32, Ordering};
 use core::cell::UnsafeCell;
-use super::asm::*;
-
-/// [Power-saving features](http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dht0008a/ch01s03s02.html)
-#[inline]
-fn wait_for_update() {
-    wfe();
-}
-
-/// [Power-saving features](http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dht0008a/ch01s03s02.html)
-#[inline]
-fn signal_update() {
-    dsb();
-    sev();
-}
+use super::{
+    spin_lock_yield, notify_spin_lock,
+    asm::{enter_critical, exit_critical}
+};

 const LOCKED: u32 = 1;
 const UNLOCKED: u32 = 0;
@ -41,18 +31,31 @@ impl<T> Mutex<T> {

    /// Lock the Mutex, blocks when already locked
    pub fn lock(&self) -> MutexGuard<T> {
-        while self.locked.compare_and_swap(UNLOCKED, LOCKED, Ordering::Acquire) != UNLOCKED {
-            wait_for_update();
+        let mut irq = unsafe { enter_critical() };
+        while self.locked.compare_and_swap(UNLOCKED, LOCKED, Ordering::AcqRel) != UNLOCKED {
+            unsafe {
+                exit_critical(irq);
+                spin_lock_yield();
+                irq = enter_critical();
+            }
+        }
+        MutexGuard { mutex: self, irq }
+    }
+
+    pub fn try_lock(&self) -> Option<MutexGuard<T>> {
+        let irq = unsafe { enter_critical() };
+        if self.locked.compare_and_swap(UNLOCKED, LOCKED, Ordering::AcqRel) != UNLOCKED {
+            unsafe { exit_critical(irq) };
+            None
+        } else {
+            Some(MutexGuard { mutex: self, irq })
        }
-        dmb();
-        MutexGuard { mutex: self }
    }

    fn unlock(&self) {
-        dmb();
        self.locked.store(UNLOCKED, Ordering::Release);

-        signal_update();
+        notify_spin_lock();
    }
 }

@ -60,6 +63,7 @@ impl<T> Mutex<T> {
 /// `Deref`/`DerefMutx`
 pub struct MutexGuard<'a, T> {
    mutex: &'a Mutex<T>,
+    irq: bool,
 }

 impl<'a, T> Deref for MutexGuard<'a, T> {
@ -79,5 +83,6 @@ impl<'a, T> DerefMut for MutexGuard<'a, T> {
 impl<'a, T> Drop for MutexGuard<'a, T> {
    fn drop(&mut self) {
        self.mutex.unlock();
+        unsafe { exit_critical(self.irq) };
    }
 }
--- a/libcortex_a9/src/regs.rs
+++ b/libcortex_a9/src/regs.rs
@ -8,10 +8,10 @@ macro_rules! def_reg_r {
        impl RegisterR for $name {
            type R = $type;

-            #[inline(always)]
+            #[inline]
            fn read(&self) -> Self::R {
                let mut value: u32;
-                unsafe { asm!($asm_instr : "=r" (value) ::: "volatile") }
+                unsafe { llvm_asm!($asm_instr : "=r" (value) ::: "volatile") }
                value.into()
            }
        }
@ -23,12 +23,13 @@ macro_rules! def_reg_w {
        impl RegisterW for $name {
            type W = $type;

-            #[inline(always)]
+            #[inline]
            fn write(&mut self, value: Self::W) {
                let value: u32 = value.into();
-                unsafe { asm!($asm_instr :: "r" (value) :: "volatile") }
+                unsafe { llvm_asm!($asm_instr :: "r" (value) :: "volatile") }
            }

+            #[inline]
            fn zeroed() -> Self::W {
                0u32.into()
            }
@ -43,6 +44,7 @@ macro_rules! wrap_reg {
                pub inner: u32,
            }
            impl From<u32> for Read {
+                #[inline]
                fn from(value: u32) -> Self {
                    Read { inner: value }
                }
@ -52,11 +54,13 @@ macro_rules! wrap_reg {
                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
                }
@ -75,8 +79,31 @@ pub struct LR;
 def_reg_r!(LR, u32, "mov $0, lr");
 def_reg_w!(LR, u32, "mov lr, $0");

+pub struct VBAR;
+def_reg_r!(VBAR, u32, "mrc p15, 0, $0, c12, c0, 0");
+def_reg_w!(VBAR, u32, "mcr p15, 0, $0, c12, c0, 0");
+
+pub struct MVBAR;
+def_reg_r!(MVBAR, u32, "mrc p15, 0, $0, c12, c0, 1");
+def_reg_w!(MVBAR, u32, "mcr p15, 0, $0, c12, c0, 1");
+
+pub struct HVBAR;
+def_reg_r!(HVBAR, u32, "mrc p15, 4, $0, c12, c0, 0");
+def_reg_w!(HVBAR, u32, "mcr p15, 4, $0, c12, c0, 0");
+
+/// Multiprocess Affinity Register
 pub struct MPIDR;
-def_reg_r!(MPIDR, u32, "mrc p15, 0, $0, c0, c0, 5");
+def_reg_r!(MPIDR, mpidr::Read, "mrc p15, 0, $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, $0, c6, c0, 0");
@ -129,10 +156,13 @@ 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 };
@ -145,6 +175,10 @@ impl ACTLR {
    pub fn enable_smp(&mut self) {
        self.modify(|_, w| w.smp(true).fw(true));
    }
+
+    pub fn enable_prefetch(&mut self) {
+        self.modify(|_, w| w.l1_prefetch_enable(true).l2_prefetch_enable(true))
+    }
 }

 /// Domain Access Control Register
--- 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_and_swap(value, value - 1, Ordering::SeqCst) == value {
+                    return Some(());
+                }
+            } else {
+                return None;
+            }
+        }
+    }
+
+    pub fn wait(&self) {
+        while self.try_wait().is_none() {
+            spin_lock_yield();
+        }
+    }
+
+    pub async fn async_wait(&self) {
+        struct Fut<'a>(&'a Semaphore);
+
+        impl Future for Fut<'_> {
+            type Output = ();
+            fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
+                match self.0.try_wait() {
+                    Some(_) => Poll::Ready(()),
+                    None => {
+                        cx.waker().wake_by_ref();
+                        Poll::Pending
+                    }
+                }
+            }
+        }
+
+        Fut(&self).await
+    }
+
+    pub fn signal(&self) {
+        loop {
+            let value = self.value.load(Ordering::Relaxed);
+            if value < self.max {
+                if self.value.compare_and_swap(value, value + 1, Ordering::SeqCst) == value {
+                    notify_spin_lock();
+                    return;
+                }
+            } else {
+                return;
+            }
+        }
+    }
+}
+
--- a/libcortex_a9/src/sync_channel.rs
+++ b/libcortex_a9/src/sync_channel.rs
@ -0,0 +1,187 @@
+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.
+/// Note that this requires `#![feature(const_in_array_repeat_expressions)]`
+macro_rules! sync_channel {
+    ($t: ty, $cap: expr) => {
+        {
+            use core::sync::atomic::{AtomicUsize, AtomicPtr};
+            use $crate::sync_channel::{Sender, Receiver};
+            static LIST: [AtomicPtr<$t>; $cap + 1] = [AtomicPtr::new(core::ptr::null_mut()); $cap + 1];
+            static WRITE: AtomicUsize = AtomicUsize::new(0);
+            static READ: AtomicUsize = AtomicUsize::new(0);
+            (Sender::new(&LIST, &WRITE, &READ), Receiver::new(&LIST, &WRITE, &READ))
+        }
+    };
+}
--- 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, LayoutErr};
+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, LayoutErr> {
+        // 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
@ -1,7 +1,7 @@
 [package]
 name = "libregister"
 version = "0.0.0"
-authors = ["Astro <astro@spaceboyz.net>"]
+authors = ["M-Labs"]
 edition = "2018"

 [dependencies]
--- a/libregister/src/lib.rs
+++ b/libregister/src/lib.rs
@ -55,6 +55,7 @@ macro_rules! register_r {
        impl libregister::RegisterR for $struct_name {
            type R = $mod_name::Read;

+            #[inline]
            fn read(&self) -> Self::R {
                let inner = self.inner.read();
                $mod_name::Read { inner }
@ -69,10 +70,12 @@ macro_rules! register_w {
        impl libregister::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);
@ -86,6 +89,7 @@ macro_rules! register_w {
 macro_rules! register_rw {
    ($mod_name: ident, $struct_name: ident) => (
        impl libregister::RegisterRW for $struct_name {
+            #[inline]
            fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
                unsafe {
                    self.inner.modify(|inner| {
@ -96,6 +100,19 @@ macro_rules! register_rw {
            }
        }
    );
+    ($mod_name: ident, $struct_name: ident, $mask: expr) => (
+        impl libregister::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)]
@ -105,6 +122,7 @@ macro_rules! register_vcell {
        impl libregister::RegisterR for $struct_name {
            type R = $mod_name::Read;

+            #[inline]
            fn read(&self) -> Self::R {
                let inner = self.inner.get();
                $mod_name::Read { inner }
@ -113,15 +131,18 @@ macro_rules! register_vcell {
        impl libregister::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 libregister::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 };
@ -160,6 +181,14 @@ macro_rules! register {
        libregister::register_common!($mod_name, $struct_name, VolatileCell<$inner>, $inner);
        libregister::register_vcell!($mod_name, $struct_name);
    );
+
+    // Define read-write register with mask on write (for WTC mixed access.)
+    ($mod_name: ident, $struct_name: ident, RW, $inner: ty, $mask: expr) => (
+        libregister::register_common!($mod_name, $struct_name, volatile_register::RW<$inner>, $inner);
+        libregister::register_r!($mod_name, $struct_name);
+        libregister::register_w!($mod_name, $struct_name);
+        libregister::register_rw!($mod_name, $struct_name, $mask);
+    );
 }

 /// Define a 1-bit field of a register
@ -169,6 +198,7 @@ macro_rules! register_bit {
        $(#[$outer])*
        impl $mod_name::Read {
            #[allow(unused)]
+            #[inline]
            pub fn $name(&self) -> bool {
                use bit_field::BitField;

@ -179,6 +209,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;

@ -187,6 +218,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
@ -195,6 +267,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;
@ -207,6 +280,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;

@ -226,6 +300,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;
@ -237,6 +312,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;
@ -254,6 +330,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,24 @@
+[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_cora_z7_10 = ["libboard_zynq/target_cora_z7_10"]
+target_redpitaya = ["libboard_zynq/target_redpitaya"]
+panic_handler = []
+dummy_irq_handler = []
+alloc_core = []
+
+default = ["panic_handler", "dummy_irq_handler"]
+
+[dependencies]
+r0 = "1"
+compiler_builtins = "0.1"
+linked_list_allocator = { version = "0.8", default-features = false }
+libregister = { path = "../libregister" }
+libcortex_a9 = { path = "../libcortex_a9" }
+libboard_zynq = { path = "../libboard_zynq" }
--- a/libsupport_zynq/src/abort.rs
+++ b/libsupport_zynq/src/abort.rs
@ -0,0 +1,70 @@
+use libregister::RegisterR;
+use libcortex_a9::regs::{DFSR, MPIDR};
+use libboard_zynq::{println, stdio};
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+pub unsafe extern "C" fn UndefinedInstruction() {
+    stdio::drop_uart();
+    println!("UndefinedInstruction");
+    loop {}
+}
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+pub unsafe extern "C" fn SoftwareInterrupt() {
+    stdio::drop_uart();
+    println!("SoftwareInterrupt");
+    loop {}
+}
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+pub unsafe extern "C" fn PrefetchAbort() {
+    stdio::drop_uart();
+    println!("PrefetchAbort");
+    loop {}
+}
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+pub unsafe extern "C" fn DataAbort() {
+    stdio::drop_uart();
+
+    println!("DataAbort on core {}", MPIDR.read().cpu_id());
+    println!("DFSR: {:03X}", DFSR.read());
+
+    loop {}
+}
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+pub unsafe extern "C" fn ReservedException() {
+    stdio::drop_uart();
+    println!("ReservedException");
+    loop {}
+}
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+#[cfg(feature = "dummy_irq_handler")]
+pub unsafe extern "C" fn IRQ() {
+    stdio::drop_uart();
+    println!("IRQ");
+    loop {}
+}
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+pub unsafe extern "C" fn FIQ() {
+    stdio::drop_uart();
+    println!("FIQ");
+    loop {}
+}
--- a/libsupport_zynq/src/boot.rs
+++ b/libsupport_zynq/src/boot.rs
@ -1,39 +1,37 @@
 use r0::zero_bss;
+use core::ptr::write_volatile;
 use libregister::{
    VolatileCell,
    RegisterR, RegisterW, RegisterRW,
 };
-use libcortex_a9::{asm, regs::*, cache, mmu};
+use libcortex_a9::{asm, l2c, regs::*, cache, mmu, spin_lock_yield, notify_spin_lock};
 use libboard_zynq::{slcr, mpcore};

 extern "C" {
    static mut __bss_start: u32;
    static mut __bss_end: u32;
-    static mut __stack_start: u32;
+    static mut __stack0_start: u32;
+    static mut __stack1_start: u32;
    fn main_core0();
    fn main_core1();
 }

-/// `0` means: wait for initialization by core0
-static mut CORE1_STACK: VolatileCell<u32> = VolatileCell::new(0);
+static mut CORE1_ENABLED: VolatileCell<bool> = VolatileCell::new(false);

 #[link_section = ".text.boot"]
 #[no_mangle]
 #[naked]
-pub unsafe extern "C" fn _boot_cores() -> ! {
-    const CORE_MASK: u32 = 0x3;
-
-    match MPIDR.read() & CORE_MASK {
+pub unsafe extern "C" fn Reset() -> ! {
+    match MPIDR.read().cpu_id() {
        0 => {
-            SP.write(&mut __stack_start as *mut _ as u32);
+            SP.write(&mut __stack0_start as *mut _ as u32);
            boot_core0();
        }
        1 => {
-            while CORE1_STACK.get() == 0 {
-                asm::wfe();
+            while !CORE1_ENABLED.get() {
+                spin_lock_yield();
            }
-
-            SP.write(CORE1_STACK.get());
+            SP.write(&mut __stack1_start as *mut _ as u32);
            boot_core1();
        }
        _ => unreachable!(),
@ -45,7 +43,7 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
 unsafe fn boot_core0() -> ! {
    l1_cache_init();

-    let mpcore = mpcore::RegisterBlock::new();
+    let mpcore = mpcore::RegisterBlock::mpcore();
    mpcore.scu_invalidate.invalidate_all_cores();

    zero_bss(&mut __bss_start, &mut __bss_end);
@ -55,10 +53,12 @@ unsafe fn boot_core0() -> ! {
    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_irq();
        main_core0();
        panic!("return from main");
    });
@ -69,16 +69,18 @@ unsafe fn boot_core0() -> ! {
 unsafe fn boot_core1() -> ! {
    l1_cache_init();

-    let mpcore = mpcore::RegisterBlock::new();
+    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_irq();
        main_core1();
        panic!("return from main_core1");
    });
@ -101,32 +103,15 @@ fn l1_cache_init() {
    //       for all of the L1 data cache rather than a (previously
    //       unspecified) combination of one cache set and one cache
    //       way.
-    dciall();
+    dciall_l1();
 }

-pub struct Core1<S: AsMut<[u32]>> {
-    pub stack: S,
+pub struct Core1 {
 }

-impl<S: AsMut<[u32]>> Core1<S> {
-    pub fn reset(&self) {
-        unsafe {
-            CORE1_STACK.set(0);
-        }
-
-        slcr::RegisterBlock::unlocked(|slcr| {
-            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(true));
-            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
-        });
-    }
-
+impl Core1 {
    /// Reset and start core1
-    ///
-    /// The stack must not be in OCM because core1 still has to
-    /// initialize its MMU before it can access DDR.
-    pub fn start(stack: S) -> Self {
-        let mut core = Core1 { stack };
-        
+    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));
@ -134,22 +119,54 @@ impl<S: AsMut<[u32]>> Core1<S> {
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
        });

-        let stack = core.stack.as_mut();
-        let stack_start = &mut stack[stack.len() - 1];
-        unsafe {
-            CORE1_STACK.set(stack_start as *mut _ as u32);
+        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);
        }
-        // Ensure stack pointer has been written to cache
-        asm::dmb();
        // Flush cache-line
-        cache::dccmvac(unsafe { &CORE1_STACK } as *const _ as u32);
+        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();

-        core
+        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/lib.rs
+++ b/libsupport_zynq/src/lib.rs
@ -9,6 +9,6 @@ pub extern crate compiler_builtins;

 pub mod boot;
 mod abort;
+#[cfg(feature = "panic_handler")]
 mod panic;
 pub mod ram;
-pub use smoltcp;
--- a/libsupport_zynq/src/panic.rs
+++ b/libsupport_zynq/src/panic.rs
@ -1,4 +1,4 @@
-use libboard_zynq::{slcr, print, println};
+use libboard_zynq::{print, println};

 #[panic_handler]
 fn panic(info: &core::panic::PanicInfo) -> ! {
@ -14,6 +14,5 @@ fn panic(info: &core::panic::PanicInfo) -> ! {
        println!("");
    }

-    slcr::RegisterBlock::unlocked(|slcr| slcr.soft_reset());
    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,72 +0,0 @@
-ENTRY(_boot_cores);
-
-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);
-
-  /DISCARD/ :
-  {
-    /* Unused exception related info that only wastes space */
-    *(.ARM.exidx);
-    *(.ARM.exidx.*);
-    *(.ARM.extab.*);
-  }
-}
--- a/nix/channel-rust-nightly.toml
+++ b/nix/channel-rust-nightly.toml
--- a/nix/fsbl.nix
+++ b/nix/fsbl.nix
@ -0,0 +1,37 @@
+{ pkgs, board ? "zc706" }:
+let
+  gnutoolchain = import ./gnutoolchain.nix { inherit pkgs; };
+in
+pkgs.stdenv.mkDerivation {
+  name = "${board}-fsbl";
+  src = pkgs.fetchFromGitHub {
+    owner = "Xilinx";
+    repo = "embeddedsw";
+    rev = "65c849ed46c88c67457e1fc742744f96db968ff1";
+    sha256 = "1rvl06ha40dzd6s9aa4sylmksh4xb9dqaxq462lffv1fdk342pda";
+  };
+  patches = [ ./fsbl.patch ];
+  nativeBuildInputs = [
+    pkgs.gnumake
+    gnutoolchain.binutils
+    gnutoolchain.gcc
+  ];
+  patchPhase =
+    ''
+    patch -p1 -i ${./fsbl.patch}
+    patchShebangs lib/sw_apps/zynq_fsbl/misc/copy_bsp.sh
+    echo 'SEARCH_DIR("${gnutoolchain.newlib}/arm-none-eabi/lib");' >> lib/sw_apps/zynq_fsbl/src/lscript.ld
+    '';
+  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;
+}
--- a/nix/fsbl.patch
+++ b/nix/fsbl.patch
@ -0,0 +1,31 @@
+diff --git a/lib/sw_apps/zynq_fsbl/src/Makefile b/lib/sw_apps/zynq_fsbl/src/Makefile
+index 0e3ccdf1c5..a5b02f386e 100644
+--- a/lib/sw_apps/zynq_fsbl/src/Makefile
+++ b/lib/sw_apps/zynq_fsbl/src/Makefile
+@@ -71,11 +71,14 @@ endif
+ all: $(EXEC)
+ 
+ $(EXEC): $(LIBS) $(OBJS) $(INCLUDES)
+-	cp $(BSP_DIR)/$(BOARD)/ps7_init.* .
+ 	$(LINKER) $(LD1FLAGS)  -o $@ $(OBJS) $(LDFLAGS)
+ 	rm -rf $(OBJS)
+-	
+-	
+
+.PHONY: ps7_init
+
+ps7_init:
+	cp $(BSP_DIR)/$(BOARD)/ps7_init.* .
+
+ $(LIBS): 
+ 	echo "Copying BSP files"
+ 	$(BSP_DIR)/copy_bsp.sh $(BOARD) $(CC)
+@@ -86,7 +89,7 @@ $(LIBS):
+         make -C $(BSP_DIR) -k all "CC=armcc" "AR=armar" "C_FLAGS= -O2 -c" "EC_FLAGS=--debug --wchar32"; \
+ 	fi;
+ 
+-%.o:%.c
+%.o:%.c ps7_init
+ 	$(CC) $(CC_FLAGS) $(CFLAGS) $(ECFLAGS) -c $< -o $@ $(INCLUDEPATH)
+ 
+ %.o:%.S
--- a/nix/gnutoolchain.nix
+++ b/nix/gnutoolchain.nix
@ -0,0 +1,134 @@
+{ pkgs ? import <nixpkgs> }:
+let 
+
+platform = "arm-none-eabi";
+
+binutils-pkg = { stdenv, buildPackages
+, fetchurl, zlib
+, extraConfigureFlags ? []
+}:
+
+stdenv.mkDerivation rec {
+  basename = "binutils";
+  version = "2.30";
+  name = "${basename}-${platform}-${version}";
+  src = fetchurl {
+    url = "https://ftp.gnu.org/gnu/binutils/binutils-${version}.tar.bz2";
+    sha256 = "028cklfqaab24glva1ks2aqa1zxa6w6xmc8q34zs1sb7h22dxspg";
+  };
+  configureFlags = [
+    "--enable-deterministic-archives"
+    "--target=${platform}"
+    "--with-cpu=cortex-a9"
+    "--with-fpu=vfpv3"
+    "--with-float=hard"
+    "--with-mode=thumb"
+  ] ++ extraConfigureFlags;
+  outputs = [ "out" "info" "man" ];
+  depsBuildBuild = [ buildPackages.stdenv.cc ];
+  buildInputs = [ zlib ];
+  enableParallelBuilding = true;
+  meta = {
+    description = "Tools for manipulating binaries (linker, assembler, etc.)";
+    longDescription = ''
+      The GNU Binutils are a collection of binary tools.  The main
+      ones are `ld' (the GNU linker) and `as' (the GNU assembler).
+      They also include the BFD (Binary File Descriptor) library,
+      `gprof', `nm', `strip', etc.
+    '';
+    homepage = http://www.gnu.org/software/binutils/;
+    license = stdenv.lib.licenses.gpl3Plus;
+    /* Give binutils a lower priority than gcc-wrapper to prevent a
+       collision due to the ld/as wrappers/symlinks in the latter. */
+    priority = "10";
+  };
+};
+
+
+gcc-pkg = { stdenv, buildPackages
+, fetchurl, gmp, mpfr, libmpc, platform-binutils
+, extraConfigureFlags ? []
+}:
+
+stdenv.mkDerivation rec {
+  basename = "gcc";
+  version = "9.1.0";
+  name = "${basename}-${platform}-${version}";
+  src = fetchurl {
+    url = "https://ftp.gnu.org/gnu/gcc/gcc-${version}/gcc-${version}.tar.xz";
+    sha256 = "1817nc2bqdc251k0lpc51cimna7v68xjrnvqzvc50q3ax4s6i9kr";
+  };
+  preConfigure =
+    ''
+    mkdir build
+    cd build
+    '';
+  configureScript = "../configure";
+  configureFlags =
+    [ "--target=${platform}"
+      "--with-arch=armv7-a"
+      "--with-tune=cortex-a9"
+      "--with-fpu=vfpv3"
+      "--with-float=hard"
+      "--disable-libssp"
+      "--enable-languages=c"
+      "--with-as=${platform-binutils}/bin/${platform}-as"
+      "--with-ld=${platform-binutils}/bin/${platform}-ld" ] ++ extraConfigureFlags;
+  outputs = [ "out" "info" "man" ];
+  hardeningDisable = [ "format" "pie" ];
+  propagatedBuildInputs = [ gmp mpfr libmpc platform-binutils ];
+  enableParallelBuilding = true;
+  dontFixup = true;
+};
+
+
+newlib-pkg = { stdenv, fetchurl, buildPackages, platform-binutils, platform-gcc }:
+
+stdenv.mkDerivation rec {
+  pname = "newlib";
+  version = "3.1.0";
+  src = fetchurl {
+    url = "ftp://sourceware.org/pub/newlib/newlib-${version}.tar.gz";
+    sha256 = "0ahh3n079zjp7d9wynggwrnrs27440aac04340chf1p9476a2kzv";
+  };
+
+  nativeBuildInputs = [ platform-binutils platform-gcc ];
+
+  configureFlags = [
+    "--target=${platform}"
+
+    "--with-cpu=cortex-a9"
+    "--with-fpu=vfpv3"
+    "--with-float=hard"
+    "--with-mode=thumb"
+    "--enable-interwork"
+    "--disable-multilib"
+
+    "--disable-newlib-supplied-syscalls"
+    "--with-gnu-ld"
+    "--with-gnu-as"
+    "--disable-newlib-io-float"
+    "--disable-werror"
+  ];
+  dontFixup = true;
+};
+
+
+in rec {
+  binutils-bootstrap = pkgs.callPackage binutils-pkg { };
+  gcc-bootstrap = pkgs.callPackage gcc-pkg {
+    platform-binutils = binutils-bootstrap;
+    extraConfigureFlags = [ "--disable-libgcc" ];
+  };
+  newlib = pkgs.callPackage newlib-pkg {
+    platform-binutils = binutils-bootstrap;
+    platform-gcc = gcc-bootstrap;
+  };
+  binutils = pkgs.callPackage binutils-pkg {
+    extraConfigureFlags = [ "--with-lib-path=${newlib}/arm-none-eabi/lib" ];
+  };
+  gcc = pkgs.callPackage gcc-pkg {
+    platform-binutils = binutils;
+    extraConfigureFlags = [ "--enable-newlib" "--with-headers=${newlib}/arm-none-eabi/include" ];
+  };
+}
--- a/Show More
+++ b/Show More
				`@ -1 +0,0 @@`
				`nix-shell --command "cargo xbuild --release" && scp -P 2204 -C target/armv7-none-eabihf/release/zc706-experiments $1@nixbld.m-labs.hk:/home/$1/zc706/zc706.elf`