#include <SPI.h>
#include <GD.h>

#define HAVE_JTAG 1

#if 0
class ReducedRSerial : public Stream
{
  public:
    void begin(long baud) {
      uint16_t baud_setting;
      bool use_u2x;

      if (baud > F_CPU / 16) {
        use_u2x = true;
      } else {
        // figure out if U2X mode would allow for a better connection

        uint8_t nonu2x_baud_error = abs((int)(255-((F_CPU/(16*(((F_CPU/8/baud-1)/2)+1))*255)/baud)));
        uint8_t u2x_baud_error = abs((int)(255-((F_CPU/(8*(((F_CPU/4/baud-1)/2)+1))*255)/baud)));
      
        // prefer non-U2X mode because it handles clock skew better
        use_u2x = (nonu2x_baud_error > u2x_baud_error);
      }
      
      if (use_u2x) {
        UCSR0A = 1 << U2X0;
        baud_setting = (F_CPU / 4 / baud - 1) / 2;
      } else {
        UCSR0A = 0;
        baud_setting = (F_CPU / 8 / baud - 1) / 2;
      }

      UBRR0H = baud_setting >> 8;
      UBRR0L = baud_setting;
      UCSR0B |= (1 << TXEN0);
      UCSR0B |= (1 << RXEN0);
    }
    virtual void write(uint8_t v) {
      while (!((UCSR0A) & (1 << UDRE0)))
        ;
      UDR0 = v;
    }
    virtual int available(void) { return 0; }
    virtual int read(void) { return 0; }
    virtual void flush(void) {}
    using Print::write; // pull in write(str) and write(buf, size) from Print
};

ReducedRSerial RSerial;
#else

static void startserial(long baud)
{
  uint16_t baud_setting;
  bool use_u2x;

  if (baud > F_CPU / 16) {
    use_u2x = true;
  } else {
    // figure out if U2X mode would allow for a better connection

    uint8_t nonu2x_baud_error = abs((int)(255-((F_CPU/(16*(((F_CPU/8/baud-1)/2)+1))*255)/baud)));
    uint8_t u2x_baud_error = abs((int)(255-((F_CPU/(8*(((F_CPU/4/baud-1)/2)+1))*255)/baud)));
  
    // prefer non-U2X mode because it handles clock skew better
    use_u2x = (nonu2x_baud_error > u2x_baud_error);
  }
  
  if (use_u2x) {
    UCSR0A = 1 << U2X0;
    baud_setting = (F_CPU / 4 / baud - 1) / 2;
  } else {
    UCSR0A = 0;
    baud_setting = (F_CPU / 8 / baud - 1) / 2;
  }

  UBRR0H = baud_setting >> 8;
  UBRR0L = baud_setting;
  UCSR0B |= (1 << TXEN0);
  UCSR0B |= (1 << RXEN0);
}

static char buf[128] = "xxx\n";

static void emit(char c)
{
  while (!((UCSR0A) & (1 << UDRE0)))
    ;
  UDR0 = c;
}

static void cr()
{
  emit('\r');
  emit('\n');
}

static byte tohex(byte v)
{
  v &= 0xf;
  if (v < 10)
    return (v + '0');
  else
    return (v - 10 + 'A');
}

static void hex1(byte v)
{
  emit(tohex(v));
}

static void hex2(byte v)
{
  hex1(v >> 4);
  hex1(v);
}

static void hex8(unsigned long v)
{
  hex1(v >> 28);
  hex1(v >> 24);
  hex1(v >> 20);
  hex1(v >> 16);
  hex1(v >> 12);
  hex1(v >> 8);
  hex1(v >> 4);
  hex1(v);
}

static void decimal(unsigned int d)
{
  if (d != 0) {
    unsigned int ld = d / 10;
    byte ud = d % 10;
    decimal(ld);
    SPI.transfer('0' + ud);
  }
}

static void decimal(int x, int y, unsigned int d)
{
  GD.__wstart((y << 6) + x);
  decimal(d);
  GD.__end();
}

