TeensyDebug/gdbstub.cpp

/**
 * @file gdbstubs.cpp
 * @author Fernando Trias
 * @brief Implement GDB stub using TeensyDebug interface
 * @version 0.1
 * @date 2020-06-09
 * 
 * @copyright Copyright (c) 2020 Fernando Trias
 * 
 */

/*
 * Notes on 'p':
 * 
 * Calling 'p' with a function as in `p func(1,2,3)` doesn't work without
 * this hack. The reason it fails is that GDB 7 performs the
 * call by:
 * 
 *   1. Setting a breakpoint at 0x60001000 (the ResetHandler)
 *   2. Setting LR to 0x60001000
 *   3. Setting PC to the funtion we want to call
 *   4. Continuing, which causes function to execute
 *   5. Upon finishing, the function returns to LR
 *   6. The breakpoint is hit and GDB takes over
 *   7. GDB unrolls the stack, etc. and gets result
 * 
 * This doesn't work because 0x60001000 is in Flash. I first tried to do a soft 
 * remap of 0x60001000 to a point in RAM, but that doesn't work. It fails 
 * with a "Assertion `get_frame_type (frame) == DUMMY_FRAME' failed."
 * I think the reason is because GDB changes SP by subtracting 4 or 8 before
 * calling the function. Unfortunately, this library doesn't support 
 * GDB changing SP. So I keep track of a "fakesp" whenever SP is changed
 * by GDB and just return that number instead of the real SP. See 
 * process_G and process_P.
 * 
 * Newer versions of GDB choose a different breakpoint location. Instead
 * of setting the breakpoint on the ResetHandler, they will set it on
 * the stack. If that's the case, then this hack will stop working
 * sinces it relies on the address of 0x60001000.
 *  
 */

#include <Arduino.h>

#define CPU_RESTART_ADDR (uint32_t *)0xE000ED0C
#define CPU_RESTART_VAL 0x5FA0004
#define CPU_RESTART (*CPU_RESTART_ADDR = CPU_RESTART_VAL);

#define GDB_DEBUG_INTERNAL
#include "TeensyDebug.h"

// #define GDB_DEBUG_COMMANDS

#define GDB_POLL_INTERVAL_MICROSEC 500

#define MAP_DUMMY_BREAKPOINT 0x60001000

/**
 * Code to communicate with GDB. Use standard nomenclature.
 * devInit() is not standard. It must be called at initialization.
 * 
 */

Stream *dev = NULL;

/**
 * @brief Get the next character from the serial
 * 
 * @return int Character or -1 if error
 */
int getDebugChar() {
  unsigned int timeout = millis() + 1000;
  while(dev->available() <= 0) {
    delay(1);
    if (millis() > timeout) {
      // Serial.println("{timeout}");
      return -1;
    }
  }
  char c = dev->read();
  // Serial.print("{");Serial.print(c);Serial.print("}");
  return c;
}

/**
 * @brief Send a character to the serial
 * 
 * @param c Character to send (one 8-bit byte)
 */
void putDebugChar(int c) {
  // Serial.print("[");Serial.print((char)c);Serial.print("]");
  dev->write(c);
}


/**
 * @brief Return 1 or more if there is data to read
 * 
 * @return int Number of characters to read
 */
int hasDebugChar() {
  // Serial.println("has?");
  return dev->available();
}

/**
 * @brief Initialize serial
 * 
 */
void devInit(Stream *device = NULL) {
  if (device) {
    dev = device;
  }
  else if (dev == NULL) {
    dev = &Serial;
    Serial.begin(9600);
  }
}

// Signal codes for ARM faults; corresponds to debug_id
const char *signal_text[] = {
  "S05", "S02", "S06", "S0B", "S07", "S07", "S04"
};

/**
 * @brief Calculate checksum for message
 * 
 * @param c Packet
 * @return int Checksum
 */
int calcChecksum(const char *c) {
  uint8_t sum = 0;
  while(*c) {
    sum += *c++;
  }
  return sum;
}

// constants for hex conversions
char int2hex[] = { '0','1','2','3','4','5','6','7','8','9','a','b','c','d','e','f' };

