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