Implement Kernel->User-space message bridge.

Change-Id: Icf6fc9e10b1c39e981dddf180607b710c597112b
Reviewed-on: http://gfw160.austin.ibm.com:8080/gerrit/249
Tested-by: Jenkins Server
Reviewed-by: A. Patrick Williams III <iawillia@us.ibm.com>

11 files changed, 468 insertions, 41 deletions

diff --git a/src/include/errno.h b/src/include/errno.h
index 664c3d46c..8833457ee 100644
--- a/src/include/errno.h
+++ b/src/include/errno.h
@@ -2,6 +2,7 @@
 #define _ERRNO_H
 
 #define EAGAIN          11      // Try again
-#define	EWOULDBLOCK     EAGAIN  // operation would block
+#define EWOULDBLOCK     EAGAIN  // operation would block
+#define EINVAL          22      // Invalid argument
 
 #endif
diff --git a/src/include/kernel/msg.H b/src/include/kernel/msg.H
index 7a32197f5..954f4ab39 100644
--- a/src/include/kernel/msg.H
+++ b/src/include/kernel/msg.H
@@ -3,6 +3,7 @@
 
 #include <kernel/types.h>
 #include <sys/msg.h>
+#include <kernel/spinlock.H>
 #include <util/locked/list.H>
 #include <util/locked/queue.H>
 
