Merge pull request #62 from riscv/trigger

Implement address and data triggers.

8 files changed, 710 insertions, 58 deletions

diff --git a/riscv/execute.cc b/riscv/execute.cc
index 20567af..7b42262 100644
--- a/riscv/execute.cc
+++ b/riscv/execute.cc
@@ -51,6 +51,11 @@ static reg_t execute_insn(processor_t* p, reg_t pc, insn_fetch_t fetch)
   return npc;
 }
 
+bool processor_t::slow_path()
+{
+  return debug || state.single_step != state.STEP_NONE || state.dcsr.cause;
+}
+
 // fetch/decode/execute loop
 void processor_t::step(size_t n)
 {
@@ -90,8 +95,7 @@ void processor_t::step(size_t n)
     {
       take_interrupt();
 
-      // When we might single step, use the slow loop instead of the fast one.
-      if (unlikely(debug || state.single_step != state.STEP_NONE || state.dcsr.cause))
+      if (unlikely(slow_path()))
       {
         while (instret < n)
         {
@@ -150,6 +154,34 @@ miss:
       take_trap(t, pc);
       n = instret;
     }
+    catch (trigger_matched_t& t)
+    {
+      if (mmu->matched_trigger) {
+        // This exception came from the MMU. That means the instruction hasn't
+        // fully executed yet. We start it again, but this time it won't throw
+        // an exception because matched_trigger is already set. (All memory
+        // instructions are idempotent so restarting is safe.)
+
+        insn_fetch_t fetch = mmu->load_insn(pc);
+        pc = execute_insn(this, pc, fetch);
+        advance_pc();
+
+        delete mmu->matched_trigger;
+        mmu->matched_trigger = NULL;
+      }
+      switch (state.mcontrol[t.index].action) {
+        case ACTION_DEBUG_MODE:
+          enter_debug_mode(DCSR_CAUSE_HWBP);
+          break;
+        case ACTION_DEBUG_EXCEPTION: {
+          mem_trap_t trap(CAUSE_BREAKPOINT, t.address);
+          take_trap(trap, pc);
+          break;
+        }
+        default:
+          abort();
+      }
+    }
 
     state.minstret += instret;
     n -= instret;
diff --git a/riscv/gdbserver.cc b/riscv/gdbserver.cc
index c561390..481e5b9 100644
--- a/riscv/gdbserver.cc
+++ b/riscv/gdbserver.cc
@@ -351,6 +351,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));
@@ -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);
@@ -1036,6 +1038,181 @@ class collect_translation_info_op_t : public operation_t
     reg_t pte_addr;
 };
 
+class hardware_breakpoint_insert_op_t : public operation_t
+{
+  public:
+    hardware_breakpoint_insert_op_t(gdbserver_t& gdbserver,
+        hardware_breakpoint_t bp) :
+      operation_t(gdbserver), state(STATE_START), bp(bp) {};
+
+    void write_new_index_program()
+    {
+      gs.dr_write_load(0, S0, SLOT_DATA1);
+      gs.dr_write32(1, csrw(S0, CSR_TSELECT));
+      gs.dr_write32(2, csrr(S0, CSR_TSELECT));
+      gs.dr_write_store(3, S0, SLOT_DATA1);
+      gs.dr_write_jump(4);
+      gs.dr_write(SLOT_DATA1, bp.index);
+    }
+
+    bool perform_step(unsigned int step)
+    {
+      switch (state) {
+        case STATE_START:
+          bp.index = 0;
+          write_new_index_program();
+          state = STATE_CHECK_INDEX;
+          break;
+
+        case STATE_CHECK_INDEX:
+          if (gs.dr_read(SLOT_DATA1) != bp.index) {
+            // We've exhausted breakpoints without finding an appropriate one.
+            gs.send_packet("E58");
+            return true;
+          }
+
+          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;
+          break;
+
+        case STATE_CHECK_MCONTROL:
+          {
+            reg_t mcontrol = gs.dr_read(SLOT_DATA0);
+            unsigned int type = mcontrol >> (gs.xlen - 4);
+            if (type == 0) {
+              // We've exhausted breakpoints without finding an appropriate one.
+              gs.send_packet("E58");
+              return true;
+            }
+
+            if (type == 2 &&
+                !get_field(mcontrol, MCONTROL_EXECUTE) &&
+                !get_field(mcontrol, MCONTROL_LOAD) &&
+                !get_field(mcontrol, MCONTROL_STORE)) {
+              // Found an unused trigger.
+              gs.dr_write_load(0, S0, SLOT_DATA1);
+              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);
+              mcontrol = set_field(mcontrol, MCONTROL_S, 1);
+              mcontrol = set_field(mcontrol, MCONTROL_U, 1);
+              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 {
+              bp.index++;
+              write_new_index_program();
+              state = STATE_CHECK_INDEX;
+            }
+          }
+          break;
+
+        case STATE_WRITE_ADDRESS:
+          {
+            gs.dr_write_load(0, S0, SLOT_DATA1);
+            gs.dr_write32(1, csrw(S0, CSR_TDATA2));
+            gs.dr_write_jump(2);
+            gs.dr_write(SLOT_DATA1, bp.vaddr);
+            gs.set_interrupt(0);
+            gs.send_packet("OK");
+
+            gs.hardware_breakpoints.insert(bp);
+
+            return true;
+          }
+      }
+
+      gs.set_interrupt(0);
+      return false;
+    }
+
+  private:
+    enum {
+      STATE_START,
+      STATE_CHECK_INDEX,
+      STATE_CHECK_MCONTROL,
+      STATE_WRITE_ADDRESS
+    } state;
+    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 hardware_breakpoint_remove_op_t : public operation_t
+{
+  public:
+    hardware_breakpoint_remove_op_t(gdbserver_t& gdbserver,
+        hardware_breakpoint_t bp) :
+      operation_t(gdbserver), bp(bp) {};
+
+    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_TDATA1));
+      gs.dr_write_jump(3);
+      gs.set_interrupt(0);
+      return true;
+    }
+
+  private:
+    hardware_breakpoint_t bp;
+};
+
 ////////////////////////////// gdbserver itself
 
 gdbserver_t::gdbserver_t(uint16_t port, sim_t *sim) :
