/* s3load.c
 * Main program for s3load
 * s3load, a free SVF loader for Digilent NEXYS2 boards
 *
 * Copyright (c) 2007-2008 Joshua Wise
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License.
 *
 * Commercial licenses are available by request.
 */

#include "libnexys.h"
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <string.h>
#include <assert.h>

enum TAPSTATE {
	TAP_UNKNOWN,
	TAP_RESET,
	TAP_IDLE,
	TAP_DRSEL,
	TAP_DRCAP,
	TAP_DRSHIFT,
	TAP_DREXIT1,
	TAP_DRPAUSE,
	TAP_DREXIT2,
	TAP_DRUPDATE,
	TAP_IRSEL,
	TAP_IRCAP,
	TAP_IRSHIFT,
	TAP_IREXIT1,
	TAP_IRPAUSE,
	TAP_IREXIT2,
	TAP_IRUPDATE,
};

typedef struct tap {
	nexys2_t nexys;
	int tapstate;
} tap_t;

int s3l_tap_reset(tap_t *tap)
{
	unsigned char reset[] = {0xAA, 0x02};
	tap->tapstate = TAP_RESET;
	return nexys2_jtag_puttmstdi(tap->nexys, 5, reset, NULL);
}

int s3l_tap_idle(tap_t *tap)
{
	switch (tap->tapstate)
	{
	case TAP_UNKNOWN:
		printf("Resetting TAP from unknown state...\n");
		s3l_tap_reset(tap);
	case TAP_RESET:
	{
		unsigned char idle[] = {0x00};
		printf("Placing TAP in idle state...\n");
		tap->tapstate = TAP_IDLE;
		return nexys2_jtag_puttmstdi(tap->nexys, 1, idle, NULL);
	}
	case TAP_IDLE:
		return 0;
	default:
		printf("cannot transition to idle from %d\n", tap->tapstate);
		abort();
	}
}

enum TOKENTYPE {
	TOK_TRST = 0,
	TOK_OFF = 1,
   	TOK_ENDIR = 2,
	TOK_IDLE = 3,
	TOK_ENDDR = 4,
	TOK_STATE = 5,
	TOK_RESET = 6,
	TOK_FREQUENCY = 7,
	TOK_1E6 = 8,
	TOK_HZ = 9,
	TOK_SEMICOLON = 10,
	TOK_TIR = 11,
	TOK_HIR = 12,
	TOK_TDR = 13,
	TOK_HDR = 14,
	TOK_SIR = 15,
	TOK_SDR = 16,
	TOK_TDI = 17,
	TOK_TDO = 18, 
	TOK_MASK = 19,
	TOK_SMASK = 20,
	TOK_NUMBER = 21,
	TOK_BITSTRING = 22,
	TOK_RUNTEST = 23,
	TOK_TCK = 24,
};

typedef struct token {
	int token;
	union {
		int i;
		char *s;
	} data;
	struct token *next;
} token_t;

int hex2nyb(char c)
{
	if (c >= '0' && c <= '9')
		return c-'0';
	if (c >= 'a' && c <= 'f')
		return c-'a'+10;
	if (c >= 'A' && c <= 'F')
		return c-'A'+10;
	printf("Bad hex nybble %c\n", c);
	abort();
}

