/*
 * ubb-patgen.c - UBB pattern generator
 *
 * Written 2013 by Werner Almesberger
 * Copyright 2013 Werner Almesberger
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 */

#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <ctype.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sched.h>
#include <assert.h>
#include <sys/mman.h>

#include <ubb/ubb.h>
#include <ubb/regs4740.h>
#include <ubb/mmcclk.h>
#include <ubb/physmem.h>


#define	DMA	5


/* ----- List available bus clock frequencies ------------------------------ */


static int cmp(const void *a, const void *b)
{
	const struct mmcclk *ma = a, *mb = b;

	if (ma->bus_clk_hz < mb->bus_clk_hz)
		return -1;
	if (ma->bus_clk_hz > mb->bus_clk_hz)
		return 1;
	return mb->clkdiv-ma->clkdiv;
}


static struct mmcclk *frequencies(int *n, int all)
{
	struct mmcclk mmc;
	struct mmcclk *clks = malloc(sizeof(struct mmcclk));
	int n_clks = 1;

	if (!clks) {
		perror("malloc");
		exit(1);
	}

	mmcclk_first(&mmc, 0,
	    MMCCLK_FLAG_WR_ONLY | (all ? MMCCLK_FLAG_ALL : 0));
	clks[0] = mmc;
	while (mmcclk_next(&mmc)) {
		clks = realloc(clks, sizeof(struct mmcclk)*(n_clks+1));
		if (!clks) {
			perror("realloc");
			exit(1);
		}
		clks[n_clks] = mmc;
		n_clks++;
	}

	qsort(clks, n_clks, sizeof(*clks), cmp);
	*n = n_clks;
	return clks;
}


static void print_freq(FILE *file, double f)
{
	const char *prefix = "";

	if (f >= 1000000) {
		f /= 1000000;
		prefix = "M";
	} else if (f >= 1000) {
		f /= 1000;
		prefix = "k";
	}
	fprintf(file, "%g %sHz", f, prefix);
}


static void show_frequencies(int quiet)
{
	const struct mmcclk *clks;
	int n, i;
	double last = 0;

	clks = frequencies(&n, 0);
	for (i = 0; i != n; i++) {
		if (quiet) {
			if (clks[i].bus_clk_hz != last)
				printf("%f\n", clks[i].bus_clk_hz);
			last = clks[i].bus_clk_hz;
		} else {
			printf("clkdiv = %u, clkrt = %u, bus_clk = ",
			    clks[i].clkdiv, clks[i].clkrt);
			print_freq(stdout, clks[i].bus_clk_hz);
			putchar('\n');
		}
	}
	free((void *) clks);
}


static int select_freq(struct mmcclk *res, int hz, int rel, int quiet, int all)
{
	const struct mmcclk *clks, *p, *best = NULL;
	double d, best_d = 0;
	int n;
	double err;

	clks = frequencies(&n, all);
	for (p = clks; p != clks+n; p++) {
		if (rel > 0 && p->bus_clk_hz < hz)
			continue;
		if (rel < 0 && p->bus_clk_hz > hz)
			continue;
		d = fabs(p->bus_clk_hz-hz);
		if (!best || d < best_d) {
			best = p;
			best_d = d;
		}
	}
	if (!best)
		return 0;
	*res = *best;
	free((void *) clks);

	if (quiet)
		return 1;

	if (res->bus_clk_hz != hz) {
		fprintf(stderr, "bus clk = ");
		print_freq(stderr, res->bus_clk_hz);
		err = (res->bus_clk_hz-hz)/hz;
		if (err <= -0.0001 || err >= 0.0001)
			fprintf(stderr, " (%+.2g%%)\n", err*100);
		else
			fprintf(stderr, " (%+d ppm)\n", (int) (err*1000000));
	}

	return 1;
}


/* ----- Pattern parser ---------------------------------------------------- */


