/*
 * Copyright (c) 2004-2005 Endace Technology Ltd, Hamilton, New Zealand.
 * All rights reserved.
 *
 * This source code is proprietary to Endace Technology Limited and no part
 * of it may be redistributed, published or disclosed except as outlined in
 * the written contract supplied with this product.
 *
 * $Id: toerf.c 12667 2010-03-30 03:44:22Z jomi.gregory $
 */
 
/* Project headers. */
#include "dagrandom.h"
#include "daggen.h"
#include "dagcrc.h"

#include "list.h"
#include "packet.h"
#include "cmd.h"
#include "toerf.h"
#include "erf.h"
#include "dagapi.h"
#include "dagcrc.h"

/* C Standard Library headers. */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

/* POSIX headers. */
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#if defined(__FreeBSD__) || defined(__linux__) || (defined(__SVR4) && defined(__sun)) || (defined(__APPLE__) && defined(__ppc__))

#include <inttypes.h>
#include <netinet/in.h>
#include <unistd.h>

#elif defined(_WIN32)

#include <wintypedefs.h>
#include <winsock2.h>
#include <io.h>
#endif /* Platform-specific code. */

#include <assert.h>
#define LRH_HDR_LEN 8
#define GRH_HDR_LEN 40
#define BTH_HDR_LEN 12
#define IB_VCRC_LEN 2
#define IB_ICRC_LEN 4
#define IB_EXT_RDETH_LEN  4
#define IB_EXT_DETH_LEN  8

typedef struct {
	unsigned int magic_number;
	unsigned short int major;
	unsigned short int minor;
	unsigned int time_zone_offset;
	unsigned int time_stamp_accuracy;
	unsigned int snapshot_length;
	unsigned int link_layer_type;
} pcap_file_header_t;


static void execCmd (cmd_t * cmd);
static void execCmdList (list_t * list);
static void execSend (cmd_t * cmd);
static void execLoop (cmd_t * cmd);
static void getErf (packet_t * packet);
static erf_t * getErfEth (packet_eth_t * packet, unsigned char type);
static erf_t * getErfPos (packet_pos_t * packet);
static erf_t * getErfAtm (packet_atm_t * packet);
static erf_t * getErfAal5 (packet_aal5_t * packet);
static erf_t *getErfIB(packet_ib_t *packet);

static packet_t * getPacket (char * packet);
static erf_t * erf_snap (erf_t * erf, int snap);
static void erf_write (erf_t * erf);
static void openFile ();
static void pcap_write (erf_t * erf);
static void pcap_write_file_header(void);
static void getLinkType(list_t *);
static unsigned char *makePayload (list_t * list, unsigned short int *length, int);
static void fillPloadStr (unsigned char *, unsigned int *, char *, unsigned int);
static void fillPloadCounter (unsigned char *, unsigned int *, counter_t *, unsigned int);
static void fillPloadDummy (unsigned char *, unsigned int *, unsigned int);
static unsigned char getHexa (char c);
static list_t * getGroupCmdList (char * id);
static void macGetVar (list_t * input_mac, unsigned char * output_mac);


extern list_t * packet_list;
extern list_t * group_list;
list_t * erf_list;
extern options_t daggen_options;
int fd;
long long ts = 0;	// timestamps
/* opcode tells  presense of deth in the packet.  Here if the array value  is -1 no deth, or else it stores the offset frm the start of extension headers*/
static int8_t opcode_to_deth_offset[256];

static void init_deth_offset()
{
    uint8_t i;
	memset(opcode_to_deth_offset, -1,256);

    /* Fill DETH offsets for RD */
    for(i = 0x40; i < 0x4d;i++)
    {
        opcode_to_deth_offset[i] = 4;
    }

    for(i = 0x53; i < 0x56 ;i++)
    {
        opcode_to_deth_offset[i] = 4;
    }
    
    opcode_to_deth_offset[0x64] = 0;
    opcode_to_deth_offset[0x65] = 0;
}


void toerf () {
	list_t * cmd_list;

	// Init ERF list
	erf_list = listNew (erf_list);

	// get command from traffic group
	cmd_list = getGroupCmdList (daggen_options.traffic_group);

	getLinkType(cmd_list);
    /* for infiniband */
    init_deth_offset();
	// ATM and AAL5 had not been implemented in PCAP as link types
	if (daggen_options.output_format == OUTPUT_FORMAT_PCAP  &&
		daggen_options.link_type != LINK_ATM      /*    &&
		daggen_options.link_type != LINK_AAL5 */) {

		// Open output file
		openFile();
		pcap_write_file_header();
	} else if (daggen_options.output_format == OUTPUT_FORMAT_ERF) {
		// Open output file
		openFile();
	} else if (daggen_options.verbose) {
		printf ("PCAP file format not supported for this link type.\n");
	}

	// Exec commands in this traffic group
	execCmdList (cmd_list);
}

// Exec commands in traffic group
list_t * getGroupCmdList (char * id) {
	char found = 0;
	group_t * aux = NULL;
	list_t * list = NULL;

	// look for this traffic group
	listPtrBegin(group_list);
	while (listHasMoreElements(group_list) && !found) {
		aux = (group_t *) listGetPtrElem(group_list);
		found = !strcmp(aux->id, id);
		listPtrNext(group_list);
	}

	if (found) {
		// Exec commands in traffic group
		list = aux->commands;
	} else {
		if (!strcmp(id, "")) {
			printf("Error: no traffic group selected.\n");
			exit(0);
		} else {
			printf("Error: traffic group '%s' not declared.\n", id);
			exit(0);
		}
	}

	return list;
}

void execCmdList (list_t * list) {
	cmd_t *cmd;

	if (list == NULL) return;

	// Run over command list
	listPtrBegin(list);
	while (listHasMoreElements(list)) {
		cmd = (cmd_t *)listGetPtrElem(list);
		execCmd (cmd);
		listPtrNext(list);
	}

}

void execCmd (cmd_t * cmd) {
	switch (cmd->type) {
		case CMDSEND:
			execSend (cmd);
			break;
		case CMDLOOP:
			execLoop (cmd);
			break;
		default:
			// do nothing, should not ever reach this case
			break;
	}
}

