m1n1.hv: Always continue on the stepped thread

Signed-off-by: Akihiko Odaki <akihiko.odaki@gmail.com>

proxyclient/m1n1/hv.py

@@ -126,7 +126,6 @@ class HV(Reloadable):
         self.vbar_el1 = None
         self.want_vbar = None
         self.vectors = [None]
-        self._stepping = False
         self._bps = [None, None, None, None, None]
         self.sym_offset = 0
         self.symbols = []
@@ -780,9 +779,7 @@ class HV(Reloadable):
                 continue
             self.u.msr(DBGBCRn_EL1(i), DBGBCR(E=1, PMC=0b11, BAS=0xf).value)
 
-        if not self._stepping:
+        return True
-            return True
-        self._stepping = False
 
     def handle_break(self, ctx):
         # disable all breakpoints so that we don't get stuck
@@ -998,8 +995,9 @@ class HV(Reloadable):
     def step(self):
         self.u.msr(MDSCR_EL1, MDSCR(SS=1, MDE=1).value)
         self.ctx.spsr.SS = 1
-        self._stepping = True
+        self.p.hv_pin_cpu(self.ctx.cpu_id)
-        raise shell.ExitConsole(EXC_RET.HANDLED)
+        self._switch_context()
+        self.p.hv_pin_cpu(0xffffffffffffffff)
 
     def _switch_context(self, exit=EXC_RET.HANDLED):
         # Flush current CPU context out to HV

proxyclient/m1n1/proxy.py

@@ -583,6 +583,7 @@ class M1N1Proxy(Reloadable):
     P_HV_START_SECONDARY = 0xc08
     P_HV_SWITCH_CPU = 0xc09
     P_HV_SET_TIME_STEALING = 0xc0a
+    P_HV_PIN_CPU = 0xc0b
 
     P_FB_INIT = 0xd00
     P_FB_SHUTDOWN = 0xd01
@@ -1007,6 +1008,8 @@ class M1N1Proxy(Reloadable):
         return self.request(self.P_HV_SWITCH_CPU, cpu)
     def hv_set_time_stealing(self, enabled):
         return self.request(self.P_HV_SET_TIME_STEALING, int(bool(enabled)))
+    def hv_pin_cpu(self, cpu):
+        return self.request(self.P_HV_PIN_CPU, cpu)
 
     def fb_init(self):
         return self.request(self.P_FB_INIT)

src/hv.c

@@ -23,6 +23,7 @@ extern char _hv_vectors_start[0];
 
 u64 hv_tick_interval;
 
+int hv_pinned_cpu;
 int hv_want_cpu;
 
 static bool hv_should_exit;
@@ -118,6 +119,7 @@ void hv_start(void *entry, u64 regs[4])
     hv_secondary_info.gxf_config = mrs(SYS_IMP_APL_GXF_CONFIG_EL1);
 
     hv_arm_tick();
+    hv_pinned_cpu = -1;
     hv_want_cpu = -1;
     hv_cpus_in_guest = 1;
 
@@ -233,6 +235,11 @@ bool hv_switch_cpu(int cpu)
     return true;
 }
 
+void hv_pin_cpu(int cpu)
+{
+    hv_pinned_cpu = cpu;
+}
+
 void hv_write_hcr(u64 val)
 {
     if (gxf_enabled() && !in_gl12())
@@ -315,7 +322,8 @@ void hv_tick(struct exc_info *ctx)
     hv_wdt_pet();
     iodev_handle_events(uartproxy_iodev);
     if (iodev_can_read(uartproxy_iodev)) {
-        hv_exc_proxy(ctx, START_HV, HV_USER_INTERRUPT, NULL);
+        if (hv_pinned_cpu == -1 || hv_pinned_cpu == smp_id())
+            hv_exc_proxy(ctx, START_HV, HV_USER_INTERRUPT, NULL);
     }
     hv_vuart_poll();
 }

src/hv.h