static void *parse_pattern(const char *s, int *nibbles)
{
	uint8_t *buf = physmem_malloc(4095);	/* maximum block size */
	int n = 0;
	uint8_t v = 0;
	char *end;
	unsigned long i;

	memset(buf, 0, 4095);
	while (*s) {
		char ch[2] = { *s, 0 };

		v = strtoul(ch, &end, 16);
		if (*end) {
			fprintf(stderr, "\"%c\" is not a hex digit\n", *s);
			exit(1);
		}
		if (s[1] == '{') {
			i = strtoul(s+2, &end, 0);
			if (!*end) {
				fprintf(stderr, "unterminated range\n");
				exit(1);
			}
			if (*end != '}' || end == s+2) {
				fprintf(stderr, "invalid range \"%.*s\"\n",
				    end-s, s+1);
				exit(1);
			}
			s = end+1;
		} else {
			i = 1;
			s++;
		}
		while (i) {
			if (n == 8192-64-1) {
				fprintf(stderr, "pattern is too long\n");
				exit(1);
			}
			buf[n >> 1] |= v << 4*(~n & 1);
			n++;
			i--;
		}
	}
	/* pad to multiples of 32 bytes */
	while (n & 63) {
		buf[n >> 1] |= v << 4*(~n & 1);
		n++;
	}
	*nibbles = n;
	return buf;
}


static const char *load_pattern(const char *s)
{
	static char buf[20000]; /* more than enough :) */
	FILE *file;
	char *p = buf;
	int comment = 0;
	int c;

	if (!strcmp(s, "-")) {
		file = stdin;
	} else {
		file = fopen(s, "r");
		if (!file)
			return s;
	}
	while ((c = fgetc(file)) != EOF) {
		if (comment) {
			comment = c != '\n';
			continue;
		}
		if (c == '#') {
			comment = 1;
			continue;
		}
		if (isspace(c))
			continue;
		if (buf+sizeof(buf)-1 == p) {
			fprintf(stderr, "%s: file is too big\n", s);
			exit(1);
		}
		*p++ = c;
	}
	if (file != stdin)
		fclose(file);
	*p = 0;
	return buf;
}


/* ----- Real-time mode ---------------------------------------------------- */


void realtimize(void)
{
	struct sched_param prm;

	prm.sched_priority = sched_get_priority_max(SCHED_FIFO);
	if (prm.sched_priority < 0) {
		perror("sched_get_priority_max SCHED_FIFO");
		exit(1);
	}
	if (sched_setscheduler(0, SCHED_FIFO, &prm) < 0) {
		perror("sched_setscheduler SCHED_FIFO");
		exit(1);
	}
}


void unrealtime(void)
{
	struct sched_param prm = { .sched_priority = 0 };

	if (sched_setscheduler(0, SCHED_OTHER, &prm) < 0) {
		perror("sched_setscheduler SCHED_OTHER");
		exit(1);
	}
}


/* ----- DMA control ------------------------------------------------------- */


static uint32_t old_dmac;


static void dma_stop(void)
{
	DCS(DMA) =
	    DCS_TT |			/* Transfer terminated */
	    DCS_HLT;			/* DMA halt */
	DCS(DMA) = 0;			/* reset DMA channel */
}


static void dma_init(void)
{
	old_dmac = DMAC;

	DMAC = DMAC_DMAE; /* activate the DMA controller (in case it's off) */
	dma_stop();

	DCM(DMA) =
	    DCM_SAI |	/* source address increment */
	    (DCM_TSZ_32BYTE << DCM_TSZ_SHIFT);
			/* transfer size is 32 bytes */
	DRT(DMA) = DRT_MSC_TX;	/* MSC transmit-fifo-empty transfer request */
}


static void dma_cleanup(void)
{
	DMAC = old_dmac;
	dma_stop();
}


static void dma_setup(unsigned long buf, int nibbles)
{
	assert(!(nibbles & 63));

	DCS(DMA) = DCS_NDES;			/* no-descriptor transfer */
	DSA(DMA) = buf;				/* source */
	DTA(DMA) = REG_PADDR(MSC_TXFIFO);	/* MUST set this each time */
	DTC(DMA) = nibbles >> 6;		/* 32 bytes per transfer */
}


