/*
 * Copyright (C) 2020 Song Chen <chensong@tj.kylinos.cn>
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <error.h>
#include <signal.h>
#include <sched.h>
#include <time.h>
#include <sys/time.h>
#include <unistd.h>
#include <pthread.h>
#include <semaphore.h>
#include <sys/timerfd.h>
#include <xeno_config.h>
#include <rtdm/testing.h>
#include <rtdm/gpio.h>
#include <boilerplate/trace.h>
#include <xenomai/init.h>
#include <sys/mman.h>
#include <getopt.h>

#define NS_PER_MS (1000000)
#define NS_PER_S (1000000000)

#define DEFAULT_PRIO 99
#define VERSION_STRING "0.1"
#define GPIO_HIGH 1
#define GPIO_LOW  0
#define MAX_HIST		100
#define MAX_CYCLES 1000000
#define DEFAULT_LIMIT 1000
#define DEV_PATH    "/dev/rtdm/"
#define TRACING_ON  "/sys/kernel/debug/tracing/tracing_on"
#define TRACING_EVENTS  "/sys/kernel/debug/tracing/events/enable"
#define TRACE_MARKER  "/sys/kernel/debug/tracing/trace_marker"
#define ON  "1"
#define OFF "0"

enum {
	MODE_LOOPBACK,
	MODE_REACT,
	MODE_ALL
};

/* Struct for statistics */
struct test_stat {
	long inner_min;
	long inner_max;
	double inner_avg;
	long *inner_hist_array;
	long inner_hist_overflow;
	long outer_min;
	long outer_max;
	double outer_avg;
	long *outer_hist_array;
	long outer_hist_overflow;
};

/* Struct for information */
struct test_info {
	unsigned long max_cycles;
	unsigned long total_cycles;
	unsigned long max_histogram;
	int mode;
	int clockid;
	int prio;
	int quiet;
	int tracelimit;
	int fd_dev_intr;
	int fd_dev_out;
	char pin_controller[32];
	pthread_t gpio_task;
	int gpio_intr;
	int gpio_out;
	struct test_stat ts;
};

struct test_info ti;
/* Print usage information */
static void display_help(void)
{
	printf("gpiobench V %s\n", VERSION_STRING);
	printf("Usage:\n"
	       "gpiobench <options>\n\n"

	       "-b       --breaktrace=USEC  send break trace command when latency > USEC\n"
	       "                            default=1000\n"
	       "-h       --histogram=US     dump a latency histogram to stdout after the run\n"
	       "                            US is the max time to be tracked in microseconds,\n"
	       "                            default=100\n"
	       "-l       --loops            number of loops, default=1000\n"
	       "-p       --prio             priority of highest prio thread, defaults=99\n"
	       "-q       --quiet            print only a summary on exit\n"
	       "-o       --output           gpio port number for output, no default value,\n"
	       "                            must be specified\n"
	       "-i       --intr             gpio port number as an interrupt, no default value,\n"
	       "                            must be specified\n"
	       "-c       --pinctrl          gpio pin controller's name, no default value,\n"
	       "                            must be specified\n"
	       "-m       --testmode         0 is loopback mode\n"
	       "                            1 is react mode which works with a latency box,\n"
	       "                            default=0\n"
	       "-k       --clockid          0 is CLOCK_REALTIME\n"
	       "                            1 is CLOCK_MONOTONIC,\n"
	       "                            default=1\n\n"

	       "e.g.     gpiobench -o 20 -i 21 -c pinctrl-bcm2835\n"
		);
}

