void EnQMsg(msg_t *p, msg_q_t *q)
{
int t_p = q->tail;
if (MsgQFull(q)) {
cons_printf("Message queue is full, can't enqueue!\n");
return;
}
MyMemCpy((char*)&(q->msgs[t_p]),(char*)p,sizeof(msg_t));
q->tail++;
if(q->tail >= NUM_MSG)
q->tail = 0;
q->count++;
}
uint8_t os_msg_post( taskproctype taskproc, Msg_t *msg, uint16_t delay, uint16_t period ) {
#if( N_QUEUES > 0 )
OSMsgQ_t *temp;
MsgQ_t queue;
uint8_t result;
uint8_t *data;
uint8_t i;
result = TaskMsgQGet( taskproc, &queue );
if ( result == MSG_QUEUE_UNDEF ) {
return result;
}
temp = &msgQList[ queue ];
if ( MsgQFull( temp ) == 1 ) {
return MSG_QUEUE_FULL;
}
data = (uint8_t*)( (uint16_t)temp->msgPool + (temp->head) * (temp->messageSize) );
msg->delay = delay;
msg->reload = period;
for ( i = 0; i < temp->messageSize; ++i ) {
data[ i ] = ((uint8_t*)msg)[i];
}
/* Look for buffer wrap around */
if ( temp->head == 0 ) {
temp->head = temp->size - 1;
}
else {
temp->head--;
}
/* Signal the change event */
os_signal_event(temp->change);
os_event_set_signaling_tid( temp->change, running_tid );
return MSG_QUEUE_POSTED;
#else
return 0;
#endif
}