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

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

/* 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 "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
//

#define STATE_NORMAL 0
#define STATE_IAC    1
#define STATE_OPT    2
#define STATE_SB     3
#define STATE_OPTDAT 4
#define STATE_SE     5
#define STATE_CLOSE	 6

//
// 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{
	TimerHandle_t RepeatSensorInfoTimer;
	uint8_t state;
	uint8_t code;
	char buf[cmdMAX_INPUT_SIZE];
	char bufptr;
	char prev_cmd[cmdMAX_INPUT_SIZE];
	unsigned char optdata[cmdMAX_INPUT_SIZE];
	uint8_t optlen;
	state_telnet_server_t telnetState;
	user_level_t telnet_code_auth;
	char login[MAX_WEB_LOGIN_LEN];
	uint8_t login_err;
	uint8_t num_connect;
	bool active_conn;
	bool flag_telnet_ip_option;
}telnetd_state_t;

static portBASE_TYPE FreeRTOS_CLIAuthProcess( int8_t * pcWriteBuffer, telnetd_state_t *s  );
static portBASE_TYPE FreeRTOS_ChangePWDProcess( int8_t * pcWriteBuffer, telnetd_state_t *s  );
void SensorInfoTimerCallback(TimerHandle_t pxTimer);

state_telnet_server_t telnetState = TELNET_AUTH;
uint8_t id_change_pwd = 0;
user_level_t telnet_code_auth = USER;

telnetd_state_t auth_tlnt_srvr_param[NUMBER_TELNET_CONNECT];

uint8_t type_term = 0;

static int32_t lSocket;
static int8_t *pcOutputString;
static bool flagTelnetChange = false;
static struct fd_set master_set, read_set, write_set;
static int max_sd;
static struct sockaddr_in sa;

const int8_t * const pcWarningMessage = ( const int8_t * ) "Количество соединенений превышено. Данное соединение будет закрыто\r\n";
#ifdef HARDWARE_BT6708
const int8_t * const pcWelcomeMessage = ( const int8_t * ) "BT6708 command server - connection accepted.\r\nlogin:";
#else
const int8_t * const pcWelcomeMessage = ( const int8_t * ) "BT6706 command server - connection accepted.\r\nlogin:";
#endif
static const int8_t * const pcEndOfCommandOutputString = ( int8_t * ) "\r\n[Нажмите клавишу ENTER для повторного выполнения предыдущей команды]\r\n>";

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

/*-----------------------------------------------------------------------------------*/
/**
 * Close a telnet session.
 *
 * This function can be called from a telnet command in order to close
 * the connection.
 *
 * \param s The connection which is to be closed.
 *
 */
/*-----------------------------------------------------------------------------------*/
void
telnetd_close(telnetd_state_t *s)
{
  s->state = STATE_CLOSE;
}

static void telnetd_input(telnetd_state_t *s)
{
	portBASE_TYPE xReturned;

	switch(s->telnetState){
		case TELNET_AUTH:
		case TELNET_AUTH_PASSW:
			if(FreeRTOS_CLIAuthProcess(pcOutputString, s)){
				send( s->num_connect, pcOutputString, strlen( ( const char * ) pcOutputString ), 0 );
				memset( s->buf, 0x00, cmdMAX_INPUT_SIZE );
			}
			else{
				s->state = STATE_CLOSE;
			}
			break;
		case TELNET_CMD:
			/* The input string has been terminated.  Was the
			input a quit command? */
			if( strcmp( "quit", ( const char * ) s->buf ) == 0 )
			{
				s->state = STATE_CLOSE;
			}
			else
			{
				/* The input string was not a quit command.
				Pass the string to the command interpreter. */

				/* See if the command is empty, indicating that the last command is
				to be executed again. */
				if( s->bufptr == 0 )
				{
					if(s->flag_telnet_ip_option ){
						s->flag_telnet_ip_option = false;
						return;
					}
					else{
						strcpy( s->buf, s->prev_cmd );
					}
				}

				/* Transmit a line separator, just to make the
				output easier to read. */
				lwip_send( s->num_connect, "\r\n", strlen( "\r\n" ), 0 );

				do
				{
					/* Ensure there is not a string lingering in
					the output buffer. */
					pcOutputString[ 0 ] = 0x00;
					telnetState = s->telnetState;
					xReturned = FreeRTOS_CLIProcessCommand( s->buf, pcOutputString, configCOMMAND_INT_MAX_OUTPUT_SIZE );
					s->telnetState = telnetState;
					send( s->num_connect, pcOutputString, strlen( ( const char * ) pcOutputString ), 0 );

				} while( xReturned != pdFALSE );

				if( strcmp( "sensor info", ( const char * ) s->buf ) == 0 ){
					strcpy( s->prev_cmd, s->buf );
					memset( s->buf, 0x00, cmdMAX_INPUT_SIZE );
					xTimerStart(s->RepeatSensorInfoTimer, 0);
				}
				else{

					/* All the strings generated by the input
					command have been sent.  Clear the input
					string ready to receive the next command.
					Remember the command that was just processed
					first in case it is to be processed again. */
					strcpy( s->prev_cmd, s->buf );
					memset( s->buf, 0x00, cmdMAX_INPUT_SIZE );
					if(s->telnetState != TELNET_CHANGE_PWD)
						send( s->num_connect, pcEndOfCommandOutputString, strlen( ( const char * ) pcEndOfCommandOutputString ), 0 );
				}
			}
			break;
		case TELNET_CHANGE_PWD:
		case TELNET_CHANGE_PWD_ACK:
			FreeRTOS_ChangePWDProcess(pcOutputString, s);
			send( s->num_connect, pcOutputString, strlen( ( const char * ) pcOutputString ), 0 );
			memset( s->buf, 0x00, cmdMAX_INPUT_SIZE );
			break;

	}
}

