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 }