#include <vsrv.h>
#include <consolestate.h>
#include <ringbuf.h>
#include <sysmemory.h>
#include <kernel.h>
#include <string.h>
#include <ctype.h>
#include <imem.h>

#include "fifobuf.h"
#include "fpga_i2s.h"

#include <volink.h>


struct fifoBuf *audioFifo = NULL;

u_int16 audio32b = 0;
#define PACKET_BUFFER_SIZE (4095)
#define PACKET_COUNT ((PACKET_BUFFER_SIZE / 32) + 1)

struct recBuf {
	u_int16 *wr;          /* Write pointer */
        u_int16 *rd;          /* Read pointer  */
        u_int16 *buf;         /* Buffer start  */
        u_int16 buffSize;     /* Buffer size   */
        u_int16 modifier;     /* modifier for buf */
	u_int16 *wrS;         /* Packet size WR   */
	u_int16 *rdS;         /* Packet size RD   */
	u_int16 *sizeBuf;     /* Packet Size buf  */
	u_int16 sizeBuffSize; /* Packet size size */
	u_int16 sizeModifier; /* Packet size mod  */
	__mem_y u_int16 *ethStart;
};

__align u_int16 sizeBuf[PACKET_COUNT];

__align u_int16 pktBuf[PACKET_BUFFER_SIZE];

u_int16 header[32];
u_int16 interrupts = 0;
struct interruptStats {
	u_int32 ok;
	u_int32 nok;
	u_int16 of;
};
struct interruptStats stats;

struct metaHeader {
	u_int16 bytes;
	u_int16 flags;
	u_int32 ts;
	u_int16 ipSrc[2];
	u_int16 ipDst[2];
	u_int16 udpSrc;
	u_int16 udpDst;
	u_int16 rtpOffset;
	u_int16 rtpSequence;
	u_int32 rtpTS;
	u_int32 rtpSynSrc;
	u_int16 rtpPad;
	u_int16 udpSize;
	u_int16 rx_filter;
	u_int16 rx_modify;
	u_int16 rx_vlan;
	u_int16 rx_format;
	u_int16 rx_mcast[4];
};
struct recBuf rcvPkts = {
	pktBuf,pktBuf,pktBuf,
	PACKET_BUFFER_SIZE,
	MAKEMODF(PACKET_BUFFER_SIZE),
	sizeBuf,sizeBuf,sizeBuf,
	PACKET_COUNT,
	MAKEMODF(PACKET_COUNT),
	(__mem_y u_int16*)ETHRAM_START
};
/* count of packets in buffer */
u_int16 PacketCount(register __i2 struct recBuf  *rcv) {
	register s_int16 count = rcv->wrS - rcv->rdS;
	if (count < 0) {
		count += rcv->sizeBuffSize;
	}
	return count;
}

/* How many bytes available in packet buffer */
u_int16 BufferSpace(register __i2 struct recBuf  *rcv) {
	register s_int16 bytes = rcv->rd - rcv->wr;
	if (bytes <=  0) {
		bytes += rcv->buffSize;
	}
	bytes -= 32 + 1; /* Reserve for header */
	if (bytes <= 0) {
		return 0;
	}
	return bytes << 1;
}

