[openocd.git] / src / target / riscv / riscv.c

/* SPDX-License-Identifier: GPL-2.0-or-later */

#include <assert.h>
#include <stdlib.h>
#include <time.h>

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

#include <helper/log.h>
#include <helper/time_support.h>
#include "target/target.h"
#include "target/algorithm.h"
#include "target/target_type.h"
#include "jtag/jtag.h"
#include "target/register.h"
#include "target/breakpoints.h"
#include "riscv.h"
#include "gdb_regs.h"
#include "rtos/rtos.h"
#include "debug_defines.h"
#include <helper/bits.h>

#define get_field(reg, mask) (((reg) & (mask)) / ((mask) & ~((mask) << 1)))
#define set_field(reg, mask, val) (((reg) & ~(mask)) | (((val) * ((mask) & ~((mask) << 1))) & (mask)))

/* Constants for legacy SiFive hardware breakpoints. */
#define CSR_BPCONTROL_X                 (1<<0)
#define CSR_BPCONTROL_W                 (1<<1)
#define CSR_BPCONTROL_R                 (1<<2)
#define CSR_BPCONTROL_U                 (1<<3)
#define CSR_BPCONTROL_S                 (1<<4)
#define CSR_BPCONTROL_H                 (1<<5)
#define CSR_BPCONTROL_M                 (1<<6)
#define CSR_BPCONTROL_BPMATCH   (0xf<<7)
#define CSR_BPCONTROL_BPACTION  (0xff<<11)

#define DEBUG_ROM_START         0x800
#define DEBUG_ROM_RESUME        (DEBUG_ROM_START + 4)
#define DEBUG_ROM_EXCEPTION     (DEBUG_ROM_START + 8)
#define DEBUG_RAM_START         0x400

#define SETHALTNOT                              0x10c

/*** JTAG registers. ***/

#define DTMCONTROL                                      0x10
#define DTMCONTROL_DBUS_RESET           (1<<16)
#define DTMCONTROL_IDLE                         (7<<10)
#define DTMCONTROL_ADDRBITS                     (0xf<<4)
#define DTMCONTROL_VERSION                      (0xf)

#define DBUS                                            0x11
#define DBUS_OP_START                           0
#define DBUS_OP_SIZE                            2
typedef enum {
        DBUS_OP_NOP = 0,
        DBUS_OP_READ = 1,
        DBUS_OP_WRITE = 2
} dbus_op_t;
typedef enum {
        DBUS_STATUS_SUCCESS = 0,
        DBUS_STATUS_FAILED = 2,
        DBUS_STATUS_BUSY = 3
} dbus_status_t;
#define DBUS_DATA_START                         2
#define DBUS_DATA_SIZE                          34
#define DBUS_ADDRESS_START                      36

typedef enum slot {
        SLOT0,
        SLOT1,
        SLOT_LAST,
} slot_t;

/*** Debug Bus registers. ***/

#define DMCONTROL                               0x10
#define DMCONTROL_INTERRUPT             (((uint64_t)1)<<33)
#define DMCONTROL_HALTNOT               (((uint64_t)1)<<32)
#define DMCONTROL_BUSERROR              (7<<19)
#define DMCONTROL_SERIAL                (3<<16)
#define DMCONTROL_AUTOINCREMENT (1<<15)
#define DMCONTROL_ACCESS                (7<<12)
#define DMCONTROL_HARTID                (0x3ff<<2)
#define DMCONTROL_NDRESET               (1<<1)
#define DMCONTROL_FULLRESET             1

#define DMINFO                                  0x11
#define DMINFO_ABUSSIZE                 (0x7fU<<25)
#define DMINFO_SERIALCOUNT              (0xf<<21)
#define DMINFO_ACCESS128                (1<<20)
#define DMINFO_ACCESS64                 (1<<19)
#define DMINFO_ACCESS32                 (1<<18)
#define DMINFO_ACCESS16                 (1<<17)
#define DMINFO_ACCESS8                  (1<<16)
#define DMINFO_DRAMSIZE                 (0x3f<<10)
#define DMINFO_AUTHENTICATED    (1<<5)
#define DMINFO_AUTHBUSY                 (1<<4)
#define DMINFO_AUTHTYPE                 (3<<2)
#define DMINFO_VERSION                  3

/*** Info about the core being debugged. ***/

#define DBUS_ADDRESS_UNKNOWN    0xffff

#define MAX_HWBPS                       16
#define DRAM_CACHE_SIZE         16

uint8_t ir_dtmcontrol[4] = {DTMCONTROL};
struct scan_field select_dtmcontrol = {
        .in_value = NULL,
        .out_value = ir_dtmcontrol
};
uint8_t ir_dbus[4] = {DBUS};
struct scan_field select_dbus = {
        .in_value = NULL,
        .out_value = ir_dbus
};
uint8_t ir_idcode[4] = {0x1};
struct scan_field select_idcode = {
        .in_value = NULL,
        .out_value = ir_idcode
};

bscan_tunnel_type_t bscan_tunnel_type;
int bscan_tunnel_ir_width; /* if zero, then tunneling is not present/active */

static const uint8_t bscan_zero[4] = {0};
static const uint8_t bscan_one[4] = {1};

uint8_t ir_user4[4];
struct scan_field select_user4 = {
        .in_value = NULL,
        .out_value = ir_user4
};


uint8_t bscan_tunneled_ir_width[4] = {5};  /* overridden by assignment in riscv_init_target */
struct scan_field _bscan_tunnel_data_register_select_dmi[] = {
                {
                        .num_bits = 3,
                        .out_value = bscan_zero,
                        .in_value = NULL,
                },
                {
                        .num_bits = 5, /* initialized in riscv_init_target to ir width of DM */
                        .out_value = ir_dbus,
                        .in_value = NULL,
                },
                {
                        .num_bits = 7,
                        .out_value = bscan_tunneled_ir_width,
                        .in_value = NULL,
                },
                {
                        .num_bits = 1,
                        .out_value = bscan_zero,
                        .in_value = NULL,
                }
};

struct scan_field _bscan_tunnel_nested_tap_select_dmi[] = {
                {
                        .num_bits = 1,
                        .out_value = bscan_zero,
                        .in_value = NULL,
                },
                {
                        .num_bits = 7,
                        .out_value = bscan_tunneled_ir_width,
                        .in_value = NULL,
                },
                {
                        .num_bits = 0, /* initialized in riscv_init_target to ir width of DM */
                        .out_value = ir_dbus,
                        .in_value = NULL,
                },
                {
                        .num_bits = 3,
                        .out_value = bscan_zero,
                        .in_value = NULL,
                }
};
struct scan_field *bscan_tunnel_nested_tap_select_dmi = _bscan_tunnel_nested_tap_select_dmi;
uint32_t bscan_tunnel_nested_tap_select_dmi_num_fields = ARRAY_SIZE(_bscan_tunnel_nested_tap_select_dmi);

struct scan_field *bscan_tunnel_data_register_select_dmi = _bscan_tunnel_data_register_select_dmi;
uint32_t bscan_tunnel_data_register_select_dmi_num_fields = ARRAY_SIZE(_bscan_tunnel_data_register_select_dmi);

struct trigger {
        uint64_t address;
        uint32_t length;
        uint64_t mask;
        uint64_t value;
        bool read, write, execute;
        int unique_id;
};

/* Wall-clock timeout for a command/access. Settable via RISC-V Target commands.*/
int riscv_command_timeout_sec = DEFAULT_COMMAND_TIMEOUT_SEC;

/* Wall-clock timeout after reset. Settable via RISC-V Target commands.*/
int riscv_reset_timeout_sec = DEFAULT_RESET_TIMEOUT_SEC;

bool riscv_enable_virt2phys = true;
bool riscv_ebreakm = true;
bool riscv_ebreaks = true;
bool riscv_ebreaku = true;

bool riscv_enable_virtual;

static enum {
        RO_NORMAL,
        RO_REVERSED
} resume_order;

const virt2phys_info_t sv32 = {
        .name = "Sv32",
        .va_bits = 32,
        .level = 2,
        .pte_shift = 2,
        .vpn_shift = {12, 22},
        .vpn_mask = {0x3ff, 0x3ff},
        .pte_ppn_shift = {10, 20},
        .pte_ppn_mask = {0x3ff, 0xfff},
        .pa_ppn_shift = {12, 22},
        .pa_ppn_mask = {0x3ff, 0xfff},
};

const virt2phys_info_t sv39 = {
        .name = "Sv39",
        .va_bits = 39,
        .level = 3,
        .pte_shift = 3,
        .vpn_shift = {12, 21, 30},
        .vpn_mask = {0x1ff, 0x1ff, 0x1ff},
        .pte_ppn_shift = {10, 19, 28},
        .pte_ppn_mask = {0x1ff, 0x1ff, 0x3ffffff},
        .pa_ppn_shift = {12, 21, 30},
        .pa_ppn_mask = {0x1ff, 0x1ff, 0x3ffffff},
};

const virt2phys_info_t sv48 = {
        .name = "Sv48",
        .va_bits = 48,
        .level = 4,
        .pte_shift = 3,
        .vpn_shift = {12, 21, 30, 39},
        .vpn_mask = {0x1ff, 0x1ff, 0x1ff, 0x1ff},
        .pte_ppn_shift = {10, 19, 28, 37},
        .pte_ppn_mask = {0x1ff, 0x1ff, 0x1ff, 0x1ffff},
        .pa_ppn_shift = {12, 21, 30, 39},
        .pa_ppn_mask = {0x1ff, 0x1ff, 0x1ff, 0x1ffff},
};

void riscv_sample_buf_maybe_add_timestamp(struct target *target, bool before)
{
        RISCV_INFO(r);
        uint32_t now = timeval_ms() & 0xffffffff;
        if (r->sample_buf.used + 5 < r->sample_buf.size) {
                if (before)
                        r->sample_buf.buf[r->sample_buf.used++] = RISCV_SAMPLE_BUF_TIMESTAMP_BEFORE;
                else
                        r->sample_buf.buf[r->sample_buf.used++] = RISCV_SAMPLE_BUF_TIMESTAMP_AFTER;
                r->sample_buf.buf[r->sample_buf.used++] = now & 0xff;
                r->sample_buf.buf[r->sample_buf.used++] = (now >> 8) & 0xff;
                r->sample_buf.buf[r->sample_buf.used++] = (now >> 16) & 0xff;
                r->sample_buf.buf[r->sample_buf.used++] = (now >> 24) & 0xff;
        }
}

static int riscv_resume_go_all_harts(struct target *target);

void select_dmi_via_bscan(struct target *target)
{
        jtag_add_ir_scan(target->tap, &select_user4, TAP_IDLE);
        if (bscan_tunnel_type == BSCAN_TUNNEL_DATA_REGISTER)
                jtag_add_dr_scan(target->tap, bscan_tunnel_data_register_select_dmi_num_fields,
                                                                                bscan_tunnel_data_register_select_dmi, TAP_IDLE);
        else /* BSCAN_TUNNEL_NESTED_TAP */
                jtag_add_dr_scan(target->tap, bscan_tunnel_nested_tap_select_dmi_num_fields,
                                                                                bscan_tunnel_nested_tap_select_dmi, TAP_IDLE);
}

uint32_t dtmcontrol_scan_via_bscan(struct target *target, uint32_t out)
{
        /* On BSCAN TAP: Select IR=USER4, issue tunneled IR scan via BSCAN TAP's DR */
        uint8_t tunneled_ir_width[4] = {bscan_tunnel_ir_width};
        uint8_t tunneled_dr_width[4] = {32};
        uint8_t out_value[5] = {0};
        uint8_t in_value[5] = {0};

        buf_set_u32(out_value, 0, 32, out);
        struct scan_field tunneled_ir[4] = {};
        struct scan_field tunneled_dr[4] = {};

        if (bscan_tunnel_type == BSCAN_TUNNEL_DATA_REGISTER) {
                tunneled_ir[0].num_bits = 3;
                tunneled_ir[0].out_value = bscan_zero;
                tunneled_ir[0].in_value = NULL;
                tunneled_ir[1].num_bits = bscan_tunnel_ir_width;
                tunneled_ir[1].out_value = ir_dtmcontrol;
                tunneled_ir[1].in_value = NULL;
                tunneled_ir[2].num_bits = 7;
                tunneled_ir[2].out_value = tunneled_ir_width;
                tunneled_ir[2].in_value = NULL;
                tunneled_ir[3].num_bits = 1;
                tunneled_ir[3].out_value = bscan_zero;
                tunneled_ir[3].in_value = NULL;

                tunneled_dr[0].num_bits = 3;
                tunneled_dr[0].out_value = bscan_zero;
                tunneled_dr[0].in_value = NULL;
                tunneled_dr[1].num_bits = 32 + 1;
                tunneled_dr[1].out_value = out_value;
                tunneled_dr[1].in_value = in_value;
                tunneled_dr[2].num_bits = 7;
                tunneled_dr[2].out_value = tunneled_dr_width;
                tunneled_dr[2].in_value = NULL;
                tunneled_dr[3].num_bits = 1;
                tunneled_dr[3].out_value = bscan_one;
                tunneled_dr[3].in_value = NULL;
        } else {
                /* BSCAN_TUNNEL_NESTED_TAP */
                tunneled_ir[3].num_bits = 3;
                tunneled_ir[3].out_value = bscan_zero;
                tunneled_ir[3].in_value = NULL;
                tunneled_ir[2].num_bits = bscan_tunnel_ir_width;
                tunneled_ir[2].out_value = ir_dtmcontrol;
                tunneled_ir[1].in_value = NULL;
                tunneled_ir[1].num_bits = 7;
                tunneled_ir[1].out_value = tunneled_ir_width;
                tunneled_ir[2].in_value = NULL;
                tunneled_ir[0].num_bits = 1;
                tunneled_ir[0].out_value = bscan_zero;
                tunneled_ir[0].in_value = NULL;

                tunneled_dr[3].num_bits = 3;
                tunneled_dr[3].out_value = bscan_zero;
                tunneled_dr[3].in_value = NULL;
                tunneled_dr[2].num_bits = 32 + 1;
                tunneled_dr[2].out_value = out_value;
                tunneled_dr[2].in_value = in_value;
                tunneled_dr[1].num_bits = 7;
                tunneled_dr[1].out_value = tunneled_dr_width;
                tunneled_dr[1].in_value = NULL;
                tunneled_dr[0].num_bits = 1;
                tunneled_dr[0].out_value = bscan_one;
                tunneled_dr[0].in_value = NULL;
        }
        jtag_add_ir_scan(target->tap, &select_user4, TAP_IDLE);
        jtag_add_dr_scan(target->tap, ARRAY_SIZE(tunneled_ir), tunneled_ir, TAP_IDLE);
        jtag_add_dr_scan(target->tap, ARRAY_SIZE(tunneled_dr), tunneled_dr, TAP_IDLE);
        select_dmi_via_bscan(target);

        int retval = jtag_execute_queue();
        if (retval != ERROR_OK) {
                LOG_ERROR("failed jtag scan: %d", retval);
                return retval;
        }
        /* Note the starting offset is bit 1, not bit 0.  In BSCAN tunnel, there is a one-bit TCK skew between
           output and input */
        uint32_t in = buf_get_u32(in_value, 1, 32);
        LOG_DEBUG("DTMCS: 0x%x -> 0x%x", out, in);

        return in;
}