static void process_options(int argc, char *argv[])
{
	int c = 0;
	static const char optstring[] = "h:p:m:l:c:b:i:o:k:q";

	struct option long_options[] = {
		{ "bracetrace", required_argument, 0, 'b'},
		{ "histogram", required_argument, 0, 'h'},
		{ "loops", required_argument, 0, 'l'},
		{ "prio", required_argument, 0, 'p'},
		{ "quiet", no_argument, 0, 'q'},
		{ "output", required_argument, 0, 'o'},
		{ "intr", required_argument, 0, 'i'},
		{ "pinctrl", required_argument, 0, 'c'},
		{ "testmode", required_argument, 0, 'm'},
		{ "clockid", required_argument, 0, 'k'},
		{ 0, 0, 0, 0},
	};

	while ((c = getopt_long(argc, argv, optstring, long_options,
						NULL)) != -1) {
		switch (c) {
		case 'h':
			ti.max_histogram = atoi(optarg);
			break;

		case 'p':
			ti.prio = atoi(optarg);
			break;

		case 'l':
			ti.max_cycles = atoi(optarg);
			break;

		case 'q':
			ti.quiet = 1;
			break;

		case 'b':
			ti.tracelimit = atoi(optarg);
			break;

		case 'i':
			ti.gpio_intr = atoi(optarg);
			break;

		case 'o':
			ti.gpio_out = atoi(optarg);
			break;

		case 'c':
			strcpy(ti.pin_controller, optarg);
			break;

		case 'm':
			ti.mode = atoi(optarg) >=
				MODE_REACT ? MODE_REACT : MODE_LOOPBACK;
			break;

		case 'k':
			ti.clockid = atoi(optarg) >= CLOCK_MONOTONIC ?
				CLOCK_MONOTONIC : CLOCK_REALTIME;
			break;

		default:
			display_help();
			exit(2);
		}
	}

	if ((ti.gpio_out == -1) || (ti.gpio_intr == -1)
				|| (strlen(ti.pin_controller) == 0)) {
		display_help();
		exit(2);
	}

	ti.prio = ti.prio > DEFAULT_PRIO ? DEFAULT_PRIO : ti.prio;
	ti.max_cycles = ti.max_cycles > MAX_CYCLES ? MAX_CYCLES : ti.max_cycles;

	ti.max_histogram = ti.max_histogram > MAX_HIST ?
		MAX_HIST : ti.max_histogram;
	ti.ts.inner_hist_array = calloc(ti.max_histogram, sizeof(long));
	ti.ts.outer_hist_array = calloc(ti.max_histogram, sizeof(long));
}

static int thread_msleep(unsigned int ms)
{
	struct timespec ts = {
		.tv_sec = (ms * NS_PER_MS) / NS_PER_S,
		.tv_nsec = (ms * NS_PER_MS) % NS_PER_S,
	};

	return -nanosleep(&ts, NULL);
}

static inline long long calc_us(struct timespec t)
{
	return ((long long)t.tv_sec * NS_PER_S + t.tv_nsec);
}

static int setevent(char *event, char *val)
{
	int fd;
	int ret;

	fd = open(event, O_WRONLY);
	if (fd < 0) {
		printf("unable to open %s\n", event);
		return -1;
	}

	ret = write(fd, val, strlen(val));
	if (ret < 0) {
		printf("unable to write %s to %s\n", val, event);
		close(fd);
		return -1;
	}

	close(fd);
	return 0;
}

static void tracing(char *enable)
{
	setevent(TRACING_EVENTS, enable);
	setevent(TRACING_ON, enable);
}

#define write_check(__fd, __buf, __len)			\
	do {						\
		int __ret = write(__fd, __buf, __len);	\
		(void)__ret;				\
	} while (0)

#define TRACEBUFSIZ 1024
static __thread char tracebuf[TRACEBUFSIZ];

static void tracemark(char *fmt, ...) __attribute__((format(printf, 1, 2)));
static void tracemark(char *fmt, ...)
{
	va_list ap;
	int len;
	int tracemark_fd;

	tracemark_fd = open(TRACE_MARKER, O_WRONLY);
	if (tracemark_fd == -1) {
		printf("unable to open trace_marker file: %s\n", TRACE_MARKER);
		return;
	}

	/* bail out if we're not tracing */
	/* or if the kernel doesn't support trace_mark */
	if (tracemark_fd < 0)
		return;

	va_start(ap, fmt);
	len = vsnprintf(tracebuf, TRACEBUFSIZ, fmt, ap);
	va_end(ap);
	write_check(tracemark_fd, tracebuf, len);

	close(tracemark_fd);
}