/**
 * @brief Convert a hex char to a number
 * 
 * @param ch Hex char '0', '1', etc.
 * @return int Number 0-15
 */
int hex(unsigned char ch) {
  if (ch >= 'a' && ch <= 'f')
    return ch - 'a' + 10;
  if (ch >= '0' && ch <= '9')
    return ch - '0';
  if (ch >= 'A' && ch <= 'F')
    return ch - 'A' + 10;
  return -1;
}

/**
 * @brief Take a memory span and format it in hexadecimal
 * 
 * @param buff Output in ascii hexadecimal
 * @param addr Memory address to convert
 * @param sz Number of bytes to convert
 * @return char* The address of the last character (a \0) in buff
 */
char *mem2hex(char *buff, const void *addr, int sz = -1) {
  if (sz < 0) sz = strlen((char*)addr);
  for (int i = 0; i < sz; i++) {
    uint8_t b = ((uint8_t*)addr)[i];
    *buff++ = int2hex[b >> 4];
    *buff++ = int2hex[b & 0x0F];
  }
  *buff = 0;
  return buff;
}

/**
 * @brief Convert hex string to actual string
 * 
 * @param buff Store string here
 * @param hexstr Hexadeciamal representation
 * @return char* Pointer to \0 at end of buff
 */
char *hex2str(char *buff, const char *hexstr) {
  while (*hexstr) {
    int c_high = hex(*hexstr++);
    int c_low = hex(*hexstr++);
    *buff++ = (c_high << 4) + c_low;
  }
  *buff = 0;
  return buff;
}

/**
 * While we find nice hex chars, build an int.
 * Return number of chars processed.
 */

/**
 * @brief Convert ascii hex into an integer. Used when parsing commands.
 * 
 * @param ptr Pointer to (char*) with text. Updated as text is parsed.
 * @param intValue Pointer to (int) that holds value parsed
 * @return int Number of characters parsed
 */
static int hexToInt(const char **ptr, int *intValue)
{
  int numChars = 0;
  int hexValue;

  *intValue = 0;

  while (**ptr) {
    hexValue = hex(**ptr);
    if (hexValue < 0)
      break;

    *intValue = (*intValue << 4) | hexValue;
    numChars++;

    (*ptr)++;
  }

  return (numChars);
}

static int hex32ToInt(const char **ptr)
{
  const char *p = *ptr;
  uint32_t intValue;
  uint8_t *i = (uint8_t*)&intValue;

  i[0] = (hex(p[0]) << 4) + hex(p[1]);
  i[1] = (hex(p[2]) << 4) + hex(p[3]);
  i[2] = (hex(p[4]) << 4) + hex(p[5]);
  i[3] = (hex(p[6]) << 4) + hex(p[7]);

  // Serial.print("parse ");Serial.println(intValue);
  *ptr += 8;
  return intValue;
}

static int strToInt(const char *str) {
  if (str[0] == '0' && str[1] == 'x') {
    int ret;
    str += 2;
    hexToInt(&str, &ret);
    return ret;
  }
  else {
    return atoi(str);
  }
}

/**
 * @brief Send result text to GDB (formatting and calculating checksum)
 * 
 * @param result String of message to send
 */
void sendResult(const char *result) {
#ifdef GDB_DEBUG_COMMANDS
  Serial.print("target reply:");Serial.println(result);
#endif
  int checksum = calcChecksum(result);
  const char *presult = result;
  putDebugChar('$');
  while (*presult) {
    putDebugChar(*presult++);
  }
  putDebugChar('#');
  putDebugChar(int2hex[checksum >> 4]);
  putDebugChar(int2hex[checksum & 0x0F]);
  // Serial.println(result);
}

// global flag to enabling or suspending debug system
volatile int debug_active = 1;

// global flag indicating debug should is simulating a "halt"; set to 0 to continue
volatile int halt_state = 0;

// global flag indicating Ctrl-C, causing system to "break" as soon as possible
volatile int cause_break = 0;

// main routine for processing GDB commands and states
void processGDB();

// from debug class indicating a fault
extern int debug_id;

// from debug indicting we are "stepping" instructions
extern int debugstep;


