/*
 * \file ks_clksynth.cydsn/src/lmx2594.c
 * \brief LMX2594 interface
 * 
 * Interface to the LMX2594 synthesizers on the KEYSTONE/SILVERTON/ASPEN PCB.
 * Register-level and high-level access, high-level requires the global
 * variables for reference clock to be set.
 *
 */

#include <math.h>

#include "lmx2594.h"

#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
#include "lmx2594_regs.h"
#include "lmx2594_cfg_regs.h"
#include "global_cfg.h"
#include "fault_logic.h"

/* Most-recently written value of R0 for 3 synths. These are cached since
once the output MUX is set to LD instead of readback, the old value cannot
be restored */
uint16_t r0_save[3];

/* Initialize LMX2594 SPI master */
void lmx2594_init(void)
{
	SYNTH1_CS_Write(1);
	SYNTH2_CS_Write(1);
	SYNTH3_CS_Write(1);
	SPIM_SYNTH_1_Start();
	SPIM_SYNTH_2_Start();
	SPIM_SYNTH_3_Start();
}

static uint8 spim_read_status(enum lmx2594_channel channel)
{
	switch (channel) {
	case LMX2594_CHN1:
		return SPIM_SYNTH_1_ReadStatus();
	case LMX2594_CHN2:
		return SPIM_SYNTH_2_ReadStatus();
	case LMX2594_CHN3:
		return SPIM_SYNTH_3_ReadStatus();
	default:
		return 0;
	}
}

static void spim_cs_write(enum lmx2594_channel channel, int value)
{
	switch (channel) {
	case LMX2594_CHN1:
		SYNTH1_CS_Write(value);
		break;
	case LMX2594_CHN2:
		SYNTH2_CS_Write(value);
		break;
	case LMX2594_CHN3:
		SYNTH3_CS_Write(value);
		break;
	default:
		break;
	}
}

static void spim_clear_fifo(enum lmx2594_channel channel)
{
	switch (channel) {
	case LMX2594_CHN1:
		SPIM_SYNTH_1_ClearFIFO();
		break;
	case LMX2594_CHN2:
		SPIM_SYNTH_2_ClearFIFO();
		break;
	case LMX2594_CHN3:
		SPIM_SYNTH_3_ClearFIFO();
		break;
	default:
		break;
	}
}

static void spim_write_byte(enum lmx2594_channel channel, uint8_t data)
{
	switch (channel) {
	case LMX2594_CHN1:
		SPIM_SYNTH_1_WriteByte(data);
		break;
	case LMX2594_CHN2:
		SPIM_SYNTH_2_WriteByte(data);
		break;
	case LMX2594_CHN3:
		SPIM_SYNTH_3_WriteByte(data);
		break;
	default:
		break;
	}
}

static uint8 spim_get_rx_buffer_size(enum lmx2594_channel channel)
{
	switch (channel) {
	case LMX2594_CHN1:
		return SPIM_SYNTH_1_GetRxBufferSize();
	case LMX2594_CHN2:
		return SPIM_SYNTH_2_GetRxBufferSize();
	case LMX2594_CHN3:
		return SPIM_SYNTH_3_GetRxBufferSize();
	default:
		return 0;
	}
}

static uint8 spim_read_rx_data(enum lmx2594_channel channel)
{
	switch (channel) {
	case LMX2594_CHN1:
		return SPIM_SYNTH_1_ReadRxData();
	case LMX2594_CHN2:
		return SPIM_SYNTH_2_ReadRxData();
	case LMX2594_CHN3:
		return SPIM_SYNTH_3_ReadRxData();
	default:
		return 0;
	}
}

int lmx2594_default_regs(enum lmx2594_channel channel)
{
	int reg;
	
	synth_unlock_isr_Disable();
	/* Reset the chip */
	lmx2594_write_word(channel, 0, LMX2594_R0_DEFAULT | LMX2594_R0_RESET);
	/* Take chip out of reset */
	lmx2594_write_word(channel, 0, LMX2594_R0_DEFAULT);
	
	/* Program registers */
	for (reg = 0; reg < LMX2594_CFG_REGS_LEN; reg++) {
		lmx2594_write_word(channel, lmx2594_cfg_regs[reg].addr, lmx2594_cfg_regs[reg].data);
	}

	/* Wait for lock */
	//vTaskDelay(pdMS_TO_TICKS(10));

	/* Reprogram R0 with FCAL_EN set */
	/* According to forum posts, no need to clear FCAL_EN, just writing with 
	FCAL_EN set performs the VCO frequency calibration */
	lmx2594_write_word(channel, 0, LMX2594_R0_DEFAULT | LMX2594_R0_FCAL_EN | LMX2594_R0_MUXOUT_LD_SEL);
	synth_unlock_isr_Enable();

	return 0;
}

