/*
 * Copyright (C) 2015 Philippe Gerum <rpm@xenomai.org>.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
 */
#include <sys/types.h>
#include <sys/mman.h>
#include <sched.h>
#include <getopt.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include <unistd.h>
#include <ctype.h>
#include <memory.h>
#include <malloc.h>
#include <stdarg.h>
#include <stdio.h>
#include <assert.h>
#include <xeno_config.h>
#include <boilerplate/lock.h>
#include <boilerplate/debug.h>
#include <boilerplate/ancillaries.h>
#include <xenomai/init.h>

struct base_setup_data __base_setup_data = {
	.no_sanity = !CONFIG_XENO_SANITY,
	.verbosity_level = 1,
	.trace_level = 0,
	.arg0 = NULL,
	.no_mlock = 0,
};

pid_t __node_id = 0;

int __config_done = 0;

const int __weak xenomai_auto_bootstrap = 0;

static int base_init_done;

static int main_init_done;

static DEFINE_PRIVATE_LIST(setup_list);

static const struct option base_options[] = {
	{
#define help_opt	0
		.name = "help",
	},
	{
#define affinity_opt	1
		.name = "cpu-affinity",
		.has_arg = required_argument,
	},
	{
#define verbose_opt	2
		.name = "verbose",
		.has_arg = optional_argument,
	},
	{
#define silent_opt	3
		.name = "silent",
		.has_arg = no_argument,
		.flag = &__base_setup_data.verbosity_level,
		.val = 0,
	},
	{
#define quiet_opt	4
		.name = "quiet",
		.has_arg = no_argument,
		.flag = &__base_setup_data.verbosity_level,
		.val = 0,
	},
	{
#define version_opt	5
		.name = "version",
		.has_arg = no_argument,
	},
	{
#define dumpconfig_opt	6
		.name = "dump-config",
		.has_arg = no_argument,
	},
	{
#define no_sanity_opt	7
		.name = "no-sanity",
		.has_arg = no_argument,
		.flag = &__base_setup_data.no_sanity,
		.val = 1
	},
	{
#define sanity_opt	8
		.name = "sanity",
		.has_arg = no_argument,
		.flag = &__base_setup_data.no_sanity,
	},
	{
#define trace_opt	9
		.name = "trace",
		.has_arg = optional_argument,
	},
	{
#define no_mlock_opt	10
#ifdef CONFIG_XENO_MERCURY
		.name = "no-mlock",
		.has_arg = no_argument,
		.flag = &__base_setup_data.no_mlock,
		.val = 1
#endif
	},
	{ /* Sentinel */ }
};

void __weak application_version(void)
{
	/*
	 * Applications can implement this hook for dumping their own
	 * version stamp.
	 */
}

static inline void print_version(void)
{
	application_version();
	fprintf(stderr, "based on %s\n", xenomai_version_string);
}

static inline void dump_configuration(void)
{
	int n;

	print_version();

	for (n = 0; config_strings[n]; n++)
		puts(config_strings[n]);

	printf("PTHREAD_STACK_DEFAULT=%d\n", PTHREAD_STACK_DEFAULT);
	printf("AUTOMATIC_BOOTSTRAP=%d\n", xenomai_auto_bootstrap);
}

static inline int resolve_cpuid(const char *s)
{
	return isdigit(*s) ? atoi(s) : -1;
}

static int collect_cpu_affinity(const char *cpu_list)
{
	char *s, *n, *range, *range_p = NULL, *id, *id_r;
	int start, end, cpu, nr_cpus, ret;

	/*
	 * We don't know which CPUs are online yet, but we may know
	 * which CPU identifier range is valid. Ask for the number of
	 * processors configured to find out.
	 */
	nr_cpus = (int)sysconf(_SC_NPROCESSORS_CONF);
	if (nr_cpus < 0) {
		ret = -errno;
		warning("sysconf(_SC_NPROCESSORS_CONF) failed [%s]", symerror(ret));
		return ret;
	}

	CPU_ZERO(&__base_setup_data.cpu_affinity);

	s = n = strdup(cpu_list);
	while ((range = strtok_r(n, ",", &range_p)) != NULL) {
		if (*range == '\0')
			goto next;
		end = -1;
		if (range[strlen(range)-1] == '-')
			end = nr_cpus - 1;
		id = strtok_r(range, "-", &id_r);
		if (id) {
			start = resolve_cpuid(id);
			if (*range == '-') {
				end = start;
				start = 0;
			}
			id = strtok_r(NULL, "-", &id_r);
			if (id)
				end = resolve_cpuid(id);
			else if (end < 0)
				end = start;
			if (start < 0 || start >= nr_cpus ||
			    end < 0 || end >= nr_cpus)
				goto fail;
		} else {
			start = 0;
			end = nr_cpus - 1;
		}
		for (cpu = start; cpu <= end; cpu++)
			CPU_SET(cpu, &__base_setup_data.cpu_affinity);
	next:
		n = NULL;
	}

	free(s);

	/*
	 * Check we may use this affinity, at least one CPU from the
	 * given set should be available for running threads. Since
	 * CPU affinity will be inherited by children threads, we only
	 * have to set it here.
	 *
	 * NOTE: we don't clear __base_setup_data.cpu_affinity on
	 * entry to this routine to allow cumulative --cpu-affinity
	 * options to appear in the command line arguments.
	 */
	ret = sched_setaffinity(0, sizeof(__base_setup_data.cpu_affinity),
				&__base_setup_data.cpu_affinity);
	if (ret) {
		ret = -errno;
		early_warning("invalid CPU in '%s'", cpu_list);
		return ret;
	}

	return 0;
fail:
	warning("invalid CPU number/range in '%s'", cpu_list);
	free(s);

	return -EINVAL;
}

