diff --git a/lib/libkse/arch/arm/Makefile.inc b/lib/libkse/arch/arm/Makefile.inc
index 6a62aa68040e..ced7063106e1 100644
--- a/lib/libkse/arch/arm/Makefile.inc
+++ b/lib/libkse/arch/arm/Makefile.inc
@@ -2,4 +2,6 @@
 
 .PATH:	${.CURDIR}/arch/${MACHINE_ARCH}/${MACHINE_ARCH}
 
+CFLAGS+=-DSYSTEM_SCOPE_ONLY
+
 SRCS+=	pthread_md.c context.S
diff --git a/lib/libkse/arch/arm/arm/context.S b/lib/libkse/arch/arm/arm/context.S
index addb399f7f42..c63880458bd4 100644
--- a/lib/libkse/arch/arm/arm/context.S
+++ b/lib/libkse/arch/arm/arm/context.S
@@ -45,7 +45,10 @@ ENTRY(__thr_setcontext)
 	moveq 	pc, lr
 	cmp	r2, #0
 	strne	r1, [r2]
+	ldr	r1, [r0, #(16 * 4)] /* CPSR */
+	msr	cpsr, r1
 	ldmia	r0, {r0-r15}
+	mov	pc, lr
 	/* XXX: FP bits ? */
 
 /*
@@ -62,7 +65,7 @@ ENTRY(__thr_getcontext)
 	cmp	r0, #0
 	moveq	r0, #-1
 	moveq	pc, lr
-	stmia	r0, {r0-r13}
+	stmia	r0, {r1-r14}
 	mov	r1, #1
 	str	r1, [r0] /* Return 1 from setcontext */
 	str	lr, [r0, #(15 * 4)] /* PC */
diff --git a/lib/libkse/arch/arm/arm/pthread_md.c b/lib/libkse/arch/arm/arm/pthread_md.c
index f8eda42c7b38..72426d467985 100644
--- a/lib/libkse/arch/arm/arm/pthread_md.c
+++ b/lib/libkse/arch/arm/arm/pthread_md.c
@@ -37,9 +37,11 @@ __FBSDID("$FreeBSD$");
 #include <string.h>
 #include <ucontext.h>
 
+#include <machine/sysarch.h>
+
 #include "pthread_md.h"
 
-struct arm_tp *_tp = NULL;
+struct arm_tp **arm_tp = (struct arm_tp **)ARM_TP_ADDRESS;
 
 struct tcb *
 _tcb_ctor(struct pthread *thread, int initial)
diff --git a/lib/libkse/arch/arm/include/pthread_md.h b/lib/libkse/arch/arm/include/pthread_md.h
index b9ac87125c2a..857fa1b1c8b4 100644
--- a/lib/libkse/arch/arm/include/pthread_md.h
+++ b/lib/libkse/arch/arm/include/pthread_md.h
@@ -81,7 +81,8 @@ struct kcb {
 	struct kse		*kcb_kse;
 };
 
-extern struct arm_tp *_tp;
+extern struct arm_tp **arm_tp;
+#define _tp (*arm_tp)
 
 #define	_tcb	((struct tcb*)((char*)(_tp) - offsetof(struct tcb, tcb_tp)))
 
@@ -235,7 +236,7 @@ _thread_switch(struct kcb *kcb, struct tcb *tcb, int setmbox)
 		return (-1);
 	_tcb_set(kcb, tcb);
 	mc = &tcb->tcb_tmbx.tm_context.uc_mcontext;
-	if (0 && _libkse_debug == 0) {
+	if (_libkse_debug == 0) {
 		tcb->tcb_tmbx.tm_lwp = kcb->kcb_kmbx.km_lwp;
 		if (setmbox)
 			_thr_setcontext(mc, (intptr_t)&tcb->tcb_tmbx,
diff --git a/lib/libpthread/arch/arm/Makefile.inc b/lib/libpthread/arch/arm/Makefile.inc
index 6a62aa68040e..ced7063106e1 100644
--- a/lib/libpthread/arch/arm/Makefile.inc
+++ b/lib/libpthread/arch/arm/Makefile.inc
@@ -2,4 +2,6 @@
 
 .PATH:	${.CURDIR}/arch/${MACHINE_ARCH}/${MACHINE_ARCH}
 
+CFLAGS+=-DSYSTEM_SCOPE_ONLY
+
 SRCS+=	pthread_md.c context.S
diff --git a/lib/libpthread/arch/arm/arm/context.S b/lib/libpthread/arch/arm/arm/context.S
index addb399f7f42..c63880458bd4 100644
--- a/lib/libpthread/arch/arm/arm/context.S
+++ b/lib/libpthread/arch/arm/arm/context.S
@@ -45,7 +45,10 @@ ENTRY(__thr_setcontext)
 	moveq 	pc, lr
 	cmp	r2, #0
 	strne	r1, [r2]
+	ldr	r1, [r0, #(16 * 4)] /* CPSR */
+	msr	cpsr, r1
 	ldmia	r0, {r0-r15}
+	mov	pc, lr
 	/* XXX: FP bits ? */
 
 /*
@@ -62,7 +65,7 @@ ENTRY(__thr_getcontext)
 	cmp	r0, #0
 	moveq	r0, #-1
 	moveq	pc, lr
-	stmia	r0, {r0-r13}
+	stmia	r0, {r1-r14}
 	mov	r1, #1
 	str	r1, [r0] /* Return 1 from setcontext */
 	str	lr, [r0, #(15 * 4)] /* PC */
diff --git a/lib/libpthread/arch/arm/arm/pthread_md.c b/lib/libpthread/arch/arm/arm/pthread_md.c
index f8eda42c7b38..72426d467985 100644
--- a/lib/libpthread/arch/arm/arm/pthread_md.c
+++ b/lib/libpthread/arch/arm/arm/pthread_md.c
@@ -37,9 +37,11 @@ __FBSDID("$FreeBSD$");
 #include <string.h>
 #include <ucontext.h>
 
+#include <machine/sysarch.h>
+
 #include "pthread_md.h"
 
-struct arm_tp *_tp = NULL;
+struct arm_tp **arm_tp = (struct arm_tp **)ARM_TP_ADDRESS;
 
 struct tcb *
 _tcb_ctor(struct pthread *thread, int initial)
diff --git a/lib/libpthread/arch/arm/include/pthread_md.h b/lib/libpthread/arch/arm/include/pthread_md.h
index b9ac87125c2a..857fa1b1c8b4 100644
--- a/lib/libpthread/arch/arm/include/pthread_md.h
+++ b/lib/libpthread/arch/arm/include/pthread_md.h
@@ -81,7 +81,8 @@ struct kcb {
 	struct kse		*kcb_kse;
 };
 
-extern struct arm_tp *_tp;
+extern struct arm_tp **arm_tp;
+#define _tp (*arm_tp)
 
 #define	_tcb	((struct tcb*)((char*)(_tp) - offsetof(struct tcb, tcb_tp)))
 
@@ -235,7 +236,7 @@ _thread_switch(struct kcb *kcb, struct tcb *tcb, int setmbox)
 		return (-1);
 	_tcb_set(kcb, tcb);
 	mc = &tcb->tcb_tmbx.tm_context.uc_mcontext;
-	if (0 && _libkse_debug == 0) {
+	if (_libkse_debug == 0) {
 		tcb->tcb_tmbx.tm_lwp = kcb->kcb_kmbx.km_lwp;
 		if (setmbox)
 			_thr_setcontext(mc, (intptr_t)&tcb->tcb_tmbx,