3 files changed, 104 insertions, 18 deletions

diff --git a/src/internal/pthread_impl.h b/src/internal/pthread_impl.h
index 052a5475..26e6e1df 100644
--- a/src/internal/pthread_impl.h
+++ b/src/internal/pthread_impl.h
@@ -148,6 +148,9 @@ hidden void __do_cleanup_push(struct __ptcb *);
 hidden void __do_cleanup_pop(struct __ptcb *);
 hidden void __pthread_tsd_run_dtors();
 
+hidden void __pthread_key_delete_synccall(void (*)(void *), void *);
+hidden int __pthread_key_delete_impl(pthread_key_t);
+
 extern hidden volatile int __block_new_threads;
 extern hidden volatile size_t __pthread_tsd_size;
 extern hidden void *__pthread_tsd_main[];
diff --git a/src/thread/pthread_key_create.c b/src/thread/pthread_key_create.c
index a78e507a..e26f199c 100644
--- a/src/thread/pthread_key_create.c
+++ b/src/thread/pthread_key_create.c
@@ -3,54 +3,123 @@
 volatile size_t __pthread_tsd_size = sizeof(void *) * PTHREAD_KEYS_MAX;
 void *__pthread_tsd_main[PTHREAD_KEYS_MAX] = { 0 };
 
-static void (*volatile keys[PTHREAD_KEYS_MAX])(void *);
+static void (*keys[PTHREAD_KEYS_MAX])(void *);
+
+static pthread_rwlock_t key_lock = PTHREAD_RWLOCK_INITIALIZER;
+
+static pthread_key_t next_key;
 
 static void nodtor(void *dummy)
 {
 }
 
+static void dirty(void *dummy)
+{
+}
+
+struct cleanup_args {
+	pthread_t caller;
+	int ret;
+};
+
+static void clean_dirty_tsd_callback(void *p)
+{
+	struct cleanup_args *args = p;
+	pthread_t self = __pthread_self();
+	pthread_key_t i;
+	for (i=0; i<PTHREAD_KEYS_MAX; i++) {
+		if (keys[i] == dirty && self->tsd[i])
+			self->tsd[i] = 0;
+	}
+	/* Arbitrary choice to avoid data race. */
+	if (args->caller == self) args->ret = 0;
+}
+
+static int clean_dirty_tsd(void)
+{
+	struct cleanup_args args = {
+		.caller = __pthread_self(),
+		.ret = EAGAIN
+	};
+	__pthread_key_delete_synccall(clean_dirty_tsd_callback, &args);
+	return args.ret;
+}
+
 int __pthread_key_create(pthread_key_t *k, void (*dtor)(void *))
 {
-	unsigned i = (uintptr_t)&k / 16 % PTHREAD_KEYS_MAX;
-	unsigned j = i;
+	pthread_key_t j = next_key;
 	pthread_t self = __pthread_self();
+	int found_dirty = 0;
 
 	/* This can only happen in the main thread before
 	 * pthread_create has been called. */
 	if (!self->tsd) self->tsd = __pthread_tsd_main;
 
+	/* Purely a sentinel value since null means slot is free. */
 	if (!dtor) dtor = nodtor;
+
+	pthread_rwlock_wrlock(&key_lock);
 	do {
-		if (!a_cas_p(keys+j, 0, (void *)dtor)) {
-			*k = j;
+		if (!keys[j]) {
+			keys[next_key = *k = j] = dtor;
+			pthread_rwlock_unlock(&key_lock);
 			return 0;
+		} else if (keys[j] == dirty) {
+			found_dirty = 1;
 		}
-	} while ((j=(j+1)%PTHREAD_KEYS_MAX) != i);
-	return EAGAIN;
+	} while ((j=(j+1)%PTHREAD_KEYS_MAX) != next_key);
+
+	/* It's possible that all slots are in use or __synccall fails. */
+	if (!found_dirty || clean_dirty_tsd()) {
+		pthread_rwlock_unlock(&key_lock);
+		return EAGAIN;
+	}
+
+	/* If this point is reached there is necessarily a newly-cleaned
+	 * slot to allocate to satisfy the caller's request. Find it and
+	 * mark any additional previously-dirty slots clean. */
+	for (j=0; j<PTHREAD_KEYS_MAX; j++) {
+		if (keys[j] == dirty) {
+			if (dtor) {
+				keys[next_key = *k = j] = dtor;
+				dtor = 0;
+			} else {
+				keys[j] = 0;
+			}
+		}
+	}
+
+	pthread_rwlock_unlock(&key_lock);
+	return 0;
 }
 
-int __pthread_key_delete(pthread_key_t k)
+int __pthread_key_delete_impl(pthread_key_t k)
 {
-	keys[k] = 0;
+	pthread_rwlock_wrlock(&key_lock);
+	keys[k] = dirty;
+	pthread_rwlock_unlock(&key_lock);
 	return 0;
 }
 
 void __pthread_tsd_run_dtors()
 {
 	pthread_t self = __pthread_self();
-	int i, j, not_finished = self->tsd_used;
-	for (j=0; not_finished && j<PTHREAD_DESTRUCTOR_ITERATIONS; j++) {
-		not_finished = 0;
+	int i, j;
+	for (j=0; self->tsd_used && j<PTHREAD_DESTRUCTOR_ITERATIONS; j++) {
+		pthread_rwlock_rdlock(&key_lock);
+		self->tsd_used = 0;
 		for (i=0; i<PTHREAD_KEYS_MAX; i++) {
-			if (self->tsd[i] && keys[i]) {
-				void *tmp = self->tsd[i];
-				self->tsd[i] = 0;
-				keys[i](tmp);
-				not_finished = 1;
+			void *val = self->tsd[i];
+			void (*dtor)(void *) = keys[i];
+			self->tsd[i] = 0;
+			if (val && dtor && dtor != nodtor && dtor != dirty) {
+				pthread_rwlock_unlock(&key_lock);
+				dtor(val);
+				pthread_rwlock_rdlock(&key_lock);
 			}
 		}
+		pthread_rwlock_unlock(&key_lock);
 	}
 }
 
-weak_alias(__pthread_key_delete, pthread_key_delete);
 weak_alias(__pthread_key_create, pthread_key_create);
diff --git a/src/thread/pthread_key_delete.c b/src/thread/pthread_key_delete.c
new file mode 100644
index 00000000..012fe2da
--- /dev/null
+++ b/src/thread/pthread_key_delete.c
@@ -0,0 +1,14 @@
+#include "pthread_impl.h"
+#include "libc.h"
+
+void __pthread_key_delete_synccall(void (*f)(void *), void *p)
+{
+	__synccall(f, p);
+}
+
+int __pthread_key_delete(pthread_key_t k)
+{
+	return __pthread_key_delete_impl(k);
+}
+
+weak_alias(__pthread_key_delete, pthread_key_delete);