From b5e35c63a5ce4bb987a9133c577366c458035e38 Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Mon, 7 Jul 2014 21:19:10 +0800
Subject: [PATCH 01/18] Add libposix templete

---
 user/apps/l4test/main.c               |  6 +++++-
 user/include/libposix/libposix.h      |  6 ++++++
 user/include/libposix/unimplemented.h | 18 ++++++++++++++++++
 user/lib/build.mk                     |  1 +
 user/lib/libposix/build.mk            |  6 ++++++
 user/lib/libposix/fork.c              |  9 +++++++++
 6 files changed, 45 insertions(+), 1 deletion(-)
 create mode 100644 user/include/libposix/libposix.h
 create mode 100644 user/include/libposix/unimplemented.h
 create mode 100644 user/lib/libposix/build.mk
 create mode 100644 user/lib/libposix/fork.c

diff --git a/user/apps/l4test/main.c b/user/apps/l4test/main.c
index 2b48cb98..1790cfb7 100644
--- a/user/apps/l4test/main.c
+++ b/user/apps/l4test/main.c
@@ -20,7 +20,10 @@
 #include "l4test.h"
 #include "assert.h"
 
-#define STACK_SIZE 512
+/* libposix */
+#include <libposix/libposix.h>
+
+#define STACK_SIZE 256
 
 /* where to start allocating RAM */
 __USER_BSS static char *free_page;