void execSend (cmd_t * cmd) {
	unsigned long l;
	erf_t * erf;
	packet_t * p;
	char * packet;
	unsigned long length;
		

	//printf("[execSend] cmd->ptr: \"%s\"\n", (char *) cmd->ptr);
	packet = (char *) cmd->ptr;
	p = getPacket (packet);

	length = getDistrNumber (cmd->distr);
	for (l=0; l<length; l++) {
		getErf (p); // needed here to update counters, distributions... for each packet

		listPtrBegin(erf_list);
		while (listHasMoreElements(erf_list)) {
			erf = (erf_t *) listGetPtrElem(erf_list);
			switch (daggen_options.output_format) {
			case OUTPUT_FORMAT_ERF:
				if (cmd->snap != NULL)
				erf = erf_snap(erf, getDistrNumber(cmd->snap));
				erf_write (erf);
				break;
			case OUTPUT_FORMAT_PCAP:
				pcap_write (erf);
				break;
			}

			free(erf);
			listRemovePtr(erf_list);
		}
	}

}

void execLoop (cmd_t * cmd) {
	unsigned long length;
	unsigned long l;
	list_t * list;

	length = getDistrNumber (cmd->distr);
	list = (list_t *) cmd->ptr;

	for (l=0; l<length; l++)
		execCmdList (list);
}

void getErf (packet_t * packet) {
	erf_t * erf;

	erf = NULL;
	switch (packet->type) {
		case PKT_ETH802_3:
		case PKT_ETH_II:
			erf = getErfEth((packet_eth_t *)packet->p, packet->type);
			listAdd(erf_list, erf, 0);
			break;
		case PKT_POS:
			erf = getErfPos((packet_pos_t *)packet->p);
			listAdd(erf_list, erf, 0);
			break;
		case PKT_ATM:
			erf = getErfAtm((packet_atm_t *)packet->p);
			listAdd(erf_list, erf, 0);
			break;
		case PKT_AAL5:
			erf = getErfAal5((packet_aal5_t *)packet->p);
			listAdd(erf_list, erf, 0);
			// put here fragmentation function (SAR)
			break;
		case PKT_IB:
			erf = getErfIB((packet_ib_t*)packet->p);
			listAdd(erf_list, erf, 0);
			break;
	}

}

//
// Packet types
//

// Contruct an ERF from the info in config file (packet)
// It handles both Eth802_3 and Eth_II types 
erf_t * getErfEth (packet_eth_t * packet, unsigned char type) {
	int total_length;
	erf_eth_t * erf;
	unsigned char * aux;
	unsigned short int p_length;	// packet length
	unsigned short int np_length;	// network order packet length (Eth 802.3)
	unsigned short int n_proto;	// network order protocol (Eth II)
	unsigned char aux_mac[6];
	unsigned char *payload;
	int flag = 0;
	int fcs_size;
	int rx_error;
	unsigned long  crc32;
	unsigned short crc16;

	crc16 = crc16_init();

	if (daggen_options.fcs_size == -1) {
		fcs_size = packet->fcs_size;
    } else {
		fcs_size = daggen_options.fcs_size;
    }

	// if wrong 4-byte CRC wanted
	if (fcs_size == 6) {
		fcs_size = 4;
		flag = 1;
    }

	payload = makePayload (packet->payload, &p_length, fcs_size - 4);

	// check packet length integrity
	if (p_length > 9582) { p_length = 9582; }
	if (p_length < 46) {
		aux = (unsigned char *) malloc (46);
		memset(aux, 0, 46);
		if (payload != NULL) {
			memcpy (aux, payload, p_length);
			free(payload);
		}
		payload = aux;
		p_length = 46;
	}

	total_length = p_length + ERF_ETH_LENGTH + HEADER_ETH_LENGTH + fcs_size;
	//printf("[ERF] total_length: %d\n", total_length);

	// For alignment purposes
	if (total_length % 8 != 0) {
		total_length += 8 - (total_length % 8);
    }
	erf = (erf_eth_t *) malloc (total_length);

	memset(erf, 0, total_length);		// zero memory
	ts += HEADER_ETH_LENGTH + p_length + fcs_size;
	
	if (daggen_options.rx_error == -1) {
		rx_error = packet->rx_error;
    } else {
		rx_error = daggen_options.rx_error;
    }

	erf->timestamp = ts;			// timestamp
	erf->type = 2; 				// ethernet
	erf->flags = daggen_options.interface_num & 0x03;	// interface
	erf->flags |= rx_error<<4 & 0x10;	// rx_error
	erf->rlen = htons(total_length); 	// record length
	erf->lctr = htons(0);			// loss counter (not needed)

	erf->wlen = htons(HEADER_ETH_LENGTH + p_length + fcs_size);	// wire length
	erf->offset = 0x00;			// number of bytes not captured from start
	erf->pad = 0x00;			// calculate! if(payload<46) pad=46-payload;

	aux = (unsigned char *) erf; // important to use aux, otherwise it turns crazy

	if (packet->dst_addr_type == MAC_STATIC) {
		macGet (packet->dst_addr, aux_mac);
    } else {
		macGetVar (packet->dst_addr_var, aux_mac);
    }
	memcpy (aux + ERF_ETH_LENGTH, aux_mac, 6);

	if (packet->src_addr_type == MAC_STATIC) {
		macGet (packet->src_addr, aux_mac);
    } else {
		macGetVar (packet->src_addr_var, aux_mac);
    }
	memcpy (aux + ERF_ETH_LENGTH + 6, aux_mac, 6);

    switch (type) {
    case PKT_ETH802_3:
        np_length = htons (p_length);
        memcpy (aux + ERF_ETH_LENGTH + 12, &np_length, 2);
        break;
    case PKT_ETH_II:
        n_proto = htons (((packet_eth_II_t*)packet)->protocol);
        memcpy (aux + ERF_ETH_LENGTH + 12, &n_proto, 2);
        break;
    default:
        assert(0); // should never get here
    }

	if (payload == NULL)
		memset (aux + ERF_ETH_LENGTH + HEADER_ETH_LENGTH, 0xAA, p_length);
	else {
		memcpy (aux + ERF_ETH_LENGTH + HEADER_ETH_LENGTH, payload, p_length);
		free(payload);
	}

	// Calculate FCS (Frame Check Sequence) == CRC (Cyclic Redundancy Check)
	switch (fcs_size) {
		case 0:
			// No CRC, we are finished :)
			break;
		case 2:
			// 2-byte CRC (16 bits)
			crc16 = crc16_update (crc16, aux + ERF_ETH_LENGTH, HEADER_ETH_LENGTH + p_length);
			memcpy (aux + ERF_ETH_LENGTH + HEADER_ETH_LENGTH + p_length, &crc16, 2);
			break;
		case 4:
			// if wrong 4-byte CRC wanted
			crc32 = crc32_calc (aux + ERF_ETH_LENGTH, HEADER_ETH_LENGTH + p_length);
			if (flag == 1) {
				/* Wrong CRC forced*/
				crc32 -= 1;
			} 
			memcpy (aux + ERF_ETH_LENGTH + HEADER_ETH_LENGTH + p_length, &crc32, 4);
			break;
	}

	return (erf_t *) erf;
}