extern int debugenabled;

// for messages that are sent seperately (like 'O', print)
char send_message[256];

/**
 * @brief Output to the GDB console using 'O' command
 * 
 * @param msg Message to display
 * @param len Number of characters
 * @return size_t Number of characters sent
 */
size_t gdb_out_write(const uint8_t *msg, size_t len) {
  if (send_message[0]) {
    int lx = strlen(send_message);
    char *p = send_message + lx;
    mem2hex(p, (const char *)msg, len);
  }
  else {
    send_message[0] = 'O';
    mem2hex(send_message+1, (const char *)msg, len);
  }
  return len;
}

/**
 * @brief Output string to GDB console using 'O'
 * 
 * @param msg String to print
 * @return size_t Number of characters sent
 */
size_t gdb_out_print(const char *msg) {
  return gdb_out_write((const uint8_t *)msg, strlen(msg));
}

int gdb_wait_for_flag(volatile int *flag, int timeout) {
  unsigned int endtime = millis()+1000;
  while(*flag) {
    if (timeout && millis() > endtime) {
      return -1;
    }
    delay(1);
    yield();
  }
  return 0;
}

int file_io_result;
int file_io_errno;
volatile int file_io_pending = 0;

int gdb_file_io(const char *cmd) {
  // Serial.println(cmd);
  file_io_pending = 1;
  sendResult(cmd);
  gdb_wait_for_flag(&file_io_pending, 1000);
  // Serial.println(file_io_result);
  return file_io_result;
}

/**
 * @brief Routing for processing breakpoints
 * 
 */
#pragma GCC push_options
#pragma GCC optimize ("O0")
void process_onbreak() {
  // send the signal
  halt_state = 1;
  sendResult(signal_text[debug_id]);
  // go into halt state and stay until flag is cleared
  gdb_wait_for_flag(&halt_state, 0);
  debug_id = 0;
}

uint32_t fakesp;

__attribute__((noinline, naked))
void fake_breakpoint() {
  // asm volatile("ldr r0, =fakesp");
  // asm volatile("str sp, [r0]");
  asm volatile("svc 0x10");
}
#pragma GCC pop_options

/**
 * @brief Add 32-bit ascii hexadecimal to string buffer
 * 
 * @param p Buffer to hold ascii hex
 * @param n Number to encode
 * @return char* Pointer last item (a \0) so you can continue appending
 */
char *append32(char *p, uint32_t n) {
  uint8_t *x = (uint8_t *) &n;
  for(int i=0; i<4; i++) {
    int c = x[i];
    *p++ = int2hex[c >> 4];
    *p++ = int2hex[c & 0x0F];
  }
  return p;
}

// extern void print_registers();

/**
 * @brief Process 'g' command to return registers
 * 
 * @param cmd Original command
 * @param result String with encoded text to return
 * @return int 0 = success
 */
int process_g(const char *cmd, char *result) {
  // print_registers();

  // See Notes above for explanation
  uint32_t pc = debug.getRegister("pc");
  uint32_t sp = debug.getRegister("sp");
  if ((pc|1) == (uint32_t)&fake_breakpoint) {
    pc = MAP_DUMMY_BREAKPOINT;
    // Serial.print("sp fake:");Serial.println(fakesp, HEX);
    // Serial.print("sp:");Serial.println(sp, HEX);
    // delay(1000);
    sp = fakesp;
    // Serial.print("Fake breakpoint sp=");Serial.println(sp, HEX);
  }

  result = append32(result, debug.getRegister("r0"));
  result = append32(result, debug.getRegister("r1"));
  result = append32(result, debug.getRegister("r2"));
  result = append32(result, debug.getRegister("r3"));
  result = append32(result, debug.getRegister("r4"));
  result = append32(result, debug.getRegister("r5"));
  result = append32(result, debug.getRegister("r6"));
  result = append32(result, debug.getRegister("r7"));
  result = append32(result, debug.getRegister("r8"));
  result = append32(result, debug.getRegister("r9"));
  result = append32(result, debug.getRegister("r10"));
  result = append32(result, debug.getRegister("r11"));
  result = append32(result, debug.getRegister("r12"));
  result = append32(result, sp);
  result = append32(result, debug.getRegister("lr"));
  result = append32(result, pc);
  result = append32(result, debug.getRegister("cpsr"));
  *result = 0;
  return 0;
}

