Ensure scratch operations happen on master core.

Change-Id: I970d645108de041d410599847edce877cb794015
Reviewed-on: http://gfw160.austin.ibm.com:8080/gerrit/275
Tested-by: Jenkins Server
Reviewed-by: Daniel M. Crowell <dcrowell@us.ibm.com>
Reviewed-by: A. Patrick Williams III <iawillia@us.ibm.com>

4 files changed, 229 insertions, 161 deletions

diff --git a/src/kernel/cpumgr.C b/src/kernel/cpumgr.C
index 3e7a83a38..d8a9960f6 100644
--- a/src/kernel/cpumgr.C
+++ b/src/kernel/cpumgr.C
@@ -49,6 +49,15 @@ cpu_t* CpuManager::getCurrentCPU()
     return cv_cpus[getPIR()];
 }
 
+cpu_t* CpuManager::getMasterCPU()
+{
+    for (int i = 0; i < MAXCPUS; i++)
+        if (cv_cpus[i] != NULL)
+            if (cv_cpus[i]->master)
+                return cv_cpus[i];
+    return NULL;
+}
+
 void CpuManager::init()
 {
     // For the initial boot we only want to set up CPU objects for the threads
@@ -115,14 +124,14 @@ void CpuManager::startCPU(ssize_t i)
 
 	// Initialize CPU.
 	cpu->cpu = i;
-    if (currentCPU)
-    {
-        cpu->master = true;
-    }
-    else
-    {
-        cpu->master = false;
-    }
+        if (currentCPU)
+        {
+            cpu->master = true;
+        }
+        else
+        {
+            cpu->master = false;
+        }
 	cpu->scheduler = &Singleton<Scheduler>::instance();
         cpu->scheduler_extra = NULL;
 	cpu->kernel_stack =
diff --git a/src/kernel/scheduler.C b/src/kernel/scheduler.C
index 9ed814463..7b57792a5 100644
--- a/src/kernel/scheduler.C
+++ b/src/kernel/scheduler.C
@@ -27,6 +27,7 @@
 #include <kernel/cpumgr.H>
 #include <kernel/console.H>
 #include <kernel/timemgr.H>
+#include <util/lockfree/atomic_construct.H>
 
 void Scheduler::addTask(task_t* t)
 {
@@ -35,11 +36,10 @@ void Scheduler::addTask(task_t* t)
         // If task is pinned to this CPU, add to the per-CPU queue.
         if (0 != t->affinity_pinned)
         {
-            // Allocate a per-CPU queue if this is the first pinning CPU.
-            if (NULL == t->cpu->scheduler_extra)
-            {
-                t->cpu->scheduler_extra = new Runqueue_t();
-            }
+            // Allocate a per-CPU queue if this is the first pinning to CPU.
+            Util::Lockfree::atomic_construct(
+                    reinterpret_cast<Runqueue_t**>(&t->cpu->scheduler_extra));
+
             // Insert into queue.
             static_cast<Runqueue_t*>(t->cpu->scheduler_extra)->insert(t);
         }
@@ -51,6 +51,22 @@ void Scheduler::addTask(task_t* t)
     }
 }
 