u_int16 GetPacketBytes(register __i2 __mem_y u_int16 *ethStart) {
	u_int16 bytes;
	printf("Bytes ");
	bytes = *ethStart++; /* This I want */
	printf("%u ", bytes);
	bytes = *ethStart++; /* Delay */
	printf("%u ", bytes);
	bytes = *ethStart++; /* Delay */
	printf("%u\n", bytes);
	return bytes;
}
void Dummy(register u_int16 d) {

}
/* 17 + bytes/2 clocks */
void CopyPacket(register __i2 struct recBuf  *rcv,
		register __c0 u_int16 byteCount) {
	Dummy(*(rcvPkts.ethStart));
	Dummy(*(rcvPkts.ethStart + 1));

	rcvPkts.wr = RING_YX_D(ringcpyYX(rcvPkts.wr, rcvPkts.modifier, rcvPkts.ethStart + 2, 1, ((byteCount + 64 + 1) >> 1)));
	*(rcvPkts.wrS) = byteCount;
	rcvPkts.wrS = ringmodX(rcvPkts.wrS, rcvPkts.sizeModifier);
}
void IntEthC(void) {
	__mem_y u_int16 *eth = (__mem_y u_int16*)ETHRAM_START;
	struct metaHeader *h = (struct metaHeader *)header;
	u_int16 words;
	Dummy(*eth++);
	Dummy(*eth++);
	memcpyYX(header, eth, 32);
	if (h->flags == 0x001f) {
		stats.ok++;
		h->bytes -= 4; /* - CRC */
		words = (h->bytes +1) >> 1;
		eth = (__mem_y u_int16*)ETHRAM_START + h->rtpOffset;
		Dummy(*eth++);
		Dummy(*eth++);
		FifoWriteY(audioFifo, eth, words);
	} else {
		stats.nok++;
	}
	if (PERIP(VSRV1_CF) & VSRV1_CF_ETHOVRRUN) {
		stats.of++;
		PERIP(VSRV1_CF) ^= VSRV1_CF_ETHOVRRUN;
	}
	PERIP(VSRV1_CF) |= VSRV1_CF_ETHRDST;
}
void IntI2sC(void) {
	static u_int16 sBuf[2];
	if (audio32b) {
		if (FifoReadY(audioFifo, (__mem_y void *)I2S_LEFT_LSB, 4) == 0) {
			memsetY((__mem_y void *)I2S_LEFT_LSB, 0, 4);
		}
	} else {
		if (FifoRead(audioFifo, sBuf, 2)) {
			PERIP(I2S_LEFT) = sBuf[0];
			PERIP(I2S_RIGHT) = sBuf[1];
		} else {
			PERIP(I2S_LEFT) = 0;
			PERIP(I2S_RIGHT) = 0;
		}
	}
}
void ReadPacket() {
	/* Check for book keeping space */
	if (rcvPkts.sizeBuffSize - PacketCount(&rcvPkts) - 1) {
		u_int16 byteCount = GetPacketBytes(rcvPkts.ethStart);
		if (byteCount < BufferSpace(&rcvPkts)) {
			printf("B: %u\n", byteCount);
			CopyPacket(&rcvPkts, byteCount);
		} else {
			printf("!#OVB#! %u %u\n", byteCount, BufferSpace(&rcvPkts));
		}
	} else {
		printf("!#OVP#!\n");
	}
	printf("VSRV1: %04x INT_ORIGIN1: %04x\n", PERIP(VSRV1_CF), PERIP(INT_ORIGIN1));
	PERIP(VSRV1_CF) |= VSRV1_CF_ETHRDST;
}

s_int16 ReadFromFifo(register u_int16 *dest, register u_int16 bytes) {
	u_int16 words;
	if (dest == NULL) {
		static __mem_y u_int16 dump; /* X->Y copy could be faster */
		/* Drop next packet */
		bytes = *(rcvPkts.rdS);
		rcvPkts.rdS = ringmodX(rcvPkts.rdS, rcvPkts.sizeModifier);
		words = ((bytes + 1) >> 1) + 32;
		/* Mod dest, doesn't advance destination so we are
		   discarding data */
		rcvPkts.rd = RING_XY_S(ringcpyXY(&dump, 0, rcvPkts.rd,
						 rcvPkts.modifier, words));
		return -3;
	} else if (PacketCount(&rcvPkts) == 0) {
		return -2;
	} else if (*(rcvPkts.rdS) > bytes) {
		return -1;
	} else {
		/* Copy packet */
		bytes = *(rcvPkts.rdS);
		rcvPkts.rdS = ringmodX(rcvPkts.rdS, rcvPkts.sizeModifier);
		words = ((bytes + 1) >> 1) + 32;
		rcvPkts.rd = RING_XX_S(ringcpyXX(dest, 1, rcvPkts.rd,
						 rcvPkts.modifier, words));
		return bytes;
	}
}

