/*
 * Copyright (C) 2016 Philippe Gerum <rpm@xenomai.org>
 *
 * 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 <sys/types.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/timerfd.h>
#include <stdio.h>
#include <error.h>
#include <semaphore.h>
#include <errno.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <smokey/smokey.h>
#include <linux/spi/spidev.h>
#include <rtdm/spi.h>

smokey_test_plugin(spi_transfer,
		   SMOKEY_ARGLIST(
			   SMOKEY_STRING(device),
			   SMOKEY_INT(speed),
			   SMOKEY_BOOL(latency),
			   SMOKEY_INT(ioctl_n)
		   ),
   "Run a SPI transfer.\n"
   "\tdevice=<device-path>\n"
   "\tspeed=<speed-hz>\n"
   "\tlatency\n"
   "\tioctl_n=<set to non-zero to use SPI_RTIOC_TRANSFER_N ioctl>"
);

#define ONE_BILLION	1000000000
#define TEN_MILLIONS	10000000

static int with_traffic = 1, with_latency, with_ioctl_n = 0;

#define SEQ_SHIFT 24
#define SEQ_MASK  ((1 << SEQ_SHIFT) - 1)

#define BAD_CRC  0x1
#define BAD_SEQ  0x2

struct frame_header {
	unsigned int seq: SEQ_SHIFT,
		crc : 8;
} __attribute__((packed));

/* We send a 32bit header followed by 32 bytes of payload. */
#define TRANSFER_SIZE (32 + sizeof(struct frame_header))

static unsigned char *i_area, *o_area;

static unsigned int seq_out;

static unsigned int seq_in = 1 << SEQ_SHIFT;

static int32_t minjitter, maxjitter, avgjitter;
static int32_t gminjitter = TEN_MILLIONS, gmaxjitter = -TEN_MILLIONS;
static uint32_t goverrun, gerrors;
static int64_t gavgjitter;

nanosecs_rel_t period_ns = ONE_BILLION / 2; /* 0.5s */

pthread_t display_tid, consumer_tid;

static sem_t display_sem;

static const int data_lines = 21;

static inline void *get_obuf(void)
{
	return o_area;
}

static inline void *get_odata(void)
{
	return o_area + sizeof(struct frame_header);
}

static inline size_t get_odlen(void)
{
	return TRANSFER_SIZE - sizeof(struct frame_header);
}

static void set_output_header(void)
{
	struct frame_header *fh = get_obuf();
	unsigned char *odata = get_odata();
	size_t odlen = get_odlen(), n;
	unsigned char csum;

	for (n = 0, csum = 0; n < odlen; n++)
		csum += *odata++;

	fh->crc = ~csum;
	fh->seq = seq_out;
	seq_out = (seq_out + 1) & SEQ_MASK;
}

static int check_input_header(void *ibuf, size_t ilen)
{
	struct frame_header *fh = ibuf;
	unsigned char *idata = ibuf + sizeof(*fh);
	size_t idlen = ilen - sizeof(*fh), n;
	unsigned int seq_next;
	unsigned char csum;
	int checkval = 0;

	for (n = 0, csum = 0; n < idlen; n++)
		csum += *idata++;

	if (fh->crc != (unsigned char)~csum)
		checkval |= BAD_CRC;

	if (seq_in > SEQ_MASK)
		seq_in = fh->seq;
	else {
		seq_next = (seq_in + 1) & SEQ_MASK;
		if (fh->seq != seq_next) {
			/* Try to resync. */
			seq_in = 1 << SEQ_SHIFT;
			checkval |= BAD_SEQ;
		} else
			seq_in = seq_next;
	}

	return checkval;
}

static void do_traffic(int round, const void *ibuf,
		       size_t ilen, int checkval)
{
	const struct frame_header *fh = ibuf;
	size_t idlen = ilen - sizeof(*fh), n;
	const unsigned char *idata = ibuf + sizeof(*fh);

	printf("%.4d> seq=%u%s, crc=%.2X%s",
	       round,
	       fh->seq, checkval & BAD_SEQ ? "?" : "",
	       fh->crc, checkval & BAD_CRC ? "?" : "");
	
	for (n = 0; n < idlen; n++) {
		if ((n % 16) == 0)
			printf("\n");
		printf("%.2X ", idata[n]);
	}
	printf("\n");
}

