Bring in my initial version of POSIX realtime extension library.

Current the library implements mqueue, timer and aio with SIGEV_THREAD
notification supported.

Earlier version reviewed by: deischen

lib/librt/Makefile

@@ -0,0 +1,15 @@
+# $FreeBSD$
+
+LIB=rt
+SHLIB_MAJOR= 1
+CFLAGS+=-I${.CURDIR}/../libc/include -I${.CURDIR}
+CFLAGS+=-Winline -Wall -g
+
+#MAN=	libthr.3
+NO_MAN=yes
+
+SRCS+= aio.c mq.c sigev_thread.c timer.c
+
+PRECIOUSLIB=
+
+.include <bsd.lib.mk>

lib/librt/aio.c

@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2005 David Xu <davidxu@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice unmodified, this list of conditions, and the following
+ *    disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ *
+ */
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+#include <sys/aio.h>
+
+#include "namespace.h"
+#include <errno.h>
+#include <pthread.h>
+#include <stddef.h>
+#include <signal.h>
+#include <unistd.h>
+#include "sigev_thread.h"
+#include "un-namespace.h"
+
+__weak_reference(__aio_read, _aio_read);
+__weak_reference(__aio_read, aio_read);
+__weak_reference(__aio_write, _aio_write);
+__weak_reference(__aio_write, aio_write);
+__weak_reference(__aio_return, _aio_return);
+__weak_reference(__aio_return, aio_return);
+__weak_reference(__aio_waitcomplete, _aio_waitcomplete);
+__weak_reference(__aio_waitcomplete, aio_waitcomplete);
+
+typedef void (*aio_func)(union sigval val, struct aiocb *iocb);
+
+extern int __sys_aio_read(struct aiocb *iocb);
+extern int __sys_aio_write(struct aiocb *iocb);
+extern int __sys_aio_waitcomplete(struct aiocb **iocbp, struct timespec *timeout);
+extern int __sys_aio_return(struct aiocb *iocb);
+extern int __sys_aio_error(struct aiocb *iocb);
+
+static void
+aio_dispatch(struct sigev_node *sn, siginfo_t *si)
+{
+	aio_func f = sn->sn_func;
+
+	f(sn->sn_value, (struct aiocb *)sn->sn_id);
+}
+
+static int
+aio_io(struct aiocb *iocb, int (*sysfunc)(struct aiocb *iocb))
+{
+	struct sigev_node *sn;
+	struct sigevent saved_ev;
+	int ret;
+
+	if (iocb->aio_sigevent.sigev_notify != SIGEV_THREAD) {
+		ret = sysfunc(iocb);
+		return (ret);
+	}
+
+	if (__sigev_check_init()) {
+		/* This might be that thread library is not enabled. */
+		errno = EINVAL;
+		return (-1);
+	}
+
+	sn = __sigev_alloc(SI_ASYNCIO, &iocb->aio_sigevent);
+	if (sn == NULL) {
+		errno = EAGAIN;
+		return (-1);
+	}
+	
+	saved_ev = iocb->aio_sigevent;
+	sn->sn_id = (sigev_id_t)iocb;
+	sn->sn_flags |= SNF_ONESHOT;
+	__sigev_get_sigevent(sn, &iocb->aio_sigevent, sn->sn_id);
+
+	__sigev_list_lock();
+	__sigev_register(sn);
+	__sigev_list_unlock();
+
+	sn->sn_dispatch = aio_dispatch;
+	ret = sysfunc(iocb);
+	iocb->aio_sigevent = saved_ev;
+
+	if (ret != 0) {
+		__sigev_list_lock();
+		__sigev_delete_node(sn);
+		__sigev_list_unlock();
+	}
+	return (ret);
+}
+
+int
+__aio_read(struct aiocb *iocb)
+{
+	return aio_io(iocb, &__sys_aio_read);
+}
+
+int
+__aio_write(struct aiocb *iocb)
+{
+	return aio_io(iocb, &__sys_aio_write);
+}
+
+int
+__aio_waitcomplete(struct aiocb **iocbp, struct timespec *timeout)
+{
+	int ret = __sys_aio_waitcomplete(iocbp, timeout);
+
+	if (*iocbp) {
+		if ((*iocbp)->aio_sigevent.sigev_notify == SIGEV_THREAD) {
+			__sigev_list_lock();
+			__sigev_delete(SI_ASYNCIO, (sigev_id_t)(*iocbp));
+			__sigev_list_unlock();
+		}
+	}
+
+	return (ret);
+}
+
+int
+__aio_return(struct aiocb *iocb)
+{
+	int ret = __sys_aio_return(iocb);
+	int err = __sys_aio_error(iocb);
+
+	if (err != EINPROGRESS &&
+	    iocb->aio_sigevent.sigev_notify == SIGEV_THREAD) {
+		__sigev_list_lock();
+		__sigev_delete(SI_ASYNCIO, (sigev_id_t)iocb);
+		__sigev_list_unlock();
+	}
+
+	return (ret);
+}

