gdb/arm-stub.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2006 by Tomasz Malesinski
  11  *
  12  * All files in this archive are subject to the GNU General Public License.
  13  * See the file COPYING in the source tree root for full license agreement.
  14  *
  15  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  16  * KIND, either express or implied.
  17  *
  18  ****************************************************************************/
  19
  20 #include <stdbool.h>
  21 #include <string.h>
  22 #include "ifp_usb_serial.h"
  23 #include "sscanf.h"
  24 #include "pnx0101.h"
  25 #include "gdb_api.h"
  26
  27 #define BUFMAX 1024
  28
  29 #define VEC_UND 1
  30 #define VEC_SWI 2
  31 #define VEC_PABT 3
  32 #define VEC_DABT 4
  33
  34 static char packet_buf[BUFMAX];
  35 static char reply_buf[BUFMAX];
  36
  37 static const char hexchars[] = "0123456789abcdef";
  38 static int gdb_exception_no, gdb_mem_access;
  39 static unsigned char watchdog_enabled;
  40 static unsigned long registers[17];
  41
  42 void gdb_api_breakpoint(void);
  43 static void gdb_api_log(char *msg);
  44
  45 __attribute__((section(".gdbapi"))) struct gdb_api gdb_api =
  46 {
  47     GDB_API_MAGIC,
  48     {gdb_api_breakpoint, gdb_api_log}
  49 };
  50
  51 static void watchdog_enable(int on)
  52 {
  53     (*(volatile unsigned long *)0x80002804) = on;
  54     watchdog_enabled = on;
  55 }
  56
  57 static void watchdog_service(void)
  58 {
  59     if (watchdog_enabled)
  60     {
  61         (*(volatile unsigned long *)0x80002804) = 0;
  62         (*(volatile unsigned long *)0x80002808) = 0;
  63         (*(volatile unsigned long *)0x80002804) = 1;
  64     }
  65 }
  66
  67 static inline bool isxdigit(char c)
  68 {
  69     return ((c >= '0') && (c <= '9'))
  70         || ((c >= 'a') && (c <= 'f')) || ((c >= 'A') && (c <= 'F'));
  71 }
  72
  73 static int hex(char ch) {
  74     if ((ch >= 'a') && (ch <= 'f'))
  75         return ch - 'a' + 10;
  76     if ((ch >= '0') && (ch <= '9'))
  77         return ch - '0';
  78     if ((ch >= 'A') && (ch <= 'F'))
  79         return ch - 'A' + 10;
  80     return -1;
  81 }
  82
  83 static void hex_byte(char *s, int byte) {
  84     s[0] = hexchars[(byte >> 4) & 0xf];
  85     s[1] = hexchars[byte & 0xf];
  86 }
  87
  88 static void hex_word(char *s, unsigned long val) {
  89     int i;
  90     for (i = 0; i < 4; i++)
  91         hex_byte(s + i * 2, (val >> (i * 8)) & 0xff);
  92 }
  93
  94 static void hex_string(char *d, char *s) {
  95     while (*s) {
  96         hex_byte(d, *s++);
  97         d += 2;
  98     }
  99     *d = 0;
 100 }
 101
 102 static int get_hex_byte(char *s) {
 103     return (hex(s[0]) << 4) + hex(s[1]);
 104 }
 105
 106 static unsigned long get_hex_word(char *s) {
 107     int i;
 108     unsigned long r = 0;
 109     for (i = 3; i >= 0; i--)
 110         r = (r << 8) + get_hex_byte(s + i * 2);
 111     return r;
 112 }
 113
 114 static void reply_error(int n, char *reply) {
 115     reply[0] = 'E';
 116     hex_byte(reply + 1, n);
 117     reply[3] = 0;
 118 }
 119
 120 static void reply_signal(int n, char *reply) {
 121     int signal;
 122     reply[0] = 'S';
 123     switch (n)
 124     {
 125         case VEC_UND:
 126             signal = 4;
 127             break;
 128         case VEC_PABT:
 129         case VEC_DABT:
 130             signal = 7;
 131             break;
 132         default:
 133             signal = 5;
 134             break;
 135     }
 136     hex_byte(reply + 1, signal);
 137     reply[3] = 0;
 138 }
 139
 140 static void reply_ok(char *reply) {
 141     strcpy(reply, "OK");
 142 }
 143
 144 static int get_byte(void) {
 145     int b;
 146     while ((b = usb_serial_try_get_byte()) < 0)
 147         watchdog_service();
 148     watchdog_service();
 149     return b;
 150 }
 151
 152 static void put_byte(unsigned char ch) {
 153     while (usb_serial_try_put_byte(ch) < 0)
 154         watchdog_service();
 155     watchdog_service();
 156 }
 157
 158 static void serial_write(unsigned char *buf, int len) {
 159     int i;
 160     for (i = 0; i < len; i++)
 161         put_byte(buf[i]);
 162 }
 163
 164 static void get_packet(char *buf, int len) {
 165     int count, checksum, escaped;
 166     int ch;
 167
 168     while (1) {
 169         do {
 170             ch = get_byte();
 171         } while (ch != '$');
 172
 173         checksum = 0;
 174         count = 0;
 175         escaped = 0;
 176         while (count < len) {
 177             ch = get_byte();
 178             if (!escaped) {
 179                 if (ch == '$') {
 180                     checksum = 0;
 181                     count = 0;
 182                 } else if (ch == '#')
 183                     break;
 184                 else if (ch == 0x7d) {
 185                     escaped = 1;
 186                     checksum += ch;
 187                 } else {
 188                     checksum += ch;
 189                     buf[count] = ch;
 190                     count++;
 191                 }
 192             } else {
 193                 escaped = 0;
 194                 checksum += ch;
 195                 buf[count] = ch ^ 0x20;
 196                 count++;
 197             }
 198         }
 199         buf[count] = 0;
 200
 201         if (ch == '#') {
 202             int rchksum;
 203
 204             ch = get_byte();
 205             rchksum = hex(ch) << 4;
 206             ch = get_byte();
 207             rchksum += hex(ch);
 208
 209             if ((checksum & 0xff) != rchksum)
 210                 put_byte('-');
 211             else {
 212                 put_byte('+');
 213                 return;
 214             }
 215         }
 216     }
 217 }
 218
 219 static void put_packet(char *buf) {
 220     int i, checksum;
 221     int ch;
 222     char tmp[3];
 223
 224     do {
 225         put_byte('$');
 226
 227         checksum = 0;
 228         for (i = 0; buf[i]; i++)
 229             checksum += buf[i];
 230
 231         serial_write(buf, i);
 232
 233         tmp[0] = '#';
 234         hex_byte(tmp + 1, checksum & 0xff);
 235         serial_write(tmp, 3);
 236
 237         ch = get_byte();
 238
 239     } while (ch != '+');
 240 }
 241
 242 static inline unsigned long get_general_reg(int n)
 243 {
 244     return registers[n + 1];
 245 }
 246
 247 static inline void set_general_reg(int n, unsigned long v)
 248 {
 249     registers[n + 1] = v;
 250 }
 251
 252 static inline unsigned long get_cpsr(void)
 253 {
 254     return registers[0];
 255 }
 256
 257 static inline void set_cpsr(unsigned long v)
 258 {
 259     registers[0] = v;
 260 }
 261
 262 static void g_reply(char *buf) {
 263     int i;
 264     char *p;
 265
 266     p = buf;
 267     for (i = 0; i < 16; i++) {
 268         hex_word(p, get_general_reg(i));
 269         p += 8;
 270     }
 271
 272     for (i = 0; i < 8; i++) {
 273         memset(p, '0', 16);
 274         p += 16;
 275     }
 276
 277     hex_word(p, 0);
 278     p += 8;
 279     hex_word(p, get_cpsr());
 280     p[8] = 0;
 281 }
 282
 283 static void cmd_get_register(char *args, char *reply) {
 284     int r;
 285
 286     if (sscanf(args, "%x", &r) != 1) {
 287         reply_error(0, reply);
 288         return;
 289     }
 290
 291     if (r >= 0 && r < 16) {
 292         hex_word(reply, get_general_reg(r));
 293         reply[8] = 0;
 294     } else if (r == 25) {
 295         hex_word(reply, get_cpsr());
 296         reply[8] = 0;
 297     } else {
 298         hex_word(reply, 0);
 299         reply[8] = 0;
 300     }
 301 }
 302
 303 static void cmd_set_register(char *args, char *reply) {
 304   int r, p;
 305   unsigned long v;
 306
 307   p = -1;
 308   sscanf(args, "%x=%n", &r, &p);
 309   if (p == -1) {
 310     reply_error(0, reply);
 311     return;
 312   }
 313
 314   v = get_hex_word(args + p);
 315   if (r >= 0 && r < 16)
 316     set_general_reg(r, v);
 317   else if (r == 25)
 318     set_cpsr(v);
 319   reply_ok(reply);
 320 }
 321
 322 static void cmd_set_registers(char *args, char *reply) {
 323     char *p;
 324     int i, len;
 325
 326     len = strlen(args);
 327
 328     p = args;
 329     for (i = 0; i < 16 && len >= (i + 1) * 8; i++) {
 330         set_general_reg(i, get_hex_word(p));
 331         p += 8;
 332     }
 333
 334     if (len >= 16 * 8 + 8 * 16 + 2 * 8)
 335     {
 336         p += 8 * 16 + 8;
 337         set_cpsr(get_hex_word(p));
 338     }
 339
 340     reply_ok(reply);
 341 }
 342
 343 static void cmd_get_memory(char *args, char *reply) {
 344     unsigned long addr, len, i;
 345
 346     if (sscanf(args, "%lx,%lx", &addr, &len) != 2) {
 347         reply_error(0, reply);
 348         return;
 349     }
 350
 351     if (len > (BUFMAX - 16) / 2) {
 352         reply_error(1, reply);
 353         return;
 354     }
 355
 356     gdb_mem_access = 1;
 357     for (i = 0; i < len; i++)
 358         hex_byte(reply + i * 2, *((unsigned char *)(addr + i)));
 359     gdb_mem_access = 0;
 360
 361     reply[len * 2] = 0;
 362 }
 363
 364 static void cmd_put_memory(char *args, char *reply) {
 365     unsigned long addr, len, i;
 366     int pos;
 367
 368     pos = -1;
 369     sscanf(args, "%lx,%lx:%n", &addr, &len, &pos);
 370     if (pos == -1) {
 371         reply_error(0, reply);
 372         return;
 373     }
 374
 375     gdb_mem_access = 1;
 376     for (i = 0; i < len; i++)
 377         *((unsigned char *)(addr + i)) = get_hex_byte(args + pos + i * 2);
 378     gdb_mem_access = 0;
 379
 380     reply_ok(reply);
 381 }
 382
 383 static void cmd_put_memory_binary(char *args, char *reply) {
 384     unsigned long addr, len, i;
 385     int pos;
 386
 387     pos = -1;
 388     sscanf(args, "%lx,%lx:%n", &addr, &len, &pos);
 389     if (pos == -1) {
 390         reply_error(0, reply);
 391         return;
 392     }
 393
 394     gdb_mem_access = 1;
 395     for (i = 0; i < len; i++)
 396         *((unsigned char *)(addr + i)) = args[pos + i];
 397     gdb_mem_access = 0;
 398
 399     reply_ok(reply);
 400 }
 401
 402 static void parse_continue_args(char *args) {
 403     int sig;
 404     unsigned long addr;
 405
 406     if (sscanf(args, "%x;%lx", &sig, &addr) == 2) {
 407         set_general_reg(15, addr);
 408     } else if (sscanf(args, "%lx", &addr) == 1) {
 409         set_general_reg(15, addr);
 410     }
 411 }
 412
 413 static void cmd_go(char *args) {
 414     parse_continue_args(args);
 415
 416     asm volatile(
 417         "    mov    r1, %0\n"
 418         "    ldr    r12, [r1], #4\n"
 419         "    mov    r0, r12\n"
 420         "    and    r0, r0, #0x1f\n"
 421         "    cmp    r0, #0x10\n"
 422         "    bne    1f\n"
 423         "    ldr    r14, [r1, #60]\n"
 424         "    msr    spsr_fsxc, r12\n"
 425         "    ldmia  r1, {r0-r14}^\n"
 426         "    movs   r15, r14\n"
 427         "1:\n"
 428         "    msr    cpsr_fsxc, r12\n"
 429         "    ldmia  r1, {r0-r15}\n"
 430         : : "r" (registers));
 431 }
 432
 433 static void remote_cmd(char *cmd, char *reply) {
 434     int i, err;
 435     i = 0;
 436     err = 0;
 437     while ((cmd[i] >= 'a' && cmd[i] <= 'z') || cmd[i] == '_')
 438         i++;
 439     if (!strncmp(cmd, "reboot", i))
 440     {
 441         reply_ok(reply);
 442         put_packet(reply);
 443         watchdog_enable(1);
 444         (*(volatile unsigned long *)0x80002804) = 1;
 445         while (1);
 446     }
 447     else if (!strncmp(cmd, "power_off", i))
 448     {
 449         reply_ok(reply);
 450         put_packet(reply);
 451         GPIO1_CLR = 1 << 16;
 452         GPIO2_SET = 1;
 453         while (1);
 454     }
 455     else if (!strncmp(cmd, "watchdog", i))
 456     {
 457         int t;
 458         if (sscanf(cmd + i, "%d", &t) == 1)
 459             watchdog_enable(t != 0);
 460         else
 461             err = 1;
 462         reply_ok(reply);
 463     }
 464     else
 465         hex_string(reply, "Unrecognized command\n");
 466     if (err)
 467         reply_error(err, reply);
 468 }
 469
 470 static void cmd_query(char *args, char *reply) {
 471     if (!strncmp(args, "Rcmd,", 5)) {
 472         unsigned i = 0;
 473         char *s = args + 5;
 474         char cmd[200];
 475         while (isxdigit(s[0]) && isxdigit(s[1]) && i < sizeof(cmd) - 1) {
 476             cmd[i++] = get_hex_byte(s);
 477             s += 2;
 478         }
 479         cmd[i] = 0;
 480         remote_cmd(cmd, reply);
 481     } else
 482         reply[0] = 0;
 483 }
 484
 485 void gdb_loop(void) {
 486     int no_reply;
 487
 488     gdb_mem_access = 0;
 489
 490     while (1) {
 491         get_packet(packet_buf, sizeof(packet_buf) - 1);
 492
 493         no_reply = 0;
 494         switch (packet_buf[0]) {
 495             case '?':
 496                 reply_signal(gdb_exception_no, reply_buf);
 497                 break;
 498
 499             case 'p':
 500                 cmd_get_register(packet_buf + 1, reply_buf);
 501                 break;
 502
 503             case 'P':
 504                 cmd_set_register(packet_buf + 1, reply_buf);
 505                 break;
 506
 507             case 'g':
 508                 g_reply(reply_buf);
 509                 break;
 510
 511             case 'G':
 512                 cmd_set_registers(packet_buf + 1, reply_buf);
 513                 break;
 514
 515             case 'm':
 516                 cmd_get_memory(packet_buf + 1, reply_buf);
 517                 break;
 518
 519             case 'M':
 520                 cmd_put_memory(packet_buf + 1, reply_buf);
 521                 break;
 522
 523             case 'X':
 524                 cmd_put_memory_binary(packet_buf + 1, reply_buf);
 525                 break;
 526
 527             case 'q':
 528                 cmd_query(packet_buf + 1, reply_buf);
 529                 break;
 530
 531             case 'c':
 532                 cmd_go(packet_buf + 1);
 533                 reply_error(1, reply_buf);
 534                 break;
 535
 536 /*             case 's': */
 537 /*                 cmd_go(packet_buf + 1); */
 538 /*                 break; */
 539
 540             default:
 541                 reply_buf[0] = 0;
 542         }
 543
 544         if (!no_reply)
 545             put_packet(reply_buf);
 546     }
 547 }
 548
 549 extern void *vectors[];
 550
 551 static void gdb_set_vector(int n, void *p)
 552 {
 553     vectors[n] = p;
 554 }
 555
 556 void gdb_und_exc(void);
 557 void gdb_swi_exc(void);
 558 void gdb_pabt_exc(void);
 559 void gdb_dabt_exc(void);
 560
 561 static void gdb_set_vectors(void)
 562 {
 563     gdb_set_vector(VEC_UND, gdb_und_exc);
 564     gdb_set_vector(VEC_SWI, gdb_swi_exc);
 565     gdb_set_vector(VEC_PABT, gdb_pabt_exc);
 566     gdb_set_vector(VEC_DABT, gdb_dabt_exc);
 567 }
 568
 569 void gdb_loop_from_exc(void)
 570 {
 571     if (gdb_mem_access)
 572         reply_error(1, reply_buf);
 573     else
 574         reply_signal(gdb_exception_no, reply_buf);
 575     put_packet(reply_buf);
 576     gdb_loop();
 577 }
 578
 579 #define IRQ_REG(reg) (*(volatile unsigned long *)(0x80300000 + (reg)))
 580
 581 static inline unsigned long irq_read(int reg)
 582 {
 583     unsigned long v, v2;
 584     do
 585     {
 586         v = IRQ_REG(reg);
 587         v2 = IRQ_REG(reg);
 588     } while (v != v2);
 589     return v;
 590 }
 591
 592 #define IRQ_WRITE_WAIT(reg, val, cond) \
 593     do { unsigned long v, v2; \
 594         do { \
 595             IRQ_REG(reg) = (val); \
 596             v = IRQ_REG(reg); \
 597             v2 = IRQ_REG(reg); \
 598         } while ((v != v2) || !(cond)); \
 599     } while (0);
 600
 601 void fiq(void)
 602 {
 603 }
 604
 605 static void system_init(void)
 606 {
 607     int i;
 608
 609     watchdog_enable(0);
 610
 611     for (i = 0; i < 0x1c; i++)
 612     {
 613         IRQ_WRITE_WAIT(0x404 + i * 4, 0x1e000001, (v & 0x3010f) == 1);
 614         IRQ_WRITE_WAIT(0x404 + i * 4, 0x4000000, (v & 0x10000) == 0);
 615         IRQ_WRITE_WAIT(0x404 + i * 4, 0x10000001, (v & 0xf) == 1);
 616     }
 617
 618     GPIO3_CLR = 1;
 619 }
 620
 621 static void gdb_api_log(char *msg)
 622 {
 623     int i;
 624
 625     reply_buf[0] = 'O';
 626     i = 1;
 627     while (*msg && i + 2 <= BUFMAX - 1)
 628     {
 629         hex_byte(reply_buf + i, *msg++);
 630         i += 2;
 631     }
 632     reply_buf[i] = 0;
 633     put_packet(reply_buf);
 634 }
 635
 636 void main(void)
 637 {
 638     system_init();
 639     usb_serial_init();
 640     gdb_mem_access = 0;
 641     gdb_set_vectors();
 642     gdb_api_breakpoint();
 643     while (1);
 644 }
 645
 646 #define str(s) #s
 647 #define xstr(s) str(s)
 648
 649 asm (".text\n"
 650      "gdb_und_exc:\n"
 651      "    ldr    sp, =_stub_stack\n"
 652      "    sub    r14, r14, #4\n"
 653      "    stmfd  sp!, {r0-r3, r12, r14}\n"
 654      "    mov    r0, #" xstr(VEC_UND) "\n"
 655      "    b      gdb_handle_exception\n"
 656      "gdb_swi_exc:\n"
 657      "    ldr    sp, =_stub_stack\n"
 658      "    stmfd  sp!, {r0-r3, r12, r14}\n"
 659      "    mov    r0, #" xstr(VEC_SWI) "\n"
 660      "    b      gdb_handle_exception\n"
 661      "gdb_pabt_exc:\n"
 662      "    ldr    sp, =_stub_stack\n"
 663      "    stmfd  sp!, {r0-r3, r12, r14}\n"
 664      "    mov    r0, #" xstr(VEC_PABT) "\n"
 665      "    b      gdb_handle_exception\n"
 666      "gdb_dabt_exc:\n"
 667      "    ldr    sp, =_stub_stack\n"
 668      "    sub    r14, r14, #4\n"
 669      "    stmfd  sp!, {r0-r3, r12, r14}\n"
 670      "    ldr    r0, =gdb_mem_access\n"
 671      "    ldr    r0, [r0]\n"
 672      "    tst    r0, r0\n"
 673      "    bne    gdb_data_abort\n"
 674      "    mov    r0, #" xstr(VEC_DABT) "\n"
 675      "    b      gdb_handle_exception\n"
 676      "gdb_handle_exception:\n"
 677      "    ldr    r1, =gdb_exception_no\n"
 678      "    str    r0, [r1]\n"
 679      "    ldr    r0, =registers\n"
 680      "    mrs    r12, spsr\n"
 681      "    str    r12, [r0], #4\n"
 682      "    ldmfd  sp!, {r2, r3}\n"
 683      "    stmia  r0!, {r2, r3}\n"
 684      "    ldmfd  sp!, {r2, r3, r12, r14}\n"
 685      "    str    r14, [r0, #52]\n"
 686      "    stmia  r0!, {r2-r12}\n"
 687      "    mrs    r1, spsr\n"
 688      "    and    r2, r1, #0x1f\n"
 689      "    cmp    r2, #0x10\n"
 690      "    bne    1f\n"
 691      "    stmia  r0, {r13, r14}^\n"
 692      "    b      gdb_data_abort\n"
 693      "1:\n"
 694      "    msr    cpsr_c, r1\n"
 695      "    stmia  r0, {r13, r14}\n"
 696      "gdb_data_abort:\n"
 697      "    msr    cpsr_c, #0xd3\n"
 698      "    ldr    sp, =_stub_stack\n"
 699      "    b      gdb_loop_from_exc\n"
 700      "gdb_api_breakpoint:\n"
 701      "    stmfd  sp!, {r0-r1}\n"
 702      "    ldr    r0, =registers\n"
 703      "    mrs    r1, cpsr\n"
 704      "    str    r1, [r0], #4\n"
 705      "    ldmfd  sp!, {r1}\n"
 706      "    str    r1, [r0], #4\n"
 707      "    ldmfd  sp!, {r1}\n"
 708      "    str    r1, [r0], #4\n"
 709      "    stmia  r0!, {r2-r14}\n"
 710      "    str    r14, [r0]\n"
 711      "    msr    cpsr_c, #0xd3\n"
 712      "    ldr    sp, =_stub_stack\n"
 713      "    ldr    r0, =gdb_exception_no\n"
 714      "    mov    r1, #5\n"
 715      "    str    r1, [r0]\n"
 716      "    b      gdb_loop_from_exc\n");