/* XSVF Parser for Project VGA, see <http://projectvga.org/>.
 * v0.2 - Early beta (works for XC95144XL)
 * (C) Copyright 2008, Michael Meeuwisse
 * All Rights Reserved.
 * Credits to Reinder de Haan
 * 
 * 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 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * How to compile in Cygwin:
 * Copy ftd2xx.h and ftd2xx.lib to the same folder this file is in.
 * gcc -Wall -o xsvfparser xsvfparser.c ftd2xx.lib
 */

#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <fcntl.h>
#include <arpa/inet.h>
#include <unistd.h>

#include <windows.h>
#include "ftd2xx.h"

/* Give debug data, 0: None, 1: Commands, 2: MPSSE Output */
#define DEBUG 0

/* Report error and force quit */
void error(const char *msg, ...) {
  va_list arg;
  fprintf(stderr, "\nError: ");
  va_start(arg, msg);
  vfprintf(stderr, msg, arg);
  va_end(arg);
  fprintf(stderr, "\n");
  
  exit(1);
}

/* wrapper-malloc */
void *wmalloc(size_t size) {
  void *ret = calloc(1, size);
  if(!ret)
    error("No memory?");
  return ret;
}

FT_HANDLE openDevice() {
  char *devID = "projectvga mod1 rev0 A";
  FT_HANDLE devHandle = NULL;
  DWORD len = 0;
// ------- DEBUG; 0x04 instead of 0x00 gives a 1.2MHz clock to make sure we're not having any speed issues
  char jtagSettings[7] = { 0x80, 0x08, 0x0B, /* Set lower bits behavior */
    0x85, /* Disconnect TDI/DO - TDO/DI */
    0x86, 0x04, 0x00 }; /* Set TCK/SK divisor */
  
  /* Open device, set some defaults, enable MPSSE, configure JTAG */
  if(FT_OpenEx(devID, FT_OPEN_BY_DESCRIPTION, &devHandle) != FT_OK ||
    FT_ResetDevice(devHandle) != FT_OK ||
    FT_SetBitMode(devHandle, 0x00, 0) != FT_OK ||
    FT_SetLatencyTimer(devHandle, 2) != FT_OK ||
    FT_SetTimeouts(devHandle, 5000, 5000) != FT_OK ||
    FT_SetBitMode(devHandle, 0x0B, 2) != FT_OK ||
    FT_Write(devHandle, jtagSettings, 7, &len) != FT_OK)
    return NULL;
  
  return devHandle;
}

/* Input file buffer & handles */
typedef struct _inbuf {
  int fileHandle;
  char *data;
  unsigned int cur;
  unsigned int len;
#if DEBUG == 0
  unsigned int infoCur;
  unsigned int infoMax;
#endif
} inbuf;

#define INBUFSIZE 65536

/* Get a byte of data from inbuf */
unsigned char get(inbuf *input) {
  if(input->cur == input->len) {
    if(!input->data)
      input->data = wmalloc(INBUFSIZE);
#if DEBUG == 0
    if(!input->infoMax) {
      input->infoMax = lseek(input->fileHandle, 0, SEEK_END);
      lseek(input->fileHandle, 0, SEEK_SET);
    }
#endif
    if((input->len = read(input->fileHandle, input->data, INBUFSIZE)) == -1)
      error("Failed to read File");
    if(input->len == 0)
      error("Premature End of File");
    input->cur = 0;
  }
#if DEBUG == 0
  if(!(input->infoCur++ % 100))
    fprintf(stderr, "\rProgress: %.2f%% done.", 
      ((float) input->infoCur / (float) input->infoMax) * 100);
#endif
  return input->data[input->cur++];
}

/* Output device buffer */
typedef struct _outbuf {
  FT_HANDLE devHandle;
  char *data;
  unsigned char *bytes;
  unsigned int bytesPending;
  unsigned int bytesSize;
  unsigned char bits;
  unsigned int bitsPending;
  unsigned int cur;
  unsigned char curState;
  enum INSTR_OPT { INSTR_W = 0, INSTR_RW = 0x20 } enableRW;
} outbuf;

/* FTDI recommends this as it equals the USB packetsize */
#define OUTBUFSIZE 3968
/* Max number of output bytes for MPSSE command */
#define MAXTCKCMD (1 << 16)
/* Round up and give back number of bytes */
#define bits2bytes(x) ((x + 7) / 8)

