From 0ac9b6d4cf7eef7f24692862580bbdce9b37024f Mon Sep 17 00:00:00 2001
From: Tomas Hruby <tom@minix3.org>
Date: Wed, 15 Sep 2010 14:10:57 +0000
Subject: [PATCH] SMP - trully idle APs

- any cpu can use smp_schedule() to tell another cpu to reschedule

- if an AP is idle, it turns off timer as there is nothing to
  preempt, no need to wakeup just to go back to sleep again

- if a cpu makes a process runnable on an idle cpu, it must wake it up
  to reschedule
---
 kernel/arch/i386/apic.c            | 16 ++++++++-----
 kernel/arch/i386/apic.h            |  7 +++---
 kernel/arch/i386/apic_asm.S        |  2 +-
 kernel/arch/i386/arch_clock.c      | 25 ++++++++++++++++++++-
 kernel/arch/i386/arch_smp.c        |  5 +++++
 kernel/arch/i386/include/hw_intr.h |  3 +++
 kernel/clock.h                     |  3 +++
 kernel/cpulocals.h                 |  1 +
 kernel/interrupt.h                 |  6 +++++
 kernel/proc.c                      | 36 ++++++++++++++++++++++--------
 kernel/smp.c                       | 19 ++++++++++++++++
 kernel/smp.h                       |  7 ++++++
 12 files changed, 111 insertions(+), 19 deletions(-)
 create mode 100644 kernel/interrupt.h

diff --git a/kernel/arch/i386/apic.c b/kernel/arch/i386/apic.c
index 8da7187bd..3a3b90ae7 100644
--- a/kernel/arch/i386/apic.c
+++ b/kernel/arch/i386/apic.c
@@ -159,7 +159,10 @@ PRIVATE	u32_t lapic_tctr0, lapic_tctr1;
 PRIVATE unsigned apic_imcrp;
 PRIVATE const unsigned nlints = 0;
 
-#define apic_eoi() do { *((volatile u32_t *) lapic_eoi_addr) = 0; } while(0)
+PUBLIC void arch_eoi(void)
+{
+	apic_eoi();
+}
 
 /*
  * FIXME this should be a cpulocal variable but there are some problems with
@@ -394,10 +397,6 @@ PUBLIC void ioapic_mask_irq(unsigned irq)
 		irq_8259_mask(irq);
 }
 
-PUBLIC void apic_ipi_sched_handler(void)
-{
-}
-
 PUBLIC unsigned int apicid(void)
 {
 	return lapic_read(LAPIC_ID);
@@ -546,6 +545,13 @@ PUBLIC void lapic_stop_timer(void)
 	lapic_write(LAPIC_LVTTR, lvtt | APIC_LVTT_MASK);
 }
 
+PUBLIC void lapic_restart_timer(void)
+{
+	u32_t lvtt;
+	lvtt = lapic_read(LAPIC_LVTTR);
+	lapic_write(LAPIC_LVTTR, lvtt & ~APIC_LVTT_MASK);
+}
+
 PUBLIC void lapic_microsec_sleep(unsigned count)
 {
 	lapic_set_timer_one_shot(count);
diff --git a/kernel/arch/i386/apic.h b/kernel/arch/i386/apic.h
index 253086568..fa6d19ae9 100644
--- a/kernel/arch/i386/apic.h
+++ b/kernel/arch/i386/apic.h
@@ -152,11 +152,14 @@ _PROTOTYPE(int apic_single_cpu_init, (void));
 _PROTOTYPE(void lapic_set_timer_periodic, (const unsigned freq));
 _PROTOTYPE(void lapic_set_timer_one_shot, (const u32_t value));
 _PROTOTYPE(void lapic_stop_timer, (void));
+_PROTOTYPE(void lapic_restart_timer, (void));
 
 _PROTOTYPE(void ioapic_set_irq, (unsigned irq));
 _PROTOTYPE(void ioapic_unset_irq, (unsigned irq));
 
- /* signal the end of interrupt handler to apic */
+/* signal the end of interrupt handler to apic */
+#define apic_eoi() do { *((volatile u32_t *) lapic_eoi_addr) = 0; } while(0)
+
 _PROTOTYPE(void ioapic_eoi, (int irq));
 
 _PROTOTYPE(void dump_apic_irq_state, (void));
