#include "ucp_cpri.h"
#include "ucp_js_subcrg.h"
#include "ucp_js_ctrl.h"
#include "ucp_sfr_c.h"
#include "jesd_timer.h"
#include "stc_timer.h"
#include "phy_para.h"
#include "ucp_drv_common.h"
#include "ucp_utility.h"
#include "ucp_printf.h"

stStcTimerPara       gStcTimerPara;
extern stPhyScsPara* phyPara;

void rfm_stc_init()
{
    int32_t apeId = get_core_id();
#ifdef PALLADIUM_TEST
    int32_t flag = 1;
    debug_write((DBG_DDR_IDX_DRV_BASE+3+(apeId<<2)), flag);	// 0x4
#endif

    uint32_t pClk = stc_pclk_init();
    if (0 == pClk)
    {
        UCP_PRINT_EMPTY("stc_init: pClk get error! \r\n");
#ifdef PALLADIUM_TEST
        flag = -1;
        debug_write((DBG_DDR_IDX_DRV_BASE+3+(apeId<<2)), flag);
#endif
    return;
    }

#ifdef PALLADIUM_TEST
    flag++;
    debug_write((DBG_DDR_IDX_DRV_BASE+3+(apeId<<2)), flag);
#endif
    stc_timer_init(pClk, STC_RT);

#ifdef PALLADIUM_TEST
    flag++;
    debug_write((DBG_DDR_IDX_DRV_BASE+3+(apeId<<2)), flag);
#endif

    do_write(&(TOD_REG_NF), (0xC0000000 | pClk)); // init finished
}