static void retrieve_default_cpu_affinity(void)
{
	CPU_ZERO(&__base_setup_data.cpu_affinity);

#ifdef CONFIG_XENO_COBALT
	/*
	 * If the Cobalt core has restricted the CPU set, update our
	 * mask accordingly.
	 */
	unsigned long cpumask;
	FILE *fp;
	int cpu;

	fp = fopen("/proc/xenomai/affinity", "r");
	if (fp == NULL)
		return;	/* uhh? */

	if (fscanf(fp, "%lx", &cpumask) == 1) {
		for (cpu = 0; cpumask; cpumask >>= 1, cpu++)
			if (cpumask & 1)
				CPU_SET(cpu, &__base_setup_data.cpu_affinity);
	}

	fclose(fp);
#endif
}

static inline char **prep_args(int argc, char *const argv[])
{
	char **uargv;
	int n;

	uargv = malloc((argc + 1) * sizeof(char *));
	if (uargv == NULL)
		return NULL;

	for (n = 0; n < argc; n++) {
		uargv[n] = strdup(argv[n]);
		if (uargv[n] == NULL)
			return NULL;
	}

	uargv[argc] = NULL;

	return uargv;
}

static inline void pack_args(int *argcp, char **argv)
{
	int in, out;

	for (in = out = 0; in < *argcp; in++) {
		if (*argv[in])
			argv[out++] = argv[in];
		else
			free(argv[in]);
	}

	argv[out] = NULL;
	*argcp = out;
}

static struct option *build_option_array(int *base_opt_startp)
{
	struct setup_descriptor *setup;
	struct option *options, *q;
	const struct option *p;
	int nopts;

	nopts = sizeof(base_options) / sizeof(base_options[0]);

	if (!pvlist_empty(&setup_list)) {
		pvlist_for_each_entry(setup, &setup_list, __reserved.next) {
			p = setup->options;
			if (p) {
				while (p->name) {
					nopts++;
					p++;
				}
			}
		}
	}

	options = malloc(sizeof(*options) * nopts);
	if (options == NULL)
		return NULL;

	q = options;

	if (!pvlist_empty(&setup_list)) {
		pvlist_for_each_entry(setup, &setup_list, __reserved.next) {
			p = setup->options;
			if (p) {
				setup->__reserved.opt_start = q - options;
				while (p->name)
					memcpy(q++, p++, sizeof(*q));
			}
			setup->__reserved.opt_end = q - options;
		}
	}

	*base_opt_startp = q - options;
	memcpy(q, base_options, sizeof(base_options));

	return options;
}

void __weak application_usage(void)
{
	/*
	 * Applications can implement this hook for dumping their own
	 * help strings.
	 */
        fprintf(stderr, "usage: %s <options>:\n", get_program_name());
}

void xenomai_usage(void)
{
	struct setup_descriptor *setup;

	print_version();

	/*
	 * Dump help strings from the highest level code to the
	 * lowest.
	 */
	application_usage();

	if (!pvlist_empty(&setup_list)) {
		pvlist_for_each_entry_reverse(setup, &setup_list,
					      __reserved.next) {
			if (setup->help)
				setup->help();
		}
	}

        fprintf(stderr, "--cpu-affinity=<cpu[,cpu]...>	set CPU affinity of threads\n");
        fprintf(stderr, "--[no-]sanity			disable/enable sanity checks\n");
        fprintf(stderr, "--verbose[=level] 		set verbosity to desired level [=1]\n");
        fprintf(stderr, "--silent, --quiet 		same as --verbose=0\n");
        fprintf(stderr, "--trace[=level] 		set tracing to desired level [=1]\n");
        fprintf(stderr, "--version			get version information\n");
        fprintf(stderr, "--dump-config			dump configuration settings\n");
#ifdef CONFIG_XENO_MERCURY
        fprintf(stderr, "--no-mlock			do not lock memory at init\n");
#endif
        fprintf(stderr, "--help				display help\n");
}

