/*********************************************************************************
 * Program to create mock-up STP data for testing PIPE mode decoding of Trace32  *
 *********************************************************************************/

#if defined(_MSC_VER)||defined(__MINGW32__)
#   define _CRT_SECURE_NO_WARNINGS 1
#   include <windows.h>
#endif

#include <stdio.h>
#include <string.h>
#if defined(__linux__)
#	include <stdlib.h>
#endif

typedef unsigned char uint8_t;
typedef unsigned short uint16_t;
typedef unsigned int uint32_t;
typedef signed char int8_t;
typedef signed short int16_t;
typedef signed int int32_t;

#include "twp.h"

static int gWithWrapper = 0;
static int myStpBufLen  = 0; /* length of buffer in nibbles */
static int gTotalLen    = 0; /* number of bytes written */
// static uint8_t myBuf[2048];

static bufferT myStpBuf;
static bufferT myItmBuf;
static bufferT myAtb1Buf;

static void writeItmMsg(int channel,int sz,uint32_t value)
{
	uint8_t *b;
	int len;
	len=myItmBuf.len;
	b=myItmBuf.buf+len;
	b[0]=(sz&0x3)|((channel&0x1F)<<3);
	switch(sz&0x3) {
	case 1: /* 8 bit */
		b[1]=(uint8_t)value;
		len+=2;
		break;
	case 2: /* 16 bit */
		*((uint16_t *)(b+1)) = (uint16_t)value;
		len+=3;
		break;
	case 3: /* 32 bit */
		*((uint32_t *)(b+1)) = (uint32_t)value;
		len+=5;
		break;
	}
	myItmBuf.len=len;
}

static void writeStpNibbles(uint8_t *buf,int nibbles)
{
	int len;
	uint8_t *b;
	uint8_t h,x;

	if (!nibbles)
		return;

	len = myStpBufLen;
	b = myStpBuf.buf+(len/2);
	if ( len&1 ) {
		len += nibbles;
		x = b[0];
		while(nibbles) {
			nibbles--;
			h=buf[0];buf++;
			x|=h<<4;
			b[0]=x;b++;
			if (!nibbles)
				break;
			nibbles--;
			x=h>>4;
		}
		b[0]=x;
	} else {
		len += nibbles;
		memcpy(b,buf,nibbles/2);
		if (nibbles&1) {
			b+=(nibbles/2);
			buf+=(nibbles/2);
			x=buf[0];
			x&=0xF;
			b[0]=x;
		}
	}
	myStpBufLen = len;
}

/* write valid 48 byte header to create file with fake data */
static void writeHeader(FILE *fp)
{
	static char canalyzerPipeSaveHeader[] =   "trace32 canalyzer PipeSAVE    \032\0";
	unsigned char header[16];

	fwrite(canalyzerPipeSaveHeader,1,32,fp);
	memset(header,0,16);

	/* format  = 0x0001, standard fake format */
	header[0]=1;
	header[1]=0;

	/* version = 0x0000 */
	header[2]=0;
	header[3]=0;
	fwrite(header,1,16,fp);
}

static void writeJunk(FILE *fp,int size)
{
	unsigned char buf[32];
	unsigned char cnt;
	int len,i;
	cnt=0;
	while (size>0) {
		len=size;
		if (len>32)
			len=32;
		for (i=0;i<len;i++) {
			buf[i]=cnt;
			cnt++;
		}
		fwrite(buf,1,32,fp);
		size-=len;
	}
}

static FILE *myOutFile;

static void flushToFile(int all)
{
	static unsigned char wrapBuf[2048*8];
	int blen,clen;
	blen=myStpBufLen;
	clen=blen/2;
	if (all && (blen&1)!=0) {
		clen++;
		blen=0;
	}
	if (clen)
	{
		if (gWithWrapper) {
			int len;
			bufferP buffers[4];
			myStpBuf.len=clen;
			for (len=0;len<clen;len++) {
				myAtb1Buf.buf[len]=(uint8_t)len;
			}
			myAtb1Buf.len=clen;
			buffers[0]=&(myStpBuf);
			buffers[1]=&(myItmBuf);
			buffers[2]=&(myAtb1Buf);
			buffers[3]=NULL;
			len = wrapMultiBuffers(wrapBuf,buffers);
			printf("Wrapped buffer: input bytes %d => wrapped bytes %d\n",clen,len);
			fwrite(wrapBuf,1,len,myOutFile);
			gTotalLen+=len;
		} else {
			fwrite(myStpBuf.buf,1,clen,myOutFile);
			gTotalLen+=clen;
		}
	}
	myStpBufLen=0;
	myStpBuf.len=0;
	myItmBuf.len=0;
	myAtb1Buf.len=0;
	if (blen&1) {
		myStpBuf.buf[0]=myStpBuf.buf[clen];
		myStpBufLen=1;
	}
	if (all && (gTotalLen&1)!=0) {
		memset(wrapBuf,0,16);
		fwrite(wrapBuf,1,1,myOutFile);
	}
}