/*-----------------------------------------------------------------------------------*/
static void getchar(telnetd_state_t *s)
{
	if(type_term == 1 && s->telnetState != TELNET_AUTH_PASSW){
		if(s->buf[s->bufptr] != 0x03 && s->buf[s->bufptr] != 0x7f){
			send( s->num_connect, &s->buf[s->bufptr], 1, 0 );
		}
	}
	else if(s->telnetState == TELNET_AUTH_PASSW){
		send( s->num_connect, " ", 1, 0 );
	}


  if(s->buf[s->bufptr] == ISO_nl ||
	s->buf[s->bufptr] == 0) {
	s->bufptr = 0;
    return;
  }

  if(s->buf[s->bufptr] == ISO_cr ||
     s->bufptr == sizeof(s->buf) - 1) {
    if(s->bufptr > 0) {
      s->buf[s->bufptr] = 0;
    }
    telnetd_input(s);
    s->bufptr = 0;
  } else if( s->buf[s->bufptr] == '\b' || s->buf[s->bufptr] == 0x7f)
	{
		/* Backspace was pressed.  Erase the last
		character in the string - if any. */
	  	s->buf[s->bufptr] = '\0';
		if( s->bufptr > 0 )
		{
			s->bufptr--;
			s->buf[s->bufptr] = '\0';
		}
	}
  else if (s->buf[s->bufptr] == 0x03){
	  xTimerStop(s->RepeatSensorInfoTimer, 0);
	  s->flag_telnet_ip_option = true;
	  if(s->telnetState != TELNET_CHANGE_PWD && s->telnetState != TELNET_CHANGE_PWD_ACK)
	  	send( s->num_connect, pcEndOfCommandOutputString, strlen( ( const char * ) pcEndOfCommandOutputString ), 0 );
  }
  else {
    ++s->bufptr;
  }
}
/*-----------------------------------------------------------------------------------*/
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 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->RepeatSensorInfoTimer, 0);
    	ts->flag_telnet_ip_option = true;
		sendopt(ts, TELNET_WILL, TELOPT_TIMING_MARK);
		sendopt(ts, TELNET_MARK, 0);
		if(ts->telnetState != TELNET_CHANGE_PWD && ts->telnetState != TELNET_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;
  }
}

/*-----------------------------------------------------------------------------------*/
static void newdata(telnetd_state_t *s)
{
	char c;

    c = s->buf[s->bufptr];
    switch(s->state) {
    case STATE_IAC:
    	switch (c) {
		  case TELNET_IAC:
			s->state = STATE_NORMAL;
			break;

		  case TELNET_WILL:
		  case TELNET_WONT:
		  case TELNET_DO:
		  case TELNET_DONT:
			s->code = c;
			s->state = STATE_OPT;
			break;

		  case TELNET_SB:
			s->state = STATE_SB;
			break;

		  default:
			s->state = STATE_NORMAL;
			break;
		}
      break;
	case STATE_OPT:
		parseopt(s, s->code, c);
		s->state = STATE_NORMAL;
		break;

	case STATE_SB:
		s->code = c;
		s->optlen = 0;
		s->state = STATE_OPTDAT;
		break;

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

	/*case STATE_SE:
		if (c == TELNET_SE) parseoptdat(s, s->code, s->optdata, s->optlen);
		s->state = STATE_NORMAL;
		break;*/
    case STATE_NORMAL:
      if(c == TELNET_IAC) {
    	  s->state = STATE_IAC;
      } else {
    	  getchar(s);
      }
      break;
    }

}

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