static void xprintf(char *fmt, ...)
{
#if 1
  va_list args;
  va_start(args, fmt);
  vsprintf(buf, fmt, args);

  char *pc = buf;
#else
  char *pc = fmt;
#endif
  while (*pc) 
    emit(*pc++);
}
#define printf xprintf

#endif

int atxy(int x, int y)
{
  return (y << 6) + x;
}

void show_stripes()
{
  int y = 5;
  int i;
  GD.fill(RAM_CHR + 2048, 0, 2048);
  for (i = 0; i < 32; i++) {
    GD.wr16(RAM_PAL + (0x80 + i) * 8, RGB(8 * i, 0, 0));
    GD.wr16(RAM_PAL + (0xa0 + i) * 8, RGB(0, 8 * i, 0));
    GD.wr16(RAM_PAL + (0xc0 + i) * 8, RGB(0, 0, 8 * i));
    GD.wr(atxy(i, y + 24), 0x80 + i);
    GD.wr(atxy(i, y + 25), 0xa0 + i);
    GD.wr(atxy(i, y + 26), 0xc0 + i);
  }
  GD.putstr(0, y + 28, "R");
  GD.putstr(0, y + 29, "G");
  GD.putstr(0, y + 30, "B");
  GD.putstr(4, y + 31, "0");
  GD.putstr(8, y + 31, "1");
  GD.putstr(16, y + 31, "2");

  GD.wr(atxy(4, y + 28), 0x80 + 4);
  GD.wr(atxy(8, y + 28), 0x80 + 8);
  GD.wr(atxy(16, y + 28), 0x80 + 16);

  GD.wr(atxy(4, y + 29), 0xa0 + 4);
  GD.wr(atxy(8, y + 29), 0xa0 + 8);
  GD.wr(atxy(16, y + 29), 0xa0 + 16);

  GD.wr(atxy(4, y + 30), 0xc0 + 4);
  GD.wr(atxy(8, y + 30), 0xc0 + 8);
  GD.wr(atxy(16, y + 30), 0xc0 + 16);
}

#include "lena.h"

static void show_lena()
{
  GD.copy(RAM_SPRPAL, GET_FAR_ADDRESS(lenapal), sizeof(lenapal));
  int i;
  for (i = 0; i < 64; i++)
    GD.sprite(i, 256 + ((i & 7) << 4), 104 + 2 * (i & 070), i, 0, 0);
  for (i = 64; i < 512; i++)
    GD.sprite(i, 400, 400, 0, 0, 0);
  GD.uncompress(RAM_SPRIMG, GET_FAR_ADDRESS(lenaimg));
}

// ----------------------------------------------------------------------
//     DTMF sending of DNA
// ----------------------------------------------------------------------

static void dtmf(byte c)
{
  int f0, f1;
  switch (c) {
    case '1': f0 = 697; f1 = 1209; break;
    case '2': f0 = 697; f1 = 1336; break;
    case '3': f0 = 697; f1 = 1477; break;
    case 'A': f0 = 697; f1 = 1633; break;
    case '4': f0 = 770; f1 = 1209; break;
    case '5': f0 = 770; f1 = 1336; break;
    case '6': f0 = 770; f1 = 1477; break;
    case 'B': f0 = 770; f1 = 1633; break;
    case '7': f0 = 852; f1 = 1209; break;
    case '8': f0 = 852; f1 = 1336; break;
    case '9': f0 = 852; f1 = 1477; break;
    case 'C': f0 = 852; f1 = 1633; break;
    case '*': f0 = 941; f1 = 1209; break;
    case '0': f0 = 941; f1 = 1336; break;
    case '#': f0 = 941; f1 = 1477; break;
    case 'D': f0 = 941; f1 = 1633; break;
    default:
      GD.voice(0, 0, 0, 0, 0);
      GD.voice(1, 0, 0, 0, 0);
      return;
  }
  GD.voice(0, 0, 4 * f0, 64, 64);
  GD.voice(1, 0, 4 * f1, 64, 64);
}

