SMT support.

15 files changed, 149 insertions, 47 deletions

diff --git a/src/include/kernel/cpu.H b/src/include/kernel/cpu.H
index 2e5a489d9..81b43a57c 100644
--- a/src/include/kernel/cpu.H
+++ b/src/include/kernel/cpu.H
@@ -2,6 +2,10 @@
 #define __KERNEL_CPU_H
 
 #include <kernel/types.h>
+#include <kernel/ppcarch.H>
+
+// Thread ID support only, Power7 (4 threads).
+#define KERNEL_MAX_SUPPORTED_CPUS 4
 
 class Scheduler;
 
@@ -11,6 +15,13 @@ struct cpu_t
     cpuid_t cpu;
 
     Scheduler* scheduler;
+    task_t* idle_task;
 };
 
+__attribute__((always_inline))
+inline uint64_t getCpuId()
+{
+    return ppc_getPIR() & (KERNEL_MAX_SUPPORTED_CPUS - 1);
+}
+
 #endif
diff --git a/src/include/kernel/cpumgr.H b/src/include/kernel/cpumgr.H
index 325f09d32..47ec78c4d 100644
--- a/src/include/kernel/cpumgr.H
+++ b/src/include/kernel/cpumgr.H
@@ -13,8 +13,10 @@ class CpuManager
 	 *  task structure in SPRG3.
 	 */
 	static cpu_t* getCurrentCPU();
+	static cpu_t* getCpu(size_t i) { return cv_cpus[i]; };
 
 	static void init();
+	static void init_slave_smp(cpu_t*);
 
     protected:
 	CpuManager();
@@ -24,9 +26,12 @@ class CpuManager
 	 *  Starts the requested CPU. Default of -1 implies current CPU.
 	 */
 	void startCPU(ssize_t i = -1);
+	void startSlaveCPU(cpu_t*);
 
     private:
-	cpu_t* iv_cpus[MAXCPUS];
+	static cpu_t* cv_cpus[MAXCPUS]; // Need to be able to access this
+					// from start.S to get initial stacks
+					// of secondary cpus / threads.
 };
 
 #endif
diff --git a/src/include/kernel/ppcarch.H b/src/include/kernel/ppcarch.H
index 13ebfe6e2..71d4516d4 100644
--- a/src/include/kernel/ppcarch.H
+++ b/src/include/kernel/ppcarch.H
@@ -55,8 +55,7 @@ __attribute__((always_inline))
 inline void ppc_setMSR(uint64_t _msr)
 {
     register uint64_t msr = _msr;
-    asm volatile("mtmsr %0; isync" : "=r" (msr));
+    asm volatile("mtmsr %0; isync" :: "r" (msr));
 }
 
-
 #endif
diff --git a/src/include/kernel/ppcconsts.S b/src/include/kernel/ppcconsts.S
index 450972026..0c5e1dc3d 100644
--- a/src/include/kernel/ppcconsts.S
+++ b/src/include/kernel/ppcconsts.S
@@ -122,6 +122,8 @@
                 .set HID0,1008
                 .set IABR,1010
 
+		.set PIR, 1023
+
 #*--------------------------------------------------------------------*#
 #*  Task offset Constants                                             *#
 #*--------------------------------------------------------------------*#
diff --git a/src/include/kernel/scheduler.H b/src/include/kernel/scheduler.H
index 76034af48..71b21559a 100644
--- a/src/include/kernel/scheduler.H
+++ b/src/include/kernel/scheduler.H
@@ -18,18 +18,12 @@ class Scheduler
 	void setNextRunnable();
 
     protected:
-	Scheduler(cpu_t* cpu) : 
-		iv_taskList(), iv_idleTask(NULL), iv_cpu(cpu) {};
+	Scheduler() : 
+		iv_taskList() {};
 	~Scheduler() {};
 
-	void setIdleTask(task_t* t) { iv_idleTask = t; };
-	task_t* getIdleTask() { return iv_idleTask; };
-
     private:
 	Util::Locked::Queue<task_t, true, Spinlock> iv_taskList;
-
-	task_t* iv_idleTask;
-	cpu_t* iv_cpu;
 };
 
 #endif
diff --git a/src/include/kernel/vmmmgr.H b/src/include/kernel/vmmmgr.H
index 7ea2350bb..e55595ed5 100644
--- a/src/include/kernel/vmmmgr.H
+++ b/src/include/kernel/vmmmgr.H
@@ -38,6 +38,7 @@ class VmmManager
 	};
 
 	static void init();
