/*
 * ===============================================================================
 *	  Project:  FEC decoder @ SW
 *
 *       Filename:  fec_decoder.cpp
 *
 *    Description:  Reads FEC frames (recognized by EtherType) from the socket 
 *		    Recognizes FEC header 
 *		    Does de-hamming (based on Cesar's with corrected table)
 *		    Hamming parity bytes at the end of each chunk (not at the 
 *		    end of the frame )
 *
 *        Version:  1.0
 *        Created:  04/09/2011 
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Maciej Lipinski (based on test-drive.c by C.Prados)
 *        Company:  CERN
 *
 * ============================================================================
 */

#include "fec.h"
#include "fec_decoder.h"

my_socket *create_socket(wr_sockaddr_t *bind_addr)
{
	struct sockaddr_ll sll;
	struct my_socket *s;
	struct ifreq f;

	int fd;

	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=(struct my_socket*)calloc(sizeof(struct my_socket), 1);
	s->if_index = f.ifr_ifindex;
	memcpy(s->local_mac, f.ifr_hwaddr.sa_data, 6);
	memcpy(&s->bind_addr, bind_addr, sizeof(wr_sockaddr_t));
	s->fd = fd;
	return (my_socket*)s;
}

int recvfrom(my_socket *sock, wr_sockaddr_t *from, void *data, size_t data_length)
{
	struct my_socket *s = (struct my_socket *)sock;
	struct etherpacket pkt;
	struct msghdr msg;
	struct iovec entry;
	struct sockaddr_ll from_addr;
	struct {
		struct cmsghdr cm;
		char control[1024];
	} control;
	struct cmsghdr *cmsg;
	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 - sizeof(struct ethhdr);


}

int main()
{
   struct my_socket *socket;
  
   wr_sockaddr_t 	bindaddr;
   wr_sockaddr_t 	dummy_addr;
   int 			recv_msg_size, i;
   
   unsigned char 	buf[PACKET_SIZE];
   unsigned char 	msg[PACKET_SIZE];
   
   const unsigned char* recoveredMessage;
   int  		recoveredMsgSize = 0;
   unsigned int  	recoveredEtherType;
   int 			fec_decoder_ret;
   
   
   /* open socket and other connection magic*/
   strcpy(bindaddr.if_name, "eth4"); //TODO
   bindaddr.ethertype = 0xDEED; 	        // FEC's etherType, fecked
   memcpy(bindaddr.mac, FEC_ADDR, sizeof(mac_addr_t));
   socket =  create_socket(&bindaddr);
    
   int cnt=1;
   
   while(1)
   {
      if((recv_msg_size = recvfrom(socket, &dummy_addr, (unsigned char*)buf, PACKET_SIZE)) > 0)
      {
	printf("\n-----------------------------  FEC_DECODER  ----------------------------\n");
	printf("received sth of size: %d bytes\n",recv_msg_size);
	cnt++;
	for(i=0;i<recv_msg_size;) 						//endianess
	{
	  //header
	  if(i < FEC_HEADE_SIZE) 					
	  {
	    msg[i]   = buf[i];
	    msg[i+1] = buf[i+1];
	    i=i+2;
	  }
	  //payload
	  else 								
	  {
	    msg[i]   = buf[i+1];
	    msg[i+1] = buf[i];
	    i=i+2;	  
	  }
	}
	
	
	recoveredMessage = fec_decode(msg,recv_msg_size,&fec_decoder_ret, &recoveredEtherType);
	
	
	if(fec_decoder_ret > 0)//message recovered
	{
	   printf("--------------------------------------------------------------------------\n");
	   printf("-------------message recovered: size = %d, etherType = 0x%x---------------\n",
				    fec_decoder_ret,recoveredEtherType);
	   printf("--------------------------------------------------------------------------");
	   for(i=0;i<fec_decoder_ret;i++)
	   {
	     if(i%16 == 0) printf("\n");
	     printf("0x%2x|",recoveredMessage[i]);
	   }
	   printf("\n");
	}
	printf("------------------------------------------------------------------------\n\n\n");
	if(cnt % 40 == 0)
	  fec_decoder_report();
      }
   }
  return 1;
}