static void dtmfhex(byte v)
{
  static const char hex[] = "0123456789ABCD*#";
  dtmf(hex[v & 0xf]);
}

// play DNA via DTMF when index is 0-41
// rising tone when index is 48-62

static void dna_dmtx(byte index)
{
  if (index < (3 * 14) && ((index % 3) < 2)) {
    byte di = index / 3;
    byte v = GD.rd(COMM + di / 2);
    if ((di & 1) == 0)
      v >>= 4;
    dtmfhex(v);
  } else if ((48 <= index) && (index <= 62)) {
    GD.voice(0, 0, 1 << (index - 48), 128, 128);
    GD.voice(1, 0, 0, 0, 0);
  } else {
    dtmf(' ');
  }
}

#include "loadcommon.h"
#include "assetlib.h"

// http://www.atmel.com/dyn/resources/prod_documents/doc3638.pdf

#if 1
#undef spix
static inline byte spix(byte v)
{
  SPDR = v;
  while (!(SPSR & _BV(SPIF)))
    ;
  return SPDR;
}
#endif

static void failmsg(const char *msg);

static byte stage[8 * 264];

static void loadpage(int pg, uint_farptr_t data)
{
  byte i;
  int j;

  byte x = (pg * 40 / 572);
  GD.wr(64 * 8 + 10 + x, 127);

  pgcmd(0x50, pg);
  UNSEL_local();
  dna_dmtx(pg / 8);
  UNSEL_local_wait();

  byte *src = stage;
  for (i = 0; i < 8; i++) {
    pgcmd(0x84, pg + i);
    for (j = 264; j; j--)
      SPDR = pgm_read_byte_far(data++);
    while (!(SPSR & _BV(SPIF)))
      ;
    UNSEL_local();

    pgcmd(0x88, pg + i);
    UNSEL_local_wait();

    pgcmd(0x60, pg + i);
    UNSEL_local_wait();
    if (status() & (1 << 6))
      failmsg("Compare failed");
  }
}

#include "flashimgall.h"

static void waitready()
{
  while ((status() & 0x80) == 0)
    ;
}

#if 0
static byte pagebuf[8 * 264];

static void erase8page(int pg)
{
  // RSerial.println(data, HEX);
  pgcmd(0x50, pg);
  UNSEL_local();
}

static void write8page(int pg)
{
  int i, j;
  byte *pc = pagebuf;
  for (i = 0; i < 8; i++, pg++) {
    pgcmd(0x84, pg);
    for (j = 0; j < 264; j++) {
      spix(*pc++);
    }
    UNSEL_local();

    pgcmd(0x88, pg);
    UNSEL_local();
    waitready();
  }
}
#endif

static void failmsg(const char *msg)
{
  emit('F'); emit(' ');
  const char *pc;
  for (pc = msg; *pc; pc++)
    emit(*pc);
  cr();

  for (;;) {
    GD.wr16(BG_COLOR, RGB(32, 0, 0));
    GD.putstr(0, 18, msg);
    delay(50);
    GD.wr16(BG_COLOR, RGB(0, 0, 0));
    GD.putstr(0, 18, "                                                  ");
    delay(50);
    if (digitalRead(40) == 0)
      asm volatile ("jmp 0x0000"); 
  }
}

static void test_crc(void)
{
  unsigned long crc = ~0;
  crc = crc_update(crc, 'H');
  crc = crc_update(crc, 'E');
  crc = crc_update(crc, 'L');
  crc = crc_update(crc, 'L');
  crc = crc_update(crc, 'O');
  crc = ~crc;
  if (crc != 0xc1446436L) {
    failmsg("internal test failed");
  }
}

static byte key(void)
{
   while ( !(UCSR0A & (1<<RXC0)) )   //Wait for data to be received 
      ; 
   return UDR0;   //Get and return received data from buffer 
}


static PROGMEM prog_uchar jtag_dna[] = {
#include "jtagcan.h"
};

#include "jtag.h"

#include "dna.h"

