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

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

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

XSysMon SysMonInst;

/*============= 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 uint8_t  jtx_logiclane_mapping_pe_brd[2][8];
extern uint8_t  jtx_logiclane_mapping_ce_brd[2][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_fan_pwm(int percent) {
    // Set Fan PWM
	// Don't try to turn on fan unless percent is above a certain value, fan has minimum turn on percent
	int period = 1500000;
	int pw = period * percent / 100;
    Xil_Out32(0x40050010, period);
    Xil_Out32(0x40050014, pw);
}

void update_fan_speed(int fpga_temp) {
	int min_temp = 30;
	int max_temp = 60;
	int delta_temp = max_temp - min_temp;
	int min_percent = 50;
	int percent = 0;

	int temp = fpga_temp - min_temp;
	if (temp < 0) {
		temp = 0;
		set_fan_pwm(0);
	}
	else {
		percent = temp * 100 / delta_temp;
		if (percent < min_percent) {
			percent = min_percent;
		}
		set_fan_pwm(percent);
	}

//	xil_printf("Temp - %d, Fan Percent - %d\r\n", fpga_temp, percent);
}

int setBit(uint32_t addr, uint32_t bit) {
	int val = Xil_In32(addr);
	val |= (1 << bit);
	Xil_Out32(addr, val);
}

int clearBit(uint32_t addr, uint32_t bit) {
	int val = Xil_In32(addr);
	val &= ~(1 << bit);
	Xil_Out32(addr, val);
}

int toggleBit(uint32_t addr, uint32_t bit) {
	int val = Xil_In32(addr);
	uint32_t current_bit = val & (1 << bit);
	if (current_bit) {
		val &= ~(1 << bit);
	} else {
		val |= (1 << bit);
	}
	Xil_Out32(addr, val);
}

void status_task( void *pvParameters ) {
	XSysMon_Config *ConfigPtr;
	XSysMon *SysMonInstPtr = &SysMonInst;

	// Initialize the SysMon driver.
	ConfigPtr = XSysMon_LookupConfig(XPAR_SYSMON_0_DEVICE_ID);
	XSysMon_CfgInitialize(SysMonInstPtr, ConfigPtr,
				ConfigPtr->BaseAddress);
	XSysMon_SelfTest(SysMonInstPtr);
	
	while (1) {
		u16 temp_data = XSysMon_GetAdcData(SysMonInstPtr, XSM_CH_TEMP);
		int fpga_temp = XSysMon_RawToTemperature(temp_data);
		update_fan_speed(fpga_temp);

		vTaskDelay(100);
	}
}

void error_print(int line_number, int err) {
	xil_printf("Line %d, Error! %d\r\n", line_number, err);
	vTaskDelay(1000);
}

void main_task( void *pvParameters ) {

	ad9081_hal_init();
	hmc7044_init();

	// select use case
	int uc = 13;
//	int uc = 10;


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

    /* 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},
                },
                .invert_mask = 0x00,
//                .lane_mapping = { { 6, 4, 3, 2, 1, 0, 7, 5 }, { 2, 0, 7, 7, 7, 7, 3, 1 } }, /* link0, link1 */
				.lane_mapping = { { 0, 1, 2, 3, 4, 5, 6, 7 }, { 0, 1, 2, 3, 4, 5, 6, 7 } }, /* link0, link1 */
            },
            .des_settings = { /* ad9081 jrx */
                .boost_mask = 0xff,
                .invert_mask = 0x00,
                .ctle_filter = { 2, 2, 2, 2, 2, 2, 2, 2 },
                .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 = { { 0, 1, 2, 3, 4, 5, 6, 7 }, { 4, 5, 6, 7, 0, 1, 2, 3 } }, /* link0, link1 */
            }
        },
        .clk_info = {
            .sysref_mode = SYSREF_NONE,
        }
    };

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

    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_204b = fpga_clk/16;
	uint64_t sysref_clk_204c = fpga_clk/32;
