rtemsbsd: Add wrapper for open, fopen, malloc, ...

Add the following rtems_bsd_program_... wrapper:

* rtems_bsd_program_open
* rtems_bsd_program_socket
* rtems_bsd_program_close
* rtems_bsd_program_fopen
* rtems_bsd_program_fclose
* rtems_bsd_program_malloc
* rtems_bsd_program_calloc
* rtems_bsd_program_realloc
* rtems_bsd_program_free
* rtems_bsd_program_strdup
* rtems_bsd_program_vasprintf
* rtems_bsd_program_asprintf

2 files changed, 581 insertions, 2 deletions

diff --git a/rtemsbsd/include/machine/rtems-bsd-program.h b/rtemsbsd/include/machine/rtems-bsd-program.h
index 4605017f..a6f8dc6c 100644
--- a/rtemsbsd/include/machine/rtems-bsd-program.h
+++ b/rtemsbsd/include/machine/rtems-bsd-program.h
@@ -43,6 +43,7 @@
 #include <sys/cdefs.h>
 #include <stdbool.h>
 #include <stdio.h>
+#include <stdarg.h>
 
 __BEGIN_DECLS
 
@@ -56,7 +57,7 @@ rtems_bsd_program_call_main(const char *name, int (*main)(int, char **),
 int
 rtems_bsd_program_call_main_with_data_restore(const char *name,
     int (*main)(int, char **), int argc, char **argv,
-    const void *data_buf, const size_t data_size);
+    void *data_buf, const size_t data_size);
 
 void
 rtems_bsd_program_exit(int exit_code) __dead2;
@@ -76,6 +77,43 @@ rtems_bsd_program_lock(void);
 void
 rtems_bsd_program_unlock(void);
 
+int
+rtems_bsd_program_open(const char *path, int oflag, ...);
+
+int
+rtems_bsd_program_socket(int domain, int type, int protocol);
+
+int
+rtems_bsd_program_close(int fd);
+
+FILE *
+rtems_bsd_program_fopen(const char *restrict filename,
+    const char *restrict mode);
+
+int
+rtems_bsd_program_fclose(FILE *file);
+
+void *
+rtems_bsd_program_malloc(size_t size);
+
+void *
+rtems_bsd_program_calloc(size_t nelem, size_t elsize);
+
+void *
+rtems_bsd_program_realloc(void *ptr, size_t size);
+
+char *
+rtems_bsd_program_strdup(const char *s1);
+
+int
+rtems_bsd_program_vasprintf(char **strp, const char *fmt, va_list ap);
+
+int
+rtems_bsd_program_asprintf(char **strp, const char *fmt, ...);
+
+void
+rtems_bsd_program_free(void *ptr);
+
 #ifndef RTEMS_BSD_PROGRAM_NO_EXIT_WRAP
   #define exit(code) rtems_bsd_program_exit(code)
 #endif
@@ -92,6 +130,58 @@ rtems_bsd_program_unlock(void);
   #define printf(...) fprintf(stdout, __VA_ARGS__)
 #endif
 
