Add poll() and select() support for Termios

5 files changed, 416 insertions, 3 deletions

diff --git a/libbsd.py b/libbsd.py
index 43107ee6..42a9f91d 100755
--- a/libbsd.py
+++ b/libbsd.py
@@ -109,8 +109,9 @@ def rtems(mm):
             'rtems/rtems-kernel-sysctlbyname.c',
             'rtems/rtems-kernel-sysctl.c',
             'rtems/rtems-kernel-sysctlnametomib.c',
-            'rtems/rtems-kernel-thread.c',
             'rtems/rtems-kernel-timesupport.c',
+            'rtems/rtems-kernel-termioskqueuepoll.c',
+            'rtems/rtems-kernel-thread.c',
             'rtems/rtems-kernel-vprintf.c',
             'rtems/rtems-legacy-rtrequest.c',
             'rtems/rtems-legacy-newproc.c',
diff --git a/libbsd_waf.py b/libbsd_waf.py
index 651645db..1dd6f81b 100644
--- a/libbsd_waf.py
+++ b/libbsd_waf.py
@@ -1309,6 +1309,7 @@ def build(bld):
               'rtemsbsd/rtems/rtems-kernel-sysctl.c',
               'rtemsbsd/rtems/rtems-kernel-sysctlbyname.c',
               'rtemsbsd/rtems/rtems-kernel-sysctlnametomib.c',
+              'rtemsbsd/rtems/rtems-kernel-termioskqueuepoll.c',
               'rtemsbsd/rtems/rtems-kernel-thread.c',
               'rtemsbsd/rtems/rtems-kernel-timesupport.c',
               'rtemsbsd/rtems/rtems-kernel-vprintf.c',
