ioutil: make the file compile on MacOS
[fw/openocd] / src / server / telnet_server.c
index b6b38a0cf56afa842ce56291477e5dd43188acf8..f182a04301828397ab9d26607a77190a248860f6 100644 (file)
@@ -2,7 +2,7 @@
  *   Copyright (C) 2005 by Dominic Rath                                    *
  *   Dominic.Rath@gmx.de                                                   *
  *                                                                         *
- *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
+ *   Copyright (C) 2007-2010 Øyvind Harboe                                 *
  *   oyvind.harboe@zylin.com                                               *
  *                                                                         *
  *   Copyright (C) 2008 by Spencer Oliver                                  *
  *   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_request.h"
-
-static unsigned short telnet_port = 4444;
+#include <target/target_request.h>
 
-int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+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 */
+       "\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 - '@')
 
@@ -47,29 +45,27 @@ static char *negotiate =
  * we write to it, we will fail. Subsequent write operations will
  * succeed. Shudder!
  */
-int telnet_write(connection_t *connection, const void *data, int len)
+static int telnet_write(struct connection *connection, const void *data,
+       int len)
 {
-       telnet_connection_t *t_con = connection->priv;
+       struct telnet_connection *t_con = connection->priv;
        if (t_con->closed)
                return ERROR_SERVER_REMOTE_CLOSED;
 
-       if (write_socket(connection->fd, data, len) == len)
-       {
+       if (connection_write(connection, data, len) == len)
                return ERROR_OK;
-       }
        t_con->closed = 1;
        return ERROR_SERVER_REMOTE_CLOSED;
 }
 
-int telnet_prompt(connection_t *connection)
+static int telnet_prompt(struct connection *connection)
 {
-       telnet_connection_t *t_con = connection->priv;
+       struct telnet_connection *t_con = connection->priv;
 
-       telnet_write(connection, "\r", 1); /* the prompt is always placed at the line beginning */
        return telnet_write(connection, t_con->prompt, strlen(t_con->prompt));
 }
 
-int telnet_outputline(connection_t *connection, const char *line)
+static int telnet_outputline(struct connection *connection, const char *line)
 {
        int len;
 
@@ -83,46 +79,43 @@ int telnet_outputline(connection_t *connection, const char *line)
                        len = strlen(line);
 
                telnet_write(connection, line, len);
-               if (line_end)
-               {
+               if (line_end) {
                        telnet_write(connection, "\r\n", 2);
                        line += len + 1;
-               }
-               else
-               {
+               } else
                        line += len;
-               }
        }
 
        return ERROR_OK;
 }
 
-int telnet_output(struct command_context_s *cmd_ctx, const char* line)
+static int telnet_output(struct command_context *cmd_ctx, const char *line)
 {
-       connection_t *connection = cmd_ctx->output_handler_priv;
+       struct connection *connection = cmd_ctx->output_handler_priv;
 
        return telnet_outputline(connection, line);
 }
 
-void telnet_log_callback(void *priv, const char *file, int line,
-               const char *function, const char *string)
+static void telnet_log_callback(void *priv, const char *file, unsigned line,
+       const char *function, const char *string)
 {
-       connection_t *connection = priv;
-       telnet_connection_t *t_con = connection->priv;
+       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)
-       {
+       if (t_con->line_cursor < 0) {
                telnet_outputline(connection, string);
                return;
        }
 
        /* clear the command line */
-       telnet_write(connection, "\r", 1);
+       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);
-       telnet_write(connection, "\r", 1);
+       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);
@@ -134,10 +127,10 @@ void telnet_log_callback(void *priv, const char *file, int line,
                telnet_write(connection, "\b", 1);
 }
 
-int telnet_new_connection(connection_t *connection)
+static int telnet_new_connection(struct connection *connection)
 {
-       telnet_connection_t *telnet_connection = malloc(sizeof(telnet_connection_t));
-       telnet_service_t *telnet_service = connection->service->priv;
+       struct telnet_connection *telnet_connection = malloc(sizeof(struct telnet_connection));
+       struct telnet_service *telnet_service = connection->service->priv;
        int i;
 
        connection->priv = telnet_connection;
@@ -157,19 +150,18 @@ int telnet_new_connection(connection_t *connection)
        telnet_write(connection, negotiate, strlen(negotiate));
 
        /* print connection banner */
-       if (telnet_service->banner)
-       {
+       if (telnet_service->banner) {
                telnet_write(connection, telnet_service->banner, strlen(telnet_service->banner));
                telnet_write(connection, "\r\n", 2);
        }
 
+       /* the prompt is always placed at the line beginning */
+       telnet_write(connection, "\r", 1);
        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;
 
@@ -178,100 +170,86 @@ int telnet_new_connection(connection_t *connection)
        return ERROR_OK;
 }
 
-void telnet_clear_line(connection_t *connection, telnet_connection_t *t_con)
+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);
-       }
+               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)
-       {
+       while (t_con->line_size > 0) {
                telnet_write(connection, "\b \b", 3);
                t_con->line_size--;
        }
        t_con->line_cursor = 0;
 }
 