static void putStpToFile(unsigned char *buf,int nibbles)
{
	writeStpNibbles(buf,nibbles);
	if (myStpBufLen>2048)
		flushToFile(0);
}

static void writeMasterID(FILE *fp,unsigned int id)
{
	unsigned char buffer[16];
	/* Create MasterID message */
	buffer[0]=0x1|(id&0xF0);
	buffer[1]=id&0x0F;
	putStpToFile(buffer,3);
}

static void writeMasterID16(FILE *fp,unsigned int id)
{
	unsigned char buffer[16];
	/* Create MasterID message */
	buffer[0]=0x1F;
	/* swap nibbles */
	id=((id&0xF0F0)>>4)|((id&0x0F0F)<<4);
	buffer[1]=(unsigned char)(id>>8);
	buffer[2]=(unsigned char)id;
	putStpToFile(buffer,6);
}

static void writeChannelID(FILE *fp,unsigned int id)
{
	unsigned char buffer[16];
	/* Create ChannelID message */
	buffer[0]=0x3|(id&0xF0);
	buffer[1]=id&0x0F;
	putStpToFile(buffer,3);
}

static void writeChannelID16(FILE *fp,unsigned int id)
{
	unsigned char buffer[16];
	/* Create C16 message */
	buffer[0]=0x3F;
	/* swap nibbles */
	id=((id&0xF0F0)>>4)|((id&0x0F0F)<<4);
	buffer[1]=(unsigned char)(id>>8);
	buffer[2]=(unsigned char)id;
	putStpToFile(buffer,6);
}

static int stp_version;
static int last_ts;

static int addTS(unsigned char *buf,int ofs,int ts)
{
	unsigned int tsbits;
	int tslen;
	int res;

	tsbits=0;
	tslen=0;
	if (stp_version<2) {
		tslen=2;
		tsbits=((ts&0xF0)>>4)|((ts&0x0F)<<4);
	} else if (stp_version==2) {
		/* NAT delta */
		int delta,i,j;
		unsigned int tsmask;
		delta=ts-last_ts;
		last_ts=ts;
		i=28;
		tsmask=0x0F000000;
		while (tsmask && (delta&tsmask)==0 ) {
			tsmask>>=4;
			i-=4;
		}
		last_ts=ts;
		tslen=0;
		j=0;
		while (j<i) {
			tsbits<<=4;
			tsbits|=(delta>>j)&0xF;
			tslen++;
			j+=4;
		}
	} else if (stp_version==3 || stp_version==4) {
		int i,j;
		unsigned int tsmask;
		if (stp_version==4) {
			ts^=ts>>1;
		}
		i=28;
		tsmask=0x0F000000;
		while (tsmask && ((ts&tsmask)==(last_ts&tsmask)) ) {
			tsmask>>=4;
			i-=4;
		}
		last_ts=ts;
		tslen=0;
		j=0;
		while (j<i) {
			tsbits<<=4;
			tsbits|=(ts>>j)&0xF;
			tslen++;
			j+=4;
		}
	}
	if (stp_version>=2) {
		tsbits<<=4;
		tsbits|=tslen;
		tslen++;
	}
	// printf("addTS tsbits 0x%0x, tslen %d\n",tsbits,tslen);

	res = tslen;
	if (ofs && tslen>0) {
		buf[0] |= (unsigned char)(tsbits<<4);
		tsbits>>=4;
		tslen--;
		buf++;
	}
	while (tslen>=0) {
		buf[0] = (unsigned char)(tsbits);
		tsbits>>=8;
		tslen-=2;
		buf++;
	}
	return res;
}

