From 2ac3f354a3b20100677b201c85409d30ca1c7816 Mon Sep 17 00:00:00 2001
From: Kyle Fleming <kyle@kylefleming.net>
Date: Mon, 12 Feb 2018 19:01:58 -0800
Subject: [PATCH 1/3] Consolidate .iram.text matcher in linker

Use wildcard matcher for .iram.text symbols.
---
 tools/sdk/ld/eagle.app.v6.common.ld | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/tools/sdk/ld/eagle.app.v6.common.ld b/tools/sdk/ld/eagle.app.v6.common.ld
index cbc1ebf871..38edb020e8 100644
--- a/tools/sdk/ld/eagle.app.v6.common.ld
+++ b/tools/sdk/ld/eagle.app.v6.common.ld
@@ -151,9 +151,7 @@ SECTIONS
     *(.entry.text)
     *(.init.literal)
     *(.init)
-    *(.literal .text .literal.* .text.* .stub .gnu.warning .gnu.linkonce.literal.* .gnu.linkonce.t.*.literal .gnu.linkonce.t.*)
-    *.cpp.o(.iram.text)
-    *.c.o(.iram.text)
+    *(.literal .text .iram.text .literal.* .text.* .stub .gnu.warning .gnu.linkonce.literal.* .gnu.linkonce.t.*.literal .gnu.linkonce.t.*)
     *(.rodata._ZTV*) /* C++ vtables */
     *(.fini.literal)
     *(.fini)

From c25d97ed752cec45d85aab1dbe901f36d0e42109 Mon Sep 17 00:00:00 2001
From: Kyle Fleming <kyle@kylefleming.net>
Date: Thu, 22 Feb 2018 15:29:58 -0800
Subject: [PATCH 2/3] Move C++ vtables too flash

---
 tools/sdk/ld/eagle.app.v6.common.ld | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tools/sdk/ld/eagle.app.v6.common.ld b/tools/sdk/ld/eagle.app.v6.common.ld
index 38edb020e8..080e5e6e3f 100644
--- a/tools/sdk/ld/eagle.app.v6.common.ld
+++ b/tools/sdk/ld/eagle.app.v6.common.ld
@@ -91,6 +91,7 @@ SECTIONS
     *.c.o( EXCLUDE_FILE (umm_malloc.c.o) .literal*, \
            EXCLUDE_FILE (umm_malloc.c.o) .text*)
     *.cpp.o(.literal*, .text*)
+    *(.rodata._ZTV*) /* C++ vtables */
     *libc.a:(.literal .text .literal.* .text.*)
     *libm.a:(.literal .text .literal.* .text.*)
     *libgcc.a:_umoddi3.o(.literal .text)
@@ -152,7 +153,6 @@ SECTIONS
     *(.init.literal)
     *(.init)
     *(.literal .text .iram.text .literal.* .text.* .stub .gnu.warning .gnu.linkonce.literal.* .gnu.linkonce.t.*.literal .gnu.linkonce.t.*)
-    *(.rodata._ZTV*) /* C++ vtables */
     *(.fini.literal)
     *(.fini)
     *(.gnu.version)

From 4877a316f23bc64452f11a155d4ae24d08921b04 Mon Sep 17 00:00:00 2001
From: Kyle Fleming <kyle@kylefleming.net>
Date: Mon, 12 Mar 2018 22:20:34 -0700
Subject: [PATCH 3/3] Add full gdb support with uart/Serial integration

---
 cores/esp8266/gdb_hooks.c                     |  14 +-
 cores/esp8266/gdb_hooks.h                     |  65 ++
 cores/esp8266/uart.c                          | 211 +++--
 libraries/GDBStub/src/internal/gdbstub-cfg.h  |  24 +-
 .../GDBStub/src/internal/gdbstub-entry.S      | 154 ++--
 libraries/GDBStub/src/internal/gdbstub.c      | 835 +++++++++++-------
 libraries/GDBStub/src/internal/gdbstub.h      |  28 +
 7 files changed, 858 insertions(+), 473 deletions(-)

diff --git a/cores/esp8266/gdb_hooks.c b/cores/esp8266/gdb_hooks.c
index 9d5eb3c3ab..26aa6b0dfb 100644
--- a/cores/esp8266/gdb_hooks.c
+++ b/cores/esp8266/gdb_hooks.c
@@ -25,12 +25,18 @@
    value is in register, it doesn't hurt to return a bool, so that the
    same stub can be used for gdb_present. */
 
-bool ICACHE_RAM_ATTR __gdb_no_op()
+static bool ICACHE_RAM_ATTR __gdb_no_op()
 {
     return false;
 }
 
-extern void gdb_init(void) __attribute__ ((weak, alias("__gdb_no_op")));
-extern void gdb_do_break(void) __attribute__ ((weak, alias("__gdb_no_op")));
-extern bool gdb_present(void) __attribute__ ((weak, alias("__gdb_no_op")));
+void gdb_init(void) __attribute__ ((weak, alias("__gdb_no_op")));
+void gdb_do_break(void) __attribute__ ((weak, alias("__gdb_no_op")));
+bool gdb_present(void) __attribute__ ((weak, alias("__gdb_no_op")));
+bool gdbstub_has_putc1_control(void) __attribute__ ((weak, alias("__gdb_no_op")));
+void gdbstub_set_putc1_callback(void (*func)(char)) __attribute__ ((weak, alias("__gdb_no_op")));
+bool gdbstub_has_uart_isr_control(void) __attribute__ ((weak, alias("__gdb_no_op")));
+void gdbstub_set_uart_isr_callback(void (*func)(void*, uint8_t), void* arg) __attribute__ ((weak, alias("__gdb_no_op")));
+void gdbstub_write_char(char c) __attribute__ ((weak, alias("__gdb_no_op")));
+void gdbstub_write(const char* buf, size_t size) __attribute__ ((weak, alias("__gdb_no_op")));
 
diff --git a/cores/esp8266/gdb_hooks.h b/cores/esp8266/gdb_hooks.h
index 1d7a375d6c..4953216a1a 100644
--- a/cores/esp8266/gdb_hooks.h
+++ b/cores/esp8266/gdb_hooks.h
@@ -52,6 +52,71 @@ void gdb_do_break(void);
  */
 bool gdb_present(void);
 
+// If gdbstub has these set true, then we will disable our own
+// usage of them, but use gdbstub's callbacks for them instead
+/**
+ * @brief Check if GDB is installing a putc1 callback.
+ * 
+ * By default, this function returns false. When GDBStub library is linked,
+ * this function is overriden and returns true.
+ * 
+ * @return true if GDB is installing a putc1 callback
+ */
+bool gdbstub_has_putc1_control(void);
+
+/**
+ * @brief Register a putc1 callback with GDB.
+ * @param func function GDB will proxy putc1 data to
+ * 
+ * By default, this function is a no-op. When GDBStub library is linked,
+ * this function is overriden and sets GDB stub's secondary putc1 callback to
+ * func. When GDB stub is linked, but a GDB session is not current attached,
+ * then GDB stub will pass putc1 chars directly to this function.
+ */
+void gdbstub_set_putc1_callback(void (*func)(char));
+
+/**
+ * @brief Check if GDB is installing a uart0 isr callback.
+ * 
+ * By default, this function returns false. When GDBStub library is linked,
+ * this function is overriden and returns true.
+ * 
+ * @return true if GDB is installing a uart0 isr callback
+ */
+bool gdbstub_has_uart_isr_control(void);
+
+/**
+ * @brief Register a uart0 isr callback with GDB.
+ * @param func function GDB will proxy uart0 isr data to
+ * 
+ * By default, this function is a no-op. When GDBStub library is linked,
+ * this function is overriden and sets GDB stub's secondary uart0 isr callback
+ * to func. When GDB stub is linked, but a GDB session is not current attached,
+ * then GDB stub will pass uart0 isr data back to this function.
+ */
+void gdbstub_set_uart_isr_callback(void (*func)(void*, uint8_t), void* arg);
+
+/**
+ * @brief Write a character for output to a GDB session on uart0.
+ * @param c character to write
+ * 
+ * By default, this function is a no-op. When GDBStub library is linked,
+ * this function is overriden and writes a char to either the GDB session on
+ * uart0 or directly to uart0 if not GDB session is attached.
+ */
+void gdbstub_write_char(char c);
+
+/**
+ * @brief Write a char buffer for output to a GDB session on uart0.
+ * @param buf buffer of data to write
+ * @param size length of buffer
+ * 
+ * By default, this function is a no-op. When GDBStub library is linked,
+ * this function is overriden and writes a buffer to either the GDB session on
+ * uart0 or directly to uart0 if not GDB session is attached.
+ */
+void gdbstub_write(const char* buf, size_t size);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/cores/esp8266/uart.c b/cores/esp8266/uart.c
index 328acca7df..9c995907a8 100644
--- a/cores/esp8266/uart.c
+++ b/cores/esp8266/uart.c
@@ -152,8 +152,20 @@ size_t uart_rx_available(uart_t* uart)
     return uart_rx_buffer_available(uart) + uart_rx_fifo_available(uart);
 }
 
+static void ICACHE_RAM_ATTR uart_isr_handle_data(void* arg, uint8_t data)
+{
+    uart_t* uart = (uart_t*)arg;
+    if(uart == NULL || !uart->rx_enabled) {
+        return;
+    }
+    size_t nextPos = (uart->rx_buffer->wpos + 1) % uart->rx_buffer->size;
+    if(nextPos != uart->rx_buffer->rpos) {
+        uart->rx_buffer->buffer[uart->rx_buffer->wpos] = data;
+        uart->rx_buffer->wpos = nextPos;
+    }
+}
 
-void ICACHE_RAM_ATTR uart_isr(void * arg)
+static void ICACHE_RAM_ATTR uart_isr(void* arg)
 {
     uart_t* uart = (uart_t*)arg;
     if(uart == NULL || !uart->rx_enabled) {
@@ -161,33 +173,41 @@ void ICACHE_RAM_ATTR uart_isr(void * arg)
         ETS_UART_INTR_DISABLE();
         return;
     }
-    if(USIS(uart->uart_nr) & ((1 << UIFF) | (1 << UITO))){
+    if(USIS(uart->uart_nr) & ((1 << UIFF) | (1 << UITO))) {
         uart_rx_copy_fifo_to_buffer(uart);
     }
     USIC(uart->uart_nr) = USIS(uart->uart_nr);
 }
 
-void uart_start_isr(uart_t* uart)
+static void uart_start_isr(uart_t* uart)
 {
     if(uart == NULL || !uart->rx_enabled) {
         return;
     }
+    if(gdbstub_has_uart_isr_control()) {
+        gdbstub_set_uart_isr_callback(uart_isr_handle_data,  (void *)uart);
+        return;
+    }
     // UCFFT value is when the RX fifo full interrupt triggers.  A value of 1
     // triggers the IRS very often.  A value of 127 would not leave much time
     // for ISR to clear fifo before the next byte is dropped.  So pick a value
     // in the middle.
-    USC1(uart->uart_nr) = (100   << UCFFT) | (0x02 << UCTOT) | (1 <<UCTOE );
+    USC1(uart->uart_nr) = (100 << UCFFT) | (0x02 << UCTOT) | (1 <<UCTOE);
     USIC(uart->uart_nr) = 0xffff;
     USIE(uart->uart_nr) = (1 << UIFF) | (1 << UIFR) | (1 << UITO);
     ETS_UART_INTR_ATTACH(uart_isr,  (void *)uart);
     ETS_UART_INTR_ENABLE();
 }
 
-void uart_stop_isr(uart_t* uart)
+static void uart_stop_isr(uart_t* uart)
 {
     if(uart == NULL || !uart->rx_enabled) {
         return;
     }
+    if(gdbstub_has_uart_isr_control()) {
+        gdbstub_set_uart_isr_callback(NULL, NULL);
+        return;
+    }
     ETS_UART_INTR_DISABLE();
     USC1(uart->uart_nr) = 0;
     USIC(uart->uart_nr) = 0xffff;
@@ -195,14 +215,22 @@ void uart_stop_isr(uart_t* uart)
     ETS_UART_INTR_ATTACH(NULL, NULL);
 }
 
+static void uart_do_write_char(int uart_nr, char c)
+{
+    while((USS(uart_nr) >> USTXC) >= 0x7f) ;
+    USF(uart_nr) = c;
+}
 
 void uart_write_char(uart_t* uart, char c)
 {
     if(uart == NULL || !uart->tx_enabled) {
         return;
     }
-    while((USS(uart->uart_nr) >> USTXC) >= 0x7f);
-    USF(uart->uart_nr) = c;
+    if(gdbstub_has_uart_isr_control() && uart->uart_nr == UART0) {
+        gdbstub_write_char(c);
+        return;
+    }
+    uart_do_write_char(uart->uart_nr, c);
 }
 
 void uart_write(uart_t* uart, const char* buf, size_t size)
@@ -210,8 +238,12 @@ void uart_write(uart_t* uart, const char* buf, size_t size)
     if(uart == NULL || !uart->tx_enabled) {
         return;
     }
+    if(gdbstub_has_uart_isr_control() && uart->uart_nr == UART0) {
+        gdbstub_write(buf, size);
+        return;
+    }
     while(size--) {
-        uart_write_char(uart, *buf++);
+        uart_do_write_char(uart->uart_nr, *buf++);
     }
 }
 