token_t *tokenize(int fd)
{
	/* First off, suck in ALL of the file */
	char *f = NULL, *fst;
	int sz = 0;
	token_t *start = NULL, *cur = NULL;
	
	while (1)
	{
		int rsz;
		f = realloc(f, sz + 16385);
		if ((rsz = read(fd, &(f[sz]), 16384)) < 16384)
		{
			sz += rsz;
			break;
		}
		sz += rsz;
	}
	f[sz] = 0;
	fst = f;
	
	while (f && *f)
	{
		if (!strncmp(f, "//", 2))
		{
			// Comment, eat the rest of the line
			f = strchr(f, '\n');
			if (f && f[1] == '\r')
				f += 2;
			else if (f)
				f++;
		} else if (*f == '\n' || *f == '\r' || *f == '\t' || *f == ' ')
			f++;
#define NEWTOKEN \
	if (!cur) \
	{ \
		start = cur = malloc(sizeof(*cur)); \
		start->next = NULL; \
	} else { \
		cur->next = malloc(sizeof(*cur)); \
		cur = cur->next; \
		cur->next = NULL; \
	}
#define MATCHSTRING(str, tok) \
	if (!strncmp(f, str, strlen(str))) { \
		NEWTOKEN \
		cur->token = tok; \
		f += strlen(str); \
	}
		else MATCHSTRING("TRST", TOK_TRST)
		else MATCHSTRING("OFF", TOK_OFF)
		else MATCHSTRING("ENDIR", TOK_ENDIR)
		else MATCHSTRING("IDLE", TOK_IDLE)
		else MATCHSTRING("ENDDR", TOK_ENDDR)
		else MATCHSTRING("STATE", TOK_STATE)
		else MATCHSTRING("RESET", TOK_RESET)
		else MATCHSTRING("FREQUENCY", TOK_FREQUENCY)
		else MATCHSTRING("1E6", TOK_1E6)
		else MATCHSTRING("HZ", TOK_HZ)
		else MATCHSTRING(";", TOK_SEMICOLON)
		else MATCHSTRING("TIR", TOK_TIR)
		else MATCHSTRING("HIR", TOK_HIR)
		else MATCHSTRING("TDR", TOK_TDR)
		else MATCHSTRING("HDR", TOK_HDR)
		else MATCHSTRING("TDI", TOK_TDI)
		else MATCHSTRING("SMASK", TOK_SMASK)
		else MATCHSTRING("SIR", TOK_SIR)
		else MATCHSTRING("MASK", TOK_MASK)
		else MATCHSTRING("TDO", TOK_TDO)
		else MATCHSTRING("SDR", TOK_SDR)
		else MATCHSTRING("RUNTEST", TOK_RUNTEST)
		else MATCHSTRING("TCK", TOK_TCK)
		else if (isdigit(*f)) {
			NEWTOKEN
			cur->token = TOK_NUMBER;
			cur->data.i = *f - '0';
			f++;
			while (isdigit(*f))
			{
				cur->data.i *= 10;
				cur->data.i += *f - '0';
				f++;
			}
		} else if (*f == '(') {
			int asz = 0;
			int l = 0;
			NEWTOKEN
			cur->token = TOK_BITSTRING;
			cur->data.s = NULL;
			f++;
			while (*f && *f != ')')
			{
				char c1, c2;
				while (isspace(*f) && *f && *f != ')')
					f++;
				if (!*f || *f == ')')
					break;
				c1 = *(f++);
				
				while (isspace(*f) && *f && *f != ')')
					f++;
				if (!*f || *f == ')')
				{
					printf("Unpaired hex digit in bitstring\n");
					abort();
				}
				c2 = *(f++);
				
				if (l == asz)
				{
					asz += 128;
					cur->data.s = realloc(cur->data.s, asz);
				}
				cur->data.s[l++] = hex2nyb(c1) << 4 | hex2nyb(c2);
			} 
			f++;
		} else {
			printf("Tokenizing error, unhandled: %s\n", f);
			abort();
		}
	}
	
	free(fst);
	
	return start;
}

typedef struct blitval {
	int len;
	char *tdi, *tdo, *mask, *smask;
} blitval_t;

/* Copies bits from an input in format:
 *  (msb first) 0 1 2 3 4 5 6 7 | 8 9 10 11 12 13 14 15 | ...
 * To an output in format:
 *  7 6 5 4 3 2 1 0 | 15 14 13 12 11 10 9 8 | ...
 */
void bitcopy(int len, unsigned char *inbits, int inpos, int indir, unsigned char *outbits, int outpos, int outdir)
{
	int i;
	if (!inbits || !outbits)
		return;
	for (i = 0; i < len; i++)
	{
		int inbit;
		inbit = inbits[(inpos + i * indir) / 8] & (1 << (7 - ((inpos + i * indir) % 8)));
		outbits[(outpos + i * outdir) / 8] |= inbit ? (1 << ((outpos + i * outdir) % 8)) : 0;
	}
}

void printprog(int done, int total)
{
	int i;
	int progresscounter = done * 32 / total;
	static int spinnerstate;
	char *spinner = "_.oOo.";
	
	printf("[");
	for (i=0; i < progresscounter; i++)
		printf("=");
	if (progresscounter != 32)
		printf(">");
	for (i=0; i < (31 - progresscounter); i++)
		printf(" ");
	printf("] %c %d/%d", spinner[(spinnerstate++) % 6], done, total);
}

