diff --git a/.travis.yml b/.travis.yml
index 2e86e059..721b6c73 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,6 +1,6 @@
 dist: trusty
 language: rust
-rust: nightly
+rust: nightly-2017-03-04
 services: docker
 sudo: required
 
diff --git a/CHANGELOG.md b/CHANGELOG.md
index ea5af0c6..dd278f72 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -7,6 +7,50 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## [Unreleased]
 
+### Added
+
+- Semihosting functionality in the `semihosting` module.
+
+- `exception::Handlers` struct that represent the section of the vector table
+  that contains the exception handlers.
+
+- A default exception handler
+
+- A high level API for the NVIC peripheral.
+
+- Context local data.
+
+- `borrow`/`borrow_mut` methods to `Mutex` that replace `lock`.
+
+- API and macros to send bytes / (formatted) strings through ITM
+
+### Changed
+
+- [breaking-change] `StackFrame` has been renamed to `StackedRegisters` and
+  moved into the `exceptions` module.
+
+- [breaking-change] Core peripherals can now be modified via a `&-` reference
+  and are no longer `Sync`.
+
+- [breaking-change] `interrupt::free`'s closure now includes a critical section
+  token, `CriticalSection`.
+
+- [breaking-change] the core register API has been revamped for type safety.
+
+- The safety of assembly wrappers like `wfi` and `interrupt::free` has been
+  reviewed. In many cases, the functions are no longer unsafe.
+
+- [breaking-change] `bkpt!` has been turned into a function. It no longer
+  accepts an immediate value.
+
+### Removed
+
+- `vector_table` and its associated `struct`, `VectorTable`. It's not a good
+  idea to give people a simple way to call the exception handlers.
+
+- `Mutex`'s `lock` method as it's unsound. You could use it to get multiple
+  `&mut -` references to the wrapped data.
+
 ## [v0.1.6] - 2017-01-22
 
 ### Added
@@ -60,10 +104,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - Functions to get the vector table
 - Wrappers over miscellaneous instructions like `bkpt`
 
-[Unreleased]: https://github.com/japaric/rustc-cfg/compare/v0.1.6...HEAD
-[v0.1.6]: https://github.com/japaric/rustc-cfg/compare/v0.1.5...v0.1.6
-[v0.1.5]: https://github.com/japaric/rustc-cfg/compare/v0.1.4...v0.1.5
-[v0.1.4]: https://github.com/japaric/rustc-cfg/compare/v0.1.3...v0.1.4
-[v0.1.3]: https://github.com/japaric/rustc-cfg/compare/v0.1.2...v0.1.3
-[v0.1.2]: https://github.com/japaric/rustc-cfg/compare/v0.1.1...v0.1.2
-[v0.1.1]: https://github.com/japaric/rustc-cfg/compare/v0.1.0...v0.1.1
+[Unreleased]: https://github.com/japaric/cortex-m/compare/v0.1.6...HEAD
+[v0.1.6]: https://github.com/japaric/cortex-m/compare/v0.1.5...v0.1.6
+[v0.1.5]: https://github.com/japaric/cortex-m/compare/v0.1.4...v0.1.5
+[v0.1.4]: https://github.com/japaric/cortex-m/compare/v0.1.3...v0.1.4
+[v0.1.3]: https://github.com/japaric/cortex-m/compare/v0.1.2...v0.1.3
+[v0.1.2]: https://github.com/japaric/cortex-m/compare/v0.1.1...v0.1.2
+[v0.1.1]: https://github.com/japaric/cortex-m/compare/v0.1.0...v0.1.1
diff --git a/Cargo.toml b/Cargo.toml
index 25b8806d..bdd3a4c1 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -6,7 +6,10 @@ keywords = ["arm", "cortex-m", "register", "peripheral"]
 license = "MIT OR Apache-2.0"
 name = "cortex-m"
 repository = "https://github.com/japaric/cortex-m"
-version = "0.1.6"
+version = "0.2.0"
 
 [dependencies]
-volatile-register = "0.1.0"
+volatile-register = "0.2.0"
+
+[dependencies.cortex-m-semihosting]
+version = "0.1.3"
\ No newline at end of file
diff --git a/build.rs b/build.rs
new file mode 100644
index 00000000..f8ad1634
--- /dev/null
+++ b/build.rs
@@ -0,0 +1,9 @@
+use std::env;
+
+fn main() {
+    let target = env::var("TARGET").unwrap();
+
+    if target.starts_with("thumbv6m-") {
+        println!("cargo:rustc-cfg=armv6m")
+    }
+}
diff --git a/ci/install.sh b/ci/install.sh
index dd8cd261..550a548e 100644
--- a/ci/install.sh
+++ b/ci/install.sh
@@ -4,11 +4,16 @@ main() {
     curl https://sh.rustup.rs -sSf | \
         sh -s -- -y --default-toolchain $TRAVIS_RUST_VERSION
 
+    local tag=$(git ls-remote --tags --refs --exit-code https://github.com/japaric/cross \
+                       | cut -d/ -f3 \
+                       | grep -E '^v[0-9.]+$' \
+                       | sort --version-sort \
+                       | tail -n1)
     curl -LSfs http://japaric.github.io/trust/install.sh | \
         sh -s -- \
            --force \
            --git japaric/cross \
-           --tag v0.1.2 \
+           --tag $tag \
            --target x86_64-unknown-linux-musl \
            --to ~/.cargo/bin
 }
diff --git a/src/asm.rs b/src/asm.rs
index 2e3368ac..c82d45d3 100644
--- a/src/asm.rs
+++ b/src/asm.rs
@@ -1,58 +1,60 @@
 //! Miscellaneous assembly instructions
 
-/// Puts the processor in Debug state. Debuggers can pick this up as a "breakpoint".
+/// Puts the processor in Debug state. Debuggers can pick this up as a
+/// "breakpoint".
 ///