void lmx2594_config_all(void)
{
	uint8_t synths_enabled = 0;

	if (g_cfg->synth1_installed) {
		lmx2594_default_regs(LMX2594_CHN1);
		lmx2594_set_frequency(LMX2594_CHN1, g_cfg->synth1_freq_mhz);
		synths_enabled |= (1 << LMX2594_CHN1);
	}
	if (g_cfg->synth2_installed) {
		lmx2594_default_regs(LMX2594_CHN2);
		lmx2594_set_frequency(LMX2594_CHN2, g_cfg->synth2_freq_mhz);
		synths_enabled |= (1 << LMX2594_CHN2);
	}
	if (g_cfg->synth3_installed) {
		lmx2594_default_regs(LMX2594_CHN3);
		lmx2594_set_frequency(LMX2594_CHN3, g_cfg->synth3_freq_mhz);
		synths_enabled |= (1 << LMX2594_CHN3);
	}
	SYNTH_CTL_REG_Write(synths_enabled);
}

#define SPIM_STS_TX_FIFO_EMPTY SPIM_SYNTH_1_STS_TX_FIFO_EMPTY

/* Read a single word from specified LMX2594 register */
int lmx2594_read_word(enum lmx2594_channel channel, int8_t addr, uint16_t *output)
{
	uint8_t status;
	
	if (addr > 112) return -1;
	if ((channel < LMX2594_CHN1) || (channel > LMX2594_CHN3)) return -1;
	
	do {
		status = spim_read_status(channel);
	} while (!(status & SPIM_STS_TX_FIFO_EMPTY));	
	
	taskENTER_CRITICAL();
	
	/* Select part */
	spim_cs_write(channel, 0);
	
	spim_clear_fifo(channel);
	
	/* Ensure that MSB of address (R/W bit) is set (for read cycle) */
	addr |= 0x80;
	spim_write_byte(channel, addr);

	/* Wait for data to show up in the receive buffer */
	while (spim_get_rx_buffer_size(channel) == 0); 
	/* Dummy read, to get rid of byte received during address cycle */
	spim_read_rx_data(channel);

	volatile uint16_t readback;
	
	/* Write two dummy bytes to read back data */
	spim_write_byte(channel, 0x00);

	while (spim_get_rx_buffer_size(channel) == 0); 

	readback = spim_read_rx_data(channel);
	readback <<= 8;

	spim_write_byte(channel, 0x00);
	
	while (spim_get_rx_buffer_size(channel) == 0);
	readback |= spim_read_rx_data(channel);
	
	*output = readback;

	/* Deselect part */
	spim_cs_write(channel, 1);
	taskEXIT_CRITICAL();
	return 0;
}

/* Write a single word to specified LMX2594 register */
int lmx2594_write_word(enum lmx2594_channel channel, int8_t addr, uint16_t data)
{
	uint8_t status;
	
	if (addr > 112) return -1;
	if ((channel < LMX2594_CHN1) || (channel > LMX2594_CHN3)) return -1;
	
	do {
		status = spim_read_status(channel);
	} while (!(status & SPIM_STS_TX_FIFO_EMPTY));
	
	if (addr == 0) r0_save[channel] = data;
	/* Select part */
	spim_cs_write(channel, 0);
	
	spim_clear_fifo(channel);
	
	/* Ensure that MSB of address (R/W bit) is clear (for write cycle) */
	addr &= ~0x80;
	spim_write_byte(channel, addr);
	/* Write data, MSB first */
	spim_write_byte(channel, (data >> 8) & 0xFF);
	spim_write_byte(channel, (data >> 0) & 0xFF);
	
	uint16_t readcount;
	
	for (readcount = 0; readcount < 3; readcount++) {
		/* Wait for data to show up in the receive buffer */
		while (spim_get_rx_buffer_size(channel) == 0); 
		/* Dummy read, to get rid of bytes received during TX */
		spim_read_rx_data(channel);
	}

	/* Deselect part */
	spim_cs_write(channel, 1);
	return 0;
}

