target-xtensa: add DBREAK data breakpoints

Add DBREAKA/DBREAKC SRs and implement DBREAK breakpoints as debug
watchpoints.

This implementation is not fully compliant to ISA: when a breakpoint is
set to an unmapped/inaccessible memory address it generates TLB/memory
protection exception instead of debug exception.

See ISA, 4.7.7.3, 4.7.7.6 for more details.

Signed-off-by: Max Filippov <jcmvbkbc@gmail.com>

5 files changed, 147 insertions, 0 deletions

diff --git a/target-xtensa/cpu.h b/target-xtensa/cpu.h
index 92441e34f9..fb8a727c66 100644
--- a/target-xtensa/cpu.h
+++ b/target-xtensa/cpu.h
@@ -128,6 +128,8 @@ enum {
     DTLBCFG = 92,
     IBREAKENABLE = 96,
     IBREAKA = 128,
+    DBREAKA = 144,
+    DBREAKC = 160,
     EPC1 = 177,
     DEPC = 192,
     EPS2 = 194,
@@ -175,12 +177,18 @@ enum {
 #define DEBUGCAUSE_DBNUM 0xf00
 #define DEBUGCAUSE_DBNUM_SHIFT 8
 
+#define DBREAKC_SB 0x80000000
+#define DBREAKC_LB 0x40000000
+#define DBREAKC_SB_LB (DBREAKC_SB | DBREAKC_LB)
+#define DBREAKC_MASK 0x3f
+
 #define MAX_NAREG 64
 #define MAX_NINTERRUPT 32
 #define MAX_NLEVEL 6
 #define MAX_NNMI 1
 #define MAX_NCCOMPARE 3
 #define MAX_TLB_WAY_SIZE 8
+#define MAX_NDBREAK 2
 
 #define REGION_PAGE_MASK 0xe0000000
 
@@ -330,6 +338,9 @@ typedef struct CPUXtensaState {
 
     int exception_taken;
 
+    /* Watchpoints for DBREAK registers */
+    CPUWatchpoint *cpu_watchpoint[MAX_NDBREAK];
+
     CPU_COMMON
 } CPUXtensaState;
 
@@ -365,6 +376,7 @@ int xtensa_get_physical_addr(CPUState *env,
         uint32_t vaddr, int is_write, int mmu_idx,
         uint32_t *paddr, uint32_t *page_size, unsigned *access);
 void dump_mmu(FILE *f, fprintf_function cpu_fprintf, CPUState *env);
+void debug_exception_env(CPUState *new_env, uint32_t cause);
 
 
 #define XTENSA_OPTION_BIT(opt) (((uint64_t)1) << (opt))
diff --git a/target-xtensa/helper.c b/target-xtensa/helper.c
index 0a26f8dd3a..2ef50d656e 100644
--- a/target-xtensa/helper.c
+++ b/target-xtensa/helper.c
@@ -58,9 +58,44 @@ void xtensa_register_core(XtensaConfigList *node)
     xtensa_cores = node;
 }
 
+static uint32_t check_hw_breakpoints(CPUState *env)
+{
+    unsigned i;
+
+    for (i = 0; i < env->config->ndbreak; ++i) {
+        if (env->cpu_watchpoint[i] &&
+                env->cpu_watchpoint[i]->flags & BP_WATCHPOINT_HIT) {
+            return DEBUGCAUSE_DB | (i << DEBUGCAUSE_DBNUM_SHIFT);
+        }
+    }
+    return 0;
+}
+
+static CPUDebugExcpHandler *prev_debug_excp_handler;
+
+static void breakpoint_handler(CPUState *env)
+{
+    if (env->watchpoint_hit) {
+        if (env->watchpoint_hit->flags & BP_CPU) {
+            uint32_t cause;
+
+            env->watchpoint_hit = NULL;
+            cause = check_hw_breakpoints(env);
+            if (cause) {
+                debug_exception_env(env, cause);
+            }
+            cpu_resume_from_signal(env, NULL);
+        }
+    }
+    if (prev_debug_excp_handler) {
+        prev_debug_excp_handler(env);
+    }
+}
+
 CPUXtensaState *cpu_xtensa_init(const char *cpu_model)
 {
     static int tcg_inited;
+    static int debug_handler_inited;
     CPUXtensaState *env;
     const XtensaConfig *config = NULL;
     XtensaConfigList *core = xtensa_cores;
@@ -84,6 +119,12 @@ CPUXtensaState *cpu_xtensa_init(const char *cpu_model)
         xtensa_translate_init();
     }
 
+    if (!debug_handler_inited && tcg_enabled()) {
+        debug_handler_inited = 1;
+        prev_debug_excp_handler =
+            cpu_set_debug_excp_handler(breakpoint_handler);
+    }
+
     xtensa_irq_init(env);
     qemu_init_vcpu(env);
     return env;
diff --git a/target-xtensa/helpers.h b/target-xtensa/helpers.h
index afe39d4fc6..48a741e46d 100644
--- a/target-xtensa/helpers.h
+++ b/target-xtensa/helpers.h
@@ -33,5 +33,7 @@ DEF_HELPER_3(wtlb, void, i32, i32, i32)
 
 DEF_HELPER_1(wsr_ibreakenable, void, i32)
 DEF_HELPER_2(wsr_ibreaka, void, i32, i32)
+DEF_HELPER_2(wsr_dbreaka, void, i32, i32)
+DEF_HELPER_2(wsr_dbreakc, void, i32, i32)
 
 #include "def-helper.h"
diff --git a/target-xtensa/op_helper.c b/target-xtensa/op_helper.c
index 1feaaee7f0..e184cf64f0 100644
--- a/target-xtensa/op_helper.c
+++ b/target-xtensa/op_helper.c
@@ -134,6 +134,14 @@ void HELPER(exception_cause_vaddr)(uint32_t pc, uint32_t cause, uint32_t vaddr)
     HELPER(exception_cause)(pc, cause);
 }
 
