summaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
authorGregory Nutt <gnutt@nuttx.org>2013-05-22 07:35:54 -0600
committerGregory Nutt <gnutt@nuttx.org>2013-05-22 07:35:54 -0600
commit08750a096b1c72e577a102a619b9a0827d2f3b9d (patch)
tree9f3860b60e8d08d412097de00b98e112a162de77 /apps
parent801977de7c84d03616329795e1c59929ab948906 (diff)
downloadpx4-nuttx-08750a096b1c72e577a102a619b9a0827d2f3b9d.tar.gz
px4-nuttx-08750a096b1c72e577a102a619b9a0827d2f3b9d.tar.bz2
px4-nuttx-08750a096b1c72e577a102a619b9a0827d2f3b9d.zip
Add TCP echo server from Max Holtzberg
Diffstat (limited to 'apps')
-rw-r--r--apps/ChangeLog.txt4
-rw-r--r--apps/examples/Kconfig1
-rw-r--r--apps/examples/Make.defs4
-rw-r--r--apps/examples/Makefile4
-rw-r--r--apps/examples/README.txt21
-rw-r--r--apps/examples/tcpecho/Kconfig54
-rw-r--r--apps/examples/tcpecho/Makefile110
-rw-r--r--apps/examples/tcpecho/tcpecho_main.c387
8 files changed, 582 insertions, 3 deletions
diff --git a/apps/ChangeLog.txt b/apps/ChangeLog.txt
index e6ceed475..474c1fd63 100644
--- a/apps/ChangeLog.txt
+++ b/apps/ChangeLog.txt
@@ -557,4 +557,6 @@
example is built as an NSH built-in application. (2013-5-16).
* apps/netutils/discover: Added a runtime configuration for the
UDP discover utility. From Max Holtzberg (2013-5-21).
-
+ * apps/examples/tcpecho: Added a simple single threaded, poll based
+ TCP echo server based on W. Richard Stevens UNIX Network Programming
+ Book. Contributed by Max Holtberg (2013-5-22).
diff --git a/apps/examples/Kconfig b/apps/examples/Kconfig
index 11a56a81c..97e6ce140 100644
--- a/apps/examples/Kconfig
+++ b/apps/examples/Kconfig
@@ -50,6 +50,7 @@ source "$APPSDIR/examples/serloop/Kconfig"
source "$APPSDIR/examples/flash_test/Kconfig"
source "$APPSDIR/examples/smart_test/Kconfig"
source "$APPSDIR/examples/smart/Kconfig"
+source "$APPSDIR/examples/tcpecho/Kconfig"
source "$APPSDIR/examples/telnetd/Kconfig"
source "$APPSDIR/examples/thttpd/Kconfig"
source "$APPSDIR/examples/tiff/Kconfig"
diff --git a/apps/examples/Make.defs b/apps/examples/Make.defs
index 67b07a21b..28399192b 100644
--- a/apps/examples/Make.defs
+++ b/apps/examples/Make.defs
@@ -226,6 +226,10 @@ ifeq ($(CONFIG_EXAMPLES_SMART),y)
CONFIGURED_APPS += examples/smart
endif
+ifeq ($(CONFIG_EXAMPLES_TCPECHO),y)
+CONFIGURED_APPS += examples/tcpecho
+endif
+
ifeq ($(CONFIG_EXAMPLES_TELNETD),y)
CONFIGURED_APPS += examples/telnetd
endif
diff --git a/apps/examples/Makefile b/apps/examples/Makefile
index 7827bf9a3..2ffe131c6 100644
--- a/apps/examples/Makefile
+++ b/apps/examples/Makefile
@@ -42,7 +42,7 @@ SUBDIRS += flash_test ftpc ftpd hello helloxx hidkbd igmp json keypadtest
SUBDIRS += lcdrw mm modbus mount mtdpart nettest nsh null nx nxconsole nxffs
SUBDIRS += nxflat nxhello nximage nxlines nxtext ostest pashello pipe poll
SUBDIRS += posix_spawn pwm qencoder relays rgmp romfs sendmail serloop
-SUBDIRS += smart smart_test telnetd thttpd tiff touchscreen udp uip
+SUBDIRS += smart smart_test tcpecho telnetd thttpd tiff touchscreen udp uip
SUBDIRS += usbserial usbstorage usbterm watchdog wget wgetjson xmlrpc
# Sub-directories that might need context setup. Directories may need
@@ -60,7 +60,7 @@ CNTXTDIRS = pwm
ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
CNTXTDIRS += adc can cdcacm composite cxxtest dhcpd discover flash_test ftpd
CNTXTDIRS += hello helloxx json keypadtestmodbus mtdpart nettest nxlines relays
-CNTXTDIRS += qencoder smart_test telnetd watchdog wgetjson
+CNTXTDIRS += qencoder smart_test tcpecho telnetd watchdog wgetjson
endif
ifeq ($(CONFIG_EXAMPLES_LCDRW_BUILTIN),y)
diff --git a/apps/examples/README.txt b/apps/examples/README.txt
index b41e2f4c2..ac922a703 100644
--- a/apps/examples/README.txt
+++ b/apps/examples/README.txt
@@ -727,6 +727,8 @@ examples/nettest
CONFIGURED_APPS += uiplib
+ See also examples/tcpecho
+
examples/nsh
^^^^^^^^^^^^
@@ -1508,6 +1510,25 @@ examples/smart_test
* CONFIG_NSH_BUILTIN_APPS=y: This test can be built only as an NSH
command
+examples/tcpecho
+^^^^^^^^^^^^^^^^
+
+ Simple single threaded, poll based TCP echo server. This example implements
+ the TCP Echo Server from W. Richard Stevens UNIX Network Programming Book.
+ Contributed by Max Holtberg.
+
+ See also examples/nettest
+
+ * CONFIG_EXAMPLES_TCPECHO =y: Enables the TCP echo server.
+ * CONFIG_XAMPLES_TCPECHO_PORT: Server Port, default 80
+ * CONFIG_EXAMPLES_TCPECHO_BACKLOG: Listen Backlog, default 8
+ * CONFIG_EXAMPLES_TCPECHO_NCONN: Number of Connections, default 8
+ * CONFIG_EXAMPLES_TCPECHO_DHCPC: DHCP Client, default n
+ * CONFIG_EXAMPLES_TCPECHO_NOMAC: Use Canned MAC Address, default n
+ * CONFIG_EXAMPLES_TCPECHO_IPADDR: Target IP address, default 0x0a000002
+ * CONFIG_EXAMPLES_TCPECHO_DRIPADDR: Default Router IP address (Gateway), default 0x0a000001
+ * CONFIG_EXAMPLES_TCPECHO_NETMASK: Network Mask, default 0xffffff00
+
examples/telnetd
^^^^^^^^^^^^^^^^
diff --git a/apps/examples/tcpecho/Kconfig b/apps/examples/tcpecho/Kconfig
new file mode 100644
index 000000000..cc063e0a5
--- /dev/null
+++ b/apps/examples/tcpecho/Kconfig
@@ -0,0 +1,54 @@
+#
+# For a description of the syntax of this configuration file,
+# see misc/tools/kconfig-language.txt.
+#
+
+config EXAMPLES_TCPECHO
+ bool "Simple TCP echo server"
+ default n
+ ---help---
+ Simple single threaded, poll based TCP echo server. This example
+ implements the TCP Echo Server from W. Richard Stevens
+ UNIX Network Programming Book.
+
+config EXAMPLES_TCPECHO_PORT
+ int "Server Port"
+ default 80
+ depends on EXAMPLES_TCPECHO
+
+config EXAMPLES_TCPECHO_BACKLOG
+ int "Listen Backlog"
+ default 8
+ depends on EXAMPLES_TCPECHO
+
+config EXAMPLES_TCPECHO_NCONN
+ int "Number of Connections"
+ default 8
+ depends on EXAMPLES_TCPECHO
+
+config EXAMPLES_TCPECHO_DHCPC
+ bool "DHCP Client"
+ default n
+ depends on EXAMPLES_TCPECHO && !NSH_BUILTIN_APPS
+ select NETUTILS_DHCPC
+ select NETUTILS_RESOLV
+
+config EXAMPLES_TCPECHO_NOMAC
+ bool "Use Canned MAC Address"
+ default n
+ depends on EXAMPLES_TCPECHO && !NSH_BUILTIN_APPS
+
+config EXAMPLES_TCPECHO_IPADDR
+ hex "Target IP address"
+ default 0x0a000002
+ depends on EXAMPLES_TCPECHO && !NSH_BUILTIN_APPS && !EXAMPLES_TCPECHO_DHCPC
+
+config EXAMPLES_TCPECHO_DRIPADDR
+ hex "Default Router IP address (Gateway)"
+ default 0x0a000001
+ depends on EXAMPLES_TCPECHO && !NSH_BUILTIN_APPS
+
+config EXAMPLES_TCPECHO_NETMASK
+ hex "Network Mask"
+ default 0xffffff00
+ depends on EXAMPLES_TCPECHO && !NSH_BUILTIN_APPS
diff --git a/apps/examples/tcpecho/Makefile b/apps/examples/tcpecho/Makefile
new file mode 100644
index 000000000..7eb59120d
--- /dev/null
+++ b/apps/examples/tcpecho/Makefile
@@ -0,0 +1,110 @@
+############################################################################
+# apps/examples/tcpecho/Makefile
+#
+# Copyright (C) 2013 Max Holtzberg. All rights reserved.
+# Copyright (C) 2008, 2010-2012 Gregory Nutt. All rights reserved.
+#
+# Authors: Max Holtzberg <mh@uvc.de>
+# Gregory Nutt <gnutt@nuttx.org>
+#
+# 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.
+# 3. Neither the name NuttX nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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
+# COPYRIGHT OWNER 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 $(TOPDIR)/.config
+-include $(TOPDIR)/Make.defs
+include $(APPDIR)/Make.defs
+
+# Discover built-in application info
+
+APPNAME = tcpecho
+PRIORITY = SCHED_PRIORITY_DEFAULT
+STACKSIZE = 2048
+
+ASRCS =
+CSRCS = tcpecho_main.c
+
+AOBJS = $(ASRCS:.S=$(OBJEXT))
+COBJS = $(CSRCS:.c=$(OBJEXT))
+
+SRCS = $(ASRCS) $(CSRCS)
+OBJS = $(AOBJS) $(COBJS)
+
+ifeq ($(CONFIG_WINDOWS_NATIVE),y)
+ BIN = ..\..\libapps$(LIBEXT)
+else
+ifeq ($(WINTOOL),y)
+ BIN = ..\\..\\libapps$(LIBEXT)
+else
+ BIN = ../../libapps$(LIBEXT)
+endif
+endif
+
+ROOTDEPPATH = --dep-path .
+
+# Common build
+
+VPATH =
+
+all: .built
+.PHONY: clean depend distclean
+
+$(AOBJS): %$(OBJEXT): %.S
+ $(call ASSEMBLE, $<, $@)
+
+$(COBJS): %$(OBJEXT): %.c
+ $(call COMPILE, $<, $@)
+
+.built: $(OBJS)
+ $(call ARCHIVE, $(BIN), $(OBJS))
+ @touch .built
+
+ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
+$(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat: $(DEPCONFIG) Makefile
+ $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main)
+
+context: $(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat
+else
+context:
+endif
+
+.depend: Makefile $(SRCS)
+ @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
+ @touch $@
+
+depend: .depend
+
+clean:
+ $(call DELFILE, .built)
+ $(call CLEAN)
+
+distclean: clean
+ $(call DELFILE, Make.dep)
+ $(call DELFILE, .depend)
+
+-include Make.dep
diff --git a/apps/examples/tcpecho/tcpecho_main.c b/apps/examples/tcpecho/tcpecho_main.c
new file mode 100644
index 000000000..c26462f14
--- /dev/null
+++ b/apps/examples/tcpecho/tcpecho_main.c
@@ -0,0 +1,387 @@
+/****************************************************************************
+ * examples/tcpecho/tcpecho_main.c
+ *
+ * Copyright (C) 2013 Max Holtzberg. All rights reserved.
+ * Copyright (C) 2008, 2011-2012 Gregory Nutt. All rights reserved.
+ *
+ * Authors: Max Holtzberg <mh@uvc.de>
+ * Gregory Nutt <gnutt@nuttx.org>
+ *
+ * This code is based upon the poll example from W. Richard Stevens'
+ * UNIX Network Programming Book.
+ *
+ * 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.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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
+ * COPYRIGHT OWNER 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <debug.h>
+#include <errno.h>
+#include <poll.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <net/if.h>
+#include <nuttx/net/uip/uip.h>
+#include <nuttx/net/uip/uip-arp.h>
+
+#include <apps/netutils/uiplib.h>
+
+#ifdef CONFIG_EXAMPLES_TCPECHO_DHCPC
+# include <arpa/inet.h>
+#endif
+
+/* Here we include the header file for the application(s) we use in
+ * our project as defined in the config/<board-name>/defconfig file
+ */
+
+/* DHCPC may be used in conjunction with any other feature (or not) */
+
+#ifdef CONFIG_EXAMPLES_TCPECHO_DHCPC
+# include <apps/netutils/resolv.h>
+# include <apps/netutils/dhcpc.h>
+#endif
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define TCPECHO_MAXLINE 64
+#define TCPECHO_POLLTIMEOUT 10000
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int tcpecho_netsetup(void);
+static int tcpecho_server(void);
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int tcpecho_netsetup()
+{
+ /* If this task is excecutated as an NSH built-in function, then the
+ * network has already been configured by NSH's start-up logic.
+ */
+
+ struct dhcpc_state ds;
+#ifndef CONFIG_NSH_BUILTIN_APPS
+ struct in_addr addr;
+#if defined(CONFIG_EXAMPLES_TCPECHO_DHCPC) || defined(CONFIG_EXAMPLES_TCPECHO_NOMAC)
+ uint8_t mac[IFHWADDRLEN];
+#endif
+#ifdef CONFIG_EXAMPLES_TCPECHO_DHCPC
+ void *handle;
+#endif
+
+/* Many embedded network interfaces must have a software assigned MAC */
+
+#ifdef CONFIG_EXAMPLES_TCPECHO_NOMAC
+ mac[0] = 0x00;
+ mac[1] = 0xe0;
+ mac[2] = 0xde;
+ mac[3] = 0xad;
+ mac[4] = 0xbe;
+ mac[5] = 0xef;
+ uip_setmacaddr("eth0", mac);
+#endif
+
+ /* Set up our host address */
+
+#ifdef CONFIG_EXAMPLES_TCPECHO_DHCPC
+ addr.s_addr = 0;
+#else
+ addr.s_addr = HTONL(CONFIG_EXAMPLES_TCPECHO_IPADDR);
+#endif
+ uip_sethostaddr("eth0", &addr);
+
+ /* Set up the default router address */
+
+ addr.s_addr = HTONL(CONFIG_EXAMPLES_TCPECHO_DRIPADDR);
+ uip_setdraddr("eth0", &addr);
+
+ /* Setup the subnet mask */
+
+ addr.s_addr = HTONL(CONFIG_EXAMPLES_TCPECHO_NETMASK);
+ uip_setnetmask("eth0", &addr);
+
+#ifdef CONFIG_EXAMPLES_TCPECHO_DHCPC
+ /* Set up the resolver */
+
+ resolv_init();
+
+ /* Get the MAC address of the NIC */
+
+ uip_getmacaddr("eth0", mac);
+
+ /* Set up the DHCPC modules */
+
+ handle = dhcpc_open(&mac, IFHWADDRLEN);
+
+ /* Get an IP address. Note: there is no logic here for renewing the address in this
+ * example. The address should be renewed in ds.lease_time/2 seconds.
+ */
+
+ if (!handle)
+ {
+ return ERROR;
+ }
+
+ if (dhcpc_request(handle, &ds) != OK)
+ {
+ return ERROR;
+ }
+
+ uip_sethostaddr("eth1", &ds.ipaddr);
+
+ if (ds.netmask.s_addr != 0)
+ {
+ uip_setnetmask("eth0", &ds.netmask);
+ }
+
+ if (ds.default_router.s_addr != 0)
+ {
+ uip_setdraddr("eth0", &ds.default_router);
+ }
+
+ if (ds.dnsaddr.s_addr != 0)
+ {
+ resolv_conf(&ds.dnsaddr);
+ }
+
+ dhcpc_close(handle);
+ printf("IP: %s\n", inet_ntoa(ds.ipaddr));
+
+#endif /* CONFIG_EXAMPLES_TCPECHO_DHCPC */
+#endif /* CONFIG_NSH_BUILTIN_APPS */
+
+ return OK;
+}
+
+static int tcpecho_server(void)
+{
+ int i, maxi, listenfd, connfd, sockfd;
+ int nready;
+ int ret;
+ ssize_t n;
+ char buf[TCPECHO_MAXLINE];
+ socklen_t clilen;
+ bool stop = false;
+ struct pollfd client[CONFIG_EXAMPLES_TCPECHO_NCONN];
+ struct sockaddr_in cliaddr, servaddr;
+
+ listenfd = socket(AF_INET, SOCK_STREAM, 0);
+
+ if (listenfd < 0)
+ {
+ perror("ERROR: failed to create socket.\n");
+ return ERROR;
+ }
+
+ bzero(&servaddr, sizeof(servaddr));
+ servaddr.sin_family = AF_INET;
+ servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+ servaddr.sin_port = htons(CONFIG_EXAMPLES_TCPECHO_PORT);
+
+ ret = bind(listenfd, (struct sockaddr*)&servaddr, sizeof(servaddr));
+ if (ret < 0)
+ {
+ perror("ERROR: failed to bind socket.\n");
+ return ERROR;
+ }
+
+ ndbg("start listening on port: %d\n", CONFIG_EXAMPLES_TCPECHO_PORT);
+
+ ret = listen(listenfd, CONFIG_EXAMPLES_TCPECHO_BACKLOG);
+ if (ret < 0)
+ {
+ perror("ERROR: failed to start listening\n");
+ return ERROR;
+ }
+
+ client[0].fd = listenfd;
+ client[0].events = POLLRDNORM;
+ for (i = 1; i < CONFIG_EXAMPLES_TCPECHO_NCONN; i++)
+ {
+ client[i].fd = -1; /* -1 indicates available entry */
+ }
+
+ maxi = 0; /* max index into client[] array */
+
+ while(!stop)
+ {
+ nready = poll(client, maxi+1, TCPECHO_POLLTIMEOUT);
+
+ if (client[0].revents & POLLRDNORM)
+ {
+ /* new client connection */
+
+ clilen = sizeof(cliaddr);
+ connfd = accept(listenfd, (struct sockaddr*)&cliaddr, &clilen);
+
+ ndbg("new client: %s\n", inet_ntoa(cliaddr.sin_addr));
+
+ for (i = 1; i < CONFIG_EXAMPLES_TCPECHO_NCONN; i++)
+ {
+ if (client[i].fd < 0)
+ {
+ client[i].fd = connfd; /* save descriptor */
+ break;
+ }
+ }
+
+ if (i == CONFIG_EXAMPLES_TCPECHO_NCONN)
+ {
+ perror("ERROR: too many clients");
+ return ERROR;
+ }
+
+ client[i].events = POLLRDNORM;
+ if (i > maxi)
+ {
+ maxi = i; /* max index in client[] array */
+ }
+
+ if (--nready <= 0)
+ {
+ continue; /* no more readable descriptors */
+ }
+ }
+
+ for (i = 1; i <= maxi; i++)
+ {
+ /* check all clients for data */
+
+ if ((sockfd = client[i].fd) < 0)
+ {
+ continue;
+ }
+
+ if (client[i].revents & (POLLRDNORM | POLLERR))
+ {
+ if ( (n = read(sockfd, buf, TCPECHO_MAXLINE)) < 0)
+ {
+ if (errno == ECONNRESET)
+ {
+ /* connection reset by client */
+
+ ndbg("client[%d] aborted connection\n", i);
+
+ close(sockfd);
+ client[i].fd = -1;
+ }
+ else
+ {
+ perror("ERROR: readline error\n");
+ close(sockfd);
+ client[i].fd = -1;
+ }
+ }
+ else if (n == 0)
+ {
+ /* connection closed by client */
+
+ ndbg("client[%d] closed connection\n", i);
+
+ close(sockfd);
+ client[i].fd = -1;
+ }
+ else
+ {
+ if (strcmp(buf, "exit\r\n") == 0)
+ {
+ ndbg("client[%d] closed connection\n", i);
+ close(sockfd);
+ client[i].fd = -1;
+ }
+ else
+ {
+ write(sockfd, buf, n);
+ }
+ }
+
+ if (--nready <= 0)
+ {
+ break; /* no more readable descriptors */
+ }
+ }
+ }
+ }
+
+ for (i = 0; i <= maxi; i++)
+ {
+ if (client[i].fd < 0)
+ {
+ continue;
+ }
+
+ close(client[i].fd);
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * discover_main
+ ****************************************************************************/
+
+int tcpecho_main(int argc, char *argv[])
+{
+ int ret;
+
+ ret = tcpecho_netsetup();
+ if (ret != OK)
+ {
+ perror("ERROR: failed to setup network\n");
+ exit(1);
+ }
+
+ printf("Start echo server\n");
+
+ ret = tcpecho_server();
+
+ return ret;
+}
+