1 files changed, 51 insertions, 8 deletions

diff --git a/arch/x86/kernel/hpet.c b/arch/x86/kernel/hpet.c
index ad0de0c2714..70bce5db1bb 100644
--- a/arch/x86/kernel/hpet.c
+++ b/arch/x86/kernel/hpet.c
@@ -319,8 +319,6 @@ static void hpet_set_mode(enum clock_event_mode mode,
                 now = hpet_readl(HPET_COUNTER);
                 cmp = now + (unsigned int) delta;
                 cfg = hpet_readl(HPET_Tn_CFG(timer));
-                /* Make sure we use edge triggered interrupts */
-                cfg &= ~HPET_TN_LEVEL;
                 cfg |= HPET_TN_ENABLE | HPET_TN_PERIODIC |
                        HPET_TN_SETVAL | HPET_TN_32BIT;
                 hpet_writel(cfg, HPET_Tn_CFG(timer));
@@ -787,15 +785,16 @@ static int hpet_clocksource_register(void)
         return 0;
 }
 
+static u32 *hpet_boot_cfg;
+
 /**
  * hpet_enable - Try to setup the HPET timer. Returns 1 on success.
  */
 int __init hpet_enable(void)
 {
-        unsigned long hpet_period;
-        unsigned int id;
+        u32 hpet_period, cfg, id;
         u64 freq;
-        int i;
+        unsigned int i, last;
 
         if (!is_hpet_capable())
                 return 0;
@@ -847,15 +846,45 @@ int __init hpet_enable(void)
         id = hpet_readl(HPET_ID);
         hpet_print_config();
 
+        last = (id & HPET_ID_NUMBER) >> HPET_ID_NUMBER_SHIFT;
+
 #ifdef CONFIG_HPET_EMULATE_RTC
         /*
          * The legacy routing mode needs at least two channels, tick timer
          * and the rtc emulation channel.
          */
-        if (!(id & HPET_ID_NUMBER))
+        if (!last)
                 goto out_nohpet;
 #endif
 
+        cfg = hpet_readl(HPET_CFG);
+        hpet_boot_cfg = kmalloc((last + 2) * sizeof(*hpet_boot_cfg),
+                                GFP_KERNEL);
+        if (hpet_boot_cfg)
+                *hpet_boot_cfg = cfg;
+        else
+                pr_warn("HPET initial state will not be saved\n");
+        cfg &= ~(HPET_CFG_ENABLE | HPET_CFG_LEGACY);
+        hpet_writel(cfg, HPET_Tn_CFG(i));
+        if (cfg)
+                pr_warn("HPET: Unrecognized bits %#x set in global cfg\n",
+                        cfg);
+
+        for (i = 0; i <= last; ++i) {
+                cfg = hpet_readl(HPET_Tn_CFG(i));
+                if (hpet_boot_cfg)
+                        hpet_boot_cfg[i + 1] = cfg;
+                cfg &= ~(HPET_TN_ENABLE | HPET_TN_LEVEL | HPET_TN_FSB);
+                hpet_writel(cfg, HPET_Tn_CFG(i));
+                cfg &= ~(HPET_TN_PERIODIC | HPET_TN_PERIODIC_CAP
+                         | HPET_TN_64BIT_CAP | HPET_TN_32BIT | HPET_TN_ROUTE
+                         | HPET_TN_FSB | HPET_TN_FSB_CAP);
+                if (cfg)
+                        pr_warn("HPET: Unrecognized bits %#x set in cfg#%u\n",
+                                cfg, i);
+        }
+        hpet_print_config();
+
         if (hpet_clocksource_register())
                 goto out_nohpet;
 
@@ -923,14 +952,28 @@ fs_initcall(hpet_late_init);
 void hpet_disable(void)
 {
         if (is_hpet_capable() && hpet_virt_address) {
-                unsigned int cfg = hpet_readl(HPET_CFG);
+                unsigned int cfg = hpet_readl(HPET_CFG), id, last;
 
-                if (hpet_legacy_int_enabled) {
+                if (hpet_boot_cfg)
+                        cfg = *hpet_boot_cfg;
+                else if (hpet_legacy_int_enabled) {
                         cfg &= ~HPET_CFG_LEGACY;
                         hpet_legacy_int_enabled = 0;
                 }
                 cfg &= ~HPET_CFG_ENABLE;
                 hpet_writel(cfg, HPET_CFG);
+
+                if (!hpet_boot_cfg)
+                        return;
+
+                id = hpet_readl(HPET_ID);
+                last = ((id & HPET_ID_NUMBER) >> HPET_ID_NUMBER_SHIFT);
+
+                for (id = 0; id <= last; ++id)
+                        hpet_writel(hpet_boot_cfg[id + 1], HPET_Tn_CFG(id));
+
+                if (*hpet_boot_cfg & HPET_CFG_ENABLE)
+                        hpet_writel(*hpet_boot_cfg, HPET_CFG);
         }
 }