static void writeD4(FILE *fp,unsigned int d,int ts)
{
	unsigned char buffer[16];
	int len;
	len=2;
	if (ts<0) {
		/* D4 */
		buffer[0]=0xC|(d<<4);
	}else {
		/* D4MTS */
		buffer[0]=0xD|(d<<4);
		len+=addTS(buffer+1,0,ts);
	}
	putStpToFile(buffer,len);
}

static void writeD8(FILE *fp,unsigned int d,int ts)
{
	unsigned char buffer[16];
	int len;
	buffer[1]=(d&0x0F);
	len=3;
	if (ts<0)
	{
		/* Create D8 message */
		buffer[0]=0x04|(d&0xF0);
	}
	else
	{
		/* Create D8MTS message */
		buffer[0]=0x08|(d&0xF0);
		len+=addTS(buffer+1,1,ts);
	}
	putStpToFile(buffer,len);
}

static void writeD16(FILE *fp,unsigned int d,int ts)
{
	int len;
	unsigned char buffer[16];
	d=((d&0xFF00)>>8)|((d&0x00FF)<<8);
	d=((d&0xF0F0)>>4)|((d&0x0F0F)<<4);

	buffer[1]=(unsigned char)(d>>4);
	buffer[2]=(unsigned char)(d>>12);
	len=5;
	if (ts<0)
	{
		/* Create D16 message */
		buffer[0]=0x05|(d<<4);
	}
	else
	{
		/* Create D16MTS message */
		buffer[0]=0x09|(d<<4);
		len+=addTS(buffer+2,1,ts);
	}
	putStpToFile(buffer,len);
}

static void writeD32(FILE *fp,unsigned int d,int ts)
{
	unsigned char buffer[16];
	int len;
	d=((d&0xFFFF0000)>>16)|((d&0x0000FFFF)<<16);
	d=((d&0xFF00FF00)>>8 )|((d&0x00FF00FF)<<8 );
	d=((d&0xF0F0F0F0)>>4 )|((d&0x0F0F0F0F)<<4 );
	buffer[1]=(unsigned char)(d>>4);
	buffer[2]=(unsigned char)(d>>12);
	buffer[3]=(unsigned char)(d>>20);
	buffer[4]=(unsigned char)(d>>28);
	len=9;
	if (ts<0)
	{
		/* Create D32 message */
		buffer[0]=0x06|(d<<4);
	}
	else
	{
		/* Create D32MTS message */
		buffer[0]=0x0A|(d<<4);
		len+=addTS(buffer+4,1,ts);
	}
	putStpToFile(buffer,len);
}

static void writeD64(FILE *fp,unsigned int dl,unsigned int dh,int ts)
{
	unsigned char buffer[16];
	int len;
	dl=((dl&0xFFFF0000)>>16)|((dl&0x0000FFFF)<<16);
	dl=((dl&0xFF00FF00)>>8 )|((dl&0x00FF00FF)<<8 );
	dl=((dl&0xF0F0F0F0)>>4 )|((dl&0x0F0F0F0F)<<4 );

	dh=((dh&0xFFFF0000)>>16)|((dh&0x0000FFFF)<<16);
	dh=((dh&0xFF00FF00)>>8 )|((dh&0x00FF00FF)<<8 );
	dh=((dh&0xF0F0F0F0)>>4 )|((dh&0x0F0F0F0F)<<4 );

	buffer[1]=(unsigned char)(dh>>4);
	buffer[2]=(unsigned char)(dh>>12);
	buffer[3]=(unsigned char)(dh>>20);
	buffer[4]=(unsigned char)((dh>>28)|(dl<<4));
	buffer[5]=(unsigned char)(dl>>4);
	buffer[6]=(unsigned char)(dl>>12);
	buffer[7]=(unsigned char)(dl>>20);
	buffer[8]=(unsigned char)(dl>>28);
	len=17;
	if (ts<0)
	{
		/* Create D64 message */
		buffer[0]=0x07|(dh<<4);
	}
	else
	{
		/* Create D64MTS message */
		buffer[0]=0x0B|(dh<<4);
		len+=addTS(buffer+8,1,ts);
	}
	putStpToFile(buffer,len);
}

static void writeD4v2(FILE *fp,unsigned int d,int ts)
{
	unsigned char buffer[16];
	int len;
	buffer[1]=d&0x0F;
	len=3;
	if (ts<0) {
		/* D4M */
		buffer[0]=0xDF;
	}else {
		/* D4TS */
		buffer[0]=0xCF;
		len+=addTS(buffer+1,1,ts);
	}
	putStpToFile(buffer,len);
}

