err.no 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  * Containes 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 repititions 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 #ifdef CONFIG_KGDB_THREAD
 247 static struct task_struct *trapped_thread;
 248 static struct task_struct *current_thread;
 249 typedef unsigned char threadref[8];
 250 #define BUF_THREAD_ID_SIZE 16
 251 #endif
 252
 253
 254 /* Convert ch to hex */
 255 static int hex(const char ch)
 256 {
 257         if ((ch >= 'a') && (ch <= 'f'))
 258                 return (ch - 'a' + 10);
 259         if ((ch >= '0') && (ch <= '9'))
 260                 return (ch - '0');
 261         if ((ch >= 'A') && (ch <= 'F'))
 262                 return (ch - 'A' + 10);
 263         return (-1);
 264 }
 265
 266 /* Convert the memory pointed to by mem into hex, placing result in buf.
 267    Returns a pointer to the last char put in buf (null) */
 268 static char *mem_to_hex(const char *mem, char *buf, const int count)
 269 {
 270         int i;
 271         int ch;
 272         unsigned short s_val;
 273         unsigned long l_val;
 274
 275         /* Check for 16 or 32 */
 276         if (count == 2 && ((long) mem & 1) == 0) {
 277                 s_val = *(unsigned short *) mem;
 278                 mem = (char *) &s_val;
 279         } else if (count == 4 && ((long) mem & 3) == 0) {
 280                 l_val = *(unsigned long *) mem;
 281                 mem = (char *) &l_val;
 282         }
 283         for (i = 0; i < count; i++) {
 284                 ch = *mem++;
 285                 *buf++ = highhex(ch);
 286                 *buf++ = lowhex(ch);
 287         }
 288         *buf = 0;
 289         return (buf);
 290 }
 291
 292 /* Convert the hex array pointed to by buf into binary, to be placed in mem.
 293    Return a pointer to the character after the last byte written */
 294 static char *hex_to_mem(const char *buf, char *mem, const int count)
 295 {
 296         int i;
 297         unsigned char ch;
 298
 299         for (i = 0; i < count; i++) {
 300                 ch = hex(*buf++) << 4;
 301                 ch = ch + hex(*buf++);
 302                 *mem++ = ch;
 303         }
 304         return (mem);
 305 }
 306
 307 /* While finding valid hex chars, convert to an integer, then return it */
 308 static int hex_to_int(char **ptr, int *int_value)
 309 {
 310         int num_chars = 0;
 311         int hex_value;
 312
 313         *int_value = 0;
 314
 315         while (**ptr) {
 316                 hex_value = hex(**ptr);
 317                 if (hex_value >= 0) {
 318                         *int_value = (*int_value << 4) | hex_value;
 319                         num_chars++;
 320                 } else
 321                         break;
 322                 (*ptr)++;
 323         }
 324         return num_chars;
 325 }
 326
 327 /*  Copy the binary array pointed to by buf into mem.  Fix $, #,
 328     and 0x7d escaped with 0x7d.  Return a pointer to the character
 329     after the last byte written. */
 330 static char *ebin_to_mem(const char *buf, char *mem, int count)
 331 {
 332         for (; count > 0; count--, buf++) {
 333                 if (*buf == 0x7d)
 334                         *mem++ = *(++buf) ^ 0x20;
 335                 else
 336                         *mem++ = *buf;
 337         }
 338         return mem;
 339 }
 340
 341 /* Pack a hex byte */
 342 static char *pack_hex_byte(char *pkt, int byte)
 343 {
 344         *pkt++ = hexchars[(byte >> 4) & 0xf];
 345         *pkt++ = hexchars[(byte & 0xf)];
 346         return pkt;
 347 }
 348
 349 #ifdef CONFIG_KGDB_THREAD
 350
 351 /* Pack a thread ID */
 352 static char *pack_threadid(char *pkt, threadref * id)
 353 {
 354         char *limit;
 355         unsigned char *altid;
 356
 357         altid = (unsigned char *) id;
 358
 359         limit = pkt + BUF_THREAD_ID_SIZE;
 360         while (pkt < limit)
 361                 pkt = pack_hex_byte(pkt, *altid++);
 362         return pkt;
 363 }
 364
 365 /* Convert an integer into our threadref */
 366 static void int_to_threadref(threadref * id, const int value)
 367 {
 368         unsigned char *scan = (unsigned char *) id;
 369         int i = 4;
 370
 371         while (i--)
 372                 *scan++ = 0;
 373
 374         *scan++ = (value >> 24) & 0xff;
 375         *scan++ = (value >> 16) & 0xff;
 376         *scan++ = (value >> 8) & 0xff;
 377         *scan++ = (value & 0xff);
 378 }
 379
 380 /* Return a task structure ptr for a particular pid */
 381 static struct task_struct *get_thread(int pid)
 382 {
 383         struct task_struct *thread;
 384
 385         /* Use PID_MAX w/gdb for pid 0 */
 386         if (pid == PID_MAX) pid = 0;
 387
 388         /* First check via PID */
 389         thread = find_task_by_pid(pid);
 390
 391         if (thread)
 392                 return thread;
 393
 394         /* Start at the start */
 395         thread = init_tasks[0];
 396
 397         /* Walk along the linked list of tasks */
 398         do {
 399                 if (thread->pid == pid)
 400                         return thread;
 401                 thread = thread->next_task;
 402         } while (thread != init_tasks[0]);
 403
 404         return NULL;
 405 }
 406
 407 #endif /* CONFIG_KGDB_THREAD */
 408
 409 /* Scan for the start char '$', read the packet and check the checksum */
 410 static void get_packet(char *buffer, int buflen)
 411 {
 412         unsigned char checksum;
 413         unsigned char xmitcsum;
 414         int i;
 415         int count;
 416         char ch;
 417
 418         do {
 419                 /* Ignore everything until the start character */
 420                 while ((ch = get_debug_char()) != '$');
 421
 422                 checksum = 0;
 423                 xmitcsum = -1;
 424                 count = 0;
 425
 426                 /* Now, read until a # or end of buffer is found */
 427                 while (count < (buflen - 1)) {
 428                         ch = get_debug_char();
 429
 430                         if (ch == '#')
 431                                 break;
 432
 433                         checksum = checksum + ch;
 434                         buffer[count] = ch;
 435                         count = count + 1;
 436                 }
 437
 438                 buffer[count] = 0;
 439
 440                 /* Continue to read checksum following # */
 441                 if (ch == '#') {
 442                         xmitcsum = hex(get_debug_char()) << 4;
 443                         xmitcsum += hex(get_debug_char());
 444
 445                         /* Checksum */
 446                         if (checksum != xmitcsum)
 447                                 put_debug_char('-');    /* Failed checksum */
 448                         else {
 449                                 /* Ack successful transfer */
 450                                 put_debug_char('+');
 451
 452                                 /* If a sequence char is present, reply
 453                                    the sequence ID */
 454                                 if (buffer[2] == ':') {
 455                                         put_debug_char(buffer[0]);
 456                                         put_debug_char(buffer[1]);
 457
 458                                         /* Remove sequence chars from buffer */
 459                                         count = strlen(buffer);
 460                                         for (i = 3; i <= count; i++)
 461                                                 buffer[i - 3] = buffer[i];
 462                                 }
 463                         }
 464                 }
 465         }
 466         while (checksum != xmitcsum);   /* Keep trying while we fail */
 467 }
 468
 469 /* Send the packet in the buffer with run-length encoding */
 470 static void put_packet(char *buffer)
 471 {
 472         int checksum;
 473         char *src;
 474         int runlen;
 475         int encode;
 476
 477         do {
 478                 src = buffer;
 479                 put_debug_char('$');
 480                 checksum = 0;
 481
 482                 /* Continue while we still have chars left */
 483                 while (*src) {
 484                         /* Check for runs up to 99 chars long */
 485                         for (runlen = 1; runlen < 99; runlen++) {
 486                                 if (src[0] != src[runlen])
 487                                         break;
 488                         }
 489
 490                         if (runlen > 3) {
 491                                 /* Got a useful amount, send encoding */
 492                                 encode = runlen + ' ' - 4;
 493                                 put_debug_char(*src);   checksum += *src;
 494                                 put_debug_char('*');    checksum += '*';
 495                                 put_debug_char(encode); checksum += encode;
 496                                 src += runlen;
 497                         } else {
 498                                 /* Otherwise just send the current char */
 499                                 put_debug_char(*src);   checksum += *src;
 500                                 src += 1;
 501                         }
 502                 }
 503
 504                 /* '#' Separator, put high and low components of checksum */
 505                 put_debug_char('#');
 506                 put_debug_char(highhex(checksum));
 507                 put_debug_char(lowhex(checksum));
 508         }
 509         while ((get_debug_char()) != '+');      /* While no ack */
 510 }
 511
 512 /* A bus error has occurred - perform a longjmp to return execution and
 513    allow handling of the error */
 514 static void kgdb_handle_bus_error(void)
 515 {
 516         longjmp(rem_com_env, 1);
 517 }
 518
 519 /* Translate SH-3/4 exception numbers to unix-like signal values */
 520 static int compute_signal(const int excep_code)
 521 {
 522         int sigval;
 523
 524         switch (excep_code) {
 525
 526         case INVALID_INSN_VEC:
 527         case INVALID_SLOT_VEC:
 528                 sigval = SIGILL;
 529                 break;
 530         case ADDRESS_ERROR_LOAD_VEC:
 531         case ADDRESS_ERROR_STORE_VEC:
 532                 sigval = SIGSEGV;
 533                 break;
 534
 535         case SERIAL_BREAK_VEC:
 536         case NMI_VEC:
 537                 sigval = SIGINT;
 538                 break;
 539
 540         case USER_BREAK_VEC:
 541         case TRAP_VEC:
 542                 sigval = SIGTRAP;
 543                 break;
 544
 545         default:
 546                 sigval = SIGBUS;        /* "software generated" */
 547                 break;
 548         }
 549
 550         return (sigval);
 551 }
 552
 553 /* Make a local copy of the registers passed into the handler (bletch) */
 554 static void kgdb_regs_to_gdb_regs(const struct kgdb_regs *regs,
 555                                   int *gdb_regs)
 556 {
 557         gdb_regs[R0] = regs->regs[R0];
 558         gdb_regs[R1] = regs->regs[R1];
 559         gdb_regs[R2] = regs->regs[R2];
 560         gdb_regs[R3] = regs->regs[R3];
 561         gdb_regs[R4] = regs->regs[R4];
 562         gdb_regs[R5] = regs->regs[R5];
 563         gdb_regs[R6] = regs->regs[R6];
 564         gdb_regs[R7] = regs->regs[R7];
 565         gdb_regs[R8] = regs->regs[R8];
 566         gdb_regs[R9] = regs->regs[R9];
 567         gdb_regs[R10] = regs->regs[R10];
 568         gdb_regs[R11] = regs->regs[R11];
 569         gdb_regs[R12] = regs->regs[R12];
 570         gdb_regs[R13] = regs->regs[R13];
 571         gdb_regs[R14] = regs->regs[R14];
 572         gdb_regs[R15] = regs->regs[R15];
 573         gdb_regs[PC] = regs->pc;
 574         gdb_regs[PR] = regs->pr;
 575         gdb_regs[GBR] = regs->gbr;
 576         gdb_regs[MACH] = regs->mach;
 577         gdb_regs[MACL] = regs->macl;
 578         gdb_regs[SR] = regs->sr;
 579         gdb_regs[VBR] = regs->vbr;
 580 }
 581
 582 /* Copy local gdb registers back to kgdb regs, for later copy to kernel */
 583 static void gdb_regs_to_kgdb_regs(const int *gdb_regs,
 584                                   struct kgdb_regs *regs)
 585 {
 586         regs->regs[R0] = gdb_regs[R0];
 587         regs->regs[R1] = gdb_regs[R1];
 588         regs->regs[R2] = gdb_regs[R2];
 589         regs->regs[R3] = gdb_regs[R3];
 590         regs->regs[R4] = gdb_regs[R4];
 591         regs->regs[R5] = gdb_regs[R5];
 592         regs->regs[R6] = gdb_regs[R6];
 593         regs->regs[R7] = gdb_regs[R7];
 594         regs->regs[R8] = gdb_regs[R8];
 595         regs->regs[R9] = gdb_regs[R9];
 596         regs->regs[R10] = gdb_regs[R10];
 597         regs->regs[R11] = gdb_regs[R11];
 598         regs->regs[R12] = gdb_regs[R12];
 599         regs->regs[R13] = gdb_regs[R13];
 600         regs->regs[R14] = gdb_regs[R14];
 601         regs->regs[R15] = gdb_regs[R15];
 602         regs->pc = gdb_regs[PC];
 603         regs->pr = gdb_regs[PR];
 604         regs->gbr = gdb_regs[GBR];
 605         regs->mach = gdb_regs[MACH];
 606         regs->macl = gdb_regs[MACL];
 607         regs->sr = gdb_regs[SR];
 608         regs->vbr = gdb_regs[VBR];
 609 }
 610
 611 #ifdef CONFIG_KGDB_THREAD
 612 /* Make a local copy of registers from the specified thread */
 613 asmlinkage void ret_from_fork(void);
 614 static void thread_regs_to_gdb_regs(const struct task_struct *thread,
 615                                     int *gdb_regs)
 616 {
 617         int regno;
 618         int *tregs;
 619
 620         /* Initialize to zero */
 621         for (regno = 0; regno < MAXREG; regno++)
 622                 gdb_regs[regno] = 0;
 623
 624         /* Just making sure... */
 625         if (thread == NULL)
 626                 return;
 627
 628         /* A new fork has pt_regs on the stack from a fork() call */
 629         if (thread->thread.pc == (unsigned long)ret_from_fork) {
 630
 631                 int vbr_val;
 632                 struct pt_regs *kregs;
 633                 kregs = (struct pt_regs*)thread->thread.sp;
 634
 635                 gdb_regs[R0] = kregs->regs[R0];
 636                 gdb_regs[R1] = kregs->regs[R1];
 637                 gdb_regs[R2] = kregs->regs[R2];
 638                 gdb_regs[R3] = kregs->regs[R3];
 639                 gdb_regs[R4] = kregs->regs[R4];
 640                 gdb_regs[R5] = kregs->regs[R5];
 641                 gdb_regs[R6] = kregs->regs[R6];
 642                 gdb_regs[R7] = kregs->regs[R7];
 643                 gdb_regs[R8] = kregs->regs[R8];
 644                 gdb_regs[R9] = kregs->regs[R9];
 645                 gdb_regs[R10] = kregs->regs[R10];
 646                 gdb_regs[R11] = kregs->regs[R11];
 647                 gdb_regs[R12] = kregs->regs[R12];
 648                 gdb_regs[R13] = kregs->regs[R13];
 649                 gdb_regs[R14] = kregs->regs[R14];
 650                 gdb_regs[R15] = kregs->regs[R15];
 651                 gdb_regs[PC] = kregs->pc;
 652                 gdb_regs[PR] = kregs->pr;
 653                 gdb_regs[GBR] = kregs->gbr;
 654                 gdb_regs[MACH] = kregs->mach;
 655                 gdb_regs[MACL] = kregs->macl;
 656                 gdb_regs[SR] = kregs->sr;
 657
 658                 asm("stc vbr, %0":"=r"(vbr_val));
 659                 gdb_regs[VBR] = vbr_val;
 660                 return;
 661         }
 662
 663         /* Otherwise, we have only some registers from switch_to() */
 664         tregs = (int *)thread->thread.sp;
 665         gdb_regs[R15] = (int)tregs;
 666         gdb_regs[R14] = *tregs++;
 667         gdb_regs[R13] = *tregs++;
 668         gdb_regs[R12] = *tregs++;
 669         gdb_regs[R11] = *tregs++;
 670         gdb_regs[R10] = *tregs++;
 671         gdb_regs[R9] = *tregs++;
 672         gdb_regs[R8] = *tregs++;
 673         gdb_regs[PR] = *tregs++;
 674         gdb_regs[GBR] = *tregs++;
 675         gdb_regs[PC] = thread->thread.pc;
 676 }
 677 #endif /* CONFIG_KGDB_THREAD */
 678
 679 /* Calculate the new address for after a step */
 680 static short *get_step_address(void)
 681 {
 682         short op = *(short *) trap_registers.pc;
 683         long addr;
 684
 685         /* BT */
 686         if (OPCODE_BT(op)) {
 687                 if (trap_registers.sr & SR_T_BIT_MASK)
 688                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 689                 else
 690                         addr = trap_registers.pc + 2;
 691         }
 692
 693         /* BTS */
 694         else if (OPCODE_BTS(op)) {
 695                 if (trap_registers.sr & SR_T_BIT_MASK)
 696                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 697                 else
 698                         addr = trap_registers.pc + 4;   /* Not in delay slot */
 699         }
 700
 701         /* BF */
 702         else if (OPCODE_BF(op)) {
 703                 if (!(trap_registers.sr & SR_T_BIT_MASK))
 704                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 705                 else
 706                         addr = trap_registers.pc + 2;
 707         }
 708
 709         /* BFS */
 710         else if (OPCODE_BFS(op)) {
 711                 if (!(trap_registers.sr & SR_T_BIT_MASK))
 712                         addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
 713                 else
 714                         addr = trap_registers.pc + 4;   /* Not in delay slot */
 715         }
 716
 717         /* BRA */
 718         else if (OPCODE_BRA(op))
 719                 addr = trap_registers.pc + 4 + OPCODE_BRA_DISP(op);
 720
 721         /* BRAF */
 722         else if (OPCODE_BRAF(op))
 723                 addr = trap_registers.pc + 4
 724                     + trap_registers.regs[OPCODE_BRAF_REG(op)];
 725
 726         /* BSR */
 727         else if (OPCODE_BSR(op))
 728                 addr = trap_registers.pc + 4 + OPCODE_BSR_DISP(op);
 729
 730         /* BSRF */
 731         else if (OPCODE_BSRF(op))
 732                 addr = trap_registers.pc + 4
 733                     + trap_registers.regs[OPCODE_BSRF_REG(op)];
 734
 735         /* JMP */
 736         else if (OPCODE_JMP(op))
 737                 addr = trap_registers.regs[OPCODE_JMP_REG(op)];
 738
 739         /* JSR */
 740         else if (OPCODE_JSR(op))
 741                 addr = trap_registers.regs[OPCODE_JSR_REG(op)];
 742
 743         /* RTS */
 744         else if (OPCODE_RTS(op))
 745                 addr = trap_registers.pr;
 746
 747         /* RTE */
 748         else if (OPCODE_RTE(op))
 749                 addr = trap_registers.regs[15];
 750
 751         /* Other */
 752         else
 753                 addr = trap_registers.pc + 2;
 754
 755         kgdb_flush_icache_range(addr, addr + 2);
 756         return (short *) addr;
 757 }
 758
 759 /* Set up a single-step.  Replace the instruction immediately after the
 760    current instruction (i.e. next in the expected flow of control) with a
 761    trap instruction, so that returning will cause only a single instruction
 762    to be executed. Note that this model is slightly broken for instructions
 763    with delay slots (e.g. B[TF]S, BSR, BRA etc), where both the branch
 764    and the instruction in the delay slot will be executed. */
 765 static void do_single_step(void)
 766 {
 767         unsigned short *addr = 0;
 768
 769         /* Determine where the target instruction will send us to */
 770         addr = get_step_address();
 771         stepped_address = (int)addr;
 772
 773         /* Replace it */
 774         stepped_opcode = *(short *)addr;
 775         *addr = STEP_OPCODE;
 776
 777         /* Flush and return */
 778         kgdb_flush_icache_range((long) addr, (long) addr + 2);
 779         return;
 780 }
 781
 782 /* Undo a single step */
 783 static void undo_single_step(void)
 784 {
 785         /* If we have stepped, put back the old instruction */
 786         /* Use stepped_address in case we stopped elsewhere */
 787         if (stepped_opcode != 0) {
 788                 *(short*)stepped_address = stepped_opcode;
 789                 kgdb_flush_icache_range(stepped_address, stepped_address + 2);
 790         }
 791         stepped_opcode = 0;
 792 }
 793
 794 /* Send a signal message */
 795 static void send_signal_msg(const int signum)
 796 {
 797 #ifndef CONFIG_KGDB_THREAD
 798         out_buffer[0] = 'S';
 799         out_buffer[1] = highhex(signum);
 800         out_buffer[2] = lowhex(signum);
 801         out_buffer[3] = 0;
 802         put_packet(out_buffer);
 803 #else /* CONFIG_KGDB_THREAD */
 804         int threadid;
 805         threadref thref;
 806         char *out = out_buffer;
 807         const char *tstring = "thread";
 808
 809         *out++ = 'T';
 810         *out++ = highhex(signum);
 811         *out++ = lowhex(signum);
 812
 813         while (*tstring) {
 814                 *out++ = *tstring++;
 815         }
 816         *out++ = ':';
 817
 818         threadid = trapped_thread->pid;
 819         if (threadid == 0) threadid = PID_MAX;
 820         int_to_threadref(&thref, threadid);
 821         pack_threadid(out, &thref);
 822         out += BUF_THREAD_ID_SIZE;
 823         *out++ = ';';
 824
 825         *out = 0;
 826         put_packet(out_buffer);
 827 #endif /* CONFIG_KGDB_THREAD */
 828 }
 829
 830 /* Reply that all was well */
 831 static void send_ok_msg(void)
 832 {
 833         strcpy(out_buffer, "OK");
 834         put_packet(out_buffer);
 835 }
 836
 837 /* Reply that an error occurred */
 838 static void send_err_msg(void)
 839 {
 840         strcpy(out_buffer, "E01");
 841         put_packet(out_buffer);
 842 }
 843
 844 /* Empty message indicates unrecognised command */
 845 static void send_empty_msg(void)
 846 {
 847         put_packet("");
 848 }
 849
 850 /* Read memory due to 'm' message */
 851 static void read_mem_msg(void)
 852 {
 853         char *ptr;
 854         int addr;
 855         int length;
 856
 857         /* Jmp, disable bus error handler */
 858         if (setjmp(rem_com_env) == 0) {
 859
 860                 kgdb_nofault = 1;
 861
 862                 /* Walk through, have m<addr>,<length> */
 863                 ptr = &in_buffer[1];
 864                 if (hex_to_int(&ptr, &addr) && (*ptr++ == ','))
 865                         if (hex_to_int(&ptr, &length)) {
 866                                 ptr = 0;
 867                                 if (length * 2 > OUTBUFMAX)
 868                                         length = OUTBUFMAX / 2;
 869                                 mem_to_hex((char *) addr, out_buffer, length);
 870                         }
 871                 if (ptr)
 872                         send_err_msg();
 873                 else
 874                         put_packet(out_buffer);
 875         } else
 876                 send_err_msg();
 877
 878         /* Restore bus error handler */
 879         kgdb_nofault = 0;
 880 }
 881
 882 /* Write memory due to 'M' or 'X' message */
 883 static void write_mem_msg(int binary)
 884 {
 885         char *ptr;
 886         int addr;
 887         int length;
 888
 889         if (setjmp(rem_com_env) == 0) {
 890
 891                 kgdb_nofault = 1;
 892
 893                 /* Walk through, have M<addr>,<length>:<data> */
 894                 ptr = &in_buffer[1];
 895                 if (hex_to_int(&ptr, &addr) && (*ptr++ == ','))
 896                         if (hex_to_int(&ptr, &length) && (*ptr++ == ':')) {
 897                                 if (binary)
 898                                         ebin_to_mem(ptr, (char*)addr, length);
 899                                 else
 900                                         hex_to_mem(ptr, (char*)addr, length);
 901                                 kgdb_flush_icache_range(addr, addr + length);
 902                                 ptr = 0;
 903                                 send_ok_msg();
 904                         }
 905                 if (ptr)
 906                         send_err_msg();
 907         } else
 908                 send_err_msg();
 909
 910         /* Restore bus error handler */
 911         kgdb_nofault = 0;
 912 }
 913
 914 /* Continue message  */
 915 static void continue_msg(void)
 916 {
 917         /* Try to read optional parameter, PC unchanged if none */
 918         char *ptr = &in_buffer[1];
 919         int addr;
 920
 921         if (hex_to_int(&ptr, &addr))
 922                 trap_registers.pc = addr;
 923 }
 924
 925 /* Continue message with signal */
 926 static void continue_with_sig_msg(void)
 927 {
 928         int signal;
 929         char *ptr = &in_buffer[1];
 930         int addr;
 931
 932         /* Report limitation */
 933         kgdb_to_gdb("Cannot force signal in kgdb, continuing anyway.\n");
 934
 935         /* Signal */
 936         hex_to_int(&ptr, &signal);
 937         if (*ptr == ';')
 938                 ptr++;
 939
 940         /* Optional address */
 941         if (hex_to_int(&ptr, &addr))
 942                 trap_registers.pc = addr;
 943 }
 944
 945 /* Step message */
 946 static void step_msg(void)
 947 {
 948         continue_msg();
 949         do_single_step();
 950 }
 951
 952 /* Step message with signal */
 953 static void step_with_sig_msg(void)
 954 {
 955         continue_with_sig_msg();
 956         do_single_step();
 957 }
 958
 959 /* Send register contents */
 960 static void send_regs_msg(void)
 961 {
 962 #ifdef CONFIG_KGDB_THREAD
 963         if (!current_thread)
 964                 kgdb_regs_to_gdb_regs(&trap_registers, registers);
 965         else
 966                 thread_regs_to_gdb_regs(current_thread, registers);
 967 #else
 968         kgdb_regs_to_gdb_regs(&trap_registers, registers);
 969 #endif
 970
 971         mem_to_hex((char *) registers, out_buffer, NUMREGBYTES);
 972         put_packet(out_buffer);
 973 }
 974
 975 /* Set register contents - currently can't set other thread's registers */
 976 static void set_regs_msg(void)
 977 {
 978 #ifdef CONFIG_KGDB_THREAD
 979         if (!current_thread) {
 980 #endif
 981                 kgdb_regs_to_gdb_regs(&trap_registers, registers);
 982                 hex_to_mem(&in_buffer[1], (char *) registers, NUMREGBYTES);
 983                 gdb_regs_to_kgdb_regs(registers, &trap_registers);
 984                 send_ok_msg();
 985 #ifdef CONFIG_KGDB_THREAD
 986         } else
 987                 send_err_msg();
 988 #endif
 989 }
 990
 991
 992 #ifdef CONFIG_KGDB_THREAD
 993
 994 /* Set the status for a thread */
 995 void set_thread_msg(void)
 996 {
 997         int threadid;
 998         struct task_struct *thread = NULL;
 999         char *ptr;
1000
1001         switch (in_buffer[1]) {
1002         /* To select which thread for gG etc messages, i.e. supported */
1003         case 'g':
1004                 ptr = &in_buffer[2];
1005                 hex_to_int(&ptr, &threadid);
1006                 thread = get_thread(threadid);
1007
1008                 /* If we haven't found it */
1009                 if (!thread) {
1010                         send_err_msg();
1011                         break;
1012                 }
1013
1014                 /* Set current_thread (or not) */
1015                 if (thread == trapped_thread)
1016                         current_thread = NULL;
1017                 else
1018                         current_thread = thread;
1019                 send_ok_msg();
1020                 break;
1021
1022         /* To select which thread for cCsS messages, i.e. unsupported */
1023         case 'c':
1024                 send_ok_msg();
1025                 break;
1026
1027         default:
1028                 send_empty_msg();
1029                 break;
1030         }
1031 }
1032
1033 /* Is a thread alive? */
1034 static void thread_status_msg(void)
1035 {
1036         char *ptr;
1037         int threadid;
1038         struct task_struct *thread = NULL;
1039
1040         ptr = &in_buffer[1];
1041         hex_to_int(&ptr, &threadid);
1042         thread = get_thread(threadid);
1043         if (thread)
1044                 send_ok_msg();
1045         else
1046                 send_err_msg();
1047 }
1048 /* Send the current thread ID */
1049 static void thread_id_msg(void)
1050 {
1051         int threadid;
1052         threadref thref;
1053
1054         out_buffer[0] = 'Q';
1055         out_buffer[1] = 'C';
1056
1057         if (current_thread)
1058                 threadid = current_thread->pid;
1059         else if (trapped_thread)
1060                 threadid = trapped_thread->pid;
1061         else /* Impossible, but just in case! */
1062         {
1063                 send_err_msg();
1064                 return;
1065         }
1066
1067         /* Translate pid 0 to PID_MAX for gdb */
1068         if (threadid == 0) threadid = PID_MAX;
1069
1070         int_to_threadref(&thref, threadid);
1071         pack_threadid(out_buffer + 2, &thref);
1072         out_buffer[2 + BUF_THREAD_ID_SIZE] = '\0';
1073         put_packet(out_buffer);
1074 }
1075
1076 /* Send thread info */
1077 static void thread_info_msg(void)
1078 {
1079         struct task_struct *thread = NULL;
1080         int threadid;
1081         char *pos;
1082         threadref thref;
1083
1084         /* Start with 'm' */
1085         out_buffer[0] = 'm';
1086         pos = &out_buffer[1];
1087
1088         /* For all possible thread IDs - this will overrun if > 44 threads! */
1089         /* Start at 1 and include PID_MAX (since GDB won't use pid 0...) */
1090         for (threadid = 1; threadid <= PID_MAX; threadid++) {
1091
1092                 read_lock(&tasklist_lock);
1093                 thread = get_thread(threadid);
1094                 read_unlock(&tasklist_lock);
1095
1096                 /* If it's a valid thread */
1097                 if (thread) {
1098                         int_to_threadref(&thref, threadid);
1099                         pack_threadid(pos, &thref);
1100                         pos += BUF_THREAD_ID_SIZE;
1101                         *pos++ = ',';
1102                 }
1103         }
1104         *--pos = 0;             /* Lose final comma */
1105         put_packet(out_buffer);
1106
1107 }
1108
1109 /* Return printable info for gdb's 'info threads' command */
1110 static void thread_extra_info_msg(void)
1111 {
1112         int threadid;
1113         struct task_struct *thread = NULL;
1114         char buffer[20], *ptr;
1115         int i;
1116
1117         /* Extract thread ID */
1118         ptr = &in_buffer[17];
1119         hex_to_int(&ptr, &threadid);
1120         thread = get_thread(threadid);
1121
1122         /* If we don't recognise it, say so */
1123         if (thread == NULL)
1124                 strcpy(buffer, "(unknown)");
1125         else
1126                 strcpy(buffer, thread->comm);
1127
1128         /* Construct packet */
1129         for (i = 0, ptr = out_buffer; buffer[i]; i++)
1130                 ptr = pack_hex_byte(ptr, buffer[i]);
1131
1132         if (thread->thread.pc == (unsigned long)ret_from_fork) {
1133                 strcpy(buffer, "<new fork>");
1134                 for (i = 0; buffer[i]; i++)
1135                         ptr = pack_hex_byte(ptr, buffer[i]);
1136         }
1137
1138         *ptr = '\0';
1139         put_packet(out_buffer);
1140 }
1141
1142 /* Handle all qFooBarBaz messages - have to use an if statement as
1143    opposed to a switch because q messages can have > 1 char id. */
1144 static void query_msg(void)
1145 {
1146         const char *q_start = &in_buffer[1];
1147
1148         /* qC = return current thread ID */
1149         if (strncmp(q_start, "C", 1) == 0)
1150                 thread_id_msg();
1151
1152         /* qfThreadInfo = query all threads (first) */
1153         else if (strncmp(q_start, "fThreadInfo", 11) == 0)
1154                 thread_info_msg();
1155
1156         /* qsThreadInfo = query all threads (subsequent). We know we have sent
1157            them all after the qfThreadInfo message, so there are no to send */
1158         else if (strncmp(q_start, "sThreadInfo", 11) == 0)
1159                 put_packet("l");        /* el = last */
1160
1161         /* qThreadExtraInfo = supply printable information per thread */
1162         else if (strncmp(q_start, "ThreadExtraInfo", 15) == 0)
1163                 thread_extra_info_msg();
1164
1165         /* Unsupported - empty message as per spec */
1166         else
1167                 send_empty_msg();
1168 }
1169 #endif /* CONFIG_KGDB_THREAD */
1170
1171 #ifdef CONFIG_SH_KGDB_CONSOLE
1172 /*
1173  * Bring up the ports..
1174  */
1175 static int kgdb_serial_setup(void)
1176 {
1177         extern int kgdb_console_setup(struct console *co, char *options);
1178         struct console dummy;
1179
1180         kgdb_console_setup(&dummy, 0);
1181
1182         return 0;
1183 }
1184 #else
1185 #define kgdb_serial_setup()     0
1186 #endif
1187
1188 /* The command loop, read and act on requests */
1189 static void kgdb_command_loop(const int excep_code, const int trapa_value)
1190 {
1191         int sigval;
1192
1193         if (excep_code == NMI_VEC) {
1194 #ifndef CONFIG_KGDB_NMI
1195                 printk(KERN_NOTICE "KGDB: Ignoring unexpected NMI?\n");
1196                 return;
1197 #else /* CONFIG_KGDB_NMI */
1198                 if (!kgdb_enabled) {
1199                         kgdb_enabled = 1;
1200                         kgdb_init();
1201                 }
1202 #endif /* CONFIG_KGDB_NMI */
1203         }
1204
1205         /* Ignore if we're disabled */
1206         if (!kgdb_enabled)
1207                 return;
1208
1209 #ifdef CONFIG_KGDB_THREAD
1210         /* Until GDB specifies a thread */
1211         current_thread = NULL;
1212         trapped_thread = current;
1213 #endif
1214
1215         /* Enter GDB mode (e.g. after detach) */
1216         if (!kgdb_in_gdb_mode) {
1217                 /* Do serial setup, notify user, issue preemptive ack */
1218                 printk(KERN_NOTICE "KGDB: Waiting for GDB\n");
1219                 kgdb_in_gdb_mode = 1;
1220                 put_debug_char('+');
1221         }
1222
1223         /* Reply to host that an exception has occurred */
1224         sigval = compute_signal(excep_code);
1225         send_signal_msg(sigval);
1226
1227         /* TRAP_VEC exception indicates a software trap inserted in place of
1228            code by GDB so back up PC by one instruction, as this instruction
1229            will later be replaced by its original one.  Do NOT do this for
1230            trap 0xff, since that indicates a compiled-in breakpoint which
1231            will not be replaced (and we would retake the trap forever) */
1232         if ((excep_code == TRAP_VEC) && (trapa_value != (0x3c << 2)))
1233                 trap_registers.pc -= 2;
1234
1235         /* Undo any stepping we may have done */
1236         undo_single_step();
1237
1238         while (1) {
1239                 out_buffer[0] = 0;
1240                 get_packet(in_buffer, BUFMAX);
1241
1242                 /* Examine first char of buffer to see what we need to do */
1243                 switch (in_buffer[0]) {
1244                 case '?':       /* Send which signal we've received */
1245                         send_signal_msg(sigval);
1246                         break;
1247
1248                 case 'g':       /* Return the values of the CPU registers */
1249                         send_regs_msg();
1250                         break;
1251
1252                 case 'G':       /* Set the value of the CPU registers */
1253                         set_regs_msg();
1254                         break;
1255
1256                 case 'm':       /* Read LLLL bytes address AA..AA */
1257                         read_mem_msg();
1258                         break;
1259
1260                 case 'M':       /* Write LLLL bytes address AA..AA, ret OK */
1261                         write_mem_msg(0);       /* 0 = data in hex */
1262                         break;
1263
1264                 case 'X':       /* Write LLLL bytes esc bin address AA..AA */
1265                         if (kgdb_bits == '8')
1266                                 write_mem_msg(1); /* 1 = data in binary */
1267                         else
1268                                 send_empty_msg();
1269                         break;
1270
1271                 case 'C':       /* Continue, signum included, we ignore it */
1272                         continue_with_sig_msg();
1273                         return;
1274
1275                 case 'c':       /* Continue at address AA..AA (optional) */
1276                         continue_msg();
1277                         return;
1278
1279                 case 'S':       /* Step, signum included, we ignore it */
1280                         step_with_sig_msg();
1281                         return;
1282
1283                 case 's':       /* Step one instruction from AA..AA */
1284                         step_msg();
1285                         return;
1286
1287 #ifdef CONFIG_KGDB_THREAD
1288
1289                 case 'H':       /* Task related */
1290                         set_thread_msg();
1291                         break;
1292
1293                 case 'T':       /* Query thread status */
1294                         thread_status_msg();
1295                         break;
1296
1297                 case 'q':       /* Handle query - currently thread-related */
1298                         query_msg();
1299                         break;
1300 #endif
1301
1302                 case 'k':       /* 'Kill the program' with a kernel ? */
1303                         break;
1304
1305                 case 'D':       /* Detach from program, send reply OK */
1306                         kgdb_in_gdb_mode = 0;
1307                         send_ok_msg();
1308                         get_debug_char();
1309                         return;
1310
1311                 default:
1312                         send_empty_msg();
1313                         break;
1314                 }
1315         }
1316 }
1317
1318 /* There has been an exception, most likely a breakpoint. */
1319 static void handle_exception(struct pt_regs *regs)
1320 {
1321         int excep_code, vbr_val;
1322         int count;
1323         int trapa_value = ctrl_inl(TRA);
1324
1325         /* Copy kernel regs (from stack) */
1326         for (count = 0; count < 16; count++)
1327                 trap_registers.regs[count] = regs->regs[count];
1328         trap_registers.pc = regs->pc;
1329         trap_registers.pr = regs->pr;
1330         trap_registers.sr = regs->sr;
1331         trap_registers.gbr = regs->gbr;
1332         trap_registers.mach = regs->mach;
1333         trap_registers.macl = regs->macl;
1334
1335         asm("stc vbr, %0":"=r"(vbr_val));
1336         trap_registers.vbr = vbr_val;
1337
1338         /* Get excode for command loop call, user access */
1339         asm("stc r2_bank, %0":"=r"(excep_code));
1340         kgdb_excode = excep_code;
1341
1342         /* Other interesting environment items for reference */
1343         asm("stc r6_bank, %0":"=r"(kgdb_g_imask));
1344         kgdb_current = current;
1345         kgdb_trapa_val = trapa_value;
1346
1347         /* Act on the exception */
1348         kgdb_command_loop(excep_code, trapa_value);
1349
1350         kgdb_current = NULL;
1351
1352         /* Copy back the (maybe modified) registers */
1353         for (count = 0; count < 16; count++)
1354                 regs->regs[count] = trap_registers.regs[count];
1355         regs->pc = trap_registers.pc;
1356         regs->pr = trap_registers.pr;
1357         regs->sr = trap_registers.sr;
1358         regs->gbr = trap_registers.gbr;
1359         regs->mach = trap_registers.mach;
1360         regs->macl = trap_registers.macl;
1361
1362         vbr_val = trap_registers.vbr;
1363         asm("ldc %0, vbr": :"r"(vbr_val));
1364 }
1365
1366 asmlinkage void kgdb_handle_exception(unsigned long r4, unsigned long r5,
1367                                       unsigned long r6, unsigned long r7,
1368                                       struct pt_regs __regs)
1369 {
1370         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
1371         handle_exception(regs);
1372 }
1373
1374 /* Initialise the KGDB data structures and serial configuration */
1375 int kgdb_init(void)
1376 {
1377         if (!kgdb_enabled)
1378                 return 1;
1379
1380         in_nmi = 0;
1381         kgdb_nofault = 0;
1382         stepped_opcode = 0;
1383         kgdb_in_gdb_mode = 0;
1384
1385         if (kgdb_serial_setup() != 0) {
1386                 printk(KERN_NOTICE "KGDB: serial setup error\n");
1387                 return -1;
1388         }
1389
1390         /* Init ptr to exception handler */
1391         kgdb_debug_hook = handle_exception;
1392         kgdb_bus_err_hook = kgdb_handle_bus_error;
1393
1394         /* Enter kgdb now if requested, or just report init done */
1395         printk(KERN_NOTICE "KGDB: stub is initialized.\n");
1396
1397         return 0;
1398 }
1399
1400 /* Make function available for "user messages"; console will use it too. */
1401
1402 char gdbmsgbuf[BUFMAX];
1403 #define MAXOUT ((BUFMAX-2)/2)
1404
1405 static void kgdb_msg_write(const char *s, unsigned count)
1406 {
1407         int i;
1408         int wcount;
1409         char *bufptr;
1410
1411         /* 'O'utput */
1412         gdbmsgbuf[0] = 'O';
1413
1414         /* Fill and send buffers... */
1415         while (count > 0) {
1416                 bufptr = gdbmsgbuf + 1;
1417
1418                 /* Calculate how many this time */
1419                 wcount = (count > MAXOUT) ? MAXOUT : count;
1420
1421                 /* Pack in hex chars */
1422                 for (i = 0; i < wcount; i++)
1423                         bufptr = pack_hex_byte(bufptr, s[i]);
1424                 *bufptr = '\0';
1425
1426                 /* Move up */
1427                 s += wcount;
1428                 count -= wcount;
1429
1430                 /* Write packet */
1431                 put_packet(gdbmsgbuf);
1432         }
1433 }
1434
1435 static void kgdb_to_gdb(const char *s)
1436 {
1437         kgdb_msg_write(s, strlen(s));
1438 }
1439
1440 #ifdef CONFIG_SH_KGDB_CONSOLE
1441 void kgdb_console_write(struct console *co, const char *s, unsigned count)
1442 {
1443         /* Bail if we're not talking to GDB */
1444         if (!kgdb_in_gdb_mode)
1445                 return;
1446
1447         kgdb_msg_write(s, count);
1448 }
1449 #endif
1450
1451 #ifdef CONFIG_KGDB_SYSRQ
1452 static void sysrq_handle_gdb(int key, struct tty_struct *tty)
1453 {
1454         printk("Entering GDB stub\n");
1455         breakpoint();
1456 }
1457
1458 static struct sysrq_key_op sysrq_gdb_op = {
1459         .handler        = sysrq_handle_gdb,
1460         .help_msg       = "Gdb",
1461         .action_msg     = "GDB",
1462 };
1463
1464 static int gdb_register_sysrq(void)
1465 {
1466         printk("Registering GDB sysrq handler\n");
1467         register_sysrq_key('g', &sysrq_gdb_op);
1468         return 0;
1469 }
1470 module_init(gdb_register_sysrq);
1471 #endif