// Contruct an ERF from the info in config file (packet)
erf_t * getErfPos (packet_pos_t * packet) {
	erf_pos_t * erf;
	unsigned char * aux;
	unsigned short int p_length;	// packet length
	int total_length;
	int header_length;
	unsigned long  crc32;
	unsigned short crc16;
	int flag = 0;
	int fcs_size;
	int rx_error;
	unsigned char *payload;

	crc16 = crc16_init();

	header_length = 2 + packet->protocol_size;

	// Get FCS size
	if (daggen_options.fcs_size == -1)
	{
		fcs_size = packet->fcs_size;
	}
	else
	{
		fcs_size = daggen_options.fcs_size;
	}
	// if wrong 4-byte CRC wanted
	if (fcs_size == 6)
        {
		fcs_size = 4;
		flag = 1;
        }
	//p_length = (unsigned short int) getDistrNumber (packet->payload);
	p_length = 0;
	payload = makePayload (packet->payload, &p_length, header_length);

	// check packet length integrity
	if (p_length > 65511) p_length = 65511;
	/* if (p_length < 64) {
		aux = (unsigned char *) malloc (64);
		bzero (aux, 64);
		if (payload != NULL) {
			memcpy (aux, payload, p_length);
			free(payload);
		}
		payload = aux;
		p_length = 64;
	} */

	total_length = p_length + ERF_LENGTH + header_length + fcs_size;

	// For alignment purposes
	if (total_length % 8 != 0)
		total_length += 8 - (total_length % 8);

	erf = (erf_pos_t *) malloc (total_length);

	memset(erf, 0, total_length);		// zero memory
	ts += p_length + header_length + fcs_size;
		
	if (daggen_options.rx_error == -1)
		rx_error = packet->rx_error;
	else
		rx_error = daggen_options.rx_error;
	
	erf->timestamp = ts;			// timestamp
	erf->type = 1; 				// pos
	erf->flags = daggen_options.interface_num & 0x03;	// interface
	erf->flags |= rx_error<<4 & 0x10;	// rx_error
	erf->rlen = htons(total_length); 	// record length
	erf->lctr = htons(0);			// loss counter (not needed)
	erf->wlen = htons(p_length + header_length + fcs_size);	// wire length

	// HDLC Header (4 bytes)
	erf->address = packet->address;
	erf->control = packet->control;

	// BUG: erf->protocol is unsigned short int (2 bytes)
	if (packet->protocol_size == 1)
		erf->protocol = (unsigned short)((unsigned char) packet->protocol);
	else // size == 2
		erf->protocol = (unsigned short int) htons(packet->protocol);

	aux = (unsigned char *) erf; // important to use aux, otherwise it turns crazy
	if (payload == NULL)
		memset (aux + ERF_LENGTH + header_length, 0xBB, p_length);
	else {
		memcpy (aux + ERF_LENGTH + header_length, payload, p_length);
		free (payload);
	}
	//memset (erf->payload, 0xBB, p_length);

	// Calculate FCS (Frame Check Sequence) == CRC (Cyclic Redundancy Check)
	switch (fcs_size) {
		case 0:
			// No CRC, we are finished :)
			break;
		case 2:
			// 2-byte CRC (16 bits)
			crc16 = crc16_update (crc16, aux + ERF_LENGTH, header_length + p_length);
			memcpy (aux + ERF_LENGTH + header_length + p_length, &crc16, 2);
			break;
		case 4:
			// 4-byte CRC (32 bits)
			crc32 = crc32_calc (aux + ERF_LENGTH, p_length + header_length);
	
			// if 4-byte wrong CRC wanted
			if(flag == 1)
				crc32 -= 1;
			
			memcpy (aux + ERF_LENGTH + header_length + p_length, &crc32, 4);
			break;
	}

	return (erf_t *) erf;
}

