/*
* Copyright c                  Realtek Semiconductor Corporation, 2002
* All rights reserved.
*
* Program : Code for packet generation
* Abstract :
* Author : 
* $Id: pktGen.c,v 1.1 2012/10/24 04:18:57 ikevin362 Exp $
*/

#include <net/rtl/rtl_types.h>

#include "xdsl_types.h"
#include "pktGen.h"
#include "crc32.h"

void l2FormEtherAddress(int8 * pktBuf, int8 * da, int8 * sa) {
	_etherHdr_t * etherPtr = (_etherHdr_t *)pktBuf;
	memcpy(etherPtr->da, da, 6);
	memcpy(etherPtr->sa, sa, 6);
}

void l2FormCrc(uint32 flag, int8 * pktBuf, uint32 payloadSize) {
	uint32 crc, i;

	/* Ethernet assume a packet will longer than 60 bytes, therefore, 
	 * we must pad packet content first if the length is smaller than 60 bytes.
	 * Then, we append L2 CRC in the last 4 bytes. */
	if ( payloadSize < 60 )
	{
		memset((void*)&pktBuf[payloadSize], 0x00/* backward compatible for drvTest*/, (60 - payloadSize));
		payloadSize = 60;
	}
	pktForm_crc32(pktBuf,payloadSize,&crc);

	for(i=0;i<4;i++){
		pktBuf[payloadSize+i] = (crc>>(i*8)) & 0xff; /* New version, reversed again */
	}
}


// user defined etherType
void l2FormOtherEtherType(uint32 flag, int8 * pktBuf, uint16 etherType, int8 * content, uint32 payloadLen) {
	
	_etherHdr_t * etherPtr;
	int8 * tmpPtr = pktBuf +  14;	//mac address add ether type 

	memcpy(tmpPtr,content,payloadLen);

	etherPtr = (_etherHdr_t*)pktBuf;
	etherPtr->etherType = htons(etherType);
}


uint32 formedFrameLen(uint32 flag, int8 * pktBuf, uint32 payloadLen) {

	return payloadLen + sizeof(_etherHdr_t);
}

/*  CLEAR_PKTBUF
 *  If defined, always clean pktBuf before build packet
 *  Not defined, ignore cleaning packet buffer to speed up
 */
#define CLEAN_PKTBUF

void protoForm_vsp_ether(int8 * pktBuf, uint32 l2Flag, 
						int8 * da, int8 * sa, 
						uint16 etherType,
						int8 * content,
						uint32 * len) {
	uint32 pktFmt=0;

#ifdef CLEAN_PKTBUF
	memset((void*)pktBuf, 0x00, (unsigned short)(*len));
#endif
	l2FormEtherAddress(pktBuf, da, sa);

	l2FormOtherEtherType(pktFmt, pktBuf, etherType, content, *len);
	*len = formedFrameLen(pktFmt, pktBuf, *len);

	l2FormCrc(pktFmt, pktBuf, *len);
}

void set_mac (int8 *a, int8 h1, int8 h2, int8 h3, int8 h4, int8 h5, int8 h6)
{
	int8 *b = a;

	*b++ = h1;
	*b++ = h2;
	*b++ = h3;
	*b++ = h4;
	*b++ = h5;
	*b = h6;
}



/*
 *  pktGen() - Generate a packet as you want
 *  before setting conf u should bzero conf struct for default value
 *  The returned value is the packet length.
 *  The length does not included L2 CRC, however, the L2 CRC is followed after the tail of packet.
 */
unsigned int pktGen(PktConf_t *Pktptr, int8 *buff)
{
	PktConf_t pkt;
	PktConf_t *ptr = &pkt;
	uint32 pktLen;
	uint32 l2Flag = 0;
	int8 dummy[16]={0};
	int8* PayloadPtr = dummy;

	// copy pkt conf to fake pkt
	memcpy(ptr, Pktptr, sizeof(PktConf_t));

	/*
			Ether-Header
	*/
	if (memcmp(ptr->conf_smac, PayloadPtr, 6) == 0){
		// default smac
		set_mac (ptr->conf_smac, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03);
	}
	if (memcmp(ptr->conf_dmac, PayloadPtr, 6) == 0){
		// default dmac (unicast)
		set_mac (ptr->conf_dmac, 0x00, 0x00, 0x00, 0x0a, 0x0b, 0x0c);
	}

	/*
		Payload
	*/
	if ((ptr->payload.length == 0) || (ptr->payload.content == NULL)){
		pktLen = 0;
	}else{
		pktLen = ptr->payload.length;
		PayloadPtr = ptr->payload.content;
	}

	protoForm_vsp_ether(buff, l2Flag, ptr->conf_dmac, ptr->conf_smac,
				ptr->conf_ethtype,
				PayloadPtr, &pktLen);

	/* If packet is smaller than 60 bytes, add tailer padding. */
	if (pktLen < 60)
	{
		pktLen = 60;
	}

	return pktLen;
}