void pokethrough(tap_t *tap, blitval_t *tbv, blitval_t *sbv, blitval_t *hbv)
{
	int totlen = tbv->len + sbv->len + hbv->len;
	int lenb = (totlen / 4) + ((totlen % 4) != 0) + 1;	// Number of nexys2_* bytes
	unsigned char *inb = calloc(lenb, 1);			// Bitstream to be fed to nexys2_*
	unsigned char *outb = calloc(lenb, 1);
	unsigned char *expoutb = calloc(lenb, 1);
	int bitp = 0;					// Position in the "inb" bitstream
	int txp = 0;
	int needout = 0;
	
	if (!totlen)
		return;
	
	printf("t:%d, s:%d, h:%d\n", tbv->len, sbv->len, hbv->len);
	
	bitp = 0;
#define ROUNDUP(x) (((x / 8) + ((x % 8) != 0)) * 8 - 1)
	bitcopy(hbv->len, hbv->tdi, ROUNDUP(hbv->len), -1, inb, bitp*2, 2); bitp += hbv->len;
	bitcopy(sbv->len, sbv->tdi, ROUNDUP(sbv->len), -1, inb, bitp*2, 2); bitp += sbv->len;
	bitcopy(tbv->len, tbv->tdi, ROUNDUP(tbv->len), -1, inb, bitp*2, 2); bitp += tbv->len;
	inb[(bitp*2-2)/8] |= 2 << ((bitp*2-2) % 8);
	assert(bitp == totlen);
	
	if (sbv->tdo)
		needout = 1;
	bitp = 0;	/* Have to throw away one bit :( */
	bitcopy(tbv->len, tbv->tdo, ROUNDUP(tbv->len), -1, expoutb, bitp, 1); bitp += tbv->len;
	bitcopy(sbv->len, sbv->tdo, ROUNDUP(sbv->len), -1, expoutb, bitp, 1); bitp += sbv->len;
	bitcopy(hbv->len, hbv->tdo, ROUNDUP(hbv->len), -1, expoutb, bitp, 1); bitp += hbv->len;
	
	
	for (txp = 0; txp < totlen; txp += 0x8000)
	{
		int len = (((txp + 0x8000) > totlen) ? totlen : (txp + 0x8000)) - txp;
		printf("Shifting: "); printprog(txp + len, totlen); printf("\r");
		fflush(stdout);
		nexys2_jtag_puttmstdi(tap->nexys, len, inb + ((txp * 2) / 8), needout ? (outb + (txp / 8)) : NULL);
	}
	printf("\n");
	
	printf("I: %02x, %02x, %02x, %02x\n", (int)inb[0], (int)inb[1], (int) inb[2], (int)inb[3]);
	printf("O: %02x, %02x, %02x, %02x\n", (int)outb[0], (int)outb[1], (int)outb[2], (int)outb[3]);
	printf("X: %02x, %02x, %02x, %02x\n", (int)expoutb[0], (int)expoutb[1], (int)expoutb[2], (int)expoutb[3]);
	
	free(inb);
	free(outb);
	free(expoutb);
}

