1 files changed, 40 insertions, 37 deletions

diff --git a/src/arch/x86/ioapic.c b/src/arch/x86/ioapic.c
index 5e3e2afb11..1a8b489ed3 100644
--- a/src/arch/x86/ioapic.c
+++ b/src/arch/x86/ioapic.c
@@ -1,5 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 
+#include <assert.h>
 #include <device/mmio.h>
 #include <arch/ioapic.h>
 #include <console/console.h>
@@ -21,7 +22,7 @@ static int ioapic_interrupt_count(void *ioapic_base)
 {
 	/* Read the available number of interrupts. */
 	int ioapic_interrupts = (io_apic_read(ioapic_base, 0x01) >> 16) & 0xff;
-	if (ioapic_interrupts == 0xff)
+	if (!ioapic_interrupts || ioapic_interrupts == 0xff)
 		ioapic_interrupts = 23;
 	ioapic_interrupts += 1; /* Bits 23-16 specify the maximum redirection
 				   entry, which is the number of interrupts
@@ -31,23 +32,21 @@ static int ioapic_interrupt_count(void *ioapic_base)
 	return ioapic_interrupts;
 }
 
-void clear_ioapic(void *ioapic_base)
+static void clear_vectors(void *ioapic_base, u8 first, u8 last)
 {
 	u32 low, high;
-	u32 i, ioapic_interrupts;
+	u8 i;
 
 	printk(BIOS_DEBUG, "IOAPIC: Clearing IOAPIC at %p\n", ioapic_base);
 
-	ioapic_interrupts = ioapic_interrupt_count(ioapic_base);
-
 	low = INT_DISABLED;
 	high = NONE;
 
-	for (i = 0; i < ioapic_interrupts; i++) {
+	for (i = first; i <= last; i++) {
 		io_apic_write(ioapic_base, i * 2 + 0x10, low);
 		io_apic_write(ioapic_base, i * 2 + 0x11, high);
 
-		printk(BIOS_SPEW, "IOAPIC: reg 0x%08x value 0x%08x 0x%08x\n",
+		printk(BIOS_SPEW, "IOAPIC: vector 0x%02x value 0x%08x 0x%08x\n",
 		       i, high, low);
 	}
 
@@ -57,15 +56,42 @@ void clear_ioapic(void *ioapic_base)
 	}
 }
 
-void set_ioapic_id(void *ioapic_base, u8 ioapic_id)
+void clear_ioapic(void *ioapic_base)
+{
+	clear_vectors(ioapic_base, 0, ioapic_interrupt_count(ioapic_base) - 1);
+}
+
+static void route_i8259_irq0(void *ioapic_base)
 {
 	u32 bsp_lapicid = lapicid();
+	u32 low, high;
+
+	ASSERT(bsp_lapicid < 255);
+
+	printk(BIOS_DEBUG, "IOAPIC: Bootstrap Processor Local APIC = 0x%02x\n",
+	       bsp_lapicid);
+
+	/* Enable Virtual Wire Mode. Should this be LOGICAL_DEST instead? */
+	low = INT_ENABLED | TRIGGER_EDGE | POLARITY_HIGH | PHYSICAL_DEST | ExtINT;
+	high = bsp_lapicid << (56 - 32);
+
+	io_apic_write(ioapic_base, 0x10, low);
+	io_apic_write(ioapic_base, 0x11, high);
+
+	if (io_apic_read(ioapic_base, 0x10) == 0xffffffff) {
+		printk(BIOS_WARNING, "IOAPIC not responding.\n");
+		return;
+	}
+
+	printk(BIOS_SPEW, "IOAPIC: reg 0x%08x value 0x%08x 0x%08x\n", 0, high, low);
+}
+
+void set_ioapic_id(void *ioapic_base, u8 ioapic_id)
+{
 	int i;
 
 	printk(BIOS_DEBUG, "IOAPIC: Initializing IOAPIC at %p\n",
 	       ioapic_base);
-	printk(BIOS_DEBUG, "IOAPIC: Bootstrap Processor Local APIC = 0x%02x\n",
-	       bsp_lapicid);
 
 	if (ioapic_id) {
 		printk(BIOS_DEBUG, "IOAPIC: ID = 0x%02x\n", ioapic_id);
@@ -84,11 +110,7 @@ void set_ioapic_id(void *ioapic_base, u8 ioapic_id)
 
 static void load_vectors(void *ioapic_base)
 {
-	u32 bsp_lapicid = lapicid();
-	u32 low, high;
-	u32 i, ioapic_interrupts;
-
-	ioapic_interrupts = ioapic_interrupt_count(ioapic_base);
+	int first = 1, last;
 
 	if (CONFIG(IOAPIC_INTERRUPTS_ON_FSB)) {
 		/*
@@ -104,29 +126,10 @@ static void load_vectors(void *ioapic_base)
 		io_apic_write(ioapic_base, 0x03, 0);
 	}
 
-	/* Enable Virtual Wire Mode. */
-	low = INT_ENABLED | TRIGGER_EDGE | POLARITY_HIGH | PHYSICAL_DEST | ExtINT;
-	high = bsp_lapicid << (56 - 32);
-
-	io_apic_write(ioapic_base, 0x10, low);
-	io_apic_write(ioapic_base, 0x11, high);
+	route_i8259_irq0(ioapic_base);
 
-	if (io_apic_read(ioapic_base, 0x10) == 0xffffffff) {
-		printk(BIOS_WARNING, "IOAPIC not responding.\n");
-		return;
-	}
-
-	printk(BIOS_SPEW, "IOAPIC: reg 0x%08x value 0x%08x 0x%08x\n",
-	       0, high, low);
-	low = INT_DISABLED;
-	high = NONE;
-	for (i = 1; i < ioapic_interrupts; i++) {
-		io_apic_write(ioapic_base, i * 2 + 0x10, low);
-		io_apic_write(ioapic_base, i * 2 + 0x11, high);
-
-		printk(BIOS_SPEW, "IOAPIC: reg 0x%08x value 0x%08x 0x%08x\n",
-		       i, high, low);
-	}
+	last = ioapic_interrupt_count(ioapic_base) - 1;
+	clear_vectors(ioapic_base, first, last);
 }
 
 void setup_ioapic(void *ioapic_base, u8 ioapic_id)