/* Removes the tack 'tcb' from the RDYQUE 'rq'.
*/
void
ready_queue_delete(RDYQUE *rq,TCB *tcb)
{
INT i, priority;
/* Remove the task 'tcb' from the associated FIFO (for that priority).
However, it that task is the only element at that priority then
the 'bitmap' field must be adjusted.
*/
priority = tcb->priority;
queue_delete(&(tcb->tskque));
if ( !queue_empty_p(&(rq->tskque[priority])) )
return;
bitmap_clear(rq, priority);
if ( priority!=rq->top_priority )
return;
/* This section of the code deals with the case where the highest
priority task was removed and as a result, the 'top_priority'
field must be updated.
*/
for ( i=priority/BITMAPSZ;i<NUM_BITMAP;i++ )
{
if ( rq->bitmap[i] )
{
rq->top_priority = i * BITMAPSZ + _ffs(rq->bitmap[i]);
return;
}
}
rq->top_priority = NUM_PRI;
}
/**
* Periodically inspect the solenoid queue and dispatch
* the next pending request.
*/
CALLSET_ENTRY (sol, idle_every_100ms)
{
if (sol_pulse_timer == 0 &&
!queue_empty_p (&sol_req_queue.header))
{
U8 sol = queue_remove (&sol_req_queue.header, SOL_REQ_QUEUE_LEN);
sol_req_start (sol);
}
}
/* This function rotates the FIFO in the priority queue RDYQUE 'rq' for
the supplied 'priority'. This involves removing the element from the
head of the queue and then reinserting it at the tail/end of the queue.
*/
void
ready_queue_rotate(RDYQUE *rq,INT priority)
{
QUEUE *entry, *queue;
if ( !queue_empty_p(queue=&(rq->tskque[priority])) )
{
entry = queue_delete_next(queue);
queue_insert(entry, queue);
}
}
int BreadthFirstTraversal(struct queue *q,Node *root,Node *subformula[],int *i)
{
double infval = mxGetInf();
Node *p = NULL;
if (root == NULL) return 0;
enqueue(q,root); /* enqueue the root node*/
while (!queue_empty_p(q)) {
if(!q->first){
p = NULL;
}
else{ /* set subformula index*/
p = dupnode(q->first);
p = q->first;
p->index = *i;
subformula[*i] = dupnode(p);
subformula[*i] = p;
subformula[*i]->BoundCheck = 0;
subformula[*i]->UBound = -infval;
subformula[*i]->LBound = infval;
subformula[*i]->LBound_nxt = infval;
subformula[*i]->UBindicator = 0;
subformula[*i]->LBindicator = 0;
subformula[*i]->LBindicator_nxt = 0;
subformula[*i]->loop_end = 0;
(*i)++;
}
dequeue(q);
if (p->lft != NULL)
BreadthFirstTraversal( q,p->lft,subformula,i);
if (p->rgt != NULL)
BreadthFirstTraversal( q,p->rgt,subformula,i);
}
return (*i-1);
}
