1/*	$NetBSD: svc_fdset.c,v 1.16 2017/04/18 11:35:34 maya Exp $	*/
2
3/*-
4 * Copyright (c) 2015 The NetBSD Foundation, Inc.
5 * All rights reserved.
6 *
7 * This code is derived from software contributed to The NetBSD Foundation
8 * by Christos Zoulas.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 * 1. Redistributions of source code must retain the above copyright
14 *    notice, this list of conditions and the following disclaimer.
15 * 2. Redistributions in binary form must reproduce the above copyright
16 *    notice, this list of conditions and the following disclaimer in the
17 *    documentation and/or other materials provided with the distribution.
18 *
19 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
20 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
21 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
22 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
23 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 * POSSIBILITY OF SUCH DAMAGE.
30 */
31
32#include <sys/cdefs.h>
33__RCSID("$NetBSD: svc_fdset.c,v 1.16 2017/04/18 11:35:34 maya Exp $");
34
35
36#include "reentrant.h"
37
38#include <sys/fd_set.h>
39
40#include <rpc/rpc.h>
41
42#ifdef FDSET_DEBUG
43#include <stdio.h>
44#include <stdarg.h>
45#include <unistd.h>
46#include <lwp.h>
47#endif
48#include <stdlib.h>
49#include <string.h>
50#include <poll.h>
51
52#include "svc_fdset.h"
53
54#undef svc_fdset
55#undef svc_maxfd
56#ifdef _LIBC
57extern __fd_set_256 svc_fdset;
58#endif
59extern int svc_maxfd;
60int __svc_flags;
61
62struct svc_fdset {
63	/* select */
64	fd_set *fdset;
65	int	fdmax;
66	int	fdsize;
67	/* poll */
68	struct pollfd *fdp;
69	int	fdnum;
70	int	fdused;
71};
72
73/* The single threaded, one global fd_set version */
74static struct svc_fdset __svc_fdset;
75
76static thread_key_t fdsetkey = -2;
77
78#ifdef FDSET_DEBUG
79
80static void  __printflike(3, 0)
81svc_header(const char *func, size_t line, const char *fmt, va_list ap)
82{
83	fprintf(stderr, "%s[%d.%d]: %s, %zu: ", getprogname(), (int)getpid(),
84	    (int)_lwp_self(), func, line);
85	vfprintf(stderr, fmt, ap);
86	va_end(ap);
87}
88
89static void __printflike(4, 5)
90svc_fdset_print(const char *func, size_t line, struct svc_fdset *fds,
91    const char *fmt, ...)
92{
93	va_list ap;
94	const char *did = "";
95
96	va_start(ap, fmt);
97	svc_header(func, line, fmt, ap);
98	va_end(ap);
99
100	fprintf(stderr, "%p[%d] fd_set<", fds->fdset, fds->fdmax);
101	for (int i = 0; i <= fds->fdmax; i++) {
102		if (!FD_ISSET(i, fds->fdset))
103			continue;
104		fprintf(stderr, "%s%d", did, i);
105		did = ", ";
106	}
107	did = "";
108	fprintf(stderr, "> poll<");
109	for (int i = 0; i < fds->fdused; i++) {
110		int fd = fds->fdp[i].fd;
111		if (fd == -1)
112			continue;
113		fprintf(stderr, "%s%d", did, fd);
114		did = ", ";
115	}
116	fprintf(stderr, ">\n");
117}
118
119static void __printflike(3, 4)
120svc_print(const char *func, size_t line, const char *fmt, ...)
121{
122	va_list ap;
123
124	va_start(ap, fmt);
125	svc_header(func, line, fmt, ap);
126	va_end(ap);
127	fprintf(stderr, "\n");
128}
129
130#define DPRINTF(...)		svc_print(__func__, __LINE__, __VA_ARGS__)
131#define DPRINTF_FDSET(...)	svc_fdset_print(__func__, __LINE__, __VA_ARGS__)
132
133#else
134
135#define DPRINTF(...)
136#define DPRINTF_FDSET(...)
137
138#endif
139
140
141static inline void
142svc_fdset_sanitize(struct svc_fdset *fds)
143{
144	while (fds->fdmax >= 0 && !FD_ISSET(fds->fdmax, fds->fdset))
145		fds->fdmax--;
146#ifdef _LIBC
147	/* Compat update */
148	if (fds == &__svc_fdset) {
149		svc_fdset = *(__fd_set_256 *)(void *)__svc_fdset.fdset;
150		svc_maxfd = __svc_fdset.fdmax;
151	}
152#endif
153}
154
155static void
156svc_fdset_free(void *v)
157{
158	struct svc_fdset *fds = v;
159	DPRINTF_FDSET(fds, "free");
160
161	free(fds->fdp);
162	free(fds->fdset);
163	free(fds);
164}
165
166static void
167svc_pollfd_init(struct pollfd *pfd, int nfd)
168{
169	for (int i = 0; i < nfd; i++) {
170		pfd[i].fd = -1;
171		pfd[i].events = POLLIN | POLLPRI | POLLRDNORM | POLLRDBAND;
172		pfd[i].revents = 0;
173	}
174}
175
176static struct pollfd *
177svc_pollfd_alloc(struct svc_fdset *fds)
178{
179	if (fds->fdp != NULL)
180		return fds->fdp;
181
182	fds->fdnum = FD_SETSIZE;
183	fds->fdp = calloc(fds->fdnum, sizeof(*fds->fdp));
184	if (fds->fdp == NULL)
185		return NULL;
186	svc_pollfd_init(fds->fdp, fds->fdnum);
187	return fds->fdp;
188}
189
190
191static struct svc_fdset *
192svc_pollfd_add(int fd, struct svc_fdset *fds)
193{
194	struct pollfd *pfd;
195
196	if ((pfd = svc_pollfd_alloc(fds)) == NULL)
197		return NULL;
198
199	for (int i = 0; i < fds->fdnum; i++)
200		if (pfd[i].fd == -1) {
201			if (i >= fds->fdused)
202				fds->fdused = i + 1;
203			DPRINTF("add fd=%d slot=%d fdused=%d",
204			    fd, i, fds->fdused);
205			pfd[i].fd = fd;
206			return fds;
207		}
208
209	pfd = realloc(fds->fdp, (fds->fdnum + FD_SETSIZE) * sizeof(*fds->fdp));
210	if (pfd == NULL)
211		return NULL;
212
213	svc_pollfd_init(pfd + fds->fdnum, FD_SETSIZE);
214	pfd[fds->fdnum].fd = fd;
215	fds->fdused = fds->fdnum + 1;
216	DPRINTF("add fd=%d slot=%d fdused=%d", fd, fds->fdnum, fds->fdused);
217	fds->fdnum += FD_SETSIZE;
218	fds->fdp = pfd;
219	return fds;
220}
221
222static struct svc_fdset *
223svc_pollfd_del(int fd, struct svc_fdset *fds)
224{
225	struct pollfd *pfd;
226
227	if ((pfd = svc_pollfd_alloc(fds)) == NULL)
228		return NULL;
229
230	for (int i = 0; i < fds->fdnum; i++) {
231		if (pfd[i].fd != fd)
232			continue;
233
234		pfd[i].fd = -1;
235		DPRINTF("del fd=%d slot=%d", fd, fds->fdused);
236		if (i != fds->fdused - 1)
237			return fds;
238
239		do
240			if (pfd[i].fd != -1)
241				break;
242		while (--i >= 0);
243
244		fds->fdused = i + 1;
245		DPRINTF("del fd=%d fdused=%d", fd, fds->fdused);
246		return fds;
247	}
248	DPRINTF("del fd=%d not found", fd);
249	return NULL;
250}
251
252static struct svc_fdset *
253svc_fdset_resize(int fd, struct svc_fdset *fds)
254{
255	if (fds->fdset && fd < fds->fdsize) {
256		DPRINTF_FDSET(fds, "keeping %d < %d", fd, fds->fdsize);
257		return fds;
258	}
259
260	fd += FD_SETSIZE;
261
262	char *newfdset = realloc(fds->fdset, __NFD_BYTES(fd));
263	if (newfdset == NULL)
264		return NULL;
265
266	memset(newfdset + __NFD_BYTES(fds->fdsize), 0,
267	    __NFD_BYTES(fd) - __NFD_BYTES(fds->fdsize));
268
269
270	fds->fdset = (void *)newfdset;
271	DPRINTF_FDSET(fds, "resize %d > %d", fd, fds->fdsize);
272	fds->fdsize = fd;
273
274	return fds;
275}
276
277static struct svc_fdset *
278svc_fdset_alloc(int fd)
279{
280	struct svc_fdset *fds;
281
282	if (!__isthreaded || fdsetkey == -2)
283		return svc_fdset_resize(fd, &__svc_fdset);
284
285	if (fdsetkey == -1)
286		thr_keycreate(&fdsetkey, svc_fdset_free);
287
288	if ((fds = thr_getspecific(fdsetkey)) == NULL) {
289
290		fds = calloc(1, sizeof(*fds));
291		if (fds == NULL)
292			return NULL;
293
294		(void)thr_setspecific(fdsetkey, fds);
295
296		if (__svc_fdset.fdsize != 0) {
297			*fds = __svc_fdset;
298			DPRINTF("switching to %p", fds->fdset);
299		} else {
300			DPRINTF("first thread time %p", fds->fdset);
301		}
302	} else {
303		DPRINTF("again for %p", fds->fdset);
304		if (fd < fds->fdsize)
305			return fds;
306	}
307
308	return svc_fdset_resize(fd, fds);
309}
310
311/* allow each thread to have their own copy */
312void
313svc_fdset_init(int flags)
314{
315	DPRINTF("%x", flags);
316	__svc_flags = flags;
317	if ((flags & SVC_FDSET_MT) && fdsetkey == -2)
318		fdsetkey = -1;
319}
320
321void
322svc_fdset_zero(void)
323{
324	DPRINTF("zero");
325
326	struct svc_fdset *fds = svc_fdset_alloc(0);
327	if (fds == NULL)
328		return;
329	memset(fds->fdset, 0, fds->fdsize);
330	fds->fdmax = -1;
331
332	free(fds->fdp);
333	fds->fdp = NULL;
334	fds->fdnum = fds->fdused = 0;
335}
336
337int
338svc_fdset_set(int fd)
339{
340	struct svc_fdset *fds = svc_fdset_alloc(fd);
341
342	if (fds == NULL)
343		return -1;
344
345	FD_SET(fd, fds->fdset);
346	if (fd > fds->fdmax)
347		fds->fdmax = fd;
348
349	int rv = svc_pollfd_add(fd, fds) ? 0 : -1;
350	DPRINTF_FDSET(fds, "%d", fd);
351
352	svc_fdset_sanitize(fds);
353	return rv;
354}
355
356int
357svc_fdset_isset(int fd)
358{
359	struct svc_fdset *fds = svc_fdset_alloc(fd);
360
361	if (fds == NULL)
362		return -1;
363
364	DPRINTF_FDSET(fds, "%d", fd);
365
366	return FD_ISSET(fd, fds->fdset) != 0;
367}
368
369int
370svc_fdset_clr(int fd)
371{
372	struct svc_fdset *fds = svc_fdset_alloc(fd);
373
374	if (fds == NULL)
375		return -1;
376
377	FD_CLR(fd, fds->fdset);
378
379	int rv = svc_pollfd_del(fd, fds) ? 0 : -1;
380	DPRINTF_FDSET(fds, "%d", fd);
381
382	svc_fdset_sanitize(fds);
383	return rv;
384}
385
386fd_set *
387svc_fdset_copy(const fd_set *orig)
388{
389	int size = svc_fdset_getsize(0);
390	if (size == -1)
391		return NULL;
392	fd_set *copy = calloc(1, __NFD_BYTES(size));
393	if (copy == NULL)
394		return NULL;
395	if (orig)
396		memcpy(copy, orig, __NFD_BYTES(size));
397	return copy;
398}
399
400fd_set *
401svc_fdset_get(void)
402{
403	struct svc_fdset *fds = svc_fdset_alloc(0);
404
405	if (fds == NULL)
406		return NULL;
407
408	DPRINTF_FDSET(fds, "get");
409	svc_fdset_sanitize(fds);
410	return fds->fdset;
411}
412
413int *
414svc_fdset_getmax(void)
415{
416	struct svc_fdset *fds = svc_fdset_alloc(0);
417
418	if (fds == NULL)
419		return NULL;
420
421	DPRINTF_FDSET(fds, "getmax");
422	svc_fdset_sanitize(fds);
423	return &fds->fdmax;
424}
425
426int
427svc_fdset_getsize(int fd)
428{
429	struct svc_fdset *fds = svc_fdset_alloc(fd);
430
431	if (fds == NULL)
432		return -1;
433
434	DPRINTF_FDSET(fds, "getsize");
435	return fds->fdsize;
436}
437
438struct pollfd *
439svc_pollfd_copy(const struct pollfd *orig)
440{
441	int size = svc_fdset_getsize(0);
442	if (size == -1)
443		return NULL;
444	struct pollfd *copy = calloc(size, sizeof(*orig));
445	if (copy == NULL)
446		return NULL;
447	if (orig)
448		memcpy(copy, orig, size * sizeof(*orig));
449	return copy;
450}
451
452struct pollfd *
453svc_pollfd_get(void)
454{
455	struct svc_fdset *fds = svc_fdset_alloc(0);
456
457	if (fds == NULL)
458		return NULL;
459
460	DPRINTF_FDSET(fds, "getpoll");
461	return fds->fdp;
462}
463
464int *
465svc_pollfd_getmax(void)
466{
467	struct svc_fdset *fds = svc_fdset_alloc(0);
468
469	if (fds == NULL)
470		return NULL;
471	return &fds->fdused;
472}
473
474int
475svc_pollfd_getsize(int fd)
476{
477	struct svc_fdset *fds = svc_fdset_alloc(fd);
478
479	if (fds == NULL)
480		return -1;
481
482	DPRINTF_FDSET(fds, "getsize");
483	return fds->fdnum;
484}
485