diff --git a/src/include/kernel/msghandler.H b/src/include/kernel/msghandler.H
new file mode 100644
index 000000000..522ffd757
--- /dev/null
+++ b/src/include/kernel/msghandler.H
@@ -0,0 +1,179 @@
+/** @file msghandler.H
+ *  @brief Interfaces for messaging from kernel-space to user-space.
+ */
+#ifndef __KERNEL_MSGHANDLER_H
+#define __KERNEL_MSGHANDLER_H
+
+#include <stdint.h>
+#include <kernel/types.h>
+#include <kernel/msg.H>
+#include <kernel/spinlock.H>
+#include <util/locked/list.H>
+
+// Forward declaration.
+namespace Systemcalls { void MsgRespond(task_t*); };
+
+/** @brief Struct used for holding pending message information. */
+struct MessageHandler_Pending
+{
+    typedef void* key_type;
+        /** Key used for duplicative association. */
+    key_type key;
+        /** (optional) Task deferred by this message. */
+    task_t* task;
+
+        // Linked list pointers used by Util::Locked::List.
+    MessageHandler_Pending* prev;
+    MessageHandler_Pending* next;
+};
+
+/** @class MessageHandler
+ *  @brief Class which facilitiates bridging messages between kernel-space and
+ *         user space.
+ *
+ *  This class provides a 'send message' interface to kernel code to send a
+ *  message into user-space.  The message is a <type, key, data> tuple sent
+ *  to the message queue associated with this object.  The expected response
+ *  from userspace is a <type, key, rc> tuple.
+ *
+ *  The typical use-case of these interfaces is to send a <va, page> pair to a
+ *  user-space resource provider to request a page of memory be read or written.
+ *  A parameter to the 'send message' interface is an optional task which is to
+ *  be deferred until the response is fully processed.
+ *
+ *  The 'send message' interface will attempt to combine duplicate messages
+ *  by associating message requests with the same key and only sending a
+ *  single message to user-space for the key (while that key is outstanding).
+ *  This behavior aids the typical use-case of these interfaces by combining
+ *  multiple tasks page-faulting on a single page into a single request to the
+ *  resource provider.
+ *
+ *  When a response is received, via the user-space msg_respond system call,
+ *  the 'recv message' interface will be called to handle the response.  This
+ *  interface will trigger an overrideable 'handle response' interface for
+ *  each outstanding <key, task> pair.  The default behavior is to resume the
+ *  task on successful responses and kill the task on error responses.  This
+ *  behavior can be changed by overriding the 'handle response' interface.
+ *
+ *  SMP safety:
+ *     The constructor of this class requests the spinlock for the kernel
+ *     subsystem associated with this message handling instance.  Since the
+ *     'recv message' interface is the entry point into this subsystem, the
+ *     spinlock will be obtained prior to handling the message receipt.  When
+ *     the 'send message' interface is called, the typical use-case is that
+ *     the subsystem spinlock is already obtained (since the subsystem is
+ *     requesting a message to be sent), so this object does not obtain the
+ *     spinlock.  This spinlock must be obtained prior to calling 'send
+ *     message' in order for the combining of duplicate messages to be
+ *     SMP-safe.
+ */
+class MessageHandler
+{
+    public:
+        /** @brief Results of the 'handle response' interface.
+         *
+         *  These are used to communicate back to the 'recv message' interface
+         *  what to do with the deferred thread.
+         *
+         *  A result of UNHANDLED_RC will cause the default behavior of
+         *  resuming the task on success and killing the task on error.
+         */
+        enum HandleResult
+        {
+                /** Response rc (if non-zero) has been successfully handled. */
+            SUCCESS,
+                /** Do not resume task yet, no matter the rc. */
+            CONTINUE_DEFER,
+                /** Nothing has been specifically handled for the rc. */
+            UNHANDLED_RC,
+        };
+
+        /** @brief Constructor.
+         *  @param[in] i_lock - Subsystem lock for this message handler, such
+         *                      as VmmManager::lock.
+         *  @param[in] i_msgq - Queue used to send messages into userspace.
+         *
+         *  @note The ownership of these parameter instances is maintained by
+         *        the caller and not cleaned up by this object upon destruction.
+         */
+        MessageHandler(Spinlock* i_lock, MessageQueue* i_msgq)
+            : iv_lock(i_lock), iv_msgq(i_msgq) {};
+
+        /** @brief Destructor.
+         *  No behavior required since ownership is maintained elsewhere.
+         */
+        virtual ~MessageHandler() {};
+
+        /** @brief 'Send message' interface.
+         *  Used to send a message into userspace.
+         *
+         *  @param[in] i_type - Message type (from sys/msg.h).
+         *  @param[in] i_key - Key (msg->data[0]) for the message.
+         *  @param[in] i_data - Data (msg->data[1]) for the message.
+         *  @param[in] i_task - Optional task being deferred due to this
+         *                      message.
+         *
+         *  The result of this message is that a message will be created and
+         *  inserted onto a user-space message queue, awaking the waiter if
+         *  blocked.  The task passed as a parameter, if not NULL, will be
+         *  deferred.
+         */
+        void sendMessage(msg_sys_types_t i_type, void* i_key,
+                         void* i_data, task_t* i_task);
+
+        /** @brief 'Handle response' interface.
+         *  Overrideable handling of the response to the 'send message'.
+         *
+         *  This is called once per previous 'send message' invocation for
+         *  the same key even if duplicate keys were combined into a single
+         *  user-space message.
+         *
+         *  This class, and specifically this method, should be inherited /
+         *  overridden if advanced handling is required for the response
+         *  above resuming the deferred task.
+         *
+         *  @param[in] i_type - The message type previously sent.
+         *  @param[in] i_key - The key value for the received message.
+         *  @param[in] i_task - The deferred task.
+         *  @param[in] i_rc - The response rc from userspace.
+         *
+         *  @return HandleResult - The desired behavior on the 'recv message'
+         *                         interface for this <key, task> pair.
+         */
+        virtual HandleResult handleResponse(msg_sys_types_t i_type, void* i_key,
+                                            task_t* i_task, int i_rc);
+
+    protected:
+        /** @brief 'Recv message' interface.
+         *  Called by the msg_respond sys-call handler to relay the response
+         *  from userspace.
+         *
+         *  This interface will call handleResponse for each of the deferred
+         *  <key, task> pairs and then, based on the return of handleResponse,
+         *  resume or kill the deferred task.
+         *
+         *  @param[in] i_msg - The message responded from user-space.
+         *
+         *  @return Return code for msg_respond.
+         */
+        int recvMessage(msg_t* i_msg);
+
+    private:
+            /** Pointer to the subsystem lock. */
+        Spinlock* const iv_lock;
+            /** Message queue to relay messages to. */
+        MessageQueue* const iv_msgq;
+            /** Queue of pending user-space responses. */
+        Util::Locked::List<MessageHandler_Pending,
+                           MessageHandler_Pending::key_type> iv_pending;
+
+            // Prevent copies.
+        MessageHandler(const MessageHandler&);
+        MessageHandler& operator=(const MessageHandler&);
+
+    public:
+            // Allow msg_respond handler to call recvMessage.
+        friend void Systemcalls::MsgRespond(task_t* t);
+};
+
+#endif
diff --git a/src/include/kernel/task.H b/src/include/kernel/task.H
index 6cd49028d..b72322d83 100644
--- a/src/include/kernel/task.H
+++ b/src/include/kernel/task.H
@@ -29,17 +29,17 @@ struct task_t
 {
         /** Pointer to the CPU this task is assigned to. */
     cpu_t* cpu;
-        /** Context information.  This MUST stay here due to 
+        /** Context information.  This MUST stay here due to
          *  save-restore asm code. */
     context_t context;
-        
+
         /** Task ID */
     tid_t tid;
         /** Determines if user-space would like this task pinned to a CPU.
          *  This value is considered a count of the number of times the pinned
          *  as been requested, so pinning can be used recursively. */
     uint64_t affinity_pinned;
-        
+
         // Pointers for queue containers.
     task_t* prev;
     task_t* next;
@@ -47,4 +47,17 @@ struct task_t
 
 enum { TASK_DEFAULT_STACK_SIZE = 4 };
 
+// Macros for manipulating task's saved contexts.
+#define TASK_GETARGN(t, n) (t->context.gprs[n+4])
+#define TASK_GETARG0(t) (TASK_GETARGN(t,0))
+#define TASK_GETARG1(t) (TASK_GETARGN(t,1))
+#define TASK_GETARG2(t) (TASK_GETARGN(t,2))
+#define TASK_GETARG3(t) (TASK_GETARGN(t,3))
+#define TASK_GETARG4(t) (TASK_GETARGN(t,4))
+#define TASK_GETARG5(t) (TASK_GETARGN(t,5))
+#define TASK_GETARG6(t) (TASK_GETARGN(t,6))
+#define TASK_GETARG7(t) (TASK_GETARGN(t,7))
+#define TASK_SETRTN(t, n) (t->context.gprs[3] = (n))
+
+
 #endif
diff --git a/src/include/kernel/vmmmgr.H b/src/include/kernel/vmmmgr.H
index 7c8c63c22..4fc112f09 100644
--- a/src/include/kernel/vmmmgr.H
+++ b/src/include/kernel/vmmmgr.H
@@ -84,6 +84,13 @@ class VmmManager
         VmmManager();
         ~VmmManager() {};
 
+        /** @brief Get spinlock for memory subsystem.
+         *  This is useful for passing to a deferred user-space message
+         *  handler so that the subsystem code is SMP-safe when the message
+         *  response is obtained.
+         */
+        static Spinlock* getLock();
+
     private:
         Spinlock lock;
 
@@ -92,6 +99,9 @@ class VmmManager
 
         bool _pteMiss(task_t*, uint64_t);
 
+    public:
+        friend class Block;
+
 };
 
 #endif
