path: root/cpukit/libdebugger/rtems-debugger-powerpc.c

/*
 * Copyright (c) 2023 Chris Johns <chrisj@rtems.org>.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#define TARGET_DEBUG 1

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <errno.h>
#include <inttypes.h>
#include <stdlib.h>

#include <rtems.h>
#include <rtems/score/threadimpl.h>

#include "rtems-debugger-target.h"
#include "rtems-debugger-threads.h"

#include <rtems/powerpc/registers.h>

/*
 * Hardware breakpoints. Limited by hardware
 */
#define RTEMS_DEBUGGER_HWBREAK_NUM 4

/*
 * Number of registers.
 */
#define RTEMS_DEBUGGER_NUMREGS 72

/*
 * Debugger registers layout.
 */
#define REG_R0            0
#define REG_R1            1
#define REG_R2            2
#define REG_R3            3
#define REG_R4            4
#define REG_R5            5
#define REG_R6            6
#define REG_R7            7
#define REG_R8            8
#define REG_R9            9
#define REG_R10          10
#define REG_R11          11
#define REG_R12          12
#define REG_R13          13
#define REG_R14          14
#define REG_R15          15
#define REG_R16          16
#define REG_R17          17
#define REG_R18          18
#define REG_R19          19
#define REG_R20          20
#define REG_R21          21
#define REG_R22          22
#define REG_R23          23
#define REG_R24          24
#define REG_R25          25
#define REG_R26          26
#define REG_R27          27
#define REG_R28          28
#define REG_R29          29
#define REG_R30          30
#define REG_R31          31
#define REG_F0           32
#define REG_F1           33
#define REG_F2           34
#define REG_F3           35
#define REG_F4           36
#define REG_F5           37
#define REG_F6           38
#define REG_F7           39
#define REG_F8           40
#define REG_F9           41
#define REG_F10          42
#define REG_F11          43
#define REG_F12          44
#define REG_F13          45
#define REG_F14          46
#define REG_F15          47
#define REG_F16          48
#define REG_F17          49
#define REG_F18          50
#define REG_F19          51
#define REG_F20          52
#define REG_F21          53
#define REG_F22          54
#define REG_F23          55
#define REG_F24          56
#define REG_F25          57
#define REG_F26          58
#define REG_F27          59
#define REG_F28          60
#define REG_F29          61
#define REG_F30          62
#define REG_F31          63
#define REG_PC           64
#define REG_MSR          65
#define REG_CND          66
#define REG_LR           67
#define REG_CNT          68
#define REG_XER          69
#define REG_FPSCR        70
#define REG_MQ           71

/**
 * Register offset table with the total as the last entry.
 *
 * Check this table in gdb with the command:
 *
 *   maint print registers
 */
static const size_t ppc_reg_offsets[RTEMS_DEBUGGER_NUMREGS + 1] =
{
  0,     /* REG_R0     4 long */
  4,     /* REG_R1     4 long */
  8,     /* REG_R2     4 long */
  12,    /* REG_R3     4 long */
  16,    /* REG_R4     4 long */
  20,    /* REG_R5     4 long */
  24,    /* REG_R6     4 long */
  28,    /* REG_R7     4 long */
  32,    /* REG_R8     4 long */
  36,    /* REG_R9     4 long */
  40,    /* REG_R10    4 long */
  44,    /* REG_R11    4 long */
  48,    /* REG_R12    4 long */
  52,    /* REG_R13    4 long */
  56,    /* REG_R14    4 long */
  60,    /* REG_R15    4 long */
  64,    /* REG_R16    4 long */
  68,    /* REG_R17    4 long */
  72,    /* REG_R18    4 long */
  76,    /* REG_R19    4 long */
  80,    /* REG_R20    4 long */
  84,    /* REG_R21    4 long */
  88,    /* REG_R22    4 long */
  92,    /* REG_R23    4 long */
  96,    /* REG_R24    4 long */
  100,   /* REG_R25    4 long */
  104,   /* REG_R26    4 long */
  108,   /* REG_R27    4 long */
  112,   /* REG_R28    4 long */
  116,   /* REG_R29    4 long */
  120,   /* REG_R30    4 long */
  124,   /* REG_R31    4 long */
  128,   /* REG_F0     8 _ieee_double */
  136,   /* REG_F1     8 _ieee_double */
  144,   /* REG_F2     8 _ieee_double */
  152,   /* REG_F3     8 _ieee_double */
  160,   /* REG_F4     8 _ieee_double */
  168,   /* REG_F5     8 _ieee_double */
  176,   /* REG_F6     8 _ieee_double */
  184,   /* REG_F7     8 _ieee_double */
  192,   /* REG_F8     8 _ieee_double */
  200,   /* REG_F9     8 _ieee_double */
  208,   /* REG_F10    8 _ieee_double */
  216,   /* REG_F11    8 _ieee_double */
  224,   /* REG_F12    8 _ieee_double */
  232,   /* REG_F13    8 _ieee_double */
  240,   /* REG_F14    8 _ieee_double */
  248,   /* REG_F15    8 _ieee_double */
  256,   /* REG_F16    8 _ieee_double */
  264,   /* REG_F17    8 _ieee_double */
  272,   /* REG_F18    8 _ieee_double */
  280,   /* REG_F19    8 _ieee_double */
  288,   /* REG_F20    8 _ieee_double */
  296,   /* REG_F21    8 _ieee_double */
  304,   /* REG_F22    8 _ieee_double */
  312,   /* REG_F23    8 _ieee_double */
  320,   /* REG_F24    8 _ieee_double */
  328,   /* REG_F25    8 _ieee_double */
  336,   /* REG_F26    8 _ieee_double */
  344,   /* REG_F27    8 _ieee_double */
  352,   /* REG_F28    8 _ieee_double */
  360,   /* REG_F29    8 _ieee_double */
  368,   /* REG_F30    8 _ieee_double */
  376,   /* REG_F31    8 _ieee_double */
  384,   /* REG_PC     4 *1 */
  388,   /* REG_MSR    4 long */
  392,   /* REG_CND    4 long */
  396,   /* REG_LR     4 *1 */
  400,   /* REG_CNT    4 long */
  404,   /* REG_XER    4 long */
  408,   /* REG_FPSCR  4 long */
  412,   /* REG_MQ     4 long */
  416    /* total size */
};