lib/librt/mq.c

@@ -0,0 +1,231 @@
+/*-
+ * Copyright (c) 2006 David Xu <davidxu@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+#include <sys/mqueue.h>
+
+#include "namespace.h"
+#include <errno.h>
+#include <pthread.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <unistd.h>
+#include "sigev_thread.h"
+#include "un-namespace.h"
+
+extern int	__sys_kmq_notify(int, const struct sigevent *);
+extern int	__sys_kmq_open(const char *, int, mode_t);
+extern int	__sys_kmq_setattr(int, const struct mq_attr *__restrict,
+		    struct mq_attr *__restrict);
+extern ssize_t	__sys_kmq_timedreceive(int, char *__restrict, size_t,
+		    unsigned *__restrict, const struct timespec *__restrict);
+extern int	__sys_kmq_timedsend(int, const char *, size_t, unsigned,
+		    const struct timespec *);
+extern int	__sys_kmq_unlink(const char *);
+extern int	__sys_close(int fd);
+
+struct __mq {
+	int oshandle;
+	struct sigev_node *node;
+};
+
+__weak_reference(__mq_open, mq_open);
+__weak_reference(__mq_open, _mq_open);
+__weak_reference(__mq_close, mq_close);
+__weak_reference(__mq_close, _mq_close);
+__weak_reference(__mq_notify, mq_notify);
+__weak_reference(__mq_notify, _mq_notify);
+__weak_reference(__mq_getattr, mq_getattr);
+__weak_reference(__mq_getattr, _mq_getattr);
+__weak_reference(__mq_setattr, mq_setattr);
+__weak_reference(__mq_setattr, _mq_setattr);
+__weak_reference(__mq_timedreceive, mq_timedreceive);
+__weak_reference(__mq_timedreceive, _mq_timedreceive);
+__weak_reference(__mq_timedsend, mq_timedsend);
+__weak_reference(__mq_timedsend, _mq_timedsend);
+__weak_reference(__mq_unlink, mq_unlink);
+__weak_reference(__mq_unlink, _mq_unlink);
+__weak_reference(__mq_send, mq_send);
+__weak_reference(__mq_send, _mq_send);
+__weak_reference(__mq_receive, mq_receive);
+__weak_reference(__mq_receive, _mq_receive);
+
+mqd_t
+__mq_open(const char *name, int oflag, mode_t mode)
+{
+	struct __mq *mq;
+	int err;
+
+	mq = malloc(sizeof(struct __mq));
+	if (mq == NULL)
+		return (NULL);
+
+	mq->oshandle = __sys_kmq_open(name, oflag, mode);
+	if (mq->oshandle != -1) {
+		mq->node = NULL;
+		return (mq);
+	}
+	err = errno;
+	free(mq);
+	errno = err;
+	return ((mqd_t)-1L);
+}
+
+int
+__mq_close(mqd_t mqd)
+{
+	int h;
+
+	if (mqd->node != NULL) {
+		__sigev_list_lock();
+		__sigev_delete_node(mqd->node);
+		__sigev_list_unlock();
+	}
+	h = mqd->oshandle;
+	free(mqd);
+	return (__sys_close(h));
+}
+
+typedef void (*mq_func)(union sigval val);
+
+static void
+mq_dispatch(struct sigev_node *sn, siginfo_t *si)
+{
+	mq_func f = sn->sn_func;
+
+	/*
+	 * Check generation before calling user function,
+	 * this should avoid expired notification.
+	 */
+	if (sn->sn_gen == si->si_value.sival_int)
+		f(sn->sn_value);
+}
+
+int
+__mq_notify(mqd_t mqd, const struct sigevent *evp)
+{
+	struct sigevent ev;
+	struct sigev_node *sn;
+	int ret;
+
+	if (evp == NULL || evp->sigev_notify != SIGEV_THREAD) {
+		if (mqd->node != NULL) {
+			__sigev_list_lock();
+			__sigev_delete_node(mqd->node);
+			mqd->node = NULL;
+			__sigev_list_unlock();
+		}
+		return __sys_kmq_notify(mqd->oshandle, evp);
+	}
+
+	if (__sigev_check_init()) {
+		/*
+		 * Thread library is not enabled.
+		 */
+		errno = EINVAL;
+		return (-1);
+	}
+
+	sn = __sigev_alloc(SI_MESGQ, evp);
+	if (sn == NULL) {
+		errno = EAGAIN;
+		return (-1);
+	}
+
+	sn->sn_id = mqd->oshandle;
+	sn->sn_dispatch = mq_dispatch;
+	__sigev_get_sigevent(sn, &ev, sn->sn_gen);
+	__sigev_list_lock();
+	if (mqd->node != NULL)
+		__sigev_delete_node(mqd->node);
+	mqd->node = sn;
+	__sigev_register(sn);
+	ret = __sys_kmq_notify(mqd->oshandle, &ev);
+	__sigev_list_unlock();
+	return (ret);
+}
+
+int
+__mq_getattr(mqd_t mqd, struct mq_attr *attr)
+{
+
+	return __sys_kmq_setattr(mqd->oshandle, NULL, attr);
+}
+
+int
+__mq_setattr(mqd_t mqd, const struct mq_attr *newattr, struct mq_attr *oldattr)
+{
+
+	return __sys_kmq_setattr(mqd->oshandle, newattr, oldattr);
+}
+
+ssize_t
+__mq_timedreceive(mqd_t mqd, char *buf, size_t len,
+	unsigned *prio, const struct timespec *timeout)
+{
+
+	return __sys_kmq_timedreceive(mqd->oshandle, buf, len, prio, timeout);
+}
+
+ssize_t
+__mq_receive(mqd_t mqd, char *buf, size_t len, unsigned *prio)
+{
+
+	return __sys_kmq_timedreceive(mqd->oshandle, buf, len, prio, NULL);
+}
+
+ssize_t
+__mq_timedsend(mqd_t mqd, char *buf, size_t len,
+	unsigned prio, const struct timespec *timeout)
+{
+
+	return __sys_kmq_timedsend(mqd->oshandle, buf, len, prio, timeout);
+}
+
+ssize_t
+__mq_send(mqd_t mqd, char *buf, size_t len, unsigned prio)
+{
+
+	return __sys_kmq_timedsend(mqd->oshandle, buf, len, prio, NULL);
+}
+
+int
+__mq_unlink(const char *path)
+{
+
+	return __sys_kmq_unlink(path);
+}
+
+int
+__mq_oshandle(mqd_t mqd)
+{
+	return mqd->oshandle;
+}

