#include "wrs-iftool.h"

#include <util.h>

#ifndef true
#define true 1
#endif
#ifndef false
#define false 0
#endif

#define DYN_TRACES 1
#define INT32_MAX 0x7FFFFFFF
#include "trace.h"
#include <strings.h>
#include <rtud_exports.h>
#include <minipc.h>
#include <rtud_exports.h>

#define MINIPC_TIMEOUT 200

unsigned int traceLevel=TRACE_LEVEL_DEBUG;
#define TRACE_VERBOSE(...)      { if (verbose)   { fprintf(stdout, __VA_ARGS__); } }

/*Test:
 * Program for sending and receiving frames from  WR ports in the Switch v3, 
 * based on the  code developed by Tomasz, Rubini &Co  
 *
 * Copyright (C) 2012 Cesar  Prados c.prados@gsi.de
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License...
 */



#define MINIPC_TIMEOUT 200


static struct minipc_ch *rtud_ch;

static net_t ports[NUMBER_PORTS];
static net_t* ifaces[NUMBER_PORTS];
static int sw_table[NUMBER_PORTS];

static char *pgr_name;
static char *cmd_name;
static int max_loop=-1;
static int verbose=0;
static int upause=500;
static int delaylog=10;
static int payload=500;

/**
 * Obtain the interface number given the name
 * input: wrXX
 * output: int with XX or with -1 if arg was empty
 */
int get_if_num(const char *ifname)
{
	int ret=-1;
	if(ifname)
	{
		if(ifname[0]=='w' && ifname[1]=='r')
		{
			ret=atoi(ifname+2); //Remove wr
		}
		else TRACE_WARNING("argument '%s' is not a correct interface name ([wr0,wr17]\n",ifname);
	}
	return ret;
}



char *mac_to_string(uint8_t mac[])
{
	char str[40];
	snprintf(str, 40, "%02x:%02x:%02x:%02x:%02x:%02x", mac[0],mac[1],mac[2],mac[3],mac[4],mac[5]);
	return strdup(str); //FIXME: can't be static but this takes memory
}


#define ETH_ALEN 6
int mac_from_str(uint8_t* tomac, const char *fromstr)
{

	int i=0;
	char* str=(char*)fromstr;

	if(tomac==0 || fromstr==0) return -1;

	//Replace : by space
	while(str[i]!=0) {
		if(str[i]==':') str[i]=' ';
		i++;
	}

	for(i=0;i<ETH_ALEN && str;i++, str++)
		tomac[i] = strtol(str,&str,16);

	return 0;
}

int fill_table(const char* str)
{
	int i=0;
	while(i<=NUMBER_PORTS) {
		if(str[i]=='0') sw_table[i]=-1;
		else sw_table[i]=NUMBER_PORTS;
		i++;
	}
}






int rtudexp_add_entry(const char *eha, int port, int mode)
{
	int val, ret;
	ret = minipc_call(rtud_ch, MINIPC_TIMEOUT, &rtud_export_add_entry,&val,eha,port,mode);
	return (ret<0)?ret:val;
}

int rtudexp_clear_entries(int netif, int force)
{
	int val, ret;
	ret = minipc_call(rtud_ch, MINIPC_TIMEOUT, &rtud_export_clear_entries,&val,netif,force);
	return (ret<0)?ret:val;
}


//-------------------------------------------
// Net init, open socket close socket
//-------------------------------------------

int netif_get_hw_addr(socket_t *sock, mac_addr_t *mac)
{
	//struct socket_t *s = (struct my_socket *)sock;
	memcpy(mac, sock->local_mac, 6);
	return 0;
}

socket_t *netif_create_socket(int sock_type, int flags, sockaddr_t *bind_addr)
{
	socket_t *s;
	struct sockaddr_ll sll;
	struct ifreq f;
	int fd;

	hexp_port_state_t pstate;

	if(sock_type != PTPD_SOCK_RAW_ETHERNET)
		return NULL;

	if(halexp_get_port_state(&pstate, bind_addr->if_name) < 0)
		return NULL;

	fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));

	if(fd < 0)
	{
		perror("socket()");
		return NULL;
	}

	fcntl(fd, F_SETFL, O_NONBLOCK);

	// Put the controller in promiscious mode, so it receives everything
	strcpy(f.ifr_name, bind_addr->if_name);
	if(ioctl(fd, SIOCGIFFLAGS,&f) < 0) { perror("ioctl()"); return NULL; }
	f.ifr_flags |= IFF_PROMISC;
	if(ioctl(fd, SIOCSIFFLAGS,&f) < 0) { perror("ioctl()"); return NULL; }

	// Find the inteface index
	strcpy(f.ifr_name, bind_addr->if_name);
	ioctl(fd, SIOCGIFINDEX, &f);


	sll.sll_ifindex = f.ifr_ifindex;
	sll.sll_family   = AF_PACKET;
	sll.sll_protocol = htons(bind_addr->ethertype);
	sll.sll_halen = 6;

	memcpy(sll.sll_addr, bind_addr->mac, 6);

	if(bind(fd, (struct sockaddr *)&sll, sizeof(struct sockaddr_ll)) < 0)
	{
		close(fd);
		perror("bind()");
		return NULL;
	}

	s=calloc(sizeof(socket_t), 1);
	s->if_index = f.ifr_ifindex;

	// get interface MAC address
	if (ioctl(fd, SIOCGIFHWADDR, &f) < 0) {
		perror("ioctl()"); return NULL;
	}

	memcpy(s->local_mac, f.ifr_hwaddr.sa_data, 6);
	memcpy(&s->bind_addr, bind_addr, sizeof(sockaddr_t));
	s->fd = fd;

	return s;
}

