1#include <mqueue.h>
 2#include <pthread.h>
 3#include <errno.h>
 4#include <sys/socket.h>
 5#include <signal.h>
 6#include <unistd.h>
 7#include <semaphore.h>
 8#include "syscall.h"
 9
10struct args {
11	sem_t sem;
12	int sock;
13	mqd_t mqd;
14	int err;
15	const struct sigevent *sev;
16};
17
18static void *start(void *p)
19{
20	struct args *args = p;
21	char buf[32];
22	ssize_t n;
23	int s = args->sock;
24	void (*func)(union sigval) = args->sev->sigev_notify_function;
25	union sigval val = args->sev->sigev_value;
26	struct sigevent sev2;
27	static const char zeros[32];
28	int err;
29
30	sev2.sigev_notify = SIGEV_THREAD;
31	sev2.sigev_signo = s;
32	sev2.sigev_value.sival_ptr = (void *)&zeros;
33
34	args->err = err = -__syscall(SYS_mq_notify, args->mqd, &sev2);
35	sem_post(&args->sem);
36	if (err) return 0;
37
38	pthread_detach(pthread_self());
39	n = recv(s, buf, sizeof(buf), MSG_NOSIGNAL|MSG_WAITALL);
40	close(s);
41	if (n==sizeof buf && buf[sizeof buf - 1] == 1)
42		func(val);
43	return 0;
44}
45
46int mq_notify(mqd_t mqd, const struct sigevent *sev)
47{
48	struct args args = { .sev = sev };
49	pthread_attr_t attr;
50	pthread_t td;
51	int s;
52	int cs;
53	sigset_t allmask, origmask;
54
55	if (!sev || sev->sigev_notify != SIGEV_THREAD)
56		return syscall(SYS_mq_notify, mqd, sev);
57
58	s = socket(AF_NETLINK, SOCK_RAW|SOCK_CLOEXEC, 0);
59	if (s < 0) return -1;
60	args.sock = s;
61	args.mqd = mqd;
62
63	if (sev->sigev_notify_attributes) attr = *sev->sigev_notify_attributes;
64	else pthread_attr_init(&attr);
65	pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
66	sem_init(&args.sem, 0, 0);
67
68	sigfillset(&allmask);
69	pthread_sigmask(SIG_BLOCK, &allmask, &origmask);
70	if (pthread_create(&td, &attr, start, &args)) {
71		__syscall(SYS_close, s);
72		pthread_sigmask(SIG_SETMASK, &origmask, 0);
73		errno = EAGAIN;
74		return -1;
75	}
76	pthread_sigmask(SIG_SETMASK, &origmask, 0);
77
78	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &cs);
79	sem_wait(&args.sem);
80	sem_destroy(&args.sem);
81
82	if (args.err) {
83		__syscall(SYS_close, s);
84		pthread_join(td, 0);
85		pthread_setcancelstate(cs, 0);
86		errno = args.err;
87		return -1;
88	}
89
90	pthread_setcancelstate(cs, 0);
91	return 0;
92}