arch/sh/kernel/kgdb_stub.c

   1 /*
   2  * May be copied or modified under the terms of the GNU General Public
   3  * License.  See linux/COPYING for more information.
   4  *
   5  * Contains extracts from code by Glenn Engel, Jim Kingdon,
   6  * David Grothe <dave@gcom.com>, Tigran Aivazian <tigran@sco.com>,
   7  * Amit S. Kale <akale@veritas.com>,  William Gatliff <bgat@open-widgets.com>,
   8  * Ben Lee, Steve Chamberlain and Benoit Miller <fulg@iname.com>.
   9  *
  10  * This version by Henry Bell <henry.bell@st.com>
  11  * Minor modifications by Jeremy Siegel <jsiegel@mvista.com>
  12  *
  13  * Contains low-level support for remote debug using GDB.
  14  *
  15  * To enable debugger support, two things need to happen. A call to
  16  * set_debug_traps() is necessary in order to allow any breakpoints
  17  * or error conditions to be properly intercepted and reported to gdb.
  18  * A breakpoint also needs to be generated to begin communication.  This
  19  * is most easily accomplished by a call to breakpoint() which does
  20  * a trapa if the initialisation phase has been successfully completed.
  21  *
  22  * In this case, set_debug_traps() is not used to "take over" exceptions;
  23  * other kernel code is modified instead to enter the kgdb functions here
  24  * when appropriate (see entry.S for breakpoint traps and NMI interrupts,
  25  * see traps.c for kernel error exceptions).
  26  *
  27  * The following gdb commands are supported:
  28  *
  29  *    Command       Function                               Return value
  30  *
  31  *    g             return the value of the CPU registers  hex data or ENN
  32  *    G             set the value of the CPU registers     OK or ENN
  33  *
  34  *    mAA..AA,LLLL  Read LLLL bytes at address AA..AA      hex data or ENN
  35  *    MAA..AA,LLLL: Write LLLL bytes at address AA.AA      OK or ENN
  36  *    XAA..AA,LLLL: Same, but data is binary (not hex)     OK or ENN
  37  *
  38  *    c             Resume at current address              SNN   ( signal NN)
  39  *    cAA..AA       Continue at address AA..AA             SNN
  40  *    CNN;          Resume at current address with signal  SNN
  41  *    CNN;AA..AA    Resume at address AA..AA with signal   SNN
  42  *
  43  *    s             Step one instruction                   SNN
  44  *    sAA..AA       Step one instruction from AA..AA       SNN
  45  *    SNN;          Step one instruction with signal       SNN
  46  *    SNNAA..AA     Step one instruction from AA..AA w/NN  SNN
  47  *
  48  *    k             kill (Detach GDB)
  49  *
  50  *    d             Toggle debug flag
  51  *    D             Detach GDB
  52  *
  53  *    Hct           Set thread t for operations,           OK or ENN
  54  *                  c = 'c' (step, cont), c = 'g' (other
  55  *                  operations)
  56  *
  57  *    qC            Query current thread ID                QCpid
  58  *    qfThreadInfo  Get list of current threads (first)    m<id>
  59  *    qsThreadInfo   "    "  "     "      "   (subsequent)
  60  *    qOffsets      Get section offsets                  Text=x;Data=y;Bss=z
  61  *
  62  *    TXX           Find if thread XX is alive             OK or ENN
  63  *    ?             What was the last sigval ?             SNN   (signal NN)
  64  *    O             Output to GDB console
  65  *
  66  * Remote communication protocol.
  67  *
  68  *    A debug packet whose contents are <data> is encapsulated for
  69  *    transmission in the form:
  70  *
  71  *       $ <data> # CSUM1 CSUM2
  72  *
  73  *       <data> must be ASCII alphanumeric and cannot include characters
  74  *       '$' or '#'.  If <data> starts with two characters followed by
  75  *       ':', then the existing stubs interpret this as a sequence number.
  76  *
  77  *       CSUM1 and CSUM2 are ascii hex representation of an 8-bit
  78  *       checksum of <data>, the most significant nibble is sent first.
  79  *       the hex digits 0-9,a-f are used.
  80  *
  81  *    Receiver responds with:
  82  *
  83  *       +       - if CSUM is correct and ready for next packet
  84  *       -       - if CSUM is incorrect
  85  *
  86  * Responses can be run-length encoded to save space.  A '*' means that
  87  * the next character is an ASCII encoding giving a repeat count which
  88  * stands for that many repetitions of the character preceding the '*'.
  89  * The encoding is n+29, yielding a printable character where n >=3
  90  * (which is where RLE starts to win).  Don't use an n > 126.
  91  *
  92  * So "0* " means the same as "0000".
  93  */
  94
  95 #include <linux/string.h>
  96 #include <linux/kernel.h>
  97 #include <linux/sched.h>
  98 #include <linux/smp.h>
  99 #include <linux/spinlock.h>
 100 #include <linux/delay.h>
 101 #include <linux/linkage.h>
 102 #include <linux/init.h>
 103 #include <linux/console.h>
 104 #include <linux/sysrq.h>
 105 #include <linux/module.h>
 106 #include <asm/system.h>
 107 #include <asm/cacheflush.h>
 108 #include <asm/current.h>
 109 #include <asm/signal.h>
 110 #include <asm/pgtable.h>
 111 #include <asm/ptrace.h>
 112 #include <asm/kgdb.h>
 113 #include <asm/io.h>
 114
 115 /* Function pointers for linkage */
 116 kgdb_debug_hook_t *kgdb_debug_hook;
 117 kgdb_bus_error_hook_t *kgdb_bus_err_hook;
 118
 119 int (*kgdb_getchar)(void);
 120 EXPORT_SYMBOL_GPL(kgdb_getchar);
 121 void (*kgdb_putchar)(int);
 122 EXPORT_SYMBOL_GPL(kgdb_putchar);
 123
 124 static void put_debug_char(int c)
 125 {
 126         if (!kgdb_putchar)
 127                 return;
 128         (*kgdb_putchar)(c);
 129 }
 130 static int get_debug_char(void)
 131 {
 132         if (!kgdb_getchar)
 133                 return -1;
 134         return (*kgdb_getchar)();
 135 }
 136
 137 /* Num chars in in/out bound buffers, register packets need NUMREGBYTES * 2 */
 138 #define BUFMAX 1024
 139 #define NUMREGBYTES (MAXREG*4)
 140 #define OUTBUFMAX (NUMREGBYTES*2+512)
 141
 142 enum {
 143         R0 = 0, R1,  R2,  R3,   R4,   R5,  R6, R7,
 144         R8, R9, R10, R11, R12,  R13,  R14, R15,
 145         PC, PR, GBR, VBR, MACH, MACL, SR,
 146         /*  */
 147         MAXREG
 148 };
 149
 150 static unsigned int registers[MAXREG];
 151 struct kgdb_regs trap_registers;
 152
 153 char kgdb_in_gdb_mode;
 154 char in_nmi;                    /* Set during NMI to prevent reentry */
 155 int kgdb_nofault;               /* Boolean to ignore bus errs (i.e. in GDB) */
 156
 157 /* Default values for SCI (can override via kernel args in setup.c) */
 158 #ifndef CONFIG_KGDB_DEFPORT
 159 #define CONFIG_KGDB_DEFPORT 1
 160 #endif
 161
 162 #ifndef CONFIG_KGDB_DEFBAUD
 163 #define CONFIG_KGDB_DEFBAUD 115200
 164 #endif
 165
 166 #if defined(CONFIG_KGDB_DEFPARITY_E)
 167 #define CONFIG_KGDB_DEFPARITY 'E'
 168 #elif defined(CONFIG_KGDB_DEFPARITY_O)
 169 #define CONFIG_KGDB_DEFPARITY 'O'
 170 #else /* CONFIG_KGDB_DEFPARITY_N */
 171 #define CONFIG_KGDB_DEFPARITY 'N'
 172 #endif
 173
 174 #ifdef CONFIG_KGDB_DEFBITS_7
 175 #define CONFIG_KGDB_DEFBITS '7'
 176 #else /* CONFIG_KGDB_DEFBITS_8 */
 177 #define CONFIG_KGDB_DEFBITS '8'
 178 #endif
 179
 180 /* SCI/UART settings, used in kgdb_console_setup() */
 181 int  kgdb_portnum = CONFIG_KGDB_DEFPORT;
 182 EXPORT_SYMBOL_GPL(kgdb_portnum);
 183 int  kgdb_baud = CONFIG_KGDB_DEFBAUD;
 184 EXPORT_SYMBOL_GPL(kgdb_baud);
 185 char kgdb_parity = CONFIG_KGDB_DEFPARITY;
 186 EXPORT_SYMBOL_GPL(kgdb_parity);
 187 char kgdb_bits = CONFIG_KGDB_DEFBITS;
 188 EXPORT_SYMBOL_GPL(kgdb_bits);
 189
 190 /* Jump buffer for setjmp/longjmp */
 191 static jmp_buf rem_com_env;
 192
 193 /* TRA differs sh3/4 */
 194 #if defined(CONFIG_CPU_SH3)
 195 #define TRA 0xffffffd0
 196 #elif defined(CONFIG_CPU_SH4)
 197 #define TRA 0xff000020
 198 #endif
 199
 200 /* Macros for single step instruction identification */
 201 #define OPCODE_BT(op)         (((op) & 0xff00) == 0x8900)
 202 #define OPCODE_BF(op)         (((op) & 0xff00) == 0x8b00)
 203 #define OPCODE_BTF_DISP(op)   (((op) & 0x80) ? (((op) | 0xffffff80) << 1) : \
 204                               (((op) & 0x7f ) << 1))
 205 #define OPCODE_BFS(op)        (((op) & 0xff00) == 0x8f00)
 206 #define OPCODE_BTS(op)        (((op) & 0xff00) == 0x8d00)
 207 #define OPCODE_BRA(op)        (((op) & 0xf000) == 0xa000)
 208 #define OPCODE_BRA_DISP(op)   (((op) & 0x800) ? (((op) | 0xfffff800) << 1) : \
 209                               (((op) & 0x7ff) << 1))
 210 #define OPCODE_BRAF(op)       (((op) & 0xf0ff) == 0x0023)
 211 #define OPCODE_BRAF_REG(op)   (((op) & 0x0f00) >> 8)
 212 #define OPCODE_BSR(op)        (((op) & 0xf000) == 0xb000)
 213 #define OPCODE_BSR_DISP(op)   (((op) & 0x800) ? (((op) | 0xfffff800) << 1) : \
 214                               (((op) & 0x7ff) << 1))
 215 #define OPCODE_BSRF(op)       (((op) & 0xf0ff) == 0x0003)
 216 #define OPCODE_BSRF_REG(op)   (((op) >> 8) & 0xf)
 217 #define OPCODE_JMP(op)        (((op) & 0xf0ff) == 0x402b)
 218 #define OPCODE_JMP_REG(op)    (((op) >> 8) & 0xf)
 219 #define OPCODE_JSR(op)        (((op) & 0xf0ff) == 0x400b)
 220 #define OPCODE_JSR_REG(op)    (((op) >> 8) & 0xf)
 221 #define OPCODE_RTS(op)        ((op) == 0xb)
 222 #define OPCODE_RTE(op)        ((op) == 0x2b)
 223
 224 #define SR_T_BIT_MASK           0x1
 225 #define STEP_OPCODE             0xc320
 226 #define BIOS_CALL_TRAP          0x3f
 227
 228 /* Exception codes as per SH-4 core manual */
 229 #define ADDRESS_ERROR_LOAD_VEC   7
 230 #define ADDRESS_ERROR_STORE_VEC  8
 231 #define TRAP_VEC                 11
 232 #define INVALID_INSN_VEC         12
 233 #define INVALID_SLOT_VEC         13
 234 #define NMI_VEC                  14
 235 #define USER_BREAK_VEC           15
 236 #define SERIAL_BREAK_VEC         58
 237
 238 /* Misc static */
 239 static int stepped_address;
 240 static short stepped_opcode;
 241 static char in_buffer[BUFMAX];
 242 static char out_buffer[OUTBUFMAX];
 243
 244 static void kgdb_to_gdb(const char *s);
 245
 246 /* Convert ch to hex */
 247 static int hex(const char ch)
 248 {
 249         if ((ch >= 'a') && (ch <= 'f'))
 250                 return (ch - 'a' + 10);
 251         if ((ch >= '0') && (ch <= '9'))
 252                 return (ch - '0');
 253         if ((ch >= 'A') && (ch <= 'F'))
 254                 return (ch - 'A' + 10);
 255         return (-1);
 256 }
 257
 258 /* Convert the memory pointed to by mem into hex, placing result in buf.
 259    Returns a pointer to the last char put in buf (null) */
 260 static char *mem_to_hex(const char *mem, char *buf, const int count)
 261 {
 262         int i;
 263         int ch;
 264         unsigned short s_val;
 265         unsigned long l_val;
 266
 267         /* Check for 16 or 32 */
 268         if (count == 2 && ((long) mem & 1) == 0) {
 269                 s_val = *(unsigned short *) mem;
 270                 mem = (char *) &s_val;
 271         } else if (count == 4 && ((long) mem & 3) == 0) {
 272                 l_val = *(unsigned long *) mem;
 273                 mem = (char *) &l_val;
 274         }
 275         for (i = 0; i < count; i++) {
 276                 ch = *mem++;
 277                 buf = pack_hex_byte(buf, ch);
 278         }
 279         *buf = 0;
 280         return (buf);
 281 }
 282
 283 /* Convert the hex array pointed to by buf into binary, to be placed in mem.
 284    Return a pointer to the character after the last byte written */
 285 static char *hex_to_mem(const char *buf, char *mem, const int count)
 286 {
 287         int i;
 288         unsigned char ch;
 289
 290         for (i = 0; i < count; i++) {
 291                 ch = hex(*buf++) << 4;
 292                 ch = ch + hex(*buf++);
 293                 *mem++ = ch;
 294         }
 295         return (mem);
 296 }
 297
 298 /* While finding valid hex chars, convert to an integer, then return it */
 299 static int hex_to_int(char **ptr, int *int_value)
 300 {
 301         int num_chars = 0;
 302         int hex_value;
 303
 304         *int_value = 0;
 305
 306         while (**ptr) {
 307                 hex_value = hex(**ptr);
 308                 if (hex_value >= 0) {
 309                         *int_value = (*int_value << 4) | hex_value;
 310                         num_chars++;
 311                 } else
 312                         break;
 313                 (*ptr)++;
 314         }
 315         return num_chars;
 316 }
 317
 318 /*  Copy the binary array pointed to by buf into mem.  Fix $, #,
 319     and 0x7d escaped with 0x7d.  Return a pointer to the character
 320     after the last byte written. */
 321 static char *ebin_to_mem(const char *buf, char *mem, int count)
 322 {
 323         for (; count > 0; count--, buf++) {
 324                 if (*buf == 0x7d)
 325                         *mem++ = *(++buf) ^ 0x20;
 326                 else
 327                         *mem++ = *buf;
 328         }
 329         return mem;
 330 }
 331
 332 /* Scan for the start char '$', read the packet and check the checksum */
 333 static void get_packet(char *buffer, int buflen)
 334 {
 335         unsigned char checksum;
 336         unsigned char xmitcsum;
 337         int i;
 338         int count;
 339         char ch;
 340
 341         do {
 342                 /* Ignore everything until the start character */
 343                 while ((ch = get_debug_char()) != '$');
 344
 345                 checksum = 0;
 346                 xmitcsum = -1;
 347                 count = 0;
 348
 349                 /* Now, read until a # or end of buffer is found */
 350                 while (count < (buflen - 1)) {
 351                         ch = get_debug_char();
 352
 353                         if (ch == '#')
 354                                 break;
 355
 356                         checksum = checksum + ch;
 357                         buffer[count] = ch;
 358                         count = count + 1;
 359                 }
 360
 361                 buffer[count] = 0;
 362
 363                 /* Continue to read checksum following # */
 364                 if (ch == '#') {
 365                         xmitcsum = hex(get_debug_char()) << 4;
 366                         xmitcsum += hex(get_debug_char());
 367
 368                         /* Checksum */
 369                         if (checksum != xmitcsum)
 370                                 put_debug_char('-');    /* Failed checksum */
 371                         else {
 372                                 /* Ack successful transfer */
 373                                 put_debug_char('+');
 374
 375                                 /* If a sequence char is present, reply
 376                                    the sequence ID */
 377                                 if (buffer[2] == ':') {
 378                                         put_debug_char(buffer[0]);
 379                                         put_debug_char(buffer[1]);
 380
 381                                         /* Remove sequence chars from buffer */
 382                                         count = strlen(buffer);
 383                                         for (i = 3; i <= count; i++)
 384                                                 buffer[i - 3] = buffer[i];
 385                                 }
 386                         }
 387                 }
 388         }
 389         while (checksum != xmitcsum);   /* Keep trying while we fail */
 390 }
 391
 392 /* Send the packet in the buffer with run-length encoding */
 393 static void put_packet(char *buffer)
 394 {
 395         int checksum;
 396         char *src;
 397         int runlen;
 398         int encode;
 399
 400         do {
 401                 src = buffer;
 402                 put_debug_char('$');
 403                 checksum = 0;
 404
 405                 /* Continue while we still have chars left */
 406                 while (*src) {
 407                         /* Check for runs up to 99 chars long */
 408                         for (runlen = 1; runlen < 99; runlen++) {
 409                                 if (src[0] != src[runlen])
 410                                         break;
 411                         }
 412
 413                         if (runlen > 3) {
 414                                 /* Got a useful amount, send encoding */
 415                                 encode = runlen + ' ' - 4;
 416                                 put_debug_char(*src);   checksum += *src;
 417                                 put_debug_char('*');    checksum += '*';
 418                                 put_debug_char(encode); checksum += encode;
 419                                 src += runlen;
 420                         } else {
 421                                 /* Otherwise just send the current char */
 422                                 put_debug_char(*src);   checksum += *src;
 423                                 src += 1;
 424                         }
 425                 }
 426
 427                 /* '#' Separator, put high and low components of checksum */
 428                 put_debug_char('#');
 429                 put_debug_char(hex_asc_hi(checksum));
 430                 put_debug_char(hex_asc_lo(checksum));
 431         }
 432         while ((get_debug_char()) != '+');      /* While no ack */
 433 }
 434
 435 /* A bus error has occurred - perform a longjmp to return execution and
 436    allow handling of the error */
 437 static void kgdb_handle_bus_error(void)
 438 {
 439         longjmp(rem_com_env, 1);
 440 }
 441
 442 /* Translate SH-3/4 exception numbers to unix-like signal values */
 443 static int compute_signal(const int excep_code)
 444 {
 445         int sigval;
 446
 447         switch (excep_code) {
 448
 449         case INVALID_INSN_VEC:
 450         case INVALID_SLOT_VEC:
 451                 sigval = SIGILL;
 452                 break;
 453         case ADDRESS_ERROR_LOAD_VEC:
 454         case ADDRESS_ERROR_STORE_VEC:
 455                 sigval = SIGSEGV;
 456                 break;
 457
 458         case SERIAL_BREAK_VEC:
 459         case NMI_VEC:
 460                 sigval = SIGINT;
 461                 break;
 462
 463         case USER_BREAK_VEC:
 464         case TRAP_VEC:
 465                 sigval = SIGTRAP;
 466                 break;
 467
 468         default:
 469                 sigval = SIGBUS;        /* "software generated" */
 470                 break;
 471         }
 472
 473         return (sigval);
 474 }
 475
 476 /* Make a local copy of the registers passed into the handler (bletch) */
 477 static void kgdb_regs_to_gdb_regs(const struct kgdb_regs *regs,
 478                                   int *gdb_regs)
 479 {
 480         gdb_regs[R0] = regs->regs[R0];
 481         gdb_regs[R1] = regs->regs[R1];
 482         gdb_regs[R2] = regs->regs[R2];
 483         gdb_regs[R3] = regs->regs[R3];
 484         gdb_regs[R4] = regs->regs[R4];
 485         gdb_regs[R5] = regs->regs[R5];
 486         gdb_regs[R6] = regs->regs[R6];
 487         gdb_regs[R7] = regs->regs[R7];
 488         gdb_regs[R8] = regs->regs[R8];
 489         gdb_regs[R9] = regs->regs[R9];
 490         gdb_regs[R10] = regs->regs[R10];
 491         gdb_regs[R11] = regs->regs[R11];
 492         gdb_regs[R12] = regs->regs[R12];
 493         gdb_regs[R13] = regs->regs[R13];
 494         gdb_regs[R14] = regs->regs[R14];
 495         gdb_regs[R15] = regs->regs[R15];
 496         gdb_regs[PC] = regs->pc;
 497         gdb_regs[PR] = regs->pr;
 498         gdb_regs[GBR] = regs->gbr;
 499         gdb_regs[MACH] = regs->mach;
 500         gdb_regs[MACL] = regs->macl;
 501         gdb_regs[SR] = regs->sr;
 502         gdb_regs[VBR] = regs->vbr;
 503 }
 504
 505 /* Copy local gdb registers back to kgdb regs, for later copy to kernel */
 506 static void gdb_regs_to_kgdb_regs(const int *gdb_regs,
 507                                   struct kgdb_regs *regs)
 508 {
 509         regs->regs[R0] = gdb_regs[R0];
 510         regs->regs[R1] = gdb_regs[R1];
 511         regs->regs[R2] = gdb_regs[R2];
 512         regs->regs[R3] = gdb_regs[R3];
 513         regs->regs[R4] = gdb_regs[R4];
 514         regs->regs[R5] = gdb_regs[R5];
 515         regs->regs[R6] = gdb_regs[R6];
 516         regs->regs[R7] = gdb_regs[R7];
 517         regs->regs[R8] = gdb_regs[R8];
 518         regs->regs[R9] = gdb_regs[R9];
 519         regs->regs[R10] = gdb_regs[R10];
 520         regs->regs[R11] = gdb_regs[R11];
 521         regs->regs[R12] = gdb_regs[R12];
 522         regs->regs[R13] = gdb_regs[R13];
 523         regs->regs[R14] = gdb_regs[R14];
 524         regs->regs[R15] = gdb_regs[R15];
 525         regs->pc = gdb_regs[PC];
 526         regs->pr = gdb_regs[PR];
 527         regs->gbr = gdb_regs[GBR];
 528         regs->mach = gdb_regs[MACH];
 529         regs->macl = gdb_regs[MACL];
 530         regs->sr = gdb_regs[SR];
 531         regs->vbr = gdb_regs[VBR];
 532 }
 533
 534 /* Calculate the new address for after a step */
 535 static short *get_step_address(void)
 536 {
 537         short op = *(short *) trap_registers.pc;
 538         long addr;
 539
 540         /* BT */
 541         if (OPCODE_BT(op)) {
 542                 if (trap_registers.sr & SR_T_BIT_MASK)
 543                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 544                 else
 545                         addr = trap_registers.pc + 2;
 546         }
 547
 548         /* BTS */
 549         else if (OPCODE_BTS(op)) {
 550                 if (trap_registers.sr & SR_T_BIT_MASK)
 551                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 552                 else
 553                         addr = trap_registers.pc + 4;   /* Not in delay slot */
 554         }
 555
 556         /* BF */
 557         else if (OPCODE_BF(op)) {
 558                 if (!(trap_registers.sr & SR_T_BIT_MASK))
 559                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 560                 else
 561                         addr = trap_registers.pc + 2;
 562         }
 563
 564         /* BFS */
 565         else if (OPCODE_BFS(op)) {
 566                 if (!(trap_registers.sr & SR_T_BIT_MASK))
 567                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 568                 else
 569                         addr = trap_registers.pc + 4;   /* Not in delay slot */
 570         }
 571
 572         /* BRA */
 573         else if (OPCODE_BRA(op))
 574                 addr = trap_registers.pc + 4 + OPCODE_BRA_DISP(op);
 575
 576         /* BRAF */
 577         else if (OPCODE_BRAF(op))
 578                 addr = trap_registers.pc + 4
 579                     + trap_registers.regs[OPCODE_BRAF_REG(op)];
 580
 581         /* BSR */
 582         else if (OPCODE_BSR(op))
 583                 addr = trap_registers.pc + 4 + OPCODE_BSR_DISP(op);
 584
 585         /* BSRF */
 586         else if (OPCODE_BSRF(op))
 587                 addr = trap_registers.pc + 4
 588                     + trap_registers.regs[OPCODE_BSRF_REG(op)];
 589
 590         /* JMP */
 591         else if (OPCODE_JMP(op))
 592                 addr = trap_registers.regs[OPCODE_JMP_REG(op)];
 593
 594         /* JSR */
 595         else if (OPCODE_JSR(op))
 596                 addr = trap_registers.regs[OPCODE_JSR_REG(op)];
 597
 598         /* RTS */
 599         else if (OPCODE_RTS(op))
 600                 addr = trap_registers.pr;
 601
 602         /* RTE */
 603         else if (OPCODE_RTE(op))
 604                 addr = trap_registers.regs[15];
 605
 606         /* Other */
 607         else
 608                 addr = trap_registers.pc + 2;
 609
 610         flush_icache_range(addr, addr + 2);
 611         return (short *) addr;
 612 }
 613
 614 /* Set up a single-step.  Replace the instruction immediately after the
 615    current instruction (i.e. next in the expected flow of control) with a
 616    trap instruction, so that returning will cause only a single instruction
 617    to be executed. Note that this model is slightly broken for instructions
 618    with delay slots (e.g. B[TF]S, BSR, BRA etc), where both the branch
 619    and the instruction in the delay slot will be executed. */
 620 static void do_single_step(void)
 621 {
 622         unsigned short *addr = 0;
 623
 624         /* Determine where the target instruction will send us to */
 625         addr = get_step_address();
 626         stepped_address = (int)addr;
 627
 628         /* Replace it */
 629         stepped_opcode = *(short *)addr;
 630         *addr = STEP_OPCODE;
 631
 632         /* Flush and return */
 633         flush_icache_range((long) addr, (long) addr + 2);
 634 }
 635
 636 /* Undo a single step */
 637 static void undo_single_step(void)
 638 {
 639         /* If we have stepped, put back the old instruction */
 640         /* Use stepped_address in case we stopped elsewhere */
 641         if (stepped_opcode != 0) {
 642                 *(short*)stepped_address = stepped_opcode;
 643                 flush_icache_range(stepped_address, stepped_address + 2);
 644         }
 645         stepped_opcode = 0;
 646 }
 647
 648 /* Send a signal message */
 649 static void send_signal_msg(const int signum)
 650 {
 651         out_buffer[0] = 'S';
 652         out_buffer[1] = hex_asc_hi(signum);
 653         out_buffer[2] = hex_asc_lo(signum);
 654         out_buffer[3] = 0;
 655         put_packet(out_buffer);
 656 }
 657
 658 /* Reply that all was well */
 659 static void send_ok_msg(void)
 660 {
 661         strcpy(out_buffer, "OK");
 662         put_packet(out_buffer);
 663 }
 664
 665 /* Reply that an error occurred */
 666 static void send_err_msg(void)
 667 {
 668         strcpy(out_buffer, "E01");
 669         put_packet(out_buffer);
 670 }
 671
 672 /* Empty message indicates unrecognised command */
 673 static void send_empty_msg(void)
 674 {
 675         put_packet("");
 676 }
 677
 678 /* Read memory due to 'm' message */
 679 static void read_mem_msg(void)
 680 {
 681         char *ptr;
 682         int addr;
 683         int length;
 684
 685         /* Jmp, disable bus error handler */
 686         if (setjmp(rem_com_env) == 0) {
 687
 688                 kgdb_nofault = 1;
 689
 690                 /* Walk through, have m<addr>,<length> */
 691                 ptr = &in_buffer[1];
 692                 if (hex_to_int(&ptr, &addr) && (*ptr++ == ','))
 693                         if (hex_to_int(&ptr, &length)) {
 694                                 ptr = 0;
 695                                 if (length * 2 > OUTBUFMAX)
 696                                         length = OUTBUFMAX / 2;
 697                                 mem_to_hex((char *) addr, out_buffer, length);
 698                         }
 699                 if (ptr)
 700                         send_err_msg();
 701                 else
 702                         put_packet(out_buffer);
 703         } else
 704                 send_err_msg();
 705
 706         /* Restore bus error handler */
 707         kgdb_nofault = 0;
 708 }
 709
 710 /* Write memory due to 'M' or 'X' message */
 711 static void write_mem_msg(int binary)
 712 {
 713         char *ptr;
 714         int addr;
 715         int length;
 716
 717         if (setjmp(rem_com_env) == 0) {
 718
 719                 kgdb_nofault = 1;
 720
 721                 /* Walk through, have M<addr>,<length>:<data> */
 722                 ptr = &in_buffer[1];
 723                 if (hex_to_int(&ptr, &addr) && (*ptr++ == ','))
 724                         if (hex_to_int(&ptr, &length) && (*ptr++ == ':')) {
 725                                 if (binary)
 726                                         ebin_to_mem(ptr, (char*)addr, length);
 727                                 else
 728                                         hex_to_mem(ptr, (char*)addr, length);
 729                                 flush_icache_range(addr, addr + length);
 730                                 ptr = 0;
 731                                 send_ok_msg();
 732                         }
 733                 if (ptr)
 734                         send_err_msg();
 735         } else
 736                 send_err_msg();
 737
 738         /* Restore bus error handler */
 739         kgdb_nofault = 0;
 740 }
 741
 742 /* Continue message  */
 743 static void continue_msg(void)
 744 {
 745         /* Try to read optional parameter, PC unchanged if none */
 746         char *ptr = &in_buffer[1];
 747         int addr;
 748
 749         if (hex_to_int(&ptr, &addr))
 750                 trap_registers.pc = addr;
 751 }
 752
 753 /* Continue message with signal */
 754 static void continue_with_sig_msg(void)
 755 {
 756         int signal;
 757         char *ptr = &in_buffer[1];
 758         int addr;
 759
 760         /* Report limitation */
 761         kgdb_to_gdb("Cannot force signal in kgdb, continuing anyway.\n");
 762
 763         /* Signal */
 764         hex_to_int(&ptr, &signal);
 765         if (*ptr == ';')
 766                 ptr++;
 767
 768         /* Optional address */
 769         if (hex_to_int(&ptr, &addr))
 770                 trap_registers.pc = addr;
 771 }
 772
 773 /* Step message */
 774 static void step_msg(void)
 775 {
 776         continue_msg();
 777         do_single_step();
 778 }
 779
 780 /* Step message with signal */
 781 static void step_with_sig_msg(void)
 782 {
 783         continue_with_sig_msg();
 784         do_single_step();
 785 }
 786
 787 /* Send register contents */
 788 static void send_regs_msg(void)
 789 {
 790         kgdb_regs_to_gdb_regs(&trap_registers, registers);
 791         mem_to_hex((char *) registers, out_buffer, NUMREGBYTES);
 792         put_packet(out_buffer);
 793 }
 794
 795 /* Set register contents - currently can't set other thread's registers */
 796 static void set_regs_msg(void)
 797 {
 798         kgdb_regs_to_gdb_regs(&trap_registers, registers);
 799         hex_to_mem(&in_buffer[1], (char *) registers, NUMREGBYTES);
 800         gdb_regs_to_kgdb_regs(registers, &trap_registers);
 801         send_ok_msg();
 802 }
 803
 804 #ifdef CONFIG_SH_KGDB_CONSOLE
 805 /*
 806  * Bring up the ports..
 807  */
 808 static int __init kgdb_serial_setup(void)
 809 {
 810         struct console dummy;
 811         return kgdb_console_setup(&dummy, 0);
 812 }
 813 #else
 814 #define kgdb_serial_setup()     0
 815 #endif
 816
 817 /* The command loop, read and act on requests */
 818 static void kgdb_command_loop(const int excep_code, const int trapa_value)
 819 {
 820         int sigval;
 821
 822         /* Enter GDB mode (e.g. after detach) */
 823         if (!kgdb_in_gdb_mode) {
 824                 /* Do serial setup, notify user, issue preemptive ack */
 825                 printk(KERN_NOTICE "KGDB: Waiting for GDB\n");
 826                 kgdb_in_gdb_mode = 1;
 827                 put_debug_char('+');
 828         }
 829
 830         /* Reply to host that an exception has occurred */
 831         sigval = compute_signal(excep_code);
 832         send_signal_msg(sigval);
 833
 834         /* TRAP_VEC exception indicates a software trap inserted in place of
 835            code by GDB so back up PC by one instruction, as this instruction
 836            will later be replaced by its original one.  Do NOT do this for
 837            trap 0xff, since that indicates a compiled-in breakpoint which
 838            will not be replaced (and we would retake the trap forever) */
 839         if ((excep_code == TRAP_VEC) && (trapa_value != (0x3c << 2)))
 840                 trap_registers.pc -= 2;
 841
 842         /* Undo any stepping we may have done */
 843         undo_single_step();
 844
 845         while (1) {
 846                 out_buffer[0] = 0;
 847                 get_packet(in_buffer, BUFMAX);
 848
 849                 /* Examine first char of buffer to see what we need to do */
 850                 switch (in_buffer[0]) {
 851                 case '?':       /* Send which signal we've received */
 852                         send_signal_msg(sigval);
 853                         break;
 854
 855                 case 'g':       /* Return the values of the CPU registers */
 856                         send_regs_msg();
 857                         break;
 858
 859                 case 'G':       /* Set the value of the CPU registers */
 860                         set_regs_msg();
 861                         break;
 862
 863                 case 'm':       /* Read LLLL bytes address AA..AA */
 864                         read_mem_msg();
 865                         break;
 866
 867                 case 'M':       /* Write LLLL bytes address AA..AA, ret OK */
 868                         write_mem_msg(0);       /* 0 = data in hex */
 869                         break;
 870
 871                 case 'X':       /* Write LLLL bytes esc bin address AA..AA */
 872                         if (kgdb_bits == '8')
 873                                 write_mem_msg(1); /* 1 = data in binary */
 874                         else
 875                                 send_empty_msg();
 876                         break;
 877
 878                 case 'C':       /* Continue, signum included, we ignore it */
 879                         continue_with_sig_msg();
 880                         return;
 881
 882                 case 'c':       /* Continue at address AA..AA (optional) */
 883                         continue_msg();
 884                         return;
 885
 886                 case 'S':       /* Step, signum included, we ignore it */
 887                         step_with_sig_msg();
 888                         return;
 889
 890                 case 's':       /* Step one instruction from AA..AA */
 891                         step_msg();
 892                         return;
 893
 894                 case 'k':       /* 'Kill the program' with a kernel ? */
 895                         break;
 896
 897                 case 'D':       /* Detach from program, send reply OK */
 898                         kgdb_in_gdb_mode = 0;
 899                         send_ok_msg();
 900                         get_debug_char();
 901                         return;
 902
 903                 default:
 904                         send_empty_msg();
 905                         break;
 906                 }
 907         }
 908 }
 909
 910 /* There has been an exception, most likely a breakpoint. */
 911 static void handle_exception(struct pt_regs *regs)
 912 {
 913         int excep_code, vbr_val;
 914         int count;
 915         int trapa_value = ctrl_inl(TRA);
 916
 917         /* Copy kernel regs (from stack) */
 918         for (count = 0; count < 16; count++)
 919                 trap_registers.regs[count] = regs->regs[count];
 920         trap_registers.pc = regs->pc;
 921         trap_registers.pr = regs->pr;
 922         trap_registers.sr = regs->sr;
 923         trap_registers.gbr = regs->gbr;
 924         trap_registers.mach = regs->mach;
 925         trap_registers.macl = regs->macl;
 926
 927         asm("stc vbr, %0":"=r"(vbr_val));
 928         trap_registers.vbr = vbr_val;
 929
 930         /* Get excode for command loop call, user access */
 931         asm("stc r2_bank, %0":"=r"(excep_code));
 932
 933         /* Act on the exception */
 934         kgdb_command_loop(excep_code, trapa_value);
 935
 936         /* Copy back the (maybe modified) registers */
 937         for (count = 0; count < 16; count++)
 938                 regs->regs[count] = trap_registers.regs[count];
 939         regs->pc = trap_registers.pc;
 940         regs->pr = trap_registers.pr;
 941         regs->sr = trap_registers.sr;
 942         regs->gbr = trap_registers.gbr;
 943         regs->mach = trap_registers.mach;
 944         regs->macl = trap_registers.macl;
 945
 946         vbr_val = trap_registers.vbr;
 947         asm("ldc %0, vbr": :"r"(vbr_val));
 948 }
 949
 950 asmlinkage void kgdb_handle_exception(unsigned long r4, unsigned long r5,
 951                                       unsigned long r6, unsigned long r7,
 952                                       struct pt_regs __regs)
 953 {
 954         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
 955         handle_exception(regs);
 956 }
 957
 958 /* Initialise the KGDB data structures and serial configuration */
 959 int __init kgdb_init(void)
 960 {
 961         in_nmi = 0;
 962         kgdb_nofault = 0;
 963         stepped_opcode = 0;
 964         kgdb_in_gdb_mode = 0;
 965
 966         if (kgdb_serial_setup() != 0) {
 967                 printk(KERN_NOTICE "KGDB: serial setup error\n");
 968                 return -1;
 969         }
 970
 971         /* Init ptr to exception handler */
 972         kgdb_debug_hook = handle_exception;
 973         kgdb_bus_err_hook = kgdb_handle_bus_error;
 974
 975         /* Enter kgdb now if requested, or just report init done */
 976         printk(KERN_NOTICE "KGDB: stub is initialized.\n");
 977
 978         return 0;
 979 }
 980
 981 /* Make function available for "user messages"; console will use it too. */
 982
 983 char gdbmsgbuf[BUFMAX];
 984 #define MAXOUT ((BUFMAX-2)/2)
 985
 986 static void kgdb_msg_write(const char *s, unsigned count)
 987 {
 988         int i;
 989         int wcount;
 990         char *bufptr;
 991
 992         /* 'O'utput */
 993         gdbmsgbuf[0] = 'O';
 994
 995         /* Fill and send buffers... */
 996         while (count > 0) {
 997                 bufptr = gdbmsgbuf + 1;
 998
 999                 /* Calculate how many this time */
1000                 wcount = (count > MAXOUT) ? MAXOUT : count;
1001
1002                 /* Pack in hex chars */
1003                 for (i = 0; i < wcount; i++)
1004                         bufptr = pack_hex_byte(bufptr, s[i]);
1005                 *bufptr = '\0';
1006
1007                 /* Move up */
1008                 s += wcount;
1009                 count -= wcount;
1010
1011                 /* Write packet */
1012                 put_packet(gdbmsgbuf);
1013         }
1014 }
1015
1016 static void kgdb_to_gdb(const char *s)
1017 {
1018         kgdb_msg_write(s, strlen(s));
1019 }
1020
1021 #ifdef CONFIG_SH_KGDB_CONSOLE
1022 void kgdb_console_write(struct console *co, const char *s, unsigned count)
1023 {
1024         /* Bail if we're not talking to GDB */
1025         if (!kgdb_in_gdb_mode)
1026                 return;
1027
1028         kgdb_msg_write(s, count);
1029 }
1030 #endif
1031
1032 #ifdef CONFIG_KGDB_SYSRQ
1033 static void sysrq_handle_gdb(int key, struct tty_struct *tty)
1034 {
1035         printk("Entering GDB stub\n");
1036         breakpoint();
1037 }
1038
1039 static struct sysrq_key_op sysrq_gdb_op = {
1040         .handler        = sysrq_handle_gdb,
1041         .help_msg       = "Gdb",
1042         .action_msg     = "GDB",
1043 };
1044
1045 static int gdb_register_sysrq(void)
1046 {
1047         printk("Registering GDB sysrq handler\n");
1048         register_sysrq_key('g', &sysrq_gdb_op);
1049         return 0;
1050 }
1051 module_init(gdb_register_sysrq);
1052 #endif