[PATCH v2 17/18] rtemsbsd: Add ptpd as a shell command

[Gabriel Moyano]

---
 libbsd.py                                     |  2 +
 rtemsbsd/include/machine/rtems-bsd-commands.h |  2 +
 rtemsbsd/include/rtems/ptpd.h                 | 20 +++++
 rtemsbsd/ptpd/rtems/ptpd.c                    | 78 +++++++++++++++++++
 rtemsbsd/rtems/rtems-bsd-shell-ptpd.c         | 66 ++++++++++++++++
 5 files changed, 168 insertions(+)
 create mode 100644 rtemsbsd/include/rtems/ptpd.h
 create mode 100644 rtemsbsd/ptpd/rtems/ptpd.c
 create mode 100644 rtemsbsd/rtems/rtems-bsd-shell-ptpd.c

diff --git a/libbsd.py b/libbsd.py
index 6ebf39f3..0b215f48 100644
--- a/libbsd.py
+++ b/libbsd.py
@@ -5403,6 +5403,8 @@ class ptpd(builder.Module):
                 'ptpd/src/ptpd.c',
                 'ptpd/src/signaling.c',
                 'ptpd/src/timingdomain.c',
+                'ptpd/rtems/ptpd.c',
+                'rtems/rtems-bsd-shell-ptpd.c',
             ],
             mm.generator['source'](['-DPTPD_NO_DAEMON',
                                     '-DDATADIR=""',
diff --git a/rtemsbsd/include/machine/rtems-bsd-commands.h b/rtemsbsd/include/machine/rtems-bsd-commands.h
index a517ed7b..83fb15d0 100644
--- a/rtemsbsd/include/machine/rtems-bsd-commands.h
+++ b/rtemsbsd/include/machine/rtems-bsd-commands.h
@@ -86,6 +86,8 @@ int rtems_bsd_command_setkey(int argc, char **argv);
 
 int rtems_bsd_command_openssl(int argc, char **argv);
 
+int rtems_bsd_command_ptpd(int argc, char **argv);
+
 __END_DECLS
 
 #endif /* _RTEMS_BSD_MACHINE_RTEMS_BSD_COMMANDS_H_ */
diff --git a/rtemsbsd/include/rtems/ptpd.h b/rtemsbsd/include/rtems/ptpd.h
new file mode 100644
index 00000000..be1c88c8
--- /dev/null
+++ b/rtemsbsd/include/rtems/ptpd.h
@@ -0,0 +1,20 @@
+#ifndef RTEMS_PTPD_H_
+#define RTEMS_PTPD_H_
+
+#include <rtems.h>
+#include <rtems/rtems/status.h>
+
+typedef struct rtems_ptpd_config {
+	rtems_task_priority priority;
+	int argc;
+	char **argv;
+	void (*prepare)(const struct rtems_ptpd_config *config,
+	    int argc, char **argv);
+	void (*destroy)(const struct rtems_ptpd_config *config,
+	    int exit_code);
+} rtems_ptpd_config;
+
+int ptpd_main(int argc, char **argv);
+rtems_status_code rtems_ptpd_start(const rtems_ptpd_config *config);
+
+#endif /* RTEMS_PTPD_H_ */
diff --git a/rtemsbsd/ptpd/rtems/ptpd.c b/rtemsbsd/ptpd/rtems/ptpd.c
new file mode 100644
index 00000000..1acfbe88
--- /dev/null
+++ b/rtemsbsd/ptpd/rtems/ptpd.c
@@ -0,0 +1,78 @@
+#include <assert.h>
+#include <rtems/ptpd.h>
+#include <machine/rtems-bsd-program.h>
+#include <rtems/shell.h>
+#include <rtems/console.h>
+#include <rtems/rfs/rtems-rfs-mutex.h>
+#include <rtems/score/timecounter.h>
+#include <machine/rtems-bsd-commands.h>
+
+
+rtems_recursive_mutex ptpd_mutex =
+    RTEMS_RECURSIVE_MUTEX_INITIALIZER("ptpd");
+
+static bool ptpd_initialized;
+
+static void
+ptpd_task(rtems_task_argument arg)
+{
+	const char *default_argv[] = { "ptpd", NULL };
+	const rtems_ptpd_config *config;
+	int argc;
+	char **argv;
+	int exit_code;
+
+	config = (const rtems_ptpd_config *)arg;
+
+	if (config != NULL) {
+		argc = config->argc;
+		argv = config->argv;
+	} else {
+		argc = RTEMS_BSD_ARGC(default_argv);
+		argv = default_argv;
+	}
+
+	exit_code = rtems_bsd_program_call_main("ptpd", ptpd_main, argc, argv);
+
+	if (config != NULL && config->destroy != NULL) {
+		(*config->destroy)(config, exit_code);
+	}
+
+	rtems_task_delete(RTEMS_SELF);
+}
+
+rtems_status_code
+rtems_ptpd_start(const rtems_ptpd_config *config)
+{
+	static const char name[] = "PTPD";
+	rtems_status_code sc;
+
+	rtems_recursive_mutex_lock(&ptpd_mutex);
+
+	if (!ptpd_initialized) {
+		rtems_task_priority priority;
+		rtems_id id;
+
+		if (config != NULL && config->priority != 0) {
+			priority = config->priority;
+		} else {
+			priority = rtems_bsd_get_task_priority(name);
+		}
+
+		sc = rtems_task_create(rtems_build_name(name[0], name[1], name[2], name[3]), priority,
+		    rtems_bsd_get_task_stack_size(name), RTEMS_DEFAULT_MODES,
+		    RTEMS_FLOATING_POINT, &id);
+		if (sc == RTEMS_SUCCESSFUL) {
+			ptpd_initialized = true;
+
+			sc = rtems_task_start(id, ptpd_task,
+			    (rtems_task_argument) config);
+			assert(sc == RTEMS_SUCCESSFUL);
+		}
+	} else {
+		sc = RTEMS_INCORRECT_STATE;
+	}
+
+	rtems_recursive_mutex_unlock(&ptpd_mutex);
+	return sc;
+}
diff --git a/rtemsbsd/rtems/rtems-bsd-shell-ptpd.c b/rtemsbsd/rtems/rtems-bsd-shell-ptpd.c
new file mode 100644
index 00000000..3e374d11
--- /dev/null
+++ b/rtemsbsd/rtems/rtems-bsd-shell-ptpd.c
@@ -0,0 +1,66 @@
+#include <stdlib.h>
+#include <rtems/shell.h>
+#include <rtems/ptpd.h>
+
+typedef struct {
+	rtems_ptpd_config config;
+	char *argv[RTEMS_ZERO_LENGTH_ARRAY];
+} ptpd_command_config;
+
+static void
+ptpd_command_destroy_config(const rtems_ptpd_config *config, int exit_code)
+{
+	char **argv;
+
+	(void)exit_code;
+
+	argv = config->argv;
+	while (*argv != NULL) {
+		free(*argv);
+		++argv;
+	}
+
+	free(RTEMS_DECONST(rtems_ptpd_config *, config));
+}
+
+int
+rtems_bsd_command_ptpd(int argc, char **argv)
+{
+	ptpd_command_config *config;
+	rtems_status_code sc;
+	int i;
+
+	config = calloc(1, sizeof(*config) + (argc + 1) * sizeof(char *));
+	if (config == NULL) {
+		fprintf(stdout, "ptpd error: not enough memory\n");
+		return 1;
+	}
+
+	for (i = 0; i < argc; ++i) {
+		config->argv[i] = strdup(argv[i]);
+		if (config->argv[i] == NULL) {
+			ptpd_command_destroy_config(&config->config, 0);
+			fprintf(stdout, "ptpd error: not enough memory\n");
+			return 1;
+		}
+	}
+
+	config->config.argc = argc;
+	config->config.argv = &config->argv[0];
+	config->config.destroy = ptpd_command_destroy_config;
+
+	sc = rtems_ptpd_start(&config->config);
+	if (sc != RTEMS_SUCCESSFUL) {
+		ptpd_command_destroy_config(&config->config, 0);
+		fprintf(stdout, "ptpd start failed: %s\n", rtems_status_text(sc));
+	}
+
+	return 0;
+}
+
+rtems_shell_cmd_t rtems_shell_PTPD_Command = {
+  .name = "ptpd",
+  .usage = "ptpd [args]",
+  .topic = "net",
+  .command = rtems_bsd_command_ptpd
+};
-- 
2.25.1