- convert all files to unix line-ending

git-svn-id: svn://svn.berlios.de/openocd/trunk@347 b42882b7-edfa-0310-969c-e2dbd0fdcd60

3 files changed, 3188 insertions, 3188 deletions

diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index fa203e7..335a4cd 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_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;

-}

+/***************************************************************************
+ *   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/server.c b/src/server/server.c
index 0aa3383..6aa72b9 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -1,450 +1,450 @@
-/***************************************************************************

- *   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 "server.h"

-

-#include "log.h"

-#include "telnet_server.h"

-#include "target.h"

-

-#include <command.h>

-#include <string.h>

-#include <stdlib.h>

-#include <errno.h>

-#include <unistd.h>

-#include <sys/types.h>

-#include <fcntl.h>

-#include <signal.h>

-

-service_t *services = NULL;

-

-/* shutdown_openocd == 1: exit the main event loop, and quit the debugger */

-static int shutdown_openocd = 0;

-int handle_shutdown_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-

-int add_connection(service_t *service, command_context_t *cmd_ctx)

-{

-	unsigned int address_size;

-	connection_t *c, **p;

-	int retval;

-	

-	c = malloc(sizeof(connection_t));

-	c->fd = -1;

-	memset(&c->sin, 0, sizeof(c->sin));

-	c->cmd_ctx = copy_command_context(cmd_ctx);

-	c->service = service;

-	c->input_pending = 0;

-	c->priv = NULL;

-	c->next = NULL;

-

-	address_size = sizeof(c->sin);

-	c->fd = accept(service->fd, (struct sockaddr *)&service->sin, &address_size);

-				

-	if ((retval = service->new_connection(c)) == ERROR_OK)

-	{

-		INFO("accepted '%s' connection from %i", service->name, c->sin.sin_port);

-	}

-	else

-	{

-		close_socket(c->fd);

-		INFO("attempted '%s' connection rejected", service->name);

-		free(c);

-	}

-	

-	/* add to the end of linked list */

-	for (p = &service->connections; *p; p = &(*p)->next);

-	*p = c;

-	

-	service->max_connections--;

-	

-	return ERROR_OK;

-}

-

-int remove_connection(service_t *service, connection_t *connection)

-{

-	connection_t **p = &service->connections;

-	connection_t *c;

-	

-	/* find connection */

-	while(c = *p)

-	{		

-		if (c->fd == connection->fd)

-		{	

-			service->connection_closed(c);

-			close_socket(c->fd);

-			command_done(c->cmd_ctx);

-			

-			/* delete connection */

-			*p = c->next;

-			free(c);

-			

-			service->max_connections++;

-			break;

-		}

-		

-		/* redirect p to next list pointer */

-		p = &(*p)->next;		

-	}

-	

-	return ERROR_OK;

-}

-

-int add_service(char *name, enum connection_type type, unsigned short port, int max_connections, new_connection_handler_t new_connection_handler, input_handler_t input_handler, connection_closed_handler_t connection_closed_handler, void *priv)

