#include "proto.h"
#include <string.h>


/**************************************************************************

	USB Protocol

**************************************************************************/

#ifndef TRUE
# define TRUE 1
#endif
#ifndef FALSE
# define FALSE 0
#endif

typedef struct
{
	int findstate;
	int drawindex1;
	int drawindex2;
	int drawindex3;
}
usbLocalData;

static const int32_t UsbClockFast = 12000000;
static const int32_t UsbClockSlow = 12000000/8;

typedef struct
{
	protoTime time;
	unsigned char value;
}
bitStream;


#define USB_FLAG_BITERROR       0x01
#define USB_FLAG_GAP            0x02
#define USB_FLAG_ERRORSTUFFING  0x04
#define USB_FLAG_EOP            0x08
#define USB_FLAG_ERROREOP       0x10
#define USB_FLAG_BUSRESET       0x20

typedef struct
{
	protoTime time;

	unsigned char index;
	unsigned char flags;
	unsigned char data;
}
usbTraceEntry;


static protoSizeT PROTOAPI PROTO_Process(protoContext context, protoPtr arrayOut, protoSizeT arrayOutSize, protoPtr arrayIn, protoSizeT arrayInSize, protoPtr localdata)
{
	int             i;
	int             timerefvalid;
	int             nbits, nbytes;
	protoSizeT      indexin, indexout;
	protoTime       time;
	protoTime       timeref;
	protoTime       timelastbit;
	int             tbuffer[2];
	int             tbufferref[2];
	int             tbufferlastbit[2];
	usbTraceEntry  *usbtracebuffer;
	protoTime       bittime, tolerance, bittimeslow, toleranceslow, bittimefast;
	protoTime       tolerancefast, delta, deltat;
	int             nstuffing, nbitstream, tbufferlastbitvalid;
	bitStream       bitstream[32];

	usbtracebuffer = (usbTraceEntry *) arrayOut;

	/*
	 * set some helper variables that contain the bit times and the allowable
	 * tollerance
	 */

	bittimefast = PROTO_TIME_DIVINT(PROTO_TIME_ONESECOND, UsbClockFast);
	tolerancefast = PROTO_TIME_DIVINT(bittimefast, 2); /* 50% error tolerance */
	bittimeslow = PROTO_TIME_DIVINT(PROTO_TIME_ONESECOND, UsbClockSlow);
	toleranceslow = PROTO_TIME_DIVINT(bittimeslow, 2);

	/* start with full speed mode */

	bittime = bittimefast;
	tolerance = tolerancefast;

	/* initialize some loop variables */

	indexout = 0;
	nbitstream = 0;
	nstuffing = 0;
	nbytes = 0;

	/* assume that we will no generate more than 10 frames for one transient */
	arrayOutSize -= 10;

	PROTO_ReadTrace(context, 0, &timeref, tbufferref);

	timerefvalid = TRUE;
	timelastbit = timeref;
	tbufferlastbit[0] = tbufferref[0];
	tbufferlastbit[1] = tbufferref[1];
	tbufferlastbitvalid = FALSE;

	for (indexin = 0; indexin < arrayInSize; indexin++) {

		if (indexout >= arrayOutSize)
			return PROTO_PROCESS_OUTOFMEMORY;

		if ((indexin & 0xfff) == 0)
			if (PROTO_Cancel(context))
				return PROTO_PROCESS_CANCEL;

		PROTO_ReadTrace(context, -1, &time, tbuffer);

		if (!(tbuffer[0] != tbufferref[0] || tbuffer[1] != tbufferref[1]))      /* any transition ? */
			continue;

		delta = PROTO_TIME_SUB(time, timeref);  /* time since last transition */

		if (PROTO_TIME_ISSMALLER(tolerance, delta)) {

			/* time since last "real" bit */

			delta = PROTO_TIME_SUB(timeref, timelastbit);

			/* number of bits that fit into this time */

			nbits = PROTO_TIME_TOINT(PROTO_TIME_DIV(PROTO_TIME_ADD(delta, PROTO_TIME_DIVINT(bittime, 2)), bittime));

			/* difference between real time and calculated bit time */

			deltat = PROTO_TIME_ABS(PROTO_TIME_SUB(delta, PROTO_TIME_MULTINT(bittime, nbits)));

			if (!tbufferlastbitvalid) {
				nbits = 100;
				tbufferlastbitvalid = TRUE;
			}
			if (tbufferlastbit[0] == 0 && tbufferlastbit[1] == 0) { /* EOP */
				if (nbits > 50) {
					usbtracebuffer[indexout].time = timelastbit;
					usbtracebuffer[indexout].flags = USB_FLAG_BUSRESET; /* BUS RESET */
					indexout++;
				} else {
					if (nbitstream) {
						usbtracebuffer[indexout].time = bitstream[0].time;
						usbtracebuffer[indexout].flags = 0;     /* regular data */
						usbtracebuffer[indexout].data  = bitstream[0].value << 0;
						usbtracebuffer[indexout].data |= bitstream[1].value << 1;
						usbtracebuffer[indexout].data |= bitstream[2].value << 2;
						usbtracebuffer[indexout].data |= bitstream[3].value << 3;
						usbtracebuffer[indexout].data |= bitstream[4].value << 4;
						usbtracebuffer[indexout].data |= bitstream[5].value << 5;
						usbtracebuffer[indexout].data |= bitstream[6].value << 6;
						usbtracebuffer[indexout].data |= bitstream[7].value << 7;
						usbtracebuffer[indexout].data &= (1 << nbitstream) - 1;
						usbtracebuffer[indexout].index = (unsigned char)nbytes;
						indexout++;
					}
					usbtracebuffer[indexout].time = timelastbit;
					usbtracebuffer[indexout].flags = USB_FLAG_EOP; /* EOP */
					indexout++;
					if (nbits != 2) {
						usbtracebuffer[indexout].time = timelastbit;
						usbtracebuffer[indexout].flags = USB_FLAG_ERROREOP;     /* EOP error */
						indexout++;
					}
				}
				nbits = 0;
				nbitstream = nstuffing = 0;
				nbytes = 0;

				tbufferlastbit[0] = tbufferref[0];
				tbufferlastbit[1] = tbufferref[1];
				timelastbit = timeref;
				tbufferref[0] = tbuffer[0];
				tbufferref[1] = tbuffer[1];
				timeref = time;
				timerefvalid = TRUE;
				tbufferlastbitvalid = FALSE;
				bittime = bittimefast;
				tolerance = tolerancefast;
				continue;
			}
			if (PROTO_TIME_ISSMALLER(tolerance, deltat) && nbits <= 9) {
				usbtracebuffer[indexout].time = timelastbit;
				usbtracebuffer[indexout].flags = USB_FLAG_BITERROR;     /* Bitfehler */
				indexout++;
				nbitstream = nstuffing = 0;
			} else {
				if (nbits > 9 || PROTO_TIME_ISZERO(delta)) {
					if (!(tbufferref[0] == 0 && tbufferref[1] == 0)) {
						usbtracebuffer[indexout].time = timelastbit;
						usbtracebuffer[indexout].flags = USB_FLAG_GAP;  /* GAP */
						indexout++;
					}
					nbits = 1;
					nbitstream = nstuffing = 0;
					nbytes = 0;
				}
				for (i = 1; i < nbits; i++) {   /* no change -> produces '1' */
					bitstream[nbitstream + nbits - i - 1].value = 1;
					bitstream[nbitstream + nbits - i - 1].time = PROTO_TIME_SUB(timeref, PROTO_TIME_MULTINT(bittime, i));
				}
				nbitstream += nbits - 1;
				/* last bit had the transition -> '0' */
				bitstream[nbitstream].value = 0;
				bitstream[nbitstream].time = timeref;
				nbitstream++;

				while (TRUE) {
					for (i = nstuffing; i < nbitstream - 6 && nstuffing < 8; i++) {
						/* bit stuffing 6x'1' -> ignore next '0' */
						if (  bitstream[i].value == 1
						   && bitstream[i + 1].value == 1
						   && bitstream[i + 2].value == 1
						   && bitstream[i + 3].value == 1
						   && bitstream[i + 4].value == 1
						   && bitstream[i + 5].value == 1) {
							if (bitstream[i + 6].value == 1) {
								usbtracebuffer[indexout].time = timelastbit;
								usbtracebuffer[indexout].flags = USB_FLAG_ERRORSTUFFING;        /* stuffing error */
								indexout++;
								nstuffing = i + 6;
								break;
							}
							i += 6;
							nstuffing = i;
							while (i < nbitstream - 1) {
								bitstream[i] = bitstream[i + 1];
								i++;
							}
							nbitstream--;
							break;
						}
						nstuffing = i;
					}
					/* do we have a complete byte? */
					if (nstuffing >= 8) {
						int             datavalue;

						datavalue = (bitstream[0].value) | (bitstream[1].value << 1) | (bitstream[2].value << 2) | (bitstream[3].value << 3)
							| (bitstream[4].value << 4) | (bitstream[5].value << 5) | (bitstream[6].value << 6) | (bitstream[7].value << 7);

						usbtracebuffer[indexout].time = bitstream[0].time;
						usbtracebuffer[indexout].flags = 0; /* regular data */
						usbtracebuffer[indexout].data = (unsigned char)datavalue;
						usbtracebuffer[indexout].index = (unsigned char)nbytes;
						indexout++;

						nbytes++;
						nstuffing -= 8;
						nbitstream -= 8;
						if (nbitstream > 0)
							memmove(bitstream, bitstream + 8, nbitstream * sizeof(bitStream));

						if (nbytes == 2 && datavalue == 0x3c) {
							/* change to low speed transfer */
							bittime = bittimeslow;
							tolerance = toleranceslow;
							if (nbitstream < 7) {
								usbtracebuffer[indexout].time = timelastbit;
								usbtracebuffer[indexout].flags = USB_FLAG_ERRORSTUFFING;        /* Stuffing Error */
								indexout++;
							}
							if (nbitstream >= 12) {
								for (i = nbitstream / 4; i < nbitstream / 2 - 1; i++) {
									if (!bitstream[i].value)
										break;
								}
								bitstream[0] = bitstream[i];
								for (i = nbitstream / 2 + nbitstream / 4; i < nbitstream - 1; i++) {
									if (!bitstream[i].value)
										break;
								}
								bitstream[1] = bitstream[i];
								nbitstream = 2;
							} else {
								for (i = nbitstream / 2; i < nbitstream - 1; i++) {
									if (!bitstream[i].value)
										break;
								}
								bitstream[0] = bitstream[i];
								nbitstream = 1;
							}
							nbytes = 0;
						}
					} else {
						break;
					}
				}
			}

			tbufferlastbit[0] = tbufferref[0];
			tbufferlastbit[1] = tbufferref[1];
			timelastbit = timeref;
			timerefvalid = FALSE;
		}
		tbufferref[0] = tbuffer[0];
		tbufferref[1] = tbuffer[1];
		if (!timerefvalid) {
			/* take the time of the FIRST change of the lines */
			timerefvalid = TRUE;
			timeref = time;
		}
	}

	return indexout;
}