@@ -166,8 +169,6 @@ _PROTOTYPE(void apic_send_ipi, (unsigned vector, unsigned cpu, int type));
 _PROTOTYPE(void apic_ipi_sched_intr, (void));
 _PROTOTYPE(void apic_ipi_halt_intr, (void));
 
-_PROTOTYPE(void apic_ipi_sched_handler, (void));
-
 #define APIC_IPI_DEST			0
 #define APIC_IPI_SELF			1
 #define APIC_IPI_TO_ALL			2
diff --git a/kernel/arch/i386/apic_asm.S b/kernel/arch/i386/apic_asm.S
index 21f3817d7..0fec5e34c 100644
--- a/kernel/arch/i386/apic_asm.S
+++ b/kernel/arch/i386/apic_asm.S
@@ -70,7 +70,7 @@ ENTRY(lapic_timer_int_handler)
 #ifdef CONFIG_SMP
 
 ENTRY(apic_ipi_sched_intr)
-	lapic_intr(_C_LABEL(apic_ipi_sched_handler))
+	lapic_intr(_C_LABEL(smp_ipi_sched_handler))
 
 ENTRY(apic_ipi_halt_intr)
 	lapic_intr(_C_LABEL(smp_ipi_halt_handler))
diff --git a/kernel/arch/i386/arch_clock.c b/kernel/arch/i386/arch_clock.c
index d56fd4492..ef0389b04 100644
--- a/kernel/arch/i386/arch_clock.c
+++ b/kernel/arch/i386/arch_clock.c
@@ -8,6 +8,7 @@
 
 #include "kernel/clock.h"
 #include "kernel/proc.h"
+#include "kernel/interrupt.h"
 #include <minix/u64.h>
 #include "glo.h"
 
@@ -144,6 +145,7 @@ PUBLIC void arch_stop_local_timer(void)
 #ifdef CONFIG_APIC
 	if (lapic_addr) {
 		lapic_stop_timer();
+		apic_eoi();
 	} else
 #endif
 	{
@@ -151,6 +153,18 @@ PUBLIC void arch_stop_local_timer(void)
 	}
 }
 
+PUBLIC void arch_restart_local_timer(void)
+{
+#ifdef CONFIG_APIC
+	if (lapic_addr) {
+		lapic_restart_timer();
+	} else
+#endif
+	{
+		init_8253A_timer(system_hz);
+	}
+}
+
 PUBLIC int arch_register_local_timer_handler(const irq_handler_t handler)
 {
 #ifdef CONFIG_APIC
@@ -243,7 +257,16 @@ PUBLIC void context_stop(struct proc * p)
 
 PUBLIC void context_stop_idle(void)
 {
-	context_stop(proc_addr(IDLE));
+	int is_idle;
+	unsigned cpu = cpuid;
+	
+	is_idle = get_cpu_var(cpu, cpu_is_idle);
+	get_cpu_var(cpu, cpu_is_idle) = 0;
+
+	context_stop(get_cpulocal_var_ptr(idle_proc));
+
+	if (is_idle)
+		arch_restart_local_timer();
 }
 
 PUBLIC u64_t ms_2_cpu_time(unsigned ms)
diff --git a/kernel/arch/i386/arch_smp.c b/kernel/arch/i386/arch_smp.c
index 6a3c8fa96..abde96569 100644
--- a/kernel/arch/i386/arch_smp.c
+++ b/kernel/arch/i386/arch_smp.c
@@ -353,3 +353,8 @@ PUBLIC void arch_smp_halt_cpu(void)
 	BKL_UNLOCK();
 	for(;;);
 }