uint8_t netIn(net_t* port, uint8_t i)
{
	mac_addr_t portMacAddress[6];

	// Create a socket:
	sockaddr_t bindaddr;
	int port_index = i;
	int if_num;

	// Port number
	if(port_index < 0)
	{
		TRACE_ERROR("port numbering problem, index returned: %d\n", port_index);
		return 1;
	}
	// establish the network interface name
	if(ptpd_netif_get_ifName(bindaddr.if_name,port_index ) == PTPD_NETIF_ERROR)
	{
		TRACE_ERROR("getting the interface name\n");
		return 1;
	}
	strncpy(port->ifaceName,bindaddr.if_name,IFACE_NAME_LENGTH);

	if_num=get_if_num(port->ifaceName);
	if(if_num<0 || if_num>=NUMBER_PORTS) TRACE_ERROR("Bad interface name: %s\n",port->ifaceName);

	ifaces[if_num]=port;

	if(!(ptpd_netif_get_port_state(port->ifaceName) == PTPD_NETIF_OK))
	{
		TRACE_INFO("Port: %s is not connected\n",port->ifaceName);
		port->linkUP=0;
		return 1;
	}
	else
	{
		port->linkUP=1;
		TRACE_INFO("Network interface : %s\n",port->ifaceName);
	}

	bindaddr.family = PTPD_SOCK_RAW_ETHERNET;
	bindaddr.ethertype = 0xcafe;
	memcpy(bindaddr.mac, MULTICAST_ADDR, sizeof(mac_addr_t)); //TODO set the mac address

	// socket for tx and rx
	port->sock = netif_create_socket(PTPD_SOCK_RAW_ETHERNET, 0, &bindaddr);

	if(port->sock ==  NULL)
	{
		return 1;
	}

	memcpy(port->unicastAddr.mac, UNICAST_ADDR,  sizeof(mac_addr_t));
	memcpy(port->multicastAddr.mac, MULTICAST_ADDR,  sizeof(mac_addr_t));

	port->unicastAddr.ethertype = 0xcafe;
	port->multicastAddr.ethertype = 0xcafe;

	netif_get_hw_addr(port->sock, portMacAddress);

	// copy mac part to uuid
	memcpy(port->port_uuid_field,portMacAddress, UUID_LENGTH);
	memcpy(port->selfAddr.mac, portMacAddress, 6);

	TRACE_INFO("mac: %x:%x:%x:%x:%x:%x\n",\
			port->port_uuid_field[0],\
			port->port_uuid_field[1],\
			port->port_uuid_field[2],\
			port->port_uuid_field[3],\
			port->port_uuid_field[4],\
			port->port_uuid_field[5]);

	TRACE_INFO("netInit: exiting OK\n");

	return 0;

}

int netif_close_socket(socket_t *sock)
{
	//struct my_socket *s = (struct my_socket *) sock;

	if(!sock)
		return 0;

	close(sock->fd);
	return 0;
}

//-------------------------------------------
// Send 
//-------------------------------------------
int netif_sendto(socket_t *s, sockaddr_t *to, void *data, uint32_t data_length)
{
	struct etherframe pkt;
	struct sockaddr_ll sll;
	int rval;

	if(s->bind_addr.family != PTPD_SOCK_RAW_ETHERNET)
		return -ENOTSUP;

	if(data_length > 1518-8) return -EINVAL;

	memset(&pkt, 0, sizeof(struct etherframe));

	memcpy(pkt.ether.h_dest, to->mac, 6);
	memcpy(pkt.ether.h_source, s->local_mac, 6);
	pkt.ether.h_proto =htons(to->ethertype);

	memcpy(pkt.data, data, data_length);

	TRACE_DEBUG("%s => %s (type=%d, %d B)\n",mac_to_string(s->local_mac),mac_to_string(to->mac),to->ethertype,data_length);

	size_t len = data_length + sizeof(struct ethhdr);

	if(len < 60) /* pad to the minimum allowed packet size */
		len = 60;

	memset(&sll, 0, sizeof(struct sockaddr_ll));

	sll.sll_ifindex = s->if_index;
	sll.sll_family = AF_PACKET;
	sll.sll_protocol = htons(to->ethertype);
	sll.sll_halen = 6;

	rval =  sendto(s->fd, &pkt, len, 0, (struct sockaddr *)&sll,
			sizeof(struct sockaddr_ll));
	return rval;
}