lib/librt/sigev_thread.c

@@ -0,0 +1,433 @@
+/*
+ * Copyright (c) 2005 David Xu <davidxu@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice unmodified, this list of conditions, and the following
+ *    disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ *
+ */
+
+#include <sys/types.h>
+#include <machine/atomic.h>
+
+#include "namespace.h"
+#include <err.h>
+#include <ucontext.h>
+#include <sys/thr.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <pthread.h>
+#include "un-namespace.h"
+
+#include "sigev_thread.h"
+
+LIST_HEAD(sigev_list_head, sigev_node);
+#define HASH_QUEUES		17
+#define	HASH(t, id)		((((id) << 3) + (t)) % HASH_QUEUES)
+static struct sigev_list_head	sigev_hash[HASH_QUEUES];
+static struct sigev_list_head	sigev_all;
+static int			sigev_generation;
+static pthread_mutex_t		*sigev_list_mtx;
+static TAILQ_HEAD(,sigev_thread_node)	sigev_threads;
+static pthread_mutex_t		*sigev_threads_mtx;
+static pthread_attr_t		sigev_default_attr;
+static pthread_once_t		sigev_once = PTHREAD_ONCE_INIT;
+
+static void __sigev_fork_prepare(void);
+static void __sigev_fork_parent(void);
+static void __sigev_fork_child(void);
+static struct sigev_thread_node *sigev_thread_create(pthread_attr_t *);
+static void *sigev_service_loop(void *);
+
+#pragma weak pthread_create
+
+__weak_reference(__sigev_stub_zero, pthread_attr_getschedpolicy);
+__weak_reference(__sigev_stub_zero, pthread_attr_getinheritsched);
+__weak_reference(__sigev_stub_zero, pthread_attr_getschedparam);
+__weak_reference(__sigev_stub_zero, pthread_attr_getscope);
+__weak_reference(__sigev_stub_zero, pthread_attr_getstacksize);
+__weak_reference(__sigev_stub_zero, pthread_attr_getstackaddr);
+__weak_reference(__sigev_stub_zero, pthread_attr_getguardsize);
+__weak_reference(__sigev_stub_zero, pthread_attr_init);
+__weak_reference(__sigev_stub_zero, pthread_attr_setscope);
+__weak_reference(__sigev_stub_zero, pthread_attr_setdetachstate);
+__weak_reference(__sigev_stub_zero, pthread_atfork);
+__weak_reference(__sigev_stub_zero, _pthread_once);
+__weak_reference(__sigev_stub_zero, pthread_cleanup_push);
+__weak_reference(__sigev_stub_zero, pthread_cleanup_pop);
+__weak_reference(__sigev_stub_zero, pthread_setcancelstate);
+
+int
+__sigev_stub_zero(void)
+{
+	return (0);
+}
+
+static __inline void
+attr2sna(pthread_attr_t *attr, struct sigev_thread_attr *sna)
+{
+	struct sched_param sched_param;
+
+	pthread_attr_getschedpolicy(attr, &sna->sna_policy);
+	pthread_attr_getinheritsched(attr, &sna->sna_inherit);
+	pthread_attr_getschedparam(attr, &sched_param);
+	sna->sna_prio = sched_param.sched_priority;
+	pthread_attr_getscope(attr, &sna->sna_scope);
+	pthread_attr_getstacksize(attr, &sna->sna_stacksize);
+	pthread_attr_getstackaddr(attr, &sna->sna_stackaddr);
+	pthread_attr_getguardsize(attr, &sna->sna_guardsize);
+}
+
+static __inline int
+sna_eq(const struct sigev_thread_attr *a, const struct sigev_thread_attr *b)
+{
+	return memcmp(a, b, sizeof(*a)) == 0;
+}
+
+static __inline int
+have_threads(void)
+{
+	return (pthread_create != NULL);
+}
+
+void
+__sigev_thread_init(void)
+{
+	static int notfirst = 0;
+	int i;
+
+	sigev_list_mtx = malloc(sizeof(pthread_mutex_t));
+	_pthread_mutex_init(sigev_list_mtx, NULL);
+	sigev_threads_mtx = malloc(sizeof(pthread_mutex_t));
+	_pthread_mutex_init(sigev_threads_mtx, NULL);
+	for (i = 0; i < HASH_QUEUES; ++i)
+		LIST_INIT(&sigev_hash[i]);
+	LIST_INIT(&sigev_all);
+	TAILQ_INIT(&sigev_threads);
+	if (!notfirst) {
+		pthread_attr_init(&sigev_default_attr);
+		pthread_attr_setscope(&sigev_default_attr, PTHREAD_SCOPE_SYSTEM);
+		pthread_attr_setdetachstate(&sigev_default_attr,
+			PTHREAD_CREATE_DETACHED);
+		pthread_atfork(__sigev_fork_prepare, __sigev_fork_parent,
+			__sigev_fork_child);
+		notfirst = 1;
+	}
+}
+
+int
+__sigev_check_init(void)
+{
+	if (!have_threads())
+		return (-1);
+
+	_pthread_once(&sigev_once, __sigev_thread_init);
+	return (0);
+}
+
+void
+__sigev_fork_prepare(void)
+{
+	__sigev_thread_list_lock();
+}
+
+void
+__sigev_fork_parent(void)
+{
+	__sigev_thread_list_unlock();
+}
+
+void
+__sigev_fork_child(void)
+{
+	__sigev_thread_init();
+}
+
+int
+__sigev_list_lock(void)
+{
+	return _pthread_mutex_lock(sigev_list_mtx);
+}
+
+int
+__sigev_list_unlock(void)
+{
+	return _pthread_mutex_unlock(sigev_list_mtx);
+}
+
+int
+__sigev_thread_list_lock(void)
+{
+	return _pthread_mutex_lock(sigev_threads_mtx);
+}
+
+int
+__sigev_thread_list_unlock(void)
+{
+	return _pthread_mutex_unlock(sigev_threads_mtx);
+}
+
+struct sigev_node *
+__sigev_alloc(int type, const struct sigevent *evp)
+{
+	struct sigev_node *sn;
+
+	sn = calloc(1, sizeof(*sn));
+	if (sn != NULL) {
+		sn->sn_value = evp->sigev_value;
+		sn->sn_func = evp->sigev_notify_function;
+		sn->sn_gen = atomic_fetchadd_int(&sigev_generation, 1);
+		sn->sn_type = type;
+		sn->sn_tn = sigev_thread_create(evp->sigev_notify_attributes);
+		if (sn->sn_tn == NULL) {
+			free(sn);
+			sn = NULL;
+		}
+	}
+	return (sn);
+}
+
+void
+__sigev_get_sigevent(struct sigev_node *sn, struct sigevent *newevp,
+	sigev_id_t id)
+{
+	/*
+	 * Build a new sigevent, and tell kernel to deliver SIGEV_SIGSERVICE
+	 * signal to the new thread.
+	 */
+	newevp->sigev_notify = SIGEV_THREAD_ID;
+	newevp->sigev_signo  = SIGEV_SIGSERVICE;
+	newevp->sigev_notify_thread_id = (lwpid_t)sn->sn_tn->tn_lwpid;
+	newevp->sigev_value.sival_ptr = (void *)id;
+}
+
+void
+__sigev_free(struct sigev_node *sn)
+{
+	free(sn);
+}
+
+struct sigev_node *
+__sigev_find(int type, sigev_id_t id)
+{
+	struct sigev_node *sn;
+	int chain = HASH(type, id);
+
+	LIST_FOREACH(sn, &sigev_hash[chain], sn_link) {
+		if (sn->sn_type == type && sn->sn_id == id)
+			break;
+	}
+	return (sn);
+}
+
+int
+__sigev_register(struct sigev_node *sn)
+{
+	int chain = HASH(sn->sn_type, sn->sn_id);
+
+	LIST_INSERT_HEAD(&sigev_hash[chain], sn, sn_link);
+	LIST_INSERT_HEAD(&sigev_all, sn, sn_allist);
+	return (0);
+}
+
+int
+__sigev_delete(int type, sigev_id_t id)
+{
+	struct sigev_node *sn;
+
+	sn = __sigev_find(type, id);
+	if (sn != NULL)
+		return (__sigev_delete_node(sn));
+	return (0);
+}
+
+int
+__sigev_delete_node(struct sigev_node *sn)
+{
+	LIST_REMOVE(sn, sn_link);
+	LIST_REMOVE(sn, sn_allist);
+
+	if (sn->sn_flags & SNF_WORKING)
+		sn->sn_flags |= SNF_REMOVED;
+	else
+		__sigev_free(sn);
+	return (0);
+}
+
+static
+sigev_id_t
+sigev_get_id(siginfo_t *si)
+{
+	switch(si->si_code) {
+	case SI_TIMER:
+		return (si->si_timerid);
+	case SI_MESGQ:
+		return (si->si_mqd);
+	case SI_ASYNCIO:
+		return (sigev_id_t)si->si_value.sival_ptr;
+	default:
+		warnx("%s %s : unknown si_code %d\n", __FILE__, __func__,
+			si->si_code);
+	}
+	return (-1);
+}
+
+static struct sigev_thread_node *
+sigev_thread_create(pthread_attr_t *pattr)
+{
+	struct sigev_thread_node *tn;
+	struct sigev_thread_attr sna;
+	sigset_t set;
+	int ret;
+
+	if (pattr == NULL)
+		pattr = &sigev_default_attr;
+	else {
+		pthread_attr_setscope(pattr, PTHREAD_SCOPE_SYSTEM);
+		pthread_attr_setdetachstate(pattr, PTHREAD_CREATE_DETACHED);
+	}
+ 
+	attr2sna(pattr, &sna);
+
+	__sigev_thread_list_lock();
+	/* Search a thread matching the required pthread_attr. */
+	TAILQ_FOREACH(tn, &sigev_threads, tn_link) {
+		if (sna.sna_stackaddr == NULL) {
+			if (sna_eq(&tn->tn_sna, &sna))
+				break;
+		} else {
+			/*
+			 * Reuse the thread if it has same stack address,
+			 * because two threads can not run on same stack.
+			 */
+			if (sna.sna_stackaddr == tn->tn_sna.sna_stackaddr)
+				break;
+		}
+	}
+	if (tn != NULL) {
+		__sigev_thread_list_unlock();
+		return (tn);
+	}
+	tn = malloc(sizeof(*tn));
+	tn->tn_sna = sna;
+	tn->tn_cur = NULL;
+	TAILQ_INSERT_TAIL(&sigev_threads, tn, tn_link);
+	sigemptyset(&set);
+	sigaddset(&set, SIGEV_SIGSERVICE);
+	_sigprocmask(SIG_BLOCK, &set, NULL);
+	_pthread_cond_init(&tn->tn_cv, NULL);
+	ret = pthread_create(&tn->tn_thread, pattr, sigev_service_loop, tn);
+	_sigprocmask(SIG_UNBLOCK, &set, NULL);
+	if (ret != 0) {
+		TAILQ_REMOVE(&sigev_threads, tn, tn_link);
+		__sigev_thread_list_unlock();
+		_pthread_cond_destroy(&tn->tn_cv);
+		free(tn);
+		tn = NULL;
+	} else {
+		/* wait the thread to get its lwpid */
+		_pthread_cond_wait(&tn->tn_cv, sigev_threads_mtx);
+		__sigev_thread_list_unlock();
+	}
+	return (tn);
+}
+
+static void
+after_dispatch(struct sigev_thread_node *tn)
+{
+	struct sigev_node *sn;
+
+	if ((sn = tn->tn_cur) != NULL) {
+		__sigev_list_lock();
+		sn->sn_flags &= ~SNF_WORKING;
+		if (sn->sn_flags & SNF_REMOVED)
+			__sigev_free(sn);
+		else if (sn->sn_flags & SNF_ONESHOT)
+			__sigev_delete_node(sn);
+		tn->tn_cur = NULL;
+		__sigev_list_unlock();
+	}
+}
+
+/*
+ * This function is called if user callback calls
+ * pthread_exit() or pthread_cancel() for the thread.
+ */
+static void
+thread_cleanup(void *arg)
+{
+	struct sigev_thread_node *tn = arg;
+
+	fprintf(stderr, "Dangerous Robinson, calling pthread_exit() from "
+			"SIGEV_THREAD is undefined.");
+	after_dispatch(tn);
+	/* longjmp(tn->tn_jbuf, 1); */
+	abort();
+}
+
+static void *
+sigev_service_loop(void *arg)
+{
+	siginfo_t si;
+	sigset_t set;
+	struct sigev_thread_node *tn;
+	struct sigev_node *sn;
+	sigev_id_t id;
+
+	tn = arg;
+	thr_self(&tn->tn_lwpid);
+	__sigev_list_lock();
+	_pthread_cond_broadcast(&tn->tn_cv);
+	__sigev_list_unlock();
+
+	/*
+	 * Service thread should not be killed by callback, if user
+	 * attempts to do so, the thread will be restarted.
+	 */
+	setjmp(tn->tn_jbuf);
+	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
+	sigemptyset(&set);
+	sigaddset(&set, SIGEV_SIGSERVICE);
+	pthread_cleanup_push(thread_cleanup, tn);
+	for (;;) {
+		if (__predict_false(sigwaitinfo(&set, &si) == -1))
+			continue;
+
+		id = sigev_get_id(&si);
+		__sigev_list_lock();
+		sn = __sigev_find(si.si_code, id);
+		if (sn != NULL) {
+			tn->tn_cur = sn;
+			sn->sn_flags |= SNF_WORKING;
+			__sigev_list_unlock();
+			sn->sn_dispatch(sn, &si);
+			after_dispatch(tn);
+		} else 
+			tn->tn_cur = NULL;
+		__sigev_list_unlock();
+	}
+	pthread_cleanup_pop(0);
+	return (0);
+}

