/*

  SetClock sets the clock frequency of VS1005g.

*/
#include <vo_stdio.h>
#include <stdlib.h>
#include <apploader.h>
#include <consolestate.h>
#include <string.h>
#include <stdlib.h>
#include <clockspeed.h>
#include <uartspeed.h>
#include <timers.h>
#include <kernel.h>
#include <imem.h>
#include <scheduler.h>
#include "asm.h"
#include "perips.h"

__mem_y u_int16 *timerCountReg = (__mem_y u_int16 *)TIMER_T0L;
s_int16 forceClock = 0;





#define CLOCKS_PER_LOOP 10

#if 1
u_int32 TestSpeedLoopAsm(register __reg_d u_int32 startTime,
			 register __c0 s_int16 ticks);
#else
u_int32 TestSpeedLoopAsm(register __reg_d u_int32 startTime,
			 register __c0 s_int16 ticks);
u_int32 loops = 0;
u_int32 endTime = startTime;

while (endTime - startTime < ticks) {
	Disable();
	/* ReadTimeCount() is variable speed, so we can't use it here. */
	endTime = timeCount;
	Enable();
	loops++;
}
return loops;
}
#endif


/* Returns load in MHz */
s_int16 TestSpeed(register s_int16 quick) {
	u_int32 loops = 0;
	double effectiveMHz, cpuMHz;
	const char *unit = "MHz";
	u_int32 startTime, endTime;
	u_int32 loops = 0;

	if (!quick) {
		printf("CPU speed: ");
		WaitUart0ToClear(1000);
	}

	endTime = ReadTimeCount();
	/* Wait for next time count switch. */
	while ((startTime = ReadTimeCount()) == endTime);
	loops = TestSpeedLoopAsm(startTime, quick ? TICKS_PER_SEC/100 : TICKS_PER_SEC);
	endTime = ReadTimeCount();
	loops = (u_int32)((double)loops*TICKS_PER_SEC/(endTime-startTime));

	effectiveMHz = loops*(1.0e-6*CLOCKS_PER_LOOP);
	cpuMHz = 1.0e-6*clockSpeed.cpuClkHz;
	if (cpuMHz < 1.0) {
		effectiveMHz *= 1000.0;
		cpuMHz *= 1000.0;
		unit = "kHz";
	}
	if (!quick) {
		printf("effective speed %3.2f/%3.2f %s = %2.1f%%, "
		       "overhead %3.2f %s\n",
		       effectiveMHz, cpuMHz, unit, 100.0*effectiveMHz/cpuMHz,
		       cpuMHz-effectiveMHz, unit);
	}

	return (s_int16)(cpuMHz-effectiveMHz+0.5);
}


void CalcClockSpeed(void);

u_int16 SetClockMultiplier(register u_int16 clockX) {
	/* Prevent from the clock going faster than the speed limit */
	while (clockX * clockSpeed.peripClkHz > 2 * clockSpeed.speedLimitHz) {
		clockX--;
	}

	if (clockX <= 2) {
		clockSpeed.pllPreDiv = 0;
		clockSpeed.pllMult = 1;
		clockSpeed.masterClkSrc = MASTER_CLK_SRC_XTALI;
	} else {
		clockSpeed.masterClkSrc = MASTER_CLK_SRC_PLL;
		if (clockX & 1) {
			clockSpeed.pllPreDiv = 1;
			clockSpeed.pllMult = clockX;
		} else {
			clockSpeed.pllPreDiv = 0;
			clockSpeed.pllMult = clockX >> 1;
		}
	}

	/* Now it's time to set our clock. First turn PLL control off and set new
	   PLL frequency. */
	PERIP(CLK_CF) = (PERIP(CLK_CF) & 0xFF00U) |
		(clockSpeed.pllMult-1) | clockSpeed.pllPreDiv*CLK_CF_DIVI;
	PERIP(VSRV1_CLK_CF) = (PERIP(VSRV1_CLK_CF) & 0xFF00U) |
		(clockSpeed.pllMult-1) | clockSpeed.pllPreDiv*VSRV1_CLK_CF_DIVI;

	/* Now, if the multiplier is over 2, wait a while, then turn it back on. */
	if (clockSpeed.pllMult > 1) {
		Delay(1);
		PERIP(CLK_CF) |= CLK_CF_FORCEPLL;
		PERIP(VSRV1_CLK_CF) |= VSRV1_CLK_CF_FORCEPLL;
	}

	CalcClockSpeed();


	return clockX;
}


