Patch 1: user-side code. Alexis.
Makefile.am | 15 + device.c | 569 +++++++++++++++++++++++++++++++++++++++++++++++++++++ drvlib.c | 636 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ init.c | 6 internal.h | 119 +++++++++++ wrappers.c | 95 ++++++++ wrappers.h | 26 ++ 7 files changed, 1463 insertions(+), 3 deletions(-)
Index: src/skins/rtdm/Makefile.am =================================================================== --- src/skins/rtdm/Makefile.am (revision 4513) +++ src/skins/rtdm/Makefile.am (working copy) @@ -1,4 +1,4 @@ -lib_LTLIBRARIES = librtdm.la +lib_LTLIBRARIES = librtdm.la librtdm_user.la librtdm_la_LDFLAGS = -version-info 1:0:0 -lpthread @@ -9,3 +9,16 @@ librtdm_la_CPPFLAGS = \ @XENO_USER_CFLAGS@ \ -I$(top_srcdir)/include + +librtdm_user_la_LDFLAGS = -version-info 1:0:0 -lpthread + +librtdm_user_la_SOURCES = \ + init.c \ + drvlib.c \ + device.c \ + wrappers.c \ + wrappers.h + +librtdm_user_la_CPPFLAGS = \ + @XENO_USER_CFLAGS@ \ + -I$(top_srcdir)/include Index: src/skins/rtdm/device.c =================================================================== --- src/skins/rtdm/device.c (revision 0) +++ src/skins/rtdm/device.c (revision 0) @@ -0,0 +1,569 @@ +/* + * Copyright (C) 2005 Joerg Langenberg <joerg.langenb...@gmx.net>. + * Copyright (C) 2008 Alexis Berlemont <alexis.berlem...@free.fr>. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + + +#include <stdio.h> +#include <stdarg.h> +#include <stddef.h> +#include <string.h> +#include <errno.h> + +#include <rtdm/rtdm.h> +#include <rtdm/rtdm_driver.h> +#include <rtdm/syscall.h> +#include <asm-generic/bits/sigshadow.h> +#include <asm-generic/bits/current.h> + +#include "internal.h" + +#define DEV_MAX_COUNT 16 +#define FD_MAX_COUNT 32 +#define MAP_MAX_COUNT 16 + +#define SET_DEFAULT_OP(device, operation) \ + (device).operation##_rt = (void *)rtdm_no_support; \ + (device).operation##_nrt = (void *)rtdm_no_support + +#define SET_DEFAULT_OP_IF_NULL(device, operation) \ + if (!(device).operation##_rt) \ + (device).operation##_rt = (void *)rtdm_no_support; \ + (device).operation##_nrt = (void *)rtdm_no_support + +struct map_entry { + unsigned long map_addr; + void *vm_private_data; + struct vm_operations_struct *vm_ops; +}; + +static atomic_tab_t reg_cxts = { + .count = FD_MAX_COUNT, + .elements = { [0 ... FD_MAX_COUNT - 1] = { {0}, NULL} } +}; + +static atomic_tab_t reg_devs = { + .count = DEV_MAX_COUNT, + .elements = { [0 ... DEV_MAX_COUNT - 1] = { {0}, NULL} } +}; + +static atomic_tab_t reg_maps = { + .count = MAP_MAX_COUNT, + .elements = { [0 ... DEV_MAX_COUNT - 1] = { {0}, NULL} } +}; + +int rtdm_no_support(void) +{ + return -ENOSYS; +} + +int rtdm_select_bind_no_support(struct rtdm_dev_context *context, + struct xnselector *selector, + unsigned type, + unsigned index) +{ + return -EBADF; +} + +int rtdm_dev_register(struct rtdm_device *device) +{ + unsigned int idx = 0; + int err = 0; + + if(device->struct_version != RTDM_DEVICE_STRUCT_VER) + return -EINVAL; + + switch (device->device_flags & RTDM_DEVICE_TYPE_MASK) { + case RTDM_NAMED_DEVICE: + SET_DEFAULT_OP_IF_NULL(*device, open); + SET_DEFAULT_OP(*device, socket); + break; + case RTDM_PROTOCOL_DEVICE: + SET_DEFAULT_OP_IF_NULL(*device, socket); + SET_DEFAULT_OP(*device, open); + break; + default: + return -EINVAL; + } + + if (!device->ops.close_rt) + device->ops.close_rt = (void *)rtdm_no_support; + + SET_DEFAULT_OP_IF_NULL(device->ops, ioctl); + SET_DEFAULT_OP_IF_NULL(device->ops, read); + SET_DEFAULT_OP_IF_NULL(device->ops, write); + SET_DEFAULT_OP_IF_NULL(device->ops, recvmsg); + SET_DEFAULT_OP_IF_NULL(device->ops, sendmsg); + + device->ops.select_bind = rtdm_select_bind_no_support; + + err = atab_try_add(®_devs, &idx); + if(err < 0) + return err; + + return atab_do_add(®_devs, idx, (void *) device); +} + +int rtdm_dev_unregister(struct rtdm_device *device, unsigned int poll_delay) +{ + int i = 0, fnd = 0, err = -EINVAL; + + do { + + err = atab_lock(®_devs, i); + if(err < 0) + return err; + + if(reg_devs.elements[i].element == (void *)device) + fnd = 1; + + err = atab_unlock(®_devs, i); + if(err < 0) + return err; + + } while( fnd == 0 && ++i < DEV_MAX_COUNT); + + err = atab_try_remove(®_devs, i); + if(err < 0) + return err; + + reg_devs.elements[i].element = NULL; + + err = atab_do_remove(®_devs, i); + + return err; +} + +int rt_dev_open(const char *path, int oflag, ...) +{ + struct rtdm_device *device; + struct rtdm_dev_context *context; + int i = 0, fnd = 0, err; + unsigned int idx; + + do { + + err = atab_lock(®_devs, i); + if(err < 0) + return err; + + device = (struct rtdm_device *)reg_devs.elements[i].element; + if(strncmp(path, device->device_name, RTDM_MAX_DEVNAME_LEN) == 0) + fnd = 1; + + err = atab_unlock(®_devs, i); + if(err < 0) + return err; + + } while( fnd == 0 && ++i < DEV_MAX_COUNT); + + if(fnd == 0) + return -ENOENT; + + err = atab_lock(®_devs, i); + if(err < 0) + return err; + + if((device->device_flags & RTDM_EXCLUSIVE) != 0 && + (xnarch_atomic_get(®_devs.elements[i].status) & + ~(__REGISTERED_ELT)) != 1) + return -EBUSY; + + err = atab_try_add(®_cxts, &idx); + if(err < 0) + return err; + + context = rtdm_malloc(sizeof(struct rtdm_dev_context) + + device->context_size); + if(context == NULL) + return -ENOMEM; + + context->device = device; + context->fd = idx; + context->ops = &device->ops; + + err = device->open_rt(context, NULL, oflag); + if(err < 0) + goto out_open; + + err = atab_do_add(®_cxts, idx, (void *) context); + +out_open: + + if(err < 0) + rtdm_free(context); + + return err; +} + +int rt_dev_socket(int protocol_family, int socket_type, int protocol) +{ + struct rtdm_device *device; + struct rtdm_dev_context *context; + int i = 0, fnd = 0, err; + unsigned int idx; + + do { + + err = atab_lock(®_devs, i); + if(err < 0) + return err; + + device = (struct rtdm_device *)reg_devs.elements[i].element; + + if(protocol_family == device->protocol_family && + socket_type == device->socket_type) + fnd = 1; + + err = atab_unlock(®_devs, i); + if(err < 0) + return err; + + } while(fnd == 0 && ++i < DEV_MAX_COUNT); + + if(fnd == 0) + return -ENOENT; + + err = atab_lock(®_devs, i); + if(err < 0) + return err; + + if((device->device_flags & RTDM_EXCLUSIVE) != 0 && + (xnarch_atomic_get(®_devs.elements[i].status) & + ~(__REGISTERED_ELT)) != 1) + return -EBUSY; + + err = atab_try_add(®_cxts, &idx); + if(err < 0) + return err; + + context = rtdm_malloc(sizeof(struct rtdm_dev_context) + + device->context_size); + if(context == NULL) + return -ENOMEM; + + context->device = device; + context->fd = idx; + context->ops = &device->ops; + + err = device->socket_rt(context, NULL, protocol); + if(err < 0) + goto out_socket; + + err = atab_do_add(®_cxts, idx, (void *) context); + +out_socket: + + if(err < 0) + rtdm_free(context); + + return err; + +} + +int rt_dev_close(int fd) +{ + struct rtdm_dev_context *context; + int i = 0, fnd = 0, err; + + /* Basic checkings on the fd argument */ + if(fd >= FD_MAX_COUNT) + return -EINVAL; + + err = atab_try_remove(®_cxts, fd); + if(err < 0) + return err; + + context = reg_cxts.elements[fd].element; + if(context == NULL) + return -EINVAL; + + /* Call the close handler */ + err = context->ops->close_rt(context, NULL); + if(err < 0) + goto out_close; + + reg_cxts.elements[fd].element = NULL; + + /* Look for the device in the tab... */ + do { + + err = atab_lock(®_devs, i); + if(err < 0) + return err; + + if(context->device == reg_devs.elements[i].element) + fnd = 1; + + err = atab_unlock(®_devs, i); + if(err < 0) + return err; + + } while( fnd == 0 && ++i < DEV_MAX_COUNT); + + if(fnd == 0) + return -ENODEV; + + /* ...and unlock it */ + atab_unlock(®_devs, i); + + err = atab_do_remove(®_cxts, fd); + if(err < 0) + return err; + + rtdm_free(context); + +out_close: + + return err; +} + +int rt_dev_ioctl(int fd, int request, ...) +{ + struct rtdm_dev_context *context; + va_list ap; + void *arg; + int err; + + va_start(ap, request); + arg = va_arg(ap, void*); + va_end(ap); + + /* Basic checkings on the fd argument */ + if(fd >= FD_MAX_COUNT) + return -EINVAL; + + err = atab_lock(®_cxts, fd); + if(err < 0) + return err; + + context = reg_cxts.elements[fd].element; + if(context == NULL) { + err = -EINVAL; + goto out_ioctl; + } + + err = context->ops->ioctl_rt(context, NULL, request, arg); + +out_ioctl: + atab_unlock(®_cxts, fd); + + return err; +} +ssize_t rt_dev_read(int fd, void *buf, size_t nbyte) +{ + struct rtdm_dev_context *context; + int err; + + /* Basic checkings on the fd argument */ + if(fd >= FD_MAX_COUNT) + return -EINVAL; + + err = atab_lock(®_cxts, fd); + if(err < 0) + return err; + + context = reg_cxts.elements[fd].element; + if(context == NULL) { + err = -EINVAL; + goto out_read; + } + + err = context->ops->read_rt(context, NULL, buf, nbyte); + +out_read: + atab_unlock(®_cxts, fd); + + return err; +} + +ssize_t rt_dev_write(int fd, const void *buf, size_t nbyte) +{ + struct rtdm_dev_context *context; + int err; + + /* Basic checkings on the fd argument */ + if(fd >= FD_MAX_COUNT) + return -EINVAL; + + err = atab_lock(®_cxts, fd); + if(err < 0) + return err; + + context = reg_cxts.elements[fd].element; + if(context == NULL) { + err = -EINVAL; + goto out_write; + } + + err = context->ops->write_rt(context, NULL, buf, nbyte); + +out_write: + atab_unlock(®_cxts, fd); + + return err; +} + +ssize_t rt_dev_recvmsg(int fd, struct msghdr *msg, int flags) +{ + struct rtdm_dev_context *context; + int err; + + /* Basic checkings on the fd argument */ + if(fd >= FD_MAX_COUNT) + return -EINVAL; + + err = atab_lock(®_cxts, fd); + if(err < 0) + return err; + + context = reg_cxts.elements[fd].element; + if(context == NULL) { + err = -EINVAL; + goto out_recvmsg; + } + + err = context->ops->recvmsg_rt(context, NULL, msg, flags); + +out_recvmsg: + atab_unlock(®_cxts, fd); + + return err; + +} + +ssize_t rt_dev_sendmsg(int fd, const struct msghdr *msg, int flags) +{ + struct rtdm_dev_context *context; + int err; + + /* Basic checkings on the fd argument */ + if(fd >= FD_MAX_COUNT) + return -EINVAL; + + err = atab_lock(®_cxts, fd); + if(err < 0) + return err; + + context = reg_cxts.elements[fd].element; + if(context == NULL) { + err = -EINVAL; + goto out_sendmsg; + } + + err = context->ops->sendmsg_rt(context, NULL, msg, flags); + +out_sendmsg: + atab_unlock(®_cxts, fd); + + return err; + +} + +ssize_t rt_dev_recvfrom(int fd, void *buf, size_t len, int flags, + struct sockaddr *from, + socklen_t *fromlen) +{ + struct iovec iov = { buf, len }; + struct msghdr msg = { + from, (from != NULL) ? *fromlen : 0, &iov, 1, NULL, 0 + }; + int err; + + err = rt_dev_recvmsg(fd, &msg, flags); + + if (from && err >= 0) + *fromlen = msg.msg_namelen; + + return err; +} + +int __rtdm_map_to_user(void *src_addr, + void **pptr, + struct vm_operations_struct *vm_ops, void *vm_private_data) +{ + struct map_entry *entry; + struct vm_area_struct vma = { vm_private_data }; + unsigned int idx; + int err; + + entry = rtdm_malloc(sizeof(struct map_entry)); + if(entry == NULL) + return -ENOMEM; + + *pptr = src_addr; + + entry->map_addr = (unsigned long) src_addr; + entry->vm_private_data = vm_private_data; + entry->vm_ops = vm_ops; + + err = atab_try_add(®_maps, &idx); + if(err < 0) + goto out_map; + + err = atab_do_add(®_maps, idx, (void *)entry); + if(err < 0) + goto out_map; + + entry->vm_ops->open(&vma); + +out_map: + if(err < 0) + rtdm_free(entry); + + return err; +} + +int rtdm_munmap(rtdm_user_info_t *user_info, void *ptr, size_t len) +{ + struct map_entry *entry; + struct vm_area_struct vma; + int i = 0, fnd = 0, err; + + do { + err = atab_lock(®_maps, i); + if(err < 0) + return err; + + entry = (struct map_entry *)reg_maps.elements[i].element; + if(entry->map_addr == (unsigned long)ptr) + fnd = 1; + + err = atab_unlock(®_devs, i); + if(err < 0) + return err; + + } while(fnd == 0 && ++i < MAP_MAX_COUNT); + + if(fnd == 0) + return -EINVAL; + + err = atab_try_remove(®_maps, i); + if(err < 0) + return err; + + err = atab_do_remove(®_maps, i); + if(err < 0) + return err; + + vma.vm_private_data = entry->vm_private_data; + entry->vm_ops->close(&vma); + + rtdm_free(entry); + + return err; +} Index: src/skins/rtdm/drvlib.c =================================================================== --- src/skins/rtdm/drvlib.c (revision 0) +++ src/skins/rtdm/drvlib.c (revision 0) @@ -0,0 +1,636 @@ +/* + * Copyright (C) 2005 Joerg Langenberg <joerg.langenb...@gmx.net>. + * Copyright (C) 2008 Alexis Berlemont <alexis.berlem...@free.fr>. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <sys/types.h> +#include <sys/mman.h> +#include <sys/ioctl.h> +#include <stdio.h> +#include <stdarg.h> +#include <stddef.h> +#include <errno.h> +#include <signal.h> +#include <fcntl.h> +#include <unistd.h> + + +#include <nucleus/heap.h> +#include <rtdm/rtdm.h> +#include <rtdm/rtdm_driver.h> +#include <rtdm/syscall.h> +#include <asm-generic/bits/sigshadow.h> +#include <asm-generic/bits/current.h> + +#include "wrappers.h" + +#ifdef HAVE___THREAD +__thread rtdm_task_t __rtdm_current __attribute__ ((tls_model ("initial-exec"))) = { + .opaque = XN_NO_HANDLE, + .opaque2 = 0 +}; +#else /* !HAVE___THREAD */ +extern pthread_key_t __rtdm_tskey; +#endif /* !HAVE___THREAD */ + + +extern int __rtdm_muxid; + +nanosecs_abs_t rtdm_clock_read(void) +{ + nanosecs_abs_t clock = 0; + + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_clock_read, &clock); + + return clock; +} + +nanosecs_abs_t rtdm_clock_read_monotonic(void) +{ + nanosecs_abs_t clock = 0; + + XENOMAI_SKINCALL1(__rtdm_muxid, + __rtdm_clock_read_monotonic, &clock); + + return clock; +} + +struct rtdm_task_iargs { + rtdm_task_t *task; + const char *name; + int prio; + nanosecs_rel_t period; + rtdm_task_proc_t task_proc; + void *arg; + xncompletion_t *completionp; +}; + +static void *__rtdm_task_trampoline(void *cookie) +{ + struct rtdm_task_iargs *iargs = (struct rtdm_task_iargs *)cookie; + struct sched_param param; + rtdm_task_proc_t task_proc; + void *arg; + struct rtdm_arg_bulk bulk; + int err; + + bulk.a1 = (u_long) iargs->task; + bulk.a2 = (u_long) iargs->name; + bulk.a3 = (u_long) iargs->prio; + bulk.a4 = (u_long) &iargs->period; + bulk.a5 = (u_long) iargs->completionp; + bulk.a6 = (u_long) xeno_init_current_mode(); + bulk.a7 = (u_long) pthread_self(); + + if(bulk.a6 == 0) { + err = -ENOMEM; + goto out_task_trampoline; + } + + if(iargs->prio > 0) { + /* + * Re-apply sched params here as some libpthread + * implementations fail doing this via pthread_create. + */ + memset(¶m, 0, sizeof(param)); + param.sched_priority = iargs->prio; + __real_pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m); + } + + /* TODO: does rtdm_task_destroy requires asynchronous cancellation ? */ + pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); + + sigshadow_install_once(); + + err = XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_task_init, &bulk); + if(err != 0) + goto out_task_trampoline; + +#ifdef HAVE___THREAD + __rtdm_current = *iargs->task; +#endif /* HAVE___THREAD */ + + /* We must save anything we'll need to use from iargs on our own + stack now before releasing the barrier, since our released + parent could unwind the stack space onto which the iargs struct + is laid on before we actually get the CPU back. */ + + task_proc = iargs->task_proc; + arg = iargs->arg; + + /* Wait on the barrier for the task to be started. The barrier + could be released in order to process Linux signals while the + Xenomai shadow is still dormant; in such a case, resume wait. */ + do + err = XENOMAI_SYSCALL2(__xn_sys_barrier, NULL, NULL); + while (err == -EINTR); + + if(err != 0) + goto out_task_trampoline; + + task_proc(arg); + +out_task_trampoline: + + pthread_exit((void *)err); + +} + +int rtdm_task_init(rtdm_task_t *task, const char *name, + rtdm_task_proc_t task_proc, void *arg, + int priority, nanosecs_rel_t period) +{ + struct rtdm_task_iargs iargs; + xncompletion_t completion; + pthread_attr_t thattr; + struct sched_param param; + pthread_t thid; + int err; + + /* The task_proc argument is not checked by the kernel-side skin, + so do it here */ + if(task_proc == NULL) + return -EINVAL; + + /* Migrate this thread to the Linux domain since we are about to + issue a series of regular kernel syscalls in order to create + the new Linux thread, which in turn will be mapped to a + real-time shadow. */ + + XENOMAI_SYSCALL1(__xn_sys_migrate, XENOMAI_LINUX_DOMAIN); + + completion.syncflag = 0; + completion.pid = -1; + + iargs.task = task; + iargs.name = name; + iargs.prio = priority; + iargs.period = period; + iargs.task_proc = task_proc; + iargs.arg = arg; + iargs.completionp = &completion; + + pthread_attr_init(&thattr); + pthread_attr_setinheritsched(&thattr, PTHREAD_EXPLICIT_SCHED); + memset(¶m, 0, sizeof(param)); + if(priority > 0) { + pthread_attr_setschedpolicy(&thattr, SCHED_FIFO); + param.sched_priority = priority; + } else + pthread_attr_setschedpolicy(&thattr, SCHED_OTHER); + + pthread_attr_setschedparam(&thattr, ¶m); + pthread_attr_setstacksize(&thattr, RTDM_USER_TASK_STACK_SIZE); + pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_DETACHED); + + err = __real_pthread_create(&thid, + &thattr, &__rtdm_task_trampoline, &iargs); + if(err != 0) + return -err; + + err = XENOMAI_SYSCALL1(__xn_sys_completion, &completion); + if(err != 0) + return err; + + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_task_start, task); +} + +void rtdm_task_destroy(rtdm_task_t *task) +{ + if(task->opaque2) { + int err; + + err = pthread_cancel((pthread_t) task->opaque2); + if(err != 0) + return; + + } else if(task->opaque == rtdm_task_current()->opaque) + pthread_exit(NULL); + + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_task_destroy, task); +} + +void rtdm_task_set_priority(rtdm_task_t *task, int priority) +{ + XENOMAI_SKINCALL2(__rtdm_muxid, __rtdm_task_set_priority, task, priority); +} + +int rtdm_task_set_period(rtdm_task_t *task, nanosecs_rel_t period) +{ + return XENOMAI_SKINCALL2(__rtdm_muxid, + __rtdm_task_set_period, task, &period); +} + +int rtdm_task_wait_period(void) +{ + return XENOMAI_SKINCALL0(__rtdm_muxid, __rtdm_task_wait_period); +} + +int rtdm_task_unblock(rtdm_task_t *task) +{ + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_task_unblock, task); +} + +rtdm_task_t *rtdm_task_current(void) +{ + rtdm_task_t *task; + +#ifdef HAVE___THREAD + task = &__rtdm_current; + + if(task->opaque == XN_NO_HANDLE) + return NULL; + +#else /* !HAVE___THREAD */ + task = (rtdm_task_t *)pthread_getspecific(__rtdm_tskey); + + if(task != NULL) + return task; + + task = rtdm_malloc(sizeof(rtdm_task_t)); + + if(!task || + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_task_current, task) != 0) { + rtdm_free(task); + return NULL; + } + + pthread_setspecific(__rtdm_tskey, task); +#endif /* HAVE___THREAD */ + + return task; +} + +int rtdm_task_sleep(nanosecs_rel_t delay) +{ + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_task_sleep, &delay); +} + +int rtdm_task_sleep_abs(nanosecs_abs_t wakeup_time, enum rtdm_timer_mode mode) +{ + return XENOMAI_SKINCALL2(__rtdm_muxid, + __rtdm_task_sleep_abs, &wakeup_time, mode); +} + +void rtdm_task_join_nrt(rtdm_task_t *task, unsigned int poll_delay) +{ + XENOMAI_SKINCALL2(__rtdm_muxid, + __rtdm_task_join_nrt, task, poll_delay); +} + +void rtdm_task_busy_sleep(nanosecs_rel_t delay) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_task_busy_sleep, &delay); +} + +void task_timer_proc(void *arg) +{ + rtdm_timer_t *timer = (rtdm_timer_t *)arg; + + while(1) { + int err = XENOMAI_SKINCALL1(__rtdm_muxid, + __rtdm_timer_wait, timer); + if(err != 0) + return; + + timer->handler(timer); + } +} + +int rtdm_timer_init(rtdm_timer_t *timer, + rtdm_timer_handler_t handler, const char *name) +{ + int err; + char task_name[XNOBJECT_NAME_LEN]; + + err = XENOMAI_SKINCALL3(__rtdm_muxid, + __rtdm_timer_init, timer, handler, name); + + if(err != 0) + return err; + + snprintf(task_name, XNOBJECT_NAME_LEN, "timer_%s\n", name); + + return rtdm_task_init(&timer->task, + task_name, + task_timer_proc, + timer, RTDM_TASK_HIGHEST_PRIORITY, 0); +} + +void rtdm_timer_destroy(rtdm_timer_t *timer) +{ + rtdm_task_destroy(&timer->task); + + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_timer_destroy, timer); +} + +int rtdm_timer_start(rtdm_timer_t *timer, + nanosecs_abs_t expiry, + nanosecs_rel_t interval, enum rtdm_timer_mode mode) +{ + return XENOMAI_SKINCALL4(__rtdm_muxid, + __rtdm_timer_start, + timer, &expiry, &interval, mode); +} + +void rtdm_timer_stop(rtdm_timer_t *timer) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_timer_stop, timer); +} + +void rtdm_toseq_init(rtdm_toseq_t *timeout_seq, nanosecs_rel_t timeout) +{ + XENOMAI_SKINCALL2(__rtdm_muxid, __rtdm_toseq_init, timeout_seq, &timeout); +} + +void rtdm_event_init(rtdm_event_t *event, unsigned long pending) +{ + XENOMAI_SKINCALL2(__rtdm_muxid, __rtdm_event_init, event, pending); +} + +void rtdm_event_destroy(rtdm_event_t *event) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_event_destroy, event); +} + +void rtdm_event_pulse(rtdm_event_t *event) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_event_pulse, event); +} + +void rtdm_event_signal(rtdm_event_t *event) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_event_signal, event); +} + +int rtdm_event_wait(rtdm_event_t *event) +{ + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_event_wait, event); +} + +int rtdm_event_timedwait(rtdm_event_t *event, + nanosecs_rel_t timeout, rtdm_toseq_t *timeout_seq) +{ + return XENOMAI_SKINCALL3(__rtdm_muxid, + __rtdm_event_timedwait, + event, &timeout, timeout_seq); +} + +void rtdm_event_clear(rtdm_event_t *event) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_event_clear, event); +} + +void rtdm_sem_init(rtdm_sem_t *sem, unsigned long value) +{ + XENOMAI_SKINCALL2(__rtdm_muxid, __rtdm_sem_init, sem, value); +} + +void rtdm_sem_destroy(rtdm_sem_t *sem) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_sem_destroy, sem); +} + +int rtdm_sem_down(rtdm_sem_t *sem) +{ + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_sem_down, sem); +} + +int rtdm_sem_timeddown(rtdm_sem_t *sem, + nanosecs_rel_t timeout, rtdm_toseq_t *timeout_seq) +{ + return XENOMAI_SKINCALL3(__rtdm_muxid, + __rtdm_sem_timeddown, sem, &timeout, timeout_seq); +} + +void rtdm_sem_up(rtdm_sem_t *sem) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_sem_up, sem); +} + +void rtdm_mutex_init(rtdm_mutex_t *mutex) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_mutex_init, mutex); +} + +void rtdm_mutex_destroy(rtdm_mutex_t *mutex) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_mutex_destroy, mutex); +} + +void rtdm_mutex_unlock(rtdm_mutex_t *mutex) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_mutex_unlock, mutex); +} + +int rtdm_mutex_lock(rtdm_mutex_t *mutex) +{ + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_mutex_lock, mutex); +} + +int rtdm_mutex_timedlock(rtdm_mutex_t *mutex, + nanosecs_rel_t timeout, rtdm_toseq_t *timeout_seq) +{ + return XENOMAI_SKINCALL3(__rtdm_muxid, + __rtdm_mutex_timedlock, + mutex, &timeout, timeout_seq); +} + +void task_irq_proc(void *arg) +{ + rtdm_irq_t *irq_handle = (rtdm_irq_t *)arg; + + while(1) { + int i, err = XENOMAI_SKINCALL1(__rtdm_muxid, + __rtdm_irq_wait, irq_handle); + + if(err < 0) + return; + + for(i = 0; i < err; i++) + irq_handle->handler(irq_handle); + } +} + +int rtdm_irq_request(rtdm_irq_t *irq_handle, unsigned int irq_no, + rtdm_irq_handler_t handler, unsigned long flags, + const char *device_name, void *arg) +{ + int err; + char task_name[XNOBJECT_NAME_LEN]; + + err = XENOMAI_SKINCALL3(__rtdm_muxid, + __rtdm_irq_request, + irq_handle, irq_no, device_name); + + if(err != 0) + return err; + + irq_handle->handler = handler; + irq_handle->arg = arg; + + snprintf(task_name, XNOBJECT_NAME_LEN, "irq_%u\n", irq_no); + + return rtdm_task_init(&irq_handle->task, + task_name, + task_irq_proc, + irq_handle, RTDM_TASK_HIGHEST_PRIORITY, 0); +} + +int rtdm_irq_free(rtdm_irq_t *irq_handle) +{ + rtdm_task_destroy(&irq_handle->task); + + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_irq_free, irq_handle); +} + +void *task_nrtsig_proc(void *arg) +{ + rtdm_nrtsig_t *nrt_sig = (rtdm_nrtsig_t *)arg; + + while(1) { + int err = XENOMAI_SKINCALL1(__rtdm_muxid, + __rtdm_nrtsig_wait, nrt_sig); + if(err < 0) + pthread_exit((void*) err); + + nrt_sig->handler(nrt_sig->virq, nrt_sig->arg); + } + pthread_exit(NULL); +} + +int rtdm_nrtsig_init(rtdm_nrtsig_t *nrt_sig, + rtdm_nrtsig_handler_t handler, void *arg) +{ + int err; + + err = XENOMAI_SKINCALL1(__rtdm_muxid, + __rtdm_nrtsig_init, nrt_sig); + + if(err != 0) + return err; + + nrt_sig->handler = handler; + nrt_sig->arg = arg; + + return __real_pthread_create((pthread_t*)&nrt_sig->opaque2, + NULL, &task_nrtsig_proc, nrt_sig); +} + +void rtdm_nrtsig_destroy(rtdm_nrtsig_t *nrt_sig) +{ + pthread_cancel((pthread_t) nrt_sig->opaque2); + + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_nrtsig_destroy, nrt_sig); +} + +void rtdm_nrtsig_pend(rtdm_nrtsig_t *nrt_sig) +{ + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_nrtsig_pend, nrt_sig); +} + +int __rtdm_irqsave(rtdm_lockctx_t *context) +{ + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_irq_save, context); +} + +int __rtdm_irqrestore(rtdm_lockctx_t *context) +{ + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_irq_restore, context); +} + +rtdm_heap_t __main_heap; + +static __attribute__ ((constructor(105))) +void __init_rtdm_mainheap(void) +{ + rtdm_heap_init(&__main_heap, RTDM_MAIN_HEAP_SIZE, 0); +} + +static int __map_heap_memory(rtdm_heap_ph_t *ph) +{ + int err, heapfd; + + /* Open the heap device to share the heap memory with the + in-kernel skin and bound clients. */ + heapfd = __real_open(XNHEAP_DEV_NAME, O_RDWR); + + if (heapfd < 0) + return -ENOENT; + + /* Bind this file instance to the shared heap. */ + err = __real_ioctl(heapfd, 0, ph->opaque2); + + if (err) + goto close_and_exit; + + /* Map the heap memory into our address space. */ + ph->mapbase = (caddr_t) __real_mmap(NULL, + ph->mapsize, + PROT_READ | PROT_WRITE, + MAP_SHARED, heapfd, 0L); + + if (ph->mapbase == MAP_FAILED) + err = -ENOMEM; + + close_and_exit: + + __real_close(heapfd); + + return err; +} + +int rtdm_heap_init(rtdm_heap_t *heap, unsigned long size, unsigned long flags) +{ + int err; + + err = XENOMAI_SKINCALL3(__rtdm_muxid, __rtdm_heap_init, heap, size, flags); + + if(err < 0) + goto out_heap_init; + + err = __map_heap_memory(heap); + + if(err < 0) { + XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_heap_destroy, heap); + memset(heap, 0, sizeof(rtdm_heap_t)); + } + +out_heap_init: + return err; +} + +int rtdm_heap_destroy(rtdm_heap_t *heap) +{ + return XENOMAI_SKINCALL1(__rtdm_muxid, __rtdm_heap_destroy, heap); +} + +void *rtdm_heap_malloc(rtdm_heap_t *heap, size_t size) +{ + void *block; + + if(XENOMAI_SKINCALL3(__rtdm_muxid, + __rtdm_heap_malloc, heap, size, &block) < 0) + return NULL; + + return block; +} + +void rtdm_heap_free(rtdm_heap_t *heap, void *ptr) +{ + XENOMAI_SKINCALL2(__rtdm_muxid, __rtdm_heap_free, heap, ptr); +} Index: src/skins/rtdm/init.c =================================================================== --- src/skins/rtdm/init.c (revision 4513) +++ src/skins/rtdm/init.c (working copy) @@ -20,12 +20,13 @@ #include <errno.h> #include <stdlib.h> #include <string.h> +#include <rtdm/rtdm_driver.h> #include <rtdm/syscall.h> #include <asm-generic/bits/bind.h> int __rtdm_muxid = -1; -static __attribute__ ((constructor)) +static __attribute__ ((constructor(101))) void __init_rtdm_interface(void) { /* The following call may fail; binding errors will be @@ -35,5 +36,6 @@ __rtdm_muxid = xeno_bind_skin_opt(RTDM_SKIN_MAGIC, "rtdm", "xeno_rtdm"); __rtdm_muxid = __xn_mux_shifted_id(__rtdm_muxid); +} -} + Index: src/skins/rtdm/internal.h =================================================================== --- src/skins/rtdm/internal.h (revision 0) +++ src/skins/rtdm/internal.h (revision 0) @@ -0,0 +1,119 @@ +#ifndef __INTERNAL_H__ +#define __INTERNAL_H__ + +#if __GNUC__ >= 3 +#define GCC_ZERO_LENGTH_ARRAY +#else +#define GCC_ZERO_LENGTH_ARRAY 0 +#endif + +#define __REGISTERING_ELT 0x80000000 +#define __REGISTERED_ELT 0x40000000 +#define __REMOVING_ELT 0x20000000 + +struct atomic_element { + xnarch_atomic_t status; + void *element; +}; + +typedef struct { + unsigned int count; + struct atomic_element elements[GCC_ZERO_LENGTH_ARRAY]; +} atomic_tab_t; + +static int atab_try_add(atomic_tab_t *tab, unsigned int *index) +{ + unsigned int i = 0; + + while(xnarch_atomic_cmpxchg(&tab->elements[i].status, + 0, __REGISTERING_ELT) != 0 && + ++i < tab->count); + + if(i >= tab->count) + return -EBUSY; + + *index = i; + + return 0; +} + +static int atab_do_add(atomic_tab_t *tab, unsigned int index, void *element) +{ + unsigned long status; + + tab->elements[index].element = element; + + status = xnarch_atomic_cmpxchg(&tab->elements[index].status, + __REGISTERING_ELT, __REGISTERED_ELT); + if(status != __REGISTERING_ELT) + return -EPERM; + + return 0; +} + +static int atab_try_remove(atomic_tab_t *tab, unsigned int index) +{ + unsigned long status; + + do { + status = xnarch_atomic_get(&tab->elements[index].status); + + if((status & __REMOVING_ELT) != 0) + return -EBUSY; + + } while(status != xnarch_atomic_cmpxchg(&tab->elements[index].status, + status, status | __REMOVING_ELT)); + + return 0; +} + +static int atab_do_remove(atomic_tab_t *tab, unsigned int index) +{ + unsigned long status; + + do { + status = xnarch_atomic_get(&tab->elements[index].status); + + if((status & ~(__REMOVING_ELT | __REGISTERED_ELT)) != 0) + return -EAGAIN; + + } while(status != xnarch_atomic_cmpxchg(&tab->elements[index].status, status, 0)); + + return 0; +} + +static int atab_lock(atomic_tab_t *tab, unsigned int index) +{ + unsigned long status; + + do { + status = xnarch_atomic_get(&tab->elements[index].status); + + if((status & __REGISTERED_ELT) == 0 || + (status & __REMOVING_ELT) != 0) + return -EINVAL; + + } while(status != xnarch_atomic_cmpxchg(&tab->elements[index].status, + status, status + 1)); + + return 0; +} + +static int atab_unlock(atomic_tab_t *tab, unsigned int index) +{ + unsigned long status; + + do { + status = xnarch_atomic_get(&tab->elements[index].status); + + if((status & __REGISTERED_ELT) == 0 || + (status & __REMOVING_ELT) != 0) + return -EINVAL; + + } while(status != xnarch_atomic_cmpxchg(&tab->elements[index].status, + status, status - 1)); + + return 0; +} + +#endif /* __INTERNAL_H__ */ Index: src/skins/rtdm/wrappers.c =================================================================== --- src/skins/rtdm/wrappers.c (revision 0) +++ src/skins/rtdm/wrappers.c (revision 0) @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2005 Heikki Lindholm <holin...@cs.helsinki.fi>. + * Copyright (C) 2008 Philippe Gerum <r...@xenomai.org>. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <sys/types.h> +#include <sys/mman.h> +#include <sys/ioctl.h> +#include <stdarg.h> +#include <pthread.h> +#include <signal.h> +#include <fcntl.h> +#include <unistd.h> + +/* + * This file maintains a list of placeholders for routines that we do + * NOT want to be wrapped to their Xenomai POSIX API counterparts when + * used internally by the RTDM interface. + */ + +__attribute__ ((weak)) +int __real_pthread_setschedparam(pthread_t thread, + int policy, const struct sched_param *param) +{ + return pthread_setschedparam(thread, policy, param); +} + +__attribute__ ((weak)) +int __real_pthread_create(pthread_t *tid, + const pthread_attr_t * attr, + void *(*start) (void *), void *arg) +{ + return pthread_create(tid, attr, start, arg); +} + +__attribute__ ((weak)) +int __real_open(const char *path, int oflag, ...) +{ + va_list ap; + mode_t mode; + + if (oflag & O_CREAT) { + va_start(ap, oflag); + mode = va_arg(ap, mode_t); + va_end(ap); + return open(path, oflag, mode); + } else + return open(path, oflag); +} + +__attribute__ ((weak)) +int __real_close(int fd) +{ + return close(fd); +} + +__attribute__ ((weak)) +int __real_ioctl(int fd, int request, ...) +{ + va_list ap; + void *arg; + + va_start(ap, request); + arg = va_arg(ap, void *); + va_end(ap); + + return ioctl(fd, request, arg); +} + +__attribute__ ((weak)) +void *__real_mmap(void *addr, + size_t len, int prot, int flags, int fd, off_t off) +{ + return mmap(addr, len, prot, flags, fd, off); +} + +__attribute__ ((weak)) +int __real_munmap(void *addr, size_t len) +{ + return munmap(addr, len); +} Index: src/skins/rtdm/wrappers.h =================================================================== --- src/skins/rtdm/wrappers.h (revision 0) +++ src/skins/rtdm/wrappers.h (revision 0) @@ -0,0 +1,26 @@ +#ifndef _XENO_WRAPPERS_H +#define _XENO_WRAPPERS_H + +#include <sys/types.h> +#include <pthread.h> + +int __real_pthread_create(pthread_t *tid, + const pthread_attr_t * attr, + void *(*start) (void *), void *arg); + +int __real_pthread_setschedparam(pthread_t thread, + int policy, const struct sched_param *param); + + +int __real_open(const char *path, int oflag, ...); + +int __real_close(int fd); + +int __real_ioctl(int fd, int request, ...); + +void *__real_mmap(void *addr, + size_t len, int prot, int flags, int fd, off_t off); + +int __real_munmap(void *addr, size_t len); + +#endif /* !_XENO_WRAPPERS_H */
_______________________________________________ Xenomai-core mailing list Xenomai-core@gna.org https://mail.gna.org/listinfo/xenomai-core