uint32_t net_send(char *buf, uint32_t length, net_t *port)
{
	uint32_t ret=0;

	ret = netif_sendto(port->sock, &port->multicastAddr,
			buf, length);

	if(ret <= 0)
		printf("ERROR sending event message\n");
	return ret;
}

//-------------------------------------------
// recvfrom 
//-------------------------------------------
uint32_t netif_recvfrom(socket_t *s, sockaddr_t *from, void *data, uint32_t data_length)
{
	struct etherframe pkt;
	struct msghdr msg;
	struct iovec entry;
	struct sockaddr_ll from_addr;
	struct {
		struct cmsghdr cm;
		char control[1024];
	} control;
	//	struct cmsghdr *cmsg;
	//	struct scm_timestamping *sts = NULL;

	size_t len = data_length + sizeof(struct ethhdr);

	memset(&msg, 0, sizeof(msg));
	msg.msg_iov = &entry;
	msg.msg_iovlen = 1;
	entry.iov_base = &pkt;
	entry.iov_len = len;
	msg.msg_name = (caddr_t)&from_addr;
	msg.msg_namelen = sizeof(from_addr);
	msg.msg_control = &control;
	msg.msg_controllen = sizeof(control);

	int ret = recvmsg(s->fd, &msg, MSG_DONTWAIT);

	if(ret < 0 && errno == EAGAIN) return 0; // would be blocking
	if(ret == -EAGAIN) return 0;

	if(ret <= 0) return ret;

	memcpy(data, pkt.data, ret - sizeof(struct ethhdr));

	from->ethertype = ntohs(pkt.ether.h_proto);
	memcpy(from->mac, pkt.ether.h_source, 6);
	memcpy(from->mac_dest, pkt.ether.h_dest, 6);

	return ret;
}

uint32_t net_recv(char *buf, net_t *port)
{
	sockaddr_t from_addr;
	int ret, drop = 1;

	ret = netif_recvfrom(port->sock, &from_addr, buf, 1518);
	if(ret < 0)
		return ret;

	if( !memcmp(from_addr.mac_dest, port->selfAddr.mac, 6) ||
			!memcmp(from_addr.mac_dest, port->multicastAddr.mac, 6) ||
			!memcmp(from_addr.mac_dest, port->unicastAddr.mac, 6))
		drop = 0;

	return drop ? 0 : ret;
}

//-------------------------------------------
//Signaling
//-------------------------------------------
uint8_t forever = 1;

void sighandler(int sig)
{
	forever = 0;
}




/**
 * Transfer from the same MAC address.
 */
int32_t iftxmac(int argc, char *argv[])
{
	int tx_ifn, tx_start=0, tx_stop=NUMBER_PORTS;
	int rx_ifn, rx_start=0, rx_stop=NUMBER_PORTS;
	net_t *if_tx, *if_rx;
	uint32_t len;
	char bufferO[ETHER_MTU], bufferI[ETHER_MTU];
	uint64_t idCounter=0;
	int loop=0;


	sockaddr_t dst;

	print_args(argc,argv);
	tx_ifn=get_if_num((argc>1)?argv[1]:0);
	TRACE_INFO("tx=wr%d, rx=wr%d\n",tx_ifn, rx_ifn);
	if_tx=ifaces[tx_ifn];

	if(if_tx && if_tx->linkUP)
	{
		printf("sending from %s...\n",if_tx->ifaceName);
		if(argc>2) mac_from_str(if_tx->multicastAddr.mac,argv[2]);
		if(argc>3) mac_from_str(if_tx->sock->local_mac,argv[3]);
		printf("tx=wr%-2d, rx=wr%-2d (%lu) %s ",tx_ifn, rx_ifn,idCounter, mac_to_string(if_tx->multicastAddr.mac_dest));


		while(idCounter< max_loop)
		{
			len = snprintf(bufferO,128, "%llu", idCounter);
			if(net_send((char*)bufferO,payload,if_tx)>0) printf("OK\n");
			else printf("KO\n");
			idCounter++;

			usleep(upause);
		}
	}

	printf("\n");
	return 1;
}



/**
 * Actually send bcast but not final function.
 */
