 @@ -1,14 +1,14 @@
-/*	$NetBSD: svc_fdset.c,v 1.11 2015/11/08 02:46:53 christos Exp $	*/
+/*	$NetBSD: svc_fdset.c,v 1.12 2015/11/08 19:30:53 christos Exp $	*/
 /*-
  * Copyright (c) 2015 The NetBSD Foundation, Inc.
  * All rights reserved.
+ *
  * This code is derived from software contributed to The NetBSD Foundation
  * by Christos Zoulas.
+ *
  * 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.
 @@ -20,27 +20,27 @@
  * ``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 FOUNDATION 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.
  */
 #include <sys/cdefs.h>
-__RCSID("$NetBSD: svc_fdset.c,v 1.11 2015/11/08 02:46:53 christos Exp $");
+__RCSID("$NetBSD: svc_fdset.c,v 1.12 2015/11/08 19:30:53 christos Exp $");
 #include "reentrant.h"
 #include <sys/fd_set.h>
 #include <rpc/rpc.h>
 #ifdef FDSET_DEBUG
 #include <stdio.h>
 #include <stdarg.h>
 #include <unistd.h>
 #include <lwp.h>
 @@ -87,33 +87,42 @@ svc_header(const char *func, size_t line
+}
 static void __printflike(4, 5)
 svc_fdset_print(const char *func, size_t line, struct svc_fdset *fds,
     const char *fmt, ...)
+{
 	va_list ap;
 	const char *did = "";
 	va_start(ap, fmt);
 	svc_header(func, line, fmt, ap);
 	va_end(ap);
-	fprintf(stderr, "%p[%d] <", fds->fdset, fds->fdmax);
+	fprintf(stderr, "%p[%d] fd_set<", fds->fdset, fds->fdmax);
 	for (int i = 0; i <= fds->fdmax; i++) {
 		if (!FD_ISSET(i, fds->fdset))
 			continue;
 		fprintf(stderr, "%s%d", did, i);
 		did = ", ";
+	}
 	did = "";
 	fprintf(stderr, "> poll<");
 	for (int i = 0; i < fds->fdused; i++) {
 		int fd = fds->fdp[i].fd;
 		if (fd == -1)
 			continue;
 		fprintf(stderr, "%s%d", did, fd);
 		did = ", ";
+	}
 	fprintf(stderr, ">\n");
+}
 static void __printflike(3, 4)
 svc_print(const char *func, size_t line, const char *fmt, ...)
+{
 	va_list ap;
 	va_start(ap, fmt);
 	svc_header(func, line, fmt, ap);
 	va_end(ap);
 	fprintf(stderr, "\n");
+}
 @@ -157,85 +166,95 @@ svc_fdset_free(void *v)
 static void
 svc_pollfd_init(struct pollfd *pfd, int nfd)
+{
 	for (int i = 0; i < nfd; i++) {
 		pfd[i].fd = -1;
 		pfd[i].events = POLLIN | POLLPRI | POLLRDNORM | POLLRDBAND;
 		pfd[i].revents = 0;
+	}
+}
 static struct pollfd *
 svc_pollfd_alloc(struct svc_fdset *fds)
+{
 	if (fds->fdp != NULL)
 		return fds->fdp;
 	fds->fdnum = FD_SETSIZE;
 	fds->fdp = calloc(fds->fdnum, sizeof(*fds->fdp));
 	if (fds->fdp == NULL)
 		return NULL;
 	svc_pollfd_init(fds->fdp, fds->fdnum);
 	return fds->fdp;
+}
 static struct svc_fdset *
 svc_pollfd_add(int fd, struct svc_fdset *fds)
