+ if (abstractcs.cmderr != abstractcs.CMDERR_NONE)
+ return true;
+ if (abstractcs.busy) {
+ abstractcs.cmderr = abstractcs.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 (regno < 0x1000 || regno >= 0x1020) {
+ abstractcs.cmderr = abstractcs.CMDERR_NOTSUP;
+ return true;
+ }
+
+ unsigned regnum = regno - 0x1000;
+
+ if (!halted[dmcontrol.hartsel]) {
+ abstractcs.cmderr = abstractcs.CMDERR_HALTRESUME;
+ return true;
+ }
+
+ switch (size) {
+ case 2:
+ if (write)
+ write32(debug_rom_code, 0, lw(regnum, ZERO, DEBUG_EXCHANGE));
+ else
+ write32(debug_rom_code, 0, sw(regnum, ZERO, DEBUG_EXCHANGE));
+ break;
+ case 3:
+ if (write)
+ write32(debug_rom_code, 0, ld(regnum, ZERO, DEBUG_EXCHANGE));
+ else
+ write32(debug_rom_code, 0, sd(regnum, ZERO, DEBUG_EXCHANGE));
+ break;
+ /*
+ case 4:
+ if (write)
+ write32(debug_rom_code, 0, lq(regnum, ZERO, DEBUG_EXCHANGE));
+ else
+ write32(debug_rom_code, 0, sq(regnum, ZERO, DEBUG_EXCHANGE));
+ break;
+ */
+ default:
+ abstractcs.cmderr = abstractcs.CMDERR_NOTSUP;
+ return true;
+ }
+ write32(debug_rom_code, 1, ebreak());
+
+ write32(debug_rom_entry, dmcontrol.hartsel,
+ jal(ZERO, DEBUG_ROM_CODE - (DEBUG_ROM_ENTRY + 4 * dmcontrol.hartsel)));
+ write32(debug_rom_exception, dmcontrol.hartsel,
+ jal(ZERO, (DEBUG_ROM_ENTRY + 4 * dmcontrol.hartsel) - DEBUG_ROM_EXCEPTION));
+ abstractcs.busy = true;
+ } else {
+ abstractcs.cmderr = abstractcs.CMDERR_NOTSUP;
+ }
+ return true;