uint32_t stc_pclk_init()
{
	uint32_t pClk = 0;
	int32_t protoSel = get_protocol_sel();
	int32_t protoOpt = get_protocol_opt();

	if (PROTOCOL_CPRI == protoSel)
	{
		while (1 != (do_read_volatile(SERDES_INIT_FLAG_ADDR))); 	// wait cpri serdes clk init finished
		
		uint32_t serdesTxClk = 0;
		switch (protoOpt)
		{
		case (CPRI_OPTION_7):
		{
			serdesTxClk = 491520000;
			break;
		}
		case (CPRI_OPTION_8):
		{
			serdesTxClk = 316800000;
			break;
		}
		case (CPRI_OPTION_9):
		{
			serdesTxClk = 380160000;
			break;
		}
		case (CPRI_OPTION_10):
		{
			serdesTxClk = 760320000;
			break;
		}
		default:
		{
			serdesTxClk = 316800000;
			break;
		}
		}
		JECS_CRG_PLLSEL &= (~(1 << 22));	// cpri_tx_clk_sel = 0, sel jecs_serdes_tx_clk as cpri_serdes_tx_clk
		//JECS_CTRL_PROTOCOL_SEL &= (~0x1);	// jecs_protocol_sel = 0, sel cpri protocol
		JECS_CTRL_REG39 |= (1 << 6);		// enable ecpri_serdes_tx_clk
		JECS_CRG_PLLSEL &= (~(1 << 6));		// pma_clk_sel2 = 0, sel jecs_ecpri_serdes_tx_clk
		JECS_CRG_PLLSEL |= (0x7 << 7);		// stc_src_clk_sel[2:0]=0x7
		JECS_CRG_CLK_CTRL3 = 0x520000;		// jecs3_div=1, stc pclk = cpri_serdes_tx_clk
		pClk = serdesTxClk;
	}
	else if (PROTOCOL_ECPRI == protoSel)
	{
		while (1 != (do_read_volatile(SERDES_INIT_FLAG_ADDR))); 	// wait ecpri serdes clk init finished
		
		uint32_t serdesTxClk = 0;
		switch (protoOpt)
		{
		case (ECPRI_OPTION_10G):
		{
			serdesTxClk = 322265625; // 322264000; // 805664000; // 156250000; // 
			break;
		}
		case (ECPRI_OPTION_25G):
		{
			serdesTxClk = 805664000; // 2014160000; // 390625000;
			break;
		}
		default:
		{
			serdesTxClk = 322265625; // 322264000; // 805664000; // 156250000;
			break;
		}
		}
//		JECS_CRG_PLLSEL &= (~(1 << 22));	// cpri_tx_clk_sel = 0, sel jecs_serdes_tx_clk as cpri_serdes_tx_clk
		//JECS_CTRL_PROTOCOL_SEL |= 0x2;		// jecs_protocol_sel = 2, sel ecpri protocol
		JECS_CTRL_REG39 |= (1 << 6);		// enable ecpri_serdes_tx_clk
		__ucps2_synch(0);
		JECS_CRG_PLLSEL &= (~(1 << 6));		// pma_clk_sel2 = 0, sel jecs_ecpri_serdes_tx_clk
		JECS_CRG_PLLSEL |= (0x7 << 7);		// stc_src_clk_sel[2:0]=0x7
		JECS_CRG_CLK_CTRL3 = 0x520000;		// jecs3_div=1, stc pclk = cpri_serdes_tx_clk
		pClk = serdesTxClk;
	}
	else if (PROTOCOL_JESD == protoSel)
	{
		while (JESD_CLK_INIT > (do_read_volatile(SERDES_INIT_FLAG_ADDR))); 	// wait jesd serdes clk init finished
		JECS_CRG_PLLSEL |= (0x6 << 7);		// stc_src_clk_sel[2:0]=0x6
		JECS_CRG_CLK_CTRL3 = 0x520000;		// jecs3_div=1, stc pclk = cpri_serdes_tx_clk
		// laneRate = IQ_sample_rate * M * N' * (10/8)/L // max_laneRate = 12.5Gbps
		// serdesTxClk = laneRate/40;
		// pClk = serdesTxClk;
		uint32_t samClk = do_read_volatile(JESD_TX_SAMPLE_RATE);
		uint32_t jesdPara = do_read_volatile(JESD_TX_CH_PARA);
		uint8_t paraL = jesdPara & 0xFF;
		uint8_t paraM = (jesdPara >> 8) & 0xFF;
		uint8_t paraN = (jesdPara >> 16) & 0xFF;
		if ((0 == samClk) || (0 == jesdPara))
		{
			pClk = 0;
		}
		else
		{
			pClk = (uint64_t)(samClk/paraL/40)*paraM*paraN*10/8; // (uint64_t)(122880000/4/40)*8*16*10/8;
		}
	}
	else
	{
		PETP_CLK_CFG_REG	 = 0x523000;	// ECS_PCLK=1G/4=250M
		JECS_CRG_CLK_CTRL3	 = 0x520000;	// STC_PCLK=ECS_PCLK/1=250M
//		JECS_CRG_PLLSEL		&= (~(1 << 9));	// stc_src_clk_sel2=0
		pClk = 250000000; // 100000000;

		UCP_PRINT_EMPTY("stc_pclk_init: use soc_crg to generate pclk, 250M. \r\n");
	}
	
    JECS_CRG_STC_RST_CTRL |= BIT24; // enable stc

	return pClk;
}

int32_t gcd_stein(uint32_t x, uint32_t y)
{
	if (x < y)
	{
		int32_t tmp = x;
		x = y;
		y = tmp;
	}
	if (0 == (x%y))
	{
		return y;
	}
	if ((0 == (x%2)) && (0 == (y%2)))
	{
		return (2*gcd_stein(x>>1, y>>1));
	}
	else if ((0 == (x%2)) && (0 != (y%2)))
	{
		return (gcd_stein(x>>1, y));
	}
	else if ((0 != (x%2)) && (0 == (y%2)))
	{
		return (gcd_stein(x, y>>1));
	}
	else if ((0 != (x%2)) && (0 != (y%2)))
	{
		return (gcd_stein(x, ((x-y)>>1)));
	}
	else
	{
		return 0;
	}
}