int32_t iftest(int argc, char *argv[])
{
	int tx_ifn, tx_start=0, tx_stop=NUMBER_PORTS;
	int rx_ifn, rx_start=0, rx_stop=NUMBER_PORTS;
	net_t *if_tx, *if_rx;
	uint32_t len;
	char bufferO[ETHER_MTU], bufferI[ETHER_MTU];
	uint64_t idCounter=1234567890;
	uint64_t nTX=0, nRX=0, nRX_OK=0, nRX_OKp=0;
	int loop=0;
	timeval_t current_t, last_t;

	print_args(argc,argv);

	//Parsing the args
	fill_table((argc>1)?argv[1]:"001110010101010101");

	printf("%s 0:%d,%d,%d,%d 4:%d,%d,%d,%d 8:%d,%d,%d,%d 12:%d,%d,%d,%d 16:%d,%d\n",cmd_name,
			sw_table[00],sw_table[01],sw_table[02],sw_table[03],
			sw_table[04],sw_table[05],sw_table[06],sw_table[07],
			sw_table[8] ,sw_table[9], sw_table[10],sw_table[11],
			sw_table[12],sw_table[13],sw_table[14],sw_table[15],
			sw_table[16],sw_table[17]);


	//Connecting to rtud
	if(	halexp_client_init() < 0)
	{
		printf("Can't conenct to HAL mini-rpc server\n");
		return -1;
	}
	rtud_ch = minipc_client_create("rtud", 0);
	if(!rtud_ch)
	{
		printf("Can't connect to RTUd mini-rpc server\n");
		return -1;
	}


	//Setting up entries
	for(tx_ifn=NUMBER_PORTS-1;tx_ifn>=0;tx_ifn--)
	{
		if_tx=ifaces[tx_ifn];
		if(if_tx && if_tx->linkUP && sw_table[tx_ifn]>=0)
		{
			TRACE_VERBOSE("sending from %s...",if_tx->ifaceName);
			memcpy(if_tx->multicastAddr.mac,BCAST_ADDR,sizeof(mac_addr_t));
			//memcpy(if_tx->multicastAddr.mac,if_tx->sock->local_mac,sizeof(mac_addr_t));
			if(net_send(bufferO,payload,if_tx)>0)
			{
				TRACE_VERBOSE("OK");
				nTX++;
			}
			TRACE_VERBOSE("\n");
			usleep(upause);
			for(rx_ifn=0;rx_ifn<NUMBER_PORTS;rx_ifn++)
			{
				if(rx_ifn!=tx_ifn) {
					if_rx=ifaces[rx_ifn];
					if(if_rx && if_rx->linkUP && sw_table[rx_ifn]>=0)
					{
						TRACE_DEBUG("tx=wr%d, rx=wr%d\n",tx_ifn, rx_ifn);
						if(net_recv(bufferI,if_rx)>0)
						{
							TRACE_VERBOSE("receiving from %s... #%s ",if_rx->ifaceName,bufferI);
							if(memcmp(bufferI,bufferO,128)==0) {
								TRACE_VERBOSE("OK\n");
							}
							else
							{
								TRACE_ERROR("Bad Packet: %llu\n",idCounter);
								TRACE_VERBOSE("\n");
							}
						}
					}
				}
			}
			idCounter++;
		}


	}





	return 0;


	gettimeofday(&last_t,NULL);
	gettimeofday(&current_t, NULL);

	while(loop++<max_loop)
	{
		for(tx_ifn=tx_start;tx_ifn<tx_stop;tx_ifn++)
		{
			if_tx=ifaces[tx_ifn];
			if(if_tx && if_tx->linkUP)
			{
				TRACE_VERBOSE("sending from %s...",if_tx->ifaceName);
				//				memcpy(if_tx->multicastAddr.mac,BCAST_ADDR,sizeof(mac_addr_t));
				len = snprintf(bufferO,128, "%llu", idCounter);
				if(net_send(bufferO,payload,if_tx)>0) {
					TRACE_VERBOSE("OK\n");
					nTX++;
				}
				else TRACE_VERBOSE("KO\n");


				for(rx_ifn=rx_start;rx_ifn<rx_stop;rx_ifn++)
				{
					if(rx_ifn!=tx_ifn) {
						if_rx=ifaces[rx_ifn];
						if(if_rx && if_rx->linkUP)
						{
							TRACE_DEBUG("tx=wr%d, rx=wr%d\n",tx_ifn, rx_ifn);
							if(net_recv(bufferI,if_rx)>0)
							{
								TRACE_VERBOSE("receiving from %s... #%s ",if_rx->ifaceName,bufferI);
								nRX++;
								if(memcmp(bufferI,bufferO,128)==0) {
									TRACE_VERBOSE("OK\n");
									nRX_OK++;
								}
								else
								{
									TRACE_ERROR("Bad Packet: %llu\n",idCounter);
									TRACE_VERBOSE("\n");
								}
							}
						}
					}
				}
				idCounter++;
			}
		}

		TRACE_VERBOSE("loop #%-8d ; err= %3.1f %% (nTX=%llu, nRX=%llu, nRX_ok=%llu, nErr=%llu)\n",loop,100.f*(float)(nTX-nRX_OK)/(float)nTX,nTX,nRX,nRX,nTX-nRX_OK);
		gettimeofday(&current_t, NULL);
		if(current_t.tv_sec > (last_t.tv_sec+delaylog))
		{
			printf("loop #%-8d ; err= %3.1f %% (nTX=%-8llu, nRX=%-8llu, nRX_ok=%-8llu, nErr=%-8llu, bw=%.2f MBps)\n",
					loop,
					100.f*(float)(nTX-nRX_OK)/(float)nTX,
					nTX,nRX,nRX,nTX-nRX_OK,
					(float)((nRX_OK-nRX_OKp)*payload)/((float)pow(2,20)*(current_t.tv_sec -last_t.tv_sec)));
			gettimeofday(&last_t,NULL);
			nRX_OKp=nRX_OK;
		}
		if(upause>0) usleep(upause);
	}
	return 1;
}