/**
 * @brief Process 'G' to write registers. Not supported.
 * 
 * @param cmd 
 * @param result 
 * @return int 
 */
int process_G(const char *cmd, char *result) {
  // Not fully supported; enable when restore*() works
  // strcpy(result, "E01");
  // return 0;
  cmd++;
  debug.setRegister("r0", hex32ToInt(&cmd));
  debug.setRegister("r1", hex32ToInt(&cmd));
  debug.setRegister("r2", hex32ToInt(&cmd));
  debug.setRegister("r3", hex32ToInt(&cmd));
  debug.setRegister("r4", hex32ToInt(&cmd));
  debug.setRegister("r5", hex32ToInt(&cmd));
  debug.setRegister("r6", hex32ToInt(&cmd));
  debug.setRegister("r7", hex32ToInt(&cmd));
  debug.setRegister("r8", hex32ToInt(&cmd));
  debug.setRegister("r9", hex32ToInt(&cmd));
  debug.setRegister("r10", hex32ToInt(&cmd));
  debug.setRegister("r11", hex32ToInt(&cmd));
  debug.setRegister("r12", hex32ToInt(&cmd));
  debug.setRegister("sp", hex32ToInt(&cmd));
  debug.setRegister("lr", hex32ToInt(&cmd));
  debug.setRegister("pc", hex32ToInt(&cmd));
  debug.setRegister("cspr", hex32ToInt(&cmd));
  strcpy(result, "OK");
  return 0;
}

int process_P(const char *cmd, char *result) {
  // Not fully supported; enable when restore*() works
  // strcpy(result, "E01");
  // return 0;
  cmd++;
  int reg = hex(*cmd++);
  cmd++; // skip '='
  uint32_t val = hex32ToInt(&cmd);
  // Serial.print("Reg ");Serial.print(reg);Serial.print("=");Serial.println(val, HEX);
  switch(reg) {
    case 0: debug.setRegister("r0", val); break;
    case 1: debug.setRegister("r1", val); break;
    case 2: debug.setRegister("r2", val); break;
    case 3: debug.setRegister("r3", val); break;
    case 4: debug.setRegister("r4", val); break;
    case 5: debug.setRegister("r5", val); break;
    case 6: debug.setRegister("r6", val); break;
    case 7: debug.setRegister("r7", val); break;
    case 8: debug.setRegister("r8", val); break;
    case 9: debug.setRegister("r9", val); break;
    case 10: debug.setRegister("r10", val); break;
    case 11: debug.setRegister("r11", val); break;
    case 12: debug.setRegister("r12", val); break;
    case 13:
      if (debug.getRegister("lr") == (uint32_t)&fake_breakpoint) {
        fakesp = val; // gdb changes sp, and we need to return it later
      }
      debug.setRegister("sp", val); break;
    case 14:
      if (val == (MAP_DUMMY_BREAKPOINT|1)) { // special breakpoint
        debug.setRegister("lr", (uint32_t)&fake_breakpoint);
        fakesp = debug.getRegister("sp"); // just in case not set later
      }
      else {
        debug.setRegister("lr", val);
      }
      break;
    case 15: debug.setRegister("pc", val); break;
    case 16: debug.setRegister("cspr", val); break;
    // case 13: strcpy(result, "E01"); return 0;
    // case 14: strcpy(result, "E01"); return 0;
    // case 15: strcpy(result, "E01"); return 0;
    // case 16: strcpy(result, "E01"); return 0;
  }
  strcpy(result, "OK");
  return 0;
}

/**
 * @brief Test is requested address is valid. GDB sometimes sends 
 * invalid memory requests that will cause a fault. Should test more
 * accurately, but this takes care of most problems.
 * 
 * @param addr Address to check
 * @return int 1 = valid; 0 = invalid
 */
int isValidAddress(uint32_t addr) {
  if (addr <= 0x20) {
    return 0;
  }
  else if (addr >= 0xF0000000) {
    return 0;
  }
  return 1;
}

