git.oblomov.eu Git - linux-2.6/blob - arch/frv/kernel/gdb-stub.c

   1 /* gdb-stub.c: FRV GDB stub
   2  *
   3  * Copyright (C) 2003,4 Red Hat, Inc. All Rights Reserved.
   4  * Written by David Howells (dhowells@redhat.com)
   5  * - Derived from Linux/MIPS version, Copyright (C) 1995 Andreas Busse
   6  *
   7  * This program is free software; you can redistribute it and/or
   8  * modify it under the terms of the GNU General Public License
   9  * as published by the Free Software Foundation; either version
  10  * 2 of the License, or (at your option) any later version.
  11  */
  12
  13 /*
  14  *  To enable debugger support, two things need to happen.  One, a
  15  *  call to set_debug_traps() is necessary in order to allow any breakpoints
  16  *  or error conditions to be properly intercepted and reported to gdb.
  17  *  Two, a breakpoint needs to be generated to begin communication.  This
  18  *  is most easily accomplished by a call to breakpoint().  Breakpoint()
  19  *  simulates a breakpoint by executing a BREAK instruction.
  20  *
  21  *
  22  *    The following gdb commands are supported:
  23  *
  24  * command          function                               Return value
  25  *
  26  *    g             return the value of the CPU registers  hex data or ENN
  27  *    G             set the value of the CPU registers     OK or ENN
  28  *
  29  *    mAA..AA,LLLL  Read LLLL bytes at address AA..AA      hex data or ENN
  30  *    MAA..AA,LLLL: Write LLLL bytes at address AA.AA      OK or ENN
  31  *
  32  *    c             Resume at current address              SNN   ( signal NN)
  33  *    cAA..AA       Continue at address AA..AA             SNN
  34  *
  35  *    s             Step one instruction                   SNN
  36  *    sAA..AA       Step one instruction from AA..AA       SNN
  37  *
  38  *    k             kill
  39  *
  40  *    ?             What was the last sigval ?             SNN   (signal NN)
  41  *
  42  *    bBB..BB       Set baud rate to BB..BB                OK or BNN, then sets
  43  *                                                         baud rate
  44  *
  45  * All commands and responses are sent with a packet which includes a
  46  * checksum.  A packet consists of
  47  *
  48  * $<packet info>#<checksum>.
  49  *
  50  * where
  51  * <packet info> :: <characters representing the command or response>
  52  * <checksum>    :: < two hex digits computed as modulo 256 sum of <packetinfo>>
  53  *
  54  * When a packet is received, it is first acknowledged with either '+' or '-'.
  55  * '+' indicates a successful transfer.  '-' indicates a failed transfer.
  56  *
  57  * Example:
  58  *
  59  * Host:                  Reply:
  60  * $m0,10#2a               +$00010203040506070809101112131415#42
  61  *
  62  *
  63  *  ==============
  64  *  MORE EXAMPLES:
  65  *  ==============
  66  *
  67  *  For reference -- the following are the steps that one
  68  *  company took (RidgeRun Inc) to get remote gdb debugging
  69  *  going. In this scenario the host machine was a PC and the
  70  *  target platform was a Galileo EVB64120A MIPS evaluation
  71  *  board.
  72  *
  73  *  Step 1:
  74  *  First download gdb-5.0.tar.gz from the internet.
  75  *  and then build/install the package.
  76  *
  77  *  Example:
  78  *    $ tar zxf gdb-5.0.tar.gz
  79  *    $ cd gdb-5.0
  80  *    $ ./configure --target=frv-elf-gdb
  81  *    $ make
  82  *    $ frv-elf-gdb
  83  *
  84  *  Step 2:
  85  *  Configure linux for remote debugging and build it.
  86  *
  87  *  Example:
  88  *    $ cd ~/linux
  89  *    $ make menuconfig <go to "Kernel Hacking" and turn on remote debugging>
  90  *    $ make dep; make vmlinux
  91  *
  92  *  Step 3:
  93  *  Download the kernel to the remote target and start
  94  *  the kernel running. It will promptly halt and wait
  95  *  for the host gdb session to connect. It does this
  96  *  since the "Kernel Hacking" option has defined
  97  *  CONFIG_REMOTE_DEBUG which in turn enables your calls
  98  *  to:
  99  *     set_debug_traps();
 100  *     breakpoint();
 101  *
 102  *  Step 4:
 103  *  Start the gdb session on the host.
 104  *
 105  *  Example:
 106  *    $ frv-elf-gdb vmlinux
 107  *    (gdb) set remotebaud 115200
 108  *    (gdb) target remote /dev/ttyS1
 109  *    ...at this point you are connected to
 110  *       the remote target and can use gdb
 111  *       in the normal fasion. Setting
 112  *       breakpoints, single stepping,
 113  *       printing variables, etc.
 114  *
 115  */
 116
 117 #include <linux/string.h>
 118 #include <linux/kernel.h>
 119 #include <linux/signal.h>
 120 #include <linux/sched.h>
 121 #include <linux/mm.h>
 122 #include <linux/console.h>
 123 #include <linux/init.h>
 124 #include <linux/slab.h>
 125 #include <linux/nmi.h>
 126
 127 #include <asm/asm-offsets.h>
 128 #include <asm/pgtable.h>
 129 #include <asm/system.h>
 130 #include <asm/gdb-stub.h>
 131
 132 #define LEDS(x) do { /* *(u32*)0xe1200004 = ~(x); mb(); */ } while(0)
 133
 134 #undef GDBSTUB_DEBUG_PROTOCOL
 135
 136 extern void debug_to_serial(const char *p, int n);
 137 extern void gdbstub_console_write(struct console *co, const char *p, unsigned n);
 138
 139 extern volatile uint32_t __break_error_detect[3]; /* ESFR1, ESR15, EAR15 */
 140
 141 struct __debug_amr {
 142         unsigned long L, P;
 143 } __attribute__((aligned(8)));
 144
 145 struct __debug_mmu {
 146         struct {
 147                 unsigned long   hsr0, pcsr, esr0, ear0, epcr0;
 148 #ifdef CONFIG_MMU
 149                 unsigned long   tplr, tppr, tpxr, cxnr;
 150 #endif
 151         } regs;
 152
 153         struct __debug_amr      iamr[16];
 154         struct __debug_amr      damr[16];
 155
 156 #ifdef CONFIG_MMU
 157         struct __debug_amr      tlb[64*2];
 158 #endif
 159 };
 160
 161 static struct __debug_mmu __debug_mmu;
 162
 163 /*
 164  * BUFMAX defines the maximum number of characters in inbound/outbound buffers
 165  * at least NUMREGBYTES*2 are needed for register packets
 166  */
 167 #define BUFMAX 2048
 168
 169 #define BREAK_INSN      0x801000c0      /* use "break" as bkpt */
 170
 171 static const char gdbstub_banner[] = "Linux/FR-V GDB Stub (c) RedHat 2003\n";
 172
 173 volatile u8     gdbstub_rx_buffer[PAGE_SIZE] __attribute__((aligned(PAGE_SIZE)));
 174 volatile u32    gdbstub_rx_inp = 0;
 175 volatile u32    gdbstub_rx_outp = 0;
 176 volatile u8     gdbstub_rx_overflow = 0;
 177 u8              gdbstub_rx_unget = 0;
 178
 179 /* set with GDB whilst running to permit step through exceptions */
 180 extern volatile u32 __attribute__((section(".bss"))) gdbstub_trace_through_exceptions;
 181
 182 static char     input_buffer[BUFMAX];
 183 static char     output_buffer[BUFMAX];
 184
 185 static const char hexchars[] = "0123456789abcdef";
 186
 187 static const char *regnames[] = {
 188         "PSR ", "ISR ", "CCR ", "CCCR",
 189         "LR  ", "LCR ", "PC  ", "_stt",
 190         "sys ", "GR8*", "GNE0", "GNE1",
 191         "IACH", "IACL",
 192         "TBR ", "SP  ", "FP  ", "GR3 ",
 193         "GR4 ", "GR5 ", "GR6 ", "GR7 ",
 194         "GR8 ", "GR9 ", "GR10", "GR11",
 195         "GR12", "GR13", "GR14", "GR15",
 196         "GR16", "GR17", "GR18", "GR19",
 197         "GR20", "GR21", "GR22", "GR23",
 198         "GR24", "GR25", "GR26", "GR27",
 199         "EFRM", "CURR", "GR30", "BFRM"
 200 };
 201
 202 struct gdbstub_bkpt {
 203         unsigned long   addr;           /* address of breakpoint */
 204         unsigned        len;            /* size of breakpoint */
 205         uint32_t        originsns[7];   /* original instructions */
 206 };
 207
 208 static struct gdbstub_bkpt gdbstub_bkpts[256];
 209
 210 /*
 211  * local prototypes
 212  */
 213
 214 static void gdbstub_recv_packet(char *buffer);
 215 static int gdbstub_send_packet(char *buffer);
 216 static int gdbstub_compute_signal(unsigned long tbr);
 217 static int hex(unsigned char ch);
 218 static int hexToInt(char **ptr, unsigned long *intValue);
 219 static unsigned char *mem2hex(const void *mem, char *buf, int count, int may_fault);
 220 static char *hex2mem(const char *buf, void *_mem, int count);
 221
 222 /*
 223  * Convert ch from a hex digit to an int
 224  */
 225 static int hex(unsigned char ch)
 226 {
 227         if (ch >= 'a' && ch <= 'f')
 228                 return ch-'a'+10;
 229         if (ch >= '0' && ch <= '9')
 230                 return ch-'0';
 231         if (ch >= 'A' && ch <= 'F')
 232                 return ch-'A'+10;
 233         return -1;
 234 }
 235
 236 void gdbstub_printk(const char *fmt, ...)
 237 {
 238         static char buf[1024];
 239         va_list args;
 240         int len;
 241
 242         /* Emit the output into the temporary buffer */
 243         va_start(args, fmt);
 244         len = vsnprintf(buf, sizeof(buf), fmt, args);
 245         va_end(args);
 246         debug_to_serial(buf, len);
 247 }
 248
 249 static inline char *gdbstub_strcpy(char *dst, const char *src)
 250 {
 251         int loop = 0;
 252         while ((dst[loop] = src[loop]))
 253                loop++;
 254         return dst;
 255 }
 256
 257 static void gdbstub_purge_cache(void)
 258 {
 259         asm volatile("  dcef    @(gr0,gr0),#1   \n"
 260                      "  icei    @(gr0,gr0),#1   \n"
 261                      "  membar                  \n"
 262                      "  bar                     \n"
 263                      );
 264 }
 265
 266 /*****************************************************************************/
 267 /*
 268  * scan for the sequence $<data>#<checksum>
 269  */
 270 static void gdbstub_recv_packet(char *buffer)
 271 {
 272         unsigned char checksum;
 273         unsigned char xmitcsum;
 274         unsigned char ch;
 275         int count, i, ret, error;
 276
 277         for (;;) {
 278                 /* wait around for the start character, ignore all other characters */
 279                 do {
 280                         gdbstub_rx_char(&ch, 0);
 281                 } while (ch != '$');
 282
 283                 checksum = 0;
 284                 xmitcsum = -1;
 285                 count = 0;
 286                 error = 0;
 287
 288                 /* now, read until a # or end of buffer is found */
 289                 while (count < BUFMAX) {
 290                         ret = gdbstub_rx_char(&ch, 0);
 291                         if (ret < 0)
 292                                 error = ret;
 293
 294                         if (ch == '#')
 295                                 break;
 296                         checksum += ch;
 297                         buffer[count] = ch;
 298                         count++;
 299                 }
 300
 301                 if (error == -EIO) {
 302                         gdbstub_proto("### GDB Rx Error - Skipping packet ###\n");
 303                         gdbstub_proto("### GDB Tx NAK\n");
 304                         gdbstub_tx_char('-');
 305                         continue;
 306                 }
 307
 308                 if (count >= BUFMAX || error)
 309                         continue;
 310
 311                 buffer[count] = 0;
 312
 313                 /* read the checksum */
 314                 ret = gdbstub_rx_char(&ch, 0);
 315                 if (ret < 0)
 316                         error = ret;
 317                 xmitcsum = hex(ch) << 4;
 318
 319                 ret = gdbstub_rx_char(&ch, 0);
 320                 if (ret < 0)
 321                         error = ret;
 322                 xmitcsum |= hex(ch);
 323
 324                 if (error) {
 325                         if (error == -EIO)
 326                                 gdbstub_proto("### GDB Rx Error - Skipping packet\n");
 327                         gdbstub_proto("### GDB Tx NAK\n");
 328                         gdbstub_tx_char('-');
 329                         continue;
 330                 }
 331
 332                 /* check the checksum */
 333                 if (checksum != xmitcsum) {
 334                         gdbstub_proto("### GDB Tx NAK\n");
 335                         gdbstub_tx_char('-');   /* failed checksum */
 336                         continue;
 337                 }
 338
 339                 gdbstub_proto("### GDB Rx '$%s#%02x' ###\n", buffer, checksum);
 340                 gdbstub_proto("### GDB Tx ACK\n");
 341                 gdbstub_tx_char('+'); /* successful transfer */
 342
 343                 /* if a sequence char is present, reply the sequence ID */
 344                 if (buffer[2] == ':') {
 345                         gdbstub_tx_char(buffer[0]);
 346                         gdbstub_tx_char(buffer[1]);
 347
 348                         /* remove sequence chars from buffer */
 349                         count = 0;
 350                         while (buffer[count]) count++;
 351                         for (i=3; i <= count; i++)
 352                                 buffer[i - 3] = buffer[i];
 353                 }
 354
 355                 break;
 356         }
 357 } /* end gdbstub_recv_packet() */
 358
 359 /*****************************************************************************/
 360 /*
 361  * send the packet in buffer.
 362  * - return 0 if successfully ACK'd
 363  * - return 1 if abandoned due to new incoming packet
 364  */
 365 static int gdbstub_send_packet(char *buffer)
 366 {
 367         unsigned char checksum;
 368         int count;
 369         unsigned char ch;
 370
 371         /* $<packet info>#<checksum> */
 372         gdbstub_proto("### GDB Tx '%s' ###\n", buffer);
 373
 374         do {
 375                 gdbstub_tx_char('$');
 376                 checksum = 0;
 377                 count = 0;
 378
 379                 while ((ch = buffer[count]) != 0) {
 380                         gdbstub_tx_char(ch);
 381                         checksum += ch;
 382                         count += 1;
 383                 }
 384
 385                 gdbstub_tx_char('#');
 386                 gdbstub_tx_char(hexchars[checksum >> 4]);
 387                 gdbstub_tx_char(hexchars[checksum & 0xf]);
 388
 389         } while (gdbstub_rx_char(&ch,0),
 390 #ifdef GDBSTUB_DEBUG_PROTOCOL
 391                  ch=='-' && (gdbstub_proto("### GDB Rx NAK\n"),0),
 392                  ch!='-' && ch!='+' && (gdbstub_proto("### GDB Rx ??? %02x\n",ch),0),
 393 #endif
 394                  ch!='+' && ch!='$');
 395
 396         if (ch=='+') {
 397                 gdbstub_proto("### GDB Rx ACK\n");
 398                 return 0;
 399         }
 400
 401         gdbstub_proto("### GDB Tx Abandoned\n");
 402         gdbstub_rx_unget = ch;
 403         return 1;
 404 } /* end gdbstub_send_packet() */
 405
 406 /*
 407  * While we find nice hex chars, build an int.
 408  * Return number of chars processed.
 409  */
 410 static int hexToInt(char **ptr, unsigned long *_value)
 411 {
 412         int count = 0, ch;
 413
 414         *_value = 0;
 415         while (**ptr) {
 416                 ch = hex(**ptr);
 417                 if (ch < 0)
 418                         break;
 419
 420                 *_value = (*_value << 4) | ((uint8_t) ch & 0xf);
 421                 count++;
 422
 423                 (*ptr)++;
 424         }
 425
 426         return count;
 427 }
 428
 429 /*****************************************************************************/
 430 /*
 431  * probe an address to see whether it maps to anything
 432  */
 433 static inline int gdbstub_addr_probe(const void *vaddr)
 434 {
 435 #ifdef CONFIG_MMU
 436         unsigned long paddr;
 437
 438         asm("lrad %1,%0,#1,#0,#0" : "=r"(paddr) : "r"(vaddr));
 439         if (!(paddr & xAMPRx_V))
 440                 return 0;
 441 #endif
 442
 443         return 1;
 444 } /* end gdbstub_addr_probe() */
 445
 446 #ifdef CONFIG_MMU
 447 static unsigned long __saved_dampr, __saved_damlr;
 448
 449 static inline unsigned long gdbstub_virt_to_pte(unsigned long vaddr)
 450 {
 451         pgd_t *pgd;
 452         pud_t *pud;
 453         pmd_t *pmd;
 454         pte_t *pte;
 455         unsigned long val, dampr5;
 456
 457         pgd = (pgd_t *) __get_DAMLR(3) + pgd_index(vaddr);
 458         pud = pud_offset(pgd, vaddr);
 459         pmd = pmd_offset(pud, vaddr);
 460
 461         if (pmd_bad(*pmd) || !pmd_present(*pmd))
 462                 return 0;
 463
 464         /* make sure dampr5 maps to the correct pmd */
 465         dampr5 = __get_DAMPR(5);
 466         val = pmd_val(*pmd);
 467         __set_DAMPR(5, val | xAMPRx_L | xAMPRx_SS_16Kb | xAMPRx_S | xAMPRx_C | xAMPRx_V);
 468
 469         /* now its safe to access pmd */
 470         pte = (pte_t *)__get_DAMLR(5) + __pte_index(vaddr);
 471         if (pte_present(*pte))
 472                 val = pte_val(*pte);
 473         else
 474                 val = 0;
 475
 476         /* restore original dampr5 */
 477         __set_DAMPR(5, dampr5);
 478
 479         return val;
 480 }
 481 #endif
 482
 483 static inline int gdbstub_addr_map(const void *vaddr)
 484 {
 485 #ifdef CONFIG_MMU
 486         unsigned long pte;
 487
 488         __saved_dampr = __get_DAMPR(2);
 489         __saved_damlr = __get_DAMLR(2);
 490 #endif
 491         if (gdbstub_addr_probe(vaddr))
 492                 return 1;
 493 #ifdef CONFIG_MMU
 494         pte = gdbstub_virt_to_pte((unsigned long) vaddr);
 495         if (pte) {
 496                 __set_DAMPR(2, pte);
 497                 __set_DAMLR(2, (unsigned long) vaddr & PAGE_MASK);
 498                 return 1;
 499         }
 500 #endif
 501         return 0;
 502 }
 503
 504 static inline void gdbstub_addr_unmap(void)
 505 {
 506 #ifdef CONFIG_MMU
 507         __set_DAMPR(2, __saved_dampr);
 508         __set_DAMLR(2, __saved_damlr);
 509 #endif
 510 }
 511
 512 /*
 513  * access potentially dodgy memory through a potentially dodgy pointer
 514  */
 515 static inline int gdbstub_read_dword(const void *addr, uint32_t *_res)
 516 {
 517         unsigned long brr;
 518         uint32_t res;
 519
 520         if (!gdbstub_addr_map(addr))
 521                 return 0;
 522
 523         asm volatile("  movgs   gr0,brr \n"
 524                      "  ld%I2   %M2,%0  \n"
 525                      "  movsg   brr,%1  \n"
 526                      : "=r"(res), "=r"(brr)
 527                      : "m"(*(uint32_t *) addr));
 528         *_res = res;
 529         gdbstub_addr_unmap();
 530         return likely(!brr);
 531 }
 532
 533 static inline int gdbstub_write_dword(void *addr, uint32_t val)
 534 {
 535         unsigned long brr;
 536
 537         if (!gdbstub_addr_map(addr))
 538                 return 0;
 539
 540         asm volatile("  movgs   gr0,brr \n"
 541                      "  st%I2   %1,%M2  \n"
 542                      "  movsg   brr,%0  \n"
 543                      : "=r"(brr)
 544                      : "r"(val), "m"(*(uint32_t *) addr));
 545         gdbstub_addr_unmap();
 546         return likely(!brr);
 547 }
 548
 549 static inline int gdbstub_read_word(const void *addr, uint16_t *_res)
 550 {
 551         unsigned long brr;
 552         uint16_t res;
 553
 554         if (!gdbstub_addr_map(addr))
 555                 return 0;
 556
 557         asm volatile("  movgs   gr0,brr \n"
 558                      "  lduh%I2 %M2,%0  \n"
 559                      "  movsg   brr,%1  \n"
 560                      : "=r"(res), "=r"(brr)
 561                      : "m"(*(uint16_t *) addr));
 562         *_res = res;
 563         gdbstub_addr_unmap();
 564         return likely(!brr);
 565 }
 566
 567 static inline int gdbstub_write_word(void *addr, uint16_t val)
 568 {
 569         unsigned long brr;
 570
 571         if (!gdbstub_addr_map(addr))
 572                 return 0;
 573
 574         asm volatile("  movgs   gr0,brr \n"
 575                      "  sth%I2  %1,%M2  \n"
 576                      "  movsg   brr,%0  \n"
 577                      : "=r"(brr)
 578                      : "r"(val), "m"(*(uint16_t *) addr));
 579         gdbstub_addr_unmap();
 580         return likely(!brr);
 581 }
 582
 583 static inline int gdbstub_read_byte(const void *addr, uint8_t *_res)
 584 {
 585         unsigned long brr;
 586         uint8_t res;
 587
 588         if (!gdbstub_addr_map(addr))
 589                 return 0;
 590
 591         asm volatile("  movgs   gr0,brr \n"
 592                      "  ldub%I2 %M2,%0  \n"
 593                      "  movsg   brr,%1  \n"
 594                      : "=r"(res), "=r"(brr)
 595                      : "m"(*(uint8_t *) addr));
 596         *_res = res;
 597         gdbstub_addr_unmap();
 598         return likely(!brr);
 599 }
 600
 601 static inline int gdbstub_write_byte(void *addr, uint8_t val)
 602 {
 603         unsigned long brr;
 604
 605         if (!gdbstub_addr_map(addr))
 606                 return 0;
 607
 608         asm volatile("  movgs   gr0,brr \n"
 609                      "  stb%I2  %1,%M2  \n"
 610                      "  movsg   brr,%0  \n"
 611                      : "=r"(brr)
 612                      : "r"(val), "m"(*(uint8_t *) addr));
 613         gdbstub_addr_unmap();
 614         return likely(!brr);
 615 }
 616
 617 static void __gdbstub_console_write(struct console *co, const char *p, unsigned n)
 618 {
 619         char outbuf[26];
 620         int qty;
 621
 622         outbuf[0] = 'O';
 623
 624         while (n > 0) {
 625                 qty = 1;
 626
 627                 while (n > 0 && qty < 20) {
 628                         mem2hex(p, outbuf + qty, 2, 0);
 629                         qty += 2;
 630                         if (*p == 0x0a) {
 631                                 outbuf[qty++] = '0';
 632                                 outbuf[qty++] = 'd';
 633                         }
 634                         p++;
 635                         n--;
 636                 }
 637
 638                 outbuf[qty] = 0;
 639                 gdbstub_send_packet(outbuf);
 640         }
 641 }
 642
 643 #if 0
 644 void debug_to_serial(const char *p, int n)
 645 {
 646         gdbstub_console_write(NULL,p,n);
 647 }
 648 #endif
 649
 650 #ifdef CONFIG_GDBSTUB_CONSOLE
 651
 652 static kdev_t gdbstub_console_dev(struct console *con)
 653 {
 654         return MKDEV(1,3); /* /dev/null */
 655 }
 656
 657 static struct console gdbstub_console = {
 658         .name   = "gdb",
 659         .write  = gdbstub_console_write,        /* in break.S */
 660         .device = gdbstub_console_dev,
 661         .flags  = CON_PRINTBUFFER,
 662         .index  = -1,
 663 };
 664
 665 #endif
 666
 667 /*****************************************************************************/
 668 /*
 669  * Convert the memory pointed to by mem into hex, placing result in buf.
 670  * - if successful, return a pointer to the last char put in buf (NUL)
 671  * - in case of mem fault, return NULL
 672  * may_fault is non-zero if we are reading from arbitrary memory, but is currently
 673  * not used.
 674  */
 675 static unsigned char *mem2hex(const void *_mem, char *buf, int count, int may_fault)
 676 {
 677         const uint8_t *mem = _mem;
 678         uint8_t ch[4] __attribute__((aligned(4)));
 679
 680         if ((uint32_t)mem&1 && count>=1) {
 681                 if (!gdbstub_read_byte(mem,ch))
 682                         return NULL;
 683                 *buf++ = hexchars[ch[0] >> 4];
 684                 *buf++ = hexchars[ch[0] & 0xf];
 685                 mem++;
 686                 count--;
 687         }
 688
 689         if ((uint32_t)mem&3 && count>=2) {
 690                 if (!gdbstub_read_word(mem,(uint16_t *)ch))
 691                         return NULL;
 692                 *buf++ = hexchars[ch[0] >> 4];
 693                 *buf++ = hexchars[ch[0] & 0xf];
 694                 *buf++ = hexchars[ch[1] >> 4];
 695                 *buf++ = hexchars[ch[1] & 0xf];
 696                 mem += 2;
 697                 count -= 2;
 698         }
 699
 700         while (count>=4) {
 701                 if (!gdbstub_read_dword(mem,(uint32_t *)ch))
 702                         return NULL;
 703                 *buf++ = hexchars[ch[0] >> 4];
 704                 *buf++ = hexchars[ch[0] & 0xf];
 705                 *buf++ = hexchars[ch[1] >> 4];
 706                 *buf++ = hexchars[ch[1] & 0xf];
 707                 *buf++ = hexchars[ch[2] >> 4];
 708                 *buf++ = hexchars[ch[2] & 0xf];
 709                 *buf++ = hexchars[ch[3] >> 4];
 710                 *buf++ = hexchars[ch[3] & 0xf];
 711                 mem += 4;
 712                 count -= 4;
 713         }
 714
 715         if (count>=2) {
 716                 if (!gdbstub_read_word(mem,(uint16_t *)ch))
 717                         return NULL;
 718                 *buf++ = hexchars[ch[0] >> 4];
 719                 *buf++ = hexchars[ch[0] & 0xf];
 720                 *buf++ = hexchars[ch[1] >> 4];
 721                 *buf++ = hexchars[ch[1] & 0xf];
 722                 mem += 2;
 723                 count -= 2;
 724         }
 725
 726         if (count>=1) {
 727                 if (!gdbstub_read_byte(mem,ch))
 728                         return NULL;
 729                 *buf++ = hexchars[ch[0] >> 4];
 730                 *buf++ = hexchars[ch[0] & 0xf];
 731         }
 732
 733         *buf = 0;
 734
 735         return buf;
 736 } /* end mem2hex() */
 737
 738 /*****************************************************************************/
 739 /*
 740  * convert the hex array pointed to by buf into binary to be placed in mem
 741  * return a pointer to the character AFTER the last byte of buffer consumed
 742  */
 743 static char *hex2mem(const char *buf, void *_mem, int count)
 744 {
 745         uint8_t *mem = _mem;
 746         union {
 747                 uint32_t l;
 748                 uint16_t w;
 749                 uint8_t  b[4];
 750         } ch;
 751
 752         if ((u32)mem&1 && count>=1) {
 753                 ch.b[0]  = hex(*buf++) << 4;
 754                 ch.b[0] |= hex(*buf++);
 755                 if (!gdbstub_write_byte(mem,ch.b[0]))
 756                         return NULL;
 757                 mem++;
 758                 count--;
 759         }
 760
 761         if ((u32)mem&3 && count>=2) {
 762                 ch.b[0]  = hex(*buf++) << 4;
 763                 ch.b[0] |= hex(*buf++);
 764                 ch.b[1]  = hex(*buf++) << 4;
 765                 ch.b[1] |= hex(*buf++);
 766                 if (!gdbstub_write_word(mem,ch.w))
 767                         return NULL;
 768                 mem += 2;
 769                 count -= 2;
 770         }
 771
 772         while (count>=4) {
 773                 ch.b[0]  = hex(*buf++) << 4;
 774                 ch.b[0] |= hex(*buf++);
 775                 ch.b[1]  = hex(*buf++) << 4;
 776                 ch.b[1] |= hex(*buf++);
 777                 ch.b[2]  = hex(*buf++) << 4;
 778                 ch.b[2] |= hex(*buf++);
 779                 ch.b[3]  = hex(*buf++) << 4;
 780                 ch.b[3] |= hex(*buf++);
 781                 if (!gdbstub_write_dword(mem,ch.l))
 782                         return NULL;
 783                 mem += 4;
 784                 count -= 4;
 785         }
 786
 787         if (count>=2) {
 788                 ch.b[0]  = hex(*buf++) << 4;
 789                 ch.b[0] |= hex(*buf++);
 790                 ch.b[1]  = hex(*buf++) << 4;
 791                 ch.b[1] |= hex(*buf++);
 792                 if (!gdbstub_write_word(mem,ch.w))
 793                         return NULL;
 794                 mem += 2;
 795                 count -= 2;
 796         }
 797
 798         if (count>=1) {
 799                 ch.b[0]  = hex(*buf++) << 4;
 800                 ch.b[0] |= hex(*buf++);
 801                 if (!gdbstub_write_byte(mem,ch.b[0]))
 802                         return NULL;
 803         }
 804
 805         return (char *) buf;
 806 } /* end hex2mem() */
 807
 808 /*****************************************************************************/
 809 /*
 810  * This table contains the mapping between FRV TBR.TT exception codes,
 811  * and signals, which are primarily what GDB understands.  It also
 812  * indicates which hardware traps we need to commandeer when
 813  * initializing the stub.
 814  */
 815 static const struct brr_to_sig_map {
 816         unsigned long   brr_mask;       /* BRR bitmask */
 817         unsigned long   tbr_tt;         /* TBR.TT code (in BRR.EBTT) */
 818         unsigned int    signo;          /* Signal that we map this into */
 819 } brr_to_sig_map[] = {
 820         { BRR_EB,       TBR_TT_INSTR_ACC_ERROR, SIGSEGV         },
 821         { BRR_EB,       TBR_TT_ILLEGAL_INSTR,   SIGILL          },
 822         { BRR_EB,       TBR_TT_PRIV_INSTR,      SIGILL          },
 823         { BRR_EB,       TBR_TT_MP_EXCEPTION,    SIGFPE          },
 824         { BRR_EB,       TBR_TT_DATA_ACC_ERROR,  SIGSEGV         },
 825         { BRR_EB,       TBR_TT_DATA_STR_ERROR,  SIGSEGV         },
 826         { BRR_EB,       TBR_TT_DIVISION_EXCEP,  SIGFPE          },
 827         { BRR_EB,       TBR_TT_COMPOUND_EXCEP,  SIGSEGV         },
 828         { BRR_EB,       TBR_TT_INTERRUPT_13,    SIGALRM         },      /* watchdog */
 829         { BRR_EB,       TBR_TT_INTERRUPT_14,    SIGINT          },      /* GDB serial */
 830         { BRR_EB,       TBR_TT_INTERRUPT_15,    SIGQUIT         },      /* NMI */
 831         { BRR_CB,       0,                      SIGUSR1         },
 832         { BRR_TB,       0,                      SIGUSR2         },
 833         { BRR_DBNEx,    0,                      SIGTRAP         },
 834         { BRR_DBx,      0,                      SIGTRAP         },      /* h/w watchpoint */
 835         { BRR_IBx,      0,                      SIGTRAP         },      /* h/w breakpoint */
 836         { BRR_CBB,      0,                      SIGTRAP         },
 837         { BRR_SB,       0,                      SIGTRAP         },
 838         { BRR_ST,       0,                      SIGTRAP         },      /* single step */
 839         { 0,            0,                      SIGHUP          }       /* default */
 840 };
 841
 842 /*****************************************************************************/
 843 /*
 844  * convert the FRV BRR register contents into a UNIX signal number
 845  */
 846 static inline int gdbstub_compute_signal(unsigned long brr)
 847 {
 848         const struct brr_to_sig_map *map;
 849         unsigned long tbr = (brr & BRR_EBTT) >> 12;
 850
 851         for (map = brr_to_sig_map; map->brr_mask; map++)
 852                 if (map->brr_mask & brr)
 853                         if (!map->tbr_tt || map->tbr_tt == tbr)
 854                                 break;
 855
 856         return map->signo;
 857 } /* end gdbstub_compute_signal() */
 858
 859 /*****************************************************************************/
 860 /*
 861  * set a software breakpoint or a hardware breakpoint or watchpoint
 862  */
 863 static int gdbstub_set_breakpoint(unsigned long type, unsigned long addr, unsigned long len)
 864 {
 865         unsigned long tmp;
 866         int bkpt, loop, xloop;
 867
 868         union {
 869                 struct {
 870                         unsigned long mask0, mask1;
 871                 };
 872                 uint8_t bytes[8];
 873         } dbmr;
 874
 875         //gdbstub_printk("setbkpt(%ld,%08lx,%ld)\n", type, addr, len);
 876
 877         switch (type) {
 878                 /* set software breakpoint */
 879         case 0:
 880                 if (addr & 3 || len > 7*4)
 881                         return -EINVAL;
 882
 883                 for (bkpt = 255; bkpt >= 0; bkpt--)
 884                         if (!gdbstub_bkpts[bkpt].addr)
 885                                 break;
 886                 if (bkpt < 0)
 887                         return -ENOSPC;
 888
 889                 for (loop = 0; loop < len/4; loop++)
 890                         if (!gdbstub_read_dword(&((uint32_t *) addr)[loop],
 891                                                 &gdbstub_bkpts[bkpt].originsns[loop]))
 892                                 return -EFAULT;
 893
 894                 for (loop = 0; loop < len/4; loop++)
 895                         if (!gdbstub_write_dword(&((uint32_t *) addr)[loop],
 896                                                  BREAK_INSN)
 897                             ) {
 898                                 /* need to undo the changes if possible */
 899                                 for (xloop = 0; xloop < loop; xloop++)
 900                                         gdbstub_write_dword(&((uint32_t *) addr)[xloop],
 901                                                             gdbstub_bkpts[bkpt].originsns[xloop]);
 902                                 return -EFAULT;
 903                         }
 904
 905                 gdbstub_bkpts[bkpt].addr = addr;
 906                 gdbstub_bkpts[bkpt].len = len;
 907
 908 #if 0
 909                 gdbstub_printk("Set BKPT[%02x]: %08lx #%d {%04x, %04x} -> { %04x, %04x }\n",
 910                                bkpt,
 911                                gdbstub_bkpts[bkpt].addr,
 912                                gdbstub_bkpts[bkpt].len,
 913                                gdbstub_bkpts[bkpt].originsns[0],
 914                                gdbstub_bkpts[bkpt].originsns[1],
 915                                ((uint32_t *) addr)[0],
 916                                ((uint32_t *) addr)[1]
 917                                );
 918 #endif
 919                 return 0;
 920
 921                 /* set hardware breakpoint */
 922         case 1:
 923                 if (addr & 3 || len != 4)
 924                         return -EINVAL;
 925
 926                 if (!(__debug_regs->dcr & DCR_IBE0)) {
 927                         //gdbstub_printk("set h/w break 0: %08lx\n", addr);
 928                         __debug_regs->dcr |= DCR_IBE0;
 929                         __debug_regs->ibar[0] = addr;
 930                         asm volatile("movgs %0,ibar0" : : "r"(addr));
 931                         return 0;
 932                 }
 933
 934                 if (!(__debug_regs->dcr & DCR_IBE1)) {
 935                         //gdbstub_printk("set h/w break 1: %08lx\n", addr);
 936                         __debug_regs->dcr |= DCR_IBE1;
 937                         __debug_regs->ibar[1] = addr;
 938                         asm volatile("movgs %0,ibar1" : : "r"(addr));
 939                         return 0;
 940                 }
 941
 942                 if (!(__debug_regs->dcr & DCR_IBE2)) {
 943                         //gdbstub_printk("set h/w break 2: %08lx\n", addr);
 944                         __debug_regs->dcr |= DCR_IBE2;
 945                         __debug_regs->ibar[2] = addr;
 946                         asm volatile("movgs %0,ibar2" : : "r"(addr));
 947                         return 0;
 948                 }
 949
 950                 if (!(__debug_regs->dcr & DCR_IBE3)) {
 951                         //gdbstub_printk("set h/w break 3: %08lx\n", addr);
 952                         __debug_regs->dcr |= DCR_IBE3;
 953                         __debug_regs->ibar[3] = addr;
 954                         asm volatile("movgs %0,ibar3" : : "r"(addr));
 955                         return 0;
 956                 }
 957
 958                 return -ENOSPC;
 959
 960                 /* set data read/write/access watchpoint */
 961         case 2:
 962         case 3:
 963         case 4:
 964                 if ((addr & ~7) != ((addr + len - 1) & ~7))
 965                         return -EINVAL;
 966
 967                 tmp = addr & 7;
 968
 969                 memset(dbmr.bytes, 0xff, sizeof(dbmr.bytes));
 970                 for (loop = 0; loop < len; loop++)
 971                         dbmr.bytes[tmp + loop] = 0;
 972
 973                 addr &= ~7;
 974
 975                 if (!(__debug_regs->dcr & (DCR_DRBE0|DCR_DWBE0))) {
 976                         //gdbstub_printk("set h/w watchpoint 0 type %ld: %08lx\n", type, addr);
 977                         tmp = type==2 ? DCR_DWBE0 : type==3 ? DCR_DRBE0 : DCR_DRBE0|DCR_DWBE0;
 978
 979                         __debug_regs->dcr |= tmp;
 980                         __debug_regs->dbar[0] = addr;
 981                         __debug_regs->dbmr[0][0] = dbmr.mask0;
 982                         __debug_regs->dbmr[0][1] = dbmr.mask1;
 983                         __debug_regs->dbdr[0][0] = 0;
 984                         __debug_regs->dbdr[0][1] = 0;
 985
 986                         asm volatile("  movgs   %0,dbar0        \n"
 987                                      "  movgs   %1,dbmr00       \n"
 988                                      "  movgs   %2,dbmr01       \n"
 989                                      "  movgs   gr0,dbdr00      \n"
 990                                      "  movgs   gr0,dbdr01      \n"
 991                                      : : "r"(addr), "r"(dbmr.mask0), "r"(dbmr.mask1));
 992                         return 0;
 993                 }
 994
 995                 if (!(__debug_regs->dcr & (DCR_DRBE1|DCR_DWBE1))) {
 996                         //gdbstub_printk("set h/w watchpoint 1 type %ld: %08lx\n", type, addr);
 997                         tmp = type==2 ? DCR_DWBE1 : type==3 ? DCR_DRBE1 : DCR_DRBE1|DCR_DWBE1;
 998
 999                         __debug_regs->dcr |= tmp;
1000                         __debug_regs->dbar[1] = addr;
1001                         __debug_regs->dbmr[1][0] = dbmr.mask0;
1002                         __debug_regs->dbmr[1][1] = dbmr.mask1;
1003                         __debug_regs->dbdr[1][0] = 0;
1004                         __debug_regs->dbdr[1][1] = 0;
1005
1006                         asm volatile("  movgs   %0,dbar1        \n"
1007                                      "  movgs   %1,dbmr10       \n"
1008                                      "  movgs   %2,dbmr11       \n"
1009                                      "  movgs   gr0,dbdr10      \n"
1010                                      "  movgs   gr0,dbdr11      \n"
1011                                      : : "r"(addr), "r"(dbmr.mask0), "r"(dbmr.mask1));
1012                         return 0;
1013                 }
1014
1015                 return -ENOSPC;
1016
1017         default:
1018                 return -EINVAL;
1019         }
1020
1021 } /* end gdbstub_set_breakpoint() */
1022
1023 /*****************************************************************************/
1024 /*
1025  * clear a breakpoint or watchpoint
1026  */
1027 int gdbstub_clear_breakpoint(unsigned long type, unsigned long addr, unsigned long len)
1028 {
1029         unsigned long tmp;
1030         int bkpt, loop;
1031
1032         union {
1033                 struct {
1034                         unsigned long mask0, mask1;
1035                 };
1036                 uint8_t bytes[8];
1037         } dbmr;
1038
1039         //gdbstub_printk("clearbkpt(%ld,%08lx,%ld)\n", type, addr, len);
1040
1041         switch (type) {
1042                 /* clear software breakpoint */
1043         case 0:
1044                 for (bkpt = 255; bkpt >= 0; bkpt--)
1045                         if (gdbstub_bkpts[bkpt].addr == addr && gdbstub_bkpts[bkpt].len == len)
1046                                 break;
1047                 if (bkpt < 0)
1048                         return -ENOENT;
1049
1050                 gdbstub_bkpts[bkpt].addr = 0;
1051
1052                 for (loop = 0; loop < len/4; loop++)
1053                         if (!gdbstub_write_dword(&((uint32_t *) addr)[loop],
1054                                                  gdbstub_bkpts[bkpt].originsns[loop]))
1055                                 return -EFAULT;
1056                 return 0;
1057
1058                 /* clear hardware breakpoint */
1059         case 1:
1060                 if (addr & 3 || len != 4)
1061                         return -EINVAL;
1062
1063 #define __get_ibar(X) ({ unsigned long x; asm volatile("movsg ibar"#X",%0" : "=r"(x)); x; })
1064
1065                 if (__debug_regs->dcr & DCR_IBE0 && __get_ibar(0) == addr) {
1066                         //gdbstub_printk("clear h/w break 0: %08lx\n", addr);
1067                         __debug_regs->dcr &= ~DCR_IBE0;
1068                         __debug_regs->ibar[0] = 0;
1069                         asm volatile("movgs gr0,ibar0");
1070                         return 0;
1071                 }
1072
1073                 if (__debug_regs->dcr & DCR_IBE1 && __get_ibar(1) == addr) {
1074                         //gdbstub_printk("clear h/w break 1: %08lx\n", addr);
1075                         __debug_regs->dcr &= ~DCR_IBE1;
1076                         __debug_regs->ibar[1] = 0;
1077                         asm volatile("movgs gr0,ibar1");
1078                         return 0;
1079                 }
1080
1081                 if (__debug_regs->dcr & DCR_IBE2 && __get_ibar(2) == addr) {
1082                         //gdbstub_printk("clear h/w break 2: %08lx\n", addr);
1083                         __debug_regs->dcr &= ~DCR_IBE2;
1084                         __debug_regs->ibar[2] = 0;
1085                         asm volatile("movgs gr0,ibar2");
1086                         return 0;
1087                 }
1088
1089                 if (__debug_regs->dcr & DCR_IBE3 && __get_ibar(3) == addr) {
1090                         //gdbstub_printk("clear h/w break 3: %08lx\n", addr);
1091                         __debug_regs->dcr &= ~DCR_IBE3;
1092                         __debug_regs->ibar[3] = 0;
1093                         asm volatile("movgs gr0,ibar3");
1094                         return 0;
1095                 }
1096
1097                 return -EINVAL;
1098
1099                 /* clear data read/write/access watchpoint */
1100         case 2:
1101         case 3:
1102         case 4:
1103                 if ((addr & ~7) != ((addr + len - 1) & ~7))
1104                         return -EINVAL;
1105
1106                 tmp = addr & 7;
1107
1108                 memset(dbmr.bytes, 0xff, sizeof(dbmr.bytes));
1109                 for (loop = 0; loop < len; loop++)
1110                         dbmr.bytes[tmp + loop] = 0;
1111
1112                 addr &= ~7;
1113
1114 #define __get_dbar(X) ({ unsigned long x; asm volatile("movsg dbar"#X",%0" : "=r"(x)); x; })
1115 #define __get_dbmr0(X) ({ unsigned long x; asm volatile("movsg dbmr"#X"0,%0" : "=r"(x)); x; })
1116 #define __get_dbmr1(X) ({ unsigned long x; asm volatile("movsg dbmr"#X"1,%0" : "=r"(x)); x; })
1117
1118                 /* consider DBAR 0 */
1119                 tmp = type==2 ? DCR_DWBE0 : type==3 ? DCR_DRBE0 : DCR_DRBE0|DCR_DWBE0;
1120
1121                 if ((__debug_regs->dcr & (DCR_DRBE0|DCR_DWBE0)) != tmp ||
1122                     __get_dbar(0) != addr ||
1123                     __get_dbmr0(0) != dbmr.mask0 ||
1124                     __get_dbmr1(0) != dbmr.mask1)
1125                         goto skip_dbar0;
1126
1127                 //gdbstub_printk("clear h/w watchpoint 0 type %ld: %08lx\n", type, addr);
1128                 __debug_regs->dcr &= ~(DCR_DRBE0|DCR_DWBE0);
1129                 __debug_regs->dbar[0] = 0;
1130                 __debug_regs->dbmr[0][0] = 0;
1131                 __debug_regs->dbmr[0][1] = 0;
1132                 __debug_regs->dbdr[0][0] = 0;
1133                 __debug_regs->dbdr[0][1] = 0;
1134
1135                 asm volatile("  movgs   gr0,dbar0       \n"
1136                              "  movgs   gr0,dbmr00      \n"
1137                              "  movgs   gr0,dbmr01      \n"
1138                              "  movgs   gr0,dbdr00      \n"
1139                              "  movgs   gr0,dbdr01      \n");
1140                 return 0;
1141
1142         skip_dbar0:
1143                 /* consider DBAR 0 */
1144                 tmp = type==2 ? DCR_DWBE1 : type==3 ? DCR_DRBE1 : DCR_DRBE1|DCR_DWBE1;
1145
1146                 if ((__debug_regs->dcr & (DCR_DRBE1|DCR_DWBE1)) != tmp ||
1147                     __get_dbar(1) != addr ||
1148                     __get_dbmr0(1) != dbmr.mask0 ||
1149                     __get_dbmr1(1) != dbmr.mask1)
1150                         goto skip_dbar1;
1151
1152                 //gdbstub_printk("clear h/w watchpoint 1 type %ld: %08lx\n", type, addr);
1153                 __debug_regs->dcr &= ~(DCR_DRBE1|DCR_DWBE1);
1154                 __debug_regs->dbar[1] = 0;
1155                 __debug_regs->dbmr[1][0] = 0;
1156                 __debug_regs->dbmr[1][1] = 0;
1157                 __debug_regs->dbdr[1][0] = 0;
1158                 __debug_regs->dbdr[1][1] = 0;
1159
1160                 asm volatile("  movgs   gr0,dbar1       \n"
1161                              "  movgs   gr0,dbmr10      \n"
1162                              "  movgs   gr0,dbmr11      \n"
1163                              "  movgs   gr0,dbdr10      \n"
1164                              "  movgs   gr0,dbdr11      \n");
1165                 return 0;
1166
1167         skip_dbar1:
1168                 return -ENOSPC;
1169
1170         default:
1171                 return -EINVAL;
1172         }
1173 } /* end gdbstub_clear_breakpoint() */
1174
1175 /*****************************************************************************/
1176 /*
1177  * check a for an internal software breakpoint, and wind the PC back if necessary
1178  */
1179 static void gdbstub_check_breakpoint(void)
1180 {
1181         unsigned long addr = __debug_frame->pc - 4;
1182         int bkpt;
1183
1184         for (bkpt = 255; bkpt >= 0; bkpt--)
1185                 if (gdbstub_bkpts[bkpt].addr == addr)
1186                         break;
1187         if (bkpt >= 0)
1188                 __debug_frame->pc = addr;
1189
1190         //gdbstub_printk("alter pc [%d] %08lx\n", bkpt, __debug_frame->pc);
1191
1192 } /* end gdbstub_check_breakpoint() */
1193
1194 /*****************************************************************************/
1195 /*
1196  *
1197  */
1198 static void __attribute__((unused)) gdbstub_show_regs(void)
1199 {
1200         unsigned long *reg;
1201         int loop;
1202
1203         gdbstub_printk("\n");
1204
1205         gdbstub_printk("Frame: @%p [%s]\n",
1206                        __debug_frame,
1207                        __debug_frame->psr & PSR_S ? "kernel" : "user");
1208
1209         reg = (unsigned long *) __debug_frame;
1210         for (loop = 0; loop < NR_PT_REGS; loop++) {
1211                 printk("%s %08lx", regnames[loop + 0], reg[loop + 0]);
1212
1213                 if (loop == NR_PT_REGS - 1 || loop % 5 == 4)
1214                         printk("\n");
1215                 else
1216                         printk(" | ");
1217         }
1218
1219         gdbstub_printk("Process %s (pid: %d)\n", current->comm, current->pid);
1220 } /* end gdbstub_show_regs() */
1221
1222 /*****************************************************************************/
1223 /*
1224  * dump debugging regs
1225  */
1226 static void __attribute__((unused)) gdbstub_dump_debugregs(void)
1227 {
1228         gdbstub_printk("DCR    %08lx  ", __debug_status.dcr);
1229         gdbstub_printk("BRR    %08lx\n", __debug_status.brr);
1230
1231         gdbstub_printk("IBAR0  %08lx  ", __get_ibar(0));
1232         gdbstub_printk("IBAR1  %08lx  ", __get_ibar(1));
1233         gdbstub_printk("IBAR2  %08lx  ", __get_ibar(2));
1234         gdbstub_printk("IBAR3  %08lx\n", __get_ibar(3));
1235
1236         gdbstub_printk("DBAR0  %08lx  ", __get_dbar(0));
1237         gdbstub_printk("DBMR00 %08lx  ", __get_dbmr0(0));
1238         gdbstub_printk("DBMR01 %08lx\n", __get_dbmr1(0));
1239
1240         gdbstub_printk("DBAR1  %08lx  ", __get_dbar(1));
1241         gdbstub_printk("DBMR10 %08lx  ", __get_dbmr0(1));
1242         gdbstub_printk("DBMR11 %08lx\n", __get_dbmr1(1));
1243
1244         gdbstub_printk("\n");
1245 } /* end gdbstub_dump_debugregs() */
1246
1247 /*****************************************************************************/
1248 /*
1249  * dump the MMU state into a structure so that it can be accessed with GDB
1250  */
1251 void gdbstub_get_mmu_state(void)
1252 {
1253         asm volatile("movsg hsr0,%0" : "=r"(__debug_mmu.regs.hsr0));
1254         asm volatile("movsg pcsr,%0" : "=r"(__debug_mmu.regs.pcsr));
1255         asm volatile("movsg esr0,%0" : "=r"(__debug_mmu.regs.esr0));
1256         asm volatile("movsg ear0,%0" : "=r"(__debug_mmu.regs.ear0));
1257         asm volatile("movsg epcr0,%0" : "=r"(__debug_mmu.regs.epcr0));
1258
1259         /* read the protection / SAT registers */
1260         __debug_mmu.iamr[0].L  = __get_IAMLR(0);
1261         __debug_mmu.iamr[0].P  = __get_IAMPR(0);
1262         __debug_mmu.iamr[1].L  = __get_IAMLR(1);
1263         __debug_mmu.iamr[1].P  = __get_IAMPR(1);
1264         __debug_mmu.iamr[2].L  = __get_IAMLR(2);
1265         __debug_mmu.iamr[2].P  = __get_IAMPR(2);
1266         __debug_mmu.iamr[3].L  = __get_IAMLR(3);
1267         __debug_mmu.iamr[3].P  = __get_IAMPR(3);
1268         __debug_mmu.iamr[4].L  = __get_IAMLR(4);
1269         __debug_mmu.iamr[4].P  = __get_IAMPR(4);
1270         __debug_mmu.iamr[5].L  = __get_IAMLR(5);
1271         __debug_mmu.iamr[5].P  = __get_IAMPR(5);
1272         __debug_mmu.iamr[6].L  = __get_IAMLR(6);
1273         __debug_mmu.iamr[6].P  = __get_IAMPR(6);
1274         __debug_mmu.iamr[7].L  = __get_IAMLR(7);
1275         __debug_mmu.iamr[7].P  = __get_IAMPR(7);
1276         __debug_mmu.iamr[8].L  = __get_IAMLR(8);
1277         __debug_mmu.iamr[8].P  = __get_IAMPR(8);
1278         __debug_mmu.iamr[9].L  = __get_IAMLR(9);
1279         __debug_mmu.iamr[9].P  = __get_IAMPR(9);
1280         __debug_mmu.iamr[10].L = __get_IAMLR(10);
1281         __debug_mmu.iamr[10].P = __get_IAMPR(10);
1282         __debug_mmu.iamr[11].L = __get_IAMLR(11);
1283         __debug_mmu.iamr[11].P = __get_IAMPR(11);
1284         __debug_mmu.iamr[12].L = __get_IAMLR(12);
1285         __debug_mmu.iamr[12].P = __get_IAMPR(12);
1286         __debug_mmu.iamr[13].L = __get_IAMLR(13);
1287         __debug_mmu.iamr[13].P = __get_IAMPR(13);
1288         __debug_mmu.iamr[14].L = __get_IAMLR(14);
1289         __debug_mmu.iamr[14].P = __get_IAMPR(14);
1290         __debug_mmu.iamr[15].L = __get_IAMLR(15);
1291         __debug_mmu.iamr[15].P = __get_IAMPR(15);
1292
1293         __debug_mmu.damr[0].L  = __get_DAMLR(0);
1294         __debug_mmu.damr[0].P  = __get_DAMPR(0);
1295         __debug_mmu.damr[1].L  = __get_DAMLR(1);
1296         __debug_mmu.damr[1].P  = __get_DAMPR(1);
1297         __debug_mmu.damr[2].L  = __get_DAMLR(2);
1298         __debug_mmu.damr[2].P  = __get_DAMPR(2);
1299         __debug_mmu.damr[3].L  = __get_DAMLR(3);
1300         __debug_mmu.damr[3].P  = __get_DAMPR(3);
1301         __debug_mmu.damr[4].L  = __get_DAMLR(4);
1302         __debug_mmu.damr[4].P  = __get_DAMPR(4);
1303         __debug_mmu.damr[5].L  = __get_DAMLR(5);
1304         __debug_mmu.damr[5].P  = __get_DAMPR(5);
1305         __debug_mmu.damr[6].L  = __get_DAMLR(6);
1306         __debug_mmu.damr[6].P  = __get_DAMPR(6);
1307         __debug_mmu.damr[7].L  = __get_DAMLR(7);
1308         __debug_mmu.damr[7].P  = __get_DAMPR(7);
1309         __debug_mmu.damr[8].L  = __get_DAMLR(8);
1310         __debug_mmu.damr[8].P  = __get_DAMPR(8);
1311         __debug_mmu.damr[9].L  = __get_DAMLR(9);
1312         __debug_mmu.damr[9].P  = __get_DAMPR(9);
1313         __debug_mmu.damr[10].L = __get_DAMLR(10);
1314         __debug_mmu.damr[10].P = __get_DAMPR(10);
1315         __debug_mmu.damr[11].L = __get_DAMLR(11);
1316         __debug_mmu.damr[11].P = __get_DAMPR(11);
1317         __debug_mmu.damr[12].L = __get_DAMLR(12);
1318         __debug_mmu.damr[12].P = __get_DAMPR(12);
1319         __debug_mmu.damr[13].L = __get_DAMLR(13);
1320         __debug_mmu.damr[13].P = __get_DAMPR(13);
1321         __debug_mmu.damr[14].L = __get_DAMLR(14);
1322         __debug_mmu.damr[14].P = __get_DAMPR(14);
1323         __debug_mmu.damr[15].L = __get_DAMLR(15);
1324         __debug_mmu.damr[15].P = __get_DAMPR(15);
1325
1326 #ifdef CONFIG_MMU
1327         do {
1328                 /* read the DAT entries from the TLB */
1329                 struct __debug_amr *p;
1330                 int loop;
1331
1332                 asm volatile("movsg tplr,%0" : "=r"(__debug_mmu.regs.tplr));
1333                 asm volatile("movsg tppr,%0" : "=r"(__debug_mmu.regs.tppr));
1334                 asm volatile("movsg tpxr,%0" : "=r"(__debug_mmu.regs.tpxr));
1335                 asm volatile("movsg cxnr,%0" : "=r"(__debug_mmu.regs.cxnr));
1336
1337                 p = __debug_mmu.tlb;
1338
1339                 /* way 0 */
1340                 asm volatile("movgs %0,tpxr" :: "r"(0 << TPXR_WAY_SHIFT));
1341                 for (loop = 0; loop < 64; loop++) {
1342                         asm volatile("tlbpr %0,gr0,#1,#0" :: "r"(loop << PAGE_SHIFT));
1343                         asm volatile("movsg tplr,%0" : "=r"(p->L));
1344                         asm volatile("movsg tppr,%0" : "=r"(p->P));
1345                         p++;
1346                 }
1347
1348                 /* way 1 */
1349                 asm volatile("movgs %0,tpxr" :: "r"(1 << TPXR_WAY_SHIFT));
1350                 for (loop = 0; loop < 64; loop++) {
1351                         asm volatile("tlbpr %0,gr0,#1,#0" :: "r"(loop << PAGE_SHIFT));
1352                         asm volatile("movsg tplr,%0" : "=r"(p->L));
1353                         asm volatile("movsg tppr,%0" : "=r"(p->P));
1354                         p++;
1355                 }
1356
1357                 asm volatile("movgs %0,tplr" :: "r"(__debug_mmu.regs.tplr));
1358                 asm volatile("movgs %0,tppr" :: "r"(__debug_mmu.regs.tppr));
1359                 asm volatile("movgs %0,tpxr" :: "r"(__debug_mmu.regs.tpxr));
1360         } while(0);
1361 #endif
1362
1363 } /* end gdbstub_get_mmu_state() */
1364
1365 /*****************************************************************************/
1366 /*
1367  * handle event interception and GDB remote protocol processing
1368  * - on entry:
1369  *      PSR.ET==0, PSR.S==1 and the CPU is in debug mode
1370  *      __debug_frame points to the saved registers
1371  *      __frame points to the kernel mode exception frame, if it was in kernel
1372  *      mode when the break happened
1373  */
1374 void gdbstub(int sigval)
1375 {
1376         unsigned long addr, length, loop, dbar, temp, temp2, temp3;
1377         uint32_t zero;
1378         char *ptr;
1379         int flush_cache = 0;
1380
1381         LEDS(0x5000);
1382
1383         if (sigval < 0) {
1384 #ifndef CONFIG_GDBSTUB_IMMEDIATE
1385                 /* return immediately if GDB immediate activation option not set */
1386                 return;
1387 #else
1388                 sigval = SIGINT;
1389 #endif
1390         }
1391
1392         save_user_regs(&__debug_frame0->uc);
1393
1394 #if 0
1395         gdbstub_printk("--> gdbstub() %08x %p %08x %08x\n",
1396                        __debug_frame->pc,
1397                        __debug_frame,
1398                        __debug_regs->brr,
1399                        __debug_regs->bpsr);
1400 //      gdbstub_show_regs();
1401 #endif
1402
1403         LEDS(0x5001);
1404
1405         /* if we were interrupted by input on the serial gdbstub serial port,
1406          * restore the context prior to the interrupt so that we return to that
1407          * directly
1408          */
1409         temp = (unsigned long) __entry_kerneltrap_table;
1410         temp2 = (unsigned long) __entry_usertrap_table;
1411         temp3 = __debug_frame->pc & ~15;
1412
1413         if (temp3 == temp + TBR_TT_INTERRUPT_15 ||
1414             temp3 == temp2 + TBR_TT_INTERRUPT_15
1415             ) {
1416                 asm volatile("movsg pcsr,%0" : "=r"(__debug_frame->pc));
1417                 __debug_frame->psr |= PSR_ET;
1418                 __debug_frame->psr &= ~PSR_S;
1419                 if (__debug_frame->psr & PSR_PS)
1420                         __debug_frame->psr |= PSR_S;
1421                 __debug_status.brr = (__debug_frame->tbr & TBR_TT) << 12;
1422                 __debug_status.brr |= BRR_EB;
1423                 sigval = SIGINT;
1424         }
1425
1426         /* handle the decrement timer going off (FR451 only) */
1427         if (temp3 == temp + TBR_TT_DECREMENT_TIMER ||
1428             temp3 == temp2 + TBR_TT_DECREMENT_TIMER
1429             ) {
1430                 asm volatile("movgs %0,timerd" :: "r"(10000000));
1431                 asm volatile("movsg pcsr,%0" : "=r"(__debug_frame->pc));
1432                 __debug_frame->psr |= PSR_ET;
1433                 __debug_frame->psr &= ~PSR_S;
1434                 if (__debug_frame->psr & PSR_PS)
1435                         __debug_frame->psr |= PSR_S;
1436                 __debug_status.brr = (__debug_frame->tbr & TBR_TT) << 12;
1437                 __debug_status.brr |= BRR_EB;
1438                 sigval = SIGXCPU;
1439         }
1440
1441         LEDS(0x5002);
1442
1443         /* after a BREAK insn, the PC lands on the far side of it */
1444         if (__debug_status.brr & BRR_SB)
1445                 gdbstub_check_breakpoint();
1446
1447         LEDS(0x5003);
1448
1449         /* handle attempts to write console data via GDB "O" commands */
1450         if (__debug_frame->pc == (unsigned long) gdbstub_console_write + 4) {
1451                 __gdbstub_console_write((struct console *) __debug_frame->gr8,
1452                                         (const char *) __debug_frame->gr9,
1453                                         (unsigned) __debug_frame->gr10);
1454                 goto done;
1455         }
1456
1457         if (gdbstub_rx_unget) {
1458                 sigval = SIGINT;
1459                 goto packet_waiting;
1460         }
1461
1462         if (!sigval)
1463                 sigval = gdbstub_compute_signal(__debug_status.brr);
1464
1465         LEDS(0x5004);
1466
1467         /* send a message to the debugger's user saying what happened if it may
1468          * not be clear cut (we can't map exceptions onto signals properly)
1469          */
1470         if (sigval != SIGINT && sigval != SIGTRAP && sigval != SIGILL) {
1471                 static const char title[] = "Break ";
1472                 static const char crlf[] = "\r\n";
1473                 unsigned long brr = __debug_status.brr;
1474                 char hx;
1475
1476                 ptr = output_buffer;
1477                 *ptr++ = 'O';
1478                 ptr = mem2hex(title, ptr, sizeof(title) - 1,0);
1479
1480                 hx = hexchars[(brr & 0xf0000000) >> 28];
1481                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1482                 hx = hexchars[(brr & 0x0f000000) >> 24];
1483                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1484                 hx = hexchars[(brr & 0x00f00000) >> 20];
1485                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1486                 hx = hexchars[(brr & 0x000f0000) >> 16];
1487                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1488                 hx = hexchars[(brr & 0x0000f000) >> 12];
1489                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1490                 hx = hexchars[(brr & 0x00000f00) >> 8];
1491                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1492                 hx = hexchars[(brr & 0x000000f0) >> 4];
1493                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1494                 hx = hexchars[(brr & 0x0000000f)];
1495                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1496
1497                 ptr = mem2hex(crlf, ptr, sizeof(crlf) - 1, 0);
1498                 *ptr = 0;
1499                 gdbstub_send_packet(output_buffer);     /* send it off... */
1500         }
1501
1502         LEDS(0x5005);
1503
1504         /* tell the debugger that an exception has occurred */
1505         ptr = output_buffer;
1506
1507         /* Send trap type (converted to signal) */
1508         *ptr++ = 'T';
1509         *ptr++ = hexchars[sigval >> 4];
1510         *ptr++ = hexchars[sigval & 0xf];
1511
1512         /* Send Error PC */
1513         *ptr++ = hexchars[GDB_REG_PC >> 4];
1514         *ptr++ = hexchars[GDB_REG_PC & 0xf];
1515         *ptr++ = ':';
1516         ptr = mem2hex(&__debug_frame->pc, ptr, 4, 0);
1517         *ptr++ = ';';
1518
1519         /*
1520          * Send frame pointer
1521          */
1522         *ptr++ = hexchars[GDB_REG_FP >> 4];
1523         *ptr++ = hexchars[GDB_REG_FP & 0xf];
1524         *ptr++ = ':';
1525         ptr = mem2hex(&__debug_frame->fp, ptr, 4, 0);
1526         *ptr++ = ';';
1527
1528         /*
1529          * Send stack pointer
1530          */
1531         *ptr++ = hexchars[GDB_REG_SP >> 4];
1532         *ptr++ = hexchars[GDB_REG_SP & 0xf];
1533         *ptr++ = ':';
1534         ptr = mem2hex(&__debug_frame->sp, ptr, 4, 0);
1535         *ptr++ = ';';
1536
1537         *ptr++ = 0;
1538         gdbstub_send_packet(output_buffer);     /* send it off... */
1539
1540         LEDS(0x5006);
1541
1542  packet_waiting:
1543         gdbstub_get_mmu_state();
1544
1545         /* wait for input from remote GDB */
1546         while (1) {
1547                 output_buffer[0] = 0;
1548
1549                 LEDS(0x5007);
1550                 gdbstub_recv_packet(input_buffer);
1551                 LEDS(0x5600 | input_buffer[0]);
1552
1553                 switch (input_buffer[0]) {
1554                         /* request repeat of last signal number */
1555                 case '?':
1556                         output_buffer[0] = 'S';
1557                         output_buffer[1] = hexchars[sigval >> 4];
1558                         output_buffer[2] = hexchars[sigval & 0xf];
1559                         output_buffer[3] = 0;
1560                         break;
1561
1562                 case 'd':
1563                         /* toggle debug flag */
1564                         break;
1565
1566                         /* return the value of the CPU registers
1567                          * - GR0,  GR1,  GR2,  GR3,  GR4,  GR5,  GR6,  GR7,
1568                          * - GR8,  GR9,  GR10, GR11, GR12, GR13, GR14, GR15,
1569                          * - GR16, GR17, GR18, GR19, GR20, GR21, GR22, GR23,
1570                          * - GR24, GR25, GR26, GR27, GR28, GR29, GR30, GR31,
1571                          * - GR32, GR33, GR34, GR35, GR36, GR37, GR38, GR39,
1572                          * - GR40, GR41, GR42, GR43, GR44, GR45, GR46, GR47,
1573                          * - GR48, GR49, GR50, GR51, GR52, GR53, GR54, GR55,
1574                          * - GR56, GR57, GR58, GR59, GR60, GR61, GR62, GR63,
1575                          * - FP0,  FP1,  FP2,  FP3,  FP4,  FP5,  FP6,  FP7,
1576                          * - FP8,  FP9,  FP10, FP11, FP12, FP13, FP14, FP15,
1577                          * - FP16, FP17, FP18, FP19, FP20, FP21, FP22, FP23,
1578                          * - FP24, FP25, FP26, FP27, FP28, FP29, FP30, FP31,
1579                          * - FP32, FP33, FP34, FP35, FP36, FP37, FP38, FP39,
1580                          * - FP40, FP41, FP42, FP43, FP44, FP45, FP46, FP47,
1581                          * - FP48, FP49, FP50, FP51, FP52, FP53, FP54, FP55,
1582                          * - FP56, FP57, FP58, FP59, FP60, FP61, FP62, FP63,
1583                          * - PC, PSR, CCR, CCCR,
1584                          * - _X132, _X133, _X134
1585                          * - TBR, BRR, DBAR0, DBAR1, DBAR2, DBAR3,
1586                          * - _X141, _X142, _X143, _X144,
1587                          * - LR, LCR
1588                          */
1589                 case 'g':
1590                         zero = 0;
1591                         ptr = output_buffer;
1592
1593                         /* deal with GR0, GR1-GR27, GR28-GR31, GR32-GR63 */
1594                         ptr = mem2hex(&zero, ptr, 4, 0);
1595
1596                         for (loop = 1; loop <= 27; loop++)
1597                                 ptr = mem2hex(&__debug_user_context->i.gr[loop], ptr, 4, 0);
1598                         temp = (unsigned long) __frame;
1599                         ptr = mem2hex(&temp, ptr, 4, 0);
1600                         ptr = mem2hex(&__debug_user_context->i.gr[29], ptr, 4, 0);
1601                         ptr = mem2hex(&__debug_user_context->i.gr[30], ptr, 4, 0);
1602 #ifdef CONFIG_MMU
1603                         ptr = mem2hex(&__debug_user_context->i.gr[31], ptr, 4, 0);
1604 #else
1605                         temp = (unsigned long) __debug_frame;
1606                         ptr = mem2hex(&temp, ptr, 4, 0);
1607 #endif
1608
1609                         for (loop = 32; loop <= 63; loop++)
1610                                 ptr = mem2hex(&__debug_user_context->i.gr[loop], ptr, 4, 0);
1611
1612                         /* deal with FR0-FR63 */
1613                         for (loop = 0; loop <= 63; loop++)
1614                                 ptr = mem2hex(&__debug_user_context->f.fr[loop], ptr, 4, 0);
1615
1616                         /* deal with special registers */
1617                         ptr = mem2hex(&__debug_frame->pc,    ptr, 4, 0);
1618                         ptr = mem2hex(&__debug_frame->psr,   ptr, 4, 0);
1619                         ptr = mem2hex(&__debug_frame->ccr,   ptr, 4, 0);
1620                         ptr = mem2hex(&__debug_frame->cccr,  ptr, 4, 0);
1621                         ptr = mem2hex(&zero, ptr, 4, 0);
1622                         ptr = mem2hex(&zero, ptr, 4, 0);
1623                         ptr = mem2hex(&zero, ptr, 4, 0);
1624                         ptr = mem2hex(&__debug_frame->tbr,   ptr, 4, 0);
1625                         ptr = mem2hex(&__debug_status.brr ,   ptr, 4, 0);
1626
1627                         asm volatile("movsg dbar0,%0" : "=r"(dbar));
1628                         ptr = mem2hex(&dbar, ptr, 4, 0);
1629                         asm volatile("movsg dbar1,%0" : "=r"(dbar));
1630                         ptr = mem2hex(&dbar, ptr, 4, 0);
1631                         asm volatile("movsg dbar2,%0" : "=r"(dbar));
1632                         ptr = mem2hex(&dbar, ptr, 4, 0);
1633                         asm volatile("movsg dbar3,%0" : "=r"(dbar));
1634                         ptr = mem2hex(&dbar, ptr, 4, 0);
1635
1636                         asm volatile("movsg scr0,%0" : "=r"(dbar));
1637                         ptr = mem2hex(&dbar, ptr, 4, 0);
1638                         asm volatile("movsg scr1,%0" : "=r"(dbar));
1639                         ptr = mem2hex(&dbar, ptr, 4, 0);
1640                         asm volatile("movsg scr2,%0" : "=r"(dbar));
1641                         ptr = mem2hex(&dbar, ptr, 4, 0);
1642                         asm volatile("movsg scr3,%0" : "=r"(dbar));
1643                         ptr = mem2hex(&dbar, ptr, 4, 0);
1644
1645                         ptr = mem2hex(&__debug_frame->lr, ptr, 4, 0);
1646                         ptr = mem2hex(&__debug_frame->lcr, ptr, 4, 0);
1647
1648                         ptr = mem2hex(&__debug_frame->iacc0, ptr, 8, 0);
1649
1650                         ptr = mem2hex(&__debug_user_context->f.fsr[0], ptr, 4, 0);
1651
1652                         for (loop = 0; loop <= 7; loop++)
1653                                 ptr = mem2hex(&__debug_user_context->f.acc[loop], ptr, 4, 0);
1654
1655                         ptr = mem2hex(&__debug_user_context->f.accg, ptr, 8, 0);
1656
1657                         for (loop = 0; loop <= 1; loop++)
1658                                 ptr = mem2hex(&__debug_user_context->f.msr[loop], ptr, 4, 0);
1659
1660                         ptr = mem2hex(&__debug_frame->gner0, ptr, 4, 0);
1661                         ptr = mem2hex(&__debug_frame->gner1, ptr, 4, 0);
1662
1663                         ptr = mem2hex(&__debug_user_context->f.fner[0], ptr, 4, 0);
1664                         ptr = mem2hex(&__debug_user_context->f.fner[1], ptr, 4, 0);
1665
1666                         break;
1667
1668                         /* set the values of the CPU registers */
1669                 case 'G':
1670                         ptr = &input_buffer[1];
1671
1672                         /* deal with GR0, GR1-GR27, GR28-GR31, GR32-GR63 */
1673                         ptr = hex2mem(ptr, &temp, 4);
1674
1675                         for (loop = 1; loop <= 27; loop++)
1676                                 ptr = hex2mem(ptr, &__debug_user_context->i.gr[loop], 4);
1677
1678                         ptr = hex2mem(ptr, &temp, 4);
1679                         __frame = (struct pt_regs *) temp;
1680                         ptr = hex2mem(ptr, &__debug_frame->gr29, 4);
1681                         ptr = hex2mem(ptr, &__debug_frame->gr30, 4);
1682 #ifdef CONFIG_MMU
1683                         ptr = hex2mem(ptr, &__debug_frame->gr31, 4);
1684 #else
1685                         ptr = hex2mem(ptr, &temp, 4);
1686 #endif
1687
1688                         for (loop = 32; loop <= 63; loop++)
1689                                 ptr = hex2mem(ptr, &__debug_user_context->i.gr[loop], 4);
1690
1691                         /* deal with FR0-FR63 */
1692                         for (loop = 0; loop <= 63; loop++)
1693                                 ptr = mem2hex(&__debug_user_context->f.fr[loop], ptr, 4, 0);
1694
1695                         /* deal with special registers */
1696                         ptr = hex2mem(ptr, &__debug_frame->pc,  4);
1697                         ptr = hex2mem(ptr, &__debug_frame->psr, 4);
1698                         ptr = hex2mem(ptr, &__debug_frame->ccr, 4);
1699                         ptr = hex2mem(ptr, &__debug_frame->cccr,4);
1700
1701                         for (loop = 132; loop <= 140; loop++)
1702                                 ptr = hex2mem(ptr, &temp, 4);
1703
1704                         ptr = hex2mem(ptr, &temp, 4);
1705                         asm volatile("movgs %0,scr0" :: "r"(temp));
1706                         ptr = hex2mem(ptr, &temp, 4);
1707                         asm volatile("movgs %0,scr1" :: "r"(temp));
1708                         ptr = hex2mem(ptr, &temp, 4);
1709                         asm volatile("movgs %0,scr2" :: "r"(temp));
1710                         ptr = hex2mem(ptr, &temp, 4);
1711                         asm volatile("movgs %0,scr3" :: "r"(temp));
1712
1713                         ptr = hex2mem(ptr, &__debug_frame->lr,  4);
1714                         ptr = hex2mem(ptr, &__debug_frame->lcr, 4);
1715
1716                         ptr = hex2mem(ptr, &__debug_frame->iacc0, 8);
1717
1718                         ptr = hex2mem(ptr, &__debug_user_context->f.fsr[0], 4);
1719
1720                         for (loop = 0; loop <= 7; loop++)
1721                                 ptr = hex2mem(ptr, &__debug_user_context->f.acc[loop], 4);
1722
1723                         ptr = hex2mem(ptr, &__debug_user_context->f.accg, 8);
1724
1725                         for (loop = 0; loop <= 1; loop++)
1726                                 ptr = hex2mem(ptr, &__debug_user_context->f.msr[loop], 4);
1727
1728                         ptr = hex2mem(ptr, &__debug_frame->gner0, 4);
1729                         ptr = hex2mem(ptr, &__debug_frame->gner1, 4);
1730
1731                         ptr = hex2mem(ptr, &__debug_user_context->f.fner[0], 4);
1732                         ptr = hex2mem(ptr, &__debug_user_context->f.fner[1], 4);
1733
1734                         gdbstub_strcpy(output_buffer,"OK");
1735                         break;
1736
1737                         /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
1738                 case 'm':
1739                         ptr = &input_buffer[1];
1740
1741                         if (hexToInt(&ptr, &addr) &&
1742                             *ptr++ == ',' &&
1743                             hexToInt(&ptr, &length)
1744                             ) {
1745                                 if (mem2hex((char *)addr, output_buffer, length, 1))
1746                                         break;
1747                                 gdbstub_strcpy (output_buffer, "E03");
1748                         }
1749                         else {
1750                                 gdbstub_strcpy(output_buffer,"E01");
1751                         }
1752                         break;
1753
1754                         /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
1755                 case 'M':
1756                         ptr = &input_buffer[1];
1757
1758                         if (hexToInt(&ptr, &addr) &&
1759                             *ptr++ == ',' &&
1760                             hexToInt(&ptr, &length) &&
1761                             *ptr++ == ':'
1762                             ) {
1763                                 if (hex2mem(ptr, (char *)addr, length)) {
1764                                         gdbstub_strcpy(output_buffer, "OK");
1765                                 }
1766                                 else {
1767                                         gdbstub_strcpy(output_buffer, "E03");
1768                                 }
1769                         }
1770                         else
1771                                 gdbstub_strcpy(output_buffer, "E02");
1772
1773                         flush_cache = 1;
1774                         break;
1775
1776                         /* PNN,=RRRRRRRR: Write value R to reg N return OK */
1777                 case 'P':
1778                         ptr = &input_buffer[1];
1779
1780                         if (!hexToInt(&ptr, &addr) ||
1781                             *ptr++ != '=' ||
1782                             !hexToInt(&ptr, &temp)
1783                             ) {
1784                                 gdbstub_strcpy(output_buffer, "E01");
1785                                 break;
1786                         }
1787
1788                         temp2 = 1;
1789                         switch (addr) {
1790                         case GDB_REG_GR(0):
1791                                 break;
1792                         case GDB_REG_GR(1) ... GDB_REG_GR(63):
1793                                 __debug_user_context->i.gr[addr - GDB_REG_GR(0)] = temp;
1794                                 break;
1795                         case GDB_REG_FR(0) ... GDB_REG_FR(63):
1796                                 __debug_user_context->f.fr[addr - GDB_REG_FR(0)] = temp;
1797                                 break;
1798                         case GDB_REG_PC:
1799                                 __debug_user_context->i.pc = temp;
1800                                 break;
1801                         case GDB_REG_PSR:
1802                                 __debug_user_context->i.psr = temp;
1803                                 break;
1804                         case GDB_REG_CCR:
1805                                 __debug_user_context->i.ccr = temp;
1806                                 break;
1807                         case GDB_REG_CCCR:
1808                                 __debug_user_context->i.cccr = temp;
1809                                 break;
1810                         case GDB_REG_BRR:
1811                                 __debug_status.brr = temp;
1812                                 break;
1813                         case GDB_REG_LR:
1814                                 __debug_user_context->i.lr = temp;
1815                                 break;
1816                         case GDB_REG_LCR:
1817                                 __debug_user_context->i.lcr = temp;
1818                                 break;
1819                         case GDB_REG_FSR0:
1820                                 __debug_user_context->f.fsr[0] = temp;
1821                                 break;
1822                         case GDB_REG_ACC(0) ... GDB_REG_ACC(7):
1823                                 __debug_user_context->f.acc[addr - GDB_REG_ACC(0)] = temp;
1824                                 break;
1825                         case GDB_REG_ACCG(0):
1826                                 *(uint32_t *) &__debug_user_context->f.accg[0] = temp;
1827                                 break;
1828                         case GDB_REG_ACCG(4):
1829                                 *(uint32_t *) &__debug_user_context->f.accg[4] = temp;
1830                                 break;
1831                         case GDB_REG_MSR(0) ... GDB_REG_MSR(1):
1832                                 __debug_user_context->f.msr[addr - GDB_REG_MSR(0)] = temp;
1833                                 break;
1834                         case GDB_REG_GNER(0) ... GDB_REG_GNER(1):
1835                                 __debug_user_context->i.gner[addr - GDB_REG_GNER(0)] = temp;
1836                                 break;
1837                         case GDB_REG_FNER(0) ... GDB_REG_FNER(1):
1838                                 __debug_user_context->f.fner[addr - GDB_REG_FNER(0)] = temp;
1839                                 break;
1840                         default:
1841                                 temp2 = 0;
1842                                 break;
1843                         }
1844
1845                         if (temp2) {
1846                                 gdbstub_strcpy(output_buffer, "OK");
1847                         }
1848                         else {
1849                                 gdbstub_strcpy(output_buffer, "E02");
1850                         }
1851                         break;
1852
1853                         /* cAA..AA    Continue at address AA..AA(optional) */
1854                 case 'c':
1855                         /* try to read optional parameter, pc unchanged if no parm */
1856                         ptr = &input_buffer[1];
1857                         if (hexToInt(&ptr, &addr))
1858                                 __debug_frame->pc = addr;
1859                         goto done;
1860
1861                         /* kill the program */
1862                 case 'k' :
1863                         goto done;      /* just continue */
1864
1865
1866                         /* reset the whole machine (FIXME: system dependent) */
1867                 case 'r':
1868                         break;
1869
1870
1871                         /* step to next instruction */
1872                 case 's':
1873                         __debug_regs->dcr |= DCR_SE;
1874                         __debug_status.dcr |= DCR_SE;
1875                         goto done;
1876
1877                         /* set baud rate (bBB) */
1878                 case 'b':
1879                         ptr = &input_buffer[1];
1880                         if (!hexToInt(&ptr, &temp)) {
1881                                 gdbstub_strcpy(output_buffer,"B01");
1882                                 break;
1883                         }
1884
1885                         if (temp) {
1886                                 /* ack before changing speed */
1887                                 gdbstub_send_packet("OK");
1888                                 gdbstub_set_baud(temp);
1889                         }
1890                         break;
1891
1892                         /* set breakpoint */
1893                 case 'Z':
1894                         ptr = &input_buffer[1];
1895
1896                         if (!hexToInt(&ptr,&temp) || *ptr++ != ',' ||
1897                             !hexToInt(&ptr,&addr) || *ptr++ != ',' ||
1898                             !hexToInt(&ptr,&length)
1899                             ) {
1900                                 gdbstub_strcpy(output_buffer,"E01");
1901                                 break;
1902                         }
1903
1904                         if (temp >= 5) {
1905                                 gdbstub_strcpy(output_buffer,"E03");
1906                                 break;
1907                         }
1908
1909                         if (gdbstub_set_breakpoint(temp, addr, length) < 0) {
1910                                 gdbstub_strcpy(output_buffer,"E03");
1911                                 break;
1912                         }
1913
1914                         if (temp == 0)
1915                                 flush_cache = 1; /* soft bkpt by modified memory */
1916
1917                         gdbstub_strcpy(output_buffer,"OK");
1918                         break;
1919
1920                         /* clear breakpoint */
1921                 case 'z':
1922                         ptr = &input_buffer[1];
1923
1924                         if (!hexToInt(&ptr,&temp) || *ptr++ != ',' ||
1925                             !hexToInt(&ptr,&addr) || *ptr++ != ',' ||
1926                             !hexToInt(&ptr,&length)
1927                             ) {
1928                                 gdbstub_strcpy(output_buffer,"E01");
1929                                 break;
1930                         }
1931
1932                         if (temp >= 5) {
1933                                 gdbstub_strcpy(output_buffer,"E03");
1934                                 break;
1935                         }
1936
1937                         if (gdbstub_clear_breakpoint(temp, addr, length) < 0) {
1938                                 gdbstub_strcpy(output_buffer,"E03");
1939                                 break;
1940                         }
1941
1942                         if (temp == 0)
1943                                 flush_cache = 1; /* soft bkpt by modified memory */
1944
1945                         gdbstub_strcpy(output_buffer,"OK");
1946                         break;
1947
1948                 default:
1949                         gdbstub_proto("### GDB Unsupported Cmd '%s'\n",input_buffer);
1950                         break;
1951                 }
1952
1953                 /* reply to the request */
1954                 LEDS(0x5009);
1955                 gdbstub_send_packet(output_buffer);
1956         }
1957
1958  done:
1959         restore_user_regs(&__debug_frame0->uc);
1960
1961         //gdbstub_dump_debugregs();
1962         //gdbstub_printk("<-- gdbstub() %08x\n", __debug_frame->pc);
1963
1964         /* need to flush the instruction cache before resuming, as we may have
1965          * deposited a breakpoint, and the icache probably has no way of
1966          * knowing that a data ref to some location may have changed something
1967          * that is in the instruction cache.  NB: We flush both caches, just to
1968          * be sure...
1969          */
1970
1971         /* note: flushing the icache will clobber EAR0 on the FR451 */
1972         if (flush_cache)
1973                 gdbstub_purge_cache();
1974
1975         LEDS(0x5666);
1976
1977 } /* end gdbstub() */
1978
1979 /*****************************************************************************/
1980 /*
1981  * initialise the GDB stub
1982  */
1983 void __init gdbstub_init(void)
1984 {
1985 #ifdef CONFIG_GDBSTUB_IMMEDIATE
1986         unsigned char ch;
1987         int ret;
1988 #endif
1989
1990         gdbstub_printk("%s", gdbstub_banner);
1991
1992         gdbstub_io_init();
1993
1994         /* try to talk to GDB (or anyone insane enough to want to type GDB protocol by hand) */
1995         gdbstub_proto("### GDB Tx ACK\n");
1996         gdbstub_tx_char('+'); /* 'hello world' */
1997
1998 #ifdef CONFIG_GDBSTUB_IMMEDIATE
1999         gdbstub_printk("GDB Stub waiting for packet\n");
2000
2001         /*
2002          * In case GDB is started before us, ack any packets
2003          * (presumably "$?#xx") sitting there.
2004          */
2005         do { gdbstub_rx_char(&ch, 0); } while (ch != '$');
2006         do { gdbstub_rx_char(&ch, 0); } while (ch != '#');
2007         do { ret = gdbstub_rx_char(&ch, 0); } while (ret != 0); /* eat first csum byte */
2008         do { ret = gdbstub_rx_char(&ch, 0); } while (ret != 0); /* eat second csum byte */
2009
2010         gdbstub_proto("### GDB Tx NAK\n");
2011         gdbstub_tx_char('-'); /* nak it */
2012
2013 #else
2014         gdbstub_printk("GDB Stub set\n");
2015 #endif
2016
2017 #if 0
2018         /* send banner */
2019         ptr = output_buffer;
2020         *ptr++ = 'O';
2021         ptr = mem2hex(gdbstub_banner, ptr, sizeof(gdbstub_banner) - 1, 0);
2022         gdbstub_send_packet(output_buffer);
2023 #endif
2024 #if defined(CONFIG_GDBSTUB_CONSOLE) && defined(CONFIG_GDBSTUB_IMMEDIATE)
2025         register_console(&gdbstub_console);
2026 #endif
2027
2028 } /* end gdbstub_init() */
2029
2030 /*****************************************************************************/
2031 /*
2032  * register the console at a more appropriate time
2033  */
2034 #if defined (CONFIG_GDBSTUB_CONSOLE) && !defined(CONFIG_GDBSTUB_IMMEDIATE)
2035 static int __init gdbstub_postinit(void)
2036 {
2037         printk("registering console\n");
2038         register_console(&gdbstub_console);
2039         return 0;
2040 } /* end gdbstub_postinit() */
2041
2042 __initcall(gdbstub_postinit);
2043 #endif
2044
2045 /*****************************************************************************/
2046 /*
2047  * send an exit message to GDB
2048  */
2049 void gdbstub_exit(int status)
2050 {
2051         unsigned char checksum;
2052         int count;
2053         unsigned char ch;
2054
2055         sprintf(output_buffer,"W%02x",status&0xff);
2056
2057         gdbstub_tx_char('$');
2058         checksum = 0;
2059         count = 0;
2060
2061         while ((ch = output_buffer[count]) != 0) {
2062                 gdbstub_tx_char(ch);
2063                 checksum += ch;
2064                 count += 1;
2065         }
2066
2067         gdbstub_tx_char('#');
2068         gdbstub_tx_char(hexchars[checksum >> 4]);
2069         gdbstub_tx_char(hexchars[checksum & 0xf]);
2070
2071         /* make sure the output is flushed, or else RedBoot might clobber it */
2072         gdbstub_tx_char('-');
2073         gdbstub_tx_flush();
2074
2075 } /* end gdbstub_exit() */
2076
2077 /*****************************************************************************/
2078 /*
2079  * GDB wants to call malloc() and free() to allocate memory for calling kernel
2080  * functions directly from its command line
2081  */
2082 static void *malloc(size_t size) __attribute__((unused));
2083 static void *malloc(size_t size)
2084 {
2085         return kmalloc(size, GFP_ATOMIC);
2086 }
2087
2088 static void free(void *p) __attribute__((unused));
2089 static void free(void *p)
2090 {
2091         kfree(p);
2092 }
2093
2094 static uint32_t ___get_HSR0(void) __attribute__((unused));
2095 static uint32_t ___get_HSR0(void)
2096 {
2097         return __get_HSR(0);
2098 }
2099
2100 static uint32_t ___set_HSR0(uint32_t x) __attribute__((unused));
2101 static uint32_t ___set_HSR0(uint32_t x)
2102 {
2103         __set_HSR(0, x);
2104         return __get_HSR(0);
2105 }