void CalcClockSpeed(void) {
	u_int32 oldPeripClkHz = clockSpeed.peripClkHz;
	int shiftRight = clockSpeed.xtalPrescaler + clockSpeed.div2 +
		clockSpeed.div256*8;

#if 0
	PERIP(ANA_CF1) = (PERIP(ANA_CF1) & ~(ANA_CF1_XTDIV)) |
		clockSpeed.xtalPrescaler*ANA_CF1_XTDIV;
#endif
	PERIP(CLK_CF) = (PERIP(CLK_CF) & ~(CLK_CF_DIV2 | CLK_CF_DIV256)) |
		clockSpeed.div2*CLK_CF_DIV2 | clockSpeed.div256*CLK_CF_DIV256;
	PERIP(VSRV1_CLK_CF) = (PERIP(VSRV1_CLK_CF) &
			       ~(VSRV1_CLK_CF_DIV2 | VSRV1_CLK_CF_DIV256)) |
		clockSpeed.div2*VSRV1_CLK_CF_DIV2 | clockSpeed.div256*VSRV1_CLK_CF_DIV256;

	if (clockSpeed.masterClkSrc == MASTER_CLK_SRC_PLL ||
	    clockSpeed.masterClkSrc == MASTER_CLK_SRC_XTALI) {
		clockSpeed.peripClkHz = (u_int32)clockSpeed.extClkKHz*1000 >> shiftRight;
		if (clockSpeed.masterClkSrc == MASTER_CLK_SRC_PLL) {
			shiftRight += clockSpeed.pllPreDiv;
			clockSpeed.cpuClkHz =
				(u_int32)clockSpeed.extClkKHz*1000 * clockSpeed.pllMult >>shiftRight;
		} else{
			clockSpeed.cpuClkHz = clockSpeed.peripClkHz;
		}
	}

	if (oldPeripClkHz != clockSpeed.peripClkHz) {
		SetUart0Speed(clockSpeed.uart0BitSpeed, 1000);
	}
}


