Function svcRtxMessageQueuePut

source/rtos2/RTX/Source/rtx_msgqueue.c:392–464

Put a Message into a Queue or timeout if Queue is full. \note API identical to osMessageQueuePut

/// Put a Message into a Queue or timeout if Queue is full.
/// \note API identical to osMessageQueuePut
static osStatus_t svcRtxMessageQueuePut (osMessageQueueId_t mq_id, const void *msg_ptr, uint8_t msg_prio, uint32_t timeout) {
  os_message_queue_t *mq = osRtxMessageQueueId(mq_id);
  os_message_t       *msg;
  os_thread_t        *thread;
  uint32_t           *reg;
  void               *ptr;
  osStatus_t          status;

  // Check parameters
  if ((mq == NULL) || (mq->id != osRtxIdMessageQueue) || (msg_ptr == NULL)) {
    EvrRtxMessageQueueError(mq, (int32_t)osErrorParameter);
    //lint -e{904} "Return statement before end of function" [MISRA Note 1]
    return osErrorParameter;
  }

  // Check if Thread is waiting to receive a Message
  if ((mq->thread_list != NULL) && (mq->thread_list->state == osRtxThreadWaitingMessageGet)) {
    EvrRtxMessageQueueInserted(mq, msg_ptr);
    // Wakeup waiting Thread with highest Priority
    thread = osRtxThreadListGet(osRtxObject(mq));
    osRtxThreadWaitExit(thread, (uint32_t)osOK, TRUE);
    // Copy Message (R2: void *msg_ptr, R3: uint8_t *msg_prio)
    reg = osRtxThreadRegPtr(thread);
    //lint -e{923} "cast from unsigned int to pointer"
    ptr = (void *)reg[2];
    memcpy(ptr, msg_ptr, mq->msg_size);
    if (reg[3] != 0U) {
      //lint -e{923} -e{9078} "cast from unsigned int to pointer"
      *((uint8_t *)reg[3]) = msg_prio;
    }
    EvrRtxMessageQueueRetrieved(mq, ptr);
    status = osOK;
  } else {
    // Try to allocate memory
    //lint -e{9079} "conversion from pointer to void to pointer to other type" [MISRA Note 5]
    msg = osRtxMemoryPoolAlloc(&mq->mp_info);
    if (msg != NULL) {
      // Copy Message
      memcpy(&msg[1], msg_ptr, mq->msg_size);
      // Put Message into Queue
      msg->id       = osRtxIdMessage;
      msg->flags    = 0U;
      msg->priority = msg_prio;
      MessageQueuePut(mq, msg);
      EvrRtxMessageQueueInserted(mq, msg_ptr);
      status = osOK;
    } else {
      // No memory available
      if (timeout != 0U) {
        EvrRtxMessageQueuePutPending(mq, msg_ptr, timeout);
        // Suspend current Thread
        if (osRtxThreadWaitEnter(osRtxThreadWaitingMessagePut, timeout)) {
          osRtxThreadListPut(osRtxObject(mq), osRtxThreadGetRunning());
          // Save arguments (R2: const void *msg_ptr, R3: uint8_t msg_prio)
          //lint -e{923} -e{9078} "cast from unsigned int to pointer"
          reg = (uint32_t *)(__get_PSP());
          //lint -e{923} -e{9078} "cast from pointer to unsigned int"
          reg[2] = (uint32_t)msg_ptr;