static void wait_dma_done(void)
{
	while (!(DCS(DMA) & DCS_TT));
}


/* ----- Send pattern using MSC and DMA ------------------------------------ */


static void wait_response(void)
{
	while (!(MSC_STAT & MSC_STAT_END_CMD_RES));
}


static void wait_fifo_empty(void)
{
	while (!(MSC_STAT & MSC_STAT_DATA_FIFO_EMPTY));
}


static void wait_shifted(const struct mmcclk *clk)
{
	/* 8 nibbles */
	double us = 8*1000000.0/clk->bus_clk_hz;

	usleep((int) us+1);
}


static void wait_trigger(const char *trigger, int debounce,
    const struct timespec *debounce_ns)
{
	struct timespec end, now;

	/*
	 * @@@ could also try to use POSIX per-process timers here. May be
	 * slightly cleaner but could increase deviations.
	 */
	while (*trigger) {
		while (PIN(UBB_CLK) != *trigger-'0');
		if (!debounce)
			goto next;
again:
		if (clock_gettime(CLOCK_REALTIME, &end)) {
			perror("clock_gettime");
			exit(1);
		}
		end.tv_sec += debounce_ns->tv_sec;
		end.tv_nsec += debounce_ns->tv_nsec;
		if (end.tv_nsec > 999999999) {
			end.tv_nsec -= 1000000000;
			end.tv_sec++;
		}
		while (PIN(UBB_CLK) == *trigger-'0') {
			if (clock_gettime(CLOCK_REALTIME, &now)) {
				perror("clock_gettime");
				exit(1);
			}
			if (now.tv_sec > end.tv_sec)
				goto next;
			if (now.tv_sec == end.tv_sec &&
			    now.tv_nsec >= end.tv_nsec)
				goto next;
		}
		goto again;
next:
		trigger++;
	}
}


static void mmc_buffer(const struct mmcclk *clk,
    uint8_t first, unsigned long buf, int nibbles, uint32_t mask,
    const char *trigger, int debounce, const struct timespec *debounce_ns,
    const struct timespec *wait_ns)
{
	/*
	 * If under control of the MMC controller, DATx tri-state until we
	 * actually send data. That's why they have been set up as GPIOs and
	 * we'll only switch them to function when the MMC controller is in a
	 * well-defined state.
	 */

	dma_setup(buf, nibbles);

	MSC_STRPCL = MSC_STRPCRL_START_CLOCK;	/* start the bus clock */
	MSC_RESTO = MSC_RESTO_MASK;	/* maximum response time-out */
	MSC_BLKLEN = MSC_BLKLEN_MASK;	/* never reach the end (with CRC) */

	MSC_CMDAT =
	    MSC_CMDAT_BUS_WIDTH_4 << MSC_CMDAT_BUS_WIDTH_SHIFT |
	    MSC_CMDAT_DMA_EN |		/* DMA */
	    MSC_CMDAT_WRITE_READ |	/* write */
	    MSC_CMDAT_DATA_EN |		/* with data transfer */
	    MSC_CMDAT_RESPONSE_FORMAT_R1; /* R1 response */

        MSC_STRPCL = MSC_STRPCRL_START_OP;

	/*
	 * Make sure we've reached the end of the command and then send the
	 * first pattern (eight times, since this is the smallest amount we
	 * can send.
	 */

	wait_response();

	MSC_TXFIFO = first*0x11111111;

	wait_fifo_empty();
	wait_shifted(clk);

	/*
	 * Since the transfer (of nominally 4095 bytes) is not done yet, the
	 * MMC controller will hold the bus at the last value sent. It's now
	 * safe to switch from GPIO to function.
	 */

	PDFUNS = mask;

	realtimize();

	if (trigger)
		wait_trigger(trigger, debounce, debounce_ns);
	if (wait_ns->tv_sec || wait_ns->tv_nsec)
		if (nanosleep(wait_ns, NULL))
			perror("nanosleep");

	/*
	 * Send the pattern with DMA. Note that we still have to send the first
	 * pattern, since the static state we begin from may not have been
	 * present long enough.
	 */

	DCS(DMA) =
	    DCS_NDES |		/* no descriptor */
	    DCS_CTE;		/* enable channel */

	unrealtime();

	wait_dma_done();
	wait_fifo_empty();
	wait_shifted(clk);

	/*
	 * We're done. As far as the MMC controller is concerned, the transfer
	 * is still not finished (i.e., we haven't sent 4095 bytes) and will
	 * therefore just hold the bus. We can now return the bus to GPIO.
	 * This form of handover also prevents the MMC controller from sending
	 * a CRC, which may confuse the recipient of the pattern.
	 */
}