/*
ldmux_state 0 = readback, 1 = lock detect
*/
int lmx2594_set_ldmux(enum lmx2594_channel channel, uint8_t ldmux_state)
{
	if ((channel < LMX2594_CHN1) || (channel > LMX2594_CHN3)) return -1;
	
	if (!ldmux_state) {
		synth_unlock_isr_Disable();
	}
	else {
		synth_unlock_isr_Enable();
	}
	return lmx2594_write_word(channel, 0,
		(r0_save[channel] & (~LMX2594_R0_MUXOUT_LD_SEL)) | 
		(ldmux_state ? LMX2594_R0_MUXOUT_LD_SEL : 0));
}

double g_ref_freq[3] = {200.0e6, 200.0e6, 200.0e6};
double g_output_freq[3] = {0.0, 0.0, 0.0};

int lmx2594_get_pfd_delay(double fvco_mhz, int mash_order)
{
	switch (mash_order) {
	case 0:
	default:
		if (fvco_mhz <= 12500.0) return 1;
		else return 2;
	case 1:
		if (fvco_mhz <= 10000.0) return 1;
		else if (fvco_mhz <= 12500.0) return 2;
		else return 3;
	case 3:
		if (fvco_mhz <= 10000.0) return 3;
		else return 4;
	case 4:
		if (fvco_mhz <= 10000.0) return 5;
		else return 6;
	}
}

int lmx2594_get_minimum_N(double fvco_mhz, int mash_order)
{
	switch (mash_order) {
	case 0:
	default:
		if (fvco_mhz <= 12500.0) return 28;
		else return 32;
	case 1:
		if (fvco_mhz <= 10000.0) return 28;
		else if (fvco_mhz <= 12500.0) return 32;
		else return 36;
	case 3:
		if (fvco_mhz <= 10000.0) return 36;
		else return 40;
	case 4:
		if (fvco_mhz <= 10000.0) return 44;
		else return 48;
	}
}

float    fvco_min[8] = {0, 7500, 8600, 9800, 10800, 12000, 12900, 13900};
float    fvco_max[8] = {0, 8600, 9800, 10800, 12000, 12900, 13900, 15000};
uint16_t c_core_min[8] = {0, 164, 165, 158, 140, 183, 155, 175};
uint16_t c_core_max[8] = {0,  12,  16,  19,   0,  36,   6,  19};
uint16_t a_core_min[8] = {0, 299, 356, 324, 383, 205, 242, 323};
uint16_t a_core_max[8] = {0, 240, 247, 224, 224, 146, 163, 244};

int lmx2594_get_vco_core(double fvco_mhz)
{
	int core;
	for (core = 1; core < 8; core++) {
		if ((fvco_mhz >= fvco_min[core]) && (fvco_mhz <= fvco_max[core])) {
			return core;
		}
	}
	return 0;
}