/* ENTRY_1 */
DLLENTRY(SetClockSpeed)
s_int16 SetClockSpeed(register u_int32 clkHz) {
	int newMultiplier = 2;
	int retCode = -1;
	u_int32 f;

	{
		int t = TestSpeed(1);
		if (t >= 12 && !forceClock) {
			printf("E: System overhead too high!\n");
			TestSpeed(0);
			return -1;
		}
	}

	clkHz &= ~(SET_CLOCK_NO_AUTO_ADJUST/*|SET_CLOCK_RF*/);

	/* Go back to basic 1x clock from XTALI */
	timeCountAdd = 1;
	WaitUart0ToClear(1000);
	PERIP(UART0_DIV) = GetUartRegister(1000*(u_int32)clockSpeed.extClkKHz,
					   clockSpeed.uart0BitSpeed);
	PERIP(CLK_CF) &= ~(CLK_CF_DIV2 | CLK_CF_DIV256 |
			   CLK_CF_USBCLK | CLK_CF_FORCEPLL);
	PERIP(VSRV1_CLK_CF) &= ~(VSRV1_CLK_CF_DIV2 | VSRV1_CLK_CF_DIV256 |
				 VSRV1_CLK_CF_FORCEPLL);

	/* If old clock was RF clock, turn RF VCO off */
#if 0
	if (clockSpeed.masterClkSrc == MASTER_CLK_SRC_RF) {
		PERIP(ANA_CF3) &= (ANA_CF3_GAIN2MASK | ANA_CF3_GAIN1MASK);
		PERIP(ANA_CF2) &= ~(ANA_CF2_UTM_ENA | ANA_CF2_2G_ENA);
	}
#endif
	clockSpeed.xtalPrescaler = 0;
	clockSpeed.div256 = 0;
	clockSpeed.div2 = 0;
	clockSpeed.masterClkSrc = MASTER_CLK_SRC_XTALI;
	CalcClockSpeed();

	/* If requested clock is extClkKHz/2 or less, activate dividers. */
	if (clkHz <= (s_int32)clockSpeed.extClkKHz*500) {
		static u_int16 div[5][4] = {
			{1024, 1, 1, 1},
			{ 512, 1, 1, 0},
			{ 256, 1, 0, 0},
			{   4, 0, 1, 1},
			{   2, 0, 1, 0},
		};
		u_int16 *divP = div[0];
		int i=0;
		int shiftRight;
		u_int32 maxBitSpeed;

		while (i<4 && clkHz > (s_int32)clockSpeed.extClkKHz*1000 / divP[0]) {
			divP += 4;
		}
		clockSpeed.div256 = divP[1];
		clockSpeed.div2 = divP[2];
		clockSpeed.xtalPrescaler = divP[3];
		shiftRight =
			clockSpeed.xtalPrescaler + clockSpeed.div2 + clockSpeed.div256*8;
		maxBitSpeed = clockSpeed.extClkKHz*(1024/8)>>shiftRight;
		if (shiftRight > 5) {
			shiftRight = 5;
		}
		printf("shiftRight = %d\n", shiftRight);

		if (clockSpeed.uart0BitSpeed > maxBitSpeed) {
			printf("E: UART speed %ld bps too high for clock, "
			       "maximum would be %ld bps\n",
			       clockSpeed.uart0BitSpeed, maxBitSpeed);
			goto finally;
		}
		PERIP32(timerCountReg) = (clockSpeed.extClkKHz>>(1+shiftRight))-1;
		if (clockSpeed.div256) {
			PERIP(CLK_CF) |= CLK_CF_DIV256;
			PERIP(VSRV1_CLK_CF) |= VSRV1_CLK_CF_DIV256;
		}
		if (clockSpeed.div2) {
			PERIP(CLK_CF) |= CLK_CF_DIV2;
			PERIP(VSRV1_CLK_CF) |= VSRV1_CLK_CF_DIV2;
		}
		//    if (clockSpeed.xtalPrescaler) PERIP(ANA_CF1) |= ANA_CF1_XTDIV;
		SetClockMultiplier(2);
		timeCountAdd = divP[0]>>shiftRight;
		retCode = 0;
		SetUart0Speed(clockSpeed.uart0BitSpeed, 1000);
		goto finally;
	}

	//  SetUart0Speed(clockSpeed.uart0BitSpeed);

	while ((f=((s_int32)clockSpeed.extClkKHz*500)*newMultiplier) <
	       clkHz && f < clockSpeed.speedLimitHz && newMultiplier < 32) {
		newMultiplier++;
	}
	if (f > clockSpeed.speedLimitHz) {
		newMultiplier--;
		f=((s_int32)clockSpeed.extClkKHz*500)*newMultiplier;
	}
	if (newMultiplier >= 16) { /* If multiplier is over 16, it can't be odd */
		newMultiplier &= ~1;
		f=((s_int32)clockSpeed.extClkKHz*500)*newMultiplier;
	}

	WaitUart0ToClear(1000);
	SetClockMultiplier(newMultiplier);
	SetUart0Speed(clockSpeed.uart0BitSpeed, 1000);

	PERIP(CLK_CF) |= CLK_CF_LCKCHK;
	PERIP(VSRV1_CLK_CF) |= VSRV1_CLK_CF_LCKCHK;
	PERIP(0);
	PERIP(CLK_CF) &= ~CLK_CF_LCKCHK;
	PERIP(VSRV1_CLK_CF) &= ~VSRV1_CLK_CF_LCKCHK;

	if (newMultiplier != 2 &&
	    (PERIP(CLK_CF) & (CLK_CF_FORCEPLL | CLK_CF_LCKST)) !=
	    (CLK_CF_FORCEPLL | CLK_CF_LCKST)) {
		goto finally;
	}

	retCode = 0;
finally:
#if 0
	if (clockSpeed.cpuClkHz >= 1000000 && !PERIP32(DAC_SRCL)) {
		PERIP32(DAC_SRCL) = 0x10000; /* Just some non-zero value. */
	}
#endif

	return retCode;
}