diff --git a/rtemsbsd/include/machine/rtems-bsd-config.h b/rtemsbsd/include/machine/rtems-bsd-config.h
index ccc65d70..72876d01 100644
--- a/rtemsbsd/include/machine/rtems-bsd-config.h
+++ b/rtemsbsd/include/machine/rtems-bsd-config.h
@@ -189,6 +189,15 @@ extern "C" {
 #endif /* RTEMS_BSD_CONFIG_SERVICE_TELNETD */
 
 /*
+ * Termios
+ */
+#if defined(RTEMS_BSD_CONFIG_TERMIOS_KQUEUE_AND_POLL)
+  #define RTEMS_BSD_CFGDECL_TERMIOS_KQUEUE_AND_POLL SYSINIT_REFERENCE(termioskqueuepoll)
+#else
+  #define RTEMS_BSD_CFGDECL_TERMIOS_KQUEUE_AND_POLL
+#endif /* RTEMS_BSD_CONFIG_TERMIOS_KQUEUE_AND_POLL */
+
+/*
  * Configure the system.
  */
 #if defined(RTEMS_BSD_CONFIG_INIT)
@@ -230,6 +239,8 @@ extern "C" {
   RTEMS_BSD_CFGDECL_TELNETD;
   RTEMS_BSD_CFGDECL_TELNETD_STACK_SIZE;
   RTEMS_BSD_CFGDECL_FTPD;
+
+  RTEMS_BSD_CFGDECL_TERMIOS_KQUEUE_AND_POLL;
 #endif /* RTEMS_BSD_CONFIG_INIT */
 
 #ifdef __cplusplus
diff --git a/rtemsbsd/rtems/rtems-kernel-termioskqueuepoll.c b/rtemsbsd/rtems/rtems-kernel-termioskqueuepoll.c
new file mode 100644
index 00000000..7e8faeb4
--- /dev/null
+++ b/rtemsbsd/rtems/rtems-kernel-termioskqueuepoll.c
@@ -0,0 +1,283 @@
+/**
+ * @file
+ *
+ * @ingroup rtems_bsd_rtems
+ *
+ * @brief TODO.
+ */
+
+/*
+ * Copyright (c) 2017 embedded brains GmbH.
+ * All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * 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.
+ */
+
+#include <machine/rtems-bsd-kernel-space.h>
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/errno.h>
+#include <sys/event.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>
+#include <sys/pcpu.h>
+#include <sys/poll.h>
+#include <sys/selinfo.h>
+
+#include <rtems/termiostypes.h>
+#include <rtems/irq-extension.h>
+
+SYSINIT_REFERENCE(irqs);
+
+typedef struct {
+	rtems_termios_tty *tty;
+	struct selinfo sel;
+	rtems_interrupt_server_request request;
+} termios_selinfo;
+
+static bool
+termios_is_eol(const rtems_termios_tty *tty, char c)
+{
+
+	return (c == '\n' || c == tty->termios.c_cc[VEOF] ||
+	    c == tty->termios.c_cc[VEOL] ||
+	    c == tty->termios.c_cc[VEOL2]);
+}
+
+static bool
+termios_can_read(rtems_termios_tty *tty)
+{
+	rtems_termios_device_context *ctx;
+	rtems_interrupt_lock_context lock_context;
+	unsigned int i;
+	unsigned int size;
+	unsigned int raw_content_size;
+	bool can;
+
+	if (tty->handler.mode == TERMIOS_POLLED) {
+		return (true);
+	}
+
+	if (tty->cindex != tty->ccount) {
+		return (true);
+	}
+
+	ctx = tty->device_context;
+	rtems_termios_device_lock_acquire(ctx, &lock_context);
+	i = tty->rawInBuf.Head;
+	size = tty->rawInBuf.Size;
+	raw_content_size = (tty->rawInBuf.Tail - i) % size;
+
+	if ((tty->termios.c_lflag & ICANON) != 0) {
+		unsigned int todo = raw_content_size;
+
+		/*
+		 * FIXME: What to do in case of a raw input buffer overflow?
+		 * For now, indicated that we can read.  However, this has
+		 * problems in case an erase takes place.
+		 */
+		can = raw_content_size == (size - 1);
+
+		while (todo > 0 && !can) {
+			char c;
+
+			i = (i + 1) % size;
+			c = tty->rawInBuf.theBuf[i];
+			can = termios_is_eol(tty, c);
+			--todo;
+		}
+	} else {
+		cc_t vmin = tty->termios.c_cc[VMIN];
+
+		if (vmin == 0) {
+			vmin = 1;
+		}
+
+		can = raw_content_size >= vmin;
+	}
+
+	if (!can) {
+		tty->tty_rcvwakeup = false;
+	}
+
+	rtems_termios_device_lock_release(ctx, &lock_context);
+	return (can);
+}
+
+static bool
+termios_can_write(const rtems_termios_tty *tty)
+{
+	rtems_termios_device_context *ctx;
+	rtems_interrupt_lock_context lock_context;
+	bool can;
+
+	if (tty->handler.mode == TERMIOS_POLLED) {
+		return (true);
+	}
+
+	ctx = tty->device_context;
+	rtems_termios_device_lock_acquire(ctx, &lock_context);
+	can = ((tty->rawOutBuf.Tail - tty->rawOutBuf.Head - 1) %
+	    tty->rawOutBuf.Size) > 0;
+	rtems_termios_device_lock_release(ctx, &lock_context);
+	return (can);
+}
+
+static void
+termios_receive_wakeup(void *arg)
+{
+	termios_selinfo *ts;
+	rtems_termios_tty *tty;
+	rtems_status_code sc;
+
+	ts = arg;
+	tty = ts->tty;
+
+	sc = rtems_semaphore_obtain(tty->isem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+	BSD_ASSERT(sc == RTEMS_SUCCESSFUL);
+
+	selwakeup(&ts->sel);
+
+	sc = rtems_semaphore_release(tty->isem);
+	BSD_ASSERT(sc == RTEMS_SUCCESSFUL);
+}
+
+static void
+termios_transmit_wakeup(void *arg)
+{
+	termios_selinfo *ts;
+	rtems_termios_tty *tty;
+	rtems_status_code sc;
+
+	ts = arg;
+	tty = ts->tty;
+
+	sc = rtems_semaphore_obtain(tty->osem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+	BSD_ASSERT(sc == RTEMS_SUCCESSFUL);
+
+	selwakeup(&ts->sel);
+
+	sc = rtems_semaphore_release(tty->osem);
+	BSD_ASSERT(sc == RTEMS_SUCCESSFUL);
+}
+
+static void
+termios_wakeup(struct termios *term, void *arg)
+{
+	termios_selinfo *ts = arg;
+
+	rtems_interrupt_server_request_submit(RTEMS_ID_NONE, &ts->request);
+}
+
+static struct selinfo *
+termios_get_selinfo(rtems_termios_tty *tty, struct ttywakeup *wk,
+    rtems_interrupt_handler handler)
+{
+	termios_selinfo *ts = wk->sw_arg;
+
+	if (ts == NULL) {
+		BSD_ASSERT(wk->sw_pfn == NULL);
+		ts = malloc(sizeof(*ts), M_TEMP, M_WAITOK | M_ZERO);
+		ts->tty = tty;
+		rtems_interrupt_server_request_initialize(&ts->request,
+		    handler, ts);
+		wk->sw_arg = ts;
+		wk->sw_pfn = termios_wakeup;
+	} else {
+		BSD_ASSERT(wk->sw_pfn == termios_wakeup);
+	}
+
+	return (&ts->sel);
+}
+
+int
+rtems_termios_kqfilter(rtems_libio_t *iop, struct knote  *kn)
+{
+
+	return (EINVAL);
+}
+
+int
+rtems_termios_poll(rtems_libio_t *iop, int events)
+{
+	struct thread *td = rtems_bsd_get_curthread_or_wait_forever();
+	struct selinfo *sel;
+	rtems_termios_tty *tty;
+	rtems_status_code sc;
+	int revents;
+
+	revents = 0;
+	tty = iop->data1;
+
+	if ((events & (POLLIN | POLLRDNORM)) != 0) {
+		sel = termios_get_selinfo(tty, &tty->tty_rcv,
+		    termios_receive_wakeup);
+
+		sc = rtems_semaphore_obtain(tty->isem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		BSD_ASSERT(sc == RTEMS_SUCCESSFUL);
+
+		if (termios_can_read(tty)) {
+			revents |= events & (POLLIN | POLLRDNORM);
+		} else {
+			selrecord(td, sel);
+		}
+
+		sc = rtems_semaphore_release(tty->isem);
+		BSD_ASSERT(sc == RTEMS_SUCCESSFUL);
+	}
+
+	if ((events & (POLLOUT | POLLWRNORM)) != 0) {
+		sel = termios_get_selinfo(tty, &tty->tty_snd,
+		    termios_transmit_wakeup);
+
+		sc = rtems_semaphore_obtain(tty->osem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		BSD_ASSERT(sc == RTEMS_SUCCESSFUL);
+
+		if (termios_can_write(tty)) {
+			revents |= events & (POLLOUT | POLLWRNORM);
+		} else {
+			selrecord(td, sel);
+		}
+
+		sc = rtems_semaphore_release(tty->osem);
+		BSD_ASSERT(sc == RTEMS_SUCCESSFUL);
+	}
+
+	return (revents);
+}
+
+static void
+termioskqueuepoll_sysinit(void)
+{
+
+	/* Do nothing */
+}
+
+SYSINIT(termioskqueuepoll, SI_SUB_TUNABLES, SI_ORDER_ANY,
+    termioskqueuepoll_sysinit, NULL);
diff --git a/testsuite/netshell01/test_main.c b/testsuite/netshell01/test_main.c
index 36eeea79..1b68971d 100644
--- a/testsuite/netshell01/test_main.c
+++ b/testsuite/netshell01/test_main.c
@@ -29,17 +29,132 @@
  *  that the target is alive after initializing the TCP/IP stack.
  */
 
-#include <stdio.h>
+#include <assert.h>
+#include <errno.h>
 #include <stdarg.h>
+#include <stdbool.h>
+#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <errno.h>
 
 #include <rtems/shell.h>
 
 #define TEST_NAME "LIBBSD NETSHELL 1"
 
 static void
+change_serial_settings(int fd, const struct termios *current, bool icanon)
+{
+	struct termios term = *current;
+
+	term.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR |
+	    ICRNL | IXON);
+	term.c_lflag &= ~(ECHO | ECHOE | ECHOK | ECHONL | ECHOPRT | ECHOCTL |
+	    ECHOKE | ICANON | ISIG | IEXTEN);
+	term.c_cflag &= ~(CSIZE | PARENB);
+	term.c_cflag |= CS8;
+	term.c_oflag &= ~(OPOST | ONLRET | ONLCR | OCRNL | ONLRET | TABDLY |
+	    OLCUC);
+
+	term.c_cc[VMIN] = 0;
+	term.c_cc[VTIME] = 10;
+
+	if (icanon) {
+		term.c_iflag |= ICRNL;
+		term.c_lflag |= ICANON;
+	}
+
+	tcsetattr(fd, TCSANOW, &term);
+}
+
+static void
+do_read_select(int fd)
+{
+	int nfds = fd + 1;
+	struct fd_set read_set;
+	struct timeval timeout = {
+		.tv_sec = 10,
+		.tv_usec = 0
+	};
+	int rv;
+
+	FD_ZERO(&read_set);
+	FD_SET(fd, &read_set);
+
+	rv = select(nfds, &read_set, NULL, NULL, &timeout);
+	if (rv == 0) {
+		printf("timeout\n");
+	} else if (rv > 0) {
+		if (FD_ISSET(fd, &read_set)) {
+			char buf[512];
+			ssize_t n = read(fd, buf, sizeof(buf));
+			printf("read returned %zi\n", n);
+		}
+	} else {
+		perror("select failed");
+	}
+}
+
+static void
+do_write_select(int fd)
+{
+	int nfds = fd + 1;
+	struct fd_set write_set;
+	struct timeval to = {
+		.tv_sec = 0,
+		.tv_usec = 1
+	};
+	struct timeval *timeout = &to;
+	char buf[512];
+	int rv;
+	size_t i;
+
+	memset(buf, 'a', sizeof(buf));
+
+	for (i = 0; i < sizeof(buf); i += 24) {
+		buf[i] = '\r';
+		buf[i + 1] = '\n';
+	}
+
+	for (i = 0; i < 10; ++i) {
+		write(fd, buf, sizeof(buf));
+
+		FD_ZERO(&write_set);
+		FD_SET(fd, &write_set);
+		rv = select(nfds, NULL, &write_set, NULL, timeout);
+		if (rv == 0) {
+			printf("timeout\n");
+		} else {
+			printf("write set: %i\n", FD_ISSET(fd, &write_set));
+		}
+
+		timeout = NULL;
+	}
+}
+
+static int
+termiosselect_command(int argc, char *argv[])
+{
+	bool icanon = argc > 1 && strcmp(argv[1], "icanon") == 0;
+	int fd = STDIN_FILENO;
+	struct termios term;
+	int rv = tcgetattr(fd, &term);
+	assert(rv == 0);
+
+	change_serial_settings(fd, &term, icanon);
+	do_read_select(fd);
+	do_write_select(STDOUT_FILENO);
+	tcsetattr(fd, TCSANOW, &term);
+	return (0);
+}
+
+rtems_shell_cmd_t rtems_shell_ARP_Command = {
+	.name = "termiosselect",
+	.usage = "termiosselect [icanon]",
+	.topic = "net",
+	.command = termiosselect_command
+};
+
+static void
 test_main(void)
 {
 	rtems_shell_env_t env;
@@ -50,4 +165,6 @@ test_main(void)
 	exit(0);
 }
 
+#define RTEMS_BSD_CONFIG_TERMIOS_KQUEUE_AND_POLL
+
 #include <rtems/bsd/test/default-init.h>