//	uint64_t hmc7044_crystal_input = 122.88e6;
	uint64_t hmc7044_crystal_input = 100e6;

	/* Configure HMC7044 SYSREF frequency output channels for SC1 Use Case */
	switch (uc){
	case 21:
	case 26:
	case 27:
	case 40:
		ad9081_dev.clk_info.sysref_mode = SYSREF_CONT;
		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_jesdv == 2) || (jtx_param[uc][0].jesd_jesdv == 2)) {
			if (err = adi_ad9081_sync_sysref_frequency_set(&ad9081_dev, &sysref_clk_204c, 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_204c);
		}
		else {
			if (err = adi_ad9081_sync_sysref_frequency_set(&ad9081_dev, &sysref_clk_204b, 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_204b);
		}
		break;
	default:
		ad9081_dev.clk_info.sysref_clk = &hmc7044_dev;
		ad9081_dev.clk_info.sysref_ctrl = app_sysref_clk_src_sel;
		break;
	}

	/*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_204c[14] = { fpga_clk,   0, ad9081_clk, sysref_clk_204c, 0, 0, fpga_clk,   0, fpga_clk, 0, fpga_clk,   0, fpga_clk,  sysref_clk_204c };
	uint64_t hmc_out_204c[14] = { fpga_clk,   0, ad9081_clk, sysref_clk_204c, 0, 0, 125e6,   0, fpga_clk, 0, 125e6,   0, fpga_clk,  sysref_clk_204c };

	/* 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_204c[3] = 0;
		hmc_out_204c[13] = 0;
	}
//	if (((jrx_param[uc].jesd_jesdv == 2) || (jtx_param[uc][0].jesd_jesdv == 2)) && (fpga_clk * 66 < 8.3e9)) { /* double fpga refclk for 204C lane rate < 8.3Gbps */
//		hmc_out_204c[8] *= 2;
//		hmc_out_204c[12] *= 2;
//	}
	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_204c), 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");

    //--------------------------------------------------------------------------------------------------------------------
    /* 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);
    }

#if !defined(AD9207_ID) && !defined(AD9209_ID)
    /* nco test case */
    if (uc == 0) {
        if (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))), err != API_CMS_ERROR_OK)
        	error_print(__LINE__, err);
    }
#endif