/**
 * @brief Process 'm' to read memory
 * 
 * @param cmd Original command
 * @param result String to return
 * @return int 0 for success
 */
int process_m(const char *cmd, char *result) {
  int addr, sz=4;

  cmd++; // skip cmd
  hexToInt(&cmd, &addr);
  if (*cmd == ',') {
    cmd++; // skip comma
    hexToInt(&cmd, &sz);
  }

  // Serial.print("read at ");Serial.println(addr, HEX);

  if (isValidAddress(addr+sz-1) == 0) {
    strcpy(result, "E01");
    return 0;
  }

  // if (addr == MAP_DUMMY_BREAKPOINT) {
  //   if (sz == 2) {
  //     strcpy(result, "fbbe");
  //     return 0;
  //   }
  //   else if (sz == 4) {
  //     strcpy(result, "fbbe0000");
  //     return 0;
  //   }
  // }

  uint8_t *m = (uint8_t *)addr;
  for (int i=0; i<sz; i++) {
    uint8_t d = m[i];
    *(result++) = int2hex[d >> 4];
    *(result++) = int2hex[d & 0x0F];
  }
  *result = 0;
  return 0;
}

/**
 * @brief Process 'M' to write memory
 * 
 * @param cmd Original command
 * @param result Results 'OK'
 * @return int 
 */
int process_M(const char *cmd, char *result) {
  int addr, sz;
  cmd++; // skip command
  hexToInt(&cmd, &addr);
  cmd++; // skip comma
  hexToInt(&cmd, &sz);
  cmd++;

  uint8_t *memory = (uint8_t*)addr;
  for(int i=0; i<sz; i++) {
    int c_high = hex(*cmd++);
    int c_low = hex(*cmd++);
    memory[i] = (c_high << 4) + c_low;
  }
  strcpy(result, "OK");
  return 0;
}

/**
 * @brief Process 'c' continue
 * 
 * @param cmd Original command
 * @param result String result
 * @return int 1 to signal caller
 */
int process_c(const char *cmd, char *result) {
  halt_state = 0; // not halted
  debugstep = 0;  // not stepping
  strcpy(result, "");
  return 1;
}

/**
 * @brief Process 's' step command to step a single instruction. 
 * Arguments to start in a different address are not supported.
 * 
 * @param cmd Original command
 * @param result String with ENN or blank
 * @return int 1 to continue; 0 if errors
 */
int process_s(const char *cmd, char *result) {
  cmd++;
  if (*cmd) {
    // int addr;
    // hexToInt(&cmd, &addr);
    // we don't support starting in a different address
    strcpy(result, "E01"); // SNN
    return 0;
  }
  debugstep = 1;   // just step
  halt_state = 0;
  strcpy(result, ""); // return comes from the actual break
  return 1;
}

/**
 * @brief Process '?' query for last stop reason. TODO.
 * 
 * @param cmd 
 * @param result 
 * @return int 
 */
int process_question(const char *cmd, char *result) {
  // sprintf(result, "S0%d", debug_id);
  strcpy(result, signal_text[debug_id]);
  return 0;
}

/**
 * @brief 'B' deprecated and not supported
 * 
 * @param cmd 
 * @param result 
 * @return int 
 */
int process_B(const char *cmd, char *result) {
  strcpy(result, "E10");
  return 0;
}

/**
 * @brief Process 'z' clear breakpoint at address
 * 
 * @param cmd Original command
 * @param result Result is ENN or OK
 * @return int 0 if success
 */
int process_z(const char *cmd, char *result) {
  int btype, addr;
  cmd++;
  hexToInt(&cmd, &btype);
  cmd++;
  hexToInt(&cmd, &addr);
//  cmd++;
//  hexToInt(&cmd, &sz);
  if (addr == 0) {
    strcpy(result, "E01");
  }
  else if (addr == MAP_DUMMY_BREAKPOINT) { // hard-coded breakpoint
    strcpy(result, "OK");
  }
  else if (debug.clearBreakpoint((void*)addr)) {
    strcpy(result, "E01");
    strcpy(result, "OK");
  }
  else {
    strcpy(result, "OK");
  }
  return 0;
}