@@ -157,6 +160,7 @@ __USER_TEXT void all_tests(void)
 __USER_TEXT
 static void *main(void *user)
 {
+	fork();
 	printf("\nL4/Pistachio test suite starts\n");
 	all_tests();
 	return NULL;
diff --git a/user/include/libposix/libposix.h b/user/include/libposix/libposix.h
new file mode 100644
index 00000000..df09c959
--- /dev/null
+++ b/user/include/libposix/libposix.h
@@ -0,0 +1,6 @@
+#ifndef __LIBPOSIX_H__
+#define __LIBPOSIX_H__
+
+int fork(void);
+
+#endif
diff --git a/user/include/libposix/unimplemented.h b/user/include/libposix/unimplemented.h
new file mode 100644
index 00000000..8356e732
--- /dev/null
+++ b/user/include/libposix/unimplemented.h
@@ -0,0 +1,18 @@
+#ifndef __UNIMPLEMENTED_H__
+#define __UNIMPLEMENTED_H__
+
+#define ESC "\e["
+#define LIGHT_RED    ESC "31;1m"
+#define BLACK        ESC "0m"
+
+#include <l4io.h>
+
+#define UNIMPLEMENTED()						\
+	do {							\
+		printf("%s", LIGHT_RED);			\
+		printf("\nUnimplemented: %s, at %s:%d",		\
+		       __func__, __FILE__, __LINE__);		\
+		printf("%s\n", BLACK);				\
+	} while (0)						\
+
+#endif
diff --git a/user/lib/build.mk b/user/lib/build.mk
index ed6cadf9..2906d4b6 100644
--- a/user/lib/build.mk
+++ b/user/lib/build.mk
@@ -5,3 +5,4 @@
 user-lib-dirs = \
 	l4 \
 	io \
+	libposix \
diff --git a/user/lib/libposix/build.mk b/user/lib/libposix/build.mk
new file mode 100644
index 00000000..d66e9051
--- /dev/null
+++ b/user/lib/libposix/build.mk
@@ -0,0 +1,6 @@
+# Copyright (c) 2013 The F9 Microkernel Project. All rights reserved.
+# Use of this source code is governed by a BSD-style license that can be
+# found in the LICENSE file.
+
+user-lib-libposix-y = \
+	fork.o
diff --git a/user/lib/libposix/fork.c b/user/lib/libposix/fork.c
new file mode 100644
index 00000000..232d9e6c
--- /dev/null
+++ b/user/lib/libposix/fork.c
@@ -0,0 +1,9 @@
+#include <libposix/libposix.h>
+#include <libposix/unimplemented.h>
+#include <platform/link.h>
+
+int __USER_TEXT fork()
+{
+	UNIMPLEMENTED();
+	return 0;
+}

From 351f92bf03c0b91f5229d23736c9e9db033997fa Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Tue, 8 Jul 2014 22:47:59 +0800
Subject: [PATCH 02/18] Move posix layer test to a new app

---
 user/apps/build.mk           |  3 ++-
 user/apps/l4test/main.c      |  4 ----
 user/apps/posixtest/build.mk |  6 ++++++
 user/apps/posixtest/main.c   | 23 +++++++++++++++++++++++
 user/include/user_runtime.h  |  1 +
 5 files changed, 32 insertions(+), 5 deletions(-)
 create mode 100644 user/apps/posixtest/build.mk
 create mode 100644 user/apps/posixtest/main.c

diff --git a/user/apps/build.mk b/user/apps/build.mk
index a623963b..c21d4ab4 100644
--- a/user/apps/build.mk
+++ b/user/apps/build.mk
@@ -4,7 +4,8 @@
 
 user-apps-dirs = \
 	l4test \
-	pingpong
+	posixtest \
+	pingpong \
 
 ifdef CONFIG_BOARD_STM32F429DISCOVERY
 user-apps-dirs += \
diff --git a/user/apps/l4test/main.c b/user/apps/l4test/main.c
index 1790cfb7..31e46089 100644
--- a/user/apps/l4test/main.c
+++ b/user/apps/l4test/main.c
@@ -20,9 +20,6 @@
 #include "l4test.h"
 #include "assert.h"
 
-/* libposix */
-#include <libposix/libposix.h>
-
 #define STACK_SIZE 256
 
 /* where to start allocating RAM */
@@ -160,7 +157,6 @@ __USER_TEXT void all_tests(void)
 __USER_TEXT
 static void *main(void *user)
 {
-	fork();
 	printf("\nL4/Pistachio test suite starts\n");
 	all_tests();
 	return NULL;
diff --git a/user/apps/posixtest/build.mk b/user/apps/posixtest/build.mk
new file mode 100644
index 00000000..c08c6ff8
--- /dev/null
+++ b/user/apps/posixtest/build.mk
@@ -0,0 +1,6 @@
+# Copyright (c) 2013 The F9 Microkernel Project. All rights reserved.
+# Use of this source code is governed by a BSD-style license that can be
+# found in the LICENSE file.
+
+user-apps-posixtest-y = \
+	main.o \
diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
new file mode 100644
index 00000000..5c35c27b
--- /dev/null
+++ b/user/apps/posixtest/main.c
@@ -0,0 +1,23 @@
+#include <user_runtime.h>
+#include <platform/link.h>
+#include <l4/utcb.h>
+#include <l4io.h>
+
+/* posix layer */
+#include <libposix/libposix.h>
+
+#define STACK_SIZE 256
+
+static __USER_TEXT void main(user_struct *user)
+{
+	printf("\nPosix Layer test starts\n");
+	fork();
+	return;
+}
+
+DECLARE_USER(
+	255,
+	posixtest,
+	main,
+	DECLARE_FPAGE(0x0, 4 * (UTCB_SIZE + STACK_SIZE))
+);
diff --git a/user/include/user_runtime.h b/user/include/user_runtime.h
index 39c47405..eafc12fa 100644
--- a/user/include/user_runtime.h
+++ b/user/include/user_runtime.h
@@ -7,6 +7,7 @@
 #define USER_RUNTIME_H
 
 #include <l4/types.h>
+#include <l4/ipc.h>
 
 typedef struct user_struct user_struct;
 typedef void (*user_entry)(user_struct *);

From 5cb089ce1bde849609f603ee60bc47b117e48d3d Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Tue, 15 Jul 2014 20:36:41 +0800
Subject: [PATCH 03/18] Add pthread_create

---
 user/include/libposix/pthread.h   | 10 ++++++
 user/include/libposix/sys/types.h | 11 +++++++
 user/lib/libposix/build.mk        |  3 +-
 user/lib/libposix/pthread.c       | 53 +++++++++++++++++++++++++++++++
 4 files changed, 76 insertions(+), 1 deletion(-)
 create mode 100644 user/include/libposix/pthread.h
 create mode 100644 user/include/libposix/sys/types.h
 create mode 100644 user/lib/libposix/pthread.c

diff --git a/user/include/libposix/pthread.h b/user/include/libposix/pthread.h
new file mode 100644
index 00000000..b9d55f2d
--- /dev/null
+++ b/user/include/libposix/pthread.h
@@ -0,0 +1,10 @@
+#ifndef __PTHREAD_H__
+#define __PTHREAD_H__
+
+#include <libposix/sys/types.h>
+
+int pthread_create(pthread_t *restrict thread,
+                   const pthread_attr_t *restrict attr,
+                   void *(*start_routine)(void*), void *restrict arg);
+
+#endif
diff --git a/user/include/libposix/sys/types.h b/user/include/libposix/sys/types.h
new file mode 100644
index 00000000..0f67788d
--- /dev/null
+++ b/user/include/libposix/sys/types.h
@@ -0,0 +1,11 @@
+#ifndef __TYPES_H_
+#define __TYPES_H_
+
+#include <stdint.h>
+
+/* FIXME: Define proper type for pthread type */
+typedef uint32_t pthread_mutex_t;
+typedef uint32_t pthread_t;
+typedef uint32_t pthread_attr_t;
+
+#endif
diff --git a/user/lib/libposix/build.mk b/user/lib/libposix/build.mk
index d66e9051..28c92178 100644
--- a/user/lib/libposix/build.mk
+++ b/user/lib/libposix/build.mk
@@ -3,4 +3,5 @@
 # found in the LICENSE file.
 
 user-lib-libposix-y = \
-	fork.o
+	fork.o \
+	pthread.o
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
new file mode 100644
index 00000000..43bb87ae
--- /dev/null
+++ b/user/lib/libposix/pthread.c
@@ -0,0 +1,53 @@
+#include <libposix/pthread.h>
+#include <l4/ipc.h>
+#include <l4/utcb.h>
+#include <platform/link.h>
+
+#define STACK_SIZE 256
+
+static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
+                                     L4_Word_t sp, L4_Word_t stack_size)
+{
+	L4_Msg_t msg;
+
+	L4_MsgClear(&msg);
+	L4_MsgAppendWord(&msg, ip);
+	L4_MsgAppendWord(&msg, sp);
+	L4_MsgAppendWord(&msg, stack_size);
+	L4_MsgLoad(&msg);
+
+	L4_Send(t);
+}
+
+__USER_TEXT int pthread_create(pthread_t *restrict thread,
+                   const pthread_attr_t *restrict attr,
+                   void *(*start_routine)(void*), void *restrict arg)
+{
+	/* FIXME: Get valid free memory space */
+	L4_Word_t free_mem = NULL;
+	L4_ThreadId_t myself = L4_MyGlobalId();
+	L4_ThreadId_t child;
+
+	/* FIXME: This should be pack into parent thread structure */
+	static L4_Word_t last_thread;
+
+	child.raw = myself.raw + (++last_thread << 14);
+
+	/* TODO: Handle attr */
+	if(!attr) {
+	}
+
+	/* TODO: pthread_sigmask, sigpending, sigaltstack */
+	/* TODO: fenv */
+	/* TODO: Initilize cpu clock to 0(pthread_getcpuclockid) */
+	/* TODO: Update thread structure, fill in new thread id */
+
+	L4_ThreadControl(child, myself, L4_nilthread, myself, (void *) free_mem);
+	free_mem += UTCB_SIZE + STACK_SIZE;
+
+	start_thread(child, (L4_Word_t)start_routine, free_mem, STACK_SIZE);
+
+	/* TODO: Error tag return */
+	return 0;
+}
+

From add8e616c5fab2bf880815c4c0d74d889035a532 Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Tue, 15 Jul 2014 21:13:52 +0800
Subject: [PATCH 04/18] Add basic pthread_* function prototype

---
 user/include/libposix/pthread.h   |  9 +++++++
 user/include/libposix/sys/types.h |  5 ++++
 user/lib/libposix/pthread.c       | 44 +++++++++++++++++++++++++++++--
 3 files changed, 56 insertions(+), 2 deletions(-)

diff --git a/user/include/libposix/pthread.h b/user/include/libposix/pthread.h
index b9d55f2d..ef86ca38 100644
--- a/user/include/libposix/pthread.h
+++ b/user/include/libposix/pthread.h
@@ -6,5 +6,14 @@
 int pthread_create(pthread_t *restrict thread,
                    const pthread_attr_t *restrict attr,
                    void *(*start_routine)(void*), void *restrict arg);
+int pthread_detach(pthread_t thread);
+void pthread_exit(void *value_ptr);
+int pthread_join(pthread_t thread, void **value_ptr);
+int pthread_mutex_destroy(pthread_mutex_t *mutex);
+int pthread_mutex_lock(pthread_mutex_t *mutex);
+int pthread_mutex_trylock(pthread_mutex_t *mutex);
+int pthread_mutex_unlock(pthread_mutex_t *mutex);
+int pthread_mutex_timedlock(pthread_mutex_t *restrict mutex,
+                            const struct timespec *restrict abstime);
 
 #endif
diff --git a/user/include/libposix/sys/types.h b/user/include/libposix/sys/types.h
index 0f67788d..c75e7369 100644
--- a/user/include/libposix/sys/types.h
+++ b/user/include/libposix/sys/types.h
@@ -3,6 +3,11 @@
 
 #include <stdint.h>
 
+/* FIXME: This should be moved to time.h */
+struct timespec {
+	uint64_t nsec;
+};
+
 /* FIXME: Define proper type for pthread type */
 typedef uint32_t pthread_mutex_t;
 typedef uint32_t pthread_t;
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index 43bb87ae..eca63cc6 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -20,8 +20,8 @@ static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
 }
 
 __USER_TEXT int pthread_create(pthread_t *restrict thread,
-                   const pthread_attr_t *restrict attr,
-                   void *(*start_routine)(void*), void *restrict arg)
+                               const pthread_attr_t *restrict attr,
+                               void *(*start_routine)(void*), void *restrict arg)
 {
 	/* FIXME: Get valid free memory space */
 	L4_Word_t free_mem = NULL;
@@ -51,3 +51,43 @@ __USER_TEXT int pthread_create(pthread_t *restrict thread,
 	return 0;
 }
 
+__USER_TEXT int pthread_detach(pthread_t thread)
+{
+	return 0;	
+}
+
+__USER_TEXT void pthread_exit(void *value_ptr) 
+{
+
+}
+
+__USER_TEXT int pthread_join(pthread_t thread, void **value_ptr)
+{
+	return 0;
+}
+
+__USER_TEXT int pthread_mutex_destroy(pthread_mutex_t *mutex)
+{
+	return 0;
+}
+
+__USER_TEXT int pthread_mutex_lock(pthread_mutex_t *mutex)
+{
+	return 0;
+}
+
+__USER_TEXT int pthread_mutex_trylock(pthread_mutex_t *mutex)
+{
+	return 0;
+}
+
+__USER_TEXT int pthread_mutex_unlock(pthread_mutex_t *mutex)
+{
+	return 0;
+}
+
+__USER_TEXT int pthread_mutex_timedlock(pthread_mutex_t *restrict mutex,
+                                        const struct timespec *restrict abstime)
+{
+	return 0;
+}

From 60acb37071a6b7057286ad11cda71863f05d321c Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sat, 9 Aug 2014 11:52:56 +0800
Subject: [PATCH 05/18] Implement basic pthread_create function

Now can correct create a new thread by pthread_create.
But the child thread will not execute their task because I write
`printf("child task start\n")` in the start_routine of child.
Nothing output so I need to figure out what happen.
---
 user/apps/posixtest/main.c  | 17 +++++++++++++--
 user/lib/libposix/pthread.c | 41 +++++++++++++++++++++++++++++--------
 user/root_thread.c          |  4 ++++
 3 files changed, 52 insertions(+), 10 deletions(-)

diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
index 5c35c27b..a6aa4b4b 100644
--- a/user/apps/posixtest/main.c
+++ b/user/apps/posixtest/main.c
@@ -4,14 +4,27 @@
 #include <l4io.h>
 
 /* posix layer */
-#include <libposix/libposix.h>
+#include <libposix/pthread.h>
 
 #define STACK_SIZE 256
 
+__USER_TEXT void *child_thread(void *args)
+{
+	printf("child task start\n");
+
+	while(1);
+
+	return 0;
+}
+
 static __USER_TEXT void main(user_struct *user)
 {
 	printf("\nPosix Layer test starts\n");
-	fork();
+	pthread_create(NULL, NULL, child_thread, NULL);
+	pthread_create(NULL, NULL, child_thread, NULL);
+
+	while(1);
+
 	return;
 }
 
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index eca63cc6..564090c5 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -1,9 +1,20 @@
 #include <libposix/pthread.h>
 #include <l4/ipc.h>
 #include <l4/utcb.h>
+#include <l4io.h>
 #include <platform/link.h>
 
 #define STACK_SIZE 256
+#define MAX_THREAD 128
+
+struct pthread_i {
+	L4_ThreadId_t tid;
+	L4_Word_t free_mem;
+	int last_thread;
+};
+
+__USER_DATA struct pthread_i parray[MAX_THREAD];
+__USER_DATA int current_thread = 0;
 
 static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
                                      L4_Word_t sp, L4_Word_t stack_size)