/* Set output frequency */
int lmx2594_set_frequency(enum lmx2594_channel channel, double frequency_mhz)
{
	if ((channel < LMX2594_CHN1) || (channel > LMX2594_CHN3)) {
		return -1;
	}
	int chdiv, chdiv_seg1_en, chdiv_enable = 1;
	double division = 1.0;
	
	if (frequency_mhz > 7500.0) {
		chdiv_enable = 0; chdiv = 0; chdiv_seg1_en = 0; division = 1.0;
	}
	else if (frequency_mhz > 3750.0) {
		chdiv_enable = 1; chdiv = 0; chdiv_seg1_en = 0; division = 2.0;
	}
	else if (frequency_mhz > 1875.0) {
		chdiv_enable = 1; chdiv = 1; chdiv_seg1_en = 1; division = 4.0;
	}
	else if (frequency_mhz > 1250.0) {
		chdiv_enable = 1; chdiv = 2; chdiv_seg1_en = 1; division = 6.0;
	}
	else if (frequency_mhz > 937.5) {
		chdiv_enable = 1; chdiv = 3; chdiv_seg1_en = 1; division = 8.0;
	}
	else if (frequency_mhz > 625.0) {
		chdiv_enable = 1; chdiv = 4; chdiv_seg1_en = 1; division = 12.0;
	}
	else if (frequency_mhz > 468.75) {
		chdiv_enable = 1; chdiv = 5; chdiv_seg1_en = 1; division = 16.0;
	}
	else if (frequency_mhz > 312.5) {
		chdiv_enable = 1; chdiv = 6; chdiv_seg1_en = 1; division = 24.0;
	}
	else if (frequency_mhz > 234.375) {
		chdiv_enable = 1; chdiv = 7; chdiv_seg1_en = 1; division = 32.0;
	}
	else if (frequency_mhz > 156.25) {
		chdiv_enable = 1; chdiv = 8; chdiv_seg1_en = 1; division = 48.0;
	}
	else if (frequency_mhz > 117.1875) {
		chdiv_enable = 1; chdiv = 9; chdiv_seg1_en = 1; division = 64.0;
	}
	else if (frequency_mhz > 104.167) {
		chdiv_enable = 1; chdiv = 10; chdiv_seg1_en = 1; division = 72.0;
	}
	else if (frequency_mhz > 78.125) {
		chdiv_enable = 1; chdiv = 11; chdiv_seg1_en = 1; division = 96.0;
	}
	else if (frequency_mhz > 58.594) {
		chdiv_enable = 1; chdiv = 12; chdiv_seg1_en = 1; division = 128.0;
	}
	else if (frequency_mhz > 39.0625) {
		chdiv_enable = 1; chdiv = 13; chdiv_seg1_en = 1; division = 192.0;
	}
	else if (frequency_mhz > 29.297) {
		chdiv_enable = 1; chdiv = 14; chdiv_seg1_en = 1; division = 256.0;
	}
	else if (frequency_mhz > 19.541) {
		chdiv_enable = 1; chdiv = 15; chdiv_seg1_en = 1; division = 384.0;
	}
	else if (frequency_mhz > 14.648) {
		chdiv_enable = 1; chdiv = 16; chdiv_seg1_en = 1; division = 512.0;
	}
	else if (frequency_mhz > 9.766) {
		chdiv_enable = 1; chdiv = 17; chdiv_seg1_en = 1; division = 768.0;
	}
	else {
		return -1;
	}
	
	double vco_freq_mhz = frequency_mhz * division;
	
	if (vco_freq_mhz > 15000.0) return -1;
	
	int vco_core = lmx2594_get_vco_core(vco_freq_mhz);
	if (vco_core == 0) return -1;
	float vco_fraction = (vco_freq_mhz - fvco_min[vco_core]) /
		(fvco_max[vco_core] - fvco_min[vco_core]);
	
	uint16_t cap_code = round(c_core_min[vco_core] - (c_core_min[vco_core] - c_core_max[vco_core]) * vco_fraction);
	uint16_t dac_iset = round(a_core_min[vco_core] + (a_core_max[vco_core] - a_core_min[vco_core]) * vco_fraction);

	/* Special case the band around 12 GHz, based on note in data sheet */
	if ((vco_freq_mhz >= 11900) && (vco_freq_mhz <= 12100)) {
		vco_core = 4;
		dac_iset = 300;
		cap_code = 1;
	}
	
	uint8_t mash_order;
	uint8_t outpwr;
	uint32_t mash_seed;
	switch (channel) {
	case LMX2594_CHN1:
	default:
		mash_order = g_cfg->synth1_order;
		outpwr = g_cfg->synth1_outpwr;
		mash_seed = g_cfg->synth1_mash_seed;
		break;
	case LMX2594_CHN2:
		mash_order = g_cfg->synth2_order;
		outpwr = g_cfg->synth2_outpwr;
		mash_seed = g_cfg->synth2_mash_seed;
		break;
	case LMX2594_CHN3:
		mash_order = g_cfg->synth3_order;
		outpwr = g_cfg->synth3_outpwr;
		mash_seed = g_cfg->synth3_mash_seed;
		break;
	}
	
	int pfd_delay = lmx2594_get_pfd_delay(vco_freq_mhz, mash_order);
	int min_N = lmx2594_get_minimum_N(vco_freq_mhz, mash_order);
	
	/* TODO: handle possible multiplier/divider */
	double pfd_freq = g_ref_freq[channel];
	
	double N_divider = vco_freq_mhz / (pfd_freq * 1e-6);
	 
	int32_t integer_N = floor(N_divider);
	if (integer_N < min_N) return -1;
	int32_t fractional_N = round((N_divider - integer_N) * g_cfg->fractional_denominator);
	
	/* Re-enable MUXOUT=readback */
	lmx2594_set_ldmux(channel, 0);
	uint16_t readback;
	lmx2594_read_word(channel, 37, &readback);
	lmx2594_write_word(channel, 37, (readback & ~0x3F00) | (pfd_delay << 8));
	
	lmx2594_read_word(channel, 45, &readback);
	if (chdiv_enable) {
		lmx2594_write_word(channel, 45, (readback & ~0x1800) | (0 << 11));
	}
	else {
		lmx2594_write_word(channel, 45, (readback & ~0x1800) | (1 << 11));
	}
	uint32_t r44 = ((outpwr & 0x3F) << 8) |
		(1 << 7) | /* Always power down OUTB */
		((outpwr == 0) ? (1 << 6) : 0) | /* power down OUTA if outpwr=0 */
		(mash_order & 0x07) << 0;
	/* Clear, then set the MASH_RESET_N bit */
	lmx2594_write_word(channel, 44, r44);
	lmx2594_write_word(channel, 44, r44 | (1 << 5));

	lmx2594_read_word(channel, 31, &readback);
	lmx2594_write_word(channel, 31, (readback & ~0x4000) | (chdiv_seg1_en << 14));

	lmx2594_read_word(channel, 75, &readback);
	lmx2594_write_word(channel, 75, (readback & ~0x07C0) | (chdiv << 6));

	lmx2594_write_word(channel, 43, fractional_N & 0xFFFF);
	lmx2594_write_word(channel, 42, (fractional_N >> 16) & 0xFFFF);
	lmx2594_write_word(channel, 41, mash_seed & 0xFFFF);
	lmx2594_write_word(channel, 40, (mash_seed >> 16) & 0xFFFF);
	lmx2594_write_word(channel, 39, g_cfg->fractional_denominator & 0xFFFF);
	lmx2594_write_word(channel, 38, (g_cfg->fractional_denominator >> 16) & 0xFFFF);
	lmx2594_write_word(channel, 36, integer_N & 0xFFFF);
	lmx2594_write_word(channel, 34, (integer_N >> 16) & 0x3);
	
	if (g_cfg->cal_assist) {
		lmx2594_read_word(channel, 20, &readback);
		lmx2594_write_word(channel, 20, (readback & ~0x3800) | ((vco_core & 0x7) << 11));
		lmx2594_write_word(channel, 17, dac_iset);
		lmx2594_read_word(channel, 78, &readback);
		lmx2594_write_word(channel, 78, (readback & ~0x01FE) | ((cap_code & 0xFF) << 1));
	}

	lmx2594_set_ldmux(channel, 1);
	return 0;
}

