[PATCH] x86_64: check_timer with io apic setup before try_apic_pin

add io apic setup before try_apic_pin

cc: Andi Kleen <ak@suse.de>
cc: Eric W. Biederman <ebiederm@xmission.com>
Signed-off-by: Yinghai Lu <yinghai.lu@amd.com>

diff --git a/arch/x86_64/kernel/io_apic.c b/arch/x86_64/kernel/io_apic.c
index 2a1dcd5..06982b4 100644
--- a/arch/x86_64/kernel/io_apic.c
+++ b/arch/x86_64/kernel/io_apic.c
@@ -273,10 +273,17 @@ static void add_pin_to_irq(unsigned int irq, int apic, int pin)
 	struct irq_pin_list *entry = irq_2_pin + irq;
 
 	BUG_ON(irq >= NR_IRQS);
-	while (entry->next)
+	while (entry->next) {
+		if (entry->apic == apic && entry->pin == pin) 
+			return;
+		if (entry->pin == -1) 
+			break;
 		entry = irq_2_pin + entry->next;
+	}
 
 	if (entry->pin != -1) {
+		if (entry->apic == apic && entry->pin == pin) 
+			return;
 		entry->next = first_free_entry;
 		entry = irq_2_pin + entry->next;
 		if (++first_free_entry >= PIN_MAP_SIZE)
@@ -286,6 +293,24 @@ static void add_pin_to_irq(unsigned int irq, int apic, int pin)
 	entry->pin = pin;
 }
 
+static void remove_pin_to_irq(unsigned int irq, int apic, int pin)
+{
+	struct irq_pin_list *entry = irq_2_pin + irq;
+
+	BUG_ON(irq >= NR_IRQS);
+
+	while (entry) {
+		if (entry->apic == apic && entry->pin == pin) {
+			entry->apic = -1;
+			entry->pin = -1;
+			break;
+		}
+		if (entry->next) 
+			entry = irq_2_pin + entry->next;
+	}
+
+}
+
 
 #define DO_ACTION(name,R,ACTION, FINAL)					\
 									\
@@ -1570,6 +1630,21 @@ static inline void unlock_ExtINT_logic(void)
  * fanatically on his truly buggy board.
  */
 
+static int set_try_apic_pin(int apic, int pin, int irq)
+{
+	int idx;
+	int ret = -1;
+	idx = find_irq_entry(apic,pin,mp_INT);
+
+	if(idx != -1) {
+		add_pin_to_irq(irq, apic, pin);
+		setup_IO_APIC_irq(apic, pin, idx, irq);
+		ret = 0;
+	}
+
+	return ret;
+}
+
 static int try_apic_pin(int apic, int pin, char *msg)
 {
 	apic_printk(APIC_VERBOSE, KERN_INFO
@@ -1588,7 +1663,7 @@ static int try_apic_pin(int apic, int pin, char *msg)
 		}
 		return 1;
 	}
-	clear_IO_APIC_pin(apic, pin);
+
 	apic_printk(APIC_QUIET, KERN_ERR " .. failed\n");
 	return 0;
 }
@@ -1599,12 +1674,12 @@ static void check_timer(void)
 	int apic1, pin1, apic2, pin2;
 	int vector;
 	cpumask_t mask;
+	int i;
 
 	/*
 	 * get/set the timer IRQ vector:
 	 */
 	disable_8259A_irq(0);
-	vector = assign_irq_vector(0, TARGET_CPUS, &mask);
 
 	/*
 	 * Subtle, code in do_timer_interrupt() expects an AEOI
@@ -1622,32 +1697,49 @@ static void check_timer(void)
 	apic2 = ioapic_i8259.apic;
 
 	/* Do this first, otherwise we get double interrupts on ATI boards */
-	if ((pin1 != -1) && try_apic_pin(apic1, pin1,"with 8259 IRQ0 disabled"))
-		return;
+	if (pin1 != -1) {
+		/* set_try_apic_pin will call disable_8259A_irq */
+		set_try_apic_pin(apic1, pin1, 0);
+		unmask_IO_APIC_irq(0);
+		if (try_apic_pin(apic1, pin1,"with 8259 IRQ0 disabled"))
+			return;
 
-	/* Now try again with IRQ0 8259A enabled.
-	   Assumes timer is on IO-APIC 0 ?!? */
-	enable_8259A_irq(0);
-	unmask_IO_APIC_irq(0);
-	if (try_apic_pin(apic1, pin1, "with 8259 IRQ0 enabled"))
-		return;
-	disable_8259A_irq(0);
+		/* Now try again with IRQ0 8259A enabled.
+		   Assumes timer is on IO-APIC 0 ?!? */
+		enable_8259A_irq(0);
+		if (try_apic_pin(apic1, pin1, "with 8259 IRQ0 enabled"))
+			return;
+		disable_8259A_irq(0);
+
+    	        clear_IO_APIC_pin(apic1, pin1);
+		remove_pin_to_irq(0, apic1, pin1);
+	}
 
 	/* Always try pin0 and pin2 on APIC 0 to handle buggy timer overrides
 	   on Nvidia boards */
-	if (!(apic1 == 0 && pin1 == 0) &&
-	    try_apic_pin(0, 0, "fallback with 8259 IRQ0 disabled"))
-		return;
-	if (!(apic1 == 0 && pin1 == 2) &&
-	    try_apic_pin(0, 2, "fallback with 8259 IRQ0 disabled"))
-		return;
+	for (i = 0; i <= 2; i += 2) 
+	if (!(apic1 == 0 && pin1 == i)) {
+		/* set_try_apic_pin will call disable_8259A_irq */
+		if (!set_try_apic_pin(0, i, 0) ) {
+			unmask_IO_APIC_irq(0);
+			if (try_apic_pin(0, i, "fallback with 8259 IRQ0 disabled"))
+				return;
+			clear_IO_APIC_pin(0, i);
+			remove_pin_to_irq(0, 0, i);
+	    	}
+	}
+
+	vector = assign_irq_vector(0, TARGET_CPUS, &mask);
 
 	/* Then try pure 8259A routing on the 8259 as reported by BIOS*/
-	enable_8259A_irq(0);
 	if (pin2 != -1) {
 		setup_ExtINT_IRQ0_pin(apic2, pin2, vector);
+		add_pin_to_irq(0, apic2, pin2);
+		enable_8259A_irq(0);
 		if (try_apic_pin(apic2,pin2,"8259A broadcast ExtINT from BIOS"))
 			return;
+		clear_IO_APIC_pin(apic2, pin2);
+		remove_pin_to_irq(0, apic2, pin2);
 	}
 
 	/* Tried all possibilities to go through the IO-APIC. Now come the