#define _CRT_SECURE_NO_WARNINGS 1

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


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

	SWD Protocol Level 1

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

typedef struct
{
    protoTime time;
    unsigned int bit;
}
swdTraceEntry1;


#define SCK()	(tbuffer[0])
#define SDIO()	(tbuffer[1])

static protoSizeT PROTOAPI PROTO_Process1(protoContext context, protoPtr arrayOut, protoSizeT arrayOutSize, protoPtr arrayIn, protoSizeT arrayInSize, protoPtr localdata)
{
    protoSizeT      indexin, indexout;
    protoTime       time;
    int             tbuffer[2],oldSck;
    swdTraceEntry1  *swdBuf;

    swdBuf = (swdTraceEntry1 *) arrayOut;

    indexout = 0;
    PROTO_ReadTrace(context, 0, &time, tbuffer);
    oldSck = SCK();

    for (indexin = 1; 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, indexin, &time, tbuffer);

	if (SCK() && !oldSck)
	{
	    swdBuf[indexout].time = time;
	    swdBuf[indexout].bit  = SDIO();
	    indexout++;
	}
	oldSck = SCK();
    }
    return indexout;
}

static void PROTOAPI PROTO_Display1(protoContext context, protoPtr pdata, protoPtr localdata)
{
    swdTraceEntry1 *data = (swdTraceEntry1 *) pdata;
    char            buf[100];

    PROTO_Puts(context, "  ");

    strcpy(buf, "SDIO=x");
    buf[5] = '0' + !!(data->bit);
    PROTO_Puts(context, buf);
}


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

	SWD Protocol Level 2

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


/* SWD states */

#define SWD_UNKNOWN 0
#define SWD_RESET   1
#define SWD_IDLE    2
#define SWD_HEADER  3
#define SWD_ACK     4
#define SWD_READ    5
#define SWD_WRITE   6

#define SWD_STOP_ERROR 0x0100
#define SWD_PAR_ERROR  0x0200
#define SWD_ACK_ERROR  0x0400
#define SWD_ERROR_MASK 0xFF00

static const char *swdstate[8] = {
    "UNKNOWN", "RESET", "IDLE", "HEADER", "ACK", "READ", "WRITE", "XXX"
};

typedef struct
{
    protoTime time;
    int state;
    unsigned int data;
}
swdTraceEntry2;