#if !defined(AD9207_ID) && !defined(AD9209_ID)
	switch (uc) {
        case 0: /* NCO Only mode */
            break;
        case 18:
        case 22:
        case 29:
        case 31:
        case 33:
        case 34:
        case 39:
            /* Power Down DAC blocks */
            if (err = adi_ad9081_dac_power_up_set(&ad9081_dev, AD9081_DAC_ALL, 0), err != API_CMS_ERROR_OK)
            	error_print(__LINE__, err);
            if (err = adi_ad9081_dac_dll_power_down(&ad9081_dev, AD9081_DAC_ALL), err != API_CMS_ERROR_OK)
            	error_print(__LINE__, err);
            if (err = adi_ad9081_jesd_rx_power_down_des(&ad9081_dev), err != API_CMS_ERROR_OK)
            	error_print(__LINE__, err);
            break;  /* Rx Only Cases */
        default:
            /* 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);
        }


        /* Application/Usecase Specific Customization if Required*/
        switch (uc) {
        case 19:
        case 20:
            /*Select Main CDUC0/1->DAC0, CDUC2/3->DAC2 */
            if (err = adi_ad9081_dac_modulation_mux_mode_set(&ad9081_dev, AD9081_DAC_PAIR_ALL, AD9081_DAC_MUX_MODE_3), err != API_CMS_ERROR_OK)
            	error_print(__LINE__, err);
            break;
        case 32:
            /*Route second IQ pair from virtual converters to DAC2
            * Select Modulation Mux Mode to DAC0 => I0, DAC1 =>Q0
            */
            if (err = adi_ad9081_dac_xbar_set(&ad9081_dev, AD9081_DAC_2, AD9081_DAC_1), err != API_CMS_ERROR_OK)
            	error_print(__LINE__, err);
            if (err = adi_ad9081_dac_modulation_mux_mode_set(&ad9081_dev, AD9081_DAC_PAIR_ALL, AD9081_DAC_MUX_MODE_2), err != API_CMS_ERROR_OK)
            	error_print(__LINE__, err);
            break;
        default:
            /*Use Default Muxing as per SDUG*/
            break;
	}

#endif


	/*Configure Primary Rx Datapath Settings*/
	switch (uc) {
        case 0:
        case 17:
        case 28:
            /*DO NOT Configure as these are TX Only Modes*/
            break;
        default:
            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);
            }
	}

    /*Enable Internal Loopback Mode*/
    switch (uc) {
//		case 13:
		case 26:
		case 27:
			printf("APP: This is being configured as a loopback usecase \n");
			if (err = adi_ad9081_jesd_loopback_mode_set(&ad9081_dev, 1), err != API_CMS_ERROR_OK)
				error_print(__LINE__, err);
			/* power down unused PHY */
			if (err = adi_ad9081_jesd_tx_power_down_ser(&ad9081_dev), err != API_CMS_ERROR_OK)
				error_print(__LINE__, err);
			if (err = adi_ad9081_jesd_rx_power_down_des(&ad9081_dev), err != API_CMS_ERROR_OK)
				error_print(__LINE__, err);
		default:
			break;

    }

    /* Set gain settings as per Application Usecase */
    switch (uc) {
		case 26:
		case 27:
			if (err = adi_ad9081_adc_ddc_fine_gain_set(&ad9081_dev, rx_fddc_select[uc], 1), err != API_CMS_ERROR_OK)
				error_print(__LINE__, err);
			break;
		default:
			break;
    }
    /*Override NCO Mode*/
    switch (uc) {
		case 15:
			if (err = adi_ad9081_adc_ddc_coarse_nco_mode_set(&ad9081_dev, rx_cddc_select[uc], AD9081_ADC_NCO_ZIF), err != API_CMS_ERROR_OK)
				error_print(__LINE__, err);
			break;
		case 26:
		case 27:
			/*Enable NCO TEST Mode*/
			if (err = adi_ad9081_adc_ddc_coarse_nco_mode_set(&ad9081_dev, rx_cddc_select[uc], AD9081_ADC_NCO_TEST), err != API_CMS_ERROR_OK)
				error_print(__LINE__, err);
			break;
		default:
			break;
    }

    /* Override/Customize Default Xbar Settings*/
    switch (uc) {
		case 19:
		case 20:
			adc_cddc_xbar = AD9081_ADC_2_ADC_REAL_MODE;   /* ADC0->CDDC0/2, ADC1-> CDDC1/3 */
			cddc_fddc_xbar = AD9081_ADC_CDDC0_TO_FDDC0 | AD9081_ADC_CDDC0_TO_FDDC1 | AD9081_ADC_CDDC1_TO_FDDC2 | AD9081_ADC_CDDC1_TO_FDDC3;
			cddc_fddc_xbar |= AD9081_ADC_CDDC2_TO_FDDC4 | AD9081_ADC_CDDC2_TO_FDDC5 | AD9081_ADC_CDDC3_TO_FDDC6 | AD9081_ADC_CDDC3_TO_FDDC7;
			if (err = adi_ad9081_adc_xbar_set(&ad9081_dev, adc_cddc_xbar, cddc_fddc_xbar), err != API_CMS_ERROR_OK)
				error_print(__LINE__, err);
			break;
		default:
			break;
    }

    /* Set ADC Nyquist Zone as per Application Usecase for ADC optimal Background Cal Operation*/
    switch (uc) {
		case 26:
		case 27:/*ADC Test Test Mode*/
			if (err = adi_ad9081_adc_nyquist_zone_set(&ad9081_dev, AD9081_ADC_ALL, AD9081_ADC_NYQUIST_ZONE_EVEN), err != API_CMS_ERROR_OK)
				error_print(__LINE__, err);
			break;
		default:
			break;
    }

