bt-67xx_universal_hw/modules/Telnet_Server/telnet_server.c

/*
 * telnet_server.c
 *
 *  Created on: 24.11.2017
 *      Author: balbekova
 */

#pragma GCC diagnostic error "-Wall"

/* Standard includes. */
#include <string.h>

/* lwIP core includes */
#include "lwip/opt.h"
#include "lwip/sockets.h"

/* FreeRTOS includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "fr_timers.h"

/* Utils includes. */
#include "FreeRTOS_CLI.h"
#include "CLI_Commands.h"
#include "cli.h"
#include "telnet_server.h"
#include "settings_api.h"
#include "parameters.h"
#include "control_symbol.h"
#include "log.h"
#include "web_params_api.h"


#define ISO_nl       0x0a
#define ISO_cr       0x0d

//
// States
//

typedef enum {
	TELNET_STATE_NORMAL = 0,
	TELNET_STATE_IAC,
	TELNET_STATE_OPT,
	TELNET_STATE_SB,
	TELNET_STATE_OPTDAT,
	//TELNET_STATE_SE,
	TELNET_STATE_CLOSE,
} telnet_state_t;

//
// Special telnet characters
//

#define TELNET_SE    240   // End of subnegotiation parameters
#define TELNET_NOP   241   // No operation
#define TELNET_MARK  242   // Data mark
#define TELNET_BRK   243   // Break
#define TELNET_IP    244   // Interrupt process
#define TELNET_AO    245   // Abort output
#define TELNET_AYT   246   // Are you there
#define TELNET_EC    247   // Erase character
#define TELNET_EL    248   // Erase line
#define TELNET_GA    249   // Go ahead
#define TELNET_SB    250   // Start of subnegotiation parameters
#define TELNET_WILL  251   // Will option code
#define TELNET_WONT  252   // Won't option code
#define TELNET_DO    253   // Do option code
#define TELNET_DONT  254   // Don't option code
#define TELNET_IAC   255   // Interpret as command

//
// Telnet options
//
#define TELOPT_TRANSMIT_BINARY      0  // Binary Transmission (RFC856)
#define TELOPT_ECHO                 1  // Echo (RFC857)
#define TELOPT_SUPPRESS_GO_AHEAD    3  // Suppress Go Ahead (RFC858)
#define TELOPT_STATUS               5  // Status (RFC859)
#define TELOPT_TIMING_MARK          6  // Timing Mark (RFC860)
#define TELOPT_NAOCRD              10  // Output Carriage-Return Disposition (RFC652)
#define TELOPT_NAOHTS              11  // Output Horizontal Tab Stops (RFC653)
#define TELOPT_NAOHTD              12  // Output Horizontal Tab Stop Disposition (RFC654)
#define TELOPT_NAOFFD              13  // Output Formfeed Disposition (RFC655)
#define TELOPT_NAOVTS              14  // Output Vertical Tabstops (RFC656)
#define TELOPT_NAOVTD              15  // Output Vertical Tab Disposition (RFC657)
#define TELOPT_NAOLFD              16  // Output Linefeed Disposition (RFC658)
#define TELOPT_EXTEND_ASCII        17  // Extended ASCII (RFC698)
#define TELOPT_TERMINAL_TYPE       24  // Terminal Type (RFC1091)
#define TELOPT_NAWS                31  // Negotiate About Window Size (RFC1073)
#define TELOPT_TERMINAL_SPEED      32  // Terminal Speed (RFC1079)
#define TELOPT_TOGGLE_FLOW_CONTROL 33  // Remote Flow Control (RFC1372)
#define TELOPT_LINEMODE            34  // Linemode (RFC1184)
#define TELOPT_DISPLAY_POS         35  // Linemode (RFC1184)
#define TELOPT_AUTHENTICATION      37  // Authentication (RFC1416)
#define TELOPT_NEW_ENV_OPTION      39  // Authentication (RFC1416)

//Sub option for LINEMODE

