diff --git a/src/gdb_main.c b/src/gdb_main.c
index bfbe8cd..e11fce0 100644
--- a/src/gdb_main.c
+++ b/src/gdb_main.c
@@ -48,6 +48,14 @@ enum gdb_signal {
 #define ERROR_IF_NO_TARGET()	\
 	if(!cur_target) { gdb_putpacketz("EFF"); break; }
 
+typedef struct
+{
+	const char *cmd_prefix;
+	void  (*func)(const char *packet,int len);
+}cmd_executer;
+
+
+
 static char pbuf[BUF_SIZE+1];
 
 static target *cur_target;
@@ -329,6 +337,40 @@ int gdb_main_loop(struct target_controller *tc, bool in_syscall)
 		}
 	}
 }
+bool exec_command(char *packet, int len, const cmd_executer *exec)
+{
+	while(exec->cmd_prefix) {
+		int l=strlen(exec->cmd_prefix);
+		if(!strncmp(packet,exec->cmd_prefix,l)) {
+			exec->func(packet+l,len-l);
+			return true;
+		}
+		exec++;
+	}
+	return false;
+}
+
+static void exec_qRcmd(const char *packet,int len)
+{
+char *data;
+int datalen;
+
+	/* calculate size and allocate buffer for command */
+	datalen = len/ 2;
+	data = alloca(datalen+1);
+	/* dehexify command */
+	unhexify(data, packet, datalen);
+	data[datalen] = 0;	/* add terminating null */
+
+	int c = command_process(cur_target, data);
+	if(c < 0)
+		gdb_putpacketz("");
+	else if(c == 0)
+		gdb_putpacketz("OK");
+	else
+		gdb_putpacket(hexify(pbuf, "Failed\n", strlen("Failed\n")),
+				2 * strlen("Failed\n"));
+}
 
 static void
 handle_q_string_reply(const char *str, const char *param)
@@ -339,76 +381,64 @@ handle_q_string_reply(const char *str, const char *param)
 		gdb_putpacketz("E01");
 		return;
 	}
-	if (addr < strlen (str)) {
-		char reply[len+2];
-		reply[0] = 'm';
-		strncpy (reply + 1, &str[addr], len);
-		if(len > strlen(&str[addr]))
-			len = strlen(&str[addr]);
-		gdb_putpacket(reply, len + 1);
-	} else if (addr == strlen (str)) {
-		gdb_putpacketz("l");
-	} else
+	if (addr > strlen (str)) {
 		gdb_putpacketz("E01");
+		return;
+	}
+	if(addr== strlen (str)) {
+		gdb_putpacketz("l");
+		return;
+	}
+	unsigned long output_len=strlen(str)-addr;
+	if(output_len>len) output_len=len;
+	gdb_putpacket2("m",1,str+addr,output_len);
+}
+static void exec_qSupported(const char *packet, int len)
+{
+	(void)packet;
+	(void)len;
+	gdb_putpacket_f("PacketSize=%X;qXfer:memory-map:read+;qXfer:features:read+", BUF_SIZE);
 }
 
