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