static int rw_gpio(int value, int index)
{
	int ret;
	struct timespec timestamp;
	struct rtdm_gpio_readout rdo;
	long long gpio_write, gpio_read, inner_diff, outer_diff;

	clock_gettime(ti.clockid, &timestamp);
	gpio_write = calc_us(timestamp);

	ret = write(ti.fd_dev_out, &value, sizeof(value));
	if (ret < 0) {
		printf("write GPIO, failed\n");
		return ret;
	}

	ret = read(ti.fd_dev_intr, &rdo, sizeof(struct rtdm_gpio_readout));
	if (ret < 0) {
		printf("read GPIO, failed\n");
		return ret;
	}

	clock_gettime(ti.clockid, &timestamp);
	gpio_read = calc_us(timestamp);

	inner_diff = (rdo.timestamp - gpio_write) / 1000;
	outer_diff = (gpio_read - gpio_write) / 1000;

	if (inner_diff < ti.ts.inner_min)
		ti.ts.inner_min = inner_diff;
	if (inner_diff > ti.ts.inner_max)
		ti.ts.inner_max = inner_diff;
	ti.ts.inner_avg += (double) inner_diff;
	if (inner_diff >= ti.max_histogram)
		ti.ts.inner_hist_overflow++;
	else
		ti.ts.inner_hist_array[inner_diff]++;

	if (outer_diff < ti.ts.outer_min)
		ti.ts.outer_min = outer_diff;
	if (inner_diff > ti.ts.outer_max)
		ti.ts.outer_max = outer_diff;
	ti.ts.outer_avg += (double) outer_diff;
	if (outer_diff >= ti.max_histogram)
		ti.ts.outer_hist_overflow++;
	else
		ti.ts.outer_hist_array[outer_diff]++;

	if (ti.quiet == 0)
		printf("index: %d, inner_diff: %8lld, outer_diff: %8lld\n",
		       index, inner_diff, outer_diff);

	return outer_diff;
}

static void *run_gpiobench_loop(void *cookie)
{
	int i, ret;

	printf("----rt task, gpio loop, test run----\n");

	for (i = 0; i < ti.max_cycles; i++) {
		ti.total_cycles = i;
		/* send a high level pulse from gpio output pin and
		 * receive an interrupt from the other gpio pin,
		 * measuring the time elapsed between the two events
		 */
		ret = rw_gpio(GPIO_HIGH, i);
		if (ret < 0) {
			printf("RW GPIO, failed\n");
			break;
		} else if (ret > ti.tracelimit) {
			tracemark("hit latency threshold (%d > %d), index: %d",
						ret, ti.tracelimit, i);
			break;
		}

		/*take a break, nanosleep here will not jeopardize the latency*/
		thread_msleep(10);

		ret = rw_gpio(GPIO_LOW, i);
		/* send a low level pulse from gpio output pin and
		 * receive an interrupt from the other gpio pin,
		 * measuring the time elapsed between the two events
		 */
		if (ret < 0) {
			printf("RW GPIO, failed\n");
			break;
		} else if (ti.tracelimit && ret > ti.tracelimit) {
			tracemark("hit latency threshold (%d > %d), index: %d",
						ret, ti.tracelimit, i);
			break;
		}

		/*take a break, nanosleep here will not jeopardize the latency*/
		thread_msleep(10);
	}

	ti.ts.inner_avg /= (ti.total_cycles * 2);
	ti.ts.outer_avg /= (ti.total_cycles * 2);

	return NULL;
}

