2 files changed, 69 insertions, 37 deletions

diff --git a/arch/x86_64/kernel/io_apic.c b/arch/x86_64/kernel/io_apic.c
index fe429e5d6b29..3b8f9c68ad3c 100644
--- a/arch/x86_64/kernel/io_apic.c
+++ b/arch/x86_64/kernel/io_apic.c
@@ -88,6 +88,52 @@ static struct irq_pin_list {
         short apic, pin, next;
 } irq_2_pin[PIN_MAP_SIZE];
 
+struct io_apic {
+        unsigned int index;
+        unsigned int unused[3];
+        unsigned int data;
+};
+
+static __attribute_const__ struct io_apic __iomem *io_apic_base(int idx)
+{
+        return (void __iomem *) __fix_to_virt(FIX_IO_APIC_BASE_0 + idx)
+                + (mp_ioapics[idx].mpc_apicaddr & ~PAGE_MASK);
+}
+
+static inline unsigned int io_apic_read(unsigned int apic, unsigned int reg)
+{
+        struct io_apic __iomem *io_apic = io_apic_base(apic);
+        writel(reg, &io_apic->index);
+        return readl(&io_apic->data);
+}
+
+static inline void io_apic_write(unsigned int apic, unsigned int reg, unsigned int value)
+{
+        struct io_apic __iomem *io_apic = io_apic_base(apic);
+        writel(reg, &io_apic->index);
+        writel(value, &io_apic->data);
+}
+
+/*
+ * Re-write a value: to be used for read-modify-write
+ * cycles where the read already set up the index register.
+ */
+static inline void io_apic_modify(unsigned int apic, unsigned int value)
+{
+        struct io_apic __iomem *io_apic = io_apic_base(apic);
+        writel(value, &io_apic->data);
+}
+
+/*
+ * Synchronize the IO-APIC and the CPU by doing
+ * a dummy read from the IO-APIC
+ */
+static inline void io_apic_sync(unsigned int apic)
+{
+        struct io_apic __iomem *io_apic = io_apic_base(apic);
+        readl(&io_apic->data);
+}
+
 #define __DO_ACTION(R, ACTION, FINAL)                                   \
                                                                         \
 {                                                                       \
@@ -126,12 +172,34 @@ static struct IO_APIC_route_entry ioapic_read_entry(int apic, int pin)
         return eu.entry;
 }
 
+/*
+ * When we write a new IO APIC routing entry, we need to write the high
+ * word first! If the mask bit in the low word is clear, we will enable
+ * the interrupt, and we need to make sure the entry is fully populated
+ * before that happens.
+ */
 static void ioapic_write_entry(int apic, int pin, struct IO_APIC_route_entry e)
 {
         unsigned long flags;
         union entry_union eu;
         eu.entry = e;
         spin_lock_irqsave(&ioapic_lock, flags);
+        io_apic_write(apic, 0x11 + 2*pin, eu.w2);
+        io_apic_write(apic, 0x10 + 2*pin, eu.w1);
+        spin_unlock_irqrestore(&ioapic_lock, flags);
+}
+
+/*
+ * When we mask an IO APIC routing entry, we need to write the low
+ * word first, in order to set the mask bit before we change the
+ * high bits!
+ */
+static void ioapic_mask_entry(int apic, int pin)
+{
+        unsigned long flags;
+        union entry_union eu = { .entry.mask = 1 };
+
+        spin_lock_irqsave(&ioapic_lock, flags);
         io_apic_write(apic, 0x10 + 2*pin, eu.w1);
         io_apic_write(apic, 0x11 + 2*pin, eu.w2);
         spin_unlock_irqrestore(&ioapic_lock, flags);
@@ -256,9 +324,7 @@ static void clear_IO_APIC_pin(unsigned int apic, unsigned int pin)
         /*
          * Disable it in the IO-APIC irq-routing table:
          */
-        memset(&entry, 0, sizeof(entry));
-        entry.mask = 1;
-        ioapic_write_entry(apic, pin, entry);
+        ioapic_mask_entry(apic, pin);
 }
 
 static void clear_IO_APIC (void)
diff --git a/include/asm-x86_64/io_apic.h b/include/asm-x86_64/io_apic.h
index 171ec2dc8c04..561ecbfd4cb5 100644
--- a/include/asm-x86_64/io_apic.h
+++ b/include/asm-x86_64/io_apic.h
@@ -12,10 +12,6 @@
 
 #define APIC_MISMATCH_DEBUG
 
-#define IO_APIC_BASE(idx) \
-                ((volatile int *)(__fix_to_virt(FIX_IO_APIC_BASE_0 + idx) \
-                + (mp_ioapics[idx].mpc_apicaddr & ~PAGE_MASK)))
-
 /*
  * The structure of the IO-APIC:
  */
@@ -119,36 +115,6 @@ extern struct mpc_config_intsrc mp_irqs[MAX_IRQ_SOURCES];
 /* non-0 if default (table-less) MP configuration */
 extern int mpc_default_type;
 
-static inline unsigned int io_apic_read(unsigned int apic, unsigned int reg)
-{
-        *IO_APIC_BASE(apic) = reg;
-        return *(IO_APIC_BASE(apic)+4);
-}
-
-static inline void io_apic_write(unsigned int apic, unsigned int reg, unsigned int value)
-{
-        *IO_APIC_BASE(apic) = reg;
-        *(IO_APIC_BASE(apic)+4) = value;
-}
-
-/*
- * Re-write a value: to be used for read-modify-write
- * cycles where the read already set up the index register.
- */
-static inline void io_apic_modify(unsigned int apic, unsigned int value)
-{
-        *(IO_APIC_BASE(apic)+4) = value;
-}
-
-/*
- * Synchronize the IO-APIC and the CPU by doing
- * a dummy read from the IO-APIC
- */
-static inline void io_apic_sync(unsigned int apic)
-{
-        (void) *(IO_APIC_BASE(apic)+4);
-}
-
 /* 1 if "noapic" boot option passed */
 extern int skip_ioapic_setup;