void _st_vp_schedule(void)
{
st_thread_t *thread;
while (_ST_RUNQ.next != &_ST_RUNQ) {
/* Pull thread off of the run queue */
thread = _ST_THREAD_PTR(_ST_RUNQ.next);
_ST_DEL_RUNQ(thread);
ST_ASSERT(thread->state == _ST_ST_RUNNABLE);
/* Resume the thread */
thread->state = _ST_ST_RUNNING;
_ST_RESTORE_CONTEXT(thread);
}
{
/* If there are no threads to run, switch to the idle thread */
thread = _st_this_vp.idle_thread;
ST_ASSERT(thread->state == _ST_ST_RUNNABLE);
thread->state = _ST_ST_RUNNING;
_ST_RESTORE_CONTEXT(thread);
/* Resume the thread */
}
}
/**************************************************************************************************
* @fn HalMagTest
*
* @brief Run a sensor self-test
*
* @return TRUE if passed, FALSE if failed
**************************************************************************************************/
bool HalMagTest(void)
{
uint8 val;
// Select this sensor on the I2C bus
HalMagSelect();
// Check who-am-i register
ST_ASSERT(HalSensorReadReg(MAG_REG_ADDR_WHO_AM_I, &val, sizeof(val)));
ST_ASSERT(val==WHO_AM_I_VALUE);
// Check CTRL_REG1 in standby mode
ST_ASSERT(HalSensorReadReg(MAG_REG_ADDR_CTRL_1, &val, sizeof(val)));
ST_ASSERT(val==MAG_REG_CTRL_OFF);
// Check that CTRL_REG1 can be written
val = CTRL1_TEST_VALUE;
ST_ASSERT(HalSensorWriteReg(MAG_REG_ADDR_CTRL_1, &val, sizeof(val)));
ST_ASSERT(HalSensorReadReg(MAG_REG_ADDR_CTRL_1, &val, sizeof(val)));
ST_ASSERT(val==CTRL1_TEST_VALUE);
// Restore default value
val = MAG_REG_CTRL_OFF;
ST_ASSERT(HalSensorWriteReg(MAG_REG_ADDR_CTRL_1, &val, sizeof(val)));
// Check that the sensor is in standby mode
ST_ASSERT(HalSensorReadReg(MAG_REG_ADDR_SYSMOD, &val, sizeof(val)));
ST_ASSERT(val==0);
return TRUE;
}
void _st_vp_check_clock(void)
{
_st_thread_t *thread;
st_utime_t elapsed, now;
now = st_utime();
elapsed = now - _ST_LAST_CLOCK;
_ST_LAST_CLOCK = now;
if (_st_curr_time && now - _st_last_tset > 999000) {
_st_curr_time = time(NULL);
_st_last_tset = now;
}
while (_ST_SLEEPQ != NULL) {
thread = _ST_SLEEPQ;
ST_ASSERT(thread->flags & _ST_FL_ON_SLEEPQ);
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)
thread->flags |= _ST_FL_TIMEDOUT;
/* Make thread runnable */
ST_ASSERT(!(thread->flags & _ST_FL_IDLE_THREAD));
thread->state = _ST_ST_RUNNABLE;
_ST_ADD_RUNQ(thread);
}
}
_st_netfd_t *st_accept(_st_netfd_t *fd, struct sockaddr *addr, int *addrlen, st_utime_t timeout)
{
int osfd, err;
_st_netfd_t *newfd;
_st_netfd_t **p = (_st_netfd_t **) fd->aux_data;
ssize_t n;
char c;
for ( ; ; ) {
if (p == NULL) {
osfd = accept(fd->osfd, addr, (socklen_t *)addrlen);
} else {
/* Get the lock */
n = st_read(p[0], &c, 1, timeout);
if (n < 0) {
return NULL;
}
ST_ASSERT(n == 1);
/* Got the lock */
osfd = accept(fd->osfd, addr, (socklen_t *)addrlen);
/* Unlock */
err = errno;
n = st_write(p[1], &c, 1, timeout);
ST_ASSERT(n == 1);
errno = err;
}
if (osfd >= 0) {
break;
}
if (errno == EINTR) {
continue;
}
if (!_IO_NOT_READY_ERROR) {
return NULL;
}
/* Wait until the socket becomes readable */
if (st_netfd_poll(fd, POLLIN, timeout) < 0) {
return NULL;
}
}
/* On some platforms the new socket created by accept() inherits */
/* the nonblocking attribute of the listening socket */
#if defined (MD_ACCEPT_NB_INHERITED)
newfd = _st_netfd_new(osfd, 0, 1);
#elif defined (MD_ACCEPT_NB_NOT_INHERITED)
newfd = _st_netfd_new(osfd, 1, 1);
#else
#error Unknown OS
#endif
if (!newfd) {
err = errno;
close(osfd);
errno = err;
}
return newfd;
}
/**************************************************************************************************
* @fn sensorMpu6500Test
*
* @brief Run a sensor self-test
*
* @return TRUE if passed, FALSE if failed
*/
bool sensorMpu6500Test(void)
{
// Select this sensor on the I2C bus
sensorMpu6500Select();
// Check the WHO AM I register
ST_ASSERT(sensorReadReg(WHO_AM_I, &val, 1));
ST_ASSERT(val == 0x70);
return true;
}
/**************************************************************************************************
* @fn HalHumiTest
*
* @brief Humidity sensor self test
*
* @return none
**************************************************************************************************/
bool HalHumiTest(void)
{
uint8 val;
HalHumiSelect();
// Verify default value
ST_ASSERT(HalSensorReadReg(SHT21_CMD_READ_U_R,&val,1));
ST_ASSERT(val==usr);
return TRUE;
}
/**
* Return color conversion shader part from StImage definition.
*/
static inline StGLImageProgram::FragToRgb getColorShader(const StImage::ImgColorModel theColorModel,
const StImage::ImgColorScale theColorScale) {
switch(theColorModel) {
case StImage::ImgColor_RGB: return StGLImageProgram::FragToRgb_FromRgb;
case StImage::ImgColor_RGBA: return StGLImageProgram::FragToRgb_FromRgba;
case StImage::ImgColor_GRAY: return StGLImageProgram::FragToRgb_FromGray;
case StImage::ImgColor_XYZ: return StGLImageProgram::FragToRgb_FromXyz;
case StImage::ImgColor_YUV: {
switch(theColorScale) {
case StImage::ImgScale_Mpeg9: return StGLImageProgram::FragToRgb_FromYuv9Mpeg;
case StImage::ImgScale_Mpeg10: return StGLImageProgram::FragToRgb_FromYuv10Mpeg;
case StImage::ImgScale_Jpeg9: return StGLImageProgram::FragToRgb_FromYuv9Full;
case StImage::ImgScale_Jpeg10: return StGLImageProgram::FragToRgb_FromYuv10Full;
case StImage::ImgScale_Mpeg: return StGLImageProgram::FragToRgb_FromYuvMpeg;
case StImage::ImgScale_Full: return StGLImageProgram::FragToRgb_FromYuvFull;
case StImage::ImgScale_NvMpeg: return StGLImageProgram::FragToRgb_FromYuvNvMpeg;
case StImage::ImgScale_NvFull: return StGLImageProgram::FragToRgb_FromYuvNvFull;
}
return StGLImageProgram::FragToRgb_FromYuvFull;
}
default: {
ST_DEBUG_LOG("No GLSL shader for this color model = " + theColorModel);
ST_ASSERT(false, "StGLImageProgram::getColorShader() - unsupported color model!");
}
}
return StGLImageProgram::FragToRgb_FromRgb;
}
StGLFrameTextures::~StGLFrameTextures() {
ST_ASSERT(!myTextures[0].isValid()
&& !myTextures[1].isValid()
&& !myTextures[2].isValid()
&& !myTextures[3].isValid(),
"~StGLFrameTextures() with unreleased GL resources");
}
/**************************************************************************************************
* @fn sensorSht21Test
*
* @brief Humidity sensor self test
*
* @return none
**************************************************************************************************/
bool sensorSht21Test(void)
{
uint8_t val;
SENSOR_SELECT();
// Verify write and read
val = USR_REG_TEST_VAL;
ST_ASSERT(sensorWriteReg(SHT21_CMD_WRITE_U_R,&val,1));
ST_ASSERT(sensorReadReg(SHT21_CMD_READ_U_R,&val,1));
ST_ASSERT(val == USR_REG_TEST_VAL);
SENSOR_DESELECT();
return true;
}
/*
* If "expired" is true, a sleeper has timed out
*/
void _st_del_sleep_q(st_thread_t *thread, int expired)
{
st_clist_t *q;
st_thread_t *t;
/* Remove from sleep queue */
ST_ASSERT(thread->flags & _ST_FL_ON_SLEEPQ);
q = thread->links.next;
if (q != &_ST_SLEEPQ) {
if (expired) {
_ST_SLEEPQMAX -= thread->sleep;
}
else {
t = _ST_THREAD_PTR(q);
t->sleep += thread->sleep;
}
}
else {
/*
* Check if prev is the beginning of the list; if so,
* we are the only element on the list.
*/
if (thread->links.prev != &_ST_SLEEPQ)
_ST_SLEEPQMAX -= thread->sleep;
else
_ST_SLEEPQMAX = 0;
}
thread->flags &= ~_ST_FL_ON_SLEEPQ;
ST_REMOVE_LINK(&thread->links);
}
ST_HIDDEN void _st_kq_addlist_add(const struct kevent *kev)
{
ST_ASSERT(_st_kq_data->addlist_cnt < _st_kq_data->addlist_size);
memcpy(_st_kq_data->addlist + _st_kq_data->addlist_cnt, kev,
sizeof(struct kevent));
_st_kq_data->addlist_cnt++;
}
bool StLibrary::loadSimple(const StString& thePath) {
// this is probably some logical error in the code if close() wasn't explicitly called before!
ST_ASSERT(!isOpened(), "StLibrary::load() - library is already opened!");
close();
myPath = thePath;
myLibH = DLibLoadFull(myPath);
return isOpened();
}
ST_LOCAL StGLMenuActionItem(StGLMenu* theParent,
const StHandle<StAction>& theAction,
StGLMenu* theSubMenu)
: StGLMenuItem(theParent, 0, 0, theSubMenu),
myAction(theAction) {
ST_ASSERT(!theAction.isNull(), "StGLMenuActionItem - Unexpected empty action makes no sense!");
StGLMenuItem::signals.onItemClick.connect(this, &StGLMenuActionItem::doItemClick);
}
StGLMesh::~StGLMesh() {
ST_ASSERT(!myVertexBuf.isValid()
&& !myNormalBuf.isValid()
&& !myTCoordBuf.isValid()
&& !myColorsBuf.isValid()
&& !myIndexBuf.isValid(),
"~StGLMesh() with unreleased GL resources");
}
StGLStereoFrameBuffer::~StGLStereoFrameBuffer() {
ST_ASSERT(myGLFBufferIds[StGLStereoTexture::LEFT_TEXTURE] == StGLFrameBuffer::NO_FRAMEBUFFER
&& myGLFBufferIds[StGLStereoTexture::RIGHT_TEXTURE] == StGLFrameBuffer::NO_FRAMEBUFFER
&& myGLDepthRBIds[StGLStereoTexture::LEFT_TEXTURE] == StGLFrameBuffer::NO_RENDERBUFFER
&& myGLDepthRBIds[StGLStereoTexture::RIGHT_TEXTURE] == StGLFrameBuffer::NO_RENDERBUFFER
&& !myVerticesBuf.isValid()
&& !myTexCoordBuf.isValid(),
"~StGLStereoFrameBuffer() with unreleased GL resources");
}
/*******************************************************************************
* @fn sensorHdc1000Test
*
* @brief Humidity sensor self test
*
* @return true if test passes
*******************************************************************************/
bool sensorHdc1000Test(void)
{
uint16_t val;
SENSOR_SELECT();
// Verify manufacturer ID
ST_ASSERT(sensorReadReg(HDC1000_REG_MANF_ID,(uint8_t*)&val,2));
val = SWAP(val);
ST_ASSERT(val == HDC1000_VAL_MANF_ID);
// Verify device ID
ST_ASSERT(sensorReadReg(HDC1000_REG_DEV_ID,(uint8_t*)&val,2));
val = SWAP(val);
ST_ASSERT(val == HDC1000_VAL_DEV_ID);
SENSOR_DESELECT();
return true;
}
void _st_add_sleep_q(st_thread_t *thread, st_utime_t timeout)
{
st_utime_t sleep;
st_clist_t *q;
st_thread_t *t;
/* sort onto global sleep queue */
sleep = timeout;
/* Check if we are longest timeout */
if (timeout >= _ST_SLEEPQMAX) {
ST_APPEND_LINK(&thread->links, &_ST_SLEEPQ);
thread->sleep = timeout - _ST_SLEEPQMAX;
_ST_SLEEPQMAX = timeout;
}
else {
/* Sort thread into global sleep queue at appropriate point */
q = _ST_SLEEPQ.next;
/* Now scan the list for where to insert this entry */
while (q != &_ST_SLEEPQ) {
t = _ST_THREAD_PTR(q);
if (sleep < t->sleep) {
/* Found sleeper to insert in front of */
break;
}
sleep -= t->sleep;
q = q->next;
}
thread->sleep = sleep;
ST_INSERT_BEFORE(&thread->links, q);
/* Subtract our sleep time from the sleeper that follows us */
ST_ASSERT(thread->links.next != &_ST_SLEEPQ);
t = _ST_THREAD_PTR(thread->links.next);
ST_ASSERT(_ST_THREAD_PTR(t->links.prev) == thread);
t->sleep -= sleep;
}
thread->flags |= _ST_FL_ON_SLEEPQ;
}
/*******************************************************************************
* @fn SensorHdc1080_test
*
* @brief Humidity sensor self test
*
* @return true if test passes
*******************************************************************************/
bool SensorHdc1080_test(void)
{
uint16_t val;
SENSOR_SELECT();
SensorI2C_writeReg(HDC1080_REG_MANF_ID, 0, 0);
SENSOR_DESELECT();
SENSOR_SELECT();
SensorI2C_read((uint8_t *)&val, 2);
SENSOR_DESELECT();
ST_ASSERT(val == HDC1080_VAL_MANF_ID);
SENSOR_SELECT();
SensorI2C_writeReg(HDC1080_REG_DEV_ID, 0, 0);
SENSOR_DESELECT();
SENSOR_SELECT();
SensorI2C_read((uint8_t *)&val, 2);
SENSOR_DESELECT();
ST_ASSERT(val == HDC1080_VAL_DEV_ID);
// // Verify manufacturer ID
// ST_ASSERT(SensorI2C_readReg(HDC1080_REG_MANF_ID,(uint8_t*)&val,2));
// val = SWAP(val);
// ST_ASSERT(val == HDC1080_VAL_MANF_ID);
// // Verify device ID
// ST_ASSERT(SensorI2C_readReg(HDC1080_REG_DEV_ID,(uint8_t*)&val,2));
// val = SWAP(val);
// ST_ASSERT(val == HDC1080_VAL_DEV_ID);
// SENSOR_DESELECT();
return true;
}
void _st_vp_check_clock(void)
{
st_thread_t *thread;
st_utime_t elapsed, now;
now = st_utime();
elapsed = now - _st_this_vp.last_clock;
_st_this_vp.last_clock = now;
if (_st_curr_time && now - _st_last_tset > 999000) {
_st_curr_time = time(NULL);
_st_last_tset = now;
}
while (_ST_SLEEPQ.next != &_ST_SLEEPQ) {
thread = _ST_THREAD_PTR(_ST_SLEEPQ.next);
ST_ASSERT(thread->flags & _ST_FL_ON_SLEEPQ);
if (elapsed < thread->sleep) {
thread->sleep -= elapsed;
_ST_SLEEPQMAX -= elapsed;
break;
}
_ST_DEL_SLEEPQ(thread, 1);
elapsed -= thread->sleep;
/* If thread is waiting on condition variable, set the time out flag */
if (thread->state == _ST_ST_COND_WAIT)
thread->flags |= _ST_FL_TIMEDOUT;
/* Make thread runnable */
ST_ASSERT(!(thread->flags & _ST_FL_IDLE_THREAD));
thread->state = _ST_ST_RUNNABLE;
_ST_ADD_RUNQ(thread);
}
}
bool StLibrary::load(const StString& thePath) {
// this is probably some logical error in the code if close() wasn't explicitly called before!
ST_ASSERT(!isOpened(), "StLibrary::load() - library is already opened!");
close();
StString aDinLibExt = StString(ST_DLIB_SUFFIX);
if(thePath.isEndsWithIgnoreCase(aDinLibExt)) {
// got the full path?
myPath = thePath;
myLibH = DLibLoadFull(myPath);
if(myLibH == NULL) {
// try to remove an extension
myPath = thePath.subString(0, thePath.getLength() - aDinLibExt.getLength());
myLibH = DLibLoad(myPath);
}
} else {
// got short name?
myPath = thePath;
myLibH = DLibLoad(myPath);
}
return isOpened();
}
void _st_iterate_threads(void)
{
static _st_thread_t *thread = NULL;
static jmp_buf orig_jb, save_jb;
_st_clist_t *q;
if (!_st_iterate_threads_flag) {
if (thread) {
memcpy(thread->context, save_jb, sizeof(jmp_buf));
MD_LONGJMP(orig_jb, 1);
}
return;
}
if (thread) {
memcpy(thread->context, save_jb, sizeof(jmp_buf));
_st_show_thread_stack(thread, NULL);
} else {
if (MD_SETJMP(orig_jb)) {
_st_iterate_threads_flag = 0;
thread = NULL;
_st_show_thread_stack(thread, "Iteration completed");
return;
}
thread = _ST_CURRENT_THREAD();
_st_show_thread_stack(thread, "Iteration started");
}
q = thread->tlink.next;
if (q == &_ST_THREADQ)
q = q->next;
ST_ASSERT(q != &_ST_THREADQ);
thread = _ST_THREAD_THREADQ_PTR(q);
if (thread == _ST_CURRENT_THREAD())
MD_LONGJMP(orig_jb, 1);
memcpy(save_jb, thread->context, sizeof(jmp_buf));
MD_LONGJMP(thread->context, 1);
}
ST_HIDDEN void _st_poll_dispatch(void)
{
int timeout, nfd;
_st_clist_t *q;
st_utime_t min_timeout;
_st_pollq_t *pq;
struct pollfd *pds, *epds, *pollfds;
/*
* Build up the array of struct pollfd to wait on.
* If existing array is not big enough, release it and allocate a new one.
*/
ST_ASSERT(_ST_POLL_OSFD_CNT >= 0);
if (_ST_POLL_OSFD_CNT > _ST_POLLFDS_SIZE) {
free(_ST_POLLFDS);
_ST_POLLFDS = (struct pollfd *) malloc((_ST_POLL_OSFD_CNT + 10) *
sizeof(struct pollfd));
ST_ASSERT(_ST_POLLFDS != NULL);
_ST_POLLFDS_SIZE = _ST_POLL_OSFD_CNT + 10;
}
pollfds = _ST_POLLFDS;
/* Gather all descriptors into one array */
for (q = _ST_IOQ.next; q != &_ST_IOQ; q = q->next) {
pq = _ST_POLLQUEUE_PTR(q);
memcpy(pollfds, pq->pds, sizeof(struct pollfd) * pq->npds);
pollfds += pq->npds;
}
ST_ASSERT(pollfds <= _ST_POLLFDS + _ST_POLLFDS_SIZE);
if (_ST_SLEEPQ == NULL) {
timeout = -1;
} else {
min_timeout = (_ST_SLEEPQ->due <= _ST_LAST_CLOCK) ? 0 :
(_ST_SLEEPQ->due - _ST_LAST_CLOCK);
timeout = (int) (min_timeout / 1000);
}
/* Check for I/O operations */
nfd = poll(_ST_POLLFDS, _ST_POLL_OSFD_CNT, timeout);
/* Notify threads that are associated with the selected descriptors */
if (nfd > 0) {
pollfds = _ST_POLLFDS;
for (q = _ST_IOQ.next; q != &_ST_IOQ; q = q->next) {
pq = _ST_POLLQUEUE_PTR(q);
epds = pollfds + pq->npds;
for (pds = pollfds; pds < epds; pds++) {
if (pds->revents)
break;
}
if (pds < epds) {
memcpy(pq->pds, pollfds, sizeof(struct pollfd) * pq->npds);
ST_REMOVE_LINK(&pq->links);
pq->on_ioq = 0;
if (pq->thread->flags & _ST_FL_ON_SLEEPQ)
_ST_DEL_SLEEPQ(pq->thread);
pq->thread->state = _ST_ST_RUNNABLE;
_ST_ADD_RUNQ(pq->thread);
_ST_POLL_OSFD_CNT -= pq->npds;
ST_ASSERT(_ST_POLL_OSFD_CNT >= 0);
}
pollfds = epds;
}
}
}
/**************************************************************************************************
* @fn HalIRTempTest
*
* @brief Run a sensor self-test
*
* @return TRUE if passed, FALSE if failed
**************************************************************************************************/
bool HalIRTempTest(void)
{
uint16 val;
// Select this sensor on the I2C bus
HalIRTempSelect();
// Check manufacturer ID
ST_ASSERT(HalSensorReadReg(TMP006_REG_MANF_ID, (uint8 *)&val, IRTEMP_REG_LEN));
val = (LO_UINT16(val) << 8) | HI_UINT16(val);
ST_ASSERT(val == TMP006_VAL_MANF_ID);
// Reset sensor
ST_ASSERT(HalSensorWriteReg(TMP006_REG_ADDR_CONFIG, configSensorReset, IRTEMP_REG_LEN));
// Check config register (reset)
ST_ASSERT(HalSensorReadReg(TMP006_REG_ADDR_CONFIG, (uint8 *)&val, IRTEMP_REG_LEN));
val = ((LO_UINT16(val) << 8) | HI_UINT16(val));
ST_ASSERT(val == TMP006_VAL_CONFIG_RESET);
// Turn sensor off
ST_ASSERT(HalSensorWriteReg(TMP006_REG_ADDR_CONFIG, configSensorOff,IRTEMP_REG_LEN));
// Check config register (off)
ST_ASSERT(HalSensorReadReg(TMP006_REG_ADDR_CONFIG, (uint8 *)&val, IRTEMP_REG_LEN));
val = ((LO_UINT16(val) << 8) | HI_UINT16(val));
ST_ASSERT(val == TMP006_VAL_CONFIG_OFF);
// Turn sensor on
ST_ASSERT(HalSensorWriteReg(TMP006_REG_ADDR_CONFIG, configSensorOn, IRTEMP_REG_LEN));
// Check config register (on)
ST_ASSERT(HalSensorReadReg(TMP006_REG_ADDR_CONFIG, (uint8 *)&val, IRTEMP_REG_LEN));
val = ((LO_UINT16(val) << 8) | HI_UINT16(val));
ST_ASSERT(val == TMP006_VAL_CONFIG_ON);
// Turn sensor off
ST_ASSERT(HalSensorWriteReg(TMP006_REG_ADDR_CONFIG, configSensorOff, IRTEMP_REG_LEN));
return TRUE;
}
StGLFontEntry::~StGLFontEntry() {
ST_ASSERT(myTextures.isEmpty(), "~StGLFontEntry() with unreleased GL resources");
}
StGLProgram::~StGLProgram() {
ST_ASSERT(!isValid(), "~StGLProgram with unreleased GL resources");
}
/* ARGSUSED */
ST_HIDDEN void _st_poll_pollset_del(struct pollfd *pds, int npds)
{
_ST_POLL_OSFD_CNT -= npds;
ST_ASSERT(_ST_POLL_OSFD_CNT >= 0);
}
