[s3load.git] / s3load.c

#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;
}
Commit	Line	Data
5ee943f9 JW	1	#include "libnexys.h"
	2	#include <stdio.h>
	3	#include <stdlib.h>
	4	#include <fcntl.h>
	5	#include <string.h>
	6	#include <assert.h>
	7
	8	enum TAPSTATE {
	9	TAP_UNKNOWN,
	10	TAP_RESET,
	11	TAP_IDLE,
	12	TAP_DRSEL,
	13	TAP_DRCAP,
	14	TAP_DRSHIFT,
	15	TAP_DREXIT1,
	16	TAP_DRPAUSE,
	17	TAP_DREXIT2,
	18	TAP_DRUPDATE,
	19	TAP_IRSEL,
	20	TAP_IRCAP,
	21	TAP_IRSHIFT,
	22	TAP_IREXIT1,
	23	TAP_IRPAUSE,
	24	TAP_IREXIT2,
	25	TAP_IRUPDATE,
	26	};
	27
	28	typedef struct tap {
	29	nexys2_t nexys;
	30	int tapstate;
	31	} tap_t;
	32
	33	int s3l_tap_reset(tap_t *tap)
	34	{
	35	unsigned char reset[] = {0xAA, 0x02};
	36	tap->tapstate = TAP_RESET;
	37	return nexys2_jtag_puttmstdi(tap->nexys, 5, reset, NULL);
	38	}
	39
	40	int s3l_tap_idle(tap_t *tap)
	41	{
	42	switch (tap->tapstate)
	43	{
	44	case TAP_UNKNOWN:
	45	printf("Resetting TAP from unknown state...\n");
	46	s3l_tap_reset(tap);
	47	case TAP_RESET:
	48	{
	49	unsigned char idle[] = {0x00};
	50	printf("Placing TAP in idle state...\n");
	51	tap->tapstate = TAP_IDLE;
	52	return nexys2_jtag_puttmstdi(tap->nexys, 1, idle, NULL);
	53	}
	54	case TAP_IDLE:
	55	return 0;
	56	default:
	57	printf("cannot transition to idle from %d\n", tap->tapstate);
	58	abort();
	59	}
	60	}
	61
	62	enum TOKENTYPE {
	63	TOK_TRST = 0,
	64	TOK_OFF = 1,
65	TOK_ENDIR = 2,
66	TOK_IDLE = 3,
67	TOK_ENDDR = 4,
68	TOK_STATE = 5,
69	TOK_RESET = 6,
70	TOK_FREQUENCY = 7,
71	TOK_1E6 = 8,
72	TOK_HZ = 9,
73	TOK_SEMICOLON = 10,
74	TOK_TIR = 11,
75	TOK_HIR = 12,
76	TOK_TDR = 13,
77	TOK_HDR = 14,
78	TOK_SIR = 15,
79	TOK_SDR = 16,
80	TOK_TDI = 17,
81	TOK_TDO = 18,
82	TOK_MASK = 19,
83	TOK_SMASK = 20,
84	TOK_NUMBER = 21,
85	TOK_BITSTRING = 22,
86	TOK_RUNTEST = 23,
87	TOK_TCK = 24,
88	};
89
90	typedef struct token {
91	int token;
92	union {
93	int i;
94	char *s;
95	} data;
96	struct token *next;
97	} token_t;
98
99	int hex2nyb(char c)
100	{
101	if (c >= '0' && c <= '9')
102	return c-'0';
103	if (c >= 'a' && c <= 'f')
104	return c-'a'+10;
105	if (c >= 'A' && c <= 'F')
106	return c-'A'+10;
107	printf("Bad hex nybble %c\n", c);
108	abort();
109	}
110
111	token_t *tokenize(int fd)
112	{
113	/* First off, suck in ALL of the file */
114	char f = NULL, fst;
115	int sz = 0;
116	token_t start = NULL, cur = NULL;
117
118	while (1)
119	{
120	int rsz;
121	f = realloc(f, sz + 16385);
122	if ((rsz = read(fd, &(f[sz]), 16384)) < 16384)
123	{
124	sz += rsz;
125	break;
126	}
127	sz += rsz;
128	}
129	f[sz] = 0;
130	fst = f;
131
132	while (f && *f)
133	{
134	if (!strncmp(f, "//", 2))
135	{
136	// Comment, eat the rest of the line
137	f = strchr(f, '\n');
138	if (f && f[1] == '\r')
139	f += 2;
140	else if (f)
141	f++;
142	} else if (f == '\n' \|\| f == '\r' \|\| f == '\t' \|\| f == ' ')
143	f++;
144	#define NEWTOKEN \
145	if (!cur) \
146	{ \
147	start = cur = malloc(sizeof(*cur)); \
148	start->next = NULL; \
149	} else { \
150	cur->next = malloc(sizeof(*cur)); \
151	cur = cur->next; \
152	cur->next = NULL; \
153	}
154	#define MATCHSTRING(str, tok) \
155	if (!strncmp(f, str, strlen(str))) { \
156	NEWTOKEN \
157	cur->token = tok; \
158	f += strlen(str); \
159	}
160	else MATCHSTRING("TRST", TOK_TRST)
161	else MATCHSTRING("OFF", TOK_OFF)
162	else MATCHSTRING("ENDIR", TOK_ENDIR)
163	else MATCHSTRING("IDLE", TOK_IDLE)
164	else MATCHSTRING("ENDDR", TOK_ENDDR)
165	else MATCHSTRING("STATE", TOK_STATE)
166	else MATCHSTRING("RESET", TOK_RESET)
167	else MATCHSTRING("FREQUENCY", TOK_FREQUENCY)
168	else MATCHSTRING("1E6", TOK_1E6)
169	else MATCHSTRING("HZ", TOK_HZ)
170	else MATCHSTRING(";", TOK_SEMICOLON)
171	else MATCHSTRING("TIR", TOK_TIR)
172	else MATCHSTRING("HIR", TOK_HIR)
173	else MATCHSTRING("TDR", TOK_TDR)
174	else MATCHSTRING("HDR", TOK_HDR)
175	else MATCHSTRING("TDI", TOK_TDI)
176	else MATCHSTRING("SMASK", TOK_SMASK)
177	else MATCHSTRING("SIR", TOK_SIR)
178	else MATCHSTRING("MASK", TOK_MASK)
179	else MATCHSTRING("TDO", TOK_TDO)
180	else MATCHSTRING("SDR", TOK_SDR)
181	else MATCHSTRING("RUNTEST", TOK_RUNTEST)
182	else MATCHSTRING("TCK", TOK_TCK)
183	else if (isdigit(*f)) {
184	NEWTOKEN
185	cur->token = TOK_NUMBER;
186	cur->data.i = *f - '0';
187	f++;
188	while (isdigit(*f))
189	{
190	cur->data.i *= 10;
191	cur->data.i += *f - '0';
192	f++;
193	}
194	} else if (*f == '(') {
195	int asz = 0;
196	int l = 0;
197	NEWTOKEN
198	cur->token = TOK_BITSTRING;
199	cur->data.s = NULL;
200	f++;
201	while (f && f != ')')
202	{
203	char c1, c2;
204	while (isspace(f) && f && *f != ')')
205	f++;
206	if (!f \|\| f == ')')
207	break;
208	c1 = *(f++);
209
210	while (isspace(f) && f && *f != ')')
211	f++;
212	if (!f \|\| f == ')')
213	{
214	printf("Unpaired hex digit in bitstring\n");
215	abort();
216	}
217	c2 = *(f++);
218
219	if (l == asz)
220	{
221	asz += 128;
222	cur->data.s = realloc(cur->data.s, asz);
223	}
224	cur->data.s[l++] = hex2nyb(c1) << 4 \| hex2nyb(c2);
225	}
226	f++;
227	} else {
228	printf("Tokenizing error, unhandled: %s\n", f);
229	abort();
230	}
231	}
232
233	free(fst);
234
235	return start;
236	}
237
238	typedef struct blitval {
239	int len;
240	char tdi, tdo, mask, smask;
241	} blitval_t;
242
243	/* Copies bits from an input in format:
244	* (msb first) 0 1 2 3 4 5 6 7 \| 8 9 10 11 12 13 14 15 \| ...
245	* To an output in format:
246	* 7 6 5 4 3 2 1 0 \| 15 14 13 12 11 10 9 8 \| ...
247	*/
248	void bitcopy(int len, unsigned char inbits, int inpos, int indir, unsigned char outbits, int outpos, int outdir)
249	{
250	int i;
251	if (!inbits \|\| !outbits)
252	return;
253	for (i = 0; i < len; i++)
254	{
255	int inbit;
256	inbit = inbits[(inpos + i * indir) / 8] & (1 << (7 - ((inpos + i * indir) % 8)));
257	outbits[(outpos + i * outdir) / 8] \|= inbit ? (1 << ((outpos + i * outdir) % 8)) : 0;
258	}
259	}
260
261	void printprog(int done, int total)
262	{
263	int i;
264	int progresscounter = done * 32 / total;
265	static int spinnerstate;
266	char *spinner = "_.oOo.";
267
268	printf("[");
269	for (i=0; i < progresscounter; i++)
270	printf("=");
271	if (progresscounter != 32)
272	printf(">");
273	for (i=0; i < (31 - progresscounter); i++)
274	printf(" ");
275	printf("] %c %d/%d", spinner[(spinnerstate++) % 6], done, total);
276	}
277
278	void pokethrough(tap_t tap, blitval_t tbv, blitval_t sbv, blitval_t hbv)
279	{
280	int totlen = tbv->len + sbv->len + hbv->len;
281	int lenb = (totlen / 4) + ((totlen % 4) != 0) + 1; // Number of nexys2_* bytes
282	unsigned char inb = calloc(lenb, 1); // Bitstream to be fed to nexys2_
283	unsigned char *outb = calloc(lenb, 1);
284	unsigned char *expoutb = calloc(lenb, 1);
285	int bitp = 0; // Position in the "inb" bitstream
286	int txp = 0;
287	int needout = 0;
288
289	if (!totlen)
290	return;
291
292	printf("t:%d, s:%d, h:%d\n", tbv->len, sbv->len, hbv->len);
293
294	bitp = 0;
295	#define ROUNDUP(x) (((x / 8) + ((x % 8) != 0)) * 8 - 1)
296	bitcopy(hbv->len, hbv->tdi, ROUNDUP(hbv->len), -1, inb, bitp*2, 2); bitp += hbv->len;
297	bitcopy(sbv->len, sbv->tdi, ROUNDUP(sbv->len), -1, inb, bitp*2, 2); bitp += sbv->len;
298	bitcopy(tbv->len, tbv->tdi, ROUNDUP(tbv->len), -1, inb, bitp*2, 2); bitp += tbv->len;
299	inb[(bitp2-2)/8] \|= 2 << ((bitp2-2) % 8);
300	assert(bitp == totlen);
301
302	if (sbv->tdo)
303	needout = 1;
304	bitp = 0; /* Have to throw away one bit :( */
305	bitcopy(tbv->len, tbv->tdo, ROUNDUP(tbv->len), -1, expoutb, bitp, 1); bitp += tbv->len;
306	bitcopy(sbv->len, sbv->tdo, ROUNDUP(sbv->len), -1, expoutb, bitp, 1); bitp += sbv->len;
307	bitcopy(hbv->len, hbv->tdo, ROUNDUP(hbv->len), -1, expoutb, bitp, 1); bitp += hbv->len;
308
309
310	for (txp = 0; txp < totlen; txp += 0x8000)
311	{
312	int len = (((txp + 0x8000) > totlen) ? totlen : (txp + 0x8000)) - txp;
313	printf("Shifting: "); printprog(txp + len, totlen); printf("\r");
314	fflush(stdout);
315	nexys2_jtag_puttmstdi(tap->nexys, len, inb + ((txp * 2) / 8), needout ? (outb + (txp / 8)) : NULL);
316	}
317	printf("\n");
318
319	printf("I: %02x, %02x, %02x, %02x\n", (int)inb[0], (int)inb[1], (int) inb[2], (int)inb[3]);
320	printf("O: %02x, %02x, %02x, %02x\n", (int)outb[0], (int)outb[1], (int)outb[2], (int)outb[3]);
321	printf("X: %02x, %02x, %02x, %02x\n", (int)expoutb[0], (int)expoutb[1], (int)expoutb[2], (int)expoutb[3]);
322
323	free(inb);
324	free(outb);
325	free(expoutb);
326	}
327
328	void runsvf(tap_t tap, token_t tlist)
329	{
330	int trst = 0;
331	int endir = TOK_IDLE, enddr = TOK_IDLE;
332	blitval_t tir, tdr, hir, hdr;
333
334	tir.len = 0;
335	tdr.len = 0;
336	hir.len = 0;
337	hdr.len = 0;
338
339	#define NEXT tlist = tlist->next
340	#define REQUIRENEXT do { NEXT; if (!tlist) { printf("Unexpected end of stream\n"); abort(); } } while(0)
341	#define EXPECT(tok) \
342	if (tlist->token != tok) \
343	{ \
344	printf("Expected " #tok ", got %d\n", tok); \
345	abort(); \
346	}
347
348	while (tlist)
349	{
350	switch (tlist->token)
351	{
352	case TOK_TRST:
353	REQUIRENEXT; EXPECT(TOK_OFF);
354	trst = 0;
355	REQUIRENEXT; EXPECT(TOK_SEMICOLON);
356	NEXT;
357	break;
358	case TOK_ENDIR:
359	REQUIRENEXT; EXPECT(TOK_IDLE);
360	endir = TOK_IDLE;
361	REQUIRENEXT; EXPECT(TOK_SEMICOLON);
362	NEXT;
363	break;
364	case TOK_ENDDR:
365	REQUIRENEXT; EXPECT(TOK_IDLE);
366	enddr = TOK_IDLE;
367	REQUIRENEXT; EXPECT(TOK_SEMICOLON);
368	NEXT;
369	break;
370	case TOK_STATE:
371	REQUIRENEXT;
372	printf("Changing state...\n");
373	if (tlist->token == TOK_RESET)
374	s3l_tap_reset(tap);
375	else if (tlist->token == TOK_IDLE)
376	s3l_tap_idle(tap);
377	else {
378	printf("Unknown token after TOK_STATE\n");
379	abort();
380	}
381	REQUIRENEXT; EXPECT(TOK_SEMICOLON);
382	NEXT;
383	break;
384	case TOK_FREQUENCY:
385	REQUIRENEXT; EXPECT(TOK_1E6);
386	REQUIRENEXT; EXPECT(TOK_HZ);
387	REQUIRENEXT; EXPECT(TOK_SEMICOLON);
388	NEXT;
389	break;
390	case TOK_TIR:
391	case TOK_TDR:
392	case TOK_HIR:
393	case TOK_HDR:
394	case TOK_SIR:
395	case TOK_SDR:
396	{
397	int oldtok = tlist->token;
398	blitval_t *bv;
399	blitval_t s;
400
401	if (oldtok == TOK_TIR)
402	bv = &tir;
403	else if (oldtok == TOK_HIR)
404	bv = &hir;
405	else if (oldtok == TOK_TDR)
406	bv = &tdr;
407	else if (oldtok == TOK_HDR)
408	bv = &hdr;
409	else
410	bv = &s;
411
412	REQUIRENEXT; EXPECT(TOK_NUMBER);
413	bv->len = tlist->data.i;
414	bv->tdi = NULL;
415	bv->tdo = NULL;
416	bv->mask = NULL;
417	bv->smask = NULL;
418	REQUIRENEXT;
419
420	while (tlist->token != TOK_SEMICOLON)
421	{
422	switch (tlist->token)
423	{
424	case TOK_TDI:
425	REQUIRENEXT; EXPECT(TOK_BITSTRING);
426	bv->tdi = tlist->data.s;
427	REQUIRENEXT;
428	break;
429	case TOK_TDO:
430	REQUIRENEXT; EXPECT(TOK_BITSTRING);
431	bv->tdo = tlist->data.s;
432	REQUIRENEXT;
433	break;
434	case TOK_MASK:
435	REQUIRENEXT; EXPECT(TOK_BITSTRING);
436	bv->mask = tlist->data.s;
437	REQUIRENEXT;
438	break;
439	case TOK_SMASK:
440	REQUIRENEXT; EXPECT(TOK_BITSTRING);
441	bv->smask = tlist->data.s;
442	REQUIRENEXT;
443	break;
444	default:
445	abort();
446	}
447	}
448	EXPECT(TOK_SEMICOLON);
449	NEXT;
450
451	if (oldtok == TOK_SIR \|\| oldtok == TOK_SDR)
452	{
453	char *capturedr = "\x02";
454	char *captureir = "\x0A";
455	char *returnidle = "\x02";
456	s3l_tap_idle(tap);
457	/* Enter capture DR is TMS 1 0 */
458	/* Enter capture IR is TMS 1 1 0 */
459	/* Return to RTI is TMS 1 0 */
460
461	if (oldtok == TOK_SIR)
462	{
463	printf("Shifting IR (%d bits)...\n", tir.len + s.len + hdr.len);
464	nexys2_jtag_puttmstdi(tap->nexys, 4, captureir, NULL);
465	pokethrough(tap, &tir, &s, &hir);
466	} else {
467	printf("Shifting DR (%d bits)...\n", tdr.len + s.len + hdr.len);
468	nexys2_jtag_puttmstdi(tap->nexys, 3, capturedr, NULL);
469	pokethrough(tap, &tdr, &s, &hdr);
470	}
471	nexys2_jtag_puttmstdi(tap->nexys, 2, returnidle, NULL);
472	}
473	break;
474	}
475	case TOK_RUNTEST:
476	{
477	int n;
478	char *loss;
479
480	REQUIRENEXT; EXPECT(TOK_NUMBER);
481	n = tlist->data.i;
482	REQUIRENEXT; EXPECT(TOK_TCK);
483	REQUIRENEXT; EXPECT(TOK_SEMICOLON);
484	NEXT;
485
486	loss = malloc(64);
487	memset(loss, 0, 64);
488	s3l_tap_idle(tap);
489
490	printf("Going RTI for %d clocks...\n", n);
491
492	while (n)
493	{
494	int b = (n < 512) ? n : 512;
495	nexys2_jtag_puttdi(tap->nexys, 0, n, loss, NULL);
496	n -= b;
497	}
498	free(loss);
499
500	break;
501	}
502	default:
503	printf("Unknown top level token %d\n", tlist->token);
504	abort();
505	}
506	}
507	}
508
509	int main(int argc, char **argv)
510	{
511	tap_t tap;
512	token_t *tlist;
513	int fd;
514
515	if (argc < 2)
516	{
517	printf("usage: s3load <file.svf>\n");
518	return 1;
519	}
520	if ((fd = open(argv[1], O_RDONLY)) < 0)
521	{
522	perror("open");
523	return 1;
524	}
525	tlist = tokenize(fd);
526	close(fd);
527
528
529	tap.tapstate = TAP_UNKNOWN;
530	tap.nexys = nexys2_init();
531	if (!tap.nexys)
532	{
533	fprintf(stderr, "Board init failed :(\n");
534	return 1;
535	}
536
537	if (nexys2_jtag_enable(tap.nexys) < 0)
538	return 1;
539
540	runsvf(&tap, tlist);
541
542	nexys2_jtag_disable(tap.nexys);
543
544	return 0;
545	}