s_int16 SetClockSpeedShowError(register u_int32 clkHz)  {
	s_int16 ret;

	if (ret = SetClockSpeed(clkHz)) {
		printf("SetClock: Couldn't set clock speed, result undefined!\n");
	}
	return ret;
}




void PrintDevSpeeds(void) {
	int shiftRight = clockSpeed.xtalPrescaler +
		clockSpeed.div2 + clockSpeed.div256*8;
	u_int16 uartDiv = PERIP(UART0_DIV);
	s_int32 uartSpeed = clockSpeed.cpuClkHz /
		(s_int32) ((((uartDiv & UART_DIV_D1_MASK) >> UART_DIV_D1_B) + 1) *
			   ((uartDiv & UART_DIV_D2_MASK) >> UART_DIV_D2_B)) >> shiftRight;
	s_int32 uartSpeedError = labs(uartSpeed-clockSpeed.uart0BitSpeed);
	double spi0Speed =
		1e-3*clockSpeed.cpuClkHz / (double)
		(2 * (((PERIP(SPI0_CLKCF)&SPI_CC_CLKDIV_MASK) >> SPI_CC_CLKDIV_B) + 1));
	double sdSpeed =
		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);
	printf("  UART nominal %ld bps, real %ld bps (%3.1f%% error), reg 0x%04x\n",
	       clockSpeed.uart0BitSpeed,
	       uartSpeed, 100.0*uartSpeedError/uartSpeed,
	       PERIP(UART0_DIV));
	if (clockSpeed.cpuClkHz >= 1000000) {
		printf("  SPI0 %3.1f Mbit/s, SD %3.1f MByte/s\n",
		       spi0Speed*0.001, sdSpeed*0.001);
	} else {
		printf("  SPI0 %d kbit/s, SD %d kByte/s\n",
		       (int)(spi0Speed+0.5), (int)(sdSpeed+0.5));

	}
}


u_int16 GetDividerL(register u_int32 f, register u_int16 maxDiv) {
	u_int16 i;
	for (i=0; i<maxDiv; i++) {
		u_int32 t = clockSpeed.cpuClkHz / (u_int32)(2 * (i+1));
		if (t < f) {
			return i;
		}
	}
	return maxDiv;
}

/* GetDivider() returns cpuKHz / speedKHz / 2 - 1, rounded upwards. */
u_int16 GetDivider(register u_int16 maxSpeedKHz, register u_int16 maxVal) {
	u_int16 div = 0;
	u_int32 maxSpeedHzTimes2 = 2000L * maxSpeedKHz;
	if (maxSpeedHzTimes2 <= clockSpeed.cpuClkHz) {
		div = (u_int16)((clockSpeed.cpuClkHz + maxSpeedHzTimes2 - 1) /
				maxSpeedHzTimes2);
		if (div > maxVal) {
			return maxVal;
		}
	}
	return div;
}

void SetSpiSpeed(register u_int32 f, register u_int16 spiPort) {
	u_int16 div = GetDividerL(f, SPI_CC_CLKDIV_MASK >> SPI_CC_CLKDIV_B);
	u_int16 reg = /*spiPort ? SPI1_CLKCF :*/ SPI0_CLKCF;
	PERIP(reg) = (PERIP(reg) & ~(SPI_CC_CLKDIV_MASK)) | (div << SPI_CC_CLKDIV_B);
}

#if 0
void SetNandSpeed(register u_int32 f) {
	u_int16 div = GetDividerL(f, NF_CF_WAITSTATES_MASK >> NF_CF_WAITSTATES_B);
	PERIP(NF_CF) =
		(PERIP(NF_CF) & ~(NF_CF_WAITSTATES_MASK)) | (div << NF_CF_WAITSTATES_B);
}
#endif