#define LINEMODE_MODE             1
#define LINEMODE_EDIT             1
#define LINEMODE_TRAPSIG          2
#define LINEMODE_MODE_ACK         4
#define LINEMODE_SOFT_TAB         8
#define LINEMODE_LIT_ECHO        16
#define LINEMODE_FORWARDMASK      2
#define LINEMODE_SLC              3
#define LINEMODE_SLC_SYNCH        1
#define LINEMODE_SLC_BRK          2
#define LINEMODE_SLC_IP           3
#define LINEMODE_SLC_AO           4
#define LINEMODE_SLC_AYT          5
#define LINEMODE_SLC_EOR          6
#define LINEMODE_SLC_ABORT        7
#define LINEMODE_SLC_EOF          8
#define LINEMODE_SLC_SUSP         9
#define LINEMODE_SLC_EC          10
#define LINEMODE_SLC_EL          11
#define LINEMODE_SLC_EW          12
#define LINEMODE_SLC_RP          13
#define LINEMODE_SLC_LNEXT       14
#define LINEMODE_SLC_XON         15
#define LINEMODE_SLC_XOFF        16
#define LINEMODE_SLC_FORW1       17
#define LINEMODE_SLC_FORW2       18
#define LINEMODE_SLC_MCL         19
#define LINEMODE_SLC_MCR         20
#define LINEMODE_SLC_MCWL        21
#define LINEMODE_SLC_MCWR        22
#define LINEMODE_SLC_MCBOL       23
#define LINEMODE_SLC_MCEOL       24
#define LINEMODE_SLC_INSRT       25
#define LINEMODE_SLC_OVER        26
#define LINEMODE_SLC_ECR         27
#define LINEMODE_SLC_EWR         28
#define LINEMODE_SLC_EBOL        29
#define LINEMODE_SLC_EEOL        30

#define LINEMODE_SLC_DEFAULT      3
#define LINEMODE_SLC_VALUE        2
#define LINEMODE_SLC_CANTCHANGE   1
#define LINEMODE_SLC_NOSUPPORT    0
#define LINEMODE_SLC_LEVELBITS    3

#define LINEMODE_SLC_ACK        128
#define LINEMODE_SLC_FLUSHIN     64
#define LINEMODE_SLC_FLUSHOUT    32
#define LINEMODE_EOF            236
#define LINEMODE_SUSP           237
#define LINEMODE_ABORT          238

/* Repeat Login timeout, 1 seconds */
#define REPEAT_SENSOR_INFO_TIME		configTICK_RATE_HZ*5*1

/* Set option to drop old connection if the new one is accepted */
#define TCP_DROP_PREV_CONNECTION    0

/**
 * A telnet connection structure.
 */
typedef struct{
	telnet_state_t state;
	uint8_t code;
	unsigned char optdata[cmdMAX_INPUT_SIZE];
	uint8_t optlen;
	uint8_t login_err;	// the number of failed password entry attempts
	uint8_t num_connect;
	bool active_conn;
	cli_state_t *cli_state;
} telnetd_state_t;

telnetd_state_t auth_tlnt_srvr_param[NUMBER_TELNET_CONNECT];

uint8_t type_term = 0;

static int32_t lSocket;
static int8_t *pcOutputString;
static struct fd_set master_set, read_set;
static int max_sd;
static struct sockaddr_in sa;

static const int8_t * const pcEndOfCommandOutputString = ( int8_t * ) "\r\n[Нажмите клавишу ENTER для повторного выполнения предыдущей команды]\r\n>";

/**
  * @brief  Общая структура настроек
  */
extern SETTINGS_t sSettings;

static void deconfigure_telnet_state(telnetd_state_t *state)
{
	if (state->cli_state) {
		free_state(state->cli_state);
		state->cli_state = NULL;
	}
	memset(state->optdata, 0, cmdMAX_INPUT_SIZE);
	state->optlen = 0;
	state->state = TELNET_STATE_NORMAL;
	state->num_connect = 0;
	state->active_conn = false;
}

static void sendopt(telnetd_state_t *s, u8_t code, u8_t option)
{
	unsigned char buf[3];
	buf[0] = TELNET_IAC;
	buf[1] = code;
	buf[2] = option;
	send( s->num_connect, buf, 3, 0 );
}