-int telnet_input(connection_t *connection)
+static int telnet_input(struct connection *connection)
 {
        int bytes_read;
-       char buffer[TELNET_BUFFER_SIZE];
-       char *buf_p;
-       telnet_connection_t *t_con = connection->priv;
-       command_context_t *command_context = connection->cmd_ctx;
+       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 = read_socket(connection->fd, buffer, TELNET_BUFFER_SIZE);
+       bytes_read = connection_read(connection, buffer, TELNET_BUFFER_SIZE);
 
        if (bytes_read == 0)
                return ERROR_SERVER_REMOTE_CLOSED;
-       else if (bytes_read == -1)
-       {
+       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)
-               {
+       while (bytes_read) {
+               switch (t_con->state) {
                        case TELNET_STATE_DATA:
-                               if (*buf_p == '\xff')
-                               {
+                               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)
-                                               {
+                               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
+                                                        * "\a" does not work, at least on windows */
+                                                       telnet_write(connection, "\x07", 1);
+                                               } 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
-                                               {
+                                               } 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);
+                                                       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);
+                                                       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 */
-                                               {
+                                       } 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 */
-                                               {
+                                               } 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)))
-                                                       {
+                                                       if ((bytes_read > 1) && ((*(buf_p + 1) == 0xa) ||
+                                                                       (*(buf_p + 1) == 0xd))) {
                                                                buf_p++;
                                                                bytes_read--;
                                                        }
-                                                       if ((bytes_read > 1) && (*(buf_p + 1) == 0))
-                                                       {
+                                                       if ((bytes_read > 1) && (*(buf_p + 1) == 0)) {
                                                                buf_p++;
                                                                bytes_read--;
                                                        }
@@ -279,16 +257,17 @@ int telnet_input(connection_t *connection)
 
                                                        telnet_write(connection, "\r\n\x00", 3);
 
-                                                       if (strcmp(t_con->line, "history") == 0)
-                                                       {
+                                                       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));
+                                                               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);
                                                                        }
                                                                }
@@ -298,34 +277,34 @@ int telnet_input(connection_t *connection)
                                                        }
 
                                                        /* 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)))
-                                                       {
+                                                       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;
+                                                               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;
+                                                               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 */
+                                                       /* to suppress prompt in log callback during command execution */
+                                                       t_con->line_cursor = -1;
 
                                                        retval = command_run_line(command_context, t_con->line);
 
@@ -334,82 +313,71 @@ int telnet_input(connection_t *connection)
                                                        if (retval == ERROR_COMMAND_CLOSE_CONNECTION)
                                                                return ERROR_SERVER_REMOTE_CLOSED;
 
+                                                       /* the prompt is always * placed at the line beginning */
+                                                       telnet_write(connection, "\r", 1);
+
                                                        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)
-                                                               {
+                                               } 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);
+                                                                       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
-                                                               {
+                                                               } else {
                                                                        t_con->line_size--;
                                                                        t_con->line_cursor--;
-                                                                       /* back space: move the 'printer' head one char back, overwrite with space, move back again */
+                                                                       /* 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 */
-                                               {
+                                               } 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)
-                                                       {
+                                               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 */
-                                               {
+                                               } 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
-                                               {
+                                               } else
                                                        LOG_DEBUG("unhandled nonprintable: %2.2x", *buf_p);
-                                               }
                                        }
                                }
                                break;
                        case TELNET_STATE_IAC:
-                               switch (*buf_p)
-                               {
-                                       case '\xfe':
-                                               t_con->state = TELNET_STATE_DONT;
-                                               break;
-                                       case '\xfd':
-                                               t_con->state = TELNET_STATE_DO;
-                                               break;
-                                       case '\xfc':
-                                               t_con->state = TELNET_STATE_WONT;
-                                               break;
-                                       case '\xfb':
-                                               t_con->state = TELNET_STATE_WILL;
-                                               break;
+                               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:
@@ -423,30 +391,22 @@ int telnet_input(connection_t *connection)
                                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)
-                                               {
+                               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 */
-                                       {
+                                       } 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);
-                                               }
+                                                       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])
-                                               {
+                                       } 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;
@@ -455,12 +415,9 @@ int telnet_input(connection_t *connection)
                                                        t_con->current_history = last_history;
                                                }
                                                t_con->state = TELNET_STATE_DATA;
-                                       }
-                                       else if (*buf_p == 'B') /* cursor down */
-                                       {
+                                       } 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])
-                                               {
+                                               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;
@@ -469,60 +426,41 @@ int telnet_input(connection_t *connection)
                                                        t_con->current_history = next_history;
                                                }
                                                t_con->state = TELNET_STATE_DATA;
