diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..6b2a4fc
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "ports/cortex-m/libopencm3"]
+	path = ports/cortex-m/libopencm3
+	url = https://github.com/libopencm3/libopencm3.git
diff --git a/kernel/atom.h b/kernel/atom.h
index 64c5ce8..3773c9b 100755
--- a/kernel/atom.h
+++ b/kernel/atom.h
@@ -42,6 +42,15 @@ extern "C" {
 /* Forward declaration */
 struct atom_tcb;
 
+/*
+ *  Define THREAD_PORT_PRIV to be empty if the used atomport.h does not define
+ *  a port specific entry for the atom_tcb struct. This way we do not have an
+ *  unused element.
+ */  
+#if !defined(THREAD_PORT_PRIV)
+#define THREAD_PORT_PRIV
+#endif
+
 typedef struct atom_tcb
 {
     /*
@@ -50,6 +59,9 @@ typedef struct atom_tcb
      */
     POINTER sp_save_ptr;
 
+    /* Thread's port specific private data. */
+    THREAD_PORT_PRIV;
+
     /* Thread priority (0-255) */
     uint8_t priority;
 
diff --git a/ports/cortex-m/Makefile b/ports/cortex-m/Makefile
index 2a1e14f..4ba03e9 100644
--- a/ports/cortex-m/Makefile
+++ b/ports/cortex-m/Makefile
@@ -3,16 +3,32 @@
 ################################################
 
 ifeq ($(V),)
-	Q := @
-	# Do not print "Entering directory ...".
-	MAKEFLAGS += --no-print-directory
+    Q := @
+    # Do not print "Entering directory ...".
+    MAKEFLAGS += --no-print-directory
 endif
 
+# Use the size optimised nano version of newlib.
+# This usually is the version of newlib you want to use, although it has some
+# limitations, like printf not having support for floating point vars.
+# This relies on your newlib installation to provide a working nano.specs
+# file. On Debian you will need at least version 2.1.0+git20141201.db59ff3-2
+# of package libnewlib-arm-none-eabi
+USE_NANO    := true
+
+# Debian's libnewlib-arm-none-eabi package version 2.2.0+git20150830.5a3d536-1
+# ships with a buggy nano.specs file that does not set up a proper include
+# path for finding the nano version of newlib.h.
+# Also, the nano version has been built with the -fshort-wchar option, making
+# it incompatible with object files using the standard ABI. By enabling this
+# option atomthreads and libopencm3 will also be compiled with -fshort-wchar.
+#FIX_DEBIAN  := true
+
 # Build directory
 ifdef O
-	build_dir=$(shell readlink -f $(O))
+    build_dir=$(shell readlink -f $(O))
 else
-	build_dir=$(CURDIR)/build
+    build_dir=$(CURDIR)/build
 endif
 
 # Source directory
@@ -24,30 +40,30 @@ aobjs :=
 
 # set default board if none is given
 ifeq ($(BOARD),)
-	BOARD = nucleo-f103rb
+    BOARD = nucleo-f103rb
 endif
 
 include $(src_dir)/boards/$(BOARD)/Makefile.include
 
 # Make sure target MCU is set
 ifndef TARGET
-	$(error TARGET undefined)
+    $(error TARGET undefined)
 endif
 
 # Configure toolchain
-CROSS_COMPILE	?=	arm-none-eabi-
+CROSS_COMPILE   ?=  arm-none-eabi-
 
-CC		:= $(CROSS_COMPILE)gcc
-CXX		:= $(CROSS_COMPILE)g++
-LD		:= $(CROSS_COMPILE)gcc
-AR		:= $(CROSS_COMPILE)ar
-AS		:= $(CROSS_COMPILE)as
-OBJCOPY		:= $(CROSS_COMPILE)objcopy
-OBJDUMP		:= $(CROSS_COMPILE)objdump
-GDB		:= $(CROSS_COMPILE)gdb
-STFLASH		= $(shell which st-flash)
+CC      := $(CROSS_COMPILE)gcc
+CXX     := $(CROSS_COMPILE)g++
+LD      := $(CROSS_COMPILE)gcc
+AR      := $(CROSS_COMPILE)ar
+AS      := $(CROSS_COMPILE)as
+OBJCOPY := $(CROSS_COMPILE)objcopy
+OBJDUMP := $(CROSS_COMPILE)objdump
+GDB     := $(CROSS_COMPILE)gdb
+STFLASH  = $(shell which st-flash)
 
-LDSCRIPT	?= linker/$(TARGET).ld
+LDSCRIPT    ?= linker/$(TARGET).ld
 
 # Enable stack-checking. WARNING: the full automated test suite currently
 # requires a little over 1KB RAM with stack-checking enabled. If you are 
@@ -64,8 +80,8 @@ tests_dir=$(src_dir)/../../tests
 # Check if user wants to use external opencm3 lib or if we have to build
 # it ourselves
 ifeq ($(OPENCM3_DIR),)
-	OPENCM3_DIR = $(src_dir)/libopencm3
-	build_lib = true
+    OPENCM3_DIR = $(src_dir)/libopencm3
+    build_lib = true
 endif
 
 ifneq ($(V),)
@@ -84,72 +100,86 @@ objs += atomtimer.o
 objs += atomqueue.o
 
 # Collection of built objects (excluding test applications)
-build_objs = $(foreach obj,$(objs),$(build_dir)/$(obj))
+build_objs  = $(foreach obj,$(objs),$(build_dir)/$(obj))
 
 # Object needed by all test applications, but not user apps
 tmobjs = tests-main.o
-build_tmobjs    = $(foreach obj,$(tmobjs),$(build_dir)/$(obj))
-build_tmobjs   += $(build_objs)
+build_tmobjs     = $(foreach obj,$(tmobjs),$(build_dir)/$(obj))
+build_tmobjs    += $(build_objs)
 
 # Set up search paths for libopencm3
-INCLUDE_DIR	= $(OPENCM3_DIR)/include
-LIB_DIR		= $(OPENCM3_DIR)/lib
-SCRIPT_DIR	= $(OPENCM3_DIR)/scripts
+INCLUDE_DIR = $(OPENCM3_DIR)/include
+LIB_DIR     = $(OPENCM3_DIR)/lib
+SCRIPT_DIR  = $(OPENCM3_DIR)/scripts
 
 # GCC flags
-CFLAGS		=  -Os -g
-CFLAGS		+= -Wall -Werror
-CFLAGS		+= -Wredundant-decls -Wstrict-prototypes
-CFLAGS		+= -fno-common -ffunction-sections -fdata-sections
+CFLAGS       =  -Os -g
+CFLAGS      += -Wall -Werror
+CFLAGS      += -Wredundant-decls -Wstrict-prototypes
+CFLAGS      += -fno-common -ffunction-sections -fdata-sections
 
 # Enable stack-checking (disable if not required)
 ifeq ($(STACK_CHECK),true)
-CFLAGS += -DATOM_STACK_CHECKING -DTESTS_LOG_STACK_USAGE
+    CFLAGS  += -DATOM_STACK_CHECKING -DTESTS_LOG_STACK_USAGE
 endif
 
 # C & C++ preprocessor common flags
-CPPFLAGS	+= -MD
-CPPFLAGS	+= -Wall -Wundef -Werror
-CPPFLAGS	+= -I$(INCLUDE_DIR) $(DEFS)
+CPPFLAGS    += -MD
+CPPFLAGS    += -Wall -Wundef -Werror
+CPPFLAGS    += -I$(INCLUDE_DIR) $(DEFS)
 CPPFLAGS    += -I$(board_dir) -I$(common_dir) -I$(src_dir) -I$(kernel_dir) -I$(tests_dir)
 
+# Assembler flags
+ASFLAGS     += -D__ASSEMBLY__ 
+ASFLAGS     += -D__NEWLIB__
+
 # Linker flags
-LDFLAGS		+= --static -nostartfiles
-LDFLAGS		+= -L$(LIB_DIR)
-LDFLAGS		+= -T$(LDSCRIPT)
-LDFLAGS		+= -Wl,-Map=$(build_dir)/$(*).map
-LDFLAGS		+= -Wl,--gc-sections
+LDFLAGS     += --static -nostartfiles
+LDFLAGS     += -L$(LIB_DIR)
+LDFLAGS     += -T$(LDSCRIPT)
+LDFLAGS     += -Wl,-Map=$(build_dir)/$(*).map
+LDFLAGS     += -Wl,--gc-sections
+LDFLAGS     += -Wl,--fatal-warnings
 ifeq ($(V),99)
-LDFLAGS		+= -Wl,--print-gc-sections
+LDFLAGS     += -Wl,--print-gc-sections
 endif
 
 ## Used libraries
 # Target specific version libopencm3
 LDLIBS		+= -l$(LIBNAME)
 
+## Gather newlib libraries and set up specfiles.
+NEWLIBS	    += -lc -lgcc
+
 ifneq ($(BOARD),qemu)
-## Flags for newlibc.    
-# This uses the version of newlib optimised for size and without libnosys.
-# This usually is the version of newlib you want to use, although it has some
-# limitations, like printf not having support for floating point vars.
-LDLIBS		+= -Wl,--start-group -lc_nano -lgcc -Wl,--end-group
+ifeq ($(USE_NANO),true)
+SPECS       := -specs=nano.specs
 
-# This is the fully bloated version of newlib. Adds about 20k compared to
-# libc_nano. 
-#LDLIBS		+= -Wl,--start-group -lc -lgcc -Wl,--end-group
+ifeq ($(FIX_DEBIAN),true)
+SPECS       += -I/usr/include/newlib/nano
+LOCM3_FLAGS += -fshort-wchar
+CFLAGS      += -fshort-wchar
+endif
+endif
 
-# Use small newlib with libnosys if you do not want to use the provided stubs.
-# Be advised that heap management will probaly break in interesting ways.
+# Uncomment to link against libnosys if you do not want to use the provided
+# stubs.
+# Be advised that heap management will probably break in interesting ways.
 # This is because libnosys' standard _sbrk() expects the stack to start at 
 # the top end of memory while the threads' stacks are positioned inside 
 # the BSS.
-#LDLIBS		+= -Wl,--start-group -lc_nano -lgcc -lnosys -Wl,--end-group
+#NEWLIBS    += -lnosys
+
 else
 # Special LDLIBS for qemu target to enable semi-hosting. 
 # TODO: Check if this is also useful for real hardware  
-LDLIBS		+= -Wl,--start-group --specs=rdimon.specs -lc -lrdimon -lgcc -Wl,--end-group
+NEWLIBS     += -lrdimon
+SPECS       := -specs=rdimon.specs
 endif
 
+# add all required newlib libraries as a group
+LDLIBS      += -Wl,--start-group $(NEWLIBS) -Wl,--end-group
+
 .PHONY: all
 all: build_all
 
@@ -182,7 +212,7 @@ $(1): $(build_dir)/$(1).elf
 $(build_dir)/$(1).elf $(build_dir)/$(1).map: $(LIB_DIR)/lib$(LIBNAME).a $(build_dir)/$(1).o $(build_objs) $(LDSCRIPT)
 	$$(Q)mkdir -p `dirname $$@`
 	$$(if $$(Q), @echo " (ELF)       $$(subst $$(build_dir)/,,$$@)")
-	$$(Q)$$(LD) $$(LDFLAGS) $$(ARCH_FLAGS) $$(build_objs) $$(build_dir)/$(1).o $(LDLIBS) -o $$@
+	$$(Q)$$(LD) $$(SPECS) $$(LDFLAGS) $$(ARCH_FLAGS) $$(build_objs) $$(build_dir)/$(1).o $(LDLIBS) -o $$@
 endef
 
 # Target application filenames .elf for each user app object
@@ -209,18 +239,17 @@ build_all: $(LIB_DIR)/lib$(LIBNAME).a $(all_bins) $(all_hexs) $(all_elfs) $(all_
 # Add build dependency for local libopencm3 if no external libopencm3 is used 
 ifeq ($(build_lib),true)
 $(LIB_DIR)/lib$(LIBNAME).a: 
-	$(Q)if [ ! -d libopencm3 ] ; then \
+	$(Q)if [ ! -f libopencm3/Makefile ] ; then \
 		printf "######## ERROR ########\n"; \
 		printf "\tlibopencm3 is not initialized.\n"; \
 		printf "\tPlease run:\n"; \
-		printf "\t$$ git submodule add https://github.com/libopencm3/libopencm3.git\n"; \
 		printf "\t$$ git submodule init\n"; \
 		printf "\t$$ git submodule update\n"; \
 		printf "\tbefore running make.\n"; \
 		printf "######## ERROR ########\n"; \
 		exit 1; \
 		fi
-	$(Q)$(MAKE) -C libopencm3 V=$(V)
+	$(Q)$(MAKE) -C libopencm3 V=$(V) CFLAGS=$(LOCM3_FLAGS)
 endif
 
 $(build_dir)/%.bin: $(build_dir)/%.elf
@@ -249,47 +278,47 @@ $(build_dir)/%.list: $(build_dir)/%.elf
 $(build_dir)/%.elf $(build_dir)/%.map: $(LIB_DIR)/lib$(LIBNAME).a $(build_dir)/%.o $(build_tmobjs) $(LDSCRIPT)
 	$(Q)mkdir -p `dirname $@`
 	$(if $(Q), @echo " (ELF)       $(subst $(build_dir)/,,$@)")
-	$(Q)$(LD) $(LDFLAGS) $(ARCH_FLAGS) $(build_tmobjs) $(build_dir)/$(*).o $(LDLIBS) -o $@
+	$(Q)$(LD) $(SPECS) $(LDFLAGS) $(ARCH_FLAGS) $(build_tmobjs) $(build_dir)/$(*).o $(LDLIBS) -o $@
 
 $(build_dir)/%.o: $(src_dir)/%.S Makefile $(board_dir)/Makefile.include
 	$(Q)mkdir -p `dirname $@`
 	$(if $(Q), @echo " (AS)        $(subst $(build_dir)/,,$@)")
-	$(Q)$(CC) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -D__ASSEMBLY__ -I`dirname $<` -c $< -o $@
+	$(Q)$(CC) $(SPECS) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) $(ASFLAGS) -I`dirname $<` -c $< -o $@
 
 $(build_dir)/%.o: $(src_dir)/%.c Makefile $(board_dir)/Makefile.include
 	$(Q)mkdir -p `dirname $@`
 	$(if $(Q), @echo " (CC)        $(subst $(build_dir)/,,$@)")
-	$(Q)$(CC) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
+	$(Q)$(CC) $(SPECS) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
 
 $(build_dir)/%.o: $(board_dir)/%.S Makefile $(board_dir)/Makefile.include
 	$(Q)mkdir -p `dirname $@`
 	$(if $(Q), @echo " (AS)        $(subst $(build_dir)/,,$@)")
-	$(Q)$(CC) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -D__ASSEMBLY__ -I`dirname $<` -c $< -o $@
+	$(Q)$(CC) $(SPECS) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) $(ASFLAGS) -I`dirname $<` -c $< -o $@
 
 $(build_dir)/%.o: $(board_dir)/%.c Makefile $(board_dir)/Makefile.include
 	$(Q)mkdir -p `dirname $@`
 	$(if $(Q), @echo " (CC)        $(subst $(build_dir)/,,$@)")
-	$(Q)$(CC) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
+	$(Q)$(CC) $(SPECS) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
 
 $(build_dir)/%.o: $(common_dir)/%.S Makefile $(board_dir)/Makefile.include
 	$(Q)mkdir -p `dirname $@`
 	$(if $(Q), @echo " (AS)        $(subst $(build_dir)/,,$@)")
-	$(Q)$(CC) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -D__ASSEMBLY__ -I`dirname $<` -c $< -o $@
+	$(Q)$(CC) $(SPECS) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) $(ASFLAGS) -I`dirname $<` -c $< -o $@
 
 $(build_dir)/%.o: $(common_dir)/%.c Makefile $(board_dir)/Makefile.include
 	$(Q)mkdir -p `dirname $@`
 	$(if $(Q), @echo " (CC)        $(subst $(build_dir)/,,$@)")