/**
 * @brief Process 'Z' set breakpoint at address
 * 
 * @param cmd Original command
 * @param result Result is ENN or OK
 * @return int 
 */
int process_Z(const char *cmd, char *result) {
  int btype, addr;
  cmd++;
  hexToInt(&cmd, &btype); // don't care. We figure out what's best.
  cmd++;
  hexToInt(&cmd, &addr);
  // optional size not used because we only support Thumb
//  cmd++;
//  hexToInt(&cmd, &sz);
  if (addr == 0) {
    strcpy(result, "E01");
  }
  else if (addr == MAP_DUMMY_BREAKPOINT) { // hard-coded breakpoint
    strcpy(result, "OK");
  }
  else if (debug.setBreakpoint((void*)addr)) {
#ifdef GDB_DEBUG_COMMANDS
  Serial.print("Breakpoint failed on ");Serial.println(addr);
#endif
    strcpy(result, "E01");
  }
  else {
    strcpy(result, "OK");
  }
  return 0;
}

char *getNextWord(char **text) {
  char *orig = *text;
  char *p = orig;
  while(*p) {
    if (*p == ' ' || *p == '(' || *p == ')' || *p == ',') {
      *p++ = 0;
      while(*p == ' ') p++;
      *text = p;
      return orig;
    }
    p++;
  }
  *text = 0;
  return orig;
}

char *getNextToken(char **text, char token) {
  char *orig = *text;
  char *p = orig;
  while(*p) {
    if (*p == token) {
      *p++ = 0;
      *text = 0;
      return orig;
    }
    p++;
  }
  *text = 0;
  return orig;
}

int (*call0)();
int (*call1)(int p1);
int (*call2)(int p1, int p2);
int (*call3)(int p1, int p2, int p3);

int process_monitor(char *cmd, char *result) {
  char *place = cmd;
  char *word;
  word = getNextWord(&place);
  // Serial.print("command :");Serial.print(word);Serial.println(":");
  if (stricmp(word, "digitalWrite") == 0) {
    char *pin = getNextWord(&place);
    char *state = getNextWord(&place);
    int ipin = strToInt(pin);
    int istate = strToInt(state);
    if (stricmp(state, "high")==0) istate = 1;
    pinMode(ipin, OUTPUT);
    digitalWrite(ipin, istate);
    strcpy(result, "OK"); 
    return 0;   
  }
  else if (stricmp(word, "digitalRead") == 0) {
    char *pin = getNextWord(&place);
    int ipin = strToInt(pin);
    pinMode(ipin, INPUT);
    int v = digitalRead(ipin);
    char x[6];
    sprintf(x, "%d\n", v);
    mem2hex(result, (const char *)x, strlen(x));
    return 0;   
  }
  else if (stricmp(word, "analogWrite") == 0) {
    char *pin = getNextWord(&place);
    char *state = getNextWord(&place);
    int ipin = strToInt(pin);
    int istate = strToInt(state);
    pinMode(ipin, OUTPUT);
    analogWrite(ipin, istate);
    strcpy(result, "OK"); 
    return 0;   
  }
  else if (stricmp(word, "analogRead") == 0) {
    char *pin = getNextWord(&place);
    int ipin = strToInt(pin);
    pinMode(ipin, INPUT);
    int v = analogRead(ipin);
    char x[6];
    sprintf(x, "%d\n", v);
    mem2hex(result, (const char *)x, strlen(x));
    return 0;   
  }
  else if (stricmp(word, "call") == 0) {
    int args = 0, p[4], ret;
    char *arg = getNextWord(&place);
    uint32_t addr = strToInt(arg);
    // Serial.print("addr ");Serial.println(addr);
    if (addr == 0) {
      mem2hex(result, "E Invalid address\n");
      return 0;
    }
    addr |= 1; // set the exchange bit
    for(int i=0; i<4; i++) {
      if (*place == 0) break; 
      arg = getNextWord(&place);
      p[args] = strToInt(arg);
      // Serial.print("parameter ");Serial.println(p[args]);
      args++;
    }
    // Serial.print("arguments ");Serial.println(args);
    switch(args) {
      case 0:
        call0 = (int (*)())addr;
        ret = call0();
        break;
      case 1:
        call1 = (int (*)(int))addr;
        ret = call1(p[0]);
        break;
      case 2:
        call2 = (int (*)(int,int))addr;
        ret = call2(p[0], p[1]);
        break;
      case 3:
        call3 = (int (*)(int,int,int))addr;
        ret = call3(p[0], p[1], p[2]);
        break;
      default:
        mem2hex(result, "E Too many parameters (max=3)\n");
        return 0;
    }
    char x[40];
    sprintf(x, "%d\n", ret);
    mem2hex(result, (const char *)x, strlen(x));
    return 0; 
  }
  else if (stricmp(word, "restart") == 0) {
    CPU_RESTART;
    strcpy(result, "");    
    return 0;   
  }
  strcpy(result, "");    
  return 0;
}