void DumpPacket() {
	static u_int16 dumpBuf[760 + 32]; /*1520 bytes + 32 header */
	static char *fields[] = {
		"Bytes",  /*  0 */
		"Flags",  /*  1 */
		"Timestamp", /*  2 */
		"IP source", /*  4 */
		"IP destination", /*  5 */
		"UDP source", /*  8 */
		"UDP destination", /*  9 */
		"RTP offset", /* 10 */
		"RTP seqence", /* 11 */
		"RTP timestamp", /* 12 */
		"RTP sync", /* 14 */
		"RTPpad", /* 16 */
		"UDPsize", /* 17 */
		"rx_filter",/* 18 */
		"rx_modify", /* 19 */
		"rx_vlan", /* 20*/
		"rx_format", /* 21 */
		"rx_mcast" /*22..25 */
	};
	char **f = fields;
	u_int16 *s = dumpBuf, i, words;
	s_int16 bytes;
	struct metaHeader *met = (struct metaHeader*)dumpBuf;
	bytes = ReadFromFifo(dumpBuf, (760 + 32) * 2);
	if ((bytes <= 0)) {
		return;
	}
	if (dumpBuf[1] != 0x1f) {
		printf("BLAH! 0x%04x\n", met->flags);
		for (i = 0; i < 16; i++) {
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);

			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);

			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);

			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);

			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x ", *s++);
			printf("%04x\n", *s++);

		}
		return;
	}
	printf("%s: %u\n", *f++, met->bytes);
	printf("%s: 0x%04x\n", *f++, met->flags);
	printf("%s: %lu\n", *f++, met->ts);
	printf("%s: %u.%u.%u.%u\n", *f++,
	       met->ipSrc[0] >> 8, met->ipSrc[0] & 0xff,
	       met->ipSrc[1] >> 8, met->ipSrc[1] & 0xff);
	printf("%s: %u.%u.%u.%u\n", *f++,
	       met->ipDst[0] >> 8, met->ipDst[0] & 0xff,
	       met->ipDst[1] >> 8, met->ipDst[1] & 0xff);
	printf("%s: %u\n", *f++, met->udpSrc);
	printf("%s: %u\n", *f++, met->udpDst);
	printf("%s: %u\n", *f++, met->rtpOffset);
	printf("%s: %u\n", *f++, met->rtpSequence);
	printf("%s: %lu\n", *f++, met->rtpTS);
	printf("%s: %lu\n", *f++, met->rtpSynSrc);
	printf("%s: %u\n", *f++, met->rtpPad);
	printf("%s: %u\n", *f++, met->udpSize);

	printf("%s: %04x\n", *f++, met->rx_filter);
	printf("%s: %04x\n", *f++, met->rx_modify);
	printf("%s: %04x\n", *f++, met->rx_vlan);
	printf("%s: %04x\n", *f++, met->rx_format);
	for (i = 0; i < 3; i++) {
		printf("%s: %04x\n", *f,  met->rx_mcast[i]);
	}

	/*
	printf("RfF: %d\n", bytes);
	for (i = 0; i < 18; i++) {
		printf("%04x %s\n", *s++, *f++);
	}
	*/


#if 1
	s = dumpBuf + 32;
	words = ((bytes + 1) >> 1);
	if (words & 0x7) {
		words += 8;
	}
	words >>=3;
	bytes = 0;
	for (i = 0; i < words; i++) {
		u_int16 lo, hi;
		printf("%04x %04x ", bytes, *s++); /* GNAH. word addressing. reuse variable. */
		printf("%04x ", *s++);
		printf("%04x ", *s++);
		printf("%04x ", *s++);

		printf("%04x ", *s++);
		printf("%04x ", *s++);
		printf("%04x ", *s++);
		printf("%04x\t", *s++);
		s -= 8;
		bytes += 8;
		lo = *s&0xff; hi = *s++ >> 8;printf("%c%c", isprint(hi)?hi:'.', isprint(lo)?lo:'.');
		lo = *s&0xff; hi = *s++ >> 8;printf("%c%c", isprint(hi)?hi:'.', isprint(lo)?lo:'.');
		lo = *s&0xff; hi = *s++ >> 8;printf("%c%c", isprint(hi)?hi:'.', isprint(lo)?lo:'.');
		lo = *s&0xff; hi = *s++ >> 8;printf("%c%c\t", isprint(hi)?hi:'.', isprint(lo)?lo:'.');

		lo = *s&0xff; hi = *s++ >> 8;printf("%c%c", isprint(hi)?hi:'.', isprint(lo)?lo:'.');
		lo = *s&0xff; hi = *s++ >> 8;printf("%c%c", isprint(hi)?hi:'.', isprint(lo)?lo:'.');
		lo = *s&0xff; hi = *s++ >> 8;printf("%c%c", isprint(hi)?hi:'.', isprint(lo)?lo:'.');
		lo = *s&0xff; hi = *s++ >> 8;printf("%c%c\n", isprint(hi)?hi:'.', isprint(lo)?lo:'.');

	}