// Contruct an ERF from the info in config file (packet)
erf_t * getErfAtm (packet_atm_t * packet) {
	int total_length;
	erf_t * erf;
	unsigned char * aux;
	unsigned short int p_length;	// packet length
	unsigned char *payload;
	unsigned int atm_header=0;
	unsigned char aux_char=0;
	unsigned short int aux_short=0;
	//unsigned int aux_int=0;

	//p_length = (unsigned short int) getDistrNumber (packet->length);
	p_length = 0;
//check me fixme depending on the assumptions for the make payload the 
//the ATM does not have CRC(FCS)
	payload = makePayload (packet->payload, &p_length, 4);

	// check packet length integrity

	if (p_length != 48) {
		aux = (unsigned char *) malloc (48);
		memset (aux, 0, 48);
		if (p_length > 48) p_length = 48;
		if (payload != NULL) {
			memcpy (aux, payload, p_length);
			free(payload);
		}
		payload = aux;
		p_length = 48;
	}

	total_length = p_length + ERF_LENGTH + 4;

	// For alignment purposes
	if (total_length % 8 != 0)
		total_length += 8 - (total_length % 8);

	// ERF Header
	erf = (erf_t *) malloc (total_length);
	memset (erf, 0, total_length);		// zero memory
	ts += p_length + 4;
	erf->timestamp = ts;			// timestamp
	erf->type = 3; 				// ATM
	erf->flags = daggen_options.interface_num & 0x03;	// interface
	erf->rlen = htons(total_length); 	// record length
	erf->lctr = htons(0);			// loss counter (not needed)
	erf->wlen = htons(p_length + 4);	// wire length

	aux = (unsigned char *) erf; // important to use aux, otherwise it turns crazy
	memcpy (aux, erf, ERF_LENGTH);

	// ATM Header
	if (packet->type == ATM_TYPE_UNI) {
		aux_char = (unsigned char) getDistrNumber(packet->gfc);
		atm_header |= ((unsigned int)aux_char & 0x0f) << 28;
		aux_short = getDistrNumber(packet->vpi);
		atm_header |= ((unsigned int)aux_short & 0x0ff) << 20;
	} else {
		aux_short = getDistrNumber(packet->vpi);
		atm_header |= ((unsigned int)aux_short & 0x0fff) << 20;
	}
	aux_short = getDistrNumber(packet->vci);
	atm_header |= ((unsigned int)aux_short & 0x0ffff) << 4;
	aux_char = getDistrNumber(packet->pt);
	atm_header |= ((unsigned int)aux_char & 0x07) << 1;
	aux_char = getDistrNumber(packet->clp);
	atm_header |= ((unsigned int)aux_char & 0x01);

	atm_header = htonl (atm_header);
	memcpy (aux + ERF_LENGTH, &atm_header, 4);

	// Payload
	memcpy (aux + ERF_LENGTH + 4, payload, p_length);
	free(payload);

	return (erf_t *) erf;
}

// Contruct an ERF from the info in config file (packet)
erf_t * getErfAal5 (packet_aal5_t * packet) {
	int total_length;
	erf_t * erf;
	unsigned char * aux;
	unsigned short int p_length;	// packet length
	unsigned char *payload;
	unsigned int atm_header=0;
	unsigned char aux_char=0;
	unsigned short int aux_short=0;
	unsigned int aux_int = 0;
	trailer_aal5_t t_aal5;
	int pad = 0;

	//p_length = (unsigned short int) getDistrNumber (packet->length);
	p_length = 0;
	payload = makePayload (packet->payload, &p_length, sizeof(t_aal5)-2);
	if (p_length > 65464) p_length = 65464;

	// should adjust padding
	pad = 48 - (((int)p_length + 8) % 48);
	if (pad == 48) pad = 0;

	total_length = p_length + ERF_LENGTH + 4 + pad + sizeof(t_aal5);

	// For alignment purposes
	if (total_length % 8 != 0)
		total_length += 8 - (total_length % 8);

	erf = (erf_t *) malloc (total_length);

	memset (erf, 0, total_length);		// zero memory
	ts += p_length + pad + sizeof(t_aal5);
	erf->timestamp = ts;			// timestamp
	erf->type = 4; 				// AAL5
	erf->flags = daggen_options.interface_num & 0x03;	// interface
	erf->rlen = htons((unsigned short int)total_length); 	// record length
	erf->lctr = (unsigned short)htons(0);			// loss counter (not needed)
	erf->wlen = htons(p_length + pad + (uint16_t)sizeof(t_aal5));	// wire length

	aux = (unsigned char *) erf; // important to use aux, otherwise it turns crazy
	memcpy (aux, erf, ERF_LENGTH);

	// ATM Header
	if (packet->type == ATM_TYPE_UNI) {
		aux_char = (unsigned char) getDistrNumber(packet->gfc);
		atm_header |= ((unsigned int)aux_char & 0x0f) << 28;
		aux_short = getDistrNumber(packet->vpi);
		atm_header |= ((unsigned int)aux_short & 0x0ff) << 20;
	} else {
		aux_short = getDistrNumber(packet->vpi);
		atm_header |= ((unsigned int)aux_short & 0x0fff) << 20;
	}
	aux_short = getDistrNumber(packet->vci);
	atm_header |= ((unsigned int)aux_short & 0x0ffff) << 4;
	aux_char = getDistrNumber(packet->pt);
	atm_header |= ((unsigned int)aux_char & 0x07) << 1;
	aux_char = getDistrNumber(packet->clp);
	atm_header |= ((unsigned int)aux_char & 0x01);

	atm_header = htonl (atm_header);
	memcpy (aux + ERF_LENGTH, &atm_header, 4);

	// Payload
	memcpy (aux + ERF_LENGTH + 4, payload, p_length);
	free(payload);

	// Add padding
	memset (aux + ERF_LENGTH + 4 + p_length, 0, pad);

	t_aal5.uu = (unsigned char) getDistrNumber (packet->uu);
	t_aal5.cpi = (unsigned char) getDistrNumber (packet->cpi);
	t_aal5.length = (unsigned short int) htons(p_length);

	// CRC
	aux_int = (unsigned int) getDistrNumber (packet->crc);
	if (aux_int == 0) {
		// automatic CRC
		t_aal5.crc = crc32_calc (aux + ERF_LENGTH + 4, p_length + pad + 4);
		t_aal5.crc = htonl (t_aal5.crc);
	} else {
		// manual CRC
		t_aal5.crc = htonl (aux_int);
	}

	memcpy (aux + ERF_LENGTH + 4 + p_length + pad, &t_aal5, sizeof(t_aal5));

	return (erf_t *) erf;
}

//
// Common stuff
//

// Get packet from packet list
packet_t * getPacket (char * packet) {
	int found = 0;
	packet_t * aux;

	aux = NULL;
	listPtrBegin(packet_list);
	while (listHasMoreElements(packet_list) && !found) {
		aux = (packet_t *)listGetPtrElem(packet_list);
		//printf ("[getPacket] loop, packet: '%s', aux->id: '%s', strcmp: %d\n", packet, aux->id, strncmp (packet, aux->id, strlen(packet)));
		//found = !strncmp (packet, aux->id, strlen(packet));
		found = !strcmp (packet, aux->id);
		listPtrNext(packet_list);
	}
	//printf ("\n");

	if (found)
		return aux;
	else
		return NULL;	// should never arrive this case
}