/**
 * Send to the designated interface using unicast as source.
 * This was use for CE marks with: testd -t 3 -p 0 -n 1000000 -l 10 ucast
 */
int32_t ifucast(int argc, char *argv[])
{
	int tx_ifn, tx_start=0, tx_stop=NUMBER_PORTS;
	int rx_ifn, rx_start=0, rx_stop=NUMBER_PORTS;
	net_t *if_tx, *if_rx;
	uint32_t len;
	char bufferO[ETHER_MTU], bufferI[ETHER_MTU];
	uint64_t idCounter=1234567890;
	uint64_t nTX=0, nRX=0, nRX_OK=0, nRX_OKp=0;
	int loop=0;
	timeval_t current_t, last_t;

	print_args(argc,argv);

	tx_ifn=get_if_num((argc>1)?argv[1]:0);
	if(0 <= tx_ifn && tx_ifn<NUMBER_PORTS)
	{
		tx_start=tx_ifn;
		tx_stop=tx_ifn+1;
	}

	rx_ifn=get_if_num((argc>2)?argv[2]:0);
	if(0 <= rx_ifn && rx_ifn<NUMBER_PORTS)
	{
		rx_start=rx_ifn;
		rx_stop=rx_ifn+1;
	}

	printf("%s tx=wr%d, rx=wr%d, payload=%d\n",cmd_name, tx_ifn, rx_ifn,payload);

	gettimeofday(&last_t,NULL);
	gettimeofday(&current_t, NULL);

	while(loop++<max_loop)
	{
		for(tx_ifn=tx_start;tx_ifn<tx_stop;tx_ifn++)
		{
			if_tx=ifaces[tx_ifn];
			if(if_tx && if_tx->linkUP)
			{
				TRACE_VERBOSE("sending from %s...",if_tx->ifaceName);
				//				memcpy(if_tx->multicastAddr.mac,BCAST_ADDR,sizeof(mac_addr_t));
				len = snprintf(bufferO,128, "%llu", idCounter);
				if(net_send(bufferO,payload,if_tx)>0) {
					TRACE_VERBOSE("OK\n");
					nTX++;
				}
				else TRACE_VERBOSE("KO\n");


				for(rx_ifn=rx_start;rx_ifn<rx_stop;rx_ifn++)
				{
					if(rx_ifn!=tx_ifn) {
						if_rx=ifaces[rx_ifn];
						if(if_rx && if_rx->linkUP)
						{
							TRACE_DEBUG("tx=wr%d, rx=wr%d\n",tx_ifn, rx_ifn);
							if(net_recv(bufferI,if_rx)>0)
							{
								TRACE_VERBOSE("receiving from %s... #%s ",if_rx->ifaceName,bufferI);
								nRX++;
								if(memcmp(bufferI,bufferO,128)==0) {
									TRACE_VERBOSE("OK\n");
									nRX_OK++;
								}
								else
								{
									TRACE_ERROR("Bad Packet: %llu\n",idCounter);
									TRACE_VERBOSE("\n");
								}
							}
						}
					}
				}
				idCounter++;
			}
		}

		TRACE_VERBOSE("loop #%-8d ; err= %3.1f %% (nTX=%llu, nRX=%llu, nRX_ok=%llu, nErr=%llu)\n",loop,100.f*(float)(nTX-nRX_OK)/(float)nTX,nTX,nRX,nRX,nTX-nRX_OK);
		gettimeofday(&current_t, NULL);
		if(current_t.tv_sec > (last_t.tv_sec+delaylog))
		{
			printf("loop #%-8d ; err= %3.1f %% (nTX=%-8llu, nRX=%-8llu, nRX_ok=%-8llu, nErr=%-8llu, bw=%.2f MBps)\n",
					loop,
					100.f*(float)(nTX-nRX_OK)/(float)nTX,
					nTX,nRX,nRX,nTX-nRX_OK,
					(float)((nRX_OK-nRX_OKp)*payload)/((float)pow(2,20)*(current_t.tv_sec -last_t.tv_sec)));
			gettimeofday(&last_t,NULL);
			nRX_OKp=nRX_OK;
		}
		if(upause>0) usleep(upause);
	}
	return 1;
}


typedef struct {
	int ID;
	int rand;
	int ifsrc_id;
	char ifsrc_name[IFACE_NAME_LEN];
} pkt_test_t;

struct _pkt_test_count {
	uint32_t ok_TX;
	uint32_t ok_RX;
	uint32_t ok_RXd;
};

/**
 * Checking if the interface exist.
 *
 * This is used in the PTS suite to check with loopback connector.
 */