static int do_process(int round)
{
	size_t odlen, n, ilen = TRANSFER_SIZE;
	unsigned char *odata, *ibuf = i_area;
	int checkval;

	checkval = check_input_header(i_area, ilen);

	if (with_traffic)
		do_traffic(round, ibuf, ilen, checkval);

	odata = get_odata();
	odlen = get_odlen();
	for (n = 0; n < odlen; n++)
		odata[n] = odata[n] + 1 ?: 1;

	set_output_header();

	return checkval ? -EPROTO : 0;
}

static void timespec_add_ns(struct timespec *t, unsigned int ns)
{
	t->tv_nsec += ns;
	if (t->tv_nsec >= ONE_BILLION) {
		t->tv_nsec -= ONE_BILLION;
		t->tv_sec++;
	}
}

static inline long long diff_ts(struct timespec *left, struct timespec *right)
{
	return (long long)(left->tv_sec - right->tv_sec) * ONE_BILLION
		+ left->tv_nsec - right->tv_nsec;
}

static void *display_thread(void *arg)
{
	long minj, gminj, maxj, gmaxj, avgj;
	time_t start, now, dt;
	int ret, n = 0;

	sem_init(&display_sem, 0, 0);

	time(&start);

	for (;;) {
		ret = sem_wait(&display_sem);
		if (ret < 0) {
			if (errno != EIDRM)
				panic("sem_wait(), %s", symerror(errno));
			return NULL;
		}

		if (smokey_verbose_mode < 1)
			continue;
	
		minj = minjitter;
		gminj = gminjitter;
		avgj = avgjitter;
		maxj = maxjitter;
		gmaxj = gmaxjitter;

		if (data_lines && (n++ % data_lines) == 0) {
			time(&now);
			dt = now - start;
			printf("RTT|  %.2ld:%.2ld:%.2ld  (%Ld us period)\n",
			       dt / 3600, (dt / 60) % 60, dt % 60,
			       (long long)period_ns / 1000);
			printf("RTH|%11s|%11s|%11s|%8s|%8s|%11s|%11s\n",
			       "----lat min", "----lat avg",
			       "----lat max", "-overrun", "-errors",
			       "---lat best", "--lat worst");
		}
		printf("RTD|%11.3f|%11.3f|%11.3f|%8d|%8d|%11.3f|%11.3f\n",
		       (double)minj / 1000,
		       (double)avgj / 1000,
		       (double)maxj / 1000,
		       goverrun,
		       gerrors,
		       (double)gminj / 1000, (double)gmaxj / 1000);
	}

	return NULL;
}

static void start_display_thread(void)
{
	struct sched_param param;
	pthread_attr_t attr;

	pthread_attr_init(&attr);
	pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
	pthread_attr_setschedpolicy(&attr, SCHED_OTHER);
	param.sched_priority = 0;
	pthread_attr_setschedparam(&attr, &param);
	pthread_create(&display_tid, &attr, display_thread, NULL);
}

static inline
nanosecs_rel_t get_start_delay(void)
{
	return ((1000000000ULL + period_ns - 1) / period_ns) * period_ns;
}

static int do_spi_loop(int fd)
{
	int ret, n, nsamples, loops = 0, tfd;
	struct timespec now, start;
	struct itimerspec its;

	memset(get_odata(), 0x1, get_odlen());
	set_output_header();

	if (with_latency) {
		nsamples = (long long)ONE_BILLION / period_ns;
		start_display_thread();
	} else
		nsamples = 1;

	tfd = timerfd_create(CLOCK_MONOTONIC, 0);
	if (tfd < 0)
		return -errno;

	clock_gettime(CLOCK_MONOTONIC, &start);
	timespec_add_ns(&start, get_start_delay());
	its.it_value = start;
	its.it_interval.tv_sec = period_ns / ONE_BILLION;
	its.it_interval.tv_nsec = period_ns % ONE_BILLION;
	ret = timerfd_settime(tfd, TFD_TIMER_ABSTIME, &its, NULL);
	if (ret)
		return -errno;

	for (;;) {
		int32_t minj = TEN_MILLIONS, maxj = -TEN_MILLIONS, dt;
		uint32_t overrun = 0, errors = 0;
		uint64_t ticks;
		int64_t sumj;

		loops++;
	
		for (n = sumj = 0; n < nsamples; n++) {
			ret = read(tfd, &ticks, sizeof(ticks));
			if (ret < 0)
				break;
			clock_gettime(CLOCK_MONOTONIC, &start);
			if (with_ioctl_n == 0) {
				if (!__Terrno(ret,
					      ioctl(fd, SPI_RTIOC_TRANSFER)))
					return ret;
			} else {
                                if (!__Terrno(ret,
					ioctl(fd, SPI_RTIOC_TRANSFER_N,
					      TRANSFER_SIZE)))
                                        return ret;
			}
			if (with_latency) {
				clock_gettime(CLOCK_MONOTONIC, &now);
				dt = (int32_t)diff_ts(&now, &start);
				if (dt > maxj)
					maxj = dt;
				if (dt < minj)
					minj = dt;
				sumj += dt;
			}
		
			ret = do_process(loops);
			if (ret)
				errors++;
		}

		if (with_latency) {
			minjitter = minj;
			if (minj < gminjitter)
				gminjitter = minj;
			maxjitter = maxj;
			if (maxj > gmaxjitter)
				gmaxjitter = maxj;
			avgjitter = sumj / nsamples;
			gavgjitter += avgjitter;
			goverrun += overrun;
			gerrors += errors;
			sem_post(&display_sem);
		}		
	}

	return 0;
}