+
+PUBLIC void arch_send_smp_schedule_ipi(unsigned cpu)
+{
+	apic_send_ipi(APIC_SMP_SCHED_PROC_VECTOR, cpu, APIC_IPI_DEST);
+}
diff --git a/kernel/arch/i386/include/hw_intr.h b/kernel/arch/i386/include/hw_intr.h
index eaf2c808e..f918a3337 100644
--- a/kernel/arch/i386/include/hw_intr.h
+++ b/kernel/arch/i386/include/hw_intr.h
@@ -33,6 +33,9 @@ _PROTOTYPE(void i8259_disable,(void));
 					ioapic_reset_pic();		\
 					lapic_disable();		\
 				} while (0)
+#ifdef CONFIG_SMP
+#define ipi_ack			apic_eoi
+#endif
 
 #else
 /* legacy PIC */
diff --git a/kernel/clock.h b/kernel/clock.h
index ebd53fa92..48f9c8493 100644
--- a/kernel/clock.h
+++ b/kernel/clock.h
@@ -10,7 +10,10 @@ _PROTOTYPE(int app_cpu_init_timer, (unsigned freq));
 _PROTOTYPE(int timer_int_handler, (void));
 
 _PROTOTYPE(int arch_init_local_timer, (unsigned freq));
+/* sto p the local timer ticking */
 _PROTOTYPE(void arch_stop_local_timer, (void));
+/* let the time tick again with the original settings after it was stopped */
+_PROTOTYPE(void arch_restart_local_timer, (void));
 _PROTOTYPE(int arch_register_local_timer_handler, (irq_handler_t handler));
 
 _PROTOTYPE( u64_t ms_2_cpu_time, (unsigned ms));
diff --git a/kernel/cpulocals.h b/kernel/cpulocals.h
index fad9ede5c..3153d3675 100644
--- a/kernel/cpulocals.h
+++ b/kernel/cpulocals.h
@@ -74,6 +74,7 @@ DECLARE_CPULOCAL(struct proc *, ptproc);
 /* CPU private run queues */
 DECLARE_CPULOCAL(struct proc *, run_q_head[NR_SCHED_QUEUES]); /* ptrs to ready list headers */
 DECLARE_CPULOCAL(struct proc *, run_q_tail[NR_SCHED_QUEUES]); /* ptrs to ready list tails */
+DECLARE_CPULOCAL(int, cpu_is_idle); /* let the others know that you are idle */
 
 DECLARE_CPULOCAL(u64_t ,tsc_ctr_switch); /* when did we switched time accounting */
 