static void send_buffer(const struct mmcclk *clk,
    const uint8_t *buf, int nibbles, uint32_t mask,
    const char *trigger, int debounce, const struct timespec *debounce_ns,
    const struct timespec *wait_ns)
{
	struct physmem_vec vec;
	int n;

	if (physmem_flush(buf, nibbles)) {
		perror("physmem_flush");
		exit(1);
	}

	n = physmem_xlat(buf, nibbles >> 1, &vec, 1);
	if (n < 0) {
		perror("physmem_xlat_vec");
		exit(1);
	}
	if (n != 1) {
		fprintf(stderr, "physmem_xlat_vec: expected 1, got %d\n", n);
		exit(1);
	}
	mmc_buffer(clk, buf[0] >> 4, vec.addr, nibbles, mask,
	    trigger, debounce, debounce_ns, wait_ns);
}


static void dma_pattern(const struct mmcclk *clk,
    const char *pattern, uint32_t mask, const char *trigger,
    int debounce, const struct timespec *debounce_ns,
    const struct timespec *wait_ns)
{
	const uint8_t *buf;
	int n;

	if (!*pattern) {
		fprintf(stderr, "pattern is empty\n");
		exit(1);
	}
	buf = parse_pattern(pattern, &n);

	if (mlockall(MCL_CURRENT | MCL_FUTURE)) {
		perror("mlockall");
		exit(1);
	}

	if (trigger) {
		PDFUNC = UBB_CLK;
		IN(UBB_CLK);
	}

	dma_init();

	/* Initial static state: the first pattern. */

	PDFUNS = UBB_CMD;
	PDDATC = ~((buf[0] >> 4) << 10) & mask;
	PDDATS = (buf[0] >> 4) << 10;
	PDDIRS = mask;

	send_buffer(clk, buf, n, mask,
	    trigger, debounce, debounce_ns, wait_ns);

	/* Final static state: the last pattern. */

	PDDATC = ~((buf[(n >> 1)-1] & 0xf) << 10) & mask;
	PDDATS = (buf[(n >> 1)-1] & 0xf) << 10;

	PDFUNC = mask;

	dma_cleanup();
}


/* ----- Command-line processing ------------------------------------------- */


static int frequency(const char *s, int *hz, int *rel)
{
	char *end;
	double f;

	f = strtod(s, &end);

	if (end == s || f < 0)
		return 0;

	switch (*end) {
	case 'M':
	case 'm':
		*hz = f*1000000;
		end++;
		break;
	case 'K':
	case 'k':
		*hz = f*1000;
		end++;
		break;
	default:
		*hz = f;
		break;
	}

	if ((end[0] == 'H' || end[0] == 'h') &&
	    (end[1] == 'Z' || end[1] == 'z'))
		end += 2;

	switch (*end) {
	case '+':
		*rel = 1;
		end++;
		break;
	case '-':
		*rel = -1;
		end++;
		break;
	default:
		*rel = 0;
		break;
	}

	return !*end;
}