-                                       }
-                                       else if (*buf_p == '3')
-                                       {
+                                       } 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')
-                               {
+                               } else if (t_con->last_escape == '3') {
                                        /* Remove character */
-                                       if (*buf_p == '~')
-                                       {
-                                               if (t_con->line_cursor < t_con->line_size)
-                                               {
+                                       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);
+                                                       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);
+                                                       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
-                                       {
+                                       } else
                                                t_con->state = TELNET_STATE_DATA;
-                                       }
-                               }
-                               else if (t_con->last_escape == '\x00')
-                               {
+                               } else if (t_con->last_escape == '\x00') {
                                        if (*buf_p == '[')
-                                       {
                                                t_con->last_escape = *buf_p;
-                                       }
                                        else
-                                       {
                                                t_con->state = TELNET_STATE_DATA;
-                                       }
-                               }
-                               else
-                               {
+                               } else {
                                        LOG_ERROR("BUG: unexpected value in t_con->last_escape");
                                        t_con->state = TELNET_STATE_DATA;
                                }
@@ -540,23 +478,20 @@ int telnet_input(connection_t *connection)
        return ERROR_OK;
 }
 
-int telnet_connection_closed(connection_t *connection)
+static int telnet_connection_closed(struct connection *connection)
 {
-       telnet_connection_t *t_con = connection->priv;
+       struct telnet_connection *t_con = connection->priv;
        int i;
 
        log_remove_callback(telnet_log_callback, connection);
 
-       if (t_con->prompt)
-       {
+       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])
-               {
+       for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++) {
+               if (t_con->history[i]) {
                        free(t_con->history[i]);
                        t_con->history[i] = NULL;
                }
@@ -565,74 +500,68 @@ int telnet_connection_closed(connection_t *connection)
        /* if this connection registered a debug-message receiver delete it */
        delete_debug_msg_receiver(connection->cmd_ctx, NULL);
 
-       if (connection->priv)
-       {
+       if (connection->priv) {
                free(connection->priv);
                connection->priv = NULL;
-       }
-       else
-       {
+       } else
                LOG_ERROR("BUG: connection->priv == NULL");
-       }
-
-       return ERROR_OK;
-}
-
-int telnet_set_prompt(connection_t *connection, char *prompt)
-{
-       telnet_connection_t *t_con = connection->priv;
-
-       if (t_con->prompt != NULL)
-               free(t_con->prompt);
-
-       t_con->prompt = strdup(prompt);
 
        return ERROR_OK;
 }
 
 int telnet_init(char *banner)
 {
-       telnet_service_t *telnet_service = malloc(sizeof(telnet_service_t));
-
-       if (telnet_port == 0)
-       {
-               LOG_INFO("telnet port disabled");
+       if (strcmp(telnet_port, "disabled") == 0) {
+               LOG_INFO("telnet server disabled");
                return ERROR_OK;
        }
 
-       telnet_service->banner = banner;
+       struct telnet_service *telnet_service = malloc(sizeof(struct telnet_service));
 
-       add_service("telnet", CONNECTION_TCP, telnet_port, 1, telnet_new_connection, telnet_input, telnet_connection_closed, telnet_service);
+       telnet_service->banner = banner;
 
-       return ERROR_OK;
+       return add_service("telnet",
+               telnet_port,
+               1,
+               telnet_new_connection,
+               telnet_input,
+               telnet_connection_closed,
+               telnet_service);
 }
 
-int telnet_register_commands(command_context_t *command_context)
+/* daemon configuration command telnet_port */
+COMMAND_HANDLER(handle_telnet_port_command)
 {
-       register_command(command_context, NULL, "exit", handle_exit_command,
-                                        COMMAND_EXEC, "exit telnet session");
-
-       register_command(command_context, NULL, "telnet_port", handle_telnet_port_command,
-                                        COMMAND_ANY, "port on which to listen for incoming telnet connections");
-
-       return ERROR_OK;
+       return CALL_COMMAND_HANDLER(server_pipe_command, &telnet_port);
 }
 
-/* daemon configuration command telnet_port */
-int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+COMMAND_HANDLER(handle_exit_command)
 {
-       if (argc == 0)
-       {
-               command_print(cmd_ctx, "%d", telnet_port);
-               return ERROR_OK;
-       }
-
-       telnet_port = strtoul(args[0], NULL, 0);
-
-       return ERROR_OK;
+       return ERROR_COMMAND_CLOSE_CONNECTION;
 }
 
-int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+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)
 {
-       return ERROR_COMMAND_CLOSE_CONNECTION;
+       telnet_port = strdup("4444");
+       return register_commands(cmd_ctx, NULL, telnet_command_handlers);
 }