lib/librt/sigev_thread.h

@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2005 David Xu <davidxu@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice unmodified, this list of conditions, and the following
+ *    disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ *
+ */
+
+#ifndef _SIGEV_THREAD_H_
+#define _SIGEV_THREAD_H_
+
+#include <sys/types.h>
+#include <sys/queue.h>
+#include <setjmp.h>
+
+struct sigev_thread_node;
+struct sigev_node;
+
+typedef uintptr_t	sigev_id_t;
+typedef void		(*sigev_dispatch_t)(struct sigev_node *, siginfo_t *);
+
+struct sigev_node {
+	LIST_ENTRY(sigev_node)		sn_link;
+	LIST_ENTRY(sigev_node)		sn_allist;
+	int				sn_type;
+	sigev_id_t			sn_id;
+	sigev_dispatch_t		sn_dispatch;
+	union sigval			sn_value;
+	void 				*sn_func;
+	int				sn_flags;
+	int				sn_gen;
+	struct sigev_thread_node *	sn_tn;
+};
+
+struct sigev_thread_attr {
+	int	sna_policy;
+	int	sna_inherit;
+	int	sna_prio;
+	int	sna_scope;
+	size_t	sna_stacksize;
+	void	*sna_stackaddr;
+	size_t	sna_guardsize;
+};
+
+struct sigev_thread_node {
+	TAILQ_ENTRY(sigev_thread_node)	tn_link;
+	pthread_t			tn_thread;
+	struct sigev_node		*tn_cur;
+	struct sigev_thread_attr	tn_sna;
+	long				tn_lwpid;
+	pthread_cond_t			tn_cv;
+	jmp_buf				tn_jbuf;
+};
+
+#define	SNF_WORKING		0x01
+#define	SNF_REMOVED		0x02
+#define SNF_ONESHOT		0x04
+
+#define	SIGEV_SIGSERVICE	(SIGTHR+1)
+
+int	__sigev_check_init();
+struct sigev_node *__sigev_alloc(int, const struct sigevent *);
+struct sigev_node *__sigev_find(int, sigev_id_t);
+void	__sigev_get_sigevent(struct sigev_node *, struct sigevent *,
+		sigev_id_t);
+int	__sigev_register(struct sigev_node *);
+int	__sigev_delete(int, sigev_id_t);
+int	__sigev_delete_node(struct sigev_node *);
+int	__sigev_list_lock(void);
+int	__sigev_list_unlock(void);
+int	__sigev_thread_list_lock(void);
+int	__sigev_thread_list_unlock(void);
+void	__sigev_free(struct sigev_node *);
+
+#endif

