#include <vo_stdio.h>
#include <vsrv.h>
#include <clockspeed.h>
#include <uartspeed.h>
#include <stdlib.h>
#include <strings.h>
#include "perips.h"

enum clockSource {
	CLOCK_SOURCE_XTALI,
	CLOCK_SOURCE_CORE,
	CLOCK_SOURCE_RF,
	CLOCK_SOURCE_RISC_V
};
struct peripClock {
	enum periphw hw;
	enum clockSource clkSrc;
	u_int16 minDiv;
	u_int32 (*GetClock)(__mem_y register struct peripClock *pc); /* Get clockspeeds in Hz */
	u_int32 (*SetClock)(__mem_y register struct peripClock *pc,
			    register u_int32 reqHz);
	void (*PrettyPrint)(__mem_y register struct peripClock *pc);
	u_int16 data;
	char *name;
	u_int32 lastSpeed;
};
static u_int32 GetUartSpeed(__mem_y register struct peripClock *pc);
static u_int32 MySetUartSpeed(__mem_y register struct peripClock *pc, register u_int32 bps);
static void PrintUart(__mem_y register struct peripClock *pc);
static u_int32 GetSpiSpeed(__mem_y register struct peripClock *pc);
static u_int32 SetSpiSpeed(__mem_y register struct peripClock *pc, register u_int32 hz);
static u_int32 GetTimerSpeed(__mem_y register struct peripClock *pc);
static u_int32 SetTimerSpeed(__mem_y register struct peripClock *pc, register u_int32 hz);
static u_int32 GetSdSpeed(__mem_y register struct peripClock *pc);
static u_int32 SetSdSpeed(__mem_y register struct peripClock *pc, register u_int32 hz);
static void PrintSd(__mem_y register struct peripClock *pc);

static u_int32 GetPllSpeed(__mem_y register struct peripClock *pc);
static u_int32 SetPllSpeed(__mem_y register struct peripClock *pc, register u_int32 hz);

u_int16 GetDivider(register u_int16 maxSpeedKHz, register u_int16 maxVal);
u_int16 GetDividerL(register u_int32 f, register u_int16 maxDiv);

