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

   1 /*
   2  * Support for RISC-V, debug version 0.13, which is currently (2/4/17) the
   3  * latest draft.
   4  */
   5
   6 #include <assert.h>
   7 #include <stdlib.h>
   8 #include <time.h>
   9
  10 #ifdef HAVE_CONFIG_H
  11 #include "config.h"
  12 #endif
  13
  14 #include "target/target.h"
  15 #include "target/algorithm.h"
  16 #include "target/target_type.h"
  17 #include "log.h"
  18 #include "jtag/jtag.h"
  19 #include "target/register.h"
  20 #include "target/breakpoints.h"
  21 #include "helper/time_support.h"
  22 #include "helper/list.h"
  23 #include "riscv.h"
  24 #include "debug_defines.h"
  25 #include "rtos/rtos.h"
  26 #include "program.h"
  27 #include "asm.h"
  28 #include "batch.h"
  29
  30 #define DMI_DATA1 (DMI_DATA0 + 1)
  31 #define DMI_PROGBUF1 (DMI_PROGBUF0 + 1)
  32
  33 static int riscv013_on_step_or_resume(struct target *target, bool step);
  34 static int riscv013_step_or_resume_current_hart(struct target *target, bool step);
  35 static void riscv013_clear_abstract_error(struct target *target);
  36
  37 /* Implementations of the functions in riscv_info_t. */
  38 static int riscv013_get_register(struct target *target,
  39                 riscv_reg_t *value, int hid, int rid);
  40 static int riscv013_set_register(struct target *target, int hartid, int regid, uint64_t value);
  41 static int riscv013_select_current_hart(struct target *target);
  42 static int riscv013_halt_current_hart(struct target *target);
  43 static int riscv013_resume_current_hart(struct target *target);
  44 static int riscv013_step_current_hart(struct target *target);
  45 static int riscv013_on_halt(struct target *target);
  46 static int riscv013_on_step(struct target *target);
  47 static int riscv013_on_resume(struct target *target);
  48 static bool riscv013_is_halted(struct target *target);
  49 static enum riscv_halt_reason riscv013_halt_reason(struct target *target);
  50 static int riscv013_write_debug_buffer(struct target *target, unsigned index,
  51                 riscv_insn_t d);
  52 static riscv_insn_t riscv013_read_debug_buffer(struct target *target, unsigned
  53                 index);
  54 static int riscv013_execute_debug_buffer(struct target *target);
  55 static void riscv013_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d);
  56 static void riscv013_fill_dmi_read_u64(struct target *target, char *buf, int a);
  57 static int riscv013_dmi_write_u64_bits(struct target *target);
  58 static void riscv013_fill_dmi_nop_u64(struct target *target, char *buf);
  59 static int register_read(struct target *target, uint64_t *value, uint32_t number);
  60 static int register_read_direct(struct target *target, uint64_t *value, uint32_t number);
  61 static int register_write_direct(struct target *target, unsigned number,
  62                 uint64_t value);
  63 static int read_memory(struct target *target, target_addr_t address,
  64                 uint32_t size, uint32_t count, uint8_t *buffer);
  65 static int write_memory(struct target *target, target_addr_t address,
  66                 uint32_t size, uint32_t count, const uint8_t *buffer);
  67 static int riscv013_test_sba_config_reg(struct target *target, target_addr_t legal_address,
  68                 uint32_t num_words, target_addr_t illegal_address, bool run_sbbusyerror_test);
  69 void write_memory_sba_simple(struct target *target, target_addr_t addr, uint32_t *write_data,
  70                 uint32_t write_size, uint32_t sbcs);
  71 void read_memory_sba_simple(struct target *target, target_addr_t addr,
  72                 uint32_t *rd_buf, uint32_t read_size, uint32_t sbcs);
  73 static int      riscv013_test_compliance(struct target *target);
  74
  75 /**
  76  * Since almost everything can be accomplish by scanning the dbus register, all
  77  * functions here assume dbus is already selected. The exception are functions
  78  * called directly by OpenOCD, which can't assume anything about what's
  79  * currently in IR. They should set IR to dbus explicitly.
  80  */
  81
  82 #define get_field(reg, mask) (((reg) & (mask)) / ((mask) & ~((mask) << 1)))
  83 #define set_field(reg, mask, val) (((reg) & ~(mask)) | (((val) * ((mask) & ~((mask) << 1))) & (mask)))
  84
  85 #define DIM(x)          (sizeof(x)/sizeof(*x))
  86
  87 #define CSR_DCSR_CAUSE_SWBP             1
  88 #define CSR_DCSR_CAUSE_TRIGGER  2
  89 #define CSR_DCSR_CAUSE_DEBUGINT 3
  90 #define CSR_DCSR_CAUSE_STEP             4
  91 #define CSR_DCSR_CAUSE_HALT             5
  92
  93 #define RISCV013_INFO(r) riscv013_info_t *r = get_info(target)
  94
  95 /*** JTAG registers. ***/
  96
  97 typedef enum {
  98         DMI_OP_NOP = 0,
  99         DMI_OP_READ = 1,
 100         DMI_OP_WRITE = 2
 101 } dmi_op_t;
 102 typedef enum {
 103         DMI_STATUS_SUCCESS = 0,
 104         DMI_STATUS_FAILED = 2,
 105         DMI_STATUS_BUSY = 3
 106 } dmi_status_t;
 107
 108 typedef enum {
 109         RE_OK,
 110         RE_FAIL,
 111         RE_AGAIN
 112 } riscv_error_t;
 113
 114 typedef enum slot {
 115         SLOT0,
 116         SLOT1,
 117         SLOT_LAST,
 118 } slot_t;
 119
 120 /*** Debug Bus registers. ***/
 121
 122 #define CMDERR_NONE                             0
 123 #define CMDERR_BUSY                             1
 124 #define CMDERR_NOT_SUPPORTED    2
 125 #define CMDERR_EXCEPTION                3
 126 #define CMDERR_HALT_RESUME              4
 127 #define CMDERR_OTHER                    7
 128
 129 /*** Info about the core being debugged. ***/
 130
 131 struct trigger {
 132         uint64_t address;
 133         uint32_t length;
 134         uint64_t mask;
 135         uint64_t value;
 136         bool read, write, execute;
 137         int unique_id;
 138 };
 139
 140 typedef enum {
 141         YNM_MAYBE,
 142         YNM_YES,
 143         YNM_NO
 144 } yes_no_maybe_t;
 145
 146 typedef struct {
 147         struct list_head list;
 148         int abs_chain_position;
 149         /* Indicates we already reset this DM, so don't need to do it again. */
 150         bool was_reset;
 151         /* Targets that are connected to this DM. */
 152         struct list_head target_list;
 153         /* The currently selected hartid on this DM. */
 154         int current_hartid;
 155 } dm013_info_t;
 156
 157 typedef struct {
 158         struct list_head list;
 159         struct target *target;
 160 } target_list_t;
 161
 162 typedef struct {
 163         /* Number of address bits in the dbus register. */
 164         unsigned abits;
 165         /* Number of abstract command data registers. */
 166         unsigned datacount;
 167         /* Number of words in the Program Buffer. */
 168         unsigned progbufsize;
 169
 170         /* We cache the read-only bits of sbcs here. */
 171         uint32_t sbcs;
 172
 173         yes_no_maybe_t progbuf_writable;
 174         /* We only need the address so that we know the alignment of the buffer. */
 175         riscv_addr_t progbuf_address;
 176
 177         /* Number of run-test/idle cycles the target requests we do after each dbus
 178          * access. */
 179         unsigned int dtmcs_idle;
 180
 181         /* This value is incremented every time a dbus access comes back as "busy".
 182          * It's used to determine how many run-test/idle cycles to feed the target
 183          * in between accesses. */
 184         unsigned int dmi_busy_delay;
 185
 186         /* Number of run-test/idle cycles to add between consecutive bus master
 187          * reads/writes respectively. */
 188         unsigned int bus_master_write_delay, bus_master_read_delay;
 189
 190         /* This value is increased every time we tried to execute two commands
 191          * consecutively, and the second one failed because the previous hadn't
 192          * completed yet.  It's used to add extra run-test/idle cycles after
 193          * starting a command, so we don't have to waste time checking for busy to
 194          * go low. */
 195         unsigned int ac_busy_delay;
 196
 197         bool abstract_read_csr_supported;
 198         bool abstract_write_csr_supported;
 199         bool abstract_read_fpr_supported;
 200         bool abstract_write_fpr_supported;
 201
 202         /* When a function returns some error due to a failure indicated by the
 203          * target in cmderr, the caller can look here to see what that error was.
 204          * (Compare with errno.) */
 205         uint8_t cmderr;
 206
 207         /* Some fields from hartinfo. */
 208         uint8_t datasize;
 209         uint8_t dataaccess;
 210         int16_t dataaddr;
 211
 212         /* The width of the hartsel field. */
 213         unsigned hartsellen;
 214
 215         /* DM that provides access to this target. */
 216         dm013_info_t *dm;
 217 } riscv013_info_t;
 218
 219 LIST_HEAD(dm_list);
 220
 221 static riscv013_info_t *get_info(const struct target *target)
 222 {
 223         riscv_info_t *info = (riscv_info_t *) target->arch_info;
 224         return (riscv013_info_t *) info->version_specific;
 225 }
 226
 227 /**
 228  * Return the DM structure for this target. If there isn't one, find it in the
 229  * global list of DMs. If it's not in there, then create one and initialize it
 230  * to 0.
 231  */
 232 static dm013_info_t *get_dm(struct target *target)
 233 {
 234         RISCV013_INFO(info);
 235         if (info->dm)
 236                 return info->dm;
 237
 238         int abs_chain_position = target->tap->abs_chain_position;
 239
 240         dm013_info_t *entry;
 241         dm013_info_t *dm = NULL;
 242         list_for_each_entry(entry, &dm_list, list) {
 243                 if (entry->abs_chain_position == abs_chain_position) {
 244                         dm = entry;
 245                         break;
 246                 }
 247         }
 248
 249         if (!dm) {
 250                 dm = calloc(1, sizeof(dm013_info_t));
 251                 dm->abs_chain_position = abs_chain_position;
 252                 dm->current_hartid = -1;
 253                 INIT_LIST_HEAD(&dm->target_list);
 254                 list_add(&dm->list, &dm_list);
 255         }
 256
 257         info->dm = dm;
 258         target_list_t *target_entry;
 259         list_for_each_entry(target_entry, &dm->target_list, list) {
 260                 if (target_entry->target == target)
 261                         return dm;
 262         }
 263         target_entry = calloc(1, sizeof(*target_entry));
 264         target_entry->target = target;
 265         list_add(&target_entry->list, &dm->target_list);
 266
 267         return dm;
 268 }
 269
 270 static uint32_t set_hartsel(uint32_t initial, uint32_t index)
 271 {
 272         initial &= ~DMI_DMCONTROL_HARTSELLO;
 273         initial &= ~DMI_DMCONTROL_HARTSELHI;
 274
 275         uint32_t index_lo = index & ((1 << DMI_DMCONTROL_HARTSELLO_LENGTH) - 1);
 276         initial |= index_lo << DMI_DMCONTROL_HARTSELLO_OFFSET;
 277         uint32_t index_hi = index >> DMI_DMCONTROL_HARTSELLO_LENGTH;
 278         assert(index_hi < 1 << DMI_DMCONTROL_HARTSELHI_LENGTH);
 279         initial |= index_hi << DMI_DMCONTROL_HARTSELHI_OFFSET;
 280
 281         return initial;
 282 }
 283
 284 static void decode_dmi(char *text, unsigned address, unsigned data)
 285 {
 286         static const struct {
 287                 unsigned address;
 288                 uint64_t mask;
 289                 const char *name;
 290         } description[] = {
 291                 { DMI_DMCONTROL, DMI_DMCONTROL_HALTREQ, "haltreq" },
 292                 { DMI_DMCONTROL, DMI_DMCONTROL_RESUMEREQ, "resumereq" },
 293                 { DMI_DMCONTROL, DMI_DMCONTROL_HARTRESET, "hartreset" },
 294                 { DMI_DMCONTROL, DMI_DMCONTROL_HASEL, "hasel" },
 295                 { DMI_DMCONTROL, DMI_DMCONTROL_HARTSELHI, "hartselhi" },
 296                 { DMI_DMCONTROL, DMI_DMCONTROL_HARTSELLO, "hartsello" },
 297                 { DMI_DMCONTROL, DMI_DMCONTROL_NDMRESET, "ndmreset" },
 298                 { DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE, "dmactive" },
 299                 { DMI_DMCONTROL, DMI_DMCONTROL_ACKHAVERESET, "ackhavereset" },
 300
 301                 { DMI_DMSTATUS, DMI_DMSTATUS_IMPEBREAK, "impebreak" },
 302                 { DMI_DMSTATUS, DMI_DMSTATUS_ALLHAVERESET, "allhavereset" },
 303                 { DMI_DMSTATUS, DMI_DMSTATUS_ANYHAVERESET, "anyhavereset" },
 304                 { DMI_DMSTATUS, DMI_DMSTATUS_ALLRESUMEACK, "allresumeack" },
 305                 { DMI_DMSTATUS, DMI_DMSTATUS_ANYRESUMEACK, "anyresumeack" },
 306                 { DMI_DMSTATUS, DMI_DMSTATUS_ALLNONEXISTENT, "allnonexistent" },
 307                 { DMI_DMSTATUS, DMI_DMSTATUS_ANYNONEXISTENT, "anynonexistent" },
 308                 { DMI_DMSTATUS, DMI_DMSTATUS_ALLUNAVAIL, "allunavail" },
 309                 { DMI_DMSTATUS, DMI_DMSTATUS_ANYUNAVAIL, "anyunavail" },
 310                 { DMI_DMSTATUS, DMI_DMSTATUS_ALLRUNNING, "allrunning" },
 311                 { DMI_DMSTATUS, DMI_DMSTATUS_ANYRUNNING, "anyrunning" },
 312                 { DMI_DMSTATUS, DMI_DMSTATUS_ALLHALTED, "allhalted" },
 313                 { DMI_DMSTATUS, DMI_DMSTATUS_ANYHALTED, "anyhalted" },
 314                 { DMI_DMSTATUS, DMI_DMSTATUS_AUTHENTICATED, "authenticated" },
 315                 { DMI_DMSTATUS, DMI_DMSTATUS_AUTHBUSY, "authbusy" },
 316                 { DMI_DMSTATUS, DMI_DMSTATUS_DEVTREEVALID, "devtreevalid" },
 317                 { DMI_DMSTATUS, DMI_DMSTATUS_VERSION, "version" },
 318
 319                 { DMI_ABSTRACTCS, DMI_ABSTRACTCS_PROGBUFSIZE, "progbufsize" },
 320                 { DMI_ABSTRACTCS, DMI_ABSTRACTCS_BUSY, "busy" },
 321                 { DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR, "cmderr" },
 322                 { DMI_ABSTRACTCS, DMI_ABSTRACTCS_DATACOUNT, "datacount" },
 323
 324                 { DMI_COMMAND, DMI_COMMAND_CMDTYPE, "cmdtype" },
 325
 326                 { DMI_SBCS, DMI_SBCS_SBREADONADDR, "sbreadonaddr" },
 327                 { DMI_SBCS, DMI_SBCS_SBACCESS, "sbaccess" },
 328                 { DMI_SBCS, DMI_SBCS_SBAUTOINCREMENT, "sbautoincrement" },
 329                 { DMI_SBCS, DMI_SBCS_SBREADONDATA, "sbreadondata" },
 330                 { DMI_SBCS, DMI_SBCS_SBERROR, "sberror" },
 331                 { DMI_SBCS, DMI_SBCS_SBASIZE, "sbasize" },
 332                 { DMI_SBCS, DMI_SBCS_SBACCESS128, "sbaccess128" },
 333                 { DMI_SBCS, DMI_SBCS_SBACCESS64, "sbaccess64" },
 334                 { DMI_SBCS, DMI_SBCS_SBACCESS32, "sbaccess32" },
 335                 { DMI_SBCS, DMI_SBCS_SBACCESS16, "sbaccess16" },
 336                 { DMI_SBCS, DMI_SBCS_SBACCESS8, "sbaccess8" },
 337         };
 338
 339         text[0] = 0;
 340         for (unsigned i = 0; i < DIM(description); i++) {
 341                 if (description[i].address == address) {
 342                         uint64_t mask = description[i].mask;
 343                         unsigned value = get_field(data, mask);
 344                         if (value) {
 345                                 if (i > 0)
 346                                         *(text++) = ' ';
 347                                 if (mask & (mask >> 1)) {
 348                                         /* If the field is more than 1 bit wide. */
 349                                         sprintf(text, "%s=%d", description[i].name, value);
 350                                 } else {
 351                                         strcpy(text, description[i].name);
 352                                 }
 353                                 text += strlen(text);
 354                         }
 355                 }
 356         }
 357 }
 358
 359 static void dump_field(int idle, const struct scan_field *field)
 360 {
 361         static const char * const op_string[] = {"-", "r", "w", "?"};
 362         static const char * const status_string[] = {"+", "?", "F", "b"};
 363
 364         if (debug_level < LOG_LVL_DEBUG)
 365                 return;
 366
 367         uint64_t out = buf_get_u64(field->out_value, 0, field->num_bits);
 368         unsigned int out_op = get_field(out, DTM_DMI_OP);
 369         unsigned int out_data = get_field(out, DTM_DMI_DATA);
 370         unsigned int out_address = out >> DTM_DMI_ADDRESS_OFFSET;
 371
 372         uint64_t in = buf_get_u64(field->in_value, 0, field->num_bits);
 373         unsigned int in_op = get_field(in, DTM_DMI_OP);
 374         unsigned int in_data = get_field(in, DTM_DMI_DATA);
 375         unsigned int in_address = in >> DTM_DMI_ADDRESS_OFFSET;
 376
 377         log_printf_lf(LOG_LVL_DEBUG,
 378                         __FILE__, __LINE__, "scan",
 379                         "%db %di %s %08x @%02x -> %s %08x @%02x",
 380                         field->num_bits, idle,
 381                         op_string[out_op], out_data, out_address,
 382                         status_string[in_op], in_data, in_address);
 383
 384         char out_text[500];
 385         char in_text[500];
 386         decode_dmi(out_text, out_address, out_data);
 387         decode_dmi(in_text, in_address, in_data);
 388         if (in_text[0] || out_text[0]) {
 389                 log_printf_lf(LOG_LVL_DEBUG, __FILE__, __LINE__, "scan", "%s -> %s",
 390                                 out_text, in_text);
 391         }
 392 }
 393
 394 /*** Utility functions. ***/
 395
 396 static void select_dmi(struct target *target)
 397 {
 398         jtag_add_ir_scan(target->tap, &select_dbus, TAP_IDLE);
 399 }
 400
 401 static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
 402 {
 403         struct scan_field field;
 404         uint8_t in_value[4];
 405         uint8_t out_value[4] = { 0 };
 406
 407         buf_set_u32(out_value, 0, 32, out);
 408
 409         jtag_add_ir_scan(target->tap, &select_dtmcontrol, TAP_IDLE);
 410
 411         field.num_bits = 32;
 412         field.out_value = out_value;
 413         field.in_value = in_value;
 414         jtag_add_dr_scan(target->tap, 1, &field, TAP_IDLE);
 415
 416         /* Always return to dmi. */
 417         select_dmi(target);
 418
 419         int retval = jtag_execute_queue();
 420         if (retval != ERROR_OK) {
 421                 LOG_ERROR("failed jtag scan: %d", retval);
 422                 return retval;
 423         }
 424
 425         uint32_t in = buf_get_u32(field.in_value, 0, 32);
 426         LOG_DEBUG("DTMCS: 0x%x -> 0x%x", out, in);
 427
 428         return in;
 429 }
 430
 431 static void increase_dmi_busy_delay(struct target *target)
 432 {
 433         riscv013_info_t *info = get_info(target);
 434         info->dmi_busy_delay += info->dmi_busy_delay / 10 + 1;
 435         LOG_DEBUG("dtmcs_idle=%d, dmi_busy_delay=%d, ac_busy_delay=%d",
 436                         info->dtmcs_idle, info->dmi_busy_delay,
 437                         info->ac_busy_delay);
 438
 439         dtmcontrol_scan(target, DTM_DTMCS_DMIRESET);
 440 }
 441
 442 /**
 443  * exec: If this is set, assume the scan results in an execution, so more
 444  * run-test/idle cycles may be required.
 445  */
 446 static dmi_status_t dmi_scan(struct target *target, uint32_t *address_in,
 447                 uint32_t *data_in, dmi_op_t op, uint32_t address_out, uint32_t data_out,
 448                 bool exec)
 449 {
 450         riscv013_info_t *info = get_info(target);
 451         RISCV_INFO(r);
 452         unsigned num_bits = info->abits + DTM_DMI_OP_LENGTH + DTM_DMI_DATA_LENGTH;
 453         size_t num_bytes = (num_bits + 7) / 8;
 454         uint8_t in[num_bytes];
 455         uint8_t out[num_bytes];
 456         struct scan_field field = {
 457                 .num_bits = num_bits,
 458                 .out_value = out,
 459                 .in_value = in
 460         };
 461
 462         if (r->reset_delays_wait >= 0) {
 463                 r->reset_delays_wait--;
 464                 if (r->reset_delays_wait < 0) {
 465                         info->dmi_busy_delay = 0;
 466                         info->ac_busy_delay = 0;
 467                 }
 468         }
 469
 470         memset(in, 0, num_bytes);
 471         memset(out, 0, num_bytes);
 472
 473         assert(info->abits != 0);
 474
 475         buf_set_u32(out, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, op);
 476         buf_set_u32(out, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, data_out);
 477         buf_set_u32(out, DTM_DMI_ADDRESS_OFFSET, info->abits, address_out);
 478
 479         /* Assume dbus is already selected. */
 480         jtag_add_dr_scan(target->tap, 1, &field, TAP_IDLE);
 481
 482         int idle_count = info->dmi_busy_delay;
 483         if (exec)
 484                 idle_count += info->ac_busy_delay;
 485
 486         if (idle_count)
 487                 jtag_add_runtest(idle_count, TAP_IDLE);
 488
 489         int retval = jtag_execute_queue();
 490         if (retval != ERROR_OK) {
 491                 LOG_ERROR("dmi_scan failed jtag scan");
 492                 return DMI_STATUS_FAILED;
 493         }
 494
 495         if (data_in)
 496                 *data_in = buf_get_u32(in, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH);
 497
 498         if (address_in)
 499                 *address_in = buf_get_u32(in, DTM_DMI_ADDRESS_OFFSET, info->abits);
 500
 501         dump_field(idle_count, &field);
 502
 503         return buf_get_u32(in, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH);
 504 }
 505
 506 /* If dmi_busy_encountered is non-NULL, this function will use it to tell the
 507  * caller whether DMI was ever busy during this call. */
 508 static int dmi_op_timeout(struct target *target, uint32_t *data_in,
 509                 bool *dmi_busy_encountered, int dmi_op, uint32_t address,
 510                 uint32_t data_out, int timeout_sec, bool exec)
 511 {
 512         select_dmi(target);
 513
 514         dmi_status_t status;
 515         uint32_t address_in;
 516
 517         if (dmi_busy_encountered)
 518                 *dmi_busy_encountered = false;
 519
 520         const char *op_name;
 521         switch (dmi_op) {
 522                 case DMI_OP_NOP:
 523                         op_name = "nop";
 524                         break;
 525                 case DMI_OP_READ:
 526                         op_name = "read";
 527                         break;
 528                 case DMI_OP_WRITE:
 529                         op_name = "write";
 530                         break;
 531                 default:
 532                         LOG_ERROR("Invalid DMI operation: %d", dmi_op);
 533                         return ERROR_FAIL;
 534         }
 535
 536         time_t start = time(NULL);
 537         /* This first loop performs the request.  Note that if for some reason this
 538          * stays busy, it is actually due to the previous access. */
 539         while (1) {
 540                 status = dmi_scan(target, NULL, NULL, dmi_op, address, data_out,
 541                                 exec);
 542                 if (status == DMI_STATUS_BUSY) {
 543                         increase_dmi_busy_delay(target);
 544                         if (dmi_busy_encountered)
 545                                 *dmi_busy_encountered = true;
 546                 } else if (status == DMI_STATUS_SUCCESS) {
 547                         break;
 548                 } else {
 549                         LOG_ERROR("failed %s at 0x%x, status=%d", op_name, address, status);
 550                         return ERROR_FAIL;
 551                 }
 552                 if (time(NULL) - start > timeout_sec)
 553                         return ERROR_TIMEOUT_REACHED;
 554         }
 555
 556         if (status != DMI_STATUS_SUCCESS) {
 557                 LOG_ERROR("Failed %s at 0x%x; status=%d", op_name, address, status);
 558                 return ERROR_FAIL;
 559         }
 560
 561         /* This second loop ensures the request succeeded, and gets back data.
 562          * Note that NOP can result in a 'busy' result as well, but that would be
 563          * noticed on the next DMI access we do. */
 564         while (1) {
 565                 status = dmi_scan(target, &address_in, data_in, DMI_OP_NOP, address, 0,
 566                                 false);
 567                 if (status == DMI_STATUS_BUSY) {
 568                         increase_dmi_busy_delay(target);
 569                 } else if (status == DMI_STATUS_SUCCESS) {
 570                         break;
 571                 } else {
 572                         LOG_ERROR("failed %s (NOP) at 0x%x, status=%d", op_name, address,
 573                                         status);
 574                         return ERROR_FAIL;
 575                 }
 576                 if (time(NULL) - start > timeout_sec)
 577                         return ERROR_TIMEOUT_REACHED;
 578         }
 579
 580         if (status != DMI_STATUS_SUCCESS) {
 581                 if (status == DMI_STATUS_FAILED || !data_in) {
 582                         LOG_ERROR("Failed %s (NOP) at 0x%x; status=%d", op_name, address,
 583                                         status);
 584                 } else {
 585                         LOG_ERROR("Failed %s (NOP) at 0x%x; value=0x%x, status=%d",
 586                                         op_name, address, *data_in, status);
 587                 }
 588                 return ERROR_FAIL;
 589         }
 590
 591         return ERROR_OK;
 592 }
 593
 594 static int dmi_op(struct target *target, uint32_t *data_in,
 595                 bool *dmi_busy_encountered, int dmi_op, uint32_t address,
 596                 uint32_t data_out, bool exec)
 597 {
 598         int result = dmi_op_timeout(target, data_in, dmi_busy_encountered, dmi_op,
 599                         address, data_out, riscv_command_timeout_sec, exec);
 600         if (result == ERROR_TIMEOUT_REACHED) {
 601                 LOG_ERROR("DMI operation didn't complete in %d seconds. The target is "
 602                                 "either really slow or broken. You could increase the "
 603                                 "timeout with riscv set_command_timeout_sec.",
 604                                 riscv_command_timeout_sec);
 605                 return ERROR_FAIL;
 606         }
 607         return result;
 608 }
 609
 610 static int dmi_read(struct target *target, uint32_t *value, uint32_t address)
 611 {
 612         return dmi_op(target, value, NULL, DMI_OP_READ, address, 0, false);
 613 }
 614
 615 static int dmi_read_exec(struct target *target, uint32_t *value, uint32_t address)
 616 {
 617         return dmi_op(target, value, NULL, DMI_OP_READ, address, 0, true);
 618 }
 619
 620 static int dmi_write(struct target *target, uint32_t address, uint32_t value)
 621 {
 622         return dmi_op(target, NULL, NULL, DMI_OP_WRITE, address, value, false);
 623 }
 624
 625 static int dmi_write_exec(struct target *target, uint32_t address, uint32_t value)
 626 {
 627         return dmi_op(target, NULL, NULL, DMI_OP_WRITE, address, value, true);
 628 }
 629
 630 int dmstatus_read_timeout(struct target *target, uint32_t *dmstatus,
 631                 bool authenticated, unsigned timeout_sec)
 632 {
 633         int result = dmi_op_timeout(target, dmstatus, NULL, DMI_OP_READ,
 634                         DMI_DMSTATUS, 0, timeout_sec, false);
 635         if (result != ERROR_OK)
 636                 return result;
 637         if (authenticated && !get_field(*dmstatus, DMI_DMSTATUS_AUTHENTICATED)) {
 638                 LOG_ERROR("Debugger is not authenticated to target Debug Module. "
 639                                 "(dmstatus=0x%x). Use `riscv authdata_read` and "
 640                                 "`riscv authdata_write` commands to authenticate.", *dmstatus);
 641                 return ERROR_FAIL;
 642         }
 643         return ERROR_OK;
 644 }
 645
 646 int dmstatus_read(struct target *target, uint32_t *dmstatus,
 647                 bool authenticated)
 648 {
 649         return dmstatus_read_timeout(target, dmstatus, authenticated,
 650                         riscv_command_timeout_sec);
 651 }
 652
 653 static void increase_ac_busy_delay(struct target *target)
 654 {
 655         riscv013_info_t *info = get_info(target);
 656         info->ac_busy_delay += info->ac_busy_delay / 10 + 1;
 657         LOG_DEBUG("dtmcs_idle=%d, dmi_busy_delay=%d, ac_busy_delay=%d",
 658                         info->dtmcs_idle, info->dmi_busy_delay,
 659                         info->ac_busy_delay);
 660 }
 661
 662 uint32_t abstract_register_size(unsigned width)
 663 {
 664         switch (width) {
 665                 case 32:
 666                         return set_field(0, AC_ACCESS_REGISTER_SIZE, 2);
 667                 case 64:
 668                         return set_field(0, AC_ACCESS_REGISTER_SIZE, 3);
 669                 case 128:
 670                         return set_field(0, AC_ACCESS_REGISTER_SIZE, 4);
 671                 default:
 672                         LOG_ERROR("Unsupported register width: %d", width);
 673                         return 0;
 674         }
 675 }
 676
 677 static int wait_for_idle(struct target *target, uint32_t *abstractcs)
 678 {
 679         RISCV013_INFO(info);
 680         time_t start = time(NULL);
 681         while (1) {
 682                 if (dmi_read(target, abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
 683                         return ERROR_FAIL;
 684
 685                 if (get_field(*abstractcs, DMI_ABSTRACTCS_BUSY) == 0)
 686                         return ERROR_OK;
 687
 688                 if (time(NULL) - start > riscv_command_timeout_sec) {
 689                         info->cmderr = get_field(*abstractcs, DMI_ABSTRACTCS_CMDERR);
 690                         if (info->cmderr != CMDERR_NONE) {
 691                                 const char *errors[8] = {
 692                                         "none",
 693                                         "busy",
 694                                         "not supported",
 695                                         "exception",
 696                                         "halt/resume",
 697                                         "reserved",
 698                                         "reserved",
 699                                         "other" };
 700
 701                                 LOG_ERROR("Abstract command ended in error '%s' (abstractcs=0x%x)",
 702                                                 errors[info->cmderr], *abstractcs);
 703                         }
 704
 705                         LOG_ERROR("Timed out after %ds waiting for busy to go low (abstractcs=0x%x). "
 706                                         "Increase the timeout with riscv set_command_timeout_sec.",
 707                                         riscv_command_timeout_sec,
 708                                         *abstractcs);
 709                         return ERROR_FAIL;
 710                 }
 711         }
 712 }
 713
 714 static int execute_abstract_command(struct target *target, uint32_t command)
 715 {
 716         RISCV013_INFO(info);
 717         if (debug_level >= LOG_LVL_DEBUG) {
 718                 switch (get_field(command, DMI_COMMAND_CMDTYPE)) {
 719                         case 0:
 720                                 LOG_DEBUG("command=0x%x; access register, size=%d, postexec=%d, "
 721                                                 "transfer=%d, write=%d, regno=0x%x",
 722                                                 command,
 723                                                 8 << get_field(command, AC_ACCESS_REGISTER_SIZE),
 724                                                 get_field(command, AC_ACCESS_REGISTER_POSTEXEC),
 725                                                 get_field(command, AC_ACCESS_REGISTER_TRANSFER),
 726                                                 get_field(command, AC_ACCESS_REGISTER_WRITE),
 727                                                 get_field(command, AC_ACCESS_REGISTER_REGNO));
 728                                 break;
 729                         default:
 730                                 LOG_DEBUG("command=0x%x", command);
 731                                 break;
 732                 }
 733         }
 734
 735         dmi_write_exec(target, DMI_COMMAND, command);
 736
 737         uint32_t abstractcs = 0;
 738         wait_for_idle(target, &abstractcs);
 739
 740         info->cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
 741         if (info->cmderr != 0) {
 742                 LOG_DEBUG("command 0x%x failed; abstractcs=0x%x", command, abstractcs);
 743                 /* Clear the error. */
 744                 dmi_write(target, DMI_ABSTRACTCS, set_field(0, DMI_ABSTRACTCS_CMDERR,
 745                                         info->cmderr));
 746                 return ERROR_FAIL;
 747         }
 748
 749         return ERROR_OK;
 750 }
 751
 752 static riscv_reg_t read_abstract_arg(struct target *target, unsigned index,
 753                 unsigned size_bits)
 754 {
 755         riscv_reg_t value = 0;
 756         uint32_t v;
 757         unsigned offset = index * size_bits / 32;
 758         switch (size_bits) {
 759                 default:
 760                         LOG_ERROR("Unsupported size: %d", size_bits);
 761                         return ~0;
 762                 case 64:
 763                         dmi_read(target, &v, DMI_DATA0 + offset + 1);
 764                         value |= ((uint64_t) v) << 32;
 765                         /* falls through */
 766                 case 32:
 767                         dmi_read(target, &v, DMI_DATA0 + offset);
 768                         value |= v;
 769         }
 770         return value;
 771 }
 772
 773 static int write_abstract_arg(struct target *target, unsigned index,
 774                 riscv_reg_t value, unsigned size_bits)
 775 {
 776         unsigned offset = index * size_bits / 32;
 777         switch (size_bits) {
 778                 default:
 779                         LOG_ERROR("Unsupported size: %d", size_bits);
 780                         return ERROR_FAIL;
 781                 case 64:
 782                         dmi_write(target, DMI_DATA0 + offset + 1, value >> 32);
 783                         /* falls through */
 784                 case 32:
 785                         dmi_write(target, DMI_DATA0 + offset, value);
 786         }
 787         return ERROR_OK;
 788 }
 789
 790 /**
 791  * @par size in bits
 792  */
 793 static uint32_t access_register_command(struct target *target, uint32_t number,
 794                 unsigned size, uint32_t flags)
 795 {
 796         uint32_t command = set_field(0, DMI_COMMAND_CMDTYPE, 0);
 797         switch (size) {
 798                 case 32:
 799                         command = set_field(command, AC_ACCESS_REGISTER_SIZE, 2);
 800                         break;
 801                 case 64:
 802                         command = set_field(command, AC_ACCESS_REGISTER_SIZE, 3);
 803                         break;
 804                 default:
 805                         assert(0);
 806         }
 807
 808         if (number <= GDB_REGNO_XPR31) {
 809                 command = set_field(command, AC_ACCESS_REGISTER_REGNO,
 810                                 0x1000 + number - GDB_REGNO_ZERO);
 811         } else if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
 812                 command = set_field(command, AC_ACCESS_REGISTER_REGNO,
 813                                 0x1020 + number - GDB_REGNO_FPR0);
 814         } else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
 815                 command = set_field(command, AC_ACCESS_REGISTER_REGNO,
 816                                 number - GDB_REGNO_CSR0);
 817         } else if (number >= GDB_REGNO_COUNT) {
 818                 /* Custom register. */
 819                 assert(target->reg_cache->reg_list[number].arch_info);
 820                 riscv_reg_info_t *reg_info = target->reg_cache->reg_list[number].arch_info;
 821                 assert(reg_info);
 822                 command = set_field(command, AC_ACCESS_REGISTER_REGNO,
 823                                 0xc000 + reg_info->custom_number);
 824         }
 825
 826         command |= flags;
 827
 828         return command;
 829 }
 830
 831 static int register_read_abstract(struct target *target, uint64_t *value,
 832                 uint32_t number, unsigned size)
 833 {
 834         RISCV013_INFO(info);
 835
 836         if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31 &&
 837                         !info->abstract_read_fpr_supported)
 838                 return ERROR_FAIL;
 839         if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095 &&
 840                         !info->abstract_read_csr_supported)
 841                 return ERROR_FAIL;
 842
 843         uint32_t command = access_register_command(target, number, size,
 844                         AC_ACCESS_REGISTER_TRANSFER);
 845
 846         int result = execute_abstract_command(target, command);
 847         if (result != ERROR_OK) {
 848                 if (info->cmderr == CMDERR_NOT_SUPPORTED) {
 849                         if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
 850                                 info->abstract_read_fpr_supported = false;
 851                                 LOG_INFO("Disabling abstract command reads from FPRs.");
 852                         } else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
 853                                 info->abstract_read_csr_supported = false;
 854                                 LOG_INFO("Disabling abstract command reads from CSRs.");
 855                         }
 856                 }
 857                 return result;
 858         }
 859
 860         if (value)
 861                 *value = read_abstract_arg(target, 0, size);
 862
 863         return ERROR_OK;
 864 }
 865
 866 static int register_write_abstract(struct target *target, uint32_t number,
 867                 uint64_t value, unsigned size)
 868 {
 869         RISCV013_INFO(info);
 870
 871         if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31 &&
 872                         !info->abstract_write_fpr_supported)
 873                 return ERROR_FAIL;
 874         if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095 &&
 875                         !info->abstract_write_csr_supported)
 876                 return ERROR_FAIL;
 877
 878         uint32_t command = access_register_command(target, number, size,
 879                         AC_ACCESS_REGISTER_TRANSFER |
 880                         AC_ACCESS_REGISTER_WRITE);
 881
 882         if (write_abstract_arg(target, 0, value, size) != ERROR_OK)
 883                 return ERROR_FAIL;
 884
 885         int result = execute_abstract_command(target, command);
 886         if (result != ERROR_OK) {
 887                 if (info->cmderr == CMDERR_NOT_SUPPORTED) {
 888                         if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
 889                                 info->abstract_write_fpr_supported = false;
 890                                 LOG_INFO("Disabling abstract command writes to FPRs.");
 891                         } else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
 892                                 info->abstract_write_csr_supported = false;
 893                                 LOG_INFO("Disabling abstract command writes to CSRs.");
 894                         }
 895                 }
 896                 return result;
 897         }
 898
 899         return ERROR_OK;
 900 }
 901
 902 static int examine_progbuf(struct target *target)
 903 {
 904         riscv013_info_t *info = get_info(target);
 905
 906         if (info->progbuf_writable != YNM_MAYBE)
 907                 return ERROR_OK;
 908
 909         /* Figure out if progbuf is writable. */
 910
 911         if (info->progbufsize < 1) {
 912                 info->progbuf_writable = YNM_NO;
 913                 LOG_INFO("No program buffer present.");
 914                 return ERROR_OK;
 915         }
 916
 917         uint64_t s0;
 918         if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
 919                 return ERROR_FAIL;
 920
 921         struct riscv_program program;
 922         riscv_program_init(&program, target);
 923         riscv_program_insert(&program, auipc(S0));
 924         if (riscv_program_exec(&program, target) != ERROR_OK)
 925                 return ERROR_FAIL;
 926
 927         if (register_read_direct(target, &info->progbuf_address, GDB_REGNO_S0) != ERROR_OK)
 928                 return ERROR_FAIL;
 929
 930         riscv_program_init(&program, target);
 931         riscv_program_insert(&program, sw(S0, S0, 0));
 932         int result = riscv_program_exec(&program, target);
 933
 934         if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
 935                 return ERROR_FAIL;
 936
 937         if (result != ERROR_OK) {
 938                 /* This program might have failed if the program buffer is not
 939                  * writable. */
 940                 info->progbuf_writable = YNM_NO;
 941                 return ERROR_OK;
 942         }
 943
 944         uint32_t written;
 945         if (dmi_read(target, &written, DMI_PROGBUF0) != ERROR_OK)
 946                 return ERROR_FAIL;
 947         if (written == (uint32_t) info->progbuf_address) {
 948                 LOG_INFO("progbuf is writable at 0x%" PRIx64,
 949                                 info->progbuf_address);
 950                 info->progbuf_writable = YNM_YES;
 951
 952         } else {
 953                 LOG_INFO("progbuf is not writeable at 0x%" PRIx64,
 954                                 info->progbuf_address);
 955                 info->progbuf_writable = YNM_NO;
 956         }
 957
 958         return ERROR_OK;
 959 }
 960
 961 typedef enum {
 962         SPACE_DMI_DATA,
 963         SPACE_DMI_PROGBUF,
 964         SPACE_DMI_RAM
 965 } memory_space_t;
 966
 967 typedef struct {
 968         /* How can the debugger access this memory? */
 969         memory_space_t memory_space;
 970         /* Memory address to access the scratch memory from the hart. */
 971         riscv_addr_t hart_address;
 972         /* Memory address to access the scratch memory from the debugger. */
 973         riscv_addr_t debug_address;
 974         struct working_area *area;
 975 } scratch_mem_t;
 976
 977 /**
 978  * Find some scratch memory to be used with the given program.
 979  */
 980 static int scratch_reserve(struct target *target,
 981                 scratch_mem_t *scratch,
 982                 struct riscv_program *program,
 983                 unsigned size_bytes)
 984 {
 985         riscv_addr_t alignment = 1;
 986         while (alignment < size_bytes)
 987                 alignment *= 2;
 988
 989         scratch->area = NULL;
 990
 991         riscv013_info_t *info = get_info(target);
 992
 993         if (info->dataaccess == 1) {
 994                 /* Sign extend dataaddr. */
 995                 scratch->hart_address = info->dataaddr;
 996                 if (info->dataaddr & (1<<11))
 997                         scratch->hart_address |= 0xfffffffffffff000ULL;
 998                 /* Align. */
 999                 scratch->hart_address = (scratch->hart_address + alignment - 1) & ~(alignment - 1);
1000
1001                 if ((size_bytes + scratch->hart_address - info->dataaddr + 3) / 4 >=
1002                                 info->datasize) {
1003                         scratch->memory_space = SPACE_DMI_DATA;
1004                         scratch->debug_address = (scratch->hart_address - info->dataaddr) / 4;
1005                         return ERROR_OK;
1006                 }
1007         }
1008
1009         if (examine_progbuf(target) != ERROR_OK)
1010                 return ERROR_FAIL;
1011
1012         /* Allow for ebreak at the end of the program. */
1013         unsigned program_size = (program->instruction_count + 1) * 4;
1014         scratch->hart_address = (info->progbuf_address + program_size + alignment - 1) &
1015                 ~(alignment - 1);
1016         if ((size_bytes + scratch->hart_address - info->progbuf_address + 3) / 4 >=
1017                         info->progbufsize) {
1018                 scratch->memory_space = SPACE_DMI_PROGBUF;
1019                 scratch->debug_address = (scratch->hart_address - info->progbuf_address) / 4;
1020                 return ERROR_OK;
1021         }
1022
1023         if (target_alloc_working_area(target, size_bytes + alignment - 1,
1024                                 &scratch->area) == ERROR_OK) {
1025                 scratch->hart_address = (scratch->area->address + alignment - 1) &
1026                         ~(alignment - 1);
1027                 scratch->memory_space = SPACE_DMI_RAM;
1028                 scratch->debug_address = scratch->hart_address;
1029                 return ERROR_OK;
1030         }
1031
1032         LOG_ERROR("Couldn't find %d bytes of scratch RAM to use. Please configure "
1033                         "a work area with 'configure -work-area-phys'.", size_bytes);
1034         return ERROR_FAIL;
1035 }
1036
1037 static int scratch_release(struct target *target,
1038                 scratch_mem_t *scratch)
1039 {
1040         if (scratch->area)
1041                 return target_free_working_area(target, scratch->area);
1042
1043         return ERROR_OK;
1044 }
1045
1046 static int scratch_read64(struct target *target, scratch_mem_t *scratch,
1047                 uint64_t *value)
1048 {
1049         uint32_t v;
1050         switch (scratch->memory_space) {
1051                 case SPACE_DMI_DATA:
1052                         if (dmi_read(target, &v, DMI_DATA0 + scratch->debug_address) != ERROR_OK)
1053                                 return ERROR_FAIL;
1054                         *value = v;
1055                         if (dmi_read(target, &v, DMI_DATA1 + scratch->debug_address) != ERROR_OK)
1056                                 return ERROR_FAIL;
1057                         *value |= ((uint64_t) v) << 32;
1058                         break;
1059                 case SPACE_DMI_PROGBUF:
1060                         if (dmi_read(target, &v, DMI_PROGBUF0 + scratch->debug_address) != ERROR_OK)
1061                                 return ERROR_FAIL;
1062                         *value = v;
1063                         if (dmi_read(target, &v, DMI_PROGBUF1 + scratch->debug_address) != ERROR_OK)
1064                                 return ERROR_FAIL;
1065                         *value |= ((uint64_t) v) << 32;
1066                         break;
1067                 case SPACE_DMI_RAM:
1068                         {
1069                                 uint8_t buffer[8];
1070                                 if (read_memory(target, scratch->debug_address, 4, 2, buffer) != ERROR_OK)
1071                                         return ERROR_FAIL;
1072                                 *value = buffer[0] |
1073                                         (((uint64_t) buffer[1]) << 8) |
1074                                         (((uint64_t) buffer[2]) << 16) |
1075                                         (((uint64_t) buffer[3]) << 24) |
1076                                         (((uint64_t) buffer[4]) << 32) |
1077                                         (((uint64_t) buffer[5]) << 40) |
1078                                         (((uint64_t) buffer[6]) << 48) |
1079                                         (((uint64_t) buffer[7]) << 56);
1080                         }
1081                         break;
1082         }
1083         return ERROR_OK;
1084 }
1085
1086 static int scratch_write64(struct target *target, scratch_mem_t *scratch,
1087                 uint64_t value)
1088 {
1089         switch (scratch->memory_space) {
1090                 case SPACE_DMI_DATA:
1091                         dmi_write(target, DMI_DATA0 + scratch->debug_address, value);
1092                         dmi_write(target, DMI_DATA1 + scratch->debug_address, value >> 32);
1093                         break;
1094                 case SPACE_DMI_PROGBUF:
1095                         dmi_write(target, DMI_PROGBUF0 + scratch->debug_address, value);
1096                         dmi_write(target, DMI_PROGBUF1 + scratch->debug_address, value >> 32);
1097                         break;
1098                 case SPACE_DMI_RAM:
1099                         {
1100                                 uint8_t buffer[8] = {
1101                                         value,
1102                                         value >> 8,
1103                                         value >> 16,
1104                                         value >> 24,
1105                                         value >> 32,
1106                                         value >> 40,
1107                                         value >> 48,
1108                                         value >> 56
1109                                 };
1110                                 if (write_memory(target, scratch->debug_address, 4, 2, buffer) != ERROR_OK)
1111                                         return ERROR_FAIL;
1112                         }
1113                         break;
1114         }
1115         return ERROR_OK;
1116 }
1117
1118 /** Return register size in bits. */
1119 static unsigned register_size(struct target *target, unsigned number)
1120 {
1121         /* If reg_cache hasn't been initialized yet, make a guess. We need this for
1122          * when this function is called during examine(). */
1123         if (target->reg_cache)
1124                 return target->reg_cache->reg_list[number].size;
1125         else
1126                 return riscv_xlen(target);
1127 }
1128
1129 /**
1130  * Immediately write the new value to the requested register. This mechanism
1131  * bypasses any caches.
1132  */
1133 static int register_write_direct(struct target *target, unsigned number,
1134                 uint64_t value)
1135 {
1136         RISCV013_INFO(info);
1137         RISCV_INFO(r);
1138
1139         LOG_DEBUG("{%d} reg[0x%x] <- 0x%" PRIx64, riscv_current_hartid(target),
1140                         number, value);
1141
1142         int result = register_write_abstract(target, number, value,
1143                         register_size(target, number));
1144         if (result == ERROR_OK && target->reg_cache) {
1145                 struct reg *reg = &target->reg_cache->reg_list[number];
1146                 buf_set_u64(reg->value, 0, reg->size, value);
1147         }
1148         if (result == ERROR_OK || info->progbufsize + r->impebreak < 2 ||
1149                         !riscv_is_halted(target))
1150                 return result;
1151
1152         struct riscv_program program;
1153         riscv_program_init(&program, target);
1154
1155         uint64_t s0;
1156         if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
1157                 return ERROR_FAIL;
1158
1159         scratch_mem_t scratch;
1160         bool use_scratch = false;
1161         if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31 &&
1162                         riscv_supports_extension(target, riscv_current_hartid(target), 'D') &&
1163                         riscv_xlen(target) < 64) {
1164                 /* There are no instructions to move all the bits from a register, so
1165                  * we need to use some scratch RAM. */
1166                 use_scratch = true;
1167                 riscv_program_insert(&program, fld(number - GDB_REGNO_FPR0, S0, 0));
1168
1169                 if (scratch_reserve(target, &scratch, &program, 8) != ERROR_OK)
1170                         return ERROR_FAIL;
1171
1172                 if (register_write_direct(target, GDB_REGNO_S0, scratch.hart_address)
1173                                 != ERROR_OK) {
1174                         scratch_release(target, &scratch);
1175                         return ERROR_FAIL;
1176                 }
1177
1178                 if (scratch_write64(target, &scratch, value) != ERROR_OK) {
1179                         scratch_release(target, &scratch);
1180                         return ERROR_FAIL;
1181                 }
1182
1183         } else {
1184                 if (register_write_direct(target, GDB_REGNO_S0, value) != ERROR_OK)
1185                         return ERROR_FAIL;
1186
1187                 if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
1188                         if (riscv_supports_extension(target, riscv_current_hartid(target), 'D'))
1189                                 riscv_program_insert(&program, fmv_d_x(number - GDB_REGNO_FPR0, S0));
1190                         else
1191                                 riscv_program_insert(&program, fmv_w_x(number - GDB_REGNO_FPR0, S0));
1192                 } else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
1193                         riscv_program_csrw(&program, S0, number);
1194                 } else {
1195                         LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
1196                         return ERROR_FAIL;
1197                 }
1198         }
1199
1200         int exec_out = riscv_program_exec(&program, target);
1201         /* Don't message on error. Probably the register doesn't exist. */
1202         if (exec_out == ERROR_OK && target->reg_cache) {
1203                 struct reg *reg = &target->reg_cache->reg_list[number];
1204                 buf_set_u64(reg->value, 0, reg->size, value);
1205         }
1206
1207         if (use_scratch)
1208                 scratch_release(target, &scratch);
1209
1210         /* Restore S0. */
1211         if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
1212                 return ERROR_FAIL;
1213
1214         return exec_out;
1215 }
1216
1217 /** Return the cached value, or read from the target if necessary. */
1218 static int register_read(struct target *target, uint64_t *value, uint32_t number)
1219 {
1220         if (number == GDB_REGNO_ZERO) {
1221                 *value = 0;
1222                 return ERROR_OK;
1223         }
1224         int result = register_read_direct(target, value, number);
1225         if (result != ERROR_OK)
1226                 return ERROR_FAIL;
1227         if (target->reg_cache) {
1228                 struct reg *reg = &target->reg_cache->reg_list[number];
1229                 buf_set_u64(reg->value, 0, reg->size, *value);
1230         }
1231         return ERROR_OK;
1232 }
1233
1234 /** Actually read registers from the target right now. */
1235 static int register_read_direct(struct target *target, uint64_t *value, uint32_t number)
1236 {
1237         RISCV013_INFO(info);
1238         RISCV_INFO(r);
1239
1240         int result = register_read_abstract(target, value, number,
1241                         register_size(target, number));
1242
1243         if (result != ERROR_OK &&
1244                         info->progbufsize + r->impebreak >= 2 &&
1245                         number > GDB_REGNO_XPR31) {
1246                 struct riscv_program program;
1247                 riscv_program_init(&program, target);
1248
1249                 scratch_mem_t scratch;
1250                 bool use_scratch = false;
1251
1252                 uint64_t s0;
1253                 if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
1254                         return ERROR_FAIL;
1255
1256                 /* Write program to move data into s0. */
1257
1258                 uint64_t mstatus;
1259                 if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
1260                         if (register_read(target, &mstatus, GDB_REGNO_MSTATUS) != ERROR_OK)
1261                                 return ERROR_FAIL;
1262                         if ((mstatus & MSTATUS_FS) == 0)
1263                                 if (register_write_direct(target, GDB_REGNO_MSTATUS,
1264                                                         set_field(mstatus, MSTATUS_FS, 1)) != ERROR_OK)
1265                                         return ERROR_FAIL;
1266
1267                         if (riscv_supports_extension(target, riscv_current_hartid(target), 'D')
1268                                         && riscv_xlen(target) < 64) {
1269                                 /* There are no instructions to move all the bits from a
1270                                  * register, so we need to use some scratch RAM. */
1271                                 riscv_program_insert(&program, fsd(number - GDB_REGNO_FPR0, S0,
1272                                                         0));
1273
1274                                 if (scratch_reserve(target, &scratch, &program, 8) != ERROR_OK)
1275                                         return ERROR_FAIL;
1276                                 use_scratch = true;
1277
1278                                 if (register_write_direct(target, GDB_REGNO_S0,
1279                                                         scratch.hart_address) != ERROR_OK) {
1280                                         scratch_release(target, &scratch);
1281                                         return ERROR_FAIL;
1282                                 }
1283                         } else if (riscv_supports_extension(target,
1284                                                 riscv_current_hartid(target), 'D')) {
1285                                 riscv_program_insert(&program, fmv_x_d(S0, number - GDB_REGNO_FPR0));
1286                         } else {
1287                                 riscv_program_insert(&program, fmv_x_w(S0, number - GDB_REGNO_FPR0));
1288                         }
1289                 } else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
1290                         riscv_program_csrr(&program, S0, number);
1291                 } else {
1292                         LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
1293                         return ERROR_FAIL;
1294                 }
1295
1296                 /* Execute program. */
1297                 result = riscv_program_exec(&program, target);
1298                 /* Don't message on error. Probably the register doesn't exist. */
1299
1300                 if (use_scratch) {
1301                         result = scratch_read64(target, &scratch, value);
1302                         scratch_release(target, &scratch);
1303                         if (result != ERROR_OK)
1304                                 return result;
1305                 } else {
1306                         /* Read S0 */
1307                         if (register_read_direct(target, value, GDB_REGNO_S0) != ERROR_OK)
1308                                 return ERROR_FAIL;
1309                 }
1310
1311                 if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31 &&
1312                                 (mstatus & MSTATUS_FS) == 0)
1313                         if (register_write_direct(target, GDB_REGNO_MSTATUS, mstatus) != ERROR_OK)
1314                                 return ERROR_FAIL;
1315
1316                 /* Restore S0. */
1317                 if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
1318                         return ERROR_FAIL;
1319         }
1320
1321         if (result == ERROR_OK) {
1322                 LOG_DEBUG("{%d} reg[0x%x] = 0x%" PRIx64, riscv_current_hartid(target),
1323                                 number, *value);
1324         }
1325
1326         return result;
1327 }
1328
1329 int wait_for_authbusy(struct target *target, uint32_t *dmstatus)
1330 {
1331         time_t start = time(NULL);
1332         while (1) {
1333                 uint32_t value;
1334                 if (dmstatus_read(target, &value, false) != ERROR_OK)
1335                         return ERROR_FAIL;
1336                 if (dmstatus)
1337                         *dmstatus = value;
1338                 if (!get_field(value, DMI_DMSTATUS_AUTHBUSY))
1339                         break;
1340                 if (time(NULL) - start > riscv_command_timeout_sec) {
1341                         LOG_ERROR("Timed out after %ds waiting for authbusy to go low (dmstatus=0x%x). "
1342                                         "Increase the timeout with riscv set_command_timeout_sec.",
1343                                         riscv_command_timeout_sec,
1344                                         value);
1345                         return ERROR_FAIL;
1346                 }
1347         }
1348
1349         return ERROR_OK;
1350 }
1351
1352 /*** OpenOCD target functions. ***/
1353
1354 static void deinit_target(struct target *target)
1355 {
1356         LOG_DEBUG("riscv_deinit_target()");
1357         riscv_info_t *info = (riscv_info_t *) target->arch_info;
1358         free(info->version_specific);
1359         /* TODO: free register arch_info */
1360         info->version_specific = NULL;
1361 }
1362
1363 static int examine(struct target *target)
1364 {
1365         /* Don't need to select dbus, since the first thing we do is read dtmcontrol. */
1366
1367         uint32_t dtmcontrol = dtmcontrol_scan(target, 0);
1368         LOG_DEBUG("dtmcontrol=0x%x", dtmcontrol);
1369         LOG_DEBUG("  dmireset=%d", get_field(dtmcontrol, DTM_DTMCS_DMIRESET));
1370         LOG_DEBUG("  idle=%d", get_field(dtmcontrol, DTM_DTMCS_IDLE));
1371         LOG_DEBUG("  dmistat=%d", get_field(dtmcontrol, DTM_DTMCS_DMISTAT));
1372         LOG_DEBUG("  abits=%d", get_field(dtmcontrol, DTM_DTMCS_ABITS));
1373         LOG_DEBUG("  version=%d", get_field(dtmcontrol, DTM_DTMCS_VERSION));
1374         if (dtmcontrol == 0) {
1375                 LOG_ERROR("dtmcontrol is 0. Check JTAG connectivity/board power.");
1376                 return ERROR_FAIL;
1377         }
1378         if (get_field(dtmcontrol, DTM_DTMCS_VERSION) != 1) {
1379                 LOG_ERROR("Unsupported DTM version %d. (dtmcontrol=0x%x)",
1380                                 get_field(dtmcontrol, DTM_DTMCS_VERSION), dtmcontrol);
1381                 return ERROR_FAIL;
1382         }
1383
1384         riscv013_info_t *info = get_info(target);
1385         info->abits = get_field(dtmcontrol, DTM_DTMCS_ABITS);
1386         info->dtmcs_idle = get_field(dtmcontrol, DTM_DTMCS_IDLE);
1387
1388         /* Reset the Debug Module. */
1389         dm013_info_t *dm = get_dm(target);
1390         if (!dm->was_reset) {
1391                 dmi_write(target, DMI_DMCONTROL, 0);
1392                 dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
1393                 dm->was_reset = true;
1394         }
1395
1396         dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_HARTSELLO |
1397                         DMI_DMCONTROL_HARTSELHI | DMI_DMCONTROL_DMACTIVE);
1398         uint32_t dmcontrol;
1399         if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
1400                 return ERROR_FAIL;
1401
1402         if (!get_field(dmcontrol, DMI_DMCONTROL_DMACTIVE)) {
1403                 LOG_ERROR("Debug Module did not become active. dmcontrol=0x%x",
1404                                 dmcontrol);
1405                 return ERROR_FAIL;
1406         }
1407
1408         uint32_t dmstatus;
1409         if (dmstatus_read(target, &dmstatus, false) != ERROR_OK)
1410                 return ERROR_FAIL;
1411         LOG_DEBUG("dmstatus:  0x%08x", dmstatus);
1412         if (get_field(dmstatus, DMI_DMSTATUS_VERSION) != 2) {
1413                 LOG_ERROR("OpenOCD only supports Debug Module version 2, not %d "
1414                                 "(dmstatus=0x%x)", get_field(dmstatus, DMI_DMSTATUS_VERSION), dmstatus);
1415                 return ERROR_FAIL;
1416         }
1417
1418         uint32_t hartsel =
1419                 (get_field(dmcontrol, DMI_DMCONTROL_HARTSELHI) <<
1420                  DMI_DMCONTROL_HARTSELLO_LENGTH) |
1421                 get_field(dmcontrol, DMI_DMCONTROL_HARTSELLO);
1422         info->hartsellen = 0;
1423         while (hartsel & 1) {
1424                 info->hartsellen++;
1425                 hartsel >>= 1;
1426         }
1427         LOG_DEBUG("hartsellen=%d", info->hartsellen);
1428
1429         uint32_t hartinfo;
1430         if (dmi_read(target, &hartinfo, DMI_HARTINFO) != ERROR_OK)
1431                 return ERROR_FAIL;
1432
1433         info->datasize = get_field(hartinfo, DMI_HARTINFO_DATASIZE);
1434         info->dataaccess = get_field(hartinfo, DMI_HARTINFO_DATAACCESS);
1435         info->dataaddr = get_field(hartinfo, DMI_HARTINFO_DATAADDR);
1436
1437         if (!get_field(dmstatus, DMI_DMSTATUS_AUTHENTICATED)) {
1438                 LOG_ERROR("Debugger is not authenticated to target Debug Module. "
1439                                 "(dmstatus=0x%x). Use `riscv authdata_read` and "
1440                                 "`riscv authdata_write` commands to authenticate.", dmstatus);
1441                 /* If we return ERROR_FAIL here, then in a multicore setup the next
1442                  * core won't be examined, which means we won't set up the
1443                  * authentication commands for them, which means the config script
1444                  * needs to be a lot more complex. */
1445                 return ERROR_OK;
1446         }
1447
1448         if (dmi_read(target, &info->sbcs, DMI_SBCS) != ERROR_OK)
1449                 return ERROR_FAIL;
1450
1451         /* Check that abstract data registers are accessible. */
1452         uint32_t abstractcs;
1453         if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
1454                 return ERROR_FAIL;
1455         info->datacount = get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT);
1456         info->progbufsize = get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE);
1457
1458         LOG_INFO("datacount=%d progbufsize=%d", info->datacount, info->progbufsize);
1459
1460         RISCV_INFO(r);
1461         r->impebreak = get_field(dmstatus, DMI_DMSTATUS_IMPEBREAK);
1462
1463         if (info->progbufsize + r->impebreak < 2) {
1464                 LOG_WARNING("We won't be able to execute fence instructions on this "
1465                                 "target. Memory may not always appear consistent. "
1466                                 "(progbufsize=%d, impebreak=%d)", info->progbufsize,
1467                                 r->impebreak);
1468         }
1469
1470         /* Before doing anything else we must first enumerate the harts. */
1471
1472         /* Don't call any riscv_* functions until after we've counted the number of
1473          * cores and initialized registers. */
1474         for (int i = 0; i < MIN(RISCV_MAX_HARTS, 1 << info->hartsellen); ++i) {
1475                 if (!riscv_rtos_enabled(target) && i != target->coreid)
1476                         continue;
1477
1478                 r->current_hartid = i;
1479                 if (riscv013_select_current_hart(target) != ERROR_OK)
1480                         return ERROR_FAIL;
1481
1482                 uint32_t s;
1483                 if (dmstatus_read(target, &s, true) != ERROR_OK)
1484                         return ERROR_FAIL;
1485                 if (get_field(s, DMI_DMSTATUS_ANYNONEXISTENT))
1486                         break;
1487                 r->hart_count = i + 1;
1488
1489                 if (get_field(s, DMI_DMSTATUS_ANYHAVERESET))
1490                         dmi_write(target, DMI_DMCONTROL,
1491                                         set_hartsel(DMI_DMCONTROL_DMACTIVE | DMI_DMCONTROL_ACKHAVERESET, i));
1492
1493                 bool halted = riscv_is_halted(target);
1494                 if (!halted) {
1495                         if (riscv013_halt_current_hart(target) != ERROR_OK) {
1496                                 LOG_ERROR("Fatal: Hart %d failed to halt during examine()", i);
1497                                 return ERROR_FAIL;
1498                         }
1499                 }
1500
1501                 /* Without knowing anything else we can at least mess with the
1502                  * program buffer. */
1503                 r->debug_buffer_size[i] = info->progbufsize;
1504
1505                 int result = register_read_abstract(target, NULL, GDB_REGNO_S0, 64);
1506                 if (result == ERROR_OK)
1507                         r->xlen[i] = 64;
1508                 else
1509                         r->xlen[i] = 32;
1510
1511                 if (register_read(target, &r->misa[i], GDB_REGNO_MISA)) {
1512                         LOG_ERROR("Fatal: Failed to read MISA from hart %d.", i);
1513                         return ERROR_FAIL;
1514                 }
1515
1516                 /* Now init registers based on what we discovered. */
1517                 if (riscv_init_registers(target) != ERROR_OK)
1518                         return ERROR_FAIL;
1519
1520                 /* Display this as early as possible to help people who are using
1521                  * really slow simulators. */
1522                 LOG_DEBUG(" hart %d: XLEN=%d, misa=0x%" PRIx64, i, r->xlen[i],
1523                                 r->misa[i]);
1524
1525                 if (!halted)
1526                         riscv013_resume_current_hart(target);
1527         }
1528
1529         LOG_DEBUG("Enumerated %d harts", r->hart_count);
1530
1531         if (r->hart_count == 0) {
1532                 LOG_ERROR("No harts found!");
1533                 return ERROR_FAIL;
1534         }
1535
1536         target_set_examined(target);
1537
1538         /* Some regression suites rely on seeing 'Examined RISC-V core' to know
1539          * when they can connect with gdb/telnet.
1540          * We will need to update those suites if we want to change that text. */
1541         LOG_INFO("Examined RISC-V core; found %d harts",
1542                         riscv_count_harts(target));
1543         for (int i = 0; i < riscv_count_harts(target); ++i) {
1544                 if (riscv_hart_enabled(target, i)) {
1545                         LOG_INFO(" hart %d: XLEN=%d, misa=0x%" PRIx64, i, r->xlen[i],
1546                                         r->misa[i]);
1547                 } else {
1548                         LOG_INFO(" hart %d: currently disabled", i);
1549                 }
1550         }
1551         return ERROR_OK;
1552 }
1553
1554 int riscv013_authdata_read(struct target *target, uint32_t *value)
1555 {
1556         if (wait_for_authbusy(target, NULL) != ERROR_OK)
1557                 return ERROR_FAIL;
1558
1559         return dmi_read(target, value, DMI_AUTHDATA);
1560 }
1561
1562 int riscv013_authdata_write(struct target *target, uint32_t value)
1563 {
1564         uint32_t before, after;
1565         if (wait_for_authbusy(target, &before) != ERROR_OK)
1566                 return ERROR_FAIL;
1567
1568         dmi_write(target, DMI_AUTHDATA, value);
1569
1570         if (wait_for_authbusy(target, &after) != ERROR_OK)
1571                 return ERROR_FAIL;
1572
1573         if (!get_field(before, DMI_DMSTATUS_AUTHENTICATED) &&
1574                         get_field(after, DMI_DMSTATUS_AUTHENTICATED)) {
1575                 LOG_INFO("authdata_write resulted in successful authentication");
1576                 int result = ERROR_OK;
1577                 dm013_info_t *dm = get_dm(target);
1578                 target_list_t *entry;
1579                 list_for_each_entry(entry, &dm->target_list, list) {
1580                         if (examine(entry->target) != ERROR_OK)
1581                                 result = ERROR_FAIL;
1582                 }
1583                 return result;
1584         }
1585
1586         return ERROR_OK;
1587 }
1588
1589 static int init_target(struct command_context *cmd_ctx,
1590                 struct target *target)
1591 {
1592         LOG_DEBUG("init");
1593         riscv_info_t *generic_info = (riscv_info_t *) target->arch_info;
1594
1595         generic_info->get_register = &riscv013_get_register;
1596         generic_info->set_register = &riscv013_set_register;
1597         generic_info->select_current_hart = &riscv013_select_current_hart;
1598         generic_info->is_halted = &riscv013_is_halted;
1599         generic_info->halt_current_hart = &riscv013_halt_current_hart;
1600         generic_info->resume_current_hart = &riscv013_resume_current_hart;
1601         generic_info->step_current_hart = &riscv013_step_current_hart;
1602         generic_info->on_halt = &riscv013_on_halt;
1603         generic_info->on_resume = &riscv013_on_resume;
1604         generic_info->on_step = &riscv013_on_step;
1605         generic_info->halt_reason = &riscv013_halt_reason;
1606         generic_info->read_debug_buffer = &riscv013_read_debug_buffer;
1607         generic_info->write_debug_buffer = &riscv013_write_debug_buffer;
1608         generic_info->execute_debug_buffer = &riscv013_execute_debug_buffer;
1609         generic_info->fill_dmi_write_u64 = &riscv013_fill_dmi_write_u64;
1610         generic_info->fill_dmi_read_u64 = &riscv013_fill_dmi_read_u64;
1611         generic_info->fill_dmi_nop_u64 = &riscv013_fill_dmi_nop_u64;
1612         generic_info->dmi_write_u64_bits = &riscv013_dmi_write_u64_bits;
1613         generic_info->authdata_read = &riscv013_authdata_read;
1614         generic_info->authdata_write = &riscv013_authdata_write;
1615         generic_info->dmi_read = &dmi_read;
1616         generic_info->dmi_write = &dmi_write;
1617         generic_info->test_sba_config_reg = &riscv013_test_sba_config_reg;
1618         generic_info->test_compliance = &riscv013_test_compliance;
1619         generic_info->version_specific = calloc(1, sizeof(riscv013_info_t));
1620         if (!generic_info->version_specific)
1621                 return ERROR_FAIL;
1622         riscv013_info_t *info = get_info(target);
1623
1624         info->progbufsize = -1;
1625
1626         info->dmi_busy_delay = 0;
1627         info->bus_master_read_delay = 0;
1628         info->bus_master_write_delay = 0;
1629         info->ac_busy_delay = 0;
1630
1631         /* Assume all these abstract commands are supported until we learn
1632          * otherwise.
1633          * TODO: The spec allows eg. one CSR to be able to be accessed abstractly
1634          * while another one isn't. We don't track that this closely here, but in
1635          * the future we probably should. */
1636         info->abstract_read_csr_supported = true;
1637         info->abstract_write_csr_supported = true;
1638         info->abstract_read_fpr_supported = true;
1639         info->abstract_write_fpr_supported = true;
1640
1641         return ERROR_OK;
1642 }
1643
1644 static int assert_reset(struct target *target)
1645 {
1646         RISCV_INFO(r);
1647
1648         select_dmi(target);
1649
1650         uint32_t control_base = set_field(0, DMI_DMCONTROL_DMACTIVE, 1);
1651
1652         if (target->rtos) {
1653                 /* There's only one target, and OpenOCD thinks each hart is a thread.
1654                  * We must reset them all. */
1655
1656                 /* TODO: Try to use hasel in dmcontrol */
1657
1658                 /* Set haltreq for each hart. */
1659                 uint32_t control = control_base;
1660                 for (int i = 0; i < riscv_count_harts(target); ++i) {
1661                         if (!riscv_hart_enabled(target, i))
1662                                 continue;
1663
1664                         control = set_hartsel(control_base, i);
1665                         control = set_field(control, DMI_DMCONTROL_HALTREQ,
1666                                         target->reset_halt ? 1 : 0);
1667                         dmi_write(target, DMI_DMCONTROL, control);
1668                 }
1669                 /* Assert ndmreset */
1670                 control = set_field(control, DMI_DMCONTROL_NDMRESET, 1);
1671                 dmi_write(target, DMI_DMCONTROL, control);
1672
1673         } else {
1674                 /* Reset just this hart. */
1675                 uint32_t control = set_hartsel(control_base, r->current_hartid);
1676                 control = set_field(control, DMI_DMCONTROL_HALTREQ,
1677                                 target->reset_halt ? 1 : 0);
1678                 control = set_field(control, DMI_DMCONTROL_NDMRESET, 1);
1679                 dmi_write(target, DMI_DMCONTROL, control);
1680         }
1681
1682         target->state = TARGET_RESET;
1683
1684         return ERROR_OK;
1685 }
1686
1687 static int deassert_reset(struct target *target)
1688 {
1689         RISCV_INFO(r);
1690         RISCV013_INFO(info);
1691         select_dmi(target);
1692
1693         /* Clear the reset, but make sure haltreq is still set */
1694         uint32_t control = 0;
1695         control = set_field(control, DMI_DMCONTROL_HALTREQ, target->reset_halt ? 1 : 0);
1696         control = set_field(control, DMI_DMCONTROL_DMACTIVE, 1);
1697         dmi_write(target, DMI_DMCONTROL,
1698                         set_hartsel(control, r->current_hartid));
1699
1700         uint32_t dmstatus;
1701         int dmi_busy_delay = info->dmi_busy_delay;
1702         time_t start = time(NULL);
1703
1704         for (int i = 0; i < riscv_count_harts(target); ++i) {
1705                 int index = i;
1706                 if (target->rtos) {
1707                         if (!riscv_hart_enabled(target, index))
1708                                 continue;
1709                         dmi_write(target, DMI_DMCONTROL,
1710                                         set_hartsel(control, index));
1711                 } else {
1712                         index = r->current_hartid;
1713                 }
1714
1715                 char *operation;
1716                 uint32_t expected_field;
1717                 if (target->reset_halt) {
1718                         operation = "halt";
1719                         expected_field = DMI_DMSTATUS_ALLHALTED;
1720                 } else {
1721                         operation = "run";
1722                         expected_field = DMI_DMSTATUS_ALLRUNNING;
1723                 }
1724                 LOG_DEBUG("Waiting for hart %d to %s out of reset.", index, operation);
1725                 while (1) {
1726                         int result = dmstatus_read_timeout(target, &dmstatus, true,
1727                                         riscv_reset_timeout_sec);
1728                         if (result == ERROR_TIMEOUT_REACHED)
1729                                 LOG_ERROR("Hart %d didn't complete a DMI read coming out of "
1730                                                 "reset in %ds; Increase the timeout with riscv "
1731                                                 "set_reset_timeout_sec.",
1732                                                 index, riscv_reset_timeout_sec);
1733                         if (result != ERROR_OK)
1734                                 return result;
1735                         if (get_field(dmstatus, expected_field))
1736                                 break;
1737                         if (time(NULL) - start > riscv_reset_timeout_sec) {
1738                                 LOG_ERROR("Hart %d didn't %s coming out of reset in %ds; "
1739                                                 "dmstatus=0x%x; "
1740                                                 "Increase the timeout with riscv set_reset_timeout_sec.",
1741                                                 index, operation, riscv_reset_timeout_sec, dmstatus);
1742                                 return ERROR_FAIL;
1743                         }
1744                 }
1745                 target->state = TARGET_HALTED;
1746
1747                 if (get_field(dmstatus, DMI_DMSTATUS_ALLHAVERESET)) {
1748                         /* Ack reset. */
1749                         dmi_write(target, DMI_DMCONTROL,
1750                                         set_hartsel(control, index) |
1751                                         DMI_DMCONTROL_ACKHAVERESET);
1752                 }
1753
1754                 if (!target->rtos)
1755                         break;
1756         }
1757         info->dmi_busy_delay = dmi_busy_delay;
1758         return ERROR_OK;
1759 }
1760
1761 /**
1762  * @par size in bytes
1763  */
1764 static void write_to_buf(uint8_t *buffer, uint64_t value, unsigned size)
1765 {
1766         switch (size) {
1767                 case 8:
1768                         buffer[7] = value >> 56;
1769                         buffer[6] = value >> 48;
1770                         buffer[5] = value >> 40;
1771                         buffer[4] = value >> 32;
1772                         /* falls through */
1773                 case 4:
1774                         buffer[3] = value >> 24;
1775                         buffer[2] = value >> 16;
1776                         /* falls through */
1777                 case 2:
1778                         buffer[1] = value >> 8;
1779                         /* falls through */
1780                 case 1:
1781                         buffer[0] = value;
1782                         break;
1783                 default:
1784                         assert(false);
1785         }
1786 }
1787
1788 static int execute_fence(struct target *target)
1789 {
1790         int old_hartid = riscv_current_hartid(target);
1791
1792         /* FIXME: For non-coherent systems we need to flush the caches right
1793          * here, but there's no ISA-defined way of doing that. */
1794         {
1795                 struct riscv_program program;
1796                 riscv_program_init(&program, target);
1797                 riscv_program_fence_i(&program);
1798                 riscv_program_fence(&program);
1799                 int result = riscv_program_exec(&program, target);
1800                 if (result != ERROR_OK)
1801                         LOG_DEBUG("Unable to execute pre-fence");
1802         }
1803
1804         for (int i = 0; i < riscv_count_harts(target); ++i) {
1805                 if (!riscv_hart_enabled(target, i))
1806                         continue;
1807
1808                 riscv_set_current_hartid(target, i);
1809
1810                 struct riscv_program program;
1811                 riscv_program_init(&program, target);
1812                 riscv_program_fence_i(&program);
1813                 riscv_program_fence(&program);
1814                 int result = riscv_program_exec(&program, target);
1815                 if (result != ERROR_OK)
1816                         LOG_DEBUG("Unable to execute fence on hart %d", i);
1817         }
1818
1819         riscv_set_current_hartid(target, old_hartid);
1820
1821         return ERROR_OK;
1822 }
1823
1824 static void log_memory_access(target_addr_t address, uint64_t value,
1825                 unsigned size_bytes, bool read)
1826 {
1827         if (debug_level < LOG_LVL_DEBUG)
1828                 return;
1829
1830         char fmt[80];
1831         sprintf(fmt, "M[0x%" TARGET_PRIxADDR "] %ss 0x%%0%d" PRIx64,
1832                         address, read ? "read" : "write", size_bytes * 2);
1833         value &= (((uint64_t) 0x1) << (size_bytes * 8)) - 1;
1834         LOG_DEBUG(fmt, value);
1835 }
1836
1837 /* Read the relevant sbdata regs depending on size, and put the results into
1838  * buffer. */
1839 static int read_memory_bus_word(struct target *target, target_addr_t address,
1840                 uint32_t size, uint8_t *buffer)
1841 {
1842         uint32_t value;
1843         if (size > 12) {
1844                 if (dmi_read(target, &value, DMI_SBDATA3) != ERROR_OK)
1845                         return ERROR_FAIL;
1846                 write_to_buf(buffer + 12, value, 4);
1847                 log_memory_access(address + 12, value, 4, true);
1848         }
1849         if (size > 8) {
1850                 if (dmi_read(target, &value, DMI_SBDATA2) != ERROR_OK)
1851                         return ERROR_FAIL;
1852                 write_to_buf(buffer + 8, value, 4);
1853                 log_memory_access(address + 8, value, 4, true);
1854         }
1855         if (size > 4) {
1856                 if (dmi_read(target, &value, DMI_SBDATA1) != ERROR_OK)
1857                         return ERROR_FAIL;
1858                 write_to_buf(buffer + 4, value, 4);
1859                 log_memory_access(address + 4, value, 4, true);
1860         }
1861         if (dmi_read(target, &value, DMI_SBDATA0) != ERROR_OK)
1862                 return ERROR_FAIL;
1863         write_to_buf(buffer, value, MIN(size, 4));
1864         log_memory_access(address, value, MIN(size, 4), true);
1865         return ERROR_OK;
1866 }
1867
1868 static uint32_t sb_sbaccess(unsigned size_bytes)
1869 {
1870         switch (size_bytes) {
1871                 case 1:
1872                         return set_field(0, DMI_SBCS_SBACCESS, 0);
1873                 case 2:
1874                         return set_field(0, DMI_SBCS_SBACCESS, 1);
1875                 case 4:
1876                         return set_field(0, DMI_SBCS_SBACCESS, 2);
1877                 case 8:
1878                         return set_field(0, DMI_SBCS_SBACCESS, 3);
1879                 case 16:
1880                         return set_field(0, DMI_SBCS_SBACCESS, 4);
1881         }
1882         assert(0);
1883         return 0;       /* Make mingw happy. */
1884 }
1885
1886 static target_addr_t sb_read_address(struct target *target)
1887 {
1888         RISCV013_INFO(info);
1889         unsigned sbasize = get_field(info->sbcs, DMI_SBCS_SBASIZE);
1890         target_addr_t address = 0;
1891         uint32_t v;
1892         if (sbasize > 32) {
1893                 dmi_read(target, &v, DMI_SBADDRESS1);
1894                 address |= v;
1895                 address <<= 32;
1896         }
1897         dmi_read(target, &v, DMI_SBADDRESS0);
1898         address |= v;
1899         return address;
1900 }
1901
1902 static int sb_write_address(struct target *target, target_addr_t address)
1903 {
1904         RISCV013_INFO(info);
1905         unsigned sbasize = get_field(info->sbcs, DMI_SBCS_SBASIZE);
1906         /* There currently is no support for >64-bit addresses in OpenOCD. */
1907         if (sbasize > 96)
1908                 dmi_write(target, DMI_SBADDRESS3, 0);
1909         if (sbasize > 64)
1910                 dmi_write(target, DMI_SBADDRESS2, 0);
1911         if (sbasize > 32)
1912                 dmi_write(target, DMI_SBADDRESS1, address >> 32);
1913         return dmi_write(target, DMI_SBADDRESS0, address);
1914 }
1915
1916 static int read_sbcs_nonbusy(struct target *target, uint32_t *sbcs)
1917 {
1918         time_t start = time(NULL);
1919         while (1) {
1920                 if (dmi_read(target, sbcs, DMI_SBCS) != ERROR_OK)
1921                         return ERROR_FAIL;
1922                 if (!get_field(*sbcs, DMI_SBCS_SBBUSY))
1923                         return ERROR_OK;
1924                 if (time(NULL) - start > riscv_command_timeout_sec) {
1925                         LOG_ERROR("Timed out after %ds waiting for sbbusy to go low (sbcs=0x%x). "
1926                                         "Increase the timeout with riscv set_command_timeout_sec.",
1927                                         riscv_command_timeout_sec, *sbcs);
1928                         return ERROR_FAIL;
1929                 }
1930         }
1931 }
1932
1933 static int read_memory_bus_v0(struct target *target, target_addr_t address,
1934                 uint32_t size, uint32_t count, uint8_t *buffer)
1935 {
1936         LOG_DEBUG("System Bus Access: size: %d\tcount:%d\tstart address: 0x%08"
1937                         TARGET_PRIxADDR, size, count, address);
1938         uint8_t *t_buffer = buffer;
1939         riscv_addr_t cur_addr = address;
1940         riscv_addr_t fin_addr = address + (count * size);
1941         uint32_t access = 0;
1942
1943         const int DMI_SBCS_SBSINGLEREAD_OFFSET = 20;
1944         const uint32_t DMI_SBCS_SBSINGLEREAD = (0x1U << DMI_SBCS_SBSINGLEREAD_OFFSET);
1945
1946         const int DMI_SBCS_SBAUTOREAD_OFFSET = 15;
1947         const uint32_t DMI_SBCS_SBAUTOREAD = (0x1U << DMI_SBCS_SBAUTOREAD_OFFSET);
1948
1949         /* ww favorise one off reading if there is an issue */
1950         if (count == 1) {
1951                 for (uint32_t i = 0; i < count; i++) {
1952                         if (dmi_read(target, &access, DMI_SBCS) != ERROR_OK)
1953                                 return ERROR_FAIL;
1954                         dmi_write(target, DMI_SBADDRESS0, cur_addr);
1955                         /* size/2 matching the bit access of the spec 0.13 */
1956                         access = set_field(access, DMI_SBCS_SBACCESS, size/2);
1957                         access = set_field(access, DMI_SBCS_SBSINGLEREAD, 1);
1958                         LOG_DEBUG("\r\nread_memory: sab: access:  0x%08x", access);
1959                         dmi_write(target, DMI_SBCS, access);
1960                         /* 3) read */
1961                         uint32_t value;
1962                         if (dmi_read(target, &value, DMI_SBDATA0) != ERROR_OK)
1963                                 return ERROR_FAIL;
1964                         LOG_DEBUG("\r\nread_memory: sab: value:  0x%08x", value);
1965                         write_to_buf(t_buffer, value, size);
1966                         t_buffer += size;
1967                         cur_addr += size;
1968                 }
1969                 return ERROR_OK;
1970         }
1971
1972         /* has to be the same size if we want to read a block */
1973         LOG_DEBUG("reading block until final address 0x%" PRIx64, fin_addr);
1974         if (dmi_read(target, &access, DMI_SBCS) != ERROR_OK)
1975                 return ERROR_FAIL;
1976         /* set current address */
1977         dmi_write(target, DMI_SBADDRESS0, cur_addr);
1978         /* 2) write sbaccess=2, sbsingleread,sbautoread,sbautoincrement
1979          * size/2 matching the bit access of the spec 0.13 */
1980         access = set_field(access, DMI_SBCS_SBACCESS, size/2);
1981         access = set_field(access, DMI_SBCS_SBAUTOREAD, 1);
1982         access = set_field(access, DMI_SBCS_SBSINGLEREAD, 1);
1983         access = set_field(access, DMI_SBCS_SBAUTOINCREMENT, 1);
1984         LOG_DEBUG("\r\naccess:  0x%08x", access);
1985         dmi_write(target, DMI_SBCS, access);
1986
1987         while (cur_addr < fin_addr) {
1988                 LOG_DEBUG("\r\nsab:autoincrement: \r\n size: %d\tcount:%d\taddress: 0x%08"
1989                                 PRIx64, size, count, cur_addr);
1990                 /* read */
1991                 uint32_t value;
1992                 if (dmi_read(target, &value, DMI_SBDATA0) != ERROR_OK)
1993                         return ERROR_FAIL;
1994                 write_to_buf(t_buffer, value, size);
1995                 cur_addr += size;
1996                 t_buffer += size;
1997
1998                 /* if we are reaching last address, we must clear autoread */
1999                 if (cur_addr == fin_addr && count != 1) {
2000                         dmi_write(target, DMI_SBCS, 0);
2001                         if (dmi_read(target, &value, DMI_SBDATA0) != ERROR_OK)
2002                                 return ERROR_FAIL;
2003                         write_to_buf(t_buffer, value, size);
2004                 }
2005         }
2006
2007         return ERROR_OK;
2008 }
2009
2010 /**
2011  * Read the requested memory using the system bus interface.
2012  */
2013 static int read_memory_bus_v1(struct target *target, target_addr_t address,
2014                 uint32_t size, uint32_t count, uint8_t *buffer)
2015 {
2016         RISCV013_INFO(info);
2017         target_addr_t next_address = address;
2018         target_addr_t end_address = address + count * size;
2019
2020         while (next_address < end_address) {
2021                 uint32_t sbcs = set_field(0, DMI_SBCS_SBREADONADDR, 1);
2022                 sbcs |= sb_sbaccess(size);
2023                 sbcs = set_field(sbcs, DMI_SBCS_SBAUTOINCREMENT, 1);
2024                 sbcs = set_field(sbcs, DMI_SBCS_SBREADONDATA, count > 1);
2025                 dmi_write(target, DMI_SBCS, sbcs);
2026
2027                 /* This address write will trigger the first read. */
2028                 sb_write_address(target, next_address);
2029
2030                 if (info->bus_master_read_delay) {
2031                         jtag_add_runtest(info->bus_master_read_delay, TAP_IDLE);
2032                         if (jtag_execute_queue() != ERROR_OK) {
2033                                 LOG_ERROR("Failed to scan idle sequence");
2034                                 return ERROR_FAIL;
2035                         }
2036                 }
2037
2038                 for (uint32_t i = (next_address - address) / size; i < count - 1; i++) {
2039                         read_memory_bus_word(target, address + i * size, size,
2040                                         buffer + i * size);
2041                 }
2042
2043                 sbcs = set_field(sbcs, DMI_SBCS_SBREADONDATA, 0);
2044                 dmi_write(target, DMI_SBCS, sbcs);
2045
2046                 read_memory_bus_word(target, address + (count - 1) * size, size,
2047                                 buffer + (count - 1) * size);
2048
2049                 if (read_sbcs_nonbusy(target, &sbcs) != ERROR_OK)
2050                         return ERROR_FAIL;
2051
2052                 if (get_field(sbcs, DMI_SBCS_SBBUSYERROR)) {
2053                         /* We read while the target was busy. Slow down and try again. */
2054                         dmi_write(target, DMI_SBCS, DMI_SBCS_SBBUSYERROR);
2055                         next_address = sb_read_address(target);
2056                         info->bus_master_read_delay += info->bus_master_read_delay / 10 + 1;
2057                         continue;
2058                 }
2059
2060                 unsigned error = get_field(sbcs, DMI_SBCS_SBERROR);
2061                 if (error == 0) {
2062                         next_address = end_address;
2063                 } else {
2064                         /* Some error indicating the bus access failed, but not because of
2065                          * something we did wrong. */
2066                         dmi_write(target, DMI_SBCS, DMI_SBCS_SBERROR);
2067                         return ERROR_FAIL;
2068                 }
2069         }
2070
2071         return ERROR_OK;
2072 }
2073
2074 static int batch_run(const struct target *target, struct riscv_batch *batch)
2075 {
2076         RISCV013_INFO(info);
2077         RISCV_INFO(r);
2078         if (r->reset_delays_wait >= 0) {
2079                 r->reset_delays_wait -= batch->used_scans;
2080                 if (r->reset_delays_wait <= 0) {
2081                         batch->idle_count = 0;
2082                         info->dmi_busy_delay = 0;
2083                         info->ac_busy_delay = 0;
2084                 }
2085         }
2086         return riscv_batch_run(batch);
2087 }
2088
2089 /**
2090  * Read the requested memory, taking care to execute every read exactly once,
2091  * even if cmderr=busy is encountered.
2092  */
2093 static int read_memory_progbuf_inner(struct target *target, target_addr_t address,
2094                 uint32_t size, uint32_t count, uint8_t *buffer)
2095 {
2096         RISCV013_INFO(info);
2097
2098         int result = ERROR_OK;
2099
2100         /* Write address to S0, and execute buffer. */
2101         result = register_write_direct(target, GDB_REGNO_S0, address);
2102         if (result != ERROR_OK)
2103                 goto error;
2104         uint32_t command = access_register_command(target, GDB_REGNO_S1,
2105                         riscv_xlen(target),
2106                         AC_ACCESS_REGISTER_TRANSFER | AC_ACCESS_REGISTER_POSTEXEC);
2107         if (execute_abstract_command(target, command) != ERROR_OK)
2108                 return ERROR_FAIL;
2109
2110         /* First read has just triggered. Result is in s1. */
2111
2112         if (count == 1) {
2113                 uint64_t value;
2114                 if (register_read_direct(target, &value, GDB_REGNO_S1) != ERROR_OK)
2115                         return ERROR_FAIL;
2116                 write_to_buf(buffer, value, size);
2117                 log_memory_access(address, value, size, true);
2118                 return ERROR_OK;
2119         }
2120
2121         if (dmi_write(target, DMI_ABSTRACTAUTO,
2122                         1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET) != ERROR_OK)
2123                 goto error;
2124         /* Read garbage from dmi_data0, which triggers another execution of the
2125          * program. Now dmi_data0 contains the first good result, and s1 the next
2126          * memory value. */
2127         if (dmi_read_exec(target, NULL, DMI_DATA0) != ERROR_OK)
2128                 goto error;
2129
2130         /* read_addr is the next address that the hart will read from, which is the
2131          * value in s0. */
2132         riscv_addr_t read_addr = address + 2 * size;
2133         riscv_addr_t fin_addr = address + (count * size);
2134         while (read_addr < fin_addr) {
2135                 LOG_DEBUG("read_addr=0x%" PRIx64 ", fin_addr=0x%" PRIx64, read_addr,
2136                                 fin_addr);
2137                 /* The pipeline looks like this:
2138                  * memory -> s1 -> dm_data0 -> debugger
2139                  * Right now:
2140                  * s0 contains read_addr
2141                  * s1 contains mem[read_addr-size]
2142                  * dm_data0 contains[read_addr-size*2]
2143                  */
2144
2145                 LOG_DEBUG("creating burst to read from 0x%" PRIx64
2146                                 " up to 0x%" PRIx64, read_addr, fin_addr);
2147                 assert(read_addr >= address && read_addr < fin_addr);
2148                 struct riscv_batch *batch = riscv_batch_alloc(target, 32,
2149                                 info->dmi_busy_delay + info->ac_busy_delay);
2150
2151                 size_t reads = 0;
2152                 for (riscv_addr_t addr = read_addr; addr < fin_addr; addr += size) {
2153                         riscv_batch_add_dmi_read(batch, DMI_DATA0);
2154
2155                         reads++;
2156                         if (riscv_batch_full(batch))
2157                                 break;
2158                 }
2159
2160                 batch_run(target, batch);
2161
2162                 /* Wait for the target to finish performing the last abstract command,
2163                  * and update our copy of cmderr. If we see that DMI is busy here,
2164                  * dmi_busy_delay will be incremented. */
2165                 uint32_t abstractcs;
2166                 if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
2167                         return ERROR_FAIL;
2168                 while (get_field(abstractcs, DMI_ABSTRACTCS_BUSY))
2169                         if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
2170                                 return ERROR_FAIL;
2171                 info->cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
2172
2173                 riscv_addr_t next_read_addr;
2174                 unsigned ignore_last = 0;
2175                 switch (info->cmderr) {
2176                         case CMDERR_NONE:
2177                                 LOG_DEBUG("successful (partial?) memory read");
2178                                 next_read_addr = read_addr + reads * size;
2179                                 break;
2180                         case CMDERR_BUSY:
2181                                 LOG_DEBUG("memory read resulted in busy response");
2182
2183                                 increase_ac_busy_delay(target);
2184                                 riscv013_clear_abstract_error(target);
2185
2186                                 dmi_write(target, DMI_ABSTRACTAUTO, 0);
2187
2188                                 uint32_t dmi_data0;
2189                                 /* This is definitely a good version of the value that we
2190                                  * attempted to read when we discovered that the target was
2191                                  * busy. */
2192                                 if (dmi_read(target, &dmi_data0, DMI_DATA0) != ERROR_OK) {
2193                                         riscv_batch_free(batch);
2194                                         goto error;
2195                                 }
2196
2197                                 /* See how far we got, clobbering dmi_data0. */
2198                                 result = register_read_direct(target, &next_read_addr,
2199                                                 GDB_REGNO_S0);
2200                                 if (result != ERROR_OK) {
2201                                         riscv_batch_free(batch);
2202                                         goto error;
2203                                 }
2204                                 write_to_buf(buffer + next_read_addr - 2 * size - address, dmi_data0, size);
2205                                 log_memory_access(next_read_addr - 2 * size, dmi_data0, size, true);
2206
2207                                 /* Restore the command, and execute it.
2208                                  * Now DMI_DATA0 contains the next value just as it would if no
2209                                  * error had occurred. */
2210                                 dmi_write_exec(target, DMI_COMMAND, command);
2211                                 next_read_addr += size;
2212
2213                                 dmi_write(target, DMI_ABSTRACTAUTO,
2214                                                 1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
2215
2216                                 ignore_last = 1;
2217
2218                                 break;
2219                         default:
2220                                 LOG_DEBUG("error when reading memory, abstractcs=0x%08lx", (long)abstractcs);
2221                                 riscv013_clear_abstract_error(target);
2222                                 riscv_batch_free(batch);
2223                                 result = ERROR_FAIL;
2224                                 goto error;
2225                 }
2226
2227                 /* Now read whatever we got out of the batch. */
2228                 dmi_status_t status = DMI_STATUS_SUCCESS;
2229                 for (size_t i = 0; i < reads; i++) {
2230                         riscv_addr_t receive_addr = read_addr + (i-2) * size;
2231                         assert(receive_addr < address + size * count);
2232                         if (receive_addr < address)
2233                                 continue;
2234                         if (receive_addr > next_read_addr - (3 + ignore_last) * size)
2235                                 break;
2236
2237                         uint64_t dmi_out = riscv_batch_get_dmi_read(batch, i);
2238                         status = get_field(dmi_out, DTM_DMI_OP);
2239                         if (status != DMI_STATUS_SUCCESS) {
2240                                 /* If we're here because of busy count, dmi_busy_delay will
2241                                  * already have been increased and busy state will have been
2242                                  * cleared in dmi_read(). */
2243                                 /* In at least some implementations, we issue a read, and then
2244                                  * can get busy back when we try to scan out the read result,
2245                                  * and the actual read value is lost forever. Since this is
2246                                  * rare in any case, we return error here and rely on our
2247                                  * caller to reread the entire block. */
2248                                 LOG_WARNING("Batch memory read encountered DMI error %d. "
2249                                                 "Falling back on slower reads.", status);
2250                                 riscv_batch_free(batch);
2251                                 result = ERROR_FAIL;
2252                                 goto error;
2253                         }
2254                         uint32_t value = get_field(dmi_out, DTM_DMI_DATA);
2255                         riscv_addr_t offset = receive_addr - address;
2256                         write_to_buf(buffer + offset, value, size);
2257                         log_memory_access(receive_addr, value, size, true);
2258
2259                         receive_addr += size;
2260                 }
2261
2262                 read_addr = next_read_addr;
2263
2264                 riscv_batch_free(batch);
2265         }
2266
2267         dmi_write(target, DMI_ABSTRACTAUTO, 0);
2268
2269         if (count > 1) {
2270                 /* Read the penultimate word. */
2271                 uint32_t value;
2272                 if (dmi_read(target, &value, DMI_DATA0) != ERROR_OK)
2273                         return ERROR_FAIL;
2274                 write_to_buf(buffer + size * (count-2), value, size);
2275                 log_memory_access(address + size * (count-2), value, size, true);
2276         }
2277
2278         /* Read the last word. */
2279         uint64_t value;
2280         result = register_read_direct(target, &value, GDB_REGNO_S1);
2281         if (result != ERROR_OK)
2282                 goto error;
2283         write_to_buf(buffer + size * (count-1), value, size);
2284         log_memory_access(address + size * (count-1), value, size, true);
2285
2286         return ERROR_OK;
2287
2288 error:
2289         dmi_write(target, DMI_ABSTRACTAUTO, 0);
2290
2291         return result;
2292 }
2293
2294 /**
2295  * Read the requested memory, silently handling memory access errors.
2296  */
2297 static int read_memory_progbuf(struct target *target, target_addr_t address,
2298                 uint32_t size, uint32_t count, uint8_t *buffer)
2299 {
2300         int result = ERROR_OK;
2301
2302         LOG_DEBUG("reading %d words of %d bytes from 0x%" TARGET_PRIxADDR, count,
2303                         size, address);
2304
2305         select_dmi(target);
2306
2307         memset(buffer, 0, count*size);
2308
2309         /* s0 holds the next address to write to
2310          * s1 holds the next data value to write
2311          */
2312         uint64_t s0, s1;
2313         if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
2314                 return ERROR_FAIL;
2315         if (register_read(target, &s1, GDB_REGNO_S1) != ERROR_OK)
2316                 return ERROR_FAIL;
2317
2318         if (execute_fence(target) != ERROR_OK)
2319                 return ERROR_FAIL;
2320
2321         /* Write the program (load, increment) */
2322         struct riscv_program program;
2323         riscv_program_init(&program, target);
2324         switch (size) {
2325                 case 1:
2326                         riscv_program_lbr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2327                         break;
2328                 case 2:
2329                         riscv_program_lhr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2330                         break;
2331                 case 4:
2332                         riscv_program_lwr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2333                         break;
2334                 default:
2335                         LOG_ERROR("Unsupported size: %d", size);
2336                         return ERROR_FAIL;
2337         }
2338         riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
2339
2340         if (riscv_program_ebreak(&program) != ERROR_OK)
2341                 return ERROR_FAIL;
2342         riscv_program_write(&program);
2343
2344         result = read_memory_progbuf_inner(target, address, size, count, buffer);
2345
2346         if (result != ERROR_OK) {
2347                 /* The full read did not succeed, so we will try to read each word individually. */
2348                 /* This will not be fast, but reading outside actual memory is a special case anyway. */
2349                 /* It will make the toolchain happier, especially Eclipse Memory View as it reads ahead. */
2350                 target_addr_t address_i = address;
2351                 uint32_t size_i = size;
2352                 uint32_t count_i = 1;
2353                 uint8_t *buffer_i = buffer;
2354
2355                 for (uint32_t i = 0; i < count; i++, address_i += size_i, buffer_i += size_i) {
2356                         /* TODO: This is much slower than it needs to be because we end up
2357                          * writing the address to read for every word we read. */
2358                         result = read_memory_progbuf_inner(target, address_i, size_i, count_i, buffer_i);
2359
2360                         /* The read of a single word failed, so we will just return 0 for that instead */
2361                         if (result != ERROR_OK) {
2362                                 LOG_DEBUG("error reading single word of %d bytes from 0x%" TARGET_PRIxADDR,
2363                                                 size_i, address_i);
2364
2365                                 uint64_t value_i = 0;
2366                                 write_to_buf(buffer_i, value_i, size_i);
2367                         }
2368                 }
2369                 result = ERROR_OK;
2370         }
2371
2372         riscv_set_register(target, GDB_REGNO_S0, s0);
2373         riscv_set_register(target, GDB_REGNO_S1, s1);
2374         return result;
2375 }
2376
2377 static int read_memory(struct target *target, target_addr_t address,
2378                 uint32_t size, uint32_t count, uint8_t *buffer)
2379 {
2380         RISCV013_INFO(info);
2381         if (info->progbufsize >= 2 && !riscv_prefer_sba)
2382                 return read_memory_progbuf(target, address, size, count, buffer);
2383
2384         if ((get_field(info->sbcs, DMI_SBCS_SBACCESS8) && size == 1) ||
2385                         (get_field(info->sbcs, DMI_SBCS_SBACCESS16) && size == 2) ||
2386                         (get_field(info->sbcs, DMI_SBCS_SBACCESS32) && size == 4) ||
2387                         (get_field(info->sbcs, DMI_SBCS_SBACCESS64) && size == 8) ||
2388                         (get_field(info->sbcs, DMI_SBCS_SBACCESS128) && size == 16)) {
2389                 if (get_field(info->sbcs, DMI_SBCS_SBVERSION) == 0)
2390                         return read_memory_bus_v0(target, address, size, count, buffer);
2391                 else if (get_field(info->sbcs, DMI_SBCS_SBVERSION) == 1)
2392                         return read_memory_bus_v1(target, address, size, count, buffer);
2393         }
2394
2395         if (info->progbufsize >= 2)
2396                 return read_memory_progbuf(target, address, size, count, buffer);
2397
2398         LOG_ERROR("Don't know how to read memory on this target.");
2399         return ERROR_FAIL;
2400 }
2401
2402 static int write_memory_bus_v0(struct target *target, target_addr_t address,
2403                 uint32_t size, uint32_t count, const uint8_t *buffer)
2404 {
2405         /*1) write sbaddress: for singlewrite and autoincrement, we need to write the address once*/
2406         LOG_DEBUG("System Bus Access: size: %d\tcount:%d\tstart address: 0x%08"
2407                         TARGET_PRIxADDR, size, count, address);
2408         dmi_write(target, DMI_SBADDRESS0, address);
2409         int64_t value = 0;
2410         int64_t access = 0;
2411         riscv_addr_t offset = 0;
2412         riscv_addr_t t_addr = 0;
2413         const uint8_t *t_buffer = buffer + offset;
2414
2415         /* B.8 Writing Memory, single write check if we write in one go */
2416         if (count == 1) { /* count is in bytes here */
2417                 /* check the size */
2418                 switch (size) {
2419                         case 1:
2420                                 value = t_buffer[0];
2421                                 break;
2422                         case 2:
2423                                 value = t_buffer[0]
2424                                         | ((uint32_t) t_buffer[1] << 8);
2425                                 break;
2426                         case 4:
2427                                 value = t_buffer[0]
2428                                         | ((uint32_t) t_buffer[1] << 8)
2429                                         | ((uint32_t) t_buffer[2] << 16)
2430                                         | ((uint32_t) t_buffer[3] << 24);
2431                                 break;
2432                         default:
2433                                 LOG_ERROR("unsupported access size: %d", size);
2434                                 return ERROR_FAIL;
2435                 }
2436
2437                 access = 0;
2438                 access = set_field(access, DMI_SBCS_SBACCESS, size/2);
2439                 dmi_write(target, DMI_SBCS, access);
2440                 LOG_DEBUG("\r\naccess:  0x%08" PRIx64, access);
2441                 LOG_DEBUG("\r\nwrite_memory:SAB: ONE OFF: value 0x%08" PRIx64, value);
2442                 dmi_write(target, DMI_SBDATA0, value);
2443                 return ERROR_OK;
2444         }
2445
2446         /*B.8 Writing Memory, using autoincrement*/
2447
2448         access = 0;
2449         access = set_field(access, DMI_SBCS_SBACCESS, size/2);
2450         access = set_field(access, DMI_SBCS_SBAUTOINCREMENT, 1);
2451         LOG_DEBUG("\r\naccess:  0x%08" PRIx64, access);
2452         dmi_write(target, DMI_SBCS, access);
2453
2454         /*2)set the value according to the size required and write*/
2455         for (riscv_addr_t i = 0; i < count; ++i) {
2456                 offset = size*i;
2457                 /* for monitoring only */
2458                 t_addr = address + offset;
2459                 t_buffer = buffer + offset;
2460
2461                 switch (size) {
2462                         case 1:
2463                                 value = t_buffer[0];
2464                                 break;
2465                         case 2:
2466                                 value = t_buffer[0]
2467                                         | ((uint32_t) t_buffer[1] << 8);
2468                                 break;
2469                         case 4:
2470                                 value = t_buffer[0]
2471                                         | ((uint32_t) t_buffer[1] << 8)
2472                                         | ((uint32_t) t_buffer[2] << 16)
2473                                         | ((uint32_t) t_buffer[3] << 24);
2474                                 break;
2475                         default:
2476                                 LOG_ERROR("unsupported access size: %d", size);
2477                                 return ERROR_FAIL;
2478                 }
2479                 LOG_DEBUG("SAB:autoincrement: expected address: 0x%08x value: 0x%08x"
2480                                 PRIx64, (uint32_t)t_addr, (uint32_t)value);
2481                 dmi_write(target, DMI_SBDATA0, value);
2482         }
2483         /*reset the autoincrement when finished (something weird is happening if this is not done at the end*/
2484         access = set_field(access, DMI_SBCS_SBAUTOINCREMENT, 0);
2485         dmi_write(target, DMI_SBCS, access);
2486
2487         return ERROR_OK;
2488 }
2489
2490 static int write_memory_bus_v1(struct target *target, target_addr_t address,
2491                 uint32_t size, uint32_t count, const uint8_t *buffer)
2492 {
2493         RISCV013_INFO(info);
2494         uint32_t sbcs = sb_sbaccess(size);
2495         sbcs = set_field(sbcs, DMI_SBCS_SBAUTOINCREMENT, 1);
2496         dmi_write(target, DMI_SBCS, sbcs);
2497
2498         target_addr_t next_address = address;
2499         target_addr_t end_address = address + count * size;
2500
2501         sb_write_address(target, next_address);
2502         while (next_address < end_address) {
2503                 for (uint32_t i = (next_address - address) / size; i < count; i++) {
2504                         const uint8_t *p = buffer + i * size;
2505                         if (size > 12)
2506                                 dmi_write(target, DMI_SBDATA3,
2507                                                 ((uint32_t) p[12]) |
2508                                                 (((uint32_t) p[13]) << 8) |
2509                                                 (((uint32_t) p[14]) << 16) |
2510                                                 (((uint32_t) p[15]) << 24));
2511                         if (size > 8)
2512                                 dmi_write(target, DMI_SBDATA2,
2513                                                 ((uint32_t) p[8]) |
2514                                                 (((uint32_t) p[9]) << 8) |
2515                                                 (((uint32_t) p[10]) << 16) |
2516                                                 (((uint32_t) p[11]) << 24));
2517                         if (size > 4)
2518                                 dmi_write(target, DMI_SBDATA1,
2519                                                 ((uint32_t) p[4]) |
2520                                                 (((uint32_t) p[5]) << 8) |
2521                                                 (((uint32_t) p[6]) << 16) |
2522                                                 (((uint32_t) p[7]) << 24));
2523                         uint32_t value = p[0];
2524                         if (size > 2) {
2525                                 value |= ((uint32_t) p[2]) << 16;
2526                                 value |= ((uint32_t) p[3]) << 24;
2527                         }
2528                         if (size > 1)
2529                                 value |= ((uint32_t) p[1]) << 8;
2530                         dmi_write(target, DMI_SBDATA0, value);
2531
2532                         log_memory_access(address + i * size, value, size, false);
2533
2534                         if (info->bus_master_write_delay) {
2535                                 jtag_add_runtest(info->bus_master_write_delay, TAP_IDLE);
2536                                 if (jtag_execute_queue() != ERROR_OK) {
2537                                         LOG_ERROR("Failed to scan idle sequence");
2538                                         return ERROR_FAIL;
2539                                 }
2540                         }
2541                 }
2542
2543                 if (read_sbcs_nonbusy(target, &sbcs) != ERROR_OK)
2544                         return ERROR_FAIL;
2545
2546                 if (get_field(sbcs, DMI_SBCS_SBBUSYERROR)) {
2547                         /* We wrote while the target was busy. Slow down and try again. */
2548                         dmi_write(target, DMI_SBCS, DMI_SBCS_SBBUSYERROR);
2549                         next_address = sb_read_address(target);
2550                         info->bus_master_write_delay += info->bus_master_write_delay / 10 + 1;
2551                         continue;
2552                 }
2553
2554                 unsigned error = get_field(sbcs, DMI_SBCS_SBERROR);
2555                 if (error == 0) {
2556                         next_address = end_address;
2557                 } else {
2558                         /* Some error indicating the bus access failed, but not because of
2559                          * something we did wrong. */
2560                         dmi_write(target, DMI_SBCS, DMI_SBCS_SBERROR);
2561                         return ERROR_FAIL;
2562                 }
2563         }
2564
2565         return ERROR_OK;
2566 }
2567
2568 static int write_memory_progbuf(struct target *target, target_addr_t address,
2569                 uint32_t size, uint32_t count, const uint8_t *buffer)
2570 {
2571         RISCV013_INFO(info);
2572
2573         LOG_DEBUG("writing %d words of %d bytes to 0x%08lx", count, size, (long)address);
2574
2575         select_dmi(target);
2576
2577         /* s0 holds the next address to write to
2578          * s1 holds the next data value to write
2579          */
2580
2581         int result = ERROR_OK;
2582         uint64_t s0, s1;
2583         if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
2584                 return ERROR_FAIL;
2585         if (register_read(target, &s1, GDB_REGNO_S1) != ERROR_OK)
2586                 return ERROR_FAIL;
2587
2588         /* Write the program (store, increment) */
2589         struct riscv_program program;
2590         riscv_program_init(&program, target);
2591
2592         switch (size) {
2593                 case 1:
2594                         riscv_program_sbr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2595                         break;
2596                 case 2:
2597                         riscv_program_shr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2598                         break;
2599                 case 4:
2600                         riscv_program_swr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2601                         break;
2602                 default:
2603                         LOG_ERROR("Unsupported size: %d", size);
2604                         result = ERROR_FAIL;
2605                         goto error;
2606         }
2607
2608         riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
2609
2610         result = riscv_program_ebreak(&program);
2611         if (result != ERROR_OK)
2612                 goto error;
2613         riscv_program_write(&program);
2614
2615         riscv_addr_t cur_addr = address;
2616         riscv_addr_t fin_addr = address + (count * size);
2617         bool setup_needed = true;
2618         LOG_DEBUG("writing until final address 0x%016" PRIx64, fin_addr);
2619         while (cur_addr < fin_addr) {
2620                 LOG_DEBUG("transferring burst starting at address 0x%016" PRIx64,
2621                                 cur_addr);
2622
2623                 struct riscv_batch *batch = riscv_batch_alloc(
2624                                 target,
2625                                 32,
2626                                 info->dmi_busy_delay + info->ac_busy_delay);
2627
2628                 /* To write another word, we put it in S1 and execute the program. */
2629                 unsigned start = (cur_addr - address) / size;
2630                 for (unsigned i = start; i < count; ++i) {
2631                         unsigned offset = size*i;
2632                         const uint8_t *t_buffer = buffer + offset;
2633
2634                         uint32_t value;
2635                         switch (size) {
2636                                 case 1:
2637                                         value = t_buffer[0];
2638                                         break;
2639                                 case 2:
2640                                         value = t_buffer[0]
2641                                                 | ((uint32_t) t_buffer[1] << 8);
2642                                         break;
2643                                 case 4:
2644                                         value = t_buffer[0]
2645                                                 | ((uint32_t) t_buffer[1] << 8)
2646                                                 | ((uint32_t) t_buffer[2] << 16)
2647                                                 | ((uint32_t) t_buffer[3] << 24);
2648                                         break;
2649                                 default:
2650                                         LOG_ERROR("unsupported access size: %d", size);
2651                                         riscv_batch_free(batch);
2652                                         result = ERROR_FAIL;
2653                                         goto error;
2654                         }
2655
2656                         log_memory_access(address + offset, value, size, false);
2657                         cur_addr += size;
2658
2659                         if (setup_needed) {
2660                                 result = register_write_direct(target, GDB_REGNO_S0,
2661                                                 address + offset);
2662                                 if (result != ERROR_OK) {
2663                                         riscv_batch_free(batch);
2664                                         goto error;
2665                                 }
2666
2667                                 /* Write value. */
2668                                 dmi_write(target, DMI_DATA0, value);
2669
2670                                 /* Write and execute command that moves value into S1 and
2671                                  * executes program buffer. */
2672                                 uint32_t command = access_register_command(target,
2673                                                 GDB_REGNO_S1, 32,
2674                                                 AC_ACCESS_REGISTER_POSTEXEC |
2675                                                 AC_ACCESS_REGISTER_TRANSFER |
2676                                                 AC_ACCESS_REGISTER_WRITE);
2677                                 result = execute_abstract_command(target, command);
2678                                 if (result != ERROR_OK) {
2679                                         riscv_batch_free(batch);
2680                                         goto error;
2681                                 }
2682
2683                                 /* Turn on autoexec */
2684                                 dmi_write(target, DMI_ABSTRACTAUTO,
2685                                                 1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
2686
2687                                 setup_needed = false;
2688                         } else {
2689                                 riscv_batch_add_dmi_write(batch, DMI_DATA0, value);
2690                                 if (riscv_batch_full(batch))
2691                                         break;
2692                         }
2693                 }
2694
2695                 result = batch_run(target, batch);
2696                 riscv_batch_free(batch);
2697                 if (result != ERROR_OK)
2698                         goto error;
2699
2700                 /* Note that if the scan resulted in a Busy DMI response, it
2701                  * is this read to abstractcs that will cause the dmi_busy_delay
2702                  * to be incremented if necessary. */
2703
2704                 uint32_t abstractcs;
2705                 bool dmi_busy_encountered;
2706                 if (dmi_op(target, &abstractcs, &dmi_busy_encountered, DMI_OP_READ,
2707                                         DMI_ABSTRACTCS, 0, false) != ERROR_OK)
2708                         goto error;
2709                 while (get_field(abstractcs, DMI_ABSTRACTCS_BUSY))
2710                         if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
2711                                 return ERROR_FAIL;
2712                 info->cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
2713                 if (info->cmderr == CMDERR_NONE && !dmi_busy_encountered) {
2714                         LOG_DEBUG("successful (partial?) memory write");
2715                 } else if (info->cmderr == CMDERR_BUSY || dmi_busy_encountered) {
2716                         if (info->cmderr == CMDERR_BUSY)
2717                                 LOG_DEBUG("Memory write resulted in abstract command busy response.");
2718                         else if (dmi_busy_encountered)
2719                                 LOG_DEBUG("Memory write resulted in DMI busy response.");
2720                         riscv013_clear_abstract_error(target);
2721                         increase_ac_busy_delay(target);
2722
2723                         dmi_write(target, DMI_ABSTRACTAUTO, 0);
2724                         result = register_read_direct(target, &cur_addr, GDB_REGNO_S0);
2725                         if (result != ERROR_OK)
2726                                 goto error;
2727                         setup_needed = true;
2728                 } else {
2729                         LOG_ERROR("error when writing memory, abstractcs=0x%08lx", (long)abstractcs);
2730                         riscv013_clear_abstract_error(target);
2731                         result = ERROR_FAIL;
2732                         goto error;
2733                 }
2734         }
2735
2736 error:
2737         dmi_write(target, DMI_ABSTRACTAUTO, 0);
2738
2739         if (register_write_direct(target, GDB_REGNO_S1, s1) != ERROR_OK)
2740                 return ERROR_FAIL;
2741         if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
2742                 return ERROR_FAIL;
2743
2744         if (execute_fence(target) != ERROR_OK)
2745                 return ERROR_FAIL;
2746
2747         return result;
2748 }
2749
2750 static int write_memory(struct target *target, target_addr_t address,
2751                 uint32_t size, uint32_t count, const uint8_t *buffer)
2752 {
2753         RISCV013_INFO(info);
2754         if (info->progbufsize >= 2 && !riscv_prefer_sba)
2755                 return write_memory_progbuf(target, address, size, count, buffer);
2756
2757         if ((get_field(info->sbcs, DMI_SBCS_SBACCESS8) && size == 1) ||
2758                         (get_field(info->sbcs, DMI_SBCS_SBACCESS16) && size == 2) ||
2759                         (get_field(info->sbcs, DMI_SBCS_SBACCESS32) && size == 4) ||
2760                         (get_field(info->sbcs, DMI_SBCS_SBACCESS64) && size == 8) ||
2761                         (get_field(info->sbcs, DMI_SBCS_SBACCESS128) && size == 16)) {
2762                 if (get_field(info->sbcs, DMI_SBCS_SBVERSION) == 0)
2763                         return write_memory_bus_v0(target, address, size, count, buffer);
2764                 else if (get_field(info->sbcs, DMI_SBCS_SBVERSION) == 1)
2765                         return write_memory_bus_v1(target, address, size, count, buffer);
2766         }
2767
2768         if (info->progbufsize >= 2)
2769                 return write_memory_progbuf(target, address, size, count, buffer);
2770
2771         LOG_ERROR("Don't know how to write memory on this target.");
2772         return ERROR_FAIL;
2773 }
2774
2775 static int arch_state(struct target *target)
2776 {
2777         return ERROR_OK;
2778 }
2779
2780 struct target_type riscv013_target = {
2781         .name = "riscv",
2782
2783         .init_target = init_target,
2784         .deinit_target = deinit_target,
2785         .examine = examine,
2786
2787         .poll = &riscv_openocd_poll,
2788         .halt = &riscv_openocd_halt,
2789         .resume = &riscv_openocd_resume,
2790         .step = &riscv_openocd_step,
2791
2792         .assert_reset = assert_reset,
2793         .deassert_reset = deassert_reset,
2794
2795         .read_memory = read_memory,
2796         .write_memory = write_memory,
2797
2798         .arch_state = arch_state,
2799 };
2800
2801 /*** 0.13-specific implementations of various RISC-V helper functions. ***/
2802 static int riscv013_get_register(struct target *target,
2803                 riscv_reg_t *value, int hid, int rid)
2804 {
2805         LOG_DEBUG("reading register %s on hart %d", gdb_regno_name(rid), hid);
2806
2807         riscv_set_current_hartid(target, hid);
2808
2809         int result = ERROR_OK;
2810         if (rid == GDB_REGNO_PC) {
2811                 result = register_read(target, value, GDB_REGNO_DPC);
2812                 LOG_DEBUG("read PC from DPC: 0x%" PRIx64, *value);
2813         } else if (rid == GDB_REGNO_PRIV) {
2814                 uint64_t dcsr;
2815                 result = register_read(target, &dcsr, GDB_REGNO_DCSR);
2816                 *value = get_field(dcsr, CSR_DCSR_PRV);
2817         } else {
2818                 result = register_read(target, value, rid);
2819                 if (result != ERROR_OK)
2820                         *value = -1;
2821         }
2822
2823         return result;
2824 }
2825
2826 static int riscv013_set_register(struct target *target, int hid, int rid, uint64_t value)
2827 {
2828         LOG_DEBUG("writing 0x%" PRIx64 " to register %s on hart %d", value,
2829                         gdb_regno_name(rid), hid);
2830
2831         riscv_set_current_hartid(target, hid);
2832
2833         if (rid <= GDB_REGNO_XPR31) {
2834                 return register_write_direct(target, rid, value);
2835         } else if (rid == GDB_REGNO_PC) {
2836                 LOG_DEBUG("writing PC to DPC: 0x%" PRIx64, value);
2837                 register_write_direct(target, GDB_REGNO_DPC, value);
2838                 uint64_t actual_value;
2839                 register_read_direct(target, &actual_value, GDB_REGNO_DPC);
2840                 LOG_DEBUG("  actual DPC written: 0x%016" PRIx64, actual_value);
2841                 if (value != actual_value) {
2842                         LOG_ERROR("Written PC (0x%" PRIx64 ") does not match read back "
2843                                         "value (0x%" PRIx64 ")", value, actual_value);
2844                         return ERROR_FAIL;
2845                 }
2846         } else if (rid == GDB_REGNO_PRIV) {
2847                 uint64_t dcsr;
2848                 register_read(target, &dcsr, GDB_REGNO_DCSR);
2849                 dcsr = set_field(dcsr, CSR_DCSR_PRV, value);
2850                 return register_write_direct(target, GDB_REGNO_DCSR, dcsr);
2851         } else {
2852                 return register_write_direct(target, rid, value);
2853         }
2854
2855         return ERROR_OK;
2856 }
2857
2858 static int riscv013_select_current_hart(struct target *target)
2859 {
2860         RISCV_INFO(r);
2861
2862         dm013_info_t *dm = get_dm(target);
2863         if (r->current_hartid == dm->current_hartid)
2864                 return ERROR_OK;
2865
2866         uint32_t dmcontrol;
2867         /* TODO: can't we just "dmcontrol = DMI_DMACTIVE"? */
2868         if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
2869                 return ERROR_FAIL;
2870         dmcontrol = set_hartsel(dmcontrol, r->current_hartid);
2871         int result = dmi_write(target, DMI_DMCONTROL, dmcontrol);
2872         dm->current_hartid = r->current_hartid;
2873         return result;
2874 }
2875
2876 static int riscv013_halt_current_hart(struct target *target)
2877 {
2878         RISCV_INFO(r);
2879         LOG_DEBUG("halting hart %d", r->current_hartid);
2880         if (riscv_is_halted(target))
2881                 LOG_ERROR("Hart %d is already halted!", r->current_hartid);
2882
2883         /* Issue the halt command, and then wait for the current hart to halt. */
2884         uint32_t dmcontrol;
2885         if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
2886                 return ERROR_FAIL;
2887         dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HALTREQ, 1);
2888         dmi_write(target, DMI_DMCONTROL, dmcontrol);
2889         for (size_t i = 0; i < 256; ++i)
2890                 if (riscv_is_halted(target))
2891                         break;
2892
2893         if (!riscv_is_halted(target)) {
2894                 uint32_t dmstatus;
2895                 if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
2896                         return ERROR_FAIL;
2897                 if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
2898                         return ERROR_FAIL;
2899
2900                 LOG_ERROR("unable to halt hart %d", r->current_hartid);
2901                 LOG_ERROR("  dmcontrol=0x%08x", dmcontrol);
2902                 LOG_ERROR("  dmstatus =0x%08x", dmstatus);
2903                 return ERROR_FAIL;
2904         }
2905
2906         dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HALTREQ, 0);
2907         dmi_write(target, DMI_DMCONTROL, dmcontrol);
2908
2909         return ERROR_OK;
2910 }
2911
2912 static int riscv013_resume_current_hart(struct target *target)
2913 {
2914         return riscv013_step_or_resume_current_hart(target, false);
2915 }
2916
2917 static int riscv013_step_current_hart(struct target *target)
2918 {
2919         return riscv013_step_or_resume_current_hart(target, true);
2920 }
2921
2922 static int riscv013_on_resume(struct target *target)
2923 {
2924         return riscv013_on_step_or_resume(target, false);
2925 }
2926
2927 static int riscv013_on_step(struct target *target)
2928 {
2929         return riscv013_on_step_or_resume(target, true);
2930 }
2931
2932 static int riscv013_on_halt(struct target *target)
2933 {
2934         return ERROR_OK;
2935 }
2936
2937 static bool riscv013_is_halted(struct target *target)
2938 {
2939         uint32_t dmstatus;
2940         if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
2941                 return false;
2942         if (get_field(dmstatus, DMI_DMSTATUS_ANYUNAVAIL))
2943                 LOG_ERROR("Hart %d is unavailable.", riscv_current_hartid(target));
2944         if (get_field(dmstatus, DMI_DMSTATUS_ANYNONEXISTENT))
2945                 LOG_ERROR("Hart %d doesn't exist.", riscv_current_hartid(target));
2946         if (get_field(dmstatus, DMI_DMSTATUS_ANYHAVERESET)) {
2947                 int hartid = riscv_current_hartid(target);
2948                 LOG_INFO("Hart %d unexpectedly reset!", hartid);
2949                 /* TODO: Can we make this more obvious to eg. a gdb user? */
2950                 uint32_t dmcontrol = DMI_DMCONTROL_DMACTIVE |
2951                         DMI_DMCONTROL_ACKHAVERESET;
2952                 dmcontrol = set_hartsel(dmcontrol, hartid);
2953                 /* If we had been halted when we reset, request another halt. If we
2954                  * ended up running out of reset, then the user will (hopefully) get a
2955                  * message that a reset happened, that the target is running, and then
2956                  * that it is halted again once the request goes through.
2957                  */
2958                 if (target->state == TARGET_HALTED)
2959                         dmcontrol |= DMI_DMCONTROL_HALTREQ;
2960                 dmi_write(target, DMI_DMCONTROL, dmcontrol);
2961         }
2962         return get_field(dmstatus, DMI_DMSTATUS_ALLHALTED);
2963 }
2964
2965 static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
2966 {
2967         riscv_reg_t dcsr;
2968         int result = register_read(target, &dcsr, GDB_REGNO_DCSR);
2969         if (result != ERROR_OK)
2970                 return RISCV_HALT_UNKNOWN;
2971
2972         switch (get_field(dcsr, CSR_DCSR_CAUSE)) {
2973         case CSR_DCSR_CAUSE_SWBP:
2974                 return RISCV_HALT_BREAKPOINT;
2975         case CSR_DCSR_CAUSE_TRIGGER:
2976                 /* We could get here before triggers are enumerated if a trigger was
2977                  * already set when we connected. Force enumeration now, which has the
2978                  * side effect of clearing any triggers we did not set. */
2979                 riscv_enumerate_triggers(target);
2980                 LOG_DEBUG("{%d} halted because of trigger", target->coreid);
2981                 return RISCV_HALT_TRIGGER;
2982         case CSR_DCSR_CAUSE_STEP:
2983                 return RISCV_HALT_SINGLESTEP;
2984         case CSR_DCSR_CAUSE_DEBUGINT:
2985         case CSR_DCSR_CAUSE_HALT:
2986                 return RISCV_HALT_INTERRUPT;
2987         }
2988
2989         LOG_ERROR("Unknown DCSR cause field: %x", (int)get_field(dcsr, CSR_DCSR_CAUSE));
2990         LOG_ERROR("  dcsr=0x%016lx", (long)dcsr);
2991         return RISCV_HALT_UNKNOWN;
2992 }
2993
2994 int riscv013_write_debug_buffer(struct target *target, unsigned index, riscv_insn_t data)
2995 {
2996         return dmi_write(target, DMI_PROGBUF0 + index, data);
2997 }
2998
2999 riscv_insn_t riscv013_read_debug_buffer(struct target *target, unsigned index)
3000 {
3001         uint32_t value;
3002         dmi_read(target, &value, DMI_PROGBUF0 + index);
3003         return value;
3004 }
3005
3006 int riscv013_execute_debug_buffer(struct target *target)
3007 {
3008         uint32_t run_program = 0;
3009         run_program = set_field(run_program, AC_ACCESS_REGISTER_SIZE, 2);
3010         run_program = set_field(run_program, AC_ACCESS_REGISTER_POSTEXEC, 1);
3011         run_program = set_field(run_program, AC_ACCESS_REGISTER_TRANSFER, 0);
3012         run_program = set_field(run_program, AC_ACCESS_REGISTER_REGNO, 0x1000);
3013
3014         return execute_abstract_command(target, run_program);
3015 }
3016
3017 void riscv013_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d)
3018 {
3019         RISCV013_INFO(info);
3020         buf_set_u64((unsigned char *)buf, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_WRITE);
3021         buf_set_u64((unsigned char *)buf, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, d);
3022         buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, a);
3023 }
3024
3025 void riscv013_fill_dmi_read_u64(struct target *target, char *buf, int a)
3026 {
3027         RISCV013_INFO(info);
3028         buf_set_u64((unsigned char *)buf, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_READ);
3029         buf_set_u64((unsigned char *)buf, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, 0);
3030         buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, a);
3031 }
3032
3033 void riscv013_fill_dmi_nop_u64(struct target *target, char *buf)
3034 {
3035         RISCV013_INFO(info);
3036         buf_set_u64((unsigned char *)buf, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_NOP);
3037         buf_set_u64((unsigned char *)buf, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, 0);
3038         buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, 0);
3039 }
3040
3041 /* Helper function for riscv013_test_sba_config_reg */
3042 static int get_max_sbaccess(struct target *target)
3043 {
3044         RISCV013_INFO(info);
3045
3046         uint32_t sbaccess128 = get_field(info->sbcs, DMI_SBCS_SBACCESS128);
3047         uint32_t sbaccess64 = get_field(info->sbcs, DMI_SBCS_SBACCESS64);
3048         uint32_t sbaccess32 = get_field(info->sbcs, DMI_SBCS_SBACCESS32);
3049         uint32_t sbaccess16 = get_field(info->sbcs, DMI_SBCS_SBACCESS16);
3050         uint32_t sbaccess8 = get_field(info->sbcs, DMI_SBCS_SBACCESS8);
3051
3052         if (sbaccess128)
3053                 return 4;
3054         else if (sbaccess64)
3055                 return 3;
3056         else if (sbaccess32)
3057                 return 2;
3058         else if (sbaccess16)
3059                 return 1;
3060         else if (sbaccess8)
3061                 return 0;
3062         else
3063                 return -1;
3064 }
3065
3066 static uint32_t get_num_sbdata_regs(struct target *target)
3067 {
3068         RISCV013_INFO(info);
3069
3070         uint32_t sbaccess128 = get_field(info->sbcs, DMI_SBCS_SBACCESS128);
3071         uint32_t sbaccess64 = get_field(info->sbcs, DMI_SBCS_SBACCESS64);
3072         uint32_t sbaccess32 = get_field(info->sbcs, DMI_SBCS_SBACCESS32);
3073
3074         if (sbaccess128)
3075                 return 4;
3076         else if (sbaccess64)
3077                 return 2;
3078         else if (sbaccess32)
3079                 return 1;
3080         else
3081                 return 0;
3082 }
3083
3084 static int riscv013_test_sba_config_reg(struct target *target,
3085                 target_addr_t legal_address, uint32_t num_words,
3086                 target_addr_t illegal_address, bool run_sbbusyerror_test)
3087 {
3088         LOG_INFO("Testing System Bus Access as defined by RISC-V Debug Spec v0.13");
3089
3090         uint32_t tests_failed = 0;
3091
3092         uint32_t rd_val;
3093         uint32_t sbcs_orig;
3094         dmi_read(target, &sbcs_orig, DMI_SBCS);
3095
3096         uint32_t sbcs = sbcs_orig;
3097         bool test_passed;
3098
3099         int max_sbaccess = get_max_sbaccess(target);
3100
3101         if (max_sbaccess == -1) {
3102                 LOG_ERROR("System Bus Access not supported in this config.");
3103                 return ERROR_FAIL;
3104         }
3105
3106         if (get_field(sbcs, DMI_SBCS_SBVERSION) != 1) {
3107                 LOG_ERROR("System Bus Access unsupported SBVERSION (%d). Only version 1 is supported.",
3108                                 get_field(sbcs, DMI_SBCS_SBVERSION));
3109                 return ERROR_FAIL;
3110         }
3111
3112         uint32_t num_sbdata_regs = get_num_sbdata_regs(target);
3113
3114         uint32_t rd_buf[num_sbdata_regs];
3115
3116         /* Test 1: Simple write/read test */
3117         test_passed = true;
3118         sbcs = set_field(sbcs_orig, DMI_SBCS_SBAUTOINCREMENT, 0);
3119         dmi_write(target, DMI_SBCS, sbcs);
3120
3121         uint32_t test_patterns[4] = {0xdeadbeef, 0xfeedbabe, 0x12345678, 0x08675309};
3122         for (uint32_t sbaccess = 0; sbaccess <= (uint32_t)max_sbaccess; sbaccess++) {
3123                 sbcs = set_field(sbcs, DMI_SBCS_SBACCESS, sbaccess);
3124                 dmi_write(target, DMI_SBCS, sbcs);
3125
3126                 uint32_t compare_mask = (sbaccess == 0) ? 0xff : (sbaccess == 1) ? 0xffff : 0xffffffff;
3127
3128                 for (uint32_t i = 0; i < num_words; i++) {
3129                         uint32_t addr = legal_address + (i << sbaccess);
3130                         uint32_t wr_data[num_sbdata_regs];
3131                         for (uint32_t j = 0; j < num_sbdata_regs; j++)
3132                                 wr_data[j] = test_patterns[j] + i;
3133                         write_memory_sba_simple(target, addr, wr_data, num_sbdata_regs, sbcs);
3134                 }
3135
3136                 for (uint32_t i = 0; i < num_words; i++) {
3137                         uint32_t addr = legal_address + (i << sbaccess);
3138                         read_memory_sba_simple(target, addr, rd_buf, num_sbdata_regs, sbcs);
3139                         for (uint32_t j = 0; j < num_sbdata_regs; j++) {
3140                                 if (((test_patterns[j]+i)&compare_mask) != (rd_buf[j]&compare_mask)) {
3141                                         LOG_ERROR("System Bus Access Test 1: Error reading non-autoincremented address %x,"
3142                                                         "expected val = %x, read val = %x", addr, test_patterns[j]+i, rd_buf[j]);
3143                                         test_passed = false;
3144                                         tests_failed++;
3145                                 }
3146                         }
3147                 }
3148         }
3149         if (test_passed)
3150                 LOG_INFO("System Bus Access Test 1: Simple write/read test PASSED.");
3151
3152         /* Test 2: Address autoincrement test */
3153         target_addr_t curr_addr;
3154         target_addr_t prev_addr;
3155         test_passed = true;
3156         sbcs = set_field(sbcs_orig, DMI_SBCS_SBAUTOINCREMENT, 1);
3157         dmi_write(target, DMI_SBCS, sbcs);
3158
3159         for (uint32_t sbaccess = 0; sbaccess <= (uint32_t)max_sbaccess; sbaccess++) {
3160                 sbcs = set_field(sbcs, DMI_SBCS_SBACCESS, sbaccess);
3161                 dmi_write(target, DMI_SBCS, sbcs);
3162
3163                 dmi_write(target, DMI_SBADDRESS0, legal_address);
3164                 read_sbcs_nonbusy(target, &sbcs);
3165                 curr_addr = legal_address;
3166                 for (uint32_t i = 0; i < num_words; i++) {
3167                         prev_addr = curr_addr;
3168                         read_sbcs_nonbusy(target, &sbcs);
3169                         curr_addr = sb_read_address(target);
3170                         if ((curr_addr - prev_addr != (uint32_t)(1 << sbaccess)) && (i != 0)) {
3171                                 LOG_ERROR("System Bus Access Test 2: Error with address auto-increment, sbaccess = %x.", sbaccess);
3172                                 test_passed = false;
3173                                 tests_failed++;
3174                         }
3175                         dmi_write(target, DMI_SBDATA0, i);
3176                 }
3177
3178                 read_sbcs_nonbusy(target, &sbcs);
3179
3180                 dmi_write(target, DMI_SBADDRESS0, legal_address);
3181
3182                 uint32_t val;
3183                 sbcs = set_field(sbcs, DMI_SBCS_SBREADONDATA, 1);
3184                 dmi_write(target, DMI_SBCS, sbcs);
3185                 dmi_read(target, &val, DMI_SBDATA0); /* Dummy read to trigger first system bus read */
3186                 curr_addr = legal_address;
3187                 for (uint32_t i = 0; i < num_words; i++) {
3188                         prev_addr = curr_addr;
3189                         read_sbcs_nonbusy(target, &sbcs);
3190                         curr_addr = sb_read_address(target);
3191                         if ((curr_addr - prev_addr != (uint32_t)(1 << sbaccess)) && (i != 0)) {
3192                                 LOG_ERROR("System Bus Access Test 2: Error with address auto-increment, sbaccess = %x", sbaccess);
3193                                 test_passed = false;
3194                                 tests_failed++;
3195                         }
3196                         dmi_read(target, &val, DMI_SBDATA0);
3197                         read_sbcs_nonbusy(target, &sbcs);
3198                         if (i != val) {
3199                                 LOG_ERROR("System Bus Access Test 2: Error reading auto-incremented address,"
3200                                                 "expected val = %x, read val = %x.", i, val);
3201                                 test_passed = false;
3202                                 tests_failed++;
3203                         }
3204                 }
3205         }
3206         if (test_passed)
3207                 LOG_INFO("System Bus Access Test 2: Address auto-increment test PASSED.");
3208
3209         /* Test 3: Read from illegal address */
3210         read_memory_sba_simple(target, illegal_address, rd_buf, 1, sbcs_orig);
3211
3212         dmi_read(target, &rd_val, DMI_SBCS);
3213         if (get_field(rd_val, DMI_SBCS_SBERROR) == 2) {
3214                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 2);
3215                 dmi_write(target, DMI_SBCS, sbcs);
3216                 dmi_read(target, &rd_val, DMI_SBCS);
3217                 if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
3218                         LOG_INFO("System Bus Access Test 3: Illegal address read test PASSED.");
3219                 else
3220                         LOG_ERROR("System Bus Access Test 3: Illegal address read test FAILED, unable to clear to 0.");
3221         } else {
3222                 LOG_ERROR("System Bus Access Test 3: Illegal address read test FAILED, unable to set error code.");
3223         }
3224
3225         /* Test 4: Write to illegal address */
3226         write_memory_sba_simple(target, illegal_address, test_patterns, 1, sbcs_orig);
3227
3228         dmi_read(target, &rd_val, DMI_SBCS);
3229         if (get_field(rd_val, DMI_SBCS_SBERROR) == 2) {
3230                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 2);
3231                 dmi_write(target, DMI_SBCS, sbcs);
3232                 dmi_read(target, &rd_val, DMI_SBCS);
3233                 if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
3234                         LOG_INFO("System Bus Access Test 4: Illegal address write test PASSED.");
3235                 else {
3236                         LOG_ERROR("System Bus Access Test 4: Illegal address write test FAILED, unable to clear to 0.");
3237                         tests_failed++;
3238                 }
3239         } else {
3240                 LOG_ERROR("System Bus Access Test 4: Illegal address write test FAILED, unable to set error code.");
3241                 tests_failed++;
3242         }
3243
3244         /* Test 5: Write with unsupported sbaccess size */
3245         uint32_t sbaccess128 = get_field(sbcs_orig, DMI_SBCS_SBACCESS128);
3246
3247         if (sbaccess128) {
3248                 LOG_INFO("System Bus Access Test 5: SBCS sbaccess error test PASSED, all sbaccess sizes supported.");
3249         } else {
3250                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBACCESS, 4);
3251
3252                 write_memory_sba_simple(target, legal_address, test_patterns, 1, sbcs);
3253
3254                 dmi_read(target, &rd_val, DMI_SBCS);
3255                 if (get_field(rd_val, DMI_SBCS_SBERROR) == 4) {
3256                         sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 4);
3257                         dmi_write(target, DMI_SBCS, sbcs);
3258                         dmi_read(target, &rd_val, DMI_SBCS);
3259                         if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
3260                                 LOG_INFO("System Bus Access Test 5: SBCS sbaccess error test PASSED.");
3261                         else {
3262                                 LOG_ERROR("System Bus Access Test 5: SBCS sbaccess error test FAILED, unable to clear to 0.");
3263                                 tests_failed++;
3264                         }
3265                 } else {
3266                         LOG_ERROR("System Bus Access Test 5: SBCS sbaccess error test FAILED, unable to set error code.");
3267                         tests_failed++;
3268                 }
3269         }
3270
3271         /* Test 6: Write to misaligned address */
3272         sbcs = set_field(sbcs_orig, DMI_SBCS_SBACCESS, 1);
3273
3274         write_memory_sba_simple(target, legal_address+1, test_patterns, 1, sbcs);
3275
3276         dmi_read(target, &rd_val, DMI_SBCS);
3277         if (get_field(rd_val, DMI_SBCS_SBERROR) == 3) {
3278                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 3);
3279                 dmi_write(target, DMI_SBCS, sbcs);
3280                 dmi_read(target, &rd_val, DMI_SBCS);
3281                 if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
3282                         LOG_INFO("System Bus Access Test 6: SBCS address alignment error test PASSED");
3283                 else {
3284                         LOG_ERROR("System Bus Access Test 6: SBCS address alignment error test FAILED, unable to clear to 0.");
3285                         tests_failed++;
3286                 }
3287         } else {
3288                 LOG_ERROR("System Bus Access Test 6: SBCS address alignment error test FAILED, unable to set error code.");
3289                 tests_failed++;
3290         }
3291
3292         /* Test 7: Set sbbusyerror, only run this case in simulation as it is likely
3293          * impossible to hit otherwise */
3294         if (run_sbbusyerror_test) {
3295                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBREADONADDR, 1);
3296                 dmi_write(target, DMI_SBCS, sbcs);
3297
3298                 for (int i = 0; i < 16; i++)
3299                         dmi_write(target, DMI_SBDATA0, 0xdeadbeef);
3300
3301                 for (int i = 0; i < 16; i++)
3302                         dmi_write(target, DMI_SBADDRESS0, legal_address);
3303
3304                 dmi_read(target, &rd_val, DMI_SBCS);
3305                 if (get_field(rd_val, DMI_SBCS_SBBUSYERROR)) {
3306                         sbcs = set_field(sbcs_orig, DMI_SBCS_SBBUSYERROR, 1);
3307                         dmi_write(target, DMI_SBCS, sbcs);
3308                         dmi_read(target, &rd_val, DMI_SBCS);
3309                         if (get_field(rd_val, DMI_SBCS_SBBUSYERROR) == 0)
3310                                 LOG_INFO("System Bus Access Test 7: SBCS sbbusyerror test PASSED.");
3311                         else {
3312                                 LOG_ERROR("System Bus Access Test 7: SBCS sbbusyerror test FAILED, unable to clear to 0.");
3313                                 tests_failed++;
3314                         }
3315                 } else {
3316                         LOG_ERROR("System Bus Access Test 7: SBCS sbbusyerror test FAILED, unable to set error code.");
3317                         tests_failed++;
3318                 }
3319         }
3320
3321         if (tests_failed == 0) {
3322                 LOG_INFO("ALL TESTS PASSED");
3323                 return ERROR_OK;
3324         } else {
3325                 LOG_ERROR("%d TESTS FAILED", tests_failed);
3326                 return ERROR_FAIL;
3327         }
3328
3329 }
3330
3331 void write_memory_sba_simple(struct target *target, target_addr_t addr,
3332                 uint32_t *write_data, uint32_t write_size, uint32_t sbcs)
3333 {
3334         RISCV013_INFO(info);
3335
3336         uint32_t rd_sbcs;
3337         uint32_t masked_addr;
3338
3339         uint32_t sba_size = get_field(info->sbcs, DMI_SBCS_SBASIZE);
3340
3341         read_sbcs_nonbusy(target, &rd_sbcs);
3342
3343         uint32_t sbcs_no_readonaddr = set_field(sbcs, DMI_SBCS_SBREADONADDR, 0);
3344         dmi_write(target, DMI_SBCS, sbcs_no_readonaddr);
3345
3346         for (uint32_t i = 0; i < sba_size/32; i++) {
3347                 masked_addr = (addr >> 32*i) & 0xffffffff;
3348
3349                 if (i != 3)
3350                         dmi_write(target, DMI_SBADDRESS0+i, masked_addr);
3351                 else
3352                         dmi_write(target, DMI_SBADDRESS3, masked_addr);
3353         }
3354
3355         /* Write SBDATA registers starting with highest address, since write to
3356          * SBDATA0 triggers write */
3357         for (int i = write_size-1; i >= 0; i--)
3358                 dmi_write(target, DMI_SBDATA0+i, write_data[i]);
3359 }
3360
3361 void read_memory_sba_simple(struct target *target, target_addr_t addr,
3362                 uint32_t *rd_buf, uint32_t read_size, uint32_t sbcs)
3363 {
3364         RISCV013_INFO(info);
3365
3366         uint32_t rd_sbcs;
3367         uint32_t masked_addr;
3368
3369         uint32_t sba_size = get_field(info->sbcs, DMI_SBCS_SBASIZE);
3370
3371         read_sbcs_nonbusy(target, &rd_sbcs);
3372
3373         uint32_t sbcs_readonaddr = set_field(sbcs, DMI_SBCS_SBREADONADDR, 1);
3374         dmi_write(target, DMI_SBCS, sbcs_readonaddr);
3375
3376         /* Write addresses starting with highest address register */
3377         for (int i = sba_size/32-1; i >= 0; i--) {
3378                 masked_addr = (addr >> 32*i) & 0xffffffff;
3379
3380                 if (i != 3)
3381                         dmi_write(target, DMI_SBADDRESS0+i, masked_addr);
3382                 else
3383                         dmi_write(target, DMI_SBADDRESS3, masked_addr);
3384         }
3385
3386         read_sbcs_nonbusy(target, &rd_sbcs);
3387
3388         for (uint32_t i = 0; i < read_size; i++)
3389                 dmi_read(target, &(rd_buf[i]), DMI_SBDATA0+i);
3390 }
3391
3392 int riscv013_dmi_write_u64_bits(struct target *target)
3393 {
3394         RISCV013_INFO(info);
3395         return info->abits + DTM_DMI_DATA_LENGTH + DTM_DMI_OP_LENGTH;
3396 }
3397
3398 static int maybe_execute_fence_i(struct target *target)
3399 {
3400         RISCV013_INFO(info);
3401         RISCV_INFO(r);
3402         if (info->progbufsize + r->impebreak >= 3)
3403                 return execute_fence(target);
3404         return ERROR_OK;
3405 }
3406
3407 /* Helper Functions. */
3408 static int riscv013_on_step_or_resume(struct target *target, bool step)
3409 {
3410         if (maybe_execute_fence_i(target) != ERROR_OK)
3411                 return ERROR_FAIL;
3412
3413         /* We want to twiddle some bits in the debug CSR so debugging works. */
3414         riscv_reg_t dcsr;
3415         int result = register_read(target, &dcsr, GDB_REGNO_DCSR);
3416         if (result != ERROR_OK)
3417                 return result;
3418         dcsr = set_field(dcsr, CSR_DCSR_STEP, step);
3419         dcsr = set_field(dcsr, CSR_DCSR_EBREAKM, 1);
3420         dcsr = set_field(dcsr, CSR_DCSR_EBREAKS, 1);
3421         dcsr = set_field(dcsr, CSR_DCSR_EBREAKU, 1);
3422         return riscv_set_register(target, GDB_REGNO_DCSR, dcsr);
3423 }
3424
3425 static int riscv013_step_or_resume_current_hart(struct target *target, bool step)
3426 {
3427         RISCV_INFO(r);
3428         LOG_DEBUG("resuming hart %d (for step?=%d)", r->current_hartid, step);
3429         if (!riscv_is_halted(target)) {
3430                 LOG_ERROR("Hart %d is not halted!", r->current_hartid);
3431                 return ERROR_FAIL;
3432         }
3433
3434         if (maybe_execute_fence_i(target) != ERROR_OK)
3435                 return ERROR_FAIL;
3436
3437         /* Issue the resume command, and then wait for the current hart to resume. */
3438         uint32_t dmcontrol = DMI_DMCONTROL_DMACTIVE;
3439         dmcontrol = set_hartsel(dmcontrol, r->current_hartid);
3440         dmi_write(target, DMI_DMCONTROL, dmcontrol | DMI_DMCONTROL_RESUMEREQ);
3441
3442         uint32_t dmstatus;
3443         for (size_t i = 0; i < 256; ++i) {
3444                 usleep(10);
3445                 if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
3446                         return ERROR_FAIL;
3447                 if (get_field(dmstatus, DMI_DMSTATUS_ALLRESUMEACK) == 0)
3448                         continue;
3449                 if (step && get_field(dmstatus, DMI_DMSTATUS_ALLHALTED) == 0)
3450                         continue;
3451
3452                 dmi_write(target, DMI_DMCONTROL, dmcontrol);
3453                 return ERROR_OK;
3454         }
3455
3456         LOG_ERROR("unable to resume hart %d", r->current_hartid);
3457         if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
3458                 return ERROR_FAIL;
3459         LOG_ERROR("  dmcontrol=0x%08x", dmcontrol);
3460         if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
3461                 return ERROR_FAIL;
3462         LOG_ERROR("  dmstatus =0x%08x", dmstatus);
3463
3464         if (step) {
3465                 LOG_ERROR("  was stepping, halting");
3466                 riscv013_halt_current_hart(target);
3467                 return ERROR_OK;
3468         }
3469
3470         return ERROR_FAIL;
3471 }
3472
3473 void riscv013_clear_abstract_error(struct target *target)
3474 {
3475         /* Wait for busy to go away. */
3476         time_t start = time(NULL);
3477         uint32_t abstractcs;
3478         dmi_read(target, &abstractcs, DMI_ABSTRACTCS);
3479         while (get_field(abstractcs, DMI_ABSTRACTCS_BUSY)) {
3480                 dmi_read(target, &abstractcs, DMI_ABSTRACTCS);
3481
3482                 if (time(NULL) - start > riscv_command_timeout_sec) {
3483                         LOG_ERROR("abstractcs.busy is not going low after %d seconds "
3484                                         "(abstractcs=0x%x). The target is either really slow or "
3485                                         "broken. You could increase the timeout with riscv "
3486                                         "set_command_timeout_sec.",
3487                                         riscv_command_timeout_sec, abstractcs);
3488                         break;
3489                 }
3490         }
3491         /* Clear the error status. */
3492         dmi_write(target, DMI_ABSTRACTCS, abstractcs & DMI_ABSTRACTCS_CMDERR);
3493 }
3494
3495 #define COMPLIANCE_TEST(b, message) \
3496 {                                   \
3497         int pass = 0;               \
3498         if (b) {                    \
3499                 pass = 1;           \
3500                 passed_tests++;     \
3501         }                           \
3502         LOG_INFO("%s test %d (%s)\n", (pass) ? "PASSED" : "FAILED",  total_tests, message); \
3503         assert(pass);               \
3504         total_tests++;              \
3505 }
3506
3507 #define COMPLIANCE_MUST_PASS(b) COMPLIANCE_TEST(ERROR_OK == (b), "Regular calls must return ERROR_OK")
3508
3509 #define COMPLIANCE_READ(target, addr, value) COMPLIANCE_MUST_PASS(dmi_read(target, addr, value))
3510 #define COMPLIANCE_WRITE(target, addr, value) COMPLIANCE_MUST_PASS(dmi_write(target, addr, value))
3511
3512 #define COMPLIANCE_CHECK_RO(target, addr)                               \
3513 {                                                                       \
3514         uint32_t orig;                                                      \
3515         uint32_t inverse;                                                   \
3516         COMPLIANCE_READ(target, &orig, addr);                               \
3517         COMPLIANCE_WRITE(target, addr, ~orig);                              \
3518         COMPLIANCE_READ(target, &inverse, addr);                            \
3519         COMPLIANCE_TEST(orig == inverse, "Register must be read-only");     \
3520 }
3521
3522 int riscv013_test_compliance(struct target *target)
3523 {
3524         LOG_INFO("Testing Compliance against RISC-V Debug Spec v0.13");
3525
3526         if (!riscv_rtos_enabled(target)) {
3527                 LOG_ERROR("Please run with -rtos riscv to run compliance test.");
3528                 return ERROR_FAIL;
3529         }
3530
3531         int total_tests = 0;
3532         int passed_tests = 0;
3533
3534         uint32_t dmcontrol_orig = DMI_DMCONTROL_DMACTIVE;
3535         uint32_t dmcontrol;
3536         uint32_t testvar;
3537         uint32_t testvar_read;
3538         riscv_reg_t value;
3539         RISCV013_INFO(info);
3540
3541         /* All the bits of HARTSEL are covered by the examine sequence. */
3542
3543         /* hartreset */
3544         /* This field is optional. Either we can read and write it to 1/0,
3545         or it is tied to 0. This check doesn't really do anything, but
3546         it does attempt to set the bit to 1 and then back to 0, which needs to
3547         work if its implemented. */
3548         COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HARTRESET, 1));
3549         COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HARTRESET, 0));
3550         COMPLIANCE_READ(target, &dmcontrol, DMI_DMCONTROL);
3551         COMPLIANCE_TEST((get_field(dmcontrol, DMI_DMCONTROL_HARTRESET) == 0),
3552                         "DMCONTROL.hartreset can be 0 or RW.");
3553
3554         /* hasel */
3555         COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HASEL, 1));
3556         COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HASEL, 0));
3557         COMPLIANCE_READ(target, &dmcontrol, DMI_DMCONTROL);
3558         COMPLIANCE_TEST((get_field(dmcontrol, DMI_DMCONTROL_HASEL) == 0),
3559                         "DMCONTROL.hasel can be 0 or RW.");
3560         /* TODO: test that hamask registers exist if hasel does. */
3561
3562         /* haltreq */
3563         COMPLIANCE_MUST_PASS(riscv_halt_all_harts(target));
3564         /* This bit is not actually readable according to the spec, so nothing to check.*/
3565
3566         /* DMSTATUS */
3567         COMPLIANCE_CHECK_RO(target, DMI_DMSTATUS);
3568
3569         /* resumereq */
3570         /* This bit is not actually readable according to the spec, so nothing to check.*/
3571         COMPLIANCE_MUST_PASS(riscv_resume_all_harts(target));
3572
3573         /* Halt all harts again so the test can continue.*/
3574         COMPLIANCE_MUST_PASS(riscv_halt_all_harts(target));
3575
3576         /* HARTINFO: Read-Only. This is per-hart, so need to adjust hartsel. */
3577         uint32_t hartinfo;
3578         COMPLIANCE_READ(target, &hartinfo, DMI_HARTINFO);
3579         for (int hartsel = 0; hartsel < riscv_count_harts(target); hartsel++) {
3580                 COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, hartsel));
3581
3582                 COMPLIANCE_CHECK_RO(target, DMI_HARTINFO);
3583
3584                 /* $dscratch CSRs */
3585                 uint32_t nscratch = get_field(hartinfo, DMI_HARTINFO_NSCRATCH);
3586                 for (unsigned int d = 0; d < nscratch; d++) {
3587                         riscv_reg_t testval, testval_read;
3588                         /* Because DSCRATCH is not guaranteed to last across PB executions, need to put
3589                         this all into one PB execution. Which may not be possible on all implementations.*/
3590                         if (info->progbufsize >= 5) {
3591                                 for (testval = 0x0011223300112233;
3592                                                  testval != 0xDEAD;
3593                                                  testval = testval == 0x0011223300112233 ? ~testval : 0xDEAD) {
3594                                         COMPLIANCE_TEST(register_write_direct(target, GDB_REGNO_S0, testval) == ERROR_OK,
3595                                                         "Need to be able to write S0 in order to test DSCRATCH.");
3596                                         struct riscv_program program32;
3597                                         riscv_program_init(&program32, target);
3598                                         riscv_program_csrw(&program32, GDB_REGNO_S0, GDB_REGNO_DSCRATCH + d);
3599                                         riscv_program_csrr(&program32, GDB_REGNO_S1, GDB_REGNO_DSCRATCH + d);
3600                                         riscv_program_fence(&program32);
3601                                         riscv_program_ebreak(&program32);
3602                                         COMPLIANCE_TEST(riscv_program_exec(&program32, target) == ERROR_OK,
3603                                                         "Accessing DSCRATCH with program buffer should succeed.");
3604                                         COMPLIANCE_TEST(register_read_direct(target, &testval_read, GDB_REGNO_S1) == ERROR_OK,
3605                                                         "Need to be able to read S1 in order to test DSCRATCH.");
3606                                         if (riscv_xlen(target) > 32) {
3607                                                 COMPLIANCE_TEST(testval == testval_read,
3608                                                                 "All DSCRATCH registers in HARTINFO must be R/W.");
3609                                         } else {
3610                                                 COMPLIANCE_TEST(testval_read == (testval & 0xFFFFFFFF),
3611                                                                 "All DSCRATCH registers in HARTINFO must be R/W.");
3612                                         }
3613                                 }
3614                         }
3615                 }
3616                 /* TODO: dataaccess */
3617                 if (get_field(hartinfo, DMI_HARTINFO_DATAACCESS)) {
3618                         /* TODO: Shadowed in memory map. */
3619                         /* TODO: datasize */
3620                         /* TODO: dataaddr */
3621                 } else {
3622                         /* TODO: Shadowed in CSRs. */
3623                         /* TODO: datasize */
3624                         /* TODO: dataaddr */
3625                 }
3626
3627         }
3628
3629         /* HALTSUM -- TODO: More than 32 harts. Would need to loop over this to set hartsel */
3630         /* TODO: HALTSUM2, HALTSUM3 */
3631         /* HALTSUM0 */
3632         uint32_t expected_haltsum0 = 0;
3633         for (int i = 0; i < MIN(riscv_count_harts(target), 32); i++)
3634                 expected_haltsum0 |= (1 << i);
3635
3636         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM0);
3637         COMPLIANCE_TEST(testvar_read == expected_haltsum0,
3638                         "HALTSUM0 should report summary of up to 32 halted harts");
3639
3640         COMPLIANCE_WRITE(target, DMI_HALTSUM0, 0xffffffff);
3641         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM0);
3642         COMPLIANCE_TEST(testvar_read == expected_haltsum0, "HALTSUM0 should be R/O");
3643
3644         COMPLIANCE_WRITE(target, DMI_HALTSUM0, 0x0);
3645         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM0);
3646         COMPLIANCE_TEST(testvar_read == expected_haltsum0, "HALTSUM0 should be R/O");
3647
3648         /* HALTSUM1 */
3649         uint32_t expected_haltsum1 = 0;
3650         for (int i = 0; i < MIN(riscv_count_harts(target), 1024); i += 32)
3651                 expected_haltsum1 |= (1 << (i/32));
3652
3653         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM1);
3654         COMPLIANCE_TEST(testvar_read == expected_haltsum1,
3655                         "HALTSUM1 should report summary of up to 1024 halted harts");
3656
3657         COMPLIANCE_WRITE(target, DMI_HALTSUM1, 0xffffffff);
3658         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM1);
3659         COMPLIANCE_TEST(testvar_read == expected_haltsum1, "HALTSUM1 should be R/O");
3660
3661         COMPLIANCE_WRITE(target, DMI_HALTSUM1, 0x0);
3662         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM1);
3663         COMPLIANCE_TEST(testvar_read == expected_haltsum1, "HALTSUM1 should be R/O");
3664
3665         /* TODO: HAWINDOWSEL */
3666
3667         /* TODO: HAWINDOW */
3668
3669         /* ABSTRACTCS */
3670
3671         uint32_t abstractcs;
3672         COMPLIANCE_READ(target, &abstractcs, DMI_ABSTRACTCS);
3673
3674         /* Check that all reported Data Words are really R/W */
3675         for (int invert = 0; invert < 2; invert++) {
3676                 for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
3677                         testvar = (i + 1) * 0x11111111;
3678                         if (invert)
3679                                 testvar = ~testvar;
3680                         COMPLIANCE_WRITE(target, DMI_DATA0 + i, testvar);
3681                 }
3682                 for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
3683                         testvar = (i + 1) * 0x11111111;
3684                         if (invert)
3685                                 testvar = ~testvar;
3686                         COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
3687                         COMPLIANCE_TEST(testvar_read == testvar, "All reported DATA words must be R/W");
3688                 }
3689         }
3690
3691         /* Check that all reported ProgBuf words are really R/W */
3692         for (int invert = 0; invert < 2; invert++) {
3693                 for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
3694                         testvar = (i + 1) * 0x11111111;
3695                         if (invert)
3696                                 testvar = ~testvar;
3697                         COMPLIANCE_WRITE(target, DMI_PROGBUF0 + i, testvar);
3698                 }
3699                 for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
3700                         testvar = (i + 1) * 0x11111111;
3701                         if (invert)
3702                                 testvar = ~testvar;
3703                         COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
3704                         COMPLIANCE_TEST(testvar_read == testvar, "All reported PROGBUF words must be R/W");
3705                 }
3706         }
3707
3708         /* TODO: Cause and clear all error types */
3709
3710         /* COMMAND
3711         According to the spec, this register is only W, so can't really check the read result.
3712         But at any rate, this is not legal and should cause an error. */
3713         COMPLIANCE_WRITE(target, DMI_COMMAND, 0xAAAAAAAA);
3714         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3715         COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR) == CMDERR_NOT_SUPPORTED,
3716                         "Illegal COMMAND should result in UNSUPPORTED");
3717         COMPLIANCE_WRITE(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
3718
3719         COMPLIANCE_WRITE(target, DMI_COMMAND, 0x55555555);
3720         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3721         COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR) == CMDERR_NOT_SUPPORTED,
3722                         "Illegal COMMAND should result in UNSUPPORTED");
3723         COMPLIANCE_WRITE(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
3724
3725         /* Basic Abstract Commands */
3726         for (unsigned int i = 1; i < 32; i = i << 1) {
3727                 riscv_reg_t testval =   i | ((i + 1ULL) << 32);
3728                 riscv_reg_t testval_read;
3729                 COMPLIANCE_TEST(ERROR_OK == register_write_direct(target, GDB_REGNO_ZERO + i, testval),
3730                                 "GPR Writes should be supported.");
3731                 COMPLIANCE_MUST_PASS(write_abstract_arg(target, 0, 0xDEADBEEFDEADBEEF, 64));
3732                 COMPLIANCE_TEST(ERROR_OK == register_read_direct(target, &testval_read, GDB_REGNO_ZERO + i),
3733                                 "GPR Reads should be supported.");
3734                 if (riscv_xlen(target) > 32) {
3735                         /* Dummy comment to satisfy linter, since removing the brances here doesn't actually compile. */
3736                         COMPLIANCE_TEST(testval == testval_read, "GPR Reads and writes should be supported.");
3737                 } else {
3738                         /* Dummy comment to satisfy linter, since removing the brances here doesn't actually compile. */
3739                         COMPLIANCE_TEST((testval & 0xFFFFFFFF) == testval_read, "GPR Reads and writes should be supported.");
3740                 }
3741         }
3742
3743         /* ABSTRACTAUTO
3744         See which bits are actually writable */
3745         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0xFFFFFFFF);
3746         uint32_t abstractauto;
3747         uint32_t busy;
3748         COMPLIANCE_READ(target, &abstractauto, DMI_ABSTRACTAUTO);
3749         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0x0);
3750         if (abstractauto > 0) {
3751                 /* This mechanism only works when you have a reasonable sized progbuf, which is not
3752                 a true compliance requirement. */
3753                 if (info->progbufsize >= 3) {
3754
3755                         testvar = 0;
3756                         COMPLIANCE_TEST(ERROR_OK == register_write_direct(target, GDB_REGNO_S0, 0),
3757                                         "Need to be able to write S0 to test ABSTRACTAUTO");
3758                         struct riscv_program program;
3759                         COMPLIANCE_MUST_PASS(riscv_program_init(&program, target));
3760                         /* This is also testing that WFI() is a NOP during debug mode. */
3761                         COMPLIANCE_MUST_PASS(riscv_program_insert(&program, wfi()));
3762                         COMPLIANCE_MUST_PASS(riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, 1));
3763                         COMPLIANCE_MUST_PASS(riscv_program_ebreak(&program));
3764                         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0x0);
3765                         COMPLIANCE_MUST_PASS(riscv_program_exec(&program, target));
3766                         testvar++;
3767                         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0xFFFFFFFF);
3768                         COMPLIANCE_READ(target, &abstractauto, DMI_ABSTRACTAUTO);
3769                         uint32_t autoexec_data = get_field(abstractauto, DMI_ABSTRACTAUTO_AUTOEXECDATA);
3770                         uint32_t autoexec_progbuf = get_field(abstractauto, DMI_ABSTRACTAUTO_AUTOEXECPROGBUF);
3771                         for (unsigned int i = 0; i < 12; i++) {
3772                                 COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
3773                                 do {
3774                                         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3775                                         busy = get_field(testvar_read, DMI_ABSTRACTCS_BUSY);
3776                                 } while (busy);
3777                                 if (autoexec_data & (1 << i)) {
3778                                         COMPLIANCE_TEST(i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT),
3779                                                         "AUTOEXEC may be writable up to DATACOUNT bits.");
3780                                         testvar++;
3781                                 }
3782                         }
3783                         for (unsigned int i = 0; i < 16; i++) {
3784                                 COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
3785                                 do {
3786                                         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3787                                         busy = get_field(testvar_read, DMI_ABSTRACTCS_BUSY);
3788                                 } while (busy);
3789                                 if (autoexec_progbuf & (1 << i)) {
3790                                         COMPLIANCE_TEST(i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE),
3791                                                         "AUTOEXEC may be writable up to PROGBUFSIZE bits.");
3792                                         testvar++;
3793                                 }
3794                         }
3795
3796                         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0);
3797                         COMPLIANCE_TEST(ERROR_OK == register_read_direct(target, &value, GDB_REGNO_S0),
3798                                         "Need to be able to read S0 to test ABSTRACTAUTO");
3799
3800                         COMPLIANCE_TEST(testvar == value,
3801                                         "ABSTRACTAUTO should cause COMMAND to run the expected number of times.");
3802                 }
3803         }
3804
3805         /* Single-Step each hart. */
3806         for (int hartsel = 0; hartsel < riscv_count_harts(target); hartsel++) {
3807                 COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, hartsel));
3808                 COMPLIANCE_MUST_PASS(riscv013_on_step(target));
3809                 COMPLIANCE_MUST_PASS(riscv013_step_current_hart(target));
3810                 COMPLIANCE_TEST(riscv_halt_reason(target, hartsel) == RISCV_HALT_SINGLESTEP,
3811                                 "Single Step should result in SINGLESTEP");
3812         }
3813
3814         /* Core Register Tests */
3815         uint64_t bogus_dpc = 0xdeadbeef;
3816         for (int hartsel = 0; hartsel < riscv_count_harts(target); hartsel++) {
3817                 COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, hartsel));
3818
3819                 /* DCSR Tests */
3820                 COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DCSR, 0x0));
3821                 COMPLIANCE_MUST_PASS(register_read_direct(target, &value, GDB_REGNO_DCSR));
3822                 COMPLIANCE_TEST(value != 0,     "Not all bits in DCSR are writable by Debugger");
3823                 COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DCSR, 0xFFFFFFFF));
3824                 COMPLIANCE_MUST_PASS(register_read_direct(target, &value, GDB_REGNO_DCSR));
3825                 COMPLIANCE_TEST(value != 0,     "At least some bits in DCSR must be 1");
3826
3827                 /* DPC. Note that DPC is sign-extended. */
3828                 riscv_reg_t dpcmask = 0xFFFFFFFCUL;
3829                 riscv_reg_t dpc;
3830
3831                 if (riscv_xlen(target) > 32)
3832                         dpcmask |= (0xFFFFFFFFULL << 32);
3833
3834                 if (riscv_supports_extension(target, riscv_current_hartid(target), 'C'))
3835                         dpcmask |= 0x2;
3836
3837                 COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DPC, dpcmask));
3838                 COMPLIANCE_MUST_PASS(register_read_direct(target, &dpc, GDB_REGNO_DPC));
3839                 COMPLIANCE_TEST(dpcmask == dpc,
3840                                 "DPC must be sign-extended to XLEN and writable to all-1s (except the least significant bits)");
3841                 COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DPC, 0));
3842                 COMPLIANCE_MUST_PASS(register_read_direct(target, &dpc, GDB_REGNO_DPC));
3843                 COMPLIANCE_TEST(dpc == 0, "DPC must be writable to 0.");
3844                 if (hartsel == 0)
3845                         bogus_dpc = dpc; /* For a later test step */
3846         }
3847
3848         /* NDMRESET
3849         Asserting non-debug module reset should not reset Debug Module state.
3850         But it should reset Hart State, e.g. DPC should get a different value.
3851         Also make sure that DCSR reports cause of 'HALT' even though previously we single-stepped.
3852         */
3853
3854         /* Write some registers. They should not be impacted by ndmreset. */
3855         COMPLIANCE_WRITE(target, DMI_COMMAND, 0xFFFFFFFF);
3856
3857         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
3858                 testvar = (i + 1) * 0x11111111;
3859                 COMPLIANCE_WRITE(target, DMI_PROGBUF0 + i, testvar);
3860         }
3861
3862         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
3863                 testvar = (i + 1) * 0x11111111;
3864                 COMPLIANCE_WRITE(target, DMI_DATA0 + i, testvar);
3865         }
3866
3867         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0xFFFFFFFF);
3868         COMPLIANCE_READ(target, &abstractauto, DMI_ABSTRACTAUTO);
3869
3870         /* Pulse reset. */
3871         target->reset_halt = true;
3872         COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, 0));
3873         COMPLIANCE_TEST(ERROR_OK == assert_reset(target), "Must be able to assert NDMRESET");
3874         COMPLIANCE_TEST(ERROR_OK == deassert_reset(target), "Must be able to deassert NDMRESET");
3875
3876         /* Verify that most stuff is not affected by ndmreset. */
3877         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3878         COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR)  == CMDERR_NOT_SUPPORTED,
3879                         "NDMRESET should not affect DMI_ABSTRACTCS");
3880         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTAUTO);
3881         COMPLIANCE_TEST(testvar_read == abstractauto, "NDMRESET should not affect DMI_ABSTRACTAUTO");
3882
3883         /* Clean up to avoid future test failures */
3884         COMPLIANCE_WRITE(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
3885         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0);
3886
3887         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
3888                 testvar = (i + 1) * 0x11111111;
3889                 COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
3890                 COMPLIANCE_TEST(testvar_read == testvar, "PROGBUF words must not be affected by NDMRESET");
3891         }
3892
3893         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
3894                 testvar = (i + 1) * 0x11111111;
3895                 COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
3896                 COMPLIANCE_TEST(testvar_read == testvar, "DATA words must not be affected by NDMRESET");
3897         }
3898
3899         /* Verify that DPC *is* affected by ndmreset. Since we don't know what it *should* be,
3900         just verify that at least it's not the bogus value anymore. */
3901
3902         COMPLIANCE_TEST(bogus_dpc != 0xdeadbeef, "BOGUS DPC should have been set somehow (bug in compliance test)");
3903         COMPLIANCE_MUST_PASS(register_read_direct(target, &value, GDB_REGNO_DPC));
3904         COMPLIANCE_TEST(bogus_dpc != value, "NDMRESET should move DPC to reset value.");
3905
3906         COMPLIANCE_TEST(riscv_halt_reason(target, 0) == RISCV_HALT_INTERRUPT,
3907                         "After NDMRESET halt, DCSR should report cause of halt");
3908
3909         /* DMACTIVE -- deasserting DMACTIVE should reset all the above values. */
3910
3911         /* Toggle dmactive */
3912         COMPLIANCE_WRITE(target, DMI_DMCONTROL, 0);
3913         COMPLIANCE_WRITE(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
3914         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3915         COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR)  == 0, "ABSTRACTCS.cmderr should reset to 0");
3916         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTAUTO);
3917         COMPLIANCE_TEST(testvar_read == 0, "ABSTRACTAUTO should reset to 0");
3918
3919         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
3920                 COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
3921                 COMPLIANCE_TEST(testvar_read == 0, "PROGBUF words should reset to 0");
3922         }
3923
3924         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
3925                 COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
3926                 COMPLIANCE_TEST(testvar_read == 0, "DATA words should reset to 0");
3927         }
3928
3929         /*
3930         * TODO:
3931         * DCSR.cause priorities
3932         * DCSR.stoptime/stopcycle
3933         * DCSR.stepie
3934         * DCSR.ebreak
3935         * DCSR.prv
3936         */
3937
3938         /* Halt every hart for any follow-up tests*/
3939         COMPLIANCE_MUST_PASS(riscv_halt_all_harts(target));
3940
3941         uint32_t failed_tests = total_tests - passed_tests;
3942         if (total_tests == passed_tests) {
3943                 LOG_INFO("ALL TESTS PASSED\n");
3944                 return ERROR_OK;
3945         } else {
3946                 LOG_INFO("%d TESTS FAILED\n", failed_tests);
3947                 return ERROR_FAIL;
3948         }
3949 }