OS_RESULT rt_evt_wait (U16 wait_flags, U16 timeout, BOOL and_wait) {
/* Wait for one or more event flags with optional time-out. */
/* "wait_flags" identifies the flags to wait for. */
/* "timeout" is the time-out limit in system ticks (0xffff if no time-out) */
/* "and_wait" specifies the AND-ing of "wait_flags" as condition to be met */
/* to complete the wait. (OR-ing if set to 0). */
U32 block_state;
if (and_wait) {
/* Check for AND-connected events */
if ((os_tsk.run->events & wait_flags) == wait_flags) {
os_tsk.run->events &= ~wait_flags;
return (OS_R_EVT);
}
block_state = WAIT_AND;
}
else {
/* Check for OR-connected events */
if (os_tsk.run->events & wait_flags) {
os_tsk.run->waits = os_tsk.run->events & wait_flags;
os_tsk.run->events &= ~wait_flags;
return (OS_R_EVT);
}
block_state = WAIT_OR;
}
/* Task has to wait */
os_tsk.run->waits = wait_flags;
rt_block (timeout, (U8)block_state);
return (OS_R_TMO);
}
OS_RESULT rt_mut_wait (OS_ID mutex, U16 timeout) {
/* Wait for a mutex, continue when mutex is free. */
P_MUCB p_MCB = mutex;
if (p_MCB->level == 0) {
p_MCB->owner = os_tsk.run;
p_MCB->prio = os_tsk.run->prio;
goto inc;
}
if (p_MCB->owner == os_tsk.run) {
/* OK, running task is the owner of this mutex. */
inc:p_MCB->level++;
return (OS_R_OK);
}
/* Mutex owned by another task, wait until released. */
if (timeout == 0) {
return (OS_R_TMO);
}
/* Raise the owner task priority if lower than current priority. */
/* This priority inversion is called priority inheritance. */
if (p_MCB->prio < os_tsk.run->prio) {
p_MCB->owner->prio = os_tsk.run->prio;
rt_resort_prio (p_MCB->owner);
}
if (p_MCB->p_lnk != NULL) {
rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
}
else {
p_MCB->p_lnk = os_tsk.run;
os_tsk.run->p_lnk = NULL;
os_tsk.run->p_rlnk = (P_TCB)p_MCB;
}
rt_block(timeout, WAIT_MUT);
return (OS_R_TMO);
}
void rt_itv_wait (void) {
/* Wait for interval end and define start of next one */
U16 delta;
delta = os_tsk.run->delta_time - (U16)os_time;
os_tsk.run->delta_time += os_tsk.run->interval_time;
if ((delta & 0x8000) == 0) {
rt_block (delta, WAIT_ITV);
}
}
OS_RESULT rt_mbx_wait (OS_ID mailbox, void **message, U16 timeout) {
/* Receive a message; possibly wait for it */
P_MCB p_MCB = mailbox;
P_TCB p_TCB;
/* If a message is available in the fifo buffer */
/* remove it from the fifo buffer and return. */
if (p_MCB->count) {
*message = p_MCB->msg[p_MCB->last];
if (++p_MCB->last == p_MCB->size) {
p_MCB->last = 0;
}
if (p_MCB->state == 2) {
/* A task is waiting to send message */
p_TCB = rt_get_first ((P_XCB)p_MCB);
if (p_MCB->p_lnk == NULL) {
p_MCB->state = 0;
}
#ifdef __CMSIS_RTOS
rt_ret_val(p_TCB, 0/*osOK*/);
#else
rt_ret_val(p_TCB, OS_R_OK);
#endif
p_MCB->msg[p_MCB->first] = p_TCB->msg;
if (++p_MCB->first == p_MCB->size) {
p_MCB->first = 0;
}
rt_rmv_dly (p_TCB);
rt_dispatch (p_TCB);
}
else {
rt_dec (&p_MCB->count);
}
return (OS_R_OK);
}
/* No message available: wait for one */
if (timeout == 0) {
return (OS_R_TMO);
}
if (p_MCB->p_lnk != NULL) {
rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
}
else {
p_MCB->p_lnk = os_tsk.run;
os_tsk.run->p_lnk = NULL;
os_tsk.run->p_rlnk = (P_TCB)p_MCB;
/* Task is waiting to receive a message */
p_MCB->state = 1;
}
rt_block(timeout, WAIT_MBX);
#ifndef __CMSIS_RTOS
os_tsk.run->msg = message;
#endif
return (OS_R_TMO);
}
OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) {
/* Send message to a mailbox */
P_MCB p_MCB = mailbox;
P_TCB p_TCB;
if (p_MCB->state == 1) {
/* A task is waiting for message */
p_TCB = rt_get_first ((P_XCB)p_MCB);
if (p_MCB->p_lnk == NULL) {
p_MCB->state = 0;
}
#ifdef __CMSIS_RTOS
rt_ret_val2(p_TCB, 0x10/*osEventMessage*/, (U32)p_msg);
#else
*p_TCB->msg = p_msg;
rt_ret_val (p_TCB, OS_R_MBX);
#endif
rt_rmv_dly (p_TCB);
rt_dispatch (p_TCB);
}
else {
/* Store message in mailbox queue */
if (p_MCB->count == p_MCB->size) {
/* No free message entry, wait for one. If message queue is full, */
/* then no task is waiting for message. The 'p_MCB->p_lnk' list */
/* pointer can now be reused for send message waits task list. */
if (timeout == 0) {
return (OS_R_TMO);
}
if (p_MCB->p_lnk != NULL) {
rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
}
else {
p_MCB->p_lnk = os_tsk.run;
os_tsk.run->p_lnk = NULL;
os_tsk.run->p_rlnk = (P_TCB)p_MCB;
/* Task is waiting to send a message */
p_MCB->state = 2;
}
os_tsk.run->msg = p_msg;
rt_block (timeout, WAIT_MBX);
return (OS_R_TMO);
}
/* Yes, there is a free entry in a mailbox. */
p_MCB->msg[p_MCB->first] = p_msg;
rt_inc (&p_MCB->count);
if (++p_MCB->first == p_MCB->size) {
p_MCB->first = 0;
}
}
return (OS_R_OK);
}
OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) {
/* Send message to a mailbox */
P_MCB p_MCB = mailbox;
P_TCB p_TCB;
if (p_MCB->p_lnk != NULL && p_MCB->count == 0) {
/* A task is waiting for message */
p_TCB = rt_get_first ((P_XCB)p_MCB);
*p_TCB->msg = p_msg;
p_TCB->ret_val = OS_R_MBX;
rt_rmv_dly (p_TCB);
rt_dispatch (p_TCB);
os_tsk.run->ret_val = OS_R_OK;
}
else {
/* Store message in mailbox queue */
if (p_MCB->count == p_MCB->size) {
/* No free message entry, wait for one. If message queue is full, */
/* then no task is waiting for message. The 'p_MCB->p_lnk' list */
/* pointer can now be reused for send message waits task list. */
if (timeout == 0) {
return (OS_R_TMO);
}
if (p_MCB->p_lnk != NULL) {
rt_put_prio ((P_XCB)p_MCB, os_tsk.run);
}
else {
p_MCB->p_lnk = os_tsk.run;
os_tsk.run->p_lnk = NULL;
os_tsk.run->p_rlnk = (P_TCB)p_MCB;
/* Signal the 'isr_mbx_receive ()' that the task is waiting */
/* to send a message */
p_MCB->isr_st = 1;
}
os_tsk.run->msg = p_msg;
rt_block (timeout, WAIT_MBX);
return (OS_R_TMO);
}
/* Yes, there is a free entry in a mailbox. */
p_MCB->msg[p_MCB->first] = p_msg;
rt_inc (&p_MCB->count);
if (++p_MCB->first == p_MCB->size) {
p_MCB->first = 0;
}
}
return (OS_R_OK);
}
void *rt_mem_alloc (void *box_mem)
{
void *ptr;
//printf("Entered malloc\n");
ptr = rt_alloc_box(box_mem);
if(ptr != NULL)
return ptr;
else {
// add task to waiting list.
rt_put_prio(&wait_list, os_tsk.run);
rt_block (0xffff, (U8)os_tsk.run);
return NULL;
}
/*
void *ptr;
ptr = rt_alloc_box(box_mem);
printf("wait_count: %d\n", wait_count);
if (ptr != NULL)
{
if (wait_count > 0)
{
if (os_tsk.run->prio <= highest_prio)
{
highest_prio = os_tsk.run->prio;
printf("highest_prio: %d\n", highest_prio);
return ptr;
}
}
else if (wait_count == 0) //Empty list, with mem available, just add task
{
return ptr;
}
}
else
{
rt_put_prio(&wait_list, os_tsk.run);
//increment counter when a task is added to wait list
wait_count++;
printf("wait_count, if mem no available: %d\n", wait_count);
rt_block (0xffff, (U8)os_tsk.run);
}
*/
}
OS_RESULT rt_sem_wait (OS_ID semaphore, U16 timeout) {
/* Obtain a token; possibly wait for it */
P_SCB p_SCB = semaphore;
if (p_SCB->tokens) {
p_SCB->tokens--;
return (OS_R_OK);
}
/* No token available: wait for one */
if (timeout == 0) {
return (OS_R_TMO);
}
if (p_SCB->p_lnk != NULL) {
rt_put_prio ((P_XCB)p_SCB, os_tsk.run);
}
else {
p_SCB->p_lnk = os_tsk.run;
os_tsk.run->p_lnk = NULL;
os_tsk.run->p_rlnk = (P_TCB)p_SCB;
}
rt_block(timeout, WAIT_SEM);
return (OS_R_TMO);
}
void rt_dly_wait (U16 delay_time) {
/* Delay task by "delay_time" */
rt_block (delay_time, WAIT_DLY);
}
