diff --git a/src/mempool.c b/src/mempool.c
index 05904bfbc742427c0873420d2fc2221c199b52a7..b6d83597bf7ad77322ab88470984f1ca671c0bb4 100644
--- a/src/mempool.c
+++ b/src/mempool.c
@@ -323,23 +323,15 @@ void *rt_mp_alloc(rt_mp_t mp, rt_int32_t time)
     rt_uint8_t *block_ptr;
     register rt_base_t level;
     struct rt_thread *thread;
+    rt_uint32_t before_sleep = 0;
+
+    /* get current thread */
+    thread = rt_thread_self();
 
     /* disable interrupt */
     level = rt_hw_interrupt_disable();
 
-    if (mp->block_free_count)
-    {
-        /* memory block is available. decrease the free block counter */
-        mp->block_free_count --;
-
-        /* get block from block list */
-        block_ptr      = mp->block_list;
-        mp->block_list = *(rt_uint8_t **)block_ptr;
-
-        /* point to memory pool */
-        *(rt_uint8_t **)block_ptr = (rt_uint8_t *)mp;
-    }
-    else
+    while (mp->block_free_count == 0)
     {
         /* memory block is unavailable. */
         if (time == 0)
@@ -347,54 +339,63 @@ void *rt_mp_alloc(rt_mp_t mp, rt_int32_t time)
             /* enable interrupt */
             rt_hw_interrupt_enable(level);
 
+            rt_set_errno(-RT_ETIMEOUT);
+
             return RT_NULL;
         }
-        else
-        {
-            RT_DEBUG_NOT_IN_INTERRUPT;
 
-            /* get current thread */
-            thread = rt_thread_self();
+        RT_DEBUG_NOT_IN_INTERRUPT;
 
-            thread->error = RT_EOK;
+        thread->error = RT_EOK;
 
-            /* need suspend thread */
-            rt_thread_suspend(thread);
-            rt_list_insert_after(&(mp->suspend_thread), &(thread->tlist));
-            mp->suspend_thread_count ++;
+        /* need suspend thread */
+        rt_thread_suspend(thread);
+        rt_list_insert_after(&(mp->suspend_thread), &(thread->tlist));
+        mp->suspend_thread_count++;
 
-            if (time > 0)
-            {
-                /* init thread timer and start it */
-                rt_timer_control(&(thread->thread_timer),
-                                 RT_TIMER_CTRL_SET_TIME,
-                                 &time);
-                rt_timer_start(&(thread->thread_timer));
-            }
+        if (time > 0)
+        {
+            /* get the start tick of timer */
+            before_sleep = rt_tick_get();
+
+            /* init thread timer and start it */
+            rt_timer_control(&(thread->thread_timer),
+                             RT_TIMER_CTRL_SET_TIME,
+                             &time);
+            rt_timer_start(&(thread->thread_timer));
+        }
 
-            /* enable interrupt */
-            rt_hw_interrupt_enable(level);
+        /* enable interrupt */
+        rt_hw_interrupt_enable(level);
+
+        /* do a schedule */
+        rt_schedule();
 
-            /* do a schedule */
-            rt_schedule();
+        if (thread->error != RT_EOK)
+            return RT_NULL;
 
-            if (thread->error != RT_EOK)
-                return RT_NULL;
+        if (time > 0)
+        {
+            time -= rt_tick_get() - before_sleep;
+            if (time < 0)
+                time = 0;
+        }
+        /* disable interrupt */
+        level = rt_hw_interrupt_disable();
+    }
 
-            /* disable interrupt */
-            level = rt_hw_interrupt_disable();
+    /* memory block is available. decrease the free block counter */
+    mp->block_free_count--;
 
-            /* decrease free block */
-            mp->block_free_count --;
+    /* get block from block list */
+    block_ptr = mp->block_list;
+    RT_ASSERT(block_ptr != RT_NULL);
 
-            /* get block from block list */
-            block_ptr      = mp->block_list;
-            mp->block_list = *(rt_uint8_t **)block_ptr;
+    /* Setup the next free node. */
+    mp->block_list = *(rt_uint8_t **)block_ptr;
 
-            /* point to memory pool */
-            *(rt_uint8_t **)block_ptr = (rt_uint8_t *)mp;
-        }
-    }
+    /* point to memory pool */
+    *(rt_uint8_t **)block_ptr = (rt_uint8_t *)mp;
 
     /* enable interrupt */
     rt_hw_interrupt_enable(level);