diff options
author | oharboe <oharboe@b42882b7-edfa-0310-969c-e2dbd0fdcd60> | 2008-02-25 17:32:53 +0000 |
---|---|---|
committer | oharboe <oharboe@b42882b7-edfa-0310-969c-e2dbd0fdcd60> | 2008-02-25 17:32:53 +0000 |
commit | 7f1944a47823fc48eef244257b80675a226f86b8 (patch) | |
tree | f070f312502705475fd5cb7918fcd4a124487ae5 /src | |
parent | 375c5f85d2871921b10125e2bc54eba0c338a67b (diff) | |
download | openocd+libswd-7f1944a47823fc48eef244257b80675a226f86b8.tar.gz openocd+libswd-7f1944a47823fc48eef244257b80675a226f86b8.tar.bz2 openocd+libswd-7f1944a47823fc48eef244257b80675a226f86b8.tar.xz openocd+libswd-7f1944a47823fc48eef244257b80675a226f86b8.zip |
Pavel Chromy
- multiple log listeners
- added OUTPUT() to replace printf
- fix formatting
git-svn-id: svn://svn.berlios.de/openocd/trunk@346 b42882b7-edfa-0310-969c-e2dbd0fdcd60
Diffstat (limited to 'src')
-rw-r--r-- | src/helper/command.c | 1225 | ||||
-rw-r--r-- | src/helper/log.c | 79 | ||||
-rw-r--r-- | src/helper/log.h | 212 | ||||
-rw-r--r-- | src/helper/options.c | 14 | ||||
-rw-r--r-- | src/openocd.c | 312 | ||||
-rw-r--r-- | src/server/gdb_server.c | 4214 | ||||
-rw-r--r-- | src/server/telnet_server.c | 1263 |
7 files changed, 3684 insertions, 3635 deletions
diff --git a/src/helper/command.c b/src/helper/command.c index 4b6de26c..f13a86c1 100644 --- a/src/helper/command.c +++ b/src/helper/command.c @@ -1,618 +1,607 @@ -/*************************************************************************** - * Copyright (C) 2005 by Dominic Rath * - * Dominic.Rath@gmx.de * - * * - * part of this file is taken from libcli (libcli.sourceforge.net) * - * Copyright (C) David Parrish (david@dparrish.com) * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ -#ifdef HAVE_CONFIG_H -#include "config.h" -#endif - -#include "replacements.h" - -#include "command.h" - -#include "log.h" -#include "time_support.h" - -#include <stdlib.h> -#include <string.h> -#include <ctype.h> -#include <stdarg.h> -#include <stdio.h> -#include <unistd.h> - -void command_print_help_line(command_context_t* context, struct command_s *command, int indent); - -int handle_sleep_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc); -int handle_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc); - -int build_unique_lengths(command_context_t *context, command_t *commands) -{ - command_t *c, *p; - - /* iterate through all commands */ - for (c = commands; c; c = c->next) - { - /* find out how many characters are required to uniquely identify a command */ - for (c->unique_len = 1; c->unique_len <= strlen(c->name); c->unique_len++) - { - int foundmatch = 0; - - /* for every command, see if the current length is enough */ - for (p = commands; p; p = p->next) - { - /* ignore the command itself */ - if (c == p) - continue; - - /* compare commands up to the current length */ - if (strncmp(p->name, c->name, c->unique_len) == 0) - foundmatch++; - } - - /* when none of the commands matched, we've found the minimum length required */ - if (!foundmatch) - break; - } - - /* if the current command has children, build the unique lengths for them */ - if (c->children) - build_unique_lengths(context, c->children); - } - - return ERROR_OK; -} - -/* Avoid evaluating this each time we add a command. Reduces overhead from O(n^2) to O(n). - * Makes a difference on ARM7 types machines and is not observable on GHz machines. - */ -static int unique_length_dirty = 1; - -command_t* register_command(command_context_t *context, command_t *parent, char *name, int (*handler)(struct command_context_s *context, char* name, char** args, int argc), enum command_mode mode, char *help) -{ - command_t *c, *p; - unique_length_dirty = 1; - - if (!context || !name) - return NULL; - - c = malloc(sizeof(command_t)); - - c->name = strdup(name); - c->parent = parent; - c->children = NULL; - c->handler = handler; - c->mode = mode; - if (help) - c->help = strdup(help); - else - c->help = NULL; - c->unique_len = 0; - c->next = NULL; - - /* place command in tree */ - if (parent) - { - if (parent->children) - { - /* find last child */ - for (p = parent->children; p && p->next; p = p->next); - if (p) - p->next = c; - } - else - { - parent->children = c; - } - } - else - { - if (context->commands) - { - /* find last command */ - for (p = context->commands; p && p->next; p = p->next); - if (p) - p->next = c; - } - else - { - context->commands = c; - } - } - - return c; -} - -int unregister_command(command_context_t *context, char *name) -{ - unique_length_dirty = 1; - - command_t *c, *p = NULL, *c2; - - if ((!context) || (!name)) - return ERROR_INVALID_ARGUMENTS; - - /* find command */ - for (c = context->commands; c; c = c->next) - { - if (strcmp(name, c->name) == 0) - { - /* unlink command */ - if (p) - { - p->next = c->next; - } - else - { - context->commands = c->next; - } - - /* unregister children */ - if (c->children) - { - for (c2 = c->children; c2; c2 = c2->next) - { - free(c2->name); - if (c2->help) - free(c2->help); - free(c2); - } - } - - /* delete command */ - free(c->name); - if (c->help) - free(c->help); - free(c); - } - - /* remember the last command for unlinking */ - p = c; - } - - return ERROR_OK; -} - -int parse_line(char *line, char *words[], int max_words) -{ - int nwords = 0; - char *p = line; - char *word_start = line; - int inquote = 0; - - while (nwords < max_words - 1) - { - /* check if we reached - * a terminating NUL - * a matching closing quote character " or ' - * we're inside a word but not a quote, and the current character is whitespace - */ - if (!*p || *p == inquote || (word_start && !inquote && isspace(*p))) - { - /* we're inside a word or quote, and reached its end*/ - if (word_start) - { - int len; - char *word_end=p; - - /* This will handle extra whitespace within quotes */ - while (isspace(*word_start)&&(word_start<word_end)) - word_start++; - while (isspace(*(word_end-1))&&(word_start<word_end)) - word_end--; - len = word_end - word_start; - - if (len>0) - { - /* copy the word */ - memcpy(words[nwords] = malloc(len + 1), word_start, len); - /* add terminating NUL */ - words[nwords++][len] = 0; - } - } - /* we're done parsing the line */ - if (!*p) - break; - - /* skip over trailing quote or whitespace*/ - if (inquote || isspace(*p)) - p++; - while (isspace(*p)) - p++; - - inquote = 0; - word_start = 0; - } - else if (*p == '"' || *p == '\'') - { - /* we've reached the beginning of a quote */ - inquote = *p++; - word_start = p; - } - else - { - /* we've reached the beginning of a new word */ - if (!word_start) - word_start = p; - - /* normal character, skip */ - p++; - } - } - - return nwords; -} - -void command_print(command_context_t *context, char *format, ...) -{ - char *buffer = NULL; - int n, size = 0; - char *p; - - /* process format string */ - for (;;) - { - va_list ap; - va_start(ap, format); - if (!buffer || (n = vsnprintf(buffer, size, format, ap)) >= size) - { - /* increase buffer until it fits the whole string */ - if (!(p = realloc(buffer, size += 4096))) - { - /* gotta free up */ - if (buffer) - free(buffer); - va_end(ap); - return; - } - - buffer = p; - - va_end(ap); - continue; - } - va_end(ap); - break; - } - - /* vsnprintf failed */ - if (n < 0) - { - if (buffer) - free(buffer); - return; - } - - p = buffer; - - /* process lines in buffer */ - do { - char *next = strchr(p, '\n'); - - if (next) - *next++ = 0; - - if (context->output_handler) - context->output_handler(context, p); - - p = next; - } while (p); - - if (buffer) - free(buffer); -} - -int find_and_run_command(command_context_t *context, command_t *commands, char *words[], int num_words, int start_word) -{ - command_t *c; - - if (unique_length_dirty) - { - unique_length_dirty = 0; - /* update unique lengths */ - build_unique_lengths(context, context->commands); - } - - for (c = commands; c; c = c->next) - { - if (strncasecmp(c->name, words[start_word], c->unique_len)) - continue; - - if (strncasecmp(c->name, words[start_word], strlen(words[start_word]))) - continue; - - if ((context->mode == COMMAND_CONFIG) || (c->mode == COMMAND_ANY) || (c->mode == context->mode) ) - { - if (!c->children) - { - if (!c->handler) - { - command_print(context, "No handler for command"); - break; - } - else - { - int retval = c->handler(context, c->name, words + start_word + 1, num_words - start_word - 1); - if (retval == ERROR_COMMAND_SYNTAX_ERROR) - { - command_print(context, "Syntax error:"); - command_print_help_line(context, c, 0); - } - return retval; - } - } - else - { - if (start_word == num_words - 1) - { - command_print(context, "Incomplete command"); - break; - } - return find_and_run_command(context, c->children, words, num_words, start_word + 1); - } - } - } - - command_print(context, "Command %s not found", words[start_word]); - return ERROR_OK; -} - -static int command_run_line_inner(command_context_t *context, char *line) -{ - int nwords; - char *words[128] = {0}; - int retval; - int i; - - if ((!context) || (!line)) - return ERROR_INVALID_ARGUMENTS; - - /* skip preceding whitespace */ - while (isspace(*line)) - line++; - - /* empty line, ignore */ - if (!*line) - return ERROR_OK; - - /* ignore comments */ - if (*line && (line[0] == '#')) - return ERROR_OK; - - if (context->echo) - { - command_print(context, "%s", line); - } - - nwords = parse_line(line, words, sizeof(words) / sizeof(words[0])); - - if (nwords > 0) - retval = find_and_run_command(context, context->commands, words, nwords, 0); - else - return ERROR_INVALID_ARGUMENTS; - - for (i = 0; i < nwords; i++) - free(words[i]); - - return retval; -} - -int command_run_line(command_context_t *context, char *line) -{ - int retval=command_run_line_inner(context, line); - // we don't want any dangling callbacks! - // - // Capturing output from logging is *very* loosly modeled on C/C++ exceptions. - // the capture must be set up at function entry and - // stops when the function call returns - log_setCallback(NULL, NULL); - return retval; -} -int command_run_file(command_context_t *context, FILE *file, enum command_mode mode) -{ - int retval = ERROR_OK; - int old_command_mode; - char *buffer=malloc(4096); - if (buffer==NULL) - { - return ERROR_INVALID_ARGUMENTS; - } - - old_command_mode = context->mode; - context->mode = mode; - - while (fgets(buffer, 4096, file)) - { - char *p; - char *cmd, *end; - - /* stop processing line after a comment (#, !) or a LF, CR were encountered */ - if ((p = strpbrk(buffer, "#!\r\n"))) - *p = 0; - - /* skip over leading whitespace */ - cmd = buffer; - while (isspace(*cmd)) - cmd++; - - /* empty (all whitespace) line? */ - if (!*cmd) - continue; - - /* search the end of the current line, ignore trailing whitespace */ - for (p = end = cmd; *p; p++) - if (!isspace(*p)) - end = p; - - /* terminate end */ - *++end = 0; - if (strcasecmp(cmd, "quit") == 0) - break; - - /* run line */ - if ((retval = command_run_line_inner(context, cmd)) == ERROR_COMMAND_CLOSE_CONNECTION) - break; - } - - context->mode = old_command_mode; - - - free(buffer); - - return retval; -} - -void command_print_help_line(command_context_t* context, struct command_s *command, int indent) -{ - command_t *c; - char indents[32] = {0}; - char *help = "no help available"; - char name_buf[64]; - int i; - - for (i = 0; i < indent; i+=2) - { - indents[i*2] = ' '; - indents[i*2+1] = '-'; - } - indents[i*2] = 0; - - if (command->help) - help = command->help; - - snprintf(name_buf, 64, command->name); - strncat(name_buf, indents, 64); - command_print(context, "%20s\t%s", name_buf, help); - - if (command->children) - { - for (c = command->children; c; c = c->next) - { - command_print_help_line(context, c, indent + 1); - } - } -} - -int command_print_help(command_context_t* context, char* name, char** args, int argc) -{ - command_t *c; - - for (c = context->commands; c; c = c->next) - { - if (argc == 1) - { - if (strncasecmp(c->name, args[0], c->unique_len)) - continue; - - if (strncasecmp(c->name, args[0], strlen(args[0]))) - continue; - } - - command_print_help_line(context, c, 0); - } - - return ERROR_OK; -} - -void command_set_output_handler(command_context_t* context, int (*output_handler)(struct command_context_s *context, char* line), void *priv) -{ - context->output_handler = output_handler; - context->output_handler_priv = priv; -} - -command_context_t* copy_command_context(command_context_t* context) -{ - command_context_t* copy_context = malloc(sizeof(command_context_t)); - - *copy_context = *context; - - return copy_context; -} - -int command_done(command_context_t *context) -{ - free(context); - context = NULL; - - return ERROR_OK; -} - -command_context_t* command_init() -{ - command_context_t* context = malloc(sizeof(command_context_t)); - - context->mode = COMMAND_EXEC; - context->commands = NULL; - context->current_target = 0; - context->echo = 0; - context->output_handler = NULL; - context->output_handler_priv = NULL; - - register_command(context, NULL, "help", command_print_help, - COMMAND_EXEC, "display this help"); - - register_command(context, NULL, "sleep", handle_sleep_command, - COMMAND_ANY, "sleep for <n> milliseconds"); - - register_command(context, NULL, "time", handle_time_command, - COMMAND_ANY, "time <cmd + args> - execute <cmd + args> and print time it took"); - - return context; -} - -/* sleep command sleeps for <n> miliseconds - * this is useful in target startup scripts - */ -int handle_sleep_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - unsigned long duration = 0; - - if (argc == 1) - { - duration = strtoul(args[0], NULL, 0); - usleep(duration * 1000); - } - - return ERROR_OK; -} - -int handle_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - if (argc<1) - return ERROR_COMMAND_SYNTAX_ERROR; - - duration_t duration; - char *duration_text; - int retval; - - duration_start_measure(&duration); - - retval = find_and_run_command(cmd_ctx, cmd_ctx->commands, args, argc, 0); - - duration_stop_measure(&duration, &duration_text); - - float t=duration.duration.tv_sec; - t+=((float)duration.duration.tv_usec / 1000000.0); - command_print(cmd_ctx, "%s took %fs", args[0], t); - - free(duration_text); - - return retval; -} +/***************************************************************************
+ * Copyright (C) 2005 by Dominic Rath *
+ * Dominic.Rath@gmx.de *
+ * *
+ * part of this file is taken from libcli (libcli.sourceforge.net) *
+ * Copyright (C) David Parrish (david@dparrish.com) *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
+#include "command.h"
+
+#include "log.h"
+#include "time_support.h"
+
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <unistd.h>
+
+void command_print_help_line(command_context_t* context, struct command_s *command, int indent);
+
+int handle_sleep_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int build_unique_lengths(command_context_t *context, command_t *commands)
+{
+ command_t *c, *p;
+
+ /* iterate through all commands */
+ for (c = commands; c; c = c->next)
+ {
+ /* find out how many characters are required to uniquely identify a command */
+ for (c->unique_len = 1; c->unique_len <= strlen(c->name); c->unique_len++)
+ {
+ int foundmatch = 0;
+
+ /* for every command, see if the current length is enough */
+ for (p = commands; p; p = p->next)
+ {
+ /* ignore the command itself */
+ if (c == p)
+ continue;
+
+ /* compare commands up to the current length */
+ if (strncmp(p->name, c->name, c->unique_len) == 0)
+ foundmatch++;
+ }
+
+ /* when none of the commands matched, we've found the minimum length required */
+ if (!foundmatch)
+ break;
+ }
+
+ /* if the current command has children, build the unique lengths for them */
+ if (c->children)
+ build_unique_lengths(context, c->children);
+ }
+
+ return ERROR_OK;
+}
+
+/* Avoid evaluating this each time we add a command. Reduces overhead from O(n^2) to O(n).
+ * Makes a difference on ARM7 types machines and is not observable on GHz machines.
+ */
+static int unique_length_dirty = 1;
+
+command_t* register_command(command_context_t *context, command_t *parent, char *name, int (*handler)(struct command_context_s *context, char* name, char** args, int argc), enum command_mode mode, char *help)
+{
+ command_t *c, *p;
+ unique_length_dirty = 1;
+
+ if (!context || !name)
+ return NULL;
+
+ c = malloc(sizeof(command_t));
+
+ c->name = strdup(name);
+ c->parent = parent;
+ c->children = NULL;
+ c->handler = handler;
+ c->mode = mode;
+ if (help)
+ c->help = strdup(help);
+ else
+ c->help = NULL;
+ c->unique_len = 0;
+ c->next = NULL;
+
+ /* place command in tree */
+ if (parent)
+ {
+ if (parent->children)
+ {
+ /* find last child */
+ for (p = parent->children; p && p->next; p = p->next);
+ if (p)
+ p->next = c;
+ }
+ else
+ {
+ parent->children = c;
+ }
+ }
+ else
+ {
+ if (context->commands)
+ {
+ /* find last command */
+ for (p = context->commands; p && p->next; p = p->next);
+ if (p)
+ p->next = c;
+ }
+ else
+ {
+ context->commands = c;
+ }
+ }
+
+ return c;
+}
+
+int unregister_command(command_context_t *context, char *name)
+{
+ unique_length_dirty = 1;
+
+ command_t *c, *p = NULL, *c2;
+
+ if ((!context) || (!name))
+ return ERROR_INVALID_ARGUMENTS;
+
+ /* find command */
+ for (c = context->commands; c; c = c->next)
+ {
+ if (strcmp(name, c->name) == 0)
+ {
+ /* unlink command */
+ if (p)
+ {
+ p->next = c->next;
+ }
+ else
+ {
+ context->commands = c->next;
+ }
+
+ /* unregister children */
+ if (c->children)
+ {
+ for (c2 = c->children; c2; c2 = c2->next)
+ {
+ free(c2->name);
+ if (c2->help)
+ free(c2->help);
+ free(c2);
+ }
+ }
+
+ /* delete command */
+ free(c->name);
+ if (c->help)
+ free(c->help);
+ free(c);
+ }
+
+ /* remember the last command for unlinking */
+ p = c;
+ }
+
+ return ERROR_OK;
+}
+
+int parse_line(char *line, char *words[], int max_words)
+{
+ int nwords = 0;
+ char *p = line;
+ char *word_start = line;
+ int inquote = 0;
+
+ while (nwords < max_words - 1)
+ {
+ /* check if we reached
+ * a terminating NUL
+ * a matching closing quote character " or '
+ * we're inside a word but not a quote, and the current character is whitespace
+ */
+ if (!*p || *p == inquote || (word_start && !inquote && isspace(*p)))
+ {
+ /* we're inside a word or quote, and reached its end*/
+ if (word_start)
+ {
+ int len;
+ char *word_end=p;
+
+ /* This will handle extra whitespace within quotes */
+ while (isspace(*word_start)&&(word_start<word_end))
+ word_start++;
+ while (isspace(*(word_end-1))&&(word_start<word_end))
+ word_end--;
+ len = word_end - word_start;
+
+ if (len>0)
+ {
+ /* copy the word */
+ memcpy(words[nwords] = malloc(len + 1), word_start, len);
+ /* add terminating NUL */
+ words[nwords++][len] = 0;
+ }
+ }
+ /* we're done parsing the line */
+ if (!*p)
+ break;
+
+ /* skip over trailing quote or whitespace*/
+ if (inquote || isspace(*p))
+ p++;
+ while (isspace(*p))
+ p++;
+
+ inquote = 0;
+ word_start = 0;
+ }
+ else if (*p == '"' || *p == '\'')
+ {
+ /* we've reached the beginning of a quote */
+ inquote = *p++;
+ word_start = p;
+ }
+ else
+ {
+ /* we've reached the beginning of a new word */
+ if (!word_start)
+ word_start = p;
+
+ /* normal character, skip */
+ p++;
+ }
+ }
+
+ return nwords;
+}
+
+void command_print(command_context_t *context, char *format, ...)
+{
+ char *buffer = NULL;
+ int n, size = 0;
+ char *p;
+
+ /* process format string */
+ for (;;)
+ {
+ va_list ap;
+ va_start(ap, format);
+ if (!buffer || (n = vsnprintf(buffer, size, format, ap)) >= size)
+ {
+ /* increase buffer until it fits the whole string */
+ if (!(p = realloc(buffer, size += 4096)))
+ {
+ /* gotta free up */
+ if (buffer)
+ free(buffer);
+ va_end(ap);
+ return;
+ }
+
+ buffer = p;
+
+ va_end(ap);
+ continue;
+ }
+ va_end(ap);
+ break;
+ }
+
+ /* vsnprintf failed */
+ if (n < 0)
+ {
+ if (buffer)
+ free(buffer);
+ return;
+ }
+
+ p = buffer;
+
+ /* process lines in buffer */
+ do {
+ char *next = strchr(p, '\n');
+
+ if (next)
+ *next++ = 0;
+
+ if (context->output_handler)
+ context->output_handler(context, p);
+
+ p = next;
+ } while (p);
+
+ if (buffer)
+ free(buffer);
+}
+
+int find_and_run_command(command_context_t *context, command_t *commands, char *words[], int num_words, int start_word)
+{
+ command_t *c;
+
+ if (unique_length_dirty)
+ {
+ unique_length_dirty = 0;
+ /* update unique lengths */
+ build_unique_lengths(context, context->commands);
+ }
+
+ for (c = commands; c; c = c->next)
+ {
+ if (strncasecmp(c->name, words[start_word], c->unique_len))
+ continue;
+
+ if (strncasecmp(c->name, words[start_word], strlen(words[start_word])))
+ continue;
+
+ if ((context->mode == COMMAND_CONFIG) || (c->mode == COMMAND_ANY) || (c->mode == context->mode) )
+ {
+ if (!c->children)
+ {
+ if (!c->handler)
+ {
+ command_print(context, "No handler for command");
+ break;
+ }
+ else
+ {
+ int retval = c->handler(context, c->name, words + start_word + 1, num_words - start_word - 1);
+ if (retval == ERROR_COMMAND_SYNTAX_ERROR)
+ {
+ command_print(context, "Syntax error:");
+ command_print_help_line(context, c, 0);
+ }
+ return retval;
+ }
+ }
+ else
+ {
+ if (start_word == num_words - 1)
+ {
+ command_print(context, "Incomplete command");
+ break;
+ }
+ return find_and_run_command(context, c->children, words, num_words, start_word + 1);
+ }
+ }
+ }
+
+ command_print(context, "Command %s not found", words[start_word]);
+ return ERROR_OK;
+}
+
+int command_run_line(command_context_t *context, char *line)
+{
+ int nwords;
+ char *words[128] = {0};
+ int retval;
+ int i;
+
+ if ((!context) || (!line))
+ return ERROR_INVALID_ARGUMENTS;
+
+ /* skip preceding whitespace */
+ while (isspace(*line))
+ line++;
+
+ /* empty line, ignore */
+ if (!*line)
+ return ERROR_OK;
+
+ /* ignore comments */
+ if (*line && (line[0] == '#'))
+ return ERROR_OK;
+
+ if (context->echo)
+ {
+ command_print(context, "%s", line);
+ }
+
+ nwords = parse_line(line, words, sizeof(words) / sizeof(words[0]));
+
+ if (nwords > 0)
+ retval = find_and_run_command(context, context->commands, words, nwords, 0);
+ else
+ return ERROR_INVALID_ARGUMENTS;
+
+ for (i = 0; i < nwords; i++)
+ free(words[i]);
+
+ return retval;
+}
+
+int command_run_file(command_context_t *context, FILE *file, enum command_mode mode)
+{
+ int retval = ERROR_OK;
+ int old_command_mode;
+ char *buffer=malloc(4096);
+ if (buffer==NULL)
+ {
+ return ERROR_INVALID_ARGUMENTS;
+ }
+
+ old_command_mode = context->mode;
+ context->mode = mode;
+
+ while (fgets(buffer, 4096, file))
+ {
+ char *p;
+ char *cmd, *end;
+
+ /* stop processing line after a comment (#, !) or a LF, CR were encountered */
+ if ((p = strpbrk(buffer, "#!\r\n")))
+ *p = 0;
+
+ /* skip over leading whitespace */
+ cmd = buffer;
+ while (isspace(*cmd))
+ cmd++;
+
+ /* empty (all whitespace) line? */
+ if (!*cmd)
+ continue;
+
+ /* search the end of the current line, ignore trailing whitespace */
+ for (p = end = cmd; *p; p++)
+ if (!isspace(*p))
+ end = p;
+
+ /* terminate end */
+ *++end = 0;
+ if (strcasecmp(cmd, "quit") == 0)
+ break;
+
+ /* run line */
+ if ((retval = command_run_line(context, cmd)) == ERROR_COMMAND_CLOSE_CONNECTION)
+ break;
+ }
+
+ context->mode = old_command_mode;
+
+
+ free(buffer);
+
+ return retval;
+}
+
+void command_print_help_line(command_context_t* context, struct command_s *command, int indent)
+{
+ command_t *c;
+ char indents[32] = {0};
+ char *help = "no help available";
+ char name_buf[64];
+ int i;
+
+ for (i = 0; i < indent; i+=2)
+ {
+ indents[i*2] = ' ';
+ indents[i*2+1] = '-';
+ }
+ indents[i*2] = 0;
+
+ if (command->help)
+ help = command->help;
+
+ snprintf(name_buf, 64, command->name);
+ strncat(name_buf, indents, 64);
+ command_print(context, "%20s\t%s", name_buf, help);
+
+ if (command->children)
+ {
+ for (c = command->children; c; c = c->next)
+ {
+ command_print_help_line(context, c, indent + 1);
+ }
+ }
+}
+
+int command_print_help(command_context_t* context, char* name, char** args, int argc)
+{
+ command_t *c;
+
+ for (c = context->commands; c; c = c->next)
+ {
+ if (argc == 1)
+ {
+ if (strncasecmp(c->name, args[0], c->unique_len))
+ continue;
+
+ if (strncasecmp(c->name, args[0], strlen(args[0])))
+ continue;
+ }
+
+ command_print_help_line(context, c, 0);
+ }
+
+ return ERROR_OK;
+}
+
+void command_set_output_handler(command_context_t* context, int (*output_handler)(struct command_context_s *context, char* line), void *priv)
+{
+ context->output_handler = output_handler;
+ context->output_handler_priv = priv;
+}
+
+command_context_t* copy_command_context(command_context_t* context)
+{
+ command_context_t* copy_context = malloc(sizeof(command_context_t));
+
+ *copy_context = *context;
+
+ return copy_context;
+}
+
+int command_done(command_context_t *context)
+{
+ free(context);
+ context = NULL;
+
+ return ERROR_OK;
+}
+
+command_context_t* command_init()
+{
+ command_context_t* context = malloc(sizeof(command_context_t));
+
+ context->mode = COMMAND_EXEC;
+ context->commands = NULL;
+ context->current_target = 0;
+ context->echo = 0;
+ context->output_handler = NULL;
+ context->output_handler_priv = NULL;
+
+ register_command(context, NULL, "help", command_print_help,
+ COMMAND_EXEC, "display this help");
+
+ register_command(context, NULL, "sleep", handle_sleep_command,
+ COMMAND_ANY, "sleep for <n> milliseconds");
+
+ register_command(context, NULL, "time", handle_time_command,
+ COMMAND_ANY, "time <cmd + args> - execute <cmd + args> and print time it took");
+
+ return context;
+}
+
+/* sleep command sleeps for <n> miliseconds
+ * this is useful in target startup scripts
+ */
+int handle_sleep_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ unsigned long duration = 0;
+
+ if (argc == 1)
+ {
+ duration = strtoul(args[0], NULL, 0);
+ usleep(duration * 1000);
+ }
+
+ return ERROR_OK;
+}
+
+int handle_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ if (argc<1)
+ return ERROR_COMMAND_SYNTAX_ERROR;
+
+ duration_t duration;
+ char *duration_text;
+ int retval;
+
+ duration_start_measure(&duration);
+
+ retval = find_and_run_command(cmd_ctx, cmd_ctx->commands, args, argc, 0);
+
+ duration_stop_measure(&duration, &duration_text);
+
+ float t=duration.duration.tv_sec;
+ t+=((float)duration.duration.tv_usec / 1000000.0);
+ command_print(cmd_ctx, "%s took %fs", args[0], t);
+
+ free(duration_text);
+
+ return retval;
+}
diff --git a/src/helper/log.c b/src/helper/log.c index 8567c037..90077ea3 100644 --- a/src/helper/log.c +++ b/src/helper/log.c @@ -33,18 +33,11 @@ int debug_level = -1;
static FILE* log_output;
+static log_callback_t *log_callbacks = NULL;
-static void *privData;
-static logCallback callback;
static time_t start;
-void log_setCallback(logCallback c, void *p)
-{
- callback = c;
- privData = p;
-}
-
-static char *log_strings[5] =
+static char *log_strings[5] =
{
"User: ",
"Error: ",
@@ -59,12 +52,22 @@ void log_printf(enum log_levels level, const char *file, int line, const char *f count++;
va_list args;
char buffer[512];
+ log_callback_t *cb;
if (level > debug_level)
return;
va_start(args, format);
vsnprintf(buffer, 512, format, args);
+ va_end(args);
+
+ if (level == LOG_OUTPUT)
+ {
+ /* do not prepend any headers, just print out what we were given and return */
+ fputs(buffer, log_output);
+ fflush(log_output);
+ return;
+ }
char *f = strrchr(file, '/');
if (f != NULL)
@@ -84,14 +87,15 @@ void log_printf(enum log_levels level, const char *file, int line, const char *f fflush(log_output);
- va_end(args);
-
/* Never forward LOG_DEBUG, too verbose and they can be found in the log if need be */
- if (callback && (level <= LOG_INFO))
+ if (level <= LOG_INFO)
{
- va_start(args, format);
- callback(privData, file, line, function, format, args);
- va_end(args);
+ for (cb = log_callbacks; cb; cb = cb->next)
+ {
+ va_start(args, format);
+ cb->fn(cb->priv, file, line, function, format, args);
+ va_end(args);
+ }
}
}
@@ -164,8 +168,51 @@ int set_log_output(struct command_context_s *cmd_ctx, FILE *output) return ERROR_OK;
}
+/* add/remove log callback handler */
+int log_add_callback(log_callback_fn fn, void *priv)
+{
+ log_callback_t *cb;
+
+ /* prevent the same callback to be registered more than once, just for sure */
+ for (cb = log_callbacks; cb; cb = cb->next)
+ {
+ if (cb->fn == fn && cb->priv == priv)
+ return ERROR_INVALID_ARGUMENTS;
+ }
+
+ /* alloc memory, it is safe just to return in case of an error, no need for the caller to check this */
+ if ((cb = malloc(sizeof(log_callback_t))) == NULL)
+ return ERROR_BUF_TOO_SMALL;
+
+ /* add item to the beginning of the linked list */
+ cb->fn = fn;
+ cb->priv = priv;
+ cb->next = log_callbacks;
+ log_callbacks = cb;
+
+ return ERROR_OK;
+}
+
+int log_remove_callback(log_callback_fn fn, void *priv)
+{
+ log_callback_t *cb, **p;
+
+ for (p = &log_callbacks; cb = *p; p = &(*p)->next)
+ {
+ if (cb->fn == fn && cb->priv == priv)
+ {
+ *p = cb->next;
+ free(cb);
+ return ERROR_OK;
+ }
+ }
+
+ /* no such item */
+ return ERROR_INVALID_ARGUMENTS;
+}
+
/* return allocated string w/printf() result */
-char *allocPrintf(const char *fmt, va_list ap)
+char *alloc_printf(const char *fmt, va_list ap)
{
char *string = NULL;
diff --git a/src/helper/log.h b/src/helper/log.h index ab32760a..61050009 100644 --- a/src/helper/log.h +++ b/src/helper/log.h @@ -1,99 +1,113 @@ -/*************************************************************************** - * Copyright (C) 2005 by Dominic Rath * - * Dominic.Rath@gmx.de * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ -#ifndef ERROR_H -#define ERROR_H - -#include "replacements.h" -#include "command.h" - -#include <stdarg.h> - -/* logging priorities - * LOG_USER - user messages. Could be anything from information - * to progress messags. These messages do not represent - * incorrect or unexpected behaviour, just normal execution. - * LOG_ERROR - fatal errors, that are likely to cause program abort - * LOG_WARNING - non-fatal errors, that may be resolved later - * LOG_INFO - state information, etc. - * LOG_DEBUG - debug statements, execution trace - */ -enum log_levels -{ - LOG_USER = -1, - LOG_ERROR = 0, - LOG_WARNING = 1, - LOG_INFO = 2, - LOG_DEBUG = 3 -}; - -extern void log_printf(enum log_levels level, const char *file, int line, - const char *function, const char *format, ...) - __attribute__ ((format (printf, 5, 6))); -extern int log_register_commands(struct command_context_s *cmd_ctx); -extern int log_init(struct command_context_s *cmd_ctx); -extern int set_log_output(struct command_context_s *cmd_ctx, FILE *output); - -typedef void (*logCallback)(void *priv, const char *file, int line, - const char *function, const char *format, va_list args); - -extern void log_setCallback(logCallback callback, void *priv); - -extern int debug_level; - -/* Avoid fn call and building parameter list if we're not outputting the information. - * Matters on feeble CPUs for DEBUG/INFO statements that are involved frequently */ - -#define DEBUG(expr ...) \ - do { if (debug_level >= LOG_DEBUG) \ - log_printf (LOG_DEBUG, __FILE__, __LINE__, __FUNCTION__, expr); \ - } while(0) - -#define INFO(expr ...) \ - do { if (debug_level >= LOG_INFO) \ - log_printf (LOG_INFO, __FILE__, __LINE__, __FUNCTION__, expr); \ - } while(0) - -#define WARNING(expr ...) \ - do { \ - log_printf (LOG_WARNING, __FILE__, __LINE__, __FUNCTION__, expr); \ - } while(0) - -#define ERROR(expr ...) \ - do { \ - log_printf (LOG_ERROR, __FILE__, __LINE__, __FUNCTION__, expr); \ - } while(0) - -#define USER(expr ...) \ - do { \ - log_printf (LOG_USER, __FILE__, __LINE__, __FUNCTION__, expr); \ - } while(0) - - -/* general failures - * error codes < 100 - */ -#define ERROR_OK (0) -#define ERROR_INVALID_ARGUMENTS (-1) -#define ERROR_NO_CONFIG_FILE (-2) -#define ERROR_BUF_TOO_SMALL (-3) - -char *allocPrintf(const char *fmt, va_list ap); - -#endif /* LOG_H */ +/***************************************************************************
+ * Copyright (C) 2005 by Dominic Rath *
+ * Dominic.Rath@gmx.de *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+#ifndef ERROR_H
+#define ERROR_H
+
+#include "replacements.h"
+#include "command.h"
+
+#include <stdarg.h>
+
+/* logging priorities
+ * LOG_USER - user messages. Could be anything from information
+ * to progress messags. These messages do not represent
+ * incorrect or unexpected behaviour, just normal execution.
+ * LOG_ERROR - fatal errors, that are likely to cause program abort
+ * LOG_WARNING - non-fatal errors, that may be resolved later
+ * LOG_INFO - state information, etc.
+ * LOG_DEBUG - debug statements, execution trace
+ */
+enum log_levels
+{
+ LOG_OUTPUT = -2,
+ LOG_USER = -1,
+ LOG_ERROR = 0,
+ LOG_WARNING = 1,
+ LOG_INFO = 2,
+ LOG_DEBUG = 3
+};
+
+extern void log_printf(enum log_levels level, const char *file, int line,
+ const char *function, const char *format, ...)
+ __attribute__ ((format (printf, 5, 6)));
+extern int log_register_commands(struct command_context_s *cmd_ctx);
+extern int log_init(struct command_context_s *cmd_ctx);
+extern int set_log_output(struct command_context_s *cmd_ctx, FILE *output);
+
+typedef void (*log_callback_fn)(void *priv, const char *file, int line,
+ const char *function, const char *format, va_list args);
+
+typedef struct log_callback_s
+{
+ log_callback_fn fn;
+ void *priv;
+ struct log_callback_s *next;
+} log_callback_t;
+
+extern int log_add_callback(log_callback_fn fn, void *priv);
+extern int log_remove_callback(log_callback_fn fn, void *priv);
+
+char *alloc_printf(const char *fmt, va_list ap);
+
+extern int debug_level;
+
+/* Avoid fn call and building parameter list if we're not outputting the information.
+ * Matters on feeble CPUs for DEBUG/INFO statements that are involved frequently */
+
+#define DEBUG(expr ...) \
+ do { if (debug_level >= LOG_DEBUG) \
+ log_printf (LOG_DEBUG, __FILE__, __LINE__, __FUNCTION__, expr); \
+ } while(0)
+
+#define INFO(expr ...) \
+ do { if (debug_level >= LOG_INFO) \
+ log_printf (LOG_INFO, __FILE__, __LINE__, __FUNCTION__, expr); \
+ } while(0)
+
+#define WARNING(expr ...) \
+ do { \
+ log_printf (LOG_WARNING, __FILE__, __LINE__, __FUNCTION__, expr); \
+ } while(0)
+
+#define ERROR(expr ...) \
+ do { \
+ log_printf (LOG_ERROR, __FILE__, __LINE__, __FUNCTION__, expr); \
+ } while(0)
+
+#define USER(expr ...) \
+ do { \
+ log_printf (LOG_USER, __FILE__, __LINE__, __FUNCTION__, expr); \
+ } while(0)
+
+#define OUTPUT(expr ...) \
+ do { \
+ log_printf (LOG_OUTPUT, __FILE__, __LINE__, __FUNCTION__, expr); \
+ } while(0)
+
+
+/* general failures
+ * error codes < 100
+ */
+#define ERROR_OK (0)
+#define ERROR_INVALID_ARGUMENTS (-1)
+#define ERROR_NO_CONFIG_FILE (-2)
+#define ERROR_BUF_TOO_SMALL (-3)
+
+#endif /* LOG_H */
diff --git a/src/helper/options.c b/src/helper/options.c index 1e717be8..4232cb44 100644 --- a/src/helper/options.c +++ b/src/helper/options.c @@ -113,13 +113,13 @@ int parse_cmdline_args(struct command_context_s *cmd_ctx, int argc, char *argv[] if (help_flag)
{
- printf("Open On-Chip Debugger\n(c) 2005 by Dominic Rath\n\n");
- printf("--help | -h\tdisplay this help\n");
- printf("--file | -f\tuse configuration file <name>\n");
- printf("--search | -s\tdir to search for config files and scripts.\n");
- printf("--debug | -d\tset debug level <0-3>\n");
- printf("--log_output | -l\tredirect log output to file <name>\n");
- printf("--command | -c\trun <command>\n");
+ OUTPUT("Open On-Chip Debugger\n(c) 2005 by Dominic Rath\n\n");
+ OUTPUT("--help | -h\tdisplay this help\n");
+ OUTPUT("--file | -f\tuse configuration file <name>\n");
+ OUTPUT("--search | -s\tdir to search for config files and scripts.\n");
+ OUTPUT("--debug | -d\tset debug level <0-3>\n");
+ OUTPUT("--log_output | -l\tredirect log output to file <name>\n");
+ OUTPUT("--command | -c\trun <command>\n");
exit(-1);
}
diff --git a/src/openocd.c b/src/openocd.c index b636d06d..c2f87764 100644 --- a/src/openocd.c +++ b/src/openocd.c @@ -1,157 +1,157 @@ -/*************************************************************************** - * Copyright (C) 2005 by Dominic Rath * - * Dominic.Rath@gmx.de * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ - -#define OPENOCD_VERSION "Open On-Chip Debugger " VERSION " (" PKGBLDDATE ") svn:" PKGBLDREV - -#ifdef HAVE_CONFIG_H -#include "config.h" -#endif - -#include "log.h" -#include "types.h" -#include "jtag.h" -#include "configuration.h" -#include "interpreter.h" -#include "xsvf.h" -#include "target.h" -#include "flash.h" -#include "nand.h" -#include "pld.h" - -#include "command.h" -#include "server.h" -#include "telnet_server.h" -#include "gdb_server.h" - -#include <sys/time.h> -#include <sys/types.h> -#include <strings.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <errno.h> - -/* Give TELNET a way to find out what version this is */ -int handle_version_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - command_print(cmd_ctx, OPENOCD_VERSION); - - return ERROR_OK; -} - -void exit_handler(void) -{ - /* close JTAG interface */ - if (jtag && jtag->quit) - jtag->quit(); -} - -int main(int argc, char *argv[]) -{ - /* initialize commandline interface */ - command_context_t *cmd_ctx, *cfg_cmd_ctx; - cmd_ctx = command_init(); - - register_command(cmd_ctx, NULL, "version", handle_version_command, - COMMAND_EXEC, "show OpenOCD version"); - - /* register subsystem commands */ - server_register_commands(cmd_ctx); - telnet_register_commands(cmd_ctx); - gdb_register_commands(cmd_ctx); - log_register_commands(cmd_ctx); - jtag_register_commands(cmd_ctx); - interpreter_register_commands(cmd_ctx); - xsvf_register_commands(cmd_ctx); - target_register_commands(cmd_ctx); - flash_register_commands(cmd_ctx); - nand_register_commands(cmd_ctx); - pld_register_commands(cmd_ctx); - - if (log_init(cmd_ctx) != ERROR_OK) - return EXIT_FAILURE; - DEBUG("log init complete"); - - printf( OPENOCD_VERSION ); - printf( "\n$URL$\n"); - - DEBUG( OPENOCD_VERSION ); +/***************************************************************************
+ * Copyright (C) 2005 by Dominic Rath *
+ * Dominic.Rath@gmx.de *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+
+#define OPENOCD_VERSION "Open On-Chip Debugger " VERSION " (" PKGBLDDATE ") svn:" PKGBLDREV
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "log.h"
+#include "types.h"
+#include "jtag.h"
+#include "configuration.h"
+#include "interpreter.h"
+#include "xsvf.h"
+#include "target.h"
+#include "flash.h"
+#include "nand.h"
+#include "pld.h"
+
+#include "command.h"
+#include "server.h"
+#include "telnet_server.h"
+#include "gdb_server.h"
+
+#include <sys/time.h>
+#include <sys/types.h>
+#include <strings.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+
+/* Give TELNET a way to find out what version this is */
+int handle_version_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ command_print(cmd_ctx, OPENOCD_VERSION);
+
+ return ERROR_OK;
+}
+
+void exit_handler(void)
+{
+ /* close JTAG interface */
+ if (jtag && jtag->quit)
+ jtag->quit();
+}
+
+int main(int argc, char *argv[])
+{
+ /* initialize commandline interface */
+ command_context_t *cmd_ctx, *cfg_cmd_ctx;
+ cmd_ctx = command_init();
+
+ register_command(cmd_ctx, NULL, "version", handle_version_command,
+ COMMAND_EXEC, "show OpenOCD version");
+
+ /* register subsystem commands */
+ server_register_commands(cmd_ctx);
+ telnet_register_commands(cmd_ctx);
+ gdb_register_commands(cmd_ctx);
+ log_register_commands(cmd_ctx);
+ jtag_register_commands(cmd_ctx);
+ interpreter_register_commands(cmd_ctx);
+ xsvf_register_commands(cmd_ctx);
+ target_register_commands(cmd_ctx);
+ flash_register_commands(cmd_ctx);
+ nand_register_commands(cmd_ctx);
+ pld_register_commands(cmd_ctx);
+
+ if (log_init(cmd_ctx) != ERROR_OK)
+ return EXIT_FAILURE;
+ DEBUG("log init complete");
+
+ printf( OPENOCD_VERSION );
+ printf( "\n$URL$\n");
+
+ DEBUG( OPENOCD_VERSION );
DEBUG( "$URL$");
- - cfg_cmd_ctx = copy_command_context(cmd_ctx); - cfg_cmd_ctx->mode = COMMAND_CONFIG; - command_set_output_handler(cfg_cmd_ctx, configuration_output_handler, NULL); - - if (parse_cmdline_args(cfg_cmd_ctx, argc, argv) != ERROR_OK) - return EXIT_FAILURE; - - if (parse_config_file(cfg_cmd_ctx) != ERROR_OK) - return EXIT_FAILURE; - - command_done(cfg_cmd_ctx); - - command_set_output_handler(cmd_ctx, configuration_output_handler, NULL); - - atexit(exit_handler); - - if (jtag_init(cmd_ctx) != ERROR_OK) - return EXIT_FAILURE; - DEBUG("jtag init complete"); - - if (target_init(cmd_ctx) != ERROR_OK) - return EXIT_FAILURE; - DEBUG("target init complete"); - - if (flash_init_drivers(cmd_ctx) != ERROR_OK) - return EXIT_FAILURE; - DEBUG("flash init complete"); - - if (nand_init(cmd_ctx) != ERROR_OK) - return EXIT_FAILURE; - DEBUG("NAND init complete"); - - if (pld_init(cmd_ctx) != ERROR_OK) - return EXIT_FAILURE; - DEBUG("pld init complete"); - - /* initialize tcp server */ - server_init(); - - /* initialize telnet subsystem */ - telnet_init("Open On-Chip Debugger"); - gdb_init(); - - /* call any target resets */ - if (target_init_reset(cmd_ctx) != ERROR_OK) - return EXIT_FAILURE; - DEBUG("target init reset complete"); - - /* handle network connections */ - server_loop(cmd_ctx); - - /* shut server down */ - server_quit(); - - /* free commandline interface */ - command_done(cmd_ctx); - - return EXIT_SUCCESS; -} +
+ cfg_cmd_ctx = copy_command_context(cmd_ctx);
+ cfg_cmd_ctx->mode = COMMAND_CONFIG;
+ command_set_output_handler(cfg_cmd_ctx, configuration_output_handler, NULL);
+
+ if (parse_cmdline_args(cfg_cmd_ctx, argc, argv) != ERROR_OK)
+ return EXIT_FAILURE;
+
+ if (parse_config_file(cfg_cmd_ctx) != ERROR_OK)
+ return EXIT_FAILURE;
+
+ command_done(cfg_cmd_ctx);
+
+ command_set_output_handler(cmd_ctx, configuration_output_handler, NULL);
+
+ atexit(exit_handler);
+
+ if (jtag_init(cmd_ctx) != ERROR_OK)
+ return EXIT_FAILURE;
+ DEBUG("jtag init complete");
+
+ if (target_init(cmd_ctx) != ERROR_OK)
+ return EXIT_FAILURE;
+ DEBUG("target init complete");
+
+ if (flash_init_drivers(cmd_ctx) != ERROR_OK)
+ return EXIT_FAILURE;
+ DEBUG("flash init complete");
+
+ if (nand_init(cmd_ctx) != ERROR_OK)
+ return EXIT_FAILURE;
+ DEBUG("NAND init complete");
+
+ if (pld_init(cmd_ctx) != ERROR_OK)
+ return EXIT_FAILURE;
+ DEBUG("pld init complete");
+
+ /* initialize tcp server */
+ server_init();
+
+ /* initialize telnet subsystem */
+ telnet_init("Open On-Chip Debugger");
+ gdb_init();
+
+ /* call any target resets */
+ if (target_init_reset(cmd_ctx) != ERROR_OK)
+ return EXIT_FAILURE;
+ DEBUG("target init reset complete");
+
+ /* handle network connections */
+ server_loop(cmd_ctx);
+
+ /* shut server down */
+ server_quit();
+
+ /* free commandline interface */
+ command_done(cmd_ctx);
+
+ return EXIT_SUCCESS;
+}
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c index 49630579..fa203e7a 100644 --- a/src/server/gdb_server.c +++ b/src/server/gdb_server.c @@ -1,2107 +1,2107 @@ -/*************************************************************************** - * Copyright (C) 2005 by Dominic Rath * - * Dominic.Rath@gmx.de * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ -#ifdef HAVE_CONFIG_H -#include "config.h" -#endif - -#include "replacements.h" - -#include "gdb_server.h" - -#include "server.h" -#include "log.h" -#include "binarybuffer.h" -#include "jtag.h" -#include "breakpoints.h" -#include "flash.h" -#include "target_request.h" -#include "configuration.h" - -#include <string.h> -#include <errno.h> -#include <unistd.h> -#include <stdlib.h> - -#if 0 -#define _DEBUG_GDB_IO_ -#endif - -static unsigned short gdb_port; -static const char *DIGITS = "0123456789abcdef"; - -static void gdb_log_callback(void *priv, const char *file, int line, - const char *function, const char *format, va_list args); - -enum gdb_detach_mode -{ - GDB_DETACH_RESUME, - GDB_DETACH_RESET, - GDB_DETACH_HALT, - GDB_DETACH_NOTHING -}; - -/* target behaviour on gdb detach */ -enum gdb_detach_mode detach_mode = GDB_DETACH_RESUME; - -/* set if we are sending a memory map to gdb - * via qXfer:memory-map:read packet */ -int gdb_use_memory_map = 0; -int gdb_flash_program = 0; - -/* if set, data aborts cause an error to be reported in memory read packets - * see the code in gdb_read_memory_packet() for further explanations */ -int gdb_report_data_abort = 0; - -int gdb_last_signal(target_t *target) -{ - switch (target->debug_reason) - { - case DBG_REASON_DBGRQ: - return 0x2; /* SIGINT */ - case DBG_REASON_BREAKPOINT: - case DBG_REASON_WATCHPOINT: - case DBG_REASON_WPTANDBKPT: - return 0x05; /* SIGTRAP */ - case DBG_REASON_SINGLESTEP: - return 0x05; /* SIGTRAP */ - case DBG_REASON_NOTHALTED: - return 0x0; /* no signal... shouldn't happen */ - default: - ERROR("BUG: undefined debug reason"); - exit(-1); - } -} - -int gdb_get_char(connection_t *connection, int* next_char) -{ - gdb_connection_t *gdb_con = connection->priv; - -#ifdef _DEBUG_GDB_IO_ - char *debug_buffer; -#endif - - if (gdb_con->buf_cnt-- > 0) - { - *next_char = *(gdb_con->buf_p++); - if (gdb_con->buf_cnt > 0) - connection->input_pending = 1; - else - connection->input_pending = 0; - -#ifdef _DEBUG_GDB_IO_ - DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char); -#endif - - return ERROR_OK; - } - - for (;;) - { -#ifndef _WIN32 - /* a non-blocking socket will block if there is 0 bytes available on the socket, - * but return with as many bytes as are available immediately - */ - struct timeval tv; - fd_set read_fds; - - FD_ZERO(&read_fds); - FD_SET(connection->fd, &read_fds); - - tv.tv_sec = 1; - tv.tv_usec = 0; - if (select(connection->fd + 1, &read_fds, NULL, NULL, &tv) == 0) - { - /* This can typically be because a "monitor" command took too long - * before printing any progress messages - */ - return ERROR_GDB_TIMEOUT; - } -#endif - gdb_con->buf_cnt = read_socket(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE); - if (gdb_con->buf_cnt > 0) - { - break; - } - if (gdb_con->buf_cnt == 0) - { - gdb_con->closed = 1; - return ERROR_SERVER_REMOTE_CLOSED; - } - -#ifdef _WIN32 - errno = WSAGetLastError(); - - switch(errno) - { - case WSAEWOULDBLOCK: - usleep(1000); - break; - case WSAECONNABORTED: - return ERROR_SERVER_REMOTE_CLOSED; - case WSAECONNRESET: - return ERROR_SERVER_REMOTE_CLOSED; - default: - ERROR("read: %d", errno); - exit(-1); - } -#else - switch(errno) - { - case EAGAIN: - usleep(1000); - break; - case ECONNABORTED: - return ERROR_SERVER_REMOTE_CLOSED; - case ECONNRESET: - return ERROR_SERVER_REMOTE_CLOSED; - default: - ERROR("read: %s", strerror(errno)); - return ERROR_SERVER_REMOTE_CLOSED; - } -#endif - } - -#ifdef _DEBUG_GDB_IO_ - debug_buffer = malloc(gdb_con->buf_cnt + 1); - memcpy(debug_buffer, gdb_con->buffer, gdb_con->buf_cnt); - debug_buffer[gdb_con->buf_cnt] = 0; - DEBUG("received '%s'", debug_buffer); - free(debug_buffer); -#endif - - gdb_con->buf_p = gdb_con->buffer; - gdb_con->buf_cnt--; - *next_char = *(gdb_con->buf_p++); - if (gdb_con->buf_cnt > 0) - connection->input_pending = 1; - else - connection->input_pending = 0; -#ifdef _DEBUG_GDB_IO_ - DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char); -#endif - - return ERROR_OK; -} - -int gdb_putback_char(connection_t *connection, int last_char) -{ - gdb_connection_t *gdb_con = connection->priv; - - if (gdb_con->buf_p > gdb_con->buffer) - { - *(--gdb_con->buf_p) = last_char; - gdb_con->buf_cnt++; - } - else - { - ERROR("BUG: couldn't put character back"); - } - - return ERROR_OK; -} - -/* The only way we can detect that the socket is closed is the first time - * we write to it, we will fail. Subsequent write operations will - * succeed. Shudder! */ -int gdb_write(connection_t *connection, void *data, int len) -{ - gdb_connection_t *gdb_con = connection->priv; - if (gdb_con->closed) - return ERROR_SERVER_REMOTE_CLOSED; - - if (write_socket(connection->fd, data, len) == len) - { - return ERROR_OK; - } - gdb_con->closed = 1; - return ERROR_SERVER_REMOTE_CLOSED; -} - -int gdb_put_packet_inner(connection_t *connection, char *buffer, int len) -{ - int i; - unsigned char my_checksum = 0; -#ifdef _DEBUG_GDB_IO_ - char *debug_buffer; -#endif - int reply; - int retval; - gdb_connection_t *gdb_con = connection->priv; - - for (i = 0; i < len; i++) - my_checksum += buffer[i]; - - while (1) - { -#ifdef _DEBUG_GDB_IO_ - debug_buffer = malloc(len + 1); - memcpy(debug_buffer, buffer, len); - debug_buffer[len] = 0; - DEBUG("sending packet '$%s#%2.2x'", debug_buffer, my_checksum); - free(debug_buffer); -#endif -#if 0 - char checksum[3]; - gdb_write(connection, "$", 1); - if (len > 0) - gdb_write(connection, buffer, len); - gdb_write(connection, "#", 1); - - snprintf(checksum, 3, "%2.2x", my_checksum); - - gdb_write(connection, checksum, 2); -#else - void *allocated = NULL; - char stackAlloc[1024]; - char *t = stackAlloc; - int totalLen = 1 + len + 1 + 2; - if (totalLen > sizeof(stackAlloc)) - { - allocated = malloc(totalLen); - t = allocated; - if (allocated == NULL) - { - ERROR("Ran out of memory trying to reply packet %d\n", totalLen); - exit(-1); - } - } - t[0] = '$'; - memcpy(t + 1, buffer, len); - t[1 + len] = '#'; - t[1 + len + 1] = DIGITS[(my_checksum >> 4) & 0xf]; - t[1 + len + 2] = DIGITS[my_checksum & 0xf]; - - gdb_write(connection, t, totalLen); - - if (allocated) - { - free(allocated); - } -#endif - if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK) - return retval; - - if (reply == '+') - break; - else if (reply == '-') - { - /* Stop sending output packets for now */ - log_setCallback(NULL, NULL); - WARNING("negative reply, retrying"); - } - else if (reply == 0x3) - { - gdb_con->ctrl_c = 1; - if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK) - return retval; - if (reply == '+') - break; - else if (reply == '-') - { - /* Stop sending output packets for now */ - log_setCallback(NULL, NULL); - WARNING("negative reply, retrying"); - } - else - { - ERROR("unknown character 0x%2.2x in reply, dropping connection", reply); - return ERROR_SERVER_REMOTE_CLOSED; - } - } - else - { - ERROR("unknown character 0x%2.2x in reply, dropping connection", reply); - return ERROR_SERVER_REMOTE_CLOSED; - } - } - if (gdb_con->closed) - return ERROR_SERVER_REMOTE_CLOSED; - - return ERROR_OK; -} - -int gdb_put_packet(connection_t *connection, char *buffer, int len) -{ - gdb_connection_t *gdb_con = connection->priv; - gdb_con->busy = 1; - int retval = gdb_put_packet_inner(connection, buffer, len); - gdb_con->busy = 0; - return retval; -} - -int gdb_get_packet_inner(connection_t *connection, char *buffer, int *len) -{ - int character; - int count = 0; - int retval; - char checksum[3]; - unsigned char my_checksum = 0; - gdb_connection_t *gdb_con = connection->priv; - - while (1) - { - do - { - if ((retval = gdb_get_char(connection, &character)) != ERROR_OK) - return retval; - -#ifdef _DEBUG_GDB_IO_ - DEBUG("character: '%c'", character); -#endif - - switch (character) - { - case '$': - break; - case '+': - WARNING("acknowledgment received, but no packet pending"); - break; - case '-': - WARNING("negative acknowledgment, but no packet pending"); - break; - case 0x3: - gdb_con->ctrl_c = 1; - *len = 0; - return ERROR_OK; - default: - WARNING("ignoring character 0x%x", character); - break; - } - } while (character != '$'); - - my_checksum = 0; - - count = 0; - gdb_connection_t *gdb_con = connection->priv; - for (;;) - { - /* The common case is that we have an entire packet with no escape chars. - * We need to leave at least 2 bytes in the buffer to have - * gdb_get_char() update various bits and bobs correctly. - */ - if ((gdb_con->buf_cnt > 2) && ((gdb_con->buf_cnt+count) < *len)) - { - /* The compiler will struggle a bit with constant propagation and - * aliasing, so we help it by showing that these values do not - * change inside the loop - */ - int i; - char *buf = gdb_con->buf_p; - int run = gdb_con->buf_cnt - 2; - i = 0; - int done = 0; - while (i < run) - { - character = *buf++; - i++; - if (character == '#') - { - /* Danger! character can be '#' when esc is - * used so we need an explicit boolean for done here. - */ - done = 1; - break; - } - - if (character == '}') - { - /* data transmitted in binary mode (X packet) - * uses 0x7d as escape character */ - my_checksum += character & 0xff; - character = *buf++; - i++; - my_checksum += character & 0xff; - buffer[count++] = (character ^ 0x20) & 0xff; - } else - { - my_checksum += character & 0xff; - buffer[count++] = character & 0xff; - } - } - gdb_con->buf_p += i; - gdb_con->buf_cnt -= i; - if (done) - break; - } - if (count > *len) - { - ERROR("packet buffer too small"); - return ERROR_GDB_BUFFER_TOO_SMALL; - } - - if ((retval = gdb_get_char(connection, &character)) != ERROR_OK) - return retval; - - if (character == '#') - break; - - if (character == '}') - { - /* data transmitted in binary mode (X packet) - * uses 0x7d as escape character */ - my_checksum += character & 0xff; - if ((retval = gdb_get_char(connection, &character)) != ERROR_OK) - return retval; - my_checksum += character & 0xff; - buffer[count++] = (character ^ 0x20) & 0xff; - } - else - { - my_checksum += character & 0xff; - buffer[count++] = character & 0xff; - } - - } - - *len = count; - - if ((retval = gdb_get_char(connection, &character)) != ERROR_OK) - return retval; - checksum[0] = character; - if ((retval = gdb_get_char(connection, &character)) != ERROR_OK) - return retval; - checksum[1] = character; - checksum[2] = 0; - - if (my_checksum == strtoul(checksum, NULL, 16)) - { - gdb_write(connection, "+", 1); - break; - } - - WARNING("checksum error, requesting retransmission"); - gdb_write(connection, "-", 1); - } - if (gdb_con->closed) - return ERROR_SERVER_REMOTE_CLOSED; - - return ERROR_OK; -} - -int gdb_get_packet(connection_t *connection, char *buffer, int *len) -{ - gdb_connection_t *gdb_con = connection->priv; - gdb_con->busy = 1; - int retval = gdb_get_packet_inner(connection, buffer, len); - gdb_con->busy = 0; - return retval; -} - -int gdb_output_con(connection_t *connection, char* line) -{ - char *hex_buffer; - int i, bin_size; - - bin_size = strlen(line); - - hex_buffer = malloc(bin_size*2 + 4); - - hex_buffer[0] = 'O'; - for (i=0; i<bin_size; i++) - snprintf(hex_buffer + 1 + i*2, 3, "%2.2x", line[i]); - hex_buffer[bin_size*2+1] = '0'; - hex_buffer[bin_size*2+2] = 'a'; - hex_buffer[bin_size*2+3] = 0x0; - - gdb_put_packet(connection, hex_buffer, bin_size*2 + 3); - - free(hex_buffer); - return ERROR_OK; -} - -int gdb_output(struct command_context_s *context, char* line) -{ - /* this will be dumped to the log and also sent as an O packet if possible */ - USER(line); - return ERROR_OK; -} - -int gdb_program_handler(struct target_s *target, enum target_event event, void *priv) -{ - FILE *script; - struct command_context_s *cmd_ctx = priv; - - if (target->gdb_program_script) - { - script = open_file_from_path(cmd_ctx, target->gdb_program_script, "r"); - if (!script) - { - ERROR("couldn't open script file %s", target->gdb_program_script); - return ERROR_OK; - } - - INFO("executing gdb_program script '%s'", target->gdb_program_script); - command_run_file(cmd_ctx, script, COMMAND_EXEC); - fclose(script); - - jtag_execute_queue(); - } - - return ERROR_OK; -} - -int gdb_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv) -{ - connection_t *connection = priv; - gdb_connection_t *gdb_connection = connection->priv; - char sig_reply[4]; - int signal; - - switch (event) - { - case TARGET_EVENT_HALTED: - /* In the GDB protocol when we are stepping or coninuing execution, - * we have a lingering reply. Upon receiving a halted event - * when we have that lingering packet, we reply to the original - * step or continue packet. - * - * Executing monitor commands can bring the target in and - * out of the running state so we'll see lots of TARGET_EVENT_XXX - * that are to be ignored. - */ - if (gdb_connection->frontend_state == TARGET_RUNNING) - { - /* stop forwarding log packets! */ - log_setCallback(NULL, NULL); - - if (gdb_connection->ctrl_c) - { - signal = 0x2; - gdb_connection->ctrl_c = 0; - } - else - { - signal = gdb_last_signal(target); - } - - snprintf(sig_reply, 4, "T%2.2x", signal); - gdb_put_packet(connection, sig_reply, 3); - gdb_connection->frontend_state = TARGET_HALTED; - } - break; - case TARGET_EVENT_GDB_PROGRAM: - gdb_program_handler(target, event, connection->cmd_ctx); - break; - default: - break; - } - - return ERROR_OK; -} - -int gdb_new_connection(connection_t *connection) -{ - gdb_connection_t *gdb_connection = malloc(sizeof(gdb_connection_t)); - gdb_service_t *gdb_service = connection->service->priv; - int retval; - int initial_ack; - - connection->priv = gdb_connection; - - /* initialize gdb connection information */ - gdb_connection->buf_p = gdb_connection->buffer; - gdb_connection->buf_cnt = 0; - gdb_connection->ctrl_c = 0; - gdb_connection->frontend_state = TARGET_HALTED; - gdb_connection->vflash_image = NULL; - gdb_connection->closed = 0; - gdb_connection->busy = 0; - - /* output goes through gdb connection */ - command_set_output_handler(connection->cmd_ctx, gdb_output, connection); - - /* register callback to be informed about target events */ - target_register_event_callback(gdb_target_callback_event_handler, connection); - - /* a gdb session just attached, put the target in halt mode */ - if (((retval = gdb_service->target->type->halt(gdb_service->target)) != ERROR_OK) && - (retval != ERROR_TARGET_ALREADY_HALTED)) - { - ERROR("error(%d) when trying to halt target, falling back to \"reset halt\"", retval); - command_run_line(connection->cmd_ctx, "reset halt"); - } - - /* This will time out after 1 second */ - command_run_line(connection->cmd_ctx, "wait_halt 1"); - - /* remove the initial ACK from the incoming buffer */ - if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK) - return retval; - - if (initial_ack != '+') - gdb_putback_char(connection, initial_ack); - - return ERROR_OK; -} - -int gdb_connection_closed(connection_t *connection) -{ - gdb_service_t *gdb_service = connection->service->priv; - gdb_connection_t *gdb_connection = connection->priv; - - /* see if an image built with vFlash commands is left */ - if (gdb_connection->vflash_image) - { - image_close(gdb_connection->vflash_image); - free(gdb_connection->vflash_image); - gdb_connection->vflash_image = NULL; - } - - /* if this connection registered a debug-message receiver delete it */ - delete_debug_msg_receiver(connection->cmd_ctx, gdb_service->target); - - if (connection->priv) - { - free(connection->priv); - connection->priv = NULL; - } - else - { - ERROR("BUG: connection->priv == NULL"); - } - - target_unregister_event_callback(gdb_target_callback_event_handler, connection); - log_setCallback(NULL, NULL); - - return ERROR_OK; -} - -void gdb_send_error(connection_t *connection, u8 the_error) -{ - char err[4]; - snprintf(err, 4, "E%2.2X", the_error ); - gdb_put_packet(connection, err, 3); -} - -int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size) -{ - char sig_reply[4]; - int signal; - - signal = gdb_last_signal(target); - - snprintf(sig_reply, 4, "S%2.2x", signal); - gdb_put_packet(connection, sig_reply, 3); - - return ERROR_OK; -} - -/* Convert register to string of bits. NB! The # of bits in the - * register might be non-divisible by 8(a byte), in which - * case an entire byte is shown. */ -void gdb_str_to_target(target_t *target, char *tstr, reg_t *reg) -{ - int i; - - u8 *buf; - int buf_len; - buf = reg->value; - buf_len = CEIL(reg->size, 8); - - if (target->endianness == TARGET_LITTLE_ENDIAN) - { - for (i = 0; i < buf_len; i++) - { - tstr[i*2] = DIGITS[(buf[i]>>4) & 0xf]; - tstr[i*2+1] = DIGITS[buf[i]&0xf]; - } - } - else - { - for (i = 0; i < buf_len; i++) - { - tstr[(buf_len-1-i)*2] = DIGITS[(buf[i]>>4)&0xf]; - tstr[(buf_len-1-i)*2+1] = DIGITS[buf[i]&0xf]; - } - } -} - -void gdb_target_to_str(target_t *target, char *tstr, char *str) -{ - int str_len = strlen(tstr); - int i; - - if (str_len % 2) - { - ERROR("BUG: gdb value with uneven number of characters encountered"); - exit(-1); - } - - if (target->endianness == TARGET_LITTLE_ENDIAN) - { - for (i = 0; i < str_len; i+=2) - { - str[str_len - i - 1] = tstr[i + 1]; - str[str_len - i - 2] = tstr[i]; - } - } - else - { - for (i = 0; i < str_len; i++) - { - str[i] = tstr[i]; - } - } -} - -int gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size) -{ - reg_t **reg_list; - int reg_list_size; - int retval; - int reg_packet_size = 0; - char *reg_packet; - char *reg_packet_p; - int i; - -#ifdef _DEBUG_GDB_IO_ - DEBUG("-"); -#endif - - if ((retval = target->type->get_gdb_reg_list(target, ®_list, ®_list_size)) != ERROR_OK) - { - switch (retval) - { - case ERROR_TARGET_NOT_HALTED: - ERROR("gdb requested registers but we're not halted, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - default: - /* this is a bug condition - get_gdb_reg_list() may not return any other error */ - ERROR("BUG: unexpected error returned by get_gdb_reg_list()"); - exit(-1); - } - } - - for (i = 0; i < reg_list_size; i++) - { - reg_packet_size += reg_list[i]->size; - } - - reg_packet = malloc(CEIL(reg_packet_size, 8) * 2); - reg_packet_p = reg_packet; - - for (i = 0; i < reg_list_size; i++) - { - gdb_str_to_target(target, reg_packet_p, reg_list[i]); - reg_packet_p += CEIL(reg_list[i]->size, 8) * 2; - } - -#ifdef _DEBUG_GDB_IO_ - { - char *reg_packet_p; - reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2); - DEBUG("reg_packet: %s", reg_packet_p); - free(reg_packet_p); - } -#endif - - gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2); - free(reg_packet); - - free(reg_list); - - return ERROR_OK; -} - -int gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - int i; - reg_t **reg_list; - int reg_list_size; - int retval; - char *packet_p; - -#ifdef _DEBUG_GDB_IO_ - DEBUG("-"); -#endif - - /* skip command character */ - packet++; - packet_size--; - - if (packet_size % 2) - { - WARNING("GDB set_registers packet with uneven characters received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - if ((retval = target->type->get_gdb_reg_list(target, ®_list, ®_list_size)) != ERROR_OK) - { - switch (retval) - { - case ERROR_TARGET_NOT_HALTED: - ERROR("gdb tried to registers but we're not halted, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - default: - /* this is a bug condition - get_gdb_reg_list() may not return any other error */ - ERROR("BUG: unexpected error returned by get_gdb_reg_list()"); - exit(-1); - } - } - - packet_p = packet; - for (i = 0; i < reg_list_size; i++) - { - u8 *bin_buf; - char *hex_buf; - reg_arch_type_t *arch_type; - - /* convert from GDB-string (target-endian) to hex-string (big-endian) */ - hex_buf = malloc(CEIL(reg_list[i]->size, 8) * 2); - gdb_target_to_str(target, packet_p, hex_buf); - - /* convert hex-string to binary buffer */ - bin_buf = malloc(CEIL(reg_list[i]->size, 8)); - str_to_buf(hex_buf, CEIL(reg_list[i]->size, 8) * 2, bin_buf, reg_list[i]->size, 16); - - /* get register arch_type, and call set method */ - arch_type = register_get_arch_type(reg_list[i]->arch_type); - if (arch_type == NULL) - { - ERROR("BUG: encountered unregistered arch type"); - exit(-1); - } - arch_type->set(reg_list[i], bin_buf); - - /* advance packet pointer */ - packet_p += (CEIL(reg_list[i]->size, 8) * 2); - - free(bin_buf); - free(hex_buf); - } - - /* free reg_t *reg_list[] array allocated by get_gdb_reg_list */ - free(reg_list); - - gdb_put_packet(connection, "OK", 2); - - return ERROR_OK; -} - -int gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - char *reg_packet; - int reg_num = strtoul(packet + 1, NULL, 16); - reg_t **reg_list; - int reg_list_size; - int retval; - -#ifdef _DEBUG_GDB_IO_ - DEBUG("-"); -#endif - - if ((retval = target->type->get_gdb_reg_list(target, ®_list, ®_list_size)) != ERROR_OK) - { - switch (retval) - { - case ERROR_TARGET_NOT_HALTED: - ERROR("gdb requested registers but we're not halted, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - default: - /* this is a bug condition - get_gdb_reg_list() may not return any other error */ - ERROR("BUG: unexpected error returned by get_gdb_reg_list()"); - exit(-1); - } - } - - if (reg_list_size <= reg_num) - { - ERROR("gdb requested a non-existing register"); - exit(-1); - } - - reg_packet = malloc(CEIL(reg_list[reg_num]->size, 8) * 2); - - gdb_str_to_target(target, reg_packet, reg_list[reg_num]); - - gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2); - - free(reg_list); - free(reg_packet); - - return ERROR_OK; -} - -int gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - char *separator; - char *hex_buf; - u8 *bin_buf; - int reg_num = strtoul(packet + 1, &separator, 16); - reg_t **reg_list; - int reg_list_size; - int retval; - reg_arch_type_t *arch_type; - - DEBUG("-"); - - if ((retval = target->type->get_gdb_reg_list(target, ®_list, ®_list_size)) != ERROR_OK) - { - switch (retval) - { - case ERROR_TARGET_NOT_HALTED: - ERROR("gdb tried to set a register but we're not halted, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - default: - /* this is a bug condition - get_gdb_reg_list() may not return any other error */ - ERROR("BUG: unexpected error returned by get_gdb_reg_list()"); - exit(-1); - } - } - - if (reg_list_size < reg_num) - { - ERROR("gdb requested a non-existing register"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - if (*separator != '=') - { - ERROR("GDB 'set register packet', but no '=' following the register number"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - /* convert from GDB-string (target-endian) to hex-string (big-endian) */ - hex_buf = malloc(CEIL(reg_list[reg_num]->size, 8) * 2); - gdb_target_to_str(target, separator + 1, hex_buf); - - /* convert hex-string to binary buffer */ - bin_buf = malloc(CEIL(reg_list[reg_num]->size, 8)); - str_to_buf(hex_buf, CEIL(reg_list[reg_num]->size, 8) * 2, bin_buf, reg_list[reg_num]->size, 16); - - /* get register arch_type, and call set method */ - arch_type = register_get_arch_type(reg_list[reg_num]->arch_type); - if (arch_type == NULL) - { - ERROR("BUG: encountered unregistered arch type"); - exit(-1); - } - arch_type->set(reg_list[reg_num], bin_buf); - - gdb_put_packet(connection, "OK", 2); - - free(bin_buf); - free(hex_buf); - free(reg_list); - - return ERROR_OK; -} - -int gdb_memory_packet_error(connection_t *connection, int retval) -{ - switch (retval) - { - case ERROR_TARGET_NOT_HALTED: - ERROR("gdb tried to read memory but we're not halted, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - case ERROR_TARGET_DATA_ABORT: - gdb_send_error(connection, EIO); - break; - case ERROR_TARGET_TRANSLATION_FAULT: - gdb_send_error(connection, EFAULT); - break; - case ERROR_TARGET_UNALIGNED_ACCESS: - gdb_send_error(connection, EFAULT); - break; - default: - /* This could be that the target reset itself. */ - ERROR("unexpected error %i. Dropping connection.", retval); - return ERROR_SERVER_REMOTE_CLOSED; - } - - return ERROR_OK; -} - -/* We don't have to worry about the default 2 second timeout for GDB packets, - * because GDB breaks up large memory reads into smaller reads. - * - * 8191 bytes by the looks of it. Why 8191 bytes instead of 8192????? - */ -int gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - char *separator; - u32 addr = 0; - u32 len = 0; - - u8 *buffer; - char *hex_buffer; - - int retval = ERROR_OK; - - /* skip command character */ - packet++; - - addr = strtoul(packet, &separator, 16); - - if (*separator != ',') - { - ERROR("incomplete read memory packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - len = strtoul(separator+1, NULL, 16); - - buffer = malloc(len); - - DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len); - - retval = target_read_buffer(target, addr, len, buffer); - - if ((retval == ERROR_TARGET_DATA_ABORT) && (!gdb_report_data_abort)) - { - /* TODO : Here we have to lie and send back all zero's lest stack traces won't work. - * At some point this might be fixed in GDB, in which case this code can be removed. - * - * OpenOCD developers are acutely aware of this problem, but there is nothing - * gained by involving the user in this problem that hopefully will get resolved - * eventually - * - * http://sourceware.org/cgi-bin/gnatsweb.pl?cmd=view%20audit-trail&database=gdb&pr=2395 - * - * For now, the default is to fix up things to make current GDB versions work. - * This can be overwritten using the gdb_report_data_abort <'enable'|'disable'> command. - */ - memset(buffer, 0, len); - retval = ERROR_OK; - } - - if (retval == ERROR_OK) - { - hex_buffer = malloc(len * 2 + 1); - - int i; - for (i = 0; i < len; i++) - { - u8 t = buffer[i]; - hex_buffer[2 * i] = DIGITS[(t >> 4) & 0xf]; - hex_buffer[2 * i + 1] = DIGITS[t & 0xf]; - } - - gdb_put_packet(connection, hex_buffer, len * 2); - - free(hex_buffer); - } - else - { - retval = gdb_memory_packet_error(connection, retval); - } - - free(buffer); - - return retval; -} - -int gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - char *separator; - u32 addr = 0; - u32 len = 0; - - u8 *buffer; - - int i; - int retval; - - /* skip command character */ - packet++; - - addr = strtoul(packet, &separator, 16); - - if (*separator != ',') - { - ERROR("incomplete write memory packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - len = strtoul(separator+1, &separator, 16); - - if (*(separator++) != ':') - { - ERROR("incomplete write memory packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - buffer = malloc(len); - - DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len); - - for (i=0; i<len; i++) - { - u32 tmp; - sscanf(separator + 2*i, "%2x", &tmp); - buffer[i] = tmp; - } - - retval = target_write_buffer(target, addr, len, buffer); - - if (retval == ERROR_OK) - { - gdb_put_packet(connection, "OK", 2); - } - else - { - if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK) - return retval; - } - - free(buffer); - - return ERROR_OK; -} - -int gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - char *separator; - u32 addr = 0; - u32 len = 0; - - int retval; - - /* skip command character */ - packet++; - - addr = strtoul(packet, &separator, 16); - - if (*separator != ',') - { - ERROR("incomplete write memory binary packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - len = strtoul(separator+1, &separator, 16); - - if (*(separator++) != ':') - { - ERROR("incomplete write memory binary packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - retval = ERROR_OK; - if (len) - { - DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len); - - retval = target_write_buffer(target, addr, len, (u8*)separator); - } - - if (retval == ERROR_OK) - { - gdb_put_packet(connection, "OK", 2); - } - else - { - if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK) - return retval; - } - - return ERROR_OK; -} - -void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - int current = 0; - u32 address = 0x0; - - DEBUG("-"); - - if (packet_size > 1) - { - packet[packet_size] = 0; - address = strtoul(packet + 1, NULL, 16); - } - else - { - current = 1; - } - - if (packet[0] == 'c') - { - DEBUG("continue"); - target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */ - } - else if (packet[0] == 's') - { - DEBUG("step"); - target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */ - } -} - -int gdb_bp_wp_packet_error(connection_t *connection, int retval) -{ - switch (retval) - { - case ERROR_TARGET_NOT_HALTED: - ERROR("gdb tried to set a breakpoint but we're not halted, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - break; - case ERROR_TARGET_RESOURCE_NOT_AVAILABLE: - gdb_send_error(connection, EBUSY); - break; - default: - ERROR("BUG: unexpected error %i", retval); - exit(-1); - } - - return ERROR_OK; -} - -int gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - int type; - enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */; - enum watchpoint_rw wp_type; - u32 address; - u32 size; - char *separator; - int retval; - - DEBUG("-"); - - type = strtoul(packet + 1, &separator, 16); - - if (type == 0) /* memory breakpoint */ - bp_type = BKPT_SOFT; - else if (type == 1) /* hardware breakpoint */ - bp_type = BKPT_HARD; - else if (type == 2) /* write watchpoint */ - wp_type = WPT_WRITE; - else if (type == 3) /* read watchpoint */ - wp_type = WPT_READ; - else if (type == 4) /* access watchpoint */ - wp_type = WPT_ACCESS; - - if (*separator != ',') - { - ERROR("incomplete breakpoint/watchpoint packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - address = strtoul(separator+1, &separator, 16); - - if (*separator != ',') - { - ERROR("incomplete breakpoint/watchpoint packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - size = strtoul(separator+1, &separator, 16); - - switch (type) - { - case 0: - case 1: - if (packet[0] == 'Z') - { - if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK) - { - if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK) - return retval; - } - else - { - gdb_put_packet(connection, "OK", 2); - } - } - else - { - breakpoint_remove(target, address); - gdb_put_packet(connection, "OK", 2); - } - break; - case 2: - case 3: - case 4: - { - if (packet[0] == 'Z') - { - if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK) - { - if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK) - return retval; - } - else - { - gdb_put_packet(connection, "OK", 2); - } - } - else - { - watchpoint_remove(target, address); - gdb_put_packet(connection, "OK", 2); - } - break; - } - default: - break; - } - - return ERROR_OK; -} - -/* print out a string and allocate more space as needed, mainly used for XML at this point */ -void xml_printf(int *retval, char **xml, int *pos, int *size, const char *fmt, ...) -{ - if (*retval != ERROR_OK) - { - return; - } - int first = 1; - - for (;;) - { - if ((*xml == NULL) || (!first)) - { - /* start by 0 to exercise all the code paths. - * Need minimum 2 bytes to fit 1 char and 0 terminator. */ - - *size = *size * 2 + 2; - char *t = *xml; - *xml = realloc(*xml, *size); - if (*xml == NULL) - { - if (t) - free(t); - *retval = ERROR_SERVER_REMOTE_CLOSED; - return; - } - } - - va_list ap; - int ret; - va_start(ap, fmt); - ret = vsnprintf(*xml + *pos, *size - *pos, fmt, ap); - va_end(ap); - if ((ret > 0) && ((ret + 1) < *size - *pos)) - { - *pos += ret; - return; - } - /* there was just enough or not enough space, allocate more. */ - first = 0; - } -} - -static int decode_xfer_read(char *buf, char **annex, int *ofs, unsigned int *len) -{ - char *separator; - - /* Extract and NUL-terminate the annex. */ - *annex = buf; - while (*buf && *buf != ':') - buf++; - if (*buf == '\0') - return -1; - *buf++ = 0; - - /* After the read marker and annex, qXfer looks like a - * traditional 'm' packet. */ - - *ofs = strtoul(buf, &separator, 16); - - if (*separator != ',') - return -1; - - *len = strtoul(separator+1, NULL, 16); - - return 0; -} - -int gdb_calc_blocksize(flash_bank_t *bank) -{ - int i; - int block_size = 0xffffffff; - - /* loop through all sectors and return smallest sector size */ - - for (i = 0; i < bank->num_sectors; i++) - { - if (bank->sectors[i].size < block_size) - block_size = bank->sectors[i].size; - } - - return block_size; -} - -int gdb_query_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - command_context_t *cmd_ctx = connection->cmd_ctx; - - if (strstr(packet, "qRcmd,")) - { - if (packet_size > 6) - { - char *cmd; - int i; - cmd = malloc((packet_size - 6)/2 + 1); - for (i=0; i < (packet_size - 6)/2; i++) - { - u32 tmp; - sscanf(packet + 6 + 2*i, "%2x", &tmp); - cmd[i] = tmp; - } - cmd[(packet_size - 6)/2] = 0x0; - - /* We want to print all debug output to GDB connection */ - log_setCallback(gdb_log_callback, connection); - target_call_timer_callbacks(); - command_run_line(cmd_ctx, cmd); - free(cmd); - } - gdb_put_packet(connection, "OK", 2); - return ERROR_OK; - } - else if (strstr(packet, "qCRC:")) - { - if (packet_size > 5) - { - int retval; - char gdb_reply[10]; - char *separator; - u32 checksum; - u32 addr = 0; - u32 len = 0; - - /* skip command character */ - packet += 5; - - addr = strtoul(packet, &separator, 16); - - if (*separator != ',') - { - ERROR("incomplete read memory packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - len = strtoul(separator + 1, NULL, 16); - - retval = target_checksum_memory(target, addr, len, &checksum); - - if (retval == ERROR_OK) - { - snprintf(gdb_reply, 10, "C%8.8x", checksum); - gdb_put_packet(connection, gdb_reply, 9); - } - else - { - if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK) - return retval; - } - - return ERROR_OK; - } - } - else if (strstr(packet, "qSupported")) - { - /* we currently support packet size and qXfer:memory-map:read (if enabled) - * disable qXfer:features:read for the moment */ - int retval = ERROR_OK; - char *buffer = NULL; - int pos = 0; - int size = 0; - - xml_printf(&retval, &buffer, &pos, &size, - "PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read-", - (GDB_BUFFER_SIZE - 1), gdb_use_memory_map == 1 ? '+' : '-'); - - if (retval != ERROR_OK) - { - gdb_send_error(connection, 01); - return ERROR_OK; - } - - gdb_put_packet(connection, buffer, strlen(buffer)); - free(buffer); - - return ERROR_OK; - } - else if (strstr(packet, "qXfer:memory-map:read::")) - { - /* We get away with only specifying flash here. Regions that are not - * specified are treated as if we provided no memory map(if not we - * could detect the holes and mark them as RAM). - * Normally we only execute this code once, but no big deal if we - * have to regenerate it a couple of times. */ - - flash_bank_t *p; - char *xml = NULL; - int size = 0; - int pos = 0; - int retval = ERROR_OK; - - int offset; - int length; - char *separator; - int blocksize; - - /* skip command character */ - packet += 23; - - offset = strtoul(packet, &separator, 16); - length = strtoul(separator + 1, &separator, 16); - - xml_printf(&retval, &xml, &pos, &size, "<memory-map>\n"); - - int i = 0; - for (;;) - { - p = get_flash_bank_by_num(i); - if (p == NULL) - break; - - /* if device has uneven sector sizes, eg. str7, lpc - * we pass the smallest sector size to gdb memory map */ - blocksize = gdb_calc_blocksize(p); - - xml_printf(&retval, &xml, &pos, &size, "<memory type=\"flash\" start=\"0x%x\" length=\"0x%x\">\n" \ - "<property name=\"blocksize\">0x%x</property>\n" \ - "</memory>\n", \ - p->base, p->size, blocksize); - i++; - } - - xml_printf(&retval, &xml, &pos, &size, "</memory-map>\n"); - - if (retval != ERROR_OK) - { - gdb_send_error(connection, retval); - return retval; - } - - if (offset + length > pos) - { - length = pos - offset; - } - - char *t = malloc(length + 1); - t[0] = 'l'; - memcpy(t + 1, xml + offset, length); - gdb_put_packet(connection, t, length + 1); - - free(t); - free(xml); - return ERROR_OK; - } - else if (strstr(packet, "qXfer:features:read:")) - { - char *xml = NULL; - int size = 0; - int pos = 0; - int retval = ERROR_OK; - - int offset; - unsigned int length; - char *annex; - - /* skip command character */ - packet += 20; - - if (decode_xfer_read(packet, &annex, &offset, &length) < 0) - { - gdb_send_error(connection, 01); - return ERROR_OK; - } - - if (strcmp(annex, "target.xml") != 0) - { - gdb_send_error(connection, 01); - return ERROR_OK; - } - - xml_printf(&retval, &xml, &pos, &size, \ - "l<target version=\"1.0\">\n<architecture>arm</architecture>\n</target>\n"); - - if (retval != ERROR_OK) - { - gdb_send_error(connection, retval); - return retval; - } - - gdb_put_packet(connection, xml, strlen(xml) + 1); - - free(xml); - return ERROR_OK; - } - - gdb_put_packet(connection, "", 0); - return ERROR_OK; -} - -int gdb_v_packet(connection_t *connection, target_t *target, char *packet, int packet_size) -{ - gdb_connection_t *gdb_connection = connection->priv; - gdb_service_t *gdb_service = connection->service->priv; - int result; - - /* if flash programming disabled - send a empty reply */ - - if (gdb_flash_program == 0) - { - gdb_put_packet(connection, "", 0); - return ERROR_OK; - } - - if (strstr(packet, "vFlashErase:")) - { - unsigned long addr; - unsigned long length; - - char *parse = packet + 12; - if (*parse == '\0') - { - ERROR("incomplete vFlashErase packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - addr = strtoul(parse, &parse, 16); - - if (*(parse++) != ',' || *parse == '\0') - { - ERROR("incomplete vFlashErase packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - length = strtoul(parse, &parse, 16); - - if (*parse != '\0') - { - ERROR("incomplete vFlashErase packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - - /* assume all sectors need erasing - stops any problems - * when flash_write is called multiple times */ - flash_set_dirty(); - - /* perform any target specific operations before the erase */ - target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_PROGRAM); - - /* perform erase */ - if ((result = flash_erase_address_range(gdb_service->target, addr, length)) != ERROR_OK) - { - /* GDB doesn't evaluate the actual error number returned, - * treat a failed erase as an I/O error - */ - gdb_send_error(connection, EIO); - ERROR("flash_erase returned %i", result); - } - else - gdb_put_packet(connection, "OK", 2); - - return ERROR_OK; - } - - if (strstr(packet, "vFlashWrite:")) - { - unsigned long addr; - unsigned long length; - char *parse = packet + 12; - - if (*parse == '\0') - { - ERROR("incomplete vFlashErase packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - addr = strtoul(parse, &parse, 16); - if (*(parse++) != ':') - { - ERROR("incomplete vFlashErase packet received, dropping connection"); - return ERROR_SERVER_REMOTE_CLOSED; - } - length = packet_size - (parse - packet); - - /* create a new image if there isn't already one */ - if (gdb_connection->vflash_image == NULL) - { - gdb_connection->vflash_image = malloc(sizeof(image_t)); - image_open(gdb_connection->vflash_image, "", "build"); - } - - /* create new section with content from packet buffer */ - image_add_section(gdb_connection->vflash_image, addr, length, 0x0, (u8*)parse); - - gdb_put_packet(connection, "OK", 2); - - return ERROR_OK; - } - - if (!strcmp(packet, "vFlashDone")) - { - u32 written; - - /* process the flashing buffer. No need to erase as GDB - * always issues a vFlashErase first. */ - if ((result = flash_write(gdb_service->target, gdb_connection->vflash_image, &written, 0)) != ERROR_OK) - { - if (result == ERROR_FLASH_DST_OUT_OF_BANK) - gdb_put_packet(connection, "E.memtype", 9); - else - gdb_send_error(connection, EIO); - } - else - { - DEBUG("wrote %u bytes from vFlash image to flash", written); - gdb_put_packet(connection, "OK", 2); - } - - image_close(gdb_connection->vflash_image); - free(gdb_connection->vflash_image); - gdb_connection->vflash_image = NULL; - - return ERROR_OK; - } - - gdb_put_packet(connection, "", 0); - return ERROR_OK; -} - -int gdb_detach(connection_t *connection, target_t *target) -{ - switch( detach_mode ) - { - case GDB_DETACH_RESUME: - target->type->resume(target, 1, 0, 1, 0); - break; - - case GDB_DETACH_RESET: - target_process_reset(connection->cmd_ctx); - break; - - case GDB_DETACH_HALT: - target->type->halt(target); - break; - - case GDB_DETACH_NOTHING: - break; - } - - gdb_put_packet(connection, "OK", 2); - - return ERROR_OK; -} - -static void gdb_log_callback(void *priv, const char *file, int line, - const char *function, const char *format, va_list args) -{ - connection_t *connection = priv; - gdb_connection_t *gdb_con = connection->priv; - - if (gdb_con->busy) - { - /* do not reply this using the O packet */ - return; - } - - char *t = allocPrintf(format, args); - if (t == NULL) - return; - - gdb_output_con(connection, t); - - free(t); -} - -int gdb_input_inner(connection_t *connection) -{ - gdb_service_t *gdb_service = connection->service->priv; - target_t *target = gdb_service->target; - char packet[GDB_BUFFER_SIZE]; - int packet_size; - int retval; - gdb_connection_t *gdb_con = connection->priv; - static int extended_protocol = 0; - - /* drain input buffer */ - do - { - packet_size = GDB_BUFFER_SIZE-1; - if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK) - { - return retval; - } - - /* terminate with zero */ - packet[packet_size] = 0; - - DEBUG("received packet: '%s'", packet); - - if (packet_size > 0) - { - retval = ERROR_OK; - switch (packet[0]) - { - case 'H': - /* Hct... -- set thread - * we don't have threads, send empty reply */ - gdb_put_packet(connection, NULL, 0); - break; - case 'q': - retval = gdb_query_packet(connection, target, packet, packet_size); - break; - case 'g': - retval = gdb_get_registers_packet(connection, target, packet, packet_size); - break; - case 'G': - retval = gdb_set_registers_packet(connection, target, packet, packet_size); - break; - case 'p': - retval = gdb_get_register_packet(connection, target, packet, packet_size); - break; - case 'P': - retval = gdb_set_register_packet(connection, target, packet, packet_size); - break; - case 'm': - retval = gdb_read_memory_packet(connection, target, packet, packet_size); - break; - case 'M': - retval = gdb_write_memory_packet(connection, target, packet, packet_size); - break; - case 'z': - case 'Z': - retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size); - break; - case '?': - gdb_last_signal_packet(connection, target, packet, packet_size); - break; - case 'c': - case 's': - { - /* We're running/stepping, in which case we can - * forward log output until the target is halted */ - gdb_connection_t *gdb_con = connection->priv; - gdb_con->frontend_state = TARGET_RUNNING; - log_setCallback(gdb_log_callback, connection); - gdb_step_continue_packet(connection, target, packet, packet_size); - } - break; - case 'v': - retval = gdb_v_packet(connection, target, packet, packet_size); - break; - case 'D': - retval = gdb_detach(connection, target); - extended_protocol = 0; - break; - case 'X': - if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK) - return retval; - break; - case 'k': - if (extended_protocol != 0) - break; - gdb_put_packet(connection, "OK", 2); - return ERROR_SERVER_REMOTE_CLOSED; - case '!': - /* handle extended remote protocol */ - extended_protocol = 1; - gdb_put_packet(connection, "OK", 2); - break; - case 'R': - /* handle extended restart packet */ - target_process_reset(connection->cmd_ctx); - break; - default: - /* ignore unkown packets */ - DEBUG("ignoring 0x%2.2x packet", packet[0]); - gdb_put_packet(connection, NULL, 0); - break; - } - - /* if a packet handler returned an error, exit input loop */ - if (retval != ERROR_OK) - return retval; - } - - if (gdb_con->ctrl_c) - { - if (target->state == TARGET_RUNNING) - { - target->type->halt(target); - gdb_con->ctrl_c = 0; - } - } - - } while (gdb_con->buf_cnt > 0); - - return ERROR_OK; -} - -int gdb_input(connection_t *connection) -{ - int retval = gdb_input_inner(connection); - if (retval == ERROR_SERVER_REMOTE_CLOSED) - return retval; - /* we'll recover from any other errors(e.g. temporary timeouts, etc.) */ - return ERROR_OK; -} - -int gdb_init() -{ - gdb_service_t *gdb_service; - target_t *target = targets; - int i = 0; - - if (!target) - { - WARNING("no gdb ports allocated as no target has been specified"); - return ERROR_OK; - } - - if (gdb_port == 0) - { - WARNING("no gdb port specified, using default port 3333"); - gdb_port = 3333; - } - - while (target) - { - char service_name[8]; - - snprintf(service_name, 8, "gdb-%2.2i", i); - - gdb_service = malloc(sizeof(gdb_service_t)); - gdb_service->target = target; - - add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service); - - DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i); - - i++; - target = target->next; - } - - return ERROR_OK; -} - -/* daemon configuration command gdb_port */ -int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - if (argc == 0) - return ERROR_OK; - - /* only if the port wasn't overwritten by cmdline */ - if (gdb_port == 0) - gdb_port = strtoul(args[0], NULL, 0); - - return ERROR_OK; -} - -int handle_gdb_detach_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - if (argc == 1) - { - if (strcmp(args[0], "resume") == 0) - { - detach_mode = GDB_DETACH_RESUME; - return ERROR_OK; - } - else if (strcmp(args[0], "reset") == 0) - { - detach_mode = GDB_DETACH_RESET; - return ERROR_OK; - } - else if (strcmp(args[0], "halt") == 0) - { - detach_mode = GDB_DETACH_HALT; - return ERROR_OK; - } - else if (strcmp(args[0], "nothing") == 0) - { - detach_mode = GDB_DETACH_NOTHING; - return ERROR_OK; - } - } - - WARNING("invalid gdb_detach configuration directive: %s", args[0]); - return ERROR_OK; -} - -int handle_gdb_memory_map_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - if (argc == 1) - { - if (strcmp(args[0], "enable") == 0) - { - gdb_use_memory_map = 1; - return ERROR_OK; - } - else if (strcmp(args[0], "disable") == 0) - { - gdb_use_memory_map = 0; - return ERROR_OK; - } - } - - WARNING("invalid gdb_memory_map configuration directive: %s", args[0]); - return ERROR_OK; -} - -int handle_gdb_flash_program_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - if (argc == 1) - { - if (strcmp(args[0], "enable") == 0) - { - gdb_flash_program = 1; - return ERROR_OK; - } - else if (strcmp(args[0], "disable") == 0) - { - gdb_flash_program = 0; - return ERROR_OK; - } - } - - WARNING("invalid gdb_memory_map configuration directive: %s", args[0]); - return ERROR_OK; -} - -int handle_gdb_report_data_abort_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - if (argc == 1) - { - if (strcmp(args[0], "enable") == 0) - { - gdb_report_data_abort = 1; - return ERROR_OK; - } - else if (strcmp(args[0], "disable") == 0) - { - gdb_report_data_abort = 0; - return ERROR_OK; - } - } - - WARNING("invalid gdb_report_data_abort configuration directive: %s", args[0]); - return ERROR_OK; -} - -int gdb_register_commands(command_context_t *command_context) -{ - register_command(command_context, NULL, "gdb_port", handle_gdb_port_command, - COMMAND_CONFIG, ""); - register_command(command_context, NULL, "gdb_detach", handle_gdb_detach_command, - COMMAND_CONFIG, ""); - register_command(command_context, NULL, "gdb_memory_map", handle_gdb_memory_map_command, - COMMAND_CONFIG, ""); - register_command(command_context, NULL, "gdb_flash_program", handle_gdb_flash_program_command, - COMMAND_CONFIG, ""); - register_command(command_context, NULL, "gdb_report_data_abort", handle_gdb_report_data_abort_command, - COMMAND_CONFIG, ""); - return ERROR_OK; -} +/***************************************************************************
+ * Copyright (C) 2005 by Dominic Rath *
+ * Dominic.Rath@gmx.de *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
+#include "gdb_server.h"
+
+#include "server.h"
+#include "log.h"
+#include "binarybuffer.h"
+#include "jtag.h"
+#include "breakpoints.h"
+#include "flash.h"
+#include "target_request.h"
+#include "configuration.h"
+
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+
+#if 0
+#define _DEBUG_GDB_IO_
+#endif
+
+static unsigned short gdb_port;
+static const char *DIGITS = "0123456789abcdef";
+
+static void gdb_log_callback(void *priv, const char *file, int line,
+ const char *function, const char *format, va_list args);
+
+enum gdb_detach_mode
+{
+ GDB_DETACH_RESUME,
+ GDB_DETACH_RESET,
+ GDB_DETACH_HALT,
+ GDB_DETACH_NOTHING
+};
+
+/* target behaviour on gdb detach */
+enum gdb_detach_mode detach_mode = GDB_DETACH_RESUME;
+
+/* set if we are sending a memory map to gdb
+ * via qXfer:memory-map:read packet */
+int gdb_use_memory_map = 0;
+int gdb_flash_program = 0;
+
+/* if set, data aborts cause an error to be reported in memory read packets
+ * see the code in gdb_read_memory_packet() for further explanations */
+int gdb_report_data_abort = 0;
+
+int gdb_last_signal(target_t *target)
+{
+ switch (target->debug_reason)
+ {
+ case DBG_REASON_DBGRQ:
+ return 0x2; /* SIGINT */
+ case DBG_REASON_BREAKPOINT:
+ case DBG_REASON_WATCHPOINT:
+ case DBG_REASON_WPTANDBKPT:
+ return 0x05; /* SIGTRAP */
+ case DBG_REASON_SINGLESTEP:
+ return 0x05; /* SIGTRAP */
+ case DBG_REASON_NOTHALTED:
+ return 0x0; /* no signal... shouldn't happen */
+ default:
+ ERROR("BUG: undefined debug reason");
+ exit(-1);
+ }
+}
+
+int gdb_get_char(connection_t *connection, int* next_char)
+{
+ gdb_connection_t *gdb_con = connection->priv;
+
+#ifdef _DEBUG_GDB_IO_
+ char *debug_buffer;
+#endif
+
+ if (gdb_con->buf_cnt-- > 0)
+ {
+ *next_char = *(gdb_con->buf_p++);
+ if (gdb_con->buf_cnt > 0)
+ connection->input_pending = 1;
+ else
+ connection->input_pending = 0;
+
+#ifdef _DEBUG_GDB_IO_
+ DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
+#endif
+
+ return ERROR_OK;
+ }
+
+ for (;;)
+ {
+#ifndef _WIN32
+ /* a non-blocking socket will block if there is 0 bytes available on the socket,
+ * but return with as many bytes as are available immediately
+ */
+ struct timeval tv;
+ fd_set read_fds;
+
+ FD_ZERO(&read_fds);
+ FD_SET(connection->fd, &read_fds);
+
+ tv.tv_sec = 1;
+ tv.tv_usec = 0;
+ if (select(connection->fd + 1, &read_fds, NULL, NULL, &tv) == 0)
+ {
+ /* This can typically be because a "monitor" command took too long
+ * before printing any progress messages
+ */
+ return ERROR_GDB_TIMEOUT;
+ }
+#endif
+ gdb_con->buf_cnt = read_socket(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE);
+ if (gdb_con->buf_cnt > 0)
+ {
+ break;
+ }
+ if (gdb_con->buf_cnt == 0)
+ {
+ gdb_con->closed = 1;
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+#ifdef _WIN32
+ errno = WSAGetLastError();
+
+ switch(errno)
+ {
+ case WSAEWOULDBLOCK:
+ usleep(1000);
+ break;
+ case WSAECONNABORTED:
+ return ERROR_SERVER_REMOTE_CLOSED;
+ case WSAECONNRESET:
+ return ERROR_SERVER_REMOTE_CLOSED;
+ default:
+ ERROR("read: %d", errno);
+ exit(-1);
+ }
+#else
+ switch(errno)
+ {
+ case EAGAIN:
+ usleep(1000);
+ break;
+ case ECONNABORTED:
+ return ERROR_SERVER_REMOTE_CLOSED;
+ case ECONNRESET:
+ return ERROR_SERVER_REMOTE_CLOSED;
+ default:
+ ERROR("read: %s", strerror(errno));
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+#endif
+ }
+
+#ifdef _DEBUG_GDB_IO_
+ debug_buffer = malloc(gdb_con->buf_cnt + 1);
+ memcpy(debug_buffer, gdb_con->buffer, gdb_con->buf_cnt);
+ debug_buffer[gdb_con->buf_cnt] = 0;
+ DEBUG("received '%s'", debug_buffer);
+ free(debug_buffer);
+#endif
+
+ gdb_con->buf_p = gdb_con->buffer;
+ gdb_con->buf_cnt--;
+ *next_char = *(gdb_con->buf_p++);
+ if (gdb_con->buf_cnt > 0)
+ connection->input_pending = 1;
+ else
+ connection->input_pending = 0;
+#ifdef _DEBUG_GDB_IO_
+ DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
+#endif
+
+ return ERROR_OK;
+}
+
+int gdb_putback_char(connection_t *connection, int last_char)
+{
+ gdb_connection_t *gdb_con = connection->priv;
+
+ if (gdb_con->buf_p > gdb_con->buffer)
+ {
+ *(--gdb_con->buf_p) = last_char;
+ gdb_con->buf_cnt++;
+ }
+ else
+ {
+ ERROR("BUG: couldn't put character back");
+ }
+
+ return ERROR_OK;
+}
+
+/* The only way we can detect that the socket is closed is the first time
+ * we write to it, we will fail. Subsequent write operations will
+ * succeed. Shudder! */
+int gdb_write(connection_t *connection, void *data, int len)
+{
+ gdb_connection_t *gdb_con = connection->priv;
+ if (gdb_con->closed)
+ return ERROR_SERVER_REMOTE_CLOSED;
+
+ if (write_socket(connection->fd, data, len) == len)
+ {
+ return ERROR_OK;
+ }
+ gdb_con->closed = 1;
+ return ERROR_SERVER_REMOTE_CLOSED;
+}
+
+int gdb_put_packet_inner(connection_t *connection, char *buffer, int len)
+{
+ int i;
+ unsigned char my_checksum = 0;
+#ifdef _DEBUG_GDB_IO_
+ char *debug_buffer;
+#endif
+ int reply;
+ int retval;
+ gdb_connection_t *gdb_con = connection->priv;
+
+ for (i = 0; i < len; i++)
+ my_checksum += buffer[i];
+
+ while (1)
+ {
+#ifdef _DEBUG_GDB_IO_
+ debug_buffer = malloc(len + 1);
+ memcpy(debug_buffer, buffer, len);
+ debug_buffer[len] = 0;
+ DEBUG("sending packet '$%s#%2.2x'", debug_buffer, my_checksum);
+ free(debug_buffer);
+#endif
+#if 0
+ char checksum[3];
+ gdb_write(connection, "$", 1);
+ if (len > 0)
+ gdb_write(connection, buffer, len);
+ gdb_write(connection, "#", 1);
+
+ snprintf(checksum, 3, "%2.2x", my_checksum);
+
+ gdb_write(connection, checksum, 2);
+#else
+ void *allocated = NULL;
+ char stackAlloc[1024];
+ char *t = stackAlloc;
+ int totalLen = 1 + len + 1 + 2;
+ if (totalLen > sizeof(stackAlloc))
+ {
+ allocated = malloc(totalLen);
+ t = allocated;
+ if (allocated == NULL)
+ {
+ ERROR("Ran out of memory trying to reply packet %d\n", totalLen);
+ exit(-1);
+ }
+ }
+ t[0] = '$';
+ memcpy(t + 1, buffer, len);
+ t[1 + len] = '#';
+ t[1 + len + 1] = DIGITS[(my_checksum >> 4) & 0xf];
+ t[1 + len + 2] = DIGITS[my_checksum & 0xf];
+
+ gdb_write(connection, t, totalLen);
+
+ if (allocated)
+ {
+ free(allocated);
+ }
+#endif
+ if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
+ return retval;
+
+ if (reply == '+')
+ break;
+ else if (reply == '-')
+ {
+ /* Stop sending output packets for now */
+ log_remove_callback(gdb_log_callback, connection);
+ WARNING("negative reply, retrying");
+ }
+ else if (reply == 0x3)
+ {
+ gdb_con->ctrl_c = 1;
+ if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
+ return retval;
+ if (reply == '+')
+ break;
+ else if (reply == '-')
+ {
+ /* Stop sending output packets for now */
+ log_remove_callback(gdb_log_callback, connection);
+ WARNING("negative reply, retrying");
+ }
+ else
+ {
+ ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+ }
+ else
+ {
+ ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+ }
+ if (gdb_con->closed)
+ return ERROR_SERVER_REMOTE_CLOSED;
+
+ return ERROR_OK;
+}
+
+int gdb_put_packet(connection_t *connection, char *buffer, int len)
+{
+ gdb_connection_t *gdb_con = connection->priv;
+ gdb_con->busy = 1;
+ int retval = gdb_put_packet_inner(connection, buffer, len);
+ gdb_con->busy = 0;
+ return retval;
+}
+
+int gdb_get_packet_inner(connection_t *connection, char *buffer, int *len)
+{
+ int character;
+ int count = 0;
+ int retval;
+ char checksum[3];
+ unsigned char my_checksum = 0;
+ gdb_connection_t *gdb_con = connection->priv;
+
+ while (1)
+ {
+ do
+ {
+ if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
+ return retval;
+
+#ifdef _DEBUG_GDB_IO_
+ DEBUG("character: '%c'", character);
+#endif
+
+ switch (character)
+ {
+ case '$':
+ break;
+ case '+':
+ WARNING("acknowledgment received, but no packet pending");
+ break;
+ case '-':
+ WARNING("negative acknowledgment, but no packet pending");
+ break;
+ case 0x3:
+ gdb_con->ctrl_c = 1;
+ *len = 0;
+ return ERROR_OK;
+ default:
+ WARNING("ignoring character 0x%x", character);
+ break;
+ }
+ } while (character != '$');
+
+ my_checksum = 0;
+
+ count = 0;
+ gdb_connection_t *gdb_con = connection->priv;
+ for (;;)
+ {
+ /* The common case is that we have an entire packet with no escape chars.
+ * We need to leave at least 2 bytes in the buffer to have
+ * gdb_get_char() update various bits and bobs correctly.
+ */
+ if ((gdb_con->buf_cnt > 2) && ((gdb_con->buf_cnt+count) < *len))
+ {
+ /* The compiler will struggle a bit with constant propagation and
+ * aliasing, so we help it by showing that these values do not
+ * change inside the loop
+ */
+ int i;
+ char *buf = gdb_con->buf_p;
+ int run = gdb_con->buf_cnt - 2;
+ i = 0;
+ int done = 0;
+ while (i < run)
+ {
+ character = *buf++;
+ i++;
+ if (character == '#')
+ {
+ /* Danger! character can be '#' when esc is
+ * used so we need an explicit boolean for done here.
+ */
+ done = 1;
+ break;
+ }
+
+ if (character == '}')
+ {
+ /* data transmitted in binary mode (X packet)
+ * uses 0x7d as escape character */
+ my_checksum += character & 0xff;
+ character = *buf++;
+ i++;
+ my_checksum += character & 0xff;
+ buffer[count++] = (character ^ 0x20) & 0xff;
+ } else
+ {
+ my_checksum += character & 0xff;
+ buffer[count++] = character & 0xff;
+ }
+ }
+ gdb_con->buf_p += i;
+ gdb_con->buf_cnt -= i;
+ if (done)
+ break;
+ }
+ if (count > *len)
+ {
+ ERROR("packet buffer too small");
+ return ERROR_GDB_BUFFER_TOO_SMALL;
+ }
+
+ if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
+ return retval;
+
+ if (character == '#')
+ break;
+
+ if (character == '}')
+ {
+ /* data transmitted in binary mode (X packet)
+ * uses 0x7d as escape character */
+ my_checksum += character & 0xff;
+ if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
+ return retval;
+ my_checksum += character & 0xff;
+ buffer[count++] = (character ^ 0x20) & 0xff;
+ }
+ else
+ {
+ my_checksum += character & 0xff;
+ buffer[count++] = character & 0xff;
+ }
+
+ }
+
+ *len = count;
+
+ if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
+ return retval;
+ checksum[0] = character;
+ if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
+ return retval;
+ checksum[1] = character;
+ checksum[2] = 0;
+
+ if (my_checksum == strtoul(checksum, NULL, 16))
+ {
+ gdb_write(connection, "+", 1);
+ break;
+ }
+
+ WARNING("checksum error, requesting retransmission");
+ gdb_write(connection, "-", 1);
+ }
+ if (gdb_con->closed)
+ return ERROR_SERVER_REMOTE_CLOSED;
+
+ return ERROR_OK;
+}
+
+int gdb_get_packet(connection_t *connection, char *buffer, int *len)
+{
+ gdb_connection_t *gdb_con = connection->priv;
+ gdb_con->busy = 1;
+ int retval = gdb_get_packet_inner(connection, buffer, len);
+ gdb_con->busy = 0;
+ return retval;
+}
+
+int gdb_output_con(connection_t *connection, char* line)
+{
+ char *hex_buffer;
+ int i, bin_size;
+
+ bin_size = strlen(line);
+
+ hex_buffer = malloc(bin_size*2 + 4);
+
+ hex_buffer[0] = 'O';
+ for (i=0; i<bin_size; i++)
+ snprintf(hex_buffer + 1 + i*2, 3, "%2.2x", line[i]);
+ hex_buffer[bin_size*2+1] = '0';
+ hex_buffer[bin_size*2+2] = 'a';
+ hex_buffer[bin_size*2+3] = 0x0;
+
+ gdb_put_packet(connection, hex_buffer, bin_size*2 + 3);
+
+ free(hex_buffer);
+ return ERROR_OK;
+}
+
+int gdb_output(struct command_context_s *context, char* line)
+{
+ /* this will be dumped to the log and also sent as an O packet if possible */
+ USER(line);
+ return ERROR_OK;
+}
+
+int gdb_program_handler(struct target_s *target, enum target_event event, void *priv)
+{
+ FILE *script;
+ struct command_context_s *cmd_ctx = priv;
+
+ if (target->gdb_program_script)
+ {
+ script = open_file_from_path(cmd_ctx, target->gdb_program_script, "r");
+ if (!script)
+ {
+ ERROR("couldn't open script file %s", target->gdb_program_script);
+ return ERROR_OK;
+ }
+
+ INFO("executing gdb_program script '%s'", target->gdb_program_script);
+ command_run_file(cmd_ctx, script, COMMAND_EXEC);
+ fclose(script);
+
+ jtag_execute_queue();
+ }
+
+ return ERROR_OK;
+}
+
+int gdb_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
+{
+ connection_t *connection = priv;
+ gdb_connection_t *gdb_connection = connection->priv;
+ char sig_reply[4];
+ int signal;
+
+ switch (event)
+ {
+ case TARGET_EVENT_HALTED:
+ /* In the GDB protocol when we are stepping or coninuing execution,
+ * we have a lingering reply. Upon receiving a halted event
+ * when we have that lingering packet, we reply to the original
+ * step or continue packet.
+ *
+ * Executing monitor commands can bring the target in and
+ * out of the running state so we'll see lots of TARGET_EVENT_XXX
+ * that are to be ignored.
+ */
+ if (gdb_connection->frontend_state == TARGET_RUNNING)
+ {
+ /* stop forwarding log packets! */
+ log_remove_callback(gdb_log_callback, connection);
+
+ if (gdb_connection->ctrl_c)
+ {
+ signal = 0x2;
+ gdb_connection->ctrl_c = 0;
+ }
+ else
+ {
+ signal = gdb_last_signal(target);
+ }
+
+ snprintf(sig_reply, 4, "T%2.2x", signal);
+ gdb_put_packet(connection, sig_reply, 3);
+ gdb_connection->frontend_state = TARGET_HALTED;
+ }
+ break;
+ case TARGET_EVENT_GDB_PROGRAM:
+ gdb_program_handler(target, event, connection->cmd_ctx);
+ break;
+ default:
+ break;
+ }
+
+ return ERROR_OK;
+}
+
+int gdb_new_connection(connection_t *connection)
+{
+ gdb_connection_t *gdb_connection = malloc(sizeof(gdb_connection_t));
+ gdb_service_t *gdb_service = connection->service->priv;
+ int retval;
+ int initial_ack;
+
+ connection->priv = gdb_connection;
+
+ /* initialize gdb connection information */
+ gdb_connection->buf_p = gdb_connection->buffer;
+ gdb_connection->buf_cnt = 0;
+ gdb_connection->ctrl_c = 0;
+ gdb_connection->frontend_state = TARGET_HALTED;
+ gdb_connection->vflash_image = NULL;
+ gdb_connection->closed = 0;
+ gdb_connection->busy = 0;
+
+ /* output goes through gdb connection */
+ command_set_output_handler(connection->cmd_ctx, gdb_output, connection);
+
+ /* register callback to be informed about target events */
+ target_register_event_callback(gdb_target_callback_event_handler, connection);
+
+ /* a gdb session just attached, put the target in halt mode */
+ if (((retval = gdb_service->target->type->halt(gdb_service->target)) != ERROR_OK) &&
+ (retval != ERROR_TARGET_ALREADY_HALTED))
+ {
+ ERROR("error(%d) when trying to halt target, falling back to \"reset halt\"", retval);
+ command_run_line(connection->cmd_ctx, "reset halt");
+ }
+
+ /* This will time out after 1 second */
+ command_run_line(connection->cmd_ctx, "wait_halt 1");
+
+ /* remove the initial ACK from the incoming buffer */
+ if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
+ return retval;
+
+ if (initial_ack != '+')
+ gdb_putback_char(connection, initial_ack);
+
+ return ERROR_OK;
+}
+
+int gdb_connection_closed(connection_t *connection)
+{
+ gdb_service_t *gdb_service = connection->service->priv;
+ gdb_connection_t *gdb_connection = connection->priv;
+
+ /* see if an image built with vFlash commands is left */
+ if (gdb_connection->vflash_image)
+ {
+ image_close(gdb_connection->vflash_image);
+ free(gdb_connection->vflash_image);
+ gdb_connection->vflash_image = NULL;
+ }
+
+ /* if this connection registered a debug-message receiver delete it */
+ delete_debug_msg_receiver(connection->cmd_ctx, gdb_service->target);
+
+ if (connection->priv)
+ {
+ free(connection->priv);
+ connection->priv = NULL;
+ }
+ else
+ {
+ ERROR("BUG: connection->priv == NULL");
+ }
+
+ target_unregister_event_callback(gdb_target_callback_event_handler, connection);
+ log_remove_callback(gdb_log_callback, connection);
+
+ return ERROR_OK;
+}
+
+void gdb_send_error(connection_t *connection, u8 the_error)
+{
+ char err[4];
+ snprintf(err, 4, "E%2.2X", the_error );
+ gdb_put_packet(connection, err, 3);
+}
+
+int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
+{
+ char sig_reply[4];
+ int signal;
+
+ signal = gdb_last_signal(target);
+
+ snprintf(sig_reply, 4, "S%2.2x", signal);
+ gdb_put_packet(connection, sig_reply, 3);
+
+ return ERROR_OK;
+}
+
+/* Convert register to string of bits. NB! The # of bits in the
+ * register might be non-divisible by 8(a byte), in which
+ * case an entire byte is shown. */
+void gdb_str_to_target(target_t *target, char *tstr, reg_t *reg)
+{
+ int i;
+
+ u8 *buf;
+ int buf_len;
+ buf = reg->value;
+ buf_len = CEIL(reg->size, 8);
+
+ if (target->endianness == TARGET_LITTLE_ENDIAN)
+ {
+ for (i = 0; i < buf_len; i++)
+ {
+ tstr[i*2] = DIGITS[(buf[i]>>4) & 0xf];
+ tstr[i*2+1] = DIGITS[buf[i]&0xf];
+ }
+ }
+ else
+ {
+ for (i = 0; i < buf_len; i++)
+ {
+ tstr[(buf_len-1-i)*2] = DIGITS[(buf[i]>>4)&0xf];
+ tstr[(buf_len-1-i)*2+1] = DIGITS[buf[i]&0xf];
+ }
+ }
+}
+
+void gdb_target_to_str(target_t *target, char *tstr, char *str)
+{
+ int str_len = strlen(tstr);
+ int i;
+
+ if (str_len % 2)
+ {
+ ERROR("BUG: gdb value with uneven number of characters encountered");
+ exit(-1);
+ }
+
+ if (target->endianness == TARGET_LITTLE_ENDIAN)
+ {
+ for (i = 0; i < str_len; i+=2)
+ {
+ str[str_len - i - 1] = tstr[i + 1];
+ str[str_len - i - 2] = tstr[i];
+ }
+ }
+ else
+ {
+ for (i = 0; i < str_len; i++)
+ {
+ str[i] = tstr[i];
+ }
+ }
+}
+
+int gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
+{
+ reg_t **reg_list;
+ int reg_list_size;
+ int retval;
+ int reg_packet_size = 0;
+ char *reg_packet;
+ char *reg_packet_p;
+ int i;
+
+#ifdef _DEBUG_GDB_IO_
+ DEBUG("-");
+#endif
+
+ if ((retval = target->type->get_gdb_reg_list(target, ®_list, ®_list_size)) != ERROR_OK)
+ {
+ switch (retval)
+ {
+ case ERROR_TARGET_NOT_HALTED:
+ ERROR("gdb requested registers but we're not halted, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ default:
+ /* this is a bug condition - get_gdb_reg_list() may not return any other error */
+ ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
+ exit(-1);
+ }
+ }
+
+ for (i = 0; i < reg_list_size; i++)
+ {
+ reg_packet_size += reg_list[i]->size;
+ }
+
+ reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);
+ reg_packet_p = reg_packet;
+
+ for (i = 0; i < reg_list_size; i++)
+ {
+ gdb_str_to_target(target, reg_packet_p, reg_list[i]);
+ reg_packet_p += CEIL(reg_list[i]->size, 8) * 2;
+ }
+
+#ifdef _DEBUG_GDB_IO_
+ {
+ char *reg_packet_p;
+ reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
+ DEBUG("reg_packet: %s", reg_packet_p);
+ free(reg_packet_p);
+ }
+#endif
+
+ gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
+ free(reg_packet);
+
+ free(reg_list);
+
+ return ERROR_OK;
+}
+
+int gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ int i;
+ reg_t **reg_list;
+ int reg_list_size;
+ int retval;
+ char *packet_p;
+
+#ifdef _DEBUG_GDB_IO_
+ DEBUG("-");
+#endif
+
+ /* skip command character */
+ packet++;
+ packet_size--;
+
+ if (packet_size % 2)
+ {
+ WARNING("GDB set_registers packet with uneven characters received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ if ((retval = target->type->get_gdb_reg_list(target, ®_list, ®_list_size)) != ERROR_OK)
+ {
+ switch (retval)
+ {
+ case ERROR_TARGET_NOT_HALTED:
+ ERROR("gdb tried to registers but we're not halted, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ default:
+ /* this is a bug condition - get_gdb_reg_list() may not return any other error */
+ ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
+ exit(-1);
+ }
+ }
+
+ packet_p = packet;
+ for (i = 0; i < reg_list_size; i++)
+ {
+ u8 *bin_buf;
+ char *hex_buf;
+ reg_arch_type_t *arch_type;
+
+ /* convert from GDB-string (target-endian) to hex-string (big-endian) */
+ hex_buf = malloc(CEIL(reg_list[i]->size, 8) * 2);
+ gdb_target_to_str(target, packet_p, hex_buf);
+
+ /* convert hex-string to binary buffer */
+ bin_buf = malloc(CEIL(reg_list[i]->size, 8));
+ str_to_buf(hex_buf, CEIL(reg_list[i]->size, 8) * 2, bin_buf, reg_list[i]->size, 16);
+
+ /* get register arch_type, and call set method */
+ arch_type = register_get_arch_type(reg_list[i]->arch_type);
+ if (arch_type == NULL)
+ {
+ ERROR("BUG: encountered unregistered arch type");
+ exit(-1);
+ }
+ arch_type->set(reg_list[i], bin_buf);
+
+ /* advance packet pointer */
+ packet_p += (CEIL(reg_list[i]->size, 8) * 2);
+
+ free(bin_buf);
+ free(hex_buf);
+ }
+
+ /* free reg_t *reg_list[] array allocated by get_gdb_reg_list */
+ free(reg_list);
+
+ gdb_put_packet(connection, "OK", 2);
+
+ return ERROR_OK;
+}
+
+int gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ char *reg_packet;
+ int reg_num = strtoul(packet + 1, NULL, 16);
+ reg_t **reg_list;
+ int reg_list_size;
+ int retval;
+
+#ifdef _DEBUG_GDB_IO_
+ DEBUG("-");
+#endif
+
+ if ((retval = target->type->get_gdb_reg_list(target, ®_list, ®_list_size)) != ERROR_OK)
+ {
+ switch (retval)
+ {
+ case ERROR_TARGET_NOT_HALTED:
+ ERROR("gdb requested registers but we're not halted, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ default:
+ /* this is a bug condition - get_gdb_reg_list() may not return any other error */
+ ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
+ exit(-1);
+ }
+ }
+
+ if (reg_list_size <= reg_num)
+ {
+ ERROR("gdb requested a non-existing register");
+ exit(-1);
+ }
+
+ reg_packet = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
+
+ gdb_str_to_target(target, reg_packet, reg_list[reg_num]);
+
+ gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
+
+ free(reg_list);
+ free(reg_packet);
+
+ return ERROR_OK;
+}
+
+int gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ char *separator;
+ char *hex_buf;
+ u8 *bin_buf;
+ int reg_num = strtoul(packet + 1, &separator, 16);
+ reg_t **reg_list;
+ int reg_list_size;
+ int retval;
+ reg_arch_type_t *arch_type;
+
+ DEBUG("-");
+
+ if ((retval = target->type->get_gdb_reg_list(target, ®_list, ®_list_size)) != ERROR_OK)
+ {
+ switch (retval)
+ {
+ case ERROR_TARGET_NOT_HALTED:
+ ERROR("gdb tried to set a register but we're not halted, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ default:
+ /* this is a bug condition - get_gdb_reg_list() may not return any other error */
+ ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
+ exit(-1);
+ }
+ }
+
+ if (reg_list_size < reg_num)
+ {
+ ERROR("gdb requested a non-existing register");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ if (*separator != '=')
+ {
+ ERROR("GDB 'set register packet', but no '=' following the register number");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ /* convert from GDB-string (target-endian) to hex-string (big-endian) */
+ hex_buf = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
+ gdb_target_to_str(target, separator + 1, hex_buf);
+
+ /* convert hex-string to binary buffer */
+ bin_buf = malloc(CEIL(reg_list[reg_num]->size, 8));
+ str_to_buf(hex_buf, CEIL(reg_list[reg_num]->size, 8) * 2, bin_buf, reg_list[reg_num]->size, 16);
+
+ /* get register arch_type, and call set method */
+ arch_type = register_get_arch_type(reg_list[reg_num]->arch_type);
+ if (arch_type == NULL)
+ {
+ ERROR("BUG: encountered unregistered arch type");
+ exit(-1);
+ }
+ arch_type->set(reg_list[reg_num], bin_buf);
+
+ gdb_put_packet(connection, "OK", 2);
+
+ free(bin_buf);
+ free(hex_buf);
+ free(reg_list);
+
+ return ERROR_OK;
+}
+
+int gdb_memory_packet_error(connection_t *connection, int retval)
+{
+ switch (retval)
+ {
+ case ERROR_TARGET_NOT_HALTED:
+ ERROR("gdb tried to read memory but we're not halted, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ case ERROR_TARGET_DATA_ABORT:
+ gdb_send_error(connection, EIO);
+ break;
+ case ERROR_TARGET_TRANSLATION_FAULT:
+ gdb_send_error(connection, EFAULT);
+ break;
+ case ERROR_TARGET_UNALIGNED_ACCESS:
+ gdb_send_error(connection, EFAULT);
+ break;
+ default:
+ /* This could be that the target reset itself. */
+ ERROR("unexpected error %i. Dropping connection.", retval);
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ return ERROR_OK;
+}
+
+/* We don't have to worry about the default 2 second timeout for GDB packets,
+ * because GDB breaks up large memory reads into smaller reads.
+ *
+ * 8191 bytes by the looks of it. Why 8191 bytes instead of 8192?????
+ */
+int gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ char *separator;
+ u32 addr = 0;
+ u32 len = 0;
+
+ u8 *buffer;
+ char *hex_buffer;
+
+ int retval = ERROR_OK;
+
+ /* skip command character */
+ packet++;
+
+ addr = strtoul(packet, &separator, 16);
+
+ if (*separator != ',')
+ {
+ ERROR("incomplete read memory packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ len = strtoul(separator+1, NULL, 16);
+
+ buffer = malloc(len);
+
+ DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
+
+ retval = target_read_buffer(target, addr, len, buffer);
+
+ if ((retval == ERROR_TARGET_DATA_ABORT) && (!gdb_report_data_abort))
+ {
+ /* TODO : Here we have to lie and send back all zero's lest stack traces won't work.
+ * At some point this might be fixed in GDB, in which case this code can be removed.
+ *
+ * OpenOCD developers are acutely aware of this problem, but there is nothing
+ * gained by involving the user in this problem that hopefully will get resolved
+ * eventually
+ *
+ * http://sourceware.org/cgi-bin/gnatsweb.pl?cmd=view%20audit-trail&database=gdb&pr=2395
+ *
+ * For now, the default is to fix up things to make current GDB versions work.
+ * This can be overwritten using the gdb_report_data_abort <'enable'|'disable'> command.
+ */
+ memset(buffer, 0, len);
+ retval = ERROR_OK;
+ }
+
+ if (retval == ERROR_OK)
+ {
+ hex_buffer = malloc(len * 2 + 1);
+
+ int i;
+ for (i = 0; i < len; i++)
+ {
+ u8 t = buffer[i];
+ hex_buffer[2 * i] = DIGITS[(t >> 4) & 0xf];
+ hex_buffer[2 * i + 1] = DIGITS[t & 0xf];
+ }
+
+ gdb_put_packet(connection, hex_buffer, len * 2);
+
+ free(hex_buffer);
+ }
+ else
+ {
+ retval = gdb_memory_packet_error(connection, retval);
+ }
+
+ free(buffer);
+
+ return retval;
+}
+
+int gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ char *separator;
+ u32 addr = 0;
+ u32 len = 0;
+
+ u8 *buffer;
+
+ int i;
+ int retval;
+
+ /* skip command character */
+ packet++;
+
+ addr = strtoul(packet, &separator, 16);
+
+ if (*separator != ',')
+ {
+ ERROR("incomplete write memory packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ len = strtoul(separator+1, &separator, 16);
+
+ if (*(separator++) != ':')
+ {
+ ERROR("incomplete write memory packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ buffer = malloc(len);
+
+ DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
+
+ for (i=0; i<len; i++)
+ {
+ u32 tmp;
+ sscanf(separator + 2*i, "%2x", &tmp);
+ buffer[i] = tmp;
+ }
+
+ retval = target_write_buffer(target, addr, len, buffer);
+
+ if (retval == ERROR_OK)
+ {
+ gdb_put_packet(connection, "OK", 2);
+ }
+ else
+ {
+ if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
+ return retval;
+ }
+
+ free(buffer);
+
+ return ERROR_OK;
+}
+
+int gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ char *separator;
+ u32 addr = 0;
+ u32 len = 0;
+
+ int retval;
+
+ /* skip command character */
+ packet++;
+
+ addr = strtoul(packet, &separator, 16);
+
+ if (*separator != ',')
+ {
+ ERROR("incomplete write memory binary packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ len = strtoul(separator+1, &separator, 16);
+
+ if (*(separator++) != ':')
+ {
+ ERROR("incomplete write memory binary packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ retval = ERROR_OK;
+ if (len)
+ {
+ DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
+
+ retval = target_write_buffer(target, addr, len, (u8*)separator);
+ }
+
+ if (retval == ERROR_OK)
+ {
+ gdb_put_packet(connection, "OK", 2);
+ }
+ else
+ {
+ if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
+ return retval;
+ }
+
+ return ERROR_OK;
+}
+
+void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ int current = 0;
+ u32 address = 0x0;
+
+ DEBUG("-");
+
+ if (packet_size > 1)
+ {
+ packet[packet_size] = 0;
+ address = strtoul(packet + 1, NULL, 16);
+ }
+ else
+ {
+ current = 1;
+ }
+
+ if (packet[0] == 'c')
+ {
+ DEBUG("continue");
+ target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
+ }
+ else if (packet[0] == 's')
+ {
+ DEBUG("step");
+ target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */
+ }
+}
+
+int gdb_bp_wp_packet_error(connection_t *connection, int retval)
+{
+ switch (retval)
+ {
+ case ERROR_TARGET_NOT_HALTED:
+ ERROR("gdb tried to set a breakpoint but we're not halted, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ break;
+ case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
+ gdb_send_error(connection, EBUSY);
+ break;
+ default:
+ ERROR("BUG: unexpected error %i", retval);
+ exit(-1);
+ }
+
+ return ERROR_OK;
+}
+
+int gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ int type;
+ enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
+ enum watchpoint_rw wp_type;
+ u32 address;
+ u32 size;
+ char *separator;
+ int retval;
+
+ DEBUG("-");
+
+ type = strtoul(packet + 1, &separator, 16);
+
+ if (type == 0) /* memory breakpoint */
+ bp_type = BKPT_SOFT;
+ else if (type == 1) /* hardware breakpoint */
+ bp_type = BKPT_HARD;
+ else if (type == 2) /* write watchpoint */
+ wp_type = WPT_WRITE;
+ else if (type == 3) /* read watchpoint */
+ wp_type = WPT_READ;
+ else if (type == 4) /* access watchpoint */
+ wp_type = WPT_ACCESS;
+
+ if (*separator != ',')
+ {
+ ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ address = strtoul(separator+1, &separator, 16);
+
+ if (*separator != ',')
+ {
+ ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ size = strtoul(separator+1, &separator, 16);
+
+ switch (type)
+ {
+ case 0:
+ case 1:
+ if (packet[0] == 'Z')
+ {
+ if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
+ {
+ if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
+ return retval;
+ }
+ else
+ {
+ gdb_put_packet(connection, "OK", 2);
+ }
+ }
+ else
+ {
+ breakpoint_remove(target, address);
+ gdb_put_packet(connection, "OK", 2);
+ }
+ break;
+ case 2:
+ case 3:
+ case 4:
+ {
+ if (packet[0] == 'Z')
+ {
+ if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK)
+ {
+ if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
+ return retval;
+ }
+ else
+ {
+ gdb_put_packet(connection, "OK", 2);
+ }
+ }
+ else
+ {
+ watchpoint_remove(target, address);
+ gdb_put_packet(connection, "OK", 2);
+ }
+ break;
+ }
+ default:
+ break;
+ }
+
+ return ERROR_OK;
+}
+
+/* print out a string and allocate more space as needed, mainly used for XML at this point */
+void xml_printf(int *retval, char **xml, int *pos, int *size, const char *fmt, ...)
+{
+ if (*retval != ERROR_OK)
+ {
+ return;
+ }
+ int first = 1;
+
+ for (;;)
+ {
+ if ((*xml == NULL) || (!first))
+ {
+ /* start by 0 to exercise all the code paths.
+ * Need minimum 2 bytes to fit 1 char and 0 terminator. */
+
+ *size = *size * 2 + 2;
+ char *t = *xml;
+ *xml = realloc(*xml, *size);
+ if (*xml == NULL)
+ {
+ if (t)
+ free(t);
+ *retval = ERROR_SERVER_REMOTE_CLOSED;
+ return;
+ }
+ }
+
+ va_list ap;
+ int ret;
+ va_start(ap, fmt);
+ ret = vsnprintf(*xml + *pos, *size - *pos, fmt, ap);
+ va_end(ap);
+ if ((ret > 0) && ((ret + 1) < *size - *pos))
+ {
+ *pos += ret;
+ return;
+ }
+ /* there was just enough or not enough space, allocate more. */
+ first = 0;
+ }
+}
+
+static int decode_xfer_read(char *buf, char **annex, int *ofs, unsigned int *len)
+{
+ char *separator;
+
+ /* Extract and NUL-terminate the annex. */
+ *annex = buf;
+ while (*buf && *buf != ':')
+ buf++;
+ if (*buf == '\0')
+ return -1;
+ *buf++ = 0;
+
+ /* After the read marker and annex, qXfer looks like a
+ * traditional 'm' packet. */
+
+ *ofs = strtoul(buf, &separator, 16);
+
+ if (*separator != ',')
+ return -1;
+
+ *len = strtoul(separator+1, NULL, 16);
+
+ return 0;
+}
+
+int gdb_calc_blocksize(flash_bank_t *bank)
+{
+ int i;
+ int block_size = 0xffffffff;
+
+ /* loop through all sectors and return smallest sector size */
+
+ for (i = 0; i < bank->num_sectors; i++)
+ {
+ if (bank->sectors[i].size < block_size)
+ block_size = bank->sectors[i].size;
+ }
+
+ return block_size;
+}
+
+int gdb_query_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ command_context_t *cmd_ctx = connection->cmd_ctx;
+
+ if (strstr(packet, "qRcmd,"))
+ {
+ if (packet_size > 6)
+ {
+ char *cmd;
+ int i;
+ cmd = malloc((packet_size - 6)/2 + 1);
+ for (i=0; i < (packet_size - 6)/2; i++)
+ {
+ u32 tmp;
+ sscanf(packet + 6 + 2*i, "%2x", &tmp);
+ cmd[i] = tmp;
+ }
+ cmd[(packet_size - 6)/2] = 0x0;
+
+ /* We want to print all debug output to GDB connection */
+ log_add_callback(gdb_log_callback, connection);
+ target_call_timer_callbacks();
+ command_run_line(cmd_ctx, cmd);
+ free(cmd);
+ }
+ gdb_put_packet(connection, "OK", 2);
+ return ERROR_OK;
+ }
+ else if (strstr(packet, "qCRC:"))
+ {
+ if (packet_size > 5)
+ {
+ int retval;
+ char gdb_reply[10];
+ char *separator;
+ u32 checksum;
+ u32 addr = 0;
+ u32 len = 0;
+
+ /* skip command character */
+ packet += 5;
+
+ addr = strtoul(packet, &separator, 16);
+
+ if (*separator != ',')
+ {
+ ERROR("incomplete read memory packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ len = strtoul(separator + 1, NULL, 16);
+
+ retval = target_checksum_memory(target, addr, len, &checksum);
+
+ if (retval == ERROR_OK)
+ {
+ snprintf(gdb_reply, 10, "C%8.8x", checksum);
+ gdb_put_packet(connection, gdb_reply, 9);
+ }
+ else
+ {
+ if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
+ return retval;
+ }
+
+ return ERROR_OK;
+ }
+ }
+ else if (strstr(packet, "qSupported"))
+ {
+ /* we currently support packet size and qXfer:memory-map:read (if enabled)
+ * disable qXfer:features:read for the moment */
+ int retval = ERROR_OK;
+ char *buffer = NULL;
+ int pos = 0;
+ int size = 0;
+
+ xml_printf(&retval, &buffer, &pos, &size,
+ "PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read-",
+ (GDB_BUFFER_SIZE - 1), gdb_use_memory_map == 1 ? '+' : '-');
+
+ if (retval != ERROR_OK)
+ {
+ gdb_send_error(connection, 01);
+ return ERROR_OK;
+ }
+
+ gdb_put_packet(connection, buffer, strlen(buffer));
+ free(buffer);
+
+ return ERROR_OK;
+ }
+ else if (strstr(packet, "qXfer:memory-map:read::"))
+ {
+ /* We get away with only specifying flash here. Regions that are not
+ * specified are treated as if we provided no memory map(if not we
+ * could detect the holes and mark them as RAM).
+ * Normally we only execute this code once, but no big deal if we
+ * have to regenerate it a couple of times. */
+
+ flash_bank_t *p;
+ char *xml = NULL;
+ int size = 0;
+ int pos = 0;
+ int retval = ERROR_OK;
+
+ int offset;
+ int length;
+ char *separator;
+ int blocksize;
+
+ /* skip command character */
+ packet += 23;
+
+ offset = strtoul(packet, &separator, 16);
+ length = strtoul(separator + 1, &separator, 16);
+
+ xml_printf(&retval, &xml, &pos, &size, "<memory-map>\n");
+
+ int i = 0;
+ for (;;)
+ {
+ p = get_flash_bank_by_num(i);
+ if (p == NULL)
+ break;
+
+ /* if device has uneven sector sizes, eg. str7, lpc
+ * we pass the smallest sector size to gdb memory map */
+ blocksize = gdb_calc_blocksize(p);
+
+ xml_printf(&retval, &xml, &pos, &size, "<memory type=\"flash\" start=\"0x%x\" length=\"0x%x\">\n" \
+ "<property name=\"blocksize\">0x%x</property>\n" \
+ "</memory>\n", \
+ p->base, p->size, blocksize);
+ i++;
+ }
+
+ xml_printf(&retval, &xml, &pos, &size, "</memory-map>\n");
+
+ if (retval != ERROR_OK)
+ {
+ gdb_send_error(connection, retval);
+ return retval;
+ }
+
+ if (offset + length > pos)
+ {
+ length = pos - offset;
+ }
+
+ char *t = malloc(length + 1);
+ t[0] = 'l';
+ memcpy(t + 1, xml + offset, length);
+ gdb_put_packet(connection, t, length + 1);
+
+ free(t);
+ free(xml);
+ return ERROR_OK;
+ }
+ else if (strstr(packet, "qXfer:features:read:"))
+ {
+ char *xml = NULL;
+ int size = 0;
+ int pos = 0;
+ int retval = ERROR_OK;
+
+ int offset;
+ unsigned int length;
+ char *annex;
+
+ /* skip command character */
+ packet += 20;
+
+ if (decode_xfer_read(packet, &annex, &offset, &length) < 0)
+ {
+ gdb_send_error(connection, 01);
+ return ERROR_OK;
+ }
+
+ if (strcmp(annex, "target.xml") != 0)
+ {
+ gdb_send_error(connection, 01);
+ return ERROR_OK;
+ }
+
+ xml_printf(&retval, &xml, &pos, &size, \
+ "l<target version=\"1.0\">\n<architecture>arm</architecture>\n</target>\n");
+
+ if (retval != ERROR_OK)
+ {
+ gdb_send_error(connection, retval);
+ return retval;
+ }
+
+ gdb_put_packet(connection, xml, strlen(xml) + 1);
+
+ free(xml);
+ return ERROR_OK;
+ }
+
+ gdb_put_packet(connection, "", 0);
+ return ERROR_OK;
+}
+
+int gdb_v_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ gdb_connection_t *gdb_connection = connection->priv;
+ gdb_service_t *gdb_service = connection->service->priv;
+ int result;
+
+ /* if flash programming disabled - send a empty reply */
+
+ if (gdb_flash_program == 0)
+ {
+ gdb_put_packet(connection, "", 0);
+ return ERROR_OK;
+ }
+
+ if (strstr(packet, "vFlashErase:"))
+ {
+ unsigned long addr;
+ unsigned long length;
+
+ char *parse = packet + 12;
+ if (*parse == '\0')
+ {
+ ERROR("incomplete vFlashErase packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ addr = strtoul(parse, &parse, 16);
+
+ if (*(parse++) != ',' || *parse == '\0')
+ {
+ ERROR("incomplete vFlashErase packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ length = strtoul(parse, &parse, 16);
+
+ if (*parse != '\0')
+ {
+ ERROR("incomplete vFlashErase packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ /* assume all sectors need erasing - stops any problems
+ * when flash_write is called multiple times */
+ flash_set_dirty();
+
+ /* perform any target specific operations before the erase */
+ target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_PROGRAM);
+
+ /* perform erase */
+ if ((result = flash_erase_address_range(gdb_service->target, addr, length)) != ERROR_OK)
+ {
+ /* GDB doesn't evaluate the actual error number returned,
+ * treat a failed erase as an I/O error
+ */
+ gdb_send_error(connection, EIO);
+ ERROR("flash_erase returned %i", result);
+ }
+ else
+ gdb_put_packet(connection, "OK", 2);
+
+ return ERROR_OK;
+ }
+
+ if (strstr(packet, "vFlashWrite:"))
+ {
+ unsigned long addr;
+ unsigned long length;
+ char *parse = packet + 12;
+
+ if (*parse == '\0')
+ {
+ ERROR("incomplete vFlashErase packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+ addr = strtoul(parse, &parse, 16);
+ if (*(parse++) != ':')
+ {
+ ERROR("incomplete vFlashErase packet received, dropping connection");
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+ length = packet_size - (parse - packet);
+
+ /* create a new image if there isn't already one */
+ if (gdb_connection->vflash_image == NULL)
+ {
+ gdb_connection->vflash_image = malloc(sizeof(image_t));
+ image_open(gdb_connection->vflash_image, "", "build");
+ }
+
+ /* create new section with content from packet buffer */
+ image_add_section(gdb_connection->vflash_image, addr, length, 0x0, (u8*)parse);
+
+ gdb_put_packet(connection, "OK", 2);
+
+ return ERROR_OK;
+ }
+
+ if (!strcmp(packet, "vFlashDone"))
+ {
+ u32 written;
+
+ /* process the flashing buffer. No need to erase as GDB
+ * always issues a vFlashErase first. */
+ if ((result = flash_write(gdb_service->target, gdb_connection->vflash_image, &written, 0)) != ERROR_OK)
+ {
+ if (result == ERROR_FLASH_DST_OUT_OF_BANK)
+ gdb_put_packet(connection, "E.memtype", 9);
+ else
+ gdb_send_error(connection, EIO);
+ }
+ else
+ {
+ DEBUG("wrote %u bytes from vFlash image to flash", written);
+ gdb_put_packet(connection, "OK", 2);
+ }
+
+ image_close(gdb_connection->vflash_image);
+ free(gdb_connection->vflash_image);
+ gdb_connection->vflash_image = NULL;
+
+ return ERROR_OK;
+ }
+
+ gdb_put_packet(connection, "", 0);
+ return ERROR_OK;
+}
+
+int gdb_detach(connection_t *connection, target_t *target)
+{
+ switch( detach_mode )
+ {
+ case GDB_DETACH_RESUME:
+ target->type->resume(target, 1, 0, 1, 0);
+ break;
+
+ case GDB_DETACH_RESET:
+ target_process_reset(connection->cmd_ctx);
+ break;
+
+ case GDB_DETACH_HALT:
+ target->type->halt(target);
+ break;
+
+ case GDB_DETACH_NOTHING:
+ break;
+ }
+
+ gdb_put_packet(connection, "OK", 2);
+
+ return ERROR_OK;
+}
+
+static void gdb_log_callback(void *priv, const char *file, int line,
+ const char *function, const char *format, va_list args)
+{
+ connection_t *connection = priv;
+ gdb_connection_t *gdb_con = connection->priv;
+
+ if (gdb_con->busy)
+ {
+ /* do not reply this using the O packet */
+ return;
+ }
+
+ char *t = alloc_printf(format, args);
+ if (t == NULL)
+ return;
+
+ gdb_output_con(connection, t);
+
+ free(t);
+}
+
+int gdb_input_inner(connection_t *connection)
+{
+ gdb_service_t *gdb_service = connection->service->priv;
+ target_t *target = gdb_service->target;
+ char packet[GDB_BUFFER_SIZE];
+ int packet_size;
+ int retval;
+ gdb_connection_t *gdb_con = connection->priv;
+ static int extended_protocol = 0;
+
+ /* drain input buffer */
+ do
+ {
+ packet_size = GDB_BUFFER_SIZE-1;
+ if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ /* terminate with zero */
+ packet[packet_size] = 0;
+
+ DEBUG("received packet: '%s'", packet);
+
+ if (packet_size > 0)
+ {
+ retval = ERROR_OK;
+ switch (packet[0])
+ {
+ case 'H':
+ /* Hct... -- set thread
+ * we don't have threads, send empty reply */
+ gdb_put_packet(connection, NULL, 0);
+ break;
+ case 'q':
+ retval = gdb_query_packet(connection, target, packet, packet_size);
+ break;
+ case 'g':
+ retval = gdb_get_registers_packet(connection, target, packet, packet_size);
+ break;
+ case 'G':
+ retval = gdb_set_registers_packet(connection, target, packet, packet_size);
+ break;
+ case 'p':
+ retval = gdb_get_register_packet(connection, target, packet, packet_size);
+ break;
+ case 'P':
+ retval = gdb_set_register_packet(connection, target, packet, packet_size);
+ break;
+ case 'm':
+ retval = gdb_read_memory_packet(connection, target, packet, packet_size);
+ break;
+ case 'M':
+ retval = gdb_write_memory_packet(connection, target, packet, packet_size);
+ break;
+ case 'z':
+ case 'Z':
+ retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
+ break;
+ case '?':
+ gdb_last_signal_packet(connection, target, packet, packet_size);
+ break;
+ case 'c':
+ case 's':
+ {
+ /* We're running/stepping, in which case we can
+ * forward log output until the target is halted */
+ gdb_connection_t *gdb_con = connection->priv;
+ gdb_con->frontend_state = TARGET_RUNNING;
+ log_add_callback(gdb_log_callback, connection);
+ gdb_step_continue_packet(connection, target, packet, packet_size);
+ }
+ break;
+ case 'v':
+ retval = gdb_v_packet(connection, target, packet, packet_size);
+ break;
+ case 'D':
+ retval = gdb_detach(connection, target);
+ extended_protocol = 0;
+ break;
+ case 'X':
+ if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK)
+ return retval;
+ break;
+ case 'k':
+ if (extended_protocol != 0)
+ break;
+ gdb_put_packet(connection, "OK", 2);
+ return ERROR_SERVER_REMOTE_CLOSED;
+ case '!':
+ /* handle extended remote protocol */
+ extended_protocol = 1;
+ gdb_put_packet(connection, "OK", 2);
+ break;
+ case 'R':
+ /* handle extended restart packet */
+ target_process_reset(connection->cmd_ctx);
+ break;
+ default:
+ /* ignore unkown packets */
+ DEBUG("ignoring 0x%2.2x packet", packet[0]);
+ gdb_put_packet(connection, NULL, 0);
+ break;
+ }
+
+ /* if a packet handler returned an error, exit input loop */
+ if (retval != ERROR_OK)
+ return retval;
+ }
+
+ if (gdb_con->ctrl_c)
+ {
+ if (target->state == TARGET_RUNNING)
+ {
+ target->type->halt(target);
+ gdb_con->ctrl_c = 0;
+ }
+ }
+
+ } while (gdb_con->buf_cnt > 0);
+
+ return ERROR_OK;
+}
+
+int gdb_input(connection_t *connection)
+{
+ int retval = gdb_input_inner(connection);
+ if (retval == ERROR_SERVER_REMOTE_CLOSED)
+ return retval;
+ /* we'll recover from any other errors(e.g. temporary timeouts, etc.) */
+ return ERROR_OK;
+}
+
+int gdb_init()
+{
+ gdb_service_t *gdb_service;
+ target_t *target = targets;
+ int i = 0;
+
+ if (!target)
+ {
+ WARNING("no gdb ports allocated as no target has been specified");
+ return ERROR_OK;
+ }
+
+ if (gdb_port == 0)
+ {
+ WARNING("no gdb port specified, using default port 3333");
+ gdb_port = 3333;
+ }
+
+ while (target)
+ {
+ char service_name[8];
+
+ snprintf(service_name, 8, "gdb-%2.2i", i);
+
+ gdb_service = malloc(sizeof(gdb_service_t));
+ gdb_service->target = target;
+
+ add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);
+
+ DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);
+
+ i++;
+ target = target->next;
+ }
+
+ return ERROR_OK;
+}
+
+/* daemon configuration command gdb_port */
+int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ if (argc == 0)
+ return ERROR_OK;
+
+ /* only if the port wasn't overwritten by cmdline */
+ if (gdb_port == 0)
+ gdb_port = strtoul(args[0], NULL, 0);
+
+ return ERROR_OK;
+}
+
+int handle_gdb_detach_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ if (argc == 1)
+ {
+ if (strcmp(args[0], "resume") == 0)
+ {
+ detach_mode = GDB_DETACH_RESUME;
+ return ERROR_OK;
+ }
+ else if (strcmp(args[0], "reset") == 0)
+ {
+ detach_mode = GDB_DETACH_RESET;
+ return ERROR_OK;
+ }
+ else if (strcmp(args[0], "halt") == 0)
+ {
+ detach_mode = GDB_DETACH_HALT;
+ return ERROR_OK;
+ }
+ else if (strcmp(args[0], "nothing") == 0)
+ {
+ detach_mode = GDB_DETACH_NOTHING;
+ return ERROR_OK;
+ }
+ }
+
+ WARNING("invalid gdb_detach configuration directive: %s", args[0]);
+ return ERROR_OK;
+}
+
+int handle_gdb_memory_map_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ if (argc == 1)
+ {
+ if (strcmp(args[0], "enable") == 0)
+ {
+ gdb_use_memory_map = 1;
+ return ERROR_OK;
+ }
+ else if (strcmp(args[0], "disable") == 0)
+ {
+ gdb_use_memory_map = 0;
+ return ERROR_OK;
+ }
+ }
+
+ WARNING("invalid gdb_memory_map configuration directive: %s", args[0]);
+ return ERROR_OK;
+}
+
+int handle_gdb_flash_program_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ if (argc == 1)
+ {
+ if (strcmp(args[0], "enable") == 0)
+ {
+ gdb_flash_program = 1;
+ return ERROR_OK;
+ }
+ else if (strcmp(args[0], "disable") == 0)
+ {
+ gdb_flash_program = 0;
+ return ERROR_OK;
+ }
+ }
+
+ WARNING("invalid gdb_memory_map configuration directive: %s", args[0]);
+ return ERROR_OK;
+}
+
+int handle_gdb_report_data_abort_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ if (argc == 1)
+ {
+ if (strcmp(args[0], "enable") == 0)
+ {
+ gdb_report_data_abort = 1;
+ return ERROR_OK;
+ }
+ else if (strcmp(args[0], "disable") == 0)
+ {
+ gdb_report_data_abort = 0;
+ return ERROR_OK;
+ }
+ }
+
+ WARNING("invalid gdb_report_data_abort configuration directive: %s", args[0]);
+ return ERROR_OK;
+}
+
+int gdb_register_commands(command_context_t *command_context)
+{
+ register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
+ COMMAND_CONFIG, "");
+ register_command(command_context, NULL, "gdb_detach", handle_gdb_detach_command,
+ COMMAND_CONFIG, "");
+ register_command(command_context, NULL, "gdb_memory_map", handle_gdb_memory_map_command,
+ COMMAND_CONFIG, "");
+ register_command(command_context, NULL, "gdb_flash_program", handle_gdb_flash_program_command,
+ COMMAND_CONFIG, "");
+ register_command(command_context, NULL, "gdb_report_data_abort", handle_gdb_report_data_abort_command,
+ COMMAND_CONFIG, "");
+ return ERROR_OK;
+}
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c index 899902a7..830cf641 100644 --- a/src/server/telnet_server.c +++ b/src/server/telnet_server.c @@ -1,632 +1,631 @@ -/*************************************************************************** - * Copyright (C) 2005 by Dominic Rath * - * Dominic.Rath@gmx.de * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ -#ifdef HAVE_CONFIG_H -#include "config.h" -#endif - -#include "replacements.h" - -#include "telnet_server.h" - -#include "server.h" -#include "log.h" -#include "command.h" -#include "target.h" -#include "target_request.h" - -#include <stdlib.h> -#include <unistd.h> -#include <errno.h> -#include <string.h> -#include <ctype.h> - -static unsigned short telnet_port = 0; - -int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc); -int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc); - -static char *negotiate = - "\xFF\xFB\x03" /* IAC WILL Suppress Go Ahead */ - "\xFF\xFB\x01" /* IAC WILL Echo */ - "\xFF\xFD\x03" /* IAC DO Suppress Go Ahead */ - "\xFF\xFE\x01"; /* IAC DON'T Echo */ - -#define CTRL(c) (c - '@') - -/* The only way we can detect that the socket is closed is the first time - * we write to it, we will fail. Subsequent write operations will - * succeed. Shudder! - */ -int telnet_write(connection_t *connection, void *data, int len) -{ - telnet_connection_t *t_con = connection->priv; - if (t_con->closed) - return ERROR_SERVER_REMOTE_CLOSED; - - if (write_socket(connection->fd, data, len) == len) - { - return ERROR_OK; - } - t_con->closed = 1; - return ERROR_SERVER_REMOTE_CLOSED; -} - -int telnet_prompt(connection_t *connection) -{ - telnet_connection_t *t_con = connection->priv; - - return telnet_write(connection, t_con->prompt, strlen(t_con->prompt)); -} - -int telnet_outputline(connection_t *connection, char* line) -{ - telnet_write(connection, line, strlen(line)); - return telnet_write(connection, "\r\n\0", 3); -} - -int telnet_output(struct command_context_s *cmd_ctx, char* line) -{ - connection_t *connection = cmd_ctx->output_handler_priv; - - return telnet_outputline(connection, line); -} - -void telnet_log_callback(void *priv, const char *file, int line, - const char *function, const char *format, va_list args) -{ - connection_t *connection = priv; - char *t = allocPrintf(format, args); - char *t2; - if (t == NULL) - return; - t2=t; - char *endline; - do - { - if ((endline=strchr(t2, '\n'))!=NULL) - { - *endline=0; - } - telnet_outputline(connection, t2); - t2=endline+1; - } while (endline); - - - free(t); -} - -int telnet_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv) -{ - struct command_context_s *cmd_ctx = priv; - connection_t *connection = cmd_ctx->output_handler_priv; - telnet_connection_t *t_con = connection->priv; - - switch (event) - { - case TARGET_EVENT_HALTED: - target_arch_state(target); - if (!t_con->suppress_prompt) - telnet_prompt(connection); - break; - case TARGET_EVENT_RESUMED: - if (!t_con->suppress_prompt) - telnet_prompt(connection); - break; - default: - break; - } - - return ERROR_OK; -} - -int telnet_new_connection(connection_t *connection) -{ - telnet_connection_t *telnet_connection = malloc(sizeof(telnet_connection_t)); - telnet_service_t *telnet_service = connection->service->priv; - int i; - - connection->priv = telnet_connection; - - /* initialize telnet connection information */ - telnet_connection->closed = 0; - telnet_connection->line_size = 0; - telnet_connection->line_cursor = 0; - telnet_connection->option_size = 0; - telnet_connection->prompt = strdup("> "); - telnet_connection->suppress_prompt = 0; - telnet_connection->state = TELNET_STATE_DATA; - - /* output goes through telnet connection */ - command_set_output_handler(connection->cmd_ctx, telnet_output, connection); - - /* negotiate telnet options */ - telnet_write(connection, negotiate, strlen(negotiate)); - - /* print connection banner */ - if (telnet_service->banner) - { - telnet_write(connection, telnet_service->banner, strlen(telnet_service->banner)); - telnet_write(connection, "\r\n\0", 3); - } - - telnet_prompt(connection); - - /* initialize history */ - for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++) - { - telnet_connection->history[i] = NULL; - } - telnet_connection->next_history = 0; - telnet_connection->current_history = 0; - - target_register_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx); - - return ERROR_OK; -} - -void telnet_clear_line(connection_t *connection, telnet_connection_t *t_con) -{ - /* move to end of line */ - if (t_con->line_cursor < t_con->line_size) - { - telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor); - } - - /* backspace, overwrite with space, backspace */ - while (t_con->line_size > 0) - { - telnet_write(connection, "\b \b", 3); - t_con->line_size--; - } - t_con->line_cursor = 0; -} - -int telnet_input(connection_t *connection) -{ - int bytes_read; - char buffer[TELNET_BUFFER_SIZE]; - char *buf_p; - telnet_connection_t *t_con = connection->priv; - command_context_t *command_context = connection->cmd_ctx; - - bytes_read = read_socket(connection->fd, buffer, TELNET_BUFFER_SIZE); - - if (bytes_read == 0) - return ERROR_SERVER_REMOTE_CLOSED; - else if (bytes_read == -1) - { - ERROR("error during read: %s", strerror(errno)); - return ERROR_SERVER_REMOTE_CLOSED; - } - - buf_p = buffer; - while (bytes_read) - { - switch (t_con->state) - { - case TELNET_STATE_DATA: - if (*buf_p == '\xff') - { - t_con->state = TELNET_STATE_IAC; - } - else - { - if (isprint(*buf_p)) /* printable character */ - { - telnet_write(connection, buf_p, 1); - if (t_con->line_cursor == t_con->line_size) - { - t_con->line[t_con->line_size++] = *buf_p; - t_con->line_cursor++; - } - else - { - int i; - memmove(t_con->line + t_con->line_cursor + 1, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor); - t_con->line[t_con->line_cursor++] = *buf_p; - t_con->line_size++; - telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor); - for (i = t_con->line_cursor; i < t_con->line_size; i++) - { - telnet_write(connection, "\b", 1); - } - } - } - else /* non-printable */ - { - if (*buf_p == 0x1b) /* escape */ - { - t_con->state = TELNET_STATE_ESCAPE; - t_con->last_escape = '\x00'; - } - else if ((*buf_p == 0xd) || (*buf_p == 0xa)) /* CR/LF */ - { - int retval; - - /* skip over combinations with CR/LF + NUL */ - if (((*(buf_p + 1) == 0xa) || (*(buf_p + 1) == 0xd)) && (bytes_read > 1)) - { - buf_p++; - bytes_read--; - } - if ((*(buf_p + 1) == 0) && (bytes_read > 1)) - { - buf_p++; - bytes_read--; - } - t_con->line[t_con->line_size] = 0; - - telnet_write(connection, "\r\n\x00", 3); - - if (strcmp(t_con->line, "history") == 0) - { - int i; - for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++) - { - if (t_con->history[i]) - { - telnet_write(connection, t_con->history[i], strlen(t_con->history[i])); - telnet_write(connection, "\r\n\x00", 3); - } - } - telnet_prompt(connection); - t_con->line_size = 0; - t_con->line_cursor = 0; - continue; - } - - log_setCallback(telnet_log_callback, connection); - t_con->suppress_prompt = 1; - - if ((retval = command_run_line(command_context, t_con->line)) != ERROR_OK) - { - if (retval == ERROR_COMMAND_CLOSE_CONNECTION) - { - return ERROR_SERVER_REMOTE_CLOSED; - } - } - - t_con->suppress_prompt = 0; - - /* Save only non-blank lines in the history */ - if (t_con->line_size > 0) - { - /* if the history slot is already taken, free it */ - if (t_con->history[t_con->next_history]) - { - free(t_con->history[t_con->next_history]); - } - - /* add line to history */ - t_con->history[t_con->next_history] = strdup(t_con->line); - - /* wrap history at TELNET_LINE_HISTORY_SIZE */ - t_con->next_history = (t_con->next_history + 1) % TELNET_LINE_HISTORY_SIZE; - - /* current history line starts at the new entry */ - t_con->current_history = t_con->next_history; - - if (t_con->history[t_con->current_history]) - { - free(t_con->history[t_con->current_history]); - } - t_con->history[t_con->current_history] = strdup(""); - } - - int t = telnet_prompt(connection); - if (t == ERROR_SERVER_REMOTE_CLOSED) - return t; - - t_con->line_size = 0; - t_con->line_cursor = 0; - } - else if ((*buf_p == 0x7f) || (*buf_p == 0x8)) /* delete character */ - { - if (t_con->line_cursor > 0) - { - if (t_con->line_cursor != t_con->line_size) - { - int i; - telnet_write(connection, "\b", 1); - t_con->line_cursor--; - t_con->line_size--; - memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor); - - telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor); - telnet_write(connection, " \b", 2); - for (i = t_con->line_cursor; i < t_con->line_size; i++) - { - telnet_write(connection, "\b", 1); - } - } - else - { - t_con->line_size--; - t_con->line_cursor--; - /* back space: move the 'printer' head one char back, overwrite with space, move back again */ - telnet_write(connection, "\b \b", 3); - } - } - } - else if (*buf_p == 0x15) /* clear line */ - { - telnet_clear_line(connection, t_con); - } - else if (*buf_p == CTRL('B')) /* cursor left */ - { - if (t_con->line_cursor > 0) - { - telnet_write(connection, "\b", 1); - t_con->line_cursor--; - } - t_con->state = TELNET_STATE_DATA; - } - else if (*buf_p == CTRL('F')) /* cursor right */ - { - if (t_con->line_cursor < t_con->line_size) - { - telnet_write(connection, t_con->line + t_con->line_cursor++, 1); - } - t_con->state = TELNET_STATE_DATA; - } - else - { - DEBUG("unhandled nonprintable: %2.2x", *buf_p); - } - } - } - break; - case TELNET_STATE_IAC: - switch (*buf_p) - { - case '\xfe': - t_con->state = TELNET_STATE_DONT; - break; - case '\xfd': - t_con->state = TELNET_STATE_DO; - break; - case '\xfc': - t_con->state = TELNET_STATE_WONT; - break; - case '\xfb': - t_con->state = TELNET_STATE_WILL; - break; - } - break; - case TELNET_STATE_SB: - break; - case TELNET_STATE_SE: - break; - case TELNET_STATE_WILL: - case TELNET_STATE_WONT: - case TELNET_STATE_DO: - case TELNET_STATE_DONT: - t_con->state = TELNET_STATE_DATA; - break; - case TELNET_STATE_ESCAPE: - if (t_con->last_escape == '[') - { - if (*buf_p == 'D') /* cursor left */ - { - if (t_con->line_cursor > 0) - { - telnet_write(connection, "\b", 1); - t_con->line_cursor--; - } - t_con->state = TELNET_STATE_DATA; - } - else if (*buf_p == 'C') /* cursor right */ - { - if (t_con->line_cursor < t_con->line_size) - { - telnet_write(connection, t_con->line + t_con->line_cursor++, 1); - } - t_con->state = TELNET_STATE_DATA; - } - else if (*buf_p == 'A') /* cursor up */ - { - int last_history = (t_con->current_history > 0) ? t_con->current_history - 1 : TELNET_LINE_HISTORY_SIZE-1; - if (t_con->history[last_history]) - { - telnet_clear_line(connection, t_con); - t_con->line_size = strlen(t_con->history[last_history]); - t_con->line_cursor = t_con->line_size; - memcpy(t_con->line, t_con->history[last_history], t_con->line_size + 1); - telnet_write(connection, t_con->line, t_con->line_size); - t_con->current_history = last_history; - } - t_con->state = TELNET_STATE_DATA; - } - else if (*buf_p == 'B') /* cursor down */ - { - int next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE; - if (t_con->history[next_history]) - { - telnet_clear_line(connection, t_con); - t_con->line_size = strlen(t_con->history[next_history]); - t_con->line_cursor = t_con->line_size; - memcpy(t_con->line, t_con->history[next_history], t_con->line_size + 1); - telnet_write(connection, t_con->line, t_con->line_size); - t_con->current_history = next_history; - } - t_con->state = TELNET_STATE_DATA; - } - else if (*buf_p == '3') - { - t_con->last_escape = *buf_p; - } - else - { - t_con->state = TELNET_STATE_DATA; - } - } - else if (t_con->last_escape == '3') - { - /* Remove character */ - if (*buf_p == '~') - { - if (t_con->line_cursor < t_con->line_size) - { - int i; - t_con->line_size--; - /* remove char from line buffer */ - memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor); - - /* print remainder of buffer */ - telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor); - /* overwrite last char with whitespace */ - telnet_write(connection, " \b", 2); - - /* move back to cursor position*/ - for (i = t_con->line_cursor; i < t_con->line_size; i++) - { - telnet_write(connection, "\b", 1); - } - } - - t_con->state = TELNET_STATE_DATA; - } - else - { - t_con->state = TELNET_STATE_DATA; - } - } - else if (t_con->last_escape == '\x00') - { - if (*buf_p == '[') - { - t_con->last_escape = *buf_p; - } - else - { - t_con->state = TELNET_STATE_DATA; - } - } - else - { - ERROR("BUG: unexpected value in t_con->last_escape"); - t_con->state = TELNET_STATE_DATA; - } - - break; - default: - ERROR("unknown telnet state"); - exit(-1); - } - - bytes_read--; - buf_p++; - } - - return ERROR_OK; -} - -int telnet_connection_closed(connection_t *connection) -{ - telnet_connection_t *t_con = connection->priv; - int i; - - if (t_con->prompt) - { - free(t_con->prompt); - t_con->prompt = NULL; - } - - for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++) - { - if (t_con->history[i]) - { - free(t_con->history[i]); - t_con->history[i] = NULL; - } - } - - /* if this connection registered a debug-message receiver delete it */ - delete_debug_msg_receiver(connection->cmd_ctx, NULL); - - if (connection->priv) - { - free(connection->priv); - connection->priv = NULL; - } - else - { - ERROR("BUG: connection->priv == NULL"); - } - - target_unregister_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx); - - return ERROR_OK; -} - -int telnet_set_prompt(connection_t *connection, char *prompt) -{ - telnet_connection_t *t_con = connection->priv; - - t_con->prompt = strdup(prompt); - - return ERROR_OK; -} - -int telnet_init(char *banner) -{ - telnet_service_t *telnet_service = malloc(sizeof(telnet_service_t)); - - if (telnet_port == 0) - { - WARNING("no telnet port specified, using default port 4444"); - telnet_port = 4444; - } - - telnet_service->banner = banner; - - add_service("telnet", CONNECTION_TELNET, telnet_port, 1, telnet_new_connection, telnet_input, telnet_connection_closed, telnet_service); - - return ERROR_OK; -} - -int telnet_register_commands(command_context_t *command_context) -{ - register_command(command_context, NULL, "exit", handle_exit_command, - COMMAND_EXEC, "exit telnet session"); - - register_command(command_context, NULL, "telnet_port", handle_telnet_port_command, - COMMAND_CONFIG, ""); - - return ERROR_OK; -} - -/* daemon configuration command telnet_port */ -int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - if (argc == 0) - return ERROR_OK; - - /* only if the port wasn't overwritten by cmdline */ - if (telnet_port == 0) - telnet_port = strtoul(args[0], NULL, 0); - - return ERROR_OK; -} - -int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) -{ - return ERROR_COMMAND_CLOSE_CONNECTION; -} +/***************************************************************************
+ * Copyright (C) 2005 by Dominic Rath *
+ * Dominic.Rath@gmx.de *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
+#include "telnet_server.h"
+
+#include "server.h"
+#include "log.h"
+#include "command.h"
+#include "target.h"
+#include "target_request.h"
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <ctype.h>
+
+static unsigned short telnet_port = 0;
+
+int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+static char *negotiate =
+ "\xFF\xFB\x03" /* IAC WILL Suppress Go Ahead */
+ "\xFF\xFB\x01" /* IAC WILL Echo */
+ "\xFF\xFD\x03" /* IAC DO Suppress Go Ahead */
+ "\xFF\xFE\x01"; /* IAC DON'T Echo */
+
+#define CTRL(c) (c - '@')
+
+/* The only way we can detect that the socket is closed is the first time
+ * we write to it, we will fail. Subsequent write operations will
+ * succeed. Shudder!
+ */
+int telnet_write(connection_t *connection, void *data, int len)
+{
+ telnet_connection_t *t_con = connection->priv;
+ if (t_con->closed)
+ return ERROR_SERVER_REMOTE_CLOSED;
+
+ if (write_socket(connection->fd, data, len) == len)
+ {
+ return ERROR_OK;
+ }
+ t_con->closed = 1;
+ return ERROR_SERVER_REMOTE_CLOSED;
+}
+
+int telnet_prompt(connection_t *connection)
+{
+ telnet_connection_t *t_con = connection->priv;
+
+ return telnet_write(connection, t_con->prompt, strlen(t_con->prompt));
+}
+
+int telnet_outputline(connection_t *connection, char* line)
+{
+ telnet_write(connection, line, strlen(line));
+ return telnet_write(connection, "\r\n\0", 3);
+}
+
+int telnet_output(struct command_context_s *cmd_ctx, char* line)
+{
+ connection_t *connection = cmd_ctx->output_handler_priv;
+
+ return telnet_outputline(connection, line);
+}
+
+void telnet_log_callback(void *priv, const char *file, int line,
+ const char *function, const char *format, va_list args)
+{
+ connection_t *connection = priv;
+ char *t = alloc_printf(format, args);
+ char *t2;
+ if (t == NULL)
+ return;
+ t2=t;
+ char *endline;
+ do
+ {
+ if ((endline=strchr(t2, '\n'))!=NULL)
+ {
+ *endline=0;
+ }
+ telnet_outputline(connection, t2);
+ t2=endline+1;
+ } while (endline);
+
+ free(t);
+}
+
+int telnet_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
+{
+ struct command_context_s *cmd_ctx = priv;
+ connection_t *connection = cmd_ctx->output_handler_priv;
+ telnet_connection_t *t_con = connection->priv;
+
+ switch (event)
+ {
+ case TARGET_EVENT_HALTED:
+ target_arch_state(target);
+ if (!t_con->suppress_prompt)
+ telnet_prompt(connection);
+ break;
+ case TARGET_EVENT_RESUMED:
+ if (!t_con->suppress_prompt)
+ telnet_prompt(connection);
+ break;
+ default:
+ break;
+ }
+
+ return ERROR_OK;
+}
+
+int telnet_new_connection(connection_t *connection)
+{
+ telnet_connection_t *telnet_connection = malloc(sizeof(telnet_connection_t));
+ telnet_service_t *telnet_service = connection->service->priv;
+ int i;
+
+ connection->priv = telnet_connection;
+
+ /* initialize telnet connection information */
+ telnet_connection->closed = 0;
+ telnet_connection->line_size = 0;
+ telnet_connection->line_cursor = 0;
+ telnet_connection->option_size = 0;
+ telnet_connection->prompt = strdup("> ");
+ telnet_connection->suppress_prompt = 0;
+ telnet_connection->state = TELNET_STATE_DATA;
+
+ /* output goes through telnet connection */
+ command_set_output_handler(connection->cmd_ctx, telnet_output, connection);
+
+ /* negotiate telnet options */
+ telnet_write(connection, negotiate, strlen(negotiate));
+
+ /* print connection banner */
+ if (telnet_service->banner)
+ {
+ telnet_write(connection, telnet_service->banner, strlen(telnet_service->banner));
+ telnet_write(connection, "\r\n\0", 3);
+ }
+
+ telnet_prompt(connection);
+
+ /* initialize history */
+ for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
+ {
+ telnet_connection->history[i] = NULL;
+ }
+ telnet_connection->next_history = 0;
+ telnet_connection->current_history = 0;
+
+ target_register_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx);
+
+ return ERROR_OK;
+}
+
+void telnet_clear_line(connection_t *connection, telnet_connection_t *t_con)
+{
+ /* move to end of line */
+ if (t_con->line_cursor < t_con->line_size)
+ {
+ telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+ }
+
+ /* backspace, overwrite with space, backspace */
+ while (t_con->line_size > 0)
+ {
+ telnet_write(connection, "\b \b", 3);
+ t_con->line_size--;
+ }
+ t_con->line_cursor = 0;
+}
+
+int telnet_input(connection_t *connection)
+{
+ int bytes_read;
+ char buffer[TELNET_BUFFER_SIZE];
+ char *buf_p;
+ telnet_connection_t *t_con = connection->priv;
+ command_context_t *command_context = connection->cmd_ctx;
+
+ bytes_read = read_socket(connection->fd, buffer, TELNET_BUFFER_SIZE);
+
+ if (bytes_read == 0)
+ return ERROR_SERVER_REMOTE_CLOSED;
+ else if (bytes_read == -1)
+ {
+ ERROR("error during read: %s", strerror(errno));
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ buf_p = buffer;
+ while (bytes_read)
+ {
+ switch (t_con->state)
+ {
+ case TELNET_STATE_DATA:
+ if (*buf_p == '\xff')
+ {
+ t_con->state = TELNET_STATE_IAC;
+ }
+ else
+ {
+ if (isprint(*buf_p)) /* printable character */
+ {
+ telnet_write(connection, buf_p, 1);
+ if (t_con->line_cursor == t_con->line_size)
+ {
+ t_con->line[t_con->line_size++] = *buf_p;
+ t_con->line_cursor++;
+ }
+ else
+ {
+ int i;
+ memmove(t_con->line + t_con->line_cursor + 1, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+ t_con->line[t_con->line_cursor++] = *buf_p;
+ t_con->line_size++;
+ telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+ for (i = t_con->line_cursor; i < t_con->line_size; i++)
+ {
+ telnet_write(connection, "\b", 1);
+ }
+ }
+ }
+ else /* non-printable */
+ {
+ if (*buf_p == 0x1b) /* escape */
+ {
+ t_con->state = TELNET_STATE_ESCAPE;
+ t_con->last_escape = '\x00';
+ }
+ else if ((*buf_p == 0xd) || (*buf_p == 0xa)) /* CR/LF */
+ {
+ int retval;
+
+ /* skip over combinations with CR/LF + NUL */
+ if (((*(buf_p + 1) == 0xa) || (*(buf_p + 1) == 0xd)) && (bytes_read > 1))
+ {
+ buf_p++;
+ bytes_read--;
+ }
+ if ((*(buf_p + 1) == 0) && (bytes_read > 1))
+ {
+ buf_p++;
+ bytes_read--;
+ }
+ t_con->line[t_con->line_size] = 0;
+
+ telnet_write(connection, "\r\n\x00", 3);
+
+ if (strcmp(t_con->line, "history") == 0)
+ {
+ int i;
+ for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
+ {
+ if (t_con->history[i])
+ {
+ telnet_write(connection, t_con->history[i], strlen(t_con->history[i]));
+ telnet_write(connection, "\r\n\x00", 3);
+ }
+ }
+ telnet_prompt(connection);
+ t_con->line_size = 0;
+ t_con->line_cursor = 0;
+ continue;
+ }
+
+ log_add_callback(telnet_log_callback, connection);
+ t_con->suppress_prompt = 1;
+
+ retval = command_run_line(command_context, t_con->line);
+
+ log_remove_callback(telnet_log_callback, connection);
+ t_con->suppress_prompt = 0;
+
+ if (retval == ERROR_COMMAND_CLOSE_CONNECTION)
+ {
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+
+ /* Save only non-blank lines in the history */
+ if (t_con->line_size > 0)
+ {
+ /* if the history slot is already taken, free it */
+ if (t_con->history[t_con->next_history])
+ {
+ free(t_con->history[t_con->next_history]);
+ }
+
+ /* add line to history */
+ t_con->history[t_con->next_history] = strdup(t_con->line);
+
+ /* wrap history at TELNET_LINE_HISTORY_SIZE */
+ t_con->next_history = (t_con->next_history + 1) % TELNET_LINE_HISTORY_SIZE;
+
+ /* current history line starts at the new entry */
+ t_con->current_history = t_con->next_history;
+
+ if (t_con->history[t_con->current_history])
+ {
+ free(t_con->history[t_con->current_history]);
+ }
+ t_con->history[t_con->current_history] = strdup("");
+ }
+
+ int t = telnet_prompt(connection);
+ if (t == ERROR_SERVER_REMOTE_CLOSED)
+ return t;
+
+ t_con->line_size = 0;
+ t_con->line_cursor = 0;
+ }
+ else if ((*buf_p == 0x7f) || (*buf_p == 0x8)) /* delete character */
+ {
+ if (t_con->line_cursor > 0)
+ {
+ if (t_con->line_cursor != t_con->line_size)
+ {
+ int i;
+ telnet_write(connection, "\b", 1);
+ t_con->line_cursor--;
+ t_con->line_size--;
+ memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor);
+
+ telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+ telnet_write(connection, " \b", 2);
+ for (i = t_con->line_cursor; i < t_con->line_size; i++)
+ {
+ telnet_write(connection, "\b", 1);
+ }
+ }
+ else
+ {
+ t_con->line_size--;
+ t_con->line_cursor--;
+ /* back space: move the 'printer' head one char back, overwrite with space, move back again */
+ telnet_write(connection, "\b \b", 3);
+ }
+ }
+ }
+ else if (*buf_p == 0x15) /* clear line */
+ {
+ telnet_clear_line(connection, t_con);
+ }
+ else if (*buf_p == CTRL('B')) /* cursor left */
+ {
+ if (t_con->line_cursor > 0)
+ {
+ telnet_write(connection, "\b", 1);
+ t_con->line_cursor--;
+ }
+ t_con->state = TELNET_STATE_DATA;
+ }
+ else if (*buf_p == CTRL('F')) /* cursor right */
+ {
+ if (t_con->line_cursor < t_con->line_size)
+ {
+ telnet_write(connection, t_con->line + t_con->line_cursor++, 1);
+ }
+ t_con->state = TELNET_STATE_DATA;
+ }
+ else
+ {
+ DEBUG("unhandled nonprintable: %2.2x", *buf_p);
+ }
+ }
+ }
+ break;
+ case TELNET_STATE_IAC:
+ switch (*buf_p)
+ {
+ case '\xfe':
+ t_con->state = TELNET_STATE_DONT;
+ break;
+ case '\xfd':
+ t_con->state = TELNET_STATE_DO;
+ break;
+ case '\xfc':
+ t_con->state = TELNET_STATE_WONT;
+ break;
+ case '\xfb':
+ t_con->state = TELNET_STATE_WILL;
+ break;
+ }
+ break;
+ case TELNET_STATE_SB:
+ break;
+ case TELNET_STATE_SE:
+ break;
+ case TELNET_STATE_WILL:
+ case TELNET_STATE_WONT:
+ case TELNET_STATE_DO:
+ case TELNET_STATE_DONT:
+ t_con->state = TELNET_STATE_DATA;
+ break;
+ case TELNET_STATE_ESCAPE:
+ if (t_con->last_escape == '[')
+ {
+ if (*buf_p == 'D') /* cursor left */
+ {
+ if (t_con->line_cursor > 0)
+ {
+ telnet_write(connection, "\b", 1);
+ t_con->line_cursor--;
+ }
+ t_con->state = TELNET_STATE_DATA;
+ }
+ else if (*buf_p == 'C') /* cursor right */
+ {
+ if (t_con->line_cursor < t_con->line_size)
+ {
+ telnet_write(connection, t_con->line + t_con->line_cursor++, 1);
+ }
+ t_con->state = TELNET_STATE_DATA;
+ }
+ else if (*buf_p == 'A') /* cursor up */
+ {
+ int last_history = (t_con->current_history > 0) ? t_con->current_history - 1 : TELNET_LINE_HISTORY_SIZE-1;
+ if (t_con->history[last_history])
+ {
+ telnet_clear_line(connection, t_con);
+ t_con->line_size = strlen(t_con->history[last_history]);
+ t_con->line_cursor = t_con->line_size;
+ memcpy(t_con->line, t_con->history[last_history], t_con->line_size + 1);
+ telnet_write(connection, t_con->line, t_con->line_size);
+ t_con->current_history = last_history;
+ }
+ t_con->state = TELNET_STATE_DATA;
+ }
+ else if (*buf_p == 'B') /* cursor down */
+ {
+ int next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE;
+ if (t_con->history[next_history])
+ {
+ telnet_clear_line(connection, t_con);
+ t_con->line_size = strlen(t_con->history[next_history]);
+ t_con->line_cursor = t_con->line_size;
+ memcpy(t_con->line, t_con->history[next_history], t_con->line_size + 1);
+ telnet_write(connection, t_con->line, t_con->line_size);
+ t_con->current_history = next_history;
+ }
+ t_con->state = TELNET_STATE_DATA;
+ }
+ else if (*buf_p == '3')
+ {
+ t_con->last_escape = *buf_p;
+ }
+ else
+ {
+ t_con->state = TELNET_STATE_DATA;
+ }
+ }
+ else if (t_con->last_escape == '3')
+ {
+ /* Remove character */
+ if (*buf_p == '~')
+ {
+ if (t_con->line_cursor < t_con->line_size)
+ {
+ int i;
+ t_con->line_size--;
+ /* remove char from line buffer */
+ memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor);
+
+ /* print remainder of buffer */
+ telnet_write(connection, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+ /* overwrite last char with whitespace */
+ telnet_write(connection, " \b", 2);
+
+ /* move back to cursor position*/
+ for (i = t_con->line_cursor; i < t_con->line_size; i++)
+ {
+ telnet_write(connection, "\b", 1);
+ }
+ }
+
+ t_con->state = TELNET_STATE_DATA;
+ }
+ else
+ {
+ t_con->state = TELNET_STATE_DATA;
+ }
+ }
+ else if (t_con->last_escape == '\x00')
+ {
+ if (*buf_p == '[')
+ {
+ t_con->last_escape = *buf_p;
+ }
+ else
+ {
+ t_con->state = TELNET_STATE_DATA;
+ }
+ }
+ else
+ {
+ ERROR("BUG: unexpected value in t_con->last_escape");
+ t_con->state = TELNET_STATE_DATA;
+ }
+
+ break;
+ default:
+ ERROR("unknown telnet state");
+ exit(-1);
+ }
+
+ bytes_read--;
+ buf_p++;
+ }
+
+ return ERROR_OK;
+}
+
+int telnet_connection_closed(connection_t *connection)
+{
+ telnet_connection_t *t_con = connection->priv;
+ int i;
+
+ if (t_con->prompt)
+ {
+ free(t_con->prompt);
+ t_con->prompt = NULL;
+ }
+
+ for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
+ {
+ if (t_con->history[i])
+ {
+ free(t_con->history[i]);
+ t_con->history[i] = NULL;
+ }
+ }
+
+ /* if this connection registered a debug-message receiver delete it */
+ delete_debug_msg_receiver(connection->cmd_ctx, NULL);
+
+ if (connection->priv)
+ {
+ free(connection->priv);
+ connection->priv = NULL;
+ }
+ else
+ {
+ ERROR("BUG: connection->priv == NULL");
+ }
+
+ target_unregister_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx);
+
+ return ERROR_OK;
+}
+
+int telnet_set_prompt(connection_t *connection, char *prompt)
+{
+ telnet_connection_t *t_con = connection->priv;
+
+ t_con->prompt = strdup(prompt);
+
+ return ERROR_OK;
+}
+
+int telnet_init(char *banner)
+{
+ telnet_service_t *telnet_service = malloc(sizeof(telnet_service_t));
+
+ if (telnet_port == 0)
+ {
+ WARNING("no telnet port specified, using default port 4444");
+ telnet_port = 4444;
+ }
+
+ telnet_service->banner = banner;
+
+ add_service("telnet", CONNECTION_TELNET, telnet_port, 1, telnet_new_connection, telnet_input, telnet_connection_closed, telnet_service);
+
+ return ERROR_OK;
+}
+
+int telnet_register_commands(command_context_t *command_context)
+{
+ register_command(command_context, NULL, "exit", handle_exit_command,
+ COMMAND_EXEC, "exit telnet session");
+
+ register_command(command_context, NULL, "telnet_port", handle_telnet_port_command,
+ COMMAND_CONFIG, "");
+
+ return ERROR_OK;
+}
+
+/* daemon configuration command telnet_port */
+int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ if (argc == 0)
+ return ERROR_OK;
+
+ /* only if the port wasn't overwritten by cmdline */
+ if (telnet_port == 0)
+ telnet_port = strtoul(args[0], NULL, 0);
+
+ return ERROR_OK;
+}
+
+int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+ return ERROR_COMMAND_CLOSE_CONNECTION;
+}
|