// Snap (trunk) an ERF record
erf_t * erf_snap (erf_t * erf, int snap) {
	int headerlength;
	int alignment;

	// Choose ERF header size
	headerlength = ERF_LENGTH;
	if (daggen_options.link_type == LINK_ETHERNET)
		headerlength = ERF_ETH_LENGTH;

	// Adjust snaplength
	if (snap > ntohs(erf->rlen) - headerlength)
		snap = ntohs(erf->rlen) - headerlength;

	// Keep 64-bit alignment
	alignment = (snap + headerlength) % 8;
	snap -= alignment;	// round down

	// Modify record length
	erf->rlen = htons(snap + headerlength);

	return erf;
}

// Write an ERF to disk
void erf_write (erf_t * erf) {
	unsigned int snaplength;
	unsigned int n_bytes;
	unsigned int headerlength;
	int alignment;

	// Choose ERF header size
	headerlength = ERF_LENGTH;
	if (daggen_options.link_type == LINK_ETHERNET)
		headerlength = ERF_ETH_LENGTH;

	// Adjust snaplength
	snaplength = daggen_options.snaplength;
	if (snaplength > ntohs(erf->rlen) - headerlength)
		snaplength = ntohs(erf->rlen) - headerlength;

	// Keep 64-bit alignment
	alignment = (snaplength + headerlength) % 8;
	if (daggen_options.snaplength_round == SNAP_ROUND_DOWN)
		snaplength -= alignment;	// round down
	else if (alignment != 0)
		snaplength += (8 - alignment);	// round up

	// Number of bytes to write
	n_bytes = headerlength + snaplength;

	// Update RLEN
	erf->rlen = htons(n_bytes);

	// Write ERF record

	if(write (fd, erf, n_bytes)!= n_bytes)
	{
		printf("write failed\n");
		exit(1);
	}
}

// Open a file for writing
void openFile () {
#ifndef _WIN32
	fd = open (daggen_options.output_file, O_CREAT | O_TRUNC | O_RDWR , 0644);
#else /* _WIN32 */
	fd = open (daggen_options.output_file, _O_CREAT | _O_TRUNC | _O_RDWR |_O_BINARY , _S_IWRITE);
#endif /* _WIN32 */
	
}
static int stemp;	
	
void pcap_write_file_header() {
	pcap_file_header_t h;

	h.magic_number = 0xA1B2C3D4;
	h.major = 2;
	h.minor = 4;
	h.time_zone_offset = 0;
	h.time_stamp_accuracy = 0;
	h.snapshot_length = 0xffff;	// all packet captured
	h.link_layer_type = daggen_options.link_type;

	stemp = write (fd, (unsigned char *) &h, sizeof(h));

}

void pcap_write (erf_t * erf) {
	unsigned int aux;
	unsigned int length;
	//unsigned char aux_char = 0;
	//unsigned short aux_short = 0;
	unsigned char * aux_erf;
	unsigned long long ts_aux;

	aux_erf = (unsigned char *) erf;

	if ((daggen_options.link_type == LINK_ETHERNET /*&& erf->type == 2*/) ||
	    (daggen_options.link_type == LINK_CHDLC    /*&& erf->type == 1*/) ||
	    (daggen_options.link_type == LINK_AAL5     /*&& erf->type == 4*/)) {

		// Timestamp
		memcpy(&ts_aux, (unsigned char *) erf, 8);
		ts_aux &= 0x0ffffffff;
		aux = (unsigned int) ts_aux * 1000000 / 0xffffffff;
		stemp = write (fd, (unsigned char *) &aux, 4);
		stemp = write (fd, (unsigned char *) erf, 4);
		//write (fd, (unsigned char *) erf, 8);	// time stamp

		aux = ntohs(erf->wlen);
		stemp = write (fd, (unsigned char *) &aux, 4);
		aux = ntohs(erf->wlen);
		stemp = write (fd, (unsigned char *) &aux, 4);
		length = aux;

		// offset for payload
		switch (erf->type) {
		case 1:	aux = 16;
			break;
		case 2: aux = 18;
			break;
		case 3: /* not handled in pcap
			aux = 20;
			length = 48;
			aux_char = 0;
			stemp = write (fd, &aux_char, 1);
			aux_char = (aux_erf[16] << 4) | ((aux_erf[17] >> 4) & 0x0f);
			stemp = write (fd, &aux_char, 1);
			aux_short = (aux_erf[17] << 12) | (aux_erf[18] << 4) |
				    ((aux_erf[19] >> 4) & 0x0f);
			aux_short = htons(aux_short);
			stemp = write (fd, &aux_short, 2); */
			break;
		case 4: aux = 20;
			break;
		default: aux = 16;
			break;
		}

		stemp = write (fd, (unsigned char *) erf + aux, length);
	}
}

void getLinkType (list_t * cmd_list) {
	char found = 0;
	cmd_t * aux = NULL;
	char * id;
	packet_t * p;

	listPtrBegin(cmd_list);
	while (listHasMoreElements(cmd_list) && !found) {
		aux = (cmd_t *)listGetPtrElem(cmd_list);
		if (aux->type == CMDSEND)
			found = 1;
		else
			listPtrNext(cmd_list);
	}

	if (found) {
		id = (char *) aux->ptr;
		p = getPacket (id);

		switch (p->type) {
		case PKT_ETH802_3:
		case PKT_ETH_II:
			daggen_options.link_type = LINK_ETHERNET;
			break;
		case PKT_POS:
			daggen_options.link_type = LINK_CHDLC;
			break;
		case PKT_ATM:
			daggen_options.link_type = LINK_ATM;
			break;
		case PKT_AAL5:
			daggen_options.link_type = LINK_AAL5;
			break;
		}
	}
}