void stc_timer_init(uint32_t pClk, uint32_t valR)
{
    int32_t apeId = get_core_id();
#ifdef PALLADIUM_TEST
    int32_t flag = 1;
    debug_write((DBG_DDR_IDX_DRV_BASE+2+(apeId<<2)), flag); // 0x8
#endif
    stc_timer_tod_init(pClk);
#ifdef PALLADIUM_TEST
    flag++;
    debug_write((DBG_DDR_IDX_DRV_BASE+2+(apeId<<2)), flag);
#endif
#if 0
	stc_pp1s_src_init(STC_PP1S_SRC_GPS);	// select gps pp1s as stc ppls input
#ifdef PALLADIUM_TEST
	flag++;
	debug_write((DBG_DDR_IDX_DRV_BASE+64+2+(apeId<<2)), flag);
#endif
#endif
    stc_timer_para_init(pClk, valR);
#ifdef PALLADIUM_TEST
    flag++;
    debug_write((DBG_DDR_IDX_DRV_BASE+2+(apeId<<2)), flag);
#endif

    stc_timer_local_init();
#ifdef PALLADIUM_TEST
    flag++;
    debug_write((DBG_DDR_IDX_DRV_BASE+2+(apeId<<2)), flag);
#endif

    stc_timer0_para_init();
#ifdef PALLADIUM_TEST
    flag++;
    debug_write((DBG_DDR_IDX_DRV_BASE+2+(apeId<<2)), flag);
#endif

    stc_timer1_para_init();
#ifdef PALLADIUM_TEST
    flag++;
    debug_write((DBG_DDR_IDX_DRV_BASE+2+(apeId<<2)), flag);
#endif
}

void stc_timer_para_init(uint32_t pClk, uint32_t valR)
{
    //int apeId = get_core_id();
    uint32_t remain = 0;
    uint32_t gcd = 0;

    memset(&gStcTimerPara, 0, sizeof(stStcTimerPara));
    gStcTimerPara.pClk = pClk;
    gStcTimerPara.R = valR;
    gStcTimerPara.usR = valR / 1000000;
    gStcTimerPara.N1 = valR/pClk;
    if (gStcTimerPara.N1 * pClk == valR)
    {
        gStcTimerPara.N1 -= 1;
    }
    gStcTimerPara.N = gStcTimerPara.N1 + 1;
	
    debug_write((DBG_DDR_IDX_DRV_BASE+24), gStcTimerPara.pClk); // 0x60
    debug_write((DBG_DDR_IDX_DRV_BASE+25), gStcTimerPara.N);	// 0x64
    
    remain = valR-gStcTimerPara.N1*pClk;
    gcd = gcd_stein(pClk, remain);
    if (0 == gcd)
    {
        UCP_PRINT_EMPTY("gcd error! \r\n");
        return;
    }
    gStcTimerPara.A = remain / gcd;
    gStcTimerPara.C = pClk / gcd;

    UCP_PRINT_EMPTY("pclk = 0x%x. \r\n", gStcTimerPara.pClk);
    UCP_PRINT_EMPTY("R = 0x%x. \r\n", gStcTimerPara.R);
    UCP_PRINT_EMPTY("N = 0x%x. \r\n", gStcTimerPara.N);
    UCP_PRINT_EMPTY("N1 = 0x%x. \r\n", gStcTimerPara.N1);
    UCP_PRINT_EMPTY("A = 0x%x. \r\n", gStcTimerPara.A);
    UCP_PRINT_EMPTY("C = 0x%x. \r\n", gStcTimerPara.C);

    debug_write((DBG_DDR_IDX_DRV_BASE+26), gStcTimerPara.A);	// 0x68
    debug_write((DBG_DDR_IDX_DRV_BASE+27), gStcTimerPara.C);	// 0x6c

}

void stc_timer_tod_init(uint32_t pClk)
{
    stc_tod_para_init(pClk);
    //stc_timer_todint_init();

    //do_write(STC_TOD_INT_ADDR, 0);	// clear tod int flag
    stc_pp1s_out_set();
}

