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 /*
1349  * handle event interception and GDB remote protocol processing
1350  * - on entry:
1351  *      PSR.ET==0, PSR.S==1 and the CPU is in debug mode
1352  *      __debug_frame points to the saved registers
1353  *      __frame points to the kernel mode exception frame, if it was in kernel
1354  *      mode when the break happened
1355  */
1356 void gdbstub(int sigval)
1357 {
1358         unsigned long addr, length, loop, dbar, temp, temp2, temp3;
1359         uint32_t zero;
1360         char *ptr;
1361         int flush_cache = 0;
1362
1363         LEDS(0x5000);
1364
1365         if (sigval < 0) {
1366 #ifndef CONFIG_GDBSTUB_IMMEDIATE
1367                 /* return immediately if GDB immediate activation option not set */
1368                 return;
1369 #else
1370                 sigval = SIGINT;
1371 #endif
1372         }
1373
1374         save_user_regs(&__debug_frame0->uc);
1375
1376 #if 0
1377         gdbstub_printk("--> gdbstub() %08x %p %08x %08x\n",
1378                        __debug_frame->pc,
1379                        __debug_frame,
1380                        __debug_regs->brr,
1381                        __debug_regs->bpsr);
1382 //      gdbstub_show_regs();
1383 #endif
1384
1385         LEDS(0x5001);
1386
1387         /* if we were interrupted by input on the serial gdbstub serial port,
1388          * restore the context prior to the interrupt so that we return to that
1389          * directly
1390          */
1391         temp = (unsigned long) __entry_kerneltrap_table;
1392         temp2 = (unsigned long) __entry_usertrap_table;
1393         temp3 = __debug_frame->pc & ~15;
1394
1395         if (temp3 == temp + TBR_TT_INTERRUPT_15 ||
1396             temp3 == temp2 + TBR_TT_INTERRUPT_15
1397             ) {
1398                 asm volatile("movsg pcsr,%0" : "=r"(__debug_frame->pc));
1399                 __debug_frame->psr |= PSR_ET;
1400                 __debug_frame->psr &= ~PSR_S;
1401                 if (__debug_frame->psr & PSR_PS)
1402                         __debug_frame->psr |= PSR_S;
1403                 __debug_status.brr = (__debug_frame->tbr & TBR_TT) << 12;
1404                 __debug_status.brr |= BRR_EB;
1405                 sigval = SIGINT;
1406         }
1407
1408         /* handle the decrement timer going off (FR451 only) */
1409         if (temp3 == temp + TBR_TT_DECREMENT_TIMER ||
1410             temp3 == temp2 + TBR_TT_DECREMENT_TIMER
1411             ) {
1412                 asm volatile("movgs %0,timerd" :: "r"(10000000));
1413                 asm volatile("movsg pcsr,%0" : "=r"(__debug_frame->pc));
1414                 __debug_frame->psr |= PSR_ET;
1415                 __debug_frame->psr &= ~PSR_S;
1416                 if (__debug_frame->psr & PSR_PS)
1417                         __debug_frame->psr |= PSR_S;
1418                 __debug_status.brr = (__debug_frame->tbr & TBR_TT) << 12;
1419                 __debug_status.brr |= BRR_EB;
1420                 sigval = SIGXCPU;
1421         }
1422
1423         LEDS(0x5002);
1424
1425         /* after a BREAK insn, the PC lands on the far side of it */
1426         if (__debug_status.brr & BRR_SB)
1427                 gdbstub_check_breakpoint();
1428
1429         LEDS(0x5003);
1430
1431         /* handle attempts to write console data via GDB "O" commands */
1432         if (__debug_frame->pc == (unsigned long) gdbstub_console_write + 4) {
1433                 __gdbstub_console_write((struct console *) __debug_frame->gr8,
1434                                         (const char *) __debug_frame->gr9,
1435                                         (unsigned) __debug_frame->gr10);
1436                 goto done;
1437         }
1438
1439         if (gdbstub_rx_unget) {
1440                 sigval = SIGINT;
1441                 goto packet_waiting;
1442         }
1443
1444         if (!sigval)
1445                 sigval = gdbstub_compute_signal(__debug_status.brr);
1446
1447         LEDS(0x5004);
1448
1449         /* send a message to the debugger's user saying what happened if it may
1450          * not be clear cut (we can't map exceptions onto signals properly)
1451          */
1452         if (sigval != SIGINT && sigval != SIGTRAP && sigval != SIGILL) {
1453                 static const char title[] = "Break ";
1454                 static const char crlf[] = "\r\n";
1455                 unsigned long brr = __debug_status.brr;
1456                 char hx;
1457
1458                 ptr = output_buffer;
1459                 *ptr++ = 'O';
1460                 ptr = mem2hex(title, ptr, sizeof(title) - 1,0);
1461
1462                 hx = hex_asc_hi(brr >> 24);
1463                 ptr = pack_hex_byte(ptr, hx);
1464                 hx = hex_asc_lo(brr >> 24);
1465                 ptr = pack_hex_byte(ptr, hx);
1466                 hx = hex_asc_hi(brr >> 16);
1467                 ptr = pack_hex_byte(ptr, hx);
1468                 hx = hex_asc_lo(brr >> 16);
1469                 ptr = pack_hex_byte(ptr, hx);
1470                 hx = hex_asc_hi(brr >> 8);
1471                 ptr = pack_hex_byte(ptr, hx);
1472                 hx = hex_asc_lo(brr >> 8);
1473                 ptr = pack_hex_byte(ptr, hx);
1474                 hx = hex_asc_hi(brr);
1475                 ptr = pack_hex_byte(ptr, hx);
1476                 hx = hex_asc_lo(brr);
1477                 ptr = pack_hex_byte(ptr, hx);
1478
1479                 ptr = mem2hex(crlf, ptr, sizeof(crlf) - 1, 0);
1480                 *ptr = 0;
1481                 gdbstub_send_packet(output_buffer);     /* send it off... */
1482         }
1483
1484         LEDS(0x5005);
1485
1486         /* tell the debugger that an exception has occurred */
1487         ptr = output_buffer;
1488
1489         /* Send trap type (converted to signal) */
1490         *ptr++ = 'T';
1491         ptr = pack_hex_byte(ptr, sigval);
1492
1493         /* Send Error PC */
1494         ptr = pack_hex_byte(ptr, GDB_REG_PC);
1495         *ptr++ = ':';
1496         ptr = mem2hex(&__debug_frame->pc, ptr, 4, 0);
1497         *ptr++ = ';';
1498
1499         /*
1500          * Send frame pointer
1501          */
1502         ptr = pack_hex_byte(ptr, GDB_REG_FP);
1503         *ptr++ = ':';
1504         ptr = mem2hex(&__debug_frame->fp, ptr, 4, 0);
1505         *ptr++ = ';';
1506
1507         /*
1508          * Send stack pointer
1509          */
1510         ptr = pack_hex_byte(ptr, GDB_REG_SP);
1511         *ptr++ = ':';
1512         ptr = mem2hex(&__debug_frame->sp, ptr, 4, 0);
1513         *ptr++ = ';';
1514
1515         *ptr++ = 0;
1516         gdbstub_send_packet(output_buffer);     /* send it off... */
1517
1518         LEDS(0x5006);
1519
1520  packet_waiting:
1521         gdbstub_get_mmu_state();
1522
1523         /* wait for input from remote GDB */
1524         while (1) {
1525                 output_buffer[0] = 0;
1526
1527                 LEDS(0x5007);
1528                 gdbstub_recv_packet(input_buffer);
1529                 LEDS(0x5600 | input_buffer[0]);
1530
1531                 switch (input_buffer[0]) {
1532                         /* request repeat of last signal number */
1533                 case '?':
1534                         output_buffer[0] = 'S';
1535                         output_buffer[1] = hex_asc_hi(sigval);
1536                         output_buffer[2] = hex_asc_lo(sigval);
1537                         output_buffer[3] = 0;
1538                         break;
1539
1540                 case 'd':
1541                         /* toggle debug flag */
1542                         break;
1543
1544                         /* return the value of the CPU registers
1545                          * - GR0,  GR1,  GR2,  GR3,  GR4,  GR5,  GR6,  GR7,
1546                          * - GR8,  GR9,  GR10, GR11, GR12, GR13, GR14, GR15,
1547                          * - GR16, GR17, GR18, GR19, GR20, GR21, GR22, GR23,
1548                          * - GR24, GR25, GR26, GR27, GR28, GR29, GR30, GR31,
1549                          * - GR32, GR33, GR34, GR35, GR36, GR37, GR38, GR39,
1550                          * - GR40, GR41, GR42, GR43, GR44, GR45, GR46, GR47,
1551                          * - GR48, GR49, GR50, GR51, GR52, GR53, GR54, GR55,
1552                          * - GR56, GR57, GR58, GR59, GR60, GR61, GR62, GR63,
1553                          * - FP0,  FP1,  FP2,  FP3,  FP4,  FP5,  FP6,  FP7,
1554                          * - FP8,  FP9,  FP10, FP11, FP12, FP13, FP14, FP15,
1555                          * - FP16, FP17, FP18, FP19, FP20, FP21, FP22, FP23,
1556                          * - FP24, FP25, FP26, FP27, FP28, FP29, FP30, FP31,
1557                          * - FP32, FP33, FP34, FP35, FP36, FP37, FP38, FP39,
1558                          * - FP40, FP41, FP42, FP43, FP44, FP45, FP46, FP47,
1559                          * - FP48, FP49, FP50, FP51, FP52, FP53, FP54, FP55,
1560                          * - FP56, FP57, FP58, FP59, FP60, FP61, FP62, FP63,
1561                          * - PC, PSR, CCR, CCCR,
1562                          * - _X132, _X133, _X134
1563                          * - TBR, BRR, DBAR0, DBAR1, DBAR2, DBAR3,
1564                          * - _X141, _X142, _X143, _X144,
1565                          * - LR, LCR
1566                          */
1567                 case 'g':
1568                         zero = 0;
1569                         ptr = output_buffer;
1570
1571                         /* deal with GR0, GR1-GR27, GR28-GR31, GR32-GR63 */
1572                         ptr = mem2hex(&zero, ptr, 4, 0);
1573
1574                         for (loop = 1; loop <= 27; loop++)
1575                                 ptr = mem2hex(&__debug_user_context->i.gr[loop], ptr, 4, 0);
1576                         temp = (unsigned long) __frame;
1577                         ptr = mem2hex(&temp, ptr, 4, 0);
1578                         ptr = mem2hex(&__debug_user_context->i.gr[29], ptr, 4, 0);
1579                         ptr = mem2hex(&__debug_user_context->i.gr[30], ptr, 4, 0);
1580 #ifdef CONFIG_MMU
1581                         ptr = mem2hex(&__debug_user_context->i.gr[31], ptr, 4, 0);
1582 #else
1583                         temp = (unsigned long) __debug_frame;
1584                         ptr = mem2hex(&temp, ptr, 4, 0);
1585 #endif
1586
1587                         for (loop = 32; loop <= 63; loop++)
1588                                 ptr = mem2hex(&__debug_user_context->i.gr[loop], ptr, 4, 0);
1589
1590                         /* deal with FR0-FR63 */
1591                         for (loop = 0; loop <= 63; loop++)
1592                                 ptr = mem2hex(&__debug_user_context->f.fr[loop], ptr, 4, 0);
1593
1594                         /* deal with special registers */
1595                         ptr = mem2hex(&__debug_frame->pc,    ptr, 4, 0);
1596                         ptr = mem2hex(&__debug_frame->psr,   ptr, 4, 0);
1597                         ptr = mem2hex(&__debug_frame->ccr,   ptr, 4, 0);
1598                         ptr = mem2hex(&__debug_frame->cccr,  ptr, 4, 0);
1599                         ptr = mem2hex(&zero, ptr, 4, 0);
1600                         ptr = mem2hex(&zero, ptr, 4, 0);
1601                         ptr = mem2hex(&zero, ptr, 4, 0);
1602                         ptr = mem2hex(&__debug_frame->tbr,   ptr, 4, 0);
1603                         ptr = mem2hex(&__debug_status.brr ,   ptr, 4, 0);
1604
1605                         asm volatile("movsg dbar0,%0" : "=r"(dbar));
1606                         ptr = mem2hex(&dbar, ptr, 4, 0);
1607                         asm volatile("movsg dbar1,%0" : "=r"(dbar));
1608                         ptr = mem2hex(&dbar, ptr, 4, 0);
1609                         asm volatile("movsg dbar2,%0" : "=r"(dbar));
1610                         ptr = mem2hex(&dbar, ptr, 4, 0);
1611                         asm volatile("movsg dbar3,%0" : "=r"(dbar));
1612                         ptr = mem2hex(&dbar, ptr, 4, 0);
1613
1614                         asm volatile("movsg scr0,%0" : "=r"(dbar));
1615                         ptr = mem2hex(&dbar, ptr, 4, 0);
1616                         asm volatile("movsg scr1,%0" : "=r"(dbar));
1617                         ptr = mem2hex(&dbar, ptr, 4, 0);
1618                         asm volatile("movsg scr2,%0" : "=r"(dbar));
1619                         ptr = mem2hex(&dbar, ptr, 4, 0);
1620                         asm volatile("movsg scr3,%0" : "=r"(dbar));
1621                         ptr = mem2hex(&dbar, ptr, 4, 0);
1622
1623                         ptr = mem2hex(&__debug_frame->lr, ptr, 4, 0);
1624                         ptr = mem2hex(&__debug_frame->lcr, ptr, 4, 0);
1625
1626                         ptr = mem2hex(&__debug_frame->iacc0, ptr, 8, 0);
1627
1628                         ptr = mem2hex(&__debug_user_context->f.fsr[0], ptr, 4, 0);
1629
1630                         for (loop = 0; loop <= 7; loop++)
1631                                 ptr = mem2hex(&__debug_user_context->f.acc[loop], ptr, 4, 0);
1632
1633                         ptr = mem2hex(&__debug_user_context->f.accg, ptr, 8, 0);
1634
1635                         for (loop = 0; loop <= 1; loop++)
1636                                 ptr = mem2hex(&__debug_user_context->f.msr[loop], ptr, 4, 0);
1637
1638                         ptr = mem2hex(&__debug_frame->gner0, ptr, 4, 0);
1639                         ptr = mem2hex(&__debug_frame->gner1, ptr, 4, 0);
1640
1641                         ptr = mem2hex(&__debug_user_context->f.fner[0], ptr, 4, 0);
1642                         ptr = mem2hex(&__debug_user_context->f.fner[1], ptr, 4, 0);
1643
1644                         break;
1645
1646                         /* set the values of the CPU registers */
1647                 case 'G':
1648                         ptr = &input_buffer[1];
1649
1650                         /* deal with GR0, GR1-GR27, GR28-GR31, GR32-GR63 */
1651                         ptr = hex2mem(ptr, &temp, 4);
1652
1653                         for (loop = 1; loop <= 27; loop++)
1654                                 ptr = hex2mem(ptr, &__debug_user_context->i.gr[loop], 4);
1655
1656                         ptr = hex2mem(ptr, &temp, 4);
1657                         __frame = (struct pt_regs *) temp;
1658                         ptr = hex2mem(ptr, &__debug_frame->gr29, 4);
1659                         ptr = hex2mem(ptr, &__debug_frame->gr30, 4);
1660 #ifdef CONFIG_MMU
1661                         ptr = hex2mem(ptr, &__debug_frame->gr31, 4);
1662 #else
1663                         ptr = hex2mem(ptr, &temp, 4);
1664 #endif
1665
1666                         for (loop = 32; loop <= 63; loop++)
1667                                 ptr = hex2mem(ptr, &__debug_user_context->i.gr[loop], 4);
1668
1669                         /* deal with FR0-FR63 */
1670                         for (loop = 0; loop <= 63; loop++)
1671                                 ptr = mem2hex(&__debug_user_context->f.fr[loop], ptr, 4, 0);
1672
1673                         /* deal with special registers */
1674                         ptr = hex2mem(ptr, &__debug_frame->pc,  4);
1675                         ptr = hex2mem(ptr, &__debug_frame->psr, 4);
1676                         ptr = hex2mem(ptr, &__debug_frame->ccr, 4);
1677                         ptr = hex2mem(ptr, &__debug_frame->cccr,4);
1678
1679                         for (loop = 132; loop <= 140; loop++)
1680                                 ptr = hex2mem(ptr, &temp, 4);
1681
1682                         ptr = hex2mem(ptr, &temp, 4);
1683                         asm volatile("movgs %0,scr0" :: "r"(temp));
1684                         ptr = hex2mem(ptr, &temp, 4);
1685                         asm volatile("movgs %0,scr1" :: "r"(temp));
1686                         ptr = hex2mem(ptr, &temp, 4);
1687                         asm volatile("movgs %0,scr2" :: "r"(temp));
1688                         ptr = hex2mem(ptr, &temp, 4);
1689                         asm volatile("movgs %0,scr3" :: "r"(temp));
1690
1691                         ptr = hex2mem(ptr, &__debug_frame->lr,  4);
1692                         ptr = hex2mem(ptr, &__debug_frame->lcr, 4);
1693
1694                         ptr = hex2mem(ptr, &__debug_frame->iacc0, 8);
1695
1696                         ptr = hex2mem(ptr, &__debug_user_context->f.fsr[0], 4);
1697
1698                         for (loop = 0; loop <= 7; loop++)
1699                                 ptr = hex2mem(ptr, &__debug_user_context->f.acc[loop], 4);
1700
1701                         ptr = hex2mem(ptr, &__debug_user_context->f.accg, 8);
1702
1703                         for (loop = 0; loop <= 1; loop++)
1704                                 ptr = hex2mem(ptr, &__debug_user_context->f.msr[loop], 4);
1705
1706                         ptr = hex2mem(ptr, &__debug_frame->gner0, 4);
1707                         ptr = hex2mem(ptr, &__debug_frame->gner1, 4);
1708
1709                         ptr = hex2mem(ptr, &__debug_user_context->f.fner[0], 4);
1710                         ptr = hex2mem(ptr, &__debug_user_context->f.fner[1], 4);
1711
1712                         gdbstub_strcpy(output_buffer,"OK");
1713                         break;
1714
1715                         /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
1716                 case 'm':
1717                         ptr = &input_buffer[1];
1718
1719                         if (hexToInt(&ptr, &addr) &&
1720                             *ptr++ == ',' &&
1721                             hexToInt(&ptr, &length)
1722                             ) {
1723                                 if (mem2hex((char *)addr, output_buffer, length, 1))
1724                                         break;
1725                                 gdbstub_strcpy (output_buffer, "E03");
1726                         }
1727                         else {
1728                                 gdbstub_strcpy(output_buffer,"E01");
1729                         }
1730                         break;
1731
1732                         /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
1733                 case 'M':
1734                         ptr = &input_buffer[1];
1735
1736                         if (hexToInt(&ptr, &addr) &&
1737                             *ptr++ == ',' &&
1738                             hexToInt(&ptr, &length) &&
1739                             *ptr++ == ':'
1740                             ) {
1741                                 if (hex2mem(ptr, (char *)addr, length)) {
1742                                         gdbstub_strcpy(output_buffer, "OK");
1743                                 }
1744                                 else {
1745                                         gdbstub_strcpy(output_buffer, "E03");
1746                                 }
1747                         }
1748                         else
1749                                 gdbstub_strcpy(output_buffer, "E02");
1750
1751                         flush_cache = 1;
1752                         break;
1753
1754                         /* PNN,=RRRRRRRR: Write value R to reg N return OK */
1755                 case 'P':
1756                         ptr = &input_buffer[1];
1757
1758                         if (!hexToInt(&ptr, &addr) ||
1759                             *ptr++ != '=' ||
1760                             !hexToInt(&ptr, &temp)
1761                             ) {
1762                                 gdbstub_strcpy(output_buffer, "E01");
1763                                 break;
1764                         }
1765
1766                         temp2 = 1;
1767                         switch (addr) {
1768                         case GDB_REG_GR(0):
1769                                 break;
1770                         case GDB_REG_GR(1) ... GDB_REG_GR(63):
1771                                 __debug_user_context->i.gr[addr - GDB_REG_GR(0)] = temp;
1772                                 break;
1773                         case GDB_REG_FR(0) ... GDB_REG_FR(63):
1774                                 __debug_user_context->f.fr[addr - GDB_REG_FR(0)] = temp;
1775                                 break;
1776                         case GDB_REG_PC:
1777                                 __debug_user_context->i.pc = temp;
1778                                 break;
1779                         case GDB_REG_PSR:
1780                                 __debug_user_context->i.psr = temp;
1781                                 break;
1782                         case GDB_REG_CCR:
1783                                 __debug_user_context->i.ccr = temp;
1784                                 break;
1785                         case GDB_REG_CCCR:
1786                                 __debug_user_context->i.cccr = temp;
1787                                 break;
1788                         case GDB_REG_BRR:
1789                                 __debug_status.brr = temp;
1790                                 break;
1791                         case GDB_REG_LR:
1792                                 __debug_user_context->i.lr = temp;
1793                                 break;
1794                         case GDB_REG_LCR:
1795                                 __debug_user_context->i.lcr = temp;
1796                                 break;
1797                         case GDB_REG_FSR0:
1798                                 __debug_user_context->f.fsr[0] = temp;
1799                                 break;
1800                         case GDB_REG_ACC(0) ... GDB_REG_ACC(7):
1801                                 __debug_user_context->f.acc[addr - GDB_REG_ACC(0)] = temp;
1802                                 break;
1803                         case GDB_REG_ACCG(0):
1804                                 *(uint32_t *) &__debug_user_context->f.accg[0] = temp;
1805                                 break;
1806                         case GDB_REG_ACCG(4):
1807                                 *(uint32_t *) &__debug_user_context->f.accg[4] = temp;
1808                                 break;
1809                         case GDB_REG_MSR(0) ... GDB_REG_MSR(1):
1810                                 __debug_user_context->f.msr[addr - GDB_REG_MSR(0)] = temp;
1811                                 break;
1812                         case GDB_REG_GNER(0) ... GDB_REG_GNER(1):
1813                                 __debug_user_context->i.gner[addr - GDB_REG_GNER(0)] = temp;
1814                                 break;
1815                         case GDB_REG_FNER(0) ... GDB_REG_FNER(1):
1816                                 __debug_user_context->f.fner[addr - GDB_REG_FNER(0)] = temp;
1817                                 break;
1818                         default:
1819                                 temp2 = 0;
1820                                 break;
1821                         }
1822
1823                         if (temp2) {
1824                                 gdbstub_strcpy(output_buffer, "OK");
1825                         }
1826                         else {
1827                                 gdbstub_strcpy(output_buffer, "E02");
1828                         }
1829                         break;
1830
1831                         /* cAA..AA    Continue at address AA..AA(optional) */
1832                 case 'c':
1833                         /* try to read optional parameter, pc unchanged if no parm */
1834                         ptr = &input_buffer[1];
1835                         if (hexToInt(&ptr, &addr))
1836                                 __debug_frame->pc = addr;
1837                         goto done;
1838
1839                         /* kill the program */
1840                 case 'k' :
1841                         goto done;      /* just continue */
1842
1843
1844                         /* reset the whole machine (FIXME: system dependent) */
1845                 case 'r':
1846                         break;
1847
1848
1849                         /* step to next instruction */
1850                 case 's':
1851                         __debug_regs->dcr |= DCR_SE;
1852                         __debug_status.dcr |= DCR_SE;
1853                         goto done;
1854
1855                         /* set baud rate (bBB) */
1856                 case 'b':
1857                         ptr = &input_buffer[1];
1858                         if (!hexToInt(&ptr, &temp)) {
1859                                 gdbstub_strcpy(output_buffer,"B01");
1860                                 break;
1861                         }
1862
1863                         if (temp) {
1864                                 /* ack before changing speed */
1865                                 gdbstub_send_packet("OK");
1866                                 gdbstub_set_baud(temp);
1867                         }
1868                         break;
1869
1870                         /* set breakpoint */
1871                 case 'Z':
1872                         ptr = &input_buffer[1];
1873
1874                         if (!hexToInt(&ptr,&temp) || *ptr++ != ',' ||
1875                             !hexToInt(&ptr,&addr) || *ptr++ != ',' ||
1876                             !hexToInt(&ptr,&length)
1877                             ) {
1878                                 gdbstub_strcpy(output_buffer,"E01");
1879                                 break;
1880                         }
1881
1882                         if (temp >= 5) {
1883                                 gdbstub_strcpy(output_buffer,"E03");
1884                                 break;
1885                         }
1886
1887                         if (gdbstub_set_breakpoint(temp, addr, length) < 0) {
1888                                 gdbstub_strcpy(output_buffer,"E03");
1889                                 break;
1890                         }
1891
1892                         if (temp == 0)
1893                                 flush_cache = 1; /* soft bkpt by modified memory */
1894
1895                         gdbstub_strcpy(output_buffer,"OK");
1896                         break;
1897
1898                         /* clear breakpoint */
1899                 case 'z':
1900                         ptr = &input_buffer[1];
1901
1902                         if (!hexToInt(&ptr,&temp) || *ptr++ != ',' ||
1903                             !hexToInt(&ptr,&addr) || *ptr++ != ',' ||
1904                             !hexToInt(&ptr,&length)
1905                             ) {
1906                                 gdbstub_strcpy(output_buffer,"E01");
1907                                 break;
1908                         }
1909
1910                         if (temp >= 5) {
1911                                 gdbstub_strcpy(output_buffer,"E03");
1912                                 break;
1913                         }
1914
1915                         if (gdbstub_clear_breakpoint(temp, addr, length) < 0) {
1916                                 gdbstub_strcpy(output_buffer,"E03");
1917                                 break;
1918                         }
1919
1920                         if (temp == 0)
1921                                 flush_cache = 1; /* soft bkpt by modified memory */
1922
1923                         gdbstub_strcpy(output_buffer,"OK");
1924                         break;
1925
1926                 default:
1927                         gdbstub_proto("### GDB Unsupported Cmd '%s'\n",input_buffer);
1928                         break;
1929                 }
1930
1931                 /* reply to the request */
1932                 LEDS(0x5009);
1933                 gdbstub_send_packet(output_buffer);
1934         }
1935
1936  done:
1937         restore_user_regs(&__debug_frame0->uc);
1938
1939         //gdbstub_dump_debugregs();
1940         //gdbstub_printk("<-- gdbstub() %08x\n", __debug_frame->pc);
1941
1942         /* need to flush the instruction cache before resuming, as we may have
1943          * deposited a breakpoint, and the icache probably has no way of
1944          * knowing that a data ref to some location may have changed something
1945          * that is in the instruction cache.  NB: We flush both caches, just to
1946          * be sure...
1947          */
1948
1949         /* note: flushing the icache will clobber EAR0 on the FR451 */
1950         if (flush_cache)
1951                 gdbstub_purge_cache();
1952
1953         LEDS(0x5666);
1954
1955 } /* end gdbstub() */
1956
1957 /*****************************************************************************/
1958 /*
1959  * initialise the GDB stub
1960  */
1961 void __init gdbstub_init(void)
1962 {
1963 #ifdef CONFIG_GDBSTUB_IMMEDIATE
1964         unsigned char ch;
1965         int ret;
1966 #endif
1967
1968         gdbstub_printk("%s", gdbstub_banner);
1969
1970         gdbstub_io_init();
1971
1972         /* try to talk to GDB (or anyone insane enough to want to type GDB protocol by hand) */
1973         gdbstub_proto("### GDB Tx ACK\n");
1974         gdbstub_tx_char('+'); /* 'hello world' */
1975
1976 #ifdef CONFIG_GDBSTUB_IMMEDIATE
1977         gdbstub_printk("GDB Stub waiting for packet\n");
1978
1979         /*
1980          * In case GDB is started before us, ack any packets
1981          * (presumably "$?#xx") sitting there.
1982          */
1983         do { gdbstub_rx_char(&ch, 0); } while (ch != '$');
1984         do { gdbstub_rx_char(&ch, 0); } while (ch != '#');
1985         do { ret = gdbstub_rx_char(&ch, 0); } while (ret != 0); /* eat first csum byte */
1986         do { ret = gdbstub_rx_char(&ch, 0); } while (ret != 0); /* eat second csum byte */
1987
1988         gdbstub_proto("### GDB Tx NAK\n");
1989         gdbstub_tx_char('-'); /* nak it */
1990
1991 #else
1992         gdbstub_printk("GDB Stub set\n");
1993 #endif
1994
1995 #if 0
1996         /* send banner */
1997         ptr = output_buffer;
1998         *ptr++ = 'O';
1999         ptr = mem2hex(gdbstub_banner, ptr, sizeof(gdbstub_banner) - 1, 0);
2000         gdbstub_send_packet(output_buffer);
2001 #endif
2002 #if defined(CONFIG_GDB_CONSOLE) && defined(CONFIG_GDBSTUB_IMMEDIATE)
2003         register_console(&gdbstub_console);
2004 #endif
2005
2006 } /* end gdbstub_init() */
2007
2008 /*****************************************************************************/
2009 /*
2010  * register the console at a more appropriate time
2011  */
2012 #if defined (CONFIG_GDB_CONSOLE) && !defined(CONFIG_GDBSTUB_IMMEDIATE)
2013 static int __init gdbstub_postinit(void)
2014 {
2015         printk("registering console\n");
2016         register_console(&gdbstub_console);
2017         return 0;
2018 } /* end gdbstub_postinit() */
2019
2020 __initcall(gdbstub_postinit);
2021 #endif
2022
2023 /*****************************************************************************/
2024 /*
2025  * send an exit message to GDB
2026  */
2027 void gdbstub_exit(int status)
2028 {
2029         unsigned char checksum;
2030         int count;
2031         unsigned char ch;
2032
2033         sprintf(output_buffer,"W%02x",status&0xff);
2034
2035         gdbstub_tx_char('$');
2036         checksum = 0;
2037         count = 0;
2038
2039         while ((ch = output_buffer[count]) != 0) {
2040                 gdbstub_tx_char(ch);
2041                 checksum += ch;
2042                 count += 1;
2043         }
2044
2045         gdbstub_tx_char('#');
2046         gdbstub_tx_char(hex_asc_hi(checksum));
2047         gdbstub_tx_char(hex_asc_lo(checksum));
2048
2049         /* make sure the output is flushed, or else RedBoot might clobber it */
2050         gdbstub_tx_char('-');
2051         gdbstub_tx_flush();
2052
2053 } /* end gdbstub_exit() */
2054
2055 /*****************************************************************************/
2056 /*
2057  * GDB wants to call malloc() and free() to allocate memory for calling kernel
2058  * functions directly from its command line
2059  */
2060 static void *malloc(size_t size) __maybe_unused;
2061 static void *malloc(size_t size)
2062 {
2063         return kmalloc(size, GFP_ATOMIC);
2064 }
2065
2066 static void free(void *p) __maybe_unused;
2067 static void free(void *p)
2068 {
2069         kfree(p);
2070 }
2071
2072 static uint32_t ___get_HSR0(void) __maybe_unused;
2073 static uint32_t ___get_HSR0(void)
2074 {
2075         return __get_HSR(0);
2076 }
2077
2078 static uint32_t ___set_HSR0(uint32_t x) __maybe_unused;
2079 static uint32_t ___set_HSR0(uint32_t x)
2080 {
2081         __set_HSR(0, x);
2082         return __get_HSR(0);
2083 }