+{
 	struct pollfd *pfd;
 	if ((pfd = svc_pollfd_alloc(fds)) == NULL)
 		return NULL;
 	for (int i = 0; i < fds->fdnum; i++)
 		if (pfd[i].fd == -1) {
-			if (i > fds->fdused)
+			if (i >= fds->fdused)
 				fds->fdused = i + 1;
 			DPRINTF("add fd=%d slot=%d fdused=%d",
 			    fd, i, fds->fdused);
 			pfd[i].fd = fd;
 			return fds;
+		}
 	pfd = realloc(fds->fdp, (fds->fdnum + FD_SETSIZE) * sizeof(*fds->fdp));
 	if (pfd == NULL)
 		return NULL;
 	svc_pollfd_init(pfd + fds->fdnum, FD_SETSIZE);
 	pfd[fds->fdnum].fd = fd;
 	fds->fdused = fds->fdnum + 1;
 	DPRINTF("add fd=%d slot=%d fdused=%d", fd, fds->fdnum, fds->fdused);
 	fds->fdnum += FD_SETSIZE;
 	return fds;
+}
 static struct svc_fdset *
 svc_pollfd_del(int fd, struct svc_fdset *fds)
+{
 	struct pollfd *pfd;
 	if ((pfd = svc_pollfd_alloc(fds)) == NULL)
 		return NULL;
 	for (int i = 0; i < fds->fdnum; i++) {
 		if (pfd[i].fd != fd)
 			continue;
 		pfd[i].fd = -1;
 		DPRINTF("del fd=%d slot=%d", fd, fds->fdused);
 		if (i != fds->fdused - 1)
 			return fds;
 		do
 			if (pfd[i].fd != -1)
 				break;
 		while (--i >= 0);
 		fds->fdused = i + 1;
 		DPRINTF("del fd=%d fdused=%d", fd, fds->fdused);
 		return fds;
+	}
 	DPRINTF("del fd=%d not found", fd);
 	return NULL;
+}
 static struct svc_fdset *
 svc_fdset_resize(int fd, struct svc_fdset *fds)
+{
 	if (fds->fdset && fd < fds->fdsize) {
 		DPRINTF_FDSET(fds, "keeping %d < %d", fd, fds->fdsize);
 		return fds;
+	}
 	fd += FD_SETSIZE;
 @@ -314,58 +333,61 @@ svc_fdset_zero(void)
 int
 svc_fdset_set(int fd)
+{
 	struct svc_fdset *fds = svc_fdset_alloc(fd);
 	if (fds == NULL)
 		return -1;
 	FD_SET(fd, fds->fdset);
 	if (fd > fds->fdmax)
 		fds->fdmax = fd;
 	int rv = svc_pollfd_add(fd, fds) ? 0 : -1;
 	DPRINTF_FDSET(fds, "%d", fd);
 	svc_fdset_sanitize(fds);
-	return svc_pollfd_add(fd, fds) ? 0 : -1;
+	return rv;
+}
 int
 svc_fdset_isset(int fd)
+{
 	struct svc_fdset *fds = svc_fdset_alloc(fd);
 	if (fds == NULL)
 		return -1;
 	DPRINTF_FDSET(fds, "%d", fd);
 	return FD_ISSET(fd, fds->fdset) != 0;
+}
 int
 svc_fdset_clr(int fd)
+{
 	struct svc_fdset *fds = svc_fdset_alloc(fd);
 	if (fds == NULL)
 		return -1;
 	FD_CLR(fd, fds->fdset);
 	int rv = svc_pollfd_del(fd, fds) ? 0 : -1;
 	DPRINTF_FDSET(fds, "%d", fd);
 	svc_fdset_sanitize(fds);
-	return svc_pollfd_del(fd, fds) ? 0 : -1;
+	return rv;
+}
 fd_set *
 svc_fdset_copy(const fd_set *orig)
+{
 	int size = svc_fdset_getsize(0);
 	fd_set *copy = calloc(1, __NFD_BYTES(size));
 	if (copy == NULL)
 		return NULL;
 	if (orig)
 		memcpy(copy, orig, __NFD_BYTES(size));
 	return copy;
+}