static protoSizeT PROTOAPI PROTO_Process2(protoContext context, protoPtr arrayOut, protoSizeT arrayOutSize, protoPtr arrayIn, protoSizeT arrayInSize, protoPtr localdata)
{
    protoSizeT      indexin, indexout;
    int             state,prevState,oneCnt,zeroCnt,clkCnt,bit;
    unsigned int    data,hdr,ack,xxx;
    swdTraceEntry1 *bufferin;
    swdTraceEntry2 *bufferout;
    protoTime       delta,     /* delta-time between two timestemps */
                    mintime;   /* minimum time between two bufferin entries */

    bufferin  = (swdTraceEntry1 *) arrayIn;
    bufferout = (swdTraceEntry2 *) arrayOut;


    indexin  = 0;
    indexout = 0;
    data = 0;

    state = SWD_IDLE;
    prevState = SWD_IDLE;
    oneCnt = 0;
    zeroCnt = 0;
    clkCnt = 0;

    mintime = PROTO_TIME_ONESECOND;
    bufferout[0].time = bufferin[0].time;
    bufferout[0].state = state;
    for(;;)
    {
	if ((indexin & 0xfff) == 0)
	    if (PROTO_Cancel(context))
		return PROTO_PROCESS_CANCEL;

	bit = bufferin[indexin].bit;
	indexin++;
	if (indexin>=arrayInSize)
	    break;

	/* delta-time between this and last record */
	delta = PROTO_TIME_SUB(bufferin[indexin].time, bufferin[indexin-1].time);
	if (PROTO_TIME_ISSMALLER(delta, mintime))
	    mintime = delta;

	if (bit)
	{
	    oneCnt++;
	    zeroCnt = 0;
	}
	else
	{
	    zeroCnt++;
	    oneCnt = 0;
	}
	clkCnt++;
	data>>=1;
	if (bit)
	    data|=0x80000000;

	switch(state&0x7)
	{
	case SWD_UNKNOWN:
	    if (zeroCnt>=48)
		state = SWD_IDLE;
	    else if (PROTO_TIME_ISSMALLER(PROTO_TIME_MULTINT(mintime,32),delta))
	    {
		/* Detected gap which is 32 times larger than the minimum bit width. */
		/* Assume that we are now in idle   */
		state = SWD_IDLE;
		if (bit)
		{
		    /* If we have a '1', we are now in SWD_HEADER */
		    state = SWD_HEADER;
		    clkCnt = 0;
		    data = 0;
		}
	    }
	    break;
	case SWD_RESET:
	    if (!bit)
		state = SWD_IDLE;
	    break;
	case SWD_IDLE:
	    if (bit)
	    {
		state = SWD_HEADER;
		clkCnt = 0;
		data = 0;
	    }
	    break;
	case SWD_HEADER:
	    if (clkCnt==8)
	    {
		unsigned int par;
		hdr=(data>>24)&0x7F;
		if ((hdr&0x60)!=0x40)
		{
		    prevState|=SWD_STOP_ERROR;
		    state=SWD_UNKNOWN;
		    break;
		}
		par=(hdr)^(hdr<<2);
		par^=(par<<1);
		par<<=1;
		par&=0x10;
		if ((hdr&0x10)!=par)
		{
		    prevState|=SWD_PAR_ERROR;
		    state=SWD_UNKNOWN;
		    break;
		}
		state=SWD_ACK;
		clkCnt = 0;
		data = 0;
	    }
	    break;
	case SWD_ACK:
	    if (hdr&0x2)
	    {
		/* READ */
		if (clkCnt==3)
		{
		    ack = data>>29;
		    if (ack==0x1)
		    {
			state = SWD_READ;
			clkCnt = 0;
			data = 0;
		    }
		}
		/* READ with not OK response */
		if (clkCnt>=4)
		{
		    state = SWD_IDLE;
		    clkCnt = 0;
		    data = 0;
		}
	    }
	    else
	    {
		/* WRITE */
		if (clkCnt==4)
		{
		    ack = (data>>28)&0x7;
		    if (ack==0x1)
			state = SWD_WRITE;
		    else
			state = SWD_IDLE;
		    clkCnt = 0;
		    data = 0;
		}
	    }
	    if (clkCnt==0 && ack!=0x1 && ack!=0x2 && ack!=0x4)
	    {
		/* Illegal ACK code */
		prevState|=SWD_ACK_ERROR;
		state = SWD_UNKNOWN;
	    }
	    break;
	case SWD_READ:
	    if (clkCnt==32)
		xxx=data;
	    if (clkCnt==33)
	    {
		unsigned int par;
		par=xxx^(xxx<<16);
		par^=(par<<8);
		par^=(par<<4);
		par^=(par<<2);
		par^=(par<<1);
		par&=0x80000000;
		data&=0x80000000;
		if ( par != data)
		    prevState|=SWD_PAR_ERROR;
	    }
	    if (clkCnt==34)
	    {

		state = SWD_IDLE;
		clkCnt = 0;
		data = 0;
	    }
	    break;
	case SWD_WRITE:
	    if (clkCnt==32)
		xxx=data;
	    if (clkCnt==33)
	    {
		unsigned int par;
		par=xxx^(xxx<<16);
		par^=(par<<8);
		par^=(par<<4);
		par^=(par<<2);
		par^=(par<<1);
		par&=0x80000000;
		data&=0x80000000;
		if ( par != data)
		    prevState|=SWD_PAR_ERROR;

		state = SWD_IDLE;
		clkCnt = 0;
		data = 0;
	    }
	    break;
	default :
	    state = SWD_UNKNOWN;
	}
	if (oneCnt>=48)
	    state = SWD_RESET;

	if (state!=(prevState&0x7))
	{
	    /* store info from previous state into current bufferout record */
	    bufferout[indexout].state=prevState;
	    switch(prevState&0x7)
	    {
		case SWD_HEADER:
		    bufferout[indexout].data = hdr;
		    break;
		case SWD_ACK:
		    bufferout[indexout].data = ack;
		    break;
		case SWD_READ:
		case SWD_WRITE:
		    bufferout[indexout].data = xxx;
		    break;
		default:
		    ;
	    }

	    /* Now record time for next state */
	    indexout++;
	    if (indexout >= arrayOutSize)
		return PROTO_PROCESS_OUTOFMEMORY;
	    bufferout[indexout].time=bufferin[indexin].time;
	}
	prevState = state;
    }
    return indexout;
}

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

    PROTO_Puts(context,swdstate[(data->state)&0x7]);
    switch(data->state&0x7)
    {
    case SWD_HEADER:
	PROTO_Printf(context," 0x%02x ",data->data&0xFF);
	if (data->state & SWD_STOP_ERROR)
	    PROTO_Puts(context,"incorrect stop/park bit");
	else if (data->state & SWD_PAR_ERROR)
	    PROTO_Puts(context,"incorrect parity");
	else if ((data->state & SWD_ERROR_MASK) == 0)
	{
	    if (data->data & 0x1)
		PROTO_Puts(context,"APACC ");
	    else
		PROTO_Puts(context,"DPACC ");
	    if (data->data & 0x2)
		PROTO_Puts(context,"READ  ");
	    else
		PROTO_Puts(context,"WRITE ");
	    PROTO_Printf(context,"0x%x",((data->data)&0xC));
	}
	break;
    case SWD_ACK:
	PROTO_Printf(context," 0x%x ",data->data&0xF);
	if (data->state & SWD_ACK_ERROR)
	    PROTO_Puts(context,"incorrect ACK");
	else if ((data->state & SWD_ERROR_MASK) == 0)
	{
	    if (data->data==0x1)
		PROTO_Puts(context,"OK");
	    else if (data->data==0x2)
		PROTO_Puts(context,"WAIT");
	    else if (data->data==0x4)
		PROTO_Puts(context,"FAULT");
	}
	break;
    case SWD_READ:
    case SWD_WRITE:
	PROTO_Printf(context," 0x%08x ",data->data);
	if (data->state & SWD_PAR_ERROR)
	    PROTO_Puts(context,"parity !WRONG!");
	else
	    PROTO_Puts(context,"parity OK");
	break;
    default:
	break;
    }
}



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

	SWD Protocol Level 3

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