diff --git a/src/include/sys/msg.h b/src/include/sys/msg.h
index b2cbc1d31..8858acaa8 100644
--- a/src/include/sys/msg.h
+++ b/src/include/sys/msg.h
@@ -15,14 +15,73 @@ typedef void* msg_q_t;
 struct msg_t
 {
     uint32_t type;
-    uint32_t __reserved__async;
+    struct
+    {
+        uint32_t __reserved__async:1;
+        uint32_t __reserved__unused:31;
+    };
     uint64_t data[2];
     void* extra_data;
 };
 
-// Message queue interfaces.
+// System-defined message types.
+/** @enum msg_sys_types_t
+ *  @brief Message types potentially sent from the kernel itself.
+ */
+enum msg_sys_types_t
+{
+    MSG_FIRST_SYS_TYPE = 0x80000000,
+
+    MSG_MM_RP_READ,
+    MSG_MM_RP_WRITE,
+    MSG_MM_RP_PERM,
+};
+
+/** @var msg_sys_types_t::MSG_MM_RP_READ
+ *  @brief Memory Management - Resource Provider Read
+ *
+ *  Sent from the kernel to a msg_q_t registered with mm_block_create when
+ *  a page is requested to be read in from a resource.
+ *
+ *  <pre>
+ *  Format:
+ *      type = MSG_MM_RP_READ
+ *      data[0] = virtual address requested
+ *      data[1] = address to place contents
+ *
+ *  Expected Response:
+ *      type = MSG_MM_RP_READ
+ *      data[0] = virtual address requested
+ *      data[1] = rc (0 or negative errno value)
+ *  </pre>
+ */
+/** @var msg_sys_types_t::MSG_MM_RP_WRITE
+ *  @brief Memory Management - Resource Provider Write
+ *
+ *  Sent from the kernel to a msg_q_t registered with mm_block_create when
+ *  a page is requested to be written back to a resource.
+ *
+ *  <pre>
+ *  Format:
+ *      type = MSG_MM_RP_WRITE
+ *      data[0] = virtual address requested
+ *      data[1] = address to read contents from
+ *
+ *  Expected Response:
+ *      type = MSG_MM_RP_WRITE
+ *      data[0] = virtual address requested
+ *      data[1] = rc (0 or negative errno value)
+ *  </pre>
+ */
+/** @var msg_sys_types_t::MSG_MM_RP_PERM
+ *  @brief Memory Management - Resource Provider Permission Fault
+ *
+ *  TODO.
+ */
 
 
+// Message queue interfaces.
+
 /** @fn msg_q_create
   * @brief Create a new message queue.
   *
@@ -44,7 +103,7 @@ void msg_q_destroy( msg_q_t q );
   *
   * @param[in] q - handle of message queue to name
   * @param[in] name - name
-  * 
+  *
   * @return  Result of msg_sendrecv where zero indicates success
   */
 int msg_q_register(msg_q_t q, const char* name);
