[PATCH 22/39] target/hexagon: Implement setprio, resched

[Brian Cain <brian.cain@oss.qualcomm.com>]

From: Brian Cain <bcain@quicinc.com>

The hardware-assisted scheduler helps manage tasks on the run queue
and interrupt steering.

This instruction is defined in the Qualcomm Hexagon V71 Programmer's Reference
Manual -
https://docs.qualcomm.com/bundle/publicresource/80-N2040-51_REV_AB_Hexagon_V71_ProgrammerS_Reference_Manual.pdf
See §11.9.2 SYSTEM MONITOR.

Signed-off-by: Brian Cain <brian.cain@oss.qualcomm.com>
---
 target/hexagon/op_helper.c | 65 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 65 insertions(+)

diff --git a/target/hexagon/op_helper.c b/target/hexagon/op_helper.c
index 0dce133d3a..d0dc4afac7 100644
--- a/target/hexagon/op_helper.c
+++ b/target/hexagon/op_helper.c
@@ -1465,6 +1465,57 @@ void HELPER(stop)(CPUHexagonState *env)
     hexagon_stop_thread(env);
 }
 
+static inline QEMU_ALWAYS_INLINE void resched(CPUHexagonState *env)
+{
+    uint32_t schedcfg;
+    uint32_t schedcfg_en;
+    int int_number;
+    CPUState *cs;
+    uint32_t lowest_th_prio = 0; /* 0 is highest prio */
+    uint32_t bestwait_reg;
+    uint32_t best_prio;
+
+    BQL_LOCK_GUARD();
+    qemu_log_mask(CPU_LOG_INT, "%s: check resched\n", __func__);
+    schedcfg = arch_get_system_reg(env, HEX_SREG_SCHEDCFG);
+    schedcfg_en = GET_FIELD(SCHEDCFG_EN, schedcfg);
+    int_number = GET_FIELD(SCHEDCFG_INTNO, schedcfg);
+
+    if (!schedcfg_en) {
+        return;
+    }
+
+    CPU_FOREACH(cs) {
+        HexagonCPU *thread = HEXAGON_CPU(cs);
+        CPUHexagonState *thread_env = &(thread->env);
+        uint32_t th_prio = GET_FIELD(
+            STID_PRIO, arch_get_system_reg(thread_env, HEX_SREG_STID));
+        if (!hexagon_thread_is_enabled(thread_env)) {
+            continue;
+        }
+
+        lowest_th_prio = (lowest_th_prio > th_prio)
+            ? lowest_th_prio
+            : th_prio;
+    }
+
+    bestwait_reg = arch_get_system_reg(env, HEX_SREG_BESTWAIT);
+    best_prio = GET_FIELD(BESTWAIT_PRIO, bestwait_reg);
+
+    /*
+     * If the lowest priority thread is lower priority than the
+     * value in the BESTWAIT register, we must raise the reschedule
+     * interrupt on the lowest priority thread.
+     */
+    if (lowest_th_prio > best_prio) {
+        qemu_log_mask(CPU_LOG_INT,
+                "%s: raising resched int %d, cur PC 0x" TARGET_FMT_lx "\n",
+                __func__, int_number, arch_get_thread_reg(env, HEX_REG_PC));
+        SET_SYSTEM_FIELD(env, HEX_SREG_BESTWAIT, BESTWAIT_PRIO, 0x1ff);
+        hex_raise_interrupts(env, 1 << int_number, CPU_INTERRUPT_SWI);
+    }
+}
+
 void HELPER(wait)(CPUHexagonState *env, target_ulong PC)
 {
     BQL_LOCK_GUARD();
@@ -1715,6 +1766,20 @@ uint64_t HELPER(greg_read_pair)(CPUHexagonState *env, uint32_t reg)
 
 void HELPER(setprio)(CPUHexagonState *env, uint32_t thread, uint32_t prio)
 {
+    CPUState *cs;
+
+    BQL_LOCK_GUARD();
+    CPU_FOREACH(cs) {
+        HexagonCPU *found_cpu = HEXAGON_CPU(cs);
+        CPUHexagonState *found_env = &found_cpu->env;
+        if (thread == found_env->threadId) {
+            SET_SYSTEM_FIELD(found_env, HEX_SREG_STID, STID_PRIO, prio);
+            qemu_log_mask(CPU_LOG_INT, "%s: tid %d prio = 0x%x\n",
+                          __func__, found_env->threadId, prio);
+            resched(env);
+            return;
+        }
+    }
     g_assert_not_reached();
 }
 
-- 
2.34.1