From ec38514d506a9d5dcbf0543f3634c99f90fc9d8f Mon Sep 17 00:00:00 2001
From: Hubert Badocha <hubert.badocha@phoenix-rtos.com>
Date: Tue, 27 Aug 2024 17:40:00 +0200
Subject: [PATCH] exit: change _atexit_call into __cxa_finalize

JIRA: RTOS-900
---
 stdlib/atexit.c | 70 ++++++++++++++++++++++++++++++++++---------------
 stdlib/exit.c   |  4 +--
 2 files changed, 51 insertions(+), 23 deletions(-)

diff --git a/stdlib/atexit.c b/stdlib/atexit.c
index db77b2fe..7abdcbaf 100644
--- a/stdlib/atexit.c
+++ b/stdlib/atexit.c
@@ -30,6 +30,7 @@ typedef void (*destructor_t)(void);
 struct atexit_node {
 	destructor_t destructors[ATEXIT_MAX];
 	void *args[ATEXIT_MAX];
+	void *handles[ATEXIT_MAX];
 	uint32_t fntype;
 	struct atexit_node *prev;
 };
@@ -39,6 +40,7 @@ struct atexit_node {
 static struct {
 	handle_t lock;
 	struct atexit_node *head;
+	struct atexit_node *newestNode;
 	unsigned int idx;
 } atexit_common = { .head = &((struct atexit_node) {}) };
 
@@ -49,11 +51,12 @@ void _atexit_init(void)
 	mutexCreate(&atexit_common.lock);
 	memset(atexit_common.head, 0, sizeof(struct atexit_node));
 	atexit_common.idx = 0;
+	atexit_common.newestNode = atexit_common.head;
 }
 
 
 /* Generic function to register destructors */
-static int _atexit_register(int isarg, void (*fn)(void), void *arg)
+static int _atexit_register(int isarg, void (*fn)(void), void *arg, void *handle)
 {
 	struct atexit_node *node;
 
@@ -71,6 +74,7 @@ static int _atexit_register(int isarg, void (*fn)(void), void *arg)
 
 		atexit_common.head = node;
 		atexit_common.idx = 0;
+		atexit_common.newestNode = node;
 	}
 
 	if (isarg != 0) {
@@ -78,6 +82,7 @@ static int _atexit_register(int isarg, void (*fn)(void), void *arg)
 		node->args[atexit_common.idx] = arg;
 	}
 	node->destructors[atexit_common.idx] = fn;
+	node->handles[atexit_common.idx] = handle;
 
 	atexit_common.idx++;
 
@@ -86,46 +91,69 @@ static int _atexit_register(int isarg, void (*fn)(void), void *arg)
 }
 
 
-/* Call all registered destructors */
-void _atexit_call(void)
+/* Call destructors registered for given object, or all in case of NULL */
+/* Conforming: https://itanium-cxx-abi.github.io/cxx-abi/abi.html#dso-dtor */
+void __cxa_finalize(void *handle)
 {
+	/* No handlers registered. */
+	if (atexit_common.idx == 0) {
+		return;
+	}
+
 	mutexLock(atexit_common.lock);
-	while ((atexit_common.head != NULL) && (atexit_common.idx != 0)) {
+	/* Iteration has to be over the atexit_common.head and newest node must be restored at the end
+	 * as the atexit functions may register new atexit functions. */
+	while (atexit_common.head != NULL) {
 		atexit_common.idx--;
 		destructor_t destructor = atexit_common.head->destructors[atexit_common.idx];
-		if (((atexit_common.head->fntype >> atexit_common.idx) & 1) == 1) {
-			void *arg = atexit_common.head->args[atexit_common.idx];
-			mutexUnlock(atexit_common.lock);
-			((void (*)(void *))destructor)(arg);
-		}
-		else {
-			mutexUnlock(atexit_common.lock);
-			destructor();
+		/* Do not call already called destructors and destructors not from current handle. */
+		if ((destructor != NULL) && ((handle == atexit_common.head->handles[atexit_common.idx]) || (handle == NULL))) {
+			/* Mark destructor as called. */
+			atexit_common.head->destructors[atexit_common.idx] = NULL;
+
+			if (((atexit_common.head->fntype >> atexit_common.idx) & 1) == 1) {
+				void *arg = atexit_common.head->args[atexit_common.idx];
+				mutexUnlock(atexit_common.lock);
+				((void (*)(void *))destructor)(arg);
+			}
+			else {
+				mutexUnlock(atexit_common.lock);
+				destructor();
+			}
+			mutexLock(atexit_common.lock);
 		}
-		mutexLock(atexit_common.lock);
 
 		if (atexit_common.idx == 0) {
+			atexit_common.head = atexit_common.head->prev;
+			atexit_common.idx = ATEXIT_MAX;
+		}
+	}
+
+	atexit_common.head = atexit_common.newestNode;
+
+	/* All nodes are fully emptied from destructors, free memory. */
+	if (handle == NULL) {
+		/* Ensure the first node that is statically allocated is not freed */
+		while (atexit_common.head->prev != NULL) {
 			struct atexit_node *last = atexit_common.head;
 			atexit_common.head = atexit_common.head->prev;
-			if (atexit_common.head != NULL) {
-				/* Free only if it's not the first node (statically allocated) */
-				free(last);
-				atexit_common.idx = ATEXIT_MAX;
-			}
+			free(last);
 		}
+		atexit_common.newestNode = atexit_common.head;
 	}
+
 	mutexUnlock(atexit_common.lock);
 }
 
 
 int atexit(void (*func)(void))
 {
-	return _atexit_register(0, func, NULL);
+	return _atexit_register(0, func, NULL, NULL);
 }
 
 
 /* Register a function to run at process termination with given arguments */
-int __cxa_atexit(void (*func)(void *), void *arg, void *handler)
+int __cxa_atexit(void (*func)(void *), void *arg, void *handle)
 {
-	return _atexit_register(1, (void (*)(void))func, arg);
+	return _atexit_register(1, (void (*)(void))func, arg, handle);
 }
diff --git a/stdlib/exit.c b/stdlib/exit.c
index 05cf190f..a688c5ba 100644
--- a/stdlib/exit.c
+++ b/stdlib/exit.c
@@ -16,7 +16,7 @@
 #include <stdlib.h>
 #include <stdio.h>
 
-extern void _atexit_call(void);
+extern void __cxa_finalize(void *);
 extern void sys_exit(int) __attribute__((noreturn));
 
 
@@ -34,7 +34,7 @@ void _Exit(int status)
 
 void exit(int status)
 {
-	_atexit_call();
+	__cxa_finalize(NULL);
 	fflush(NULL);
 	_exit(status);
 	for(;;);