static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
{
        struct scan_field field;
        uint8_t in_value[4];
        uint8_t out_value[4] = { 0 };

        if (bscan_tunnel_ir_width != 0)
                return dtmcontrol_scan_via_bscan(target, out);


        buf_set_u32(out_value, 0, 32, out);

        jtag_add_ir_scan(target->tap, &select_dtmcontrol, TAP_IDLE);

        field.num_bits = 32;
        field.out_value = out_value;
        field.in_value = in_value;
        jtag_add_dr_scan(target->tap, 1, &field, TAP_IDLE);

        /* Always return to dbus. */
        jtag_add_ir_scan(target->tap, &select_dbus, TAP_IDLE);

        int retval = jtag_execute_queue();
        if (retval != ERROR_OK) {
                LOG_ERROR("failed jtag scan: %d", retval);
                return retval;
        }

        uint32_t in = buf_get_u32(field.in_value, 0, 32);
        LOG_DEBUG("DTMCONTROL: 0x%x -> 0x%x", out, in);

        return in;
}

static struct target_type *get_target_type(struct target *target)
{
        riscv_info_t *info = (riscv_info_t *) target->arch_info;

        if (!info) {
                LOG_ERROR("Target has not been initialized");
                return NULL;
        }

        switch (info->dtm_version) {
                case 0:
                        return &riscv011_target;
                case 1:
                        return &riscv013_target;
                default:
                        LOG_ERROR("Unsupported DTM version: %d", info->dtm_version);
                        return NULL;
        }
}

static int riscv_create_target(struct target *target, Jim_Interp *interp)
{
        LOG_DEBUG("riscv_create_target()");
        target->arch_info = calloc(1, sizeof(riscv_info_t));
        if (!target->arch_info) {
                LOG_ERROR("Failed to allocate RISC-V target structure.");
                return ERROR_FAIL;
        }
        riscv_info_init(target, target->arch_info);
        return ERROR_OK;
}

static int riscv_init_target(struct command_context *cmd_ctx,
                struct target *target)
{
        LOG_DEBUG("riscv_init_target()");
        RISCV_INFO(info);
        info->cmd_ctx = cmd_ctx;

        select_dtmcontrol.num_bits = target->tap->ir_length;
        select_dbus.num_bits = target->tap->ir_length;
        select_idcode.num_bits = target->tap->ir_length;

        if (bscan_tunnel_ir_width != 0) {
                assert(target->tap->ir_length >= 6);
                uint32_t ir_user4_raw = 0x23 << (target->tap->ir_length - 6);
                ir_user4[0] = (uint8_t)ir_user4_raw;
                ir_user4[1] = (uint8_t)(ir_user4_raw >>= 8);
                ir_user4[2] = (uint8_t)(ir_user4_raw >>= 8);
                ir_user4[3] = (uint8_t)(ir_user4_raw >>= 8);
                select_user4.num_bits = target->tap->ir_length;
                bscan_tunneled_ir_width[0] = bscan_tunnel_ir_width;
                if (bscan_tunnel_type == BSCAN_TUNNEL_DATA_REGISTER)
                        bscan_tunnel_data_register_select_dmi[1].num_bits = bscan_tunnel_ir_width;
                else /* BSCAN_TUNNEL_NESTED_TAP */
                        bscan_tunnel_nested_tap_select_dmi[2].num_bits = bscan_tunnel_ir_width;
        }

        riscv_semihosting_init(target);

        target->debug_reason = DBG_REASON_DBGRQ;

        return ERROR_OK;
}

static void riscv_free_registers(struct target *target)
{
        /* Free the shared structure use for most registers. */
        if (target->reg_cache) {
                if (target->reg_cache->reg_list) {
                        free(target->reg_cache->reg_list[0].arch_info);
                        /* Free the ones we allocated separately. */
                        for (unsigned i = GDB_REGNO_COUNT; i < target->reg_cache->num_regs; i++)
                                free(target->reg_cache->reg_list[i].arch_info);
                        free(target->reg_cache->reg_list);
                }
                free(target->reg_cache);
        }
}

static void riscv_deinit_target(struct target *target)
{
        LOG_DEBUG("riscv_deinit_target()");

        riscv_info_t *info = target->arch_info;
        struct target_type *tt = get_target_type(target);

        if (tt && info->version_specific)
                tt->deinit_target(target);

        riscv_free_registers(target);

        range_list_t *entry, *tmp;
        list_for_each_entry_safe(entry, tmp, &info->expose_csr, list) {
                free(entry->name);
                free(entry);
        }

        list_for_each_entry_safe(entry, tmp, &info->expose_custom, list) {
                free(entry->name);
                free(entry);
        }

        free(info->reg_names);
        free(target->arch_info);

        target->arch_info = NULL;
}

static void trigger_from_breakpoint(struct trigger *trigger,
                const struct breakpoint *breakpoint)
{
        trigger->address = breakpoint->address;
        trigger->length = breakpoint->length;
        trigger->mask = ~0LL;
        trigger->read = false;
        trigger->write = false;
        trigger->execute = true;
        /* unique_id is unique across both breakpoints and watchpoints. */
        trigger->unique_id = breakpoint->unique_id;
}

static int maybe_add_trigger_t1(struct target *target,
                struct trigger *trigger, uint64_t tdata1)
{
        RISCV_INFO(r);

        const uint32_t bpcontrol_x = 1<<0;
        const uint32_t bpcontrol_w = 1<<1;
        const uint32_t bpcontrol_r = 1<<2;
        const uint32_t bpcontrol_u = 1<<3;
        const uint32_t bpcontrol_s = 1<<4;
        const uint32_t bpcontrol_h = 1<<5;
        const uint32_t bpcontrol_m = 1<<6;
        const uint32_t bpcontrol_bpmatch = 0xf << 7;
        const uint32_t bpcontrol_bpaction = 0xff << 11;

        if (tdata1 & (bpcontrol_r | bpcontrol_w | bpcontrol_x)) {
                /* Trigger is already in use, presumably by user code. */
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }

        tdata1 = set_field(tdata1, bpcontrol_r, trigger->read);
        tdata1 = set_field(tdata1, bpcontrol_w, trigger->write);
        tdata1 = set_field(tdata1, bpcontrol_x, trigger->execute);
        tdata1 = set_field(tdata1, bpcontrol_u,
                        !!(r->misa & BIT('U' - 'A')));
        tdata1 = set_field(tdata1, bpcontrol_s,
                        !!(r->misa & BIT('S' - 'A')));
        tdata1 = set_field(tdata1, bpcontrol_h,
                        !!(r->misa & BIT('H' - 'A')));
        tdata1 |= bpcontrol_m;
        tdata1 = set_field(tdata1, bpcontrol_bpmatch, 0); /* exact match */
        tdata1 = set_field(tdata1, bpcontrol_bpaction, 0); /* cause bp exception */

        riscv_set_register(target, GDB_REGNO_TDATA1, tdata1);

        riscv_reg_t tdata1_rb;
        if (riscv_get_register(target, &tdata1_rb, GDB_REGNO_TDATA1) != ERROR_OK)
                return ERROR_FAIL;
        LOG_DEBUG("tdata1=0x%" PRIx64, tdata1_rb);

        if (tdata1 != tdata1_rb) {
                LOG_DEBUG("Trigger doesn't support what we need; After writing 0x%"
                                PRIx64 " to tdata1 it contains 0x%" PRIx64,
                                tdata1, tdata1_rb);
                riscv_set_register(target, GDB_REGNO_TDATA1, 0);
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }

        riscv_set_register(target, GDB_REGNO_TDATA2, trigger->address);

        return ERROR_OK;
}

static int maybe_add_trigger_t2(struct target *target,
                struct trigger *trigger, uint64_t tdata1)
{
        RISCV_INFO(r);

        /* tselect is already set */
        if (tdata1 & (MCONTROL_EXECUTE | MCONTROL_STORE | MCONTROL_LOAD)) {
                /* Trigger is already in use, presumably by user code. */
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }

        /* address/data match trigger */
        tdata1 |= MCONTROL_DMODE(riscv_xlen(target));
        tdata1 = set_field(tdata1, MCONTROL_ACTION,
                        MCONTROL_ACTION_DEBUG_MODE);
        tdata1 = set_field(tdata1, MCONTROL_MATCH, MCONTROL_MATCH_EQUAL);
        tdata1 |= MCONTROL_M;
        if (r->misa & (1 << ('S' - 'A')))
                tdata1 |= MCONTROL_S;
        if (r->misa & (1 << ('U' - 'A')))
                tdata1 |= MCONTROL_U;

        if (trigger->execute)
                tdata1 |= MCONTROL_EXECUTE;
        if (trigger->read)
                tdata1 |= MCONTROL_LOAD;
        if (trigger->write)
                tdata1 |= MCONTROL_STORE;

        riscv_set_register(target, GDB_REGNO_TDATA1, tdata1);

        uint64_t tdata1_rb;
        int result = riscv_get_register(target, &tdata1_rb, GDB_REGNO_TDATA1);
        if (result != ERROR_OK)
                return result;
        LOG_DEBUG("tdata1=0x%" PRIx64, tdata1_rb);

        if (tdata1 != tdata1_rb) {
                LOG_DEBUG("Trigger doesn't support what we need; After writing 0x%"
                                PRIx64 " to tdata1 it contains 0x%" PRIx64,
                                tdata1, tdata1_rb);
                riscv_set_register(target, GDB_REGNO_TDATA1, 0);
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }

        riscv_set_register(target, GDB_REGNO_TDATA2, trigger->address);

        return ERROR_OK;
}

static int maybe_add_trigger_t6(struct target *target,
                struct trigger *trigger, uint64_t tdata1)
{
        RISCV_INFO(r);

        /* tselect is already set */
        if (tdata1 & (CSR_MCONTROL6_EXECUTE | CSR_MCONTROL6_STORE | CSR_MCONTROL6_LOAD)) {
                /* Trigger is already in use, presumably by user code. */
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }

        /* address/data match trigger */
        tdata1 |= MCONTROL_DMODE(riscv_xlen(target));
        tdata1 = set_field(tdata1, CSR_MCONTROL6_ACTION,
                        MCONTROL_ACTION_DEBUG_MODE);
        tdata1 = set_field(tdata1, CSR_MCONTROL6_MATCH, MCONTROL_MATCH_EQUAL);
        tdata1 |= CSR_MCONTROL6_M;
        if (r->misa & (1 << ('H' - 'A')))
                tdata1 |= CSR_MCONTROL6_VS | CSR_MCONTROL6_VU;
        if (r->misa & (1 << ('S' - 'A')))
                tdata1 |= CSR_MCONTROL6_S;
        if (r->misa & (1 << ('U' - 'A')))
                tdata1 |= CSR_MCONTROL6_U;

        if (trigger->execute)
                tdata1 |= CSR_MCONTROL6_EXECUTE;
        if (trigger->read)
                tdata1 |= CSR_MCONTROL6_LOAD;
        if (trigger->write)
                tdata1 |= CSR_MCONTROL6_STORE;

        riscv_set_register(target, GDB_REGNO_TDATA1, tdata1);

        uint64_t tdata1_rb;
        int result = riscv_get_register(target, &tdata1_rb, GDB_REGNO_TDATA1);
        if (result != ERROR_OK)
                return result;
        LOG_DEBUG("tdata1=0x%" PRIx64, tdata1_rb);

        if (tdata1 != tdata1_rb) {
                LOG_DEBUG("Trigger doesn't support what we need; After writing 0x%"
                                PRIx64 " to tdata1 it contains 0x%" PRIx64,
                                tdata1, tdata1_rb);
                riscv_set_register(target, GDB_REGNO_TDATA1, 0);
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }

        riscv_set_register(target, GDB_REGNO_TDATA2, trigger->address);

        return ERROR_OK;
}

static int add_trigger(struct target *target, struct trigger *trigger)
{
        RISCV_INFO(r);

        if (riscv_enumerate_triggers(target) != ERROR_OK)
                return ERROR_FAIL;

        riscv_reg_t tselect;
        if (riscv_get_register(target, &tselect, GDB_REGNO_TSELECT) != ERROR_OK)
                return ERROR_FAIL;

        unsigned int i;
        for (i = 0; i < r->trigger_count; i++) {
                if (r->trigger_unique_id[i] != -1)
                        continue;

                riscv_set_register(target, GDB_REGNO_TSELECT, i);

                uint64_t tdata1;
                int result = riscv_get_register(target, &tdata1, GDB_REGNO_TDATA1);
                if (result != ERROR_OK)
                        return result;
                int type = get_field(tdata1, MCONTROL_TYPE(riscv_xlen(target)));

                result = ERROR_OK;
                switch (type) {
                        case 1:
                                result = maybe_add_trigger_t1(target, trigger, tdata1);
                                break;
                        case 2:
                                result = maybe_add_trigger_t2(target, trigger, tdata1);
                                break;
                        case 6:
                                result = maybe_add_trigger_t6(target, trigger, tdata1);
                                break;
                        default:
                                LOG_DEBUG("trigger %d has unknown type %d", i, type);
                                continue;
                }

                if (result != ERROR_OK)
                        continue;

                LOG_DEBUG("[%d] Using trigger %d (type %d) for bp %d", target->coreid,
                                i, type, trigger->unique_id);
                r->trigger_unique_id[i] = trigger->unique_id;
                break;
        }

        riscv_set_register(target, GDB_REGNO_TSELECT, tselect);

        if (i >= r->trigger_count) {
                LOG_ERROR("Couldn't find an available hardware trigger.");
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }

        return ERROR_OK;
}

/**
 * Write one memory item of given "size". Use memory access of given "access_size".
 * Utilize read-modify-write, if needed.
 * */
static int write_by_given_size(struct target *target, target_addr_t address,
                uint32_t size, uint8_t *buffer, uint32_t access_size)
{
        assert(size == 1 || size == 2 || size == 4 || size == 8);
        assert(access_size == 1 || access_size == 2 || access_size == 4 || access_size == 8);

        if (access_size <= size && address % access_size == 0)
                /* Can do the memory access directly without a helper buffer. */
                return target_write_memory(target, address, access_size, size / access_size, buffer);

        unsigned int offset_head = address % access_size;
        unsigned int n_blocks = ((size + offset_head) <= access_size) ? 1 : 2;
        uint8_t helper_buf[n_blocks * access_size];

        /* Read from memory */
        if (target_read_memory(target, address - offset_head, access_size, n_blocks, helper_buf) != ERROR_OK)
                return ERROR_FAIL;

        /* Modify and write back */
        memcpy(helper_buf + offset_head, buffer, size);
        return target_write_memory(target, address - offset_head, access_size, n_blocks, helper_buf);
}

/**
 * Read one memory item of given "size". Use memory access of given "access_size".
 * Read larger section of memory and pick out the required portion, if needed.
 * */