-/// Optionally, an "immediate" value (in the 0-255 range) can be passed to `bkpt!`. The debugger can
-/// then read this value using the Program Counter (PC).
-#[cfg(target_arch = "arm")]
-#[macro_export]
-macro_rules! bkpt {
-    () => {
-        asm!("bkpt" :::: "volatile");
-    };
-    ($imm:expr) => {
-        asm!(concat!("bkpt #", stringify!($imm)) :::: "volatile");
-    };
+/// NOTE calling `bkpt` when the processor is not connected to a debugger will
+/// cause an exception
+#[inline(always)]
+pub fn bkpt() {
+    #[cfg(target_arch = "arm")]
+    unsafe {
+        asm!("bkpt"
+             :
+             :
+             :
+             : "volatile");
+    }
 }
 
-/// Puts the processor in Debug state. Debuggers can pick this up as a "breakpoint".
-///
-/// Optionally, an "immediate" value (in the 0-255 range) can be passed to `bkpt!`. The debugger can
-/// then read this value using the Program Counter (PC).
-#[cfg(not(target_arch = "arm"))]
-#[macro_export]
-macro_rules! bkpt {
-    () => {
-        asm!("nop" :::: "volatile");
-    };
-    ($e:expr) => {
-        asm!("nop" :::: "volatile");
-    };
+/// A no-operation. Useful to prevent delay loops from being optimized away.
+pub fn nop() {
+    unsafe {
+        asm!("nop"
+             :
+             :
+             :
+             : "volatile");
+    }
 }
-
-/// Wait for event
-pub unsafe fn wfe() {
+/// Wait For Event
+pub fn wfe() {
     match () {
         #[cfg(target_arch = "arm")]
-        () => asm!("wfe" :::: "volatile"),
+        () => unsafe {
+            asm!("wfe"
+                 :
+                 :
+                 :
+                 : "volatile")
+        },
         #[cfg(not(target_arch = "arm"))]
         () => {}
     }
 }
 
-/// Wait for interupt
-pub unsafe fn wfi() {
+/// Wait For Interrupt
+pub fn wfi() {
     match () {
         #[cfg(target_arch = "arm")]
-        () => asm!("wfi" :::: "volatile"),
+        () => unsafe{
+            asm!("wfi"
+                 :
+                 :
+                 :
+                 : "volatile")
+        },
         #[cfg(not(target_arch = "arm"))]
         () => {}
     }
 }
-
-/// A no-operation. Useful to stop delay loops being elided.
-pub fn nop() {
-    unsafe {
-        asm!("nop" :::: "volatile");
-    }
-}
diff --git a/src/ctxt.rs b/src/ctxt.rs
new file mode 100644
index 00000000..727090b7
--- /dev/null
+++ b/src/ctxt.rs
@@ -0,0 +1,160 @@
+//! Interrupt / Exception context local data
+//!
+//! The main use case is safely adding state to exception / interrupt handlers.
+//!
+//! This is done in two stages, first you define a token that will appear in the
+//! interrupt handler signature; each handler will have its unique token. This
+//! token must be zero sized type because interrupt handlers' real signature is
+//! `fn()` and it must also implement the `Context` trait. You must also make
+//! sure that the token can't be constructed outside of the crate where it's
+//! defined.
+//!
+//! ```
+//! # use cortex_m::ctxt::Context;
+//! // This must be in a library crate
+//! /// Token unique to the TIM7 interrupt handler
+//! pub struct Tim7 { _0: () }
+//!
+//! unsafe impl Context for Tim7 {}
+//! ```
+//!
+//! Then in the application one can pin data to the interrupt handler using
+//! `Local`.
+//!
+//! ```
+//! # #![feature(const_fn)]
+//! # use std::cell::Cell;
+//! # use cortex_m::ctxt::{Context, Local};
+//! # struct Tim7;
+//! # unsafe impl Context for Tim7 {}
+//! // omitted: how to put this handler in the vector table
+//! extern "C" fn tim7(ctxt: Tim7) {
+//!     static STATE: Local<Cell<bool>, Tim7> = Local::new(Cell::new(false));
+//!
+//!     let state = STATE.borrow(&ctxt);
+//!
+//!     // toggle state
+//!     state.set(!state.get());
+//!
+//!     if state.get() {
+//!         // something
+//!     } else {
+//!         // something else
+//!     }
+//! }
+//! ```
+//!
+//! Note that due to the uniqueness of tokens, other handlers won't be able to
+//! access context local data. (Given that you got the signatures right)
+//!
+//! ```
+//! # #![feature(const_fn)]
+//! # use std::cell::Cell;
+//! # use cortex_m::ctxt::{Context, Local};
+//! # struct Tim3;
+//! # struct Tim4;
+//! static TIM3_DATA: Local<Cell<bool>, Tim3> = Local::new(Cell::new(false));
+//!
+//! extern "C" fn tim3(ctxt: Tim3) {
+//!     let data = TIM3_DATA.borrow(&ctxt);
+//! }
+//!
+//! extern "C" fn tim4(ctxt: Tim4) {
+//!     //let data = TIM3_DATA.borrow(&ctxt);
+//!     // ^ wouldn't work
+//! }
+//! # unsafe impl Context for Tim3 {}
+//! # fn main() {}
+//! ```
+//!
+//! To have the application use these tokenized function signatures, you can
+//! define, in a library, a `Handlers` struct that represents the vector table:
+//!
+//! ```
+//! # struct Tim1;
+//! # struct Tim2;
+//! # struct Tim3;
+//! # struct Tim4;
+//! # extern "C" fn default_handler<T>(_: T) {}
+//! #[repr(C)]
+//! pub struct Handlers {
+//!     tim1: extern "C" fn(Tim1),
+//!     tim2: extern "C" fn(Tim2),
+//!     tim3: extern "C" fn(Tim3),
+//!     tim4: extern "C" fn(Tim4),
+//!     /* .. */
+//! }
+//!
+//! pub const DEFAULT_HANDLERS: Handlers = Handlers {
+//!     tim1: default_handler,
+//!     tim2: default_handler,
+//!     tim3: default_handler,
+//!     tim4: default_handler,
+//!     /* .. */
+//! };
+//! ```
+//!
+//! Then have the user use that `struct` to register the interrupt handlers:
+//!
+//! ```
+//! # struct Tim3;
+//! # struct Handlers { tim3: extern "C" fn(Tim3), tim4: extern "C" fn(Tim3) }
+//! # const DEFAULT_HANDLERS: Handlers = Handlers { tim3: tim3, tim4: tim3 };
+//! extern "C" fn tim3(ctxt: Tim3) { /* .. */ }
+//!
+//! // override the TIM3 interrupt handler
+//! #[no_mangle]
+//! static _INTERRUPTS: Handlers = Handlers {
+//!     tim3: tim3, ..DEFAULT_HANDLERS
+//! };
+//! ```
+//!
+//! This pattern is implemented for exceptions in this crate. See
+//! `exception::Handlers` and `exception::DEFAULT_HANDLERS`.
+
+use core::marker::PhantomData;
+use core::cell::UnsafeCell;
+
+/// Data local to a context
+pub struct Local<T, Ctxt>
+where
+    Ctxt: Context,
+{
+    _ctxt: PhantomData<Ctxt>,
+    data: UnsafeCell<T>,
+}
+
+impl<T, Ctxt> Local<T, Ctxt>
+where
+    Ctxt: Context,
+{
+    /// Initializes context local data
+    pub const fn new(value: T) -> Self {
+        Local {
+            _ctxt: PhantomData,
+            data: UnsafeCell::new(value),
+        }
+    }
+
+    /// Acquires a reference to the context local data
+    pub fn borrow<'ctxt>(&'static self, _ctxt: &'ctxt Ctxt) -> &'ctxt T {
+        unsafe { &*self.data.get() }
+    }
+
+    /// Acquires a mutable reference to the context local data
+    pub fn borrow_mut<'ctxt>(
+        &'static self,
+        _ctxt: &'ctxt mut Ctxt,
+    ) -> &'ctxt mut T {
+        unsafe { &mut *self.data.get() }
+    }
+}
+
+unsafe impl<T, Ctxt> Sync for Local<T, Ctxt>
+where
+    Ctxt: Context,
+{
+}
+
+/// A token unique to a context
+pub unsafe trait Context {}
diff --git a/src/exception.rs b/src/exception.rs
index 70460808..f5c8c288 100644
--- a/src/exception.rs
+++ b/src/exception.rs
@@ -1,4 +1,9 @@
-/// Kind of exception
+//! Exceptions
+
+use ctxt::Context;
+use Reserved;
+
+/// Enumeration of all exceptions
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 pub enum Exception {
     /// i.e. currently not servicing an exception
@@ -29,7 +34,7 @@ pub enum Exception {
 impl Exception {
     /// Returns the kind of exception that's currently being serviced
     pub fn current() -> Exception {
-        match ::peripheral::scb().icsr.read() as u8 {
+        match unsafe { (*::peripheral::SCB.get()).icsr.read() } as u8 {
             0 => Exception::ThreadMode,
             2 => Exception::Nmi,
             3 => Exception::HardFault,
@@ -41,7 +46,163 @@ impl Exception {
             15 => Exception::Systick,
             n if n >= 16 => Exception::Interrupt(n - 16),
             _ => Exception::Reserved,
+        }
+    }
+}
 
+/// Exception handlers
+#[repr(C)]
+pub struct Handlers {
+    /// Non-maskable interrupt
+    pub nmi: extern "C" fn(Nmi),
+    /// All class of fault
+    pub hard_fault: extern "C" fn(HardFault),
+    /// Memory management
+    pub mem_manage: extern "C" fn(MemManage),
+    /// Pre-fetch fault, memory access fault
+    pub bus_fault: extern "C" fn(BusFault),
+    /// Undefined instruction or illegal state
+    pub usage_fault: extern "C" fn(UsageFault),
+    /// Reserved spots in the vector table
+    pub _reserved0: [Reserved; 4],
+    /// System service call via SWI instruction
+    pub svcall: extern "C" fn(Svcall),
+    /// Reserved spots in the vector table
+    pub _reserved1: [Reserved; 2],
+    /// Pendable request for system service
+    pub pendsv: extern "C" fn(Pendsv),
+    /// System tick timer
+    pub sys_tick: extern "C" fn(SysTick),
+}
+
+/// Non-maskable interrupt
+pub struct Nmi {
+    _0: (),
+}
+
+/// All class of fault
+pub struct HardFault {
+    _0: (),
+}
+
+/// Memory management
+pub struct MemManage {
+    _0: (),
+}
+
+/// Pre-fetch fault, memory access fault
+pub struct BusFault {
+    _0: (),
+}
+
+/// Undefined instruction or illegal state
+pub struct UsageFault {
+    _0: (),
+}
+
+/// System service call via SWI instruction
+pub struct Svcall {
+    _0: (),
+}
+
+/// Pendable request for system service
+pub struct Pendsv {
+    _0: (),
+}
+
+/// System tick timer
+pub struct SysTick {
+    _0: (),
+}
+
+unsafe impl Context for Nmi {}
+
+unsafe impl Context for HardFault {}
+
+unsafe impl Context for MemManage {}
+
+unsafe impl Context for BusFault {}
+
+unsafe impl Context for UsageFault {}
+
+unsafe impl Context for Svcall {}
+
+unsafe impl Context for Pendsv {}
+
+unsafe impl Context for SysTick {}
+
+/// Default exception handlers
+pub const DEFAULT_HANDLERS: Handlers = Handlers {
+    _reserved0: [Reserved::Vector; 4],
+    _reserved1: [Reserved::Vector; 2],
+    bus_fault: default_handler,
+    hard_fault: default_handler,
+    mem_manage: default_handler,
+    nmi: default_handler,
+    pendsv: default_handler,
+    svcall: default_handler,
+    sys_tick: default_handler,
+    usage_fault: default_handler,
+};
+
+/// The default exception handler
+///
+/// This handler triggers a breakpoint (`bkpt`) and gives you access, within a
+/// GDB session, to the stack frame (`_sf`) where the exception occurred.
+// This needs asm!, #[naked] and unreachable() to avoid modifying the stack
+// pointer (MSP), that way it points to the stacked registers
+#[naked]
+pub extern "C" fn default_handler<T>(_token: T)
+where
+    T: Context,
+{
+    // This is the actual exception handler. `_sf` is a pointer to the previous
+    // stack frame
+    #[cfg(target_arch = "arm")]
+    extern "C" fn handler(_sr: &StackedRegisters) -> ! {
+        ::asm::bkpt();
+
+        loop {}
+    }
+
+    match () {
+        #[cfg(target_arch = "arm")]
+        () => {
+            unsafe {
+                // "trampoline" to get to the real exception handler.
+                asm!("mrs r0, MSP
+                  ldr r1, [r0, #20]
+                  b $0"
+                     :
+                     : "i"(handler as extern "C" fn(&StackedRegisters) -> !)
+                     :
+                     : "volatile");
+
+                ::core::intrinsics::unreachable()
+            }
         }
+        #[cfg(not(target_arch = "arm"))]
+        () => {}
     }
 }
+
+/// Registers stacked during an exception
+#[repr(C)]
+pub struct StackedRegisters {
+    /// (General purpose) Register 0
+    pub r0: u32,
+    /// (General purpose) Register 1
+    pub r1: u32,
+    /// (General purpose) Register 2
+    pub r2: u32,
+    /// (General purpose) Register 3
+    pub r3: u32,
+    /// (General purpose) Register 12
+    pub r12: u32,
+    /// Linker Register
+    pub lr: u32,
+    /// Program Counter
+    pub pc: u32,
+    /// Program Status Register
+    pub xpsr: u32,
+}
diff --git a/src/interrupt.rs b/src/interrupt.rs
index 43050717..e11fdc3c 100644
--- a/src/interrupt.rs
+++ b/src/interrupt.rs
@@ -15,62 +15,86 @@ impl<T> Mutex<T> {
 }
 
 impl<T> Mutex<T> {
-    /// Gets access to the inner data
-    ///
-    /// NOTE this prevents interrupts handlers from running thus gaining
-    /// exclusive access to the processor
-    pub fn lock<F, R>(&self, f: F) -> R
-        where F: FnOnce(&mut T) -> R
-    {
-        unsafe { ::interrupt::free(|| f(&mut *self.inner.get())) }
+    /// Borrows the data for the duration of the critical section
+    pub fn borrow<'cs>(&self, _ctxt: &'cs CriticalSection) -> &'cs T {
+        unsafe { &*self.inner.get() }
     }
+
+    /// Mutably borrows the data for the duration of the critical section
+    pub fn borrow_mut<'cs>(
+        &self,
+        _ctxt: &'cs mut CriticalSection,
+    ) -> &'cs mut T {
+        unsafe { &mut *self.inner.get() }
+    }
+}
+
+/// Interrupt number
+pub unsafe trait Nr {
+    /// Returns the number associated with this interrupt
+    fn nr(&self) -> u8;
 }
 
-// FIXME `T` should have some bound: `Send` or `Sync`?
 unsafe impl<T> Sync for Mutex<T> {}
 
-/// Disable interrupts, globally
+/// Disables all interrupts
 #[inline(always)]
-pub unsafe fn disable() {
+pub fn disable() {
     match () {
         #[cfg(target_arch = "arm")]
-        () => {
-            asm!("cpsid i" :::: "volatile");
-        }
+        () => unsafe {
+            asm!("cpsid i"
+                 :
+                 :
+                 :
+                 : "volatile");
+        },
         #[cfg(not(target_arch = "arm"))]
         () => {}
     }
 }
 
-/// Enable interrupts, globally
+/// Enables all the interrupts
 #[inline(always)]
-pub unsafe fn enable() {
+pub fn enable() {
     match () {
         #[cfg(target_arch = "arm")]
-        () => {
-            asm!("cpsie i" :::: "volatile");
-        }
+        () => unsafe {
+            asm!("cpsie i"
+                 :
+                 :
+                 :
+                 : "volatile");
+        },
         #[cfg(not(target_arch = "arm"))]
         () => {}
     }
 }
 
+/// Critical section context
+///
+/// Indicates that you are executing code within a critical section
+pub struct CriticalSection {
+    _0: (),
+}
+
 /// Execute closure `f` in an interrupt-free context.
+///
 /// This as also known as a "critical section".
-pub unsafe fn free<F, R>(f: F) -> R
-    where F: FnOnce() -> R
+pub fn free<F, R>(f: F) -> R
+where
+    F: FnOnce(CriticalSection) -> R,
 {
     let primask = ::register::primask::read();
 
+    // disable interrupts
     disable();
 
-    let r = f();
+    let r = f(CriticalSection { _0: () });
 
-    // If the interrupts were enabled before our `disable` call, then re-enable
+    // If the interrupts were active before our `disable` call, then re-enable
     // them. Otherwise, keep them disabled
-    // PRIMASK & 1 = 1 indicates that the interrupts were disabled
-    // PRIMASK & 1 = 0 indicates that they were enabled
-    if primask & 1 == 0 {
+    if primask.is_active() {
         enable();
     }
 
diff --git a/src/itm.rs b/src/itm.rs
new file mode 100644
index 00000000..4c49d149
--- /dev/null
+++ b/src/itm.rs
@@ -0,0 +1,77 @@
+//! Instrumentation Trace Macrocell
+
+use core::{fmt, ptr, slice};
+use peripheral::Stim;
+
+fn round_up_to_multiple_of(x: usize, k: usize) -> usize {
+    let rem = x % k;
+
+    if rem == 0 { x } else { x + k - rem }
+}
+
+fn round_down_to_multiple_of(x: usize, k: usize) -> usize {
+    x - (x % k)
+}
+
+unsafe fn split(buffer: &[u8]) -> (&[u8], &[u32], &[u8]) {
+    let start = buffer.as_ptr();
+    let end = start.offset(buffer.len() as isize);
+    let sbody = round_up_to_multiple_of(start as usize, 4);
+    let ebody = round_down_to_multiple_of(end as usize, 4);
+
+    let head = slice::from_raw_parts(start, sbody - start as usize);
+    let body = slice::from_raw_parts(sbody as *const _, (ebody - sbody) >> 2);
+    let tail = slice::from_raw_parts(ebody as *const _, end as usize - ebody);
+
+    (head, body, tail)
+}
+
+fn write_bytes(stim: &Stim, bytes: &[u8]) {
+    for byte in bytes {
+        while !stim.is_fifo_ready() {}
+        stim.write_u8(*byte);
+    }
+}
+
+// NOTE assumes that `bytes` is 32-bit aligned
+unsafe fn write_words(stim: &Stim, bytes: &[u32]) {
+    let mut p = bytes.as_ptr();
+    for _ in 0..bytes.len() {
+        while !stim.is_fifo_ready() {}
+        stim.write_u32(ptr::read(p));
+        p = p.offset(1);
+    }
+}
+
+struct Port<'p>(&'p Stim);
+
+impl<'p> fmt::Write for Port<'p> {
+    fn write_str(&mut self, s: &str) -> fmt::Result {
+        write_all(self.0, s.as_bytes());
+        Ok(())
+    }
+}
+
+/// Writes a `buffer` to the ITM `port`
+pub fn write_all(port: &Stim, buffer: &[u8]) {
+    if buffer.len() < 7 {
+        write_bytes(port, buffer);
+    } else {
+        let (head, body, tail) = unsafe { split(buffer) };
+        write_bytes(port, head);
+        unsafe { write_words(port, body) }
+        write_bytes(port, tail);
+    }
+}
+
+/// Writes `fmt::Arguments` to the ITM `port`
+pub fn write_fmt(port: &Stim, args: fmt::Arguments) {
+    use core::fmt::Write;
+
+    Port(port).write_fmt(args).ok();
+}
+
+/// Writes a string to the ITM `port`
+pub fn write_str(port: &Stim, string: &str) {
+    write_all(port, string.as_bytes())
+}
diff --git a/src/lib.rs b/src/lib.rs
index 09eeceba..95a1b691 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,104 +1,40 @@
 //! Low level access to Cortex-M processors
 //!
-//! This crate provides access to:
+//! This crate provides:
 //!
-//! - Core peripherals like NVIC, SCB and SysTick.
-//! - Core registers like CONTROL, MSP and PSR.
+//! - Access to core peripherals like NVIC, SCB and SysTick.
+//! - Access to core registers like CONTROL, MSP and PSR.
 //! - Interrupt manipulation mechanisms
 //! - Data structures like the vector table
-//! - Miscellaneous assembly instructions like `bkpt`
-//!
+//! - Safe wrappers around assembly instructions like `bkpt`
 
+#![cfg_attr(target_arch = "arm", feature(core_intrinsics))]
 #![deny(missing_docs)]
 #![deny(warnings)]
 #![feature(asm)]
 #![feature(const_fn)]
+#![feature(naked_functions)]
 #![no_std]
 
+pub extern crate cortex_m_semihosting as semihosting;
 extern crate volatile_register;
 
+#[macro_use]
+mod macros;
+
+#[macro_use]
 pub mod asm;
+pub mod ctxt;
+pub mod exception;
 pub mod interrupt;
+pub mod itm;
 pub mod peripheral;
 pub mod register;
 
-mod exception;
-
-pub use exception::Exception;
-
-/// Stack frame
-#[repr(C)]
-pub struct StackFrame {
-    /// (General purpose) Register 0
-    pub r0: u32,
-    /// (General purpose) Register 1
-    pub r1: u32,
-    /// (General purpose) Register 2
-    pub r2: u32,
-    /// (General purpose) Register 3
-    pub r3: u32,
-    /// (General purpose) Register 12
-    pub r12: u32,
-    /// Linker Register
-    pub lr: u32,
-    /// Program Counter
-    pub pc: u32,
-    /// Program Status Register
-    pub xpsr: u32,
-}
-
-/// Vector Table
-///
-/// # References
-///
-/// - ARMv7-M Architecture Reference Manual (issue E.b) - Section B1.5 - ARMv7-M exception model
-#[repr(C)]
-pub struct VectorTable {
-    /// Reset value of the Main Stack Pointer (MSP)
-    pub sp_main: &'static (),
-    /// Reset
-    pub reset: extern "C" fn() -> !,
-    /// Non Maskable Interrupt
-    pub nmi: Option<Handler>,
-    /// Hard Fault
-    pub hard_fault: Option<Handler>,
-    /// Memory Management
-    pub mem_manage: Option<Handler>,
-    /// Bus Fault
-    pub bus_fault: Option<Handler>,
-    /// Usage Fault
-    pub usage_fault: Option<Handler>,
-    reserved0: [u32; 4],
-    /// Supervisor Call
-    pub svcall: Option<Handler>,
-    /// Debug Monitor
-    pub debug_monitor: Option<Handler>,
-    reserved1: u32,
-    /// PendSV
-    pub pendsv: Option<Handler>,
-    /// SysTick
-    pub sys_tick: Option<Handler>,
-    /// Interrupts. An IMPLEMENTATION DEFINED number of them.
-    pub interrupts: [Option<Handler>; 0],
-}
-
-/// Returns the vector table
-pub fn vector_table() -> &'static VectorTable {
-    unsafe { deref(peripheral::scb().vtor.read() as usize) }
-}
-
-/// Exception/Interrupt Handler
-pub type Handler = unsafe extern "C" fn();
-
-#[cfg(test)]
-fn address<T>(r: &T) -> usize {
-    r as *const T as usize
-}
-
-unsafe fn deref<T>(a: usize) -> &'static T {
-    &*(a as *const T)
-}
-
-unsafe fn deref_mut<T>(a: usize) -> &'static mut T {
-    &mut *(a as *mut T)
+/// A reserved spot in the vector table
+#[derive(Clone, Copy)]
+#[repr(u32)]
+pub enum Reserved {
+    /// Reserved
+    Vector = 0,
 }
diff --git a/src/macros.rs b/src/macros.rs
new file mode 100644
index 00000000..1ee2cf2b
--- /dev/null
+++ b/src/macros.rs
@@ -0,0 +1,72 @@
+/// Macro for printing to the **host's** standard stderr
+#[macro_export]
+macro_rules! ehprint {
+    ($s:expr) => {
+        $crate::semihosting:::io:ewrite_str($s);
+    };
+    ($($arg:tt)*) => {
+        $crate::semihosting::io::ewrite_fmt(format_args!($($arg)*));
+    };
+}
+
+/// Macro for printing to the **host's** standard error, with a newline.
+#[macro_export]
+macro_rules! ehprintln {
+    () => (ehprint!("\n"));
+    ($fmt:expr) => {
+        ehprint!(concat!($fmt, "\n"));
+    };
+    ($fmt:expr, $($arg:tt)*) => {
+        ehprint!(concat!($fmt, "\n"), $($arg)*);
+    };
+}
+
+/// Macro for printing to the **host's** standard output
+#[macro_export]
+macro_rules! hprint {
+    ($s:expr) => {
+        $crate::semihosting::io::write_str($s);
+    };
+    ($($arg:tt)*) => {
+        $crate::semihosting::io::write_fmt(format_args!($($arg)*));
+    };
+}
+
+/// Macro for printing to the **host's** standard output, with a newline.
+#[macro_export]
+macro_rules! hprintln {
+    () => {
+        hprint!("\n");
+    };
+    ($fmt:expr) => {
+        hprint!(concat!($fmt, "\n"));
+    };
+    ($fmt:expr, $($arg:tt)*) => {
+        hprint!(concat!($fmt, "\n"), $($arg)*);
+    };
+}
+
+/// Macro for sending a formatted string through an ITM channel
+#[macro_export]
+macro_rules! iprint {
+    ($channel:expr, $s:expr) => {
+        $crate::itm::write_str($channel, $s);
+    };
+    ($channel:expr, $($arg:tt)*) => {
+        $crate::itm::write_fmt($channel, format_args!($($arg)*));
+    };
+}
+
+/// Macro for sending a formatted string through an ITM channel, with a newline.
+#[macro_export]
+macro_rules! iprintln {
+    ($channel:expr) => {
+        iprint!($channel, "\n");
+    };
+    ($channel:expr, $fmt:expr) => {
+        iprint!($channel, concat!($fmt, "\n"));
+    };
+    ($channel:expr, $fmt:expr, $($arg:tt)*) => {
+        iprint!($channel, concat!($fmt, "\n"), $($arg)*);
+    };
+}
diff --git a/src/peripheral/cpuid.rs b/src/peripheral/cpuid.rs
deleted file mode 100644
index 0dc140f7..00000000
--- a/src/peripheral/cpuid.rs
+++ /dev/null
@@ -1,30 +0,0 @@
-//! CPUID
-
-use volatile_register::RO;
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// CPUID base
-    pub base: RO<u32>,
-    reserved0: [u32; 15],
-    /// Processor Feature
-    pub pfr: [RO<u32>; 2],
-    /// Debug Feature
-    pub dfr: RO<u32>,
-    /// Auxiliary Feature
-    pub afr: RO<u32>,
-    /// Memory Model Feature
-    pub mmfr: [RO<u32>; 4],
-    /// Instruction Set Attribute
-    pub isar: [RO<u32>; 5],
-    reserved1: u32,
-    /// Cache Level ID
-    pub clidr: RO<u32>,
-    /// Cache Type
-    pub ctr: RO<u32>,
-    /// Cache Size ID
-    pub ccsidr: RO<u32>,
-    /// Cache Size Selection
-    pub csselr: RO<u32>,
-}
diff --git a/src/peripheral/dcb.rs b/src/peripheral/dcb.rs
deleted file mode 100644
index 93a056b0..00000000
--- a/src/peripheral/dcb.rs
+++ /dev/null
@@ -1,16 +0,0 @@
-//! Debug Control Block
-
-use volatile_register::{RW, WO};
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// Debug Halting Control and Status
-    pub dhcsr: RW<u32>,
-    /// Debug Core Register Selector
-    pub dcrsr: WO<u32>,
-    /// Debug Core Register Data
-    pub dcrdr: RW<u32>,
-    /// Debug Exception and Monitor Control
-    pub demcr: RW<u32>,
-}
diff --git a/src/peripheral/dwt.rs b/src/peripheral/dwt.rs
deleted file mode 100644
index ecd214ea..00000000
--- a/src/peripheral/dwt.rs
+++ /dev/null
@@ -1,43 +0,0 @@
-//! Data Watchpoint and Trace unit
-
-use volatile_register::{RO, RW, WO};
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// Control
-    pub ctrl: RW<u32>,
-    /// Cycle Count
-    pub cyccnt: RW<u32>,
-    /// CPI Count
-    pub cpicnt: RW<u32>,
-    /// Exception Overhead Count
-    pub exccnt: RW<u32>,
-    /// Sleep Count
-    pub sleepcnt: RW<u32>,
-    /// LSU Count
-    pub lsucnt: RW<u32>,
-    /// Folded-instruction Count
-    pub foldcnt: RW<u32>,
-    /// Program Counter Sample
-    pub pcsr: RO<u32>,
-    /// Comparators
-    pub c: [Comparator; 16],
-    reserved: [u32; 932],
-    /// Lock Access
-    pub lar: WO<u32>,
-    /// Lock Status
-    pub lsr: RO<u32>,
-}
-
-/// Comparator
-#[repr(C)]
-pub struct Comparator {
-    /// Comparator
-    pub comp: RW<u32>,
-    /// Comparator Mask
-    pub mask: RW<u32>,
-    /// Comparator Function
-    pub function: RW<u32>,
-    reserved: u32,
-}
diff --git a/src/peripheral/fpb.rs b/src/peripheral/fpb.rs
deleted file mode 100644
index 6aa6fbb7..00000000
--- a/src/peripheral/fpb.rs
+++ /dev/null
@@ -1,19 +0,0 @@
-//! Flash Patch and Breakpoint unit
-
-use volatile_register::{RO, RW, WO};
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// Control
-    pub ctrl: RW<u32>,
-    /// Remap
-    pub remap: RW<u32>,
-    /// Comparator
-    pub comp: [RW<u32>; 127],
-    reserved: [u32; 875],
-    /// Lock Access
-    pub lar: WO<u32>,
-    /// Lock Status
-    pub lsr: RO<u32>,
-}
diff --git a/src/peripheral/fpu.rs b/src/peripheral/fpu.rs
deleted file mode 100644
index 5bbf3521..00000000
--- a/src/peripheral/fpu.rs
+++ /dev/null
@@ -1,17 +0,0 @@
-//! Floating Point Unit
-
-use volatile_register::{RO, RW};
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    reserved: u32,
-    /// Floating Point Context Control
-    pub fpccr: RW<u32>,
-    /// Floating Point Context Address
-    pub fpcar: RW<u32>,
-    /// Floating Point Default Status Control
-    pub fpdscr: RW<u32>,
-    /// Media and FP Feature
-    pub mvfr: [RO<u32>; 3],
-}
diff --git a/src/peripheral/itm.rs b/src/peripheral/itm.rs
deleted file mode 100644
index b56d1b5f..00000000
--- a/src/peripheral/itm.rs
+++ /dev/null
@@ -1,54 +0,0 @@
-//! Instrumentation Trace Macrocell
-
-use volatile_register::{RO, RW, WO};
-
-use core::cell::UnsafeCell;
-use core::ptr;
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// Stimulus Port
-    pub stim: [Stim; 256],
-    reserved0: [u32; 640],
-    /// Trace Enable
-    pub ter: [RW<u32>; 8],
-    reserved1: [u32; 8],
-    /// Trace Privilege
-    pub tpr: RW<u32>,
-    reserved2: [u32; 15],
-    /// Trace Control
-    pub tcr: RW<u32>,
-    reserved3: [u32; 75],
-    /// Lock Access
-    pub lar: WO<u32>,
-    /// Lock Status
-    pub lsr: RO<u32>,
-}
-
-/// Stimulus Port
-pub struct Stim {
-    register: UnsafeCell<u32>,
-}
-
-impl Stim {
-    /// Writes an `u8` payload into the stimulus port
-    pub fn write_u8(&self, value: u8) {
-        unsafe { ptr::write_volatile(self.register.get() as *mut u8, value) }
-    }
-
-    /// Writes an `u16` payload into the stimulus port
-    pub fn write_u16(&self, value: u16) {
-        unsafe { ptr::write_volatile(self.register.get() as *mut u16, value) }
-    }
-
-    /// Writes an `u32` payload into the stimulus port
-    pub fn write_u32(&self, value: u32) {
-        unsafe { ptr::write_volatile(self.register.get(), value) }
-    }
-
-    /// Returns `true` if the stimulus port is ready to accept more data
-    pub fn is_fifo_ready(&self) -> bool {
-        unsafe { ptr::read_volatile(self.register.get()) == 1 }
-    }
-}
diff --git a/src/peripheral/mod.rs b/src/peripheral/mod.rs
index 9f02aa39..1f9e1476 100644
--- a/src/peripheral/mod.rs
+++ b/src/peripheral/mod.rs
@@ -1,149 +1,461 @@
 //! Core peripherals
 //!