static void PROTOAPI PROTO_Display(protoContext context, protoPtr pdata, protoPtr localdata)
{
	usbTraceEntry  *data = (usbTraceEntry *) pdata;

	PROTO_Control(context, PROTO_CONTROL_INDENT);

	if (data->flags & USB_FLAG_BITERROR) {
		PROTO_Control(context, PROTO_ATTRIBUTE_ERROR);
		PROTO_Puts(context, "error:bit ");
	} else if (data->flags & USB_FLAG_ERRORSTUFFING) {
		PROTO_Control(context, PROTO_ATTRIBUTE_ERROR);
		PROTO_Puts(context, "error:stuffing ");
	} else if (data->flags & USB_FLAG_BUSRESET) {
		PROTO_Control(context, PROTO_ATTRIBUTE_LEVEL1);
		PROTO_Puts(context, "BUS RESET ");
		PROTO_Control(context, PROTO_CONTROL_LINETILLEND);
	} else if (data->flags & USB_FLAG_ERROREOP) {
		PROTO_Control(context, PROTO_ATTRIBUTE_ERROR);
		PROTO_Puts(context, "error:eop ");
	} else if (data->flags & USB_FLAG_EOP) {
		PROTO_Puts(context, "eop");
	} else if (data->flags & USB_FLAG_GAP) {
		PROTO_Control(context, PROTO_ATTRIBUTE_LEVEL1);
		PROTO_Puts(context, "gap ");
		PROTO_Control(context, PROTO_CONTROL_LINETILLEND);
	} else {
		PROTO_Control(context, PROTO_ATTRIBUTE_LIGHT);

		if (data->index >= 2) {
			PROTO_Printf(context, " %02x ", (data->index - 2));
		} else {
			PROTO_Puts(context, " -- ");
		}

		PROTO_Control(context, PROTO_ATTRIBUTE_NORM);
		PROTO_Printf(context, "%02x", data->data);

		PROTO_Control(context, PROTO_ATTRIBUTE_LEVEL1);

		if (data->index == 0) {
			if (data->data != 0x80) {
				PROTO_Control(context, PROTO_ATTRIBUTE_ERROR);
				PROTO_Puts(context, " error:sync");
			} else {
				PROTO_Puts(context, " sync");
			}
		} else if (data->index == 1) {
			switch (data->data) {
			case 0xa5:
				PROTO_Puts(context, " sof");
				break;
			case 0x2d:
				PROTO_Puts(context, " #setup");
				break;
			case 0x3c:
				PROTO_Puts(context, " #pre");
				break;
			case 0x69:
				PROTO_Puts(context, " #in");
				break;
			case 0xe1:
				PROTO_Puts(context, " #out");
				break;
			case 0xd2:
				PROTO_Puts(context, " #ack");
				break;
			case 0x5a:
				PROTO_Puts(context, " #nak");
				break;
			case 0x1e:
				PROTO_Puts(context, " #stall");
				break;
			case 0x4b:
				PROTO_Puts(context, " #data1");
				break;
			case 0xc3:
				PROTO_Puts(context, " #data0");
				break;
			default:
				PROTO_Control(context, PROTO_ATTRIBUTE_ERROR);
				PROTO_Puts(context, " error:illegal packet");
			}
		}
	}
	PROTO_Control(context, PROTO_ATTRIBUTE_LEVEL0);
}


