Register unused HPET timers with /dev/hpet driver

Index: linux-2.6.21/arch/i386/kernel/hpet.c
===================================================================
@@ -76,44 +76,7 @@
 	return is_hpet_capable() && hpet_legacy_int_enabled;
 }
 
-/*
- * When the hpet driver (/dev/hpet) is enabled, we need to reserve
- * timer 0 and timer 1 in case of RTC emulation.
- */
-#ifdef CONFIG_HPET
-static void hpet_reserve_platform_timers(unsigned long id)
-{
-	struct hpet __iomem *hpet = hpet_virt_address;
-	struct hpet_timer __iomem *timer = &hpet->hpet_timers[2];
-	unsigned int nrtimers, i;
-	struct hpet_data hd;
-
-	nrtimers = ((id & HPET_ID_NUMBER) >> HPET_ID_NUMBER_SHIFT) + 1;
-
-	memset(&hd, 0, sizeof (hd));
-	hd.hd_phys_address = hpet_address;
-	hd.hd_address = hpet_virt_address;
-	hd.hd_nirqs = nrtimers;
-	hd.hd_flags = HPET_DATA_PLATFORM;
-	hpet_reserve_timer(&hd, 0);
-
-#ifdef CONFIG_HPET_EMULATE_RTC
-	hpet_reserve_timer(&hd, 1);
-#endif
-
-	hd.hd_irq[0] = HPET_LEGACY_8254;
-	hd.hd_irq[1] = HPET_LEGACY_RTC;
-
-	for (i = 2; i < nrtimers; timer++, i++)
-		hd.hd_irq[i] = (timer->hpet_config & Tn_INT_ROUTE_CNF_MASK) >>
-			Tn_INT_ROUTE_CNF_SHIFT;
-
-	hpet_alloc(&hd);
-
-}
-#else
-static void hpet_reserve_platform_timers(unsigned long id) { }
-#endif
+static void hpet_reserve_platform_timers(unsigned long id);
 
 /*
  * Common hpet info

@@ -730,6 +693,7 @@
 int __init hpet_enable(int early_call)
 {
 	unsigned long id;
+	int retval = 0;
 
 	if (hpet_virt_address)
 		return 0;

@@ -767,14 +731,18 @@
 	hpet_clocksource_register();
 
 	if (hpet_preferred_int_mode == HPET_INT_STD) {
-		hpet_reserve_platform_timers(id);
-		return hpet_std_clockevent_register();
+		if (hpet_std_clockevent_register() == 0)
+			retval = 1;
 	} else if (id & HPET_ID_LEGSUP) {
-		hpet_reserve_platform_timers(id);
 		hpet_legacy_clockevent_register();
-		return 1;
+		retval = 1;
 	}
-	return 0;
+
+	if (retval) {
+		hpet_reserve_platform_timers(id);
+	}
+
+	return retval;
 
 out_nohpet:
 	iounmap(hpet_virt_address);

@@ -809,6 +777,59 @@
 module_init(hpet_late_init);
 
 
+/*
+ * When the hpet driver (/dev/hpet) is enabled, we need to reserve
+ * timer 0 and timer 1 in case of RTC emulation.
+ */
+#ifdef CONFIG_HPET
+static void hpet_reserve_platform_timers(unsigned long id)
+{
+	struct hpet __iomem *hpet = hpet_virt_address;
+	struct hpet_timer __iomem *timer = &hpet->hpet_timers[2];
+	unsigned int nrtimers, i;
+	struct hpet_data hd;
+	int rem_timers = 0;
+
+	nrtimers = ((id & HPET_ID_NUMBER) >> HPET_ID_NUMBER_SHIFT) + 1;
+
+	memset(&hd, 0, sizeof (hd));
+	hd.hd_phys_address = hpet_address;
+	hd.hd_address = hpet_virt_address;
+	hd.hd_nirqs = nrtimers;
+	hd.hd_flags = HPET_DATA_PLATFORM;
+
+	if (hpet_legacy_int_enabled && (id & HPET_ID_LEGSUP)) {
+		hpet_reserve_timer(&hd, 0);
+#ifdef CONFIG_HPET_EMULATE_RTC
+		hpet_reserve_timer(&hd, 1);
+#endif
+		hd.hd_irq[0] = HPET_LEGACY_8254;
+		hd.hd_irq[1] = HPET_LEGACY_RTC;
+		rem_timers = 2;
+	} else if (cpu_hpet_dev) {
+		int i;
+		for (i = 0; i < hpet_num_timers_used; i++) {
+			struct hpet_dev *hdev;
+			hdev = per_cpu_ptr(cpu_hpet_dev, i);
+
+			hpet_reserve_timer(&hd, i);
+			hd.hd_irq[i] = hdev->irq;
+		}
+		rem_timers = i;
+	}
+
+	for (i = rem_timers; i < nrtimers; timer++, i++)
+		hd.hd_irq[i] = (timer->hpet_config & Tn_INT_ROUTE_CNF_MASK) >>
+			Tn_INT_ROUTE_CNF_SHIFT;
+
+	hpet_alloc(&hd);
+
+}
+#else
+static void hpet_reserve_platform_timers(unsigned long id) { }
+#endif
+
+
 #ifdef CONFIG_HPET_EMULATE_RTC
 
 /* HPET in LegacyReplacement Mode eats up RTC interrupt line. When, HPET

@@ -893,6 +914,9 @@
 	if (!is_hpet_enabled())
 		return 0;
 
+	if (!hpet_legacy_int_enabled)
+		return 0;
+
 	hpet_rtc_flags &= ~bit_mask;
 	return 1;
 }

@@ -904,6 +928,9 @@
 	if (!is_hpet_enabled())
 		return 0;
 
+	if (!hpet_legacy_int_enabled)
+		return 0;
+
 	hpet_rtc_flags |= bit_mask;
 
 	if (!oldbits)