+void Scheduler::addTaskMasterCPU(task_t* t)
+{
+    if (t->cpu->idle_task != t)
+    {
+        cpu_t* master = CpuManager::getMasterCPU();
+
+        // Allocate a per-CPU queue if this is the first pinning to CPU.
+        Util::Lockfree::atomic_construct(
+                reinterpret_cast<Runqueue_t**>(&master->scheduler_extra));
+
+        // Insert into queue.
+        static_cast<Runqueue_t*>(master->scheduler_extra)->insert(t);
+
+    }
+}
+
 void Scheduler::returnRunnable()
 {
     this->addTask(TaskManager::getCurrentTask());
@@ -60,13 +76,13 @@ void Scheduler::setNextRunnable()
 {
     task_t* t = NULL;
     cpu_t* cpu = CpuManager::getCurrentCPU();
-    
+
     // Check for ready task in local run-queue, if it exists.
     if (NULL != cpu->scheduler_extra)
     {
         t = static_cast<Runqueue_t*>(cpu->scheduler_extra)->remove();
     }
-    
+
     // Check for ready task in global run-queue.
     if (NULL == t)
     {
@@ -84,6 +100,6 @@ void Scheduler::setNextRunnable()
     {
         setDEC(TimeManager::getTimeSliceCount());
     }
-   
+
     TaskManager::setCurrentTask(t);
 }
diff --git a/src/kernel/start.S b/src/kernel/start.S
index 2bbe41184..332fe7796 100644
--- a/src/kernel/start.S
+++ b/src/kernel/start.S
@@ -401,14 +401,40 @@ system_call_fast_path:
 3:
     cmpi cr0, r3, 0x802
     bne cr0, 4f
+        ;// Check for being on master processor.
+    mfsprg3 r6          ;// Get task structure.
+    ld r6, 0(r6)        ;// Get CPU structure.
+    lbz r6, 12(r6)      ;// Read master boolean.
+    cmpi cr0, r6, 0x0
+    beq cr0, 300f       ;// Call TASK_MIGRATE_TO_MASTER if not on master.
+        ;// Read scratch.
     mtspr 276, r4
     isync
     mfspr r3, 277
     b 1f                        ;// Jump to exit point.
+        ;// Migrate task via TASK_MIGRATE_TO_MASTER
+300:
+        ;// Roll back NIA one instruction.
+    mfsrr0 r6
+    addi r6, r6, -4
+    mtsrr0 r6
+        ;// Move our syscall number to r6 (for TASK_MIGRATE_TO_MASTER handler).
+    mr r6, r3
+        ;// Set up TASK_MIGRATE_TO_MASTER syscall number.
+    li r3, 3
+        ;// Call back to syscall handler.
+    b intvect_system_call
         ;// Check if this is SCRATCH write (0x803).
 4:
     cmpi cr0, r3, 0x803
     bne cr0, 5f
+        ;// Check for master processor.
+    mfsprg3 r6          ;// Get task structure.
+    ld r6, 0(r6)        ;// Get CPU structure.
+    lbz r6, 12(r6)      ;// Read master boolean.
+    cmpi cr0, r6, 0x0
+    beq cr0, 300b       ;// Call TASK_MIGRATE_TO_MASTER if not on master.
+        ;// Write scratch.
     mtspr 276, r4
     isync
     mtspr 277, r5
diff --git a/src/kernel/syscall.C b/src/kernel/syscall.C
index 098474d8e..8f69e934e 100644
--- a/src/kernel/syscall.C
+++ b/src/kernel/syscall.C
@@ -60,7 +60,7 @@ namespace Systemcalls
     void TaskYield(task_t*);
     void TaskStart(task_t*);
     void TaskEnd(task_t*);
-    void TaskGettid(task_t*);
+    void TaskMigrateToMaster(task_t*);
     void MsgQCreate(task_t*);
     void MsgQDestroy(task_t*);
     void MsgQRegisterRoot(task_t*);
@@ -82,38 +82,39 @@ namespace Systemcalls
     void MmAllocBlock(task_t *t);
 
     syscall syscalls[] =
-	{
-	    &TaskYield,  // TASK_YIELD
-	    &TaskStart,  // TASK_START
-	    &TaskEnd,  // TASK_END
-
-	    &MsgQCreate,  // MSGQ_CREATE
-	    &MsgQDestroy,  // MSGQ_DESTROY
-	    &MsgQRegisterRoot,  // MSGQ_REGISTER_ROOT
-	    &MsgQResolveRoot,  // MSGQ_RESOLVE_ROOT
-
-	    &MsgSend,  // MSG_SEND
-	    &MsgSendRecv,  // MSG_SENDRECV
-	    &MsgRespond,  // MSG_RESPOND
-	    &MsgWait,  // MSG_WAIT
-
-	    &MmioMap,  // MMIO_MAP
-	    &MmioUnmap,  // MMIO_UNMAP
+        {
+            &TaskYield,  // TASK_YIELD
+            &TaskStart,  // TASK_START
+            &TaskEnd,  // TASK_END
+            &TaskMigrateToMaster, // TASK_MIGRATE_TO_MASTER
+
+            &MsgQCreate,  // MSGQ_CREATE
+            &MsgQDestroy,  // MSGQ_DESTROY
+            &MsgQRegisterRoot,  // MSGQ_REGISTER_ROOT
+            &MsgQResolveRoot,  // MSGQ_RESOLVE_ROOT
+
+            &MsgSend,  // MSG_SEND
+            &MsgSendRecv,  // MSG_SENDRECV
+            &MsgRespond,  // MSG_RESPOND
+            &MsgWait,  // MSG_WAIT
+
+            &MmioMap,  // MMIO_MAP
+            &MmioUnmap,  // MMIO_UNMAP
             &DevMap,  // DEV_MAP
             &DevUnmap,  // DEV_UNMAP
 
-	    &TimeNanosleep,  // TIME_NANOSLEEP
+            &TimeNanosleep,  // TIME_NANOSLEEP
 
-        &FutexWait,  // FUTEX_WAIT
-        &FutexWake,  // FUTEX_WAKE
+            &FutexWait,  // FUTEX_WAIT
+            &FutexWake,  // FUTEX_WAKE
 
-        &Shutdown,  // MISC_SHUTDOWN
+            &Shutdown,  // MISC_SHUTDOWN
 
-        &CpuCoreType,  // MISC_CPUCORETYPE
-        &CpuDDLevel,  // MISC_CPUDDLEVEL
+            &CpuCoreType,  // MISC_CPUCORETYPE
+            &CpuDDLevel,  // MISC_CPUDDLEVEL
 
-        &MmAllocBlock, // MM_ALLOC_BLOCK
-	};
+            &MmAllocBlock, // MM_ALLOC_BLOCK
+        };
 };
 
 extern "C"
@@ -125,13 +126,13 @@ void kernel_execute_system_call()
     uint64_t syscall = t->context.gprs[3];
     if (syscall > SYSCALL_MAX)
     {
-	// TODO : kill task.
-	printk("Invalid syscall : %ld\n", syscall);
-	while(1);
+        // TODO : kill task.
+        printk("Invalid syscall : %ld\n", syscall);
+        while(1);
     }
     else
     {
-	syscalls[syscall](t);
+        syscalls[syscall](t);
     }
 }
 
