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