static int run_spi_transfer(struct smokey_test *t, int argc, char *const argv[])
{
	int fd, ret, speed_hz = 40000000;
	struct rtdm_spi_config config;
	struct rtdm_spi_iobufs iobufs;
	const char *device = NULL;
	struct sched_param param;
	void *p;
	
	smokey_parse_args(t, argc, argv);

	if (SMOKEY_ARG_ISSET(spi_transfer, latency) &&
	    SMOKEY_ARG_BOOL(spi_transfer, latency)) {
		with_latency = 1;
		/* Disable traffic tracing when monitoring latency. */
		with_traffic = 0;
	}

	if (SMOKEY_ARG_ISSET(spi_transfer, speed))
		speed_hz = SMOKEY_ARG_INT(spi_transfer, speed);

        if (SMOKEY_ARG_ISSET(spi_transfer, ioctl_n)) {
		with_ioctl_n = SMOKEY_ARG_INT(spi_transfer, ioctl_n);
		smokey_note("ioctl_n enabled; using SPI_RTIOC_TRANSFER_N");
	}

	if (!SMOKEY_ARG_ISSET(spi_transfer, device)) {
		warning("missing device= specification");
		return -EINVAL;
	}

	device = SMOKEY_ARG_STRING(spi_transfer, device);
	fd = open(device, O_RDWR);
	if (fd < 0) {
		ret = -errno;
		warning("cannot open device %s [%s]",
			device, symerror(ret));
		return ret;
	}

	iobufs.io_len = TRANSFER_SIZE;
	if (!__Terrno(ret, ioctl(fd, SPI_RTIOC_SET_IOBUFS, &iobufs)))
		return ret;

	p = mmap(NULL, iobufs.map_len, PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0);
	if (!__Fassert(p == MAP_FAILED))
		return -EINVAL;
	
	smokey_trace("input_area[%u..%u], output_area[%u..%u], mapping length=%u",
		     iobufs.i_offset, iobufs.i_offset + TRANSFER_SIZE - 1,
		     iobufs.o_offset, iobufs.o_offset + TRANSFER_SIZE - 1,
		     iobufs.map_len);
	
	i_area = p + iobufs.i_offset;
	o_area = p + iobufs.o_offset;

	config.mode = SPI_MODE_0;
	config.bits_per_word = 8;
	config.speed_hz = speed_hz;
	if (!__Terrno(ret, ioctl(fd, SPI_RTIOC_SET_CONFIG, &config)))
		return ret;

	if (!__Terrno(ret, ioctl(fd, SPI_RTIOC_GET_CONFIG, &config)))
		return ret;

	smokey_trace("speed=%u hz, mode=%#x, bits=%u",
		     config.speed_hz, config.mode, config.bits_per_word);
	
	/* Switch current thread to real-time. */
	param.sched_priority = 10;
	if (!__T(ret, pthread_setschedparam(pthread_self(),
				    SCHED_FIFO, &param)))
		return ret;

	if (!__T(ret, do_spi_loop(fd)))
		return ret;

	return 0;
}

int main(int argc, char *const argv[])
{
	struct smokey_test *t;
	int ret, fails = 0;

	if (pvlist_empty(&smokey_test_list))
		return 0;

	for_each_smokey_test(t) {
		ret = t->run(t, argc, argv);
		if (ret) {
			if (ret == -ENOSYS) {
				smokey_note("%s skipped (no kernel support)",
					    t->name);
				continue;
			}
			fails++;
			if (smokey_keep_going)
				continue;
			if (smokey_verbose_mode)
				error(1, -ret, "test %s failed", t->name);
			return 1;
		}
		smokey_note("%s OK", t->name);
	}

	return fails != 0;
}