git.oblomov.eu Git - linux-2.6/blob - arch/sh/kernel/kgdb_stub.c

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