@@ -97,6 +97,7 @@ void hv_start(void *entry, u64 regs[4]);
 void hv_start_secondary(int cpu, void *entry, u64 regs[4]);
 void hv_rendezvous(void);
 bool hv_switch_cpu(int cpu);
+void hv_pin_cpu(int cpu);
 void hv_arm_tick(void);
 void hv_rearm(void);
 void hv_maybe_exit(void);

src/hv_exc.c

@@ -37,6 +37,7 @@ static u64 stolen_time = 0;
 static u64 exc_entry_time;
 
 extern u32 hv_cpus_in_guest;
+extern int hv_pinned_cpu;
 extern int hv_want_cpu;
 
 static bool time_stealing = true;
@@ -111,10 +112,22 @@ static void hv_maybe_switch_cpu(struct exc_info *ctx, uartproxy_boot_reason_t re
 void hv_exc_proxy(struct exc_info *ctx, uartproxy_boot_reason_t reason, u32 type, void *extra)
 {
     /*
-     * If we end up in the proxy due to an event while the host is trying to switch CPUs,
+     * Wait while another CPU is pinned or being switched to.
-     * handle it as a CPU switch first. We still tell the host the real reason code, though.
+     * If a CPU switch is requested, handle it before actually handling the
+     * exception. We still tell the host the real reason code, though.
      */
-    hv_maybe_switch_cpu(ctx, reason, type, extra);
+    while ((hv_pinned_cpu != -1 && hv_pinned_cpu != smp_id()) || hv_want_cpu != -1) {
+        if (hv_want_cpu == smp_id()) {
+            hv_want_cpu = -1;
+            _hv_exc_proxy(ctx, reason, type, extra);
+        } else {
+            // Unlock the HV so the target CPU can get into the proxy
+            spin_unlock(&bhl);
+            while ((hv_pinned_cpu != -1 && hv_pinned_cpu != smp_id()) || hv_want_cpu != -1)
+                sysop("dmb sy");
+            spin_lock(&bhl);
+        }
+    }
 
     /* Handle the actual exception */
     _hv_exc_proxy(ctx, reason, type, extra);
@@ -424,8 +437,12 @@ void hv_exc_fiq(struct exc_info *ctx)
         tick = true;
     }
 
-    if (mrs(TPIDR_EL2) != 0 && !(mrs(ISR_EL1) & 0x40) && hv_want_cpu == -1) {
+    int interruptible_cpu = hv_pinned_cpu;
-        // Secondary CPU and it was just a timer tick (or spurious), so just update FIQs
+    if (interruptible_cpu == -1)
+        interruptible_cpu = 0;
+
+    if (smp_id() != interruptible_cpu && !(mrs(ISR_EL1) & 0x40) && hv_want_cpu == -1) {
+        // Non-interruptible CPU and it was just a timer tick (or spurious), so just update FIQs
         hv_update_fiq();
         hv_arm_tick();
         return;
@@ -435,9 +452,9 @@ void hv_exc_fiq(struct exc_info *ctx)
     hv_wdt_breadcrumb('F');
     hv_exc_entry(ctx);
 
-    // Only poll for HV events in CPU 0
+    // Only poll for HV events in the interruptible CPU
     if (tick) {
-        if (mrs(TPIDR_EL2) == 0)
+        if (smp_id() == interruptible_cpu)
             hv_tick(ctx);
         hv_arm_tick();
     }

src/proxy.c

@@ -468,6 +468,9 @@ int proxy_process(ProxyRequest *request, ProxyReply *reply)
         case P_HV_SET_TIME_STEALING:
             hv_set_time_stealing(request->args[0]);
             break;
+        case P_HV_PIN_CPU:
+            hv_pin_cpu(request->args[0]);
+            break;
 
         case P_FB_INIT:
             fb_init(request->args[0]);

src/proxy.h

@@ -128,6 +128,7 @@ typedef enum {
     P_HV_START_SECONDARY,
     P_HV_SWITCH_CPU,
     P_HV_SET_TIME_STEALING,
+    P_HV_PIN_CPU,
 
     P_FB_INIT = 0xd00,
     P_FB_SHUTDOWN,