void lmx2594_get_status(enum lmx2594_channel channel,
	uint16_t *tune_status, uint16_t *cap_ctrl, uint16_t *dac_iset)
{
	lmx2594_set_ldmux(channel, 0);
	lmx2594_read_word(channel, 110, tune_status);
	lmx2594_read_word(channel, 111, cap_ctrl);
	lmx2594_read_word(channel, 112, dac_iset);
	lmx2594_set_ldmux(channel, 1);

}

/*
 * Test if all installed LMX2594 modules are locked by looking at the GPIOs only
 */
uint32_t lmx2594_check_faults(void)
{
	uint32_t faults = 0;
	
	if (g_cfg->synth1_installed) {
		if (SYNTH1_MISO_Read() == 0) {
			faults |= FAULT_SYNTH1_UNLOCKED;
		}
	}
	if (g_cfg->synth2_installed) {
		if (SYNTH2_MISO_Read() == 0) {
			faults |= FAULT_SYNTH2_UNLOCKED;
		}
	}
	if (g_cfg->synth3_installed) {
		if (SYNTH3_MISO_Read() == 0) {
			faults |= FAULT_SYNTH3_UNLOCKED;
		}
	}
	return faults;
}

/* Test if synthesizers are locked based on gpio only */
void lmx2594_check_locks(uint8_t *s1, uint8_t *s2, uint8_t *s3)
{
	if (g_cfg->synth1_installed) {
		if (SYNTH1_MISO_Read() == 0) {
			*s1 = 0;
		}
		else {
			*s1 = 1;
		}
	}
	else {
		*s1 = 0xFF;
	}
	if (g_cfg->synth2_installed) {
		if (SYNTH2_MISO_Read() == 0) {
			*s2 = 0;
		}
		else {
			*s2 = 1;
		}
	}
	else {
		*s2 = 0xFF;
	}
	if (g_cfg->synth3_installed) {
		if (SYNTH3_MISO_Read() == 0) {
			*s3 = 0;
		}
		else {
			*s3 = 1;
		}
	}
	else {
		*s3 = 0xFF;
	}
}