Put a Message into Queue sorted by Priority (Highest at Head). \param[in] mq message queue object. \param[in] msg message object.
| 40 | /// \param[in] mq message queue object. |
| 41 | /// \param[in] msg message object. |
| 42 | static void MessageQueuePut (os_message_queue_t *mq, os_message_t *msg) { |
| 43 | #if (EXCLUSIVE_ACCESS == 0) |
| 44 | uint32_t primask = __get_PRIMASK(); |
| 45 | #endif |
| 46 | os_message_t *prev, *next; |
| 47 | |
| 48 | if (mq->msg_last != NULL) { |
| 49 | prev = mq->msg_last; |
| 50 | next = NULL; |
| 51 | while ((prev != NULL) && (prev->priority < msg->priority)) { |
| 52 | next = prev; |
| 53 | prev = prev->prev; |
| 54 | } |
| 55 | msg->prev = prev; |
| 56 | msg->next = next; |
| 57 | if (prev != NULL) { |
| 58 | prev->next = msg; |
| 59 | } else { |
| 60 | mq->msg_first = msg; |
| 61 | } |
| 62 | if (next != NULL) { |
| 63 | next->prev = msg; |
| 64 | } else { |
| 65 | mq->msg_last = msg; |
| 66 | } |
| 67 | } else { |
| 68 | msg->prev = NULL; |
| 69 | msg->next = NULL; |
| 70 | mq->msg_first= msg; |
| 71 | mq->msg_last = msg; |
| 72 | } |
| 73 | |
| 74 | #if (EXCLUSIVE_ACCESS == 0) |
| 75 | __disable_irq(); |
| 76 | |
| 77 | mq->msg_count++; |
| 78 | |
| 79 | if (primask == 0U) { |
| 80 | __enable_irq(); |
| 81 | } |
| 82 | #else |
| 83 | (void)atomic_inc32(&mq->msg_count); |
| 84 | #endif |
| 85 | } |
| 86 | |
| 87 | /// Get a Message from Queue with Highest Priority. |
| 88 | /// \param[in] mq message queue object. |
no test coverage detected