/* Send off the data buffer to the device */
void flush(outbuf *output) {
  int len = output->cur, retries = 5;
  DWORD tmp;
  
#if DEBUG == 2
  fprintf(stderr, "Flush: ");
  for(len = 0; len < output->cur; len++)
    fprintf(stderr, "%02x", 0xFF & output->data[len]);
  fprintf(stderr, "\n");
  len = output->cur;
#endif
  
  /* Retry if not the entire buffer was send, fail after 5 attempts */
  while(len && retries) {
    if(FT_Write(output->devHandle, output->data + output->cur - len, len, &tmp) != FT_OK || !retries--)
      error("Couldn't write to Device");
    len -= tmp;
  }
  output->cur = 0;
}

/* Put a byte in the output buffer */
void put(outbuf *output, unsigned char data) {
  if(output->cur >= OUTBUFSIZE - 1)
    flush(output);
  output->data[output->cur++] = data;
}

/* Put instruction to write bits on TDI in output buffer */
void putBitsInstr(outbuf *output, unsigned int numBits, unsigned char bits) {
  if(!numBits)
    return;
  put(output, 0x1B | output->enableRW);
  put(output, 0x07 & (numBits - 1));
  put(output, bits);
}

/* Put instructions to write bytes on TDI in output buffer */
void putBytesInstr(outbuf *output, unsigned int bufSize, unsigned char *data) {
  while(bufSize > 1) {
    unsigned int i = bufSize;
    if(bufSize > MAXTCKCMD)
      bufSize -= MAXTCKCMD;
    else
      bufSize = 0;
    put(output, 0x19 | output->enableRW);
    put(output, 0xFF & ((i - bufSize) - 1));
    put(output, 0xFF & (((i - bufSize) - 1) >> 8));
    while(i-- > bufSize)
      if(data)
        put(output, data[i]);
      else
        put(output, 0x00);
  }
  if(bufSize) {
    if(data)
      putBitsInstr(output, 0x08, data[0]);
    else
      putBitsInstr(output, 0x08, 0x00);
    bufSize = 0;
  }
}

/* Create instructions for pending buffer */
void buildTCKInstr(outbuf *output) {
  putBytesInstr(output, output->bytesPending, output->bytes);
  output->bytesPending = 0;
  if(output->bitsPending) {
    putBitsInstr(output, output->bitsPending, output->bits);
    output->bitsPending = 0;
  }
}

/* Put x bits from inbuf in pending buffer for outbuf */
void putBits(outbuf *output, inbuf *input, unsigned int numBits) {
  unsigned char bits = 0x00, numBytes = numBits / 8;
  if(!numBits)
    error("Trying to write 0 bits");
  numBits %= 8;
  if(numBits)
    bits = get(input);
  buildTCKInstr(output);
  while(numBytes--) {
    /* Also used for IR, which can be accessed before XSDRSIZE */
    if(output->bytesPending > output->bytesSize)
      buildTCKInstr(output);
    output->bytes[output->bytesPending++] = get(input);
  }
  output->bits = bits;
  output->bitsPending = numBits;
}

/* Put x bits from cache in pending buffer for outbuf */
void putBitsFromCache(outbuf *output, unsigned char *cache, unsigned int numBits) {
  unsigned int numBytes = numBits / 8;
  if(!numBits)
    error("Trying to read from null-sized cache");
  numBits %= 8;
  buildTCKInstr(output);
  output->bits = cache[numBytes + (numBits? 1: 0)];
  output->bitsPending = numBits;
  while(numBytes--)
    output->bytes[output->bytesPending++] = cache[numBytes];
}

/* Set pending buffer size */
void setBuffersSize(outbuf *output, unsigned int numBits) {
  if(!numBits || bits2bytes(numBits) < output->bytesSize)
    return;
  buildTCKInstr(output);
  if(output->bytes)
    free(output->bytes);
  output->bytesSize = bits2bytes(numBits);
  output->bytes = wmalloc(bits2bytes(numBits));
}

/* Set or Clear the Readback option for TDO */
void setEnableRW(outbuf *output, enum INSTR_OPT direction) {
  /* Clear buffer if it contains data not matching the new option */
  if(output->enableRW != direction)
    buildTCKInstr(output);
  output->enableRW = direction;
}

