void semQueuePendQGet
(
Q_HEAD *pQHead, /* pend queue to get first task off */
Q_NODE *pQNode
)
{
FAST WIND_TCB *pTcb;
struct osl_wait_data * entry_temp = QUEUE_NODE_PTR_TO_TCB(pQNode);
pTcb = entry_temp->task;
Q_REMOVE(pQHead, pQNode);
#ifdef _WRS_CONFIG_SV_INSTRUMENTATION
/* system viewer - level 2 event logging */
EVT_TASK_1 (EVENT_WINDPENDQGET, (int) pTcb); /* log event */
#endif /* _WRS_CONFIG_SV_INSTRUMENTATION */
pTcb->status &= ~WIND_PEND; /* clear pended flag */
if (pTcb->status & WIND_DELAY) /* task was timing out */
{
pTcb->status &= ~WIND_DELAY;
Q_REMOVE (&tickQHead, &pTcb->tickNode); /* remove from queue */
}
if (pTcb->status == WIND_READY) /* task is now ready */
READY_Q_PUT (pTcb);
}
static void discard_vcc(VCC *vcc)
{
if (do_close(vcc->fd))
diag(COMPONENT,DIAG_ERROR,"close: %s",strerror(errno));
if (vcc->entry) Q_REMOVE(vcc->entry->vccs,vcc);
else Q_REMOVE(unidirectional_vccs,vcc);
free(vcc);
}
static void agent_wake(struct sched_state *s, int tid)
{
struct agent *a = agent_by_tid_or_null(&s->dq, tid);
if (a) {
Q_REMOVE(&s->dq, a, nobe);
} else {
a = agent_by_tid(&s->sq, tid);
Q_REMOVE(&s->sq, a, nobe);
}
Q_INSERT_FRONT(&s->rq, a, nobe);
}
static void current_dequeue(struct sched_state *s)
{
struct agent *a = agent_by_tid_or_null(&s->rq, s->cur_agent->tid);
if (a == NULL) {
a = agent_by_tid(&s->dq, s->cur_agent->tid);
Q_REMOVE(&s->dq, a, nobe);
} else {
Q_REMOVE(&s->rq, a, nobe);
}
assert(a == s->cur_agent);
}
void discard_entry(ENTRY *entry)
{
if (entry->notify)
diag(COMPONENT,DIAG_FATAL,"discard_entry: entry %p had notifications");
STOP_TIMER(entry);
discard_vccs(entry);
if (!entry->itf) Q_REMOVE(unknown_incoming,entry);
else {
if (entry == entry->itf->arp_srv) entry->itf->arp_srv = NULL;
Q_REMOVE(entry->itf->table,entry);
}
if (entry->addr) free(entry->addr);
free(entry);
}
void free_thread(thread_t *thread)
{
event_t *e;
while ((e = Q_GET_HEAD(&thread->nb_events))) {
Q_REMOVE(&thread->nb_events, e, gc_link);
free_nb_event(e);
}
buf_t *buf;
while ((buf = Q_GET_HEAD(&thread->bufs))) {
Q_REMOVE(&thread->bufs, buf, gc_link);
free_buf(buf);
}
free(thread);
}
/** @brief Function to awaken all threads waiting on the
* condition variable
*
* Broadcast signal to wake all threads in the waiting list.
*
* @param *cv the cond variable that is being broadcasted
* @return Void.
*/
void
cond_broadcast (cond_t * cv)
{
cond_t *node_ptr = NULL;
assert (cv != NULL);
/* Lock the queue */
spin_lock_request (&cv->spinlock);
while (1) {
/* Wake up all the thread in the queue */
node_ptr = Q_GET_FRONT (&cv->waiting_list);
if (node_ptr == NULL) {
lprintf ("In cond_broadcast: no threads!");
break;
}else {
/* Remove node from waiting list */
Q_REMOVE (&cv->waiting_list, node_ptr, cond_link);
/* Make the thread runnable */
make_runnable (node_ptr->tid);
/* After move the node, free space */
free (node_ptr);
}
}
/* Unlock the thread */
spin_lock_release (&cv->spinlock);
return;
}
STATUS qRemove(
Q_HEAD *pQHead,
Q_NODE *pQNode
)
{
return Q_REMOVE(pQHead, pQNode);
}
void _mtimer_stop(mtimer_t *timer)
{
mtimer_t *walk;
for (walk = timers; walk; walk = walk->next)
if( walk == timer ) {
Q_REMOVE(timers,timer);
free(timer);
return;
}
}
// need to have the sched lock
void wakeup_sleepers(void) {
struct timespec time;
clock_gettime(CLOCK_REALTIME, &time);
thread_t *t;
while ((t = Q_GET_FRONT(&state.sleep_queue)) &&
time_lt(t->sleep_event.u.wakeup, time)) {
Q_REMOVE(&state.sleep_queue, t, q_link);
Q_INSERT_TAIL(&state.sched_queue, t, q_link);
}
}
static void handle_unsleep(struct sched_state *s, int tid)
{
/* If a thread-change happens to an agent on the sleep queue, that means
* it has woken up but runnable() hasn't seen it yet. So put it on the
* dq, which will satisfy whether or not runnable() triggers. */
struct agent *a = agent_by_tid_or_null(&s->sq, tid);
if (a != NULL) {
Q_REMOVE(&s->sq, a, nobe);
Q_INSERT_FRONT(&s->dq, a, nobe);
}
}
AsnInt setNetPrefix(VarBind *varbind, Variable *var)
{
int cmp;
NetPrefixNode *prefix, *newPrefix;
AsnOid *varBindName;
diag(COMPONENT,DIAG_DEBUG,"setNetPrefix");
varBindName = &varbind->name;
if(varbind->value->choiceId != OBJECTSYNTAX_SIMPLE ||
varbind->value->a.simple->choiceId != SIMPLESYNTAX_NUMBER ||
(varbind->value->a.simple->a.number != VALID &&
varbind->value->a.simple->a.number != INVALID))
return BADVALUE;
if(AsnOidSize(varBindName) != NETPREFIX_LEN + INDEX_LEN ||
varBindName->octs[NETPREFIX_LEN] != LOCALUNI ||
varBindName->octs[NETPREFIX_LEN + 1] != NETPREFIX_STRINGLEN)
return NOSUCHNAME;
for(prefix = (NetPrefixNode *) var->value, cmp = AsnOidLess;
prefix != NULL && (cmp = AsnOidCompare(varBindName, prefix->name)) <
AsnOidEqual;
prefix = prefix->next);
if(varbind->value->a.simple->a.number == VALID && cmp != AsnOidEqual)
{
newPrefix = alloc_t(NetPrefixNode);
newPrefix->name = alloc_t(AsnOid);
newPrefix->name->octs = alloc(varBindName->octetLen);
AsnOidCopy(newPrefix->name, varBindName);
{
NetPrefixNode *tmp = (NetPrefixNode *) var->value;
Q_INSERT_BEFORE(tmp, newPrefix, prefix);
}
if(atmNetPrefix.octs == NULL)
{
atmNetPrefix.octetLen = varBindName->octetLen - NETPREFIX_LEN - 2;
atmNetPrefix.octs = alloc(atmNetPrefix.octetLen);
memcpy(atmNetPrefix.octs, &varBindName->octs[NETPREFIX_LEN + 2], atmNetPrefix.octetLen);
}
}
else if (varbind->value->a.simple->a.number == INVALID && cmp == AsnOidEqual)
{
{
NetPrefixNode *tmp = (NetPrefixNode *) var->value;
Q_REMOVE(tmp, prefix);
}
}
return NOERROR;
}
void destroyBlock(Block *block)
{
FaceBlock neighbors[NUM_PORTS+1];
tellNeighborsDestroyed(block, neighbors);
Q_REMOVE(&blockList, block, blockLink);
block->destroyed = 1;
allBlocks[block->id] = NULL;
free(block);
// now set neighbor count
int i;
for (i=0; neighbors[i].block != 0; i++) {
msg2vm(neighbors[i].block, CMD_ADD_NCOUNT, countNeighbors(neighbors[i].block));
msg2vm(neighbors[i].block, CMD_ADD_VACANT, neighbors[i].face);
}
}
bool arbiter_pop_choice(struct arbiter_state *r, unsigned int *tid, bool *txn, unsigned int *xabort_code, struct abort_set *aborts)
{
struct choice *c = Q_GET_TAIL(&r->choices);
if (c) {
lsprintf(DEV, "using requested tid %d\n", c->tid);
Q_REMOVE(&r->choices, c, nobe);
*tid = c->tid;
*txn = c->txn;
*xabort_code = c->xabort_code;
*aborts = c->aborts;
MM_FREE(c);
return true;
} else {
return false;
}
}
// A single threaded main loop
void work_loop(void)
{
for (;;) {
thread_t *thread;
if ((thread = Q_GET_HEAD(&state.sched_queue))) {
Q_REMOVE(&state.sched_queue, thread, q_link);
run_thread(thread);
state.events_until_epoll--;
}
bool can_sleep = !Q_GET_HEAD(&state.sched_queue);
if (can_sleep || state.events_until_epoll == 0) {
struct timespec next_wakeup = get_next_wakeup();
do_poll(can_sleep, next_wakeup);
state.events_until_epoll = Q_GET_SIZE(&state.sched_queue);
wakeup_sleepers(); // maybe not every time through?
}
}
}
// A multithreaded main loop
void *work_loop_mt(void *p)
{
for (;;) {
thread_t *thread;
sched_lock();
wakeup_sleepers(); // maybe not every time through?
if ((thread = Q_GET_HEAD(&state.sched_queue))) {
Q_REMOVE(&state.sched_queue, thread, q_link);
sched_unlock();
run_thread(thread);
sched_lock();
}
bool can_sleep = !Q_GET_HEAD(&state.sched_queue);
struct timespec next_wakeup = get_next_wakeup();
sched_unlock();
do_poll(can_sleep, next_wakeup);
}
return NULL;
}
msg_data_t channel_recv(channel_t *ch)
{
msg_data_t ret = {-1, {0}};
msg_t *msg;
rt_mutex_lock(&ch->lock);
if ((msg = Q_GET_HEAD(&ch->msgs))) {
Q_REMOVE(&ch->msgs, msg, q_link);
ret = msg->data;
rt_mutex_unlock(&ch->lock);
free(msg);
} else {
rt_mutex_unlock(&ch->lock);
}
return ret;
}
void vcc_failed(VCC *vcc)
{
ENTRY *entry,*next;
diag(COMPONENT,DIAG_DEBUG,"failed VCC 0x%p",vcc);
Q_REMOVE(vcc->entry->vccs,vcc);
if (!(vcc->entry->flags & ATF_ARPSRV) || vcc->entry->vccs) {
/* VCC is already closed */
vcc_detach(vcc->entry);
free(vcc);
return;
}
for (entry = vcc->entry->itf->table; entry; entry = next) {
next = entry->next;
if (entry == vcc->entry || entry->state != as_resolv) continue;
if (entry->vccs || (entry->flags & ATF_PERM)) entry->state = as_invalid;
else discard_entry(entry);
}
START_TIMER(vcc->entry,RETRY);
free(vcc);
}
/** @brief Function to wake up a thread waiting on the conditon
* variable
*
* Wake the first thread in the waiting list of the condition
* variable.
*
* @param *cv the cond variable we are signaling
* @return void
*/
void
cond_signal (cond_t * cv)
{
assert (cv != NULL);
cond_t *node_ptr = NULL;
int tid = DEFAULT_TID;
/* Lock the queue */
spin_lock_request (&cv->spinlock);
/* Get the head node(thread) in the queue */
node_ptr = Q_GET_FRONT (&cv->waiting_list);
/* If there are no thread in the queue, unlock and return */
if (node_ptr == NULL) {
lprintf ("No blocked threads!");
spin_lock_release (&cv->spinlock);
return;
}
else {
/* If get, remove the first node from the queue */
Q_REMOVE (&cv->waiting_list, node_ptr, cond_link);
}
/* Got one waitting thread from the queue, unlock the queue */
tid = node_ptr->tid;
spin_lock_release (&cv->spinlock);
/* Make that thread runnable */
while (make_runnable (tid) < 0) {
yield (tid);
}
/* Free */
free (node_ptr);
return;
}
static void agent_deschedule(struct sched_state *s, int tid)
{
struct agent *a = agent_by_tid_or_null(&s->rq, tid);
if (a != NULL) {
Q_REMOVE(&s->rq, a, nobe);
Q_INSERT_FRONT(&s->dq, a, nobe);
/* If it's not on the runqueue, we must have already special-case moved
* it off in the thread-change event. */
} else if (agent_by_tid_or_null(&s->sq, tid) == NULL) {
/* Either it's on the sleep queue, or it vanished. */
if (agent_by_tid_or_null(&s->dq, tid) != NULL) {
conf_object_t *cpu = SIM_get_object("cpu0");
char *stack = stack_trace(cpu, GET_CPU_ATTR(cpu, eip),
s->cur_agent->tid);
lsprintf(ALWAYS, COLOUR_BOLD COLOUR_RED "TID %d is "
"already off the runqueue at tell_off_rq(); "
"probably incorrect annotations?\n", tid);
lsprintf(ALWAYS, COLOUR_BOLD COLOUR_RED "Current stack: %s\n"
COLOUR_DEFAULT, stack);
assert(0);
}
}
}
STATUS taskDelay(
unsigned timeout
)
{
STATUS status;
if (INT_RESTRICT() != OK)
{
errnoSet(S_intLib_NOT_ISR_CALLABLE);
status = ERROR;
}
else
{
/* Enter kernel mode */
kernelState = TRUE;
/* If no wait, then just re-insert it */
if (timeout == WAIT_NONE)
{
Q_REMOVE(&readyQHead, taskIdCurrent);
Q_PUT(&readyQHead, taskIdCurrent, taskIdCurrent->priority);
}
else
{
/* Put to sleep */
vmxDelay(timeout);
}
/* Exit trough kernel, and check for error */
if ((status = vmxExit()) == SIG_RESTART)
{
status = ERROR;
}
}
return status;
}
void queue_remove(QUEUE *q,BUFFER *b)
{
if (q->last == b) q->last = b->prev;
Q_REMOVE(q->first,b);
}
void _mtimer_pop(mtimer_t *timer)
{
Q_REMOVE(timers,timer);
timer->callback(timer->user);
free(timer);
}
int
createtree(struct fh *rootfh, int depth,
int d_max, dist_func_t d_cnts, dist_func_t d_weights,
int f_max, dist_func_t f_cnts, dist_func_t f_weights,
int l_max, dist_func_t l_cnts, dist_func_t l_weights,
dist_func_t f_sizes, int scale)
{
int i, num_dirs_at_root = 1, create_reported = 0, ret=-1;
nameset_entry_t rootnse;
struct cr_rec *cr;
int rexmit_max_preserve = 0;
/*
* remember the distributions.
*/
cr_d_cnts = d_cnts;
cr_d_weights = d_weights;
cr_d_max = d_max;
cr_f_cnts = f_cnts;
cr_f_weights = f_weights;
cr_f_max = f_max;
cr_l_cnts = l_cnts;
cr_l_weights = l_weights;
cr_l_max = l_max;
cr_f_sizes = f_sizes;
cr_scale = scale;
Q_INIT(&cr_worklist);
/*
* create a root name entry.
*/
if ((rootnse = nameset_alloc(NULL, NFDIR, 0/*never pick*/)) == NULL) {
report_error(FATAL, "nameset_alloc error");
goto out;
}
nameset_setfh(rootnse, rootfh->data, rootfh->len);
/*
* set up the file contents data block.
*/
if ((filedata = malloc(8192)) == NULL) {
report_perror(FATAL, "malloc error");
goto out;
}
memset(filedata, 'x', 8192);
rexmit_max_preserve = rexmit_max;
rexmit_max = 2; /* rexmit a few times before cancel */
/*
* go!
*/
for (i=0 ; i<num_dirs_at_root ; i++) {
cr_newdir(rootnse, depth);
}
/*
* create everything in parallel.
*/
while ((cr = Q_FIRST(&cr_worklist)) != NULL) {
/*
* some feedback that things are moving along...
*/
if (created - create_reported >= 100) {
printf("%d ", created);
fflush(stdout);
create_reported = created;
}
Q_REMOVE(&cr_worklist, cr, link);
switch(cr->nse->type) {
case NFDIR:
cr_dir(cr);
break;
case NFREG:
cr_file(cr);
break;
case NFLNK:
cr_symlink(cr);
break;
default:
report_error(FATAL, "bad type %d", cr->nse->type);
goto out;
}
/*
* wait for replies b/c they register new worklist items.
*/
if (Q_FIRST(&cr_worklist) == NULL) {
op_barrier(0);
}
}
/*
* wait for everything to finish. (redundant with above loop.)
*/
if (op_barrier(0) < 0) {
report_error(FATAL, "op_barrier error");
goto out;
}
ret = 0;
out:
rexmit_max = rexmit_max_preserve;
if (filedata) {
free(filedata);
filedata = NULL;
}
return ret;
}
static int learn(VCC *vcc,uint32_t ip,struct sockaddr_atmsvc *addr)
{
ENTRY *entry;
ITF *itf;
VCC *walk,*next;
unsigned char *ipp;
int result = 0;
if (!ip) return 0;
ipp = (unsigned char *) &ip;
itf = lookup_itf_by_ip(ip);
if (!itf) {
diag(COMPONENT,DIAG_ERROR,"got unroutable IP address %d.%d.%d.%d",
ipp[0],ipp[1],ipp[2],ipp[3]);
return 0;
}
entry = lookup_ip(itf,ip);
assert(!vcc || vcc->entry);
/*
* If the entry on which we received the update isn't dangling but it
* doesn't correspond to the one with the address, ...
*/
if (entry && vcc && vcc->entry->itf && entry != vcc->entry) {
diag(COMPONENT,DIAG_DEBUG,"collision on %d.%d.%d.%d",ipp[0],ipp[1],
ipp[2],ipp[3]);
return 0;
}
/*
* If the entry on which we received the update is dangling and we found
* an entry that already describes that IP address, ...
*/
if (entry && vcc && !vcc->entry->itf) {
if (!entry->svc) {
diag(COMPONENT,DIAG_ERROR,"attempt to overwrite PVC for IP "
"%d.%d.%d.%d",ipp[0],ipp[1],ipp[2],ipp[3]);
return 0;
}
STOP_TIMER(vcc->entry);
Q_REMOVE(unknown_incoming,vcc->entry);
free(vcc->entry);
vcc->entry = entry;
Q_INSERT_HEAD(entry->vccs,vcc);
set_sndbuf(vcc);
entry->flags &= ~ATF_NOVC;
assert(!vcc->connecting);
if (set_ip(vcc->fd,ip) < 0) {
diag(COMPONENT,DIAG_ERROR,"set_ip: %s",strerror(errno));
disconnect_vcc(vcc);
vcc = NULL;
result = -1;
}
}
/*
* If we still don't have an entry, we try to use the entry that already
* belongs to the VCC (InARP), or we create a new one (ARP).
*/
if (!entry) {
if (vcc) {
entry = vcc->entry;
if (!entry->itf) {
Q_REMOVE(unknown_incoming,entry);
entry->sndbuf = itf->sndbuf;
set_sndbuf(vcc);
}
else if (entry->ip && entry->ip != ip && (entry->flags & ATF_PERM)
&& !(entry->flags & ATF_ARPSRV)) {
diag(COMPONENT,DIAG_ERROR,"ignoring attempt to change IP "
"address of permanent entry (to %d.%d.%d.%d)",ipp[0],
ipp[1],ipp[2],ipp[3]);
return result;
}
}
else {
entry = alloc_entry(1);
entry->flags = ATF_PUBL;
}
}
if (!atmsvc_addr_in_use(*addr)) addr = NULL;
if (entry->addr && addr && (entry->flags & ATF_PERM) &&
!atm_equal((struct sockaddr *) entry->addr,(struct sockaddr *) addr,0,0))
{
diag(COMPONENT,DIAG_ERROR,"ignoring attempt to change ATM address of "
"permanent entry");
return result;
}
if (entry->state == as_valid && entry->ip == ip && (!addr || (entry->addr
&& atm_equal((struct sockaddr *) entry->addr,(struct sockaddr *) addr,0,
0)))) return result; /* no news */
STOP_TIMER(entry);
if (entry->ip != ip) send_notifications(entry,0);
entry->ip = ip;
if (!entry->itf) {
entry->itf = itf;
/* @@@
* Need to fix this is in case we allow entries without a valid IP
* address but with a pre-set QOS, e.g. a VC on a given PVC with an
* unknown remote end.
*/
entry->qos = entry->itf->qos;
adjust_qos(entry->itf,&entry->qos,0);
Q_INSERT_HEAD(itf->table,entry);
}
if (entry->itf != itf)
diag(COMPONENT,DIAG_ERROR,"interesting, interface has changed ... "
"(%d -> %d)",entry->itf->number,itf->number);
if (addr) {
if (!entry->addr) entry->addr = alloc(sizeof(*addr));
*entry->addr = *addr;
if (merge) {
ENTRY *incoming;
while ((incoming = lookup_incoming(addr))) {
STOP_TIMER(incoming);
Q_REMOVE(unknown_incoming,incoming);
incoming->vccs->entry = entry;
Q_INSERT_HEAD(entry->vccs,incoming->vccs);
set_sndbuf(incoming->vccs);
free(incoming);
}
}
}
for (walk = entry->vccs; walk; walk = next) {
next = walk->next;
if (!walk->connecting)
if (set_ip(walk->fd,ip) < 0) {
diag(COMPONENT,DIAG_ERROR,"set_ip: %s",strerror(errno));
disconnect_vcc(walk);
if (walk == vcc) result = -1;
}
}
if (entry->state != as_valid) {
if (!entry->vccs && itf->arp_srv && !(entry->flags & ATF_NOVC))
connect_me(entry);
send_notifications(entry,1);
}
if ((entry->flags & ATF_ARPSRV) || !(entry->flags & ATF_PERM)) {
if (entry->itf->arp_srv) START_TIMER(entry,CREVAL);
else START_TIMER(entry,SREVAL);
}
entry->state = as_valid;
return result;
}