/* 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");
}

/* 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;
}


void vBasicSocketsCommandInterpreterTask( void *pvParameters )
{
	int32_t lBytes, lAddrLen = sizeof( struct sockaddr_in );
	struct sockaddr_in sLocalAddr;
	struct sockaddr_in client_addr;
	int new_sd;
    int desc_ready, rc;
    struct timeval timeout;
    uint16_t port;
    bool enabled;
    bool firstrun = true;
#ifdef HARDWARE_BT6708
    struct sockaddr_in sa_temp;
    socklen_t len;
#endif
    static int active_sd = -1;
    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();
			}

			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 i=0; i <= max_sd  &&  desc_ready > 0; ++i) {
			/*******************************************************/
			/* Check to see if this descriptor is ready            */
			/*******************************************************/
			if (FD_ISSET(i, &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 (i == 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;
						}
#ifdef HARDWARE_BT6708
						lwip_getpeername(new_sd, &sa_temp, &len);
#endif
					//	recv( new_sd, &auth_tlnt_srvr_param[cur_cnt].buf, 27, 0 );
						// Send initial options
						if(cnt_conn < NUMBER_TELNET_CONNECT){
							for(uint8_t k = 0; k < NUMBER_TELNET_CONNECT; k ++){
								if(auth_tlnt_srvr_param[k].active_conn == false){
									auth_tlnt_srvr_param[k].active_conn = true;
									auth_tlnt_srvr_param[k].num_connect = new_sd;
									auth_tlnt_srvr_param[k].telnetState = TELNET_AUTH;
									auth_tlnt_srvr_param[k].state = STATE_NORMAL;

									while(recv( new_sd, &auth_tlnt_srvr_param[k].buf[auth_tlnt_srvr_param[k].bufptr], 1,  MSG_DONTWAIT ) > 0){
										newdata(&auth_tlnt_srvr_param[k]);
										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_DO, TELOPT_ECHO);
									vTaskDelay(50);
									while(recv( new_sd, &auth_tlnt_srvr_param[k].buf[auth_tlnt_srvr_param[k].bufptr], 1,  MSG_DONTWAIT ) > 0){
										newdata(&auth_tlnt_srvr_param[k]);
										vTaskDelay(5);
									}
									send( new_sd, pcWelcomeMessage, strlen( ( const char * ) pcWelcomeMessage ), 0 );
									break;
								}
							}
							cnt_conn ++;
						}
						else{
							send( new_sd, pcWarningMessage, strlen( ( const char * ) pcWarningMessage ), 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", i);

					for(cur_cnt = 0; cur_cnt < NUMBER_TELNET_CONNECT; cur_cnt ++){
						if(auth_tlnt_srvr_param[cur_cnt].num_connect == i){
							telnetState = auth_tlnt_srvr_param[cur_cnt].telnetState;
							telnet_code_auth = auth_tlnt_srvr_param[cur_cnt].telnet_code_auth;
							switch (telnet_code_auth) {
							  case ADMIN:
								  snprintf(name_login_telnet, sizeof(name_login_telnet), "Администратор");
								  break;
							  case USER:
								  snprintf(name_login_telnet, sizeof(name_login_telnet), "Пользователь");
								  break;
							  default:
								  break;
							}
							break;
						}
					}

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

							newdata(&auth_tlnt_srvr_param[cur_cnt]);
					}

					if (lBytes < 0) {
						if (errno != EWOULDBLOCK){
							DBG printf("  recv() failed\n");
							auth_tlnt_srvr_param[cur_cnt].state = 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 = 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 == STATE_CLOSE) {
						memset(auth_tlnt_srvr_param[cur_cnt].buf, 0, cmdMAX_INPUT_SIZE);
						memset(auth_tlnt_srvr_param[cur_cnt].prev_cmd, 0, cmdMAX_INPUT_SIZE);
						memset(auth_tlnt_srvr_param[cur_cnt].optdata, 0, cmdMAX_INPUT_SIZE);
						auth_tlnt_srvr_param[cur_cnt].optlen = 0;
						auth_tlnt_srvr_param[cur_cnt].state = STATE_NORMAL;
						auth_tlnt_srvr_param[cur_cnt].num_connect = 0;
						auth_tlnt_srvr_param[cur_cnt].active_conn = false;
						auth_tlnt_srvr_param[cur_cnt].telnetState = TELNET_AUTH;
						auth_tlnt_srvr_param[cur_cnt].telnet_code_auth = USER;
						auth_tlnt_srvr_param[cur_cnt].flag_telnet_ip_option = false;
						xTimerStop(auth_tlnt_srvr_param[cur_cnt].RepeatSensorInfoTimer, 0);
						cnt_conn -= 1;
						closesocket(i);
						FD_CLR(i, &master_set);
						if (i == max_sd) {
						   while (FD_ISSET(max_sd, &master_set) == false) {
							  max_sd -= 1;
						   }
						}
					}
				}
			}
		}
    }
}