/* Get x bytes from TDO */
unsigned int getTDO(outbuf *output, unsigned char *buffer, unsigned int numBytes) {
  DWORD ret;
  /* Make sure nothing is pending */
  buildTCKInstr(output);
  flush(output);
  if(FT_Read(output->devHandle, buffer, numBytes, &ret) != FT_OK || numBytes != ret)
    error("Failed to read from Device %d %d", numBytes, ret);
  return ret;
}

/* Shift between stable moves and from these to Shift-DR or Shift-IR */
static unsigned char moveState[6 /* From */][6 /* To */][2 /* TMS sequence, TCK times */] = {
/* To:      RTI      Shift-DR      Pause-DR      Shift-IR      Pause-IR      From: */
  {{0x00, 0x01}, {0x02, 0x04}, {0x0A, 0x05}, {0x06, 0x05}, {0x16, 0x06}}, /* TLR */
  {{0x00, 0x00}, {0x01, 0x03}, {0x05, 0x04}, {0x03, 0x04}, {0x0B, 0x05}}, /* RTI */
  {{0x03, 0x03}, {0x00, 0x00}, {0x01, 0x02}, {0x0F, 0x06}, {0x2F, 0x07}}, /* Shift-DR */
  {{0x03, 0x03}, {0x01, 0x02}, {0x00, 0x00}, {0x0F, 0x06}, {0x2F, 0x07}}, /* Pause-DR */
  {{0x03, 0x03}, {0x07, 0x05}, {0x17, 0x06}, {0x00, 0x00}, {0x01, 0x02}}, /* Shift-IR */
  {{0x03, 0x03}, {0x07, 0x05}, {0x17, 0x06}, {0x01, 0x02}, {0x00, 0x00}}  /* Pause-IR */
};

/* Translate value from xsvf to state value which, if < 0x06, can be looked up in moveState */
static unsigned char xsvfState[16] = {
  0x00, 0x01, 0x06, 0x07, 0x02, 0x08, 0x03, 0x09, 
  0x0A, 0x0B, 0x0C, 0x04, 0x0D, 0x05, 0x0E, 0x0F
};

/* If TMS = 0, what state would you end up in next */
static unsigned char nextState[16] = {
  0x01, 0x01, 0x02, 0x03, 0x04, 0x05, 0x07, 0x02, 
  0x03, 0x02, 0x01, 0x0C, 0x04, 0x05, 0x04, 0x01
};

/* Move from current state to newState (as XSVF value) */
void gotoState(outbuf *output, unsigned char newState) {
  unsigned char tck = 0x00;
  newState = xsvfState[newState];
  /* Are we there yet? - unless TLR */
  if(output->curState == newState && newState != 0x00)
    return;
  /* Steal bit from pending buffer, overlap TDI output with TMS output */
  if(output->bytesPending || output->bitsPending) {
    if(!output->bitsPending) {
      output->bits = output->bytes[output->bytesPending--];
      output->bitsPending = 8;
    }
    tck = 0x80 & (output->bits << (8 - output->bitsPending--));
  }
  buildTCKInstr(output);
  /* Reset */
  if(newState == 0x00) {
    put(output, 0x4B);
    put(output, 0x05);
    put(output, tck | 0x1F);
    output->curState = newState;
    return;
  }
  /* Single state */
  if(output->curState > 0x05 || newState > 0x05) {
    put(output, 0x4B);
    put(output, 0x00);
    put(output, tck | (nextState[output->curState] == newState? 0x00: 0x01));
    output->curState = newState;
    return;
  }
  if(output->curState > 0x05)
    error("Invalid state move from 0x%02x to 0x%02x", output->curState, newState);
  /* Stable states */
  put(output, 0x4B);
  put(output, moveState[output->curState][newState - 1][1] - 1);
  put(output, tck | moveState[output->curState][newState - 1][0]);
  output->curState = newState;
}

/* XSVF state values */
#define RTI 0x01
#define SHIFTDR 0x04
#define EXIT1DR 0x05
#define PAUSEDR 0x06
#define UPDATDR 0x08
#define SHIFTIR 0x0B
#define PAUSEIR 0x0D

/* Go to RTI, output x clocks, wait x microseconds */
void putRunTest(outbuf *output, unsigned int numBits) {
  enum INSTR_OPT cacheRW = output->enableRW;
  output->enableRW = INSTR_W;
  gotoState(output, RTI);
  if(!numBits)
    return;
  putBytesInstr(output, bits2bytes(numBits), NULL);
  flush(output);
  usleep(numBits);
  output->enableRW = cacheRW;
}

