/*
 * 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 <string.h>
#include <math.h>
#include <time.h>
#include <assert.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)
{
	struct mmcclk mmc;
	struct mmcclk *clks = malloc(sizeof(struct mmcclk));
	int n_clks = 1;

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

	mmcclk_first(&mmc, 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(void)
{
	const struct mmcclk *clks;
	int n, i;

	clks = frequencies(&n);
	for (i = 0; i != n; i++) {
		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)
{
	const struct mmcclk *clks, *p, *best = NULL;
	double d, best_d = 0;
	int n;

	clks = frequencies(&n);
	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);
	return 1;
}


/* ----- DMA the pattern --------------------------------------------------- */


static uint32_t old_dmac;


static void dma_stop(void)
{
	DCS(DMA) = (1 << 3) | (1 << 2);	/* halt DMA channel */
	DCS(DMA) = 0;			/* reset DMA channel */
}


static void dma_init(void)
{
	old_dmac = DMAC;

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

	DCM(DMA) =
	    (1 << 23) |	/* source address increment */
	    (4 << 8);	/* transfer size is 32 bytes */
	DRT(DMA) = 26;	/* 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) = 1 << 31;			/* 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) >> 3) & 1));	/* DCS.TT */
}


static void wait_response(void)
{
	while (!((MSC_STAT >> 11 ) & 1)); /* MSC_STAT.END_CMD_RES */
}


static void wait_fifo_empty(void)
{
	while (!((MSC_STAT >> 6 ) & 1)); /* 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 mmc_buffer(const struct mmcclk *clk,
    uint8_t first, unsigned long buf, int nibbles, uint32_t mask)
{
	MSC_STRPCL = 1 << 3;	/* reset the MSC */

	while (MSC_STAT & (1 << 15));	/* wait until reset finishes */

	dma_setup(buf, nibbles);

	MSC_CLKRT = clk->clkrt;	/* cleared by MSC reset */
	MSC_STRPCL = 2;		/* start the bus clock */
	MSC_RESTO = 0xffff;	/* maximum response time-out */
	MSC_BLKLEN = 0xfff;	/* never reach the end (with CRC) */

	MSC_CMDAT =
	    (2 << 9) |		/* 4 bit bus */
	    (1 << 8) |		/* DMA */
	    (1 << 4) |		/* write */
	    (1 << 3) |		/* with data transfer */
	    1;			/* R1 response */

        MSC_STRPCL = 4;         /* START_OP */

	wait_response();

	MSC_TXFIFO = first*0x11111111;

	wait_fifo_empty();
	wait_shifted(clk);

	PDFUNS = mask;

	DCS(DMA) =
	    (1 << 31) |         /* no descriptor */
	    1;			/* enable transfer */

	wait_dma_done();

	wait_fifo_empty();
	wait_shifted(clk);
}


static void send_buffer(const struct mmcclk *clk,
    const uint8_t *buf, int nibbles, uint32_t mask)
{
	unsigned long phys;

	phys = physmem_xlat((void *) buf);
	mmc_buffer(clk, buf[0] >> 4, phys, nibbles, mask);
}


static int dma_pattern(const struct mmcclk *clk,
    const char *pattern, uint32_t mask)
{
	int n = strlen(pattern);
	int rounded = (n+63) & ~63;
	uint8_t *buf = physmem_malloc(rounded >> 1);
	int i;

	if (!n)
		return 1;

	memset(buf, 0, rounded);
	for (i = 0; i != rounded; i++) {
		char ch[2] = { pattern[i < n ? i : n-1], 0 };
		char *end;

		buf[i >> 1] |= strtoul(ch, &end, 16) << 4*(~i & 1);
		if (*end)
			return 0;
	}

	dma_init();

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

	send_buffer(clk, buf, rounded, mask);

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

	PDFUNC = mask;

	dma_cleanup();

	return 1;
}


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


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

	f = strtod(s, &end);

	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 void usage(const char *name)
{
        fprintf(stderr,
"usage: %s\n"
"usage: %s [-b freq_hz] [-f freq_hz] [-c] [-q] [pattern] active_s\n\n"
"  -b freq_hz  set bus clock to the specified frequency (default: 1 MHz)\n"
"  -c          output bus clock on CLK\n"
"  -f freq_hz  set pattern rate (default: same as bus clock)\n"
"  -q          quiet. Don't report clock differences.\n\n"
"  active_s    keep running that many seconds after setting the clock\n"
"  pattern     send the specified pattern on DAT0 through DAT3\n\n"
"Frequency: the frequency in Hz, 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"
"Pattern: hex digits corresponding to 1 for DAT0, 2 for DAT1, etc.\n"
    , name, name);
	exit(1);
}


int main(int argc, char **argv)
{
	struct mmcclk clk;
	int bus_hz = 0, clkout = 0, bus_rel = 0;
	int pattern_hz = 0, pattern_rel = 0;
	const char *pattern = NULL;
	int quiet = 0;
	double active_s;
	struct timespec active_ns;
	char *end;
	int c;

	while ((c = getopt(argc, argv, "b:cq")) != EOF)
		switch (c) {
		case 'b':
			if (!frequency(optarg, &bus_hz, &bus_rel))
				usage(*argv);
			break;
		case 'f':
			if (!frequency(optarg, &pattern_hz, &pattern_rel))
				usage(*argv);
			break;
		case 'c':
			clkout = 1;
			break;
		case 'q':
			quiet = 1;
			break;
		default:
			usage(*argv);
		}

	switch (argc-optind) {
	case 0:
		if (clkout || quiet)
			usage(*argv);
		ubb_open(UBB_ALL);
		show_frequencies();
		return 1;
	case 2:
		pattern = argv[optind];
		/* fall through */
	case 1:
		active_s = strtod(argv[argc-1], &end);
		if (*end)
			usage(*argv);
		active_ns.tv_sec = (int) active_s;
		active_ns.tv_nsec = (active_s-(int) active_s)*1e9;
		break;
	default:
		usage(*argv);
	}

	ubb_open(UBB_ALL);

	PDFUNS = UBB_CMD;

	if (!bus_hz)
		bus_hz = 1000000;

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

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

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

	if (pattern)
		if (!dma_pattern(&clk, pattern,
		    UBB_DAT0 | UBB_DAT1 | UBB_DAT2 | UBB_DAT3))
			usage(*argv);

	if (nanosleep(&active_ns, NULL))
		perror("nanosleep");

	mmcclk_stop();

	ubb_close(UBB_DAT0 | UBB_DAT1 | UBB_DAT2 | UBB_DAT3);

	return 0;
}