update dependencies

flake output hashes
up cargo dependencies
2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00 · 2024-12-20 12:17:01 +08:00
117 changed files with 10896 additions and 19335 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/.gitignore
+++ b/.gitignore
@ -1 +1,2 @@
 /target
+result*
--- a/Cargo.lock
+++ b/Cargo.lock
@ -1,183 +1,240 @@
 # This file is automatically @generated by Cargo.
 # It is not intended for manual editing.
+version = 3
+
 [[package]]
 name = "bit_field"
-version = "0.10.0"
+version = "0.10.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dcb6dd1c2376d2e096796e234a70e17e94cc2d5d54ff8ce42b28cef1d0d359a4"

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

 [[package]]
 name = "byteorder"
-version = "1.3.4"
+version = "1.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "60f0b0d4c0a382d2734228fd12b5a6b5dac185c60e938026fd31b265b94f9bd2"
+
+[[package]]
+name = "cc"
+version = "1.0.73"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2fff2a6927b3bb87f9595d67196a70493f627687a71d87a0d692242c33f58c11"

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

 [[package]]
 name = "compiler_builtins"
-version = "0.1.27"
+version = "0.1.49"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "20b1438ef42c655665a8ab2c1c6d605a305f031d38d9be689ddfef41a20f3aa2"
+
+[[package]]
+name = "core_io"
+version = "0.1.0"
+source = "git+https://git.m-labs.hk/M-Labs/rs-core_io.git?rev=e9d3edf027#e9d3edf0272502b0dd6c26e8a4869c2912657615"

 [[package]]
 name = "embedded-hal"
-version = "0.2.3"
+version = "0.2.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff"
 dependencies = [
- "nb 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)",
- "void 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)",
+ "nb 0.1.3",
+ "void",
 ]

 [[package]]
 name = "experiments"
 version = "0.0.0"
 dependencies = [
- "embedded-hal 0.2.3 (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",
- "libsupport_zynq 0.0.0",
- "log 0.4.8 (registry+https://github.com/rust-lang/crates.io-index)",
+ "embedded-hal",
+ "libasync",
+ "libboard_zynq",
+ "libcortex_a9",
+ "libregister",
+ "libsupport_zynq",
+ "log",
+]
+
+[[package]]
+name = "fatfs"
+version = "0.3.6"
+source = "git+https://git.m-labs.hk/M-Labs/rust-fatfs.git?rev=4b5e420084#4b5e420084fd1c4a9c105680b687523909b6469c"
+dependencies = [
+ "bitflags",
+ "byteorder",
+ "core_io",
+ "log",
 ]

 [[package]]
 name = "libasync"
 version = "0.0.0"
 dependencies = [
- "embedded-hal 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)",
- "libcortex_a9 0.0.0",
- "nb 0.1.2 (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)",
+ "embedded-hal",
+ "libcortex_a9",
+ "nb 1.0.0",
+ "pin-utils",
+ "smoltcp",
 ]

 [[package]]
 name = "libboard_zynq"
 version = "0.0.0"
 dependencies = [
- "bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)",
- "embedded-hal 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)",
- "libcortex_a9 0.0.0",
- "libregister 0.0.0",
- "log 0.4.8 (registry+https://github.com/rust-lang/crates.io-index)",
- "nb 0.1.2 (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)",
+ "bit_field",
+ "embedded-hal",
+ "libasync",
+ "libcortex_a9",
+ "libregister",
+ "log",
+ "nb 0.1.3",
+ "smoltcp",
+ "void",
+ "volatile-register",
+]
+
+[[package]]
+name = "libconfig"
+version = "0.1.0"
+dependencies = [
+ "core_io",
+ "fatfs",
+ "libboard_zynq",
+ "log",
 ]

 [[package]]
 name = "libcortex_a9"
 version = "0.0.0"
 dependencies = [
- "bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)",
- "libregister 0.0.0",
- "volatile-register 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "bit_field",
+ "libregister",
+ "volatile-register",
 ]

 [[package]]
 name = "libregister"
 version = "0.0.0"
 dependencies = [
- "bit_field 0.10.0 (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)",
+ "bit_field",
+ "vcell",
+ "volatile-register",
 ]

 [[package]]
 name = "libsupport_zynq"
 version = "0.0.0"
 dependencies = [
- "compiler_builtins 0.1.27 (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.3 (registry+https://github.com/rust-lang/crates.io-index)",
- "r0 1.0.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "cc",
+ "compiler_builtins",
+ "libboard_zynq",
+ "libcortex_a9",
+ "libregister",
+ "linked_list_allocator",
+ "r0",
 ]

 [[package]]
 name = "linked_list_allocator"
-version = "0.8.3"
+version = "0.8.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "822add9edb1860698b79522510da17bef885171f75aa395cff099d770c609c24"

 [[package]]
 name = "log"
-version = "0.4.8"
+version = "0.4.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "51b9bbe6c47d51fc3e1a9b945965946b4c44142ab8792c50835a980d362c2710"
 dependencies = [
- "cfg-if 0.1.10 (registry+https://github.com/rust-lang/crates.io-index)",
+ "cfg-if",
 ]

 [[package]]
 name = "managed"
-version = "0.7.1"
+version = "0.7.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c75de51135344a4f8ed3cfe2720dc27736f7711989703a0b43aadf3753c55577"

 [[package]]
 name = "nb"
-version = "0.1.2"
+version = "0.1.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f"
+dependencies = [
+ "nb 1.0.0",
+]
+
+[[package]]
+name = "nb"
+version = "1.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae"

 [[package]]
 name = "pin-utils"
 version = "0.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"

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

 [[package]]
 name = "smoltcp"
-version = "0.6.0"
+version = "0.7.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3e4a069bef843d170df47e7c0a8bf8d037f217d9f5b325865acc3e466ffe40d3"
 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)",
- "managed 0.7.1 (registry+https://github.com/rust-lang/crates.io-index)",
+ "bitflags",
+ "byteorder",
+ "managed",
+]
+
+[[package]]
+name = "szl"
+version = "0.1.0"
+dependencies = [
+ "byteorder",
+ "core_io",
+ "libboard_zynq",
+ "libconfig",
+ "libcortex_a9",
+ "libregister",
+ "libsupport_zynq",
+ "log",
 ]

 [[package]]
 name = "vcell"
-version = "0.1.2"
+version = "0.1.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002"

 [[package]]
 name = "void"
 version = "1.0.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d"

 [[package]]
 name = "volatile-register"
-version = "0.2.0"
+version = "0.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9ee8f19f9d74293faf70901bc20ad067dc1ad390d2cbf1e3f75f721ffee908b6"
 dependencies = [
- "vcell 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)",
+ "vcell",
 ]
-
-[metadata]
-"checksum bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)" = "a165d606cf084741d4ac3a28fb6e9b1eb0bd31f6cd999098cfddb0b2ab381dc0"
-"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.27 (registry+https://github.com/rust-lang/crates.io-index)" = "38f18416546abfbf8d801c555a0e99524453e7214f9cc9107ad49de3d5948ccc"
-"checksum embedded-hal 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)" = "ee4908a155094da7723c2d60d617b820061e3b4efcc3d9e293d206a5a76c170b"
-"checksum linked_list_allocator 0.8.3 (registry+https://github.com/rust-lang/crates.io-index)" = "d6b60501dd4c850950bb43f970d544f6ce04e0ca021da2db2538fbe9d923f19e"
-"checksum log 0.4.8 (registry+https://github.com/rust-lang/crates.io-index)" = "14b6052be84e6b71ab17edffc2eeabf5c2c3ae1fdb464aae35ac50c67a44e1f7"
-"checksum managed 0.7.1 (registry+https://github.com/rust-lang/crates.io-index)" = "fdcec5e97041c7f0f1c5b7d93f12e57293c831c646f4cc7a5db59460c7ea8de6"
-"checksum nb 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)" = "b1411551beb3c11dedfb0a90a0fa256b47d28b9ec2cdff34c25a2fa59e45dbdc"
-"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,19 +1,20 @@
 [workspace]
 members = [
-  "libregister", "libcortex_a9",
-  "libboard_zynq", "libsupport_zynq",
+  "libregister",
+  "libcortex_a9",
+  "libboard_zynq",
+  "libsupport_zynq",
  "libasync",
+  "libconfig",
  "experiments",
+  "szl",
 ]

-[profile.dev]
-panic = "abort"
-lto = false
-
 [profile.release]
 panic = "abort"
 debug = true
-# Link-Time Optimization:
-# turn off if you get unusable debug symbols.
+codegen-units = 1
+opt-level = 's'
 lto = true
-opt-level = 'z'  # Optimize for size.
+debug-assertions = false
+overflow-checks = false
--- a/165
+++ b/165
@ -0,0 +1,165 @@
+                   GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+  This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+  0. Additional Definitions.
+
+  As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+  "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+  An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+  A "Combined Work" is a work produced by combining or linking an
+Application with the Library.  The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+  The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+  The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+  1. Exception to Section 3 of the GNU GPL.
+
+  You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+  2. Conveying Modified Versions.
+
+  If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+   a) under this License, provided that you make a good faith effort to
+   ensure that, in the event an Application does not supply the
+   function or data, the facility still operates, and performs
+   whatever part of its purpose remains meaningful, or
+
+   b) under the GNU GPL, with none of the additional permissions of
+   this License applicable to that copy.
+
+  3. Object Code Incorporating Material from Library Header Files.
+
+  The object code form of an Application may incorporate material from
+a header file that is part of the Library.  You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+   a) Give prominent notice with each copy of the object code that the
+   Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the object code with a copy of the GNU GPL and this license
+   document.
+
+  4. Combined Works.
+
+  You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+   a) Give prominent notice with each copy of the Combined Work that
+   the Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the Combined Work with a copy of the GNU GPL and this license
+   document.
+
+   c) For a Combined Work that displays copyright notices during
+   execution, include the copyright notice for the Library among
+   these notices, as well as a reference directing the user to the
+   copies of the GNU GPL and this license document.
+
+   d) Do one of the following:
+
+       0) Convey the Minimal Corresponding Source under the terms of this
+       License, and the Corresponding Application Code in a form
+       suitable for, and under terms that permit, the user to
+       recombine or relink the Application with a modified version of
+       the Linked Version to produce a modified Combined Work, in the
+       manner specified by section 6 of the GNU GPL for conveying
+       Corresponding Source.
+
+       1) Use a suitable shared library mechanism for linking with the
+       Library.  A suitable mechanism is one that (a) uses at run time
+       a copy of the Library already present on the user's computer
+       system, and (b) will operate properly with a modified version
+       of the Library that is interface-compatible with the Linked
+       Version.
+
+   e) Provide Installation Information, but only if you would otherwise
+   be required to provide such information under section 6 of the
+   GNU GPL, and only to the extent that such information is
+   necessary to install and execute a modified version of the
+   Combined Work produced by recombining or relinking the
+   Application with a modified version of the Linked Version. (If
+   you use option 4d0, the Installation Information must accompany
+   the Minimal Corresponding Source and Corresponding Application
+   Code. If you use option 4d1, you must provide the Installation
+   Information in the manner specified by section 6 of the GNU GPL
+   for conveying Corresponding Source.)
+
+  5. Combined Libraries.
+
+  You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+   a) Accompany the combined library with a copy of the same work based
+   on the Library, uncombined with any other library facilities,
+   conveyed under the terms of this License.
+
+   b) Give prominent notice with the combined library that part of it
+   is a work based on the Library, and explaining where to find the
+   accompanying uncombined form of the same work.
+
+  6. Revised Versions of the GNU Lesser General Public License.
+
+  The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+  Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+  If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.
--- a/README.md
+++ b/README.md
@ -1,47 +1,49 @@
-# Build
+# Bare-metal Rust on Zynq-7000
+
+Supported features:
+
+* Clocking setup
+* UART
+* SDRAM setup
+* Ethernet with smoltcp and async-await on TCP sockets
+* SD card
+* PL programming and startup
+* Pure Rust SZL first-stage bootloader, with SD boot and netboot
+* Control of second CPU core and message passing, with async-await support
+
+
+Supported boards:
+ * Kasli-SoC
+ * ZC706
+ * Red Pitaya
+ * Cora Z7-10 (seems to also run on Cora Z7-07S, including dual-core support)
+
+## Build
+
+Zynq-rs is packaged using the [Nix](https://nixos.org) Flakes system. Install Nix 2.4+ and enable flakes by adding ``experimental-features = nix-command flakes`` to ``nix.conf`` (e.g. ``~/.config/nix/nix.conf``). 
+
+You can build SZL or experiments crate for the platform of your choice by using ``nix build`` command, e.g.

 ```shell
-nix-shell --command "cargo xbuild --release"
+nix build .#coraz7-experiments
 ```

-Currently the ELF output is placed at `target/armv7-none-eabihf/release/experiments`
+Alternatively, you can still use ``cargo xbuild`` within ``nix develop`` shell.

-# 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
+nix develop
+cargo xbuild --release -p experiments
 ```

-Connect to target (given it is connected and you have permissions):
-```tcl
-connect arm hw
-```
+Currently the ELF output is placed at `target/armv7-none-eabihf/release/experiments`, or `result/experiments.elf` for Nix Flakes build.

-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
+## Debug

 ### Running on the ZC706

 ```shell
-nix-shell --command "cargo xbuild --release"
+nix develop
+cargo xbuild --release -p experiments
 cd openocd
 openocd -f zc706.cfg
 ```
@ -49,7 +51,8 @@ openocd -f zc706.cfg
 ### Running on the Cora Z7-10

 ```shell
-nix-shell --command "cd experiments && cargo xbuild --release --no-default-features --features=target_cora_z7_10"
+nix develop
+cargo xbuild --release -p experiments --no-default-features --features=target_coraz7
 cd openocd
 openocd -f cora-z7-10.cfg
 ```
@ -57,43 +60,10 @@ openocd -f cora-z7-10.cfg
 ### Loading a bitstream into volatile memory

 ```shell
-  openocd -f zc706.cfg -c "pld load 0 blinker_migen.bit; exit"
+openocd -f zc706.cfg -c "pld load 0 blinker_migen.bit; exit"
 ```

-### Development Process
-
-Clone this repo onto your development/build machine and the raspberry pi that controls the Xilinx 7000 board
-
-On the dev machine, the below script builds zc706 and secure copies it to the target pi (in your pi $HOME directory)
-```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`.
+## License

+Copyright (C) 2019-2022 M-Labs Limited.
+Released under the GNU LGPL v3. See the LICENSE file for details.
--- a/armv7-none-eabihf.json
+++ b/armv7-none-eabihf.json
@ -1,18 +1,10 @@
 {
-  "abi-blacklist": [
-    "stdcall",
-    "fastcall",
-    "vectorcall",
-    "thiscall",
-    "win64",
-    "sysv64"
-  ],
  "arch": "arm",
  "data-layout": "e-m:e-p:32:32-Fi8-i64:64-v128:64:128-a:0:32-n32-S64",
  "emit-debug-gdb-scripts": false,
  "env": "",
  "executables": true,
-  "features": "+v7,+vfp3,-d32,+thumb2,-neon,+strict-align",
+  "features": "+v7,+vfp3,-d32,+thumb2,-neon",
  "is-builtin": false,
  "linker": "rust-lld",
  "linker-flavor": "ld.lld",
--- 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/channel-rust-nightly.toml
+++ b/channel-rust-nightly.toml
--- a/default.nix
+++ b/default.nix
@ -1,63 +0,0 @@
-{ # 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-04-25
-    rev = "14b15521c52549ebbb113173b4abecd124b5a823";
-    sha256 = "0a6bi8g636cajpdrpcfkpza95b7ss7041m9cs6hxcd7h8bf6xhwi";
-    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 ];
-      buildPhase = ''
-        pushd ${crateSubdir}
-        cargo xbuild --release --frozen \
-          --no-default-features \
-          --features=${cargoFeatures}
-        popd
-      '';
-      XARGO_RUST_SRC = "${rustcSrc}/src";
-      installPhase = ''
-        mkdir $out
-        ls -la target/armv7-none-eabihf/release/
-        cp target/armv7-none-eabihf/release/${name} $out/${name}.elf
-      '';
-    });
-  xbuildCrate = name: crate: features: xbuildRustPackage rec {
-    name = "${crate}";
-    src = ./.;
-    crateSubdir = crate;
-    cargoSha256 = "0xlynsr94dyv0g41qwk5490w3wnzd5g70msaih6mcbgr3v4s2q34";
-    cargoFeatures = features;
-    doCheck = false;
-    dontFixup = true;
-  };
-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";
-  };
-}
--- a/experiments/Cargo.toml
+++ b/experiments/Cargo.toml
@ -2,12 +2,15 @@
 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", "libsupport_zynq/target_zc706"]
-target_cora_z7_10 = ["libboard_zynq/target_cora_z7_10", "libsupport_zynq/target_cora_z7_10"]
+target_coraz7 = ["libboard_zynq/target_coraz7", "libsupport_zynq/target_coraz7"]
+target_ebaz4205 = ["libboard_zynq/target_ebaz4205", "libsupport_zynq/target_ebaz4205"]
+target_redpitaya = ["libboard_zynq/target_redpitaya", "libsupport_zynq/target_redpitaya"]
+target_kasli_soc = ["libboard_zynq/target_kasli_soc", "libsupport_zynq/target_kasli_soc"]
 default = ["target_zc706"]

 [dependencies]
@ -16,5 +19,5 @@ embedded-hal = "0.2"
 libregister = { path = "../libregister" }
 libcortex_a9 = { path = "../libcortex_a9" }
 libboard_zynq = { path = "../libboard_zynq" }
-libsupport_zynq = { path = "../libsupport_zynq" }
+libsupport_zynq = { path = "../libsupport_zynq", default-features = false, features = ["panic_handler", "dummy_fiq_handler"]}
 libasync = { path = "../libasync" }
--- a/experiments/link.x
+++ b/experiments/link.x
@ -1,14 +1,4 @@
-ENTRY(_boot_cores);
-
-/* 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);
+ENTRY(Reset);

 MEMORY
 {
@ -42,19 +32,37 @@ SECTIONS
        *(.bss .bss.*);
        . = ALIGN(4);
        __bss_end = .;
-    } > OCM
+    } > OCM3
+
+    .irq_stack1 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack1_end = .;
+        . += 0x100;
+        __irq_stack1_start = .;
+    } > OCM3
+
+    .irq_stack0 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack0_end = .;
+        . += 0x100;
+        __irq_stack0_start = .;
+    } > OCM3

    .stack1 (NOLOAD) : ALIGN(8) {
        __stack1_end = .;
        . += 0x200;
        __stack1_start = .;
-    } > OCM
+    } > OCM3

    .stack0 (NOLOAD) : ALIGN(8) {
        __stack0_end = .;
-        . = ORIGIN(OCM) + LENGTH(OCM) - 8;
+        . = ORIGIN(OCM3) + LENGTH(OCM3) - 8;
        __stack0_start = .;
-    } > OCM
+
+        /* unused heap0 to prevent the linker from complaining*/
+        __heap0_start = .;
+        __heap0_end = .;
+    } > OCM3

    /DISCARD/ :
    {
--- a/experiments/src/main.rs
+++ b/experiments/src/main.rs
@ -1,10 +1,14 @@
 #![no_std]
 #![no_main]
+#![allow(incomplete_features)]
+#![feature(naked_functions)]
+#![feature(asm)]
+#![feature(inline_const)]

 extern crate alloc;

-use alloc::{borrow::ToOwned, collections::BTreeMap, format};
-use core::task::Poll;
+use alloc::collections::BTreeMap;
+use core::arch::asm;
 use libasync::{
    delay,
    smoltcp::{Sockets, TcpStream},
@ -14,65 +18,119 @@ use libboard_zynq::{
    self as zynq,
    clocks::source::{ArmPll, ClockSource, IoPll},
    clocks::Clocks,
-    print, println,
-    setup_l2cache,
-    sdio::sd_card::SdCard,
+    println, stdio,
+    mpcore,
+    gic,
    smoltcp::{
-        self,
        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,
-    sync_channel::{self, sync_channel},
+    l2c::enable_l2_cache,
+    sync_channel::{Sender, Receiver},
+    sync_channel,
+    regs::{MPIDR, SP},
+    spin_lock_yield, notify_spin_lock,
+    asm, interrupt_handler
 };
-use libregister::RegisterR;
+use libregister::{RegisterR, RegisterW};
 use libsupport_zynq::{
-    boot, ram,
+    boot, exception_vectors, ram,
 };
 use log::{info, warn};
-
-mod ps7_init;
+use core::sync::atomic::{AtomicBool, Ordering};

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

+static mut CORE1_REQ: (Sender<usize>, Receiver<usize>) = sync_channel!(usize, 10);
+static mut CORE1_RES: (Sender<usize>, Receiver<usize>) = sync_channel!(usize, 10);
+
+extern "C" {
+    static mut __stack1_start: u32;
+}
+
+static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
+
+interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
+    let mpcore = mpcore::RegisterBlock::mpcore();
+    let mut gic = gic::InterruptController::gic(mpcore);
+    let id = gic.get_interrupt_id();
+    match MPIDR.read().cpu_id(){
+        0 => {
+            if id.0 == 0 {
+                println!("Interrupting core0...");
+                gic.end_interrupt(id);
+                return;
+            }
+        },
+        1 => {
+            if id.0 == 0 {
+                gic.end_interrupt(id);
+                asm::exit_irq();
+                SP.write(&mut __stack1_start as *mut _ as u32);
+                asm::enable_irq();
+                CORE1_RESTART.store(false, Ordering::Relaxed);
+                notify_spin_lock();
+                main_core1();
+            }
+        },
+        _ => {}
+    }
+    stdio::drop_uart();
+    println!("IRQ");
+    loop {}
+});
+
+pub fn restart_core1() {
+    let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());
+    CORE1_RESTART.store(true, Ordering::Relaxed);
+    interrupt_controller.send_sgi(gic::InterruptId(0), gic::CPUCore::Core1.into());
+    while CORE1_RESTART.load(Ordering::Relaxed) {
+        spin_lock_yield();
+    }
+}
+
 #[no_mangle]
 pub fn main_core0() {
+    exception_vectors::set_vector_table(0x0);
    // zynq::clocks::CpuClocks::enable_io(1_250_000_000);
-    println!("\nzc706 main");
+    enable_l2_cache(0x8);
+    println!("\nZynq experiments");
+    let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());
+    interrupt_controller.enable_interrupts();

    libboard_zynq::logger::init().unwrap();
    log::set_max_level(log::LevelFilter::Trace);

    info!(
        "Boot mode: {:?}",
-        zynq::slcr::RegisterBlock::new()
+        zynq::slcr::RegisterBlock::slcr()
            .boot_mode
            .read()
            .boot_mode_pins()
    );

-    #[cfg(feature = "target_zc706")]
+    #[cfg(any(
+        feature = "target_zc706",
+        feature = "target_ebaz4205",
+        feature = "target_redpitaya",
+        feature = "target_kasli_soc",
+    ))]
    const CPU_FREQ: u32 = 800_000_000;
-    #[cfg(feature = "target_cora_z7_10")]
+    #[cfg(feature = "target_coraz7")]
    const CPU_FREQ: u32 = 650_000_000;

    info!("Setup clock sources...");
    ArmPll::setup(2 * CPU_FREQ);
    Clocks::set_cpu_freq(CPU_FREQ);
-    #[cfg(feature = "target_zc706")]
-    {
-        IoPll::setup(1_000_000_000);
-        libboard_zynq::stdio::drop_uart();
-    }
-    #[cfg(feature = "target_cora_z7_10")]
-    {
-        IoPll::setup(1_000_000_000);
-        libboard_zynq::stdio::drop_uart();
-    }
+    IoPll::setup(1_000_000_000);
+    libboard_zynq::stdio::drop_uart();
    info!("PLLs set up");
    let clocks = zynq::clocks::Clocks::get();
    info!(
@ -82,109 +140,82 @@ pub fn main_core0() {
        clocks.cpu_2x(),
        clocks.cpu_1x()
    );
-    info!("Setup L2Cache");
-    setup_l2cache();
-    info!("L2Cache done");
-
-if false {
-    let sd = libboard_zynq::sdio::SDIO::sdio0(true);
-    // only test SD card if it is inserted
-    if sd.is_card_inserted() {
-        let result = SdCard::from_sdio(sd);
-        match &result {
-            Ok(_) => info!("OK!"),
-            Err(a) => info!("{}", a),
-        };
-        const SIZE: usize = 512 * 2 + 1;
-        let mut sd_card = result.unwrap();
-        {
-            let buffer: [u8; SIZE] = [5; SIZE];
-            sd_card.write_block(0x0, 2, &buffer).unwrap();
-        }
-        let mut buffer: [u8; SIZE] = [0; SIZE];
-        sd_card.read_block(0x1, 2, &mut buffer[1..]).unwrap();
-        for i in 0..buffer.len() {
-            info!("buffer[{}] = {}", i, buffer[i]);
-        }
-        info!("End");
-    }
-
-    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!("");
-    }
-    let mut flash = flash.stop();
-}

    let timer = libboard_zynq::timer::GlobalTimer::start();

-    let mut ddr = zynq::ddr::DdrRam::new();
+    let mut ddr = zynq::ddr::DdrRam::ddrram();
    #[cfg(not(feature = "target_zc706"))]
    ddr.memtest();
    ram::init_alloc_ddr(&mut ddr);

-if false {
-    #[cfg(dev)]
-    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);
-        }
-        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);
-            }
-        }
-        println!("");
+    info!("Send software interrupt to core0");
+    interrupt_controller.send_sgi(gic::InterruptId(0), gic::CPUCore::Core0.into());
+    info!("Core0 returned from interrupt");

-        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);
+    boot::Core1::start(false);

-        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 = boot::Core1::start(false);
-
-    let (mut core1_req, rx) = sync_channel(10);
-    *CORE1_REQ.lock() = Some(rx);
-    let (tx, mut core1_res) = sync_channel(10);
-    *CORE1_RES.lock() = Some(tx);
+    let core1_req = unsafe { &mut CORE1_REQ.0 };
+    let core1_res = unsafe { &mut CORE1_RES.1 };
    task::block_on(async {
        for i in 0..10 {
+            restart_core1();
            core1_req.async_send(i).await;
            let j = core1_res.async_recv().await;
            println!("{} -> {}", i, j);
        }
    });
-    core1.disable();
-}
+    unsafe {
+        core1_req.drop_elements();
+    }

-    let eth = zynq::eth::Eth::default(HWADDR.clone());
+    // Test I2C
+    #[cfg(feature = "target_zc706")]
+    {
+        let mut i2c = zynq::i2c::I2c::i2c0();
+        i2c.init().unwrap();
+        println!("I2C bit-banging enabled");
+        let mut eeprom = zynq::i2c::eeprom::EEPROM::new(&mut i2c, 16);
+        // Write to 0x00 and 0x08
+        let eeprom_buffer: [u8; 22] = [
+            0x66, 0x77, 0x88, 0x99, 0xaa, 0xbb,
+            0x77, 0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee,
+            0xef, 0xcd, 0xab, 0x89, 0x67, 0x45, 0x23, 0x01,
+        ];
+        eeprom.write(0x00, &eeprom_buffer[0..6]).unwrap();
+        eeprom.write(0x08, &eeprom_buffer[6..22]).unwrap();
+        println!("Data written to EEPROM");
+        let mut eeprom_buffer = [0u8; 24];
+        // Read from 0x00
+        eeprom.read(0x00, &mut eeprom_buffer).unwrap();
+        print!("Data read from EEPROM @ 0x00: (hex) ");
+        for i in 0..6 {
+            print!("{:02x} ", eeprom_buffer[i]);
+        }
+        println!("");
+        // Read from 0x08
+        eeprom.read(0x08, &mut eeprom_buffer).unwrap();
+        print!("Data read from EEPROM @ 0x08: (hex) ");
+        for i in 0..16 {
+            print!("{:02x} ", eeprom_buffer[i]);
+        }
+        println!("");
+    }
+
+    #[cfg(feature = "target_kasli_soc")]
+    {
+        let mut err_cdwn = timer.countdown();
+        let mut err_state = true;
+        let mut led = zynq::error_led::ErrorLED::error_led();
+        task::spawn( async move { 
+            loop {
+                led.toggle(err_state);
+                err_state = !err_state;
+                delay(&mut err_cdwn, Milliseconds(1000)).await;
+            }
+        });
+    }
+
+    let eth = zynq::eth::Eth::eth0(HWADDR.clone());
    println!("Eth on");

    const RX_LEN: usize = 4096;
@ -207,44 +238,9 @@ if false {
        .neighbor_cache(neighbor_cache)
        .finalize();

-    #[cfg(feature = "target_zc706")]
-    ps7_init::report_differences();
-
    Sockets::init(32);
-    /// `chargen`
-    const TCP_PORT: u16 = 19;
-    async fn handle_connection(stream: TcpStream) -> smoltcp::Result<()> {
-        stream.send("Enter your name: ".bytes()).await?;
-        let name = stream
-            .recv(|buf| {
-                for (i, b) in buf.iter().enumerate() {
-                    if *b == '\n' as u8 {
-                        return match core::str::from_utf8(&buf[0..i]) {
-                            Ok(name) => Poll::Ready((i + 1, Some(name.to_owned()))),
-                            Err(_) => Poll::Ready((i + 1, None)),
-                        };
-                    }
-                }
-                if buf.len() > 100 {
-                    // Too much input, consume all
-                    Poll::Ready((buf.len(), None))
-                } else {
-                    Poll::Pending
-                }
-            })
-            .await?;
-        match name {
-            Some(name) => stream.send(format!("Hello {}!\n", name).bytes()).await?,
-            None => {
-                stream
-                    .send("I had trouble reading your name.\n".bytes())
-                    .await?
-            }
-        }
-        let _ = stream.close().await;
-        Ok(())
-    }

+    const TCP_PORT: u16 = 19;
    // (rx, tx)
    let stats = alloc::rc::Rc::new(core::cell::RefCell::new((0, 0)));
    let stats_tx = stats.clone();
@ -252,12 +248,12 @@ if false {
        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(65536).collect::<alloc::vec::Vec<u8>>();
+                let tx_data = (0..=255).cycle().take(4096).collect::<alloc::vec::Vec<u8>>();
                loop {
                    // const CHUNK_SIZE: usize = 65536;
                    // match stream.send((0..=255).cycle().take(CHUNK_SIZE)).await {
                    match stream.send_slice(&tx_data[..]).await {
-                        Ok(len) => stats_tx.borrow_mut().1 += tx_data.len(), //CHUNK_SIZE,
+                        Ok(_len) => stats_tx.borrow_mut().1 += tx_data.len(), //CHUNK_SIZE,
                        Err(e) => {
                            warn!("tx: {:?}", e);
                            break
@ -273,7 +269,7 @@ if false {
            let stats_rx = stats_rx.clone();
            task::spawn(async move {
                loop {
-                    match stream.recv(|buf| Poll::Ready((buf.len(), buf.len()))).await {
+                    match stream.recv(|buf| (buf.len(), buf.len())).await {
                        Ok(len) => stats_rx.borrow_mut().0 += len,
                        Err(e) => {
                            warn!("rx: {:?}", e);
@ -290,7 +286,7 @@ if false {
        loop {
            delay(&mut countdown, Milliseconds(1000)).await;

-            let timestamp = timer.get_us();
+            let timestamp = timer.get_us().0;
            let seconds = timestamp / 1_000_000;
            let micros = timestamp % 1_000_000;
            let (rx, tx) = {
@ -308,27 +304,18 @@ if false {
    })
 }

-static CORE1_REQ: Mutex<Option<sync_channel::Receiver<usize>>> = Mutex::new(None);
-static CORE1_RES: Mutex<Option<sync_channel::Sender<usize>>> = Mutex::new(None);
 static DONE: Mutex<bool> = Mutex::new(false);

 #[no_mangle]
 pub fn main_core1() {
    println!("Hello from core1!");
-
-    let mut req = None;
-    while req.is_none() {
-        req = CORE1_REQ.lock().take();
-    }
-    let req = req.unwrap();
-    let mut res = None;
-    while res.is_none() {
-        res = CORE1_RES.lock().take();
-    }
-    let mut res = res.unwrap();
+    let mut interrupt_controller = gic::InterruptController::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);
+        res.send(i * i);
    }

    println!("core1 done!");
--- a/experiments/src/ps7_init/mod.rs
+++ b/experiments/src/ps7_init/mod.rs
@ -1,68 +0,0 @@
-#![cfg(feature = "target_zc706")]
-
-use libboard_zynq::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 enum InitOp {
-    MaskWrite(usize, usize, usize),
-    MaskPoll(usize, usize),
-}
-
-impl InitOp {
-    fn address(&self) -> usize {
-        match self {
-            InitOp::MaskWrite(address, _, _) => *address,
-            InitOp::MaskPoll(address, _) => *address,
-        }
-    }
-
-    fn read(&self) -> usize {
-        unsafe { *(self.address() as *const usize) }
-    }
-
-    fn difference(&self) -> Option<(usize, usize)> {
-        let (mask, expected) = match self {
-            InitOp::MaskWrite(_, mask, expected) =>
-                (*mask, *expected),
-            InitOp::MaskPoll(_, mask) =>
-                (*mask, *mask),
-        };
-        let actual = self.read();
-        if actual & mask == expected {
-            None
-        } else {
-            Some((actual & mask, expected))
-        }
-    }
-
-    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
-            );
-        }
-    }
-}
--- a/experiments/src/ps7_init/zc706.rs
+++ b/experiments/src/ps7_init/zc706.rs
@ -1,203 +0,0 @@
-use super::InitOp::{self, *};
-
-pub const INIT_DATA: &'static [InitOp] = &[
-    // ps7_mio_init_data_1_0
-    MaskWrite(0xF8000B40, 0x00000FFF, 0x00000600),
-    MaskWrite(0xF8000B44, 0x00000FFF, 0x00000600),
-    MaskWrite(0xF8000B48, 0x00000FFF, 0x00000672),
-    MaskWrite(0xF8000B4C, 0x00000FFF, 0x00000672),
-    MaskWrite(0xF8000B50, 0x00000FFF, 0x00000674),
-    MaskWrite(0xF8000B54, 0x00000FFF, 0x00000674),
-    MaskWrite(0xF8000B58, 0x00000FFF, 0x00000600),
-    MaskWrite(0xF8000B5C, 0xFFFFFFFF, 0x0018C61C),
-    MaskWrite(0xF8000B60, 0xFFFFFFFF, 0x00F9861C),
-    MaskWrite(0xF8000B64, 0xFFFFFFFF, 0x00F9861C),
-    MaskWrite(0xF8000B68, 0xFFFFFFFF, 0x00F9861C),
-    MaskWrite(0xF8000B6C, 0x000073FF, 0x00000209),
-    MaskWrite(0xF8000B70, 0x00000021, 0x00000021),
-    MaskWrite(0xF8000B70, 0x00000021, 0x00000020),
-    MaskWrite(0xF8000B70, 0x07FFFFFF, 0x00000823),
-    MaskWrite(0xF8000700, 0x00003FFF, 0x00000600),
-    MaskWrite(0xF8000704, 0x00003FFF, 0x00000702),
-    MaskWrite(0xF8000708, 0x00003FFF, 0x00000702),
-    MaskWrite(0xF800070C, 0x00003FFF, 0x00000702),
-    MaskWrite(0xF8000710, 0x00003FFF, 0x00000702),
-    MaskWrite(0xF8000714, 0x00003FFF, 0x00000702),
-    MaskWrite(0xF8000718, 0x00003FFF, 0x00000702),
-    MaskWrite(0xF800071C, 0x00003FFF, 0x00000600),
-    MaskWrite(0xF8000720, 0x00003FFF, 0x00000700),
-    MaskWrite(0xF8000724, 0x00003FFF, 0x00000600),
-    MaskWrite(0xF8000728, 0x00003FFF, 0x00000600),
-    MaskWrite(0xF800072C, 0x00003FFF, 0x00000600),
-    MaskWrite(0xF8000730, 0x00003FFF, 0x00000600),
-    MaskWrite(0xF8000734, 0x00003FFF, 0x00000600),
-    MaskWrite(0xF8000738, 0x00003FFF, 0x00000600),
-    MaskWrite(0xF800073C, 0x00003FFF, 0x00000600),
-    MaskWrite(0xF8000740, 0x00003FFF, 0x00000302),
-    MaskWrite(0xF8000744, 0x00003FFF, 0x00000302),
-    MaskWrite(0xF8000748, 0x00003FFF, 0x00000302),
-    MaskWrite(0xF800074C, 0x00003FFF, 0x00000302),
-    MaskWrite(0xF8000750, 0x00003FFF, 0x00000302),
-    MaskWrite(0xF8000754, 0x00003FFF, 0x00000302),
-    MaskWrite(0xF8000758, 0x00003FFF, 0x00000303),
-    MaskWrite(0xF800075C, 0x00003FFF, 0x00000303),
-    MaskWrite(0xF8000760, 0x00003FFF, 0x00000303),
-    MaskWrite(0xF8000764, 0x00003FFF, 0x00000303),
-    MaskWrite(0xF8000768, 0x00003FFF, 0x00000303),
-    MaskWrite(0xF800076C, 0x00003FFF, 0x00000303),
-    MaskWrite(0xF8000770, 0x00003FFF, 0x00000304),
-    MaskWrite(0xF8000774, 0x00003FFF, 0x00000305),
-    MaskWrite(0xF8000778, 0x00003FFF, 0x00000304),
-    MaskWrite(0xF800077C, 0x00003FFF, 0x00000305),
-    MaskWrite(0xF8000780, 0x00003FFF, 0x00000304),
-    MaskWrite(0xF8000784, 0x00003FFF, 0x00000304),
-    MaskWrite(0xF8000788, 0x00003FFF, 0x00000304),
-    MaskWrite(0xF800078C, 0x00003FFF, 0x00000304),
-    MaskWrite(0xF8000790, 0x00003FFF, 0x00000305),
-    MaskWrite(0xF8000794, 0x00003FFF, 0x00000304),
-    MaskWrite(0xF8000798, 0x00003FFF, 0x00000304),
-    MaskWrite(0xF800079C, 0x00003FFF, 0x00000304),
-    MaskWrite(0xF80007A0, 0x00003FFF, 0x00000380),
-    MaskWrite(0xF80007A4, 0x00003FFF, 0x00000380),
-    MaskWrite(0xF80007A8, 0x00003FFF, 0x00000380),
-    MaskWrite(0xF80007AC, 0x00003FFF, 0x00000380),
-    MaskWrite(0xF80007B0, 0x00003FFF, 0x00000380),
-    MaskWrite(0xF80007B4, 0x00003FFF, 0x00000380),
-    MaskWrite(0xF80007B8, 0x00003F01, 0x00000201),
-    MaskWrite(0xF80007BC, 0x00003F01, 0x00000201),
-    MaskWrite(0xF80007C0, 0x00003FFF, 0x000002E0),
-    MaskWrite(0xF80007C4, 0x00003FFF, 0x000002E1),
-    MaskWrite(0xF80007C8, 0x00003FFF, 0x00000200),
-    MaskWrite(0xF80007CC, 0x00003FFF, 0x00000200),
-    MaskWrite(0xF80007D0, 0x00003FFF, 0x00000280),
-    MaskWrite(0xF80007D4, 0x00003FFF, 0x00000280),
-    MaskWrite(0xF8000830, 0x003F003F, 0x002F002E),
-    // ps7_pll_init_data_1_0
-    MaskWrite(0xF8000110, 0x003FFFF0, 0x000FA220),
-    MaskWrite(0xF8000100, 0x0007F000, 0x00028000),
-    MaskWrite(0xF8000100, 0x00000010, 0x00000010),
-    MaskWrite(0xF8000100, 0x00000001, 0x00000001),
-    MaskWrite(0xF8000100, 0x00000001, 0x00000000),
-    MaskPoll(0xF800010C, 0x00000001),
-    MaskWrite(0xF8000100, 0x00000010, 0x00000000),
-    MaskWrite(0xF8000120, 0x1F003F30, 0x1F000200),
-    MaskWrite(0xF8000114, 0x003FFFF0, 0x0012C220),
-    MaskWrite(0xF8000104, 0x0007F000, 0x00020000),
-    MaskWrite(0xF8000104, 0x00000010, 0x00000010),
-    MaskWrite(0xF8000104, 0x00000001, 0x00000001),
-    MaskWrite(0xF8000104, 0x00000001, 0x00000000),
-    MaskPoll(0xF800010C, 0x00000002),
-    MaskWrite(0xF8000104, 0x00000010, 0x00000000),
-    MaskWrite(0xF8000124, 0xFFF00003, 0x0C200003),
-    MaskWrite(0xF8000118, 0x003FFFF0, 0x001452C0),
-    MaskWrite(0xF8000108, 0x0007F000, 0x0001E000),
-    MaskWrite(0xF8000108, 0x00000010, 0x00000010),
-    MaskWrite(0xF8000108, 0x00000001, 0x00000001),
-    MaskWrite(0xF8000108, 0x00000001, 0x00000000),
-    MaskPoll(0xF800010C, 0x00000004),
-    MaskWrite(0xF8000108, 0x00000010, 0x00000000),
-    // ps7_clock_init_data_1_0
-    MaskWrite(0xF8000128, 0x03F03F01, 0x00700F01),
-    MaskWrite(0xF8000138, 0x00000011, 0x00000001),
-    MaskWrite(0xF8000140, 0x03F03F71, 0x00100801),
-    MaskWrite(0xF800014C, 0x00003F31, 0x00000501),
-    MaskWrite(0xF8000150, 0x00003F33, 0x00001401),
-    MaskWrite(0xF8000154, 0x00003F33, 0x00001402),
-    MaskWrite(0xF8000168, 0x00003F31, 0x00000501),
-    MaskWrite(0xF8000170, 0x03F03F30, 0x00200400),
-    MaskWrite(0xF80001C4, 0x00000001, 0x00000001),
-    MaskWrite(0xF800012C, 0x01FFCCCD, 0x01EC044D),
-    // ps7_ddr_init_data_1_0
-    MaskWrite(0xF8006000, 0x0001FFFF, 0x00000080),
-    MaskWrite(0xF8006004, 0x1FFFFFFF, 0x00081081),
-    MaskWrite(0xF8006008, 0x03FFFFFF, 0x03C0780F),
-    MaskWrite(0xF800600C, 0x03FFFFFF, 0x02001001),
-    MaskWrite(0xF8006010, 0x03FFFFFF, 0x00014001),
-    MaskWrite(0xF8006014, 0x001FFFFF, 0x0004159B),
-    MaskWrite(0xF8006018, 0xF7FFFFFF, 0x452460D2),
-    MaskWrite(0xF800601C, 0xFFFFFFFF, 0x720238E5),
-    MaskWrite(0xF8006020, 0xFFFFFFFC, 0x272872D0),
-    MaskWrite(0xF8006024, 0x0FFFFFFF, 0x0000003C),
-    MaskWrite(0xF8006028, 0x00003FFF, 0x00002007),
-    MaskWrite(0xF800602C, 0xFFFFFFFF, 0x00000008),
-    MaskWrite(0xF8006030, 0xFFFFFFFF, 0x00040930),
-    MaskWrite(0xF8006034, 0x13FF3FFF, 0x000116D4),
-    MaskWrite(0xF8006038, 0x00001FC3, 0x00000000),
-    MaskWrite(0xF800603C, 0x000FFFFF, 0x00000777),
-    MaskWrite(0xF8006040, 0xFFFFFFFF, 0xFFF00000),
-    MaskWrite(0xF8006044, 0x0FFFFFFF, 0x0FF66666),
-    MaskWrite(0xF8006048, 0x3FFFFFFF, 0x0003C248),
-    MaskWrite(0xF8006050, 0xFF0F8FFF, 0x77010800),
-    MaskWrite(0xF8006058, 0x0001FFFF, 0x00000101),
-    MaskWrite(0xF800605C, 0x0000FFFF, 0x00005003),
-    MaskWrite(0xF8006060, 0x000017FF, 0x0000003E),
-    MaskWrite(0xF8006064, 0x00021FE0, 0x00020000),
-    MaskWrite(0xF8006068, 0x03FFFFFF, 0x00284141),
-    MaskWrite(0xF800606C, 0x0000FFFF, 0x00001610),
-    MaskWrite(0xF80060A0, 0x00FFFFFF, 0x00008000),
-    MaskWrite(0xF80060A4, 0xFFFFFFFF, 0x10200802),
-    MaskWrite(0xF80060A8, 0x0FFFFFFF, 0x0690CB73),
-    MaskWrite(0xF80060AC, 0x000001FF, 0x000001FE),
-    MaskWrite(0xF80060B0, 0x1FFFFFFF, 0x1CFFFFFF),
-    MaskWrite(0xF80060B4, 0x000007FF, 0x00000200),
-    MaskWrite(0xF80060B8, 0x01FFFFFF, 0x00200066),
-    MaskWrite(0xF80060C4, 0x00000003, 0x00000000),
-    MaskWrite(0xF80060C8, 0x000000FF, 0x00000000),
-    MaskWrite(0xF80060DC, 0x00000001, 0x00000000),
-    MaskWrite(0xF80060F0, 0x0000FFFF, 0x00000000),
-    MaskWrite(0xF80060F4, 0x0000000F, 0x00000008),
-    MaskWrite(0xF8006114, 0x000000FF, 0x00000000),
-    MaskWrite(0xF8006118, 0x7FFFFFFF, 0x40000001),
-    MaskWrite(0xF800611C, 0x7FFFFFFF, 0x40000001),
-    MaskWrite(0xF8006120, 0x7FFFFFFF, 0x40000001),
-    MaskWrite(0xF8006124, 0x7FFFFFFF, 0x40000001),
-    MaskWrite(0xF800612C, 0x000FFFFF, 0x00033C03),
-    MaskWrite(0xF8006130, 0x000FFFFF, 0x00034003),
-    MaskWrite(0xF8006134, 0x000FFFFF, 0x0002F400),
-    MaskWrite(0xF8006138, 0x000FFFFF, 0x00030400),
-    MaskWrite(0xF8006140, 0x000FFFFF, 0x00000035),
-    MaskWrite(0xF8006144, 0x000FFFFF, 0x00000035),
-    MaskWrite(0xF8006148, 0x000FFFFF, 0x00000035),
-    MaskWrite(0xF800614C, 0x000FFFFF, 0x00000035),
-    MaskWrite(0xF8006154, 0x000FFFFF, 0x00000083),
-    MaskWrite(0xF8006158, 0x000FFFFF, 0x00000083),
-    MaskWrite(0xF800615C, 0x000FFFFF, 0x00000080),
-    MaskWrite(0xF8006160, 0x000FFFFF, 0x00000080),
-    MaskWrite(0xF8006168, 0x001FFFFF, 0x00000124),
-    MaskWrite(0xF800616C, 0x001FFFFF, 0x00000125),
-    MaskWrite(0xF8006170, 0x001FFFFF, 0x00000112),
-    MaskWrite(0xF8006174, 0x001FFFFF, 0x00000116),
-    MaskWrite(0xF800617C, 0x000FFFFF, 0x000000C3),
-    MaskWrite(0xF8006180, 0x000FFFFF, 0x000000C3),
-    MaskWrite(0xF8006184, 0x000FFFFF, 0x000000C0),
-    MaskWrite(0xF8006188, 0x000FFFFF, 0x000000C0),
-    MaskWrite(0xF8006190, 0xFFFFFFFF, 0x10040080),
-    MaskWrite(0xF8006194, 0x000FFFFF, 0x0001FC82),
-    MaskWrite(0xF8006204, 0xFFFFFFFF, 0x00000000),
-    MaskWrite(0xF8006208, 0x000F03FF, 0x000803FF),
-    MaskWrite(0xF800620C, 0x000F03FF, 0x000803FF),
-    MaskWrite(0xF8006210, 0x000F03FF, 0x000803FF),
-    MaskWrite(0xF8006214, 0x000F03FF, 0x000803FF),
-    MaskWrite(0xF8006218, 0x000F03FF, 0x000003FF),
-    MaskWrite(0xF800621C, 0x000F03FF, 0x000003FF),
-    MaskWrite(0xF8006220, 0x000F03FF, 0x000003FF),
-    MaskWrite(0xF8006224, 0x000F03FF, 0x000003FF),
-    MaskWrite(0xF80062A8, 0x00000FF7, 0x00000000),
-    MaskWrite(0xF80062AC, 0xFFFFFFFF, 0x00000000),
-    MaskWrite(0xF80062B0, 0x003FFFFF, 0x00005125),
-    MaskWrite(0xF80062B4, 0x0003FFFF, 0x000012A8),
-    MaskPoll(0xF8000B74, 0x00002000),
-    MaskWrite(0xF8006000, 0x0001FFFF, 0x00000081),
-    MaskPoll(0xF8006054, 0x00000007),
-    // ps7_peripherals_init_data_1_0
-    MaskWrite(0xF8000B48, 0x00000180, 0x00000180),
-    MaskWrite(0xF8000B4C, 0x00000180, 0x00000180),
-    MaskWrite(0xF8000B50, 0x00000180, 0x00000180),
-    MaskWrite(0xF8000B54, 0x00000180, 0x00000180),
-    MaskWrite(0xE0001034, 0x000000FF, 0x00000006),
-    MaskWrite(0xE0001018, 0x0000FFFF, 0x0000003E),
-    MaskWrite(0xE0001000, 0x000001FF, 0x00000017),
-    MaskWrite(0xE0001004, 0x00000FFF, 0x00000020),
-    MaskWrite(0xE000D000, 0x00080000, 0x00080000),
-    MaskWrite(0xF8007000, 0x20000000, 0x00000000),
-];
--- a/flake.lock
+++ b/flake.lock
@ -0,0 +1,49 @@
+{
+  "nodes": {
+    "nixpkgs": {
+      "locked": {
+        "lastModified": 1734529975,
+        "narHash": "sha256-ze3IJksru9dN0keqUxY0WNf8xrwfs8Ty/z9v/keyBbg=",
+        "owner": "NixOS",
+        "repo": "nixpkgs",
+        "rev": "72d11d40b9878a67c38f003c240c2d2e1811e72a",
+        "type": "github"
+      },
+      "original": {
+        "owner": "NixOS",
+        "ref": "nixos-24.05",
+        "repo": "nixpkgs",
+        "type": "github"
+      }
+    },
+    "root": {
+      "inputs": {
+        "nixpkgs": "nixpkgs",
+        "rust-overlay": "rust-overlay"
+      }
+    },
+    "rust-overlay": {
+      "inputs": {
+        "nixpkgs": [
+          "nixpkgs"
+        ]
+      },
+      "locked": {
+        "lastModified": 1719454714,
+        "narHash": "sha256-MojqG0lyUINkEk0b3kM2drsU5vyaF8DFZe/FAlZVOGs=",
+        "owner": "oxalica",
+        "repo": "rust-overlay",
+        "rev": "d1c527659cf076ecc4b96a91c702d080b213801e",
+        "type": "github"
+      },
+      "original": {
+        "owner": "oxalica",
+        "ref": "snapshot/2024-08-01",
+        "repo": "rust-overlay",
+        "type": "github"
+      }
+    }
+  },
+  "root": "root",
+  "version": 7
+}
--- a/flake.nix
+++ b/flake.nix
@ -0,0 +1,177 @@
+{
+  description = "Bare-metal Rust on Zynq-7000";
+
+  inputs.nixpkgs.url = github:NixOS/nixpkgs/nixos-24.05;
+  inputs.rust-overlay = {
+    url = "github:oxalica/rust-overlay?ref=snapshot/2024-08-01";
+    inputs.nixpkgs.follows = "nixpkgs";
+  };
+
+  outputs = { self, nixpkgs, rust-overlay }:
+    let
+      pkgs = import nixpkgs { system = "x86_64-linux"; overlays = [ (import rust-overlay) crosspkgs-overlay ]; };
+      
+      rust = pkgs.rust-bin.nightly."2021-09-01".default.override {
+        extensions = [ "rust-src" ];
+        targets = [ ];
+      };
+      rustPlatform = pkgs.makeRustPlatform {
+        rustc = rust // {
+          # https://github.com/oxalica/rust-overlay/commit/c48c2d76b68dd9ede0815fec53479375c61af857
+          targetPlatforms = pkgs.lib.platforms.all;
+          tier1TargetPlatforms = pkgs.lib.platforms.all;
+          badTargetPlatforms = [ ];
+        };
+        cargo = rust;
+      };
+
+      crosspkgs-overlay = (self: super: {
+        pkgsCross = super.pkgsCross // {
+          zynq-baremetal = import super.path {
+            system = "x86_64-linux";
+            crossSystem = {
+              config = "arm-none-eabihf";
+              libc = "newlib";
+              gcc.cpu = "cortex-a9";
+              gcc.fpu = "vfpv3";
+            };
+          };
+        };
+      });
+
+      mkbootimage = pkgs.stdenv.mkDerivation {
+        pname = "mkbootimage";
+        version = "2.3dev";
+
+        src = pkgs.fetchFromGitHub {
+          owner = "antmicro";
+          repo = "zynq-mkbootimage";
+          rev = "872363ce32c249f8278cf107bc6d3bdeb38d849f";
+          sha256 = "sha256-5FPyAhUWZDwHbqmp9J2ZXTmjaXPz+dzrJMolaNwADHs=";
+        };
+
+        propagatedBuildInputs = [ pkgs.libelf pkgs.pcre ];
+        patchPhase =
+        ''
+          substituteInPlace Makefile --replace "git rev-parse --short HEAD" "echo nix"
+        '';
+        installPhase =
+        ''
+          mkdir -p $out/bin
+          cp mkbootimage $out/bin
+        '';
+        hardeningDisable = [ "fortify" ];
+      };
+
+      fsbl = { board ? "zc706" }: pkgs.stdenv.mkDerivation {
+        name = "${board}-fsbl";
+        src = pkgs.fetchFromGitHub {
+          owner = "Xilinx";
+          repo = "embeddedsw";
+          rev = "xilinx_v2022.2";
+          sha256 = "sha256-UDz9KK/Hw3qM1BAeKif30rE8Bi6C2uvuZlvyvtJCMfw=";
+        };
+        nativeBuildInputs = [
+          pkgs.pkgsCross.zynq-baremetal.buildPackages.binutils
+          pkgs.pkgsCross.zynq-baremetal.buildPackages.gcc
+        ];
+        patchPhase = ''
+          patchShebangs lib/sw_apps/zynq_fsbl/misc/copy_bsp.sh
+
+          for x in lib/sw_apps/zynq_fsbl/src/Makefile lib/sw_apps/zynq_fsbl/misc/copy_bsp.sh lib/bsp/standalone/src/arm/cortexa9/gcc/Makefile; do
+            substituteInPlace $x \
+              --replace "arm-none-eabi-" "arm-none-eabihf-"
+          done
+        '';
+        buildPhase = ''
+          cd lib/sw_apps/zynq_fsbl/src
+          make BOARD=${board} "CFLAGS=-DFSBL_DEBUG_INFO -g"
+        '';
+        installPhase = ''
+          mkdir $out
+          cp fsbl.elf $out
+        '';
+        doCheck = false;
+        dontFixup = true;
+      };
+
+      cargo-xbuild = pkgs.cargo-xbuild.overrideAttrs(oa: {
+        postPatch = "substituteInPlace src/sysroot.rs --replace 2021 2018";
+      });
+
+      build-crate = name: crate: features: rustPlatform.buildRustPackage rec {
+        name = "${crate}";
+
+        src = builtins.filterSource (path: type:
+          baseNameOf path != "target"
+        ) ./.;
+        cargoLock = { 
+          lockFile = ./Cargo.lock;
+          outputHashes = {
+            "core_io-0.1.0" = "sha256-0HINFWRiJx8pjMgUOL/CS336ih7SENSRh3Kah9LPRrw="; 
+            "fatfs-0.3.6" = "sha256-Nz9hCq/1YgSXF8ltJ5ZawV0Hc8WV44KNK0tJdVnNb4U=";
+          };
+        };
+
+        nativeBuildInputs = [ cargo-xbuild pkgs.llvmPackages_13.clang-unwrapped ];
+        buildPhase = ''
+          export XARGO_RUST_SRC="${rust}/lib/rustlib/src/rust/library"
+          export CARGO_HOME=$(mktemp -d cargo-home.XXX)
+          pushd ${crate}
+          cargo xbuild --release --frozen \
+            --no-default-features \
+            --features=${features}
+          popd
+        '';
+
+        installPhase = ''
+          mkdir -p $out $out/nix-support
+          cp target/armv7-none-eabihf/release/${name} $out/${name}.elf
+          echo file binary-dist $out/${name}.elf >> $out/nix-support/hydra-build-products
+        '';
+
+        doCheck = false;
+        dontFixup = true;
+        auditable = false;
+      };
+
+      targetCrates = target: {
+        "${target}-experiments" = build-crate "${target}-experiments" "experiments" "target_${target}";
+        "${target}-szl" = build-crate "${target}-szl" "szl" "target_${target}";
+      };
+      targets = ["zc706" "coraz7" "redpitaya" "kasli_soc" "ebaz4205"];
+      allTargetCrates = (builtins.foldl' (results: target:
+        results // targetCrates target
+      ) {} targets);
+      
+      szl = pkgs.runCommand "szl" {} (builtins.foldl' (commands: target:
+        let
+          szlResult = builtins.getAttr "${target}-szl" allTargetCrates;
+        in
+          commands + "ln -s ${szlResult}/szl.elf $out/szl-${target}.elf\n"
+      ) "mkdir $out\n" targets);
+    in rec {
+      packages.x86_64-linux = { 
+        inherit cargo-xbuild szl mkbootimage;
+        zc706-fsbl = fsbl { board = "zc706"; };
+      } // allTargetCrates ;
+
+      hydraJobs = packages.x86_64-linux;
+
+      inherit rust rustPlatform;
+
+      devShell.x86_64-linux = pkgs.mkShell {
+        name = "zynq-rs-dev-shell";
+        buildInputs = [
+          rust
+          cargo-xbuild
+          mkbootimage
+
+          pkgs.openocd pkgs.gdb
+          pkgs.openssh pkgs.rsync
+          pkgs.llvmPackages_13.clang-unwrapped
+          (pkgs.python3.withPackages(ps: [ ps.pyftdi ]))
+        ];
+      };
+    };
+}
--- a/kasli_soc_por.py
+++ b/kasli_soc_por.py
@ -0,0 +1,17 @@
+from time import sleep
+from pyftdi.ftdi import Ftdi
+
+POR = 1 << 7
+
+def main():
+    dev = Ftdi()
+    dev.open_bitbang_from_url("ftdi://ftdi:4232h/0")
+    dev.set_bitmode(POR, Ftdi.BitMode.BITBANG)
+    dev.write_data(bytes([0]))
+    sleep(0.1)
+    dev.write_data(bytes([POR]))
+    sleep(0.1)
+    dev.close()
+
+if __name__ == "__main__":
+    main()
--- a/libasync/Cargo.toml
+++ b/libasync/Cargo.toml
@ -2,17 +2,17 @@
 name = "libasync"
 description = "low-level async support"
 version = "0.0.0"
-authors = ["Astro <astro@spaceboyz.net>"]
+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"
+nb = "1.0"
 libcortex_a9 = { path = "../libcortex_a9" }

 [dependencies.smoltcp]
-version = "0.6"
+version = "0.7"
 default-features = false
 features = ["alloc"]
--- a/libasync/src/executor.rs
+++ b/libasync/src/executor.rs
@ -6,7 +6,7 @@ use core::{
    sync::atomic::{AtomicBool, Ordering},
    task::{Context, Poll, RawWaker, RawWakerVTable, Waker},
 };
-use alloc::{boxed::Box, collections::VecDeque as Deque};
+use alloc::{boxed::Box, vec::Vec};
 //use futures::future::FutureExt;
 use pin_utils::pin_mut;

@ -31,7 +31,7 @@ static VTABLE: RawWakerVTable = {
 /// 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)) }
+    unsafe { Waker::from_raw(RawWaker::new(ready as *const _ as *const (), &VTABLE)) }
 }

 /// A single-threaded executor
@ -44,7 +44,7 @@ pub struct Executor {
    /// 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<Deque<Pin<Box<Task>>>>,
+    tasks: RefCell<Vec<Pin<Box<Task>>>>,
 }

 impl Executor {
@ -52,7 +52,7 @@ impl Executor {
    pub fn new() -> Self {
        Self {
            in_block_on: RefCell::new(false),
-            tasks: RefCell::new(Deque::new()),
+            tasks: RefCell::new(Vec::new()),
        }
    }

@ -72,6 +72,7 @@ impl Executor {
        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) {
@ -85,10 +86,9 @@ impl Executor {
                // println!("ran block_on");
            }

-            // println!("tasks: {}", self.tasks.borrow().len());
-            // advance other tasks
-            let next_task = self.tasks.borrow_mut().pop_front();
-            if let Some(mut task) = next_task {
+            // 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
@ -106,7 +106,7 @@ impl Executor {
                    }
                }
                // Requeue
-                self.tasks.borrow_mut().push_back(task);
+                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
@ -119,7 +119,7 @@ impl Executor {

    pub fn spawn(&self, f: impl Future + 'static) {
        let task = Box::pin(Task::new(f));
-        self.tasks.borrow_mut().push_back(task);
+        self.tasks.borrow_mut().push(task);
    }
 }

--- a/libasync/src/smoltcp/mod.rs
+++ b/libasync/src/smoltcp/mod.rs
@ -7,17 +7,23 @@ use smoltcp::{
    iface::EthernetInterface,
    phy::Device,
    socket::SocketSet,
-    time::Instant,
+    time::{Duration, Instant},
 };
 use crate::task;

 mod tcp_stream;
 pub use tcp_stream::TcpStream;

+pub trait LinkCheck {
+    type Link;
+    fn is_idle(&self) -> bool;
+    fn check_link_change(&mut self) -> Option<Self::Link>;
+}
+
 static mut SOCKETS: Option<Sockets> = None;

 pub struct Sockets {
-    sockets: RefCell<SocketSet<'static, 'static, 'static>>,
+    sockets: RefCell<SocketSet<'static>>,
    wakers: RefCell<Vec<Waker>>,
 }

@ -30,7 +36,7 @@ impl Sockets {
        let sockets = RefCell::new(SocketSet::new(sockets_storage));

        let wakers = RefCell::new(Vec::new());
-        
+
        let instance = Sockets {
            sockets,
            wakers,
@ -41,14 +47,24 @@ impl Sockets {

    /// 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>,
+    pub fn run<'b, D: for<'d> Device<'d> + LinkCheck>(
+        iface: &mut EthernetInterface<'b, D>,
        mut get_time: impl FnMut() -> Instant,
    ) -> ! {
        task::block_on(async {
+            let mut last_link_check = Instant::from_millis(0);
+            const LINK_CHECK_INTERVAL: u64 = 500;
+
            loop {
                let instant = get_time();
                Self::instance().poll(iface, instant);
+
+                let dev = iface.device_mut();
+                if dev.is_idle() && instant >= last_link_check + Duration::from_millis(LINK_CHECK_INTERVAL) {
+                    dev.check_link_change();
+                    last_link_check = instant;
+                }
+
                task::r#yield().await;
            }
        })
@ -57,10 +73,10 @@ impl Sockets {
    pub(crate) fn instance() -> &'static Self {
        unsafe { SOCKETS.as_ref().expect("Sockets") }
    }
-    
-    fn poll<'b, 'c, 'e, D: for<'d> Device<'d>>(
+
+    fn poll<'b, D: for<'d> Device<'d>>(
        &self,
-        iface: &mut EthernetInterface<'b, 'c, 'e, D>,
+        iface: &mut EthernetInterface<'b, D>,
        instant: Instant
    ) {
        let processed = {
@ -81,7 +97,14 @@ impl Sockets {
    /// 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) {
-        Self::instance().wakers.borrow_mut()
-            .push(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
@ -103,21 +103,19 @@ impl TcpStream {

    /// Probe the receive buffer
    ///
-    /// Instead of handing you the data on the heap all at once,
-    /// smoltcp's read interface is wrapped so that your callback can
-    /// just return `Poll::Pending` if there is not enough data
-    /// yet. Likewise, return the amount of bytes consumed from the
-    /// buffer in the `Poll::Ready` result.
+    /// 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]) -> Poll<(usize, R)>,
+        F: Fn(&[u8]) -> (usize, R),
    {
-        struct Recv<'a, F: FnOnce(&[u8]) -> Poll<(usize, R)>, R> {
+        struct Recv<'a, F: FnOnce(&[u8]) -> (usize, R), R> {
            stream: &'a TcpStream,
            f: F,
        }

-        impl<'a, F: Fn(&[u8]) -> Poll<(usize, R)>, R> Future for Recv<'a, F, R> {
+        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> {
@ -128,13 +126,9 @@ impl TcpStream {

                    socket.recv(|buf| {
                        if buf.len() > 0 {
-                            match (self.f)(buf) {
-                                Poll::Ready((amount, result)) =>
-                                    (amount, Poll::Ready(Ok(result))),
-                                Poll::Pending =>
-                                    // 0 bytes consumed
-                                    (0, Poll::Pending),
-                            }
+                            let (amount, result) = (self.f)(buf);
+                            assert!(amount > 0);
+                            (amount, Poll::Ready(Ok(result)))
                        } else {
                            (0, Poll::Pending)
                        }
@ -268,6 +262,14 @@ impl TcpStream {
    pub fn set_timeout(&mut self, duration: Option<Duration>) {
        self.with_socket(|mut socket| socket.set_timeout(duration));
    }
+
+    pub fn ack_delay(&self) -> Option<Duration> {
+        self.with_socket(|socket| socket.ack_delay())
+    }
+
+    pub fn set_ack_delay(&mut self, duration: Option<Duration>) {
+        self.with_socket(|mut socket| socket.set_ack_delay(duration));
+    }
 }

 impl Drop for TcpStream {
--- a/libboard_zynq/Cargo.toml
+++ b/libboard_zynq/Cargo.toml
@ -2,12 +2,16 @@
 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_coraz7 = []
+target_ebaz4205 = []
+target_redpitaya = []
+target_kasli_soc = []
+ipv6 = [ "smoltcp/proto-ipv6" ]

 [dependencies]
 volatile-register = "0.2"
@ -18,9 +22,9 @@ void = { version = "1", default-features = false }
 log = "0.4"
 libregister = { path = "../libregister" }
 libcortex_a9 = { path = "../libcortex_a9" }
+libasync = { path = "../libasync" }

 [dependencies.smoltcp]
-version = "0.6"
-# features = ["ethernet", "proto-ipv4", "socket-tcp", "log"]
+version = "0.7"
 features = ["ethernet", "proto-ipv4", "socket-tcp"]
 default-features = false
--- a/libboard_zynq/src/clocks/mod.rs
+++ b/libboard_zynq/src/clocks/mod.rs
@ -1,3 +1,5 @@
+use core::unimplemented;
+
 use libregister::{RegisterR, RegisterRW};
 use super::slcr;
 pub use slcr::ArmPllSource;
@ -14,7 +16,7 @@ enum CpuClockMode {

 impl CpuClockMode {
    pub fn get() -> Self {
-        let regs = slcr::RegisterBlock::new();
+        let regs = slcr::RegisterBlock::slcr();
        if regs.clk_621_true.read().clk_621_true() {
            CpuClockMode::C621
        } else {
@ -59,7 +61,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 +94,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 =>
@ -101,12 +103,14 @@ impl Clocks {
                self.ddr,
            slcr::PllSource::IoPll =>
                self.io,
+            slcr::PllSource::Emio =>
+                unimplemented!(),
        };
        pll / u32::from(uart_clk_ctrl.divisor())
    }

    pub fn sdio_ref_clk(&self) -> u32 {
-        let regs = slcr::RegisterBlock::new();
+        let regs = slcr::RegisterBlock::slcr();
        let sdio_clk_ctrl = regs.sdio_clk_ctrl.read();
        let pll = match sdio_clk_ctrl.srcsel() {
            slcr::PllSource::ArmPll =>
@ -115,6 +119,8 @@ impl Clocks {
                self.ddr,
            slcr::PllSource::IoPll =>
                self.io,
+            slcr::PllSource::Emio =>
+                unimplemented!(),
        };
        pll / u32::from(sdio_clk_ctrl.divisor())
    }
--- a/libboard_zynq/src/clocks/source.rs
+++ b/libboard_zynq/src/clocks/source.rs
@ -4,8 +4,14 @@ use super::slcr;

 #[cfg(feature = "target_zc706")]
 pub const PS_CLK: u32 = 33_333_333;
-#[cfg(feature = "target_cora_z7_10")]
+#[cfg(feature = "target_coraz7")]
 pub const PS_CLK: u32 = 50_000_000;
+#[cfg(feature = "target_ebaz4205")]
+pub const PS_CLK: u32 = 33_333_333;
+#[cfg(feature = "target_redpitaya")]
+pub const PS_CLK: u32 = 33_333_333;
+#[cfg(feature = "target_kasli_soc")]
+pub const PS_CLK: u32 = 33_333_333;

 /// (pll_fdiv_max, (pll_cp, pll_res, lock_cnt))
 const PLL_FDIV_LOCK_PARAM: &[(u16, (u8, u8, u16))] = &[
@ -44,7 +50,7 @@ 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
    }
@ -55,7 +61,7 @@ pub trait ClockSource {
    /// 25.10.4 PLLs
    fn setup(target_freq: u32) {
        let fdiv = (target_freq / PS_CLK).min(66) as u16;
-        let (pll_res, pll_cp, lock_cnt) = PLL_FDIV_LOCK_PARAM.iter()
+        let (pll_cp, pll_res, lock_cnt) = PLL_FDIV_LOCK_PARAM.iter()
            .filter(|(fdiv_max, _)| fdiv <= *fdiv_max)
            .nth(0)
            .expect("PLL_FDIV_LOCK_PARAM")
--- a/libboard_zynq/src/ddr/mod.rs
+++ b/libboard_zynq/src/ddr/mod.rs
@ -1,7 +1,9 @@
 use libregister::{RegisterR, RegisterW, RegisterRW};
 use log::{debug, info, error};
 use crate::{print, println};
-use super::slcr::{self, DdriobVrefSel};
+use super::slcr;
+#[cfg(feature = "target_zc706")]
+use super::slcr::DdriobVrefSel;
 use super::clocks::{Clocks, source::{DdrPll, ClockSource}};

 mod regs;
@ -10,26 +12,36 @@ mod regs;
 /// Micron MT41J256M8HX-15E: 667 MHz DDR3
 const DDR_FREQ: u32 = 666_666_666;

-#[cfg(feature = "target_cora_z7_10")]
+#[cfg(feature = "target_coraz7")]
 /// Micron MT41K256M16HA-125: 800 MHz DDR3L, max supported 533 MHz
 const DDR_FREQ: u32 = 525_000_000;

-/// MT41K256M16HA-125
-const DCI_FREQ: u32 = 10_000_000;
+#[cfg(feature = "target_ebaz4205")]
+/// EtronTech Memory EM6GD16EWKG-12H: 800 MHz DDR3 at 533 MHz
+const DDR_FREQ: u32 = 533_333_333;
+
+#[cfg(feature = "target_redpitaya")]
+/// Alliance Memory AS4C256M16D3B: 800 MHz DDR3 at 533 MHz
+const DDR_FREQ: u32 = 533_333_333;
+
+#[cfg(feature = "target_kasli_soc")]
+/// MT41K256M16HA-125:E: 800 MHz DDR3L at 533 MHz
+const DDR_FREQ: u32 = 533_333_333;
+
+const DCI_MAX_FREQ: u32 = 10_000_000;

 pub struct DdrRam {
    regs: &'static mut regs::RegisterBlock,
 }

 impl DdrRam {
-    pub fn 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() };
+        Self::calibrate_iob_impedance(&clocks);
+        let regs = regs::RegisterBlock::ddrc();
        let mut ddr = DdrRam { regs };
-        ddr.reset_ddrc();
+        ddr.reset_ddrc(|ddr| ddr.configure());
        ddr
    }

@ -55,14 +67,35 @@ impl DdrRam {
        clocks
    }

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

        slcr::RegisterBlock::unlocked(|slcr| {
            // Step 1.
@ -118,13 +151,23 @@ impl DdrRam {
                .output_en(slcr::DdriobOutputEn::Obuf);
            #[cfg(feature = "target_zc706")]
            let data1_config = data0_config.clone();
-            #[cfg(feature = "target_cora_z7_10")]
+            #[cfg(any(
+                feature = "target_coraz7",
+                feature = "target_ebaz4205",
+                feature = "target_redpitaya",
+                feature = "target_kasli_soc",
+            ))]
            let data0_config = slcr::DdriobConfig::zeroed()
                .inp_type(slcr::DdriobInputType::VrefDifferential)
                .term_en(true)
                .dci_type(slcr::DdriobDciType::Termination)
                .output_en(slcr::DdriobOutputEn::Obuf);
-            #[cfg(feature = "target_cora_z7_10")]
+            #[cfg(any(
+                feature = "target_coraz7",
+                feature = "target_ebaz4205",
+                feature = "target_redpitaya",
+                feature = "target_kasli_soc",
+            ))]
            let data1_config = slcr::DdriobConfig::zeroed()
                .pullup_en(true);
            slcr.ddriob_data0.write(data0_config);
@ -138,16 +181,25 @@ impl DdrRam {
                .output_en(slcr::DdriobOutputEn::Obuf);
            #[cfg(feature = "target_zc706")]
            let diff1_config = diff0_config.clone();
-            #[cfg(feature = "target_cora_z7_10")]
+            #[cfg(any(
+                feature = "target_coraz7",
+                feature = "target_ebaz4205",
+                feature = "target_redpitaya",
+                feature = "target_kasli_soc",
+            ))]
            let diff0_config = slcr::DdriobConfig::zeroed()
                .inp_type(slcr::DdriobInputType::Differential)
                .term_en(true)
                .dci_type(slcr::DdriobDciType::Termination)
                .output_en(slcr::DdriobOutputEn::Obuf);
-            #[cfg(feature = "target_cora_z7_10")]
+            #[cfg(any(
+                feature = "target_coraz7",
+                feature = "target_ebaz4205",
+                feature = "target_redpitaya",
+                feature = "target_kasli_soc",
+            ))]
            let diff1_config = slcr::DdriobConfig::zeroed()
                .pullup_en(true);
-
            slcr.ddriob_diff0.write(diff0_config);
            slcr.ddriob_diff1.write(diff1_config);

@ -164,12 +216,17 @@ impl DdrRam {
                slcr.ddriob_drive_slew_clock.write(0x00F9861C);
            }

-            // Enable external V[REF]
-            #[cfg(feature = "target_cora_z7_10")]
+            #[cfg(any(
+                feature = "target_coraz7",
+                feature = "target_ebaz4205",
+                feature = "target_redpitaya",
+                feature = "target_kasli_soc",
+            ))]
            slcr.ddriob_ddr_ctrl.modify(|_, w| w
                    .vref_int_en(false)
                    .vref_ext_en_lower(true)
                    .vref_ext_en_upper(false)
+                    .refio_en(true)
            );
            #[cfg(feature = "target_zc706")]
            slcr.ddriob_ddr_ctrl.modify(|_, w| w
@ -181,8 +238,200 @@ impl DdrRam {
        });
    }

+    fn configure(&mut self) {
+        #[cfg(any(feature = "target_coraz7", feature = "target_kasli_soc"))]
+        self.regs.dram_param0.write(
+            regs::DramParam0::zeroed()
+                .t_rc(0x1a)
+                .t_rfc_min(0x9e)
+                .post_selfref_gap_x32(0x10)
+        );
+        #[cfg(feature = "target_ebaz4205")]
+        self.regs.dram_param0.write(
+            regs::DramParam0::zeroed()
+                .t_rc(0x1a)
+                .t_rfc_min(0x56)
+                .post_selfref_gap_x32(0x10)
+        );
+        #[cfg(feature = "target_redpitaya")]
+        self.regs.dram_param0.write(
+            regs::DramParam0::zeroed()
+                .t_rc(0x1b)
+                .t_rfc_min(0xa0)
+                .post_selfref_gap_x32(0x10)
+        );
+        #[cfg(feature = "target_zc706")]
+        self.regs.dram_param0.write(
+            regs::DramParam0::zeroed()
+                .t_rc(0x1b)
+                .t_rfc_min(0x56)
+                .post_selfref_gap_x32(0x10)
+        );
+        #[cfg(feature = "target_ebaz4205")]
+        self.regs.dram_param1.modify(
+            |_, w| w
+                .t_faw(0x16)
+                .t_ras_min(0x13)
+        );
+        #[cfg(feature = "target_redpitaya")]
+        self.regs.dram_param1.modify(
+            |_, w| w
+                .wr2pre(0x12)
+                .powerdown_to_x32(6)
+                .t_faw(0x16)
+                .t_ras_max(0x24)
+                .t_ras_min(0x13)
+                .t_cke(4)
+        );
+
+        self.regs.dram_param2.write(
+            regs::DramParam2::zeroed()
+                .write_latency(0x5)
+                .rd2wr(0x7)
+                .wr2rd(0xe)
+                .t_xp(0x4)
+                .pad_pd(0x0)
+                .rd2pre(0x4)
+                .t_rcd(0x7)
+        );
+        #[cfg(feature = "target_ebaz4205")]
+        self.regs.dram_param3.modify(
+            |_, w| w
+                .t_rp(7)
+        );
+        #[cfg(feature = "target_redpitaya")]
+        self.regs.dram_param3.modify(
+            |_, w| w
+                .t_ccd(4)
+                .t_rrd(6)
+                .refresh_margin(2)
+                .t_rp(7)
+                .refresh_to_x32(8)
+                .mobile(false)
+                .dfi_dram_clk_disable(false)
+                .read_latency(7)
+                .mode_ddr1_ddr2(true)
+                .dis_pad_pd(false)
+        );
+
+        self.regs.dram_emr_mr.write(
+            regs::DramEmrMr::zeroed()
+                .mr(0x930)
+                .emr(0x4)
+        );
+
+        #[cfg(any(
+            feature = "target_coraz7",
+            feature = "target_ebaz4205",
+            feature = "target_redpitaya",
+            feature = "target_kasli_soc",
+        ))]
+        self.regs.phy_configs[2].modify(
+            |_, w| w.data_slice_in_use(false)
+        );
+        #[cfg(any(
+            feature = "target_coraz7",
+            feature = "target_ebaz4205",
+            feature = "target_redpitaya",
+            feature = "target_kasli_soc",
+        ))]
+        self.regs.phy_configs[3].modify(
+            |_, w| w.data_slice_in_use(false)
+        );
+
+        self.regs.phy_cmd_timeout_rddata_cpt.modify(
+            |_, w| w
+                .rd_cmd_to_data(0x0)
+                .wr_cmd_to_data(0x0)
+                .we_to_re_delay(0x8)
+                .rdc_fifo_rst_disable(false)
+                .use_fixed_re(true)
+                .rdc_fifo_rst_err_cnt_clr(false)
+                .dis_phy_ctrl_rstn(false)
+                .clk_stall_level(false)
+                .gatelvl_num_of_dq0(0x7)
+                .wrlvl_num_of_dq0(0x7)
+        );
+
+        self.regs.reg_2c.write(
+            regs::Reg2C::zeroed()
+                .wrlvl_max_x1024(0xfff)
+                .rdlvl_max_x1024(0xfff)
+                .twrlvl_max_error(false)
+                .trdlvl_max_error(false)
+                .dfi_wr_level_en(true)
+                .dfi_rd_dqs_gate_level(true)
+                .dfi_rd_data_eye_train(true)
+        );
+
+        self.regs.dfi_timing.write(
+            regs::DfiTiming::zeroed()
+                .rddata_en(0x6)
+                .ctrlup_min(0x3)
+                .ctrlup_max(0x40)
+        );
+
+        #[cfg(feature = "target_zc706")]
+        self.regs.phy_init_ratios[3].write(
+            regs::PhyInitRatio::zeroed()
+                .wrlvl_init_ratio(0x21)
+                .gatelvl_init_ratio(0xee)
+        );
+
+        #[cfg(any(
+            feature = "target_coraz7",
+            feature = "target_ebaz4205",
+            feature = "target_kasli_soc"),
+        )]
+        self.regs.reg_64.modify(
+            |_, w| w
+                .phy_ctrl_slave_ratio(0x100)
+                .phy_invert_clkout(true)
+        );
+        #[cfg(feature = "target_redpitaya")]
+        self.regs.reg_64.modify(
+            |_, w| w
+                .phy_bl2(false)
+                .phy_invert_clkout(true)
+                .phy_sel_logic(false)
+                .phy_ctrl_slave_ratio(0x100)
+                .phy_ctrl_slave_force(false)
+                .phy_ctrl_slave_delay(0)
+                .phy_lpddr(false)
+                .phy_cmd_latency(false)
+        );
+
+        self.regs.reg_65.write(
+            regs::Reg65::zeroed()
+                .wr_rl_delay(0x2)
+                .rd_rl_delay(0x4)
+                .dll_lock_diff(0xf)
+                .use_wr_level(true)
+                .use_rd_dqs_gate_level(true)
+                .use_rd_data_eye_level(true)
+                .dis_calib_rst(false)
+                .ctrl_slave_delay(0x0)
+        );
+    }
+
    /// Reset DDR controller
-    fn reset_ddrc(&mut self) {
+    fn reset_ddrc<F: FnMut(&mut Self)>(&mut self, mut f: F) {
+        #[cfg(feature = "target_zc706")]
+        let width = regs::DataBusWidth::Width32bit;
+        #[cfg(any(
+            feature = "target_coraz7",
+            feature = "target_ebaz4205",
+            feature = "target_redpitaya",
+            feature = "target_kasli_soc",
+        ))]
+        let width = regs::DataBusWidth::Width16bit;
+        self.regs.ddrc_ctrl.modify(|_, w| w
+            .soft_rstb(false)
+            .powerdown_en(false)
+            .data_bus_width(width)
+        );
+        f(self);
+
        #[cfg(feature = "target_zc706")]
        unsafe {
            // row/column address bits
@ -190,16 +439,19 @@ impl DdrRam {
            self.regs.dram_addr_map_col.write(0xFFF00000);
            self.regs.dram_addr_map_row.write(0x0F666666);
        }
+        #[cfg(any(
+            feature = "target_coraz7",
+            feature = "target_ebaz4205",
+            feature = "target_redpitaya",
+            feature = "target_kasli_soc",
+        ))]
+        unsafe {
+            // row/column address bits
+            self.regs.dram_addr_map_bank.write(0x00000666);
+            self.regs.dram_addr_map_col.write(0xFFFF0000);
+            self.regs.dram_addr_map_row.write(0x0F555555);
+        }

-        #[cfg(feature = "target_zc706")]
-        let width = regs::DataBusWidth::Width32bit;
-        #[cfg(feature = "target_cora_z7_10")]
-        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)
@ -210,7 +462,7 @@ 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 {
@ -220,10 +472,18 @@ impl DdrRam {
    /// actually there's 1 MB more but starting at 0x0000_0000
    /// overlaps with OCM.
    pub fn size(&self) -> usize {
+        // DDR range ends at 0x3FFF_FFFF in the default SCU address
+        // filtering address map
        #[cfg(feature = "target_zc706")]
        let megabytes = 1023;
-        #[cfg(feature = "target_cora_z7_10")]
-        let megabytes = 511;
+        #[cfg(any(
+            feature = "target_coraz7",
+            feature = "target_redpitaya",
+            feature = "target_kasli_soc",
+        ))]
+        let megabytes = 512;
+        #[cfg(feature = "target_ebaz4205")]
+        let megabytes = 256;

        megabytes * 1024 * 1024
    }
@ -239,7 +499,7 @@ impl DdrRam {

            for megabyte in 0..slice.len() / (1024 * 1024) {
                let start = megabyte * 1024 * 1024 / 4;
-                let end = ((megabyte + 1) * 1024 * 1024 / 4);
+                let end = (megabyte + 1) * 1024 * 1024 / 4;
                for b in slice[start..end].iter_mut() {
                    expected.map(|expected| {
                        let read: u32 = *b;
--- a/libboard_zynq/src/ddr/regs.rs
+++ b/libboard_zynq/src/ddr/regs.rs
@ -1,6 +1,6 @@
 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)]
@ -29,70 +29,64 @@ 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: DramParam1,
+    pub dram_param2: DramParam2,
+    pub dram_param3: DramParam3,
+    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_burst8_rdwr: RW<u32>,
+    pub dram_emr: RW<u32>,
+    pub dram_emr_mr: DramEmrMr,
+    pub dram_burst8_rdwr: Burst8Rdwr,
    pub dram_disable_dq: RW<u32>,
    pub dram_addr_map_bank: RW<u32>,
    pub dram_addr_map_col: RW<u32>,
    pub dram_addr_map_row: RW<u32>,
-    pub dram_odt_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: CheTZq,
+    pub che_t_zq_short_interval: RW<u32>,
+    pub deep_pwrdwn: RW<u32>,
+    pub reg_2c: Reg2C,
    pub reg_2d: RW<u32>,
-    pub dfi_timing: 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>,
-    pub phy_config1: RW<u32>,
-    pub phy_config2: RW<u32>,
-    pub phy_config3: RW<u32>,
+    pub phy_configs: [PhyConfig; 4],
    _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_ratios: [PhyInitRatio; 4],
    _unused5: RO<u32>,
    pub phy_rd_dqs_cfg0: RW<u32>,
    pub phy_rd_dqs_cfg1: RW<u32>,
@ -114,8 +108,8 @@ pub struct RegisterBlock {
    pub wr_data_slv2: RW<u32>,
    pub wr_data_slv3: RW<u32>,
    _unused9: RO<u32>,
-    pub reg_64: RW<u32>,
-    pub reg_65: RW<u32>,
+    pub reg_64: Reg64,
+    pub reg_65: Reg65,
    _unused10: [RO<u32>; 3],
    pub reg69_6a0: RW<u32>,
    pub reg69_6a1: RW<u32>,
@ -134,18 +128,12 @@ 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>,
-    pub axi_priority_wr_port0: RW<u32>,
-    pub axi_priority_wr_port1: RW<u32>,
-    pub axi_priority_wr_port2: RW<u32>,
-    pub axi_priority_wr_port3: RW<u32>,
-    pub axi_priority_rd_port0: RW<u32>,
-    pub axi_priority_rd_port1: RW<u32>,
-    pub axi_priority_rd_port2: RW<u32>,
-    pub axi_priority_rd_port3: RW<u32>,
+    pub axi_priority_wr_ports: [RW<u32>; 4],
+    pub axi_priority_rd_ports: [AxiPriorityRd; 4],
    _unused15: [RO<u32>; 27],
    pub excl_access_cfg0: RW<u32>,
    pub excl_access_cfg1: RW<u32>,
@ -158,11 +146,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,
@ -172,8 +156,124 @@ register_bit!(ddrc_ctrl, powerdown_en, 1);
 register_bits_typed!(ddrc_ctrl, data_bus_width, u8, DataBusWidth, 2, 3);
 // (ddrc_ctrl) ...

+register!(dram_param0, DramParam0, RW, u32);
+register_bits!(dram_param0, t_rc, u8, 0, 5);
+register_bits!(dram_param0, t_rfc_min, u8, 6, 13);
+register_bits!(dram_param0, post_selfref_gap_x32, u8, 14, 20);
+
+register!(dram_param1, DramParam1, RW, u32);
+register_bits!(dram_param1, wr2pre, u8, 0, 4);
+register_bits!(dram_param1, powerdown_to_x32, u8, 5, 9);
+register_bits!(dram_param1, t_faw, u8, 10, 15);
+register_bits!(dram_param1, t_ras_max, u8, 16, 21);
+register_bits!(dram_param1, t_ras_min, u8, 22, 26);
+register_bits!(dram_param1, t_cke, u8, 28, 31);
+
+register!(dram_param2, DramParam2, RW, u32);
+register_bits!(dram_param2, write_latency, u8, 0, 4);
+register_bits!(dram_param2, rd2wr, u8, 5, 9);
+register_bits!(dram_param2, wr2rd, u8, 10, 14);
+register_bits!(dram_param2, t_xp, u8, 15, 19);
+register_bits!(dram_param2, pad_pd, u8, 20, 22);
+register_bits!(dram_param2, rd2pre, u8, 23, 27);
+register_bits!(dram_param2, t_rcd, u8, 28, 31);
+
+register!(dram_param3, DramParam3, RW, u32);
+register_bits!(dram_param3, t_ccd, u8, 2, 4);
+register_bits!(dram_param3, t_rrd, u8, 5, 7);
+register_bits!(dram_param3, refresh_margin, u8, 8, 11);
+register_bits!(dram_param3, t_rp, u8, 12, 15);
+register_bits!(dram_param3, refresh_to_x32, u8, 16, 20);
+register_bit!(dram_param3, sdram, 21);
+register_bit!(dram_param3, mobile, 22);
+register_bit!(dram_param3, dfi_dram_clk_disable, 23);
+register_bits!(dram_param3, read_latency, u8, 24, 28);
+register_bit!(dram_param3, mode_ddr1_ddr2, 29);
+register_bit!(dram_param3, dis_pad_pd, 30);
+
+register!(dram_emr_mr, DramEmrMr, RW, u32);
+register_bits!(dram_emr_mr, mr, u16, 0, 15);
+register_bits!(dram_emr_mr, emr, u16, 16, 31);
+
+register!(burst8_rdwr, Burst8Rdwr, RW, u32);
+register_bits!(burst8_rdwr, burst_rdwr, u8, 0, 3);
+register_bits!(burst8_rdwr, pre_cke_x1024, u16, 4, 13);
+register_bits!(burst8_rdwr, post_cke_x1024, u16, 16, 25);
+register_bit!(burst8_rdwr, burstchop, 28);
+
+register!(phy_cmd_timeout_rddata_cpt, PhyCmdTimeoutRddataCpt, RW, u32);
+register_bits!(phy_cmd_timeout_rddata_cpt, rd_cmd_to_data, u8, 0, 3);
+register_bits!(phy_cmd_timeout_rddata_cpt, wr_cmd_to_data, u8, 4, 7);
+register_bits!(phy_cmd_timeout_rddata_cpt, we_to_re_delay, u8, 8, 11);
+register_bit!(phy_cmd_timeout_rddata_cpt, rdc_fifo_rst_disable, 15);
+register_bit!(phy_cmd_timeout_rddata_cpt, use_fixed_re, 16);
+register_bit!(phy_cmd_timeout_rddata_cpt, rdc_fifo_rst_err_cnt_clr, 17);
+register_bit!(phy_cmd_timeout_rddata_cpt, dis_phy_ctrl_rstn, 18);
+register_bit!(phy_cmd_timeout_rddata_cpt, clk_stall_level, 19);
+register_bits!(phy_cmd_timeout_rddata_cpt, gatelvl_num_of_dq0, u8, 24, 27);
+register_bits!(phy_cmd_timeout_rddata_cpt, wrlvl_num_of_dq0, u8, 28, 31);
+
+register!(che_t_zq, CheTZq, RW, u32);
+register_bit!(che_t_zq, dis_auto_zq, 0);
+register_bit!(che_t_zq, ddr3, 1);
+register_bits!(che_t_zq, t_mod, u8, 2, 11);
+register_bits!(che_t_zq, t_zq_long_nop, u16, 12, 21);
+register_bits!(che_t_zq, t_zq_short_nop, u16, 22, 31);
+
+register!(reg_2c, Reg2C, RW, u32);
+register_bits!(reg_2c, wrlvl_max_x1024, u16, 0, 11);
+register_bits!(reg_2c, rdlvl_max_x1024, u16, 12, 23);
+register_bit!(reg_2c, twrlvl_max_error, 24);
+register_bit!(reg_2c, trdlvl_max_error, 25);
+register_bit!(reg_2c, dfi_wr_level_en, 26);
+register_bit!(reg_2c, dfi_rd_dqs_gate_level, 27);
+register_bit!(reg_2c, dfi_rd_data_eye_train, 28);
+
+register!(dfi_timing, DfiTiming, RW, u32);
+register_bits!(dfi_timing, rddata_en, u8, 0, 4);
+register_bits!(dfi_timing, ctrlup_min, u16, 5, 14);
+register_bits!(dfi_timing, ctrlup_max, u16, 15, 24);
+
+register!(phy_config, PhyConfig, RW, u32);
+register_bit!(phy_config, data_slice_in_use, 0);
+register_bit!(phy_config, rdlvl_inc_mode, 1);
+register_bit!(phy_config, gatelvl_inc_mode, 2);
+register_bit!(phy_config, wrlvl_inc_mode, 3);
+register_bits!(phy_config, dq_offset, u8, 24, 30);
+
+register!(phy_init_ratio, PhyInitRatio, RW, u32);
+register_bits!(phy_init_ratio, wrlvl_init_ratio, u16, 0, 9);
+register_bits!(phy_init_ratio, gatelvl_init_ratio, u16, 10, 19);
+
+register!(reg_64, Reg64, RW, u32);
+register_bit!(reg_64, phy_bl2, 1);
+register_bit!(reg_64, phy_invert_clkout, 7);
+register_bit!(reg_64, phy_sel_logic, 9);
+register_bits!(reg_64, phy_ctrl_slave_ratio, u16, 10, 19);
+register_bit!(reg_64, phy_ctrl_slave_force, 20);
+register_bits!(reg_64, phy_ctrl_slave_delay, u8, 21, 27);
+register_bit!(reg_64, phy_lpddr, 29);
+register_bit!(reg_64, phy_cmd_latency, 30);
+
+register!(reg_65, Reg65, RW, u32);
+register_bits!(reg_65, wr_rl_delay, u8, 0, 4);
+register_bits!(reg_65, rd_rl_delay, u8, 5, 9);
+register_bits!(reg_65, dll_lock_diff, u8, 10, 13);
+register_bit!(reg_65, use_wr_level, 14);
+register_bit!(reg_65, use_rd_dqs_gate_level, 15);
+register_bit!(reg_65, use_rd_data_eye_level, 16);
+register_bit!(reg_65, dis_calib_rst, 17);
+register_bits!(reg_65, ctrl_slave_delay, u8, 18, 19);
+
 // Controller operation mode status
 register!(mode_sts_reg,
          ModeStsReg, RO, u32);
 register_bits_typed!(mode_sts_reg, operating_mode, u8, ControllerStatus, 0, 2);
 // (mode_sts_reg) ...
+
+register!(axi_priority_rd, AxiPriorityRd, RW, u32);
+register_bits!(axi_priority_rd, arb_pri_rd_portn, u16, 0, 9);
+register_bit!(axi_priority_rd, arb_disable_aging_rd_portn, 16);
+register_bit!(axi_priority_rd, arb_disable_urgent_rd_portn, 17);
+register_bit!(axi_priority_rd, arb_disable_page_match_rd_portn, 18);
+register_bit!(axi_priority_rd, arb_set_hpr_rd_portn, 19);
--- a/libboard_zynq/src/devc/mod.rs
+++ b/libboard_zynq/src/devc/mod.rs
@ -1,4 +1,3 @@
-use super::clocks::Clocks;
 use super::time::Milliseconds;
 use crate::slcr;
 use embedded_hal::timer::CountDown;
@ -11,7 +10,7 @@ mod regs;
 pub struct DevC {
    regs: &'static mut regs::RegisterBlock,
    enabled: bool,
-    count_down: super::timer::global::CountDown,
+    count_down: super::timer::global::CountDown<Milliseconds>,
    timeout_ms: Milliseconds,
 }

--- a/libboard_zynq/src/dmac/mod.rs
+++ b/libboard_zynq/src/dmac/mod.rs
@ -1,3 +0,0 @@
-//! PrimeCell DMA Controller (PL330)
-
-mod regs;
--- a/libboard_zynq/src/dmac/regs.rs
+++ b/libboard_zynq/src/dmac/regs.rs
@ -1,386 +0,0 @@
-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 ftc: [Ftc; 8],
-    pub cs0: Cs,
-    pub cpc0: Cpc,
-    pub cs1: Cs,
-    pub cpc1: Cpc,
-    pub cs2: Cs,
-    pub cpc2: Cpc,
-    pub cs3: Cs,
-    pub cpc3: Cpc,
-    pub cs4: Cs,
-    pub cpc4: Cpc,
-    pub cs5: Cs,
-    pub cpc5: Cpc,
-    pub cs6: Cs,
-    pub cpc6: Cpc,
-    pub cs7: Cs,
-    pub cpc7: Cpc,
-    pub sa0: Sa,
-    pub da0: Da,
-    pub cc0: Cc,
-    pub lc0_0: Lc,
-    pub lc0_1: Lc,
-    pub sa1: Sa,
-    pub da1: Da,
-    pub cc1: Cc,
-    pub lc1_0: Lc,
-    pub lc1_1: Lc,
-    pub sa2: Sa,
-    pub da2: Da,
-    pub cc2: Cc,
-    pub lc2_0: Lc,
-    pub lc2_1: Lc,
-    pub sa3: Sa,
-    pub da3: Da,
-    pub cc3: Cc,
-    pub lc3_0: Lc,
-    pub lc3_1: Lc,
-    pub sa4: Sa,
-    pub da4: Da,
-    pub cc4: Cc,
-    pub lc4_0: Lc,
-    pub lc4_1: Lc,
-    pub sa5: Sa,
-    pub da5: Da,
-    pub cc5: Cc,
-    pub lc5_0: Lc,
-    pub lc5_1: Lc,
-    pub sa6: Sa,
-    pub da6: Da,
-    pub cc6: Cc,
-    pub lc6_0: Lc,
-    pub lc6_1: Lc,
-    pub sa7: Sa,
-    pub da7: Da,
-    pub cc7: Cc,
-    pub lc7_0: Lc,
-    pub lc7_1: Lc,
-    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);
-
-impl RegisterBlock {
-    pub fn channel_regs(&mut self, channel: usize) -> Option<ChannelRegisters>
-    {
-        match channel {
-            0 => Some(ChannelRegisters {
-                ftc: &mut self.ftc[0],
-                cs: &mut self.cs0,
-                cpc: &mut self.cpc0,
-                sa: &mut self.sa0,
-                da: &mut self.da0,
-                cc: &mut self.cc0,
-                lc: [&mut self.lc0_0, &mut self.lc0_1],
-            }),
-            1 => Some(ChannelRegisters {
-                ftc: &mut self.ftc[1],
-                cs: &mut self.cs1,
-                cpc: &mut self.cpc1,
-                sa: &mut self.sa1,
-                da: &mut self.da1,
-                cc: &mut self.cc1,
-                lc: [&mut self.lc1_0, &mut self.lc1_1],
-            }),
-            2 => Some(ChannelRegisters {
-                ftc: &mut self.ftc[2],
-                cs: &mut self.cs2,
-                cpc: &mut self.cpc2,
-                sa: &mut self.sa2,
-                da: &mut self.da2,
-                cc: &mut self.cc2,
-                lc: [&mut self.lc2_0, &mut self.lc2_1],
-            }),
-            3 => Some(ChannelRegisters {
-                ftc: &mut self.ftc[3],
-                cs: &mut self.cs3,
-                cpc: &mut self.cpc3,
-                sa: &mut self.sa3,
-                da: &mut self.da3,
-                cc: &mut self.cc3,
-                lc: [&mut self.lc3_0, &mut self.lc3_1],
-            }),
-            4 => Some(ChannelRegisters {
-                ftc: &mut self.ftc[4],
-                cs: &mut self.cs4,
-                cpc: &mut self.cpc4,
-                sa: &mut self.sa4,
-                da: &mut self.da4,
-                cc: &mut self.cc4,
-                lc: [&mut self.lc4_0, &mut self.lc4_1],
-            }),
-            5 => Some(ChannelRegisters {
-                ftc: &mut self.ftc[5],
-                cs: &mut self.cs5,
-                cpc: &mut self.cpc5,
-                sa: &mut self.sa5,
-                da: &mut self.da5,
-                cc: &mut self.cc5,
-                lc: [&mut self.lc5_0, &mut self.lc5_1],
-            }),
-            6 => Some(ChannelRegisters {
-                ftc: &mut self.ftc[6],
-                cs: &mut self.cs6,
-                cpc: &mut self.cpc6,
-                sa: &mut self.sa6,
-                da: &mut self.da6,
-                cc: &mut self.cc6,
-                lc: [&mut self.lc6_0, &mut self.lc6_1],
-            }),
-            7 => Some(ChannelRegisters {
-                ftc: &mut self.ftc[7],
-                cs: &mut self.cs7,
-                cpc: &mut self.cpc7,
-                sa: &mut self.sa7,
-                da: &mut self.da7,
-                cc: &mut self.cc7,
-                lc: [&mut self.lc7_0, &mut self.lc7_1],
-            }),
-            _ => None,
-        }
-    }
-}
-
-pub struct ChannelRegisters<'a> {
-    ftc: &'a mut Ftc,
-    cs: &'a mut Cs,
-    cpc: &'a mut Cpc,
-    sa: &'a mut Sa,
-    da: &'a mut Da,
-    cc: &'a mut Cc,
-    lc: [&'a mut Lc; 2],
-}
-
-#[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!(ftc, Ftc, RW, u32);
-register_bit!(ftc, lockup_err, 31);
-register_bit!(ftc, dbg_instr, 30);
-register_bit!(ftc, data_read_err, 18);
-register_bit!(ftc, data_write_err, 17);
-register_bit!(ftc, instr_fetch_err, 16);
-register_bit!(ftc, st_data_unavailable, 13);
-register_bit!(ftc, mfifo_err, 12);
-register_bit!(ftc, ch_rdwr_err, 7);
-register_bit!(ftc, ch_periph_err, 6);
-register_bit!(ftc, ch_evnt_err, 5);
-register_bit!(ftc, operand_invalid, 1);
-register_bit!(ftc, undef_instr, 0);
-
-register!(cs, Cs, RW, u32);
-register_bit!(cs, cns, 21);
-register_bit!(cs, dmawfp_periph, 15);
-register_bit!(cs, dmawfp_b_ns, 14);
-register_bits!(cs, wakeup_num, u8, 4, 8);
-register_bits!(cs, channel_status, u8, 0, 3);
-
-register!(cpc, Cpc, RW, u32);
-register_bits!(cpc, pc_chnl, u8, 0, 31);
-
-register!(sa, Sa, RW, u32);
-register_bits!(sa, src_addr, u8, 0, 31);
-
-register!(da, Da, RW, u32);
-register_bits!(da, dest_addr, u8, 0, 31);
-
-register!(cc, Cc, RW, u32);
-register_bits!(cc, endian_swap_size, u8, 28, 30);
-register_bits!(cc, dst_cache_ctrl, u8, 25, 27);
-register_bits!(cc, dst_prot_ctrl, u8, 22, 24);
-register_bits!(cc, dst_burst_len, u8, 18, 21);
-register_bits!(cc, dst_burst_size, u8, 15, 17);
-register_bit!(cc, dst_inc, 14);
-register_bits!(cc, src_cache_ctrl, u8, 11, 13);
-register_bits!(cc, src_prot_ctrl, u8, 8, 10);
-register_bits!(cc, src_burst_len, u8, 4, 7);
-register_bits!(cc, src_burst_size, u8, 1, 3);
-register_bit!(cc, src_inc, 0);
-
-register!(lc0, Lc, RW, u32);
-register_bits!(lc0, 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/error_led.rs
+++ b/libboard_zynq/src/error_led.rs
@ -0,0 +1,114 @@
+use libregister::{RegisterRW, RegisterW};
+use libregister::{register, register_at, register_bit, register_bits};
+use super::slcr;
+
+pub struct ErrorLED {
+    regs: RegisterBlock,
+}
+
+impl ErrorLED {
+    #[cfg(feature = "target_kasli_soc")]
+    pub fn error_led() -> Self {
+        slcr::RegisterBlock::unlocked(|slcr| {
+            // Error LED at MIO pin 37
+            slcr.mio_pin_37.write(
+                slcr::MioPin37::zeroed()
+                    .l3_sel(0b000)
+                    .io_type(slcr::IoBufferType::Lvcmos25)
+                    .pullup(true)
+                    .disable_rcvr(true)
+            );
+        });
+
+        Self::error_led_common(0xFFFF - 0x0080)
+    }
+
+    fn error_led_common(gpio_output_mask: u16) -> Self {
+        // Setup register block
+        let self_ = Self {
+            regs: RegisterBlock::error_led(),
+        };
+
+        // Setup GPIO output mask
+        self_.regs.gpio_output_mask.modify(|_, w| {
+            w.mask(gpio_output_mask)
+        });
+
+        self_.regs.gpio_direction.modify(|_, w| {
+            w.lederr(true)
+        });
+
+        self_
+    }
+
+    fn led_oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+             w.lederr(oe)
+        })
+    }
+
+    fn led_o(&mut self, o: bool) {
+        self.regs.gpio_output_mask.modify(|_, w| {
+             w.lederr_o(o)
+        })
+    }
+
+    pub fn toggle(&mut self, state: bool) {
+        self.led_o(state);
+        self.led_oe(state);
+    }
+}
+
+
+pub struct RegisterBlock {
+    pub gpio_output_mask: &'static mut GPIOOutputMask,
+    pub gpio_direction: &'static mut GPIODirection,
+    pub gpio_output_enable: &'static mut GPIOOutputEnable,
+}
+
+impl RegisterBlock {
+    pub fn error_led() -> Self {
+        Self {
+            gpio_output_mask: GPIOOutputMask::new(),
+            gpio_direction: GPIODirection::new(),
+            gpio_output_enable: GPIOOutputEnable::new()
+        }
+    }
+}
+
+register!(gpio_output_mask,
+            /// MASK_DATA_1_LSW:
+            /// Maskable output data for MIO[47:32]
+            GPIOOutputMask, RW, u32);
+#[cfg(feature = "target_kasli_soc")]
+register_at!(GPIOOutputMask, 0xE000A008, new);
+#[cfg(feature = "target_kasli_soc")]
+register_bit!(gpio_output_mask,
+              /// Output for LED_ERR (MIO[37])
+              lederr_o, 5);
+#[cfg(feature = "target_kasli_soc")]
+register_bits!(gpio_output_mask,
+               mask, u16, 16, 31);
+
+register!(gpio_direction,
+          /// DIRM_1:
+          /// Direction mode for MIO[53:32]; 0/1 = in/out
+          GPIODirection, RW, u32);
+#[cfg(feature = "target_kasli_soc")]
+register_at!(GPIODirection, 0xE000A244, new);
+#[cfg(feature = "target_kasli_soc")]
+register_bit!(gpio_direction,
+              /// Direction for LED_ERR
+              lederr, 5);
+
+register!(gpio_output_enable,
+          /// OEN_1:
+          /// Output enable for MIO[53:32]
+          GPIOOutputEnable, RW, u32);
+#[cfg(feature = "target_kasli_soc")]
+register_at!(GPIOOutputEnable, 0xE000A248, new);
+#[cfg(feature = "target_kasli_soc")]
+register_bit!(gpio_output_enable,
+              /// Output enable for LED_ERR
+              lederr, 5);
+              
--- a/libboard_zynq/src/eth/mod.rs
+++ b/libboard_zynq/src/eth/mod.rs
@ -1,5 +1,8 @@
-use core::ops::{Deref, DerefMut};
-use log::{error, info, warn};
+use core::{
+    marker::PhantomData,
+    ops::{Deref, DerefMut},
+};
+use log::{debug, info, warn, error};
 use libregister::*;
 use super::slcr;
 use super::clocks::Clocks;
@ -10,6 +13,9 @@ mod regs;
 pub mod rx;
 pub mod tx;

+use super::time::Milliseconds;
+use embedded_hal::timer::CountDown;
+
 /// Size of all the buffers
 pub const MTU: usize = 1536;
 /// Maximum MDC clock
@ -24,7 +30,7 @@ const TX_1000: u32 = 125_000_000;
 pub struct Buffer(pub [u8; MTU]);

 impl Buffer {
-    pub fn new() -> Self {
+    pub const fn new() -> Self {
        Buffer([0; MTU])
    }
 }
@ -42,15 +48,127 @@ impl DerefMut for Buffer {
    }
 }

-pub struct Eth<'r, RX, TX> {
-    rx: RX,
-    tx: TX,
-    inner: EthInner<'r>,
-    phy: Phy,
+/// Gigabit Ethernet Peripheral
+pub trait Gem {
+    fn setup_clock(tx_clock: u32);
+    fn regs() -> &'static mut regs::RegisterBlock;
 }

-impl<'r> Eth<'r, (), ()> {
-    pub fn default(macaddr: [u8; 6]) -> Self {
+/// first Gigabit Ethernet peripheral
+pub struct Gem0;
+
+impl Gem for Gem0 {
+    fn setup_clock(tx_clock: u32) {
+        let (divisor0, divisor1) = calculate_tx_divisors(tx_clock);
+
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.gem0_clk_ctrl.write(
+                // 0x0050_0801: 8, 5: 100 Mb/s
+                // ...: 8, 1: 1000 Mb/s
+                #[cfg(not(feature = "target_ebaz4205"))]
+                slcr::GemClkCtrl::zeroed()
+                    .clkact(true)
+                    .srcsel(slcr::PllSource::IoPll)
+                    .divisor(divisor0 as u8)
+                    .divisor1(divisor1 as u8),
+                // ebaz4205 -- EMIO
+                #[cfg(feature = "target_ebaz4205")]
+                slcr::GemClkCtrl::zeroed()
+                    .clkact(true)
+                    .srcsel(slcr::PllSource::Emio)
+                    .divisor(divisor0 as u8)
+                    .divisor1(divisor1 as u8)
+            );
+            // Enable gem0 recv clock
+            slcr.gem0_rclk_ctrl.write(
+                // 0x0000_0801
+                #[cfg(not(feature = "target_ebaz4205"))]
+                slcr::RclkCtrl::zeroed()
+                    .clkact(true),
+                // ebaz4205 -- EMIO
+                #[cfg(feature = "target_ebaz4205")]
+                slcr::RclkCtrl::zeroed()
+                    .clkact(true)
+                    .srcsel(true)
+            );
+        });
+    }
+
+    fn regs() -> &'static mut regs::RegisterBlock {
+        regs::RegisterBlock::gem0()
+    }
+}
+
+/// second Gigabit Ethernet peripheal
+pub struct Gem1;
+
+impl Gem for Gem1 {
+    fn setup_clock(tx_clock: u32) {
+        let (divisor0, divisor1) = calculate_tx_divisors(tx_clock);
+
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.gem1_clk_ctrl.write(
+                slcr::GemClkCtrl::zeroed()
+                    .clkact(true)
+                    .srcsel(slcr::PllSource::IoPll)
+                    .divisor(divisor0 as u8)
+                    .divisor1(divisor1 as u8)
+            );
+            // Enable gem1 recv clock
+            slcr.gem1_rclk_ctrl.write(
+                // 0x0000_0801
+                slcr::RclkCtrl::zeroed()
+                    .clkact(true)
+            );
+        });
+    }
+
+    fn regs() -> &'static mut regs::RegisterBlock {
+        regs::RegisterBlock::gem1()
+    }
+}
+
+fn calculate_tx_divisors(tx_clock: u32) -> (u8, u8) {
+    let io_pll = Clocks::get().io;
+    let target = (tx_clock - 1 + io_pll) / tx_clock;
+
+    let mut best = None;
+    let mut best_error = 0;
+    for divisor0 in 1..63 {
+        for divisor1 in 1..63 {
+            let current = (divisor0 as u32) * (divisor1 as u32);
+            let error = if current > target {
+                current - target
+            } else {
+                target - current
+            };
+            if best.is_none() || best_error > error {
+                best = Some((divisor0, divisor1));
+                best_error = error;
+            }
+        }
+    }
+    let result = best.unwrap();
+    debug!("Eth TX clock for {}: {} / {} / {} = {}",
+           tx_clock, io_pll,
+           result.0, result.1,
+           io_pll / result.0 as u32 / result.1 as u32
+    );
+    result
+}
+
+pub struct Eth<GEM: Gem, RX, TX> {
+    rx: RX,
+    tx: TX,
+    inner: EthInner<GEM>,
+    phy: Phy,
+    /// keep track of RX path occupation to avoid needless `check_link_change()`
+    idle: bool,
+}
+
+impl Eth<Gem0, (), ()> {
+    pub fn eth0(macaddr: [u8; 6]) -> Self {
+        #[cfg(not(feature = "target_ebaz4205"))]
        slcr::RegisterBlock::unlocked(|slcr| {
            // Manual example: 0x0000_1280
            // MDIO
@ -126,48 +244,48 @@ impl<'r> Eth<'r, (), ()> {
            // RX_CLK
            slcr.mio_pin_22.write(
                slcr::MioPin22::zeroed()
-                    .tri_enable(true)
                    .l0_sel(true)
+                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RX_CTRL
            slcr.mio_pin_27.write(
                slcr::MioPin27::zeroed()
-                    .tri_enable(true)
                    .l0_sel(true)
+                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RXD3
            slcr.mio_pin_26.write(
                slcr::MioPin26::zeroed()
-                    .tri_enable(true)
                    .l0_sel(true)
+                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RXD2
            slcr.mio_pin_25.write(
                slcr::MioPin25::zeroed()
-                    .tri_enable(true)
                    .l0_sel(true)
+                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RXD1
            slcr.mio_pin_24.write(
                slcr::MioPin24::zeroed()
-                    .tri_enable(true)
                    .l0_sel(true)
+                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
            // RXD0
            slcr.mio_pin_23.write(
                slcr::MioPin23::zeroed()
-                    .tri_enable(true)
                    .l0_sel(true)
+                    .speed(true)
                    .io_type(slcr::IoBufferType::Hstl)
                    .pullup(true)
            );
@ -182,132 +300,102 @@ 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);
+
+        #[cfg(feature="target_kasli_soc")]
+        {
+            let mut eth_reset_pin = PhyRst::rst_pin();
+            eth_reset_pin.reset();
+        }

-    fn from_regs(regs: &'r mut regs::RegisterBlock, macaddr: [u8; 6]) -> Self {
        let mut inner = EthInner {
-            regs,
+            gem: PhantomData,
            link: None,
        };
        inner.init();
+
        inner.configure(macaddr);

        let phy = Phy::find(&mut inner).expect("phy");
        phy.reset(&mut inner);
        phy.restart_autoneg(&mut inner);
+        #[cfg(feature="target_kasli_soc")]
+        phy.set_leds(&mut inner);

        Eth {
            rx: (),
            tx: (),
            inner,
            phy,
+            idle: true,
        }
    }
 }

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

-    pub fn start_tx(self, tx_size: usize) -> Eth<'r, RX, tx::DescList> {
+    pub fn start_tx(self, tx_size: usize) -> Eth<GEM, RX, tx::DescList> {
        let new_self = Eth {
            rx: self.rx,
            tx: tx::DescList::new(tx_size),
            inner: self.inner,
            phy: self.phy,
+            idle: self.idle,
        };
        let list_addr = &new_self.tx.list_addr();
        assert!(list_addr & 0b11 == 0);
-        new_self.inner.regs.tx_qbar.write(
+        GEM::regs().tx_qbar.write(
            regs::TxQbar::zeroed()
                .tx_q_baseaddr(list_addr >> 2)
        );
-        new_self.inner.regs.net_ctrl.modify(|_, w|
+        GEM::regs().net_ctrl.modify(|_, w|
            w.tx_en(true)
        );
        new_self
    }
 }

-impl<'r, TX> Eth<'r, rx::DescList, TX> {
+impl<GEM: Gem, TX> Eth<GEM, rx::DescList, TX> {
    pub fn recv_next<'s: 'p, 'p>(&'s mut self) -> Result<Option<rx::PktRef<'p>>, rx::Error> {
-        let status = self.inner.regs.rx_status.read();
+        let status = GEM::regs().rx_status.read();
        if status.hresp_not_ok() {
            // Clear
-            self.inner.regs.rx_status.write(
+            GEM::regs().rx_status.write(
                regs::RxStatus::zeroed()
                    .hresp_not_ok(true)
            );
@ -315,7 +403,7 @@ impl<'r, TX> Eth<'r, rx::DescList, TX> {
        }
        if status.rx_overrun() {
            // Clear
-            self.inner.regs.rx_status.write(
+            GEM::regs().rx_status.write(
                regs::RxStatus::zeroed()
                    .rx_overrun(true)
            );
@ -323,7 +411,7 @@ impl<'r, TX> Eth<'r, rx::DescList, TX> {
        }
        if status.buffer_not_avail() {
            // Clear
-            self.inner.regs.rx_status.write(
+            GEM::regs().rx_status.write(
                regs::RxStatus::zeroed()
                    .buffer_not_avail(true)
            );
@ -335,28 +423,42 @@ impl<'r, TX> Eth<'r, rx::DescList, TX> {
            match result {
                Ok(None) => {
                    // No packet, clear status bit
-                    self.inner.regs.rx_status.write(
+                    GEM::regs().rx_status.write(
                        regs::RxStatus::zeroed()
                            .frame_recd(true)
                    );
+                    self.idle = true;
                }
-                _ => {}
+                _ =>
+                    self.idle = false,
            }
            result
        } else {
-            self.inner.check_link_change(&self.phy);
+            self.idle = true;
            Ok(None)
        }
    }
 }

-impl<'r, RX> Eth<'r, 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)
+impl<GEM: Gem, TX> libasync::smoltcp::LinkCheck for &mut Eth<GEM, rx::DescList, TX> {
+    type Link = Option<phy::Link>;
+
+    fn check_link_change(&mut self) -> Option<Self::Link> {
+        self.inner.check_link_change(&self.phy)
+    }
+
+    fn is_idle(&self) -> bool {
+        self.idle
    }
 }

-impl<'r, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescList, tx::DescList> {
+impl<GEM: Gem, RX> Eth<GEM, RX, tx::DescList> {
+    pub fn send<'s: 'p, 'p>(&'s mut self, length: usize) -> Option<tx::PktRef<'p>> {
+        self.tx.send(GEM::regs(), length)
+    }
+}
+
+impl<'a, GEM: Gem> smoltcp::phy::Device<'a> for &mut Eth<GEM, rx::DescList, tx::DescList> {
    type RxToken = rx::PktRef<'a>;
    type TxToken = tx::Token<'a>;

@ -370,6 +472,7 @@ impl<'r, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescList, tx::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
@ -379,13 +482,14 @@ impl<'r, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescList, tx::DescLis
        match self.rx.recv_next() {
            Ok(Some(pktref)) => {
                let tx_token = tx::Token {
-                    regs: self.inner.regs,
+                    regs: GEM::regs(),
                    desc_list: &mut self.tx,
                };
+                self.idle = false;
                Some((pktref, tx_token))
            }
            Ok(None) => {
-                self.inner.check_link_change(&self.phy);
+                self.idle = true;
                None
            }
            Err(e) => {
@ -397,33 +501,95 @@ impl<'r, 'a> smoltcp::phy::Device<'a> for &mut Eth<'r, rx::DescList, tx::DescLis

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

+pub struct PhyRst {
+    regs: regs::GpioRegisterBlock,
+    count_down: super::timer::global::CountDown<Milliseconds>,
+}
+
+impl PhyRst {
+    pub fn rst_pin() -> Self {
+        slcr::RegisterBlock::unlocked(|slcr| {
+            // Hardware Reset for PHY
+            slcr.mio_pin_47.write(
+                slcr::MioPin47::zeroed()
+                    .l3_sel(0b000)
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+                    .disable_rcvr(true)
+            );
+        });
+        Self::eth_reset_common(0xFFFF - 0x8000)
+    }
+    
+    fn delay_ms(&mut self, ms: u64) {
+        self.count_down.start(Milliseconds(ms));
+        nb::block!(self.count_down.wait()).unwrap();
+    }
+
+    fn eth_reset_common(gpio_output_mask: u16) -> Self {
+        let self_ = Self {
+            regs: regs::GpioRegisterBlock::regs(),
+            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown(),
+        };
+
+        // Setup GPIO output mask
+        self_.regs.gpio_output_mask.modify(|_, w| {
+            w.mask(gpio_output_mask)
+        });
+
+        self_.regs.gpio_direction.modify(|_, w| {
+            w.phy_rst(true)
+        });
+        
+        self_
+    }
+
+    fn oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+            w.phy_rst(oe)
+        })
+    }
+
+    fn toggle(&mut self, o: bool) {
+        self.regs.gpio_output_mask.modify(|_, w| {
+            w.phy_rst(o)
+        })
+    }
+
+    pub fn reset(&mut self) {
+        self.toggle(false); // drive phy_rst (active LOW) pin low
+        self.oe(true);         // enable pin's output
+        self.delay_ms(10);
+        self.toggle(true);
+    }
 }


-struct EthInner<'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)
@ -437,7 +603,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)
@ -467,29 +633,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
@ -497,24 +666,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)
@ -530,7 +700,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)
        );
@ -538,17 +708,11 @@ 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() {}
    }


-    fn check_link_change(&mut self, phy: &Phy) {
-        // As the PHY access takes some time, exit early if there was
-        // already a link. TODO: check once per second.
-        if self.link.is_some() {
-            return
-        }
-
+    fn check_link_change(&mut self, phy: &Phy) -> Option<Option<phy::Link>> {
        let link = phy.get_link(self);

        // Check link state transition
@ -557,17 +721,15 @@ impl<'r> EthInner<'r> {
                Some(link) => {
                    info!("eth: got {:?}", link);

-                    use phy::LinkSpeed::*;
+                    use phy::{LinkDuplex::Full, LinkSpeed::*};
                    let txclock = match link.speed {
                        S10 => TX_10,
                        S100 => TX_100,
                        S1000 => TX_1000,
                    };
-                    Eth::<(), ()>::setup_gem0_clock(txclock);
-                    /* .full_duplex(false) doesn't work even if
-                       half duplex has been negotiated. */
-                    self.regs.net_cfg.modify(|_, w| w
-                        .full_duplex(true)
+                    GEM::setup_clock(txclock);
+                    GEM::regs().net_cfg.modify(|_, w| w
+                        .full_duplex(link.duplex == Full)
                        .gige_en(link.speed == S1000)
                        .speed(link.speed != S10)
                    );
@ -582,14 +744,17 @@ impl<'r> EthInner<'r> {
            }

            self.link = link;
+            Some(link)
+        } else {
+            None
        }
    }
 }

-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)
@ -598,12 +763,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/control.rs
+++ b/libboard_zynq/src/eth/phy/control.rs
@ -82,6 +82,10 @@ impl PhyRegister for Control {
    fn addr() -> u8 {
        0
    }
+    
+    fn page() -> u8 {
+        0
+    }
 }

 impl From<u16> for Control {
--- a/libboard_zynq/src/eth/phy/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/id.rs
+++ b/libboard_zynq/src/eth/phy/id.rs
@ -11,6 +11,9 @@ pub struct PhyIdentifier {
 }

 pub fn identify_phy<PA: PhyAccess>(pa: &mut PA, addr: u8) -> Option<PhyIdentifier> {
+    #[cfg(feature = "target_kasli_soc")]
+    pa.write_phy(addr, 0x16, 0); //reset page
+    
    let id1 = pa.read_phy(addr, 2);
    let id2 = pa.read_phy(addr, 3);
    if id1 != 0xFFFF || id2 != 0xFFFF {
--- a/libboard_zynq/src/eth/phy/leds.rs
+++ b/libboard_zynq/src/eth/phy/leds.rs
@ -0,0 +1,79 @@
+use bit_field::BitField;
+use super::{PhyRegister, Led0Control, Led1Control};
+
+#[derive(Clone, Copy, Debug)]
+/// LED Control Register
+pub struct Leds(pub u16);
+
+impl Leds {
+    pub fn led0(&self) -> Led0Control {
+        match self.0.get_bits(0..=3) {
+            0b0000 => Led0Control::OnLinkOffNoLink,
+            0b0001 => Led0Control::OnLinkBlinkActivityOffNoLink,
+            0b0010 => Led0Control::BlinkDependingOnLink,
+            0b0011 => Led0Control::OnActivityOffNoActivity,
+            0b0100 => Led0Control::BlinkActivityOffNoActivity,
+            0b0101 => Led0Control::OnTransmitOffNoTransmit,
+            0b0110 => Led0Control::OnCopperLinkOffElse,
+            0b0111 => Led0Control::On1000LinkOffElse,
+            0b1000 => Led0Control::ForceOff,
+            0b1001 => Led0Control::ForceOn,
+            0b1010 => Led0Control::ForceHiZ,
+            0b1011 => Led0Control::ForceBlink,
+            0b1100 => Led0Control::Mode1,
+            0b1101 => Led0Control::Mode2,
+            0b1110 => Led0Control::Mode3,
+            0b1111 => Led0Control::Mode4,
+            _ => unreachable!()
+        }
+    }
+    pub fn led1(&self) -> Led1Control {
+        match self.0.get_bits(4..=7) {
+            0b0000 => Led1Control::OnReceiveOffNoReceive,
+            0b0001 => Led1Control::OnLinkBlinkActivityOffNoLink,
+            0b0010 => Led1Control::OnLinkBlinkReceiveOffNoLink,
+            0b0011 => Led1Control::OnActivityOffNoActivity,
+            0b0100 => Led1Control::BlinkActivityOffNoActivity,
+            0b0101 => Led1Control::On100OrFiberOffElse,
+            0b0110 => Led1Control::On1001000LinkOffElse,
+            0b0111 => Led1Control::On100LinkOffElse,
+            0b1000 => Led1Control::ForceOff,
+            0b1001 => Led1Control::ForceOn,
+            0b1010 => Led1Control::ForceHiZ,
+            0b1011 => Led1Control::ForceBlink,
+            _ => unreachable!()
+        }
+    }
+
+    pub fn set_led0(mut self, setting: Led0Control) -> Self {
+        self.0.set_bits(0..=3, setting as u16);
+        self
+    }
+
+    pub fn set_led1(mut self, setting: Led1Control) -> Self {
+        self.0.set_bits(4..=7, setting as u16);
+        self
+    }
+}
+
+impl PhyRegister for Leds {
+    fn addr() -> u8 {
+        0x10
+    }
+        
+    fn page() -> u8 {
+        3
+    }
+}
+
+impl From<u16> for Leds {
+    fn from(value: u16) -> Self {
+        Leds(value)
+    }
+}
+
+impl Into<u16> for Leds {
+    fn into(self) -> u16 {
+        self.0
+    }
+}
--- a/libboard_zynq/src/eth/phy/mod.rs
+++ b/libboard_zynq/src/eth/phy/mod.rs
@ -2,12 +2,14 @@ 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;
+mod leds;
+pub use leds::Leds;

-#[derive(Clone, Debug, PartialEq)]
+#[derive(Copy, Clone, Debug, PartialEq)]
 pub struct Link {
    pub speed: LinkSpeed,
    pub duplex: LinkDuplex,
@ -26,58 +28,105 @@ pub enum LinkDuplex {
    Full,
 }

+#[derive(Copy, Clone, Debug, PartialEq)]
+pub enum Led0Control {
+    OnLinkOffNoLink = 0b0000,
+    OnLinkBlinkActivityOffNoLink = 0b0001,
+    BlinkDependingOnLink = 0b0010,
+    OnActivityOffNoActivity = 0b0011,
+    BlinkActivityOffNoActivity = 0b0100,
+    OnTransmitOffNoTransmit = 0b0101,
+    OnCopperLinkOffElse = 0b0110,
+    On1000LinkOffElse = 0b0111,
+    ForceOff = 0b1000,
+    ForceOn = 0b1001,
+    ForceHiZ = 0b1010,
+    ForceBlink = 0b1011,
+    Mode1 = 0b1100,
+    Mode2 = 0b1101,
+    Mode3 = 0b1110,
+    Mode4 = 0b1111
+}
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub enum Led1Control {
+    OnReceiveOffNoReceive = 0b0000,
+    OnLinkBlinkActivityOffNoLink = 0b0001,
+    OnLinkBlinkReceiveOffNoLink = 0b0010,
+    OnActivityOffNoActivity = 0b0011,
+    BlinkActivityOffNoActivity = 0b0100,
+    On100OrFiberOffElse = 0b0101,
+    On1001000LinkOffElse = 0b0110,
+    On100LinkOffElse = 0b0111,
+    ForceOff = 0b1000,
+    ForceOn = 0b1001,
+    ForceHiZ = 0b1010,
+    ForceBlink = 0b1011,
+}
+
 pub trait PhyAccess {
    fn read_phy(&mut self, addr: u8, reg: u8) -> u16;
    fn write_phy(&mut self, addr: u8, reg: u8, data: u16);
 }

+pub trait PhyRegister {
+    fn addr() -> u8;
+    fn page() -> u8;
+}
+
+
 #[derive(Clone)]
 pub struct Phy {
    pub addr: u8,
-    device: PhyDevice,
 }

-#[derive(Clone, Copy)]
-pub enum PhyDevice {
-    Marvel88E1116R,
-    Rtl8211E,
-}
-
-const OUI_MARVEL: u32 = 0x005043;
+const OUI_MARVELL: u32 = 0x005043;
 const OUI_REALTEK: u32 = 0x000732;
+const OUI_LANTIQ : u32 = 0x355969;
+const OUI_ICPLUS : u32 = 0x0090c3;
+
+//only change pages on Kasli-SoC's Marvel 88E11xx
+#[cfg(feature="target_kasli_soc")]
+const PAGE_REGISTER: u8 = 0x16;

 impl Phy {
    /// Probe all addresses on MDIO for a known PHY
    pub fn find<PA: PhyAccess>(pa: &mut PA) -> Option<Phy> {
-        for addr in 1..32 {
-            let device = match identify_phy(pa, addr) {
+        (0..32).find(|addr| {
+            match identify_phy(pa, *addr) {
                Some(PhyIdentifier {
-                    oui: OUI_MARVEL,
+                    oui: OUI_MARVELL,
+                    // Marvell 88E1116R
                    model: 36,
                    ..
-                }) => Some(PhyDevice::Marvel88E1116R),
+                }) => true,
+                Some(PhyIdentifier {
+                    oui: OUI_MARVELL,
+                    // Marvell 88E1512
+                    model: 29,
+                    ..
+                }) => true,
                Some(PhyIdentifier {
                    oui: OUI_REALTEK,
+                    // RTL 8211E
                    model: 0b010001,
                    rev: 0b0101,
-                }) => Some(PhyDevice::Rtl8211E),
-                _ => None,
-            };
-            match device {
-                Some(device) =>
-                    return Some(Phy { addr, device }),
-                None => {}
+                }) => true,
+                Some(PhyIdentifier {
+                    oui: OUI_LANTIQ,
+                    // Intel XWAY PHY11G (PEF 7071/PEF 7072) v1.5 / v1.6
+                    model: 0,
+                    ..
+                }) => true,
+                Some(PhyIdentifier {
+                    oui: OUI_ICPLUS,
+                    // IP101G-DS-R01
+                    model: 5,
+                    rev: 4,
+                }) => true,
+                _ => false,
            }
-        }
-
-        None
-    }
-
-    pub fn name(&self) -> &'static str {
-        match self.device {
-            PhyDevice::Marvel88E1116R => &"Marvel 88E1116R",
-            PhyDevice::Rtl8211E => &"RTL8211E",
-        }
+        }).map(|addr| Phy { addr })
    }

    pub fn read_reg<PA, PR>(&self, pa: &mut PA) -> PR
@ -85,6 +134,9 @@ impl Phy {
        PA: PhyAccess,
        PR: PhyRegister + From<u16>,
    {
+        #[cfg(feature="target_kasli_soc")]
+        pa.write_phy(self.addr, PAGE_REGISTER, PR::page().into());
+
        pa.read_phy(self.addr, PR::addr()).into()
    }

@ -94,6 +146,9 @@ impl Phy {
        PR: PhyRegister + From<u16> + Into<u16>,
        F: FnMut(PR) -> PR,
    {
+        #[cfg(feature="target_kasli_soc")]
+        pa.write_phy(self.addr, PAGE_REGISTER, PR::page().into());
+        
        let reg = pa.read_phy(self.addr, PR::addr()).into();
        let reg = f(reg);
        pa.write_phy(self.addr, PR::addr(), reg.into())
@ -107,6 +162,14 @@ impl Phy {
        self.modify_reg(pa, f)
    }

+    pub fn modify_leds<PA, F>(&self, pa: &mut PA, f: F)
+    where
+        PA: PhyAccess,
+        F: FnMut(Leds) -> Leds,
+    {
+        self.modify_reg(pa, f)   
+    }
+
    pub fn get_control<PA: PhyAccess>(&self, pa: &mut PA) -> Control {
        self.read_reg(pa)
    }
@ -120,12 +183,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()
        }
@ -144,8 +203,12 @@ impl Phy {
                .set_restart_autoneg(true)
        );
    }
-}

-pub trait PhyRegister {
-    fn addr() -> u8;
+    #[cfg(feature="target_kasli_soc")]
+    pub fn set_leds<PA: PhyAccess>(&self, pa: &mut PA) {
+        self.modify_leds(pa, |leds|
+            leds.set_led0(Led0Control::OnCopperLinkOffElse)
+                .set_led1(Led1Control::BlinkActivityOffNoActivity)
+        );
+    }
 }
--- a/libboard_zynq/src/eth/phy/pssr.rs
+++ b/libboard_zynq/src/eth/phy/pssr.rs
@ -0,0 +1,56 @@
+use bit_field::BitField;
+use super::{PhyRegister, Link, LinkDuplex, LinkSpeed};
+
+#[derive(Clone, Copy, Debug)]
+/// PHY-Specific Status Register
+pub struct PSSR(pub u16);
+
+impl PSSR {
+    pub fn link(&self) -> bool {
+        self.0.get_bit(10)
+    }
+
+    pub fn duplex(&self) -> LinkDuplex {
+        if self.0.get_bit(13) {
+            LinkDuplex::Full
+        } else {
+            LinkDuplex::Half
+        }
+    }
+
+    pub fn speed(&self) -> Option<LinkSpeed> {
+        match self.0.get_bits(14..=15) {
+            0b00 => Some(LinkSpeed::S10),
+            0b01 => Some(LinkSpeed::S100),
+            0b10 => Some(LinkSpeed::S1000),
+            _ => None,
+        }
+    }
+
+    pub fn get_link(&self) -> Option<Link> {
+        if self.link() {
+            Some(Link {
+                speed: self.speed()?,
+                duplex: self.duplex(),
+            })
+        } else {
+            None
+        }
+    }
+}
+
+impl PhyRegister for PSSR {
+    fn addr() -> u8 {
+        0x11
+    }
+        
+    fn page() -> u8 {
+        0
+    }
+}
+
+impl From<u16> for PSSR {
+    fn from(value: u16) -> Self {
+        PSSR(value)
+    }
+}
--- a/libboard_zynq/src/eth/phy/status.rs
+++ b/libboard_zynq/src/eth/phy/status.rs
@ -55,12 +55,22 @@ impl Status {
    pub fn get_link(&self) -> Option<Link> {
        if ! self.link_status() {
            None
-        } else if self.cap_10base_t_half() {
+        } else if self.cap_100base_tx_full() {
            Some(Link {
-                speed: LinkSpeed::S10,
+                speed: LinkSpeed::S100,
+                duplex: LinkDuplex::Full,
+            })
+        } else if self.cap_100base_tx_half() {
+            Some(Link {
+                speed: LinkSpeed::S100,
                duplex: LinkDuplex::Half,
            })
-        } else if self.cap_10base_t_full() {
+        } else if self.cap_100base_t4() {
+            Some(Link {
+                speed: LinkSpeed::S100,
+                duplex: LinkDuplex::Half,
+            })
+        } else if self.cap_10base_t2_full() {
            Some(Link {
                speed: LinkSpeed::S10,
                duplex: LinkDuplex::Full,
@ -70,26 +80,16 @@ impl Status {
                speed: LinkSpeed::S10,
                duplex: LinkDuplex::Half,
            })
-        } else if self.cap_10base_t2_full() {
+        } else if self.cap_10base_t_full() {
            Some(Link {
                speed: LinkSpeed::S10,
                duplex: LinkDuplex::Full,
            })
-        } else if self.cap_100base_t4() {
+        } else if self.cap_10base_t_half() {
            Some(Link {
-                speed: LinkSpeed::S100,
+                speed: LinkSpeed::S10,
                duplex: LinkDuplex::Half,
            })
-        } else if self.cap_100base_tx_half() {
-            Some(Link {
-                speed: LinkSpeed::S100,
-                duplex: LinkDuplex::Half,
-            })
-        } else if self.cap_100base_tx_full() {
-            Some(Link {
-                speed: LinkSpeed::S100,
-                duplex: LinkDuplex::Full,
-            })
        } else {
            None
        }
@ -100,6 +100,10 @@ impl PhyRegister for Status {
    fn addr() -> u8 {
        1
    }
+        
+    fn page() -> u8 {
+        0
+    }
 }

 impl From<u16> for Status {
--- a/libboard_zynq/src/eth/regs.rs
+++ b/libboard_zynq/src/eth/regs.rs
@ -1,6 +1,6 @@
 use volatile_register::{RO, WO, RW};

-use 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,19 +110,52 @@ 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 struct GpioRegisterBlock {
+    pub gpio_output_mask: &'static mut OutputMask,
+    pub gpio_direction: &'static mut Direction,
+    pub gpio_output_enable: &'static mut OutputEnable,
+}

-    pub fn gem0() -> &'static mut Self {
-        unsafe { &mut *Self::GEM0 }
-    }
-
-    pub fn gem1() -> &'static mut Self {
-        unsafe { &mut *Self::GEM1 }
+impl GpioRegisterBlock {
+    pub fn regs() -> Self {
+        Self {
+            gpio_output_mask: OutputMask::new(),
+            gpio_direction: Direction::new(),
+            gpio_output_enable: OutputEnable::new(),
+        }
    }
 }

+register!(gpio_output_mask, 
+            /// MASK_DATA_1_SW:
+            /// Maskable output data for MIO[47:32]
+            OutputMask, RW, u32);
+register_at!(OutputMask, 0xE000A008, new);
+register_bit!(gpio_output_mask, 
+            /// Output for PHY_RST (MIO[47])
+            phy_rst, 15);
+register_bits!(gpio_output_mask,
+            mask, u16, 16, 31);
+register!(gpio_direction, 
+            /// DIRM_1:
+            /// Direction mode for MIO[53:32]; 0/1 = in/out
+            Direction, RW, u32);
+register_at!(Direction, 0xE000A244, new);
+register_bit!(gpio_direction,
+            /// Direction for PHY_RST
+            phy_rst, 15);
+register!(gpio_output_enable,
+            /// OEN_1:
+            /// Output enable for MIO[53:32]
+            OutputEnable, RW, u32);
+register_at!(OutputEnable, 0xE000A248, new);
+register_bit!(gpio_output_enable,
+            /// Output enable for PHY_RST
+            phy_rst, 15);
+    
+register_at!(RegisterBlock, 0xE000B000, gem0);
+register_at!(RegisterBlock, 0xE000C000, gem1);
+
 register!(net_ctrl, NetCtrl, RW, u32);
 register_bit!(net_ctrl, loopback_local, 1);
 register_bit!(net_ctrl, rx_en, 2);
--- a/libboard_zynq/src/eth/rx.rs
+++ b/libboard_zynq/src/eth/rx.rs
@ -2,8 +2,6 @@ use core::ops::Deref;
 use alloc::{vec, vec::Vec};
 use libcortex_a9::{asm::*, cache::*, UncachedSlice};
 use libregister::*;
-use log::debug;
-use crate::l2cache;
 use super::Buffer;

 #[derive(Debug)]
@ -83,6 +81,9 @@ impl DescList {
            entry.word1.write(
                DescWord1::zeroed()
            );
+            // Flush buffer from cache, to be filled by the peripheral
+            // before next read
+            dcci_slice(&buffer[..]);
        }

        DescList {
@ -92,6 +93,10 @@ impl DescList {
        }
    }

+    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
    }
@ -103,10 +108,21 @@ impl DescList {
        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];
-            // Invalidate caches for packet buffer
-            l2cache().invalidate_slice(&mut buffer[..]);
-            dcci_slice(&buffer[..]);

            self.next += 1;
            if self.next >= list_len {
@ -115,10 +131,8 @@ impl DescList {

            let pkt = PktRef { entry, buffer };
            if word1.start_of_frame() && word1.end_of_frame() {
-                // debug!("pkt {}: {:08X}..{:08X}", len, &pkt.buffer[0] as *const _ as usize, &pkt.buffer[pkt.len()-1] as *const _ as usize);
                Ok(Some(pkt))
            } else {
-                debug!("pkt trunc");
                Err(Error::Truncated)
            }
        } else {
@ -135,7 +149,6 @@ 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,9 +1,7 @@
 use core::ops::{Deref, DerefMut};
 use alloc::{vec, vec::Vec};
-use libcortex_a9::{asm::dmb, cache::dcc_slice, UncachedSlice};
+use libcortex_a9::{cache::dcc_slice, UncachedSlice};
 use libregister::*;
-use log::{debug, warn};
-use crate::l2cache;
 use super::{Buffer, regs};

 /// Descriptor entry
@ -87,15 +85,17 @@ impl DescList {
        }
    }

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

    pub fn send<'s: 'p, 'p>(&'s mut self, regs: &'s mut regs::RegisterBlock, length: usize) -> Option<PktRef<'p>> {
-        // debug!("send {}", length);
        let list_len = self.list.len();
        let entry = &mut self.list[self.next];
-        dmb();
        if entry.word1.read().used() {
            let buffer = &mut self.buffers[self.next][0..length];
            entry.word1.write(DescWord1::zeroed()
@ -113,7 +113,6 @@ impl DescList {
            Some(PktRef { entry, buffer, regs })
        } else {
            // Still in use by HW (sending too fast, ring exceeded)
-            warn!("tx ring overflow");
            None
        }
    }
@ -129,17 +128,12 @@ pub struct PktRef<'a> {

 impl<'a> Drop for PktRef<'a> {
    fn drop(&mut self) {
-        // Write back all dirty cachelines of packet buffer
+        // Write back all dirty cachelines of this buffer
        dcc_slice(self.buffer);
-        l2cache().clean_slice(self.buffer);

        self.entry.word1.modify(|_, w| w.used(false));
-        dmb();
-        // dsb();
-        if ! self.regs.tx_status.read().tx_go() {
-            // Start TX if not already running
-            self.regs.net_ctrl.modify(|_, w| w.start_tx(true));
-        }
+        // Start the TX engine
+        self.regs.net_ctrl.modify(|_, w| w.start_tx(true));
    }
 }

@ -170,10 +164,7 @@ impl<'a> smoltcp::phy::TxToken for Token<'a> {
            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,503 +0,0 @@
-//! Quad-SPI Flash Controller
-
-use core::marker::PhantomData;
-use log::{error, info, warn};
-use libregister::{RegisterR, RegisterW, RegisterRW};
-use crate::{print, println};
-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;
-/// Instruction: Read
-const INST_READ: u8 = 0x03;
-/// Instruction: Quad I/O Fast Read
-const INST_4IO_FAST_READ: u8 = 0xEB;
-/// 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: 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 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)
-            );
-
-            // 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)
-                    .pullup(true)
-            );
-
-            // 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)
-                    .pullup(true)
-            );
-            slcr.mio_pin_11.write(
-                slcr::MioPin11::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-                    .pullup(true)
-            );
-            slcr.mio_pin_12.write(
-                slcr::MioPin12::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-                    .pullup(true)
-            );
-            slcr.mio_pin_13.write(
-                slcr::MioPin13::zeroed()
-                    .l0_sel(true)
-                    .io_type(slcr::IoBufferType::Lvcmos18)
-                    .pullup(true)
-            );
-
-            // 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)
-                    .pullup(true)
-            );
-        });
-    }
-
-    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(INST_4IO_FAST_READ)
-            .dummy_mask(0x2)
-            .mode_en(false)
-            .mode_bits(0xFF)
-            // 2 devices
-            .two_mem(true)
-            .u_page(false)
-            // Quad SPI 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()
-            // Quad I/O Fast Read
-            .inst_code(INST_READ)
-            .dummy_mask(0x2)
-            .mode_en(false)
-            .mode_bits(0xFF)
-            // 2 devices
-            .two_mem(true)
-            .u_page(chip_index != 0)
-            // Quad SPI 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() {
-            error!("E_ERR");
-        } else if sr1.p_err() {
-            error!("P_ERR");
-        } else if sr1.wip() {
-            info!("Erase in progress");
-            while self.read_reg::<SR1>().wip() {
-                print!(".");
-            }
-            println!("");
-        } else {
-            warn!("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() {
-            error!("E_ERR");
-        } else if sr1.p_err() {
-            error!("P_ERR");
-        } else if sr1.wip() {
-            info!("Program in progress");
-            while self.read_reg::<SR1>().wip() {
-                print!(".");
-            }
-            println!("");
-        } else {
-            warn!("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_mask, 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));
+        }
+
+        // sensitivity
+        let m = (id.0 >> 4) as usize;
+        let n = (2 * (id.0 & 0xF)) as usize;
+        unsafe {
+            self.mpcore.icdicfr[m].modify(|mut icdicfr| *icdicfr.set_bits(n..=n+1, match sensitivity {
+                InterruptSensitivity::Level => 0b00,
+                InterruptSensitivity::Edge  => 0b10,
+            }));
+        }
+
+        // priority
+        let offset = (id.0 % 4) * 8;
+        let priority: u32 = (priority as u32) << (offset + 3);
+        let mask: u32 = 0xFFFFFFFF ^ (0xFF << offset);
+        unsafe {
+            self.mpcore.icdipr[id.0 as usize / 4].modify(|v| (v & mask) | priority);
+        }
+
+        self.enable_interrupts();
+    }
+
+    pub fn end_interrupt(&mut self, id: InterruptId) {
+        self.mpcore.icceoir.modify(|_, w| w.eoiintid(id.0 as u32));
+    }
+
+    pub fn get_interrupt_id(&self) -> InterruptId {
+        InterruptId(self.mpcore.icciar.read().ackintid() as u8)
+    }
+
+}
--- a/libboard_zynq/src/i2c/eeprom.rs
+++ b/libboard_zynq/src/i2c/eeprom.rs
@ -0,0 +1,130 @@
+use super::I2c;
+use crate::time::Milliseconds;
+use embedded_hal::timer::CountDown;
+
+pub struct EEPROM<'a> {
+    i2c: &'a mut I2c,
+    #[cfg(not(feature = "target_ebaz4205"))]
+    port: u8,
+    address: u8,
+    page_size: u8,
+    count_down: crate::timer::global::CountDown<Milliseconds>
+}
+
+impl<'a> EEPROM<'a> {
+    #[cfg(feature = "target_zc706")]
+    pub fn new(i2c: &'a mut I2c, page_size: u8) -> Self {
+        EEPROM {
+            i2c: i2c,
+            port: 2,
+            address: 0b1010100,
+            page_size: page_size,
+            count_down: unsafe { crate::timer::GlobalTimer::get() }.countdown()
+        }
+    }
+
+    #[cfg(feature = "target_kasli_soc")]
+    pub fn new(i2c: &'a mut I2c, page_size: u8) -> Self {
+        EEPROM {
+            i2c: i2c,
+            port: 3,
+            address: 0x57,
+            page_size: page_size,
+            count_down: unsafe { crate::timer::GlobalTimer::get() }.countdown()
+        }
+    }
+
+    #[cfg(feature = "target_zc706")]
+    fn select(&mut self) -> Result<(), &'static str> {
+        self.i2c.pca954x_select(0b1110100, Some(self.port))?;
+        Ok(())
+    }
+
+    #[cfg(feature = "target_kasli_soc")]
+    fn select(&mut self) -> Result<(), &'static str> {
+        // tca9548 is compatible with pca9548
+        self.i2c.pca954x_select(0b1110001, Some(self.port))?;
+        Ok(())
+    }
+
+    #[cfg(feature = "target_ebaz4205")]
+    fn select(&mut self) -> Result<(), &'static str> {
+        Ok(())
+    }
+
+    /// Random read
+    pub fn read<'r>(&mut self, addr: u8, buf: &'r mut [u8]) -> Result<(), &'static str> {
+        self.select()?;
+
+        self.i2c.start()?;
+        self.i2c.write(self.address << 1)?;
+        self.i2c.write(addr)?;
+
+        self.i2c.restart()?;
+        self.i2c.write((self.address << 1) | 1)?;
+        let buf_len = buf.len();
+        for (i, byte) in buf.iter_mut().enumerate() {
+            *byte = self.i2c.read(i < buf_len - 1)?;
+        }
+
+        self.i2c.stop()?;
+
+        Ok(())
+    }
+
+    /// Smart multi-page writing
+    /// Using the "Page Write" function of an EEPROM, the memory region for each transaction
+    /// (i.e. from byte `addr` to byte `addr+buf.len()`) should fit under each page 
+    /// (i.e. `addr+buf.len()` < `addr/self.page_size+1`); otherwise, a roll-oever occurs, 
+    /// where bytes beyond the page end. This smart function takes care of the scenario to avoid
+    /// any roll-over when writing ambiguous memory regions.
+    pub fn write(&mut self, addr: u8, buf: &[u8]) -> Result<(), &'static str> {
+        self.select()?;
+
+        let buf_len = buf.len();
+        let mut pb: u8 = addr % self.page_size;
+        for (i, byte) in buf.iter().enumerate() {
+            if (i == 0) || (pb == 0) {
+                self.i2c.start()?;
+                self.i2c.write(self.address << 1)?;
+                self.i2c.write(addr + (i as u8))?;
+            }
+            self.i2c.write(*byte)?;
+            pb += 1;
+
+            if (i == buf_len-1) || (pb == self.page_size) {
+                self.i2c.stop()?;
+                self.poll(1_000)?;
+                pb = 0;
+            }
+        }
+
+        Ok(())
+    }
+
+    /// Poll
+    pub fn poll(&mut self, timeout_ms: u64) -> Result<(), &'static str> {
+        self.select()?;
+
+        self.count_down.start(Milliseconds(timeout_ms));
+        loop {
+            self.i2c.start()?;
+            let ack = self.i2c.write(self.address << 1)?;
+            self.i2c.stop()?;
+            if ack {
+                break
+            };
+            if !self.count_down.waiting() {
+                return Err("I2C polling timeout")
+            }
+        }
+
+        Ok(())
+    }
+
+    pub fn read_eui48<'r>(&mut self) -> Result<[u8; 6], &'static str> {
+        let mut buffer = [0u8; 6];
+        self.read(0xFA, &mut buffer)?;
+        Ok(buffer)
+    }
+}
--- a/libboard_zynq/src/i2c/mod.rs
+++ b/libboard_zynq/src/i2c/mod.rs
@ -0,0 +1,336 @@
+//! I2C Bit-banging Controller
+
+mod regs;
+pub mod eeprom;
+#[cfg(not(feature = "target_ebaz4205"))]
+use super::slcr;
+use super::time::Microseconds;
+use embedded_hal::timer::CountDown;
+use libregister::{RegisterR, RegisterRW};
+#[cfg(not(feature = "target_ebaz4205"))]
+use libregister::RegisterW;
+#[cfg(feature = "target_kasli_soc")]
+use log::info;
+
+pub enum I2cMultiplexer {
+    PCA9548 = 0,
+    #[cfg(feature = "target_kasli_soc")]
+    PCA9547 = 1,
+}
+
+pub struct I2c {
+    regs: regs::RegisterBlock,
+    count_down: super::timer::global::CountDown<Microseconds>,
+    pca_type: I2cMultiplexer
+}
+
+impl I2c {
+    #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+    pub fn i2c0() -> Self {
+        // Route I2C 0 SCL / SDA Signals to MIO Pins 50 / 51
+        #[cfg(not(feature = "target_ebaz4205"))]
+        slcr::RegisterBlock::unlocked(|slcr| {
+            // SCL
+            slcr.mio_pin_50.write(
+                slcr::MioPin50::zeroed()
+                    .l3_sel(0b000)  // as GPIO 50
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+                    .disable_rcvr(true)
+            );
+            // SDA
+            slcr.mio_pin_51.write(
+                slcr::MioPin51::zeroed()
+                    .l3_sel(0b000)  // as GPIO 51
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+                    .disable_rcvr(true)
+            );
+            // On Kasli-SoC prototype, leakage through the unconfigured I2C_SW_RESET
+            // MIO pin develops enough voltage on the T21 gate to assert the reset.
+            // Configure the pin to avoid this problem.
+            #[cfg(feature = "target_kasli_soc")]
+            slcr.mio_pin_33.write(
+                slcr::MioPin33::zeroed()
+                    .l3_sel(0b000)
+                    .io_type(slcr::IoBufferType::Lvcmos33)
+                    .pullup(false)
+                    .disable_rcvr(true)
+            );
+        });
+
+        Self::i2c_common(0xFFFF - 0x000C, 0xFFFF - 0x0002)
+    }
+
+    fn i2c_common(gpio_output_mask: u16, _gpio_output_mask_lower: u16) -> Self {
+        // Setup register block
+        let self_ = Self {
+            regs: regs::RegisterBlock::i2c(),
+            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown(),
+            pca_type: I2cMultiplexer::PCA9548 //default for zc706
+        };
+
+        // Setup GPIO output mask
+        self_.regs.gpio_output_mask.modify(|_, w| {
+            w.mask(gpio_output_mask)
+        });
+        // Setup GPIO driver direction
+        self_.regs.gpio_direction.modify(|_, w| {
+            w.scl(true).sda(true)
+        });
+
+        //Kasli-SoC only: I2C_SW_RESET configuration
+        #[cfg(feature = "target_kasli_soc")]
+        {
+            self_.regs.gpio_output_mask_lower.modify(|_, w| {
+                w.mask(_gpio_output_mask_lower)
+            });
+            self_.regs.gpio_direction.modify(|_, w| {
+                w.i2cswr(true)
+            });
+        }
+
+        self_
+    }
+
+    /// Delay for I2C operations, simple wrapper for nb.
+    fn delay_us(&mut self, us: u64) {
+        self.count_down.start(Microseconds(us));
+        nb::block!(self.count_down.wait()).unwrap();
+    }
+
+    fn unit_delay(&mut self) { self.delay_us(100) }
+
+    fn sda_i(&mut self) -> bool {
+        self.regs.gpio_input.read().sda()
+    }
+
+    fn scl_i(&mut self) -> bool {
+        self.regs.gpio_input.read().scl()
+    }
+
+    fn sda_oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+             w.sda(oe)
+        })
+    }
+
+    fn sda_o(&mut self, o: bool) {
+        self.regs.gpio_output_mask.modify(|_, w| {
+             w.sda_o(o)
+        })
+    }
+
+    fn scl_oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+             w.scl(oe)
+        })
+    }
+
+    fn scl_o(&mut self, o: bool) {
+        self.regs.gpio_output_mask.modify(|_, w| {
+             w.scl_o(o)
+        })
+    }
+
+    #[cfg(feature = "target_kasli_soc")]
+    fn i2cswr_oe(&mut self, oe: bool) {
+        self.regs.gpio_output_enable.modify(|_, w| {
+             w.i2cswr(oe)
+        })
+    }
+
+    #[cfg(feature = "target_kasli_soc")]
+    fn i2cswr_o(&mut self, o: bool) {
+        self.regs.gpio_output_mask_lower.modify(|_, w| {
+             w.i2cswr_o(o)
+        })
+    }
+
+    #[cfg(feature = "target_kasli_soc")]
+    fn pca_autodetect(&mut self) -> Result<I2cMultiplexer, &'static str> {
+        // start with resetting the PCA954X
+        // SDA must be clear (before start)
+        // reset time is 500ns, unit_delay (100us) to account for propagation
+        self.i2cswr_o(true);
+        self.unit_delay();
+        self.i2cswr_o(false);
+        self.unit_delay();
+    
+        let pca954x_read_addr = (0x71 << 1) | 0x01;
+
+        self.start()?;
+        // read the config register
+        if !self.write(pca954x_read_addr)? {
+            return Err("PCA954X failed to ack read address");
+        }
+        let config = self.read(false)?;
+
+        let pca = match config {
+            0x00 => { info!("PCA9548 detected"); I2cMultiplexer::PCA9548 },
+            0x08 => { info!("PCA9547 detected"); I2cMultiplexer::PCA9547 },
+            _ => { return Err("Unknown response for PCA954X autodetect")},
+        };
+        self.stop()?;
+        Ok(pca)
+    }
+
+    pub fn init(&mut self) -> Result<(), &'static str> {
+        self.scl_oe(false);
+        self.sda_oe(false);
+        self.scl_o(false);
+        self.sda_o(false);
+
+        // Check the I2C bus is ready
+        self.unit_delay();
+        self.unit_delay();
+        if !self.sda_i() {
+            // Try toggling SCL a few times
+            for _bit in 0..8 {
+                self.scl_oe(true);
+                self.unit_delay();
+                self.scl_oe(false);
+                self.unit_delay();
+            }
+        }
+
+        if !self.sda_i() {
+            return Err("SDA is stuck low and doesn't get unstuck");
+        }
+        if !self.scl_i() {
+            return Err("SCL is stuck low");
+        }
+        // postcondition: SCL and SDA high
+        
+        #[cfg(feature = "target_kasli_soc")]
+        {
+            self.i2cswr_oe(true);
+            self.pca_type = self.pca_autodetect()?;
+        }
+        
+        Ok(())
+    }
+
+    pub fn start(&mut self) -> Result<(), &'static str> {
+        // precondition: SCL and SDA high
+        if !self.scl_i() {
+            return Err("SCL is stuck low");
+        }
+        if !self.sda_i() {
+            return Err("SDA arbitration lost");
+        }
+        self.sda_oe(true);
+        self.unit_delay();
+        self.scl_oe(true);
+        self.unit_delay();
+        // postcondition: SCL and SDA low
+        Ok(())
+    }
+
+    pub fn restart(&mut self) -> Result<(), &'static str> {
+        // precondition SCL and SDA low
+        self.sda_oe(false);
+        self.unit_delay();
+        self.scl_oe(false);
+        self.unit_delay();
+        self.start()?;
+        // postcondition: SCL and SDA low
+        Ok(())
+    }
+
+    pub fn stop(&mut self) -> Result<(), &'static str> {
+        // precondition: SCL and SDA low
+        self.unit_delay();
+        self.scl_oe(false);
+        self.unit_delay();
+        self.sda_oe(false);
+        self.unit_delay();
+        if !self.sda_i() {
+            return Err("SDA arbitration lost");
+        }
+        // postcondition: SCL and SDA high
+        Ok(())
+    }
+
+    pub fn write(&mut self, data: u8) -> Result<bool, &'static str> {
+        // precondition: SCL and SDA low
+        // MSB first
+        for bit in (0..8).rev() {
+            self.sda_oe(data & (1 << bit) == 0);
+            self.unit_delay();
+            self.scl_oe(false);
+            self.unit_delay();
+            self.scl_oe(true);
+            self.unit_delay();
+        }
+        self.sda_oe(false);
+        self.unit_delay();
+        self.scl_oe(false);
+        self.unit_delay();
+        // Read ack/nack
+        let ack = !self.sda_i();
+        self.scl_oe(true);
+        self.unit_delay();
+        self.sda_oe(true);
+        // postcondition: SCL and SDA low
+
+        Ok(ack)
+    }
+
+    pub fn read(&mut self, ack: bool) -> Result<u8, &'static str> {
+        // precondition: SCL and SDA low
+        self.sda_oe(false);
+
+        let mut data: u8 = 0;
+
+        // MSB first
+        for bit in (0..8).rev() {
+            self.unit_delay();
+            self.scl_oe(false);
+            self.unit_delay();
+            if self.sda_i() { data |= 1 << bit }
+            self.scl_oe(true);
+        }
+        // Send ack/nack (true = nack, false = ack)
+        self.sda_oe(ack);
+        self.unit_delay();
+        self.scl_oe(false);
+        self.unit_delay();
+        self.scl_oe(true);
+        self.sda_oe(true);
+        // postcondition: SCL and SDA low
+
+        Ok(data)
+    }
+
+    pub fn pca954x_select(&mut self, address: u8, channel: Option<u8>) -> Result<(), &'static str> {
+        self.start()?;
+        // PCA9547 supports only one channel at a time
+        // for compatibility, PCA9548 is treated as such too
+        // channel - Some(x) - # of the channel [0,7], or None for all disabled
+        let setting = match self.pca_type {
+            I2cMultiplexer::PCA9548 => { 
+                match channel {
+                    Some(ch) => 1 << ch,
+                    None => 0,
+                }
+            },
+            #[cfg(feature = "target_kasli_soc")]
+            I2cMultiplexer::PCA9547 => {
+                match channel {
+                    Some(ch) => ch | 0x08,
+                    None => 0,
+                }
+            }
+        };
+
+        if !self.write(address << 1)? {
+            return Err("PCA954X failed to ack write address")
+        }
+        if !self.write(setting)? {
+            return Err("PCA954X failed to ack control word")
+        }
+        self.stop()?;
+        Ok(())
+    }
+}
--- a/libboard_zynq/src/i2c/regs.rs
+++ b/libboard_zynq/src/i2c/regs.rs
@ -0,0 +1,135 @@
+use libregister::{
+    register, register_at,
+    register_bit, register_bits
+};
+
+// With reference to:
+//
+// artiq:artiq/gateware/targets/kasli.py:
+// self.submodules.i2c = gpio.GPIOTristate([i2c.scl, i2c.sda])
+//
+// misoc:misoc/cores/gpio.py:
+// class GPIOTristate(Module, AutoCSR):
+//    def __init__(self, signals, reset_out=0, reset_oe=0):
+//        l = len(signals)
+//        self._in = CSRStatus(l)
+//        self._out = CSRStorage(l, reset=reset_out)
+//        self._oe = CSRStorage(l, reset=reset_oe)
+// 
+// Hence, using GPIOs as SCL and SDA GPIOs respectively.
+//
+// Current compatibility:
+// zc706: GPIO 50, 51 == SCL, SDA
+// kasli_soc: GPIO 50, 51 == SCL, SDA; GPIO 33 == I2C_SW_RESET
+// ebaz4205: GPIO (EMIO)
+
+pub struct RegisterBlock {
+    pub gpio_output_mask: &'static mut GPIOOutputMask,
+    pub gpio_input: &'static mut GPIOInput,
+    pub gpio_direction: &'static mut GPIODirection,
+    pub gpio_output_enable: &'static mut GPIOOutputEnable,
+    #[cfg(feature = "target_kasli_soc")]
+    pub gpio_output_mask_lower: &'static mut GPIOOutputMaskLower,
+}
+
+impl RegisterBlock {
+    pub fn i2c() -> Self {
+        Self {
+            gpio_output_mask: GPIOOutputMask::new(),
+            gpio_input: GPIOInput::new(),
+            gpio_direction: GPIODirection::new(),
+            gpio_output_enable: GPIOOutputEnable::new(),
+            #[cfg(feature = "target_kasli_soc")]
+            gpio_output_mask_lower: GPIOOutputMaskLower::new(),
+        }
+    }
+}
+
+register!(gpio_output_mask,
+          /// MASK_DATA_1_MSW:
+          /// Maskable output data for MIO[53:48]
+          GPIOOutputMask, RW, u32);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_at!(GPIOOutputMask, 0xE000A00C, new);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_bit!(gpio_output_mask,
+              /// Output for SCL
+              scl_o, 2);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_bit!(gpio_output_mask,
+              /// Output for SDA
+              sda_o, 3);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_bits!(gpio_output_mask,
+               /// Mask for keeping bits except SCL and SDA unchanged
+               mask, u16, 16, 31);
+
+
+register!(gpio_output_mask_lower,
+            /// MASK_DATA_1_LSW:
+            /// Maskable output data for MIO[47:32]
+            GPIOOutputMaskLower, RW, u32);
+#[cfg(feature = "target_kasli_soc")]
+register_at!(GPIOOutputMaskLower, 0xE000A008, new);
+#[cfg(feature = "target_kasli_soc")]
+register_bit!(gpio_output_mask_lower,
+              /// Output for I2C_SW_RESET (MIO[33])
+              i2cswr_o, 1);
+#[cfg(feature = "target_kasli_soc")]
+register_bits!(gpio_output_mask_lower,
+               mask, u16, 16, 31);
+
+register!(gpio_input,
+          /// DATA_1_RO:
+          /// Input data for MIO[53:32]
+          GPIOInput, RO, u32);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_at!(GPIOInput, 0xE000A064, new);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_bit!(gpio_input,
+              /// Input for SCL
+              scl, 18);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_bit!(gpio_input,
+              /// Input for SDA
+              sda, 19);
+
+
+register!(gpio_direction,
+          /// DIRM_1:
+          /// Direction mode for MIO[53:32]; 0/1 = in/out
+          GPIODirection, RW, u32);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_at!(GPIODirection, 0xE000A244, new);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_bit!(gpio_direction,
+              /// Direction for SCL
+              scl, 18);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_bit!(gpio_direction,
+              /// Direction for SDA
+              sda, 19);
+#[cfg(feature = "target_kasli_soc")]
+register_bit!(gpio_direction,
+              /// Direction for I2C_SW_RESET
+              i2cswr, 1);
+
+register!(gpio_output_enable,
+          /// OEN_1:
+          /// Output enable for MIO[53:32]
+          GPIOOutputEnable, RW, u32);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_at!(GPIOOutputEnable, 0xE000A248, new);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_bit!(gpio_output_enable,
+              /// Output enable for SCL
+              scl, 18);
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+register_bit!(gpio_output_enable,
+              /// Output enable for SDA
+              sda, 19);
+#[cfg(feature = "target_kasli_soc")]
+register_bit!(gpio_output_enable,
+              /// Output enable for I2C_SW_RESET
+              i2cswr, 1);
+              
--- a/libboard_zynq/src/lib.rs
+++ b/libboard_zynq/src/lib.rs
@ -15,37 +15,13 @@ pub mod axi_hp;
 pub mod axi_gp;
 pub mod ddr;
 pub mod mpcore;
-pub mod flash;
-pub mod dmac;
+pub mod gic;
 pub mod time;
 pub mod timer;
 pub mod sdio;
+#[cfg(any(feature = "target_zc706", feature = "target_kasli_soc", feature = "target_ebaz4205"))]
+pub mod i2c;
 pub mod logger;
-
-pub use libcortex_a9::pl310::L2Cache;
-
-pub fn l2cache() -> L2Cache {
-    const PL310_BASEADDR: usize = 0xF8F02000;
-    L2Cache::new(PL310_BASEADDR)
-}
-
-pub fn setup_l2cache() {
-    slcr::RegisterBlock::unlocked(|slcr| {
-        assert_eq!(&slcr.unnamed1 as *const _ as u32, 0xF8000A1C);
-        unsafe { slcr.unnamed1.write(0x020202); }
-    });
-    
-    let mut l2 = l2cache();
-    use log::info;
-    info!("l2 aux={:08X}", l2.regs.aux_control.read());
-    // TODO: set prefetch
-
-    // Configure ZYNQ-specific latency
-    l2.set_tag_ram_latencies(1, 1, 1);
-    l2.set_data_ram_latencies(1, 2, 1);
-
-    l2.disable_interrupts();
-    l2.reset_interrupts();
-    l2.invalidate_all();
-    l2.enable();
-}
+pub mod ps7_init;
+#[cfg(feature="target_kasli_soc")]
+pub mod error_led;
--- a/libboard_zynq/src/logger.rs
+++ b/libboard_zynq/src/logger.rs
@ -19,7 +19,7 @@ impl log::Log for Logger {
        if self.enabled(record.metadata()) {
            let timestamp = unsafe {
                GlobalTimer::get()
-            }.get_us();
+            }.get_us().0;
            let seconds   = timestamp / 1_000_000;
            let micros    = timestamp % 1_000_000;

--- a/libboard_zynq/src/mpcore.rs
+++ b/libboard_zynq/src/mpcore.rs
@ -8,47 +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>,
-    reserved2: [u32; 42],
-    pub iccicr: RW<u32>,
-    pub iccpmw: RW<u32>,
-    pub iccbpr: RW<u32>,
-    pub icciar: RW<u32>,
-    pub icceoir: RW<u32>,
-    pub iccrpr: RW<u32>,
-    pub icchpir: RW<u32>,
-    pub iccabpr: RW<u32>,
-    reserved3: [u32; 55],
-    pub iccidr: RW<u32>,
+    unused0: [u32; 12],
+    /// Filtering Start Address Register
+    pub filtering_start_address: FilteringStartAddressRegister,
+    /// Defined by FILTEREND input
+    pub filtering_end_address: FilteringEndAddressRegister,
+    unused1: [u32; 2],
+    /// SCU Access Control (SAC) Register
+    pub scu_access_control_sac: SCUAccessControlRegisterSAC,
+    /// SCU Non-secure Access Control Register SNSAC
+    pub scu_non_secure_access_control: SCUNonSecureAccessControlRegister,
+    unused2: [u32; 42],
+    /// CPU Interface Control Register
+    pub iccicr: ICCICR,
+    /// Interrupt Priority Mask Register
+    pub iccpmr: ICCPMR,
+    /// Binary Point Register
+    pub iccbpr: ICCBPR,
+    /// Interrupt Acknowledge Register
+    pub icciar: ICCIAR,
+    /// End Of Interrupt Register
+    pub icceoir: ICCEOIR,
+    /// Running Priority Register
+    pub iccrpr: ICCRPR,
+    /// Highest Pending Interrupt Register
+    pub icchpir: ICCHPIR,
+    /// Aliased Non-secure Binary Point Register
+    pub iccabpr: ICCABPR,
+    unused3: [u32; 55],
+    /// CPU Interface Implementer Identification Register
+    pub iccidr: ICCIDR,
+    /// Global Timer Counter Register 0
    pub global_timer_counter0: ValueRegister,
    pub global_timer_counter1: ValueRegister,
+    /// Global Timer Control Register
    pub global_timer_control: GlobalTimerControl,
-    pub global_timer_interrupt_status: RW<u32>,
+    /// Global Timer Interrupt Status Register
+    pub global_timer_interrupt_status: GlobalTimerInterruptStatusRegister,
+    /// Comparator Value Register_0
    pub comparator_value0: ValueRegister,
    pub comparator_value1: ValueRegister,
-    pub auto_increment: ValueRegister,
-    reserved4: [u32; 249],
-    pub private_timer_load: ValueRegister,
-    pub private_timer_counter: ValueRegister,
-    pub private_timer_control: RW<u32>,
-    pub private_timer_interrupt_status: RW<u32>,
-    reserved5: [u32; 4],
-    pub watchdog_load: ValueRegister,
-    pub watchdog_counter: ValueRegister,
-    pub watchdog_control: RW<u32>,
-    pub watchdog_interrupt_status: RW<u32>,
-    // there is plenty more (unimplemented)
+    /// Auto-increment Register
+    pub auto_increment: RW<u32>,
+    unused4: [u32; 249],
+    /// Private Timer Load Register
+    pub private_timer_load: RW<u32>,
+    /// Private Timer Counter Register
+    pub private_timer_counter: RW<u32>,
+    /// Private Timer Control Register
+    pub private_timer_control: PrivateTimerControlRegister,
+    /// Private Timer Interrupt Status Register
+    pub private_timer_interrupt_status: PrivateTimerInterruptStatusRegister,
+    unused5: [u32; 4],
+    /// Watchdog Load Register
+    pub watchdog_load: RW<u32>,
+    /// Watchdog Counter Register
+    pub watchdog_counter: RW<u32>,
+    /// Watchdog Control Register
+    pub watchdog_control: WatchdogControlRegister,
+    /// Watchdog Interrupt Status Register
+    pub watchdog_interrupt_status: WatchdogInterruptStatusRegister,
+    /// Watchdog Reset Status Register
+    pub watchdog_reset_status: WatchdogResetStatusRegister,
+    /// Watchdog Disable Register
+    pub watchdog_disable: RW<u32>,
+    unused6: [u32; 626],
+    /// Distributor Control Register
+    pub icddcr: ICDDCR,
+    /// Interrupt Controller Type Register
+    pub icdictr: ICDICTR,
+    /// Distributor Implementer Identification Register
+    pub icdiidr: ICDIIDR,
+    unused7: [u32; 29],
+    /// Interrupt Security Register
+    pub icdisr0: RW<u32>,
+    pub icdisr1: RW<u32>,
+    pub icdisr2: RW<u32>,
+    unused8: [u32; 29],
+    /// Interrupt Set-enable Registers
+    pub icdiser: [RW<u32>; 3],
+    unused9: [u32; 29],
+    /// Interrupt Clear-Enable Register 0
+    pub icdicer0: RW<u32>,
+    /// Interrupt Clear-Enable Register 1
+    pub icdicer1: RW<u32>,
+    /// Interrupt Clear-Enable Register 2
+    pub icdicer2: RW<u32>,
+    unused10: [u32; 29],
+    /// Interrupt Set-pending Register
+    pub icdispr0: RW<u32>,
+    pub icdispr1: RW<u32>,
+    pub icdispr2: RW<u32>,
+    unused11: [u32; 29],
+    /// Interrupt Clear-Pending Register
+    pub icdicpr0: RW<u32>,
+    pub icdicpr1: RW<u32>,
+    pub icdicpr2: RW<u32>,
+    unused12: [u32; 29],
+    /// Active Bit register
+    pub icdabr0: RW<u32>,
+    pub icdabr1: RW<u32>,
+    pub icdabr2: RW<u32>,
+    unused13: [u32; 61],
+    /// Interrupt Priority Register
+    pub icdipr: [RW<u32>; 24],
+    unused14: [u32; 232],
+    /// Interrupt Processor Targets Registers
+    pub icdiptr: [RW<u32>; 24],
+    unused15: [u32; 232],
+    /// Interrupt Configuration Registers
+    pub icdicfr: [RW<u32>; 6],
+    unused16: [u32; 58],
+    /// PPI Status Register
+    pub ppi_status: PpiStatus,
+    /// SPI Status Register 0
+    pub spi_status_0: RO<u32>,
+    /// SPI Status Register 1
+    pub spi_status_1: RO<u32>,
+    unused17: [u32; 125],
+    /// Software Generated Interrupt Register
+    pub icdsgir: ICDSGIR,
 }
-register_at!(RegisterBlock, 0xF8F00000, new);
+
+register_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);
@ -61,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);
@ -88,8 +192,71 @@ impl ScuInvalidate {
    }
 }

-register!(value_register, ValueRegister, RW, u32);
-register_bits!(value_register, value, u32, 0, 31);
+register!(filtering_start_address, FilteringStartAddressRegister, RW, u32);
+register_bits!(filtering_start_address, filtering_start_address, u32, 20, 31);
+register_bits!(filtering_start_address, sbz, u32, 0, 19);
+
+register!(filtering_end_address, FilteringEndAddressRegister, RW, u32);
+register_bits!(filtering_end_address, filtering_end_address, u32, 20, 31);
+register_bits!(filtering_end_address, sbz, u32, 0, 19);
+
+register!(scu_access_control_sac, SCUAccessControlRegisterSAC, RW, u32);
+register_bit!(scu_access_control_sac, cp_u3, 3);
+register_bit!(scu_access_control_sac, cp_u2, 2);
+register_bit!(scu_access_control_sac, cp_u1, 1);
+register_bit!(scu_access_control_sac, cp_u0, 0);
+
+register!(scu_non_secure_access_control, SCUNonSecureAccessControlRegister, RO, u32);
+register_bits!(scu_non_secure_access_control, sbz, u32, 12, 31);
+register_bit!(scu_non_secure_access_control, cpu3_global_timer, 11);
+register_bit!(scu_non_secure_access_control, cpu2_global_timer, 10);
+register_bit!(scu_non_secure_access_control, cpu1_global_timer, 9);
+register_bit!(scu_non_secure_access_control, cpu0_global_timer, 8);
+register_bit!(scu_non_secure_access_control, private_timers_for_cpu3, 7);
+register_bit!(scu_non_secure_access_control, private_timers_for_cpu2, 6);
+register_bit!(scu_non_secure_access_control, private_timers_for_cpu1, 5);
+register_bit!(scu_non_secure_access_control, private_timers_for_cpu0, 4);
+register_bit!(scu_non_secure_access_control, component_access_for_cpu3, 3);
+register_bit!(scu_non_secure_access_control, component_access_for_cpu2, 2);
+register_bit!(scu_non_secure_access_control, component_access_for_cpu1, 1);
+register_bit!(scu_non_secure_access_control, component_access_for_cpu0, 0);
+
+register!(iccicr, ICCICR, RW, u32);
+register_bit!(iccicr, sbpr, 4);
+register_bit!(iccicr, fiq_en, 3);
+register_bit!(iccicr, ack_ctl, 2);
+register_bit!(iccicr, enable_ns, 1);
+register_bit!(iccicr, enable_s, 0);
+
+register!(iccpmr, ICCPMR, RW, u32);
+register_bits!(iccpmr, priority, u8, 0, 7);
+
+register!(iccbpr, ICCBPR, RW, u32);
+register_bits!(iccbpr, binary_point, u8, 0, 2);
+
+register!(icciar, ICCIAR, RW, u32);
+register_bits!(icciar, cpuid, u8, 10, 12);
+register_bits!(icciar, ackintid, u32, 0, 9);
+
+register!(icceoir, ICCEOIR, RW, u32);
+register_bits!(icceoir, cpuid, u8, 10, 12);
+register_bits!(icceoir, eoiintid, u32, 0, 9);
+
+register!(iccrpr, ICCRPR, RW, u32);
+register_bits!(iccrpr, priority, u8, 0, 7);
+
+register!(icchpir, ICCHPIR, RW, u32);
+register_bits!(icchpir, cpuid, u8, 10, 12);
+register_bits!(icchpir, pendintid, u32, 0, 9);
+
+register!(iccabpr, ICCABPR, RW, u32);
+register_bits!(iccabpr, binary_point, u8, 0, 2);
+
+register!(iccidr, ICCIDR, RO, u32);
+register_bits!(iccidr, part_number, u32, 20, 31);
+register_bits!(iccidr, architecture_version, u8, 16, 19);
+register_bits!(iccidr, revision_number, u8, 12, 15);
+register_bits!(iccidr, implementer, u32, 0, 11);

 register!(global_timer_control, GlobalTimerControl, RW, u32);
 register_bits!(global_timer_control, prescaler, u8, 8, 15);
@ -97,3 +264,58 @@ register_bit!(global_timer_control, auto_increment_mode, 3);
 register_bit!(global_timer_control, irq_enable, 2);
 register_bit!(global_timer_control, comp_enablea, 1);
 register_bit!(global_timer_control, timer_enable, 0);
+
+register!(global_timer_interrupt_status, GlobalTimerInterruptStatusRegister, RW, u32);
+register_bit!(global_timer_interrupt_status, event_flag, 0);
+
+register!(private_timer_control, PrivateTimerControlRegister, RW, u32);
+register_bits!(private_timer_control, sbzp, u32, 16, 31);
+register_bits!(private_timer_control, prescaler, u8, 8, 15);
+register_bits!(private_timer_control, unk_sbzp, u8, 3, 7);
+register_bit!(private_timer_control, irq_enable, 2);
+register_bit!(private_timer_control, auto_reload, 1);
+register_bit!(private_timer_control, timer_enable, 0);
+
+register!(private_timer_interrupt_status, PrivateTimerInterruptStatusRegister, RW, u32);
+register_bits!(private_timer_interrupt_status, unk_sbzp, u32, 1, 31);
+
+register!(watchdog_control, WatchdogControlRegister, RW, u32);
+register_bits!(watchdog_control, prescaler, u8, 8, 15);
+register_bit!(watchdog_control, watchdog_mode, 3);
+register_bit!(watchdog_control, it_enable, 2);
+register_bit!(watchdog_control, auto_reload, 1);
+register_bit!(watchdog_control, watchdog_enable, 0);
+
+register!(watchdog_interrupt_status, WatchdogInterruptStatusRegister, RW, u32);
+register_bit!(watchdog_interrupt_status, event_flag, 0);
+
+register!(watchdog_reset_status, WatchdogResetStatusRegister, RW, u32);
+register_bit!(watchdog_reset_status, reset_flag, 0);
+
+register!(icddcr, ICDDCR, RW, u32);
+register_bit!(icddcr, enable_non_secure, 1);
+register_bit!(icddcr, enable_secure, 0);
+
+register!(icdictr, ICDICTR, RO, u32);
+register_bits!(icdictr, lspi, u8, 11, 15);
+register_bit!(icdictr, security_extn, 10);
+register_bits!(icdictr, sbz, u8, 8, 9);
+register_bits!(icdictr, cpu_number, u8, 5, 7);
+register_bits!(icdictr, it_lines_number, u8, 0, 4);
+
+register!(icdiidr, ICDIIDR, RO, u32);
+register_bits!(icdiidr, implementation_version, u8, 24, 31);
+register_bits!(icdiidr, revision_number, u32, 12, 23);
+register_bits!(icdiidr, implementer, u32, 0, 11);
+
+register!(ppi_status, PpiStatus, RO, u32);
+register_bits!(ppi_status, ppi_status, u8, 11, 15);
+register_bits!(ppi_status, sbz, u32, 0, 10);
+
+register!(icdsgir, ICDSGIR, RW, u32);
+register_bits!(icdsgir, target_list_filter, u8, 24, 25);
+register_bits!(icdsgir, cpu_target_list, u8, 16, 23);
+register_bit!(icdsgir, satt, 15);
+register_bits!(icdsgir, sbz, u32, 4, 14);
+register_bits!(icdsgir, sgiintid, u8, 0, 3);
+
--- a/libboard_zynq/src/ps7_init/mod.rs
+++ b/libboard_zynq/src/ps7_init/mod.rs
@ -0,0 +1,108 @@
+use crate::println;
+
+#[cfg(feature = "target_zc706")]
+mod zc706;
+#[cfg(not(feature = "target_zc706"))]
+mod none;
+
+#[cfg(feature = "target_zc706")]
+use zc706 as target;
+#[cfg(not(feature = "target_zc706"))]
+use none as target;
+
+pub fn report_differences() {
+    for (i, op) in target::INIT_DATA.iter().enumerate() {
+        let address = op.address();
+        let overwritten_later = target::INIT_DATA[(i + 1)..].iter()
+            .any(|later_op| later_op.address() == address);
+
+        if !overwritten_later {
+            op.report_difference();
+        }
+    }
+}
+
+pub fn apply() {
+    for op in target::INIT_DATA {
+        op.apply();
+    }
+}
+
+#[derive(Clone, Debug)]
+pub enum InitOp {
+    MaskWrite(usize, usize, usize),
+    MaskPoll(usize, usize),
+    MaskDelay(usize, usize),
+}
+
+impl InitOp {
+    fn address(&self) -> usize {
+        match self {
+            InitOp::MaskWrite(address, _, _) => *address,
+            InitOp::MaskPoll(address, _) => *address,
+            InitOp::MaskDelay(address, _) => *address,
+        }
+    }
+
+    fn read(&self) -> usize {
+        unsafe { *(self.address() as *const usize) }
+    }
+
+    fn difference(&self) -> Option<(usize, usize)> {
+        let expected = match self {
+            InitOp::MaskWrite(_, mask, expected) =>
+                Some((*mask, *expected)),
+            InitOp::MaskPoll(_, mask) =>
+                Some((*mask, *mask)),
+            _ => None,
+        };
+        match expected {
+            Some((mask, expected)) => {
+                let actual = self.read();
+                if actual & mask == expected {
+                    None
+                } else {
+                    Some((actual & mask, expected))
+                }
+            }
+            None =>
+                None
+        }
+    }
+
+    pub fn report_difference(&self) {
+        if let Some((actual, expected)) = self.difference() {
+            println!(
+                "Register {:08X} is {:08X}&={:08X} != {:08X} expected",
+                self.address(),
+                self.read(),
+                actual,
+                expected
+            );
+        }
+    }
+
+    pub fn apply(&self) {
+        let reg = self.address() as *mut usize;
+        println!("apply {:?}", self);
+        match self {
+            InitOp::MaskWrite(_, mask, val) =>
+                unsafe {
+                    *reg = (val & mask) | (*reg & !mask);
+                },
+            InitOp::MaskPoll(_, mask) =>
+                while unsafe { *reg } & mask == 0 {},
+            InitOp::MaskDelay(_, mask) => {
+                let delay = get_number_of_cycles_for_delay(*mask);
+                while unsafe { *reg } < delay {
+                    println!("W");
+                }
+            }
+        }
+    }
+}
+
+fn get_number_of_cycles_for_delay(delay: usize) -> usize {
+    const APU_FREQ: usize = 666666687;
+    APU_FREQ * delay/ (2 * 1000)
+}
--- a/libboard_zynq/src/ps7_init/none.rs
+++ b/libboard_zynq/src/ps7_init/none.rs
@ -0,0 +1,4 @@
+use super::InitOp;
+
+pub const INIT_DATA: &'static [InitOp] = &[
+];
--- a/libboard_zynq/src/ps7_init/zc706.rs
+++ b/libboard_zynq/src/ps7_init/zc706.rs
--- a/libboard_zynq/src/sdio/adma.rs
+++ b/libboard_zynq/src/sdio/adma.rs
@ -1,6 +1,6 @@
 /// ADMA library
 use core::mem::MaybeUninit;
-use super::SDIO;
+use super::Sdio;
 use libcortex_a9::cache;
 use libregister::{
    register, register_bit,
@ -32,7 +32,7 @@ impl Adma2DescTable {
    }

    /// Initialize the table and setup `adma_system_address`
-    pub fn setup(&mut self, sdio: &mut SDIO, blk_cnt: u32, buffer: &[u8]) {
+    pub fn setup(&mut self, sdio: &mut Sdio, blk_cnt: u32, buffer: &[u8]) {
        let descr_table = &mut self.0;
        let blk_size = sdio
            .regs
--- a/libboard_zynq/src/sdio/mod.rs
+++ b/libboard_zynq/src/sdio/mod.rs
@ -12,9 +12,9 @@ use log::{trace, debug};
 use nb;

 /// Basic SDIO Struct with common low-level functions.
-pub struct SDIO {
+pub struct Sdio {
    regs: &'static mut regs::RegisterBlock,
-    count_down: super::timer::global::CountDown,
+    count_down: super::timer::global::CountDown<Milliseconds>,
    input_clk_hz: u32,
    card_type: CardType,
    card_detect: bool,
@ -48,7 +48,7 @@ pub enum CardType {
    CardMmc,
 }

-impl SDIO {
+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)
@ -105,7 +105,7 @@ impl SDIO {
                );
            }
            // cora card detect pin
-            #[cfg(feature = "target_cora_z7_10")]
+            #[cfg(feature = "target_coraz7")]
            {
                unsafe {
                    slcr.sd0_wp_cd_sel.write(47 << 16);
@ -116,12 +116,38 @@ impl SDIO {
                        .speed(true),
                );
            }
+            // kasli_soc and redpitaya card detect pin
+            #[cfg(any(feature = "target_kasli_soc", feature = "target_redpitaya"))]
+            {
+                unsafe {
+                    slcr.sd0_wp_cd_sel.write(46 << 16);
+                }
+                slcr.mio_pin_46.write(
+                    slcr::MioPin46::zeroed()
+                        .io_type(slcr::IoBufferType::Lvcmos25)
+                        .speed(true),
+                );
+            }
+            // ebaz4205 card detect pin
+            #[cfg(feature = "target_ebaz4205")]
+            {
+                unsafe {
+                    slcr.sd0_wp_cd_sel.write(34 << 16);
+                }
+                slcr.mio_pin_34.write(
+                    slcr::MioPin34::zeroed()
+                        .io_type(slcr::IoBufferType::Lvcmos33)
+                        .pullup(true)
+                        .speed(true),
+                );
+            }
+
            slcr.sdio_rst_ctrl.reset_sdio0();
            slcr.aper_clk_ctrl.enable_sdio0();
            slcr.sdio_clk_ctrl.enable_sdio0();
        });
        let clocks = Clocks::get();
-        let mut self_ = SDIO {
+        let mut self_ = Sdio {
            regs: regs::RegisterBlock::sdio0(),
            count_down: unsafe { super::timer::GlobalTimer::get() }.countdown(),
            input_clk_hz: clocks.sdio_ref_clk(),
--- a/libboard_zynq/src/sdio/sd_card.rs
+++ b/libboard_zynq/src/sdio/sd_card.rs
@ -1,4 +1,4 @@
-use super::{adma::Adma2DescTable, cmd, CardType, CmdTransferError, SDIO};
+use super::{adma::Adma2DescTable, cmd, CardType, CmdTransferError, Sdio};
 use libcortex_a9::cache;
 use libregister::{RegisterR, RegisterRW, RegisterW};
 use log::{trace, debug};
@ -37,7 +37,7 @@ enum CardVersion {
 }

 pub struct SdCard {
-    sdio: SDIO,
+    sdio: Sdio,
    adma2_desc_table: Adma2DescTable,
    card_version: CardVersion,
    hcs: bool,
@ -171,8 +171,8 @@ impl SdCard {
        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> {
+    /// 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),
@ -192,8 +192,8 @@ impl SdCard {
        Ok(_self)
    }

-    /// Convert SdCard struct back to SDIO struct.
-    pub fn to_sdio(self) -> SDIO {
+    /// Convert SdCard struct back to Sdio struct.
+    pub fn to_sdio(self) -> Sdio {
        self.sdio
    }

--- a/libboard_zynq/src/slcr.rs
+++ b/libboard_zynq/src/slcr.rs
@ -9,9 +9,11 @@ use libregister::{

 #[repr(u8)]
 pub enum PllSource {
-    IoPll  = 0b00,
-    ArmPll = 0b10,
-    DdrPll = 0b11,
+    IoPll  = 0b000,
+    ArmPll = 0b010,
+    DdrPll = 0b011,
+    // Ethernet controller 0 EMIO clock
+    Emio   = 0b100,
 }

 #[repr(u8)]
@ -102,19 +104,19 @@ pub struct RegisterBlock {
    pub dbg_clk_ctrl: RW<u32>,
    pub pcap_clk_ctrl: RW<u32>,
    pub topsw_clk_ctrl: RW<u32>,
-    pub fpga0_clk_ctrl: RW<u32>,
+    pub fpga0_clk_ctrl: Fpga0ClkCtrl,
    pub fpga0_thr_ctrl: RW<u32>,
    pub fpga0_thr_cnt: RW<u32>,
    pub fpga0_thr_sta: RO<u32>,
-    pub fpga1_clk_ctrl: RW<u32>,
+    pub fpga1_clk_ctrl: Fpga1ClkCtrl,
    pub fpga1_thr_ctrl: RW<u32>,
    pub fpga1_thr_cnt: RW<u32>,
    pub fpga1_thr_sta: RO<u32>,
-    pub fpga2_clk_ctrl: RW<u32>,
+    pub fpga2_clk_ctrl: Fpga2ClkCtrl,
    pub fpga2_thr_ctrl: RW<u32>,
    pub fpga2_thr_cnt: RW<u32>,
    pub fpga2_thr_sta: RO<u32>,
-    pub fpga3_clk_ctrl: RW<u32>,
+    pub fpga3_clk_ctrl: Fpga3ClkCtrl,
    pub fpga3_thr_ctrl: RW<u32>,
    pub fpga3_thr_cnt: RW<u32>,
    pub fpga3_thr_sta: RO<u32>,
@ -132,7 +134,7 @@ pub struct RegisterBlock {
    pub can_rst_ctrl: RW<u32>,
    pub i2c_rst_ctrl: RW<u32>,
    pub uart_rst_ctrl: UartRstCtrl,
-    pub gpio_rst_ctrl: RW<u32>,
+    pub gpio_rst_ctrl: GpioRstCtrl,
    pub lqspi_rst_ctrl: LqspiRstCtrl,
    pub smc_rst_ctrl: RW<u32>,
    pub ocm_rst_ctrl: RW<u32>,
@ -229,18 +231,15 @@ pub struct RegisterBlock {
    pub lvl_shftr_en: LvlShftr,
    reserved18: [u32; 3],
    pub ocm_cfg: RW<u32>,
-    reserved19: [u32; 66],
-    /// barely documented unnamed register to prepare L2 cache setup
-    pub unnamed1: RW<u32>,
-    reserved120: [u32; 56],
+    reserved19: [u32; 123],
    pub gpiob_ctrl: GpiobCtrl,
    pub gpiob_cfg_cmos18: RW<u32>,
    pub gpiob_cfg_cmos25: RW<u32>,
    pub gpiob_cfg_cmos33: RW<u32>,
-    reserved21: [u32; 1],
+    reserved20: [u32; 1],
    pub gpiob_cfg_hstl: RW<u32>,
    pub gpiob_drvr_bias_ctrl: RW<u32>,
-    reserved22: [u32; 9],
+    reserved21: [u32; 9],
    pub ddriob_addr0: DdriobConfig,
    pub ddriob_addr1: DdriobConfig,
    pub ddriob_data0: DdriobConfig,
@ -256,26 +255,18 @@ 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(
@ -542,6 +533,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);
@ -551,6 +556,26 @@ 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);
@ -564,22 +589,36 @@ register_bit!(a9_cpu_rst_ctrl, a9_clkstop0, 4);
 register_bit!(a9_cpu_rst_ctrl, a9_rst1, 1);
 register_bit!(a9_cpu_rst_ctrl, a9_rst0, 0);

+pub fn reboot() {
+    RegisterBlock::unlocked(|slcr| {
+        unsafe {
+            let reboot = slcr.reboot_status.read();
+            slcr.reboot_status.write(reboot & 0xF0FFFFFF);
+            slcr.pss_rst_ctrl.modify(|_, w| w.soft_rst(true));
+        }
+    });
+}
+
+
 #[derive(Clone, Copy, Debug, PartialEq)]
 #[repr(u8)]
 pub enum BootModePins {
+    // CAUTION!
+    // The BOOT_MODE bits table 6-4 in UG585 are *out of order*.
    Jtag = 0b000,
-    Nor = 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);
+register_bit!(pss_rst_ctrl, soft_rst, 0);

 /// Used for MioPin*.io_type
 #[repr(u8)]
--- 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,14 @@ 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_coraz7", feature = "target_redpitaya"))]
+                let uart = Uart::uart0(UART_RATE);
+                #[cfg(any(
+                    feature = "target_zc706",
+                    feature = "target_ebaz4205",
+                    feature = "target_kasli_soc",
+                ))]
+                let uart = Uart::uart1(UART_RATE);
                *self = LazyUart::Initialized(uart);
                self
            }
--- a/libboard_zynq/src/time.rs
+++ b/libboard_zynq/src/time.rs
@ -8,3 +8,18 @@ impl core::ops::Add for Milliseconds {
        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
@ -1,9 +1,10 @@
+use core::ops::Add;
 use void::Void;
 use libregister::{RegisterR, RegisterW};
 use crate::{
    clocks::Clocks,
    mpcore,
-    time::Milliseconds,
+    time::{Milliseconds, Microseconds, TimeSource},
 };

 /// "uptime"
@ -15,13 +16,13 @@ pub struct GlobalTimer {
 impl GlobalTimer {
    /// Get the potentially uninitialized timer
    pub unsafe fn get() -> GlobalTimer {
-        let regs = mpcore::RegisterBlock::new();
+        let regs = mpcore::RegisterBlock::mpcore();
        GlobalTimer { regs }
    }

    /// Get the timer with a reset
    pub fn start() -> GlobalTimer {
-        let mut regs = mpcore::RegisterBlock::new();
+        let mut regs = mpcore::RegisterBlock::mpcore();
        Self::reset(&mut regs);
        GlobalTimer { regs }
    }
@ -79,41 +80,91 @@ impl GlobalTimer {
    }

    /// read with high precision
-    pub fn get_us(&self) -> u64 {
+    pub fn get_us(&self) -> Microseconds {
        let prescaler = self.regs.global_timer_control.read().prescaler() as u64;
        let clocks = Clocks::get();

-        1_000_000 * self.get_counter() * (prescaler + 1) / clocks.cpu_3x2x() as u64
+        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(&self) -> CountDown {
+    pub fn countdown<U>(&self) -> CountDown<U>
+    where
+        Self: TimeSource<U>,
+    {
        CountDown {
            timer: self.clone(),
-            timeout: Milliseconds(0),
+            timeout: self.now(),
        }
    }
 }

-#[derive(Clone)]
-pub struct CountDown {
-    timer: GlobalTimer,
-    timeout: Milliseconds,
+impl TimeSource<Milliseconds> for GlobalTimer {
+    fn now(&self) -> Milliseconds {
+        self.get_time()
+    }
 }

-impl embedded_hal::timer::CountDown for CountDown {
-    type Time = Milliseconds;
+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.get_time() + count.into();
+        self.timeout = self.timer.now() + count.into();
    }

    fn wait(&mut self) -> nb::Result<(), Void> {
-        if self.timer.get_time() < self.timeout {
+        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/uart/mod.rs
+++ b/libboard_zynq/src/uart/mod.rs
@ -13,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_coraz7", feature = "target_redpitaya"))]
+    pub fn uart0(baudrate: u32) -> Self {
        slcr::RegisterBlock::unlocked(|slcr| {
            // Route UART 0 RxD/TxD Signals to MIO Pins
            // TX pin
@ -56,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();
@ -72,7 +46,60 @@ impl Uart {
        self_
    }

+    #[cfg(any(feature = "target_zc706", feature = "target_kasli_soc"))]
    pub fn uart1(baudrate: u32) -> Self {
+        slcr::RegisterBlock::unlocked(|slcr| {
+            // Route UART 1 RxD/TxD Signals to MIO Pins
+            // TX pin
+            slcr.mio_pin_48.write(
+                slcr::MioPin48::zeroed()
+                    .l3_sel(0b111)
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+            );
+            // RX pin
+            slcr.mio_pin_49.write(
+                slcr::MioPin49::zeroed()
+                    .tri_enable(true)
+                    .l3_sel(0b111)
+                    .io_type(slcr::IoBufferType::Lvcmos18)
+                    .pullup(true)
+            );
+        });
+
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.uart_rst_ctrl.reset_uart1();
+            slcr.aper_clk_ctrl.enable_uart1();
+            slcr.uart_clk_ctrl.enable_uart1();
+        });
+        let mut self_ = Uart {
+            regs: regs::RegisterBlock::uart1(),
+        };
+        self_.configure(baudrate);
+        self_
+    }
+
+    #[cfg(feature = "target_ebaz4205")]
+    pub fn uart1(baudrate: u32) -> Self {
+        slcr::RegisterBlock::unlocked(|slcr| {
+            // Route UART 1 RxD/TxD Signals to MIO Pins
+            // TX pin
+            slcr.mio_pin_24.write(
+                slcr::MioPin24::zeroed()
+                    .l3_sel(0b111)
+                    .io_type(slcr::IoBufferType::Lvcmos33)
+                    .pullup(true)
+            );
+            // RX pin
+            slcr.mio_pin_25.write(
+                slcr::MioPin25::zeroed()
+                    .tri_enable(true)
+                    .l3_sel(0b111)
+                    .io_type(slcr::IoBufferType::Lvcmos33)
+                    .pullup(true)
+            );
+        });
+
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.uart_rst_ctrl.reset_uart1();
            slcr.aper_clk_ctrl.enable_uart1();
--- a/libconfig/Cargo.toml
+++ b/libconfig/Cargo.toml
@ -0,0 +1,29 @@
+[package]
+name = "libconfig"
+version = "0.1.0"
+authors = ["M-Labs"]
+edition = "2018"
+
+[dependencies]
+libboard_zynq = { path = "../libboard_zynq" }
+log = "0.4"
+
+[dependencies.core_io]
+git = "https://git.m-labs.hk/M-Labs/rs-core_io.git"
+rev = "e9d3edf027"
+features = ["collections"]
+
+[dependencies.fatfs]
+git = "https://git.m-labs.hk/M-Labs/rust-fatfs.git"
+rev = "4b5e420084"
+default-features = false
+features = ["core_io"]
+
+[features]
+target_zc706 = []
+target_coraz7 = []
+target_ebaz4205 = []
+target_redpitaya = []
+target_kasli_soc = []
+ipv6 = []
+fat_lfn = [ "fatfs/alloc" ]
--- a/libconfig/src/bootgen.rs
+++ b/libconfig/src/bootgen.rs
@ -0,0 +1,181 @@
+use alloc::vec::Vec;
+use core_io::{Error, Read, Seek, SeekFrom};
+use libboard_zynq::devc;
+use log::debug;
+
+#[derive(Debug)]
+pub enum BootgenLoadingError {
+    InvalidBootImageHeader,
+    MissingPartition,
+    EncryptedBitstream,
+    IoError(Error),
+    DevcError(devc::DevcError),
+}
+
+impl From<Error> for BootgenLoadingError {
+    fn from(error: Error) -> Self {
+        BootgenLoadingError::IoError(error)
+    }
+}
+
+impl From<devc::DevcError> for BootgenLoadingError {
+    fn from(error: devc::DevcError) -> Self {
+        BootgenLoadingError::DevcError(error)
+    }
+}
+
+impl core::fmt::Display for BootgenLoadingError {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        use BootgenLoadingError::*;
+        match self {
+            InvalidBootImageHeader => write!(
+                f,
+                "Invalid boot image header. Check if the file is correct."
+            ),
+            MissingPartition => write!(f, "Partition not found. Check your compile configuration."),
+            EncryptedBitstream => write!(f, "Encrypted bitstream is not supported."),
+            IoError(e) => write!(f, "Error while reading: {}", e),
+            DevcError(e) => write!(f, "PCAP interface error: {}", e),
+        }
+    }
+}
+
+#[repr(C)]
+struct PartitionHeader {
+    pub encrypted_length: u32,
+    pub unencrypted_length: u32,
+    pub word_length: u32,
+    pub dest_load_addr: u32,
+    pub dest_exec_addr: u32,
+    pub data_offset: u32,
+    pub attribute_bits: u32,
+    pub section_count: u32,
+    pub checksum_offset: u32,
+    pub header_offset: u32,
+    pub cert_offset: u32,
+    pub reserved: [u32; 4],
+    pub checksum: u32,
+}
+
+/// Read a u32 word from the reader.
+fn read_u32<Reader: Read>(reader: &mut Reader) -> Result<u32, BootgenLoadingError> {
+    let mut buffer: [u8; 4] = [0; 4];
+    reader.read_exact(&mut buffer)?;
+    let mut result: u32 = 0;
+    for i in 0..4 {
+        result |= (buffer[i] as u32) << (i * 8);
+    }
+    Ok(result)
+}
+
+/// Load PL partition header.
+fn load_pl_header<File: Read + Seek>(
+    file: &mut File,
+) -> Result<Option<PartitionHeader>, BootgenLoadingError> {
+    let mut buffer: [u8; 0x40] = [0; 0x40];
+    file.read_exact(&mut buffer)?;
+    let header = unsafe { core::mem::transmute::<_, PartitionHeader>(buffer) };
+    if header.attribute_bits & (2 << 4) != 0 {
+        Ok(Some(header))
+    } else {
+        Ok(None)
+    }
+}
+
+fn load_ps_header<File: Read + Seek>(
+    file: &mut File,
+) -> Result<Option<PartitionHeader>, BootgenLoadingError> {
+    let mut buffer: [u8; 0x40] = [0; 0x40];
+    file.read_exact(&mut buffer)?;
+    let header = unsafe { core::mem::transmute::<_, PartitionHeader>(buffer) };
+    if header.attribute_bits & (1 << 4) != 0 {
+        Ok(Some(header))
+    } else {
+        Ok(None)
+    }
+}
+
+/// Locate the partition from the image, and return the size (in bytes) of the partition if successful.
+/// This function would seek the file to the location of the partition.
+fn locate<
+    File: Read + Seek,
+    F: Fn(&mut File) -> Result<Option<PartitionHeader>, BootgenLoadingError>,
+>(
+    file: &mut File,
+    f: F,
+) -> Result<usize, BootgenLoadingError> {
+    file.seek(SeekFrom::Start(0))?;
+    const BOOT_HEADER_SIGN: u32 = 0x584C4E58;
+    // read boot header signature
+    file.seek(SeekFrom::Start(0x24))?;
+    if read_u32(file)? != BOOT_HEADER_SIGN {
+        return Err(BootgenLoadingError::InvalidBootImageHeader);
+    }
+    // find fsbl offset
+    file.seek(SeekFrom::Start(0x30))?;
+    // the length is in bytes, we have to convert it to words to compare with the partition offset
+    // later
+    let fsbl = read_u32(file)? / 4;
+    // read partition header offset
+    file.seek(SeekFrom::Start(0x9C))?;
+    let ptr = read_u32(file)?;
+    debug!("Partition header pointer = {:0X}", ptr);
+    file.seek(SeekFrom::Start(ptr as u64))?;
+
+    // at most 3 partition headers
+    for _ in 0..3 {
+        if let Some(header) = f(file)? {
+            let encrypted_length = header.encrypted_length;
+            let unencrypted_length = header.unencrypted_length;
+            debug!("Unencrypted length = {:0X}", unencrypted_length);
+            if encrypted_length != unencrypted_length {
+                return Err(BootgenLoadingError::EncryptedBitstream);
+            }
+
+            let start_addr = header.data_offset;
+            // skip fsbl
+            if start_addr == fsbl {
+                continue;
+            }
+            debug!("Partition start address: {:0X}", start_addr);
+            file.seek(SeekFrom::Start(start_addr as u64 * 4))?;
+
+            return Ok(unencrypted_length as usize * 4);
+        }
+    }
+    Err(BootgenLoadingError::MissingPartition)
+}
+
+/// Load bitstream from bootgen file.
+/// This function parses the file, locate the bitstream and load it through the PCAP driver.
+/// It requires a large buffer, please enable the DDR RAM before using it.
+pub fn load_bitstream<File: Read + Seek>(file: &mut File) -> Result<(), BootgenLoadingError> {
+    let size = locate(file, load_pl_header)?;
+    unsafe {
+        // align to 64 bytes
+        let ptr = alloc::alloc::alloc(alloc::alloc::Layout::from_size_align(size, 64).unwrap());
+        let buffer = core::slice::from_raw_parts_mut(ptr, size);
+        file.read_exact(buffer).map_err(|e| {
+            core::ptr::drop_in_place(ptr);
+            e
+        })?;
+        let mut devcfg = devc::DevC::new();
+        devcfg.enable();
+        devcfg.program(&buffer).map_err(|e| {
+            core::ptr::drop_in_place(ptr);
+            e
+        })?;
+        core::ptr::drop_in_place(ptr);
+        Ok(())
+    }
+}
+
+pub fn get_runtime<File: Read + Seek>(file: &mut File) -> Result<Vec<u8>, BootgenLoadingError> {
+    let size = locate(file, load_ps_header)?;
+    let mut buffer = Vec::with_capacity(size);
+    unsafe {
+        buffer.set_len(size);
+    }
+    file.read_exact(&mut buffer)?;
+    Ok(buffer)
+}
--- a/libconfig/src/lib.rs
+++ b/libconfig/src/lib.rs
@ -0,0 +1,174 @@
+#![no_std]
+extern crate alloc;
+
+use core::fmt;
+use alloc::{string::FromUtf8Error, string::String, vec::Vec, rc::Rc};
+use core_io::{self as io, BufRead, BufReader, Read, Write, Seek, SeekFrom};
+use libboard_zynq::sdio;
+
+pub mod sd_reader;
+pub mod net_settings;
+pub mod bootgen;
+
+#[derive(Debug)]
+pub enum Error<'a> {
+    SdError(sdio::sd_card::CardInitializationError),
+    IoError(io::Error),
+    Utf8Error(FromUtf8Error),
+    KeyNotFoundError(&'a str),
+    NoConfig,
+}
+
+pub type Result<'a, T> = core::result::Result<T, Error<'a>>;
+
+impl<'a> fmt::Display for Error<'a> {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        match self {
+            Error::SdError(error) => write!(f, "SD error: {}", error),
+            Error::IoError(error) => write!(f, "I/O error: {}", error),
+            Error::Utf8Error(error) => write!(f, "UTF-8 error: {}", error),
+            Error::KeyNotFoundError(name) => write!(f, "Configuration key `{}` not found", name),
+            Error::NoConfig => write!(f, "Configuration not present"),
+        }
+    }
+}
+
+impl<'a> From<sdio::sd_card::CardInitializationError> for Error<'a> {
+    fn from(error: sdio::sd_card::CardInitializationError) -> Self {
+        Error::SdError(error)
+    }
+}
+
+impl<'a> From<io::Error> for Error<'a> {
+    fn from(error: io::Error) -> Self {
+        Error::IoError(error)
+    }
+}
+
+impl<'a> From<FromUtf8Error> for Error<'a> {
+    fn from(error: FromUtf8Error) -> Self {
+        Error::Utf8Error(error)
+    }
+}
+
+fn parse_config<'a>(
+    key: &'a str,
+    buffer: &mut Vec<u8>,
+    file: fatfs::File<sd_reader::SdReader>,
+) -> Result<'a, ()> {
+    let prefix = [key, "="].concat().to_ascii_lowercase();
+    for line in BufReader::new(file).lines() {
+        let line = line?.to_ascii_lowercase();
+        if line.starts_with(&prefix) {
+            buffer.extend(line[prefix.len()..].as_bytes());
+            return Ok(());
+        }
+    }
+    Err(Error::KeyNotFoundError(key))
+}
+
+pub struct Config {
+    fs: Option<Rc<fatfs::FileSystem<sd_reader::SdReader>>>,
+}
+
+const NEWLINE: &[u8] = b"\n";
+
+impl Config {
+    pub fn new() -> Result<'static, Self> {
+        let sdio = sdio::Sdio::sdio0(true);
+        if !sdio.is_card_inserted() {
+            Err(sdio::sd_card::CardInitializationError::NoCardInserted)?;
+        }
+        let sd = sdio::sd_card::SdCard::from_sdio(sdio)?;
+        let reader = sd_reader::SdReader::new(sd);
+
+        let fs = reader.mount_fatfs(sd_reader::PartitionEntry::Entry1)?;
+        Ok(Config { fs: Some(Rc::new(fs)) })
+    }
+
+    pub fn from_fs(fs: Option<Rc<fatfs::FileSystem<sd_reader::SdReader>>>) -> Self {
+        Config { fs }
+    }
+
+    pub fn new_dummy() -> Self {
+        Config { fs: None }
+    }
+
+    pub fn read<'b>(&self, key: &'b str) -> Result<'b, Vec<u8>> {
+        if let Some(fs) = &self.fs {
+            let root_dir = fs.root_dir();
+            let mut buffer: Vec<u8> = Vec::new();
+            match root_dir.open_file(&["/CONFIG/", key, ".BIN"].concat()) {
+                Ok(mut f) => f.read_to_end(&mut buffer).map(|_| ())?,
+                Err(_) => match root_dir.open_file("/CONFIG.TXT") {
+                    Ok(f) => parse_config(key, &mut buffer, f)?,
+                    Err(_) => return Err(Error::KeyNotFoundError(key)),
+                },
+            };
+            Ok(buffer)
+        } else {
+            Err(Error::NoConfig)
+        }
+    }
+
+    pub fn read_str<'b>(&self, key: &'b str) -> Result<'b, String> {
+        Ok(String::from_utf8(self.read(key)?)?)
+    }
+
+    pub fn remove<'b>(&self, key: &'b str) -> Result<'b, ()> {
+        if let Some(fs) = &self.fs {
+            let root_dir = fs.root_dir();
+            match root_dir.remove(&["/CONFIG/", key, ".BIN"].concat()) {
+                Ok(()) => Ok(()),
+                Err(_) => {
+                    let prefix = [key, "="].concat().to_ascii_lowercase();
+                    match root_dir.create_file("/CONFIG.TXT") {
+                        Ok(mut f) => {
+                            let mut buffer = String::new();
+                            f.read_to_string(&mut buffer)?;
+                            f.seek(SeekFrom::Start(0))?;
+                            f.truncate()?;
+                            for line in buffer.lines() {
+                                if line.len() > 0 && !line.to_ascii_lowercase().starts_with(&prefix) {
+                                    f.write(line.as_bytes())?;
+                                    f.write(NEWLINE)?;
+                                }
+                            }
+                            Ok(())
+                        },
+                        Err(_) => Err(Error::KeyNotFoundError(key))
+                    }
+                }
+            }
+        } else {
+            Err(Error::NoConfig)
+        }
+    }
+
+    pub fn write<'b>(&self, key: &'b str, value: Vec<u8>) -> Result<'b, ()> {
+        if self.fs.is_none() {
+            return Err(Error::NoConfig);
+        }
+        let fs = self.fs.as_ref().unwrap();
+        let root_dir = fs.root_dir();
+        let is_str = value.len() <= 100 && value.is_ascii() && !value.contains(&b'\n');
+        if key == "boot" {
+            let mut f = root_dir.create_file("/BOOT.BIN")?;
+            f.truncate()?;
+            f.write_all(&value)?;
+            drop(f);
+        } else {
+            let _ = self.remove(key);
+            if is_str {
+                let mut f = root_dir.create_file("/CONFIG.TXT")?;
+                f.seek(SeekFrom::End(0))?;
+                write!(f, "{}={}\n", key, String::from_utf8(value).unwrap())?;
+            } else {
+                let dir = root_dir.create_dir("/CONFIG")?;
+                let mut f = dir.create_file(&[key, ".BIN"].concat())?;
+                f.write_all(&value)?;
+            }
+        }
+        Ok(())
+    }
+}
--- a/libconfig/src/net_settings.rs
+++ b/libconfig/src/net_settings.rs
@ -0,0 +1,92 @@
+use core::fmt;
+
+use libboard_zynq::smoltcp::wire::{EthernetAddress, IpAddress};
+
+use super::Config;
+
+pub struct NetAddresses {
+    pub hardware_addr: EthernetAddress,
+    pub ipv4_addr: IpAddress,
+    #[cfg(feature = "ipv6")]
+    pub ipv6_ll_addr: IpAddress,
+    #[cfg(feature = "ipv6")]
+    pub ipv6_addr: Option<IpAddress>
+}
+
+impl fmt::Display for NetAddresses {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(f, "MAC={} IPv4={} ",
+            self.hardware_addr, self.ipv4_addr)?;
+
+        #[cfg(feature = "ipv6")]
+        {
+            write!(f, "IPv6-LL={}", self.ipv6_ll_addr)?;
+            match self.ipv6_addr {
+                Some(addr) => write!(f, " {}", addr)?,
+                None => write!(f, " IPv6: no configured address")?
+            }
+        }
+        Ok(())
+    }
+}
+
+#[cfg(feature = "target_kasli_soc")]
+fn get_address_from_eeprom() -> EthernetAddress {
+    use libboard_zynq::i2c::{I2c, eeprom};
+
+    let mut i2c = I2c::i2c0();
+    i2c.init().unwrap();
+    let mut eeprom = eeprom::EEPROM::new(&mut i2c, 16);
+    let address = eeprom.read_eui48().unwrap_or([0x02, 0x00, 0x00, 0x00, 0x00, 0x56]);
+
+    EthernetAddress(address)
+}
+
+pub fn get_addresses(cfg: &Config) -> NetAddresses {
+    #[cfg(feature = "target_zc706")]
+    let mut hardware_addr = EthernetAddress([0x02, 0x00, 0x00, 0x00, 0x00, 0x52]);
+    #[cfg(feature = "target_zc706")]
+    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 52);
+    #[cfg(feature = "target_coraz7")]
+    let mut hardware_addr = EthernetAddress([0x02, 0x00, 0x00, 0x00, 0x00, 0x54]);
+    #[cfg(feature = "target_coraz7")]
+    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 54);
+    #[cfg(feature = "target_redpitaya")]
+    let mut hardware_addr = EthernetAddress([0x02, 0x00, 0x00, 0x00, 0x00, 0x55]);
+    #[cfg(feature = "target_redpitaya")]
+    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 55);
+    #[cfg(feature = "target_kasli_soc")]
+    let mut hardware_addr = get_address_from_eeprom();
+    #[cfg(feature = "target_kasli_soc")]
+    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 56);
+    #[cfg(feature = "target_ebaz4205")]
+    let mut hardware_addr = EthernetAddress([0x02, 0x00, 0x00, 0x00, 0x00, 0x57]);
+    #[cfg(feature = "target_ebaz4205")]
+    let mut ipv4_addr = IpAddress::v4(192, 168, 1, 57);
+
+    if let Ok(Ok(addr)) = cfg.read_str("mac").map(|s| s.parse()) {
+        hardware_addr = addr;
+    }
+    if let Ok(Ok(addr)) = cfg.read_str("ip").map(|s| s.parse()) {
+        ipv4_addr = addr;
+    }
+    #[cfg(feature = "ipv6")]
+    let ipv6_addr = cfg.read_str("ip6").ok().and_then(|s| s.parse().ok());
+
+    #[cfg(feature = "ipv6")]
+    let ipv6_ll_addr = IpAddress::v6(
+        0xfe80, 0x0000, 0x0000, 0x0000,
+        (((hardware_addr.0[0] ^ 0x02) as u16) << 8) | (hardware_addr.0[1] as u16),
+        ((hardware_addr.0[2] as u16) << 8) | 0x00ff,
+        0xfe00 | (hardware_addr.0[3] as u16),
+        ((hardware_addr.0[4] as u16) << 8) | (hardware_addr.0[5] as u16));
+
+    NetAddresses {
+        hardware_addr,
+        ipv4_addr,
+        #[cfg(feature = "ipv6")]
+        ipv6_ll_addr,
+        #[cfg(feature = "ipv6")]
+        ipv6_addr
+    }
+}
--- a/libconfig/src/sd_reader.rs
+++ b/libconfig/src/sd_reader.rs
@ -0,0 +1,304 @@
+use core_io::{BufRead, Error, ErrorKind, Read, Result as IoResult, Seek, SeekFrom, Write};
+use fatfs;
+use libboard_zynq::sdio::{sd_card::SdCard, CmdTransferError};
+use log::debug;
+use alloc::vec::Vec;
+
+const MBR_SIGNATURE: [u8; 2] = [0x55, 0xAA];
+const PARTID_FAT12: u8 = 0x01;
+const PARTID_FAT16_LESS32M: u8 = 0x04;
+const PARTID_FAT16: u8 = 0x06;
+const PARTID_FAT32: u8 = 0x0B;
+const PARTID_FAT32_LBA: u8 = 0x0C;
+const PARTID_FAT16_LBA: u8 = 0x0E;
+
+fn cmd_error_to_io_error(_: CmdTransferError) -> Error {
+    Error::new(ErrorKind::Other, "Command transfer error")
+}
+
+const BLOCK_SIZE: usize = 512;
+
+/// SdReader struct implementing `Read + BufRead + Write + Seek` traits for `core_io`.
+/// Used as an adaptor for fatfs crate, but could be used directly for raw data access.
+///
+/// Implementation: all read/writes would be split into unaligned and block-aligned parts,
+/// unaligned read/writes would do a buffered read/write using a block-sized internal buffer,
+/// while aligned transactions would be sent to the SD card directly for performance reason.
+pub struct SdReader {
+    /// Internal SdCard handle.
+    sd: SdCard,
+    /// Read buffer with the size of 1 block.
+    buffer: Vec<u8>,
+    /// Address for the next byte.
+    byte_addr: u32,
+    /// Internal index for the next byte.
+    /// Normally in range `[0, BLOCK_SIZE - 1]`.
+    ///
+    /// `index = BLOCK_SIZE` means that the `buffer` is invalid for the current `byte_addr`,
+    /// the next `fill_buf` call would fill the buffer.
+    index: usize,
+    /// Dirty flag indicating the content has to be flushed.
+    dirty: bool,
+    /// Base offset for translation from logical address to physical address.
+    offset: u32,
+}
+
+#[derive(Copy, Clone)]
+#[allow(unused)]
+// Partition entry enum, normally we would use entry1.
+pub enum PartitionEntry {
+    Entry1 = 0x1BE,
+    Entry2 = 0x1CE,
+    Entry3 = 0x1DE,
+    Entry4 = 0x1EE,
+}
+
+impl SdReader {
+    /// Create SdReader from SdCard
+    pub fn new(sd: SdCard) -> SdReader {
+        let mut vec: Vec<u8> = Vec::with_capacity(BLOCK_SIZE);
+        unsafe {
+            vec.set_len(vec.capacity());
+        }
+        SdReader {
+            sd,
+            buffer: vec,
+            byte_addr: 0,
+            index: BLOCK_SIZE,
+            dirty: false,
+            offset: 0,
+        }
+    }
+
+    /// Internal read function for unaligned read.
+    /// The read must not cross block boundary.
+    fn read_unaligned(&mut self, buf: &mut [u8]) -> IoResult<usize> {
+        if buf.len() == 0 {
+            return Ok(0);
+        }
+        let filled_buffer = self.fill_buf()?;
+        for (dest, src) in buf.iter_mut().zip(filled_buffer.iter()) {
+            *dest = *src;
+        }
+        self.consume(buf.len());
+        Ok(buf.len())
+    }
+
+    /// Internal write function for unaligned write.
+    /// The write must not cross block boundary.
+    fn write_unaligned(&mut self, buf: &[u8]) -> IoResult<usize> {
+        if buf.len() == 0 {
+            return Ok(0);
+        }
+        // update buffer if needed, as we will flush the entire block later.
+        self.fill_buf()?;
+        self.dirty = true;
+        let dest_buffer = &mut self.buffer[self.index..];
+        for (src, dest) in buf.iter().zip(dest_buffer.iter_mut()) {
+            *dest = *src;
+        }
+        self.consume(buf.len());
+        Ok(buf.len())
+    }
+
+    /// Split the slice into three segments, with the middle block-aligned.
+    /// Alignment depends on the current `self.byte_addr` instead of the slice pointer address
+    fn block_align<'b>(&self, buf: &'b [u8]) -> (&'b [u8], &'b [u8], &'b [u8]) {
+        let head_len = BLOCK_SIZE - (self.byte_addr as usize % BLOCK_SIZE);
+        if head_len > buf.len() {
+            (buf, &[], &[])
+        } else {
+            let remaining_length = buf.len() - head_len;
+            let mid_length = remaining_length - remaining_length % BLOCK_SIZE;
+            let (head, remaining) = buf.split_at(head_len);
+            let (mid, tail) = remaining.split_at(mid_length);
+            (head, mid, tail)
+        }
+    }
+
+    /// Split the mutable slice into three segments, with the middle block-aligned.
+    /// Alignment depends on the current `self.byte_addr` instead of the slice pointer address
+    fn block_align_mut<'b>(&self, buf: &'b mut [u8]) -> (&'b mut [u8], &'b mut [u8], &'b mut [u8]) {
+        let head_len = BLOCK_SIZE - (self.byte_addr as usize % BLOCK_SIZE);
+        if head_len > buf.len() {
+            (buf, &mut [], &mut [])
+        } else {
+            let remaining_length = buf.len() - head_len;
+            let mid_length = remaining_length - remaining_length % BLOCK_SIZE;
+            let (head, remaining) = buf.split_at_mut(head_len);
+            let (mid, tail) = remaining.split_at_mut(mid_length);
+            (head, mid, tail)
+        }
+    }
+
+    /// Invalidate the buffer, so later unaligned read/write would reload the buffer from SD card.
+    fn invalidate_buffer(&mut self) {
+        self.index = BLOCK_SIZE;
+    }
+
+    /// Set the base offset of the SD card, to transform from physical address to logical address.
+    fn set_base_offset(&mut self, offset: u32) -> IoResult<u64> {
+        self.offset = offset;
+        self.seek(SeekFrom::Start(0))
+    }
+
+    /// Mount fatfs from partition entry, and return the fatfs object if success.
+    /// This takes the ownership of self, so currently there is no way to recover from an error,
+    /// except creating a new SD card instance.
+    pub fn mount_fatfs(mut self, entry: PartitionEntry) -> IoResult<fatfs::FileSystem<Self>> {
+        let mut buffer: [u8; 4] = [0; 4];
+        self.seek(SeekFrom::Start(0x1FE))?;
+        self.read_exact(&mut buffer[..2])?;
+        // check MBR signature
+        if buffer[..2] != MBR_SIGNATURE {
+            return Err(Error::new(
+                ErrorKind::InvalidData,
+                "Incorrect signature for MBR sector.",
+            ));
+        }
+        // Read partition ID.
+        self.seek(SeekFrom::Start(entry as u64 + 0x4))?;
+        self.read_exact(&mut buffer[..1])?;
+        debug!("Partition ID: {:0X}", buffer[0]);
+        match buffer[0] {
+            PARTID_FAT12 | PARTID_FAT16_LESS32M | PARTID_FAT16 |
+            PARTID_FAT16_LBA | PARTID_FAT32 | PARTID_FAT32_LBA => {}
+            _ => {
+                return Err(Error::new(
+                    ErrorKind::InvalidData,
+                    "No FAT partition found for the specified entry.",
+                ));
+            }
+        }
+        // Read LBA
+        self.seek(SeekFrom::Current(0x3))?;
+        self.read_exact(&mut buffer)?;
+        let mut lba: u32 = 0;
+        // Little endian
+        for i in 0..4 {
+            lba |= (buffer[i] as u32) << (i * 8);
+        }
+        // Set to logical address
+        self.set_base_offset(lba * BLOCK_SIZE as u32)?;
+        // setup fatfs
+        fatfs::FileSystem::new(self, fatfs::FsOptions::new())
+    }
+}
+
+impl Read for SdReader {
+    fn read(&mut self, buf: &mut [u8]) -> IoResult<usize> {
+        let total_length = buf.len();
+        let (a, b, c) = self.block_align_mut(buf);
+        self.read_unaligned(a)?;
+        if b.len() > 0 {
+            // invalidate internal buffer
+            self.invalidate_buffer();
+            if let Err(_) = self.sd.read_block(
+                self.byte_addr / BLOCK_SIZE as u32,
+                (b.len() / BLOCK_SIZE) as u16,
+                b,
+            ) {
+                // we have to allow partial read, as per the trait required
+                return Ok(a.len());
+            }
+            self.byte_addr += b.len() as u32;
+        }
+        if let Err(_) = self.read_unaligned(c) {
+            // we have to allow partial read, as per the trait required
+            return Ok(a.len() + b.len());
+        }
+        Ok(total_length)
+    }
+}
+
+impl BufRead for SdReader {
+    fn fill_buf(&mut self) -> IoResult<&[u8]> {
+        if self.index == BLOCK_SIZE {
+            // flush the buffer if it is dirty before overwriting it with new data
+            if self.dirty {
+                self.flush()?;
+            }
+            // reload buffer
+            self.sd
+                .read_block(self.byte_addr / (BLOCK_SIZE as u32), 1, &mut self.buffer)
+                .map_err(cmd_error_to_io_error)?;
+            self.index = (self.byte_addr as usize) % BLOCK_SIZE;
+        }
+        Ok(&self.buffer[self.index..])
+    }
+
+    fn consume(&mut self, amt: usize) {
+        self.index += amt;
+        self.byte_addr += amt as u32;
+    }
+}
+
+impl Write for SdReader {
+    fn write(&mut self, buf: &[u8]) -> IoResult<usize> {
+        let (a, b, c) = self.block_align(buf);
+        self.write_unaligned(a)?;
+        if b.len() > 0 {
+            self.flush()?;
+            self.invalidate_buffer();
+            if let Err(_) = self.sd.write_block(
+                self.byte_addr / BLOCK_SIZE as u32,
+                (b.len() / BLOCK_SIZE) as u16,
+                b,
+            ) {
+                return Ok(a.len());
+            }
+            self.byte_addr += b.len() as u32;
+        }
+        if let Err(_) = self.write_unaligned(c) {
+            return Ok(a.len() + b.len());
+        }
+        Ok(buf.len())
+    }
+
+    fn flush(&mut self) -> IoResult<()> {
+        if self.dirty {
+            let block_addr = (self.byte_addr - self.index as u32) / (BLOCK_SIZE as u32);
+            self.sd
+                .write_block(block_addr, 1, &self.buffer)
+                .map_err(cmd_error_to_io_error)?;
+            self.dirty = false;
+        }
+        Ok(())
+    }
+}
+
+impl Seek for SdReader {
+    fn seek(&mut self, pos: SeekFrom) -> IoResult<u64> {
+        let raw_target = match pos {
+            SeekFrom::Start(x) => self.offset as i64 + x as i64,
+            SeekFrom::Current(x) => self.byte_addr as i64 + x,
+            SeekFrom::End(_) => panic!("SD card does not support seek from end"),
+        };
+        if raw_target < self.offset as i64 || raw_target > core::u32::MAX as i64 {
+            return Err(Error::new(ErrorKind::InvalidInput, "Invalid address"));
+        }
+        let target_byte_addr = raw_target as u32;
+        let address_same_block =
+            self.byte_addr / (BLOCK_SIZE as u32) == target_byte_addr / (BLOCK_SIZE as u32);
+        // if the buffer was invalidated, we consider seek as different block
+        let same_block = address_same_block && self.index != BLOCK_SIZE;
+        if !same_block {
+            self.flush()?;
+        }
+        self.byte_addr = target_byte_addr;
+        self.index = if same_block {
+            target_byte_addr as usize % BLOCK_SIZE
+        } else {
+            // invalidate the buffer as we moved to a different block
+            BLOCK_SIZE
+        };
+        Ok((self.byte_addr - self.offset) as u64)
+    }
+}
+
+impl Drop for SdReader {
+    fn drop(&mut self) {
+        // just try to flush it, ignore error if any
+        self.flush().unwrap_or(());
+    }
+}
--- a/libcortex_a9/Cargo.toml
+++ b/libcortex_a9/Cargo.toml
@ -1,13 +1,12 @@
 [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 = []
-default = ["target_zc706"]
+power_saving = []
+default = []

 [dependencies]
 bit_field = "0.10"
--- a/libcortex_a9/src/asm.rs
+++ b/libcortex_a9/src/asm.rs
@ -1,35 +1,83 @@
+use core::arch::asm;
+
 /// The classic no-op
 #[inline]
 pub fn nop() {
-    unsafe { llvm_asm!("nop" :::: "volatile") }
+    unsafe { asm!("nop") }
 }

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

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

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

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

 /// Instruction Synchronization Barrier
 #[inline]
 pub fn isb() {
-    unsafe { llvm_asm!("isb" :::: "volatile") }
+    unsafe { asm!("isb") }
+}
+
+/// Enable FIQ
+#[inline]
+pub unsafe fn enable_fiq() {
+    asm!("cpsie f");
+}
+
+/// Enable IRQ
+#[inline]
+pub unsafe fn enable_irq() {
+    asm!("cpsie i");
+}
+
+/// Disable IRQ, return if IRQ was originally enabled.
+#[inline]
+pub unsafe fn enter_critical() -> bool {
+    let mut cpsr: u32;
+    asm!(
+        "mrs {}, cpsr
+         cpsid i", lateout(reg) cpsr);
+    (cpsr & (1 << 7)) == 0
+}
+
+#[inline]
+pub unsafe fn exit_critical(enable: bool) {
+    // https://stackoverflow.com/questions/40019929/temporarily-disable-interrupts-on-arm
+    let mask: u32 = if enable {
+        1 << 7
+    } else {
+        0
+    };
+    asm!(
+        "mrs r1, cpsr
+         bic r1, r1, {}
+         msr cpsr_c, r1"
+         , in(reg) mask, out("r1") _);
+}
+
+/// Exiting IRQ
+#[inline]
+pub unsafe fn exit_irq() {
+    asm!("
+        mrs r0, SPSR
+        msr CPSR, r0
+        ", out("r0") _);
 }
--- a/libcortex_a9/src/cache.rs
+++ b/libcortex_a9/src/cache.rs
@ -1,10 +1,12 @@
 use super::asm::{dmb, dsb};
+use super::l2c::*;
+use core::arch::asm;

 /// Invalidate TLBs
 #[inline(always)]
 pub fn tlbiall() {
    unsafe {
-        llvm_asm!("mcr p15, 0, $0, c8, c7, 0" :: "r" (0) :: "volatile");
+        asm!("mcr p15, 0, {}, c8, c7, 0", in(reg) 0);
    }
 }

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

@ -20,7 +22,7 @@ pub fn iciallu() {
 #[inline(always)]
 pub fn bpiall() {
    unsafe {
-        llvm_asm!("mcr p15, 0, $0, c7, c5, 6" :: "r" (0) :: "volatile");
+        asm!("mcr p15, 0, {}, c7, c5, 6", in(reg) 0);
    }
 }

@ -28,7 +30,7 @@ pub fn bpiall() {
 #[inline(always)]
 pub fn dccsw(setway: u32) {
    unsafe {
-        llvm_asm!("mcr p15, 0, $0, c7, c10, 2" :: "r" (setway) :: "volatile");
+        asm!("mcr p15, 0, {}, c7, c10, 2", in(reg) setway);
    }
 }

@ -40,7 +42,7 @@ 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.
-        llvm_asm!("mcr p15, 0, $0, c7, c6, 2" :: "r" (setway) :: "volatile");
+        asm!("mcr p15, 0, {}, c7, c6, 2", in(reg) setway);
    }
 }

@ -48,14 +50,13 @@ pub fn dcisw(setway: u32) {
 #[inline(always)]
 pub fn dccisw(setway: u32) {
    unsafe {
-        llvm_asm!("mcr p15, 0, $0, c7, c14, 2" :: "r" (setway) :: "volatile");
+        asm!("mcr p15, 0, {}, c7, c14, 2", in(reg) setway);
    }
 }

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

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

    // Invalidate entire D-Cache by iterating every set and every way
@ -80,9 +81,17 @@ pub fn dciall() {
    }
 }

+/// 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() {
+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;
@ -96,7 +105,7 @@ pub fn dcciall() {

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

    // Invalidate entire D-Cache by iterating every set and every way
@ -107,6 +116,15 @@ pub fn dcciall() {
    }
 }

+#[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;
@ -139,13 +157,22 @@ fn slice_cache_line_addrs<T>(slice: &[T]) -> impl Iterator<Item = usize> {
 #[inline(always)]
 pub fn dccimvac(addr: usize) {
    unsafe {
-        llvm_asm!("mcr p15, 0, $0, c7, c14, 1" :: "r" (addr) :: "volatile");
+        asm!("mcr p15, 0, {}, c7, c14, 1", in(reg) addr);
    }
 }

 /// Data cache clean and invalidate for an object.
 pub fn dcci<T>(object: &T) {
+    // ref: L2C310 TRM 3.3.10
    dmb();
+    for addr in object_cache_line_addrs(object) {
+        dccmvac(addr);
+    }
+    dsb();
+    for addr in object_cache_line_addrs(object) {
+        l2_cache_clean_invalidate(addr);
+    }
+    l2_cache_sync();
    for addr in object_cache_line_addrs(object) {
        dccimvac(addr);
    }
@ -154,6 +181,14 @@ pub fn dcci<T>(object: &T) {

 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);
    }
@ -164,10 +199,9 @@ pub fn dcci_slice<T>(slice: &[T]) {
 #[inline(always)]
 pub fn dccmvac(addr: usize) {
    unsafe {
-        llvm_asm!("mcr p15, 0, $0, c7, c10, 1" :: "r" (addr) :: "volatile");
+        asm!("mcr p15, 0, {}, c7, c10, 1", in(reg) addr);
    }
 }
-
 /// Data cache clean for an object.
 pub fn dcc<T>(object: &T) {
    dmb();
@ -175,17 +209,28 @@ pub fn dcc<T>(object: &T) {
        dccmvac(addr);
    }
    dsb();
+    for addr in object_cache_line_addrs(object) {
+        l2_cache_clean(addr);
+    }
+    l2_cache_sync();
 }

 /// Data cache clean for an object. Panics if not properly
 /// aligned and properly sized to be contained in an exact number of
 /// cache lines.
 pub fn dcc_slice<T>(slice: &[T]) {
+    if slice.len() == 0 {
+        return;
+    }
    dmb();
    for addr in slice_cache_line_addrs(slice) {
        dccmvac(addr);
    }
    dsb();
+    for addr in slice_cache_line_addrs(slice) {
+        l2_cache_clean(addr);
+    }
+    l2_cache_sync();
 }

 /// Data cache invalidate by memory virtual address. This and
@ -194,10 +239,10 @@ pub fn dcc_slice<T>(slice: &[T]) {
 /// affecting more data than intended.
 #[inline(always)]
 pub unsafe fn dcimvac(addr: usize) {
-    llvm_asm!("mcr p15, 0, $0, c7, c6, 1" :: "r" (addr) :: "volatile");
+    asm!("mcr p15, 0, {}, c7, c6, 1", in(reg) addr);
 }

-/// Data cache invalidate for an object.
+/// 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);
@ -205,6 +250,10 @@ pub unsafe fn dci<T>(object: &mut T) {
    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);
    }
@ -219,6 +268,10 @@ pub unsafe fn dci_slice<T>(slice: &mut [T]) {
    assert_eq!(beyond_addr & CACHE_LINE_MASK, 0, "dci slice beyond_addr must be aligned");

    dmb();
+    for addr in (first_addr..beyond_addr).step_by(CACHE_LINE) {
+        l2_cache_invalidate(addr);
+    }
+    l2_cache_sync();
    for addr in (first_addr..beyond_addr).step_by(CACHE_LINE) {
        dcimvac(addr);
    }
--- a/libcortex_a9/src/fpu.rs
+++ b/libcortex_a9/src/fpu.rs
@ -0,0 +1,15 @@
+use core::arch::asm;
+/// Enable FPU in the current core.
+pub fn enable_fpu() {
+    unsafe {
+        asm!("
+            mrc p15, 0, r1, c1, c0, 2
+            orr r1, r1, (0b1111<<20)
+            mcr p15, 0, r1, c1, c0, 2
+
+            vmrs r1, fpexc
+            orr r1, r1, (1<<30)
+            vmsr fpexc, r1
+            ", out("r1") _);
+    }
+}
--- a/libcortex_a9/src/l2c.rs
+++ b/libcortex_a9/src/l2c.rs
@ -0,0 +1,333 @@
+use libregister::{register, register_at, register_bit, register_bits, RegisterRW, RegisterR, RegisterW};
+use super::asm::dmb;
+use volatile_register::RW;
+
+/// enable L2 cache with specific prefetch offset
+/// prefetch offset requires manual tuning, it seems that 8 is good for ZC706 current settings
+pub fn enable_l2_cache(offset: u8) {
+    dmb();
+    let regs = RegisterBlock::new();
+    // disable L2 cache
+    regs.reg1_control.modify(|_, w| w.l2_enable(false));
+
+    regs.reg15_prefetch_ctrl.modify(|_, w|
+        w.instr_prefetch_en(true)
+            .data_prefetch_en(true)
+            .double_linefill_en(true)
+            .incr_double_linefill_en(true)
+            .pref_drop_en(true)
+            .prefetch_offset(offset)
+    );
+    regs.reg1_aux_control.modify(|_, w| {
+        w.early_bresp_en(true)
+            .instr_prefetch_en(true)
+            .data_prefetch_en(true)
+            .cache_replace_policy(true)
+            .way_size(3)
+    });
+    regs.reg1_tag_ram_control.modify(|_, w| w.ram_wr_access_lat(1).ram_rd_access_lat(1).ram_setup_lat(1));
+    regs.reg1_data_ram_control.modify(|_, w| w.ram_wr_access_lat(1).ram_rd_access_lat(2).ram_setup_lat(1));
+    // invalidate L2 ways
+    unsafe {
+        regs.reg7_inv_way.write(0xFFFF);
+    }
+    // poll for completion
+    while regs.reg7_cache_sync.read().c() {}
+    // write to a magic memory location with a magic sequence
+    // required in UG585 Section 3.4.10 Initialization Sequence
+    unsafe {
+        core::ptr::write_volatile(0xF8000008usize as *mut u32, 0xDF0D);
+        core::ptr::write_volatile(0xF8000A1Cusize as *mut u32, 0x020202);
+        core::ptr::write_volatile(0xF8000004usize as *mut u32, 0x767B);
+    }
+    regs.reg1_control.modify(|_, w| w.l2_enable(true));
+    dmb();
+}
+
+#[inline(always)]
+pub fn l2_cache_invalidate_all()  {
+    let regs = RegisterBlock::new();
+    unsafe {
+        regs.reg7_inv_way.write(0xFFFF);
+    }
+    // poll for completion
+    while regs.reg7_cache_sync.read().c() {}
+}
+
+#[inline(always)]
+pub fn l2_cache_clean_all()  {
+    let regs = RegisterBlock::new();
+    unsafe  {
+        regs.reg7_clean_way.write(0xFFFF);
+    }
+    // poll for completion
+    while regs.reg7_cache_sync.read().c() {}
+}
+
+#[inline(always)]
+pub fn l2_cache_clean_invalidate_all()  {
+    let regs = RegisterBlock::new();
+    unsafe {
+        regs.reg7_clean_inv_way.write(0xFFFF);
+    }
+    // poll for completion
+    while regs.reg7_cache_sync.read().c() {}
+}
+
+/// L2 cache sync, similar to dsb for L1 cache
+#[inline(always)]
+pub fn l2_cache_sync() {
+    let regs = RegisterBlock::new();
+    regs.reg7_cache_sync.write(Reg7CacheSync::zeroed().c(false));
+}
+
+#[inline(always)]
+pub fn l2_cache_clean(addr: usize) {
+    let regs = RegisterBlock::new();
+    unsafe {
+        regs.reg7_clean_pa.write(addr as u32);
+    }
+}
+
+#[inline(always)]
+pub fn l2_cache_invalidate(addr: usize) {
+    let regs = RegisterBlock::new();
+    unsafe {
+        regs.reg7_inv_pa.write(addr as u32);
+    }
+}
+
+#[inline(always)]
+pub fn l2_cache_clean_invalidate(addr: usize) {
+    let regs = RegisterBlock::new();
+    unsafe {
+        regs.reg7_clean_inv_pa.write(addr as u32);
+    }
+}
+
+#[repr(C)]
+struct RegisterBlock {
+    /// cache ID register, Returns the 32-bit device ID code it reads off the CACHEID input bus.
+    /// The value is specified by the system integrator. Reset value: 0x410000c8
+    pub reg0_cache_id: Reg0CacheId,
+    /// cache type register, Returns the 32-bit cache type. Reset value: 0x1c100100
+    pub reg0_cache_type: Reg0CacheType,
+    unused0: [u32; 62],
+    /// control register, reset value: 0x0
+    pub reg1_control: Reg1Control,
+    /// auxilary control register, reset value: 0x02020000
+    pub reg1_aux_control: Reg1AuxControl,
+    /// Configures Tag RAM latencies
+    pub reg1_tag_ram_control: Reg1TagRamControl,
+    /// configures data RAM latencies
+    pub reg1_data_ram_control: Reg1DataRamControl,
+    unused1: [u32; 60],
+    /// Permits the event counters to be enabled and reset.
+    pub reg2_ev_counter_ctrl: Reg2EvCounterCtrl,
+    /// Enables event counter 1 to be driven by a specific event. Counter 1 increments when the
+    /// event occurs.
+    pub reg2_ev_counter1_cfg: Reg2EvCounter1Cfg,
+    /// Enables event counter 0 to be driven by a specific event. Counter 0 increments when the
+    /// event occurs.
+    pub reg2_ev_counter0_cfg: Reg2EvCounter0Cfg,
+    /// Enable the programmer to read off the counter value. The counter counts an event as
+    /// specified by the Counter Configuration Registers. The counter can be preloaded if counting
+    /// is disabled and reset by the Event Counter Control Register.
+    pub reg2_ev_counter1: RW<u32>,
+    /// Enable the programmer to read off the counter value. The counter counts an event as
+    /// specified by the Counter Configuration Registers. The counter can be preloaded if counting
+    /// is disabled and reset by the Event Counter Control Register.
+    pub reg2_ev_counter0: RW<u32>,
+    /// This register enables or masks interrupts from being triggered on the external pins of the
+    /// cache controller. Figure 3-8 on page 3-17 shows the register bit assignments. The bit
+    /// assignments enables the masking of the interrupts on both their individual outputs and the
+    /// combined L2CCINTR line. Clearing a bit by writing a 0, disables the interrupt triggering on
+    /// that pin. All bits are cleared by a reset. You must write to the register bits with a 1 to
+    /// enable the generation of interrupts. 1 = Enabled. 0 = Masked. This is the default.
+    pub reg2_int_mask: Reg2IntMask,
+    /// This register is a read-only.It returns the masked interrupt status. This register can be
+    /// accessed by secure and non-secure operations. The register gives an AND function of the raw
+    /// interrupt status with the values of the interrupt mask register. All the bits are cleared
+    /// by a reset. A write to this register is ignored. Bits read can be HIGH or LOW: HIGH If the
+    /// bits read HIGH, they reflect the status of the input lines triggering an interrupt. LOW If
+    /// the bits read LOW, either no interrupt has been generated, or the interrupt is masked.
+    pub reg2_int_mask_status: Reg2IntMaskStatus,
+    /// The Raw Interrupt Status Register enables the interrupt status that excludes the masking
+    /// logic. Bits read can be HIGH or LOW: HIGH If the bits read HIGH, they reflect the status of
+    /// the input lines triggering an interrupt. LOW If the bits read LOW, no interrupt has been
+    /// generated.
+    pub reg2_int_raw_status: Reg2IntRawStatus,
+    /// Clears the Raw Interrupt Status Register bits. When a bit is written as 1, it clears the
+    /// corresponding bit in the Raw Interrupt Status Register. When a bit is written as 0, it has
+    /// no effect
+    pub reg2_int_clear: Reg2IntClear,
+    unused2: [u32; 323],
+    /// Drain the STB. Operation complete when all buffers, LRB, LFB, STB, and EB, are empty
+    pub reg7_cache_sync: Reg7CacheSync,
+    unused3: [u32; 15],
+    /// Invalidate Line by PA: Specific L2 cache line is marked as not valid
+    pub reg7_inv_pa: RW<u32>,
+    unused4: [u32; 2],
+    /// Invalidate by Way Invalidate all data in specified ways, including dirty data. An
+    /// Invalidate by way while selecting all cache ways is equivalent to invalidating all cache
+    /// entries. Completes as a background task with the way, or ways, locked, preventing
+    /// allocation.
+    pub reg7_inv_way: RW<u32>,
+    unused5: [u32; 12],
+    /// Clean Line by PA Write the specific L2 cache line to L3 main memory if the line is marked
+    /// as valid and dirty. The line is marked as not dirty. The valid bit is unchanged
+    pub reg7_clean_pa: RW<u32>,
+    unused6: [u32; 1],
+    /// Clean Line by Set/Way Write the specific L2 cache line within the specified way to L3 main
+    /// memory if the line is marked as valid and dirty. The line is marked as not dirty. The valid
+    /// bit is unchanged
+    pub reg7_clean_index: Reg7CleanIndex,
+    /// Clean by Way Writes each line of the specified L2 cache ways to L3 main memory if the line
+    /// is marked as valid and dirty. The lines are marked as not dirty. The valid bits are
+    /// unchanged. Completes as a background task with the way, or ways, locked, preventing
+    /// allocation.
+    pub reg7_clean_way: RW<u32>,
+    unused7: [u32; 12],
+    /// Clean and Invalidate Line by PA Write the specific L2 cache line to L3 main memory if the
+    /// line is marked as valid and dirty. The line is marked as not valid
+    pub reg7_clean_inv_pa: RW<u32>,
+    unused8: [u32; 1],
+    /// Clean and Invalidate Line by Set/Way Write the specific L2 cache line within the specified
+    /// way to L3 main memory if the line is marked as valid and dirty. The line is marked as not
+    /// valid
+    pub reg7_clean_inv_index: Reg7CleanInvIndex,
+    /// Clean and Invalidate by Way Writes each line of the specified L2 cache ways to L3 main
+    /// memory if the line is marked as valid and dirty. The lines are marked as not valid.
+    /// Completes as a background task with the way, or ways, locked, preventing allocation.
+    pub reg7_clean_inv_way: RW<u32>,
+    unused9: [u32; 0x1D8],
+    pub reg15_prefetch_ctrl: Reg15PrefetechCtrl,
+}
+
+register_at!(RegisterBlock, 0xF8F02000, new);
+
+register!(reg0_cache_id, Reg0CacheId, RW, u32);
+register_bits!(reg0_cache_id, implementer, u8, 24, 31);
+register_bits!(reg0_cache_id, cache_id, u8, 10, 15);
+register_bits!(reg0_cache_id, part_num, u8, 6, 9);
+register_bits!(reg0_cache_id, rtl_release, u8, 0, 5);
+
+register!(reg0_cache_type, Reg0CacheType, RW, u32);
+register_bit!(reg0_cache_type, data_banking, 31);
+register_bits!(reg0_cache_type, ctype, u8, 25, 28);
+register_bit!(reg0_cache_type, h, 24);
+register_bits!(reg0_cache_type, dsize_middsize_19, u8, 20, 22);
+register_bit!(reg0_cache_type, l2_assoc_d, 18);
+register_bits!(reg0_cache_type, l2cache_line_len_disize_11, u8, 12, 13);
+register_bits!(reg0_cache_type, isize_midisize_7, u8, 8, 10);
+register_bit!(reg0_cache_type, l2_assoc_i, 6);
+register_bits!(reg0_cache_type, l2cache_line_len_i, u8, 0, 1);
+
+register!(reg1_control, Reg1Control, RW, u32);
+register_bit!(reg1_control, l2_enable, 0);
+
+register!(reg1_aux_control, Reg1AuxControl, RW, u32);
+register_bit!(reg1_aux_control, early_bresp_en, 30);
+register_bit!(reg1_aux_control, instr_prefetch_en, 29);
+register_bit!(reg1_aux_control, data_prefetch_en, 28);
+register_bit!(reg1_aux_control, nonsec_inte_access_ctrl, 27);
+register_bit!(reg1_aux_control, nonsec_lockdown_en, 26);
+register_bit!(reg1_aux_control, cache_replace_policy, 25);
+register_bits!(reg1_aux_control, force_write_alloc, u8, 23, 24);
+register_bit!(reg1_aux_control, shared_attr_override_en, 22);
+register_bit!(reg1_aux_control, parity_en, 21);
+register_bit!(reg1_aux_control, event_mon_bus_en, 20);
+register_bits!(reg1_aux_control, way_size, u8, 17, 19);
+register_bit!(reg1_aux_control, associativity, 16);
+register_bit!(reg1_aux_control, shared_attr_inva_en, 13);
+register_bit!(reg1_aux_control, ex_cache_config, 12);
+register_bit!(reg1_aux_control, store_buff_dev_lim_en, 11);
+register_bit!(reg1_aux_control, high_pr_so_dev_rd_en, 10);
+register_bit!(reg1_aux_control, full_line_zero_enable, 0);
+
+register!(reg1_tag_ram_control, Reg1TagRamControl, RW, u32);
+register_bits!(reg1_tag_ram_control, ram_wr_access_lat, u8, 8, 10);
+register_bits!(reg1_tag_ram_control, ram_rd_access_lat, u8, 4, 6);
+register_bits!(reg1_tag_ram_control, ram_setup_lat, u8, 0, 2);
+
+register!(reg1_data_ram_control, Reg1DataRamControl, RW, u32);
+register_bits!(reg1_data_ram_control, ram_wr_access_lat, u8, 8, 10);
+register_bits!(reg1_data_ram_control, ram_rd_access_lat, u8, 4, 6);
+register_bits!(reg1_data_ram_control, ram_setup_lat, u8, 0, 2);
+
+register!(reg2_ev_counter_ctrl, Reg2EvCounterCtrl, RW, u32);
+register_bit!(reg2_ev_counter_ctrl, ev_ctr_en, 0);
+
+register!(reg2_ev_counter1_cfg, Reg2EvCounter1Cfg, RW, u32);
+register_bits!(reg2_ev_counter1_cfg, ctr_ev_src, u8, 2, 5);
+register_bits!(reg2_ev_counter1_cfg, ev_ctr_intr_gen, u8, 0, 1);
+
+register!(reg2_ev_counter0_cfg, Reg2EvCounter0Cfg, RW, u32);
+register_bits!(reg2_ev_counter0_cfg, ctr_ev_src, u8, 2, 5);
+register_bits!(reg2_ev_counter0_cfg, ev_ctr_intr_gen, u8, 0, 1);
+
+register!(reg2_int_mask, Reg2IntMask, RW, u32);
+register_bit!(reg2_int_mask, decerr, 8);
+register_bit!(reg2_int_mask, slverr, 7);
+register_bit!(reg2_int_mask, errrd, 6);
+register_bit!(reg2_int_mask, errrt, 5);
+register_bit!(reg2_int_mask, errwd, 4);
+register_bit!(reg2_int_mask, errwt, 3);
+register_bit!(reg2_int_mask, parrd, 2);
+register_bit!(reg2_int_mask, parrt, 1);
+register_bit!(reg2_int_mask, ecntr, 0);
+
+register!(reg2_int_mask_status, Reg2IntMaskStatus, RW, u32);
+register_bit!(reg2_int_mask_status, decerr, 8);
+register_bit!(reg2_int_mask_status, slverr, 7);
+register_bit!(reg2_int_mask_status, errrd, 6);
+register_bit!(reg2_int_mask_status, errrt, 5);
+register_bit!(reg2_int_mask_status, errwd, 4);
+register_bit!(reg2_int_mask_status, errwt, 3);
+register_bit!(reg2_int_mask_status, parrd, 2);
+register_bit!(reg2_int_mask_status, parrt, 1);
+register_bit!(reg2_int_mask_status, ecntr, 0);
+
+register!(reg2_int_raw_status, Reg2IntRawStatus, RW, u32);
+register_bit!(reg2_int_raw_status, decerr, 8);
+register_bit!(reg2_int_raw_status, slverr, 7);
+register_bit!(reg2_int_raw_status, errrd, 6);
+register_bit!(reg2_int_raw_status, errrt, 5);
+register_bit!(reg2_int_raw_status, errwd, 4);
+register_bit!(reg2_int_raw_status, errwt, 3);
+register_bit!(reg2_int_raw_status, parrd, 2);
+register_bit!(reg2_int_raw_status, parrt, 1);
+register_bit!(reg2_int_raw_status, ecntr, 0);
+
+register!(reg2_int_clear, Reg2IntClear, RW, u32, 0);
+register_bit!(reg2_int_clear, decerr, 8, WTC);
+register_bit!(reg2_int_clear, slverr, 7, WTC);
+register_bit!(reg2_int_clear, errrd, 6, WTC);
+register_bit!(reg2_int_clear, errrt, 5, WTC);
+register_bit!(reg2_int_clear, errwd, 4, WTC);
+register_bit!(reg2_int_clear, errwt, 3, WTC);
+register_bit!(reg2_int_clear, parrd, 2, WTC);
+register_bit!(reg2_int_clear, parrt, 1, WTC);
+register_bit!(reg2_int_clear, ecntr, 0, WTC);
+
+register!(reg7_cache_sync, Reg7CacheSync, RW, u32);
+register_bit!(reg7_cache_sync, c, 0);
+
+register!(reg7_clean_index, Reg7CleanIndex, RW, u32);
+register_bits!(reg7_clean_index, way, u8, 28, 30);
+register_bits!(reg7_clean_index, index, u8, 5, 11);
+register_bit!(reg7_clean_index, c, 0);
+
+register!(reg7_clean_inv_index, Reg7CleanInvIndex, RW, u32);
+register_bits!(reg7_clean_inv_index, way, u8, 28, 30);
+register_bits!(reg7_clean_inv_index, index, u8, 5, 11);
+register_bit!(reg7_clean_inv_index, c, 0);
+
+register!(reg15_prefetch_ctrl, Reg15PrefetechCtrl, RW, u32);
+register_bit!(reg15_prefetch_ctrl, double_linefill_en, 30);
+register_bit!(reg15_prefetch_ctrl, instr_prefetch_en, 29);
+register_bit!(reg15_prefetch_ctrl, data_prefetch_en, 28);
+register_bit!(reg15_prefetch_ctrl, pref_drop_en, 24);
+register_bit!(reg15_prefetch_ctrl, incr_double_linefill_en, 23);
+register_bits!(reg15_prefetch_ctrl, prefetch_offset, u8, 0, 4);
+
--- a/libcortex_a9/src/lib.rs
+++ b/libcortex_a9/src/lib.rs
@ -1,17 +1,85 @@
 #![no_std]
-#![feature(llvm_asm, global_asm)]
 #![feature(never_type)]
+#![feature(global_asm)]
+#![feature(asm)]
+#![allow(incomplete_features)]
+#![feature(inline_const)]
+#![feature(const_fn_trait_bound)]

 extern crate alloc;

 pub mod asm;
-pub mod regs;
 pub mod cache;
+mod fpu;
+pub mod l2c;
 pub mod mmu;
 pub mod mutex;
+pub mod regs;
+pub mod semaphore;
 pub mod sync_channel;
 mod uncached;
+pub use fpu::enable_fpu;
 pub use uncached::UncachedSlice;
-pub mod pl310;
+use core::arch::global_asm;

 global_asm!(include_str!("exceptions.s"));
+
+#[inline]
+pub fn spin_lock_yield() {
+    #[cfg(feature = "power_saving")]
+    asm::wfe();
+}
+
+#[inline]
+pub fn notify_spin_lock() {
+    #[cfg(feature = "power_saving")]
+    {
+        asm::dsb();
+        asm::sev();
+    }
+}
+
+#[macro_export]
+/// Interrupt handler, which setup the stack and preserve registers before jumping to actual interrupt handler.
+/// Registers r0-r12, PC, SP and CPSR are restored after the actual handler.
+/// 
+/// - `name` is the name of the interrupt, should be the same as the one defined in vector table.
+/// - `name2` is the name for the actual handler, should be different from name.
+/// - `stack0` is the stack for the interrupt handler when called from core0.
+/// - `stack1` is the stack for the interrupt handler when called from core1.
+/// - `body` is the body of the actual interrupt handler, should be a normal unsafe rust function
+/// body.
+///
+/// Note that the interrupt handler would use the same stack as normal programs by default.
+macro_rules! interrupt_handler {
+    ($name:ident, $name2:ident, $stack0:ident, $stack1:ident, $body:block) => {
+        #[link_section = ".text.boot"]
+        #[no_mangle]
+        #[naked]
+        pub unsafe extern "C" fn $name() -> ! {
+            asm!(
+                // setup SP, depending on CPU 0 or 1
+                // and preserve registers
+                "sub lr, lr, #4",
+                "stmfd sp!, {{r0-r12, lr}}",
+                "mrc p15, #0, r0, c0, c0, #5",
+                concat!("movw r1, :lower16:", stringify!($stack0)),
+                concat!("movt r1, :upper16:", stringify!($stack0)),
+                "tst r0, #3",
+                concat!("movwne r1, :lower16:", stringify!($stack1)),
+                concat!("movtne r1, :upper16:", stringify!($stack1)),
+                "mov r0, sp",
+                "mov sp, r1",
+                "push {{r0, r1}}",                                      // 2 registers are pushed to maintain 8 byte stack alignment 
+                concat!("bl ", stringify!($name2)),
+                "pop {{r0, r1}}",
+                "mov sp, r0",
+                "ldmfd sp!, {{r0-r12, pc}}^",                           // caret ^ : copy SPSR to the CPSR  
+                options(noreturn)
+            );
+        }
+
+        #[no_mangle]
+        pub unsafe extern "C" fn $name2() $body
+    };
+}
--- a/libcortex_a9/src/mmu.rs
+++ b/libcortex_a9/src/mmu.rs
@ -158,7 +158,7 @@ impl L1Table {
                global: true,
                shareable: true,
                access: AccessPermissions::FullAccess,
-                tex: 0b111,
+                tex: 0b0,
                domain: 0b1111,
                exec: true,
                cacheable: true,
@ -213,7 +213,7 @@ impl L1Table {
                access: AccessPermissions::FullAccess,
                tex: 0,
                domain: 0,
-                exec: true,
+                exec: false,
                cacheable: false,
                bufferable: true,
            });
@ -338,7 +338,7 @@ impl L1Table {
        /* 0xfff00000 - 0xffffffff (256K OCM when mapped to high address space) */
        self.direct_mapped_section(0xfff, L1Section {
            global: true,
-            shareable: false,
+            shareable: true,
            access: AccessPermissions::FullAccess,
            tex: 0b100,
            domain: 0,
@ -410,6 +410,7 @@ pub fn with_mmu<F: FnMut() -> !>(l1table: &L1Table, mut f: F) -> ! {
            .a(false)
            .c(true)
            .i(true)
+            .z(true)
            .unaligned(true)
    );

--- a/libcortex_a9/src/mutex.rs
+++ b/libcortex_a9/src/mutex.rs
@ -1,20 +1,13 @@
 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 core::task::{Context, Poll};
+use core::pin::Pin;
+use core::future::Future;
+use super::{
+    spin_lock_yield, notify_spin_lock,
+    asm::{enter_critical, exit_critical}
+};

 const LOCKED: u32 = 1;
 const UNLOCKED: u32 = 0;
@ -30,6 +23,23 @@ pub struct Mutex<T> {
 unsafe impl<T: Send> Sync for Mutex<T> {}
 unsafe impl<T: Send> Send for Mutex<T> {}

+struct Fut<'a, T>(&'a Mutex<T>);
+
+impl<'a, T> Future for Fut<'a, T> {
+    type Output = MutexGuard<'a, T>;
+    fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
+        let irq = unsafe { enter_critical() };
+        if self.0.locked.compare_exchange_weak(UNLOCKED, LOCKED, Ordering::AcqRel, Ordering::Relaxed).is_err() {
+            unsafe { exit_critical(irq) };
+            cx.waker().wake_by_ref();
+            Poll::Pending
+        }
+        else {
+            Poll::Ready(MutexGuard { mutex: self.0, irq })
+        }
+    }
+}
+
 impl<T> Mutex<T> {
    /// Constructor, const-fn
    pub const fn new(inner: T) -> Self {
@ -41,18 +51,35 @@ 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_exchange_weak(UNLOCKED, LOCKED, Ordering::AcqRel, Ordering::Relaxed).is_err() {
+            unsafe {
+                exit_critical(irq);
+                spin_lock_yield();
+                irq = enter_critical();
+            }
+        }
+        MutexGuard { mutex: self, irq }
+    }
+
+    pub async fn async_lock(&self) -> MutexGuard<'_, T> {
+        Fut(&self).await
+    }
+
+    pub fn try_lock(&self) -> Option<MutexGuard<T>> {
+        let irq = unsafe { enter_critical() };
+        if self.locked.compare_exchange_weak(UNLOCKED, LOCKED, Ordering::AcqRel, Ordering::Relaxed).is_err() {
+            unsafe { exit_critical(irq) };
+            None
+        } else {
+            Some(MutexGuard { mutex: self, irq })
        }
-        dmb();
-        MutexGuard { mutex: self }
    }

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

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

@ -60,6 +87,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 +107,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/pl310/mod.rs
+++ b/libcortex_a9/src/pl310/mod.rs
@ -1,166 +0,0 @@
-//! L2 cache controller
-
-use libregister::RegisterW;
-use crate::asm::*;
-
-mod regs;
-
-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)
-}
-
-pub struct L2Cache {
-    pub regs: &'static mut regs::RegisterBlock,
-}
-
-impl L2Cache {
-    pub fn new(register_baseaddr: usize) -> Self {
-        let regs = unsafe {
-            regs::RegisterBlock::new_at(register_baseaddr)
-        };
-        L2Cache { regs }
-    }
-
-    pub fn set_tag_ram_latencies(&mut self, setup_lat: u8, rd_access_lat: u8, wr_access_lat: u8) {
-        self.regs.tag_ram_control.write(
-            regs::RamControl::zeroed()
-                .setup_lat(setup_lat)
-                .rd_access_lat(rd_access_lat)
-                .wr_access_lat(wr_access_lat)
-        );
-    }
-
-    pub fn set_data_ram_latencies(&mut self, setup_lat: u8, rd_access_lat: u8, wr_access_lat: u8) {
-        self.regs.data_ram_control.write(
-            regs::RamControl::zeroed()
-                .setup_lat(setup_lat)
-                .rd_access_lat(rd_access_lat)
-                .wr_access_lat(wr_access_lat)
-        );
-    }
-
-    pub fn disable_interrupts(&mut self) {
-        self.regs.int_mask.write(
-            regs::Interrupts::zeroed()
-                .ecntr(true)
-                .parrt(true)
-                .parrd(true)
-                .errwt(true)
-                .errwd(true)
-                .errrt(true)
-                .errrd(true)
-                .slverr(true)
-                .decerr(true)
-        );
-    }
-
-    pub fn reset_interrupts(&mut self) {
-        self.regs.int_clear.write(
-            regs::Interrupts::zeroed()
-                .ecntr(true)
-                .parrt(true)
-                .parrd(true)
-                .errwt(true)
-                .errwd(true)
-                .errrt(true)
-                .errrd(true)
-                .slverr(true)
-                .decerr(true)
-        );
-    }
-
-    pub fn invalidate_all(&mut self) {
-        unsafe { self.regs.inv_way.write(0xFFFF); }
-        unsafe { self.regs.cache_sync.write(1); }
-        while self.regs.cache_sync.read() != 0 {}
-    }
-
-    pub fn enable(&mut self) {
-        dmb();
-        self.regs.control.write(
-            regs::Control::zeroed()
-                .l2_enable(true)
-        );
-        dsb();
-    }
-
-    pub fn clean_invalidate<T>(&mut self, obj: &T) {
-        dmb();
-        for addr in object_cache_line_addrs(obj) {
-            unsafe {
-                self.regs.clean_inv_pa.write(addr as u32);
-            }
-        }
-        dsb();
-        unsafe { self.regs.cache_sync.write(1); }
-        while self.regs.cache_sync.read() != 0 {}
-    }
-
-    pub fn clean_invalidate_slice<T>(&mut self, slice: &[T]) {
-        dmb();
-        for addr in slice_cache_line_addrs(slice) {
-            unsafe {
-                self.regs.clean_inv_pa.write(addr as u32);
-            }
-        }
-        dsb();
-        unsafe { self.regs.cache_sync.write(1); }
-        while self.regs.cache_sync.read() != 0 {}
-    }
-
-    pub fn clean_slice<T>(&mut self, slice: &[T]) {
-        dmb();
-        for addr in slice_cache_line_addrs(slice) {
-            unsafe {
-                self.regs.clean_pa.write(addr as u32);
-            }
-        }
-        dsb();
-        unsafe { self.regs.cache_sync.write(1); }
-        while self.regs.cache_sync.read() != 0 {}
-    }
-
-    pub fn invalidate<T>(&mut self, obj: &mut T) {
-        dmb();
-        for addr in object_cache_line_addrs(obj) {
-            unsafe {
-                self.regs.inv_pa.write(addr as u32);
-            }
-        }
-        dsb();
-        unsafe { self.regs.cache_sync.write(1); }
-        while self.regs.cache_sync.read() != 0 {}
-    }
-
-    pub fn invalidate_slice<T>(&mut self, slice: &mut [T]) {
-        dmb();
-        for addr in slice_cache_line_addrs(slice) {
-            unsafe {
-                self.regs.inv_pa.write(addr as u32);
-            }
-        }
-        dsb();
-        unsafe { self.regs.cache_sync.write(1); }
-        while self.regs.cache_sync.read() != 0 {}
-    }
-}
--- a/libcortex_a9/src/pl310/regs.rs
+++ b/libcortex_a9/src/pl310/regs.rs
@ -1,93 +0,0 @@
-use volatile_register::{RO, WO, RW};
-use libregister::{register, register_bit, register_bits, RegisterW};
-
-#[repr(C)]
-pub struct RegisterBlock {
-    pub cache_id: RW<u32>,
-    pub cache_type: RW<u32>,
-    pub _unused1: [RO<u32>; 62],
-    pub control: Control,
-    pub aux_control: RW<u32>,
-    pub tag_ram_control: RamControl,
-    pub data_ram_control: RamControl,
-    pub _unused2: [RO<u32>; 60],
-    pub ev_counter_ctrl: RW<u32>,
-    pub ev_counter1_cfg: RW<u32>,
-    pub ev_counter2_cfg: RW<u32>,
-    pub ev_counter1: RW<u32>,
-    pub ev_counter2: RW<u32>,
-    pub int_mask: Interrupts,
-    pub int_mask_status: Interrupts,
-    pub int_raw_status: Interrupts,
-    pub int_clear: Interrupts,
-    pub _unused3: [RO<u32>; 323],
-    pub cache_sync: RW<u32>,
-    pub _unused4: [RO<u32>; 15],
-    pub inv_pa: RW<u32>,
-    pub _unused5: [RO<u32>; 2],
-    pub inv_way: RW<u32>,
-    pub _unused6: [RO<u32>; 12],
-    pub clean_pa: RW<u32>,
-    pub _unused7: [RO<u32>; 1],
-    pub clean_index: RW<u32>,
-    pub clean_way: RW<u32>,
-    pub _unused8: [RO<u32>; 12],
-    pub clean_inv_pa: RW<u32>,
-    pub _unused9: [RO<u32>; 1],
-    pub clean_inv_index: RW<u32>,
-    pub clean_inv_way: RW<u32>,
-    pub _unused10: [RO<u32>; 64],
-    pub d_lockdown0: RW<u32>,
-    pub i_lockdown0: RW<u32>,
-    pub d_lockdown1: RW<u32>,
-    pub i_lockdown1: RW<u32>,
-    pub d_lockdown2: RW<u32>,
-    pub i_lockdown2: RW<u32>,
-    pub d_lockdown3: RW<u32>,
-    pub i_lockdown3: RW<u32>,
-    pub d_lockdown4: RW<u32>,
-    pub i_lockdown4: RW<u32>,
-    pub d_lockdown5: RW<u32>,
-    pub i_lockdown5: RW<u32>,
-    pub d_lockdown6: RW<u32>,
-    pub i_lockdown6: RW<u32>,
-    pub d_lockdown7: RW<u32>,
-    pub i_lockdown7: RW<u32>,
-    pub _unused11: [RO<u32>; 4],
-    pub lock_line_en: RW<u32>,
-    pub unlock_way: RW<u32>,
-    pub _unused12: [RO<u32>; 170],
-    pub addr_filtering_start: RW<u32>,
-    pub addr_filtering_end: RW<u32>,
-    pub _unused13: [RO<u32>; 206],
-    pub debug_ctrl: RW<u32>,
-    pub _unused14: [RO<u32>; 7],
-    pub prefetch_ctrl: RW<u32>,
-    pub _unused15: [RO<u32>; 7],
-    pub power_ctrl: RW<u32>,
-}
-
-impl RegisterBlock {
-    pub unsafe fn new_at(baseaddr: usize) -> &'static mut Self {
-        &mut *(baseaddr as *mut _)
-    }
-}
-
-register!(control, Control, RW, u32);
-register_bit!(control, l2_enable, 0);
-
-register!(ram_control, RamControl, RW, u32);
-register_bits!(ram_control, setup_lat, u8, 0, 2);
-register_bits!(ram_control, rd_access_lat, u8, 4, 6);
-register_bits!(ram_control, wr_access_lat, u8, 8, 10);
-
-register!(interrupts, Interrupts, RW, u32);
-register_bit!(interrupts, ecntr, 0);
-register_bit!(interrupts, parrt, 1);
-register_bit!(interrupts, parrd, 2);
-register_bit!(interrupts, errwt, 3);
-register_bit!(interrupts, errwd, 4);
-register_bit!(interrupts, errrt, 5);
-register_bit!(interrupts, errrd, 6);
-register_bit!(interrupts, slverr, 7);
-register_bit!(interrupts, decerr, 8);
--- a/libcortex_a9/src/regs.rs
+++ b/libcortex_a9/src/regs.rs
@ -2,6 +2,7 @@ use libregister::{
    register_bit, register_bits,
    RegisterR, RegisterW, RegisterRW,
 };
+use core::arch::asm;

 macro_rules! def_reg_r {
    ($name:tt, $type: ty, $asm_instr:tt) => {
@ -11,7 +12,7 @@ macro_rules! def_reg_r {
            #[inline]
            fn read(&self) -> Self::R {
                let mut value: u32;
-                unsafe { llvm_asm!($asm_instr : "=r" (value) ::: "volatile") }
+                unsafe { asm!($asm_instr, lateout(reg) value) }
                value.into()
            }
        }
@ -26,7 +27,7 @@ macro_rules! def_reg_w {
            #[inline]
            fn write(&mut self, value: Self::W) {
                let value: u32 = value.into();
-                unsafe { llvm_asm!($asm_instr :: "r" (value) :: "volatile") }
+                unsafe { asm!($asm_instr, in(reg) value) }
            }

            #[inline]
@ -71,39 +72,50 @@ macro_rules! wrap_reg {

 /// Stack Pointer
 pub struct SP;
-def_reg_r!(SP, u32, "mov $0, sp");
-def_reg_w!(SP, u32, "mov sp, $0");
+def_reg_r!(SP, u32, "mov {}, sp");
+def_reg_w!(SP, u32, "mov sp, {}");

 /// Link register (function call return address)
 pub struct LR;
-def_reg_r!(LR, u32, "mov $0, lr");
-def_reg_w!(LR, u32, "mov lr, $0");
+def_reg_r!(LR, u32, "mov {}, lr");
+def_reg_w!(LR, u32, "mov lr, {}");

 pub struct VBAR;
-def_reg_r!(VBAR, u32, "mrc p15, 0, $0, c12, c0, 0");
-def_reg_w!(VBAR, u32, "mcr p15, 0, $0, c12, c0, 0");
+def_reg_r!(VBAR, u32, "mrc p15, 0, {}, c12, c0, 0");
+def_reg_w!(VBAR, u32, "mcr p15, 0, {}, c12, c0, 0");

 pub struct MVBAR;
-def_reg_r!(MVBAR, u32, "mrc p15, 0, $0, c12, c0, 1");
-def_reg_w!(MVBAR, u32, "mcr p15, 0, $0, c12, c0, 1");
+def_reg_r!(MVBAR, u32, "mrc p15, 0, {}, c12, c0, 1");
+def_reg_w!(MVBAR, u32, "mcr p15, 0, {}, c12, c0, 1");

 pub struct HVBAR;
-def_reg_r!(HVBAR, u32, "mrc p15, 4, $0, c12, c0, 0");
-def_reg_w!(HVBAR, u32, "mcr p15, 4, $0, c12, c0, 0");
+def_reg_r!(HVBAR, u32, "mrc p15, 4, {}, c12, c0, 0");
+def_reg_w!(HVBAR, u32, "mcr p15, 4, {}, c12, c0, 0");

+/// Multiprocess Affinity Register
 pub struct MPIDR;
-def_reg_r!(MPIDR, u32, "mrc p15, 0, $0, c0, c0, 5");
+def_reg_r!(MPIDR, mpidr::Read, "mrc p15, 0, {}, c0, c0, 5");
+wrap_reg!(mpidr);
+register_bits!(mpidr,
+               /// CPU core index
+               cpu_id, u8, 0, 1);
+register_bits!(mpidr,
+               /// Processor index in "multi-socket" systems
+               cluster_id, u8, 8, 11);
+register_bit!(mpidr,
+              /// true if part of uniprocessor system
+              u, 30);

 pub struct DFAR;
-def_reg_r!(DFAR, u32, "mrc p15, 0, $0, c6, c0, 0");
+def_reg_r!(DFAR, u32, "mrc p15, 0, {}, c6, c0, 0");

 pub struct DFSR;
-def_reg_r!(DFSR, u32, "mrc p15, 0, $0, c5, c0, 0");
+def_reg_r!(DFSR, u32, "mrc p15, 0, {}, c5, c0, 0");

 pub struct SCTLR;
 wrap_reg!(sctlr);
-def_reg_r!(SCTLR, sctlr::Read, "mrc p15, 0, $0, c1, c0, 0");
-def_reg_w!(SCTLR, sctlr::Write, "mcr p15, 0, $0, c1, c0, 0");
+def_reg_r!(SCTLR, sctlr::Read, "mrc p15, 0, {}, c1, c0, 0");
+def_reg_w!(SCTLR, sctlr::Write, "mcr p15, 0, {}, c1, c0, 0");
 register_bit!(sctlr,
              /// Enables MMU
              m, 0);
@ -136,14 +148,17 @@ register_bit!(sctlr,
 /// Auxiliary Control Register
 pub struct ACTLR;
 wrap_reg!(actlr);
-def_reg_r!(ACTLR, actlr::Read, "mrc p15, 0, $0, c1, c0, 1");
-def_reg_w!(ACTLR, actlr::Write, "mcr p15, 0, $0, c1, c0, 1");
+def_reg_r!(ACTLR, actlr::Read, "mrc p15, 0, {}, c1, c0, 1");
+def_reg_w!(ACTLR, actlr::Write, "mcr p15, 0, {}, c1, c0, 1");
+// SMP bit
 register_bit!(actlr, parity_on, 9);
 register_bit!(actlr, alloc_one_way, 8);
 register_bit!(actlr, excl, 7);
 register_bit!(actlr, smp, 6);
 register_bit!(actlr, write_full_line_of_zeros, 3);
 register_bit!(actlr, l1_prefetch_enable, 2);
+// L2 cache prefetch hint, in UG585 section 3.4.8
+register_bit!(actlr, l2_prefetch_enable, 1);
 // Cache/TLB maintenance broadcast
 register_bit!(actlr, fw, 0);

@ -159,23 +174,27 @@ impl RegisterRW for ACTLR {

 impl ACTLR {
    pub fn enable_smp(&mut self) {
-        self.modify(|_, w| w.smp(true).fw(true));
+        self.modify(|_, w| w.smp(true).fw(true).alloc_one_way(true));
+    }
+
+    pub fn enable_prefetch(&mut self) {
+        self.modify(|_, w| w.l1_prefetch_enable(true).l2_prefetch_enable(true))
    }
 }

 /// Domain Access Control Register
 pub struct DACR;
-def_reg_r!(DACR, u32, "mrc p15, 0, $0, c3, c0, 0");
-def_reg_w!(DACR, u32, "mcr p15, 0, $0, c3, c0, 0");
+def_reg_r!(DACR, u32, "mrc p15, 0, {}, c3, c0, 0");
+def_reg_w!(DACR, u32, "mcr p15, 0, {}, c3, c0, 0");

 /// Translation Table Base Register 0
 pub struct TTBR0;
 /// Translation Table Base Register 1
 pub struct TTBR1;
-def_reg_r!(TTBR0, ttbr::Read, "mrc p15, 0, $0, c2, c0, 0");
-def_reg_w!(TTBR0, ttbr::Write, "mcr p15, 0, $0, c2, c0, 0");
-def_reg_r!(TTBR1, ttbr::Read, "mrc p15, 0, $0, c2, c0, 1");
-def_reg_w!(TTBR1, ttbr::Write, "mcr p15, 0, $0, c2, c0, 1");
+def_reg_r!(TTBR0, ttbr::Read, "mrc p15, 0, {}, c2, c0, 0");
+def_reg_w!(TTBR0, ttbr::Write, "mcr p15, 0, {}, c2, c0, 0");
+def_reg_r!(TTBR1, ttbr::Read, "mrc p15, 0, {}, c2, c0, 1");
+def_reg_w!(TTBR1, ttbr::Write, "mcr p15, 0, {}, c2, c0, 1");
 wrap_reg!(ttbr);
 register_bits!(ttbr, table_base, u32, 14, 31);
 register_bit!(ttbr, irgn0, 6);
--- a/libcortex_a9/src/semaphore.rs
+++ b/libcortex_a9/src/semaphore.rs
@ -0,0 +1,71 @@
+use super::{spin_lock_yield, notify_spin_lock};
+use core::{
+    task::{Context, Poll},
+    pin::Pin,
+    future::Future,
+    sync::atomic::{AtomicI32, Ordering}
+};
+
+pub struct Semaphore {
+    value: AtomicI32,
+    max: i32
+}
+
+impl Semaphore {
+    pub const fn new(value: i32, max: i32) -> Self {
+        Semaphore { value: AtomicI32::new(value), max}
+    }
+
+    pub fn try_wait(&self) -> Option<()> {
+        loop {
+            let value = self.value.load(Ordering::Relaxed);
+            if value > 0 {
+                if self.value.compare_exchange_weak(value, value - 1, Ordering::SeqCst, Ordering::Relaxed).is_ok() {
+                    return Some(());
+                }
+            } else {
+                return None;
+            }
+        }
+    }
+
+    pub fn wait(&self) {
+        while self.try_wait().is_none() {
+            spin_lock_yield();
+        }
+    }
+
+    pub async fn async_wait(&self) {
+        struct Fut<'a>(&'a Semaphore);
+
+        impl Future for Fut<'_> {
+            type Output = ();
+            fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
+                match self.0.try_wait() {
+                    Some(_) => Poll::Ready(()),
+                    None => {
+                        cx.waker().wake_by_ref();
+                        Poll::Pending
+                    }
+                }
+            }
+        }
+
+        Fut(&self).await
+    }
+
+    pub fn signal(&self) {
+        loop {
+            let value = self.value.load(Ordering::Relaxed);
+            if value < self.max {
+                if self.value.compare_exchange_weak(value, value + 1, Ordering::SeqCst, Ordering::Relaxed).is_ok() {
+                    notify_spin_lock();
+                    return;
+                }
+            } else {
+                return;
+            }
+        }
+    }
+}
+
--- a/libcortex_a9/src/sync_channel.rs
+++ b/libcortex_a9/src/sync_channel.rs
@ -1,115 +1,72 @@
 use core::{
-    future::Future,
    pin::Pin,
-    ptr::null_mut,
-    sync::atomic::{AtomicPtr, Ordering},
+    future::Future,
+    sync::atomic::{AtomicPtr, AtomicUsize, Ordering},
    task::{Context, Poll},
 };
-use alloc::{
-    boxed::Box,
-    sync::Arc,
-    vec::Vec,
-};
-use super::asm::*;
+use alloc::boxed::Box;
+use super::{spin_lock_yield, notify_spin_lock};

-
-type Channel<T> = Vec<AtomicPtr<T>>;
-
-/// Create a bounded channel
-///
-/// Returns `(tx, rx)` where one should be used one the local core,
-/// and the other is to be shared with another core.
-pub fn sync_channel<T>(bound: usize) -> (Sender<T>, Receiver<T>) {
-    // allow for bound=0
-    let len = bound + 1;
-    let mut channel = Vec::with_capacity(len);
-    for _ in 0..len {
-        channel.push(AtomicPtr::default());
-    }
-
-    let channel = Arc::new(channel);
-    let sender = Sender {
-        channel: channel.clone(),
-        pos: 0,
-    };
-    let receiver = Receiver {
-        channel: channel,
-        pos: 0,
-    };
-    (sender, receiver)
+pub struct Sender<'a, T> where T: Clone {
+    list: &'a [AtomicPtr<T>],
+    write: &'a AtomicUsize,
+    read: &'a AtomicUsize,
 }

-/// Sending half of a channel
-pub struct Sender<T> {
-    channel: Arc<Channel<T>>,
-    pos: usize,
+pub struct Receiver<'a, T> where T: Clone {
+    list: &'a [AtomicPtr<T>],
+    write: &'a AtomicUsize,
+    read: &'a AtomicUsize,
 }

-impl<T> Sender<T> {
-    /// Blocking send
-    pub fn send<B: Into<Box<T>>>(&mut self, content: B) {
-        let ptr = Box::into_raw(content.into());
-        let entry = &self.channel[self.pos];
-        // try to write the new pointer if the current pointer is
-        // NULL, retrying while it is not NULL
-        while entry.compare_and_swap(null_mut(), ptr, Ordering::Acquire) != null_mut() {
-            // power-saving
-            wfe();
-        }
-        dsb();
-        // wake power-saving receivers
-        sev();
-
-        // advance
-        self.pos += 1;
-        // wrap
-        if self.pos >= self.channel.len() {
-            self.pos = 0;
-        }
+impl<'a, T> Sender<'a, T> where T: Clone {
+    pub const fn new(list: &'static [AtomicPtr<T>], write: &'static AtomicUsize, read: &'static AtomicUsize) -> Self {
+        Sender {list, write, read}
    }

-    /// Non-blocking send, handing you back ownership of the content on **failure**
-    pub fn try_send<B: Into<Box<T>>>(&mut self, content: B) -> Option<Box<T>> {
-        let ptr = Box::into_raw(content.into());
-        let entry = &self.channel[self.pos];
-        // try to write the new pointer if the current pointer is
-        // NULL
-        if entry.compare_and_swap(null_mut(), ptr, Ordering::Acquire) == null_mut() {
-            dsb();
-            // wake power-saving receivers
-            sev();
-
-            // advance
-            self.pos += 1;
-            // wrap
-            if self.pos >= self.channel.len() {
-                self.pos = 0;
-            }
-
-            // success
-            None
+    pub fn try_send<B: Into<Box<T>>>(&mut self, content: B) -> Result<(), B> {
+        let write = self.write.load(Ordering::Relaxed);
+        if (write + 1) % self.list.len() == self.read.load(Ordering::Acquire) {
+            Err(content)
        } else {
-            let content = unsafe { Box::from_raw(ptr) };
-            // failure
-            Some(content)
+            let ptr = Box::into_raw(content.into());
+            let entry = &self.list[write];
+            let prev = entry.swap(ptr, Ordering::Relaxed);
+            // we allow other end get it first
+            self.write.store((write + 1) % self.list.len(), Ordering::Release);
+            notify_spin_lock();
+            if !prev.is_null() {
+                unsafe {
+                    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, T> {
-            sender: &'a mut Sender<T>,
-            content: Option<Box<T>>,
+        struct Send<'a, 'b, T> where T: Clone, 'b: 'a {
+            sender: &'a mut Sender<'b, T>,
+            content: Result<(), Box<T>>,
        }

-        impl<T> Future for Send<'_, T> {
+        impl<T> Future for Send<'_, '_, T> where T: Clone {
            type Output = ();

            fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
-                match self.content.take() {
-                    Some(content) => {
-                        if let Some(content) = self.sender.try_send(content) {
+                match core::mem::replace(&mut self.content, Ok(())) {
+                    Err(content) => {
+                        if let Err(content) = self.sender.try_send(content) {
                            // failure
-                            self.content = Some(content);
+                            self.content = Err(content);
                            cx.waker().wake_by_ref();
                            Poll::Pending
                        } else {
@ -117,93 +74,80 @@ impl<T> Sender<T> {
                            Poll::Ready(())
                        }
                    }
-                    None => panic!("Send future polled after success"),
+                    Ok(_) => panic!("Send future polled after success"),
                }
            }
        }

        Send {
            sender: self,
-            content: Some(content.into()),
+            content: Err(content.into()),
        }.await
    }
-}

-
-
-
-/// Receiving half of a channel
-pub struct Receiver<T> {
-    channel: Arc<Channel<T>>,
-    pos: usize,
-}
-
-impl<T> Receiver<T> {
-    /// Blocking receive
-    pub fn recv(&mut self) -> Box<T> {
-        let entry = &self.channel[self.pos];
-
-        loop {
-            dmb();
-            let ptr = entry.swap(null_mut(), Ordering::Release);
-            if ptr != null_mut() {
-                dsb();
-                // wake power-saving senders
-                sev();
-
-                let content = unsafe { Box::from_raw(ptr) };
-
-                // advance
-                self.pos += 1;
-                // wrap
-                if self.pos >= self.channel.len() {
-                    self.pos = 0;
-                }
-
-                return content;
+    /// free all items in the queue. It is the user's responsibility to
+    /// ensure no reader is trying to copy the data.
+    pub unsafe fn drop_elements(&mut self) {
+        for v in self.list.iter() {
+            let original = v.swap(core::ptr::null_mut(), Ordering::Relaxed);
+            if !original.is_null() {
+                Box::from_raw(original);
            }
-
-            // power-saving
-            wfe();
        }
    }

-    /// Non-blocking receive
-    pub fn try_recv(&mut self) -> Option<Box<T>> {
-        let entry = &self.channel[self.pos];
+    /// Reset the `sync_channel`, *forget* all items in the queue. Affects both the sender and
+    /// receiver.
+    pub unsafe fn reset(&mut self) {
+        self.write.store(0, Ordering::Relaxed);
+        self.read.store(0, Ordering::Relaxed);
+        for v in self.list.iter() {
+            v.store(core::ptr::null_mut(), Ordering::Relaxed);
+        }
+    }
+}

-        dmb();
-        let ptr = entry.swap(null_mut(), Ordering::Release);
-        if ptr != null_mut() {
-            dsb();
-            // wake power-saving senders
-            sev();
+impl<'a, T> Receiver<'a, T> where T: Clone {
+    pub const fn new(list: &'static [AtomicPtr<T>], write: &'static AtomicUsize, read: &'static AtomicUsize) -> Self {
+        Receiver {list, write, read}
+    }

-            let content = unsafe { Box::from_raw(ptr) };
-
-            // advance
-            self.pos += 1;
-            // wrap
-            if self.pos >= self.channel.len() {
-                self.pos = 0;
-            }
-
-            Some(content)
+    pub fn try_recv(&mut self) -> Result<T, ()> {
+        let read = self.read.load(Ordering::Relaxed);
+        if read == self.write.load(Ordering::Acquire) {
+            Err(())
        } else {
-            None
+            let entry = &self.list[read];
+            let data = unsafe {
+                // we cannot deallocate the box
+                Box::leak(Box::from_raw(entry.load(Ordering::Relaxed)))
+            };
+            let result = data.clone();
+            self.read.store((read + 1) % self.list.len(), Ordering::Release);
+            notify_spin_lock();
+            Ok(result)
        }
    }

-    pub async fn async_recv(&mut self) -> Box<T> {
-        struct Recv<'a, T> {
-            receiver: &'a mut Receiver<T>,
+    pub fn recv(&mut self) -> T {
+        loop {
+            if let Ok(data) = self.try_recv() {
+                return data;
+            }
+            spin_lock_yield();
+        }
+    }
+
+    pub async fn async_recv(&mut self) -> T {
+        struct Recv<'a, 'b, T> where T: Clone, 'b: 'a {
+            receiver: &'a mut Receiver<'b, T>,
        }

-        impl<T> Future for Recv<'_, T> {
-            type Output = Box<T>;
+        impl<T> Future for Recv<'_, '_, T> where T: Clone {
+            type Output = T;

            fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
-                if let Some(content) = self.receiver.try_recv() {
+                if let Ok(content) = self.receiver.try_recv() {
                    Poll::Ready(content)
                } else {
                    cx.waker().wake_by_ref();
@ -218,10 +162,28 @@ impl<T> Receiver<T> {
    }
 }

-impl<T> Iterator for Receiver<T> {
-    type Item = Box<T>;
+impl<'a, T> Iterator for Receiver<'a, T> where T: Clone {
+    type Item = T;

    fn next(&mut self) -> Option<Self::Item> {
        Some(self.recv())
    }
 }
+
+#[macro_export]
+/// Macro for initializing the sync_channel with static buffer and indexes.
+macro_rules! sync_channel {
+    ($t: ty, $cap: expr) => {
+        {
+            use core::sync::atomic::{AtomicUsize, AtomicPtr};
+            use $crate::sync_channel::{Sender, Receiver};
+            const fn new_atomic() -> AtomicPtr<$t> {
+                AtomicPtr::new(core::ptr::null_mut())
+            }
+            static LIST: [AtomicPtr<$t>; $cap + 1] = [const { new_atomic() }; $cap + 1];
+            static WRITE: AtomicUsize = AtomicUsize::new(0);
+            static READ: AtomicUsize = AtomicUsize::new(0);
+            (Sender::new(&LIST, &WRITE, &READ), Receiver::new(&LIST, &WRITE, &READ))
+        }
+    };
+}
--- a/libcortex_a9/src/uncached.rs
+++ b/libcortex_a9/src/uncached.rs
@ -2,7 +2,7 @@ use core::{
    ops::{Deref, DerefMut},
    mem::{align_of, size_of},
 };
-use alloc::alloc::{dealloc, Layout, LayoutErr};
+use alloc::alloc::{dealloc, Layout, LayoutError};
 use crate::mmu::{L1_PAGE_SIZE, L1Table};

 pub struct UncachedSlice<T: 'static> {
@ -12,7 +12,7 @@ pub struct UncachedSlice<T: 'static> {

 impl<T> UncachedSlice<T> {
    /// allocates in chunks of 1 MB
-    pub fn new<F: Fn() -> T>(len: usize, default: F) -> Result<Self, LayoutErr> {
+    pub fn new<F: Fn() -> T>(len: usize, default: F) -> Result<Self, LayoutError> {
        // round to full pages
        let size = ((len * size_of::<T>() - 1) | (L1_PAGE_SIZE - 1)) + 1;
        let align = align_of::<T>()
@ -23,12 +23,12 @@ impl<T> UncachedSlice<T> {
        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| {
-                    // Shareable Device
-                    l1_section.tex = 0b000;
-                    l1_section.cacheable = false;
-                    l1_section.bufferable = true;
+                    l1_section.tex = 0b10;
+                    l1_section.cacheable = true;
+                    l1_section.bufferable = false;
                });
        }

--- 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
@ -30,8 +30,9 @@ pub trait RegisterRW: RegisterR + RegisterW {
 #[doc(hidden)]
 #[macro_export]
 macro_rules! register_common {
-    ($mod_name: ident, $struct_name: ident, $access: ty, $inner: ty) => (
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, $access: ty, $inner: ty) => (
        #[repr(C)]
+        $(#[$outer])*
        pub struct $struct_name {
            inner: $access,
        }
@ -52,7 +53,7 @@ macro_rules! register_common {
 #[macro_export]
 macro_rules! register_r {
    ($mod_name: ident, $struct_name: ident) => (
-        impl libregister::RegisterR for $struct_name {
+        impl $crate::RegisterR for $struct_name {
            type R = $mod_name::Read;

            #[inline]
@ -67,7 +68,7 @@ macro_rules! register_r {
 #[macro_export]
 macro_rules! register_w {
    ($mod_name: ident, $struct_name: ident) => (
-        impl libregister::RegisterW for $struct_name {
+        impl $crate::RegisterW for $struct_name {
            type W = $mod_name::Write;

            #[inline]
@ -88,7 +89,7 @@ macro_rules! register_w {
 #[macro_export]
 macro_rules! register_rw {
    ($mod_name: ident, $struct_name: ident) => (
-        impl libregister::RegisterRW for $struct_name {
+        impl $crate::RegisterRW for $struct_name {
            #[inline]
            fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
                unsafe {
@ -101,7 +102,7 @@ macro_rules! register_rw {
        }
    );
    ($mod_name: ident, $struct_name: ident, $mask: expr) => (
-        impl libregister::RegisterRW for $struct_name {
+        impl $crate::RegisterRW for $struct_name {
            #[inline]
            fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
                unsafe {
@ -119,7 +120,7 @@ macro_rules! register_rw {
 #[macro_export]
 macro_rules! register_vcell {
    ($mod_name: ident, $struct_name: ident) => (
-        impl libregister::RegisterR for $struct_name {
+        impl $crate::RegisterR for $struct_name {
            type R = $mod_name::Read;

            #[inline]
@ -128,7 +129,7 @@ macro_rules! register_vcell {
                $mod_name::Read { inner }
            }
        }
-        impl libregister::RegisterW for $struct_name {
+        impl $crate::RegisterW for $struct_name {
            type W = $mod_name::Write;

            #[inline]
@ -141,7 +142,7 @@ macro_rules! register_vcell {
                self.inner.set(w.inner);
            }
        }
-        impl libregister::RegisterRW for $struct_name {
+        impl $crate::RegisterRW for $struct_name {
            #[inline]
            fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
                let r = self.read();
@ -157,37 +158,37 @@ macro_rules! register_vcell {
 #[macro_export]
 macro_rules! register {
    // Define read-only register
-    ($mod_name: ident, $struct_name: ident, RO, $inner: ty) => (
-        libregister::register_common!($mod_name, $struct_name, libregister::RO<$inner>, $inner);
-        libregister::register_r!($mod_name, $struct_name);
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, RO, $inner: ty) => (
+        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, $crate::RO<$inner>, $inner);
+        $crate::register_r!($mod_name, $struct_name);
    );

    // Define write-only register
-    ($mod_name: ident, $struct_name: ident, WO, $inner: ty) => (
-        libregister::register_common!($mod_name, $struct_name, volatile_register::WO<$inner>, $inner);
-        libregister::register_w!($mod_name, $struct_name);
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, WO, $inner: ty) => (
+        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, volatile_register::WO<$inner>, $inner);
+        $crate::register_w!($mod_name, $struct_name);
    );

    // Define read-write register
-    ($mod_name: ident, $struct_name: ident, RW, $inner: ty) => (
-        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);
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, RW, $inner: ty) => (
+        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, volatile_register::RW<$inner>, $inner);
+        $crate::register_r!($mod_name, $struct_name);
+        $crate::register_w!($mod_name, $struct_name);
+        $crate::register_rw!($mod_name, $struct_name);
    );

    // Define read-write register
-    ($mod_name: ident, $struct_name: ident, VolatileCell, $inner: ty) => (
-        libregister::register_common!($mod_name, $struct_name, VolatileCell<$inner>, $inner);
-        libregister::register_vcell!($mod_name, $struct_name);
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, VolatileCell, $inner: ty) => (
+        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, VolatileCell<$inner>, $inner);
+        $crate::register_vcell!($mod_name, $struct_name);
    );

    // Define read-write register with mask on write (for WTC mixed access.)
-    ($mod_name: ident, $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);
+    ($mod_name: ident, $(#[$outer:meta])* $struct_name: ident, RW, $inner: ty, $mask: expr) => (
+        $crate::register_common!($mod_name, $(#[$outer])* $struct_name, volatile_register::RW<$inner>, $inner);
+        $crate::register_r!($mod_name, $struct_name);
+        $crate::register_w!($mod_name, $struct_name);
+        $crate::register_rw!($mod_name, $struct_name, $mask);
    );
 }

--- a/libsupport_zynq/Cargo.toml
+++ b/libsupport_zynq/Cargo.toml
@ -2,17 +2,29 @@
 name = "libsupport_zynq"
 description = "Software support for running in the Zynq PS"
 version = "0.0.0"
-authors = ["Astro <astro@spaceboyz.net>"]
+authors = ["M-Labs"]
 edition = "2018"

 [features]
 target_zc706 = ["libboard_zynq/target_zc706"]
-target_cora_z7_10 = ["libboard_zynq/target_cora_z7_10"]
+target_coraz7 = ["libboard_zynq/target_coraz7"]
+target_ebaz4205 = ["libboard_zynq/target_ebaz4205"]
+target_redpitaya = ["libboard_zynq/target_redpitaya"]
+target_kasli_soc = ["libboard_zynq/target_kasli_soc"]
+panic_handler = []
+dummy_irq_handler = []
+dummy_fiq_handler = []
+alloc_core = []
+
+default = ["panic_handler", "dummy_irq_handler", "dummy_fiq_handler"]

 [dependencies]
 r0 = "1"
-compiler_builtins = "0.1"
-linked_list_allocator = { version = "0.8", default-features = false }
+compiler_builtins = "=0.1.49"
+linked_list_allocator = { version = "0.8", default-features = false, features = ["const_mut_refs"] }
 libregister = { path = "../libregister" }
 libcortex_a9 = { path = "../libcortex_a9" }
 libboard_zynq = { path = "../libboard_zynq" }
+
+[build-dependencies]
+cc = { version = "1.0" }
--- a/libsupport_zynq/build.rs
+++ b/libsupport_zynq/build.rs
@ -0,0 +1,24 @@
+fn main() {
+    println!("cargo:rerun-if-changed=build.rs");
+    compile_memcpy();
+}
+
+fn compile_memcpy() {
+    use std::path::Path;
+    extern crate cc;
+    let cfg = &mut cc::Build::new();
+    cfg.compiler("clang");
+    cfg.no_default_flags(true);
+    cfg.warnings(false);
+    cfg.flag("--target=armv7-none-eabihf");
+    let sources = vec![
+        "memcpy.S",
+    ];
+    let root = Path::new("src/asm");
+    for src in sources {
+        println!("cargo:rerun-if-changed={}", src);
+        cfg.file(root.join(src));
+    }
+    cfg.compile("memcpy");
+}
+
--- a/libsupport_zynq/src/abort.rs
+++ b/libsupport_zynq/src/abort.rs
@ -1,24 +0,0 @@
-use libregister::RegisterR;
-use libcortex_a9::regs::DFSR;
-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");
-    println!("DFSR: {:03X}", DFSR.read());
-
-    slcr::RegisterBlock::unlocked(|slcr| slcr.soft_reset());
-    loop {}
-}
--- a/libsupport_zynq/src/asm/memcpy.S
+++ b/libsupport_zynq/src/asm/memcpy.S
@ -0,0 +1,626 @@
+/* Copyright (c) 2013, Linaro Limited
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions
+   are met:
+
+      * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+
+      * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+
+      * Neither the name of Linaro Limited nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+   HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+   This memcpy routine is optimised for Cortex-A15 cores and takes advantage
+   of VFP or NEON when built with the appropriate flags.
+
+   Assumptions:
+
+    ARMv6 (ARMv7-a if using Neon)
+    ARM state
+    Unaligned accesses
+    LDRD/STRD support unaligned word accesses
+
+   If compiled with GCC, this file should be enclosed within following
+   pre-processing check:
+   if defined (__ARM_ARCH_7A__) && defined (__ARM_FEATURE_UNALIGNED)
+
+ */
+	.syntax unified
+	/* This implementation requires ARM state.  */
+	.arm
+
+#ifdef __ARM_NEON__
+
+	.fpu	neon
+	.arch	armv7-a
+# define FRAME_SIZE	4
+# define USE_VFP
+# define USE_NEON
+
+#elif !defined (__SOFTFP__)
+
+	.arch	armv6
+	.fpu	vfpv2
+# define FRAME_SIZE	32
+# define USE_VFP
+
+#else
+	.arch	armv6
+# define FRAME_SIZE    32
+
+#endif
+
+/* Old versions of GAS incorrectly implement the NEON align semantics.  */
+#ifdef BROKEN_ASM_NEON_ALIGN
+#define ALIGN(addr, align) addr,:align
+#else
+#define ALIGN(addr, align) addr:align
+#endif
+
+#define PC_OFFSET	8	/* PC pipeline compensation.  */
+#define INSN_SIZE	4
+
+/* Call parameters.  */
+#define dstin	r0
+#define src	r1
+#define count	r2
+
+/* Locals.  */
+#define tmp1	r3
+#define dst	ip
+#define tmp2	r10
+
+#ifndef USE_NEON
+/* For bulk copies using GP registers.  */
+#define	A_l	r2		/* Call-clobbered.  */
+#define	A_h	r3		/* Call-clobbered.  */
+#define	B_l	r4
+#define	B_h	r5
+#define	C_l	r6
+#define	C_h	r7
+#define	D_l	r8
+#define	D_h	r9
+#endif
+
+/* Number of lines ahead to pre-fetch data.  If you change this the code
+   below will need adjustment to compensate.  */
+
+#define prefetch_lines	5
+
+#ifdef USE_VFP
+	.macro	cpy_line_vfp vreg, base
+	vstr	\vreg, [dst, #\base]
+	vldr	\vreg, [src, #\base]
+	vstr	d0, [dst, #\base + 8]
+	vldr	d0, [src, #\base + 8]
+	vstr	d1, [dst, #\base + 16]
+	vldr	d1, [src, #\base + 16]
+	vstr	d2, [dst, #\base + 24]
+	vldr	d2, [src, #\base + 24]
+	vstr	\vreg, [dst, #\base + 32]
+	vldr	\vreg, [src, #\base + prefetch_lines * 64 - 32]
+	vstr	d0, [dst, #\base + 40]
+	vldr	d0, [src, #\base + 40]
+	vstr	d1, [dst, #\base + 48]
+	vldr	d1, [src, #\base + 48]
+	vstr	d2, [dst, #\base + 56]
+	vldr	d2, [src, #\base + 56]
+	.endm
+
+	.macro	cpy_tail_vfp vreg, base
+	vstr	\vreg, [dst, #\base]
+	vldr	\vreg, [src, #\base]
+	vstr	d0, [dst, #\base + 8]
+	vldr	d0, [src, #\base + 8]
+	vstr	d1, [dst, #\base + 16]
+	vldr	d1, [src, #\base + 16]
+	vstr	d2, [dst, #\base + 24]
+	vldr	d2, [src, #\base + 24]
+	vstr	\vreg, [dst, #\base + 32]
+	vstr	d0, [dst, #\base + 40]
+	vldr	d0, [src, #\base + 40]
+	vstr	d1, [dst, #\base + 48]
+	vldr	d1, [src, #\base + 48]
+	vstr	d2, [dst, #\base + 56]
+	vldr	d2, [src, #\base + 56]
+	.endm
+#endif
+
+	.macro def_fn f p2align=0
+	.text
+	.p2align \p2align
+	.global \f
+	.type \f, %function
+\f:
+	.endm
+
+	.global __aeabi_memcpy
+	.global __aeabi_memcpy4
+	.global __aeabi_memcpy8
+	.set __aeabi_memcpy, fast_memcpy
+	.set __aeabi_memcpy4, fast_memcpy
+	.set __aeabi_memcpy8, fast_memcpy
+def_fn fast_memcpy p2align=6
+
+	mov	dst, dstin	/* Preserve dstin, we need to return it.  */
+	cmp	count, #64
+	bge	.Lcpy_not_short
+	/* Deal with small copies quickly by dropping straight into the
+	   exit block.  */
+
+.Ltail63unaligned:
+#ifdef USE_NEON
+	and	tmp1, count, #0x38
+	rsb	tmp1, tmp1, #(56 - PC_OFFSET + INSN_SIZE)
+	add	pc, pc, tmp1
+	vld1.8	{d0}, [src]!	/* 14 words to go.  */
+	vst1.8	{d0}, [dst]!
+	vld1.8	{d0}, [src]!	/* 12 words to go.  */
+	vst1.8	{d0}, [dst]!
+	vld1.8	{d0}, [src]!	/* 10 words to go.  */
+	vst1.8	{d0}, [dst]!
+	vld1.8	{d0}, [src]!	/* 8 words to go.  */
+	vst1.8	{d0}, [dst]!
+	vld1.8	{d0}, [src]!	/* 6 words to go.  */
+	vst1.8	{d0}, [dst]!
+	vld1.8	{d0}, [src]!	/* 4 words to go.  */
+	vst1.8	{d0}, [dst]!
+	vld1.8	{d0}, [src]!	/* 2 words to go.  */
+	vst1.8	{d0}, [dst]!
+
+	tst	count, #4
+	ldrne	tmp1, [src], #4
+	strne	tmp1, [dst], #4
+#else
+	/* Copy up to 15 full words of data.  May not be aligned.  */
+	/* Cannot use VFP for unaligned data.  */
+	and	tmp1, count, #0x3c
+	add	dst, dst, tmp1
+	add	src, src, tmp1
+	rsb	tmp1, tmp1, #(60 - PC_OFFSET/2 + INSN_SIZE/2)
+	/* Jump directly into the sequence below at the correct offset.  */
+	add	pc, pc, tmp1, lsl #1
+
+	ldr	tmp1, [src, #-60]	/* 15 words to go.  */
+	str	tmp1, [dst, #-60]
+
+	ldr	tmp1, [src, #-56]	/* 14 words to go.  */
+	str	tmp1, [dst, #-56]
+	ldr	tmp1, [src, #-52]
+	str	tmp1, [dst, #-52]
+
+	ldr	tmp1, [src, #-48]	/* 12 words to go.  */
+	str	tmp1, [dst, #-48]
+	ldr	tmp1, [src, #-44]
+	str	tmp1, [dst, #-44]
+
+	ldr	tmp1, [src, #-40]	/* 10 words to go.  */
+	str	tmp1, [dst, #-40]
+	ldr	tmp1, [src, #-36]
+	str	tmp1, [dst, #-36]
+
+	ldr	tmp1, [src, #-32]	/* 8 words to go.  */
+	str	tmp1, [dst, #-32]
+	ldr	tmp1, [src, #-28]
+	str	tmp1, [dst, #-28]
+
+	ldr	tmp1, [src, #-24]	/* 6 words to go.  */
+	str	tmp1, [dst, #-24]
+	ldr	tmp1, [src, #-20]
+	str	tmp1, [dst, #-20]
+
+	ldr	tmp1, [src, #-16]	/* 4 words to go.  */
+	str	tmp1, [dst, #-16]
+	ldr	tmp1, [src, #-12]
+	str	tmp1, [dst, #-12]
+
+	ldr	tmp1, [src, #-8]	/* 2 words to go.  */
+	str	tmp1, [dst, #-8]
+	ldr	tmp1, [src, #-4]
+	str	tmp1, [dst, #-4]
+#endif
+
+	lsls	count, count, #31
+	ldrhcs	tmp1, [src], #2
+	ldrbne	src, [src]		/* Src is dead, use as a scratch.  */
+	strhcs	tmp1, [dst], #2
+	strbne	src, [dst]
+	bx	lr
+
+.Lcpy_not_short:
+	/* At least 64 bytes to copy, but don't know the alignment yet.  */
+	str	tmp2, [sp, #-FRAME_SIZE]!
+	and	tmp2, src, #7
+	and	tmp1, dst, #7
+	cmp	tmp1, tmp2
+	bne	.Lcpy_notaligned
+
+#ifdef USE_VFP
+	/* Magic dust alert!  Force VFP on Cortex-A9.  Experiments show
+	   that the FP pipeline is much better at streaming loads and
+	   stores.  This is outside the critical loop.  */
+	vmov.f32	s0, s0
+#endif
+
+	/* SRC and DST have the same mutual 32-bit alignment, but we may
+	   still need to pre-copy some bytes to get to natural alignment.
+	   We bring DST into full 64-bit alignment.  */
+	lsls	tmp2, dst, #29
+	beq	1f
+	rsbs	tmp2, tmp2, #0
+	sub	count, count, tmp2, lsr #29
+	ldrmi	tmp1, [src], #4
+	strmi	tmp1, [dst], #4
+	lsls	tmp2, tmp2, #2
+	ldrhcs	tmp1, [src], #2
+	ldrbne	tmp2, [src], #1
+	strhcs	tmp1, [dst], #2
+	strbne	tmp2, [dst], #1
+
+1:
+	subs	tmp2, count, #64	/* Use tmp2 for count.  */
+	blt	.Ltail63aligned
+
+	cmp	tmp2, #512
+	bge	.Lcpy_body_long
+
+.Lcpy_body_medium:			/* Count in tmp2.  */
+#ifdef USE_VFP
+1:
+	vldr	d0, [src, #0]
+	subs	tmp2, tmp2, #64
+	vldr	d1, [src, #8]
+	vstr	d0, [dst, #0]
+	vldr	d0, [src, #16]
+	vstr	d1, [dst, #8]
+	vldr	d1, [src, #24]
+	vstr	d0, [dst, #16]
+	vldr	d0, [src, #32]
+	vstr	d1, [dst, #24]
+	vldr	d1, [src, #40]
+	vstr	d0, [dst, #32]
+	vldr	d0, [src, #48]
+	vstr	d1, [dst, #40]
+	vldr	d1, [src, #56]
+	vstr	d0, [dst, #48]
+	add	src, src, #64
+	vstr	d1, [dst, #56]
+	add	dst, dst, #64
+	bge	1b
+	tst	tmp2, #0x3f
+	beq	.Ldone
+
+.Ltail63aligned:			/* Count in tmp2.  */
+	and	tmp1, tmp2, #0x38
+	add	dst, dst, tmp1
+	add	src, src, tmp1
+	rsb	tmp1, tmp1, #(56 - PC_OFFSET + INSN_SIZE)
+	add	pc, pc, tmp1
+
+	vldr	d0, [src, #-56]	/* 14 words to go.  */
+	vstr	d0, [dst, #-56]
+	vldr	d0, [src, #-48]	/* 12 words to go.  */
+	vstr	d0, [dst, #-48]
+	vldr	d0, [src, #-40]	/* 10 words to go.  */
+	vstr	d0, [dst, #-40]
+	vldr	d0, [src, #-32]	/* 8 words to go.  */
+	vstr	d0, [dst, #-32]
+	vldr	d0, [src, #-24]	/* 6 words to go.  */
+	vstr	d0, [dst, #-24]
+	vldr	d0, [src, #-16]	/* 4 words to go.  */
+	vstr	d0, [dst, #-16]
+	vldr	d0, [src, #-8]	/* 2 words to go.  */
+	vstr	d0, [dst, #-8]
+#else
+	sub	src, src, #8
+	sub	dst, dst, #8
+1:
+	ldrd	A_l, A_h, [src, #8]
+	strd	A_l, A_h, [dst, #8]
+	ldrd	A_l, A_h, [src, #16]
+	strd	A_l, A_h, [dst, #16]
+	ldrd	A_l, A_h, [src, #24]
+	strd	A_l, A_h, [dst, #24]
+	ldrd	A_l, A_h, [src, #32]
+	strd	A_l, A_h, [dst, #32]
+	ldrd	A_l, A_h, [src, #40]
+	strd	A_l, A_h, [dst, #40]
+	ldrd	A_l, A_h, [src, #48]
+	strd	A_l, A_h, [dst, #48]
+	ldrd	A_l, A_h, [src, #56]
+	strd	A_l, A_h, [dst, #56]
+	ldrd	A_l, A_h, [src, #64]!
+	strd	A_l, A_h, [dst, #64]!
+	subs	tmp2, tmp2, #64
+	bge	1b
+	tst	tmp2, #0x3f
+	bne	1f
+	ldr	tmp2,[sp], #FRAME_SIZE
+	bx	lr
+1:
+	add	src, src, #8
+	add	dst, dst, #8
+
+.Ltail63aligned:			/* Count in tmp2.  */
+	/* Copy up to 7 d-words of data.  Similar to Ltail63unaligned, but
+	   we know that the src and dest are 32-bit aligned so we can use
+	   LDRD/STRD to improve efficiency.  */
+	/* TMP2 is now negative, but we don't care about that.  The bottom
+	   six bits still tell us how many bytes are left to copy.  */
+
+	and	tmp1, tmp2, #0x38
+	add	dst, dst, tmp1
+	add	src, src, tmp1
+	rsb	tmp1, tmp1, #(56 - PC_OFFSET + INSN_SIZE)
+	add	pc, pc, tmp1
+	ldrd	A_l, A_h, [src, #-56]	/* 14 words to go.  */
+	strd	A_l, A_h, [dst, #-56]
+	ldrd	A_l, A_h, [src, #-48]	/* 12 words to go.  */
+	strd	A_l, A_h, [dst, #-48]
+	ldrd	A_l, A_h, [src, #-40]	/* 10 words to go.  */
+	strd	A_l, A_h, [dst, #-40]
+	ldrd	A_l, A_h, [src, #-32]	/* 8 words to go.  */
+	strd	A_l, A_h, [dst, #-32]
+	ldrd	A_l, A_h, [src, #-24]	/* 6 words to go.  */
+	strd	A_l, A_h, [dst, #-24]
+	ldrd	A_l, A_h, [src, #-16]	/* 4 words to go.  */
+	strd	A_l, A_h, [dst, #-16]
+	ldrd	A_l, A_h, [src, #-8]	/* 2 words to go.  */
+	strd	A_l, A_h, [dst, #-8]
+
+#endif
+	tst	tmp2, #4
+	ldrne	tmp1, [src], #4
+	strne	tmp1, [dst], #4
+	lsls	tmp2, tmp2, #31		/* Count (tmp2) now dead. */
+	ldrhcs	tmp1, [src], #2
+	ldrbne	tmp2, [src]
+	strhcs	tmp1, [dst], #2
+	strbne	tmp2, [dst]
+
+.Ldone:
+	ldr	tmp2, [sp], #FRAME_SIZE
+	bx	lr
+
+.Lcpy_body_long:			/* Count in tmp2.  */
+
+	/* Long copy.  We know that there's at least (prefetch_lines * 64)
+	   bytes to go.  */
+#ifdef USE_VFP
+	/* Don't use PLD.  Instead, read some data in advance of the current
+	   copy position into a register.  This should act like a PLD
+	   operation but we won't have to repeat the transfer.  */
+
+	vldr	d3, [src, #0]
+	vldr	d4, [src, #64]
+	vldr	d5, [src, #128]
+	vldr	d6, [src, #192]
+	vldr	d7, [src, #256]
+
+	vldr	d0, [src, #8]
+	vldr	d1, [src, #16]
+	vldr	d2, [src, #24]
+	add	src, src, #32
+
+	subs	tmp2, tmp2, #prefetch_lines * 64 * 2
+	blt	2f
+1:
+	cpy_line_vfp	d3, 0
+	cpy_line_vfp	d4, 64
+	cpy_line_vfp	d5, 128
+	add	dst, dst, #3 * 64
+	add	src, src, #3 * 64
+	cpy_line_vfp	d6, 0
+	cpy_line_vfp	d7, 64
+	add	dst, dst, #2 * 64
+	add	src, src, #2 * 64
+	subs	tmp2, tmp2, #prefetch_lines * 64
+	bge	1b
+
+2:
+	cpy_tail_vfp	d3, 0
+	cpy_tail_vfp	d4, 64
+	cpy_tail_vfp	d5, 128
+	add	src, src, #3 * 64
+	add	dst, dst, #3 * 64
+	cpy_tail_vfp	d6, 0
+	vstr	d7, [dst, #64]
+	vldr	d7, [src, #64]
+	vstr	d0, [dst, #64 + 8]
+	vldr	d0, [src, #64 + 8]
+	vstr	d1, [dst, #64 + 16]
+	vldr	d1, [src, #64 + 16]
+	vstr	d2, [dst, #64 + 24]
+	vldr	d2, [src, #64 + 24]
+	vstr	d7, [dst, #64 + 32]
+	add	src, src, #96
+	vstr	d0, [dst, #64 + 40]
+	vstr	d1, [dst, #64 + 48]
+	vstr	d2, [dst, #64 + 56]
+	add	dst, dst, #128
+	add	tmp2, tmp2, #prefetch_lines * 64
+	b	.Lcpy_body_medium
+#else
+	/* Long copy.  Use an SMS style loop to maximize the I/O
+	   bandwidth of the core.  We don't have enough spare registers
+	   to synthesise prefetching, so use PLD operations.  */
+	/* Pre-bias src and dst.  */
+	sub	src, src, #8
+	sub	dst, dst, #8
+	pld	[src, #8]
+	pld	[src, #72]
+	subs	tmp2, tmp2, #64
+	pld	[src, #136]
+	ldrd	A_l, A_h, [src, #8]
+	strd	B_l, B_h, [sp, #8]
+	ldrd	B_l, B_h, [src, #16]
+	strd	C_l, C_h, [sp, #16]
+	ldrd	C_l, C_h, [src, #24]
+	strd	D_l, D_h, [sp, #24]
+	pld	[src, #200]
+	ldrd	D_l, D_h, [src, #32]!
+	b	1f
+	.p2align	6
+2:
+	pld	[src, #232]
+	strd	A_l, A_h, [dst, #40]
+	ldrd	A_l, A_h, [src, #40]
+	strd	B_l, B_h, [dst, #48]
+	ldrd	B_l, B_h, [src, #48]
+	strd	C_l, C_h, [dst, #56]
+	ldrd	C_l, C_h, [src, #56]
+	strd	D_l, D_h, [dst, #64]!
+	ldrd	D_l, D_h, [src, #64]!
+	subs	tmp2, tmp2, #64
+1:
+	strd	A_l, A_h, [dst, #8]
+	ldrd	A_l, A_h, [src, #8]
+	strd	B_l, B_h, [dst, #16]
+	ldrd	B_l, B_h, [src, #16]
+	strd	C_l, C_h, [dst, #24]
+	ldrd	C_l, C_h, [src, #24]
+	strd	D_l, D_h, [dst, #32]
+	ldrd	D_l, D_h, [src, #32]
+	bcs	2b
+	/* Save the remaining bytes and restore the callee-saved regs.  */
+	strd	A_l, A_h, [dst, #40]
+	add	src, src, #40
+	strd	B_l, B_h, [dst, #48]
+	ldrd	B_l, B_h, [sp, #8]
+	strd	C_l, C_h, [dst, #56]
+	ldrd	C_l, C_h, [sp, #16]
+	strd	D_l, D_h, [dst, #64]
+	ldrd	D_l, D_h, [sp, #24]
+	add	dst, dst, #72
+	tst	tmp2, #0x3f
+	bne	.Ltail63aligned
+	ldr	tmp2, [sp], #FRAME_SIZE
+	bx	lr
+#endif
+
+.Lcpy_notaligned:
+	pld	[src]
+	pld	[src, #64]
+	/* There's at least 64 bytes to copy, but there is no mutual
+	   alignment.  */
+	/* Bring DST to 64-bit alignment.  */
+	lsls	tmp2, dst, #29
+	pld	[src, #(2 * 64)]
+	beq	1f
+	rsbs	tmp2, tmp2, #0
+	sub	count, count, tmp2, lsr #29
+	ldrmi	tmp1, [src], #4
+	strmi	tmp1, [dst], #4
+	lsls	tmp2, tmp2, #2
+	ldrbne	tmp1, [src], #1
+	ldrhcs	tmp2, [src], #2
+	strbne	tmp1, [dst], #1
+	strhcs	tmp2, [dst], #2
+1:
+	pld	[src, #(3 * 64)]
+	subs	count, count, #64
+	ldrmi	tmp2, [sp], #FRAME_SIZE
+	bmi	.Ltail63unaligned
+	pld	[src, #(4 * 64)]
+
+#ifdef USE_NEON
+	vld1.8	{d0-d3}, [src]!
+	vld1.8	{d4-d7}, [src]!
+	subs	count, count, #64
+	bmi	2f
+1:
+	pld	[src, #(4 * 64)]
+	vst1.8	{d0-d3}, [ALIGN (dst, 64)]!
+	vld1.8	{d0-d3}, [src]!
+	vst1.8	{d4-d7}, [ALIGN (dst, 64)]!
+	vld1.8	{d4-d7}, [src]!
+	subs	count, count, #64
+	bpl	1b
+2:
+	vst1.8	{d0-d3}, [ALIGN (dst, 64)]!
+	vst1.8	{d4-d7}, [ALIGN (dst, 64)]!
+	ands	count, count, #0x3f
+#else
+	/* Use an SMS style loop to maximize the I/O bandwidth.  */
+	sub	src, src, #4
+	sub	dst, dst, #8
+	subs	tmp2, count, #64	/* Use tmp2 for count.  */
+	ldr	A_l, [src, #4]
+	ldr	A_h, [src, #8]
+	strd	B_l, B_h, [sp, #8]
+	ldr	B_l, [src, #12]
+	ldr	B_h, [src, #16]
+	strd	C_l, C_h, [sp, #16]
+	ldr	C_l, [src, #20]
+	ldr	C_h, [src, #24]
+	strd	D_l, D_h, [sp, #24]
+	ldr	D_l, [src, #28]
+	ldr	D_h, [src, #32]!
+	b	1f
+	.p2align	6
+2:
+	pld	[src, #(5 * 64) - (32 - 4)]
+	strd	A_l, A_h, [dst, #40]
+	ldr	A_l, [src, #36]
+	ldr	A_h, [src, #40]
+	strd	B_l, B_h, [dst, #48]
+	ldr	B_l, [src, #44]
+	ldr	B_h, [src, #48]
+	strd	C_l, C_h, [dst, #56]
+	ldr	C_l, [src, #52]
+	ldr	C_h, [src, #56]
+	strd	D_l, D_h, [dst, #64]!
+	ldr	D_l, [src, #60]
+	ldr	D_h, [src, #64]!
+	subs	tmp2, tmp2, #64
+1:
+	strd	A_l, A_h, [dst, #8]
+	ldr	A_l, [src, #4]
+	ldr	A_h, [src, #8]
+	strd	B_l, B_h, [dst, #16]
+	ldr	B_l, [src, #12]
+	ldr	B_h, [src, #16]
+	strd	C_l, C_h, [dst, #24]
+	ldr	C_l, [src, #20]
+	ldr	C_h, [src, #24]
+	strd	D_l, D_h, [dst, #32]
+	ldr	D_l, [src, #28]
+	ldr	D_h, [src, #32]
+	bcs	2b
+
+	/* Save the remaining bytes and restore the callee-saved regs.  */
+	strd	A_l, A_h, [dst, #40]
+	add	src, src, #36
+	strd	B_l, B_h, [dst, #48]
+	ldrd	B_l, B_h, [sp, #8]
+	strd	C_l, C_h, [dst, #56]
+	ldrd	C_l, C_h, [sp, #16]
+	strd	D_l, D_h, [dst, #64]
+	ldrd	D_l, D_h, [sp, #24]
+	add	dst, dst, #72
+	ands	count, tmp2, #0x3f
+#endif
+	ldr	tmp2, [sp], #FRAME_SIZE
+	bne	.Ltail63unaligned
+	bx	lr
+
+	.size	memcpy, . - memcpy
+
--- a/libsupport_zynq/src/boot.rs
+++ b/libsupport_zynq/src/boot.rs
@ -1,10 +1,11 @@
 use r0::zero_bss;
 use core::ptr::write_volatile;
+use core::arch::asm;
 use libregister::{
    VolatileCell,
-    RegisterR, RegisterW, RegisterRW,
+    RegisterR, RegisterRW,
 };
-use libcortex_a9::{asm, regs::*, cache, mmu};
+use libcortex_a9::{asm, l2c, regs::*, cache, mmu, spin_lock_yield, notify_spin_lock, enable_fpu, interrupt_handler};
 use libboard_zynq::{slcr, mpcore};

 extern "C" {
@ -18,34 +19,28 @@ extern "C" {

 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 {
+interrupt_handler!(Reset, reset_irq, __stack0_start, __stack1_start, {
+    // no need to setup stack here, as we already did when entering the handler
+    match MPIDR.read().cpu_id() {
        0 => {
-            SP.write(&mut __stack0_start as *mut _ as u32);
            boot_core0();
        }
        1 => {
            while !CORE1_ENABLED.get() {
-                asm::wfe();
+                spin_lock_yield();
            }
-            SP.write(&mut __stack1_start as *mut _ as u32);
            boot_core1();
        }
        _ => unreachable!(),
    }
-}
+});

-#[naked]
 #[inline(never)]
-unsafe fn boot_core0() -> ! {
+unsafe extern "C" fn boot_core0() -> ! {
    l1_cache_init();

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

    zero_bss(&mut __bss_start, &mut __bss_end);
@ -55,30 +50,35 @@ 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_fiq();
+        asm::enable_irq();
        main_core0();
        panic!("return from main");
    });
 }

-#[naked]
 #[inline(never)]
-unsafe fn boot_core1() -> ! {
+unsafe extern "C" 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_fiq();
+        asm::enable_irq();
        main_core1();
        panic!("return from main_core1");
    });
@ -101,7 +101,7 @@ 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 {
@ -131,12 +131,13 @@ impl Core1 {
        unsafe {
            CORE1_ENABLED.set(true);
        }
-        // Ensure values have been written to cache
-        asm::dmb();
        // Flush cache-line
-        cache::dccmvac(unsafe { &CORE1_ENABLED } as *const _ as usize);
+        cache::dcc(unsafe { &CORE1_ENABLED });
        if sdram {
            cache::dccmvac(0);
+            asm::dsb();
+            l2c::l2_cache_clean(0);
+            l2c::l2_cache_sync();
        }

        // wake up core1
@ -144,6 +145,7 @@ impl Core1 {
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(false));
        });
+        notify_spin_lock();

        Core1 {}
    }
@ -151,6 +153,8 @@ impl Core1 {
    pub fn disable(&self) {
        unsafe {
            CORE1_ENABLED.set(false);
+            cache::dccmvac(&CORE1_ENABLED  as *const _ as usize);
+            asm::dsb();
        }
        self.restart();
    }
@ -158,7 +162,9 @@ impl Core1 {
    pub fn restart(&self) {
        slcr::RegisterBlock::unlocked(|slcr| {
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(true));
+            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(true));
            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
+            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(false));
        });
    }
 }
--- a/libsupport_zynq/src/exception_vectors.rs
+++ b/libsupport_zynq/src/exception_vectors.rs
@ -0,0 +1,55 @@
+use libregister::{RegisterR, RegisterW};
+use libcortex_a9::{regs::{DFSR, MPIDR, VBAR}, interrupt_handler};
+use libboard_zynq::{println, stdio}; 
+use core::arch::asm;
+
+pub fn set_vector_table(base_addr: u32){
+    VBAR.write(base_addr);
+}
+
+interrupt_handler!(UndefinedInstruction, undefined_instruction, __irq_stack0_start, __irq_stack1_start, {
+    stdio::drop_uart();
+    println!("UndefinedInstruction");
+    loop {}
+});
+
+interrupt_handler!(SoftwareInterrupt, software_interrupt, __irq_stack0_start, __irq_stack1_start, {
+    stdio::drop_uart();
+    println!("SoftwareInterrupt");
+    loop {}
+});
+
+interrupt_handler!(PrefetchAbort, prefetch_abort, __irq_stack0_start, __irq_stack1_start, {
+    stdio::drop_uart();
+    println!("PrefetchAbort");
+    loop {}
+});
+
+interrupt_handler!(DataAbort, data_abort, __irq_stack0_start, __irq_stack1_start, {
+    stdio::drop_uart();
+
+    println!("DataAbort on core {}", MPIDR.read().cpu_id());
+    println!("DFSR: {:03X}", DFSR.read());
+
+    loop {}
+});
+
+interrupt_handler!(ReservedException, reserved_exception, __irq_stack0_start, __irq_stack1_start, {
+    stdio::drop_uart();
+    println!("ReservedException");
+    loop {}
+});
+
+#[cfg(feature = "dummy_irq_handler")]
+interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
+    stdio::drop_uart();
+    println!("IRQ");
+    loop {}
+});
+
+#[cfg(feature = "dummy_fiq_handler")]
+interrupt_handler!(FIQ, fiq, __irq_stack0_start, __irq_stack1_start, {
+    stdio::drop_uart();
+    println!("FIQ");
+    loop {}
+});
--- a/libsupport_zynq/src/lib.rs
+++ b/libsupport_zynq/src/lib.rs
@ -1,13 +1,17 @@
 #![no_std]

-#![feature(naked_functions)]
 #![feature(alloc_error_handler)]
 #![feature(panic_info_message)]
+#![feature(naked_functions)]
+#![feature(global_asm)]
+#![feature(asm)]

 pub extern crate alloc;
 pub extern crate compiler_builtins;

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

 #[panic_handler]
 fn panic(info: &core::panic::PanicInfo) -> ! {
@ -13,7 +15,10 @@ fn panic(info: &core::panic::PanicInfo) -> ! {
    } else {
        println!("");
    }
-
-    slcr::RegisterBlock::unlocked(|slcr| slcr.soft_reset());
+    #[cfg(feature = "target_kasli_soc")]
+    {
+        let mut err_led = ErrorLED::error_led();
+        err_led.toggle(true);
+    }
    loop {}
 }
--- a/libsupport_zynq/src/ram.rs
+++ b/libsupport_zynq/src/ram.rs
@ -1,55 +1,100 @@
+use alloc::alloc::Layout;
 use core::alloc::GlobalAlloc;
 use core::ptr::NonNull;
-use alloc::alloc::Layout;
+use libcortex_a9::{
+    mutex::Mutex,
+    regs::MPIDR
+};
+use libregister::RegisterR;
 use linked_list_allocator::Heap;
-use libcortex_a9::mutex::Mutex;
+#[cfg(not(feature = "alloc_core"))]
 use libboard_zynq::ddr::DdrRam;

 #[global_allocator]
-static ALLOCATOR: CortexA9Alloc = CortexA9Alloc(Mutex::new(Heap::empty()));
+static ALLOCATOR: CortexA9Alloc = CortexA9Alloc(
+    Mutex::new(Heap::empty()),
+    Mutex::new(Heap::empty()),
+);

-/// LockedHeap doesn't lock properly
-struct CortexA9Alloc(Mutex<Heap>);
+struct CortexA9Alloc(Mutex<Heap>, Mutex<Heap>);

 unsafe impl Sync for CortexA9Alloc {}

 unsafe impl GlobalAlloc for CortexA9Alloc {
    unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
-        self.0.lock()
-            .allocate_first_fit(layout)
-            .ok()
-            .map_or(0 as *mut u8, |allocation| allocation.as_ptr())
+        if cfg!(not(feature = "alloc_core")) || MPIDR.read().cpu_id() == 0 {
+            &self.0
+        } else {
+            &self.1
+        }
+        .lock()
+        .allocate_first_fit(layout)
+        .ok()
+        .map_or(0 as *mut u8, |allocation| allocation.as_ptr())
    }

    unsafe fn dealloc(&self, ptr: *mut u8, layout: Layout) {
-        self.0.lock()
-            .deallocate(NonNull::new_unchecked(ptr), layout)
+        if cfg!(not(feature = "alloc_core"))
+            || ((&__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()
+        ALLOCATOR
+            .0
+            .lock()
            .init(ddr.ptr::<u8>() as usize, ddr.size());
    }
 }

 extern "C" {
-    static __heap_start: usize;
-    static __heap_end: usize;
+    static __heap0_start: usize;
+    static __heap0_end: usize;
+    #[cfg(feature = "alloc_core")]
+    static __heap1_start: usize;
+    #[cfg(feature = "alloc_core")]
+    static __heap1_end: usize;
 }

-pub fn init_alloc_linker() {
+pub fn init_alloc_core0() {
    unsafe {
-        let start = &__heap_start as *const usize as usize;
-        let end = &__heap_end as *const usize as usize;
-        ALLOCATOR.0.lock()
-            .init(start, end - start);
+        let start = &__heap0_start as *const usize as usize;
+        let end = &__heap0_end as *const usize as usize;
+        ALLOCATOR.0.lock().init(start, end - start);
    }
 }

+#[cfg(feature = "alloc_core")]
+pub fn init_alloc_core1() {
+    unsafe {
+        let start = &__heap1_start as *const usize as usize;
+        let end = &__heap1_end as *const usize as usize;
+        ALLOCATOR.1.lock().init(start, end - start);
+    }
+}

 #[alloc_error_handler]
-fn alloc_error(_: core::alloc::Layout) -> ! {
-    panic!("alloc_error")
+fn alloc_error(layout: core::alloc::Layout) -> ! {
+    let id = MPIDR.read().cpu_id();
+    let used = if cfg!(not(feature = "alloc_core")) || id == 0 {
+        ALLOCATOR.0.lock().used()
+    } else {
+        ALLOCATOR.1.lock().used()
+    };
+    panic!(
+        "Core {} alloc_error, layout: {:?}, used memory: {}",
+        id,
+        layout,
+        used
+    );
 }
--- a/openocd/common.cfg
+++ b/openocd/common.cfg
@ -0,0 +1,20 @@
+set XC7_JSHUTDOWN 0x0d
+set XC7_JPROGRAM 0x0b
+set XC7_JSTART 0x0c
+set XC7_BYPASS 0x3f
+
+proc xc7_program {tap} {
+    global XC7_JSHUTDOWN XC7_JPROGRAM XC7_JSTART XC7_BYPASS
+    irscan $tap $XC7_JSHUTDOWN
+    irscan $tap $XC7_JPROGRAM
+    runtest 60000
+    #JSTART prevents this from working...
+    #irscan $tap $XC7_JSTART
+    runtest 2000
+    irscan $tap $XC7_BYPASS
+    runtest 2000
+}
+
+pld device virtex2 zynq.tap 1
+init
+xc7_program zynq.tap
--- a/openocd/cora-z7-10.cfg
+++ b/openocd/cora-z7-10.cfg
@ -5,37 +5,15 @@ set PL_TAPID 0x13722093
 set SMP 1

 source ./zynq-7000.cfg
-source ./xilinx-tcl.cfg
-source ./ps7_init.tcl

 reset_config srst_only srst_push_pull

-set XC7_JSHUTDOWN 0x0d
-set XC7_JPROGRAM 0x0b
-set XC7_JSTART 0x0c
-set XC7_BYPASS 0x3f
+source ./common.cfg

-proc xc7_program {tap} {
-    global XC7_JSHUTDOWN XC7_JPROGRAM XC7_JSTART XC7_BYPASS
-    irscan $tap $XC7_JSHUTDOWN
-    irscan $tap $XC7_JPROGRAM
-    runtest 60000
-    #JSTART prevents this from working...
-    #irscan $tap $XC7_JSTART
-    runtest 2000
-    irscan $tap $XC7_BYPASS
-    runtest 2000
-}
-
-pld device virtex2 zynq.tap 1
-init
-xc7_program zynq.tap
-
-xilinx_ps7_init
+reset halt

 # Disable MMU
 targets $_TARGETNAME_1
-arm mcr 15 0 1 0 0 [expr [arm mrc 15 0 1 0 0] & ~0xd]
+arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
 targets $_TARGETNAME_0
-arm mcr 15 0 1 0 0 [expr [arm mrc 15 0 1 0 0] & ~0xd]
-
+arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
--- a/openocd/digilent-hs2.cfg
+++ b/openocd/digilent-hs2.cfg
@ -0,0 +1,12 @@
+# this is the original file from OpenOCD, but with ftdi_device_desc
+# removed because some cables don't have it programmed.
+
+# this supports JTAG-HS2 (and apparently Nexys4 as well)
+
+adapter driver ftdi
+ftdi_vid_pid 0x0403 0x6014
+
+ftdi_channel 0
+ftdi_layout_init 0x00e8 0x60eb
+
+reset_config none
--- a/openocd/digilent_jtag_smt2_nc.cfg
+++ b/openocd/digilent_jtag_smt2_nc.cfg
@ -1,18 +0,0 @@
-#
-# Digilent JTAG-SMT2-NC
-#
-# http://store.digilentinc.com/jtag-smt2-nc-surface-mount-programming-module/
-# https://reference.digilentinc.com/_media/jtag_smt2nc/jtag-smt2-nc_rm.pdf
-#
-# Based on reference sheet (above) and Xilinx KCU105 schematics
-# https://www.xilinx.com/products/boards-and-kits/kcu105.html#documentation
-#
-# Note that the digilent_jtag_smt2 layout does not work and hangs while
-# the ftdi_device_desc from digilent_hs2 is wrong.
-
-interface ftdi
-ftdi_device_desc "Digilent USB Device"
-ftdi_vid_pid 0x0403 0x6014
-ftdi_channel 0
-ftdi_layout_init 0x00e8 0x60eb
-ftdi_layout_signal nSRST -data 0x2000
--- a/openocd/ebaz4205.cfg
+++ b/openocd/ebaz4205.cfg
@ -0,0 +1,33 @@
+# The contents of this file are partially dependend on
+# the adapter that you have. Please modify accordingly.
+adapter driver ftdi
+ftdi vid_pid 0x0403 0x6010
+ftdi channel 0
+# Every pin set as high impedance except TCK, TDI, TDO and TMS
+ftdi layout_init 0x0088 0x008b
+
+# nSRST defined on pin CN2-13 of the MiniModule (pin ADBUS5 [AD5] on the FT2232H chip)
+# This choice is arbitrary. Use other GPIO pin if desired.
+ftdi layout_signal nSRST -data 0x0020 -oe 0x0020
+
+transport select jtag
+adapter speed 10000
+
+set PL_TAPID 0x13722093
+set SMP 1
+
+source ./zynq-7000.cfg
+
+reset_config srst_only srst_open_drain
+adapter srst pulse_width 250
+adapter srst delay 400
+
+source ./common.cfg
+
+reset halt
+
+# Disable MMU
+targets $_TARGETNAME_1
+arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
+targets $_TARGETNAME_0
+arm mcr 15 0 1 0 0 [expr { [arm mrc 15 0 1 0 0] & ~0xd }]
--- a/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`