/*
 * Number of bytes of registers.
 */
#define RTEMS_DEBUGGER_NUMREGBYTES ppc_reg_offsets[RTEMS_DEBUGGER_NUMREGS]

/*
 * Exception handler to hook.
 */
typedef CPU_Exception_frame BSP_Exception_frame;
typedef void (*exception_handler_t)(BSP_Exception_frame*);
extern exception_handler_t globalExceptHdl;

/**
 * The `sc` instruction
 */
#define TARGET_BKPT 0xcc

static const uint8_t breakpoint[1] = { TARGET_BKPT };

/**
 * Target lock.
 */
RTEMS_INTERRUPT_LOCK_DEFINE(static, target_lock, "target_lock")

/**
 * The orginal exception handler.
 */
static void (*orig_currentExcHandler)(CPU_Exception_frame* frame);

#if TARGET_DEBUG
#include <rtems/bspIo.h>
static void target_printk(const char* format, ...) RTEMS_PRINTFLIKE(1, 2);
static void
target_printk(const char* format, ...)
{
  va_list ap;
  va_start(ap, format);
  vprintk(format, ap);
  va_end(ap);
}
#else
#define target_printk(_fmt, ...)
#endif

/*
 * The CPU Ident code is taken from libcpu/cpuIndent.h because
 * this cpukit code cannot reach over into the BSP headers.
 * Adding the code here is not optimal but it solves the need.
 */
#define ASM_RESET_VECTOR                     0x01
#define ASM_MACH_VECTOR                      0x02
#define ASM_PROT_VECTOR                      0x03
#define ASM_ISI_VECTOR                       0x04
#define ASM_EXT_VECTOR                       0x05
#define ASM_ALIGN_VECTOR                     0x06
#define ASM_PROG_VECTOR                      0x07
#define ASM_FLOAT_VECTOR                     0x08
#define ASM_DEC_VECTOR                       0x09
#define ASM_SYS_VECTOR                       0x0C
#define ASM_TRACE_VECTOR                     0x0D

#define ASM_60X_VEC_VECTOR                   0x0A
#define ASM_60X_PERFMON_VECTOR               0x0F
#define ASM_60X_IMISS_VECTOR                 0x10
#define ASM_60X_DLMISS_VECTOR                0x11
#define ASM_60X_DSMISS_VECTOR                0x12
#define ASM_60X_ADDR_VECTOR                  0x13
#define ASM_60X_SYSMGMT_VECTOR               0x14
#define ASM_60X_VEC_ASSIST_VECTOR            0x16
#define ASM_60X_ITM_VECTOR                   0x17

typedef enum
{
  PPC_601 = 0x1,
  PPC_5XX = 0x2,
  PPC_603 = 0x3,
  PPC_604 = 0x4,
  PPC_603e = 0x6,
  PPC_603ev = 0x7,
  PPC_750 = 0x8,
  PPC_750_IBM = 0x7000,
  PPC_604e = 0x9,
  PPC_604r = 0xA,
  PPC_7400 = 0xC,
  PPC_405  = 0x2001,  /* Xilinx Virtex-II Pro or -4 */
  PPC_405EX = 0x1291,   /* + 405EXr */
  PPC_405GP = 0x4011,   /* + 405CR */
  PPC_405GPr = 0x5091,
  PPC_405EZ = 0x4151,
  PPC_405EP = 0x5121,
  PPC_440 = 0x7ff2,  /* Xilinx Virtex-5*/
  PPC_7455 = 0x8001, /* Kate Feng */
  PPC_7457 = 0x8002,
  PPC_620 = 0x16,
  PPC_860 = 0x50,
  PPC_821 = PPC_860,
  PPC_823 = PPC_860,
  PPC_8260 = 0x81,
  PPC_8240 = PPC_8260,
  PPC_8245 = 0x8081,
  PPC_8540 = 0x8020,
  PPC_e500v2 = 0x8021,
  PPC_e6500 = 0x8040,
  PPC_603le = 0x8082, /* 603le core, in MGT5100 and MPC5200 */
  PPC_e300c1  = 0x8083, /* e300c1  core, in MPC83xx*/
  PPC_e300c2  = 0x8084, /* e300c2  core */
  PPC_e300c3  = 0x8085, /* e300c3  core */
  PPC_e200z0  = 0x8170,
  PPC_e200z1  = 0x8140,
  PPC_e200z4  = 0x8150,
  PPC_e200z6  = 0x8110,
  PPC_e200z7  = 0x8160,
  PPC_PSIM    = 0xfffe,  /* GDB PowerPC simulator -- fake version */
  PPC_UNKNOWN = 0xffff
} ppc_cpu_id;

#define PPC_BOOKE_405   1 /* almost like booke but with some significant differences */
#define PPC_BOOKE_STD   2
#define PPC_BOOKE_E500  3 /* bookE with extensions */

