13 files changed, 367 insertions, 203 deletions

diff --git a/src/include/kernel/cpu.H b/src/include/kernel/cpu.H
index 420ec8827..ee24bdd64 100644
--- a/src/include/kernel/cpu.H
+++ b/src/include/kernel/cpu.H
@@ -22,7 +22,7 @@
 //  IBM_PROLOG_END
 /** @file cpu.H
  *  @brief Defines kernel information and functions about CPUs.
- *  
+ *
  *  In this kernel the term CPU refers to a hardware thread (SMT), not core.
  */
 #ifndef __KERNEL_CPU_H
@@ -40,6 +40,12 @@ class Scheduler;
 
 /** @struct cpu_t
  *  @brief Stores per-CPU kernel information.
+ *
+ *  @note kernel_stack and master need to be at fixed locations in this
+ *        structure due to usages in start.S.
+ *
+ *  - kernel_stack is a double-word at cpu_t[0 bytes].
+ *  - master is a byte at cpu_t[12 bytes].
  */
 struct cpu_t
 {
@@ -49,8 +55,14 @@ struct cpu_t
     /** ID of the CPU (PIR value) */
     cpuid_t cpu;
 
-    /** If the CPU is the master */
-    bool master;
+    struct
+    {
+        /** If the CPU is the master */
+        bool master;
+
+        /** Ensure alignment of master attribute for asm code. */
+        uint64_t __reserved_master:24;
+    } PACKED;
 
     /** Pointer to the scheduler for this CPU (may not be unique) */
     Scheduler* scheduler;
diff --git a/src/include/kernel/cpumgr.H b/src/include/kernel/cpumgr.H
index e6fb33a77..31759564b 100644
--- a/src/include/kernel/cpumgr.H
+++ b/src/include/kernel/cpumgr.H
@@ -30,14 +30,18 @@ class CpuManager
 {
     public:
 	enum { MAXCPUS = KERNEL_MAX_SUPPORTED_CPUS };
-	
-	/** @fn getCurrentCPU 
+
+	/** @fn getCurrentCPU
 	 *  Returns a pointer to the current CPU structure by using the
 	 *  task structure in SPRG3.
 	 */
 	static cpu_t* getCurrentCPU();
 	static cpu_t* getCpu(size_t i) { return cv_cpus[i]; }
 
+        /** @brief Return pointer to master CPU object.
+         */
+        static cpu_t* getMasterCPU();
+
 	static void init();
 	static void init_slave_smp(cpu_t*);
 
diff --git a/src/include/kernel/scheduler.H b/src/include/kernel/scheduler.H
index c7a991c45..16861b6e6 100644
--- a/src/include/kernel/scheduler.H
+++ b/src/include/kernel/scheduler.H
@@ -35,12 +35,13 @@ class Scheduler
 	friend class CpuManager;
 
 	void addTask(task_t*);
+        void addTaskMasterCPU(task_t*);
 
 	void returnRunnable();
 	void setNextRunnable();
 
     protected:
-	Scheduler() : 
+	Scheduler() :
 		iv_taskList() {};
 	~Scheduler() {};
 
diff --git a/src/include/kernel/syscalls.H b/src/include/kernel/syscalls.H
index 47295ecc2..332273a07 100644
--- a/src/include/kernel/syscalls.H
+++ b/src/include/kernel/syscalls.H
@@ -36,62 +36,66 @@ namespace Systemcalls
      *  These are passed by userspace code via r3 when the sc instruction is
      *  executed.  The kernel performs a case statement to switch to the
      *  appropriate system call handler.
+     *
+     *  @note TASK_MIGRATE_TO_MASTER value must be kept in sync with start.S.
      */
     enum SysCalls
     {
             /** task_yield() */
-	TASK_YIELD = 0,
+        TASK_YIELD = 0,
             /** task_create() */
-	TASK_START,
+        TASK_START = 1,
             /** task_end() */
-	TASK_END,
+        TASK_END = 2,
+            /** task_affinity_migrate_to_master() */
+        TASK_MIGRATE_TO_MASTER = 3,
 
             /** msgq_create() */
-	MSGQ_CREATE,
+        MSGQ_CREATE,
             /** msgq_destroy() */
-	MSGQ_DESTROY,
+        MSGQ_DESTROY,
             /** VFS internal */
-	MSGQ_REGISTER_ROOT,
+        MSGQ_REGISTER_ROOT,
             /** VFS internal */
-	MSGQ_RESOLVE_ROOT,
+        MSGQ_RESOLVE_ROOT,
 
             /** msg_send() */
-	MSG_SEND,
+        MSG_SEND,
             /** msg_sendrecv() */
-	MSG_SENDRECV,
+        MSG_SENDRECV,
             /** msg_respond() */
-	MSG_RESPOND,
+        MSG_RESPOND,
             /** msg_wait() */
-	MSG_WAIT,
+        MSG_WAIT,
 
             /** mmio_map() */
-	MMIO_MAP,
+        MMIO_MAP,
             /** mmio_unmap() */
-	MMIO_UNMAP,
+        MMIO_UNMAP,
             /** dev_map() */
-    DEV_MAP,
+        DEV_MAP,
             /** dev_unmap() */
-    DEV_UNMAP,
+        DEV_UNMAP,
 
             /** nanosleep() */
-	TIME_NANOSLEEP,
+        TIME_NANOSLEEP,
 
             /** futex_wait() */
-    FUTEX_WAIT,
+        FUTEX_WAIT,
             /** futex_wake() */
-    FUTEX_WAKE,
+        FUTEX_WAKE,
 
             /** shutdown() */
-    MISC_SHUTDOWN,
+        MISC_SHUTDOWN,
             /** cpu_core_type() */
-    MISC_CPUCORETYPE,
+        MISC_CPUCORETYPE,
             /** cpu_dd_level() */
-    MISC_CPUDDLEVEL,
+        MISC_CPUDDLEVEL,
 
             /** mm_alloc_block() */
-    MM_ALLOC_BLOCK,
+        MM_ALLOC_BLOCK,
 
-	SYSCALL_MAX
+        SYSCALL_MAX
     };
 
     /** @enum SysCalls_FastPath
diff --git a/src/include/kernel/types.h b/src/include/kernel/types.h
index a53dd79e9..7b201b0de 100644
--- a/src/include/kernel/types.h
+++ b/src/include/kernel/types.h
@@ -25,11 +25,11 @@
 
 #include <stdint.h>
 
-typedef uint16_t 	tid_t;	// This is 16-bit for the VMM mapping of 
+typedef uint16_t 	tid_t;	// This is 16-bit for the VMM mapping of
 				// stacks.  See VmmManager.
 struct task_t;
 
-typedef uint64_t	cpuid_t;
+typedef uint32_t	cpuid_t;
 struct cpu_t;
 
 #endif
diff --git a/src/include/sys/task.h b/src/include/sys/task.h
index fc1604607..9f43996c4 100644
--- a/src/include/sys/task.h
+++ b/src/include/sys/task.h
@@ -30,7 +30,7 @@
 #include <kernel/types.h>
 
 #ifdef __cplusplus
-extern "C" 
+extern "C"
 {
 #endif
 
@@ -102,8 +102,8 @@ tid_t task_exec(const char* path, void* arg);
 /** @fn task_affinity_pin
  *  @brief Pins a task onto the CPU it is currently executing on.
  *
- *  This function may be called any number of times and each should be paired 
- *  with a task_affinity_unpin call.  This is so that callers do not need to 
+ *  This function may be called any number of times and each should be paired
+ *  with a task_affinity_unpin call.  This is so that callers do not need to
  *  be concerned with affinity pinning desires of functions above and below in
  *  a call stack.
  *
@@ -121,6 +121,15 @@ void task_affinity_pin();
  */
 void task_affinity_unpin();
 
+/** @fn task_affinity_migrate_to_master
+ *  @brief Moves a task from the CPU it is on to the master core/thread.
+ *
+ *  Unless the affinity is pinned, the task could be migrated to another
+ *  core at any point in time.  Suggestion is to use task_affinity_pin
+ *  prior to this call.
+ */
+void task_affinity_migrate_to_master();
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/include/util/lockfree/atomic_construct.H b/src/include/util/lockfree/atomic_construct.H
new file mode 100644
index 000000000..6946a1c60
--- /dev/null
+++ b/src/include/util/lockfree/atomic_construct.H
@@ -0,0 +1,62 @@
+//  IBM_PROLOG_BEGIN_TAG
+//  This is an automatically generated prolog.
+//
+//  $Source: src/include/util/lockfree/atomic_construct.H $
+//
+//  IBM CONFIDENTIAL
+//
+//  COPYRIGHT International Business Machines Corp. 2011
+//
+//  p1
+//
+//  Object Code Only (OCO) source materials
+//  Licensed Internal Code Source Materials
+//  IBM HostBoot Licensed Internal Code
+//
+//  The source code for this program is not published or other-
+//  wise divested of its trade secrets, irrespective of what has
+//  been deposited with the U.S. Copyright Office.
+//
+//  Origin: 30
+//
+//  IBM_PROLOG_END
+#ifndef __UTIL_LOCKFREE_ATOMIC_CONSTRUCT_H
+#define __UTIL_LOCKFREE_ATOMIC_CONSTRUCT_H
+
+namespace Util
+{
+    namespace Lockfree
+    {
+
+        /** @brief Atomically construct an object and assign it to a pointer.
+         *
+         *  This function will check if a pointer is still NULL and
+         *  construct an object, atomically, if needed.  If the pointer is
+         *  not NULL, the current instance will be left alone.
+         *
+         *  @param[in] ptr - Pointer-pointer to the object.
+         *
+         *  Example:
+         *      Foo* iv_foo;  //<--- instance variable in class.
+         *      atomic_construct(&iv_foo);
+         *
+         *  @note It is possible for multiple instances to temporarily exist,
+         *        if this code is called from multiple CPUs, but only the one
+         *        instance will exist outside the lifetime of this function.
+         */
+        template<typename _T>
+            void atomic_construct(_T** ptr)
+            {
+                if (__sync_bool_compare_and_swap(ptr, NULL, NULL))
+                {
+                    _T* instance = new _T();
+                    if (!__sync_bool_compare_and_swap(ptr, NULL, instance))
+                    {
+                        delete instance;
+                    }
+                }
+            }
+    };
+};
+
+#endif
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();
     }
 
     /**
diff --git a/src/lib/syscall_misc.C b/src/lib/syscall_misc.C
index 3ce97396e..b5a44f80a 100644
--- a/src/lib/syscall_misc.C
+++ b/src/lib/syscall_misc.C
@@ -28,7 +28,6 @@ using namespace Systemcalls;
 
 void shutdown(uint64_t i_status)
 {
-    mmio_scratch_write(MMIO_SCRATCH_IPLSTEP_CONFIG, 0x1234ABCD);
     _syscall1(MISC_SHUTDOWN, reinterpret_cast<void*>(i_status));
 }
 
diff --git a/src/lib/syscall_task.C b/src/lib/syscall_task.C
index d986d5374..2e28afd0e 100644
--- a/src/lib/syscall_task.C
+++ b/src/lib/syscall_task.C
@@ -70,9 +70,9 @@ tid_t task_exec(const char* file, void* ptr)
 {
     // The VFS process is responsible for finding the module and creating the
     // new process.  So, we send a message over to that process.
-    
-    tid_t child = -1; 
-    
+
+    tid_t child = -1;
+
     // Create message, send.
     msg_q_t vfsQ = (msg_q_t)_syscall0(MSGQ_RESOLVE_ROOT);
     msg_t* msg = msg_allocate();
@@ -83,14 +83,14 @@ tid_t task_exec(const char* file, void* ptr)
 
     if (0 == rc)
     {
-	// Get child ID from message data 0.
-	child = (tid_t) msg->data[0];
+        // Get child ID from message data 0.
+        child = (tid_t) msg->data[0];
     }
     else
     {
-	child = rc;
+        child = rc;
     }
-    
+
     msg_free(msg);
     return child;
 }
@@ -114,3 +114,8 @@ void task_affinity_unpin()
     // Decrement pin count.
     __sync_sub_and_fetch(&task->affinity_pinned, 1);
 }
+
+void task_affinity_migrate_to_master()
+{
+    _syscall0(TASK_MIGRATE_TO_MASTER);
+}