Separate page faults from physical memory access exceptions
[riscv-isa-sim.git] / riscv / gdbserver.cc
index db88d30dfddab67d1b60157243caafab6b9bcb64..01defdaa43247210ba7949f9c914007374365800 100644 (file)
@@ -9,6 +9,7 @@
 
 #include <algorithm>
 #include <cassert>
+#include <cinttypes>
 #include <cstdio>
 #include <vector>
 
@@ -16,9 +17,7 @@
 #include "sim.h"
 #include "gdbserver.h"
 #include "mmu.h"
-
-#define C_EBREAK        0x9002
-#define EBREAK          0x00100073
+#include "encoding.h"
 
 //////////////////////////////////////// Utility Functions
 
@@ -44,6 +43,7 @@ enum {
   REG_FPR0 = 33,
   REG_FPR31 = 64,
   REG_CSR0 = 65,
+  REG_MSTATUS = CSR_MSTATUS + REG_CSR0,
   REG_CSR4095 = 4160,
   REG_PRIV = 4161
 };
@@ -210,21 +210,19 @@ static uint32_t fsd(unsigned int src, unsigned int base, uint16_t offset)
     MATCH_FSD;
 }
 
-static uint32_t flw(unsigned int src, unsigned int base, uint16_t offset)
+static uint32_t flw(unsigned int dest, unsigned int base, uint16_t offset)
 {
-  return (bits(offset, 11, 5) << 25) |
-    (bits(src, 4, 0) << 20) |
+  return (bits(offset, 11, 0) << 20) |
     (base << 15) |
-    (bits(offset, 4, 0) << 7) |
+    (bits(dest, 4, 0) << 7) |
     MATCH_FLW;
 }
 
-static uint32_t fld(unsigned int src, unsigned int base, uint16_t offset)
+static uint32_t fld(unsigned int dest, unsigned int base, uint16_t offset)
 {
-  return (bits(offset, 11, 5) << 25) |
-    (bits(src, 4, 0) << 20) |
+  return (bits(offset, 11, 0) << 20) |
     (base << 15) |
-    (bits(offset, 4, 0) << 7) |
+    (bits(dest, 4, 0) << 7) |
     MATCH_FLD;
 }
 
@@ -327,7 +325,7 @@ void circular_buffer_t<T>::append(const T *src, unsigned int count)
   count -= copy;
   if (count > 0) {
     assert(count < contiguous_empty_size());
-    memcpy(contiguous_empty(), src, count * sizeof(T));
+    memcpy(contiguous_empty(), src+copy, count * sizeof(T));
     data_added(count);
   }
 }