#if !defined(AD9177_ID)
	/* Configure PFILT */
	int16_t pfir_coeffs[192] = {-32,-32,-7,31,31,31,31,31,-32,-32,-32,31,31,31,-12,31,31,-32,-32,-32,31,31,31,31,
		1770,1297,-2047,-2048,-1503,1814,1522,-116,1647,2047,1789,-2048,-7400,-1458,3267,676,-639,7720,14648,2089,-23120,-29208,-1139,32767,
		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,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
		32767,-1139,-29208,-23120,2089,14648,7720,-639,676,3267,-1458,-7400,-2048,1789,2047,1647,-116,1522,1814,-1503,
		-2048,-2047,1297,1770,31,31,31,31,-32,-32,-32,31,31,-12,31,31,31,-32,-32,-32,31,31,31,31,31,-7,-32,-32,
		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,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
	adi_ad9081_adc_pfir_gain_t pfir_gain = {
		.ix_gain = AD9081_ADC_PFIR_GAIN_0DB,
		.iy_gain = AD9081_ADC_PFIR_GAIN_0DB,
		.qx_gain = AD9081_ADC_PFIR_GAIN_0DB,
		.qy_gain = AD9081_ADC_PFIR_GAIN_0DB};
	switch(uc) {
        case 41:
            /* 2949.12 sampling rate, BPF for 24-44% of NZ1*/
            if (err = adi_ad9081_adc_pfir_coeff_table_load_set(&ad9081_dev, AD9081_ADC_PFIR_ADC_PAIR_ALL, AD9081_ADC_PFIR_COEFF_PAGE0, AD9081_ADC_PFIR_I_MODE_REAL_N2, AD9081_ADC_PFIR_Q_MODE_REAL_N2,
                &pfir_gain, pfir_coeffs, 1), err != API_CMS_ERROR_OK)
            	error_print(__LINE__, err);
            break;
        default:
            break;
	}
#endif

	/* Configure Synchronization Options as per Application Use-case*/
	/* By Default Application uses Subclass 0 and Internal Sysref Synchronization*/
	/* Note 21/26/27 Are examples of Subclass 1*/
	switch (uc) {
        case 0: /*NCO Test Mode- No JESD*/
            if (err = adi_ad9081_jesd_oneshot_sync(&ad9081_dev,  0), err != API_CMS_ERROR_OK){
                if (err == API_CMS_ERROR_JESD_SYNC_NOT_DONE) {
                    printf("APP: JESD Oneshot Synchronization Not Completed");
                }
                error_print(__LINE__, err);
            }
            break;
        case 21: /* Subclass 1 Use case - default set to continuous pulse */
        case 40:
            printf("APP: JESD RX Synchronization Mode: %s\n", ((jrx_param[uc].jesd_subclass == JESD_SUBCLASS_1) ? "JESD_SUBCLASS_1" : "JESD_SUBCLASS_0"));
            /* 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);

            break;
        case 26: /*ADC Test Test Mode*/
            printf("APP: JESD RX Synchronization Mode: %s\n", ((jrx_param[uc].jesd_subclass == JESD_SUBCLASS_1) ? "JESD_SUBCLASS_1" : "JESD_SUBCLASS_0"));
            /* 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);
            /* 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_adc_nco_master_slave_sync(&ad9081_dev, 1, 2, 0, 1), err != API_CMS_ERROR_OK) /*mcs: main*/
            	error_print(__LINE__, err);
            break;
        case 27: /*ADC Test Test Mode*/
            printf("APP: JESD RX Synchronization Mode: %s\n", ((jrx_param[uc].jesd_subclass == JESD_SUBCLASS_1) ? "JESD_SUBCLASS_1" : "JESD_SUBCLASS_0"));
            /* 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);
            /* 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_adc_nco_master_slave_sync(&ad9081_dev, 0, 2, 0, 1), err != API_CMS_ERROR_OK) /*mcs: subordinate*/
            	error_print(__LINE__, err);
            break;
        default:
            printf("APP: JESD RX Synchronization Mode: %s\n", ((jrx_param[uc].jesd_subclass == JESD_SUBCLASS_1) ? "JESD_SUBCLASS_1" : "JESD_SUBCLASS_0"));
            /* 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) {
                    printf("APP: JESD Oneshot Synchronization Not Completed");
                }
                error_print(__LINE__, err);
            }
            break;
	}

    /* 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
    */
    if (uc > 0) {
        /* 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);
        }

#if !defined(AD9177_ID)
        /* enable ad9081 Rx Path: JESD Tx Link links */
        switch (uc) {
			case 17:
			case 28:
				/*TX ONLY MODES*/
				break;
			default:
				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);
				break;
        }
#endif
#if !defined(AD9207_ID) && !defined(AD9209_ID)
        /* enable ad9081 Tx Path: JESD Rx Link links */
        switch (uc) {
			case 18:
			case 22:
			case 29:
			case 31:
			case 33:
			case 34:
			case 39:
				/*RX ONLY MODES*/
				break;
			default:
				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);
				break;
			}
#endif

        /*Enable FPGA JESD RxTx/ Initiate Playback */
//        if (err = adi_ads9_reg_set(0x106, 0x000), err != API_CMS_ERROR_OK) /* skip_rx_link_init = 0 */
//            error_print(__LINE__, err);
//        if (err = adi_ads9_reg_set(0x947, 2), err != API_CMS_ERROR_OK) /* bidir_start = 1 */
//            error_print(__LINE__, err);
//        if (err = ads9_wait_us(NULL, 100000), err != API_CMS_ERROR_OK)
//            error_print(__LINE__, err);
        if (ad9081_dev.clk_info.sysref_mode == SYSREF_ONESHOT) {
            if (err = adi_hmc7044_pulse_gen_set(&hmc7044_dev), err != API_CMS_ERROR_OK)
                error_print(__LINE__, err);
        }
//        if (err = adi_ads9_reg_set(0x106, 0x400), err != API_CMS_ERROR_OK) /* skip_rx_link_init = 1 */
//            error_print(__LINE__, err);
//        if (err = adi_ads9_reg_set(0x537, 4), err != API_CMS_ERROR_OK) /* gt_tx_ptn_play_stop = 1 */
//            error_print(__LINE__, err);

#if !defined(AD9207_ID) && !defined(AD9209_ID)
        if (err = adi_ad9081_jesd_rx_link_enable_set(&ad9081_dev, AD9081_LINK_ALL, 0), err != API_CMS_ERROR_OK)
            error_print(__LINE__, err);
#endif
//        if (err = adi_ads9_reg_set(0x537, 1), err != API_CMS_ERROR_OK) /* gt_tx_ptn_play_start = 1 */
//            error_print(__LINE__, err);
#if !defined(AD9207_ID) && !defined(AD9209_ID)
        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);