@@ -65,10 +124,10 @@ msg_q_t msg_q_resolve(const char* name);
 
 // Message interfaces.
 /** @fn msg_allocate
-  * @brief Allocate space for message 
+  * @brief Allocate space for message
   * @return Pointer to message
   */
-ALWAYS_INLINE 
+ALWAYS_INLINE
     inline msg_t* msg_allocate() { return (msg_t*)malloc(sizeof(msg_t)); }
 
 
@@ -81,11 +140,11 @@ ALWAYS_INLINE
     inline void msg_free(msg_t* m) { free(m); }
 
 
-/** @fn msg_send 
+/** @fn msg_send
   * @brief Send a message asynchronously.
   *
-  * This call adds the message queue then returns 
-  * to the caller. Any process waiting for a message 
+  * This call adds the message queue then returns
+  * to the caller. Any process waiting for a message
   * in the queue will awake with this message.
   *
   * @param[in] q - message queue
@@ -98,8 +157,8 @@ int msg_send(msg_q_t q, msg_t* msg);
 /** @fn msg_sendrecv
   * @brief Send a message to a server and get a response synchronously.
   *
-  * The calling [client] thread blocks until the recipient [server] receives 
-  * and replies to the message. 
+  * The calling [client] thread blocks until the recipient [server] receives
+  * and replies to the message.
   *
   * @param[in] q - message queue
   * @param[in,out] msg - message
@@ -112,8 +171,8 @@ int msg_sendrecv(msg_q_t q, msg_t* msg);
   * @brief Respond to a synchronous message.
   *
   * This is how server-side code responds to synchronous
-  * messaging when clients call msg_sendrecv().  
-  * 
+  * messaging when clients call msg_sendrecv().
+  *
   * @param[in] q - message queue
   * @param[in] msg - response message
   * @return Zero on success, else negative.
@@ -123,10 +182,10 @@ int msg_respond(msg_q_t q, msg_t* msg);
 
 /** @fn msg_wait
   * @brief  Read a message from the message queue.
-  * 
+  *
   * If a message is already on the queue, this call will return immediately
   * with the message. Otherwise the calling thread will block and wait for
-  * a message. 
+  * a message.
   *
   * @param[in] q - message queue to read
   * @return the message posted to the queue
@@ -142,7 +201,7 @@ msg_t* msg_wait(msg_q_t q);
   * @return true if asynchronous message
   */
 ALWAYS_INLINE
