diff --git a/include/limits.h b/include/limits.h
index 2301683c..10051751 100644
--- a/include/limits.h
+++ b/include/limits.h
@@ -18,6 +18,10 @@
 
 #include <arch.h>
 
+
+#define _POSIX_THREAD_DESTRUCTOR_ITERATIONS 4
+#define PTHREAD_DESTRUCTOR_ITERATIONS       _POSIX_THREAD_DESTRUCTOR_ITERATIONS
+
 #ifdef __ARCH_LIMITS
 #include __ARCH_LIMITS
 #else
diff --git a/pthread/pthread.c b/pthread/pthread.c
index c64403bc..1abce201 100644
--- a/pthread/pthread.c
+++ b/pthread/pthread.c
@@ -5,8 +5,8 @@
  *
  * pthread
  *
- * Copyright 2017, 2019 Phoenix Systems
- * Author: Pawel Pisarczyk, Marcin Baran
+ * Copyright 2017, 2019, 2023 Phoenix Systems
+ * Author: Pawel Pisarczyk, Marcin Baran, Hubert Badocha
  *
  * This file is part of Phoenix-RTOS.
  *
@@ -22,8 +22,9 @@
 #include <pthread.h>
 #include <unistd.h>
 
-
-#define CEIL(value, size)	((((value) + (size) - 1) / (size)) * (size))
+/* clang-format off */
+#define CEIL(value, size) ((((value) + (size) - 1) / (size)) * (size))
+/* clang-format on */
 
 #define PTHREAD_ONCE_DONE          0
 #define PTHREAD_ONCE_IN_PROGRESS   2
@@ -38,12 +39,13 @@ typedef struct pthread_ctx {
 	size_t stacksize;
 	struct pthread_ctx *next;
 	struct pthread_ctx *prev;
-	int detached;
+	int is_detached;
 	int cancelstate;
 	struct __errno_t e;
 	int refcount;
 	struct pthread_key_data_t *key_data_list;
 	struct _pthread_cleanup_t *cleanup_list;
+	int exiting;
 } pthread_ctx;
 
 
@@ -65,6 +67,10 @@ static struct {
 	pthread_cond_t pthread_once_cond;
 	pthread_ctx *pthread_list;
 	pthread_fork_handlers_t *pthread_fork_handlers;
+	struct {
+		void *stack;
+		size_t stacksize;
+	} to_cleanup;
 } pthread_common;
 
 
@@ -80,13 +86,6 @@ typedef struct pthread_key_data_t {
 } pthread_key_data_t;
 
 
-typedef struct _pthread_key_cleanup_t {
-	void (*destructor)(void *);
-	void *value;
-	struct _pthread_key_cleanup_t *next;
-} _pthread_key_cleanup_t;
-
-
 typedef struct _pthread_cleanup_t {
 	void (*routine)(void *);
 	void *arg;
@@ -103,6 +102,9 @@ static const pthread_attr_t pthread_attr_default = {
 };
 
 
+static __attribute__((noreturn)) void pthread_do_exit(pthread_ctx *ctx, void *value_ptr, int cleanup);
+
+
 static void _pthread_ctx_get(pthread_ctx *ctx)
 {
 	++ctx->refcount;
@@ -135,59 +137,15 @@ static void pthread_ctx_put(pthread_ctx *ctx)
 }
 
 
-static void start_point(void *args)
+static void pthread_start_point(void *args)
 {
 	pthread_ctx *ctx = (pthread_ctx *)args;
 
 	_errno_new(&ctx->e);
 
-	ctx->retval = (void *)(ctx->start_routine(ctx->arg));
-
-	endthread();
-}
-
-
-static _pthread_key_cleanup_t *pthread_key_cleanup(pthread_ctx *ctx)
-{
-	_pthread_key_cleanup_t *head = NULL, *next;
-	pthread_key_data_t *thread_data;
-
-	mutexLock(pthread_common.pthread_key_lock);
-
-	while (ctx->key_data_list != NULL) {
-		thread_data = ctx->key_data_list;
-		ctx->key_data_list = ctx->key_data_list->next;
+	void *retval = (void *)(ctx->start_routine(ctx->arg));
 
-		next = malloc(sizeof(_pthread_key_cleanup_t));
-		if (next == NULL) {
-			mutexUnlock(pthread_common.pthread_key_lock);
-			return head;
-		}
-		next->destructor = thread_data->key->destructor;
-		next->value = thread_data->value;
-		next->next = head;
-		head = next;
-
-		free(thread_data);
-	}
-	mutexUnlock(pthread_common.pthread_key_lock);
-
-	return head;
-}
-
-
-static void pthread_full_key_cleanup(_pthread_key_cleanup_t *head)
-{
-	_pthread_key_cleanup_t *next;
-
-	while (head != NULL) {
-		next = head->next;
-		if (head->destructor != NULL) {
-			head->destructor(head->value);
-		}
-		free(head);
-		head = next;
-	}
+	pthread_do_exit(ctx, retval, 0);
 }
 
 
@@ -207,7 +165,7 @@ static void _pthread_do_cleanup(pthread_ctx *ctx)
 }
 
 