int ifcheck(int argc, char *argv[])
{
	int i,loop=0;
	uint32_t len;
	uint8_t if_ok_sum=0;
	char bufferTX[ETHER_MTU], bufferRX[ETHER_MTU];
	pkt_test_t ptest_in[NUMBER_PORTS] = {0};
	struct _pkt_test_count ptest_count[NUMBER_PORTS] = {0};
	pkt_test_t ptest_RX;

	uint64_t idCounter=0;
	net_t* port;


	/* stdin is line-buffered: to see "any" key we need to unbuffer it */
	setbuf(stdin, NULL);
	fprintf(stderr,"Checking WR interface with loopback\nTo escape this test press: Esc+Enter...\n\n");

	TRACE_INFO("loop=%d. max=%d\n",loop,max_loop);

	while(loop++!=max_loop)
	{
		fd_set set;
		struct timeval tv = {0};

		TRACE_INFO("loop=#%-6d\n",loop);
		//loop on the port
		for(i=0;i<NUMBER_PORTS;i++)
		{
			port=ifaces[i];
			if(port && port->linkUP && ptest_count[i].ok_RXd==0)
			{
				idCounter++;
				ptest_in[i].ID=idCounter;
				ptest_in[i].rand=rand();
				memcpy(bufferTX,&ptest_in[i],sizeof(pkt_test_t));

				//memcpy(port->multicastAddr.mac,port->sock->local_mac,sizeof(mac_addr_t));
				//memcpy(port->sock->local_mac,port->multicastAddr.mac,sizeof(mac_addr_t));
				//port->sock->local_mac[3]=~port->sock->local_mac[3];
				//port->sock->local_mac[0]=0xFF;
				//memcpy(port->multicastAddr.mac,CPU_ADDR,sizeof(mac_addr_t));

				if(net_send(bufferTX,payload,port)>0)
				{
					TRACE_VERBOSE("wr%d ... ",i);
					ptest_count[i].ok_TX++;
				}

				usleep(upause);


				if(net_recv(bufferRX,port)>0)
				{
					TRACE_VERBOSE("Frame Rx wr%d... ",i);
					ptest_count[i].ok_RX++;
					memcpy(&ptest_RX,bufferRX,sizeof(pkt_test_t));

					if(memcmp(&ptest_in[ptest_RX.ifsrc_id],&ptest_RX,sizeof(pkt_test_t))==0)
					{
						fprintf(stderr,"\n%s => %s OK\n",ptest_RX.ifsrc_name,port->ifaceName);
						ptest_count[i].ok_RXd++;
						if_ok_sum++;
					}
					else
					{
						TRACE_DEBUG("TX #%d:%s > %d, %d\n",ptest_in[i].ifsrc_id,ptest_in[i].ifsrc_name,ptest_in[i].ID,ptest_in[i].rand);
						TRACE_DEBUG("RX #%d:%s > %d, %d\n",ptest_RX.ifsrc_id,ptest_RX.ifsrc_name,ptest_RX.ID,ptest_RX.rand);
					}
				}
			}
		}


		//Here you should try to match the sent frame with the incoming frames!!!!
		//Close & reope
		TRACE_INFO(" Restaring...\n");
		for(i=0;i<NUMBER_PORTS;i++)
		{
			netif_close_socket(ports[i].sock);
			netIn(&ports[i], i);
			memcpy(ptest_in[i].ifsrc_name,ports[i].ifaceName,IFACE_NAME_LENGTH);
			ptest_in[i].ifsrc_id=i;
		}

		if(if_ok_sum>=NUMBER_PORTS) break;


		/* getc/getchar are blocking, can't use them */
		FD_ZERO(&set);
		FD_SET(STDIN_FILENO, &set);
		if (select(STDIN_FILENO + 1, &set, NULL, NULL, &tv) != 0)
			if(getchar()==27)
				break;

		//if(kbhit() && getchar_unlocked()==27) break; //27 is ESC char
	}

	printf("\niface:\t\t TX_ok\t RX_ok\t RXD_ok\n");
	for(i=0;i<NUMBER_PORTS;i++)
	{
		printf("#%02d (wr%-2d):\t %5u\t %5u\t %6u\n",i+1,i, ptest_count[i].ok_TX, ptest_count[i].ok_RX, ptest_count[i].ok_RXd);
	}


	return (if_ok_sum>=NUMBER_PORTS)?0:-1;
}


/**
 * Send a bcast and try to see who respond.
 */