static const char *PROTO_Chart(protoContext context, protoPtr pdata, protoPtr localdata)
{
	usbTraceEntry  *data = (usbTraceEntry *) pdata;

	static const char *states[16] = {
		"busreset", "gap", "sync", "eop", "errors",
		"sof", "#setup", "#pre", "#in",
		"#out", "#ack", "#nak", "#stall",
		"#data1", "#data0", "#other"
	};

	if (data->flags & USB_FLAG_BUSRESET) {
		return states[0];
	} else if (data->flags & USB_FLAG_GAP) {
		return states[1];
	} else if (data->flags & USB_FLAG_EOP) {
		return states[3];
	} else if (data->flags) {
		return states[4];
	} else {
		if (data->index < 1)
			return states[2];
		data = data - data->index + 1;
		switch (data->data) {
		case 0xa5:
			return states[5];
			break;
		case 0x2d:
			return states[6];
			break;
		case 0x3c:
			return states[7];
			break;
		case 0x69:
			return states[8];
			break;
		case 0xe1:
			return states[9];
			break;
		case 0xd2:
			return states[10];
			break;
		case 0x5a:
			return states[11];
			break;
		case 0x1e:
			return states[12];
			break;
		case 0x4b:
			return states[13];
			break;
		case 0xc3:
			return states[14];
			break;
		default:
			return states[15];
		}
	}
}


