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