void telnet_server_init(void) {
	for(uint8_t i = 0; i < NUMBER_TELNET_CONNECT; i++){
		memset(auth_tlnt_srvr_param[i].buf, 0, cmdMAX_INPUT_SIZE);
		memset(auth_tlnt_srvr_param[i].prev_cmd, 0, cmdMAX_INPUT_SIZE);
		memset(auth_tlnt_srvr_param[i].optdata, 0, cmdMAX_INPUT_SIZE);
		auth_tlnt_srvr_param[i].optlen = 0;
		auth_tlnt_srvr_param[i].num_connect = 0;
		auth_tlnt_srvr_param[i].active_conn = false;
		auth_tlnt_srvr_param[i].telnetState = TELNET_AUTH;
		auth_tlnt_srvr_param[i].telnet_code_auth = USER;
		auth_tlnt_srvr_param[i].flag_telnet_ip_option = false;
		auth_tlnt_srvr_param[i].RepeatSensorInfoTimer = xTimerCreate("SensorInfoTmr", REPEAT_SENSOR_INFO_TIME, pdFALSE, ( void * ) i, SensorInfoTimerCallback);
	}
	vRegisterCLICommands();
	xTaskCreate(vBasicSocketsCommandInterpreterTask, ( char * ) "vBasicSocketsCommandInterpreterTask", 8*configMINIMAL_STACK_SIZE , NULL, tskIDLE_PRIORITY + 1, NULL);
}

void SensorInfoTimerCallback(TimerHandle_t pxTimer) {
	portBASE_TYPE xReturned = pdTRUE;
	uint8_t num_timer;
	for(uint8_t i = 0; i < NUMBER_TELNET_CONNECT; i ++){
		if(pxTimer == auth_tlnt_srvr_param[i].RepeatSensorInfoTimer){
			num_timer = i;
			break;
		}
	}
	do
	{
		/* Ensure there is not a string lingering in
		the output buffer. */
		pcOutputString[ 0 ] = 0x00;
		xReturned = FreeRTOS_CLIProcessCommand( "sensor info", pcOutputString, configCOMMAND_INT_MAX_OUTPUT_SIZE );
		send( auth_tlnt_srvr_param[num_timer].num_connect, pcOutputString, strlen( ( const char * ) pcOutputString ), 0 );

	} while( xReturned != pdFALSE );
	xTimerStart(auth_tlnt_srvr_param[num_timer].RepeatSensorInfoTimer, 0);
}