/**
 * @brief Process 'q' query command. For now report back PacketSize.
 * Handle 'monitor' commands.
 * 
 * @param cmd Original command
 * @param result Results or ""
 * @return int 0
 */
int process_q(const char *cmd, char *result) {
  if (strncmp(cmd, "qSupported", 10) == 0) {
    strcpy(result, "PacketSize=1024");
    return 0;
  }
  else if (strncmp(cmd, "qRcmd", 5) == 0) {
    char x[256];
    hex2str(x, cmd+6);
    return process_monitor(x, result);
  }
  strcpy(result, "");    
  return 0;
}

/**
 * @brief Functions are not supported, but would be very useful in the future
 * 
 * @param cmd Function specification
 * @param result 
 * @return int 
 */
int process_F(const char *cmd, char *result) {
  // Serial.println(cmd);
  cmd++;
  // error
  if (*cmd == '-') { 
    cmd++;
    hexToInt(&cmd, &file_io_result); // don't care. We figure out what's best.
    file_io_result = -file_io_result;
  }
  else {
    hexToInt(&cmd, &file_io_result); // don't care. We figure out what's best.
  }
  if (*cmd == ',') {
    cmd++;
    hexToInt(&cmd, &file_io_errno);
  }
  else {
    file_io_errno = 0;
  }
  file_io_pending = 0;
  strcpy(result, "OK"); 
  return 0;
}

/**
 * @brief Process 'R' restart
 * 
 * @param cmd 
 * @param result 
 * @return int 
 */
int process_R(const char *cmd, char *result) {
  // _reboot_Teensyduino_();
  // _restart_Teensyduino_();
  CPU_RESTART;
  strcpy(result, "OK");    
  return 0;
}

int process_k(const char *cmd, char *result) {
  // _reboot_Teensyduino_();
  // _restart_Teensyduino_();
  CPU_RESTART;
  strcpy(result, "OK");    
  return 0;
}

int process_v(char *cmd, char *result) {
  char *work = getNextToken(&cmd, ';');
  // Serial.print("v:");Serial.println(work);
  if (strcmp(work, "vKill") == 0) {
    strcpy(result, "OK");    
  }
  else {
    result[0] = 0;
  }
  return 0;
}

int gdb_active_flag = 0;

/**
 * @brief Process a command by calling appropriate delegation function
 * 
 * @param cmd Command and parameters
 * @param result Result to send back
 * @return int 0
 */
int processCommand(char *cmd, char *result) {
  gdb_active_flag = 1;
  switch(cmd[0]) {
    case 'g': return process_g(cmd, result);
    case 'G': return process_G(cmd, result);
    case 'P': return process_P(cmd, result);
    case 'm': return process_m(cmd, result);
    case 'M': return process_M(cmd, result);
    case 'c': return process_c(cmd, result);
    case 's': return process_s(cmd, result);
    case 'F': return process_F(cmd, result);
    case 'R': return process_R(cmd, result);
    case 'r': return process_R(cmd, result);
    case 'k': return process_k(cmd, result);
    case 'v': return process_v(cmd, result);    
    case '?': return process_question(cmd, result);
//    case 'B': return process_B(cmd, result);
    case 'z': return process_z(cmd, result);
    case 'Z': return process_Z(cmd, result);
    case 'q': return process_q(cmd, result);
  }
  // if it's not listed above, it's not supported
  result[0] = 0;
  return 0;
}