static int parse_base_options(int *argcp, char **uargv,
			      const struct option *options,
			      int base_opt_start)
{
	int c, lindex, ret, n;

	__base_setup_data.arg0 = uargv[0];
	opterr = 0;

	/*
	 * NOTE: since we pack the argument vector on the fly while
	 * processing the options, optarg should be considered as
	 * volatile by option handlers; i.e. strdup() is required if
	 * the value has to be retained. Values from the user vector
	 * returned by xenomai_init() live in permanent memory though.
	 */

	for (;;) {
		lindex = -1;
		c = getopt_long(*argcp, uargv, "-", options, &lindex);
		if (c == EOF)
			break;
		if (lindex == -1)
			continue;

		switch (lindex - base_opt_start) {
		case affinity_opt:
			ret = collect_cpu_affinity(optarg);
			if (ret)
				return ret;
			break;
		case verbose_opt:
			__base_setup_data.verbosity_level = 1;
			if (optarg)
				__base_setup_data.verbosity_level = atoi(optarg);
			break;
		case trace_opt:
			__base_setup_data.trace_level = 1;
			if (optarg)
				__base_setup_data.trace_level = atoi(optarg);
			break;
		case silent_opt:
		case quiet_opt:
		case no_mlock_opt:
		case no_sanity_opt:
		case sanity_opt:
			break;
		case version_opt:
			print_version();
			exit(0);
		case dumpconfig_opt:
			dump_configuration();
			exit(0);
		case help_opt:
			xenomai_usage();
			exit(0);
		default:
			/* Skin option, don't process yet. */
			continue;
		}

		/*
		 * Clear the first byte of the base option we found
		 * (including any companion argument), pack_args()
		 * will expunge all options we have already handled.
		 *
		 * NOTE: only options with double-dash prefix may have
		 * been recognized by getopt_long() as Xenomai
		 * ones. This reserves short options to the
		 * application layer, sharing only the long option
		 * namespace with the Xenomai core libs. In addition,
		 * the user can delimit the start of the application
		 * arguments, preceeding them by the '--' separator on
		 * the command line.
		 */
		n = optind - 1;
		if (uargv[n][0] != '-' || uargv[n][1] != '-')
			/* Clear the separate argument value. */
			uargv[n--][0] = '\0';
		uargv[n][0] = '\0'; /* Clear the option switch. */
	}

	pack_args(argcp, uargv);

	optind = 0;

	return 0;
}

static int parse_setup_options(int *argcp, char **uargv,
			       const struct option *options)
{
	struct setup_descriptor *setup;
	int lindex, n, c, ret;

	for (;;) {
		lindex = -1;
		/*
		 * We want to keep the original order of parameters in
		 * the vector, disable getopt's parameter shuffling.
		 */
		c = getopt_long(*argcp, uargv, "-", options, &lindex);
		if (c == EOF)
			break;
		if (lindex == -1)
			continue; /* Not handled here. */
		pvlist_for_each_entry(setup, &setup_list, __reserved.next) {
			if (setup->__reserved.done ||
			    setup->parse_option == NULL)
				continue;
			if (lindex < setup->__reserved.opt_start ||
			    lindex >= setup->__reserved.opt_end)
				continue;
			lindex -= setup->__reserved.opt_start;
			trace_me("%s->parse_options()", setup->name);
			ret = setup->parse_option(lindex, optarg);
			if (ret == 0)
				break;
			return ret;
		}
		n = optind - 1;
		if (uargv[n][0] != '-' || uargv[n][1] != '-')
			/* Clear the separate argument value. */
			uargv[n--][0] = '\0';
		uargv[n][0] = '\0'; /* Clear the option switch. */
	}

	pack_args(argcp, uargv);

	optind = 0;

	return 0;
}