+	static void init_slb();
 
     protected:
 	VmmManager();
diff --git a/src/include/sys/task.h b/src/include/sys/task.h
index e50c6daee..520008e6d 100644
--- a/src/include/sys/task.h
+++ b/src/include/sys/task.h
@@ -14,6 +14,7 @@ tid_t task_create(void(*)(void*), void*);
 void task_end();
 
 tid_t task_gettid();
+cpuid_t task_getcpuid();
 
 #ifdef __cplusplus
 }
diff --git a/src/kernel/cpumgr.C b/src/kernel/cpumgr.C
index a04d0e1fe..58fabdd8c 100644
--- a/src/kernel/cpumgr.C
+++ b/src/kernel/cpumgr.C
@@ -8,10 +8,12 @@
 #include <util/singleton.H>
 #include <kernel/ppcarch.H>
 
+cpu_t* CpuManager::cv_cpus[CpuManager::MAXCPUS] = { NULL };
+
 CpuManager::CpuManager()
 {
     for (int i = 0; i < MAXCPUS; i++)
-	iv_cpus[i] = NULL;
+	cv_cpus[i] = NULL;
 }
 
 cpu_t* CpuManager::getCurrentCPU()
@@ -22,7 +24,13 @@ cpu_t* CpuManager::getCurrentCPU()
 
 void CpuManager::init()
 {
-    Singleton<CpuManager>::instance().startCPU();
+    for (int i = 0; i < KERNEL_MAX_SUPPORTED_CPUS; i++)
+	Singleton<CpuManager>::instance().startCPU(i);
+}
+
+void CpuManager::init_slave_smp(cpu_t* cpu)
+{
+    Singleton<CpuManager>::instance().startSlaveCPU(cpu);
 }
 
 void CpuManager::startCPU(ssize_t i)
@@ -30,48 +38,56 @@ void CpuManager::startCPU(ssize_t i)
     bool currentCPU = false;
     if (i < 0)
     {
-	// TODO: Determine position of this CPU, somehow.
-	i = 0;
-
+	i = getCpuId();
+	currentCPU = true;
+    }
+    else if (getCpuId() == i)
+    {
 	currentCPU = true;
     }
-
-    printk("Starting CPU %d...", i);    
 
     // Initialize CPU structure.
-    if (NULL == iv_cpus[i])
+    if (NULL == cv_cpus[i])
     {
-	cpu_t* cpu = iv_cpus[i] = new cpu_t;
+	printk("Starting CPU %d...", i);    
+	cpu_t* cpu = cv_cpus[i] = new cpu_t;
 	
 	// Initialize CPU.
 	cpu->cpu = i;
-	cpu->scheduler = new Scheduler(cpu);
+	cpu->scheduler = &Singleton<Scheduler>::instance();
 	cpu->kernel_stack = 
 	    (void*) (((uint64_t)PageManager::allocatePage(4)) + 16320);
 	
 	// Create idle task.
-	task_t * idle_task = TaskManager::createIdleTask();
-	idle_task->cpu = cpu;
+	cpu->idle_task = TaskManager::createIdleTask();
+	cpu->idle_task->cpu = cpu;
 	
-	// Add to scheduler.
-	cpu->scheduler->setIdleTask(idle_task);
+	printk("done\n");
     }
 
     if (currentCPU)
     {
-	ppc_setSPRG3((uint64_t) iv_cpus[i]->scheduler->getIdleTask());
+	ppc_setSPRG3((uint64_t) cv_cpus[i]->idle_task);
 
 	// TODO: Set up decrementer properly.
 	register uint64_t decrementer = 0x0f000000;
 	asm volatile("mtdec %0" :: "r"(decrementer));
-
-    }
-    else
-    {
-	// TODO once we get to SMP.
     }
+    return;
+}
+
+void CpuManager::startSlaveCPU(cpu_t* cpu)
+{
+    ppc_setSPRG3((uint64_t) cpu->idle_task);
     
-    printk("done\n");
+    // TODO: Set up decrementer properly.
+    register uint64_t decrementer = 0x0f000000;
+    asm volatile("mtdec %0" :: "r"(decrementer));
 
     return;
 }
