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

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, &reg_list, &reg_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, &reg_list, &reg_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, &reg_list, &reg_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, &reg_list, &reg_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, &reg_list, &reg_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, &reg_list, &reg_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, &reg_list, &reg_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, &reg_list, &reg_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;

+}