Partial support of KSE for arm.

8 files changed, 132 insertions, 118 deletions

diff --git a/lib/libkse/arch/arm/arm/context.S b/lib/libkse/arch/arm/arm/context.S
index f5e0113..addb399 100644
--- a/lib/libkse/arch/arm/arm/context.S
+++ b/lib/libkse/arch/arm/arm/context.S
@@ -43,18 +43,9 @@ ENTRY(__thr_setcontext)
 	cmp 	r0, #0
 	moveq 	r0, #-1
 	moveq 	pc, lr
-	add	ip, r0, #8
-	ldmia	ip, {r2-r12}
 	cmp	r2, #0
-	str	r1, [r2]
-	add	ip, r0, #4
-	str	ip, [r1] /* Restore r1. */
-	add	ip, r0, #64
-	msr	cpsr, ip
-	add	ip, r0, #52
-	mov	r0, #0 /* Return 0. */
-	ldr	sp, [ip] /* Restore stack pointer. */
-	mov	pc, lr /* Return. */
+	strne	r1, [r2]
+	ldmia	r0, {r0-r15}
 	/* XXX: FP bits ? */
 
 /*
@@ -72,10 +63,14 @@ ENTRY(__thr_getcontext)
 	moveq	r0, #-1
 	moveq	pc, lr
 	stmia	r0, {r0-r13}
+	mov	r1, #1
+	str	r1, [r0] /* Return 1 from setcontext */
+	str	lr, [r0, #(15 * 4)] /* PC */
+	mrs	r1, cpsr
+	str	r1, [r0, #(16 * 4)] /* CPSR */
 	mov	r0, #0 /* Return 0. */
 	mov	pc, lr
 
 ENTRY(_arm_enter_uts)
-	add	r4, r2, r3 /* Stack addr + size. */
-	mov	lr, pc
+	add	sp, r2, r3 /* Stack addr + size. */
 	mov	pc, r1
diff --git a/lib/libkse/arch/arm/arm/pthread_md.c b/lib/libkse/arch/arm/arm/pthread_md.c
index d6bf95d..f8eda42 100644
--- a/lib/libkse/arch/arm/arm/pthread_md.c
+++ b/lib/libkse/arch/arm/arm/pthread_md.c
@@ -39,19 +39,15 @@ __FBSDID("$FreeBSD$");
 
 #include "pthread_md.h"
 
+struct arm_tp *_tp = NULL;
+
 struct tcb *
 _tcb_ctor(struct pthread *thread, int initial)
 {
 	struct tcb *tcb;
-	void *addr;
 
-	addr = malloc(sizeof(struct tcb) + 63);
-	if (addr == NULL)
-		tcb = NULL;
-	else {
-		tcb = (struct tcb *)(((uintptr_t)(addr) + 63) & ~63);
+	if ((tcb = malloc(sizeof(struct tcb)))) {
 		bzero(tcb, sizeof(struct tcb));
-		tcb->tcb_addr = addr;
 		tcb->tcb_thread = thread;
 		/* XXX - Allocate tdv/tls */
 	}
@@ -61,11 +57,8 @@ _tcb_ctor(struct pthread *thread, int initial)
 void
 _tcb_dtor(struct tcb *tcb)
 {
-	void *addr;
 
-	addr = tcb->tcb_addr;
-	tcb->tcb_addr = NULL;
-	free(addr);
+	free(tcb);
 }
 
 struct kcb *
diff --git a/lib/libkse/arch/arm/include/atomic_ops.h b/lib/libkse/arch/arm/include/atomic_ops.h
index e36a6bf..1ee5d80 100644
--- a/lib/libkse/arch/arm/include/atomic_ops.h
+++ b/lib/libkse/arch/arm/include/atomic_ops.h
@@ -29,6 +29,9 @@
 #ifndef	_ATOMIC_OPS_H_
 #define	_ATOMIC_OPS_H_
 
+#include <machine/atomic.h>
+#include "thr_private.h"
+
 /*
  * Atomic swap:
  *   Atomic (tmp = *dst, *dst = val), then *res = tmp
@@ -38,9 +41,7 @@
 static inline void
 atomic_swap32(intptr_t *dst, intptr_t val, intptr_t *res)
 {
-	__asm __volatile(
-	"swp %2, %2, [%1]; mov %2, %0"
-	 : "=r" (*res) : "r" (dst), "r" (val) : "cc");
+	*res = __swp(val, dst);
 }
 
 #define	atomic_swap_ptr(d, v, r) \
@@ -49,3 +50,19 @@ atomic_swap32(intptr_t *dst, intptr_t val, intptr_t *res)
 #define	atomic_swap_int(d, v, r) \
 	atomic_swap32((intptr_t *)d, (intptr_t)v, (intptr_t *)r)
 #endif
+
+static inline u_int32_t
+atomic_cmpset_32(volatile u_int32_t *p, u_int32_t cmpval, u_int32_t newval)
+{
+	kse_critical_t crit = _kse_critical_enter();
+	int ret;
+
+	if (*p == cmpval) {
+		*p = newval;
+		ret = 1;
+	} else
+		ret = 0;
+	_kse_critical_leave(crit);
+	return (ret);
+}
+
diff --git a/lib/libkse/arch/arm/include/pthread_md.h b/lib/libkse/arch/arm/include/pthread_md.h
index 5fa0a85..b9ac871 100644
--- a/lib/libkse/arch/arm/include/pthread_md.h
+++ b/lib/libkse/arch/arm/include/pthread_md.h
@@ -64,19 +64,15 @@ struct tdv;	/* We don't know what this is yet? */
  */
 struct arm_tp {
 	struct tdv		*tp_tdv;	/* dynamic TLS */
-	uint32_t		_reserved_;
-	long double		tp_tls[0];	/* static TLS */
 };
 
 struct tcb {
 	struct pthread		*tcb_thread;
-	void			*tcb_addr;	/* allocated tcb address */
 	struct kcb		*tcb_curkcb;
 	uint32_t		tcb_isfake;
-	uint32_t		tcb_spare[4];
 	struct kse_thr_mailbox	tcb_tmbx;	/* needs 32-byte alignment */
 	struct arm_tp		tcb_tp;
-} __aligned(32);
+};
 
 struct kcb {
 	struct kse_mailbox	kcb_kmbx;
@@ -85,7 +81,7 @@ struct kcb {
 	struct kse		*kcb_kse;
 };
 
-register struct arm_tp *_tp __asm("%r6");
+extern struct arm_tp *_tp;
 
 #define	_tcb	((struct tcb*)((char*)(_tp) - offsetof(struct tcb, tcb_tp)))
 
@@ -97,12 +93,21 @@ void		_tcb_dtor(struct tcb *);
 struct kcb	*_kcb_ctor(struct kse *kse);
 void		_kcb_dtor(struct kcb *);
 
+static __inline uint32_t
+__kcb_swp(uint32_t val, void *ptr)
+{       
+	
+	__asm __volatile("swp   %0, %1, [%2]"
+	    : "=r" (val) : "r" (val) , "r" (ptr) : "memory");
+    	return (val);
+}
+
 /* Called from the KSE to set its private data. */
 static __inline void
 _kcb_set(struct kcb *kcb)
 {
 	/* There is no thread yet; use the fake tcb. */
-	_tp = &kcb->kcb_faketcb.tcb_tp;
+	__kcb_swp((uint32_t)&kcb->kcb_faketcb.tcb_tp, &_tp);
 }
 
 /*
@@ -126,30 +131,21 @@ static __inline struct kse_thr_mailbox *
 _kcb_critical_enter(void)
 {
 	struct kse_thr_mailbox *crit;
-	uint32_t flags;
 
-	if (_tcb->tcb_isfake != 0) {
-		/*
-		 * We already are in a critical region since
-		 * there is no current thread.
-		 */
-		crit = NULL;
-	} else {
-		flags = _tcb->tcb_tmbx.tm_flags;
-		_tcb->tcb_tmbx.tm_flags |= TMF_NOUPCALL;
-		crit = _tcb->tcb_curkcb->kcb_kmbx.km_curthread;
-		_tcb->tcb_curkcb->kcb_kmbx.km_curthread = NULL;
-		_tcb->tcb_tmbx.tm_flags = flags;
-	}
+	if (_tcb->tcb_isfake)
+		return (NULL);
+	crit = (struct kse_thr_mailbox *)__kcb_swp((uint32_t)NULL, 
+	    &_tcb->tcb_curkcb->kcb_kmbx.km_curthread);
 	return (crit);
 }
 
 static __inline void
 _kcb_critical_leave(struct kse_thr_mailbox *crit)
 {
-	/* No need to do anything if this is a fake tcb. */
+
 	if (_tcb->tcb_isfake == 0)
-		_tcb->tcb_curkcb->kcb_kmbx.km_curthread = crit;
+		__kcb_swp((uint32_t)crit, 
+		    &_tcb->tcb_curkcb->kcb_kmbx.km_curthread);
 }
 
 static __inline int
@@ -158,6 +154,7 @@ _kcb_in_critical(void)
 	uint32_t flags;
 	int ret;
 
+	return (_tcb->tcb_curkcb->kcb_kmbx.km_curthread == NULL);
 	if (_tcb->tcb_isfake != 0) {
 		/*
 		 * We are in a critical region since there is no
@@ -176,11 +173,11 @@ _kcb_in_critical(void)
 static __inline void
 _tcb_set(struct kcb *kcb, struct tcb *tcb)
 {
-	if (tcb == NULL)
+	if (tcb == NULL) 
 		tcb = &kcb->kcb_faketcb;
+	__kcb_swp((uint32_t)&tcb->tcb_tp, &_tp);
 	kcb->kcb_curtcb = tcb;
 	tcb->tcb_curkcb = kcb;
-	_tp = &tcb->tcb_tp;
 }
 
 static __inline struct tcb *
@@ -206,22 +203,25 @@ _get_curkse(void)
 	return (_tcb->tcb_curkcb->kcb_kse);
 }
 
-void _arm_enter_uts(kse_func_t uts, struct kse_mailbox *km, void *stack,
+void _arm_enter_uts(struct kse_mailbox *km, kse_func_t uts, void *stack,
     size_t stacksz);
 
 static __inline int
 _thread_enter_uts(struct tcb *tcb, struct kcb *kcb)
 {
-	if (_thr_getcontext(&tcb->tcb_tmbx.tm_context.uc_mcontext) == 0) {
-		/* Make the fake tcb the current thread. */
+	int ret;
+	
+	if ((ret = _thr_getcontext(&tcb->tcb_tmbx.tm_context.uc_mcontext))
+	    == 0) {
 		kcb->kcb_curtcb = &kcb->kcb_faketcb;
-		_tp = &kcb->kcb_faketcb.tcb_tp;
-		_arm_enter_uts(kcb->kcb_kmbx.km_func, &kcb->kcb_kmbx,
+		__kcb_swp((int)&kcb->kcb_faketcb.tcb_tp, &_tp);
+		_arm_enter_uts(&kcb->kcb_kmbx, kcb->kcb_kmbx.km_func,
 		    kcb->kcb_kmbx.km_stack.ss_sp,
 		    kcb->kcb_kmbx.km_stack.ss_size);
 		/* We should not reach here. */
 		return (-1);
-	}
+	} else if (ret < 0)
+		return (-1);
 	return (0);
 }
 
@@ -231,9 +231,11 @@ _thread_switch(struct kcb *kcb, struct tcb *tcb, int setmbox)
 	extern int _libkse_debug;
 	mcontext_t *mc;
 
+	if (!tcb || !kcb)
+		return (-1);
 	_tcb_set(kcb, tcb);
 	mc = &tcb->tcb_tmbx.tm_context.uc_mcontext;
-	if (_libkse_debug == 0) {
+	if (0 && _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/arm/context.S b/lib/libpthread/arch/arm/arm/context.S
index f5e0113..addb399 100644
--- a/lib/libpthread/arch/arm/arm/context.S
+++ b/lib/libpthread/arch/arm/arm/context.S
@@ -43,18 +43,9 @@ ENTRY(__thr_setcontext)
 	cmp 	r0, #0
 	moveq 	r0, #-1
 	moveq 	pc, lr
-	add	ip, r0, #8
-	ldmia	ip, {r2-r12}
 	cmp	r2, #0
-	str	r1, [r2]
-	add	ip, r0, #4
-	str	ip, [r1] /* Restore r1. */
-	add	ip, r0, #64
-	msr	cpsr, ip
-	add	ip, r0, #52
-	mov	r0, #0 /* Return 0. */
-	ldr	sp, [ip] /* Restore stack pointer. */
-	mov	pc, lr /* Return. */
+	strne	r1, [r2]
+	ldmia	r0, {r0-r15}
 	/* XXX: FP bits ? */
 
 /*
@@ -72,10 +63,14 @@ ENTRY(__thr_getcontext)
 	moveq	r0, #-1
 	moveq	pc, lr
 	stmia	r0, {r0-r13}
+	mov	r1, #1
+	str	r1, [r0] /* Return 1 from setcontext */
+	str	lr, [r0, #(15 * 4)] /* PC */
+	mrs	r1, cpsr
+	str	r1, [r0, #(16 * 4)] /* CPSR */
 	mov	r0, #0 /* Return 0. */
 	mov	pc, lr
 
 ENTRY(_arm_enter_uts)
-	add	r4, r2, r3 /* Stack addr + size. */
-	mov	lr, pc
+	add	sp, r2, r3 /* Stack addr + size. */
 	mov	pc, r1
diff --git a/lib/libpthread/arch/arm/arm/pthread_md.c b/lib/libpthread/arch/arm/arm/pthread_md.c
index d6bf95d..f8eda42 100644
--- a/lib/libpthread/arch/arm/arm/pthread_md.c
+++ b/lib/libpthread/arch/arm/arm/pthread_md.c
@@ -39,19 +39,15 @@ __FBSDID("$FreeBSD$");
 
 #include "pthread_md.h"
 
+struct arm_tp *_tp = NULL;
+
 struct tcb *
 _tcb_ctor(struct pthread *thread, int initial)
 {
 	struct tcb *tcb;
-	void *addr;
 
-	addr = malloc(sizeof(struct tcb) + 63);
-	if (addr == NULL)
-		tcb = NULL;
-	else {
-		tcb = (struct tcb *)(((uintptr_t)(addr) + 63) & ~63);
+	if ((tcb = malloc(sizeof(struct tcb)))) {
 		bzero(tcb, sizeof(struct tcb));
-		tcb->tcb_addr = addr;
 		tcb->tcb_thread = thread;
 		/* XXX - Allocate tdv/tls */
 	}
@@ -61,11 +57,8 @@ _tcb_ctor(struct pthread *thread, int initial)
 void
 _tcb_dtor(struct tcb *tcb)
 {
-	void *addr;
 
-	addr = tcb->tcb_addr;
-	tcb->tcb_addr = NULL;
-	free(addr);
+	free(tcb);
 }
 
 struct kcb *
diff --git a/lib/libpthread/arch/arm/include/atomic_ops.h b/lib/libpthread/arch/arm/include/atomic_ops.h
index e36a6bf..1ee5d80 100644
--- a/lib/libpthread/arch/arm/include/atomic_ops.h
+++ b/lib/libpthread/arch/arm/include/atomic_ops.h
@@ -29,6 +29,9 @@
 #ifndef	_ATOMIC_OPS_H_
 #define	_ATOMIC_OPS_H_
 
+#include <machine/atomic.h>
+#include "thr_private.h"
+
 /*
  * Atomic swap:
  *   Atomic (tmp = *dst, *dst = val), then *res = tmp
@@ -38,9 +41,7 @@
 static inline void
 atomic_swap32(intptr_t *dst, intptr_t val, intptr_t *res)
 {
-	__asm __volatile(
-	"swp %2, %2, [%1]; mov %2, %0"
-	 : "=r" (*res) : "r" (dst), "r" (val) : "cc");
+	*res = __swp(val, dst);
 }
 
 #define	atomic_swap_ptr(d, v, r) \
@@ -49,3 +50,19 @@ atomic_swap32(intptr_t *dst, intptr_t val, intptr_t *res)
 #define	atomic_swap_int(d, v, r) \
 	atomic_swap32((intptr_t *)d, (intptr_t)v, (intptr_t *)r)
 #endif
+
+static inline u_int32_t
+atomic_cmpset_32(volatile u_int32_t *p, u_int32_t cmpval, u_int32_t newval)
+{
+	kse_critical_t crit = _kse_critical_enter();
+	int ret;
+
+	if (*p == cmpval) {
+		*p = newval;
+		ret = 1;
+	} else
+		ret = 0;
+	_kse_critical_leave(crit);
+	return (ret);
+}
+
diff --git a/lib/libpthread/arch/arm/include/pthread_md.h b/lib/libpthread/arch/arm/include/pthread_md.h
index 5fa0a85..b9ac871 100644
--- a/lib/libpthread/arch/arm/include/pthread_md.h
+++ b/lib/libpthread/arch/arm/include/pthread_md.h
@@ -64,19 +64,15 @@ struct tdv;	/* We don't know what this is yet? */
  */
 struct arm_tp {
 	struct tdv		*tp_tdv;	/* dynamic TLS */
-	uint32_t		_reserved_;
-	long double		tp_tls[0];	/* static TLS */
 };
 
 struct tcb {
 	struct pthread		*tcb_thread;
-	void			*tcb_addr;	/* allocated tcb address */
 	struct kcb		*tcb_curkcb;
 	uint32_t		tcb_isfake;
-	uint32_t		tcb_spare[4];
 	struct kse_thr_mailbox	tcb_tmbx;	/* needs 32-byte alignment */
 	struct arm_tp		tcb_tp;
-} __aligned(32);
+};
 
 struct kcb {
 	struct kse_mailbox	kcb_kmbx;
@@ -85,7 +81,7 @@ struct kcb {
 	struct kse		*kcb_kse;
 };
 
-register struct arm_tp *_tp __asm("%r6");
+extern struct arm_tp *_tp;
 
 #define	_tcb	((struct tcb*)((char*)(_tp) - offsetof(struct tcb, tcb_tp)))
 
@@ -97,12 +93,21 @@ void		_tcb_dtor(struct tcb *);
 struct kcb	*_kcb_ctor(struct kse *kse);
 void		_kcb_dtor(struct kcb *);
 
+static __inline uint32_t
+__kcb_swp(uint32_t val, void *ptr)
+{       
+	
+	__asm __volatile("swp   %0, %1, [%2]"
+	    : "=r" (val) : "r" (val) , "r" (ptr) : "memory");
+    	return (val);
+}
+
 /* Called from the KSE to set its private data. */
 static __inline void
 _kcb_set(struct kcb *kcb)
 {
 	/* There is no thread yet; use the fake tcb. */
-	_tp = &kcb->kcb_faketcb.tcb_tp;
+	__kcb_swp((uint32_t)&kcb->kcb_faketcb.tcb_tp, &_tp);
 }
 
 /*
@@ -126,30 +131,21 @@ static __inline struct kse_thr_mailbox *
 _kcb_critical_enter(void)
 {
 	struct kse_thr_mailbox *crit;
-	uint32_t flags;
 
-	if (_tcb->tcb_isfake != 0) {
-		/*
-		 * We already are in a critical region since
-		 * there is no current thread.
-		 */
-		crit = NULL;
-	} else {
-		flags = _tcb->tcb_tmbx.tm_flags;
-		_tcb->tcb_tmbx.tm_flags |= TMF_NOUPCALL;
-		crit = _tcb->tcb_curkcb->kcb_kmbx.km_curthread;
-		_tcb->tcb_curkcb->kcb_kmbx.km_curthread = NULL;
-		_tcb->tcb_tmbx.tm_flags = flags;
-	}
+	if (_tcb->tcb_isfake)
+		return (NULL);
+	crit = (struct kse_thr_mailbox *)__kcb_swp((uint32_t)NULL, 
+	    &_tcb->tcb_curkcb->kcb_kmbx.km_curthread);
 	return (crit);
 }
 
 static __inline void
 _kcb_critical_leave(struct kse_thr_mailbox *crit)
 {
-	/* No need to do anything if this is a fake tcb. */
+
 	if (_tcb->tcb_isfake == 0)
-		_tcb->tcb_curkcb->kcb_kmbx.km_curthread = crit;
+		__kcb_swp((uint32_t)crit, 
+		    &_tcb->tcb_curkcb->kcb_kmbx.km_curthread);
 }
 
 static __inline int
@@ -158,6 +154,7 @@ _kcb_in_critical(void)
 	uint32_t flags;
 	int ret;
 
+	return (_tcb->tcb_curkcb->kcb_kmbx.km_curthread == NULL);
 	if (_tcb->tcb_isfake != 0) {
 		/*
 		 * We are in a critical region since there is no
@@ -176,11 +173,11 @@ _kcb_in_critical(void)
 static __inline void
 _tcb_set(struct kcb *kcb, struct tcb *tcb)
 {
-	if (tcb == NULL)
+	if (tcb == NULL) 
 		tcb = &kcb->kcb_faketcb;
+	__kcb_swp((uint32_t)&tcb->tcb_tp, &_tp);
 	kcb->kcb_curtcb = tcb;
 	tcb->tcb_curkcb = kcb;
-	_tp = &tcb->tcb_tp;
 }
 
 static __inline struct tcb *
@@ -206,22 +203,25 @@ _get_curkse(void)
 	return (_tcb->tcb_curkcb->kcb_kse);
 }
 
-void _arm_enter_uts(kse_func_t uts, struct kse_mailbox *km, void *stack,
+void _arm_enter_uts(struct kse_mailbox *km, kse_func_t uts, void *stack,
     size_t stacksz);
 
 static __inline int
 _thread_enter_uts(struct tcb *tcb, struct kcb *kcb)
 {
-	if (_thr_getcontext(&tcb->tcb_tmbx.tm_context.uc_mcontext) == 0) {
-		/* Make the fake tcb the current thread. */
+	int ret;
+	
+	if ((ret = _thr_getcontext(&tcb->tcb_tmbx.tm_context.uc_mcontext))
+	    == 0) {
 		kcb->kcb_curtcb = &kcb->kcb_faketcb;
-		_tp = &kcb->kcb_faketcb.tcb_tp;
-		_arm_enter_uts(kcb->kcb_kmbx.km_func, &kcb->kcb_kmbx,
+		__kcb_swp((int)&kcb->kcb_faketcb.tcb_tp, &_tp);
+		_arm_enter_uts(&kcb->kcb_kmbx, kcb->kcb_kmbx.km_func,
 		    kcb->kcb_kmbx.km_stack.ss_sp,
 		    kcb->kcb_kmbx.km_stack.ss_size);
 		/* We should not reach here. */
 		return (-1);
-	}
+	} else if (ret < 0)
+		return (-1);
 	return (0);
 }
 
@@ -231,9 +231,11 @@ _thread_switch(struct kcb *kcb, struct tcb *tcb, int setmbox)
 	extern int _libkse_debug;
 	mcontext_t *mc;
 
+	if (!tcb || !kcb)
+		return (-1);
 	_tcb_set(kcb, tcb);
 	mc = &tcb->tcb_tmbx.tm_context.uc_mcontext;
-	if (_libkse_debug == 0) {
+	if (0 && _libkse_debug == 0) {
 		tcb->tcb_tmbx.tm_lwp = kcb->kcb_kmbx.km_lwp;
 		if (setmbox)
 			_thr_setcontext(mc, (intptr_t)&tcb->tcb_tmbx,