static void sendopt_cli(cli_state_t *s, u8_t code, u8_t option)
{
	unsigned char buf[3];
	buf[0] = TELNET_IAC;
	buf[1] = code;
	buf[2] = option;
	send( s->num_connect, buf, 3, 0 );
}

static void sendsubopt(telnetd_state_t *s, u8_t code, u8_t *option, u8_t len)
{
	unsigned char buf[cmdMAX_INPUT_SIZE+3];
	buf[0] = TELNET_IAC;
	buf[1] = TELNET_SB;
	buf[2] = code;
	memcpy(&buf[3], option, len);
	buf[len + 3] = TELNET_IAC;
	buf[len + 4] = TELNET_SE;
		send( s->num_connect, buf, (len + 5), 0 );
}

void parseopt(telnetd_state_t *ts, uint8_t code, uint8_t option)
{
	switch (option) {
		case TELOPT_ECHO:
			if (code == TELNET_WONT) {
				type_term = 0;
			}
			else if (code == TELNET_WILL ){
				type_term = 1;
			}
			break;
		case TELOPT_SUPPRESS_GO_AHEAD:
		case TELOPT_LINEMODE:
		case TELOPT_NAWS:
		case TELOPT_DISPLAY_POS:
		case TELOPT_NEW_ENV_OPTION:
		case TELOPT_TERMINAL_SPEED:
		case TELOPT_TERMINAL_TYPE:
		case TELOPT_STATUS:
		case TELOPT_TOGGLE_FLOW_CONTROL:
			break;

		case TELOPT_TIMING_MARK:
			xTimerStop(ts->cli_state->RepeatSensorInfoTimer, 0);
			ts->cli_state->flag_telnet_ip_option = true;
			sendopt(ts, TELNET_WILL, TELOPT_TIMING_MARK);
			sendopt(ts, TELNET_MARK, 0);
			if(ts->cli_state->input_state != CLI_CHANGE_PWD && ts->cli_state->input_state != CLI_CHANGE_PWD_ACK)
				send( ts->num_connect, pcEndOfCommandOutputString, strlen( ( const char * ) pcEndOfCommandOutputString ), 0 );
			break;

		default:
			if (code == TELNET_WILL || code == TELNET_WONT) {
				sendopt(ts, TELNET_DONT, option);
			} else {
				sendopt(ts, TELNET_WONT, option);
			}
			break;
	}
}

static void parseoptdat(telnetd_state_t *ts, int option, unsigned char *data, uint8_t len) {

	uint8_t subopt = 0;
	uint8_t subopt_val = 0;
	uint8_t sub_opt_data[2] = {LINEMODE_MODE, LINEMODE_EDIT};

	switch (option) {
		case TELOPT_NAWS:
			break;

		case TELOPT_TERMINAL_SPEED:
			break;

		case TELOPT_TERMINAL_TYPE:
			break;
		case TELOPT_LINEMODE:
			subopt = data[0];
			switch (subopt) {
			case LINEMODE_SLC:
			if(data[2] & LINEMODE_SLC_ACK){
				return;
			}
			sendsubopt(ts, option, sub_opt_data, 2);
			for(uint8_t i = 1; i < len; i += 3){
				subopt_val = data[i];
				switch (subopt_val) {
				case LINEMODE_SLC_IP:
					data[i+1] = LINEMODE_SLC_VALUE | LINEMODE_SLC_FLUSHIN | LINEMODE_SLC_FLUSHOUT | LINEMODE_SLC_ACK;
					break;
				case LINEMODE_SLC_XON:
				case LINEMODE_SLC_XOFF:
				case LINEMODE_SLC_EC:
					data[i+1] = LINEMODE_SLC_VALUE | LINEMODE_SLC_ACK;
					break;
				default:
					data[i+1] = LINEMODE_SLC_NOSUPPORT;
					data[i+2] = 0;
					break;
				}
			}
			sendsubopt(ts, option, data, len);
			break;
			}
		break;
	}
}

