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