static int duration(const char *s, double *res, int *rel)
{
	char *end;
	double d;

	d = strtod(s, &end);

	if (end == s || d < 0)
		return 0;

	switch (*end) {
	case 'M':
	case 'm':
		d /= 1e3;
		end++;
		break;
	case 'U':
	case 'u':
		d /= 1e6;
		end++;
		break;
	case 'N':
	case 'n':
		d /= 1e9;
		end++;
		break;
	default:
		break;
	}

	if (*end == 'S' || *end == 's')
		end++;

	switch (*end) {
	case '+':
		*rel = 1;
		end++;
		break;
	case '-':
		*rel = -1;
		end++;
		break;
	default:
		*rel = 0;
		break;
	}

	if (*end)
		return 0;

	*res = d;

	return 1;
}


static int duration_timespec(const char *s, struct timespec *res, int *rel)
{
	double d;

	if (!duration(s, &d, rel))
		return 0;
	res->tv_sec = d;
	res->tv_nsec = (d-res->tv_sec)*1e9;
	return 1;
}


static int interval(const char *s, int *hz, int *rel)
{
	double d;

	if (!duration(s, &d, rel))
		return 0;
	*hz = 1/d;
	*rel = -*rel;
	return 1;
}


static void usage(const char *name)
{
        fprintf(stderr,
"usage: %s\n"
"       %s [-q] (-f|-F) freq_hz|(-i|-I) interval_s\n"
"       %s [-q] [(-f|-F) freq_hz|(-i|-I) interval_s] -c [active_s]\n"
"       %s [-q] [(-f|-F) freq_hz|(-i|-I) interval_s]\n"
"         [-C|-t 0|1... [-d debounce_s]] [-w wait_s] [-m mask] [-p]\n"
"         file|pattern\n\n"
"  -c          output bus clock on CLK without sending a pattern\n"
"  -C          temporarily output bus clock on CLK (for debugging)\n"
"  -d deb_s    trigger debounce time (default: no debouncing)\n"
"  -f freq_hz  set bus clock to the specified frequency (default: 1 MHz)\n"
"  -F freq_hz  like -f, but also allow \"overclocking\"\n"
"  -i inter_s  set bus clock such that one cycle equals the specified "
  "interval\n"
"  -I inter_s  like -i, but also allow \"overclocking\"\n"
"  -m mask     use only the DATx lines specified in the mask (default: 0xf)\n"
"  -p          force interpretation of argument as pattern (and not file)\n"
"  -q          quiet. Don't pretty-print frequencies; don't report clock\n"
"              differences.\n"
"  -t 0|1...   start pattern when trigger/CLK has passed through the sequence\n"
"              (default: start pattern immediately)\n"
"  -w wait_s   wait between trigger and sending the pattern\n\n"
"  active_s    keep running that many seconds after setting the clock\n"
"              (default: exit immediately but leave the clock on)\n"
"  file        file containing the pattern\n"
"  pattern     send the specified pattern on DAT0 through DAT3\n\n"
"Frequency: the frequency in hertz, optionally followed by \"M\" or \"k\",\n"
"  optionally followed by \"Hz\", optionally followed by \"+\" or \"-\".\n"
"  \"+\" selects a frequency >= the specified one, \"-\" one <=.\n"
"  Without +/-, the closest available frequency is selected.\n"
"Duration: the duration in seconds, optionally followed by \"m\", \"u\", or\n"
"  \"n\", optionally followed by \"s\", optionally followed by \"+\" or \"-\"."
  "\n"
"Pattern: hex digits corresponding to 1 for DAT0, 2 for DAT1, etc.\n"
"  {n} repeats the preceding digit n times, e.g., 1{3} is equivalent to 111.\n"
    , name, name, name, name);
	exit(1);
}


