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