-//! # Notes
-//!
-//! - Although the `*_mut()` functions always return a valid/live reference, the API doesn't prevent
-//!   the user from creating multiple mutable aliases. It's up to the user to ensure that no
-//!   unsynchonized concurrent access is performed through these references.
-//!
-//! # Caveats
-//!
-//! - The API doesn't check if the value passed to `write` is valid (e.g. reserved bits are not
-//!   modified) or not. It's up to the user to verify that.
-//!
 //! # References
 //!
 //! - ARMv7-M Architecture Reference Manual (Issue E.b) - Chapter B3
 
-pub mod cpuid;
-pub mod dcb;
-pub mod dwt;
-pub mod fpb;
-pub mod fpu;
-pub mod itm;
-pub mod mpu;
-pub mod nvic;
-pub mod scb;
-pub mod syst;
-pub mod tpiu;
+use core::cell::UnsafeCell;
+use core::marker::PhantomData;
+use core::ptr;
+
+use volatile_register::{RO, RW, WO};
 
+use interrupt::{CriticalSection, Nr};
+
+#[cfg(test)]
 mod test;
 
-const CPUID: usize = 0xE000_ED00;
-const DCB: usize = 0xE000_EDF0;
-const DWT: usize = 0xE000_1000;
-const FPB: usize = 0xE000_2000;
-const FPU: usize = 0xE000_EF30;
-const ITM: usize = 0xE000_0000;
-const MPU: usize = 0xE000_ED90;
-const NVIC: usize = 0xE000_E100;
-const SCB: usize = 0xE000_ED04;
-const SYST: usize = 0xE000_E010;
-const TPIU: usize = 0xE004_0000;
+/// CPUID
+pub const CPUID: Peripheral<Cpuid> = unsafe { Peripheral::new(0xE000_ED00) };
+
+/// Debug Control Block
+pub const DCB: Peripheral<Dcb> = unsafe { Peripheral::new(0xE000_EDF0) };
+
+/// Data Watchpoint and Trace unit
+pub const DWT: Peripheral<Dwt> = unsafe { Peripheral::new(0xE000_1000) };
+
+/// Flash Patch and Breakpoint unit
+pub const FPB: Peripheral<Fpb> = unsafe { Peripheral::new(0xE000_2000) };
+
+/// Floating Point Unit
+pub const FPU: Peripheral<Fpu> = unsafe { Peripheral::new(0xE000_EF30) };
+
+/// Instrumentation Trace Macrocell
+pub const ITM: Peripheral<Itm> = unsafe { Peripheral::new(0xE000_0000) };
+
+/// Memory Protection Unit
+pub const MPU: Peripheral<Mpu> = unsafe { Peripheral::new(0xE000_ED90) };
+
+/// Nested Vector Interrupt Controller
+pub const NVIC: Peripheral<Nvic> = unsafe { Peripheral::new(0xE000_E100) };
+
+/// System Control Block
+pub const SCB: Peripheral<Scb> = unsafe { Peripheral::new(0xE000_ED04) };
+
+/// SysTick: System Timer
+pub const SYST: Peripheral<Syst> = unsafe { Peripheral::new(0xE000_E010) };
+
+/// Trace Port Interface Unit;
+pub const TPIU: Peripheral<Tpiu> = unsafe { Peripheral::new(0xE004_0000) };
 
 // TODO stand-alone registers: ICTR, ACTLR and STIR
 
