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