static int read_by_given_size(struct target *target, target_addr_t address,
        uint32_t size, uint8_t *buffer, uint32_t access_size)
{
        assert(size == 1 || size == 2 || size == 4 || size == 8);
        assert(access_size == 1 || access_size == 2 || access_size == 4 || access_size == 8);

        if (access_size <= size && address % access_size == 0)
                /* Can do the memory access directly without a helper buffer. */
                return target_read_memory(target, address, access_size, size / access_size, buffer);

        unsigned int offset_head = address % access_size;
        unsigned int n_blocks = ((size + offset_head) <= access_size) ? 1 : 2;
        uint8_t helper_buf[n_blocks * access_size];

        /* Read from memory */
        if (target_read_memory(target, address - offset_head, access_size, n_blocks, helper_buf) != ERROR_OK)
                return ERROR_FAIL;

        /* Pick the requested portion from the buffer */
        memcpy(buffer, helper_buf + offset_head, size);
        return ERROR_OK;
}

/**
 * Write one memory item using any memory access size that will work.
 * Utilize read-modify-write, if needed.
 * */
int riscv_write_by_any_size(struct target *target, target_addr_t address, uint32_t size, uint8_t *buffer)
{
        assert(size == 1 || size == 2 ||  size == 4 || size == 8);

        /* Find access size that correspond to data size and the alignment. */
        unsigned int preferred_size = size;
        while (address % preferred_size != 0)
                preferred_size /= 2;

        /* First try the preferred (most natural) access size. */
        if (write_by_given_size(target, address, size, buffer, preferred_size) == ERROR_OK)
                return ERROR_OK;

        /* On failure, try other access sizes.
           Minimize the number of accesses by trying first the largest size. */
        for (unsigned int access_size = 8; access_size > 0; access_size /= 2) {
                if (access_size == preferred_size)
                        /* Already tried this size. */
                        continue;

                if (write_by_given_size(target, address, size, buffer, access_size) == ERROR_OK)
                        return ERROR_OK;
        }

        /* No access attempt succeeded. */
        return ERROR_FAIL;
}

/**
 * Read one memory item using any memory access size that will work.
 * Read larger section of memory and pick out the required portion, if needed.
 * */
int riscv_read_by_any_size(struct target *target, target_addr_t address, uint32_t size, uint8_t *buffer)
{
        assert(size == 1 || size == 2 ||  size == 4 || size == 8);

        /* Find access size that correspond to data size and the alignment. */
        unsigned int preferred_size = size;
        while (address % preferred_size != 0)
                preferred_size /= 2;

        /* First try the preferred (most natural) access size. */
        if (read_by_given_size(target, address, size, buffer, preferred_size) == ERROR_OK)
                return ERROR_OK;

        /* On failure, try other access sizes.
           Minimize the number of accesses by trying first the largest size. */
        for (unsigned int access_size = 8; access_size > 0; access_size /= 2) {
                if (access_size == preferred_size)
                        /* Already tried this size. */
                        continue;

                if (read_by_given_size(target, address, size, buffer, access_size) == ERROR_OK)
                        return ERROR_OK;
        }

        /* No access attempt succeeded. */
        return ERROR_FAIL;
}

int riscv_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
{
        LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, breakpoint->address);
        assert(breakpoint);
        if (breakpoint->type == BKPT_SOFT) {
                /** @todo check RVC for size/alignment */
                if (!(breakpoint->length == 4 || breakpoint->length == 2)) {
                        LOG_ERROR("Invalid breakpoint length %d", breakpoint->length);
                        return ERROR_FAIL;
                }

                if (0 != (breakpoint->address % 2)) {
                        LOG_ERROR("Invalid breakpoint alignment for address 0x%" TARGET_PRIxADDR, breakpoint->address);
                        return ERROR_FAIL;
                }

                /* Read the original instruction. */
                if (riscv_read_by_any_size(
                                target, breakpoint->address, breakpoint->length, breakpoint->orig_instr) != ERROR_OK) {
                        LOG_ERROR("Failed to read original instruction at 0x%" TARGET_PRIxADDR,
                                        breakpoint->address);
                        return ERROR_FAIL;
                }

                uint8_t buff[4] = { 0 };
                buf_set_u32(buff, 0, breakpoint->length * CHAR_BIT, breakpoint->length == 4 ? ebreak() : ebreak_c());
                /* Write the ebreak instruction. */
                if (riscv_write_by_any_size(target, breakpoint->address, breakpoint->length, buff) != ERROR_OK) {
                        LOG_ERROR("Failed to write %d-byte breakpoint instruction at 0x%"
                                        TARGET_PRIxADDR, breakpoint->length, breakpoint->address);
                        return ERROR_FAIL;
                }

        } else if (breakpoint->type == BKPT_HARD) {
                struct trigger trigger;
                trigger_from_breakpoint(&trigger, breakpoint);
                int const result = add_trigger(target, &trigger);
                if (result != ERROR_OK)
                        return result;
        } else {
                LOG_INFO("OpenOCD only supports hardware and software breakpoints.");
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }

        breakpoint->set = true;
        return ERROR_OK;
}

static int remove_trigger(struct target *target, struct trigger *trigger)
{
        RISCV_INFO(r);

        if (riscv_enumerate_triggers(target) != ERROR_OK)
                return ERROR_FAIL;

        unsigned int i;
        for (i = 0; i < r->trigger_count; i++) {
                if (r->trigger_unique_id[i] == trigger->unique_id)
                        break;
        }
        if (i >= r->trigger_count) {
                LOG_ERROR("Couldn't find the hardware resources used by hardware "
                                "trigger.");
                return ERROR_FAIL;
        }
        LOG_DEBUG("[%d] Stop using resource %d for bp %d", target->coreid, i,
                        trigger->unique_id);

        riscv_reg_t tselect;
        int result = riscv_get_register(target, &tselect, GDB_REGNO_TSELECT);
        if (result != ERROR_OK)
                return result;
        riscv_set_register(target, GDB_REGNO_TSELECT, i);
        riscv_set_register(target, GDB_REGNO_TDATA1, 0);
        riscv_set_register(target, GDB_REGNO_TSELECT, tselect);
        r->trigger_unique_id[i] = -1;

        return ERROR_OK;
}

int riscv_remove_breakpoint(struct target *target,
                struct breakpoint *breakpoint)
{
        if (breakpoint->type == BKPT_SOFT) {
                /* Write the original instruction. */
                if (riscv_write_by_any_size(
                                target, breakpoint->address, breakpoint->length, breakpoint->orig_instr) != ERROR_OK) {
                        LOG_ERROR("Failed to restore instruction for %d-byte breakpoint at "
                                        "0x%" TARGET_PRIxADDR, breakpoint->length, breakpoint->address);
                        return ERROR_FAIL;
                }

        } else if (breakpoint->type == BKPT_HARD) {
                struct trigger trigger;
                trigger_from_breakpoint(&trigger, breakpoint);
                int result = remove_trigger(target, &trigger);
                if (result != ERROR_OK)
                        return result;

        } else {
                LOG_INFO("OpenOCD only supports hardware and software breakpoints.");
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }

        breakpoint->set = false;

        return ERROR_OK;
}

static void trigger_from_watchpoint(struct trigger *trigger,
                const struct watchpoint *watchpoint)
{
        trigger->address = watchpoint->address;
        trigger->length = watchpoint->length;
        trigger->mask = watchpoint->mask;
        trigger->value = watchpoint->value;
        trigger->read = (watchpoint->rw == WPT_READ || watchpoint->rw == WPT_ACCESS);
        trigger->write = (watchpoint->rw == WPT_WRITE || watchpoint->rw == WPT_ACCESS);
        trigger->execute = false;
        /* unique_id is unique across both breakpoints and watchpoints. */
        trigger->unique_id = watchpoint->unique_id;
}

int riscv_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
{
        struct trigger trigger;
        trigger_from_watchpoint(&trigger, watchpoint);

        int result = add_trigger(target, &trigger);
        if (result != ERROR_OK)
                return result;
        watchpoint->set = true;

        return ERROR_OK;
}

int riscv_remove_watchpoint(struct target *target,
                struct watchpoint *watchpoint)
{
        LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, watchpoint->address);

        struct trigger trigger;
        trigger_from_watchpoint(&trigger, watchpoint);

        int result = remove_trigger(target, &trigger);
        if (result != ERROR_OK)
                return result;
        watchpoint->set = false;

        return ERROR_OK;
}

/* Sets *hit_watchpoint to the first watchpoint identified as causing the
 * current halt.
 *
 * The GDB server uses this information to tell GDB what data address has
 * been hit, which enables GDB to print the hit variable along with its old
 * and new value. */
int riscv_hit_watchpoint(struct target *target, struct watchpoint **hit_watchpoint)
{
        struct watchpoint *wp = target->watchpoints;

        LOG_DEBUG("Current hartid = %d", riscv_current_hartid(target));

        /*TODO instead of disassembling the instruction that we think caused the
         * trigger, check the hit bit of each watchpoint first. The hit bit is
         * simpler and more reliable to check but as it is optional and relatively
         * new, not all hardware will implement it  */
        riscv_reg_t dpc;
        riscv_get_register(target, &dpc, GDB_REGNO_DPC);
        const uint8_t length = 4;
        LOG_DEBUG("dpc is 0x%" PRIx64, dpc);

        /* fetch the instruction at dpc */
        uint8_t buffer[length];
        if (target_read_buffer(target, dpc, length, buffer) != ERROR_OK) {
                LOG_ERROR("Failed to read instruction at dpc 0x%" PRIx64, dpc);
                return ERROR_FAIL;
        }

        uint32_t instruction = 0;

        for (int i = 0; i < length; i++) {
                LOG_DEBUG("Next byte is %x", buffer[i]);
                instruction += (buffer[i] << 8 * i);
        }
        LOG_DEBUG("Full instruction is %x", instruction);

        /* find out which memory address is accessed by the instruction at dpc */
        /* opcode is first 7 bits of the instruction */
        uint8_t opcode = instruction & 0x7F;
        uint32_t rs1;
        int16_t imm;
        riscv_reg_t mem_addr;

        if (opcode == MATCH_LB || opcode == MATCH_SB) {
                rs1 = (instruction & 0xf8000) >> 15;
                riscv_get_register(target, &mem_addr, rs1);

                if (opcode == MATCH_SB) {
                        LOG_DEBUG("%x is store instruction", instruction);
                        imm = ((instruction & 0xf80) >> 7) | ((instruction & 0xfe000000) >> 20);
                } else {
                        LOG_DEBUG("%x is load instruction", instruction);
                        imm = (instruction & 0xfff00000) >> 20;
                }
                /* sign extend 12-bit imm to 16-bits */
                if (imm & (1 << 11))
                        imm |= 0xf000;
                mem_addr += imm;
                LOG_DEBUG("memory address=0x%" PRIx64, mem_addr);
        } else {
                LOG_DEBUG("%x is not a RV32I load or store", instruction);
                return ERROR_FAIL;
        }

        while (wp) {
                /*TODO support length/mask */
                if (wp->address == mem_addr) {
                        *hit_watchpoint = wp;
                        LOG_DEBUG("Hit address=%" TARGET_PRIxADDR, wp->address);
                        return ERROR_OK;
                }
                wp = wp->next;
        }

        /* No match found - either we hit a watchpoint caused by an instruction that
         * this function does not yet disassemble, or we hit a breakpoint.
         *
         * OpenOCD will behave as if this function had never been implemented i.e.
         * report the halt to GDB with no address information. */
        return ERROR_FAIL;
}


static int oldriscv_step(struct target *target, int current, uint32_t address,
                int handle_breakpoints)
{
        struct target_type *tt = get_target_type(target);
        return tt->step(target, current, address, handle_breakpoints);
}

static int old_or_new_riscv_step(struct target *target, int current,
                target_addr_t address, int handle_breakpoints)
{
        RISCV_INFO(r);
        LOG_DEBUG("handle_breakpoints=%d", handle_breakpoints);
        if (!r->is_halted)
                return oldriscv_step(target, current, address, handle_breakpoints);
        else
                return riscv_openocd_step(target, current, address, handle_breakpoints);
}


static int riscv_examine(struct target *target)
{
        LOG_DEBUG("riscv_examine()");
        if (target_was_examined(target)) {
                LOG_DEBUG("Target was already examined.");
                return ERROR_OK;
        }

        /* Don't need to select dbus, since the first thing we do is read dtmcontrol. */

        RISCV_INFO(info);
        uint32_t dtmcontrol = dtmcontrol_scan(target, 0);
        LOG_DEBUG("dtmcontrol=0x%x", dtmcontrol);
        info->dtm_version = get_field(dtmcontrol, DTMCONTROL_VERSION);
        LOG_DEBUG("  version=0x%x", info->dtm_version);

        struct target_type *tt = get_target_type(target);
        if (!tt)
                return ERROR_FAIL;

        int result = tt->init_target(info->cmd_ctx, target);
        if (result != ERROR_OK)
                return result;

        return tt->examine(target);
}

static int oldriscv_poll(struct target *target)
{
        struct target_type *tt = get_target_type(target);
        return tt->poll(target);
}

static int old_or_new_riscv_poll(struct target *target)
{
        RISCV_INFO(r);
        if (!r->is_halted)
                return oldriscv_poll(target);
        else
                return riscv_openocd_poll(target);
}

int riscv_select_current_hart(struct target *target)
{
        return riscv_set_current_hartid(target, target->coreid);
}

int halt_prep(struct target *target)
{
        RISCV_INFO(r);

        LOG_DEBUG("[%s] prep hart, debug_reason=%d", target_name(target),
                                target->debug_reason);
        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;
        if (riscv_is_halted(target)) {
                LOG_DEBUG("[%s] Hart is already halted (reason=%d).",
                                target_name(target), target->debug_reason);
        } else {
                if (r->halt_prep(target) != ERROR_OK)
                        return ERROR_FAIL;
                r->prepped = true;
        }

        return ERROR_OK;
}

int riscv_halt_go_all_harts(struct target *target)
{
        RISCV_INFO(r);

        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;
        if (riscv_is_halted(target)) {
                LOG_DEBUG("[%s] Hart is already halted.", target_name(target));
        } else {
                if (r->halt_go(target) != ERROR_OK)
                        return ERROR_FAIL;
        }

        riscv_invalidate_register_cache(target);

        return ERROR_OK;
}

int halt_go(struct target *target)
{
        riscv_info_t *r = riscv_info(target);
        int result;
        if (!r->is_halted) {
                struct target_type *tt = get_target_type(target);
                result = tt->halt(target);
        } else {
                result = riscv_halt_go_all_harts(target);
        }
        target->state = TARGET_HALTED;
        if (target->debug_reason == DBG_REASON_NOTHALTED)
                target->debug_reason = DBG_REASON_DBGRQ;

        return result;
}

static int halt_finish(struct target *target)
{
        return target_call_event_callbacks(target, TARGET_EVENT_HALTED);
}