void runsvf(tap_t *tap, token_t *tlist)
{
	int trst = 0;
	int endir = TOK_IDLE, enddr = TOK_IDLE;
	blitval_t tir, tdr, hir, hdr;
	
	tir.len = 0;
	tdr.len = 0;
	hir.len = 0;
	hdr.len = 0;
	
#define NEXT tlist = tlist->next
#define REQUIRENEXT do { NEXT; if (!tlist) { printf("Unexpected end of stream\n"); abort(); } } while(0)
#define EXPECT(tok) \
		if (tlist->token != tok) \
		{ \
			printf("Expected " #tok ", got %d\n", tok); \
			abort(); \
		}
	
	while (tlist)
	{
		switch (tlist->token)
		{
		case TOK_TRST:
			REQUIRENEXT; EXPECT(TOK_OFF);
			trst = 0;
			REQUIRENEXT; EXPECT(TOK_SEMICOLON);
			NEXT;
			break;
		case TOK_ENDIR:
			REQUIRENEXT; EXPECT(TOK_IDLE);
			endir = TOK_IDLE;
			REQUIRENEXT; EXPECT(TOK_SEMICOLON);
			NEXT;
			break;
		case TOK_ENDDR:
			REQUIRENEXT; EXPECT(TOK_IDLE);
			enddr = TOK_IDLE;
			REQUIRENEXT; EXPECT(TOK_SEMICOLON);
			NEXT;
			break;
		case TOK_STATE:
			REQUIRENEXT;
			printf("Changing state...\n");
			if (tlist->token == TOK_RESET)
				s3l_tap_reset(tap);
			else if (tlist->token == TOK_IDLE)
				s3l_tap_idle(tap);
			else {
				printf("Unknown token after TOK_STATE\n");
				abort();
			}
			REQUIRENEXT; EXPECT(TOK_SEMICOLON);
			NEXT;
			break;
		case TOK_FREQUENCY:
			REQUIRENEXT; EXPECT(TOK_1E6);
			REQUIRENEXT; EXPECT(TOK_HZ);
			REQUIRENEXT; EXPECT(TOK_SEMICOLON);
			NEXT;
			break;
		case TOK_TIR:
		case TOK_TDR:
		case TOK_HIR:
		case TOK_HDR:
		case TOK_SIR:
		case TOK_SDR:
		{
			int oldtok = tlist->token;
			blitval_t *bv;
			blitval_t s;
						
			if (oldtok == TOK_TIR)
				bv = &tir;
			else if (oldtok == TOK_HIR)
				bv = &hir;
			else if (oldtok == TOK_TDR)
				bv = &tdr;
			else if (oldtok == TOK_HDR)
				bv = &hdr;
			else
				bv = &s;
			
			REQUIRENEXT; EXPECT(TOK_NUMBER);
			bv->len = tlist->data.i;
			bv->tdi = NULL;
			bv->tdo = NULL;
			bv->mask = NULL;
			bv->smask = NULL;
			REQUIRENEXT;
			
			while (tlist->token != TOK_SEMICOLON)
			{
				switch (tlist->token)
				{
				case TOK_TDI:
					REQUIRENEXT; EXPECT(TOK_BITSTRING);
					bv->tdi = tlist->data.s;
					REQUIRENEXT;
					break;
				case TOK_TDO:
					REQUIRENEXT; EXPECT(TOK_BITSTRING);
					bv->tdo = tlist->data.s;
					REQUIRENEXT;
					break;
				case TOK_MASK:
					REQUIRENEXT; EXPECT(TOK_BITSTRING);
					bv->mask = tlist->data.s;
					REQUIRENEXT;
					break;
				case TOK_SMASK:
					REQUIRENEXT; EXPECT(TOK_BITSTRING);
					bv->smask = tlist->data.s;
					REQUIRENEXT;
					break;
				default:
					abort();
				}
			}
			EXPECT(TOK_SEMICOLON);
			NEXT;
			
			if (oldtok == TOK_SIR || oldtok == TOK_SDR)
			{
				char *capturedr = "\x02";
				char *captureir = "\x0A";
				char *returnidle = "\x02";
				s3l_tap_idle(tap);
				/* Enter capture DR is TMS 1 0  */
				/* Enter capture IR is TMS 1 1 0 */
				/* Return to RTI is TMS 1 0 */
				
				if (oldtok == TOK_SIR)
				{
					printf("Shifting IR (%d bits)...\n", tir.len + s.len + hdr.len);
					nexys2_jtag_puttmstdi(tap->nexys, 4, captureir, NULL);
					pokethrough(tap, &tir, &s, &hir);
				} else {
					printf("Shifting DR (%d bits)...\n", tdr.len + s.len + hdr.len);
					nexys2_jtag_puttmstdi(tap->nexys, 3, capturedr, NULL);
					pokethrough(tap, &tdr, &s, &hdr);
				}
				nexys2_jtag_puttmstdi(tap->nexys, 2, returnidle, NULL);
			}
			break;
		}
		case TOK_RUNTEST:
		{
			int n;
			char *loss;
			
			REQUIRENEXT; EXPECT(TOK_NUMBER);
			n = tlist->data.i;
			REQUIRENEXT; EXPECT(TOK_TCK);
			REQUIRENEXT; EXPECT(TOK_SEMICOLON);
			NEXT;
			
			loss = malloc(64);
			memset(loss, 0, 64);
			s3l_tap_idle(tap);
			
			printf("Going RTI for %d clocks...\n", n);
			
			while (n)
			{
				int b = (n < 512) ? n : 512;
				nexys2_jtag_puttdi(tap->nexys, 0, n, loss, NULL);
				n -= b;
			}
			free(loss);
			
			break;
		}
		default:
			printf("Unknown top level token %d\n", tlist->token);
			abort();
		}
	}
}

int main(int argc, char **argv)
{
	tap_t tap;
	token_t *tlist;
	int fd;
	
	if (argc < 2)
	{
		printf("usage: s3load <file.svf>\n");
		return 1;
	}
	if ((fd = open(argv[1], O_RDONLY)) < 0)
	{
		perror("open");
		return 1;
	}
	tlist = tokenize(fd);
	close(fd);
	
	
	tap.tapstate = TAP_UNKNOWN;
	tap.nexys = nexys2_init();
	if (!tap.nexys)
	{
		fprintf(stderr, "Board init failed :(\n");
		return 1;
	}
	
	if (nexys2_jtag_enable(tap.nexys) < 0)
		return 1;
	
	runsvf(&tap, tlist);
	
	nexys2_jtag_disable(tap.nexys);
	
	return 0;
}