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 _RTAI_ASM_UVM_SYSTEM_H
00047 #define _RTAI_ASM_UVM_SYSTEM_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 <rtai_config.h>
00059 #include <nucleus/asm/atomic.h>
00060 #include <nucleus/fusion.h>
00061
00062
00063 #define vartype(var) var ## _ ## tYpE
00064 #define MODULE_DESCRIPTION(s);
00065 #define MODULE_LICENSE(s);
00066 #define MODULE_AUTHOR(s);
00067 #define MODULE_PARM(var,type) static const char *vartype(var) = type
00068 #define MODULE_PARM_DESC(var,desc);
00069 #define MODULE_PARM_VALUE(var) (xnarch_read_environ(#var,&vartype(var),&var),var)
00070
00071
00072 #define EXPORT_SYMBOL(sym);
00073 #define module_init(sym);
00074 #define module_exit(sym);
00075
00076 typedef int spl_t;
00077
00078 typedef unsigned long cpumask_t;
00079
00080 #ifdef CONFIG_SMP
00081 #error "SMP not supported for UVM yet"
00082 #endif
00083
00084 extern int vml_irqlock;
00085
00086 #define splhigh(x) ((x) = xnarch_lock_irq())
00087 #define splexit(x) xnarch_unlock_irq(x)
00088 #define splnone() xnarch_unlock_irq(0)
00089
00090 typedef unsigned long xnlock_t;
00091
00092 #define XNARCH_LOCK_UNLOCKED 0
00093
00094 #define xnlock_init(lock) do { } while(0)
00095 #define xnlock_get_irqsave(lock,x) ((x) = xnarch_lock_irq())
00096 #define xnlock_put_irqrestore(lock,x) xnarch_unlock_irq(x)
00097 #define xnlock_clear_irqoff(lock) xnarch_lock_irq()
00098 #define xnlock_clear_irqon(lock) xnarch_unlock_irq(0)
00099
00100 #define XNARCH_NR_CPUS 1
00101
00102 #define XNARCH_DEFAULT_TICK 1000000
00103 #define XNARCH_SIG_RESTART SIGUSR1
00104 #define XNARCH_HOST_TICK 0
00105
00106 #define XNARCH_THREAD_STACKSZ 0
00107 #define XNARCH_ROOT_STACKSZ 0
00108
00109 #define XNARCH_PROMPT "RTAI[nucleus/UVM]: "
00110 #define xnarch_loginfo(fmt,args...) fprintf(stdout, XNARCH_PROMPT fmt , ##args)
00111 #define xnarch_logwarn(fmt,args...) fprintf(stderr, XNARCH_PROMPT fmt , ##args)
00112 #define xnarch_logerr(fmt,args...) fprintf(stderr, XNARCH_PROMPT fmt , ##args)
00113 #define xnarch_printf(fmt,args...) fprintf(stdout, fmt, ##args)
00114 #define printk(fmt,args...) xnarch_loginfo(fmt, ##args)
00115
00116 typedef unsigned long xnarch_cpumask_t;
00117 #define xnarch_num_online_cpus() XNARCH_NR_CPUS
00118 #define xnarch_cpu_online_map ((1<<xnarch_num_online_cpus()) - 1)
00119 #define xnarch_cpu_set(cpu, mask) ((mask) |= 1 << (cpu))
00120 #define xnarch_cpu_clear(cpu, mask) ((mask) &= 1 << (cpu))
00121 #define xnarch_cpus_clear(mask) ((mask) = 0UL)
00122 #define xnarch_cpu_isset(cpu, mask) (!!((mask) & (1 << (cpu))))
00123 #define xnarch_cpus_and(dst, src1, src2) ((dst) = (src1) & (src2))
00124 #define xnarch_cpus_equal(mask1, mask2) ((mask1) == (mask2))
00125 #define xnarch_cpus_empty(mask) ((mask) == 0UL)
00126 #define xnarch_cpumask_of_cpu(cpu) (1 << (cpu))
00127 #define xnarch_first_cpu(mask) (ffnz(mask))
00128 #define XNARCH_CPU_MASK_ALL (~0UL)
00129
00130 #define xnarch_ullmod(ull,uld,rem) ((*rem) = ((ull) % (uld)))
00131 #define xnarch_uldivrem(ull,uld,rem) ((u_long)xnarch_ulldiv((ull),(uld),(rem)))
00132 #define xnarch_uldiv(ull, d) xnarch_uldivrem(ull, d, NULL)
00133 #define xnarch_ulmod(ull, d) ({ u_long _rem; \
00134 rthal_uldivrem(ull,uld,&_rem); _rem; })
00135
00136 static inline int xnarch_imuldiv(int i, int mult, int div)
00137 {
00138 unsigned long long ull = (unsigned long long) (unsigned) i * (unsigned) mult;
00139 return ull / (unsigned) div;
00140 }
00141
00142 static inline long long xnarch_llimd(long long ll, u_long m, u_long d) {
00143
00144 unsigned long long h, l, ull = (unsigned long long) ll;
00145 u_long qh, rh, ql;
00146
00147 h = (ull >> 32) * m;
00148 l = (ull & 0xffffffff) * m;
00149 h += l >> 32;
00150 l &= 0xffffffff;
00151
00152 qh = h / d;
00153 rh = h % d;
00154 l += ((unsigned long long) rh) << 32;
00155
00156 ql = l / d;
00157 ull = (((unsigned long long) qh) << 32) + ql;
00158 return (long long) ull;
00159 }
00160
00161 static inline unsigned long long xnarch_ullmul(unsigned long m1,
00162 unsigned long m2) {
00163 return (unsigned long long) m1 * m2;
00164 }
00165
00166 static inline unsigned long long xnarch_ulldiv (unsigned long long ull,
00167 unsigned long uld,
00168 unsigned long *rem)
00169 {
00170 if (rem)
00171 *rem = ull % uld;
00172
00173 return ull / uld;
00174 }
00175
00176 static inline unsigned long ffnz (unsigned long word) {
00177 return ffs((int)word) - 1;
00178 }
00179
00180 #define xnarch_stack_size(tcb) ((tcb)->stacksize)
00181 #define xnarch_fpu_ptr(tcb) (NULL)
00182
00183 struct xnthread;
00184
00185 typedef struct xnarchtcb {
00186
00187 const char *name;
00188 struct xnthread *thread;
00189 void *khandle;
00190 void (*entry)(void *);
00191 void *cookie;
00192 int imask;
00193 jmp_buf rstenv;
00194 pid_t ppid;
00195 int syncflag;
00196 pthread_t thid;
00197
00198
00199
00200 unsigned stacksize;
00201 unsigned long *stackbase;
00202
00203 } xnarchtcb_t;
00204
00205 extern xnarchtcb_t *vml_root;
00206
00207 extern xnarchtcb_t *vml_current;
00208
00209 typedef void *xnarch_fltinfo_t;
00210
00211 #define xnarch_fault_trap(fi) (0)
00212 #define xnarch_fault_code(fi) (0)
00213 #define xnarch_fault_pc(fi) (0L)
00214
00215 typedef struct xnarch_heapcb {
00216
00217 #if (__GNUC__ <= 2)
00218 int old_gcc_dislikes_emptiness;
00219 #endif
00220
00221 } xnarch_heapcb_t;
00222
00223 static inline void xnarch_init_heapcb (xnarch_heapcb_t *cb) {
00224 }
00225
00226 static inline int __attribute__ ((unused))
00227 xnarch_read_environ (const char *name, const char **ptype, void *pvar)
00228
00229 {
00230 char *value;
00231
00232 if (*ptype == NULL)
00233 return 0;
00234
00235 *ptype = NULL;
00236
00237 value = getenv(name);
00238
00239 if (!value)
00240 return -1;
00241
00242 if (**ptype == 's')
00243 *((char **)pvar) = value;
00244 else
00245 *((int *)pvar) = atoi(value);
00246
00247 return 1;
00248 }
00249
00250 static int inline xnarch_lock_irq (void) {
00251
00252 return xnarch_atomic_xchg(&vml_irqlock,1);
00253 }
00254
00255 static inline void xnarch_unlock_irq (int x) {
00256
00257 extern int vml_irqpend;
00258
00259 if (!x && vml_irqlock)
00260 {
00261 if (xnarch_atomic_xchg(&vml_irqpend,0))
00262 __pthread_release_vm(&vml_irqlock);
00263 else
00264 vml_irqlock = 0;
00265 }
00266 }
00267
00268 void xnarch_sync_irq(void);
00269
00270 int xnarch_setimask(int imask);
00271
00272 #ifdef __cplusplus
00273 extern "C" {
00274 #endif
00275
00276 void xnpod_welcome_thread(struct xnthread *);
00277
00278 #ifdef XENO_INTR_MODULE
00279
00280 int vml_irqlock = 0;
00281
00282 int vml_irqpend = 0;
00283
00284 void xnarch_sync_irq (void)
00285
00286 {
00287 if (vml_irqlock)
00288 __pthread_hold_vm(&vml_irqpend);
00289 }
00290
00291 static inline int xnarch_hook_irq (unsigned irq,
00292 void (*handler)(unsigned irq,
00293 void *cookie),
00294 void *cookie) {
00295 if (irq == 0)
00296 return -EINVAL;
00297
00298 return -ENOSYS;
00299 }
00300
00301 static inline int xnarch_release_irq (unsigned irq) {
00302 return -ENOSYS;
00303 }
00304
00305 static inline int xnarch_enable_irq (unsigned irq) {
00306 return -ENOSYS;
00307 }
00308
00309 static inline int xnarch_disable_irq (unsigned irq) {
00310 return -ENOSYS;
00311 }
00312
00313 static inline void xnarch_isr_chain_irq (unsigned irq) {
00314
00315 }
00316
00317 static inline void xnarch_isr_enable_irq (unsigned irq) {
00318
00319 }
00320
00321 static inline unsigned long xnarch_set_irq_affinity (unsigned irq,
00322 unsigned long affinity) {
00323 return 0;
00324 }
00325
00326 #define xnarch_relay_tick()
00327
00328 #endif
00329
00330 #ifdef XENO_MAIN_MODULE
00331
00332 int __xeno_main_init(void);
00333
00334 void __xeno_main_exit(void);
00335
00336 int __xeno_skin_init(void);
00337
00338 void __xeno_skin_exit(void);
00339
00340 int __xeno_user_init(void);
00341
00342 void __xeno_user_exit(void);
00343
00344 int vml_done = 0;
00345
00346 static inline int xnarch_init (void) {
00347 return 0;
00348 }
00349
00350 static inline void xnarch_exit (void) {
00351 }
00352
00353 static void xnarch_restart_handler (int sig) {
00354
00355 longjmp(vml_current->rstenv,1);
00356 }
00357
00358 void xnarch_exit_handler (int sig)
00359
00360 {
00361 vml_done = 1;
00362 __pthread_activate_vm(vml_root->khandle,vml_current->khandle);
00363 exit(99);
00364 }
00365
00366 int main (int argc, char *argv[])
00367
00368 {
00369 struct sigaction sa;
00370 int err;
00371
00372 if (geteuid() !=0)
00373 {
00374 fprintf(stderr,"This program must be run with root privileges");
00375 exit(1);
00376 }
00377
00378 err = __xeno_main_init();
00379
00380 if (err)
00381 {
00382 fprintf(stderr,"main_init() failed, err=%x\n",err);
00383 exit(2);
00384 }
00385
00386 err = __xeno_skin_init();
00387
00388 if (err)
00389 {
00390 fprintf(stderr,"skin_init() failed, err=%x\n",err);
00391 exit(3);
00392 }
00393
00394 err = __xeno_user_init();
00395
00396 if (err)
00397 {
00398 fprintf(stderr,"user_init() failed, err=%x\n",err);
00399 exit(4);
00400 }
00401
00402 sa.sa_handler = &xnarch_restart_handler;
00403 sigemptyset(&sa.sa_mask);
00404 sa.sa_flags = SA_RESTART;
00405 sigaction(XNARCH_SIG_RESTART,&sa,NULL);
00406
00407 sa.sa_handler = &xnarch_exit_handler;
00408 sa.sa_flags = 0;
00409 sigaction(SIGTERM,&sa,NULL);
00410 sigaction(SIGHUP,&sa,NULL);
00411 sigaction(SIGINT,&sa,NULL);
00412
00413 while (!vml_done)
00414 __pthread_idle_vm(&vml_irqlock);
00415
00416 __xeno_user_exit();
00417 __xeno_skin_exit();
00418 __xeno_main_exit();
00419
00420 exit(0);
00421 }
00422
00423 #endif
00424
00425 #ifdef XENO_TIMER_MODULE
00426
00427 void *vml_timer_handle;
00428
00429 static inline void xnarch_program_timer_shot (unsigned long long delay) {
00430
00431 }
00432
00433 static inline void xnarch_stop_timer (void) {
00434 __pthread_cancel_vm(vml_timer_handle,NULL);
00435 }
00436
00437 static inline void xnarch_read_timings (unsigned long long *shot,
00438 unsigned long long *delivery,
00439 unsigned long long defval)
00440 {
00441 *shot = defval;
00442 *delivery = defval;
00443 }
00444
00445 #endif
00446
00447 #ifdef XENO_POD_MODULE
00448
00449 extern void *vml_timer_handle;
00450
00451 xnsysinfo_t vml_info;
00452
00453 xnarchtcb_t *vml_root;
00454
00455 xnarchtcb_t *vml_current;
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471 struct xnarch_tick_parms {
00472 time_t sec;
00473 long nsec;
00474 void (*tickhandler)(void);
00475 pid_t ppid;
00476 int syncflag;
00477 };
00478
00479 static void *xnarch_timer_thread (void *cookie)
00480
00481 {
00482 struct xnarch_tick_parms *p = (struct xnarch_tick_parms *)cookie;
00483 void (*tickhandler)(void) = p->tickhandler;
00484 struct timespec ts;
00485
00486 pthread_create_rt("vmtimer",NULL,p->ppid,&p->syncflag,&vml_timer_handle);
00487 pthread_barrier_rt();
00488
00489 ts.tv_sec = p->sec;
00490 ts.tv_nsec = p->nsec;
00491
00492 for (;;)
00493 {
00494 nanosleep(&ts,NULL);
00495 xnarch_sync_irq();
00496 tickhandler();
00497 }
00498
00499 return NULL;
00500 }
00501
00502 static inline int xnarch_start_timer (unsigned long nstick,
00503 void (*tickhandler)(void))
00504 {
00505 struct xnarch_tick_parms parms;
00506 struct sched_param param;
00507 unsigned long tickval;
00508 pthread_attr_t thattr;
00509 pthread_t thid;
00510
00511 if (nstick == 0)
00512
00513 return -ENODEV;
00514
00515 pthread_attr_init(&thattr);
00516 pthread_attr_setdetachstate(&thattr,PTHREAD_CREATE_DETACHED);
00517 pthread_attr_setschedpolicy(&thattr,SCHED_FIFO);
00518 param.sched_priority = sched_get_priority_min(SCHED_FIFO) + 2;
00519 pthread_attr_setschedparam(&thattr,¶m);
00520
00521 if (vml_info.tickval > nstick)
00522 {
00523 fprintf(stderr,"Xenomai/VM: warning: VM tick freq > nucleus tick freq\n");
00524 fprintf(stderr," : rounding VM tick to %lu us\n",vml_info.tickval / 1000);
00525 tickval = vml_info.tickval;
00526 }
00527 else
00528 {
00529 tickval = ((nstick + vml_info.tickval - 1) / vml_info.tickval) * vml_info.tickval;
00530
00531 if (tickval != nstick)
00532 {
00533 fprintf(stderr,"Xenomai/VM: warning: VM tick not a multiple of nucleus tick\n");
00534 fprintf(stderr," : rounding VM tick to %lu us\n",tickval / 1000);
00535 }
00536 }
00537
00538 parms.sec = tickval / 1000000000;
00539 parms.nsec = tickval % 1000000000;
00540 parms.tickhandler = tickhandler;
00541 parms.syncflag = 0;
00542 parms.ppid = getpid();
00543
00544 pthread_create(&thid,&thattr,&xnarch_timer_thread,&parms);
00545 pthread_sync_rt(&parms.syncflag);
00546 pthread_start_rt(vml_timer_handle);
00547
00548 return 0;
00549 }
00550
00551 static inline void xnarch_leave_root(xnarchtcb_t *rootcb) {
00552 }
00553
00554 static inline void xnarch_enter_root(xnarchtcb_t *rootcb) {
00555 }
00556
00557 static inline void xnarch_switch_to (xnarchtcb_t *out_tcb,
00558 xnarchtcb_t *in_tcb) {
00559 vml_current = in_tcb;
00560 __pthread_activate_vm(in_tcb->khandle,out_tcb->khandle);
00561 }
00562
00563 static inline void xnarch_finalize_and_switch (xnarchtcb_t *dead_tcb,
00564 xnarchtcb_t *next_tcb) {
00565 vml_current = next_tcb;
00566 __pthread_cancel_vm(dead_tcb->khandle,next_tcb->khandle);
00567 }
00568
00569 static inline void xnarch_finalize_no_switch (xnarchtcb_t *dead_tcb) {
00570
00571 __pthread_cancel_vm(dead_tcb->khandle,NULL);
00572 }
00573
00574 static inline void xnarch_init_root_tcb (xnarchtcb_t *tcb,
00575 struct xnthread *thread,
00576 const char *name)
00577 {
00578 struct sched_param param;
00579
00580 param.sched_priority = sched_get_priority_min(SCHED_FIFO);
00581
00582 if (sched_setscheduler(0,SCHED_FIFO,¶m) < 0 ||
00583 pthread_info_rt(&vml_info) < 0)
00584 {
00585 perror("Xenomai/VM");
00586 exit(1);
00587 }
00588
00589 pthread_init_rt("vmroot",tcb,&tcb->khandle);
00590
00591 tcb->name = name;
00592 vml_root = vml_current = tcb;
00593 }
00594
00595 static inline void xnarch_init_tcb (xnarchtcb_t *tcb) {
00596
00597 tcb->khandle = NULL;
00598 }
00599
00600 static void *xnarch_thread_trampoline (void *cookie)
00601
00602 {
00603 xnarchtcb_t *tcb = (xnarchtcb_t *)cookie;
00604
00605 if (!setjmp(tcb->rstenv))
00606 {
00607 pthread_create_rt(tcb->name,tcb,tcb->ppid,&tcb->syncflag,&tcb->khandle);
00608 pthread_barrier_rt();
00609 }
00610
00611 xnarch_setimask(tcb->imask);
00612
00613 xnpod_welcome_thread(tcb->thread);
00614
00615 tcb->entry(tcb->cookie);
00616
00617 return NULL;
00618 }
00619
00620 static inline void xnarch_init_thread (xnarchtcb_t *tcb,
00621 void (*entry)(void *),
00622 void *cookie,
00623 int imask,
00624 struct xnthread *thread,
00625 char *name)
00626 {
00627 struct sched_param param;
00628 pthread_attr_t thattr;
00629
00630 if (tcb->khandle)
00631 {
00632 pthread_kill(tcb->thid,XNARCH_SIG_RESTART);
00633 return;
00634 }
00635
00636 tcb->imask = imask;
00637 tcb->entry = entry;
00638 tcb->cookie = cookie;
00639 tcb->thread = thread;
00640 tcb->name = name;
00641 tcb->ppid = getpid();
00642 tcb->syncflag = 0;
00643
00644 pthread_attr_init(&thattr);
00645 pthread_attr_setdetachstate(&thattr,PTHREAD_CREATE_DETACHED);
00646 pthread_attr_setschedpolicy(&thattr,SCHED_FIFO);
00647 param.sched_priority = sched_get_priority_min(SCHED_FIFO);
00648 pthread_attr_setschedparam(&thattr,¶m);
00649 pthread_create(&tcb->thid,&thattr,&xnarch_thread_trampoline,tcb);
00650
00651 pthread_sync_rt(&tcb->syncflag);
00652 pthread_start_rt(&tcb->khandle);
00653 }
00654
00655 static inline void xnarch_init_fpu(xnarchtcb_t *tcb) {
00656
00657 }
00658
00659 static inline void xnarch_save_fpu(xnarchtcb_t *tcb) {
00660
00661 }
00662
00663 static inline void xnarch_restore_fpu(xnarchtcb_t *tcb) {
00664
00665 }
00666
00667 int xnarch_setimask (int imask)
00668
00669 {
00670 spl_t s;
00671 splhigh(s);
00672 splexit(!!imask);
00673 return !!s;
00674 }
00675
00676 static inline int xnarch_send_ipi (cpumask_t cpumask) {
00677
00678 return 0;
00679 }
00680
00681 static inline int xnarch_hook_ipi (void (*handler)(void)) {
00682
00683 return 0;
00684 }
00685
00686 static inline int xnarch_release_ipi (void) {
00687
00688 return 0;
00689 }
00690
00691 int xnarch_sleep_on (int *flagp)
00692
00693 {
00694 while (!*flagp)
00695 {
00696 struct timespec ts;
00697 ts.tv_sec = 0;
00698 ts.tv_nsec = 10000000;
00699 #if !CONFIG_RTAI_OPT_DEBUG
00700 nanosleep(&ts,NULL);
00701 #else
00702 if(nanosleep(&ts, NULL))
00703 return -errno;
00704 #endif
00705 }
00706 return 0;
00707 }
00708
00709 static inline void xnarch_escalate (void) {
00710
00711 void xnpod_schedule_handler(void);
00712 xnpod_schedule_handler();
00713 }
00714
00715 #define xnarch_notify_ready()
00716 #define xnarch_notify_shutdown()
00717 #define xnarch_notify_halt()
00718
00719 #endif
00720
00721 extern xnsysinfo_t vml_info;
00722
00723 void xnarch_exit_handler(int);
00724
00725 static inline unsigned long long xnarch_tsc_to_ns (unsigned long long ts) {
00726 return ts;
00727 }
00728
00729 static inline unsigned long long xnarch_ns_to_tsc (unsigned long long ns) {
00730 return ns;
00731 }
00732
00733 static inline unsigned long long xnarch_get_cpu_time (void) {
00734 unsigned long long t;
00735 pthread_time_rt(&t);
00736 return t;
00737 }
00738
00739 static inline unsigned long long xnarch_get_cpu_tsc (void) {
00740 return xnarch_get_cpu_time();
00741 }
00742
00743 static inline unsigned long long xnarch_get_cpu_freq (void) {
00744 return vml_info.cpufreq;
00745 }
00746
00747 static inline void xnarch_halt (const char *emsg) {
00748 fprintf(stderr,"Xenomai/VM: fatal: %s\n",emsg);
00749 fflush(stderr);
00750 xnarch_exit_handler(SIGKILL);
00751 exit(99);
00752 }
00753
00754 static inline void *xnarch_sysalloc (u_long bytes) {
00755 return malloc(bytes);
00756 }
00757
00758 static inline void xnarch_sysfree (void *chunk, u_long bytes) {
00759 free(chunk);
00760 }
00761
00762 #define xnarch_current_cpu() 0
00763 #define xnarch_declare_cpuid const int cpuid = 0
00764 #define xnarch_get_cpu(x) do { (x) = (x); } while(0)
00765 #define xnarch_put_cpu(x) do { } while(0)
00766
00767 #define xnarch_alloc_stack xnmalloc
00768 #define xnarch_free_stack xnfree
00769
00770 #ifdef __cplusplus
00771 }
00772 #endif
00773
00774
00775 #define XNARCH_DECL_DISPLAY_CONTEXT();
00776 #define xnarch_init_display_context(obj)
00777 #define xnarch_create_display(obj,name,tag)
00778 #define xnarch_delete_display(obj)
00779 #define xnarch_post_graph(obj,state)
00780 #define xnarch_post_graph_if(obj,state,cond)
00781
00782 #endif