-    inline uint32_t msg_is_async(msg_t* msg) 
+    inline uint32_t msg_is_async(msg_t* msg)
 	{ return 0 == msg->__reserved__async; }
 
 #ifdef __cplusplus
diff --git a/src/kernel/makefile b/src/kernel/makefile
index c3af165c9..5ccbf8da9 100644
--- a/src/kernel/makefile
+++ b/src/kernel/makefile
@@ -3,7 +3,7 @@ ROOTPATH = ../..
 OBJS = start.o kernel.o console.o pagemgr.o heapmgr.o taskmgr.o cpumgr.o
 OBJS += syscall.o scheduler.o spinlock.o exception.o vmmmgr.o timemgr.o 
 OBJS += futexmgr.o ptmgr.o segmentmgr.o devicesegment.o basesegment.o
-OBJS += block.o cpuid.o misc.o
+OBJS += block.o cpuid.o misc.o msghandler.o
 
 include ${ROOTPATH}/config.mk
 
diff --git a/src/kernel/msghandler.C b/src/kernel/msghandler.C
new file mode 100644
index 000000000..f5f43fa1e
--- /dev/null
+++ b/src/kernel/msghandler.C
@@ -0,0 +1,141 @@
+#include <assert.h>
+#include <errno.h>
+#include <kernel/msghandler.H>
+#include <kernel/task.H>
+#include <kernel/cpu.H>
+#include <kernel/scheduler.H>
+#include <kernel/taskmgr.H>
+#include <kernel/console.H>
+
+namespace Systemcalls { void TaskEnd(task_t*); };
+
+void MessageHandler::sendMessage(msg_sys_types_t i_type, void* i_key,
+                                 void* i_data, task_t* i_task)
+{
+    // Save pending info for when we get the response.
+    MessageHandler_Pending* mp = new MessageHandler_Pending;
+    mp->key = i_key;
+    mp->task = i_task;
+
+    // Send userspace message if one hasn't been sent for this key.
+    if (!iv_pending.find(i_key))
+    {
+        // Create message.
+        msg_t* m = new msg_t;
+        m->type = i_type;
+        m->data[0] = reinterpret_cast<uint64_t>(i_key);
+        m->data[1] = reinterpret_cast<uint64_t>(i_data);
+        m->__reserved__async = 1;
+
+        // Create pending response object.
+        MessagePending* mp = new MessagePending();
+        mp->key = m;
+        mp->task = reinterpret_cast<task_t*>(this);
+
+        // Send to userspace...
+        iv_msgq->lock.lock();
+        task_t* waiter = iv_msgq->waiting.remove();
+        if (NULL == waiter) // No waiting task, queue for msg_wait call.
+        {
+            iv_msgq->messages.insert(mp);
+        }
+        else // Waiting task, set msg as return and release.
+        {
+            TASK_SETRTN(waiter, (uint64_t) m);
+            iv_msgq->responses.insert(mp);
+            waiter->cpu = i_task->cpu;
+            TaskManager::setCurrentTask(waiter);
+        }
+        iv_msgq->lock.unlock();
+    }
+
+    // Defer task while waiting for message response.
+    if ((NULL != i_task) && (TaskManager::getCurrentTask() == i_task))
+    {
+        i_task->cpu->scheduler->setNextRunnable();
+    }
+
+    // Insert pending info into our queue until response is recv'd.
+    iv_pending.insert(mp);
+}
+
+int MessageHandler::recvMessage(msg_t* i_msg)
+{
+    // Verify userspace didn't give a non-kernel message type.
+    if (i_msg->type < MSG_FIRST_SYS_TYPE)
+    {
+        return -EINVAL;
+    }
+
+    // Lock subsystem spinlock.
+    if (iv_lock) iv_lock->lock();
+
+    // Get <key, rc> from response.
+    MessageHandler_Pending::key_type key =
+        reinterpret_cast<MessageHandler_Pending::key_type>(i_msg->data[0]);
+    int msg_rc = static_cast<int>(i_msg->data[1]);
+
+    // Handle all pending responses.
+    bool restored_task = false;
+    MessageHandler_Pending* mp = NULL;
+    while (NULL != (mp = iv_pending.find(key)))
+    {
+        // Call 'handle response'.
+        HandleResult rc = this->handleResponse(
+                static_cast<msg_sys_types_t>(i_msg->type),
+                key, mp->task, msg_rc);
+
+        // Remove pending information from outstanding queue.
+        iv_pending.erase(mp);
+        delete mp;
+
+        // If there is no associated task then there is nothing to do, find
+        // next pending response.
+        if (!mp->task) continue;
+
+        // Handle action requested from 'handle response'.
+        if ((SUCCESS == rc) || (!msg_rc && UNHANDLED_RC == rc))
+        {
+            // Successful response, resume task.
+
+            if (!restored_task) // Immediately execute first deferred task.
+            {
+                restored_task = true;
+                TaskManager::setCurrentTask(mp->task);
+            }
+            else // Add other deferred tasks to scheduler ready queue.
+            {
+                mp->task->cpu->scheduler->addTask(mp->task);
+            }
+        }
+        else if (UNHANDLED_RC == rc)
+        {
+            // Unsuccessful, unhandled response.  Kill task.
+            printk("Unhandled msg rc %d for key %p on task %d @ %p\n",
+                   msg_rc, key, mp->task->tid, mp->task->context.nip);
+            Systemcalls::TaskEnd(mp->task);
+        }
+        else if (CONTINUE_DEFER == rc)
+        {
+            // Requested to continue deferring task.  Do nothing.
+        }
+        else
+        {
+            // Logic bug (new HandleResult?).  Shouldn't be here.
+            kassert(false);
+        }
+    }
+
+    // Finished handling the response, unlock subsystem.
+    if (iv_lock) iv_lock->unlock();
+
+    return 0;
+}
+
+MessageHandler::HandleResult MessageHandler::handleResponse(
+        msg_sys_types_t i_type, void* i_key, task_t* i_task, int i_rc)
+{
+    // Indicate nothing specific has been done for this response.  Request
+    // default behavior of resume/kill task based on rc.
+    return UNHANDLED_RC;
+}
diff --git a/src/kernel/syscall.C b/src/kernel/syscall.C
index 51009195d..a71217bec 100644
--- a/src/kernel/syscall.C
+++ b/src/kernel/syscall.C
@@ -1,4 +1,5 @@
 #include <assert.h>