int riscv_halt(struct target *target)
{
        RISCV_INFO(r);

        if (!r->is_halted) {
                struct target_type *tt = get_target_type(target);
                return tt->halt(target);
        }

        LOG_DEBUG("[%d] halting all harts", target->coreid);

        int result = ERROR_OK;
        if (target->smp) {
                for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
                        struct target *t = tlist->target;
                        if (halt_prep(t) != ERROR_OK)
                                result = ERROR_FAIL;
                }

                for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
                        struct target *t = tlist->target;
                        riscv_info_t *i = riscv_info(t);
                        if (i->prepped) {
                                if (halt_go(t) != ERROR_OK)
                                        result = ERROR_FAIL;
                        }
                }

                for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
                        struct target *t = tlist->target;
                        if (halt_finish(t) != ERROR_OK)
                                return ERROR_FAIL;
                }

        } else {
                if (halt_prep(target) != ERROR_OK)
                        result = ERROR_FAIL;
                if (halt_go(target) != ERROR_OK)
                        result = ERROR_FAIL;
                if (halt_finish(target) != ERROR_OK)
                        return ERROR_FAIL;
        }

        return result;
}

static int riscv_assert_reset(struct target *target)
{
        LOG_DEBUG("[%d]", target->coreid);
        struct target_type *tt = get_target_type(target);
        riscv_invalidate_register_cache(target);
        return tt->assert_reset(target);
}

static int riscv_deassert_reset(struct target *target)
{
        LOG_DEBUG("[%d]", target->coreid);
        struct target_type *tt = get_target_type(target);
        return tt->deassert_reset(target);
}

int riscv_resume_prep_all_harts(struct target *target)
{
        RISCV_INFO(r);

        LOG_DEBUG("[%s] prep hart", target_name(target));
        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;
        if (riscv_is_halted(target)) {
                if (r->resume_prep(target) != ERROR_OK)
                        return ERROR_FAIL;
        } else {
                LOG_DEBUG("[%s] hart requested resume, but was already resumed",
                                target_name(target));
        }

        LOG_DEBUG("[%s] mark as prepped", target_name(target));
        r->prepped = true;

        return ERROR_OK;
}

/* state must be riscv_reg_t state[RISCV_MAX_HWBPS] = {0}; */
static int disable_triggers(struct target *target, riscv_reg_t *state)
{
        RISCV_INFO(r);

        LOG_DEBUG("deal with triggers");

        if (riscv_enumerate_triggers(target) != ERROR_OK)
                return ERROR_FAIL;

        if (r->manual_hwbp_set) {
                /* Look at every trigger that may have been set. */
                riscv_reg_t tselect;
                if (riscv_get_register(target, &tselect, GDB_REGNO_TSELECT) != ERROR_OK)
                        return ERROR_FAIL;
                for (unsigned int t = 0; t < r->trigger_count; t++) {
                        if (riscv_set_register(target, GDB_REGNO_TSELECT, t) != ERROR_OK)
                                return ERROR_FAIL;
                        riscv_reg_t tdata1;
                        if (riscv_get_register(target, &tdata1, GDB_REGNO_TDATA1) != ERROR_OK)
                                return ERROR_FAIL;
                        if (tdata1 & MCONTROL_DMODE(riscv_xlen(target))) {
                                state[t] = tdata1;
                                if (riscv_set_register(target, GDB_REGNO_TDATA1, 0) != ERROR_OK)
                                        return ERROR_FAIL;
                        }
                }
                if (riscv_set_register(target, GDB_REGNO_TSELECT, tselect) != ERROR_OK)
                        return ERROR_FAIL;

        } else {
                /* Just go through the triggers we manage. */
                struct watchpoint *watchpoint = target->watchpoints;
                int i = 0;
                while (watchpoint) {
                        LOG_DEBUG("watchpoint %d: set=%d", i, watchpoint->set);
                        state[i] = watchpoint->set;
                        if (watchpoint->set) {
                                if (riscv_remove_watchpoint(target, watchpoint) != ERROR_OK)
                                        return ERROR_FAIL;
                        }
                        watchpoint = watchpoint->next;
                        i++;
                }
        }

        return ERROR_OK;
}

static int enable_triggers(struct target *target, riscv_reg_t *state)
{
        RISCV_INFO(r);

        if (r->manual_hwbp_set) {
                /* Look at every trigger that may have been set. */
                riscv_reg_t tselect;
                if (riscv_get_register(target, &tselect, GDB_REGNO_TSELECT) != ERROR_OK)
                        return ERROR_FAIL;
                for (unsigned int t = 0; t < r->trigger_count; t++) {
                        if (state[t] != 0) {
                                if (riscv_set_register(target, GDB_REGNO_TSELECT, t) != ERROR_OK)
                                        return ERROR_FAIL;
                                if (riscv_set_register(target, GDB_REGNO_TDATA1, state[t]) != ERROR_OK)
                                        return ERROR_FAIL;
                        }
                }
                if (riscv_set_register(target, GDB_REGNO_TSELECT, tselect) != ERROR_OK)
                        return ERROR_FAIL;

        } else {
                struct watchpoint *watchpoint = target->watchpoints;
                int i = 0;
                while (watchpoint) {
                        LOG_DEBUG("watchpoint %d: cleared=%" PRId64, i, state[i]);
                        if (state[i]) {
                                if (riscv_add_watchpoint(target, watchpoint) != ERROR_OK)
                                        return ERROR_FAIL;
                        }
                        watchpoint = watchpoint->next;
                        i++;
                }
        }

        return ERROR_OK;
}

/**
 * Get everything ready to resume.
 */
static int resume_prep(struct target *target, int current,
                target_addr_t address, int handle_breakpoints, int debug_execution)
{
        RISCV_INFO(r);
        LOG_DEBUG("[%d]", target->coreid);

        if (!current)
                riscv_set_register(target, GDB_REGNO_PC, address);

        if (target->debug_reason == DBG_REASON_WATCHPOINT) {
                /* To be able to run off a trigger, disable all the triggers, step, and
                 * then resume as usual. */
                riscv_reg_t trigger_state[RISCV_MAX_HWBPS] = {0};

                if (disable_triggers(target, trigger_state) != ERROR_OK)
                        return ERROR_FAIL;

                if (old_or_new_riscv_step(target, true, 0, false) != ERROR_OK)
                        return ERROR_FAIL;

                if (enable_triggers(target, trigger_state) != ERROR_OK)
                        return ERROR_FAIL;
        }

        if (r->is_halted) {
                if (riscv_resume_prep_all_harts(target) != ERROR_OK)
                        return ERROR_FAIL;
        }

        LOG_DEBUG("[%d] mark as prepped", target->coreid);
        r->prepped = true;

        return ERROR_OK;
}

/**
 * Resume all the harts that have been prepped, as close to instantaneous as
 * possible.
 */
static int resume_go(struct target *target, int current,
                target_addr_t address, int handle_breakpoints, int debug_execution)
{
        riscv_info_t *r = riscv_info(target);
        int result;
        if (!r->is_halted) {
                struct target_type *tt = get_target_type(target);
                result = tt->resume(target, current, address, handle_breakpoints,
                                debug_execution);
        } else {
                result = riscv_resume_go_all_harts(target);
        }

        return result;
}

static int resume_finish(struct target *target)
{
        register_cache_invalidate(target->reg_cache);

        target->state = TARGET_RUNNING;
        target->debug_reason = DBG_REASON_NOTHALTED;
        return target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
}

/**
 * @par single_hart When true, only resume a single hart even if SMP is
 * configured.  This is used to run algorithms on just one hart.
 */
int riscv_resume(
                struct target *target,
                int current,
                target_addr_t address,
                int handle_breakpoints,
                int debug_execution,
                bool single_hart)
{
        LOG_DEBUG("handle_breakpoints=%d", handle_breakpoints);
        int result = ERROR_OK;
        if (target->smp && !single_hart) {
                for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
                        struct target *t = tlist->target;
                        if (resume_prep(t, current, address, handle_breakpoints,
                                                debug_execution) != ERROR_OK)
                                result = ERROR_FAIL;
                }

                for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
                        struct target *t = tlist->target;
                        riscv_info_t *i = riscv_info(t);
                        if (i->prepped) {
                                if (resume_go(t, current, address, handle_breakpoints,
                                                        debug_execution) != ERROR_OK)
                                        result = ERROR_FAIL;
                        }
                }

                for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
                        struct target *t = tlist->target;
                        if (resume_finish(t) != ERROR_OK)
                                return ERROR_FAIL;
                }

        } else {
                if (resume_prep(target, current, address, handle_breakpoints,
                                        debug_execution) != ERROR_OK)
                        result = ERROR_FAIL;
                if (resume_go(target, current, address, handle_breakpoints,
                                        debug_execution) != ERROR_OK)
                        result = ERROR_FAIL;
                if (resume_finish(target) != ERROR_OK)
                        return ERROR_FAIL;
        }

        return result;
}

static int riscv_target_resume(struct target *target, int current, target_addr_t address,
                int handle_breakpoints, int debug_execution)
{
        return riscv_resume(target, current, address, handle_breakpoints,
                        debug_execution, false);
}

static int riscv_mmu(struct target *target, int *enabled)
{
        if (!riscv_enable_virt2phys) {
                *enabled = 0;
                return ERROR_OK;
        }

        /* Don't use MMU in explicit or effective M (machine) mode */
        riscv_reg_t priv;
        if (riscv_get_register(target, &priv, GDB_REGNO_PRIV) != ERROR_OK) {
                LOG_ERROR("Failed to read priv register.");
                return ERROR_FAIL;
        }

        riscv_reg_t mstatus;
        if (riscv_get_register(target, &mstatus, GDB_REGNO_MSTATUS) != ERROR_OK) {
                LOG_ERROR("Failed to read mstatus register.");
                return ERROR_FAIL;
        }

        if ((get_field(mstatus, MSTATUS_MPRV) ? get_field(mstatus, MSTATUS_MPP) : priv) == PRV_M) {
                LOG_DEBUG("SATP/MMU ignored in Machine mode (mstatus=0x%" PRIx64 ").", mstatus);
                *enabled = 0;
                return ERROR_OK;
        }

        riscv_reg_t satp;
        if (riscv_get_register(target, &satp, GDB_REGNO_SATP) != ERROR_OK) {
                LOG_DEBUG("Couldn't read SATP.");
                /* If we can't read SATP, then there must not be an MMU. */
                *enabled = 0;
                return ERROR_OK;
        }

        if (get_field(satp, RISCV_SATP_MODE(riscv_xlen(target))) == SATP_MODE_OFF) {
                LOG_DEBUG("MMU is disabled.");
                *enabled = 0;
        } else {
                LOG_DEBUG("MMU is enabled.");
                *enabled = 1;
        }

        return ERROR_OK;
}

static int riscv_address_translate(struct target *target,
                target_addr_t virtual, target_addr_t *physical)
{
        RISCV_INFO(r);
        riscv_reg_t satp_value;
        int mode;
        uint64_t ppn_value;
        target_addr_t table_address;
        const virt2phys_info_t *info;
        uint64_t pte = 0;
        int i;

        int result = riscv_get_register(target, &satp_value, GDB_REGNO_SATP);
        if (result != ERROR_OK)
                return result;

        unsigned xlen = riscv_xlen(target);
        mode = get_field(satp_value, RISCV_SATP_MODE(xlen));
        switch (mode) {
                case SATP_MODE_SV32:
                        info = &sv32;
                        break;
                case SATP_MODE_SV39:
                        info = &sv39;
                        break;
                case SATP_MODE_SV48:
                        info = &sv48;
                        break;
                case SATP_MODE_OFF:
                        LOG_ERROR("No translation or protection." \
                                      " (satp: 0x%" PRIx64 ")", satp_value);
                        return ERROR_FAIL;
                default:
                        LOG_ERROR("The translation mode is not supported." \
                                      " (satp: 0x%" PRIx64 ")", satp_value);
                        return ERROR_FAIL;
        }
        LOG_DEBUG("virtual=0x%" TARGET_PRIxADDR "; mode=%s", virtual, info->name);

        /* verify bits xlen-1:va_bits-1 are all equal */
        target_addr_t mask = ((target_addr_t)1 << (xlen - (info->va_bits - 1))) - 1;
        target_addr_t masked_msbs = (virtual >> (info->va_bits - 1)) & mask;
        if (masked_msbs != 0 && masked_msbs != mask) {
                LOG_ERROR("Virtual address 0x%" TARGET_PRIxADDR " is not sign-extended "
                                "for %s mode.", virtual, info->name);
                return ERROR_FAIL;
        }

        ppn_value = get_field(satp_value, RISCV_SATP_PPN(xlen));
        table_address = ppn_value << RISCV_PGSHIFT;
        i = info->level - 1;
        while (i >= 0) {
                uint64_t vpn = virtual >> info->vpn_shift[i];
                vpn &= info->vpn_mask[i];
                target_addr_t pte_address = table_address +
                                                                        (vpn << info->pte_shift);
                uint8_t buffer[8];
                assert(info->pte_shift <= 3);
                int retval = r->read_memory(target, pte_address,
                                4, (1 << info->pte_shift) / 4, buffer, 4);
                if (retval != ERROR_OK)
                        return ERROR_FAIL;

                if (info->pte_shift == 2)
                        pte = buf_get_u32(buffer, 0, 32);
                else
                        pte = buf_get_u64(buffer, 0, 64);

                LOG_DEBUG("i=%d; PTE @0x%" TARGET_PRIxADDR " = 0x%" PRIx64, i,
                                pte_address, pte);

                if (!(pte & PTE_V) || (!(pte & PTE_R) && (pte & PTE_W)))
                        return ERROR_FAIL;

                if ((pte & PTE_R) || (pte & PTE_X)) /* Found leaf PTE. */
                        break;

                i--;
                if (i < 0)
                        break;
                ppn_value = pte >> PTE_PPN_SHIFT;
                table_address = ppn_value << RISCV_PGSHIFT;
        }

        if (i < 0) {
                LOG_ERROR("Couldn't find the PTE.");
                return ERROR_FAIL;
        }

        /* Make sure to clear out the high bits that may be set. */
        *physical = virtual & (((target_addr_t)1 << info->va_bits) - 1);

        while (i < info->level) {
                ppn_value = pte >> info->pte_ppn_shift[i];
                ppn_value &= info->pte_ppn_mask[i];
                *physical &= ~(((target_addr_t)info->pa_ppn_mask[i]) <<
                                info->pa_ppn_shift[i]);
                *physical |= (ppn_value << info->pa_ppn_shift[i]);
                i++;
        }
        LOG_DEBUG("0x%" TARGET_PRIxADDR " -> 0x%" TARGET_PRIxADDR, virtual,
                        *physical);

        return ERROR_OK;
}

static int riscv_virt2phys(struct target *target, target_addr_t virtual, target_addr_t *physical)
{
        int enabled;
        if (riscv_mmu(target, &enabled) == ERROR_OK) {
                if (!enabled)
                        return ERROR_FAIL;

                if (riscv_address_translate(target, virtual, physical) == ERROR_OK)
                        return ERROR_OK;
        }

        return ERROR_FAIL;
}

static int riscv_read_phys_memory(struct target *target, target_addr_t phys_address,
                        uint32_t size, uint32_t count, uint8_t *buffer)
{
        RISCV_INFO(r);
        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;
        return r->read_memory(target, phys_address, size, count, buffer, size);
}

