diff --git a/arch/x86/kernel/hpet.c b/arch/x86/kernel/hpet.c index ad0de0c2714e..70bce5db1bb9 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); } }