static void jtagpage(uint_farptr_t src)
{
  int i;
  digitalWrite(6, 0);
  for (i = 0; i < (8 * 264); i++) {
    byte v = pgm_read_byte_far(src++);

#if 0
    int j;
    for (j = 0; j < 8; j++) {
      digitalWrite(5, (v >> 7) & 1);
      digitalWrite(7, 1);
      digitalWrite(7, 0);
      v <<= 1;
    }
#else
#define CLK() (PORTH = 0x10, PORTH = 0x00)
    byte h = (v >> 4);
    PORTE = h & 0x08; CLK(); h <<= 1;
    PORTE = h & 0x08; CLK(); h <<= 1;
    PORTE = h & 0x08; CLK(); h <<= 1;
    PORTE = h & 0x08; CLK();
    h = v;
    PORTE = h & 0x08; CLK(); h <<= 1;
    PORTE = h & 0x08; CLK(); h <<= 1;
    PORTE = h & 0x08; CLK(); h <<= 1;
    PORTE = h & 0x08; CLK();
#endif
  }
}

static void LoadBSIR_BS(byte instruction)
{
  if (current_state != SHIFT_IR) {
    rti();
    go_state(1); go_state(1); go_state(0); go_state(0);
  }
  ASSERT_STATE(SHIFT_IR);
  do_nbit(6, instruction);
  go_state(1); 
  ASSERT_STATE(UPDATE_IR);
  go_state(1); go_state(0); go_state(0);
  ASSERT_STATE(SHIFT_DR);

  jtagpage(GET_FAR_ADDRESS(img0));
  jtagpage(GET_FAR_ADDRESS(img1));
  jtagpage(GET_FAR_ADDRESS(img2));
  jtagpage(GET_FAR_ADDRESS(img3));
  jtagpage(GET_FAR_ADDRESS(img4));
  jtagpage(GET_FAR_ADDRESS(img5));
  jtagpage(GET_FAR_ADDRESS(img6));
  jtagpage(GET_FAR_ADDRESS(img7));
  jtagpage(GET_FAR_ADDRESS(img8));
  jtagpage(GET_FAR_ADDRESS(img9));
  jtagpage(GET_FAR_ADDRESS(img10));
  jtagpage(GET_FAR_ADDRESS(img11));
  jtagpage(GET_FAR_ADDRESS(img12));
  jtagpage(GET_FAR_ADDRESS(img13));
  jtagpage(GET_FAR_ADDRESS(img14));
  jtagpage(GET_FAR_ADDRESS(img15));
  jtagpage(GET_FAR_ADDRESS(img16));
  jtagpage(GET_FAR_ADDRESS(img17));
  jtagpage(GET_FAR_ADDRESS(img18));
  jtagpage(GET_FAR_ADDRESS(img19));
  jtagpage(GET_FAR_ADDRESS(img20));
  jtagpage(GET_FAR_ADDRESS(img21));
  jtagpage(GET_FAR_ADDRESS(img22));
  jtagpage(GET_FAR_ADDRESS(img23));
  jtagpage(GET_FAR_ADDRESS(img24));
  jtagpage(GET_FAR_ADDRESS(img25));
  jtagpage(GET_FAR_ADDRESS(img26));
  jtagpage(GET_FAR_ADDRESS(img27));
  jtagpage(GET_FAR_ADDRESS(img28));
  jtagpage(GET_FAR_ADDRESS(img29));
  jtagpage(GET_FAR_ADDRESS(img30));
  jtagpage(GET_FAR_ADDRESS(img31));
  jtagpage(GET_FAR_ADDRESS(img32));
  jtagpage(GET_FAR_ADDRESS(img33));
  jtagpage(GET_FAR_ADDRESS(img34));
  jtagpage(GET_FAR_ADDRESS(img35));
  jtagpage(GET_FAR_ADDRESS(img36));
  jtagpage(GET_FAR_ADDRESS(img37));
  jtagpage(GET_FAR_ADDRESS(img38));
  jtagpage(GET_FAR_ADDRESS(img39));
  jtagpage(GET_FAR_ADDRESS(img40));
  jtagpage(GET_FAR_ADDRESS(img41));
  jtagpage(GET_FAR_ADDRESS(img42));
  jtagpage(GET_FAR_ADDRESS(img43));
  jtagpage(GET_FAR_ADDRESS(img44));
  jtagpage(GET_FAR_ADDRESS(img45));
  jtagpage(GET_FAR_ADDRESS(img46));
  jtagpage(GET_FAR_ADDRESS(img47));
  jtagpage(GET_FAR_ADDRESS(img48));
  jtagpage(GET_FAR_ADDRESS(img49));
  jtagpage(GET_FAR_ADDRESS(img50));
  jtagpage(GET_FAR_ADDRESS(img51));
  jtagpage(GET_FAR_ADDRESS(img52));
  jtagpage(GET_FAR_ADDRESS(img53));
  jtagpage(GET_FAR_ADDRESS(img54));
  jtagpage(GET_FAR_ADDRESS(img55));
  jtagpage(GET_FAR_ADDRESS(img56));
  jtagpage(GET_FAR_ADDRESS(img57));
  jtagpage(GET_FAR_ADDRESS(img58));
  jtagpage(GET_FAR_ADDRESS(img59));
  jtagpage(GET_FAR_ADDRESS(img60));
  jtagpage(GET_FAR_ADDRESS(img61));
  jtagpage(GET_FAR_ADDRESS(img62));
  jtagpage(GET_FAR_ADDRESS(img63));
  jtagpage(GET_FAR_ADDRESS(img64));
  jtagpage(GET_FAR_ADDRESS(img65));
  jtagpage(GET_FAR_ADDRESS(img66));
  jtagpage(GET_FAR_ADDRESS(img67));
  jtagpage(GET_FAR_ADDRESS(img68));
  jtagpage(GET_FAR_ADDRESS(img69));
  jtagpage(GET_FAR_ADDRESS(img70));
  go_state(1); 

  go_state(1); 
  ASSERT_STATE(UPDATE_DR);
  go_state(0); 
  ASSERT_STATE(RUN_TEST_IDLE);
}

