+ if (abstractcs.cmderr != CMDERR_NONE)
+ return true;
+ if (abstractcs.busy) {
+ abstractcs.cmderr = CMDERR_BUSY;
+ return true;
+ }
+
+ if ((command >> 24) == 0) {
+ // register access
+ unsigned size = get_field(command, AC_ACCESS_REGISTER_SIZE);
+ bool write = get_field(command, AC_ACCESS_REGISTER_WRITE);
+ unsigned regno = get_field(command, AC_ACCESS_REGISTER_REGNO);
+
+ if (!halted[dmcontrol.hartsel]) {
+ abstractcs.cmderr = CMDERR_HALTRESUME;
+ return true;
+ }
+
+ if (get_field(command, AC_ACCESS_REGISTER_TRANSFER)) {
+
+ if (regno < 0x1000 || regno >= 0x1020) {
+ abstractcs.cmderr = CMDERR_NOTSUP;
+ return true;
+ }
+
+ unsigned regnum = regno - 0x1000;
+
+ switch (size) {
+ case 2:
+ if (write)
+ write32(debug_abstract, 0, lw(regnum, ZERO, debug_data_start));
+ else
+ write32(debug_abstract, 0, sw(regnum, ZERO, debug_data_start));
+ break;
+ case 3:
+ if (write)
+ write32(debug_abstract, 0, ld(regnum, ZERO, debug_data_start));
+ else
+ write32(debug_abstract, 0, sd(regnum, ZERO, debug_data_start));
+ break;
+ /*
+ case 4:
+ if (write)
+ write32(debug_rom_code, 0, lq(regnum, ZERO, debug_data_start));
+ else
+ write32(debug_rom_code, 0, sq(regnum, ZERO, debug_data_start));
+ break;
+ */
+ default:
+ abstractcs.cmderr = CMDERR_NOTSUP;
+ return true;
+ }
+ } else {
+ //NOP
+ write32(debug_abstract, 0, addi(ZERO, ZERO, 0));
+ }
+
+ if (get_field(command, AC_ACCESS_REGISTER_POSTEXEC)) {
+ // Since the next instruction is what we will use, just use nother NOP
+ // to get there.
+ write32(debug_abstract, 1, addi(ZERO, ZERO, 0));
+ } else {
+ write32(debug_abstract, 1, ebreak());
+ }
+
+ debug_rom_flags[dmcontrol.hartsel] |= 1 << DEBUG_ROM_FLAG_GO;
+
+ abstractcs.busy = true;
+ } else {
+ abstractcs.cmderr = CMDERR_NOTSUP;
+ }
+ return true;