unsigned char *makePayload (list_t * list, unsigned short int *length, int hlen) {
	payload_t * aux;		// Aux payload pointer
	counter_t * counter_aux;	// Aux counter pointer
	distribution_t * distr_aux;	// Aux distribution pointer
	unsigned int val;		// Aux int value
	unsigned char * payload;	// Payload buffer
	unsigned int offset;		// Offset inside payload buffer
	list_t * distr_list;		// List of distribution numbers
	unsigned int * val_aux;		// Pointer to distribution number
	unsigned int tmp_length;	// Temporal length
	int i;				// variable for 'for' loop

	// If there is no payload defined then return
	if (list == NULL) return NULL;

	// Init distribution numbers list
	distr_list = NULL;	// so gcc doesn't complains
	distr_list = listNew (distr_list);

	// First must count how many space do we need to store the payload
	*length = 0;
	tmp_length = 0;

	listPtrBegin(list);
	while (listHasMoreElements(list)) {
		aux = (payload_t *) listGetPtrElem(list);

		switch (aux->type) {
		case PLOAD_HEXASTRING:
			val = (unsigned int)strlen((char *)aux->contents);
			tmp_length += (val >> 1) + (val % 2);
			if (tmp_length > 65535) tmp_length = 65535;
			break;
		case PLOAD_COUNTER:
			counter_aux = (counter_t *) aux->contents;
			val = 1;
			if (aux->distr != NULL)
				val = getDistrNumber (aux->distr);

			if (counter_aux != NULL) {
				tmp_length += (unsigned short int) counter_aux->size * val;
				if (tmp_length > 65535) tmp_length = 65535;
			}

			// Store distribution number in list
			val_aux = (unsigned int *) malloc (sizeof(unsigned int));
			*val_aux = val;
			listAdd (distr_list, val_aux, 0);
			break;
		case PLOAD_COUNTER_MASK:
			counter_aux = (counter_t *) aux->contents;
			val = getDistrNumber (aux->distr);

			if (!(val <= counter_aux->size && val >= 0))
				val = counter_aux->size;

			tmp_length += val;
			if (tmp_length > 65535) tmp_length = 65535;

			// Store distribution number in list
			val_aux = (unsigned int *) malloc (sizeof(unsigned int));
			*val_aux = val;
			listAdd (distr_list, val_aux, 0);
			break;
		case PLOAD_DUMMY:
			distr_aux = (distribution_t *) aux->contents;
			val = getDistrNumber (distr_aux);

			// Check number limits
			if (val > 65535) val = 65535;
			if (val + tmp_length > 65535) val = 65535 - tmp_length;

			// Store distribution number in list
			val_aux = (unsigned int *) malloc (sizeof(unsigned int));
			*val_aux = val;
			listAdd (distr_list, val_aux, 0);
			tmp_length += val;
			break;
		case PLOAD_WLEN:
			tmp_length += 4;
			if (tmp_length > 65535) tmp_length = 65535;
			break;
		}

		listPtrNext(list);
	}

	// Check again
	if (tmp_length == 0) return NULL;
	if (tmp_length > 65535) tmp_length = 65535;
	*length = (unsigned short int) tmp_length;
	//if (*length > 0xffff - 85) *length = 0xffff - 85;

	payload = (unsigned char *) malloc (*length);
	memset (payload, 0, *length);

	// This time for real. Fill payload buffer
	offset = 0;
	listPtrBegin(list);
	listPtrBegin(distr_list);
	while (listHasMoreElements(list)) {
		aux = (payload_t *) listGetPtrElem(list);

		switch (aux->type) {
		case PLOAD_HEXASTRING:
			val = (unsigned int)strlen((char *)aux->contents);
			val = (val >> 1) + (val % 2);
			if (val + offset >= *length) val = *length - offset;
			fillPloadStr (payload, &offset, aux->contents, val);
			break;
		case PLOAD_COUNTER:
			counter_aux = (counter_t *) aux->contents;
			val_aux = (unsigned int *) listGetPtrElem(distr_list);
			if (counter_aux != NULL) {
				val = (unsigned short int) counter_aux->size;
				for (i=0; i<(signed)*val_aux; i++) {
					if (val + offset >= *length) val = *length - offset;
					fillPloadCounter (payload, &offset, counter_aux, val);
				}
			}
			free(val_aux);
			listPtrNext(distr_list);
			break;
		case PLOAD_COUNTER_MASK:
			counter_aux = (counter_t *) aux->contents;
			val_aux = (unsigned int *) listGetPtrElem(distr_list);
			if (counter_aux != NULL) {
				if (*val_aux + offset >= *length) *val_aux = *length - offset;
				fillPloadCounter (payload, &offset, counter_aux, *val_aux);
			}
			free(val_aux);
			listPtrNext(distr_list);
			break;
		case PLOAD_DUMMY:
			val_aux = (unsigned int *) listGetPtrElem(distr_list);
			if (*val_aux + offset >= *length) *val_aux = *length - offset;
			fillPloadDummy (payload, &offset, *val_aux);
			free(val_aux);
			listPtrNext(distr_list);
			break;
		case PLOAD_WLEN:
			val = htonl((*length) + hlen );		
			//val = htonl(*length);
			memcpy (&payload[offset], &val, 4);
			offset += 4;
			break;

		}

		listPtrNext(list);
	}

	// should do: listFree(distr_list);

	return payload;
}

void fillPloadStr (unsigned char * payload, unsigned int *offset,
		   char * str, unsigned int val) {

	int i;

	for (i=0; i<(int)strlen(str); i++)
		payload[*offset + (i >> 1)] |= (getHexa(str[i]) << (4 - (4 * (i%2))));

	*offset += val;
}

void fillPloadCounter (unsigned char * payload, unsigned int *offset,
		  counter_t * counter, unsigned int val) {
	int i;

	unsigned char      aux1;
	unsigned short int aux2;
	unsigned int aux4;

	i = getDistrNumber(counter->distr);
	switch(counter->size) {
	case 1:	aux1 = (unsigned char) i;
		if (val == 1) payload[*offset]=aux1;
		break;

	case 2: aux2 = (unsigned short int) i;
		aux2 = htons (aux2);
		memcpy (&payload[*offset], &aux2, val);
		break;

	case 4: aux4 = (unsigned int) i;
		aux4 = htonl (aux4);
		memcpy (&payload[*offset], &aux4, val);
		break;
	}

	*offset += val;
}