#endif
        /* calibrate jrx when lane rate is high for 204c */
        printf("APP: Run JESD RX 204C Calibration & Enable TX Path Links\n");
#if !defined(AD9207_ID) && !defined(AD9209_ID)
        char ctle_file[] = "ctle_coeffs";
        if ((jrx_param[uc].jesd_l > 0) && (jrx_param[uc].jesd_jesdv == 2) && ((clk_hz[uc][1] * 66) > AD9081_JESDRX_204C_CAL_THRESH)) {
//            if (ad9081_dev.serdes_info.des_settings.cal_mode == AD9081_CAL_MODE_BYPASS) {
//                app_cal_ctle_manual_config_load(&ad9081_dev, ctle_file);
//            }

            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) {
                printf("APP: ad9081 JESD RX Calibration Error\n");
                error_print(__LINE__, err);
            }

//            if (ad9081_dev.serdes_info.des_settings.cal_mode == AD9081_CAL_MODE_RUN_AND_SAVE) {
//                app_cal_ctle_manual_config_save(&ad9081_dev, ctle_file);
//            }

            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);
        }
        /* run 2D eye scan */
        switch (uc)
        {
			case 16:
				/* half rate mode */
//				if (err = app_test_eye_scan(&ad9081_dev, &jrx_param[uc], uc, AD9081_HALF_RATE, PRBS7, 1), err != API_CMS_ERROR_OK)
//					error_print(__LINE__, err);
				break;
			case 11:
				/* quarter rate mode */
//				if (err = app_test_eye_scan(&ad9081_dev, &jrx_param[uc], uc, AD9081_QUART_RATE, PRBS7, 1), err != API_CMS_ERROR_OK)
//					error_print(__LINE__, err);
				break;
			default:
				break;
        }