extern int8_t cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];

portBASE_TYPE FreeRTOS_CLIAuthProcess( int8_t * pcWriteBuffer, cli_state_t *s)
{
	portBASE_TYPE xReturn = pdTRUE;
	uint32_t len;
	user_level_t user_id = MAX_USER_LEVELS;
	char password[cmdMAX_INPUT_SIZE] = { 0 };
	const int8_t * const pcPSWHeader = ( int8_t * ) "\r\npassword:";
	const int8_t * const pcLoginHeader = ( int8_t * ) "\r\nlogin:";

	printf("CAP buf: %s\r\n", s->buf);
	memset(pcWriteBuffer, 0, configCOMMAND_INT_MAX_OUTPUT_SIZE);

	switch(s->input_state){
	case CLI_AUTH:
		memset(s->login, 0, MAX_WEB_LOGIN_LEN);
		len = strlen(s->buf);
		if (len < MAX_WEB_LOGIN_LEN){
			strncpy(s->login, s->buf, len);
			sendopt_cli(s, TELNET_WILL, TELOPT_ECHO);
			strncpy( ( char * ) pcWriteBuffer, ( const char * ) pcPSWHeader, strlen( ( char * ) pcPSWHeader ) );
			s->input_state = CLI_AUTH_PASSW;
			xReturn = pdTRUE;
		} else {
			xReturn = pdFALSE;
		}
		break;
	case CLI_AUTH_PASSW:
		sendopt_cli(s, TELNET_WONT, TELOPT_ECHO);
		memset(password, 0, MAX_WEB_PASSWD_LEN);
		len = strlen(s->buf);
		strncpy(password, s->buf, len);
		user_id = cli_auth_user((const char *)s->login, password, LOG_LOGIN_TELNET);
		if (user_id != MAX_USER_LEVELS) {
			s->user_id = user_id;
			s->login_err = 0;
			strcpy( ( char * ) pcWriteBuffer, "\r\nАвторизация успешно пройдена\r\n>" );
			s->input_state = CLI_CMD;
			xReturn = pdTRUE;
		} else {
			xReturn = pdFALSE;
		}
		break;
	default:
		// called auth when already authorized
		xReturn = pdFALSE;
		s->input_state = CLI_AUTH;
		return xReturn;
		break;
	}

	if (xReturn == pdFALSE){
		s->input_state = CLI_AUTH;
		if (s->login_err < 4){
			s->login_err ++;
			strcpy( ( char * ) pcWriteBuffer, "\r\nОшибка авторизации\r\n" );
			strncat( ( char * ) pcWriteBuffer, ( const char * ) pcLoginHeader, strlen( ( char * ) pcLoginHeader ) );
			xReturn = pdTRUE;
		} else {
			// password entry tries exceeded
			s->login_err = 0;
			xReturn = pdFALSE;
		}
		printf("login error, errors so far: %d\r\n", s->login_err);
	}

	return xReturn;
}

/*-----------------------------------------------------------------------------------*/
static void newdata(telnetd_state_t *s, char c)
{
	switch(s->state) {
	case TELNET_STATE_IAC:
		switch (c) {
		case TELNET_IAC:
			s->state = TELNET_STATE_NORMAL;
			break;
		case TELNET_WILL:
		case TELNET_WONT:
		case TELNET_DO:
		case TELNET_DONT:
			s->code = c;
			s->state = TELNET_STATE_OPT;
			break;
		case TELNET_SB:
			s->state = TELNET_STATE_SB;
			break;
		default:
			s->state = TELNET_STATE_NORMAL;
			break;
		}
		break;
	case TELNET_STATE_OPT:
		parseopt(s, s->code, c);
		s->state = TELNET_STATE_NORMAL;
		break;

	case TELNET_STATE_SB:
		s->code = c;
		s->optlen = 0;
		s->state = TELNET_STATE_OPTDAT;
		break;

	case TELNET_STATE_OPTDAT:
		if (c == TELNET_SE && s->optdata[s->optlen-1] == TELNET_IAC) {
			parseoptdat(s, s->code, s->optdata, (s->optlen-1));
			s->state = TELNET_STATE_NORMAL;
			//s->state = TELNET_STATE_SE;
		} else if (s->optlen < sizeof(s->optdata)) {
			s->optdata[s->optlen++] = c;
		}
		break;

	/*case TELNET_STATE_SE:
		if (c == TELNET_SE) parseoptdat(s, s->code, s->optdata, s->optlen);
		s->state = TELNET_STATE_NORMAL;
		break;*/
	case TELNET_STATE_NORMAL:
		if(c == TELNET_IAC) {
			s->state = TELNET_STATE_IAC;
		} else {
			// incoming user input
			cli_getchar(s->cli_state, c, type_term == 1);
		}
		break;
	default:
		printf("unexpected telnet state\r\n");
	}
}

