#include <stdint.h>
#include <stdio.h>
#include <libftdi/ftdi.h>
#include <signal.h>

#define DOT_DIV 100
int run =1;
void ctrl_c(int sig)
{
  run = 0;
}

int main(int argc, char **args)
{
  char const *desc    = 0;
  char const *serial  = 0;
  int vendor    = 0;
  int product   = 0;
  int read;
  int written;
  int nr_tests= 1;
  int latency = 1;
  int i;
  int tms_bits;
  struct ftdi_context ftdi;
  uint16_t value;
  uint8_t  buf[256]={ SET_BITS_LOW, 0x00, 0x0b,
		      TCK_DIVISOR,  0x00, 0x00 ,
		      SET_BITS_HIGH, ~0x84, 0x84};

  while(1) 
    {
      switch(getopt(argc, args, "?hD:L:P:s:V:t:T:")) 
	{
	case -1:
	  goto args_done;
	  
	case 'D':
	  desc = optarg;
	  break;
	  
	case 'V':
	  value = strtol(optarg, NULL, 0);
	  vendor = value;
	  break;
	  
	case 'L':
	  value = strtol(optarg, NULL, 0);
	  latency = value;
	  break;

	case 'P':
	  value = strtol(optarg, NULL, 0);
	  product = value;
	  break;
	  
	case 's':
	  serial = optarg;
	  break;
	
	case 't':
	  tms_bits = strtol(optarg, NULL, 0);
	  break;

	case 'T':
	  nr_tests = strtol(optarg, NULL, 0);
	  break;
	
	case '?':
	case 'h':
	default:
	  fprintf
	    ( stderr,
	      "\nUsage:\t%s [-D Product string] [-s SerialNumber string] [-V VID] [-P PID]\n", args[0]);
	  exit(255);
	}
    }
 args_done:

  if (vendor == 0)
    vendor = 0x403;
  if(product == 0)
    product = 0x6010;
  if (tms_bits >7)
    tms_bits = 7;
  else if (tms_bits < 1)
    tms_bits = 1;

  signal (SIGINT, ctrl_c);
  ftdi_init(&ftdi);
  if(ftdi_set_interface(&ftdi, INTERFACE_A) < 0) 
      fprintf(stderr,"ftdi_set_interface: %s\n",
	      ftdi_get_error_string(&ftdi));
  else if (ftdi_usb_open_desc(&ftdi, vendor, product, desc, serial) < 0)
      fprintf(stderr,"ftdi_usb_open: %s",
	      ftdi_get_error_string(&ftdi));
  else if(ftdi_set_latency_timer(&ftdi, latency) <0)
      fprintf(stderr,"ftdi_set_latency_timer: %s\n",
	      ftdi_get_error_string(&ftdi));
  else if(ftdi_set_bitmode(&ftdi, 0xfb, BITMODE_MPSSE) < 0)
      fprintf(stderr,"ftdi_set_bitmode: %s\n",
	      ftdi_get_error_string(&ftdi));
  else if(ftdi_usb_purge_buffers(&ftdi) < 0)
      fprintf(stderr,"ftdi_usb_purge_buffers: %s\n",
	      ftdi_get_error_string(&ftdi));

  // Prepare for JTAG operation
  /* FIXME: Without this read, consecutive runs on the FT2232H may hang */
  if(ftdi.type == TYPE_2232H)
    ftdi_read_data(&ftdi, buf+16,5);

  written = ftdi_write_data(&ftdi, buf, 9);
  if(written != 9) 
    {
      fprintf(stderr,"mpsse_send: Short write %d vs %d, Err: %s\n", 
	      written, 9, ftdi_get_error_string(&ftdi));
      goto err_exit;
    }
  buf[0] = 0x4b;
  buf[1] = (tms_bits-1);
  buf[2] = 0x85;
  buf[3] = 0x19;
  buf[4] = 0;
  buf[5] = 0;
  buf[6] = 0;
  buf[7] = 0x81;

  i =0;
  while(run && (i < nr_tests))
    {
      written = ftdi_write_data(&ftdi, buf, 8);
      if(written != 8) 
	{
	  fprintf(stderr,"\nShort write %d vs %d at run %8d, Err: %s\n", 
		  written, 8, i, ftdi_get_error_string(&ftdi));
	  goto err_exit;
	}
      if(i%DOT_DIV == (DOT_DIV-1))
	{
	  fprintf(stderr,".");
	  fflush(stderr);
	}
      i++;
      usleep(10000);
      read = ftdi_read_data(&ftdi, buf+10, 1);
      if(read != 1) 
	{
	  fprintf(stderr,"Short read at run %d %d vs %d requested, Err: %s\n", 
		  i, read, 1, ftdi_get_error_string(&ftdi));
	  goto err_exit;
	}
    }
  fprintf(stderr,"\n");
  if (buf[10] & 0x08)
    fprintf(stdout,"Unexpected TMS value due to chip bug with %d tms_bits, Low Byte 0x%02x\n",
	    tms_bits, buf[10]);
  else
    fprintf(stdout,"All fine for  %d tms_bits!\n", tms_bits);
 err_exit:
  buf[0] = SET_BITS_LOW;
  buf[1] = 0x00;
  buf[2] = 0x00;
  ftdi_write_data(&ftdi, buf, 3);
  usleep(100000);
  ftdi_set_bitmode(&ftdi, 0, BITMODE_RESET);
  usleep(100000);
  ftdi_usb_reset(&ftdi);
  usleep(100000);
  ftdi_usb_close(&ftdi);
  ftdi_deinit(&ftdi);
  return 0;
  }