+#include <errno.h>
 #include <kernel/cpu.H>
 #include <kernel/cpumgr.H>
 #include <kernel/scheduler.H>
@@ -12,6 +13,7 @@
 #include <kernel/futexmgr.H>
 #include <kernel/cpuid.H>
 #include <kernel/misc.H>
+#include <kernel/msghandler.H>
 
 extern "C"
 void kernel_execute_decrementer()
@@ -110,18 +112,6 @@ void kernel_execute_system_call()
     }
 }
 
-#define TASK_GETARGN(t, n) (t->context.gprs[n+4])
-#define TASK_GETARG0(t) (TASK_GETARGN(t,0))
-#define TASK_GETARG1(t) (TASK_GETARGN(t,1))
-#define TASK_GETARG2(t) (TASK_GETARGN(t,2))
-#define TASK_GETARG3(t) (TASK_GETARGN(t,3))
-#define TASK_GETARG4(t) (TASK_GETARGN(t,4))
-#define TASK_GETARG5(t) (TASK_GETARGN(t,5))
-#define TASK_GETARG6(t) (TASK_GETARGN(t,6))
-#define TASK_GETARG7(t) (TASK_GETARGN(t,7))
-
-#define TASK_SETRTN(t, n) (t->context.gprs[3] = (n))
-
 namespace Systemcalls
 {
     void TaskYield(task_t* t)
@@ -186,6 +176,12 @@ namespace Systemcalls
 	msg_t* m = (msg_t*) TASK_GETARG1(t);
 	m->__reserved__async = 0; // set to async msg.
 
+        if (m->type >= MSG_FIRST_SYS_TYPE)
+        {
+            TASK_SETRTN(t, -EINVAL);
+            return;
+        }
+
 	mq->lock.lock();
 
 	// Get waiting (server) task.
@@ -213,11 +209,19 @@ namespace Systemcalls
 	msg_t* m = (msg_t*) TASK_GETARG1(t);
 	m->__reserved__async = 1; // set to sync msg.
 
-	mq->lock.lock();
+        if (m->type >= MSG_FIRST_SYS_TYPE)
+        {
+            TASK_SETRTN(t, -EINVAL);
+            return;
+        }
+
+        // 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.
@@ -249,20 +253,34 @@ namespace Systemcalls
 	    task_t* waiter = mp->task;
 
 	    mq->responses.erase(mp);
+            mq->lock.unlock();
 	    delete mp;
 
-	    waiter->cpu = t->cpu;
-	    TaskManager::setCurrentTask(waiter);
-            TASK_SETRTN(waiter,0);
-
-	    TASK_SETRTN(t,0);
-	    t->cpu->scheduler->addTask(t);
+            if (m->type >= MSG_FIRST_SYS_TYPE)
+            {
+                TASK_SETRTN(t,
+                            ((MessageHandler*)waiter)->recvMessage(m));
+
+                if (TaskManager::getCurrentTask() != t)
+                {
+                    t->cpu->scheduler->addTask(t);
+                }
+            }
+            else
+            {
+                waiter->cpu = t->cpu;
+                TaskManager::setCurrentTask(waiter);
+                TASK_SETRTN(waiter,0);
+
+                TASK_SETRTN(t,0);
+                t->cpu->scheduler->addTask(t);
+            }
 	}
 	else
 	{
 	    TASK_SETRTN(t, -1);
+            mq->lock.unlock();
 	}
