

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#define BIT_SLICE(A,F,L) (((A)>>(L)) & ((1<<((F)-(L)+1))-1))

struct jesd204b_regs {
	uint32_t version;
	uint32_t glob_reset;
	uint32_t res0[6];
	uint32_t reset;
	uint32_t config;
	uint32_t sysref;
	uint32_t status;
};

#define JESD204B_LANE_REGS_OFFSET	0x100
struct jesd204b_lane_regs {
	uint32_t lane_status;
	uint32_t res0[3];
	uint32_t ila_cfg_data[4];
	uint32_t mon_phys_err_cnt;
	uint32_t mon_phys_err_res_cnt;
	uint32_t mon_align_f_err_cnt;
	uint32_t mon_replace_f_cnt;
	uint32_t mon_replace_a_cnt;
	uint32_t mon_unknown_k_cnt;
	uint32_t mon_ilas_err_cnt;
};



struct jesd204b_dev {
	volatile struct jesd204b_regs *regs;
	volatile struct jesd204b_lane_regs *lane_regs;
	int octets_per_frame;	/* F */
	int frames_per_multiframe; /* K */
	int number_of_lanes;

};

#define JESD204B_GLBL_RST	(1<<0)
#define JESD204B_RST		(1<<0)
#define CONFIG_F(F)			(((F) & 0xff) <<  0)
#define CONFIG_K(K)			(((K) & 0x1f) <<  8)
#define CONFIG_SUBCLASS(K)	(((K) & 0x03) <<  16)



struct jesd204b_dev *jesd204b_init(void *base, int octets_per_frame, int frames_per_multiframe,
		int number_of_lanes)
{
	int size;
	struct jesd204b_dev *dev = NULL;

	size = sizeof(struct jesd204b_dev);
	dev = (struct jesd204b_dev*) malloc(size);
	if(!dev)
		return dev;
	memset(dev, 0, size);

	dev->regs = (struct jesd204b_regs*)base;
	dev->lane_regs = (volatile struct jesd204b_lane_regs*)((char*)base + JESD204B_LANE_REGS_OFFSET);

	dev->octets_per_frame = octets_per_frame;
	dev->frames_per_multiframe = frames_per_multiframe;
	dev->number_of_lanes = number_of_lanes;

	return dev;
}

void jesd204b_free(struct jesd204b_dev **dev)
{
	if (*dev == NULL)
		return;

	free(*dev);
	*dev = NULL;
}


void jesd204b_print(struct jesd204b_dev *dev)
{
	uint32_t ver = dev->regs->version;

	xil_printf("\n\r------------------------------\n\r");
	xil_printf("JESD204B core\n\r");
	xil_printf("   Magic       : 0x%04x (Expected 0xDCF0)\n\r", BIT_SLICE(ver, 31, 16));
	xil_printf("   Version       : %d.%d.%d\n\r", BIT_SLICE(ver, 11, 8), BIT_SLICE(ver, 7, 4), BIT_SLICE(ver, 3, 0));
	xil_printf("-----------------------------------------------------------\n\r\n\r");
}

/*
 * Global reset
 * It should reset all registers and keep
 * core unoperable.
 */
void jesd204b_glbl_reset(struct jesd204b_dev *dev, int rst)
{
	if (rst) {
		/* Put core to complete reset */
		dev->regs->glob_reset |= JESD204B_GLBL_RST;
	} else {
		/* Take core out off complete reset */
		dev->regs->glob_reset &= ~JESD204B_GLBL_RST;
	}
}

void jesd204b_start(struct jesd204b_dev *dev)
{

	uint32_t val = 0;
	/* Put core to reset */
	dev->regs->reset |= JESD204B_RST;

	/* set main parameters */
	//dev->regs->ila_support = 1; /* capture ILA */
	//dev->regs->scrambling = 0; /*  No scrambling */
	//dev->regs->sysref_handling = 0; /* No sysref required on resync, no additional cycles, sync once only */
	//dev->regs->test_modes = 0; /* normal */
	val = CONFIG_F(dev->octets_per_frame - 1) |
			CONFIG_K(dev->frames_per_multiframe - 1) |
			CONFIG_SUBCLASS(0);
	xil_printf("config val write = %08x\n\r", val);
	dev->regs->config = val;
	xil_printf("config val read = %08x\n\r", dev->regs->config);
	//dev->regs->lanes_in_use = 0xf;
	//dev->regs->subclass_mode = 2;

	/* Reset core to propagate all parameters */
	dev->regs->reset &= ~JESD204B_RST;


}

void jesd204b_print_status(struct jesd204b_dev *dev)
{
	int i,j;

	xil_printf("\n\r------------------------------\n\r");
	xil_printf("JESD204B core status\n\r");
	xil_printf("   reset            : 0x%08x\n\r", dev->regs->reset);
	xil_printf("   config           : 0x%08x\n\r", dev->regs->config);
	xil_printf("   sysref           : 0x%08x\n\r", dev->regs->sysref);
	xil_printf("   status           : 0x%08x\n\r", dev->regs->status);


	/* Go through lanes */
	for (i = 0; i < dev->number_of_lanes; i++) {
		xil_printf("---Lane %d\n\r", i);
		/* Go through ILA config registers */
		for (j = 0; j < 4; j++) {
			xil_printf("      ILA cfg data %d  : 0x%08x\n\r", j, dev->lane_regs[i].ila_cfg_data[j]);
		}

		xil_printf("\n\r");
		xil_printf(    "      Test mode err cnt: 0x%08x\n\r", dev->lane_regs[i].lane_status);
		xil_printf(    "      Link err cnt     : 0x%08x\n\r", dev->lane_regs[i].mon_phys_err_cnt);
		xil_printf(    "      Link err cnt     : 0x%08x\n\r", dev->lane_regs[i].mon_phys_err_res_cnt);
		xil_printf(    "      Link err cnt     : 0x%08x\n\r", dev->lane_regs[i].mon_align_f_err_cnt);
		xil_printf(    "      Link err cnt     : 0x%08x\n\r", dev->lane_regs[i].mon_replace_f_cnt);
		xil_printf(    "      Link err cnt     : 0x%08x\n\r", dev->lane_regs[i].mon_replace_a_cnt);
		xil_printf(    "      Link err cnt     : 0x%08x\n\r", dev->lane_regs[i].mon_unknown_k_cnt);
		xil_printf(    "      Link err cnt     : 0x%08x\n\r", dev->lane_regs[i].mon_ilas_err_cnt);

	}
	
	xil_printf("-----------------------------------------------------------\n\r\n\r");
}

