summaryrefslogtreecommitdiff
path: root/src/server
diff options
context:
space:
mode:
authordrath <drath@b42882b7-edfa-0310-969c-e2dbd0fdcd60>2006-06-02 10:36:31 +0000
committerdrath <drath@b42882b7-edfa-0310-969c-e2dbd0fdcd60>2006-06-02 10:36:31 +0000
commit8b4e882a1630d63bbc9840fa3f968e36b6ac3702 (patch)
treee8f8ab1ba76fc7bef013fed6e5fea28e1cb7a554 /src/server
downloadopenocd+libswd-8b4e882a1630d63bbc9840fa3f968e36b6ac3702.tar.gz
openocd+libswd-8b4e882a1630d63bbc9840fa3f968e36b6ac3702.tar.bz2
openocd+libswd-8b4e882a1630d63bbc9840fa3f968e36b6ac3702.tar.xz
openocd+libswd-8b4e882a1630d63bbc9840fa3f968e36b6ac3702.zip
- prepare OpenOCD for branching, created ./trunk/
git-svn-id: svn://svn.berlios.de/openocd/trunk@64 b42882b7-edfa-0310-969c-e2dbd0fdcd60
Diffstat (limited to 'src/server')
-rw-r--r--src/server/Makefile.am5
-rw-r--r--src/server/gdb_server.c1108
-rw-r--r--src/server/gdb_server.h47
-rw-r--r--src/server/server.c368
-rw-r--r--src/server/server.h75
-rw-r--r--src/server/telnet_server.c570
-rw-r--r--src/server/telnet_server.h68
7 files changed, 2241 insertions, 0 deletions
diff --git a/src/server/Makefile.am b/src/server/Makefile.am
new file mode 100644
index 00000000..2397a7f0
--- /dev/null
+++ b/src/server/Makefile.am
@@ -0,0 +1,5 @@
+INCLUDES = -I$(top_srcdir)/src/helper -I$(top_srcdir)/src/target $(all_includes)
+METASOURCES = AUTO
+noinst_LIBRARIES = libserver.a
+noinst_HEADERS = server.h telnet_server.h gdb_server.h
+libserver_a_SOURCES = server.c telnet_server.c gdb_server.c
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
new file mode 100644
index 00000000..125206dc
--- /dev/null
+++ b/src/server/gdb_server.c
@@ -0,0 +1,1108 @@
+/***************************************************************************
+ * 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. *
+ ***************************************************************************/
+#include "gdb_server.h"
+
+#include "server.h"
+#include "log.h"
+#include "binarybuffer.h"
+#include "breakpoints.h"
+
+#define __USE_GNU
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+
+// -ino: 060521-1116
+#ifdef __FreeBSD__
+#include <stdio.h>
+char * strndup(char * str, int n) {
+ unsigned char * tmp = malloc((size_t)n+1);
+ if (! tmp) perror("gdb_server malloc failed");
+ if (strlcpy(tmp, str, n) > n) perror("gdb_server strndup: too long");
+ return tmp;
+}
+#endif
+#if 0
+#define _DEBUG_GDB_IO_
+#endif
+
+static unsigned short gdb_port;
+
+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;
+ char *debug_buffer;
+
+ 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;
+ }
+
+ while ((gdb_con->buf_cnt = read(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE)) <= 0)
+ {
+ if (gdb_con->buf_cnt == 0)
+ return ERROR_SERVER_REMOTE_CLOSED;
+
+ 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));
+ exit(-1);
+ }
+ }
+
+ 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);
+
+ 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_put_packet(connection_t *connection, char *buffer, int len)
+{
+ int i;
+ unsigned char my_checksum = 0;
+ char checksum[3];
+ char *debug_buffer;
+ int reply;
+ int retval;
+ gdb_connection_t *gdb_con = connection->priv;
+
+ for (i = 0; i < len; i++)
+ my_checksum += buffer[i];
+
+ while (1)
+ {
+
+ 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);
+
+ write(connection->fd, "$", 1);
+ if (len > 0)
+ write(connection->fd, buffer, len);
+ write(connection->fd, "#", 1);
+
+ snprintf(checksum, 3, "%2.2x", my_checksum);
+
+ write(connection->fd, checksum, 2);
+
+ if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
+ return retval;
+
+ if (reply == '+')
+ break;
+ else if (reply == '-')
+ 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 == '-')
+ 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;
+ }
+ }
+
+ return ERROR_OK;
+}
+
+int gdb_get_packet(connection_t *connection, char *buffer, int *len)
+{
+ int character;
+ int count = 0;
+ int retval;
+ int first_char = 0;
+ int packet_type;
+ 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;
+
+ 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;
+
+ do
+ {
+ if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
+ return retval;
+
+ if( !first_char ) {
+ packet_type = character;
+ first_char = 1;
+ }
+
+ if( packet_type == 'X' )
+ {
+ switch (character)
+ {
+ case '#':
+ break;
+ case 0x7d:
+ /* data transmitted in binary mode (X packet)
+ * uses 0x7d as escape character */
+ my_checksum += character & 0xff;
+ gdb_get_char(connection, &character);
+ my_checksum += character & 0xff;
+ buffer[count++] = (character ^ 0x20) & 0xff;
+ if (count > *len)
+ {
+ ERROR("packet buffer too small");
+ return ERROR_GDB_BUFFER_TOO_SMALL;
+ }
+ break;
+ default:
+ buffer[count++] = character & 0xff;
+ my_checksum += character & 0xff;
+ if (count > *len)
+ {
+ ERROR("packet buffer too small");
+ return ERROR_GDB_BUFFER_TOO_SMALL;
+ }
+ break;
+ }
+ }
+ else
+ {
+ switch (character)
+ {
+ case '#':
+ break;
+ case 0x3:
+ gdb_con->ctrl_c = 1;
+ break;
+ default:
+ buffer[count++] = character & 0xff;
+ my_checksum += character & 0xff;
+ if (count > *len)
+ {
+ ERROR("packet buffer too small");
+ return ERROR_GDB_BUFFER_TOO_SMALL;
+ }
+ break;
+ }
+ }
+ } while (character != '#');
+
+ *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))
+ {
+ write (connection->fd, "+", 1);
+ break;
+ }
+
+ WARNING("checksum error, requesting retransmission");
+ write(connection->fd, "-", 1);
+ }
+
+ return ERROR_OK;
+}
+
+int gdb_output(struct command_context_s *context, char* line)
+{
+ connection_t *connection = context->output_handler_priv;
+ 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_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:
+ if (gdb_connection->frontend_state == TARGET_RUNNING)
+ {
+ 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_RESUMED:
+ if (gdb_connection->frontend_state == TARGET_HALTED)
+ {
+ gdb_connection->frontend_state = TARGET_RUNNING;
+ }
+ 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;
+
+ /* 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 when trying to halt target");
+ exit(-1);
+ }
+
+ while (gdb_service->target->state != TARGET_HALTED)
+ {
+ gdb_service->target->type->poll(gdb_service->target);
+ }
+
+ /* remove the initial ACK from the incoming buffer */
+ if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
+ return retval;
+
+ return ERROR_OK;
+}
+
+int gdb_connection_closed(connection_t *connection)
+{
+ if (connection->priv)
+ free(connection->priv);
+ else
+ ERROR("BUG: connection->priv == NULL");
+
+ target_unregister_event_callback(gdb_target_callback_event_handler, connection);
+
+ return ERROR_OK;
+}
+
+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;
+}
+
+void 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;
+
+ 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 requested registers, but we're not halted");
+ exit(-1);
+ default:
+ 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++)
+ {
+ int j;
+ char *hex_buf = buf_to_char(reg_list[i]->value, reg_list[i]->size);
+ DEBUG("hex_buf: %s", hex_buf);
+ for (j = CEIL(reg_list[i]->size, 8) * 2; j > 0; j -= 2)
+ {
+ *reg_packet_p++ = hex_buf[j - 2];
+ *reg_packet_p++ = hex_buf[j - 1];
+ }
+ free(hex_buf);
+ }
+
+ reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
+ DEBUG("reg_packet: %s", reg_packet_p);
+ free(reg_packet_p);
+
+ gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
+ free(reg_packet);
+
+}
+
+void 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;
+
+ DEBUG("");
+
+ /* skip command character */
+ packet++;
+ packet_size--;
+
+ if (packet_size % 2)
+ {
+ WARNING("GDB set_registers packet with uneven characters received");
+ return;
+ }
+
+ 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");
+ exit(-1);
+ default:
+ ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
+ exit(-1);
+ }
+ }
+
+ packet_p = packet;
+ for (i = 0; i < reg_list_size; i++)
+ {
+ char_to_buf(packet, CEIL(reg_list[i]->size, 8) * 2, reg_list[i]->value, reg_list[i]->size);
+ reg_list[i]->dirty = 1;
+ }
+
+ gdb_put_packet(connection, "OK", 2);
+}
+
+void gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ char *hex_buf;
+ char *reg_packet;
+ char *reg_packet_p;
+ int reg_num = strtoul(packet + 1, NULL, 16);
+ reg_t **reg_list;
+ int reg_list_size;
+ int retval;
+ int i;
+
+ 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 requested registers, but we're not halted");
+ exit(-1);
+ default:
+ 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);
+ }
+
+ hex_buf = buf_to_char(reg_list[reg_num]->value, reg_list[reg_num]->size);
+ reg_packet = reg_packet_p = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
+
+ for (i = CEIL(reg_list[reg_num]->size, 8) * 2; i > 0; i -= 2)
+ {
+ *reg_packet_p++ = hex_buf[i - 2];
+ *reg_packet_p++ = hex_buf[i - 1];
+ }
+
+ gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
+
+ free(reg_packet);
+ free(hex_buf);
+
+}
+
+void gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ char *separator;
+ int reg_num = strtoul(packet + 1, &separator, 16);
+ reg_t **reg_list;
+ int reg_list_size;
+ int retval;
+
+ 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 requested registers, but we're not halted");
+ exit(-1);
+ default:
+ 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);
+ }
+
+ if (*separator != '=')
+ {
+ ERROR("GDB set register packet, but no '=' following the register number");
+ exit(-1);
+ }
+
+ char_to_buf(separator + 1, CEIL(reg_list[reg_num]->size, 8) * 2, reg_list[reg_num]->value, reg_list[reg_num]->size);
+ reg_list[reg_num]->dirty = 1;
+
+ gdb_put_packet(connection, "OK", 2);
+
+}
+
+void 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 i;
+
+ /* skip command character */
+ packet++;
+
+ addr = strtoul(packet, &separator, 16);
+
+ if (*separator != ',')
+ return;
+
+ len = strtoul(separator+1, NULL, 16);
+
+ buffer = malloc(len);
+
+ DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
+
+ switch (len)
+ {
+ case 4:
+ if ((addr % 4) == 0)
+ target->type->read_memory(target, addr, 4, 1, buffer);
+ else
+ target->type->read_memory(target, addr, 1, len, buffer);
+ break;
+ case 2:
+ if ((addr % 2) == 0)
+ target->type->read_memory(target, addr, 2, 1, buffer);
+ else
+ target->type->read_memory(target, addr, 1, len, buffer);
+ break;
+ default:
+ if (((addr % 4) == 0) && ((len % 4) == 0))
+ target->type->read_memory(target, addr, 4, len / 4, buffer);
+ else
+ target->type->read_memory(target, addr, 1, len, buffer);
+ }
+
+ hex_buffer = malloc(len * 2 + 1);
+
+ for (i=0; i<len; i++)
+ snprintf(hex_buffer + 2*i, 3, "%2.2x", buffer[i]);
+
+ gdb_put_packet(connection, hex_buffer, len * 2);
+
+ free(hex_buffer);
+ free(buffer);
+}
+
+void 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;
+
+ /* skip command character */
+ packet++;
+
+ addr = strtoul(packet, &separator, 16);
+
+ if (*separator != ',')
+ return;
+
+ len = strtoul(separator+1, &separator, 16);
+
+ if (*(separator++) != ':')
+ return;
+
+ 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;
+ }
+
+ switch (len)
+ {
+ /* handle sized writes */
+ case 4:
+ if ((addr % 4) == 0)
+ target->type->write_memory(target, addr, 4, 1, buffer);
+ else
+ target->type->write_memory(target, addr, 1, len, buffer);
+ break;
+ case 2:
+ if ((addr % 2) == 0)
+ target->type->write_memory(target, addr, 2, 1, buffer);
+ else
+ target->type->write_memory(target, addr, 1, len, buffer);
+ break;
+ case 3:
+ case 1:
+ target->type->write_memory(target, addr, 1, len, buffer);
+ break;
+ /* handle bulk writes */
+ default:
+ target_write_buffer(target, addr, len, buffer);
+ break;
+ }
+
+ gdb_put_packet(connection, "OK", 2);
+
+ free(buffer);
+}
+
+void gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ char *separator;
+ u32 addr = 0;
+ u32 len = 0;
+
+ u8 *buffer;
+
+ /* skip command character */
+ packet++;
+
+ addr = strtoul(packet, &separator, 16);
+
+ if (*separator != ',')
+ return;
+
+ len = strtoul(separator+1, &separator, 16);
+
+ if (*(separator++) != ':')
+ return;
+
+ if( len ) {
+
+ buffer = malloc(len);
+
+ DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
+
+ memcpy( buffer, separator, len );
+
+ switch (len)
+ {
+ case 4:
+ if ((addr % 4) == 0)
+ target->type->write_memory(target, addr, 4, 1, buffer);
+ else
+ target->type->write_memory(target, addr, 1, len, buffer);
+ break;
+ case 2:
+ if ((addr % 2) == 0)
+ target->type->write_memory(target, addr, 2, 1, buffer);
+ else
+ target->type->write_memory(target, addr, 1, len, buffer);
+ break;
+ case 3:
+ case 1:
+ target->type->write_memory(target, addr, 1, len, buffer);
+ break;
+ default:
+ target_write_buffer(target, addr, len, buffer);
+ break;
+ }
+
+ free(buffer);
+ }
+
+ gdb_put_packet(connection, "OK", 2);
+}
+
+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)
+ {
+ u32 address = 0;
+ 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 */
+ }
+}
+
+void gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+ int type;
+ enum breakpoint_type bp_type;
+ 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 != ',')
+ return;
+
+ address = strtoul(separator+1, &separator, 16);
+
+ if (*separator != ',')
+ return;
+
+ 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 == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+ {
+ gdb_put_packet(connection, "E00", 3);
+ break;
+ }
+ }
+ }
+ else
+ {
+ breakpoint_remove(target, address);
+ }
+ gdb_put_packet(connection, "OK", 2);
+ break;
+ case 2:
+ case 3:
+ case 4:
+ {
+ if (packet[0] == 'Z')
+ watchpoint_add(target, address, size, type-2, 0, 0xffffffffu);
+ else
+ watchpoint_remove(target, address);
+ gdb_put_packet(connection, "OK", 2);
+ break;
+ }
+ default:
+ break;
+ }
+
+}
+
+void gdb_query_packet(connection_t *connection, char *packet, int packet_size)
+{
+ command_context_t *cmd_ctx = connection->cmd_ctx;
+ gdb_service_t *gdb_service = connection->service->priv;
+ target_t *target = gdb_service->target;
+
+ 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;
+ command_run_line(cmd_ctx, cmd);
+ free(cmd);
+ }
+ gdb_put_packet(connection, "OK", 2);
+ return;
+ }
+
+ gdb_put_packet(connection, "", 0);
+}
+
+int gdb_input(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;
+
+ /* drain input buffer */
+ do
+ {
+ packet_size = GDB_BUFFER_SIZE-1;
+ if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
+ {
+ switch (retval)
+ {
+ case ERROR_GDB_BUFFER_TOO_SMALL:
+ ERROR("BUG: buffer supplied for gdb packet was too small");
+ exit(-1);
+ case ERROR_SERVER_REMOTE_CLOSED:
+ return ERROR_SERVER_REMOTE_CLOSED;
+ default:
+ ERROR("unexpected error");
+ exit(-1);
+ }
+ }
+
+ /* terminate with zero */
+ packet[packet_size] = 0;
+
+ DEBUG("recevied packet: '%s'", packet);
+
+ if (packet_size > 0)
+ {
+ 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':
+ gdb_query_packet(connection, packet, packet_size);
+ break;
+ case 'g':
+ gdb_get_registers_packet(connection, target, packet, packet_size);
+ break;
+ case 'G':
+ gdb_set_registers_packet(connection, target, packet, packet_size);
+ break;
+ case 'p':
+ gdb_get_register_packet(connection, target, packet, packet_size);
+ break;
+ case 'P':
+ gdb_set_register_packet(connection, target, packet, packet_size);
+ break;
+ case 'm':
+ gdb_read_memory_packet(connection, target, packet, packet_size);
+ break;
+ case 'M':
+ gdb_write_memory_packet(connection, target, packet, packet_size);
+ break;
+ case 'z':
+ case 'Z':
+ 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':
+ gdb_step_continue_packet(connection, target, packet, packet_size);
+ break;
+ case 'D':
+ target->type->resume(target, 1, 0, 1, 0);
+ gdb_put_packet(connection, "OK", 2);
+ break;
+ case 'X':
+ gdb_write_memory_binary_packet(connection, target, packet, packet_size);
+ break;
+ case 'k':
+ gdb_put_packet(connection, "OK", 2);
+ return ERROR_SERVER_REMOTE_CLOSED;
+ default:
+ /* ignore unkown packets */
+ DEBUG("ignoring 0x%2.2x packet", packet[0]);
+ gdb_put_packet(connection, NULL, 0);
+ break;
+ }
+ }
+
+ 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_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);
+
+ 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 gdb_register_commands(command_context_t *command_context)
+{
+ register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
+ COMMAND_CONFIG, "");
+
+ return ERROR_OK;
+}
diff --git a/src/server/gdb_server.h b/src/server/gdb_server.h
new file mode 100644
index 00000000..860b29ca
--- /dev/null
+++ b/src/server/gdb_server.h
@@ -0,0 +1,47 @@
+/***************************************************************************
+ * Copyright (C) 2005 by Dominic Rath *
+ * Dominic.Rath@gmx.de *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+#ifndef GDB_SERVER_H
+#define GDB_SERVER_H
+
+#include "target.h"
+#include "server.h"
+
+#define GDB_BUFFER_SIZE 2048
+
+typedef struct gdb_connection_s
+{
+ char buffer[GDB_BUFFER_SIZE];
+ char *buf_p;
+ int buf_cnt;
+ int ctrl_c;
+ enum target_state frontend_state;
+} gdb_connection_t;
+
+typedef struct gdb_service_s
+{
+ struct target_s *target;
+} gdb_service_t;
+
+extern int gdb_init();
+extern int gdb_register_commands(command_context_t *command_context);
+
+#define ERROR_GDB_BUFFER_TOO_SMALL (-800)
+
+#endif /* GDB_SERVER_H */
diff --git a/src/server/server.c b/src/server/server.c
new file mode 100644
index 00000000..a034da79
--- /dev/null
+++ b/src/server/server.c
@@ -0,0 +1,368 @@
+/***************************************************************************
+ * 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. *
+ ***************************************************************************/
+#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 <sys/socket.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(c->fd);
+ INFO("attempted '%s' connection rejected", service->name);
+ free(c);
+ }
+
+ if (service->connections)
+ {
+ for (p = service->connections; p && p->next; p = p->next);
+ if (p)
+ p->next = c;
+ }
+ else
+ {
+ service->connections = c;
+ }
+
+ service->max_connections--;
+
+ return ERROR_OK;
+}
+
+int remove_connection(service_t *service, connection_t *connection)
+{
+ connection_t *c, *p = NULL;
+
+ /* find connection */
+ for (c = service->connections; c; c = c->next)
+ {
+ if (c->fd == connection->fd)
+ {
+ /* unlink connection */
+ if (p)
+ p->next = c->next;
+ else
+ service->connections = c->next;
+
+ service->connection_closed(c);
+ close(c->fd);
+
+ command_done(c->cmd_ctx);
+
+ /* delete connection */
+ free(c);
+
+ service->max_connections++;
+ break;
+ }
+
+ /* remember the last connection for unlinking */
+ p = c;
+ }
+
+ 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;
+ int oldopts;
+
+ 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, &so_reuseaddr_option, sizeof(int));
+
+ oldopts = fcntl(c->fd, F_GETFL, 0);
+ fcntl(c->fd, F_SETFL, oldopts | O_NONBLOCK);
+
+ 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);
+ }
+
+ if (services)
+ {
+ for (p = services; p && p->next; p = p->next);
+ if (p)
+ p->next = c;
+ }
+ else
+ {
+ services = c;
+ }
+
+ return ERROR_OK;
+}
+
+int remove_service(unsigned short port)
+{
+ service_t *c, *p = NULL;
+
+ /* find service */
+ for (c = services; c; c = c->next)
+ {
+ if (c->port == port)
+ {
+ /* unlink service */
+ if (p)
+ p->next = c->next;
+ else
+ services = c->next;
+
+ if (c->name)
+ free(c->name);
+
+ /* delete service */
+ free(c);
+ }
+
+ /* remember the last service for unlinking */
+ p = c;
+ }
+
+ 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;
+
+ if (signal(SIGPIPE, SIG_IGN) == SIG_ERR)
+ ERROR("couldn't set SIGPIPE to SIG_IGN");
+
+ /* 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;
+ }
+ }
+ }
+
+ /* add STDIN to read_fds */
+ FD_SET(fileno(stdin), &read_fds);
+
+ if ((retval = select(fd_max + 1, &read_fds, NULL, NULL, &tv)) == -1)
+ {
+ if (errno == EINTR)
+ FD_ZERO(&read_fds);
+ else
+ {
+ ERROR("error during select: %s", strerror(errno));
+ exit(-1);
+ }
+ }
+
+ target_call_timer_callbacks();
+
+ if (retval == 0)
+ {
+ /* do regular tasks after at most 100ms */
+ tv.tv_sec = 0;
+ tv.tv_usec = 10000;
+
+#if 0
+ if (shutdown_openocd)
+ return ERROR_COMMAND_CLOSE_CONNECTION;
+
+ handle_target();
+#endif
+ }
+
+ 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(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;
+ }
+ }
+ }
+
+ if (FD_ISSET(fileno(stdin), &read_fds))
+ {
+ if (getc(stdin) == 'x')
+ {
+ shutdown_openocd = 1;
+ }
+ }
+ }
+
+ return ERROR_OK;
+}
+
+int server_init()
+{
+
+ 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/server.h b/src/server/server.h
new file mode 100644
index 00000000..625c364e
--- /dev/null
+++ b/src/server/server.h
@@ -0,0 +1,75 @@
+/***************************************************************************
+ * Copyright (C) 2005 by Dominic Rath *
+ * Dominic.Rath@gmx.de *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+#ifndef SERVER_H
+#define SERVER_H
+
+#include "command.h"
+#include "binarybuffer.h"
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+enum connection_type
+{
+ CONNECTION_GDB,
+ CONNECTION_TELNET,
+};
+
+typedef struct connection_s
+{
+ int fd;
+ struct sockaddr_in sin;
+ command_context_t *cmd_ctx;
+ struct service_s *service;
+ int input_pending;
+ void *priv;
+ struct connection_s *next;
+} connection_t;
+
+typedef int (*new_connection_handler_t)(connection_t *connection);
+typedef int (*input_handler_t)(connection_t *connection);
+typedef int (*connection_closed_handler_t)(connection_t *connection);
+
+typedef struct service_s
+{
+ char *name;
+ enum connection_type type;
+ unsigned short port;
+ int fd;
+ struct sockaddr_in sin;
+ int max_connections;
+ connection_t *connections;
+ new_connection_handler_t new_connection;
+ input_handler_t input;
+ connection_closed_handler_t connection_closed;
+ void *priv;
+ struct service_s *next;
+} service_t;
+
+extern 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);
+extern int server_init();
+extern int server_loop(command_context_t *command_context);
+extern int server_register_commands(command_context_t *context);
+
+#define ERROR_SERVER_REMOTE_CLOSED (-400)
+#define ERROR_CONNECTION_REJECTED (-401)
+
+#endif /* SERVER_H */
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
new file mode 100644
index 00000000..d7933293
--- /dev/null
+++ b/src/server/telnet_server.c
@@ -0,0 +1,570 @@
+/***************************************************************************
+ * 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. *
+ ***************************************************************************/
+#include "telnet_server.h"
+
+#include "server.h"
+#include "log.h"
+#include "command.h"
+#include "target.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 - '@')
+
+void telnet_prompt(connection_t *connection)
+{
+ telnet_connection_t *t_con = connection->priv;
+
+ write(connection->fd, t_con->prompt, strlen(t_con->prompt));
+}
+
+int telnet_output(struct command_context_s *cmd_ctx, char* line)
+{
+ connection_t *connection = cmd_ctx->output_handler_priv;
+
+ write(connection->fd, line, strlen(line));
+ write(connection->fd, "\r\n\0", 3);
+
+ return ERROR_OK;
+}
+
+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;
+ char buffer[512];
+
+ switch (event)
+ {
+ case TARGET_EVENT_HALTED:
+ command_print(cmd_ctx, "Target %i halted", get_num_by_target(target));
+ target->type->arch_state(target, buffer, 512);
+ buffer[511] = 0;
+ command_print(cmd_ctx, "%s", buffer);
+ telnet_prompt(connection);
+ t_con->surpress_prompt = 1;
+ break;
+ case TARGET_EVENT_RESUMED:
+ command_print(cmd_ctx, "Target %i resumed", get_num_by_target(target));
+ telnet_prompt(connection);
+ t_con->surpress_prompt = 1;
+ 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->line_size = 0;
+ telnet_connection->line_cursor = 0;
+ telnet_connection->option_size = 0;
+ telnet_connection->prompt = strdup("> ");
+ telnet_connection->surpress_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 */
+ write(connection->fd, negotiate, strlen(negotiate));
+
+ /* print connection banner */
+ if (telnet_service->banner)
+ {
+ write(connection->fd, telnet_service->banner, strlen(telnet_service->banner));
+ write(connection->fd, "\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)
+ {
+ write(connection->fd, 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)
+ {
+ write(connection->fd, "\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(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 */
+ {
+ write(connection->fd, 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++;
+ write(connection->fd, 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++)
+ {
+ write(connection->fd, "\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;
+
+ write(connection->fd, "\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])
+ {
+ write(connection->fd, t_con->history[i], strlen(t_con->history[i]));
+ write(connection->fd, "\r\n\x00", 3);
+ }
+ }
+ telnet_prompt(connection);
+ t_con->line_size = 0;
+ t_con->line_cursor = 0;
+ continue;
+ }
+
+ /* we're running a command, so we need a prompt
+ * if the output handler is called, this gets set again */
+ t_con->surpress_prompt = 0;
+ if ((retval = command_run_line(command_context, t_con->line)) != ERROR_OK)
+ {
+ if (retval == ERROR_COMMAND_CLOSE_CONNECTION)
+ {
+ return ERROR_SERVER_REMOTE_CLOSED;
+ }
+ }
+
+ /* 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);
+
+ /* 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("");
+
+ /* wrap history at TELNET_LINE_HISTORY_SIZE */
+ if (t_con->next_history > TELNET_LINE_HISTORY_SIZE - 1)
+ t_con->next_history = 0;
+
+ if (!t_con->surpress_prompt)
+ {
+ telnet_prompt(connection);
+ }
+ else
+ {
+ t_con->surpress_prompt = 0;
+ }
+
+ 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;
+ write(connection->fd, "\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);
+
+ write(connection->fd, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+ write(connection->fd, " \b", 2);
+ for (i = t_con->line_cursor; i < t_con->line_size; i++)
+ {
+ write(connection->fd, "\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 */
+ write(connection->fd, "\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)
+ {
+ write(connection->fd, "\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)
+ {
+ write(connection->fd, 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)
+ {
+ write(connection->fd, "\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)
+ {
+ write(connection->fd, 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 - 1 >= 0) ? t_con->current_history - 1 : 127;
+ 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);
+ write(connection->fd, 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 < 128) ? t_con->current_history + 1 : 0;
+ 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);
+ write(connection->fd, 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 */
+ write(connection->fd, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+ /* overwrite last char with whitespace */
+ write(connection->fd, " \b", 2);
+
+ /* move back to cursor position*/
+ for (i = t_con->line_cursor; i < t_con->line_size; i++)
+ {
+ write(connection->fd, "\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);
+
+ for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
+ {
+ if (t_con->history[i])
+ free(t_con->history[i]);
+ }
+
+ if (connection->priv)
+ free(connection->priv);
+ 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;
+}
diff --git a/src/server/telnet_server.h b/src/server/telnet_server.h
new file mode 100644
index 00000000..d6ca0e86
--- /dev/null
+++ b/src/server/telnet_server.h
@@ -0,0 +1,68 @@
+/***************************************************************************
+ * Copyright (C) 2005 by Dominic Rath *
+ * Dominic.Rath@gmx.de *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+#ifndef TELNET_SERVER_H
+#define TELNET_SERVER_H
+
+#include "server.h"
+
+#define TELNET_BUFFER_SIZE (1024)
+
+#define TELNET_OPTION_MAX_SIZE (128)
+#define TELNET_LINE_HISTORY_SIZE (128)
+#define TELNET_LINE_MAX_SIZE (256)
+
+enum telnet_states
+{
+ TELNET_STATE_DATA,
+ TELNET_STATE_IAC,
+ TELNET_STATE_SB,
+ TELNET_STATE_SE,
+ TELNET_STATE_WILL,
+ TELNET_STATE_WONT,
+ TELNET_STATE_DO,
+ TELNET_STATE_DONT,
+ TELNET_STATE_ESCAPE,
+};
+
+typedef struct telnet_connection_s
+{
+ char *prompt;
+ int surpress_prompt;
+ enum telnet_states state;
+ char line[TELNET_LINE_MAX_SIZE];
+ int line_size;
+ int line_cursor;
+ char option[TELNET_OPTION_MAX_SIZE];
+ int option_size;
+ char last_escape;
+ char *history[TELNET_LINE_HISTORY_SIZE];
+ int next_history;
+ int current_history;
+} telnet_connection_t;
+
+typedef struct telnet_service_s
+{
+ char *banner;
+} telnet_service_t;
+
+extern int telnet_init(char *banner);
+extern int telnet_register_commands(command_context_t *command_context);
+
+#endif /* TELNET_SERVER_H */