From: Brian Cain <bcain@quicinc.com>

Reviewed-by: Taylor Simpson <ltaylorsimpson@gmail.com>
Signed-off-by: Brian Cain <brian.cain@oss.qualcomm.com>
---
 target/hexagon/cpu_helper.h |  1 +
 target/hexagon/cpu.c        | 26 ++++++++++++++++++-----
 target/hexagon/cpu_helper.c | 41 +++++++++++++++++++++++++++++++++++++
 3 files changed, 63 insertions(+), 5 deletions(-)

diff --git a/target/hexagon/cpu_helper.h b/target/hexagon/cpu_helper.h
index c8b0a37b7f..9f2eb27e63 100644
--- a/target/hexagon/cpu_helper.h
+++ b/target/hexagon/cpu_helper.h
@@ -15,6 +15,7 @@ void hexagon_set_sys_pcycle_count(CPUHexagonState *env, uint64_t);
 void hexagon_set_sys_pcycle_count_low(CPUHexagonState *env, uint32_t);
 void hexagon_set_sys_pcycle_count_high(CPUHexagonState *env, uint32_t);
 void hexagon_modify_ssr(CPUHexagonState *env, uint32_t new, uint32_t old);
+int get_cpu_mode(CPUHexagonState *env);
 int get_exe_mode(CPUHexagonState *env);
 void clear_wait_mode(CPUHexagonState *env);
 void hexagon_ssr_set_cause(CPUHexagonState *env, uint32_t cause);
diff --git a/target/hexagon/cpu.c b/target/hexagon/cpu.c
index 8afa7e55e6..6d2013ce89 100644
--- a/target/hexagon/cpu.c
+++ b/target/hexagon/cpu.c
@@ -329,20 +329,16 @@ bool hexagon_thread_is_enabled(CPUHexagonState *env)
 
     return E_bit;
 }
-#endif
 
 static bool hexagon_cpu_has_work(CPUState *cs)
 {
-#ifndef CONFIG_USER_ONLY
     CPUHexagonState *env = cpu_env(cs);
 
     return hexagon_thread_is_enabled(env) &&
         (cs->interrupt_request & (CPU_INTERRUPT_HARD | CPU_INTERRUPT_SWI
             | CPU_INTERRUPT_K0_UNLOCK | CPU_INTERRUPT_TLB_UNLOCK));
-#else
-    return true;
-#endif
 }
+#endif
 
 static void hexagon_restore_state_to_opc(CPUState *cs,
                                          const TranslationBlock *tb,
@@ -463,6 +459,26 @@ static void hexagon_cpu_realize(DeviceState *dev, Error **errp)
 
 static int hexagon_cpu_mmu_index(CPUState *cs, bool ifetch)
 {
+#ifndef CONFIG_USER_ONLY
+    BQL_LOCK_GUARD();
+    CPUHexagonState *env = cpu_env(cs);
+    HexagonCPU *cpu = HEXAGON_CPU(cs);
+    if (cpu->globalregs) {
+        uint32_t syscfg = arch_get_system_reg(env, HEX_SREG_SYSCFG);
+        uint8_t mmuen = GET_SYSCFG_FIELD(SYSCFG_MMUEN, syscfg);
+        if (!mmuen) {
+            return MMU_KERNEL_IDX;
+        }
+    }
+
+    int cpu_mode = get_cpu_mode(env);
+    if (cpu_mode == HEX_CPU_MODE_MONITOR) {
+        return MMU_KERNEL_IDX;
+    } else if (cpu_mode == HEX_CPU_MODE_GUEST) {
+        return MMU_GUEST_IDX;
+    }
+#endif
+
     return MMU_USER_IDX;
 }
 
diff --git a/target/hexagon/cpu_helper.c b/target/hexagon/cpu_helper.c
index 18c929bf5f..5d0ba23d02 100644
--- a/target/hexagon/cpu_helper.c
+++ b/target/hexagon/cpu_helper.c
@@ -389,4 +389,45 @@ void hexagon_stop_thread(CPUHexagonState *env)
     }
 }
 
+static int sys_in_monitor_mode_ssr(uint32_t ssr)
+{
+    if ((GET_SSR_FIELD(SSR_EX, ssr) != 0) ||
+       ((GET_SSR_FIELD(SSR_EX, ssr) == 0) && (GET_SSR_FIELD(SSR_UM, ssr) == 0)))
+        return 1;
+    return 0;
+}
+
+static int sys_in_guest_mode_ssr(uint32_t ssr)
+{
+    if ((GET_SSR_FIELD(SSR_EX, ssr) == 0) &&
+        (GET_SSR_FIELD(SSR_UM, ssr) != 0) &&
+        (GET_SSR_FIELD(SSR_GM, ssr) != 0))
+        return 1;
+    return 0;
+}
+
+static int sys_in_user_mode_ssr(uint32_t ssr)
+{
+    if ((GET_SSR_FIELD(SSR_EX, ssr) == 0) &&
+        (GET_SSR_FIELD(SSR_UM, ssr) != 0) &&
+        (GET_SSR_FIELD(SSR_GM, ssr) == 0))
+        return 1;
+   return 0;
+}
+
+int get_cpu_mode(CPUHexagonState *env)
+
+{
+    uint32_t ssr = arch_get_system_reg(env, HEX_SREG_SSR);
+
+    if (sys_in_monitor_mode_ssr(ssr)) {
+        return HEX_CPU_MODE_MONITOR;
+    } else if (sys_in_guest_mode_ssr(ssr)) {
+        return HEX_CPU_MODE_GUEST;
+    } else if (sys_in_user_mode_ssr(ssr)) {
+        return HEX_CPU_MODE_USER;
+    }
+    return HEX_CPU_MODE_MONITOR;
+}
+
 #endif
-- 
2.34.1