+
+uint64_t isCPU0()
+{
+    return (0 == getCpuId());
+}
diff --git a/src/kernel/kernel.C b/src/kernel/kernel.C
index 00f3002e5..7b119d232 100644
--- a/src/kernel/kernel.C
+++ b/src/kernel/kernel.C
@@ -14,6 +14,7 @@
 
 extern "C" void kernel_dispatch_task();
 extern void init_main(void* unused);
+extern uint64_t kernel_other_thread_spinlock;
 
 class Kernel
 {
@@ -27,6 +28,7 @@ class Kernel
 	Kernel() {};
 };
 
+extern "C"
 int main()
 {
     printk("Booting %s kernel...\n\n", "Chenoo");
@@ -37,12 +39,25 @@ int main()
     kernel.cpuBootstrap();
 
     kernel.inittaskBootstrap();
+    
+    // Ready to let the other CPUs go.
+    kernel_other_thread_spinlock = 1;
 
     kernel_dispatch_task(); // no return.    
     while(1);
     return 0;
 }
 
+extern "C"
+int smp_slave_main(cpu_t* cpu)
+{
+    CpuManager::init_slave_smp(cpu);
+    VmmManager::init_slb();
+    cpu->scheduler->setNextRunnable();
+    kernel_dispatch_task();
+    return 0;
+}
+
 void Kernel::cppBootstrap()
 {
     // Call default constructors for any static objects.
@@ -73,6 +88,5 @@ void Kernel::inittaskBootstrap()
     task_t * t = TaskManager::createTask(&init_main, NULL);
     t->cpu = CpuManager::getCurrentCPU();
     TaskManager::setCurrentTask(t);
-
 }
 
diff --git a/src/kernel/scheduler.C b/src/kernel/scheduler.C
index c7c26448c..36acd20b4 100644
--- a/src/kernel/scheduler.C
+++ b/src/kernel/scheduler.C
@@ -1,10 +1,13 @@
 #include <kernel/task.H>
 #include <kernel/scheduler.H>
 #include <kernel/taskmgr.H>
+#include <kernel/cpu.H>
+#include <kernel/cpumgr.H>
+#include <kernel/console.H>
 
 void Scheduler::addTask(task_t* t)
 {
-    if (iv_idleTask != t)
+    if (t->cpu->idle_task != t)
     {
 	iv_taskList.insert(t);
     }
@@ -21,7 +24,10 @@ void Scheduler::setNextRunnable()
 
     if (NULL == t)
     {
-	t = iv_idleTask;
+	t = CpuManager::getCurrentCPU()->idle_task;
+	// Set short decrementer.
+	register uint64_t decrementer = 0x000f0000;
+	asm volatile("mtdec %0" :: "r"(decrementer));
     }
     
     TaskManager::setCurrentTask(t);
diff --git a/src/kernel/start.S b/src/kernel/start.S
index 169497601..6df27281c 100644
--- a/src/kernel/start.S
+++ b/src/kernel/start.S
@@ -4,14 +4,14 @@
 
 .global _start
 _start:
-    ;// Enter 64 bit mode
-    mfmsr r0
-    lis r11, 0x8000
-    sldi r11,r11, 32
-    or r11,r11,r0
-    mtmsr r11
-    isync
-    
+    ;// Check if first thread.
+    mfspr r1, PIR
+    li r2, 0x0003
+    and r1, r1, r2
+    li r2, 0x0
+    cmpl cr0, r1, r2
+    bnel cr0, _other_thread_spinlock
+
     ;// Relocate code
     bl pre_relocate	;// fill LR with address
 pre_relocate:
@@ -123,6 +123,8 @@ UNIMPL_INTERRUPT(vector_unavail, 0xF20)
 UNIMPL_INTERRUPT(vsx_unavail, 0xF40)
 
 .section .text
+;// _main:
+;//	Set up stack and TOC and call kernel's main.
 _main:
     ;// Set up initial TOC Base
     lis r2, main@h
@@ -139,6 +141,36 @@ _main:
 _main_loop:
     b _main_loop
 
+;// _other_thread_spinlock:
+;//	Used for threads other than 0 to wait for the system to boot to a 
+;//	stable point where we can start the other threads.  At this point
+;//	nothing is initalized in the thread except r1 = thread ID.
+_other_thread_spinlock:
+    ;// Read spinlock value.
+    lis r2, kernel_other_thread_spinlock@h
+    ori r2, r2, kernel_other_thread_spinlock@l
+    ld r3, 0(r2)
+    ;// Loop until value is not 0...
+    li r4, 0
+    cmp cr0, r3, r4
+    bne _other_thread_spinlock_complete
+    b _other_thread_spinlock
+;// Now released by primary thread.
+_other_thread_spinlock_complete:
+    ;// Get CPU object from thread ID.
+    lis r2, _ZN10CpuManager7cv_cpusE@h
+    ori r2, r2, _ZN10CpuManager7cv_cpusE@l
+    muli r3, r1, 8
+    add r2, r3, r2
+    ld r3, 0(r2)		;// Load CPU object.
+    ld r1, 0(r3)		;// Load initial stack.
+    
+    lis r2, smp_slave_main@h 	;// Load TOC base.
+    ori r2, r2, smp_slave_main@l
+    ld r2, 8(r2)
+    bl smp_slave_main		;// Call smp_slave_main
+    b _main_loop
+
     ;// @fn kernel_save_task
     ;// Saves context to task structure and branches back to requested addr.
     ;// 
@@ -274,6 +306,10 @@ kernel_dispatch_task:
 kernel_stack:
     .space 16*1024
 
+.global kernel_other_thread_spinlock
+kernel_other_thread_spinlock:
+    .space 8
+
 .section .text.hreset
 hreset:
     b _start
diff --git a/src/kernel/taskmgr.C b/src/kernel/taskmgr.C
index 7ed190709..c21a19d97 100644
--- a/src/kernel/taskmgr.C
+++ b/src/kernel/taskmgr.C
@@ -19,6 +19,7 @@ task_t* TaskManager::getCurrentTask()
 
 void TaskManager::setCurrentTask(task_t* t)
 {
+    t->cpu = getCurrentTask()->cpu;
     ppc_setSPRG3((uint64_t)t);
     return;
 }
diff --git a/src/kernel/vmmmgr.C b/src/kernel/vmmmgr.C
index 57e59eace..de7029483 100644
--- a/src/kernel/vmmmgr.C
+++ b/src/kernel/vmmmgr.C
@@ -19,6 +19,13 @@ void VmmManager::init()
     printk("done.\n");
 };
 
