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