- reapply fix with telnet prompt while running/halting gdb
[fw/openocd] / src / server / telnet_server.c
1 /***************************************************************************
2  *   Copyright (C) 2005 by Dominic Rath                                    *
3  *   Dominic.Rath@gmx.de                                                   *
4  *                                                                         *
5  *   This program is free software; you can redistribute it and/or modify  *
6  *   it under the terms of the GNU General Public License as published by  *
7  *   the Free Software Foundation; either version 2 of the License, or     *
8  *   (at your option) any later version.                                   *
9  *                                                                         *
10  *   This program is distributed in the hope that it will be useful,       *
11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
13  *   GNU General Public License for more details.                          *
14  *                                                                         *
15  *   You should have received a copy of the GNU General Public License     *
16  *   along with this program; if not, write to the                         *
17  *   Free Software Foundation, Inc.,                                       *
18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
19  ***************************************************************************/
20 #ifdef HAVE_CONFIG_H
21 #include "config.h"
22 #endif
23
24 #include "replacements.h"
25
26 #include "telnet_server.h"
27
28 #include "server.h"
29 #include "log.h"
30 #include "command.h"
31 #include "target.h"
32 #include "target_request.h"
33
34 #include <stdlib.h>
35 #include <unistd.h>
36 #include <errno.h>
37 #include <string.h>
38 #include <ctype.h>
39
40 static unsigned short telnet_port = 0;
41
42 int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
43 int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
44
45 static char *negotiate =
46                 "\xFF\xFB\x03"          /* IAC WILL Suppress Go Ahead */
47                 "\xFF\xFB\x01"          /* IAC WILL Echo */
48                 "\xFF\xFD\x03"          /* IAC DO Suppress Go Ahead */
49                 "\xFF\xFE\x01";         /* IAC DON'T Echo */
50                 
51 #define CTRL(c) (c - '@')
52         
53 /* The only way we can detect that the socket is closed is the first time
54  * we write to it, we will fail. Subsequent write operations will
55  * succeed. Shudder!
56  */
57 int telnet_write(connection_t *connection, void *data, int len)
58 {
59         telnet_connection_t *t_con = connection->priv;
60         if (t_con->closed)
61                 return ERROR_SERVER_REMOTE_CLOSED;
62
63         if (write_socket(connection->fd, data, len) == len)
64         {
65                 return ERROR_OK;
66         }
67         t_con->closed = 1;
68         return ERROR_SERVER_REMOTE_CLOSED;
69 }
70
71 int telnet_prompt(connection_t *connection)
72 {
73         telnet_connection_t *t_con = connection->priv;
74
75         return telnet_write(connection, t_con->prompt, strlen(t_con->prompt));
76 }
77
78 int telnet_outputline(connection_t *connection, char* line)
79 {
80         telnet_write(connection, line, strlen(line));
81         return telnet_write(connection, "\r\n\0", 3);
82 }
83
84 int telnet_output(struct command_context_s *cmd_ctx, char* line)
85 {
86         connection_t *connection = cmd_ctx->output_handler_priv;
87         
88         return telnet_outputline(connection, line);
89 }
90
91 void telnet_log_callback(void *priv, const char *file, int line, 
92                 const char *function, const char *format, va_list args)
93 {
94         connection_t *connection = priv;
95         char *t = allocPrintf(format, args);
96         if (t == NULL)
97                 return;
98         
99         telnet_outputline(connection, t);
100         
101         free(t);
102 }
103
104 int telnet_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
105 {
106         struct command_context_s *cmd_ctx = priv;
107         connection_t *connection = cmd_ctx->output_handler_priv;
108         telnet_connection_t *t_con = connection->priv;
109         char buffer[512];
110         
111         switch (event)
112         {
113                 case TARGET_EVENT_HALTED:
114                         command_print(cmd_ctx, "Target %i halted", get_num_by_target(target));
115                         target->type->arch_state(target, buffer, 512);
116                         buffer[511] = 0;
117                         command_print(cmd_ctx, "%s", buffer);
118                         if (!t_con->suppress_prompt)
119                                 telnet_prompt(connection);
120                         break;
121                 case TARGET_EVENT_RESUMED:
122                         command_print(cmd_ctx, "Target %i resumed", get_num_by_target(target));
123                         if (!t_con->suppress_prompt)
124                                 telnet_prompt(connection);
125                         break;
126                 default:
127                         break;
128         }
129
130         return ERROR_OK;
131 }
132
133 int telnet_new_connection(connection_t *connection)
134 {
135         telnet_connection_t *telnet_connection = malloc(sizeof(telnet_connection_t));
136         telnet_service_t *telnet_service = connection->service->priv;
137         int i;
138         
139         connection->priv = telnet_connection;
140         
141         /* initialize telnet connection information */
142         telnet_connection->closed = 0;
143         telnet_connection->line_size = 0;
144         telnet_connection->line_cursor = 0;
145         telnet_connection->option_size = 0;
146         telnet_connection->prompt = strdup("> ");
147         telnet_connection->suppress_prompt = 0;
148         telnet_connection->state = TELNET_STATE_DATA;
149         
150         /* output goes through telnet connection */
151         command_set_output_handler(connection->cmd_ctx, telnet_output, connection);
152         
153         /* negotiate telnet options */
154         telnet_write(connection, negotiate, strlen(negotiate));
155         
156         /* print connection banner */
157         if (telnet_service->banner)
158         {
159                 telnet_write(connection, telnet_service->banner, strlen(telnet_service->banner));
160                 telnet_write(connection, "\r\n\0", 3);
161         }
162         
163         telnet_prompt(connection);
164         
165         /* initialize history */
166         for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
167         {
168                 telnet_connection->history[i] = NULL;
169         }
170         telnet_connection->next_history = 0;
171         telnet_connection->current_history = 0;
172
173         target_register_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx);
174         
175         return ERROR_OK;
176 }
177
178 void telnet_clear_line(connection_t *connection, telnet_connection_t *t_con)
179 {
180         /* move to end of line */
181         if (t_con->line_cursor < t_con->line_size)
182         {
183                 telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
184         }
185                                                         
186         /* backspace, overwrite with space, backspace */
187         while (t_con->line_size > 0)
188         {
189                 telnet_write(connection, "\b \b", 3);
190                 t_con->line_size--;
191         }
192         t_con->line_cursor = 0;
193 }
194
195 int telnet_input(connection_t *connection)
196 {
197         int bytes_read;
198         char buffer[TELNET_BUFFER_SIZE];
199         char *buf_p;
200         telnet_connection_t *t_con = connection->priv;
201         command_context_t *command_context = connection->cmd_ctx;
202         
203         bytes_read = read_socket(connection->fd, buffer, TELNET_BUFFER_SIZE);
204         
205         if (bytes_read == 0)
206                 return ERROR_SERVER_REMOTE_CLOSED;
207         else if (bytes_read == -1)
208         {
209                 ERROR("error during read: %s", strerror(errno));
210                 return ERROR_SERVER_REMOTE_CLOSED;
211         }
212         
213         buf_p = buffer;
214         while (bytes_read)
215         {
216                 switch (t_con->state)
217                 {
218                         case TELNET_STATE_DATA:
219                                 if (*buf_p == '\xff')
220                                 {
221                                         t_con->state = TELNET_STATE_IAC;
222                                 }
223                                 else
224                                 {
225                                         if (isprint(*buf_p)) /* printable character */
226                                         {
227                                                 telnet_write(connection, buf_p, 1);
228                                                 if (t_con->line_cursor == t_con->line_size)
229                                                 {
230                                                         t_con->line[t_con->line_size++] = *buf_p;
231                                                         t_con->line_cursor++;
232                                                 }
233                                                 else
234                                                 {
235                                                         int i;
236                                                         memmove(t_con->line + t_con->line_cursor + 1, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
237                                                         t_con->line[t_con->line_cursor++] = *buf_p;
238                                                         t_con->line_size++;
239                                                         telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
240                                                         for (i = t_con->line_cursor; i < t_con->line_size; i++)
241                                                         {
242                                                                 telnet_write(connection, "\b", 1);
243                                                         }
244                                                 }
245                                         }
246                                         else /* non-printable */
247                                         {
248                                                 if (*buf_p == 0x1b) /* escape */
249                                                 {
250                                                         t_con->state = TELNET_STATE_ESCAPE;
251                                                         t_con->last_escape = '\x00';
252                                                 }
253                                                 else if ((*buf_p == 0xd) || (*buf_p == 0xa)) /* CR/LF */
254                                                 {
255                                                         int retval;
256                                                         
257                                                         /* skip over combinations with CR/LF + NUL */
258                                                         if (((*(buf_p + 1) == 0xa) || (*(buf_p + 1) == 0xd)) && (bytes_read > 1))
259                                                         {
260                                                                 buf_p++;
261                                                                 bytes_read--;
262                                                         }
263                                                         if ((*(buf_p + 1) == 0) && (bytes_read > 1))
264                                                         {
265                                                                 buf_p++;
266                                                                 bytes_read--;
267                                                         }
268                                                         t_con->line[t_con->line_size] = 0;
269                                                         
270                                                         telnet_write(connection, "\r\n\x00", 3);
271                                                         
272                                                         if (strcmp(t_con->line, "history") == 0)
273                                                         {
274                                                                 int i;
275                                                                 for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
276                                                                 {
277                                                                         if (t_con->history[i])
278                                                                         {
279                                                                                 telnet_write(connection, t_con->history[i], strlen(t_con->history[i]));
280                                                                                 telnet_write(connection, "\r\n\x00", 3);
281                                                                         }
282                                                                 }
283                                                                 telnet_prompt(connection);
284                                                                 t_con->line_size = 0;
285                                                                 t_con->line_cursor = 0;
286                                                                 continue;
287                                                         }
288                                                         
289                                                         log_setCallback(telnet_log_callback, connection);
290                                                         t_con->suppress_prompt = 1;
291                                                         
292                                                         if ((retval = command_run_line(command_context, t_con->line)) != ERROR_OK)
293                                                         {
294                                                                 if (retval == ERROR_COMMAND_CLOSE_CONNECTION)
295                                                                 {
296                                                                         return ERROR_SERVER_REMOTE_CLOSED;
297                                                                 }
298                                                         }
299                                                         
300                                                         t_con->suppress_prompt = 0;
301                                                         
302                                                         /* Save only non-blank lines in the history */
303                                                         if (t_con->line_size > 0)
304                                                         {
305                                                                 /* if the history slot is already taken, free it */
306                                                                 if (t_con->history[t_con->next_history])
307                                                                 {
308                                                                         free(t_con->history[t_con->next_history]);
309                                                                 }
310                 
311                                                                 /* add line to history */
312                                                                 t_con->history[t_con->next_history] = strdup(t_con->line);
313
314                                                                 /* wrap history at TELNET_LINE_HISTORY_SIZE */
315                                                                 t_con->next_history = (t_con->next_history + 1) % TELNET_LINE_HISTORY_SIZE;
316                                                         
317                                                                 /* current history line starts at the new entry */
318                                                                 t_con->current_history = t_con->next_history;
319                                                         
320                                                                 if (t_con->history[t_con->current_history])
321                                                                 {
322                                                                         free(t_con->history[t_con->current_history]);
323                                                                 }
324                                                                 t_con->history[t_con->current_history] = strdup("");
325                                                         }
326                                                         
327                                                         int t = telnet_prompt(connection);
328                                                         if (t == ERROR_SERVER_REMOTE_CLOSED)
329                                                                 return t;
330                                                         
331                                                         t_con->line_size = 0;
332                                                         t_con->line_cursor = 0;
333                                                 }
334                                                 else if ((*buf_p == 0x7f) || (*buf_p == 0x8)) /* delete character */
335                                                 {
336                                                         if (t_con->line_cursor > 0)
337                                                         {
338                                                                 if (t_con->line_cursor != t_con->line_size)
339                                                                 {
340                                                                         int i;
341                                                                         telnet_write(connection, "\b", 1);
342                                                                         t_con->line_cursor--;
343                                                                         t_con->line_size--;
344                                                                         memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor);
345                                                                         
346                                                                         telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
347                                                                         telnet_write(connection, " \b", 2);
348                                                                         for (i = t_con->line_cursor; i < t_con->line_size; i++)
349                                                                         {
350                                                                                 telnet_write(connection, "\b", 1);
351                                                                         }
352                                                                 }
353                                                                 else
354                                                                 {
355                                                                         t_con->line_size--;
356                                                                         t_con->line_cursor--;
357                                                                         /* back space: move the 'printer' head one char back, overwrite with space, move back again */
358                                                                         telnet_write(connection, "\b \b", 3);
359                                                                 }
360                                                         }
361                                                 }
362                                                 else if (*buf_p == 0x15) /* clear line */
363                                                 {
364                                                         telnet_clear_line(connection, t_con);
365                                                 }
366                                                 else if (*buf_p == CTRL('B')) /* cursor left */
367                                                 {
368                                                         if (t_con->line_cursor > 0)
369                                                         {
370                                                                 telnet_write(connection, "\b", 1);
371                                                                 t_con->line_cursor--;
372                                                         }
373                                                         t_con->state = TELNET_STATE_DATA;
374                                                 }
375                                                 else if (*buf_p == CTRL('F')) /* cursor right */
376                                                 {
377                                                         if (t_con->line_cursor < t_con->line_size)
378                                                         {
379                                                                 telnet_write(connection, t_con->line + t_con->line_cursor++, 1);
380                                                         }
381                                                         t_con->state = TELNET_STATE_DATA;
382                                                 }
383                                                 else
384                                                 {
385                                                         DEBUG("unhandled nonprintable: %2.2x", *buf_p);
386                                                 }
387                                         }
388                                 }
389                                 break;
390                         case TELNET_STATE_IAC:
391                                 switch (*buf_p)
392                                 {
393                                         case '\xfe':
394                                                 t_con->state = TELNET_STATE_DONT;
395                                                 break;
396                                         case '\xfd':
397                                                 t_con->state = TELNET_STATE_DO;
398                                                 break;
399                                         case '\xfc':
400                                                 t_con->state = TELNET_STATE_WONT;
401                                                 break;
402                                         case '\xfb':
403                                                 t_con->state = TELNET_STATE_WILL;
404                                                 break;
405                                 }
406                                 break;
407                         case TELNET_STATE_SB:
408                                 break;
409                         case TELNET_STATE_SE:
410                                 break;
411                         case TELNET_STATE_WILL:
412                         case TELNET_STATE_WONT:
413                         case TELNET_STATE_DO:
414                         case TELNET_STATE_DONT:
415                                 t_con->state = TELNET_STATE_DATA;
416                                 break;
417                         case TELNET_STATE_ESCAPE:
418                                 if (t_con->last_escape == '[')
419                                 {
420                                         if (*buf_p == 'D') /* cursor left */
421                                         {
422                                                 if (t_con->line_cursor > 0)
423                                                 {
424                                                         telnet_write(connection, "\b", 1);
425                                                         t_con->line_cursor--;
426                                                 }
427                                                 t_con->state = TELNET_STATE_DATA;
428                                         }
429                                         else if (*buf_p == 'C') /* cursor right */
430                                         {
431                                                 if (t_con->line_cursor < t_con->line_size)
432                                                 {
433                                                         telnet_write(connection, t_con->line + t_con->line_cursor++, 1);
434                                                 }
435                                                 t_con->state = TELNET_STATE_DATA;
436                                         }
437                                         else if (*buf_p == 'A') /* cursor up */
438                                         {
439                                                 int last_history = (t_con->current_history > 0) ? t_con->current_history - 1 : TELNET_LINE_HISTORY_SIZE-1;
440                                                 if (t_con->history[last_history])
441                                                 {
442                                                         telnet_clear_line(connection, t_con);
443                                                         t_con->line_size = strlen(t_con->history[last_history]);
444                                                         t_con->line_cursor = t_con->line_size;
445                                                         memcpy(t_con->line, t_con->history[last_history], t_con->line_size + 1);
446                                                         telnet_write(connection, t_con->line, t_con->line_size);
447                                                         t_con->current_history = last_history;
448                                                 }
449                                                 t_con->state = TELNET_STATE_DATA;
450                                         }
451                                         else if (*buf_p == 'B') /* cursor down */
452                                         {
453                                                 int next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE;
454                                                 if (t_con->history[next_history])
455                                                 {
456                                                         telnet_clear_line(connection, t_con);
457                                                         t_con->line_size = strlen(t_con->history[next_history]);
458                                                         t_con->line_cursor = t_con->line_size;
459                                                         memcpy(t_con->line, t_con->history[next_history], t_con->line_size + 1);
460                                                         telnet_write(connection, t_con->line, t_con->line_size);
461                                                         t_con->current_history = next_history;
462                                                 }
463                                                 t_con->state = TELNET_STATE_DATA;
464                                         }
465                                         else if (*buf_p == '3')
466                                         {
467                                                 t_con->last_escape = *buf_p;
468                                         }
469                                         else
470                                         {
471                                                 t_con->state = TELNET_STATE_DATA;
472                                         }
473                                 }
474                                 else if (t_con->last_escape == '3')
475                                 {
476                                         /* Remove character */
477                                         if (*buf_p == '~')
478                                         {
479                                                 if (t_con->line_cursor < t_con->line_size)
480                                                 {
481                                                         int i;
482                                                         t_con->line_size--;
483                                                         /* remove char from line buffer */
484                                                         memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor);
485                                                         
486                                                         /* print remainder of buffer */
487                                                         telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
488                                                         /* overwrite last char with whitespace */
489                                                         telnet_write(connection, " \b", 2);
490                                                         
491                                                         /* move back to cursor position*/
492                                                         for (i = t_con->line_cursor; i < t_con->line_size; i++)
493                                                         {
494                                                                 telnet_write(connection, "\b", 1);
495                                                         }
496                                                 }
497                                                         
498                                                 t_con->state = TELNET_STATE_DATA;
499                                         }
500                                         else
501                                         {
502                                                 t_con->state = TELNET_STATE_DATA;
503                                         }
504                                 }
505                                 else if (t_con->last_escape == '\x00')
506                                 {
507                                         if (*buf_p == '[')
508                                         {
509                                                 t_con->last_escape = *buf_p;
510                                         }
511                                         else
512                                         {
513                                                 t_con->state = TELNET_STATE_DATA;
514                                         }
515                                 }
516                                 else
517                                 {
518                                         ERROR("BUG: unexpected value in t_con->last_escape");
519                                         t_con->state = TELNET_STATE_DATA;
520                                 }
521                                 
522                                 break;
523                         default:
524                                 ERROR("unknown telnet state");
525                                 exit(-1);
526                 }
527
528                 bytes_read--;
529                 buf_p++;
530         }
531         
532         return ERROR_OK;
533 }
534
535 int telnet_connection_closed(connection_t *connection)
536 {
537         telnet_connection_t *t_con = connection->priv;
538         int i;
539         
540         if (t_con->prompt)
541         {
542                 free(t_con->prompt);
543                 t_con->prompt = NULL;
544         }
545         
546         for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
547         {
548                 if (t_con->history[i])
549                 {
550                         free(t_con->history[i]);
551                         t_con->history[i] = NULL;
552                 }
553         }
554         
555         /* if this connection registered a debug-message receiver delete it */
556         delete_debug_msg_receiver(connection->cmd_ctx, NULL);
557         
558         if (connection->priv)
559         {
560                 free(connection->priv);
561                 connection->priv = NULL;
562         }
563         else
564         {
565                 ERROR("BUG: connection->priv == NULL");
566         }
567         
568         target_unregister_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx);
569
570         return ERROR_OK;
571 }
572
573 int telnet_set_prompt(connection_t *connection, char *prompt)
574 {
575         telnet_connection_t *t_con = connection->priv;
576
577         t_con->prompt = strdup(prompt);
578         
579         return ERROR_OK;
580 }
581
582 int telnet_init(char *banner)
583 {
584         telnet_service_t *telnet_service = malloc(sizeof(telnet_service_t));
585         
586         if (telnet_port == 0)
587         {
588                 WARNING("no telnet port specified, using default port 4444");
589                 telnet_port = 4444;
590         }
591         
592         telnet_service->banner = banner;
593         
594         add_service("telnet", CONNECTION_TELNET, telnet_port, 1, telnet_new_connection, telnet_input, telnet_connection_closed, telnet_service);
595         
596         return ERROR_OK;
597 }
598
599 int telnet_register_commands(command_context_t *command_context)
600 {
601         register_command(command_context, NULL, "exit", handle_exit_command,
602                                          COMMAND_EXEC, "exit telnet session");
603         
604         register_command(command_context, NULL, "telnet_port", handle_telnet_port_command,
605                                          COMMAND_CONFIG, "");
606         
607         return ERROR_OK;
608 }
609
610 /* daemon configuration command telnet_port */
611 int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
612 {
613         if (argc == 0)
614                 return ERROR_OK;
615
616         /* only if the port wasn't overwritten by cmdline */
617         if (telnet_port == 0)
618                 telnet_port = strtoul(args[0], NULL, 0);
619
620         return ERROR_OK;
621 }
622
623 int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
624 {
625         return ERROR_COMMAND_CLOSE_CONNECTION;
626 }