/*
 * (C) Copyright 2018 Leo C. <erbl259-lmu@yahoo.de>
 *
 * SPDX-License-Identifier:	GPL-2.0
 */

#include "cmd_cpu.h"
//#include <ctype.h>
#include <util/atomic.h>

#include "z80-if.h"
#include "con-utils.h"
#include "env.h"
#include "eval_arg.h"
#include "timer.h"
#include "getopt-min.h"
#include "debug.h"

/* hack to get Z180 loadfile into flash memory */
#define const const FLASH
#include "../z180/cpuinfo.h"
#undef const

#define DEBUG_CPU	1	/* set to 1 to debug */

#define debug_cpu(fmt, args...)		                                              \
	debug_cond(DEBUG_CPU, fmt, ##args)


/*
 * delay for <count> ms...
 */
static void	test_delay(uint32_t count)
{
	uint32_t ts = get_timer(0);

	while (get_timer(ts) <= count);
}

static uint32_t z80_measure_phi(uint_fast8_t cycles)
{
	uint16_t ref_stop;
	uint16_t ref_ovfl;
	uint8_t x_ovfl;
	uint32_t x_freq;


	PRR1 &= ~_BV(PRTIM3);
	TCCR3A = 0;
	TCCR3B = 0b000<<CS30;	/* stop counter */
	TCNT3 = 0;
	x_ovfl = 0;
	TIFR3 = _BV(TOV3);
	ref_ovfl = 0;

	ATOMIC_BLOCK(ATOMIC_FORCEON) {
		/* Reset pending int */
		EIFR = _BV(INTF6);
		/* Wait for falling edge */
		while ((EIFR & _BV(INTF6)) == 0)
			;
		TCCR3B = 0b110<<CS30;	/* Count falling edges on T3 (==INT6) */
		OCR4B = TCNT4;
		TIFR4 = _BV(OCF4B);		/* clear compare match flag */
	}
	while (ref_ovfl < 60) {
		ATOMIC_BLOCK(ATOMIC_FORCEON) {
			if ((TIFR4 & _BV(OCF4B)) != 0) {
				TIFR4 = _BV(OCF4B);
				ref_ovfl++;
			}
			if ((TIFR3 & _BV(TOV3)) != 0) {
				TIFR3 = _BV(TOV3);
				x_ovfl++;
			}
		}
	}

	ATOMIC_BLOCK(ATOMIC_FORCEON) {
		EIFR = _BV(INTF6);
		for (;;) {
			if (EIFR & _BV(INTF6)) {
				TCCR3B = 0b000<<CS30;	/* stop counter */
				ref_stop = TCNT4;
				break;
			}
			if ((TIFR4 & _BV(OCF4B)) != 0) {
				TIFR4 = _BV(OCF4B);
				ref_ovfl++;
			}
		}
	}

	if ((TIFR3 & _BV(TOV3)) != 0) {
		TIFR3 = _BV(TOV3);
		x_ovfl++;
	}

	uint32_t ref_cnt = (ref_stop - OCR4B) + ((uint32_t)ref_ovfl << 16);
	uint32_t x_cnt = TCNT3 + ((uint32_t) x_ovfl << 16);
	uint64_t x_tmp = (uint64_t) 100000 * (x_cnt * cycles);

	debug_cpu("TCNT3: %6u, ref_cnt: %9lu\n", TCNT3, ref_cnt);
	debug_cpu("x_tmp: %lu %lu\n", (uint32_t) (x_tmp >> 32), (uint32_t) (x_tmp & 0xffffffff));

	x_tmp = (x_tmp * getenv_ulong(PSTR(ENV_FMON), 10, F_CPU) + (ref_cnt / 2)) / ref_cnt;

	debug_cpu("x_tmp: %lu %lu\n", (uint32_t) (x_tmp >> 32), (uint32_t) (x_tmp & 0xffffffff));

	/* round to 5 decimal digits */
	int_fast8_t sc = 5;
	while (sc > 0 || x_tmp >= 100000) {
		x_tmp = (x_tmp + 5)/10;
	    sc--;
	}
	x_freq = x_tmp;
	while (sc < 0) {
		x_freq *= 10;
	    sc++;
	}

	/* Stop Timer */
	TCCR3B = 0;
	PRR1 |= _BV(PRTIM3);

	return x_freq;
}

static const FLASH uint8_t loop_code[] = {
/* 0000 */  0x00,             /* nop                                        */
/* 0001 */  0xAF,             /* xor  a                                     */
/* 0005 */  0xD3,0x32,        /* out  (032h),a  ;DCNTL                      */
/* 0002 */  0xD3,0x36,        /* out  (036h),a  ;RCR                        */
/*      */                    /*                                            */
/* 0006 */  0xD3,0x40,        /* out  (040H),a  ;Ready                      */
/*      */                    /*                                            */
/*      */                    /*                ;Z80    Z180(0W) Z180(MaxW) */
/* 0008 */                    /* loop:          ;-------------------------- */
/* 0008 */  0xDB,0x50,        /* in   a,(050h)  ;11       10     +3*3  19   */
/* 000A */  0xC3,0x08,0x00    /* jp   loop      ;10        9     +3*3  18   */
                              /*                ;-------------------------- */
                              /*                ;21       19           37   */
};

command_ret_t do_cpu_freq(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int argc, char * const argv[])
{

#define O_SILENT		(1<<0)
#define O_WENV			(1<<1)
#define O_LOAD_LOOP     (1<<2)
#define O_UNLOAD_LOOP   (1<<3)

	uint_fast8_t options = O_LOAD_LOOP | O_UNLOAD_LOOP;
	uint_fast8_t lcycles = 19;
	uint16_t timeout = 1000;
	uint8_t eimsk_save;

	uint8_t mem_save[ARRAY_SIZE(loop_code)];

	int opt;
	while ((opt = getopt(argc, argv, PSTR("swnuc:t:"))) != -1) {
		switch (opt) {
		case 's':
			options |= O_SILENT;
			break;
		case 'w':
			options |= O_WENV;
			break;
		case 'n':
			options &= ~O_LOAD_LOOP;
			break;
		case 'u':
			options &= ~O_UNLOAD_LOOP;
			break;
		case 'c':
			lcycles = eval_arg(optarg, NULL);
			break;
		case 't':
			timeout = eval_arg(optarg, NULL);
			break;
		default: /* '?' */
			return CMD_RET_USAGE;
		}
	}
	if (argc - optind != 0)
		return CMD_RET_USAGE;

	if (z80_bus_state() & ZST_RUNNING) {
		if (!(options & O_SILENT))
			printf_P(PSTR("Frequency measuring failed. CPU allready running!\n"));
		return CMD_RET_FAILURE;
	}

	ATOMIC_BLOCK(ATOMIC_FORCEON) {
		/* Save state and disable INT5/INT6 */
		eimsk_save = EIMSK;
		EIMSK &= ~_BV(INT6);
		EIMSK &= ~_BV(INT5);
	}

	z80_bus_cmd(Request);
	if (options & O_LOAD_LOOP) {
		z80_read_block(mem_save, 0, ARRAY_SIZE(loop_code));
		z80_write_block_P(loop_code, 0, ARRAY_SIZE(loop_code));
	}
	EIFR = _BV(INTF5);				/* Reset pending int */
	z80_bus_cmd(Release);
	z80_bus_cmd(Run);

	clear_ctrlc();					/* forget any previous Control C */
	ERRNUM err = 0;

	/* Wait for falling edge */
	do {
		/* check for ctrl-c to abort... */
		if (had_ctrlc() || ctrlc()) {
			err = EINTR;
			break;
		}
	} while ((EIFR & _BV(INTF5)) == 0);

	uint32_t cpu_freq = 0;
	if (!err)
		cpu_freq = z80_measure_phi(lcycles);

	z80_bus_cmd(Reset);
	if (options & O_UNLOAD_LOOP) {
		z80_bus_cmd(Request);
		z80_write_block(mem_save, 0, ARRAY_SIZE(loop_code));
		z80_bus_cmd(Release);
	}
	ATOMIC_BLOCK(ATOMIC_FORCEON) {
		/* Restore INT5/INT6 */
		if ((eimsk_save & _BV(INT5)) != 0)
			EIMSK |= _BV(INT5);
		if ((eimsk_save & _BV(INT6)) != 0)
			EIMSK |= _BV(INT6);
		/* Reset pending int */
		EIFR = _BV(INTF5);
		EIFR = _BV(INTF6);
	}

	Stat &= ~S_MSG_PENDING;
	Stat &= ~S_CON_PENDING;

	if (err)
		cmd_error(CMD_RET_FAILURE, err, NULL);

	if (!(options & O_SILENT)) {
		uint8_t sc = cpu_freq >> 28;
		printf_P(PSTR("%lu %3u\n"), cpu_freq & 0x0fffffff, sc);
	}
#if 0
	if (options & O_WENV) {
		if (setenv_ulong(PSTR(ENV_CPU_FREQ), cpu_freq)) {
			if (!(options & O_SILENT))
				printf_P(PSTR("'SETENV (%S, %lu)' failed!\n"), PSTR(ENV_CPU_FREQ), cpu_freq);
			return CMD_RET_FAILURE;
		}
	}
#endif
	return CMD_RET_SUCCESS;
}

static const FLASH char * const FLASH cpu_strings[] = {
	FSTR("Unknown"),
	FSTR("8080"),
	FSTR("8085"),
	FSTR("Z80"),
	FSTR("x180"),
	FSTR("HD64180"),
	FSTR("Z80180"),
	FSTR("Z80S180"),
};

command_ret_t do_cpuchk(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int argc UNUSED, char * const argv[] UNUSED)
{
	uint_fast8_t cputype = 0;
	ERRNUM err = ESUCCESS;
	uint8_t eimsk_save;
	uint8_t ram_save[cpuinfo_length];

	if (z80_bus_state() & ZST_RUNNING) {
		err = ERUNNING;
	} else {
		z80_bus_request_or_exit();
		z80_read_block(ram_save, 0, cpuinfo_length);
		z80_load_mem(0, cpuinfo,
					&cpuinfo_sections,
					cpuinfo_address,
					cpuinfo_length_of_sections);
		z80_bus_cmd(Release);

		if (argv[1] && (argv[1][0] == 'n'))
			goto donot;

		ATOMIC_BLOCK(ATOMIC_FORCEON) {
			/* Save state and disable INT5/INT6 */
			eimsk_save = EIMSK;
			EIMSK &= ~_BV(INT6);
			EIMSK &= ~_BV(INT5);
		}
		EIFR = _BV(INTF5);				/* Reset pending int */
		z80_bus_cmd(Run);

		clear_ctrlc();		/* forget any previous Control C */
		do {
			/* check for ctrl-c to abort... */
			if (had_ctrlc() || ctrlc()) {
				err = EINTR;
				break;
			}
		} while ((EIFR & _BV(INTF5)) == 0);
		z80_bus_cmd(Reset);
		ATOMIC_BLOCK(ATOMIC_FORCEON) {
			/* Restore INT5/INT6 */
			if ((eimsk_save & _BV(INT5)) != 0)
				EIMSK |= _BV(INT5);
			if ((eimsk_save & _BV(INT6)) != 0)
				EIMSK |= _BV(INT6);
			/* Reset pending int */
			EIFR = _BV(INTF5);
			EIFR = _BV(INTF6);
		}
		Stat &= ~S_MSG_PENDING;
		Stat &= ~S_CON_PENDING;
		z80_bus_cmd(Request);
		if (z80_read(3) == 0xFF) {
			cputype = z80_read(4);
		}
		z80_write_block(ram_save, 0, cpuinfo_length);
		z80_bus_cmd(Release);
	}

donot:

	if (err)
		cmd_error(CMD_RET_FAILURE, err, NULL);

	if (cputype >= ARRAY_SIZE(cpu_strings))
		cputype = 0;
	printf_P(PSTR("Detected CPU: %S\n"), cpu_strings[cputype]);

	return CMD_RET_SUCCESS;
}

command_ret_t do_cpu_test(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int argc, char * const argv[])
{

	uint32_t pulsewidth = 10; /* ms */

	int opt;
	while ((opt = getopt(argc, argv, PSTR("t:"))) != -1) {
		switch (opt) {
		case 't':
			pulsewidth = eval_arg(optarg, NULL);
			break;
		default: /* '?' */
			return CMD_RET_USAGE;
		}
	}

	if ((z80_bus_state() & ZST_ACQUIRED) != RESET)
		cmd_error(CMD_RET_FAILURE, ERUNNING, NULL);

	clear_ctrlc();		/* forget any previous Control C */
	do {
		z80_bus_cmd(Request);
		test_delay(pulsewidth);
		z80_bus_cmd(Release);
		test_delay(pulsewidth);
	} while (!(had_ctrlc() || ctrlc()));

	return CMD_RET_SUCCESS;
}

command_ret_t do_bus_test(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int argc UNUSED, char * const argv[] UNUSED)
{
	int ch;

#if 0
	int opt;
	while ((opt = getopt(argc, argv, PSTR("t:"))) != -1) {
		switch (opt) {
		case 't':
			pulsewidth = eval_arg(optarg, NULL);
			break;
		default: /* '?' */
			return CMD_RET_USAGE;
		}
	}
#endif

	my_puts_P(PSTR(
		"  1:  RESET         4:  RUN           r:  Toggle /RESET\n"
		"  2:  REQUEST       5:  RESTART       b:  Toggle /BUSREQ\n"
		"  3:  RELEASE       6:  M_CYCLE\n"
		"\n"
		//"Bus state: "
	));

	do {
		ch = my_getchar(1);
		if (ch >= 0) {
			switch (ch) {
				case '1':		/* bus_cmd RESET   */
				case '2':		/* bus_cmd REQUEST */
				case '3':		/* bus_cmd RELEASE */
				case '4':		/* bus_cmd RUN     */
				case '5':		/* bus_cmd RESTART */
				case '6':		/* bus_cmd M_CYCLE */
						z80_bus_cmd(ch - '1' + Reset);
						break;
				case 'r':		/* Toggle RESET    */
						z80_toggle_reset();
						break;
				case 'b':		/* Toggle BUSREQ   */
						z80_toggle_busreq();
						break;
			}
			test_delay(10);
			uint32_t cycles = z80_get_busreq_cycles();
			printf_P(PSTR("\rState: %.2x, cycles: %lu, time: %luus      "),
					z80_bus_state(), cycles, (uint32_t) (cycles * 1000000LL / F_CPU));
		}
	} while (ch != 0x03);

	putchar('\n');
	return CMD_RET_SUCCESS;
}

command_ret_t do_busack_test(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int argc UNUSED, char * const argv[] UNUSED)
{

	if ((z80_bus_state() & ZST_ACQUIRED) != RESET)
		cmd_error(CMD_RET_FAILURE, ERUNNING, NULL);

	z80_bus_cmd(Request);
	uint32_t result = z80_get_busreq_cycles();
	test_delay(20);
	z80_bus_cmd(Release);

#if 0
	long div;

	pinconf = gpio_config_get(pin);
	if (pinconf == OUTPUT_TIMER) {
		div = gpio_clockdiv_get(pin);
	}
#endif


	printf_P(PSTR("cycles: %lu, time: %luus\n"), result, (uint32_t) (result * 1000000LL / F_CPU));

	return CMD_RET_SUCCESS;
}


/*
 * command table for subcommands
 */

cmd_tbl_t cmd_tbl_cpu[] = {
CMD_TBL_ITEM(
	chkcpu,	CONFIG_SYS_MAXARGS,	CTBL_RPT|CTBL_SUBCMDAUTO, do_cpuchk,
	"Check/Identify CPU",
	""
),
CMD_TBL_ITEM(
	buscmd,	CONFIG_SYS_MAXARGS,	CTBL_RPT,	do_bus_test,
	"Bus commands",
	""
),
CMD_TBL_ITEM(
	test,	CONFIG_SYS_MAXARGS,	1,	do_cpu_test,
	"Do bus request/release cycles",
	"[-t pulsewidth]"
),
CMD_TBL_ITEM(
	busack,	2,	1,	do_busack_test,
	"Get time from /Reset high to /BUSACK low",
	""
),
CMD_TBL_ITEM(
	freq,	CONFIG_SYS_MAXARGS,	1,	do_cpu_freq,
	"Measure cpu frequency",
	"[-qwn] [-c loopcycles] [-t timeout]\n"
	"     -q Be quiet\n"
//	"     -w  Write result to environment variable '"ENV_CPU_FREQ"'"
),

CMD_TBL_ITEM(
	help,	CONFIG_SYS_MAXARGS,	CTBL_RPT,	do_help,
	"Print sub command description/usage",
	"\n"
	"       - print brief description of all sub commands\n"
	"fat help command ...\n"
	"       - print detailed usage of sub cmd 'command'"
),

/* This does not use the CMD_TBL_ITEM macro as ? can't be used in symbol names */
	{FSTR("?"),   CONFIG_SYS_MAXARGS, 1, do_help,
	 NULL,
#ifdef  CONFIG_SYS_LONGHELP
	FSTR(""),
#endif /* CONFIG_SYS_LONGHELP */
	NULL,
#ifdef CONFIG_AUTO_COMPLETE
	NULL,
#endif
},
/* Mark end of table */
CMD_TBL_END(cmd_tbl_cpu)
};


command_ret_t do_cpu(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int argc UNUSED, char * const argv[] UNUSED)
{
	//puts_P(PSTR("Huch?"));

	return CMD_RET_USAGE;
}


#if 0		/* Z180 Single Step Functions */
/*
 * Z180 Single Step Functions
 *
 */


#define P_RUN		PORTG
#define RUN			1
#define DDR_RUN		DDRG
#define P_STEP		PORTG
#define STEP		0
#define DDR_STEP	DDRG
#define P_WAIT		PORTG
#define WAIT		2
#define DDR_WAIT	DDRG
/* All three signals are on the same Port (PortG) */
#define PORT_SS		PORTG
#define DDR_SS		DDRG
#define PIN_SS		PING

static bool ss_available;

int single_step_setup(void)
{
	ss_available = false;

#if 0
	if (z80_bus_state() & ZST_RUNNING ||
			!(z80_bus_cmd(Request) & ZST_ACQUIRED))
		return  -1;
#endif

	/* STEP, RUN output, WAIT input */

	PORT_SS |= _BV(RUN) | _BV(STEP);
	DDR_SS |= _BV(RUN) | _BV(STEP);
	DDR_SS &= ~_BV(WAIT);

	/* RUN high, MREQ pulse --> WAIT should be low */
	z80_mreq_pulse();

	if ((PIN_SS & _BV(WAIT)) == 0) {

		/* RUN high, STEP pulse --> WAIT should be high */
		PIN_SS = _BV(STEP);
		PIN_SS = _BV(STEP);
		if ((PIN_SS & _BV(WAIT)) != 0) {

			/* RUN high, MREQ pulse --> WAIT should be low */
			z80_mreq_pulse();
			if ((PIN_SS & _BV(WAIT)) == 0) {

				/* RUN low --> WAIT should be high */
				PIN_SS = _BV(RUN);
				if ((PIN_SS & _BV(WAIT)) != 0) {

					/* RUN low, STEP pulse --> WAIT should be high */
					PIN_SS = _BV(STEP);
					PIN_SS = _BV(STEP);
					if ((PIN_SS & _BV(WAIT)) != 0) {

						/* all tests passed */
						ss_available = true;
					}
				}
			}
		}
	}

	if (!ss_available) {
		DDR_SS &= ~(_BV(STEP) | _BV(RUN));
		PORT_SS |= _BV(RUN) | _BV(STEP);
	}

	return ss_available ? 0 : -1;
}
#endif