static void *run_gpiobench_react(void *cookie)
{
	int value, ret, i;
	struct rtdm_gpio_readout rdo;

	printf("----rt task, gpio react, test run----\n");

	for (i = 0; i < ti.max_cycles; i++) {
		/* received a pulse from latency box from one of
		 * the gpio pin pair
		 */
		ret = read(ti.fd_dev_intr, &rdo, sizeof(rdo));
		if (ret < 0) {
			printf("RW GPIO read, failed\n");
			break;
		}

		if (ti.quiet == 0)
			printf("idx: %d, received signal from latency box\n",
				    i);

		/* send a signal back from the other gpio pin
		 * to latency box as the acknowledge,
		 * latency box will measure the time elapsed
		 * between the two events
		 */
		value = GPIO_HIGH;
		ret = write(ti.fd_dev_out, &value, sizeof(value));
		if (ret < 0) {
			printf("RW GPIO write, failed\n");
			break;
		}

		if (ti.quiet == 0)
			printf("idx: %d, sent reaction to latency box\n", i);
	}

	return NULL;
}

static void setup_sched_parameters(pthread_attr_t *attr, int prio)
{
	struct sched_param p;
	int ret;

	ret = pthread_attr_init(attr);
	if (ret) {
		printf("pthread_attr_init(), failed\n");
		return;
	}

	ret = pthread_attr_setinheritsched(attr, PTHREAD_EXPLICIT_SCHED);
	if (ret) {
		printf("pthread_attr_setinheritsched(), failed\n");
		return;
	}

	ret = pthread_attr_setschedpolicy(attr,
				prio ? SCHED_FIFO : SCHED_OTHER);
	if (ret) {
		printf("pthread_attr_setschedpolicy(), failed\n");
		return;
	}

	p.sched_priority = prio;
	ret = pthread_attr_setschedparam(attr, &p);
	if (ret) {
		printf("pthread_attr_setschedparam(), failed\n");
		return;
	}
}

static void init_ti(void)
{
	memset(&ti, 0, sizeof(struct test_info));
	ti.prio = DEFAULT_PRIO;
	ti.max_cycles = MAX_CYCLES;
	ti.total_cycles = MAX_CYCLES;
	ti.max_histogram = MAX_HIST;
	ti.tracelimit = DEFAULT_LIMIT;
	ti.quiet = 0;
	ti.gpio_out = -1;
	ti.gpio_intr = -1;
	ti.mode = MODE_LOOPBACK;
	ti.clockid = CLOCK_MONOTONIC;

	ti.ts.inner_min = ti.ts.outer_min = DEFAULT_LIMIT;
	ti.ts.inner_max = ti.ts.outer_max = 0;
	ti.ts.inner_avg = ti.ts.outer_avg = 0.0;
}

static void print_hist(void)
{
	int i;

	if (ti.total_cycles < (ti.max_cycles - 1)) {
		/*
		 * Test is interrupted. Force to calculate
		 * even though it is not accurate but to avoid
		 * large latency surprising us.
		 */
		printf("\nTest is interrupted and exit exceptionally\n");
		printf("Please run again till it exit normally\n");

		ti.ts.inner_avg /= (ti.total_cycles * 2);
		ti.ts.outer_avg /= (ti.total_cycles * 2);

	}
	printf("\n");
	printf("# Inner Loop Histogram\n");
	printf("# Inner Loop latency is the latency in kernel space\n"
		   "# between gpio_set_value and irq handler\n");

	for (i = 0; i < ti.max_histogram; i++) {
		unsigned long curr_latency = ti.ts.inner_hist_array[i];

		printf("%06d ", i);
		printf("%06lu\n", curr_latency);
	}

	printf("# Total:");
	printf(" %09lu", ti.total_cycles);
	printf("\n");

	printf("# Min Latencies:");
	printf(" %05lu", ti.ts.inner_min);
	printf("\n");
	printf("# Avg Latencies:");
	printf(" %05lf", ti.ts.inner_avg);
	printf("\n");
	printf("# Max Latencies:");
	printf(" %05lu", ti.ts.inner_max);
	printf("\n");

	printf("\n");
	printf("\n");

	printf("# Outer Loop Histogram\n");
	printf("# Outer Loop latency is the latency in user space\n"
		   "# between write and read\n"
		   "# Technically, outer loop latency is inner loop latercy\n"
		   "# plus overhead of event wakeup\n");

	for (i = 0; i < ti.max_histogram; i++) {
		unsigned long curr_latency = ti.ts.outer_hist_array[i];

		printf("%06d ", i);
		printf("%06lu\n", curr_latency);
	}

	printf("# Total:");
	printf(" %09lu", ti.total_cycles);
	printf("\n");

	printf("# Min Latencies:");
	printf(" %05lu", ti.ts.outer_min);
	printf("\n");
	printf("# Avg Latencies:");
	printf(" %05lf", ti.ts.outer_avg);
	printf("\n");
	printf("# Max Latencies:");
	printf(" %05lu", ti.ts.outer_max);
	printf("\n");
}