static int riscv_read_memory(struct target *target, target_addr_t address,
                uint32_t size, uint32_t count, uint8_t *buffer)
{
        if (count == 0) {
                LOG_WARNING("0-length read from 0x%" TARGET_PRIxADDR, address);
                return ERROR_OK;
        }

        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;

        target_addr_t physical_addr;
        if (target->type->virt2phys(target, address, &physical_addr) == ERROR_OK)
                address = physical_addr;

        RISCV_INFO(r);
        return r->read_memory(target, address, size, count, buffer, size);
}

static int riscv_write_phys_memory(struct target *target, target_addr_t phys_address,
                        uint32_t size, uint32_t count, const uint8_t *buffer)
{
        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;
        struct target_type *tt = get_target_type(target);
        return tt->write_memory(target, phys_address, size, count, buffer);
}

static int riscv_write_memory(struct target *target, target_addr_t address,
                uint32_t size, uint32_t count, const uint8_t *buffer)
{
        if (count == 0) {
                LOG_WARNING("0-length write to 0x%" TARGET_PRIxADDR, address);
                return ERROR_OK;
        }

        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;

        target_addr_t physical_addr;
        if (target->type->virt2phys(target, address, &physical_addr) == ERROR_OK)
                address = physical_addr;

        struct target_type *tt = get_target_type(target);
        return tt->write_memory(target, address, size, count, buffer);
}

const char *riscv_get_gdb_arch(struct target *target)
{
        switch (riscv_xlen(target)) {
                case 32:
                        return "riscv:rv32";
                case 64:
                        return "riscv:rv64";
        }
        LOG_ERROR("Unsupported xlen: %d", riscv_xlen(target));
        return NULL;
}

static int riscv_get_gdb_reg_list_internal(struct target *target,
                struct reg **reg_list[], int *reg_list_size,
                enum target_register_class reg_class, bool read)
{
        RISCV_INFO(r);
        LOG_DEBUG("current_hartid=%d, reg_class=%d, read=%d",
                        r->current_hartid, reg_class, read);

        if (!target->reg_cache) {
                LOG_ERROR("Target not initialized. Return ERROR_FAIL.");
                return ERROR_FAIL;
        }

        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;

        switch (reg_class) {
                case REG_CLASS_GENERAL:
                        *reg_list_size = 33;
                        break;
                case REG_CLASS_ALL:
                        *reg_list_size = target->reg_cache->num_regs;
                        break;
                default:
                        LOG_ERROR("Unsupported reg_class: %d", reg_class);
                        return ERROR_FAIL;
        }

        *reg_list = calloc(*reg_list_size, sizeof(struct reg *));
        if (!*reg_list)
                return ERROR_FAIL;

        for (int i = 0; i < *reg_list_size; i++) {
                assert(!target->reg_cache->reg_list[i].valid ||
                                target->reg_cache->reg_list[i].size > 0);
                (*reg_list)[i] = &target->reg_cache->reg_list[i];
                if (read &&
                                target->reg_cache->reg_list[i].exist &&
                                !target->reg_cache->reg_list[i].valid) {
                        if (target->reg_cache->reg_list[i].type->get(
                                                &target->reg_cache->reg_list[i]) != ERROR_OK)
                                return ERROR_FAIL;
                }
        }

        return ERROR_OK;
}

static int riscv_get_gdb_reg_list_noread(struct target *target,
                struct reg **reg_list[], int *reg_list_size,
                enum target_register_class reg_class)
{
        return riscv_get_gdb_reg_list_internal(target, reg_list, reg_list_size,
                        reg_class, false);
}

static int riscv_get_gdb_reg_list(struct target *target,
                struct reg **reg_list[], int *reg_list_size,
                enum target_register_class reg_class)
{
        return riscv_get_gdb_reg_list_internal(target, reg_list, reg_list_size,
                        reg_class, true);
}

static int riscv_arch_state(struct target *target)
{
        struct target_type *tt = get_target_type(target);
        return tt->arch_state(target);
}

/* Algorithm must end with a software breakpoint instruction. */
static int riscv_run_algorithm(struct target *target, int num_mem_params,
                struct mem_param *mem_params, int num_reg_params,
                struct reg_param *reg_params, target_addr_t entry_point,
                target_addr_t exit_point, int timeout_ms, void *arch_info)
{
        RISCV_INFO(info);

        if (num_mem_params > 0) {
                LOG_ERROR("Memory parameters are not supported for RISC-V algorithms.");
                return ERROR_FAIL;
        }

        if (target->state != TARGET_HALTED) {
                LOG_WARNING("target not halted");
                return ERROR_TARGET_NOT_HALTED;
        }

        /* Save registers */
        struct reg *reg_pc = register_get_by_name(target->reg_cache, "pc", true);
        if (!reg_pc || reg_pc->type->get(reg_pc) != ERROR_OK)
                return ERROR_FAIL;
        uint64_t saved_pc = buf_get_u64(reg_pc->value, 0, reg_pc->size);
        LOG_DEBUG("saved_pc=0x%" PRIx64, saved_pc);

        uint64_t saved_regs[32];
        for (int i = 0; i < num_reg_params; i++) {
                LOG_DEBUG("save %s", reg_params[i].reg_name);
                struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, false);
                if (!r) {
                        LOG_ERROR("Couldn't find register named '%s'", reg_params[i].reg_name);
                        return ERROR_FAIL;
                }

                if (r->size != reg_params[i].size) {
                        LOG_ERROR("Register %s is %d bits instead of %d bits.",
                                        reg_params[i].reg_name, r->size, reg_params[i].size);
                        return ERROR_FAIL;
                }

                if (r->number > GDB_REGNO_XPR31) {
                        LOG_ERROR("Only GPRs can be use as argument registers.");
                        return ERROR_FAIL;
                }

                if (r->type->get(r) != ERROR_OK)
                        return ERROR_FAIL;
                saved_regs[r->number] = buf_get_u64(r->value, 0, r->size);

                if (reg_params[i].direction == PARAM_OUT || reg_params[i].direction == PARAM_IN_OUT) {
                        if (r->type->set(r, reg_params[i].value) != ERROR_OK)
                                return ERROR_FAIL;
                }
        }


        /* Disable Interrupts before attempting to run the algorithm. */
        uint64_t current_mstatus;
        uint8_t mstatus_bytes[8] = { 0 };

        LOG_DEBUG("Disabling Interrupts");
        struct reg *reg_mstatus = register_get_by_name(target->reg_cache,
                        "mstatus", true);
        if (!reg_mstatus) {
                LOG_ERROR("Couldn't find mstatus!");
                return ERROR_FAIL;
        }

        reg_mstatus->type->get(reg_mstatus);
        current_mstatus = buf_get_u64(reg_mstatus->value, 0, reg_mstatus->size);
        uint64_t ie_mask = MSTATUS_MIE | MSTATUS_HIE | MSTATUS_SIE | MSTATUS_UIE;
        buf_set_u64(mstatus_bytes, 0, info->xlen, set_field(current_mstatus,
                                ie_mask, 0));

        reg_mstatus->type->set(reg_mstatus, mstatus_bytes);

        /* Run algorithm */
        LOG_DEBUG("resume at 0x%" TARGET_PRIxADDR, entry_point);
        if (riscv_resume(target, 0, entry_point, 0, 0, true) != ERROR_OK)
                return ERROR_FAIL;

        int64_t start = timeval_ms();
        while (target->state != TARGET_HALTED) {
                LOG_DEBUG("poll()");
                int64_t now = timeval_ms();
                if (now - start > timeout_ms) {
                        LOG_ERROR("Algorithm timed out after %" PRId64 " ms.", now - start);
                        riscv_halt(target);
                        old_or_new_riscv_poll(target);
                        enum gdb_regno regnums[] = {
                                GDB_REGNO_RA, GDB_REGNO_SP, GDB_REGNO_GP, GDB_REGNO_TP,
                                GDB_REGNO_T0, GDB_REGNO_T1, GDB_REGNO_T2, GDB_REGNO_FP,
                                GDB_REGNO_S1, GDB_REGNO_A0, GDB_REGNO_A1, GDB_REGNO_A2,
                                GDB_REGNO_A3, GDB_REGNO_A4, GDB_REGNO_A5, GDB_REGNO_A6,
                                GDB_REGNO_A7, GDB_REGNO_S2, GDB_REGNO_S3, GDB_REGNO_S4,
                                GDB_REGNO_S5, GDB_REGNO_S6, GDB_REGNO_S7, GDB_REGNO_S8,
                                GDB_REGNO_S9, GDB_REGNO_S10, GDB_REGNO_S11, GDB_REGNO_T3,
                                GDB_REGNO_T4, GDB_REGNO_T5, GDB_REGNO_T6,
                                GDB_REGNO_PC,
                                GDB_REGNO_MSTATUS, GDB_REGNO_MEPC, GDB_REGNO_MCAUSE,
                        };
                        for (unsigned i = 0; i < ARRAY_SIZE(regnums); i++) {
                                enum gdb_regno regno = regnums[i];
                                riscv_reg_t reg_value;
                                if (riscv_get_register(target, &reg_value, regno) != ERROR_OK)
                                        break;
                                LOG_ERROR("%s = 0x%" PRIx64, gdb_regno_name(regno), reg_value);
                        }
                        return ERROR_TARGET_TIMEOUT;
                }

                int result = old_or_new_riscv_poll(target);
                if (result != ERROR_OK)
                        return result;
        }

        /* The current hart id might have been changed in poll(). */
        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;

        if (reg_pc->type->get(reg_pc) != ERROR_OK)
                return ERROR_FAIL;
        uint64_t final_pc = buf_get_u64(reg_pc->value, 0, reg_pc->size);
        if (exit_point && final_pc != exit_point) {
                LOG_ERROR("PC ended up at 0x%" PRIx64 " instead of 0x%"
                                TARGET_PRIxADDR, final_pc, exit_point);
                return ERROR_FAIL;
        }

        /* Restore Interrupts */
        LOG_DEBUG("Restoring Interrupts");
        buf_set_u64(mstatus_bytes, 0, info->xlen, current_mstatus);
        reg_mstatus->type->set(reg_mstatus, mstatus_bytes);

        /* Restore registers */
        uint8_t buf[8] = { 0 };
        buf_set_u64(buf, 0, info->xlen, saved_pc);
        if (reg_pc->type->set(reg_pc, buf) != ERROR_OK)
                return ERROR_FAIL;

        for (int i = 0; i < num_reg_params; i++) {
                if (reg_params[i].direction == PARAM_IN ||
                                reg_params[i].direction == PARAM_IN_OUT) {
                        struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, false);
                        if (r->type->get(r) != ERROR_OK) {
                                LOG_ERROR("get(%s) failed", r->name);
                                return ERROR_FAIL;
                        }
                        buf_cpy(r->value, reg_params[i].value, reg_params[i].size);
                }
                LOG_DEBUG("restore %s", reg_params[i].reg_name);
                struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, false);
                buf_set_u64(buf, 0, info->xlen, saved_regs[r->number]);
                if (r->type->set(r, buf) != ERROR_OK) {
                        LOG_ERROR("set(%s) failed", r->name);
                        return ERROR_FAIL;
                }
        }

        return ERROR_OK;
}

static int riscv_checksum_memory(struct target *target,
                target_addr_t address, uint32_t count,
                uint32_t *checksum)
{
        struct working_area *crc_algorithm;
        struct reg_param reg_params[2];
        int retval;

        LOG_DEBUG("address=0x%" TARGET_PRIxADDR "; count=0x%" PRIx32, address, count);

        static const uint8_t riscv32_crc_code[] = {
#include "contrib/loaders/checksum/riscv32_crc.inc"
        };
        static const uint8_t riscv64_crc_code[] = {
#include "contrib/loaders/checksum/riscv64_crc.inc"
        };

        static const uint8_t *crc_code;

        unsigned xlen = riscv_xlen(target);
        unsigned crc_code_size;
        if (xlen == 32) {
                crc_code = riscv32_crc_code;
                crc_code_size = sizeof(riscv32_crc_code);
        } else {
                crc_code = riscv64_crc_code;
                crc_code_size = sizeof(riscv64_crc_code);
        }

        if (count < crc_code_size * 4) {
                /* Don't use the algorithm for relatively small buffers. It's faster
                 * just to read the memory.  target_checksum_memory() will take care of
                 * that if we fail. */
                return ERROR_FAIL;
        }

        retval = target_alloc_working_area(target, crc_code_size, &crc_algorithm);
        if (retval != ERROR_OK)
                return retval;

        if (crc_algorithm->address + crc_algorithm->size > address &&
                        crc_algorithm->address < address + count) {
                /* Region to checksum overlaps with the work area we've been assigned.
                 * Bail. (Would be better to manually checksum what we read there, and
                 * use the algorithm for the rest.) */
                target_free_working_area(target, crc_algorithm);
                return ERROR_FAIL;
        }

        retval = target_write_buffer(target, crc_algorithm->address, crc_code_size,
                        crc_code);
        if (retval != ERROR_OK) {
                LOG_ERROR("Failed to write code to " TARGET_ADDR_FMT ": %d",
                                crc_algorithm->address, retval);
                target_free_working_area(target, crc_algorithm);
                return retval;
        }

        init_reg_param(&reg_params[0], "a0", xlen, PARAM_IN_OUT);
        init_reg_param(&reg_params[1], "a1", xlen, PARAM_OUT);
        buf_set_u64(reg_params[0].value, 0, xlen, address);
        buf_set_u64(reg_params[1].value, 0, xlen, count);

        /* 20 second timeout/megabyte */
        int timeout = 20000 * (1 + (count / (1024 * 1024)));

        retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
                        crc_algorithm->address,
                        0,      /* Leave exit point unspecified because we don't know. */
                        timeout, NULL);

        if (retval == ERROR_OK)
                *checksum = buf_get_u32(reg_params[0].value, 0, 32);
        else
                LOG_ERROR("error executing RISC-V CRC algorithm");

        destroy_reg_param(&reg_params[0]);
        destroy_reg_param(&reg_params[1]);

        target_free_working_area(target, crc_algorithm);

        LOG_DEBUG("checksum=0x%" PRIx32 ", result=%d", *checksum, retval);

        return retval;
}

/*** OpenOCD Helper Functions ***/

enum riscv_poll_hart {
        RPH_NO_CHANGE,
        RPH_DISCOVERED_HALTED,
        RPH_DISCOVERED_RUNNING,
        RPH_ERROR
};
static enum riscv_poll_hart riscv_poll_hart(struct target *target, int hartid)
{
        RISCV_INFO(r);
        if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
                return RPH_ERROR;

        LOG_DEBUG("polling hart %d, target->state=%d", hartid, target->state);

        /* If OpenOCD thinks we're running but this hart is halted then it's time
         * to raise an event. */
        bool halted = riscv_is_halted(target);
        if (target->state != TARGET_HALTED && halted) {
                LOG_DEBUG("  triggered a halt");
                r->on_halt(target);
                return RPH_DISCOVERED_HALTED;
        } else if (target->state != TARGET_RUNNING && !halted) {
                LOG_DEBUG("  triggered running");
                target->state = TARGET_RUNNING;
                target->debug_reason = DBG_REASON_NOTHALTED;
                return RPH_DISCOVERED_RUNNING;
        }

        return RPH_NO_CHANGE;
}