static void __xenomai_init(int *argcp, char *const **argvp, const char *me)
{
	struct setup_descriptor *setup;
	int ret, base_opt_start;
	struct option *options;
	struct service svc;
	char **uargv;

	/*
	 * Build the global option array, merging all option sets.
	 */
	options = build_option_array(&base_opt_start);
	if (options == NULL) {
		ret = -ENOMEM;
		goto fail;
	}

	/*
	 * Prepare a user argument vector we can modify, copying the
	 * one we have been given by the bootstrap module. This vector
	 * will be expunged from Xenomai's base options as we discover
	 * them.
	 */
	uargv = prep_args(*argcp, *argvp);
	if (uargv == NULL) {
		ret = -ENOMEM;
		goto fail;
	}

	if (base_init_done) {
		trace_me("warm init from %s", me);
		goto setup;
	}

	/* Our node id. is the tid of the main thread. */
	__node_id = get_thread_pid();

	/* No ifs, no buts: we must be called over the main thread. */
	assert(getpid() == __node_id);

	/* Retrieve the default CPU affinity. */
	retrieve_default_cpu_affinity();

	/*
	 * Parse the base options first, to bootstrap the core with
	 * the right config values.
	 */
	ret = parse_base_options(argcp, uargv,
				 options, base_opt_start);
	if (ret)
		goto fail;

	trace_me("cold init from %s", me);
	
#ifndef CONFIG_SMP
	if (__base_setup_data.no_sanity == 0) {
		ret = get_static_cpu_count();
		if (ret > 0)
			early_panic("running non-SMP libraries on SMP kernel?\n"
	    "              build with --enable-smp or disable check with --no-sanity");
	}
#endif

#ifdef CONFIG_XENO_MERCURY
	if (__base_setup_data.no_mlock == 0) {
		ret = mlockall(MCL_CURRENT | MCL_FUTURE);
		if (ret) {
			ret = -errno;
			early_warning("failed to lock memory");
			goto fail;
		}
		trace_me("memory locked");
	} else
		trace_me("memory NOT locked");
#endif

	/*
	 * Now that we have bootstrapped the core, we may call the
	 * setup handlers for tuning the configuration, then parsing
	 * their own options, and eventually doing the init chores.
	 */
setup:
	if (!pvlist_empty(&setup_list)) {

		CANCEL_DEFER(svc);

		pvlist_for_each_entry(setup, &setup_list, __reserved.next) {
			if (!setup->__reserved.done && setup->tune) {
				trace_me("%s->tune()", setup->name);
				ret = setup->tune();
				if (ret)
					break;
			}
		}
		
		ret = parse_setup_options(argcp, uargv, options);
		if (ret)
			goto fail;

		/*
		 * From now on, we may not assign configuration
		 * tunables anymore.
		 */
		__config_done = 1;
	
		pvlist_for_each_entry(setup, &setup_list, __reserved.next) {
			if (setup->__reserved.done)
				continue;
			if (setup->init) {
				trace_me("%s->init()", setup->name);
				ret = setup->init();
				if (ret)
					break;
				setup->__reserved.done = 1;
			}
		}

		CANCEL_RESTORE(svc);

		if (ret) {
			early_warning("setup call %s failed", setup->name);
			goto fail;
		}
	} else
		__config_done = 1;

	free(options);

#ifdef CONFIG_XENO_DEBUG
	if (!base_init_done && __base_setup_data.verbosity_level > 0) {
		early_warning("Xenomai compiled with %s debug enabled,\n"
			"                              "
			"%shigh latencies expected [--enable-debug=%s]",
#ifdef CONFIG_XENO_DEBUG_FULL
			"full", "very ", "full"
#else
			"partial", "", "partial"
#endif
			);
	}
#endif

	/*
	 * The final user arg vector only contains options we could
	 * not handle. The caller should be able to process them, or
	 * bail out.
	 */
	*argvp = uargv;
	base_init_done = 1;

	return;
fail:
	early_panic("initialization failed, %s", symerror(ret));
}

void xenomai_init(int *argcp, char *const **argvp)
{
	const char *me = get_program_name();

	if (main_init_done) {
		early_warning("duplicate call from main program "
			      "to %s() ignored", __func__);
		early_warning("(xeno-config --no-auto-init disables implicit call)");
	}

	__xenomai_init(argcp, argvp, me);
	main_init_done = 1;
	trace_me("%s bootstrap done", me);
}

void xenomai_init_dso(int *argcp, char *const **argvp)
{
	__xenomai_init(argcp, argvp, "DSO");
	trace_me("DSO bootstrap done");
}

void __trace_me(const char *fmt, ...)
{
	va_list ap;

	va_start(ap, fmt);
	fprintf(stderr, "--  ");
	vfprintf(stderr, fmt, ap);
	fputc('\n', stderr);
	fflush(stderr);
	va_end(ap);
}

void __register_setup_call(struct setup_descriptor *p, int id)
{
	struct setup_descriptor *pos;

	/*
	 * Trap late registration due to wrong constructor priorities.
	 */
	assert(!main_init_done);
	p->__reserved.id = id;
	p->__reserved.done = 0;

	/*
	 * Insert the new descriptor (highest id first).
	 */
	if (!pvlist_empty(&setup_list)) {
		pvlist_for_each_entry_reverse(pos, &setup_list, __reserved.next) {
			if (id >= pos->__reserved.id) {
				atpvh(&pos->__reserved.next, &p->__reserved.next);
				return;
			}
		}
	}
	pvlist_prepend(&p->__reserved.next, &setup_list);
}

const char *get_program_name(void)
{
	return basename(__base_setup_data.arg0 ?: "program");
}