diff --git a/kernel/interrupt.h b/kernel/interrupt.h
new file mode 100644
index 000000000..a93c8f3d7
--- /dev/null
+++ b/kernel/interrupt.h
@@ -0,0 +1,6 @@
+#ifndef __INTERRUPT_H__
+#define __INTERRUPT_H__
+
+#include "hw_intr.h"
+
+#endif /* __INTERRUPT_H__ */
diff --git a/kernel/proc.c b/kernel/proc.c
index 3b934506f..92415002d 100644
--- a/kernel/proc.c
+++ b/kernel/proc.c
@@ -149,6 +149,7 @@ PUBLIC void proc_init(void)
 	/* initialize IDLE structures for every CPU */
 	for (i = 0; i < CONFIG_MAX_CPUS; i++) {
 		struct proc * ip = get_cpu_var_ptr(i, idle_proc);
+		ip->p_endpoint = IDLE;
 		ip->p_priv = &idle_priv;
 		/* must not let idle ever get scheduled */
 		ip->p_rts_flags |= RTS_PROC_STOP;
@@ -187,6 +188,11 @@ PRIVATE void idle(void)
 
 	switch_address_space_idle();
 
+	/* we don't need to keep time on APs as it is handled on the BSP */
+	if (cpuid != bsp_cpu_id)
+		arch_stop_local_timer();
+	get_cpulocal_var(cpu_is_idle) = 1;
+
 	/* start accounting for the idle time */
 	context_stop(proc_addr(KERNEL));
 	halt_cpu();
@@ -1277,7 +1283,6 @@ PUBLIC void enqueue(
  * process is assigned to.
  */
   int q = rp->p_priority;	 		/* scheduling queue to use */
-  struct proc * p;
   struct proc **rdy_head, **rdy_tail;
   
   assert(proc_is_runnable(rp));
@@ -1298,16 +1303,29 @@ PUBLIC void enqueue(
       rp->p_nextready = NULL;		/* mark new end */
   }
 
+  if (cpuid == rp->p_cpu) {
+	  /*
+	   * enqueueing a process with a higher priority than the current one,
+	   * it gets preempted. The current process must be preemptible. Testing
+	   * the priority also makes sure that a process does not preempt itself
+	   */
+	  struct proc * p;
+	  p = get_cpulocal_var(proc_ptr);
+	  assert(p);
+	  if((p->p_priority > rp->p_priority) &&
+			  (priv(p)->s_flags & PREEMPTIBLE))
+		  RTS_SET(p, RTS_PREEMPTED); /* calls dequeue() */
+  }
+#ifdef CONFIG_SMP
   /*
-   * enqueueing a process with a higher priority than the current one, it gets
-   * preempted. The current process must be preemptible. Testing the priority
-   * also makes sure that a process does not preempt itself
+   * if the process was enqueued on a different cpu and the cpu is idle, i.e.
+   * the time is off, we need to wake up that cpu and let it schedule this new
+   * process
    */
-  p = get_cpulocal_var(proc_ptr);
-  assert(p);
-  if((p->p_priority > rp->p_priority) &&
-		  (priv(p)->s_flags & PREEMPTIBLE))
-     RTS_SET(p, RTS_PREEMPTED); /* calls dequeue() */
+  else if (get_cpu_var(rp->p_cpu, cpu_is_idle)) {
+	  smp_schedule(rp->p_cpu);
+  }
+#endif
 
 #if DEBUG_SANITYCHECKS
   assert(runqueues_ok_local());
diff --git a/kernel/smp.c b/kernel/smp.c
index 2415e4add..e915c0dbb 100644
--- a/kernel/smp.c
+++ b/kernel/smp.c
@@ -1,4 +1,5 @@
 #include "smp.h"
+#include "interrupt.h"
 
 unsigned ncpus;
 unsigned ht_per_core;
@@ -28,7 +29,25 @@ PUBLIC void ap_boot_finished(unsigned cpu)
 
 PUBLIC void smp_ipi_halt_handler(void)
 {
+	ipi_ack();
 	arch_stop_local_timer();
 	arch_smp_halt_cpu();
 }
 
+PUBLIC void smp_schedule(unsigned cpu)
+{
+	arch_send_smp_schedule_ipi(cpu);
+}
+
+PUBLIC void smp_ipi_sched_handler(void)
+{
+	struct proc * p;
+	
+	ipi_ack();
+	
+	p = get_cpulocal_var(proc_ptr);
+
+	if (p->p_endpoint != IDLE)
+		RTS_SET(p, RTS_PREEMPTED); /* calls dequeue() */
+}
+
diff --git a/kernel/smp.h b/kernel/smp.h
index e9fba1f84..0cb209d2e 100644
--- a/kernel/smp.h
+++ b/kernel/smp.h
@@ -53,7 +53,14 @@ SPINLOCK_DECLARE(boot_lock)
 	
 _PROTOTYPE(void wait_for_APs_to_finish_booting, (void));
 _PROTOTYPE(void ap_boot_finished, (unsigned cpu));
+
+/* IPI handlers */
 _PROTOTYPE(void smp_ipi_halt_handler, (void));
+_PROTOTYPE(void smp_ipi_sched_handler, (void));
+
+_PROTOTYPE(void smp_schedule, (unsigned cpu));
+
+_PROTOTYPE(void arch_send_smp_schedule_ipi, (unsigned cpu));
 _PROTOTYPE(void arch_smp_halt_cpu, (void));
 
 #endif /* __ASSEMBLY__ */