PriorityQueue and MessagePool::Iterator and their unit-test (#1096)

This commit contains the following:

- It adds methods to `Message` to assign a priority level to messages.
  Currently there are four priority levels: High, Medium, Low, VeryLow
  but this is easily changeable.

- It introduces a new class `PriorityQueue` which implements a FIFO queue
  for storing messages based on their priority level in addition to oder
  in which they are added.
  If the priority level of an already queued message in a `PriorityQueue`
  is changed, the associated queue will automatically move the message
  within the queue based on its new priority level.
  Internally the implementation uses a circular doubly linked-list with
  an array of tail pointers associated with different priority levels.

- It adds new methods and an `Iterator` class  in `MessagePool` to access
  the priority queue where all allocated and queued messages from this
  pool are stored.  The `Iterator` class allows bi-directional iteration
  through the list (from highest priority to lowest or reverse).

- This commit also contain detailed unit-test for all the newly added
  features/methods.

etc/visual-studio/UnitTests.vcxproj

@@ -72,6 +72,7 @@
     <ClCompile Include="..\..\tests\unit\test_message_queue.cpp" />
     <ClCompile Include="..\..\tests\unit\test_ncp_buffer.cpp" />
     <ClCompile Include="..\..\tests\unit\test_platform.cpp" />
+    <ClCompile Include="..\..\tests\unit\test_priority_queue.cpp" />
     <ClCompile Include="..\..\tests\unit\test_timer.cpp" />
     <ClCompile Include="..\..\tests\unit\test_toolchain_c.c" />
     <ClCompile Include="..\..\tests\unit\test_toolchain.cpp" />

etc/visual-studio/UnitTests.vcxproj.filters

@@ -39,6 +39,9 @@
     <ClCompile Include="..\..\tests\unit\test_message_queue.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="..\..\tests\unit\test_priority_queue.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
     <ClCompile Include="..\..\tests\unit\test_timer.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>

src/core/common/message.cpp

@@ -41,10 +41,10 @@
 
 namespace Thread {
 
-MessagePool::MessagePool(void)
+MessagePool::MessagePool(void) :
+    mAllQueue()
 {
     memset(mBuffers, 0, sizeof(mBuffers));
-    mAllListTail = NULL;
 
     mFreeBuffers = mBuffers;
 
@@ -74,6 +74,7 @@ Message *MessagePool::New(uint8_t aType, uint16_t aReserved)
     message->SetType(aType);
     message->SetReserved(aReserved);
     message->SetLinkSecurityEnabled(true);
+    message->SetPriority(kDefaultMessagePriority);
 
     if (message->SetLength(0) != kThreadError_None)
     {
@@ -96,12 +97,6 @@ ThreadError MessagePool::Free(Message *aMessage)
     return FreeBuffers(static_cast<Buffer *>(aMessage));
 }
 
-Message *MessagePool::GetAllMessagesListHead(void) const
-{
-    return (mAllListTail == NULL) ? NULL : mAllListTail->Next(MessageInfo::kListAll);
-}
-
-
 Buffer *MessagePool::NewBuffer(void)
 {
     Buffer *buffer = NULL;
@@ -157,6 +152,63 @@ ThreadError MessagePool::ReclaimBuffers(int aNumBuffers)
     return (aNumBuffers <= mNumFreeBuffers) ? kThreadError_None : kThreadError_NoBufs;
 }
 
+Message *MessagePool::Iterator::Next(void) const
+{
+    Message *next;
+
+    VerifyOrExit(mMessage != NULL, next = NULL);
+
+    if (mMessage == mMessage->GetMessagePool()->GetAllMessagesTail().GetMessage())
+    {
+        next = NULL;
+    }
+    else
+    {
+        next = mMessage->Next(MessageInfo::kListAll);
+    }
+
+exit:
+    return next;
+}
+
+Message *MessagePool::Iterator::Prev(void) const
+{
+    Message *prev;
+
+    VerifyOrExit(mMessage != NULL, prev = NULL);
+
+    if (mMessage == mMessage->GetMessagePool()->GetAllMessagesHead().GetMessage())
+    {
+        prev = NULL;
+    }
+    else
+    {
+        prev = mMessage->Prev(MessageInfo::kListAll);
+    }
+
+exit:
+    return prev;
+}
+
+MessagePool::Iterator MessagePool::GetAllMessagesHead(void) const
+{
+    Message *head;
+    Message *tail;
+
+    tail = GetAllMessagesTail().GetMessage();
+
+    if (tail != NULL)
+    {
+        head = tail->Next(MessageInfo::kListAll);
+    }
+    else
+    {
+        head = NULL;
+    }
+
+    return Iterator(head);
+}
+
 ThreadError Message::ResizeMessage(uint16_t aLength)
 {
     ThreadError error = kThreadError_None;
@@ -196,9 +248,26 @@ ThreadError Message::Free(void)
 
 Message *Message::GetNext(void) const
 {
-    assert(GetMessageQueue() != NULL);
+    Message *next;
+    Message *tail;
 
-    return (this == GetMessageQueue()->mTail) ? NULL : Next(MessageInfo::kListInterface);
+    if (mInfo.mInPriorityQ)
+    {
+        PriorityQueue *priorityQueue = GetPriorityQueue();
+        VerifyOrExit(priorityQueue != NULL, next = NULL);
+        tail = priorityQueue->GetTail();
+    }
+    else
+    {
+        MessageQueue *messageQueue = GetMessageQueue();
+        VerifyOrExit(messageQueue != NULL, next = NULL);
+        tail = messageQueue->GetTail();
+    }
+
+    next = (this == tail) ? NULL : Next(MessageInfo::kListInterface);
+
+exit:
+    return next;
 }
 
 uint16_t Message::GetLength(void) const
@@ -296,6 +365,46 @@ void Message::SetSubType(uint8_t aSubType)
     mInfo.mSubType = aSubType;
 }
 
+uint8_t Message::GetPriority(void) const
+{
+    return mInfo.mPriority;
+}
+
+ThreadError Message::SetPriority(uint8_t aPriority)
+{
+    ThreadError error = kThreadError_None;
+    PriorityQueue *priorityQueue = NULL;
+
+    VerifyOrExit(aPriority < kNumPriorities, error = kThreadError_InvalidArgs);
+
+    VerifyOrExit(IsInAQueue(), mInfo.mPriority = aPriority);
+    VerifyOrExit(mInfo.mPriority != aPriority, ;);
+
+    if (mInfo.mInPriorityQ)
+    {
+        priorityQueue = mInfo.mPriorityQueue;
+        priorityQueue->Dequeue(*this);
+    }
+    else
+    {
+        GetMessagePool()->GetAllMessagesQueue()->RemoveFromList(MessageInfo::kListAll, *this);
+    }
+
+    mInfo.mPriority = aPriority;
+
+    if (priorityQueue != NULL)
+    {
+        priorityQueue->Enqueue(*this);
+    }
+    else
+    {
+        GetMessagePool()->GetAllMessagesQueue()->AddToList(MessageInfo::kListAll, *this);
+    }
+
+exit:
+    return error;
+}
+
 ThreadError Message::Append(const void *aBuf, uint16_t aLength)
 {
     ThreadError error = kThreadError_None;
@@ -752,47 +861,59 @@ void Message::SetReserved(uint16_t aReserved)
     mInfo.mReserved = aReserved;
 }
 
+void Message::SetMessageQueue(MessageQueue *aMessageQueue)
+{
+    mInfo.mMessageQueue = aMessageQueue;
+    mInfo.mInPriorityQ = false;
+}
+
+void Message::SetPriorityQueue(PriorityQueue *aPriorityQueue)
+{
+    mInfo.mPriorityQueue = aPriorityQueue;
+    mInfo.mInPriorityQ = true;
+}
+
 MessageQueue::MessageQueue(void) :
     mTail(NULL)
 {
 }
 
-void MessageQueue::AddToList(Message *&aListTail, uint8_t aList, Message &aMessage)
+void MessageQueue::AddToList(uint8_t aList, Message &aMessage)
 {
     Message *head;
 
     assert((aMessage.Next(aList) == NULL) && (aMessage.Prev(aList) == NULL));
 
-    if (aListTail == NULL)
+    if (mTail == NULL)
     {
         aMessage.Next(aList) = &aMessage;
         aMessage.Prev(aList) = &aMessage;
     }
     else
     {
-        head = aListTail->Next(aList);
+        head = mTail->Next(aList);
 
         aMessage.Next(aList) = head;
-        aMessage.Prev(aList) = aListTail;
+        aMessage.Prev(aList) = mTail;
 
         head->Prev(aList) = &aMessage;
-        aListTail->Next(aList) = &aMessage;
+        mTail->Next(aList) = &aMessage;
     }
 
-    aListTail = &aMessage;
+    mTail = &aMessage;
 }
 
-void MessageQueue::RemoveFromList(Message *&aListTail, uint8_t aList, Message &aMessage)
+void MessageQueue::RemoveFromList(uint8_t aList, Message &aMessage)
 {
     assert((aMessage.Next(aList) != NULL) && (aMessage.Prev(aList) != NULL));
 
-    if (&aMessage == aListTail)
+    if (&aMessage == mTail)
     {
-        aListTail = aListTail->Prev(aList);
+        mTail = mTail->Prev(aList);
 
-        if (&aMessage == aListTail)
+        if (&aMessage == mTail)
         {
-            aListTail = NULL;
+            mTail = NULL;
         }
     }
 
@@ -812,12 +933,12 @@ ThreadError MessageQueue::Enqueue(Message &aMessage)
 {
     ThreadError error = kThreadError_None;
 
-    VerifyOrExit(aMessage.GetMessageQueue() == NULL, error = kThreadError_Already);
+    VerifyOrExit(!aMessage.IsInAQueue(), error = kThreadError_Already);
 
     aMessage.SetMessageQueue(this);
 
-    AddToList(aMessage.GetMessagePool()->mAllListTail, MessageInfo::kListAll, aMessage);
-    AddToList(mTail, MessageInfo::kListInterface, aMessage);
+    AddToList(MessageInfo::kListInterface, aMessage);
+    aMessage.GetMessagePool()->GetAllMessagesQueue()->AddToList(MessageInfo::kListAll, aMessage);
 
 exit:
     return error;
@@ -829,8 +950,8 @@ ThreadError MessageQueue::Dequeue(Message &aMessage)
 
     VerifyOrExit(aMessage.GetMessageQueue() == this, error = kThreadError_NotFound);
 
-    RemoveFromList(aMessage.GetMessagePool()->mAllListTail, MessageInfo::kListAll, aMessage);
-    RemoveFromList(mTail, MessageInfo::kListInterface, aMessage);
+    RemoveFromList(MessageInfo::kListInterface, aMessage);
+    aMessage.GetMessagePool()->GetAllMessagesQueue()->RemoveFromList(MessageInfo::kListAll, aMessage);
 
     aMessage.SetMessageQueue(NULL);
 
@@ -850,4 +971,166 @@ void MessageQueue::GetInfo(uint16_t &aMessageCount, uint16_t &aBufferCount) cons
     }
 }
 
+PriorityQueue::PriorityQueue(void)
+{
+    for (int priority = 0; priority < Message::kNumPriorities; priority++)
+    {
+        mTails[priority] = NULL;
+    }
+}
+
+Message *PriorityQueue::FindFirstNonNullTail(uint8_t aStartPriorityLevel) const
+{
+    Message *tail = NULL;
+    uint8_t priority;
+
+    priority = aStartPriorityLevel;
+
+    do
+    {
+        if (mTails[priority] != NULL)
+        {
+            tail = mTails[priority];
+            break;
+        }
+
+        priority = PrevPriority(priority);
+    }
+    while (priority != aStartPriorityLevel);
+
+    return tail;
+}
+
+Message *PriorityQueue::GetHead(void) const
+{
+    Message *tail;
+
+    tail = FindFirstNonNullTail(Message::kNumPriorities - 1);
+
+    return (tail == NULL) ? NULL : tail->Next(MessageInfo::kListInterface);
+}
+
+Message *PriorityQueue::GetHeadForPriority(uint8_t aPriority) const
+{
+    Message *head;
+    Message *previousTail;
+
+    if (mTails[aPriority] != NULL)
+    {
+        previousTail = FindFirstNonNullTail(PrevPriority(aPriority));
+
+        assert(previousTail != NULL);
+
+        head = previousTail->Next(MessageInfo::kListInterface);
+    }
+    else
+    {
+        head = NULL;
+    }
+
+    return head;
+}
+
+Message *PriorityQueue::GetTail(void) const
+{
+    return FindFirstNonNullTail(Message::kNumPriorities - 1);
+}
+
+void PriorityQueue::AddToList(uint8_t aList, Message &aMessage)
+{
+    uint8_t priority;
+    Message *tail;
+    Message *next;
+
+    priority = aMessage.GetPriority();
+
+    tail = FindFirstNonNullTail(priority);
+
+    if (tail != NULL)
+    {
+        next = tail->Next(aList);
+
+        aMessage.Next(aList) = next;
+        aMessage.Prev(aList) = tail;
+        next->Prev(aList) = &aMessage;
+        tail->Next(aList) = &aMessage;
+    }
+    else
+    {
+        aMessage.Next(aList) = &aMessage;
+        aMessage.Prev(aList) = &aMessage;
+    }
+
+    mTails[priority] = &aMessage;
+}
+
+void PriorityQueue::RemoveFromList(uint8_t aList, Message &aMessage)
+{
+    uint8_t priority;
+    Message *tail;
+
+    priority = aMessage.GetPriority();
+
+    tail = mTails[priority];
+
+    if (&aMessage == tail)
+    {
+        tail = tail->Prev(aList);
+
+        if ((&aMessage == tail) || (tail->GetPriority() != priority))
+        {
+            tail = NULL;
+        }
+
+        mTails[priority] = tail;
+    }
+
+    aMessage.Next(aList)->Prev(aList) = aMessage.Prev(aList);
+    aMessage.Prev(aList)->Next(aList) = aMessage.Next(aList);
+    aMessage.Next(aList) = NULL;
+    aMessage.Prev(aList) = NULL;
+}
+
+ThreadError PriorityQueue::Enqueue(Message &aMessage)
+{
+    ThreadError error = kThreadError_None;
+
+    VerifyOrExit(!aMessage.IsInAQueue(), error = kThreadError_Already);
+
+    aMessage.SetPriorityQueue(this);
+
+    AddToList(MessageInfo::kListInterface, aMessage);
+    aMessage.GetMessagePool()->GetAllMessagesQueue()->AddToList(MessageInfo::kListAll, aMessage);
+
+exit:
+    return error;
+}
+
+ThreadError PriorityQueue::Dequeue(Message &aMessage)
+{
+    ThreadError error = kThreadError_None;
+
+    VerifyOrExit(aMessage.GetPriorityQueue() == this, error = kThreadError_NotFound);
+
+    RemoveFromList(MessageInfo::kListInterface, aMessage);
+    aMessage.GetMessagePool()->GetAllMessagesQueue()->RemoveFromList(MessageInfo::kListAll, aMessage);
+
+    aMessage.SetMessageQueue(NULL);
+
+exit:
+    return error;
+}
+
+void PriorityQueue::GetInfo(uint16_t &aMessageCount, uint16_t &aBufferCount) const
+{
+    aMessageCount = 0;
+    aBufferCount = 0;
+
+    for (const Message *message = GetHead(); message != NULL; message = message->GetNext())
+    {
+        aMessageCount++;
+        aBufferCount += message->GetBufferCount();
+    }
+}
+
 }  // namespace Thread

src/core/common/message.hpp

@@ -70,6 +70,7 @@ enum
 class Message;
 class MessagePool;
 class MessageQueue;
+class PriorityQueue;
 
 /**
  * This structure contains metdata about a Message.
@@ -87,7 +88,11 @@ struct MessageInfo
     Message         *mNext[kNumLists];   ///< A pointer to the next Message in a doubly linked list.
     Message         *mPrev[kNumLists];   ///< A pointer to the previous Message in a doubly linked list.
     MessagePool     *mMessagePool;       ///< Identifies the message pool for this message.
-    MessageQueue    *mMessageQueue;      ///< Identifies the message queue (if any) where this message is queued.
+    union
+    {
+        MessageQueue    *mMessageQueue;  ///< Identifies the message queue (if any) where this message is queued.
+        PriorityQueue   *mPriorityQueue; ///< Identifies the priority queue (if any) where this message is queued.
+    };
 
     uint16_t         mReserved;          ///< Number of header bytes reserved for the message.
     uint16_t         mLength;            ///< Number of bytes within the message.
@@ -107,6 +112,8 @@ struct MessageInfo
     uint8_t          mSubType : 3;       ///< Identifies the message sub type.
     bool             mDirectTx : 1;      ///< Used to indicate whether a direct transmission is required.
     bool             mLinkSecurity : 1;  ///< Indicates whether or not link security is enabled.
+    uint8_t          mPriority: 2;       ///< Identifies the message priority level (lower value is higher priority).
+    bool             mInPriorityQ : 1;   ///< Indicates whether the message is queued in normal or priority queue.
 };
 
 /**
@@ -190,6 +197,7 @@ class Message: private Buffer
 {
     friend class MessagePool;
     friend class MessageQueue;
+    friend class PriorityQueue;
 
 public:
     enum
@@ -208,6 +216,16 @@ public:
         kSubTypeJoinerEntrust       = 4,  ///< Joiner Entrust
     };
 
+    enum
+    {
+        kPriorityHigh       = 0,    ///< High priority level.
+        kPriorityMedium     = 1,    ///< Medium priority level.
+        kPriorityLow        = 2,    ///< Low priority level.
+        kPriorityVeryLow    = 3,    ///< Very low priority level.
+
+        kNumPriorities      = 4,    ///< Number of priority levels.
+    };
+
     /**
      * This method frees this message buffer.
      *
@@ -217,7 +235,7 @@ public:
     /**
      * This method returns a pointer to the next message in the same interface list.
      *
-     * @returns A pointer to the next message in the same interface list.
+     * @returns A pointer to the next message in the same interface list or NULL if at the end of the list.
      *
      */
     Message *GetNext(void) const;
@@ -301,6 +319,27 @@ public:
      */
     void SetSubType(uint8_t aSubType);
 
+    /**
+     * This method returns the message priority level.
+     *
+     * @returns The priority level associated with this message.
+     *
+     */
+    uint8_t GetPriority(void) const;
+
+    /**
+     * This method sets the messages priority.
+     * If the message is already queued in a priority queue, changing the priority ensures to
+     * update the message in the associated queue.
+     *
+     * @param[in]  aPrority  The message priority level.
+     *
+     * @retval kThreadError_None          Successfully set the priority for the message.
+     * @retval kThreadError_InvalidArgs   Priority level is not invalid.
+     *
+     */
+    ThreadError SetPriority(uint8_t aPriority);
+
     /**
      * This method prepends bytes to the front of the message.
      *
@@ -587,11 +626,20 @@ private:
     void SetMessagePool(MessagePool *aMessagePool) { mInfo.mMessagePool = aMessagePool; }
 
     /**
-     * This method returns a pointer to the message queue (if any) where this message is queued.
+     * This method returns `true` if the message is enqueued in any queue (`MessageQueue` or `PriorityQueue`).
      *
+     * @returns `true` if the message is in any queue, `false` otherwise.
      *
      */
-    MessageQueue *GetMessageQueue(void) const { return mInfo.mMessageQueue; }
+    bool IsInAQueue(void) const { return (mInfo.mMessageQueue != NULL); }
+
+    /**
+     * This method returns a pointer to the message queue (if any) where this message is queued.
+     *
+     * @returns A pointer to the message queue or NULL if not in any message queue.
+     *
+     */
+    MessageQueue *GetMessageQueue(void) const { return (!mInfo.mInPriorityQ) ? mInfo.mMessageQueue : NULL; }
 
     /**
      * This method sets the message queue information for the message.
@@ -599,7 +647,23 @@ private:
      * @param[in]  aMessageQueue  A pointer to the message queue where this message is queued.
      *
      */
-    void SetMessageQueue(MessageQueue *aMessageQueue) { mInfo.mMessageQueue = aMessageQueue; }
+    void SetMessageQueue(MessageQueue *aMessageQueue);
+
+    /**
+     * This method returns a pointer to the priority message queue (if any) where this message is queued.
+     *
+     * @returns A pointer to the priority queue or NULL if not in any priority queue.
+     *
+     */
+    PriorityQueue *GetPriorityQueue(void) const { return (mInfo.mInPriorityQ) ? mInfo.mPriorityQueue : NULL; }
+
+    /**
+     * This method sets the message queue information for the message.
+     *
+     * @param[in]  aPriorityQueue  A pointer to the priority queue where this message is queued.
+     *
+     */
+    void SetPriorityQueue(PriorityQueue *aPriorityQueue);
 
     /**
      * This method returns a reference to the `mNext` pointer for a given list.
@@ -674,6 +738,7 @@ private:
 class MessageQueue
 {
     friend class Message;
+    friend class PriorityQueue;
 
 public:
     /**
@@ -724,34 +789,245 @@ public:
 private:
 
     /**
-     * This static method adds a message to a list.
+     * This method returns the tail of the list (last message in the list)
      *
-     * @param[in]  aListTail A reference to the list tail pointer.
-     * @param[in]  aListId   The list to add @p aMessage to.
-     * @param[in]  aMessage  The message to add to @p aListId.
+     * @returns A pointer to the tail of the list.
      *
      */
-    static void AddToList(Message *&aListTail, uint8_t aListId, Message &aMessage);
+    Message *GetTail(void) const { return mTail; }
 
     /**
-     * This static method removes a message from a list.
+     * This method adds a message to a list.
      *
-     * @param[in]  aListTail A reference to the list tale pointer.
      * @param[in]  aListId   The list to add @p aMessage to.
      * @param[in]  aMessage  The message to add to @p aListId.
      *
      */
-    static void RemoveFromList(Message *&aListTail, uint8_t aListId, Message &aMessage);
+    void AddToList(uint8_t aListId, Message &aMessage);
+
+    /**
+     * This method removes a message from a list.
+     *
+     * @param[in]  aListId   The list to add @p aMessage to.
+     * @param[in]  aMessage  The message to add to @p aListId.
+     *
+     */
+    void RemoveFromList(uint8_t aListId, Message &aMessage);
 
     Message *mTail;   ///< A pointer to the last Message in the list.
 };
 
+/**
+ * This class implements a priority queue.
+ *
+ */
+class PriorityQueue
+{
+    friend class Message;
+    friend class MessageQueue;
+    friend class MessagePool;
+
+public:
+    /**
+     * This constructor initializes the priority queue.
+     *
+     */
+    PriorityQueue(void);
+
+    /**
+     * This method returns a pointer to the first message.
+     *
+     * @returns A pointer to the first message.
+     *
+     */
+    Message *GetHead(void) const;
+
+    /**
+      * This method returns a pointer to the first message for a given priority level.
+      *
+      * @param[in] aPriority   Priority level.
+      *
+      * @returns A pointer to the first message with given priority level or NULL if there is no messages with
+      *          this priority level.
+      *
+      */
+    Message *GetHeadForPriority(uint8_t aPriority) const;
+
+    /**
+     * This method adds a message to the queue.
+     *
+     * @param[in]  aMessage  The message to add.
+     *
+     * @retval kThreadError_None     Successfully added the message to the list.
+     * @retval kThreadError_Already  The message is already enqueued in a list.
+     *
+     */
+    ThreadError Enqueue(Message &aMessage);
+
+    /**
+     * This method removes a message from the list.
+     *
+     * @param[in]  aMessage  The message to remove.
+     *
+     * @retval kThreadError_None      Successfully removed the message from the list.
+     * @retval kThreadError_NotFound  The message is not enqueued in a list.
+     *
+     */
+    ThreadError Dequeue(Message &aMessage);
+
+    /**
+     * This method returns the number of messages and buffers enqueued.
+     *
+     * @param[out]  aMessageCount  Returns the number of messages enqueued.
+     * @param[out]  aBufferCount   Returns the number of buffers enqueued.
+     *
+     */
+    void GetInfo(uint16_t &aMessageCount, uint16_t &aBufferCount) const;
+
+private:
+
+    /**
+     * This method returns the tail of the list (last message in the list)
+     *
+     * @returns A pointer to the tail of the list.
+     *
+     */
+    Message *GetTail(void) const;
+
+    /**
+     * This method adds a message to a list.
+     *
+     * @param[in]  aListId   The list to add @p aMessage to.
+     * @param[in]  aMessage  The message to add to @p aListId.
+     *
+     */
+    void AddToList(uint8_t aListId, Message &aMessage);
+
+    /**
+     * This method removes a message from a list.
+     *
+     * @param[in]  aListId   The list to add @p aMessage to.
+     * @param[in]  aMessage  The message to add to @p aListId.
+     *
+     */
+    void RemoveFromList(uint8_t aListId, Message &aMessage);
+
+    /**
+     * This method decreases (moves back) the given priority while ensuring to wrap from
+     * priority value 0 back to `kNumPriorities` -1.
+     *
+     * @param[in] aPriority  A given priority level
+     *
+     * @returns Decreased/Moved back priority level
+     */
+    uint8_t PrevPriority(uint8_t aPriority) const {
+        return (aPriority == 0) ? (Message::kNumPriorities - 1) : (aPriority - 1);
+    }
+
+    /**
+     * This private method finds the first non-NULL tail starting from the given priority level and moving back.
+     * It wraps from priority value 0 back to `kNumPriorities` -1.
+     *
+     * aStartPriorityLevel  Starting priority level.
+     *
+     * @returns The first non-NULL tail pointer, or NULL if all the
+     *
+     */
+    Message *FindFirstNonNullTail(uint8_t aStartPriorityLevel) const;
+
+private:
+
+    Message *mTails[Message::kNumPriorities];   ///< Tail pointers associated with different priority levels.
+};
+
+/**
+ * This class represents a message pool
+ *
+ */
 class MessagePool
 {
     friend class Message;
     friend class MessageQueue;
+    friend class PriorityQueue;
 
 public:
+
+    /**
+    * This class represents an iterator for iterating through all queued message from this pool.
+    *
+    */
+    class Iterator
+    {
+        friend class MessagePool;
+
+    public:
+        /**
+         * This construct initializes an empty iterator.
+         */
+        Iterator(void) : mMessage(NULL) { }
+
+        /**
+         * This method returns the associated message with the iterator.
+         *
+         * @returns A pointer to associated message with this iterator.
+         *
+         */
+        Message *GetMessage(void) const { return mMessage; }
+
+        /**
+         * This method returns `true` if the iterator is empty (i.e., associated with a NULL message)
+         *
+         * @returns `true` if the iterator is empty, `false` otherwise.
+         */
+        bool IsEmpty(void) const { return (mMessage == NULL); }
+
+        /**
+         * This method returns `true` if the iterator has ended (beyond the last message on list).
+         *
+         * @returns `true` if the iterator has ended , `false` otherwise.
+         */
+        bool HasEnded(void) const { return IsEmpty();}
+
+        /**
+         * This method returns a new iterator corresponding to next message on the list.
+         *
+         * @returns An iterator corresponding to next message on the list.
+         *
+         */
+        Iterator GetNext(void) const { return Iterator(Next()); }
+
+        /**
+         * This method returns a new iterator corresponding to previous message on the list.
+         *
+         * @returns An iterator corresponding to previous message on the list.
+         *
+         */
+        Iterator GetPrev(void) const { return Iterator(Prev()); }
+
+        /**
+         * This method moves the current iterator to the next message on the list.
+         *
+         * @returns A reference to current iterator.
+         *
+         */
+        Iterator &GoToNext(void) { mMessage = Next(); return *this; }
+
+        /**
+         * This method moves the current iterator to the previous message on the list.
+         *
+         * @returns A reference to current iterator.
+         *
+         */
+        Iterator &GoToPrev(void) { mMessage = Prev(); return *this; }
+
+    private:
+        Iterator(Message *aMessage) : mMessage(aMessage) { }
+        Message *Next(void) const;
+        Message *Prev(void) const;
+
+        Message *mMessage;
+    };
+
     /**
      * This constructor initializes the object.
      *
@@ -759,7 +1035,8 @@ public:
     MessagePool(void);
 
     /**
-     * This method is used to obtain a new message.
+     * This method is used to obtain a new message. The default priority `kDefaultMessagePriority`
+     * is assigned to the message.
      *
      * @param[in]  aType           The message type.
      * @param[in]  aReserveHeader  The number of header bytes to reserve.
@@ -781,12 +1058,22 @@ public:
     ThreadError Free(Message *aMessage);
 
     /**
-     * This method returns a pointer to the first message in the all-messages list.
+     * This method returns a pointer to the first message (head) in the all-messages list.
+     * Messages are sorted based on their priority (head with highest priority) and order by which they are enqueued.
      *
      * @returns A pointer to the first message.
      *
      */
-    Message *GetAllMessagesListHead(void) const;
+    Iterator GetAllMessagesHead(void) const;
+
+    /**
+     * This method returns a pointer to the last message (head) in the all-messages list.
+     * Messages are sorted based on their priority (head with highest priority) and order by which they are enqueued.
+     *
+     * @returns A pointer to the last message.
+     *
+     */
+    Iterator GetAllMessagesTail(void) const { return Iterator(mAllQueue.GetTail()); }
 
     /**
      * This method returns the number of free buffers.
@@ -797,14 +1084,20 @@ public:
     uint16_t GetFreeBufferCount(void) const { return static_cast<uint16_t>(mNumFreeBuffers); }
 
 private:
+    enum
+    {
+        kDefaultMessagePriority = Message::kPriorityLow,
+    };
+
     Buffer *NewBuffer(void);
     ThreadError FreeBuffers(Buffer *aBuffer);
     ThreadError ReclaimBuffers(int aNumBuffers);
+    PriorityQueue *GetAllMessagesQueue(void) { return &mAllQueue; }
 
     int mNumFreeBuffers;
     Buffer mBuffers[kNumBuffers];
     Buffer *mFreeBuffers;
-    Message *mAllListTail;
+    PriorityQueue mAllQueue;
 };
 
 /**

tests/unit/Makefile.am

@@ -81,6 +81,7 @@ check_PROGRAMS                                                      = \
     test-mac-frame                                                    \
     test-message                                                      \
     test-message-queue                                                \
+    test-priority-queue                                               \
     test-timer                                                        \
     test-toolchain                                                    \
     $(NULL)
@@ -145,6 +146,9 @@ test_message_queue_SOURCES   = test_platform.cpp test_message_queue.cpp
 test_ncp_buffer_LDADD        = $(COMMON_LDADD)
 test_ncp_buffer_SOURCES      = test_platform.cpp test_ncp_buffer.cpp
 
+test_priority_queue_LDADD    = $(COMMON_LDADD)
+test_priority_queue_SOURCES  = test_platform.cpp test_priority_queue.cpp
+
 test_timer_LDADD             = $(COMMON_LDADD)
 test_timer_SOURCES           = test_platform.cpp test_timer.cpp
 

tests/unit/test_message_queue.cpp

@@ -33,9 +33,9 @@
 #include <string.h>
 #include <stdarg.h>
 
-#define kNumMessages      5
+#define kNumTestMessages      5
 
-// This function verifies the content of the message to matches the passed in messages
+// This function verifies the content of the message queue to match the passed in messages
 void VerifyMessageQueueContent(Thread::MessageQueue &aMessageQueue, int aExpectedLength, ...)
 {
     va_list args;
@@ -47,21 +47,21 @@ void VerifyMessageQueueContent(Thread::MessageQueue &aMessageQueue, int aExpecte
     if (aExpectedLength == 0)
     {
         message = aMessageQueue.GetHead();
-        VerifyOrQuit(message == NULL, "MessageQueue is not empty when expected len is zero.");
+        VerifyOrQuit(message == NULL, "MessageQueue is not empty when expected len is zero.\n");
     }
     else
     {
         for (message = aMessageQueue.GetHead(); message != NULL; message = message->GetNext())
         {
-            VerifyOrQuit(aExpectedLength != 0, "MessageQueue contains more entries than expected");
+            VerifyOrQuit(aExpectedLength != 0, "MessageQueue contains more entries than expected\n");
 
             msgArg = va_arg(args, Thread::Message *);
-            VerifyOrQuit(msgArg == message, "MessageQueue content does not match what is expected.");
+            VerifyOrQuit(msgArg == message, "MessageQueue content does not match what is expected.\n");
 
             aExpectedLength--;
         }
 
-        VerifyOrQuit(aExpectedLength == 0, "MessageQueue contains less entries than expected");
+        VerifyOrQuit(aExpectedLength == 0, "MessageQueue contains less entries than expected\n");
     }
 
     va_end(args);
@@ -71,9 +71,10 @@ void TestMessageQueue(void)
 {
     Thread::MessagePool messagePool;
     Thread::MessageQueue messageQueue;
-    Thread::Message *msg[kNumMessages];
+    Thread::Message *msg[kNumTestMessages];
+    uint16_t msgCount, bufferCount;
 
-    for (int i = 0; i < kNumMessages; i++)
+    for (int i = 0; i < kNumTestMessages; i++)
     {
         msg[i] = messagePool.New(Thread::Message::kTypeIp6, 0);
         VerifyOrQuit(msg[i] != NULL, "Message::New failed\n");
@@ -81,53 +82,54 @@ void TestMessageQueue(void)
 
     VerifyMessageQueueContent(messageQueue, 0);
 
-    //printf("\n- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -");
-    //printf("\nEnqueue/Dequeue one message");
-
     // Enqueue 1 message and remove it
-    SuccessOrQuit(messageQueue.Enqueue(*msg[0]), "MessageQueue::Enqueue() failed.");
+    SuccessOrQuit(messageQueue.Enqueue(*msg[0]), "MessageQueue::Enqueue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 1, msg[0]);
-    SuccessOrQuit(messageQueue.Dequeue(*msg[0]), "MessageQueue::Dequeue() failed.");
+    SuccessOrQuit(messageQueue.Dequeue(*msg[0]), "MessageQueue::Dequeue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 0);
 
     // Enqueue 5 messages
-    SuccessOrQuit(messageQueue.Enqueue(*msg[0]), "MessageQueue::Enqueue() failed.");
+    SuccessOrQuit(messageQueue.Enqueue(*msg[0]), "MessageQueue::Enqueue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 1, msg[0]);
-    SuccessOrQuit(messageQueue.Enqueue(*msg[1]), "MessageQueue::Enqueue() failed.");
+    SuccessOrQuit(messageQueue.Enqueue(*msg[1]), "MessageQueue::Enqueue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 2, msg[0], msg[1]);
-    SuccessOrQuit(messageQueue.Enqueue(*msg[2]), "MessageQueue::Enqueue() failed.");
+    SuccessOrQuit(messageQueue.Enqueue(*msg[2]), "MessageQueue::Enqueue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 3, msg[0], msg[1], msg[2]);
-    SuccessOrQuit(messageQueue.Enqueue(*msg[3]), "MessageQueue::Enqueue() failed.");
+    SuccessOrQuit(messageQueue.Enqueue(*msg[3]), "MessageQueue::Enqueue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 4, msg[0], msg[1], msg[2], msg[3]);
-    SuccessOrQuit(messageQueue.Enqueue(*msg[4]), "MessageQueue::Enqueue() failed.");
+    SuccessOrQuit(messageQueue.Enqueue(*msg[4]), "MessageQueue::Enqueue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 5, msg[0], msg[1], msg[2], msg[3], msg[4]);
 
+    // Check the GetInfo()
+    messageQueue.GetInfo(msgCount, bufferCount);
+    VerifyOrQuit(msgCount == 5, "MessageQueue::GetInfo() failed.\n");
+
     // Remove from head
-    SuccessOrQuit(messageQueue.Dequeue(*msg[0]), "MessageQueue::Dequeue() failed.");
+    SuccessOrQuit(messageQueue.Dequeue(*msg[0]), "MessageQueue::Dequeue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 4, msg[1], msg[2], msg[3], msg[4]);
 
     // Remove a message in middle
-    SuccessOrQuit(messageQueue.Dequeue(*msg[3]), "MessageQueue::Dequeue() failed.");
+    SuccessOrQuit(messageQueue.Dequeue(*msg[3]), "MessageQueue::Dequeue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 3, msg[1], msg[2], msg[4]);
 
     // Remove from tail
-    SuccessOrQuit(messageQueue.Dequeue(*msg[4]), "MessageQueue::Dequeue() failed.");
+    SuccessOrQuit(messageQueue.Dequeue(*msg[4]), "MessageQueue::Dequeue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 2, msg[1], msg[2]);
 
     // Add after removes
-    SuccessOrQuit(messageQueue.Enqueue(*msg[0]), "MessageQueue::Enqueue() failed.");
+    SuccessOrQuit(messageQueue.Enqueue(*msg[0]), "MessageQueue::Enqueue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 3, msg[1], msg[2], msg[0]);
-    SuccessOrQuit(messageQueue.Enqueue(*msg[3]), "MessageQueue::Enqueue() failed.");
+    SuccessOrQuit(messageQueue.Enqueue(*msg[3]), "MessageQueue::Enqueue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 4, msg[1], msg[2], msg[0], msg[3]);
 
     // Remove all messages
-    SuccessOrQuit(messageQueue.Dequeue(*msg[2]), "MessageQueue::Dequeue() failed.");
+    SuccessOrQuit(messageQueue.Dequeue(*msg[2]), "MessageQueue::Dequeue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 3, msg[1], msg[0], msg[3]);
-    SuccessOrQuit(messageQueue.Dequeue(*msg[1]), "MessageQueue::Dequeue() failed.");
+    SuccessOrQuit(messageQueue.Dequeue(*msg[1]), "MessageQueue::Dequeue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 2, msg[0], msg[3]);
-    SuccessOrQuit(messageQueue.Dequeue(*msg[3]), "MessageQueue::Dequeue() failed.");
+    SuccessOrQuit(messageQueue.Dequeue(*msg[3]), "MessageQueue::Dequeue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 1, msg[0]);
-    SuccessOrQuit(messageQueue.Dequeue(*msg[0]), "MessageQueue::Dequeue() failed.");
+    SuccessOrQuit(messageQueue.Dequeue(*msg[0]), "MessageQueue::Dequeue() failed.\n");
     VerifyMessageQueueContent(messageQueue, 0);
 }
 

tests/unit/test_priority_queue.cpp

@@ -0,0 +1,404 @@
+/*
+ *  Copyright (c) 2016, The OpenThread Authors.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *  2. Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *  3. Neither the name of the copyright holder nor the
+ *     names of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "test_util.h"
+#include <openthread.h>
+#include <common/debug.hpp>
+#include <common/message.hpp>
+#include <string.h>
+#include <stdarg.h>
+
+#define kNumTestMessages      3
+
+// This function verifies the content of the priority queue to matches the passed in messages
+void VerifyPriorityQueueContent(Thread::PriorityQueue &aPriorityQueue, int aExpectedLength, ...)
+{
+    va_list args;
+    Thread::Message *message;
+    Thread::Message *msgArg;
+    uint8_t curPriority = 0xff;
+    uint16_t msgCount, bufCount;
+
+    // Check the `GetInfo`
+    aPriorityQueue.GetInfo(msgCount, bufCount);
+    VerifyOrQuit(msgCount == aExpectedLength, "GetInfo() result does not match expected len.\n");
+
+    va_start(args, aExpectedLength);
+
+    if (aExpectedLength == 0)
+    {
+        message = aPriorityQueue.GetHead();
+        VerifyOrQuit(message == NULL, "PriorityQueue is not empty when expected len is zero.\n");
+
+        VerifyOrQuit(aPriorityQueue.GetHeadForPriority(0) == NULL, "GetHeadForPriority() non-NULL when empty\n");
+        VerifyOrQuit(aPriorityQueue.GetHeadForPriority(1) == NULL, "GetHeadForPriority() non-NULL when empty\n");
+        VerifyOrQuit(aPriorityQueue.GetHeadForPriority(2) == NULL, "GetHeadForPriority() non-NULL when empty\n");
+        VerifyOrQuit(aPriorityQueue.GetHeadForPriority(3) == NULL, "GetHeadForPriority() non-NULL when empty\n");
+    }
+    else
+    {
+        // Go through all messages in the queue and verify they match the passed-in messages
+        for (message = aPriorityQueue.GetHead(); message != NULL; message = message->GetNext())
+        {
+            VerifyOrQuit(aExpectedLength != 0, "PriorityQueue contains more entries than expected.\n");
+
+            msgArg = va_arg(args, Thread::Message *);
+
+            if (msgArg->GetPriority() != curPriority)
+            {
+                for (curPriority++; curPriority != msgArg->GetPriority(); curPriority++)
+                {
+                    // Check the `GetHeadForPriority` is NULL if there are no expected message for this priority level.
+                    VerifyOrQuit(
+                        aPriorityQueue.GetHeadForPriority(curPriority) == NULL,
+                        "PriorityQueue::GetHeadForPriority is non-NULL when no expected msg for this priority.\n"
+                    );
+                }
+
+                // Check the `GetHeadForPriority`.
+                VerifyOrQuit(
+                    aPriorityQueue.GetHeadForPriority(curPriority) == msgArg,
+                    "PriorityQueue::GetHeadForPriority failed.\n"
+                );
+            }
+
+            // Check the queued message to match the one from argument list
+            VerifyOrQuit(msgArg == message, "PriorityQueue content does not match what is expected.\n");
+
+            aExpectedLength--;
+        }
+
+        VerifyOrQuit(aExpectedLength == 0, "PriorityQueue contains less entries than expected.\n");
+
+        // Check the `GetHeadForPriority` is NULL if there are no expected message for any remaining priority level.
+        for (curPriority++; curPriority < 4; curPriority++)
+        {
+            VerifyOrQuit(
+                aPriorityQueue.GetHeadForPriority(curPriority) == NULL,
+                "PriorityQueue::GetHeadForPriority is non-NULL when no expected msg for this priority.\n"
+            );
+        }
+    }
+
+    va_end(args);
+}
+
+// This function verifies the content of the all message queue to match the passed in messages
+void VerifyAllMessagesContent(Thread::MessagePool &aMessagePool, int aExpectedLength, ...)
+{
+    va_list args;
+    Thread::MessagePool::Iterator it;
+    Thread::Message *msgArg;
+
+    va_start(args, aExpectedLength);
+
+    if (aExpectedLength == 0)
+    {
+        VerifyOrQuit(aMessagePool.GetAllMessagesHead().IsEmpty(), "Head is not empty when expected len is zero.\n");
+        VerifyOrQuit(aMessagePool.GetAllMessagesTail().IsEmpty(), "Tail is not empty when expected len is zero.\n");
+    }
+    else
+    {
+        for (it = aMessagePool.GetAllMessagesHead(); !it.HasEnded() ; it.GoToNext())
+        {
+            VerifyOrQuit(aExpectedLength != 0, "AllMessagesQueue contains more entries than expected.\n");
+            msgArg = va_arg(args, Thread::Message *);
+            VerifyOrQuit(msgArg == it.GetMessage(), "AllMessagesQueue content does not match what is expected.\n");
+            aExpectedLength--;
+        }
+
+        VerifyOrQuit(aExpectedLength == 0, "AllMessagesQueue contains less entries than expected.\n");
+    }
+
+    va_end(args);
+}
+
+// This function verifies the content of the all message queue to match the passed in messages , but it goes
+// through the AllMessages list in reverse.
+void VerifyAllMessagesContentInReverse(Thread::MessagePool &aMessagePool, int aExpectedLength, ...)
+{
+    va_list args;
+    Thread::MessagePool::Iterator it;
+    Thread::Message *msgArg;
+
+    va_start(args, aExpectedLength);
+
+    if (aExpectedLength == 0)
+    {
+        VerifyOrQuit(aMessagePool.GetAllMessagesHead().IsEmpty(), "Head is not empty when expected len is zero.\n");
+        VerifyOrQuit(aMessagePool.GetAllMessagesTail().IsEmpty(), "Tail is not empty when expected len is zero.\n");
+    }
+    else
+    {
+        for (it = aMessagePool.GetAllMessagesTail(); !it.HasEnded() ; it.GoToPrev())
+        {
+            VerifyOrQuit(aExpectedLength != 0, "AllMessagesQueue contains more entries than expected.\n");
+            msgArg = va_arg(args, Thread::Message *);
+            VerifyOrQuit(msgArg == it.GetMessage(), "AllMessagesQueue content does not match what is expected.\n");
+            aExpectedLength--;
+        }
+
+        VerifyOrQuit(aExpectedLength == 0, "AllMessagesQueue contains less entries than expected.\n");
+    }
+
+    va_end(args);
+}
+
+// This function verifies the content of the message queue to match the passed in messages
+void VerifyMsgQueueContent(Thread::MessageQueue &aMessageQueue, int aExpectedLength, ...)
+{
+    va_list args;
+    Thread::Message *message;
+    Thread::Message *msgArg;
+
+    va_start(args, aExpectedLength);
+
+    if (aExpectedLength == 0)
+    {
+        message = aMessageQueue.GetHead();
+        VerifyOrQuit(message == NULL, "MessageQueue is not empty when expected len is zero.\n");
+    }
+    else
+    {
+        for (message = aMessageQueue.GetHead(); message != NULL; message = message->GetNext())
+        {
+            VerifyOrQuit(aExpectedLength != 0, "MessageQueue contains more entries than expected\n");
+
+            msgArg = va_arg(args, Thread::Message *);
+            VerifyOrQuit(msgArg == message, "MessageQueue content does not match what is expected.\n");
+
+            aExpectedLength--;
+        }
+
+        VerifyOrQuit(aExpectedLength == 0, "MessageQueue contains less entries than expected\n");
+    }
+
+    va_end(args);
+}
+
+void TestPriorityQueue(void)
+{
+    Thread::MessagePool messagePool;
+    Thread::PriorityQueue queue;
+    Thread::MessageQueue messageQueue;
+    Thread::Message *msgHigh    [kNumTestMessages];
+    Thread::Message *msgMed     [kNumTestMessages];
+    Thread::Message *msgLow     [kNumTestMessages];
+    Thread::Message *msgVeryLow [kNumTestMessages];
+    Thread::MessagePool::Iterator it;
+
+    // Allocate messages with different priorities.
+    for (int i = 0; i < kNumTestMessages; i++)
+    {
+        msgHigh[i] = messagePool.New(Thread::Message::kTypeIp6, 0);
+        VerifyOrQuit(msgHigh[i] != NULL, "Message::New failed\n");
+        SuccessOrQuit(msgHigh[i]->SetPriority(0), "Message:SetPriority failed\n");
+        msgMed[i] = messagePool.New(Thread::Message::kTypeIp6, 0);
+        VerifyOrQuit(msgMed[i] != NULL, "Message::New failed\n");
+        SuccessOrQuit(msgMed[i]->SetPriority(1), "Message:SetPriority failed\n");
+        msgLow[i] = messagePool.New(Thread::Message::kTypeIp6, 0);
+        VerifyOrQuit(msgLow[i] != NULL, "Message::New failed\n");
+        SuccessOrQuit(msgLow[i]->SetPriority(2), "Message:SetPriority failed\n");
+        msgVeryLow[i] = messagePool.New(Thread::Message::kTypeIp6, 0);
+        VerifyOrQuit(msgVeryLow[i] != NULL, "Message::New failed\n");
+        SuccessOrQuit(msgVeryLow[i]->SetPriority(3), "Message:SetPriority failed\n");
+    }
+
+    // Check the failure case for `SetPriority` for invalid argument.
+    VerifyOrQuit(
+        msgHigh[2]->SetPriority(Thread::Message::kNumPriorities) == kThreadError_InvalidArgs,
+        "Message::SetPriority() with out of range value did not fail as expected.\n"
+    );
+
+    // Check the `GetPriority()`
+    for (int i = 0; i < kNumTestMessages; i++)
+    {
+        VerifyOrQuit(msgHigh[i]->GetPriority() == 0, "Message::GetPriority failed.\n");
+        VerifyOrQuit(msgMed[i]->GetPriority() == 1, "Message::GetPriority failed.\n");
+        VerifyOrQuit(msgLow[i]->GetPriority() == 2, "Message::GetPriority failed.\n");
+        VerifyOrQuit(msgVeryLow[i]->GetPriority() == 3, "Message::GetPriority failed.\n");
+    }
+
+    // Verify case of an empty queue.
+    VerifyPriorityQueueContent(queue, 0);
+
+    // Add msgs in different orders and check the content of queue.
+    SuccessOrQuit(queue.Enqueue(*msgMed[0]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 1, msgMed[0]);
+    SuccessOrQuit(queue.Enqueue(*msgMed[1]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 2, msgMed[0], msgMed[1]);
+    SuccessOrQuit(queue.Enqueue(*msgHigh[0]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 3, msgHigh[0], msgMed[0], msgMed[1]);
+    SuccessOrQuit(queue.Enqueue(*msgHigh[1]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 4, msgHigh[0], msgHigh[1], msgMed[0], msgMed[1]);
+    SuccessOrQuit(queue.Enqueue(*msgMed[2]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 5, msgHigh[0], msgHigh[1], msgMed[0], msgMed[1], msgMed[2]);
+    SuccessOrQuit(queue.Enqueue(*msgVeryLow[0]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 6, msgHigh[0], msgHigh[1], msgMed[0], msgMed[1], msgMed[2], msgVeryLow[0]);
+    SuccessOrQuit(queue.Enqueue(*msgLow[0]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 7, msgHigh[0], msgHigh[1], msgMed[0], msgMed[1], msgMed[2], msgLow[0],
+                               msgVeryLow[0]);
+
+    // Check the MessagePool::Iterator methods.
+    VerifyOrQuit(it.IsEmpty(), "Iterator::IsEmpty() failed to return `true` for an empty iterator.\n");
+    VerifyOrQuit(it.GetNext().IsEmpty(), "Iterator::IsEmpty() failed to return `true` for an empty iterator.\n");
+    VerifyOrQuit(it.GetPrev().IsEmpty(), "Iterator::IsEmpty() failed to return `true` for an empty iterator.\n");
+    it.GoToNext();
+    VerifyOrQuit(it.IsEmpty(), "Iterator::IsEmpty() failed to return `true` for an empty iterator.\n");
+    it.GoToNext();
+    VerifyOrQuit(it.IsEmpty(), "Iterator::IsEmpty() failed to return `true` for an empty iterator.\n");
+
+    it = messagePool.GetAllMessagesHead();
+    VerifyOrQuit(!it.IsEmpty(), "Iterator::IsEmpty() failed to return `false` when it is not empty.\n");
+    VerifyOrQuit(it.GetMessage() == msgHigh[0], "Iterator::GetMessage() failed.\n");
+    it = it.GetNext();
+    VerifyOrQuit(!it.IsEmpty(), "Iterator::IsEmpty() failed to return `false` when it is not empty.\n");
+    VerifyOrQuit(it.GetMessage() == msgHigh[1], "Iterator::GetNext() failed.\n");
+    it = it.GetPrev();
+    VerifyOrQuit(it.GetMessage() == msgHigh[0], "Iterator::GetPrev() failed.\n");
+    it = it.GetPrev();
+    VerifyOrQuit(it.HasEnded(), "Iterator::GetPrev() failed to return empty at head.\n");
+    it = messagePool.GetAllMessagesTail();
+    it = it.GetNext();
+    VerifyOrQuit(it.HasEnded(), "Iterator::GetNext() failed to return empty at tail.\n");
+
+    // Check the AllMessage queue contents (should match the content of priority queue).
+    VerifyAllMessagesContent(messagePool, 7, msgHigh[0], msgHigh[1], msgMed[0], msgMed[1], msgMed[2], msgLow[0],
+                             msgVeryLow[0]);
+    VerifyAllMessagesContentInReverse(messagePool, 7, msgVeryLow[0], msgLow[0], msgMed[2], msgMed[1], msgMed[0],
+                                      msgHigh[1], msgHigh[0]);
+
+    // Remove messages in different order and check the content of queue in each step.
+    SuccessOrQuit(queue.Dequeue(*msgHigh[0]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyPriorityQueueContent(queue, 6, msgHigh[1], msgMed[0], msgMed[1], msgMed[2], msgLow[0], msgVeryLow[0]);
+    SuccessOrQuit(queue.Dequeue(*msgMed[2]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyPriorityQueueContent(queue, 5, msgHigh[1], msgMed[0], msgMed[1], msgLow[0], msgVeryLow[0]);
+    SuccessOrQuit(queue.Dequeue(*msgLow[0]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyPriorityQueueContent(queue, 4, msgHigh[1], msgMed[0], msgMed[1], msgVeryLow[0]);
+    SuccessOrQuit(queue.Dequeue(*msgMed[1]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyPriorityQueueContent(queue, 3, msgHigh[1], msgMed[0], msgVeryLow[0]);
+    SuccessOrQuit(queue.Dequeue(*msgVeryLow[0]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyPriorityQueueContent(queue, 2, msgHigh[1], msgMed[0]);
+    SuccessOrQuit(queue.Dequeue(*msgHigh[1]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyPriorityQueueContent(queue, 1, msgMed[0]);
+    SuccessOrQuit(queue.Dequeue(*msgMed[0]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyPriorityQueueContent(queue, 0);
+    VerifyAllMessagesContent(messagePool, 0);
+
+    // Check the failure cases: Enqueuing an already queued message, or dequeuing a message not queued.
+    SuccessOrQuit(queue.Enqueue(*msgHigh[0]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 1, msgHigh[0]);
+    VerifyOrQuit(
+        queue.Enqueue(*msgHigh[0]) == kThreadError_Already,
+        "Enqueuing an already queued message did not fail as expected.\n"
+    );
+    VerifyOrQuit(
+        queue.Dequeue(*msgMed[0]) == kThreadError_NotFound,
+        "Dequeuing a message not queued, did not fail as expected.\n"
+    );
+    SuccessOrQuit(queue.Dequeue(*msgHigh[0]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyPriorityQueueContent(queue, 0);
+
+    // Change the priority of an already queued message and check the order change in the queue.
+    SuccessOrQuit(queue.Enqueue(*msgLow[0]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 1, msgLow[0]);
+    SuccessOrQuit(queue.Enqueue(*msgMed[0]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 2, msgMed[0], msgLow[0]);
+    SuccessOrQuit(queue.Enqueue(*msgVeryLow[0]), "PriorityQueue::Enqueue() failed.\n");
+    VerifyPriorityQueueContent(queue, 3, msgMed[0], msgLow[0], msgVeryLow[0]);
+    VerifyAllMessagesContent(messagePool, 3, msgMed[0], msgLow[0], msgVeryLow[0]);
+
+    SuccessOrQuit(msgLow[0]->SetPriority(0), "SetPriority failed for an already queued message.\n");
+    VerifyPriorityQueueContent(queue, 3, msgLow[0], msgMed[0], msgVeryLow[0]);
+    SuccessOrQuit(msgVeryLow[0]->SetPriority(3), "SetPriority failed for an already queued message.\n");
+    VerifyPriorityQueueContent(queue, 3, msgLow[0], msgMed[0], msgVeryLow[0]);
+    SuccessOrQuit(msgVeryLow[0]->SetPriority(2), "SetPriority failed for an already queued message.\n");
+    VerifyPriorityQueueContent(queue, 3, msgLow[0], msgMed[0], msgVeryLow[0]);
+    SuccessOrQuit(msgVeryLow[0]->SetPriority(1), "SetPriority failed for an already queued message.\n");
+    VerifyPriorityQueueContent(queue, 3, msgLow[0], msgMed[0], msgVeryLow[0]);
+    VerifyAllMessagesContent(messagePool, 3, msgLow[0], msgMed[0], msgVeryLow[0]);
+    SuccessOrQuit(msgVeryLow[0]->SetPriority(0), "SetPriority failed for an already queued message.\n");
+    VerifyPriorityQueueContent(queue, 3, msgLow[0], msgVeryLow[0], msgMed[0]);
+    SuccessOrQuit(msgLow[0]->SetPriority(2), "SetPriority failed for an already queued message.\n");
+    SuccessOrQuit(msgVeryLow[0]->SetPriority(3), "SetPriority failed for an already queued message.\n");
+    VerifyPriorityQueueContent(queue, 3, msgMed[0], msgLow[0], msgVeryLow[0]);
+    VerifyAllMessagesContent(messagePool, 3, msgMed[0], msgLow[0], msgVeryLow[0]);
+    VerifyAllMessagesContentInReverse(messagePool, 3, msgVeryLow[0], msgLow[0], msgMed[0]);
+
+    // Checking the AllMessages queue when adding messages from same pool to another queue.
+    SuccessOrQuit(messageQueue.Enqueue(*msgLow[1]), "MessageQueue::Enqueue() failed.\n");
+    VerifyAllMessagesContent(messagePool, 4, msgMed[0], msgLow[0], msgLow[1], msgVeryLow[0]);
+    SuccessOrQuit(messageQueue.Enqueue(*msgMed[1]), "MessageQueue::Enqueue() failed.\n");
+    VerifyAllMessagesContent(messagePool, 5, msgMed[0], msgMed[1], msgLow[0], msgLow[1], msgVeryLow[0]);
+    VerifyAllMessagesContentInReverse(messagePool, 5, msgVeryLow[0], msgLow[1], msgLow[0], msgMed[1], msgMed[0]);
+    SuccessOrQuit(messageQueue.Enqueue(*msgHigh[1]), "MessageQueue::Enqueue() failed.\n");
+    VerifyAllMessagesContent(messagePool, 6, msgHigh[1], msgMed[0], msgMed[1], msgLow[0], msgLow[1], msgVeryLow[0]);
+    VerifyMsgQueueContent(messageQueue, 3, msgLow[1], msgMed[1], msgHigh[1]);
+
+    // Change priority of message and check that order changes in the AllMessage queue and not in messageQueue.
+    SuccessOrQuit(msgLow[1]->SetPriority(0), "SetPriority failed for an already queued message.\n");
+    VerifyAllMessagesContent(messagePool, 6, msgHigh[1], msgLow[1], msgMed[0], msgMed[1], msgLow[0], msgVeryLow[0]);
+    VerifyAllMessagesContentInReverse(messagePool, 6, msgVeryLow[0], msgLow[0], msgMed[1], msgMed[0], msgLow[1],
+                                      msgHigh[1]);
+    VerifyMsgQueueContent(messageQueue, 3, msgLow[1], msgMed[1], msgHigh[1]);
+
+    SuccessOrQuit(msgVeryLow[0]->SetPriority(1), "SetPriority failed for an already queued message.\n");
+    VerifyAllMessagesContent(messagePool, 6, msgHigh[1], msgLow[1], msgMed[0], msgMed[1], msgVeryLow[0], msgLow[0]);
+    VerifyPriorityQueueContent(queue, 3, msgMed[0], msgVeryLow[0], msgLow[0]);
+    VerifyMsgQueueContent(messageQueue, 3, msgLow[1], msgMed[1], msgHigh[1]);
+
+    // Remove messages from the two queues and verify that AllMessage queue is updated correctly.
+    SuccessOrQuit(queue.Dequeue(*msgMed[0]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyAllMessagesContent(messagePool, 5, msgHigh[1], msgLow[1], msgMed[1], msgVeryLow[0], msgLow[0]);
+    VerifyPriorityQueueContent(queue, 2, msgVeryLow[0], msgLow[0]);
+    VerifyMsgQueueContent(messageQueue, 3, msgLow[1], msgMed[1], msgHigh[1]);
+
+    SuccessOrQuit(messageQueue.Dequeue(*msgHigh[1]), "MessageQueue::Dequeue() failed.\n");
+    VerifyAllMessagesContent(messagePool, 4, msgLow[1], msgMed[1], msgVeryLow[0], msgLow[0]);
+    VerifyPriorityQueueContent(queue, 2, msgVeryLow[0], msgLow[0]);
+    VerifyMsgQueueContent(messageQueue, 2, msgLow[1], msgMed[1]);
+
+    SuccessOrQuit(messageQueue.Dequeue(*msgMed[1]), "MessageQueue::Dequeue() failed.\n");
+    VerifyAllMessagesContent(messagePool, 3, msgLow[1], msgVeryLow[0], msgLow[0]);
+    VerifyPriorityQueueContent(queue, 2, msgVeryLow[0], msgLow[0]);
+    VerifyMsgQueueContent(messageQueue, 1, msgLow[1]);
+
+    SuccessOrQuit(queue.Dequeue(*msgVeryLow[0]), "PriorityQueue::Dequeue() failed.\n");
+    VerifyAllMessagesContent(messagePool, 2, msgLow[1], msgLow[0]);
+    VerifyPriorityQueueContent(queue, 1, msgLow[0]);
+    VerifyMsgQueueContent(messageQueue, 1, msgLow[1]);
+}
+
+#ifdef ENABLE_TEST_MAIN
+int main(void)
+{
+    TestPriorityQueue();
+    printf("All tests passed\n");
+    return 0;
+}
+#endif

tests/unit/test_windows.cpp

@@ -68,6 +68,9 @@ void TestMessage();
 // test_message_queue.cpp
 void TestMessageQueue();
 
+// test_priority_queue.cpp
+void TestPriorityQueue();
+
 // test_ncp_buffer.cpp
 namespace Thread
 {
@@ -151,6 +154,9 @@ namespace Thread
         // test_message_queue.cpp
         TEST_METHOD(TestMessageQueue) { ::TestMessageQueue(); }
 
+        // test_message_queue.cpp
+        TEST_METHOD(TestPriorityQueue) { ::TestPriorityQueue(); }
+
         // test_timer.cpp
         TEST_METHOD(TestOneTimer) { ::TestOneTimer(); }
         TEST_METHOD(TestTenTimers) { ::TestTenTimers(); }