-/// `&cpuid::Registers`
-pub fn cpuid() -> &'static cpuid::Registers {
-    unsafe { ::deref(CPUID) }
+/// A peripheral
+pub struct Peripheral<T>
+where
+    T: 'static,
+{
+    address: usize,
+    _marker: PhantomData<&'static mut T>,
 }
 
-/// `&dcb::Registers`
-pub fn dcb() -> &'static dcb::Registers {
-    unsafe { ::deref(DCB) }
-}
+impl<T> Peripheral<T> {
+    /// Creates a new peripheral
+    ///
+    /// `address` is the base address of the register block
+    pub const unsafe fn new(address: usize) -> Self {
+        Peripheral {
+            address: address,
+            _marker: PhantomData,
+        }
+    }
 
-/// `&mut dcb::Registers`
-pub unsafe fn dcb_mut() -> &'static mut dcb::Registers {
-    ::deref_mut(DCB)
-}
+    /// Borrows the peripheral for the duration of a critical section
+    pub fn borrow<'cs>(&self, _ctxt: &'cs CriticalSection) -> &'cs T {
+        unsafe { &*self.get() }
+    }
 
-/// `&dwt::Registers`
-pub fn dwt() -> &'static dwt::Registers {
-    unsafe { ::deref(DWT) }
+    /// Returns a pointer to the register block
+    pub fn get(&self) -> *mut T {
+        self.address as *mut T
+    }
 }
 
-/// `&mut dwt::Registers`
-pub unsafe fn dwt_mut() -> &'static mut dwt::Registers {
-    ::deref_mut(DWT)
+/// CPUID register block
+#[repr(C)]
+pub struct Cpuid {
+    /// CPUID base
+    pub base: RO<u32>,
+    reserved0: [u32; 15],
+    /// Processor Feature
+    pub pfr: [RO<u32>; 2],
+    /// Debug Feature
+    pub dfr: RO<u32>,
+    /// Auxiliary Feature
+    pub afr: RO<u32>,
+    /// Memory Model Feature
+    pub mmfr: [RO<u32>; 4],
+    /// Instruction Set Attribute
+    pub isar: [RO<u32>; 5],
+    reserved1: u32,
+    /// Cache Level ID
+    pub clidr: RO<u32>,
+    /// Cache Type
+    pub ctr: RO<u32>,
+    /// Cache Size ID
+    pub ccsidr: RO<u32>,
+    /// Cache Size Selection
+    pub csselr: RO<u32>,
 }
 
-/// `&fpb::Registers`
-pub fn fpb() -> &'static fpb::Registers {
-    unsafe { ::deref(FPB) }
+/// DCB register block
+#[repr(C)]
+pub struct Dcb {
+    /// Debug Halting Control and Status
+    pub dhcsr: RW<u32>,
+    /// Debug Core Register Selector
+    pub dcrsr: WO<u32>,
+    /// Debug Core Register Data
+    pub dcrdr: RW<u32>,
+    /// Debug Exception and Monitor Control
+    pub demcr: RW<u32>,
 }
 
-/// `&mut fpb::Registers`
-pub unsafe fn fpb_mut() -> &'static mut fpb::Registers {
-    ::deref_mut(FPB)
+/// DWT register block
+#[repr(C)]
+pub struct Dwt {
+    /// Control
+    pub ctrl: RW<u32>,
+    /// Cycle Count
+    pub cyccnt: RW<u32>,
+    /// CPI Count
+    pub cpicnt: RW<u32>,
+    /// Exception Overhead Count
+    pub exccnt: RW<u32>,
+    /// Sleep Count
+    pub sleepcnt: RW<u32>,
+    /// LSU Count
+    pub lsucnt: RW<u32>,
+    /// Folded-instruction Count
+    pub foldcnt: RW<u32>,
+    /// Program Counter Sample
+    pub pcsr: RO<u32>,
+    /// Comparators
+    pub c: [Comparator; 16],
+    reserved: [u32; 932],
+    /// Lock Access
+    pub lar: WO<u32>,
+    /// Lock Status
+    pub lsr: RO<u32>,
 }
 
-/// `&fpu::Registers`
-pub fn fpu() -> &'static fpu::Registers {
-    unsafe { ::deref(FPU) }
+/// Comparator
+#[repr(C)]
+pub struct Comparator {
+    /// Comparator
+    pub comp: RW<u32>,
+    /// Comparator Mask
+    pub mask: RW<u32>,
+    /// Comparator Function
+    pub function: RW<u32>,
+    reserved: u32,
 }
 
-/// `&mut fpu::Registers`
-pub unsafe fn fpu_mut() -> &'static mut fpu::Registers {
-    ::deref_mut(FPU)
+/// FPB register block
+#[repr(C)]
+pub struct Fpb {
+    /// Control
+    pub ctrl: RW<u32>,
+    /// Remap
+    pub remap: RW<u32>,
+    /// Comparator
+    pub comp: [RW<u32>; 127],
+    reserved: [u32; 875],
+    /// Lock Access
+    pub lar: WO<u32>,
+    /// Lock Status
+    pub lsr: RO<u32>,
 }
 
-/// `&itm::Registers`
-pub fn itm() -> &'static itm::Registers {
-    unsafe { ::deref(ITM) }
+/// FPU register block
+#[repr(C)]
+pub struct Fpu {
+    reserved: u32,
+    /// Floating Point Context Control
+    pub fpccr: RW<u32>,
+    /// Floating Point Context Address
+    pub fpcar: RW<u32>,
+    /// Floating Point Default Status Control
+    pub fpdscr: RW<u32>,
+    /// Media and FP Feature
+    pub mvfr: [RO<u32>; 3],
 }
 
-/// `&mut itm::Registers`
-pub unsafe fn itm_mut() -> &'static mut itm::Registers {
-    ::deref_mut(ITM)
+/// ITM register block
+#[repr(C)]
+pub struct Itm {
+    /// Stimulus Port
+    pub stim: [Stim; 256],
+    reserved0: [u32; 640],
+    /// Trace Enable
+    pub ter: [RW<u32>; 8],
+    reserved1: [u32; 8],
+    /// Trace Privilege
+    pub tpr: RW<u32>,
+    reserved2: [u32; 15],
+    /// Trace Control
+    pub tcr: RW<u32>,
+    reserved3: [u32; 75],
+    /// Lock Access
+    pub lar: WO<u32>,
+    /// Lock Status
+    pub lsr: RO<u32>,
 }
 
-/// `&mpu::Registers`
-pub fn mpu() -> &'static mpu::Registers {
-    unsafe { ::deref(MPU) }
+/// Stimulus Port
+pub struct Stim {
+    register: UnsafeCell<u32>,
 }
 
