telnet: allow hiding selected commands during auto-completion
[fw/openocd] / src / server / telnet_server.c
1 /***************************************************************************
2  *   Copyright (C) 2005 by Dominic Rath                                    *
3  *   Dominic.Rath@gmx.de                                                   *
4  *                                                                         *
5  *   Copyright (C) 2007-2010 Ã˜yvind Harboe                                 *
6  *   oyvind.harboe@zylin.com                                               *
7  *                                                                         *
8  *   Copyright (C) 2008 by Spencer Oliver                                  *
9  *   spen@spen-soft.co.uk                                                  *
10  *                                                                         *
11  *   This program is free software; you can redistribute it and/or modify  *
12  *   it under the terms of the GNU General Public License as published by  *
13  *   the Free Software Foundation; either version 2 of the License, or     *
14  *   (at your option) any later version.                                   *
15  *                                                                         *
16  *   This program is distributed in the hope that it will be useful,       *
17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
19  *   GNU General Public License for more details.                          *
20  *                                                                         *
21  *   You should have received a copy of the GNU General Public License     *
22  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
23  ***************************************************************************/
24
25 #ifdef HAVE_CONFIG_H
26 #include "config.h"
27 #endif
28
29 #include "telnet_server.h"
30 #include <target/target_request.h>
31 #include <helper/configuration.h>
32 #include <helper/list.h>
33
34 static char *telnet_port;
35
36 static char *negotiate =
37         "\xFF\xFB\x03"                  /* IAC WILL Suppress Go Ahead */
38         "\xFF\xFB\x01"                  /* IAC WILL Echo */
39         "\xFF\xFD\x03"                  /* IAC DO Suppress Go Ahead */
40         "\xFF\xFE\x01";                 /* IAC DON'T Echo */
41
42 #define CTRL(c) (c - '@')
43 #define TELNET_HISTORY  ".openocd_history"
44
45 /* The only way we can detect that the socket is closed is the first time
46  * we write to it, we will fail. Subsequent write operations will
47  * succeed. Shudder!
48  */
49 static int telnet_write(struct connection *connection, const void *data,
50         int len)
51 {
52         struct telnet_connection *t_con = connection->priv;
53         if (t_con->closed)
54                 return ERROR_SERVER_REMOTE_CLOSED;
55
56         if (connection_write(connection, data, len) == len)
57                 return ERROR_OK;
58         t_con->closed = true;
59         return ERROR_SERVER_REMOTE_CLOSED;
60 }
61
62 /* output an audible bell */
63 static int telnet_bell(struct connection *connection)
64 {
65         /* ("\a" does not work, at least on windows) */
66         return telnet_write(connection, "\x07", 1);
67 }
68
69 static int telnet_prompt(struct connection *connection)
70 {
71         struct telnet_connection *t_con = connection->priv;
72
73         return telnet_write(connection, t_con->prompt, strlen(t_con->prompt));
74 }
75
76 static int telnet_outputline(struct connection *connection, const char *line)
77 {
78         int len;
79
80         /* process lines in buffer */
81         while (*line) {
82                 char *line_end = strchr(line, '\n');
83
84                 if (line_end)
85                         len = line_end-line;
86                 else
87                         len = strlen(line);
88
89                 telnet_write(connection, line, len);
90                 if (line_end) {
91                         telnet_write(connection, "\r\n", 2);
92                         line += len + 1;
93                 } else
94                         line += len;
95         }
96
97         return ERROR_OK;
98 }
99
100 static int telnet_output(struct command_context *cmd_ctx, const char *line)
101 {
102         struct connection *connection = cmd_ctx->output_handler_priv;
103
104         return telnet_outputline(connection, line);
105 }
106
107 static void telnet_log_callback(void *priv, const char *file, unsigned line,
108         const char *function, const char *string)
109 {
110         struct connection *connection = priv;
111         struct telnet_connection *t_con = connection->priv;
112         size_t i;
113         size_t tmp;
114
115         /* If the prompt is not visible, simply output the message. */
116         if (!t_con->prompt_visible) {
117                 telnet_outputline(connection, string);
118                 return;
119         }
120
121         /* Clear the command line. */
122         tmp = strlen(t_con->prompt) + t_con->line_size;
123
124         for (i = 0; i < tmp; i += 16)
125                 telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
126                         MIN(tmp - i, 16));
127
128         for (i = 0; i < tmp; i += 16)
129                 telnet_write(connection, "                ", MIN(tmp - i, 16));
130
131         for (i = 0; i < tmp; i += 16)
132                 telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
133                         MIN(tmp - i, 16));
134
135         telnet_outputline(connection, string);
136
137         /* Put the command line to its previous state. */
138         telnet_prompt(connection);
139         telnet_write(connection, t_con->line, t_con->line_size);
140
141         for (i = t_con->line_cursor; i < t_con->line_size; i++)
142                 telnet_write(connection, "\b", 1);
143 }
144
145 static void telnet_load_history(struct telnet_connection *t_con)
146 {
147         FILE *histfp;
148         char buffer[TELNET_BUFFER_SIZE];
149         int i = 0;
150
151         char *history = get_home_dir(TELNET_HISTORY);
152
153         if (history == NULL) {
154                 LOG_INFO("unable to get user home directory, telnet history will be disabled");
155                 return;
156         }
157
158         histfp = fopen(history, "rb");
159
160         if (histfp) {
161
162                 while (fgets(buffer, sizeof(buffer), histfp) != NULL) {
163
164                         char *p = strchr(buffer, '\n');
165                         if (p)
166                                 *p = '\0';
167                         if (buffer[0] && i < TELNET_LINE_HISTORY_SIZE)
168                                 t_con->history[i++] = strdup(buffer);
169                 }
170
171                 t_con->next_history = i;
172                 t_con->next_history %= TELNET_LINE_HISTORY_SIZE;
173                 /* try to set to last entry - 1, that way we skip over any exit/shutdown cmds */
174                 t_con->current_history = t_con->next_history > 0 ? i - 1 : 0;
175                 fclose(histfp);
176         }
177
178         free(history);
179 }
180
181 static void telnet_save_history(struct telnet_connection *t_con)
182 {
183         FILE *histfp;
184         int i;
185         int num;
186
187         char *history = get_home_dir(TELNET_HISTORY);
188
189         if (history == NULL) {
190                 LOG_INFO("unable to get user home directory, telnet history will be disabled");
191                 return;
192         }
193
194         histfp = fopen(history, "wb");
195
196         if (histfp) {
197
198                 num = TELNET_LINE_HISTORY_SIZE;
199                 i = t_con->current_history + 1;
200                 i %= TELNET_LINE_HISTORY_SIZE;
201
202                 while (t_con->history[i] == NULL && num > 0) {
203                         i++;
204                         i %= TELNET_LINE_HISTORY_SIZE;
205                         num--;
206                 }
207
208                 if (num > 0) {
209                         for (; num > 0; num--) {
210                                 fprintf(histfp, "%s\n", t_con->history[i]);
211                                 i++;
212                                 i %= TELNET_LINE_HISTORY_SIZE;
213                         }
214                 }
215                 fclose(histfp);
216         }
217
218         free(history);
219 }
220
221 static int telnet_new_connection(struct connection *connection)
222 {
223         struct telnet_connection *telnet_connection;
224         struct telnet_service *telnet_service = connection->service->priv;
225         int i;
226
227         telnet_connection = malloc(sizeof(struct telnet_connection));
228
229         if (!telnet_connection) {
230                 LOG_ERROR("Failed to allocate telnet connection.");
231                 return ERROR_FAIL;
232         }
233
234         connection->priv = telnet_connection;
235
236         /* initialize telnet connection information */
237         telnet_connection->closed = false;
238         telnet_connection->line_size = 0;
239         telnet_connection->line_cursor = 0;
240         telnet_connection->prompt = strdup("> ");
241         telnet_connection->prompt_visible = true;
242         telnet_connection->state = TELNET_STATE_DATA;
243
244         /* output goes through telnet connection */
245         command_set_output_handler(connection->cmd_ctx, telnet_output, connection);
246
247         /* negotiate telnet options */
248         telnet_write(connection, negotiate, strlen(negotiate));
249
250         /* print connection banner */
251         if (telnet_service->banner) {
252                 telnet_write(connection, telnet_service->banner, strlen(telnet_service->banner));
253                 telnet_write(connection, "\r\n", 2);
254         }
255
256         /* the prompt is always placed at the line beginning */
257         telnet_write(connection, "\r", 1);
258         telnet_prompt(connection);
259
260         /* initialize history */
261         for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
262                 telnet_connection->history[i] = NULL;
263         telnet_connection->next_history = 0;
264         telnet_connection->current_history = 0;
265         telnet_load_history(telnet_connection);
266
267         log_add_callback(telnet_log_callback, connection);
268
269         return ERROR_OK;
270 }
271
272 static void telnet_clear_line(struct connection *connection,
273         struct telnet_connection *t_con)
274 {
275         /* move to end of line */
276         if (t_con->line_cursor < t_con->line_size)
277                 telnet_write(connection,
278                         t_con->line + t_con->line_cursor,
279                         t_con->line_size - t_con->line_cursor);
280
281         /* backspace, overwrite with space, backspace */
282         while (t_con->line_size > 0) {
283                 telnet_write(connection, "\b \b", 3);
284                 t_con->line_size--;
285         }
286         t_con->line_cursor = 0;
287 }
288
289 static void telnet_history_go(struct connection *connection, int idx)
290 {
291         struct telnet_connection *t_con = connection->priv;
292
293         if (t_con->history[idx]) {
294                 telnet_clear_line(connection, t_con);
295                 t_con->line_size = strlen(t_con->history[idx]);
296                 t_con->line_cursor = t_con->line_size;
297                 memcpy(t_con->line, t_con->history[idx], t_con->line_size);
298                 telnet_write(connection, t_con->line, t_con->line_size);
299                 t_con->current_history = idx;
300         }
301         t_con->state = TELNET_STATE_DATA;
302 }
303
304 static void telnet_history_up(struct connection *connection)
305 {
306         struct telnet_connection *t_con = connection->priv;
307
308         size_t last_history = (t_con->current_history > 0) ?
309                                 t_con->current_history - 1 :
310                                 TELNET_LINE_HISTORY_SIZE-1;
311         telnet_history_go(connection, last_history);
312 }
313
314 static void telnet_history_down(struct connection *connection)
315 {
316         struct telnet_connection *t_con = connection->priv;
317         size_t next_history;
318
319         next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE;
320         telnet_history_go(connection, next_history);
321 }
322
323 static int telnet_history_print(struct connection *connection)
324 {
325         struct telnet_connection *tc;
326
327         tc = connection->priv;
328
329         for (size_t i = 1; i < TELNET_LINE_HISTORY_SIZE; i++) {
330                 char *line;
331
332                 /*
333                  * The tc->next_history line contains empty string (unless NULL), thus
334                  * it is not printed.
335                  */
336                 line = tc->history[(tc->next_history + i) % TELNET_LINE_HISTORY_SIZE];
337
338                 if (line) {
339                         telnet_write(connection, line, strlen(line));
340                         telnet_write(connection, "\r\n\x00", 3);
341                 }
342         }
343
344         tc->line_size = 0;
345         tc->line_cursor = 0;
346
347         /* The prompt is always placed at the line beginning. */
348         telnet_write(connection, "\r", 1);
349
350         return telnet_prompt(connection);
351 }
352
353 static void telnet_move_cursor(struct connection *connection, size_t pos)
354 {
355         struct telnet_connection *tc;
356         size_t tmp;
357
358         tc = connection->priv;
359
360         if (pos < tc->line_cursor) {
361                 tmp = tc->line_cursor - pos;
362
363                 for (size_t i = 0; i < tmp; i += 16)
364                         telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
365                                 MIN(tmp - i, 16));
366         } else {
367                 tmp = pos - tc->line_cursor;
368
369                 for (size_t i = 0; i < tmp; i += 16)
370                         telnet_write(connection, tc->line + tc->line_cursor + i,
371                                 MIN(tmp - i, 16));
372         }
373
374         tc->line_cursor = pos;
375 }
376
377 /* check buffer size leaving one spare character for string null termination */
378 static inline bool telnet_can_insert(struct connection *connection, size_t len)
379 {
380         struct telnet_connection *t_con = connection->priv;
381
382         return t_con->line_size + len < TELNET_LINE_MAX_SIZE;
383 }
384
385 /* write to telnet console, and update the telnet_connection members
386  * this function is capable of inserting in the middle of a line
387  * please ensure that data does not contain special characters (\n, \r, \t, \b ...)
388  *
389  * returns false when it fails to insert the requested data
390  */
391 static bool telnet_insert(struct connection *connection, const void *data, size_t len)
392 {
393         struct telnet_connection *t_con = connection->priv;
394
395         if (!telnet_can_insert(connection, len)) {
396                 telnet_bell(connection);
397                 return false;
398         }
399
400         if (t_con->line_cursor < t_con->line_size) {
401                 /* we have some content after the cursor */
402                 memmove(t_con->line + t_con->line_cursor + len,
403                                 t_con->line + t_con->line_cursor,
404                                 t_con->line_size - t_con->line_cursor);
405         }
406
407         strncpy(t_con->line + t_con->line_cursor, data, len);
408
409         telnet_write(connection,
410                         t_con->line + t_con->line_cursor,
411                         t_con->line_size + len - t_con->line_cursor);
412
413         t_con->line_size += len;
414         t_con->line_cursor += len;
415
416         for (size_t i = t_con->line_cursor; i < t_con->line_size; i++)
417                 telnet_write(connection, "\b", 1);
418
419         return true;
420 }
421
422 static void telnet_auto_complete(struct connection *connection)
423 {
424         struct telnet_connection *t_con = connection->priv;
425         struct command_context *command_context = connection->cmd_ctx;
426
427         struct cmd_match {
428                 char *cmd;
429                 struct list_head lh;
430         };
431
432         LIST_HEAD(matches);
433
434         /* user command sequence, either at line beginning
435          * or we start over after these characters ';', '[', '{' */
436         size_t seq_start = (t_con->line_cursor == 0) ? 0 : (t_con->line_cursor - 1);
437         while (seq_start > 0) {
438                 char c = t_con->line[seq_start];
439                 if (c == ';' || c == '[' || c == '{') {
440                         seq_start++;
441                         break;
442                 }
443
444                 seq_start--;
445         }
446
447         /* user command position in the line, ignore leading spaces */
448         size_t usr_cmd_pos = seq_start;
449         while ((usr_cmd_pos < t_con->line_cursor) && isspace(t_con->line[usr_cmd_pos]))
450                 usr_cmd_pos++;
451
452         /* user command length */
453         size_t usr_cmd_len = t_con->line_cursor - usr_cmd_pos;
454
455         /* optimize multiple spaces in the user command,
456          * because info commands does not tolerate multiple spaces */
457         size_t optimized_spaces = 0;
458         char query[usr_cmd_len + 1];
459         for (size_t i = 0; i < usr_cmd_len; i++) {
460                 if ((i < usr_cmd_len - 1) && isspace(t_con->line[usr_cmd_pos + i])
461                                 && isspace(t_con->line[usr_cmd_pos + i + 1])) {
462                         optimized_spaces++;
463                         continue;
464                 }
465
466                 query[i - optimized_spaces] = t_con->line[usr_cmd_pos + i];
467         }
468
469         usr_cmd_len -= optimized_spaces;
470         query[usr_cmd_len] = '\0';
471
472         /* filter commands */
473         char *query_cmd = alloc_printf("_telnet_autocomplete_helper {%s*}", query);
474
475         if (!query_cmd) {
476                 LOG_ERROR("Out of memory");
477                 return;
478         }
479
480         int retval = Jim_EvalSource(command_context->interp, __FILE__, __LINE__, query_cmd);
481         free(query_cmd);
482         if (retval != JIM_OK)
483                 return;
484
485         Jim_Obj *list = Jim_GetResult(command_context->interp);
486         Jim_IncrRefCount(list);
487
488         /* common prefix length of the matched commands */
489         size_t common_len = 0;
490         char *first_match = NULL; /* used to compute the common prefix length */
491
492         int len = Jim_ListLength(command_context->interp, list);
493         for (int i = 0; i < len; i++) {
494                 Jim_Obj *elem = Jim_ListGetIndex(command_context->interp, list, i);
495                 Jim_IncrRefCount(elem);
496
497                 char *name = (char *)Jim_GetString(elem, NULL);
498
499                 /* validate the command */
500                 bool ignore_cmd = false;
501                 Jim_Cmd *jim_cmd = Jim_GetCommand(command_context->interp, elem, JIM_NONE);
502
503                 if (!jim_cmd)
504                         ignore_cmd = true;
505                 else {
506                         if (!jim_cmd->isproc) {
507                                 /* ignore commands without handler
508                                  * and those with COMMAND_CONFIG mode */
509                                 /* FIXME it's better to use jimcmd_is_ocd_command(jim_cmd)
510                                  * or command_find_from_name(command_context->interp, name) */
511                                 struct command *cmd = jim_cmd->u.native.privData;
512                                 if (!cmd)
513                                         ignore_cmd = true;
514                                 /* make Valgrind happy by checking that cmd is not NULL  */
515                                 else if (cmd != NULL && !cmd->handler && !cmd->jim_handler)
516                                         ignore_cmd = true;
517                                 else if (cmd != NULL && cmd->mode == COMMAND_CONFIG)
518                                         ignore_cmd = true;
519                         }
520                 }
521
522                 /* save the command in the prediction list */
523                 if (!ignore_cmd) {
524                         struct cmd_match *match = calloc(1, sizeof(struct cmd_match));
525                         if (!match) {
526                                 LOG_ERROR("Out of memory");
527                                 Jim_DecrRefCount(command_context->interp, elem);
528                                 break; /* break the for loop */
529                         }
530
531                         if (list_empty(&matches)) {
532                                 common_len = strlen(name);
533                                 first_match = name;
534                         } else {
535                                 size_t new_common_len = usr_cmd_len; /* save some loops */
536
537                                 while (new_common_len < common_len && first_match[new_common_len] == name[new_common_len])
538                                         new_common_len++;
539
540                                 common_len = new_common_len;
541                         }
542
543                         match->cmd = name;
544                         list_add_tail(&match->lh, &matches);
545                 }
546
547                 Jim_DecrRefCount(command_context->interp, elem);
548         }
549         /* end of command filtering */
550
551         /* proceed with auto-completion */
552         if (list_empty(&matches))
553                 telnet_bell(connection);
554         else if (common_len == usr_cmd_len && list_is_singular(&matches) && t_con->line_cursor == t_con->line_size)
555                 telnet_insert(connection, " ", 1);
556         else if (common_len > usr_cmd_len) {
557                 int completion_size = common_len - usr_cmd_len;
558                 if (telnet_insert(connection, first_match + usr_cmd_len, completion_size)) {
559                         /* in bash this extra space is only added when the cursor in at the end of line */
560                         if (list_is_singular(&matches) && t_con->line_cursor == t_con->line_size)
561                                 telnet_insert(connection, " ", 1);
562                 }
563         } else if (!list_is_singular(&matches)) {
564                 telnet_write(connection, "\n\r", 2);
565
566                 struct cmd_match *match;
567                 list_for_each_entry(match, &matches, lh) {
568                         telnet_write(connection, match->cmd, strlen(match->cmd));
569                         telnet_write(connection, "\n\r", 2);
570                 }
571
572                 telnet_prompt(connection);
573                 telnet_write(connection, t_con->line, t_con->line_size);
574
575                 /* restore the terminal visible cursor location */
576                 for (size_t i = t_con->line_cursor; i < t_con->line_size; i++)
577                         telnet_write(connection, "\b", 1);
578         }
579
580         /* destroy the command_list */
581         struct cmd_match *tmp, *match;
582         list_for_each_entry_safe(match, tmp, &matches, lh)
583                 free(match);
584
585         Jim_DecrRefCount(command_context->interp, list);
586 }
587
588 static int telnet_input(struct connection *connection)
589 {
590         int bytes_read;
591         unsigned char buffer[TELNET_BUFFER_SIZE];
592         unsigned char *buf_p;
593         struct telnet_connection *t_con = connection->priv;
594         struct command_context *command_context = connection->cmd_ctx;
595
596         bytes_read = connection_read(connection, buffer, TELNET_BUFFER_SIZE);
597
598         if (bytes_read == 0)
599                 return ERROR_SERVER_REMOTE_CLOSED;
600         else if (bytes_read == -1) {
601                 LOG_ERROR("error during read: %s", strerror(errno));
602                 return ERROR_SERVER_REMOTE_CLOSED;
603         }
604
605         buf_p = buffer;
606         while (bytes_read) {
607                 switch (t_con->state) {
608                         case TELNET_STATE_DATA:
609                                 if (*buf_p == 0xff)
610                                         t_con->state = TELNET_STATE_IAC;
611                                 else {
612                                         if (isprint(*buf_p)) {  /* printable character */
613                                                 telnet_insert(connection, buf_p, 1);
614                                         } else {        /* non-printable */
615                                                 if (*buf_p == 0x1b) {   /* escape */
616                                                         t_con->state = TELNET_STATE_ESCAPE;
617                                                         t_con->last_escape = '\x00';
618                                                 } else if ((*buf_p == 0xd) || (*buf_p == 0xa)) {        /* CR/LF */
619                                                         int retval;
620
621                                                         /* skip over combinations with CR/LF and NUL characters */
622                                                         if ((bytes_read > 1) && ((*(buf_p + 1) == 0xa) ||
623                                                                         (*(buf_p + 1) == 0xd))) {
624                                                                 buf_p++;
625                                                                 bytes_read--;
626                                                         }
627                                                         if ((bytes_read > 1) && (*(buf_p + 1) == 0)) {
628                                                                 buf_p++;
629                                                                 bytes_read--;
630                                                         }
631                                                         t_con->line[t_con->line_size] = 0;
632
633                                                         telnet_write(connection, "\r\n\x00", 3);
634
635                                                         if (strcmp(t_con->line, "history") == 0) {
636                                                                 retval = telnet_history_print(connection);
637
638                                                                 if (retval != ERROR_OK)
639                                                                         return retval;
640
641                                                                 continue;
642                                                         }
643
644                                                         /* save only non-blank not repeating lines in the history */
645                                                         char *prev_line = t_con->history[(t_con->current_history > 0) ?
646                                                                         t_con->current_history - 1 : TELNET_LINE_HISTORY_SIZE-1];
647                                                         if (*t_con->line && (prev_line == NULL ||
648                                                                         strcmp(t_con->line, prev_line))) {
649                                                                 /* if the history slot is already taken, free it */
650                                                                 free(t_con->history[t_con->next_history]);
651
652                                                                 /* add line to history */
653                                                                 t_con->history[t_con->next_history] = strdup(t_con->line);
654
655                                                                 /* wrap history at TELNET_LINE_HISTORY_SIZE */
656                                                                 t_con->next_history = (t_con->next_history + 1) %
657                                                                                 TELNET_LINE_HISTORY_SIZE;
658
659                                                                 /* current history line starts at the new entry */
660                                                                 t_con->current_history =
661                                                                                 t_con->next_history;
662
663                                                                 free(t_con->history[t_con->current_history]);
664                                                                 t_con->history[t_con->current_history] = strdup("");
665                                                         }
666
667                                                         t_con->line_size = 0;
668
669                                                         /* to suppress prompt in log callback during command execution */
670                                                         t_con->prompt_visible = false;
671
672                                                         if (strcmp(t_con->line, "shutdown") == 0)
673                                                                 telnet_save_history(t_con);
674
675                                                         retval = command_run_line(command_context, t_con->line);
676
677                                                         t_con->line_cursor = 0;
678                                                         t_con->prompt_visible = true;
679
680                                                         if (retval == ERROR_COMMAND_CLOSE_CONNECTION)
681                                                                 return ERROR_SERVER_REMOTE_CLOSED;
682
683                                                         /* the prompt is always * placed at the line beginning */
684                                                         telnet_write(connection, "\r", 1);
685
686                                                         retval = telnet_prompt(connection);
687                                                         if (retval == ERROR_SERVER_REMOTE_CLOSED)
688                                                                 return ERROR_SERVER_REMOTE_CLOSED;
689
690                                                 } else if ((*buf_p == 0x7f) || (*buf_p == 0x8)) {       /* delete character */
691                                                         if (t_con->line_cursor > 0) {
692                                                                 if (t_con->line_cursor != t_con->line_size) {
693                                                                         size_t i;
694                                                                         telnet_write(connection, "\b", 1);
695                                                                         t_con->line_cursor--;
696                                                                         t_con->line_size--;
697                                                                         memmove(t_con->line + t_con->line_cursor,
698                                                                                         t_con->line + t_con->line_cursor + 1,
699                                                                                         t_con->line_size -
700                                                                                         t_con->line_cursor);
701
702                                                                         telnet_write(connection,
703                                                                                         t_con->line + t_con->line_cursor,
704                                                                                         t_con->line_size -
705                                                                                         t_con->line_cursor);
706                                                                         telnet_write(connection, " \b", 2);
707                                                                         for (i = t_con->line_cursor; i < t_con->line_size; i++)
708                                                                                 telnet_write(connection, "\b", 1);
709                                                                 } else {
710                                                                         t_con->line_size--;
711                                                                         t_con->line_cursor--;
712                                                                         /* back space: move the 'printer' head one char
713                                                                          * back, overwrite with space, move back again */
714                                                                         telnet_write(connection, "\b \b", 3);
715                                                                 }
716                                                         }
717                                                 } else if (*buf_p == 0x15) /* clear line */
718                                                         telnet_clear_line(connection, t_con);
719                                                 else if (*buf_p == CTRL('B')) { /* cursor left */
720                                                         if (t_con->line_cursor > 0) {
721                                                                 telnet_write(connection, "\b", 1);
722                                                                 t_con->line_cursor--;
723                                                         }
724                                                         t_con->state = TELNET_STATE_DATA;
725                                                 } else if (*buf_p == CTRL('F')) {       /* cursor right */
726                                                         if (t_con->line_cursor < t_con->line_size)
727                                                                 telnet_write(connection, t_con->line + t_con->line_cursor++, 1);
728                                                         t_con->state = TELNET_STATE_DATA;
729                                                 } else if (*buf_p == CTRL('P'))         /* cursor up */
730                                                         telnet_history_up(connection);
731                                                 else if (*buf_p == CTRL('N'))           /* cursor down */
732                                                         telnet_history_down(connection);
733                                                 else if (*buf_p == CTRL('A'))
734                                                         telnet_move_cursor(connection, 0);
735                                                 else if (*buf_p == CTRL('E'))
736                                                         telnet_move_cursor(connection, t_con->line_size);
737                                                 else if (*buf_p == CTRL('K')) {         /* kill line to end */
738                                                         if (t_con->line_cursor < t_con->line_size) {
739                                                                 /* overwrite with space, until end of line, move back */
740                                                                 for (size_t i = t_con->line_cursor; i < t_con->line_size; i++)
741                                                                         telnet_write(connection, " ", 1);
742                                                                 for (size_t i = t_con->line_cursor; i < t_con->line_size; i++)
743                                                                         telnet_write(connection, "\b", 1);
744                                                                 t_con->line[t_con->line_cursor] = '\0';
745                                                                 t_con->line_size = t_con->line_cursor;
746                                                         }
747                                                 } else if (*buf_p == '\t')
748                                                         telnet_auto_complete(connection);
749                                                 else
750                                                         LOG_DEBUG("unhandled nonprintable: %2.2x", *buf_p);
751                                         }
752                                 }
753                                 break;
754                         case TELNET_STATE_IAC:
755                                 switch (*buf_p) {
756                                 case 0xfe:
757                                         t_con->state = TELNET_STATE_DONT;
758                                         break;
759                                 case 0xfd:
760                                         t_con->state = TELNET_STATE_DO;
761                                         break;
762                                 case 0xfc:
763                                         t_con->state = TELNET_STATE_WONT;
764                                         break;
765                                 case 0xfb:
766                                         t_con->state = TELNET_STATE_WILL;
767                                         break;
768                                 }
769                                 break;
770                         case TELNET_STATE_SB:
771                                 break;
772                         case TELNET_STATE_SE:
773                                 break;
774                         case TELNET_STATE_WILL:
775                         case TELNET_STATE_WONT:
776                         case TELNET_STATE_DO:
777                         case TELNET_STATE_DONT:
778                                 t_con->state = TELNET_STATE_DATA;
779                                 break;
780                         case TELNET_STATE_ESCAPE:
781                                 if (t_con->last_escape == '[') {
782                                         if (*buf_p == 'D') {    /* cursor left */
783                                                 if (t_con->line_cursor > 0) {
784                                                         telnet_write(connection, "\b", 1);
785                                                         t_con->line_cursor--;
786                                                 }
787                                                 t_con->state = TELNET_STATE_DATA;
788                                         } else if (*buf_p == 'C') {     /* cursor right */
789                                                 if (t_con->line_cursor < t_con->line_size)
790                                                         telnet_write(connection,
791                                                                         t_con->line + t_con->line_cursor++, 1);
792                                                 t_con->state = TELNET_STATE_DATA;
793                                         } else if (*buf_p == 'A') {     /* cursor up */
794                                                 telnet_history_up(connection);
795                                         } else if (*buf_p == 'B') {     /* cursor down */
796                                                 telnet_history_down(connection);
797                                         } else if (*buf_p == 'F') { /* end key */
798                                                 telnet_move_cursor(connection, t_con->line_size);
799                                                 t_con->state = TELNET_STATE_DATA;
800                                         } else if (*buf_p == 'H') { /* home key */
801                                                 telnet_move_cursor(connection, 0);
802                                                 t_con->state = TELNET_STATE_DATA;
803                                         } else if (*buf_p == '3')
804                                                 t_con->last_escape = *buf_p;
805                                         else
806                                                 t_con->state = TELNET_STATE_DATA;
807                                 } else if (t_con->last_escape == '3') {
808                                         /* Remove character */
809                                         if (*buf_p == '~') {
810                                                 if (t_con->line_cursor < t_con->line_size) {
811                                                         size_t i;
812                                                         t_con->line_size--;
813                                                         /* remove char from line buffer */
814                                                         memmove(t_con->line + t_con->line_cursor,
815                                                                         t_con->line + t_con->line_cursor + 1,
816                                                                         t_con->line_size - t_con->line_cursor);
817
818                                                         /* print remainder of buffer */
819                                                         telnet_write(connection, t_con->line + t_con->line_cursor,
820                                                                         t_con->line_size - t_con->line_cursor);
821                                                         /* overwrite last char with whitespace */
822                                                         telnet_write(connection, " \b", 2);
823
824                                                         /* move back to cursor position*/
825                                                         for (i = t_con->line_cursor; i < t_con->line_size; i++)
826                                                                 telnet_write(connection, "\b", 1);
827                                                 }
828
829                                                 t_con->state = TELNET_STATE_DATA;
830                                         } else
831                                                 t_con->state = TELNET_STATE_DATA;
832                                 } else if (t_con->last_escape == '\x00') {
833                                         if (*buf_p == '[')
834                                                 t_con->last_escape = *buf_p;
835                                         else
836                                                 t_con->state = TELNET_STATE_DATA;
837                                 } else {
838                                         LOG_ERROR("BUG: unexpected value in t_con->last_escape");
839                                         t_con->state = TELNET_STATE_DATA;
840                                 }
841
842                                 break;
843                         default:
844                                 LOG_ERROR("unknown telnet state");
845                                 return ERROR_FAIL;
846                 }
847
848                 bytes_read--;
849                 buf_p++;
850         }
851
852         return ERROR_OK;
853 }
854
855 static int telnet_connection_closed(struct connection *connection)
856 {
857         struct telnet_connection *t_con = connection->priv;
858         int i;
859
860         log_remove_callback(telnet_log_callback, connection);
861
862         free(t_con->prompt);
863         t_con->prompt = NULL;
864
865         /* save telnet history */
866         telnet_save_history(t_con);
867
868         for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++) {
869                 free(t_con->history[i]);
870                 t_con->history[i] = NULL;
871         }
872
873         /* if this connection registered a debug-message receiver delete it */
874         delete_debug_msg_receiver(connection->cmd_ctx, NULL);
875
876         free(connection->priv);
877         connection->priv = NULL;
878
879         return ERROR_OK;
880 }
881
882 int telnet_init(char *banner)
883 {
884         if (strcmp(telnet_port, "disabled") == 0) {
885                 LOG_INFO("telnet server disabled");
886                 return ERROR_OK;
887         }
888
889         struct telnet_service *telnet_service =
890                 malloc(sizeof(struct telnet_service));
891
892         if (!telnet_service) {
893                 LOG_ERROR("Failed to allocate telnet service.");
894                 return ERROR_FAIL;
895         }
896
897         telnet_service->banner = banner;
898
899         int ret = add_service("telnet", telnet_port, CONNECTION_LIMIT_UNLIMITED,
900                 telnet_new_connection, telnet_input, telnet_connection_closed,
901                 telnet_service);
902
903         if (ret != ERROR_OK) {
904                 free(telnet_service);
905                 return ret;
906         }
907
908         return ERROR_OK;
909 }
910
911 /* daemon configuration command telnet_port */
912 COMMAND_HANDLER(handle_telnet_port_command)
913 {
914         return CALL_COMMAND_HANDLER(server_pipe_command, &telnet_port);
915 }
916
917 COMMAND_HANDLER(handle_exit_command)
918 {
919         return ERROR_COMMAND_CLOSE_CONNECTION;
920 }
921
922 static const struct command_registration telnet_command_handlers[] = {
923         {
924                 .name = "exit",
925                 .handler = handle_exit_command,
926                 .mode = COMMAND_EXEC,
927                 .usage = "",
928                 .help = "exit telnet session",
929         },
930         {
931                 .name = "telnet_port",
932                 .handler = handle_telnet_port_command,
933                 .mode = COMMAND_CONFIG,
934                 .help = "Specify port on which to listen "
935                         "for incoming telnet connections.  "
936                         "Read help on 'gdb_port'.",
937                 .usage = "[port_num]",
938         },
939         COMMAND_REGISTRATION_DONE
940 };
941
942 int telnet_register_commands(struct command_context *cmd_ctx)
943 {
944         telnet_port = strdup("4444");
945         return register_commands(cmd_ctx, NULL, telnet_command_handlers);
946 }
947
948 void telnet_service_free(void)
949 {
950         free(telnet_port);
951 }