very basic telnet server

telnetserv.c

// todo: backtrace

#define _XOPEN_SOURCE 600	/* for the pseudo terminals */
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <errno.h>
#include <termios.h>

#include <fcntl.h>
#include <getopt.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/epoll.h>
#include <sys/stat.h>
#include <sys/ioctl.h>

#include <arpa/inet.h>
#include <netinet/in.h>

#define PROGRAM "telnetserv"

#define log_errno(str, ...) fprintf(stderr, "error: " str " (%s)\n", ##__VA_ARGS__, strerror(errno)); exit(0)
#define log_error(str, ...) fprintf(stderr, "error: " str "\n", ##__VA_ARGS__);
#define log_info(str, ...) do { \
	if (g_verbose) \
		fprintf(stderr, str "\n", ##__VA_ARGS__); \
	} while (0)
#define log_debug(str, ...) do { \
	if (g_debug) \
		fprintf(stdout, "[DEBUG] %s:%d - " str "\n", __FUNCTION__, __LINE__, ##__VA_ARGS__); \
	} while (0)
#define log_oom(str, ...) do { \
	fprintf(stderr, "OOM on %s:%d - " str "\n", __FUNCTION__, __LINE__, ##__VA_ARGS__); \
	abort(); \
	} while (0)


static int g_debug = 0;
static int g_verbose = 0;
static struct sockaddr_in g_listen_addr;
static int g_epoll_fd;
static int g_fds = 0;

struct fd;
struct session {
	struct fd *sfd, *ptym;
	pid_t leader;
};

#define TYPE_SOCKET 1
#define TYPE_PTYM 2
#define TYPE_INVALID 3

struct fd {
	int fd;
	int type;
	int listen;
	struct session *session;
};


/* ARGUMENT PARSING */

static void
help(void)
{
	printf(
		PROGRAM " - a Telnet server\n\n"
		"Usage: " PROGRAM " [OPTIONS]\n\n"
		"OPTIONS can be the following:\n\n"
		"-d                Run in debug mode.\n"
		"-h                Show this and exit.\n"
		"-s <addr>         Bind the server to <addr>. Addr is an IPv4 address.\n"
		"                  Default is 0.0.0.0 (listen on all interfaces).\n\n"
		"-p <port>         Listen on port <port>. Default is 23.\n"
		"                  NOTE: you might need superuser priviledges to listen\n"
		"                  on the default port.\n\n"
		"-v                Run on verbose mode.\n"
	);
}


static void
parse_opts(int argc, char **argv)
{
	int arg;
	opterr = 0;
	while (((arg = getopt(argc, argv, "dhs:p:v"))) != -1) {
		switch (arg) {
			case 'd':
				g_debug = 1;
				break;
			case 'h':
				help();
				exit(1);
			case 's':
				if (inet_pton(AF_INET, optarg, &g_listen_addr.sin_addr) != 1) {
					log_error("invalid address: %s", optarg);
					exit(1);
				}
				break;
			case 'p':
				g_listen_addr.sin_port = htons(atoi(optarg));
				break;
			case 'v':
				g_verbose = 1;
				break;
			case '?':
				log_error("unknown argument: %c", optopt);
				exit(1);
		}
	}
}


/* EXTRA */

static void
print_banner(void)
{
	char addr[INET_ADDRSTRLEN];
	inet_ntop(AF_INET, &g_listen_addr.sin_addr, addr, INET_ADDRSTRLEN);
	printf(PROGRAM " - Listening on %.*s:%hu - Live!\n", INET_ADDRSTRLEN, addr, ntohs(g_listen_addr.sin_port));
}


/* SERVER */

