git.gag.com Git - fw/openocd/blob - src/target/riscv/riscv.c

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2
   3 #include <assert.h>
   4 #include <stdlib.h>
   5 #include <time.h>
   6
   7 #ifdef HAVE_CONFIG_H
   8 #include "config.h"
   9 #endif
  10
  11 #include "target/target.h"
  12 #include "target/algorithm.h"
  13 #include "target/target_type.h"
  14 #include "log.h"
  15 #include "jtag/jtag.h"
  16 #include "target/register.h"
  17 #include "target/breakpoints.h"
  18 #include "helper/time_support.h"
  19 #include "riscv.h"
  20 #include "gdb_regs.h"
  21 #include "rtos/rtos.h"
  22
  23 #define get_field(reg, mask) (((reg) & (mask)) / ((mask) & ~((mask) << 1)))
  24 #define set_field(reg, mask, val) (((reg) & ~(mask)) | (((val) * ((mask) & ~((mask) << 1))) & (mask)))
  25
  26 /* Constants for legacy SiFive hardware breakpoints. */
  27 #define CSR_BPCONTROL_X                 (1<<0)
  28 #define CSR_BPCONTROL_W                 (1<<1)
  29 #define CSR_BPCONTROL_R                 (1<<2)
  30 #define CSR_BPCONTROL_U                 (1<<3)
  31 #define CSR_BPCONTROL_S                 (1<<4)
  32 #define CSR_BPCONTROL_H                 (1<<5)
  33 #define CSR_BPCONTROL_M                 (1<<6)
  34 #define CSR_BPCONTROL_BPMATCH   (0xf<<7)
  35 #define CSR_BPCONTROL_BPACTION  (0xff<<11)
  36
  37 #define DEBUG_ROM_START         0x800
  38 #define DEBUG_ROM_RESUME        (DEBUG_ROM_START + 4)
  39 #define DEBUG_ROM_EXCEPTION     (DEBUG_ROM_START + 8)
  40 #define DEBUG_RAM_START         0x400
  41
  42 #define SETHALTNOT                              0x10c
  43
  44 /*** JTAG registers. ***/
  45
  46 #define DTMCONTROL                                      0x10
  47 #define DTMCONTROL_DBUS_RESET           (1<<16)
  48 #define DTMCONTROL_IDLE                         (7<<10)
  49 #define DTMCONTROL_ADDRBITS                     (0xf<<4)
  50 #define DTMCONTROL_VERSION                      (0xf)
  51
  52 #define DBUS                                            0x11
  53 #define DBUS_OP_START                           0
  54 #define DBUS_OP_SIZE                            2
  55 typedef enum {
  56         DBUS_OP_NOP = 0,
  57         DBUS_OP_READ = 1,
  58         DBUS_OP_WRITE = 2
  59 } dbus_op_t;
  60 typedef enum {
  61         DBUS_STATUS_SUCCESS = 0,
  62         DBUS_STATUS_FAILED = 2,
  63         DBUS_STATUS_BUSY = 3
  64 } dbus_status_t;
  65 #define DBUS_DATA_START                         2
  66 #define DBUS_DATA_SIZE                          34
  67 #define DBUS_ADDRESS_START                      36
  68
  69 typedef enum slot {
  70         SLOT0,
  71         SLOT1,
  72         SLOT_LAST,
  73 } slot_t;
  74
  75 /*** Debug Bus registers. ***/
  76
  77 #define DMCONTROL                               0x10
  78 #define DMCONTROL_INTERRUPT             (((uint64_t)1)<<33)
  79 #define DMCONTROL_HALTNOT               (((uint64_t)1)<<32)
  80 #define DMCONTROL_BUSERROR              (7<<19)
  81 #define DMCONTROL_SERIAL                (3<<16)
  82 #define DMCONTROL_AUTOINCREMENT (1<<15)
  83 #define DMCONTROL_ACCESS                (7<<12)
  84 #define DMCONTROL_HARTID                (0x3ff<<2)
  85 #define DMCONTROL_NDRESET               (1<<1)
  86 #define DMCONTROL_FULLRESET             1
  87
  88 #define DMINFO                                  0x11
  89 #define DMINFO_ABUSSIZE                 (0x7fU<<25)
  90 #define DMINFO_SERIALCOUNT              (0xf<<21)
  91 #define DMINFO_ACCESS128                (1<<20)
  92 #define DMINFO_ACCESS64                 (1<<19)
  93 #define DMINFO_ACCESS32                 (1<<18)
  94 #define DMINFO_ACCESS16                 (1<<17)
  95 #define DMINFO_ACCESS8                  (1<<16)
  96 #define DMINFO_DRAMSIZE                 (0x3f<<10)
  97 #define DMINFO_AUTHENTICATED    (1<<5)
  98 #define DMINFO_AUTHBUSY                 (1<<4)
  99 #define DMINFO_AUTHTYPE                 (3<<2)
 100 #define DMINFO_VERSION                  3
 101
 102 /*** Info about the core being debugged. ***/
 103
 104 #define DBUS_ADDRESS_UNKNOWN    0xffff
 105
 106 #define MAX_HWBPS                       16
 107 #define DRAM_CACHE_SIZE         16
 108
 109 uint8_t ir_dtmcontrol[4] = {DTMCONTROL};
 110 struct scan_field select_dtmcontrol = {
 111         .in_value = NULL,
 112         .out_value = ir_dtmcontrol
 113 };
 114 uint8_t ir_dbus[4] = {DBUS};
 115 struct scan_field select_dbus = {
 116         .in_value = NULL,
 117         .out_value = ir_dbus
 118 };
 119 uint8_t ir_idcode[4] = {0x1};
 120 struct scan_field select_idcode = {
 121         .in_value = NULL,
 122         .out_value = ir_idcode
 123 };
 124
 125 bscan_tunnel_type_t bscan_tunnel_type;
 126 int bscan_tunnel_ir_width; /* if zero, then tunneling is not present/active */
 127
 128 static uint8_t bscan_zero[4] = {0};
 129 static uint8_t bscan_one[4] = {1};
 130
 131 uint8_t ir_user4[4] = {0x23};
 132 struct scan_field select_user4 = {
 133         .in_value = NULL,
 134         .out_value = ir_user4
 135 };
 136
 137
 138 uint8_t bscan_tunneled_ir_width[4] = {5};  /* overridden by assignment in riscv_init_target */
 139 struct scan_field _bscan_tunnel_data_register_select_dmi[] = {
 140                 {
 141                         .num_bits = 3,
 142                         .out_value = bscan_zero,
 143                         .in_value = NULL,
 144                 },
 145                 {
 146                         .num_bits = 5, /* initialized in riscv_init_target to ir width of DM */
 147                         .out_value = ir_dbus,
 148                         .in_value = NULL,
 149                 },
 150                 {
 151                         .num_bits = 7,
 152                         .out_value = bscan_tunneled_ir_width,
 153                         .in_value = NULL,
 154                 },
 155                 {
 156                         .num_bits = 1,
 157                         .out_value = bscan_zero,
 158                         .in_value = NULL,
 159                 }
 160 };
 161
 162 struct scan_field _bscan_tunnel_nested_tap_select_dmi[] = {
 163                 {
 164                         .num_bits = 1,
 165                         .out_value = bscan_zero,
 166                         .in_value = NULL,
 167                 },
 168                 {
 169                         .num_bits = 7,
 170                         .out_value = bscan_tunneled_ir_width,
 171                         .in_value = NULL,
 172                 },
 173                 {
 174                         .num_bits = 0, /* initialized in riscv_init_target to ir width of DM */
 175                         .out_value = ir_dbus,
 176                         .in_value = NULL,
 177                 },
 178                 {
 179                         .num_bits = 3,
 180                         .out_value = bscan_zero,
 181                         .in_value = NULL,
 182                 }
 183 };
 184 struct scan_field *bscan_tunnel_nested_tap_select_dmi = _bscan_tunnel_nested_tap_select_dmi;
 185 uint32_t bscan_tunnel_nested_tap_select_dmi_num_fields = ARRAY_SIZE(_bscan_tunnel_nested_tap_select_dmi);
 186
 187 struct scan_field *bscan_tunnel_data_register_select_dmi = _bscan_tunnel_data_register_select_dmi;
 188 uint32_t bscan_tunnel_data_register_select_dmi_num_fields = ARRAY_SIZE(_bscan_tunnel_data_register_select_dmi);
 189
 190 struct trigger {
 191         uint64_t address;
 192         uint32_t length;
 193         uint64_t mask;
 194         uint64_t value;
 195         bool read, write, execute;
 196         int unique_id;
 197 };
 198
 199 /* Wall-clock timeout for a command/access. Settable via RISC-V Target commands.*/
 200 int riscv_command_timeout_sec = DEFAULT_COMMAND_TIMEOUT_SEC;
 201
 202 /* Wall-clock timeout after reset. Settable via RISC-V Target commands.*/
 203 int riscv_reset_timeout_sec = DEFAULT_RESET_TIMEOUT_SEC;
 204
 205 bool riscv_prefer_sba;
 206 bool riscv_enable_virt2phys = true;
 207 bool riscv_ebreakm = true;
 208 bool riscv_ebreaks = true;
 209 bool riscv_ebreaku = true;
 210
 211 bool riscv_enable_virtual;
 212
 213 typedef struct {
 214         uint16_t low, high;
 215 } range_t;
 216
 217 /* In addition to the ones in the standard spec, we'll also expose additional
 218  * CSRs in this list.
 219  * The list is either NULL, or a series of ranges (inclusive), terminated with
 220  * 1,0. */
 221 range_t *expose_csr;
 222 /* Same, but for custom registers. */
 223 range_t *expose_custom;
 224
 225 static enum {
 226         RO_NORMAL,
 227         RO_REVERSED
 228 } resume_order;
 229
 230 virt2phys_info_t sv32 = {
 231         .name = "Sv32",
 232         .va_bits = 32,
 233         .level = 2,
 234         .pte_shift = 2,
 235         .vpn_shift = {12, 22},
 236         .vpn_mask = {0x3ff, 0x3ff},
 237         .pte_ppn_shift = {10, 20},
 238         .pte_ppn_mask = {0x3ff, 0xfff},
 239         .pa_ppn_shift = {12, 22},
 240         .pa_ppn_mask = {0x3ff, 0xfff},
 241 };
 242
 243 virt2phys_info_t sv39 = {
 244         .name = "Sv39",
 245         .va_bits = 39,
 246         .level = 3,
 247         .pte_shift = 3,
 248         .vpn_shift = {12, 21, 30},
 249         .vpn_mask = {0x1ff, 0x1ff, 0x1ff},
 250         .pte_ppn_shift = {10, 19, 28},
 251         .pte_ppn_mask = {0x1ff, 0x1ff, 0x3ffffff},
 252         .pa_ppn_shift = {12, 21, 30},
 253         .pa_ppn_mask = {0x1ff, 0x1ff, 0x3ffffff},
 254 };
 255
 256 virt2phys_info_t sv48 = {
 257         .name = "Sv48",
 258         .va_bits = 48,
 259         .level = 4,
 260         .pte_shift = 3,
 261         .vpn_shift = {12, 21, 30, 39},
 262         .vpn_mask = {0x1ff, 0x1ff, 0x1ff, 0x1ff},
 263         .pte_ppn_shift = {10, 19, 28, 37},
 264         .pte_ppn_mask = {0x1ff, 0x1ff, 0x1ff, 0x1ffff},
 265         .pa_ppn_shift = {12, 21, 30, 39},
 266         .pa_ppn_mask = {0x1ff, 0x1ff, 0x1ff, 0x1ffff},
 267 };
 268
 269 static int riscv_resume_go_all_harts(struct target *target);
 270
 271 void select_dmi_via_bscan(struct target *target)
 272 {
 273         jtag_add_ir_scan(target->tap, &select_user4, TAP_IDLE);
 274         if (bscan_tunnel_type == BSCAN_TUNNEL_DATA_REGISTER)
 275                 jtag_add_dr_scan(target->tap, bscan_tunnel_data_register_select_dmi_num_fields,
 276                                                                                 bscan_tunnel_data_register_select_dmi, TAP_IDLE);
 277         else /* BSCAN_TUNNEL_NESTED_TAP */
 278                 jtag_add_dr_scan(target->tap, bscan_tunnel_nested_tap_select_dmi_num_fields,
 279                                                                                 bscan_tunnel_nested_tap_select_dmi, TAP_IDLE);
 280 }
 281
 282 uint32_t dtmcontrol_scan_via_bscan(struct target *target, uint32_t out)
 283 {
 284         /* On BSCAN TAP: Select IR=USER4, issue tunneled IR scan via BSCAN TAP's DR */
 285         uint8_t tunneled_ir_width[4] = {bscan_tunnel_ir_width};
 286         uint8_t tunneled_dr_width[4] = {32};
 287         uint8_t out_value[5] = {0};
 288         uint8_t in_value[5] = {0};
 289
 290         buf_set_u32(out_value, 0, 32, out);
 291         struct scan_field tunneled_ir[4] = {};
 292         struct scan_field tunneled_dr[4] = {};
 293
 294         if (bscan_tunnel_type == BSCAN_TUNNEL_DATA_REGISTER) {
 295                 tunneled_ir[0].num_bits = 3;
 296                 tunneled_ir[0].out_value = bscan_zero;
 297                 tunneled_ir[0].in_value = NULL;
 298                 tunneled_ir[1].num_bits = bscan_tunnel_ir_width;
 299                 tunneled_ir[1].out_value = ir_dtmcontrol;
 300                 tunneled_ir[1].in_value = NULL;
 301                 tunneled_ir[2].num_bits = 7;
 302                 tunneled_ir[2].out_value = tunneled_ir_width;
 303                 tunneled_ir[2].in_value = NULL;
 304                 tunneled_ir[3].num_bits = 1;
 305                 tunneled_ir[3].out_value = bscan_zero;
 306                 tunneled_ir[3].in_value = NULL;
 307
 308                 tunneled_dr[0].num_bits = 3;
 309                 tunneled_dr[0].out_value = bscan_zero;
 310                 tunneled_dr[0].in_value = NULL;
 311                 tunneled_dr[1].num_bits = 32 + 1;
 312                 tunneled_dr[1].out_value = out_value;
 313                 tunneled_dr[1].in_value = in_value;
 314                 tunneled_dr[2].num_bits = 7;
 315                 tunneled_dr[2].out_value = tunneled_dr_width;
 316                 tunneled_dr[2].in_value = NULL;
 317                 tunneled_dr[3].num_bits = 1;
 318                 tunneled_dr[3].out_value = bscan_one;
 319                 tunneled_dr[3].in_value = NULL;
 320         } else {
 321                 /* BSCAN_TUNNEL_NESTED_TAP */
 322                 tunneled_ir[3].num_bits = 3;
 323                 tunneled_ir[3].out_value = bscan_zero;
 324                 tunneled_ir[3].in_value = NULL;
 325                 tunneled_ir[2].num_bits = bscan_tunnel_ir_width;
 326                 tunneled_ir[2].out_value = ir_dtmcontrol;
 327                 tunneled_ir[1].in_value = NULL;
 328                 tunneled_ir[1].num_bits = 7;
 329                 tunneled_ir[1].out_value = tunneled_ir_width;
 330                 tunneled_ir[2].in_value = NULL;
 331                 tunneled_ir[0].num_bits = 1;
 332                 tunneled_ir[0].out_value = bscan_zero;
 333                 tunneled_ir[0].in_value = NULL;
 334
 335                 tunneled_dr[3].num_bits = 3;
 336                 tunneled_dr[3].out_value = bscan_zero;
 337                 tunneled_dr[3].in_value = NULL;
 338                 tunneled_dr[2].num_bits = 32 + 1;
 339                 tunneled_dr[2].out_value = out_value;
 340                 tunneled_dr[2].in_value = in_value;
 341                 tunneled_dr[1].num_bits = 7;
 342                 tunneled_dr[1].out_value = tunneled_dr_width;
 343                 tunneled_dr[1].in_value = NULL;
 344                 tunneled_dr[0].num_bits = 1;
 345                 tunneled_dr[0].out_value = bscan_one;
 346                 tunneled_dr[0].in_value = NULL;
 347         }
 348         jtag_add_ir_scan(target->tap, &select_user4, TAP_IDLE);
 349         jtag_add_dr_scan(target->tap, ARRAY_SIZE(tunneled_ir), tunneled_ir, TAP_IDLE);
 350         jtag_add_dr_scan(target->tap, ARRAY_SIZE(tunneled_dr), tunneled_dr, TAP_IDLE);
 351         select_dmi_via_bscan(target);
 352
 353         int retval = jtag_execute_queue();
 354         if (retval != ERROR_OK) {
 355                 LOG_ERROR("failed jtag scan: %d", retval);
 356                 return retval;
 357         }
 358         /* Note the starting offset is bit 1, not bit 0.  In BSCAN tunnel, there is a one-bit TCK skew between
 359            output and input */
 360         uint32_t in = buf_get_u32(in_value, 1, 32);
 361         LOG_DEBUG("DTMCS: 0x%x -> 0x%x", out, in);
 362
 363         return in;
 364 }
 365
 366
 367
 368 static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
 369 {
 370         struct scan_field field;
 371         uint8_t in_value[4];
 372         uint8_t out_value[4] = { 0 };
 373
 374         if (bscan_tunnel_ir_width != 0)
 375                 return dtmcontrol_scan_via_bscan(target, out);
 376
 377
 378         buf_set_u32(out_value, 0, 32, out);
 379
 380         jtag_add_ir_scan(target->tap, &select_dtmcontrol, TAP_IDLE);
 381
 382         field.num_bits = 32;
 383         field.out_value = out_value;
 384         field.in_value = in_value;
 385         jtag_add_dr_scan(target->tap, 1, &field, TAP_IDLE);
 386
 387         /* Always return to dbus. */
 388         jtag_add_ir_scan(target->tap, &select_dbus, TAP_IDLE);
 389
 390         int retval = jtag_execute_queue();
 391         if (retval != ERROR_OK) {
 392                 LOG_ERROR("failed jtag scan: %d", retval);
 393                 return retval;
 394         }
 395
 396         uint32_t in = buf_get_u32(field.in_value, 0, 32);
 397         LOG_DEBUG("DTMCONTROL: 0x%x -> 0x%x", out, in);
 398
 399         return in;
 400 }
 401
 402 static struct target_type *get_target_type(struct target *target)
 403 {
 404         riscv_info_t *info = (riscv_info_t *) target->arch_info;
 405
 406         if (!info) {
 407                 LOG_ERROR("Target has not been initialized");
 408                 return NULL;
 409         }
 410
 411         switch (info->dtm_version) {
 412                 case 0:
 413                         return &riscv011_target;
 414                 case 1:
 415                         return &riscv013_target;
 416                 default:
 417                         LOG_ERROR("Unsupported DTM version: %d", info->dtm_version);
 418                         return NULL;
 419         }
 420 }
 421
 422 static int riscv_init_target(struct command_context *cmd_ctx,
 423                 struct target *target)
 424 {
 425         LOG_DEBUG("riscv_init_target()");
 426         target->arch_info = calloc(1, sizeof(riscv_info_t));
 427         if (!target->arch_info)
 428                 return ERROR_FAIL;
 429         riscv_info_t *info = (riscv_info_t *) target->arch_info;
 430         riscv_info_init(target, info);
 431         info->cmd_ctx = cmd_ctx;
 432
 433         select_dtmcontrol.num_bits = target->tap->ir_length;
 434         select_dbus.num_bits = target->tap->ir_length;
 435         select_idcode.num_bits = target->tap->ir_length;
 436
 437         if (bscan_tunnel_ir_width != 0) {
 438                 select_user4.num_bits = target->tap->ir_length;
 439                 bscan_tunneled_ir_width[0] = bscan_tunnel_ir_width;
 440                 if (bscan_tunnel_type == BSCAN_TUNNEL_DATA_REGISTER)
 441                         bscan_tunnel_data_register_select_dmi[1].num_bits = bscan_tunnel_ir_width;
 442                 else /* BSCAN_TUNNEL_NESTED_TAP */
 443                         bscan_tunnel_nested_tap_select_dmi[2].num_bits = bscan_tunnel_ir_width;
 444         }
 445
 446         riscv_semihosting_init(target);
 447
 448         target->debug_reason = DBG_REASON_DBGRQ;
 449
 450         return ERROR_OK;
 451 }
 452
 453 static void riscv_free_registers(struct target *target)
 454 {
 455         /* Free the shared structure use for most registers. */
 456         if (target->reg_cache) {
 457                 if (target->reg_cache->reg_list) {
 458                         free(target->reg_cache->reg_list[0].arch_info);
 459                         /* Free the ones we allocated separately. */
 460                         for (unsigned i = GDB_REGNO_COUNT; i < target->reg_cache->num_regs; i++)
 461                                 free(target->reg_cache->reg_list[i].arch_info);
 462                         free(target->reg_cache->reg_list);
 463                 }
 464                 free(target->reg_cache);
 465         }
 466 }
 467
 468 static void riscv_deinit_target(struct target *target)
 469 {
 470         LOG_DEBUG("riscv_deinit_target()");
 471         struct target_type *tt = get_target_type(target);
 472         if (tt) {
 473                 tt->deinit_target(target);
 474                 riscv_info_t *info = (riscv_info_t *) target->arch_info;
 475                 free(info->reg_names);
 476                 free(info);
 477         }
 478
 479         riscv_free_registers(target);
 480
 481         target->arch_info = NULL;
 482 }
 483
 484 static void trigger_from_breakpoint(struct trigger *trigger,
 485                 const struct breakpoint *breakpoint)
 486 {
 487         trigger->address = breakpoint->address;
 488         trigger->length = breakpoint->length;
 489         trigger->mask = ~0LL;
 490         trigger->read = false;
 491         trigger->write = false;
 492         trigger->execute = true;
 493         /* unique_id is unique across both breakpoints and watchpoints. */
 494         trigger->unique_id = breakpoint->unique_id;
 495 }
 496
 497 static int maybe_add_trigger_t1(struct target *target, unsigned hartid,
 498                 struct trigger *trigger, uint64_t tdata1)
 499 {
 500         RISCV_INFO(r);
 501
 502         const uint32_t bpcontrol_x = 1<<0;
 503         const uint32_t bpcontrol_w = 1<<1;
 504         const uint32_t bpcontrol_r = 1<<2;
 505         const uint32_t bpcontrol_u = 1<<3;
 506         const uint32_t bpcontrol_s = 1<<4;
 507         const uint32_t bpcontrol_h = 1<<5;
 508         const uint32_t bpcontrol_m = 1<<6;
 509         const uint32_t bpcontrol_bpmatch = 0xf << 7;
 510         const uint32_t bpcontrol_bpaction = 0xff << 11;
 511
 512         if (tdata1 & (bpcontrol_r | bpcontrol_w | bpcontrol_x)) {
 513                 /* Trigger is already in use, presumably by user code. */
 514                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 515         }
 516
 517         tdata1 = set_field(tdata1, bpcontrol_r, trigger->read);
 518         tdata1 = set_field(tdata1, bpcontrol_w, trigger->write);
 519         tdata1 = set_field(tdata1, bpcontrol_x, trigger->execute);
 520         tdata1 = set_field(tdata1, bpcontrol_u,
 521                         !!(r->misa[hartid] & (1 << ('U' - 'A'))));
 522         tdata1 = set_field(tdata1, bpcontrol_s,
 523                         !!(r->misa[hartid] & (1 << ('S' - 'A'))));
 524         tdata1 = set_field(tdata1, bpcontrol_h,
 525                         !!(r->misa[hartid] & (1 << ('H' - 'A'))));
 526         tdata1 |= bpcontrol_m;
 527         tdata1 = set_field(tdata1, bpcontrol_bpmatch, 0); /* exact match */
 528         tdata1 = set_field(tdata1, bpcontrol_bpaction, 0); /* cause bp exception */
 529
 530         riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA1, tdata1);
 531
 532         riscv_reg_t tdata1_rb;
 533         if (riscv_get_register_on_hart(target, &tdata1_rb, hartid,
 534                                 GDB_REGNO_TDATA1) != ERROR_OK)
 535                 return ERROR_FAIL;
 536         LOG_DEBUG("tdata1=0x%" PRIx64, tdata1_rb);
 537
 538         if (tdata1 != tdata1_rb) {
 539                 LOG_DEBUG("Trigger doesn't support what we need; After writing 0x%"
 540                                 PRIx64 " to tdata1 it contains 0x%" PRIx64,
 541                                 tdata1, tdata1_rb);
 542                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA1, 0);
 543                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 544         }
 545
 546         riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA2, trigger->address);
 547
 548         return ERROR_OK;
 549 }
 550
 551 static int maybe_add_trigger_t2(struct target *target, unsigned hartid,
 552                 struct trigger *trigger, uint64_t tdata1)
 553 {
 554         RISCV_INFO(r);
 555
 556         /* tselect is already set */
 557         if (tdata1 & (MCONTROL_EXECUTE | MCONTROL_STORE | MCONTROL_LOAD)) {
 558                 /* Trigger is already in use, presumably by user code. */
 559                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 560         }
 561
 562         /* address/data match trigger */
 563         tdata1 |= MCONTROL_DMODE(riscv_xlen(target));
 564         tdata1 = set_field(tdata1, MCONTROL_ACTION,
 565                         MCONTROL_ACTION_DEBUG_MODE);
 566         tdata1 = set_field(tdata1, MCONTROL_MATCH, MCONTROL_MATCH_EQUAL);
 567         tdata1 |= MCONTROL_M;
 568         if (r->misa[hartid] & (1 << ('H' - 'A')))
 569                 tdata1 |= MCONTROL_H;
 570         if (r->misa[hartid] & (1 << ('S' - 'A')))
 571                 tdata1 |= MCONTROL_S;
 572         if (r->misa[hartid] & (1 << ('U' - 'A')))
 573                 tdata1 |= MCONTROL_U;
 574
 575         if (trigger->execute)
 576                 tdata1 |= MCONTROL_EXECUTE;
 577         if (trigger->read)
 578                 tdata1 |= MCONTROL_LOAD;
 579         if (trigger->write)
 580                 tdata1 |= MCONTROL_STORE;
 581
 582         riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA1, tdata1);
 583
 584         uint64_t tdata1_rb;
 585         int result = riscv_get_register_on_hart(target, &tdata1_rb, hartid, GDB_REGNO_TDATA1);
 586         if (result != ERROR_OK)
 587                 return result;
 588         LOG_DEBUG("tdata1=0x%" PRIx64, tdata1_rb);
 589
 590         if (tdata1 != tdata1_rb) {
 591                 LOG_DEBUG("Trigger doesn't support what we need; After writing 0x%"
 592                                 PRIx64 " to tdata1 it contains 0x%" PRIx64,
 593                                 tdata1, tdata1_rb);
 594                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA1, 0);
 595                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 596         }
 597
 598         riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA2, trigger->address);
 599
 600         return ERROR_OK;
 601 }
 602
 603 static int add_trigger(struct target *target, struct trigger *trigger)
 604 {
 605         RISCV_INFO(r);
 606
 607         if (riscv_enumerate_triggers(target) != ERROR_OK)
 608                 return ERROR_FAIL;
 609
 610         /* In RTOS mode, we need to set the same trigger in the same slot on every
 611          * hart, to keep up the illusion that each hart is a thread running on the
 612          * same core. */
 613
 614         /* Otherwise, we just set the trigger on the one hart this target deals
 615          * with. */
 616
 617         riscv_reg_t tselect[RISCV_MAX_HARTS];
 618
 619         int first_hart = -1;
 620         for (int hartid = 0; hartid < riscv_count_harts(target); ++hartid) {
 621                 if (!riscv_hart_enabled(target, hartid))
 622                         continue;
 623                 if (first_hart < 0)
 624                         first_hart = hartid;
 625                 int result = riscv_get_register_on_hart(target, &tselect[hartid],
 626                                 hartid, GDB_REGNO_TSELECT);
 627                 if (result != ERROR_OK)
 628                         return result;
 629         }
 630         assert(first_hart >= 0);
 631
 632         unsigned int i;
 633         for (i = 0; i < r->trigger_count[first_hart]; i++) {
 634                 if (r->trigger_unique_id[i] != -1)
 635                         continue;
 636
 637                 riscv_set_register_on_hart(target, first_hart, GDB_REGNO_TSELECT, i);
 638
 639                 uint64_t tdata1;
 640                 int result = riscv_get_register_on_hart(target, &tdata1, first_hart,
 641                                 GDB_REGNO_TDATA1);
 642                 if (result != ERROR_OK)
 643                         return result;
 644                 int type = get_field(tdata1, MCONTROL_TYPE(riscv_xlen(target)));
 645
 646                 result = ERROR_OK;
 647                 for (int hartid = first_hart; hartid < riscv_count_harts(target); ++hartid) {
 648                         if (!riscv_hart_enabled(target, hartid))
 649                                 continue;
 650                         if (hartid > first_hart)
 651                                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TSELECT, i);
 652                         switch (type) {
 653                                 case 1:
 654                                         result = maybe_add_trigger_t1(target, hartid, trigger, tdata1);
 655                                         break;
 656                                 case 2:
 657                                         result = maybe_add_trigger_t2(target, hartid, trigger, tdata1);
 658                                         break;
 659                                 default:
 660                                         LOG_DEBUG("trigger %d has unknown type %d", i, type);
 661                                         continue;
 662                         }
 663
 664                         if (result != ERROR_OK)
 665                                 continue;
 666                 }
 667
 668                 if (result != ERROR_OK)
 669                         continue;
 670
 671                 LOG_DEBUG("[%d] Using trigger %d (type %d) for bp %d", target->coreid,
 672                                 i, type, trigger->unique_id);
 673                 r->trigger_unique_id[i] = trigger->unique_id;
 674                 break;
 675         }
 676
 677         for (int hartid = first_hart; hartid < riscv_count_harts(target); ++hartid) {
 678                 if (!riscv_hart_enabled(target, hartid))
 679                         continue;
 680                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TSELECT,
 681                                 tselect[hartid]);
 682         }
 683
 684         if (i >= r->trigger_count[first_hart]) {
 685                 LOG_ERROR("Couldn't find an available hardware trigger.");
 686                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 687         }
 688
 689         return ERROR_OK;
 690 }
 691
 692 int riscv_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 693 {
 694         LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, breakpoint->address);
 695         assert(breakpoint);
 696         if (breakpoint->type == BKPT_SOFT) {
 697                 /** @todo check RVC for size/alignment */
 698                 if (!(breakpoint->length == 4 || breakpoint->length == 2)) {
 699                         LOG_ERROR("Invalid breakpoint length %d", breakpoint->length);
 700                         return ERROR_FAIL;
 701                 }
 702
 703                 if (0 != (breakpoint->address % 2)) {
 704                         LOG_ERROR("Invalid breakpoint alignment for address 0x%" TARGET_PRIxADDR, breakpoint->address);
 705                         return ERROR_FAIL;
 706                 }
 707
 708                 if (target_read_memory(target, breakpoint->address, 2, breakpoint->length / 2,
 709                                         breakpoint->orig_instr) != ERROR_OK) {
 710                         LOG_ERROR("Failed to read original instruction at 0x%" TARGET_PRIxADDR,
 711                                         breakpoint->address);
 712                         return ERROR_FAIL;
 713                 }
 714
 715                 uint8_t buff[4] = { 0 };
 716                 buf_set_u32(buff, 0, breakpoint->length * CHAR_BIT, breakpoint->length == 4 ? ebreak() : ebreak_c());
 717                 int const retval = target_write_memory(target, breakpoint->address, 2, breakpoint->length / 2, buff);
 718
 719                 if (retval != ERROR_OK) {
 720                         LOG_ERROR("Failed to write %d-byte breakpoint instruction at 0x%"
 721                                         TARGET_PRIxADDR, breakpoint->length, breakpoint->address);
 722                         return ERROR_FAIL;
 723                 }
 724
 725         } else if (breakpoint->type == BKPT_HARD) {
 726                 struct trigger trigger;
 727                 trigger_from_breakpoint(&trigger, breakpoint);
 728                 int const result = add_trigger(target, &trigger);
 729                 if (result != ERROR_OK)
 730                         return result;
 731         } else {
 732                 LOG_INFO("OpenOCD only supports hardware and software breakpoints.");
 733                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 734         }
 735
 736         breakpoint->set = true;
 737         return ERROR_OK;
 738 }
 739
 740 static int remove_trigger(struct target *target, struct trigger *trigger)
 741 {
 742         RISCV_INFO(r);
 743
 744         if (riscv_enumerate_triggers(target) != ERROR_OK)
 745                 return ERROR_FAIL;
 746
 747         int first_hart = -1;
 748         for (int hartid = 0; hartid < riscv_count_harts(target); ++hartid) {
 749                 if (!riscv_hart_enabled(target, hartid))
 750                         continue;
 751                 if (first_hart < 0) {
 752                         first_hart = hartid;
 753                         break;
 754                 }
 755         }
 756         assert(first_hart >= 0);
 757
 758         unsigned int i;
 759         for (i = 0; i < r->trigger_count[first_hart]; i++) {
 760                 if (r->trigger_unique_id[i] == trigger->unique_id)
 761                         break;
 762         }
 763         if (i >= r->trigger_count[first_hart]) {
 764                 LOG_ERROR("Couldn't find the hardware resources used by hardware "
 765                                 "trigger.");
 766                 return ERROR_FAIL;
 767         }
 768         LOG_DEBUG("[%d] Stop using resource %d for bp %d", target->coreid, i,
 769                         trigger->unique_id);
 770         for (int hartid = first_hart; hartid < riscv_count_harts(target); ++hartid) {
 771                 if (!riscv_hart_enabled(target, hartid))
 772                         continue;
 773                 riscv_reg_t tselect;
 774                 int result = riscv_get_register_on_hart(target, &tselect, hartid, GDB_REGNO_TSELECT);
 775                 if (result != ERROR_OK)
 776                         return result;
 777                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TSELECT, i);
 778                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA1, 0);
 779                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TSELECT, tselect);
 780         }
 781         r->trigger_unique_id[i] = -1;
 782
 783         return ERROR_OK;
 784 }
 785
 786 int riscv_remove_breakpoint(struct target *target,
 787                 struct breakpoint *breakpoint)
 788 {
 789         if (breakpoint->type == BKPT_SOFT) {
 790                 if (target_write_memory(target, breakpoint->address, 2, breakpoint->length / 2,
 791                                         breakpoint->orig_instr) != ERROR_OK) {
 792                         LOG_ERROR("Failed to restore instruction for %d-byte breakpoint at "
 793                                         "0x%" TARGET_PRIxADDR, breakpoint->length, breakpoint->address);
 794                         return ERROR_FAIL;
 795                 }
 796
 797         } else if (breakpoint->type == BKPT_HARD) {
 798                 struct trigger trigger;
 799                 trigger_from_breakpoint(&trigger, breakpoint);
 800                 int result = remove_trigger(target, &trigger);
 801                 if (result != ERROR_OK)
 802                         return result;
 803
 804         } else {
 805                 LOG_INFO("OpenOCD only supports hardware and software breakpoints.");
 806                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 807         }
 808
 809         breakpoint->set = false;
 810
 811         return ERROR_OK;
 812 }
 813
 814 static void trigger_from_watchpoint(struct trigger *trigger,
 815                 const struct watchpoint *watchpoint)
 816 {
 817         trigger->address = watchpoint->address;
 818         trigger->length = watchpoint->length;
 819         trigger->mask = watchpoint->mask;
 820         trigger->value = watchpoint->value;
 821         trigger->read = (watchpoint->rw == WPT_READ || watchpoint->rw == WPT_ACCESS);
 822         trigger->write = (watchpoint->rw == WPT_WRITE || watchpoint->rw == WPT_ACCESS);
 823         trigger->execute = false;
 824         /* unique_id is unique across both breakpoints and watchpoints. */
 825         trigger->unique_id = watchpoint->unique_id;
 826 }
 827
 828 int riscv_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
 829 {
 830         struct trigger trigger;
 831         trigger_from_watchpoint(&trigger, watchpoint);
 832
 833         int result = add_trigger(target, &trigger);
 834         if (result != ERROR_OK)
 835                 return result;
 836         watchpoint->set = true;
 837
 838         return ERROR_OK;
 839 }
 840
 841 int riscv_remove_watchpoint(struct target *target,
 842                 struct watchpoint *watchpoint)
 843 {
 844         LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, watchpoint->address);
 845
 846         struct trigger trigger;
 847         trigger_from_watchpoint(&trigger, watchpoint);
 848
 849         int result = remove_trigger(target, &trigger);
 850         if (result != ERROR_OK)
 851                 return result;
 852         watchpoint->set = false;
 853
 854         return ERROR_OK;
 855 }
 856
 857 /* Sets *hit_watchpoint to the first watchpoint identified as causing the
 858  * current halt.
 859  *
 860  * The GDB server uses this information to tell GDB what data address has
 861  * been hit, which enables GDB to print the hit variable along with its old
 862  * and new value. */
 863 int riscv_hit_watchpoint(struct target *target, struct watchpoint **hit_watchpoint)
 864 {
 865         struct watchpoint *wp = target->watchpoints;
 866
 867         if (riscv_rtos_enabled(target))
 868                 riscv_set_current_hartid(target, target->rtos->current_thread - 1);
 869         LOG_DEBUG("Current hartid = %d", riscv_current_hartid(target));
 870
 871         /*TODO instead of disassembling the instruction that we think caused the
 872          * trigger, check the hit bit of each watchpoint first. The hit bit is
 873          * simpler and more reliable to check but as it is optional and relatively
 874          * new, not all hardware will implement it  */
 875         riscv_reg_t dpc;
 876         riscv_get_register(target, &dpc, GDB_REGNO_DPC);
 877         const uint8_t length = 4;
 878         LOG_DEBUG("dpc is 0x%" PRIx64, dpc);
 879
 880         /* fetch the instruction at dpc */
 881         uint8_t buffer[length];
 882         if (target_read_buffer(target, dpc, length, buffer) != ERROR_OK) {
 883                 LOG_ERROR("Failed to read instruction at dpc 0x%" PRIx64, dpc);
 884                 return ERROR_FAIL;
 885         }
 886
 887         uint32_t instruction = 0;
 888
 889         for (int i = 0; i < length; i++) {
 890                 LOG_DEBUG("Next byte is %x", buffer[i]);
 891                 instruction += (buffer[i] << 8 * i);
 892         }
 893         LOG_DEBUG("Full instruction is %x", instruction);
 894
 895         /* find out which memory address is accessed by the instruction at dpc */
 896         /* opcode is first 7 bits of the instruction */
 897         uint8_t opcode = instruction & 0x7F;
 898         uint32_t rs1;
 899         int16_t imm;
 900         riscv_reg_t mem_addr;
 901
 902         if (opcode == MATCH_LB || opcode == MATCH_SB) {
 903                 rs1 = (instruction & 0xf8000) >> 15;
 904                 riscv_get_register(target, &mem_addr, rs1);
 905
 906                 if (opcode == MATCH_SB) {
 907                         LOG_DEBUG("%x is store instruction", instruction);
 908                         imm = ((instruction & 0xf80) >> 7) | ((instruction & 0xfe000000) >> 20);
 909                 } else {
 910                         LOG_DEBUG("%x is load instruction", instruction);
 911                         imm = (instruction & 0xfff00000) >> 20;
 912                 }
 913                 /* sign extend 12-bit imm to 16-bits */
 914                 if (imm & (1 << 11))
 915                         imm |= 0xf000;
 916                 mem_addr += imm;
 917                 LOG_DEBUG("memory address=0x%" PRIx64, mem_addr);
 918         } else {
 919                 LOG_DEBUG("%x is not a RV32I load or store", instruction);
 920                 return ERROR_FAIL;
 921         }
 922
 923         while (wp) {
 924                 /*TODO support length/mask */
 925                 if (wp->address == mem_addr) {
 926                         *hit_watchpoint = wp;
 927                         LOG_DEBUG("Hit address=%" TARGET_PRIxADDR, wp->address);
 928                         return ERROR_OK;
 929                 }
 930                 wp = wp->next;
 931         }
 932
 933         /* No match found - either we hit a watchpoint caused by an instruction that
 934          * this function does not yet disassemble, or we hit a breakpoint.
 935          *
 936          * OpenOCD will behave as if this function had never been implemented i.e.
 937          * report the halt to GDB with no address information. */
 938         return ERROR_FAIL;
 939 }
 940
 941
 942 static int oldriscv_step(struct target *target, int current, uint32_t address,
 943                 int handle_breakpoints)
 944 {
 945         struct target_type *tt = get_target_type(target);
 946         return tt->step(target, current, address, handle_breakpoints);
 947 }
 948
 949 static int old_or_new_riscv_step(struct target *target, int current,
 950                 target_addr_t address, int handle_breakpoints)
 951 {
 952         RISCV_INFO(r);
 953         LOG_DEBUG("handle_breakpoints=%d", handle_breakpoints);
 954         if (r->is_halted == NULL)
 955                 return oldriscv_step(target, current, address, handle_breakpoints);
 956         else
 957                 return riscv_openocd_step(target, current, address, handle_breakpoints);
 958 }
 959
 960
 961 static int riscv_examine(struct target *target)
 962 {
 963         LOG_DEBUG("riscv_examine()");
 964         if (target_was_examined(target)) {
 965                 LOG_DEBUG("Target was already examined.");
 966                 return ERROR_OK;
 967         }
 968
 969         /* Don't need to select dbus, since the first thing we do is read dtmcontrol. */
 970
 971         riscv_info_t *info = (riscv_info_t *) target->arch_info;
 972         uint32_t dtmcontrol = dtmcontrol_scan(target, 0);
 973         LOG_DEBUG("dtmcontrol=0x%x", dtmcontrol);
 974         info->dtm_version = get_field(dtmcontrol, DTMCONTROL_VERSION);
 975         LOG_DEBUG("  version=0x%x", info->dtm_version);
 976
 977         struct target_type *tt = get_target_type(target);
 978         if (tt == NULL)
 979                 return ERROR_FAIL;
 980
 981         int result = tt->init_target(info->cmd_ctx, target);
 982         if (result != ERROR_OK)
 983                 return result;
 984
 985         return tt->examine(target);
 986 }
 987
 988 static int oldriscv_poll(struct target *target)
 989 {
 990         struct target_type *tt = get_target_type(target);
 991         return tt->poll(target);
 992 }
 993
 994 static int old_or_new_riscv_poll(struct target *target)
 995 {
 996         RISCV_INFO(r);
 997         if (r->is_halted == NULL)
 998                 return oldriscv_poll(target);
 999         else
1000                 return riscv_openocd_poll(target);
1001 }
1002
1003 int halt_prep(struct target *target)
1004 {
1005         RISCV_INFO(r);
1006         for (int i = 0; i < riscv_count_harts(target); ++i) {
1007                 if (!riscv_hart_enabled(target, i))
1008                         continue;
1009
1010                 LOG_DEBUG("[%s] prep hart, debug_reason=%d", target_name(target),
1011                                   target->debug_reason);
1012                 if (riscv_set_current_hartid(target, i) != ERROR_OK)
1013                         return ERROR_FAIL;
1014                 if (riscv_is_halted(target)) {
1015                         LOG_DEBUG("Hart %d is already halted (reason=%d).", i,
1016                                           target->debug_reason);
1017                 } else {
1018                         if (r->halt_prep(target) != ERROR_OK)
1019                                 return ERROR_FAIL;
1020                         r->prepped = true;
1021                 }
1022         }
1023         return ERROR_OK;
1024 }
1025
1026 int riscv_halt_go_all_harts(struct target *target)
1027 {
1028         RISCV_INFO(r);
1029         for (int i = 0; i < riscv_count_harts(target); ++i) {
1030                 if (!riscv_hart_enabled(target, i))
1031                         continue;
1032
1033                 if (riscv_set_current_hartid(target, i) != ERROR_OK)
1034                         return ERROR_FAIL;
1035                 if (riscv_is_halted(target)) {
1036                         LOG_DEBUG("Hart %d is already halted.", i);
1037                 } else {
1038                         if (r->halt_go(target) != ERROR_OK)
1039                                 return ERROR_FAIL;
1040                 }
1041         }
1042
1043         riscv_invalidate_register_cache(target);
1044
1045         return ERROR_OK;
1046 }
1047
1048 int halt_go(struct target *target)
1049 {
1050         riscv_info_t *r = riscv_info(target);
1051         int result;
1052         if (r->is_halted == NULL) {
1053                 struct target_type *tt = get_target_type(target);
1054                 result = tt->halt(target);
1055         } else {
1056                 result = riscv_halt_go_all_harts(target);
1057         }
1058         target->state = TARGET_HALTED;
1059         if (target->debug_reason == DBG_REASON_NOTHALTED)
1060                 target->debug_reason = DBG_REASON_DBGRQ;
1061
1062         return result;
1063 }
1064
1065 static int halt_finish(struct target *target)
1066 {
1067         return target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1068 }
1069
1070 int riscv_halt(struct target *target)
1071 {
1072         RISCV_INFO(r);
1073
1074         if (r->is_halted == NULL) {
1075                 struct target_type *tt = get_target_type(target);
1076                 return tt->halt(target);
1077         }
1078
1079         LOG_DEBUG("[%d] halting all harts", target->coreid);
1080
1081         int result = ERROR_OK;
1082         if (target->smp) {
1083                 for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
1084                         struct target *t = tlist->target;
1085                         if (halt_prep(t) != ERROR_OK)
1086                                 result = ERROR_FAIL;
1087                 }
1088
1089                 for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
1090                         struct target *t = tlist->target;
1091                         riscv_info_t *i = riscv_info(t);
1092                         if (i->prepped) {
1093                                 if (halt_go(t) != ERROR_OK)
1094                                         result = ERROR_FAIL;
1095                         }
1096                 }
1097
1098                 for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
1099                         struct target *t = tlist->target;
1100                         if (halt_finish(t) != ERROR_OK)
1101                                 return ERROR_FAIL;
1102                 }
1103
1104         } else {
1105                 if (halt_prep(target) != ERROR_OK)
1106                         result = ERROR_FAIL;
1107                 if (halt_go(target) != ERROR_OK)
1108                         result = ERROR_FAIL;
1109                 if (halt_finish(target) != ERROR_OK)
1110                         return ERROR_FAIL;
1111         }
1112
1113         if (riscv_rtos_enabled(target)) {
1114                 if (r->rtos_hartid != -1) {
1115                         LOG_DEBUG("halt requested on RTOS hartid %d", r->rtos_hartid);
1116                         target->rtos->current_threadid = r->rtos_hartid + 1;
1117                         target->rtos->current_thread = r->rtos_hartid + 1;
1118                 } else
1119                         LOG_DEBUG("halt requested, but no known RTOS hartid");
1120         }
1121
1122         return result;
1123 }
1124
1125 static int riscv_assert_reset(struct target *target)
1126 {
1127         LOG_DEBUG("[%d]", target->coreid);
1128         struct target_type *tt = get_target_type(target);
1129         riscv_invalidate_register_cache(target);
1130         return tt->assert_reset(target);
1131 }
1132
1133 static int riscv_deassert_reset(struct target *target)
1134 {
1135         LOG_DEBUG("[%d]", target->coreid);
1136         struct target_type *tt = get_target_type(target);
1137         return tt->deassert_reset(target);
1138 }
1139
1140 int riscv_resume_prep_all_harts(struct target *target)
1141 {
1142         RISCV_INFO(r);
1143         for (int i = 0; i < riscv_count_harts(target); ++i) {
1144                 if (!riscv_hart_enabled(target, i))
1145                         continue;
1146
1147                 LOG_DEBUG("prep hart %d", i);
1148                 if (riscv_set_current_hartid(target, i) != ERROR_OK)
1149                         return ERROR_FAIL;
1150                 if (riscv_is_halted(target)) {
1151                         if (r->resume_prep(target) != ERROR_OK)
1152                                 return ERROR_FAIL;
1153                 } else {
1154                         LOG_DEBUG("  hart %d requested resume, but was already resumed", i);
1155                 }
1156         }
1157
1158         LOG_DEBUG("[%d] mark as prepped", target->coreid);
1159         r->prepped = true;
1160
1161         return ERROR_OK;
1162 }
1163
1164 /* state must be riscv_reg_t state[RISCV_MAX_HWBPS] = {0}; */
1165 static int disable_triggers(struct target *target, riscv_reg_t *state)
1166 {
1167         RISCV_INFO(r);
1168
1169         LOG_DEBUG("deal with triggers");
1170
1171         if (riscv_enumerate_triggers(target) != ERROR_OK)
1172                 return ERROR_FAIL;
1173
1174         int hartid = riscv_current_hartid(target);
1175         if (r->manual_hwbp_set) {
1176                 /* Look at every trigger that may have been set. */
1177                 riscv_reg_t tselect;
1178                 if (riscv_get_register(target, &tselect, GDB_REGNO_TSELECT) != ERROR_OK)
1179                         return ERROR_FAIL;
1180                 for (unsigned t = 0; t < r->trigger_count[hartid]; t++) {
1181                         if (riscv_set_register(target, GDB_REGNO_TSELECT, t) != ERROR_OK)
1182                                 return ERROR_FAIL;
1183                         riscv_reg_t tdata1;
1184                         if (riscv_get_register(target, &tdata1, GDB_REGNO_TDATA1) != ERROR_OK)
1185                                 return ERROR_FAIL;
1186                         if (tdata1 & MCONTROL_DMODE(riscv_xlen(target))) {
1187                                 state[t] = tdata1;
1188                                 if (riscv_set_register(target, GDB_REGNO_TDATA1, 0) != ERROR_OK)
1189                                         return ERROR_FAIL;
1190                         }
1191                 }
1192                 if (riscv_set_register(target, GDB_REGNO_TSELECT, tselect) != ERROR_OK)
1193                         return ERROR_FAIL;
1194
1195         } else {
1196                 /* Just go through the triggers we manage. */
1197                 struct watchpoint *watchpoint = target->watchpoints;
1198                 int i = 0;
1199                 while (watchpoint) {
1200                         LOG_DEBUG("watchpoint %d: set=%d", i, watchpoint->set);
1201                         state[i] = watchpoint->set;
1202                         if (watchpoint->set) {
1203                                 if (riscv_remove_watchpoint(target, watchpoint) != ERROR_OK)
1204                                         return ERROR_FAIL;
1205                         }
1206                         watchpoint = watchpoint->next;
1207                         i++;
1208                 }
1209         }
1210
1211         return ERROR_OK;
1212 }
1213
1214 static int enable_triggers(struct target *target, riscv_reg_t *state)
1215 {
1216         RISCV_INFO(r);
1217
1218         int hartid = riscv_current_hartid(target);
1219
1220         if (r->manual_hwbp_set) {
1221                 /* Look at every trigger that may have been set. */
1222                 riscv_reg_t tselect;
1223                 if (riscv_get_register(target, &tselect, GDB_REGNO_TSELECT) != ERROR_OK)
1224                         return ERROR_FAIL;
1225                 for (unsigned t = 0; t < r->trigger_count[hartid]; t++) {
1226                         if (state[t] != 0) {
1227                                 if (riscv_set_register(target, GDB_REGNO_TSELECT, t) != ERROR_OK)
1228                                         return ERROR_FAIL;
1229                                 if (riscv_set_register(target, GDB_REGNO_TDATA1, state[t]) != ERROR_OK)
1230                                         return ERROR_FAIL;
1231                         }
1232                 }
1233                 if (riscv_set_register(target, GDB_REGNO_TSELECT, tselect) != ERROR_OK)
1234                         return ERROR_FAIL;
1235
1236         } else {
1237                 struct watchpoint *watchpoint = target->watchpoints;
1238                 int i = 0;
1239                 while (watchpoint) {
1240                         LOG_DEBUG("watchpoint %d: cleared=%" PRId64, i, state[i]);
1241                         if (state[i]) {
1242                                 if (riscv_add_watchpoint(target, watchpoint) != ERROR_OK)
1243                                         return ERROR_FAIL;
1244                         }
1245                         watchpoint = watchpoint->next;
1246                         i++;
1247                 }
1248         }
1249
1250         return ERROR_OK;
1251 }
1252
1253 /**
1254  * Get everything ready to resume.
1255  */
1256 static int resume_prep(struct target *target, int current,
1257                 target_addr_t address, int handle_breakpoints, int debug_execution)
1258 {
1259         RISCV_INFO(r);
1260         LOG_DEBUG("[%d]", target->coreid);
1261
1262         if (!current)
1263                 riscv_set_register(target, GDB_REGNO_PC, address);
1264
1265         if (target->debug_reason == DBG_REASON_WATCHPOINT) {
1266                 /* To be able to run off a trigger, disable all the triggers, step, and
1267                  * then resume as usual. */
1268                 riscv_reg_t trigger_state[RISCV_MAX_HWBPS] = {0};
1269
1270                 if (disable_triggers(target, trigger_state) != ERROR_OK)
1271                         return ERROR_FAIL;
1272
1273                 if (old_or_new_riscv_step(target, true, 0, false) != ERROR_OK)
1274                         return ERROR_FAIL;
1275
1276                 if (enable_triggers(target, trigger_state) != ERROR_OK)
1277                         return ERROR_FAIL;
1278         }
1279
1280         if (r->is_halted) {
1281                 if (riscv_resume_prep_all_harts(target) != ERROR_OK)
1282                         return ERROR_FAIL;
1283         }
1284
1285         LOG_DEBUG("[%d] mark as prepped", target->coreid);
1286         r->prepped = true;
1287
1288         return ERROR_OK;
1289 }
1290
1291 /**
1292  * Resume all the harts that have been prepped, as close to instantaneous as
1293  * possible.
1294  */
1295 static int resume_go(struct target *target, int current,
1296                 target_addr_t address, int handle_breakpoints, int debug_execution)
1297 {
1298         riscv_info_t *r = riscv_info(target);
1299         int result;
1300         if (r->is_halted == NULL) {
1301                 struct target_type *tt = get_target_type(target);
1302                 result = tt->resume(target, current, address, handle_breakpoints,
1303                                 debug_execution);
1304         } else {
1305                 result = riscv_resume_go_all_harts(target);
1306         }
1307
1308         return result;
1309 }
1310
1311 static int resume_finish(struct target *target)
1312 {
1313         register_cache_invalidate(target->reg_cache);
1314
1315         target->state = TARGET_RUNNING;
1316         target->debug_reason = DBG_REASON_NOTHALTED;
1317         return target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
1318 }
1319
1320 /**
1321  * @par single_hart When true, only resume a single hart even if SMP is
1322  * configured.  This is used to run algorithms on just one hart.
1323  */
1324 int riscv_resume(
1325                 struct target *target,
1326                 int current,
1327                 target_addr_t address,
1328                 int handle_breakpoints,
1329                 int debug_execution,
1330                 bool single_hart)
1331 {
1332         LOG_DEBUG("handle_breakpoints=%d", handle_breakpoints);
1333         int result = ERROR_OK;
1334         if (target->smp && !single_hart) {
1335                 for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
1336                         struct target *t = tlist->target;
1337                         if (resume_prep(t, current, address, handle_breakpoints,
1338                                                 debug_execution) != ERROR_OK)
1339                                 result = ERROR_FAIL;
1340                 }
1341
1342                 for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
1343                         struct target *t = tlist->target;
1344                         riscv_info_t *i = riscv_info(t);
1345                         if (i->prepped) {
1346                                 if (resume_go(t, current, address, handle_breakpoints,
1347                                                         debug_execution) != ERROR_OK)
1348                                         result = ERROR_FAIL;
1349                         }
1350                 }
1351
1352                 for (struct target_list *tlist = target->head; tlist; tlist = tlist->next) {
1353                         struct target *t = tlist->target;
1354                         if (resume_finish(t) != ERROR_OK)
1355                                 return ERROR_FAIL;
1356                 }
1357
1358         } else {
1359                 if (resume_prep(target, current, address, handle_breakpoints,
1360                                         debug_execution) != ERROR_OK)
1361                         result = ERROR_FAIL;
1362                 if (resume_go(target, current, address, handle_breakpoints,
1363                                         debug_execution) != ERROR_OK)
1364                         result = ERROR_FAIL;
1365                 if (resume_finish(target) != ERROR_OK)
1366                         return ERROR_FAIL;
1367         }
1368
1369         return result;
1370 }
1371
1372 static int riscv_target_resume(struct target *target, int current, target_addr_t address,
1373                 int handle_breakpoints, int debug_execution)
1374 {
1375         return riscv_resume(target, current, address, handle_breakpoints,
1376                         debug_execution, false);
1377 }
1378
1379 static int riscv_select_current_hart(struct target *target)
1380 {
1381         RISCV_INFO(r);
1382         if (riscv_rtos_enabled(target)) {
1383                 if (r->rtos_hartid == -1)
1384                         r->rtos_hartid = target->rtos->current_threadid - 1;
1385                 return riscv_set_current_hartid(target, r->rtos_hartid);
1386         } else
1387                 return riscv_set_current_hartid(target, target->coreid);
1388 }
1389
1390 static int riscv_mmu(struct target *target, int *enabled)
1391 {
1392         if (!riscv_enable_virt2phys) {
1393                 *enabled = 0;
1394                 return ERROR_OK;
1395         }
1396
1397         if (riscv_rtos_enabled(target))
1398                 riscv_set_current_hartid(target, target->rtos->current_thread - 1);
1399
1400         /* Don't use MMU in explicit or effective M (machine) mode */
1401         riscv_reg_t priv;
1402         if (riscv_get_register(target, &priv, GDB_REGNO_PRIV) != ERROR_OK) {
1403                 LOG_ERROR("Failed to read priv register.");
1404                 return ERROR_FAIL;
1405         }
1406
1407         riscv_reg_t mstatus;
1408         if (riscv_get_register(target, &mstatus, GDB_REGNO_MSTATUS) != ERROR_OK) {
1409                 LOG_ERROR("Failed to read mstatus register.");
1410                 return ERROR_FAIL;
1411         }
1412
1413         if ((get_field(mstatus, MSTATUS_MPRV) ? get_field(mstatus, MSTATUS_MPP) : priv) == PRV_M) {
1414                 LOG_DEBUG("SATP/MMU ignored in Machine mode (mstatus=0x%" PRIx64 ").", mstatus);
1415                 *enabled = 0;
1416                 return ERROR_OK;
1417         }
1418
1419         riscv_reg_t satp;
1420         if (riscv_get_register(target, &satp, GDB_REGNO_SATP) != ERROR_OK) {
1421                 LOG_DEBUG("Couldn't read SATP.");
1422                 /* If we can't read SATP, then there must not be an MMU. */
1423                 *enabled = 0;
1424                 return ERROR_OK;
1425         }
1426
1427         if (get_field(satp, RISCV_SATP_MODE(riscv_xlen(target))) == SATP_MODE_OFF) {
1428                 LOG_DEBUG("MMU is disabled.");
1429                 *enabled = 0;
1430         } else {
1431                 LOG_DEBUG("MMU is enabled.");
1432                 *enabled = 1;
1433         }
1434
1435         return ERROR_OK;
1436 }
1437
1438 static int riscv_address_translate(struct target *target,
1439                 target_addr_t virtual, target_addr_t *physical)
1440 {
1441         RISCV_INFO(r);
1442         riscv_reg_t satp_value;
1443         int mode;
1444         uint64_t ppn_value;
1445         target_addr_t table_address;
1446         virt2phys_info_t *info;
1447         uint64_t pte = 0;
1448         int i;
1449
1450         if (riscv_rtos_enabled(target))
1451                 riscv_set_current_hartid(target, target->rtos->current_thread - 1);
1452
1453         int result = riscv_get_register(target, &satp_value, GDB_REGNO_SATP);
1454         if (result != ERROR_OK)
1455                 return result;
1456
1457         unsigned xlen = riscv_xlen(target);
1458         mode = get_field(satp_value, RISCV_SATP_MODE(xlen));
1459         switch (mode) {
1460                 case SATP_MODE_SV32:
1461                         info = &sv32;
1462                         break;
1463                 case SATP_MODE_SV39:
1464                         info = &sv39;
1465                         break;
1466                 case SATP_MODE_SV48:
1467                         info = &sv48;
1468                         break;
1469                 case SATP_MODE_OFF:
1470                         LOG_ERROR("No translation or protection." \
1471                                       " (satp: 0x%" PRIx64 ")", satp_value);
1472                         return ERROR_FAIL;
1473                 default:
1474                         LOG_ERROR("The translation mode is not supported." \
1475                                       " (satp: 0x%" PRIx64 ")", satp_value);
1476                         return ERROR_FAIL;
1477         }
1478         LOG_DEBUG("virtual=0x%" TARGET_PRIxADDR "; mode=%s", virtual, info->name);
1479
1480         /* verify bits xlen-1:va_bits-1 are all equal */
1481         target_addr_t mask = ((target_addr_t)1 << (xlen - (info->va_bits - 1))) - 1;
1482         target_addr_t masked_msbs = (virtual >> (info->va_bits - 1)) & mask;
1483         if (masked_msbs != 0 && masked_msbs != mask) {
1484                 LOG_ERROR("Virtual address 0x%" TARGET_PRIxADDR " is not sign-extended "
1485                                 "for %s mode.", virtual, info->name);
1486                 return ERROR_FAIL;
1487         }
1488
1489         ppn_value = get_field(satp_value, RISCV_SATP_PPN(xlen));
1490         table_address = ppn_value << RISCV_PGSHIFT;
1491         i = info->level - 1;
1492         while (i >= 0) {
1493                 uint64_t vpn = virtual >> info->vpn_shift[i];
1494                 vpn &= info->vpn_mask[i];
1495                 target_addr_t pte_address = table_address +
1496                                                                         (vpn << info->pte_shift);
1497                 uint8_t buffer[8];
1498                 assert(info->pte_shift <= 3);
1499                 int retval = r->read_memory(target, pte_address,
1500                                 4, (1 << info->pte_shift) / 4, buffer, 4);
1501                 if (retval != ERROR_OK)
1502                         return ERROR_FAIL;
1503
1504                 if (info->pte_shift == 2)
1505                         pte = buf_get_u32(buffer, 0, 32);
1506                 else
1507                         pte = buf_get_u64(buffer, 0, 64);
1508
1509                 LOG_DEBUG("i=%d; PTE @0x%" TARGET_PRIxADDR " = 0x%" PRIx64, i,
1510                                 pte_address, pte);
1511
1512                 if (!(pte & PTE_V) || (!(pte & PTE_R) && (pte & PTE_W)))
1513                         return ERROR_FAIL;
1514
1515                 if ((pte & PTE_R) || (pte & PTE_X)) /* Found leaf PTE. */
1516                         break;
1517
1518                 i--;
1519                 if (i < 0)
1520                         break;
1521                 ppn_value = pte >> PTE_PPN_SHIFT;
1522                 table_address = ppn_value << RISCV_PGSHIFT;
1523         }
1524
1525         if (i < 0) {
1526                 LOG_ERROR("Couldn't find the PTE.");
1527                 return ERROR_FAIL;
1528         }
1529
1530         /* Make sure to clear out the high bits that may be set. */
1531         *physical = virtual & (((target_addr_t)1 << info->va_bits) - 1);
1532
1533         while (i < info->level) {
1534                 ppn_value = pte >> info->pte_ppn_shift[i];
1535                 ppn_value &= info->pte_ppn_mask[i];
1536                 *physical &= ~(((target_addr_t)info->pa_ppn_mask[i]) <<
1537                                 info->pa_ppn_shift[i]);
1538                 *physical |= (ppn_value << info->pa_ppn_shift[i]);
1539                 i++;
1540         }
1541         LOG_DEBUG("0x%" TARGET_PRIxADDR " -> 0x%" TARGET_PRIxADDR, virtual,
1542                         *physical);
1543
1544         return ERROR_OK;
1545 }
1546
1547 static int riscv_virt2phys(struct target *target, target_addr_t virtual, target_addr_t *physical)
1548 {
1549         int enabled;
1550         if (riscv_mmu(target, &enabled) == ERROR_OK) {
1551                 if (!enabled)
1552                         return ERROR_FAIL;
1553
1554                 if (riscv_address_translate(target, virtual, physical) == ERROR_OK)
1555                         return ERROR_OK;
1556         }
1557
1558         return ERROR_FAIL;
1559 }
1560
1561 static int riscv_read_phys_memory(struct target *target, target_addr_t phys_address,
1562                         uint32_t size, uint32_t count, uint8_t *buffer)
1563 {
1564         RISCV_INFO(r);
1565         if (riscv_select_current_hart(target) != ERROR_OK)
1566                 return ERROR_FAIL;
1567         return r->read_memory(target, phys_address, size, count, buffer, size);
1568 }
1569
1570 static int riscv_read_memory(struct target *target, target_addr_t address,
1571                 uint32_t size, uint32_t count, uint8_t *buffer)
1572 {
1573         if (count == 0) {
1574                 LOG_WARNING("0-length read from 0x%" TARGET_PRIxADDR, address);
1575                 return ERROR_OK;
1576         }
1577
1578         if (riscv_select_current_hart(target) != ERROR_OK)
1579                 return ERROR_FAIL;
1580
1581         target_addr_t physical_addr;
1582         if (target->type->virt2phys(target, address, &physical_addr) == ERROR_OK)
1583                 address = physical_addr;
1584
1585         RISCV_INFO(r);
1586         return r->read_memory(target, address, size, count, buffer, size);
1587 }
1588
1589 static int riscv_write_phys_memory(struct target *target, target_addr_t phys_address,
1590                         uint32_t size, uint32_t count, const uint8_t *buffer)
1591 {
1592         if (riscv_select_current_hart(target) != ERROR_OK)
1593                 return ERROR_FAIL;
1594         struct target_type *tt = get_target_type(target);
1595         return tt->write_memory(target, phys_address, size, count, buffer);
1596 }
1597
1598 static int riscv_write_memory(struct target *target, target_addr_t address,
1599                 uint32_t size, uint32_t count, const uint8_t *buffer)
1600 {
1601         if (count == 0) {
1602                 LOG_WARNING("0-length write to 0x%" TARGET_PRIxADDR, address);
1603                 return ERROR_OK;
1604         }
1605
1606         if (riscv_select_current_hart(target) != ERROR_OK)
1607                 return ERROR_FAIL;
1608
1609         target_addr_t physical_addr;
1610         if (target->type->virt2phys(target, address, &physical_addr) == ERROR_OK)
1611                 address = physical_addr;
1612
1613         struct target_type *tt = get_target_type(target);
1614         return tt->write_memory(target, address, size, count, buffer);
1615 }
1616
1617 static int riscv_get_gdb_reg_list_internal(struct target *target,
1618                 struct reg **reg_list[], int *reg_list_size,
1619                 enum target_register_class reg_class, bool read)
1620 {
1621         RISCV_INFO(r);
1622         LOG_DEBUG("rtos_hartid=%d, current_hartid=%d, reg_class=%d, read=%d",
1623                         r->rtos_hartid, r->current_hartid, reg_class, read);
1624
1625         if (!target->reg_cache) {
1626                 LOG_ERROR("Target not initialized. Return ERROR_FAIL.");
1627                 return ERROR_FAIL;
1628         }
1629
1630         if (riscv_select_current_hart(target) != ERROR_OK)
1631                 return ERROR_FAIL;
1632
1633         switch (reg_class) {
1634                 case REG_CLASS_GENERAL:
1635                         *reg_list_size = 33;
1636                         break;
1637                 case REG_CLASS_ALL:
1638                         *reg_list_size = target->reg_cache->num_regs;
1639                         break;
1640                 default:
1641                         LOG_ERROR("Unsupported reg_class: %d", reg_class);
1642                         return ERROR_FAIL;
1643         }
1644
1645         *reg_list = calloc(*reg_list_size, sizeof(struct reg *));
1646         if (!*reg_list)
1647                 return ERROR_FAIL;
1648
1649         for (int i = 0; i < *reg_list_size; i++) {
1650                 assert(!target->reg_cache->reg_list[i].valid ||
1651                                 target->reg_cache->reg_list[i].size > 0);
1652                 (*reg_list)[i] = &target->reg_cache->reg_list[i];
1653                 if (read &&
1654                                 target->reg_cache->reg_list[i].exist &&
1655                                 !target->reg_cache->reg_list[i].valid) {
1656                         if (target->reg_cache->reg_list[i].type->get(
1657                                                 &target->reg_cache->reg_list[i]) != ERROR_OK)
1658                                 return ERROR_FAIL;
1659                 }
1660         }
1661
1662         return ERROR_OK;
1663 }
1664
1665 static int riscv_get_gdb_reg_list_noread(struct target *target,
1666                 struct reg **reg_list[], int *reg_list_size,
1667                 enum target_register_class reg_class)
1668 {
1669         return riscv_get_gdb_reg_list_internal(target, reg_list, reg_list_size,
1670                         reg_class, false);
1671 }
1672
1673 static int riscv_get_gdb_reg_list(struct target *target,
1674                 struct reg **reg_list[], int *reg_list_size,
1675                 enum target_register_class reg_class)
1676 {
1677         return riscv_get_gdb_reg_list_internal(target, reg_list, reg_list_size,
1678                         reg_class, true);
1679 }
1680
1681 static int riscv_arch_state(struct target *target)
1682 {
1683         struct target_type *tt = get_target_type(target);
1684         return tt->arch_state(target);
1685 }
1686
1687 /* Algorithm must end with a software breakpoint instruction. */
1688 static int riscv_run_algorithm(struct target *target, int num_mem_params,
1689                 struct mem_param *mem_params, int num_reg_params,
1690                 struct reg_param *reg_params, target_addr_t entry_point,
1691                 target_addr_t exit_point, int timeout_ms, void *arch_info)
1692 {
1693         riscv_info_t *info = (riscv_info_t *) target->arch_info;
1694         int hartid = riscv_current_hartid(target);
1695
1696         if (num_mem_params > 0) {
1697                 LOG_ERROR("Memory parameters are not supported for RISC-V algorithms.");
1698                 return ERROR_FAIL;
1699         }
1700
1701         if (target->state != TARGET_HALTED) {
1702                 LOG_WARNING("target not halted");
1703                 return ERROR_TARGET_NOT_HALTED;
1704         }
1705
1706         /* Save registers */
1707         struct reg *reg_pc = register_get_by_name(target->reg_cache, "pc", true);
1708         if (!reg_pc || reg_pc->type->get(reg_pc) != ERROR_OK)
1709                 return ERROR_FAIL;
1710         uint64_t saved_pc = buf_get_u64(reg_pc->value, 0, reg_pc->size);
1711         LOG_DEBUG("saved_pc=0x%" PRIx64, saved_pc);
1712
1713         uint64_t saved_regs[32];
1714         for (int i = 0; i < num_reg_params; i++) {
1715                 LOG_DEBUG("save %s", reg_params[i].reg_name);
1716                 struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, false);
1717                 if (!r) {
1718                         LOG_ERROR("Couldn't find register named '%s'", reg_params[i].reg_name);
1719                         return ERROR_FAIL;
1720                 }
1721
1722                 if (r->size != reg_params[i].size) {
1723                         LOG_ERROR("Register %s is %d bits instead of %d bits.",
1724                                         reg_params[i].reg_name, r->size, reg_params[i].size);
1725                         return ERROR_FAIL;
1726                 }
1727
1728                 if (r->number > GDB_REGNO_XPR31) {
1729                         LOG_ERROR("Only GPRs can be use as argument registers.");
1730                         return ERROR_FAIL;
1731                 }
1732
1733                 if (r->type->get(r) != ERROR_OK)
1734                         return ERROR_FAIL;
1735                 saved_regs[r->number] = buf_get_u64(r->value, 0, r->size);
1736
1737                 if (reg_params[i].direction == PARAM_OUT || reg_params[i].direction == PARAM_IN_OUT) {
1738                         if (r->type->set(r, reg_params[i].value) != ERROR_OK)
1739                                 return ERROR_FAIL;
1740                 }
1741         }
1742
1743
1744         /* Disable Interrupts before attempting to run the algorithm. */
1745         uint64_t current_mstatus;
1746         uint8_t mstatus_bytes[8] = { 0 };
1747
1748         LOG_DEBUG("Disabling Interrupts");
1749         struct reg *reg_mstatus = register_get_by_name(target->reg_cache,
1750                         "mstatus", true);
1751         if (!reg_mstatus) {
1752                 LOG_ERROR("Couldn't find mstatus!");
1753                 return ERROR_FAIL;
1754         }
1755
1756         reg_mstatus->type->get(reg_mstatus);
1757         current_mstatus = buf_get_u64(reg_mstatus->value, 0, reg_mstatus->size);
1758         uint64_t ie_mask = MSTATUS_MIE | MSTATUS_HIE | MSTATUS_SIE | MSTATUS_UIE;
1759         buf_set_u64(mstatus_bytes, 0, info->xlen[0], set_field(current_mstatus,
1760                                 ie_mask, 0));
1761
1762         reg_mstatus->type->set(reg_mstatus, mstatus_bytes);
1763
1764         /* Run algorithm */
1765         LOG_DEBUG("resume at 0x%" TARGET_PRIxADDR, entry_point);
1766         if (riscv_resume(target, 0, entry_point, 0, 0, true) != ERROR_OK)
1767                 return ERROR_FAIL;
1768
1769         int64_t start = timeval_ms();
1770         while (target->state != TARGET_HALTED) {
1771                 LOG_DEBUG("poll()");
1772                 int64_t now = timeval_ms();
1773                 if (now - start > timeout_ms) {
1774                         LOG_ERROR("Algorithm timed out after %" PRId64 " ms.", now - start);
1775                         riscv_halt(target);
1776                         old_or_new_riscv_poll(target);
1777                         enum gdb_regno regnums[] = {
1778                                 GDB_REGNO_RA, GDB_REGNO_SP, GDB_REGNO_GP, GDB_REGNO_TP,
1779                                 GDB_REGNO_T0, GDB_REGNO_T1, GDB_REGNO_T2, GDB_REGNO_FP,
1780                                 GDB_REGNO_S1, GDB_REGNO_A0, GDB_REGNO_A1, GDB_REGNO_A2,
1781                                 GDB_REGNO_A3, GDB_REGNO_A4, GDB_REGNO_A5, GDB_REGNO_A6,
1782                                 GDB_REGNO_A7, GDB_REGNO_S2, GDB_REGNO_S3, GDB_REGNO_S4,
1783                                 GDB_REGNO_S5, GDB_REGNO_S6, GDB_REGNO_S7, GDB_REGNO_S8,
1784                                 GDB_REGNO_S9, GDB_REGNO_S10, GDB_REGNO_S11, GDB_REGNO_T3,
1785                                 GDB_REGNO_T4, GDB_REGNO_T5, GDB_REGNO_T6,
1786                                 GDB_REGNO_PC,
1787                                 GDB_REGNO_MSTATUS, GDB_REGNO_MEPC, GDB_REGNO_MCAUSE,
1788                         };
1789                         for (unsigned i = 0; i < ARRAY_SIZE(regnums); i++) {
1790                                 enum gdb_regno regno = regnums[i];
1791                                 riscv_reg_t reg_value;
1792                                 if (riscv_get_register(target, &reg_value, regno) != ERROR_OK)
1793                                         break;
1794                                 LOG_ERROR("%s = 0x%" PRIx64, gdb_regno_name(regno), reg_value);
1795                         }
1796                         return ERROR_TARGET_TIMEOUT;
1797                 }
1798
1799                 int result = old_or_new_riscv_poll(target);
1800                 if (result != ERROR_OK)
1801                         return result;
1802         }
1803
1804         /* The current hart id might have been changed in poll(). */
1805         if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
1806                 return ERROR_FAIL;
1807
1808         if (reg_pc->type->get(reg_pc) != ERROR_OK)
1809                 return ERROR_FAIL;
1810         uint64_t final_pc = buf_get_u64(reg_pc->value, 0, reg_pc->size);
1811         if (exit_point && final_pc != exit_point) {
1812                 LOG_ERROR("PC ended up at 0x%" PRIx64 " instead of 0x%"
1813                                 TARGET_PRIxADDR, final_pc, exit_point);
1814                 return ERROR_FAIL;
1815         }
1816
1817         /* Restore Interrupts */
1818         LOG_DEBUG("Restoring Interrupts");
1819         buf_set_u64(mstatus_bytes, 0, info->xlen[0], current_mstatus);
1820         reg_mstatus->type->set(reg_mstatus, mstatus_bytes);
1821
1822         /* Restore registers */
1823         uint8_t buf[8] = { 0 };
1824         buf_set_u64(buf, 0, info->xlen[0], saved_pc);
1825         if (reg_pc->type->set(reg_pc, buf) != ERROR_OK)
1826                 return ERROR_FAIL;
1827
1828         for (int i = 0; i < num_reg_params; i++) {
1829                 if (reg_params[i].direction == PARAM_IN ||
1830                                 reg_params[i].direction == PARAM_IN_OUT) {
1831                         struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, false);
1832                         if (r->type->get(r) != ERROR_OK) {
1833                                 LOG_ERROR("get(%s) failed", r->name);
1834                                 return ERROR_FAIL;
1835                         }
1836                         buf_cpy(r->value, reg_params[i].value, reg_params[i].size);
1837                 }
1838                 LOG_DEBUG("restore %s", reg_params[i].reg_name);
1839                 struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, false);
1840                 buf_set_u64(buf, 0, info->xlen[0], saved_regs[r->number]);
1841                 if (r->type->set(r, buf) != ERROR_OK) {
1842                         LOG_ERROR("set(%s) failed", r->name);
1843                         return ERROR_FAIL;
1844                 }
1845         }
1846
1847         return ERROR_OK;
1848 }
1849
1850 static int riscv_checksum_memory(struct target *target,
1851                 target_addr_t address, uint32_t count,
1852                 uint32_t *checksum)
1853 {
1854         struct working_area *crc_algorithm;
1855         struct reg_param reg_params[2];
1856         int retval;
1857
1858         LOG_DEBUG("address=0x%" TARGET_PRIxADDR "; count=0x%" PRIx32, address, count);
1859
1860         static const uint8_t riscv32_crc_code[] = {
1861 #include "../../contrib/loaders/checksum/riscv32_crc.inc"
1862         };
1863         static const uint8_t riscv64_crc_code[] = {
1864 #include "../../contrib/loaders/checksum/riscv64_crc.inc"
1865         };
1866
1867         static const uint8_t *crc_code;
1868
1869         unsigned xlen = riscv_xlen(target);
1870         unsigned crc_code_size;
1871         if (xlen == 32) {
1872                 crc_code = riscv32_crc_code;
1873                 crc_code_size = sizeof(riscv32_crc_code);
1874         } else {
1875                 crc_code = riscv64_crc_code;
1876                 crc_code_size = sizeof(riscv64_crc_code);
1877         }
1878
1879         if (count < crc_code_size * 4) {
1880                 /* Don't use the algorithm for relatively small buffers. It's faster
1881                  * just to read the memory.  target_checksum_memory() will take care of
1882                  * that if we fail. */
1883                 return ERROR_FAIL;
1884         }
1885
1886         retval = target_alloc_working_area(target, crc_code_size, &crc_algorithm);
1887         if (retval != ERROR_OK)
1888                 return retval;
1889
1890         if (crc_algorithm->address + crc_algorithm->size > address &&
1891                         crc_algorithm->address < address + count) {
1892                 /* Region to checksum overlaps with the work area we've been assigned.
1893                  * Bail. (Would be better to manually checksum what we read there, and
1894                  * use the algorithm for the rest.) */
1895                 target_free_working_area(target, crc_algorithm);
1896                 return ERROR_FAIL;
1897         }
1898
1899         retval = target_write_buffer(target, crc_algorithm->address, crc_code_size,
1900                         crc_code);
1901         if (retval != ERROR_OK) {
1902                 LOG_ERROR("Failed to write code to " TARGET_ADDR_FMT ": %d",
1903                                 crc_algorithm->address, retval);
1904                 target_free_working_area(target, crc_algorithm);
1905                 return retval;
1906         }
1907
1908         init_reg_param(&reg_params[0], "a0", xlen, PARAM_IN_OUT);
1909         init_reg_param(&reg_params[1], "a1", xlen, PARAM_OUT);
1910         buf_set_u64(reg_params[0].value, 0, xlen, address);
1911         buf_set_u64(reg_params[1].value, 0, xlen, count);
1912
1913         /* 20 second timeout/megabyte */
1914         int timeout = 20000 * (1 + (count / (1024 * 1024)));
1915
1916         retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
1917                         crc_algorithm->address,
1918                         0,      /* Leave exit point unspecified because we don't know. */
1919                         timeout, NULL);
1920
1921         if (retval == ERROR_OK)
1922                 *checksum = buf_get_u32(reg_params[0].value, 0, 32);
1923         else
1924                 LOG_ERROR("error executing RISC-V CRC algorithm");
1925
1926         destroy_reg_param(&reg_params[0]);
1927         destroy_reg_param(&reg_params[1]);
1928
1929         target_free_working_area(target, crc_algorithm);
1930
1931         LOG_DEBUG("checksum=0x%" PRIx32 ", result=%d", *checksum, retval);
1932
1933         return retval;
1934 }
1935
1936 /*** OpenOCD Helper Functions ***/
1937
1938 enum riscv_poll_hart {
1939         RPH_NO_CHANGE,
1940         RPH_DISCOVERED_HALTED,
1941         RPH_DISCOVERED_RUNNING,
1942         RPH_ERROR
1943 };
1944 static enum riscv_poll_hart riscv_poll_hart(struct target *target, int hartid)
1945 {
1946         RISCV_INFO(r);
1947         if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
1948                 return RPH_ERROR;
1949
1950         LOG_DEBUG("polling hart %d, target->state=%d", hartid, target->state);
1951
1952         /* If OpenOCD thinks we're running but this hart is halted then it's time
1953          * to raise an event. */
1954         bool halted = riscv_is_halted(target);
1955         if (target->state != TARGET_HALTED && halted) {
1956                 LOG_DEBUG("  triggered a halt");
1957                 r->on_halt(target);
1958                 return RPH_DISCOVERED_HALTED;
1959         } else if (target->state != TARGET_RUNNING && !halted) {
1960                 LOG_DEBUG("  triggered running");
1961                 target->state = TARGET_RUNNING;
1962                 target->debug_reason = DBG_REASON_NOTHALTED;
1963                 return RPH_DISCOVERED_RUNNING;
1964         }
1965
1966         return RPH_NO_CHANGE;
1967 }
1968
1969 int set_debug_reason(struct target *target, enum riscv_halt_reason halt_reason)
1970 {
1971         switch (halt_reason) {
1972                 case RISCV_HALT_BREAKPOINT:
1973                         target->debug_reason = DBG_REASON_BREAKPOINT;
1974                         break;
1975                 case RISCV_HALT_TRIGGER:
1976                         target->debug_reason = DBG_REASON_WATCHPOINT;
1977                         break;
1978                 case RISCV_HALT_INTERRUPT:
1979                 case RISCV_HALT_GROUP:
1980                         target->debug_reason = DBG_REASON_DBGRQ;
1981                         break;
1982                 case RISCV_HALT_SINGLESTEP:
1983                         target->debug_reason = DBG_REASON_SINGLESTEP;
1984                         break;
1985                 case RISCV_HALT_UNKNOWN:
1986                         target->debug_reason = DBG_REASON_UNDEFINED;
1987                         break;
1988                 case RISCV_HALT_ERROR:
1989                         return ERROR_FAIL;
1990         }
1991         LOG_DEBUG("[%s] debug_reason=%d", target_name(target), target->debug_reason);
1992         return ERROR_OK;
1993 }
1994
1995 /*** OpenOCD Interface ***/
1996 int riscv_openocd_poll(struct target *target)
1997 {
1998         LOG_DEBUG("polling all harts");
1999         int halted_hart = -1;
2000         if (riscv_rtos_enabled(target)) {
2001                 /* Check every hart for an event. */
2002                 for (int i = 0; i < riscv_count_harts(target); ++i) {
2003                         enum riscv_poll_hart out = riscv_poll_hart(target, i);
2004                         switch (out) {
2005                         case RPH_NO_CHANGE:
2006                         case RPH_DISCOVERED_RUNNING:
2007                                 continue;
2008                         case RPH_DISCOVERED_HALTED:
2009                                 halted_hart = i;
2010                                 break;
2011                         case RPH_ERROR:
2012                                 return ERROR_FAIL;
2013                         }
2014                 }
2015                 if (halted_hart == -1) {
2016                         LOG_DEBUG("  no harts just halted, target->state=%d", target->state);
2017                         return ERROR_OK;
2018                 }
2019                 LOG_DEBUG("  hart %d halted", halted_hart);
2020
2021                 target->state = TARGET_HALTED;
2022                 enum riscv_halt_reason halt_reason = riscv_halt_reason(target, halted_hart);
2023                 if (set_debug_reason(target, halt_reason) != ERROR_OK)
2024                         return ERROR_FAIL;
2025
2026                 target->rtos->current_threadid = halted_hart + 1;
2027                 target->rtos->current_thread = halted_hart + 1;
2028                 riscv_set_rtos_hartid(target, halted_hart);
2029
2030                 /* If we're here then at least one hart triggered.  That means we want
2031                  * to go and halt _every_ hart (configured with -rtos riscv) in the
2032                  * system, as that's the invariant we hold here.  Some harts might have
2033                  * already halted (as we're either in single-step mode or they also
2034                  * triggered a breakpoint), so don't attempt to halt those harts.
2035                  * riscv_halt() will do all that for us. */
2036                 riscv_halt(target);
2037
2038         } else if (target->smp) {
2039                 unsigned halts_discovered = 0;
2040                 unsigned total_targets = 0;
2041                 unsigned should_remain_halted = 0;
2042                 unsigned should_resume = 0;
2043                 unsigned i = 0;
2044                 for (struct target_list *list = target->head; list != NULL;
2045                                 list = list->next, i++) {
2046                         total_targets++;
2047                         struct target *t = list->target;
2048                         riscv_info_t *r = riscv_info(t);
2049                         enum riscv_poll_hart out = riscv_poll_hart(t, r->current_hartid);
2050                         switch (out) {
2051                         case RPH_NO_CHANGE:
2052                                 break;
2053                         case RPH_DISCOVERED_RUNNING:
2054                                 t->state = TARGET_RUNNING;
2055                                 t->debug_reason = DBG_REASON_NOTHALTED;
2056                                 break;
2057                         case RPH_DISCOVERED_HALTED:
2058                                 halts_discovered++;
2059                                 t->state = TARGET_HALTED;
2060                                 enum riscv_halt_reason halt_reason =
2061                                         riscv_halt_reason(t, r->current_hartid);
2062                                 if (set_debug_reason(t, halt_reason) != ERROR_OK)
2063                                         return ERROR_FAIL;
2064
2065                                 if (halt_reason == RISCV_HALT_BREAKPOINT) {
2066                                         int retval;
2067                                         switch (riscv_semihosting(t, &retval)) {
2068                                         case SEMI_NONE:
2069                                         case SEMI_WAITING:
2070                                                 /* This hart should remain halted. */
2071                                                 should_remain_halted++;
2072                                                 break;
2073                                         case SEMI_HANDLED:
2074                                                 /* This hart should be resumed, along with any other
2075                                                          * harts that halted due to haltgroups. */
2076                                                 should_resume++;
2077                                                 break;
2078                                         case SEMI_ERROR:
2079                                                 return retval;
2080                                         }
2081                                 } else if (halt_reason != RISCV_HALT_GROUP) {
2082                                         should_remain_halted++;
2083                                 }
2084                                 break;
2085
2086                         case RPH_ERROR:
2087                                 return ERROR_FAIL;
2088                         }
2089                 }
2090
2091                 LOG_DEBUG("should_remain_halted=%d, should_resume=%d",
2092                                   should_remain_halted, should_resume);
2093                 if (should_remain_halted && should_resume) {
2094                         LOG_WARNING("%d harts should remain halted, and %d should resume.",
2095                                                 should_remain_halted, should_resume);
2096                 }
2097                 if (should_remain_halted) {
2098                         LOG_DEBUG("halt all");
2099                         riscv_halt(target);
2100                 } else if (should_resume) {
2101                         LOG_DEBUG("resume all");
2102                         riscv_resume(target, true, 0, 0, 0, false);
2103                 }
2104                 return ERROR_OK;
2105
2106         } else {
2107                 enum riscv_poll_hart out = riscv_poll_hart(target,
2108                                 riscv_current_hartid(target));
2109                 if (out == RPH_NO_CHANGE || out == RPH_DISCOVERED_RUNNING)
2110                         return ERROR_OK;
2111                 else if (out == RPH_ERROR)
2112                         return ERROR_FAIL;
2113
2114                 halted_hart = riscv_current_hartid(target);
2115                 LOG_DEBUG("  hart %d halted", halted_hart);
2116
2117                 enum riscv_halt_reason halt_reason = riscv_halt_reason(target, halted_hart);
2118                 if (set_debug_reason(target, halt_reason) != ERROR_OK)
2119                         return ERROR_FAIL;
2120                 target->state = TARGET_HALTED;
2121         }
2122
2123         if (target->debug_reason == DBG_REASON_BREAKPOINT) {
2124                 int retval;
2125                 switch (riscv_semihosting(target, &retval)) {
2126                         case SEMI_NONE:
2127                         case SEMI_WAITING:
2128                                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
2129                                 break;
2130                         case SEMI_HANDLED:
2131                                 if (riscv_resume(target, true, 0, 0, 0, false) != ERROR_OK)
2132                                         return ERROR_FAIL;
2133                                 break;
2134                         case SEMI_ERROR:
2135                                 return retval;
2136                 }
2137         } else {
2138                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
2139         }
2140
2141         return ERROR_OK;
2142 }
2143
2144 int riscv_openocd_step(struct target *target, int current,
2145                 target_addr_t address, int handle_breakpoints)
2146 {
2147         LOG_DEBUG("stepping rtos hart");
2148
2149         if (!current)
2150                 riscv_set_register(target, GDB_REGNO_PC, address);
2151
2152         riscv_reg_t trigger_state[RISCV_MAX_HWBPS] = {0};
2153         if (disable_triggers(target, trigger_state) != ERROR_OK)
2154                 return ERROR_FAIL;
2155
2156         int out = riscv_step_rtos_hart(target);
2157         if (out != ERROR_OK) {
2158                 LOG_ERROR("unable to step rtos hart");
2159                 return out;
2160         }
2161
2162         register_cache_invalidate(target->reg_cache);
2163
2164         if (enable_triggers(target, trigger_state) != ERROR_OK)
2165                 return ERROR_FAIL;
2166
2167         target->state = TARGET_RUNNING;
2168         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
2169         target->state = TARGET_HALTED;
2170         target->debug_reason = DBG_REASON_SINGLESTEP;
2171         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
2172         return out;
2173 }
2174
2175 /* Command Handlers */
2176 COMMAND_HANDLER(riscv_set_command_timeout_sec)
2177 {
2178         if (CMD_ARGC != 1) {
2179                 LOG_ERROR("Command takes exactly 1 parameter");
2180                 return ERROR_COMMAND_SYNTAX_ERROR;
2181         }
2182         int timeout = atoi(CMD_ARGV[0]);
2183         if (timeout <= 0) {
2184                 LOG_ERROR("%s is not a valid integer argument for command.", CMD_ARGV[0]);
2185                 return ERROR_FAIL;
2186         }
2187
2188         riscv_command_timeout_sec = timeout;
2189
2190         return ERROR_OK;
2191 }
2192
2193 COMMAND_HANDLER(riscv_set_reset_timeout_sec)
2194 {
2195         if (CMD_ARGC != 1) {
2196                 LOG_ERROR("Command takes exactly 1 parameter");
2197                 return ERROR_COMMAND_SYNTAX_ERROR;
2198         }
2199         int timeout = atoi(CMD_ARGV[0]);
2200         if (timeout <= 0) {
2201                 LOG_ERROR("%s is not a valid integer argument for command.", CMD_ARGV[0]);
2202                 return ERROR_FAIL;
2203         }
2204
2205         riscv_reset_timeout_sec = timeout;
2206         return ERROR_OK;
2207 }
2208
2209 COMMAND_HANDLER(riscv_test_compliance) {
2210
2211         struct target *target = get_current_target(CMD_CTX);
2212
2213         RISCV_INFO(r);
2214
2215         if (CMD_ARGC > 0) {
2216                 LOG_ERROR("Command does not take any parameters.");
2217                 return ERROR_COMMAND_SYNTAX_ERROR;
2218         }
2219
2220         if (r->test_compliance) {
2221                 return r->test_compliance(target);
2222         } else {
2223                 LOG_ERROR("This target does not support this command (may implement an older version of the spec).");
2224                 return ERROR_FAIL;
2225         }
2226 }
2227
2228 COMMAND_HANDLER(riscv_set_prefer_sba)
2229 {
2230         if (CMD_ARGC != 1) {
2231                 LOG_ERROR("Command takes exactly 1 parameter");
2232                 return ERROR_COMMAND_SYNTAX_ERROR;
2233         }
2234         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_prefer_sba);
2235         return ERROR_OK;
2236 }
2237
2238 COMMAND_HANDLER(riscv_set_enable_virtual)
2239 {
2240         if (CMD_ARGC != 1) {
2241                 LOG_ERROR("Command takes exactly 1 parameter");
2242                 return ERROR_COMMAND_SYNTAX_ERROR;
2243         }
2244         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_enable_virtual);
2245         return ERROR_OK;
2246 }
2247
2248 void parse_error(const char *string, char c, unsigned position)
2249 {
2250         char buf[position+2];
2251         for (unsigned i = 0; i < position; i++)
2252                 buf[i] = ' ';
2253         buf[position] = '^';
2254         buf[position + 1] = 0;
2255
2256         LOG_ERROR("Parse error at character %c in:", c);
2257         LOG_ERROR("%s", string);
2258         LOG_ERROR("%s", buf);
2259 }
2260
2261 int parse_ranges(range_t **ranges, const char **argv)
2262 {
2263         for (unsigned pass = 0; pass < 2; pass++) {
2264                 unsigned range = 0;
2265                 unsigned low = 0;
2266                 bool parse_low = true;
2267                 unsigned high = 0;
2268                 for (unsigned i = 0; i == 0 || argv[0][i-1]; i++) {
2269                         char c = argv[0][i];
2270                         if (isspace(c)) {
2271                                 /* Ignore whitespace. */
2272                                 continue;
2273                         }
2274
2275                         if (parse_low) {
2276                                 if (isdigit(c)) {
2277                                         low *= 10;
2278                                         low += c - '0';
2279                                 } else if (c == '-') {
2280                                         parse_low = false;
2281                                 } else if (c == ',' || c == 0) {
2282                                         if (pass == 1) {
2283                                                 (*ranges)[range].low = low;
2284                                                 (*ranges)[range].high = low;
2285                                         }
2286                                         low = 0;
2287                                         range++;
2288                                 } else {
2289                                         parse_error(argv[0], c, i);
2290                                         return ERROR_COMMAND_SYNTAX_ERROR;
2291                                 }
2292
2293                         } else {
2294                                 if (isdigit(c)) {
2295                                         high *= 10;
2296                                         high += c - '0';
2297                                 } else if (c == ',' || c == 0) {
2298                                         parse_low = true;
2299                                         if (pass == 1) {
2300                                                 (*ranges)[range].low = low;
2301                                                 (*ranges)[range].high = high;
2302                                         }
2303                                         low = 0;
2304                                         high = 0;
2305                                         range++;
2306                                 } else {
2307                                         parse_error(argv[0], c, i);
2308                                         return ERROR_COMMAND_SYNTAX_ERROR;
2309                                 }
2310                         }
2311                 }
2312
2313                 if (pass == 0) {
2314                         free(*ranges);
2315                         *ranges = calloc(range + 2, sizeof(range_t));
2316                         if (!*ranges)
2317                                 return ERROR_FAIL;
2318                 } else {
2319                         (*ranges)[range].low = 1;
2320                         (*ranges)[range].high = 0;
2321                 }
2322         }
2323
2324         return ERROR_OK;
2325 }
2326
2327 COMMAND_HANDLER(riscv_set_expose_csrs)
2328 {
2329         if (CMD_ARGC != 1) {
2330                 LOG_ERROR("Command takes exactly 1 parameter");
2331                 return ERROR_COMMAND_SYNTAX_ERROR;
2332         }
2333
2334         return parse_ranges(&expose_csr, CMD_ARGV);
2335 }
2336
2337 COMMAND_HANDLER(riscv_set_expose_custom)
2338 {
2339         if (CMD_ARGC != 1) {
2340                 LOG_ERROR("Command takes exactly 1 parameter");
2341                 return ERROR_COMMAND_SYNTAX_ERROR;
2342         }
2343
2344         return parse_ranges(&expose_custom, CMD_ARGV);
2345 }
2346
2347 COMMAND_HANDLER(riscv_authdata_read)
2348 {
2349         if (CMD_ARGC != 0) {
2350                 LOG_ERROR("Command takes no parameters");
2351                 return ERROR_COMMAND_SYNTAX_ERROR;
2352         }
2353
2354         struct target *target = get_current_target(CMD_CTX);
2355         if (!target) {
2356                 LOG_ERROR("target is NULL!");
2357                 return ERROR_FAIL;
2358         }
2359
2360         RISCV_INFO(r);
2361         if (!r) {
2362                 LOG_ERROR("riscv_info is NULL!");
2363                 return ERROR_FAIL;
2364         }
2365
2366         if (r->authdata_read) {
2367                 uint32_t value;
2368                 if (r->authdata_read(target, &value) != ERROR_OK)
2369                         return ERROR_FAIL;
2370                 command_print_sameline(CMD, "0x%08" PRIx32, value);
2371                 return ERROR_OK;
2372         } else {
2373                 LOG_ERROR("authdata_read is not implemented for this target.");
2374                 return ERROR_FAIL;
2375         }
2376 }
2377
2378 COMMAND_HANDLER(riscv_authdata_write)
2379 {
2380         if (CMD_ARGC != 1) {
2381                 LOG_ERROR("Command takes exactly 1 argument");
2382                 return ERROR_COMMAND_SYNTAX_ERROR;
2383         }
2384
2385         struct target *target = get_current_target(CMD_CTX);
2386         RISCV_INFO(r);
2387
2388         uint32_t value;
2389         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], value);
2390
2391         if (r->authdata_write) {
2392                 return r->authdata_write(target, value);
2393         } else {
2394                 LOG_ERROR("authdata_write is not implemented for this target.");
2395                 return ERROR_FAIL;
2396         }
2397 }
2398
2399 COMMAND_HANDLER(riscv_dmi_read)
2400 {
2401         if (CMD_ARGC != 1) {
2402                 LOG_ERROR("Command takes 1 parameter");
2403                 return ERROR_COMMAND_SYNTAX_ERROR;
2404         }
2405
2406         struct target *target = get_current_target(CMD_CTX);
2407         if (!target) {
2408                 LOG_ERROR("target is NULL!");
2409                 return ERROR_FAIL;
2410         }
2411
2412         RISCV_INFO(r);
2413         if (!r) {
2414                 LOG_ERROR("riscv_info is NULL!");
2415                 return ERROR_FAIL;
2416         }
2417
2418         if (r->dmi_read) {
2419                 uint32_t address, value;
2420                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
2421                 if (r->dmi_read(target, &value, address) != ERROR_OK)
2422                         return ERROR_FAIL;
2423                 command_print(CMD, "0x%" PRIx32, value);
2424                 return ERROR_OK;
2425         } else {
2426                 LOG_ERROR("dmi_read is not implemented for this target.");
2427                 return ERROR_FAIL;
2428         }
2429 }
2430
2431
2432 COMMAND_HANDLER(riscv_dmi_write)
2433 {
2434         if (CMD_ARGC != 2) {
2435                 LOG_ERROR("Command takes exactly 2 arguments");
2436                 return ERROR_COMMAND_SYNTAX_ERROR;
2437         }
2438
2439         struct target *target = get_current_target(CMD_CTX);
2440         RISCV_INFO(r);
2441
2442         uint32_t address, value;
2443         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
2444         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
2445
2446         if (r->dmi_write) {
2447                 return r->dmi_write(target, address, value);
2448         } else {
2449                 LOG_ERROR("dmi_write is not implemented for this target.");
2450                 return ERROR_FAIL;
2451         }
2452 }
2453
2454 COMMAND_HANDLER(riscv_test_sba_config_reg)
2455 {
2456         if (CMD_ARGC != 4) {
2457                 LOG_ERROR("Command takes exactly 4 arguments");
2458                 return ERROR_COMMAND_SYNTAX_ERROR;
2459         }
2460
2461         struct target *target = get_current_target(CMD_CTX);
2462         RISCV_INFO(r);
2463
2464         target_addr_t legal_address;
2465         uint32_t num_words;
2466         target_addr_t illegal_address;
2467         bool run_sbbusyerror_test;
2468
2469         COMMAND_PARSE_NUMBER(target_addr, CMD_ARGV[0], legal_address);
2470         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], num_words);
2471         COMMAND_PARSE_NUMBER(target_addr, CMD_ARGV[2], illegal_address);
2472         COMMAND_PARSE_ON_OFF(CMD_ARGV[3], run_sbbusyerror_test);
2473
2474         if (r->test_sba_config_reg) {
2475                 return r->test_sba_config_reg(target, legal_address, num_words,
2476                                 illegal_address, run_sbbusyerror_test);
2477         } else {
2478                 LOG_ERROR("test_sba_config_reg is not implemented for this target.");
2479                 return ERROR_FAIL;
2480         }
2481 }
2482
2483 COMMAND_HANDLER(riscv_reset_delays)
2484 {
2485         int wait = 0;
2486
2487         if (CMD_ARGC > 1) {
2488                 LOG_ERROR("Command takes at most one argument");
2489                 return ERROR_COMMAND_SYNTAX_ERROR;
2490         }
2491
2492         if (CMD_ARGC == 1)
2493                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], wait);
2494
2495         struct target *target = get_current_target(CMD_CTX);
2496         RISCV_INFO(r);
2497         r->reset_delays_wait = wait;
2498         return ERROR_OK;
2499 }
2500
2501 COMMAND_HANDLER(riscv_set_ir)
2502 {
2503         if (CMD_ARGC != 2) {
2504                 LOG_ERROR("Command takes exactly 2 arguments");
2505                 return ERROR_COMMAND_SYNTAX_ERROR;
2506         }
2507
2508         uint32_t value;
2509         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
2510
2511         if (!strcmp(CMD_ARGV[0], "idcode"))
2512                 buf_set_u32(ir_idcode, 0, 32, value);
2513         else if (!strcmp(CMD_ARGV[0], "dtmcs"))
2514                 buf_set_u32(ir_dtmcontrol, 0, 32, value);
2515         else if (!strcmp(CMD_ARGV[0], "dmi"))
2516                 buf_set_u32(ir_dbus, 0, 32, value);
2517         else
2518                 return ERROR_FAIL;
2519
2520         return ERROR_OK;
2521 }
2522
2523 COMMAND_HANDLER(riscv_resume_order)
2524 {
2525         if (CMD_ARGC > 1) {
2526                 LOG_ERROR("Command takes at most one argument");
2527                 return ERROR_COMMAND_SYNTAX_ERROR;
2528         }
2529
2530         if (!strcmp(CMD_ARGV[0], "normal")) {
2531                 resume_order = RO_NORMAL;
2532         } else if (!strcmp(CMD_ARGV[0], "reversed")) {
2533                 resume_order = RO_REVERSED;
2534         } else {
2535                 LOG_ERROR("Unsupported resume order: %s", CMD_ARGV[0]);
2536                 return ERROR_FAIL;
2537         }
2538
2539         return ERROR_OK;
2540 }
2541
2542 COMMAND_HANDLER(riscv_use_bscan_tunnel)
2543 {
2544         int irwidth = 0;
2545         int tunnel_type = BSCAN_TUNNEL_NESTED_TAP;
2546
2547         if (CMD_ARGC > 2) {
2548                 LOG_ERROR("Command takes at most two arguments");
2549                 return ERROR_COMMAND_SYNTAX_ERROR;
2550         } else if (CMD_ARGC == 1) {
2551                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], irwidth);
2552         } else if (CMD_ARGC == 2) {
2553                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], irwidth);
2554                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], tunnel_type);
2555         }
2556         if (tunnel_type == BSCAN_TUNNEL_NESTED_TAP)
2557                 LOG_INFO("Nested Tap based Bscan Tunnel Selected");
2558         else if (tunnel_type == BSCAN_TUNNEL_DATA_REGISTER)
2559                 LOG_INFO("Simple Register based Bscan Tunnel Selected");
2560         else
2561                 LOG_INFO("Invalid Tunnel type selected ! : selecting default Nested Tap Type");
2562
2563         bscan_tunnel_type = tunnel_type;
2564         bscan_tunnel_ir_width = irwidth;
2565         return ERROR_OK;
2566 }
2567
2568 COMMAND_HANDLER(riscv_set_enable_virt2phys)
2569 {
2570         if (CMD_ARGC != 1) {
2571                 LOG_ERROR("Command takes exactly 1 parameter");
2572                 return ERROR_COMMAND_SYNTAX_ERROR;
2573         }
2574         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_enable_virt2phys);
2575         return ERROR_OK;
2576 }
2577
2578 COMMAND_HANDLER(riscv_set_ebreakm)
2579 {
2580         if (CMD_ARGC != 1) {
2581                 LOG_ERROR("Command takes exactly 1 parameter");
2582                 return ERROR_COMMAND_SYNTAX_ERROR;
2583         }
2584         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_ebreakm);
2585         return ERROR_OK;
2586 }
2587
2588 COMMAND_HANDLER(riscv_set_ebreaks)
2589 {
2590         if (CMD_ARGC != 1) {
2591                 LOG_ERROR("Command takes exactly 1 parameter");
2592                 return ERROR_COMMAND_SYNTAX_ERROR;
2593         }
2594         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_ebreaks);
2595         return ERROR_OK;
2596 }
2597
2598 COMMAND_HANDLER(riscv_set_ebreaku)
2599 {
2600         if (CMD_ARGC != 1) {
2601                 LOG_ERROR("Command takes exactly 1 parameter");
2602                 return ERROR_COMMAND_SYNTAX_ERROR;
2603         }
2604         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_ebreaku);
2605         return ERROR_OK;
2606 }
2607
2608 static const struct command_registration riscv_exec_command_handlers[] = {
2609         {
2610                 .name = "test_compliance",
2611                 .handler = riscv_test_compliance,
2612                 .usage = "",
2613                 .mode = COMMAND_EXEC,
2614                 .help = "Runs a basic compliance test suite against the RISC-V Debug Spec."
2615         },
2616         {
2617                 .name = "set_command_timeout_sec",
2618                 .handler = riscv_set_command_timeout_sec,
2619                 .mode = COMMAND_ANY,
2620                 .usage = "[sec]",
2621                 .help = "Set the wall-clock timeout (in seconds) for individual commands"
2622         },
2623         {
2624                 .name = "set_reset_timeout_sec",
2625                 .handler = riscv_set_reset_timeout_sec,
2626                 .mode = COMMAND_ANY,
2627                 .usage = "[sec]",
2628                 .help = "Set the wall-clock timeout (in seconds) after reset is deasserted"
2629         },
2630         {
2631                 .name = "set_prefer_sba",
2632                 .handler = riscv_set_prefer_sba,
2633                 .mode = COMMAND_ANY,
2634                 .usage = "on|off",
2635                 .help = "When on, prefer to use System Bus Access to access memory. "
2636                         "When off (default), prefer to use the Program Buffer to access memory."
2637         },
2638         {
2639                 .name = "set_enable_virtual",
2640                 .handler = riscv_set_enable_virtual,
2641                 .mode = COMMAND_ANY,
2642                 .usage = "on|off",
2643                 .help = "When on, memory accesses are performed on physical or virtual "
2644                                 "memory depending on the current system configuration. "
2645                                 "When off (default), all memory accessses are performed on physical memory."
2646         },
2647         {
2648                 .name = "expose_csrs",
2649                 .handler = riscv_set_expose_csrs,
2650                 .mode = COMMAND_ANY,
2651                 .usage = "n0[-m0][,n1[-m1]]...",
2652                 .help = "Configure a list of inclusive ranges for CSRs to expose in "
2653                                 "addition to the standard ones. This must be executed before "
2654                                 "`init`."
2655         },
2656         {
2657                 .name = "expose_custom",
2658                 .handler = riscv_set_expose_custom,
2659                 .mode = COMMAND_ANY,
2660                 .usage = "n0[-m0][,n1[-m1]]...",
2661                 .help = "Configure a list of inclusive ranges for custom registers to "
2662                         "expose. custom0 is accessed as abstract register number 0xc000, "
2663                         "etc. This must be executed before `init`."
2664         },
2665         {
2666                 .name = "authdata_read",
2667                 .handler = riscv_authdata_read,
2668                 .usage = "",
2669                 .mode = COMMAND_ANY,
2670                 .help = "Return the 32-bit value read from authdata."
2671         },
2672         {
2673                 .name = "authdata_write",
2674                 .handler = riscv_authdata_write,
2675                 .mode = COMMAND_ANY,
2676                 .usage = "value",
2677                 .help = "Write the 32-bit value to authdata."
2678         },
2679         {
2680                 .name = "dmi_read",
2681                 .handler = riscv_dmi_read,
2682                 .mode = COMMAND_ANY,
2683                 .usage = "address",
2684                 .help = "Perform a 32-bit DMI read at address, returning the value."
2685         },
2686         {
2687                 .name = "dmi_write",
2688                 .handler = riscv_dmi_write,
2689                 .mode = COMMAND_ANY,
2690                 .usage = "address value",
2691                 .help = "Perform a 32-bit DMI write of value at address."
2692         },
2693         {
2694                 .name = "test_sba_config_reg",
2695                 .handler = riscv_test_sba_config_reg,
2696                 .mode = COMMAND_ANY,
2697                 .usage = "legal_address num_words "
2698                         "illegal_address run_sbbusyerror_test[on/off]",
2699                 .help = "Perform a series of tests on the SBCS register. "
2700                         "Inputs are a legal, 128-byte aligned address and a number of words to "
2701                         "read/write starting at that address (i.e., address range [legal address, "
2702                         "legal_address+word_size*num_words) must be legally readable/writable), "
2703                         "an illegal, 128-byte aligned address for error flag/handling cases, "
2704                         "and whether sbbusyerror test should be run."
2705         },
2706         {
2707                 .name = "reset_delays",
2708                 .handler = riscv_reset_delays,
2709                 .mode = COMMAND_ANY,
2710                 .usage = "[wait]",
2711                 .help = "OpenOCD learns how many Run-Test/Idle cycles are required "
2712                         "between scans to avoid encountering the target being busy. This "
2713                         "command resets those learned values after `wait` scans. It's only "
2714                         "useful for testing OpenOCD itself."
2715         },
2716         {
2717                 .name = "resume_order",
2718                 .handler = riscv_resume_order,
2719                 .mode = COMMAND_ANY,
2720                 .usage = "normal|reversed",
2721                 .help = "Choose the order that harts are resumed in when `hasel` is not "
2722                         "supported. Normal order is from lowest hart index to highest. "
2723                         "Reversed order is from highest hart index to lowest."
2724         },
2725         {
2726                 .name = "set_ir",
2727                 .handler = riscv_set_ir,
2728                 .mode = COMMAND_ANY,
2729                 .usage = "[idcode|dtmcs|dmi] value",
2730                 .help = "Set IR value for specified JTAG register."
2731         },
2732         {
2733                 .name = "use_bscan_tunnel",
2734                 .handler = riscv_use_bscan_tunnel,
2735                 .mode = COMMAND_ANY,
2736                 .usage = "value [type]",
2737                 .help = "Enable or disable use of a BSCAN tunnel to reach DM.  Supply "
2738                         "the width of the DM transport TAP's instruction register to "
2739                         "enable.  Supply a value of 0 to disable. Pass A second argument "
2740                         "(optional) to indicate Bscan Tunnel Type {0:(default) NESTED_TAP , "
2741                         "1: DATA_REGISTER}"
2742         },
2743         {
2744                 .name = "set_enable_virt2phys",
2745                 .handler = riscv_set_enable_virt2phys,
2746                 .mode = COMMAND_ANY,
2747                 .usage = "on|off",
2748                 .help = "When on (default), enable translation from virtual address to "
2749                         "physical address."
2750         },
2751         {
2752                 .name = "set_ebreakm",
2753                 .handler = riscv_set_ebreakm,
2754                 .mode = COMMAND_ANY,
2755                 .usage = "on|off",
2756                 .help = "Control dcsr.ebreakm. When off, M-mode ebreak instructions "
2757                         "don't trap to OpenOCD. Defaults to on."
2758         },
2759         {
2760                 .name = "set_ebreaks",
2761                 .handler = riscv_set_ebreaks,
2762                 .mode = COMMAND_ANY,
2763                 .usage = "on|off",
2764                 .help = "Control dcsr.ebreaks. When off, S-mode ebreak instructions "
2765                         "don't trap to OpenOCD. Defaults to on."
2766         },
2767         {
2768                 .name = "set_ebreaku",
2769                 .handler = riscv_set_ebreaku,
2770                 .mode = COMMAND_ANY,
2771                 .usage = "on|off",
2772                 .help = "Control dcsr.ebreaku. When off, U-mode ebreak instructions "
2773                         "don't trap to OpenOCD. Defaults to on."
2774         },
2775         COMMAND_REGISTRATION_DONE
2776 };
2777
2778 /*
2779  * To be noted that RISC-V targets use the same semihosting commands as
2780  * ARM targets.
2781  *
2782  * The main reason is compatibility with existing tools. For example the
2783  * Eclipse OpenOCD/SEGGER J-Link/QEMU plug-ins have several widgets to
2784  * configure semihosting, which generate commands like `arm semihosting
2785  * enable`.
2786  * A secondary reason is the fact that the protocol used is exactly the
2787  * one specified by ARM. If RISC-V will ever define its own semihosting
2788  * protocol, then a command like `riscv semihosting enable` will make
2789  * sense, but for now all semihosting commands are prefixed with `arm`.
2790  */
2791 extern const struct command_registration semihosting_common_handlers[];
2792
2793 const struct command_registration riscv_command_handlers[] = {
2794         {
2795                 .name = "riscv",
2796                 .mode = COMMAND_ANY,
2797                 .help = "RISC-V Command Group",
2798                 .usage = "",
2799                 .chain = riscv_exec_command_handlers
2800         },
2801         {
2802                 .name = "arm",
2803                 .mode = COMMAND_ANY,
2804                 .help = "ARM Command Group",
2805                 .usage = "",
2806                 .chain = semihosting_common_handlers
2807         },
2808         COMMAND_REGISTRATION_DONE
2809 };
2810
2811 static unsigned riscv_xlen_nonconst(struct target *target)
2812 {
2813         return riscv_xlen(target);
2814 }
2815
2816 static unsigned int riscv_data_bits(struct target *target)
2817 {
2818         RISCV_INFO(r);
2819         if (r->data_bits)
2820                 return r->data_bits(target);
2821         return riscv_xlen(target);
2822 }
2823
2824 struct target_type riscv_target = {
2825         .name = "riscv",
2826
2827         .init_target = riscv_init_target,
2828         .deinit_target = riscv_deinit_target,
2829         .examine = riscv_examine,
2830
2831         /* poll current target status */
2832         .poll = old_or_new_riscv_poll,
2833
2834         .halt = riscv_halt,
2835         .resume = riscv_target_resume,
2836         .step = old_or_new_riscv_step,
2837
2838         .assert_reset = riscv_assert_reset,
2839         .deassert_reset = riscv_deassert_reset,
2840
2841         .read_memory = riscv_read_memory,
2842         .write_memory = riscv_write_memory,
2843         .read_phys_memory = riscv_read_phys_memory,
2844         .write_phys_memory = riscv_write_phys_memory,
2845
2846         .checksum_memory = riscv_checksum_memory,
2847
2848         .mmu = riscv_mmu,
2849         .virt2phys = riscv_virt2phys,
2850
2851         .get_gdb_reg_list = riscv_get_gdb_reg_list,
2852         .get_gdb_reg_list_noread = riscv_get_gdb_reg_list_noread,
2853
2854         .add_breakpoint = riscv_add_breakpoint,
2855         .remove_breakpoint = riscv_remove_breakpoint,
2856
2857         .add_watchpoint = riscv_add_watchpoint,
2858         .remove_watchpoint = riscv_remove_watchpoint,
2859         .hit_watchpoint = riscv_hit_watchpoint,
2860
2861         .arch_state = riscv_arch_state,
2862
2863         .run_algorithm = riscv_run_algorithm,
2864
2865         .commands = riscv_command_handlers,
2866
2867         .address_bits = riscv_xlen_nonconst,
2868         .data_bits = riscv_data_bits
2869 };
2870
2871 /*** RISC-V Interface ***/
2872
2873 void riscv_info_init(struct target *target, riscv_info_t *r)
2874 {
2875         memset(r, 0, sizeof(*r));
2876         r->dtm_version = 1;
2877         r->registers_initialized = false;
2878         r->current_hartid = target->coreid;
2879
2880         memset(r->trigger_unique_id, 0xff, sizeof(r->trigger_unique_id));
2881
2882         for (size_t h = 0; h < RISCV_MAX_HARTS; ++h)
2883                 r->xlen[h] = -1;
2884 }
2885
2886 static int riscv_resume_go_all_harts(struct target *target)
2887 {
2888         RISCV_INFO(r);
2889
2890         /* Dummy variables to make mingw32-gcc happy. */
2891         int first = 0;
2892         int last = 1;
2893         int step = 1;
2894         switch (resume_order) {
2895                 case RO_NORMAL:
2896                         first = 0;
2897                         last = riscv_count_harts(target) - 1;
2898                         step = 1;
2899                         break;
2900                 case RO_REVERSED:
2901                         first = riscv_count_harts(target) - 1;
2902                         last = 0;
2903                         step = -1;
2904                         break;
2905                 default:
2906                         assert(0);
2907         }
2908
2909         for (int i = first; i != last + step; i += step) {
2910                 if (!riscv_hart_enabled(target, i))
2911                         continue;
2912
2913                 LOG_DEBUG("resuming hart %d", i);
2914                 if (riscv_set_current_hartid(target, i) != ERROR_OK)
2915                         return ERROR_FAIL;
2916                 if (riscv_is_halted(target)) {
2917                         if (r->resume_go(target) != ERROR_OK)
2918                                 return ERROR_FAIL;
2919                 } else {
2920                         LOG_DEBUG("  hart %d requested resume, but was already resumed", i);
2921                 }
2922         }
2923
2924         riscv_invalidate_register_cache(target);
2925         return ERROR_OK;
2926 }
2927
2928 int riscv_step_rtos_hart(struct target *target)
2929 {
2930         RISCV_INFO(r);
2931         int hartid = r->current_hartid;
2932         if (riscv_rtos_enabled(target)) {
2933                 hartid = r->rtos_hartid;
2934                 if (hartid == -1) {
2935                         LOG_DEBUG("GDB has asked me to step \"any\" thread, so I'm stepping hart 0.");
2936                         hartid = 0;
2937                 }
2938         }
2939         if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
2940                 return ERROR_FAIL;
2941         LOG_DEBUG("stepping hart %d", hartid);
2942
2943         if (!riscv_is_halted(target)) {
2944                 LOG_ERROR("Hart isn't halted before single step!");
2945                 return ERROR_FAIL;
2946         }
2947         riscv_invalidate_register_cache(target);
2948         r->on_step(target);
2949         if (r->step_current_hart(target) != ERROR_OK)
2950                 return ERROR_FAIL;
2951         riscv_invalidate_register_cache(target);
2952         r->on_halt(target);
2953         if (!riscv_is_halted(target)) {
2954                 LOG_ERROR("Hart was not halted after single step!");
2955                 return ERROR_FAIL;
2956         }
2957         return ERROR_OK;
2958 }
2959
2960 bool riscv_supports_extension(struct target *target, int hartid, char letter)
2961 {
2962         RISCV_INFO(r);
2963         unsigned num;
2964         if (letter >= 'a' && letter <= 'z')
2965                 num = letter - 'a';
2966         else if (letter >= 'A' && letter <= 'Z')
2967                 num = letter - 'A';
2968         else
2969                 return false;
2970         return r->misa[hartid] & (1 << num);
2971 }
2972
2973 unsigned riscv_xlen(const struct target *target)
2974 {
2975         return riscv_xlen_of_hart(target, riscv_current_hartid(target));
2976 }
2977
2978 int riscv_xlen_of_hart(const struct target *target, int hartid)
2979 {
2980         RISCV_INFO(r);
2981         assert(r->xlen[hartid] != -1);
2982         return r->xlen[hartid];
2983 }
2984
2985 bool riscv_rtos_enabled(const struct target *target)
2986 {
2987         return false;
2988 }
2989
2990 int riscv_set_current_hartid(struct target *target, int hartid)
2991 {
2992         RISCV_INFO(r);
2993         if (!r->select_current_hart)
2994                 return ERROR_OK;
2995
2996         int previous_hartid = riscv_current_hartid(target);
2997         r->current_hartid = hartid;
2998         assert(riscv_hart_enabled(target, hartid));
2999         LOG_DEBUG("setting hartid to %d, was %d", hartid, previous_hartid);
3000         if (r->select_current_hart(target) != ERROR_OK)
3001                 return ERROR_FAIL;
3002
3003         /* This might get called during init, in which case we shouldn't be
3004          * setting up the register cache. */
3005         if (target_was_examined(target) && riscv_rtos_enabled(target))
3006                 riscv_invalidate_register_cache(target);
3007
3008         return ERROR_OK;
3009 }
3010
3011 void riscv_invalidate_register_cache(struct target *target)
3012 {
3013         RISCV_INFO(r);
3014
3015         LOG_DEBUG("[%d]", target->coreid);
3016         register_cache_invalidate(target->reg_cache);
3017         for (size_t i = 0; i < target->reg_cache->num_regs; ++i) {
3018                 struct reg *reg = &target->reg_cache->reg_list[i];
3019                 reg->valid = false;
3020         }
3021
3022         r->registers_initialized = true;
3023 }
3024
3025 int riscv_current_hartid(const struct target *target)
3026 {
3027         RISCV_INFO(r);
3028         return r->current_hartid;
3029 }
3030
3031 void riscv_set_all_rtos_harts(struct target *target)
3032 {
3033         RISCV_INFO(r);
3034         r->rtos_hartid = -1;
3035 }
3036
3037 void riscv_set_rtos_hartid(struct target *target, int hartid)
3038 {
3039         LOG_DEBUG("setting RTOS hartid %d", hartid);
3040         RISCV_INFO(r);
3041         r->rtos_hartid = hartid;
3042 }
3043
3044 int riscv_count_harts(struct target *target)
3045 {
3046         if (target == NULL)
3047                 return 1;
3048         RISCV_INFO(r);
3049         if (r == NULL || r->hart_count == NULL)
3050                 return 1;
3051         return r->hart_count(target);
3052 }
3053
3054 bool riscv_has_register(struct target *target, int hartid, int regid)
3055 {
3056         return 1;
3057 }
3058
3059 /**
3060  * If write is true:
3061  *   return true iff we are guaranteed that the register will contain exactly
3062  *       the value we just wrote when it's read.
3063  * If write is false:
3064  *   return true iff we are guaranteed that the register will read the same
3065  *       value in the future as the value we just read.
3066  */
3067 static bool gdb_regno_cacheable(enum gdb_regno regno, bool write)
3068 {
3069         /* GPRs, FPRs, vector registers are just normal data stores. */
3070         if (regno <= GDB_REGNO_XPR31 ||
3071                         (regno >= GDB_REGNO_FPR0 && regno <= GDB_REGNO_FPR31) ||
3072                         (regno >= GDB_REGNO_V0 && regno <= GDB_REGNO_V31))
3073                 return true;
3074
3075         /* Most CSRs won't change value on us, but we can't assume it about rbitrary
3076          * CSRs. */
3077         switch (regno) {
3078                 case GDB_REGNO_DPC:
3079                         return true;
3080
3081                 case GDB_REGNO_VSTART:
3082                 case GDB_REGNO_VXSAT:
3083                 case GDB_REGNO_VXRM:
3084                 case GDB_REGNO_VLENB:
3085                 case GDB_REGNO_VL:
3086                 case GDB_REGNO_VTYPE:
3087                 case GDB_REGNO_MISA:
3088                 case GDB_REGNO_DCSR:
3089                 case GDB_REGNO_DSCRATCH0:
3090                 case GDB_REGNO_MSTATUS:
3091                 case GDB_REGNO_MEPC:
3092                 case GDB_REGNO_MCAUSE:
3093                 case GDB_REGNO_SATP:
3094                         /*
3095                          * WARL registers might not contain the value we just wrote, but
3096                          * these ones won't spontaneously change their value either. *
3097                          */
3098                         return !write;
3099
3100                 case GDB_REGNO_TSELECT: /* I think this should be above, but then it doesn't work. */
3101                 case GDB_REGNO_TDATA1:  /* Changes value when tselect is changed. */
3102                 case GDB_REGNO_TDATA2:  /* Changse value when tselect is changed. */
3103                 default:
3104                         return false;
3105         }
3106 }
3107
3108 /**
3109  * This function is called when the debug user wants to change the value of a
3110  * register. The new value may be cached, and may not be written until the hart
3111  * is resumed. */
3112 int riscv_set_register(struct target *target, enum gdb_regno r, riscv_reg_t v)
3113 {
3114         return riscv_set_register_on_hart(target, riscv_current_hartid(target), r, v);
3115 }
3116
3117 int riscv_set_register_on_hart(struct target *target, int hartid,
3118                 enum gdb_regno regid, uint64_t value)
3119 {
3120         RISCV_INFO(r);
3121         LOG_DEBUG("{%d} %s <- %" PRIx64, hartid, gdb_regno_name(regid), value);
3122         assert(r->set_register);
3123
3124         /* TODO: Hack to deal with gdb that thinks these registers still exist. */
3125         if (regid > GDB_REGNO_XPR15 && regid <= GDB_REGNO_XPR31 && value == 0 &&
3126                         riscv_supports_extension(target, hartid, 'E'))
3127                 return ERROR_OK;
3128
3129         struct reg *reg = &target->reg_cache->reg_list[regid];
3130         buf_set_u64(reg->value, 0, reg->size, value);
3131
3132         int result = r->set_register(target, hartid, regid, value);
3133         if (result == ERROR_OK)
3134                 reg->valid = gdb_regno_cacheable(regid, true);
3135         else
3136                 reg->valid = false;
3137         LOG_DEBUG("[%s]{%d} wrote 0x%" PRIx64 " to %s valid=%d",
3138                           target_name(target), hartid, value, reg->name, reg->valid);
3139         return result;
3140 }
3141
3142 int riscv_get_register(struct target *target, riscv_reg_t *value,
3143                 enum gdb_regno r)
3144 {
3145         return riscv_get_register_on_hart(target, value,
3146                         riscv_current_hartid(target), r);
3147 }
3148
3149 int riscv_get_register_on_hart(struct target *target, riscv_reg_t *value,
3150                 int hartid, enum gdb_regno regid)
3151 {
3152         RISCV_INFO(r);
3153
3154         struct reg *reg = &target->reg_cache->reg_list[regid];
3155         if (!reg->exist) {
3156                 LOG_DEBUG("[%s]{%d} %s does not exist.",
3157                                   target_name(target), hartid, gdb_regno_name(regid));
3158                 return ERROR_FAIL;
3159         }
3160
3161         if (reg && reg->valid && hartid == riscv_current_hartid(target)) {
3162                 *value = buf_get_u64(reg->value, 0, reg->size);
3163                 LOG_DEBUG("{%d} %s: %" PRIx64 " (cached)", hartid,
3164                                   gdb_regno_name(regid), *value);
3165                 return ERROR_OK;
3166         }
3167
3168         /* TODO: Hack to deal with gdb that thinks these registers still exist. */
3169         if (regid > GDB_REGNO_XPR15 && regid <= GDB_REGNO_XPR31 &&
3170                         riscv_supports_extension(target, hartid, 'E')) {
3171                 *value = 0;
3172                 return ERROR_OK;
3173         }
3174
3175         int result = r->get_register(target, value, hartid, regid);
3176
3177         if (result == ERROR_OK)
3178                 reg->valid = gdb_regno_cacheable(regid, false);
3179
3180         LOG_DEBUG("{%d} %s: %" PRIx64, hartid, gdb_regno_name(regid), *value);
3181         return result;
3182 }
3183
3184 bool riscv_is_halted(struct target *target)
3185 {
3186         RISCV_INFO(r);
3187         assert(r->is_halted);
3188         return r->is_halted(target);
3189 }
3190
3191 enum riscv_halt_reason riscv_halt_reason(struct target *target, int hartid)
3192 {
3193         RISCV_INFO(r);
3194         if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
3195                 return RISCV_HALT_ERROR;
3196         if (!riscv_is_halted(target)) {
3197                 LOG_ERROR("Hart is not halted!");
3198                 return RISCV_HALT_UNKNOWN;
3199         }
3200         return r->halt_reason(target);
3201 }
3202
3203 size_t riscv_debug_buffer_size(struct target *target)
3204 {
3205         RISCV_INFO(r);
3206         return r->debug_buffer_size[riscv_current_hartid(target)];
3207 }
3208
3209 int riscv_write_debug_buffer(struct target *target, int index, riscv_insn_t insn)
3210 {
3211         RISCV_INFO(r);
3212         r->write_debug_buffer(target, index, insn);
3213         return ERROR_OK;
3214 }
3215
3216 riscv_insn_t riscv_read_debug_buffer(struct target *target, int index)
3217 {
3218         RISCV_INFO(r);
3219         return r->read_debug_buffer(target, index);
3220 }
3221
3222 int riscv_execute_debug_buffer(struct target *target)
3223 {
3224         RISCV_INFO(r);
3225         return r->execute_debug_buffer(target);
3226 }
3227
3228 void riscv_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d)
3229 {
3230         RISCV_INFO(r);
3231         r->fill_dmi_write_u64(target, buf, a, d);
3232 }
3233
3234 void riscv_fill_dmi_read_u64(struct target *target, char *buf, int a)
3235 {
3236         RISCV_INFO(r);
3237         r->fill_dmi_read_u64(target, buf, a);
3238 }
3239
3240 void riscv_fill_dmi_nop_u64(struct target *target, char *buf)
3241 {
3242         RISCV_INFO(r);
3243         r->fill_dmi_nop_u64(target, buf);
3244 }
3245
3246 int riscv_dmi_write_u64_bits(struct target *target)
3247 {
3248         RISCV_INFO(r);
3249         return r->dmi_write_u64_bits(target);
3250 }
3251
3252 bool riscv_hart_enabled(struct target *target, int hartid)
3253 {
3254         /* FIXME: Add a hart mask to the RTOS. */
3255         if (riscv_rtos_enabled(target))
3256                 return hartid < riscv_count_harts(target);
3257
3258         return hartid == target->coreid;
3259 }
3260
3261 /**
3262  * Count triggers, and initialize trigger_count for each hart.
3263  * trigger_count is initialized even if this function fails to discover
3264  * something.
3265  * Disable any hardware triggers that have dmode set. We can't have set them
3266  * ourselves. Maybe they're left over from some killed debug session.
3267  * */
3268 int riscv_enumerate_triggers(struct target *target)
3269 {
3270         RISCV_INFO(r);
3271
3272         if (r->triggers_enumerated)
3273                 return ERROR_OK;
3274
3275         r->triggers_enumerated = true;  /* At the very least we tried. */
3276
3277         for (int hartid = 0; hartid < riscv_count_harts(target); ++hartid) {
3278                 if (!riscv_hart_enabled(target, hartid))
3279                         continue;
3280
3281                 riscv_reg_t tselect;
3282                 int result = riscv_get_register_on_hart(target, &tselect, hartid,
3283                                 GDB_REGNO_TSELECT);
3284                 if (result != ERROR_OK)
3285                         return result;
3286
3287                 for (unsigned t = 0; t < RISCV_MAX_TRIGGERS; ++t) {
3288                         r->trigger_count[hartid] = t;
3289
3290                         /* If we can't write tselect, then this hart does not support triggers. */
3291                         if (riscv_set_register_on_hart(target, hartid, GDB_REGNO_TSELECT, t) != ERROR_OK)
3292                                 break;
3293                         uint64_t tselect_rb;
3294                         result = riscv_get_register_on_hart(target, &tselect_rb, hartid,
3295                                         GDB_REGNO_TSELECT);
3296                         if (result != ERROR_OK)
3297                                 return result;
3298                         /* Mask off the top bit, which is used as tdrmode in old
3299                          * implementations. */
3300                         tselect_rb &= ~(1ULL << (riscv_xlen(target)-1));
3301                         if (tselect_rb != t)
3302                                 break;
3303                         uint64_t tdata1;
3304                         result = riscv_get_register_on_hart(target, &tdata1, hartid,
3305                                         GDB_REGNO_TDATA1);
3306                         if (result != ERROR_OK)
3307                                 return result;
3308
3309                         int type = get_field(tdata1, MCONTROL_TYPE(riscv_xlen(target)));
3310                         if (type == 0)
3311                                 break;
3312                         switch (type) {
3313                                 case 1:
3314                                         /* On these older cores we don't support software using
3315                                          * triggers. */
3316                                         riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA1, 0);
3317                                         break;
3318                                 case 2:
3319                                         if (tdata1 & MCONTROL_DMODE(riscv_xlen(target)))
3320                                                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA1, 0);
3321                                         break;
3322                         }
3323                 }
3324
3325                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TSELECT, tselect);
3326
3327                 LOG_INFO("[%d] Found %d triggers", hartid, r->trigger_count[hartid]);
3328         }
3329
3330         return ERROR_OK;
3331 }
3332
3333 const char *gdb_regno_name(enum gdb_regno regno)
3334 {
3335         static char buf[32];
3336
3337         switch (regno) {
3338                 case GDB_REGNO_ZERO:
3339                         return "zero";
3340                 case GDB_REGNO_RA:
3341                         return "ra";
3342                 case GDB_REGNO_SP:
3343                         return "sp";
3344                 case GDB_REGNO_GP:
3345                         return "gp";
3346                 case GDB_REGNO_TP:
3347                         return "tp";
3348                 case GDB_REGNO_T0:
3349                         return "t0";
3350                 case GDB_REGNO_T1:
3351                         return "t1";
3352                 case GDB_REGNO_T2:
3353                         return "t2";
3354                 case GDB_REGNO_S0:
3355                         return "s0";
3356                 case GDB_REGNO_S1:
3357                         return "s1";
3358                 case GDB_REGNO_A0:
3359                         return "a0";
3360                 case GDB_REGNO_A1:
3361                         return "a1";
3362                 case GDB_REGNO_A2:
3363                         return "a2";
3364                 case GDB_REGNO_A3:
3365                         return "a3";
3366                 case GDB_REGNO_A4:
3367                         return "a4";
3368                 case GDB_REGNO_A5:
3369                         return "a5";
3370                 case GDB_REGNO_A6:
3371                         return "a6";
3372                 case GDB_REGNO_A7:
3373                         return "a7";
3374                 case GDB_REGNO_S2:
3375                         return "s2";
3376                 case GDB_REGNO_S3:
3377                         return "s3";
3378                 case GDB_REGNO_S4:
3379                         return "s4";
3380                 case GDB_REGNO_S5:
3381                         return "s5";
3382                 case GDB_REGNO_S6:
3383                         return "s6";
3384                 case GDB_REGNO_S7:
3385                         return "s7";
3386                 case GDB_REGNO_S8:
3387                         return "s8";
3388                 case GDB_REGNO_S9:
3389                         return "s9";
3390                 case GDB_REGNO_S10:
3391                         return "s10";
3392                 case GDB_REGNO_S11:
3393                         return "s11";
3394                 case GDB_REGNO_T3:
3395                         return "t3";
3396                 case GDB_REGNO_T4:
3397                         return "t4";
3398                 case GDB_REGNO_T5:
3399                         return "t5";
3400                 case GDB_REGNO_T6:
3401                         return "t6";
3402                 case GDB_REGNO_PC:
3403                         return "pc";
3404                 case GDB_REGNO_FPR0:
3405                         return "fpr0";
3406                 case GDB_REGNO_FPR31:
3407                         return "fpr31";
3408                 case GDB_REGNO_CSR0:
3409                         return "csr0";
3410                 case GDB_REGNO_TSELECT:
3411                         return "tselect";
3412                 case GDB_REGNO_TDATA1:
3413                         return "tdata1";
3414                 case GDB_REGNO_TDATA2:
3415                         return "tdata2";
3416                 case GDB_REGNO_MISA:
3417                         return "misa";
3418                 case GDB_REGNO_DPC:
3419                         return "dpc";
3420                 case GDB_REGNO_DCSR:
3421                         return "dcsr";
3422                 case GDB_REGNO_DSCRATCH0:
3423                         return "dscratch0";
3424                 case GDB_REGNO_MSTATUS:
3425                         return "mstatus";
3426                 case GDB_REGNO_MEPC:
3427                         return "mepc";
3428                 case GDB_REGNO_MCAUSE:
3429                         return "mcause";
3430                 case GDB_REGNO_PRIV:
3431                         return "priv";
3432                 case GDB_REGNO_SATP:
3433                         return "satp";
3434                 case GDB_REGNO_VTYPE:
3435                         return "vtype";
3436                 case GDB_REGNO_VL:
3437                         return "vl";
3438                 case GDB_REGNO_V0:
3439                         return "v0";
3440                 case GDB_REGNO_V1:
3441                         return "v1";
3442                 case GDB_REGNO_V2:
3443                         return "v2";
3444                 case GDB_REGNO_V3:
3445                         return "v3";
3446                 case GDB_REGNO_V4:
3447                         return "v4";
3448                 case GDB_REGNO_V5:
3449                         return "v5";
3450                 case GDB_REGNO_V6:
3451                         return "v6";
3452                 case GDB_REGNO_V7:
3453                         return "v7";
3454                 case GDB_REGNO_V8:
3455                         return "v8";
3456                 case GDB_REGNO_V9:
3457                         return "v9";
3458                 case GDB_REGNO_V10:
3459                         return "v10";
3460                 case GDB_REGNO_V11:
3461                         return "v11";
3462                 case GDB_REGNO_V12:
3463                         return "v12";
3464                 case GDB_REGNO_V13:
3465                         return "v13";
3466                 case GDB_REGNO_V14:
3467                         return "v14";
3468                 case GDB_REGNO_V15:
3469                         return "v15";
3470                 case GDB_REGNO_V16:
3471                         return "v16";
3472                 case GDB_REGNO_V17:
3473                         return "v17";
3474                 case GDB_REGNO_V18:
3475                         return "v18";
3476                 case GDB_REGNO_V19:
3477                         return "v19";
3478                 case GDB_REGNO_V20:
3479                         return "v20";
3480                 case GDB_REGNO_V21:
3481                         return "v21";
3482                 case GDB_REGNO_V22:
3483                         return "v22";
3484                 case GDB_REGNO_V23:
3485                         return "v23";
3486                 case GDB_REGNO_V24:
3487                         return "v24";
3488                 case GDB_REGNO_V25:
3489                         return "v25";
3490                 case GDB_REGNO_V26:
3491                         return "v26";
3492                 case GDB_REGNO_V27:
3493                         return "v27";
3494                 case GDB_REGNO_V28:
3495                         return "v28";
3496                 case GDB_REGNO_V29:
3497                         return "v29";
3498                 case GDB_REGNO_V30:
3499                         return "v30";
3500                 case GDB_REGNO_V31:
3501                         return "v31";
3502                 default:
3503                         if (regno <= GDB_REGNO_XPR31)
3504                                 sprintf(buf, "x%d", regno - GDB_REGNO_ZERO);
3505                         else if (regno >= GDB_REGNO_CSR0 && regno <= GDB_REGNO_CSR4095)
3506                                 sprintf(buf, "csr%d", regno - GDB_REGNO_CSR0);
3507                         else if (regno >= GDB_REGNO_FPR0 && regno <= GDB_REGNO_FPR31)
3508                                 sprintf(buf, "f%d", regno - GDB_REGNO_FPR0);
3509                         else
3510                                 sprintf(buf, "gdb_regno_%d", regno);
3511                         return buf;
3512         }
3513 }
3514
3515 static int register_get(struct reg *reg)
3516 {
3517         riscv_reg_info_t *reg_info = reg->arch_info;
3518         struct target *target = reg_info->target;
3519         RISCV_INFO(r);
3520
3521         if (reg->number >= GDB_REGNO_V0 && reg->number <= GDB_REGNO_V31) {
3522                 if (!r->get_register_buf) {
3523                         LOG_ERROR("Reading register %s not supported on this RISC-V target.",
3524                                         gdb_regno_name(reg->number));
3525                         return ERROR_FAIL;
3526                 }
3527
3528                 if (r->get_register_buf(target, reg->value, reg->number) != ERROR_OK)
3529                         return ERROR_FAIL;
3530         } else {
3531                 uint64_t value;
3532                 int result = riscv_get_register(target, &value, reg->number);
3533                 if (result != ERROR_OK)
3534                         return result;
3535                 buf_set_u64(reg->value, 0, reg->size, value);
3536         }
3537         reg->valid = gdb_regno_cacheable(reg->number, false);
3538         char *str = buf_to_hex_str(reg->value, reg->size);
3539         LOG_DEBUG("[%d]{%d} read 0x%s from %s (valid=%d)", target->coreid,
3540                         riscv_current_hartid(target), str, reg->name, reg->valid);
3541         free(str);
3542         return ERROR_OK;
3543 }
3544
3545 static int register_set(struct reg *reg, uint8_t *buf)
3546 {
3547         riscv_reg_info_t *reg_info = reg->arch_info;
3548         struct target *target = reg_info->target;
3549         RISCV_INFO(r);
3550
3551         char *str = buf_to_hex_str(buf, reg->size);
3552         LOG_DEBUG("[%d]{%d} write 0x%s to %s (valid=%d)", target->coreid,
3553                         riscv_current_hartid(target), str, reg->name, reg->valid);
3554         free(str);
3555
3556         memcpy(reg->value, buf, DIV_ROUND_UP(reg->size, 8));
3557         reg->valid = gdb_regno_cacheable(reg->number, true);
3558
3559         if (reg->number == GDB_REGNO_TDATA1 ||
3560                         reg->number == GDB_REGNO_TDATA2) {
3561                 r->manual_hwbp_set = true;
3562                 /* When enumerating triggers, we clear any triggers with DMODE set,
3563                  * assuming they were left over from a previous debug session. So make
3564                  * sure that is done before a user might be setting their own triggers.
3565                  */
3566                 if (riscv_enumerate_triggers(target) != ERROR_OK)
3567                         return ERROR_FAIL;
3568         }
3569
3570         if (reg->number >= GDB_REGNO_V0 && reg->number <= GDB_REGNO_V31) {
3571                 if (!r->set_register_buf) {
3572                         LOG_ERROR("Writing register %s not supported on this RISC-V target.",
3573                                         gdb_regno_name(reg->number));
3574                         return ERROR_FAIL;
3575                 }
3576
3577                 if (r->set_register_buf(target, reg->number, reg->value) != ERROR_OK)
3578                         return ERROR_FAIL;
3579         } else {
3580                 uint64_t value = buf_get_u64(buf, 0, reg->size);
3581                 if (riscv_set_register(target, reg->number, value) != ERROR_OK)
3582                         return ERROR_FAIL;
3583         }
3584
3585         return ERROR_OK;
3586 }
3587
3588 static struct reg_arch_type riscv_reg_arch_type = {
3589         .get = register_get,
3590         .set = register_set
3591 };
3592
3593 struct csr_info {
3594         unsigned number;
3595         const char *name;
3596 };
3597
3598 static int cmp_csr_info(const void *p1, const void *p2)
3599 {
3600         return (int) (((struct csr_info *)p1)->number) - (int) (((struct csr_info *)p2)->number);
3601 }
3602
3603 int riscv_init_registers(struct target *target)
3604 {
3605         RISCV_INFO(info);
3606
3607         riscv_free_registers(target);
3608
3609         target->reg_cache = calloc(1, sizeof(*target->reg_cache));
3610         if (!target->reg_cache)
3611                 return ERROR_FAIL;
3612         target->reg_cache->name = "RISC-V Registers";
3613         target->reg_cache->num_regs = GDB_REGNO_COUNT;
3614
3615         if (expose_custom) {
3616                 for (unsigned i = 0; expose_custom[i].low <= expose_custom[i].high; i++) {
3617                         for (unsigned number = expose_custom[i].low;
3618                                         number <= expose_custom[i].high;
3619                                         number++)
3620                                 target->reg_cache->num_regs++;
3621                 }
3622         }
3623
3624         LOG_DEBUG("create register cache for %d registers",
3625                         target->reg_cache->num_regs);
3626
3627         target->reg_cache->reg_list =
3628                 calloc(target->reg_cache->num_regs, sizeof(struct reg));
3629         if (!target->reg_cache->reg_list)
3630                 return ERROR_FAIL;
3631
3632         const unsigned int max_reg_name_len = 12;
3633         free(info->reg_names);
3634         info->reg_names =
3635                 calloc(target->reg_cache->num_regs, max_reg_name_len);
3636         if (!info->reg_names)
3637                 return ERROR_FAIL;
3638         char *reg_name = info->reg_names;
3639
3640         int hartid = riscv_current_hartid(target);
3641
3642         static struct reg_feature feature_cpu = {
3643                 .name = "org.gnu.gdb.riscv.cpu"
3644         };
3645         static struct reg_feature feature_fpu = {
3646                 .name = "org.gnu.gdb.riscv.fpu"
3647         };
3648         static struct reg_feature feature_csr = {
3649                 .name = "org.gnu.gdb.riscv.csr"
3650         };
3651         static struct reg_feature feature_vector = {
3652                 .name = "org.gnu.gdb.riscv.vector"
3653         };
3654         static struct reg_feature feature_virtual = {
3655                 .name = "org.gnu.gdb.riscv.virtual"
3656         };
3657         static struct reg_feature feature_custom = {
3658                 .name = "org.gnu.gdb.riscv.custom"
3659         };
3660
3661         /* These types are built into gdb. */
3662         static struct reg_data_type type_ieee_single = { .type = REG_TYPE_IEEE_SINGLE, .id = "ieee_single" };
3663         static struct reg_data_type type_ieee_double = { .type = REG_TYPE_IEEE_DOUBLE, .id = "ieee_double" };
3664         static struct reg_data_type_union_field single_double_fields[] = {
3665                 {"float", &type_ieee_single, single_double_fields + 1},
3666                 {"double", &type_ieee_double, NULL},
3667         };
3668         static struct reg_data_type_union single_double_union = {
3669                 .fields = single_double_fields
3670         };
3671         static struct reg_data_type type_ieee_single_double = {
3672                 .type = REG_TYPE_ARCH_DEFINED,
3673                 .id = "FPU_FD",
3674                 .type_class = REG_TYPE_CLASS_UNION,
3675                 .reg_type_union = &single_double_union
3676         };
3677         static struct reg_data_type type_uint8 = { .type = REG_TYPE_UINT8, .id = "uint8" };
3678         static struct reg_data_type type_uint16 = { .type = REG_TYPE_UINT16, .id = "uint16" };
3679         static struct reg_data_type type_uint32 = { .type = REG_TYPE_UINT32, .id = "uint32" };
3680         static struct reg_data_type type_uint64 = { .type = REG_TYPE_UINT64, .id = "uint64" };
3681         static struct reg_data_type type_uint128 = { .type = REG_TYPE_UINT128, .id = "uint128" };
3682
3683         /* This is roughly the XML we want:
3684          * <vector id="bytes" type="uint8" count="16"/>
3685          * <vector id="shorts" type="uint16" count="8"/>
3686          * <vector id="words" type="uint32" count="4"/>
3687          * <vector id="longs" type="uint64" count="2"/>
3688          * <vector id="quads" type="uint128" count="1"/>
3689          * <union id="riscv_vector_type">
3690          *   <field name="b" type="bytes"/>
3691          *   <field name="s" type="shorts"/>
3692          *   <field name="w" type="words"/>
3693          *   <field name="l" type="longs"/>
3694          *   <field name="q" type="quads"/>
3695          * </union>
3696          */
3697
3698         info->vector_uint8.type = &type_uint8;
3699         info->vector_uint8.count = info->vlenb[hartid];
3700         info->type_uint8_vector.type = REG_TYPE_ARCH_DEFINED;
3701         info->type_uint8_vector.id = "bytes";
3702         info->type_uint8_vector.type_class = REG_TYPE_CLASS_VECTOR;
3703         info->type_uint8_vector.reg_type_vector = &info->vector_uint8;
3704
3705         info->vector_uint16.type = &type_uint16;
3706         info->vector_uint16.count = info->vlenb[hartid] / 2;
3707         info->type_uint16_vector.type = REG_TYPE_ARCH_DEFINED;
3708         info->type_uint16_vector.id = "shorts";
3709         info->type_uint16_vector.type_class = REG_TYPE_CLASS_VECTOR;
3710         info->type_uint16_vector.reg_type_vector = &info->vector_uint16;
3711
3712         info->vector_uint32.type = &type_uint32;
3713         info->vector_uint32.count = info->vlenb[hartid] / 4;
3714         info->type_uint32_vector.type = REG_TYPE_ARCH_DEFINED;
3715         info->type_uint32_vector.id = "words";
3716         info->type_uint32_vector.type_class = REG_TYPE_CLASS_VECTOR;
3717         info->type_uint32_vector.reg_type_vector = &info->vector_uint32;
3718
3719         info->vector_uint64.type = &type_uint64;
3720         info->vector_uint64.count = info->vlenb[hartid] / 8;
3721         info->type_uint64_vector.type = REG_TYPE_ARCH_DEFINED;
3722         info->type_uint64_vector.id = "longs";
3723         info->type_uint64_vector.type_class = REG_TYPE_CLASS_VECTOR;
3724         info->type_uint64_vector.reg_type_vector = &info->vector_uint64;
3725
3726         info->vector_uint128.type = &type_uint128;
3727         info->vector_uint128.count = info->vlenb[hartid] / 16;
3728         info->type_uint128_vector.type = REG_TYPE_ARCH_DEFINED;
3729         info->type_uint128_vector.id = "quads";
3730         info->type_uint128_vector.type_class = REG_TYPE_CLASS_VECTOR;
3731         info->type_uint128_vector.reg_type_vector = &info->vector_uint128;
3732
3733         info->vector_fields[0].name = "b";
3734         info->vector_fields[0].type = &info->type_uint8_vector;
3735         if (info->vlenb[hartid] >= 2) {
3736                 info->vector_fields[0].next = info->vector_fields + 1;
3737                 info->vector_fields[1].name = "s";
3738                 info->vector_fields[1].type = &info->type_uint16_vector;
3739         } else {
3740                 info->vector_fields[0].next = NULL;
3741         }
3742         if (info->vlenb[hartid] >= 4) {
3743                 info->vector_fields[1].next = info->vector_fields + 2;
3744                 info->vector_fields[2].name = "w";
3745                 info->vector_fields[2].type = &info->type_uint32_vector;
3746         } else {
3747                 info->vector_fields[1].next = NULL;
3748         }
3749         if (info->vlenb[hartid] >= 8) {
3750                 info->vector_fields[2].next = info->vector_fields + 3;
3751                 info->vector_fields[3].name = "l";
3752                 info->vector_fields[3].type = &info->type_uint64_vector;
3753         } else {
3754                 info->vector_fields[2].next = NULL;
3755         }
3756         if (info->vlenb[hartid] >= 16) {
3757                 info->vector_fields[3].next = info->vector_fields + 4;
3758                 info->vector_fields[4].name = "q";
3759                 info->vector_fields[4].type = &info->type_uint128_vector;
3760         } else {
3761                 info->vector_fields[3].next = NULL;
3762         }
3763         info->vector_fields[4].next = NULL;
3764
3765         info->vector_union.fields = info->vector_fields;
3766
3767         info->type_vector.type = REG_TYPE_ARCH_DEFINED;
3768         info->type_vector.id = "riscv_vector";
3769         info->type_vector.type_class = REG_TYPE_CLASS_UNION;
3770         info->type_vector.reg_type_union = &info->vector_union;
3771
3772         struct csr_info csr_info[] = {
3773 #define DECLARE_CSR(name, number) { number, #name },
3774 #include "encoding.h"
3775 #undef DECLARE_CSR
3776         };
3777         /* encoding.h does not contain the registers in sorted order. */
3778         qsort(csr_info, ARRAY_SIZE(csr_info), sizeof(*csr_info), cmp_csr_info);
3779         unsigned csr_info_index = 0;
3780
3781         unsigned custom_range_index = 0;
3782         int custom_within_range = 0;
3783
3784         riscv_reg_info_t *shared_reg_info = calloc(1, sizeof(riscv_reg_info_t));
3785         if (!shared_reg_info)
3786                 return ERROR_FAIL;
3787         shared_reg_info->target = target;
3788
3789         /* When gdb requests register N, gdb_get_register_packet() assumes that this
3790          * is register at index N in reg_list. So if there are certain registers
3791          * that don't exist, we need to leave holes in the list (or renumber, but
3792          * it would be nice not to have yet another set of numbers to translate
3793          * between). */
3794         for (uint32_t number = 0; number < target->reg_cache->num_regs; number++) {
3795                 struct reg *r = &target->reg_cache->reg_list[number];
3796                 r->dirty = false;
3797                 r->valid = false;
3798                 r->exist = true;
3799                 r->type = &riscv_reg_arch_type;
3800                 r->arch_info = shared_reg_info;
3801                 r->number = number;
3802                 r->size = riscv_xlen(target);
3803                 /* r->size is set in riscv_invalidate_register_cache, maybe because the
3804                  * target is in theory allowed to change XLEN on us. But I expect a lot
3805                  * of other things to break in that case as well. */
3806                 if (number <= GDB_REGNO_XPR31) {
3807                         r->exist = number <= GDB_REGNO_XPR15 ||
3808                                 !riscv_supports_extension(target, hartid, 'E');
3809                         /* TODO: For now we fake that all GPRs exist because otherwise gdb
3810                          * doesn't work. */
3811                         r->exist = true;
3812                         r->caller_save = true;
3813                         switch (number) {
3814                                 case GDB_REGNO_ZERO:
3815                                         r->name = "zero";
3816                                         break;
3817                                 case GDB_REGNO_RA:
3818                                         r->name = "ra";
3819                                         break;
3820                                 case GDB_REGNO_SP:
3821                                         r->name = "sp";
3822                                         break;
3823                                 case GDB_REGNO_GP:
3824                                         r->name = "gp";
3825                                         break;
3826                                 case GDB_REGNO_TP:
3827                                         r->name = "tp";
3828                                         break;
3829                                 case GDB_REGNO_T0:
3830                                         r->name = "t0";
3831                                         break;
3832                                 case GDB_REGNO_T1:
3833                                         r->name = "t1";
3834                                         break;
3835                                 case GDB_REGNO_T2:
3836                                         r->name = "t2";
3837                                         break;
3838                                 case GDB_REGNO_FP:
3839                                         r->name = "fp";
3840                                         break;
3841                                 case GDB_REGNO_S1:
3842                                         r->name = "s1";
3843                                         break;
3844                                 case GDB_REGNO_A0:
3845                                         r->name = "a0";
3846                                         break;
3847                                 case GDB_REGNO_A1:
3848                                         r->name = "a1";
3849                                         break;
3850                                 case GDB_REGNO_A2:
3851                                         r->name = "a2";
3852                                         break;
3853                                 case GDB_REGNO_A3:
3854                                         r->name = "a3";
3855                                         break;
3856                                 case GDB_REGNO_A4:
3857                                         r->name = "a4";
3858                                         break;
3859                                 case GDB_REGNO_A5:
3860                                         r->name = "a5";
3861                                         break;
3862                                 case GDB_REGNO_A6:
3863                                         r->name = "a6";
3864                                         break;
3865                                 case GDB_REGNO_A7:
3866                                         r->name = "a7";
3867                                         break;
3868                                 case GDB_REGNO_S2:
3869                                         r->name = "s2";
3870                                         break;
3871                                 case GDB_REGNO_S3:
3872                                         r->name = "s3";
3873                                         break;
3874                                 case GDB_REGNO_S4:
3875                                         r->name = "s4";
3876                                         break;
3877                                 case GDB_REGNO_S5:
3878                                         r->name = "s5";
3879                                         break;
3880                                 case GDB_REGNO_S6:
3881                                         r->name = "s6";
3882                                         break;
3883                                 case GDB_REGNO_S7:
3884                                         r->name = "s7";
3885                                         break;
3886                                 case GDB_REGNO_S8:
3887                                         r->name = "s8";
3888                                         break;
3889                                 case GDB_REGNO_S9:
3890                                         r->name = "s9";
3891                                         break;
3892                                 case GDB_REGNO_S10:
3893                                         r->name = "s10";
3894                                         break;
3895                                 case GDB_REGNO_S11:
3896                                         r->name = "s11";
3897                                         break;
3898                                 case GDB_REGNO_T3:
3899                                         r->name = "t3";
3900                                         break;
3901                                 case GDB_REGNO_T4:
3902                                         r->name = "t4";
3903                                         break;
3904                                 case GDB_REGNO_T5:
3905                                         r->name = "t5";
3906                                         break;
3907                                 case GDB_REGNO_T6:
3908                                         r->name = "t6";
3909                                         break;
3910                         }
3911                         r->group = "general";
3912                         r->feature = &feature_cpu;
3913                 } else if (number == GDB_REGNO_PC) {
3914                         r->caller_save = true;
3915                         sprintf(reg_name, "pc");
3916                         r->group = "general";
3917                         r->feature = &feature_cpu;
3918                 } else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
3919                         r->caller_save = true;
3920                         if (riscv_supports_extension(target, hartid, 'D')) {
3921                                 r->size = 64;
3922                                 if (riscv_supports_extension(target, hartid, 'F'))
3923                                         r->reg_data_type = &type_ieee_single_double;
3924                                 else
3925                                         r->reg_data_type = &type_ieee_double;
3926                         } else if (riscv_supports_extension(target, hartid, 'F')) {
3927                                 r->reg_data_type = &type_ieee_single;
3928                                 r->size = 32;
3929                         } else {
3930                                 r->exist = false;
3931                         }
3932                         switch (number) {
3933                                 case GDB_REGNO_FT0:
3934                                         r->name = "ft0";
3935                                         break;
3936                                 case GDB_REGNO_FT1:
3937                                         r->name = "ft1";
3938                                         break;
3939                                 case GDB_REGNO_FT2:
3940                                         r->name = "ft2";
3941                                         break;
3942                                 case GDB_REGNO_FT3:
3943                                         r->name = "ft3";
3944                                         break;
3945                                 case GDB_REGNO_FT4:
3946                                         r->name = "ft4";
3947                                         break;
3948                                 case GDB_REGNO_FT5:
3949                                         r->name = "ft5";
3950                                         break;
3951                                 case GDB_REGNO_FT6:
3952                                         r->name = "ft6";
3953                                         break;
3954                                 case GDB_REGNO_FT7:
3955                                         r->name = "ft7";
3956                                         break;
3957                                 case GDB_REGNO_FS0:
3958                                         r->name = "fs0";
3959                                         break;
3960                                 case GDB_REGNO_FS1:
3961                                         r->name = "fs1";
3962                                         break;
3963                                 case GDB_REGNO_FA0:
3964                                         r->name = "fa0";
3965                                         break;
3966                                 case GDB_REGNO_FA1:
3967                                         r->name = "fa1";
3968                                         break;
3969                                 case GDB_REGNO_FA2:
3970                                         r->name = "fa2";
3971                                         break;
3972                                 case GDB_REGNO_FA3:
3973                                         r->name = "fa3";
3974                                         break;
3975                                 case GDB_REGNO_FA4:
3976                                         r->name = "fa4";
3977                                         break;
3978                                 case GDB_REGNO_FA5:
3979                                         r->name = "fa5";
3980                                         break;
3981                                 case GDB_REGNO_FA6:
3982                                         r->name = "fa6";
3983                                         break;
3984                                 case GDB_REGNO_FA7:
3985                                         r->name = "fa7";
3986                                         break;
3987                                 case GDB_REGNO_FS2:
3988                                         r->name = "fs2";
3989                                         break;
3990                                 case GDB_REGNO_FS3:
3991                                         r->name = "fs3";
3992                                         break;
3993                                 case GDB_REGNO_FS4:
3994                                         r->name = "fs4";
3995                                         break;
3996                                 case GDB_REGNO_FS5:
3997                                         r->name = "fs5";
3998                                         break;
3999                                 case GDB_REGNO_FS6:
4000                                         r->name = "fs6";
4001                                         break;
4002                                 case GDB_REGNO_FS7:
4003                                         r->name = "fs7";
4004                                         break;
4005                                 case GDB_REGNO_FS8:
4006                                         r->name = "fs8";
4007                                         break;
4008                                 case GDB_REGNO_FS9:
4009                                         r->name = "fs9";
4010                                         break;
4011                                 case GDB_REGNO_FS10:
4012                                         r->name = "fs10";
4013                                         break;
4014                                 case GDB_REGNO_FS11:
4015                                         r->name = "fs11";
4016                                         break;
4017                                 case GDB_REGNO_FT8:
4018                                         r->name = "ft8";
4019                                         break;
4020                                 case GDB_REGNO_FT9:
4021                                         r->name = "ft9";
4022                                         break;
4023                                 case GDB_REGNO_FT10:
4024                                         r->name = "ft10";
4025                                         break;
4026                                 case GDB_REGNO_FT11:
4027                                         r->name = "ft11";
4028                                         break;
4029                         }
4030                         r->group = "float";
4031                         r->feature = &feature_fpu;
4032                 } else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
4033                         r->group = "csr";
4034                         r->feature = &feature_csr;
4035                         unsigned csr_number = number - GDB_REGNO_CSR0;
4036
4037                         while (csr_info[csr_info_index].number < csr_number &&
4038                                         csr_info_index < ARRAY_SIZE(csr_info) - 1) {
4039                                 csr_info_index++;
4040                         }
4041                         if (csr_info[csr_info_index].number == csr_number) {
4042                                 r->name = csr_info[csr_info_index].name;
4043                         } else {
4044                                 sprintf(reg_name, "csr%d", csr_number);
4045                                 /* Assume unnamed registers don't exist, unless we have some
4046                                  * configuration that tells us otherwise. That's important
4047                                  * because eg. Eclipse crashes if a target has too many
4048                                  * registers, and apparently has no way of only showing a
4049                                  * subset of registers in any case. */
4050                                 r->exist = false;
4051                         }
4052
4053                         switch (csr_number) {
4054                                 case CSR_FFLAGS:
4055                                 case CSR_FRM:
4056                                 case CSR_FCSR:
4057                                         r->exist = riscv_supports_extension(target, hartid, 'F');
4058                                         r->group = "float";
4059                                         r->feature = &feature_fpu;
4060                                         break;
4061                                 case CSR_SSTATUS:
4062                                 case CSR_STVEC:
4063                                 case CSR_SIP:
4064                                 case CSR_SIE:
4065                                 case CSR_SCOUNTEREN:
4066                                 case CSR_SSCRATCH:
4067                                 case CSR_SEPC:
4068                                 case CSR_SCAUSE:
4069                                 case CSR_STVAL:
4070                                 case CSR_SATP:
4071                                         r->exist = riscv_supports_extension(target, hartid, 'S');
4072                                         break;
4073                                 case CSR_MEDELEG:
4074                                 case CSR_MIDELEG:
4075                                         /* "In systems with only M-mode, or with both M-mode and
4076                                          * U-mode but without U-mode trap support, the medeleg and
4077                                          * mideleg registers should not exist." */
4078                                         r->exist = riscv_supports_extension(target, hartid, 'S') ||
4079                                                 riscv_supports_extension(target, hartid, 'N');
4080                                         break;
4081
4082                                 case CSR_PMPCFG1:
4083                                 case CSR_PMPCFG3:
4084                                 case CSR_CYCLEH:
4085                                 case CSR_TIMEH:
4086                                 case CSR_INSTRETH:
4087                                 case CSR_HPMCOUNTER3H:
4088                                 case CSR_HPMCOUNTER4H:
4089                                 case CSR_HPMCOUNTER5H:
4090                                 case CSR_HPMCOUNTER6H:
4091                                 case CSR_HPMCOUNTER7H:
4092                                 case CSR_HPMCOUNTER8H:
4093                                 case CSR_HPMCOUNTER9H:
4094                                 case CSR_HPMCOUNTER10H:
4095                                 case CSR_HPMCOUNTER11H:
4096                                 case CSR_HPMCOUNTER12H:
4097                                 case CSR_HPMCOUNTER13H:
4098                                 case CSR_HPMCOUNTER14H:
4099                                 case CSR_HPMCOUNTER15H:
4100                                 case CSR_HPMCOUNTER16H:
4101                                 case CSR_HPMCOUNTER17H:
4102                                 case CSR_HPMCOUNTER18H:
4103                                 case CSR_HPMCOUNTER19H:
4104                                 case CSR_HPMCOUNTER20H:
4105                                 case CSR_HPMCOUNTER21H:
4106                                 case CSR_HPMCOUNTER22H:
4107                                 case CSR_HPMCOUNTER23H:
4108                                 case CSR_HPMCOUNTER24H:
4109                                 case CSR_HPMCOUNTER25H:
4110                                 case CSR_HPMCOUNTER26H:
4111                                 case CSR_HPMCOUNTER27H:
4112                                 case CSR_HPMCOUNTER28H:
4113                                 case CSR_HPMCOUNTER29H:
4114                                 case CSR_HPMCOUNTER30H:
4115                                 case CSR_HPMCOUNTER31H:
4116                                 case CSR_MCYCLEH:
4117                                 case CSR_MINSTRETH:
4118                                 case CSR_MHPMCOUNTER3H:
4119                                 case CSR_MHPMCOUNTER4H:
4120                                 case CSR_MHPMCOUNTER5H:
4121                                 case CSR_MHPMCOUNTER6H:
4122                                 case CSR_MHPMCOUNTER7H:
4123                                 case CSR_MHPMCOUNTER8H:
4124                                 case CSR_MHPMCOUNTER9H:
4125                                 case CSR_MHPMCOUNTER10H:
4126                                 case CSR_MHPMCOUNTER11H:
4127                                 case CSR_MHPMCOUNTER12H:
4128                                 case CSR_MHPMCOUNTER13H:
4129                                 case CSR_MHPMCOUNTER14H:
4130                                 case CSR_MHPMCOUNTER15H:
4131                                 case CSR_MHPMCOUNTER16H:
4132                                 case CSR_MHPMCOUNTER17H:
4133                                 case CSR_MHPMCOUNTER18H:
4134                                 case CSR_MHPMCOUNTER19H:
4135                                 case CSR_MHPMCOUNTER20H:
4136                                 case CSR_MHPMCOUNTER21H:
4137                                 case CSR_MHPMCOUNTER22H:
4138                                 case CSR_MHPMCOUNTER23H:
4139                                 case CSR_MHPMCOUNTER24H:
4140                                 case CSR_MHPMCOUNTER25H:
4141                                 case CSR_MHPMCOUNTER26H:
4142                                 case CSR_MHPMCOUNTER27H:
4143                                 case CSR_MHPMCOUNTER28H:
4144                                 case CSR_MHPMCOUNTER29H:
4145                                 case CSR_MHPMCOUNTER30H:
4146                                 case CSR_MHPMCOUNTER31H:
4147                                         r->exist = riscv_xlen(target) == 32;
4148                                         break;
4149
4150                                 case CSR_VSTART:
4151                                 case CSR_VXSAT:
4152                                 case CSR_VXRM:
4153                                 case CSR_VL:
4154                                 case CSR_VTYPE:
4155                                 case CSR_VLENB:
4156                                         r->exist = riscv_supports_extension(target, hartid, 'V');
4157                                         break;
4158                         }
4159
4160                         if (!r->exist && expose_csr) {
4161                                 for (unsigned i = 0; expose_csr[i].low <= expose_csr[i].high; i++) {
4162                                         if (csr_number >= expose_csr[i].low && csr_number <= expose_csr[i].high) {
4163                                                 LOG_INFO("Exposing additional CSR %d", csr_number);
4164                                                 r->exist = true;
4165                                                 break;
4166                                         }
4167                                 }
4168                         }
4169
4170                 } else if (number == GDB_REGNO_PRIV) {
4171                         sprintf(reg_name, "priv");
4172                         r->group = "general";
4173                         r->feature = &feature_virtual;
4174                         r->size = 8;
4175
4176                 } else if (number >= GDB_REGNO_V0 && number <= GDB_REGNO_V31) {
4177                         r->caller_save = false;
4178                         r->exist = riscv_supports_extension(target, hartid, 'V') && info->vlenb[hartid];
4179                         r->size = info->vlenb[hartid] * 8;
4180                         sprintf(reg_name, "v%d", number - GDB_REGNO_V0);
4181                         r->group = "vector";
4182                         r->feature = &feature_vector;
4183                         r->reg_data_type = &info->type_vector;
4184
4185                 } else if (number >= GDB_REGNO_COUNT) {
4186                         /* Custom registers. */
4187                         assert(expose_custom);
4188
4189                         range_t *range = &expose_custom[custom_range_index];
4190                         assert(range->low <= range->high);
4191                         unsigned custom_number = range->low + custom_within_range;
4192
4193                         r->group = "custom";
4194                         r->feature = &feature_custom;
4195                         r->arch_info = calloc(1, sizeof(riscv_reg_info_t));
4196                         if (!r->arch_info)
4197                                 return ERROR_FAIL;
4198                         ((riscv_reg_info_t *) r->arch_info)->target = target;
4199                         ((riscv_reg_info_t *) r->arch_info)->custom_number = custom_number;
4200                         sprintf(reg_name, "custom%d", custom_number);
4201
4202                         custom_within_range++;
4203                         if (custom_within_range > range->high - range->low) {
4204                                 custom_within_range = 0;
4205                                 custom_range_index++;
4206                         }
4207                 }
4208
4209                 if (reg_name[0])
4210                         r->name = reg_name;
4211                 reg_name += strlen(reg_name) + 1;
4212                 assert(reg_name < info->reg_names + target->reg_cache->num_regs *
4213                                 max_reg_name_len);
4214                 r->value = info->reg_cache_values[number];
4215         }
4216
4217         return ERROR_OK;
4218 }
4219
4220
4221 void riscv_add_bscan_tunneled_scan(struct target *target, struct scan_field *field,
4222                                         riscv_bscan_tunneled_scan_context_t *ctxt)
4223 {
4224         jtag_add_ir_scan(target->tap, &select_user4, TAP_IDLE);
4225
4226         memset(ctxt->tunneled_dr, 0, sizeof(ctxt->tunneled_dr));
4227         if (bscan_tunnel_type == BSCAN_TUNNEL_DATA_REGISTER) {
4228                 ctxt->tunneled_dr[3].num_bits = 1;
4229                 ctxt->tunneled_dr[3].out_value = bscan_one;
4230                 ctxt->tunneled_dr[2].num_bits = 7;
4231                 ctxt->tunneled_dr_width = field->num_bits;
4232                 ctxt->tunneled_dr[2].out_value = &ctxt->tunneled_dr_width;
4233                 /* for BSCAN tunnel, there is a one-TCK skew between shift in and shift out, so
4234                    scanning num_bits + 1, and then will right shift the input field after executing the queues */
4235
4236                 ctxt->tunneled_dr[1].num_bits = field->num_bits + 1;
4237                 ctxt->tunneled_dr[1].out_value = field->out_value;
4238                 ctxt->tunneled_dr[1].in_value = field->in_value;
4239
4240                 ctxt->tunneled_dr[0].num_bits = 3;
4241                 ctxt->tunneled_dr[0].out_value = bscan_zero;
4242         } else {
4243                 /* BSCAN_TUNNEL_NESTED_TAP */
4244                 ctxt->tunneled_dr[0].num_bits = 1;
4245                 ctxt->tunneled_dr[0].out_value = bscan_one;
4246                 ctxt->tunneled_dr[1].num_bits = 7;
4247                 ctxt->tunneled_dr_width = field->num_bits;
4248                 ctxt->tunneled_dr[1].out_value = &ctxt->tunneled_dr_width;
4249                 /* for BSCAN tunnel, there is a one-TCK skew between shift in and shift out, so
4250                    scanning num_bits + 1, and then will right shift the input field after executing the queues */
4251                 ctxt->tunneled_dr[2].num_bits = field->num_bits + 1;
4252                 ctxt->tunneled_dr[2].out_value = field->out_value;
4253                 ctxt->tunneled_dr[2].in_value = field->in_value;
4254                 ctxt->tunneled_dr[3].num_bits = 3;
4255                 ctxt->tunneled_dr[3].out_value = bscan_zero;
4256         }
4257         jtag_add_dr_scan(target->tap, ARRAY_SIZE(ctxt->tunneled_dr), ctxt->tunneled_dr, TAP_IDLE);
4258 }