/*\
 * WR Switch PHY testing tool
 	 Tomasz Wlostowski / 2012

	 WARNING: This is just my internal code for testing some timing-related stuff.
	 				  I'll document it and clean it up in the future, but for now, please
	 				  don't ask questions. sorry
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <time.h>
#include <assert.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <signal.h>

#define __EXPORTED_HEADERS__ /* prevent a #warning notice from linux/types.h */
//#define _LINUX_IF_ETHER_H /* prevent linux/if_ether.h that redefines ethhdr  */
#include <linux/mii.h>
#include <libwr/shw_io.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/if_ether.h>
#include <linux/if_arp.h>
#include <linux/if.h>

#include "../libwr/i2c_sfp.h"

#include <regs/endpoint-regs.h>
#include <regs/endpoint-mdio.h>

#undef PACKED
#include <libwr/ptpd_netif.h>
#define __EXPORTED_HEADERS__ /* prevent a #warning notice from linux/types.h */
#define _LINUX_IF_ETHER_H	/* prevent linux/if_ether.h that redefines ethhdr  */
#include <linux/mii.h>

#include <rt_ipc.h>
#include <libwr/switch_hw.h>

#include <libwr/shmem.h>
#include <libwr/hal_shmem.h>
#include <libwr/wrs-msg.h>
#include <libwr/util.h>

#include "../wrsw_hal/driver_stuff.h"

int hal_ports_fd;

int pcs_write(int port, int reg,uint16_t value )
{
	struct ifreq ifr;
	uint32_t rv;

	sprintf(ifr.ifr_name, "wri%d", port+1);

	rv = NIC_WRITE_PHY_CMD(reg, value);
	ifr.ifr_data = (void *)&rv;
	if (ioctl(hal_ports_fd, PRIV_IOCPHYREG, &ifr) < 0)
	{
		pr_error("%s: ioctl failed writing at register adress %d\n",
				__func__,reg);
		return -1;
	};

	return 0;
}

uint32_t pcs_read(int port, int reg )
{
	struct ifreq ifr;
	uint32_t rv;

	sprintf(ifr.ifr_name, "wri%d", port+1);
	
	rv = NIC_READ_PHY_CMD(reg);
	ifr.ifr_data = (void *)&rv;
	if (ioctl(hal_ports_fd, PRIV_IOCPHYREG, &ifr) < 0) {
		pr_error("%s: ioctl failed reading register at address %d\n",
				__func__, reg);
		return -1;
	}

	return NIC_RESULT_DATA(rv);
	
}

void dump_rx_pattern(int port)
{
	uint32_t rw = MDIO_LPC_CTRL_TX_ENABLE | MDIO_LPC_CTRL_DMTD_SOURCE_TXOUTCLK;
	uint32_t rr;
	int nbits = 40;
	int i;
	char rval[41];
	pcs_write(port, MDIO_LPC_CTRL, rw | MDIO_LPC_CTRL_DBG_TRIG );
	pcs_write(port, MDIO_LPC_CTRL, rw );

	for(i = 0; i < nbits; i++)
	{
		rr = pcs_read( port, MDIO_LPC_STAT );
		if ( rr & MDIO_LPC_STAT_DBG_DATA )
			rval[i] = '1';
		else
			rval[i]= '0';

		pcs_write(port, MDIO_LPC_CTRL, rw | MDIO_LPC_CTRL_DBG_SHIFT_EN );
		pcs_write(port, MDIO_LPC_CTRL, rw );
	}

	rval[nbits] = 0;

	printf("Port %d rxPattern='%s' ELKUP=%d\n", port, rval, (rr & MDIO_LPC_STAT_LINK_UP ? 1 : 0));

}

void try_rx_reset(int port, int attempts )
{
	int i;
	int fails=0;

	for(i=0;i<attempts;i++)
	{
		pcs_write(port, MDIO_LPC_CTRL, MDIO_LPC_CTRL_TX_ENABLE | MDIO_LPC_CTRL_RESET_RX | MDIO_LPC_CTRL_DMTD_SOURCE_TXOUTCLK);
		shw_udelay(10);
		pcs_write(port, MDIO_LPC_CTRL, MDIO_LPC_CTRL_TX_ENABLE | MDIO_LPC_CTRL_DMTD_SOURCE_TXOUTCLK);
		shw_udelay(1000);

		uint32_t r = pcs_read(port, MDIO_LPC_STAT);
		//	printf("stat %d %x ph %d\n", i, r, phases[i] );
			if (! (r & MDIO_LPC_STAT_LINK_UP) )
				fails++;				

	}
	printf("Port %d RX reset test: %d iterations/%d fails\n", port, attempts, fails);
}