static void writeD8v2(FILE *fp,unsigned int d,int ts)
{
	unsigned char buffer[16];
	int len;
	buffer[1]=(unsigned char)d;

	len=4;
	if (ts<0) {
		/* D8M */
		buffer[0]=0x8F;
	} else {
		/* D8TS */
		buffer[0]=0x4F;
		len+=addTS(buffer+2,0,ts);
	}
	putStpToFile(buffer,len);
}

static void writeD16v2(FILE *fp,unsigned int d,int ts)
{
	unsigned char buffer[16];
	int len;
	d=((d&0xF0F0)>>4)|((d&0x0F0F)<<4);
	buffer[1]=(unsigned char)(d>>8);
	buffer[2]=(unsigned char)(d);

	len=6;
	if (ts<0) {
		/* D16M */
		buffer[0]=0x9F;
	}else {
		/* D16TS */
		buffer[0]=0x5F;
		len+=addTS(buffer+3,0,ts);
	}
	putStpToFile(buffer,len);
}

static void writeD32v2(FILE *fp,unsigned int d,int ts)
{
	unsigned char buffer[16];
	int len;
	d=((d&0xF0F0F0F0)>>4)|((d&0x0F0F0F0F)<<4);
	buffer[1]=(unsigned char)(d>>24);
	buffer[2]=(unsigned char)(d>>16);
	buffer[3]=(unsigned char)(d>>8);
	buffer[4]=(unsigned char)(d);

	len=10;
	if (ts<0) {
		/* D32M */
		buffer[0]=0xAF;
	} else {
		/* D32TS */
		buffer[0]=0x6F;
		len+=addTS(buffer+5,0,ts);
	}
	putStpToFile(buffer,len);
}

static void writeD64v2(FILE *fp,unsigned int dl,unsigned int dh,int ts)
{
	unsigned char buffer[16];
	int len;
	dl=((dl&0xF0F0F0F0)>>4)|((dl&0x0F0F0F0F)<<4);
	dh=((dh&0xF0F0F0F0)>>4)|((dh&0x0F0F0F0F)<<4);

	buffer[1]=(unsigned char)(dh>>24);
	buffer[2]=(unsigned char)(dh>>16);
	buffer[3]=(unsigned char)(dh>>8);
	buffer[4]=(unsigned char)(dh);

	buffer[5]=(unsigned char)(dl>>24);
	buffer[6]=(unsigned char)(dl>>16);
	buffer[7]=(unsigned char)(dl>>8);
	buffer[8]=(unsigned char)(dl);

	len=18;
	if (ts<0) {
		/* D64M  */
		buffer[0]=0xBF;
	} else {
		/* D64TS */
		buffer[0]=0x7F;
		len+=addTS(buffer+9,0,ts);
	}
	putStpToFile(buffer,len);
}

static void writeAsync(FILE *fp)
{
	unsigned char buffer[16];
	buffer[ 0]=0xFF;
	buffer[ 1]=0xFF;
	buffer[ 2]=0xFF;
	buffer[ 3]=0xFF;
	buffer[ 4]=0xFF;
	buffer[ 5]=0xFF;
	buffer[ 6]=0xFF;
	buffer[ 7]=0xFF;
	buffer[ 8]=0xFF;
	buffer[ 9]=0xFF;
	buffer[10]=0x0F;
	buffer[11]=0x00;
	buffer[12]=0x00;
	putStpToFile(buffer,26);
}

static void writeVersion(FILE *fp)
{
	unsigned char buffer[16];
	buffer[ 0]=0x0F;
	buffer[ 1]=0x00|(stp_version<<4);
	buffer[ 2]=0x00;
	putStpToFile(buffer,6);
	last_ts = 0x0FFFFFFF;
}

#define nextCount(__cmp) \
	{ \
		if (__cmp & 0x80000000) \
		{ \
			__cmp <<= 1; \
			__cmp ^= 0x10904081; \
		} \
		else \
			__cmp <<= 1; \
	}