/**
 * @brief Read input, if available and process any commands
 * 
 */
void processGDBinput() {
  char result[1024];

  // no data? do nothing
  if (! hasDebugChar()) return;

  int c = getDebugChar();

  if (c < 0) {
    // Serial.println("Error reading");
    return;
  }

  // GDB ack'd our last command; don't do anything yet with this
  if (c == '+') {
    // Serial.println("ACK");
    return;
  }

  // GDB had a problem with last command; should resend. TODO
  if (c == '-') {
    // Serial.println("NAK");
    if (file_io_pending) {
      file_io_result = -1;
      file_io_pending = 0;
    }
    return;
  }

  // User hit Ctrl-C or other break
  if (c == 0x03) {
    // Serial.println("Ctrl-C");
    cause_break = 1; // cause break later so we don't break internals
    return;
  }

  // If we don't have a valid start command, then something went wrong so
  // we just ignore it.
  if (c != '$') {
    // Serial.print("Bad char: ");
    // Serial.println((char)c, HEX);
    return; // wait for start char
  }

  // buffer to read command; matches our PacketSize
  const int cmd_max = 1024;
  char cmd[cmd_max];      // buffer
  char *pcmd = cmd;       // pointer to last char
  // int sum;               
  int checksum = 0;   // to store checksum

  // 2-second timeout
  while(1) {
    // read next char or timeout
    c = getDebugChar();

    if (c == -1) {
      // Serial.println("read error");
      putDebugChar('-');
      return;
    }

    if (c == '#') break; // checksum follows
    *pcmd++ = c;
    if (pcmd >= cmd+cmd_max) { // overrun
      pcmd = cmd;
    }
  }
  *pcmd = 0;
  
#ifdef GDB_DEBUG_COMMANDS
  Serial.print("gdb command:");Serial.println(cmd);
#endif

  c = getDebugChar();
  checksum = hex(c) << 4;
  c = getDebugChar();
  checksum += hex(c);
  if (checksum != calcChecksum(cmd)) {
    // Serial.println("bad checksum");
    // Serial.println(calcChecksum(cmd), HEX);
    putDebugChar('-');
    return;
  }

  // all good, so ACK
  putDebugChar('+');
  
  int r = processCommand(cmd, result);
  // r == 1 means there are no results for now. A step or continue
  // don't return immediate results. Results are returned upon
  // hitting the break or successful step
  if (r==1) return;

  // toss results back to GDB
  sendResult(result);
}

/**
 * @brief Process GDB messages, including Break
 * 
 */
void processGDB() {
#if 0
  static unsigned int nexttick = millis() + 1000;
  if (millis() > nexttick) {
    // Serial.println("tick");
    gdb_out_print("ping");
    nexttick += 1000;
  }
#endif
  if (! debug_active) return;
  while (hasDebugChar()) {
    processGDBinput();
  }
  if (send_message[0]) {
    // Serial.print("send ");Serial.println(send_message);
    sendResult(send_message);
    send_message[0] = 0;
  }
  if (cause_break) {
    // Serial.println("BREAK!!");
    cause_break = 0;
    debug_id = 1;
    // NVIC_SET_PENDING(IRQ_SOFTWARE); 
    asm volatile("svc 0x12");
  }
}

// void setup_main();

// Check for GDB commands periodically
IntervalTimer gdb_timer;

/**
 * @brief Initialize debug system
 * 
 * @param device Optional device that inherits from Stream; default is Serial
 */
void gdb_init(Stream *device) {
  send_message[0] = 0;
  devInit(device);
  gdb_timer.begin(processGDB, GDB_POLL_INTERVAL_MICROSEC);
  debug.setCallback(process_onbreak);
  debug_active = 1;

#ifdef GDB_HALT_ON_STARTUP
  #ifdef REMAP_SETUP
    debug.setBreakpoint(setup_main, 1);
  #else
    debug.setBreakpoint(setup, 1);
  #endif
#endif
}