void fillPloadDummy (unsigned char * payload, unsigned int *offset,
		     unsigned int val) {
	unsigned char aux;
	int i;

	for (i=0; i<(int)val; i++) {
#ifndef _WIN32
		aux = (unsigned char) random();
#else /* _WIN32 */
		aux = (unsigned char) rand();
#endif /* _WIN32 */
		payload[*offset + i] = aux;
	}

	*offset += val;
}

unsigned char getHexa (char c) {
	unsigned char res;

	res = 0;
	switch(c) {
	case '0':	res = 0;
			break;
	case '1':	res = 1;
			break;
	case '2':	res = 2;
			break;
	case '3':	res = 3;
			break;
	case '4':	res = 4;
			break;
	case '5':	res = 5;
			break;
	case '6':	res = 6;
			break;
	case '7':	res = 7;
			break;
	case '8':	res = 8;
			break;
	case '9':	res = 9;
			break;
	case 'A':
	case 'a':	res = 10;
			break;
	case 'B':
	case 'b':	res = 11;
			break;
	case 'C':
	case 'c':	res = 12;
			break;
	case 'D':
	case 'd':	res = 13;
			break;
	case 'E':
	case 'e':	res = 14;
			break;
	case 'F':
	case 'f':	res = 15;
			break;
	}

	return res;
}

//
// Mac stuff (you can find more in packet.c)
//
void macGetVar (list_t * input_mac, unsigned char * output_mac) {
	unsigned char * aux_mac;
	unsigned short int length;

	// I know it is not a payload, but the function is exactly the same
	// in daggen2 this name should change
	aux_mac = makePayload (input_mac, &length, 0);

	memset (output_mac, 0, 6);
	if (length > 6) length = 6;
	memcpy (output_mac, aux_mac, length);
	free (aux_mac);
}




