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)
 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)
 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)
 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) {
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) {
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) {
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 const char *riscv_get_gdb_arch(struct target *target)
1618 {
1619         switch (riscv_xlen(target)) {
1620                 case 32:
1621                         return "riscv:rv32";
1622                 case 64:
1623                         return "riscv:rv64";
1624         }
1625         LOG_ERROR("Unsupported xlen: %d", riscv_xlen(target));
1626         return NULL;
1627 }
1628
1629 static int riscv_get_gdb_reg_list_internal(struct target *target,
1630                 struct reg **reg_list[], int *reg_list_size,
1631                 enum target_register_class reg_class, bool read)
1632 {
1633         RISCV_INFO(r);
1634         LOG_DEBUG("rtos_hartid=%d, current_hartid=%d, reg_class=%d, read=%d",
1635                         r->rtos_hartid, r->current_hartid, reg_class, read);
1636
1637         if (!target->reg_cache) {
1638                 LOG_ERROR("Target not initialized. Return ERROR_FAIL.");
1639                 return ERROR_FAIL;
1640         }
1641
1642         if (riscv_select_current_hart(target) != ERROR_OK)
1643                 return ERROR_FAIL;
1644
1645         switch (reg_class) {
1646                 case REG_CLASS_GENERAL:
1647                         *reg_list_size = 33;
1648                         break;
1649                 case REG_CLASS_ALL:
1650                         *reg_list_size = target->reg_cache->num_regs;
1651                         break;
1652                 default:
1653                         LOG_ERROR("Unsupported reg_class: %d", reg_class);
1654                         return ERROR_FAIL;
1655         }
1656
1657         *reg_list = calloc(*reg_list_size, sizeof(struct reg *));
1658         if (!*reg_list)
1659                 return ERROR_FAIL;
1660
1661         for (int i = 0; i < *reg_list_size; i++) {
1662                 assert(!target->reg_cache->reg_list[i].valid ||
1663                                 target->reg_cache->reg_list[i].size > 0);
1664                 (*reg_list)[i] = &target->reg_cache->reg_list[i];
1665                 if (read &&
1666                                 target->reg_cache->reg_list[i].exist &&
1667                                 !target->reg_cache->reg_list[i].valid) {
1668                         if (target->reg_cache->reg_list[i].type->get(
1669                                                 &target->reg_cache->reg_list[i]) != ERROR_OK)
1670                                 return ERROR_FAIL;
1671                 }
1672         }
1673
1674         return ERROR_OK;
1675 }
1676
1677 static int riscv_get_gdb_reg_list_noread(struct target *target,
1678                 struct reg **reg_list[], int *reg_list_size,
1679                 enum target_register_class reg_class)
1680 {
1681         return riscv_get_gdb_reg_list_internal(target, reg_list, reg_list_size,
1682                         reg_class, false);
1683 }
1684
1685 static int riscv_get_gdb_reg_list(struct target *target,
1686                 struct reg **reg_list[], int *reg_list_size,
1687                 enum target_register_class reg_class)
1688 {
1689         return riscv_get_gdb_reg_list_internal(target, reg_list, reg_list_size,
1690                         reg_class, true);
1691 }
1692
1693 static int riscv_arch_state(struct target *target)
1694 {
1695         struct target_type *tt = get_target_type(target);
1696         return tt->arch_state(target);
1697 }
1698
1699 /* Algorithm must end with a software breakpoint instruction. */
1700 static int riscv_run_algorithm(struct target *target, int num_mem_params,
1701                 struct mem_param *mem_params, int num_reg_params,
1702                 struct reg_param *reg_params, target_addr_t entry_point,
1703                 target_addr_t exit_point, int timeout_ms, void *arch_info)
1704 {
1705         riscv_info_t *info = (riscv_info_t *) target->arch_info;
1706         int hartid = riscv_current_hartid(target);
1707
1708         if (num_mem_params > 0) {
1709                 LOG_ERROR("Memory parameters are not supported for RISC-V algorithms.");
1710                 return ERROR_FAIL;
1711         }
1712
1713         if (target->state != TARGET_HALTED) {
1714                 LOG_WARNING("target not halted");
1715                 return ERROR_TARGET_NOT_HALTED;
1716         }
1717
1718         /* Save registers */
1719         struct reg *reg_pc = register_get_by_name(target->reg_cache, "pc", true);
1720         if (!reg_pc || reg_pc->type->get(reg_pc) != ERROR_OK)
1721                 return ERROR_FAIL;
1722         uint64_t saved_pc = buf_get_u64(reg_pc->value, 0, reg_pc->size);
1723         LOG_DEBUG("saved_pc=0x%" PRIx64, saved_pc);
1724
1725         uint64_t saved_regs[32];
1726         for (int i = 0; i < num_reg_params; i++) {
1727                 LOG_DEBUG("save %s", reg_params[i].reg_name);
1728                 struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, false);
1729                 if (!r) {
1730                         LOG_ERROR("Couldn't find register named '%s'", reg_params[i].reg_name);
1731                         return ERROR_FAIL;
1732                 }
1733
1734                 if (r->size != reg_params[i].size) {
1735                         LOG_ERROR("Register %s is %d bits instead of %d bits.",
1736                                         reg_params[i].reg_name, r->size, reg_params[i].size);
1737                         return ERROR_FAIL;
1738                 }
1739
1740                 if (r->number > GDB_REGNO_XPR31) {
1741                         LOG_ERROR("Only GPRs can be use as argument registers.");
1742                         return ERROR_FAIL;
1743                 }
1744
1745                 if (r->type->get(r) != ERROR_OK)
1746                         return ERROR_FAIL;
1747                 saved_regs[r->number] = buf_get_u64(r->value, 0, r->size);
1748
1749                 if (reg_params[i].direction == PARAM_OUT || reg_params[i].direction == PARAM_IN_OUT) {
1750                         if (r->type->set(r, reg_params[i].value) != ERROR_OK)
1751                                 return ERROR_FAIL;
1752                 }
1753         }
1754
1755
1756         /* Disable Interrupts before attempting to run the algorithm. */
1757         uint64_t current_mstatus;
1758         uint8_t mstatus_bytes[8] = { 0 };
1759
1760         LOG_DEBUG("Disabling Interrupts");
1761         struct reg *reg_mstatus = register_get_by_name(target->reg_cache,
1762                         "mstatus", true);
1763         if (!reg_mstatus) {
1764                 LOG_ERROR("Couldn't find mstatus!");
1765                 return ERROR_FAIL;
1766         }
1767
1768         reg_mstatus->type->get(reg_mstatus);
1769         current_mstatus = buf_get_u64(reg_mstatus->value, 0, reg_mstatus->size);
1770         uint64_t ie_mask = MSTATUS_MIE | MSTATUS_HIE | MSTATUS_SIE | MSTATUS_UIE;
1771         buf_set_u64(mstatus_bytes, 0, info->xlen[0], set_field(current_mstatus,
1772                                 ie_mask, 0));
1773
1774         reg_mstatus->type->set(reg_mstatus, mstatus_bytes);
1775
1776         /* Run algorithm */
1777         LOG_DEBUG("resume at 0x%" TARGET_PRIxADDR, entry_point);
1778         if (riscv_resume(target, 0, entry_point, 0, 0, true) != ERROR_OK)
1779                 return ERROR_FAIL;
1780
1781         int64_t start = timeval_ms();
1782         while (target->state != TARGET_HALTED) {
1783                 LOG_DEBUG("poll()");
1784                 int64_t now = timeval_ms();
1785                 if (now - start > timeout_ms) {
1786                         LOG_ERROR("Algorithm timed out after %" PRId64 " ms.", now - start);
1787                         riscv_halt(target);
1788                         old_or_new_riscv_poll(target);
1789                         enum gdb_regno regnums[] = {
1790                                 GDB_REGNO_RA, GDB_REGNO_SP, GDB_REGNO_GP, GDB_REGNO_TP,
1791                                 GDB_REGNO_T0, GDB_REGNO_T1, GDB_REGNO_T2, GDB_REGNO_FP,
1792                                 GDB_REGNO_S1, GDB_REGNO_A0, GDB_REGNO_A1, GDB_REGNO_A2,
1793                                 GDB_REGNO_A3, GDB_REGNO_A4, GDB_REGNO_A5, GDB_REGNO_A6,
1794                                 GDB_REGNO_A7, GDB_REGNO_S2, GDB_REGNO_S3, GDB_REGNO_S4,
1795                                 GDB_REGNO_S5, GDB_REGNO_S6, GDB_REGNO_S7, GDB_REGNO_S8,
1796                                 GDB_REGNO_S9, GDB_REGNO_S10, GDB_REGNO_S11, GDB_REGNO_T3,
1797                                 GDB_REGNO_T4, GDB_REGNO_T5, GDB_REGNO_T6,
1798                                 GDB_REGNO_PC,
1799                                 GDB_REGNO_MSTATUS, GDB_REGNO_MEPC, GDB_REGNO_MCAUSE,
1800                         };
1801                         for (unsigned i = 0; i < ARRAY_SIZE(regnums); i++) {
1802                                 enum gdb_regno regno = regnums[i];
1803                                 riscv_reg_t reg_value;
1804                                 if (riscv_get_register(target, &reg_value, regno) != ERROR_OK)
1805                                         break;
1806                                 LOG_ERROR("%s = 0x%" PRIx64, gdb_regno_name(regno), reg_value);
1807                         }
1808                         return ERROR_TARGET_TIMEOUT;
1809                 }
1810
1811                 int result = old_or_new_riscv_poll(target);
1812                 if (result != ERROR_OK)
1813                         return result;
1814         }
1815
1816         /* The current hart id might have been changed in poll(). */
1817         if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
1818                 return ERROR_FAIL;
1819
1820         if (reg_pc->type->get(reg_pc) != ERROR_OK)
1821                 return ERROR_FAIL;
1822         uint64_t final_pc = buf_get_u64(reg_pc->value, 0, reg_pc->size);
1823         if (exit_point && final_pc != exit_point) {
1824                 LOG_ERROR("PC ended up at 0x%" PRIx64 " instead of 0x%"
1825                                 TARGET_PRIxADDR, final_pc, exit_point);
1826                 return ERROR_FAIL;
1827         }
1828
1829         /* Restore Interrupts */
1830         LOG_DEBUG("Restoring Interrupts");
1831         buf_set_u64(mstatus_bytes, 0, info->xlen[0], current_mstatus);
1832         reg_mstatus->type->set(reg_mstatus, mstatus_bytes);
1833
1834         /* Restore registers */
1835         uint8_t buf[8] = { 0 };
1836         buf_set_u64(buf, 0, info->xlen[0], saved_pc);
1837         if (reg_pc->type->set(reg_pc, buf) != ERROR_OK)
1838                 return ERROR_FAIL;
1839
1840         for (int i = 0; i < num_reg_params; i++) {
1841                 if (reg_params[i].direction == PARAM_IN ||
1842                                 reg_params[i].direction == PARAM_IN_OUT) {
1843                         struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, false);
1844                         if (r->type->get(r) != ERROR_OK) {
1845                                 LOG_ERROR("get(%s) failed", r->name);
1846                                 return ERROR_FAIL;
1847                         }
1848                         buf_cpy(r->value, reg_params[i].value, reg_params[i].size);
1849                 }
1850                 LOG_DEBUG("restore %s", reg_params[i].reg_name);
1851                 struct reg *r = register_get_by_name(target->reg_cache, reg_params[i].reg_name, false);
1852                 buf_set_u64(buf, 0, info->xlen[0], saved_regs[r->number]);
1853                 if (r->type->set(r, buf) != ERROR_OK) {
1854                         LOG_ERROR("set(%s) failed", r->name);
1855                         return ERROR_FAIL;
1856                 }
1857         }
1858
1859         return ERROR_OK;
1860 }
1861
1862 static int riscv_checksum_memory(struct target *target,
1863                 target_addr_t address, uint32_t count,
1864                 uint32_t *checksum)
1865 {
1866         struct working_area *crc_algorithm;
1867         struct reg_param reg_params[2];
1868         int retval;
1869
1870         LOG_DEBUG("address=0x%" TARGET_PRIxADDR "; count=0x%" PRIx32, address, count);
1871
1872         static const uint8_t riscv32_crc_code[] = {
1873 #include "../../contrib/loaders/checksum/riscv32_crc.inc"
1874         };
1875         static const uint8_t riscv64_crc_code[] = {
1876 #include "../../contrib/loaders/checksum/riscv64_crc.inc"
1877         };
1878
1879         static const uint8_t *crc_code;
1880
1881         unsigned xlen = riscv_xlen(target);
1882         unsigned crc_code_size;
1883         if (xlen == 32) {
1884                 crc_code = riscv32_crc_code;
1885                 crc_code_size = sizeof(riscv32_crc_code);
1886         } else {
1887                 crc_code = riscv64_crc_code;
1888                 crc_code_size = sizeof(riscv64_crc_code);
1889         }
1890
1891         if (count < crc_code_size * 4) {
1892                 /* Don't use the algorithm for relatively small buffers. It's faster
1893                  * just to read the memory.  target_checksum_memory() will take care of
1894                  * that if we fail. */
1895                 return ERROR_FAIL;
1896         }
1897
1898         retval = target_alloc_working_area(target, crc_code_size, &crc_algorithm);
1899         if (retval != ERROR_OK)
1900                 return retval;
1901
1902         if (crc_algorithm->address + crc_algorithm->size > address &&
1903                         crc_algorithm->address < address + count) {
1904                 /* Region to checksum overlaps with the work area we've been assigned.
1905                  * Bail. (Would be better to manually checksum what we read there, and
1906                  * use the algorithm for the rest.) */
1907                 target_free_working_area(target, crc_algorithm);
1908                 return ERROR_FAIL;
1909         }
1910
1911         retval = target_write_buffer(target, crc_algorithm->address, crc_code_size,
1912                         crc_code);
1913         if (retval != ERROR_OK) {
1914                 LOG_ERROR("Failed to write code to " TARGET_ADDR_FMT ": %d",
1915                                 crc_algorithm->address, retval);
1916                 target_free_working_area(target, crc_algorithm);
1917                 return retval;
1918         }
1919
1920         init_reg_param(&reg_params[0], "a0", xlen, PARAM_IN_OUT);
1921         init_reg_param(&reg_params[1], "a1", xlen, PARAM_OUT);
1922         buf_set_u64(reg_params[0].value, 0, xlen, address);
1923         buf_set_u64(reg_params[1].value, 0, xlen, count);
1924
1925         /* 20 second timeout/megabyte */
1926         int timeout = 20000 * (1 + (count / (1024 * 1024)));
1927
1928         retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
1929                         crc_algorithm->address,
1930                         0,      /* Leave exit point unspecified because we don't know. */
1931                         timeout, NULL);
1932
1933         if (retval == ERROR_OK)
1934                 *checksum = buf_get_u32(reg_params[0].value, 0, 32);
1935         else
1936                 LOG_ERROR("error executing RISC-V CRC algorithm");
1937
1938         destroy_reg_param(&reg_params[0]);
1939         destroy_reg_param(&reg_params[1]);
1940
1941         target_free_working_area(target, crc_algorithm);
1942
1943         LOG_DEBUG("checksum=0x%" PRIx32 ", result=%d", *checksum, retval);
1944
1945         return retval;
1946 }
1947
1948 /*** OpenOCD Helper Functions ***/
1949
1950 enum riscv_poll_hart {
1951         RPH_NO_CHANGE,
1952         RPH_DISCOVERED_HALTED,
1953         RPH_DISCOVERED_RUNNING,
1954         RPH_ERROR
1955 };
1956 static enum riscv_poll_hart riscv_poll_hart(struct target *target, int hartid)
1957 {
1958         RISCV_INFO(r);
1959         if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
1960                 return RPH_ERROR;
1961
1962         LOG_DEBUG("polling hart %d, target->state=%d", hartid, target->state);
1963
1964         /* If OpenOCD thinks we're running but this hart is halted then it's time
1965          * to raise an event. */
1966         bool halted = riscv_is_halted(target);
1967         if (target->state != TARGET_HALTED && halted) {
1968                 LOG_DEBUG("  triggered a halt");
1969                 r->on_halt(target);
1970                 return RPH_DISCOVERED_HALTED;
1971         } else if (target->state != TARGET_RUNNING && !halted) {
1972                 LOG_DEBUG("  triggered running");
1973                 target->state = TARGET_RUNNING;
1974                 target->debug_reason = DBG_REASON_NOTHALTED;
1975                 return RPH_DISCOVERED_RUNNING;
1976         }
1977
1978         return RPH_NO_CHANGE;
1979 }
1980
1981 int set_debug_reason(struct target *target, enum riscv_halt_reason halt_reason)
1982 {
1983         switch (halt_reason) {
1984                 case RISCV_HALT_BREAKPOINT:
1985                         target->debug_reason = DBG_REASON_BREAKPOINT;
1986                         break;
1987                 case RISCV_HALT_TRIGGER:
1988                         target->debug_reason = DBG_REASON_WATCHPOINT;
1989                         break;
1990                 case RISCV_HALT_INTERRUPT:
1991                 case RISCV_HALT_GROUP:
1992                         target->debug_reason = DBG_REASON_DBGRQ;
1993                         break;
1994                 case RISCV_HALT_SINGLESTEP:
1995                         target->debug_reason = DBG_REASON_SINGLESTEP;
1996                         break;
1997                 case RISCV_HALT_UNKNOWN:
1998                         target->debug_reason = DBG_REASON_UNDEFINED;
1999                         break;
2000                 case RISCV_HALT_ERROR:
2001                         return ERROR_FAIL;
2002         }
2003         LOG_DEBUG("[%s] debug_reason=%d", target_name(target), target->debug_reason);
2004         return ERROR_OK;
2005 }
2006
2007 /*** OpenOCD Interface ***/
2008 int riscv_openocd_poll(struct target *target)
2009 {
2010         LOG_DEBUG("polling all harts");
2011         int halted_hart = -1;
2012         if (riscv_rtos_enabled(target)) {
2013                 /* Check every hart for an event. */
2014                 for (int i = 0; i < riscv_count_harts(target); ++i) {
2015                         enum riscv_poll_hart out = riscv_poll_hart(target, i);
2016                         switch (out) {
2017                         case RPH_NO_CHANGE:
2018                         case RPH_DISCOVERED_RUNNING:
2019                                 continue;
2020                         case RPH_DISCOVERED_HALTED:
2021                                 halted_hart = i;
2022                                 break;
2023                         case RPH_ERROR:
2024                                 return ERROR_FAIL;
2025                         }
2026                 }
2027                 if (halted_hart == -1) {
2028                         LOG_DEBUG("  no harts just halted, target->state=%d", target->state);
2029                         return ERROR_OK;
2030                 }
2031                 LOG_DEBUG("  hart %d halted", halted_hart);
2032
2033                 target->state = TARGET_HALTED;
2034                 enum riscv_halt_reason halt_reason = riscv_halt_reason(target, halted_hart);
2035                 if (set_debug_reason(target, halt_reason) != ERROR_OK)
2036                         return ERROR_FAIL;
2037
2038                 target->rtos->current_threadid = halted_hart + 1;
2039                 target->rtos->current_thread = halted_hart + 1;
2040                 riscv_set_rtos_hartid(target, halted_hart);
2041
2042                 /* If we're here then at least one hart triggered.  That means we want
2043                  * to go and halt _every_ hart (configured with -rtos riscv) in the
2044                  * system, as that's the invariant we hold here.  Some harts might have
2045                  * already halted (as we're either in single-step mode or they also
2046                  * triggered a breakpoint), so don't attempt to halt those harts.
2047                  * riscv_halt() will do all that for us. */
2048                 riscv_halt(target);
2049
2050         } else if (target->smp) {
2051                 unsigned halts_discovered = 0;
2052                 unsigned total_targets = 0;
2053                 unsigned should_remain_halted = 0;
2054                 unsigned should_resume = 0;
2055                 unsigned i = 0;
2056                 for (struct target_list *list = target->head; list;
2057                                 list = list->next, i++) {
2058                         total_targets++;
2059                         struct target *t = list->target;
2060                         riscv_info_t *r = riscv_info(t);
2061                         enum riscv_poll_hart out = riscv_poll_hart(t, r->current_hartid);
2062                         switch (out) {
2063                         case RPH_NO_CHANGE:
2064                                 break;
2065                         case RPH_DISCOVERED_RUNNING:
2066                                 t->state = TARGET_RUNNING;
2067                                 t->debug_reason = DBG_REASON_NOTHALTED;
2068                                 break;
2069                         case RPH_DISCOVERED_HALTED:
2070                                 halts_discovered++;
2071                                 t->state = TARGET_HALTED;
2072                                 enum riscv_halt_reason halt_reason =
2073                                         riscv_halt_reason(t, r->current_hartid);
2074                                 if (set_debug_reason(t, halt_reason) != ERROR_OK)
2075                                         return ERROR_FAIL;
2076
2077                                 if (halt_reason == RISCV_HALT_BREAKPOINT) {
2078                                         int retval;
2079                                         switch (riscv_semihosting(t, &retval)) {
2080                                         case SEMI_NONE:
2081                                         case SEMI_WAITING:
2082                                                 /* This hart should remain halted. */
2083                                                 should_remain_halted++;
2084                                                 break;
2085                                         case SEMI_HANDLED:
2086                                                 /* This hart should be resumed, along with any other
2087                                                          * harts that halted due to haltgroups. */
2088                                                 should_resume++;
2089                                                 break;
2090                                         case SEMI_ERROR:
2091                                                 return retval;
2092                                         }
2093                                 } else if (halt_reason != RISCV_HALT_GROUP) {
2094                                         should_remain_halted++;
2095                                 }
2096                                 break;
2097
2098                         case RPH_ERROR:
2099                                 return ERROR_FAIL;
2100                         }
2101                 }
2102
2103                 LOG_DEBUG("should_remain_halted=%d, should_resume=%d",
2104                                   should_remain_halted, should_resume);
2105                 if (should_remain_halted && should_resume) {
2106                         LOG_WARNING("%d harts should remain halted, and %d should resume.",
2107                                                 should_remain_halted, should_resume);
2108                 }
2109                 if (should_remain_halted) {
2110                         LOG_DEBUG("halt all");
2111                         riscv_halt(target);
2112                 } else if (should_resume) {
2113                         LOG_DEBUG("resume all");
2114                         riscv_resume(target, true, 0, 0, 0, false);
2115                 }
2116                 return ERROR_OK;
2117
2118         } else {
2119                 enum riscv_poll_hart out = riscv_poll_hart(target,
2120                                 riscv_current_hartid(target));
2121                 if (out == RPH_NO_CHANGE || out == RPH_DISCOVERED_RUNNING)
2122                         return ERROR_OK;
2123                 else if (out == RPH_ERROR)
2124                         return ERROR_FAIL;
2125
2126                 halted_hart = riscv_current_hartid(target);
2127                 LOG_DEBUG("  hart %d halted", halted_hart);
2128
2129                 enum riscv_halt_reason halt_reason = riscv_halt_reason(target, halted_hart);
2130                 if (set_debug_reason(target, halt_reason) != ERROR_OK)
2131                         return ERROR_FAIL;
2132                 target->state = TARGET_HALTED;
2133         }
2134
2135         if (target->debug_reason == DBG_REASON_BREAKPOINT) {
2136                 int retval;
2137                 switch (riscv_semihosting(target, &retval)) {
2138                         case SEMI_NONE:
2139                         case SEMI_WAITING:
2140                                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
2141                                 break;
2142                         case SEMI_HANDLED:
2143                                 if (riscv_resume(target, true, 0, 0, 0, false) != ERROR_OK)
2144                                         return ERROR_FAIL;
2145                                 break;
2146                         case SEMI_ERROR:
2147                                 return retval;
2148                 }
2149         } else {
2150                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
2151         }
2152
2153         return ERROR_OK;
2154 }
2155
2156 int riscv_openocd_step(struct target *target, int current,
2157                 target_addr_t address, int handle_breakpoints)
2158 {
2159         LOG_DEBUG("stepping rtos hart");
2160
2161         if (!current)
2162                 riscv_set_register(target, GDB_REGNO_PC, address);
2163
2164         riscv_reg_t trigger_state[RISCV_MAX_HWBPS] = {0};
2165         if (disable_triggers(target, trigger_state) != ERROR_OK)
2166                 return ERROR_FAIL;
2167
2168         int out = riscv_step_rtos_hart(target);
2169         if (out != ERROR_OK) {
2170                 LOG_ERROR("unable to step rtos hart");
2171                 return out;
2172         }
2173
2174         register_cache_invalidate(target->reg_cache);
2175
2176         if (enable_triggers(target, trigger_state) != ERROR_OK)
2177                 return ERROR_FAIL;
2178
2179         target->state = TARGET_RUNNING;
2180         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
2181         target->state = TARGET_HALTED;
2182         target->debug_reason = DBG_REASON_SINGLESTEP;
2183         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
2184         return out;
2185 }
2186
2187 /* Command Handlers */
2188 COMMAND_HANDLER(riscv_set_command_timeout_sec)
2189 {
2190         if (CMD_ARGC != 1) {
2191                 LOG_ERROR("Command takes exactly 1 parameter");
2192                 return ERROR_COMMAND_SYNTAX_ERROR;
2193         }
2194         int timeout = atoi(CMD_ARGV[0]);
2195         if (timeout <= 0) {
2196                 LOG_ERROR("%s is not a valid integer argument for command.", CMD_ARGV[0]);
2197                 return ERROR_FAIL;
2198         }
2199
2200         riscv_command_timeout_sec = timeout;
2201
2202         return ERROR_OK;
2203 }
2204
2205 COMMAND_HANDLER(riscv_set_reset_timeout_sec)
2206 {
2207         if (CMD_ARGC != 1) {
2208                 LOG_ERROR("Command takes exactly 1 parameter");
2209                 return ERROR_COMMAND_SYNTAX_ERROR;
2210         }
2211         int timeout = atoi(CMD_ARGV[0]);
2212         if (timeout <= 0) {
2213                 LOG_ERROR("%s is not a valid integer argument for command.", CMD_ARGV[0]);
2214                 return ERROR_FAIL;
2215         }
2216
2217         riscv_reset_timeout_sec = timeout;
2218         return ERROR_OK;
2219 }
2220
2221 COMMAND_HANDLER(riscv_test_compliance) {
2222
2223         struct target *target = get_current_target(CMD_CTX);
2224
2225         RISCV_INFO(r);
2226
2227         if (CMD_ARGC > 0) {
2228                 LOG_ERROR("Command does not take any parameters.");
2229                 return ERROR_COMMAND_SYNTAX_ERROR;
2230         }
2231
2232         if (r->test_compliance) {
2233                 return r->test_compliance(target);
2234         } else {
2235                 LOG_ERROR("This target does not support this command (may implement an older version of the spec).");
2236                 return ERROR_FAIL;
2237         }
2238 }
2239
2240 COMMAND_HANDLER(riscv_set_prefer_sba)
2241 {
2242         if (CMD_ARGC != 1) {
2243                 LOG_ERROR("Command takes exactly 1 parameter");
2244                 return ERROR_COMMAND_SYNTAX_ERROR;
2245         }
2246         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_prefer_sba);
2247         return ERROR_OK;
2248 }
2249
2250 COMMAND_HANDLER(riscv_set_enable_virtual)
2251 {
2252         if (CMD_ARGC != 1) {
2253                 LOG_ERROR("Command takes exactly 1 parameter");
2254                 return ERROR_COMMAND_SYNTAX_ERROR;
2255         }
2256         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_enable_virtual);
2257         return ERROR_OK;
2258 }
2259
2260 void parse_error(const char *string, char c, unsigned position)
2261 {
2262         char buf[position+2];
2263         for (unsigned i = 0; i < position; i++)
2264                 buf[i] = ' ';
2265         buf[position] = '^';
2266         buf[position + 1] = 0;
2267
2268         LOG_ERROR("Parse error at character %c in:", c);
2269         LOG_ERROR("%s", string);
2270         LOG_ERROR("%s", buf);
2271 }
2272
2273 int parse_ranges(range_t **ranges, const char **argv)
2274 {
2275         for (unsigned pass = 0; pass < 2; pass++) {
2276                 unsigned range = 0;
2277                 unsigned low = 0;
2278                 bool parse_low = true;
2279                 unsigned high = 0;
2280                 for (unsigned i = 0; i == 0 || argv[0][i-1]; i++) {
2281                         char c = argv[0][i];
2282                         if (isspace(c)) {
2283                                 /* Ignore whitespace. */
2284                                 continue;
2285                         }
2286
2287                         if (parse_low) {
2288                                 if (isdigit(c)) {
2289                                         low *= 10;
2290                                         low += c - '0';
2291                                 } else if (c == '-') {
2292                                         parse_low = false;
2293                                 } else if (c == ',' || c == 0) {
2294                                         if (pass == 1) {
2295                                                 (*ranges)[range].low = low;
2296                                                 (*ranges)[range].high = low;
2297                                         }
2298                                         low = 0;
2299                                         range++;
2300                                 } else {
2301                                         parse_error(argv[0], c, i);
2302                                         return ERROR_COMMAND_SYNTAX_ERROR;
2303                                 }
2304
2305                         } else {
2306                                 if (isdigit(c)) {
2307                                         high *= 10;
2308                                         high += c - '0';
2309                                 } else if (c == ',' || c == 0) {
2310                                         parse_low = true;
2311                                         if (pass == 1) {
2312                                                 (*ranges)[range].low = low;
2313                                                 (*ranges)[range].high = high;
2314                                         }
2315                                         low = 0;
2316                                         high = 0;
2317                                         range++;
2318                                 } else {
2319                                         parse_error(argv[0], c, i);
2320                                         return ERROR_COMMAND_SYNTAX_ERROR;
2321                                 }
2322                         }
2323                 }
2324
2325                 if (pass == 0) {
2326                         free(*ranges);
2327                         *ranges = calloc(range + 2, sizeof(range_t));
2328                         if (!*ranges)
2329                                 return ERROR_FAIL;
2330                 } else {
2331                         (*ranges)[range].low = 1;
2332                         (*ranges)[range].high = 0;
2333                 }
2334         }
2335
2336         return ERROR_OK;
2337 }
2338
2339 COMMAND_HANDLER(riscv_set_expose_csrs)
2340 {
2341         if (CMD_ARGC != 1) {
2342                 LOG_ERROR("Command takes exactly 1 parameter");
2343                 return ERROR_COMMAND_SYNTAX_ERROR;
2344         }
2345
2346         return parse_ranges(&expose_csr, CMD_ARGV);
2347 }
2348
2349 COMMAND_HANDLER(riscv_set_expose_custom)
2350 {
2351         if (CMD_ARGC != 1) {
2352                 LOG_ERROR("Command takes exactly 1 parameter");
2353                 return ERROR_COMMAND_SYNTAX_ERROR;
2354         }
2355
2356         return parse_ranges(&expose_custom, CMD_ARGV);
2357 }
2358
2359 COMMAND_HANDLER(riscv_authdata_read)
2360 {
2361         if (CMD_ARGC != 0) {
2362                 LOG_ERROR("Command takes no parameters");
2363                 return ERROR_COMMAND_SYNTAX_ERROR;
2364         }
2365
2366         struct target *target = get_current_target(CMD_CTX);
2367         if (!target) {
2368                 LOG_ERROR("target is NULL!");
2369                 return ERROR_FAIL;
2370         }
2371
2372         RISCV_INFO(r);
2373         if (!r) {
2374                 LOG_ERROR("riscv_info is NULL!");
2375                 return ERROR_FAIL;
2376         }
2377
2378         if (r->authdata_read) {
2379                 uint32_t value;
2380                 if (r->authdata_read(target, &value) != ERROR_OK)
2381                         return ERROR_FAIL;
2382                 command_print_sameline(CMD, "0x%08" PRIx32, value);
2383                 return ERROR_OK;
2384         } else {
2385                 LOG_ERROR("authdata_read is not implemented for this target.");
2386                 return ERROR_FAIL;
2387         }
2388 }
2389
2390 COMMAND_HANDLER(riscv_authdata_write)
2391 {
2392         if (CMD_ARGC != 1) {
2393                 LOG_ERROR("Command takes exactly 1 argument");
2394                 return ERROR_COMMAND_SYNTAX_ERROR;
2395         }
2396
2397         struct target *target = get_current_target(CMD_CTX);
2398         RISCV_INFO(r);
2399
2400         uint32_t value;
2401         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], value);
2402
2403         if (r->authdata_write) {
2404                 return r->authdata_write(target, value);
2405         } else {
2406                 LOG_ERROR("authdata_write is not implemented for this target.");
2407                 return ERROR_FAIL;
2408         }
2409 }
2410
2411 COMMAND_HANDLER(riscv_dmi_read)
2412 {
2413         if (CMD_ARGC != 1) {
2414                 LOG_ERROR("Command takes 1 parameter");
2415                 return ERROR_COMMAND_SYNTAX_ERROR;
2416         }
2417
2418         struct target *target = get_current_target(CMD_CTX);
2419         if (!target) {
2420                 LOG_ERROR("target is NULL!");
2421                 return ERROR_FAIL;
2422         }
2423
2424         RISCV_INFO(r);
2425         if (!r) {
2426                 LOG_ERROR("riscv_info is NULL!");
2427                 return ERROR_FAIL;
2428         }
2429
2430         if (r->dmi_read) {
2431                 uint32_t address, value;
2432                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
2433                 if (r->dmi_read(target, &value, address) != ERROR_OK)
2434                         return ERROR_FAIL;
2435                 command_print(CMD, "0x%" PRIx32, value);
2436                 return ERROR_OK;
2437         } else {
2438                 LOG_ERROR("dmi_read is not implemented for this target.");
2439                 return ERROR_FAIL;
2440         }
2441 }
2442
2443
2444 COMMAND_HANDLER(riscv_dmi_write)
2445 {
2446         if (CMD_ARGC != 2) {
2447                 LOG_ERROR("Command takes exactly 2 arguments");
2448                 return ERROR_COMMAND_SYNTAX_ERROR;
2449         }
2450
2451         struct target *target = get_current_target(CMD_CTX);
2452         RISCV_INFO(r);
2453
2454         uint32_t address, value;
2455         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
2456         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
2457
2458         if (r->dmi_write) {
2459                 return r->dmi_write(target, address, value);
2460         } else {
2461                 LOG_ERROR("dmi_write is not implemented for this target.");
2462                 return ERROR_FAIL;
2463         }
2464 }
2465
2466 COMMAND_HANDLER(riscv_test_sba_config_reg)
2467 {
2468         if (CMD_ARGC != 4) {
2469                 LOG_ERROR("Command takes exactly 4 arguments");
2470                 return ERROR_COMMAND_SYNTAX_ERROR;
2471         }
2472
2473         struct target *target = get_current_target(CMD_CTX);
2474         RISCV_INFO(r);
2475
2476         target_addr_t legal_address;
2477         uint32_t num_words;
2478         target_addr_t illegal_address;
2479         bool run_sbbusyerror_test;
2480
2481         COMMAND_PARSE_NUMBER(target_addr, CMD_ARGV[0], legal_address);
2482         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], num_words);
2483         COMMAND_PARSE_NUMBER(target_addr, CMD_ARGV[2], illegal_address);
2484         COMMAND_PARSE_ON_OFF(CMD_ARGV[3], run_sbbusyerror_test);
2485
2486         if (r->test_sba_config_reg) {
2487                 return r->test_sba_config_reg(target, legal_address, num_words,
2488                                 illegal_address, run_sbbusyerror_test);
2489         } else {
2490                 LOG_ERROR("test_sba_config_reg is not implemented for this target.");
2491                 return ERROR_FAIL;
2492         }
2493 }
2494
2495 COMMAND_HANDLER(riscv_reset_delays)
2496 {
2497         int wait = 0;
2498
2499         if (CMD_ARGC > 1) {
2500                 LOG_ERROR("Command takes at most one argument");
2501                 return ERROR_COMMAND_SYNTAX_ERROR;
2502         }
2503
2504         if (CMD_ARGC == 1)
2505                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], wait);
2506
2507         struct target *target = get_current_target(CMD_CTX);
2508         RISCV_INFO(r);
2509         r->reset_delays_wait = wait;
2510         return ERROR_OK;
2511 }
2512
2513 COMMAND_HANDLER(riscv_set_ir)
2514 {
2515         if (CMD_ARGC != 2) {
2516                 LOG_ERROR("Command takes exactly 2 arguments");
2517                 return ERROR_COMMAND_SYNTAX_ERROR;
2518         }
2519
2520         uint32_t value;
2521         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
2522
2523         if (!strcmp(CMD_ARGV[0], "idcode"))
2524                 buf_set_u32(ir_idcode, 0, 32, value);
2525         else if (!strcmp(CMD_ARGV[0], "dtmcs"))
2526                 buf_set_u32(ir_dtmcontrol, 0, 32, value);
2527         else if (!strcmp(CMD_ARGV[0], "dmi"))
2528                 buf_set_u32(ir_dbus, 0, 32, value);
2529         else
2530                 return ERROR_FAIL;
2531
2532         return ERROR_OK;
2533 }
2534
2535 COMMAND_HANDLER(riscv_resume_order)
2536 {
2537         if (CMD_ARGC > 1) {
2538                 LOG_ERROR("Command takes at most one argument");
2539                 return ERROR_COMMAND_SYNTAX_ERROR;
2540         }
2541
2542         if (!strcmp(CMD_ARGV[0], "normal")) {
2543                 resume_order = RO_NORMAL;
2544         } else if (!strcmp(CMD_ARGV[0], "reversed")) {
2545                 resume_order = RO_REVERSED;
2546         } else {
2547                 LOG_ERROR("Unsupported resume order: %s", CMD_ARGV[0]);
2548                 return ERROR_FAIL;
2549         }
2550
2551         return ERROR_OK;
2552 }
2553
2554 COMMAND_HANDLER(riscv_use_bscan_tunnel)
2555 {
2556         int irwidth = 0;
2557         int tunnel_type = BSCAN_TUNNEL_NESTED_TAP;
2558
2559         if (CMD_ARGC > 2) {
2560                 LOG_ERROR("Command takes at most two arguments");
2561                 return ERROR_COMMAND_SYNTAX_ERROR;
2562         } else if (CMD_ARGC == 1) {
2563                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], irwidth);
2564         } else if (CMD_ARGC == 2) {
2565                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], irwidth);
2566                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], tunnel_type);
2567         }
2568         if (tunnel_type == BSCAN_TUNNEL_NESTED_TAP)
2569                 LOG_INFO("Nested Tap based Bscan Tunnel Selected");
2570         else if (tunnel_type == BSCAN_TUNNEL_DATA_REGISTER)
2571                 LOG_INFO("Simple Register based Bscan Tunnel Selected");
2572         else
2573                 LOG_INFO("Invalid Tunnel type selected ! : selecting default Nested Tap Type");
2574
2575         bscan_tunnel_type = tunnel_type;
2576         bscan_tunnel_ir_width = irwidth;
2577         return ERROR_OK;
2578 }
2579
2580 COMMAND_HANDLER(riscv_set_enable_virt2phys)
2581 {
2582         if (CMD_ARGC != 1) {
2583                 LOG_ERROR("Command takes exactly 1 parameter");
2584                 return ERROR_COMMAND_SYNTAX_ERROR;
2585         }
2586         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_enable_virt2phys);
2587         return ERROR_OK;
2588 }
2589
2590 COMMAND_HANDLER(riscv_set_ebreakm)
2591 {
2592         if (CMD_ARGC != 1) {
2593                 LOG_ERROR("Command takes exactly 1 parameter");
2594                 return ERROR_COMMAND_SYNTAX_ERROR;
2595         }
2596         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_ebreakm);
2597         return ERROR_OK;
2598 }
2599
2600 COMMAND_HANDLER(riscv_set_ebreaks)
2601 {
2602         if (CMD_ARGC != 1) {
2603                 LOG_ERROR("Command takes exactly 1 parameter");
2604                 return ERROR_COMMAND_SYNTAX_ERROR;
2605         }
2606         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_ebreaks);
2607         return ERROR_OK;
2608 }
2609
2610 COMMAND_HANDLER(riscv_set_ebreaku)
2611 {
2612         if (CMD_ARGC != 1) {
2613                 LOG_ERROR("Command takes exactly 1 parameter");
2614                 return ERROR_COMMAND_SYNTAX_ERROR;
2615         }
2616         COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_ebreaku);
2617         return ERROR_OK;
2618 }
2619
2620 static const struct command_registration riscv_exec_command_handlers[] = {
2621         {
2622                 .name = "test_compliance",
2623                 .handler = riscv_test_compliance,
2624                 .usage = "",
2625                 .mode = COMMAND_EXEC,
2626                 .help = "Runs a basic compliance test suite against the RISC-V Debug Spec."
2627         },
2628         {
2629                 .name = "set_command_timeout_sec",
2630                 .handler = riscv_set_command_timeout_sec,
2631                 .mode = COMMAND_ANY,
2632                 .usage = "[sec]",
2633                 .help = "Set the wall-clock timeout (in seconds) for individual commands"
2634         },
2635         {
2636                 .name = "set_reset_timeout_sec",
2637                 .handler = riscv_set_reset_timeout_sec,
2638                 .mode = COMMAND_ANY,
2639                 .usage = "[sec]",
2640                 .help = "Set the wall-clock timeout (in seconds) after reset is deasserted"
2641         },
2642         {
2643                 .name = "set_prefer_sba",
2644                 .handler = riscv_set_prefer_sba,
2645                 .mode = COMMAND_ANY,
2646                 .usage = "on|off",
2647                 .help = "When on, prefer to use System Bus Access to access memory. "
2648                         "When off (default), prefer to use the Program Buffer to access memory."
2649         },
2650         {
2651                 .name = "set_enable_virtual",
2652                 .handler = riscv_set_enable_virtual,
2653                 .mode = COMMAND_ANY,
2654                 .usage = "on|off",
2655                 .help = "When on, memory accesses are performed on physical or virtual "
2656                                 "memory depending on the current system configuration. "
2657                                 "When off (default), all memory accessses are performed on physical memory."
2658         },
2659         {
2660                 .name = "expose_csrs",
2661                 .handler = riscv_set_expose_csrs,
2662                 .mode = COMMAND_ANY,
2663                 .usage = "n0[-m0][,n1[-m1]]...",
2664                 .help = "Configure a list of inclusive ranges for CSRs to expose in "
2665                                 "addition to the standard ones. This must be executed before "
2666                                 "`init`."
2667         },
2668         {
2669                 .name = "expose_custom",
2670                 .handler = riscv_set_expose_custom,
2671                 .mode = COMMAND_ANY,
2672                 .usage = "n0[-m0][,n1[-m1]]...",
2673                 .help = "Configure a list of inclusive ranges for custom registers to "
2674                         "expose. custom0 is accessed as abstract register number 0xc000, "
2675                         "etc. This must be executed before `init`."
2676         },
2677         {
2678                 .name = "authdata_read",
2679                 .handler = riscv_authdata_read,
2680                 .usage = "",
2681                 .mode = COMMAND_ANY,
2682                 .help = "Return the 32-bit value read from authdata."
2683         },
2684         {
2685                 .name = "authdata_write",
2686                 .handler = riscv_authdata_write,
2687                 .mode = COMMAND_ANY,
2688                 .usage = "value",
2689                 .help = "Write the 32-bit value to authdata."
2690         },
2691         {
2692                 .name = "dmi_read",
2693                 .handler = riscv_dmi_read,
2694                 .mode = COMMAND_ANY,
2695                 .usage = "address",
2696                 .help = "Perform a 32-bit DMI read at address, returning the value."
2697         },
2698         {
2699                 .name = "dmi_write",
2700                 .handler = riscv_dmi_write,
2701                 .mode = COMMAND_ANY,
2702                 .usage = "address value",
2703                 .help = "Perform a 32-bit DMI write of value at address."
2704         },
2705         {
2706                 .name = "test_sba_config_reg",
2707                 .handler = riscv_test_sba_config_reg,
2708                 .mode = COMMAND_ANY,
2709                 .usage = "legal_address num_words "
2710                         "illegal_address run_sbbusyerror_test[on/off]",
2711                 .help = "Perform a series of tests on the SBCS register. "
2712                         "Inputs are a legal, 128-byte aligned address and a number of words to "
2713                         "read/write starting at that address (i.e., address range [legal address, "
2714                         "legal_address+word_size*num_words) must be legally readable/writable), "
2715                         "an illegal, 128-byte aligned address for error flag/handling cases, "
2716                         "and whether sbbusyerror test should be run."
2717         },
2718         {
2719                 .name = "reset_delays",
2720                 .handler = riscv_reset_delays,
2721                 .mode = COMMAND_ANY,
2722                 .usage = "[wait]",
2723                 .help = "OpenOCD learns how many Run-Test/Idle cycles are required "
2724                         "between scans to avoid encountering the target being busy. This "
2725                         "command resets those learned values after `wait` scans. It's only "
2726                         "useful for testing OpenOCD itself."
2727         },
2728         {
2729                 .name = "resume_order",
2730                 .handler = riscv_resume_order,
2731                 .mode = COMMAND_ANY,
2732                 .usage = "normal|reversed",
2733                 .help = "Choose the order that harts are resumed in when `hasel` is not "
2734                         "supported. Normal order is from lowest hart index to highest. "
2735                         "Reversed order is from highest hart index to lowest."
2736         },
2737         {
2738                 .name = "set_ir",
2739                 .handler = riscv_set_ir,
2740                 .mode = COMMAND_ANY,
2741                 .usage = "[idcode|dtmcs|dmi] value",
2742                 .help = "Set IR value for specified JTAG register."
2743         },
2744         {
2745                 .name = "use_bscan_tunnel",
2746                 .handler = riscv_use_bscan_tunnel,
2747                 .mode = COMMAND_ANY,
2748                 .usage = "value [type]",
2749                 .help = "Enable or disable use of a BSCAN tunnel to reach DM.  Supply "
2750                         "the width of the DM transport TAP's instruction register to "
2751                         "enable.  Supply a value of 0 to disable. Pass A second argument "
2752                         "(optional) to indicate Bscan Tunnel Type {0:(default) NESTED_TAP , "
2753                         "1: DATA_REGISTER}"
2754         },
2755         {
2756                 .name = "set_enable_virt2phys",
2757                 .handler = riscv_set_enable_virt2phys,
2758                 .mode = COMMAND_ANY,
2759                 .usage = "on|off",
2760                 .help = "When on (default), enable translation from virtual address to "
2761                         "physical address."
2762         },
2763         {
2764                 .name = "set_ebreakm",
2765                 .handler = riscv_set_ebreakm,
2766                 .mode = COMMAND_ANY,
2767                 .usage = "on|off",
2768                 .help = "Control dcsr.ebreakm. When off, M-mode ebreak instructions "
2769                         "don't trap to OpenOCD. Defaults to on."
2770         },
2771         {
2772                 .name = "set_ebreaks",
2773                 .handler = riscv_set_ebreaks,
2774                 .mode = COMMAND_ANY,
2775                 .usage = "on|off",
2776                 .help = "Control dcsr.ebreaks. When off, S-mode ebreak instructions "
2777                         "don't trap to OpenOCD. Defaults to on."
2778         },
2779         {
2780                 .name = "set_ebreaku",
2781                 .handler = riscv_set_ebreaku,
2782                 .mode = COMMAND_ANY,
2783                 .usage = "on|off",
2784                 .help = "Control dcsr.ebreaku. When off, U-mode ebreak instructions "
2785                         "don't trap to OpenOCD. Defaults to on."
2786         },
2787         COMMAND_REGISTRATION_DONE
2788 };
2789
2790 /*
2791  * To be noted that RISC-V targets use the same semihosting commands as
2792  * ARM targets.
2793  *
2794  * The main reason is compatibility with existing tools. For example the
2795  * Eclipse OpenOCD/SEGGER J-Link/QEMU plug-ins have several widgets to
2796  * configure semihosting, which generate commands like `arm semihosting
2797  * enable`.
2798  * A secondary reason is the fact that the protocol used is exactly the
2799  * one specified by ARM. If RISC-V will ever define its own semihosting
2800  * protocol, then a command like `riscv semihosting enable` will make
2801  * sense, but for now all semihosting commands are prefixed with `arm`.
2802  */
2803 extern const struct command_registration semihosting_common_handlers[];
2804
2805 const struct command_registration riscv_command_handlers[] = {
2806         {
2807                 .name = "riscv",
2808                 .mode = COMMAND_ANY,
2809                 .help = "RISC-V Command Group",
2810                 .usage = "",
2811                 .chain = riscv_exec_command_handlers
2812         },
2813         {
2814                 .name = "arm",
2815                 .mode = COMMAND_ANY,
2816                 .help = "ARM Command Group",
2817                 .usage = "",
2818                 .chain = semihosting_common_handlers
2819         },
2820         COMMAND_REGISTRATION_DONE
2821 };
2822
2823 static unsigned riscv_xlen_nonconst(struct target *target)
2824 {
2825         return riscv_xlen(target);
2826 }
2827
2828 static unsigned int riscv_data_bits(struct target *target)
2829 {
2830         RISCV_INFO(r);
2831         if (r->data_bits)
2832                 return r->data_bits(target);
2833         return riscv_xlen(target);
2834 }
2835
2836 struct target_type riscv_target = {
2837         .name = "riscv",
2838
2839         .init_target = riscv_init_target,
2840         .deinit_target = riscv_deinit_target,
2841         .examine = riscv_examine,
2842
2843         /* poll current target status */
2844         .poll = old_or_new_riscv_poll,
2845
2846         .halt = riscv_halt,
2847         .resume = riscv_target_resume,
2848         .step = old_or_new_riscv_step,
2849
2850         .assert_reset = riscv_assert_reset,
2851         .deassert_reset = riscv_deassert_reset,
2852
2853         .read_memory = riscv_read_memory,
2854         .write_memory = riscv_write_memory,
2855         .read_phys_memory = riscv_read_phys_memory,
2856         .write_phys_memory = riscv_write_phys_memory,
2857
2858         .checksum_memory = riscv_checksum_memory,
2859
2860         .mmu = riscv_mmu,
2861         .virt2phys = riscv_virt2phys,
2862
2863         .get_gdb_arch = riscv_get_gdb_arch,
2864         .get_gdb_reg_list = riscv_get_gdb_reg_list,
2865         .get_gdb_reg_list_noread = riscv_get_gdb_reg_list_noread,
2866
2867         .add_breakpoint = riscv_add_breakpoint,
2868         .remove_breakpoint = riscv_remove_breakpoint,
2869
2870         .add_watchpoint = riscv_add_watchpoint,
2871         .remove_watchpoint = riscv_remove_watchpoint,
2872         .hit_watchpoint = riscv_hit_watchpoint,
2873
2874         .arch_state = riscv_arch_state,
2875
2876         .run_algorithm = riscv_run_algorithm,
2877
2878         .commands = riscv_command_handlers,
2879
2880         .address_bits = riscv_xlen_nonconst,
2881         .data_bits = riscv_data_bits
2882 };
2883
2884 /*** RISC-V Interface ***/
2885
2886 void riscv_info_init(struct target *target, riscv_info_t *r)
2887 {
2888         memset(r, 0, sizeof(*r));
2889         r->dtm_version = 1;
2890         r->registers_initialized = false;
2891         r->current_hartid = target->coreid;
2892
2893         memset(r->trigger_unique_id, 0xff, sizeof(r->trigger_unique_id));
2894
2895         for (size_t h = 0; h < RISCV_MAX_HARTS; ++h)
2896                 r->xlen[h] = -1;
2897 }
2898
2899 static int riscv_resume_go_all_harts(struct target *target)
2900 {
2901         RISCV_INFO(r);
2902
2903         /* Dummy variables to make mingw32-gcc happy. */
2904         int first = 0;
2905         int last = 1;
2906         int step = 1;
2907         switch (resume_order) {
2908                 case RO_NORMAL:
2909                         first = 0;
2910                         last = riscv_count_harts(target) - 1;
2911                         step = 1;
2912                         break;
2913                 case RO_REVERSED:
2914                         first = riscv_count_harts(target) - 1;
2915                         last = 0;
2916                         step = -1;
2917                         break;
2918                 default:
2919                         assert(0);
2920         }
2921
2922         for (int i = first; i != last + step; i += step) {
2923                 if (!riscv_hart_enabled(target, i))
2924                         continue;
2925
2926                 LOG_DEBUG("resuming hart %d", i);
2927                 if (riscv_set_current_hartid(target, i) != ERROR_OK)
2928                         return ERROR_FAIL;
2929                 if (riscv_is_halted(target)) {
2930                         if (r->resume_go(target) != ERROR_OK)
2931                                 return ERROR_FAIL;
2932                 } else {
2933                         LOG_DEBUG("  hart %d requested resume, but was already resumed", i);
2934                 }
2935         }
2936
2937         riscv_invalidate_register_cache(target);
2938         return ERROR_OK;
2939 }
2940
2941 int riscv_step_rtos_hart(struct target *target)
2942 {
2943         RISCV_INFO(r);
2944         int hartid = r->current_hartid;
2945         if (riscv_rtos_enabled(target)) {
2946                 hartid = r->rtos_hartid;
2947                 if (hartid == -1) {
2948                         LOG_DEBUG("GDB has asked me to step \"any\" thread, so I'm stepping hart 0.");
2949                         hartid = 0;
2950                 }
2951         }
2952         if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
2953                 return ERROR_FAIL;
2954         LOG_DEBUG("stepping hart %d", hartid);
2955
2956         if (!riscv_is_halted(target)) {
2957                 LOG_ERROR("Hart isn't halted before single step!");
2958                 return ERROR_FAIL;
2959         }
2960         riscv_invalidate_register_cache(target);
2961         r->on_step(target);
2962         if (r->step_current_hart(target) != ERROR_OK)
2963                 return ERROR_FAIL;
2964         riscv_invalidate_register_cache(target);
2965         r->on_halt(target);
2966         if (!riscv_is_halted(target)) {
2967                 LOG_ERROR("Hart was not halted after single step!");
2968                 return ERROR_FAIL;
2969         }
2970         return ERROR_OK;
2971 }
2972
2973 bool riscv_supports_extension(struct target *target, int hartid, char letter)
2974 {
2975         RISCV_INFO(r);
2976         unsigned num;
2977         if (letter >= 'a' && letter <= 'z')
2978                 num = letter - 'a';
2979         else if (letter >= 'A' && letter <= 'Z')
2980                 num = letter - 'A';
2981         else
2982                 return false;
2983         return r->misa[hartid] & (1 << num);
2984 }
2985
2986 unsigned riscv_xlen(const struct target *target)
2987 {
2988         return riscv_xlen_of_hart(target, riscv_current_hartid(target));
2989 }
2990
2991 int riscv_xlen_of_hart(const struct target *target, int hartid)
2992 {
2993         RISCV_INFO(r);
2994         assert(r->xlen[hartid] != -1);
2995         return r->xlen[hartid];
2996 }
2997
2998 bool riscv_rtos_enabled(const struct target *target)
2999 {
3000         return false;
3001 }
3002
3003 int riscv_set_current_hartid(struct target *target, int hartid)
3004 {
3005         RISCV_INFO(r);
3006         if (!r->select_current_hart)
3007                 return ERROR_OK;
3008
3009         int previous_hartid = riscv_current_hartid(target);
3010         r->current_hartid = hartid;
3011         assert(riscv_hart_enabled(target, hartid));
3012         LOG_DEBUG("setting hartid to %d, was %d", hartid, previous_hartid);
3013         if (r->select_current_hart(target) != ERROR_OK)
3014                 return ERROR_FAIL;
3015
3016         /* This might get called during init, in which case we shouldn't be
3017          * setting up the register cache. */
3018         if (target_was_examined(target) && riscv_rtos_enabled(target))
3019                 riscv_invalidate_register_cache(target);
3020
3021         return ERROR_OK;
3022 }
3023
3024 void riscv_invalidate_register_cache(struct target *target)
3025 {
3026         RISCV_INFO(r);
3027
3028         LOG_DEBUG("[%d]", target->coreid);
3029         register_cache_invalidate(target->reg_cache);
3030         for (size_t i = 0; i < target->reg_cache->num_regs; ++i) {
3031                 struct reg *reg = &target->reg_cache->reg_list[i];
3032                 reg->valid = false;
3033         }
3034
3035         r->registers_initialized = true;
3036 }
3037
3038 int riscv_current_hartid(const struct target *target)
3039 {
3040         RISCV_INFO(r);
3041         return r->current_hartid;
3042 }
3043
3044 void riscv_set_all_rtos_harts(struct target *target)
3045 {
3046         RISCV_INFO(r);
3047         r->rtos_hartid = -1;
3048 }
3049
3050 void riscv_set_rtos_hartid(struct target *target, int hartid)
3051 {
3052         LOG_DEBUG("setting RTOS hartid %d", hartid);
3053         RISCV_INFO(r);
3054         r->rtos_hartid = hartid;
3055 }
3056
3057 int riscv_count_harts(struct target *target)
3058 {
3059         if (!target)
3060                 return 1;
3061         RISCV_INFO(r);
3062         if (!r || !r->hart_count)
3063                 return 1;
3064         return r->hart_count(target);
3065 }
3066
3067 bool riscv_has_register(struct target *target, int hartid, int regid)
3068 {
3069         return 1;
3070 }
3071
3072 /**
3073  * If write is true:
3074  *   return true iff we are guaranteed that the register will contain exactly
3075  *       the value we just wrote when it's read.
3076  * If write is false:
3077  *   return true iff we are guaranteed that the register will read the same
3078  *       value in the future as the value we just read.
3079  */
3080 static bool gdb_regno_cacheable(enum gdb_regno regno, bool write)
3081 {
3082         /* GPRs, FPRs, vector registers are just normal data stores. */
3083         if (regno <= GDB_REGNO_XPR31 ||
3084                         (regno >= GDB_REGNO_FPR0 && regno <= GDB_REGNO_FPR31) ||
3085                         (regno >= GDB_REGNO_V0 && regno <= GDB_REGNO_V31))
3086                 return true;
3087
3088         /* Most CSRs won't change value on us, but we can't assume it about rbitrary
3089          * CSRs. */
3090         switch (regno) {
3091                 case GDB_REGNO_DPC:
3092                         return true;
3093
3094                 case GDB_REGNO_VSTART:
3095                 case GDB_REGNO_VXSAT:
3096                 case GDB_REGNO_VXRM:
3097                 case GDB_REGNO_VLENB:
3098                 case GDB_REGNO_VL:
3099                 case GDB_REGNO_VTYPE:
3100                 case GDB_REGNO_MISA:
3101                 case GDB_REGNO_DCSR:
3102                 case GDB_REGNO_DSCRATCH0:
3103                 case GDB_REGNO_MSTATUS:
3104                 case GDB_REGNO_MEPC:
3105                 case GDB_REGNO_MCAUSE:
3106                 case GDB_REGNO_SATP:
3107                         /*
3108                          * WARL registers might not contain the value we just wrote, but
3109                          * these ones won't spontaneously change their value either. *
3110                          */
3111                         return !write;
3112
3113                 case GDB_REGNO_TSELECT: /* I think this should be above, but then it doesn't work. */
3114                 case GDB_REGNO_TDATA1:  /* Changes value when tselect is changed. */
3115                 case GDB_REGNO_TDATA2:  /* Changse value when tselect is changed. */
3116                 default:
3117                         return false;
3118         }
3119 }
3120
3121 /**
3122  * This function is called when the debug user wants to change the value of a
3123  * register. The new value may be cached, and may not be written until the hart
3124  * is resumed. */
3125 int riscv_set_register(struct target *target, enum gdb_regno r, riscv_reg_t v)
3126 {
3127         return riscv_set_register_on_hart(target, riscv_current_hartid(target), r, v);
3128 }
3129
3130 int riscv_set_register_on_hart(struct target *target, int hartid,
3131                 enum gdb_regno regid, uint64_t value)
3132 {
3133         RISCV_INFO(r);
3134         LOG_DEBUG("{%d} %s <- %" PRIx64, hartid, gdb_regno_name(regid), value);
3135         assert(r->set_register);
3136
3137         /* TODO: Hack to deal with gdb that thinks these registers still exist. */
3138         if (regid > GDB_REGNO_XPR15 && regid <= GDB_REGNO_XPR31 && value == 0 &&
3139                         riscv_supports_extension(target, hartid, 'E'))
3140                 return ERROR_OK;
3141
3142         struct reg *reg = &target->reg_cache->reg_list[regid];
3143         buf_set_u64(reg->value, 0, reg->size, value);
3144
3145         int result = r->set_register(target, hartid, regid, value);
3146         if (result == ERROR_OK)
3147                 reg->valid = gdb_regno_cacheable(regid, true);
3148         else
3149                 reg->valid = false;
3150         LOG_DEBUG("[%s]{%d} wrote 0x%" PRIx64 " to %s valid=%d",
3151                           target_name(target), hartid, value, reg->name, reg->valid);
3152         return result;
3153 }
3154
3155 int riscv_get_register(struct target *target, riscv_reg_t *value,
3156                 enum gdb_regno r)
3157 {
3158         return riscv_get_register_on_hart(target, value,
3159                         riscv_current_hartid(target), r);
3160 }
3161
3162 int riscv_get_register_on_hart(struct target *target, riscv_reg_t *value,
3163                 int hartid, enum gdb_regno regid)
3164 {
3165         RISCV_INFO(r);
3166
3167         struct reg *reg = &target->reg_cache->reg_list[regid];
3168         if (!reg->exist) {
3169                 LOG_DEBUG("[%s]{%d} %s does not exist.",
3170                                   target_name(target), hartid, gdb_regno_name(regid));
3171                 return ERROR_FAIL;
3172         }
3173
3174         if (reg && reg->valid && hartid == riscv_current_hartid(target)) {
3175                 *value = buf_get_u64(reg->value, 0, reg->size);
3176                 LOG_DEBUG("{%d} %s: %" PRIx64 " (cached)", hartid,
3177                                   gdb_regno_name(regid), *value);
3178                 return ERROR_OK;
3179         }
3180
3181         /* TODO: Hack to deal with gdb that thinks these registers still exist. */
3182         if (regid > GDB_REGNO_XPR15 && regid <= GDB_REGNO_XPR31 &&
3183                         riscv_supports_extension(target, hartid, 'E')) {
3184                 *value = 0;
3185                 return ERROR_OK;
3186         }
3187
3188         int result = r->get_register(target, value, hartid, regid);
3189
3190         if (result == ERROR_OK)
3191                 reg->valid = gdb_regno_cacheable(regid, false);
3192
3193         LOG_DEBUG("{%d} %s: %" PRIx64, hartid, gdb_regno_name(regid), *value);
3194         return result;
3195 }
3196
3197 bool riscv_is_halted(struct target *target)
3198 {
3199         RISCV_INFO(r);
3200         assert(r->is_halted);
3201         return r->is_halted(target);
3202 }
3203
3204 enum riscv_halt_reason riscv_halt_reason(struct target *target, int hartid)
3205 {
3206         RISCV_INFO(r);
3207         if (riscv_set_current_hartid(target, hartid) != ERROR_OK)
3208                 return RISCV_HALT_ERROR;
3209         if (!riscv_is_halted(target)) {
3210                 LOG_ERROR("Hart is not halted!");
3211                 return RISCV_HALT_UNKNOWN;
3212         }
3213         return r->halt_reason(target);
3214 }
3215
3216 size_t riscv_debug_buffer_size(struct target *target)
3217 {
3218         RISCV_INFO(r);
3219         return r->debug_buffer_size[riscv_current_hartid(target)];
3220 }
3221
3222 int riscv_write_debug_buffer(struct target *target, int index, riscv_insn_t insn)
3223 {
3224         RISCV_INFO(r);
3225         r->write_debug_buffer(target, index, insn);
3226         return ERROR_OK;
3227 }
3228
3229 riscv_insn_t riscv_read_debug_buffer(struct target *target, int index)
3230 {
3231         RISCV_INFO(r);
3232         return r->read_debug_buffer(target, index);
3233 }
3234
3235 int riscv_execute_debug_buffer(struct target *target)
3236 {
3237         RISCV_INFO(r);
3238         return r->execute_debug_buffer(target);
3239 }
3240
3241 void riscv_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d)
3242 {
3243         RISCV_INFO(r);
3244         r->fill_dmi_write_u64(target, buf, a, d);
3245 }
3246
3247 void riscv_fill_dmi_read_u64(struct target *target, char *buf, int a)
3248 {
3249         RISCV_INFO(r);
3250         r->fill_dmi_read_u64(target, buf, a);
3251 }
3252
3253 void riscv_fill_dmi_nop_u64(struct target *target, char *buf)
3254 {
3255         RISCV_INFO(r);
3256         r->fill_dmi_nop_u64(target, buf);
3257 }
3258
3259 int riscv_dmi_write_u64_bits(struct target *target)
3260 {
3261         RISCV_INFO(r);
3262         return r->dmi_write_u64_bits(target);
3263 }
3264
3265 bool riscv_hart_enabled(struct target *target, int hartid)
3266 {
3267         /* FIXME: Add a hart mask to the RTOS. */
3268         if (riscv_rtos_enabled(target))
3269                 return hartid < riscv_count_harts(target);
3270
3271         return hartid == target->coreid;
3272 }
3273
3274 /**
3275  * Count triggers, and initialize trigger_count for each hart.
3276  * trigger_count is initialized even if this function fails to discover
3277  * something.
3278  * Disable any hardware triggers that have dmode set. We can't have set them
3279  * ourselves. Maybe they're left over from some killed debug session.
3280  * */
3281 int riscv_enumerate_triggers(struct target *target)
3282 {
3283         RISCV_INFO(r);
3284
3285         if (r->triggers_enumerated)
3286                 return ERROR_OK;
3287
3288         r->triggers_enumerated = true;  /* At the very least we tried. */
3289
3290         for (int hartid = 0; hartid < riscv_count_harts(target); ++hartid) {
3291                 if (!riscv_hart_enabled(target, hartid))
3292                         continue;
3293
3294                 riscv_reg_t tselect;
3295                 int result = riscv_get_register_on_hart(target, &tselect, hartid,
3296                                 GDB_REGNO_TSELECT);
3297                 if (result != ERROR_OK)
3298                         return result;
3299
3300                 for (unsigned t = 0; t < RISCV_MAX_TRIGGERS; ++t) {
3301                         r->trigger_count[hartid] = t;
3302
3303                         /* If we can't write tselect, then this hart does not support triggers. */
3304                         if (riscv_set_register_on_hart(target, hartid, GDB_REGNO_TSELECT, t) != ERROR_OK)
3305                                 break;
3306                         uint64_t tselect_rb;
3307                         result = riscv_get_register_on_hart(target, &tselect_rb, hartid,
3308                                         GDB_REGNO_TSELECT);
3309                         if (result != ERROR_OK)
3310                                 return result;
3311                         /* Mask off the top bit, which is used as tdrmode in old
3312                          * implementations. */
3313                         tselect_rb &= ~(1ULL << (riscv_xlen(target)-1));
3314                         if (tselect_rb != t)
3315                                 break;
3316                         uint64_t tdata1;
3317                         result = riscv_get_register_on_hart(target, &tdata1, hartid,
3318                                         GDB_REGNO_TDATA1);
3319                         if (result != ERROR_OK)
3320                                 return result;
3321
3322                         int type = get_field(tdata1, MCONTROL_TYPE(riscv_xlen(target)));
3323                         if (type == 0)
3324                                 break;
3325                         switch (type) {
3326                                 case 1:
3327                                         /* On these older cores we don't support software using
3328                                          * triggers. */
3329                                         riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA1, 0);
3330                                         break;
3331                                 case 2:
3332                                         if (tdata1 & MCONTROL_DMODE(riscv_xlen(target)))
3333                                                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TDATA1, 0);
3334                                         break;
3335                         }
3336                 }
3337
3338                 riscv_set_register_on_hart(target, hartid, GDB_REGNO_TSELECT, tselect);
3339
3340                 LOG_INFO("[%d] Found %d triggers", hartid, r->trigger_count[hartid]);
3341         }
3342
3343         return ERROR_OK;
3344 }
3345
3346 const char *gdb_regno_name(enum gdb_regno regno)
3347 {
3348         static char buf[32];
3349
3350         switch (regno) {
3351                 case GDB_REGNO_ZERO:
3352                         return "zero";
3353                 case GDB_REGNO_RA:
3354                         return "ra";
3355                 case GDB_REGNO_SP:
3356                         return "sp";
3357                 case GDB_REGNO_GP:
3358                         return "gp";
3359                 case GDB_REGNO_TP:
3360                         return "tp";
3361                 case GDB_REGNO_T0:
3362                         return "t0";
3363                 case GDB_REGNO_T1:
3364                         return "t1";
3365                 case GDB_REGNO_T2:
3366                         return "t2";
3367                 case GDB_REGNO_S0:
3368                         return "s0";
3369                 case GDB_REGNO_S1:
3370                         return "s1";
3371                 case GDB_REGNO_A0:
3372                         return "a0";
3373                 case GDB_REGNO_A1:
3374                         return "a1";
3375                 case GDB_REGNO_A2:
3376                         return "a2";
3377                 case GDB_REGNO_A3:
3378                         return "a3";
3379                 case GDB_REGNO_A4:
3380                         return "a4";
3381                 case GDB_REGNO_A5:
3382                         return "a5";
3383                 case GDB_REGNO_A6:
3384                         return "a6";
3385                 case GDB_REGNO_A7:
3386                         return "a7";
3387                 case GDB_REGNO_S2:
3388                         return "s2";
3389                 case GDB_REGNO_S3:
3390                         return "s3";
3391                 case GDB_REGNO_S4:
3392                         return "s4";
3393                 case GDB_REGNO_S5:
3394                         return "s5";
3395                 case GDB_REGNO_S6:
3396                         return "s6";
3397                 case GDB_REGNO_S7:
3398                         return "s7";
3399                 case GDB_REGNO_S8:
3400                         return "s8";
3401                 case GDB_REGNO_S9:
3402                         return "s9";
3403                 case GDB_REGNO_S10:
3404                         return "s10";
3405                 case GDB_REGNO_S11:
3406                         return "s11";
3407                 case GDB_REGNO_T3:
3408                         return "t3";
3409                 case GDB_REGNO_T4:
3410                         return "t4";
3411                 case GDB_REGNO_T5:
3412                         return "t5";
3413                 case GDB_REGNO_T6:
3414                         return "t6";
3415                 case GDB_REGNO_PC:
3416                         return "pc";
3417                 case GDB_REGNO_FPR0:
3418                         return "fpr0";
3419                 case GDB_REGNO_FPR31:
3420                         return "fpr31";
3421                 case GDB_REGNO_CSR0:
3422                         return "csr0";
3423                 case GDB_REGNO_TSELECT:
3424                         return "tselect";
3425                 case GDB_REGNO_TDATA1:
3426                         return "tdata1";
3427                 case GDB_REGNO_TDATA2:
3428                         return "tdata2";
3429                 case GDB_REGNO_MISA:
3430                         return "misa";
3431                 case GDB_REGNO_DPC:
3432                         return "dpc";
3433                 case GDB_REGNO_DCSR:
3434                         return "dcsr";
3435                 case GDB_REGNO_DSCRATCH0:
3436                         return "dscratch0";
3437                 case GDB_REGNO_MSTATUS:
3438                         return "mstatus";
3439                 case GDB_REGNO_MEPC:
3440                         return "mepc";
3441                 case GDB_REGNO_MCAUSE:
3442                         return "mcause";
3443                 case GDB_REGNO_PRIV:
3444                         return "priv";
3445                 case GDB_REGNO_SATP:
3446                         return "satp";
3447                 case GDB_REGNO_VTYPE:
3448                         return "vtype";
3449                 case GDB_REGNO_VL:
3450                         return "vl";
3451                 case GDB_REGNO_V0:
3452                         return "v0";
3453                 case GDB_REGNO_V1:
3454                         return "v1";
3455                 case GDB_REGNO_V2:
3456                         return "v2";
3457                 case GDB_REGNO_V3:
3458                         return "v3";
3459                 case GDB_REGNO_V4:
3460                         return "v4";
3461                 case GDB_REGNO_V5:
3462                         return "v5";
3463                 case GDB_REGNO_V6:
3464                         return "v6";
3465                 case GDB_REGNO_V7:
3466                         return "v7";
3467                 case GDB_REGNO_V8:
3468                         return "v8";
3469                 case GDB_REGNO_V9:
3470                         return "v9";
3471                 case GDB_REGNO_V10:
3472                         return "v10";
3473                 case GDB_REGNO_V11:
3474                         return "v11";
3475                 case GDB_REGNO_V12:
3476                         return "v12";
3477                 case GDB_REGNO_V13:
3478                         return "v13";
3479                 case GDB_REGNO_V14:
3480                         return "v14";
3481                 case GDB_REGNO_V15:
3482                         return "v15";
3483                 case GDB_REGNO_V16:
3484                         return "v16";
3485                 case GDB_REGNO_V17:
3486                         return "v17";
3487                 case GDB_REGNO_V18:
3488                         return "v18";
3489                 case GDB_REGNO_V19:
3490                         return "v19";
3491                 case GDB_REGNO_V20:
3492                         return "v20";
3493                 case GDB_REGNO_V21:
3494                         return "v21";
3495                 case GDB_REGNO_V22:
3496                         return "v22";
3497                 case GDB_REGNO_V23:
3498                         return "v23";
3499                 case GDB_REGNO_V24:
3500                         return "v24";
3501                 case GDB_REGNO_V25:
3502                         return "v25";
3503                 case GDB_REGNO_V26:
3504                         return "v26";
3505                 case GDB_REGNO_V27:
3506                         return "v27";
3507                 case GDB_REGNO_V28:
3508                         return "v28";
3509                 case GDB_REGNO_V29:
3510                         return "v29";
3511                 case GDB_REGNO_V30:
3512                         return "v30";
3513                 case GDB_REGNO_V31:
3514                         return "v31";
3515                 default:
3516                         if (regno <= GDB_REGNO_XPR31)
3517                                 sprintf(buf, "x%d", regno - GDB_REGNO_ZERO);
3518                         else if (regno >= GDB_REGNO_CSR0 && regno <= GDB_REGNO_CSR4095)
3519                                 sprintf(buf, "csr%d", regno - GDB_REGNO_CSR0);
3520                         else if (regno >= GDB_REGNO_FPR0 && regno <= GDB_REGNO_FPR31)
3521                                 sprintf(buf, "f%d", regno - GDB_REGNO_FPR0);
3522                         else
3523                                 sprintf(buf, "gdb_regno_%d", regno);
3524                         return buf;
3525         }
3526 }
3527
3528 static int register_get(struct reg *reg)
3529 {
3530         riscv_reg_info_t *reg_info = reg->arch_info;
3531         struct target *target = reg_info->target;
3532         RISCV_INFO(r);
3533
3534         if (reg->number >= GDB_REGNO_V0 && reg->number <= GDB_REGNO_V31) {
3535                 if (!r->get_register_buf) {
3536                         LOG_ERROR("Reading register %s not supported on this RISC-V target.",
3537                                         gdb_regno_name(reg->number));
3538                         return ERROR_FAIL;
3539                 }
3540
3541                 if (r->get_register_buf(target, reg->value, reg->number) != ERROR_OK)
3542                         return ERROR_FAIL;
3543         } else {
3544                 uint64_t value;
3545                 int result = riscv_get_register(target, &value, reg->number);
3546                 if (result != ERROR_OK)
3547                         return result;
3548                 buf_set_u64(reg->value, 0, reg->size, value);
3549         }
3550         reg->valid = gdb_regno_cacheable(reg->number, false);
3551         char *str = buf_to_hex_str(reg->value, reg->size);
3552         LOG_DEBUG("[%d]{%d} read 0x%s from %s (valid=%d)", target->coreid,
3553                         riscv_current_hartid(target), str, reg->name, reg->valid);
3554         free(str);
3555         return ERROR_OK;
3556 }
3557
3558 static int register_set(struct reg *reg, uint8_t *buf)
3559 {
3560         riscv_reg_info_t *reg_info = reg->arch_info;
3561         struct target *target = reg_info->target;
3562         RISCV_INFO(r);
3563
3564         char *str = buf_to_hex_str(buf, reg->size);
3565         LOG_DEBUG("[%d]{%d} write 0x%s to %s (valid=%d)", target->coreid,
3566                         riscv_current_hartid(target), str, reg->name, reg->valid);
3567         free(str);
3568
3569         memcpy(reg->value, buf, DIV_ROUND_UP(reg->size, 8));
3570         reg->valid = gdb_regno_cacheable(reg->number, true);
3571
3572         if (reg->number == GDB_REGNO_TDATA1 ||
3573                         reg->number == GDB_REGNO_TDATA2) {
3574                 r->manual_hwbp_set = true;
3575                 /* When enumerating triggers, we clear any triggers with DMODE set,
3576                  * assuming they were left over from a previous debug session. So make
3577                  * sure that is done before a user might be setting their own triggers.
3578                  */
3579                 if (riscv_enumerate_triggers(target) != ERROR_OK)
3580                         return ERROR_FAIL;
3581         }
3582
3583         if (reg->number >= GDB_REGNO_V0 && reg->number <= GDB_REGNO_V31) {
3584                 if (!r->set_register_buf) {
3585                         LOG_ERROR("Writing register %s not supported on this RISC-V target.",
3586                                         gdb_regno_name(reg->number));
3587                         return ERROR_FAIL;
3588                 }
3589
3590                 if (r->set_register_buf(target, reg->number, reg->value) != ERROR_OK)
3591                         return ERROR_FAIL;
3592         } else {
3593                 uint64_t value = buf_get_u64(buf, 0, reg->size);
3594                 if (riscv_set_register(target, reg->number, value) != ERROR_OK)
3595                         return ERROR_FAIL;
3596         }
3597
3598         return ERROR_OK;
3599 }
3600
3601 static struct reg_arch_type riscv_reg_arch_type = {
3602         .get = register_get,
3603         .set = register_set
3604 };
3605
3606 struct csr_info {
3607         unsigned number;
3608         const char *name;
3609 };
3610
3611 static int cmp_csr_info(const void *p1, const void *p2)
3612 {
3613         return (int) (((struct csr_info *)p1)->number) - (int) (((struct csr_info *)p2)->number);
3614 }
3615
3616 int riscv_init_registers(struct target *target)
3617 {
3618         RISCV_INFO(info);
3619
3620         riscv_free_registers(target);
3621
3622         target->reg_cache = calloc(1, sizeof(*target->reg_cache));
3623         if (!target->reg_cache)
3624                 return ERROR_FAIL;
3625         target->reg_cache->name = "RISC-V Registers";
3626         target->reg_cache->num_regs = GDB_REGNO_COUNT;
3627
3628         if (expose_custom) {
3629                 for (unsigned i = 0; expose_custom[i].low <= expose_custom[i].high; i++) {
3630                         for (unsigned number = expose_custom[i].low;
3631                                         number <= expose_custom[i].high;
3632                                         number++)
3633                                 target->reg_cache->num_regs++;
3634                 }
3635         }
3636
3637         LOG_DEBUG("create register cache for %d registers",
3638                         target->reg_cache->num_regs);
3639
3640         target->reg_cache->reg_list =
3641                 calloc(target->reg_cache->num_regs, sizeof(struct reg));
3642         if (!target->reg_cache->reg_list)
3643                 return ERROR_FAIL;
3644
3645         const unsigned int max_reg_name_len = 12;
3646         free(info->reg_names);
3647         info->reg_names =
3648                 calloc(target->reg_cache->num_regs, max_reg_name_len);
3649         if (!info->reg_names)
3650                 return ERROR_FAIL;
3651         char *reg_name = info->reg_names;
3652
3653         int hartid = riscv_current_hartid(target);
3654
3655         static struct reg_feature feature_cpu = {
3656                 .name = "org.gnu.gdb.riscv.cpu"
3657         };
3658         static struct reg_feature feature_fpu = {
3659                 .name = "org.gnu.gdb.riscv.fpu"
3660         };
3661         static struct reg_feature feature_csr = {
3662                 .name = "org.gnu.gdb.riscv.csr"
3663         };
3664         static struct reg_feature feature_vector = {
3665                 .name = "org.gnu.gdb.riscv.vector"
3666         };
3667         static struct reg_feature feature_virtual = {
3668                 .name = "org.gnu.gdb.riscv.virtual"
3669         };
3670         static struct reg_feature feature_custom = {
3671                 .name = "org.gnu.gdb.riscv.custom"
3672         };
3673
3674         /* These types are built into gdb. */
3675         static struct reg_data_type type_ieee_single = { .type = REG_TYPE_IEEE_SINGLE, .id = "ieee_single" };
3676         static struct reg_data_type type_ieee_double = { .type = REG_TYPE_IEEE_DOUBLE, .id = "ieee_double" };
3677         static struct reg_data_type_union_field single_double_fields[] = {
3678                 {"float", &type_ieee_single, single_double_fields + 1},
3679                 {"double", &type_ieee_double, NULL},
3680         };
3681         static struct reg_data_type_union single_double_union = {
3682                 .fields = single_double_fields
3683         };
3684         static struct reg_data_type type_ieee_single_double = {
3685                 .type = REG_TYPE_ARCH_DEFINED,
3686                 .id = "FPU_FD",
3687                 .type_class = REG_TYPE_CLASS_UNION,
3688                 .reg_type_union = &single_double_union
3689         };
3690         static struct reg_data_type type_uint8 = { .type = REG_TYPE_UINT8, .id = "uint8" };
3691         static struct reg_data_type type_uint16 = { .type = REG_TYPE_UINT16, .id = "uint16" };
3692         static struct reg_data_type type_uint32 = { .type = REG_TYPE_UINT32, .id = "uint32" };
3693         static struct reg_data_type type_uint64 = { .type = REG_TYPE_UINT64, .id = "uint64" };
3694         static struct reg_data_type type_uint128 = { .type = REG_TYPE_UINT128, .id = "uint128" };
3695
3696         /* This is roughly the XML we want:
3697          * <vector id="bytes" type="uint8" count="16"/>
3698          * <vector id="shorts" type="uint16" count="8"/>
3699          * <vector id="words" type="uint32" count="4"/>
3700          * <vector id="longs" type="uint64" count="2"/>
3701          * <vector id="quads" type="uint128" count="1"/>
3702          * <union id="riscv_vector_type">
3703          *   <field name="b" type="bytes"/>
3704          *   <field name="s" type="shorts"/>
3705          *   <field name="w" type="words"/>
3706          *   <field name="l" type="longs"/>
3707          *   <field name="q" type="quads"/>
3708          * </union>
3709          */
3710
3711         info->vector_uint8.type = &type_uint8;
3712         info->vector_uint8.count = info->vlenb[hartid];
3713         info->type_uint8_vector.type = REG_TYPE_ARCH_DEFINED;
3714         info->type_uint8_vector.id = "bytes";
3715         info->type_uint8_vector.type_class = REG_TYPE_CLASS_VECTOR;
3716         info->type_uint8_vector.reg_type_vector = &info->vector_uint8;
3717
3718         info->vector_uint16.type = &type_uint16;
3719         info->vector_uint16.count = info->vlenb[hartid] / 2;
3720         info->type_uint16_vector.type = REG_TYPE_ARCH_DEFINED;
3721         info->type_uint16_vector.id = "shorts";
3722         info->type_uint16_vector.type_class = REG_TYPE_CLASS_VECTOR;
3723         info->type_uint16_vector.reg_type_vector = &info->vector_uint16;
3724
3725         info->vector_uint32.type = &type_uint32;
3726         info->vector_uint32.count = info->vlenb[hartid] / 4;
3727         info->type_uint32_vector.type = REG_TYPE_ARCH_DEFINED;
3728         info->type_uint32_vector.id = "words";
3729         info->type_uint32_vector.type_class = REG_TYPE_CLASS_VECTOR;
3730         info->type_uint32_vector.reg_type_vector = &info->vector_uint32;
3731
3732         info->vector_uint64.type = &type_uint64;
3733         info->vector_uint64.count = info->vlenb[hartid] / 8;
3734         info->type_uint64_vector.type = REG_TYPE_ARCH_DEFINED;
3735         info->type_uint64_vector.id = "longs";
3736         info->type_uint64_vector.type_class = REG_TYPE_CLASS_VECTOR;
3737         info->type_uint64_vector.reg_type_vector = &info->vector_uint64;
3738
3739         info->vector_uint128.type = &type_uint128;
3740         info->vector_uint128.count = info->vlenb[hartid] / 16;
3741         info->type_uint128_vector.type = REG_TYPE_ARCH_DEFINED;
3742         info->type_uint128_vector.id = "quads";
3743         info->type_uint128_vector.type_class = REG_TYPE_CLASS_VECTOR;
3744         info->type_uint128_vector.reg_type_vector = &info->vector_uint128;
3745
3746         info->vector_fields[0].name = "b";
3747         info->vector_fields[0].type = &info->type_uint8_vector;
3748         if (info->vlenb[hartid] >= 2) {
3749                 info->vector_fields[0].next = info->vector_fields + 1;
3750                 info->vector_fields[1].name = "s";
3751                 info->vector_fields[1].type = &info->type_uint16_vector;
3752         } else {
3753                 info->vector_fields[0].next = NULL;
3754         }
3755         if (info->vlenb[hartid] >= 4) {
3756                 info->vector_fields[1].next = info->vector_fields + 2;
3757                 info->vector_fields[2].name = "w";
3758                 info->vector_fields[2].type = &info->type_uint32_vector;
3759         } else {
3760                 info->vector_fields[1].next = NULL;
3761         }
3762         if (info->vlenb[hartid] >= 8) {
3763                 info->vector_fields[2].next = info->vector_fields + 3;
3764                 info->vector_fields[3].name = "l";
3765                 info->vector_fields[3].type = &info->type_uint64_vector;
3766         } else {
3767                 info->vector_fields[2].next = NULL;
3768         }
3769         if (info->vlenb[hartid] >= 16) {
3770                 info->vector_fields[3].next = info->vector_fields + 4;
3771                 info->vector_fields[4].name = "q";
3772                 info->vector_fields[4].type = &info->type_uint128_vector;
3773         } else {
3774                 info->vector_fields[3].next = NULL;
3775         }
3776         info->vector_fields[4].next = NULL;
3777
3778         info->vector_union.fields = info->vector_fields;
3779
3780         info->type_vector.type = REG_TYPE_ARCH_DEFINED;
3781         info->type_vector.id = "riscv_vector";
3782         info->type_vector.type_class = REG_TYPE_CLASS_UNION;
3783         info->type_vector.reg_type_union = &info->vector_union;
3784
3785         struct csr_info csr_info[] = {
3786 #define DECLARE_CSR(name, number) { number, #name },
3787 #include "encoding.h"
3788 #undef DECLARE_CSR
3789         };
3790         /* encoding.h does not contain the registers in sorted order. */
3791         qsort(csr_info, ARRAY_SIZE(csr_info), sizeof(*csr_info), cmp_csr_info);
3792         unsigned csr_info_index = 0;
3793
3794         unsigned custom_range_index = 0;
3795         int custom_within_range = 0;
3796
3797         riscv_reg_info_t *shared_reg_info = calloc(1, sizeof(riscv_reg_info_t));
3798         if (!shared_reg_info)
3799                 return ERROR_FAIL;
3800         shared_reg_info->target = target;
3801
3802         /* When gdb requests register N, gdb_get_register_packet() assumes that this
3803          * is register at index N in reg_list. So if there are certain registers
3804          * that don't exist, we need to leave holes in the list (or renumber, but
3805          * it would be nice not to have yet another set of numbers to translate
3806          * between). */
3807         for (uint32_t number = 0; number < target->reg_cache->num_regs; number++) {
3808                 struct reg *r = &target->reg_cache->reg_list[number];
3809                 r->dirty = false;
3810                 r->valid = false;
3811                 r->exist = true;
3812                 r->type = &riscv_reg_arch_type;
3813                 r->arch_info = shared_reg_info;
3814                 r->number = number;
3815                 r->size = riscv_xlen(target);
3816                 /* r->size is set in riscv_invalidate_register_cache, maybe because the
3817                  * target is in theory allowed to change XLEN on us. But I expect a lot
3818                  * of other things to break in that case as well. */
3819                 if (number <= GDB_REGNO_XPR31) {
3820                         r->exist = number <= GDB_REGNO_XPR15 ||
3821                                 !riscv_supports_extension(target, hartid, 'E');
3822                         /* TODO: For now we fake that all GPRs exist because otherwise gdb
3823                          * doesn't work. */
3824                         r->exist = true;
3825                         r->caller_save = true;
3826                         switch (number) {
3827                                 case GDB_REGNO_ZERO:
3828                                         r->name = "zero";
3829                                         break;
3830                                 case GDB_REGNO_RA:
3831                                         r->name = "ra";
3832                                         break;
3833                                 case GDB_REGNO_SP:
3834                                         r->name = "sp";
3835                                         break;
3836                                 case GDB_REGNO_GP:
3837                                         r->name = "gp";
3838                                         break;
3839                                 case GDB_REGNO_TP:
3840                                         r->name = "tp";
3841                                         break;
3842                                 case GDB_REGNO_T0:
3843                                         r->name = "t0";
3844                                         break;
3845                                 case GDB_REGNO_T1:
3846                                         r->name = "t1";
3847                                         break;
3848                                 case GDB_REGNO_T2:
3849                                         r->name = "t2";
3850                                         break;
3851                                 case GDB_REGNO_FP:
3852                                         r->name = "fp";
3853                                         break;
3854                                 case GDB_REGNO_S1:
3855                                         r->name = "s1";
3856                                         break;
3857                                 case GDB_REGNO_A0:
3858                                         r->name = "a0";
3859                                         break;
3860                                 case GDB_REGNO_A1:
3861                                         r->name = "a1";
3862                                         break;
3863                                 case GDB_REGNO_A2:
3864                                         r->name = "a2";
3865                                         break;
3866                                 case GDB_REGNO_A3:
3867                                         r->name = "a3";
3868                                         break;
3869                                 case GDB_REGNO_A4:
3870                                         r->name = "a4";
3871                                         break;
3872                                 case GDB_REGNO_A5:
3873                                         r->name = "a5";
3874                                         break;
3875                                 case GDB_REGNO_A6:
3876                                         r->name = "a6";
3877                                         break;
3878                                 case GDB_REGNO_A7:
3879                                         r->name = "a7";
3880                                         break;
3881                                 case GDB_REGNO_S2:
3882                                         r->name = "s2";
3883                                         break;
3884                                 case GDB_REGNO_S3:
3885                                         r->name = "s3";
3886                                         break;
3887                                 case GDB_REGNO_S4:
3888                                         r->name = "s4";
3889                                         break;
3890                                 case GDB_REGNO_S5:
3891                                         r->name = "s5";
3892                                         break;
3893                                 case GDB_REGNO_S6:
3894                                         r->name = "s6";
3895                                         break;
3896                                 case GDB_REGNO_S7:
3897                                         r->name = "s7";
3898                                         break;
3899                                 case GDB_REGNO_S8:
3900                                         r->name = "s8";
3901                                         break;
3902                                 case GDB_REGNO_S9:
3903                                         r->name = "s9";
3904                                         break;
3905                                 case GDB_REGNO_S10:
3906                                         r->name = "s10";
3907                                         break;
3908                                 case GDB_REGNO_S11:
3909                                         r->name = "s11";
3910                                         break;
3911                                 case GDB_REGNO_T3:
3912                                         r->name = "t3";
3913                                         break;
3914                                 case GDB_REGNO_T4:
3915                                         r->name = "t4";
3916                                         break;
3917                                 case GDB_REGNO_T5:
3918                                         r->name = "t5";
3919                                         break;
3920                                 case GDB_REGNO_T6:
3921                                         r->name = "t6";
3922                                         break;
3923                         }
3924                         r->group = "general";
3925                         r->feature = &feature_cpu;
3926                 } else if (number == GDB_REGNO_PC) {
3927                         r->caller_save = true;
3928                         sprintf(reg_name, "pc");
3929                         r->group = "general";
3930                         r->feature = &feature_cpu;
3931                 } else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
3932                         r->caller_save = true;
3933                         if (riscv_supports_extension(target, hartid, 'D')) {
3934                                 r->size = 64;
3935                                 if (riscv_supports_extension(target, hartid, 'F'))
3936                                         r->reg_data_type = &type_ieee_single_double;
3937                                 else
3938                                         r->reg_data_type = &type_ieee_double;
3939                         } else if (riscv_supports_extension(target, hartid, 'F')) {
3940                                 r->reg_data_type = &type_ieee_single;
3941                                 r->size = 32;
3942                         } else {
3943                                 r->exist = false;
3944                         }
3945                         switch (number) {
3946                                 case GDB_REGNO_FT0:
3947                                         r->name = "ft0";
3948                                         break;
3949                                 case GDB_REGNO_FT1:
3950                                         r->name = "ft1";
3951                                         break;
3952                                 case GDB_REGNO_FT2:
3953                                         r->name = "ft2";
3954                                         break;
3955                                 case GDB_REGNO_FT3:
3956                                         r->name = "ft3";
3957                                         break;
3958                                 case GDB_REGNO_FT4:
3959                                         r->name = "ft4";
3960                                         break;
3961                                 case GDB_REGNO_FT5:
3962                                         r->name = "ft5";
3963                                         break;
3964                                 case GDB_REGNO_FT6:
3965                                         r->name = "ft6";
3966                                         break;
3967                                 case GDB_REGNO_FT7:
3968                                         r->name = "ft7";
3969                                         break;
3970                                 case GDB_REGNO_FS0:
3971                                         r->name = "fs0";
3972                                         break;
3973                                 case GDB_REGNO_FS1:
3974                                         r->name = "fs1";
3975                                         break;
3976                                 case GDB_REGNO_FA0:
3977                                         r->name = "fa0";
3978                                         break;
3979                                 case GDB_REGNO_FA1:
3980                                         r->name = "fa1";
3981                                         break;
3982                                 case GDB_REGNO_FA2:
3983                                         r->name = "fa2";
3984                                         break;
3985                                 case GDB_REGNO_FA3:
3986                                         r->name = "fa3";
3987                                         break;
3988                                 case GDB_REGNO_FA4:
3989                                         r->name = "fa4";
3990                                         break;
3991                                 case GDB_REGNO_FA5:
3992                                         r->name = "fa5";
3993                                         break;
3994                                 case GDB_REGNO_FA6:
3995                                         r->name = "fa6";
3996                                         break;
3997                                 case GDB_REGNO_FA7:
3998                                         r->name = "fa7";
3999                                         break;
4000                                 case GDB_REGNO_FS2:
4001                                         r->name = "fs2";
4002                                         break;
4003                                 case GDB_REGNO_FS3:
4004                                         r->name = "fs3";
4005                                         break;
4006                                 case GDB_REGNO_FS4:
4007                                         r->name = "fs4";
4008                                         break;
4009                                 case GDB_REGNO_FS5:
4010                                         r->name = "fs5";
4011                                         break;
4012                                 case GDB_REGNO_FS6:
4013                                         r->name = "fs6";
4014                                         break;
4015                                 case GDB_REGNO_FS7:
4016                                         r->name = "fs7";
4017                                         break;
4018                                 case GDB_REGNO_FS8:
4019                                         r->name = "fs8";
4020                                         break;
4021                                 case GDB_REGNO_FS9:
4022                                         r->name = "fs9";
4023                                         break;
4024                                 case GDB_REGNO_FS10:
4025                                         r->name = "fs10";
4026                                         break;
4027                                 case GDB_REGNO_FS11:
4028                                         r->name = "fs11";
4029                                         break;
4030                                 case GDB_REGNO_FT8:
4031                                         r->name = "ft8";
4032                                         break;
4033                                 case GDB_REGNO_FT9:
4034                                         r->name = "ft9";
4035                                         break;
4036                                 case GDB_REGNO_FT10:
4037                                         r->name = "ft10";
4038                                         break;
4039                                 case GDB_REGNO_FT11:
4040                                         r->name = "ft11";
4041                                         break;
4042                         }
4043                         r->group = "float";
4044                         r->feature = &feature_fpu;
4045                 } else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
4046                         r->group = "csr";
4047                         r->feature = &feature_csr;
4048                         unsigned csr_number = number - GDB_REGNO_CSR0;
4049
4050                         while (csr_info[csr_info_index].number < csr_number &&
4051                                         csr_info_index < ARRAY_SIZE(csr_info) - 1) {
4052                                 csr_info_index++;
4053                         }
4054                         if (csr_info[csr_info_index].number == csr_number) {
4055                                 r->name = csr_info[csr_info_index].name;
4056                         } else {
4057                                 sprintf(reg_name, "csr%d", csr_number);
4058                                 /* Assume unnamed registers don't exist, unless we have some
4059                                  * configuration that tells us otherwise. That's important
4060                                  * because eg. Eclipse crashes if a target has too many
4061                                  * registers, and apparently has no way of only showing a
4062                                  * subset of registers in any case. */
4063                                 r->exist = false;
4064                         }
4065
4066                         switch (csr_number) {
4067                                 case CSR_FFLAGS:
4068                                 case CSR_FRM:
4069                                 case CSR_FCSR:
4070                                         r->exist = riscv_supports_extension(target, hartid, 'F');
4071                                         r->group = "float";
4072                                         r->feature = &feature_fpu;
4073                                         break;
4074                                 case CSR_SSTATUS:
4075                                 case CSR_STVEC:
4076                                 case CSR_SIP:
4077                                 case CSR_SIE:
4078                                 case CSR_SCOUNTEREN:
4079                                 case CSR_SSCRATCH:
4080                                 case CSR_SEPC:
4081                                 case CSR_SCAUSE:
4082                                 case CSR_STVAL:
4083                                 case CSR_SATP:
4084                                         r->exist = riscv_supports_extension(target, hartid, 'S');
4085                                         break;
4086                                 case CSR_MEDELEG:
4087                                 case CSR_MIDELEG:
4088                                         /* "In systems with only M-mode, or with both M-mode and
4089                                          * U-mode but without U-mode trap support, the medeleg and
4090                                          * mideleg registers should not exist." */
4091                                         r->exist = riscv_supports_extension(target, hartid, 'S') ||
4092                                                 riscv_supports_extension(target, hartid, 'N');
4093                                         break;
4094
4095                                 case CSR_PMPCFG1:
4096                                 case CSR_PMPCFG3:
4097                                 case CSR_CYCLEH:
4098                                 case CSR_TIMEH:
4099                                 case CSR_INSTRETH:
4100                                 case CSR_HPMCOUNTER3H:
4101                                 case CSR_HPMCOUNTER4H:
4102                                 case CSR_HPMCOUNTER5H:
4103                                 case CSR_HPMCOUNTER6H:
4104                                 case CSR_HPMCOUNTER7H:
4105                                 case CSR_HPMCOUNTER8H:
4106                                 case CSR_HPMCOUNTER9H:
4107                                 case CSR_HPMCOUNTER10H:
4108                                 case CSR_HPMCOUNTER11H:
4109                                 case CSR_HPMCOUNTER12H:
4110                                 case CSR_HPMCOUNTER13H:
4111                                 case CSR_HPMCOUNTER14H:
4112                                 case CSR_HPMCOUNTER15H:
4113                                 case CSR_HPMCOUNTER16H:
4114                                 case CSR_HPMCOUNTER17H:
4115                                 case CSR_HPMCOUNTER18H:
4116                                 case CSR_HPMCOUNTER19H:
4117                                 case CSR_HPMCOUNTER20H:
4118                                 case CSR_HPMCOUNTER21H:
4119                                 case CSR_HPMCOUNTER22H:
4120                                 case CSR_HPMCOUNTER23H:
4121                                 case CSR_HPMCOUNTER24H:
4122                                 case CSR_HPMCOUNTER25H:
4123                                 case CSR_HPMCOUNTER26H:
4124                                 case CSR_HPMCOUNTER27H:
4125                                 case CSR_HPMCOUNTER28H:
4126                                 case CSR_HPMCOUNTER29H:
4127                                 case CSR_HPMCOUNTER30H:
4128                                 case CSR_HPMCOUNTER31H:
4129                                 case CSR_MCYCLEH:
4130                                 case CSR_MINSTRETH:
4131                                 case CSR_MHPMCOUNTER3H:
4132                                 case CSR_MHPMCOUNTER4H:
4133                                 case CSR_MHPMCOUNTER5H:
4134                                 case CSR_MHPMCOUNTER6H:
4135                                 case CSR_MHPMCOUNTER7H:
4136                                 case CSR_MHPMCOUNTER8H:
4137                                 case CSR_MHPMCOUNTER9H:
4138                                 case CSR_MHPMCOUNTER10H:
4139                                 case CSR_MHPMCOUNTER11H:
4140                                 case CSR_MHPMCOUNTER12H:
4141                                 case CSR_MHPMCOUNTER13H:
4142                                 case CSR_MHPMCOUNTER14H:
4143                                 case CSR_MHPMCOUNTER15H:
4144                                 case CSR_MHPMCOUNTER16H:
4145                                 case CSR_MHPMCOUNTER17H:
4146                                 case CSR_MHPMCOUNTER18H:
4147                                 case CSR_MHPMCOUNTER19H:
4148                                 case CSR_MHPMCOUNTER20H:
4149                                 case CSR_MHPMCOUNTER21H:
4150                                 case CSR_MHPMCOUNTER22H:
4151                                 case CSR_MHPMCOUNTER23H:
4152                                 case CSR_MHPMCOUNTER24H:
4153                                 case CSR_MHPMCOUNTER25H:
4154                                 case CSR_MHPMCOUNTER26H:
4155                                 case CSR_MHPMCOUNTER27H:
4156                                 case CSR_MHPMCOUNTER28H:
4157                                 case CSR_MHPMCOUNTER29H:
4158                                 case CSR_MHPMCOUNTER30H:
4159                                 case CSR_MHPMCOUNTER31H:
4160                                         r->exist = riscv_xlen(target) == 32;
4161                                         break;
4162
4163                                 case CSR_VSTART:
4164                                 case CSR_VXSAT:
4165                                 case CSR_VXRM:
4166                                 case CSR_VL:
4167                                 case CSR_VTYPE:
4168                                 case CSR_VLENB:
4169                                         r->exist = riscv_supports_extension(target, hartid, 'V');
4170                                         break;
4171                         }
4172
4173                         if (!r->exist && expose_csr) {
4174                                 for (unsigned i = 0; expose_csr[i].low <= expose_csr[i].high; i++) {
4175                                         if (csr_number >= expose_csr[i].low && csr_number <= expose_csr[i].high) {
4176                                                 LOG_INFO("Exposing additional CSR %d", csr_number);
4177                                                 r->exist = true;
4178                                                 break;
4179                                         }
4180                                 }
4181                         }
4182
4183                 } else if (number == GDB_REGNO_PRIV) {
4184                         sprintf(reg_name, "priv");
4185                         r->group = "general";
4186                         r->feature = &feature_virtual;
4187                         r->size = 8;
4188
4189                 } else if (number >= GDB_REGNO_V0 && number <= GDB_REGNO_V31) {
4190                         r->caller_save = false;
4191                         r->exist = riscv_supports_extension(target, hartid, 'V') && info->vlenb[hartid];
4192                         r->size = info->vlenb[hartid] * 8;
4193                         sprintf(reg_name, "v%d", number - GDB_REGNO_V0);
4194                         r->group = "vector";
4195                         r->feature = &feature_vector;
4196                         r->reg_data_type = &info->type_vector;
4197
4198                 } else if (number >= GDB_REGNO_COUNT) {
4199                         /* Custom registers. */
4200                         assert(expose_custom);
4201
4202                         range_t *range = &expose_custom[custom_range_index];
4203                         assert(range->low <= range->high);
4204                         unsigned custom_number = range->low + custom_within_range;
4205
4206                         r->group = "custom";
4207                         r->feature = &feature_custom;
4208                         r->arch_info = calloc(1, sizeof(riscv_reg_info_t));
4209                         if (!r->arch_info)
4210                                 return ERROR_FAIL;
4211                         ((riscv_reg_info_t *) r->arch_info)->target = target;
4212                         ((riscv_reg_info_t *) r->arch_info)->custom_number = custom_number;
4213                         sprintf(reg_name, "custom%d", custom_number);
4214
4215                         custom_within_range++;
4216                         if (custom_within_range > range->high - range->low) {
4217                                 custom_within_range = 0;
4218                                 custom_range_index++;
4219                         }
4220                 }
4221
4222                 if (reg_name[0])
4223                         r->name = reg_name;
4224                 reg_name += strlen(reg_name) + 1;
4225                 assert(reg_name < info->reg_names + target->reg_cache->num_regs *
4226                                 max_reg_name_len);
4227                 r->value = info->reg_cache_values[number];
4228         }
4229
4230         return ERROR_OK;
4231 }
4232
4233
4234 void riscv_add_bscan_tunneled_scan(struct target *target, struct scan_field *field,
4235                                         riscv_bscan_tunneled_scan_context_t *ctxt)
4236 {
4237         jtag_add_ir_scan(target->tap, &select_user4, TAP_IDLE);
4238
4239         memset(ctxt->tunneled_dr, 0, sizeof(ctxt->tunneled_dr));
4240         if (bscan_tunnel_type == BSCAN_TUNNEL_DATA_REGISTER) {
4241                 ctxt->tunneled_dr[3].num_bits = 1;
4242                 ctxt->tunneled_dr[3].out_value = bscan_one;
4243                 ctxt->tunneled_dr[2].num_bits = 7;
4244                 ctxt->tunneled_dr_width = field->num_bits;
4245                 ctxt->tunneled_dr[2].out_value = &ctxt->tunneled_dr_width;
4246                 /* for BSCAN tunnel, there is a one-TCK skew between shift in and shift out, so
4247                    scanning num_bits + 1, and then will right shift the input field after executing the queues */
4248
4249                 ctxt->tunneled_dr[1].num_bits = field->num_bits + 1;
4250                 ctxt->tunneled_dr[1].out_value = field->out_value;
4251                 ctxt->tunneled_dr[1].in_value = field->in_value;
4252
4253                 ctxt->tunneled_dr[0].num_bits = 3;
4254                 ctxt->tunneled_dr[0].out_value = bscan_zero;
4255         } else {
4256                 /* BSCAN_TUNNEL_NESTED_TAP */
4257                 ctxt->tunneled_dr[0].num_bits = 1;
4258                 ctxt->tunneled_dr[0].out_value = bscan_one;
4259                 ctxt->tunneled_dr[1].num_bits = 7;
4260                 ctxt->tunneled_dr_width = field->num_bits;
4261                 ctxt->tunneled_dr[1].out_value = &ctxt->tunneled_dr_width;
4262                 /* for BSCAN tunnel, there is a one-TCK skew between shift in and shift out, so
4263                    scanning num_bits + 1, and then will right shift the input field after executing the queues */
4264                 ctxt->tunneled_dr[2].num_bits = field->num_bits + 1;
4265                 ctxt->tunneled_dr[2].out_value = field->out_value;
4266                 ctxt->tunneled_dr[2].in_value = field->in_value;
4267                 ctxt->tunneled_dr[3].num_bits = 3;
4268                 ctxt->tunneled_dr[3].out_value = bscan_zero;
4269         }
4270         jtag_add_dr_scan(target->tap, ARRAY_SIZE(ctxt->tunneled_dr), ctxt->tunneled_dr, TAP_IDLE);
4271 }