static void
setup_server(void)
{
	int sfd, value = 1;
	struct fd *sfd_struc = malloc(sizeof(*sfd_struc));
	if (!sfd) log_oom("setup_server: cannot allocate fd structure");
	struct epoll_event evt;

	log_debug("initializing epoll");
	g_epoll_fd = epoll_create1(0);
	if (g_epoll_fd < 0) {
		log_errno("could not create epoll fd");
		goto cleanup;
	}

	log_debug("creating socket and adding to the epoll interest list");
	sfd = socket(AF_INET, SOCK_STREAM, 0);
	if (sfd < 0) {
		log_errno("could not create socket");
		goto cleanup_epoll;
	}

	if (setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, &value, sizeof(value)) < 0) {
		log_errno("could not set socket option SO_REUSEADDR");
		goto cleanup_socket;
	}

	if (bind(sfd, (struct sockaddr *) &g_listen_addr, sizeof(g_listen_addr)) < 0) {
		log_errno("could not bind address");
		goto cleanup_socket;
	}

	if (listen(sfd, SOMAXCONN) < 0) {
		log_errno("could not start listening");
		goto cleanup_socket;
	}

	sfd_struc->fd = sfd;
	sfd_struc->type = TYPE_SOCKET;
	sfd_struc->listen = 1;
	sfd_struc->session = NULL;

	evt.data.ptr = sfd_struc;
	evt.events = EPOLLIN;
	if (epoll_ctl(g_epoll_fd, EPOLL_CTL_ADD, sfd, &evt) < 0) {
		log_errno("epoll_ctl failure");
		goto cleanup_socket;
	}

	g_fds++;
	return;

cleanup_socket:
	close(sfd);
cleanup_epoll:
	close(g_epoll_fd);
cleanup:
	free(sfd_struc);
	exit(0);
}


static void
process_new(struct fd *fd)
{
	int connfd, ptym;
	struct sockaddr_in addr;
	socklen_t addr_len = sizeof(addr);
	char addr_str[INET_ADDRSTRLEN];
	struct epoll_event evt_sfd, evt_ptym;
	struct fd *fd_sfd = malloc(sizeof(*fd_sfd)), *fd_ptym = malloc(sizeof(*fd_ptym));
	struct session *session = malloc(sizeof(*session));
	if (!fd_sfd || !fd_ptym) log_oom("cannot allocate fd structure on process_new");
	if (!session) log_oom("cannot allocate session structure on process_new");

	/* accept new connection and print information */
	log_debug("accepting new connection");
	connfd = accept(fd->fd, (struct sockaddr *) &addr, &addr_len);
	if (connfd < 0) {
		log_errno("accept failure");
		return;
	}

	inet_ntop(AF_INET, &addr.sin_addr, addr_str, INET_ADDRSTRLEN);
	log_info("New connection from %.*s:%hu", INET_ADDRSTRLEN, addr_str, ntohs(addr.sin_port));

	fd_sfd->fd = connfd;
	fd_sfd->type = TYPE_SOCKET;
	fd_sfd->listen = 0;
	fd_sfd->session = session;
	session->sfd = fd_sfd;
	session->ptym = fd_ptym;

	/* create pty and child process */
	log_debug("creating pty");
	ptym = posix_openpt(O_NOCTTY | O_RDWR);
	if (ptym < 0) {
		log_errno("posix_openpt failure");
		goto cleanup_socket;
	}
	grantpt(ptym);
	unlockpt(ptym);
	fd_ptym->fd = ptym;
	fd_ptym->type = TYPE_PTYM;
	fd_ptym->listen = 0;
	fd_ptym->session = session;

	log_debug("forking");
	session->leader = fork();
	if (session->leader < 0) {
		log_errno("fork failure");
		goto cleanup_ptym;
	}

	if (session->leader == 0) {
		int ptys = open(ptsname(ptym), O_RDWR | O_NOCTTY);
		if (ptys < 0) {
			log_errno("(child) open failure");
			_exit(1);
		}

		struct termios tos;
		tcgetattr(ptys, &tos);
		tos.c_lflag &= ~ECHO;
		tos.c_iflag |= IGNCR | INLCR;
		tos.c_oflag |= ONOCR;
		tcsetattr(ptys, TCSANOW, &tos);

		setsid();
		dup2(ptys, STDIN_FILENO);
		dup2(ptys, STDOUT_FILENO);
		dup2(ptys, STDERR_FILENO);
		for (long i = 3; i < sysconf(_SC_OPEN_MAX); i++)
			close(i);

		ioctl(STDIN_FILENO, TIOCSCTTY, 0);

		execl("/usr/bin/bash", "bash", NULL);
		_exit(1);
	}

	log_debug("adding connection fd to interest list");
	/* add to epoll interest list */
	evt_sfd.events = EPOLLIN;
	evt_sfd.data.ptr = fd_sfd;
	if (epoll_ctl(g_epoll_fd, EPOLL_CTL_ADD, connfd, &evt_sfd) < 0) {
		log_errno("epoll_ctl failure: could not add new connection fd to interest list");
		goto cleanup_ptym;
	}
	g_fds++;

	log_debug("adding pty master to interest list");
	evt_ptym.events = EPOLLIN;
	evt_ptym.data.ptr = fd_ptym;
	if (epoll_ctl(g_epoll_fd, EPOLL_CTL_ADD, ptym, &evt_ptym) < 0) {
		log_errno("epoll_ctl failure: could not add master pty fd to interest list");
		goto cleanup_epoll_conn;
	}
	g_fds++;

	return;

cleanup_epoll_conn:
	g_fds--;
	epoll_ctl(g_epoll_fd, EPOLL_CTL_DEL, connfd, NULL);
cleanup_ptym:
	close(ptym);
cleanup_socket:
	free(session);
	free(fd_sfd);
	free(fd_ptym);
	close(connfd);
}