@@ -252,8 +284,10 @@ void uart_flush(uart_t* uart)
         tmp |= (1 << UCTXRST);
     }
 
-    USC0(uart->uart_nr) |= (tmp);
-    USC0(uart->uart_nr) &= ~(tmp);
+    if(!gdbstub_has_uart_isr_control() || uart->uart_nr != UART0) {
+        USC0(uart->uart_nr) |= (tmp);
+        USC0(uart->uart_nr) &= ~(tmp);
+    }
 }
 
 void uart_set_baudrate(uart_t* uart, int baud_rate)
@@ -273,6 +307,43 @@ int uart_get_baudrate(uart_t* uart)
     return uart->baud_rate;
 }
 
+static void uart0_enable_tx_pin(uint8_t pin)
+{
+    switch(pin) {
+    case 1:
+        pinMode(pin, SPECIAL);
+        break;
+    case 2:
+    case 15:
+        pinMode(pin, FUNCTION_4);
+        break;
+    }
+}
+
+static void uart0_enable_rx_pin(uint8_t pin)
+{
+    switch(pin) {
+        case 3:
+            pinMode(pin, SPECIAL);
+            break;
+        case 13:
+            pinMode(pin, FUNCTION_4);
+            break;
+    }
+}
+
+static void uart1_enable_tx_pin(uint8_t pin)
+{
+    if(pin == 2) {
+        pinMode(pin, SPECIAL);
+    }
+}
+
+static void uart_disable_pin(uint8_t pin)
+{
+    pinMode(pin, INPUT);
+}
+
 uart_t* uart_init(int uart_nr, int baudrate, int config, int mode, int tx_pin, size_t rx_size)
 {
     uart_t* uart = (uart_t*) malloc(sizeof(uart_t));
@@ -285,7 +356,9 @@ uart_t* uart_init(int uart_nr, int baudrate, int config, int mode, int tx_pin, s
     switch(uart->uart_nr) {
     case UART0:
         ETS_UART_INTR_DISABLE();
-        ETS_UART_INTR_ATTACH(NULL, NULL);
+        if(!gdbstub_has_uart_isr_control()) {
+            ETS_UART_INTR_ATTACH(NULL, NULL);
+        }
         uart->rx_enabled = (mode != UART_TX_ONLY);
         uart->tx_enabled = (mode != UART_RX_ONLY);
         uart->rx_pin = (uart->rx_enabled)?3:255;
@@ -305,16 +378,15 @@ uart_t* uart_init(int uart_nr, int baudrate, int config, int mode, int tx_pin, s
               return NULL;
             }
             uart->rx_buffer = rx_buffer;
-            pinMode(uart->rx_pin, SPECIAL);
+            uart0_enable_rx_pin(uart->rx_pin);
         }
         if(uart->tx_enabled) {
             if (tx_pin == 2) {
                 uart->tx_pin = 2;
-                pinMode(uart->tx_pin, FUNCTION_4);
             } else {
                 uart->tx_pin = 1;
-                pinMode(uart->tx_pin, FUNCTION_0);
             }
+            uart0_enable_tx_pin(uart->tx_pin);
         } else {
             uart->tx_pin = 255;
         }
@@ -327,7 +399,7 @@ uart_t* uart_init(int uart_nr, int baudrate, int config, int mode, int tx_pin, s
         uart->rx_pin = 255;
         uart->tx_pin = (uart->tx_enabled)?2:255;  // GPIO7 as TX not possible! See GPIO pins used by UART
         if(uart->tx_enabled) {
-            pinMode(uart->tx_pin, SPECIAL);
+            uart1_enable_tx_pin(uart->tx_pin);
         }
         break;
     case UART_NO:
@@ -339,12 +411,19 @@ uart_t* uart_init(int uart_nr, int baudrate, int config, int mode, int tx_pin, s
 
     uart_set_baudrate(uart, baudrate);
     USC0(uart->uart_nr) = config;
-    uart_flush(uart);
-    USC1(uart->uart_nr) = 0;
-    USIC(uart->uart_nr) = 0xffff;
-    USIE(uart->uart_nr) = 0;
-    if(uart->uart_nr == UART0 && uart->rx_enabled) {
-        uart_start_isr(uart);
+    if(!gdbstub_has_uart_isr_control() || uart->uart_nr != UART0) {
+        uart_flush(uart);
+        USC1(uart->uart_nr) = 0;
+        USIC(uart->uart_nr) = 0xffff;
+        USIE(uart->uart_nr) = 0;
+    }
+    if(uart->uart_nr == UART0) {
+        if(uart->rx_enabled && !gdbstub_has_uart_isr_control()) {
+            uart_start_isr(uart);
+        }
+        if(gdbstub_has_uart_isr_control()) {
+            ETS_UART_INTR_ENABLE();
+        }
     }
 
     return uart;
@@ -356,31 +435,17 @@ void uart_uninit(uart_t* uart)
         return;
     }
 
-    switch(uart->rx_pin) {
-    case 3:
-        pinMode(3, INPUT);
-        break;
-    case 13:
-        pinMode(13, INPUT);
-        break;
-    }
-
-    switch(uart->tx_pin) {
-    case 1:
-        pinMode(1, INPUT);
-        break;
-    case 2:
-        pinMode(2, INPUT);
-        break;
-    case 15:
-        pinMode(15, INPUT);
-        break;
+    if(uart->tx_enabled && (!gdbstub_has_uart_isr_control() || uart->uart_nr != UART0)) {
+        uart_disable_pin(uart->tx_pin);
     }
 
-    if(uart->rx_enabled){
+    if(uart->rx_enabled) {
         free(uart->rx_buffer->buffer);
         free(uart->rx_buffer);
-        uart_stop_isr(uart);
+        if(!gdbstub_has_uart_isr_control()) {
+            uart_disable_pin(uart->rx_pin);
+            uart_stop_isr(uart);
+        }
     }
     free(uart);
 }
@@ -392,40 +457,39 @@ void uart_swap(uart_t* uart, int tx_pin)
     }
     switch(uart->uart_nr) {
     case UART0:
-        if(((uart->tx_pin == 1 || uart->tx_pin == 2) && uart->tx_enabled) || (uart->rx_pin == 3 && uart->rx_enabled)) {
+        if(uart->tx_enabled) { //TX
+            uart_disable_pin(uart->tx_pin);
+        }
+        if(uart->rx_enabled) { //RX
+            uart_disable_pin(uart->rx_pin);
+        }
+
+        if(((uart->tx_pin == 1 || uart->tx_pin == 2) && uart->tx_enabled)
+                || (uart->rx_pin == 3 && uart->rx_enabled)) {
             if(uart->tx_enabled) { //TX
-                pinMode(uart->tx_pin, INPUT);
                 uart->tx_pin = 15;
             }
             if(uart->rx_enabled) { //RX
-                pinMode(uart->rx_pin, INPUT);
                 uart->rx_pin = 13;
             }
-            if(uart->tx_enabled) {
-                pinMode(uart->tx_pin, FUNCTION_4);    //TX
-            }
-            if(uart->rx_enabled) {
-                pinMode(uart->rx_pin, FUNCTION_4);    //RX
-            }
             IOSWAP |= (1 << IOSWAPU0);
         } else {
             if(uart->tx_enabled) { //TX
-                pinMode(uart->tx_pin, INPUT);
                 uart->tx_pin = (tx_pin == 2)?2:1;
             }
             if(uart->rx_enabled) { //RX
-                pinMode(uart->rx_pin, INPUT);
                 uart->rx_pin = 3;
             }
-            if(uart->tx_enabled) {
-                pinMode(uart->tx_pin, (tx_pin == 2)?FUNCTION_4:SPECIAL);    //TX
-            }
-            if(uart->rx_enabled) {
-                pinMode(3, SPECIAL);    //RX
-            }
             IOSWAP &= ~(1 << IOSWAPU0);
         }
 
+        if(uart->tx_enabled) { //TX
+            uart0_enable_tx_pin(uart->tx_pin);
+        }
+        if(uart->rx_enabled) { //RX
+            uart0_enable_rx_pin(uart->rx_pin);
+        }
+
         break;
     case UART1:
         // Currently no swap possible! See GPIO pins used by UART
@@ -444,13 +508,13 @@ void uart_set_tx(uart_t* uart, int tx_pin)
     case UART0:
         if(uart->tx_enabled) {
             if (uart->tx_pin == 1 && tx_pin == 2) {
-                pinMode(uart->tx_pin, INPUT);
+                uart_disable_pin(uart->tx_pin);
                 uart->tx_pin = 2;
-                pinMode(uart->tx_pin, FUNCTION_4);
+                uart0_enable_tx_pin(uart->tx_pin);
             } else if (uart->tx_pin == 2 && tx_pin != 2) {
-                pinMode(uart->tx_pin, INPUT);
+                uart_disable_pin(uart->tx_pin);
                 uart->tx_pin = 1;
-                pinMode(uart->tx_pin, SPECIAL);
+                uart0_enable_tx_pin(uart->tx_pin);
             }
         }
 
@@ -471,7 +535,7 @@ void uart_set_pins(uart_t* uart, int tx, int rx)
 
     if(uart->uart_nr == UART0) { // Only UART0 allows pin changes
         if(uart->tx_enabled && uart->tx_pin != tx) {
-            if( rx == 13 && tx == 15) {
+            if(rx == 13 && tx == 15) {
                 uart_swap(uart, 15);
             } else if (rx == 3 && (tx == 1 || tx == 2)) {
                 if (uart->rx_pin != rx) {
@@ -529,24 +593,31 @@ static void uart1_write_char(char c)
 void uart_set_debug(int uart_nr)
 {
     s_uart_debug_nr = uart_nr;
+    void (*func)(char) = NULL;
     switch(s_uart_debug_nr) {
     case UART0:
-        system_set_os_print(1);
-        ets_install_putc1((void *) &uart0_write_char);
+        func = &uart0_write_char;
         break;
     case UART1:
-        system_set_os_print(1);
-        ets_install_putc1((void *) &uart1_write_char);
+        func = &uart1_write_char;
         break;
     case UART_NO:
     default:
-        system_set_os_print(0);
-        ets_install_putc1((void *) &uart_ignore_char);
+        func = &uart_ignore_char;
         break;
     }
+    if(!gdbstub_has_putc1_control()) {
+        system_set_os_print((uint8)((uart_nr == UART0 || uart_nr == UART1)?1:0));
+        ets_install_putc1((void *) func);
+    } else {
+        gdbstub_set_putc1_callback(func);
+    }
 }
 
 int uart_get_debug()
 {
     return s_uart_debug_nr;
 }
+
+void gdbstub_hook_enable_tx_pin_uart0(uint8_t pin) __attribute__((alias("uart0_enable_tx_pin")));
+void gdbstub_hook_enable_rx_pin_uart0(uint8_t pin) __attribute__((alias("uart0_enable_rx_pin")));
diff --git a/libraries/GDBStub/src/internal/gdbstub-cfg.h b/libraries/GDBStub/src/internal/gdbstub-cfg.h
index 361de6c7f7..d3c624673a 100644
--- a/libraries/GDBStub/src/internal/gdbstub-cfg.h
+++ b/libraries/GDBStub/src/internal/gdbstub-cfg.h
@@ -1,5 +1,5 @@
 #ifndef GDBSTUB_CFG_H
-#define  GDBSTUB_CFG_H
+#define GDBSTUB_CFG_H
 
 /*
 Enable this define if you're using the RTOS SDK. It will use a custom exception handler instead of the HAL
@@ -19,6 +19,14 @@ stops when you run into an error in your code, try enabling this.
 #define GDBSTUB_USE_OWN_STACK 0
 #endif
 
+/*
+Enable this to cause the program to pause and wait for gdb to be connected when an exception is
+encountered.
+*/
+#ifndef GDBSTUB_BREAK_ON_EXCEPTION
+#define GDBSTUB_BREAK_ON_EXCEPTION 1
+#endif
+
 /*
 If this is defined, gdbstub will break the program when you press Ctrl-C in gdb. it does this by
 hooking the UART interrupt. Unfortunately, this means receiving stuff over the serial port won't
@@ -26,7 +34,7 @@ work for your program anymore. This will fail if your program sets an UART inter
 the gdbstub_init call.
 */
 #ifndef GDBSTUB_CTRLC_BREAK
-#define GDBSTUB_CTRLC_BREAK 0
+#define GDBSTUB_CTRLC_BREAK 1
 #endif
 
 /*
@@ -35,7 +43,7 @@ will show up in your gdb session, which is useful if you use gdb to do stuff. It
 you use a normal terminal, you can't read the printfs anymore.
 */
 #ifndef GDBSTUB_REDIRECT_CONSOLE_OUTPUT
-#define GDBSTUB_REDIRECT_CONSOLE_OUTPUT 0
+#define GDBSTUB_REDIRECT_CONSOLE_OUTPUT 1
 #endif
 
 /*
@@ -55,7 +63,13 @@ flash somehow is disabled (eg during SPI operations or flash write/erase operati
 are called when the flash is disabled (eg due to a Ctrl-C at the wrong time), the ESP8266 will most
 likely crash.
 */
-#define ATTR_GDBINIT	ICACHE_FLASH_ATTR
-#define ATTR_GDBFN		ICACHE_RAM_ATTR
+#define ATTR_GDBINIT		ICACHE_FLASH_ATTR
+#define ATTR_GDBFN			ICACHE_RAM_ATTR
+#define ATTR_GDBEXTERNFN	ICACHE_FLASH_ATTR
+
+#define ASATTR_GDBINIT		.section .irom0.text
+#define ASATTR_GDBFN		.section .iram.text
+#define ASATTR_GDBEXTERNFN	.section .irom0.text
+
 
 #endif
diff --git a/libraries/GDBStub/src/internal/gdbstub-entry.S b/libraries/GDBStub/src/internal/gdbstub-entry.S
index 8ff28da66e..7ea3a40522 100644
--- a/libraries/GDBStub/src/internal/gdbstub-entry.S
+++ b/libraries/GDBStub/src/internal/gdbstub-entry.S
@@ -24,10 +24,13 @@
 .global gdbstub_exceptionStack
 #endif
 
-	.text
+	ASATTR_GDBFN
 .literal_position
 
-	.text
+	ASATTR_GDBINIT
+.literal_position
+
+	ASATTR_GDBFN
 	.align	4
 
 /*
@@ -51,6 +54,7 @@ This is the debugging exception routine; it's called by the debugging vector
 We arrive here with all regs intact except for a2. The old contents of A2 are saved
 into the DEBUG_EXCSAVE special function register. EPC is the original PC.
 */
+	.type gdbstub_debug_exception_entry, @function
 gdbstub_debug_exception_entry:
 /*
 	//Minimum no-op debug exception handler, for debug
@@ -64,32 +68,32 @@ gdbstub_debug_exception_entry:
 //Save all regs to structure
 	movi	a2, gdbstub_savedRegs
 	s32i	a0, a2, 0x10
-	s32i	a1, a2, 0x58
+	s32i	a1, a2, 0x14
 	rsr		a0, DEBUG_PS
 	s32i	a0, a2, 0x04
 	rsr		a0, DEBUG_EXCSAVE //was R2
-	s32i	a0, a2, 0x14
-	s32i	a3, a2, 0x18
-	s32i	a4, a2, 0x1c
-	s32i	a5, a2, 0x20
-	s32i	a6, a2, 0x24
-	s32i	a7, a2, 0x28
-	s32i	a8, a2, 0x2c
-	s32i	a9, a2, 0x30
-	s32i	a10, a2, 0x34
-	s32i	a11, a2, 0x38
-	s32i	a12, a2, 0x3c
-	s32i	a13, a2, 0x40
-	s32i	a14, a2, 0x44
-	s32i	a15, a2, 0x48
+	s32i	a0, a2, 0x18
+	s32i	a3, a2, 0x1c
+	s32i	a4, a2, 0x20
+	s32i	a5, a2, 0x24
+	s32i	a6, a2, 0x28
+	s32i	a7, a2, 0x2c
+	s32i	a8, a2, 0x30
+	s32i	a9, a2, 0x34
+	s32i	a10, a2, 0x38
+	s32i	a11, a2, 0x3c
+	s32i	a12, a2, 0x40
+	s32i	a13, a2, 0x44
+	s32i	a14, a2, 0x48
+	s32i	a15, a2, 0x4c
 	rsr		a0, SAR
 	s32i	a0, a2, 0x08
 	rsr		a0, LITBASE
-	s32i	a0, a2, 0x4C
-	rsr		a0, 176
 	s32i	a0, a2, 0x50
-	rsr		a0, 208
+	rsr		a0, 176
 	s32i	a0, a2, 0x54
+	rsr		a0, 208
+	s32i	a0, a2, 0x58
 	rsr		a0, DEBUGCAUSE
 	s32i	a0, a2, 0x5C
 	rsr		a4, DEBUG_PC
@@ -127,33 +131,33 @@ DebugExceptionExit:
 	movi	a2, gdbstub_savedRegs
 	l32i	a0, a2, 0x00
 	wsr		a0, DEBUG_PC
-//	l32i	a0, a2, 0x54
+//	l32i	a0, a2, 0x58
 //	wsr		a0, 208
-	l32i	a0, a2, 0x50
+	l32i	a0, a2, 0x54
 	//wsr		a0, 176		//Some versions of gcc do not understand this...
 	.byte  0x00, 176, 0x13	//so we hand-assemble the instruction.
-	l32i	a0, a2, 0x4C
+	l32i	a0, a2, 0x50
 	wsr		a0, LITBASE
 	l32i	a0, a2, 0x08
 	wsr		a0, SAR
-	l32i	a15, a2, 0x48
-	l32i	a14, a2, 0x44
-	l32i	a13, a2, 0x40
-	l32i	a12, a2, 0x3c
-	l32i	a11, a2, 0x38
-	l32i	a10, a2, 0x34
-	l32i	a9, a2, 0x30
-	l32i	a8, a2, 0x2c
-	l32i	a7, a2, 0x28
-	l32i	a6, a2, 0x24
-	l32i	a5, a2, 0x20
-	l32i	a4, a2, 0x1c
-	l32i	a3, a2, 0x18
-	l32i	a0, a2, 0x14
+	l32i	a15, a2, 0x4c
+	l32i	a14, a2, 0x48
+	l32i	a13, a2, 0x44
+	l32i	a12, a2, 0x40
+	l32i	a11, a2, 0x3c
+	l32i	a10, a2, 0x38
+	l32i	a9, a2, 0x34
+	l32i	a8, a2, 0x30
+	l32i	a7, a2, 0x2c
+	l32i	a6, a2, 0x28
+	l32i	a5, a2, 0x24
+	l32i	a4, a2, 0x20
+	l32i	a3, a2, 0x1c
+	l32i	a0, a2, 0x18
 	wsr		a0, DEBUG_EXCSAVE //was R2
 	l32i	a0, a2, 0x04
 	wsr		a0, DEBUG_PS
-	l32i	a1, a2, 0x58
+	l32i	a1, a2, 0x14
 	l32i	a0, a2, 0x10
 
 	//Read back vector-saved a2 value, put back address of this routine.
@@ -162,8 +166,10 @@ DebugExceptionExit:
 
 	//All done. Return to where we came from.
 	rfi	XCHAL_DEBUGLEVEL
+	.size gdbstub_debug_exception_entry, .-gdbstub_debug_exception_entry
 
 
+#if GDBSTUB_BREAK_ON_EXCEPTION
 
 #if GDBSTUB_FREERTOS
 /*
@@ -184,32 +190,34 @@ the user exception handler vector:
 */
 	.global gdbstub_handle_user_exception
 	.global gdbstub_user_exception_entry
+	.type gdbstub_user_exception_entry, @function
+	ASATTR_GDBFN
 	.align	4
 gdbstub_user_exception_entry:
 //Save all regs to structure
 	movi	a0, gdbstub_savedRegs
-	s32i	a1, a0, 0x14 //was a2
-	s32i	a3, a0, 0x18
-	s32i	a4, a0, 0x1c
-	s32i	a5, a0, 0x20
-	s32i	a6, a0, 0x24
-	s32i	a7, a0, 0x28
-	s32i	a8, a0, 0x2c
-	s32i	a9, a0, 0x30
-	s32i	a10, a0, 0x34
-	s32i	a11, a0, 0x38
-	s32i	a12, a0, 0x3c
-	s32i	a13, a0, 0x40
-	s32i	a14, a0, 0x44
-	s32i	a15, a0, 0x48
+	s32i	a1, a0, 0x18 //was a2
+	s32i	a3, a0, 0x1c
+	s32i	a4, a0, 0x20
+	s32i	a5, a0, 0x24
+	s32i	a6, a0, 0x28
+	s32i	a7, a0, 0x2c
+	s32i	a8, a0, 0x30
+	s32i	a9, a0, 0x34
+	s32i	a10, a0, 0x38
+	s32i	a11, a0, 0x3c
+	s32i	a12, a0, 0x40
+	s32i	a13, a0, 0x44
+	s32i	a14, a0, 0x48
+	s32i	a15, a0, 0x4c
 	rsr		a2, SAR
 	s32i	a2, a0, 0x08
 	rsr		a2, LITBASE
-	s32i	a2, a0, 0x4C
-	rsr		a2, 176
 	s32i	a2, a0, 0x50
-	rsr		a2, 208
+	rsr		a2, 176
 	s32i	a2, a0, 0x54
+	rsr		a2, 208
+	s32i	a2, a0, 0x58
 	rsr		a2, EXCCAUSE
 	s32i	a2, a0, 0x5C
 
@@ -243,10 +251,13 @@ is still something we need to implement later, if there's any demand for it, or
 FreeRTOS to allow this in the future. (Which will then kill backwards compatibility... hmmm.)
 */
 	j UserExceptionExit
+	.size gdbstub_user_exception_entry, .-gdbstub_user_exception_entry
 
 
 	.global gdbstub_handle_uart_int
 	.global gdbstub_uart_entry
+	.type gdbstub_uart_entry, @function
+	ASATTR_GDBFN
 	.align	4
 gdbstub_uart_entry:
 	//On entry, the stack frame is at SP+16.
@@ -255,29 +266,37 @@ gdbstub_uart_entry:
 	add		a2, a2, a1
 	movi	a3, gdbstub_handle_uart_int
 	jx		a3
+	.size gdbstub_uart_entry, .-gdbstub_uart_entry
+
+#endif
 
 #endif
 
 
 
 	.global gdbstub_save_extra_sfrs_for_exception
+	.type gdbstub_save_extra_sfrs_for_exception, @function
+	ASATTR_GDBFN
 	.align 4
 //The Xtensa OS HAL does not save all the special function register things. This bit of assembly
 //fills the gdbstub_savedRegs struct with them.
 gdbstub_save_extra_sfrs_for_exception:
 	movi	a2, gdbstub_savedRegs
 	rsr		a3, LITBASE
-	s32i	a3, a2, 0x4C
-	rsr		a3, 176
 	s32i	a3, a2, 0x50
-	rsr		a3, 208
+	rsr		a3, 176
 	s32i	a3, a2, 0x54
+	rsr		a3, 208
+	s32i	a3, a2, 0x58
 	rsr		a3, EXCCAUSE
 	s32i	a3, a2, 0x5C
 	ret
+	.size gdbstub_save_extra_sfrs_for_exception, .-gdbstub_save_extra_sfrs_for_exception
 
 	.global gdbstub_init_debug_entry
 	.global _DebugExceptionVector
+	.type gdbstub_init_debug_entry, @function
+	ASATTR_GDBINIT
 	.align	4
 gdbstub_init_debug_entry:
 //This puts the following 2 instructions into the debug exception vector:
@@ -294,10 +313,13 @@ gdbstub_init_debug_entry:
 	wsr		a2, DEBUG_EXCSAVE
 
 	ret
+	.size gdbstub_init_debug_entry, .-gdbstub_init_debug_entry
 
 
 //Set up ICOUNT register to step one single instruction
 	.global gdbstub_icount_ena_single_step
+	.type gdbstub_icount_ena_single_step, @function
+	ASATTR_GDBFN
 	.align 4
 gdbstub_icount_ena_single_step:
 	movi	a3, XCHAL_DEBUGLEVEL //Only count steps in non-debug mode
@@ -306,6 +328,7 @@ gdbstub_icount_ena_single_step:
 	wsr		a2, ICOUNT
 	isync
 	ret
+	.size gdbstub_icount_ena_single_step, .-gdbstub_icount_ena_single_step
 
 
 //These routines all assume only one breakpoint and watchpoint is available, which
@@ -313,6 +336,8 @@ gdbstub_icount_ena_single_step:
 
 
 	.global gdbstub_set_hw_breakpoint
+	.type gdbstub_set_hw_breakpoint, @function
+	ASATTR_GDBFN
 gdbstub_set_hw_breakpoint:
 	//a2 - addr, a3 - len (unused here)
 	rsr		a4, IBREAKENABLE
@@ -323,8 +348,11 @@ gdbstub_set_hw_breakpoint:
 	isync
 	movi 	a2, 1
 	ret
+	.size gdbstub_set_hw_breakpoint, .-gdbstub_set_hw_breakpoint
 
 	.global gdbstub_del_hw_breakpoint
+	.type gdbstub_del_hw_breakpoint, @function
+	ASATTR_GDBFN
 gdbstub_del_hw_breakpoint:
 	//a2 - addr
 	rsr		a5, IBREAKENABLE
@@ -336,8 +364,11 @@ gdbstub_del_hw_breakpoint:
 	isync
 	movi	a2, 1
 	ret
+	.size gdbstub_del_hw_breakpoint, .-gdbstub_del_hw_breakpoint
 
 	.global gdbstub_set_hw_watchpoint
+	.type gdbstub_set_hw_watchpoint, @function
+	ASATTR_GDBFN
 	//a2 - addr, a3 - mask, a4 - type (1=read, 2=write, 3=access)
 gdbstub_set_hw_watchpoint:
 	//Check if any of the masked address bits are set. If so, that is an error.
@@ -362,9 +393,12 @@ gdbstub_set_hw_watchpoint:
 	mov		a2, a3
 	isync
 	ret
+	.size gdbstub_set_hw_watchpoint, .-gdbstub_set_hw_watchpoint
 
 
 	.global gdbstub_del_hw_watchpoint
+	.type gdbstub_del_hw_watchpoint, @function
+	ASATTR_GDBFN
 	//a2 - addr
 gdbstub_del_hw_watchpoint:
 	//See if the address matches
@@ -384,11 +418,14 @@ gdbstub_del_hw_watchpoint:
 return_w_error:
 	movi	a2, 0
 	ret
+	.size gdbstub_del_hw_watchpoint, .-gdbstub_del_hw_watchpoint
 
 
 //Breakpoint, with an attempt at a functional function prologue and epilogue...
 	.global gdbstub_do_break_breakpoint_addr
 	.global gdbstub_do_break
+	.type gdbstub_do_break, @function
+	ASATTR_GDBFN
 	.align	4
 gdbstub_do_break:
 	addi	a1, a1, -16
@@ -402,3 +439,4 @@ gdbstub_do_break_breakpoint_addr:
 	l32i	a15, a1, 12
 	addi	a1, a1, 16
 	ret
+	.size gdbstub_do_break, .-gdbstub_do_break
diff --git a/libraries/GDBStub/src/internal/gdbstub.c b/libraries/GDBStub/src/internal/gdbstub.c
index 08bfc5925d..606b3e95c4 100644
--- a/libraries/GDBStub/src/internal/gdbstub.c
+++ b/libraries/GDBStub/src/internal/gdbstub.c
@@ -14,8 +14,8 @@
 #include "c_types.h"
 #include "gpio.h"
 #include "xtensa/corebits.h"
+#include "uart_register.h"
 
-#include "gdbstub.h"
 #include "gdbstub-entry.h"
 #include "gdbstub-cfg.h"
 
@@ -26,18 +26,17 @@ struct XTensa_exception_frame_s {
 	uint32_t ps;
 	uint32_t sar;
 	uint32_t vpri;
-	uint32_t a0;
-	uint32_t a[14]; //a2..a15
+	uint32_t a[16]; //a0..a15
 //These are added manually by the exception code; the HAL doesn't set these on an exception.
 	uint32_t litbase;
 	uint32_t sr176;
 	uint32_t sr208;
-	uint32_t a1;
 	 //'reason' is abused for both the debug and the exception vector: if bit 7 is set,
 	//this contains an exception reason, otherwise it contains a debug vector bitmap.
 	uint32_t reason;
 };
 
+#if GDBSTUB_FREERTOS
 
 struct XTensa_rtos_int_frame_s {
 	uint32_t exitPtr;
@@ -47,18 +46,16 @@ struct XTensa_rtos_int_frame_s {
 	uint32_t sar;
 };
 
-#if GDBSTUB_FREERTOS
 /*
 Definitions for FreeRTOS. This redefines some os_* functions to use their non-os* counterparts. It
 also sets up some function pointers for ROM functions that aren't in the FreeRTOS ld files.
 */
 #include <string.h>
 #include <stdio.h>
-void _xt_isr_attach(int inum, void *fn);
-void _xt_isr_unmask(int inum);
+void os_isr_attach(int inum, void *fn);
 void os_install_putc1(void (*p)(char c));
 #define os_printf(...) printf(__VA_ARGS__)
-#define os_memcpy(a,b,c) memcpy(a,b,c)
+#define os_strncmp(...) strncmp(__VA_ARGS__)
 typedef void wdtfntype();
 static wdtfntype *ets_wdt_disable=(wdtfntype *)0x400030f0;
 static wdtfntype *ets_wdt_enable=(wdtfntype *)0x40002fa0;
@@ -72,32 +69,12 @@ the xthal stack frame struct.
 #include "user_interface.h"
 
 void _xtos_set_exception_handler(int cause, void (exhandler)(struct XTensa_exception_frame_s *frame));
-int os_printf_plus(const char *format, ...)  __attribute__ ((format (printf, 1, 2)));
 
 #endif
 
 #define EXCEPTION_GDB_SP_OFFSET 0x100
 
 
-//We need some UART register defines.
-#define ETS_UART_INUM 5
-#define REG_UART_BASE( i )  (0x60000000+(i)*0xf00)
-#define UART_STATUS( i )                        (REG_UART_BASE( i ) + 0x1C)
-#define UART_RXFIFO_CNT 0x000000FF
-#define UART_RXFIFO_CNT_S 0
-#define UART_TXFIFO_CNT 0x000000FF
-#define UART_TXFIFO_CNT_S                   16
-#define UART_FIFO( i )                          (REG_UART_BASE( i ) + 0x0)
-#define UART_INT_ENA(i)                     (REG_UART_BASE(i) + 0xC)
-#define UART_INT_CLR(i)                 (REG_UART_BASE(i) + 0x10)
-#define UART_RXFIFO_TOUT_INT_ENA            (BIT(8))
-#define UART_RXFIFO_FULL_INT_ENA            (BIT(0))
-#define UART_RXFIFO_TOUT_INT_CLR            (BIT(8))
-#define UART_RXFIFO_FULL_INT_CLR            (BIT(0))
-
-
-
-
 //Length of buffer used to reserve GDB commands. Has to be at least able to fit the G command, which
 //implies a minimum size of about 190 bytes.
 #define PBUFLEN 256
@@ -111,14 +88,41 @@ struct XTensa_exception_frame_s gdbstub_savedRegs;
 int exceptionStack[256];
 #endif
 
+static bool gdb_attached=false;
+static bool paused=false;
 static unsigned char cmd[PBUFLEN];		//GDB command input buffer
 static char chsum;						//Running checksum of the output packet
+#if GDBSTUB_CTRLC_BREAK
+static void (*uart_isr_callback)(void*, uint8_t)=NULL;
+static void* uart_isr_arg=NULL;
+#endif
 #if GDBSTUB_REDIRECT_CONSOLE_OUTPUT
-static unsigned char obuf[OBUFLEN];		//GDB stdout buffer
-static int obufpos=0;					//Current position in the buffer
+static void (*uart_putc1_callback)(char)=NULL;
 #endif
+static unsigned char obuf[OBUFLEN];		//GDB stdout buffer
+static size_t obufpos=0;					//Current position in the buffer
 static int32_t singleStepPs=-1;			//Stores ps when single-stepping instruction. -1 when not in use.
 
+//Uart libs can reference these to see if gdb is attaching to them
+bool gdbstub_has_putc1_control() {
+#if GDBSTUB_REDIRECT_CONSOLE_OUTPUT
+	return true;
+#else
+	return false;
+#endif
+}
+bool gdbstub_has_uart_isr_control() {
+#if GDBSTUB_CTRLC_BREAK
+	return true;
+#else
+	return false;
+#endif
+}
+
+void __attribute__((weak)) ATTR_GDBEXTERNFN gdbstub_hook_enable_tx_pin_uart0(uint8_t pin) { (void)pin; }
+void __attribute__((weak)) ATTR_GDBEXTERNFN gdbstub_hook_enable_rx_pin_uart0(uint8_t pin) { (void)pin; }
+
+
 //Small function to feed the hardware watchdog. Needed to stop the ESP from resetting
 //due to a watchdog timeout while reading a command.
 static void ATTR_GDBFN keepWDTalive() {
@@ -129,68 +133,13 @@ static void ATTR_GDBFN keepWDTalive() {
 	*wdtctl|=(1<<31);
 }
 
-//Receive a char from the uart. Uses polling and feeds the watchdog.
-static int ATTR_GDBFN gdbRecvChar() {
-	int i;
-	while (((READ_PERI_REG(UART_STATUS(0))>>UART_RXFIFO_CNT_S)&UART_RXFIFO_CNT)==0) {
-		keepWDTalive();
-	}
-	i=READ_PERI_REG(UART_FIFO(0));
-	return i;
-}
-
-//Send a char to the uart.
-static void ATTR_GDBFN gdbSendChar(char c) {
-	while (((READ_PERI_REG(UART_STATUS(0))>>UART_TXFIFO_CNT_S)&UART_TXFIFO_CNT)>=126) ;
-	WRITE_PERI_REG(UART_FIFO(0), c);
-}
-
-//Send the start of a packet; reset checksum calculation.
-static void ATTR_GDBFN gdbPacketStart() {
-	chsum=0;
-	gdbSendChar('$');
-}
-
-//Send a char as part of a packet
-static void ATTR_GDBFN gdbPacketChar(char c) {
-	if (c=='#' || c=='$' || c=='}' || c=='*') {
-		gdbSendChar('}');
-		gdbSendChar(c^0x20);
-		chsum+=(c^0x20)+'}';
-	} else {
-		gdbSendChar(c);
-		chsum+=c;
-	}
-}
-
-//Send a string as part of a packet
-static void ATTR_GDBFN gdbPacketStr(char *c) {
-	while (*c!=0) {
-		gdbPacketChar(*c);
-		c++;
-	}
-}
-
-//Send a hex val as part of a packet. 'bits'/4 dictates the number of hex chars sent.
-static void ATTR_GDBFN gdbPacketHex(int val, int bits) {
-	char hexChars[]="0123456789abcdef";
-	int i;
-	for (i=bits; i>0; i-=4) {
-		gdbPacketChar(hexChars[(val>>(i-4))&0xf]);
-	}
-}
-
-//Finish sending a packet.
-static void ATTR_GDBFN gdbPacketEnd() {
-	gdbSendChar('#');
-	gdbPacketHex(chsum, 8);
-}
 
 //Error states used by the routines that grab stuff from the incoming gdb packet
 #define ST_ENDPACKET -1
 #define ST_ERR -2
 #define ST_OK -3
 #define ST_CONT -4
+#define ST_DETACH -5
 
 //Grab a hex value from the gdb packet. Ptr will get positioned on the end
 //of the hex string, as far as the routine has read into it. Bits/4 indicates
@@ -244,8 +193,8 @@ static int ATTR_GDBFN iswap(int i) {
 
 //Read a byte from the ESP8266 memory.
 static unsigned char ATTR_GDBFN readbyte(unsigned int p) {
-	int *i=(int*)(p&(~3));
 	if (p<0x20000000 || p>=0x60000000) return -1;
+	int *i=(int*)(p&(~3));
 	return *i>>((p&3)*8);
 }
 
@@ -261,28 +210,94 @@ static void ATTR_GDBFN writeByte(unsigned int p, unsigned char d) {
 
 //Returns 1 if it makes sense to write to addr p
 static int ATTR_GDBFN validWrAddr(int p) {
-	if (p>=0x3ff00000 && p<0x40000000) return 1;
-	if (p>=0x40100000 && p<0x40140000) return 1;
-	if (p>=0x60000000 && p<0x60002000) return 1;
-	return 0;
+	return (p>=0x3ff00000 && p<0x40000000)
+		|| (p>=0x40100000 && p<0x40140000)
+		|| (p>=0x60000000 && p<0x60002000);
 }
 
-/*
-Register file in the format lx106 gdb port expects it.
-Inspired by gdb/regformats/reg-xtensa.dat from
-https://github.com/jcmvbkbc/crosstool-NG/blob/lx106-g%2B%2B/overlays/xtensa_lx106.tar
-As decoded by Cesanta.
-*/
-struct regfile {
-	uint32_t a[16];
-	uint32_t pc;
-	uint32_t sar;
-	uint32_t litbase;
-	uint32_t sr176;
-	uint32_t sr208;
-	uint32_t ps;
-};
 
+static bool ATTR_GDBFN gdbRxFifoIsEmpty() {
+	return ((READ_PERI_REG(UART_STATUS(0))>>UART_RXFIFO_CNT_S)&UART_RXFIFO_CNT)==0;
+}
+
+static inline bool ATTR_GDBFN gdbTxFifoIsFull() {
+	return ((READ_PERI_REG(UART_STATUS(0))>>UART_TXFIFO_CNT_S)&UART_TXFIFO_CNT)>=126;
+}
+
+//Send a char to the uart.
+static void ATTR_GDBFN gdbSendChar(char c) {
+	while (gdbTxFifoIsFull()) ;
+	WRITE_PERI_REG(UART_FIFO(0), c);
+}
+
+
+//Send the start of a packet; reset checksum calculation.
+static void ATTR_GDBFN gdbPacketStart() {
+	chsum=0;
+	gdbSendChar('$');
+}
+
+//Send a char as part of a packet
+static void ATTR_GDBFN gdbPacketChar(char c) {
+	if (c=='#' || c=='$' || c=='}' || c=='*') {
+		gdbSendChar('}');
+		chsum+='}';
+		c^=0x20;
+	}
+	gdbSendChar(c);
+	chsum+=c;
+}
+
+//Can't be a local variable or gcc will implicitly call memcpy, which could be
+//in flash.
+static const char hexChars[]="0123456789abcdef";
+
+//Send a hex val as part of a packet. 'bits'/4 dictates the number of hex chars sent.
+static void ATTR_GDBFN gdbPacketHex(int val, int bits) {
+	int i;
+	for (i=bits; i>0; i-=4) {
+		gdbPacketChar(hexChars[(val>>(i-4))&0xf]);
+	}
+}
+
+//Send a hex val as part of a packet. 'bits'/4 dictates the number of hex chars sent.
+static void ATTR_GDBFN gdbPacketSwappedHexInt(int val) {
+	gdbPacketHex(iswap(val), 32);
+}
+
+//Finish sending a packet.
+static void ATTR_GDBFN gdbPacketEnd() {
+	gdbSendChar('#');
+	//Ok to use packet version here since hex char can never be an
+	//excape-requiring character
+	gdbPacketHex(chsum, 8);
+}
+
+// Send a complete packet containing str
+static void ATTR_GDBFN gdbSendPacketStr(const char *c) {
+	gdbPacketStart();
+	while (*c!=0) {
+		gdbPacketChar(*c);
+		c++;
+	}
+	gdbPacketEnd();
+}
+
+// Send a complete packet containing str as an output command
+static inline void ATTR_GDBFN gdbSendOutputPacketStr(const unsigned char* buf, size_t size) {
+	size_t i;
+	gdbPacketStart();
+	gdbPacketChar('O');
+	for (i=0; i<size; i++) gdbPacketHex(buf[i], 8);
+	gdbPacketEnd();
+}
+
+static long ATTR_GDBFN gdbGetSwappedHexInt(unsigned char **ptr) {
+	return iswap(gdbGetHexVal(ptr, 32));
+}
+
+
+static const char exceptionSignal[]={4,31,11,11,2,6,8,0,6,7,0,0,7,7,7,7};
 
 //Send the reason execution is stopped to GDB.
 static void ATTR_GDBFN sendReason() {
@@ -290,8 +305,7 @@ static void ATTR_GDBFN sendReason() {
 	char *reason=""; //default
 #endif
 	//exception-to-signal mapping
-	char exceptionSignal[]={4,31,11,11,2,6,8,0,6,7,0,0,7,7,7,7};
-	unsigned int i=0;
+	size_t i;
 	gdbPacketStart();
 	gdbPacketChar('T');
 	if (gdbstub_savedRegs.reason==0xff) {
@@ -319,37 +333,103 @@ static void ATTR_GDBFN sendReason() {
 	gdbPacketEnd();
 }
 
+//Can't be a local variables or gcc will implicitly call memcpy, which could be
+//in flash.
+static const char * const gdb_ok_str="OK";
+static inline void ATTR_GDBFN gdbSendPacketOK() {
+	gdbSendPacketStr(gdb_ok_str);
+}
+
+static const char * const gdb_e01_str="E01";
+static inline void ATTR_GDBFN gdbSendPacketE01() {
+	gdbSendPacketStr(gdb_e01_str);
+}
+
+static inline void ATTR_GDBFN gdbSendEmptyPacket() {
+	gdbPacketStart();
+	gdbPacketEnd();
+}
+
+void ATTR_GDBFN gdbstub_flush() {
+	size_t i;
+	if (obufpos>0) {
+		if (gdb_attached) {
+			gdbSendOutputPacketStr(obuf, obufpos);
+		} else {
+			for (i=0; i<obufpos; i++) gdbSendChar(obuf[i]);
+		}
+		obufpos=0;
+	}
+}
+
+void ATTR_GDBEXTERNFN gdbstub_buffered_write_char(char c) {
+	obuf[obufpos++]=c;
+	if (c=='\n' || obufpos==OBUFLEN || paused) {
+		gdbstub_flush();
+	}
+}
+
+void ATTR_GDBEXTERNFN gdbstub_write_char(char c) {
+	obuf[obufpos++]=c;
+	gdbstub_flush();
+}
+
+void ATTR_GDBEXTERNFN gdbstub_write(const char* buf, size_t size) {
+	size_t i;
+	for (i=0; i<size; i++) gdbstub_buffered_write_char(buf[i]);
+	gdbstub_flush();
+}
+
+/*
+Register file in the format lx106 gdb port expects it.
+Inspired by gdb/regformats/reg-xtensa.dat from
+https://github.com/jcmvbkbc/crosstool-NG/blob/lx106-g%2B%2B/overlays/xtensa_lx106.tar
+As decoded by Cesanta.
+
+struct regfile {
+	uint32_t a[16];
+	uint32_t pc;
+	uint32_t sar;
+	uint32_t litbase;
+	uint32_t sr176;
+	uint32_t sr208;
+	uint32_t ps;
+};
+*/
+
+//Can't be a local variables or gcc will implicitly call memcpy, which could be
+//in flash.
+static const char * const gdb_supported_str="Supported";
+// PacketSize is in hex
+static const char * const gdb_supported_resp_str="swbreak+;hwbreak+;PacketSize=FF";
+static const char * const gdb_attached_str="Attached";
+static const char * const gdb_1_str="1";
+
 //Handle a command as received from GDB.
-static int ATTR_GDBFN gdbHandleCommand(unsigned char *cmd, int len) {
+static inline int ATTR_GDBFN gdbHandleCommand(unsigned char *cmd, int len) {
 	(void)len; // unused
 	//Handle a command
 	int i, j, k;
 	unsigned char *data=cmd+1;
 	if (cmd[0]=='g') {		//send all registers to gdb
 		gdbPacketStart();
-		gdbPacketHex(iswap(gdbstub_savedRegs.a0), 32);
-		gdbPacketHex(iswap(gdbstub_savedRegs.a1), 32);
-		for (i=2; i<16; i++) gdbPacketHex(iswap(gdbstub_savedRegs.a[i-2]), 32);
-		gdbPacketHex(iswap(gdbstub_savedRegs.pc), 32);
-		gdbPacketHex(iswap(gdbstub_savedRegs.sar), 32);
-		gdbPacketHex(iswap(gdbstub_savedRegs.litbase), 32);
-		gdbPacketHex(iswap(gdbstub_savedRegs.sr176), 32);
+		for (i=0; i<16; i++) gdbPacketSwappedHexInt(gdbstub_savedRegs.a[i]);
+		gdbPacketSwappedHexInt(gdbstub_savedRegs.pc);
+		gdbPacketSwappedHexInt(gdbstub_savedRegs.sar);
+		gdbPacketSwappedHexInt(gdbstub_savedRegs.litbase);
+		gdbPacketSwappedHexInt(gdbstub_savedRegs.sr176);
 		gdbPacketHex(0, 32);
-		gdbPacketHex(iswap(gdbstub_savedRegs.ps), 32);
+		gdbPacketSwappedHexInt(gdbstub_savedRegs.ps);
 		gdbPacketEnd();
 	} else if (cmd[0]=='G') {	//receive content for all registers from gdb
-		gdbstub_savedRegs.a0=iswap(gdbGetHexVal(&data, 32));
-		gdbstub_savedRegs.a1=iswap(gdbGetHexVal(&data, 32));
-		for (i=2; i<16; i++) gdbstub_savedRegs.a[i-2]=iswap(gdbGetHexVal(&data, 32));
-		gdbstub_savedRegs.pc=iswap(gdbGetHexVal(&data, 32));
-		gdbstub_savedRegs.sar=iswap(gdbGetHexVal(&data, 32));
-		gdbstub_savedRegs.litbase=iswap(gdbGetHexVal(&data, 32));
-		gdbstub_savedRegs.sr176=iswap(gdbGetHexVal(&data, 32));
+		for (i=0; i<16; i++) gdbstub_savedRegs.a[i]=gdbGetSwappedHexInt(&data);
+		gdbstub_savedRegs.pc=gdbGetSwappedHexInt(&data);
+		gdbstub_savedRegs.sar=gdbGetSwappedHexInt(&data);
+		gdbstub_savedRegs.litbase=gdbGetSwappedHexInt(&data);
+		gdbstub_savedRegs.sr176=gdbGetSwappedHexInt(&data);
 		gdbGetHexVal(&data, 32);
-		gdbstub_savedRegs.ps=iswap(gdbGetHexVal(&data, 32));
-		gdbPacketStart();
-		gdbPacketStr("OK");
-		gdbPacketEnd();
+		gdbstub_savedRegs.ps=gdbGetSwappedHexInt(&data);
+		gdbSendPacketOK();
 	} else if (cmd[0]=='m') {	//read memory to gdb
 		i=gdbGetHexVal(&data, -1);
 		data++;
@@ -371,24 +451,16 @@ static int ATTR_GDBFN gdbHandleCommand(unsigned char *cmd, int len) {
 			}
 			//Make sure caches are up-to-date. Procedure according to Xtensa ISA document, ISYNC inst desc.
 			asm volatile("ISYNC\nISYNC\n");
-			gdbPacketStart();
-			gdbPacketStr("OK");
-			gdbPacketEnd();
+			gdbSendPacketOK();
 		} else {
 			//Trying to do a software breakpoint on a flash proc, perhaps?
-			gdbPacketStart();
-			gdbPacketStr("E01");
-			gdbPacketEnd();
+			gdbSendPacketE01();
 		}
 	} else if (cmd[0]=='?') {	//Reply with stop reason
 		sendReason();
-//	} else if (strncmp(cmd, "vCont?", 6)==0) {
-//		gdbPacketStart();
-//		gdbPacketStr("vCont;c;s");
-//		gdbPacketEnd();
-	} else if (strncmp((char*)cmd, "vCont;c", 7)==0 || cmd[0]=='c') {	//continue execution
+	} else if (cmd[0]=='c') {	//continue execution
 		return ST_CONT;
-	} else if (strncmp((char*)cmd, "vCont;s", 7)==0 || cmd[0]=='s') {	//single-step instruction
+	} else if (cmd[0]=='s') {	//single-step instruction
 		//Single-stepping can go wrong if an interrupt is pending, especially when it is e.g. a task switch:
 		//the ICOUNT register will overflow in the task switch code. That is why we disable interupts when
 		//doing single-instruction stepping.
@@ -396,32 +468,36 @@ static int ATTR_GDBFN gdbHandleCommand(unsigned char *cmd, int len) {
 		gdbstub_savedRegs.ps=(gdbstub_savedRegs.ps & ~0xf)|(XCHAL_DEBUGLEVEL-1);
 		gdbstub_icount_ena_single_step();
 		return ST_CONT;
+	} else if (cmd[0]=='D') {	//detach
+		gdbSendPacketOK();
+		return ST_DETACH;
+	} else if (cmd[0]=='k') {	//kill
+		system_restart_core();
 	} else if (cmd[0]=='q') {	//Extended query
-		if (strncmp((char*)&cmd[1], "Supported", 9)==0) { //Capabilities query
-			gdbPacketStart();
-			gdbPacketStr("swbreak+;hwbreak+;PacketSize=255");
-			gdbPacketEnd();
+		if (os_strncmp((char*)&cmd[1], gdb_supported_str, 9)==0) { //Capabilities query
+			gdbSendPacketStr(gdb_supported_resp_str); // PacketSize is in hex
+		} else if (os_strncmp((char*)&cmd[1], gdb_attached_str, 8)==0) {
+			//Let gdb know that it is attaching to a running program
+			//In general that just means it detaches instead of killing when it exits
+			gdbSendPacketStr(gdb_1_str);
 		} else {
 			//We don't support other queries.
-			gdbPacketStart();
-			gdbPacketEnd();
-			return ST_ERR;
+			gdbSendEmptyPacket();
 		}
 	} else if (cmd[0]=='Z') {	//Set hardware break/watchpoint.
 		data+=2; //skip 'x,'
 		i=gdbGetHexVal(&data, -1);
 		data++; //skip ','
 		j=gdbGetHexVal(&data, -1);
-		gdbPacketStart();
 		if (cmd[1]=='1') {	//Set breakpoint
 			if (gdbstub_set_hw_breakpoint(i, j)) {
-				gdbPacketStr("OK");
+				gdbSendPacketOK();
 			} else {
-				gdbPacketStr("E01");
+				gdbSendPacketE01();
 			}
 		} else if (cmd[1]=='2' || cmd[1]=='3' || cmd[1]=='4') { //Set watchpoint
 			int access=0;
-			int mask=0;
+			unsigned int mask=0;
 			if (cmd[1]=='2') access=2; //write
 			if (cmd[1]=='3') access=1; //read
 			if (cmd[1]=='4') access=3; //access
@@ -433,142 +509,218 @@ static int ATTR_GDBFN gdbHandleCommand(unsigned char *cmd, int len) {
 			if (j==32) mask=0x20;
 			if (j==64) mask=0x00;
 			if (mask!=0 && gdbstub_set_hw_watchpoint(i,mask, access)) {
-				gdbPacketStr("OK");
+				gdbSendPacketOK();
 			} else {
-				gdbPacketStr("E01");
+				gdbSendPacketE01();
 			}
+		} else {
+			gdbSendEmptyPacket();
 		}
-		gdbPacketEnd();
 	} else if (cmd[0]=='z') {	//Clear hardware break/watchpoint
 		data+=2; //skip 'x,'
 		i=gdbGetHexVal(&data, -1);
 		data++; //skip ','
 		j=gdbGetHexVal(&data, -1);
-		gdbPacketStart();
 		if (cmd[1]=='1') {	//hardware breakpoint
 			if (gdbstub_del_hw_breakpoint(i)) {
-				gdbPacketStr("OK");
+				gdbSendPacketOK();
 			} else {
-				gdbPacketStr("E01");
+				gdbSendPacketE01();
 			}
 		} else if (cmd[1]=='2' || cmd[1]=='3' || cmd[1]=='4') { //hardware watchpoint
 			if (gdbstub_del_hw_watchpoint(i)) {
-				gdbPacketStr("OK");
+				gdbSendPacketOK();
 			} else {
-				gdbPacketStr("E01");
+				gdbSendPacketE01();
 			}
+		} else {
+			gdbSendEmptyPacket();
 		}
-		gdbPacketEnd();
 	} else {
 		//We don't recognize or support whatever GDB just sent us.
-		gdbPacketStart();
-		gdbPacketEnd();
-		return ST_ERR;
+		gdbSendEmptyPacket();
 	}
 	return ST_OK;
 }
 
+static inline void ATTR_GDBFN gdbPause() {
+	paused=true;
+	ETS_UART_INTR_DISABLE();
+	ets_wdt_disable();
+	gdbstub_flush();
+}
+
+static inline void ATTR_GDBFN gdbUnpause() {
+	paused=false;
+	ets_wdt_enable();
+	ETS_UART_INTR_ENABLE();
+}
+
+enum gdb_read_state_t {
+	gdb_read_state_init,
+	gdb_read_state_cmd_read,
+	gdb_read_state_escape_char,
+	gdb_read_state_read_chsum_1,
+	gdb_read_state_read_chsum_2,
+};
 
 //Lower layer: grab a command packet and check the checksum
 //Calls gdbHandleCommand on the packet if the checksum is OK
-//Returns ST_OK on success, ST_ERR when checksum fails, a
-//character if it is received instead of the GDB packet
-//start char.
-static int ATTR_GDBFN gdbReadCommand() {
+//Returns only if execution of the user program should continue
+//Otherwise keeps reading uart data and executing commands
+//Flags that gdb has been attached whenever a gdb formatted
+// packet is received
+//While gdb is attached, checks for ctl-c (\x03) if it's not
+// already paused
+//Keeps reading commands if it is paused, until either a
+// continue, detach, or kill command is received
+//It is not necessary for gdb to be attached for it to be paused
+//For example, during an exception break, the program is
+// paused but gdb might not be attached yet
+static void ATTR_GDBFN gdbReadCommand(bool system_stopped) {
+	static enum gdb_read_state_t read_state=gdb_read_state_init;
+	static unsigned char chsum;
+	static unsigned char sentchs[2];
+	static int p;
 	unsigned char c;
-	unsigned char chsum=0, rchsum;
-	unsigned char sentchs[2];
-	int p=0;
+	unsigned char rchsum;
 	unsigned char *ptr;
-	c=gdbRecvChar();
-	if (c!='$') return c;
-	while(1) {
-		c=gdbRecvChar();
-		if (c=='#') {	//end of packet, checksum follows
-			cmd[p]=0;
+	if (system_stopped) {
+		gdbPause();
+		sendReason();
+	}
+	while (true) {
+		if (paused) {
+			while (gdbRxFifoIsEmpty()) {
+				keepWDTalive();
+			}
+		}
+		if (gdbRxFifoIsEmpty()) {
 			break;
 		}
-		chsum+=c;
-		if (c=='$') {
-			//Wut, restart packet?
-			chsum=0;
-			p=0;
-			continue;
+		c=READ_PERI_REG(UART_FIFO(0)) & 0xFF;
+#if GDBSTUB_CTRLC_BREAK
+		if (!gdb_attached && uart_isr_callback!=NULL) {
+			uart_isr_callback(uart_isr_arg, c);
 		}
-		if (c=='}') {		//escape the next char
-			c=gdbRecvChar();
+#endif
+		switch (read_state) {
+		case gdb_read_state_init:
+			if (c=='$') {
+				read_state=gdb_read_state_cmd_read;
+				chsum=0;
+				p=0;
+			} else if (c==0x3 && gdb_attached && !paused) {
+				gdbPause();
+				gdbstub_savedRegs.reason=0xff; //mark as user break reason
+				sendReason();
+			}
+			break;
+		case gdb_read_state_cmd_read:
+			if (c=='#') {	//end of packet, checksum follows
+				read_state=gdb_read_state_read_chsum_1;
+				cmd[p]=0;
+			} else if (c=='$') {
+				//Wut, restart packet?
+				chsum=0;
+				p=0;
+			} else if ((p+1)>=PBUFLEN) {
+				//Received more than the size of the command buffer
+				read_state=gdb_read_state_init;
+			} else {
+				chsum+=c;
+				if (c=='}') {		//escape the next char
+					read_state=gdb_read_state_escape_char;
+				} else {
+					cmd[p++]=c;
+				}
+			}
+			break;
+		case gdb_read_state_escape_char:
 			chsum+=c;
-			c^=0x20;
+			cmd[p++]=c^0x20;
+			break;
+		case gdb_read_state_read_chsum_1:
+			read_state=gdb_read_state_read_chsum_2;
+			sentchs[0]=c;
+			break;
+		case gdb_read_state_read_chsum_2:
+			read_state=gdb_read_state_init;
+			sentchs[1]=c;
+			ptr=&sentchs[0];
+			rchsum=gdbGetHexVal(&ptr, 8);
+			if (rchsum==chsum) {
+				gdb_attached=true;
+				if (!paused) {
+					gdbPause();
+					gdbstub_savedRegs.reason=0xff; //mark as user break reason
+				}
+				gdbSendChar('+');
+				switch (gdbHandleCommand(cmd, p)) {
+				case ST_DETACH:
+					gdb_attached=false;
+					if (system_stopped) {
+						break;
+					}
+				case ST_CONT:
+					gdbUnpause();
+				}
+			} else if (gdb_attached) {
+				gdbSendChar('-');
+			}
+			break;
 		}
-		cmd[p++]=c;
-		if (p>=PBUFLEN) return ST_ERR;
-	}
-	//A # has been received. Get and check the received chsum.
-	sentchs[0]=gdbRecvChar();
-	sentchs[1]=gdbRecvChar();
-	ptr=&sentchs[0];
-	rchsum=gdbGetHexVal(&ptr, 8);
-//	os_printf("c %x r %x\n", chsum, rchsum);
-	if (rchsum!=chsum) {
-		gdbSendChar('-');
-		return ST_ERR;
-	} else {
-		gdbSendChar('+');
-		return gdbHandleCommand(cmd, p);
 	}
 }
 
+
+
 //Get the value of one of the A registers
 static unsigned int ATTR_GDBFN getaregval(int reg) {
-	if (reg==0) return gdbstub_savedRegs.a0;
-	if (reg==1) return gdbstub_savedRegs.a1;
-	return gdbstub_savedRegs.a[reg-2];
+	return gdbstub_savedRegs.a[reg];
 }
 
 //Set the value of one of the A registers
-static void ATTR_GDBFN setaregval(int reg, unsigned int val) {
-	os_printf("%x -> %x\n", val, reg);
-	if (reg==0) gdbstub_savedRegs.a0=val;
-	if (reg==1) gdbstub_savedRegs.a1=val;
-	gdbstub_savedRegs.a[reg-2]=val;
+static inline void ATTR_GDBFN setaregval(int reg, unsigned int val) {
+	// os_printf("%x -> %x\n", val, reg);
+	gdbstub_savedRegs.a[reg]=val;
 }
 
 //Emulate the l32i/s32i instruction we're stopped at.
-static void ATTR_GDBFN emulLdSt() {
+static inline void ATTR_GDBFN emulLdSt() {
 	unsigned char i0=readbyte(gdbstub_savedRegs.pc);
 	unsigned char i1=readbyte(gdbstub_savedRegs.pc+1);
-	unsigned char i2=readbyte(gdbstub_savedRegs.pc+2);
+	unsigned char i2;
 	int *p;
-	if ((i0&0xf)==2 && (i1&0xf0)==0x20) {
-		//l32i
-		p=(int*)getaregval(i1&0xf)+(i2*4);
-		setaregval(i0>>4, *p);
-		gdbstub_savedRegs.pc+=3;
-	} else if ((i0&0xf)==0x8) {
-		//l32i.n
-		p=(int*)getaregval(i1&0xf)+((i1>>4)*4);
-		setaregval(i0>>4, *p);
-		gdbstub_savedRegs.pc+=2;
-	} else if ((i0&0xf)==2 && (i1&0xf0)==0x60) {
-		//s32i
+
+	if ((i0&0xf)==2 && (i1&0xb0)==0x20) {
+		//l32i or s32i
+		i2=readbyte(gdbstub_savedRegs.pc+2);
 		p=(int*)getaregval(i1&0xf)+(i2*4);
-		*p=getaregval(i0>>4);
+		i0>>=4;
+		if ((i1&0xf0)==0x20) { //l32i
+			setaregval(i0, *p);
+		} else { //s32i
+			*p=getaregval(i0);
+		}
 		gdbstub_savedRegs.pc+=3;
-	} else if ((i0&0xf)==0x9) {
-		//s32i.n
+	} else if ((i0&0xe)==0x8) {
+		//l32i.n or s32i.n
 		p=(int*)getaregval(i1&0xf)+((i1>>4)*4);
-		*p=getaregval(i0>>4);
+		if ((i0&0xf)==0x8) { //l32i.n
+			setaregval(i0>>4, *p);
+		} else {
+			*p=getaregval(i0>>4);
+		}
 		gdbstub_savedRegs.pc+=2;
-	} else {
-		os_printf("GDBSTUB: No l32i/s32i instruction: %x %x %x. Huh?", i2, i1, i0);
+	// } else {
+	// 	os_printf("GDBSTUB: No l32i/s32i instruction: %x %x. Huh?", i1, i0);
 	}
 }
 
 //We just caught a debug exception and need to handle it. This is called from an assembly
 //routine in gdbstub-entry.S
 void ATTR_GDBFN gdbstub_handle_debug_exception() {
-	ets_wdt_disable();
-
 	if (singleStepPs!=-1) {
 		//We come here after single-stepping an instruction. Interrupts are disabled
 		//for the single step. Re-enable them here.
@@ -576,8 +728,7 @@ void ATTR_GDBFN gdbstub_handle_debug_exception() {
 		singleStepPs=-1;
 	}
 
-	sendReason();
-	while(gdbReadCommand()!=ST_CONT);
+	gdbReadCommand(true);
 	if ((gdbstub_savedRegs.reason&0x84)==0x4) {
 		//We stopped due to a watchpoint. We can't re-execute the current instruction
 		//because it will happily re-trigger the same watchpoint, so we emulate it
@@ -600,74 +751,39 @@ void ATTR_GDBFN gdbstub_handle_debug_exception() {
 			gdbstub_savedRegs.pc+=3;
 		}
 	}
-	ets_wdt_enable();
 }
 
 
-#if GDBSTUB_FREERTOS
-//Freetos exception. This routine is called by an assembly routine in gdbstub-entry.S
-void ATTR_GDBFN gdbstub_handle_user_exception() {
-	ets_wdt_disable();
-	gdbstub_savedRegs.reason|=0x80; //mark as an exception reason
-	sendReason();
-	while(gdbReadCommand()!=ST_CONT);
-	ets_wdt_enable();
-}
-#else
 
-//Non-OS exception handler. Gets called by the Xtensa HAL.
-static void ATTR_GDBFN gdb_exception_handler(struct XTensa_exception_frame_s *frame) {
-	//Save the extra registers the Xtensa HAL doesn't save
-	gdbstub_save_extra_sfrs_for_exception();
+#if GDBSTUB_BREAK_ON_EXCEPTION || GDBSTUB_CTRLC_BREAK
+
+#if !GDBSTUB_FREERTOS
+static inline void ATTR_GDBFN gdbReadCommandWithFrame(bool system_stopped, void* frame) {
 	//Copy registers the Xtensa HAL did save to gdbstub_savedRegs
-	os_memcpy(&gdbstub_savedRegs, frame, 19*4);
+	os_memcpy(&gdbstub_savedRegs, frame, 5*4);
+	os_memcpy(&gdbstub_savedRegs.a[2], ((uint32_t*)frame)+5, 14*4);
 	//Credits go to Cesanta for this trick. A1 seems to be destroyed, but because it
 	//has a fixed offset from the address of the passed frame, we can recover it.
-	gdbstub_savedRegs.a1=(uint32_t)frame+EXCEPTION_GDB_SP_OFFSET;
-
-	gdbstub_savedRegs.reason|=0x80; //mark as an exception reason
-
-	ets_wdt_disable();
-	*((uint32_t*)UART_INT_ENA(0)) = 0;
-	sendReason();
-	while(gdbReadCommand()!=ST_CONT);
-	ets_wdt_enable();
+	gdbstub_savedRegs.a[1]=(uint32_t)frame+EXCEPTION_GDB_SP_OFFSET;
 
+	gdbReadCommand(system_stopped);
 	//Copy any changed registers back to the frame the Xtensa HAL uses.
-	os_memcpy(frame, &gdbstub_savedRegs, 19*4);
+	os_memcpy(frame, &gdbstub_savedRegs, 5*4);
+	os_memcpy(((uint32_t*)frame)+5, &gdbstub_savedRegs.a[2], 14*4);
 }
 #endif
 
-#if GDBSTUB_REDIRECT_CONSOLE_OUTPUT
-//Replacement putchar1 routine. Instead of spitting out the character directly, it will buffer up to
-//OBUFLEN characters (or up to a \n, whichever comes earlier) and send it out as a gdb stdout packet.
-static void ATTR_GDBFN gdb_semihost_putchar1(char c) {
-	int i;
-	obuf[obufpos++]=c;
-	if (c=='\n' || obufpos==OBUFLEN) {
-		gdbPacketStart();
-		gdbPacketChar('O');
-		for (i=0; i<obufpos; i++) gdbPacketHex(obuf[i], 8);
-		gdbPacketEnd();
-		obufpos=0;
-	}
-}
 #endif
 
-#if !GDBSTUB_FREERTOS
-//The OS-less SDK uses the Xtensa HAL to handle exceptions. We can use those functions to catch any
-//fatal exceptions and invoke the debugger when this happens.
-static void ATTR_GDBINIT install_exceptions() {
-	unsigned int i;
-	int exno[]={EXCCAUSE_ILLEGAL, EXCCAUSE_SYSCALL, EXCCAUSE_INSTR_ERROR, EXCCAUSE_LOAD_STORE_ERROR,
-			EXCCAUSE_DIVIDE_BY_ZERO, EXCCAUSE_UNALIGNED, EXCCAUSE_INSTR_DATA_ERROR, EXCCAUSE_LOAD_STORE_DATA_ERROR,
-			EXCCAUSE_INSTR_ADDR_ERROR, EXCCAUSE_LOAD_STORE_ADDR_ERROR, EXCCAUSE_INSTR_PROHIBITED,
-			EXCCAUSE_LOAD_PROHIBITED, EXCCAUSE_STORE_PROHIBITED};
-	for (i=0; i<(sizeof(exno)/sizeof(exno[0])); i++) {
-		_xtos_set_exception_handler(exno[i], gdb_exception_handler);
-	}
+#if GDBSTUB_BREAK_ON_EXCEPTION
+
+#if GDBSTUB_FREERTOS
+//Freertos exception. This routine is called by an assembly routine in gdbstub-entry.S
+void ATTR_GDBFN gdbstub_handle_user_exception() {
+	gdbstub_savedRegs.reason|=0x80; //mark as an exception reason
+	gdbReadCommand(true);
 }
-#else
+
 //FreeRTOS doesn't use the Xtensa HAL for exceptions, but uses its own fatal exception handler.
 //We use a small hack to replace that with a jump to our own handler, which then has the task of
 //decyphering and re-instating the registers the FreeRTOS code left.
@@ -680,56 +796,77 @@ static void ATTR_GDBINIT install_exceptions() {
 	//This mess encodes as a relative jump instruction to user_fatal_exception_handler
 	*ufe=((((int)gdbstub_user_exception_entry-(int)user_fatal_exception_handler)-4)<<6)|6;
 }
-#endif
-
-
-
-#if GDBSTUB_CTRLC_BREAK
-
-#if !GDBSTUB_FREERTOS
-
-static void ATTR_GDBFN uart_hdlr(void *arg, void *frame) {
-	int doDebug=0, fifolen=0;
+#else
+//Non-OS exception handler. Gets called by the Xtensa HAL.
+static void ATTR_GDBFN gdbstub_exception_handler(struct XTensa_exception_frame_s *frame) {
 	//Save the extra registers the Xtensa HAL doesn't save
 	gdbstub_save_extra_sfrs_for_exception();
+	gdbstub_savedRegs.reason|=0x80; //mark as an exception reason
+	*((uint32_t*)UART_INT_ENA(0)) = 0;
+	gdbReadCommandWithFrame(true, (void*)frame);
+}
 
-	fifolen=(READ_PERI_REG(UART_STATUS(0))>>UART_RXFIFO_CNT_S)&UART_RXFIFO_CNT;
-	while (fifolen!=0) {
-		if ((READ_PERI_REG(UART_FIFO(0)) & 0xFF)==0x3) doDebug=1; //Check if any of the chars is control-C. Throw away rest.
-		fifolen--;
+//The OS-less SDK uses the Xtensa HAL to handle exceptions. We can use those functions to catch any
+//fatal exceptions and invoke the debugger when this happens.
+static void ATTR_GDBINIT install_exceptions() {
+	unsigned int i;
+	int exno[]={EXCCAUSE_ILLEGAL, EXCCAUSE_SYSCALL, EXCCAUSE_INSTR_ERROR, EXCCAUSE_LOAD_STORE_ERROR,
+			EXCCAUSE_DIVIDE_BY_ZERO, EXCCAUSE_UNALIGNED, EXCCAUSE_INSTR_DATA_ERROR, EXCCAUSE_LOAD_STORE_DATA_ERROR,
+			EXCCAUSE_INSTR_ADDR_ERROR, EXCCAUSE_LOAD_STORE_ADDR_ERROR, EXCCAUSE_INSTR_PROHIBITED,
+			EXCCAUSE_LOAD_PROHIBITED, EXCCAUSE_STORE_PROHIBITED};
+	for (i=0; i<(sizeof(exno)/sizeof(exno[0])); i++) {
+		_xtos_set_exception_handler(exno[i], gdbstub_exception_handler);
 	}
-	WRITE_PERI_REG(UART_INT_CLR(0), UART_RXFIFO_FULL_INT_CLR|UART_RXFIFO_TOUT_INT_CLR);
+}
+#endif
 
-	if (doDebug) {
-		//Copy registers the Xtensa HAL did save to gdbstub_savedRegs
-		os_memcpy(&gdbstub_savedRegs, frame, 19*4);
-		gdbstub_savedRegs.a1=(uint32_t)frame+EXCEPTION_GDB_SP_OFFSET;
+#endif
 
-		gdbstub_savedRegs.reason=0xff; //mark as user break reason
 
-		ets_wdt_disable();
-		sendReason();
-		while(gdbReadCommand()!=ST_CONT);
-		ets_wdt_enable();
-		//Copy any changed registers back to the frame the Xtensa HAL uses.
-		os_memcpy(frame, &gdbstub_savedRegs, 19*4);
+
+#if GDBSTUB_REDIRECT_CONSOLE_OUTPUT
+//Replacement putchar1 routine. Instead of spitting out the character directly, it will buffer up to
+//OBUFLEN characters (or up to a \n, whichever comes earlier) and send it out as a gdb stdout packet.
+static void ATTR_GDBEXTERNFN gdbstub_semihost_putchar1(char c) {
+	if (!gdb_attached && uart_putc1_callback != NULL) {
+		uart_putc1_callback(c);
+		return;
 	}
+	gdbstub_buffered_write_char(c);
 }
 
-static void ATTR_GDBINIT install_uart_hdlr() {
-	ets_isr_attach(ETS_UART_INUM, uart_hdlr, NULL);
-	SET_PERI_REG_MASK(UART_INT_ENA(0), UART_RXFIFO_FULL_INT_ENA|UART_RXFIFO_TOUT_INT_ENA);
-	ets_isr_unmask((1<<ETS_UART_INUM)); //enable uart interrupt
+void ATTR_GDBINIT gdbstub_set_putc1_callback(void (*func)(char)) {
+	uart_putc1_callback=func;
 }
+#endif
 
+
+
+#if GDBSTUB_FREERTOS
+static void ATTR_GDBINIT configure_uart() {}
 #else
+static void ATTR_GDBINIT configure_uart() {
+    gdbstub_hook_enable_tx_pin_uart0(1);
+    gdbstub_hook_enable_rx_pin_uart0(3);
+	
+	WRITE_PERI_REG(UART_CONF0(0), 0b00011100); //8N1
+
+	SET_PERI_REG_MASK(UART_CONF0(0), UART_RXFIFO_RST | UART_TXFIFO_RST);    //RESET FIFO
+	CLEAR_PERI_REG_MASK(UART_CONF0(0), UART_RXFIFO_RST | UART_TXFIFO_RST);
+}
+#endif
+
+#if GDBSTUB_CTRLC_BREAK
+
+#if GDBSTUB_FREERTOS
 
 void ATTR_GDBFN gdbstub_handle_uart_int(struct XTensa_rtos_int_frame_s *frame) {
 	int doDebug=0, fifolen=0, x;
 
 	fifolen=(READ_PERI_REG(UART_STATUS(0))>>UART_RXFIFO_CNT_S)&UART_RXFIFO_CNT;
 	while (fifolen!=0) {
-		if ((READ_PERI_REG(UART_FIFO(0)) & 0xFF)==0x3) doDebug=1; //Check if any of the chars is control-C. Throw away rest.
+		//Check if any of the chars is control-C. Throw away rest.
+		if ((READ_PERI_REG(UART_FIFO(0)) & 0xFF)==0x3) doDebug=1;
 		fifolen--;
 	}
 	WRITE_PERI_REG(UART_INT_CLR(0), UART_RXFIFO_FULL_INT_CLR|UART_RXFIFO_TOUT_INT_CLR);
@@ -739,32 +876,59 @@ void ATTR_GDBFN gdbstub_handle_uart_int(struct XTensa_rtos_int_frame_s *frame) {
 		gdbstub_savedRegs.pc=frame->pc;
 		gdbstub_savedRegs.ps=frame->ps;
 		gdbstub_savedRegs.sar=frame->sar;
-		gdbstub_savedRegs.a0=frame->a[0];
-		gdbstub_savedRegs.a1=frame->a[1];
-		for (x=2; x<16; x++) gdbstub_savedRegs.a[x-2]=frame->a[x];
+		for (x=0; x<16; x++) gdbstub_savedRegs.a[x]=frame->a[x];
 
 //		gdbstub_savedRegs.a1=(uint32_t)frame+EXCEPTION_GDB_SP_OFFSET;
 
 		gdbstub_savedRegs.reason=0xff; //mark as user break reason
 
-//		ets_wdt_disable();
 		sendReason();
-		while(gdbReadCommand()!=ST_CONT);
-//		ets_wdt_enable();
+		gdbReadCommand(false);
 		//Copy any changed registers back to the frame the Xtensa HAL uses.
 		frame->pc=gdbstub_savedRegs.pc;
 		frame->ps=gdbstub_savedRegs.ps;
 		frame->sar=gdbstub_savedRegs.sar;
-		frame->a[0]=gdbstub_savedRegs.a0;
-		frame->a[1]=gdbstub_savedRegs.a1;
-		for (x=2; x<16; x++) frame->a[x]=gdbstub_savedRegs.a[x-2];
+		for (x=0; x<16; x++) frame->a[x]=gdbstub_savedRegs.a[x];
 	}
 }
 
 static void ATTR_GDBINIT install_uart_hdlr() {
-	_xt_isr_attach(ETS_UART_INUM, gdbstub_uart_entry);
+	os_isr_attach(ETS_UART_INUM, gdbstub_uart_entry);
 	SET_PERI_REG_MASK(UART_INT_ENA(0), UART_RXFIFO_FULL_INT_ENA|UART_RXFIFO_TOUT_INT_ENA);
-	_xt_isr_unmask((1<<ETS_UART_INUM)); //enable uart interrupt
+	ETS_UART_INTR_ENABLE();
+}
+
+#else
+
+static void ATTR_GDBFN gdbstub_uart_hdlr(void* arg, void* frame) {
+	(void) arg;
+	//Save the extra registers the Xtensa HAL doesn't save
+	gdbstub_save_extra_sfrs_for_exception();
+	gdbReadCommandWithFrame(false, frame);
+	WRITE_PERI_REG(UART_INT_CLR(0), UART_RXFIFO_FULL_INT_CLR|UART_RXFIFO_TOUT_INT_CLR);
+}
+
+static void ATTR_GDBINIT install_uart_hdlr() {
+	ETS_UART_INTR_DISABLE();
+	ETS_UART_INTR_ATTACH(gdbstub_uart_hdlr, NULL);
+
+	configure_uart();
+
+	WRITE_PERI_REG(UART_CONF1(0),
+		((100 & UART_RXFIFO_FULL_THRHD) << UART_RXFIFO_FULL_THRHD_S) |
+		((0x02 & UART_RX_TOUT_THRHD) << UART_RX_TOUT_THRHD_S) |
+		UART_RX_TOUT_EN);
+
+	WRITE_PERI_REG(UART_INT_CLR(0), 0xffff);
+	SET_PERI_REG_MASK(UART_INT_ENA(0), UART_RXFIFO_FULL_INT_ENA|UART_RXFIFO_TOUT_INT_ENA);
+	ETS_UART_INTR_ENABLE();
+}
+
+void ATTR_GDBINIT gdbstub_set_uart_isr_callback(void (*func)(void*, uint8_t), void* arg) {
+	ETS_UART_INTR_DISABLE();
+	uart_isr_callback=func;
+	uart_isr_arg=arg;
+	ETS_UART_INTR_ENABLE();
 }
 
 #endif
@@ -776,27 +940,26 @@ static void ATTR_GDBINIT install_uart_hdlr() {
 //gdbstub initialization routine.
 void ATTR_GDBINIT gdbstub_init() {
 #if GDBSTUB_REDIRECT_CONSOLE_OUTPUT
-	os_install_putc1(gdb_semihost_putchar1);
+	os_install_putc1(gdbstub_semihost_putchar1);
 #endif
 #if GDBSTUB_CTRLC_BREAK
 	install_uart_hdlr();
+#else
+	configure_uart();
 #endif
+#if GDBSTUB_BREAK_ON_EXCEPTION
 	install_exceptions();
+#endif
 	gdbstub_init_debug_entry();
 #if GDBSTUB_BREAK_ON_INIT
 	gdbstub_do_break();
 #endif
 }
 
-void ATTR_GDBFN gdbstub_do_break_wrapper() {
-	gdbstub_do_break();
-}
-
-bool ATTR_GDBINIT gdb_present()
+bool ATTR_GDBEXTERNFN gdb_present()
 {
 	return true;
 }
 
-extern void gdb_do_break() __attribute__((alias("gdbstub_do_break_wrapper")));
-extern void gdb_init() __attribute__((alias("gdbstub_init")));
-
+void ATTR_GDBFN gdb_do_break() { gdbstub_do_break(); }
+void ATTR_GDBINIT gdb_init() __attribute__((alias("gdbstub_init")));
diff --git a/libraries/GDBStub/src/internal/gdbstub.h b/libraries/GDBStub/src/internal/gdbstub.h
index 68c0d6cb11..4a4d7a8a78 100644
--- a/libraries/GDBStub/src/internal/gdbstub.h
+++ b/libraries/GDBStub/src/internal/gdbstub.h
@@ -1,12 +1,40 @@
 #ifndef GDBSTUB_H
 #define GDBSTUB_H
 
+#include <stdbool.h>
+#include <stdint.h>
+#include <stddef.h>
+
+#include "gdbstub-cfg.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 void gdbstub_init();
 
+//Indicates whether gdbstub will attach to these or not
+//Useful for other uart libs to avoid conflicts
+bool gdbstub_has_putc1_control();
+bool gdbstub_has_uart_isr_control();
+
+#if GDBSTUB_REDIRECT_CONSOLE_OUTPUT
+void gdbstub_set_putc1_callback(void (*callback)(char));
+#endif
+
+void gdbstub_write_char(char c);
+void gdbstub_write(const char* buf, size_t size);
+void gdbstub_buffered_write_char(char c);
+void gdbstub_flush();
+
+#if GDBSTUB_CTRLC_BREAK && !GDBSTUB_FREERTOS
+void gdbstub_set_uart_isr_callback(void (*callback)(void*, uint8_t), void* arg);
+
+//Override points for enabling tx and rx pins for uart0
+void gdbstub_hook_enable_tx_pin_uart0(uint8_t pin);
+void gdbstub_hook_enable_rx_pin_uart0(uint8_t pin);
+#endif
+
 #ifdef __cplusplus
 }
 #endif