@@ -19,19 +30,33 @@ static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
 	L4_Send(t);
 }
 
+__USER_TEXT void _pthread_create(L4_ThreadId_t tid, L4_Word_t free_mem)
+{
+	parray[current_thread].tid = tid;
+	parray[current_thread].free_mem = free_mem;
+	parray[current_thread].last_thread = 0;
+	current_thread++;
+}
+
 __USER_TEXT int pthread_create(pthread_t *restrict thread,
                                const pthread_attr_t *restrict attr,
                                void *(*start_routine)(void*), void *restrict arg)
 {
-	/* FIXME: Get valid free memory space */
-	L4_Word_t free_mem = NULL;
+	int index;
 	L4_ThreadId_t myself = L4_MyGlobalId();
 	L4_ThreadId_t child;
 
-	/* FIXME: This should be pack into parent thread structure */
-	static L4_Word_t last_thread;
+	for(index = 0; index < current_thread; index++) {
+		if(parray[index].tid.raw == myself.raw) {
+			break;
+		}
+
+		if(index == current_thread - 1) {
+			return -1;
+		}
+	}
 
-	child.raw = myself.raw + (++last_thread << 14);
+	child.raw = myself.raw + (++parray[index].last_thread << 14);
 
 	/* TODO: Handle attr */
 	if(!attr) {
@@ -42,10 +67,10 @@ __USER_TEXT int pthread_create(pthread_t *restrict thread,
 	/* TODO: Initilize cpu clock to 0(pthread_getcpuclockid) */
 	/* TODO: Update thread structure, fill in new thread id */
 
-	L4_ThreadControl(child, myself, L4_nilthread, myself, (void *) free_mem);
-	free_mem += UTCB_SIZE + STACK_SIZE;
+	L4_ThreadControl(child, myself, L4_nilthread, myself, (void *) parray[index].free_mem);
+	parray[index].free_mem += UTCB_SIZE + STACK_SIZE;
 
-	start_thread(child, (L4_Word_t)start_routine, free_mem, STACK_SIZE);
+	start_thread(child, (L4_Word_t)start_routine, parray[index].free_mem, STACK_SIZE);
 
 	/* TODO: Error tag return */
 	return 0;
diff --git a/user/root_thread.c b/user/root_thread.c
index 0401bf26..d63d8210 100644
--- a/user/root_thread.c
+++ b/user/root_thread.c
@@ -7,6 +7,7 @@
 #include <l4/kip.h>
 #include <l4/utcb.h>
 #include <l4/ipc.h>
+#include <l4io.h>
 #include <types.h>
 #include <user_runtime.h>
 
@@ -77,6 +78,8 @@ static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
 
 #define STACK_SIZE 0x200
 
+extern void _pthread_create(L4_ThreadId_t tid, L4_Word_t free_mem);
+
 void __USER_TEXT __root_thread(kip_t *kip_ptr, utcb_t *utcb_ptr)
 {
 	L4_ThreadId_t myself = {.raw = utcb_ptr->t_globalid};
@@ -113,6 +116,7 @@ void __USER_TEXT __root_thread(kip_t *kip_ptr, utcb_t *utcb_ptr)
 			free_mem += fpage->size;
 			fpage++;
 		}
+		_pthread_create(tid, ptr->fpages[0].base);
 
 		/* start thread */
 		start_thread(tid, (L4_Word_t)ptr->entry, stack, STACK_SIZE);

From 828be6119f1327bb151336dd2af6ab6808a5c4f4 Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sat, 9 Aug 2014 13:27:10 +0800
Subject: [PATCH 06/18] fix starvation problem in child thread

---
 user/apps/posixtest/main.c | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
index a6aa4b4b..2ebc2eca 100644
--- a/user/apps/posixtest/main.c
+++ b/user/apps/posixtest/main.c
@@ -1,6 +1,7 @@
 #include <user_runtime.h>
 #include <platform/link.h>
 #include <l4/utcb.h>
+#include <l4/ipc.h>
 #include <l4io.h>
 
 /* posix layer */
@@ -12,7 +13,8 @@ __USER_TEXT void *child_thread(void *args)
 {
 	printf("child task start\n");
 
-	while(1);
+	while(1)
+		L4_Sleep(L4_Never);
 
 	return 0;
 }
@@ -23,7 +25,8 @@ static __USER_TEXT void main(user_struct *user)
 	pthread_create(NULL, NULL, child_thread, NULL);
 	pthread_create(NULL, NULL, child_thread, NULL);
 
-	while(1);
+	while(1)
+		L4_Sleep(L4_Never);
 
 	return;
 }

From 37b7194ba9fe4ba2f966b5b92c7282843755ea74 Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sat, 9 Aug 2014 15:15:28 +0800
Subject: [PATCH 07/18] Use pointer to free_mem instead of duplicating free_mem
 variable

---
 user/apps/posixtest/main.c  |  2 ++
 user/lib/libposix/pthread.c | 11 ++++++-----
 user/root_thread.c          |  4 ++--
 3 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
index 2ebc2eca..774cc9a5 100644
--- a/user/apps/posixtest/main.c
+++ b/user/apps/posixtest/main.c
@@ -22,6 +22,8 @@ __USER_TEXT void *child_thread(void *args)
 static __USER_TEXT void main(user_struct *user)
 {
 	printf("\nPosix Layer test starts\n");
+	L4_ThreadId_t pager = L4_Pager();
+	printf("Pager = %x\n", pager);
 	pthread_create(NULL, NULL, child_thread, NULL);
 	pthread_create(NULL, NULL, child_thread, NULL);
 
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index 564090c5..882ef89e 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -9,7 +9,7 @@
 
 struct pthread_i {
 	L4_ThreadId_t tid;
-	L4_Word_t free_mem;
+	L4_Word_t *free_mem;
 	int last_thread;
 };
 
@@ -30,7 +30,8 @@ static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
 	L4_Send(t);
 }
 
-__USER_TEXT void _pthread_create(L4_ThreadId_t tid, L4_Word_t free_mem)
+
+__USER_TEXT void _pthread_create(L4_ThreadId_t tid, L4_Word_t *free_mem)
 {
 	parray[current_thread].tid = tid;
 	parray[current_thread].free_mem = free_mem;
@@ -67,10 +68,10 @@ __USER_TEXT int pthread_create(pthread_t *restrict thread,
 	/* TODO: Initilize cpu clock to 0(pthread_getcpuclockid) */
 	/* TODO: Update thread structure, fill in new thread id */
 
-	L4_ThreadControl(child, myself, L4_nilthread, myself, (void *) parray[index].free_mem);
-	parray[index].free_mem += UTCB_SIZE + STACK_SIZE;
+	L4_ThreadControl(child, myself, L4_nilthread, myself, (void *)*parray[index].free_mem);
+	*parray[index].free_mem += UTCB_SIZE + STACK_SIZE;
 
-	start_thread(child, (L4_Word_t)start_routine, parray[index].free_mem, STACK_SIZE);
+	start_thread(child, (L4_Word_t)start_routine, *parray[index].free_mem, STACK_SIZE);
 
 	/* TODO: Error tag return */
 	return 0;
diff --git a/user/root_thread.c b/user/root_thread.c
index d63d8210..320e2335 100644
--- a/user/root_thread.c
+++ b/user/root_thread.c
@@ -78,7 +78,7 @@ static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
 
 #define STACK_SIZE 0x200
 
-extern void _pthread_create(L4_ThreadId_t tid, L4_Word_t free_mem);
+extern void _pthread_create(L4_ThreadId_t tid, L4_Word_t *free_mem);
 
 void __USER_TEXT __root_thread(kip_t *kip_ptr, utcb_t *utcb_ptr)
 {
@@ -116,7 +116,7 @@ void __USER_TEXT __root_thread(kip_t *kip_ptr, utcb_t *utcb_ptr)
 			free_mem += fpage->size;
 			fpage++;
 		}
-		_pthread_create(tid, ptr->fpages[0].base);
+		_pthread_create(tid, &ptr->fpages[0].base);
 
 		/* start thread */
 		start_thread(tid, (L4_Word_t)ptr->entry, stack, STACK_SIZE);

From 853a37c8329d89d54daa3471d6b0696fb71324c2 Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sat, 9 Aug 2014 15:52:31 +0800
Subject: [PATCH 08/18] Maintain child thread structure in the list.

---
 user/apps/posixtest/main.c  | 42 +++++++++++++++++++++++++++++++------
 user/lib/libposix/pthread.c |  6 ++++++
 2 files changed, 42 insertions(+), 6 deletions(-)

diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
index 774cc9a5..465913f9 100644
--- a/user/apps/posixtest/main.c
+++ b/user/apps/posixtest/main.c
@@ -9,9 +9,41 @@
 
 #define STACK_SIZE 256
 
-__USER_TEXT void *child_thread(void *args)
+__USER_TEXT void *child_thread1_1(void *args)
 {
-	printf("child task start\n");
+	printf("child task 1-1 start\n");
+
+	while(1)
+		L4_Sleep(L4_Never);
+
+	return 0;
+}
+
+__USER_TEXT void *child_thread1(void *args)
+{
+	printf("child task 1 start\n");
+	pthread_create(NULL, NULL, child_thread1_1, NULL);
+
+	while(1)
+		L4_Sleep(L4_Never);
+
+	return 0;
+}
+
+__USER_TEXT void *child_thread2_1(void *args)
+{
+	printf("child task 2-1 start\n");
+
+	while(1)
+		L4_Sleep(L4_Never);
+
+	return 0;
+}
+
+__USER_TEXT void *child_thread2(void *args)
+{
+	printf("child task 2 start\n");
+	pthread_create(NULL, NULL, child_thread2_1, NULL);
 
 	while(1)
 		L4_Sleep(L4_Never);
@@ -22,10 +54,8 @@ __USER_TEXT void *child_thread(void *args)
 static __USER_TEXT void main(user_struct *user)
 {
 	printf("\nPosix Layer test starts\n");
-	L4_ThreadId_t pager = L4_Pager();
-	printf("Pager = %x\n", pager);
-	pthread_create(NULL, NULL, child_thread, NULL);
-	pthread_create(NULL, NULL, child_thread, NULL);
+	pthread_create(NULL, NULL, child_thread1, NULL);
+	pthread_create(NULL, NULL, child_thread2, NULL);
 
 	while(1)
 		L4_Sleep(L4_Never);
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index 882ef89e..5baa8988 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -59,6 +59,12 @@ __USER_TEXT int pthread_create(pthread_t *restrict thread,
 
 	child.raw = myself.raw + (++parray[index].last_thread << 14);
 
+	/* Add new pthread */
+	parray[current_thread].tid = child;
+	parray[current_thread].free_mem = parray[index].free_mem;
+	parray[current_thread].last_thread = 0;
+	current_thread++;
+
 	/* TODO: Handle attr */
 	if(!attr) {
 	}

From c2a58c1c4c4af2c0a5976a9fc217e6e4c97c9f9e Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sat, 9 Aug 2014 16:20:54 +0800
Subject: [PATCH 09/18] Add pthread_mutex_trylock and unlock.

---
 user/apps/posixtest/main.c  | 42 ++++++++++++++++---------------------
 user/lib/libposix/pthread.c | 21 ++++++++++++++++++-
 2 files changed, 38 insertions(+), 25 deletions(-)

diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
index 465913f9..d493cdee 100644
--- a/user/apps/posixtest/main.c
+++ b/user/apps/posixtest/main.c
@@ -9,33 +9,22 @@
 
 #define STACK_SIZE 256
 
-__USER_TEXT void *child_thread1_1(void *args)
-{
-	printf("child task 1-1 start\n");
+__USER_DATA pthread_mutex_t mutex;
 
-	while(1)
-		L4_Sleep(L4_Never);
-
-	return 0;
-}
+__USER_DATA int shared = 0;
 
 __USER_TEXT void *child_thread1(void *args)
 {
 	printf("child task 1 start\n");
-	pthread_create(NULL, NULL, child_thread1_1, NULL);
-
-	while(1)
-		L4_Sleep(L4_Never);
 
-	return 0;
-}
-
-__USER_TEXT void *child_thread2_1(void *args)
-{
-	printf("child task 2-1 start\n");
-
-	while(1)
-		L4_Sleep(L4_Never);
+	while(1) {
+		pthread_mutex_trylock(&mutex);
+		shared = shared + 1;
+		L4_Sleep(L4_TimePeriod(1000));
+		printf("task 1: %d\n", shared);
+		pthread_mutex_unlock(&mutex);
+		L4_Sleep(L4_TimePeriod(1000));
+	}
 
 	return 0;
 }
@@ -43,10 +32,14 @@ __USER_TEXT void *child_thread2_1(void *args)
 __USER_TEXT void *child_thread2(void *args)
 {
 	printf("child task 2 start\n");
-	pthread_create(NULL, NULL, child_thread2_1, NULL);
 
-	while(1)
-		L4_Sleep(L4_Never);
+	while(1) {
+		pthread_mutex_trylock(&mutex);
+		shared = shared + 1;
+		printf("task 2: %d\n", shared);
+		pthread_mutex_unlock(&mutex);
+		L4_Sleep(L4_TimePeriod(1000));
+	}
 
 	return 0;
 }
@@ -54,6 +47,7 @@ __USER_TEXT void *child_thread2(void *args)
 static __USER_TEXT void main(user_struct *user)
 {
 	printf("\nPosix Layer test starts\n");
+	mutex = 0;
 	pthread_create(NULL, NULL, child_thread1, NULL);
 	pthread_create(NULL, NULL, child_thread2, NULL);
 
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index 5baa8988..cead8fa6 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -110,11 +110,30 @@ __USER_TEXT int pthread_mutex_lock(pthread_mutex_t *mutex)
 
 __USER_TEXT int pthread_mutex_trylock(pthread_mutex_t *mutex)
 {
-	return 0;
+	register int result = 1;
+
+	/* Busy trying */
+	while(result) {
+		__asm__ __volatile__(
+		    "mov r1, #1\n"
+		    "mov r2, %[mutex]\n"
+		    "ldrex r0, [r2]\n"	/* Load value [r2] */
+		    "cmp r0, #0\n"	/* Checking is word set to 1 */
+
+		    "itt eq\n"
+		    "strexeq r0, r1, [r2]\n"
+		    "moveq %[result], r0\n"
+		    : [result] "=r"(result)
+		    : [mutex] "r"(mutex)
+		    : "r0", "r1", "r2");
+	}
+
+	return result == 0;
 }
 
 __USER_TEXT int pthread_mutex_unlock(pthread_mutex_t *mutex)
 {
+	*mutex = 0;
 	return 0;
 }
 

From 010ef4704cd789ee706e987269052602318bf6c5 Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sat, 9 Aug 2014 19:44:19 +0800
Subject: [PATCH 10/18] Add pthread_mutex_init prototype

---
 user/include/libposix/pthread.h   | 2 ++
 user/include/libposix/sys/types.h | 1 +
 user/lib/libposix/pthread.c       | 6 ++++++
 3 files changed, 9 insertions(+)

diff --git a/user/include/libposix/pthread.h b/user/include/libposix/pthread.h
index ef86ca38..40eb96ff 100644
--- a/user/include/libposix/pthread.h
+++ b/user/include/libposix/pthread.h
@@ -9,6 +9,8 @@ int pthread_create(pthread_t *restrict thread,
 int pthread_detach(pthread_t thread);
 void pthread_exit(void *value_ptr);
 int pthread_join(pthread_t thread, void **value_ptr);
+int pthread_mutex_init(pthread_mutex_t *mutex,
+                       const pthread_mutexattr_t *restrict attr);
 int pthread_mutex_destroy(pthread_mutex_t *mutex);
 int pthread_mutex_lock(pthread_mutex_t *mutex);
 int pthread_mutex_trylock(pthread_mutex_t *mutex);
diff --git a/user/include/libposix/sys/types.h b/user/include/libposix/sys/types.h
index c75e7369..7fa3f41d 100644
--- a/user/include/libposix/sys/types.h
+++ b/user/include/libposix/sys/types.h
@@ -10,6 +10,7 @@ struct timespec {
 
 /* FIXME: Define proper type for pthread type */
 typedef uint32_t pthread_mutex_t;
+typedef uint32_t pthread_mutexattr_t;
 typedef uint32_t pthread_t;
 typedef uint32_t pthread_attr_t;
 
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index cead8fa6..2a43e88a 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -98,6 +98,12 @@ __USER_TEXT int pthread_join(pthread_t thread, void **value_ptr)
 	return 0;
 }
 
+__USER_TEXT int pthread_mutex_init(pthread_mutex_t *mutex,
+		                   const pthread_mutexattr_t *restrict attr)
+{
+	return 0;
+}
+
 __USER_TEXT int pthread_mutex_destroy(pthread_mutex_t *mutex)
 {
 	return 0;

From 586f1791cc79493bfc1c0f457c5f22cd345af61e Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sat, 9 Aug 2014 19:52:37 +0800
Subject: [PATCH 11/18] Fix trylock, it should be non-blocking. Implement lock.

---
 user/lib/libposix/pthread.c | 25 ++++++++++++++++++++-----
 1 file changed, 20 insertions(+), 5 deletions(-)

diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index 2a43e88a..c01b0067 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -110,11 +110,6 @@ __USER_TEXT int pthread_mutex_destroy(pthread_mutex_t *mutex)
 }
 
 __USER_TEXT int pthread_mutex_lock(pthread_mutex_t *mutex)
-{
-	return 0;
-}
-
-__USER_TEXT int pthread_mutex_trylock(pthread_mutex_t *mutex)
 {
 	register int result = 1;
 
@@ -137,6 +132,26 @@ __USER_TEXT int pthread_mutex_trylock(pthread_mutex_t *mutex)
 	return result == 0;
 }
 
+__USER_TEXT int pthread_mutex_trylock(pthread_mutex_t *mutex)
+{
+	register int result = 1;
+
+	__asm__ __volatile__(
+	    "mov r1, #1\n"
+	    "mov r2, %[mutex]\n"
+	    "ldrex r0, [r2]\n"	/* Load value [r2] */
+	    "cmp r0, #0\n"	/* Checking is word set to 1 */
+
+	    "itt eq\n"
+	    "strexeq r0, r1, [r2]\n"
+	    "moveq %[result], r0\n"
+	    : [result] "=r"(result)
+	    : [mutex] "r"(mutex)
+	    : "r0", "r1", "r2");
+
+	return result == 0;
+}
+
 __USER_TEXT int pthread_mutex_unlock(pthread_mutex_t *mutex)
 {
 	*mutex = 0;

From 591fc7f314303a90ae1eef89eac7c3226dbc1281 Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sat, 9 Aug 2014 23:04:54 +0800
Subject: [PATCH 12/18] Remove completed todo item, update new XXX label.

---
 user/lib/libposix/pthread.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index c01b0067..78a43839 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -72,8 +72,8 @@ __USER_TEXT int pthread_create(pthread_t *restrict thread,
 	/* TODO: pthread_sigmask, sigpending, sigaltstack */
 	/* TODO: fenv */
 	/* TODO: Initilize cpu clock to 0(pthread_getcpuclockid) */
-	/* TODO: Update thread structure, fill in new thread id */
 
+	// XXX: L4_ThreadControl may fail but can not check it here.
 	L4_ThreadControl(child, myself, L4_nilthread, myself, (void *)*parray[index].free_mem);
 	*parray[index].free_mem += UTCB_SIZE + STACK_SIZE;
 

From 8625aa24c026074b4c3880b64b8c7a7c0e8aa7fa Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Thu, 14 Aug 2014 21:24:52 +0800
Subject: [PATCH 13/18] reuse trylock in lock function, modify return value.

---
 user/lib/libposix/pthread.c | 21 +++------------------
 1 file changed, 3 insertions(+), 18 deletions(-)

diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index 78a43839..fe5c46cf 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -111,25 +111,10 @@ __USER_TEXT int pthread_mutex_destroy(pthread_mutex_t *mutex)
 
 __USER_TEXT int pthread_mutex_lock(pthread_mutex_t *mutex)
 {
-	register int result = 1;
-
 	/* Busy trying */
-	while(result) {
-		__asm__ __volatile__(
-		    "mov r1, #1\n"
-		    "mov r2, %[mutex]\n"
-		    "ldrex r0, [r2]\n"	/* Load value [r2] */
-		    "cmp r0, #0\n"	/* Checking is word set to 1 */
-
-		    "itt eq\n"
-		    "strexeq r0, r1, [r2]\n"
-		    "moveq %[result], r0\n"
-		    : [result] "=r"(result)
-		    : [mutex] "r"(mutex)
-		    : "r0", "r1", "r2");
-	}
+	while(pthread_mutex_trylock(mutex));
 
-	return result == 0;
+	return 0;
 }
 
 __USER_TEXT int pthread_mutex_trylock(pthread_mutex_t *mutex)
@@ -149,7 +134,7 @@ __USER_TEXT int pthread_mutex_trylock(pthread_mutex_t *mutex)
 	    : [mutex] "r"(mutex)
 	    : "r0", "r1", "r2");
 
-	return result == 0;
+	return result;
 }
 
 __USER_TEXT int pthread_mutex_unlock(pthread_mutex_t *mutex)

From ddccc2b3396eb01cda517c8e0b0eaaf1c432e0a0 Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sat, 16 Aug 2014 23:37:25 +0800
Subject: [PATCH 14/18] Restructure pthread tree

---
 user/apps/posixtest/main.c  |  21 ++--
 user/lib/libposix/pthread.c | 204 +++++++++++++++++++++++++++++++-----
 user/root_thread.c          |   6 +-
 3 files changed, 186 insertions(+), 45 deletions(-)

diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
index d493cdee..e4213d1c 100644
--- a/user/apps/posixtest/main.c
+++ b/user/apps/posixtest/main.c
@@ -13,32 +13,26 @@ __USER_DATA pthread_mutex_t mutex;
 
 __USER_DATA int shared = 0;
 
-__USER_TEXT void *child_thread1(void *args)
+__USER_TEXT void *child_thread2(void *args)
 {
-	printf("child task 1 start\n");
+	printf("child task 2 start\n");
 
 	while(1) {
-		pthread_mutex_trylock(&mutex);
 		shared = shared + 1;
-		L4_Sleep(L4_TimePeriod(1000));
-		printf("task 1: %d\n", shared);
-		pthread_mutex_unlock(&mutex);
+		printf("task 2: %d\n", shared);
 		L4_Sleep(L4_TimePeriod(1000));
 	}
 
 	return 0;
 }
 
-__USER_TEXT void *child_thread2(void *args)
+__USER_TEXT void *child_thread1(void *args)
 {
-	printf("child task 2 start\n");
+	printf("child task 1 start\n");
+	pthread_create(NULL, NULL, child_thread2, NULL);
 
 	while(1) {
-		pthread_mutex_trylock(&mutex);
-		shared = shared + 1;
-		printf("task 2: %d\n", shared);
-		pthread_mutex_unlock(&mutex);
-		L4_Sleep(L4_TimePeriod(1000));
+		L4_Sleep(L4_Never);
 	}
 
 	return 0;
@@ -49,7 +43,6 @@ static __USER_TEXT void main(user_struct *user)
 	printf("\nPosix Layer test starts\n");
 	mutex = 0;
 	pthread_create(NULL, NULL, child_thread1, NULL);
-	pthread_create(NULL, NULL, child_thread2, NULL);
 
 	while(1)
 		L4_Sleep(L4_Never);
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index fe5c46cf..81d7a314 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -7,14 +7,46 @@
 #define STACK_SIZE 256
 #define MAX_THREAD 128
 
-struct pthread_i {
+#define ROOT_THREAD 	1
+#define USER_ROOT	2
+#define USER_THREAD	3
+
+/* pthread tree architecture, e.g
+ *   +------+
+ *   | Root |
+ *   +------+
+ *     |  ^
+ *     v  |
+ *   +-------+    +-------+    +-------+
+ *   | User1 |<-->| User2 |<-->| User3 |
+ *   +-------+    +-------+    +-------+
+ *     |  ^                      | ^
+ *     v  |                      v |
+ *   +--------+    +--------+  +--------+
+ *   | Child1 |<-->| Child2 |  | Child1 +
+ *   +--------+    +--------+  +--------+
+ *
+ * Every user will have a free_mem address.
+ * When nested pthread_create() occurs, they will use
+ * the free_mem address in their user's free_mem.
+ */    
+typedef struct pthread_i {
 	L4_ThreadId_t tid;
-	L4_Word_t *free_mem;
-	int last_thread;
-};
+	L4_Word_t free_mem;
+	struct pthread_i *child;
+	struct pthread_i *parent;
+	struct pthread_i *next;
+	struct pthread_i *prev;
+	int index;
+	int type;
+	int count_thread;
+} pthread_i;
+
+static __USER_DATA pthread_i *root;
+static __USER_DATA pthread_i parray[MAX_THREAD];
 
-__USER_DATA struct pthread_i parray[MAX_THREAD];
-__USER_DATA int current_thread = 0;
+// TODO: use bitmap to trace allocate state
+__USER_DATA uint8_t flag[MAX_THREAD];
 
 static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
                                      L4_Word_t sp, L4_Word_t stack_size)
@@ -30,40 +62,154 @@ static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
 	L4_Send(t);
 }
 
+__USER_TEXT static void _pthread_dump(void)
+{
+	int i;
+
+	for(i = 0; i < MAX_THREAD; i++) {
+		if(flag[i]) {
+			printf("%d: %#x, parent:%p, child:%p, next:%p, prev:%p\n"
+			       , i, parray[i].tid.raw, parray[i].parent,
+			       parray[i].child, parray[i].next, parray[i].prev);
+		}
+	}
+}
 
-__USER_TEXT void _pthread_create(L4_ThreadId_t tid, L4_Word_t *free_mem)
+void __USER_TEXT _pthread_init(void)
 {
-	parray[current_thread].tid = tid;
-	parray[current_thread].free_mem = free_mem;
-	parray[current_thread].last_thread = 0;
-	current_thread++;
+	int i;
+
+	for(i = 1; i < MAX_THREAD; i++) {
+		flag[i] = 0;
+		parray[i].index = i;
+	}
+
+	flag[0] = 1;
+	root = &parray[0];
+	root->tid.raw = 0x8000; // ref to include/thread.h
+	root->child = NULL;
+	root->parent = NULL;
+	root->next = NULL;
+	root->prev = NULL;
+	root->index = 0;
+	root->type = ROOT_THREAD;
+	root->count_thread = 0;
 }
 
-__USER_TEXT int pthread_create(pthread_t *restrict thread,
-                               const pthread_attr_t *restrict attr,
-                               void *(*start_routine)(void*), void *restrict arg)
+__USER_TEXT static int _get_free_index(void)
 {
-	int index;
+	int i;
+
+	for(i = 1; i < MAX_THREAD; i++) {
+		if(!flag[i]) {
+			flag[i] = 1;
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+__USER_TEXT static pthread_i *_find_user_root(void)
+{
+	int i;
+	L4_ThreadId_t myself = L4_MyGlobalId();
+
+	for(i = 0; i < MAX_THREAD; i++) {
+		if(flag[i] && parray[i].tid.raw == myself.raw) {
+			break;
+		}
+	}
+
+	if(parray[i].type == USER_ROOT) {
+		return &parray[i];
+	}
+	else if(parray[i].type == USER_THREAD) {
+		pthread_i *it;
+		for(it = &parray[i]; it->type != USER_ROOT; it = it->parent);
+		return it;
+	}
+
+	/* bug reach here */
+	printf("_find_user_root bug\n");
+
+	return NULL;
+}
+
+/*
+__USER_TEXT static void _pthread_remove(pthread_i *node)
+{
+
+}
+*/
+
+__USER_TEXT static void _pthread_insert(pthread_i *child)
+{
+	int i;
 	L4_ThreadId_t myself = L4_MyGlobalId();
-	L4_ThreadId_t child;
 
-	for(index = 0; index < current_thread; index++) {
-		if(parray[index].tid.raw == myself.raw) {
+	for(i = 0; i < MAX_THREAD; i++) {
+		if(flag[i] && parray[i].tid.raw == myself.raw) {
 			break;
 		}
 
-		if(index == current_thread - 1) {
-			return -1;
+		if(i == MAX_THREAD - 1) {
+			/* bug reach here */
+			printf("pthread: pthread_insert bug\n");
+			return;
 		}
 	}
 
-	child.raw = myself.raw + (++parray[index].last_thread << 14);
+	if(parray[i].child == NULL) {
+		parray[i].child = child;
+		child->parent = &parray[i];
+	}
+	else {
+		pthread_i *it;
+
+		for(it = parray[i].child; it->next != NULL; it = it->next);
+		it->next = child;
+		child->prev = it;
+	}
+
+	_pthread_dump();
+}
+
+void __USER_TEXT _pthread_add_user(L4_ThreadId_t tid, L4_Word_t free_mem)
+{
+	int index = _get_free_index();
+
+	parray[index].tid.raw = tid.raw;
+	parray[index].free_mem = free_mem;
+	parray[index].child = NULL;
+	parray[index].parent = NULL;
+	parray[index].next = NULL;
+	parray[index].prev = NULL;
+	parray[index].index = index;
+	parray[index].type = USER_ROOT;
+	parray[index].count_thread = 0;
+
+	_pthread_insert(&parray[index]);
+}
+
+int __USER_TEXT pthread_create(pthread_t *restrict thread,
+                               const pthread_attr_t *restrict attr,
+                               void *(*start_routine)(void*), void *restrict arg)
+{
+	int index = _get_free_index();
+	L4_ThreadId_t myself = L4_MyGlobalId();
+	L4_ThreadId_t child;
+	pthread_i *user_root = _find_user_root();
+
+	child.raw = myself.raw + (++parray[index].count_thread << 14);
+
+	/* Init new pthread_i node */
+	parray[index].tid = child;
+	parray[index].index = index;
+	parray[index].type = USER_THREAD;
+	parray[index].count_thread = 0;
 
-	/* Add new pthread */
-	parray[current_thread].tid = child;
-	parray[current_thread].free_mem = parray[index].free_mem;
-	parray[current_thread].last_thread = 0;
-	current_thread++;
+	_pthread_insert(&parray[index]);
 
 	/* TODO: Handle attr */
 	if(!attr) {
@@ -74,10 +220,10 @@ __USER_TEXT int pthread_create(pthread_t *restrict thread,
 	/* TODO: Initilize cpu clock to 0(pthread_getcpuclockid) */
 
 	// XXX: L4_ThreadControl may fail but can not check it here.
-	L4_ThreadControl(child, myself, L4_nilthread, myself, (void *)*parray[index].free_mem);
-	*parray[index].free_mem += UTCB_SIZE + STACK_SIZE;
+	L4_ThreadControl(child, myself, L4_nilthread, myself, (void *)user_root->free_mem);
+	user_root->free_mem += UTCB_SIZE + STACK_SIZE;
 
-	start_thread(child, (L4_Word_t)start_routine, *parray[index].free_mem, STACK_SIZE);
+	start_thread(child, (L4_Word_t)start_routine, user_root->free_mem, STACK_SIZE);
 
 	/* TODO: Error tag return */
 	return 0;
diff --git a/user/root_thread.c b/user/root_thread.c
index 320e2335..aec7cb3b 100644
--- a/user/root_thread.c
+++ b/user/root_thread.c
@@ -78,13 +78,15 @@ static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
 
 #define STACK_SIZE 0x200
 
-extern void _pthread_create(L4_ThreadId_t tid, L4_Word_t *free_mem);
+extern void _pthread_init(void);
+extern void _pthread_add_user(L4_ThreadId_t tid, L4_Word_t free_mem);
 
 void __USER_TEXT __root_thread(kip_t *kip_ptr, utcb_t *utcb_ptr)
 {
 	L4_ThreadId_t myself = {.raw = utcb_ptr->t_globalid};
 	char *free_mem = (char *) get_free_base(kip_ptr);
 
+	_pthread_init();
 	for (user_struct *ptr = user_runtime_start; ptr != user_runtime_end; ++ptr) {
 		L4_ThreadId_t tid;
 		L4_Word_t stack;
@@ -116,7 +118,7 @@ void __USER_TEXT __root_thread(kip_t *kip_ptr, utcb_t *utcb_ptr)
 			free_mem += fpage->size;
 			fpage++;
 		}
-		_pthread_create(tid, &ptr->fpages[0].base);
+		_pthread_add_user(tid, ptr->fpages[0].base);
 
 		/* start thread */
 		start_thread(tid, (L4_Word_t)ptr->entry, stack, STACK_SIZE);

From 28b96fdaed8f11404fd7c26ab3a04983f9489aae Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Sun, 17 Aug 2014 23:47:16 +0800
Subject: [PATCH 15/18] Add pthread_exit. But something need to be fixed.

---
 user/apps/posixtest/main.c  |  8 ++++++++
 user/lib/libposix/pthread.c | 38 ++++++++++++++++++++++++++++++++++---
 2 files changed, 43 insertions(+), 3 deletions(-)

diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
index e4213d1c..c24510bf 100644
--- a/user/apps/posixtest/main.c
+++ b/user/apps/posixtest/main.c
@@ -31,6 +31,14 @@ __USER_TEXT void *child_thread1(void *args)
 	printf("child task 1 start\n");
 	pthread_create(NULL, NULL, child_thread2, NULL);
 
+	for(int i = 0; i <= 10; i++) {
+		printf("%d\n", 10 - i);
+		L4_Sleep(L4_TimePeriod(500));
+	}
+
+	printf("suicide\n");
+	pthread_exit(0);
+
 	while(1) {
 		L4_Sleep(L4_Never);
 	}
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index 81d7a314..89a8efbb 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -136,12 +136,36 @@ __USER_TEXT static pthread_i *_find_user_root(void)
 	return NULL;
 }
 
-/*
-__USER_TEXT static void _pthread_remove(pthread_i *node)
+__USER_TEXT static void _pthread_remove(L4_ThreadId_t myself)
 {
+	int i;
+	pthread_i *node = NULL;
+
+	for(i = 0; i < MAX_THREAD; i++) {
+		if(flag[i] && parray[i].tid.raw == myself.raw) {
+			node = &parray[i];
+			break;
+		}
+	}
+
+	if(node->child != NULL) {
+		_pthread_remove(node->child->tid);
+	}
 
+	if(node->parent->child == node && node->next != NULL) {
+		if(node->next != NULL) {
+			node->parent->child = node->next;
+			node->next->prev = NULL;
+		}
+		else {
+			node->parent->child = NULL;
+		}
+	}
+
+	flag[i] = 0;
+
+	_pthread_dump();
 }
-*/
 
 __USER_TEXT static void _pthread_insert(pthread_i *child)
 {
@@ -236,7 +260,15 @@ __USER_TEXT int pthread_detach(pthread_t thread)
 
 __USER_TEXT void pthread_exit(void *value_ptr) 
 {
+	L4_ThreadId_t myself = L4_MyGlobalId();
+
+	_pthread_remove(myself);
 
+	// FIXME:
+	//   1. L4_ThreadControl has bug, it won't terminate task. It just removes
+	//      the task from task list.
+	//   2. We should traverse the pthread list and kill the child task
+	L4_ThreadControl(myself, L4_nilthread, L4_nilthread, myself, NULL);
 }
 
 __USER_TEXT int pthread_join(pthread_t thread, void **value_ptr)

From 441da6cb621fe728d7e27a2328d357a5cd9fe289 Mon Sep 17 00:00:00 2001
From: Rampant <rampant1018@gmail.com>
Date: Fri, 22 Aug 2014 00:07:07 +0800
Subject: [PATCH 16/18] fix L4_ThreadControl suicide bug.

---
 user/apps/posixtest/main.c  |  7 ++++++-
 user/lib/libposix/pthread.c | 19 +++++++++++++------
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
index c24510bf..9924a3eb 100644
--- a/user/apps/posixtest/main.c
+++ b/user/apps/posixtest/main.c
@@ -36,9 +36,14 @@ __USER_TEXT void *child_thread1(void *args)
 		L4_Sleep(L4_TimePeriod(500));
 	}
 
-	printf("suicide\n");
+	printf("task 1: suicide\n");
 	pthread_exit(0);
 
+	for(int i = 0; i <= 10; i++) {
+		printf("%d\n", 10 - i);
+		L4_Sleep(L4_TimePeriod(500));
+	}
+
 	while(1) {
 		L4_Sleep(L4_Never);
 	}
diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index 89a8efbb..69c28077 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -148,7 +148,11 @@ __USER_TEXT static void _pthread_remove(L4_ThreadId_t myself)
 		}
 	}
 
+
 	if(node->child != NULL) {
+		while(node->child->next != NULL) {
+			_pthread_remove(node->child->next->tid);
+		}
 		_pthread_remove(node->child->tid);
 	}
 
@@ -165,6 +169,15 @@ __USER_TEXT static void _pthread_remove(L4_ThreadId_t myself)
 	flag[i] = 0;
 
 	_pthread_dump();
+
+	// FIXME:
+	//   L4_ThreadControl maybe has bug. It can not kill thread otherwise 
+	//   current thread.
+	//   Ex. Assume t1 has child t2 and then invoke pthread_exit(0) in t1.
+	//   t1 will be terminated(another issue here) and t2 will keep running.
+	//   However t1 and t2 in thread list in KDB are remove both.
+	L4_ThreadControl(myself, L4_nilthread, L4_nilthread,
+	                 L4_nilthread, (void *) - 1);
 }
 
 __USER_TEXT static void _pthread_insert(pthread_i *child)
@@ -263,12 +276,6 @@ __USER_TEXT void pthread_exit(void *value_ptr)
 	L4_ThreadId_t myself = L4_MyGlobalId();
 
 	_pthread_remove(myself);
-
-	// FIXME:
-	//   1. L4_ThreadControl has bug, it won't terminate task. It just removes
-	//      the task from task list.
-	//   2. We should traverse the pthread list and kill the child task
-	L4_ThreadControl(myself, L4_nilthread, L4_nilthread, myself, NULL);
 }
 
 __USER_TEXT int pthread_join(pthread_t thread, void **value_ptr)

From 70c90e2e8a0ee290a7a1827bc349467a21f9838b Mon Sep 17 00:00:00 2001
From: Jeff Liaw <rampant1018@gmail.com>
Date: Tue, 30 Dec 2014 15:46:52 +0800
Subject: [PATCH 17/18] Revise implementation of pthread due to pager

---
 user/lib/libposix/pthread.c | 262 +-----------------------------------
 user/root_thread.c          |   5 -
 2 files changed, 5 insertions(+), 262 deletions(-)

diff --git a/user/lib/libposix/pthread.c b/user/lib/libposix/pthread.c
index 69c28077..46a84bee 100644
--- a/user/lib/libposix/pthread.c
+++ b/user/lib/libposix/pthread.c
@@ -1,269 +1,19 @@
 #include <libposix/pthread.h>
 #include <l4/ipc.h>
 #include <l4/utcb.h>
+#include <l4/pager.h>
 #include <l4io.h>
 #include <platform/link.h>
 
-#define STACK_SIZE 256
-#define MAX_THREAD 128
-
-#define ROOT_THREAD 	1
-#define USER_ROOT	2
-#define USER_THREAD	3
-
-/* pthread tree architecture, e.g
- *   +------+
- *   | Root |
- *   +------+
- *     |  ^
- *     v  |
- *   +-------+    +-------+    +-------+
- *   | User1 |<-->| User2 |<-->| User3 |
- *   +-------+    +-------+    +-------+
- *     |  ^                      | ^
- *     v  |                      v |
- *   +--------+    +--------+  +--------+
- *   | Child1 |<-->| Child2 |  | Child1 +
- *   +--------+    +--------+  +--------+
- *
- * Every user will have a free_mem address.
- * When nested pthread_create() occurs, they will use
- * the free_mem address in their user's free_mem.
- */    
-typedef struct pthread_i {
-	L4_ThreadId_t tid;
-	L4_Word_t free_mem;
-	struct pthread_i *child;
-	struct pthread_i *parent;
-	struct pthread_i *next;
-	struct pthread_i *prev;
-	int index;
-	int type;
-	int count_thread;
-} pthread_i;
-
-static __USER_DATA pthread_i *root;
-static __USER_DATA pthread_i parray[MAX_THREAD];
-
-// TODO: use bitmap to trace allocate state
-__USER_DATA uint8_t flag[MAX_THREAD];
-
-static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
-                                     L4_Word_t sp, L4_Word_t stack_size)
-{
-	L4_Msg_t msg;
-
-	L4_MsgClear(&msg);
-	L4_MsgAppendWord(&msg, ip);
-	L4_MsgAppendWord(&msg, sp);
-	L4_MsgAppendWord(&msg, stack_size);
-	L4_MsgLoad(&msg);
-
-	L4_Send(t);
-}
-
-__USER_TEXT static void _pthread_dump(void)
-{
-	int i;
-
-	for(i = 0; i < MAX_THREAD; i++) {
-		if(flag[i]) {
-			printf("%d: %#x, parent:%p, child:%p, next:%p, prev:%p\n"
-			       , i, parray[i].tid.raw, parray[i].parent,
-			       parray[i].child, parray[i].next, parray[i].prev);
-		}
-	}
-}
-
-void __USER_TEXT _pthread_init(void)
-{
-	int i;
-
-	for(i = 1; i < MAX_THREAD; i++) {
-		flag[i] = 0;
-		parray[i].index = i;
-	}
-
-	flag[0] = 1;
-	root = &parray[0];
-	root->tid.raw = 0x8000; // ref to include/thread.h
-	root->child = NULL;
-	root->parent = NULL;
-	root->next = NULL;
-	root->prev = NULL;
-	root->index = 0;
-	root->type = ROOT_THREAD;
-	root->count_thread = 0;
-}
-
-__USER_TEXT static int _get_free_index(void)
-{
-	int i;
-
-	for(i = 1; i < MAX_THREAD; i++) {
-		if(!flag[i]) {
-			flag[i] = 1;
-			return i;
-		}
-	}
-
-	return -1;
-}
-
-__USER_TEXT static pthread_i *_find_user_root(void)
-{
-	int i;
-	L4_ThreadId_t myself = L4_MyGlobalId();
-
-	for(i = 0; i < MAX_THREAD; i++) {
-		if(flag[i] && parray[i].tid.raw == myself.raw) {
-			break;
-		}
-	}
-
-	if(parray[i].type == USER_ROOT) {
-		return &parray[i];
-	}
-	else if(parray[i].type == USER_THREAD) {
-		pthread_i *it;
-		for(it = &parray[i]; it->type != USER_ROOT; it = it->parent);
-		return it;
-	}
-
-	/* bug reach here */
-	printf("_find_user_root bug\n");
-
-	return NULL;
-}
-
-__USER_TEXT static void _pthread_remove(L4_ThreadId_t myself)
-{
-	int i;
-	pthread_i *node = NULL;
-
-	for(i = 0; i < MAX_THREAD; i++) {
-		if(flag[i] && parray[i].tid.raw == myself.raw) {
-			node = &parray[i];
-			break;
-		}
-	}
-
-
-	if(node->child != NULL) {
-		while(node->child->next != NULL) {
-			_pthread_remove(node->child->next->tid);
-		}
-		_pthread_remove(node->child->tid);
-	}
-
-	if(node->parent->child == node && node->next != NULL) {
-		if(node->next != NULL) {
-			node->parent->child = node->next;
-			node->next->prev = NULL;
-		}
-		else {
-			node->parent->child = NULL;
-		}
-	}
-
-	flag[i] = 0;
-
-	_pthread_dump();
-
-	// FIXME:
-	//   L4_ThreadControl maybe has bug. It can not kill thread otherwise 
-	//   current thread.
-	//   Ex. Assume t1 has child t2 and then invoke pthread_exit(0) in t1.
-	//   t1 will be terminated(another issue here) and t2 will keep running.
-	//   However t1 and t2 in thread list in KDB are remove both.
-	L4_ThreadControl(myself, L4_nilthread, L4_nilthread,
-	                 L4_nilthread, (void *) - 1);
-}
-
-__USER_TEXT static void _pthread_insert(pthread_i *child)
-{
-	int i;
-	L4_ThreadId_t myself = L4_MyGlobalId();
-
-	for(i = 0; i < MAX_THREAD; i++) {
-		if(flag[i] && parray[i].tid.raw == myself.raw) {
-			break;
-		}
-
-		if(i == MAX_THREAD - 1) {
-			/* bug reach here */
-			printf("pthread: pthread_insert bug\n");
-			return;
-		}
-	}
-
-	if(parray[i].child == NULL) {
-		parray[i].child = child;
-		child->parent = &parray[i];
-	}
-	else {
-		pthread_i *it;
-
-		for(it = parray[i].child; it->next != NULL; it = it->next);
-		it->next = child;
-		child->prev = it;
-	}
-
-	_pthread_dump();
-}
-
-void __USER_TEXT _pthread_add_user(L4_ThreadId_t tid, L4_Word_t free_mem)
-{
-	int index = _get_free_index();
-
-	parray[index].tid.raw = tid.raw;
-	parray[index].free_mem = free_mem;
-	parray[index].child = NULL;
-	parray[index].parent = NULL;
-	parray[index].next = NULL;
-	parray[index].prev = NULL;
-	parray[index].index = index;
-	parray[index].type = USER_ROOT;
-	parray[index].count_thread = 0;
-
-	_pthread_insert(&parray[index]);
-}
-
 int __USER_TEXT pthread_create(pthread_t *restrict thread,
                                const pthread_attr_t *restrict attr,
                                void *(*start_routine)(void*), void *restrict arg)
 {
-	int index = _get_free_index();
-	L4_ThreadId_t myself = L4_MyGlobalId();
-	L4_ThreadId_t child;
-	pthread_i *user_root = _find_user_root();
-
-	child.raw = myself.raw + (++parray[index].count_thread << 14);
+	L4_ThreadId_t tid = pager_create_thread();
 
-	/* Init new pthread_i node */
-	parray[index].tid = child;
-	parray[index].index = index;
-	parray[index].type = USER_THREAD;
-	parray[index].count_thread = 0;
+	pager_start_thread(tid, start_routine, NULL);
 
-	_pthread_insert(&parray[index]);
-
-	/* TODO: Handle attr */
-	if(!attr) {
-	}
-
-	/* TODO: pthread_sigmask, sigpending, sigaltstack */
-	/* TODO: fenv */
-	/* TODO: Initilize cpu clock to 0(pthread_getcpuclockid) */
-
-	// XXX: L4_ThreadControl may fail but can not check it here.
-	L4_ThreadControl(child, myself, L4_nilthread, myself, (void *)user_root->free_mem);
-	user_root->free_mem += UTCB_SIZE + STACK_SIZE;
-
-	start_thread(child, (L4_Word_t)start_routine, user_root->free_mem, STACK_SIZE);
-
-	/* TODO: Error tag return */
-	return 0;
+	return *(int *)&tid;
 }
 
 __USER_TEXT int pthread_detach(pthread_t thread)
@@ -273,9 +23,7 @@ __USER_TEXT int pthread_detach(pthread_t thread)
 
 __USER_TEXT void pthread_exit(void *value_ptr) 
 {
-	L4_ThreadId_t myself = L4_MyGlobalId();
-
-	_pthread_remove(myself);
+	return;
 }
 
 __USER_TEXT int pthread_join(pthread_t thread, void **value_ptr)
diff --git a/user/root_thread.c b/user/root_thread.c
index aec7cb3b..ed7bf086 100644
--- a/user/root_thread.c
+++ b/user/root_thread.c
@@ -78,15 +78,11 @@ static void __USER_TEXT start_thread(L4_ThreadId_t t, L4_Word_t ip,
 
 #define STACK_SIZE 0x200
 
-extern void _pthread_init(void);
-extern void _pthread_add_user(L4_ThreadId_t tid, L4_Word_t free_mem);
-
 void __USER_TEXT __root_thread(kip_t *kip_ptr, utcb_t *utcb_ptr)
 {
 	L4_ThreadId_t myself = {.raw = utcb_ptr->t_globalid};
 	char *free_mem = (char *) get_free_base(kip_ptr);
 
-	_pthread_init();
 	for (user_struct *ptr = user_runtime_start; ptr != user_runtime_end; ++ptr) {
 		L4_ThreadId_t tid;
 		L4_Word_t stack;
@@ -118,7 +114,6 @@ void __USER_TEXT __root_thread(kip_t *kip_ptr, utcb_t *utcb_ptr)
 			free_mem += fpage->size;
 			fpage++;
 		}
-		_pthread_add_user(tid, ptr->fpages[0].base);
 
 		/* start thread */
 		start_thread(tid, (L4_Word_t)ptr->entry, stack, STACK_SIZE);

From 8588ea8b63f27354bd1276a64111c97e5a40928e Mon Sep 17 00:00:00 2001
From: Jeff Liaw <rampant1018@gmail.com>
Date: Tue, 30 Dec 2014 15:52:51 +0800
Subject: [PATCH 18/18] Ensure pthread_create works correctly.

---
 user/apps/build.mk         |  2 --
 user/apps/posixtest/main.c | 10 ++++------
 2 files changed, 4 insertions(+), 8 deletions(-)

diff --git a/user/apps/build.mk b/user/apps/build.mk
index c21d4ab4..eaee7464 100644
--- a/user/apps/build.mk
+++ b/user/apps/build.mk
@@ -3,9 +3,7 @@
 # found in the LICENSE file.
 
 user-apps-dirs = \
-	l4test \
 	posixtest \
-	pingpong \
 
 ifdef CONFIG_BOARD_STM32F429DISCOVERY
 user-apps-dirs += \
diff --git a/user/apps/posixtest/main.c b/user/apps/posixtest/main.c
index 9924a3eb..f42c6dae 100644
--- a/user/apps/posixtest/main.c
+++ b/user/apps/posixtest/main.c
@@ -7,7 +7,7 @@
 /* posix layer */
 #include <libposix/pthread.h>
 
-#define STACK_SIZE 256
+#define STACK_SIZE 512
 
 __USER_DATA pthread_mutex_t mutex;
 
@@ -36,9 +36,6 @@ __USER_TEXT void *child_thread1(void *args)
 		L4_Sleep(L4_TimePeriod(500));
 	}
 
-	printf("task 1: suicide\n");
-	pthread_exit(0);
-
 	for(int i = 0; i <= 10; i++) {
 		printf("%d\n", 10 - i);
 		L4_Sleep(L4_TimePeriod(500));
@@ -51,7 +48,7 @@ __USER_TEXT void *child_thread1(void *args)
 	return 0;
 }
 
-static __USER_TEXT void main(user_struct *user)
+static __USER_TEXT void *main(void *user)
 {
 	printf("\nPosix Layer test starts\n");
 	mutex = 0;
@@ -60,7 +57,7 @@ static __USER_TEXT void main(user_struct *user)
 	while(1)
 		L4_Sleep(L4_Never);
 
-	return;
+	return NULL;
 }
 
 DECLARE_USER(
@@ -68,4 +65,5 @@ DECLARE_USER(
 	posixtest,
 	main,
 	DECLARE_FPAGE(0x0, 4 * (UTCB_SIZE + STACK_SIZE))
+	DECLARE_FPAGE(0x0, 512)
 );