/* Get string of x bits from inbuf and put it in buf */
void getString(inbuf *input, char *buf, unsigned int numBits) {
  unsigned int len = bits2bytes(numBits);
  if(!buf)
    error("Trying to write to null-sized buffer");
  while(len--)
    buf[len] = get(input);
}

/* Get integer from inbuf */
unsigned int getInt(inbuf *input) {
  unsigned int ret = get(input) | get(input) << 8 | get(input) << 16 | get(input) << 24;
  return ntohl(ret);
}

/* Get short from inbuf */
unsigned short getShort(inbuf *input) {
  unsigned short ret = get(input) | get(input) << 8;
  return ntohs(ret);
}

/* Compare tdoCache with tdoMatch after applying (optional) tdoMask for len bits */
unsigned int compareTDO(outbuf *output, unsigned char *tdoMask, 
  unsigned char *tdoCache, unsigned char *tdoMatch, unsigned int len) {
  unsigned int failed = 0, bits = len % 8, tmp;
  getTDO(output, tdoCache, bits2bytes(len));
  len = tmp = bits2bytes(len) - 1;
  if(bits)
    tdoCache[len] = tdoCache[len] >> (8 - bits);
  do {
    if(!tdoMask) {
      if(tdoCache[len] != tdoMatch[len])
        failed = 1;
    } else
      if((tdoCache[len] & tdoMask[len]) != (tdoMatch[len] & tdoMask[len]))
        failed = 1;
  } while(len-- && !failed);
#if DEBUG == 1
  if(failed) {
    len = tmp;
    fprintf(stderr, "tdoMask:\t0x");
    do
      fprintf(stderr, "%02x", 0xFF & tdoMask[len]);
    while(len--);
    len = tmp;
    fprintf(stderr, "\ntdoCache:\t0x");
    do
      fprintf(stderr, "%02x", 0xFF & tdoCache[len]);
    while(len--);
    len = tmp;
    fprintf(stderr, "\ntdoMatch:\t0x");
    do
      fprintf(stderr, "%02x", 0xFF & tdoMatch[len]);
    while(len--);
    fprintf(stderr, "\n");
  }
#endif
  return failed;
}