/*-----------------------------------------------------------*/

/* Stop server */
static void stop_server(void) {
	/* Clean up all of the sockets that are open */
	for (int i = 0; i <= max_sd; ++i)
	{
		 if (FD_ISSET(i, &master_set)) {
				DBG printf("Close sock %d\n", i);
				closesocket(i);
				FD_CLR(i, &master_set);
		 }
	}
	DBG printf("Portgw stopped\n");

	for (int i = 0; i < array_len(auth_tlnt_srvr_param); ++i) {
		deconfigure_telnet_state(auth_tlnt_srvr_param + i);
	}
}

/* Start server */
static bool start_server(uint16_t port)
{
	int res;

	lSocket = socket(PF_INET, SOCK_STREAM, 0);

	if (lSocket < 0) {
		DBG printf("Socket create failed\r\n");
		return false;
	}

	res = fcntl(lSocket, F_SETFL, O_NONBLOCK);
	if (res < 0) {
			DBG printf("fcntl() failed");
			closesocket(lSocket);
			return false;
	}

	memset(&sa, 0, sizeof(struct sockaddr_in));
	sa.sin_family = AF_INET;
	sa.sin_addr.s_addr = IPADDR_ANY;
	sa.sin_port = htons(port);

	if (bind(lSocket, (struct sockaddr *)&sa, sizeof(sa)) == -1)
	{
			DBG printf("Bind to port %d failed\n", port);
			closesocket(lSocket);
			return false;
	}

	res = listen(lSocket, 20);
	if (res < 0) {
			DBG printf("Listen failed failed\r\n");
			closesocket(lSocket);
			return false;
	}

	FD_ZERO(&master_set);
	max_sd = lSocket;
	FD_SET(lSocket, &master_set);

	DBG printf("Port %d opened\n", port);

	return true;
}

static void cli_send(intptr_t fd, const char *str, unsigned len)
{
	send(fd, str, len, 0);
}

