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 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_GDB_CONSOLE
 651
 652 static struct console gdbstub_console = {
 653         .name   = "gdb",
 654         .write  = gdbstub_console_write,        /* in break.S */
 655         .flags  = CON_PRINTBUFFER,
 656         .index  = -1,
 657 };
 658
 659 #endif
 660
 661 /*****************************************************************************/
 662 /*
 663  * Convert the memory pointed to by mem into hex, placing result in buf.
 664  * - if successful, return a pointer to the last char put in buf (NUL)
 665  * - in case of mem fault, return NULL
 666  * may_fault is non-zero if we are reading from arbitrary memory, but is currently
 667  * not used.
 668  */
 669 static unsigned char *mem2hex(const void *_mem, char *buf, int count, int may_fault)
 670 {
 671         const uint8_t *mem = _mem;
 672         uint8_t ch[4] __attribute__((aligned(4)));
 673
 674         if ((uint32_t)mem&1 && count>=1) {
 675                 if (!gdbstub_read_byte(mem,ch))
 676                         return NULL;
 677                 *buf++ = hexchars[ch[0] >> 4];
 678                 *buf++ = hexchars[ch[0] & 0xf];
 679                 mem++;
 680                 count--;
 681         }
 682
 683         if ((uint32_t)mem&3 && count>=2) {
 684                 if (!gdbstub_read_word(mem,(uint16_t *)ch))
 685                         return NULL;
 686                 *buf++ = hexchars[ch[0] >> 4];
 687                 *buf++ = hexchars[ch[0] & 0xf];
 688                 *buf++ = hexchars[ch[1] >> 4];
 689                 *buf++ = hexchars[ch[1] & 0xf];
 690                 mem += 2;
 691                 count -= 2;
 692         }
 693
 694         while (count>=4) {
 695                 if (!gdbstub_read_dword(mem,(uint32_t *)ch))
 696                         return NULL;
 697                 *buf++ = hexchars[ch[0] >> 4];
 698                 *buf++ = hexchars[ch[0] & 0xf];
 699                 *buf++ = hexchars[ch[1] >> 4];
 700                 *buf++ = hexchars[ch[1] & 0xf];
 701                 *buf++ = hexchars[ch[2] >> 4];
 702                 *buf++ = hexchars[ch[2] & 0xf];
 703                 *buf++ = hexchars[ch[3] >> 4];
 704                 *buf++ = hexchars[ch[3] & 0xf];
 705                 mem += 4;
 706                 count -= 4;
 707         }
 708
 709         if (count>=2) {
 710                 if (!gdbstub_read_word(mem,(uint16_t *)ch))
 711                         return NULL;
 712                 *buf++ = hexchars[ch[0] >> 4];
 713                 *buf++ = hexchars[ch[0] & 0xf];
 714                 *buf++ = hexchars[ch[1] >> 4];
 715                 *buf++ = hexchars[ch[1] & 0xf];
 716                 mem += 2;
 717                 count -= 2;
 718         }
 719
 720         if (count>=1) {
 721                 if (!gdbstub_read_byte(mem,ch))
 722                         return NULL;
 723                 *buf++ = hexchars[ch[0] >> 4];
 724                 *buf++ = hexchars[ch[0] & 0xf];
 725         }
 726
 727         *buf = 0;
 728
 729         return buf;
 730 } /* end mem2hex() */
 731
 732 /*****************************************************************************/
 733 /*
 734  * convert the hex array pointed to by buf into binary to be placed in mem
 735  * return a pointer to the character AFTER the last byte of buffer consumed
 736  */
 737 static char *hex2mem(const char *buf, void *_mem, int count)
 738 {
 739         uint8_t *mem = _mem;
 740         union {
 741                 uint32_t l;
 742                 uint16_t w;
 743                 uint8_t  b[4];
 744         } ch;
 745
 746         if ((u32)mem&1 && count>=1) {
 747                 ch.b[0]  = hex(*buf++) << 4;
 748                 ch.b[0] |= hex(*buf++);
 749                 if (!gdbstub_write_byte(mem,ch.b[0]))
 750                         return NULL;
 751                 mem++;
 752                 count--;
 753         }
 754
 755         if ((u32)mem&3 && count>=2) {
 756                 ch.b[0]  = hex(*buf++) << 4;
 757                 ch.b[0] |= hex(*buf++);
 758                 ch.b[1]  = hex(*buf++) << 4;
 759                 ch.b[1] |= hex(*buf++);
 760                 if (!gdbstub_write_word(mem,ch.w))
 761                         return NULL;
 762                 mem += 2;
 763                 count -= 2;
 764         }
 765
 766         while (count>=4) {
 767                 ch.b[0]  = hex(*buf++) << 4;
 768                 ch.b[0] |= hex(*buf++);
 769                 ch.b[1]  = hex(*buf++) << 4;
 770                 ch.b[1] |= hex(*buf++);
 771                 ch.b[2]  = hex(*buf++) << 4;
 772                 ch.b[2] |= hex(*buf++);
 773                 ch.b[3]  = hex(*buf++) << 4;
 774                 ch.b[3] |= hex(*buf++);
 775                 if (!gdbstub_write_dword(mem,ch.l))
 776                         return NULL;
 777                 mem += 4;
 778                 count -= 4;
 779         }
 780
 781         if (count>=2) {
 782                 ch.b[0]  = hex(*buf++) << 4;
 783                 ch.b[0] |= hex(*buf++);
 784                 ch.b[1]  = hex(*buf++) << 4;
 785                 ch.b[1] |= hex(*buf++);
 786                 if (!gdbstub_write_word(mem,ch.w))
 787                         return NULL;
 788                 mem += 2;
 789                 count -= 2;
 790         }
 791
 792         if (count>=1) {
 793                 ch.b[0]  = hex(*buf++) << 4;
 794                 ch.b[0] |= hex(*buf++);
 795                 if (!gdbstub_write_byte(mem,ch.b[0]))
 796                         return NULL;
 797         }
 798
 799         return (char *) buf;
 800 } /* end hex2mem() */
 801
 802 /*****************************************************************************/
 803 /*
 804  * This table contains the mapping between FRV TBR.TT exception codes,
 805  * and signals, which are primarily what GDB understands.  It also
 806  * indicates which hardware traps we need to commandeer when
 807  * initializing the stub.
 808  */
 809 static const struct brr_to_sig_map {
 810         unsigned long   brr_mask;       /* BRR bitmask */
 811         unsigned long   tbr_tt;         /* TBR.TT code (in BRR.EBTT) */
 812         unsigned int    signo;          /* Signal that we map this into */
 813 } brr_to_sig_map[] = {
 814         { BRR_EB,       TBR_TT_INSTR_ACC_ERROR, SIGSEGV         },
 815         { BRR_EB,       TBR_TT_ILLEGAL_INSTR,   SIGILL          },
 816         { BRR_EB,       TBR_TT_PRIV_INSTR,      SIGILL          },
 817         { BRR_EB,       TBR_TT_MP_EXCEPTION,    SIGFPE          },
 818         { BRR_EB,       TBR_TT_DATA_ACC_ERROR,  SIGSEGV         },
 819         { BRR_EB,       TBR_TT_DATA_STR_ERROR,  SIGSEGV         },
 820         { BRR_EB,       TBR_TT_DIVISION_EXCEP,  SIGFPE          },
 821         { BRR_EB,       TBR_TT_COMPOUND_EXCEP,  SIGSEGV         },
 822         { BRR_EB,       TBR_TT_INTERRUPT_13,    SIGALRM         },      /* watchdog */
 823         { BRR_EB,       TBR_TT_INTERRUPT_14,    SIGINT          },      /* GDB serial */
 824         { BRR_EB,       TBR_TT_INTERRUPT_15,    SIGQUIT         },      /* NMI */
 825         { BRR_CB,       0,                      SIGUSR1         },
 826         { BRR_TB,       0,                      SIGUSR2         },
 827         { BRR_DBNEx,    0,                      SIGTRAP         },
 828         { BRR_DBx,      0,                      SIGTRAP         },      /* h/w watchpoint */
 829         { BRR_IBx,      0,                      SIGTRAP         },      /* h/w breakpoint */
 830         { BRR_CBB,      0,                      SIGTRAP         },
 831         { BRR_SB,       0,                      SIGTRAP         },
 832         { BRR_ST,       0,                      SIGTRAP         },      /* single step */
 833         { 0,            0,                      SIGHUP          }       /* default */
 834 };
 835
 836 /*****************************************************************************/
 837 /*
 838  * convert the FRV BRR register contents into a UNIX signal number
 839  */
 840 static inline int gdbstub_compute_signal(unsigned long brr)
 841 {
 842         const struct brr_to_sig_map *map;
 843         unsigned long tbr = (brr & BRR_EBTT) >> 12;
 844
 845         for (map = brr_to_sig_map; map->brr_mask; map++)
 846                 if (map->brr_mask & brr)
 847                         if (!map->tbr_tt || map->tbr_tt == tbr)
 848                                 break;
 849
 850         return map->signo;
 851 } /* end gdbstub_compute_signal() */
 852
 853 /*****************************************************************************/
 854 /*
 855  * set a software breakpoint or a hardware breakpoint or watchpoint
 856  */
 857 static int gdbstub_set_breakpoint(unsigned long type, unsigned long addr, unsigned long len)
 858 {
 859         unsigned long tmp;
 860         int bkpt, loop, xloop;
 861
 862         union {
 863                 struct {
 864                         unsigned long mask0, mask1;
 865                 };
 866                 uint8_t bytes[8];
 867         } dbmr;
 868
 869         //gdbstub_printk("setbkpt(%ld,%08lx,%ld)\n", type, addr, len);
 870
 871         switch (type) {
 872                 /* set software breakpoint */
 873         case 0:
 874                 if (addr & 3 || len > 7*4)
 875                         return -EINVAL;
 876
 877                 for (bkpt = 255; bkpt >= 0; bkpt--)
 878                         if (!gdbstub_bkpts[bkpt].addr)
 879                                 break;
 880                 if (bkpt < 0)
 881                         return -ENOSPC;
 882
 883                 for (loop = 0; loop < len/4; loop++)
 884                         if (!gdbstub_read_dword(&((uint32_t *) addr)[loop],
 885                                                 &gdbstub_bkpts[bkpt].originsns[loop]))
 886                                 return -EFAULT;
 887
 888                 for (loop = 0; loop < len/4; loop++)
 889                         if (!gdbstub_write_dword(&((uint32_t *) addr)[loop],
 890                                                  BREAK_INSN)
 891                             ) {
 892                                 /* need to undo the changes if possible */
 893                                 for (xloop = 0; xloop < loop; xloop++)
 894                                         gdbstub_write_dword(&((uint32_t *) addr)[xloop],
 895                                                             gdbstub_bkpts[bkpt].originsns[xloop]);
 896                                 return -EFAULT;
 897                         }
 898
 899                 gdbstub_bkpts[bkpt].addr = addr;
 900                 gdbstub_bkpts[bkpt].len = len;
 901
 902 #if 0
 903                 gdbstub_printk("Set BKPT[%02x]: %08lx #%d {%04x, %04x} -> { %04x, %04x }\n",
 904                                bkpt,
 905                                gdbstub_bkpts[bkpt].addr,
 906                                gdbstub_bkpts[bkpt].len,
 907                                gdbstub_bkpts[bkpt].originsns[0],
 908                                gdbstub_bkpts[bkpt].originsns[1],
 909                                ((uint32_t *) addr)[0],
 910                                ((uint32_t *) addr)[1]
 911                                );
 912 #endif
 913                 return 0;
 914
 915                 /* set hardware breakpoint */
 916         case 1:
 917                 if (addr & 3 || len != 4)
 918                         return -EINVAL;
 919
 920                 if (!(__debug_regs->dcr & DCR_IBE0)) {
 921                         //gdbstub_printk("set h/w break 0: %08lx\n", addr);
 922                         __debug_regs->dcr |= DCR_IBE0;
 923                         __debug_regs->ibar[0] = addr;
 924                         asm volatile("movgs %0,ibar0" : : "r"(addr));
 925                         return 0;
 926                 }
 927
 928                 if (!(__debug_regs->dcr & DCR_IBE1)) {
 929                         //gdbstub_printk("set h/w break 1: %08lx\n", addr);
 930                         __debug_regs->dcr |= DCR_IBE1;
 931                         __debug_regs->ibar[1] = addr;
 932                         asm volatile("movgs %0,ibar1" : : "r"(addr));
 933                         return 0;
 934                 }
 935
 936                 if (!(__debug_regs->dcr & DCR_IBE2)) {
 937                         //gdbstub_printk("set h/w break 2: %08lx\n", addr);
 938                         __debug_regs->dcr |= DCR_IBE2;
 939                         __debug_regs->ibar[2] = addr;
 940                         asm volatile("movgs %0,ibar2" : : "r"(addr));
 941                         return 0;
 942                 }
 943
 944                 if (!(__debug_regs->dcr & DCR_IBE3)) {
 945                         //gdbstub_printk("set h/w break 3: %08lx\n", addr);
 946                         __debug_regs->dcr |= DCR_IBE3;
 947                         __debug_regs->ibar[3] = addr;
 948                         asm volatile("movgs %0,ibar3" : : "r"(addr));
 949                         return 0;
 950                 }
 951
 952                 return -ENOSPC;
 953
 954                 /* set data read/write/access watchpoint */
 955         case 2:
 956         case 3:
 957         case 4:
 958                 if ((addr & ~7) != ((addr + len - 1) & ~7))
 959                         return -EINVAL;
 960
 961                 tmp = addr & 7;
 962
 963                 memset(dbmr.bytes, 0xff, sizeof(dbmr.bytes));
 964                 for (loop = 0; loop < len; loop++)
 965                         dbmr.bytes[tmp + loop] = 0;
 966
 967                 addr &= ~7;
 968
 969                 if (!(__debug_regs->dcr & (DCR_DRBE0|DCR_DWBE0))) {
 970                         //gdbstub_printk("set h/w watchpoint 0 type %ld: %08lx\n", type, addr);
 971                         tmp = type==2 ? DCR_DWBE0 : type==3 ? DCR_DRBE0 : DCR_DRBE0|DCR_DWBE0;
 972
 973                         __debug_regs->dcr |= tmp;
 974                         __debug_regs->dbar[0] = addr;
 975                         __debug_regs->dbmr[0][0] = dbmr.mask0;
 976                         __debug_regs->dbmr[0][1] = dbmr.mask1;
 977                         __debug_regs->dbdr[0][0] = 0;
 978                         __debug_regs->dbdr[0][1] = 0;
 979
 980                         asm volatile("  movgs   %0,dbar0        \n"
 981                                      "  movgs   %1,dbmr00       \n"
 982                                      "  movgs   %2,dbmr01       \n"
 983                                      "  movgs   gr0,dbdr00      \n"
 984                                      "  movgs   gr0,dbdr01      \n"
 985                                      : : "r"(addr), "r"(dbmr.mask0), "r"(dbmr.mask1));
 986                         return 0;
 987                 }
 988
 989                 if (!(__debug_regs->dcr & (DCR_DRBE1|DCR_DWBE1))) {
 990                         //gdbstub_printk("set h/w watchpoint 1 type %ld: %08lx\n", type, addr);
 991                         tmp = type==2 ? DCR_DWBE1 : type==3 ? DCR_DRBE1 : DCR_DRBE1|DCR_DWBE1;
 992
 993                         __debug_regs->dcr |= tmp;
 994                         __debug_regs->dbar[1] = addr;
 995                         __debug_regs->dbmr[1][0] = dbmr.mask0;
 996                         __debug_regs->dbmr[1][1] = dbmr.mask1;
 997                         __debug_regs->dbdr[1][0] = 0;
 998                         __debug_regs->dbdr[1][1] = 0;
 999
1000                         asm volatile("  movgs   %0,dbar1        \n"
1001                                      "  movgs   %1,dbmr10       \n"
1002                                      "  movgs   %2,dbmr11       \n"
1003                                      "  movgs   gr0,dbdr10      \n"
1004                                      "  movgs   gr0,dbdr11      \n"
1005                                      : : "r"(addr), "r"(dbmr.mask0), "r"(dbmr.mask1));
1006                         return 0;
1007                 }
1008
1009                 return -ENOSPC;
1010
1011         default:
1012                 return -EINVAL;
1013         }
1014
1015 } /* end gdbstub_set_breakpoint() */
1016
1017 /*****************************************************************************/
1018 /*
1019  * clear a breakpoint or watchpoint
1020  */
1021 int gdbstub_clear_breakpoint(unsigned long type, unsigned long addr, unsigned long len)
1022 {
1023         unsigned long tmp;
1024         int bkpt, loop;
1025
1026         union {
1027                 struct {
1028                         unsigned long mask0, mask1;
1029                 };
1030                 uint8_t bytes[8];
1031         } dbmr;
1032
1033         //gdbstub_printk("clearbkpt(%ld,%08lx,%ld)\n", type, addr, len);
1034
1035         switch (type) {
1036                 /* clear software breakpoint */
1037         case 0:
1038                 for (bkpt = 255; bkpt >= 0; bkpt--)
1039                         if (gdbstub_bkpts[bkpt].addr == addr && gdbstub_bkpts[bkpt].len == len)
1040                                 break;
1041                 if (bkpt < 0)
1042                         return -ENOENT;
1043
1044                 gdbstub_bkpts[bkpt].addr = 0;
1045
1046                 for (loop = 0; loop < len/4; loop++)
1047                         if (!gdbstub_write_dword(&((uint32_t *) addr)[loop],
1048                                                  gdbstub_bkpts[bkpt].originsns[loop]))
1049                                 return -EFAULT;
1050                 return 0;
1051
1052                 /* clear hardware breakpoint */
1053         case 1:
1054                 if (addr & 3 || len != 4)
1055                         return -EINVAL;
1056
1057 #define __get_ibar(X) ({ unsigned long x; asm volatile("movsg ibar"#X",%0" : "=r"(x)); x; })
1058
1059                 if (__debug_regs->dcr & DCR_IBE0 && __get_ibar(0) == addr) {
1060                         //gdbstub_printk("clear h/w break 0: %08lx\n", addr);
1061                         __debug_regs->dcr &= ~DCR_IBE0;
1062                         __debug_regs->ibar[0] = 0;
1063                         asm volatile("movgs gr0,ibar0");
1064                         return 0;
1065                 }
1066
1067                 if (__debug_regs->dcr & DCR_IBE1 && __get_ibar(1) == addr) {
1068                         //gdbstub_printk("clear h/w break 1: %08lx\n", addr);
1069                         __debug_regs->dcr &= ~DCR_IBE1;
1070                         __debug_regs->ibar[1] = 0;
1071                         asm volatile("movgs gr0,ibar1");
1072                         return 0;
1073                 }
1074
1075                 if (__debug_regs->dcr & DCR_IBE2 && __get_ibar(2) == addr) {
1076                         //gdbstub_printk("clear h/w break 2: %08lx\n", addr);
1077                         __debug_regs->dcr &= ~DCR_IBE2;
1078                         __debug_regs->ibar[2] = 0;
1079                         asm volatile("movgs gr0,ibar2");
1080                         return 0;
1081                 }
1082
1083                 if (__debug_regs->dcr & DCR_IBE3 && __get_ibar(3) == addr) {
1084                         //gdbstub_printk("clear h/w break 3: %08lx\n", addr);
1085                         __debug_regs->dcr &= ~DCR_IBE3;
1086                         __debug_regs->ibar[3] = 0;
1087                         asm volatile("movgs gr0,ibar3");
1088                         return 0;
1089                 }
1090
1091                 return -EINVAL;
1092
1093                 /* clear data read/write/access watchpoint */
1094         case 2:
1095         case 3:
1096         case 4:
1097                 if ((addr & ~7) != ((addr + len - 1) & ~7))
1098                         return -EINVAL;
1099
1100                 tmp = addr & 7;
1101
1102                 memset(dbmr.bytes, 0xff, sizeof(dbmr.bytes));
1103                 for (loop = 0; loop < len; loop++)
1104                         dbmr.bytes[tmp + loop] = 0;
1105
1106                 addr &= ~7;
1107
1108 #define __get_dbar(X) ({ unsigned long x; asm volatile("movsg dbar"#X",%0" : "=r"(x)); x; })
1109 #define __get_dbmr0(X) ({ unsigned long x; asm volatile("movsg dbmr"#X"0,%0" : "=r"(x)); x; })
1110 #define __get_dbmr1(X) ({ unsigned long x; asm volatile("movsg dbmr"#X"1,%0" : "=r"(x)); x; })
1111
1112                 /* consider DBAR 0 */
1113                 tmp = type==2 ? DCR_DWBE0 : type==3 ? DCR_DRBE0 : DCR_DRBE0|DCR_DWBE0;
1114
1115                 if ((__debug_regs->dcr & (DCR_DRBE0|DCR_DWBE0)) != tmp ||
1116                     __get_dbar(0) != addr ||
1117                     __get_dbmr0(0) != dbmr.mask0 ||
1118                     __get_dbmr1(0) != dbmr.mask1)
1119                         goto skip_dbar0;
1120
1121                 //gdbstub_printk("clear h/w watchpoint 0 type %ld: %08lx\n", type, addr);
1122                 __debug_regs->dcr &= ~(DCR_DRBE0|DCR_DWBE0);
1123                 __debug_regs->dbar[0] = 0;
1124                 __debug_regs->dbmr[0][0] = 0;
1125                 __debug_regs->dbmr[0][1] = 0;
1126                 __debug_regs->dbdr[0][0] = 0;
1127                 __debug_regs->dbdr[0][1] = 0;
1128
1129                 asm volatile("  movgs   gr0,dbar0       \n"
1130                              "  movgs   gr0,dbmr00      \n"
1131                              "  movgs   gr0,dbmr01      \n"
1132                              "  movgs   gr0,dbdr00      \n"
1133                              "  movgs   gr0,dbdr01      \n");
1134                 return 0;
1135
1136         skip_dbar0:
1137                 /* consider DBAR 0 */
1138                 tmp = type==2 ? DCR_DWBE1 : type==3 ? DCR_DRBE1 : DCR_DRBE1|DCR_DWBE1;
1139
1140                 if ((__debug_regs->dcr & (DCR_DRBE1|DCR_DWBE1)) != tmp ||
1141                     __get_dbar(1) != addr ||
1142                     __get_dbmr0(1) != dbmr.mask0 ||
1143                     __get_dbmr1(1) != dbmr.mask1)
1144                         goto skip_dbar1;
1145
1146                 //gdbstub_printk("clear h/w watchpoint 1 type %ld: %08lx\n", type, addr);
1147                 __debug_regs->dcr &= ~(DCR_DRBE1|DCR_DWBE1);
1148                 __debug_regs->dbar[1] = 0;
1149                 __debug_regs->dbmr[1][0] = 0;
1150                 __debug_regs->dbmr[1][1] = 0;
1151                 __debug_regs->dbdr[1][0] = 0;
1152                 __debug_regs->dbdr[1][1] = 0;
1153
1154                 asm volatile("  movgs   gr0,dbar1       \n"
1155                              "  movgs   gr0,dbmr10      \n"
1156                              "  movgs   gr0,dbmr11      \n"
1157                              "  movgs   gr0,dbdr10      \n"
1158                              "  movgs   gr0,dbdr11      \n");
1159                 return 0;
1160
1161         skip_dbar1:
1162                 return -ENOSPC;
1163
1164         default:
1165                 return -EINVAL;
1166         }
1167 } /* end gdbstub_clear_breakpoint() */
1168
1169 /*****************************************************************************/
1170 /*
1171  * check a for an internal software breakpoint, and wind the PC back if necessary
1172  */
1173 static void gdbstub_check_breakpoint(void)
1174 {
1175         unsigned long addr = __debug_frame->pc - 4;
1176         int bkpt;
1177
1178         for (bkpt = 255; bkpt >= 0; bkpt--)
1179                 if (gdbstub_bkpts[bkpt].addr == addr)
1180                         break;
1181         if (bkpt >= 0)
1182                 __debug_frame->pc = addr;
1183
1184         //gdbstub_printk("alter pc [%d] %08lx\n", bkpt, __debug_frame->pc);
1185
1186 } /* end gdbstub_check_breakpoint() */
1187
1188 /*****************************************************************************/
1189 /*
1190  *
1191  */
1192 static void __maybe_unused gdbstub_show_regs(void)
1193 {
1194         unsigned long *reg;
1195         int loop;
1196
1197         gdbstub_printk("\n");
1198
1199         gdbstub_printk("Frame: @%p [%s]\n",
1200                        __debug_frame,
1201                        __debug_frame->psr & PSR_S ? "kernel" : "user");
1202
1203         reg = (unsigned long *) __debug_frame;
1204         for (loop = 0; loop < NR_PT_REGS; loop++) {
1205                 printk("%s %08lx", regnames[loop + 0], reg[loop + 0]);
1206
1207                 if (loop == NR_PT_REGS - 1 || loop % 5 == 4)
1208                         printk("\n");
1209                 else
1210                         printk(" | ");
1211         }
1212
1213         gdbstub_printk("Process %s (pid: %d)\n", current->comm, current->pid);
1214 } /* end gdbstub_show_regs() */
1215
1216 /*****************************************************************************/
1217 /*
1218  * dump debugging regs
1219  */
1220 static void __maybe_unused gdbstub_dump_debugregs(void)
1221 {
1222         gdbstub_printk("DCR    %08lx  ", __debug_status.dcr);
1223         gdbstub_printk("BRR    %08lx\n", __debug_status.brr);
1224
1225         gdbstub_printk("IBAR0  %08lx  ", __get_ibar(0));
1226         gdbstub_printk("IBAR1  %08lx  ", __get_ibar(1));
1227         gdbstub_printk("IBAR2  %08lx  ", __get_ibar(2));
1228         gdbstub_printk("IBAR3  %08lx\n", __get_ibar(3));
1229
1230         gdbstub_printk("DBAR0  %08lx  ", __get_dbar(0));
1231         gdbstub_printk("DBMR00 %08lx  ", __get_dbmr0(0));
1232         gdbstub_printk("DBMR01 %08lx\n", __get_dbmr1(0));
1233
1234         gdbstub_printk("DBAR1  %08lx  ", __get_dbar(1));
1235         gdbstub_printk("DBMR10 %08lx  ", __get_dbmr0(1));
1236         gdbstub_printk("DBMR11 %08lx\n", __get_dbmr1(1));
1237
1238         gdbstub_printk("\n");
1239 } /* end gdbstub_dump_debugregs() */
1240
1241 /*****************************************************************************/
1242 /*
1243  * dump the MMU state into a structure so that it can be accessed with GDB
1244  */
1245 void gdbstub_get_mmu_state(void)
1246 {
1247         asm volatile("movsg hsr0,%0" : "=r"(__debug_mmu.regs.hsr0));
1248         asm volatile("movsg pcsr,%0" : "=r"(__debug_mmu.regs.pcsr));
1249         asm volatile("movsg esr0,%0" : "=r"(__debug_mmu.regs.esr0));
1250         asm volatile("movsg ear0,%0" : "=r"(__debug_mmu.regs.ear0));
1251         asm volatile("movsg epcr0,%0" : "=r"(__debug_mmu.regs.epcr0));
1252
1253         /* read the protection / SAT registers */
1254         __debug_mmu.iamr[0].L  = __get_IAMLR(0);
1255         __debug_mmu.iamr[0].P  = __get_IAMPR(0);
1256         __debug_mmu.iamr[1].L  = __get_IAMLR(1);
1257         __debug_mmu.iamr[1].P  = __get_IAMPR(1);
1258         __debug_mmu.iamr[2].L  = __get_IAMLR(2);
1259         __debug_mmu.iamr[2].P  = __get_IAMPR(2);
1260         __debug_mmu.iamr[3].L  = __get_IAMLR(3);
1261         __debug_mmu.iamr[3].P  = __get_IAMPR(3);
1262         __debug_mmu.iamr[4].L  = __get_IAMLR(4);
1263         __debug_mmu.iamr[4].P  = __get_IAMPR(4);
1264         __debug_mmu.iamr[5].L  = __get_IAMLR(5);
1265         __debug_mmu.iamr[5].P  = __get_IAMPR(5);
1266         __debug_mmu.iamr[6].L  = __get_IAMLR(6);
1267         __debug_mmu.iamr[6].P  = __get_IAMPR(6);
1268         __debug_mmu.iamr[7].L  = __get_IAMLR(7);
1269         __debug_mmu.iamr[7].P  = __get_IAMPR(7);
1270         __debug_mmu.iamr[8].L  = __get_IAMLR(8);
1271         __debug_mmu.iamr[8].P  = __get_IAMPR(8);
1272         __debug_mmu.iamr[9].L  = __get_IAMLR(9);
1273         __debug_mmu.iamr[9].P  = __get_IAMPR(9);
1274         __debug_mmu.iamr[10].L = __get_IAMLR(10);
1275         __debug_mmu.iamr[10].P = __get_IAMPR(10);
1276         __debug_mmu.iamr[11].L = __get_IAMLR(11);
1277         __debug_mmu.iamr[11].P = __get_IAMPR(11);
1278         __debug_mmu.iamr[12].L = __get_IAMLR(12);
1279         __debug_mmu.iamr[12].P = __get_IAMPR(12);
1280         __debug_mmu.iamr[13].L = __get_IAMLR(13);
1281         __debug_mmu.iamr[13].P = __get_IAMPR(13);
1282         __debug_mmu.iamr[14].L = __get_IAMLR(14);
1283         __debug_mmu.iamr[14].P = __get_IAMPR(14);
1284         __debug_mmu.iamr[15].L = __get_IAMLR(15);
1285         __debug_mmu.iamr[15].P = __get_IAMPR(15);
1286
1287         __debug_mmu.damr[0].L  = __get_DAMLR(0);
1288         __debug_mmu.damr[0].P  = __get_DAMPR(0);
1289         __debug_mmu.damr[1].L  = __get_DAMLR(1);
1290         __debug_mmu.damr[1].P  = __get_DAMPR(1);
1291         __debug_mmu.damr[2].L  = __get_DAMLR(2);
1292         __debug_mmu.damr[2].P  = __get_DAMPR(2);
1293         __debug_mmu.damr[3].L  = __get_DAMLR(3);
1294         __debug_mmu.damr[3].P  = __get_DAMPR(3);
1295         __debug_mmu.damr[4].L  = __get_DAMLR(4);
1296         __debug_mmu.damr[4].P  = __get_DAMPR(4);
1297         __debug_mmu.damr[5].L  = __get_DAMLR(5);
1298         __debug_mmu.damr[5].P  = __get_DAMPR(5);
1299         __debug_mmu.damr[6].L  = __get_DAMLR(6);
1300         __debug_mmu.damr[6].P  = __get_DAMPR(6);
1301         __debug_mmu.damr[7].L  = __get_DAMLR(7);
1302         __debug_mmu.damr[7].P  = __get_DAMPR(7);
1303         __debug_mmu.damr[8].L  = __get_DAMLR(8);
1304         __debug_mmu.damr[8].P  = __get_DAMPR(8);
1305         __debug_mmu.damr[9].L  = __get_DAMLR(9);
1306         __debug_mmu.damr[9].P  = __get_DAMPR(9);
1307         __debug_mmu.damr[10].L = __get_DAMLR(10);
1308         __debug_mmu.damr[10].P = __get_DAMPR(10);
1309         __debug_mmu.damr[11].L = __get_DAMLR(11);
1310         __debug_mmu.damr[11].P = __get_DAMPR(11);
1311         __debug_mmu.damr[12].L = __get_DAMLR(12);
1312         __debug_mmu.damr[12].P = __get_DAMPR(12);
1313         __debug_mmu.damr[13].L = __get_DAMLR(13);
1314         __debug_mmu.damr[13].P = __get_DAMPR(13);
1315         __debug_mmu.damr[14].L = __get_DAMLR(14);
1316         __debug_mmu.damr[14].P = __get_DAMPR(14);
1317         __debug_mmu.damr[15].L = __get_DAMLR(15);
1318         __debug_mmu.damr[15].P = __get_DAMPR(15);
1319
1320 #ifdef CONFIG_MMU
1321         do {
1322                 /* read the DAT entries from the TLB */
1323                 struct __debug_amr *p;
1324                 int loop;
1325
1326                 asm volatile("movsg tplr,%0" : "=r"(__debug_mmu.regs.tplr));
1327                 asm volatile("movsg tppr,%0" : "=r"(__debug_mmu.regs.tppr));
1328                 asm volatile("movsg tpxr,%0" : "=r"(__debug_mmu.regs.tpxr));
1329                 asm volatile("movsg cxnr,%0" : "=r"(__debug_mmu.regs.cxnr));
1330
1331                 p = __debug_mmu.tlb;
1332
1333                 /* way 0 */
1334                 asm volatile("movgs %0,tpxr" :: "r"(0 << TPXR_WAY_SHIFT));
1335                 for (loop = 0; loop < 64; loop++) {
1336                         asm volatile("tlbpr %0,gr0,#1,#0" :: "r"(loop << PAGE_SHIFT));
1337                         asm volatile("movsg tplr,%0" : "=r"(p->L));
1338                         asm volatile("movsg tppr,%0" : "=r"(p->P));
1339                         p++;
1340                 }
1341
1342                 /* way 1 */
1343                 asm volatile("movgs %0,tpxr" :: "r"(1 << TPXR_WAY_SHIFT));
1344                 for (loop = 0; loop < 64; loop++) {
1345                         asm volatile("tlbpr %0,gr0,#1,#0" :: "r"(loop << PAGE_SHIFT));
1346                         asm volatile("movsg tplr,%0" : "=r"(p->L));
1347                         asm volatile("movsg tppr,%0" : "=r"(p->P));
1348                         p++;
1349                 }
1350
1351                 asm volatile("movgs %0,tplr" :: "r"(__debug_mmu.regs.tplr));
1352                 asm volatile("movgs %0,tppr" :: "r"(__debug_mmu.regs.tppr));
1353                 asm volatile("movgs %0,tpxr" :: "r"(__debug_mmu.regs.tpxr));
1354         } while(0);
1355 #endif
1356
1357 } /* end gdbstub_get_mmu_state() */
1358
1359 /*****************************************************************************/
1360 /*
1361  * handle event interception and GDB remote protocol processing
1362  * - on entry:
1363  *      PSR.ET==0, PSR.S==1 and the CPU is in debug mode
1364  *      __debug_frame points to the saved registers
1365  *      __frame points to the kernel mode exception frame, if it was in kernel
1366  *      mode when the break happened
1367  */
1368 void gdbstub(int sigval)
1369 {
1370         unsigned long addr, length, loop, dbar, temp, temp2, temp3;
1371         uint32_t zero;
1372         char *ptr;
1373         int flush_cache = 0;
1374
1375         LEDS(0x5000);
1376
1377         if (sigval < 0) {
1378 #ifndef CONFIG_GDBSTUB_IMMEDIATE
1379                 /* return immediately if GDB immediate activation option not set */
1380                 return;
1381 #else
1382                 sigval = SIGINT;
1383 #endif
1384         }
1385
1386         save_user_regs(&__debug_frame0->uc);
1387
1388 #if 0
1389         gdbstub_printk("--> gdbstub() %08x %p %08x %08x\n",
1390                        __debug_frame->pc,
1391                        __debug_frame,
1392                        __debug_regs->brr,
1393                        __debug_regs->bpsr);
1394 //      gdbstub_show_regs();
1395 #endif
1396
1397         LEDS(0x5001);
1398
1399         /* if we were interrupted by input on the serial gdbstub serial port,
1400          * restore the context prior to the interrupt so that we return to that
1401          * directly
1402          */
1403         temp = (unsigned long) __entry_kerneltrap_table;
1404         temp2 = (unsigned long) __entry_usertrap_table;
1405         temp3 = __debug_frame->pc & ~15;
1406
1407         if (temp3 == temp + TBR_TT_INTERRUPT_15 ||
1408             temp3 == temp2 + TBR_TT_INTERRUPT_15
1409             ) {
1410                 asm volatile("movsg pcsr,%0" : "=r"(__debug_frame->pc));
1411                 __debug_frame->psr |= PSR_ET;
1412                 __debug_frame->psr &= ~PSR_S;
1413                 if (__debug_frame->psr & PSR_PS)
1414                         __debug_frame->psr |= PSR_S;
1415                 __debug_status.brr = (__debug_frame->tbr & TBR_TT) << 12;
1416                 __debug_status.brr |= BRR_EB;
1417                 sigval = SIGINT;
1418         }
1419
1420         /* handle the decrement timer going off (FR451 only) */
1421         if (temp3 == temp + TBR_TT_DECREMENT_TIMER ||
1422             temp3 == temp2 + TBR_TT_DECREMENT_TIMER
1423             ) {
1424                 asm volatile("movgs %0,timerd" :: "r"(10000000));
1425                 asm volatile("movsg pcsr,%0" : "=r"(__debug_frame->pc));
1426                 __debug_frame->psr |= PSR_ET;
1427                 __debug_frame->psr &= ~PSR_S;
1428                 if (__debug_frame->psr & PSR_PS)
1429                         __debug_frame->psr |= PSR_S;
1430                 __debug_status.brr = (__debug_frame->tbr & TBR_TT) << 12;
1431                 __debug_status.brr |= BRR_EB;
1432                 sigval = SIGXCPU;
1433         }
1434
1435         LEDS(0x5002);
1436
1437         /* after a BREAK insn, the PC lands on the far side of it */
1438         if (__debug_status.brr & BRR_SB)
1439                 gdbstub_check_breakpoint();
1440
1441         LEDS(0x5003);
1442
1443         /* handle attempts to write console data via GDB "O" commands */
1444         if (__debug_frame->pc == (unsigned long) gdbstub_console_write + 4) {
1445                 __gdbstub_console_write((struct console *) __debug_frame->gr8,
1446                                         (const char *) __debug_frame->gr9,
1447                                         (unsigned) __debug_frame->gr10);
1448                 goto done;
1449         }
1450
1451         if (gdbstub_rx_unget) {
1452                 sigval = SIGINT;
1453                 goto packet_waiting;
1454         }
1455
1456         if (!sigval)
1457                 sigval = gdbstub_compute_signal(__debug_status.brr);
1458
1459         LEDS(0x5004);
1460
1461         /* send a message to the debugger's user saying what happened if it may
1462          * not be clear cut (we can't map exceptions onto signals properly)
1463          */
1464         if (sigval != SIGINT && sigval != SIGTRAP && sigval != SIGILL) {
1465                 static const char title[] = "Break ";
1466                 static const char crlf[] = "\r\n";
1467                 unsigned long brr = __debug_status.brr;
1468                 char hx;
1469
1470                 ptr = output_buffer;
1471                 *ptr++ = 'O';
1472                 ptr = mem2hex(title, ptr, sizeof(title) - 1,0);
1473
1474                 hx = hexchars[(brr & 0xf0000000) >> 28];
1475                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1476                 hx = hexchars[(brr & 0x0f000000) >> 24];
1477                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1478                 hx = hexchars[(brr & 0x00f00000) >> 20];
1479                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1480                 hx = hexchars[(brr & 0x000f0000) >> 16];
1481                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1482                 hx = hexchars[(brr & 0x0000f000) >> 12];
1483                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1484                 hx = hexchars[(brr & 0x00000f00) >> 8];
1485                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1486                 hx = hexchars[(brr & 0x000000f0) >> 4];
1487                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1488                 hx = hexchars[(brr & 0x0000000f)];
1489                 *ptr++ = hexchars[hx >> 4];     *ptr++ = hexchars[hx & 0xf];
1490
1491                 ptr = mem2hex(crlf, ptr, sizeof(crlf) - 1, 0);
1492                 *ptr = 0;
1493                 gdbstub_send_packet(output_buffer);     /* send it off... */
1494         }
1495
1496         LEDS(0x5005);
1497
1498         /* tell the debugger that an exception has occurred */
1499         ptr = output_buffer;
1500
1501         /* Send trap type (converted to signal) */
1502         *ptr++ = 'T';
1503         *ptr++ = hexchars[sigval >> 4];
1504         *ptr++ = hexchars[sigval & 0xf];
1505
1506         /* Send Error PC */
1507         *ptr++ = hexchars[GDB_REG_PC >> 4];
1508         *ptr++ = hexchars[GDB_REG_PC & 0xf];
1509         *ptr++ = ':';
1510         ptr = mem2hex(&__debug_frame->pc, ptr, 4, 0);
1511         *ptr++ = ';';
1512
1513         /*
1514          * Send frame pointer
1515          */
1516         *ptr++ = hexchars[GDB_REG_FP >> 4];
1517         *ptr++ = hexchars[GDB_REG_FP & 0xf];
1518         *ptr++ = ':';
1519         ptr = mem2hex(&__debug_frame->fp, ptr, 4, 0);
1520         *ptr++ = ';';
1521
1522         /*
1523          * Send stack pointer
1524          */
1525         *ptr++ = hexchars[GDB_REG_SP >> 4];
1526         *ptr++ = hexchars[GDB_REG_SP & 0xf];
1527         *ptr++ = ':';
1528         ptr = mem2hex(&__debug_frame->sp, ptr, 4, 0);
1529         *ptr++ = ';';
1530
1531         *ptr++ = 0;
1532         gdbstub_send_packet(output_buffer);     /* send it off... */
1533
1534         LEDS(0x5006);
1535
1536  packet_waiting:
1537         gdbstub_get_mmu_state();
1538
1539         /* wait for input from remote GDB */
1540         while (1) {
1541                 output_buffer[0] = 0;
1542
1543                 LEDS(0x5007);
1544                 gdbstub_recv_packet(input_buffer);
1545                 LEDS(0x5600 | input_buffer[0]);
1546
1547                 switch (input_buffer[0]) {
1548                         /* request repeat of last signal number */
1549                 case '?':
1550                         output_buffer[0] = 'S';
1551                         output_buffer[1] = hexchars[sigval >> 4];
1552                         output_buffer[2] = hexchars[sigval & 0xf];
1553                         output_buffer[3] = 0;
1554                         break;
1555
1556                 case 'd':
1557                         /* toggle debug flag */
1558                         break;
1559
1560                         /* return the value of the CPU registers
1561                          * - GR0,  GR1,  GR2,  GR3,  GR4,  GR5,  GR6,  GR7,
1562                          * - GR8,  GR9,  GR10, GR11, GR12, GR13, GR14, GR15,
1563                          * - GR16, GR17, GR18, GR19, GR20, GR21, GR22, GR23,
1564                          * - GR24, GR25, GR26, GR27, GR28, GR29, GR30, GR31,
1565                          * - GR32, GR33, GR34, GR35, GR36, GR37, GR38, GR39,
1566                          * - GR40, GR41, GR42, GR43, GR44, GR45, GR46, GR47,
1567                          * - GR48, GR49, GR50, GR51, GR52, GR53, GR54, GR55,
1568                          * - GR56, GR57, GR58, GR59, GR60, GR61, GR62, GR63,
1569                          * - FP0,  FP1,  FP2,  FP3,  FP4,  FP5,  FP6,  FP7,
1570                          * - FP8,  FP9,  FP10, FP11, FP12, FP13, FP14, FP15,
1571                          * - FP16, FP17, FP18, FP19, FP20, FP21, FP22, FP23,
1572                          * - FP24, FP25, FP26, FP27, FP28, FP29, FP30, FP31,
1573                          * - FP32, FP33, FP34, FP35, FP36, FP37, FP38, FP39,
1574                          * - FP40, FP41, FP42, FP43, FP44, FP45, FP46, FP47,
1575                          * - FP48, FP49, FP50, FP51, FP52, FP53, FP54, FP55,
1576                          * - FP56, FP57, FP58, FP59, FP60, FP61, FP62, FP63,
1577                          * - PC, PSR, CCR, CCCR,
1578                          * - _X132, _X133, _X134
1579                          * - TBR, BRR, DBAR0, DBAR1, DBAR2, DBAR3,
1580                          * - _X141, _X142, _X143, _X144,
1581                          * - LR, LCR
1582                          */
1583                 case 'g':
1584                         zero = 0;
1585                         ptr = output_buffer;
1586
1587                         /* deal with GR0, GR1-GR27, GR28-GR31, GR32-GR63 */
1588                         ptr = mem2hex(&zero, ptr, 4, 0);
1589
1590                         for (loop = 1; loop <= 27; loop++)
1591                                 ptr = mem2hex(&__debug_user_context->i.gr[loop], ptr, 4, 0);
1592                         temp = (unsigned long) __frame;
1593                         ptr = mem2hex(&temp, ptr, 4, 0);
1594                         ptr = mem2hex(&__debug_user_context->i.gr[29], ptr, 4, 0);
1595                         ptr = mem2hex(&__debug_user_context->i.gr[30], ptr, 4, 0);
1596 #ifdef CONFIG_MMU
1597                         ptr = mem2hex(&__debug_user_context->i.gr[31], ptr, 4, 0);
1598 #else
1599                         temp = (unsigned long) __debug_frame;
1600                         ptr = mem2hex(&temp, ptr, 4, 0);
1601 #endif
1602
1603                         for (loop = 32; loop <= 63; loop++)
1604                                 ptr = mem2hex(&__debug_user_context->i.gr[loop], ptr, 4, 0);
1605
1606                         /* deal with FR0-FR63 */
1607                         for (loop = 0; loop <= 63; loop++)
1608                                 ptr = mem2hex(&__debug_user_context->f.fr[loop], ptr, 4, 0);
1609
1610                         /* deal with special registers */
1611                         ptr = mem2hex(&__debug_frame->pc,    ptr, 4, 0);
1612                         ptr = mem2hex(&__debug_frame->psr,   ptr, 4, 0);
1613                         ptr = mem2hex(&__debug_frame->ccr,   ptr, 4, 0);
1614                         ptr = mem2hex(&__debug_frame->cccr,  ptr, 4, 0);
1615                         ptr = mem2hex(&zero, ptr, 4, 0);
1616                         ptr = mem2hex(&zero, ptr, 4, 0);
1617                         ptr = mem2hex(&zero, ptr, 4, 0);
1618                         ptr = mem2hex(&__debug_frame->tbr,   ptr, 4, 0);
1619                         ptr = mem2hex(&__debug_status.brr ,   ptr, 4, 0);
1620
1621                         asm volatile("movsg dbar0,%0" : "=r"(dbar));
1622                         ptr = mem2hex(&dbar, ptr, 4, 0);
1623                         asm volatile("movsg dbar1,%0" : "=r"(dbar));
1624                         ptr = mem2hex(&dbar, ptr, 4, 0);
1625                         asm volatile("movsg dbar2,%0" : "=r"(dbar));
1626                         ptr = mem2hex(&dbar, ptr, 4, 0);
1627                         asm volatile("movsg dbar3,%0" : "=r"(dbar));
1628                         ptr = mem2hex(&dbar, ptr, 4, 0);
1629
1630                         asm volatile("movsg scr0,%0" : "=r"(dbar));
1631                         ptr = mem2hex(&dbar, ptr, 4, 0);
1632                         asm volatile("movsg scr1,%0" : "=r"(dbar));
1633                         ptr = mem2hex(&dbar, ptr, 4, 0);
1634                         asm volatile("movsg scr2,%0" : "=r"(dbar));
1635                         ptr = mem2hex(&dbar, ptr, 4, 0);
1636                         asm volatile("movsg scr3,%0" : "=r"(dbar));
1637                         ptr = mem2hex(&dbar, ptr, 4, 0);
1638
1639                         ptr = mem2hex(&__debug_frame->lr, ptr, 4, 0);
1640                         ptr = mem2hex(&__debug_frame->lcr, ptr, 4, 0);
1641
1642                         ptr = mem2hex(&__debug_frame->iacc0, ptr, 8, 0);
1643
1644                         ptr = mem2hex(&__debug_user_context->f.fsr[0], ptr, 4, 0);
1645
1646                         for (loop = 0; loop <= 7; loop++)
1647                                 ptr = mem2hex(&__debug_user_context->f.acc[loop], ptr, 4, 0);
1648
1649                         ptr = mem2hex(&__debug_user_context->f.accg, ptr, 8, 0);
1650
1651                         for (loop = 0; loop <= 1; loop++)
1652                                 ptr = mem2hex(&__debug_user_context->f.msr[loop], ptr, 4, 0);
1653
1654                         ptr = mem2hex(&__debug_frame->gner0, ptr, 4, 0);
1655                         ptr = mem2hex(&__debug_frame->gner1, ptr, 4, 0);
1656
1657                         ptr = mem2hex(&__debug_user_context->f.fner[0], ptr, 4, 0);
1658                         ptr = mem2hex(&__debug_user_context->f.fner[1], ptr, 4, 0);
1659
1660                         break;
1661
1662                         /* set the values of the CPU registers */
1663                 case 'G':
1664                         ptr = &input_buffer[1];
1665
1666                         /* deal with GR0, GR1-GR27, GR28-GR31, GR32-GR63 */
1667                         ptr = hex2mem(ptr, &temp, 4);
1668
1669                         for (loop = 1; loop <= 27; loop++)
1670                                 ptr = hex2mem(ptr, &__debug_user_context->i.gr[loop], 4);
1671
1672                         ptr = hex2mem(ptr, &temp, 4);
1673                         __frame = (struct pt_regs *) temp;
1674                         ptr = hex2mem(ptr, &__debug_frame->gr29, 4);
1675                         ptr = hex2mem(ptr, &__debug_frame->gr30, 4);
1676 #ifdef CONFIG_MMU
1677                         ptr = hex2mem(ptr, &__debug_frame->gr31, 4);
1678 #else
1679                         ptr = hex2mem(ptr, &temp, 4);
1680 #endif
1681
1682                         for (loop = 32; loop <= 63; loop++)
1683                                 ptr = hex2mem(ptr, &__debug_user_context->i.gr[loop], 4);
1684
1685                         /* deal with FR0-FR63 */
1686                         for (loop = 0; loop <= 63; loop++)
1687                                 ptr = mem2hex(&__debug_user_context->f.fr[loop], ptr, 4, 0);
1688
1689                         /* deal with special registers */
1690                         ptr = hex2mem(ptr, &__debug_frame->pc,  4);
1691                         ptr = hex2mem(ptr, &__debug_frame->psr, 4);
1692                         ptr = hex2mem(ptr, &__debug_frame->ccr, 4);
1693                         ptr = hex2mem(ptr, &__debug_frame->cccr,4);
1694
1695                         for (loop = 132; loop <= 140; loop++)
1696                                 ptr = hex2mem(ptr, &temp, 4);
1697
1698                         ptr = hex2mem(ptr, &temp, 4);
1699                         asm volatile("movgs %0,scr0" :: "r"(temp));
1700                         ptr = hex2mem(ptr, &temp, 4);
1701                         asm volatile("movgs %0,scr1" :: "r"(temp));
1702                         ptr = hex2mem(ptr, &temp, 4);
1703                         asm volatile("movgs %0,scr2" :: "r"(temp));
1704                         ptr = hex2mem(ptr, &temp, 4);
1705                         asm volatile("movgs %0,scr3" :: "r"(temp));
1706
1707                         ptr = hex2mem(ptr, &__debug_frame->lr,  4);
1708                         ptr = hex2mem(ptr, &__debug_frame->lcr, 4);
1709
1710                         ptr = hex2mem(ptr, &__debug_frame->iacc0, 8);
1711
1712                         ptr = hex2mem(ptr, &__debug_user_context->f.fsr[0], 4);
1713
1714                         for (loop = 0; loop <= 7; loop++)
1715                                 ptr = hex2mem(ptr, &__debug_user_context->f.acc[loop], 4);
1716
1717                         ptr = hex2mem(ptr, &__debug_user_context->f.accg, 8);
1718
1719                         for (loop = 0; loop <= 1; loop++)
1720                                 ptr = hex2mem(ptr, &__debug_user_context->f.msr[loop], 4);
1721
1722                         ptr = hex2mem(ptr, &__debug_frame->gner0, 4);
1723                         ptr = hex2mem(ptr, &__debug_frame->gner1, 4);
1724
1725                         ptr = hex2mem(ptr, &__debug_user_context->f.fner[0], 4);
1726                         ptr = hex2mem(ptr, &__debug_user_context->f.fner[1], 4);
1727
1728                         gdbstub_strcpy(output_buffer,"OK");
1729                         break;
1730
1731                         /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
1732                 case 'm':
1733                         ptr = &input_buffer[1];
1734
1735                         if (hexToInt(&ptr, &addr) &&
1736                             *ptr++ == ',' &&
1737                             hexToInt(&ptr, &length)
1738                             ) {
1739                                 if (mem2hex((char *)addr, output_buffer, length, 1))
1740                                         break;
1741                                 gdbstub_strcpy (output_buffer, "E03");
1742                         }
1743                         else {
1744                                 gdbstub_strcpy(output_buffer,"E01");
1745                         }
1746                         break;
1747
1748                         /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
1749                 case 'M':
1750                         ptr = &input_buffer[1];
1751
1752                         if (hexToInt(&ptr, &addr) &&
1753                             *ptr++ == ',' &&
1754                             hexToInt(&ptr, &length) &&
1755                             *ptr++ == ':'
1756                             ) {
1757                                 if (hex2mem(ptr, (char *)addr, length)) {
1758                                         gdbstub_strcpy(output_buffer, "OK");
1759                                 }
1760                                 else {
1761                                         gdbstub_strcpy(output_buffer, "E03");
1762                                 }
1763                         }
1764                         else
1765                                 gdbstub_strcpy(output_buffer, "E02");
1766
1767                         flush_cache = 1;
1768                         break;
1769
1770                         /* PNN,=RRRRRRRR: Write value R to reg N return OK */
1771                 case 'P':
1772                         ptr = &input_buffer[1];
1773
1774                         if (!hexToInt(&ptr, &addr) ||
1775                             *ptr++ != '=' ||
1776                             !hexToInt(&ptr, &temp)
1777                             ) {
1778                                 gdbstub_strcpy(output_buffer, "E01");
1779                                 break;
1780                         }
1781
1782                         temp2 = 1;
1783                         switch (addr) {
1784                         case GDB_REG_GR(0):
1785                                 break;
1786                         case GDB_REG_GR(1) ... GDB_REG_GR(63):
1787                                 __debug_user_context->i.gr[addr - GDB_REG_GR(0)] = temp;
1788                                 break;
1789                         case GDB_REG_FR(0) ... GDB_REG_FR(63):
1790                                 __debug_user_context->f.fr[addr - GDB_REG_FR(0)] = temp;
1791                                 break;
1792                         case GDB_REG_PC:
1793                                 __debug_user_context->i.pc = temp;
1794                                 break;
1795                         case GDB_REG_PSR:
1796                                 __debug_user_context->i.psr = temp;
1797                                 break;
1798                         case GDB_REG_CCR:
1799                                 __debug_user_context->i.ccr = temp;
1800                                 break;
1801                         case GDB_REG_CCCR:
1802                                 __debug_user_context->i.cccr = temp;
1803                                 break;
1804                         case GDB_REG_BRR:
1805                                 __debug_status.brr = temp;
1806                                 break;
1807                         case GDB_REG_LR:
1808                                 __debug_user_context->i.lr = temp;
1809                                 break;
1810                         case GDB_REG_LCR:
1811                                 __debug_user_context->i.lcr = temp;
1812                                 break;
1813                         case GDB_REG_FSR0:
1814                                 __debug_user_context->f.fsr[0] = temp;
1815                                 break;
1816                         case GDB_REG_ACC(0) ... GDB_REG_ACC(7):
1817                                 __debug_user_context->f.acc[addr - GDB_REG_ACC(0)] = temp;
1818                                 break;
1819                         case GDB_REG_ACCG(0):
1820                                 *(uint32_t *) &__debug_user_context->f.accg[0] = temp;
1821                                 break;
1822                         case GDB_REG_ACCG(4):
1823                                 *(uint32_t *) &__debug_user_context->f.accg[4] = temp;
1824                                 break;
1825                         case GDB_REG_MSR(0) ... GDB_REG_MSR(1):
1826                                 __debug_user_context->f.msr[addr - GDB_REG_MSR(0)] = temp;
1827                                 break;
1828                         case GDB_REG_GNER(0) ... GDB_REG_GNER(1):
1829                                 __debug_user_context->i.gner[addr - GDB_REG_GNER(0)] = temp;
1830                                 break;
1831                         case GDB_REG_FNER(0) ... GDB_REG_FNER(1):
1832                                 __debug_user_context->f.fner[addr - GDB_REG_FNER(0)] = temp;
1833                                 break;
1834                         default:
1835                                 temp2 = 0;
1836                                 break;
1837                         }
1838
1839                         if (temp2) {
1840                                 gdbstub_strcpy(output_buffer, "OK");
1841                         }
1842                         else {
1843                                 gdbstub_strcpy(output_buffer, "E02");
1844                         }
1845                         break;
1846
1847                         /* cAA..AA    Continue at address AA..AA(optional) */
1848                 case 'c':
1849                         /* try to read optional parameter, pc unchanged if no parm */
1850                         ptr = &input_buffer[1];
1851                         if (hexToInt(&ptr, &addr))
1852                                 __debug_frame->pc = addr;
1853                         goto done;
1854
1855                         /* kill the program */
1856                 case 'k' :
1857                         goto done;      /* just continue */
1858
1859
1860                         /* reset the whole machine (FIXME: system dependent) */
1861                 case 'r':
1862                         break;
1863
1864
1865                         /* step to next instruction */
1866                 case 's':
1867                         __debug_regs->dcr |= DCR_SE;
1868                         __debug_status.dcr |= DCR_SE;
1869                         goto done;
1870
1871                         /* set baud rate (bBB) */
1872                 case 'b':
1873                         ptr = &input_buffer[1];
1874                         if (!hexToInt(&ptr, &temp)) {
1875                                 gdbstub_strcpy(output_buffer,"B01");
1876                                 break;
1877                         }
1878
1879                         if (temp) {
1880                                 /* ack before changing speed */
1881                                 gdbstub_send_packet("OK");
1882                                 gdbstub_set_baud(temp);
1883                         }
1884                         break;
1885
1886                         /* set breakpoint */
1887                 case 'Z':
1888                         ptr = &input_buffer[1];
1889
1890                         if (!hexToInt(&ptr,&temp) || *ptr++ != ',' ||
1891                             !hexToInt(&ptr,&addr) || *ptr++ != ',' ||
1892                             !hexToInt(&ptr,&length)
1893                             ) {
1894                                 gdbstub_strcpy(output_buffer,"E01");
1895                                 break;
1896                         }
1897
1898                         if (temp >= 5) {
1899                                 gdbstub_strcpy(output_buffer,"E03");
1900                                 break;
1901                         }
1902
1903                         if (gdbstub_set_breakpoint(temp, addr, length) < 0) {
1904                                 gdbstub_strcpy(output_buffer,"E03");
1905                                 break;
1906                         }
1907
1908                         if (temp == 0)
1909                                 flush_cache = 1; /* soft bkpt by modified memory */
1910
1911                         gdbstub_strcpy(output_buffer,"OK");
1912                         break;
1913
1914                         /* clear breakpoint */
1915                 case 'z':
1916                         ptr = &input_buffer[1];
1917
1918                         if (!hexToInt(&ptr,&temp) || *ptr++ != ',' ||
1919                             !hexToInt(&ptr,&addr) || *ptr++ != ',' ||
1920                             !hexToInt(&ptr,&length)
1921                             ) {
1922                                 gdbstub_strcpy(output_buffer,"E01");
1923                                 break;
1924                         }
1925
1926                         if (temp >= 5) {
1927                                 gdbstub_strcpy(output_buffer,"E03");
1928                                 break;
1929                         }
1930
1931                         if (gdbstub_clear_breakpoint(temp, addr, length) < 0) {
1932                                 gdbstub_strcpy(output_buffer,"E03");
1933                                 break;
1934                         }
1935
1936                         if (temp == 0)
1937                                 flush_cache = 1; /* soft bkpt by modified memory */
1938
1939                         gdbstub_strcpy(output_buffer,"OK");
1940                         break;
1941
1942                 default:
1943                         gdbstub_proto("### GDB Unsupported Cmd '%s'\n",input_buffer);
1944                         break;
1945                 }
1946
1947                 /* reply to the request */
1948                 LEDS(0x5009);
1949                 gdbstub_send_packet(output_buffer);
1950         }
1951
1952  done:
1953         restore_user_regs(&__debug_frame0->uc);
1954
1955         //gdbstub_dump_debugregs();
1956         //gdbstub_printk("<-- gdbstub() %08x\n", __debug_frame->pc);
1957
1958         /* need to flush the instruction cache before resuming, as we may have
1959          * deposited a breakpoint, and the icache probably has no way of
1960          * knowing that a data ref to some location may have changed something
1961          * that is in the instruction cache.  NB: We flush both caches, just to
1962          * be sure...
1963          */
1964
1965         /* note: flushing the icache will clobber EAR0 on the FR451 */
1966         if (flush_cache)
1967                 gdbstub_purge_cache();
1968
1969         LEDS(0x5666);
1970
1971 } /* end gdbstub() */
1972
1973 /*****************************************************************************/
1974 /*
1975  * initialise the GDB stub
1976  */
1977 void __init gdbstub_init(void)
1978 {
1979 #ifdef CONFIG_GDBSTUB_IMMEDIATE
1980         unsigned char ch;
1981         int ret;
1982 #endif
1983
1984         gdbstub_printk("%s", gdbstub_banner);
1985
1986         gdbstub_io_init();
1987
1988         /* try to talk to GDB (or anyone insane enough to want to type GDB protocol by hand) */
1989         gdbstub_proto("### GDB Tx ACK\n");
1990         gdbstub_tx_char('+'); /* 'hello world' */
1991
1992 #ifdef CONFIG_GDBSTUB_IMMEDIATE
1993         gdbstub_printk("GDB Stub waiting for packet\n");
1994
1995         /*
1996          * In case GDB is started before us, ack any packets
1997          * (presumably "$?#xx") sitting there.
1998          */
1999         do { gdbstub_rx_char(&ch, 0); } while (ch != '$');
2000         do { gdbstub_rx_char(&ch, 0); } while (ch != '#');
2001         do { ret = gdbstub_rx_char(&ch, 0); } while (ret != 0); /* eat first csum byte */
2002         do { ret = gdbstub_rx_char(&ch, 0); } while (ret != 0); /* eat second csum byte */
2003
2004         gdbstub_proto("### GDB Tx NAK\n");
2005         gdbstub_tx_char('-'); /* nak it */
2006
2007 #else
2008         gdbstub_printk("GDB Stub set\n");
2009 #endif
2010
2011 #if 0
2012         /* send banner */
2013         ptr = output_buffer;
2014         *ptr++ = 'O';
2015         ptr = mem2hex(gdbstub_banner, ptr, sizeof(gdbstub_banner) - 1, 0);
2016         gdbstub_send_packet(output_buffer);
2017 #endif
2018 #if defined(CONFIG_GDB_CONSOLE) && defined(CONFIG_GDBSTUB_IMMEDIATE)
2019         register_console(&gdbstub_console);
2020 #endif
2021
2022 } /* end gdbstub_init() */
2023
2024 /*****************************************************************************/
2025 /*
2026  * register the console at a more appropriate time
2027  */
2028 #if defined (CONFIG_GDB_CONSOLE) && !defined(CONFIG_GDBSTUB_IMMEDIATE)
2029 static int __init gdbstub_postinit(void)
2030 {
2031         printk("registering console\n");
2032         register_console(&gdbstub_console);
2033         return 0;
2034 } /* end gdbstub_postinit() */
2035
2036 __initcall(gdbstub_postinit);
2037 #endif
2038
2039 /*****************************************************************************/
2040 /*
2041  * send an exit message to GDB
2042  */
2043 void gdbstub_exit(int status)
2044 {
2045         unsigned char checksum;
2046         int count;
2047         unsigned char ch;
2048
2049         sprintf(output_buffer,"W%02x",status&0xff);
2050
2051         gdbstub_tx_char('$');
2052         checksum = 0;
2053         count = 0;
2054
2055         while ((ch = output_buffer[count]) != 0) {
2056                 gdbstub_tx_char(ch);
2057                 checksum += ch;
2058                 count += 1;
2059         }
2060
2061         gdbstub_tx_char('#');
2062         gdbstub_tx_char(hexchars[checksum >> 4]);
2063         gdbstub_tx_char(hexchars[checksum & 0xf]);
2064
2065         /* make sure the output is flushed, or else RedBoot might clobber it */
2066         gdbstub_tx_char('-');
2067         gdbstub_tx_flush();
2068
2069 } /* end gdbstub_exit() */
2070
2071 /*****************************************************************************/
2072 /*
2073  * GDB wants to call malloc() and free() to allocate memory for calling kernel
2074  * functions directly from its command line
2075  */
2076 static void *malloc(size_t size) __maybe_unused;
2077 static void *malloc(size_t size)
2078 {
2079         return kmalloc(size, GFP_ATOMIC);
2080 }
2081
2082 static void free(void *p) __maybe_unused;
2083 static void free(void *p)
2084 {
2085         kfree(p);
2086 }
2087
2088 static uint32_t ___get_HSR0(void) __maybe_unused;
2089 static uint32_t ___get_HSR0(void)
2090 {
2091         return __get_HSR(0);
2092 }
2093
2094 static uint32_t ___set_HSR0(uint32_t x) __maybe_unused;
2095 static uint32_t ___set_HSR0(uint32_t x)
2096 {
2097         __set_HSR(0, x);
2098         return __get_HSR(0);
2099 }