void SetSdSpeed(register u_int32 f) {
	u_int16 div = GetDividerL(f, SD_ST_WAITSTATES_MASK >> SD_ST_WAITSTATES_B);
	PERIP(SD_ST) =
		(PERIP(SD_ST) & ~(SD_ST_WAITSTATES_MASK)) | (div << SD_ST_WAITSTATES_B);
}

void PrintTime(register u_int32 milliseconds) {
	u_int16 days, hours, minutes;
	ldiv_t ld = ldiv(milliseconds/100,10L*24*60*60);
	days = ld.quot;
	ld = ldiv(ld.rem,10L*60*60);
	hours = ld.quot;
	ld = ldiv(ld.rem,10L*60);
	minutes = ld.quot;

	if (days) {
		printf("%dd%02d:%02d:", days, hours, minutes);	/* 1d01:23:45.6 */
	} else if (hours) {
		printf("%d:%02d:", hours, minutes);			/* 1:23:45.6 */
	} else {
		printf("%d:", minutes);				/* 1:23.4 */
	}
	printf("%04.1f, time counter interrupt %2.1f times/s\n",
	       0.1*ld.rem, 1000.0/timeCountAdd);
}

ioresult main(char *parameters) {
	int nParam, i;
	char *p = parameters;
	ioresult ret = S_ERROR;
	int nothingToDo = 1;
	int verbose = 0;
	int testSpeed = 0;
	int t;
	double ft;
	enum periphw hw = ERROR_HW;
	u_int32 setHz;
#if 0
	PERIP(UTM_TRIMRES) = GetTrim(ttUSB);
	GetVoltages();
#endif

	nParam = RunProgram("ParamSpl", parameters);

	for (i=0; i<nParam; i++) {
		if (hw != ERROR_HW) {
			s_int32 v;
			char *end;
			v = strtol(p, &end, 10);
			if (*end || v < 0) {
				printf("Bad frequency '%s' %ld", p, v);
				goto finally;
			}
			setHz = (u_int32)v;
			setHz = SetPeripSpeed(hw, setHz);
			end = GetSetClockErr(setHz);
			if (end) {
				printf("Set failed: %s\n", end);
				goto finally;
			} else {
				printf("Set to: %lu Hz\n", setHz);
			}
			hw = ERROR_HW;
		} else if (!strcmp(p, "-h")) {
			printf("Usage: SetClock [x|-lx|-xx|-uf|-sf|-df|\n"
			       "                 -wx|-F|+F|-v|+v|-h]\n"
			       "x\tSet clock to x MHz, auto-adjust(1)\n"
			       "\tx may also be \"usb\" or \"rtc\"\n"
			       "-lx\tLimit top speed to x MHz (USE WITH CAUTION!)\n"
			       "-xx\tTell that XTALI is x MHz (USE WITH CAUTION!)\n"
			       "-uf\tSet UART speed to f bps\n"
			       "-sf\tSet SPI0 speed to f MHz\n"
			       "-df\tSet SD speed to f MHz\n"
			       "-wx\tWait x milliseconds\n"
			       "-Pperip hz\tSet perip perip speed to hz Hz (USE WITH CAUTION!)\n"
			       "-F|+F\tForce clock even if high load on/off (USE WITH CAUTION!)\n"
			       "-v/+v\tVerbose on/off\n"
			       "-t\tTest execution speed\n"
			       "-h\tShow this help\n"
			       "\n(1) Auto-adjustment affects CVDD, RAM Delay, and POR.\n");
			goto finally;
		} else if (!strcmp(p, "-v")) {
			verbose = 1;
		} else if (!strcmp(p, "+v")) {
			verbose = -1;
		} else if (!strncmp(p, "-l", 2)) {
			u_int32 speedLimitMHz = strtol(p+2, NULL, 0);
			if (speedLimitMHz) {
				clockSpeed.speedLimitHz = 1000000*speedLimitMHz;
			}
			nothingToDo = 0;
		} else if (!strncmp(p, "-x", 2) && (ft = strtod(p+2, NULL)) >= 0.0) {
			clockSpeed.extClkKHz = (u_int32)(ft*1000.0+0.5);
			CalcClockSpeed();
			nothingToDo = 0;
		} else if (!strncmp(p, "-u", 2)) {
			u_int32 uartSpeed = strtol(p+2, NULL, 0);
			if (!uartSpeed) {
				printf("E: Unknown parameter \"%s\"\n", p);
				ret = S_ERROR;
				goto finally;
			}
			SetUart0Speed(uartSpeed, 1000);
			clockSpeed.uart0BitSpeed = uartSpeed;
			nothingToDo = 0;
		} else if (!strncmp(p, "-s", 2)) {
			SetSpiSpeed((s_int32)(atof(p+2)*1e6), 0);
			nothingToDo = 0;
		} else if (!strncmp(p, "-P", 2)) {
			hw = GetPeripHw(p + 2);
			if (hw == ERROR_HW) {
				printf("no such perip %s\n", p + 2);
				goto finally;
			}
		/* } else if (!strncmp(p, "-p", 2)) { */
		/* 	SetSpiSpeed((s_int32)(atof(p+2)*1e6), 1); */
		/* 	nothingToDo = 0; */
		} else if (!strncmp(p, "-d", 2)) {
			SetSdSpeed((s_int32)(atof(p+2)*1e6));
			nothingToDo = 0;
		} else if (!strncmp(p, "-w", 2)) {
			Delay(atoi(p+2));
			nothingToDo = 0;
		} else if (!strcmp(p, "-F")) {
			forceClock = 1;
		} else if (!strcmp(p, "+F")) {
			forceClock = 0;
		} else if (!strcmp(p, "-t")) {
			testSpeed = 1;
			nothingToDo = 0;
		} else if ((ft = strtod(p, NULL)) > 0.0) {
			if (SetClockSpeedShowError((u_int32)(ft*1000000.0+0.5))) {
				goto finally;
			}
			nothingToDo = 0;
		} else {
			printf("E: Unknown parameter \"%s\"\n", p);
			goto finally;
		}
		p += strlen(p)+1;
	}

	if ((nothingToDo || verbose>0) && verbose >= 0) {
		static char *master[4] = {"XTALI", "PLL", "RTC"};
#if 0
		if (nothingToDo) goto finally;
#endif
		printf("SetClock running on %s, clocks:\n"
		       "  XTALI %5.3fMHz, VSCLKI %5.3fMHz, lim %ldMHz, "
		       "src %s, RVCLKI %5.3fMHz\n",
		       "VSRV01",
		       clockSpeed.extClkKHz*1e-3,
		       clockSpeed.cpuClkHz*1e-6,
		       clockSpeed.speedLimitHz/1000000,
		       (clockSpeed.masterClkSrc<4) ? master[clockSpeed.masterClkSrc]:"??",
		       1e-6*GetPeripSpeed(GetPeripHw("RISCVPLL")));
		PrintDevSpeeds();
		printf("Uptime ");
		PrintTime(ReadTimeCount());
	}
	if (clockSpeed.masterClkSrc == MASTER_CLK_SRC_PLL &&
	    (PERIP(CLK_CF) & (CLK_CF_FORCEPLL|CLK_CF_LCKST)) == CLK_CF_FORCEPLL) {
		printf("SetClock: VSDSP PLL lock lost, speed may be unexpected\n");
	}
	if (clockSpeed.masterClkSrc == MASTER_CLK_SRC_PLL &&
	    (PERIP(VSRV1_CLK_CF) & (VSRV1_CLK_CF_FORCEPLL|VSRV1_CLK_CF_LCKST)) ==
	    VSRV1_CLK_CF_FORCEPLL) {
		printf("SetClock: Risc-V PLL lock lost, speed may be unexpected\n");
	}

	if (testSpeed) {
		TestSpeed(0);
	}
	printf("\n");
	PrintPeripSpeeds(verbose);
	ret = S_OK;
finally:

	clockSpeed.rvClkHz = GetPeripSpeed(GetPeripHw("RISCVPLL"));
	
	return ret;
}