@@ -351,6 +349,7 @@ class halt_op_t : public operation_t
 
     bool perform_step(unsigned int step) {
       switch (state) {
+        gs.tselect_valid = false;
         case ST_ENTER:
           if (gs.xlen == 0) {
             gs.dr_write32(0, xori(S1, ZERO, -1));
@@ -399,6 +398,7 @@ class halt_op_t : public operation_t
 
         case ST_MSTATUS:
           gs.mstatus = gs.dr_read(SLOT_DATA0);
+          gs.mstatus_dirty = false;
           gs.dr_write32(0, csrr(S0, CSR_DCSR));
           gs.dr_write32(1, sw(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
           gs.dr_write_jump(2);
@@ -433,6 +433,7 @@ class halt_op_t : public operation_t
                 break;
             }
           }
+
           return true;
 
         default:
@@ -458,6 +459,7 @@ class continue_op_t : public operation_t
       operation_t(gdbserver), single_step(single_step) {};
 
     bool perform_step(unsigned int step) {
+      D(fprintf(stderr, "continue step %d\n", step));
       switch (step) {
         case 0:
           gs.dr_write_load(0, S0, SLOT_DATA0);
@@ -590,8 +592,16 @@ class register_read_op_t : public operation_t
       switch (step) {
         case 0:
           if (reg >= REG_XPR0 && reg <= REG_XPR31) {
-            die("handle_register_read");
-            // send(p->state.XPR[reg - REG_XPR0]);
+            unsigned int i = 0;
+            if (reg == S0) {
+                gs.dr_write32(i++, csrr(S0, CSR_DSCRATCH));
+            }
+            if (gs.xlen == 32) {
+              gs.dr_write32(i++, sw(reg - REG_XPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
+            } else {
+              gs.dr_write32(i++, sd(reg - REG_XPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
+            }
+            gs.dr_write_jump(i);
           } else if (reg == REG_PC) {
             gs.start_packet();
             if (gs.xlen == 32) {
@@ -602,13 +612,25 @@ class register_read_op_t : public operation_t
             gs.end_packet();
             return true;
           } else if (reg >= REG_FPR0 && reg <= REG_FPR31) {
-            // send(p->state.FPR[reg - REG_FPR0]);
+            gs.dr_write_load(0, S0, SLOT_DATA1);
+            gs.dr_write(SLOT_DATA1, set_field(gs.mstatus, MSTATUS_FS, 1));
+            gs.dr_write32(1, csrw(S0, CSR_MSTATUS));
+            gs.mstatus_dirty = true;
             if (gs.xlen == 32) {
-              gs.dr_write32(0, fsw(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
+              gs.dr_write32(2, fsw(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
             } else {
-              gs.dr_write32(0, fsd(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
+              gs.dr_write32(2, fsd(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
             }
-            gs.dr_write_jump(1);
+            gs.dr_write_jump(3);
+          } else if (reg == REG_MSTATUS) {
+            gs.start_packet();
+            if (gs.xlen == 32) {
+              gs.send((uint32_t) gs.mstatus);
+            } else {
+              gs.send(gs.mstatus);
+            }
+            gs.end_packet();
+            return true;
           } else if (reg >= REG_CSR0 && reg <= REG_CSR4095) {
             gs.dr_write32(0, csrr(S0, reg - REG_CSR0));
             gs.dr_write_store(1, S0, SLOT_DATA0);
@@ -629,14 +651,21 @@ class register_read_op_t : public operation_t
           return false;
 
         case 1:
-          gs.start_packet();
-          if (gs.xlen == 32) {
-            gs.send(gs.dr_read32(4));
-          } else {
-            gs.send(gs.dr_read(SLOT_DATA0));
+          {
+            unsigned result = gs.dr_read32(DEBUG_RAM_SIZE / 4 - 1);
+            if (result) {
+              gs.send_packet("E03");
+              return true;
+            }
+            gs.start_packet();
+            if (gs.xlen == 32) {
+              gs.send(gs.dr_read32(4));
+            } else {
+              gs.send(gs.dr_read(SLOT_DATA0));
+            }
+            gs.end_packet();
+            return true;
           }
-          gs.end_packet();
-          return true;
       }
       return false;
     }
@@ -653,44 +682,67 @@ class register_write_op_t : public operation_t
 
     bool perform_step(unsigned int step)
     {
-      gs.dr_write_load(0, S0, SLOT_DATA0);
-      gs.dr_write(SLOT_DATA0, value);
-      if (reg == S0) {
-        gs.dr_write32(1, csrw(S0, CSR_DSCRATCH));
-        gs.dr_write_jump(2);
-      } else if (reg == S1) {
-        gs.dr_write_store(1, S0, SLOT_DATA_LAST);
-        gs.dr_write_jump(2);
-      } else if (reg >= REG_XPR0 && reg <= REG_XPR31) {
-        gs.dr_write32(1, addi(reg, S0, 0));
-        gs.dr_write_jump(2);
-      } else if (reg == REG_PC) {
-        gs.dpc = value;
-        return true;
-      } else if (reg >= REG_FPR0 && reg <= REG_FPR31) {
-        if (gs.xlen == 32) {
-          gs.dr_write32(0, flw(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
-        } else {
-          gs.dr_write32(0, fld(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
-        }
-        gs.dr_write_jump(1);
-      } else if (reg >= REG_CSR0 && reg <= REG_CSR4095) {
-        gs.dr_write32(1, csrw(S0, reg - REG_CSR0));
-        gs.dr_write_jump(2);
-        if (reg == REG_CSR0 + CSR_SPTBR) {
-          gs.sptbr = value;
-          gs.sptbr_valid = true;
-        }
-      } else if (reg == REG_PRIV) {
-        gs.dcsr = set_field(gs.dcsr, DCSR_PRV, value);
-        return true;
-      } else {
-        gs.send_packet("E02");
-        return true;
+      switch (step) {
+        case 0:
+          gs.dr_write_load(0, S0, SLOT_DATA0);
+          gs.dr_write(SLOT_DATA0, value);
+          if (reg == S0) {
+            gs.dr_write32(1, csrw(S0, CSR_DSCRATCH));
+            gs.dr_write_jump(2);
+          } else if (reg == S1) {
+            gs.dr_write_store(1, S0, SLOT_DATA_LAST);
+            gs.dr_write_jump(2);
+          } else if (reg >= REG_XPR0 && reg <= REG_XPR31) {
+            gs.dr_write32(1, addi(reg, S0, 0));
+            gs.dr_write_jump(2);
+          } else if (reg == REG_PC) {
+            gs.dpc = value;
+            return true;
+          } else if (reg >= REG_FPR0 && reg <= REG_FPR31) {
+            gs.dr_write_load(0, S0, SLOT_DATA1);
+            gs.dr_write(SLOT_DATA1, set_field(gs.mstatus, MSTATUS_FS, 1));
+            gs.dr_write32(1, csrw(S0, CSR_MSTATUS));
+            gs.mstatus_dirty = true;
+            if (gs.xlen == 32) {
+              gs.dr_write32(2, flw(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
+            } else {
+              gs.dr_write32(2, fld(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
+            }
+            gs.dr_write_jump(3);
+          } else if (reg == REG_MSTATUS) {
+            gs.mstatus = value;
+            gs.mstatus_dirty = true;
+            return true;
+          } else if (reg >= REG_CSR0 && reg <= REG_CSR4095) {
+            gs.dr_write32(1, csrw(S0, reg - REG_CSR0));
+            gs.dr_write_jump(2);
+            if (reg == REG_CSR0 + CSR_SPTBR) {
+              gs.sptbr = value;
+              gs.sptbr_valid = true;
+            }
+          } else if (reg == REG_PRIV) {
+            gs.dcsr = set_field(gs.dcsr, DCSR_PRV, value);
+            return true;
+          } else {
+            gs.send_packet("E02");
+            return true;
+          }
+          gs.set_interrupt(0);
+          return false;
+
+        case 1:
+          {
+            unsigned result = gs.dr_read32(DEBUG_RAM_SIZE / 4 - 1);
+            if (result) {
+              gs.send_packet("E03");
+              return true;
+            }
+            gs.send_packet("OK");
+            return true;
+          }
       }
-      gs.set_interrupt(0);
-      gs.send_packet("OK");
-      return true;
+
+      assert(0);
     }
 
   private:
@@ -705,7 +757,15 @@ class memory_read_op_t : public operation_t
     // If data is NULL, send the result straight to gdb.
     memory_read_op_t(gdbserver_t& gdbserver, reg_t vaddr, unsigned int length,
         unsigned char *data=NULL) :
-      operation_t(gdbserver), vaddr(vaddr), length(length), data(data) {};
+      operation_t(gdbserver), vaddr(vaddr), length(length), data(data), index(0)
+  {
+    buf = new uint8_t[length];
+  };
+
+    ~memory_read_op_t()
+    {
+      delete[] buf;
+    }
 
     bool perform_step(unsigned int step)
     {
@@ -734,21 +794,24 @@ class memory_read_op_t : public operation_t
         gs.dr_write(SLOT_DATA0, paddr);
         gs.set_interrupt(0);
 
-        if (!data) {
-          gs.start_packet();
-        }
         return false;
       }
 
-      char buffer[3];
+      if (gs.dr_read32(DEBUG_RAM_SIZE / 4 - 1)) {
+        // Note that OpenOCD doesn't report this error to gdb by default. They
+        // think it can mess up stack tracing. So far I haven't seen any
+        // problems.
+        gs.send_packet("E99");
+        return true;
+      }
+
       reg_t value = gs.dr_read(SLOT_DATA1);
       for (unsigned int i = 0; i < access_size; i++) {
         if (data) {
           *(data++) = value & 0xff;
           D(fprintf(stderr, "%02x", (unsigned int) (value & 0xff)));
         } else {
-          sprintf(buffer, "%02x", (unsigned int) (value & 0xff));
-          gs.send(buffer);
+          buf[index++] = value & 0xff;
         }
         value >>= 8;
       }
@@ -760,6 +823,12 @@ class memory_read_op_t : public operation_t
 
       if (length == 0) {
         if (!data) {
+          gs.start_packet();
+          char buffer[3];
+          for (unsigned int i = 0; i < index; i++) {
+            sprintf(buffer, "%02x", (unsigned int) buf[i]);
+            gs.send(buffer);
+          }
           gs.end_packet();
         }
         return true;
@@ -776,6 +845,8 @@ class memory_read_op_t : public operation_t
     unsigned char* data;
     reg_t paddr;
     unsigned int access_size;
+    unsigned int index;
+    uint8_t *buf;
 };
 
 class memory_write_op_t : public operation_t
@@ -811,8 +882,8 @@ class memory_write_op_t : public operation_t
       if (step == 0) {
         access_size = gs.find_access_size(paddr, length);
 
-        D(fprintf(stderr, "write to 0x%lx -> 0x%lx (access=%d): ", vaddr, paddr,
-            access_size));
+        D(fprintf(stderr, "write to 0x%" PRIx64 " -> 0x%" PRIx64 " (access=%d): ",
+              vaddr, paddr, access_size));
         for (unsigned int i = 0; i < length; i++) {
           D(fprintf(stderr, "%02x", data[i]));
         }
@@ -846,8 +917,9 @@ class memory_write_op_t : public operation_t
                 (data[6] << 16) | (data[7] << 24));
             break;
           default:
-            fprintf(stderr, "gdbserver error: write %d bytes to 0x%lx -> 0x%lx; "
-                "access_size=%d\n", length, vaddr, paddr, access_size);
+            fprintf(stderr, "gdbserver error: write %d bytes to 0x%016" PRIx64
+                    " -> 0x%016" PRIx64 "; access_size=%d\n",
+                    length, vaddr, paddr, access_size);
             gs.send_packet("E12");
             return true;
         }
@@ -859,8 +931,8 @@ class memory_write_op_t : public operation_t
       }
 
       if (gs.dr_read32(DEBUG_RAM_SIZE / 4 - 1)) {
-        fprintf(stderr, "Exception happened while writing to 0x%lx -> 0x%lx\n",
-            vaddr, paddr);
+        gs.send_packet("E98");
+        return true;
       }
 
       offset += access_size;
@@ -915,40 +987,6 @@ class collect_translation_info_op_t : public operation_t
 
     bool perform_step(unsigned int step)
     {
-      unsigned int vm = gs.virtual_memory();
-
-      if (step == 0) {
-        switch (vm) {
-          case VM_MBARE:
-            // Nothing to be done.
-            return true;
-
-          case VM_SV32:
-            levels = 2;
-            ptidxbits = 10;
-            ptesize = 4;
-            break;
-          case VM_SV39:
-            levels = 3;
-            ptidxbits = 9;
-            ptesize = 8;
-            break;
-          case VM_SV48:
-            levels = 4;
-            ptidxbits = 9;
-            ptesize = 8;
-            break;
-
-          default:
-            {
-              char buf[100];
-              sprintf(buf, "VM mode %d is not supported by gdbserver.cc.", vm);
-              die(buf);
-              return true;        // die doesn't return, but gcc doesn't know that.
-            }
-        }
-      }
-
       // Perform any reads from the just-completed action.
       switch (state) {
         case STATE_START:
@@ -956,15 +994,19 @@ class collect_translation_info_op_t : public operation_t
         case STATE_READ_SPTBR:
           gs.sptbr = gs.dr_read(SLOT_DATA0);
           gs.sptbr_valid = true;
+          vm = decode_vm_info(gs.xlen, gs.privilege_mode(), gs.sptbr);
+          if (vm.levels == 0)
+            return true;
           break;
         case STATE_READ_PTE:
-          if (ptesize == 4) {
+          if (vm.ptesize == 4) {
               gs.pte_cache[pte_addr] = gs.dr_read32(4);
           } else {
               gs.pte_cache[pte_addr] = ((uint64_t) gs.dr_read32(5) << 32) |
                   gs.dr_read32(4);
           }
-          D(fprintf(stderr, "pte_cache[0x%lx] = 0x%lx\n", pte_addr, gs.pte_cache[pte_addr]));
+          D(fprintf(stderr, "pte_cache[0x%" PRIx64 "] = 0x%" PRIx64 "\n", pte_addr,
+                    gs.pte_cache[pte_addr]));
           break;
       }
 
@@ -980,16 +1022,16 @@ class collect_translation_info_op_t : public operation_t
         return false;
       }
 
-      reg_t base = gs.sptbr << PGSHIFT;
-      int ptshift = (levels - 1) * ptidxbits;
-      for (unsigned int i = 0; i < levels; i++, ptshift -= ptidxbits) {
-        reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << ptidxbits) - 1);
+      reg_t base = vm.ptbase;
+      for (int i = vm.levels - 1; i >= 0; i--) {
+        int ptshift = i * vm.idxbits;
+        reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << vm.idxbits) - 1);
 
-        pte_addr = base + idx * ptesize;
+        pte_addr = base + idx * vm.ptesize;
         auto it = gs.pte_cache.find(pte_addr);
         if (it == gs.pte_cache.end()) {
           state = STATE_READ_PTE;
-          if (ptesize == 4) {
+          if (vm.ptesize == 4) {
             gs.dr_write32(0, lw(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
             gs.dr_write32(1, lw(S1, S0, 0));
             gs.dr_write32(2, sw(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
@@ -999,7 +1041,7 @@ class collect_translation_info_op_t : public operation_t
             gs.dr_write32(1, ld(S1, S0, 0));
             gs.dr_write32(2, sd(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
           }
-          gs.dr_write32(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
+          gs.dr_write_jump(3);
           gs.dr_write32(4, pte_addr);
           gs.dr_write32(5, pte_addr >> 32);
           gs.set_interrupt(0);
@@ -1017,7 +1059,7 @@ class collect_translation_info_op_t : public operation_t
         }
       }
       fprintf(stderr,
-          "ERROR: gdbserver couldn't find appropriate PTEs to translate 0x%lx\n",
+          "ERROR: gdbserver couldn't find appropriate PTEs to translate 0x%016" PRIx64 "\n",
           vaddr);
       return true;
     }
@@ -1030,9 +1072,7 @@ class collect_translation_info_op_t : public operation_t
     } state;
     reg_t vaddr;
     size_t length;
-    unsigned int levels;
-    unsigned int ptidxbits;
-    unsigned int ptesize;
+    vm_info vm;
     reg_t pte_addr;
 };
 
@@ -1069,7 +1109,7 @@ class hardware_breakpoint_insert_op_t : public operation_t
             return true;
           }
 
-          gs.dr_write32(0, csrr(S0, CSR_TDATA0));
+          gs.dr_write32(0, csrr(S0, CSR_TDATA1));
           gs.dr_write_store(1, S0, SLOT_DATA0);
           gs.dr_write_jump(2);
           state = STATE_CHECK_MCONTROL;
@@ -1091,9 +1131,10 @@ class hardware_breakpoint_insert_op_t : public operation_t
                 !get_field(mcontrol, MCONTROL_STORE)) {
               // Found an unused trigger.
               gs.dr_write_load(0, S0, SLOT_DATA1);
-              gs.dr_write32(1, csrw(S0, CSR_TDATA0));
+              gs.dr_write32(1, csrw(S0, CSR_TDATA1));
               gs.dr_write_jump(2);
               mcontrol = set_field(0, MCONTROL_ACTION, MCONTROL_ACTION_DEBUG_MODE);
+              mcontrol = set_field(mcontrol, MCONTROL_DMODE(gs.xlen), 1);
               mcontrol = set_field(mcontrol, MCONTROL_MATCH, MCONTROL_MATCH_EQUAL);
               mcontrol = set_field(mcontrol, MCONTROL_M, 1);
               mcontrol = set_field(mcontrol, MCONTROL_H, 1);
@@ -1102,6 +1143,16 @@ class hardware_breakpoint_insert_op_t : public operation_t
               mcontrol = set_field(mcontrol, MCONTROL_EXECUTE, bp.execute);
               mcontrol = set_field(mcontrol, MCONTROL_LOAD, bp.load);
               mcontrol = set_field(mcontrol, MCONTROL_STORE, bp.store);
+              // For store triggers it's nicer to fire just before the
+              // instruction than just after. However, gdb doesn't clear the
+              // breakpoints and step before resuming from a store trigger.
+              // That means that without extra code, you'll keep hitting the
+              // same watchpoint over and over again. That's not useful at all.
+              // Instead of fixing this the right way, just set timing=1 for
+              // those triggers.
+              if (bp.load || bp.store)
+                mcontrol = set_field(mcontrol, MCONTROL_TIMING, 1);
+
               gs.dr_write(SLOT_DATA1, mcontrol);
               state = STATE_WRITE_ADDRESS;
             } else {
@@ -1115,7 +1166,7 @@ class hardware_breakpoint_insert_op_t : public operation_t
         case STATE_WRITE_ADDRESS:
           {
             gs.dr_write_load(0, S0, SLOT_DATA1);
-            gs.dr_write32(1, csrw(S0, CSR_TDATA1));
+            gs.dr_write32(1, csrw(S0, CSR_TDATA2));
             gs.dr_write_jump(2);
             gs.dr_write(SLOT_DATA1, bp.vaddr);
             gs.set_interrupt(0);
@@ -1141,6 +1192,60 @@ class hardware_breakpoint_insert_op_t : public operation_t
     hardware_breakpoint_t bp;
 };
 
+class maybe_save_tselect_op_t : public operation_t
+{
+  public:
+    maybe_save_tselect_op_t(gdbserver_t& gdbserver) : operation_t(gdbserver) {};
+    bool perform_step(unsigned int step) {
+      if (gs.tselect_valid)
+        return true;
+
+      switch (step) {
+        case 0:
+          gs.dr_write32(0, csrr(S0, CSR_TDATA1));
+          gs.dr_write_store(1, S0, SLOT_DATA0);
+          gs.dr_write_jump(2);
+          gs.set_interrupt(0);
+          return false;
+        case 1:
+          gs.tselect = gs.dr_read(SLOT_DATA0);
+          gs.tselect_valid = true;
+          break;
+      }
+      return true;
+    }
+};
+
+class maybe_restore_tselect_op_t : public operation_t
+{
+  public:
+    maybe_restore_tselect_op_t(gdbserver_t& gdbserver) : operation_t(gdbserver) {};
+    bool perform_step(unsigned int step) {
+      if (gs.tselect_valid) {
+        gs.dr_write_load(0, S0, SLOT_DATA1);
+        gs.dr_write32(1, csrw(S0, CSR_TSELECT));
+        gs.dr_write_jump(2);
+        gs.dr_write(SLOT_DATA1, gs.tselect);
+      }
+      return true;
+    }
+};
+
+class maybe_restore_mstatus_op_t : public operation_t
+{
+  public:
+    maybe_restore_mstatus_op_t(gdbserver_t& gdbserver) : operation_t(gdbserver) {};
+    bool perform_step(unsigned int step) {
+      if (gs.mstatus_dirty) {
+        gs.dr_write_load(0, S0, SLOT_DATA1);
+        gs.dr_write32(1, csrw(S0, CSR_MSTATUS));
+        gs.dr_write_jump(2);
+        gs.dr_write(SLOT_DATA1, gs.mstatus);
+      }
+      return true;
+    }
+};
+
 class hardware_breakpoint_remove_op_t : public operation_t
 {
   public:
@@ -1151,7 +1256,7 @@ class hardware_breakpoint_remove_op_t : public operation_t
     bool perform_step(unsigned int step) {
       gs.dr_write32(0, addi(S0, ZERO, bp.index));
       gs.dr_write32(1, csrw(S0, CSR_TSELECT));
-      gs.dr_write32(2, csrw(ZERO, CSR_TDATA0));
+      gs.dr_write32(2, csrw(ZERO, CSR_TDATA1));
       gs.dr_write_jump(3);
       gs.set_interrupt(0);
       return true;
@@ -1167,7 +1272,8 @@ gdbserver_t::gdbserver_t(uint16_t port, sim_t *sim) :
   xlen(0),
   sim(sim),
   client_fd(0),
-  recv_buf(64 * 1024), send_buf(64 * 1024)
+  // gdb likes to send 0x100000 bytes at once when downloading.
+  recv_buf(0x180000), send_buf(64 * 1024)
 {
   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
   if (socket_fd == -1) {
@@ -1212,50 +1318,22 @@ unsigned int gdbserver_t::find_access_size(reg_t address, int length)
 
 reg_t gdbserver_t::translate(reg_t vaddr)
 {
-  unsigned int vm = virtual_memory();
-  unsigned int levels, ptidxbits, ptesize;
-
-  switch (vm) {
-    case VM_MBARE:
-      return vaddr;
-
-    case VM_SV32:
-      levels = 2;
-      ptidxbits = 10;
-      ptesize = 4;
-      break;
-    case VM_SV39:
-      levels = 3;
-      ptidxbits = 9;
-      ptesize = 8;
-      break;
-    case VM_SV48:
-      levels = 4;
-      ptidxbits = 9;
-      ptesize = 8;
-      break;
-
-    default:
-      {
-        char buf[100];
-        sprintf(buf, "VM mode %d is not supported by gdbserver.cc.", vm);
-        die(buf);
-        return true;        // die doesn't return, but gcc doesn't know that.
-      }
-  }
+  vm_info vm = decode_vm_info(xlen, privilege_mode(), sptbr);
+  if (vm.levels == 0)
+    return vaddr;
 
   // Handle page tables here. There's a bunch of duplicated code with
   // collect_translation_info_op_t. :-(
-  reg_t base = sptbr << PGSHIFT;
-  int ptshift = (levels - 1) * ptidxbits;
-  for (unsigned int i = 0; i < levels; i++, ptshift -= ptidxbits) {
-    reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << ptidxbits) - 1);
+  reg_t base = vm.ptbase;
+  for (int i = vm.levels - 1; i >= 0; i--) {
+    int ptshift = i * vm.idxbits;
+    reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << vm.idxbits) - 1);
 
-    reg_t pte_addr = base + idx * ptesize;
+    reg_t pte_addr = base + idx * vm.ptesize;
     auto it = pte_cache.find(pte_addr);
     if (it == pte_cache.end()) {
-      fprintf(stderr, "ERROR: gdbserver tried to translate 0x%lx without first "
-          "collecting the relevant PTEs.\n", vaddr);
+      fprintf(stderr, "ERROR: gdbserver tried to translate 0x%016" PRIx64
+          " without first collecting the relevant PTEs.\n", vaddr);
       die("gdbserver_t::translate()");
     }
 
@@ -1269,13 +1347,13 @@ reg_t gdbserver_t::translate(reg_t vaddr)
       reg_t vpn = vaddr >> PGSHIFT;
       reg_t paddr = (ppn | (vpn & ((reg_t(1) << ptshift) - 1))) << PGSHIFT;
       paddr += vaddr & (PGSIZE-1);
-      D(fprintf(stderr, "gdbserver translate 0x%lx -> 0x%lx\n", vaddr, paddr));
+      D(fprintf(stderr, "gdbserver translate 0x%" PRIx64 " -> 0x%" PRIx64 "\n", vaddr, paddr));
       return paddr;
     }
   }
 
-  fprintf(stderr, "ERROR: gdbserver tried to translate 0x%lx but the relevant "
-      "PTEs are invalid.\n", vaddr);
+  fprintf(stderr, "ERROR: gdbserver tried to translate 0x%016" PRIx64
+          " but the relevant PTEs are invalid.\n", vaddr);
   // TODO: Is it better to throw an exception here?
   return -1;
 }
@@ -1288,14 +1366,6 @@ unsigned int gdbserver_t::privilege_mode()
   return mode;
 }
 
-unsigned int gdbserver_t::virtual_memory()
-{
-  unsigned int mode = privilege_mode();
-  if (mode == PRV_M)
-    return VM_MBARE;
-  return get_field(mstatus, MSTATUS_VM);
-}
-
 void gdbserver_t::dr_write32(unsigned int index, uint32_t value)
 {
   sim->debug_module.ram_write32(index, value);
@@ -1427,7 +1497,6 @@ void gdbserver_t::read()
   // available.
 
   size_t count = recv_buf.contiguous_empty_size();
-  assert(count > 0);
   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
   if (bytes == -1) {
     if (errno == EAGAIN) {
@@ -1464,8 +1533,8 @@ void gdbserver_t::write()
       // Client can't take any more data right now.
       break;
     } else {
-      D(fprintf(stderr, "wrote %ld bytes: ", bytes));
-      for (unsigned int i = 0; i < bytes; i++) {
+      D(fprintf(stderr, "wrote %zd bytes: ", bytes));
+      for (int i = 0; i < bytes; i++) {
         D(fprintf(stderr, "%c", send_buf[i]));
       }
       D(fprintf(stderr, "\n"));
@@ -1535,7 +1604,7 @@ void gdbserver_t::process_requests()
       if (b == '$') {
         // Start of new packet.
         if (!packet.empty()) {
-          fprintf(stderr, "Received malformed %ld-byte packet from debug client: ",
+          fprintf(stderr, "Received malformed %zd-byte packet from debug client: ",
               packet.size());
           print_packet(packet);
           recv_buf.consume(i);
@@ -1560,6 +1629,25 @@ void gdbserver_t::process_requests()
       break;
     }
   }
+
+  if (recv_buf.full()) {
+    fprintf(stderr,
+        "Receive buffer is full, but no complete packet was found!\n");
+    for (unsigned line = 0; line < 8; line++) {
+      for (unsigned i = 0; i < 16; i++) {
+        fprintf(stderr, "%02x ", recv_buf.entry(line * 16 + i));
+      }
+      for (unsigned i = 0; i < 16; i++) {
+        uint8_t e = recv_buf.entry(line * 16 + i);
+        if (e >= ' ' && e <= '~')
+          fprintf(stderr, "%c", e);
+        else
+          fprintf(stderr, ".");
+      }
+      fprintf(stderr, "\n");
+    }
+    assert(!recv_buf.full());
+  }
 }
 
 void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
@@ -1597,7 +1685,8 @@ uint64_t consume_hex_number(std::vector<uint8_t>::const_iterator &iter,
 
 // First byte is the least-significant one.
 // Eg. "08675309" becomes 0x09536708
-uint64_t consume_hex_number_le(std::vector<uint8_t>::const_iterator &iter,
+uint64_t gdbserver_t::consume_hex_number_le(
+    std::vector<uint8_t>::const_iterator &iter,
     std::vector<uint8_t>::const_iterator end)
 {
   uint64_t value = 0;
@@ -1615,6 +1704,12 @@ uint64_t consume_hex_number_le(std::vector<uint8_t>::const_iterator &iter,
     else
       shift -= 4;
   }
+  if (shift > (xlen+4)) {
+    fprintf(stderr,
+        "gdb sent too many data bytes. That means it thinks XLEN is greater "
+        "than %d.\nTo fix that, tell gdb: set arch riscv:rv%d\n",
+        xlen, xlen);
+  }
   return value;
 }
 
@@ -1656,8 +1751,6 @@ void gdbserver_t::handle_register_write(const std::vector<uint8_t> &packet)
   processor_t *p = sim->get_core(0);
 
   add_operation(new register_write_op_t(*this, n, value));
-
-  return send_packet("OK");
 }
 
 void gdbserver_t::handle_memory_read(const std::vector<uint8_t> &packet)
@@ -1733,6 +1826,8 @@ void gdbserver_t::handle_continue(const std::vector<uint8_t> &packet)
       return send_packet("E30");
   }
 
+  add_operation(new maybe_restore_tselect_op_t(*this));
+  add_operation(new maybe_restore_mstatus_op_t(*this));
   add_operation(new continue_op_t(*this, false));
 }
 
@@ -1747,6 +1842,7 @@ void gdbserver_t::handle_step(const std::vector<uint8_t> &packet)
       return send_packet("E40");
   }
 
+  add_operation(new maybe_restore_tselect_op_t(*this));
   add_operation(new continue_op_t(*this, true));
 }
 
@@ -1772,13 +1868,13 @@ void gdbserver_t::software_breakpoint_insert(reg_t vaddr, unsigned int size)
   add_operation(new collect_translation_info_op_t(*this, vaddr, size));
   unsigned char* inst = new unsigned char[4];
   if (size == 2) {
-    inst[0] = C_EBREAK & 0xff;
-    inst[1] = (C_EBREAK >> 8) & 0xff;
+    inst[0] = MATCH_C_EBREAK & 0xff;
+    inst[1] = (MATCH_C_EBREAK >> 8) & 0xff;
   } else {
-    inst[0] = EBREAK & 0xff;
-    inst[1] = (EBREAK >> 8) & 0xff;
-    inst[2] = (EBREAK >> 16) & 0xff;
-    inst[3] = (EBREAK >> 24) & 0xff;
+    inst[0] = MATCH_EBREAK & 0xff;
+    inst[1] = (MATCH_EBREAK >> 8) & 0xff;
+    inst[2] = (MATCH_EBREAK >> 16) & 0xff;
+    inst[3] = (MATCH_EBREAK >> 24) & 0xff;
   }
 
   software_breakpoint_t bp = {
@@ -1806,11 +1902,13 @@ void gdbserver_t::software_breakpoint_remove(reg_t vaddr, unsigned int size)
 
 void gdbserver_t::hardware_breakpoint_insert(const hardware_breakpoint_t &bp)
 {
+  add_operation(new maybe_save_tselect_op_t(*this));
   add_operation(new hardware_breakpoint_insert_op_t(*this, bp));
 }
 
 void gdbserver_t::hardware_breakpoint_remove(const hardware_breakpoint_t &bp)
 {
+  add_operation(new maybe_save_tselect_op_t(*this));
   hardware_breakpoint_t found = *hardware_breakpoints.find(bp);
   add_operation(new hardware_breakpoint_remove_op_t(*this, found));
 }
@@ -1916,14 +2014,14 @@ void gdbserver_t::handle_query(const std::vector<uint8_t> &packet)
 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
 {
   if (compute_checksum(packet) != extract_checksum(packet)) {
-    fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
+    fprintf(stderr, "Received %zd-byte packet with invalid checksum\n", packet.size());
     fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
     print_packet(packet);
     send("-");
     return;
   }
 
-  D(fprintf(stderr, "Received %ld-byte packet from debug client: ", packet.size()));
+  D(fprintf(stderr, "Received %zd-byte packet from debug client: ", packet.size()));
   D(print_packet(packet));
   send("+");