int main(int argc, char **argv)
{
	FILE *fp;
	int i,j,numRec;
	unsigned int c;
	unsigned int ch;

	if (argc<3 || argc>5) {
		printf("Usage: create_fake_stp <filename> <number of D32 messages> [stp ts version] [junk at beginning]\n");
		return 1;
	}
	fp=fopen(argv[1],"wb");

	if (fp==NULL) {
		printf("Cannot open %s for writing\n",argv[1]);
		return 2;
	}
	/* Initialize by writing header to file */
	writeHeader(fp);
	myOutFile=fp;
	numRec=strtoul(argv[2],NULL,0);
	stp_version=0;
	myStpBuf.atbid=4;
	myItmBuf.atbid=5;
	myAtb1Buf.atbid=6;

	if (argc>=4) {
		stp_version=strtoul(argv[3],NULL,0);
		gWithWrapper=stp_version&0x10;
		stp_version&=0xF;
	}
	if (argc>=5) {
		c=strtoul(argv[4],NULL,0);
		writeJunk(fp,c);
	}
	/* Each byte contains two nibbles.
	   Bit 3-0 of the byte contains the first  nibble,
	   Bit 7-4 of the byte contains the second nibble
	*/

	/*
	   The following code will create STP data which
	   is compatible to the various "pread..." applications
	*/
	c=0xFF00FF00;
	for (i=0,j=0;i<numRec;i+=4,j++) {

		if ( stp_version && (j&0xFF)==0x0 ) {
			writeAsync(fp);
			writeVersion(fp);
		}
		/* Sometimes output "MasterID" messages */
		if ( (j&0x1F)==0 ) {
			if (stp_version && (j&0x20) != 0)
				writeMasterID16(fp,((((j>>6)&0x7)+1)<<8)|(((j>>6)&0x7)+1));
			else
				writeMasterID(fp,((j>>6)&0x7)+1);
		}

		nextCount(c);
		ch=c&0xFF;
		if ( stp_version && (j&0x1) )
			writeChannelID16(fp,(ch<<8)|ch);
		else
			writeChannelID(fp,ch);

		nextCount(c);
		writeD8(fp,c,-1);
		writeItmMsg(ch,1,c);

		nextCount(c);
		writeD8(fp,c,i*0x10+1);
		writeItmMsg(ch,1,c);

		nextCount(c);
		writeD16(fp,c,-1);
		writeItmMsg(ch,2,c);

		nextCount(c);
		writeD16(fp,c,i*0x10+2);
		writeItmMsg(ch,2,c);

		nextCount(c);
		writeD32(fp,c,-1);
		writeItmMsg(ch,3,c);

		nextCount(c);
		writeD32(fp,c,i*0x10+3);
		writeItmMsg(ch,3,c);

		nextCount(c);ch=c;nextCount(c);
		writeD64(fp,c,ch,-1);
		writeItmMsg(ch,3,c);
		writeItmMsg(ch,3,c);

		nextCount(c);ch=c;nextCount(c);
		writeD64(fp,c,ch,i*0x10+4);
		writeItmMsg(ch,3,c);
		writeItmMsg(ch,3,c);

		if (stp_version) {
			nextCount(c);
			writeD4(fp,c,-1);
			writeItmMsg(ch,1,c);

			nextCount(c);
			writeD4(fp,c,i*0x10+5);

			nextCount(c);
			writeD8v2(fp,c,-1);
			writeItmMsg(ch,1,c);

			nextCount(c);
			writeD16v2(fp,c,-1);
			writeItmMsg(ch,2,c);

			nextCount(c);
			writeD32v2(fp,c,-1);
			writeItmMsg(ch,3,c);

			nextCount(c);ch=c;nextCount(c);
			writeD64v2(fp,c,ch,-1);
			writeItmMsg(ch,3,c);
			writeItmMsg(ch,3,c);

			nextCount(c);
			writeD4v2(fp,c,-1);
			writeItmMsg(ch,1,c);

			nextCount(c);
			writeD8v2(fp,c,i*0x10+6);

			nextCount(c);
			writeD16v2(fp,c,i*0x10+7);
			writeItmMsg(ch,2,c);

			nextCount(c);
			writeD32v2(fp,c,i*0x10+8);
			writeItmMsg(ch,3,c);

			nextCount(c);ch=c;nextCount(c);
			writeD64v2(fp,c,ch,i*0x10+9);
			writeItmMsg(ch,3,c);
			writeItmMsg(ch,3,c);

			nextCount(c);
			writeD4v2(fp,c,i*0x10+10);
			writeItmMsg(ch,3,c);
			writeItmMsg(ch,3,c);
		}
	}
	flushToFile(1);
	fclose(fp);
	return 0;
}