-static void
-handle_q_packet(char *packet, int len)
+static void exec_qMemoryMap(const char *packet,int len)
 {
+	(void)packet;
+	(void)len;
+	/* Read target XML memory map */
+	if((!cur_target) && last_target) {
+		/* Attach to last target if detached. */
+		cur_target = target_attach(last_target,
+				   &gdb_controller);
+	}
+	if (!cur_target) {
+		gdb_putpacketz("E01");
+		return;
+	}
+	char buf[1024];
+	target_mem_map(cur_target, buf, sizeof(buf)); /* Fixme: Check size!*/
+	handle_q_string_reply(buf, packet);
+}
+
+static void exec_qFeatureRead(const char *packet, int len)
+{
+	(void)len;
+	/* Read target description */
+	if((!cur_target) && last_target) {
+	  /* Attach to last target if detached. */
+	  cur_target = target_attach(last_target, &gdb_controller);
+	}
+	if (!cur_target) {
+	  gdb_putpacketz("E01");
+	  return;
+	}
+	handle_q_string_reply(target_tdesc(cur_target), packet );
+}
+
+static void exec_qCRC(const char *packet, int len)
+{
+	(void)len;
 	uint32_t addr, alen;
-
-	if(!strncmp(packet, "qRcmd,", 6)) {
-		char *data;
-		int datalen;
-
-		/* calculate size and allocate buffer for command */
-		datalen = (len - 6) / 2;
-		data = alloca(datalen+1);
-		/* dehexify command */
-		unhexify(data, packet+6, datalen);
-		data[datalen] = 0;	/* add terminating null */
-
-		int c = command_process(cur_target, data);
-		if(c < 0)
-			gdb_putpacketz("");
-		else if(c == 0)
-			gdb_putpacketz("OK");
-		else
-			gdb_putpacket(hexify(pbuf, "Failed\n", strlen("Failed\n")),
-						  2 * strlen("Failed\n"));
-
-	} else if (!strncmp (packet, "qSupported", 10)) {
-		/* Query supported protocol features */
-		gdb_putpacket_f("PacketSize=%X;qXfer:memory-map:read+;qXfer:features:read+", BUF_SIZE);
-
-	} else if (strncmp (packet, "qXfer:memory-map:read::", 23) == 0) {
-		/* Read target XML memory map */
-		if((!cur_target) && last_target) {
-			/* Attach to last target if detached. */
-			cur_target = target_attach(last_target,
-						   &gdb_controller);
-		}
-		if (!cur_target) {
-			gdb_putpacketz("E01");
-			return;
-		}
-		char buf[1024];
-		target_mem_map(cur_target, buf, sizeof(buf)); /* Fixme: Check size!*/
-		handle_q_string_reply(buf, packet + 23);
-
-	} else if (strncmp (packet, "qXfer:features:read:target.xml:", 31) == 0) {
-		/* Read target description */
-		if((!cur_target) && last_target) {
-			/* Attach to last target if detached. */
-			cur_target = target_attach(last_target,
-						   &gdb_controller);
-		}
-		if (!cur_target) {
-			gdb_putpacketz("E01");
-			return;
-		}
-		handle_q_string_reply(target_tdesc(cur_target), packet + 31);
-	} else if (sscanf(packet, "qCRC:%" PRIx32 ",%" PRIx32, &addr, &alen) == 2) {
+	if (sscanf(packet, "%" PRIx32 ",%" PRIx32, &addr, &alen) == 2) {
 		if(!cur_target) {
 			gdb_putpacketz("E01");
 			return;
@@ -419,12 +449,28 @@ handle_q_packet(char *packet, int len)
 			gdb_putpacketz("E03");
 		else
 			gdb_putpacket_f("C%lx", crc);
-
-	} else {
-		DEBUG_GDB("*** Unsupported packet: %s\n", packet);
-		gdb_putpacket("", 0);
 	}
 }
+static const cmd_executer q_commands[]=
+{
+	{"qRcmd,",                         exec_qRcmd},
+	{"qSupported",                     exec_qSupported},
+	{"qXfer:memory-map:read::",        exec_qMemoryMap},
+	{"qXfer:features:read:target.xml:",exec_qFeatureRead},
+	{"qCRC:",                          exec_qCRC},
+
+	{NULL,NULL},
+};
+
+static void
+handle_q_packet(char *packet, int len)
+{
+	if(exec_command(packet,len,q_commands)) {
+		return;
+	}
+	DEBUG_GDB("*** Unsupported packet: %s\n", packet);
+	gdb_putpacket("", 0);
+}
 
 static void
 handle_v_packet(char *packet, int plen)
diff --git a/src/gdb_packet.c b/src/gdb_packet.c
index 80095b7..e25cd5c 100644
--- a/src/gdb_packet.c
+++ b/src/gdb_packet.c
@@ -160,7 +160,6 @@ void gdb_putpacket2(const char *packet1, int size1,const char *packet2, int size
 {
 	int i;
 	unsigned char csum;
-	unsigned char c;
 	char xmit_csum[3];
 	int tries = 0;
 
@@ -185,7 +184,6 @@ void gdb_putpacket(const char *packet, int size)
 {
 	int i;
 	unsigned char csum;
-	unsigned char c;
 	char xmit_csum[3];
 	int tries = 0;
 
@@ -219,7 +217,6 @@ void gdb_putpacket_f(const char *fmt, ...)
 void gdb_out(const char *buf)
 {
 	char *hexdata;
-	int i;
 
 	int l=strlen(buf);
 	hexdata = alloca(2*l+1);