SAFE public projects git trees. - safe/jmp/linux-2.6/blob - arch/frv/kernel/gdb-stub.c

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