-	mq->lock.unlock();
     }
 
     void MsgWait(task_t* t)
diff --git a/src/kernel/vmmmgr.C b/src/kernel/vmmmgr.C
index 46fca9fef..74c3178d0 100644
--- a/src/kernel/vmmmgr.C
+++ b/src/kernel/vmmmgr.C
@@ -111,3 +111,8 @@ int VmmManager::mmAllocBlock(MessageQueue* i_mq,void* i_va,uint64_t i_size)
     return BaseSegment::mmAllocBlock(i_mq,i_va,i_size);
 }
 
+Spinlock* VmmManager::getLock()
+{
+    return &Singleton<VmmManager>::instance().lock;
+}
+
diff --git a/src/makefile b/src/makefile
index 68dcc4f82..afe0e5339 100644
--- a/src/makefile
+++ b/src/makefile
@@ -30,7 +30,7 @@ DIRECT_BOOT_OBJECTS = start.o kernel.o taskmgr.o cpumgr.o syscall.o \
                       syscall_msg.o syscall_mmio.o syscall_time.o \
                       syscall_mm.o init_main.o vfs_main.o sync.o futexmgr.o \
                       ptmgr.o segmentmgr.o basesegment.o devicesegment.o \
-		      block.o cxxtest_data.o cpuid.o misc.o
+		      block.o cxxtest_data.o cpuid.o misc.o msghandler.o
 
 ## STUB_TESTCASE_OBJECT = cxxtest_stub.o