int main(int argc, const char *argv[])
{
unsigned long long start;
int dev, dev2;
printf("Setup\n");
check("modprobe", system("modprobe xeno_rtdmtest"), 0);
dev = check_no_error("open", open(devname, O_RDWR));
printf("Exclusive open\n");
check("open", open(devname, O_RDWR), -EBUSY);
printf("Successive open\n");
dev2 = check("open", open(devname2, O_RDWR), dev + 1);
check("close", close(dev2), 0);
printf("Defer close by driver handler\n");
check("ioctl", ioctl(dev, RTTST_RTIOC_RTDM_DEFER_CLOSE,
RTTST_RTDM_DEFER_CLOSE_HANDLER), 0);
start = rt_timer_tsc();
check("close", close(dev), 0);
check("open", open(devname, O_RDWR), -EBUSY);
dev2 = check("open", open(devname2, O_RDWR), dev);
check("close", close(dev2), 0);
usleep(300000);
dev = check("open", open(devname, O_RDWR), dev);
printf("Defer close by pending reference\n");
check("ioctl", ioctl(dev, RTTST_RTIOC_RTDM_DEFER_CLOSE,
RTTST_RTDM_DEFER_CLOSE_CONTEXT), 0);
start = rt_timer_tsc();
check("close", close(dev), 0);
check("open", open(devname, O_RDWR), -EBUSY);
dev2 = check("open", open(devname2, O_RDWR), dev);
check("close", close(dev2), 0);
usleep(300000);
dev = check("open", open(devname, O_RDWR), dev);
printf("Normal close\n");
check("ioctl", ioctl(dev, RTTST_RTIOC_RTDM_DEFER_CLOSE,
RTTST_RTDM_NORMAL_CLOSE), 0);
check("close", close(dev), 0);
dev = check("open", open(devname, O_RDWR), dev);
printf("Deferred module unload\n");
check("ioctl", ioctl(dev, RTTST_RTIOC_RTDM_DEFER_CLOSE,
RTTST_RTDM_DEFER_CLOSE_CONTEXT), 0);
start = rt_timer_tsc();
check("close", close(dev), 0);
check("rmmod", system("rmmod xeno_rtdmtest"), 0);
check_sleep("rmmod", start);
return 0;
}
static void event(void *cookie)
{
int err;
err = rt_task_set_periodic(NULL,
TM_NOW,
rt_timer_ns2ticks(sampling_period));
if (err) {
warning("failed to enter periodic timing (%s)\n",
symerror(err));
return;
}
for (;;) {
err = rt_task_wait_period(NULL);
if (err) {
if (err != -ETIMEDOUT)
exit(EXIT_FAILURE);
late++;
}
switch_count++;
err = rt_sem_broadcast(&switch_sem);
switch_tsc = rt_timer_tsc();
if (err) {
if (err != -EIDRM && err != -EINVAL)
warning("failed to broadcast semaphore (%s)\n",
symerror(err));
break;
}
}
}
void simple_condwait(void)
{
unsigned long long start;
mutex_t mutex;
cond_t cond;
struct cond_mutex cm = {
.mutex = &mutex,
.cond = &cond,
};
thread_t cond_signaler_tid;
fprintf(stderr, "%s\n", __FUNCTION__);
check("mutex_init", mutex_init(&mutex, PTHREAD_MUTEX_DEFAULT, 0), 0);
check("cond_init", cond_init(&cond, 0), 0);
check("mutex_lock", mutex_lock(&mutex), 0);
check("thread_spawn",
thread_spawn(&cond_signaler_tid, 2, cond_signaler, &cm), 0);
thread_msleep(11);
start = rt_timer_tsc();
check("cond_wait", cond_wait(&cond, &mutex, XN_INFINITE), 0);
check_sleep("cond_wait", start);
thread_msleep(10);
check("mutex_unlock", mutex_unlock(&mutex), 0);
check("thread_join", thread_join(cond_signaler_tid), 0);
check("mutex_destroy", mutex_destroy(&mutex), 0);
check("cond_destroy", cond_destroy(&cond), 0);
}
static int gpio_interrupt(rtdm_irq_t *irq_handle)
{
RTIME temp_time;
static SRTIME curr_time;
static SRTIME prev_time;
static SRTIME diff_time;
static int freq;
temp_time = rt_timer_tsc();
curr_time = rt_timer_tsc2ns(temp_time);
diff_time = curr_time - prev_time;
prev_time = curr_time;
//Get frequency
freq = (uint32_t)div_u64(100000000000, diff_time);
//rtdm_printk("F: %u \n", freq);
set_next_tooth(curr_time);
//freq = 100000000000 / diff_time; //Diff_time is in ns, freq = times*1000
return RTDM_IRQ_HANDLED;
}
void event(void *cookie)
{
int err;
err = rt_task_set_periodic(NULL,
TM_NOW,
rt_timer_ns2ticks(sampling_period));
if (err) {
fprintf(stderr,"switch: failed to set periodic, code %d\n", err);
return;
}
for (;;) {
err = rt_task_wait_period(NULL);
if (err) {
if (err != -ETIMEDOUT) {
/* Timer stopped. */
rt_task_delete(NULL);
}
}
switch_count++;
switch_tsc = rt_timer_tsc();
rt_sem_v(&switch_sem);
}
}
void rt_task_body(void *cookie)
{
RTIME end;
int err;
rt_task_thread = pthread_self();
rt_printf("syscall\n");
setup_checkdebug(SIGDEBUG_MIGRATE_SYSCALL);
sched_yield();
check_sigdebug_received("SIGDEBUG_MIGRATE_SYSCALL");
rt_printf("signal\n");
setup_checkdebug(SIGDEBUG_MIGRATE_SIGNAL);
err = rt_sem_v(&send_signal);
check_no_error("rt_sem_v", err);
rt_task_sleep(rt_timer_ns2ticks(10000000LL));
check_sigdebug_received("SIGDEBUG_MIGRATE_SIGNAL");
rt_printf("relaxed mutex owner\n");
setup_checkdebug(SIGDEBUG_MIGRATE_PRIOINV);
err = rt_mutex_acquire(&prio_invert, TM_INFINITE);
check("rt_mutex_acquire", err, -EINTR);
check_sigdebug_received("SIGDEBUG_MIGRATE_PRIOINV");
rt_printf("page fault\n");
setup_checkdebug(SIGDEBUG_MIGRATE_FAULT);
rt_task_sleep(0);
*mem ^= 0xFF;
check_sigdebug_received("SIGDEBUG_MIGRATE_FAULT");
if (wd) {
rt_printf("watchdog\n");
rt_print_flush_buffers();
setup_checkdebug(SIGDEBUG_WATCHDOG);
end = rt_timer_tsc() + rt_timer_ns2tsc(2100000000ULL);
rt_task_sleep(0);
while (rt_timer_tsc() < end && !sigdebug_received)
/* busy loop */;
check_sigdebug_received("SIGDEBUG_WATCHDOG");
}
}
void check_sleep_inner(const char *fn,
const char *prefix, unsigned long long start)
{
unsigned long long diff = rt_timer_tsc2ns(rt_timer_tsc() - start);
if (diff < 10 * NS_PER_MS) {
fprintf(stderr, "%s waited %Ld.%03u us\n",
prefix, diff / 1000, (unsigned)(diff % 1000));
exit(EXIT_FAILURE);
}
}
static void check_sleep_inner(const char *fn, int line,
const char *msg, unsigned long long start)
{
unsigned long long diff = rt_timer_tsc2ns(rt_timer_tsc() - start);
if (diff < 300 * NS_PER_MS) {
fprintf(stderr, "FAILED %s:%d: %s waited only %Ld.%06u ms\n",
fn, line, msg, diff / 1000000,
(unsigned)(diff % 1000000));
exit(EXIT_FAILURE);
}
}
void *cond_signaler(void *cookie)
{
unsigned long long start;
struct cond_mutex *cm = cookie;
start = rt_timer_tsc();
check("mutex_lock", mutex_lock(cm->mutex), 0);
check_sleep("mutex_lock", start);
thread_msleep(10);
check("cond_signal", cond_signal(cm->cond), 0);
check("mutex_unlock", mutex_unlock(cm->mutex), 0);
return NULL;
}
void *cond_killer(void *cookie)
{
unsigned long long start;
struct cond_mutex *cm = cookie;
start = rt_timer_tsc();
check("mutex_lock", mutex_lock(cm->mutex), 0);
check_sleep("mutex_lock", start);
thread_msleep(10);
check("thread_kill", thread_kill(cm->tid, SIGRTMIN), 0);
check("mutex_unlock", mutex_unlock(cm->mutex), 0);
return NULL;
}
void cond_destroy_whilewait(void)
{
unsigned long long start;
mutex_t mutex;
cond_t cond;
struct cond_mutex cm = {
.mutex = &mutex,
.cond = &cond,
.tid = thread_self(),
};
thread_t cond_destroyer_tid;
struct sigaction sa = {
.sa_handler = sighandler,
.sa_flags = SA_RESTART,
};
sigemptyset(&sa.sa_mask);
fprintf(stderr, "%s\n", __FUNCTION__);
check_unix("sigaction", sigaction(SIGRTMIN, &sa, NULL), 0);
check("mutex_init", mutex_init(&mutex, PTHREAD_MUTEX_DEFAULT, 0), 0);
check("cond_init", cond_init(&cond, 0), 0);
check("mutex_lock", mutex_lock(&mutex), 0);
check("thread_spawn",
thread_spawn(&cond_destroyer_tid, 2, cond_destroyer, &cm), 0);
thread_msleep(11);
start = rt_timer_tsc();
#ifdef XENO_POSIX
check("cond_wait", cond_wait(&cond, &mutex, 10 * NS_PER_MS), -ETIMEDOUT);
check_sleep("cond_wait", start);
thread_msleep(10);
check("mutex_unlock", mutex_unlock(&mutex), 0);
#else /* native */
check("cond_wait", cond_wait(&cond, &mutex, XN_INFINITE), -EIDRM);
check_sleep("cond_wait", start);
#endif /* native */
check("thread_join", thread_join(cond_destroyer_tid), 0);
check("mutex_destroy", mutex_destroy(&mutex), 0);
#ifdef XENO_POSIX
check("cond_destroy", cond_destroy(&cond), 0);
#else /* native */
check("cond_destroy", cond_destroy(&cond), -ESRCH);
#endif /* native */
}
void *cond_destroyer(void *cookie)
{
unsigned long long start;
struct cond_mutex *cm = cookie;
start = rt_timer_tsc();
check("mutex_lock", mutex_lock(cm->mutex), 0);
check_sleep("mutex_lock", start);
thread_msleep(10);
#ifdef XENO_POSIX
check("cond_destroy", cond_destroy(cm->cond), -EBUSY);
#else /* native */
check("cond_destroy", cond_destroy(cm->cond), 0);
#endif /* native */
check("mutex_unlock", mutex_unlock(cm->mutex), 0);
return NULL;
}
void absolute_condwait(void)
{
unsigned long long start;
mutex_t mutex;
cond_t cond;
fprintf(stderr, "%s\n", __FUNCTION__);
check("mutex_init", mutex_init(&mutex, PTHREAD_MUTEX_DEFAULT, 0), 0);
check("cond_init", cond_init(&cond, 1), 0);
check("mutex_lock", mutex_lock(&mutex), 0);
start = rt_timer_tsc();
check("cond_wait",
cond_wait_until(&cond, &mutex, timer_read() + 10 * NS_PER_MS),
-ETIMEDOUT);
check_sleep("cond_wait", start);
check("mutex_unlock", mutex_unlock(&mutex), 0);
check("mutex_destroy", mutex_destroy(&mutex), 0);
check("cond_destroy", cond_destroy(&cond), 0);
}
void _rtapi_delay_hook(long int nsec)
{
long long int release = rt_timer_tsc() + nsec;
while (rt_timer_tsc() < release);
}
/* This returns a result in clocks instead of nS, and needs to be used
with care around CPUs that change the clock speed to save power and
other disgusting, non-realtime oriented behavior. But at least it
doesn't take a week every time you call it.
*/
long long int _rtapi_get_clocks_hook(void) {
// Gilles says: do this - it's portable
return rt_timer_tsc();
}
void latency (void *cookie)
{
int err, count, nsamples;
RTIME expected, period;
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
nsamples = ONE_BILLION / sampling_period;
period = rt_timer_ns2ticks(sampling_period);
expected = rt_timer_tsc();
err = rt_task_set_periodic(NULL,TM_NOW,sampling_period);
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
unsigned long ov;
expected += period;
err = rt_task_wait_period(&ov);
if (err)
{
if (err != -ETIMEDOUT)
rt_task_delete(NULL); /* Timer stopped. */
overrun += ov;
}
dt = (long)(rt_timer_tsc() - expected);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!finished && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = rt_timer_ticks2ns(minj);
maxjitter = rt_timer_ticks2ns(maxj);
avgjitter = rt_timer_ticks2ns(sumj / nsamples);
rt_sem_v(&display_sem);
}
}
void latency(void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks, fault_threshold;
RT_TIMER_INFO timer_info;
unsigned old_relaxed = 0;
err = rt_timer_inquire(&timer_info);
if (err) {
fprintf(stderr, "latency: rt_timer_inquire, code %d\n", err);
return;
}
fault_threshold = rt_timer_ns2tsc(CONFIG_XENO_DEFAULT_PERIOD);
nsamples = ONE_BILLION / period_ns / 1000;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err =
rt_task_set_periodic(NULL, start_ticks,
rt_timer_ns2ticks(period_ns));
if (err) {
fprintf(stderr, "latency: failed to set periodic, code %d\n",
err);
return;
}
for (;;) {
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt;
long overrun = 0;
long long sumj;
test_loops++;
for (count = sumj = 0; count < nsamples; count++) {
unsigned new_relaxed;
unsigned long ov;
expected_tsc += period_tsc;
err = rt_task_wait_period(&ov);
dt = (long)(rt_timer_tsc() - expected_tsc);
new_relaxed = sampling_relaxed;
if (dt > maxj) {
if (new_relaxed != old_relaxed
&& dt > fault_threshold)
max_relaxed +=
new_relaxed - old_relaxed;
maxj = dt;
}
old_relaxed = new_relaxed;
if (dt < minj)
minj = dt;
sumj += dt;
if (err) {
if (err != -ETIMEDOUT) {
fprintf(stderr,
"latency: wait period failed, code %d\n",
err);
exit(EXIT_FAILURE); /* Timer stopped. */
}
overrun += ov;
expected_tsc += period_tsc * ov;
}
if (freeze_max && (dt > gmaxjitter)
&& !(finished || warmup)) {
xntrace_user_freeze(rt_timer_tsc2ns(dt), 0);
gmaxjitter = dt;
}
if (!(finished || warmup) && need_histo())
add_histogram(histogram_avg, dt);
}
if (!warmup) {
if (!finished && need_histo()) {
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if (minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if (maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
}
if (warmup && test_loops == WARMUP_TIME) {
test_loops = 0;
warmup = 0;
}
}
}
long long RT::OS::getTime(void)
{
return rt_timer_tsc2ns(rt_timer_tsc());
}
void motor_cmd_routine(void *m_arg)
{
int ret;
RT_TIMER_INFO timer_info;
long long task_period;
unsigned long overruns = 0;
int16_t req_current = 0;
int sync_ref_counter=0;
float cos_el;
float sin_el;
float v_req_az;
float V_REQ_AZ = 0;
float P_term_az, error_az;
float p_az = 20.0;
float i_az = 1.0;
static float az_integral = 0.0;
float I_term_az, INTEGRAL_CUTOFF=0.5;
printf("Starting Motor Commanding task\n");
rt_timer_inquire(&timer_info);
if (timer_info.period == TM_ONESHOT)
{
// When using an aperiodic timer, task period is specified in ns
task_period = rt_timer_ns2ticks(1000000000ll / 100);
}
else
{
// When using a periodic timer, task period is specified in number of timer periods
task_period = (1000000000ll / 100) / timer_info.period;
}
ret = rt_task_set_periodic(NULL, TM_NOW, task_period);
if (ret)
{
printf("error while set periodic, code %d\n", ret);
return;
}
// Make sure we are in primary mode before entering the timer loop
rt_task_set_mode(0, T_PRIMARY, NULL);
while (!stop)
{
unsigned long ov;
// Wait for next time period
ret = rt_task_wait_period(&ov);
if (ret && ret != -ETIMEDOUT)
{
printf("error while rt_task_wait_period, code %d (%s)\n", ret,
strerror(-ret));
break;
}
overruns = overruns + ov;
ecrt_master_receive(master);
ecrt_domain_process(domain);
// write application time to master
ecrt_master_application_time(master, rt_timer_tsc2ns(rt_timer_tsc()));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; // sync every cycle
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
/*******************************************************************\
* Card0: Drive the Azimuth Motor (Reaction Wheel) *
\*******************************************************************/
/* Read sin and cos of the inner frame elevation, calculated by mcp */
// cos_el = 1.0; //( COS_EL*0.000030517578125 ) - 1.0;
// sin_el = 0.0; //( SIN_EL*0.000030517578125 ) - 1.0;
//
// v_req_az = 0.0; //(float)(V_REQ_AZ-32768.0)*0.0016276041666666666666666666666667; // = vreq/614.4
//
// //roll, yaw contributions to az both -'ve (?)
// error_az = (gy_ifroll*sin_el + gy_ifyaw*cos_el) + v_req_az;
//
// P_term_az = p_az*error_az;
//
// if( (p_az == 0.0) || (i_az == 0.0) ) {
// az_integral = 0.0;
// } else {
// az_integral = (1.0 - INTEGRAL_CUTOFF)*az_integral + INTEGRAL_CUTOFF*error_az;
// }
//
// I_term_az = az_integral * p_az * i_az;
// if (I_term_az > 100.0) {
// I_term_az = 100.0;
// az_integral = az_integral *0.9;
// }
// if (I_term_az < -100.0) {
// I_term_az = -100.0;
// az_integral = az_integral * 0.9;
// }
// if (P_term_az > 1.0 || P_term_az < -1.0) printf("error_az: %f\tI: %f\tP: %f\n", error_az, I_term_az, P_term_az);
// req_current = 0.5 *(-(P_term_az + I_term_az) ) ;
req_current = 100;
if (req_current > 200)
printf("Error! Requested current is %d\n", req_current);
else {
EC_WRITE_S16(rx_controller_state.current_val, req_current);
}
ecrt_domain_queue(domain);
ecrt_master_send(master);
}
//switch to secondary mode
ret = rt_task_set_mode(T_PRIMARY, 0, NULL);
if (ret)
{
printf("error while rt_task_set_mode, code %d\n", ret);
return;
}
}
static void worker(void *cookie)
{
long long minj = 10000000, maxj = -10000000, dt, sumj = 0;
unsigned long long count = 0;
int err, n;
err = rt_sem_create(&switch_sem, "dispsem", 0, S_FIFO);
if (err) {
warning("failed to create semaphore (%s)\n",
symerror(err));
return;
}
for (n = 0; n < nsamples; n++) {
err = rt_sem_p(&switch_sem, TM_INFINITE);
if (err) {
if (err != -EIDRM && err != -EINVAL)
warning("failed to pend on semaphore (%s)\n",
symerror(err));
exit(EXIT_FAILURE);
}
dt = (long) (rt_timer_tsc() - switch_tsc);
if (switch_count - count > 1) {
lost += switch_count - count;
count = switch_count;
continue;
}
if (++count < warmup)
continue;
if (dt > maxj)
maxj = dt;
if (dt < minj)
minj = dt;
sumj += dt;
if (do_histogram)
add_histogram(dt);
}
rt_sem_delete(&switch_sem);
minjitter = minj;
maxjitter = maxj;
avgjitter = sumj / n;
printf("RTH|%12s|%12s|%12s|%12s\n",
"lat min", "lat avg", "lat max", "lost");
printf("RTD|%12.3f|%12.3f|%12.3f|%12lld\n",
rt_timer_tsc2ns(minjitter) / 1000.0,
rt_timer_tsc2ns(avgjitter) / 1000.0,
rt_timer_tsc2ns(maxjitter) / 1000.0, lost);
if (late)
printf("LATE: %d\n", late);
if (do_histogram)
dump_histogram();
exit(0);
}
void sig_norestart_condwait(void)
{
unsigned long long start;
mutex_t mutex;
cond_t cond;
struct cond_mutex cm = {
.mutex = &mutex,
.cond = &cond,
.tid = thread_self(),
};
thread_t cond_killer_tid;
struct sigaction sa = {
.sa_handler = sighandler,
.sa_flags = 0,
};
sigemptyset(&sa.sa_mask);
fprintf(stderr, "%s\n", __FUNCTION__);
check_unix("sigaction", sigaction(SIGRTMIN, &sa, NULL), 0);
check("mutex_init", mutex_init(&mutex, PTHREAD_MUTEX_DEFAULT, 0), 0);
check("cond_init", cond_init(&cond, 0), 0);
check("mutex_lock", mutex_lock(&mutex), 0);
check("thread_spawn",
thread_spawn(&cond_killer_tid, 2, cond_killer, &cm), 0);
thread_msleep(11);
start = rt_timer_tsc();
sig_seen = 0;
#ifdef XENO_POSIX
check("cond_wait", cond_wait(&cond, &mutex, XN_INFINITE), 0);
#else /* native */
{
int err = cond_wait(&cond, &mutex, XN_INFINITE);
if (err == 0)
err = -EINTR;
check("cond_wait", err, -EINTR);
}
#endif /* native */
check_sleep("cond_wait", start);
check("sig_seen", sig_seen, 1);
check("mutex_unlock", mutex_unlock(&mutex), 0);
check("thread_join", thread_join(cond_killer_tid), 0);
check("mutex_destroy", mutex_destroy(&mutex), 0);
check("cond_destroy", cond_destroy(&cond), 0);
}
void sig_restart_condwait(void)
{
unsigned long long start;
mutex_t mutex;
cond_t cond;
struct cond_mutex cm = {
.mutex = &mutex,
.cond = &cond,
.tid = thread_self(),
};
thread_t cond_killer_tid;
struct sigaction sa = {
.sa_handler = sighandler,
.sa_flags = 0,
};
sigemptyset(&sa.sa_mask);
fprintf(stderr, "%s\n", __FUNCTION__);
check_unix("sigaction", sigaction(SIGRTMIN, &sa, NULL), 0);
check("mutex_init", mutex_init(&mutex, PTHREAD_MUTEX_DEFAULT, 0), 0);
check("cond_init", cond_init(&cond, 0), 0);
check("mutex_lock", mutex_lock(&mutex), 0);
check("thread_spawn",
thread_spawn(&cond_killer_tid, 2, cond_killer, &cm), 0);
thread_msleep(11);
start = rt_timer_tsc();
sig_seen = 0;
#ifdef XENO_POSIX
check("cond_wait", cond_wait(&cond, &mutex, XN_INFINITE), 0);
#else /* native */
{
int err = cond_wait(&cond, &mutex, XN_INFINITE);
if (err == 0)
err = -EINTR;
check("cond_wait", err, -EINTR);
}
#endif /* native */
check_sleep("cond_wait", start);
check("sig_seen", sig_seen, 1);
check("mutex_unlock", mutex_unlock(&mutex), 0);
check("thread_join", thread_join(cond_killer_tid), 0);
check("mutex_destroy", mutex_destroy(&mutex), 0);
check("cond_destroy", cond_destroy(&cond), 0);
}
void *mutex_killer(void *cookie)
{
unsigned long long start;
struct cond_mutex *cm = cookie;
start = rt_timer_tsc();
check("mutex_lock", mutex_lock(cm->mutex), 0);
check_sleep("mutex_lock", start);
check("cond_signal", cond_signal(cm->cond), 0);
thread_msleep(10);
check("thread_kill", thread_kill(cm->tid, SIGRTMIN), 0);
check("mutex_unlock", mutex_unlock(cm->mutex), 0);
return NULL;
}
void worker(void *cookie)
{
long long minj = 10000000, maxj = -10000000, dt, sumj = 0;
unsigned long long count = 0;
int err, n;
err = rt_sem_create(&switch_sem, "dispsem", 0, S_FIFO);
if (err) {
fprintf(stderr,"switch: cannot create semaphore: %s\n",
strerror(-err));
return;
}
for (n=0; n<nsamples; n++) {
err = rt_sem_p(&switch_sem, TM_INFINITE);
if (err) {
if (err != -EIDRM)
fprintf(stderr,"switch: failed to pend on semaphore, code %d\n", err);
rt_task_delete(NULL);
}
if (++count != switch_count) {
count = switch_count;
lost++;
continue;
}
// First few switches are slow.
// Probably due to the Linux <-> RT context migration at task startup.
if (count < ignore)
continue;
dt = (long) (rt_timer_tsc() - switch_tsc);
if (dt > maxj)
maxj = dt;
if (dt < minj)
minj = dt;
sumj += dt;
if (do_histogram)
add_histogram(dt);
}
rt_sem_delete(&switch_sem);
minjitter = minj;
maxjitter = maxj;
avgjitter = sumj / n;
printf("RTH|%12s|%12s|%12s|%12s\n",
"lat min", "lat avg", "lat max", "lost");
printf("RTD|%12.3f|%12.3f|%12.3f|%12lld\n",
rt_timer_tsc2ns(minjitter) / 1000.0,
rt_timer_tsc2ns(avgjitter) / 1000.0,
rt_timer_tsc2ns(maxjitter) / 1000.0, lost);
if (do_histogram)
dump_histogram();
exit(0);
}
void sig_norestart_double(void)
{
unsigned long long start;
mutex_t mutex;
cond_t cond;
struct cond_mutex cm = {
.mutex = &mutex,
.cond = &cond,
.tid = thread_self(),
};
thread_t double_killer_tid;
struct sigaction sa = {
.sa_handler = sighandler,
.sa_flags = 0,
};
sigemptyset(&sa.sa_mask);
fprintf(stderr, "%s\n", __FUNCTION__);
check_unix("sigaction", sigaction(SIGRTMIN, &sa, NULL), 0);
check("mutex_init", mutex_init(&mutex, PTHREAD_MUTEX_DEFAULT, 0), 0);
check("cond_init", cond_init(&cond, 0), 0);
check("mutex_lock", mutex_lock(&mutex), 0);
check("thread_spawn",
thread_spawn(&double_killer_tid, 2, double_killer, &cm), 0);
thread_msleep(11);
sig_seen = 0;
start = rt_timer_tsc();
check("cond_wait", cond_wait(&cond, &mutex, XN_INFINITE), 0);
check_sleep("cond_wait", start);
check("sig_seen", sig_seen, 2);
thread_msleep(10);
check("mutex_unlock", mutex_unlock(&mutex), 0);
check("thread_join", thread_join(double_killer_tid), 0);
check("mutex_destroy", mutex_destroy(&mutex), 0);
check("cond_destroy", cond_destroy(&cond), 0);
}
void sig_restart_double(void)
{
unsigned long long start;
mutex_t mutex;
cond_t cond;
struct cond_mutex cm = {
.mutex = &mutex,
.cond = &cond,
.tid = thread_self(),
};
thread_t double_killer_tid;
struct sigaction sa = {
.sa_handler = sighandler,
.sa_flags = SA_RESTART,
};
sigemptyset(&sa.sa_mask);
fprintf(stderr, "%s\n", __FUNCTION__);
check_unix("sigaction", sigaction(SIGRTMIN, &sa, NULL), 0);
check("mutex_init", mutex_init(&mutex, PTHREAD_MUTEX_DEFAULT, 0), 0);
check("cond_init", cond_init(&cond, 0), 0);
check("mutex_lock", mutex_lock(&mutex), 0);
check("thread_spawn",
thread_spawn(&double_killer_tid, 2, double_killer, &cm), 0);
thread_msleep(11);
sig_seen = 0;
start = rt_timer_tsc();
check("cond_wait", cond_wait(&cond, &mutex, XN_INFINITE), 0);
check_sleep("cond_wait", start);
check("sig_seen", sig_seen, 2);
thread_msleep(10);
check("mutex_unlock", mutex_unlock(&mutex), 0);
check("thread_join", thread_join(double_killer_tid), 0);
check("mutex_destroy", mutex_destroy(&mutex), 0);
check("cond_destroy", cond_destroy(&cond), 0);
}
void latency (void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks;
RT_TIMER_INFO timer_info;
RT_QUEUE q;
rt_queue_create(&q, "queue", 0, 100, 0);
if (!(hard_timer_running = rt_is_hard_timer_running())) {
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
}
err = rt_timer_inquire(&timer_info);
if (err)
{
fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err);
return;
}
nsamples = ONE_BILLION / period_ns / 1;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err = rt_task_set_periodic(NULL,start_ticks,period_ns);
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
long overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
expected_tsc += period_tsc;
err = rt_task_wait_period(NULL);
if (err)
{
if (err != -ETIMEDOUT) {
rt_queue_delete(&q);
rt_task_delete(NULL); /* Timer stopped. */
}
overrun++;
}
dt = (long)(rt_timer_tsc() - expected_tsc);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!(finished || warmup) && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if(!warmup)
{
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if(minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if(maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
struct smpl_t { long minjitter, avgjitter, maxjitter, overrun; } *smpl;
smpl = rt_queue_alloc(&q, sizeof(struct smpl_t));
#if 1
smpl->minjitter = rt_timer_tsc2ns(minj);
smpl->maxjitter = rt_timer_tsc2ns(maxj);
smpl->avgjitter = rt_timer_tsc2ns(sumj / nsamples);
smpl->overrun = goverrun;
rt_queue_send(&q, smpl, sizeof(struct smpl_t), TM_NONBLOCK);
#endif
}
if(warmup && test_loops == WARMUP_TIME)
{
test_loops = 0;
warmup = 0;
}
}
}
void latency (void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks;
RT_TIMER_INFO timer_info;
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
err = rt_timer_inquire(&timer_info);
if (err)
{
fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err);
return;
}
nsamples = ONE_BILLION / period_ns;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err = rt_task_set_periodic(NULL,start_ticks,rt_timer_ns2ticks(period_ns));
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
long overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
expected_tsc += period_tsc;
err = rt_task_wait_period(NULL);
if (err)
{
if (err != -ETIMEDOUT)
{
fprintf(stderr,"latency: wait period failed, code %d\n",err);
rt_task_delete(NULL); /* Timer stopped. */
}
overrun++;
}
dt = (long)(rt_timer_tsc() - expected_tsc);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!(finished || warmup) && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if(!warmup)
{
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if(minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if(maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
}
if(warmup && test_loops == WARMUP_TIME)
{
test_loops = 0;
warmup = 0;
}
}
}
