posix: pthread: implement pthread_cleanup_push() / pop()

doc/services/portability/posix/option_groups/index.rst

@@ -32,8 +32,8 @@ multiple processes.
     pthread_barrierattr_init(),yes
     pthread_barrierattr_setpshared(),yes
     pthread_cancel(),yes
-    pthread_cleanup_pop(),
-    pthread_cleanup_push(),
+    pthread_cleanup_pop(),yes
+    pthread_cleanup_push(),yes
     pthread_cond_broadcast(),yes
     pthread_cond_destroy(),yes
     pthread_cond_init(),yes

include/zephyr/posix/pthread.h

@@ -480,6 +480,18 @@ int pthread_atfork(void (*prepare)(void), void (*parent)(void), void (*child)(vo
 int pthread_getconcurrency(void);
 int pthread_setconcurrency(int new_level);
 
+void __z_pthread_cleanup_push(void *cleanup[3], void (*routine)(void *arg), void *arg);
+void __z_pthread_cleanup_pop(int execute);
+
+#define pthread_cleanup_push(_rtn, _arg)                                                           \
+	do /* enforce '{'-like behaviour */ {                                                      \
+		void *_z_pthread_cleanup[3];                                                       \
+		__z_pthread_cleanup_push(_z_pthread_cleanup, _rtn, _arg)
+
+#define pthread_cleanup_pop(_ex)                                                                   \
+		__z_pthread_cleanup_pop(_ex);                                                      \
+	} /* enforce '}'-like behaviour */ while (0)
+
 /* Glibc / Oracle Extension Functions */
 
 /**

lib/posix/posix_internal.h

@@ -24,6 +24,9 @@
 struct posix_thread {
 	struct k_thread thread;
 
+	/* List nodes for pthread_cleanup_push() / pthread_cleanup_pop() */
+	sys_slist_t cleanup_list;
+
 	/* List node for ready_q, run_q, or done_q */
 	sys_dnode_t q_node;
 

lib/posix/pthread.c

@@ -30,6 +30,12 @@ LOG_MODULE_REGISTER(pthread, CONFIG_PTHREAD_LOG_LEVEL);
 
 #define PTHREAD_INIT_FLAGS PTHREAD_CANCEL_ENABLE
 
+struct __pthread_cleanup {
+	void (*routine)(void *arg);
+	void *arg;
+	sys_snode_t node;
+};
+
 enum posix_thread_qid {
 	/* ready to be started via pthread_create() */
 	POSIX_THREAD_READY_Q,
@@ -142,6 +148,46 @@ int pthread_equal(pthread_t pt1, pthread_t pt2)
 	return (pt1 == pt2);
 }
 
+static inline void __z_pthread_cleanup_init(struct __pthread_cleanup *c, void (*routine)(void *arg),
+					    void *arg)
+{
+	*c = (struct __pthread_cleanup){
+		.routine = routine,
+		.arg = arg,
+		.node = {0},
+	};
+}
+
+void __z_pthread_cleanup_push(void *cleanup[3], void (*routine)(void *arg), void *arg)
+{
+	struct posix_thread *const t = to_posix_thread(pthread_self());
+	struct __pthread_cleanup *const c = (struct __pthread_cleanup *)cleanup;
+
+	BUILD_ASSERT(3 * sizeof(void *) == sizeof(*c));
+	__ASSERT_NO_MSG(t != NULL);
+	__ASSERT_NO_MSG(c != NULL);
+	__ASSERT_NO_MSG(routine != NULL);
+	__z_pthread_cleanup_init(c, routine, arg);
+	sys_slist_prepend(&t->cleanup_list, &c->node);
+}
+
+void __z_pthread_cleanup_pop(int execute)
+{
+	sys_snode_t *node;
+	struct __pthread_cleanup *c;
+	struct posix_thread *const t = to_posix_thread(pthread_self());
+
+	__ASSERT_NO_MSG(t != NULL);
+	node = sys_slist_get(&t->cleanup_list);
+	__ASSERT_NO_MSG(node != NULL);
+	c = CONTAINER_OF(node, struct __pthread_cleanup, node);
+	__ASSERT_NO_MSG(c != NULL);
+	__ASSERT_NO_MSG(c->routine != NULL);
+	if (execute) {
+		c->routine(c->arg);
+	}
+}
+
 static bool is_posix_policy_prio_valid(uint32_t priority, int policy)
 {
 	if (priority >= sched_get_priority_min(policy) &&
@@ -414,6 +460,7 @@ int pthread_create(pthread_t *th, const pthread_attr_t *_attr, void *(*threadrou
 		}
 		t->cancel_pending = false;
 		sys_slist_init(&t->key_list);
+		sys_slist_init(&t->cleanup_list);
 		t->dynamic_stack = _attr == NULL ? attr->stack : NULL;
 	}
 	k_spin_unlock(&pthread_pool_lock, key);

tests/posix/common/src/pthread.c

@@ -905,3 +905,33 @@ ZTEST(posix_apis, test_pthread_set_get_concurrency)
 	/* EAGAIN if the a system resource to be exceeded */
 	zassert_equal(EAGAIN, pthread_setconcurrency(CONFIG_MP_MAX_NUM_CPUS + 1));
 }
+
+static void cleanup_handler(void *arg)
+{
+	bool *boolp = (bool *)arg;
+
+	*boolp = true;
+}
+
+static void *test_pthread_cleanup_entry(void *arg)
+{
+	bool executed[2] = {0};
+
+	pthread_cleanup_push(cleanup_handler, &executed[0]);
+	pthread_cleanup_push(cleanup_handler, &executed[1]);
+	pthread_cleanup_pop(false);
+	pthread_cleanup_pop(true);
+
+	zassert_true(executed[0]);
+	zassert_false(executed[1]);
+
+	return NULL;
+}
+
+ZTEST(posix_apis, test_pthread_cleanup)
+{
+	pthread_t th;
+
+	zassert_ok(pthread_create(&th, NULL, test_pthread_cleanup_entry, NULL));
+	zassert_ok(pthread_join(th, NULL));
+}