static void jtag_cold()
{
  pinMode(7, OUTPUT);   // TCK
  pinMode(6, OUTPUT);   // TMS
  pinMode(5, OUTPUT);   // TDI
  pinMode(4, INPUT);    // TDO
  digitalWrite(7, 0);
  rti();
}

static void loadjtag()
{
  jtag_cold();

  if (0x02218093 != LoadBSIRthenBSDR(IDCODE, 0))
    failmsg("Bad IDCODE");

  LoadBSIR(JPROGRAM);
  LoadBSIR(CFG_IN);
  delay(1);
  // long started = millis();
  LoadBSIR_BS(CFG_IN);
  tlr();
  LoadBSIR(JSTART);
  // emit('t'); hex8(millis() - started); cr();
  if (LoadBSIRthenBSDR(USERCODE, 0) != 0x09470947)
    failmsg("Bad USERCODE");
}


void screenshot(unsigned int frame)
{
  int yy, xx;
  byte undone[38];  // 300-long bitmap of lines pending

  // initialize to 300 ones
  memset(undone, 0xff, 37);
  undone[37] = 0xf;
  int nundone = 300;

  emit(0xa5);   // sync byte
  emit(lowByte(frame));
  emit(highByte(frame));

  while (nundone) {
    // find a pending line a short distance ahead of the raster
    int hwline = GD.rd16(SCREENSHOT_Y) & 0x1ff;
    for (yy = (hwline + 7) % 300; ((undone[yy>>3] >> (yy&7)) & 1) == 0; yy = (yy + 1) % 300)
      ;
    GD.wr16(SCREENSHOT_Y, 0x8000 | yy);   // ask for it

    // housekeeping while waiting: mark line done and send yy
    undone[yy>>3] ^= (1 << (yy&7));
    nundone--;
    emit(lowByte(yy));
    emit(highByte(yy));
    while ((GD.rd(SCREENSHOT_Y + 1) & 0x80) == 0)
      ;

    // Now send the line, compressing zero pixels
    uint16_t zeroes = 0;
    for (xx = 0; xx < 800; xx += 2) {
      uint16_t v = GD.rd16(SCREENSHOT + xx);
      if (v == 0) {
        zeroes++;
      } else {
        if (zeroes) {
          emit(lowByte(zeroes));
          emit(0x80 | highByte(zeroes));
          zeroes = 0;
        }
        emit(lowByte(v));
        emit(highByte(v));
      }
    }
    if (zeroes) {
      emit(lowByte(zeroes));
      emit(0x80 | highByte(zeroes));
    }
  }
  GD.wr16(SCREENSHOT_Y, 0);   // restore screen to normal
}

