00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 #ifndef _VM_ASM_RTAI_XENO_H
00047 #define _VM_ASM_RTAI_XENO_H
00048
00049 #include <sys/time.h>
00050 #include <errno.h>
00051 #include <malloc.h>
00052 #include <stdlib.h>
00053 #include <unistd.h>
00054 #include <string.h>
00055 #include <signal.h>
00056 #include <stdio.h>
00057 #include <setjmp.h>
00058 #include <asm/rtai_xnatomic.h>
00059 #include <xenomai/fusion.h>
00060
00061 #define XNARCH_DEFAULT_TICK 1000000
00062 #define XNARCH_IRQ_MAX 32
00063 #define XNARCH_SIG_RESTART SIGUSR1
00064 #define XNARCH_HOST_TICK 0
00065 #define XNARCH_APERIODIC_PREC 0
00066 #define XNARCH_SCHED_LATENCY 0
00067
00068 struct xnthread;
00069
00070 typedef struct xnarchtcb {
00071
00072 const char *name;
00073 struct xnthread *thread;
00074 void *khandle;
00075 void (*entry)(void *);
00076 void *cookie;
00077 int imask;
00078 jmp_buf rstenv;
00079 pid_t ppid;
00080 int syncflag;
00081 pthread_t thid;
00082
00083
00084
00085 unsigned stacksize;
00086 unsigned long *stackbase;
00087
00088 } xnarchtcb_t;
00089
00090 extern xnarchtcb_t *vml_root;
00091
00092 extern xnarchtcb_t *vml_current;
00093
00094 extern int vml_irqlock;
00095
00096 typedef void *xnarch_fltinfo_t;
00097
00098 #define xnarch_fault_trap(fi) (0)
00099 #define xnarch_fault_code(fi) (0)
00100 #define xnarch_fault_pc(fi) (0L)
00101
00102 #define XNARCH_THREAD_COOKIE NULL
00103 #define XNARCH_THREAD_STACKSZ 0
00104 #define XNARCH_ROOT_STACKSZ 0
00105
00106 #define xnarch_printf printf
00107 #define printk printf
00108 #define xnarch_llimd(ll,m,d) ((int)(ll) * (int)(m) / (int)(d))
00109 #define xnarch_imuldiv(i,m,d) ((int)(i) * (int)(m) / (int)(d))
00110 #define xnarch_ulldiv(ull,uld,rem) (((*rem) = ((ull) % (uld))), (ull) / (uld))
00111 #define xnarch_ullmod(ull,uld,rem) ((*rem) = ((ull) % (uld)))
00112
00113 #define xnarch_stack_size(tcb) ((tcb)->stacksize)
00114 #define xnarch_fpu_ptr(tcb) (NULL)
00115
00116 static inline int __attribute__ ((unused))
00117 xnarch_read_environ (const char *name, const char **ptype, void *pvar)
00118
00119 {
00120 char *value;
00121
00122 if (*ptype == NULL)
00123 return 0;
00124
00125 *ptype = NULL;
00126
00127 value = getenv(name);
00128
00129 if (!value)
00130 return -1;
00131
00132 if (**ptype == 's')
00133 *((char **)pvar) = value;
00134 else
00135 *((int *)pvar) = atoi(value);
00136
00137 return 1;
00138 }
00139
00140 static int inline xnarch_lock_irq (void) {
00141
00142 extern int vml_irqlock;
00143 return xnarch_atomic_xchg(&vml_irqlock,1);
00144 }
00145
00146 static inline void xnarch_unlock_irq (int x) {
00147
00148 extern int vml_irqlock, vml_irqpend;
00149
00150 if (!x && vml_irqlock)
00151 {
00152 if (xnarch_atomic_xchg(&vml_irqpend,0))
00153 __pthread_release_vm(&vml_irqlock);
00154 else
00155 vml_irqlock = 0;
00156 }
00157 }
00158
00159 void xnarch_sync_irq(void);
00160
00161 int xnarch_setimask(int imask);
00162
00163 typedef int spl_t;
00164
00165 #define splhigh(x) ((x) = xnarch_lock_irq())
00166 #define splexit(x) xnarch_unlock_irq(x)
00167 #define splnone() xnarch_unlock_irq(0)
00168
00169
00170 #define vartype(var) var ## _ ## tYpE
00171 #define MODULE_DESCRIPTION(s);
00172 #define MODULE_LICENSE(s);
00173 #define MODULE_AUTHOR(s);
00174 #define MODULE_PARM(var,type) static const char *vartype(var) = type
00175 #define MODULE_PARM_DESC(var,desc);
00176 #define MODULE_PARM_VALUE(var) (xnarch_read_environ(#var,&vartype(var),&var),var)
00177
00178
00179 #define EXPORT_SYMBOL(sym);
00180 #define module_init(sym);
00181 #define module_exit(sym);
00182
00183 #ifdef __cplusplus
00184 extern "C" {
00185 #endif
00186
00187 void xnpod_welcome_thread(struct xnthread *);
00188
00189 #ifdef XENO_INTR_MODULE
00190
00191 int vml_irqlock = 0;
00192
00193 int vml_irqpend = 0;
00194
00195 void xnarch_sync_irq (void)
00196
00197 {
00198 if (vml_irqlock)
00199 __pthread_hold_vm(&vml_irqpend);
00200 }
00201
00202 static inline int xnarch_hook_irq (unsigned irq,
00203 void (*handler)(unsigned irq,
00204 void *cookie),
00205 void *cookie) {
00206 if (irq == 0)
00207 return -EINVAL;
00208
00209 return -ENOSYS;
00210 }
00211
00212 static inline int xnarch_release_irq (unsigned irq) {
00213 return -ENOSYS;
00214 }
00215
00216 static inline int xnarch_enable_irq (unsigned irq) {
00217 return -ENOSYS;
00218 }
00219
00220 static inline int xnarch_disable_irq (unsigned irq) {
00221 return -ENOSYS;
00222 }
00223
00224 static inline void xnarch_isr_chain_irq (unsigned irq) {
00225
00226 }
00227
00228 static inline void xnarch_isr_enable_irq (unsigned irq) {
00229
00230 }
00231
00232 #endif
00233
00234 #ifdef XENO_MAIN_MODULE
00235
00236 int __xeno_main_init(void);
00237
00238 void __xeno_main_exit(void);
00239
00240 int __xeno_skin_init(void);
00241
00242 void __xeno_skin_exit(void);
00243
00244 int __xeno_user_init(void);
00245
00246 void __xeno_user_exit(void);
00247
00248 int vml_done = 0;
00249
00250 static inline int xnarch_init (void) {
00251 return 0;
00252 }
00253
00254 static inline void xnarch_exit (void) {
00255 }
00256
00257 static void xnarch_restart_handler (int sig) {
00258
00259 longjmp(vml_current->rstenv,1);
00260 }
00261
00262 void xnarch_exit_handler (int sig)
00263
00264 {
00265 vml_done = 1;
00266 __pthread_activate_vm(vml_root->khandle,vml_current->khandle);
00267 exit(99);
00268 }
00269
00270 int main (int argc, char *argv[])
00271
00272 {
00273 struct sigaction sa;
00274 int err;
00275
00276 if (geteuid() !=0)
00277 {
00278 fprintf(stderr,"This program must be run with root privileges");
00279 exit(1);
00280 }
00281
00282 err = __xeno_main_init();
00283
00284 if (err)
00285 {
00286 fprintf(stderr,"main_init() failed, err=%x\n",err);
00287 exit(2);
00288 }
00289
00290 err = __xeno_skin_init();
00291
00292 if (err)
00293 {
00294 fprintf(stderr,"skin_init() failed, err=%x\n",err);
00295 exit(3);
00296 }
00297
00298 err = __xeno_user_init();
00299
00300 if (err)
00301 {
00302 fprintf(stderr,"user_init() failed, err=%x\n",err);
00303 exit(4);
00304 }
00305
00306 sa.sa_handler = &xnarch_restart_handler;
00307 sigemptyset(&sa.sa_mask);
00308 sa.sa_flags = SA_RESTART;
00309 sigaction(XNARCH_SIG_RESTART,&sa,NULL);
00310
00311 sa.sa_handler = &xnarch_exit_handler;
00312 sa.sa_flags = 0;
00313 sigaction(SIGTERM,&sa,NULL);
00314 sigaction(SIGHUP,&sa,NULL);
00315 sigaction(SIGINT,&sa,NULL);
00316
00317 while (!vml_done)
00318 __pthread_idle_vm(&vml_irqlock);
00319
00320 __xeno_user_exit();
00321 __xeno_skin_exit();
00322 __xeno_main_exit();
00323
00324 exit(0);
00325 }
00326
00327 #endif
00328
00329 #ifdef XENO_HEAP_MODULE
00330
00331 void *xnarch_sysalloc (unsigned bytes) {
00332 return malloc(bytes);
00333 }
00334
00335 void xnarch_sysfree (void *chunk, unsigned bytes) {
00336 free(chunk);
00337 }
00338
00339 #else
00340
00341 void *xnarch_sysalloc(unsigned bytes);
00342
00343 void xnarch_sysfree(void *chunk,
00344 unsigned bytes);
00345
00346 #endif
00347
00348 #ifdef XENO_TIMER_MODULE
00349
00350 void *vml_timer_handle;
00351
00352 static inline void xnarch_stop_timer (void) {
00353 __pthread_cancel_vm(vml_timer_handle,NULL);
00354 }
00355
00356 #endif
00357
00358 #ifdef XENO_POD_MODULE
00359
00360 extern void *vml_timer_handle;
00361
00362 xnsysinfo_t vml_info;
00363
00364 xnarchtcb_t *vml_root;
00365
00366 xnarchtcb_t *vml_current;
00367
00368 #define xnarch_relay_tick()
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385 struct xnarch_tick_parms {
00386 time_t sec;
00387 long nsec;
00388 void (*tickhandler)(void);
00389 pid_t ppid;
00390 int syncflag;
00391 };
00392
00393 static void *xnarch_timer_thread (void *cookie)
00394
00395 {
00396 struct xnarch_tick_parms *p = (struct xnarch_tick_parms *)cookie;
00397 void (*tickhandler)(void) = p->tickhandler;
00398 struct timespec ts;
00399
00400 pthread_create_rt("vmtimer",NULL,p->ppid,&p->syncflag,&vml_timer_handle);
00401 pthread_migrate_rt(FUSION_RTAI_DOMAIN);
00402
00403 ts.tv_sec = p->sec;
00404 ts.tv_nsec = p->nsec;
00405
00406 for (;;)
00407 {
00408 nanosleep(&ts,NULL);
00409 xnarch_sync_irq();
00410 tickhandler();
00411 }
00412
00413 return NULL;
00414 }
00415
00416 static inline void xnarch_start_timer (unsigned long nstick,
00417 void (*tickhandler)(void))
00418 {
00419 struct xnarch_tick_parms parms;
00420 struct sched_param param;
00421 unsigned long tickval;
00422 pthread_attr_t thattr;
00423 pthread_t thid;
00424
00425 pthread_attr_init(&thattr);
00426 pthread_attr_setdetachstate(&thattr,PTHREAD_CREATE_DETACHED);
00427 pthread_attr_setschedpolicy(&thattr,SCHED_FIFO);
00428 param.sched_priority = sched_get_priority_min(SCHED_FIFO) + 2;
00429 pthread_attr_setschedparam(&thattr,¶m);
00430
00431 if (vml_info.tickval > nstick)
00432 {
00433 fprintf(stderr,"Xenomai/VM: warning: VM tick freq > nucleus tick freq\n");
00434 fprintf(stderr," : rounding VM tick to %lu us\n",vml_info.tickval / 1000);
00435 tickval = vml_info.tickval;
00436 }
00437 else
00438 {
00439 tickval = ((nstick + vml_info.tickval - 1) / vml_info.tickval) * vml_info.tickval;
00440
00441 if (tickval != nstick)
00442 {
00443 fprintf(stderr,"Xenomai/VM: warning: VM tick not a multiple of nucleus tick\n");
00444 fprintf(stderr," : rounding VM tick to %lu us\n",tickval / 1000);
00445 }
00446 }
00447
00448 parms.sec = tickval / 1000000000;
00449 parms.nsec = tickval % 1000000000;
00450 parms.tickhandler = tickhandler;
00451 parms.syncflag = 0;
00452 parms.ppid = getpid();
00453
00454 pthread_create(&thid,&thattr,&xnarch_timer_thread,&parms);
00455 pthread_sync_rt(&parms.syncflag);
00456 pthread_start_rt(vml_timer_handle);
00457 }
00458
00459 static inline void xnarch_leave_root(xnarchtcb_t *rootcb) {
00460 }
00461
00462 static inline void xnarch_enter_root(xnarchtcb_t *rootcb) {
00463 }
00464
00465 static inline void xnarch_switch_to (xnarchtcb_t *out_tcb,
00466 xnarchtcb_t *in_tcb) {
00467 vml_current = in_tcb;
00468 __pthread_activate_vm(in_tcb->khandle,out_tcb->khandle);
00469 }
00470
00471 static inline void xnarch_finalize_and_switch (xnarchtcb_t *dead_tcb,
00472 xnarchtcb_t *next_tcb) {
00473 vml_current = next_tcb;
00474 __pthread_cancel_vm(dead_tcb->khandle,next_tcb->khandle);
00475 }
00476
00477 static inline void xnarch_finalize_no_switch (xnarchtcb_t *dead_tcb) {
00478
00479 __pthread_cancel_vm(dead_tcb->khandle,NULL);
00480 }
00481
00482 static inline void xnarch_save_fpu(xnarchtcb_t *tcb) {
00483
00484 }
00485
00486 static inline void xnarch_restore_fpu(xnarchtcb_t *tcb) {
00487
00488 }
00489
00490 static inline void xnarch_init_root_tcb (xnarchtcb_t *tcb,
00491 struct xnthread *thread,
00492 const char *name)
00493 {
00494 struct sched_param param;
00495
00496 param.sched_priority = sched_get_priority_min(SCHED_FIFO);
00497
00498 if (sched_setscheduler(0,SCHED_FIFO,¶m) < 0 ||
00499 pthread_info_rt(&vml_info) < 0)
00500 {
00501 perror("Xenomai/VM");
00502 exit(1);
00503 }
00504
00505 pthread_init_rt("vmroot",tcb,&tcb->khandle);
00506 pthread_migrate_rt(FUSION_RTAI_DOMAIN);
00507
00508 tcb->name = name;
00509 vml_root = vml_current = tcb;
00510 }
00511
00512 static inline void xnarch_init_tcb (xnarchtcb_t *tcb,
00513 void *adcookie) {
00514 tcb->khandle = NULL;
00515 }
00516
00517 static void *xnarch_thread_trampoline (void *cookie)
00518
00519 {
00520 xnarchtcb_t *tcb = (xnarchtcb_t *)cookie;
00521
00522 if (!setjmp(tcb->rstenv))
00523 {
00524
00525 pthread_create_rt(tcb->name,tcb,tcb->ppid,&tcb->syncflag,&tcb->khandle);
00526 pthread_migrate_rt(FUSION_RTAI_DOMAIN);
00527 }
00528
00529 xnarch_setimask(tcb->imask);
00530
00531 xnpod_welcome_thread(tcb->thread);
00532
00533 tcb->entry(tcb->cookie);
00534
00535 return NULL;
00536 }
00537
00538 static inline void xnarch_init_thread (xnarchtcb_t *tcb,
00539 void (*entry)(void *),
00540 void *cookie,
00541 int imask,
00542 struct xnthread *thread,
00543 char *name)
00544 {
00545 struct sched_param param;
00546 pthread_attr_t thattr;
00547
00548 if (tcb->khandle)
00549 {
00550 pthread_kill(tcb->thid,XNARCH_SIG_RESTART);
00551 return;
00552 }
00553
00554 tcb->imask = imask;
00555 tcb->entry = entry;
00556 tcb->cookie = cookie;
00557 tcb->thread = thread;
00558 tcb->name = name;
00559 tcb->ppid = getpid();
00560 tcb->syncflag = 0;
00561
00562 pthread_attr_init(&thattr);
00563 pthread_attr_setdetachstate(&thattr,PTHREAD_CREATE_DETACHED);
00564 pthread_attr_setschedpolicy(&thattr,SCHED_FIFO);
00565 param.sched_priority = sched_get_priority_min(SCHED_FIFO);
00566 pthread_attr_setschedparam(&thattr,¶m);
00567 pthread_create(&tcb->thid,&thattr,&xnarch_thread_trampoline,tcb);
00568
00569 pthread_sync_rt(&tcb->syncflag);
00570 }
00571
00572 static inline void xnarch_init_fpu(xnarchtcb_t *tcb) {
00573
00574 }
00575
00576 int xnarch_setimask (int imask)
00577
00578 {
00579 spl_t s;
00580 splhigh(s);
00581 splexit(!!imask);
00582 return !!s;
00583 }
00584
00585 #define xnarch_notify_ready()
00586
00587 #endif
00588
00589 extern xnsysinfo_t vml_info;
00590
00591 void xnarch_exit_handler(int);
00592
00593 static inline unsigned long long xnarch_tsc_to_ns (unsigned long long ts) {
00594 return ts;
00595 }
00596
00597 static inline unsigned long long xnarch_ns_to_tsc (unsigned long long ns) {
00598 return ns;
00599 }
00600
00601 static inline unsigned long long xnarch_get_cpu_time (void) {
00602 unsigned long long t;
00603 pthread_time_rt(&t);
00604 return t;
00605 }
00606
00607 static inline unsigned long long xnarch_get_cpu_tsc (void) {
00608 return xnarch_get_cpu_time();
00609 }
00610
00611 static inline unsigned long long xnarch_get_cpu_freq (void) {
00612 return vml_info.cpufreq;
00613 }
00614
00615 static inline void xnarch_halt (const char *emsg) {
00616 fprintf(stderr,"Xenomai/VM: fatal: %s\n",emsg);
00617 fflush(stderr);
00618 xnarch_exit_handler(SIGKILL);
00619 exit(99);
00620 }
00621
00622 #ifdef __cplusplus
00623 }
00624 #endif
00625
00626
00627 #define XNARCH_DECL_DISPLAY_CONTEXT();
00628 #define xnarch_init_display_context(obj)
00629 #define xnarch_create_display(obj,name,tag)
00630 #define xnarch_delete_display(obj)
00631 #define xnarch_post_graph(obj,state)
00632 #define xnarch_post_graph_if(obj,state,cond)
00633
00634 #endif