__mem_y struct peripClock peripherals[] = {
	{UART0_HW, CLOCK_SOURCE_CORE, 8, GetUartSpeed, MySetUartSpeed, PrintUart, UART0_DIV, "UART0"},
	{UART0_TX_HW, CLOCK_SOURCE_CORE, 8, GetUartSpeed, MySetUartSpeed, PrintUart, UART0_TXDIV, "UART0TX"},
	{UARTRV_HW,CLOCK_SOURCE_CORE, 8, GetUartSpeed, MySetUartSpeed, PrintUart, UARTRV_DIV, "UARTRV"},
	{UARTRV_TX_HW,CLOCK_SOURCE_CORE, 8, GetUartSpeed, MySetUartSpeed, PrintUart, UARTRV_TXDIV, "UARTRVTX"},
	{UARTMUX_HW,CLOCK_SOURCE_CORE, 8, GetUartSpeed, MySetUartSpeed, PrintUart, UARTMUX_DIV, "UARTMUX"},
	{UARTMUX_TX_HW,CLOCK_SOURCE_CORE, 8, GetUartSpeed, MySetUartSpeed, PrintUart, UARTMUX_TXDIV, "UARTMUXTX"},
	{SPI0_HW,CLOCK_SOURCE_CORE, 1, GetSpiSpeed, SetSpiSpeed, NULL, SPI0_CLKCF, "SPI0"},
	{SD_HW, CLOCK_SOURCE_CORE, 0, GetSdSpeed, SetSdSpeed, PrintSd, 0, "SD"},
	/* {SPI1_HW,CLOCK_SOURCE_CORE, GetSpiSpeed, SetSpiSpeed}, */
	{TIMER0_HW,CLOCK_SOURCE_XTALI, 2, GetTimerSpeed, SetTimerSpeed, NULL, TIMER_T0L, "TIMER0"},
	{TIMER1_HW,CLOCK_SOURCE_XTALI, 2, GetTimerSpeed, SetTimerSpeed, NULL, TIMER_T1L, "TIMER1"},
	{TIMER2_HW,CLOCK_SOURCE_XTALI, 2, GetTimerSpeed, SetTimerSpeed, NULL, TIMER_T2L, "TIMER2"},
	{CORE_PLL_HW, CLOCK_SOURCE_XTALI, 0, GetPllSpeed, SetPllSpeed, NULL, CLK_CF, "COREPLL"}, /* PLL */
	{RISCV_HW, CLOCK_SOURCE_XTALI, 0, GetPllSpeed, SetPllSpeed, NULL, VSRV1_CLK_CF, "RISCVPLL"}, /* PLL */
	{RF_PLL_HW, CLOCK_SOURCE_XTALI, 0, NULL, NULL, NULL, 0, "RFPLL"}, /* PLL */
};
static u_int32 GetClockSourceSpeed(enum clockSource clkSrc) {
	switch(clkSrc) {
	case CLOCK_SOURCE_XTALI:
		return clockSpeed.peripClkHz;
	case CLOCK_SOURCE_CORE:
		return clockSpeed.cpuClkHz;
	default:
		return 42;
	}
}
const char *GetSetClockErr(register u_int32 hz) {
	switch(hz) {
	case SETCLOCK_BAD_PERIP:
		return "No such perip";
	case SETCLOCK_MAX_VIOLATION:
		return "Exceeding maximum frequency";
	case SETCLOCK_NOT_SET:
		return "Perip speed not set";
	default:
		return NULL;
	}
}
enum periphw GetPeripHw(register const char *name) {
	u_int16 i;
	__mem_y struct peripClock *p = peripherals;
	for (i = 0; i < (sizeof(peripherals) / sizeof(peripherals[0])); i++) {
		if (strcasecmp(p->name, name) == 0) {
			//	printf("Perip: '%s'\n", p->name);
			return p->hw;
		}
		p++;
	}
	return ERROR_HW;
}
u_int32 GetPeripSpeed(register enum periphw hw) {
	u_int16 i;
	__mem_y struct peripClock *p = peripherals;
	for (i = 0; i < (sizeof(peripherals) / sizeof(peripherals[0])); i++) {
		if (p->hw == hw && p->GetClock) {
			u_int32 speed = p->GetClock(p);
			if (speed < SETCLOCK_MAX_CLOCK) {
				p->lastSpeed = speed;
			}
			return speed;
		}
		p++;
	}
	return SETCLOCK_BAD_PERIP;
}
void PrintPeripSpeeds(register u_int16 verbose) {
	u_int16 i;
	__mem_y struct peripClock *p = peripherals;
	for (i = 0; i < (sizeof(peripherals) / sizeof(peripherals[0])); i++) {
		if (p->GetClock) {
			static char * magn[] = {"Hz", "kHz", "MHz", "GHz"};
			char **suf = magn;
			u_int32 f = p->GetClock(p);
			if ((f != SETCLOCK_NOT_SET) || verbose) {
				double speed = f;
				while (speed > 1000) {
					speed /= 1000;
					suf++;
				}
				printf("%s %3.2f %s", p->name, speed, *suf);
				if (p->PrettyPrint) {
					printf(" ");
					p->PrettyPrint(p);
				}
				printf("\n");
			}
		}
		p++;
	}
}
u_int32 SetPeripSpeed(register enum periphw hw, register u_int32 hz) {
	u_int16 i;
	u_int32 ret = SETCLOCK_BAD_PERIP;
	__mem_y struct peripClock *p = peripherals;

	for (i = 0; i < (sizeof(peripherals) / sizeof(peripherals[0])); i++) {
		if (p->hw == hw && p->SetClock) {
			if (p->minDiv && ((GetClockSourceSpeed(p->clkSrc) / p->minDiv) < hz)) {
				return SETCLOCK_MAX_VIOLATION;
			}
			return  p->SetClock(p, hz);
		}
		p++;
	}
}