-{

-	service_t *c, **p;

-	int so_reuseaddr_option = 1;

-	

-	c = malloc(sizeof(service_t));

-	

-	c->name = strdup(name);

-	c->type = type;

-	c->port = port;

-	c->max_connections = max_connections;

-	c->fd = -1;

-	c->connections = NULL;

-	c->new_connection = new_connection_handler;

-	c->input = input_handler;

-	c->connection_closed = connection_closed_handler;

-	c->priv = priv;

-	c->next = NULL;

-	

-	if ((c->fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)

-	{

-		ERROR("error creating socket: %s", strerror(errno));

-		exit(-1);

-	}

-	

-	setsockopt(c->fd, SOL_SOCKET, SO_REUSEADDR, (void*)&so_reuseaddr_option, sizeof(int));

-	

-	socket_nonblock(c->fd);

-	

-	memset(&c->sin, 0, sizeof(c->sin));

-	c->sin.sin_family = AF_INET;

-	c->sin.sin_addr.s_addr = INADDR_ANY;

-	c->sin.sin_port = htons(port);

-	

-	if (bind(c->fd, (struct sockaddr *)&c->sin, sizeof(c->sin)) == -1)

-	{

-		ERROR("couldn't bind to socket: %s", strerror(errno));

-		exit(-1);

-	}

-	

-	if (listen(c->fd, 1) == -1)

-	{

-		ERROR("couldn't listen on socket: %s", strerror(errno));

-		exit(-1);

-	}

-	

-	/* add to the end of linked list */

-	for (p = &services; *p; p = &(*p)->next);

-	*p = c;

-	

-	return ERROR_OK;

-}

-

-int remove_service(unsigned short port)

-{

-	service_t **p = &services;

-	service_t *c;

-	

-	/* find service */

-	while(c = *p)

-	{		

-		if (c->port == port)

-		{	

-			if (c->name)

-				free(c->name);

-			

-			if (c->priv)

-				free(c->priv);

-			

-			/* delete service */

-			*p = c->next;

-			free(c);

-		}

-

-		/* redirect p to next list pointer */

-		p = &(*p)->next;

-	}

-	

-	return ERROR_OK;

-}

-

-int remove_services()

-{

-	service_t *c = services;

-

-	/* loop service */

-	while(c)

-	{

-		service_t *next = c->next;

-

-		if (c->name)

-			free(c->name);

-

-		if (c->priv)

-			free(c->priv);

-

-		/* delete service */

-		free(c);

-

-		/* remember the last service for unlinking */

-		c = next;

-	}

-

-	services = NULL;

-	

-	return ERROR_OK;

-}

-

-int server_loop(command_context_t *command_context)

-{

-	service_t *service;

-

-	/* used in select() */

-	fd_set read_fds;

-	struct timeval tv;

-	int fd_max;

-	

-	/* used in accept() */

-	int retval;

-	

-#ifndef _WIN32

-	if (signal(SIGPIPE, SIG_IGN) == SIG_ERR)

-		ERROR("couldn't set SIGPIPE to SIG_IGN");

-#endif

-	

-	/* do regular tasks after at most 10ms */

-	tv.tv_sec = 0;

-	tv.tv_usec = 10000;

-	

-	while(!shutdown_openocd)

-	{

-		/* monitor sockets for acitvity */

-		fd_max = 0;

-		FD_ZERO(&read_fds);

-

-		/* add service and connection fds to read_fds */

-		for (service = services; service; service = service->next)

-		{

-			if (service->fd != -1)

-			{

-				/* listen for new connections */

-				FD_SET(service->fd, &read_fds);

-

-				if (service->fd > fd_max)

-					fd_max = service->fd;

-			}

-			

-			if (service->connections)

-			{

-				connection_t *c;

-				

-				for (c = service->connections; c; c = c->next)

-				{

-					/* check for activity on the connection */

-					FD_SET(c->fd, &read_fds);

-					if (c->fd > fd_max)

-						fd_max = c->fd;

-				}

-			}

-		}

-		

-#ifndef _WIN32

-		/* add STDIN to read_fds */

-		FD_SET(fileno(stdin), &read_fds);

-#endif

-

-		retval = select(fd_max + 1, &read_fds, NULL, NULL, &tv);

-		

-		if (retval == -1)

-		{

-#ifdef _WIN32

-

-			errno = WSAGetLastError();

-

-			if (errno == WSAEINTR)

-				FD_ZERO(&read_fds);

-			else

-			{

-				ERROR("error during select: %s", strerror(errno));

-				exit(-1);

-			}

-#else

-

-			if (errno == EINTR)

-			{

-				FD_ZERO(&read_fds);

-			}

-			else

-			{

-				ERROR("error during select: %s", strerror(errno));

-				exit(-1);

-			}

-#endif

-		}

-		

-		target_call_timer_callbacks();

-

-		if (retval == 0)

-		{

-			/* do regular tasks after at most 100ms */

-			tv.tv_sec = 0;

-			tv.tv_usec = 10000;

-			FD_ZERO(&read_fds); /* eCos leaves read_fds unchanged in this case!  */

-		}

-		

-		for (service = services; service; service = service->next)

-		{

-			/* handle new connections on listeners */

-			if ((service->fd != -1) 

-				&& (FD_ISSET(service->fd, &read_fds))) 

-			{

-				if (service->max_connections > 0)

-				{

-					add_connection(service, command_context);

-				}

-				else

-				{

-					struct sockaddr_in sin;

-					unsigned int address_size = sizeof(sin);

-					int tmp_fd;

-					tmp_fd = accept(service->fd, (struct sockaddr *)&service->sin, &address_size);

-					close_socket(tmp_fd);

-					INFO("rejected '%s' connection, no more connections allowed", service->name);

-				}

-			}

-			

-			/* handle activity on connections */

-			if (service->connections)

-			{

-				connection_t *c;

-				

-				for (c = service->connections; c;)

-				{

-					if ((FD_ISSET(c->fd, &read_fds)) || c->input_pending)

-					{

-						if (service->input(c) != ERROR_OK)

-						{

-							connection_t *next = c->next;

-							remove_connection(service, c);

-							INFO("dropped '%s' connection", service->name);

-							c = next;

-							continue;

-						}

-					}

-					c = c->next;

-				}

-			}

-		}

-		

-#ifndef _WIN32

-		if (FD_ISSET(fileno(stdin), &read_fds))

-		{

-			if (getc(stdin) == 'x')

-			{

-				shutdown_openocd = 1;

-			}

-		}

-#else

-		MSG msg;

-		while (PeekMessage(&msg,NULL,0,0,PM_REMOVE))

-		{

-			if (msg.message == WM_QUIT)

-				shutdown_openocd = 1;

-		}

-#endif

-	}

-	

-	return ERROR_OK;

-}

-

-#ifdef _WIN32

-BOOL WINAPI ControlHandler(DWORD dwCtrlType)

-{

-    shutdown_openocd = 1;

-    return TRUE;

-}

-

-void sig_handler(int sig) {

-    shutdown_openocd = 1;

-}

-#endif

-

-int server_init()

-{

-#ifdef _WIN32

-	WORD wVersionRequested;

-	WSADATA wsaData;

-

-	wVersionRequested = MAKEWORD( 2, 2 );

-

-	if (WSAStartup(wVersionRequested, &wsaData) != 0)

-	{

-		ERROR("Failed to Open Winsock");

-		exit(-1);

-	}

-

-	SetConsoleCtrlHandler( ControlHandler, TRUE );

-

-	signal(SIGINT, sig_handler);

-	signal(SIGTERM, sig_handler);

-	signal(SIGBREAK, sig_handler);

-	signal(SIGABRT, sig_handler);

-#endif

-

-	

-	return ERROR_OK;

-}

-

-int server_quit()

-{

-	remove_services();

-

-#ifdef _WIN32

-	WSACleanup();

-	SetConsoleCtrlHandler( ControlHandler, FALSE );

-#endif

-

-	return ERROR_OK;

-}

-

-int server_register_commands(command_context_t *context)

-{

-	register_command(context, NULL, "shutdown", handle_shutdown_command,

-					 COMMAND_ANY, "shut the server down");

-	

-	return ERROR_OK;

-}

-

-/* tell the server we want to shut down */

-int handle_shutdown_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	shutdown_openocd = 1;

-

-	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 "server.h"
+
+#include "log.h"
+#include "telnet_server.h"
+#include "target.h"
+
+#include <command.h>
+#include <string.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <signal.h>
+
+service_t *services = NULL;
+
+/* shutdown_openocd == 1: exit the main event loop, and quit the debugger */
+static int shutdown_openocd = 0;
+int handle_shutdown_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int add_connection(service_t *service, command_context_t *cmd_ctx)
+{
+	unsigned int address_size;
+	connection_t *c, **p;
+	int retval;
+	
+	c = malloc(sizeof(connection_t));
+	c->fd = -1;
+	memset(&c->sin, 0, sizeof(c->sin));
+	c->cmd_ctx = copy_command_context(cmd_ctx);
+	c->service = service;
+	c->input_pending = 0;
+	c->priv = NULL;
+	c->next = NULL;
+
+	address_size = sizeof(c->sin);
+	c->fd = accept(service->fd, (struct sockaddr *)&service->sin, &address_size);
+				
+	if ((retval = service->new_connection(c)) == ERROR_OK)
+	{
+		INFO("accepted '%s' connection from %i", service->name, c->sin.sin_port);
+	}
+	else
+	{
+		close_socket(c->fd);
+		INFO("attempted '%s' connection rejected", service->name);
+		free(c);
+	}
+	
+	/* add to the end of linked list */
+	for (p = &service->connections; *p; p = &(*p)->next);
+	*p = c;
+	
+	service->max_connections--;
+	
+	return ERROR_OK;
+}
+
+int remove_connection(service_t *service, connection_t *connection)
+{
+	connection_t **p = &service->connections;
+	connection_t *c;
+	
+	/* find connection */
+	while(c = *p)
+	{		
+		if (c->fd == connection->fd)
+		{	
+			service->connection_closed(c);
+			close_socket(c->fd);
+			command_done(c->cmd_ctx);
+			
+			/* delete connection */
+			*p = c->next;
+			free(c);
+			
+			service->max_connections++;
+			break;
+		}
+		
+		/* redirect p to next list pointer */
+		p = &(*p)->next;		
+	}
+	
+	return ERROR_OK;
+}
+
+int add_service(char *name, enum connection_type type, unsigned short port, int max_connections, new_connection_handler_t new_connection_handler, input_handler_t input_handler, connection_closed_handler_t connection_closed_handler, void *priv)
+{
+	service_t *c, **p;
+	int so_reuseaddr_option = 1;
+	
+	c = malloc(sizeof(service_t));
+	
+	c->name = strdup(name);
+	c->type = type;
+	c->port = port;
+	c->max_connections = max_connections;
+	c->fd = -1;
+	c->connections = NULL;
+	c->new_connection = new_connection_handler;
+	c->input = input_handler;
+	c->connection_closed = connection_closed_handler;
+	c->priv = priv;
+	c->next = NULL;
+	
+	if ((c->fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
+	{
+		ERROR("error creating socket: %s", strerror(errno));
+		exit(-1);
+	}
+	
+	setsockopt(c->fd, SOL_SOCKET, SO_REUSEADDR, (void*)&so_reuseaddr_option, sizeof(int));
+	
+	socket_nonblock(c->fd);
+	
+	memset(&c->sin, 0, sizeof(c->sin));
+	c->sin.sin_family = AF_INET;
+	c->sin.sin_addr.s_addr = INADDR_ANY;
+	c->sin.sin_port = htons(port);
+	
+	if (bind(c->fd, (struct sockaddr *)&c->sin, sizeof(c->sin)) == -1)
+	{
+		ERROR("couldn't bind to socket: %s", strerror(errno));
+		exit(-1);
+	}
+	
+	if (listen(c->fd, 1) == -1)
+	{
+		ERROR("couldn't listen on socket: %s", strerror(errno));
+		exit(-1);
+	}
+	
+	/* add to the end of linked list */
+	for (p = &services; *p; p = &(*p)->next);
+	*p = c;
+	
+	return ERROR_OK;
+}
+
+int remove_service(unsigned short port)
+{
+	service_t **p = &services;
+	service_t *c;
+	
+	/* find service */
+	while(c = *p)
+	{		
+		if (c->port == port)
+		{	
+			if (c->name)
+				free(c->name);
+			
+			if (c->priv)
+				free(c->priv);
+			
+			/* delete service */
+			*p = c->next;
+			free(c);
+		}
+
+		/* redirect p to next list pointer */
+		p = &(*p)->next;
+	}
+	
+	return ERROR_OK;
+}
+
+int remove_services()
+{
+	service_t *c = services;
+
+	/* loop service */
+	while(c)
+	{
+		service_t *next = c->next;
+
+		if (c->name)
+			free(c->name);
+
+		if (c->priv)
+			free(c->priv);
+
+		/* delete service */
+		free(c);
+
+		/* remember the last service for unlinking */
+		c = next;
+	}
+
+	services = NULL;
+	
+	return ERROR_OK;
+}
+
+int server_loop(command_context_t *command_context)
+{
+	service_t *service;
+
+	/* used in select() */
+	fd_set read_fds;
+	struct timeval tv;
+	int fd_max;
+	
+	/* used in accept() */
+	int retval;
+	
+#ifndef _WIN32
+	if (signal(SIGPIPE, SIG_IGN) == SIG_ERR)
+		ERROR("couldn't set SIGPIPE to SIG_IGN");
+#endif
+	
+	/* do regular tasks after at most 10ms */
+	tv.tv_sec = 0;
+	tv.tv_usec = 10000;
+	
+	while(!shutdown_openocd)
+	{
+		/* monitor sockets for acitvity */
+		fd_max = 0;
+		FD_ZERO(&read_fds);
+
+		/* add service and connection fds to read_fds */
+		for (service = services; service; service = service->next)
+		{
+			if (service->fd != -1)
+			{
+				/* listen for new connections */
+				FD_SET(service->fd, &read_fds);
+
+				if (service->fd > fd_max)
+					fd_max = service->fd;
+			}
+			
+			if (service->connections)
+			{
+				connection_t *c;
+				
+				for (c = service->connections; c; c = c->next)
+				{
+					/* check for activity on the connection */
+					FD_SET(c->fd, &read_fds);
+					if (c->fd > fd_max)
+						fd_max = c->fd;
+				}
+			}
+		}
+		
+#ifndef _WIN32
+		/* add STDIN to read_fds */
+		FD_SET(fileno(stdin), &read_fds);
+#endif
+
+		retval = select(fd_max + 1, &read_fds, NULL, NULL, &tv);
+		
+		if (retval == -1)
+		{
+#ifdef _WIN32
+
+			errno = WSAGetLastError();
+
+			if (errno == WSAEINTR)
+				FD_ZERO(&read_fds);
+			else
+			{
+				ERROR("error during select: %s", strerror(errno));
+				exit(-1);
+			}
+#else
+
+			if (errno == EINTR)
+			{
+				FD_ZERO(&read_fds);
+			}
+			else
+			{
+				ERROR("error during select: %s", strerror(errno));
+				exit(-1);
+			}
+#endif
+		}
+		
+		target_call_timer_callbacks();
+
+		if (retval == 0)
+		{
+			/* do regular tasks after at most 100ms */
+			tv.tv_sec = 0;
+			tv.tv_usec = 10000;
+			FD_ZERO(&read_fds); /* eCos leaves read_fds unchanged in this case!  */
+		}
+		
+		for (service = services; service; service = service->next)
+		{
+			/* handle new connections on listeners */
+			if ((service->fd != -1) 
+				&& (FD_ISSET(service->fd, &read_fds))) 
+			{
+				if (service->max_connections > 0)
+				{
+					add_connection(service, command_context);
+				}
+				else
+				{
+					struct sockaddr_in sin;
+					unsigned int address_size = sizeof(sin);
+					int tmp_fd;
+					tmp_fd = accept(service->fd, (struct sockaddr *)&service->sin, &address_size);
+					close_socket(tmp_fd);
+					INFO("rejected '%s' connection, no more connections allowed", service->name);
+				}
+			}
+			
+			/* handle activity on connections */
+			if (service->connections)
+			{
+				connection_t *c;
+				
+				for (c = service->connections; c;)
+				{
+					if ((FD_ISSET(c->fd, &read_fds)) || c->input_pending)
+					{
+						if (service->input(c) != ERROR_OK)
+						{
+							connection_t *next = c->next;
+							remove_connection(service, c);
+							INFO("dropped '%s' connection", service->name);
+							c = next;
+							continue;
+						}
+					}
+					c = c->next;
+				}
+			}
+		}
+		
+#ifndef _WIN32
+		if (FD_ISSET(fileno(stdin), &read_fds))
+		{
+			if (getc(stdin) == 'x')
+			{
+				shutdown_openocd = 1;
+			}
+		}
+#else
+		MSG msg;
+		while (PeekMessage(&msg,NULL,0,0,PM_REMOVE))
+		{
+			if (msg.message == WM_QUIT)
+				shutdown_openocd = 1;
+		}
+#endif
+	}
+	
+	return ERROR_OK;
+}
+
+#ifdef _WIN32
+BOOL WINAPI ControlHandler(DWORD dwCtrlType)
+{
+    shutdown_openocd = 1;
+    return TRUE;
+}
+
+void sig_handler(int sig) {
+    shutdown_openocd = 1;
+}
+#endif
+
+int server_init()
+{
+#ifdef _WIN32
+	WORD wVersionRequested;
+	WSADATA wsaData;
+
+	wVersionRequested = MAKEWORD( 2, 2 );
+
+	if (WSAStartup(wVersionRequested, &wsaData) != 0)
+	{
+		ERROR("Failed to Open Winsock");
+		exit(-1);
+	}
+
+	SetConsoleCtrlHandler( ControlHandler, TRUE );
+
+	signal(SIGINT, sig_handler);
+	signal(SIGTERM, sig_handler);
+	signal(SIGBREAK, sig_handler);
+	signal(SIGABRT, sig_handler);
+#endif
+
+	
+	return ERROR_OK;
+}
+
+int server_quit()
+{
+	remove_services();
+
+#ifdef _WIN32
+	WSACleanup();
+	SetConsoleCtrlHandler( ControlHandler, FALSE );
+#endif
+
+	return ERROR_OK;
+}
+
+int server_register_commands(command_context_t *context)
+{
+	register_command(context, NULL, "shutdown", handle_shutdown_command,
+					 COMMAND_ANY, "shut the server down");
+	
+	return ERROR_OK;
+}
+
+/* tell the server we want to shut down */
+int handle_shutdown_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	shutdown_openocd = 1;
+
+	return ERROR_COMMAND_CLOSE_CONNECTION;
+}
+
+
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index 830cf64..5cfb0ab 100644
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -1,631 +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 = 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;

-}

+/***************************************************************************
+ *   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;
+}