static int PROTO_Find(protoContext context, protoPtr pdata, protoPtr localdata)
{
	int             state;
	usbLocalData   *ptr = (usbLocalData *) localdata;
	usbTraceEntry  *data = (usbTraceEntry *) pdata;

	if (data->flags & USB_FLAG_BUSRESET) {
		state = 0;
	} else if (data->flags & USB_FLAG_GAP) {
		state = 1;
	} else if (data->flags & USB_FLAG_EOP) {
		state = 3;
	} else if (data->flags) {
		state = 4;
	} else {
		if (data->index == 0) {
			state = 2;
		} else if (data->index == 1) {
			data = data - data->index + 1;
			switch (data->data) {
			case 0xa5:
				state = 5;
				break;
			case 0x2d:
				state = 6;
				break;
			case 0x3c:
				state = 7;
				break;
			case 0x69:
				state = 8;
				break;
			case 0xe1:
				state = 9;
				break;
			case 0xd2:
				state = 10;
				break;
			case 0x5a:
				state = 11;
				break;
			case 0x1e:
				state = 12;
				break;
			case 0x4b:
				state = 13;
				break;
			case 0xc3:
				state = 14;
				break;
			default:
				state = 15;
			}
		} else {
			return FALSE;
		}
	}

	return (state == ptr->findstate);
}


