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                 dmi_read(target, &v, DMI_SBADDRESS1);
1896                 address |= v;
1897                 address <<= 32;
1898         }
1899         dmi_read(target, &v, DMI_SBADDRESS0);
1900         address |= v;
1901         return address;
1902 }
1903
1904 static int sb_write_address(struct target *target, target_addr_t address)
1905 {
1906         RISCV013_INFO(info);
1907         unsigned sbasize = get_field(info->sbcs, DMI_SBCS_SBASIZE);
1908         /* There currently is no support for >64-bit addresses in OpenOCD. */
1909         if (sbasize > 96)
1910                 dmi_write(target, DMI_SBADDRESS3, 0);
1911         if (sbasize > 64)
1912                 dmi_write(target, DMI_SBADDRESS2, 0);
1913         if (sbasize > 32)
1914                 dmi_write(target, DMI_SBADDRESS1, address >> 32);
1915         return dmi_write(target, DMI_SBADDRESS0, address);
1916 }
1917
1918 static int read_sbcs_nonbusy(struct target *target, uint32_t *sbcs)
1919 {
1920         time_t start = time(NULL);
1921         while (1) {
1922                 if (dmi_read(target, sbcs, DMI_SBCS) != ERROR_OK)
1923                         return ERROR_FAIL;
1924                 if (!get_field(*sbcs, DMI_SBCS_SBBUSY))
1925                         return ERROR_OK;
1926                 if (time(NULL) - start > riscv_command_timeout_sec) {
1927                         LOG_ERROR("Timed out after %ds waiting for sbbusy to go low (sbcs=0x%x). "
1928                                         "Increase the timeout with riscv set_command_timeout_sec.",
1929                                         riscv_command_timeout_sec, *sbcs);
1930                         return ERROR_FAIL;
1931                 }
1932         }
1933 }
1934
1935 static int read_memory_bus_v0(struct target *target, target_addr_t address,
1936                 uint32_t size, uint32_t count, uint8_t *buffer)
1937 {
1938         LOG_DEBUG("System Bus Access: size: %d\tcount:%d\tstart address: 0x%08"
1939                         TARGET_PRIxADDR, size, count, address);
1940         uint8_t *t_buffer = buffer;
1941         riscv_addr_t cur_addr = address;
1942         riscv_addr_t fin_addr = address + (count * size);
1943         uint32_t access = 0;
1944
1945         const int DMI_SBCS_SBSINGLEREAD_OFFSET = 20;
1946         const uint32_t DMI_SBCS_SBSINGLEREAD = (0x1U << DMI_SBCS_SBSINGLEREAD_OFFSET);
1947
1948         const int DMI_SBCS_SBAUTOREAD_OFFSET = 15;
1949         const uint32_t DMI_SBCS_SBAUTOREAD = (0x1U << DMI_SBCS_SBAUTOREAD_OFFSET);
1950
1951         /* ww favorise one off reading if there is an issue */
1952         if (count == 1) {
1953                 for (uint32_t i = 0; i < count; i++) {
1954                         if (dmi_read(target, &access, DMI_SBCS) != ERROR_OK)
1955                                 return ERROR_FAIL;
1956                         dmi_write(target, DMI_SBADDRESS0, cur_addr);
1957                         /* size/2 matching the bit access of the spec 0.13 */
1958                         access = set_field(access, DMI_SBCS_SBACCESS, size/2);
1959                         access = set_field(access, DMI_SBCS_SBSINGLEREAD, 1);
1960                         LOG_DEBUG("\r\nread_memory: sab: access:  0x%08x", access);
1961                         dmi_write(target, DMI_SBCS, access);
1962                         /* 3) read */
1963                         uint32_t value;
1964                         if (dmi_read(target, &value, DMI_SBDATA0) != ERROR_OK)
1965                                 return ERROR_FAIL;
1966                         LOG_DEBUG("\r\nread_memory: sab: value:  0x%08x", value);
1967                         write_to_buf(t_buffer, value, size);
1968                         t_buffer += size;
1969                         cur_addr += size;
1970                 }
1971                 return ERROR_OK;
1972         }
1973
1974         /* has to be the same size if we want to read a block */
1975         LOG_DEBUG("reading block until final address 0x%" PRIx64, fin_addr);
1976         if (dmi_read(target, &access, DMI_SBCS) != ERROR_OK)
1977                 return ERROR_FAIL;
1978         /* set current address */
1979         dmi_write(target, DMI_SBADDRESS0, cur_addr);
1980         /* 2) write sbaccess=2, sbsingleread,sbautoread,sbautoincrement
1981          * size/2 matching the bit access of the spec 0.13 */
1982         access = set_field(access, DMI_SBCS_SBACCESS, size/2);
1983         access = set_field(access, DMI_SBCS_SBAUTOREAD, 1);
1984         access = set_field(access, DMI_SBCS_SBSINGLEREAD, 1);
1985         access = set_field(access, DMI_SBCS_SBAUTOINCREMENT, 1);
1986         LOG_DEBUG("\r\naccess:  0x%08x", access);
1987         dmi_write(target, DMI_SBCS, access);
1988
1989         while (cur_addr < fin_addr) {
1990                 LOG_DEBUG("\r\nsab:autoincrement: \r\n size: %d\tcount:%d\taddress: 0x%08"
1991                                 PRIx64, size, count, cur_addr);
1992                 /* read */
1993                 uint32_t value;
1994                 if (dmi_read(target, &value, DMI_SBDATA0) != ERROR_OK)
1995                         return ERROR_FAIL;
1996                 write_to_buf(t_buffer, value, size);
1997                 cur_addr += size;
1998                 t_buffer += size;
1999
2000                 /* if we are reaching last address, we must clear autoread */
2001                 if (cur_addr == fin_addr && count != 1) {
2002                         dmi_write(target, DMI_SBCS, 0);
2003                         if (dmi_read(target, &value, DMI_SBDATA0) != ERROR_OK)
2004                                 return ERROR_FAIL;
2005                         write_to_buf(t_buffer, value, size);
2006                 }
2007         }
2008
2009         return ERROR_OK;
2010 }
2011
2012 /**
2013  * Read the requested memory using the system bus interface.
2014  */
2015 static int read_memory_bus_v1(struct target *target, target_addr_t address,
2016                 uint32_t size, uint32_t count, uint8_t *buffer)
2017 {
2018         RISCV013_INFO(info);
2019         target_addr_t next_address = address;
2020         target_addr_t end_address = address + count * size;
2021
2022         while (next_address < end_address) {
2023                 uint32_t sbcs = set_field(0, DMI_SBCS_SBREADONADDR, 1);
2024                 sbcs |= sb_sbaccess(size);
2025                 sbcs = set_field(sbcs, DMI_SBCS_SBAUTOINCREMENT, 1);
2026                 sbcs = set_field(sbcs, DMI_SBCS_SBREADONDATA, count > 1);
2027                 dmi_write(target, DMI_SBCS, sbcs);
2028
2029                 /* This address write will trigger the first read. */
2030                 sb_write_address(target, next_address);
2031
2032                 if (info->bus_master_read_delay) {
2033                         jtag_add_runtest(info->bus_master_read_delay, TAP_IDLE);
2034                         if (jtag_execute_queue() != ERROR_OK) {
2035                                 LOG_ERROR("Failed to scan idle sequence");
2036                                 return ERROR_FAIL;
2037                         }
2038                 }
2039
2040                 for (uint32_t i = (next_address - address) / size; i < count - 1; i++) {
2041                         read_memory_bus_word(target, address + i * size, size,
2042                                         buffer + i * size);
2043                 }
2044
2045                 sbcs = set_field(sbcs, DMI_SBCS_SBREADONDATA, 0);
2046                 dmi_write(target, DMI_SBCS, sbcs);
2047
2048                 read_memory_bus_word(target, address + (count - 1) * size, size,
2049                                 buffer + (count - 1) * size);
2050
2051                 if (read_sbcs_nonbusy(target, &sbcs) != ERROR_OK)
2052                         return ERROR_FAIL;
2053
2054                 if (get_field(sbcs, DMI_SBCS_SBBUSYERROR)) {
2055                         /* We read while the target was busy. Slow down and try again. */
2056                         dmi_write(target, DMI_SBCS, DMI_SBCS_SBBUSYERROR);
2057                         next_address = sb_read_address(target);
2058                         info->bus_master_read_delay += info->bus_master_read_delay / 10 + 1;
2059                         continue;
2060                 }
2061
2062                 unsigned error = get_field(sbcs, DMI_SBCS_SBERROR);
2063                 if (error == 0) {
2064                         next_address = end_address;
2065                 } else {
2066                         /* Some error indicating the bus access failed, but not because of
2067                          * something we did wrong. */
2068                         dmi_write(target, DMI_SBCS, DMI_SBCS_SBERROR);
2069                         return ERROR_FAIL;
2070                 }
2071         }
2072
2073         return ERROR_OK;
2074 }
2075
2076 static int batch_run(const struct target *target, struct riscv_batch *batch)
2077 {
2078         RISCV013_INFO(info);
2079         RISCV_INFO(r);
2080         if (r->reset_delays_wait >= 0) {
2081                 r->reset_delays_wait -= batch->used_scans;
2082                 if (r->reset_delays_wait <= 0) {
2083                         batch->idle_count = 0;
2084                         info->dmi_busy_delay = 0;
2085                         info->ac_busy_delay = 0;
2086                 }
2087         }
2088         return riscv_batch_run(batch);
2089 }
2090
2091 /**
2092  * Read the requested memory, taking care to execute every read exactly once,
2093  * even if cmderr=busy is encountered.
2094  */
2095 static int read_memory_progbuf_inner(struct target *target, target_addr_t address,
2096                 uint32_t size, uint32_t count, uint8_t *buffer)
2097 {
2098         RISCV013_INFO(info);
2099
2100         int result = ERROR_OK;
2101
2102         /* Write address to S0, and execute buffer. */
2103         result = register_write_direct(target, GDB_REGNO_S0, address);
2104         if (result != ERROR_OK)
2105                 goto error;
2106         uint32_t command = access_register_command(target, GDB_REGNO_S1,
2107                         riscv_xlen(target),
2108                         AC_ACCESS_REGISTER_TRANSFER | AC_ACCESS_REGISTER_POSTEXEC);
2109         if (execute_abstract_command(target, command) != ERROR_OK)
2110                 return ERROR_FAIL;
2111
2112         /* First read has just triggered. Result is in s1. */
2113
2114         if (count == 1) {
2115                 uint64_t value;
2116                 if (register_read_direct(target, &value, GDB_REGNO_S1) != ERROR_OK)
2117                         return ERROR_FAIL;
2118                 write_to_buf(buffer, value, size);
2119                 log_memory_access(address, value, size, true);
2120                 return ERROR_OK;
2121         }
2122
2123         if (dmi_write(target, DMI_ABSTRACTAUTO,
2124                         1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET) != ERROR_OK)
2125                 goto error;
2126         /* Read garbage from dmi_data0, which triggers another execution of the
2127          * program. Now dmi_data0 contains the first good result, and s1 the next
2128          * memory value. */
2129         if (dmi_read_exec(target, NULL, DMI_DATA0) != ERROR_OK)
2130                 goto error;
2131
2132         /* read_addr is the next address that the hart will read from, which is the
2133          * value in s0. */
2134         riscv_addr_t read_addr = address + 2 * size;
2135         riscv_addr_t fin_addr = address + (count * size);
2136         while (read_addr < fin_addr) {
2137                 LOG_DEBUG("read_addr=0x%" PRIx64 ", fin_addr=0x%" PRIx64, read_addr,
2138                                 fin_addr);
2139                 /* The pipeline looks like this:
2140                  * memory -> s1 -> dm_data0 -> debugger
2141                  * Right now:
2142                  * s0 contains read_addr
2143                  * s1 contains mem[read_addr-size]
2144                  * dm_data0 contains[read_addr-size*2]
2145                  */
2146
2147                 LOG_DEBUG("creating burst to read from 0x%" PRIx64
2148                                 " up to 0x%" PRIx64, read_addr, fin_addr);
2149                 assert(read_addr >= address && read_addr < fin_addr);
2150                 struct riscv_batch *batch = riscv_batch_alloc(target, 32,
2151                                 info->dmi_busy_delay + info->ac_busy_delay);
2152
2153                 size_t reads = 0;
2154                 for (riscv_addr_t addr = read_addr; addr < fin_addr; addr += size) {
2155                         riscv_batch_add_dmi_read(batch, DMI_DATA0);
2156
2157                         reads++;
2158                         if (riscv_batch_full(batch))
2159                                 break;
2160                 }
2161
2162                 batch_run(target, batch);
2163
2164                 /* Wait for the target to finish performing the last abstract command,
2165                  * and update our copy of cmderr. If we see that DMI is busy here,
2166                  * dmi_busy_delay will be incremented. */
2167                 uint32_t abstractcs;
2168                 if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
2169                         return ERROR_FAIL;
2170                 while (get_field(abstractcs, DMI_ABSTRACTCS_BUSY))
2171                         if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
2172                                 return ERROR_FAIL;
2173                 info->cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
2174
2175                 riscv_addr_t next_read_addr;
2176                 unsigned ignore_last = 0;
2177                 switch (info->cmderr) {
2178                         case CMDERR_NONE:
2179                                 LOG_DEBUG("successful (partial?) memory read");
2180                                 next_read_addr = read_addr + reads * size;
2181                                 break;
2182                         case CMDERR_BUSY:
2183                                 LOG_DEBUG("memory read resulted in busy response");
2184
2185                                 increase_ac_busy_delay(target);
2186                                 riscv013_clear_abstract_error(target);
2187
2188                                 dmi_write(target, DMI_ABSTRACTAUTO, 0);
2189
2190                                 uint32_t dmi_data0;
2191                                 /* This is definitely a good version of the value that we
2192                                  * attempted to read when we discovered that the target was
2193                                  * busy. */
2194                                 if (dmi_read(target, &dmi_data0, DMI_DATA0) != ERROR_OK) {
2195                                         riscv_batch_free(batch);
2196                                         goto error;
2197                                 }
2198
2199                                 /* See how far we got, clobbering dmi_data0. */
2200                                 result = register_read_direct(target, &next_read_addr,
2201                                                 GDB_REGNO_S0);
2202                                 if (result != ERROR_OK) {
2203                                         riscv_batch_free(batch);
2204                                         goto error;
2205                                 }
2206                                 write_to_buf(buffer + next_read_addr - 2 * size - address, dmi_data0, size);
2207                                 log_memory_access(next_read_addr - 2 * size, dmi_data0, size, true);
2208
2209                                 /* Restore the command, and execute it.
2210                                  * Now DMI_DATA0 contains the next value just as it would if no
2211                                  * error had occurred. */
2212                                 dmi_write_exec(target, DMI_COMMAND, command);
2213                                 next_read_addr += size;
2214
2215                                 dmi_write(target, DMI_ABSTRACTAUTO,
2216                                                 1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
2217
2218                                 ignore_last = 1;
2219
2220                                 break;
2221                         default:
2222                                 LOG_DEBUG("error when reading memory, abstractcs=0x%08lx", (long)abstractcs);
2223                                 riscv013_clear_abstract_error(target);
2224                                 riscv_batch_free(batch);
2225                                 result = ERROR_FAIL;
2226                                 goto error;
2227                 }
2228
2229                 /* Now read whatever we got out of the batch. */
2230                 dmi_status_t status = DMI_STATUS_SUCCESS;
2231                 for (size_t i = 0; i < reads; i++) {
2232                         riscv_addr_t receive_addr = read_addr + (i-2) * size;
2233                         assert(receive_addr < address + size * count);
2234                         if (receive_addr < address)
2235                                 continue;
2236                         if (receive_addr > next_read_addr - (3 + ignore_last) * size)
2237                                 break;
2238
2239                         uint64_t dmi_out = riscv_batch_get_dmi_read(batch, i);
2240                         status = get_field(dmi_out, DTM_DMI_OP);
2241                         if (status != DMI_STATUS_SUCCESS) {
2242                                 /* If we're here because of busy count, dmi_busy_delay will
2243                                  * already have been increased and busy state will have been
2244                                  * cleared in dmi_read(). */
2245                                 /* In at least some implementations, we issue a read, and then
2246                                  * can get busy back when we try to scan out the read result,
2247                                  * and the actual read value is lost forever. Since this is
2248                                  * rare in any case, we return error here and rely on our
2249                                  * caller to reread the entire block. */
2250                                 LOG_WARNING("Batch memory read encountered DMI error %d. "
2251                                                 "Falling back on slower reads.", status);
2252                                 riscv_batch_free(batch);
2253                                 result = ERROR_FAIL;
2254                                 goto error;
2255                         }
2256                         uint32_t value = get_field(dmi_out, DTM_DMI_DATA);
2257                         riscv_addr_t offset = receive_addr - address;
2258                         write_to_buf(buffer + offset, value, size);
2259                         log_memory_access(receive_addr, value, size, true);
2260
2261                         receive_addr += size;
2262                 }
2263
2264                 read_addr = next_read_addr;
2265
2266                 riscv_batch_free(batch);
2267         }
2268
2269         dmi_write(target, DMI_ABSTRACTAUTO, 0);
2270
2271         if (count > 1) {
2272                 /* Read the penultimate word. */
2273                 uint32_t value;
2274                 if (dmi_read(target, &value, DMI_DATA0) != ERROR_OK)
2275                         return ERROR_FAIL;
2276                 write_to_buf(buffer + size * (count-2), value, size);
2277                 log_memory_access(address + size * (count-2), value, size, true);
2278         }
2279
2280         /* Read the last word. */
2281         uint64_t value;
2282         result = register_read_direct(target, &value, GDB_REGNO_S1);
2283         if (result != ERROR_OK)
2284                 goto error;
2285         write_to_buf(buffer + size * (count-1), value, size);
2286         log_memory_access(address + size * (count-1), value, size, true);
2287
2288         return ERROR_OK;
2289
2290 error:
2291         dmi_write(target, DMI_ABSTRACTAUTO, 0);
2292
2293         return result;
2294 }
2295
2296 /**
2297  * Read the requested memory, silently handling memory access errors.
2298  */
2299 static int read_memory_progbuf(struct target *target, target_addr_t address,
2300                 uint32_t size, uint32_t count, uint8_t *buffer)
2301 {
2302         int result = ERROR_OK;
2303
2304         LOG_DEBUG("reading %d words of %d bytes from 0x%" TARGET_PRIxADDR, count,
2305                         size, address);
2306
2307         select_dmi(target);
2308
2309         memset(buffer, 0, count*size);
2310
2311         /* s0 holds the next address to write to
2312          * s1 holds the next data value to write
2313          */
2314         uint64_t s0, s1;
2315         if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
2316                 return ERROR_FAIL;
2317         if (register_read(target, &s1, GDB_REGNO_S1) != ERROR_OK)
2318                 return ERROR_FAIL;
2319
2320         if (execute_fence(target) != ERROR_OK)
2321                 return ERROR_FAIL;
2322
2323         /* Write the program (load, increment) */
2324         struct riscv_program program;
2325         riscv_program_init(&program, target);
2326         switch (size) {
2327                 case 1:
2328                         riscv_program_lbr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2329                         break;
2330                 case 2:
2331                         riscv_program_lhr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2332                         break;
2333                 case 4:
2334                         riscv_program_lwr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2335                         break;
2336                 default:
2337                         LOG_ERROR("Unsupported size: %d", size);
2338                         return ERROR_FAIL;
2339         }
2340         riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
2341
2342         if (riscv_program_ebreak(&program) != ERROR_OK)
2343                 return ERROR_FAIL;
2344         riscv_program_write(&program);
2345
2346         result = read_memory_progbuf_inner(target, address, size, count, buffer);
2347
2348         if (result != ERROR_OK) {
2349                 /* The full read did not succeed, so we will try to read each word individually. */
2350                 /* This will not be fast, but reading outside actual memory is a special case anyway. */
2351                 /* It will make the toolchain happier, especially Eclipse Memory View as it reads ahead. */
2352                 target_addr_t address_i = address;
2353                 uint32_t size_i = size;
2354                 uint32_t count_i = 1;
2355                 uint8_t *buffer_i = buffer;
2356
2357                 for (uint32_t i = 0; i < count; i++, address_i += size_i, buffer_i += size_i) {
2358                         /* TODO: This is much slower than it needs to be because we end up
2359                          * writing the address to read for every word we read. */
2360                         result = read_memory_progbuf_inner(target, address_i, size_i, count_i, buffer_i);
2361
2362                         /* The read of a single word failed, so we will just return 0 for that instead */
2363                         if (result != ERROR_OK) {
2364                                 LOG_DEBUG("error reading single word of %d bytes from 0x%" TARGET_PRIxADDR,
2365                                                 size_i, address_i);
2366
2367                                 uint64_t value_i = 0;
2368                                 write_to_buf(buffer_i, value_i, size_i);
2369                         }
2370                 }
2371                 result = ERROR_OK;
2372         }
2373
2374         riscv_set_register(target, GDB_REGNO_S0, s0);
2375         riscv_set_register(target, GDB_REGNO_S1, s1);
2376         return result;
2377 }
2378
2379 static int read_memory(struct target *target, target_addr_t address,
2380                 uint32_t size, uint32_t count, uint8_t *buffer)
2381 {
2382         RISCV013_INFO(info);
2383         if (info->progbufsize >= 2 && !riscv_prefer_sba)
2384                 return read_memory_progbuf(target, address, size, count, buffer);
2385
2386         if ((get_field(info->sbcs, DMI_SBCS_SBACCESS8) && size == 1) ||
2387                         (get_field(info->sbcs, DMI_SBCS_SBACCESS16) && size == 2) ||
2388                         (get_field(info->sbcs, DMI_SBCS_SBACCESS32) && size == 4) ||
2389                         (get_field(info->sbcs, DMI_SBCS_SBACCESS64) && size == 8) ||
2390                         (get_field(info->sbcs, DMI_SBCS_SBACCESS128) && size == 16)) {
2391                 if (get_field(info->sbcs, DMI_SBCS_SBVERSION) == 0)
2392                         return read_memory_bus_v0(target, address, size, count, buffer);
2393                 else if (get_field(info->sbcs, DMI_SBCS_SBVERSION) == 1)
2394                         return read_memory_bus_v1(target, address, size, count, buffer);
2395         }
2396
2397         if (info->progbufsize >= 2)
2398                 return read_memory_progbuf(target, address, size, count, buffer);
2399
2400         LOG_ERROR("Don't know how to read memory on this target.");
2401         return ERROR_FAIL;
2402 }
2403
2404 static int write_memory_bus_v0(struct target *target, target_addr_t address,
2405                 uint32_t size, uint32_t count, const uint8_t *buffer)
2406 {
2407         /*1) write sbaddress: for singlewrite and autoincrement, we need to write the address once*/
2408         LOG_DEBUG("System Bus Access: size: %d\tcount:%d\tstart address: 0x%08"
2409                         TARGET_PRIxADDR, size, count, address);
2410         dmi_write(target, DMI_SBADDRESS0, address);
2411         int64_t value = 0;
2412         int64_t access = 0;
2413         riscv_addr_t offset = 0;
2414         riscv_addr_t t_addr = 0;
2415         const uint8_t *t_buffer = buffer + offset;
2416
2417         /* B.8 Writing Memory, single write check if we write in one go */
2418         if (count == 1) { /* count is in bytes here */
2419                 /* check the size */
2420                 switch (size) {
2421                         case 1:
2422                                 value = t_buffer[0];
2423                                 break;
2424                         case 2:
2425                                 value = t_buffer[0]
2426                                         | ((uint32_t) t_buffer[1] << 8);
2427                                 break;
2428                         case 4:
2429                                 value = t_buffer[0]
2430                                         | ((uint32_t) t_buffer[1] << 8)
2431                                         | ((uint32_t) t_buffer[2] << 16)
2432                                         | ((uint32_t) t_buffer[3] << 24);
2433                                 break;
2434                         default:
2435                                 LOG_ERROR("unsupported access size: %d", size);
2436                                 return ERROR_FAIL;
2437                 }
2438
2439                 access = 0;
2440                 access = set_field(access, DMI_SBCS_SBACCESS, size/2);
2441                 dmi_write(target, DMI_SBCS, access);
2442                 LOG_DEBUG("\r\naccess:  0x%08" PRIx64, access);
2443                 LOG_DEBUG("\r\nwrite_memory:SAB: ONE OFF: value 0x%08" PRIx64, value);
2444                 dmi_write(target, DMI_SBDATA0, value);
2445                 return ERROR_OK;
2446         }
2447
2448         /*B.8 Writing Memory, using autoincrement*/
2449
2450         access = 0;
2451         access = set_field(access, DMI_SBCS_SBACCESS, size/2);
2452         access = set_field(access, DMI_SBCS_SBAUTOINCREMENT, 1);
2453         LOG_DEBUG("\r\naccess:  0x%08" PRIx64, access);
2454         dmi_write(target, DMI_SBCS, access);
2455
2456         /*2)set the value according to the size required and write*/
2457         for (riscv_addr_t i = 0; i < count; ++i) {
2458                 offset = size*i;
2459                 /* for monitoring only */
2460                 t_addr = address + offset;
2461                 t_buffer = buffer + offset;
2462
2463                 switch (size) {
2464                         case 1:
2465                                 value = t_buffer[0];
2466                                 break;
2467                         case 2:
2468                                 value = t_buffer[0]
2469                                         | ((uint32_t) t_buffer[1] << 8);
2470                                 break;
2471                         case 4:
2472                                 value = t_buffer[0]
2473                                         | ((uint32_t) t_buffer[1] << 8)
2474                                         | ((uint32_t) t_buffer[2] << 16)
2475                                         | ((uint32_t) t_buffer[3] << 24);
2476                                 break;
2477                         default:
2478                                 LOG_ERROR("unsupported access size: %d", size);
2479                                 return ERROR_FAIL;
2480                 }
2481                 LOG_DEBUG("SAB:autoincrement: expected address: 0x%08x value: 0x%08x"
2482                                 PRIx64, (uint32_t)t_addr, (uint32_t)value);
2483                 dmi_write(target, DMI_SBDATA0, value);
2484         }
2485         /*reset the autoincrement when finished (something weird is happening if this is not done at the end*/
2486         access = set_field(access, DMI_SBCS_SBAUTOINCREMENT, 0);
2487         dmi_write(target, DMI_SBCS, access);
2488
2489         return ERROR_OK;
2490 }
2491
2492 static int write_memory_bus_v1(struct target *target, target_addr_t address,
2493                 uint32_t size, uint32_t count, const uint8_t *buffer)
2494 {
2495         RISCV013_INFO(info);
2496         uint32_t sbcs = sb_sbaccess(size);
2497         sbcs = set_field(sbcs, DMI_SBCS_SBAUTOINCREMENT, 1);
2498         dmi_write(target, DMI_SBCS, sbcs);
2499
2500         target_addr_t next_address = address;
2501         target_addr_t end_address = address + count * size;
2502
2503         sb_write_address(target, next_address);
2504         while (next_address < end_address) {
2505                 for (uint32_t i = (next_address - address) / size; i < count; i++) {
2506                         const uint8_t *p = buffer + i * size;
2507                         if (size > 12)
2508                                 dmi_write(target, DMI_SBDATA3,
2509                                                 ((uint32_t) p[12]) |
2510                                                 (((uint32_t) p[13]) << 8) |
2511                                                 (((uint32_t) p[14]) << 16) |
2512                                                 (((uint32_t) p[15]) << 24));
2513                         if (size > 8)
2514                                 dmi_write(target, DMI_SBDATA2,
2515                                                 ((uint32_t) p[8]) |
2516                                                 (((uint32_t) p[9]) << 8) |
2517                                                 (((uint32_t) p[10]) << 16) |
2518                                                 (((uint32_t) p[11]) << 24));
2519                         if (size > 4)
2520                                 dmi_write(target, DMI_SBDATA1,
2521                                                 ((uint32_t) p[4]) |
2522                                                 (((uint32_t) p[5]) << 8) |
2523                                                 (((uint32_t) p[6]) << 16) |
2524                                                 (((uint32_t) p[7]) << 24));
2525                         uint32_t value = p[0];
2526                         if (size > 2) {
2527                                 value |= ((uint32_t) p[2]) << 16;
2528                                 value |= ((uint32_t) p[3]) << 24;
2529                         }
2530                         if (size > 1)
2531                                 value |= ((uint32_t) p[1]) << 8;
2532                         dmi_write(target, DMI_SBDATA0, value);
2533
2534                         log_memory_access(address + i * size, value, size, false);
2535
2536                         if (info->bus_master_write_delay) {
2537                                 jtag_add_runtest(info->bus_master_write_delay, TAP_IDLE);
2538                                 if (jtag_execute_queue() != ERROR_OK) {
2539                                         LOG_ERROR("Failed to scan idle sequence");
2540                                         return ERROR_FAIL;
2541                                 }
2542                         }
2543                 }
2544
2545                 if (read_sbcs_nonbusy(target, &sbcs) != ERROR_OK)
2546                         return ERROR_FAIL;
2547
2548                 if (get_field(sbcs, DMI_SBCS_SBBUSYERROR)) {
2549                         /* We wrote while the target was busy. Slow down and try again. */
2550                         dmi_write(target, DMI_SBCS, DMI_SBCS_SBBUSYERROR);
2551                         next_address = sb_read_address(target);
2552                         info->bus_master_write_delay += info->bus_master_write_delay / 10 + 1;
2553                         continue;
2554                 }
2555
2556                 unsigned error = get_field(sbcs, DMI_SBCS_SBERROR);
2557                 if (error == 0) {
2558                         next_address = end_address;
2559                 } else {
2560                         /* Some error indicating the bus access failed, but not because of
2561                          * something we did wrong. */
2562                         dmi_write(target, DMI_SBCS, DMI_SBCS_SBERROR);
2563                         return ERROR_FAIL;
2564                 }
2565         }
2566
2567         return ERROR_OK;
2568 }
2569
2570 static int write_memory_progbuf(struct target *target, target_addr_t address,
2571                 uint32_t size, uint32_t count, const uint8_t *buffer)
2572 {
2573         RISCV013_INFO(info);
2574
2575         LOG_DEBUG("writing %d words of %d bytes to 0x%08lx", count, size, (long)address);
2576
2577         select_dmi(target);
2578
2579         /* s0 holds the next address to write to
2580          * s1 holds the next data value to write
2581          */
2582
2583         int result = ERROR_OK;
2584         uint64_t s0, s1;
2585         if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
2586                 return ERROR_FAIL;
2587         if (register_read(target, &s1, GDB_REGNO_S1) != ERROR_OK)
2588                 return ERROR_FAIL;
2589
2590         /* Write the program (store, increment) */
2591         struct riscv_program program;
2592         riscv_program_init(&program, target);
2593
2594         switch (size) {
2595                 case 1:
2596                         riscv_program_sbr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2597                         break;
2598                 case 2:
2599                         riscv_program_shr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2600                         break;
2601                 case 4:
2602                         riscv_program_swr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
2603                         break;
2604                 default:
2605                         LOG_ERROR("Unsupported size: %d", size);
2606                         result = ERROR_FAIL;
2607                         goto error;
2608         }
2609
2610         riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
2611
2612         result = riscv_program_ebreak(&program);
2613         if (result != ERROR_OK)
2614                 goto error;
2615         riscv_program_write(&program);
2616
2617         riscv_addr_t cur_addr = address;
2618         riscv_addr_t fin_addr = address + (count * size);
2619         bool setup_needed = true;
2620         LOG_DEBUG("writing until final address 0x%016" PRIx64, fin_addr);
2621         while (cur_addr < fin_addr) {
2622                 LOG_DEBUG("transferring burst starting at address 0x%016" PRIx64,
2623                                 cur_addr);
2624
2625                 struct riscv_batch *batch = riscv_batch_alloc(
2626                                 target,
2627                                 32,
2628                                 info->dmi_busy_delay + info->ac_busy_delay);
2629
2630                 /* To write another word, we put it in S1 and execute the program. */
2631                 unsigned start = (cur_addr - address) / size;
2632                 for (unsigned i = start; i < count; ++i) {
2633                         unsigned offset = size*i;
2634                         const uint8_t *t_buffer = buffer + offset;
2635
2636                         uint32_t value;
2637                         switch (size) {
2638                                 case 1:
2639                                         value = t_buffer[0];
2640                                         break;
2641                                 case 2:
2642                                         value = t_buffer[0]
2643                                                 | ((uint32_t) t_buffer[1] << 8);
2644                                         break;
2645                                 case 4:
2646                                         value = t_buffer[0]
2647                                                 | ((uint32_t) t_buffer[1] << 8)
2648                                                 | ((uint32_t) t_buffer[2] << 16)
2649                                                 | ((uint32_t) t_buffer[3] << 24);
2650                                         break;
2651                                 default:
2652                                         LOG_ERROR("unsupported access size: %d", size);
2653                                         riscv_batch_free(batch);
2654                                         result = ERROR_FAIL;
2655                                         goto error;
2656                         }
2657
2658                         log_memory_access(address + offset, value, size, false);
2659                         cur_addr += size;
2660
2661                         if (setup_needed) {
2662                                 result = register_write_direct(target, GDB_REGNO_S0,
2663                                                 address + offset);
2664                                 if (result != ERROR_OK) {
2665                                         riscv_batch_free(batch);
2666                                         goto error;
2667                                 }
2668
2669                                 /* Write value. */
2670                                 dmi_write(target, DMI_DATA0, value);
2671
2672                                 /* Write and execute command that moves value into S1 and
2673                                  * executes program buffer. */
2674                                 uint32_t command = access_register_command(target,
2675                                                 GDB_REGNO_S1, 32,
2676                                                 AC_ACCESS_REGISTER_POSTEXEC |
2677                                                 AC_ACCESS_REGISTER_TRANSFER |
2678                                                 AC_ACCESS_REGISTER_WRITE);
2679                                 result = execute_abstract_command(target, command);
2680                                 if (result != ERROR_OK) {
2681                                         riscv_batch_free(batch);
2682                                         goto error;
2683                                 }
2684
2685                                 /* Turn on autoexec */
2686                                 dmi_write(target, DMI_ABSTRACTAUTO,
2687                                                 1 << DMI_ABSTRACTAUTO_AUTOEXECDATA_OFFSET);
2688
2689                                 setup_needed = false;
2690                         } else {
2691                                 riscv_batch_add_dmi_write(batch, DMI_DATA0, value);
2692                                 if (riscv_batch_full(batch))
2693                                         break;
2694                         }
2695                 }
2696
2697                 result = batch_run(target, batch);
2698                 riscv_batch_free(batch);
2699                 if (result != ERROR_OK)
2700                         goto error;
2701
2702                 /* Note that if the scan resulted in a Busy DMI response, it
2703                  * is this read to abstractcs that will cause the dmi_busy_delay
2704                  * to be incremented if necessary. */
2705
2706                 uint32_t abstractcs;
2707                 bool dmi_busy_encountered;
2708                 if (dmi_op(target, &abstractcs, &dmi_busy_encountered, DMI_OP_READ,
2709                                         DMI_ABSTRACTCS, 0, false) != ERROR_OK)
2710                         goto error;
2711                 while (get_field(abstractcs, DMI_ABSTRACTCS_BUSY))
2712                         if (dmi_read(target, &abstractcs, DMI_ABSTRACTCS) != ERROR_OK)
2713                                 return ERROR_FAIL;
2714                 info->cmderr = get_field(abstractcs, DMI_ABSTRACTCS_CMDERR);
2715                 if (info->cmderr == CMDERR_NONE && !dmi_busy_encountered) {
2716                         LOG_DEBUG("successful (partial?) memory write");
2717                 } else if (info->cmderr == CMDERR_BUSY || dmi_busy_encountered) {
2718                         if (info->cmderr == CMDERR_BUSY)
2719                                 LOG_DEBUG("Memory write resulted in abstract command busy response.");
2720                         else if (dmi_busy_encountered)
2721                                 LOG_DEBUG("Memory write resulted in DMI busy response.");
2722                         riscv013_clear_abstract_error(target);
2723                         increase_ac_busy_delay(target);
2724
2725                         dmi_write(target, DMI_ABSTRACTAUTO, 0);
2726                         result = register_read_direct(target, &cur_addr, GDB_REGNO_S0);
2727                         if (result != ERROR_OK)
2728                                 goto error;
2729                         setup_needed = true;
2730                 } else {
2731                         LOG_ERROR("error when writing memory, abstractcs=0x%08lx", (long)abstractcs);
2732                         riscv013_clear_abstract_error(target);
2733                         result = ERROR_FAIL;
2734                         goto error;
2735                 }
2736         }
2737
2738 error:
2739         dmi_write(target, DMI_ABSTRACTAUTO, 0);
2740
2741         if (register_write_direct(target, GDB_REGNO_S1, s1) != ERROR_OK)
2742                 return ERROR_FAIL;
2743         if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
2744                 return ERROR_FAIL;
2745
2746         if (execute_fence(target) != ERROR_OK)
2747                 return ERROR_FAIL;
2748
2749         return result;
2750 }
2751
2752 static int write_memory(struct target *target, target_addr_t address,
2753                 uint32_t size, uint32_t count, const uint8_t *buffer)
2754 {
2755         RISCV013_INFO(info);
2756         if (info->progbufsize >= 2 && !riscv_prefer_sba)
2757                 return write_memory_progbuf(target, address, size, count, buffer);
2758
2759         if ((get_field(info->sbcs, DMI_SBCS_SBACCESS8) && size == 1) ||
2760                         (get_field(info->sbcs, DMI_SBCS_SBACCESS16) && size == 2) ||
2761                         (get_field(info->sbcs, DMI_SBCS_SBACCESS32) && size == 4) ||
2762                         (get_field(info->sbcs, DMI_SBCS_SBACCESS64) && size == 8) ||
2763                         (get_field(info->sbcs, DMI_SBCS_SBACCESS128) && size == 16)) {
2764                 if (get_field(info->sbcs, DMI_SBCS_SBVERSION) == 0)
2765                         return write_memory_bus_v0(target, address, size, count, buffer);
2766                 else if (get_field(info->sbcs, DMI_SBCS_SBVERSION) == 1)
2767                         return write_memory_bus_v1(target, address, size, count, buffer);
2768         }
2769
2770         if (info->progbufsize >= 2)
2771                 return write_memory_progbuf(target, address, size, count, buffer);
2772
2773         LOG_ERROR("Don't know how to write memory on this target.");
2774         return ERROR_FAIL;
2775 }
2776
2777 static int arch_state(struct target *target)
2778 {
2779         return ERROR_OK;
2780 }
2781
2782 struct target_type riscv013_target = {
2783         .name = "riscv",
2784
2785         .init_target = init_target,
2786         .deinit_target = deinit_target,
2787         .examine = examine,
2788
2789         .poll = &riscv_openocd_poll,
2790         .halt = &riscv_openocd_halt,
2791         .resume = &riscv_openocd_resume,
2792         .step = &riscv_openocd_step,
2793
2794         .assert_reset = assert_reset,
2795         .deassert_reset = deassert_reset,
2796
2797         .read_memory = read_memory,
2798         .write_memory = write_memory,
2799
2800         .arch_state = arch_state,
2801 };
2802
2803 /*** 0.13-specific implementations of various RISC-V helper functions. ***/
2804 static int riscv013_get_register(struct target *target,
2805                 riscv_reg_t *value, int hid, int rid)
2806 {
2807         LOG_DEBUG("reading register %s on hart %d", gdb_regno_name(rid), hid);
2808
2809         riscv_set_current_hartid(target, hid);
2810
2811         int result = ERROR_OK;
2812         if (rid == GDB_REGNO_PC) {
2813                 result = register_read(target, value, GDB_REGNO_DPC);
2814                 LOG_DEBUG("read PC from DPC: 0x%" PRIx64, *value);
2815         } else if (rid == GDB_REGNO_PRIV) {
2816                 uint64_t dcsr;
2817                 result = register_read(target, &dcsr, GDB_REGNO_DCSR);
2818                 *value = get_field(dcsr, CSR_DCSR_PRV);
2819         } else {
2820                 result = register_read(target, value, rid);
2821                 if (result != ERROR_OK)
2822                         *value = -1;
2823         }
2824
2825         return result;
2826 }
2827
2828 static int riscv013_set_register(struct target *target, int hid, int rid, uint64_t value)
2829 {
2830         LOG_DEBUG("writing 0x%" PRIx64 " to register %s on hart %d", value,
2831                         gdb_regno_name(rid), hid);
2832
2833         riscv_set_current_hartid(target, hid);
2834
2835         if (rid <= GDB_REGNO_XPR31) {
2836                 return register_write_direct(target, rid, value);
2837         } else if (rid == GDB_REGNO_PC) {
2838                 LOG_DEBUG("writing PC to DPC: 0x%" PRIx64, value);
2839                 register_write_direct(target, GDB_REGNO_DPC, value);
2840                 uint64_t actual_value;
2841                 register_read_direct(target, &actual_value, GDB_REGNO_DPC);
2842                 LOG_DEBUG("  actual DPC written: 0x%016" PRIx64, actual_value);
2843                 if (value != actual_value) {
2844                         LOG_ERROR("Written PC (0x%" PRIx64 ") does not match read back "
2845                                         "value (0x%" PRIx64 ")", value, actual_value);
2846                         return ERROR_FAIL;
2847                 }
2848         } else if (rid == GDB_REGNO_PRIV) {
2849                 uint64_t dcsr;
2850                 register_read(target, &dcsr, GDB_REGNO_DCSR);
2851                 dcsr = set_field(dcsr, CSR_DCSR_PRV, value);
2852                 return register_write_direct(target, GDB_REGNO_DCSR, dcsr);
2853         } else {
2854                 return register_write_direct(target, rid, value);
2855         }
2856
2857         return ERROR_OK;
2858 }
2859
2860 static int riscv013_select_current_hart(struct target *target)
2861 {
2862         RISCV_INFO(r);
2863
2864         dm013_info_t *dm = get_dm(target);
2865         if (r->current_hartid == dm->current_hartid)
2866                 return ERROR_OK;
2867
2868         uint32_t dmcontrol;
2869         /* TODO: can't we just "dmcontrol = DMI_DMACTIVE"? */
2870         if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
2871                 return ERROR_FAIL;
2872         dmcontrol = set_hartsel(dmcontrol, r->current_hartid);
2873         int result = dmi_write(target, DMI_DMCONTROL, dmcontrol);
2874         dm->current_hartid = r->current_hartid;
2875         return result;
2876 }
2877
2878 static int riscv013_halt_current_hart(struct target *target)
2879 {
2880         RISCV_INFO(r);
2881         LOG_DEBUG("halting hart %d", r->current_hartid);
2882         if (riscv_is_halted(target))
2883                 LOG_ERROR("Hart %d is already halted!", r->current_hartid);
2884
2885         /* Issue the halt command, and then wait for the current hart to halt. */
2886         uint32_t dmcontrol;
2887         if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
2888                 return ERROR_FAIL;
2889         dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HALTREQ, 1);
2890         dmi_write(target, DMI_DMCONTROL, dmcontrol);
2891         for (size_t i = 0; i < 256; ++i)
2892                 if (riscv_is_halted(target))
2893                         break;
2894
2895         if (!riscv_is_halted(target)) {
2896                 uint32_t dmstatus;
2897                 if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
2898                         return ERROR_FAIL;
2899                 if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
2900                         return ERROR_FAIL;
2901
2902                 LOG_ERROR("unable to halt hart %d", r->current_hartid);
2903                 LOG_ERROR("  dmcontrol=0x%08x", dmcontrol);
2904                 LOG_ERROR("  dmstatus =0x%08x", dmstatus);
2905                 return ERROR_FAIL;
2906         }
2907
2908         dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HALTREQ, 0);
2909         dmi_write(target, DMI_DMCONTROL, dmcontrol);
2910
2911         return ERROR_OK;
2912 }
2913
2914 static int riscv013_resume_current_hart(struct target *target)
2915 {
2916         return riscv013_step_or_resume_current_hart(target, false);
2917 }
2918
2919 static int riscv013_step_current_hart(struct target *target)
2920 {
2921         return riscv013_step_or_resume_current_hart(target, true);
2922 }
2923
2924 static int riscv013_on_resume(struct target *target)
2925 {
2926         return riscv013_on_step_or_resume(target, false);
2927 }
2928
2929 static int riscv013_on_step(struct target *target)
2930 {
2931         return riscv013_on_step_or_resume(target, true);
2932 }
2933
2934 static int riscv013_on_halt(struct target *target)
2935 {
2936         return ERROR_OK;
2937 }
2938
2939 static bool riscv013_is_halted(struct target *target)
2940 {
2941         uint32_t dmstatus;
2942         if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
2943                 return false;
2944         if (get_field(dmstatus, DMI_DMSTATUS_ANYUNAVAIL))
2945                 LOG_ERROR("Hart %d is unavailable.", riscv_current_hartid(target));
2946         if (get_field(dmstatus, DMI_DMSTATUS_ANYNONEXISTENT))
2947                 LOG_ERROR("Hart %d doesn't exist.", riscv_current_hartid(target));
2948         if (get_field(dmstatus, DMI_DMSTATUS_ANYHAVERESET)) {
2949                 int hartid = riscv_current_hartid(target);
2950                 LOG_INFO("Hart %d unexpectedly reset!", hartid);
2951                 /* TODO: Can we make this more obvious to eg. a gdb user? */
2952                 uint32_t dmcontrol = DMI_DMCONTROL_DMACTIVE |
2953                         DMI_DMCONTROL_ACKHAVERESET;
2954                 dmcontrol = set_hartsel(dmcontrol, hartid);
2955                 /* If we had been halted when we reset, request another halt. If we
2956                  * ended up running out of reset, then the user will (hopefully) get a
2957                  * message that a reset happened, that the target is running, and then
2958                  * that it is halted again once the request goes through.
2959                  */
2960                 if (target->state == TARGET_HALTED)
2961                         dmcontrol |= DMI_DMCONTROL_HALTREQ;
2962                 dmi_write(target, DMI_DMCONTROL, dmcontrol);
2963         }
2964         return get_field(dmstatus, DMI_DMSTATUS_ALLHALTED);
2965 }
2966
2967 static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
2968 {
2969         riscv_reg_t dcsr;
2970         int result = register_read(target, &dcsr, GDB_REGNO_DCSR);
2971         if (result != ERROR_OK)
2972                 return RISCV_HALT_UNKNOWN;
2973
2974         switch (get_field(dcsr, CSR_DCSR_CAUSE)) {
2975         case CSR_DCSR_CAUSE_SWBP:
2976                 return RISCV_HALT_BREAKPOINT;
2977         case CSR_DCSR_CAUSE_TRIGGER:
2978                 /* We could get here before triggers are enumerated if a trigger was
2979                  * already set when we connected. Force enumeration now, which has the
2980                  * side effect of clearing any triggers we did not set. */
2981                 riscv_enumerate_triggers(target);
2982                 LOG_DEBUG("{%d} halted because of trigger", target->coreid);
2983                 return RISCV_HALT_TRIGGER;
2984         case CSR_DCSR_CAUSE_STEP:
2985                 return RISCV_HALT_SINGLESTEP;
2986         case CSR_DCSR_CAUSE_DEBUGINT:
2987         case CSR_DCSR_CAUSE_HALT:
2988                 return RISCV_HALT_INTERRUPT;
2989         }
2990
2991         LOG_ERROR("Unknown DCSR cause field: %x", (int)get_field(dcsr, CSR_DCSR_CAUSE));
2992         LOG_ERROR("  dcsr=0x%016lx", (long)dcsr);
2993         return RISCV_HALT_UNKNOWN;
2994 }
2995
2996 int riscv013_write_debug_buffer(struct target *target, unsigned index, riscv_insn_t data)
2997 {
2998         return dmi_write(target, DMI_PROGBUF0 + index, data);
2999 }
3000
3001 riscv_insn_t riscv013_read_debug_buffer(struct target *target, unsigned index)
3002 {
3003         uint32_t value;
3004         dmi_read(target, &value, DMI_PROGBUF0 + index);
3005         return value;
3006 }
3007
3008 int riscv013_execute_debug_buffer(struct target *target)
3009 {
3010         uint32_t run_program = 0;
3011         run_program = set_field(run_program, AC_ACCESS_REGISTER_SIZE, 2);
3012         run_program = set_field(run_program, AC_ACCESS_REGISTER_POSTEXEC, 1);
3013         run_program = set_field(run_program, AC_ACCESS_REGISTER_TRANSFER, 0);
3014         run_program = set_field(run_program, AC_ACCESS_REGISTER_REGNO, 0x1000);
3015
3016         return execute_abstract_command(target, run_program);
3017 }
3018
3019 void riscv013_fill_dmi_write_u64(struct target *target, char *buf, int a, uint64_t d)
3020 {
3021         RISCV013_INFO(info);
3022         buf_set_u64((unsigned char *)buf, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_WRITE);
3023         buf_set_u64((unsigned char *)buf, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, d);
3024         buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, a);
3025 }
3026
3027 void riscv013_fill_dmi_read_u64(struct target *target, char *buf, int a)
3028 {
3029         RISCV013_INFO(info);
3030         buf_set_u64((unsigned char *)buf, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_READ);
3031         buf_set_u64((unsigned char *)buf, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, 0);
3032         buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, a);
3033 }
3034
3035 void riscv013_fill_dmi_nop_u64(struct target *target, char *buf)
3036 {
3037         RISCV013_INFO(info);
3038         buf_set_u64((unsigned char *)buf, DTM_DMI_OP_OFFSET, DTM_DMI_OP_LENGTH, DMI_OP_NOP);
3039         buf_set_u64((unsigned char *)buf, DTM_DMI_DATA_OFFSET, DTM_DMI_DATA_LENGTH, 0);
3040         buf_set_u64((unsigned char *)buf, DTM_DMI_ADDRESS_OFFSET, info->abits, 0);
3041 }
3042
3043 /* Helper function for riscv013_test_sba_config_reg */
3044 static int get_max_sbaccess(struct target *target)
3045 {
3046         RISCV013_INFO(info);
3047
3048         uint32_t sbaccess128 = get_field(info->sbcs, DMI_SBCS_SBACCESS128);
3049         uint32_t sbaccess64 = get_field(info->sbcs, DMI_SBCS_SBACCESS64);
3050         uint32_t sbaccess32 = get_field(info->sbcs, DMI_SBCS_SBACCESS32);
3051         uint32_t sbaccess16 = get_field(info->sbcs, DMI_SBCS_SBACCESS16);
3052         uint32_t sbaccess8 = get_field(info->sbcs, DMI_SBCS_SBACCESS8);
3053
3054         if (sbaccess128)
3055                 return 4;
3056         else if (sbaccess64)
3057                 return 3;
3058         else if (sbaccess32)
3059                 return 2;
3060         else if (sbaccess16)
3061                 return 1;
3062         else if (sbaccess8)
3063                 return 0;
3064         else
3065                 return -1;
3066 }
3067
3068 static uint32_t get_num_sbdata_regs(struct target *target)
3069 {
3070         RISCV013_INFO(info);
3071
3072         uint32_t sbaccess128 = get_field(info->sbcs, DMI_SBCS_SBACCESS128);
3073         uint32_t sbaccess64 = get_field(info->sbcs, DMI_SBCS_SBACCESS64);
3074         uint32_t sbaccess32 = get_field(info->sbcs, DMI_SBCS_SBACCESS32);
3075
3076         if (sbaccess128)
3077                 return 4;
3078         else if (sbaccess64)
3079                 return 2;
3080         else if (sbaccess32)
3081                 return 1;
3082         else
3083                 return 0;
3084 }
3085
3086 static int riscv013_test_sba_config_reg(struct target *target,
3087                 target_addr_t legal_address, uint32_t num_words,
3088                 target_addr_t illegal_address, bool run_sbbusyerror_test)
3089 {
3090         LOG_INFO("Testing System Bus Access as defined by RISC-V Debug Spec v0.13");
3091
3092         uint32_t tests_failed = 0;
3093
3094         uint32_t rd_val;
3095         uint32_t sbcs_orig;
3096         dmi_read(target, &sbcs_orig, DMI_SBCS);
3097
3098         uint32_t sbcs = sbcs_orig;
3099         bool test_passed;
3100
3101         int max_sbaccess = get_max_sbaccess(target);
3102
3103         if (max_sbaccess == -1) {
3104                 LOG_ERROR("System Bus Access not supported in this config.");
3105                 return ERROR_FAIL;
3106         }
3107
3108         if (get_field(sbcs, DMI_SBCS_SBVERSION) != 1) {
3109                 LOG_ERROR("System Bus Access unsupported SBVERSION (%d). Only version 1 is supported.",
3110                                 get_field(sbcs, DMI_SBCS_SBVERSION));
3111                 return ERROR_FAIL;
3112         }
3113
3114         uint32_t num_sbdata_regs = get_num_sbdata_regs(target);
3115
3116         uint32_t rd_buf[num_sbdata_regs];
3117
3118         /* Test 1: Simple write/read test */
3119         test_passed = true;
3120         sbcs = set_field(sbcs_orig, DMI_SBCS_SBAUTOINCREMENT, 0);
3121         dmi_write(target, DMI_SBCS, sbcs);
3122
3123         uint32_t test_patterns[4] = {0xdeadbeef, 0xfeedbabe, 0x12345678, 0x08675309};
3124         for (uint32_t sbaccess = 0; sbaccess <= (uint32_t)max_sbaccess; sbaccess++) {
3125                 sbcs = set_field(sbcs, DMI_SBCS_SBACCESS, sbaccess);
3126                 dmi_write(target, DMI_SBCS, sbcs);
3127
3128                 uint32_t compare_mask = (sbaccess == 0) ? 0xff : (sbaccess == 1) ? 0xffff : 0xffffffff;
3129
3130                 for (uint32_t i = 0; i < num_words; i++) {
3131                         uint32_t addr = legal_address + (i << sbaccess);
3132                         uint32_t wr_data[num_sbdata_regs];
3133                         for (uint32_t j = 0; j < num_sbdata_regs; j++)
3134                                 wr_data[j] = test_patterns[j] + i;
3135                         write_memory_sba_simple(target, addr, wr_data, num_sbdata_regs, sbcs);
3136                 }
3137
3138                 for (uint32_t i = 0; i < num_words; i++) {
3139                         uint32_t addr = legal_address + (i << sbaccess);
3140                         read_memory_sba_simple(target, addr, rd_buf, num_sbdata_regs, sbcs);
3141                         for (uint32_t j = 0; j < num_sbdata_regs; j++) {
3142                                 if (((test_patterns[j]+i)&compare_mask) != (rd_buf[j]&compare_mask)) {
3143                                         LOG_ERROR("System Bus Access Test 1: Error reading non-autoincremented address %x,"
3144                                                         "expected val = %x, read val = %x", addr, test_patterns[j]+i, rd_buf[j]);
3145                                         test_passed = false;
3146                                         tests_failed++;
3147                                 }
3148                         }
3149                 }
3150         }
3151         if (test_passed)
3152                 LOG_INFO("System Bus Access Test 1: Simple write/read test PASSED.");
3153
3154         /* Test 2: Address autoincrement test */
3155         target_addr_t curr_addr;
3156         target_addr_t prev_addr;
3157         test_passed = true;
3158         sbcs = set_field(sbcs_orig, DMI_SBCS_SBAUTOINCREMENT, 1);
3159         dmi_write(target, DMI_SBCS, sbcs);
3160
3161         for (uint32_t sbaccess = 0; sbaccess <= (uint32_t)max_sbaccess; sbaccess++) {
3162                 sbcs = set_field(sbcs, DMI_SBCS_SBACCESS, sbaccess);
3163                 dmi_write(target, DMI_SBCS, sbcs);
3164
3165                 dmi_write(target, DMI_SBADDRESS0, legal_address);
3166                 read_sbcs_nonbusy(target, &sbcs);
3167                 curr_addr = legal_address;
3168                 for (uint32_t i = 0; i < num_words; i++) {
3169                         prev_addr = curr_addr;
3170                         read_sbcs_nonbusy(target, &sbcs);
3171                         curr_addr = sb_read_address(target);
3172                         if ((curr_addr - prev_addr != (uint32_t)(1 << sbaccess)) && (i != 0)) {
3173                                 LOG_ERROR("System Bus Access Test 2: Error with address auto-increment, sbaccess = %x.", sbaccess);
3174                                 test_passed = false;
3175                                 tests_failed++;
3176                         }
3177                         dmi_write(target, DMI_SBDATA0, i);
3178                 }
3179
3180                 read_sbcs_nonbusy(target, &sbcs);
3181
3182                 dmi_write(target, DMI_SBADDRESS0, legal_address);
3183
3184                 uint32_t val;
3185                 sbcs = set_field(sbcs, DMI_SBCS_SBREADONDATA, 1);
3186                 dmi_write(target, DMI_SBCS, sbcs);
3187                 dmi_read(target, &val, DMI_SBDATA0); /* Dummy read to trigger first system bus read */
3188                 curr_addr = legal_address;
3189                 for (uint32_t i = 0; i < num_words; i++) {
3190                         prev_addr = curr_addr;
3191                         read_sbcs_nonbusy(target, &sbcs);
3192                         curr_addr = sb_read_address(target);
3193                         if ((curr_addr - prev_addr != (uint32_t)(1 << sbaccess)) && (i != 0)) {
3194                                 LOG_ERROR("System Bus Access Test 2: Error with address auto-increment, sbaccess = %x", sbaccess);
3195                                 test_passed = false;
3196                                 tests_failed++;
3197                         }
3198                         dmi_read(target, &val, DMI_SBDATA0);
3199                         read_sbcs_nonbusy(target, &sbcs);
3200                         if (i != val) {
3201                                 LOG_ERROR("System Bus Access Test 2: Error reading auto-incremented address,"
3202                                                 "expected val = %x, read val = %x.", i, val);
3203                                 test_passed = false;
3204                                 tests_failed++;
3205                         }
3206                 }
3207         }
3208         if (test_passed)
3209                 LOG_INFO("System Bus Access Test 2: Address auto-increment test PASSED.");
3210
3211         /* Test 3: Read from illegal address */
3212         read_memory_sba_simple(target, illegal_address, rd_buf, 1, sbcs_orig);
3213
3214         dmi_read(target, &rd_val, DMI_SBCS);
3215         if (get_field(rd_val, DMI_SBCS_SBERROR) == 2) {
3216                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 2);
3217                 dmi_write(target, DMI_SBCS, sbcs);
3218                 dmi_read(target, &rd_val, DMI_SBCS);
3219                 if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
3220                         LOG_INFO("System Bus Access Test 3: Illegal address read test PASSED.");
3221                 else
3222                         LOG_ERROR("System Bus Access Test 3: Illegal address read test FAILED, unable to clear to 0.");
3223         } else {
3224                 LOG_ERROR("System Bus Access Test 3: Illegal address read test FAILED, unable to set error code.");
3225         }
3226
3227         /* Test 4: Write to illegal address */
3228         write_memory_sba_simple(target, illegal_address, test_patterns, 1, sbcs_orig);
3229
3230         dmi_read(target, &rd_val, DMI_SBCS);
3231         if (get_field(rd_val, DMI_SBCS_SBERROR) == 2) {
3232                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 2);
3233                 dmi_write(target, DMI_SBCS, sbcs);
3234                 dmi_read(target, &rd_val, DMI_SBCS);
3235                 if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
3236                         LOG_INFO("System Bus Access Test 4: Illegal address write test PASSED.");
3237                 else {
3238                         LOG_ERROR("System Bus Access Test 4: Illegal address write test FAILED, unable to clear to 0.");
3239                         tests_failed++;
3240                 }
3241         } else {
3242                 LOG_ERROR("System Bus Access Test 4: Illegal address write test FAILED, unable to set error code.");
3243                 tests_failed++;
3244         }
3245
3246         /* Test 5: Write with unsupported sbaccess size */
3247         uint32_t sbaccess128 = get_field(sbcs_orig, DMI_SBCS_SBACCESS128);
3248
3249         if (sbaccess128) {
3250                 LOG_INFO("System Bus Access Test 5: SBCS sbaccess error test PASSED, all sbaccess sizes supported.");
3251         } else {
3252                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBACCESS, 4);
3253
3254                 write_memory_sba_simple(target, legal_address, test_patterns, 1, sbcs);
3255
3256                 dmi_read(target, &rd_val, DMI_SBCS);
3257                 if (get_field(rd_val, DMI_SBCS_SBERROR) == 4) {
3258                         sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 4);
3259                         dmi_write(target, DMI_SBCS, sbcs);
3260                         dmi_read(target, &rd_val, DMI_SBCS);
3261                         if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
3262                                 LOG_INFO("System Bus Access Test 5: SBCS sbaccess error test PASSED.");
3263                         else {
3264                                 LOG_ERROR("System Bus Access Test 5: SBCS sbaccess error test FAILED, unable to clear to 0.");
3265                                 tests_failed++;
3266                         }
3267                 } else {
3268                         LOG_ERROR("System Bus Access Test 5: SBCS sbaccess error test FAILED, unable to set error code.");
3269                         tests_failed++;
3270                 }
3271         }
3272
3273         /* Test 6: Write to misaligned address */
3274         sbcs = set_field(sbcs_orig, DMI_SBCS_SBACCESS, 1);
3275
3276         write_memory_sba_simple(target, legal_address+1, test_patterns, 1, sbcs);
3277
3278         dmi_read(target, &rd_val, DMI_SBCS);
3279         if (get_field(rd_val, DMI_SBCS_SBERROR) == 3) {
3280                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBERROR, 3);
3281                 dmi_write(target, DMI_SBCS, sbcs);
3282                 dmi_read(target, &rd_val, DMI_SBCS);
3283                 if (get_field(rd_val, DMI_SBCS_SBERROR) == 0)
3284                         LOG_INFO("System Bus Access Test 6: SBCS address alignment error test PASSED");
3285                 else {
3286                         LOG_ERROR("System Bus Access Test 6: SBCS address alignment error test FAILED, unable to clear to 0.");
3287                         tests_failed++;
3288                 }
3289         } else {
3290                 LOG_ERROR("System Bus Access Test 6: SBCS address alignment error test FAILED, unable to set error code.");
3291                 tests_failed++;
3292         }
3293
3294         /* Test 7: Set sbbusyerror, only run this case in simulation as it is likely
3295          * impossible to hit otherwise */
3296         if (run_sbbusyerror_test) {
3297                 sbcs = set_field(sbcs_orig, DMI_SBCS_SBREADONADDR, 1);
3298                 dmi_write(target, DMI_SBCS, sbcs);
3299
3300                 for (int i = 0; i < 16; i++)
3301                         dmi_write(target, DMI_SBDATA0, 0xdeadbeef);
3302
3303                 for (int i = 0; i < 16; i++)
3304                         dmi_write(target, DMI_SBADDRESS0, legal_address);
3305
3306                 dmi_read(target, &rd_val, DMI_SBCS);
3307                 if (get_field(rd_val, DMI_SBCS_SBBUSYERROR)) {
3308                         sbcs = set_field(sbcs_orig, DMI_SBCS_SBBUSYERROR, 1);
3309                         dmi_write(target, DMI_SBCS, sbcs);
3310                         dmi_read(target, &rd_val, DMI_SBCS);
3311                         if (get_field(rd_val, DMI_SBCS_SBBUSYERROR) == 0)
3312                                 LOG_INFO("System Bus Access Test 7: SBCS sbbusyerror test PASSED.");
3313                         else {
3314                                 LOG_ERROR("System Bus Access Test 7: SBCS sbbusyerror test FAILED, unable to clear to 0.");
3315                                 tests_failed++;
3316                         }
3317                 } else {
3318                         LOG_ERROR("System Bus Access Test 7: SBCS sbbusyerror test FAILED, unable to set error code.");
3319                         tests_failed++;
3320                 }
3321         }
3322
3323         if (tests_failed == 0) {
3324                 LOG_INFO("ALL TESTS PASSED");
3325                 return ERROR_OK;
3326         } else {
3327                 LOG_ERROR("%d TESTS FAILED", tests_failed);
3328                 return ERROR_FAIL;
3329         }
3330
3331 }
3332
3333 void write_memory_sba_simple(struct target *target, target_addr_t addr,
3334                 uint32_t *write_data, uint32_t write_size, uint32_t sbcs)
3335 {
3336         RISCV013_INFO(info);
3337
3338         uint32_t rd_sbcs;
3339         uint32_t masked_addr;
3340
3341         uint32_t sba_size = get_field(info->sbcs, DMI_SBCS_SBASIZE);
3342
3343         read_sbcs_nonbusy(target, &rd_sbcs);
3344
3345         uint32_t sbcs_no_readonaddr = set_field(sbcs, DMI_SBCS_SBREADONADDR, 0);
3346         dmi_write(target, DMI_SBCS, sbcs_no_readonaddr);
3347
3348         for (uint32_t i = 0; i < sba_size/32; i++) {
3349                 masked_addr = (addr >> 32*i) & 0xffffffff;
3350
3351                 if (i != 3)
3352                         dmi_write(target, DMI_SBADDRESS0+i, masked_addr);
3353                 else
3354                         dmi_write(target, DMI_SBADDRESS3, masked_addr);
3355         }
3356
3357         /* Write SBDATA registers starting with highest address, since write to
3358          * SBDATA0 triggers write */
3359         for (int i = write_size-1; i >= 0; i--)
3360                 dmi_write(target, DMI_SBDATA0+i, write_data[i]);
3361 }
3362
3363 void read_memory_sba_simple(struct target *target, target_addr_t addr,
3364                 uint32_t *rd_buf, uint32_t read_size, uint32_t sbcs)
3365 {
3366         RISCV013_INFO(info);
3367
3368         uint32_t rd_sbcs;
3369         uint32_t masked_addr;
3370
3371         uint32_t sba_size = get_field(info->sbcs, DMI_SBCS_SBASIZE);
3372
3373         read_sbcs_nonbusy(target, &rd_sbcs);
3374
3375         uint32_t sbcs_readonaddr = set_field(sbcs, DMI_SBCS_SBREADONADDR, 1);
3376         dmi_write(target, DMI_SBCS, sbcs_readonaddr);
3377
3378         /* Write addresses starting with highest address register */
3379         for (int i = sba_size/32-1; i >= 0; i--) {
3380                 masked_addr = (addr >> 32*i) & 0xffffffff;
3381
3382                 if (i != 3)
3383                         dmi_write(target, DMI_SBADDRESS0+i, masked_addr);
3384                 else
3385                         dmi_write(target, DMI_SBADDRESS3, masked_addr);
3386         }
3387
3388         read_sbcs_nonbusy(target, &rd_sbcs);
3389
3390         for (uint32_t i = 0; i < read_size; i++)
3391                 dmi_read(target, &(rd_buf[i]), DMI_SBDATA0+i);
3392 }
3393
3394 int riscv013_dmi_write_u64_bits(struct target *target)
3395 {
3396         RISCV013_INFO(info);
3397         return info->abits + DTM_DMI_DATA_LENGTH + DTM_DMI_OP_LENGTH;
3398 }
3399
3400 static int maybe_execute_fence_i(struct target *target)
3401 {
3402         RISCV013_INFO(info);
3403         RISCV_INFO(r);
3404         if (info->progbufsize + r->impebreak >= 3)
3405                 return execute_fence(target);
3406         return ERROR_OK;
3407 }
3408
3409 /* Helper Functions. */
3410 static int riscv013_on_step_or_resume(struct target *target, bool step)
3411 {
3412         if (maybe_execute_fence_i(target) != ERROR_OK)
3413                 return ERROR_FAIL;
3414
3415         /* We want to twiddle some bits in the debug CSR so debugging works. */
3416         riscv_reg_t dcsr;
3417         int result = register_read(target, &dcsr, GDB_REGNO_DCSR);
3418         if (result != ERROR_OK)
3419                 return result;
3420         dcsr = set_field(dcsr, CSR_DCSR_STEP, step);
3421         dcsr = set_field(dcsr, CSR_DCSR_EBREAKM, 1);
3422         dcsr = set_field(dcsr, CSR_DCSR_EBREAKS, 1);
3423         dcsr = set_field(dcsr, CSR_DCSR_EBREAKU, 1);
3424         return riscv_set_register(target, GDB_REGNO_DCSR, dcsr);
3425 }
3426
3427 static int riscv013_step_or_resume_current_hart(struct target *target, bool step)
3428 {
3429         RISCV_INFO(r);
3430         LOG_DEBUG("resuming hart %d (for step?=%d)", r->current_hartid, step);
3431         if (!riscv_is_halted(target)) {
3432                 LOG_ERROR("Hart %d is not halted!", r->current_hartid);
3433                 return ERROR_FAIL;
3434         }
3435
3436         if (maybe_execute_fence_i(target) != ERROR_OK)
3437                 return ERROR_FAIL;
3438
3439         /* Issue the resume command, and then wait for the current hart to resume. */
3440         uint32_t dmcontrol = DMI_DMCONTROL_DMACTIVE;
3441         dmcontrol = set_hartsel(dmcontrol, r->current_hartid);
3442         dmi_write(target, DMI_DMCONTROL, dmcontrol | DMI_DMCONTROL_RESUMEREQ);
3443
3444         uint32_t dmstatus;
3445         for (size_t i = 0; i < 256; ++i) {
3446                 usleep(10);
3447                 if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
3448                         return ERROR_FAIL;
3449                 if (get_field(dmstatus, DMI_DMSTATUS_ALLRESUMEACK) == 0)
3450                         continue;
3451                 if (step && get_field(dmstatus, DMI_DMSTATUS_ALLHALTED) == 0)
3452                         continue;
3453
3454                 dmi_write(target, DMI_DMCONTROL, dmcontrol);
3455                 return ERROR_OK;
3456         }
3457
3458         LOG_ERROR("unable to resume hart %d", r->current_hartid);
3459         if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
3460                 return ERROR_FAIL;
3461         LOG_ERROR("  dmcontrol=0x%08x", dmcontrol);
3462         if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
3463                 return ERROR_FAIL;
3464         LOG_ERROR("  dmstatus =0x%08x", dmstatus);
3465
3466         if (step) {
3467                 LOG_ERROR("  was stepping, halting");
3468                 riscv013_halt_current_hart(target);
3469                 return ERROR_OK;
3470         }
3471
3472         return ERROR_FAIL;
3473 }
3474
3475 void riscv013_clear_abstract_error(struct target *target)
3476 {
3477         /* Wait for busy to go away. */
3478         time_t start = time(NULL);
3479         uint32_t abstractcs;
3480         dmi_read(target, &abstractcs, DMI_ABSTRACTCS);
3481         while (get_field(abstractcs, DMI_ABSTRACTCS_BUSY)) {
3482                 dmi_read(target, &abstractcs, DMI_ABSTRACTCS);
3483
3484                 if (time(NULL) - start > riscv_command_timeout_sec) {
3485                         LOG_ERROR("abstractcs.busy is not going low after %d seconds "
3486                                         "(abstractcs=0x%x). The target is either really slow or "
3487                                         "broken. You could increase the timeout with riscv "
3488                                         "set_command_timeout_sec.",
3489                                         riscv_command_timeout_sec, abstractcs);
3490                         break;
3491                 }
3492         }
3493         /* Clear the error status. */
3494         dmi_write(target, DMI_ABSTRACTCS, abstractcs & DMI_ABSTRACTCS_CMDERR);
3495 }
3496
3497 #define COMPLIANCE_TEST(b, message) \
3498 {                                   \
3499         int pass = 0;               \
3500         if (b) {                    \
3501                 pass = 1;           \
3502                 passed_tests++;     \
3503         }                           \
3504         LOG_INFO("%s test %d (%s)\n", (pass) ? "PASSED" : "FAILED",  total_tests, message); \
3505         assert(pass);               \
3506         total_tests++;              \
3507 }
3508
3509 #define COMPLIANCE_MUST_PASS(b) COMPLIANCE_TEST(ERROR_OK == (b), "Regular calls must return ERROR_OK")
3510
3511 #define COMPLIANCE_READ(target, addr, value) COMPLIANCE_MUST_PASS(dmi_read(target, addr, value))
3512 #define COMPLIANCE_WRITE(target, addr, value) COMPLIANCE_MUST_PASS(dmi_write(target, addr, value))
3513
3514 #define COMPLIANCE_CHECK_RO(target, addr)                               \
3515 {                                                                       \
3516         uint32_t orig;                                                      \
3517         uint32_t inverse;                                                   \
3518         COMPLIANCE_READ(target, &orig, addr);                               \
3519         COMPLIANCE_WRITE(target, addr, ~orig);                              \
3520         COMPLIANCE_READ(target, &inverse, addr);                            \
3521         COMPLIANCE_TEST(orig == inverse, "Register must be read-only");     \
3522 }
3523
3524 int riscv013_test_compliance(struct target *target)
3525 {
3526         LOG_INFO("Testing Compliance against RISC-V Debug Spec v0.13");
3527
3528         if (!riscv_rtos_enabled(target)) {
3529                 LOG_ERROR("Please run with -rtos riscv to run compliance test.");
3530                 return ERROR_FAIL;
3531         }
3532
3533         int total_tests = 0;
3534         int passed_tests = 0;
3535
3536         uint32_t dmcontrol_orig = DMI_DMCONTROL_DMACTIVE;
3537         uint32_t dmcontrol;
3538         uint32_t testvar;
3539         uint32_t testvar_read;
3540         riscv_reg_t value;
3541         RISCV013_INFO(info);
3542
3543         /* All the bits of HARTSEL are covered by the examine sequence. */
3544
3545         /* hartreset */
3546         /* This field is optional. Either we can read and write it to 1/0,
3547         or it is tied to 0. This check doesn't really do anything, but
3548         it does attempt to set the bit to 1 and then back to 0, which needs to
3549         work if its implemented. */
3550         COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HARTRESET, 1));
3551         COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HARTRESET, 0));
3552         COMPLIANCE_READ(target, &dmcontrol, DMI_DMCONTROL);
3553         COMPLIANCE_TEST((get_field(dmcontrol, DMI_DMCONTROL_HARTRESET) == 0),
3554                         "DMCONTROL.hartreset can be 0 or RW.");
3555
3556         /* hasel */
3557         COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HASEL, 1));
3558         COMPLIANCE_WRITE(target, DMI_DMCONTROL, set_field(dmcontrol_orig, DMI_DMCONTROL_HASEL, 0));
3559         COMPLIANCE_READ(target, &dmcontrol, DMI_DMCONTROL);
3560         COMPLIANCE_TEST((get_field(dmcontrol, DMI_DMCONTROL_HASEL) == 0),
3561                         "DMCONTROL.hasel can be 0 or RW.");
3562         /* TODO: test that hamask registers exist if hasel does. */
3563
3564         /* haltreq */
3565         COMPLIANCE_MUST_PASS(riscv_halt_all_harts(target));
3566         /* This bit is not actually readable according to the spec, so nothing to check.*/
3567
3568         /* DMSTATUS */
3569         COMPLIANCE_CHECK_RO(target, DMI_DMSTATUS);
3570
3571         /* resumereq */
3572         /* This bit is not actually readable according to the spec, so nothing to check.*/
3573         COMPLIANCE_MUST_PASS(riscv_resume_all_harts(target));
3574
3575         /* Halt all harts again so the test can continue.*/
3576         COMPLIANCE_MUST_PASS(riscv_halt_all_harts(target));
3577
3578         /* HARTINFO: Read-Only. This is per-hart, so need to adjust hartsel. */
3579         uint32_t hartinfo;
3580         COMPLIANCE_READ(target, &hartinfo, DMI_HARTINFO);
3581         for (int hartsel = 0; hartsel < riscv_count_harts(target); hartsel++) {
3582                 COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, hartsel));
3583
3584                 COMPLIANCE_CHECK_RO(target, DMI_HARTINFO);
3585
3586                 /* $dscratch CSRs */
3587                 uint32_t nscratch = get_field(hartinfo, DMI_HARTINFO_NSCRATCH);
3588                 for (unsigned int d = 0; d < nscratch; d++) {
3589                         riscv_reg_t testval, testval_read;
3590                         /* Because DSCRATCH is not guaranteed to last across PB executions, need to put
3591                         this all into one PB execution. Which may not be possible on all implementations.*/
3592                         if (info->progbufsize >= 5) {
3593                                 for (testval = 0x0011223300112233;
3594                                                  testval != 0xDEAD;
3595                                                  testval = testval == 0x0011223300112233 ? ~testval : 0xDEAD) {
3596                                         COMPLIANCE_TEST(register_write_direct(target, GDB_REGNO_S0, testval) == ERROR_OK,
3597                                                         "Need to be able to write S0 in order to test DSCRATCH.");
3598                                         struct riscv_program program32;
3599                                         riscv_program_init(&program32, target);
3600                                         riscv_program_csrw(&program32, GDB_REGNO_S0, GDB_REGNO_DSCRATCH + d);
3601                                         riscv_program_csrr(&program32, GDB_REGNO_S1, GDB_REGNO_DSCRATCH + d);
3602                                         riscv_program_fence(&program32);
3603                                         riscv_program_ebreak(&program32);
3604                                         COMPLIANCE_TEST(riscv_program_exec(&program32, target) == ERROR_OK,
3605                                                         "Accessing DSCRATCH with program buffer should succeed.");
3606                                         COMPLIANCE_TEST(register_read_direct(target, &testval_read, GDB_REGNO_S1) == ERROR_OK,
3607                                                         "Need to be able to read S1 in order to test DSCRATCH.");
3608                                         if (riscv_xlen(target) > 32) {
3609                                                 COMPLIANCE_TEST(testval == testval_read,
3610                                                                 "All DSCRATCH registers in HARTINFO must be R/W.");
3611                                         } else {
3612                                                 COMPLIANCE_TEST(testval_read == (testval & 0xFFFFFFFF),
3613                                                                 "All DSCRATCH registers in HARTINFO must be R/W.");
3614                                         }
3615                                 }
3616                         }
3617                 }
3618                 /* TODO: dataaccess */
3619                 if (get_field(hartinfo, DMI_HARTINFO_DATAACCESS)) {
3620                         /* TODO: Shadowed in memory map. */
3621                         /* TODO: datasize */
3622                         /* TODO: dataaddr */
3623                 } else {
3624                         /* TODO: Shadowed in CSRs. */
3625                         /* TODO: datasize */
3626                         /* TODO: dataaddr */
3627                 }
3628
3629         }
3630
3631         /* HALTSUM -- TODO: More than 32 harts. Would need to loop over this to set hartsel */
3632         /* TODO: HALTSUM2, HALTSUM3 */
3633         /* HALTSUM0 */
3634         uint32_t expected_haltsum0 = 0;
3635         for (int i = 0; i < MIN(riscv_count_harts(target), 32); i++)
3636                 expected_haltsum0 |= (1 << i);
3637
3638         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM0);
3639         COMPLIANCE_TEST(testvar_read == expected_haltsum0,
3640                         "HALTSUM0 should report summary of up to 32 halted harts");
3641
3642         COMPLIANCE_WRITE(target, DMI_HALTSUM0, 0xffffffff);
3643         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM0);
3644         COMPLIANCE_TEST(testvar_read == expected_haltsum0, "HALTSUM0 should be R/O");
3645
3646         COMPLIANCE_WRITE(target, DMI_HALTSUM0, 0x0);
3647         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM0);
3648         COMPLIANCE_TEST(testvar_read == expected_haltsum0, "HALTSUM0 should be R/O");
3649
3650         /* HALTSUM1 */
3651         uint32_t expected_haltsum1 = 0;
3652         for (int i = 0; i < MIN(riscv_count_harts(target), 1024); i += 32)
3653                 expected_haltsum1 |= (1 << (i/32));
3654
3655         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM1);
3656         COMPLIANCE_TEST(testvar_read == expected_haltsum1,
3657                         "HALTSUM1 should report summary of up to 1024 halted harts");
3658
3659         COMPLIANCE_WRITE(target, DMI_HALTSUM1, 0xffffffff);
3660         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM1);
3661         COMPLIANCE_TEST(testvar_read == expected_haltsum1, "HALTSUM1 should be R/O");
3662
3663         COMPLIANCE_WRITE(target, DMI_HALTSUM1, 0x0);
3664         COMPLIANCE_READ(target, &testvar_read, DMI_HALTSUM1);
3665         COMPLIANCE_TEST(testvar_read == expected_haltsum1, "HALTSUM1 should be R/O");
3666
3667         /* TODO: HAWINDOWSEL */
3668
3669         /* TODO: HAWINDOW */
3670
3671         /* ABSTRACTCS */
3672
3673         uint32_t abstractcs;
3674         COMPLIANCE_READ(target, &abstractcs, DMI_ABSTRACTCS);
3675
3676         /* Check that all reported Data Words are really R/W */
3677         for (int invert = 0; invert < 2; invert++) {
3678                 for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
3679                         testvar = (i + 1) * 0x11111111;
3680                         if (invert)
3681                                 testvar = ~testvar;
3682                         COMPLIANCE_WRITE(target, DMI_DATA0 + i, testvar);
3683                 }
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_READ(target, &testvar_read, DMI_DATA0 + i);
3689                         COMPLIANCE_TEST(testvar_read == testvar, "All reported DATA words must be R/W");
3690                 }
3691         }
3692
3693         /* Check that all reported ProgBuf words are really R/W */
3694         for (int invert = 0; invert < 2; invert++) {
3695                 for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
3696                         testvar = (i + 1) * 0x11111111;
3697                         if (invert)
3698                                 testvar = ~testvar;
3699                         COMPLIANCE_WRITE(target, DMI_PROGBUF0 + i, testvar);
3700                 }
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_READ(target, &testvar_read, DMI_PROGBUF0 + i);
3706                         COMPLIANCE_TEST(testvar_read == testvar, "All reported PROGBUF words must be R/W");
3707                 }
3708         }
3709
3710         /* TODO: Cause and clear all error types */
3711
3712         /* COMMAND
3713         According to the spec, this register is only W, so can't really check the read result.
3714         But at any rate, this is not legal and should cause an error. */
3715         COMPLIANCE_WRITE(target, DMI_COMMAND, 0xAAAAAAAA);
3716         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3717         COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR) == CMDERR_NOT_SUPPORTED, \
3718                         "Illegal COMMAND should result in UNSUPPORTED");
3719         COMPLIANCE_WRITE(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
3720
3721         COMPLIANCE_WRITE(target, DMI_COMMAND, 0x55555555);
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         /* Basic Abstract Commands */
3728         for (unsigned int i = 1; i < 32; i = i << 1) {
3729                 riscv_reg_t testval =   i | ((i + 1ULL) << 32);
3730                 riscv_reg_t testval_read;
3731                 COMPLIANCE_TEST(ERROR_OK == register_write_direct(target, GDB_REGNO_ZERO + i, testval),
3732                                 "GPR Writes should be supported.");
3733                 COMPLIANCE_MUST_PASS(write_abstract_arg(target, 0, 0xDEADBEEFDEADBEEF, 64));
3734                 COMPLIANCE_TEST(ERROR_OK == register_read_direct(target, &testval_read, GDB_REGNO_ZERO + i),
3735                                 "GPR Reads should be supported.");
3736                 if (riscv_xlen(target) > 32) {
3737                         /* Dummy comment to satisfy linter, since removing the brances here doesn't actually compile. */
3738                         COMPLIANCE_TEST(testval == testval_read, "GPR Reads and writes should be supported.");
3739                 } else {
3740                         /* Dummy comment to satisfy linter, since removing the brances here doesn't actually compile. */
3741                         COMPLIANCE_TEST((testval & 0xFFFFFFFF) == testval_read, "GPR Reads and writes should be supported.");
3742                 }
3743         }
3744
3745         /* ABSTRACTAUTO
3746         See which bits are actually writable */
3747         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0xFFFFFFFF);
3748         uint32_t abstractauto;
3749         uint32_t busy;
3750         COMPLIANCE_READ(target, &abstractauto, DMI_ABSTRACTAUTO);
3751         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0x0);
3752         if (abstractauto > 0) {
3753                 /* This mechanism only works when you have a reasonable sized progbuf, which is not
3754                 a true compliance requirement. */
3755                 if (info->progbufsize >= 3) {
3756
3757                         testvar = 0;
3758                         COMPLIANCE_TEST(ERROR_OK == register_write_direct(target, GDB_REGNO_S0, 0),
3759                                         "Need to be able to write S0 to test ABSTRACTAUTO");
3760                         struct riscv_program program;
3761                         COMPLIANCE_MUST_PASS(riscv_program_init(&program, target));
3762                         /* This is also testing that WFI() is a NOP during debug mode. */
3763                         COMPLIANCE_MUST_PASS(riscv_program_insert(&program, wfi()));
3764                         COMPLIANCE_MUST_PASS(riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, 1));
3765                         COMPLIANCE_MUST_PASS(riscv_program_ebreak(&program));
3766                         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0x0);
3767                         COMPLIANCE_MUST_PASS(riscv_program_exec(&program, target));
3768                         testvar++;
3769                         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0xFFFFFFFF);
3770                         COMPLIANCE_READ(target, &abstractauto, DMI_ABSTRACTAUTO);
3771                         uint32_t autoexec_data = get_field(abstractauto, DMI_ABSTRACTAUTO_AUTOEXECDATA);
3772                         uint32_t autoexec_progbuf = get_field(abstractauto, DMI_ABSTRACTAUTO_AUTOEXECPROGBUF);
3773                         for (unsigned int i = 0; i < 12; i++) {
3774                                 COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
3775                                 do {
3776                                         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3777                                         busy = get_field(testvar_read, DMI_ABSTRACTCS_BUSY);
3778                                 } while (busy);
3779                                 if (autoexec_data & (1 << i)) {
3780                                         COMPLIANCE_TEST(i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT),
3781                                                         "AUTOEXEC may be writable up to DATACOUNT bits.");
3782                                         testvar++;
3783                                 }
3784                         }
3785                         for (unsigned int i = 0; i < 16; i++) {
3786                                 COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
3787                                 do {
3788                                         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3789                                         busy = get_field(testvar_read, DMI_ABSTRACTCS_BUSY);
3790                                 } while (busy);
3791                                 if (autoexec_progbuf & (1 << i)) {
3792                                         COMPLIANCE_TEST(i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE),
3793                                                         "AUTOEXEC may be writable up to PROGBUFSIZE bits.");
3794                                         testvar++;
3795                                 }
3796                         }
3797
3798                         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0);
3799                         COMPLIANCE_TEST(ERROR_OK == register_read_direct(target, &value, GDB_REGNO_S0),
3800                                         "Need to be able to read S0 to test ABSTRACTAUTO");
3801
3802                         COMPLIANCE_TEST(testvar == value,
3803                                         "ABSTRACTAUTO should cause COMMAND to run the expected number of times.");
3804                 }
3805         }
3806
3807         /* Single-Step each hart. */
3808         for (int hartsel = 0; hartsel < riscv_count_harts(target); hartsel++) {
3809                 COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, hartsel));
3810                 COMPLIANCE_MUST_PASS(riscv013_on_step(target));
3811                 COMPLIANCE_MUST_PASS(riscv013_step_current_hart(target));
3812                 COMPLIANCE_TEST(riscv_halt_reason(target, hartsel) == RISCV_HALT_SINGLESTEP,
3813                                 "Single Step should result in SINGLESTEP");
3814         }
3815
3816         /* Core Register Tests */
3817         uint64_t bogus_dpc = 0xdeadbeef;
3818         for (int hartsel = 0; hartsel < riscv_count_harts(target); hartsel++) {
3819                 COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, hartsel));
3820
3821                 /* DCSR Tests */
3822                 COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DCSR, 0x0));
3823                 COMPLIANCE_MUST_PASS(register_read_direct(target, &value, GDB_REGNO_DCSR));
3824                 COMPLIANCE_TEST(value != 0,     "Not all bits in DCSR are writable by Debugger");
3825                 COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DCSR, 0xFFFFFFFF));
3826                 COMPLIANCE_MUST_PASS(register_read_direct(target, &value, GDB_REGNO_DCSR));
3827                 COMPLIANCE_TEST(value != 0,     "At least some bits in DCSR must be 1");
3828
3829                 /* DPC. Note that DPC is sign-extended. */
3830                 riscv_reg_t dpcmask = 0xFFFFFFFCUL;
3831                 riscv_reg_t dpc;
3832
3833                 if (riscv_xlen(target) > 32)
3834                         dpcmask |= (0xFFFFFFFFULL << 32);
3835
3836                 if (riscv_supports_extension(target, riscv_current_hartid(target), 'C'))
3837                         dpcmask |= 0x2;
3838
3839                 COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DPC, dpcmask));
3840                 COMPLIANCE_MUST_PASS(register_read_direct(target, &dpc, GDB_REGNO_DPC));
3841                 COMPLIANCE_TEST(dpcmask == dpc,
3842                                 "DPC must be sign-extended to XLEN and writable to all-1s (except the least significant bits)");
3843                 COMPLIANCE_MUST_PASS(register_write_direct(target, GDB_REGNO_DPC, 0));
3844                 COMPLIANCE_MUST_PASS(register_read_direct(target, &dpc, GDB_REGNO_DPC));
3845                 COMPLIANCE_TEST(dpc == 0, "DPC must be writable to 0.");
3846                 if (hartsel == 0)
3847                         bogus_dpc = dpc; /* For a later test step */
3848         }
3849
3850         /* NDMRESET
3851         Asserting non-debug module reset should not reset Debug Module state.
3852         But it should reset Hart State, e.g. DPC should get a different value.
3853         Also make sure that DCSR reports cause of 'HALT' even though previously we single-stepped.
3854         */
3855
3856         /* Write some registers. They should not be impacted by ndmreset. */
3857         COMPLIANCE_WRITE(target, DMI_COMMAND, 0xFFFFFFFF);
3858
3859         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
3860                 testvar = (i + 1) * 0x11111111;
3861                 COMPLIANCE_WRITE(target, DMI_PROGBUF0 + i, testvar);
3862         }
3863
3864         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
3865                 testvar = (i + 1) * 0x11111111;
3866                 COMPLIANCE_WRITE(target, DMI_DATA0 + i, testvar);
3867         }
3868
3869         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0xFFFFFFFF);
3870         COMPLIANCE_READ(target, &abstractauto, DMI_ABSTRACTAUTO);
3871
3872         /* Pulse reset. */
3873         target->reset_halt = true;
3874         COMPLIANCE_MUST_PASS(riscv_set_current_hartid(target, 0));
3875         COMPLIANCE_TEST(ERROR_OK == assert_reset(target), "Must be able to assert NDMRESET");
3876         COMPLIANCE_TEST(ERROR_OK == deassert_reset(target), "Must be able to deassert NDMRESET");
3877
3878         /* Verify that most stuff is not affected by ndmreset. */
3879         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3880         COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR)  == CMDERR_NOT_SUPPORTED,
3881                         "NDMRESET should not affect DMI_ABSTRACTCS");
3882         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTAUTO);
3883         COMPLIANCE_TEST(testvar_read == abstractauto, "NDMRESET should not affect DMI_ABSTRACTAUTO");
3884
3885         /* Clean up to avoid future test failures */
3886         COMPLIANCE_WRITE(target, DMI_ABSTRACTCS, DMI_ABSTRACTCS_CMDERR);
3887         COMPLIANCE_WRITE(target, DMI_ABSTRACTAUTO, 0);
3888
3889         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
3890                 testvar = (i + 1) * 0x11111111;
3891                 COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
3892                 COMPLIANCE_TEST(testvar_read == testvar, "PROGBUF words must not be affected by NDMRESET");
3893         }
3894
3895         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
3896                 testvar = (i + 1) * 0x11111111;
3897                 COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
3898                 COMPLIANCE_TEST(testvar_read == testvar, "DATA words must not be affected by NDMRESET");
3899         }
3900
3901         /* Verify that DPC *is* affected by ndmreset. Since we don't know what it *should* be,
3902         just verify that at least it's not the bogus value anymore. */
3903
3904         COMPLIANCE_TEST(bogus_dpc != 0xdeadbeef, "BOGUS DPC should have been set somehow (bug in compliance test)");
3905         COMPLIANCE_MUST_PASS(register_read_direct(target, &value, GDB_REGNO_DPC));
3906         COMPLIANCE_TEST(bogus_dpc != value, "NDMRESET should move DPC to reset value.");
3907
3908         COMPLIANCE_TEST(riscv_halt_reason(target, 0) == RISCV_HALT_INTERRUPT,
3909                         "After NDMRESET halt, DCSR should report cause of halt");
3910
3911         /* DMACTIVE -- deasserting DMACTIVE should reset all the above values. */
3912
3913         /* Toggle dmactive */
3914         COMPLIANCE_WRITE(target, DMI_DMCONTROL, 0);
3915         COMPLIANCE_WRITE(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
3916         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTCS);
3917         COMPLIANCE_TEST(get_field(testvar_read, DMI_ABSTRACTCS_CMDERR)  == 0, "ABSTRACTCS.cmderr should reset to 0");
3918         COMPLIANCE_READ(target, &testvar_read, DMI_ABSTRACTAUTO);
3919         COMPLIANCE_TEST(testvar_read == 0, "ABSTRACTAUTO should reset to 0");
3920
3921         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_PROGBUFSIZE); i++) {
3922                 COMPLIANCE_READ(target, &testvar_read, DMI_PROGBUF0 + i);
3923                 COMPLIANCE_TEST(testvar_read == 0, "PROGBUF words should reset to 0");
3924         }
3925
3926         for (unsigned int i = 0; i < get_field(abstractcs, DMI_ABSTRACTCS_DATACOUNT); i++) {
3927                 COMPLIANCE_READ(target, &testvar_read, DMI_DATA0 + i);
3928                 COMPLIANCE_TEST(testvar_read == 0, "DATA words should reset to 0");
3929         }
3930
3931         /*
3932         * TODO:
3933         * DCSR.cause priorities
3934         * DCSR.stoptime/stopcycle
3935         * DCSR.stepie
3936         * DCSR.ebreak
3937         * DCSR.prv
3938         */
3939
3940         /* Halt every hart for any follow-up tests*/
3941         COMPLIANCE_MUST_PASS(riscv_halt_all_harts(target));
3942
3943         uint32_t failed_tests = total_tests - passed_tests;
3944         if (total_tests == passed_tests) {
3945                 LOG_INFO("ALL TESTS PASSED\n");
3946                 return ERROR_OK;
3947         } else {
3948                 LOG_INFO("%d TESTS FAILED\n", failed_tests);
3949                 return ERROR_FAIL;
3950         }
3951 }