-static pthread_ctx *find_pthread(handle_t id)
+static pthread_ctx *pthread_find(handle_t id)
 {
 	mutexLock(pthread_common.pthread_list_lock);
 	pthread_ctx *ctx = pthread_common.pthread_list;
@@ -239,15 +197,14 @@ static int pthread_create_main(void)
 	ctx->retval = NULL;
 	ctx->stack = NULL;
 	ctx->stacksize = 0;
-	ctx->detached = pthread_attr_default.detached;
+	ctx->is_detached = (pthread_attr_default.detached == PTHREAD_CREATE_DETACHED) ? 1 : 0;
 	ctx->cancelstate = PTHREAD_CANCEL_ENABLE;
 	ctx->refcount = 1;
 	ctx->key_data_list = NULL;
 	ctx->cleanup_list = NULL;
+	ctx->exiting = 0;
 
-	mutexLock(pthread_common.pthread_list_lock);
 	LIST_ADD(&pthread_common.pthread_list, ctx);
-	mutexUnlock(pthread_common.pthread_list_lock);
 
 	return 0;
 }
@@ -258,14 +215,16 @@ int pthread_create(pthread_t *thread, const pthread_attr_t *attr,
 {
 	const pthread_attr_t *attrs = &pthread_attr_default;
 
-	if (attr != NULL)
+	if (attr != NULL) {
 		attrs = attr;
+	}
 
 	void *stack = mmap(attrs->stackaddr, attrs->stacksize,
 		PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS, NULL, 0);
 
-	if (stack == MAP_FAILED || stack == NULL)
+	if ((stack == MAP_FAILED) || (stack == NULL)) {
 		return EAGAIN;
+	}
 
 	pthread_ctx *ctx = (pthread_ctx *)malloc(sizeof(pthread_ctx));
 
@@ -276,7 +235,7 @@ int pthread_create(pthread_t *thread, const pthread_attr_t *attr,
 
 	ctx->refcount = 1;
 	ctx->retval = NULL;
-	ctx->detached = attrs->detached;
+	ctx->is_detached = (pthread_attr_default.detached == PTHREAD_CREATE_DETACHED) ? 1 : 0;
 	ctx->start_routine = start_routine;
 	ctx->arg = arg;
 	ctx->stack = stack;
@@ -288,21 +247,15 @@ int pthread_create(pthread_t *thread, const pthread_attr_t *attr,
 
 	mutexLock(pthread_common.pthread_list_lock);
 
-	int err = beginthreadex(start_point, attrs->priority, stack,
+	int err = beginthreadex(pthread_start_point, attrs->priority, stack,
 		attrs->stacksize, (void *)ctx, &ctx->id);
 
 	if (err != 0) {
-		munmap(stack, attrs->stacksize);
 		_pthread_ctx_put(ctx);
-		thread = NULL;
+		munmap(stack, attrs->stacksize);
 	}
 	else {
-		if (ctx->detached == PTHREAD_CREATE_JOINABLE) {
-			if (pthread_common.pthread_list != NULL && pthread_common.pthread_list->id == 0) {
-				pthread_common.pthread_list = NULL;
-			}
-			LIST_ADD(&pthread_common.pthread_list, ctx);
-		}
+		LIST_ADD(&pthread_common.pthread_list, ctx);
 		mutexUnlock(pthread_common.pthread_list_lock);
 	}
 
@@ -310,10 +263,47 @@ int pthread_create(pthread_t *thread, const pthread_attr_t *attr,
 }
 
 
+static void _pthread_release(pthread_ctx *ctx, int self)
+{
+	void *prev_stack = pthread_common.to_cleanup.stack;
+	size_t prev_stacksize = pthread_common.to_cleanup.stacksize;
+	void *stack = ctx->stack;
+	size_t stacksize = ctx->stacksize;
+
+	while (ctx->cleanup_list != NULL) {
+		pthread_cleanup_t *head = ctx->cleanup_list;
+		ctx->cleanup_list = head->next;
+		free(head);
+	}
+
+	_errno_remove(&ctx->e);
+
+	LIST_REMOVE(&pthread_common.pthread_list, ctx);
+
+	if (self != 0) {
+		pthread_common.to_cleanup.stack = ctx->stack;
+		pthread_common.to_cleanup.stacksize = ctx->stacksize;
+	}
+	else {
+		pthread_common.to_cleanup.stack = NULL;
+	}
+
+	_pthread_ctx_put(ctx);
+
+	if (prev_stack != NULL) {
+		munmap(prev_stack, prev_stacksize);
+	}
+	if (self == 0) {
+		munmap(stack, stacksize);
+	}
+}
+
+
 int pthread_join(pthread_t thread, void **value_ptr)
 {
-	if (thread == pthread_self())
+	if (pthread_equal(thread, pthread_self())) {
 		return EDEADLK;
+	}
 
 	pthread_ctx *ctx = (pthread_ctx *)thread;
 
@@ -323,37 +313,29 @@ int pthread_join(pthread_t thread, void **value_ptr)
 
 	mutexLock(pthread_common.pthread_list_lock);
 
-	if (ctx->detached != PTHREAD_CREATE_DETACHED) {
-		int err, id = ctx->id;
+	if (ctx->is_detached != 0) {
 		mutexUnlock(pthread_common.pthread_list_lock);
+		return EINVAL;
+	}
 
-		do {
-			err = threadJoin(id, 0);
-		} while (err == -EINTR);
+	int err, id = ctx->id;
+	mutexUnlock(pthread_common.pthread_list_lock);
 
-		if (err < 0) {
-			return err;
-		}
-		mutexLock(pthread_common.pthread_list_lock);
-	}
+	do {
+		err = threadJoin(id, 0);
+	} while (err == -EINTR);
 
-	while (ctx->cleanup_list != NULL) {
-		pthread_cleanup_t *head = ctx->cleanup_list;
-		ctx->cleanup_list = head->next;
-		free(head);
+	if (err < 0) {
+		return err;
 	}
+	mutexLock(pthread_common.pthread_list_lock);
+
 
 	if (value_ptr != NULL) {
 		*value_ptr = ctx->retval;
 	}
 
-	_errno_remove(&ctx->e);
-
-	LIST_REMOVE(&pthread_common.pthread_list, ctx);
-	mutexUnlock(pthread_common.pthread_list_lock);
-
-	munmap(ctx->stack, ctx->stacksize);
-	pthread_ctx_put(ctx);
+	_pthread_release(ctx, 0);
 
 	return 0;
 }
@@ -369,14 +351,14 @@ int pthread_detach(pthread_t thread)
 		return ESRCH;
 	}
 
-	if (ctx->detached != PTHREAD_CREATE_JOINABLE) {
+	if (ctx->is_detached != 0) {
 		mutexUnlock(pthread_common.pthread_list_lock);
 		return EINVAL;
 	}
 
-	LIST_REMOVE(&pthread_common.pthread_list, ctx);
+	ctx->is_detached = 1;
 
-	_pthread_ctx_put(ctx);
+	mutexUnlock(pthread_common.pthread_list_lock);
 
 	return 0;
 }
@@ -403,11 +385,51 @@ int pthread_setcancelstate(int state, int *oldstate)
 }
 
 
+static void pthread_key_cleanup(pthread_ctx *ctx)
+{
+	ctx->exiting = 1;
+
+	mutexLock(pthread_common.pthread_key_lock);
+
+	for (int i = 0; i <= PTHREAD_DESTRUCTOR_ITERATIONS; i++) {
+		int all_null = 1;
+		for (pthread_key_data_t *key_data = ctx->key_data_list; key_data != NULL; key_data = key_data->next) {
+			if (key_data->key == NULL) {
+				continue;
+			}
+			void (*destructor)(void *) = key_data->key->destructor;
+			void *value = key_data->value;
+
+			if ((value != NULL) && (destructor != NULL)) {
+				all_null = 0;
+				key_data->value = NULL;
+				mutexUnlock(pthread_common.pthread_key_lock);
+
+				destructor(value);
+
+				mutexLock(pthread_common.pthread_key_lock);
+			}
+		}
+		if (all_null != 0) {
+			break;
+		}
+	}
+
+	pthread_key_data_t *key_data = ctx->key_data_list;
+	while (key_data != NULL) {
+		pthread_key_data_t *curr = key_data;
+		key_data = key_data->next;
+		free(curr);
+	}
+
+	mutexUnlock(pthread_common.pthread_key_lock);
+}
+
+
 int pthread_cancel(pthread_t thread)
 {
 	int err = 0, id;
 	pthread_ctx *ctx = (pthread_ctx *)thread;
-	_pthread_key_cleanup_t *cleanup;
 
 	if (ctx == NULL) {
 		err = -ESRCH;
@@ -425,11 +447,12 @@ int pthread_cancel(pthread_t thread)
 		}
 		else {
 			if (ctx->cancelstate == PTHREAD_CANCEL_ENABLE) {
-				cleanup = pthread_key_cleanup(ctx);
+				_pthread_do_cleanup(ctx);
 				ctx->retval = (void *)PTHREAD_CANCELED;
 				id = ctx->id;
-				_pthread_ctx_put(ctx);
-				pthread_full_key_cleanup(cleanup);
+				mutexUnlock(pthread_common.pthread_list_lock);
+				pthread_key_cleanup(ctx);
+				pthread_ctx_put(ctx);
 				err = signalPost(getpid(), id, signal_cancel);
 			}
 			else {
@@ -443,7 +466,7 @@ int pthread_cancel(pthread_t thread)
 
 pthread_t pthread_self(void)
 {
-	pthread_ctx *ctx = find_pthread(gettid());
+	pthread_ctx *ctx = pthread_find(gettid());
 	if (ctx != NULL) {
 		pthread_ctx_put(ctx);
 	}
@@ -457,24 +480,35 @@ int pthread_equal(pthread_t t1, pthread_t t2)
 }
 
 
-void pthread_exit(void *value_ptr)
+static __attribute__((noreturn)) void pthread_do_exit(pthread_ctx *ctx, void *value_ptr, int cleanup)
 {
-	pthread_ctx *ctx = (pthread_ctx *)pthread_self();
-	_pthread_key_cleanup_t *cleanup;
-
 	if (ctx != NULL) {
-		mutexLock(pthread_common.pthread_list_lock);
-		_pthread_do_cleanup(ctx);
-		ctx->retval = value_ptr;
-		cleanup = pthread_key_cleanup(ctx);
-		_pthread_ctx_put(ctx);
-		pthread_full_key_cleanup(cleanup);
+		if (cleanup != 0) {
+			mutexLock(pthread_common.pthread_list_lock);
+			_pthread_do_cleanup(ctx);
+			mutexUnlock(pthread_common.pthread_list_lock);
+		}
+
+		pthread_key_cleanup(ctx);
+		if (ctx->is_detached == 0) {
+			ctx->retval = value_ptr;
+		}
+		else {
+			mutexLock(pthread_common.pthread_list_lock);
+			_pthread_release(ctx, 1);
+		}
 	}
 
 	endthread();
 }
 
 
+__attribute__((noreturn)) void pthread_exit(void *value_ptr)
+{
+	pthread_do_exit((pthread_ctx *)pthread_self(), value_ptr, 1);
+}
+
+
 int pthread_attr_init(pthread_attr_t *attr)
 {
 	*attr = pthread_attr_default;
@@ -618,9 +652,10 @@ int pthread_attr_getscope(const pthread_attr_t *attr,
 
 int pthread_attr_setdetachstate(pthread_attr_t *attr, int detachstate)
 {
-	if (detachstate != PTHREAD_CREATE_DETACHED ||
-			detachstate != PTHREAD_CREATE_JOINABLE)
+	if ((detachstate != PTHREAD_CREATE_DETACHED) ||
+			(detachstate != PTHREAD_CREATE_JOINABLE)) {
 		return EINVAL;
+	}
 
 	attr->detached = detachstate;
 
@@ -631,8 +666,9 @@ int pthread_attr_setdetachstate(pthread_attr_t *attr, int detachstate)
 int pthread_attr_getdetachstate(const pthread_attr_t *attr,
 	int *detachstate)
 {
-	if (attr == NULL)
+	if (attr == NULL) {
 		return EINVAL;
+	}
 
 	*detachstate = attr->detached;
 
@@ -1018,6 +1054,7 @@ int pthread_key_create(pthread_key_t *key, void (*destructor)(void *))
 
 int pthread_key_delete(pthread_key_t key)
 {
+	int err = EINVAL;
 	mutexLock(pthread_common.pthread_list_lock);
 
 	pthread_ctx *first = pthread_common.pthread_list, *curr = pthread_common.pthread_list;
@@ -1029,13 +1066,20 @@ int pthread_key_delete(pthread_key_t key)
 			pthread_key_data_t *prev = NULL;
 			while (head != NULL) {
 				if (head->key == key) {
-					if (prev == NULL) {
-						curr->key_data_list = head->next;
+					err = 0;
+					if (curr->exiting == 0) {
+						if (prev == NULL) {
+							curr->key_data_list = head->next;
+						}
+						else {
+							prev->next = head->next;
+						}
+						free(head);
 					}
 					else {
-						prev->next = head->next;
+						/* Prevent further calls to destructor. */
+						head->key = NULL;
 					}
-					free(head);
 					break;
 				}
 				prev = head;
@@ -1047,7 +1091,7 @@ int pthread_key_delete(pthread_key_t key)
 	}
 	mutexUnlock(pthread_common.pthread_list_lock);
 	free(key);
-	return 0;
+	return err;
 }
 
 
@@ -1171,10 +1215,10 @@ void _pthread_atfork_prepare(void)
 void _pthread_atfork_parent(void)
 {
 	mutexLock(pthread_common.pthread_atfork_lock);
-	pthread_fork_handlers_t *first = pthread_common.pthread_fork_handlers,
-							*curr = pthread_common.pthread_fork_handlers;
+	pthread_fork_handlers_t *first = pthread_common.pthread_fork_handlers;
 
 	if (first != NULL) {
+		pthread_fork_handlers_t *curr = first;
 		do {
 			if (curr->parent != NULL) {
 				curr->parent();
@@ -1189,10 +1233,10 @@ void _pthread_atfork_parent(void)
 void _pthread_atfork_child(void)
 {
 	mutexLock(pthread_common.pthread_atfork_lock);
-	pthread_fork_handlers_t *first = pthread_common.pthread_fork_handlers,
-		*curr = pthread_common.pthread_fork_handlers;
+	pthread_fork_handlers_t *first = pthread_common.pthread_fork_handlers;
 
 	if (first != NULL) {
+		pthread_fork_handlers_t *curr = first;
 		do {
 			if (curr->child != NULL) {
 				curr->child();
@@ -1206,7 +1250,7 @@ void _pthread_atfork_child(void)
 
 void pthread_cleanup_push(void (*routine)(void *), void *arg)
 {
-	pthread_ctx *ctx = find_pthread(gettid());
+	pthread_ctx *ctx = pthread_find(gettid());
 
 	if (ctx == NULL) {
 		return;
@@ -1232,7 +1276,7 @@ void pthread_cleanup_push(void (*routine)(void *), void *arg)
 
 void pthread_cleanup_pop(int execute)
 {
-	pthread_ctx *ctx = find_pthread(gettid());
+	pthread_ctx *ctx = pthread_find(gettid());
 
 	if (ctx == NULL) {
 		return;
@@ -1272,4 +1316,5 @@ void _pthread_init(void)
 	pthread_common.pthread_list = NULL;
 	pthread_common.pthread_fork_handlers = NULL;
 	pthread_create_main();
+	pthread_common.to_cleanup.stack = NULL;
 }