-/// `&mut mpu::Registers`
-pub unsafe fn mpu_mut() -> &'static mut mpu::Registers {
-    ::deref_mut(MPU)
-}
+impl Stim {
+    /// Writes an `u8` payload into the stimulus port
+    pub fn write_u8(&self, value: u8) {
+        unsafe { ptr::write_volatile(self.register.get() as *mut u8, value) }
+    }
 
-/// `&nvic::Registers`
-pub fn nvic() -> &'static nvic::Registers {
-    unsafe { ::deref(NVIC) }
-}
+    /// Writes an `u16` payload into the stimulus port
+    pub fn write_u16(&self, value: u16) {
+        unsafe { ptr::write_volatile(self.register.get() as *mut u16, value) }
+    }
+
+    /// Writes an `u32` payload into the stimulus port
+    pub fn write_u32(&self, value: u32) {
+        unsafe { ptr::write_volatile(self.register.get(), value) }
+    }
 
-/// `&mut nvic::Registers`
-pub unsafe fn nvic_mut() -> &'static mut nvic::Registers {
-    ::deref_mut(NVIC)
+    /// Returns `true` if the stimulus port is ready to accept more data
+    pub fn is_fifo_ready(&self) -> bool {
+        unsafe { ptr::read_volatile(self.register.get()) == 1 }
+    }
 }
 
-/// `&scb::Registers`
-pub fn scb() -> &'static scb::Registers {
-    unsafe { ::deref(SCB) }
+/// MPU register block
+#[repr(C)]
+pub struct Mpu {
+    /// Type
+    pub _type: RO<u32>,
+    /// Control
+    pub ctrl: RW<u32>,
+    /// Region Number
+    pub rnr: RW<u32>,
+    /// Region Base Address
+    pub rbar: RW<u32>,
+    /// Region Attribute and Size
+    pub rasr: RW<u32>,
+    /// Alias 1 of RBAR
+    pub rbar_a1: RW<u32>,
+    /// Alias 1 of RSAR
+    pub rsar_a1: RW<u32>,
+    /// Alias 2 of RBAR
+    pub rbar_a2: RW<u32>,
+    /// Alias 2 of RSAR
+    pub rsar_a2: RW<u32>,
+    /// Alias 3 of RBAR
+    pub rbar_a3: RW<u32>,
+    /// Alias 3 of RSAR
+    pub rsar_a3: RW<u32>,
 }
 
-/// `&mut scb::Registers`
-pub unsafe fn scb_mut() -> &'static mut scb::Registers {
-    ::deref_mut(SCB)
+/// NVIC register block
+#[repr(C)]
+pub struct Nvic {
+    /// Interrupt Set-Enable
+    pub iser: [RW<u32>; 8],
+    reserved0: [u32; 24],
+    /// Interrupt Clear-Enable
+    pub icer: [RW<u32>; 8],
+    reserved1: [u32; 24],
+    /// Interrupt Set-Pending
+    pub ispr: [RW<u32>; 8],
+    reserved2: [u32; 24],
+    /// Interrupt Clear-Pending
+    pub icpr: [RW<u32>; 8],
+    reserved3: [u32; 24],
+    /// Interrupt Active Bit
+    pub iabr: [RO<u32>; 8],
+    reserved4: [u32; 56],
+    /// Interrupt Priority
+    pub ipr: [RW<u8>; 240],
 }
 
-/// `&syst::Registers`
-pub fn syst() -> &'static syst::Registers {
-    unsafe { ::deref(SYST) }
+impl Nvic {
+    /// Clears `interrupt`'s pending state
+    pub fn clear_pending<I>(&self, interrupt: I)
+    where
+        I: Nr,
+    {
+        let nr = interrupt.nr();
+
+        unsafe { self.icpr[usize::from(nr / 32)].write(1 << (nr % 32)) }
+    }
+
+    /// Disables `interrupt`
+    pub fn disable<I>(&self, interrupt: I)
+    where
+        I: Nr,
+    {
+        let nr = interrupt.nr();
+
+        unsafe { self.icer[usize::from(nr / 32)].write(1 << (nr % 32)) }
+    }
+
+    /// Enables `interrupt`
+    pub fn enable<I>(&self, interrupt: I)
+    where
+        I: Nr,
+    {
+        let nr = interrupt.nr();
+
+        unsafe { self.iser[usize::from(nr / 32)].write(1 << (nr % 32)) }
+    }
+
+    /// Gets the "priority" of `interrupt`
+    ///
+    /// NOTE NVIC encodes priority in the highest bits of a byte so values like
+    /// `1` and `2` have the same priority. Also for NVIC priorities, a lower
+    /// value (e.g. `16`) has higher priority than a larger value (e.g. `32`).
+    pub fn get_priority<I>(&self, interrupt: I) -> u8
+    where
+        I: Nr,
+    {
+        let nr = interrupt.nr();
+
+        self.ipr[usize::from(nr)].read()
+    }
+
+    /// Is `interrupt` active or pre-empted and stacked
+    pub fn is_active<I>(&self, interrupt: I) -> bool
+    where
+        I: Nr,
+    {
+        let nr = interrupt.nr();
+        let mask = 1 << (nr % 32);
+
+        (self.iabr[usize::from(nr / 32)].read() & mask) == mask
+    }
+
+    /// Checks if `interrupt` is enabled
+    pub fn is_enabled<I>(&self, interrupt: I) -> bool
+    where
+        I: Nr,
+    {
+        let nr = interrupt.nr();
+        let mask = 1 << (nr % 32);
+
+        (self.iser[usize::from(nr / 32)].read() & mask) == mask
+    }
+
+    /// Checks if `interrupt` is pending
+    pub fn is_pending<I>(&self, interrupt: I) -> bool
+    where
+        I: Nr,
+    {
+        let nr = interrupt.nr();
+        let mask = 1 << (nr % 32);
+
+        (self.ispr[usize::from(nr / 32)].read() & mask) == mask
+    }
+
+    /// Forces `interrupt` into pending state
+    pub fn set_pending<I>(&self, interrupt: I)
+    where
+        I: Nr,
+    {
+        let nr = interrupt.nr();
+
+        unsafe { self.ispr[usize::from(nr / 32)].write(1 << (nr % 32)) }
+    }
+
+    /// Sets the "priority" of `interrupt` to `prio`
+    ///
+    /// NOTE See `get_priority` method for an explanation of how NVIC priorities
+    /// work.
+    pub fn set_priority<I>(&self, interrupt: I, prio: u8)
+    where
+        I: Nr,
+    {
+        let nr = interrupt.nr();
+
+        unsafe { self.ipr[usize::from(nr)].write(prio) }
+    }
 }
 
-/// `&mut syst::Registers`
-pub unsafe fn syst_mut() -> &'static mut syst::Registers {
-    ::deref_mut(SYST)
+/// SCB register block
+#[repr(C)]
+pub struct Scb {
+    /// Interrupt Control and State
+    pub icsr: RW<u32>,
+    /// Vector Table Offset
+    pub vtor: RW<u32>,
+    /// Application Interrupt and Reset Control
+    pub aircr: RW<u32>,
+    /// System Control
+    pub scr: RW<u32>,
+    /// Configuration and Control
+    pub ccr: RW<u32>,
+    /// System Handler Priority
+    pub shpr: [RW<u8>; 12],
+    /// System Handler Control and State
+    pub shpcrs: RW<u32>,
+    /// Configurable Fault Status
+    pub cfsr: RW<u32>,
+    /// HardFault Status
+    pub hfsr: RW<u32>,
+    /// Debug Fault Status
+    pub dfsr: RW<u32>,
+    /// MemManage Fault Address
+    pub mmar: RW<u32>,
+    /// BusFault Address
+    pub bfar: RW<u32>,
+    /// Auxiliary Fault Status
+    pub afsr: RW<u32>,
+    reserved: [u32; 18],
+    /// Coprocessor Access Control
+    pub cpacr: RW<u32>,
 }
 
-/// `&tpiu::Registers`
-pub fn tpiu() -> &'static tpiu::Registers {
-    unsafe { ::deref(TPIU) }
+/// SysTick register block
+#[repr(C)]
+pub struct Syst {
+    /// Control and Status
+    pub csr: RW<u32>,
+    /// Reload Value
+    pub rvr: RW<u32>,
+    /// Current Value
+    pub cvr: RW<u32>,
+    /// Calibration Value
+    pub calib: RO<u32>,
 }
 
-/// `&mut tpiu::Registers`
-pub unsafe fn tpiu_mut() -> &'static mut tpiu::Registers {
-    ::deref_mut(TPIU)
+/// TPIU register block
+#[repr(C)]
+pub struct Tpiu {
+    /// Supported Parallel Port Sizes
+    pub sspsr: RO<u32>,
+    /// Current Parallel Port Size
+    pub cspsr: RW<u32>,
+    reserved0: [u32; 2],
+    /// Asynchronous Clock Prescaler
+    pub acpr: RW<u32>,
+    reserved1: [u32; 55],
+    /// Selected Pin Control
+    pub sppr: RW<u32>,
+    reserved2: [u32; 943],
+    /// Lock Access
+    pub lar: WO<u32>,
+    /// Lock Status
+    pub lsr: RO<u32>,
+    reserved3: [u32; 4],
+    /// TPIU Type
+    pub _type: RO<u32>,
 }