static u_int32 GetUartSpeed(__mem_y register struct peripClock *pc) {
	u_int16 divider = PERIP(pc->data);
	u_int32 ret = SETCLOCK_NOT_SET;
	if (divider && clockSpeed.cpuClkHz) {
		ret = clockSpeed.cpuClkHz / (divider & 0xff);
		divider >>= 8;
		divider++;
		ret /= divider;
	}
	return ret;
}
static u_int16 WaitUartToBeClear(register u_int16 maxWaitMS, register u_int16 uart_status) {
	volatile __mem_y u_int16 *st = (volatile __mem_y u_int16 *)uart_status;
	u_int16 consecutiveTxStopped = 0;
	u_int32 startTime = ReadTimeCount();
	while ((consecutiveTxStopped < 64) && ((ReadTimeCount() - startTime) < maxWaitMS)) {
		if (*st & (UART_ST_TXRUNNING | UART_ST_TXFULL)) {
			consecutiveTxStopped = 0;
		} else {
			consecutiveTxStopped++;
		}
	}
	return 64 - consecutiveTxStopped;
}
static u_int32 MySetUartSpeed(__mem_y register struct peripClock *pc, register u_int32 bps) {
	u_int16 regVal;
	regVal = GetUartRegister(clockSpeed.cpuClkHz, bps);
	WaitUartToBeClear(1000, pc->data & ~0x7);
	PERIP(pc->data) = regVal;
	return GetUartSpeed(pc);
}
static void PrintUart(__mem_y register struct peripClock *pc) {
	static const __mem_y u_int16 bps1[] = {
		/* 50, 75, 110, 134, 150, Unreachable even on 1X */
		200, 300, 600, 1200,
		/* Min @ 98 MHz */
		1800, 2400, 4800, 9600, 19200, 38400, 57600
	};
	static const __mem_y u_int16 bps100[] = {
		1152, 2304, 4608, 5000, 5760, 9216, 10000, 11520,
		15000, /* 1X max: 1536000 */
		20000, 25000, 30000, 35000, 40000, 50000,
		60000 /* can't have faster targets in here */
	};
	u_int32 bps = GetUartSpeed(pc);
	u_int32 target = 0;
	__mem_y u_int16 *ts = bps1;
	u_int16 i;
	for (i = 0; i < (sizeof(bps1)/sizeof(bps1[0])); i++) {
		if (labs(bps - *ts) < labs(bps - target)) {
			target = *ts;
		}
		ts++;
	}
	ts = bps100;
	for (i = 0; i < (sizeof(bps100)/sizeof(bps100[0])); i++) {
		if (labs(bps - (*ts * 100UL)) < labs(bps - target)) {
			target = *ts * 100UL;
		}
		ts++;
	}
	if (target == (bps100[(sizeof(bps100)/sizeof(bps100[0])) - 1] * 100UL)) {
		printf("Error may be wrong. Maximum ");
	}
	printf("target: %lu bps, error %3.1f%%", target, labs(bps - target) * 100.0 / bps);
}
static u_int32 GetSpiSpeed(__mem_y register struct peripClock *pc) {
	u_int16 divider = PERIP(pc->data);
	u_int32 ret = clockSpeed.cpuClkHz;
	divider >>= 2;
	divider = (divider + 1) * 2;
	return ret / divider;
}
static u_int32 SetSpiSpeed(__mem_y register struct peripClock *pc, register u_int32 hz) {
	u_int16 regVal, divider;
	divider = GetDivider((u_int16)(hz/1000), SPI_CC_CLKDIV_MASK >> SPI_CC_CLKDIV_B);
	regVal = PERIP(pc->data) & ~SPI_CC_CLKDIV_MASK;
	regVal |= (divider << SPI_CC_CLKDIV_B);
	PERIP(pc->data) = regVal;
	return GetSpiSpeed(pc);
}
static u_int32 GetTimerSpeed(__mem_y register struct peripClock *pc) {
	u_int32 divider = PERIP32(pc->data);
	double f = clockSpeed.peripClkHz;
	f /= divider;
	f += 0.5;
	return (u_int32)f;
}
static u_int32 SetTimerSpeed(__mem_y register struct peripClock *pc, register u_int32 hz) {
	double f = clockSpeed.peripClkHz;

	/* How to set less than 1 Hz? */
	f /= hz;
	f += 0.5;
	PERIP32(pc->data) = (u_int32)f;
}
static u_int32 GetSdSpeed(__mem_y register struct peripClock *pc) {
	u_int16 ws = ((PERIP(SD_ST)&SD_ST_WAITSTATES_MASK) >> SD_ST_WAITSTATES_B);
	double f = clockSpeed.cpuClkHz / (2.0 * (ws + 1));
	/*
	if (PERIP(SD_CF) & SD_CF_4BIT) {
		f*= 4;
	}
	*/
	/*
	  Is this correct? cpu/(2*(ws+1)/2:8???
	  1e-3*clockSpeed.cpuClkHz / (double)
	  (2 * (((PERIP(SD_ST)&SD_ST_WAITSTATES_MASK) >> SD_ST_WAITSTATES_B) + 1)) *
	  ((PERIP(SD_CF) & SD_CF_4BIT) ? 0.5 : 0.125);
	*/
	return (u_int32)f;
}
static u_int32 SetSdSpeed(__mem_y register struct peripClock *pc, register u_int32 hz) {
	u_int16 divider = GetDividerL(hz, SD_ST_WAITSTATES_MASK >> SD_ST_WAITSTATES_B);
	PERIP(SD_ST) =
		(PERIP(SD_ST) & ~(SD_ST_WAITSTATES_MASK)) | (divider << SD_ST_WAITSTATES_B);

}
static void PrintSd(__mem_y register struct peripClock *pc) {
	static const char magn[] = " kMG";
	double f = GetSdSpeed(pc);
	char *suf = magn;
	if (PERIP(SD_CF) & SD_CF_4BIT) {
		f /= 2;
	} else {
		f /= 8;
	}
	while (f > 1000) {
		f /= 1000;
		suf++;
	}
	printf("%3.2f %cB/s", f, *suf);
}