#endif
#if !defined(AD9177_ID)
    /* TDD Power Savings*/
        switch (uc)
        {
			case 25:
				if (err = adi_ad9081_adc_adc0_rxen_pwdn_ctrl_set(&ad9081_dev, 0, 1, 0, 1), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_adc1_rxen_pwdn_ctrl_set(&ad9081_dev, 1, 0, 1, 0), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_adc2_rxen_pwdn_ctrl_set(&ad9081_dev, 1, 0, 1, 0), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_adc3_rxen_pwdn_ctrl_set(&ad9081_dev, 0, 1, 0, 1), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_rxen0_sel_set(&ad9081_dev, 7, 255, 0, 0, 255), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_rxen0_ctrl_set(&ad9081_dev, 0, 0, 0, 0), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_rxen1_sel_set(&ad9081_dev, 0, 0, 0, 0, 0), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_rxen1_ctrl_set(&ad9081_dev, 0, 0, 0, 0), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_rxengp0_sel_set(&ad9081_dev, 0, 0, 0, 0, 0), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_rxengp0_ctrl_set(&ad9081_dev, 0, 0, 0), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_rxengp1_sel_set(&ad9081_dev, 0, 0, 0, 0, 0), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				if (err = adi_ad9081_adc_rxengp1_ctrl_set(&ad9081_dev, 0, 0, 0), err != API_CMS_ERROR_OK)
					error_print(__LINE__, err);
				break;
		}
#endif
    }

    vTaskDelay(100);
    int subclass = 1;

    xil_printf("GPI:                 0x%x\r\n", Xil_In32(0x40050000 + 0x00C));

	xil_printf("Reset FPGA JESD Cores\r\n");
	Xil_Out32(0x40050000 + 0x008, 0x31 | 0x300 | 0xC0);
	vTaskDelay(100);
	Xil_Out32(0x40050000 + 0x008, 0x31);

	// Wait for reset to complete
	xil_printf("Wait for TX 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(0x40050000 + 0x00C));
		vTaskDelay(10);
	}

	xil_printf("Wait for RX 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);
	}

	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 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, 2);
	Xil_Out32(JESD_RX + 0x50, 2);


	Xil_Out32(JESD_RX + CTRL_RX_BUF_ADV, 0);
	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");
	Xil_Out32(0x40050000 + 0x008, 0x31 | 0x300 | 0xC0);
	vTaskDelay(100);
	Xil_Out32(0x40050000 + 0x008, 0x31);

	// Wait for reset to complete
	xil_printf("Wait for TX to complete reset\r\n");
	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(0x40050000 + 0x00C));
		vTaskDelay(10);
	}

	xil_printf("Wait for RX 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);
	}

	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);
		vTaskDelay(100);
	}
	xil_printf("* Block sync achieved\r\n");

	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);
		vTaskDelay(100);
	}
	xil_printf("* Extended Multiblock lock achieved\r\n");


//	    $display("Enable TX data and command stream");
//	    axi_write(TX_ADDR + CTRL_ENABLE_REG , ENABLE_DATA_CMD);
//
//	    //wait for ten MBs to avoid synchronization problems
//	    repeat (320)
//	    begin
//	      @(posedge core_clk_p);
//	    end
//
	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(0x40050000 + 0x00C));

	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));


