aarch64: handle semihosting in aarch32 state

[fw/openocd] / src / server / telnet_server.c

diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index 9077b6c46d2e51b3d31f1cf0b51ff9fba38a1e7a..e9de4f033b30d93354089d55a19d0d9bf58be40b 100644 (file)
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -312,6 +312,36 @@ static void telnet_history_down(struct connection *connection)
        telnet_history_go(connection, next_history);
 }
 
+static int telnet_history_print(struct connection *connection)
+{
+       struct telnet_connection *tc;
+
+       tc = connection->priv;
+
+       for (size_t i = 1; i < TELNET_LINE_HISTORY_SIZE; i++) {
+               char *line;
+
+               /*
+                * The tc->next_history line contains empty string (unless NULL), thus
+                * it is not printed.
+                */
+               line = tc->history[(tc->next_history + i) % TELNET_LINE_HISTORY_SIZE];
+
+               if (line) {
+                       telnet_write(connection, line, strlen(line));
+                       telnet_write(connection, "\r\n\x00", 3);
+               }
+       }
+
+       tc->line_size = 0;
+       tc->line_cursor = 0;
+
+       /* The prompt is always placed at the line beginning. */
+       telnet_write(connection, "\r", 1);
+
+       return telnet_prompt(connection);
+}
+
 static void telnet_move_cursor(struct connection *connection, size_t pos)
 {
        struct telnet_connection *tc;
@@ -407,21 +437,11 @@ static int telnet_input(struct connection *connection)
                                                        telnet_write(connection, "\r\n\x00", 3);
 
                                                        if (strcmp(t_con->line, "history") == 0) {
-                                                               size_t 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;
+                                                               retval = telnet_history_print(connection);
+
+                                                               if (retval != ERROR_OK)
+                                                                       return retval;
+
                                                                continue;
                                                        }
 
@@ -431,8 +451,7 @@ static int telnet_input(struct connection *connection)
                                                        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]);
+                                                               free(t_con->history[t_con->next_history]);
 
                                                                /* add line to history */
                                                                t_con->history[t_con->next_history] = strdup(t_con->line);
@@ -445,8 +464,7 @@ static int telnet_input(struct connection *connection)
                                                                t_con->current_history =
                                                                                t_con->next_history;
 
-                                                               if (t_con->history[t_con->current_history])
-                                                                       free(t_con->history[t_con->current_history]);
+                                                               free(t_con->history[t_con->current_history]);
                                                                t_con->history[t_con->current_history] = strdup("");
                                                        }
 
@@ -520,7 +538,17 @@ static int telnet_input(struct connection *connection)
                                                        telnet_move_cursor(connection, 0);
                                                else if (*buf_p == CTRL('E'))
                                                        telnet_move_cursor(connection, t_con->line_size);
-                                               else
+                                               else if (*buf_p == CTRL('K')) {         /* kill line to end */
+                                                       if (t_con->line_cursor < t_con->line_size) {
+                                                               /* overwrite with space, until end of line, move back */
+                                                               for (size_t i = t_con->line_cursor; i < t_con->line_size; i++)
+                                                                       telnet_write(connection, " ", 1);
+                                                               for (size_t i = t_con->line_cursor; i < t_con->line_size; i++)
+                                                                       telnet_write(connection, "\b", 1);
+                                                               t_con->line[t_con->line_cursor] = '\0';
+                                                               t_con->line_size = t_con->line_cursor;
+                                                       }
+                                               } else
                                                        LOG_DEBUG("unhandled nonprintable: %2.2x", *buf_p);
                                        }
                                }
@@ -568,6 +596,12 @@ static int telnet_input(struct connection *connection)
                                                telnet_history_up(connection);
                                        } else if (*buf_p == 'B') {     /* cursor down */
                                                telnet_history_down(connection);
+                                       } else if (*buf_p == 'F') { /* end key */
+                                               telnet_move_cursor(connection, t_con->line_size);
+                                               t_con->state = TELNET_STATE_DATA;
+                                       } else if (*buf_p == 'H') { /* home key */
+                                               telnet_move_cursor(connection, 0);
+                                               t_con->state = TELNET_STATE_DATA;
                                        } else if (*buf_p == '3')
                                                t_con->last_escape = *buf_p;
                                        else
@@ -627,29 +661,22 @@ static int telnet_connection_closed(struct connection *connection)
 
        log_remove_callback(telnet_log_callback, connection);
 
-       if (t_con->prompt) {
-               free(t_con->prompt);
-               t_con->prompt = NULL;
-       }
+       free(t_con->prompt);
+       t_con->prompt = NULL;
 
        /* save telnet history */
        telnet_save_history(t_con);
 
        for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++) {
-               if (t_con->history[i]) {
-                       free(t_con->history[i]);
-                       t_con->history[i] = NULL;
-               }
+               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");
+       free(connection->priv);
+       connection->priv = NULL;
 
        return ERROR_OK;
 }
@@ -705,7 +732,7 @@ static const struct command_registration telnet_command_handlers[] = {
        {
                .name = "telnet_port",
                .handler = handle_telnet_port_command,
-               .mode = COMMAND_ANY,
+               .mode = COMMAND_CONFIG,
                .help = "Specify port on which to listen "
                        "for incoming telnet connections.  "
                        "Read help on 'gdb_port'.",
@@ -719,3 +746,8 @@ int telnet_register_commands(struct command_context *cmd_ctx)
        telnet_port = strdup("4444");
        return register_commands(cmd_ctx, NULL, telnet_command_handlers);
 }
+
+void telnet_service_free(void)
+{
+       free(telnet_port);
+}