SAFE public projects git trees. - safe/jmp/linux-2.6/blob - 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++ = highhex(ch);
 278                 *buf++ = lowhex(ch);
 279         }
 280         *buf = 0;
 281         return (buf);
 282 }
 283
 284 /* Convert the hex array pointed to by buf into binary, to be placed in mem.
 285    Return a pointer to the character after the last byte written */
 286 static char *hex_to_mem(const char *buf, char *mem, const int count)
 287 {
 288         int i;
 289         unsigned char ch;
 290
 291         for (i = 0; i < count; i++) {
 292                 ch = hex(*buf++) << 4;
 293                 ch = ch + hex(*buf++);
 294                 *mem++ = ch;
 295         }
 296         return (mem);
 297 }
 298
 299 /* While finding valid hex chars, convert to an integer, then return it */
 300 static int hex_to_int(char **ptr, int *int_value)
 301 {
 302         int num_chars = 0;
 303         int hex_value;
 304
 305         *int_value = 0;
 306
 307         while (**ptr) {
 308                 hex_value = hex(**ptr);
 309                 if (hex_value >= 0) {
 310                         *int_value = (*int_value << 4) | hex_value;
 311                         num_chars++;
 312                 } else
 313                         break;
 314                 (*ptr)++;
 315         }
 316         return num_chars;
 317 }
 318
 319 /*  Copy the binary array pointed to by buf into mem.  Fix $, #,
 320     and 0x7d escaped with 0x7d.  Return a pointer to the character
 321     after the last byte written. */
 322 static char *ebin_to_mem(const char *buf, char *mem, int count)
 323 {
 324         for (; count > 0; count--, buf++) {
 325                 if (*buf == 0x7d)
 326                         *mem++ = *(++buf) ^ 0x20;
 327                 else
 328                         *mem++ = *buf;
 329         }
 330         return mem;
 331 }
 332
 333 /* Scan for the start char '$', read the packet and check the checksum */
 334 static void get_packet(char *buffer, int buflen)
 335 {
 336         unsigned char checksum;
 337         unsigned char xmitcsum;
 338         int i;
 339         int count;
 340         char ch;
 341
 342         do {
 343                 /* Ignore everything until the start character */
 344                 while ((ch = get_debug_char()) != '$');
 345
 346                 checksum = 0;
 347                 xmitcsum = -1;
 348                 count = 0;
 349
 350                 /* Now, read until a # or end of buffer is found */
 351                 while (count < (buflen - 1)) {
 352                         ch = get_debug_char();
 353
 354                         if (ch == '#')
 355                                 break;
 356
 357                         checksum = checksum + ch;
 358                         buffer[count] = ch;
 359                         count = count + 1;
 360                 }
 361
 362                 buffer[count] = 0;
 363
 364                 /* Continue to read checksum following # */
 365                 if (ch == '#') {
 366                         xmitcsum = hex(get_debug_char()) << 4;
 367                         xmitcsum += hex(get_debug_char());
 368
 369                         /* Checksum */
 370                         if (checksum != xmitcsum)
 371                                 put_debug_char('-');    /* Failed checksum */
 372                         else {
 373                                 /* Ack successful transfer */
 374                                 put_debug_char('+');
 375
 376                                 /* If a sequence char is present, reply
 377                                    the sequence ID */
 378                                 if (buffer[2] == ':') {
 379                                         put_debug_char(buffer[0]);
 380                                         put_debug_char(buffer[1]);
 381
 382                                         /* Remove sequence chars from buffer */
 383                                         count = strlen(buffer);
 384                                         for (i = 3; i <= count; i++)
 385                                                 buffer[i - 3] = buffer[i];
 386                                 }
 387                         }
 388                 }
 389         }
 390         while (checksum != xmitcsum);   /* Keep trying while we fail */
 391 }
 392
 393 /* Send the packet in the buffer with run-length encoding */
 394 static void put_packet(char *buffer)
 395 {
 396         int checksum;
 397         char *src;
 398         int runlen;
 399         int encode;
 400
 401         do {
 402                 src = buffer;
 403                 put_debug_char('$');
 404                 checksum = 0;
 405
 406                 /* Continue while we still have chars left */
 407                 while (*src) {
 408                         /* Check for runs up to 99 chars long */
 409                         for (runlen = 1; runlen < 99; runlen++) {
 410                                 if (src[0] != src[runlen])
 411                                         break;
 412                         }
 413
 414                         if (runlen > 3) {
 415                                 /* Got a useful amount, send encoding */
 416                                 encode = runlen + ' ' - 4;
 417                                 put_debug_char(*src);   checksum += *src;
 418                                 put_debug_char('*');    checksum += '*';
 419                                 put_debug_char(encode); checksum += encode;
 420                                 src += runlen;
 421                         } else {
 422                                 /* Otherwise just send the current char */
 423                                 put_debug_char(*src);   checksum += *src;
 424                                 src += 1;
 425                         }
 426                 }
 427
 428                 /* '#' Separator, put high and low components of checksum */
 429                 put_debug_char('#');
 430                 put_debug_char(highhex(checksum));
 431                 put_debug_char(lowhex(checksum));
 432         }
 433         while ((get_debug_char()) != '+');      /* While no ack */
 434 }
 435
 436 /* A bus error has occurred - perform a longjmp to return execution and
 437    allow handling of the error */
 438 static void kgdb_handle_bus_error(void)
 439 {
 440         longjmp(rem_com_env, 1);
 441 }
 442
 443 /* Translate SH-3/4 exception numbers to unix-like signal values */
 444 static int compute_signal(const int excep_code)
 445 {
 446         int sigval;
 447
 448         switch (excep_code) {
 449
 450         case INVALID_INSN_VEC:
 451         case INVALID_SLOT_VEC:
 452                 sigval = SIGILL;
 453                 break;
 454         case ADDRESS_ERROR_LOAD_VEC:
 455         case ADDRESS_ERROR_STORE_VEC:
 456                 sigval = SIGSEGV;
 457                 break;
 458
 459         case SERIAL_BREAK_VEC:
 460         case NMI_VEC:
 461                 sigval = SIGINT;
 462                 break;
 463
 464         case USER_BREAK_VEC:
 465         case TRAP_VEC:
 466                 sigval = SIGTRAP;
 467                 break;
 468
 469         default:
 470                 sigval = SIGBUS;        /* "software generated" */
 471                 break;
 472         }
 473
 474         return (sigval);
 475 }
 476
 477 /* Make a local copy of the registers passed into the handler (bletch) */
 478 static void kgdb_regs_to_gdb_regs(const struct kgdb_regs *regs,
 479                                   int *gdb_regs)
 480 {
 481         gdb_regs[R0] = regs->regs[R0];
 482         gdb_regs[R1] = regs->regs[R1];
 483         gdb_regs[R2] = regs->regs[R2];
 484         gdb_regs[R3] = regs->regs[R3];
 485         gdb_regs[R4] = regs->regs[R4];
 486         gdb_regs[R5] = regs->regs[R5];
 487         gdb_regs[R6] = regs->regs[R6];
 488         gdb_regs[R7] = regs->regs[R7];
 489         gdb_regs[R8] = regs->regs[R8];
 490         gdb_regs[R9] = regs->regs[R9];
 491         gdb_regs[R10] = regs->regs[R10];
 492         gdb_regs[R11] = regs->regs[R11];
 493         gdb_regs[R12] = regs->regs[R12];
 494         gdb_regs[R13] = regs->regs[R13];
 495         gdb_regs[R14] = regs->regs[R14];
 496         gdb_regs[R15] = regs->regs[R15];
 497         gdb_regs[PC] = regs->pc;
 498         gdb_regs[PR] = regs->pr;
 499         gdb_regs[GBR] = regs->gbr;
 500         gdb_regs[MACH] = regs->mach;
 501         gdb_regs[MACL] = regs->macl;
 502         gdb_regs[SR] = regs->sr;
 503         gdb_regs[VBR] = regs->vbr;
 504 }
 505
 506 /* Copy local gdb registers back to kgdb regs, for later copy to kernel */
 507 static void gdb_regs_to_kgdb_regs(const int *gdb_regs,
 508                                   struct kgdb_regs *regs)
 509 {
 510         regs->regs[R0] = gdb_regs[R0];
 511         regs->regs[R1] = gdb_regs[R1];
 512         regs->regs[R2] = gdb_regs[R2];
 513         regs->regs[R3] = gdb_regs[R3];
 514         regs->regs[R4] = gdb_regs[R4];
 515         regs->regs[R5] = gdb_regs[R5];
 516         regs->regs[R6] = gdb_regs[R6];
 517         regs->regs[R7] = gdb_regs[R7];
 518         regs->regs[R8] = gdb_regs[R8];
 519         regs->regs[R9] = gdb_regs[R9];
 520         regs->regs[R10] = gdb_regs[R10];
 521         regs->regs[R11] = gdb_regs[R11];
 522         regs->regs[R12] = gdb_regs[R12];
 523         regs->regs[R13] = gdb_regs[R13];
 524         regs->regs[R14] = gdb_regs[R14];
 525         regs->regs[R15] = gdb_regs[R15];
 526         regs->pc = gdb_regs[PC];
 527         regs->pr = gdb_regs[PR];
 528         regs->gbr = gdb_regs[GBR];
 529         regs->mach = gdb_regs[MACH];
 530         regs->macl = gdb_regs[MACL];
 531         regs->sr = gdb_regs[SR];
 532         regs->vbr = gdb_regs[VBR];
 533 }
 534
 535 /* Calculate the new address for after a step */
 536 static short *get_step_address(void)
 537 {
 538         short op = *(short *) trap_registers.pc;
 539         long addr;
 540
 541         /* BT */
 542         if (OPCODE_BT(op)) {
 543                 if (trap_registers.sr & SR_T_BIT_MASK)
 544                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 545                 else
 546                         addr = trap_registers.pc + 2;
 547         }
 548
 549         /* BTS */
 550         else if (OPCODE_BTS(op)) {
 551                 if (trap_registers.sr & SR_T_BIT_MASK)
 552                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 553                 else
 554                         addr = trap_registers.pc + 4;   /* Not in delay slot */
 555         }
 556
 557         /* BF */
 558         else if (OPCODE_BF(op)) {
 559                 if (!(trap_registers.sr & SR_T_BIT_MASK))
 560                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 561                 else
 562                         addr = trap_registers.pc + 2;
 563         }
 564
 565         /* BFS */
 566         else if (OPCODE_BFS(op)) {
 567                 if (!(trap_registers.sr & SR_T_BIT_MASK))
 568                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 569                 else
 570                         addr = trap_registers.pc + 4;   /* Not in delay slot */
 571         }
 572
 573         /* BRA */
 574         else if (OPCODE_BRA(op))
 575                 addr = trap_registers.pc + 4 + OPCODE_BRA_DISP(op);
 576
 577         /* BRAF */
 578         else if (OPCODE_BRAF(op))
 579                 addr = trap_registers.pc + 4
 580                     + trap_registers.regs[OPCODE_BRAF_REG(op)];
 581
 582         /* BSR */
 583         else if (OPCODE_BSR(op))
 584                 addr = trap_registers.pc + 4 + OPCODE_BSR_DISP(op);
 585
 586         /* BSRF */
 587         else if (OPCODE_BSRF(op))
 588                 addr = trap_registers.pc + 4
 589                     + trap_registers.regs[OPCODE_BSRF_REG(op)];
 590
 591         /* JMP */
 592         else if (OPCODE_JMP(op))
 593                 addr = trap_registers.regs[OPCODE_JMP_REG(op)];
 594
 595         /* JSR */
 596         else if (OPCODE_JSR(op))
 597                 addr = trap_registers.regs[OPCODE_JSR_REG(op)];
 598
 599         /* RTS */
 600         else if (OPCODE_RTS(op))
 601                 addr = trap_registers.pr;
 602
 603         /* RTE */
 604         else if (OPCODE_RTE(op))
 605                 addr = trap_registers.regs[15];
 606
 607         /* Other */
 608         else
 609                 addr = trap_registers.pc + 2;
 610
 611         flush_icache_range(addr, addr + 2);
 612         return (short *) addr;
 613 }
 614
 615 /* Set up a single-step.  Replace the instruction immediately after the
 616    current instruction (i.e. next in the expected flow of control) with a
 617    trap instruction, so that returning will cause only a single instruction
 618    to be executed. Note that this model is slightly broken for instructions
 619    with delay slots (e.g. B[TF]S, BSR, BRA etc), where both the branch
 620    and the instruction in the delay slot will be executed. */
 621 static void do_single_step(void)
 622 {
 623         unsigned short *addr = 0;
 624
 625         /* Determine where the target instruction will send us to */
 626         addr = get_step_address();
 627         stepped_address = (int)addr;
 628
 629         /* Replace it */
 630         stepped_opcode = *(short *)addr;
 631         *addr = STEP_OPCODE;
 632
 633         /* Flush and return */
 634         flush_icache_range((long) addr, (long) addr + 2);
 635 }
 636
 637 /* Undo a single step */
 638 static void undo_single_step(void)
 639 {
 640         /* If we have stepped, put back the old instruction */
 641         /* Use stepped_address in case we stopped elsewhere */
 642         if (stepped_opcode != 0) {
 643                 *(short*)stepped_address = stepped_opcode;
 644                 flush_icache_range(stepped_address, stepped_address + 2);
 645         }
 646         stepped_opcode = 0;
 647 }
 648
 649 /* Send a signal message */
 650 static void send_signal_msg(const int signum)
 651 {
 652         out_buffer[0] = 'S';
 653         out_buffer[1] = highhex(signum);
 654         out_buffer[2] = lowhex(signum);
 655         out_buffer[3] = 0;
 656         put_packet(out_buffer);
 657 }
 658
 659 /* Reply that all was well */
 660 static void send_ok_msg(void)
 661 {
 662         strcpy(out_buffer, "OK");
 663         put_packet(out_buffer);
 664 }
 665
 666 /* Reply that an error occurred */
 667 static void send_err_msg(void)
 668 {
 669         strcpy(out_buffer, "E01");
 670         put_packet(out_buffer);
 671 }
 672
 673 /* Empty message indicates unrecognised command */
 674 static void send_empty_msg(void)
 675 {
 676         put_packet("");
 677 }
 678
 679 /* Read memory due to 'm' message */
 680 static void read_mem_msg(void)
 681 {
 682         char *ptr;
 683         int addr;
 684         int length;
 685
 686         /* Jmp, disable bus error handler */
 687         if (setjmp(rem_com_env) == 0) {
 688
 689                 kgdb_nofault = 1;
 690
 691                 /* Walk through, have m<addr>,<length> */
 692                 ptr = &in_buffer[1];
 693                 if (hex_to_int(&ptr, &addr) && (*ptr++ == ','))
 694                         if (hex_to_int(&ptr, &length)) {
 695                                 ptr = 0;
 696                                 if (length * 2 > OUTBUFMAX)
 697                                         length = OUTBUFMAX / 2;
 698                                 mem_to_hex((char *) addr, out_buffer, length);
 699                         }
 700                 if (ptr)
 701                         send_err_msg();
 702                 else
 703                         put_packet(out_buffer);
 704         } else
 705                 send_err_msg();
 706
 707         /* Restore bus error handler */
 708         kgdb_nofault = 0;
 709 }
 710
 711 /* Write memory due to 'M' or 'X' message */
 712 static void write_mem_msg(int binary)
 713 {
 714         char *ptr;
 715         int addr;
 716         int length;
 717
 718         if (setjmp(rem_com_env) == 0) {
 719
 720                 kgdb_nofault = 1;
 721
 722                 /* Walk through, have M<addr>,<length>:<data> */
 723                 ptr = &in_buffer[1];
 724                 if (hex_to_int(&ptr, &addr) && (*ptr++ == ','))
 725                         if (hex_to_int(&ptr, &length) && (*ptr++ == ':')) {
 726                                 if (binary)
 727                                         ebin_to_mem(ptr, (char*)addr, length);
 728                                 else
 729                                         hex_to_mem(ptr, (char*)addr, length);
 730                                 flush_icache_range(addr, addr + length);
 731                                 ptr = 0;
 732                                 send_ok_msg();
 733                         }
 734                 if (ptr)
 735                         send_err_msg();
 736         } else
 737                 send_err_msg();
 738
 739         /* Restore bus error handler */
 740         kgdb_nofault = 0;
 741 }
 742
 743 /* Continue message  */
 744 static void continue_msg(void)
 745 {
 746         /* Try to read optional parameter, PC unchanged if none */
 747         char *ptr = &in_buffer[1];
 748         int addr;
 749
 750         if (hex_to_int(&ptr, &addr))
 751                 trap_registers.pc = addr;
 752 }
 753
 754 /* Continue message with signal */
 755 static void continue_with_sig_msg(void)
 756 {
 757         int signal;
 758         char *ptr = &in_buffer[1];
 759         int addr;
 760
 761         /* Report limitation */
 762         kgdb_to_gdb("Cannot force signal in kgdb, continuing anyway.\n");
 763
 764         /* Signal */
 765         hex_to_int(&ptr, &signal);
 766         if (*ptr == ';')
 767                 ptr++;
 768
 769         /* Optional address */
 770         if (hex_to_int(&ptr, &addr))
 771                 trap_registers.pc = addr;
 772 }
 773
 774 /* Step message */
 775 static void step_msg(void)
 776 {
 777         continue_msg();
 778         do_single_step();
 779 }
 780
 781 /* Step message with signal */
 782 static void step_with_sig_msg(void)
 783 {
 784         continue_with_sig_msg();
 785         do_single_step();
 786 }
 787
 788 /* Send register contents */
 789 static void send_regs_msg(void)
 790 {
 791         kgdb_regs_to_gdb_regs(&trap_registers, registers);
 792         mem_to_hex((char *) registers, out_buffer, NUMREGBYTES);
 793         put_packet(out_buffer);
 794 }
 795
 796 /* Set register contents - currently can't set other thread's registers */
 797 static void set_regs_msg(void)
 798 {
 799         kgdb_regs_to_gdb_regs(&trap_registers, registers);
 800         hex_to_mem(&in_buffer[1], (char *) registers, NUMREGBYTES);
 801         gdb_regs_to_kgdb_regs(registers, &trap_registers);
 802         send_ok_msg();
 803 }
 804
 805 #ifdef CONFIG_SH_KGDB_CONSOLE
 806 /*
 807  * Bring up the ports..
 808  */
 809 static int __init kgdb_serial_setup(void)
 810 {
 811         struct console dummy;
 812         return kgdb_console_setup(&dummy, 0);
 813 }
 814 #else
 815 #define kgdb_serial_setup()     0
 816 #endif
 817
 818 /* The command loop, read and act on requests */
 819 static void kgdb_command_loop(const int excep_code, const int trapa_value)
 820 {
 821         int sigval;
 822
 823         /* Enter GDB mode (e.g. after detach) */
 824         if (!kgdb_in_gdb_mode) {
 825                 /* Do serial setup, notify user, issue preemptive ack */
 826                 printk(KERN_NOTICE "KGDB: Waiting for GDB\n");
 827                 kgdb_in_gdb_mode = 1;
 828                 put_debug_char('+');
 829         }
 830
 831         /* Reply to host that an exception has occurred */
 832         sigval = compute_signal(excep_code);
 833         send_signal_msg(sigval);
 834
 835         /* TRAP_VEC exception indicates a software trap inserted in place of
 836            code by GDB so back up PC by one instruction, as this instruction
 837            will later be replaced by its original one.  Do NOT do this for
 838            trap 0xff, since that indicates a compiled-in breakpoint which
 839            will not be replaced (and we would retake the trap forever) */
 840         if ((excep_code == TRAP_VEC) && (trapa_value != (0x3c << 2)))
 841                 trap_registers.pc -= 2;
 842
 843         /* Undo any stepping we may have done */
 844         undo_single_step();
 845
 846         while (1) {
 847                 out_buffer[0] = 0;
 848                 get_packet(in_buffer, BUFMAX);
 849
 850                 /* Examine first char of buffer to see what we need to do */
 851                 switch (in_buffer[0]) {
 852                 case '?':       /* Send which signal we've received */
 853                         send_signal_msg(sigval);
 854                         break;
 855
 856                 case 'g':       /* Return the values of the CPU registers */
 857                         send_regs_msg();
 858                         break;
 859
 860                 case 'G':       /* Set the value of the CPU registers */
 861                         set_regs_msg();
 862                         break;
 863
 864                 case 'm':       /* Read LLLL bytes address AA..AA */
 865                         read_mem_msg();
 866                         break;
 867
 868                 case 'M':       /* Write LLLL bytes address AA..AA, ret OK */
 869                         write_mem_msg(0);       /* 0 = data in hex */
 870                         break;
 871
 872                 case 'X':       /* Write LLLL bytes esc bin address AA..AA */
 873                         if (kgdb_bits == '8')
 874                                 write_mem_msg(1); /* 1 = data in binary */
 875                         else
 876                                 send_empty_msg();
 877                         break;
 878
 879                 case 'C':       /* Continue, signum included, we ignore it */
 880                         continue_with_sig_msg();
 881                         return;
 882
 883                 case 'c':       /* Continue at address AA..AA (optional) */
 884                         continue_msg();
 885                         return;
 886
 887                 case 'S':       /* Step, signum included, we ignore it */
 888                         step_with_sig_msg();
 889                         return;
 890
 891                 case 's':       /* Step one instruction from AA..AA */
 892                         step_msg();
 893                         return;
 894
 895                 case 'k':       /* 'Kill the program' with a kernel ? */
 896                         break;
 897
 898                 case 'D':       /* Detach from program, send reply OK */
 899                         kgdb_in_gdb_mode = 0;
 900                         send_ok_msg();
 901                         get_debug_char();
 902                         return;
 903
 904                 default:
 905                         send_empty_msg();
 906                         break;
 907                 }
 908         }
 909 }
 910
 911 /* There has been an exception, most likely a breakpoint. */
 912 static void handle_exception(struct pt_regs *regs)
 913 {
 914         int excep_code, vbr_val;
 915         int count;
 916         int trapa_value = ctrl_inl(TRA);
 917
 918         /* Copy kernel regs (from stack) */
 919         for (count = 0; count < 16; count++)
 920                 trap_registers.regs[count] = regs->regs[count];
 921         trap_registers.pc = regs->pc;
 922         trap_registers.pr = regs->pr;
 923         trap_registers.sr = regs->sr;
 924         trap_registers.gbr = regs->gbr;
 925         trap_registers.mach = regs->mach;
 926         trap_registers.macl = regs->macl;
 927
 928         asm("stc vbr, %0":"=r"(vbr_val));
 929         trap_registers.vbr = vbr_val;
 930
 931         /* Get excode for command loop call, user access */
 932         asm("stc r2_bank, %0":"=r"(excep_code));
 933
 934         /* Act on the exception */
 935         kgdb_command_loop(excep_code, trapa_value);
 936
 937         /* Copy back the (maybe modified) registers */
 938         for (count = 0; count < 16; count++)
 939                 regs->regs[count] = trap_registers.regs[count];
 940         regs->pc = trap_registers.pc;
 941         regs->pr = trap_registers.pr;
 942         regs->sr = trap_registers.sr;
 943         regs->gbr = trap_registers.gbr;
 944         regs->mach = trap_registers.mach;
 945         regs->macl = trap_registers.macl;
 946
 947         vbr_val = trap_registers.vbr;
 948         asm("ldc %0, vbr": :"r"(vbr_val));
 949 }
 950
 951 asmlinkage void kgdb_handle_exception(unsigned long r4, unsigned long r5,
 952                                       unsigned long r6, unsigned long r7,
 953                                       struct pt_regs __regs)
 954 {
 955         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
 956         handle_exception(regs);
 957 }
 958
 959 /* Initialise the KGDB data structures and serial configuration */
 960 int __init kgdb_init(void)
 961 {
 962         in_nmi = 0;
 963         kgdb_nofault = 0;
 964         stepped_opcode = 0;
 965         kgdb_in_gdb_mode = 0;
 966
 967         if (kgdb_serial_setup() != 0) {
 968                 printk(KERN_NOTICE "KGDB: serial setup error\n");
 969                 return -1;
 970         }
 971
 972         /* Init ptr to exception handler */
 973         kgdb_debug_hook = handle_exception;
 974         kgdb_bus_err_hook = kgdb_handle_bus_error;
 975
 976         /* Enter kgdb now if requested, or just report init done */
 977         printk(KERN_NOTICE "KGDB: stub is initialized.\n");
 978
 979         return 0;
 980 }
 981
 982 /* Make function available for "user messages"; console will use it too. */
 983
 984 char gdbmsgbuf[BUFMAX];
 985 #define MAXOUT ((BUFMAX-2)/2)
 986
 987 static void kgdb_msg_write(const char *s, unsigned count)
 988 {
 989         int i;
 990         int wcount;
 991         char *bufptr;
 992
 993         /* 'O'utput */
 994         gdbmsgbuf[0] = 'O';
 995
 996         /* Fill and send buffers... */
 997         while (count > 0) {
 998                 bufptr = gdbmsgbuf + 1;
 999
1000                 /* Calculate how many this time */
1001                 wcount = (count > MAXOUT) ? MAXOUT : count;
1002
1003                 /* Pack in hex chars */
1004                 for (i = 0; i < wcount; i++)
1005                         bufptr = pack_hex_byte(bufptr, s[i]);
1006                 *bufptr = '\0';
1007
1008                 /* Move up */
1009                 s += wcount;
1010                 count -= wcount;
1011
1012                 /* Write packet */
1013                 put_packet(gdbmsgbuf);
1014         }
1015 }
1016
1017 static void kgdb_to_gdb(const char *s)
1018 {
1019         kgdb_msg_write(s, strlen(s));
1020 }
1021
1022 #ifdef CONFIG_SH_KGDB_CONSOLE
1023 void kgdb_console_write(struct console *co, const char *s, unsigned count)
1024 {
1025         /* Bail if we're not talking to GDB */
1026         if (!kgdb_in_gdb_mode)
1027                 return;
1028
1029         kgdb_msg_write(s, count);
1030 }
1031 #endif
1032
1033 #ifdef CONFIG_KGDB_SYSRQ
1034 static void sysrq_handle_gdb(int key, struct tty_struct *tty)
1035 {
1036         printk("Entering GDB stub\n");
1037         breakpoint();
1038 }
1039
1040 static struct sysrq_key_op sysrq_gdb_op = {
1041         .handler        = sysrq_handle_gdb,
1042         .help_msg       = "Gdb",
1043         .action_msg     = "GDB",
1044 };
1045
1046 static int gdb_register_sysrq(void)
1047 {
1048         printk("Registering GDB sysrq handler\n");
1049         register_sysrq_key('g', &sysrq_gdb_op);
1050         return 0;
1051 }
1052 module_init(gdb_register_sysrq);
1053 #endif