@@ -1608,6 +1785,7 @@ 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 continue_op_t(*this, false));
 }
 
@@ -1622,6 +1800,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));
 }
 
@@ -1641,64 +1820,123 @@ void gdbserver_t::handle_extended(const std::vector<uint8_t> &packet)
   extended_mode = true;
 }
 
+void gdbserver_t::software_breakpoint_insert(reg_t vaddr, unsigned int size)
+{
+  fence_i_required = true;
+  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;
+  } else {
+    inst[0] = EBREAK & 0xff;
+    inst[1] = (EBREAK >> 8) & 0xff;
+    inst[2] = (EBREAK >> 16) & 0xff;
+    inst[3] = (EBREAK >> 24) & 0xff;
+  }
+
+  software_breakpoint_t bp = {
+    .vaddr = vaddr,
+    .size = size
+  };
+  software_breakpoints[vaddr] = bp;
+  add_operation(new memory_read_op_t(*this, bp.vaddr, bp.size,
+        software_breakpoints[bp.vaddr].instruction));
+  add_operation(new memory_write_op_t(*this, bp.vaddr, bp.size, inst));
+}
+
+void gdbserver_t::software_breakpoint_remove(reg_t vaddr, unsigned int size)
+{
+  fence_i_required = true;
+  add_operation(new collect_translation_info_op_t(*this, vaddr, size));
+
+  software_breakpoint_t found_bp = software_breakpoints[vaddr];
+  unsigned char* instruction = new unsigned char[4];
+  memcpy(instruction, found_bp.instruction, 4);
+  add_operation(new memory_write_op_t(*this, found_bp.vaddr,
+        found_bp.size, instruction));
+  software_breakpoints.erase(vaddr);
+}
+
+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));
+}
+
 void gdbserver_t::handle_breakpoint(const std::vector<uint8_t> &packet)
 {
-  // insert: Z type,addr,kind
-  // remove: z type,addr,kind
+  // insert: Z type,addr,length
+  // remove: z type,addr,length
+
+  // type: 0 - software breakpoint, 1 - hardware breakpoint, 2 - write
+  // watchpoint, 3 - read watchpoint, 4 - access watchpoint; addr is address;
+  // length is in bytes. For a software breakpoint, length specifies the size
+  // of the instruction to be patched. For hardware breakpoints and watchpoints
+  // length specifies the memory region to be monitored. To avoid potential
+  // problems with duplicate packets, the operations should be implemented in
+  // an idempotent way.
 
-  software_breakpoint_t bp;
   bool insert = (packet[1] == 'Z');
   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
-  int type = consume_hex_number(iter, packet.end());
+  gdb_breakpoint_type_t type = static_cast<gdb_breakpoint_type_t>(
+      consume_hex_number(iter, packet.end()));
   if (*iter != ',')
     return send_packet("E50");
   iter++;
-  bp.address = consume_hex_number(iter, packet.end());
+  reg_t address = consume_hex_number(iter, packet.end());
   if (*iter != ',')
     return send_packet("E51");
   iter++;
-  bp.size = consume_hex_number(iter, packet.end());
+  unsigned int size = consume_hex_number(iter, packet.end());
   // There may be more options after a ; here, but we don't support that.
   if (*iter != '#')
     return send_packet("E52");
 
-  if (type != 0) {
-    // Only software breakpoints are supported.
-    return send_packet("");
-  }
-
-  if (bp.size != 2 && bp.size != 4) {
-    return send_packet("E53");
-  }
-
-  fence_i_required = true;
-  add_operation(new collect_translation_info_op_t(*this, bp.address, bp.size));
-  if (insert) {
-    unsigned char* swbp = new unsigned char[4];
-    if (bp.size == 2) {
-      swbp[0] = C_EBREAK & 0xff;
-      swbp[1] = (C_EBREAK >> 8) & 0xff;
-    } else {
-      swbp[0] = EBREAK & 0xff;
-      swbp[1] = (EBREAK >> 8) & 0xff;
-      swbp[2] = (EBREAK >> 16) & 0xff;
-      swbp[3] = (EBREAK >> 24) & 0xff;
-    }
+  switch (type) {
+    case GB_SOFTWARE:
+      if (size != 2 && size != 4) {
+        return send_packet("E53");
+      }
+      if (insert) {
+        software_breakpoint_insert(address, size);
+      } else {
+        software_breakpoint_remove(address, size);
+      }
+      break;
 
-    breakpoints[bp.address] = new software_breakpoint_t(bp);
-    add_operation(new memory_read_op_t(*this, bp.address, bp.size,
-          breakpoints[bp.address]->instruction));
-    add_operation(new memory_write_op_t(*this, bp.address, bp.size, swbp));
+    case GB_HARDWARE:
+    case GB_WRITE:
+    case GB_READ:
+    case GB_ACCESS:
+      {
+        hardware_breakpoint_t bp = {
+          .vaddr = address,
+          .size = size
+        };
+        bp.load = (type == GB_READ || type == GB_ACCESS);
+        bp.store = (type == GB_WRITE || type == GB_ACCESS);
+        bp.execute = (type == GB_HARDWARE || type == GB_ACCESS);
+        if (insert) {
+          hardware_breakpoint_insert(bp);
+          // Insert might fail if there's no space, so the insert operation will
+          // send its own OK (or not).
+          return;
+        } else {
+          hardware_breakpoint_remove(bp);
+        }
+      }
+      break;
 
-  } else {
-    software_breakpoint_t *found_bp;
-    found_bp = breakpoints[bp.address];
-    unsigned char* instruction = new unsigned char[4];
-    memcpy(instruction, found_bp->instruction, 4);
-    add_operation(new memory_write_op_t(*this, found_bp->address,
-          found_bp->size, instruction));
-    breakpoints.erase(bp.address);
-    delete found_bp;
+    default:
+      return send_packet("E56");
   }
 
   return send_packet("OK");
diff --git a/riscv/gdbserver.h b/riscv/gdbserver.h
index f0e7fca..c2689da 100644
--- a/riscv/gdbserver.h
+++ b/riscv/gdbserver.h
@@ -48,10 +48,27 @@ public:
 // Class to track software breakpoints that we set.
 class software_breakpoint_t
 {
-public:
-  reg_t address;
-  unsigned int size;
-  unsigned char instruction[4];
+  public:
+    reg_t vaddr;
+    unsigned int size;
+    unsigned char instruction[4];
+};
+
+class hardware_breakpoint_t
+{
+  public:
+    reg_t vaddr;
+    unsigned int size;
+    unsigned int index;
+    bool load, store, execute;
+};
+
+struct hardware_breakpoint_compare_t {
+  bool operator()(const hardware_breakpoint_t& a, const hardware_breakpoint_t& b) const {
+    if (a.vaddr != b.vaddr)
+      return a.vaddr < b.vaddr;
+    return a.size < b.size;
+  }
 };
 
 class gdbserver_t;
@@ -111,6 +128,14 @@ static const unsigned int slot_offset32[] = {0, 4, 5, DEBUG_RAM_SIZE/4 - 1};
 static const unsigned int slot_offset64[] = {0, 4, 6, DEBUG_RAM_SIZE/4 - 2};
 static const unsigned int slot_offset128[] = {0, 4, 8, DEBUG_RAM_SIZE/4 - 4};
 
+typedef enum {
+  GB_SOFTWARE = 0,
+  GB_HARDWARE = 1,
+  GB_WRITE = 2,
+  GB_READ = 3,
+  GB_ACCESS = 4,
+} gdb_breakpoint_type_t;
+
 class gdbserver_t
 {
 public:
@@ -124,6 +149,11 @@ public:
   void handle_packet(const std::vector<uint8_t> &packet);
   void handle_interrupt();
 
+  void software_breakpoint_remove(reg_t vaddr, unsigned int size);
+  void software_breakpoint_insert(reg_t vaddr, unsigned int size);
+  void hardware_breakpoint_remove(const hardware_breakpoint_t &bp);
+  void hardware_breakpoint_insert(const hardware_breakpoint_t &bp);
+
   void handle_breakpoint(const std::vector<uint8_t> &packet);
   void handle_continue(const std::vector<uint8_t> &packet);
   void handle_extended(const std::vector<uint8_t> &packet);
@@ -185,6 +215,8 @@ public:
   reg_t mstatus;
   reg_t sptbr;
   bool sptbr_valid;
+  reg_t tselect;
+  bool tselect_valid;
   bool fence_i_required;
 
   std::map<reg_t, reg_t> pte_cache;
@@ -199,6 +231,9 @@ public:
 
   unsigned int xlen;
 
+  std::set<hardware_breakpoint_t, hardware_breakpoint_compare_t>
+    hardware_breakpoints;
+
 private:
   sim_t *sim;
   int socket_fd;
@@ -212,7 +247,7 @@ private:
   // but it isn't, we need to tell gdb about it.
   bool running;
 
-  std::map <reg_t, software_breakpoint_t*> breakpoints;
+  std::map<reg_t, software_breakpoint_t> software_breakpoints;
 
   // Read pending data from the client.
   void read();
diff --git a/riscv/mmu.cc b/riscv/mmu.cc
index 4b7166f..878d849 100644
--- a/riscv/mmu.cc
+++ b/riscv/mmu.cc
@@ -5,7 +5,11 @@
 #include "processor.h"
 
 mmu_t::mmu_t(sim_t* sim, processor_t* proc)
- : sim(sim), proc(proc)
+ : sim(sim), proc(proc),
+  check_triggers_fetch(false),
+  check_triggers_load(false),
+  check_triggers_store(false),
+  matched_trigger(NULL)
 {
   flush_tlb();
 }
@@ -69,9 +73,36 @@ const uint16_t* mmu_t::fetch_slow_path(reg_t vaddr)
   }
 }
 
+reg_t reg_from_bytes(size_t len, const uint8_t* bytes)
+{
+  switch (len) {
+    case 1:
+      return bytes[0];
+    case 2:
+      return bytes[0] |
+        (((reg_t) bytes[1]) << 8);
+    case 4:
+      return bytes[0] |
+        (((reg_t) bytes[1]) << 8) |
+        (((reg_t) bytes[2]) << 16) |
+        (((reg_t) bytes[3]) << 24);
+    case 8:
+      return bytes[0] |
+        (((reg_t) bytes[1]) << 8) |
+        (((reg_t) bytes[2]) << 16) |
+        (((reg_t) bytes[3]) << 24) |
+        (((reg_t) bytes[4]) << 32) |
+        (((reg_t) bytes[5]) << 40) |
+        (((reg_t) bytes[6]) << 48) |
+        (((reg_t) bytes[7]) << 56);
+  }
+  abort();
+}
+
 void mmu_t::load_slow_path(reg_t addr, reg_t len, uint8_t* bytes)
 {
   reg_t paddr = translate(addr, LOAD);
+
   if (sim->addr_is_mem(paddr)) {
     memcpy(bytes, sim->addr_to_mem(paddr), len);
     if (tracer.interested_in_range(paddr, paddr + PGSIZE, LOAD))
@@ -81,11 +112,26 @@ void mmu_t::load_slow_path(reg_t addr, reg_t len, uint8_t* bytes)
   } else if (!sim->mmio_load(paddr, len, bytes)) {
     throw trap_load_access_fault(addr);
   }
+
+  if (!matched_trigger) {
+    reg_t data = reg_from_bytes(len, bytes);
+    matched_trigger = trigger_exception(OPERATION_LOAD, addr, data);
+    if (matched_trigger)
+      throw *matched_trigger;
+  }
 }
 
 void mmu_t::store_slow_path(reg_t addr, reg_t len, const uint8_t* bytes)
 {
   reg_t paddr = translate(addr, STORE);
+
+  if (!matched_trigger) {
+    reg_t data = reg_from_bytes(len, bytes);
+    matched_trigger = trigger_exception(OPERATION_STORE, addr, data);
+    if (matched_trigger)
+      throw *matched_trigger;
+  }
+
   if (sim->addr_is_mem(paddr)) {
     memcpy(sim->addr_to_mem(paddr), bytes, len);
     if (tracer.interested_in_range(paddr, paddr + PGSIZE, STORE))
@@ -102,9 +148,17 @@ void mmu_t::refill_tlb(reg_t vaddr, reg_t paddr, access_type type)
   reg_t idx = (vaddr >> PGSHIFT) % TLB_ENTRIES;
   reg_t expected_tag = vaddr >> PGSHIFT;
 
-  if (tlb_load_tag[idx] != expected_tag) tlb_load_tag[idx] = -1;
-  if (tlb_store_tag[idx] != expected_tag) tlb_store_tag[idx] = -1;
-  if (tlb_insn_tag[idx] != expected_tag) tlb_insn_tag[idx] = -1;
+  if ((tlb_load_tag[idx] & ~TLB_CHECK_TRIGGERS) != expected_tag)
+    tlb_load_tag[idx] = -1;
+  if ((tlb_store_tag[idx] & ~TLB_CHECK_TRIGGERS) != expected_tag)
+    tlb_store_tag[idx] = -1;
+  if ((tlb_insn_tag[idx] & ~TLB_CHECK_TRIGGERS) != expected_tag)
+    tlb_insn_tag[idx] = -1;
+
+  if ((check_triggers_fetch && type == FETCH) ||
+      (check_triggers_load && type == LOAD) ||
+      (check_triggers_store && type == STORE))
+    expected_tag |= TLB_CHECK_TRIGGERS;
 
   if (type == FETCH) tlb_insn_tag[idx] = expected_tag;
   else if (type == STORE) tlb_store_tag[idx] = expected_tag;
diff --git a/riscv/mmu.h b/riscv/mmu.h
index 0652d47..1f8d34b 100644
--- a/riscv/mmu.h
+++ b/riscv/mmu.h
@@ -30,6 +30,19 @@ struct icache_entry_t {
   insn_fetch_t data;
 };
 
+class trigger_matched_t
+{
+  public:
+    trigger_matched_t(int index,
+        trigger_operation_t operation, reg_t address, reg_t data) :
+      index(index), operation(operation), address(address), data(data) {}
+
+    int index;
+    trigger_operation_t operation;
+    reg_t address;
+    reg_t data;
+};
+
 // this class implements a processor's port into the virtual memory system.
 // an MMU and instruction cache are maintained for simulator performance.
 class mmu_t
@@ -46,6 +59,15 @@ public:
       reg_t vpn = addr >> PGSHIFT; \
       if (likely(tlb_load_tag[vpn % TLB_ENTRIES] == vpn)) \
         return *(type##_t*)(tlb_data[vpn % TLB_ENTRIES] + addr); \
+      if (unlikely(tlb_load_tag[vpn % TLB_ENTRIES] == (vpn | TLB_CHECK_TRIGGERS))) { \
+        type##_t data = *(type##_t*)(tlb_data[vpn % TLB_ENTRIES] + addr); \
+        if (!matched_trigger) { \
+          matched_trigger = trigger_exception(OPERATION_LOAD, addr, data); \
+          if (matched_trigger) \
+            throw *matched_trigger; \
+        } \
+        return data; \
+      } \
       type##_t res; \
       load_slow_path(addr, sizeof(type##_t), (uint8_t*)&res); \
       return res; \
@@ -71,6 +93,14 @@ public:
       reg_t vpn = addr >> PGSHIFT; \
       if (likely(tlb_store_tag[vpn % TLB_ENTRIES] == vpn)) \
         *(type##_t*)(tlb_data[vpn % TLB_ENTRIES] + addr) = val; \
+      else if (unlikely(tlb_store_tag[vpn % TLB_ENTRIES] == (vpn | TLB_CHECK_TRIGGERS))) { \
+        if (!matched_trigger) { \
+          matched_trigger = trigger_exception(OPERATION_STORE, addr, val); \
+          if (matched_trigger) \
+            throw *matched_trigger; \
+        } \
+        *(type##_t*)(tlb_data[vpn % TLB_ENTRIES] + addr) = val; \
+      } \
       else \
         store_slow_path(addr, sizeof(type##_t), (const uint8_t*)&val); \
     }
@@ -150,6 +180,9 @@ private:
 
   // implement a TLB for simulator performance
   static const reg_t TLB_ENTRIES = 256;
+  // If a TLB tag has TLB_CHECK_TRIGGERS set, then the MMU must check for a
+  // trigger match before completing an access.
+  static const reg_t TLB_CHECK_TRIGGERS = 1L<<63;
   char* tlb_data[TLB_ENTRIES];
   reg_t tlb_insn_tag[TLB_ENTRIES];
   reg_t tlb_load_tag[TLB_ENTRIES];
@@ -173,9 +206,37 @@ private:
     reg_t vpn = addr >> PGSHIFT;
     if (likely(tlb_insn_tag[vpn % TLB_ENTRIES] == vpn))
       return (uint16_t*)(tlb_data[vpn % TLB_ENTRIES] + addr);
+    if (unlikely(tlb_insn_tag[vpn % TLB_ENTRIES] == (vpn | TLB_CHECK_TRIGGERS))) {
+      uint16_t* ptr = (uint16_t*)(tlb_data[vpn % TLB_ENTRIES] + addr);
+      int match = proc->trigger_match(OPERATION_EXECUTE, addr, *ptr);
+      if (match >= 0)
+        throw trigger_matched_t(match, OPERATION_EXECUTE, addr, *ptr);
+      return ptr;
+    }
     return fetch_slow_path(addr);
   }
 
+  inline trigger_matched_t *trigger_exception(trigger_operation_t operation,
+      reg_t address, reg_t data)
+  {
+    if (!proc) {
+      return NULL;
+    }
+    int match = proc->trigger_match(operation, address, data);
+    if (match == -1)
+      return NULL;
+    if (proc->state.mcontrol[match].timing == 0) {
+      throw trigger_matched_t(match, operation, address, data);
+    }
+    return new trigger_matched_t(match, operation, address, data);
+  }
+
+  bool check_triggers_fetch;
+  bool check_triggers_load;
+  bool check_triggers_store;
+  // The exception describing a matched trigger, or NULL.
+  trigger_matched_t *matched_trigger;
+
   friend class processor_t;
 };
 
diff --git a/riscv/processor.cc b/riscv/processor.cc
index 78fb5f1..6d0b983 100644
--- a/riscv/processor.cc
+++ b/riscv/processor.cc
@@ -118,6 +118,9 @@ void state_t::reset()
   pc = DEFAULT_RSTVEC;
   mtvec = DEFAULT_MTVEC;
   load_reservation = -1;
+  tselect = 0;
+  for (unsigned int i = 0; i < num_triggers; i++)
+    mcontrol[i].type = 2;
 }
 
 void processor_t::set_debug(bool value)
@@ -154,6 +157,7 @@ void processor_t::raise_interrupt(reg_t which)
   throw trap_t(((reg_t)1 << (max_xlen-1)) | which);
 }
 
+// Count number of contiguous 0 bits starting from the LSB.
 static int ctz(reg_t val)
 {
   int res = 0;
@@ -195,7 +199,6 @@ void processor_t::enter_debug_mode(uint8_t cause)
   set_privilege(PRV_M);
   state.dpc = state.pc;
   state.pc = DEBUG_ROM_START;
-  //debug = true; // TODO
 }
 
 void processor_t::take_trap(trap_t& t, reg_t epc)
@@ -389,6 +392,43 @@ void processor_t::set_csr(int which, reg_t val)
     case CSR_MSCRATCH: state.mscratch = val; break;
     case CSR_MCAUSE: state.mcause = val; break;
     case CSR_MBADADDR: state.mbadaddr = val; break;
+    case CSR_TSELECT:
+      if (val < state.num_triggers) {
+        state.tselect = val;
+      }
+      break;
+    case CSR_TDATA1:
+      {
+        mcontrol_t *mc = &state.mcontrol[state.tselect];
+        if (mc->dmode && !state.dcsr.cause) {
+          throw trap_illegal_instruction();
+        }
+        mc->dmode = get_field(val, MCONTROL_DMODE(xlen));
+        mc->select = get_field(val, MCONTROL_SELECT);
+        mc->timing = get_field(val, MCONTROL_TIMING);
+        mc->action = (mcontrol_action_t) get_field(val, MCONTROL_ACTION);
+        mc->chain = get_field(val, MCONTROL_CHAIN);
+        mc->match = (mcontrol_match_t) get_field(val, MCONTROL_MATCH);
+        mc->m = get_field(val, MCONTROL_M);
+        mc->h = get_field(val, MCONTROL_H);
+        mc->s = get_field(val, MCONTROL_S);
+        mc->u = get_field(val, MCONTROL_U);
+        mc->execute = get_field(val, MCONTROL_EXECUTE);
+        mc->store = get_field(val, MCONTROL_STORE);
+        mc->load = get_field(val, MCONTROL_LOAD);
+        // Assume we're here because of csrw.
+        if (mc->execute)
+          mc->timing = 0;
+        if (mc->load)
+          mc->timing = 1;
+        trigger_updated();
+      }
+      break;
+    case CSR_TDATA2:
+      if (state.tselect < state.num_triggers) {
+        state.tdata2[state.tselect] = val;
+      }
+      break;
     case CSR_DCSR:
       state.dcsr.prv = get_field(val, DCSR_PRV);
       state.dcsr.step = get_field(val, DCSR_STEP);
@@ -494,9 +534,38 @@ reg_t processor_t::get_csr(int which)
     case CSR_MTVEC: return state.mtvec;
     case CSR_MEDELEG: return state.medeleg;
     case CSR_MIDELEG: return state.mideleg;
-    case CSR_TSELECT: return 0;
-    case CSR_TDATA1: return 0;
-    case CSR_TDATA2: return 0;
+    case CSR_TSELECT: return state.tselect;
+    case CSR_TDATA1:
+      if (state.tselect < state.num_triggers) {
+        reg_t v = 0;
+        mcontrol_t *mc = &state.mcontrol[state.tselect];
+        v = set_field(v, MCONTROL_TYPE(xlen), mc->type);
+        v = set_field(v, MCONTROL_DMODE(xlen), mc->dmode);
+        v = set_field(v, MCONTROL_MASKMAX(xlen), mc->maskmax);
+        v = set_field(v, MCONTROL_SELECT, mc->select);
+        v = set_field(v, MCONTROL_TIMING, mc->timing);
+        v = set_field(v, MCONTROL_ACTION, mc->action);
+        v = set_field(v, MCONTROL_CHAIN, mc->chain);
+        v = set_field(v, MCONTROL_MATCH, mc->match);
+        v = set_field(v, MCONTROL_M, mc->m);
+        v = set_field(v, MCONTROL_H, mc->h);
+        v = set_field(v, MCONTROL_S, mc->s);
+        v = set_field(v, MCONTROL_U, mc->u);
+        v = set_field(v, MCONTROL_EXECUTE, mc->execute);
+        v = set_field(v, MCONTROL_STORE, mc->store);
+        v = set_field(v, MCONTROL_LOAD, mc->load);
+        return v;
+      } else {
+        return 0;
+      }
+      break;
+    case CSR_TDATA2:
+      if (state.tselect < state.num_triggers) {
+        return state.tdata2[state.tselect];
+      } else {
+        return 0;
+      }
+      break;
     case CSR_TDATA3: return 0;
     case CSR_DCSR:
       {
@@ -627,3 +696,23 @@ bool processor_t::store(reg_t addr, size_t len, const uint8_t* bytes)
       return false;
   }
 }
+
+void processor_t::trigger_updated()
+{
+  mmu->flush_tlb();
+  mmu->check_triggers_fetch = false;
+  mmu->check_triggers_load = false;
+  mmu->check_triggers_store = false;
+
+  for (unsigned i = 0; i < state.num_triggers; i++) {
+    if (state.mcontrol[i].execute) {
+      mmu->check_triggers_fetch = true;
+    }
+    if (state.mcontrol[i].load) {
+      mmu->check_triggers_load = true;
+    }
+    if (state.mcontrol[i].store) {
+      mmu->check_triggers_store = true;
+    }
+  }
+}
diff --git a/riscv/processor.h b/riscv/processor.h
index 3f3d66b..4d8dd64 100644
--- a/riscv/processor.h
+++ b/riscv/processor.h
@@ -43,17 +43,57 @@ typedef struct
   uint8_t cause;
 } dcsr_t;
 
+typedef enum
+{
+  ACTION_DEBUG_EXCEPTION = MCONTROL_ACTION_DEBUG_EXCEPTION,
+  ACTION_DEBUG_MODE = MCONTROL_ACTION_DEBUG_MODE,
+  ACTION_TRACE_START = MCONTROL_ACTION_TRACE_START,
+  ACTION_TRACE_STOP = MCONTROL_ACTION_TRACE_STOP,
+  ACTION_TRACE_EMIT = MCONTROL_ACTION_TRACE_EMIT
+} mcontrol_action_t;
+
+typedef enum
+{
+  MATCH_EQUAL = MCONTROL_MATCH_EQUAL,
+  MATCH_NAPOT = MCONTROL_MATCH_NAPOT,
+  MATCH_GE = MCONTROL_MATCH_GE,
+  MATCH_LT = MCONTROL_MATCH_LT,
+  MATCH_MASK_LOW = MCONTROL_MATCH_MASK_LOW,
+  MATCH_MASK_HIGH = MCONTROL_MATCH_MASK_HIGH
+} mcontrol_match_t;
+
+typedef struct
+{
+  uint8_t type;
+  bool dmode;
+  uint8_t maskmax;
+  bool select;
+  bool timing;
+  mcontrol_action_t action;
+  bool chain;
+  mcontrol_match_t match;
+  bool m;
+  bool h;
+  bool s;
+  bool u;
+  bool execute;
+  bool store;
+  bool load;
+} mcontrol_t;
+
 // architectural state of a RISC-V hart
 struct state_t
 {
   void reset();
 
+  static const int num_triggers = 4;
+
   reg_t pc;
   regfile_t<reg_t, NXPR, true> XPR;
   regfile_t<freg_t, NFPR, false> FPR;
 
   // control and status registers
-  reg_t prv;
+  reg_t prv;    // TODO: Can this be an enum instead?
   reg_t mstatus;
   reg_t mepc;
   reg_t mbadaddr;
@@ -76,6 +116,9 @@ struct state_t
   reg_t dpc;
   reg_t dscratch;
   dcsr_t dcsr;
+  reg_t tselect;
+  mcontrol_t mcontrol[num_triggers];
+  reg_t tdata2[num_triggers];
 
   uint32_t fflags;
   uint32_t frm;
@@ -97,6 +140,21 @@ struct state_t
 #endif
 };
 
+typedef enum {
+  OPERATION_EXECUTE,
+  OPERATION_STORE,
+  OPERATION_LOAD,
+} trigger_operation_t;
+
+// Count number of contiguous 1 bits starting from the LSB.
+static int cto(reg_t val)
+{
+  int res = 0;
+  while ((val & 1) == 1)
+    val >>= 1, res++;
+  return res;
+}
+
 // this class represents one processor in a RISC-V machine.
 class processor_t : public abstract_device_t
 {
@@ -132,6 +190,91 @@ public:
 
   // When true, display disassembly of each instruction that's executed.
   bool debug;
+  // When true, take the slow simulation path.
+  bool slow_path();
+
+  // Return the index of a trigger that matched, or -1.
+  inline int trigger_match(trigger_operation_t operation, reg_t address, reg_t data)
+  {
+    if (state.dcsr.cause)
+      return -1;
+
+    bool chain_ok = true;
+
+    for (unsigned int i = 0; i < state.num_triggers; i++) {
+      if (!chain_ok) {
+        chain_ok |= !state.mcontrol[i].chain;
+        continue;
+      }
+
+      if ((operation == OPERATION_EXECUTE && !state.mcontrol[i].execute) ||
+          (operation == OPERATION_STORE && !state.mcontrol[i].store) ||
+          (operation == OPERATION_LOAD && !state.mcontrol[i].load) ||
+          (state.prv == PRV_M && !state.mcontrol[i].m) ||
+          (state.prv == PRV_H && !state.mcontrol[i].h) ||
+          (state.prv == PRV_S && !state.mcontrol[i].s) ||
+          (state.prv == PRV_U && !state.mcontrol[i].u)) {
+        continue;
+      }
+
+      reg_t value;
+      if (state.mcontrol[i].select) {
+        value = data;
+      } else {
+        value = address;
+      }
+
+      // We need this because in 32-bit mode sometimes the PC bits get sign
+      // extended.
+      if (xlen == 32) {
+        value &= 0xffffffff;
+      }
+
+      switch (state.mcontrol[i].match) {
+        case MATCH_EQUAL:
+          if (value != state.tdata2[i])
+            continue;
+          break;
+        case MATCH_NAPOT:
+          {
+            reg_t mask = ~((1 << cto(state.tdata2[i])) - 1);
+            if ((value & mask) != (state.tdata2[i] & mask))
+              continue;
+          }
+          break;
+        case MATCH_GE:
+          if (value < state.tdata2[i])
+            continue;
+          break;
+        case MATCH_LT:
+          if (value >= state.tdata2[i])
+            continue;
+          break;
+        case MATCH_MASK_LOW:
+          {
+            reg_t mask = state.tdata2[i] >> (xlen/2);
+            if ((value & mask) != (state.tdata2[i] & mask))
+              continue;
+          }
+          break;
+        case MATCH_MASK_HIGH:
+          {
+            reg_t mask = state.tdata2[i] >> (xlen/2);
+            if (((value >> (xlen/2)) & mask) != (state.tdata2[i] & mask))
+              continue;
+          }
+          break;
+      }
+
+      if (!state.mcontrol[i].chain) {
+        return i;
+      }
+      chain_ok = true;
+    }
+    return -1;
+  }
+
+  void trigger_updated();
 
 private:
   sim_t* sim;
diff --git a/riscv/sim.cc b/riscv/sim.cc
index 283d2da..0e7c387 100644
--- a/riscv/sim.cc
+++ b/riscv/sim.cc
@@ -78,7 +78,7 @@ void sim_t::main()
     else
       step(INTERLEAVE);
     if (gdbserver) {
-        gdbserver->handle();
+      gdbserver->handle();
     }
   }
 }