typedef struct
{
    protoTime time;
    unsigned int hdr;
    unsigned int data;
}
swdTraceEntry3;

static protoSizeT PROTOAPI PROTO_Process3(protoContext context, protoPtr arrayOut, protoSizeT arrayOutSize, protoPtr arrayIn, protoSizeT arrayInSize, protoPtr localdata)
{
    protoSizeT     indexin, indexout;
    swdTraceEntry2 *bufferin;
    swdTraceEntry3 *bufferout;

    bufferin  = (swdTraceEntry2 *) arrayIn;
    bufferout = (swdTraceEntry3 *) arrayOut;
    indexout = 0;

    for(indexin=0;indexin<arrayInSize;indexin++)
    {
	if ((indexin & 0xfff) == 0)
	    if (PROTO_Cancel(context))
		return PROTO_PROCESS_CANCEL;

	if ((bufferin[indexin].state&0x7)==SWD_HEADER)
	{
	    bufferout[indexout].time=bufferin[indexin].time;
	    bufferout[indexout].hdr=(bufferin[indexin].data&0xFF);
	}
	if ( ((bufferin[indexin].state&0x7)==SWD_READ) || ((bufferin[indexin].state&0x7)==SWD_WRITE) )
	{
	    bufferout[indexout].hdr|=(bufferin[indexin].state&SWD_ERROR_MASK);
	    bufferout[indexout].data=bufferin[indexin].data;
	    indexout++;
	    if (indexout >= arrayOutSize)
		return PROTO_PROCESS_OUTOFMEMORY;
	}
    }
    return indexout;
}

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

    if (data->hdr & 0x1)
	PROTO_Puts(context,"APACC ");
    else
	PROTO_Puts(context,"DPACC ");
    if (data->hdr & 0x2)
    {
	PROTO_Puts(context,"READ  ");
	PROTO_Printf(context,"0x%x -> ",((data->hdr)&0xC));
    }
    else
    {
	PROTO_Puts(context,"WRITE ");
	PROTO_Printf(context,"0x%x <- ",((data->hdr)&0xC));
    }

    PROTO_Printf(context," 0x%08x ",data->data);
    if (data->hdr & SWD_PAR_ERROR)
	PROTO_Puts(context,"parity !WRONG!");
}

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

	SWD Protocol Parser

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

int PROTOAPI PROTO_Init(protoContext context, int command)
{
    PROTO_Parse(context, (protoPtr) 0, "<sck>", PROTO_PARSE_CHANNEL);
    PROTO_Parse(context, (protoPtr) 0, "<sdio>", PROTO_PARSE_CHANNEL);

    PROTO_RegisterProcessCallback(context, PROTO_Process1, (protoPtr) NULL, sizeof(swdTraceEntry1), 1);

    PROTO_RegisterDisplayCallback(context, PROTO_Display1, (protoPtr) NULL, 1);

    PROTO_RegisterProcessCallback(context, PROTO_Process2, (protoPtr) NULL, sizeof(swdTraceEntry2), 2);

    PROTO_RegisterDisplayCallback(context, PROTO_Display2, (protoPtr) NULL, 2);

    PROTO_RegisterProcessCallback(context, PROTO_Process3, (protoPtr) NULL, sizeof(swdTraceEntry3), 3);

    PROTO_RegisterDisplayCallback(context, PROTO_Display3, (protoPtr) NULL, 3);

    PROTO_SetDefaultLevel(context, 3);

    return PROTO_OK;
}