+#ifndef RTEMS_BSD_PROGRAM_NO_OPEN_WRAP
+  #define open(path, oflag, ...) \
+      rtems_bsd_program_open(path, oflag, ## __VA_ARGS__)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_SOCKET_WRAP
+  #define socket(domain, type, protocol) \
+      rtems_bsd_program_socket(domain, type, protocol)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_CLOSE_WRAP
+  #define close(fildes) rtems_bsd_program_close(fildes)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_FOPEN_WRAP
+  #define fopen(filename, mode) rtems_bsd_program_fopen(filename, mode)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_FCLOSE_WRAP
+  #define fclose(file) rtems_bsd_program_fclose(file)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_MALLOC_WRAP
+  #define malloc(size) rtems_bsd_program_malloc(size)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_CALLOC_WRAP
+  #define calloc(nelem, elsize) rtems_bsd_program_calloc(nelem, elsize)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_REALLOC_WRAP
+  #define realloc(ptr, size) rtems_bsd_program_realloc(ptr, size)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_STRDUP_WRAP
+  #define strdup(s1) rtems_bsd_program_strdup(s1)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_VASPRINTF_WRAP
+  #define vasprintf(strp, fmt, ap) \
+      rtems_bsd_program_vasprintf(strp, fmt, ap)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_ASPRINTF_WRAP
+  #define asprintf(strp, fmt, ...) \
+      rtems_bsd_program_asprintf(strp, fmt, ## __VA_ARGS__)
+#endif
+
+#ifndef RTEMS_BSD_PROGRAM_NO_FREE_WRAP
+  #define free(ptr) rtems_bsd_program_free(ptr);
+#endif
+
 __END_DECLS
 
 #endif /* _RTEMS_BSD_MACHINE_RTEMS_BSD_PROGRAM_H_ */
diff --git a/rtemsbsd/rtems/rtems-kernel-program.c b/rtemsbsd/rtems/rtems-kernel-program.c
index 0355c585..85a2193e 100644
--- a/rtemsbsd/rtems/rtems-kernel-program.c
+++ b/rtemsbsd/rtems/rtems-kernel-program.c
@@ -42,6 +42,7 @@
 
 #include <rtems/bsd/sys/param.h>
 #include <sys/types.h>
+#include <sys/socket.h>
 #include <sys/systm.h>
 #include <sys/kernel.h>
 #include <sys/proc.h>
@@ -49,11 +50,26 @@
 #include <rtems/bsd/sys/lock.h>
 #include <sys/mutex.h>
 
+#include <fcntl.h>
 #include <setjmp.h>
 #include <stdarg.h>
 #include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
 
 #undef printf
+#define RTEMS_BSD_PROGRAM_NO_OPEN_WRAP
+#define RTEMS_BSD_PROGRAM_NO_SOCKET_WRAP
+#define RTEMS_BSD_PROGRAM_NO_CLOSE_WRAP
+#define RTEMS_BSD_PROGRAM_NO_FOPEN_WRAP
+#define RTEMS_BSD_PROGRAM_NO_FCLOSE_WRAP
+#define RTEMS_BSD_PROGRAM_NO_MALLOC_WRAP
+#define RTEMS_BSD_PROGRAM_NO_CALLOC_WRAP
+#define RTEMS_BSD_PROGRAM_NO_REALLOC_WRAP
+#define RTEMS_BSD_PROGRAM_NO_STRDUP_WRAP
+#define RTEMS_BSD_PROGRAM_NO_VASPRINTF_WRAP
+#define RTEMS_BSD_PROGRAM_NO_ASPRINTF_WRAP
+#define RTEMS_BSD_PROGRAM_NO_FREE_WRAP
 #include <machine/rtems-bsd-program.h>
 
 struct rtems_bsd_program_control {
@@ -61,8 +77,213 @@ struct rtems_bsd_program_control {
 	int exit_code;
 	const char *name;
 	jmp_buf return_context;
+	LIST_HEAD(, rtems_bsd_fd_list) open_fd;
+	LIST_HEAD(, rtems_bsd_file_list) open_file;
+	LIST_HEAD(, rtems_bsd_allocmem_list) allocated_mem;
 };
 
+struct rtems_bsd_fd_list {
+	int	fd;
+	LIST_ENTRY(rtems_bsd_fd_list) entries;
+};
+
+struct rtems_bsd_file_list {
+	FILE	*file;
+	LIST_ENTRY(rtems_bsd_file_list) entries;
+};
+
+struct rtems_bsd_allocmem_list {
+	void	*ptr;
+	LIST_ENTRY(rtems_bsd_allocmem_list) entries;
+};
+
+enum rtems_bsd_alloc_type {
+	RTEMS_BSD_ALLOC_CALL_MALLOC,
+	RTEMS_BSD_ALLOC_CALL_CALLOC,
+	RTEMS_BSD_ALLOC_CALL_REALLOC,
+	RTEMS_BSD_ALLOC_CALL_STRDUP,
+};
+
+static struct rtems_bsd_program_control *
+rtems_bsd_program_get_cur_prog_ctrl_or_null(void)
+{
+	struct rtems_bsd_program_control *prog_ctrl = NULL;
+	struct thread *td = rtems_bsd_get_curthread_or_null();
+
+	if (td != NULL) {
+		prog_ctrl = td->td_prog_ctrl;
+
+		if (prog_ctrl == NULL) {
+			panic("unexpected BSD program state");
+		}
+	} else {
+		errno = ENOMEM;
+	}
+
+	return prog_ctrl;
+}
+
+static int
+fd_remove(struct rtems_bsd_program_control *prog_ctrl, int fd)
+{
+	struct rtems_bsd_fd_list *current;
+	int rv = -1;
+
+	for(current = prog_ctrl->open_fd.lh_first;
+	    current != NULL;
+	    current = current->entries.le_next) {
+		if(current->fd == fd) {
+			LIST_REMOVE(current, entries);
+			free(current, M_TEMP);
+			rv = 0;
+			break;
+		}
+	}
+
+	return rv;
+}
+
+static int
+fd_close_remove(struct rtems_bsd_program_control *prog_ctrl, int fd)
+{
+	int rv = -1;
+
+	rv = fd_remove(prog_ctrl, fd);
+	if(rv == 0) {
+		rv = close(fd);
+	}
+
+	return rv;
+}
+
+static void
+fd_close_all(struct rtems_bsd_program_control *prog_ctrl)
+{
+	while(prog_ctrl->open_fd.lh_first != NULL) {
+		struct rtems_bsd_fd_list *current;
+		int fd;
+		int rv;
+
+		current = prog_ctrl->open_fd.lh_first;
+		fd = current->fd;
+
+		rv = fd_close_remove(prog_ctrl, fd);
+		if(rv != 0) {
+			syslog(LOG_ERR,
+			    "BSD Program: Could not close file %d or could not remove it from list of open files",
+			    fd);
+		}
+	}
+}
+
+static int
+file_remove(struct rtems_bsd_program_control *prog_ctrl, FILE *file)
+{
+	struct rtems_bsd_file_list *current;
+	int rv = EOF;
+
+	for(current = prog_ctrl->open_file.lh_first;
+	    current != NULL;
+	    current = current->entries.le_next) {
+		if(current->file == file) {
+			LIST_REMOVE(current, entries);
+			free(current, M_TEMP);
+			rv = 0;
+			break;
+		}
+	}
+
+	return rv;
+}
+
+static int
+file_close_remove(struct rtems_bsd_program_control *prog_ctrl, FILE *file)
+{
+	int rv = EOF;
+
+	rv = file_remove(prog_ctrl, file);
+	if(rv == 0) {
+		rv = fclose(file);
+	}
+
+	return rv;
+}
+
+static void
+file_close_all(struct rtems_bsd_program_control *prog_ctrl)
+{
+	while(prog_ctrl->open_file.lh_first != NULL) {
+		struct rtems_bsd_file_list *current;
+		FILE *file;
+		int rv;
+
+		current = prog_ctrl->open_file.lh_first;
+		file = current->file;
+
+		rv = file_close_remove(prog_ctrl, file);
+		if(rv != 0) {
+			syslog(LOG_ERR,
+			    "BSD Program: Could not close stream %p or could not remove it from list of open streams",
+			    file);
+		}
+	}
+}
+
+static int
+allocmem_remove(struct rtems_bsd_program_control *prog_ctrl, void *ptr)
+{
+	struct rtems_bsd_allocmem_list *current;
+	int rv = -1;
+
+	for(current = prog_ctrl->allocated_mem.lh_first;
+	    current != NULL;
+	    current = current->entries.le_next) {
+		if(current->ptr == ptr) {
+			LIST_REMOVE(current, entries);
+			rv = 0;
+			break;
+		}
+	}
+
+	return rv;
+}
+
+static int
+allocmem_free_remove(struct rtems_bsd_program_control *prog_ctrl, void *ptr)
+{
+	int rv = -1;
+
+	rv = allocmem_remove(prog_ctrl, ptr);
+	if(rv == 0) {
+		free(ptr, M_TEMP);
+	}
+
+	return rv;
+}
+
+static void
+allocmem_free_all(struct rtems_bsd_program_control *prog_ctrl)
+{
+	while(prog_ctrl->allocated_mem.lh_first != NULL) {
+		struct rtems_bsd_allocmem_list *current;
+		void *ptr;
+		int rv;
+
+		current = prog_ctrl->allocated_mem.lh_first;
+		ptr = current->ptr;
+
+		rv = allocmem_free_remove(prog_ctrl, ptr);
+		if(rv != 0) {
+			/* This should never happen. It would mean that someone
+			 * changed the allocmem list while we are removing the
+			 * element. */
+			syslog(LOG_ERR,
+			    "BSD Program: Could not remove %p from list of allocated memory",
+			    ptr);
+		}
+	}
+}
+
 int
 rtems_bsd_program_call(const char *name, int (*prog)(void *), void *context)
 {
@@ -82,12 +303,20 @@ rtems_bsd_program_call(const char *name, int (*prog)(void *), void *context)
 				prog_ctrl->name = name;
 				prog_ctrl->exit_code = exit_code;
 
+				LIST_INIT(&(prog_ctrl->open_fd));
+				LIST_INIT(&(prog_ctrl->open_file));
+				LIST_INIT(&(prog_ctrl->allocated_mem));
+
 				if (setjmp(prog_ctrl->return_context) == 0) {
 					exit_code = (*prog)(context);
 				} else {
 					exit_code = prog_ctrl->exit_code;
 				}
 
+				fd_close_all(prog_ctrl);
+				file_close_all(prog_ctrl);
+				allocmem_free_all(prog_ctrl);
+
 				td->td_prog_ctrl = NULL;
 				free(prog_ctrl, M_TEMP);
 			} else {
@@ -203,7 +432,7 @@ rtems_bsd_program_call_main(const char *name, int (*main)(int, char **),
 int
 rtems_bsd_program_call_main_with_data_restore(const char *name,
     int (*main)(int, char **), int argc, char **argv,
-    const void *data_buf, const size_t data_size)
+    void *data_buf, const size_t data_size)
 {
 	int exit_code = EXIT_FAILURE;
 	void *savebuf;
@@ -238,3 +467,263 @@ rtems_bsd_program_unlock(void)
 {
 	mtx_unlock(&program_mtx);
 }
+
+int
+rtems_bsd_program_open(const char *path, int oflag, ...)
+{
+	struct rtems_bsd_program_control *prog_ctrl =
+	    rtems_bsd_program_get_cur_prog_ctrl_or_null();
+	va_list list;
+	mode_t mode = 0;
+	int fd = -1;
+
+	if (prog_ctrl != NULL) {
+		struct rtems_bsd_fd_list *new =
+		    malloc(sizeof(struct rtems_bsd_fd_list), M_TEMP, 0);
+
+		if(new != NULL) {
+			va_start(list, oflag);
+			mode = va_arg(list, mode_t);
+
+			fd = open(path, oflag, mode);
+
+			va_end(list);
+
+			if(fd != -1) {
+				new->fd = fd;
+				LIST_INSERT_HEAD(&(prog_ctrl->open_fd),
+				    new, entries);
+			} else {
+				free(new, M_TEMP);
+			}
+		} else {
+			errno = ENOMEM;
+		}
+	}
+
+	return fd;
+}
+
+int
+rtems_bsd_program_socket(int domain, int type, int protocol)
+{
+	struct rtems_bsd_program_control *prog_ctrl =
+	    rtems_bsd_program_get_cur_prog_ctrl_or_null();
+	int fd = -1;
+
+	if (prog_ctrl != NULL) {
+		struct rtems_bsd_fd_list *new =
+		    malloc(sizeof(struct rtems_bsd_fd_list), M_TEMP, 0);
+
+		if(new != NULL) {
+			/* FIXME: Why is there an implicit declaration warning?
+			 */
+			fd = socket(domain, type, protocol);
+
+			if(fd != -1) {
+				new->fd = fd;
+				LIST_INSERT_HEAD(&(prog_ctrl->open_fd),
+				    new, entries);
+			} else {
+				free(new, M_TEMP);
+			}
+		} else {
+			errno = ENOMEM;
+		}
+	}
+
+	return fd;
+}
+
+int
+rtems_bsd_program_close(int fd)
+{
+	struct rtems_bsd_program_control *prog_ctrl =
+	    rtems_bsd_program_get_cur_prog_ctrl_or_null();
+	int rv = -1;
+
+	if (prog_ctrl != NULL) {
+		rv = fd_close_remove(prog_ctrl, fd);
+	}
+
+	return rv;
+}
+
+FILE *
+rtems_bsd_program_fopen(const char *restrict filename,
+    const char *restrict mode)
+{
+	FILE *file = NULL;
+	struct rtems_bsd_program_control *prog_ctrl =
+	    rtems_bsd_program_get_cur_prog_ctrl_or_null();
+
+	if (prog_ctrl != NULL) {
+		struct rtems_bsd_file_list *new =
+		    malloc(sizeof(struct rtems_bsd_file_list), M_TEMP, 0);
+
+		if(new != NULL) {
+			file = fopen(filename, mode);
+
+			if(file != NULL) {
+				new->file = file;
+				LIST_INSERT_HEAD(
+				    &(prog_ctrl->open_file), new,
+				    entries);
+			} else {
+				free(new, M_TEMP);
+			}
+		} else {
+			errno = ENOMEM;
+		}
+	}
+
+	return file;
+}
+
+int
+rtems_bsd_program_fclose(FILE *file)
+{
+	struct rtems_bsd_program_control *prog_ctrl =
+	    rtems_bsd_program_get_cur_prog_ctrl_or_null();
+	int rv = EOF;
+
+	if (prog_ctrl != NULL) {
+		rv = file_close_remove(prog_ctrl, file);
+	}
+
+	return rv;
+}
+
+static void *
+rtems_bsd_program_alloc(size_t size, void *org_ptr)
+{
+	struct rtems_bsd_program_control *prog_ctrl =
+	    rtems_bsd_program_get_cur_prog_ctrl_or_null();
+	void *ptr = NULL;
+	size_t size_with_list;
+	size_t size_alligned;
+
+	if (prog_ctrl != NULL) {
+		/* align the end to the next word address */
+		size_alligned = size;
+		if((size_alligned & 0x3) != 0) {
+			size_alligned = (size_alligned | 0x03) + 1;
+		}
+		size_with_list = size_alligned +
+		    sizeof(struct rtems_bsd_allocmem_list);
+
+		if(org_ptr != NULL) {
+			/* It's a reallocation. So first remove the old pointer
+			 * from the list */
+			allocmem_remove(prog_ctrl, org_ptr);
+		}
+
+		ptr = realloc(org_ptr, size_with_list, M_TEMP, 0);
+
+		if(ptr != NULL) {
+			struct rtems_bsd_allocmem_list *listele;
+			listele = ptr + size_alligned;
+			listele->ptr = ptr;
+			LIST_INSERT_HEAD(&(prog_ctrl->allocated_mem),
+			    listele, entries);
+		}
+	}
+
+	return ptr;
+}
+
+void *
+rtems_bsd_program_malloc(size_t size)
+{
+	return rtems_bsd_program_alloc(size, NULL);
+}
+
+void *
+rtems_bsd_program_calloc(size_t nelem, size_t elsize)
+{
+	void *ptr;
+	size_t size = elsize * nelem;
+
+	ptr = rtems_bsd_program_alloc(size, NULL);
+	if(ptr != NULL) {
+		memset(ptr, 0, size);
+	}
+
+	return ptr;
+}
+
+void *
+rtems_bsd_program_realloc(void *ptr, size_t size)
+{
+	return rtems_bsd_program_alloc(size, ptr);
+}
+
+char *
+rtems_bsd_program_strdup(const char *s1)
+{
+	size_t size = strlen(s1) + 1; /* add one for null termination */
+	char *new;
+
+	new = rtems_bsd_program_alloc(size, NULL);
+
+	if(new != NULL) {
+		memcpy(new, s1, size);
+	}
+
+	return new;
+}
+
+int
+rtems_bsd_program_vasprintf(char **strp, const char *fmt, va_list ap)
+{
+	va_list aq;
+	int size;
+	int rv = -1;
+
+	va_copy(aq, ap);
+	size = vsnprintf(NULL, 0, fmt, aq);
+	va_end(aq);
+
+	size += 1; /* Add space for terminating null byte */
+	*strp = rtems_bsd_program_alloc(size, NULL);
+
+	if(*strp != NULL) {
+		rv = vsnprintf(*strp, size, fmt, ap);
+	}
+
+	return rv;
+}
+
+int
+rtems_bsd_program_asprintf(char **strp, const char *fmt, ...)
+{
+	va_list ap;
+	int rv;
+
+	va_start(ap, fmt);
+
+	rv = rtems_bsd_program_vasprintf(strp, fmt, ap);
+
+	va_end(ap);
+
+	return rv;
+}
+
+void
+rtems_bsd_program_free(void *ptr)
+{
+	if(ptr != NULL) {
+		struct rtems_bsd_program_control *prog_ctrl =
+		    rtems_bsd_program_get_cur_prog_ctrl_or_null();
+
+		if (prog_ctrl != NULL) {
+			int rv = allocmem_free_remove(prog_ctrl, ptr);
+			if(rv != 0) {
+				panic("unexpected BSD program state: could not find pointer to free");
+			}
+		} else {
+			/* Outside of program context. Just free it. */
+			free(ptr, M_TEMP);
+		}
+	}
+}