#endif
}
extern u_int16 IntEthVector;
extern u_int16 IntI2sVector;

ioresult main(char *params) {
	__mem_y u_int16 *e;
	u_int16 t = 1;
	printf("Graah 1, 0x1002 = %04x!\n", USEX(0x1002)); Delay(100);
	PERIP(INT_ENABLE1_HP) &= ~(INTF1_ETH | INTF1_I2S);
	PERIP(INT_ENABLE1_LP) &= ~(INTF1_ETH | INTF1_I2S);
	printf("Graah 2, 0x1002 = %04x!\n", USEX(0x1002)); Delay(100);

	/* FIFO */
	audioFifo = FifoRealloc(NULL, 3, 0);
	if (audioFifo == NULL) {
		return S_ERROR;
	}
	printf("Graah 3, 0x1002 = %04x!\n", USEX(0x1002)); Delay(100);

	audioFifo->wr.x = pktBuf;
	audioFifo->rd.x = pktBuf;
	audioFifo->buf.x = pktBuf;
	audioFifo->bufSize = PACKET_BUFFER_SIZE;
	//audioFifo = FifoRealloc(NULL, PACKET_BUFFER_SIZE, 0);
	printf("Graah 4, 0x1002 = %04x!\n", USEX(0x1002)); Delay(100);
#if 0
	if (audioFifo == NULL) {
		printf("fifoAlloc fail\n");
		return S_ERROR;
	}
	t = (FIFO_READ_NON_BLOCK | FIFO_WRITE_NON_BLOCK | FIFO_WRITE_OVER);
	FifoFeatures(audioFifo, &t, NULL);
	t = 2; /* Stereo samples */
	FifoFeatures(audioFifo, NULL, &t);
	//printf("blaah");

	/* ETH MEM */
	e = AllocMemAbsY(ETHRAM_START, ETHRAM_SIZE);
	t = 1;
	if (e == NULL) {
		printf("Someone planted bad things in my memory\nGiving up\n");
		return S_ERROR;
	}
	printf("ETH Start: %p\n", rcvPkts.ethStart);
	memsetY(e, 0, ETHRAM_SIZE);
	Delay(1000);
	PERIP(CF_SYSTEM1) |= CF_SYSTEM1_YETH_SEL;
	printf("INT_ENA0 %04x %04x\n", PERIP(INT_ENABLE0_LP), PERIP(INT_ENABLE0_HP));
	printf("INT_ENA1 %04x %04x\n", PERIP(INT_ENABLE1_LP), PERIP(INT_ENABLE1_HP));
	printf("VSRV1: %04x INT_ORIGIN1: %04x\n", PERIP(VSRV1_CF), PERIP(INT_ORIGIN1));
	PERIP(VSRV1_CF) |= VSRV1_CF_ETHRDENA;
	printf("VSRV1: %04x INT_ORIGIN1: %04x\n", PERIP(VSRV1_CF), PERIP(INT_ORIGIN1));
	/* Packet memory accessible */

	/* I2S */
	PERIP(I2S_CF) = 0;
	PERIP(I2S_CF) = (I2S_CF_MASTER | I2S_CF_ENA | I2S_CF_MODE | I2S_CF_INTENA | I2S_CF_FS48K16B);
	WriteIMem((0x20+INTV_ETH), ReadIMem((u_int16)(&IntEthVector)));
	WriteIMem((0x20+INTV_I2S), ReadIMem((u_int16)(&IntI2sVector)));
	PERIP(INT_ENABLE1_HP) |= INTF1_I2S;
	while((appFlags & APP_FLAG_QUIT) == 0) {
		if (ioctl(stdin, IOCTL_TEST, 0)) {
			u_int16 ch = fgetc(stdin);
			switch(ch) {
			case 'a':
				PERIP(VSRV1_CF) |= VSRV1_CF_ETHRDST;
				break;
			case 'b':
				printf("Bw: %p Br: %p BSw: %p BSr: %p\n",
				       rcvPkts.wr, rcvPkts.rd, rcvPkts.wrS, rcvPkts.rdS);
				break;
			case 'o':
				PERIP(VSRV1_CF) ^= VSRV1_CF_ETHOVRRUN;
				break;
			case 'g':
				PERIP(VSRV1_CF) |= VSRV1_CF_ETHRDENA;
				break;
			case 'r':
				PERIP(VSRV1_CF) &= ~VSRV1_CF_ETHRDENA;
				break;
			case 'i':
				PERIP(VSRV1_CF) &= ~VSRV1_CF_INT;
				break;
			case '!':
				PERIP(INT_ENABLE1_HP) ^= INTF1_ETH;
				break;
			case '1':
				PERIP(VSRV1_CF) |= VSRV1_CF_ETHMEMTST * 2;
				break;
			case '0':
				PERIP(VSRV1_CF) |= VSRV1_CF_ETHMEMTST;
				break;
			case 'z':
				PERIP(VSRV1_CF) &= ~(3 * VSRV1_CF_ETHMEMTST);
				break;
			case 'd':
			{
				__mem_y u_int16 *d = (__mem_y void*)ETHRAM_START, i;
				printf("IO: %04x T: %u\n", PERIP(INT_ORIGIN1) & INTF1_ETH, t);
				for (i = 0; i < 8; i++) {
					printf("%04x ", *d++);
					printf("%04x ", *d++);
					printf("%04x ", *d++);
					printf("%04x ", *d++);
					printf("%04x ", *d++);
					printf("%04x ", *d++);
					printf("%04x ", *d++);
					printf("%04x\n", *d++);
				}
				if ( (PERIP(INT_ORIGIN1) & INTF1_ETH)) {
					PERIP(INT_ORIGIN1) = INTF1_ETH;
					PERIP(VSRV1_CF) |= VSRV1_CF_ETHRDST;
				}
				break;
			}
			case 'p':
				DumpPacket();
				break;
			case 's':
				//PERIP(CF_SYSTEM1) ^= CF_SYSTEM1_YETH_SEL;
				printf("ETH_SEL %x\n", PERIP(CF_SYSTEM1) & CF_SYSTEM1_YETH_SEL);
				break;
			case 't':
				t ^= 1;
				printf("T: %u\n", t);
				break;
			}
			printf("%c VSRV1: %04x %04x %u %u\n", ch, PERIP(VSRV1_CF), PERIP(INT_ENABLE1_HP), interrupts, PacketCount(&rcvPkts));
			printf("G: %lu B: %lu O: %lu\n", stats.ok, stats.nok, stats.of);
			printf("UF: %lu OF: %lu\n", audioFifo->underflows, audioFifo->overflows);
		}
		if (t && (PERIP(INT_ORIGIN1) & INTF1_ETH)) {
			printf("INT\n");
			PERIP(INT_ORIGIN1) = INTF1_ETH;
			ReadPacket();
			DumpPacket();
		}

	}
	PERIP(INT_ENABLE1_HP) &= ~(INTF1_ETH | INTF1_I2S);
	PERIP(INT_ENABLE1_LP) &= ~(INTF1_ETH | INTF1_I2S);
	PERIP(VSRV1_CF) &= ~VSRV1_CF_ETHRDENA;
	PERIP(CF_SYSTEM1) &= ~CF_SYSTEM1_YETH_SEL;
	FreeMemY(e, ETHRAM_SIZE);
	audioFifo = FifoRealloc(audioFifo, 0, 0);
#endif
	/* Packet memory gone */
	//printf("Goodbye world!\n");
	return S_OK;
}