static ppc_cpu_id ppc_cpu;
static int is_bookE;

#define xppc_read_spr(reg, val) \
        __asm__ __volatile__("mfspr %0,"#reg : "=r" (val))
#define xppc_write_spr(reg, val) \
        __asm__ __volatile__("mtspr "#reg",%0" : : "r" (val))
#define ppc_read_spr(reg, val) xppc_read_spr(reg, val)
#define ppc_write_spr(reg, val) xppc_write_spr(reg, val)

static const char *ppc_get_cpu_type_name(ppc_cpu_id cpu)
{
  switch (cpu) {
    case PPC_405:     return "PPC405";
    case PPC_405GP:   return "PPC405GP";
    case PPC_405EX:   return "PPC405EX";
    case PPC_440:     return "PPC440";
    case PPC_601:     return "MPC601";
    case PPC_5XX:     return "MPC5XX";
    case PPC_603:     return "MPC603";
    case PPC_603ev:   return "MPC603ev";
    case PPC_604:     return "MPC604";
    case PPC_750:     return "MPC750";
    case PPC_750_IBM: return "IBM PPC750";
    case PPC_7400:    return "MPC7400";
    case PPC_7455:    return "MPC7455";
    case PPC_7457:    return "MPC7457";
    case PPC_603le:   return "MPC603le";
    case PPC_604e:    return "MPC604e";
    case PPC_604r:    return "MPC604r";
    case PPC_620:     return "MPC620";
    case PPC_860:     return "MPC860";
    case PPC_8260:    return "MPC8260";
    case PPC_8245:    return "MPC8245";
    case PPC_8540:    return "MPC8540";
    case PPC_PSIM:    return "PSIM";
    case PPC_e200z0:  return "e200z0";
    case PPC_e200z1:  return "e200z1";
    case PPC_e200z4:  return "e200z4";
    case PPC_e200z6:  return "e200z6";
    case PPC_e200z7:  return "e200z7";
    case PPC_e500v2:  return "e500v2";
    case PPC_e6500:   return "e6500";
    default:
      break;
  }
  return "unknown";
}

static bool ppc_is_bookE(void)
{
  return is_bookE != 0;
}

static int ppc_probe_cpu_type(void)
{
  /*
   * cpu types listed here have the lowermost nibble as a version identifier
   * we will tweak them to the standard version
   */
  const uint32_t ppc_cpu_id_version_nibble[] = {
    PPC_e200z0,
    PPC_e200z1,
    PPC_e200z4,
    PPC_e200z6,
    PPC_e200z7
  };
  #define NUM_CPU_ID_VERSION \
    (sizeof(ppc_cpu_id_version_nibble) / sizeof(ppc_cpu_id_version_nibble[0]))

  uint32_t pvr;
  int i;

  ppc_read_spr(PPC_PVR, pvr);
  pvr >>= 16;

  /*
   * apply tweaks to ignore version
   */
  for (i = 0; i < NUM_CPU_ID_VERSION; ++i) {
    if ((pvr & 0xfff0) == (ppc_cpu_id_version_nibble[i] & 0xfff0)) {
      pvr = ppc_cpu_id_version_nibble[i];
      break;
    }
  }

  ppc_cpu = (ppc_cpu_id) pvr;

  switch (pvr) {
    case PPC_405:
    case PPC_405GP:
    case PPC_405EX:
    case PPC_440:
    case PPC_601:
    case PPC_5XX:
    case PPC_603:
    case PPC_603ev:
    case PPC_603le:
    case PPC_604:
    case PPC_604r:
    case PPC_750:
    case PPC_750_IBM:
    case PPC_7400:
    case PPC_7455:
    case PPC_7457:
    case PPC_604e:
    case PPC_620:
    case PPC_860:
    case PPC_8260:
    case PPC_8245:
    case PPC_PSIM:
    case PPC_8540:
    case PPC_e200z0:
    case PPC_e200z1:
    case PPC_e200z4:
    case PPC_e200z6:
    case PPC_e200z7:
    case PPC_e300c1:
    case PPC_e300c2:
    case PPC_e300c3:
    case PPC_e500v2:
    case PPC_e6500:
      break;
    default:
      rtems_debugger_printf("rtems-db: powerpc: unknown CPU\n");
      return -1;
  }

  switch (ppc_cpu) {
    case PPC_405:
    case PPC_405GP:
    case PPC_405EX:
      is_bookE = PPC_BOOKE_405;
      break;
    case PPC_440:
      is_bookE = PPC_BOOKE_STD;
      break;
    case PPC_8540:
    case PPC_e200z0:
    case PPC_e200z1:
    case PPC_e200z4:
    case PPC_e200z6:
    case PPC_e200z7:
    case PPC_e500v2:
    case PPC_e6500:
      is_bookE = PPC_BOOKE_E500;
      break;
    default:
      break;
  }

  rtems_debugger_printf("rtems-db: powerpc: %s\n",
                        ppc_get_cpu_type_name(ppc_cpu));

  return 0;
}

#ifdef BOOKE_DBSR

#endif

static void ppc_set_dbsr(uint32_t dbsr)
{
  if (ppc_is_bookE()) {
    ppc_write_spr(BOOKE_DBSR, dbsr);
  } else {
    ppc_write_spr(0x3f0, dbsr);
  }
}

static void ppc_set_dbcr0(uint32_t dbcr)
{
  if (ppc_is_bookE()) {
    ppc_write_spr(BOOKE_DBCR0, dbcr);
  } else {
    ppc_write_spr(0x3f2, dbcr);
  }
}

static int ppc_debug_probe(rtems_debugger_target* target)
{
  ppc_set_dbsr(0xffffffffUL);
  ppc_set_dbcr0(0);
  return ppc_probe_cpu_type();
}

int
rtems_debugger_target_configure(rtems_debugger_target* target)
{
  target->capabilities = (RTEMS_DEBUGGER_TARGET_CAP_SWBREAK);
  target->reg_num = RTEMS_DEBUGGER_NUMREGS;
  target->reg_offset = ppc_reg_offsets;
  target->breakpoint = &breakpoint[0];
  target->breakpoint_size = sizeof(breakpoint);
  return ppc_debug_probe(target);;
}

static void
target_exception(CPU_Exception_frame* frame)
{
  target_printk("[} frame = %08" PRIx32 " sig=%d (%" PRIx32 ")\n",
                (uint32_t) frame,
                rtems_debugger_target_exception_to_signal(frame),
                frame->_EXC_number);
#ifndef __SPE__
  target_printk("[} SRR0 = %" PRIx32 " SRR1 = %" PRIx32 "\n",
                frame->EXC_SRR0, frame->EXC_SRR1);
#else
  target_printk("[} SRR0 = %" PRIx32 " SRR1 = %" PRIx32 \
                " SPEFSCR = %" PRIx32 " ACC = %" PRIx32 "\n",
                frame->EXC_SRR0, frame->EXC_SRR1,
                frame->EXC_SPEFSCR, frame->EXC_ACC);
#endif
  target_printk("[} LR = %" PRIx32 " CR = %" PRIx32 \
                " XER = %" PRIx32 " CTR = %" PRIx32 "\n",
                frame->EXC_LR, frame->EXC_CR, frame->EXC_XER, frame->EXC_CTR);
  uintptr_t* gpr = &frame->GPR0;
  int r = 0;
  while (gpr != &frame->GPR31) {
    target_printk("[} GPR%d = %" PRIx32 " GPR%d = %" PRIx32 \
                  " GPR%d = %" PRIx32 " GPR%d = %" PRIx32 "\n",
                  r, *gpr, r + 1, *(gpr + 1), r + 2,
                  *(gpr + 2), r + 3, *(gpr + 3));
    gpr += 4;
    r += 4;
  }

  switch (rtems_debugger_target_exception(frame)) {
  case rtems_debugger_target_exc_consumed:
  default:
    break;
  case rtems_debugger_target_exc_step:
    break;
  case rtems_debugger_target_exc_cascade:
    orig_currentExcHandler(frame);
    break;
  }
}

static bool
rtems_debugger_is_int_reg(size_t reg)
{
  const size_t size = ppc_reg_offsets[reg + 1] - ppc_reg_offsets[reg];
  return size == RTEMS_DEBUGGER_NUMREGBYTES;
}

static void
rtems_debugger_set_int_reg(rtems_debugger_thread* thread,
                           const uintptr_t        reg,
                           const uint32_t         value)
{
  const size_t offset = ppc_reg_offsets[reg];
  /*
   * Use memcpy to avoid alignment issues.
   */
  memcpy(&thread->registers[offset], &value, sizeof(uintptr_t));
}

static const uintptr_t
rtems_debugger_get_int_reg(rtems_debugger_thread* thread, const size_t reg)
{
  const size_t offset = ppc_reg_offsets[reg];
  uintptr_t value;
  memcpy(&value, &thread->registers[offset], sizeof(uintptr_t));
  return value;
}

static void rtems_debugger_set_fp_reg(rtems_debugger_thread* thread,
                                      const size_t           reg,
                                      uint64_t               value)
{
  const size_t offset = ppc_reg_offsets[reg];
  /*
   * Use memcpy to avoid alignment issues.
   */
  memcpy(&thread->registers[offset], &value, sizeof(uint64_t));
}

static uint64_t rtems_debugger_get_fp_reg(rtems_debugger_thread* thread,
                                          const size_t           reg)
{
  const size_t offset = ppc_reg_offsets[reg];
  uint64_t value;
  memcpy(&value, &thread->registers[offset], sizeof(uint64_t));
  return value;
}

int
rtems_debugger_target_enable(void)
{
  rtems_interrupt_lock_context lock_context;
  rtems_interrupt_lock_acquire(&target_lock, &lock_context);
  if (orig_currentExcHandler == NULL) {
    orig_currentExcHandler = globalExceptHdl;
    globalExceptHdl = target_exception;
  }
  rtems_interrupt_lock_release(&target_lock, &lock_context);
  return 0;
}

int
rtems_debugger_target_disable(void)
{
  rtems_interrupt_lock_context lock_context;
  rtems_interrupt_lock_acquire(&target_lock, &lock_context);
  ppc_set_dbsr(0xffffffffUL);
  ppc_set_dbcr0(0);
  if (orig_currentExcHandler != NULL)
    globalExceptHdl = orig_currentExcHandler;
  rtems_interrupt_lock_release(&target_lock, &lock_context);
  return 0;
}

int
rtems_debugger_target_read_regs(rtems_debugger_thread* thread)
{
  if (!rtems_debugger_thread_flag(thread,
                                  RTEMS_DEBUGGER_THREAD_FLAG_REG_VALID)) {
    static const uintptr_t good_address = (uintptr_t) &good_address;
    size_t i;

    for (i = 0; i < rtems_debugger_target_reg_num(); ++i) {
      if (rtems_debugger_is_int_reg(i)) {
        rtems_debugger_set_int_reg(thread, i, (uintptr_t) &good_address);
      }
    }

    if (thread->frame) {
      CPU_Exception_frame* frame = thread->frame;
      rtems_debugger_set_int_reg(thread, REG_R0, frame->GPR0);
      rtems_debugger_set_int_reg(thread, REG_R1, frame->GPR1);
      rtems_debugger_set_int_reg(thread, REG_R2, frame->GPR2);
      rtems_debugger_set_int_reg(thread, REG_R3, frame->GPR3);
      rtems_debugger_set_int_reg(thread, REG_R4, frame->GPR4);
      rtems_debugger_set_int_reg(thread, REG_R5, frame->GPR5);
      rtems_debugger_set_int_reg(thread, REG_R6, frame->GPR6);
      rtems_debugger_set_int_reg(thread, REG_R7, frame->GPR7);
      rtems_debugger_set_int_reg(thread, REG_R8, frame->GPR8);
      rtems_debugger_set_int_reg(thread, REG_R9, frame->GPR9);
      rtems_debugger_set_int_reg(thread, REG_R10, frame->GPR10);
      rtems_debugger_set_int_reg(thread, REG_R11, frame->GPR11);
      rtems_debugger_set_int_reg(thread, REG_R12, frame->GPR12);
      rtems_debugger_set_int_reg(thread, REG_R13, frame->GPR13);
      rtems_debugger_set_int_reg(thread, REG_R14, frame->GPR14);
      rtems_debugger_set_int_reg(thread, REG_R15, frame->GPR15);
      rtems_debugger_set_int_reg(thread, REG_R16, frame->GPR16);
      rtems_debugger_set_int_reg(thread, REG_R17, frame->GPR17);
      rtems_debugger_set_int_reg(thread, REG_R18, frame->GPR18);
      rtems_debugger_set_int_reg(thread, REG_R19, frame->GPR19);
      rtems_debugger_set_int_reg(thread, REG_R20, frame->GPR20);
      rtems_debugger_set_int_reg(thread, REG_R21, frame->GPR21);
      rtems_debugger_set_int_reg(thread, REG_R22, frame->GPR22);
      rtems_debugger_set_int_reg(thread, REG_R23, frame->GPR23);
      rtems_debugger_set_int_reg(thread, REG_R24, frame->GPR24);
      rtems_debugger_set_int_reg(thread, REG_R25, frame->GPR25);
      rtems_debugger_set_int_reg(thread, REG_R26, frame->GPR26);
      rtems_debugger_set_int_reg(thread, REG_R27, frame->GPR27);
      rtems_debugger_set_int_reg(thread, REG_R28, frame->GPR28);
      rtems_debugger_set_int_reg(thread, REG_R29, frame->GPR29);
      rtems_debugger_set_int_reg(thread, REG_R30, frame->GPR30);
      rtems_debugger_set_int_reg(thread, REG_R31, frame->GPR31);

      rtems_debugger_set_int_reg(thread, REG_PC, frame->EXC_SRR0);
      rtems_debugger_set_int_reg(thread, REG_MSR, frame->EXC_SRR1);
      rtems_debugger_set_int_reg(thread, REG_CND, frame->EXC_CR);
      rtems_debugger_set_int_reg(thread, REG_LR, frame->EXC_LR);
      rtems_debugger_set_int_reg(thread, REG_CNT, frame->EXC_CTR);
      rtems_debugger_set_int_reg(thread, REG_XER, frame->EXC_XER);

#if defined(PPC_MULTILIB_FPU)
      rtems_debugger_set_int_reg(thread, REG_FPSCR, frame->FPSCR);
      rtems_debugger_set_fp_reg(thread, REG_F0 + r, frame->F0);
      rtems_debugger_set_fp_reg(thread, REG_F1 + r, frame->F1);
      rtems_debugger_set_fp_reg(thread, REG_F2 + r, frame->F2);
      rtems_debugger_set_fp_reg(thread, REG_F3 + r, frame->F3);
      rtems_debugger_set_fp_reg(thread, REG_F4 + r, frame->F4);
      rtems_debugger_set_fp_reg(thread, REG_F5 + r, frame->F5);
      rtems_debugger_set_fp_reg(thread, REG_F6 + r, frame->F6);
      rtems_debugger_set_fp_reg(thread, REG_F7 + r, frame->F7);
      rtems_debugger_set_fp_reg(thread, REG_F8 + r, frame->F8);
      rtems_debugger_set_fp_reg(thread, REG_F9 + r, frame->F9);
      rtems_debugger_set_fp_reg(thread, REG_F10 + r, frame->F10);
      rtems_debugger_set_fp_reg(thread, REG_F11 + r, frame->F11);
      rtems_debugger_set_fp_reg(thread, REG_F12 + r, frame->F12);
      rtems_debugger_set_fp_reg(thread, REG_F13 + r, frame->F13);
      rtems_debugger_set_fp_reg(thread, REG_F14 + r, frame->F14);
      rtems_debugger_set_fp_reg(thread, REG_F15 + r, frame->F15);
      rtems_debugger_set_fp_reg(thread, REG_F16 + r, frame->F16);
      rtems_debugger_set_fp_reg(thread, REG_F17 + r, frame->F17);
      rtems_debugger_set_fp_reg(thread, REG_F18 + r, frame->F18);
      rtems_debugger_set_fp_reg(thread, REG_F19 + r, frame->F19);
      rtems_debugger_set_fp_reg(thread, REG_F20 + r, frame->F20);
      rtems_debugger_set_fp_reg(thread, REG_F21 + r, frame->F21);
      rtems_debugger_set_fp_reg(thread, REG_F22 + r, frame->F22);
      rtems_debugger_set_fp_reg(thread, REG_F23 + r, frame->F23);
      rtems_debugger_set_fp_reg(thread, REG_F24 + r, frame->F24);
      rtems_debugger_set_fp_reg(thread, REG_F25 + r, frame->F25);
      rtems_debugger_set_fp_reg(thread, REG_F26 + r, frame->F26);
      rtems_debugger_set_fp_reg(thread, REG_F27 + r, frame->F27);
      rtems_debugger_set_fp_reg(thread, REG_F28 + r, frame->F28);
      rtems_debugger_set_fp_reg(thread, REG_F29 + r, frame->F29);
      rtems_debugger_set_fp_reg(thread, REG_F30 + r, frame->F30);
      rtems_debugger_set_fp_reg(thread, REG_F31 + r, frame->F31);
#elif defined(PPC_MULTILIB_ALTIVEC)
      #warning "ALTVEC floats not supported"
#elif defined(__ALTIVEC__)
      #warning "ALTVEC floats not supported"
#endif

      /*
       * Get the signal from the frame.
       */
      thread->signal = rtems_debugger_target_exception_to_signal(frame);
    }
    else {
      ppc_context* thread_ctx = ppc_get_context(&thread->tcb->Registers);
      rtems_debugger_set_int_reg(thread, REG_R1,  thread_ctx->gpr1);
      rtems_debugger_set_int_reg(thread, REG_R14, thread_ctx->gpr14);
      rtems_debugger_set_int_reg(thread, REG_R15, thread_ctx->gpr15);
      rtems_debugger_set_int_reg(thread, REG_R16, thread_ctx->gpr16);
      rtems_debugger_set_int_reg(thread, REG_R17, thread_ctx->gpr17);
      rtems_debugger_set_int_reg(thread, REG_R18, thread_ctx->gpr18);
      rtems_debugger_set_int_reg(thread, REG_R19, thread_ctx->gpr19);
      rtems_debugger_set_int_reg(thread, REG_R20, thread_ctx->gpr20);
      rtems_debugger_set_int_reg(thread, REG_R21, thread_ctx->gpr21);
      rtems_debugger_set_int_reg(thread, REG_R22, thread_ctx->gpr22);
      rtems_debugger_set_int_reg(thread, REG_R23, thread_ctx->gpr23);
      rtems_debugger_set_int_reg(thread, REG_R24, thread_ctx->gpr24);
      rtems_debugger_set_int_reg(thread, REG_R25, thread_ctx->gpr25);
      rtems_debugger_set_int_reg(thread, REG_R26, thread_ctx->gpr26);
      rtems_debugger_set_int_reg(thread, REG_R27, thread_ctx->gpr27);
      rtems_debugger_set_int_reg(thread, REG_R28, thread_ctx->gpr28);
      rtems_debugger_set_int_reg(thread, REG_R29, thread_ctx->gpr29);
      rtems_debugger_set_int_reg(thread, REG_R30, thread_ctx->gpr30);
      rtems_debugger_set_int_reg(thread, REG_R31, thread_ctx->gpr31);

      rtems_debugger_set_int_reg(thread, REG_PC, thread_ctx->lr);
      rtems_debugger_set_int_reg(thread, REG_MSR, thread_ctx->msr);
      rtems_debugger_set_int_reg(thread, REG_CND, thread_ctx->cr);
      rtems_debugger_set_int_reg(thread, REG_LR, thread_ctx->lr);
      rtems_debugger_set_int_reg(thread, REG_CNT, 0);
      rtems_debugger_set_int_reg(thread, REG_XER, 0);
      rtems_debugger_set_int_reg(thread, REG_FPSCR, 0);

      /*
       * Blocked threads have no signal.
       */
      thread->signal = 0;
    }

    thread->flags |= RTEMS_DEBUGGER_THREAD_FLAG_REG_VALID;
    thread->flags &= ~RTEMS_DEBUGGER_THREAD_FLAG_REG_DIRTY;
  }

  return 0;
}

int
rtems_debugger_target_write_regs(rtems_debugger_thread* thread)
{
  if (rtems_debugger_thread_flag(thread,
                                 RTEMS_DEBUGGER_THREAD_FLAG_REG_DIRTY)) {
    /*
     * Only write to debugger controlled threads. Do not touch the registers
     * for threads blocked in the context switcher.
     */
    if (rtems_debugger_thread_flag(thread,
                                   RTEMS_DEBUGGER_THREAD_FLAG_EXCEPTION)) {
      CPU_Exception_frame* frame = thread->frame;
      frame->GPR0 = rtems_debugger_get_int_reg(thread, REG_R0);
      frame->GPR1 = rtems_debugger_get_int_reg(thread, REG_R1);
      frame->GPR2 = rtems_debugger_get_int_reg(thread, REG_R2);
      frame->GPR3 = rtems_debugger_get_int_reg(thread, REG_R3);
      frame->GPR4 = rtems_debugger_get_int_reg(thread, REG_R4);
      frame->GPR5 = rtems_debugger_get_int_reg(thread, REG_R5);
      frame->GPR6 = rtems_debugger_get_int_reg(thread, REG_R6);
      frame->GPR7 = rtems_debugger_get_int_reg(thread, REG_R7);
      frame->GPR8 = rtems_debugger_get_int_reg(thread, REG_R8);
      frame->GPR9 = rtems_debugger_get_int_reg(thread, REG_R9);
      frame->GPR10 = rtems_debugger_get_int_reg(thread, REG_R10);
      frame->GPR11 = rtems_debugger_get_int_reg(thread, REG_R11);
      frame->GPR12 = rtems_debugger_get_int_reg(thread, REG_R12);
      frame->GPR13 = rtems_debugger_get_int_reg(thread, REG_R13);
      frame->GPR14 = rtems_debugger_get_int_reg(thread, REG_R14);
      frame->GPR15 = rtems_debugger_get_int_reg(thread, REG_R15);
      frame->GPR16 = rtems_debugger_get_int_reg(thread, REG_R16);
      frame->GPR17 = rtems_debugger_get_int_reg(thread, REG_R17);
      frame->GPR18 = rtems_debugger_get_int_reg(thread, REG_R18);
      frame->GPR19 = rtems_debugger_get_int_reg(thread, REG_R19);
      frame->GPR20 = rtems_debugger_get_int_reg(thread, REG_R20);
      frame->GPR21 = rtems_debugger_get_int_reg(thread, REG_R21);
      frame->GPR22 = rtems_debugger_get_int_reg(thread, REG_R22);
      frame->GPR23 = rtems_debugger_get_int_reg(thread, REG_R23);
      frame->GPR24 = rtems_debugger_get_int_reg(thread, REG_R24);
      frame->GPR25 = rtems_debugger_get_int_reg(thread, REG_R25);
      frame->GPR26 = rtems_debugger_get_int_reg(thread, REG_R26);
      frame->GPR27 = rtems_debugger_get_int_reg(thread, REG_R27);
      frame->GPR28 = rtems_debugger_get_int_reg(thread, REG_R28);
      frame->GPR29 = rtems_debugger_get_int_reg(thread, REG_R29);
      frame->GPR30 = rtems_debugger_get_int_reg(thread, REG_R30);
      frame->GPR31 = rtems_debugger_get_int_reg(thread, REG_R31);

      frame->EXC_SRR0 = rtems_debugger_get_int_reg(thread, REG_PC);
      frame->EXC_SRR1 = rtems_debugger_get_int_reg(thread, REG_MSR);
      frame->EXC_CR = rtems_debugger_get_int_reg(thread, REG_CND);
      frame->EXC_LR = rtems_debugger_get_int_reg(thread, REG_LR);
      frame->EXC_CTR = rtems_debugger_get_int_reg(thread, REG_CNT);
      frame->EXC_XER = rtems_debugger_get_int_reg(thread, REG_XER);

#ifdef PPC_MULTILIB_FPU
      frame->FPSCR = rtems_debugger_get_int_reg(thread, REG_FPSCR);
      frame->F0 = rtems_debugger_get_fp_reg(thread, REG_F0);
      frame->F1 = rtems_debugger_get_fp_reg(thread, REG_F1);
      frame->F2 = rtems_debugger_get_fp_reg(thread, REG_F2);
      frame->F3 = rtems_debugger_get_fp_reg(thread, REG_F3);
      frame->F4 = rtems_debugger_get_fp_reg(thread, REG_F4);
      frame->F5 = rtems_debugger_get_fp_reg(thread, REG_F5);
      frame->F6 = rtems_debugger_get_fp_reg(thread, REG_F6);
      frame->F7 = rtems_debugger_get_fp_reg(thread, REG_F7);
      frame->F8 = rtems_debugger_get_fp_reg(thread, REG_F8);
      frame->F9 = rtems_debugger_get_fp_reg(thread, REG_F9);
      frame->F10 = rtems_debugger_get_fp_reg(thread, REG_F10);
      frame->F11 = rtems_debugger_get_fp_reg(thread, REG_F11);
      frame->F12 = rtems_debugger_get_fp_reg(thread, REG_F12);
      frame->F13 = rtems_debugger_get_fp_reg(thread, REG_F13);
      frame->F14 = rtems_debugger_get_fp_reg(thread, REG_F14);
      frame->F15 = rtems_debugger_get_fp_reg(thread, REG_F15);
      frame->F16 = rtems_debugger_get_fp_reg(thread, REG_F16);
      frame->F17 = rtems_debugger_get_fp_reg(thread, REG_F17);
      frame->F18 = rtems_debugger_get_fp_reg(thread, REG_F18);
      frame->F19 = rtems_debugger_get_fp_reg(thread, REG_F19);
      frame->F20 = rtems_debugger_get_fp_reg(thread, REG_F20);
      frame->F21 = rtems_debugger_get_fp_reg(thread, REG_F21);
      frame->F22 = rtems_debugger_get_fp_reg(thread, REG_F22);
      frame->F23 = rtems_debugger_get_fp_reg(thread, REG_F23);
      frame->F24 = rtems_debugger_get_fp_reg(thread, REG_F24);
      frame->F25 = rtems_debugger_get_fp_reg(thread, REG_F25);
      frame->F26 = rtems_debugger_get_fp_reg(thread, REG_F26);
      frame->F27 = rtems_debugger_get_fp_reg(thread, REG_F27);
      frame->F28 = rtems_debugger_get_fp_reg(thread, REG_F28);
      frame->F29 = rtems_debugger_get_fp_reg(thread, REG_F29);
      frame->F30 = rtems_debugger_get_fp_reg(thread, REG_F30);
      frame->F31 = rtems_debugger_get_fp_reg(thread, REG_F31);
#endif
    }
    thread->flags &= ~RTEMS_DEBUGGER_THREAD_FLAG_REG_DIRTY;
  }
  return 0;
}

uintptr_t
rtems_debugger_target_reg_pc(rtems_debugger_thread* thread)
{
  int r;
  r = rtems_debugger_target_read_regs(thread);
  if (r >= 0) {
    return rtems_debugger_get_int_reg(thread, REG_PC);
  }
  return 0;
}

uintptr_t
rtems_debugger_target_frame_pc(CPU_Exception_frame* frame)
{
  return (uintptr_t) frame->EXC_SRR0;
}

uintptr_t
rtems_debugger_target_reg_sp(rtems_debugger_thread* thread)
{
  int r;
  r = rtems_debugger_target_read_regs(thread);
  if (r >= 0) {
    return rtems_debugger_get_int_reg(thread, REG_R1);
  }
  return 0;
}

uintptr_t
rtems_debugger_target_tcb_sp(rtems_debugger_thread* thread)
{
  ppc_context* thread_ctx = ppc_get_context(&thread->tcb->Registers);
  return (DB_UINT) thread_ctx->gpr1;
}

int
rtems_debugger_target_thread_stepping(rtems_debugger_thread* thread)
{
  if (rtems_debugger_thread_flag(thread,
                                 (RTEMS_DEBUGGER_THREAD_FLAG_STEP |
                                  RTEMS_DEBUGGER_THREAD_FLAG_STEPPING))) {
    //CPU_Exception_frame* frame = thread->frame;
    /*
     * Single step instructions with interrupts masked to avoid stepping into
     * an interrupt handler.
     */
#if 0
    if ((frame->eflags & EFLAGS_INTR_ENABLE) == 0)
      thread->flags |= RTEMS_DEBUGGER_THREAD_FLAG_INTS_DISABLED;
    else
      frame->eflags &= ~EFLAGS_INTR_ENABLE;
#endif
  }
  return 0;
}

int
rtems_debugger_target_exception_to_signal(CPU_Exception_frame* frame)
{
  int sig = RTEMS_DEBUGGER_SIGNAL_HUP;
  switch (frame->_EXC_number) {
  case ASM_MACH_VECTOR:
    sig = RTEMS_DEBUGGER_SIGNAL_BUS;
    break;
  case ASM_PROT_VECTOR:
  case ASM_ISI_VECTOR:
  case ASM_ALIGN_VECTOR:
  case ASM_60X_IMISS_VECTOR:
  case ASM_60X_DLMISS_VECTOR:
  case ASM_60X_DSMISS_VECTOR:
    sig = RTEMS_DEBUGGER_SIGNAL_SEGV;
    break;
  case ASM_PROG_VECTOR:
    sig = RTEMS_DEBUGGER_SIGNAL_TRAP;
    break;
  case ASM_FLOAT_VECTOR:
    sig = RTEMS_DEBUGGER_SIGNAL_FPE;
    break;
  case ASM_DEC_VECTOR:
    sig = RTEMS_DEBUGGER_SIGNAL_ALRM;
    break;
  case ASM_SYS_VECTOR:
    sig = RTEMS_DEBUGGER_SIGNAL_SYS;
    break;
  case ASM_TRACE_VECTOR:
    sig = RTEMS_DEBUGGER_SIGNAL_TRAP;
  default:
    break;
  }
  return sig;
}

void
rtems_debugger_target_exception_print(CPU_Exception_frame* frame)
{
#ifndef __SPE__
  rtems_debugger_printf(" SRR0 = %" PRIx32 " SRR1 = %" PRIx32 "\n",
                        frame->EXC_SRR0, frame->EXC_SRR1);
#else
  rtems_debugger_printf(" SRR0 = %" PRIx32 " SRR1 = %" PRIx32 \
                        " SPEFSCR = %" PRIx32 " ACC = %" PRIx32 "\n",
                        frame->EXC_SRR0, frame->EXC_SRR1,
                        frame->EXC_SPEFSCR, frame->EXC_ACC);
#endif
  rtems_debugger_printf(" LR = %" PRIx32 " CR = %" PRIx32 \
                        " XER = %" PRIx32 " CTR = %" PRIx32 "\n",
                        frame->EXC_LR, frame->EXC_CR,
                        frame->EXC_XER, frame->EXC_CTR);
  uintptr_t* gpr = &frame->GPR0;
  int r = 0;
  while (gpr != &frame->GPR31) {
    rtems_debugger_printf(" GPR%d = %" PRIx32 " GPR%d = %" PRIx32 \
                  " GPR%d = %" PRIx32 " GPR%d = %" PRIx32 "\n",
                  r, *gpr, r + 1, *(gpr + 1), r + 2,
                  *(gpr + 2), r + 3, *(gpr + 3));
    gpr += 4;
    r += 4;
  }
}

int
rtems_debugger_target_hwbreak_insert(void)
{
  /*
   * Do nothing, load on exit of the exception handler.
   */
  return 0;
}

int
rtems_debugger_target_hwbreak_remove(void)
{
  return 0;
}

int
rtems_debugger_target_hwbreak_control(rtems_debugger_target_watchpoint wp,
                                      bool                             insert,
                                      uintptr_t                        addr,
                                      DB_UINT                          kind)
{
  /*
   * To do.
   */
  return 0;
}

int
rtems_debugger_target_cache_sync(rtems_debugger_target_swbreak* swbreak)
{
  /*
   * Flush the data cache and invalidate the instruction cache.
   */
  rtems_cache_flush_multiple_data_lines(
    swbreak->address,
    sizeof( breakpoint )
  );
  rtems_cache_instruction_sync_after_code_change(
    swbreak->address,
    sizeof( breakpoint )
  );
  return 0;
}