From 1cb1611fc6e6223b5281e5e0187855eb0b9f719d Mon Sep 17 00:00:00 2001
From: Simon Glass <sjg@chromium.org>
Date: Fri, 11 Mar 2016 22:07:34 -0700
Subject: [PATCH] x86: Add a function to set the IOAPIC ID

Add a function to set the ID in the IOAPIC.

Signed-off-by: Simon Glass <sjg@chromium.org>
Reviewed-by: Bin Meng <bmeng.cn@gmail.com>
---
 arch/x86/cpu/ioapic.c         | 16 ++++++++++++++++
 arch/x86/include/asm/ioapic.h |  2 ++
 2 files changed, 18 insertions(+)

diff --git a/arch/x86/cpu/ioapic.c b/arch/x86/cpu/ioapic.c
index 112a9c6..d15e86c 100644
--- a/arch/x86/cpu/ioapic.c
+++ b/arch/x86/cpu/ioapic.c
@@ -7,6 +7,7 @@
 #include <common.h>
 #include <asm/io.h>
 #include <asm/ioapic.h>
+#include <asm/lapic.h>
 
 u32 io_apic_read(u32 reg)
 {
@@ -19,3 +20,18 @@ void io_apic_write(u32 reg, u32 val)
 	writel(reg, IO_APIC_INDEX);
 	writel(val, IO_APIC_DATA);
 }
+
+void io_apic_set_id(int ioapic_id)
+{
+	int bsp_lapicid = lapicid();
+
+	debug("IOAPIC: Initialising IOAPIC at %08x\n", IO_APIC_ADDR);
+	debug("IOAPIC: Bootstrap Processor Local APIC = %#02x\n", bsp_lapicid);
+
+	if (ioapic_id) {
+		debug("IOAPIC: ID = 0x%02x\n", ioapic_id);
+		/* Set IOAPIC ID if it has been specified */
+		io_apic_write(0x00, (io_apic_read(0x00) & 0xf0ffffff) |
+			      (ioapic_id << 24));
+	}
+}
diff --git a/arch/x86/include/asm/ioapic.h b/arch/x86/include/asm/ioapic.h
index 77c443e..2feed86 100644
--- a/arch/x86/include/asm/ioapic.h
+++ b/arch/x86/include/asm/ioapic.h
@@ -39,4 +39,6 @@ u32 io_apic_read(u32 reg);
  */
 void io_apic_write(u32 reg, u32 val);
 
+void io_apic_set_id(int ioapic_id);
+
 #endif