|
|
|
@ -47,7 +47,6 @@
|
|
|
|
|
#include <errno.h>
|
|
|
|
|
#include "common.h"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* Global data */
|
|
|
|
|
_st_vp_t _st_this_vp; /* This VP */
|
|
|
|
|
_st_thread_t *_st_this_thread; /* Current thread */
|
|
|
|
@ -56,7 +55,6 @@ int _st_active_count = 0; /* Active thread count */
|
|
|
|
|
time_t _st_curr_time = 0; /* Current time as returned by time(2) */
|
|
|
|
|
st_utime_t _st_last_tset; /* Last time it was fetched */
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int st_poll(struct pollfd *pds, int npds, st_utime_t timeout)
|
|
|
|
|
{
|
|
|
|
|
struct pollfd *pd;
|
|
|
|
@ -71,16 +69,18 @@ int st_poll(struct pollfd *pds, int npds, st_utime_t timeout)
|
|
|
|
|
return -1;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ((*_st_eventsys->pollset_add)(pds, npds) < 0)
|
|
|
|
|
if ((*_st_eventsys->pollset_add)(pds, npds) < 0) {
|
|
|
|
|
return -1;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
pq.pds = pds;
|
|
|
|
|
pq.npds = npds;
|
|
|
|
|
pq.thread = me;
|
|
|
|
|
pq.on_ioq = 1;
|
|
|
|
|
_ST_ADD_IOQ(pq);
|
|
|
|
|
if (timeout != ST_UTIME_NO_TIMEOUT)
|
|
|
|
|
if (timeout != ST_UTIME_NO_TIMEOUT) {
|
|
|
|
|
_ST_ADD_SLEEPQ(me, timeout);
|
|
|
|
|
}
|
|
|
|
|
me->state = _ST_ST_IO_WAIT;
|
|
|
|
|
|
|
|
|
|
_ST_SWITCH_CONTEXT(me);
|
|
|
|
@ -93,10 +93,11 @@ int st_poll(struct pollfd *pds, int npds, st_utime_t timeout)
|
|
|
|
|
} else {
|
|
|
|
|
/* Count the number of ready descriptors */
|
|
|
|
|
for (pd = pds; pd < epd; pd++) {
|
|
|
|
|
if (pd->revents)
|
|
|
|
|
if (pd->revents) {
|
|
|
|
|
n++;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (me->flags & _ST_FL_INTERRUPT) {
|
|
|
|
|
me->flags &= ~_ST_FL_INTERRUPT;
|
|
|
|
@ -107,7 +108,6 @@ int st_poll(struct pollfd *pds, int npds, st_utime_t timeout)
|
|
|
|
|
return n;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void _st_vp_schedule(void)
|
|
|
|
|
{
|
|
|
|
|
_st_thread_t *thread;
|
|
|
|
@ -127,7 +127,6 @@ void _st_vp_schedule(void)
|
|
|
|
|
_ST_RESTORE_CONTEXT(thread);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Initialize this Virtual Processor
|
|
|
|
|
*/
|
|
|
|
@ -143,8 +142,9 @@ int st_init(void)
|
|
|
|
|
/* We can ignore return value here */
|
|
|
|
|
st_set_eventsys(ST_EVENTSYS_DEFAULT);
|
|
|
|
|
|
|
|
|
|
if (_st_io_init() < 0)
|
|
|
|
|
if (_st_io_init() < 0) {
|
|
|
|
|
return -1;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
memset(&_st_this_vp, 0, sizeof(_st_vp_t));
|
|
|
|
|
|
|
|
|
@ -155,8 +155,9 @@ int st_init(void)
|
|
|
|
|
ST_INIT_CLIST(&_ST_THREADQ);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
if ((*_st_eventsys->init)() < 0)
|
|
|
|
|
if ((*_st_eventsys->init)() < 0) {
|
|
|
|
|
return -1;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_st_this_vp.pagesize = getpagesize();
|
|
|
|
|
_st_this_vp.last_clock = st_utime();
|
|
|
|
@ -164,10 +165,10 @@ int st_init(void)
|
|
|
|
|
/*
|
|
|
|
|
* Create idle thread
|
|
|
|
|
*/
|
|
|
|
|
_st_this_vp.idle_thread = st_thread_create(_st_idle_thread_start,
|
|
|
|
|
NULL, 0, 0);
|
|
|
|
|
if (!_st_this_vp.idle_thread)
|
|
|
|
|
_st_this_vp.idle_thread = st_thread_create(_st_idle_thread_start, NULL, 0, 0);
|
|
|
|
|
if (!_st_this_vp.idle_thread) {
|
|
|
|
|
return -1;
|
|
|
|
|
}
|
|
|
|
|
_st_this_vp.idle_thread->flags = _ST_FL_IDLE_THREAD;
|
|
|
|
|
_st_active_count--;
|
|
|
|
|
_ST_DEL_RUNQ(_st_this_vp.idle_thread);
|
|
|
|
@ -177,8 +178,9 @@ int st_init(void)
|
|
|
|
|
*/
|
|
|
|
|
thread = (_st_thread_t *) calloc(1, sizeof(_st_thread_t) +
|
|
|
|
|
(ST_KEYS_MAX * sizeof(void *)));
|
|
|
|
|
if (!thread)
|
|
|
|
|
if (!thread) {
|
|
|
|
|
return -1;
|
|
|
|
|
}
|
|
|
|
|
thread->private_data = (void **) (thread + 1);
|
|
|
|
|
thread->state = _ST_ST_RUNNING;
|
|
|
|
|
thread->flags = _ST_FL_PRIMORDIAL;
|
|
|
|
@ -191,7 +193,6 @@ int st_init(void)
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#ifdef ST_SWITCH_CB
|
|
|
|
|
st_switch_cb_t st_set_switch_in_cb(st_switch_cb_t cb)
|
|
|
|
|
{
|
|
|
|
@ -208,7 +209,6 @@ st_switch_cb_t st_set_switch_out_cb(st_switch_cb_t cb)
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Start function for the idle thread
|
|
|
|
|
*/
|
|
|
|
@ -235,7 +235,6 @@ void *_st_idle_thread_start(void *arg)
|
|
|
|
|
return NULL;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void st_thread_exit(void *retval)
|
|
|
|
|
{
|
|
|
|
|
_st_thread_t *thread = _ST_CURRENT_THREAD();
|
|
|
|
@ -263,15 +262,15 @@ void st_thread_exit(void *retval)
|
|
|
|
|
_ST_DEL_THREADQ(thread);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
if (!(thread->flags & _ST_FL_PRIMORDIAL))
|
|
|
|
|
if (!(thread->flags & _ST_FL_PRIMORDIAL)) {
|
|
|
|
|
_st_stack_free(thread->stack);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Find another thread to run */
|
|
|
|
|
_ST_SWITCH_CONTEXT(thread);
|
|
|
|
|
/* Not going to land here */
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int st_thread_join(_st_thread_t *thread, void **retvalp)
|
|
|
|
|
{
|
|
|
|
|
_st_cond_t *term = thread->term;
|
|
|
|
@ -293,12 +292,14 @@ int st_thread_join(_st_thread_t *thread, void **retvalp)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
while (thread->state != _ST_ST_ZOMBIE) {
|
|
|
|
|
if (st_cond_timedwait(term, ST_UTIME_NO_TIMEOUT) != 0)
|
|
|
|
|
if (st_cond_timedwait(term, ST_UTIME_NO_TIMEOUT) != 0) {
|
|
|
|
|
return -1;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (retvalp)
|
|
|
|
|
if (retvalp) {
|
|
|
|
|
*retvalp = thread->retval;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Remove target thread from the zombie queue and make it runnable.
|
|
|
|
@ -311,7 +312,6 @@ int st_thread_join(_st_thread_t *thread, void **retvalp)
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void _st_thread_main(void)
|
|
|
|
|
{
|
|
|
|
|
_st_thread_t *thread = _ST_CURRENT_THREAD();
|
|
|
|
@ -330,13 +330,13 @@ void _st_thread_main(void)
|
|
|
|
|
st_thread_exit(thread->retval);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Insert "thread" into the timeout heap, in the position
|
|
|
|
|
* specified by thread->heap_index. See docs/timeout_heap.txt
|
|
|
|
|
* for details about the timeout heap.
|
|
|
|
|
*/
|
|
|
|
|
static _st_thread_t **heap_insert(_st_thread_t *thread) {
|
|
|
|
|
static _st_thread_t **heap_insert(_st_thread_t *thread)
|
|
|
|
|
{
|
|
|
|
|
int target = thread->heap_index;
|
|
|
|
|
int s = target;
|
|
|
|
|
_st_thread_t **p = &_ST_SLEEPQ;
|
|
|
|
@ -348,6 +348,7 @@ static _st_thread_t **heap_insert(_st_thread_t *thread) {
|
|
|
|
|
s >>= 1;
|
|
|
|
|
bits++;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
for (bit = bits - 2; bit >= 0; bit--) {
|
|
|
|
|
if (thread->due < (*p)->due) {
|
|
|
|
|
_st_thread_t *t = *p;
|
|
|
|
@ -365,17 +366,19 @@ static _st_thread_t **heap_insert(_st_thread_t *thread) {
|
|
|
|
|
p = &((*p)->left);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
thread->heap_index = index;
|
|
|
|
|
*p = thread;
|
|
|
|
|
thread->left = thread->right = NULL;
|
|
|
|
|
|
|
|
|
|
return p;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Delete "thread" from the timeout heap.
|
|
|
|
|
*/
|
|
|
|
|
static void heap_delete(_st_thread_t *thread) {
|
|
|
|
|
static void heap_delete(_st_thread_t *thread)
|
|
|
|
|
{
|
|
|
|
|
_st_thread_t *t, **p;
|
|
|
|
|
int bits = 0;
|
|
|
|
|
int s, bit;
|
|
|
|
@ -387,6 +390,7 @@ static void heap_delete(_st_thread_t *thread) {
|
|
|
|
|
s >>= 1;
|
|
|
|
|
bits++;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
for (bit = bits - 2; bit >= 0; bit--) {
|
|
|
|
|
if (_ST_SLEEPQ_SIZE & (1 << bit)) {
|
|
|
|
|
p = &((*p)->right);
|
|
|
|
@ -394,6 +398,7 @@ static void heap_delete(_st_thread_t *thread) {
|
|
|
|
|
p = &((*p)->left);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
t = *p;
|
|
|
|
|
*p = NULL;
|
|
|
|
|
--_ST_SLEEPQ_SIZE;
|
|
|
|
@ -413,14 +418,17 @@ static void heap_delete(_st_thread_t *thread) {
|
|
|
|
|
for (;;) {
|
|
|
|
|
_st_thread_t *y; /* The younger child */
|
|
|
|
|
int index_tmp;
|
|
|
|
|
if (t->left == NULL)
|
|
|
|
|
|
|
|
|
|
if (t->left == NULL) {
|
|
|
|
|
break;
|
|
|
|
|
else if (t->right == NULL)
|
|
|
|
|
} else if (t->right == NULL) {
|
|
|
|
|
y = t->left;
|
|
|
|
|
else if (t->left->due < t->right->due)
|
|
|
|
|
} else if (t->left->due < t->right->due) {
|
|
|
|
|
y = t->left;
|
|
|
|
|
else
|
|
|
|
|
} else {
|
|
|
|
|
y = t->right;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (t->due > y->due) {
|
|
|
|
|
_st_thread_t *tl = y->left;
|
|
|
|
|
_st_thread_t *tr = y->right;
|
|
|
|
@ -444,10 +452,10 @@ static void heap_delete(_st_thread_t *thread) {
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
thread->left = thread->right = NULL;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void _st_add_sleep_q(_st_thread_t *thread, st_utime_t timeout)
|
|
|
|
|
{
|
|
|
|
|
thread->due = _ST_LAST_CLOCK + timeout;
|
|
|
|
@ -456,14 +464,12 @@ void _st_add_sleep_q(_st_thread_t *thread, st_utime_t timeout)
|
|
|
|
|
heap_insert(thread);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void _st_del_sleep_q(_st_thread_t *thread)
|
|
|
|
|
{
|
|
|
|
|
heap_delete(thread);
|
|
|
|
|
thread->flags &= ~_ST_FL_ON_SLEEPQ;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void _st_vp_check_clock(void)
|
|
|
|
|
{
|
|
|
|
|
_st_thread_t *thread;
|
|
|
|
@ -481,13 +487,15 @@ void _st_vp_check_clock(void)
|
|
|
|
|
while (_ST_SLEEPQ != NULL) {
|
|
|
|
|
thread = _ST_SLEEPQ;
|
|
|
|
|
ST_ASSERT(thread->flags & _ST_FL_ON_SLEEPQ);
|
|
|
|
|
if (thread->due > now)
|
|
|
|
|
if (thread->due > now) {
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
_ST_DEL_SLEEPQ(thread);
|
|
|
|
|
|
|
|
|
|
/* If thread is waiting on condition variable, set the time out flag */
|
|
|
|
|
if (thread->state == _ST_ST_COND_WAIT)
|
|
|
|
|
if (thread->state == _ST_ST_COND_WAIT) {
|
|
|
|
|
thread->flags |= _ST_FL_TIMEDOUT;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Make thread runnable */
|
|
|
|
|
ST_ASSERT(!(thread->flags & _ST_FL_IDLE_THREAD));
|
|
|
|
@ -496,29 +504,29 @@ void _st_vp_check_clock(void)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void st_thread_interrupt(_st_thread_t *thread)
|
|
|
|
|
{
|
|
|
|
|
/* If thread is already dead */
|
|
|
|
|
if (thread->state == _ST_ST_ZOMBIE)
|
|
|
|
|
if (thread->state == _ST_ST_ZOMBIE) {
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
thread->flags |= _ST_FL_INTERRUPT;
|
|
|
|
|
|
|
|
|
|
if (thread->state == _ST_ST_RUNNING || thread->state == _ST_ST_RUNNABLE)
|
|
|
|
|
if (thread->state == _ST_ST_RUNNING || thread->state == _ST_ST_RUNNABLE) {
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (thread->flags & _ST_FL_ON_SLEEPQ)
|
|
|
|
|
if (thread->flags & _ST_FL_ON_SLEEPQ) {
|
|
|
|
|
_ST_DEL_SLEEPQ(thread);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Make thread runnable */
|
|
|
|
|
thread->state = _ST_ST_RUNNABLE;
|
|
|
|
|
_ST_ADD_RUNQ(thread);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
_st_thread_t *st_thread_create(void *(*start)(void *arg), void *arg,
|
|
|
|
|
int joinable, int stk_size)
|
|
|
|
|
_st_thread_t *st_thread_create(void *(*start)(void *arg), void *arg, int joinable, int stk_size)
|
|
|
|
|
{
|
|
|
|
|
_st_thread_t *thread;
|
|
|
|
|
_st_stack_t *stack;
|
|
|
|
@ -529,17 +537,19 @@ _st_thread_t *st_thread_create(void *(*start)(void *arg), void *arg,
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
/* Adjust stack size */
|
|
|
|
|
if (stk_size == 0)
|
|
|
|
|
if (stk_size == 0) {
|
|
|
|
|
stk_size = ST_DEFAULT_STACK_SIZE;
|
|
|
|
|
}
|
|
|
|
|
stk_size = ((stk_size + _ST_PAGE_SIZE - 1) / _ST_PAGE_SIZE) * _ST_PAGE_SIZE;
|
|
|
|
|
stack = _st_stack_new(stk_size);
|
|
|
|
|
if (!stack)
|
|
|
|
|
if (!stack) {
|
|
|
|
|
return NULL;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Allocate thread object and per-thread data off the stack */
|
|
|
|
|
#if defined (MD_STACK_GROWS_DOWN)
|
|
|
|
|
sp = stack->stk_top;
|
|
|
|
|
#ifdef __ia64__
|
|
|
|
|
#ifdef __ia64__
|
|
|
|
|
/*
|
|
|
|
|
* The stack segment is split in the middle. The upper half is used
|
|
|
|
|
* as backing store for the register stack which grows upward.
|
|
|
|
@ -550,18 +560,20 @@ _st_thread_t *st_thread_create(void *(*start)(void *arg), void *arg,
|
|
|
|
|
sp -= (stk_size >> 1);
|
|
|
|
|
bsp = sp;
|
|
|
|
|
/* Make register stack 64-byte aligned */
|
|
|
|
|
if ((unsigned long)bsp & 0x3f)
|
|
|
|
|
if ((unsigned long)bsp & 0x3f) {
|
|
|
|
|
bsp = bsp + (0x40 - ((unsigned long)bsp & 0x3f));
|
|
|
|
|
}
|
|
|
|
|
stack->bsp = bsp + _ST_STACK_PAD_SIZE;
|
|
|
|
|
#endif
|
|
|
|
|
#endif
|
|
|
|
|
sp = sp - (ST_KEYS_MAX * sizeof(void *));
|
|
|
|
|
ptds = (void **) sp;
|
|
|
|
|
sp = sp - sizeof(_st_thread_t);
|
|
|
|
|
thread = (_st_thread_t *) sp;
|
|
|
|
|
|
|
|
|
|
/* Make stack 64-byte aligned */
|
|
|
|
|
if ((unsigned long)sp & 0x3f)
|
|
|
|
|
if ((unsigned long)sp & 0x3f) {
|
|
|
|
|
sp = sp - ((unsigned long)sp & 0x3f);
|
|
|
|
|
}
|
|
|
|
|
stack->sp = sp - _ST_STACK_PAD_SIZE;
|
|
|
|
|
#elif defined (MD_STACK_GROWS_UP)
|
|
|
|
|
sp = stack->stk_bottom;
|
|
|
|
@ -571,11 +583,12 @@ _st_thread_t *st_thread_create(void *(*start)(void *arg), void *arg,
|
|
|
|
|
sp = sp + (ST_KEYS_MAX * sizeof(void *));
|
|
|
|
|
|
|
|
|
|
/* Make stack 64-byte aligned */
|
|
|
|
|
if ((unsigned long)sp & 0x3f)
|
|
|
|
|
if ((unsigned long)sp & 0x3f) {
|
|
|
|
|
sp = sp + (0x40 - ((unsigned long)sp & 0x3f));
|
|
|
|
|
}
|
|
|
|
|
stack->sp = sp + _ST_STACK_PAD_SIZE;
|
|
|
|
|
#else
|
|
|
|
|
#error Unknown OS
|
|
|
|
|
#error Unknown OS
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
memset(thread, 0, sizeof(_st_thread_t));
|
|
|
|
@ -613,18 +626,15 @@ _st_thread_t *st_thread_create(void *(*start)(void *arg), void *arg,
|
|
|
|
|
return thread;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
_st_thread_t *st_thread_self(void)
|
|
|
|
|
{
|
|
|
|
|
return _ST_CURRENT_THREAD();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#ifdef DEBUG
|
|
|
|
|
/* ARGSUSED */
|
|
|
|
|
void _st_show_thread_stack(_st_thread_t *thread, const char *messg)
|
|
|
|
|
{
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* To be set from debugger */
|
|
|
|
@ -659,12 +669,14 @@ void _st_iterate_threads(void)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
q = thread->tlink.next;
|
|
|
|
|
if (q == &_ST_THREADQ)
|
|
|
|
|
if (q == &_ST_THREADQ) {
|
|
|
|
|
q = q->next;
|
|
|
|
|
}
|
|
|
|
|
ST_ASSERT(q != &_ST_THREADQ);
|
|
|
|
|
thread = _ST_THREAD_THREADQ_PTR(q);
|
|
|
|
|
if (thread == _ST_CURRENT_THREAD())
|
|
|
|
|
if (thread == _ST_CURRENT_THREAD()) {
|
|
|
|
|
MD_LONGJMP(orig_jb, 1);
|
|
|
|
|
}
|
|
|
|
|
memcpy(save_jb, thread->context, sizeof(jmp_buf));
|
|
|
|
|
MD_LONGJMP(thread->context, 1);
|
|
|
|
|
}
|
|
|
|
|