@@ -139,65 +140,81 @@ namespace Systemcalls
 {
     void TaskYield(task_t* t)
     {
-	Scheduler* s = t->cpu->scheduler;
-	s->returnRunnable();
-	s->setNextRunnable();
+        Scheduler* s = t->cpu->scheduler;
+        s->returnRunnable();
+        s->setNextRunnable();
     }
 
     void TaskStart(task_t* t)
     {
-	task_t* newTask =
-	    TaskManager::createTask((TaskManager::task_fn_t)TASK_GETARG0(t),
-				    (void*)TASK_GETARG1(t));
-	newTask->cpu = t->cpu;
-	t->cpu->scheduler->addTask(newTask);
+        task_t* newTask =
+            TaskManager::createTask((TaskManager::task_fn_t)TASK_GETARG0(t),
+                                    (void*)TASK_GETARG1(t));
+        newTask->cpu = t->cpu;
+        t->cpu->scheduler->addTask(newTask);
 
-	TASK_SETRTN(t, newTask->tid);
+        TASK_SETRTN(t, newTask->tid);
     }
 
     void TaskEnd(task_t* t)
     {
-	// Make sure task pointers are updated before we delete this task.
-	t->cpu->scheduler->setNextRunnable();
+        // Make sure task pointers are updated before we delete this task.
+        t->cpu->scheduler->setNextRunnable();
 
-	// TODO: Deal with join.
+        // TODO: Deal with join.
 
-	// Clean up task memory.
-	PageManager::freePage(t->context.stack_ptr, TASK_DEFAULT_STACK_SIZE);
-	delete t;
+        // Clean up task memory.
+        PageManager::freePage(t->context.stack_ptr, TASK_DEFAULT_STACK_SIZE);
+        delete t;
+    }
+
+    void TaskMigrateToMaster(task_t* t)
+    {
+        // Move r6 to r3.
+        //     This is needed so that this system call can be called from
+        //     within a "fast" system call in start.S.  The fast system call
+        //     will populate r6 with it's own syscall number.  When we return
+        //     from this system call, on the master processor, we'll be back
+        //     at the 'sc' instruction with r3 back to the fast syscall, and
+        //     the fast syscall will be executed on the master processor.
+        TASK_SETRTN(t, TASK_GETARG2(t));
+
+        // Move task to master CPU and pick a new task.
+        t->cpu->scheduler->addTaskMasterCPU(t);
+        t->cpu->scheduler->setNextRunnable();
     }
 
     void MsgQCreate(task_t* t)
     {
-	TASK_SETRTN(t, (uint64_t) new MessageQueue());
+        TASK_SETRTN(t, (uint64_t) new MessageQueue());
     }
 
     void MsgQDestroy(task_t* t)
     {
-	MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
-	if (NULL != mq)
-	    delete mq;
-	TASK_SETRTN(t, 0);
+        MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
+        if (NULL != mq)
+            delete mq;
+        TASK_SETRTN(t, 0);
     }
 
     static MessageQueue* msgQRoot = NULL;
 
     void MsgQRegisterRoot(task_t* t)
     {
-	msgQRoot = (MessageQueue*) TASK_GETARG0(t);
-	TASK_SETRTN(t, 0);
+        msgQRoot = (MessageQueue*) TASK_GETARG0(t);
+        TASK_SETRTN(t, 0);
     }
 
     void MsgQResolveRoot(task_t* t)
     {
-	TASK_SETRTN(t, (uint64_t) msgQRoot);
+        TASK_SETRTN(t, (uint64_t) msgQRoot);
     }
 
     void MsgSend(task_t* t)
     {
-	MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
-	msg_t* m = (msg_t*) TASK_GETARG1(t);
-	m->__reserved__async = 0; // set to async msg.
+        MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
+        msg_t* m = (msg_t*) TASK_GETARG1(t);
+        m->__reserved__async = 0; // set to async msg.
 
         if (m->type >= MSG_FIRST_SYS_TYPE)
         {
@@ -206,32 +223,32 @@ namespace Systemcalls
             return;
         }
 
-	mq->lock.lock();
-
-	// Get waiting (server) task.
-	task_t* waiter = mq->waiting.remove();
-	if (NULL == waiter) // None found, add to 'messages' queue.
-	{
-	    MessagePending* mp = new MessagePending();
-	    mp->key = m;
-	    mp->task = t;
-	    mq->messages.insert(mp);
-	}
-	else // Add waiter back to its scheduler.
-	{
-	    TASK_SETRTN(waiter, (uint64_t) m);
-	    waiter->cpu->scheduler->addTask(waiter);
-	}
-
-	mq->lock.unlock();
-	TASK_SETRTN(t, 0);
+        mq->lock.lock();
+
+        // Get waiting (server) task.
+        task_t* waiter = mq->waiting.remove();
+        if (NULL == waiter) // None found, add to 'messages' queue.
+        {
+            MessagePending* mp = new MessagePending();
+            mp->key = m;
+            mp->task = t;
+            mq->messages.insert(mp);
+        }
+        else // Add waiter back to its scheduler.
+        {
+            TASK_SETRTN(waiter, (uint64_t) m);
+            waiter->cpu->scheduler->addTask(waiter);
+        }
+
+        mq->lock.unlock();
+        TASK_SETRTN(t, 0);
     }
 
     void MsgSendRecv(task_t* t)
     {
-	MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
-	msg_t* m = (msg_t*) TASK_GETARG1(t);
-	m->__reserved__async = 1; // set to sync msg.
+        MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
+        msg_t* m = (msg_t*) TASK_GETARG1(t);
+        m->__reserved__async = 1; // set to sync msg.
 
         if (m->type >= MSG_FIRST_SYS_TYPE)
         {
@@ -241,45 +258,45 @@ namespace Systemcalls
         }
 
         // Create pending response object.
-	MessagePending* mp = new MessagePending();
-	mp->key = m;
-	mp->task = t;
-
-	mq->lock.lock();
-
-	// Get waiting (server) task.
-	task_t* waiter = mq->waiting.remove();
-	if (NULL == waiter) // None found, add to 'messages' queue.
-	{
-	    mq->messages.insert(mp);
-	    // Choose next thread to execute, this one is delayed.
-	    t->cpu->scheduler->setNextRunnable();
-	}
-	else // Context switch to waiter.
-	{
-	    TASK_SETRTN(waiter, (uint64_t) m);
-	    mq->responses.insert(mp);
-	    waiter->cpu = t->cpu;
-	    TaskManager::setCurrentTask(waiter);
-	}
-
-	mq->lock.unlock();
+        MessagePending* mp = new MessagePending();
+        mp->key = m;
+        mp->task = t;
+
+        mq->lock.lock();
+
+        // Get waiting (server) task.
+        task_t* waiter = mq->waiting.remove();
+        if (NULL == waiter) // None found, add to 'messages' queue.
+        {
+            mq->messages.insert(mp);
+            // Choose next thread to execute, this one is delayed.
+            t->cpu->scheduler->setNextRunnable();
+        }
+        else // Context switch to waiter.
+        {
+            TASK_SETRTN(waiter, (uint64_t) m);
+            mq->responses.insert(mp);
+            waiter->cpu = t->cpu;
+            TaskManager::setCurrentTask(waiter);
+        }
+
+        mq->lock.unlock();
     }
 
     void MsgRespond(task_t* t)
     {
-    	MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
-	msg_t* m = (msg_t*) TASK_GETARG1(t);
+        MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
+        msg_t* m = (msg_t*) TASK_GETARG1(t);
 
-	mq->lock.lock();
-	MessagePending* mp = mq->responses.find(m);
-	if (NULL != mp)
-	{
-	    task_t* waiter = mp->task;
+        mq->lock.lock();
+        MessagePending* mp = mq->responses.find(m);
+        if (NULL != mp)
+        {
+            task_t* waiter = mp->task;
 
-	    mq->responses.erase(mp);
+            mq->responses.erase(mp);
             mq->lock.unlock();
-	    delete mp;
+            delete mp;
 
             if (m->type >= MSG_FIRST_SYS_TYPE)
             {
@@ -300,52 +317,52 @@ namespace Systemcalls
                 TASK_SETRTN(t,0);
                 t->cpu->scheduler->addTask(t);
             }
-	}
-	else
-	{
-	    TASK_SETRTN(t, -1);
+        }
+        else
+        {
+            TASK_SETRTN(t, -1);
             mq->lock.unlock();
-	}
+        }
     }
 
     void MsgWait(task_t* t)
     {
-    	MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
-
-	mq->lock.lock();
-	MessagePending* mp = mq->messages.remove();
-
-	if (NULL == mp)
-	{
-	    mq->waiting.insert(t);
-	    t->cpu->scheduler->setNextRunnable();
-	}
-	else
-	{
-	    msg_t* m = mp->key;
-	    if (m->__reserved__async)
-		mq->responses.insert(mp);
-	    else
-		delete mp;
-	    TASK_SETRTN(t, (uint64_t) m);
-	}
-	mq->lock.unlock();
+        MessageQueue* mq = (MessageQueue*) TASK_GETARG0(t);
+
+        mq->lock.lock();
+        MessagePending* mp = mq->messages.remove();
+
+        if (NULL == mp)
+        {
+            mq->waiting.insert(t);
+            t->cpu->scheduler->setNextRunnable();
+        }
+        else
+        {
+            msg_t* m = mp->key;
+            if (m->__reserved__async)
+                mq->responses.insert(mp);
+            else
+                delete mp;
+            TASK_SETRTN(t, (uint64_t) m);
+        }
+        mq->lock.unlock();
     }
 
     void MmioMap(task_t* t)
     {
-	void* ra = (void*)TASK_GETARG0(t);
-	size_t pages = TASK_GETARG1(t);
+        void* ra = (void*)TASK_GETARG0(t);
+        size_t pages = TASK_GETARG1(t);
 
-	TASK_SETRTN(t, (uint64_t) VmmManager::mmioMap(ra,pages));
+        TASK_SETRTN(t, (uint64_t) VmmManager::mmioMap(ra,pages));
     }
 
     void MmioUnmap(task_t* t)
     {
-	void* ea = (void*)TASK_GETARG0(t);
-	size_t pages = TASK_GETARG1(t);
+        void* ea = (void*)TASK_GETARG0(t);
+        size_t pages = TASK_GETARG1(t);
 
-	TASK_SETRTN(t, VmmManager::mmioUnmap(ea,pages));
+        TASK_SETRTN(t, VmmManager::mmioUnmap(ea,pages));
     }
 
     /**
@@ -374,11 +391,11 @@ namespace Systemcalls
 
     void TimeNanosleep(task_t* t)
     {
-	TimeManager::delayTask(t, TASK_GETARG0(t), TASK_GETARG1(t));
-	TASK_SETRTN(t, 0);
+        TimeManager::delayTask(t, TASK_GETARG0(t), TASK_GETARG1(t));
+        TASK_SETRTN(t, 0);
 
-	Scheduler* s = t->cpu->scheduler;
-	s->setNextRunnable();
+        Scheduler* s = t->cpu->scheduler;
+        s->setNextRunnable();
     }
 
     /**