 
     LED_LOW;
 }
+
+static void n64Interrupt()
+{
+    noInterrupts();
+
+    unsigned char data, addr;
+    int ticksSinceLast = readTimer();
+    startTimer();
+    volatile uint32_t *config = portConfigRegister(N64_PIN);
+    uint32_t oldConfig = *config;
+    bool haveFrame;
+//    unsigned int curPos;
+
+    *config &= ~0x000F0000;
+
+    // Wait for incoming 64 command
+    // this will block until the N64 sends us a command
+    get_n64_command();
+
+    // 0x00 is identify command
+    // 0x01 is status
+    // 0x02 is read
+    // 0x03 is write
+    switch (n64_command)
+    {
+        case 0x00:
+        case 0xFF:
+            // identify
+            // mutilate the output_buffer array with our status
+            // we return 0x050001 to indicate we have a rumble pack
+            // or 0x050002 to indicate the expansion slot is empty
+            //
+            // 0xFF I've seen sent from Mario 64 and Shadows of the Empire.
+            // I don't know why it's different, but the controllers seem to
+            // send a set of status bytes afterwards the same as 0x00, and
+            // it won't work without it.
+            output_buffer[0] = 0x05;
+            output_buffer[1] = 0x00;
+            output_buffer[2] = 0x01;
+
+            n64_send(output_buffer, 3, 1);
+            Serial.println("P:");
+            break;
+        case 0x01:
+            haveFrame = (!finished && bufferHasData);
+            // If the TAS is finished, there's nothing left to do.
+            if (haveFrame)
+              memcpy(output_buffer, inputBuffer + bufferPos, 4);
+            else
+              memset(output_buffer, 0, 4);
+        
+            // blast out the pre-assembled array in output_buffer
+            n64_send(output_buffer, 4, 1);
+
+            logFrame(output_buffer, 4, curFrame + 1);
+
+            if (!haveFrame)
+              break;
+
+            curFrame++;
+
+            // update input buffer and make sure it doesn't take too long
+
+           /*Serial.print(F("Pos: "));
+            Serial.print(bufferPos);
+            Serial.print(F(" Data: "));
+            Serial.println(inputBuffer[bufferPos]);*/
+            
+            if (curFrame == numFrames) {
+              Serial.println("C:");
+              finished = true;
+            }
+
+            advanceBuffer(4);
+
+//            while (Serial.availableForWrite() && readTimer() < 10 * 1000 * MICRO_CYCLES && N64_QUERY);
+
+/*            if (!finished)
+              curPos = (curFrame * screen.width()) / numFrames;
+            screen.fill(255,0,0);
+            while (progressPos < curPos) {
+              screen.point(progressPos++, screen.height() - 1);
+            }
+            screen.fill(255,255,255);*/
+
+            break;
+        case 0x02:
+            // A read. If the address is 0x8000, return 32 bytes of 0x80 bytes,
+            // and a CRC byte.  this tells the system our attached controller
+            // pack is a rumble pack
+
+            // Assume it's a read for 0x8000, which is the only thing it should
+            // be requesting anyways
+            memset(output_buffer, 0x80, 32);
+            output_buffer[32] = 0xB8; // CRC
+
+            n64_send(output_buffer, 33, 1);
+            Serial.println(F("L:Got a read, what?"));
+
+            //Serial.println("It was 0x02: the read command");
+            break;
+        case 0x03:
+            // A write. we at least need to respond with a single CRC byte.  If
+            // the write was to address 0xC000 and the data was 0x01, turn on
+            // rumble! All other write addresses are ignored. (but we still
+            // need to return a CRC)
+
+            // decode the first data byte (fourth overall byte), bits indexed
+            // at 24 through 31
+            data = 0;
+            data |= (n64_raw_dump[16] != 0) << 7;
+            data |= (n64_raw_dump[17] != 0) << 6;
+            data |= (n64_raw_dump[18] != 0) << 5;
+            data |= (n64_raw_dump[19] != 0) << 4;
+            data |= (n64_raw_dump[20] != 0) << 3;
+            data |= (n64_raw_dump[21] != 0) << 2;
+            data |= (n64_raw_dump[22] != 0) << 1;
+            data |= (n64_raw_dump[23] != 0);
+
+            // get crc byte, invert it, as per the protocol for
+            // having a memory card attached
+            output_buffer[0] = crc_repeating_table[data] ^ 0xFF;
+
+            // send it
+            n64_send(output_buffer, 1, 1);
+            Serial.println(F("L:Got a write, what?"));
+
+            // end of time critical code
+            // was the address the rumble latch at 0xC000?
+            // decode the first half of the address, bits
+            // 8 through 15
+            addr = 0;
+            addr |= (n64_raw_dump[0] != 0) << 7;
+            addr |= (n64_raw_dump[1] != 0) << 6;
+            addr |= (n64_raw_dump[2] != 0) << 5;
+            addr |= (n64_raw_dump[3] != 0) << 4;
+            addr |= (n64_raw_dump[4] != 0) << 3;
+            addr |= (n64_raw_dump[5] != 0) << 2;
+            addr |= (n64_raw_dump[6] != 0) << 1;
+            addr |= (n64_raw_dump[7] != 0);
+
+            //Serial.println("It was 0x03: the write command");
+            //Serial.print("Addr was 0x");
+            //Serial.print(addr, HEX);
+            //Serial.print(" and data was 0x");
+            //Serial.println(data, HEX);
+            break;
+
+        case -1:
+            if (finished)
+              break;
+            Serial.print(F("W:Read timeout; "));
+            Serial.print(edgesRead);
+            Serial.println(F(" edges read"));
+            if (edgesRead >= 2) {
+              Serial.print(F("W: partial byte: "));
+              Serial.println(incompleteCommand, HEX);
+            }
+            Serial.print(F("W:"));
+            Serial.print(ticksSinceLast);
+            Serial.println(F(" cycles since last"));
+            if (curFrame > 100)
+              Serial.println(F("D:timeout"));
+            waitForIdle(1000);
+            break;
+
+        default:
+            if (finished)
+              break;
+            Serial.print(F("W:Unknown command: 0x"));
+            Serial.println(n64_command, HEX);
+            if (curFrame > 100)
+              Serial.println(F("D:inval"));
+            waitForIdle(1000);
+            break;
+    }
+
+    *config = oldConfig;
+
+    interrupts();
+}
+
+static void handleCommand(const String& cmd)
+{
+  if (cmd.startsWith("M:")) {
+    openTAS(cmd.substring(2));
+  } else if (cmd.startsWith("O:")) {
+    //dummy
+  } else if (cmd.startsWith("L:")) {
+    emitList(cmd.substring(2));
+  } else if (cmd.startsWith("MK:")) {
+    lockedPrintln("MK:", sd.mkdir(cmd.substring(3).c_str()));
+  } else if (cmd.startsWith("CR:")) {
+    if (writeFile.isOpen()) {
+      writeFile.close();
+    }
+    if (writeFile.open(&sd, cmd.substring(3).c_str(), O_WRITE | O_CREAT | O_TRUNC)) {
+      lockedPrintln("CR:OK");
+    } else {
+      lockedPrintln("CR:NAK");
+    }
+  } else if (cmd.startsWith("AP:")) {
+    appendData(hextobin(cmd.substring(3)));
+  } else if (cmd.startsWith("IN:")) {
+    size_t len = appendInputs(hextobin(cmd.substring(3)));
+    lockedPrintln("IN:", len);
+  } else if (cmd.startsWith("HL:")) {
+    hold = cmd[3] == '1';
+  } else if (cmd.startsWith("CL:")) {
+    writeFile.close();
+    lockedPrintln("CL:OK");
+  } else if (cmd.startsWith("PM:")) {
+    setPinMode(cmd.substring(3));
+  } else if (cmd.startsWith("DW:")) {
+    writePin(cmd.substring(3));
+  } else {
+    lockedPrintln("E:Unknown CMD:", cmd.c_str());
+  }
+}
+
+static void inputLoop()
+{
+  int newChar;
+  int charsRead = 0;
+  while ((newChar = Serial.read()) != -1 && (charsRead++ <= MAX_LOOP_LEN)) {
+    if (newChar == '\n') {
+      if (skippingInput)
+        Serial.println("E:Skipped too-long line");
+      else
+        handleCommand(inputString);
+      inputString = "";
+      skippingInput = false;
+    } else if (inputString.length() > MAX_CMD_LEN) {
+      skippingInput = true;
+    } else {
+      inputString += (char)newChar;
+    }
+  }
+}
+
+static void mainLoop()
+{
+  updateInputBuffer();
+  
+  // Record if it took longer than expected
+  /*updateTime = readTimer();
+  if (updateTime > 1000 * MICRO_CYCLES) {
+      Serial.print(F("Input buffer update took too long ("));
+      Serial.print(updateTime / MICRO_CYCLES);
+      Serial.println(F(" us)"));
+  }*/
+}
+
+void loop()
+{
+    inputLoop();
+    mainLoop();
+}
+
+void setup()
+{
+  ARM_DEMCR |= ARM_DEMCR_TRCENA;
+  ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
+
+  Serial.begin(SERIAL_BAUD_RATE);
+  while (!Serial) { ; } // wait for serial port to connect. Needed for native USB port only
+
+  // Status LED
+  digitalWrite(STATUS_PIN, LOW);
+  pinMode(STATUS_PIN, OUTPUT);
+
+  // Configure controller pins
+  pinMode(N64_PIN, INPUT_PULLUP);
+  digitalWrite(N64_PIN, LOW);
+
+  attachInterrupt(N64_PIN, n64Interrupt, FALLING);
+
+  // Let the N64 line interrupt anything else
+  NVIC_SET_PRIORITY(IRQ_PORTD, 1);
+
+  // Initialize SD card
+  if (!sd.begin()) {
+    Serial.println(F("E:SD initialization failed!"));
+    return;
+  }
+  Serial.println(F("L:SD initialization done."));
+
+  // Setup buffer
+  initBuffers();
+
+  Serial.println(F("L:Initialization done."));
+}
+
+


#define INPUT_BUFFER_UPDATE_TIMEOUT 10 // 10 ms

template<class T> void lockedPrintln(const T& input)
{
  noInterrupts();
  Serial.println(input);
  interrupts();
}

template<class A, class B> void lockedPrintln(const A& a, const B& b)
{
  noInterrupts();
  Serial.print(a);
  Serial.println(b);
  interrupts();
}

static char n64_raw_dump[36]; // maximum recv is 1+2+32 bytes + 1 bit
// n64_raw_dump does not include the command byte. That gets pushed into

// bytes to send to the 64
// maximum we'll need to send is 33, 32 for a read request and 1 CRC byte
static unsigned char output_buffer[33];

// Simple switch buffer. (If buffer A fails to load while buffer B is in use,
// we still okay, and will try again next loop)