static void say_serial()
{
  byte i;
  for (i = 0; i < 7; i++)
    hex2(GD.rd(COMM + i));
  emit('%');
}

#include "reload.h"

void reload()
{
  GD.microcode(GET_FAR_ADDRESS(reload_code), sizeof(reload_code));
}

byte isconnected(byte arduino, byte fpga)
{
  pinMode(arduino, OUTPUT);
  byte i;
  for (i = 0; i < 10; i++) {
    byte v = i & 1;
    digitalWrite(arduino, v);
    if (sample(fpga) != v)
      return 0;
  }
  pinMode(arduino, INPUT);
  return 1;
}


////////////////////////////////////////////////////////////////////////

unsigned int tcnt2;

static void setupisr()
{
   /* First disable the timer overflow interrupt while we're configuring */  
  TIMSK2 &= ~(1<<TOIE2);  
  
  /* Configure timer2 in normal mode (pure counting, no PWM etc.) */  
  TCCR2A &= ~((1<<WGM21) | (1<<WGM20));  
  TCCR2B &= ~(1<<WGM22);  
  
  /* Select clock source: internal I/O clock */  
  ASSR &= ~(1<<AS2);  
  
  /* Disable Compare Match A interrupt enable (only want overflow) */  
  TIMSK2 &= ~(1<<OCIE2A);  
  
  /* Now configure the prescaler to CPU clock divided by 128 */  
  TCCR2B |= (1<<CS22)  | (1<<CS20); // Set bits  
  TCCR2B &= ~(1<<CS21);             // Clear bit  
  
  /* We need to calculate a proper value to load the timer counter. 
   * The following loads the value 131 into the Timer 2 counter register 
   * The math behind this is: 
   * (CPU frequency) / (prescaler value) = 125000 Hz = 8us. 
   * (desired period) / 8us = 125. 
   * MAX(uint8) + 1 - 125 = 131; 
   */  
  /* Save value globally for later reload in ISR */  
  tcnt2 = 131;   
  
  /* Finally load end enable the timer */  
  TCNT2 = tcnt2;  
  TIMSK2 |= (1<<TOIE2);  
}

byte ledcode[4] = { 0x0, 0x0, 0x0, 0x0 };

static void frobled()
{
  // http://www.jameco.com/Jameco/Products/ProdDS/2006810.pdf
#define SA 1
#define SB 2
#define SC 4
#define SD 8
#define SE 16
#define SF 32
#define SG 64
  //      A
  //   F     B
  //      G
  //   E     C
  //      D

  byte segments[] = {
    /* 0 */ SA|SF|SE|SD|SC|SB, 
    /* 1 */ SB|SC,
    /* 2 */ SA|SB|SG|SE|SD,
    /* 3 */ SA|SB|SG|SC|SD,
    /* 4 */ SF|SG|SB|SC,
    /* 5 */ SA|SG|SD| SF|SC,
    /* 6 */ SA|SG|SD| SF|SC|SE,
    /* 7 */ SB|SC| SA,
    /* 8 */ SA|SG|SD| SF|SC|SE | SB,
    /* 9 */ SA|SG|SD| SF|SC|SB,
    /* A */ SA|SG| SF|SC|SE | SB,
    /* b */ SG|SD| SF|SC|SE,
    /* c */ SG|SE|SD,
    /* d */ SG|SE|SD| SB|SC,
    /* E */ SA|SG|SD| SF|SE,
    /* F */ SA|SG| SF|SE,
    /*   */ 0,
  };
  static byte dig = 0;
  byte i;
  for (i = 0; i < 7; i++)
    digitalWrite(14 + i, HIGH);

  digitalWrite(A15, (dig == 0) ? HIGH : LOW);
  digitalWrite(A14, (dig == 1) ? HIGH : LOW);
  digitalWrite(A13, (dig == 2) ? HIGH : LOW);
  digitalWrite(A12, (dig == 3) ? HIGH : LOW);

  for (i = 0; i < 7; i++)
    digitalWrite(14 + i, ((segments[ledcode[dig]] >> i) & 1) ? LOW : HIGH);

  dig = (dig + 1) & 3;
}

