1 files changed, 168 insertions, 26 deletions

diff --git a/gdb/arm-stub.c b/gdb/arm-stub.c
index 68b24a7457..0717af49c3 100644
--- a/gdb/arm-stub.c
+++ b/gdb/arm-stub.c
@@ -21,6 +21,8 @@
 #include <string.h>
 #include "usb_serial.h"
 #include "sscanf.h"
+#include "pnx0101.h"
+#include "gdb_api.h"
 
 #define BUFMAX 1024
 
@@ -34,8 +36,34 @@ static char reply_buf[BUFMAX];
 
 static const char hexchars[] = "0123456789abcdef";
 static int gdb_exception_no, gdb_mem_access;
+static unsigned char watchdog_enabled;
 static unsigned long registers[17];
 
+void gdb_api_breakpoint(void);
+static void gdb_api_log(char *msg);
+
+__attribute__((section(".gdbapi"))) struct gdb_api gdb_api =
+{
+    GDB_API_MAGIC,
+    {gdb_api_breakpoint, gdb_api_log}
+};
+
+static void watchdog_enable(int on)
+{
+    (*(volatile unsigned long *)0x80002804) = on;
+    watchdog_enabled = on;
+}
+
+static void watchdog_service(void)
+{
+    if (watchdog_enabled)
+    {
+        (*(volatile unsigned long *)0x80002804) = 0;
+        (*(volatile unsigned long *)0x80002808) = 0;
+        (*(volatile unsigned long *)0x80002804) = 1;
+    }        
+}
+
 static inline bool isxdigit(char c)
 {
     return ((c >= '0') && (c <= '9'))
@@ -113,33 +141,58 @@ static void reply_ok(char *reply) {
     strcpy(reply, "OK");
 }
 
+static int get_byte(void) {
+    int b;
+    while ((b = usb_serial_try_get_byte()) < 0)
+        watchdog_service();
+    watchdog_service();
+    return b;
+}
+
+static void put_byte(unsigned char ch) {
+    while (usb_serial_try_put_byte(ch) < 0)
+        watchdog_service();
+    watchdog_service();
+}
+
 static void serial_write(unsigned char *buf, int len) {
     int i;
     for (i = 0; i < len; i++)
-        usb_serial_put_byte(buf[i]);
+        put_byte(buf[i]);
 }
 
-static void  get_packet(char *buf, int len) {
-    int count, checksum;
+static void get_packet(char *buf, int len) {
+    int count, checksum, escaped;
     int ch;
 
     while (1) {
         do {
-            ch = usb_serial_get_byte();
+            ch = get_byte();
         } while (ch != '$');
 
         checksum = 0;
         count = 0;
+        escaped = 0;
         while (count < len) {
-            ch = usb_serial_get_byte();
-            if (ch == '$') {
-                checksum = 0;
-                count = 0;
-            } else if (ch == '#')
-                break;
-            else {
+            ch = get_byte();
+            if (!escaped) {
+                if (ch == '$') {
+                    checksum = 0;
+                    count = 0;
+                } else if (ch == '#')
+                    break;
+                else if (ch == 0x7d) {
+                    escaped = 1;
+                    checksum += ch;
+                } else {
+                    checksum += ch;
+                    buf[count] = ch;
+                    count++;
+                }
+            } else {
+                escaped = 0;
                 checksum += ch;
-                buf[count] = ch;
+                buf[count] = ch ^ 0x20;
                 count++;
             }
         }
@@ -148,15 +201,15 @@ static void  get_packet(char *buf, int len) {
         if (ch == '#') {
             int rchksum;
 
-            ch = usb_serial_get_byte();
+            ch = get_byte();
             rchksum = hex(ch) << 4;
-            ch = usb_serial_get_byte();
+            ch = get_byte();
             rchksum += hex(ch);
 
             if ((checksum & 0xff) != rchksum)
-                usb_serial_put_byte('-');
+                put_byte('-');
             else {
-                usb_serial_put_byte('+');
+                put_byte('+');
                 return;
             }
         }
@@ -169,7 +222,7 @@ static void put_packet(char *buf) {
     char tmp[3];
 
     do {
-        usb_serial_put_byte('$');
+        put_byte('$');
 
         checksum = 0;
         for (i = 0; buf[i]; i++)
@@ -181,7 +234,7 @@ static void put_packet(char *buf) {
         hex_byte(tmp + 1, checksum & 0xff);
         serial_write(tmp, 3);
 
-        ch = usb_serial_get_byte();
+        ch = get_byte();
 
     } while (ch != '+');
 }
@@ -327,6 +380,25 @@ static void cmd_put_memory(char *args, char *reply) {
     reply_ok(reply);
 }
 
+static void cmd_put_memory_binary(char *args, char *reply) {
+    unsigned long addr, len, i;
+    int pos;
+
+    pos = -1;
+    sscanf(args, "%lx,%lx:%n", &addr, &len, &pos);
+    if (pos == -1) {
+        reply_error(0, reply);
+        return;
+    }
+
+    gdb_mem_access = 1;
+    for (i = 0; i < len; i++)
+        *((unsigned char *)(addr + i)) = args[pos + i];
+    gdb_mem_access = 0;
+
+    reply_ok(reply);
+}
+
 static void parse_continue_args(char *args) {
     int sig;
     unsigned long addr;
@@ -359,8 +431,40 @@ static void cmd_go(char *args) {
 }
 
 static void remote_cmd(char *cmd, char *reply) {
-    (void)cmd;
-    hex_string(reply, "Unrecognized command\n");
+    int i, err;
+    i = 0;
+    err = 0;
+    while ((cmd[i] >= 'a' && cmd[i] <= 'z') || cmd[i] == '_')
+        i++;
+    if (!strncmp(cmd, "reboot", i))
+    {
+        reply_ok(reply);
+        put_packet(reply);
+        watchdog_enable(1);
+        (*(volatile unsigned long *)0x80002804) = 1;
+        while (1);
+    }
+    else if (!strncmp(cmd, "power_off", i))
+    {
+        reply_ok(reply);
+        put_packet(reply);
+        GPIO1_CLR = 1 << 16;
+        GPIO2_SET = 1;
+        while (1);
+    }
+    else if (!strncmp(cmd, "watchdog", i))
+    {
+        int t;
+        if (sscanf(cmd + i, "%d", &t) == 1)
+            watchdog_enable(t != 0);
+        else
+            err = 1;
+        reply_ok(reply);
+    }
+    else
+        hex_string(reply, "Unrecognized command\n");
+    if (err)
+        reply_error(err, reply);
 }
 
 static void cmd_query(char *args, char *reply) {
@@ -416,6 +520,10 @@ void gdb_loop(void) {
                 cmd_put_memory(packet_buf + 1, reply_buf);
                 break;
 
+            case 'X':
+                cmd_put_memory_binary(packet_buf + 1, reply_buf);
+                break;
+
             case 'q':
                 cmd_query(packet_buf + 1, reply_buf);
                 break;
@@ -468,8 +576,6 @@ void gdb_loop_from_exc(void)
     gdb_loop();
 }
 
-#define GPIO3_CLR  (*(volatile unsigned long *)0x800030d8)
-
 #define IRQ_REG(reg) (*(volatile unsigned long *)(0x80300000 + (reg)))
 
 static inline unsigned long irq_read(int reg)
@@ -492,12 +598,15 @@ static inline unsigned long irq_read(int reg)
         } while ((v != v2) || !(cond)); \
     } while (0);
 
+void fiq(void)
+{
+}
+
 static void system_init(void)
 {
     int i;
 
-    /* turn off watchdog */
-    (*(volatile unsigned long *)0x80002804) = 0;
+    watchdog_enable(0);
 
     for (i = 0; i < 0x1c; i++)
     {
@@ -509,13 +618,29 @@ static void system_init(void)
     GPIO3_CLR = 1;
 }
 
+static void gdb_api_log(char *msg)
+{
+    int i;
+
+    reply_buf[0] = 'O';
+    i = 1;
+    while (*msg && i + 2 <= BUFMAX - 1)
+    {
+        hex_byte(reply_buf + i, *msg++);
+        i += 2;
+    }
+    reply_buf[i] = 0;
+    put_packet(reply_buf);
+}
+
 void main(void)
 {
     system_init();
     usb_serial_init();
-    gdb_exception_no = VEC_SWI;
+    gdb_mem_access = 0;
     gdb_set_vectors();
-    gdb_loop();
+    gdb_api_breakpoint();
+    while (1);
 }
 
 #define str(s) #s
@@ -571,4 +696,21 @@ asm (".text\n"
      "gdb_data_abort:\n"
      "    msr    cpsr_c, #0xd3\n"
      "    ldr    sp, =_stub_stack\n"
+     "    b      gdb_loop_from_exc\n"
+     "gdb_api_breakpoint:\n"
+     "    stmfd  sp!, {r0-r1}\n"
+     "    ldr    r0, =registers\n"
+     "    mrs    r1, cpsr\n"
+     "    str    r1, [r0], #4\n"
+     "    ldmfd  sp!, {r1}\n"
+     "    str    r1, [r0], #4\n"
+     "    ldmfd  sp!, {r1}\n"
+     "    str    r1, [r0], #4\n"
+     "    stmia  r0!, {r2-r14}\n"
+     "    str    r14, [r0]\n"
+     "    msr    cpsr_c, #0xd3\n"
+     "    ldr    sp, =_stub_stack\n"
+     "    ldr    r0, =gdb_exception_no\n"
+     "    mov    r1, #5\n"
+     "    str    r1, [r0]\n"
      "    b      gdb_loop_from_exc\n");