lib/librt/timer.c

@@ -0,0 +1,181 @@
+/*
+ * Copyright (c) 2006 David Xu <davidxu@freebsd.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice unmodified, this list of conditions, and the following
+ *    disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ *
+ */
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+
+#include "namespace.h"
+#include <errno.h>
+#include <pthread.h>
+#include <stddef.h>
+#include <signal.h>
+#include <stdlib.h>
+#include <time.h>
+#include <unistd.h>
+#include "sigev_thread.h"
+#include "un-namespace.h"
+
+extern int __sys_ktimer_create(clockid_t, struct sigevent *__restrict,
+	int *__restrict);
+extern int __sys_ktimer_delete(int);
+extern int __sys_ktimer_gettime(int, struct itimerspec *);
+extern int __sys_ktimer_getoverrun(int);
+extern int __sys_ktimer_settime(int, int,
+	const struct itimerspec *__restrict, struct itimerspec *__restrict);
+
+struct __timer {
+	int oshandle;
+	struct sigev_node *node;
+};
+
+__weak_reference(__timer_create, timer_create);
+__weak_reference(__timer_create, _timer_create);
+__weak_reference(__timer_delete, timer_delete);
+__weak_reference(__timer_delete, _timer_delete);
+__weak_reference(__timer_gettime, timer_gettime);
+__weak_reference(__timer_gettime, _timer_gettime);
+__weak_reference(__timer_settime, timer_settime);
+__weak_reference(__timer_settime, _timer_settime);
+__weak_reference(__timer_getoverrun, timer_getoverrun);
+__weak_reference(__timer_getoverrun, _timer_getoverrun);
+
+typedef void (*timer_func)(union sigval val, int timerid, int overrun);
+
+static void
+timer_dispatch(struct sigev_node *sn, siginfo_t *si)
+{
+	timer_func f = sn->sn_func;
+
+	/* I want to avoid expired notification. */
+	if (si->si_value.sival_int == sn->sn_gen)
+		f(sn->sn_value, si->si_timerid, si->si_overrun);
+}
+
+int
+__timer_create(clockid_t clockid, struct sigevent *evp, timer_t *timerid)
+{
+	struct __timer *timer;
+	struct sigevent ev;
+	struct sigev_node *sn;
+	int ret, err;
+
+	timer = malloc(sizeof(struct __timer));
+	if (timer == NULL)
+		return (-1);
+
+	if (evp == NULL || evp->sigev_notify != SIGEV_THREAD) {
+		ret = __sys_ktimer_create(clockid, evp, &timer->oshandle);
+		if (ret == -1) {
+			err = errno;
+			free(timer);
+			errno = err;
+			return (ret);
+		}
+		timer->node = NULL;
+		*timerid = timer;
+		return (0);
+	}
+
+	if (__sigev_check_init()) {
+		errno = EINVAL;
+		return (-1);
+	}
+
+	sn = __sigev_alloc(SI_TIMER, evp);
+	if (sn == NULL) {
+		errno = EAGAIN;
+		return (-1);
+	}
+
+	__sigev_get_sigevent(sn, &ev, sn->sn_gen);
+	ret = __sys_ktimer_create(clockid, &ev, &timer->oshandle);
+	if (ret != 0) {
+		err = errno;
+		__sigev_free(sn);
+		free(timer);
+		errno = err;
+		return (-1);
+	}
+	sn->sn_dispatch = timer_dispatch;
+	sn->sn_id = timer->oshandle;
+	__sigev_list_lock();
+	__sigev_register(sn);
+	__sigev_list_unlock();
+	*timerid = timer;
+	return (0);
+}
+
+int
+__timer_delete(timer_t timerid)
+{
+	int ret, err;
+
+	if (timerid->node != NULL) {
+		__sigev_list_lock();
+		__sigev_delete_node(timerid->node);
+		__sigev_list_unlock();
+	}
+	ret = __sys_ktimer_delete(timerid->oshandle);
+	err = errno;
+	free(timerid);
+	errno = err;
+	return (ret);	
+}
+
+int
+__timer_gettime(timer_t timerid, struct itimerspec *value)
+{
+
+	return __sys_ktimer_gettime(timerid->oshandle, value);
+}
+
+int
+__timer_getoverrun(timer_t timerid)
+{
+
+	return __sys_ktimer_getoverrun(timerid->oshandle);
+}
+
+int
+__timer_settime(timer_t timerid, int flags,
+	const struct itimerspec *__restrict value,
+	struct itimerspec *__restrict ovalue)
+{
+
+	return __sys_ktimer_settime(timerid->oshandle,
+		flags, value, ovalue);
+}
+
+int
+__timer_oshandle(timer_t timerid)
+{
+
+	return (timerid->oshandle);
+}