+void debug_exception_env(CPUState *new_env, uint32_t cause)
+{
+    if (xtensa_get_cintlevel(new_env) < new_env->config->debug_level) {
+        env = new_env;
+        HELPER(debug_exception)(env->pc, cause);
+    }
+}
+
 void HELPER(debug_exception)(uint32_t pc, uint32_t cause)
 {
     unsigned level = env->config->debug_level;
@@ -700,3 +708,57 @@ void HELPER(wsr_ibreaka)(uint32_t i, uint32_t v)
     }
     env->sregs[IBREAKA + i] = v;
 }
+
+static void set_dbreak(unsigned i, uint32_t dbreaka, uint32_t dbreakc)
+{
+    int flags = BP_CPU | BP_STOP_BEFORE_ACCESS;
+    uint32_t mask = dbreakc | ~DBREAKC_MASK;
+
+    if (env->cpu_watchpoint[i]) {
+        cpu_watchpoint_remove_by_ref(env, env->cpu_watchpoint[i]);
+    }
+    if (dbreakc & DBREAKC_SB) {
+        flags |= BP_MEM_WRITE;
+    }
+    if (dbreakc & DBREAKC_LB) {
+        flags |= BP_MEM_READ;
+    }
+    /* contiguous mask after inversion is one less than some power of 2 */
+    if ((~mask + 1) & ~mask) {
+        qemu_log("DBREAKC mask is not contiguous: 0x%08x\n", dbreakc);
+        /* cut mask after the first zero bit */
+        mask = 0xffffffff << (32 - clo32(mask));
+    }
+    if (cpu_watchpoint_insert(env, dbreaka & mask, ~mask + 1,
+            flags, &env->cpu_watchpoint[i])) {
+        env->cpu_watchpoint[i] = NULL;
+        qemu_log("Failed to set data breakpoint at 0x%08x/%d\n",
+                dbreaka & mask, ~mask + 1);
+    }
+}
+
+void HELPER(wsr_dbreaka)(uint32_t i, uint32_t v)
+{
+    uint32_t dbreakc = env->sregs[DBREAKC + i];
+
+    if ((dbreakc & DBREAKC_SB_LB) &&
+            env->sregs[DBREAKA + i] != v) {
+        set_dbreak(i, v, dbreakc);
+    }
+    env->sregs[DBREAKA + i] = v;
+}
+
+void HELPER(wsr_dbreakc)(uint32_t i, uint32_t v)
+{
+    if ((env->sregs[DBREAKC + i] ^ v) & (DBREAKC_SB_LB | DBREAKC_MASK)) {
+        if (v & DBREAKC_SB_LB) {
+            set_dbreak(i, env->sregs[DBREAKA + i], v);
+        } else {
+            if (env->cpu_watchpoint[i]) {
+                cpu_watchpoint_remove_by_ref(env, env->cpu_watchpoint[i]);
+                env->cpu_watchpoint[i] = NULL;
+            }
+        }
+    }
+    env->sregs[DBREAKC + i] = v;
+}
diff --git a/target-xtensa/translate.c b/target-xtensa/translate.c
index 3bf880f5cb..9e8e20a904 100644
--- a/target-xtensa/translate.c
+++ b/target-xtensa/translate.c
@@ -98,6 +98,10 @@ static const char * const sregnames[256] = {
     [IBREAKENABLE] = "IBREAKENABLE",
     [IBREAKA] = "IBREAKA0",
     [IBREAKA + 1] = "IBREAKA1",
+    [DBREAKA] = "DBREAKA0",
+    [DBREAKA + 1] = "DBREAKA1",
+    [DBREAKC] = "DBREAKC0",
+    [DBREAKC + 1] = "DBREAKC1",
     [EPC1] = "EPC1",
     [EPC1 + 1] = "EPC2",
     [EPC1 + 2] = "EPC3",
@@ -536,6 +540,28 @@ static void gen_wsr_ibreaka(DisasContext *dc, uint32_t sr, TCGv_i32 v)
     }
 }
 