void stc_pp1s_src_init(uint8_t srcId)
{
	//                             gnss_sel     stc_sel     cpri_sel
	//JECS_CTRL_CPRI_GMAC_PHY_INT = (0x0 << 4) | (0x5 << 7) | (0x6<<10);
	JECS_CTRL_CPRI_GMAC_PHY_INT &= ~(0x7 << 7);
	JECS_CTRL_CPRI_GMAC_PHY_INT |= (srcId << 7);	// select gps pp1s as stc ppls input
}

void stc_pp1s_out_set()
{

	PC_PMUX_REG &= ~(0x3<<28);
	PC_PMUX_REG |= (0x2<<28); // PC14: GNSS_PPS
    
	JECS_CTRL_CPRI_GMAC_PHY_INT &= ~(0x7 << 4);
	JECS_CTRL_CPRI_GMAC_PHY_INT |= (0x6 << 4);	// select tod_one_pps_out as gnss ppls output
}

void stc_timer_local_init()
{
    LTBG_REG_R = gStcTimerPara.R;
    LTBG_TIME_SET = 0;
    LTBG_REG_C = gStcTimerPara.C;
	__ucps2_synch(0);
    LTBG_REG_A = gStcTimerPara.A;
    LTBG_REG_N = gStcTimerPara.N;
    LTBG_REG_N1 = gStcTimerPara.N1;
}

void stc_timer0_para_init()
{
    LTBG0_REG_R = gStcTimerPara.R;
    LTBG0_TIME_SET = 0;
    LTBG0_REG_C = gStcTimerPara.C;
	__ucps2_synch(0);
    LTBG0_REG_A = gStcTimerPara.A;
    LTBG0_REG_N = gStcTimerPara.N;
    LTBG0_REG_N1 = gStcTimerPara.N1;
}

void stc_timer1_para_init()
{
    LTBG1_REG_R = gStcTimerPara.R;
    LTBG1_TIME_SET = 0;
    LTBG1_REG_C = gStcTimerPara.C;
	__ucps2_synch(0);
    LTBG1_REG_A = gStcTimerPara.A;
    LTBG1_REG_N = gStcTimerPara.N;
    LTBG1_REG_N1 = gStcTimerPara.N1;
}

void stc_tod_para_init(uint32_t pClk)
{
    //TOD_REG_FT_LO = 0;    // 0xB0AAAAAA;  // 0x11AAAAAA; //
    //TOD_REG_FT_HI = 40;   // 40;          //4;  //
    double nsOfClk = 1000000000.0 / pClk;
    uint32_t hiOfFt = 1000000000 / pClk;
    uint64_t coff = (1ULL<<32);
    uint32_t loOfFt = (uint32_t)((double)(nsOfClk - hiOfFt) * coff); // ???

#ifdef PALLADIUM_TEST
	debug_write((DBG_DDR_IDX_DRV_BASE+5), loOfFt); // 0x8
	debug_write((DBG_DDR_IDX_DRV_BASE+6), hiOfFt); // 0xc
#endif
    do_write((&TOD_REG_FT_LO), loOfFt);
    do_write((&TOD_REG_FT_HI), hiOfFt);

    uint32_t loOfRt = 0;
    uint32_t hiOfRt = 0;
    //#ifdef PALLADIUM_TEST
    //  hiOfRt = STC_RT_PALLADIUM; // one tod period: 100ms
    //  do_write(STC_RT_ADDR, STC_RT_PALLADIUM);
    //#else
    hiOfRt = STC_RT;
    do_write(STC_RT_ADDR, STC_RT);
    //#endif

    UCP_PRINT_EMPTY("RT LO = 0x%x. \r\n", loOfRt);
    UCP_PRINT_EMPTY("RT HI = 0x%x. \r\n", hiOfRt);

    do_write((&TOD_REG_RT_LO), loOfRt);
    do_write((&TOD_REG_RT_HI), hiOfRt);

    TOD_REG_CONFIG |= (1<<3);	// onepps_output_enable
    do_write(STC_ONEPPS_OUT_ADDR, 1);
}