static void cleanup(void)
{
	int ret;

	if (ti.tracelimit < DEFAULT_LIMIT)
		tracing(OFF);

	ret = close(ti.fd_dev_out);
	if (ret < 0)
		printf("can't close gpio_out device\n");

	ret = close(ti.fd_dev_intr);
	if (ret < 0)
		printf("can't close gpio_intr device\n");

	if (ti.mode == MODE_LOOPBACK)
		print_hist();

}

static void cleanup_and_exit(int sig)
{
	printf("Signal %d received\n", sig);
	cleanup();
	exit(0);
}

int main(int argc, char **argv)
{
	struct sigaction sa __attribute__((unused));
	int ret = 0;
	pthread_attr_t tattr;
	int trigger, value;
	char dev_name[64];

	init_ti();

	process_options(argc, argv);

	ret = mlockall(MCL_CURRENT|MCL_FUTURE);
	if (ret) {
		printf("mlockall failed\n");
		goto out;
	}

	sprintf(dev_name, "%s%s/gpio%d",
		    DEV_PATH, ti.pin_controller, ti.gpio_out);
	ti.fd_dev_out = open(dev_name, O_RDWR);
	if (ti.fd_dev_out < 0) {
		printf("can't open %s\n", dev_name);
		goto out;
	}

	if (ti.gpio_out) {
		value = 0;
		ret = ioctl(ti.fd_dev_out, GPIO_RTIOC_DIR_OUT, &value);
		if (ret) {
			printf("ioctl gpio port output, failed\n");
			goto out;
		}
	}

	sprintf(dev_name, "%s%s/gpio%d",
		    DEV_PATH, ti.pin_controller, ti.gpio_intr);
	ti.fd_dev_intr = open(dev_name, O_RDWR);
	if (ti.fd_dev_intr < 0) {
		printf("can't open %s\n", dev_name);
		goto out;
	}

	if (ti.gpio_intr) {
		trigger = GPIO_TRIGGER_EDGE_FALLING|GPIO_TRIGGER_EDGE_RISING;
		value = 1;

		ret = ioctl(ti.fd_dev_intr, GPIO_RTIOC_IRQEN, &trigger);
		if (ret) {
			printf("ioctl gpio port interrupt, failed\n");
			goto out;
		}

		ret = ioctl(ti.fd_dev_intr,
			ti.clockid == CLOCK_MONOTONIC ?
			GPIO_RTIOC_TS_MONO : GPIO_RTIOC_TS_REAL, &value);
		if (ret) {
			printf("ioctl gpio port ts, failed\n");
			goto out;
		}
	}

	if (ti.tracelimit < DEFAULT_LIMIT)
		tracing(ON);

	signal(SIGTERM, cleanup_and_exit);
	signal(SIGINT, cleanup_and_exit);
	setup_sched_parameters(&tattr, ti.prio);

	if (ti.mode == MODE_LOOPBACK)
		ret = pthread_create(&ti.gpio_task, &tattr,
					run_gpiobench_loop, NULL);
	else
		ret = pthread_create(&ti.gpio_task, &tattr,
					run_gpiobench_react, NULL);

	if (ret) {
		printf("pthread_create(gpiotask), failed\n");
		goto out;
	}

	pthread_join(ti.gpio_task, NULL);
	pthread_attr_destroy(&tattr);

out:
	cleanup();
	return 0;
}