static u_int32 GetPllSpeed(__mem_y register struct peripClock *pc) {
	u_int32 ret = SETCLOCK_NOT_SET;
	u_int16 clk_cf = PERIP(pc->data);
	if (clk_cf & CLK_CF_FORCEPLL) {
		/* PLL on */
		ret = clockSpeed.extClkKHz * 1000UL * (((clk_cf & CLK_CF_MULT_MASK) >> CLK_CF_MULT_B) + 1);
		if (clk_cf & CLK_CF_DIVI) {
			ret /= 2;
		}
	}
	return ret;
}
static u_int32 SetPllSpeed(__mem_y register struct peripClock *pc, register u_int32 hz) {
	if (hz > clockSpeed.speedLimitHz) {
		return SETCLOCK_MAX_VIOLATION;
	}
	if (hz > (clockSpeed.extClkKHz * 1000UL)) {
		u_int16 mult = 32;
		u_int16 prediv = CLK_CF_DIVI;
		u_int32 xtali2 = clockSpeed.extClkKHz * 500UL;
		/* Go one under */
		while ((mult * xtali2) > hz) {
			mult--;
		}
		/* Get back above  */
		mult++;
		/* Stay in usable area */
		if ((mult * xtali2) > clockSpeed.speedLimitHz) {
			mult--;
		}
		if (mult > 16) {
			prediv = 0;
			mult >>= 1;
		}
		mult--;
		PERIP(pc->data) &= ~(CLK_CF_DIV256 | CLK_CF_DIV2 | CLK_CF_FORCEPLL);
		PERIP(pc->data) |= prediv | (mult);
		Delay(1); /* Give 1 ms to lock */
		PERIP(pc->data) |= CLK_CF_LCKCHK;
		PERIP(pc->data) &= ~CLK_CF_LCKCHK;
		if (PERIP(pc->data) & CLK_CF_LCKST) {
			printf("PLL Lock\n");
		} else {
			printf("PLL not locked\n");
		}
		PERIP(pc->data) |= CLK_CF_FORCEPLL;
	}
}