ISR(TIMER2_OVF_vect) {  
  /* Reload the timer */  
  TCNT2 = tcnt2;  
  frobled();
}

static void setup_gd()
{
  int i;

  memset(ledcode, 16, sizeof(ledcode));
  if (HAVE_JTAG) {
    jtag_cold();
    hex8(LoadBSIRthenBSDR(IDCODE, 0)); cr();

    if (0x02218093 != LoadBSIRthenBSDR(IDCODE, 0))
      failmsg("JTAG failed");

    if (0) {
      char fails[160] = "";
      LoadBSIR(JSHUTDOWN);
      SPI.end();

      if (!isconnected(9, 88))
        strcat(fails, "pin 9 FAIL (check R12).  ");
      if (!isconnected(13, 84))
        strcat(fails, "pin 13 FAIL (check R15).  ");
      if (!isconnected(11, 85))
        strcat(fails, "pin 11 FAIL (check R13).  ");
      if (!isconnected(12, 86))
        strcat(fails, "pin 12 FAIL (check R14).  ");
      if (strlen(fails) != 0)
        failmsg(fails);

      int hist[2] = {0,0};
      for (i = 0; i < 30; i++) {
        hist[sample(83)]++;
      }
      if ((hist[0] == 0) || (hist[1] == 0))
        failmsg("CLOCK test failed");
    }

    //if (0x02218093 != LoadBSIRthenBSDR(IDCODE, 0))
      //failmsg("Bad IDCODE");

    loadjtag();
  }

  GD.begin();

  if (GD.rd(IDENT) != 0x6d)
    failmsg("No IDENT");

  GD.microcode(GET_FAR_ADDRESS(dna_code), sizeof(dna_code));
  delay(1);

  ledcode[0] = GD.rd(COMM + 5) >> 4;
  ledcode[1] = GD.rd(COMM + 5) & 15;
  ledcode[2] = GD.rd(COMM + 6) >> 4;
  ledcode[3] = GD.rd(COMM + 6) & 15;

  // tell host that testing is started
  say_serial();
  emit('T');
  cr();

  GD.ascii();
  GD.putstr(0, 0, "Manufacturing setup");
  show_stripes();
  // show_lena();

  pinMode(2, OUTPUT);
  digitalWrite(2, HIGH);
  delay(1);
  GD.wr(IOMODE, 'F');

  // COMM,COMM+6 now hold 56-bit DNA

  if ((status() & 0x83) != 0x80)
    failmsg("Bad status - flash fault?");

  GD.putstr(0, 6, "Serial");
  for (i = 0; i < 7; i++) {
    byte v = GD.rd(COMM + i);
    GD.wr(atxy(10 + 2 * i + 0, 6), tohex(v >> 4));
    GD.wr(atxy(10 + 2 * i + 1, 6), tohex(v));
  }

  GD.putstr(0, 8, "loading");
  loadall();

  GD.putstr(0, 10, "verifing");
  unsigned long sum = 0;
  unsigned long li;
  pgcmd(0x03, 0);
#if 0
  byte p = spix(0);
  for (li = 0; li < (264L * img_SIZE) - 1; li++) {
    SPDR = 0; sum += p; p = SPDR;
    SPDR = 0; sum += p; p = SPDR;
    SPDR = 0; sum += p; p = SPDR;
    SPDR = 0; sum += p; p = SPDR;
    SPDR = 0; sum += p; p = SPDR;
    SPDR = 0; sum += p; p = SPDR;
    SPDR = 0; sum += p; p = SPDR;
    SPDR = 0; sum += p; p = SPDR;
  }
  sum += p;
#else
  for (li = 0; li < (264L * 8 * img_SIZE) - 1; li++)
    sum += spix(0);
#endif
  UNSEL_local();

  if (sum != img_SUM)
    failmsg("load failed - bad Flash?");
  GD.putstr(10, 10, "Flash loaded OK");

/* JCB */  // Read out the logged stats from 'clone' on page 1000
/* JCB */  {
/* JCB */    say_serial();
/* JCB */    pgcmd(0x03, 1000);
/* JCB */    emit('I');
/* JCB */    emit('%');
/* JCB */    for (i = 0; i < 6; i++)
/* JCB */      hex2(spix(0));
/* JCB */    UNSEL_local();
/* JCB */    cr();
/* JCB */  }

  if (1) {
    long t0 = millis();

    page = ASSET0_PAGE; GD_uncompress(GET_FAR_ADDRESS(assetlib0));
    page = ASSET1_PAGE; GD_uncompress(GET_FAR_ADDRESS(assetlib1));
    page = ASSET2_PAGE; GD_uncompress(GET_FAR_ADDRESS(assetlib2));
    if (flash_sum(ASSET0_PAGE, ASSET0_LEN) != ASSET0_SUM)
      failmsg("asset0 load fail");
    if (flash_sum(ASSET1_PAGE, ASSET1_LEN) != ASSET1_SUM)
      failmsg("asset1 load fail");
    if (flash_sum(ASSET2_PAGE, ASSET2_LEN) != ASSET2_SUM)
      failmsg("asset2 load fail");

    GD.putstr(10, 12, "Assets loaded");

    emit('t'); hex8(millis() - t0); cr();
  }

  GD.putstr(0, 14, "reload");
  if (GD.rd(IOMODE) != 'F')
    failmsg("expected IOMODE F");

  if (HAVE_JTAG) {
    delay(200);
    reload();
    delay(700);
    if (GD.rd(IDENT) != 0x6d)
      failmsg("No IDENT");
    if (GD.rd(IOMODE) != 0)
      failmsg("expcted IOMODE 00");
  }
  GD.ascii();
  GD.putstr(0, 0, "Setup complete");
  show_stripes();
  GD.microcode(GET_FAR_ADDRESS(dna_code), sizeof(dna_code));
  GD.wr(IOMODE, 'F');
  for (i = 0; i < 64; i++)
    GD.voice(i, 0, 0, 0, 0);

  // screenshot(0);

  // long took = millis() - t0;
  // GD.putstr(0, 30, "took");
  // decimal(5, 30, (int)took);

  // Success! write DNA, REVISION, and flash sig to host
  say_serial();
  emit('F');
  emit('%');
  hex2(REVISION);
  emit('%');
  pgcmd(0x77, 0);
  for (i = 0; i < 64; i++)
    spix(0);
  for (i = 0; i < 64; i++)
    hex2(spix(0));
  UNSEL_local();
  GD.wr(IOMODE, 0);     // disconnect from flash
  cr();
  emit('!'); cr();
}

void setup()
{
  int i;

  startserial(115200);
  emit('/'); cr();

  if (1) {
    pinMode(A15, OUTPUT);
    pinMode(A14, OUTPUT);
    pinMode(A13, OUTPUT);
    pinMode(A12, OUTPUT);
    for (i = 14; i < 21; i++)
      pinMode(i, OUTPUT);

    setupisr();
    // while (1) { delay(5); frobled(); }
  }
  pinMode(40, INPUT);
  digitalWrite(40, HIGH);
}

void loop()
{
  setup_gd();
  for (;;) {
    byte i;
    for (i = 0; i < 75; i++) {
      dna_dmtx(i);
      delay(30);
      if (digitalRead(40) == 0)
        goto done;
    }
  }
done:
  ;
}