static portBASE_TYPE FreeRTOS_CLIAuthProcess( int8_t * pcWriteBuffer, telnetd_state_t *s  )
{
	portBASE_TYPE xReturn = pdTRUE;
	uint32_t len;
	uint8_t valueLen, user_id;
	char WebPassword[MAX_WEB_PASSWD_LEN];
	char WebLogin[MAX_WEB_LOGIN_LEN];
	char password[cmdMAX_INPUT_SIZE] = { 0 };
	const int8_t * const pcPSWHeader = ( int8_t * ) "\r\npassword:";
	const int8_t * const pcLoginHeader = ( int8_t * ) "\r\nlogin:";

	memset(pcWriteBuffer, 0, configCOMMAND_INT_MAX_OUTPUT_SIZE);

	switch(s->telnetState){
	case TELNET_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(s, TELNET_WILL, TELOPT_ECHO);
			strncpy( ( char * ) pcWriteBuffer, ( const char * ) pcPSWHeader, strlen( ( char * ) pcPSWHeader ) );
			s->telnetState = TELNET_AUTH_PASSW;
			xReturn = pdTRUE;
		}
		else{
			xReturn = pdFALSE;
		}
		break;
	case TELNET_AUTH_PASSW:
		sendopt(s, TELNET_WONT, TELOPT_ECHO);
		memset(name_login_telnet, 0, 50);
		memset(password, 0, MAX_WEB_PASSWD_LEN);
		len = strlen(s->buf);
		strncpy(password, s->buf, len);
		for (user_id = 0; user_id < MAX_WEB_USERS; user_id++) {

			GetUserLogin(user_id, WebLogin, &valueLen);
			GetUserPassword(user_id, WebPassword, &valueLen);

			/* Check login and password */
			if ((strncmp(WebLogin, s->login, MAX_WEB_LOGIN_LEN) == 0) &&
				(strncmp(WebPassword, password, MAX_WEB_PASSWD_LEN) == 0)) {

				/* Login and pass are valid */
				s->telnet_code_auth = user_id;
				s->login_err = 0;
				strcpy( ( char * ) pcWriteBuffer, "\r\nАвторизация успешно пройдена\r\n>" );
				s->telnetState = TELNET_CMD;
				switch (user_id) {
				  case 0:
					  snprintf(name_login_telnet, sizeof(name_login_telnet), "Администратор");
					  break;
				  case 1:
					  snprintf(name_login_telnet, sizeof(name_login_telnet), "Пользователь");
					  break;
				  default:
					  break;
				}

				log_event_data(LOG_LOGIN_TELNET, name_login_telnet);
				xReturn = pdTRUE;
				break;
			}
			else{
				xReturn = pdFALSE;
			}
		}
		break;
	default:
		xReturn = pdFALSE;
		s->telnetState = TELNET_AUTH;
		return xReturn;
		break;
	}

	if(xReturn == pdFALSE){
		s->telnetState = TELNET_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{
			s->login_err = 0;
			xReturn = pdFALSE;
		}
	}

	return xReturn;
}

static portBASE_TYPE FreeRTOS_ChangePWDProcess( int8_t * pcWriteBuffer, telnetd_state_t *s  )
{
	portBASE_TYPE xReturn = pdTRUE;
	uint32_t len;
	static char password[ MAX_WEB_PASSWD_LEN ] = { 0 };
	char password2[ MAX_WEB_PASSWD_LEN ] = { 0 };
	const int8_t * const pcNewPSWHeader = ( int8_t * ) "\r\nВведите повторно новый пароль:";

	memset(pcWriteBuffer, 0, configCOMMAND_INT_MAX_OUTPUT_SIZE);

	len = strlen(s->buf);
	if(s->telnetState == TELNET_CHANGE_PWD){
		memset(password, 0, MAX_WEB_PASSWD_LEN);
		if(len >= MAX_WEB_PASSWD_LEN){
			strcpy( ( char * ) pcWriteBuffer, "\r\nОшибка при вводе нового пароля\r\n>" );
			s->telnetState = TELNET_CMD;
		}
		else{
			if(!control_string_en_digit(s->buf, len)){
				strcpy( ( char * ) pcWriteBuffer, "\r\nОшибка при вводе нового пароля\r\n>" );
				s->telnetState = TELNET_CMD;
			}
			else{
				strncpy(password, s->buf, len);
				strncpy( ( char * ) pcWriteBuffer, ( const char * ) pcNewPSWHeader, strlen( ( char * ) pcNewPSWHeader ) );
				s->telnetState = TELNET_CHANGE_PWD_ACK;
			}
		}
	}
	else{
		s->telnetState = TELNET_CHANGE_PWD;
		memset(password2, 0, cmdMAX_INPUT_SIZE);
		if(len >= MAX_WEB_LOGIN_LEN){
			strcpy( ( char * ) pcWriteBuffer, "\r\nОшибка при вводе нового пароля\r\n>" );
			s->telnetState = TELNET_CMD;
		}
		else{
			strncpy(password2, s->buf, len);

			if (strncmp(password, password2, MAX_WEB_PASSWD_LEN) == 0) {
				memcpy(sSettings.sAuth[id_change_pwd].password, password, 11);
				telnet_act = true;
				HTTP_SaveSettings();
				log_event_data(LOG_PSW_CHANGE, name_login_telnet);
				strcpy( ( char * ) pcWriteBuffer, "\r\nПароль успешно изменен\r\n>" );
				s->telnetState = TELNET_CMD;
				xReturn = pdTRUE;
			}
			else{
				strcpy( ( char * ) pcWriteBuffer, "\r\nОшибка при вводе нового пароля\r\n>" );
				s->telnetState = TELNET_CMD;
			}
		}
	}


	return xReturn;
}