static void
process_disconnect(struct fd *fd)
{
	/* two things happened here: we either received hangup/error from the
	   connection socket or the master pty. what we do here is to invalidate
	   the other socket (i.e. if the condition is on a socket, invalidate the
	   master pty, and the other way round) */
	if (fd->type == TYPE_SOCKET) {
		char addr_str[INET_ADDRSTRLEN];
		struct sockaddr_in addr;
		socklen_t addr_len = sizeof(addr);
		if (getpeername(fd->fd, (struct sockaddr *) &addr, &addr_len) < 0) {
			log_errno("getpeername failure on process_disconnect");
			log_info("Received hangup/error from <unknown>");
		} else {
			inet_ntop(AF_INET, &addr.sin_addr, addr_str, INET_ADDRSTRLEN);
			log_info("Received hangup/error from %.*s:%hu", INET_ADDRSTRLEN,
				addr_str, ntohs(addr.sin_port));
		}
		close(fd->session->ptym->fd);
		fd->session->ptym->type = TYPE_INVALID;
	} else if (fd->type == TYPE_PTYM) {
		log_info("Terminal hangup/error from session with leader PID %d", fd->session->leader);
		close(fd->session->sfd->fd);
		fd->session->sfd->type = TYPE_INVALID;
	} else {
		log_error("invalid fd type on process_disconnect");
		abort();
	}
	close(fd->fd);
	free(fd->session);
	free(fd);
	g_fds -= 2;
}


static int
process_telnet_commands(struct fd *fd, unsigned char *buf, int len, unsigned char *output)
{
	/* yes i know. i do technically violate the glorious telnet specification by not
	   applying properties only after the next characters. */
	int i, j, is_command, will, wont, doyes, dont;
	is_command = will = wont = doyes = dont = 0;
	for (i = 0, j = 0; i < len; i++) {
		if (is_command) {
			if (will || wont || doyes || dont) {
				/* since we do not implement any options at all,
				   just ignore them, to make think the server we do
				   everything it thinks we do */
				char buf[] = {0xff, 0, buf[i]};
				if (will) {
					log_debug("WILL %hhx", buf[i]);
				} else if (wont) {
					log_debug("WONT %hhx", buf[i]);
				} else if (doyes) {
					log_debug("DO %hhx", buf[i]);
				} else if (dont) {
					log_debug("DONT %hhx", buf[i]);
				}
				will = wont = doyes = dont = is_command = 0;
				continue;
			}

			if (buf[i] == 251) {
				will = 1;
			} else if (buf[i] == 252) {
				wont = 1;
			} else if (buf[i] == 253) {
				doyes = 1;
			} else if (buf[i] == 254) {
				dont = 1;
			} else {
				log_info("Unknown telnet command, ignoring");
			}
			continue;
		}

		if (buf[i] == 255)
			is_command = 1;
		else {
			output[j++] = buf[i];
		}
	}
	return j;
}