int main(int argc, char **argv)
{
	const int n_ports = 10;
	int fails=0;
	int iter = 0;


	hal_ports_fd = socket(AF_PACKET, SOCK_DGRAM, 0);

	struct rts_pll_state pstate;
	int i;

	if (rts_connect(NULL) < 0)
	{
		printf("Can't connect to the RT subsys\n");
		return;
	}
		shw_udelay_init();

	rts_set_mode(RTS_MODE_GM_FREERUNNING);
	printf("Locking PLL: ");
	while (1)
	{
		rts_get_state(&pstate);
		if (pstate.flags & RTS_DMTD_LOCKED)
			break;
		printf(".");
		fflush(stdout);
		sleep(1);
	}
	printf("\n");

	for (i = 0; i < n_ports; i++)
		rts_ptracker_set_average_samples(i, 10);

	int check_phases = 0;
	int n_rx_reset_attempts = 100;

	for (;;)
	{
		for (i = 0; i < n_ports; i++)
			rts_enable_ptracker(i, 0);

		for (i = 0; i < n_ports; i++)
		{
			pcs_write(i, MDIO_LPC_CTRL, MDIO_LPC_CTRL_RESET_RX | MDIO_LPC_CTRL_RESET_TX | MDIO_LPC_CTRL_DMTD_SOURCE_TXOUTCLK);
			shw_udelay(10);
		}
		shw_udelay(10);

		for (i = 0; i < n_ports; i++)
		{
			pcs_write(i, MDIO_LPC_CTRL, MDIO_LPC_CTRL_TX_ENABLE | MDIO_LPC_CTRL_DMTD_SOURCE_TXOUTCLK);
			shw_udelay(10);
		}
		shw_udelay(10);
/*
		for (i = 0; i < n_ports; i++)
		{
			pcs_write(i, MDIO_LPC_CTRL, MDIO_LPC_CTRL_TX_ENABLE | MDIO_LPC_CTRL_RESET_RX | MDIO_LPC_CTRL_DMTD_SOURCE_TXOUTCLK);
					shw_udelay(10);

		}

		shw_udelay(10);
		for (i = 0; i < n_ports; i++)
		{
			pcs_write(i, MDIO_LPC_CTRL, MDIO_LPC_CTRL_TX_ENABLE | MDIO_LPC_CTRL_DMTD_SOURCE_TXOUTCLK);
					shw_udelay(10);

		}
*/

		if( check_phases )
		{
			for (i = 0; i < n_ports; i++)
				rts_enable_ptracker(i, 1);

			int phases[n_ports];
			while (1)
			{
				int all_phases_ok = 1;
				rts_get_state(&pstate);
				for (i = 0; i < n_ports; i++)
				{
					//printf("ch%d: flags %x\n", i, pstate.channels[i].flags );
					if ((pstate.channels[i].flags & CHAN_PMEAS_READY) == 0)
					{
						all_phases_ok = 0;
					} else {
						phases[i] = pstate.channels[i].phase_loopback;
					}
				}
				if (all_phases_ok)
					break;
			}
		}
		shw_udelay(10000);

		int ready_mask[n_ports];

		for (i = 0; i < n_ports; i++)
			ready_mask[i] = 0;

		for (i = 0; i < n_ports; i++)
		{
			uint32_t r = pcs_read(i, MDIO_LPC_STAT);
		//	printf("stat %d %x ph %d\n", i, r, phases[i] );
			if (r & MDIO_LPC_STAT_LINK_UP)
			{
				ready_mask[i] = 1;
			}
		}

		for (i = 0; i < n_ports; i++)
		{
			if (!ready_mask[i])
			{
				printf("\nport %d failing...\n", i);
				printf("Prior to reset: \n");
				dump_rx_pattern(i);
				try_rx_reset(i, n_rx_reset_attempts);
				printf("After reset: \n");
				dump_rx_pattern(i);
				fails++;
			}
		}
		
		if ( (iter % 1000) == 0)
		{
			printf("%d iterations done (%d fails) \r", iter, fails);
			fflush(stdout);
		}

		iter++;
		/*printf("Iter %d: ports: ", iter++);
		for (i = 0; i < n_ports; i++)
			printf("%d ", ready_mask[i]);
		printf("\n");*/
	}

	return 0;
}
