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