From d203a070f42752acea259746ee2b3f3f3de80176 Mon Sep 17 00:00:00 2001
From: Stefan Lankes <lankes@lfbs.rwth-aachen.de>
Date: Tue, 19 Jul 2011 07:16:49 +0200
Subject: [PATCH] add IPI support to flush the TLB on the other cores

---
 arch/x86/include/asm/processor.h | 14 ++++++++
 arch/x86/kernel/apic.c           | 58 ++++++++++++++++++++++++++++++--
 2 files changed, 70 insertions(+), 2 deletions(-)

diff --git a/arch/x86/include/asm/processor.h b/arch/x86/include/asm/processor.h
index cbab6148..91f30387 100644
--- a/arch/x86/include/asm/processor.h
+++ b/arch/x86/include/asm/processor.h
@@ -227,12 +227,23 @@ static inline void write_cr4(uint32_t val) {
 	asm volatile("mov %0, %%cr4" : : "r"(val));
 }
 
+int ipi_tlb_flush(void);
+
 /** @brief Flush a specific page entry in TLB
  * @param addr The (virtual) address of the page to flush
  */
 static inline void tlb_flush_one_page(uint32_t addr)
 {
 	asm volatile("invlpg (%0)" : : "r"(addr) : "memory");
+#if MAX_CORES > 1
+	/*
+	 * Currently, we didn't support user-level threads.
+	 * => User-level applications run only on one
+	 * and we didn't flush the TLB of the other cores
+	 */
+	if (addr <= KERNEL_SPACE)
+		ipi_tlb_flush();
+#endif
 }
 
 /** @brief Invalidate the whole TLB
@@ -245,6 +256,9 @@ static inline void tlb_flush(void)
 
 	if (val)
 		write_cr3(val);
+#if MAX_CORES > 1
+	ipi_tlb_flush();
+#endif
 }
 
 /** @brief Read EFLAGS
diff --git a/arch/x86/kernel/apic.c b/arch/x86/kernel/apic.c
index 25309ee8..ba933b56 100644
--- a/arch/x86/kernel/apic.c
+++ b/arch/x86/kernel/apic.c
@@ -38,6 +38,10 @@
 #include <asm/RCCE_lib.h>
 #endif
 
+#if defined(CONFIG_ROCKCREEK) && (MAX_CORES > 1)
+#error RockCreek is not a SMP system
+#endif
+
 // IO APIC MMIO structure: write reg, then read or write data.
 typedef struct {
 	uint32_t reg;
@@ -149,6 +153,45 @@ static inline void set_ipi_dest(uint32_t cpu_id) {
 	lapic_write(APIC_ICR2, tmp);
 }
 
+int ipi_tlb_flush(void)
+{
+	uint32_t flags;
+	uint32_t i, j;
+
+	if (atomic_int32_read(&cpu_online) == 1)
+		return 0;
+
+	if (lapic_read(APIC_ICR1) & APIC_ICR_BUSY) {
+		kputs("ERROR: previous send not complete");
+		return -EIO;
+	}
+
+	flags = irq_nested_disable();
+	if (atomic_int32_read(&cpu_online) == ncores) {
+		lapic_write(APIC_ICR1, APIC_INT_ASSERT|APIC_DEST_ALLBUT|APIC_DM_FIXED|124);
+
+		j = 0;
+		while((lapic_read(APIC_ICR1) & APIC_ICR_BUSY) && (j < 1000))
+			j++; // wait for it to finish, give up eventualy tho
+	} else {
+		for(i=0; i<atomic_int32_read(&cpu_online); i++)
+		{
+			if (i == smp_id())
+				continue;
+
+			set_ipi_dest(i);
+			lapic_write(APIC_ICR1, APIC_INT_ASSERT|APIC_DM_FIXED|124);
+
+			j = 0;
+			while((lapic_read(APIC_ICR1) & APIC_ICR_BUSY) && (j < 1000))
+				j++; // wait for it to finish, give up eventualy tho
+		}
+	}
+	irq_nested_enable(flags);
+
+	return 0;
+}
+
 #if 0
 static int apic_send_ipi(uint32_t id, uint32_t mode, uint32_t vector)
 {
@@ -220,9 +263,7 @@ static int wakeup_ap(uint32_t start_eip, uint32_t id)
 
 	return ((lapic_read(APIC_ICR1) & APIC_ICR_BUSY) ? -EIO : 0); // did it fail (still delivering) or succeed ?
 }
-#endif
 
-#if MAX_CORES > 1
 /* 
  * This is defined in entry.asm. We use this to properly reload
  * the new segment registers
@@ -673,6 +714,16 @@ no_mp:
 	goto check_lapic;
 }
 
+#if MAX_CORES > 1
+static void apic_tlb_handler(struct state *s)
+{
+	uint32_t val = read_cr3();
+
+	if (val)
+		write_cr3(val);
+}
+#endif
+
 static void apic_err_handler(struct state *s)
 {
 	kprintf("Got APIC error 0x%x\n", lapic_read(APIC_ESR));
@@ -688,6 +739,9 @@ int apic_init(void)
 
 	// set APIC error handler
 	irq_install_handler(126, apic_err_handler);
+#if MAX_CORES > 1
+	irq_install_handler(124, apic_tlb_handler);
+#endif
 
 #if 0
 	// initialize local apic