/*
* message_claim
*/
int message_claim(message_queue_t * MessageQueue)
{
MessageElem_t * Elem_p;
int i;
BOOL SemWait = FALSE;
if (! MessageInitDone)
{
printf("%s() not initialized !!!\n", __FUNCTION__);
return((int)NULL);
}
if (MessageQueue->Index < QUEUE_NB_MAX)
{
PrintMessageQ_Debug(("COMPAT: Claiming (%d)\n", MessageQueue->Index));
Elem_p = &MessageArray[ MessageQueue->Index ];
do
{
if (SemWait)
{
PrintMessageQ_FullDebug(("COMPAT: Semaphore unlocked (%d)\n", MessageQueue->Index));
}
/* Scan every element to find first free one */
locker_lock(message_mutex);
for (i=0 ; i<Elem_p->NoElements ; i++)
{
if (! Elem_p->Used_p[i])
{
PrintMessageQ_Debug(("COMPAT: Claiming done, elem:%d\n", i));
Elem_p->Used_p[i] = TRUE;
locker_unlock(message_mutex);
return( (int)(Elem_p->Memory_p) + (int)(i * Elem_p->ElementSize) );
}
}
locker_unlock(message_mutex);
/* No free message has been found, waiting for message release */
SemWait = TRUE;
PrintMessageQ_FullDebug(("COMPAT: message_claim, No free message, waiting ...\n"));
}
while (semaphore_wait_timeout(MessageQueue->ClaimSemaphore_p, FYF_WAIT_FOREVER) == 0);
PrintMessageQ_FullDebug(("COMPAT: Claiming timeout\n"));
}
return((int)NULL);
}
/*
* Block forever waiting for EVENT to be signalled,
* or timeout after timeout (ms) have expired
* set timeout to EMBX_TIMEOUT_INFINITE to wait forever
*
* Returns EMBX_SUCCESS if signalled, EMBX_SYSTEM_TIMEOUT otherwise
*/
EMBX_ERROR EMBX_OS_EventWaitTimeout(EMBX_EVENT ev, unsigned long timeout)
{
int res;
osclock_t time;
EMBX_Assert(ev);
if (timeout != EMBX_TIMEOUT_INFINITE)
{
/* OS21 semaphore timeout takes an absolute time value
* so we calculate that here
*/
time = time_plus(time_now(), (timeout*time_ticks_per_sec())/1000);
res = semaphore_wait_timeout(ev->event, &time);
}
else
/* Infinite wait */
res = semaphore_wait(ev->event);
if (res == OS21_FAILURE)
return EMBX_SYSTEM_TIMEOUT;
return EMBX_SUCCESS;
}
/*
* message_receive_timeout
*/
void * message_receive_timeout (message_queue_t* MessageQueue, int waittime)
{
MessageSend_t * Pending_p;
MessageElem_t * Elem_p;
void * Message_p = NULL;
BOOL SemWait = FALSE;
if (! MessageInitDone)
{
printf( "%s(): not initialized !!!\n", __FUNCTION__);
return (Message_p);
}
if (MessageQueue->Index < QUEUE_NB_MAX)
{
PrintMessageQ_FullDebug(("COMPAT: Receiving (%d)\n", MessageQueue->Index));
#ifdef MESSAGE_QUEUE_DEBUG
Elem_p = &MessageArray[ MessageQueue->Index ];
locker_lock(message_mutex);
Pending_p = Elem_p->Pending_p;
if (Pending_p != NULL)
{
PrintMessageQ_Debug(("COMPAT: Queue %d: ", MessageQueue->Index));
while (Pending_p != NULL)
{
PrintMessageQ_Debug(("-> 0x%x ", Pending_p->Message_p));
Pending_p = Pending_p->Next_p;
}
PrintMessageQ_Debug(("-> NULL\n"));
}
locker_unlock(message_mutex);
#endif
do
{
if (SemWait)
{
PrintMessageQ_FullDebug(("COMPAT: Semaphore unlocked (%d)\n", MessageQueue->Index));
}
Elem_p = &MessageArray[ MessageQueue->Index ];
locker_lock(message_mutex);
Pending_p = Elem_p->Pending_p;
if (Pending_p != NULL)
{
Elem_p->Pending_p = Pending_p->Next_p;
Message_p = Pending_p->Message_p;
wrapper_deallocate(Pending_p);
PrintMessageQ_Debug(("COMPAT: Queue %d: Received 0x%x \n", MessageQueue->Index, Message_p));
locker_unlock(message_mutex);
return (Message_p);
}
locker_unlock(message_mutex);
SemWait = TRUE;
PrintMessageQ_FullDebug(("COMPAT: message_receive (%d), No message available, waiting ...\n", MessageQueue->Index));
}
while (semaphore_wait_timeout(MessageQueue->MsgSemaphore_p, waittime) == 0);
PrintMessageQ_FullDebug(("COMPAT: Receive timeout (%d) \n", MessageQueue->Index));
}
return (Message_p);
}