#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

#include "xsysmon.h"
#include "xspi.h"

#include "project.h"
#include "hmc7044.h"
#include "ad9081_hal_functions.h"
#include "ad9081_app_helper.h"
#include "adi_ad9081.h"
#include "adi_hmc7044.h"

/*============= UC DATA ====================*/
/*Eval Application Usecase Datapath Configuration*/
/*Refer to uc_settings.c for list of uc definitions*/
/*Clock Scheme Sources and Frequencies*/
extern uint64_t clk_hz[][4];
/* Transmit Datapath Configuration*/
extern uint8_t  tx_dac_chan_xbar[][4];  /*Transmit Channel and Main DAC Datapaths*/
extern int8_t   tx_chan_gain[][8];      /*Transmit Channel Gain DAC Datapaths*/
extern int64_t  tx_main_shift[][4];     /*Transmit Main/Coarse DUC NCO Frequency Setting*/
extern int64_t  tx_chan_shift[][8];     /*Transmit Channel/Fine DUC NCO Frequency Setting*/
extern uint8_t  tx_interp[][2];         /*Trnacmit Data Interpolation for Coarse/Fine DUCs */
extern uint8_t  rx_cddc_select[];
extern uint8_t  rx_fddc_select[];
extern int64_t  rx_cddc_shift[][4];
extern int64_t  rx_fddc_shift[][8];
extern uint8_t  rx_cddc_dcm[][4];
extern uint8_t  rx_fddc_dcm[][8];
extern uint8_t  rx_cddc_c2r[][4];
extern uint8_t  rx_fddc_c2r[8];

extern adi_ad9081_jtx_conv_sel_t jtx_conv_sel[][2];
extern adi_cms_jesd_param_t    jrx_param[];
extern adi_cms_jesd_param_t    jtx_param[][2];
extern uint8_t jtx_chip_dcm[][2];
/*============= END DATA ====================*/
void set_lane_cal(int lane, int pre, int post, int swing) {
	// Select Lane
	Xil_Out32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x24, lane);

	// Update Precursor 0 - 20 Valid (~0.22dB steps)
	Xil_Out32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x418, pre);
	// Update Postcursor 0 - 32 Valid (~0.22dB steps)
	Xil_Out32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x414, post);
	// UPdate Drive Strength
	Xil_Out32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x508, swing);


	// Update RX Equalizer Setting 0 = DFE 1 = LPM
	Xil_Out32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x608, 0);
	Xil_Out32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x60C, 1);
	Xil_Out32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x60C, 0);

	uint32_t val0 = Xil_In32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x418);
	uint32_t val1 = Xil_In32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x414);
	uint32_t val2 = Xil_In32(XPAR_JESD_JESD204_PHY_0_BASEADDR + 0x608);
	xil_printf("Lane %d, Pre %d, Post %d, RX %d\r\n", lane, val0, val1, val2);
}


void print_rx_ila() {
	xil_printf("                CFG0       CFG1       CFG2       CFG3       CFG4       CFG5       CFG6       CFG7       \r\n");

	for (int i = 0; i < 8; i++) {
		xil_printf("Lane %2d ILA: ", i);
		for (int j = 0; j < 8; j++) {
			xil_printf("0x%08x ", Xil_In32(JESD_RX + 0x400 + i * 0x080 + 0x30 + j*4));
		}
		xil_printf("\r\n");
	}
}

void reset_jesd() {
	uint32_t gpo = Xil_In32(GPO_REG);
	Xil_Out32(GPO_REG, gpo | ALL_JESD_RST);
	vTaskDelay(100);
	Xil_Out32(GPO_REG, gpo);

	// Wait for reset to complete
	xil_printf("Wait for RX to complete reset\r\n");
	int val = 1;
	while (val) {
		val = Xil_In32(JESD_RX + RESET_REG);
		xil_printf("rx reset state: 0x%x\r\n", val);
		xil_printf("GPI:                 0x%x\r\n", Xil_In32(GPI_REG));
		vTaskDelay(10);
	}

	xil_printf("Wait for TX to complete reset\r\n");
	val = 1;
	while (val) {
		val = Xil_In32(JESD_TX + RESET_REG);
		xil_printf("tx reset state: 0x%x\r\n", val);
		vTaskDelay(10);
	}
}

void check_jesd_core_clk() {
	uint32_t gpi = Xil_In32(GPI_REG);
	uint32_t jesd_core_clk_locked = gpi & 0x8;
	if (jesd_core_clk_locked == 0) {
		// Not locked, try the other clock input
		xil_printf("JESD Core Clk not locked!!! Trying other clock 0x%x\r\n", gpi);
		uint32_t gpo = Xil_In32(GPO_REG);
		gpo |= JESD_CORE_CLK_SEL;
		Xil_Out32(GPO_REG, gpo);
		vTaskDelay(100);
		uint32_t gpi = Xil_In32(GPI_REG);
		jesd_core_clk_locked = gpi & 0x8;
	}

	if (jesd_core_clk_locked) {
		xil_printf("JESD Core Clk Locked!\r\n");
	} else {
		xil_printf("JESD Core Clk Locked FAILED!\r\n");
		vTaskDelay(1000);
	}
}

adi_ad9081_device_t * ad9081_dev_ptr;