int32_t ifbcast(int argc, char *argv[])
{
	int tx_ifn, tx_start=0, tx_stop=NUMBER_PORTS;
	int rx_ifn, rx_start=0, rx_stop=NUMBER_PORTS;
	net_t *if_tx, *if_rx;
	uint32_t len;
	char bufferO[ETHER_MTU], bufferI[ETHER_MTU];
	uint64_t idCounter=1234567890;
	uint64_t nTX=0, nRXup=0, nRX_OK=0, nRX_OKp=0;
	int loop=0;
	timeval_t current_t, last_t;

	print_args(argc,argv);

	//Parsing the args
	fill_table((argc>1)?argv[1]:"001110010101010101");

	printf("%s 0:%d,%d,%d,%d 4:%d,%d,%d,%d 8:%d,%d,%d,%d 12:%d,%d,%d,%d 16:%d,%d\n",cmd_name,
			sw_table[00],sw_table[01],sw_table[02],sw_table[03],
			sw_table[04],sw_table[05],sw_table[06],sw_table[07],
			sw_table[8] ,sw_table[9], sw_table[10],sw_table[11],
			sw_table[12],sw_table[13],sw_table[14],sw_table[15],
			sw_table[16],sw_table[17]);

	gettimeofday(&last_t,NULL);
	gettimeofday(&current_t, NULL);

	while(loop++<max_loop)
	{
		//Setting up entries
		for(tx_ifn=NUMBER_PORTS-1;tx_ifn>=0;tx_ifn--)
		{
			if_tx=ifaces[tx_ifn];
			if(if_tx && if_tx->linkUP && sw_table[tx_ifn]>=0)
			{
				TRACE_VERBOSE("sending from %s...",if_tx->ifaceName);
				memcpy(if_tx->multicastAddr.mac,BCAST_ADDR,sizeof(mac_addr_t));
				//memcpy(if_tx->multicastAddr.mac,if_tx->sock->local_mac,sizeof(mac_addr_t));
				if(net_send(bufferO,payload,if_tx)>0)
				{
					TRACE_VERBOSE("OK");
					nTX++;
				}
				TRACE_VERBOSE("\n");
				//usleep(upause);
				for(rx_ifn=0;rx_ifn<NUMBER_PORTS;rx_ifn++)
				{
					if(rx_ifn!=tx_ifn) {
						if_rx=ifaces[rx_ifn];
						if(if_rx && if_rx->linkUP && sw_table[rx_ifn]>=0)
						{
							nRXup++;
							TRACE_DEBUG("tx=wr%d, rx=wr%d\n",tx_ifn, rx_ifn);
							if(net_recv(bufferI,if_rx)>0)
							{
								TRACE_VERBOSE("receiving from %s... #%s ",if_rx->ifaceName,bufferI);
								if(memcmp(bufferI,bufferO,128)==0) {
									TRACE_VERBOSE("OK\n");
									nRX_OK++;
								}
								else
								{
									TRACE_ERROR("Bad Packet: %llu\n",idCounter);
									TRACE_VERBOSE("\n");
								}
							}
						}
					}
				}
				idCounter++;
			}


		}


		TRACE_VERBOSE("loop #%-8d ; err= %3.1f %% (nB=%3.1f, nTX=%llu, nRX_up=%llu, nRX_ok=%llu, nErr=%llu)\n",
				loop,100.f*(float)(nRXup-nRX_OK)/(float)nRXup,((float)nRXup/(float)nTX),nTX,nRXup,nRX_OK,nRXup-nRX_OK);
		gettimeofday(&current_t, NULL);
		if(current_t.tv_sec > (last_t.tv_sec+delaylog))
		{
			printf("loop #%-8d ; err= %3.1f %% (nB=%3.1f, nTX=%-8llu, nRX_up=%-8llu, nRX_ok=%-8llu, nErr=%-8llu, bw=%.2f MBps)\n",
					loop,
					100.f*(float)(nRXup-nRX_OK)/(float)nRXup,((float)nRXup/(float)nTX),
					nTX,nRXup,nRX_OK,nRXup-nRX_OK,
					(float)((nRX_OK-nRX_OKp)*payload)/((float)pow(2,20)*(current_t.tv_sec -last_t.tv_sec)));
			gettimeofday(&last_t,NULL);
			nRX_OKp=nRX_OK;
		}
		if(upause>0) usleep(upause);
	}
	return 1;
}

void iflist()
{
	net_t* port;
	int i;
	TRACE_DEBUG("listing\n")

	for(i=0; i < NUMBER_PORTS; i++)
	{
		port=ifaces[i];
		if(port==0) continue;

		if(port->linkUP)
		{
			printf("#%02d: iface %-4s is OK (",i+1, port->ifaceName);
			printf("mac: %x:%x:%x:%x:%x:%x)\n",\
					port->port_uuid_field[0],\
					port->port_uuid_field[1],\
					port->port_uuid_field[2],\
					port->port_uuid_field[3],\
					port->port_uuid_field[4],\
					port->port_uuid_field[5]);

		}
		else
		{

			printf("#%02d: iface %-4s is not connected\n",i, port->ifaceName);
			port->linkUP=0;
		}
	}

}



void ifstatic(int argc, char *argv[])
{
	int i, is_ok=true;
	net_t* port;
	char eha[20];

	int if_start=get_if_num((argc>1)?argv[1]:0);
	int if_end=get_if_num((argc>2)?argv[2]:0);


	if(	halexp_client_init() < 0)
	{
		printf("Can't conenct to HAL mini-rpc server\n");
		return;
	}


	rtud_ch = minipc_client_create("rtud", 0);

	if(!rtud_ch)
	{
		printf("Can't connect to RTUd mini-rpc server\n");
		return;
	}
	minipc_set_logfile(rtud_ch,stderr);


	print_args(argc,argv);

	//Checking arguments
	if((0< if_start && if_start< NUMBER_PORTS)==0) return;
	if((if_start<if_end && if_end <= NUMBER_PORTS)==0) if_end=if_start;

	for(i=if_start;i<=if_end;i++)
	{
		port=ifaces[i];
		if(port==0) continue;

		if(port->linkUP)
		{
			printf("#%02d: iface %-4s is now static \n",i+1, port->ifaceName);
			snprintf(eha,18,"%02x:%02x:%02x:%02x:%02x:%02x",\
					port->port_uuid_field[0],\
					port->port_uuid_field[1],\
					port->port_uuid_field[2],\
					port->port_uuid_field[3],\
					port->port_uuid_field[4],\
					port->port_uuid_field[5]);

			is_ok = is_ok && (rtudexp_add_entry(eha,i,0)==0);
		}
	}
}




