server/telnet: Handle Ctrl+A and Ctrl+E

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

diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index 2187dbe280bdf71585ad2905acd40cd5f6194879..06a9c7a6c74a74597d2f5f5626f5a9f90eabc932 100644 (file)
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -19,9 +19,7 @@
  *   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.,                                       *
- *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
@@ -207,17 +205,23 @@ static void telnet_save_history(struct telnet_connection *t_con)
 
 static int telnet_new_connection(struct connection *connection)
 {
-       struct telnet_connection *telnet_connection = malloc(sizeof(struct telnet_connection));
+       struct telnet_connection *telnet_connection;
        struct telnet_service *telnet_service = connection->service->priv;
        int i;
 
+       telnet_connection = malloc(sizeof(struct telnet_connection));
+
+       if (!telnet_connection) {
+               LOG_ERROR("Failed to allocate telnet connection.");
+               return ERROR_FAIL;
+       }
+
        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;
 
@@ -299,6 +303,30 @@ static void telnet_history_down(struct connection *connection)
        telnet_history_go(connection, next_history);
 }
 
+static void telnet_move_cursor(struct connection *connection, size_t pos)
+{
+       struct telnet_connection *tc;
+       size_t tmp;
+
+       tc = connection->priv;
+
+       if (pos < tc->line_cursor) {
+               tmp = tc->line_cursor - pos;
+
+               for (size_t i = 0; i < tmp; i += 16)
+                       telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
+                               MIN(tmp - i, 16));
+       } else {
+               tmp = pos - tc->line_cursor;
+
+               for (size_t i = 0; i < tmp; i += 16)
+                       telnet_write(connection, tc->line + tc->line_cursor + i,
+                               MIN(tmp - i, 16));
+       }
+
+       tc->line_cursor = pos;
+}
+
 static int telnet_input(struct connection *connection)
 {
        int bytes_read;
@@ -478,6 +506,10 @@ static int telnet_input(struct connection *connection)
                                                        telnet_history_up(connection);
                                                else if (*buf_p == CTRL('N'))           /* cursor down */
                                                        telnet_history_down(connection);
+                                               else if (*buf_p == CTRL('A'))
+                                                       telnet_move_cursor(connection, 0);
+                                               else if (*buf_p == CTRL('E'))
+                                                       telnet_move_cursor(connection, t_con->line_size);
                                                else
                                                        LOG_DEBUG("unhandled nonprintable: %2.2x", *buf_p);
                                        }
@@ -568,7 +600,7 @@ static int telnet_input(struct connection *connection)
                                break;
                        default:
                                LOG_ERROR("unknown telnet state");
-                               exit(-1);
+                               return ERROR_FAIL;
                }
 
                bytes_read--;
@@ -619,17 +651,26 @@ int telnet_init(char *banner)
                return ERROR_OK;
        }
 
-       struct telnet_service *telnet_service = malloc(sizeof(struct telnet_service));
+       struct telnet_service *telnet_service =
+               malloc(sizeof(struct telnet_service));
+
+       if (!telnet_service) {
+               LOG_ERROR("Failed to allocate telnet service.");
+               return ERROR_FAIL;
+       }
 
        telnet_service->banner = banner;
 
-       return add_service("telnet",
-               telnet_port,
-               CONNECTION_LIMIT_UNLIMITED,
-               telnet_new_connection,
-               telnet_input,
-               telnet_connection_closed,
+       int ret = add_service("telnet", telnet_port, CONNECTION_LIMIT_UNLIMITED,
+               telnet_new_connection, telnet_input, telnet_connection_closed,
                telnet_service);
+
+       if (ret != ERROR_OK) {
+               free(telnet_service);
+               return ret;
+       }
+
+       return ERROR_OK;
 }
 
 /* daemon configuration command telnet_port */