erf_t * getErfIB (packet_ib_t * packet)
{
    int total_length;
    erf_t * erf;
    unsigned char * aux;
    unsigned short int p_length = 0;    // packet length
    unsigned short int pload_length = 0;    // payload length
    unsigned char *payload;
    int header_length = 0;
    ib_lrh_t     *this_lrh = NULL;
    ib_bth_t    *this_bth = NULL;
    ib_grh_t    *this_grh = NULL;
    ib_lrh_t *masked_lrh  = NULL;
    ib_grh_t *masked_grh = NULL;
    ib_bth_t *masked_bth = NULL;
    uint8_t *masked_ib_hdrs = NULL;
    uint32_t masked_len = 0;
    uint32_t    icrc = 0xffffffff;
    uint16_t    vcrc = 0xffff;
    ib_ext_deth_t   *this_deth = NULL;
    //ib_ext_rdeth_t   *this_rdeth = NULL;
    static int sequence_counter = 0;
    uint8_t fill_src_qp = 0;
    uint32_t src_qp_value = 0;
    uint8_t opcode_value = 0;
   
    /* use local variables for src_qp and opcode as they are accessed in multiple places
     *  Previous direct usage of getDistrNumber() was giving unexpected values if  values are specified 
     * as distributions in daggen input file */
    src_qp_value = getDistrNumber(packet->src_qp);
    opcode_value = (uint8_t) getDistrNumber(packet->bth_opcode);
    payload = makePayload (packet->payload, &pload_length, 0/*dont knw - unsused*/ );
    if (pload_length > 65464) pload_length = 65464;
    
    // assuming LRH + GRH ( if present ) + BTH +ext Hdres ( RDETH + DETH (if present ) ) 
    header_length = LRH_HDR_LEN ;
    if (packet->type == IB_TYPE_GLOBAL )
    {
        header_length += GRH_HDR_LEN;
    }
    if ( packet->type == IB_TYPE_LOCAL ||  packet->type == IB_TYPE_GLOBAL)
    {
        header_length += BTH_HDR_LEN;
    }
    /* source qp goes into DETH header.so RDETH(if needed ) and DETH headers will be added */
    if (src_qp_value)
    {
        /* increase the header_length by the lenth of ext headers ( RDETH and/or DETH ) */
        if (-1 != opcode_to_deth_offset[opcode_value] )//( (getDistrNumber( packet->bth_opcode) & 0xe0) == 0x40 )
        {
            /* depending on the opcode, the packet may have RDETH so use opcode_to_deth_offset to get the DETH's offset */
            header_length +=   IB_EXT_DETH_LEN +  opcode_to_deth_offset[opcode_value];
            fill_src_qp = 1;
        }
        
        #if 0
else if (-1 != opcode_to_deth_offset[packet->bth_opcode] ) //(( getDistrNumber( packet->bth_opcode) & 0xe0) == 0x60)
        {
            /* its a Unreliable datagaram so assume DETH only */
            header_length +=  IB_EXT_DETH_LEN;
        }
#endif 
        else
        {
            fprintf(stderr, "Warning - src-qp is not supposed to come with this opcode.No DETH for this packet. Omitting src-qp \n");

        }
    }
    //header_length += BTH_HDR_LEN;
    /* packet length is the length of the header + paylod + ICRC */
    /* IB packet length has to be 4 byte aligned - so adjust the length here */
    p_length = header_length + pload_length + IB_ICRC_LEN;
    if (p_length % 4 != 0)
         p_length += 4 - (p_length % 4);
    total_length = ERF_LENGTH + p_length + IB_VCRC_LEN;

    /* ERF has to be 8 byte aligned */
    if (total_length % 8 != 0)
        total_length += 8 - (total_length % 8);

    // ERF Header
    erf = (erf_t *) malloc (total_length);
    memset (erf, 0, total_length);        // zero memory
    ts += p_length + 4;
    erf->timestamp = ts;            // timestamp
    erf->type = 21;                 // ATM
    erf->flags = daggen_options.interface_num & 0x03;    // interface
    erf->rlen = htons(total_length);     // record length
    erf->lctr = htons(0);            // loss counter (not needed)
    erf->wlen = htons( p_length + IB_VCRC_LEN );    // wire lengthpacket->type == IB_TYPE_GLOBAL

    aux = (unsigned char *) erf; 
    memcpy (aux, erf, ERF_LENGTH);

    /* fill LRH */
    aux += ERF_LENGTH;
    this_lrh = (ib_lrh_t*) aux ;
    this_lrh->virtual_lane = getDistrNumber( packet->lrh_vl );
    this_lrh->link_version = getDistrNumber( packet->lrh_lver );
    this_lrh->service_level =  getDistrNumber( packet->lrh_slevel );
    this_lrh->src_local_id = htons ( (uint16_t)getDistrNumber( packet->lrh_slid ));
    this_lrh->dest_local_id = htons ( (uint16_t)getDistrNumber( packet->lrh_dlid ) );
    this_lrh->lnh = packet->type;//getDistrNumber(packet->lrh_lnh);
    this_lrh->packet_length = htons ( (uint16_t) (  p_length  / 4 )  & 0x07ff);

    if ( packet->type == IB_TYPE_GLOBAL )
    {
        /* fill GRH */
        aux = (uint8_t*) erf + ( ERF_LENGTH + LRH_HDR_LEN );
        this_grh = (ib_grh_t*) aux;
        this_grh->next_header = 0x1b; /* for BTH - to verify */
        this_grh->word0 = 0x6 << 28;
        /* flow label is set with the value of an counter */
        this_grh->word0 |= ( sequence_counter++ & 0x000fffff);
        this_grh->word0 = htonl( this_grh->word0);
    
    }
   
    if ( packet->type == IB_TYPE_GLOBAL || packet->type == IB_TYPE_LOCAL)
     {
        /* fill BTH */
        aux= (uint8_t*)erf +(ERF_LENGTH +  LRH_HDR_LEN + ( (packet->type == IB_TYPE_GLOBAL) ? GRH_HDR_LEN : 0 ) );
        this_bth = (ib_bth_t*)  aux ;
        this_bth->op_code = opcode_value;
        this_bth->dest_qp = htonl ( (getDistrNumber( packet->bth_qp) << 8) & 0xffffff00 );
        this_bth->packet_seq_number = htonl( (getDistrNumber(packet->bth_psn) << 8 ) & 0xffffff00);
        this_bth->t_header_version = getDistrNumber(packet->bth_tver);
    }
    /* if source qp is given fill it in DETH */
    if (fill_src_qp )
    {
        
        /*  commenting out temporary - trying to fill rdeth */
        /*
        aux = (uint8_t*) erf + ERF_LENGTH + header_length - IB_EXT_DETH_LEN -IB_EXT_RDETH_LEN ;
        this_rdeth = (ib_ext_rdeth_t*)aux ;
        this_rdeth->ee_context = 0x223344;
        */
        aux = (uint8_t*) erf + ERF_LENGTH + header_length - IB_EXT_DETH_LEN ;
        
        this_deth = (ib_ext_deth_t*) aux;
        this_deth->source_qp = htonl ((src_qp_value << 8) & 0xffffff00 );
  //      printf("SRC QP 0x%05x \n",this_deth->source_qp);
    }
    // Payload
    aux = (uint8_t*) erf + (ERF_LENGTH +  header_length);
    memcpy (aux , payload, pload_length);
    free(payload);

    /* packet filling is done.now calculate ICRC - same as done in dagbits*/
    if ( NULL == this_grh )
    {
        /* Local packet - No GRH - Mask the required fields in LRH and BTH*/
        masked_len = sizeof ( ib_lrh_t ) + sizeof(ib_bth_t) ;
        masked_ib_hdrs = (uint8_t *) malloc ( masked_len );
        
        //memcpy(masked_lrh, this_lrh, sizeof ( ib_lrh_t));
        memcpy( masked_ib_hdrs, (uint8_t*)erf + ERF_LENGTH, masked_len);
        masked_lrh = (ib_lrh_t *) masked_ib_hdrs;
        masked_lrh->virtual_lane = 0xf;

        masked_bth = (ib_bth_t*) (masked_ib_hdrs + sizeof(ib_lrh_t));
        masked_bth->reserved1 = 0xff;
    }
    else 
    {
        /* has GRH - Mask whole LRH and the required fileds of BTH and GRH */
        masked_len = sizeof ( ib_lrh_t ) + sizeof(ib_grh_t) + sizeof(ib_bth_t);
        masked_ib_hdrs = (uint8_t*) malloc( masked_len);
        memcpy( masked_ib_hdrs, (uint8_t*)erf + ERF_LENGTH, masked_len);

        masked_lrh = (ib_lrh_t*) masked_ib_hdrs;
        /* mask the whole LRH */
        masked_lrh->link_version = 0xf;
        masked_lrh->virtual_lane = 0xf;
        masked_lrh->lnh = 0x3;
        masked_lrh->reserved1 = 0x3;
        masked_lrh->service_level = 0xf;
        masked_lrh->dest_local_id = 0xffff;
        masked_lrh->packet_length = 0xffff;
        masked_lrh->src_local_id = 0xffff;
        /* mask the grh fields */
        masked_grh = (ib_grh_t*) (masked_ib_hdrs + sizeof(ib_lrh_t));
        /* mask the Flow label, Traffic Class*/
        masked_grh->word0 |= 0xffffff0f;
        masked_grh->hop_limit = 0xff;
        masked_bth = (ib_bth_t*) (masked_ib_hdrs + sizeof(ib_lrh_t) + sizeof( ib_grh_t ));
        masked_bth->reserved1 = 0xff;    
    }
   /* calculate the ICRC here */
     icrc = daggen_infiniband_icrc(0, masked_ib_hdrs, masked_len, (uint8_t*)erf + ERF_LENGTH +  masked_len,  ( p_length - (masked_len + IB_ICRC_LEN) ));
    /* Fill icrc */
    aux = (uint8_t*) erf +  (p_length + ERF_LENGTH - IB_ICRC_LEN);
    //icrc = htonl( icrc );
    memcpy ( aux , &icrc, 4);

    /* calcuate vcrc */
    vcrc =  (daggen_infiniband_vcrc16( 0, ((uint8_t*) erf + ERF_LENGTH) , p_length ) );
    /* copy the vcrc */
    aux = (uint8_t*) erf + ERF_LENGTH + p_length;
    memcpy((uint8_t*) aux ,(uint8_t*)&vcrc,2);

	/* free the malloc'ed buffer */
    free (masked_ib_hdrs);
    return erf;
}

