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