int set_debug_reason(struct target *target, enum riscv_halt_reason halt_reason)
{
        switch (halt_reason) {
                case RISCV_HALT_BREAKPOINT:
                        target->debug_reason = DBG_REASON_BREAKPOINT;
                        break;
                case RISCV_HALT_TRIGGER:
                        target->debug_reason = DBG_REASON_WATCHPOINT;
                        break;
                case RISCV_HALT_INTERRUPT:
                case RISCV_HALT_GROUP:
                        target->debug_reason = DBG_REASON_DBGRQ;
                        break;
                case RISCV_HALT_SINGLESTEP:
                        target->debug_reason = DBG_REASON_SINGLESTEP;
                        break;
                case RISCV_HALT_UNKNOWN:
                        target->debug_reason = DBG_REASON_UNDEFINED;
                        break;
                case RISCV_HALT_ERROR:
                        return ERROR_FAIL;
        }
        LOG_DEBUG("[%s] debug_reason=%d", target_name(target), target->debug_reason);
        return ERROR_OK;
}

int sample_memory(struct target *target)
{
        RISCV_INFO(r);

        if (!r->sample_buf.buf || !r->sample_config.enabled)
                return ERROR_OK;

        LOG_DEBUG("buf used/size: %d/%d", r->sample_buf.used, r->sample_buf.size);

        uint64_t start = timeval_ms();
        riscv_sample_buf_maybe_add_timestamp(target, true);
        int result = ERROR_OK;
        if (r->sample_memory) {
                result = r->sample_memory(target, &r->sample_buf, &r->sample_config,
                                                                          start + TARGET_DEFAULT_POLLING_INTERVAL);
                if (result != ERROR_NOT_IMPLEMENTED)
                        goto exit;
        }

        /* Default slow path. */
        while (timeval_ms() - start < TARGET_DEFAULT_POLLING_INTERVAL) {
                for (unsigned int i = 0; i < ARRAY_SIZE(r->sample_config.bucket); i++) {
                        if (r->sample_config.bucket[i].enabled &&
                                        r->sample_buf.used + 1 + r->sample_config.bucket[i].size_bytes < r->sample_buf.size) {
                                assert(i < RISCV_SAMPLE_BUF_TIMESTAMP_BEFORE);
                                r->sample_buf.buf[r->sample_buf.used] = i;
                                result = riscv_read_phys_memory(
                                        target, r->sample_config.bucket[i].address,
                                        r->sample_config.bucket[i].size_bytes, 1,
                                        r->sample_buf.buf + r->sample_buf.used + 1);
                                if (result == ERROR_OK)
                                        r->sample_buf.used += 1 + r->sample_config.bucket[i].size_bytes;
                                else
                                        goto exit;
                        }
                }
        }

exit:
        riscv_sample_buf_maybe_add_timestamp(target, false);
        if (result != ERROR_OK) {
                LOG_INFO("Turning off memory sampling because it failed.");
                r->sample_config.enabled = false;
        }
        return result;
}

/*** OpenOCD Interface ***/
int riscv_openocd_poll(struct target *target)
{
        LOG_DEBUG("polling all harts");
        int halted_hart = -1;

        if (target->smp) {
                unsigned halts_discovered = 0;
                unsigned should_remain_halted = 0;
                unsigned should_resume = 0;
                unsigned i = 0;
                for (struct target_list *list = target->head; list;
                                list = list->next, i++) {
                        struct target *t = list->target;
                        riscv_info_t *r = riscv_info(t);
                        enum riscv_poll_hart out = riscv_poll_hart(t, r->current_hartid);
                        switch (out) {
                        case RPH_NO_CHANGE:
                                break;
                        case RPH_DISCOVERED_RUNNING:
                                t->state = TARGET_RUNNING;
                                t->debug_reason = DBG_REASON_NOTHALTED;
                                break;
                        case RPH_DISCOVERED_HALTED:
                                halts_discovered++;
                                t->state = TARGET_HALTED;
                                enum riscv_halt_reason halt_reason =
                                        riscv_halt_reason(t, r->current_hartid);
                                if (set_debug_reason(t, halt_reason) != ERROR_OK)
                                        return ERROR_FAIL;

                                if (halt_reason == RISCV_HALT_BREAKPOINT) {
                                        int retval;
                                        switch (riscv_semihosting(t, &retval)) {
                                        case SEMI_NONE:
                                        case SEMI_WAITING:
                                                /* This hart should remain halted. */
                                                should_remain_halted++;
                                                break;
                                        case SEMI_HANDLED:
                                                /* This hart should be resumed, along with any other
                                                         * harts that halted due to haltgroups. */
                                                should_resume++;
                                                break;
                                        case SEMI_ERROR:
                                                return retval;
                                        }
                                } else if (halt_reason != RISCV_HALT_GROUP) {
                                        should_remain_halted++;
                                }
                                break;

                        case RPH_ERROR:
                                return ERROR_FAIL;
                        }
                }

                LOG_DEBUG("should_remain_halted=%d, should_resume=%d",
                                  should_remain_halted, should_resume);
                if (should_remain_halted && should_resume) {
                        LOG_WARNING("%d harts should remain halted, and %d should resume.",
                                                should_remain_halted, should_resume);
                }
                if (should_remain_halted) {
                        LOG_DEBUG("halt all");
                        riscv_halt(target);
                } else if (should_resume) {
                        LOG_DEBUG("resume all");
                        riscv_resume(target, true, 0, 0, 0, false);
                }

                /* Sample memory if any target is running. */
                for (struct target_list *list = target->head; list;
                                list = list->next, i++) {
                        struct target *t = list->target;
                        if (t->state == TARGET_RUNNING) {
                                sample_memory(target);
                                break;
                        }
                }

                return ERROR_OK;

        } else {
                enum riscv_poll_hart out = riscv_poll_hart(target,
                                riscv_current_hartid(target));
                if (out == RPH_NO_CHANGE || out == RPH_DISCOVERED_RUNNING) {
                        if (target->state == TARGET_RUNNING)
                                sample_memory(target);
                        return ERROR_OK;
                } else if (out == RPH_ERROR) {
                        return ERROR_FAIL;
                }

                halted_hart = riscv_current_hartid(target);
                LOG_DEBUG("  hart %d halted", halted_hart);

                enum riscv_halt_reason halt_reason = riscv_halt_reason(target, halted_hart);
                if (set_debug_reason(target, halt_reason) != ERROR_OK)
                        return ERROR_FAIL;
                target->state = TARGET_HALTED;
        }

        if (target->debug_reason == DBG_REASON_BREAKPOINT) {
                int retval;
                switch (riscv_semihosting(target, &retval)) {
                        case SEMI_NONE:
                        case SEMI_WAITING:
                                target_call_event_callbacks(target, TARGET_EVENT_HALTED);
                                break;
                        case SEMI_HANDLED:
                                if (riscv_resume(target, true, 0, 0, 0, false) != ERROR_OK)
                                        return ERROR_FAIL;
                                break;
                        case SEMI_ERROR:
                                return retval;
                }
        } else {
                target_call_event_callbacks(target, TARGET_EVENT_HALTED);
        }

        return ERROR_OK;
}

int riscv_openocd_step(struct target *target, int current,
                target_addr_t address, int handle_breakpoints)
{
        LOG_DEBUG("stepping rtos hart");

        if (!current)
                riscv_set_register(target, GDB_REGNO_PC, address);

        riscv_reg_t trigger_state[RISCV_MAX_HWBPS] = {0};
        if (disable_triggers(target, trigger_state) != ERROR_OK)
                return ERROR_FAIL;

        int out = riscv_step_rtos_hart(target);
        if (out != ERROR_OK) {
                LOG_ERROR("unable to step rtos hart");
                return out;
        }

        register_cache_invalidate(target->reg_cache);

        if (enable_triggers(target, trigger_state) != ERROR_OK)
                return ERROR_FAIL;

        target->state = TARGET_RUNNING;
        target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
        target->state = TARGET_HALTED;
        target->debug_reason = DBG_REASON_SINGLESTEP;
        target_call_event_callbacks(target, TARGET_EVENT_HALTED);
        return out;
}

