int main(void)
{
unsigned long long before;
RT_ALARM nalrm;
RT_BUFFER nbuf;
RT_COND ncond;
RT_EVENT nevt;
RT_HEAP nheap;
RT_MUTEX nmtx;
RT_PIPE npipe;
RT_QUEUE nq;
RT_SEM nsem;
RT_TASK ntsk;
int failed = 0;
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_print_auto_init(1);
rt_fprintf(stderr, "Checking for leaks in native skin services\n");
before = get_used();
check_native(rt_alarm_create(&nalrm, NULL));
check_native(rt_alarm_delete(&nalrm));
check_used("alarm", before, failed);
before = get_used();
check_native(rt_buffer_create(&nbuf, NULL, 16384, B_PRIO));
check_native(rt_buffer_delete(&nbuf));
check_used("buffer", before, failed);
before = get_used();
check_native(rt_cond_create(&ncond, NULL));
check_native(rt_cond_delete(&ncond));
check_used("cond", before, failed);
before = get_used();
check_native(rt_event_create(&nevt, NULL, 0, EV_PRIO));
check_native(rt_event_delete(&nevt));
check_used("event", before, failed);
before = get_used();
check_native(rt_heap_create(&nheap, "heap", 16384, H_PRIO | H_SHARED));
check_native(rt_heap_delete(&nheap));
check_used("heap", before, failed);
before = get_used();
check_native(rt_mutex_create(&nmtx, NULL));
check_native(rt_mutex_delete(&nmtx));
check_used("mutex", before, failed);
before = get_used();
check_native(rt_pipe_create(&npipe, NULL, P_MINOR_AUTO, 0));
check_native(rt_pipe_delete(&npipe));
check_used("pipe", before, failed);
before = get_used();
check_native(rt_queue_create(&nq, "queue", 16384, Q_UNLIMITED, Q_PRIO));
check_native(rt_queue_delete(&nq));
check_used("queue", before, failed);
before = get_used();
check_native(rt_sem_create(&nsem, NULL, 0, S_PRIO));
check_native(rt_sem_delete(&nsem));
check_used("sem", before, failed);
before = get_used();
check_native(rt_task_spawn(&ntsk, NULL, 0, 1, T_JOINABLE, empty, NULL));
check_native(rt_task_join(&ntsk));
sleep(1); /* Leave some time for xnheap
* deferred free */
check_used("task", before, failed);
return failed ? EXIT_FAILURE : EXIT_SUCCESS;
}
int __init init_mod(void)
{
int ret = -1, i;
RTIME tick_period, requested_ticks, now;
ec_slave_config_t *sc;
printk(KERN_INFO PFX "Starting...\n");
rt_sem_init(&master_sem, 1);
t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000;
master = ecrt_request_master(0);
if (!master) {
ret = -EBUSY;
printk(KERN_ERR PFX "Requesting master 0 failed!\n");
goto out_return;
}
ecrt_master_callbacks(master, send_callback, receive_callback, master);
printk(KERN_INFO PFX "Registering domain...\n");
if (!(domain1 = ecrt_master_create_domain(master))) {
printk(KERN_ERR PFX "Domain creation failed!\n");
goto out_release_master;
}
// if (!(domain2 = ecrt_master_create_domain(master))) {
// printk(KERN_ERR PFX "Domain2 creation failed!\n");
// goto out_release_master;
// }
printk(KERN_INFO PFX "Configuring PDOs...\n");
if (!(sc = ecrt_master_slave_config(master, yas, yaskawa))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
// ecrt_slave_config_dc(sc, 0x300, 1000000, 0, 0, 0);
if (ecrt_slave_config_sdo8(sc, 0x6060, 0, 8)){
printk(KERN_ERR PFX "Failed to configure SDOs.\n");
goto out_release_master;
}
if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs.\n");
goto out_release_master;
}
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
printk(KERN_ERR PFX "1st motor RX_PDO entry registration failed!\n");
goto out_release_master;
}
printk(KERN_INFO PFX "Activating master...\n");
if (ecrt_master_activate(master)) {
printk(KERN_ERR PFX "Failed to activate master!\n");
goto out_release_master;
}
// Get internal process data for domain
domain1_pd = ecrt_domain_data(domain1);
// domain2_pd = ecrt_domain_data(domain2);
printk(KERN_INFO PFX "Starting cyclic sample thread...\n");
requested_ticks = nano2count(TIMERTICKS);
tick_period = start_rt_timer(requested_ticks);
printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n",
(int) tick_period, (int) requested_ticks);
if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) {
printk(KERN_ERR PFX "Failed to init RTAI task!\n");
goto out_stop_timer;
}
now = rt_get_time();
if (rt_task_make_periodic(&task, now + tick_period, tick_period)) {
printk(KERN_ERR PFX "Failed to run RTAI task!\n");
goto out_stop_task;
}
printk(KERN_INFO PFX "Initialized.\n");
return 0;
out_stop_task:
rt_task_delete(&task);
out_stop_timer:
stop_rt_timer();
out_release_master:
printk(KERN_ERR PFX "Releasing master...\n");
ecrt_release_master(master);
out_return:
rt_sem_delete(&master_sem);
printk(KERN_ERR PFX "Failed to load. Aborting.\n");
return ret;
}
static void* rt_system_thread(void * arg)
{
unsigned char slaves_OP=0;
struct timeval tv;
int64_t ts1, ts2;
SEM * shm_sem;
SEM * sync_sem;
RT_TASK *task;
int i, j;
M3EcSystemShm * sys = (M3EcSystemShm *)arg;
printf("Starting real-time thread\n",0);
RTIME t_last;
int64_t cntr=0;
task = rt_task_init_schmod(nam2num("M3SYSP"), 0, 0, 0, SCHED_FIFO, 0xF);
rt_allow_nonroot_hrt();
if (task==NULL)
{
printf("Failed to create RT-TASK M3SYSP\n",0);
return 0;
}
shm_sem=(SEM*)rt_get_adr(nam2num(SEMNAM_M3LSHM));
if (!shm_sem)
{
printf("Unable to find the SEMNAM_M3LSHM semaphore.\n",0);
rt_task_delete(task);
return 0;
}
else
printf("Allocated shm_sem semaphore %08x \n",shm_sem);
sync_sem=(SEM*)rt_get_adr(nam2num(SEMNAM_M3SYNC));
if (!sync_sem)
{
printf("Unable to find the SEMNAM_M3SYNC semaphore.\n",0);
rt_task_delete(task);
rt_sem_delete(shm_sem);
return 0;
}
else
printf("Allocated sync_sem semaphore %08x \n",sync_sem);
RTIME tick_period = nano2count(RT_TIMER_TICKS_NS);
RTIME now = rt_get_time();
rt_task_make_periodic(task, now + tick_period, tick_period);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
t_last=now;
sys_thread_active=1;
printf("Waiting for Slaves to be in OP state\n");
while((!slaves_OP)&&(!sys_thread_end))
{
rt_sem_wait(sync_sem);
rt_sem_wait(shm_sem);
slaves_OP=1;
for(i=0;i<sys->slaves_responding;i++)
{
if((sys->slave[i].al_state!=8)&&(sys->slave[i].active))
{
//printf("al_state %d : %d\n",i,sys->slave[i].al_state);
slaves_OP=0;
}
}
rt_sem_signal(shm_sem);
rt_task_wait_period();
}
uint64_t tl;
while(!sys_thread_end)
{
rt_sem_wait(sync_sem);
rt_sem_wait(shm_sem);
if(cntr%200==0)
PrintStats(sys);
CalcStats(sys);
cntr++;
rt_sem_signal(shm_sem);
rt_task_wait_period();
}
printf("Exiting RealTime Thread...\n",0);
rt_make_soft_real_time();
rt_task_delete(task);
sys_thread_active=0;
return 0;
}
int main(int argc, char **argv)
{
char tempname[] = "/tmp/sigdebug-XXXXXX";
char buf[BUFSIZ], dev[BUFSIZ];
RT_TASK main_task, rt_task;
long int start, trash, end;
unsigned char *mayday, *p;
struct sigaction sa;
int old_wd_value;
char r, w, x, s;
int tmp_fd, d;
FILE *maps;
int err;
rt_print_auto_init(1);
if (argc < 2 || strcmp(argv[1], "--skip-watchdog") != 0) {
wd = fopen("/sys/module/xeno_nucleus/parameters/"
"watchdog_timeout", "w+");
if (!wd) {
fprintf(stderr, "FAILURE: no watchdog available and "
"--skip-watchdog not specified\n");
exit(EXIT_FAILURE);
}
err = fscanf(wd, "%d", &old_wd_value);
check("get watchdog", err, 1);
err = fprintf(wd, "2");
check("set watchdog", err, 1);
fflush(wd);
}
maps = fopen("/proc/self/maps", "r");
if (maps == NULL) {
perror("open /proc/self/maps");
exit(EXIT_FAILURE);
}
while (fgets(buf, sizeof(buf), maps)) {
if (sscanf(buf, "%lx-%lx %c%c%c%c %lx %x:%x %d%s\n",
&start, &end, &r, &w, &x, &s, &trash,
&d, &d, &d, dev) == 11
&& r == 'r' && x == 'x'
&& !strcmp(dev, "/dev/rtheap") && end - start == 4096) {
printf("mayday page starting at 0x%lx [%s]\n"
"mayday code:", start, dev);
mayday = (unsigned char *)start;
for (p = mayday; p < mayday + 32; p++)
printf(" %.2x", *p);
printf("\n");
}
}
fclose(maps);
sigemptyset(&sa.sa_mask);
sa.sa_sigaction = sigdebug_handler;
sa.sa_flags = SA_SIGINFO;
sigaction(SIGDEBUG, &sa, NULL);
sa.sa_sigaction = dummy_handler;
sigaction(SIGUSR1, &sa, NULL);
printf("mlockall\n");
munlockall();
setup_checkdebug(SIGDEBUG_NOMLOCK);
err = rt_task_shadow(&main_task, "main_task", 0, 0);
check("rt_task_shadow", err, -EINTR);
check_sigdebug_received("SIGDEBUG_NOMLOCK");
mlockall(MCL_CURRENT | MCL_FUTURE);
errno = 0;
tmp_fd = mkstemp(tempname);
check_no_error("mkstemp", -errno);
unlink(tempname);
check_no_error("unlink", -errno);
mem = mmap(NULL, 1, PROT_READ | PROT_WRITE, MAP_SHARED, tmp_fd, 0);
check_no_error("mmap", -errno);
err = write(tmp_fd, "X", 1);
check("write", err, 1);
err = rt_task_shadow(&main_task, "main_task", 0, 0);
check_no_error("rt_task_shadow", err);
err = rt_mutex_create(&prio_invert, "prio_invert");
check_no_error("rt_mutex_create", err);
err = rt_mutex_acquire(&prio_invert, TM_INFINITE);
check_no_error("rt_mutex_acquire", err);
err = rt_sem_create(&send_signal, "send_signal", 0, S_PRIO);
check_no_error("rt_sem_create", err);
err = rt_task_spawn(&rt_task, "rt_task", 0, 1, T_WARNSW | T_JOINABLE,
rt_task_body, NULL);
check_no_error("rt_task_spawn", err);
err = rt_sem_p(&send_signal, TM_INFINITE);
check_no_error("rt_sem_signal", err);
pthread_kill(rt_task_thread, SIGUSR1);
rt_task_sleep(rt_timer_ns2ticks(20000000LL));
err = rt_mutex_release(&prio_invert);
check_no_error("rt_mutex_release", err);
err = rt_task_join(&rt_task);
check_no_error("rt_task_join", err);
err = rt_mutex_delete(&prio_invert);
check_no_error("rt_mutex_delete", err);
err = rt_sem_delete(&send_signal);
check_no_error("rt_sem_delete", err);
if (wd) {
fprintf(wd, "%d", old_wd_value);
fclose(wd);
}
fprintf(stderr, "Test OK\n");
return 0;
}
int pthread_create (pthread_t *tid, const pthread_attr_t *attr,
void *(*start) (void *), void *parameter)
{
int result;
void* stack;
char name[RT_NAME_MAX];
static rt_uint16_t pthread_number = 0;
_pthread_data_t *ptd;
/* tid shall be provided */
RT_ASSERT(tid != RT_NULL);
/* allocate posix thread data */
ptd = (_pthread_data_t*)rt_malloc(sizeof(_pthread_data_t));
if (ptd == RT_NULL) return ENOMEM;
/* clean posix thread data memory */
rt_memset(ptd, 0, sizeof(_pthread_data_t));
ptd->canceled = 0;
ptd->cancelstate = PTHREAD_CANCEL_DISABLE;
ptd->canceltype = PTHREAD_CANCEL_DEFERRED;
ptd->magic = PTHREAD_MAGIC;
if (attr != RT_NULL) ptd->attr = *attr;
else
{
/* use default attribute */
pthread_attr_init(&ptd->attr);
}
rt_snprintf(name, sizeof(name), "pth%02d", pthread_number ++);
if (ptd->attr.stack_base == 0)
{
stack = (void*)rt_malloc(ptd->attr.stack_size);
}
else stack = (void*)(ptd->attr.stack_base);
if (stack == RT_NULL)
{
rt_free(ptd);
return ENOMEM;
}
/* pthread is a static thread object */
ptd->tid = (rt_thread_t) rt_malloc(sizeof(struct rt_thread));
if (ptd->tid == RT_NULL)
{
if (ptd->attr.stack_base ==0) rt_free(stack);
rt_free(ptd);
return ENOMEM;
}
if (ptd->attr.detachstate == PTHREAD_CREATE_JOINABLE)
{
ptd->joinable_sem = rt_sem_create(name, 0, RT_IPC_FLAG_FIFO);
if (ptd->joinable_sem == RT_NULL)
{
if (ptd->attr.stack_base !=0) rt_free(stack);
rt_free(ptd);
return ENOMEM;
}
}
else ptd->joinable_sem = RT_NULL;
/* set parameter */
ptd->thread_entry = start;
ptd->thread_parameter = parameter;
/* initial this pthread to system */
if (rt_thread_init(ptd->tid, name, pthread_entry_stub, ptd,
stack, ptd->attr.stack_size,
ptd->attr.priority, 5) != RT_EOK)
{
if (ptd->attr.stack_base ==0) rt_free(stack);
if (ptd->joinable_sem != RT_NULL) rt_sem_delete(ptd->joinable_sem);
rt_free(ptd);
return EINVAL;
}
/* set pthread id */
*tid = ptd->tid;
/* set pthread cleanup function and ptd data */
(*tid)->cleanup = _pthread_cleanup;
(*tid)->user_data = (rt_uint32_t)ptd;
/* start thread */
result = rt_thread_startup(*tid);
if (result == RT_EOK) return 0;
/* start thread failed */
rt_thread_detach(ptd->tid);
if (ptd->attr.stack_base ==0) rt_free(stack);
if (ptd->joinable_sem != RT_NULL) rt_sem_delete(ptd->joinable_sem);
rt_free(ptd);
return EINVAL;
}
int main(void)
{
RTIME tsr, tss, tsm, trpc;
RT_TASK *mainbuddy;
int i, k, s;
unsigned long msg;
printf("\n\nWait for it ...\n");
if (!(mainbuddy = rt_thread_init(nam2num("MASTER"), 1000, 0, SCHED_FIFO, 0x1))) {
printf("CANNOT INIT TASK %lu\n", nam2num("MASTER"));
exit(1);
}
sem = rt_sem_init(nam2num("SEMAPH"), 1);
change = 0;
for (i = 0; i < NR_RT_TASKS; i++) {
indx[i] = i;
if (!(thread[i] = rt_thread_create(thread_fun, indx + i, 0))) {
printf("ERROR IN CREATING THREAD %d\n", indx[i]);
exit(1);
}
}
do {
msleep(50);
s = 0;
for (i = 0; i < NR_RT_TASKS; i++) {
s += hrt[i];
}
} while (s != NR_RT_TASKS);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
tsr = rt_get_cpu_time_ns();
for (i = 0; i < LOOPS; i++) {
for (k = 0; k < NR_RT_TASKS; k++) {
rt_task_resume(mytask[k]);
}
}
tsr = rt_get_cpu_time_ns() - tsr;
change = 1;
for (k = 0; k < NR_RT_TASKS; k++) {
rt_task_resume(mytask[k]);
}
tss = rt_get_cpu_time_ns();
for (i = 0; i < LOOPS; i++) {
for (k = 0; k < NR_RT_TASKS; k++) {
rt_sem_signal(sem);
}
}
tss = rt_get_cpu_time_ns() - tss;
change = 2;
for (k = 0; k < NR_RT_TASKS; k++) {
rt_sem_signal(sem);
}
tsm = rt_get_cpu_time_ns();
for (i = 0; i < LOOPS; i++) {
for (k = 0; k < NR_RT_TASKS; k++) {
rt_send(mytask[k], 0);
}
}
tsm = rt_get_cpu_time_ns() - tsm;
change = 3;
for (k = 0; k < NR_RT_TASKS; k++) {
rt_send(mytask[k], 0);
}
trpc = rt_get_cpu_time_ns();
for (i = 0; i < LOOPS; i++) {
for (k = 0; k < NR_RT_TASKS; k++) {
rt_rpc(mytask[k], 0, &msg);
}
}
trpc = rt_get_cpu_time_ns() - trpc;
rt_make_soft_real_time();
printf("\n\nFOR %d TASKS: ", NR_RT_TASKS);
printf("TIME %d (ms), SUSP/RES SWITCHES %d, ", (int)(tsr/1000000), 2*NR_RT_TASKS*LOOPS);
printf("SWITCH TIME %d (ns)\n", (int)(tsr/(2*NR_RT_TASKS*LOOPS)));
printf("\nFOR %d TASKS: ", NR_RT_TASKS);
printf("TIME %d (ms), SEM SIG/WAIT SWITCHES %d, ", (int)(tss/1000000), 2*NR_RT_TASKS*LOOPS);
printf("SWITCH TIME %d (ns)\n", (int)(tss/(2*NR_RT_TASKS*LOOPS)));
printf("\nFOR %d TASKS: ", NR_RT_TASKS);
printf("TIME %d (ms), SEND/RCV SWITCHES %d, ", (int)(tsm/1000000), 2*NR_RT_TASKS*LOOPS);
printf("SWITCH TIME %d (ns)\n", (int)(tsm/(2*NR_RT_TASKS*LOOPS)));
printf("\nFOR %d TASKS: ", NR_RT_TASKS);
printf("TIME %d (ms), RPC/RCV-RET SWITCHES %d, ", (int)(tsm/1000000), 2*NR_RT_TASKS*LOOPS);
printf("SWITCH TIME %d (ns)\n\n", (int)(trpc/(2*NR_RT_TASKS*LOOPS)));
fflush(stdout);
end = 1;
for (i = 0; i < NR_RT_TASKS; i++) {
rt_rpc(mytask[i], 0, &msg);
}
do {
msleep(50);
s = 0;
for (i = 0; i < NR_RT_TASKS; i++) {
s += hrt[i];
}
} while (s);
rt_sem_delete(sem);
rt_task_delete(mainbuddy);
for (i = 0; i < NR_RT_TASKS; i++) {
rt_thread_join(thread[i]);
}
return 0;
}
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);
exit(EXIT_FAILURE);
}
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);
}
int main(int argc, char* argv[])
{
unsigned long mtsk_name = nam2num("MTSK");
unsigned long btsk_name = nam2num("BTSK");
unsigned long sem_name = nam2num("SEM");
unsigned long smbx_name = nam2num("SMBX");
unsigned long rmbx_name = nam2num("RMBX");
unsigned long msg;
long long mbx_msg;
long long llmsg = 0xaaaaaaaaaaaaaaaaLL;
RT_TASK *mtsk, *rcvd_from;
SEM *sem;
MBX *smbx, *rmbx;
int pid, count;
if (!(mtsk = rt_task_init_schmod(mtsk_name, 0, 0, 0, SCHED_FIFO, 0x1))) {
printf("CANNOT INIT MASTER TASK\n");
exit(1);
}
printf("MASTER TASK INIT: name = %lx, address = %p.\n", mtsk_name, mtsk);
printf("MASTER TASK STARTS THE ONESHOT TIMER\n");
rt_set_oneshot_mode();
start_rt_timer(nano2count(10000000));
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
rt_sleep(1000000);
printf("MASTER TASK MAKES ITSELF PERIODIC WITH A PERIOD OF 1 ms\n");
rt_task_make_periodic(mtsk, rt_get_time(), nano2count(PERIOD));
rt_sleep(nano2count(1000000000));
count = PERIODIC_LOOPS;
printf("MASTER TASK LOOPS ON WAIT_PERIOD FOR %d PERIODS\n", count);
while(count--) {
printf("PERIOD %d\n", count);
rt_task_wait_period();
}
count = SLEEP_LOOPS;
printf("MASTER TASK LOOPS ON SLEEP 0.1 s FOR %d PERIODS\n", count);
while(count--) {
printf("SLEEPING %d\n", count);
rt_sleep(nano2count(DELAY));
}
printf("MASTER TASK YIELDS ITSELF\n");
rt_task_yield();
printf("MASTER TASK CREATES BUDDY TASK\n");
pid = fork();
if (!pid) {
execl("./slave", "./slave", NULL);
}
printf("MASTER TASK SUSPENDS ITSELF, TO BE RESUMED BY BUDDY TASK\n");
rt_task_suspend(mtsk);
printf("MASTER TASK RESUMED BY BUDDY TASK\n");
if (!(sem = rt_sem_init(sem_name, 0))) {
printf("CANNOT CREATE SEMAPHORE %lx\n", sem_name);
exit(1);
}
printf("MASTER TASK CREATES SEM: name = %lx, address = %p.\n", sem_name, sem);
printf("MASTER TASK WAIT_IF ON SEM\n");
rt_sem_wait_if(sem);
printf("MASTER STEP BLOCKS WAITING ON SEM\n");
rt_sem_wait(sem);
printf("MASTER TASK SIGNALLED BY BUDDY TASK WAKES UP AND BLOCKS WAIT TIMED 1 s ON SEM\n");
rt_sem_wait_timed(sem, nano2count(1000000000));
printf("MASTER TASK DELETES SEM\n");
rt_sem_delete(sem);
printf("MASTER TASK BLOCKS RECEIVING FROM ANY\n");
rcvd_from = rt_receive(0, (void *)&msg);
printf("MASTER TASK RECEIVED MESSAGE %lx FROM BUDDY TASK\n", msg);
printf("MASTER TASK RPCS TO BUDDY TASK THE MESSAGE %lx\n", 0xabcdefL);
rcvd_from = rt_rpc(rcvd_from, 0xabcdef, (void *)&msg);
printf("MASTER TASK RECEIVED THE MESSAGE %lx RETURNED BY BUDDY TASK\n", msg);
//exit(1);
if (!(smbx = rt_mbx_init(smbx_name, 1))) {
printf("CANNOT CREATE MAILBOX %lx\n", smbx_name);
exit(1);
}
if (!(rmbx = rt_mbx_init(rmbx_name, 1))) {
printf("CANNOT CREATE MAILBOX %lx\n", rmbx_name);
exit(1);
}
printf("MASTER TASK CREATED TWO MAILBOXES %p %p %p %p \n", smbx, rmbx, &mtsk_name, &msg);
count = MBX_LOOPS;
while(count--) {
rt_mbx_send(smbx, &llmsg, sizeof(llmsg));
printf("%d MASTER TASK SENDS THE MESSAGE %llx MBX\n", count, llmsg);
mbx_msg = 0;
rt_mbx_receive_timed(rmbx, &mbx_msg, sizeof(mbx_msg), nano2count(MSG_DELAY));
printf("%d MASTER TASK RECEIVED THE MESSAGE %llx FROM MBX\n", count, mbx_msg);
rt_sleep(nano2count(DELAY));
}
printf("MASTER TASK SENDS THE MESSAGE %lx TO BUDDY TO ALLOW ITS END\n", 0xeeeeeeeeL);
rt_send(rcvd_from, 0xeeeeeeee);
printf("MASTER TASK WAITS FOR BUDDY TASK END\n");
while (rt_get_adr(btsk_name)) {
rt_sleep(nano2count(1000000000));
}
printf("MASTER TASK STOPS THE PERIODIC TIMER\n");
stop_rt_timer();
printf("MASTER TASK DELETES MAILBOX %p\n", smbx);
rt_mbx_delete(smbx);
printf("MASTER TASK DELETES MAILBOX %p\n", rmbx);
rt_mbx_delete(rmbx);
printf("MASTER TASK DELETES ITSELF\n");
rt_task_delete(mtsk);
printf("END MASTER TASK\n");
return 0;
}
static int rt_Main(int priority)
{
SEM *hard_timers_cnt;
char name[7];
RTIME rt_BaseTaskPeriod;
struct timespec err_timeout;
int i;
rt_allow_nonroot_hrt();
for (i = 0; i < MAX_NTARGETS; i++) {
sprintf(name,"MNT%d",i);
if (!rt_get_adr(nam2num(name))) break;
}
if (!(rt_MainTask = rt_task_init_schmod(nam2num(name), rt_MainTaskPriority, 0, 0, SCHED_RR, 0xFF))) {
fprintf(stderr,"Cannot init rt_MainTask.\n");
return 1;
}
sem_init(&err_sem, 0, 0);
printf("TARGET STARTS.\n");
pthread_create(&rt_HostInterfaceThread, NULL, rt_HostInterface, NULL);
err_timeout.tv_sec = (long int)(time(NULL)) + 1;
err_timeout.tv_nsec = 0;
if ((sem_timedwait(&err_sem, &err_timeout)) != 0) {
fprintf(stderr, "Target is terminated.\n");
goto finish;
}
pthread_create(&rt_BaseRateThread, NULL, rt_BaseRate, &priority);
err_timeout.tv_sec = (long int)(time(NULL)) + 1;
err_timeout.tv_nsec = 0;
if ((sem_timedwait(&err_sem, &err_timeout)) != 0) {
endInterface = 1;
rt_send(rt_HostInterfaceTask, 0);
pthread_join(rt_HostInterfaceThread, NULL);
fprintf(stderr, "Target is terminated.\n");
goto finish;
}
rt_BaseTaskPeriod = (RTIME) (1e9*get_tsamp());
if (InternTimer) {
WaitTimingEvent = (void *)rt_task_wait_period;
if (!(hard_timers_cnt = rt_get_adr(nam2num("HTMRCN")))) {
if (!ClockTick) {
rt_set_oneshot_mode();
start_rt_timer(0);
rt_BaseRateTick = nano2count(rt_BaseTaskPeriod);
}
else {
rt_set_periodic_mode();
rt_BaseRateTick = start_rt_timer(nano2count(rt_BaseTaskPeriod));
}
hard_timers_cnt = rt_sem_init(nam2num("HTMRCN"), 0);
}
else {
rt_BaseRateTick = nano2count(rt_BaseTaskPeriod);
rt_sem_signal(hard_timers_cnt);
}
}
else {
WaitTimingEvent = (void *)DummyWait;
SendTimingEvent = (void *)DummySend;
}
if (verbose) {
printf("Model : %s .\n", modelname);
printf("Executes on CPU map : %x.\n", CpuMap);
printf("Sampling time : %e (s).\n", get_tsamp());
}
{
int msg;
rt_receive(0, &msg);
}
if (WaitToStart) {
if (verbose) {
printf("Target is waiting to start ... ");
fflush(stdout);
}
rt_task_suspend(rt_MainTask);
}
if (verbose) {
printf("Target is running.\n");
}
rt_return(rt_BaseRateTask, 0);
isRunning = 1;
while (!endex && (!FinalTime || SIM_TIME < FinalTime)) {
msleep(POLL_PERIOD);
}
endBaseRate = 1;
if (!InternTimer) {
SendTimingEvent(TimingEventArg);
}
pthread_join(rt_BaseRateThread, NULL);
isRunning = 0;
endInterface = 1;
rt_send(rt_HostInterfaceTask, 0);
if (verbose) {
printf("Target has been stopped.\n");
}
pthread_join(rt_HostInterfaceThread, NULL);
if (InternTimer) {
if (!rt_sem_wait_if(hard_timers_cnt)) {
rt_sem_delete(hard_timers_cnt);
}
}
finish:
for (i=0 ; i<NSCOPE ; i++)
RT_named_mbx_delete(0, 0, rtaiScope[i].mbx);
for (i=0 ; i<NLOGS ; i++)
RT_named_mbx_delete(0, 0, rtaiLogData[i].mbx);
for (i=0 ; i<NLEDS ; i++)
RT_named_mbx_delete(0, 0, rtaiLed[i].mbx);
for (i=0 ; i<NMETERS ; i++)
RT_named_mbx_delete(0, 0, rtaiMeter[i].mbx);
for ( i=0 ; i<MAX_COMEDI_DEVICES ; i++ ){
if ( ComediDev[i] != NULL ){
comedi_close(ComediDev[i]);
}
}
for ( i=0 ; i<N_DATAIN ; i++){
free( ComediDataIn[i].comdev );
}
for ( i=0 ; i<N_DATAOUT ; i++){
free( ComediDataOut[i].comdev );
}
for ( i=0 ; i<N_DIOIN ; i++){
free( ComediDioIn[i].comdev );
}
for ( i=0 ; i<N_DIOOUT ; i++){
free( ComediDioOut[i].comdev );
}
SA_Output_To_File();
rt_task_delete(rt_MainTask);
printf("TARGET ENDS.\n");
return 0;
}
int cleanup_cmdcrn(void)
{
rt_sem_delete(notEmpty);
rt_sem_delete(notFull);
return 0;
}
int __init init_mod(void)
{
int ret = -1, i;
RTIME tick_period, requested_ticks, now;
ec_slave_config_t *sc;
printk(KERN_INFO PFX "Starting...\n");
rt_sem_init(&master_sem, 1);
t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000;
master = ecrt_request_master(0);
if (!master) {
ret = -EBUSY;
printk(KERN_ERR PFX "Requesting master 0 failed!\n");
goto out_return;
}
ecrt_master_callbacks(master, send_callback, receive_callback, master);
printk(KERN_INFO PFX "Registering domain...\n");
if (!(domain1 = ecrt_master_create_domain(master))) {
printk(KERN_ERR PFX "Domain creation failed!\n");
goto out_release_master;
}
printk(KERN_INFO PFX "Configuring PDOs...\n");
// create configuration for reference clock FIXME
if (!(sc = ecrt_master_slave_config(master, 0, 0, Beckhoff_EK1100))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
for (i = 0; i < NUM_DIG_OUT; i++) {
if (!(sc = ecrt_master_slave_config(master,
DigOutSlavePos(i), Beckhoff_EL2008))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
if (ecrt_slave_config_pdos(sc, EC_END, el2008_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs.\n");
goto out_release_master;
}
off_dig_out[i] = ecrt_slave_config_reg_pdo_entry(sc,
0x7000, 1, domain1, NULL);
if (off_dig_out[i] < 0)
goto out_release_master;
}
if (!(sc = ecrt_master_slave_config(master,
CounterSlavePos, IDS_Counter))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,
0x6020, 0x11, domain1, NULL);
if (off_counter_in < 0)
goto out_release_master;
off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,
0x7020, 1, domain1, NULL);
if (off_counter_out < 0)
goto out_release_master;
// configure SYNC signals for this slave
ecrt_slave_config_dc(sc, 0x0700, 1000000, 440000, 0, 0);
printk(KERN_INFO PFX "Activating master...\n");
if (ecrt_master_activate(master)) {
printk(KERN_ERR PFX "Failed to activate master!\n");
goto out_release_master;
}
// Get internal process data for domain
domain1_pd = ecrt_domain_data(domain1);
printk(KERN_INFO PFX "Starting cyclic sample thread...\n");
requested_ticks = nano2count(TIMERTICKS);
tick_period = start_rt_timer(requested_ticks);
printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n",
(int) tick_period, (int) requested_ticks);
if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) {
printk(KERN_ERR PFX "Failed to init RTAI task!\n");
goto out_stop_timer;
}
now = rt_get_time();
if (rt_task_make_periodic(&task, now + tick_period, tick_period)) {
printk(KERN_ERR PFX "Failed to run RTAI task!\n");
goto out_stop_task;
}
printk(KERN_INFO PFX "Initialized.\n");
return 0;
out_stop_task:
rt_task_delete(&task);
out_stop_timer:
stop_rt_timer();
out_release_master:
printk(KERN_ERR PFX "Releasing master...\n");
ecrt_release_master(master);
out_return:
rt_sem_delete(&master_sem);
printk(KERN_ERR PFX "Failed to load. Aborting.\n");
return ret;
}
int main(void){
/* Perform auto-init of rt_print buffers if the task doesn't do so */
rt_print_auto_init(1);
/* Lock memory : avoid memory swapping for this program */
mlockall(MCL_CURRENT|MCL_FUTURE);
/* Make main a Real-Time thread */
if(rt_task_shadow(NULL, NULL, RT_MAIN_PRI, T_CPU(CPU_ID) ) != SUCCESS){
printf("RT Thread main failed to be created!\n");
exit(1);
}else{
/* print success */
rt_printf("RT Thread main initiated successfully \n");
}
/* Create semaphore for synchronized start */
if(rt_sem_create(&sync_sem, "sync_start", 0, S_PRIO) != SUCCESS){
rt_printf("sync semaphore failed to be created \n");
exit(1);
}
/* Create Barrier */
#ifdef semaphore
if(rt_sem_create(&sem, "Barrier_sem", 0, S_PRIO) != SUCCESS){
rt_printf("Barrier_sem failed to be created \n");
exit(1);
}
#endif
#ifdef mutex
if(rt_mutex_create(&mut, "Barrier_mut") != SUCCESS){
rt_printf("Barrier_mut failed to be created \n");
exit(1);
}
#endif
/* Initialize Real-Time threads */
int* id;
int i;
RT_TASK rt_threads[N_RT_THREADS];
for(i=0; i<N_RT_THREADS; i++){
/* Give the new thread the channel to respond to by argument */
int *method = malloc(sizeof(int));
*method = USE_METHOD;
/* Create new thread */
if(rt_task_create(&rt_threads[i], NULL, 0, RT_THREADS_PRI[i], T_CPU(CPU_ID)|T_JOINABLE) != SUCCESS){
rt_printf("RT Thread %i failed to be created!\n", i);
exit(1);
}
/* Execute task */
if(rt_task_start(&rt_threads[i], RT_FUNC[i], id) != SUCCESS){
rt_printf("RT Thread %i failed to start!\n", (void*) i);
exit(1);
}
/* print success */
rt_printf("RT Thread %i initiated successfully \n", i);
}
/* Wait untill all threads have been blocked, restart them */
rt_task_sleep(SLEEP_DELAY);
if(rt_sem_broadcast(&sync_sem)!= SUCCESS){
rt_printf("main thread failed to broadcast on BARRIER semaphore \n");
exit(1);
}
rt_task_sleep(SLEEP_DELAY);
/* Wait for all threads to terminate */
for(i=0; i<N_RT_THREADS; i++){
rt_task_join(&rt_threads[i]);
}
/* Clean up */
rt_sem_delete(&sem);
rt_sem_delete(&sync_sem);
rt_mutex_delete(&mut);
return 0;
}
void linux_process_termination(void)
{
RT_TASK *task2delete, *task2unblock, *base_linux_tasks[NR_RT_CPUS];
int cpu, slot, nr_task_lists;
pid_t my_pid;
struct task_struct *ltsk;
unsigned long num;
void *adr;
/*
* Linux is just about to schedule *ltsk out of existence.
* With this feature, LXRT frees the real time resources allocated
* by the task ltsk.
*/
ltsk = current;
rt_get_base_linux_task(base_linux_tasks);
nr_task_lists = rt_sched_type() == MUP_SCHED ? NR_RT_CPUS : 1;
rt_global_cli();
for (cpu = 0; cpu < nr_task_lists; cpu++) {
task2delete = base_linux_tasks[cpu];
// Try to find if RTAI was aware of this dying Linux task.
while ((task2delete = task2delete->next) && task2delete->lnxtsk != ltsk);
// First let's free the registered resources.
for (slot = 1; slot <= MAX_SLOTS; slot++) {
if ((num = is_process_registered(ltsk)) > 0) {
adr = rt_get_adr(num);
switch (rt_get_type(num)) {
case IS_SEM:
rt_printk("LXRT Informed releases SEM %p\n", adr);
rt_sem_delete(adr);
rt_free(adr);
break;
case IS_MBX:
rt_printk("LXRT Informed releases MBX %p\n", adr);
rt_mbx_delete(adr);
rt_free(adr);
break;
case IS_PRX:
rt_printk("LXRT Informed releases PRX %p\n", adr);
rt_Proxy_detach(rttask2pid(adr));
break;
// to do: case IS_SHMEM:
}
rt_drg_on_adr(adr);
}
}
// Synchronous IPC pid may need to be released
if ((my_pid = rttask2pid(task2delete))) {
rt_printk("Release vc %04X\n", my_pid);
rt_vc_release(my_pid);
}
if (!task2delete) {
continue; // The user deleted the task but forgot to delete the resources.
}
// Other RTAI tasks may be SEND, RPC or RETURN blocked on task2delete.
Loop:
task2unblock = base_linux_tasks[cpu];
while ((task2unblock = task2unblock->next)) {
if (!(task2unblock->state & (SEND | RPC | RETURN))) {
continue;
} else if (task2unblock->msg_queue.task == task2delete) {
task2unblock->state &= ~(SEND | RPC | RETURN | DELAYED);
LXRT_RESUME(task2unblock);
rt_global_cli();
goto Loop;
}
}
// To do: other RTAI tasks may want to be informed as well.
// Ok, let's delete the task.
if (!rt_task_delete(task2delete)) {
rt_printk("LXRT Informed releases RT %p, lnxpid %d (%p), name %s.\n", task2delete, ltsk->pid, ltsk, ltsk->comm);
rt_free(task2delete->msg_buf[0]);
rt_free(task2delete);
rt_drg_on_adr(task2delete);
break;
}
}
rt_global_sti();
}
void sys_sem_free(sys_sem_t *sem)
{
RT_DEBUG_NOT_IN_INTERRUPT;
rt_sem_delete(*sem);
}
int rtos_mutex_rec_destroy(rt_mutex_t* m )
{
CHK_LXRT_CALL();
CHK_LXRT_PTR(m->sem);
return rt_sem_delete(m->sem);
}
int main(void)
{
time_t aclock;
struct tm *newtime;
i = 0;
printf("\n\nBeginning the Deadline Monotonic Scheduler\n\n");
/* Dados de controle da Thread */
pthread_t *threadControle_11;
pthread_t *threadControle_12;
pthread_t *threadControle_13;
pthread_t *threadControle_21;
pthread_t *threadControle_22;
pthread_t *threadControle_31;
pthread_t *threadControle_32;
pthread_t *threadControle_33;
rMutex = rt_sem_init(nam2num("MySEM"), 1);
rt_set_periodic_mode();
sampling = start_rt_timer(nano2count(TICK));
printf("\nInit module function\n");
threadControle_11 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_11 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n\n");
return(NULL);
}
if(pthread_create(threadControle_11, NULL, signalIchi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n");
return(NULL);
}
threadControle_12 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_12 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n\n");
return(NULL);
}
if(pthread_create(threadControle_12, NULL, signalIchi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n");
return(NULL);
}
threadControle_13 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_13 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n\n");
return(NULL);
}
if(pthread_create(threadControle_13, NULL, signalIchi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n");
return(NULL);
}
threadControle_21 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_21 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_21, NULL, signalNi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
threadControle_22 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_22 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_22, NULL, signalNi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
threadControle_31 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_31 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_31, NULL, signalSan, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
threadControle_32 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_32 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_32, NULL, signalSan, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
threadControle_33 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_33 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_33, NULL, signalSan, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf(" Inicio =======> %s", asctime(newtime));
begin = 3600 * newtime->tm_hour + 60 * newtime->tm_min + newtime->tm_sec;
while(!getchar());
rt_sem_wait(rMutex);
rt_sem_delete(rMutex);
pthread_cancel((pthread_t) *threadControle_11);
pthread_cancel((pthread_t) *threadControle_12);
pthread_cancel((pthread_t) *threadControle_13);
pthread_cancel((pthread_t) *threadControle_21);
pthread_cancel((pthread_t) *threadControle_22);
pthread_cancel((pthread_t) *threadControle_31);
pthread_cancel((pthread_t) *threadControle_32);
pthread_cancel((pthread_t) *threadControle_33);
printf("\nFim do Escalonamento\n");
return 0;
}
int rtos_sem_destroy(rt_sem_t* m )
{
CHK_LXRT_CALL();
return rt_sem_delete(m->sem);
}
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);
}
int main(int argc, char* argv[])
{
int semDonnee;
int semObs;
int mutDonnee;
int mutObs;
/*initialize random seed*/
srand(time(NULL));
mutDonnee = rt_mutex_create(&mutex_donnee,"mutexDonnee");
mutObs = rt_mutex_create(&mutex_obs,"mutexObstacle");
semDonnee = rt_sem_create(&sem_donnee,"Semaphore Donnee",SEM_INIT,SEM_MODE);
semObs = rt_sem_create(&sem_obs,"Semaphore Obstacle",SEM_INIT,SEM_MODE);
/* Avoids memory swapping for this program */
mlockall(MCL_CURRENT|MCL_FUTURE);
gettimeofday(&tim, NULL);
start = tim.tv_sec+(tim.tv_usec/1000000.0);
/*
* Arguments: &task,
* name,
* stack size (0=default),
* priority,
* mode (FPU, start suspended, ...)
*/
rt_task_create(&sensorTaskt, "sensor", 0, 99, 0);
rt_task_create(&computationTaskt, "computation", 0, 99, 0);
rt_task_create(&printTaskt, "print", 0, 99, 0);
/*
* Arguments: &task,
* task function,
* function argument
*/
rt_task_start(&sensorTaskt, &sensorTask, NULL);
rt_task_start(&computationTaskt, &computationTask, NULL);
rt_task_start(&printTaskt, &printTask, NULL);
pause();
rt_task_delete(&sensorTask);
rt_task_delete(&computationTask);
rt_task_delete(&printTask);
rt_sem_delete(&sem_donnee);
rt_sem_delete(&sem_obs);
rt_mutex_delete(&mutex_donnee);
rt_mutex_delete(&mutex_obs);
obstacle* temp1 = obstacleList;
obstacle* temp2;
while(temp1->nxt != NULL){
temp2 = temp1->nxt;
free(temp1);
temp1=temp2;
}
free(temp1);
}