diff --git a/src/peripheral/mpu.rs b/src/peripheral/mpu.rs
deleted file mode 100644
index e024e625..00000000
--- a/src/peripheral/mpu.rs
+++ /dev/null
@@ -1,30 +0,0 @@
-//! Memory Protection Unit
-
-use volatile_register::{RO, RW};
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// Type
-    pub _type: RO<u32>,
-    /// Control
-    pub ctrl: RW<u32>,
-    /// Region Number
-    pub rnr: RW<u32>,
-    /// Region Base Address
-    pub rbar: RW<u32>,
-    /// Region Attribute and Size
-    pub rasr: RW<u32>,
-    /// Alias 1 of RBAR
-    pub rbar_a1: RW<u32>,
-    /// Alias 1 of RSAR
-    pub rsar_a1: RW<u32>,
-    /// Alias 2 of RBAR
-    pub rbar_a2: RW<u32>,
-    /// Alias 2 of RSAR
-    pub rsar_a2: RW<u32>,
-    /// Alias 3 of RBAR
-    pub rbar_a3: RW<u32>,
-    /// Alias 3 of RSAR
-    pub rsar_a3: RW<u32>,
-}
diff --git a/src/peripheral/nvic.rs b/src/peripheral/nvic.rs
deleted file mode 100644
index 4570c0f3..00000000
--- a/src/peripheral/nvic.rs
+++ /dev/null
@@ -1,25 +0,0 @@
-//! Nested Vector Interrupt Controller
-
-use volatile_register::{RO, RW};
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// Interrupt Set-Enable
-    pub iser: [RW<u32>; 16],
-    reserved0: [u32; 16],
-    /// Interrupt Clear-Enable
-    pub icer: [RW<u32>; 16],
-    reserved1: [u32; 16],
-    /// Interrupt Set-Pending
-    pub ispr: [RW<u32>; 16],
-    reserved2: [u32; 16],
-    /// Interrupt Clear-Pending
-    pub icpr: [RW<u32>; 16],
-    reserved3: [u32; 16],
-    /// Interrupt Active Bit
-    pub iabr: [RO<u32>; 16],
-    reserved4: [u32; 48],
-    /// Interrupt Priority
-    pub ipr: [RW<u32>; 124],
-}
diff --git a/src/peripheral/scb.rs b/src/peripheral/scb.rs
deleted file mode 100644
index d2b204ad..00000000
--- a/src/peripheral/scb.rs
+++ /dev/null
@@ -1,37 +0,0 @@
-//! System Control Block
-
-use volatile_register::RW;
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// Interrupt Control and State
-    pub icsr: RW<u32>,
-    /// Vector Table Offset
-    pub vtor: RW<u32>,
-    /// Application Interrupt and Reset Control
-    pub aircr: RW<u32>,
-    /// System Control
-    pub scr: RW<u32>,
-    /// Configuration and Control
-    pub ccr: RW<u32>,
-    /// System Handler Priority
-    pub shpr: [RW<u8>; 12],
-    /// System Handler Control and State
-    pub shpcrs: RW<u32>,
-    /// Configurable Fault Status
-    pub cfsr: RW<u32>,
-    /// HardFault Status
-    pub hfsr: RW<u32>,
-    /// Debug Fault Status
-    pub dfsr: RW<u32>,
-    /// MemManage Fault Address
-    pub mmar: RW<u32>,
-    /// BusFault Address
-    pub bfar: RW<u32>,
-    /// Auxiliary Fault Status
-    pub afsr: RW<u32>,
-    reserved: [u32; 18],
-    /// Coprocessor Access Control
-    pub cpacr: RW<u32>,
-}
diff --git a/src/peripheral/syst.rs b/src/peripheral/syst.rs
deleted file mode 100644
index 8ee70a14..00000000
--- a/src/peripheral/syst.rs
+++ /dev/null
@@ -1,16 +0,0 @@
-//! SysTick: System Timer
-
-use volatile_register::{RO, RW};
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// Control and Status
-    pub csr: RW<u32>,
-    /// Reload Value
-    pub rvr: RW<u32>,
-    /// Current Value
-    pub cvr: RW<u32>,
-    /// Calibration Value
-    pub calib: RO<u32>,
-}
diff --git a/src/peripheral/test.rs b/src/peripheral/test.rs
index c291b161..1770e03b 100644
--- a/src/peripheral/test.rs
+++ b/src/peripheral/test.rs
@@ -1,162 +1,167 @@
 #[test]
 fn cpuid() {
-    let cpuid = ::peripheral::cpuid();
-
-    assert_eq!(::address(&cpuid.base), 0xE000_ED00);
-    assert_eq!(::address(&cpuid.pfr), 0xE000_ED40);
-    assert_eq!(::address(&cpuid.dfr), 0xE000_ED48);
-    assert_eq!(::address(&cpuid.afr), 0xE000_ED4C);
-    assert_eq!(::address(&cpuid.mmfr), 0xE000_ED50);
-    assert_eq!(::address(&cpuid.isar), 0xE000_ED60);
-    assert_eq!(::address(&cpuid.clidr), 0xE000_ED78);
-    assert_eq!(::address(&cpuid.ctr), 0xE000_ED7C);
-    assert_eq!(::address(&cpuid.ccsidr), 0xE000_ED80);
-    assert_eq!(::address(&cpuid.csselr), 0xE000_ED84);
+    let cpuid = unsafe { &*::peripheral::CPUID.get() };
+
+    assert_eq!(address(&cpuid.base), 0xE000_ED00);
+    assert_eq!(address(&cpuid.pfr), 0xE000_ED40);
+    assert_eq!(address(&cpuid.dfr), 0xE000_ED48);
+    assert_eq!(address(&cpuid.afr), 0xE000_ED4C);
+    assert_eq!(address(&cpuid.mmfr), 0xE000_ED50);
+    assert_eq!(address(&cpuid.isar), 0xE000_ED60);
+    assert_eq!(address(&cpuid.clidr), 0xE000_ED78);
+    assert_eq!(address(&cpuid.ctr), 0xE000_ED7C);
+    assert_eq!(address(&cpuid.ccsidr), 0xE000_ED80);
+    assert_eq!(address(&cpuid.csselr), 0xE000_ED84);
 }
 
 #[test]
 fn dcb() {
-    for dcb in &[::peripheral::dcb(), unsafe { ::peripheral::dcb_mut() }] {
-        assert_eq!(::address(&dcb.dhcsr), 0xE000_EDF0);
-        assert_eq!(::address(&dcb.dcrsr), 0xE000_EDF4);
-        assert_eq!(::address(&dcb.dcrdr), 0xE000_EDF8);
-        assert_eq!(::address(&dcb.demcr), 0xE000_EDFC);
-    }
+    let dcb = unsafe { &*::peripheral::DCB.get() };
+
+    assert_eq!(address(&dcb.dhcsr), 0xE000_EDF0);
+    assert_eq!(address(&dcb.dcrsr), 0xE000_EDF4);
+    assert_eq!(address(&dcb.dcrdr), 0xE000_EDF8);
+    assert_eq!(address(&dcb.demcr), 0xE000_EDFC);
 }
 
 #[test]
 fn dwt() {
-    for dwt in &[::peripheral::dwt(), unsafe { ::peripheral::dwt_mut() }] {
-        assert_eq!(::address(&dwt.ctrl), 0xE000_1000);
-        assert_eq!(::address(&dwt.cyccnt), 0xE000_1004);
-        assert_eq!(::address(&dwt.cpicnt), 0xE000_1008);
-        assert_eq!(::address(&dwt.exccnt), 0xE000_100C);
-        assert_eq!(::address(&dwt.sleepcnt), 0xE000_1010);
-        assert_eq!(::address(&dwt.lsucnt), 0xE000_1014);
-        assert_eq!(::address(&dwt.foldcnt), 0xE000_1018);
-        assert_eq!(::address(&dwt.pcsr), 0xE000_101C);
-        assert_eq!(::address(&dwt.c[0].comp), 0xE000_1020);
-        assert_eq!(::address(&dwt.c[0].mask), 0xE000_1024);
-        assert_eq!(::address(&dwt.c[0].function), 0xE000_1028);
-        assert_eq!(::address(&dwt.c[1].comp), 0xE000_1030);
-        assert_eq!(::address(&dwt.c[1].mask), 0xE000_1034);
-        assert_eq!(::address(&dwt.c[1].function), 0xE000_1038);
-        assert_eq!(::address(&dwt.lar), 0xE000_1FB0);
-        assert_eq!(::address(&dwt.lsr), 0xE000_1FB4);
-    }
+    let dwt = unsafe { &*::peripheral::DWT.get() };
+
+    assert_eq!(address(&dwt.ctrl), 0xE000_1000);
+    assert_eq!(address(&dwt.cyccnt), 0xE000_1004);
+    assert_eq!(address(&dwt.cpicnt), 0xE000_1008);
+    assert_eq!(address(&dwt.exccnt), 0xE000_100C);
+    assert_eq!(address(&dwt.sleepcnt), 0xE000_1010);
+    assert_eq!(address(&dwt.lsucnt), 0xE000_1014);
+    assert_eq!(address(&dwt.foldcnt), 0xE000_1018);
+    assert_eq!(address(&dwt.pcsr), 0xE000_101C);
+    assert_eq!(address(&dwt.c[0].comp), 0xE000_1020);
+    assert_eq!(address(&dwt.c[0].mask), 0xE000_1024);
+    assert_eq!(address(&dwt.c[0].function), 0xE000_1028);
+    assert_eq!(address(&dwt.c[1].comp), 0xE000_1030);
+    assert_eq!(address(&dwt.c[1].mask), 0xE000_1034);
+    assert_eq!(address(&dwt.c[1].function), 0xE000_1038);
+    assert_eq!(address(&dwt.lar), 0xE000_1FB0);
+    assert_eq!(address(&dwt.lsr), 0xE000_1FB4);
 }
 
 #[test]
 fn fpb() {
-    for fpb in &[::peripheral::fpb(), unsafe { ::peripheral::fpb_mut() }] {
-        assert_eq!(::address(&fpb.ctrl), 0xE000_2000);
-        assert_eq!(::address(&fpb.remap), 0xE000_2004);
-        assert_eq!(::address(&fpb.comp), 0xE000_2008);
-        assert_eq!(::address(&fpb.comp[1]), 0xE000_200C);
-        assert_eq!(::address(&fpb.lar), 0xE000_2FB0);
-        assert_eq!(::address(&fpb.lsr), 0xE000_2FB4);
-    }
+    let fpb = unsafe { &*::peripheral::FPB.get() };
+
+    assert_eq!(address(&fpb.ctrl), 0xE000_2000);
+    assert_eq!(address(&fpb.remap), 0xE000_2004);
+    assert_eq!(address(&fpb.comp), 0xE000_2008);
+    assert_eq!(address(&fpb.comp[1]), 0xE000_200C);
+    assert_eq!(address(&fpb.lar), 0xE000_2FB0);
+    assert_eq!(address(&fpb.lsr), 0xE000_2FB4);
 }
 
 #[test]
 fn fpu() {
-    for fpu in &[::peripheral::fpu(), unsafe { ::peripheral::fpu_mut() }] {
-        assert_eq!(::address(&fpu.fpccr), 0xE000_EF34);
-        assert_eq!(::address(&fpu.fpcar), 0xE000_EF38);
-        assert_eq!(::address(&fpu.fpdscr), 0xE000_EF3C);
-        assert_eq!(::address(&fpu.mvfr), 0xE000_EF40);
-        assert_eq!(::address(&fpu.mvfr[1]), 0xE000_EF44);
-        assert_eq!(::address(&fpu.mvfr[2]), 0xE000_EF48);
-    }
+    let fpu = unsafe { &*::peripheral::FPU.get() };
+
+    assert_eq!(address(&fpu.fpccr), 0xE000_EF34);
+    assert_eq!(address(&fpu.fpcar), 0xE000_EF38);
+    assert_eq!(address(&fpu.fpdscr), 0xE000_EF3C);
+    assert_eq!(address(&fpu.mvfr), 0xE000_EF40);
+    assert_eq!(address(&fpu.mvfr[1]), 0xE000_EF44);
+    assert_eq!(address(&fpu.mvfr[2]), 0xE000_EF48);
 }
 
 #[test]
 fn itm() {
-    for itm in &[::peripheral::itm(), unsafe { ::peripheral::itm_mut() }] {
-        assert_eq!(::address(&itm.stim), 0xE000_0000);
-        assert_eq!(::address(&itm.ter), 0xE000_0E00);
-        assert_eq!(::address(&itm.tpr), 0xE000_0E40);
-        assert_eq!(::address(&itm.tcr), 0xE000_0E80);
-        assert_eq!(::address(&itm.lar), 0xE000_0FB0);
-        assert_eq!(::address(&itm.lsr), 0xE000_0FB4);
-    }
+    let itm = unsafe { &*::peripheral::ITM.get() };
+
+    assert_eq!(address(&itm.stim), 0xE000_0000);
+    assert_eq!(address(&itm.ter), 0xE000_0E00);
+    assert_eq!(address(&itm.tpr), 0xE000_0E40);
+    assert_eq!(address(&itm.tcr), 0xE000_0E80);
+    assert_eq!(address(&itm.lar), 0xE000_0FB0);
+    assert_eq!(address(&itm.lsr), 0xE000_0FB4);
 }
 
 #[test]
 fn mpu() {
-    for mpu in &[::peripheral::mpu(), unsafe { ::peripheral::mpu_mut() }] {
-        assert_eq!(::address(&mpu._type), 0xE000ED90);
-        assert_eq!(::address(&mpu.ctrl), 0xE000ED94);
-        assert_eq!(::address(&mpu.rnr), 0xE000ED98);
-        assert_eq!(::address(&mpu.rbar), 0xE000ED9C);
-        assert_eq!(::address(&mpu.rasr), 0xE000EDA0);
-        assert_eq!(::address(&mpu.rbar_a1), 0xE000EDA4);
-        assert_eq!(::address(&mpu.rsar_a1), 0xE000EDA8);
-        assert_eq!(::address(&mpu.rbar_a2), 0xE000EDAC);
-        assert_eq!(::address(&mpu.rsar_a2), 0xE000EDB0);
-        assert_eq!(::address(&mpu.rbar_a3), 0xE000EDB4);
-        assert_eq!(::address(&mpu.rsar_a3), 0xE000EDB8);
-    }
+    let mpu = unsafe { &*::peripheral::MPU.get() };
+
+    assert_eq!(address(&mpu._type), 0xE000ED90);
+    assert_eq!(address(&mpu.ctrl), 0xE000ED94);
+    assert_eq!(address(&mpu.rnr), 0xE000ED98);
+    assert_eq!(address(&mpu.rbar), 0xE000ED9C);
+    assert_eq!(address(&mpu.rasr), 0xE000EDA0);
+    assert_eq!(address(&mpu.rbar_a1), 0xE000EDA4);
+    assert_eq!(address(&mpu.rsar_a1), 0xE000EDA8);
+    assert_eq!(address(&mpu.rbar_a2), 0xE000EDAC);
+    assert_eq!(address(&mpu.rsar_a2), 0xE000EDB0);
+    assert_eq!(address(&mpu.rbar_a3), 0xE000EDB4);
+    assert_eq!(address(&mpu.rsar_a3), 0xE000EDB8);
 }
 
 #[test]
 fn nvic() {
-    for nvic in &[::peripheral::nvic(), unsafe { ::peripheral::nvic_mut() }] {
-        assert_eq!(::address(&nvic.iser), 0xE000E100);
-        assert_eq!(::address(&nvic.iser[15]), 0xE000E13C);
-        assert_eq!(::address(&nvic.icer), 0xE000E180);
-        assert_eq!(::address(&nvic.icer[7]), 0xE000E19C);
-        assert_eq!(::address(&nvic.icer[15]), 0xE000E1BC);
-        assert_eq!(::address(&nvic.ispr), 0xE000E200);
-        assert_eq!(::address(&nvic.ispr[15]), 0xE000E23C);
-        assert_eq!(::address(&nvic.icpr), 0xE000E280);
-        assert_eq!(::address(&nvic.icpr[15]), 0xE000E2BC);
-        assert_eq!(::address(&nvic.iabr), 0xE000E300);
-        assert_eq!(::address(&nvic.iabr[15]), 0xE000E33C);
-        assert_eq!(::address(&nvic.ipr), 0xE000E400);
-        assert_eq!(::address(&nvic.ipr[59]), 0xE000E4EC);
-    }
+    let nvic = unsafe { &*::peripheral::NVIC.get() };
+
+    assert_eq!(address(&nvic.iser), 0xE000E100);
+    assert_eq!(address(&nvic.iser[7]), 0xE000E11C);
+    assert_eq!(address(&nvic.icer), 0xE000E180);
+    assert_eq!(address(&nvic.icer[7]), 0xE000E19C);
+    assert_eq!(address(&nvic.ispr), 0xE000E200);
+    assert_eq!(address(&nvic.ispr[7]), 0xE000E21C);
+    assert_eq!(address(&nvic.icpr), 0xE000E280);
+    assert_eq!(address(&nvic.icpr[7]), 0xE000E29C);
+    assert_eq!(address(&nvic.iabr), 0xE000E300);
+    assert_eq!(address(&nvic.iabr[7]), 0xE000E31C);
+    assert_eq!(address(&nvic.ipr), 0xE000E400);
+    assert_eq!(address(&nvic.ipr[239]), 0xE000E4eF);
 }
 
 #[test]
 fn scb() {
-    for scb in &[::peripheral::scb(), unsafe { ::peripheral::scb_mut() }] {
-        assert_eq!(::address(&scb.icsr), 0xE000_ED04);
-        assert_eq!(::address(&scb.vtor), 0xE000_ED08);
-        assert_eq!(::address(&scb.aircr), 0xE000_ED0C);
-        assert_eq!(::address(&scb.scr), 0xE000_ED10);
-        assert_eq!(::address(&scb.ccr), 0xE000_ED14);
-        assert_eq!(::address(&scb.shpr), 0xE000_ED18);
-        assert_eq!(::address(&scb.shpcrs), 0xE000_ED24);
-        assert_eq!(::address(&scb.cfsr), 0xE000_ED28);
-        assert_eq!(::address(&scb.hfsr), 0xE000_ED2C);
-        assert_eq!(::address(&scb.dfsr), 0xE000_ED30);
-        assert_eq!(::address(&scb.mmar), 0xE000_ED34);
-        assert_eq!(::address(&scb.bfar), 0xE000_ED38);
-        assert_eq!(::address(&scb.afsr), 0xE000_ED3C);
-        assert_eq!(::address(&scb.cpacr), 0xE000_ED88);
-    }
+    let scb = unsafe { &*::peripheral::SCB.get() };
+
+    assert_eq!(address(&scb.icsr), 0xE000_ED04);
+    assert_eq!(address(&scb.vtor), 0xE000_ED08);
+    assert_eq!(address(&scb.aircr), 0xE000_ED0C);
+    assert_eq!(address(&scb.scr), 0xE000_ED10);
+    assert_eq!(address(&scb.ccr), 0xE000_ED14);
+    assert_eq!(address(&scb.shpr), 0xE000_ED18);
+    assert_eq!(address(&scb.shpcrs), 0xE000_ED24);
+    assert_eq!(address(&scb.cfsr), 0xE000_ED28);
+    assert_eq!(address(&scb.hfsr), 0xE000_ED2C);
+    assert_eq!(address(&scb.dfsr), 0xE000_ED30);
+    assert_eq!(address(&scb.mmar), 0xE000_ED34);
+    assert_eq!(address(&scb.bfar), 0xE000_ED38);
+    assert_eq!(address(&scb.afsr), 0xE000_ED3C);
+    assert_eq!(address(&scb.cpacr), 0xE000_ED88);
+
 }
 
 #[test]
 fn syst() {
-    for syst in &[::peripheral::syst(), unsafe { ::peripheral::syst_mut() }] {
-        assert_eq!(::address(&syst.csr), 0xE000_E010);
-        assert_eq!(::address(&syst.rvr), 0xE000_E014);
-        assert_eq!(::address(&syst.cvr), 0xE000_E018);
-        assert_eq!(::address(&syst.calib), 0xE000_E01C);
-    }
+    let syst = unsafe { &*::peripheral::SYST.get() };
+
+    assert_eq!(address(&syst.csr), 0xE000_E010);
+    assert_eq!(address(&syst.rvr), 0xE000_E014);
+    assert_eq!(address(&syst.cvr), 0xE000_E018);
+    assert_eq!(address(&syst.calib), 0xE000_E01C);
+
 }
 
 #[test]
 fn tpiu() {
-    for tpiu in &[::peripheral::tpiu(), unsafe { ::peripheral::tpiu_mut() }] {
-        assert_eq!(::address(&tpiu.sspsr), 0xE004_0000);
-        assert_eq!(::address(&tpiu.cspsr), 0xE004_0004);
-        assert_eq!(::address(&tpiu.acpr), 0xE004_0010);
-        assert_eq!(::address(&tpiu.sppr), 0xE004_00F0);
-        assert_eq!(::address(&tpiu.lar), 0xE004_0FB0);
-        assert_eq!(::address(&tpiu.lsr), 0xE004_0FB4);
-        assert_eq!(::address(&tpiu._type), 0xE004_0FC8);
-    }
+    let tpiu = unsafe { &*::peripheral::TPIU.get() };
+
+    assert_eq!(address(&tpiu.sspsr), 0xE004_0000);
+    assert_eq!(address(&tpiu.cspsr), 0xE004_0004);
+    assert_eq!(address(&tpiu.acpr), 0xE004_0010);
+    assert_eq!(address(&tpiu.sppr), 0xE004_00F0);
+    assert_eq!(address(&tpiu.lar), 0xE004_0FB0);
+    assert_eq!(address(&tpiu.lsr), 0xE004_0FB4);
+    assert_eq!(address(&tpiu._type), 0xE004_0FC8);
+}
+
+fn address<T>(r: *const T) -> usize {
+    r as usize
 }
