[PATCH v2 3/3] x86/hpet: Restore old configuration if Legacy Replacement mode doesn't help

[Andrew Cooper <andrew.cooper3@citrix.com>]

If Legacy Replacement mode doesn't help in check_timer(), restore the old
configuration before falling back to other workarounds.

Signed-off-by: Andrew Cooper <andrew.cooper3@citrix.com>
---
CC: Jan Beulich <JBeulich@suse.com>
CC: Roger Pau Monné <roger.pau@citrix.com>
CC: Wei Liu <wl@xen.org>
CC: Ian Jackson <iwj@xenproject.org>
CC: Marek Marczykowski-Górecki <marmarek@invisiblethingslab.com>
CC: Frédéric Pierret <frederic.pierret@qubes-os.org>

v2:
 * New.

For 4.15: Attempt to unbreak AMD Ryzen 1800X systems.

I'm tempted to fold this into the previous patch, but its presented here in
isolation for ease of review.

Tested by repositioning the timer_irq_works() calls on a system with a working
PIT.

(XEN) ENABLING IO-APIC IRQs
(XEN)  -> Using old ACK method
(XEN) ..TIMER: vector=0xF0 apic1=0 pin1=2 apic2=0 pin2=0
(XEN) ..no 8254 timer found - trying HPET Legacy Replacement Mode
(XEN) ..no HPET timer found - reverting Legacy Replacement Mode
(XEN) TSC deadline timer enabled
---
 xen/arch/x86/hpet.c        | 27 ++++++++++++++++++++++++++-
 xen/arch/x86/io_apic.c     |  4 ++++
 xen/include/asm-x86/hpet.h |  6 ++++++
 3 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/xen/arch/x86/hpet.c b/xen/arch/x86/hpet.c
index bfa75f135a..afe104dc93 100644
--- a/xen/arch/x86/hpet.c
+++ b/xen/arch/x86/hpet.c
@@ -783,6 +783,9 @@ int hpet_legacy_irq_tick(void)
 
 static u32 *hpet_boot_cfg;
 static uint64_t __initdata hpet_rate;
+static __initdata struct {
+    uint32_t cmp, cfg;
+} pre_legacy_c0;
 
 bool __init hpet_enable_legacy_replacement_mode(void)
 {
@@ -796,8 +799,11 @@ bool __init hpet_enable_legacy_replacement_mode(void)
     /* Stop the main counter. */
     hpet_write32(cfg & ~HPET_CFG_ENABLE, HPET_CFG);
 
+    /* Stash channel 0's old CFG/CMP incase we need to undo. */
+    pre_legacy_c0.cfg = c0_cfg = hpet_read32(HPET_Tn_CFG(0));
+    pre_legacy_c0.cmp = hpet_read32(HPET_Tn_CMP(0));
+
     /* Reconfigure channel 0 to be 32bit periodic. */
-    c0_cfg = hpet_read32(HPET_Tn_CFG(0));
     c0_cfg |= (HPET_TN_ENABLE | HPET_TN_PERIODIC | HPET_TN_SETVAL |
                HPET_TN_32BIT);
     hpet_write32(c0_cfg, HPET_Tn_CFG(0));
@@ -843,6 +849,25 @@ bool __init hpet_enable_legacy_replacement_mode(void)
     return true;
 }
 
+void __init hpet_disable_legacy_replacement_mode(void)
+{
+    unsigned int cfg = hpet_read32(HPET_CFG);
+
+    ASSERT(hpet_rate);
+
+    cfg &= ~(HPET_CFG_LEGACY | HPET_CFG_ENABLE);
+
+    /* Stop the main counter and disable legacy mode. */
+    hpet_write32(cfg, HPET_CFG);
+
+    /* Restore pre-Legacy Replacement Mode settings. */
+    hpet_write32(pre_legacy_c0.cfg, HPET_Tn_CFG(0));
+    hpet_write32(pre_legacy_c0.cmp, HPET_Tn_CMP(0));
+
+    /* Restart the main counter. */
+    hpet_write32(cfg | HPET_CFG_ENABLE, HPET_CFG);
+}
+
 u64 __init hpet_setup(void)
 {
     unsigned int hpet_id, hpet_period;
diff --git a/xen/arch/x86/io_apic.c b/xen/arch/x86/io_apic.c
index 3f131a81fb..58b26d962c 100644
--- a/xen/arch/x86/io_apic.c
+++ b/xen/arch/x86/io_apic.c
@@ -1955,6 +1955,10 @@ static void __init check_timer(void)
                 local_irq_restore(flags);
                 return;
             }
+
+            /* Legacy Replacement mode hasn't helped.  Undo it. */
+            printk(XENLOG_ERR "..no HPET timer found - reverting Legacy Replacement Mode\n");
+            hpet_disable_legacy_replacement_mode();
         }
 
         clear_IO_APIC_pin(apic1, pin1);
diff --git a/xen/include/asm-x86/hpet.h b/xen/include/asm-x86/hpet.h
index 07bc8d6079..8f9725a95e 100644
--- a/xen/include/asm-x86/hpet.h
+++ b/xen/include/asm-x86/hpet.h
@@ -80,6 +80,12 @@ int hpet_legacy_irq_tick(void);
 bool hpet_enable_legacy_replacement_mode(void);
 
 /*
+ * Undo the effects of hpet_disable_legacy_replacement_mode().  Must not be
+ * called unless enable() returned true.
+ */
+void hpet_disable_legacy_replacement_mode(void);
+
+/*
  * Temporarily use an HPET event counter for timer interrupt handling,
  * rather than using the LAPIC timer. Used for Cx state entry.
  */
-- 
2.11.0