void help()
{
	printf("WhiteRabbit Interface Test (c) B.R 2012\n");
	//printf("Compiled by %s (%s)\ngit rev:%s\n\n",git_user,build_time,git_revision);
	printf("usage: %s [Options] <command> [<args>] \n", pgr_name);
	printf("Options:\n"
			"	-n <num>     Number of Loop (def=%d)\n"
			"	-p <u_sec>   Pause in usec (def=%d)\n"
			"	-t <level>   Trace Level (def=%d)\n"
			"	-z payload   Packet payload in bytes (def=%d)\n"
			"	-l <sec>     Log refreshing rate (def=%d)\n"
			"	-v           Verbose (show all packet)\n"
			"\n",max_loop,upause,traceLevel,payload,delaylog);
	printf("Command:\n"
			"	list                         List the interfaces\n"
			"	check                        Check if the interface is ok using loopback SFPs\n"
			"	static <ifStart> <ifEnd>     Set the interface as static between if_start & if_end\n"
			"	bcast <ifMAP> 			     Broadcast to the interface in ifMAP\n"
			"	ucast [<ifTX>] [<ifRX>]      Send a unicast packet from ifTX and wait on one ifRX to receive it\n"
			"	mac <ifTX> [dstMAC] [srcMAC] Send from ifTX to a dstMAC (with a specific srcMAC)\n"
			"	test                         Test the interfaces (not fully working)\n"
			"	help                         Show this message\n"
			"\n");
	printf("\nifMAP:110000000000000011 (Only send from the two firsts and the two lasts interfaces)\n");
	exit(0);
}


void print_args(int argc, char *argv[])
{
	TRACE_DEBUG("argc=%d: ",argc);
	while(argc>0)
	{
		TRACE_DEBUG("%s, ",argv[0]);
		argc--;
		argv++;
	}
	TRACE_DEBUG("\n");
}

int atoidefl(const char* str, int val, int min, int max)
{
	int t=atoidef(str,val);
	return (t>max)?max:((t<min)?min:t);
}

//-------------------------------------------
//Main
//-------------------------------------------
int32_t main(int argc, char *argv[])
{

	uint8_t i;
	char opt=0;	int optind=1;

	pgr_name=argv[0];
	if (argc == 1) help();

	signal( SIGINT,&sighandler);


	//Parse General Options
	while ((opt = getopt(argc, argv, "n:p:t:z:l:vh?")) != 0xFF)
	{
		optind++;
		switch (opt)
		{
		case 'n': max_loop=atoidef(optarg,max_loop); optind++; break;
		case 't': traceLevel=atoidef(optarg,traceLevel); optind++; break;
		case 'p': upause=atoidef(optarg,upause); optind++; break;
		case 'z': payload=atoidefl(optarg,payload,10,1500); optind++; break;
		case 'l': delaylog=atoidefl(optarg,delaylog,0,INT32_MAX); optind++; break;
		case 'v': verbose=1; break;
		case 'h':
		case '?': help();
		default:
			printf("find: illegal option %c (%x)\n", opt,opt);
			break;
		}
	}
	argc-=optind;
	argv+=optind;

	TRACE_DEBUG("n=%d, t=%d, p=%d, v=%d\n",max_loop,traceLevel,upause,verbose);
	TRACE_DEBUG("%s %s\n",__DATE__, __TIME__);

	// start netif
	if(ptpd_netif_init()<0)
		return -1;

	// initialization of the ports
	for(i=0; i < NUMBER_PORTS; i++)
	{
		netIn(&ports[i], i);
	}
	TRACE_DEBUG("Init done.\n");


	//Parse Commands
	cmd_name=argv[0];
	if (strcmp(cmd_name, "help") == 0) 			help();
	else if (strcmp(cmd_name, "list") == 0) 	iflist();
	else if (strcmp(cmd_name, "check") == 0) 	ifcheck(argc,argv);
	else if (strcmp(cmd_name, "static") == 0) 	ifstatic(argc,argv);
	else if (strcmp(cmd_name, "ucast") == 0) 	ifucast(argc,argv);
	else if (strcmp(cmd_name, "bcast") == 0) 	ifbcast(argc,argv);
	else if (strcmp(cmd_name, "mac") == 0) 		iftxmac(argc,argv);
	else if (strcmp(cmd_name, "test") == 0) 	iftest(argc,argv);
	else {
		if(cmd_name) printf("Command: '%s' is unknown\n",cmd_name);
		help();
	}


	TRACE_DEBUG("Closing sockets... \n");

	for(i=0;i < NUMBER_PORTS; i++)
		netif_close_socket(ports[i].sock);

	return 0;
}