-	$(Q)$(CC) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
+	$(Q)$(CC) $(SPECS) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
 
 $(build_dir)/%.o: $(kernel_dir)/%.c Makefile $(board_dir)/Makefile.include
 	$(Q)mkdir -p `dirname $@`
 	$(if $(Q), @echo " (CC)        $(subst $(build_dir)/,,$@)")
-	$(Q)$(CC) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
+	$(Q)$(CC) $(SPECS) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
 
 $(build_dir)/%.o: $(tests_dir)/%.c Makefile $(board_dir)/Makefile.include
 	$(Q)mkdir -p `dirname $@`
 	$(if $(Q), @echo " (CC)        $(subst $(build_dir)/,,$@)")
-	$(Q)$(CC) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
+	$(Q)$(CC) $(SPECS) $(CFLAGS) $(CPPFLAGS) $(ARCH_FLAGS) -I`dirname $<` -c $< -o $@
 
 # Clean. Remove only atomthread's object files and images
 .PHONY: clean
@@ -363,4 +392,4 @@ else
 endif
 
 # Include auto-generated dependencies
--include $(all_objs:.o=.d)
\ No newline at end of file
+-include $(all_objs:.o=.d)
diff --git a/ports/cortex-m/README.md b/ports/cortex-m/README.md
index 53b61a3..580e6a3 100644
--- a/ports/cortex-m/README.md
+++ b/ports/cortex-m/README.md
@@ -20,6 +20,19 @@ apt-get install gcc-arm-none-eabi binutils-arm-none-eabi
 apt-get install libnewlib-arm-none-eabi libnewlib-dev
 apt-get install openocd
 ```
+**N.B.** Usually you will want to compile and link against the size optimised
+"nano" version of newlib. This is done by default. If your version
+of newlib does not support this (Debian's libnewlib package version before
+2.1.0+git20141201.db59ff3-2) you will have to comment out the line
+`USE_NANO := true` in the Makefile or pass `USE_NANO=` as
+a command line option to make.
+
+**N.B.** Debian's libnewlib-arm-none-eabi version 2.2.0+git20150830.5a3d536-1
+ships with broken nano support. To enable necessary workarounds, uncomment
+the line `#FIX_DEBIAN := true` in the Makefile or pass `FIX_DEBIAN=true`
+as a command line option to make.
+If you are using this fix, be advised that when switching between nano
+and regular builds, you will have to do a `make realclean` first.
 
 ## Code Layout
 The "classic" port components (code needed for task set-up and context
@@ -52,17 +65,9 @@ which will be removed by `make clean`.
 Unless you decide to use an external installation of libopencm3, you will have
 to set up the libopencm3 sub-module:
 ```
-git submodule add https://github.com/libopencm3/libopencm3.git
 git submodule init
 git submodule update
 ```
-Optional: As of 2015-07-08 the libopencm3 API has not been declared stable. If
-future changes break the build, you can check out the older revision used while
-developing this port:
-```
-cd libopencm3
-git checkout a4bb8f7e240c9f238384cf86d009002ba42a25ed
-```
 
 ## Building and Flashing
 To build the test suite, run
diff --git a/ports/cortex-m/asm_offsets.h b/ports/cortex-m/asm_offsets.h
new file mode 100644
index 0000000..495bbdd
--- /dev/null
+++ b/ports/cortex-m/asm_offsets.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2015, Tido Klaassen. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ * 3. No personal names or organizations' names associated with the
+ *    Atomthreads project may be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE ATOMTHREADS PROJECT 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 PROJECT 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.
+ */
+
+#ifndef __ATOM_PORT_ASM_OFFSETS_H
+#define __ATOM_PORT_ASM_OFFSETS_H
+
+#define CTX_RUN_OFF     0
+#define CTX_NEXT_OFF    4
+#define CTX_REENT_OFF   8
+
+#endif /* __ATOM_PORT_ASM_OFFSETS_H */
diff --git a/ports/cortex-m/atomport-asm.S b/ports/cortex-m/atomport-asm.S
index 6098ba3..d1c8454 100644
--- a/ports/cortex-m/atomport-asm.S
+++ b/ports/cortex-m/atomport-asm.S
@@ -27,6 +27,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include "asm_offsets.h"
+
 .syntax unified
 
 /**
@@ -49,6 +51,13 @@
 .extern CTX_SW_NFO
 .extern vector_table
 
+#if defined(__NEWLIB__)
+/**
+ * When using newlib, reentry context needs to be updated on task switch
+ */
+.extern _impure_ptr
+#endif
+
 /**
  * Some bit masks and registers used
  */
@@ -58,11 +67,11 @@
 
 .text
 
-.global archFirstThreadRestore
-.func   archFirstThreadRestore
-.type   archFirstThreadRestore,%function
+.global _archFirstThreadRestore
+.func   _archFirstThreadRestore
+.type   _archFirstThreadRestore,%function
 .thumb_func
-archFirstThreadRestore:
+_archFirstThreadRestore:
     /**
      * Disable interrupts. They should be disabled anyway, but just
      * to make sure...
@@ -80,8 +89,18 @@ archFirstThreadRestore:
     
     /* Update ctx_switch_info, set this thread as both running and next */
     ldr     r1,         = CTX_SW_NFO
-    str     r0,         [r1, #0]
-    str     r0,         [r1, #4]
+    str     r0,         [r1, #CTX_RUN_OFF]
+    str     r0,         [r1, #CTX_NEXT_OFF]
+
+#if defined(__NEWLIB__)
+    /**
+     * Store the thread's reentry context address in _impure_ptr. This
+     * will have been stored in ctx_switch_info.reent.
+     */ 
+    ldr     r2,         [r1, #CTX_REENT_OFF]
+    ldr     r3,         = _impure_ptr
+    str     r2,         [r3, #0]
+#endif
 
     /* Get thread stack pointer from tcb. Conveniently the first element */
     ldr     r1,         [r0, #0]
@@ -179,7 +198,7 @@ archFirstThreadRestore:
     pop     {r2,r3,pc}
     nop
 
-.size   archFirstThreadRestore, . - archFirstThreadRestore
+.size   _archFirstThreadRestore, . - _archFirstThreadRestore
 .endfunc
 
 .global pend_sv_handler
@@ -216,14 +235,15 @@ pend_sv_handler:
      * gets called.
      */
     ldr     r0,         = CTX_SW_NFO
-    ldr     r1,         [r0, #0]
-    ldr     r2,         [r0, #4]
+    ldr     r1,         [r0, #CTX_RUN_OFF]
+    ldr     r2,         [r0, #CTX_NEXT_OFF]
     cmp     r1,         r2
     beq     no_switch
 
     /**
      * Copy running thread's process stack pointer to r3 and use it to push
-     * */
+     * the thread's register context on its stack
+     */
     mrs     r3,         PSP
 
 #if defined(THUMB_2)
@@ -252,7 +272,7 @@ pend_sv_handler:
     /**
      * Save old thread's register context on Cortex-M0.
      * Push running thread's remaining registers on stack.
-     * Thumb-1 can only use stm on low registers, so we
+     * Thumb-1 can use stm only on low registers, so we
      * have to do this in two steps.
      */
 
@@ -281,8 +301,8 @@ pend_sv_handler:
     subs    r3,         r3, #32
 #endif // !THUMB_2
     /**
-     * Address of running TCB still in r1. Store threads current stack top
-     * into its sp_save_ptr, which is the struct's first element
+     * Address of running TCB still in r1. Store thread's current stack top
+     * into its sp_save_ptr, which is the struct's first element.
      */
     str     r3,         [r1, #0]
     
@@ -290,8 +310,18 @@ pend_sv_handler:
      * ctx_switch_info.next_tcb is going to become ctx_switch_info.running_tcb,
      * so we update the pointer.
      */
-    str     r2,         [r0, #0]
+    str     r2,         [r0, #CTX_RUN_OFF]
     
+#if defined(__NEWLIB__)
+    /**
+     * Store the thread's reentry context address in _impure_ptr. This
+     * will have been stored in ctx_switch_info.reent.
+     */ 
+    ldr     r4,         [r0, #CTX_REENT_OFF]
+    ldr     r3,         = _impure_ptr
+    str     r4,         [r3, #0]
+#endif
+
     /**
      * Fetch next thread's stack pointer from its TCB's sp_save_ptr and restore
      * the thread's register context.
diff --git a/ports/cortex-m/atomport-private.h b/ports/cortex-m/atomport-private.h
index 6fe0e86..b670673 100644
--- a/ports/cortex-m/atomport-private.h
+++ b/ports/cortex-m/atomport-private.h
@@ -112,6 +112,9 @@ struct task_fpu_stack {
 struct task_switch_info {
     volatile struct atom_tcb *running_tcb;
     volatile struct atom_tcb *next_tcb;
+#if defined(__NEWLIB__)
+    struct _reent *reent;
+#endif
 } __attribute__((packed));
 
 #endif /* __ATOMPORT_PRIVATE_H_ */
diff --git a/ports/cortex-m/atomport.c b/ports/cortex-m/atomport.c
index 49ea73d..3ec72a1 100644
--- a/ports/cortex-m/atomport.c
+++ b/ports/cortex-m/atomport.c
@@ -27,6 +27,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <string.h>
+
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/cm3/systick.h>
 #include <libopencm3/cm3/cortex.h>
@@ -35,18 +37,27 @@
 
 #include "atomport.h"
 #include "atomport-private.h"
+#include "asm_offsets.h"
 
 static void thread_shell(void);
 
-/**
- *
- */
 struct task_switch_info ctx_switch_info asm("CTX_SW_NFO") =
 {
     .running_tcb = NULL,
     .next_tcb    = NULL,
 };
 
+extern void _archFirstThreadRestore(ATOM_TCB *);
+void archFirstThreadRestore(ATOM_TCB *new_tcb_ptr)
+{
+#if defined(__NEWLIB__)
+    ctx_switch_info.reent = &(new_tcb_ptr->port_priv.reent);
+    __dmb();
+#endif
+
+    _archFirstThreadRestore(new_tcb_ptr);
+}
+
 /**
  * We do not perform the context switch directly. Instead we mark the new tcb
  * as should-be-running in ctx_switch_info and trigger a PendSv-interrupt.
@@ -68,7 +79,9 @@ archContextSwitch(ATOM_TCB *old_tcb_ptr __maybe_unused, ATOM_TCB *new_tcb_ptr)
 {
     if(likely(ctx_switch_info.running_tcb != NULL)){
         ctx_switch_info.next_tcb = new_tcb_ptr;
-
+#if defined(__NEWLIB__)
+        ctx_switch_info.reent = &(new_tcb_ptr->port_priv.reent);
+#endif
         __dmb();
 
         SCB_ICSR = SCB_ICSR_PENDSVSET;
@@ -143,6 +156,17 @@ void archThreadContextInit(ATOM_TCB *tcb_ptr, void *stack_top,
     struct isr_stack *isr_ctx;
     struct task_stack *tsk_ctx;
 
+    /**
+     * Do compile time verification for offsets used in _archFirstThreadRestore
+     * and pend_sv_handler. If compilation aborts here, you will have to adjust
+     * the offsets for struct task_switch_info's members in asm-offsets.h
+     */
+    assert_static(offsetof(struct task_switch_info, running_tcb) == CTX_RUN_OFF);
+    assert_static(offsetof(struct task_switch_info, next_tcb) == CTX_NEXT_OFF);
+#if defined(__NEWLIB__)
+    assert_static(offsetof(struct task_switch_info, reent) == CTX_REENT_OFF);
+#endif
+
     /**
      * Enforce initial stack alignment
      */
@@ -201,5 +225,12 @@ void archThreadContextInit(ATOM_TCB *tcb_ptr, void *stack_top,
     tcb_ptr->sp_save_ptr = tsk_ctx;
     tcb_ptr->entry_point = entry_point;
     tcb_ptr->entry_param = entry_param;
+
+#if defined(__NEWLIB__)
+    /**
+     * Initialise thread's reentry context for newlib
+     */ 
+    _REENT_INIT_PTR(&(tcb_ptr->port_priv.reent));
+#endif
 }
 
diff --git a/ports/cortex-m/atomport.h b/ports/cortex-m/atomport.h
index 562b84e..d3337ea 100644
--- a/ports/cortex-m/atomport.h
+++ b/ports/cortex-m/atomport.h
@@ -33,6 +33,7 @@
 #include <stdint.h>
 #include <stddef.h>
 #include <libopencm3/cm3/cortex.h>
+#include <stdlib.h>
 
 /* Required number of system ticks per second (normally 100 for 10ms tick) */
 #define SYSTEM_TICKS_PER_SEC            100
@@ -51,6 +52,11 @@
 #define unlikely(x)         __builtin_expect(!!(x), 0)
 #define __maybe_unused      __attribute__((unused))
 
+#define assert_static(e) \
+   do { \
+      enum { assert_static__ = 1/(e) }; \
+   } while (0)
+
 /**
  * Critical region protection: this should disable interrupts
  * to protect OS data structures during modification. It must
@@ -61,6 +67,18 @@
 #define CRITICAL_START()    __irq_flags = cm_mask_interrupts(true)
 #define CRITICAL_END()      (void) cm_mask_interrupts(__irq_flags)
 
+/**
+ * When using newlib, define port private field in atom_tcb to be a
+ * struct _reent.
+ */
+#if defined(__NEWLIB__)
+struct cortex_port_priv {
+    struct _reent reent;
+};
+
+#define THREAD_PORT_PRIV    struct cortex_port_priv port_priv
+#endif
+
 /* Uncomment to enable stack-checking */
 /* #define ATOM_STACK_CHECKING */
 
diff --git a/ports/cortex-m/common/stubs.c b/ports/cortex-m/common/stubs.c
index 4bcf302..d12a17c 100644
--- a/ports/cortex-m/common/stubs.c
+++ b/ports/cortex-m/common/stubs.c
@@ -64,19 +64,23 @@ static char *heap_end = 0;
 caddr_t _sbrk(int incr)
 {
     char *prev_end;
+    CRITICAL_STORE;
 
-    if(heap_end == 0){
+    prev_end = NULL;
+
+    CRITICAL_START();
+
+    if(unlikely(heap_end == 0)){
         heap_end = &end;
     }
 
-    prev_end = heap_end;
-
-    if(heap_end + incr + MST_SIZE > (char *) vector_table.initial_sp_value){
-        /* new heap size would collide with main stack area*/
-        return (caddr_t) 0;
+    /* make sure new heap size does not collide with main stack area*/
+    if(heap_end + incr + MST_SIZE <= (char *) vector_table.initial_sp_value){
+        prev_end = heap_end;
+        heap_end += incr;
     }
 
-    heap_end += incr;
+    CRITICAL_END();
 
     return (caddr_t) prev_end;
 }
diff --git a/ports/cortex-m/libopencm3 b/ports/cortex-m/libopencm3
new file mode 160000
index 0000000..27b826b
--- /dev/null
+++ b/ports/cortex-m/libopencm3
@@ -0,0 +1 @@
+Subproject commit 27b826bc4a09345f63d3b8b5fcd3cb9f145cd1a5