+void VmmManager::init_slb()
+{
+    VmmManager& v = Singleton<VmmManager>::instance();
+    v.initSLB();
+    v.initSDR1();
+}
+
 void VmmManager::initSLB()
 {
     register uint64_t slbRS, slbRB;
diff --git a/src/lib/syscall_task.C b/src/lib/syscall_task.C
index d637b45af..a6b575b5e 100644
--- a/src/lib/syscall_task.C
+++ b/src/lib/syscall_task.C
@@ -2,6 +2,7 @@
 #include <sys/syscall.h>
 #include <kernel/task.H>
 #include <kernel/taskmgr.H>
+#include <kernel/cpu.H>
 
 using namespace Systemcalls;
 
@@ -32,3 +33,10 @@ tid_t task_gettid()
     return task->tid;
     //return (tid_t)_syscall0(TASK_GETTID);
 }
+
+cpuid_t task_getcpuid()
+{
+    register task_t* task;
+    asm volatile("addi %0, 13, 0" : "=r"(task));
+    return task->cpu->cpu;
+}
diff --git a/src/sys/init/init_main.C b/src/sys/init/init_main.C
index f2b40d497..91c30bee4 100644
--- a/src/sys/init/init_main.C
+++ b/src/sys/init/init_main.C
@@ -9,8 +9,9 @@ mutex_t global_mutex;
 void init_child(void* unused)
 {
     mutex_lock(global_mutex);
-    printk("Here I am %d\n", task_gettid());
+    printk("Crun: %d on %d\n", task_gettid(), task_getcpuid());
     mutex_unlock(global_mutex);
+    for (volatile int i = 0 ; i < 100000; i++);
     task_end();
 }
 
@@ -20,7 +21,7 @@ void init_main(void* unused)
 {
     printk("Starting init!\n");
     
-    printk("Bringing up VFS...");
+    printk("Bringing up VFS..."); 
     task_create(&vfs_main, NULL);
     task_yield(); // TODO... add a barrier to ensure VFS is fully up.
     
@@ -40,8 +41,8 @@ void init_main(void* unused)
     {
 	mutex_lock(global_mutex);
 	int t = task_create(&init_child, NULL);
-	printk("Created child %d\n", t);
-	for (volatile int i = 0 ; i < 10000000; i++);
+	printk("Create child %d\n", t);
+	for (volatile int i = 0 ; i < 1000000; i++);
 	mutex_unlock(global_mutex);
     }
 }