void setup_data_converter() {

	// Reset Ethernet
	setBit(GPO_REG, 15);
	vTaskDelay(1);
	clearBit(GPO_REG, 15);

	ad9081_hal_init();
	hmc7044_init();

	// select use case
	int uc = 0;

    uint64_t app_jrx_lane_rate = 0;
	uint64_t app_jtx_lane_rate[2] = {0};
	int err;


	//	FPGA   	AD9081
	//  M2C		ADC
	//  7		1
	//	6		7
	//	5		0
	//	4		6
	//	3		5
	//	2		4
	//	1		3
	//	0		2
	// 5 7 0 1 2 3 4 6

	//	FPGA   	AD9081
	//  C2M		DAC
	//  7		0
	//	6		4
	//	5		1
	//	4		7
	//	3		6
	//	2		5
	//	1		3
	//	0		2
	// 7 5 0 1 6 2 3 4



    /* connect to platform */
    adi_ad9081_device_t ad9081_dev = {
        .hal_info = {
            .sdo = SPI_SDO,
            .msb = SPI_MSB_FIRST,
            .addr_inc = SPI_ADDR_INC_AUTO,
            .log_write = ad9081_hal_log_write,
            .delay_us = ad9081_hal_wait_us,
            .spi_xfer = ad9081_hal_spi_xfer_ad9081,
            .reset_pin_ctrl = ad9081_hal_hw_rst_pin_ctrl_ad9081,
        },
        .serdes_info = {
            .ser_settings = { /* ad9081 jtx */
//                .lane_settings = {
//                    {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_0DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
//                    {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_0DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
//                    {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_0DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
//                    {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_0DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
//                    {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_0DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
//                    {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_0DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
//                    {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_0DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
//                    {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_0DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
//                },
                    .lane_settings = {
                        {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_3DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
                        {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_3DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
                        {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_3DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
                        {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_3DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
                        {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_3DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
                        {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_3DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
                        {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_3DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
                        {.swing_setting = AD9081_SER_SWING_850,.pre_emp_setting = AD9081_SER_PRE_EMP_3DB,.post_emp_setting = AD9081_SER_POST_EMP_0DB},
                    },

                .invert_mask = 0x00,
				.lane_mapping = { { 5, 7, 0, 1, 2, 3, 4, 6 }, { 2, 0, 7, 7, 7, 7, 3, 1 } }, /* link0, link1 */

//				.lane_mapping = { { 5, 7, 0, 1, 2, 3, 4, 6 }, { 2, 0, 7, 7, 7, 7, 3, 1 } }, /* link0, link1 */ //204B // 5, 7, 0, 1, 2, 3, 4, 6
//				.lane_mapping = { { 0, 1, 2, 3, 4, 5, 6, 7 }, { 2, 0, 7, 7, 7, 7, 3, 1 } }, /* link0, link1 */ //204B // 2, 3, 4, 5, 6, 0, 7, 1
//				.lane_mapping = { { 2, 1, 0, 3, 4, 5, 6, 7 }, { 2, 0, 7, 7, 7, 7, 3, 1 } }, /* link0, link1 */ //204B // 2, 3, 4, 5, 6, 0, 7, 1
//				.lane_mapping = { { 2, 3, 0, 1, 4, 5, 6, 7 }, { 2, 0, 7, 7, 7, 7, 3, 1 } }, /* link0, link1 */ //204B // 2, 3, 4, 5, 6, 0, 7, 1
//				.lane_mapping = { { 0, 1, 4, 5, 2, 3, 6, 7 }, { 2, 0, 7, 7, 7, 7, 3, 1 } }, /* link0, link1 */ //204B
            },
            .des_settings = { /* ad9081 jrx */
                .boost_mask = 0xFF,
                .invert_mask = 0x00,
                .ctle_filter = { 2, 2, 2, 2, 2, 2, 2, 2 },
//				.ctle_filter = { 4, 4, 4, 4, 4, 4, 4, 4 },
                .cal_mode = AD9081_CAL_MODE_RUN,
                .ctle_coeffs = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}, /* CTLE 1-4 for lanes 0-7 */
				.lane_mapping = { { 7, 5, 0, 1, 6, 2, 3, 4 }, { 4, 5, 6, 7, 0, 1, 2, 3 } }, /* link0, link1 */
            }
        },
        .clk_info = {
            .sysref_mode = SYSREF_NONE,
//			.sysref_mode = SYSREF_ONESHOT,
//			.sysref_mode = SYSREF_CONT,
        }
    };

    adi_hmc7044_device_t hmc7044_dev = {
        .hal_info = {
            .spi_xfer = hal_spi_xfer_hmc7044,
            .delay_us = ad9081_hal_wait_us
        }
    };

    ad9081_dev_ptr = &ad9081_dev;

    uint8_t ad9081_rev[3];
    adi_ad9081_device_api_revision_get(&ad9081_dev, &ad9081_rev[0], &ad9081_rev[1], &ad9081_rev[2]);
    printf("AD9081 API v%d.%d.%d\n", ad9081_rev[0], ad9081_rev[1], ad9081_rev[2]);

    printf("APP: Configure Usecase:%d, Tx Path: DAC Clk: %lld, JESD Rx Mode: %d & Rx Path: ADC CLK: %lld, Jesdmode Tx Mode: %d \r\n",
        uc, clk_hz[uc][2], jrx_param[uc].jesd_mode_id, clk_hz[uc][3], jtx_param[uc][0].jesd_mode_id);

    adi_ad9081_device_reset(&ad9081_dev, AD9081_HARD_RESET);

	app_calc_tx_lane_rate(clk_hz[uc], &jrx_param[uc], tx_interp[uc], &app_jrx_lane_rate);
	app_calc_rx_lane_rate(clk_hz[uc], jtx_param[uc], jtx_chip_dcm[uc], app_jtx_lane_rate);
	printf("APP: Tx Lane Rate : %llu Rx0 Lane Rate : %llu Rx1 Lane Rate : %llu \r\n", app_jrx_lane_rate, app_jtx_lane_rate[0], app_jtx_lane_rate[1]);

	//--------------------------------------------------------------------------------------------------------------------
    printf("APP: Configure Platform Reference Clocks\n");
    //--------------------------------------------------------------------------------------------------------------------
    // configure 7044 to generate clock
	uint64_t ad9081_clk = clk_hz[uc][0];
	uint64_t fpga_clk = clk_hz[uc][1];
	uint64_t sysref_clk_204 = fpga_clk/32;
	if (((jrx_param[uc].jesd_jesdv == 0) && (jtx_param[uc][0].jesd_jesdv == 0))) {
		sysref_clk_204 = fpga_clk/16;
	}
	uint64_t hmc7044_crystal_input = 100e6;
	uint64_t jesd_core_clk = fpga_clk;

	/* Configure HMC7044 SYSREF frequency output channels for SC1 Use Case */
	ad9081_dev.clk_info.sysref_clk = &hmc7044_dev;
	ad9081_dev.clk_info.sysref_ctrl = app_sysref_clk_src_sel;
	/* Calculate SYSREF Freq for SC1 Use cases*/
	if (((jrx_param[uc].jesd_subclass == 1) && (jtx_param[uc][0].jesd_subclass == 1))) {
		ad9081_dev.clk_info.sysref_mode = SYSREF_CONT;
	}
	if (err = adi_ad9081_sync_sysref_frequency_set(&ad9081_dev, &sysref_clk_204, ad9081_clk, clk_hz[uc][2], clk_hz[uc][3], tx_interp[uc][0], tx_interp[uc][1], rx_cddc_dcm[uc], rx_fddc_dcm[uc], jtx_param[uc][0].jesd_duallink, &jrx_param[uc], jtx_param[uc]), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	printf("APP: SYSREF Clk: %lld\n", sysref_clk_204);

	/*Configure HMC7044 to Provide Clocks For Txfe & FPGA*/
	uint8_t  hmc_priority[] = { 1, 0, 2, 3 };
	uint16_t hmc_out_ch = HMC7044_OP_CH_0 | HMC7044_OP_CH_2 | HMC7044_OP_CH_3 | HMC7044_OP_CH_6 | HMC7044_OP_CH_8 | HMC7044_OP_CH_10 | HMC7044_OP_CH_12 | HMC7044_OP_CH_13;
	uint64_t hmc_out_204[14];

	hmc_out_204[0] = fpga_clk;
	hmc_out_204[1] = 0;
	hmc_out_204[2] = ad9081_clk;
	hmc_out_204[3] = sysref_clk_204;
	hmc_out_204[4] = 0;
	hmc_out_204[5] = 0;
	hmc_out_204[6] = jesd_core_clk;
	hmc_out_204[7] = 0;
	hmc_out_204[8] = fpga_clk;
	hmc_out_204[9] = 0;
	hmc_out_204[10] = jesd_core_clk;
	hmc_out_204[11] = 0;
	hmc_out_204[12] = fpga_clk;
	hmc_out_204[13] = sysref_clk_204;

	/* Disable SYSREF signal channels for Subclass 0 */
	if (((jrx_param[uc].jesd_subclass == 0) && (jtx_param[uc][0].jesd_subclass == 0))) {
		hmc_out_ch &= ~(HMC7044_OP_CH_3 | HMC7044_OP_CH_13);
		hmc_out_204[3] = 0;
		hmc_out_204[13] = 0;
		xil_printf("Disabling Sysref!!!!!!!\r\n");
	}

	if (err = adi_hmc7044_device_init(&hmc7044_dev), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_hmc7044_device_reset(&hmc7044_dev, 0), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);

   /*Disable Configure but Output Drivers Prior to Clocking Scheme Configuration*/
	for (int i = 0; i< HMC7044_NOF_OP_CH; i++) {
		if (err = adi_hmc7044_output_config_set(&hmc7044_dev, i, HMC7044_OP_SIG_CH_DIV, 0, 0, 0), err != API_CMS_ERROR_OK)
			error_print(__LINE__, err);
	}
	if (err = adi_hmc7044_input_reference_set(&hmc7044_dev, 0, IPBUFFER_INTERNAL_100_OHM_EN | IPBUFFER_AC_COUPLED_MODE_EN, 1), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_hmc7044_input_reference_set(&hmc7044_dev, 1, IPBUFFER_INTERNAL_100_OHM_EN | IPBUFFER_AC_COUPLED_MODE_EN, 1), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_hmc7044_input_reference_los_config_set(&hmc7044_dev, 7, 0, 0), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_hmc7044_vco_sel_set(&hmc7044_dev, HMC7044_VCO_INTERNAL_3GHZ, 0), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_hmc7044_output_sync_config_set(&hmc7044_dev, 3, ((ad9081_dev.clk_info.sysref_mode == SYSREF_ONESHOT) ? 1 : 0), 1, 1), err != API_CMS_ERROR_OK) /* clkout3 as async mode or pulse gen mode*/
		error_print(__LINE__, err);
	if (err = adi_hmc7044_output_sync_config_set(&hmc7044_dev, 13, ((ad9081_dev.clk_info.sysref_mode == SYSREF_ONESHOT) ? 1 : 0), 1, 1), err != API_CMS_ERROR_OK) /* clkout13 as async mode or pulse gen mode*/
		error_print(__LINE__, err);
	if (err = adi_hmc7044_output_multi_slip_config_set(&hmc7044_dev, 3, 0, 0), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_hmc7044_output_multi_slip_config_set(&hmc7044_dev, 13, 0, 0), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);

	/* Configure number of pulses generated from HMC7044 SYSREF - default set to continuous pulses */
	if (err = adi_hmc7044_sysref_config_set(&hmc7044_dev, HMC7044_SYSREF_CONTINUOUS_MODE), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);

	if (err = adi_hmc7044_clk_config(&hmc7044_dev, HMC7044_CLK_IN_0, hmc_priority, hmc7044_crystal_input, hmc7044_crystal_input, hmc_out_ch, hmc_out_204), err != API_CMS_ERROR_OK) {
		if (err == API_CMS_ERROR_INVALID_PARAM)
			printf("APP: HMC7044: Invalid param passed.\n");
		error_print(__LINE__, err);
	}

	if (err = adi_hmc7044_device_sysref_enable_control_set(&hmc7044_dev, 1, 1), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_hmc7044_high_performance_set(&hmc7044_dev), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_hmc7044_reg_update(&hmc7044_dev), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_hmc7044_reseed_request_set(&hmc7044_dev), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = ad9081_hal_wait_us(NULL, 100000), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	uint8_t  hmc_pll_locked = 0;
	if (err = adi_hmc7044_device_pll_lock_status_get(&hmc7044_dev, &hmc_pll_locked), err != API_CMS_ERROR_OK) {
		printf("APP: HMC7044: PLL is not locked.\n");
		error_print(__LINE__, err);
	}
    printf("APP: Reference Clocks Configured\n");
//    check_jesd_core_clk();

    //--------------------------------------------------------------------------------------------------------------------
    /* AD9081 Device Data Path Configuration Sequenece
    *  Do AD9081 Device RESET, adi_ad9081_device_reset
    *  Do AD9081 Device Initialization: adi_ad9081_device_init
    *  Do AD9081 Device Clocks Config (REF CLK, DAC CLK, ADC Clock):adi_ad9081_device_clk_config_set
    *  Do AD9081 Device TX Datapath (JESD RX to DAC) Primary Configuration: adi_ad9081_device_startup_tx
    *  Do AD9081 Device Tx Datapath Secondary Configuration as per specific UC requirements
    *       eg  TX Gain APIs
    *                     - adi_ad9081_dac_duc_nco_gains_set
    *           TX Datapath Customization API:
    *                     -adi_ad9081_dac_modulation_mux_mode_set
    *                     -adi_ad9081_dac_xbar_set
    *           TX Test Modes APIs:
    *                     -adi_ad9081_device_startup_nco_test_mode
    * Do AD9081 Device RX Datapath (ADC to JESD TX) Primary Configuration: adi_ad9081_device_startup_rx
    * Do AD9081 Device Rx Datapath Secondary Configuration as per specific UC requirements
    *            eg: RX Gain APIs:
    *                   - adi_ad9081_adc_ddc_fine_gain_set
    *              RX Datapath Customization APIs:
    *                   - adi_ad9081_adc_nyquist_zone_set
    *                   - adi_ad9081_adc_xbar_set
    *              RX Test Mode APIs:
    *                   - adi_ad9081_jesd_loopback_mode_set
    *                   - adi_ad9081_adc_ddc_coarse_nco_mode_set
    *
    */
    //--------------------------------------------------------------------------------------------------------------------
    uint8_t adc_cddc_xbar, cddc_fddc_xbar;
    uint16_t phase = 0;
    printf("APP: Configure ad9081 Device\n");

    /* reset ad9081 */
    if (err = adi_ad9081_device_reset(&ad9081_dev, AD9081_SOFT_RESET), err != API_CMS_ERROR_OK) {
        printf("APP: ad9081 Initialisation Error\n");
        error_print(__LINE__, err);
    }

    /* init ad9081 */
    if (err = adi_ad9081_device_init(&ad9081_dev), err != API_CMS_ERROR_OK) {
        printf("APP: ad9081 Initialisation Error\n");
        error_print(__LINE__, err);
    }

    /* setup ad9081 clock */
    err = adi_ad9081_device_clk_config_set(&ad9081_dev, clk_hz[uc][2], clk_hz[uc][3], clk_hz[uc][0]);
    uint8_t ad9081_pll_locked = 0;
    adi_ad9081_device_clk_pll_lock_status_get(&ad9081_dev, &ad9081_pll_locked);
    if (ad9081_pll_locked == 0x3) {
        printf("APP: ad9081 PLL LOCKED\n");
    }
    if (err != API_CMS_ERROR_OK) {
       printf("APP: Clock Configuration error\n");
       error_print(__LINE__, err);
    }

//    // DAC NCO TEST MODE!!!!!!
//    xil_printf("!!!!!!!!!!! DAC NCO TEST MODE !!!!!!!!!!!!!!!!!!!!!!!\r\n");
//    // DC Offset from examples was 0x5a82
//    err = adi_ad9081_device_startup_nco_test_mode(&ad9081_dev,
//												  tx_interp[uc][0],
//												  tx_interp[uc][1],
//												  tx_dac_chan_xbar[uc],
//												  tx_main_shift[uc],
//												  tx_chan_shift[uc],
//												  &jrx_param[uc],
//												  (uint16_t)pow(10, ((0 + 20 * log10(0x5a82)) / 20)));
//    if (err != API_CMS_ERROR_OK) {
//    	error_print(__LINE__, err);
//    }

	/* start ad9081 tx */
	if (err = adi_ad9081_device_startup_tx(&ad9081_dev, tx_interp[uc][0], tx_interp[uc][1], tx_dac_chan_xbar[uc],
		tx_main_shift[uc], tx_chan_shift[uc], &jrx_param[uc]), err != API_CMS_ERROR_OK) {
		printf("APP: ad9081 Tx  Path Configuration Error \n");
		if (err == API_CMS_ERROR_JESD_PLL_NOT_LOCKED) {
			printf("APP: ad9081 Tx  Path Configuration JESD PLL Not Locked \n");
		}
		error_print(__LINE__, err);
	}
	/* Setup ad9081 tx channel gain */
	uint16_t tx_chan_gains[8];
	for (int i = 0; i < 8; i++)
		tx_chan_gains[i] = (uint16_t)(pow(2, 11) * pow(10, (tx_chan_gain[uc][i]) / 20.0));
	if (err = adi_ad9081_dac_duc_nco_gains_set(&ad9081_dev, tx_chan_gains), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);

	/*Configure Primary Rx Datapath Settings*/
	if (err = adi_ad9081_device_startup_rx(&ad9081_dev, rx_cddc_select[uc], rx_fddc_select[uc], rx_cddc_shift[uc], rx_fddc_shift[uc],
		rx_cddc_dcm[uc], rx_fddc_dcm[uc], rx_cddc_c2r[uc], rx_fddc_c2r, jtx_param[uc], jtx_conv_sel[uc]), err != API_CMS_ERROR_OK) {
		printf("APP: ad9081 Rx Path Configuration Error \n");
		error_print(__LINE__, err);
	}

	/* Configure Synchronization Options as per Application Use-case*/
	xil_printf("APP: JESD RX Synchronization Mode: %s\r\n", ((jrx_param[uc].jesd_subclass == JESD_SUBCLASS_1) ? "JESD_SUBCLASS_1" : "JESD_SUBCLASS_0"));
	if (jtx_param[uc][0].jesd_subclass == 1) {
		/* Configure Sysref Receiver and Input mode */
		if (err = adi_ad9081_sync_sysref_input_config_set(&ad9081_dev, COUPLING_AC, SIGNAL_CML, 0, 0), err != API_CMS_ERROR_OK)
			error_print(__LINE__, err);
		/* Configure cddc nco sync */
		if (err = adi_ad9081_adc_ddc_coarse_sync_enable_set(&ad9081_dev, AD9081_ADC_CDDC_ALL, 1), err != API_CMS_ERROR_OK)
			error_print(__LINE__, err);
		if (err = adi_ad9081_adc_ddc_coarse_sync_next_set(&ad9081_dev, AD9081_ADC_CDDC_ALL, 1), err != API_CMS_ERROR_OK)
			error_print(__LINE__, err);
		if (err = adi_ad9081_adc_ddc_coarse_trig_nco_reset_enable_set(&ad9081_dev, AD9081_ADC_CDDC_ALL, 0), err != API_CMS_ERROR_OK)
			error_print(__LINE__, err);
		/* Perform oneshot sync */
		if (err = adi_ad9081_jesd_oneshot_sync(&ad9081_dev,  1), err != API_CMS_ERROR_OK){
			if (err == API_CMS_ERROR_JESD_SYNC_NOT_DONE) {
				printf("APP: JESD Oneshot Synchronization Not Completed\n");
			}
			error_print(__LINE__, err);
		}
		if (err = adi_ad9081_jesd_sysref_monitor_phase_get(&ad9081_dev, &phase), err != API_CMS_ERROR_OK)
			error_print(__LINE__, err);
		printf("APP: Phase offset between incoming SYSREF and internal LMFC/LEMC: %d DAC clock units\n", phase);
	} else {
		/* Power down Sysref Receiver circuitry*/
		if (err = adi_ad9081_jesd_sysref_input_mode_set(&ad9081_dev, jrx_param[uc].jesd_subclass > 0 ? 1 : 0, jrx_param[uc].jesd_subclass > 0 ? 1 : 0, COUPLING_AC), err != API_CMS_ERROR_OK)
			error_print(__LINE__, err);
		/* Perform oneshot sync */
		if (err = adi_ad9081_jesd_oneshot_sync(&ad9081_dev,  0), err != API_CMS_ERROR_OK){
			if (err == API_CMS_ERROR_JESD_SYNC_NOT_DONE) {
				xil_printf("APP: JESD Oneshot Synchronization Not Completed\r\n");
			}
			error_print(__LINE__, err);
		}
	}

    /* SYSTEM Link Bring Up Sequenece
    *  Check AD9081 JESD PLL Lock Status
    *  Enable AD9081 JESD Rx/ JESD TX Links
    *  Ensure FPGA JESD RX/ JESD TX Links are configured
    *  Ensure FPGA is transmitting Data
    *  Toggle AD9081 JESD RX Links Enable
    *  Run AD9081 JESD RX 204C Calibration if Lane Rate is above Threshold AD9081_JESDRX_204C_CAL_THRESH
    *  Toggle AD9081 JESD RX Links Enable
    *  Check Link Status after short period time
    */
	/* Check JESD PLL LOCK Status */
	uint8_t uc_jesd_pll_status = 0x00;
	err = adi_ad9081_jesd_pll_lock_status_get(&ad9081_dev, &uc_jesd_pll_status);
	printf("APP: ad9081 JESD PLL lock Status: %s : %s\n", (uc_jesd_pll_status ? "LOCKED" : "NOT LOCKED"), (uc_jesd_pll_status ? "Enabling Links" : "Exiting"));
	if (err != API_CMS_ERROR_OK) {
		error_print(__LINE__, err);
	}

	// Enable RX and TX Link
	if (err = adi_ad9081_jesd_tx_link_enable_set(&ad9081_dev, (jtx_param[uc][0].jesd_duallink > 0) ? AD9081_LINK_ALL : AD9081_LINK_0, 1), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_ad9081_jesd_rx_link_enable_set(&ad9081_dev, (jrx_param[uc].jesd_duallink > 0) ? AD9081_LINK_ALL : AD9081_LINK_0, 1), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);

	vTaskDelay(100);
	uint16_t status;
	if (err = adi_ad9081_jesd_tx_link_status_get(&ad9081_dev, AD9081_LINK_0, &status), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	xil_printf("Link Status 0x%x\r\n", status);

//	// ILA TEST MODE
//	adi_ad9081_jesd_tx_ilas_test_mode_enable_set(&ad9081_dev, AD9081_LINK_0, 1);


#ifdef IBERT_TESTING
	adi_ad9081_jesd_tx_phy_prbs_test(&ad9081_dev, AD9081_LINK_ALL, PRBS31);
#endif

#ifndef IBERT_TESTING

	// Update FPGA TX Transceiver settings
//	set_lane_cal(0, 0, 0, 11);
//	set_lane_cal(1, 10, 5, 11);
//	set_lane_cal(2, 5, 0, 11);
//	set_lane_cal(3, 0, 0, 11);
//	set_lane_cal(4, 0, 0, 11);
//	set_lane_cal(5, 0, 0, 11);
//	set_lane_cal(6, 12, 0, 11);
//	set_lane_cal(7, 0, 0, 11);

	set_lane_cal(0, 9, 0, 7);
	set_lane_cal(1, 9, 0, 7);
	set_lane_cal(2, 9, 0, 7);
	set_lane_cal(3, 9, 0, 7);
	set_lane_cal(4, 9, 0, 7);
	set_lane_cal(5, 9, 0, 7);
	set_lane_cal(6, 9, 0, 7);
	set_lane_cal(7, 9, 0, 7);

    vTaskDelay(100);
    int subclass = jtx_param[uc][0].jesd_subclass;
    int jesd_version = jtx_param[uc][0].jesd_jesdv;

    xil_printf("GPI:                 0x%x\r\n", Xil_In32(GPI_REG));

	xil_printf("Reset FPGA JESD Cores\r\n");
	reset_jesd();

	xil_printf("RX IP Config 0x%x\r\n", Xil_In32(JESD_RX + 4));
	xil_printf("TX IP Config 0x%x\r\n", Xil_In32(JESD_TX + 4));


	xil_printf("Changing Subclass\r\n");
	Xil_Out32(JESD_TX + 0x34, subclass);
	Xil_Out32(JESD_RX + 0x34, subclass);

	xil_printf("Changing Ctrl Sysref\r\n");
	Xil_Out32(JESD_TX + 0x50, 0);
	Xil_Out32(JESD_RX + 0x50, 0);
	if (subclass == 1) {
		Xil_Out32(JESD_TX + 0x50, 2);
		Xil_Out32(JESD_RX + 0x50, 2);
	}

	if (jesd_version == JESD_204C) {
		// 204C
		xil_printf("Changing MB in EMB\r\n");
		Xil_Out32(JESD_TX + CTRL_MB_IN_EMB, 1);
		Xil_Out32(JESD_RX + CTRL_MB_IN_EMB, 1);

		xil_printf("Changing Meta\r\n");
		Xil_Out32(JESD_TX + 0x34, 0);
		Xil_Out32(JESD_RX + 0x34, 0);
//		Xil_Out32(JESD_TX + 0x34, 3); // FEC
//		Xil_Out32(JESD_RX + 0x34, 3); // FEC
	}

	if (jesd_version == JESD_204B) {
		//204B
		xil_printf("204B CTRL_8B10B_CFG\r\n");
		Xil_Out32(JESD_TX + 0x3C, 0x03031F01);
		Xil_Out32(JESD_RX + 0x3C, 0x03031F01);
	}

	Xil_Out32(JESD_RX + CTRL_RX_BUF_ADV, 0);
	uint32_t val = Xil_In32(JESD_RX + CTRL_RX_BUF_ADV);
	if (val != 0) {
		xil_printf("ERROR. Buffer Advance (RX) not updated.\r\n");
		val = Xil_In32(JESD_RX + STAT_STATUS_REG);
		xil_printf("* STATUS_REG = 0x%x\r\n", val);
	}

	xil_printf("Reset both modules to update configuration\r\n");
	reset_jesd();

	if (jesd_version == JESD_204C) {
		xil_printf("Wait for Block Sync\r\n");
		val = 0;
		while ((val & STATUS_SH_LOCK_BIT) != STATUS_SH_LOCK_BIT) {
			val = Xil_In32(JESD_RX + STAT_STATUS_REG);
			xil_printf("STAT_STATUS_REG 0x%x\r\n", val);
			xil_printf("RX STAT_LOCK_DEBUG:  0x%x\r\n", Xil_In32(JESD_RX + 0x54));
			vTaskDelay(200);
		}
		xil_printf("* Block sync achieved\r\n");
		xil_printf("STAT_STATUS_REG 0x%x\r\n", val);
		xil_printf("RX STAT_LOCK_DEBUG:  0x%x\r\n", Xil_In32(JESD_RX + 0x54));

		xil_printf("Wait for Extended Multiblock lock\r\n");
		val = 0;
		while ((val & STATUS_EMB_LOCK_BIT) != STATUS_EMB_LOCK_BIT) {
			val = Xil_In32(JESD_RX + STAT_STATUS_REG);
			xil_printf("STAT_STATUS_REG 0x%x\r\n", val);
			xil_printf("RX STAT_LOCK_DEBUG:  0x%x\r\n", Xil_In32(JESD_RX + 0x54));
			xil_printf("RX EMB:  0x%x\r\n", Xil_In32(JESD_RX + CTRL_MB_IN_EMB));
			uint8_t data;
	//		adi_ad9081_hal_bf_set(&ad9081_dev, 0x00000667, BF_JTX_CRC_FEC_REVERSE_CFG_INFO, 1);
			adi_ad9081_hal_reg_get(&ad9081_dev, 0x00000667, &data);
			xil_printf("0x00000667:  0x%x\r\n", data);


	//		for (int i = 0; i < 8; i++) {
	//			// Clear Error Counts
	//			uint32_t err_cnt = Xil_In32(JESD_RX + 0x400 + i * 0x080 + 0x10);
	//			xil_printf("    Lane %d  STAT_RX_ERROR_CNT0 = 0x%x\r\n", err_cnt);
	//		}
			vTaskDelay(100);
		}
		xil_printf("* Extended Multiblock lock achieved\r\n");
	}
	if (jesd_version == JESD_204B) {
//		xil_printf("Enable TX data and command stream\r\n");
//		Xil_Out32(JESD_TX + CTRL_ENABLE_REG, ENABLE_DATA_CMD);
//
//		xil_printf("Enable RX data and command stream\r\n");
//		Xil_Out32(JESD_RX + CTRL_ENABLE_REG, ENABLE_DATA_CMD);

		val = 0;
		while ((val & 0x7000) != 0x7000) {
			val = Xil_In32(JESD_RX + STAT_STATUS_REG);
			xil_printf("STAT_STATUS_REG 0x%x\r\n", val);
			xil_printf("RX 204B STAT_RX_ERR:  0x%x\r\n", Xil_In32(JESD_RX + 0x58));
			xil_printf("RX 204B STAT_RX_DEBUG:  0x%x\r\n", Xil_In32(JESD_RX + 0x5C));
			uint16_t status;
			if (err = adi_ad9081_jesd_tx_link_status_get(&ad9081_dev, AD9081_LINK_0, &status), err != API_CMS_ERROR_OK)
				error_print(__LINE__, err);
			xil_printf("Link Status 0x%x\r\n", status);
			xil_printf("GPI:  0x%x\r\n", Xil_In32(GPI_REG));

			print_rx_ila();

			vTaskDelay(400);
		}
	}

	xil_printf("Enable TX data and command stream\r\n");
	Xil_Out32(JESD_TX + CTRL_ENABLE_REG, ENABLE_DATA_CMD);

	xil_printf("Enable RX data and command stream\r\n");
	Xil_Out32(JESD_RX + CTRL_ENABLE_REG, ENABLE_DATA_CMD);


	vTaskDelay(100);

    xil_printf("GPI:                 0x%x\r\n", Xil_In32(GPI_REG));

	xil_printf("RX Version:          0x%x\r\n", Xil_In32(JESD_RX + 0x000));
	xil_printf("RX Config:           0x%x\r\n", Xil_In32(JESD_RX + 0x004));
	xil_printf("RX Stat Lock Debug:  0x%x\r\n", Xil_In32(JESD_RX + 0x054));
	xil_printf("RX Stat Status:      0x%x\r\n", Xil_In32(JESD_RX + 0x060));
	// TX Core
	xil_printf("TX Version:          0x%x\r\n", Xil_In32(JESD_TX + 0x000));
	xil_printf("TX Config:           0x%x\r\n", Xil_In32(JESD_TX + 0x004));
	xil_printf("TX Stat Status:      0x%x\r\n", Xil_In32(JESD_TX + 0x060));


	// RX Core
	xil_printf("RX Version:          0x%x\r\n", Xil_In32(XPAR_JESD_JESD204C_0_BASEADDR + 0x000));
	xil_printf("RX Config:           0x%x\r\n", Xil_In32(XPAR_JESD_JESD204C_0_BASEADDR + 0x004));
	xil_printf("RX Stat Lock Debug:  0x%x\r\n", Xil_In32(XPAR_JESD_JESD204C_0_BASEADDR + 0x054));
	xil_printf("RX Stat Status:      0x%x\r\n", Xil_In32(XPAR_JESD_JESD204C_0_BASEADDR + 0x060));
	xil_printf("RX MB IN EBD:        0x%x\r\n", Xil_In32(JESD_RX + CTRL_MB_IN_EMB));
	xil_printf("RX SUBCLASS MODE:    0x%x\r\n", Xil_In32(JESD_RX + 0x34));
	xil_printf("RX CTRL_META_MODE:   0x%x\r\n", Xil_In32(JESD_RX + CTRL_META_MODE));
	xil_printf("RX CTRL_ENABLE_REG:  0x%x\r\n", Xil_In32(JESD_RX + CTRL_ENABLE_REG));
	xil_printf("RX CTRL_LANE_ENA:    0x%x\r\n", Xil_In32(JESD_RX + 0x40));
	xil_printf("RX CTRL_SYSREF:      0x%x\r\n", Xil_In32(JESD_RX + 0x50));
	xil_printf("RX STAT_LOCK_DEBUG:  0x%x\r\n", Xil_In32(JESD_RX + 0x54));
	xil_printf("RX STAT_RX_ERR:      0x%x\r\n", Xil_In32(JESD_RX + 0x58));
	xil_printf("RX STAT_RX_DEBUG:    0x%x\r\n", Xil_In32(JESD_RX + 0x5C));

#endif

	// Toggle JESD RX Enable
	if (err = adi_ad9081_jesd_rx_link_enable_set(&ad9081_dev, AD9081_LINK_ALL, 0), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);
	if (err = adi_ad9081_jesd_rx_link_enable_set(&ad9081_dev, (jrx_param[uc].jesd_duallink > 0) ? AD9081_LINK_ALL : AD9081_LINK_0, 1), err != API_CMS_ERROR_OK)
		error_print(__LINE__, err);

	/* calibrate jrx when lane rate is high for 204c */
	if ((jrx_param[uc].jesd_l > 0) && (jrx_param[uc].jesd_jesdv == 2) && ((clk_hz[uc][1] * 66) > AD9081_JESDRX_204C_CAL_THRESH)) {
//	if (1) {
		xil_printf("APP: Run JESD RX 204C Calibration & Enable TX Path Links\r\n");
//		if (err = adi_ad9081_jesd_rx_calibrate_204c(&ad9081_dev, 1, 0x00, (ad9081_dev.serdes_info.des_settings.cal_mode == AD9081_CAL_MODE_RUN_AND_SAVE) ? 0 : 1), err != API_CMS_ERROR_OK) {
//			xil_printf("APP: ad9081 JESD RX Calibration Error\r\n");
//			error_print(__LINE__, err);
//		}
//
		if (err = adi_ad9081_jesd_rx_calibrate_204c(&ad9081_dev, 1, 0x00, 1), err != API_CMS_ERROR_OK) {
			xil_printf("APP: ad9081 JESD RX Calibration Error\r\n");
			error_print(__LINE__, err);
		}
//		if (err = adi_ad9081_jesd_rx_calibrate_204c(&ad9081_dev, 1, 0xFF, 1), err != API_CMS_ERROR_OK) {
//			xil_printf("APP: ad9081 JESD RX Calibration Error\r\n");
//			error_print(__LINE__, err);
//		}

//		if (err = adi_ad9081_jesd_rx_link_enable_set(&ad9081_dev, AD9081_LINK_ALL, 0), err != API_CMS_ERROR_OK)
//			error_print(__LINE__, err);
//		if (err = adi_ad9081_jesd_rx_link_enable_set(&ad9081_dev, (jrx_param[uc].jesd_duallink > 0) ? AD9081_LINK_ALL : AD9081_LINK_0, 1), err != API_CMS_ERROR_OK)
//			error_print(__LINE__, err);
//		if (err = adi_ad9081_jesd_rx_link_enable_set(&ad9081_dev, AD9081_LINK_ALL, 0), err != API_CMS_ERROR_OK)
//			error_print(__LINE__, err);
//		if (err = adi_ad9081_jesd_rx_link_enable_set(&ad9081_dev, (jrx_param[uc].jesd_duallink > 0) ? AD9081_LINK_ALL : AD9081_LINK_0, 1), err != API_CMS_ERROR_OK)
//			error_print(__LINE__, err);
	}

#ifndef IBERT_TESTING
	for (int i = 0; i < 8; i++) {
		// Clear Error Counts
		Xil_In32(JESD_RX + 0x400 + i * 0x080 + 0x10);
	}
	vTaskDelay(100);
	for (int i = 0; i < 8; i++) {
		xil_printf("Lane %d Buf Level: 0x%x\r\n", i, Xil_In32(JESD_RX + 0x400 + i * 0x080 + 0x0));
		xil_printf("Lane %d Stat 0: 0x%x\r\n", i, Xil_In32(JESD_RX + 0x400 + i * 0x080 + 0x10));
		xil_printf("Lane %d Stat 1: 0x%x\r\n", i, Xil_In32(JESD_RX + 0x400 + i * 0x080 + 0x14));
	}
#endif


#ifdef IBERT_TESTING
	int cal_count = 0;
	while (1) {
		adi_ad9081_jesd_rx_phy_prbs_test(&ad9081_dev, PRBS31, 100);
		xil_printf("Check PRBS Errors\r\n");
		for (int i = 0; i < 8; i++) {
			adi_ad9081_prbs_test_t prbs_res;
			adi_ad9081_jesd_rx_phy_prbs_test_result_get(&ad9081_dev, i, &prbs_res);
			xil_printf("    Lane %d, Errors %d\r\n", i, prbs_res.phy_prbs_err_cnt);
		}

		cal_count++;

//		if (cal_count > 9) {
//			cal_count = 0;
//			if (err = adi_ad9081_jesd_rx_calibrate_204c(&ad9081_dev, 1, 0x00, 1), err != API_CMS_ERROR_OK) {
//				xil_printf("APP: ad9081 JESD RX Calibration Error\r\n");
//				error_print(__LINE__, err);
//			}
//		}

	}
#endif



}

