SMT support.

6 files changed, 112 insertions, 32 deletions

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;