initial commit of s3load
[s3load.git] / s3load.c
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[(bitp*2-2)/8] |= 2 << ((bitp*2-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 }
This page took 0.130635 seconds and 4 git commands to generate.