build: cleanup src/server directory
[openocd.git] / src / server / telnet_server.c
index 830cf64..5d2e3bc 100644 (file)
-/***************************************************************************\r
- *   Copyright (C) 2005 by Dominic Rath                                    *\r
- *   Dominic.Rath@gmx.de                                                   *\r
- *                                                                         *\r
- *   This program is free software; you can redistribute it and/or modify  *\r
- *   it under the terms of the GNU General Public License as published by  *\r
- *   the Free Software Foundation; either version 2 of the License, or     *\r
- *   (at your option) any later version.                                   *\r
- *                                                                         *\r
- *   This program is distributed in the hope that it will be useful,       *\r
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
- *   GNU General Public License for more details.                          *\r
- *                                                                         *\r
- *   You should have received a copy of the GNU General Public License     *\r
- *   along with this program; if not, write to the                         *\r
- *   Free Software Foundation, Inc.,                                       *\r
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
- ***************************************************************************/\r
-#ifdef HAVE_CONFIG_H\r
-#include "config.h"\r
-#endif\r
-\r
-#include "replacements.h"\r
-\r
-#include "telnet_server.h"\r
-\r
-#include "server.h"\r
-#include "log.h"\r
-#include "command.h"\r
-#include "target.h"\r
-#include "target_request.h"\r
-\r
-#include <stdlib.h>\r
-#include <unistd.h>\r
-#include <errno.h>\r
-#include <string.h>\r
-#include <ctype.h>\r
-\r
-static unsigned short telnet_port = 0;\r
-\r
-int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
-int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
-\r
-static char *negotiate =\r
-               "\xFF\xFB\x03"          /* IAC WILL Suppress Go Ahead */\r
-               "\xFF\xFB\x01"          /* IAC WILL Echo */\r
-               "\xFF\xFD\x03"          /* IAC DO Suppress Go Ahead */\r
-               "\xFF\xFE\x01";         /* IAC DON'T Echo */\r
-               \r
-#define CTRL(c) (c - '@')\r
-       \r
-/* The only way we can detect that the socket is closed is the first time\r
- * we write to it, we will fail. Subsequent write operations will\r
- * succeed. Shudder!\r
- */\r
-int telnet_write(connection_t *connection, void *data, int len)\r
-{\r
-       telnet_connection_t *t_con = connection->priv;\r
-       if (t_con->closed)\r
-               return ERROR_SERVER_REMOTE_CLOSED;\r
-\r
-       if (write_socket(connection->fd, data, len) == len)\r
-       {\r
-               return ERROR_OK;\r
-       }\r
-       t_con->closed = 1;\r
-       return ERROR_SERVER_REMOTE_CLOSED;\r
-}\r
-\r
-int telnet_prompt(connection_t *connection)\r
-{\r
-       telnet_connection_t *t_con = connection->priv;\r
-\r
-       return telnet_write(connection, t_con->prompt, strlen(t_con->prompt));\r
-}\r
-\r
-int telnet_outputline(connection_t *connection, char* line)\r
-{\r
-       telnet_write(connection, line, strlen(line));\r
-       return telnet_write(connection, "\r\n\0", 3);\r
-}\r
-\r
-int telnet_output(struct command_context_s *cmd_ctx, char* line)\r
-{\r
-       connection_t *connection = cmd_ctx->output_handler_priv;\r
-       \r
-       return telnet_outputline(connection, line);\r
-}\r
-\r
-void telnet_log_callback(void *priv, const char *file, int line, \r
-               const char *function, const char *format, va_list args)\r
-{\r
-       connection_t *connection = priv;\r
-       char *t = alloc_printf(format, args);\r
-       char *t2;\r
-       if (t == NULL)\r
-               return;\r
-       t2=t;\r
-       char *endline;\r
-       do \r
-       {\r
-               if ((endline=strchr(t2, '\n'))!=NULL)\r
-               {\r
-                       *endline=0;\r
-               }\r
-               telnet_outputline(connection, t2);\r
-               t2=endline+1;\r
-       } while (endline);\r
-       \r
-       free(t);\r
-}\r
-\r
-int telnet_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)\r
-{\r
-       struct command_context_s *cmd_ctx = priv;\r
-       connection_t *connection = cmd_ctx->output_handler_priv;\r
-       telnet_connection_t *t_con = connection->priv;\r
-       \r
-       switch (event)\r
-       {\r
-               case TARGET_EVENT_HALTED:\r
-                       target_arch_state(target);\r
-                       if (!t_con->suppress_prompt)\r
-                               telnet_prompt(connection);\r
-                       break;\r
-               case TARGET_EVENT_RESUMED:\r
-                       if (!t_con->suppress_prompt)\r
-                               telnet_prompt(connection);\r
-                       break;\r
-               default:\r
-                       break;\r
-       }\r
-\r
-       return ERROR_OK;\r
-}\r
-\r
-int telnet_new_connection(connection_t *connection)\r
-{\r
-       telnet_connection_t *telnet_connection = malloc(sizeof(telnet_connection_t));\r
-       telnet_service_t *telnet_service = connection->service->priv;\r
-       int i;\r
-       \r
-       connection->priv = telnet_connection;\r
-       \r
-       /* initialize telnet connection information */\r
-       telnet_connection->closed = 0;\r
-       telnet_connection->line_size = 0;\r
-       telnet_connection->line_cursor = 0;\r
-       telnet_connection->option_size = 0;\r
-       telnet_connection->prompt = strdup("> ");\r
-       telnet_connection->suppress_prompt = 0;\r
-       telnet_connection->state = TELNET_STATE_DATA;\r
-       \r
-       /* output goes through telnet connection */\r
-       command_set_output_handler(connection->cmd_ctx, telnet_output, connection);\r
-       \r
-       /* negotiate telnet options */\r
-       telnet_write(connection, negotiate, strlen(negotiate));\r
-       \r
-       /* print connection banner */\r
-       if (telnet_service->banner)\r
-       {\r
-               telnet_write(connection, telnet_service->banner, strlen(telnet_service->banner));\r
-               telnet_write(connection, "\r\n\0", 3);\r
-       }\r
-       \r
-       telnet_prompt(connection);\r
-       \r
-       /* initialize history */\r
-       for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)\r
-       {\r
-               telnet_connection->history[i] = NULL;\r
-       }\r
-       telnet_connection->next_history = 0;\r
-       telnet_connection->current_history = 0;\r
-\r
-       target_register_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx);\r
-       \r
-       return ERROR_OK;\r
-}\r
-\r
-void telnet_clear_line(connection_t *connection, telnet_connection_t *t_con)\r
-{\r
-       /* move to end of line */\r
-       if (t_con->line_cursor < t_con->line_size)\r
-       {\r
-               telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);\r
-       }\r
-                                                       \r
-       /* backspace, overwrite with space, backspace */\r
-       while (t_con->line_size > 0)\r
-       {\r
-               telnet_write(connection, "\b \b", 3);\r
-               t_con->line_size--;\r
-       }\r
-       t_con->line_cursor = 0;\r
-}\r
-\r
-int telnet_input(connection_t *connection)\r
-{\r
-       int bytes_read;\r
-       char buffer[TELNET_BUFFER_SIZE];\r
-       char *buf_p;\r
-       telnet_connection_t *t_con = connection->priv;\r
-       command_context_t *command_context = connection->cmd_ctx;\r
-       \r
-       bytes_read = read_socket(connection->fd, buffer, TELNET_BUFFER_SIZE);\r
-       \r
-       if (bytes_read == 0)\r
-               return ERROR_SERVER_REMOTE_CLOSED;\r
-       else if (bytes_read == -1)\r
-       {\r
-               ERROR("error during read: %s", strerror(errno));\r
-               return ERROR_SERVER_REMOTE_CLOSED;\r
-       }\r
-       \r
-       buf_p = buffer;\r
-       while (bytes_read)\r
-       {\r
-               switch (t_con->state)\r
-               {\r
-                       case TELNET_STATE_DATA:\r
-                               if (*buf_p == '\xff')\r
-                               {\r
-                                       t_con->state = TELNET_STATE_IAC;\r
-                               }\r
-                               else\r
-                               {\r
-                                       if (isprint(*buf_p)) /* printable character */\r
-                                       {\r
-                                               telnet_write(connection, buf_p, 1);\r
-                                               if (t_con->line_cursor == t_con->line_size)\r
-                                               {\r
-                                                       t_con->line[t_con->line_size++] = *buf_p;\r
-                                                       t_con->line_cursor++;\r
-                                               }\r
-                                               else\r
-                                               {\r
-                                                       int i;\r
-                                                       memmove(t_con->line + t_con->line_cursor + 1, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);\r
-                                                       t_con->line[t_con->line_cursor++] = *buf_p;\r
-                                                       t_con->line_size++;\r
-                                                       telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);\r
-                                                       for (i = t_con->line_cursor; i < t_con->line_size; i++)\r
-                                                       {\r
-                                                               telnet_write(connection, "\b", 1);\r
-                                                       }\r
-                                               }\r
-                                       }\r
-                                       else /* non-printable */\r
-                                       {\r
-                                               if (*buf_p == 0x1b) /* escape */\r
-                                               {\r
-                                                       t_con->state = TELNET_STATE_ESCAPE;\r
-                                                       t_con->last_escape = '\x00';\r
-                                               }\r
-                                               else if ((*buf_p == 0xd) || (*buf_p == 0xa)) /* CR/LF */\r
-                                               {\r
-                                                       int retval;\r
-                                                       \r
-                                                       /* skip over combinations with CR/LF + NUL */\r
-                                                       if (((*(buf_p + 1) == 0xa) || (*(buf_p + 1) == 0xd)) && (bytes_read > 1))\r
-                                                       {\r
-                                                               buf_p++;\r
-                                                               bytes_read--;\r
-                                                       }\r
-                                                       if ((*(buf_p + 1) == 0) && (bytes_read > 1))\r
-                                                       {\r
-                                                               buf_p++;\r
-                                                               bytes_read--;\r
-                                                       }\r
-                                                       t_con->line[t_con->line_size] = 0;\r
-                                                       \r
-                                                       telnet_write(connection, "\r\n\x00", 3);\r
-                                                       \r
-                                                       if (strcmp(t_con->line, "history") == 0)\r
-                                                       {\r
-                                                               int i;\r
-                                                               for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)\r
-                                                               {\r
-                                                                       if (t_con->history[i])\r
-                                                                       {\r
-                                                                               telnet_write(connection, t_con->history[i], strlen(t_con->history[i]));\r
-                                                                               telnet_write(connection, "\r\n\x00", 3);\r
-                                                                       }\r
-                                                               }\r
-                                                               telnet_prompt(connection);\r
-                                                               t_con->line_size = 0;\r
-                                                               t_con->line_cursor = 0;\r
-                                                               continue;\r
-                                                       }\r
-                                                       \r
-                                                       log_add_callback(telnet_log_callback, connection);\r
-                                                       t_con->suppress_prompt = 1;\r
-\r
-                                                       retval = command_run_line(command_context, t_con->line);\r
-                                                       \r
-                                                       log_remove_callback(telnet_log_callback, connection);\r
-                                                       t_con->suppress_prompt = 0;\r
-\r
-                                                       if (retval == ERROR_COMMAND_CLOSE_CONNECTION)\r
-                                                       {\r
-                                                               return ERROR_SERVER_REMOTE_CLOSED;\r
-                                                       }\r
-                                                       \r
-                                                       /* Save only non-blank lines in the history */\r
-                                                       if (t_con->line_size > 0)\r
-                                                       {\r
-                                                               /* if the history slot is already taken, free it */\r
-                                                               if (t_con->history[t_con->next_history])\r
-                                                               {\r
-                                                                       free(t_con->history[t_con->next_history]);\r
-                                                               }\r
-               \r
-                                                               /* add line to history */\r
-                                                               t_con->history[t_con->next_history] = strdup(t_con->line);\r
-\r
-                                                               /* wrap history at TELNET_LINE_HISTORY_SIZE */\r
-                                                               t_con->next_history = (t_con->next_history + 1) % TELNET_LINE_HISTORY_SIZE;\r
-                                                       \r
-                                                               /* current history line starts at the new entry */\r
-                                                               t_con->current_history = t_con->next_history;\r
-                                                       \r
-                                                               if (t_con->history[t_con->current_history])\r
-                                                               {\r
-                                                                       free(t_con->history[t_con->current_history]);\r
-                                                               }\r
-                                                               t_con->history[t_con->current_history] = strdup("");\r
-                                                       }\r
-                                                       \r
-                                                       int t = telnet_prompt(connection);\r
-                                                       if (t == ERROR_SERVER_REMOTE_CLOSED)\r
-                                                               return t;\r
-                                                       \r
-                                                       t_con->line_size = 0;\r
-                                                       t_con->line_cursor = 0;\r
-                                               }\r
-                                               else if ((*buf_p == 0x7f) || (*buf_p == 0x8)) /* delete character */\r
-                                               {\r
-                                                       if (t_con->line_cursor > 0)\r
-                                                       {\r
-                                                               if (t_con->line_cursor != t_con->line_size)\r
-                                                               {\r
-                                                                       int i;\r
-                                                                       telnet_write(connection, "\b", 1);\r
-                                                                       t_con->line_cursor--;\r
-                                                                       t_con->line_size--;\r
-                                                                       memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor);\r
-                                                                       \r
-                                                                       telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);\r
-                                                                       telnet_write(connection, " \b", 2);\r
-                                                                       for (i = t_con->line_cursor; i < t_con->line_size; i++)\r
-                                                                       {\r
-                                                                               telnet_write(connection, "\b", 1);\r
-                                                                       }\r
-                                                               }\r
-                                                               else\r
-                                                               {\r
-                                                                       t_con->line_size--;\r
-                                                                       t_con->line_cursor--;\r
-                                                                       /* back space: move the 'printer' head one char back, overwrite with space, move back again */\r
-                                                                       telnet_write(connection, "\b \b", 3);\r
-                                                               }\r
-                                                       }\r
-                                               }\r
-                                               else if (*buf_p == 0x15) /* clear line */\r
-                                               {\r
-                                                       telnet_clear_line(connection, t_con);\r
-                                               }\r
-                                               else if (*buf_p == CTRL('B')) /* cursor left */\r
-                                               {\r
-                                                       if (t_con->line_cursor > 0)\r
-                                                       {\r
-                                                               telnet_write(connection, "\b", 1);\r
-                                                               t_con->line_cursor--;\r
-                                                       }\r
-                                                       t_con->state = TELNET_STATE_DATA;\r
-                                               }\r
-                                               else if (*buf_p == CTRL('F')) /* cursor right */\r
-                                               {\r
-                                                       if (t_con->line_cursor < t_con->line_size)\r
-                                                       {\r
-                                                               telnet_write(connection, t_con->line + t_con->line_cursor++, 1);\r
-                                                       }\r
-                                                       t_con->state = TELNET_STATE_DATA;\r
-                                               }\r
-                                               else\r
-                                               {\r
-                                                       DEBUG("unhandled nonprintable: %2.2x", *buf_p);\r
-                                               }\r
-                                       }\r
-                               }\r
-                               break;\r
-                       case TELNET_STATE_IAC:\r
-                               switch (*buf_p)\r
-                               {\r
-                                       case '\xfe':\r
-                                               t_con->state = TELNET_STATE_DONT;\r
-                                               break;\r
-                                       case '\xfd':\r
-                                               t_con->state = TELNET_STATE_DO;\r
-                                               break;\r
-                                       case '\xfc':\r
-                                               t_con->state = TELNET_STATE_WONT;\r
-                                               break;\r
-                                       case '\xfb':\r
-                                               t_con->state = TELNET_STATE_WILL;\r
-                                               break;\r
-                               }\r
-                               break;\r
-                       case TELNET_STATE_SB:\r
-                               break;\r
-                       case TELNET_STATE_SE:\r
-                               break;\r
-                       case TELNET_STATE_WILL:\r
-                       case TELNET_STATE_WONT:\r
-                       case TELNET_STATE_DO:\r
-                       case TELNET_STATE_DONT:\r
-                               t_con->state = TELNET_STATE_DATA;\r
-                               break;\r
-                       case TELNET_STATE_ESCAPE:\r
-                               if (t_con->last_escape == '[')\r
-                               {\r
-                                       if (*buf_p == 'D') /* cursor left */\r
-                                       {\r
-                                               if (t_con->line_cursor > 0)\r
-                                               {\r
-                                                       telnet_write(connection, "\b", 1);\r
-                                                       t_con->line_cursor--;\r
-                                               }\r
-                                               t_con->state = TELNET_STATE_DATA;\r
-                                       }\r
-                                       else if (*buf_p == 'C') /* cursor right */\r
-                                       {\r
-                                               if (t_con->line_cursor < t_con->line_size)\r
-                                               {\r
-                                                       telnet_write(connection, t_con->line + t_con->line_cursor++, 1);\r
-                                               }\r
-                                               t_con->state = TELNET_STATE_DATA;\r
-                                       }\r
-                                       else if (*buf_p == 'A') /* cursor up */\r
-                                       {\r
-                                               int last_history = (t_con->current_history > 0) ? t_con->current_history - 1 : TELNET_LINE_HISTORY_SIZE-1;\r
-                                               if (t_con->history[last_history])\r
-                                               {\r
-                                                       telnet_clear_line(connection, t_con);\r
-                                                       t_con->line_size = strlen(t_con->history[last_history]);\r
-                                                       t_con->line_cursor = t_con->line_size;\r
-                                                       memcpy(t_con->line, t_con->history[last_history], t_con->line_size + 1);\r
-                                                       telnet_write(connection, t_con->line, t_con->line_size);\r
-                                                       t_con->current_history = last_history;\r
-                                               }\r
-                                               t_con->state = TELNET_STATE_DATA;\r
-                                       }\r
-                                       else if (*buf_p == 'B') /* cursor down */\r
-                                       {\r
-                                               int next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE;\r
-                                               if (t_con->history[next_history])\r
-                                               {\r
-                                                       telnet_clear_line(connection, t_con);\r
-                                                       t_con->line_size = strlen(t_con->history[next_history]);\r
-                                                       t_con->line_cursor = t_con->line_size;\r
-                                                       memcpy(t_con->line, t_con->history[next_history], t_con->line_size + 1);\r
-                                                       telnet_write(connection, t_con->line, t_con->line_size);\r
-                                                       t_con->current_history = next_history;\r
-                                               }\r
-                                               t_con->state = TELNET_STATE_DATA;\r
-                                       }\r
-                                       else if (*buf_p == '3')\r
-                                       {\r
-                                               t_con->last_escape = *buf_p;\r
-                                       }\r
-                                       else\r
-                                       {\r
-                                               t_con->state = TELNET_STATE_DATA;\r
-                                       }\r
-                               }\r
-                               else if (t_con->last_escape == '3')\r
-                               {\r
-                                       /* Remove character */\r
-                                       if (*buf_p == '~')\r
-                                       {\r
-                                               if (t_con->line_cursor < t_con->line_size)\r
-                                               {\r
-                                                       int i;\r
-                                                       t_con->line_size--;\r
-                                                       /* remove char from line buffer */\r
-                                                       memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor);\r
-                                                       \r
-                                                       /* print remainder of buffer */\r
-                                                       telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);\r
-                                                       /* overwrite last char with whitespace */\r
-                                                       telnet_write(connection, " \b", 2);\r
-                                                       \r
-                                                       /* move back to cursor position*/\r
-                                                       for (i = t_con->line_cursor; i < t_con->line_size; i++)\r
-                                                       {\r
-                                                               telnet_write(connection, "\b", 1);\r
-                                                       }\r
-                                               }\r
-                                                       \r
-                                               t_con->state = TELNET_STATE_DATA;\r
-                                       }\r
-                                       else\r
-                                       {\r
-                                               t_con->state = TELNET_STATE_DATA;\r
-                                       }\r
-                               }\r
-                               else if (t_con->last_escape == '\x00')\r
-                               {\r
-                                       if (*buf_p == '[')\r
-                                       {\r
-                                               t_con->last_escape = *buf_p;\r
-                                       }\r
-                                       else\r
-                                       {\r
-                                               t_con->state = TELNET_STATE_DATA;\r
-                                       }\r
-                               }\r
-                               else\r
-                               {\r
-                                       ERROR("BUG: unexpected value in t_con->last_escape");\r
-                                       t_con->state = TELNET_STATE_DATA;\r
-                               }\r
-                               \r
-                               break;\r
-                       default:\r
-                               ERROR("unknown telnet state");\r
-                               exit(-1);\r
-               }\r
-\r
-               bytes_read--;\r
-               buf_p++;\r
-       }\r
-       \r
-       return ERROR_OK;\r
-}\r
-\r
-int telnet_connection_closed(connection_t *connection)\r
-{\r
-       telnet_connection_t *t_con = connection->priv;\r
-       int i;\r
-       \r
-       if (t_con->prompt)\r
-       {\r
-               free(t_con->prompt);\r
-               t_con->prompt = NULL;\r
-       }\r
-       \r
-       for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)\r
-       {\r
-               if (t_con->history[i])\r
-               {\r
-                       free(t_con->history[i]);\r
-                       t_con->history[i] = NULL;\r
-               }\r
-       }\r
-       \r
-       /* if this connection registered a debug-message receiver delete it */\r
-       delete_debug_msg_receiver(connection->cmd_ctx, NULL);\r
-       \r
-       if (connection->priv)\r
-       {\r
-               free(connection->priv);\r
-               connection->priv = NULL;\r
-       }\r
-       else\r
-       {\r
-               ERROR("BUG: connection->priv == NULL");\r
-       }\r
-       \r
-       target_unregister_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx);\r
-\r
-       return ERROR_OK;\r
-}\r
-\r
-int telnet_set_prompt(connection_t *connection, char *prompt)\r
-{\r
-       telnet_connection_t *t_con = connection->priv;\r
-\r
-       t_con->prompt = strdup(prompt);\r
-       \r
-       return ERROR_OK;\r
-}\r
-\r
-int telnet_init(char *banner)\r
-{\r
-       telnet_service_t *telnet_service = malloc(sizeof(telnet_service_t));\r
-       \r
-       if (telnet_port == 0)\r
-       {\r
-               WARNING("no telnet port specified, using default port 4444");\r
-               telnet_port = 4444;\r
-       }\r
-       \r
-       telnet_service->banner = banner;\r
-       \r
-       add_service("telnet", CONNECTION_TELNET, telnet_port, 1, telnet_new_connection, telnet_input, telnet_connection_closed, telnet_service);\r
-       \r
-       return ERROR_OK;\r
-}\r
-\r
-int telnet_register_commands(command_context_t *command_context)\r
-{\r
-       register_command(command_context, NULL, "exit", handle_exit_command,\r
-                                        COMMAND_EXEC, "exit telnet session");\r
-       \r
-       register_command(command_context, NULL, "telnet_port", handle_telnet_port_command,\r
-                                        COMMAND_CONFIG, "");\r
-       \r
-       return ERROR_OK;\r
-}\r
-\r
-/* daemon configuration command telnet_port */\r
-int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
-{\r
-       if (argc == 0)\r
-               return ERROR_OK;\r
-\r
-       /* only if the port wasn't overwritten by cmdline */\r
-       if (telnet_port == 0)\r
-               telnet_port = strtoul(args[0], NULL, 0);\r
-\r
-       return ERROR_OK;\r
-}\r
-\r
-int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
-{\r
-       return ERROR_COMMAND_CLOSE_CONNECTION;\r
-}\r
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2007-2010 √ėyvind Harboe                                 *
+ *   oyvind.harboe@zylin.com                                               *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "telnet_server.h"
+#include <target/target_request.h>
+
+static const char *telnet_port;
+
+static char *negotiate =
+       "\xFF\xFB\x03"                  /* IAC WILL Suppress Go Ahead */
+       "\xFF\xFB\x01"                  /* IAC WILL Echo */
+       "\xFF\xFD\x03"                  /* IAC DO Suppress Go Ahead */
+       "\xFF\xFE\x01";                 /* IAC DON'T Echo */
+
+#define CTRL(c) (c - '@')
+
+/* The only way we can detect that the socket is closed is the first time
+ * we write to it, we will fail. Subsequent write operations will
+ * succeed. Shudder!
+ */
+static int telnet_write(struct connection *connection, const void *data,
+       int len)
+{
+       struct telnet_connection *t_con = connection->priv;
+       if (t_con->closed)
+               return ERROR_SERVER_REMOTE_CLOSED;
+
+       if (connection_write(connection, data, len) == len)
+               return ERROR_OK;
+       t_con->closed = 1;
+       return ERROR_SERVER_REMOTE_CLOSED;
+}
+
+static int telnet_prompt(struct connection *connection)
+{
+       struct telnet_connection *t_con = connection->priv;
+
+       return telnet_write(connection, t_con->prompt, strlen(t_con->prompt));
+}
+
+static int telnet_outputline(struct connection *connection, const char *line)
+{
+       int len;
+
+       /* process lines in buffer */
+       while (*line) {
+               char *line_end = strchr(line, '\n');
+
+               if (line_end)
+                       len = line_end-line;
+               else
+                       len = strlen(line);
+
+               telnet_write(connection, line, len);
+               if (line_end) {
+                       telnet_write(connection, "\r\n", 2);
+                       line += len + 1;
+               } else
+                       line += len;
+       }
+
+       return ERROR_OK;
+}
+
+static int telnet_output(struct command_context *cmd_ctx, const char *line)
+{
+       struct connection *connection = cmd_ctx->output_handler_priv;
+
+       return telnet_outputline(connection, line);
+}
+
+static void telnet_log_callback(void *priv, const char *file, unsigned line,
+       const char *function, const char *string)
+{
+       struct connection *connection = priv;
+       struct telnet_connection *t_con = connection->priv;
+       int i;
+
+       /* if there is no prompt, simply output the message */
+       if (t_con->line_cursor < 0) {
+               telnet_outputline(connection, string);
+               return;
+       }
+
+       /* clear the command line */
+       for (i = strlen(t_con->prompt) + t_con->line_size; i > 0; i -= 16)
+               telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b", i > 16 ? 16 : i);
+       for (i = strlen(t_con->prompt) + t_con->line_size; i > 0; i -= 16)
+               telnet_write(connection, "                ", i > 16 ? 16 : i);
+       for (i = strlen(t_con->prompt) + t_con->line_size; i > 0; i -= 16)
+               telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b", i > 16 ? 16 : i);
+
+       /* output the message */
+       telnet_outputline(connection, string);
+
+       /* put the command line to its previous state */
+       telnet_prompt(connection);
+       telnet_write(connection, t_con->line, t_con->line_size);
+       for (i = t_con->line_size; i > t_con->line_cursor; i--)
+               telnet_write(connection, "\b", 1);
+}
+
+static int telnet_new_connection(struct connection *connection)
+{
+       struct telnet_connection *telnet_connection = malloc(sizeof(struct telnet_connection));
+       struct telnet_service *telnet_service = connection->service->priv;
+       int i;
+
+       connection->priv = telnet_connection;
+
+       /* initialize telnet connection information */
+       telnet_connection->closed = 0;
+       telnet_connection->line_size = 0;
+       telnet_connection->line_cursor = 0;
+       telnet_connection->option_size = 0;
+       telnet_connection->prompt = strdup("> ");
+       telnet_connection->state = TELNET_STATE_DATA;
+
+       /* output goes through telnet connection */
+       command_set_output_handler(connection->cmd_ctx, telnet_output, connection);
+
+       /* negotiate telnet options */
+       telnet_write(connection, negotiate, strlen(negotiate));
+
+       /* print connection banner */
+       if (telnet_service->banner) {
+               telnet_write(connection, telnet_service->banner, strlen(telnet_service->banner));
+               telnet_write(connection, "\r\n", 2);
+       }
+
+       telnet_write(connection, "\r", 1);      /* the prompt is always placed at the line beginning
+                                                **/
+       telnet_prompt(connection);
+
+       /* initialize history */
+       for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
+               telnet_connection->history[i] = NULL;
+       telnet_connection->next_history = 0;
+       telnet_connection->current_history = 0;
+
+       log_add_callback(telnet_log_callback, connection);
+
+       return ERROR_OK;
+}
+
+static void telnet_clear_line(struct connection *connection,
+       struct telnet_connection *t_con)
+{
+       /* move to end of line */
+       if (t_con->line_cursor < t_con->line_size)
+               telnet_write(connection,
+                       t_con->line + t_con->line_cursor,
+                       t_con->line_size - t_con->line_cursor);
+
+       /* backspace, overwrite with space, backspace */
+       while (t_con->line_size > 0) {
+               telnet_write(connection, "\b \b", 3);
+               t_con->line_size--;
+       }
+       t_con->line_cursor = 0;
+}
+
+static int telnet_input(struct connection *connection)
+{
+       int bytes_read;
+       unsigned char buffer[TELNET_BUFFER_SIZE];
+       unsigned char *buf_p;
+       struct telnet_connection *t_con = connection->priv;
+       struct command_context *command_context = connection->cmd_ctx;
+
+       bytes_read = connection_read(connection, buffer, TELNET_BUFFER_SIZE);
+
+       if (bytes_read == 0)
+               return ERROR_SERVER_REMOTE_CLOSED;
+       else if (bytes_read == -1) {
+               LOG_ERROR("error during read: %s", strerror(errno));
+               return ERROR_SERVER_REMOTE_CLOSED;
+       }
+
+       buf_p = buffer;
+       while (bytes_read) {
+               switch (t_con->state) {
+                       case TELNET_STATE_DATA:
+                               if (*buf_p == 0xff)
+                                       t_con->state = TELNET_STATE_IAC;
+                               else {
+                                       if (isprint(*buf_p)) {  /* printable character */
+                                               /* watch buffer size leaving one spare character for
+                                                *string null termination */
+                                               if (t_con->line_size == TELNET_LINE_MAX_SIZE-1) {
+                                                       /* output audible bell if buffer is full */
+                                                       telnet_write(connection, "\x07", 1);    /*
+                                                                                                *"\a"
+                                                                                                *does
+                                                                                                *not
+                                                                                                *work,
+                                                                                                *at
+                                                                                                *least
+                                                                                                *on
+                                                                                                *windows
+                                                                                                **/
+                                               } else if (t_con->line_cursor == t_con->line_size) {
+                                                       telnet_write(connection, buf_p, 1);
+                                                       t_con->line[t_con->line_size++] = *buf_p;
+                                                       t_con->line_cursor++;
+                                               } else {
+                                                       int i;
+                                                       memmove(t_con->line + t_con->line_cursor + 1,
+                                                                       t_con->line + t_con->line_cursor,
+                                                                       t_con->line_size - t_con->line_cursor);
+                                                       t_con->line[t_con->line_cursor] = *buf_p;
+                                                       t_con->line_size++;
+                                                       telnet_write(connection,
+                                                                       t_con->line + t_con->line_cursor,
+                                                                       t_con->line_size - t_con->line_cursor);
+                                                       t_con->line_cursor++;
+                                                       for (i = t_con->line_cursor; i < t_con->line_size; i++)
+                                                               telnet_write(connection, "\b", 1);
+                                               }
+                                       } else {        /* non-printable */
+                                               if (*buf_p == 0x1b) {   /* escape */
+                                                       t_con->state = TELNET_STATE_ESCAPE;
+                                                       t_con->last_escape = '\x00';
+                                               } else if ((*buf_p == 0xd) || (*buf_p == 0xa)) {        /*
+                                                                                                *CR/LF
+                                                                                                **/
+                                                       int retval;
+
+                                                       /* skip over combinations with CR/LF and NUL
+                                                        *characters */
+                                                       if ((bytes_read > 1) && ((*(buf_p + 1) == 0xa) ||
+                                                                       (*(buf_p + 1) == 0xd))) {
+                                                               buf_p++;
+                                                               bytes_read--;
+                                                       }
+                                                       if ((bytes_read > 1) && (*(buf_p + 1) == 0)) {
+                                                               buf_p++;
+                                                               bytes_read--;
+                                                       }
+                                                       t_con->line[t_con->line_size] = 0;
+
+                                                       telnet_write(connection, "\r\n\x00", 3);
+
+                                                       if (strcmp(t_con->line, "history") == 0) {
+                                                               int i;
+                                                               for (i = 1; i < TELNET_LINE_HISTORY_SIZE; i++) {
+                                                                       /* the t_con->next_history
+                                                                        *line contains empty string
+                                                                        *(unless NULL), thus it is
+                                                                        *not printed */
+                                                                       char *history_line = t_con->history[(t_con->
+                                                                                       next_history + i) %
+                                                                                       TELNET_LINE_HISTORY_SIZE];
+                                                                       if (history_line) {
+                                                                               telnet_write(connection, history_line,
+                                                                                               strlen(history_line));
+                                                                               telnet_write(connection, "\r\n\x00", 3);
+                                                                       }
+                                                               }
+                                                               t_con->line_size = 0;
+                                                               t_con->line_cursor = 0;
+                                                               continue;
+                                                       }
+
+                                                       /* save only non-blank not repeating lines
+                                                        *in the history */
+                                                       char *prev_line = t_con->history[(t_con->current_history > 0) ?
+                                                                       t_con->current_history - 1 : TELNET_LINE_HISTORY_SIZE-1];
+                                                       if (*t_con->line && (prev_line == NULL ||
+                                                                       strcmp(t_con->line, prev_line))) {
+                                                               /* if the history slot is already
+                                                                *taken, free it */
+                                                               if (t_con->history[t_con->next_history])
+                                                                       free(t_con->history[t_con->next_history]);
+
+                                                               /* add line to history */
+                                                               t_con->history[t_con->next_history] = strdup(t_con->line);
+
+                                                               /* wrap history at
+                                                                *TELNET_LINE_HISTORY_SIZE */
+                                                               t_con->next_history = (t_con->next_history + 1) %
+                                                                               TELNET_LINE_HISTORY_SIZE;
+
+                                                               /* current history line starts at
+                                                                *the new entry */
+                                                               t_con->current_history =
+                                                                               t_con->next_history;
+
+                                                               if (t_con->history[t_con->current_history])
+                                                                       free(t_con->history[t_con->current_history]);
+                                                               t_con->history[t_con->current_history] = strdup("");
+                                                       }
+
+                                                       t_con->line_size = 0;
+
+                                                       t_con->line_cursor = -1;        /* to supress prompt
+                                                                                *in log callback
+                                                                                *during command
+                                                                                *execution */
+
+                                                       retval = command_run_line(command_context, t_con->line);
+
+                                                       t_con->line_cursor = 0;
+
+                                                       if (retval == ERROR_COMMAND_CLOSE_CONNECTION)
+                                                               return ERROR_SERVER_REMOTE_CLOSED;
+
+                                                       telnet_write(connection, "\r", 1);              /*
+                                                                                                *the
+                                                                                                *prompt
+                                                                                                *is
+                                                                                                *always
+                                                                                                *placed
+                                                                                                *at
+                                                                                                *the
+                                                                                                *line
+                                                                                                *beginning
+                                                                                                **/
+                                                       retval = telnet_prompt(connection);
+                                                       if (retval == ERROR_SERVER_REMOTE_CLOSED)
+                                                               return ERROR_SERVER_REMOTE_CLOSED;
+
+                                               } else if ((*buf_p == 0x7f) || (*buf_p == 0x8)) {       /*
+                                                                                                *delete
+                                                                                                *character
+                                                                                                **/
+                                                       if (t_con->line_cursor > 0) {
+                                                               if (t_con->line_cursor != t_con->line_size) {
+                                                                       int i;
+                                                                       telnet_write(connection, "\b", 1);
+                                                                       t_con->line_cursor--;
+                                                                       t_con->line_size--;
+                                                                       memmove(t_con->line + t_con->line_cursor,
+                                                                                       t_con->line + t_con->line_cursor + 1,
+                                                                                       t_con->line_size -
+                                                                                       t_con->line_cursor);
+
+                                                                       telnet_write(connection,
+                                                                                       t_con->line + t_con->line_cursor,
+                                                                                       t_con->line_size -
+                                                                                       t_con->line_cursor);
+                                                                       telnet_write(connection, " \b", 2);
+                                                                       for (i = t_con->line_cursor; i < t_con->line_size; i++)
+                                                                               telnet_write(connection, "\b", 1);
+                                                               } else {
+                                                                       t_con->line_size--;
+                                                                       t_con->line_cursor--;
+                                                                       /* back space: move the
+                                                                        *'printer' head one char
+                                                                        *back, overwrite with
+                                                                        *space, move back again */
+                                                                       telnet_write(connection, "\b \b", 3);
+                                                               }
+                                                       }
+                                               } else if (*buf_p == 0x15) /* clear line */
+                                                       telnet_clear_line(connection, t_con);
+                                               else if (*buf_p == CTRL('B')) { /* cursor left */
+                                                       if (t_con->line_cursor > 0) {
+                                                               telnet_write(connection, "\b", 1);
+                                                               t_con->line_cursor--;
+                                                       }
+                                                       t_con->state = TELNET_STATE_DATA;
+                                               } else if (*buf_p == CTRL('F')) {       /* cursor right */
+                                                       if (t_con->line_cursor < t_con->line_size)
+                                                               telnet_write(connection, t_con->line + t_con->line_cursor++, 1);
+                                                       t_con->state = TELNET_STATE_DATA;
+                                               } else
+                                                       LOG_DEBUG("unhandled nonprintable: %2.2x", *buf_p);
+                                       }
+                               }
+                               break;
+                       case TELNET_STATE_IAC:
+                               switch (*buf_p) {
+                               case 0xfe:
+                                       t_con->state = TELNET_STATE_DONT;
+                                       break;
+                               case 0xfd:
+                                       t_con->state = TELNET_STATE_DO;
+                                       break;
+                               case 0xfc:
+                                       t_con->state = TELNET_STATE_WONT;
+                                       break;
+                               case 0xfb:
+                                       t_con->state = TELNET_STATE_WILL;
+                                       break;
+                               }
+                               break;
+                       case TELNET_STATE_SB:
+                               break;
+                       case TELNET_STATE_SE:
+                               break;
+                       case TELNET_STATE_WILL:
+                       case TELNET_STATE_WONT:
+                       case TELNET_STATE_DO:
+                       case TELNET_STATE_DONT:
+                               t_con->state = TELNET_STATE_DATA;
+                               break;
+                       case TELNET_STATE_ESCAPE:
+                               if (t_con->last_escape == '[') {
+                                       if (*buf_p == 'D') {    /* cursor left */
+                                               if (t_con->line_cursor > 0) {
+                                                       telnet_write(connection, "\b", 1);
+                                                       t_con->line_cursor--;
+                                               }
+                                               t_con->state = TELNET_STATE_DATA;
+                                       } else if (*buf_p == 'C') {     /* cursor right */
+                                               if (t_con->line_cursor < t_con->line_size)
+                                                       telnet_write(connection,
+                                                                       t_con->line + t_con->line_cursor++, 1);
+                                               t_con->state = TELNET_STATE_DATA;
+                                       } else if (*buf_p == 'A') {     /* cursor up */
+                                               int last_history = (t_con->current_history > 0) ?
+                                                               t_con->current_history - 1 : TELNET_LINE_HISTORY_SIZE-1;
+                                               if (t_con->history[last_history]) {
+                                                       telnet_clear_line(connection, t_con);
+                                                       t_con->line_size = strlen(t_con->history[last_history]);
+                                                       t_con->line_cursor = t_con->line_size;
+                                                       memcpy(t_con->line, t_con->history[last_history], t_con->line_size);
+                                                       telnet_write(connection, t_con->line, t_con->line_size);
+                                                       t_con->current_history = last_history;
+                                               }
+                                               t_con->state = TELNET_STATE_DATA;
+                                       } else if (*buf_p == 'B') {     /* cursor down */
+                                               int next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE;
+                                               if (t_con->history[next_history]) {
+                                                       telnet_clear_line(connection, t_con);
+                                                       t_con->line_size = strlen(t_con->history[next_history]);
+                                                       t_con->line_cursor = t_con->line_size;
+                                                       memcpy(t_con->line, t_con->history[next_history], t_con->line_size);
+                                                       telnet_write(connection, t_con->line, t_con->line_size);
+                                                       t_con->current_history = next_history;
+                                               }
+                                               t_con->state = TELNET_STATE_DATA;
+                                       } else if (*buf_p == '3')
+                                               t_con->last_escape = *buf_p;
+                                       else
+                                               t_con->state = TELNET_STATE_DATA;
+                               } else if (t_con->last_escape == '3') {
+                                       /* Remove character */
+                                       if (*buf_p == '~') {
+                                               if (t_con->line_cursor < t_con->line_size) {
+                                                       int i;
+                                                       t_con->line_size--;
+                                                       /* remove char from line buffer */
+                                                       memmove(t_con->line + t_con->line_cursor,
+                                                                       t_con->line + t_con->line_cursor + 1,
+                                                                       t_con->line_size - t_con->line_cursor);
+
+                                                       /* print remainder of buffer */
+                                                       telnet_write(connection, t_con->line + t_con->line_cursor,
+                                                                       t_con->line_size - t_con->line_cursor);
+                                                       /* overwrite last char with whitespace */
+                                                       telnet_write(connection, " \b", 2);
+
+                                                       /* move back to cursor position*/
+                                                       for (i = t_con->line_cursor; i < t_con->line_size; i++)
+                                                               telnet_write(connection, "\b", 1);
+                                               }
+
+                                               t_con->state = TELNET_STATE_DATA;
+                                       } else
+                                               t_con->state = TELNET_STATE_DATA;
+                               } else if (t_con->last_escape == '\x00') {
+                                       if (*buf_p == '[')
+                                               t_con->last_escape = *buf_p;
+                                       else
+                                               t_con->state = TELNET_STATE_DATA;
+                               } else {
+                                       LOG_ERROR("BUG: unexpected value in t_con->last_escape");
+                                       t_con->state = TELNET_STATE_DATA;
+                               }
+
+                               break;
+                       default:
+                               LOG_ERROR("unknown telnet state");
+                               exit(-1);
+               }
+
+               bytes_read--;
+               buf_p++;
+       }
+
+       return ERROR_OK;
+}
+
+static int telnet_connection_closed(struct connection *connection)
+{
+       struct telnet_connection *t_con = connection->priv;
+       int i;
+
+       log_remove_callback(telnet_log_callback, connection);
+
+       if (t_con->prompt) {
+               free(t_con->prompt);
+               t_con->prompt = NULL;
+       }
+
+       for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++) {
+               if (t_con->history[i]) {
+                       free(t_con->history[i]);
+                       t_con->history[i] = NULL;
+               }
+       }
+
+       /* if this connection registered a debug-message receiver delete it */
+       delete_debug_msg_receiver(connection->cmd_ctx, NULL);
+
+       if (connection->priv) {
+               free(connection->priv);
+               connection->priv = NULL;
+       } else
+               LOG_ERROR("BUG: connection->priv == NULL");
+
+       return ERROR_OK;
+}
+
+int telnet_init(char *banner)
+{
+       if (strcmp(telnet_port, "disabled") == 0) {
+               LOG_INFO("telnet server disabled");
+               return ERROR_OK;
+       }
+
+       struct telnet_service *telnet_service = malloc(sizeof(struct telnet_service));
+
+       telnet_service->banner = banner;
+
+       return add_service("telnet",
+               telnet_port,
+               1,
+               telnet_new_connection,
+               telnet_input,
+               telnet_connection_closed,
+               telnet_service);
+}
+
+/* daemon configuration command telnet_port */
+COMMAND_HANDLER(handle_telnet_port_command)
+{
+       return CALL_COMMAND_HANDLER(server_pipe_command, &telnet_port);
+}
+
+COMMAND_HANDLER(handle_exit_command)
+{
+       return ERROR_COMMAND_CLOSE_CONNECTION;
+}
+
+static const struct command_registration telnet_command_handlers[] = {
+       {
+               .name = "exit",
+               .handler = handle_exit_command,
+               .mode = COMMAND_EXEC,
+               .usage = "",
+               .help = "exit telnet session",
+       },
+       {
+               .name = "telnet_port",
+               .handler = handle_telnet_port_command,
+               .mode = COMMAND_ANY,
+               .help = "Specify port on which to listen "
+                       "for incoming telnet connections.  "
+                       "Read help on 'gdb_port'.",
+               .usage = "[port_num]",
+       },
+       COMMAND_REGISTRATION_DONE
+};
+
+int telnet_register_commands(struct command_context *cmd_ctx)
+{
+       telnet_port = strdup("4444");
+       return register_commands(cmd_ctx, NULL, telnet_command_handlers);
+}