void vBasicSocketsCommandInterpreterTask( void *pvParameters )
{
	int32_t lBytes;
	int new_sd;
	int desc_ready, rc;
	struct timeval timeout;
	uint16_t port;
	bool enabled;
	bool firstrun = true;
#if HARDWARE_BT6709 || HARDWARE_BT6709_MTS 
	struct sockaddr_in sa_temp;
	socklen_t len;
#endif
#if TCP_DROP_PREV_CONNECTION
	static int active_sd = -1;
#endif
	static uint8_t cnt_conn = 0;
	uint8_t cur_cnt = 0;

	FD_ZERO(&master_set);

	timeout.tv_sec = 5;
	timeout.tv_usec = 0;

	( void ) pvParameters;
	pcOutputString = FreeRTOS_CLIGetOutputBuffer();
	enabled = sSettings.sTelnet.TelnetEnable;
	port = ( uint16_t ) sSettings.sTelnet.port;

	while (1) {
		/* Check if network settings was changed */
		if ((sSettings.sTelnet.port != port) ||
				(sSettings.sTelnet.TelnetEnable != enabled) ||
				(firstrun))
		{

			if (!firstrun || sSettings.sTelnet.port != port) {
				/* Stop server */
				stop_server();
				cnt_conn = 0;
			}

			if (sSettings.sTelnet.TelnetEnable) {
				/* (Re)start server */
				if (!start_server(sSettings.sTelnet.port)) {
					DBG printf("Server start error\n");
					firstrun = true;
					vTaskDelay(5000);
					continue;
				}
			}
			else {
				/* Obtain the address of the output buffer.  Note there is no mutual
				exclusion on this buffer as it is assumed only one command console
				interface will be used at any one time. */
				firstrun = true;
				vTaskDelay(5000);
				continue;
			}
			firstrun = false;

			port = sSettings.sTelnet.port;
			enabled = sSettings.sTelnet.TelnetEnable;
		}

		if (!enabled) {
			firstrun = true;
			vTaskDelay(5000);
			continue;
		}



		memcpy(&read_set, &master_set, sizeof(master_set));

		DBG printf("Waiting on select()...\n");
		rc = select(max_sd + 1, &read_set, NULL, NULL, &timeout);

		DBG printf("  select() returned %d\n", rc);

		if (rc < 0) {
			DBG printf("  select() failed\n");
		}

		if (rc == 0) {
			DBG printf("  select() timed out.\n");
		}

		/* One or more descriptors are readable.  Need to         \
		 * determine which ones they are.                         */
		desc_ready = rc;

		for (int sock = 0; sock <= max_sd && desc_ready > 0; ++sock) {
			/*******************************************************/
			/* Check to see if this descriptor is ready            */
			/*******************************************************/
			if (FD_ISSET(sock, &read_set)) {
				/* A descriptor was found that was readable - one  \
				* less has to be looked for.  This is being done   \
				* so that we can stop looking at the working set   \
				* once we have found all of the descriptors that   \
				* were ready.                                      */
				desc_ready -= 1;

				/* Check to see if this is the listening socket     */
				if (sock == lSocket) {
					DBG printf("  Listening socket is readable\n");

					/* Accept all incoming connections that are      */
					/* queued up on the listening socket before we   */
					/* loop back and call select again.              */
					do {

						/* Accept each incoming connection.  If       */
						/* accept fails with EWOULDBLOCK, then we     */
						/* have accepted all of them.  Any other      */
						/* failure on accept will cause us to end the */
						/* server.                                    */
						new_sd = accept(lSocket, NULL, NULL);
						if (new_sd < 0) {
							if (errno != EWOULDBLOCK) {
								DBG printf("  accept() failed\n");
							}
							break;
						}

						/* Add the new incoming connection to the     */
						/* master read set                            */
						printf("  New incoming connection - %d\n", new_sd);
						FD_SET(new_sd, &master_set);
						if (new_sd > max_sd) {
							max_sd = new_sd;
						}
#if HARDWARE_BT6709 || HARDWARE_BT6709_MTS 
						lwip_getpeername(new_sd, &sa_temp, &len);
#endif
					//	recv( new_sd, &auth_tlnt_srvr_param[cur_cnt].buf, 27, 0 );
						// Send initial options
						bool not_enough_memory = false;
						if(cnt_conn < NUMBER_TELNET_CONNECT){
							cli_state_t *cli_state = 0;
							for(uint8_t k = 0; k < NUMBER_TELNET_CONNECT; k ++){
								if(auth_tlnt_srvr_param[k].active_conn == false && (cli_state = alloc_state())){
									auth_tlnt_srvr_param[k].active_conn = true;
									auth_tlnt_srvr_param[k].num_connect = new_sd;
									auth_tlnt_srvr_param[k].state = TELNET_STATE_NORMAL;
									cli_state->num_connect = new_sd;
									cli_state->send = cli_send;
									cli_state->state = STATE_NORMAL;
									auth_tlnt_srvr_param[k].cli_state = cli_state;

									char c;
									while (recv(new_sd, &c, 1, MSG_DONTWAIT) > 0) {
										newdata(&auth_tlnt_srvr_param[k], c);
										vTaskDelay(10);
									}
									sendopt(&auth_tlnt_srvr_param[k], TELNET_DO, TELOPT_SUPPRESS_GO_AHEAD);
									sendopt(&auth_tlnt_srvr_param[k], TELNET_DO, TELOPT_LINEMODE);
									sendopt(&auth_tlnt_srvr_param[k], TELNET_DONT, TELOPT_ECHO);
									vTaskDelay(50);
									while (recv(new_sd, &c, 1, MSG_DONTWAIT) > 0) {
										newdata(&auth_tlnt_srvr_param[k], c);
										vTaskDelay(5);
									}
									send( new_sd, TELNET_CLI_WELCOME_MESSAGE, strlen( TELNET_CLI_WELCOME_MESSAGE ), 0 );
									break;
								}
							}
							if (!cli_state) {
								not_enough_memory = true;
							} else {
								cnt_conn++;
							}
						} else {
							not_enough_memory = true;
						}
						if (not_enough_memory) {
							send(new_sd, pcWarningMessage, pcWarningMessageLen, 0);
							closesocket(new_sd);
							FD_CLR(new_sd, &master_set);
							while (FD_ISSET(max_sd, &master_set) == false) {
								max_sd -= 1;
							}
						}

#if TCP_DROP_PREV_CONNECTION
						/* Close previous active connection */
						if (active_sd != -1 && active_sd != new_sd) {
								DBG printf("	Close prev active connection %d\n", active_sd);
								close(active_sd);
								FD_CLR(active_sd, &master_set);
								if (active_sd == max_sd) {
										while (FD_ISSET(max_sd, &master_set) == false) {
												max_sd -= 1;
										}
								}
						}
						/* Mark new connection as active */
						active_sd = new_sd;
						DBG printf("	New active connection %d\n", active_sd);
#endif

						/* Loop back up and accept another incoming   */
						/* connection                                 */
					} while (new_sd != -1);
				}
				/* This is not the listening socket, therefore an   */
				/* existing connection must be readable             */
				else {
					DBG printf("  Descriptor %d is readable\n", sock);

					for(cur_cnt = 0; cur_cnt < NUMBER_TELNET_CONNECT; cur_cnt ++){
						if(auth_tlnt_srvr_param[cur_cnt].num_connect == sock){
							break;
						}
					}

					/* Receive data on this connection until the  */
					/* recv fails with EWOULDBLOCK.  If any other */
					/* failure occurs, we will close the          */
					/* connection.                                */
					char c;
					if ((lBytes = recv(sock, &c, 1, 0)) > 0) {
						newdata(&auth_tlnt_srvr_param[cur_cnt], c);
					}

					if (lBytes < 0) {
						if (errno != EWOULDBLOCK){
							DBG printf("  recv() failed\n");
							auth_tlnt_srvr_param[cur_cnt].state = TELNET_STATE_CLOSE;
						}
					}

					/* Check to see if the connection has been    */
					/* closed by the client                       */
					if (lBytes == 0) {
						DBG printf("  Connection closed\n");
						auth_tlnt_srvr_param[cur_cnt].state = TELNET_STATE_CLOSE;
					}

					/* If the close_conn flag was turned on, we need */
					/* to clean up this active connection.  This     */
					/* clean up process includes removing the        */
					/* descriptor from the master set and            */
					/* determining the new maximum descriptor value  */
					/* based on the bits that are still turned on in */
					/* the master set.                               */
					if (auth_tlnt_srvr_param[cur_cnt].state == TELNET_STATE_CLOSE || auth_tlnt_srvr_param[cur_cnt].cli_state->state == STATE_CLOSE) {
						deconfigure_telnet_state(auth_tlnt_srvr_param + cur_cnt);
						cnt_conn -= 1;
						closesocket(sock);
						FD_CLR(sock, &master_set);
						if (sock == max_sd) {
							while (FD_ISSET(max_sd, &master_set) == false) {
								max_sd -= 1;
							}
						}
					}
				}
			}
		}
	}
}

void telnet_server_init(void) {
	xTaskCreate(vBasicSocketsCommandInterpreterTask, ( char * ) "vBasicSocketsCommandInterpreterTask", 8*configMINIMAL_STACK_SIZE , NULL, tskIDLE_PRIORITY + 1, NULL);
}