/* Parse XSVF commands from inbuf and output commands to outbuf */
int parseFile(outbuf *output, inbuf *input) {
  unsigned int len = 0, xruntest = 0;
  unsigned char xendirState = RTI, xenddrState = RTI, xrepeat = 0x20;
  unsigned char *tdoMask = NULL, *tdoCache = NULL, *tdoMatch = NULL, *tdiCache = NULL;
  while(1) {
    unsigned char cmd = get(input);
#if DEBUG == 1
    fprintf(stderr, "cmd: 0x%02x\n", cmd);
#endif
    switch(cmd) {
    case 0x00: /* XCOMPLETE */
//      if(output->curState != 0x00)
//        gotoState(output, 0x00);
      buildTCKInstr(output);
      flush(output);
#if DEBUG == 0
      fprintf(stdout, "\rProgress: 100%% done.     \n");
#endif
      return 0;
    case 0x01: /* XTDOMASK */
      getString(input, tdoMask, len);
      break;
    case 0x04: /* XRUNTEST */
      xruntest = getInt(input);
#if DEBUG == 1
      fprintf(stderr, "RUN: %d\n", xruntest);
#endif
      break;
    case 0x07: /* XREPEAT */
      xrepeat = get(input);
#if DEBUG == 1
      fprintf(stderr, "REP: %d\n", xrepeat);
#endif
      break;
    case 0x08: /* XSDRSIZE */
      if(len) {
        free(tdoMask);
        free(tdoCache);
        free(tdoMatch);
        free(tdiCache);
      }
      len = getInt(input);
#if DEBUG == 1
      fprintf(stderr, "LEN: %d\n", len);
#endif
      tdoMask = wmalloc(bits2bytes(len));
      tdoCache = wmalloc(bits2bytes(len));
      tdoMatch = wmalloc(bits2bytes(len));
      tdiCache = wmalloc(bits2bytes(len));
      setBuffersSize(output, len);
      break;
    case 0x03: /* XSDR */
    case 0x09: /* XSDRTDO */ {
        unsigned int tries = 0, tdoResult = 0;
        /* Use cache, we might need to output these commands xrepeat times */
        getString(input, tdiCache, len);
        if(cmd == 0x09)
          getString(input, tdoMatch, len);
        setEnableRW(output, INSTR_RW);
        do {
          gotoState(output, SHIFTDR);
          putBitsFromCache(output, tdiCache, len);
          gotoState(output, EXIT1DR);
          if((tdoResult = compareTDO(output, tdoMask, tdoCache, tdoMatch, len))) {
            /* Error handling routine for XC9500 */
            gotoState(output, PAUSEDR);
            gotoState(output, SHIFTDR);
            xruntest *= 1.25;
            /* Don't report the first few times - that gets annoying */
            if(tries < xrepeat && tries > 0x03)
              fprintf(stderr, "\nWARNING: compareTDO failed, retrying %d/%d", tries + 1, xrepeat);
          } else
            gotoState(output, UPDATDR);
          /* Now wait for a moment */
          putRunTest(output, xruntest);
        } while(tdoResult && tries++ < xrepeat);
        if(tdoResult)
          error("Failed TDO check instruction 0x%02x", cmd);
      }
      break;
    case 0x0C: /* XSDRB */
      gotoState(output, SHIFTDR);
    case 0x0D: /* XSDRC */
    case 0x0E: /* XSDRE */
      setEnableRW(output, INSTR_W);
      putBits(output, input, len);
      if(cmd == 0x0E)
        gotoState(output, xenddrState);
      break;
    case 0x0F: /* XSDRTDOB */
      gotoState(output, SHIFTDR);
    case 0x10: /* XSDRTDOC */
    case 0x11: /* XSDRTDOE */
      setEnableRW(output, INSTR_RW);
      putBits(output, input, len);
      getString(input, tdoMatch, len);
      if(compareTDO(output, NULL, tdoCache, tdoMatch, len))
        error("Failed TDO check instruction 0x%02x", cmd);
      if(cmd == 0x11)
        gotoState(output, xenddrState);
      break;
    case 0x12: /* XSTATE */
      gotoState(output, get(input));
      break;
    case 0x13: /* XENDIR */ {
        unsigned char new = get(input);
        if(new > 0x01)
          error("Invalid XENDIR State 0x%02x", new);
        xendirState = new? PAUSEIR: RTI;
      }
      break;
    case 0x14: /* XENDDR */ {
        unsigned char new = get(input);
        if(new > 0x01)
          error("Invalid XENDDR State 0x%02x", new);
        xenddrState = new? PAUSEDR: RTI;
      }
      break;
    case 0x02: /* XSIR */
    case 0x15: /* XSIR2 */ {
        unsigned int tdiLen = (int) (cmd == 0x02? get(input): getShort(input));
#if DEBUG == 1
        fprintf(stderr, "SIR %d\n", tdiLen);
#endif
        gotoState(output, SHIFTIR);
        setEnableRW(output, INSTR_W);
        putBits(output, input, tdiLen);
        if(xruntest)
          putRunTest(output, xruntest);
        else
          gotoState(output, xendirState);
      }
      break;
    case 0x16: /* XCOMMENT */
      while(get(input));
      break;
    case 0x17: /* XWAIT */ {
        unsigned char end = 0x00;
        gotoState(output, get(input));
        end = get(input);
        usleep(getInt(input));
        gotoState(output, end);
      }
      break;
    default:
      error("Unknown command 0x%02x", cmd);
    }
  }
  return 0;
}

int main(int argc, char *argv[]) {
  inbuf *input = wmalloc(sizeof(inbuf));
  outbuf *output = wmalloc(sizeof(outbuf));
  output->data = wmalloc(OUTBUFSIZE);
  /* Unknown size yet, but it's also used for XSIR so set it to something */
  output->bytes = wmalloc(128);
  output->bytesSize = 128;
  
  if(argc < 2) {
    fprintf(stderr, "XSVF Parser for FT2232 based JTAG Programmers");
    error("Usage: %s File_to_parse", argv[0]);
  }

  if(!(output->devHandle = openDevice()))
    error("Unable to open Device");
  
  if((input->fileHandle = open(argv[1], O_RDONLY)) == -1)
    error("Unable to open File");
  
  if(parseFile(output, input))
    error("Couldn't parse File");
  
  close(input->fileHandle);
  free(input);
  FT_Close(output->devHandle);
  free(output->data);
  free(output->bytes);
  free(output);
  
  return 0;
}