/* Command Handlers */
COMMAND_HANDLER(riscv_set_command_timeout_sec)
{
        if (CMD_ARGC != 1) {
                LOG_ERROR("Command takes exactly 1 parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
        int timeout = atoi(CMD_ARGV[0]);
        if (timeout <= 0) {
                LOG_ERROR("%s is not a valid integer argument for command.", CMD_ARGV[0]);
                return ERROR_FAIL;
        }

        riscv_command_timeout_sec = timeout;

        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_reset_timeout_sec)
{
        if (CMD_ARGC != 1) {
                LOG_ERROR("Command takes exactly 1 parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
        int timeout = atoi(CMD_ARGV[0]);
        if (timeout <= 0) {
                LOG_ERROR("%s is not a valid integer argument for command.", CMD_ARGV[0]);
                return ERROR_FAIL;
        }

        riscv_reset_timeout_sec = timeout;
        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_prefer_sba)
{
        struct target *target = get_current_target(CMD_CTX);
        RISCV_INFO(r);
        bool prefer_sba;
        LOG_WARNING("`riscv set_prefer_sba` is deprecated. Please use `riscv set_mem_access` instead.");
        if (CMD_ARGC != 1) {
                LOG_ERROR("Command takes exactly 1 parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
        COMMAND_PARSE_ON_OFF(CMD_ARGV[0], prefer_sba);
        if (prefer_sba) {
                /* Use system bus with highest priority */
                r->mem_access_methods[0] = RISCV_MEM_ACCESS_SYSBUS;
                r->mem_access_methods[1] = RISCV_MEM_ACCESS_PROGBUF;
                r->mem_access_methods[2] = RISCV_MEM_ACCESS_ABSTRACT;
        } else {
                /* Use progbuf with highest priority */
                r->mem_access_methods[0] = RISCV_MEM_ACCESS_PROGBUF;
                r->mem_access_methods[1] = RISCV_MEM_ACCESS_SYSBUS;
                r->mem_access_methods[2] = RISCV_MEM_ACCESS_ABSTRACT;
        }

        /* Reset warning flags */
        r->mem_access_progbuf_warn = true;
        r->mem_access_sysbus_warn = true;
        r->mem_access_abstract_warn = true;

        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_mem_access)
{
        struct target *target = get_current_target(CMD_CTX);
        RISCV_INFO(r);
        int progbuf_cnt = 0;
        int sysbus_cnt = 0;
        int abstract_cnt = 0;

        if (CMD_ARGC < 1 || CMD_ARGC > RISCV_NUM_MEM_ACCESS_METHODS) {
                LOG_ERROR("Command takes 1 to %d parameters", RISCV_NUM_MEM_ACCESS_METHODS);
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        /* Check argument validity */
        for (unsigned int i = 0; i < CMD_ARGC; i++) {
                if (strcmp("progbuf", CMD_ARGV[i]) == 0) {
                        progbuf_cnt++;
                } else if (strcmp("sysbus", CMD_ARGV[i]) == 0) {
                        sysbus_cnt++;
                } else if (strcmp("abstract", CMD_ARGV[i]) == 0) {
                        abstract_cnt++;
                } else {
                        LOG_ERROR("Unknown argument '%s'. "
                                "Must be one of: 'progbuf', 'sysbus' or 'abstract'.", CMD_ARGV[i]);
                        return ERROR_COMMAND_SYNTAX_ERROR;
                }
        }
        if (progbuf_cnt > 1 || sysbus_cnt > 1 || abstract_cnt > 1) {
                LOG_ERROR("Syntax error - duplicate arguments to `riscv set_mem_access`.");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        /* Args are valid, store them */
        for (unsigned int i = 0; i < RISCV_NUM_MEM_ACCESS_METHODS; i++)
                r->mem_access_methods[i] = RISCV_MEM_ACCESS_UNSPECIFIED;
        for (unsigned int i = 0; i < CMD_ARGC; i++) {
                if (strcmp("progbuf", CMD_ARGV[i]) == 0)
                        r->mem_access_methods[i] = RISCV_MEM_ACCESS_PROGBUF;
                else if (strcmp("sysbus", CMD_ARGV[i]) == 0)
                        r->mem_access_methods[i] = RISCV_MEM_ACCESS_SYSBUS;
                else if (strcmp("abstract", CMD_ARGV[i]) == 0)
                        r->mem_access_methods[i] = RISCV_MEM_ACCESS_ABSTRACT;
        }

        /* Reset warning flags */
        r->mem_access_progbuf_warn = true;
        r->mem_access_sysbus_warn = true;
        r->mem_access_abstract_warn = true;

        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_enable_virtual)
{
        if (CMD_ARGC != 1) {
                LOG_ERROR("Command takes exactly 1 parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
        COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_enable_virtual);
        return ERROR_OK;
}

int parse_ranges(struct list_head *ranges, const char *tcl_arg, const char *reg_type, unsigned int max_val)
{
        char *args = strdup(tcl_arg);
        if (!args)
                return ERROR_FAIL;

        /* For backward compatibility, allow multiple parameters within one TCL argument, separated by ',' */
        char *arg = strtok(args, ",");
        while (arg) {
                unsigned low = 0;
                unsigned high = 0;
                char *name = NULL;

                char *dash = strchr(arg, '-');
                char *equals = strchr(arg, '=');
                unsigned int pos;

                if (!dash && !equals) {
                        /* Expecting single register number. */
                        if (sscanf(arg, "%u%n", &low, &pos) != 1 || pos != strlen(arg)) {
                                LOG_ERROR("Failed to parse single register number from '%s'.", arg);
                                free(args);
                                return ERROR_COMMAND_SYNTAX_ERROR;
                        }
                } else if (dash && !equals) {
                        /* Expecting register range - two numbers separated by a dash: ##-## */
                        *dash = 0;
                        dash++;
                        if (sscanf(arg, "%u%n", &low, &pos) != 1 || pos != strlen(arg)) {
                                LOG_ERROR("Failed to parse single register number from '%s'.", arg);
                                free(args);
                                return ERROR_COMMAND_SYNTAX_ERROR;
                        }
                        if (sscanf(dash, "%u%n", &high, &pos) != 1 || pos != strlen(dash)) {
                                LOG_ERROR("Failed to parse single register number from '%s'.", dash);
                                free(args);
                                return ERROR_COMMAND_SYNTAX_ERROR;
                        }
                        if (high < low) {
                                LOG_ERROR("Incorrect range encountered [%u, %u].", low, high);
                                free(args);
                                return ERROR_FAIL;
                        }
                } else if (!dash && equals) {
                        /* Expecting single register number with textual name specified: ##=name */
                        *equals = 0;
                        equals++;
                        if (sscanf(arg, "%u%n", &low, &pos) != 1 || pos != strlen(arg)) {
                                LOG_ERROR("Failed to parse single register number from '%s'.", arg);
                                free(args);
                                return ERROR_COMMAND_SYNTAX_ERROR;
                        }

                        name = calloc(1, strlen(equals) + strlen(reg_type) + 2);
                        if (!name) {
                                LOG_ERROR("Failed to allocate register name.");
                                free(args);
                                return ERROR_FAIL;
                        }

                        /* Register prefix: "csr_" or "custom_" */
                        strcpy(name, reg_type);
                        name[strlen(reg_type)] = '_';

                        if (sscanf(equals, "%[_a-zA-Z0-9]%n", name + strlen(reg_type) + 1, &pos) != 1 || pos != strlen(equals)) {
                                LOG_ERROR("Failed to parse register name from '%s'.", equals);
                                free(args);
                                free(name);
                                return ERROR_COMMAND_SYNTAX_ERROR;
                        }
                } else {
                        LOG_ERROR("Invalid argument '%s'.", arg);
                        free(args);
                        return ERROR_COMMAND_SYNTAX_ERROR;
                }

                high = high > low ? high : low;

                if (high > max_val) {
                        LOG_ERROR("Cannot expose %s register number %u, maximum allowed value is %u.", reg_type, high, max_val);
                        free(name);
                        free(args);
                        return ERROR_FAIL;
                }

                /* Check for overlap, name uniqueness. */
                range_list_t *entry;
                list_for_each_entry(entry, ranges, list) {
                        if ((entry->low <= high) && (low <= entry->high)) {
                                if (low == high)
                                        LOG_WARNING("Duplicate %s register number - "
                                                        "Register %u has already been exposed previously", reg_type, low);
                                else
                                        LOG_WARNING("Overlapping register ranges - Register range starting from %u overlaps "
                                                        "with already exposed register/range at %u.", low, entry->low);
                        }

                        if (entry->name && name && (strcasecmp(entry->name, name) == 0)) {
                                LOG_ERROR("Duplicate register name \"%s\" found.", name);
                                free(name);
                                free(args);
                                return ERROR_FAIL;
                        }
                }

                range_list_t *range = calloc(1, sizeof(range_list_t));
                if (!range) {
                        LOG_ERROR("Failed to allocate range list.");
                        free(name);
                        free(args);
                        return ERROR_FAIL;
                }

                range->low = low;
                range->high = high;
                range->name = name;
                list_add(&range->list, ranges);

                arg = strtok(NULL, ",");
        }

        free(args);
        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_expose_csrs)
{
        if (CMD_ARGC == 0) {
                LOG_ERROR("Command expects parameters");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        struct target *target = get_current_target(CMD_CTX);
        RISCV_INFO(info);
        int ret = ERROR_OK;

        for (unsigned int i = 0; i < CMD_ARGC; i++) {
                ret = parse_ranges(&info->expose_csr, CMD_ARGV[i], "csr", 0xfff);
                if (ret != ERROR_OK)
                        break;
        }

        return ret;
}

COMMAND_HANDLER(riscv_set_expose_custom)
{
        if (CMD_ARGC == 0) {
                LOG_ERROR("Command expects parameters");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        struct target *target = get_current_target(CMD_CTX);
        RISCV_INFO(info);
        int ret = ERROR_OK;

        for (unsigned int i = 0; i < CMD_ARGC; i++) {
                ret = parse_ranges(&info->expose_custom, CMD_ARGV[i], "custom", 0x3fff);
                if (ret != ERROR_OK)
                        break;
        }

        return ret;
}

COMMAND_HANDLER(riscv_authdata_read)
{
        unsigned int index = 0;
        if (CMD_ARGC == 0) {
                /* nop */
        } else if (CMD_ARGC == 1) {
                COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], index);
        } else {
                LOG_ERROR("Command takes at most one parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        struct target *target = get_current_target(CMD_CTX);
        if (!target) {
                LOG_ERROR("target is NULL!");
                return ERROR_FAIL;
        }

        RISCV_INFO(r);
        if (!r) {
                LOG_ERROR("riscv_info is NULL!");
                return ERROR_FAIL;
        }

        if (r->authdata_read) {
                uint32_t value;
                if (r->authdata_read(target, &value, index) != ERROR_OK)
                        return ERROR_FAIL;
                command_print_sameline(CMD, "0x%08" PRIx32, value);
                return ERROR_OK;
        } else {
                LOG_ERROR("authdata_read is not implemented for this target.");
                return ERROR_FAIL;
        }
}

COMMAND_HANDLER(riscv_authdata_write)
{
        uint32_t value;
        unsigned int index = 0;

        if (CMD_ARGC == 0) {
                /* nop */
        } else if (CMD_ARGC == 1) {
                COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], value);
        } else if (CMD_ARGC == 2) {
                COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], index);
                COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
        } else {
                LOG_ERROR("Command takes at most 2 arguments");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        struct target *target = get_current_target(CMD_CTX);
        RISCV_INFO(r);

        if (r->authdata_write) {
                return r->authdata_write(target, value, index);
        } else {
                LOG_ERROR("authdata_write is not implemented for this target.");
                return ERROR_FAIL;
        }
}

COMMAND_HANDLER(riscv_dmi_read)
{
        if (CMD_ARGC != 1) {
                LOG_ERROR("Command takes 1 parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        struct target *target = get_current_target(CMD_CTX);
        if (!target) {
                LOG_ERROR("target is NULL!");
                return ERROR_FAIL;
        }

        RISCV_INFO(r);
        if (!r) {
                LOG_ERROR("riscv_info is NULL!");
                return ERROR_FAIL;
        }

        if (r->dmi_read) {
                uint32_t address, value;
                COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
                if (r->dmi_read(target, &value, address) != ERROR_OK)
                        return ERROR_FAIL;
                command_print(CMD, "0x%" PRIx32, value);
                return ERROR_OK;
        } else {
                LOG_ERROR("dmi_read is not implemented for this target.");
                return ERROR_FAIL;
        }
}


COMMAND_HANDLER(riscv_dmi_write)
{
        if (CMD_ARGC != 2) {
                LOG_ERROR("Command takes exactly 2 arguments");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        struct target *target = get_current_target(CMD_CTX);
        RISCV_INFO(r);

        uint32_t address, value;
        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);

        if (r->dmi_write) {
                return r->dmi_write(target, address, value);
        } else {
                LOG_ERROR("dmi_write is not implemented for this target.");
                return ERROR_FAIL;
        }
}

COMMAND_HANDLER(riscv_test_sba_config_reg)
{
        if (CMD_ARGC != 4) {
                LOG_ERROR("Command takes exactly 4 arguments");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        struct target *target = get_current_target(CMD_CTX);
        RISCV_INFO(r);

        target_addr_t legal_address;
        uint32_t num_words;
        target_addr_t illegal_address;
        bool run_sbbusyerror_test;

        COMMAND_PARSE_NUMBER(target_addr, CMD_ARGV[0], legal_address);
        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], num_words);
        COMMAND_PARSE_NUMBER(target_addr, CMD_ARGV[2], illegal_address);
        COMMAND_PARSE_ON_OFF(CMD_ARGV[3], run_sbbusyerror_test);

        if (r->test_sba_config_reg) {
                return r->test_sba_config_reg(target, legal_address, num_words,
                                illegal_address, run_sbbusyerror_test);
        } else {
                LOG_ERROR("test_sba_config_reg is not implemented for this target.");
                return ERROR_FAIL;
        }
}

COMMAND_HANDLER(riscv_reset_delays)
{
        int wait = 0;

        if (CMD_ARGC > 1) {
                LOG_ERROR("Command takes at most one argument");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        if (CMD_ARGC == 1)
                COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], wait);

        struct target *target = get_current_target(CMD_CTX);
        RISCV_INFO(r);
        r->reset_delays_wait = wait;
        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_ir)
{
        if (CMD_ARGC != 2) {
                LOG_ERROR("Command takes exactly 2 arguments");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        uint32_t value;
        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);

        if (!strcmp(CMD_ARGV[0], "idcode"))
                buf_set_u32(ir_idcode, 0, 32, value);
        else if (!strcmp(CMD_ARGV[0], "dtmcs"))
                buf_set_u32(ir_dtmcontrol, 0, 32, value);
        else if (!strcmp(CMD_ARGV[0], "dmi"))
                buf_set_u32(ir_dbus, 0, 32, value);
        else
                return ERROR_FAIL;

        return ERROR_OK;
}

COMMAND_HANDLER(riscv_resume_order)
{
        if (CMD_ARGC > 1) {
                LOG_ERROR("Command takes at most one argument");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }

        if (!strcmp(CMD_ARGV[0], "normal")) {
                resume_order = RO_NORMAL;
        } else if (!strcmp(CMD_ARGV[0], "reversed")) {
                resume_order = RO_REVERSED;
        } else {
                LOG_ERROR("Unsupported resume order: %s", CMD_ARGV[0]);
                return ERROR_FAIL;
        }

        return ERROR_OK;
}

COMMAND_HANDLER(riscv_use_bscan_tunnel)
{
        int irwidth = 0;
        int tunnel_type = BSCAN_TUNNEL_NESTED_TAP;

        if (CMD_ARGC > 2) {
                LOG_ERROR("Command takes at most two arguments");
                return ERROR_COMMAND_SYNTAX_ERROR;
        } else if (CMD_ARGC == 1) {
                COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], irwidth);
        } else if (CMD_ARGC == 2) {
                COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], irwidth);
                COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], tunnel_type);
        }
        if (tunnel_type == BSCAN_TUNNEL_NESTED_TAP)
                LOG_INFO("Nested Tap based Bscan Tunnel Selected");
        else if (tunnel_type == BSCAN_TUNNEL_DATA_REGISTER)
                LOG_INFO("Simple Register based Bscan Tunnel Selected");
        else
                LOG_INFO("Invalid Tunnel type selected ! : selecting default Nested Tap Type");

        bscan_tunnel_type = tunnel_type;
        bscan_tunnel_ir_width = irwidth;
        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_enable_virt2phys)
{
        if (CMD_ARGC != 1) {
                LOG_ERROR("Command takes exactly 1 parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
        COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_enable_virt2phys);
        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_ebreakm)
{
        if (CMD_ARGC != 1) {
                LOG_ERROR("Command takes exactly 1 parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
        COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_ebreakm);
        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_ebreaks)
{
        if (CMD_ARGC != 1) {
                LOG_ERROR("Command takes exactly 1 parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
        COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_ebreaks);
        return ERROR_OK;
}

COMMAND_HANDLER(riscv_set_ebreaku)
{
        if (CMD_ARGC != 1) {
                LOG_ERROR("Command takes exactly 1 parameter");
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
        COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_ebreaku);
        return ERROR_OK;
}

COMMAND_HELPER(riscv_print_info_line, const char *section, const char *key,
                           unsigned int value)
{
        char full_key[80];
        snprintf(full_key, sizeof(full_key), "%s.%s", section, key);
        command_print(CMD, "%-21s %3d", full_key, value);
        return 0;
}

COMMAND_HANDLER(handle_info)
{
        struct target *target = get_current_target(CMD_CTX);
        RISCV_INFO(r);

        /* This output format can be fed directly into TCL's "array set". */

        riscv_print_info_line(CMD, "hart", "xlen", riscv_xlen(target));
        riscv_enumerate_triggers(target);
        riscv_print_info_line(CMD, "hart", "trigger_count",
                                                  r->trigger_count);

        if (r->print_info)
                return CALL_COMMAND_HANDLER(r->print_info, target);

        return 0;
}

static const struct command_registration riscv_exec_command_handlers[] = {
        {
                .name = "info",
                .handler = handle_info,
                .mode = COMMAND_EXEC,
                .usage = "",
                .help = "Displays some information OpenOCD detected about the target."
        },
        {
                .name = "set_command_timeout_sec",
                .handler = riscv_set_command_timeout_sec,
                .mode = COMMAND_ANY,
                .usage = "[sec]",
                .help = "Set the wall-clock timeout (in seconds) for individual commands"
        },
        {
                .name = "set_reset_timeout_sec",
                .handler = riscv_set_reset_timeout_sec,
                .mode = COMMAND_ANY,
                .usage = "[sec]",
                .help = "Set the wall-clock timeout (in seconds) after reset is deasserted"
        },
        {
                .name = "set_prefer_sba",
                .handler = riscv_set_prefer_sba,
                .mode = COMMAND_ANY,
                .usage = "on|off",
                .help = "When on, prefer to use System Bus Access to access memory. "
                        "When off (default), prefer to use the Program Buffer to access memory."
        },
        {
                .name = "set_mem_access",
                .handler = riscv_set_mem_access,
                .mode = COMMAND_ANY,
                .usage = "method1 [method2] [method3]",
                .help = "Set which memory access methods shall be used and in which order "
                        "of priority. Method can be one of: 'progbuf', 'sysbus' or 'abstract'."
        },
        {
                .name = "set_enable_virtual",
                .handler = riscv_set_enable_virtual,
                .mode = COMMAND_ANY,
                .usage = "on|off",
                .help = "When on, memory accesses are performed on physical or virtual "
                                "memory depending on the current system configuration. "
                                "When off (default), all memory accessses are performed on physical memory."
        },
        {
                .name = "expose_csrs",
                .handler = riscv_set_expose_csrs,
                .mode = COMMAND_CONFIG,
                .usage = "n0[-m0|=name0][,n1[-m1|=name1]]...",
                .help = "Configure a list of inclusive ranges for CSRs to expose in "
                                "addition to the standard ones. This must be executed before "
                                "`init`."
        },
        {
                .name = "expose_custom",
                .handler = riscv_set_expose_custom,
                .mode = COMMAND_CONFIG,
                .usage = "n0[-m0|=name0][,n1[-m1|=name1]]...",
                .help = "Configure a list of inclusive ranges for custom registers to "
                        "expose. custom0 is accessed as abstract register number 0xc000, "
                        "etc. This must be executed before `init`."
        },
        {
                .name = "authdata_read",
                .handler = riscv_authdata_read,
                .usage = "[index]",
                .mode = COMMAND_ANY,
                .help = "Return the 32-bit value read from authdata or authdata0 "
                                "(index=0), or authdata1 (index=1)."
        },
        {
                .name = "authdata_write",
                .handler = riscv_authdata_write,
                .mode = COMMAND_ANY,
                .usage = "[index] value",
                .help = "Write the 32-bit value to authdata or authdata0 (index=0), "
                                "or authdata1 (index=1)."
        },
        {
                .name = "dmi_read",
                .handler = riscv_dmi_read,
                .mode = COMMAND_ANY,
                .usage = "address",
                .help = "Perform a 32-bit DMI read at address, returning the value."
        },
        {
                .name = "dmi_write",
                .handler = riscv_dmi_write,
                .mode = COMMAND_ANY,
                .usage = "address value",
                .help = "Perform a 32-bit DMI write of value at address."
        },
        {
                .name = "test_sba_config_reg",
                .handler = riscv_test_sba_config_reg,
                .mode = COMMAND_ANY,
                .usage = "legal_address num_words "
                        "illegal_address run_sbbusyerror_test[on/off]",
                .help = "Perform a series of tests on the SBCS register. "
                        "Inputs are a legal, 128-byte aligned address and a number of words to "
                        "read/write starting at that address (i.e., address range [legal address, "
                        "legal_address+word_size*num_words) must be legally readable/writable), "
                        "an illegal, 128-byte aligned address for error flag/handling cases, "
                        "and whether sbbusyerror test should be run."
        },
        {
                .name = "reset_delays",
                .handler = riscv_reset_delays,
                .mode = COMMAND_ANY,
                .usage = "[wait]",
                .help = "OpenOCD learns how many Run-Test/Idle cycles are required "
                        "between scans to avoid encountering the target being busy. This "
                        "command resets those learned values after `wait` scans. It's only "
                        "useful for testing OpenOCD itself."
        },
        {
                .name = "resume_order",
                .handler = riscv_resume_order,
                .mode = COMMAND_ANY,
                .usage = "normal|reversed",
                .help = "Choose the order that harts are resumed in when `hasel` is not "
                        "supported. Normal order is from lowest hart index to highest. "
                        "Reversed order is from highest hart index to lowest."
        },
        {
                .name = "set_ir",
                .handler = riscv_set_ir,
                .mode = COMMAND_ANY,
                .usage = "[idcode|dtmcs|dmi] value",
                .help = "Set IR value for specified JTAG register."
        },
        {
                .name = "use_bscan_tunnel",
                .handler = riscv_use_bscan_tunnel,
                .mode = COMMAND_ANY,
                .usage = "value [type]",
                .help = "Enable or disable use of a BSCAN tunnel to reach DM.  Supply "
                        "the width of the DM transport TAP's instruction register to "
                        "enable.  Supply a value of 0 to disable. Pass A second argument "
                        "(optional) to indicate Bscan Tunnel Type {0:(default) NESTED_TAP , "
                        "1: DATA_REGISTER}"
        },
        {
                .name = "set_enable_virt2phys",
                .handler = riscv_set_enable_virt2phys,
                .mode = COMMAND_ANY,
                .usage = "on|off",
                .help = "When on (default), enable translation from virtual address to "
                        "physical address."
        },
        {
                .name = "set_ebreakm",
                .handler = riscv_set_ebreakm,
                .mode = COMMAND_ANY,
                .usage = "on|off",
                .help = "Control dcsr.ebreakm. When off, M-mode ebreak instructions "
                        "don't trap to OpenOCD. Defaults to on."
        },
        {
                .name = "set_ebreaks",
                .handler = riscv_set_ebreaks,
                .mode = COMMAND_ANY,
                .usage = "on|off",
                .help = "Control dcsr.ebreaks. When off, S-mode ebreak instructions "
                        "don't trap to OpenOCD. Defaults to on."
        },
        {
                .name = "set_ebreaku",
                .handler = riscv_set_ebreaku,
                .mode = COMMAND_ANY,
                .usage = "on|off",
                .help = "Control dcsr.ebreaku. When off, U-mode ebreak instructions "
                        "don't trap to OpenOCD. Defaults to on."
        },
        COMMAND_REGISTRATION_DONE
};

/*
 * To be noted that RISC-V targets use the same semihosting commands as
 * ARM targets.
 *
 * The main reason is compatibility with existing tools. For example the
 * Eclipse OpenOCD/SEGGER J-Link/QEMU plug-ins have several widgets to
 * configure semihosting, which generate commands like `arm semihosting
 * enable`.
 * A secondary reason is the fact that the protocol used is exactly the
 * one specified by ARM. If RISC-V will ever define its own semihosting
 * protocol, then a command like `riscv semihosting enable` will make
 * sense, but for now all semihosting commands are prefixed with `arm`.
 */
extern const struct command_registration semihosting_common_handlers[];

const struct command_registration riscv_command_handlers[] = {
        {
                .name = "riscv",
                .mode = COMMAND_ANY,
                .help = "RISC-V Command Group",
                .usage = "",
                .chain = riscv_exec_command_handlers
        },
        {
                .name = "arm",
                .mode = COMMAND_ANY,
                .help = "ARM Command Group",
                .usage = "",
                .chain = semihosting_common_handlers
        },
        COMMAND_REGISTRATION_DONE
};

static unsigned riscv_xlen_nonconst(struct target *target)
{
        return riscv_xlen(target);
}

static unsigned int riscv_data_bits(struct target *target)
{
        RISCV_INFO(r);
        if (r->data_bits)
                return r->data_bits(target);
        return riscv_xlen(target);
}

struct target_type riscv_target = {
        .name = "riscv",

        .target_create = riscv_create_target,
        .init_target = riscv_init_target,
        .deinit_target = riscv_deinit_target,
        .examine = riscv_examine,

        /* poll current target status */
        .poll = old_or_new_riscv_poll,

        .halt = riscv_halt,
        .resume = riscv_target_resume,
        .step = old_or_new_riscv_step,

        .assert_reset = riscv_assert_reset,
        .deassert_reset = riscv_deassert_reset,

        .read_memory = riscv_read_memory,
        .write_memory = riscv_write_memory,
        .read_phys_memory = riscv_read_phys_memory,
        .write_phys_memory = riscv_write_phys_memory,

        .checksum_memory = riscv_checksum_memory,

        .mmu = riscv_mmu,
        .virt2phys = riscv_virt2phys,

        .get_gdb_arch = riscv_get_gdb_arch,
        .get_gdb_reg_list = riscv_get_gdb_reg_list,
        .get_gdb_reg_list_noread = riscv_get_gdb_reg_list_noread,

        .add_breakpoint = riscv_add_breakpoint,
        .remove_breakpoint = riscv_remove_breakpoint,

        .add_watchpoint = riscv_add_watchpoint,
        .remove_watchpoint = riscv_remove_watchpoint,
        .hit_watchpoint = riscv_hit_watchpoint,

        .arch_state = riscv_arch_state,

        .run_algorithm = riscv_run_algorithm,

        .commands = riscv_command_handlers,

        .address_bits = riscv_xlen_nonconst,
        .data_bits = riscv_data_bits
};

/*** RISC-V Interface ***/

void riscv_info_init(struct target *target, riscv_info_t *r)
{
        memset(r, 0, sizeof(*r));
        r->dtm_version = 1;
        r->registers_initialized = false;
        r->current_hartid = target->coreid;
        r->version_specific = NULL;

        memset(r->trigger_unique_id, 0xff, sizeof(r->trigger_unique_id));

        r->xlen = -1;

        r->mem_access_methods[0] = RISCV_MEM_ACCESS_PROGBUF;
        r->mem_access_methods[1] = RISCV_MEM_ACCESS_SYSBUS;
        r->mem_access_methods[2] = RISCV_MEM_ACCESS_ABSTRACT;

        r->mem_access_progbuf_warn = true;
        r->mem_access_sysbus_warn = true;
        r->mem_access_abstract_warn = true;

        INIT_LIST_HEAD(&r->expose_csr);
        INIT_LIST_HEAD(&r->expose_custom);
}

static int riscv_resume_go_all_harts(struct target *target)
{
        RISCV_INFO(r);

        LOG_DEBUG("[%s] resuming hart", target_name(target));
        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;
        if (riscv_is_halted(target)) {
                if (r->resume_go(target) != ERROR_OK)
                        return ERROR_FAIL;
        } else {
                LOG_DEBUG("[%s] hart requested resume, but was already resumed",
                                target_name(target));
        }

        riscv_invalidate_register_cache(target);
        return ERROR_OK;
}

int riscv_step_rtos_hart(struct target *target)
{
        RISCV_INFO(r);
        if (riscv_select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;
        LOG_DEBUG("[%s] stepping", target_name(target));

        if (!riscv_is_halted(target)) {
                LOG_ERROR("Hart isn't halted before single step!");
                return ERROR_FAIL;
        }
        riscv_invalidate_register_cache(target);
        r->on_step(target);
        if (r->step_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;
        riscv_invalidate_register_cache(target);
        r->on_halt(target);
        if (!riscv_is_halted(target)) {
                LOG_ERROR("Hart was not halted after single step!");
                return ERROR_FAIL;
        }
        return ERROR_OK;
}

bool riscv_supports_extension(struct target *target, char letter)
{
        RISCV_INFO(r);
        unsigned num;
        if (letter >= 'a' && letter <= 'z')
                num = letter - 'a';
        else if (letter >= 'A' && letter <= 'Z')
                num = letter - 'A';
        else
                return false;
        return r->misa & BIT(num);
}

unsigned riscv_xlen(const struct target *target)
{
        RISCV_INFO(r);
        return r->xlen;
}

int riscv_set_current_hartid(struct target *target, int hartid)
{
        RISCV_INFO(r);
        if (!r->select_current_hart)
                return ERROR_OK;

        int previous_hartid = riscv_current_hartid(target);
        r->current_hartid = hartid;
        LOG_DEBUG("setting hartid to %d, was %d", hartid, previous_hartid);
        if (r->select_current_hart(target) != ERROR_OK)
                return ERROR_FAIL;

        return ERROR_OK;
}

void riscv_invalidate_register_cache(struct target *target)
{
        RISCV_INFO(r);

        LOG_DEBUG("[%d]", target->coreid);
        register_cache_invalidate(target->reg_cache);
        for (size_t i = 0; i < target->reg_cache->num_regs; ++i) {
                struct reg *reg = &target->reg_cache->reg_list[i];
                reg->valid = false;
        }

        r->registers_initialized = true;
}

int riscv_current_hartid(const struct target *target)
{
        RISCV_INFO(r);
        return r->current_hartid;
}

int riscv_count_harts(struct target *target)
{
        if (!target)
                return 1;
        RISCV_INFO(r);
        if (!r || !r->hart_count)
                return 1;
        return r->hart_count(target);
}

/**
 * If write is true:
 *   return true iff we are guaranteed that the register will contain exactly
 *       the value we just wrote when it's read.
 * If write is false:
 *   return true iff we are guaranteed that the register will read the same
 *       value in the future as the value we just read.
 */
static bool gdb_regno_cacheable(enum gdb_regno regno, bool write)
{
        /* GPRs, FPRs, vector registers are just normal data stores. */
        if (regno <= GDB_REGNO_XPR31 ||
                        (regno >= GDB_REGNO_FPR0 && regno <= GDB_REGNO_FPR31) ||
                        (regno >= GDB_REGNO_V0 && regno <= GDB_REGNO_V31))
                return true;

        /* Most CSRs won't change value on us, but we can't assume it about arbitrary
         * CSRs. */
        switch (regno) {
                case GDB_REGNO_DPC:
                        return true;

                case GDB_REGNO_VSTART:
                case GDB_REGNO_VXSAT:
                case GDB_REGNO_VXRM:
                case GDB_REGNO_VLENB:
                case GDB_REGNO_VL:
                case GDB_REGNO_VTYPE:
                case GDB_REGNO_MISA:
                case GDB_REGNO_DCSR:
                case GDB_REGNO_DSCRATCH0:
                case GDB_REGNO_MSTATUS:
                case GDB_REGNO_MEPC:
                case GDB_REGNO_MCAUSE:
                case GDB_REGNO_SATP:
                        /*
                         * WARL registers might not contain the value we just wrote, but
                         * these ones won't spontaneously change their value either. *
                         */
                        return !write;

                case GDB_REGNO_TSELECT: /* I think this should be above, but then it doesn't work. */
                case GDB_REGNO_TDATA1:  /* Changes value when tselect is changed. */
                case GDB_REGNO_TDATA2:  /* Changse value when tselect is changed. */
                default:
                        return false;
        }
}

/**
 * This function is called when the debug user wants to change the value of a
 * register. The new value may be cached, and may not be written until the hart
 * is resumed. */
int riscv_set_register(struct target *target, enum gdb_regno regid, riscv_reg_t value)
{
        RISCV_INFO(r);
        LOG_DEBUG("[%s] %s <- %" PRIx64, target_name(target), gdb_regno_name(regid), value);
        assert(r->set_register);

        keep_alive();

        /* TODO: Hack to deal with gdb that thinks these registers still exist. */
        if (regid > GDB_REGNO_XPR15 && regid <= GDB_REGNO_XPR31 && value == 0 &&
                        riscv_supports_extension(target, 'E'))
                return ERROR_OK;

        struct reg *reg = &target->reg_cache->reg_list[regid];
        buf_set_u64(reg->value, 0, reg->size, value);

        int result = r->set_register(target, regid, value);
        if (result == ERROR_OK)
                reg->valid = gdb_regno_cacheable(regid, true);
        else
                reg->valid = false;
        LOG_DEBUG("[%s] wrote 0x%" PRIx64 " to %s valid=%d",
                          target_name(target), value, reg->name, reg->valid);
        return result;
}

int riscv_get_register(struct target *target, riscv_reg_t *value,
                enum gdb_regno regid)
{
        RISCV_INFO(r);

        keep_alive();

        struct reg *reg = &target->reg_cache->reg_list[regid];
        if (!reg->exist) {
                LOG_DEBUG("[%s] %s does not exist.",
                                  target_name(target), gdb_regno_name(regid));
                return ERROR_FAIL;
        }

        if (reg && reg->valid) {
                *value = buf_get_u64(reg->value, 0, reg->size);
                LOG_DEBUG("[%s] %s: %" PRIx64 " (cached)", target_name(target),
                                  gdb_regno_name(regid), *value);
                return ERROR_OK;
        }

        /* TODO: Hack to deal with gdb that thinks these registers still exist. */
        if (regid > GDB_REGNO_XPR15 && regid <= GDB_REGNO_XPR31 &&
                        riscv_supports_extension(target, 'E')) {
                *value = 0;
                return ERROR_OK;
        }

        int result = r->get_register(target, value, regid);

        if (result == ERROR_OK)
                reg->valid = gdb_regno_cacheable(regid, false);

        LOG_DEBUG("[%s] %s: %" PRIx64, target_name(target),
                        gdb_regno_name(regid), *value);
        return result;
}

bool riscv_is_halted(struct target *target)
{
        RISCV_INFO(r);
        assert(r->is_halted);
        return r->is_halted(target);
}

enum riscv_halt_reason riscv_halt_reason(struct target *target, int hartid)
{
        RISCV_INFO(r);
        if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
                return RISCV_HALT_ERROR;
        if (!riscv_is_halted(target)) {
                LOG_ERROR("Hart is not halted!");
                return RISCV_HALT_UNKNOWN;
        }
        return r->halt_reason(target);
}

size_t riscv_debug_buffer_size(struct target *target)
{
        RISCV_INFO(r);
        return r->debug_buffer_size;
}

int riscv_write_debug_buffer(struct target *target, int index, riscv_insn_t insn)
{
        RISCV_INFO(r);
        r->write_debug_buffer(target, index, insn);
        return ERROR_OK;
}

riscv_insn_t riscv_read_debug_buffer(struct target *target, int index)
{
        RISCV_INFO(r);
        return r->read_debug_buffer(target, index);
}

int riscv_execute_debug_buffer(struct target *target)
{
        RISCV_INFO(r);
        return r->execute_debug_buffer(target);
}

void riscv_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d)
{
        RISCV_INFO(r);
        r->fill_dmi_write_u64(target, buf, a, d);
}

void riscv_fill_dmi_read_u64(struct target *target, char *buf, int a)
{
        RISCV_INFO(r);
        r->fill_dmi_read_u64(target, buf, a);
}

void riscv_fill_dmi_nop_u64(struct target *target, char *buf)
{
        RISCV_INFO(r);
        r->fill_dmi_nop_u64(target, buf);
}

int riscv_dmi_write_u64_bits(struct target *target)
{
        RISCV_INFO(r);
        return r->dmi_write_u64_bits(target);
}

/**
 * Count triggers, and initialize trigger_count for each hart.
 * trigger_count is initialized even if this function fails to d