static void *__map_umm(const char *name, uint32_t *size_r)
{
struct cobalt_memdev_stat statbuf;
int fd, ret;
void *addr;
fd = __RT(open(name, O_RDWR));
if (fd < 0) {
early_warning("cannot open RTDM device %s: %s", name,
strerror(errno));
return MAP_FAILED;
}
ret = __RT(ioctl(fd, MEMDEV_RTIOC_STAT, &statbuf));
if (ret) {
early_warning("failed getting status of %s: %s",
name, strerror(errno));
return MAP_FAILED;
}
addr = __RT(mmap(NULL, statbuf.size, PROT_READ|PROT_WRITE,
MAP_SHARED, fd, 0));
__RT(close(fd));
*size_r = statbuf.size;
return addr;
}
int clockobj_init(struct clockobj *clkobj,
unsigned int resolution_ns)
{
pthread_mutexattr_t mattr;
struct timespec now;
int ret;
if (resolution_ns == 0)
return __bt(-EINVAL);
memset(clkobj, 0, sizeof(*clkobj));
ret = __clockobj_set_resolution(clkobj, resolution_ns);
if (ret)
return __bt(ret);
/*
* FIXME: this lock is only used to protect the wallclock
* offset readings from updates. We should replace this by a
* confirmed reading loop.
*/
pthread_mutexattr_init(&mattr);
pthread_mutexattr_settype(&mattr, mutex_type_attribute);
pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
pthread_mutexattr_setpshared(&mattr, PTHREAD_PROCESS_PRIVATE);
ret = __bt(-__RT(pthread_mutex_init(&clkobj->lock, &mattr)));
pthread_mutexattr_destroy(&mattr);
if (ret)
return ret;
__RT(clock_gettime(CLOCK_COPPERPLATE, &now));
timespec_sub(&clkobj->offset, &clkobj->epoch, &now);
return 0;
}
void __boilerplate_init(void)
{
__RT(clock_gettime(CLOCK_MONOTONIC, &init_date));
__RT(pthread_mutex_init(&__printlock, NULL));
debug_init();
init_done = 1;
}
int rt_cond_delete(RT_COND *cond)
{
struct alchemy_cond *ccb;
struct service svc;
int ret = 0;
if (threadobj_async_p())
return -EPERM;
COPPERPLATE_PROTECT(svc);
ccb = get_alchemy_cond(cond, &ret);
if (ccb == NULL)
goto out;
ret = -__RT(pthread_cond_destroy(&ccb->cond));
if (ret) {
if (ret == -EBUSY)
put_alchemy_cond(ccb);
goto out;
}
ccb->magic = ~cond_magic;
put_alchemy_cond(ccb);
cluster_delobj(&alchemy_cond_table, &ccb->cobj);
__RT(pthread_mutex_destroy(&ccb->safe));
xnfree(ccb);
out:
COPPERPLATE_UNPROTECT(svc);
return ret;
}
static void trank_alarm_handler(void *arg)
{
struct trank_alarm_wait *aw = arg;
__RT(pthread_mutex_lock(&aw->lock));
aw->alarm_pulses++;
__RT(pthread_cond_broadcast(&aw->event));
__RT(pthread_mutex_unlock(&aw->lock));
}
int __heapobj_extend(struct heapobj *hobj, size_t size, void *mem)
{
__RT(pthread_mutex_lock(&hobj->lock));
hobj->size = add_new_area(hobj->pool, size, mem);
__RT(pthread_mutex_unlock(&hobj->lock));
if (hobj->size == (size_t)-1)
return __bt(-EINVAL);
return 0;
}
void *__heapobj_alloc(struct heapobj *hobj, size_t size)
{
void *p;
__RT(pthread_mutex_lock(&hobj->lock));
p = malloc_ex(size, hobj->pool);
__RT(pthread_mutex_unlock(&hobj->lock));
return p;
}
size_t __heapobj_inquire(struct heapobj *hobj, void *ptr)
{
size_t size;
__RT(pthread_mutex_lock(&hobj->lock));
size = malloc_usable_size_ex(ptr, hobj->pool);
__RT(pthread_mutex_unlock(&hobj->lock));
return size;
}
static STATUS msem_take(struct wind_sem *sem, int timeout)
{
struct wind_task *current;
struct timespec ts;
int ret;
if (threadobj_irq_p())
return S_intLib_NOT_ISR_CALLABLE;
/*
* We allow threads from other APIs to grab a VxWorks mutex
* ignoring the safe option in such a case.
*/
current = wind_task_current();
if (current && (sem->options & SEM_DELETE_SAFE))
__RT(pthread_mutex_lock(¤t->safelock));
if (timeout == NO_WAIT) {
ret = __RT(pthread_mutex_trylock(&sem->u.msem.lock));
goto check;
}
if (timeout == WAIT_FOREVER) {
ret = __RT(pthread_mutex_lock(&sem->u.msem.lock));
goto check;
}
__clockobj_ticks_to_timeout(&wind_clock, CLOCK_REALTIME, timeout, &ts);
ret = __RT(pthread_mutex_timedlock(&sem->u.msem.lock, &ts));
check:
switch (ret) {
case 0:
return OK;
case EINVAL:
ret = S_objLib_OBJ_ID_ERROR;
break;
case EBUSY:
ret = S_objLib_OBJ_UNAVAILABLE;
break;
case ETIMEDOUT:
ret = S_objLib_OBJ_TIMEOUT;
break;
case EOWNERDEAD:
case ENOTRECOVERABLE:
warning("owner of mutex-type semaphore %p died", sem);
ret = S_objLib_OBJ_UNAVAILABLE;
break;
}
if (current != NULL && (sem->options & SEM_DELETE_SAFE))
__RT(pthread_mutex_unlock(¤t->safelock));
return ret;
}
/* Conversion from CLOCK_COPPERPLATE to clk_id. */
void clockobj_convert_clocks(struct clockobj *clkobj,
const struct timespec *in,
clockid_t clk_id,
struct timespec *out)
{
struct timespec now, delta;
__RT(clock_gettime(CLOCK_COPPERPLATE, &now));
/* Offset from CLOCK_COPPERPLATE epoch. */
timespec_sub(&delta, in, &now);
/* Current time for clk_id. */
__RT(clock_gettime(clk_id, &now));
/* Absolute timeout again, clk_id-based this time. */
timespec_add(out, &delta, &now);
}
static ssize_t do_write_pipe(RT_PIPE *pipe,
const void *buf, size_t size, int flags)
{
struct alchemy_pipe *pcb;
struct service svc;
ssize_t ret;
int err = 0;
CANCEL_DEFER(svc);
pcb = find_alchemy_pipe(pipe, &err);
if (pcb == NULL) {
ret = err;
goto out;
}
ret = __RT(sendto(pcb->sock, buf, size, flags, NULL, 0));
if (ret < 0) {
ret = -errno;
if (ret == -EBADF)
ret = -EIDRM;
}
out:
CANCEL_RESTORE(svc);
return ret;
}
void __printout(const char *name, const char *header,
const char *fmt, va_list ap)
{
struct timespec now, delta;
unsigned long long ns;
unsigned int ms, us;
/*
* Catch early printouts, when the init sequence is not
* completed yet. In such event, we don't care for serializing
* output, since we must be running over the main thread
* uncontended.
*/
if (!init_done) {
__do_printout(name, header, 0, 0, fmt, ap);
return;
}
__RT(clock_gettime(CLOCK_MONOTONIC, &now));
timespec_sub(&delta, &now, &init_date);
ns = delta.tv_sec * 1000000000ULL;
ns += delta.tv_nsec;
ms = ns / 1000000ULL;
us = (ns % 1000000ULL) / 1000ULL;
SIGSAFE_LOCK_ENTRY(&__printlock);
__do_printout(name, header, ms, us, fmt, ap);
SIGSAFE_LOCK_EXIT(&__printlock);
}
static struct alchemy_cond *
__get_alchemy_cond(struct alchemy_cond *ccb, int *err_r)
{
int ret;
if (ccb->magic == ~cond_magic)
goto dead_handle;
if (ccb->magic != cond_magic)
goto bad_handle;
ret = __RT(pthread_mutex_lock(&ccb->safe));
if (ret)
goto bad_handle;
/* Recheck under lock. */
if (ccb->magic == cond_magic)
return ccb;
dead_handle:
/* Removed under our feet. */
*err_r = -EIDRM;
return NULL;
bad_handle:
*err_r = -EINVAL;
return NULL;
}
/**
* @fn int rt_pipe_delete(RT_PIPE *pipe)
* @brief Delete a message pipe.
*
* This routine deletes a pipe object previously created by a call to
* rt_pipe_create(). All resources attached to that pipe are
* automatically released, all pending data is flushed.
*
* @param pipe The pipe descriptor.
*
* @return Zero is returned upon success. Otherwise:
*
* - -EINVAL is returned if @a pipe is not a valid pipe descriptor.
*
* - -EIDRM is returned if @a pipe is a closed pipe descriptor.
*
* - -EPERM is returned if this service was called from an
* asynchronous context.
*
* @apitags{thread-unrestricted, switch-secondary}
*/
int rt_pipe_delete(RT_PIPE *pipe)
{
struct alchemy_pipe *pcb;
struct service svc;
int ret = 0;
if (threadobj_irq_p())
return -EPERM;
CANCEL_DEFER(svc);
pcb = find_alchemy_pipe(pipe, &ret);
if (pcb == NULL)
goto out;
ret = __RT(close(pcb->sock));
if (ret) {
ret = -errno;
if (ret == -EBADF)
ret = -EIDRM;
goto out;
}
syncluster_delobj(&alchemy_pipe_table, &pcb->cobj);
pcb->magic = ~pipe_magic;
out:
CANCEL_RESTORE(svc);
return ret;
}
int rt_cond_wait_until(RT_COND *cond, RT_MUTEX *mutex,
RTIME timeout)
{
struct alchemy_mutex *mcb;
struct alchemy_cond *ccb;
struct service svc;
struct timespec ts;
int ret = 0;
if (timeout == TM_NONBLOCK)
return -EWOULDBLOCK;
COPPERPLATE_PROTECT(svc);
ccb = get_alchemy_cond(cond, &ret);
if (ccb == NULL)
goto out;
mcb = find_alchemy_mutex(mutex, &ret);
if (mcb == NULL)
goto unlock;
ccb->nwaiters++;
put_alchemy_cond(ccb);
if (timeout != TM_INFINITE) {
clockobj_ticks_to_timeout(&alchemy_clock, timeout, &ts);
ret = -__RT(pthread_cond_timedwait(&ccb->cond, &mcb->lock, &ts));
} else
ret = -__RT(pthread_cond_wait(&ccb->cond, &mcb->lock));
/*
* Be cautious, grab the internal safe lock again to update
* the control block.
*/
ccb = __get_alchemy_cond(ccb, &ret);
if (ccb == NULL)
goto out;
ccb->nwaiters--;
unlock:
put_alchemy_cond(ccb);
out:
COPPERPLATE_UNPROTECT(svc);
return ret;
}
int rt_cond_create(RT_COND *cond, const char *name)
{
pthread_mutexattr_t mattr;
struct alchemy_cond *ccb;
pthread_condattr_t cattr;
struct service svc;
if (threadobj_async_p())
return -EPERM;
COPPERPLATE_PROTECT(svc);
ccb = xnmalloc(sizeof(*ccb));
if (ccb == NULL) {
COPPERPLATE_UNPROTECT(svc);
return -ENOMEM;
}
strncpy(ccb->name, name, sizeof(ccb->name));
ccb->name[sizeof(ccb->name) - 1] = '\0';
ccb->nwaiters = 0;
if (cluster_addobj(&alchemy_cond_table, ccb->name, &ccb->cobj)) {
xnfree(ccb);
COPPERPLATE_UNPROTECT(svc);
return -EEXIST;
}
__RT(pthread_mutexattr_init(&mattr));
__RT(pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT));
__RT(pthread_mutexattr_setpshared(&mattr, mutex_scope_attribute));
__RT(pthread_mutex_init(&ccb->safe, &mattr));
__RT(pthread_mutexattr_destroy(&mattr));
__RT(pthread_condattr_init(&cattr));
__RT(pthread_condattr_setpshared(&cattr, mutex_scope_attribute));
__RT(pthread_condattr_setclock(&cattr, CLOCK_COPPERPLATE));
__RT(pthread_cond_init(&ccb->cond, &cattr));
__RT(pthread_condattr_destroy(&cattr));
ccb->magic = cond_magic;
cond->handle = mainheap_ref(ccb, uintptr_t);
COPPERPLATE_UNPROTECT(svc);
return 0;
}
void backtrace_init_context(struct backtrace_data *btd,
const char *name)
{
__RT(pthread_mutex_init(&btd->lock, NULL));
btd->inner = NULL;
btd->name = name ?: "<anonymous>";
pthread_setspecific(btkey, btd);
}
void __clockobj_ticks_to_timeout(struct clockobj *clkobj,
clockid_t clk_id,
ticks_t ticks, struct timespec *ts)
{
struct timespec now, delta;
__RT(clock_gettime(clk_id, &now));
__clockobj_ticks_to_timespec(clkobj, ticks, &delta);
timespec_add(ts, &now, &delta);
}
int rt_alarm_create(RT_ALARM *alarm, const char *name)
{
struct trank_alarm_wait *aw;
pthread_mutexattr_t mattr;
pthread_condattr_t cattr;
int ret;
aw = xnmalloc(sizeof(*aw));
if (aw == NULL)
return -ENOMEM;
aw->alarm_pulses = 0;
pthread_mutexattr_init(&mattr);
pthread_mutexattr_settype(&mattr, mutex_type_attribute);
pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
pthread_mutexattr_setpshared(&mattr, PTHREAD_PROCESS_PRIVATE);
ret = __bt(-__RT(pthread_mutex_init(&aw->lock, &mattr)));
pthread_mutexattr_destroy(&mattr);
if (ret)
goto fail_lock;
pthread_condattr_init(&cattr);
pthread_condattr_setpshared(&cattr, PTHREAD_PROCESS_PRIVATE);
ret = __bt(-pthread_cond_init(&aw->event, &cattr));
pthread_condattr_destroy(&cattr);
if (ret)
goto fail_cond;
ret = __CURRENT(rt_alarm_create(alarm, name, trank_alarm_handler, aw));
if (ret)
goto fail_alarm;
return 0;
fail_alarm:
__RT(pthread_cond_destroy(&aw->event));
fail_cond:
__RT(pthread_mutex_destroy(&aw->lock));
fail_lock:
xnfree(aw);
return ret;
}
int traceobj_init(struct traceobj *trobj, const char *label, int nr_marks)
{
pthread_mutexattr_t mattr;
pthread_condattr_t cattr;
int ret;
pthread_mutexattr_init(&mattr);
pthread_mutexattr_settype(&mattr, mutex_type_attribute);
pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
pthread_mutexattr_setpshared(&mattr, PTHREAD_PROCESS_PRIVATE);
ret = __bt(-__RT(pthread_mutex_init(&trobj->lock, &mattr)));
pthread_mutexattr_destroy(&mattr);
if (ret)
return ret;
pthread_condattr_init(&cattr);
pthread_condattr_setpshared(&cattr, PTHREAD_PROCESS_PRIVATE);
ret = __bt(-__RT(pthread_cond_init(&trobj->join, &cattr)));
pthread_condattr_destroy(&cattr);
if (ret) {
__RT(pthread_mutex_destroy(&trobj->lock));
return ret;
}
/*
* We make sure not to unblock from threadobj_join() until at
* least one thread has called trace_enter() for this trace
* object.
*/
trobj->nr_threads = -1;
trobj->label = label;
trobj->nr_marks = nr_marks;
trobj->cur_mark = 0;
if (nr_marks > 0) {
trobj->marks = pvmalloc(sizeof(struct tracemark) * nr_marks);
if (trobj->marks == NULL)
panic("cannot allocate mark table for tracing");
}
return 0;
}
int rt_alarm_wait(RT_ALARM *alarm)
{
struct threadobj *current = threadobj_current();
struct sched_param_ex param_ex;
struct trank_alarm_wait *aw;
struct alchemy_alarm *acb;
int ret, prio, pulses;
acb = find_alarm(alarm);
if (acb == NULL)
return -EINVAL;
threadobj_lock(current);
prio = threadobj_get_priority(current);
if (prio != threadobj_irq_prio) {
param_ex.sched_priority = threadobj_irq_prio;
/* Working on self, so -EIDRM can't happen. */
threadobj_set_schedparam(current, SCHED_FIFO, ¶m_ex);
}
threadobj_unlock(current);
aw = acb->arg;
/*
* Emulate the original behavior: wait for the next pulse (no
* event buffering, broadcast to all waiters), while
* preventing spurious wakeups.
*/
__RT(pthread_mutex_lock(&aw->lock));
pulses = aw->alarm_pulses;
for (;;) {
ret = -__RT(pthread_cond_wait(&aw->event, &aw->lock));
if (ret || aw->alarm_pulses != pulses)
break;
}
__RT(pthread_mutex_unlock(&aw->lock));
return __bt(ret);
}
void smokey_note(const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
if (!smokey_quiet_mode)
__RT(vfprintf(stdout, fmt, ap));
va_end(ap);
}
static void task_finalizer(struct threadobj *thobj)
{
struct wind_task *task = container_of(thobj, struct wind_task, thobj);
task->tcb->status |= WIND_DEAD;
cluster_delobj(&wind_task_table, &task->cobj);
registry_destroy_file(&task->fsobj);
__RT(pthread_mutex_destroy(&task->safelock));
threadobj_destroy(&task->thobj);
xnfree(task);
}
int rt_alarm_delete(RT_ALARM *alarm)
{
struct trank_alarm_wait *aw;
struct alchemy_alarm *acb;
int ret;
acb = find_alarm(alarm);
if (acb == NULL)
return -EINVAL;
aw = acb->arg;
ret = __CURRENT(rt_alarm_delete(alarm));
if (ret)
return ret;
__RT(pthread_cond_destroy(&aw->event));
__RT(pthread_mutex_destroy(&aw->lock));
xnfree(aw);
return 0;
}
static STATUS msem_give(struct wind_sem *sem)
{
struct wind_task *current;
int ret;
if (threadobj_irq_p())
return S_intLib_NOT_ISR_CALLABLE;
ret = __RT(pthread_mutex_unlock(&sem->u.msem.lock));
if (ret == EINVAL)
return S_objLib_OBJ_ID_ERROR;
if (ret == EPERM)
return S_semLib_INVALID_OPERATION;
if (sem->options & SEM_DELETE_SAFE) {
current = wind_task_current();
if (current)
__RT(pthread_mutex_unlock(¤t->safelock));
}
return OK;
}
static void compare_marks(struct traceobj *trobj, int tseq[], int nr_seq) /* lock held */
{
int mark;
for (mark = 0; mark < trobj->cur_mark || mark < nr_seq; mark++) {
if (mark >= trobj->cur_mark) {
fprintf(stderr, " <missing mark> | [%d] expected\n",
tseq[mark]);
} else if (mark < nr_seq)
__RT(fprintf(stderr, "at %s:%d | [%d] should be [%d]\n",
trobj->marks[mark].file,
trobj->marks[mark].line,
trobj->marks[mark].mark,
tseq[mark]));
else
__RT(fprintf(stderr, "at %s:%d | unexpected [%d]\n",
trobj->marks[mark].file,
trobj->marks[mark].line,
trobj->marks[mark].mark));
}
fflush(stderr);
}
/**
* @fn ssize_t rt_pipe_read_timed(RT_PIPE *pipe, void *buf, size_t size, const struct timespec *abs_timeout)
* @brief Read a message from a pipe.
*
* This service reads the next available message from a given pipe.
*
* @param pipe The pipe descriptor.
*
* @param buf A pointer to a memory area which will be written upon
* success with the message received.
*
* @param size The count of bytes from the received message to read up
* into @a buf. If @a size is lower than the actual message size,
* -ENOBUFS is returned since the incompletely received message would
* be lost. If @a size is zero, this call returns immediately with no
* other action.
*
* @param abs_timeout An absolute date expressed in clock ticks,
* specifying a time limit to wait for a message to be available from
* the pipe (see note). Passing NULL causes the caller to block
* indefinitely until a message is available. Passing { .tv_sec = 0,
* .tv_nsec = 0 } causes the service to return immediately without
* blocking in case no message is available.
*
* @return The number of bytes available from the received message is
* returned upon success. Otherwise:
*
* - -ETIMEDOUT is returned if @a abs_timeout is reached before a
* message arrives.
*
* - -EWOULDBLOCK is returned if @a abs_timeout is { .tv_sec = 0,
* .tv_nsec = 0 } and no message is immediately available on entry to
* the call.
*
* - -EINTR is returned if rt_task_unblock() was called for the
* current task before a message was available.
*
* - -EINVAL is returned if @a pipe is not a valid pipe descriptor.
*
* - -EIDRM is returned if @a pipe is deleted while the caller was
* waiting for a message. In such event, @a pipe is no more valid upon
* return of this service.
*
* - -EPERM is returned if this service should block, but was not
* called from a Xenomai thread.
*
* @apitags{xthread-nowait, switch-primary}
*
* @note @a abs_timeout is interpreted as a multiple of the Alchemy
* clock resolution (see --alchemy-clock-resolution option, defaults
* to 1 nanosecond).
*/
ssize_t rt_pipe_read_timed(RT_PIPE *pipe,
void *buf, size_t size,
const struct timespec *abs_timeout)
{
struct alchemy_pipe *pcb;
int err = 0, flags;
struct timeval tv;
ssize_t ret;
pcb = find_alchemy_pipe(pipe, &err);
if (pcb == NULL)
return err;
if (alchemy_poll_mode(abs_timeout))
flags = MSG_DONTWAIT;
else {
if (!threadobj_current_p())
return -EPERM;
if (abs_timeout) {
tv.tv_sec = abs_timeout->tv_sec;
tv.tv_usec = abs_timeout->tv_nsec / 1000;
} else {
tv.tv_sec = 0;
tv.tv_usec = 0;
}
__RT(setsockopt(pcb->sock, SOL_SOCKET,
SO_RCVTIMEO, &tv, sizeof(tv)));
flags = 0;
}
ret = __RT(recvfrom(pcb->sock, buf, size, flags, NULL, 0));
if (ret < 0)
ret = -errno;
return ret;
}
void clockobj_get_date(struct clockobj *clkobj, ticks_t *pticks)
{
struct timespec now, date;
read_lock_nocancel(&clkobj->lock);
__RT(clock_gettime(CLOCK_COPPERPLATE, &now));
/* Add offset from epoch to current system time. */
timespec_add(&date, &clkobj->offset, &now);
/* Convert the time value to ticks,. */
*pticks = (ticks_t)date.tv_sec * clockobj_get_frequency(clkobj)
+ (ticks_t)date.tv_nsec / clockobj_get_resolution(clkobj);
read_unlock(&clkobj->lock);
}
int rt_cond_broadcast(RT_COND *cond)
{
struct alchemy_cond *ccb;
struct service svc;
int ret = 0;
COPPERPLATE_PROTECT(svc);
ccb = find_alchemy_cond(cond, &ret);
if (ccb == NULL)
goto out;
ret = -__RT(pthread_cond_broadcast(&ccb->cond));
out:
COPPERPLATE_UNPROTECT(svc);
return ret;
}
void clockobj_set_date(struct clockobj *clkobj, ticks_t ticks)
{
struct timespec now;
/*
* XXX: we grab the lock to exclude other threads from reading
* the clock offset while we update it, so that they either
* compute against the old value, or the new one, but always
* see a valid offset.
*/
read_lock_nocancel(&clkobj->lock);
__RT(clock_gettime(CLOCK_COPPERPLATE, &now));
__clockobj_ticks_to_timespec(clkobj, ticks, &clkobj->epoch);
timespec_sub(&clkobj->offset, &clkobj->epoch, &now);
read_unlock(&clkobj->lock);
}