int main(int argc, char **argv)
{
	struct mmcclk clk;
	int all = 0;
	int bus_hz = 0, clk_only = 0, clkout = 0, bus_rel = 0;
	const char *pattern = NULL;
	int quiet = 0, force_pattern = 0;
	struct timespec active_ns;
	int active_rel;
	int keep_clk = 1;
	uint8_t mask = 0xf;
	const char *trigger = NULL;
	struct timespec debounce_ns;
	int debounce = 0, debounce_rel;
	struct timespec wait_ns = { 0, 0 };
	int wait_rel;
	char *end;
	int c;
	unsigned long tmp;
	const char *p;

	while ((c = getopt(argc, argv, "cCd:f:F:i:I:m:pqt:w:")) != EOF)
		switch (c) {
		case 'c':
			clk_only = 1;
			break;
		case 'C':
			clkout = 1;
			break;
		case 'd':
			if (!duration_timespec(optarg,
			    &debounce_ns, &debounce_rel))
				usage(*argv);
			if (debounce_rel < 0)
				usage(*argv);
			debounce = 1;
			break;
		case 'F':
			all = 1;
			/* fall through */
		case 'f':
			if (!frequency(optarg, &bus_hz, &bus_rel))
				usage(*argv);
			break;
		case 'I':
			all = 1;
			/* fall through */
		case 'i':
			if (!interval(optarg, &bus_hz, &bus_rel))
				usage(*argv);
			break;
		case 'm':
			tmp = strtoul(optarg, &end, 0);
			if (*end)
				usage(*argv);
			if (tmp & ~0xfUL) {
				fprintf(stderr, "mask is too large\n");
				exit(1);
			}
			mask = tmp;
			break;
		case 'p':
			force_pattern = 1;
			break;
		case 'q':
			quiet = 1;
			break;
		case 't':
			trigger = optarg;
			if (!*trigger)
				usage(*argv);
			for (p = trigger; *p; p++)
				if (*p != '0' && *p != '1')
					usage(*argv);
			break;
		case 'w':
			if (!duration_timespec(optarg,
			    &wait_ns, &wait_rel))
				usage(*argv);
			if (wait_rel < 0)
				usage(*argv);
			break;
		default:
			usage(*argv);
		}

	if (clkout && clk_only)
		usage(*argv);
	if ((clkout || clk_only) && trigger)
		usage(*argv);

	switch (argc-optind) {
	case 0:
		if (clk_only)
			break;
		if (clkout || force_pattern || trigger)
			usage(*argv);

		ubb_open(UBB_ALL);
		if (bus_hz) {
			if (!select_freq(&clk, bus_hz, bus_rel, quiet, all)) {
				fprintf(stderr,
				    "no suitable frequency found\n");
				exit(1);
			}
			printf("%f\n", clk.bus_clk_hz);
		} else {
			show_frequencies(quiet);
		}
		return 0;
	case 1:
		if (clk_only) {
			if (force_pattern)
				usage(*argv);
			if (!duration_timespec(argv[optind],
			    &active_ns, &active_rel))
				usage(*argv);
			if (active_rel < 0)
				usage(*argv);
			keep_clk = 0;
		} else {
			pattern = argv[optind];
		}
		break;
	default:
		usage(*argv);
	}

	if (pattern && !force_pattern)
		pattern = load_pattern(pattern);

	ubb_open(UBB_ALL);

	PDFUNS = UBB_CMD;

	if (!bus_hz)
		bus_hz = 1000000;

	if (!select_freq(&clk, bus_hz, bus_rel, quiet, all)) {
		fprintf(stderr, "no suitable frequency found\n");
		exit(1);
	}

	if (clkout || clk_only)
		PDFUNS = UBB_CLK;
	mmcclk_start(&clk);

	if (pattern)
		dma_pattern(&clk, pattern, mask << 10,
		    trigger, debounce, &debounce_ns, &wait_ns);

	if (!keep_clk)
		if (nanosleep(&active_ns, NULL))
			perror("nanosleep");
	if (pattern) {
		mmcclk_stop();
		ubb_close(mask << 10 | (trigger ? UBB_CLK : 0));
	} else if (keep_clk) {
		ubb_close(UBB_CLK);
	} else {
		mmcclk_stop();
		ubb_close(0);
	}

	return 0;
}