diff --git a/src/peripheral/tpiu.rs b/src/peripheral/tpiu.rs
deleted file mode 100644
index 50473512..00000000
--- a/src/peripheral/tpiu.rs
+++ /dev/null
@@ -1,26 +0,0 @@
-//! Trace Port Interface Unit
-
-use volatile_register::{RO, RW, WO};
-
-/// Registers
-#[repr(C)]
-pub struct Registers {
-    /// Supported Parallel Port Sizes
-    pub sspsr: RO<u32>,
-    /// Current Parallel Port Size
-    pub cspsr: RW<u32>,
-    reserved0: [u32; 2],
-    /// Asynchronous Clock Prescaler
-    pub acpr: RW<u32>,
-    reserved1: [u32; 55],
-    /// Selected Pin Control
-    pub sppr: RW<u32>,
-    reserved2: [u32; 943],
-    /// Lock Access
-    pub lar: WO<u32>,
-    /// Lock Status
-    pub lsr: RO<u32>,
-    reserved3: [u32; 4],
-    /// TPIU Type
-    pub _type: RO<u32>,
-}
diff --git a/src/register.rs b/src/register.rs
deleted file mode 100644
index 014ba11c..00000000
--- a/src/register.rs
+++ /dev/null
@@ -1,124 +0,0 @@
-//! Processor core registers
-//!
-//! The following registers can only be accessed in PRIVILEGED mode:
-//!
-//! - MSP
-//! - IPSR
-//! - EPSR
-//! - PRIMASK
-//! - FAULTMASK
-//! - BASEPRI
-//! - CONTROL
-//!
-//! The rest of registers (see list below) can be accessed in either, PRIVILEGED or UNPRIVILEGED,
-//! mode.
-//!
-//! - PSP
-//! - LR
-//! - PC
-//! - APSR
-//!
-//! # Caveats
-//!
-//! - The API doesn't check if the value passed to `write` is valid (e.g. reserved bits are not
-//!   modified) or not. It's up to the user to verify that.
-//!
-//! # References
-//!
-//! - Cortex-M* Devices Generic User Guide - Section 2.1.3 Core registers
-
-// NOTE all the functions here are `always(inline)` to prevent a function call which may change the
-// contents of the core registers.
-
-macro_rules! sr {
-    ($name:ident) => {
-        /// Reads the special register
-        #[inline(always)]
-        pub unsafe fn read() -> u32 {
-            let r: u32;
-            match () {
-                #[cfg(target_arch = "arm")]
-                () => asm!(concat!("mrs ", "$0,", stringify!($name)) : "=r"(r) ::: "volatile"),
-
-                #[cfg(not(target_arch = "arm"))]
-                () => r = 0,
-            }
-            r
-        }
-    };
-}
-
-macro_rules! srw {
-    (#[$attr:meta] $name:ident) => {
-        #[$attr]
-        pub mod $name {
-            sr!($name);
-
-            /// Writes to the special register
-            #[inline(always)]
-            pub unsafe fn write(r: u32) {
-                match r {
-                    #[cfg(target_arch = "arm")]
-                    _ => asm!(concat!("msr ", stringify!($name), ",$0") :: "r"(r) ::: "volatile"),
-
-                    #[cfg(not(target_arch = "arm"))]
-                    _ => {},
-                }
-            }
-        }
-    };
-}
-
-macro_rules! sro {
-    (#[$attr:meta] $name:ident) => {
-        #[$attr]
-        pub mod $name {
-            sr!($name);
-        }
-    }
-}
-
-macro_rules! rw {
-    (#[$attr:meta] $name:ident : $r:ident) => {
-        #[$attr]
-        pub mod $name {
-            /// Reads the special register
-            #[inline(always)]
-            pub unsafe fn read() -> u32 {
-                let r: u32;
-                match () {
-                    #[cfg(target_arch = "arm")]
-                    () => asm!(concat!("mov ", "$0,", stringify!($r)) : "=r"(r) ::: "volatile"),
-
-                    #[cfg(not(target_arch = "arm"))]
-                    () => r = 0,
-                }
-                r
-            }
-
-            /// Writes to the special register
-            #[inline(always)]
-            pub unsafe fn write(r: u32) {
-                match r {
-                    #[cfg(target_arch = "arm")]
-                    _ => asm!(concat!("mov ", stringify!($r), ",$0") :: "r"(r) ::: "volatile"),
-
-                    #[cfg(not(target_arch = "arm"))]
-                    _ => {}
-                }
-            }
-        }
-    }
-}
-
-srw!(#[doc = "Main Stack Pointer"] msp);
-srw!(#[doc = "Process Stack Pointer"] psp);
-rw!(#[doc = "Link Register"] lr: r14);
-rw!(#[doc = "Program Counter"] pc: r15);
-srw!(#[doc = "Application Program Status Register"] apsr);
-sro!(#[doc = "Interrupt Program Status Register"] ipsr);
-sro!(#[doc = "Exception Program Status Register"] epsr);
-srw!(#[doc = "Priority Mask Register"] primask);
-srw!(#[doc = "Fault Mask Register"] faultmask);
-srw!(#[doc = "Base Priority Mask Register"] basepri);
-srw!(#[doc = "Control Register"] control);
diff --git a/src/register/apsr.rs b/src/register/apsr.rs
new file mode 100644
index 00000000..338c6847
--- /dev/null
+++ b/src/register/apsr.rs
@@ -0,0 +1,52 @@
+//! Application Program Status Register
+
+/// Application Program Status Register
+pub struct Apsr {
+    bits: u32,
+}
+
+impl Apsr {
+    /// Returns the contents of the register as raw bits
+    pub fn bits(&self) -> u32 {
+        self.bits
+    }
+
+    /// DSP overflow and saturation flag
+    pub fn q(&self) -> bool {
+        self.bits & (1 << 27) == (1 << 27)
+    }
+
+    /// Overflow flag
+    pub fn v(&self) -> bool {
+        self.bits & (1 << 28) == (1 << 28)
+    }
+
+    /// Carry or borrow flag
+    pub fn c(&self) -> bool {
+        self.bits & (1 << 29) == (1 << 29)
+    }
+
+    /// Zero flag
+    pub fn z(&self) -> bool {
+        self.bits & (1 << 30) == (1 << 30)
+    }
+
+    /// Negative flag
+    pub fn n(&self) -> bool {
+        self.bits & (1 << 31) == (1 << 31)
+    }
+}
+
+/// Reads the CPU register
+#[inline(always)]
+pub fn read() -> Apsr {
+    let r: u32;
+    unsafe {
+        asm!("mrs $0, APSR"
+             : "=r"(r)
+             :
+             :
+             : "volatile");
+    }
+    Apsr { bits: r }
+}
diff --git a/src/register/basepri.rs b/src/register/basepri.rs
new file mode 100644
index 00000000..e9164c1a
--- /dev/null
+++ b/src/register/basepri.rs
@@ -0,0 +1,27 @@
+//! Base Priority Mask Register
+
+/// Reads the CPU register
+#[inline(always)]
+pub fn read() -> u8 {
+    let r: u32;
+    unsafe {
+        asm!("mrs $0, BASEPRI"
+             : "=r"(r)
+             :
+             :
+             : "volatile");
+    }
+    r as u8
+}
+
+/// Writes to the CPU register
+#[inline(always)]
+pub fn write(basepri: u8) {
+    unsafe {
+        asm!("msr BASEPRI, $0"
+             :
+             : "r"(basepri)
+             :
+             : "volatile");
+    }
+}
diff --git a/src/register/basepri_max.rs b/src/register/basepri_max.rs
new file mode 100644
index 00000000..e7594e61
--- /dev/null
+++ b/src/register/basepri_max.rs
@@ -0,0 +1,16 @@
+//! Base Priority Mask Register (conditional write)
+
+/// Writes to BASEPRI *if*
+///
+/// - `basepri != 0` AND `basepri::read() == 0`, OR
+/// - `basepri != 0` AND `basepri < basepri::read()`
+#[inline(always)]
+pub fn write(basepri: u8) {
+    unsafe {
+        asm!("msr BASEPRI_MAX, $0"
+             :
+             : "r"(basepri)
+             :
+             : "volatile");
+    }
+}
diff --git a/src/register/control.rs b/src/register/control.rs
new file mode 100644
index 00000000..62ebff69
--- /dev/null
+++ b/src/register/control.rs
@@ -0,0 +1,117 @@
+//! Control register
+
+/// Control register
+pub struct Control {
+    bits: u32,
+}
+
+impl Control {
+    /// Returns the contents of the register as raw bits
+    pub fn bits(&self) -> u32 {
+        self.bits
+    }
+
+    /// Thread mode privilege level
+    pub fn npriv(&self) -> Npriv {
+        if self.bits & (1 << 0) == (1 << 0) {
+            Npriv::Unprivileged
+        } else {
+            Npriv::Privileged
+        }
+    }
+
+    /// Currently active stack pointer
+    pub fn spsel(&self) -> Spsel {
+        if self.bits & (1 << 1) == (1 << 1) {
+            Spsel::Psp
+        } else {
+            Spsel::Msp
+        }
+    }
+
+    /// Whether context floating-point is currently active
+    pub fn fpca(&self) -> Fpca {
+        if self.bits & (1 << 2) == (1 << 2) {
+            Fpca::Active
+        } else {
+            Fpca::NotActive
+        }
+    }
+}
+
+/// Thread mode privilege level
+#[derive(Clone, Copy, Eq, PartialEq)]
+pub enum Npriv {
+    /// Privileged
+    Privileged,
+    /// Unprivileged
+    Unprivileged,
+}
+
+impl Npriv {
+    /// Is in privileged thread mode?
+    pub fn is_privileged(&self) -> bool {
+        *self == Npriv::Privileged
+    }
+
+    /// Is in unprivileged thread mode?
+    pub fn is_unprivileged(&self) -> bool {
+        *self == Npriv::Unprivileged
+    }
+}
+
+/// Currently active stack pointer
+#[derive(Clone, Copy, Eq, PartialEq)]
+pub enum Spsel {
+    /// MSP is the current stack pointer
+    Msp,
+    /// PSP is the current stack pointer
+    Psp,
+}
+
+impl Spsel {
+    /// Is MSP the current stack pointer?
+    pub fn is_msp(&self) -> bool {
+        *self == Spsel::Msp
+    }
+
+    /// Is PSP the current stack pointer?
+    pub fn is_psp(&self) -> bool {
+        *self == Spsel::Psp
+    }
+}
+
+/// Whether context floating-point is currently active
+#[derive(Clone, Copy, Eq, PartialEq)]
+pub enum Fpca {
+    /// Floating-point context active.
+    Active,
+    /// No floating-point context active
+    NotActive,
+}
+
+impl Fpca {
+    /// Is a floating-point context active?
+    pub fn is_active(&self) -> bool {
+        *self == Fpca::Active
+    }
+
+    /// Is a floating-point context not active?
+    pub fn is_not_active(&self) -> bool {
+        *self == Fpca::NotActive
+    }
+}
+
+/// Reads the CPU register
+#[inline(always)]
+pub fn read() -> Control {
+    let r: u32;
+    unsafe {
+        asm!("mrs $0, CONTROL"
+             : "=r"(r)
+             :
+             :
+             : "volatile");
+    }
+    Control { bits: r }
+}
diff --git a/src/register/faultmask.rs b/src/register/faultmask.rs
new file mode 100644
index 00000000..5a06b37e
--- /dev/null
+++ b/src/register/faultmask.rs
@@ -0,0 +1,40 @@
+//! Fault Mask Register
+
+/// All exceptions are ...
+#[derive(Clone, Copy, Eq, PartialEq)]
+pub enum Faultmask {
+    /// Active
+    Active,
+    /// Inactive, expect for NMI
+    Inactive,
+}
+
+impl Faultmask {
+    /// All exceptions are active
+    pub fn is_active(&self) -> bool {
+        *self == Faultmask::Active
+    }
+
+    /// All exceptions, except for NMI, are inactive
+    pub fn is_inactive(&self) -> bool {
+        *self == Faultmask::Inactive
+    }
+}
+
+/// Reads the CPU register
+#[inline(always)]
+pub fn read() -> Faultmask {
+    let r: u32;
+    unsafe {
+        asm!("mrs $0, FAULTMASK"
+             : "=r"(r)
+             :
+             :
+             : "volatile");
+    }
+    if r & (1 << 0) == (1 << 0) {
+        Faultmask::Inactive
+    } else {
+        Faultmask::Active
+    }
+}
diff --git a/src/register/lr.rs b/src/register/lr.rs
new file mode 100644
index 00000000..fecfecbe
--- /dev/null
+++ b/src/register/lr.rs
@@ -0,0 +1,25 @@
+//! Link register
+
+/// Reads the CPU register
+#[inline(always)]
+pub fn read() -> u32 {
+    let r: u32;
+    unsafe {
+        asm!("mov $0,R14"
+             : "=r"(r)
+             :
+             :
+             : "volatile");
+    }
+    r
+}
+
+/// Writes `bits` to the CPU register
+#[inline(always)]
+pub unsafe fn write(bits: u32) {
+    asm!("mov R14,$0"
+         :
+         : "r"(bits)
+         :
+         : "volatile");
+}
diff --git a/src/register/mod.rs b/src/register/mod.rs
new file mode 100644
index 00000000..17f6fdad
--- /dev/null
+++ b/src/register/mod.rs
@@ -0,0 +1,41 @@
+//! Processor core registers
+//!
+//! The following registers can only be accessed in PRIVILEGED mode:
+//!
+//! - BASEPRI
+//! - CONTROL
+//! - FAULTMASK
+//! - MSP
+//! - PRIMASK
+//!
+//! The rest of registers (see list below) can be accessed in either, PRIVILEGED
+//! or UNPRIVILEGED, mode.
+//!
+//! - APSR
+//! - LR
+//! - PC
+//! - PSP
+//!
+//! The following registers are NOT available on ARMv6-M devices
+//! (`thumbv6m-none-eabi`):
+//!
+//! - BASEPRI
+//! - FAULTMASK
+//!
+//! # References
+//!
+//! - Cortex-M* Devices Generic User Guide - Section 2.1.3 Core registers
+
+pub mod apsr;
+#[cfg(not(armv6m))]
+pub mod basepri;
+#[cfg(not(armv6m))]
+pub mod basepri_max;
+pub mod control;
+#[cfg(not(armv6m))]
+pub mod faultmask;
+pub mod lr;
+pub mod msp;
+pub mod pc;
+pub mod primask;
+pub mod psp;
diff --git a/src/register/msp.rs b/src/register/msp.rs
new file mode 100644
index 00000000..ebea6ed5
--- /dev/null
+++ b/src/register/msp.rs
@@ -0,0 +1,25 @@
+//! Main Stack Pointer
+
+/// Reads the CPU register
+#[inline(always)]
+pub fn read() -> u32 {
+    let r;
+    unsafe {
+        asm!("mrs $0,MSP"
+             : "=r"(r)
+             :
+             :
+             : "volatile");
+    }
+    r
+}
+
+/// Writes `bits` to the CPU register
+#[inline(always)]
+pub unsafe fn write(bits: u32) {
+    asm!("msr MSP,$0"
+         :
+         : "r"(bits)
+         :
+         : "volatile");
+}
diff --git a/src/register/pc.rs b/src/register/pc.rs
new file mode 100644
index 00000000..3fec1aeb
--- /dev/null
+++ b/src/register/pc.rs
@@ -0,0 +1,25 @@
+//! Program counter
+
+/// Reads the CPU register
+#[inline(always)]
+pub fn read() -> u32 {
+    let r;
+    unsafe {
+        asm!("mov $0,R15"
+             : "=r"(r)
+             :
+             :
+             : "volatile");
+    }
+    r
+}
+
+/// Writes `bits` to the CPU register
+#[inline(always)]
+pub unsafe fn write(bits: u32) {
+    asm!("mov R15,$0"
+         :
+         : "r"(bits)
+         :
+         : "volatile");
+}
diff --git a/src/register/primask.rs b/src/register/primask.rs
new file mode 100644
index 00000000..1e24b730
--- /dev/null
+++ b/src/register/primask.rs
@@ -0,0 +1,40 @@
+//! Priority mask register
+
+/// All exceptions with configurable priority are ...
+#[derive(Clone, Copy, Eq, PartialEq)]
+pub enum Primask {
+    /// Active
+    Active,
+    /// Inactive
+    Inactive,
+}
+
+impl Primask {
+    /// All exceptions with configurable priority are active
+    pub fn is_active(&self) -> bool {
+        *self == Primask::Active
+    }
+
+    /// All exceptions with configurable priority are inactive
+    pub fn is_inactive(&self) -> bool {
+        *self == Primask::Inactive
+    }
+}
+
+/// Reads the CPU register
+#[inline(always)]
+pub fn read() -> Primask {
+    let r: u32;
+    unsafe {
+        asm!("mrs $0, PRIMASK"
+             : "=r"(r)
+             :
+             :
+             : "volatile");
+    }
+    if r & (1 << 0) == (1 << 0) {
+        Primask::Inactive
+    } else {
+        Primask::Active
+    }
+}
diff --git a/src/register/psp.rs b/src/register/psp.rs
new file mode 100644
index 00000000..ecd6f9cb
--- /dev/null
+++ b/src/register/psp.rs
@@ -0,0 +1,25 @@
+//! Process Stack Pointer
+
+/// Reads the CPU register
+#[inline(always)]
+pub fn read() -> u32 {
+    let r;
+    unsafe {
+        asm!("mrs $0,PSP"
+             : "=r"(r)
+             :
+             :
+             : "volatile");
+    }
+    r
+}
+
+/// Writes `bits` to the CPU register
+#[inline(always)]
+pub unsafe fn write(bits: u32) {
+    asm!("msr PSP,$0"
+         :
+         : "r"(bits)
+         :
+         : "volatile");
+}