+static void gen_wsr_dbreaka(DisasContext *dc, uint32_t sr, TCGv_i32 v)
+{
+    unsigned id = sr - DBREAKA;
+
+    if (id < dc->config->ndbreak) {
+        TCGv_i32 tmp = tcg_const_i32(id);
+        gen_helper_wsr_dbreaka(tmp, v);
+        tcg_temp_free(tmp);
+    }
+}
+
+static void gen_wsr_dbreakc(DisasContext *dc, uint32_t sr, TCGv_i32 v)
+{
+    unsigned id = sr - DBREAKC;
+
+    if (id < dc->config->ndbreak) {
+        TCGv_i32 tmp = tcg_const_i32(id);
+        gen_helper_wsr_dbreakc(tmp, v);
+        tcg_temp_free(tmp);
+    }
+}
+
 static void gen_wsr_intset(DisasContext *dc, uint32_t sr, TCGv_i32 v)
 {
     tcg_gen_andi_i32(cpu_SR[sr], v,
@@ -634,6 +660,10 @@ static void gen_wsr(DisasContext *dc, uint32_t sr, TCGv_i32 s)
         [IBREAKENABLE] = gen_wsr_ibreakenable,
         [IBREAKA] = gen_wsr_ibreaka,
         [IBREAKA + 1] = gen_wsr_ibreaka,
+        [DBREAKA] = gen_wsr_dbreaka,
+        [DBREAKA + 1] = gen_wsr_dbreaka,
+        [DBREAKC] = gen_wsr_dbreakc,
+        [DBREAKC + 1] = gen_wsr_dbreakc,
         [INTSET] = gen_wsr_intset,
         [INTCLEAR] = gen_wsr_intclear,
         [INTENABLE] = gen_wsr_intenable,