static void
process_connection(struct fd *fd)
{
	int ndata;
	if (ioctl(fd->fd, FIONREAD, &ndata) < 0) {
		log_errno("ioctl FIONREAD failure on process_connection");
		return;
	}

	if (ndata == 0) {
		process_disconnect(fd);	/* eof conditions are treated as read conditions. */
		return;
	}

	void *tmp = malloc(ndata), *buf = malloc(ndata);
	if (!tmp || !buf) log_oom("cannot malloc buffer on process_connection");

	if (read(fd->fd, tmp, ndata) != ndata) {
		log_errno("invalid amount of data read on process_connection");
		free(tmp);
		free(buf);
		return;
	}

	ndata = process_telnet_commands(fd, tmp, ndata, buf);

	if (write(fd->session->ptym->fd, buf, ndata) != ndata) {
		log_errno("invalid amount of data written on process_connection");
		free(tmp);
		free(buf);
		return;
	}

	free(tmp);
	free(buf);
}


static void
process_ptym(struct fd *fd)
{
	int ndata;
	if (ioctl(fd->fd, FIONREAD, &ndata) < 0) {
		log_errno("ioctl FIONREAD failure on process_ptym");
		return;
	}

	char *buf = malloc(ndata);
	if (!buf) log_oom("cannot malloc buffer on process_ptym");

	if (read(fd->fd, buf, ndata) != ndata) {
		log_errno("invalid amount of data read on process_ptym");
		free(buf);
		return;
	}

	if (write(fd->session->sfd->fd, buf, ndata) != ndata) {
		log_errno("invalid amount of data written on process_ptym");
		free(buf);
		return;
	}

	free(buf);
}


static void
main_loop(void)
{
	print_banner();

	while (1) {
		struct epoll_event *events = malloc(sizeof(*events) * g_fds);
		int ret;

		log_debug("waiting for descriptors, number of fds = %d", g_fds);
		if ((ret = epoll_wait(g_epoll_fd, events, g_fds, -1)) < 0) {
			log_errno("epoll_wait failure");
			exit(1);
		}
		log_debug("epoll_wait returned, ret = %d", ret);

		for (int i = 0; i < ret; i++) {
			struct fd *fd_data = events[i].data.ptr;
			if (events[i].events & (EPOLLHUP | EPOLLERR)) {
				process_disconnect(fd_data);
				continue;
			}

			if (fd_data->listen) {	/* listening socket */
				process_new(fd_data);
			} else if (fd_data->type == TYPE_SOCKET) { /* connection socket */
				process_connection(fd_data);
			} else if (fd_data->type == TYPE_PTYM) { /* master terminal */
				process_ptym(fd_data);
			} else if (fd_data->type == TYPE_INVALID) { /* set when counterpair disconnect, see process_disconnect */
				free(fd_data);
			}
			else {
				log_error("invalid fd type on main_loop");
				abort();
			}
		}

		free(events);
	}
}


int
main(int argc, char **argv)
{
	g_listen_addr.sin_family = AF_INET;
	g_listen_addr.sin_addr.s_addr = htonl(INADDR_ANY);
	g_listen_addr.sin_port = htons(23);

	parse_opts(argc, argv);
	setup_server();
	main_loop();
}