extern void eoprot_fun_ONSAY_mc(const EOnv* nv, const eOropdescriptor_t* rd)
{
// marco.accame on 18 mar 2014: this function is called when a say<id32, data> rop is received
// and the id32 is about the motion control endpoint. this function is common to every board.
// it is used this function and not another one because inside the hostTransceiver object it was called:
// eoprot_config_onsay_endpoint_set(eoprot_endpoint_motioncontrol, eoprot_fun_ONSAY_mc);
// the aim of this function is to wake up a thread which is blocked because it has sent an ask<id32>
// the wake up funtionality is implemented in two modes, depending on the wait mechanism used:
// a. in initialisation, embObjMotionControl sets some values and then reads them back.
// the read back sends an ask<id32, signature=0xaa000000>. in such a case the board sends back
// a say<id32, data, signature = 0xaa000000>. thus, if the received signature is 0xaa000000, then
// we must unblock using feat_signal_network_reply().
// b. during runtime, some methods send a blocking ask<id32> without signature. It is the case of instance
// of getPidRaw() which waits with a eoThreadEntry::synch() call. in such a case the board send back a
// normal say<id32, data> with nos signature. in this case we unlock with wake().
if(0xaa000000 == rd->signature)
{ // case a:
if(fakestdbool_false == feat_signal_network_reply(eo_nv_GetBRD(nv), rd->id32, rd->signature))
{
char str[256] = {0};
char nvinfo[128];
eoprot_ID2information(rd->id32, nvinfo, sizeof(nvinfo));
snprintf(str, sizeof(str), "eoprot_fun_ONSAY_mc() received an unexpected message w/ 0xaa000000 signature for %s", nvinfo);
embObjPrintWarning(str);
return;
}
}
else
{ //case b:
wake(nv);
}
}
atapi_regs[ATAPI_REG_CMDSTATUS] = ATAPI_STAT_DRDY;
atapi_regs[ATAPI_REG_INTREASON] = ATAPI_INTREASON_IO|ATAPI_INTREASON_COMMAND;
}
else
{
// indicate data ready: set DRQ and DMA ready, and IO in INTREASON
if (atapi_regs[ATAPI_REG_FEATURES] & 0x01) // DMA feature
{
atapi_regs[ATAPI_REG_CMDSTATUS] = ATAPI_STAT_BSY | ATAPI_STAT_DRDY | ATAPI_STAT_SERVDSC;
}
else
{
atapi_regs[ATAPI_REG_CMDSTATUS] = ATAPI_STAT_DRQ | ATAPI_STAT_SERVDSC | ATAPI_STAT_DRQ;
}
atapi_regs[ATAPI_REG_INTREASON] = ATAPI_INTREASON_IO;
}
switch( phase )
{
case SCSI_PHASE_DATAOUT:
atapi_cdata_wait = atapi_xferlen;
break;
}
// perform special ATAPI processing of certain commands
switch (atapi_data[0]&0xff)
{
case 0x00: // BUS RESET / TEST UNIT READY
case 0xbb: // SET CDROM SPEED
atapi_regs[ATAPI_REG_CMDSTATUS] = 0;
break;
void Looper::sendMessageAtTime(nsecs_t uptime, const sp<MessageHandler>& handler,
const Message& message) {
#if DEBUG_CALLBACKS
LOGD("%p ~ sendMessageAtTime - uptime=%lld, handler=%p, what=%d",
this, uptime, handler.get(), message.what);
#endif
size_t i = 0;
{ // acquire lock
AutoMutex _l(mLock);
size_t messageCount = mMessageEnvelopes.size();
while (i < messageCount && uptime >= mMessageEnvelopes.itemAt(i).uptime) {
i += 1;
}
MessageEnvelope messageEnvelope(uptime, handler, message);
mMessageEnvelopes.insertAt(messageEnvelope, i, 1);
// Optimization: If the Looper is currently sending a message, then we can skip
// the call to wake() because the next thing the Looper will do after processing
// messages is to decide when the next wakeup time should be. In fact, it does
// not even matter whether this code is running on the Looper thread.
if (mSendingMessage) {
return;
}
} // release lock
// Wake the poll loop only when we enqueue a new message at the head.
if (i == 0) {
wake();
}
}
ctrlc_set(int4 dummy_param)
{
int4 status;
msgtype message;
error_def(ERR_LASTFILCMPLD);
if (!IS_MCODE_RUNNING)
{
message.arg_cnt = 4;
message.def_opts = message.new_opts = 0;
message.msg_number = ERR_LASTFILCMPLD;
message.fp_cnt = 2;
message.fp[0].n = strlen(source_file_name);
message.fp[1].cp = source_file_name;
sys$putmsg(&message, 0, 0, 0);
} else if (!outofband)
{
if (ctrlc_on)
{
status = sys$setef(efn_outofband);
assert(SS$_WASCLR == status);
if (status != SS$_WASCLR && status != SS$_WASSET)
GTMASSERT;
ctrap_action_is = 0;
outofband = ctrlc;
xfer_table[xf_linefetch] = op_fetchintrrpt;
xfer_table[xf_linestart] = op_startintrrpt;
xfer_table[xf_zbfetch] = op_fetchintrrpt;
xfer_table[xf_zbstart] = op_startintrrpt;
xfer_table[xf_forchk1] = op_startintrrpt;
xfer_table[xf_forloop] = op_forintrrpt;
sys$wake(0,0);
}
}
}
void sendFlush(Args...args)
{
EnterCriticalSection(&mLock);
send<T,Args...>(args...);
LeaveCriticalSection(&mLock);
wake();
}
void MythSystemIOHandler::insert(HANDLE h, QBuffer *buff)
{
m_pLock.lock();
m_pMap.insert(h, buff);
m_pLock.unlock();
wake();
}
static int convert_lock(int mode)
{
int status;
int old_mode = cur_mode;
printf("converting to %d\n", mode);
/* Lock it */
status = sys$enq(0,
mode,
&our_lksb,
LCK$M_CONVERT | LCK$M_VALBLK,
NULL,
0, /* parent */
compast_routine,
0, /* astp */
blockast_routine,
PSL$C_USER,
RSDM$K_PROCESS_RSDM_ID,
0);
if (status != SS$_NORMAL)
{
printf("convert enq failed : %s\n", strerror(EVMSERR, status));
sys$wake();
}
else
{
cur_mode = mode;
status = 0; /* simulate Unix retcodes */
}
return status;
}
void MythSystemIOHandler::insert(int fd, QBuffer *buff)
{
m_pLock.lock();
m_pMap.insert(fd, buff);
BuildFDs();
m_pLock.unlock();
wake();
}
/**
* PCA9685 PWM set frequency prescale
*/
void PCA9685::_setPreScale(uint8_t prescale)
{
// must be in sleep mode to set the prescaler
sleep();
// set the prescaler
_write8bits(PCA9685_PRESCALE, prescale);
wake();
}
void Looper::scheduleEpollRebuildLocked() {
if (!mEpollRebuildRequired) {
#if DEBUG_CALLBACKS
ALOGD("%p ~ scheduleEpollRebuildLocked - scheduling epoll set rebuild", this);
#endif
mEpollRebuildRequired = true;
wake();
}
}
void LLThread::unpause()
{
if (mPaused)
{
mPaused = 0;
}
wake(); // wake up the thread if necessary
}
void Evloop::queueInLoop(PendFuncType &&cb)
{
{
std::lock_guard<std::mutex> guard(mutex_);
pendfctrList_.push_back(cb);
}
if(!isInLoopThread() || inHandingPendingFctrs_)
wake();
}
void LLThread::setQuitting()
{
mRunCondition->lock();
if (mStatus == RUNNING)
{
mStatus = QUITTING;
}
mRunCondition->unlock();
wake();
}
void turn_on_led_and_reset_timer(void) {
orange_led_on = orange_led_on ? 0 : 1;
/* in handler mode, don't use svc call */
current_millis = get_current_millis();
if (current_millis - start_millis > 30000) {
time_remaining = 0;
} else {
wake(flash_orange_led_pid);
}
}
void flush()
{
// FIXME: On Wine, a glFlush may cause a repaint of the window from the
// GL's front buffer, which is fine except for the added processing we
// don't want. However, it would be nice to ensure OpenGL is processing
// all the commands that got sent to it up to this point.
//sendFlush<FlushGLCmd>();
EnterCriticalSection(&mLock);
LeaveCriticalSection(&mLock);
wake();
}
static void unlock()
{
int status;
status = sys$deq(our_lksb.sb_lkid, NULL,0,0,0);
if (status != SS$_NORMAL)
{
printf("denq failed : %s\n", strerror(EVMSERR, status));
sys$wake();
}
}
void LLQueuedThread::incQueue()
{
// Something has been added to the queue
if (!isPaused())
{
if (mThreaded)
{
wake(); // Wake the thread up if necessary.
}
}
}
void Looper::wakeAndLock() {
mLock.lock();
mWaiters += 1;
while (mPolling) {
wake();
mAwake.wait(mLock);
}
mWaiters -= 1;
if (mWaiters == 0) {
mResume.signal();
}
}
void Adafruit_Thermal::begin(int heatTime) {
// The printer can't start receiving data immediately upon power up --
// it needs a moment to cold boot and initialize. Allow at least 1/2
// sec of uptime before printer can receive data.
timeoutSet(500000L);
wake();
reset();
// ESC 7 n1 n2 n3 Setting Control Parameter Command
// n1 = "max heating dots" 0-255 -- max number of thermal print head
// elements that will fire simultaneously. Units = 8 dots (minus 1).
// Printer default is 7 (64 dots, or 1/6 of 384-dot width), this code
// sets it to 11 (96 dots, or 1/4 of width).
// n2 = "heating time" 3-255 -- duration that heating dots are fired.
// Units = 10 us. Printer default is 80 (800 us), this code sets it
// to value passed (default 120, or 1.2 ms -- a little longer than
// the default because we've increased the max heating dots).
// n3 = "heating interval" 0-255 -- recovery time between groups of
// heating dots on line; possibly a function of power supply.
// Units = 10 us. Printer default is 2 (20 us), this code sets it
// to 40 (throttled back due to 2A supply).
// More heating dots = more peak current, but faster printing speed.
// More heating time = darker print, but slower printing speed and
// possibly paper 'stiction'. More heating interval = clearer print,
// but slower printing speed.
writeBytes(ASCII_ESC, '7'); // Esc 7 (print settings)
writeBytes(11, heatTime, 40); // Heating dots, heat time, heat interval
// Print density description from manual:
// DC2 # n Set printing density
// D4..D0 of n is used to set the printing density. Density is
// 50% + 5% * n(D4-D0) printing density.
// D7..D5 of n is used to set the printing break time. Break time
// is n(D7-D5)*250us.
// (Unsure of the default value for either -- not documented)
#define printDensity 10 // 100% (? can go higher, text is darker but fuzzy)
#define printBreakTime 2 // 500 uS
writeBytes(ASCII_DC2, '#', (printBreakTime << 5) | printDensity);
dotPrintTime = 30000; // See comments near top of file for
dotFeedTime = 2100; // an explanation of these values.
maxChunkHeight = 256;
}
void Adafruit_Thermal::begin(SERIAL_IMPL* serial, int heatTime, int maxHeatingDots, int heatingInterval, int printDensity, int printBreakTime) {
_printer = serial;
// The printer can't start receiving data immediately upon power up --
// it needs a moment to cold boot and initialize. Allow at least 1/2
// sec of uptime before printer can receive data.
timeoutSet(500000L);
wake();
reset();
// Description of print settings from page 23 of the manual:
// ESC 7 n1 n2 n3 Setting Control Parameter Command
// Decimal: 27 55 n1 n2 n3
// Set "max heating dots", "heating time", "heating interval"
// n1 = 0-255 Max printing dots, Unit (8dots), Default: 7 (64 dots)
// n2 = 3-255 Heating time, Unit (10us), Default: 80 (800us)
// n3 = 0-255 Heating interval, Unit (10us), Default: 2 (20us)
// The more max heating dots, the more peak current will cost
// when printing, the faster printing speed. The max heating
// dots is 8*(n1+1). The more heating time, the more density,
// but the slower printing speed. If heating time is too short,
// blank page may occur. The more heating interval, the more
// clear, but the slower printing speed.
writeBytes(27, 55); // Esc 7 (print settings)
writeBytes(maxHeatingDots); // Heating dots (20=balance of darkness vs no jams)
writeBytes(heatTime); // Library default = 255 (max)
writeBytes(heatingInterval); // Heat interval (500 uS = slower, but darker)
// Description of print density from page 23 of the manual:
// DC2 # n Set printing density
// Decimal: 18 35 n
// D4..D0 of n is used to set the printing density. Density is
// 50% + 5% * n(D4-D0) printing density.
// D7..D5 of n is used to set the printing break time. Break time
// is n(D7-D5)*250us.
// (Unsure of the default value for either -- not documented)
writeBytes(18, 35); // DC2 # (print density)
writeBytes((printBreakTime << 5) | printDensity);
dotPrintTime = 30000; // See comments near top of file for
dotFeedTime = 2100; // an explanation of these values.
maxChunkHeight = 1;
}
/*
**++
** ROUTINE: sp_once_ast
**
** FUNCTIONAL DESCRIPTION:
**
**
** RETURNS: cond_value, longword (unsigned), write only, by value
**
** PROTOTYPE:
**
** sp_once(struct ONCE *ctx)
**
** IMPLICIT INPUTS: None.
**
** IMPLICIT OUTPUTS: None.
**
** COMPLETION CODES:
** SS$_NORMAL: normal successful completion
** SS$_NONEXPR: subprocess doesn't exist any more
**
** SIDE EFFECTS: None.
**
**--
*/
static unsigned int sp_once_ast(void *once) {
struct ONCE *ctx = once;
struct dsc$descriptor rcvstr;
INIT_DYNDESC(rcvstr);
while (OK(sp_receive(&ctx->spctx, &rcvstr, 0))) {
if (rcvstr.dsc$w_length > ctx->eom_len &&
strncmp(rcvstr.dsc$a_pointer, ctx->eom, ctx->eom_len) == 0) {
ctx->command_complete = 1;
sys$wake(0,0);
break;
}
(ctx->actrtn)(ctx->param, &rcvstr);
}
str$free1_dx(&rcvstr);
return SS$_NORMAL;
} /* sp_once_ast */
void mutex_unlock(mutex * m){
uint64_t v = disableScheduler();
MutexNode * mn = mutexnode_getbyid(m->id);
if(mn->waitn > 0){
pidnode * pidn = mn->first;
wake(pidn->pid);
if(mn->waitn == 1){
mn->first = mn->last = NULL;
} else {
mn->first = pidn->next;
}
la_free(pidn);
mn->waitn --;
} else {
itryunlock(&(m->m));
}
enableScheduler(v);
}
void Scheduler::checkFutures()
{
auto it = _futures.begin();
while (it != _futures.end())
{
if (it->first->ready())
{
wake(it->second);
it->second->_future = it->first;
it = _futures.erase(it);
}
else
{
it++;
}
}
}
void Poll::sendMessageAtTime(nsecs_t uptime, const MessageHandler& handler,
const Message& message) {
#if DEBUG_CALLBACKS
ALOGD("%p ~ sendMessageAtTime - uptime=%lld, handler=%p, what=%d",
this, uptime, handler.get(), message.what);
#endif
size_t i = 0;
{ // acquire lock
AutoMutex _l(mLock);
//more ....
} // release lock
// Wake the poll loop only when we enqueue a new message at the head.
if (i == 0) {
wake();
}
}
void UARTSerial::rx_irq(void)
{
bool was_empty = _rxbuf.empty();
/* Fill in the receive buffer if the peripheral is readable
* and receive buffer is not full. */
while (SerialBase::readable()) {
char data = SerialBase::_base_getc();
if (!_rxbuf.full()) {
_rxbuf.push(data);
} else {
/* Drop - can we report in some way? */
}
}
/* Report the File handler that data is ready to be read from the buffer. */
if (was_empty && !_rxbuf.empty()) {
wake();
}
}
// marco.accame: the pc104 should send set<id32_cmmnd_setpoint, value_cmmnd_setpoint> and never send ask<id32_cmmnd_setpoint>.
// the cmmnd variables should be write-only ....
// thus it should never receive say<id32_cmmnd_setpoint, value> or sig<id32_cmmnd_setpoint, value>.
// the reason is that the ems board manages the cmmd by writing the value_cmmnd_setpoint in its ram, but then it does not guarantee
// that the value stays unchanged. also, it may be that many sepoints of different kind are sent. in this case the last overwrite the previous,
// best way to know is to have a status variable whcih keeps the last received setpoint of some kind.
extern void eoprot_fun_UPDT_mc_joint_cmmnds_setpoint(const EOnv* nv, const eOropdescriptor_t* rd)
{
eOmc_setpoint_t *setpoint_got = (eOmc_setpoint_t*) rd->data;
static int32_t zero = 360;
static prev = 0;
int32_t pos;
// printf("Callback recv setpoint: \n");
uint16_t *checkProg = (uint16_t*) setpoint_got;
// printf("Prog Num %d\n", checkProg[1]);
pos = (int32_t)(setpoint_got->to.position.value / DEG_2_ICUBDEG) + zero;
// printf("position %d\n", pos);
// printf("velocity %d\n", setpoint_got->to.position.withvelocity);
if( (checkProg[1] - prev) != 1)
printf(">>>>>>Missing packet!! prev was %d, actual is %d (missing 0x%04X) <<<<<\n", prev, checkProg[1], prev+1);
prev = checkProg[1];
wake(nv);
}
Thread::~Thread() {
_run = false;
if (linkedlist_node_hanble) {
threads_mutex->lock();
threads->remove(linkedlist_node_hanble);
if (thread) {
if (status == THREAD_WAITING)
wake();
}
linkedlist_node_hanble = 0;
threads_mutex->release();
if (thread)
pthread_join(thread, 0);
}
}
void UARTSerial::rx_irq(void)
{
bool was_empty = _rxbuf.empty();
/* Fill in the receive buffer if the peripheral is readable
* and receive buffer is not full. */
while (!_rxbuf.full() && SerialBase::readable()) {
char data = SerialBase::_base_getc();
_rxbuf.push(data);
}
if (_rx_irq_enabled && _rxbuf.full()) {
SerialBase::attach(NULL, RxIrq);
_rx_irq_enabled = false;
}
/* Report the File handler that data is ready to be read from the buffer. */
if (was_empty && !_rxbuf.empty()) {
wake();
}
}
MojErr MojGmainReactor::notify(MojSockT sock, SockSignal::SlotRef slot, guint events, SockSignal SockInfo::* sig)
{
MojAssert(sock != MojInvalidSock);
MojAssert(m_mainLoop.get());
MojThreadGuard guard(m_mutex);
MojAssert(m_sources.contains(sock));
SourceMap::ConstIterator source = m_sources.find(sock);
if (source == m_sources.end())
MojErrThrow(MojErrNotFound);
SockInfo* info = (*source)->m_info;
(info->*sig).connect(slot);
guint newEvents = info->m_pollFd.events | events;
if (info->m_pollFd.events != newEvents) {
info->m_pollFd.events = (gushort) newEvents;
wake();
}
return MojErrNone;
}
int
clock_nanosleep(clockid_t clockid, int flags, const struct timespec *req, struct timespec *rem)
{
struct freebsd_timespec ftp = {req->tv_sec, req->tv_nsec};
int i, ret, timeout;
struct thread *thread;
/* we could support these but not needed */
if (flags != 0 || rem != NULL) {
errno = EINVAL;
return -1;
}
if (__platform_npoll == 0) {
return __platform_nanosleep(&ftp, NULL);
}
/* use poll instead as we might have network events */
timeout = req->tv_sec * 1000 + req->tv_nsec / 1000000;
ret = __platform_poll(__platform_pollfd, __platform_npoll, timeout);
if (ret == -1) {
errno = EINVAL;
return -1;
}
if (ret == 0)
return 0;
for (i = 0; i < __platform_npoll; i++) {
if (__platform_pollfd[i].revents) {
thread = __franken_fd[__platform_pollfd[i].fd].wake;
if (thread)
wake(thread);
}
}
return 0;
}