static int PROTO_Draw(protoContext context, protoPtr pdata, protoPtr localdata, int *presult)
{
	usbLocalData   *ptr = (usbLocalData *) localdata;
	usbTraceEntry  *data = (usbTraceEntry *) pdata;

	if (data->flags) {
		return 0;
	} else {
		if (data->index == ptr->drawindex1) {
			presult[0] = data->data;
			return 1 << 0;
		} else if (data->index == ptr->drawindex2) {
			presult[1] = data->data;
			return 1 << 1;
		} else if (data->index == ptr->drawindex3) {
			presult[2] = data->data;
			return 1 << 2;
		}
	}

	return 0;
}


static int PROTOAPI PROTO_Share(protoContext context, protoPtr sharedata, protoPtr localdata)
{
	/* Always shareable because the USB protocol has no parameters relevant to
	 * the PROTO_Process function. */
	return 1;
}



/**************************************************************************

	USB Protocol Parser

**************************************************************************/

int PROTOAPI PROTO_Init(protoContext context, int command)
{
	usbLocalData   *ptr;
	int             state, index, channels;

	if (PROTO_RequestVersion(context, 2) != PROTO_OK) {
		PROTO_Puts(context, "Requires newer trace32 version");
		return PROTO_FAIL;
	}

	ptr = (usbLocalData *) PROTO_Alloc(context, sizeof(usbLocalData));

	PROTO_Parse(context, (protoPtr) 0, "<+signal>", PROTO_PARSE_CHANNEL);
	PROTO_Parse(context, (protoPtr) 0, "<-signal>", PROTO_PARSE_CHANNEL);

	if (command == PROTO_COMMAND_FIND) {
		state = 0;
		PROTO_Parse(context, (protoPtr) & state, "BUSRESET,GAP,SYNC,EOP,ERRORS,SOF,#SETUP,#PRE,#IN,#OUT,#ACK,#NAK,#STALL,#DATA1,#DATA0,#OTHER", PROTO_PARSE_SELECTION);
		ptr->findstate = state;
	}

	channels = 0;
	if (command == PROTO_COMMAND_DRAW) {
		index = -1;
		PROTO_Parse(context, (protoPtr) & index, "<bytepos1>", PROTO_PARSE_INTEGER);
		ptr->drawindex1 = index;
		channels++;
		index = -1;
		PROTO_Parse(context, (protoPtr) & index, "<bytepos2>", PROTO_PARSE_INTEGER | PROTO_PARSE_OPTIONAL);
		ptr->drawindex2 = index;
		if (index >= 0)
			channels++;
		index = -1;
		PROTO_Parse(context, (protoPtr) & index, "<bytepos3>", PROTO_PARSE_INTEGER | PROTO_PARSE_OPTIONAL);
		ptr->drawindex3 = index;
		if (index >= 0)
			channels++;
	}

	PROTO_RegisterProcessCallback(context, PROTO_Process, (protoPtr) ptr, sizeof(usbTraceEntry), 1);

	PROTO_RegisterDisplayCallback(context, PROTO_Display, (protoPtr) ptr, 1);

	PROTO_RegisterChartCallback(context, PROTO_Chart, (protoPtr) ptr, 1);

	PROTO_RegisterFindCallback(context, PROTO_Find, (protoPtr) ptr, 1);

	PROTO_RegisterDrawCallback(context, PROTO_Draw, (protoPtr) ptr, 1, channels, 0, 255);

	PROTO_RegisterShareCallback(context, PROTO_Share, (protoPtr) ptr, 0);

	return PROTO_OK;
}