//	typedef struct {
//	    uint8_t jesd_l;                             /*!< No of lanes */
//	    uint8_t jesd_f;                             /*!< No of octets in a frame */
//	    uint8_t jesd_m;                             /*!< No of converters */
//	    uint8_t jesd_s;                             /*!< No of samples */
//	    uint8_t jesd_hd;                            /*!< High Density */
//	    uint16_t jesd_k;                            /*!< No of frames for a multi-frame */
//	    uint8_t jesd_n;                             /*!< Converter resolution */
//	    uint8_t jesd_np;                            /*!< Bit packing sample */
//	    uint8_t jesd_cf;                            /*!< Parameter CF */
//	    uint8_t jesd_cs;                            /*!< Parameter CS */
//	    uint8_t jesd_did;                           /*!< Device ID DID */
//	    uint8_t jesd_bid;                           /*!< Bank ID.  BID */
//	    uint8_t jesd_lid0;                          /*!< Lane ID for lane0 */
//	    uint8_t jesd_subclass;                      /*!< Subclass */
//	    uint8_t jesd_scr;                           /*!< Scramble enable */
//	    uint8_t jesd_duallink;                      /*!< Link mode (single/dual) */
//	    uint8_t jesd_jesdv;                         /*!< Version (0:204A, 1:204B, 2:204C) */
//	    uint8_t jesd_mode_id;                       /*!< JESD mode ID */
//	    uint8_t jesd_mode_c2r_en;                   /*!< JESD mode C2R enable */
//	    uint8_t jesd_mode_s_sel;                    /*!< JESD mode S value */
//	}adi_cms_jesd_param_t;


	for (int i = 0; i < 8; i++) {
		xil_printf("Lane %d Buf Level: 0x%x\r\n", i, Xil_In32(JESD_RX + 0x400 + i * 0x100 + 0x10));
		xil_printf("Lane %d Stat 0: 0x%x\r\n", i, Xil_In32(JESD_RX + 0x400 + i * 0x100 + 0x10));
		xil_printf("Lane %d Stat 1: 0x%x\r\n", i, Xil_In32(JESD_RX + 0x400 + i * 0x100 + 0x14));
//		// CFG 0
//		Xil_Out32(JESD_RX + i * 0x100 + 0x430, 0x200 | subclass);
//		// CFG 1
//		Xil_Out32(JESD_RX + i * 0x100 + 0x434, jtx_param[uc][0].jesd_f - 1);
//		// CFG 2
//		Xil_Out32(JESD_RX + i * 0x100 + 0x438, jtx_param[uc][0].jesd_k - 1);
//		// CFG 3
//		Xil_Out32(JESD_RX + i * 0x100 + 0x43C, ((jtx_param[uc][0].jesd_l -1) << 24) |
//				                               (i << 16) |
//											   (jtx_param[uc][0].jesd_bid << 8) |
//											   jtx_param[uc][0].jesd_did);
//		// CFG 4
//		Xil_Out32(JESD_RX + i * 0x100 + 0x440, (jtx_param[uc][0].jesd_cs << 24) |
//				                               ((jtx_param[uc][0].jesd_np - 1) << 16) |
//											   ((jtx_param[uc][0].jesd_n - 1) << 8) |
//											   (jtx_param[uc][0].jesd_m - 1));
//		// CFG 5
//		Xil_Out32(JESD_RX + i * 0x100 + 0x444, (jtx_param[uc][0].jesd_cf << 24) |
//                                               (jtx_param[uc][0].jesd_hd << 16) |
//				                               ((jtx_param[uc][0].jesd_s - 1) << 8) |
//				                               jtx_param[uc][0].jesd_scr);
//		// CFG 6
//		Xil_Out32(JESD_RX + i * 0x100 + 0x448, 0);
//
//		// CFG 7
//		Xil_Out32(JESD_RX + i * 0x100 + 0x44C, 0);
//		for (int j = 0; j < 8; j++) {
//			xil_printf("ILA:      0x%x\r\n", Xil_In32(JESD_RX + 0x400 + i * 0x100 + 0x30 + j * 4));
//		}
	}


	while (1) {
//		// RX Core
//		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));
//		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));
//
//		uint8_t data8;
//		adi_ad9081_hal_reg_get(&ad9081_dev, 0x668, &data8);
//		xil_printf("AD9081 E:      0x%x\r\n", Xil_In32(JESD_RX + 0x50));
//
//		for (int i = 0; i < 8; i++) {
//			xil_printf("Lane %d Stat 0: 0x%x\r\n", i, Xil_In32(JESD_RX + 0x400 + i * 0x100 + 0x10));
//			xil_printf("Lane %d Stat 1: 0x%x\r\n", i, Xil_In32(JESD_RX + 0x400 + i * 0x100 + 0x14));
//			for (int j = 0; j < 8; j++) {
//				xil_printf("ILA:      0x%x\r\n", Xil_In32(JESD_RX + 0x400 + i * 0x100 + 0x30 + j * 4));
//			}
//		}
		// TX Core
//		xil_printf("TX Version:          0x%x\r\n", Xil_In32(XPAR_JESD_JESD204C_1_BASEADDR + 0x000));
//		xil_printf("TX Config:           0x%x\r\n", Xil_In32(XPAR_JESD_JESD204C_1_BASEADDR + 0x004));
//		xil_printf("TX Stat Status:      0x%x\r\n", Xil_In32(XPAR_JESD_JESD204C_1_BASEADDR + 0x060));

		vTaskDelay(200);
		toggleBit(0x40050008, 0);  // Toggle LED

	}




}




int main(void) {
    xil_printf("\n\r\n\r================= Start ====================\n\r\n\r");

    Xil_Out32(0x40050008, 0x11);

	xTaskCreate( 	status_task,
					( const char * ) "status",
					0x1000,
					NULL,
					TASK_PRIORITY_STATUS,
					NULL );

	xTaskCreate( 	main_task,
					( const char * ) "main",
					0x1000,
					NULL,
					TASK_PRIORITY_MAIN,
					NULL );

//	// This is the thread that hosts lwip.  It must be created using sys_thread_new
//	sys_thread_new("tcp_server_task", (void(*)(void*))tcp_server_task, 0,
//					1024,
//					TASK_PRIORITY_LWIP);

    /* Start the tasks and timer running. */
    vTaskStartScheduler();

    return 0;
}


