From 5c88a52bcc07c8fc238d50185af94f5f5dfa6c18 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Tue, 10 May 2022 16:11:01 +0200
Subject: Add program destructor support

Update #4649.
---
 rtemsbsd/include/machine/rtems-bsd-program.h |  6 ++++
 rtemsbsd/rtems/program-internal.h            |  7 ++++
 rtemsbsd/rtems/rtems-program.c               | 51 ++++++++++++++++++++++++++++
 3 files changed, 64 insertions(+)

diff --git a/rtemsbsd/include/machine/rtems-bsd-program.h b/rtemsbsd/include/machine/rtems-bsd-program.h
index f71ac9cd..96d4bfef 100644
--- a/rtemsbsd/include/machine/rtems-bsd-program.h
+++ b/rtemsbsd/include/machine/rtems-bsd-program.h
@@ -60,6 +60,12 @@ rtems_bsd_program_call_main_with_data_restore(const char *name,
     int (*main)(int, char **), int argc, char **argv,
     void *data_buf, const size_t data_size);
 
+void *
+rtems_bsd_program_add_destructor(void (*destructor)(void *), void *arg);
+
+void
+rtems_bsd_program_remove_destructor(void *cookie, bool call);
+
 void
 rtems_bsd_program_exit(int exit_code) __dead2;
 
diff --git a/rtemsbsd/rtems/program-internal.h b/rtemsbsd/rtems/program-internal.h
index da817130..2104c064 100644
--- a/rtemsbsd/rtems/program-internal.h
+++ b/rtemsbsd/rtems/program-internal.h
@@ -60,6 +60,12 @@ struct program_allocmem_item {
 	LIST_ENTRY(program_allocmem_item) entries;
 };
 
+struct program_destructor {
+	void	(*destructor)(void *);
+	void	*arg;
+	LIST_ENTRY(program_destructor) link;
+};
+
 struct rtems_bsd_program_control {
 	void *context;
 	int exit_code;
@@ -68,6 +74,7 @@ struct rtems_bsd_program_control {
 	LIST_HEAD(, program_fd_item) open_fd;
 	LIST_HEAD(, program_file_item) open_file;
 	LIST_HEAD(, program_allocmem_item) allocated_mem;
+	LIST_HEAD(, program_destructor) destructors;
 };
 
 struct rtems_bsd_program_control *rtems_bsd_program_get_control_or_null(void);
diff --git a/rtemsbsd/rtems/rtems-program.c b/rtemsbsd/rtems/rtems-program.c
index 204ed248..1ca8e3b9 100644
--- a/rtemsbsd/rtems/rtems-program.c
+++ b/rtemsbsd/rtems/rtems-program.c
@@ -224,6 +224,18 @@ allocmem_free_all(struct rtems_bsd_program_control *prog_ctrl)
 	}
 }
 
+static void
+call_destructors(struct rtems_bsd_program_control *prog_ctrl)
+{
+	struct program_destructor *node;
+	struct program_destructor *tmp;
+
+	LIST_FOREACH_SAFE(node, &prog_ctrl->destructors, link, tmp) {
+		(*node->destructor)(node->arg);
+		free(node);
+	}
+}
+
 int
 rtems_bsd_program_call(const char *name, int (*prog)(void *), void *context)
 {
@@ -251,6 +263,7 @@ rtems_bsd_program_call(const char *name, int (*prog)(void *), void *context)
 	LIST_INIT(&prog_ctrl->open_fd);
 	LIST_INIT(&prog_ctrl->open_file);
 	LIST_INIT(&prog_ctrl->allocated_mem);
+	LIST_INIT(&prog_ctrl->destructors);
 
 	if (setjmp(prog_ctrl->return_context) == 0) {
 		exit_code = (*prog)(context);
@@ -262,10 +275,48 @@ rtems_bsd_program_call(const char *name, int (*prog)(void *), void *context)
 	fd_close_all(prog_ctrl);
 	file_close_all(prog_ctrl);
 	allocmem_free_all(prog_ctrl);
+	call_destructors(prog_ctrl);
 	free(prog_ctrl);
 	return (exit_code);
 }
 
+void *
+rtems_bsd_program_add_destructor(void (*destructor)(void *), void *arg)
+{
+	struct rtems_bsd_program_control *prog_ctrl;
+	struct program_destructor *node;
+
+	prog_ctrl = rtems_bsd_program_get_control_or_null();
+	if (prog_ctrl == NULL) {
+		return (NULL);
+	}
+
+	node = malloc(sizeof(*node));
+	if (node == NULL) {
+		return (NULL);
+	}
+
+	node->destructor = destructor;
+	node->arg = arg;
+	LIST_INSERT_HEAD(&prog_ctrl->destructors, node, link);
+	return (node);
+}
+
+void
+rtems_bsd_program_remove_destructor(void *cookie, bool call)
+{
+	struct program_destructor *node;
+
+	node = cookie;
+	LIST_REMOVE(node, link);
+
+	if (call) {
+		(*node->destructor)(node->arg);
+	}
+
+	free(node);
+}
+
 void
 rtems_bsd_program_exit(int exit_code)
 {
-- 
cgit v1.2.3