smp : infra for smp minimum support

8 files changed, 384 insertions, 24 deletions

diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 0b808589..aeb4e2e8 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -11,6 +11,9 @@
  *   Copyright (C) 2011 by Broadcom Corporation                            *
  *   Evan Hunter - ehunter@broadcom.com                                    *
  *                                                                         *
+ *   Copyright (C) ST-Ericsson SA 2011                                     *
+ *   michel.jaouen@stericsson.com : smp minimum support                    *
+ *                                                                         *
  *   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     *
@@ -39,6 +42,7 @@
 #include <target/image.h>
 #include <jtag/jtag.h>
 #include "rtos/rtos.h"
+#include "target/smp.h"
 
 
 /**
@@ -62,7 +66,7 @@ struct gdb_connection
 	int closed;
 	int busy;
 	int noack_mode;
-	bool sync; 	/* set flag to true if you want the next stepi to return immediately.
+	bool sync;	/* set flag to true if you want the next stepi to return immediately.
 	               allowing GDB to pick up a fresh set of register values from the target
 	               without modifying the target state. */
 	/* We delay reporting memory write errors until next step/continue or memory
@@ -2360,6 +2364,24 @@ static int gdb_input_inner(struct connection *connection)
 							"ocd_gdb_restart %s",
 							target_name(target));
 					break;
+
+				case 'j':
+				    /*  packet supported only by smp target i.e cortex_a.c*/
+					/* handle smp packet replying coreid played to gbd */
+					gdb_read_smp_packet(
+							connection, target,
+							packet, packet_size);
+					break;
+
+				case 'J':
+					/*  packet supported only by smp target i.e cortex_a.c */
+					/*  handle smp packet setting coreid to be played at next
+					 *  resume to gdb */
+					gdb_write_smp_packet(
+							connection, target,
+							packet, packet_size);
+					break;
+
 				default:
 					/* ignore unknown packets */
 					LOG_DEBUG("ignoring 0x%2.2x packet", packet[0]);
@@ -2411,21 +2433,43 @@ static int gdb_input(struct connection *connection)
 
 static int gdb_target_start(struct target *target, const char *port)
 {
-	struct gdb_service *gdb_service = malloc(sizeof(struct gdb_service));
+
+	struct gdb_service *gdb_service;
+	int ret;
+	gdb_service = malloc(sizeof(struct gdb_service));
+
 	if (NULL == gdb_service)
 		return -ENOMEM;
 
 	gdb_service->target = target;
+	gdb_service->core[0] = -1;
+	gdb_service->core[1] = -1;
+	target->gdb_service = gdb_service;
 
-	return add_service("gdb",
+	ret = add_service("gdb",
 			port, 1, &gdb_new_connection, &gdb_input,
 			&gdb_connection_closed, gdb_service);
+	/* initialialize all targets gdb service with the same pointer */
+	{
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+		while(head != (struct target_list*)NULL)
+		{
+			curr = head->target;
+			if (curr != target) curr->gdb_service = gdb_service;
+			head = head->next;	
+		}
+	}
+	return ret;
 }
 
 static int gdb_target_add_one(struct target *target)
 {
+	/*  one gdb instance per smp list */
+	if ((target->smp) && (target->gdb_service)) return ERROR_OK;
 	int retval = gdb_target_start(target, gdb_port_next);
-	if (retval == ERROR_OK)
+	if (retval == ERROR_OK) 
 	{
 		long portnumber;
 		/* If we can parse the port number
diff --git a/src/server/gdb_server.h b/src/server/gdb_server.h
index cb3962ff..e393fb72 100644
--- a/src/server/gdb_server.h
+++ b/src/server/gdb_server.h
@@ -30,15 +30,11 @@
 #define GDB_SERVER_H
 
 struct image;
+struct reg;
 #include <target/target.h>
 
 #define GDB_BUFFER_SIZE	16384
 
-struct gdb_service
-{
-	struct target *target;
-};
-
 int gdb_target_add_all(struct target *target);
 int gdb_register_commands(struct command_context *command_context);
 
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 5907e83f..1a2fbd6c 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -42,7 +42,8 @@ TARGET_CORE_SRC = \
 	breakpoints.c \
 	target.c \
 	target_request.c \
-	testee.c
+	testee.c \
+	smp.c
 
 ARMV4_5_SRC = \
 	armv4_5.c \
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index 917dfc78..80f98dc1 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -2,6 +2,9 @@
  *   Copyright (C) 2005 by Dominic Rath                                    *
  *   Dominic.Rath@gmx.de                                                   *
  *                                                                         *
+ *   Copyright (C) ST-Ericsson SA 2011                                     *
+ *   michel.jaouen@stericsson.com : smp minimum support                    *
+ *                                                                         *
  *   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     *
@@ -42,10 +45,11 @@ static char *watchpoint_rw_strings[] =
 // monotonic counter/id-number for breakpoints and watch points
 static int bpwp_unique_id;
 
-int breakpoint_add(struct target *target, uint32_t address, uint32_t length, enum breakpoint_type type)
+int breakpoint_add_internal(struct target *target, uint32_t address, uint32_t length, enum breakpoint_type type)
 {
 	struct breakpoint *breakpoint = target->breakpoints;
 	struct breakpoint **breakpoint_p = &target->breakpoints;
+	char *reason;
 	int retval;
 	int n;
 
@@ -76,9 +80,19 @@ int breakpoint_add(struct target *target, uint32_t address, uint32_t length, enu
 	(*breakpoint_p)->unique_id = bpwp_unique_id++;
 
 	retval = target_add_breakpoint(target, *breakpoint_p);
-	if (retval != ERROR_OK)
-	{
-		LOG_ERROR("could not add breakpoint");
+	switch (retval) {
+	case ERROR_OK:
+		break;
+	case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
+		reason = "resource not available";
+		goto fail;
+	case ERROR_TARGET_NOT_HALTED:
+		reason = "target running";
+		goto fail;
+	default:
+		reason = "unknown reason";
+fail:
+		LOG_ERROR("can't add breakpoint: %s", reason);
 		free((*breakpoint_p)->orig_instr);
 		free(*breakpoint_p);
 		*breakpoint_p = NULL;
@@ -93,6 +107,29 @@ int breakpoint_add(struct target *target, uint32_t address, uint32_t length, enu
 	return ERROR_OK;
 }
 
+int breakpoint_add(struct target *target, uint32_t address, uint32_t length, enum breakpoint_type type)
+{
+
+int retval = ERROR_OK;
+    if (target->smp)
+	{
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+		while(head != (struct target_list*)NULL)
+		{
+			curr = head->target;
+			retval = breakpoint_add_internal(curr, address,length, type);
+			if (retval != ERROR_OK) return retval;
+			head = head->next;	
+		}
+		return retval;
+	}
+	else
+	return(breakpoint_add_internal(target, address, length, type));
+
+}
+
 /* free up a breakpoint */
 static void breakpoint_free(struct target *target, struct breakpoint *breakpoint_to_remove)
 {
@@ -119,7 +156,7 @@ static void breakpoint_free(struct target *target, struct breakpoint *breakpoint
 	free(breakpoint);
 }
 
-void breakpoint_remove(struct target *target, uint32_t address)
+void breakpoint_remove_internal(struct target *target, uint32_t address)
 {
 	struct breakpoint *breakpoint = target->breakpoints;
 	struct breakpoint **breakpoint_p = &target->breakpoints;
@@ -141,8 +178,24 @@ void breakpoint_remove(struct target *target, uint32_t address)
 		LOG_ERROR("no breakpoint at address 0x%8.8" PRIx32 " found", address);
 	}
 }
+void breakpoint_remove(struct target *target, uint32_t address)
+{
+    if ((target->smp))
+	{
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+		while(head != (struct target_list*)NULL)
+		{
+			curr = head->target;
+			breakpoint_remove_internal(curr, address);
+			head = head->next;	
+		}
+	}
+	else  breakpoint_remove_internal(target, address);
+}
 
-void breakpoint_clear_target(struct target *target)
+void breakpoint_clear_target_internal(struct target *target)
 {
 	struct breakpoint *breakpoint;
 
@@ -154,6 +207,25 @@ void breakpoint_clear_target(struct target *target)
 	}
 }
 
+void breakpoint_clear_target(struct target *target)
+{
+    if (target->smp)
+	{
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+		while(head != (struct target_list*)NULL)
+		{
+			curr = head->target;
+		    breakpoint_clear_target_internal(curr);
+			head = head->next;	
+		}
+     }
+	 else breakpoint_clear_target_internal(target);
+	
+}
+
+
 struct breakpoint* breakpoint_find(struct target *target, uint32_t address)
 {
 	struct breakpoint *breakpoint = target->breakpoints;
@@ -174,6 +246,7 @@ int watchpoint_add(struct target *target, uint32_t address, uint32_t length,
 	struct watchpoint *watchpoint = target->watchpoints;
 	struct watchpoint **watchpoint_p = &target->watchpoints;
 	int retval;
+	char *reason;
 
 	while (watchpoint)
 	{
@@ -204,11 +277,21 @@ int watchpoint_add(struct target *target, uint32_t address, uint32_t length,
 	(*watchpoint_p)->unique_id = bpwp_unique_id++;
 
 	retval = target_add_watchpoint(target, *watchpoint_p);
-	if (retval != ERROR_OK)
-	{
-		LOG_ERROR("can't add %s watchpoint at 0x%8.8" PRIx32,
+	switch (retval) {
+	case ERROR_OK:
+		break;
+	case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
+		reason = "resource not available";
+		goto bye;
+	case ERROR_TARGET_NOT_HALTED:
+		reason = "target running";
+		goto bye;
+	default:
+		reason = "unrecognized error";
+bye:
+		LOG_ERROR("can't add %s watchpoint at 0x%8.8" PRIx32 ", %s",
 			 watchpoint_rw_strings[(*watchpoint_p)->rw],
-			 address);
+			 address, reason);
 		free (*watchpoint_p);
 		*watchpoint_p = NULL;
 		return retval;
diff --git a/src/target/smp.c b/src/target/smp.c
new file mode 100644
index 00000000..aabfa5b8
--- /dev/null
+++ b/src/target/smp.c
@@ -0,0 +1,116 @@
+/***************************************************************************
+ *                                                                         *
+ * Copyright (C) ST-Ericsson SA 2011                                       *
+ * Author: Michel Jaouen <michel.jaouen@stericsson.com> for ST-Ericsson.   *
+ *   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 "server/server.h"
+#include <helper/types.h>
+
+#include "target/target.h"
+
+#include "server/gdb_server.h"
+#include "smp.h"
+
+
+/*  implementation of new packet in gdb interface for smp feature          */
+/*                                                                         */
+/*   j : smp  status request                                               */
+/*   J : smp  set request                                                  */
+/*                                                                         */
+/*   jc :read core id displayed by gdb connection                          */
+/*   reply XXXXXXXX core id is int32_t , 8 hex digits                      */
+/*                                                                         */
+/*   Reply ENN error not supported (target not smp)                        */
+/*                                                                         */
+/*   JcXX  set core id displayed at next gdb continue                      */
+/*   maximum 8 bytes described core id int32_t (8 hex digits)              */
+/*  (core id -1 , reserved for returning to normal continue mode) */
+/*  Reply ENN error not supported(target not smp,core id out of range)     */
+/*  Reply OK : for success                                                 */
+/*                                                                         */
+/*  handling of this packet within gdb can be done by the creation         */
+/*  internal variable by mean of function allocate_computed_value          */
+/*  set $_core 1 => Jc01 packet is sent                                    */
+/*  print $_core => jc packet is sent and result is affected in $          */
+/*  Another way to test this packet is the usage of maintenance packet     */
+/*  maint packet Jc01                                                      */
+/*  maint packet jc                                                        */
+                                                
+static const char DIGITS[16] = "0123456789abcdef";
+
+
+/* packet j :smp status request */
+int gdb_read_smp_packet(struct connection *connection,
+		struct target *target, char *packet, int packet_size)
+{
+	uint32_t len = sizeof(int32_t);
+	uint8_t *buffer;
+	char *hex_buffer;
+	int retval = ERROR_OK;
+	if (target->smp)
+	{
+		if (strstr(packet, "jc"))
+		{
+			hex_buffer = malloc(len * 2 + 1);
+			buffer = (uint8_t *)&target->gdb_service->core[0];
+			uint32_t i;
+			for (i = 0; i < 4; i++)
+			{
+				uint8_t 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_put_packet(connection,"E01",3);
+	return retval;
+}
+
+/* J :  smp set request */
+int gdb_write_smp_packet(struct connection *connection,
+		struct target *target, char *packet, int packet_size)
+{
+	char *separator;
+	int coreid = 0;
+    int retval = ERROR_OK;
+
+	/* skip command character */
+	if (target->smp)
+	{
+		if (strstr(packet, "Jc"))
+		{
+			packet+=2;
+			coreid = strtoul(packet, &separator, 16);
+			target->gdb_service->core[1] = coreid;
+			gdb_put_packet(connection, "OK", 2);
+		}
+	}
+	else
+	{
+		retval = gdb_put_packet(connection,"E01",3);
+	}
+	return ERROR_OK;
+}
diff --git a/src/target/smp.h b/src/target/smp.h
new file mode 100644
index 00000000..f85c9a4a
--- /dev/null
+++ b/src/target/smp.h
@@ -0,0 +1,25 @@
+/***************************************************************************
+ *                                                                         *
+ * Copyright (C) ST-Ericsson SA 2011                                       *
+ * Author: Michel Jaouen <michel.jaouen@stericsson.com> for ST-Ericsson.   *
+ *   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/server.h"
+int gdb_read_smp_packet(struct connection *connection,
+		struct target *target, char *packet, int packet_size);
+int gdb_write_smp_packet(struct connection *connection,
+		struct target *target, char *packet, int packet_size);
+
diff --git a/src/target/target.c b/src/target/target.c
index abe1b43a..a2e3ccfb 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -17,6 +17,9 @@
  *   Copyright (C) 2011 by Broadcom Corporation                            *
  *   Evan Hunter - ehunter@broadcom.com                                    *
  *                                                                         *
+ *   Copyright (C) ST-Ericsson SA 2011                                     *
+ *   michel.jaouen@stericsson.com : smp minimum support                    *
+ *                                                                         *
  *   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     *
@@ -729,7 +732,7 @@ int target_bulk_write_memory(struct target *target,
 int target_add_breakpoint(struct target *target,
 		struct breakpoint *breakpoint)
 {
-	if (target->state != TARGET_HALTED) {
+	if ((target->state != TARGET_HALTED)&&(breakpoint->type!=BKPT_HARD)) {
 		LOG_WARNING("target %s is not halted", target->cmd_name);
 		return ERROR_TARGET_NOT_HALTED;
 	}
@@ -3931,6 +3934,7 @@ static int target_configure(Jim_GetOptInfo *goi, struct target *target)
 			/* loop for more e*/
 			break;
 
+
 		case TCFG_ENDIAN:
 			if (goi->isconfigure) {
 				e = Jim_GetOpt_Nvp(goi, nvp_target_endian, &n);
@@ -3981,7 +3985,7 @@ static int target_configure(Jim_GetOptInfo *goi, struct target *target)
 				if (e != JIM_OK) {
 					return e;
 				}
-				target->coreid = (int)w;
+				target->coreid = (int32_t)w;
 			} else {
 				if (goi->argc != 0) {
 					goto no_params;
@@ -4893,6 +4897,61 @@ static int jim_target_names(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 	return JIM_OK;
 }
 
+static int jim_target_smp(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
+{
+	int i;
+	const char *targetname;
+	int retval,len;
+	struct target *target;
+	struct target_list *head, *curr, *new;
+    curr = (struct target_list*) NULL;
+	head = (struct target_list*) NULL;
+	new = (struct target_list*) NULL;
+
+	retval = 0;
+	LOG_DEBUG("%d",argc);
+	/* argv[1] = target to associate in smp
+	 * argv[2] = target to assoicate in smp 
+	 * argv[3] ...
+	 */
+
+	for(i=1;i<argc;i++)
+	{
+
+		targetname = Jim_GetString(argv[i], &len);
+		target = get_target(targetname);
+		LOG_DEBUG("%s ",targetname);
+		if (target)
+		{
+			new=malloc(sizeof(struct target_list));
+			new->target = target;
+			new->next = (struct target_list*)NULL;
+			if (head == (struct target_list*)NULL)
+			{
+				head = new;
+				curr = head;
+			}
+			else
+			{
+				curr->next = new;
+				curr = new;
+			}
+		}
+	}
+    /*  now parse the list of cpu and put the target in smp mode*/
+	curr=head;
+
+    while(curr!=(struct target_list *)NULL)
+	{
+    target=curr->target;
+	target->smp = 1;
+	target->head = head;
+	curr=curr->next;
+	}
+	return retval; 
+}
+
+
 static int jim_target_create(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 {
 	Jim_GetOptInfo goi;
@@ -5008,6 +5067,14 @@ static const struct command_registration target_subcommand_handlers[] = {
 		.help = "Returns the number of targets as an integer "
 			"(DEPRECATED)",
 	},
+	{
+		.name = "smp",
+		.mode = COMMAND_ANY,
+		.jim_handler = jim_target_smp,
+		.usage = "targetname1 targetname2 ...",
+		.help = "gather several target in a smp list"
+	},
+
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/target.h b/src/target/target.h
index 5b67bf34..74f74dee 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -11,6 +11,9 @@
  *   Copyright (C) 2011 by Broadcom Corporation                            *
  *   Evan Hunter - ehunter@broadcom.com                                    *
  *                                                                         *
+ *   Copyright (C) ST-Ericsson SA 2011                                     *
+ *   michel.jaouen@stericsson.com : smp minimum support                    *
+ *                                                                         *
  *   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     *
@@ -38,7 +41,7 @@ struct breakpoint;
 struct watchpoint;
 struct mem_param;
 struct reg_param;
-
+struct target_list;
 
 /*
  * TARGET_UNKNOWN = 0: we don't know anything about the target yet
@@ -102,6 +105,17 @@ struct working_area
 	struct working_area **user;
 	struct working_area *next;
 };
+ 
+struct gdb_service
+{
+	struct target *target;
+	/*  field for smp display  */
+	/*  element 0 coreid currently displayed ( 1 till n) */
+    /*  element 1 coreid to be displayed at next resume 1 till n 0 means resume
+	 *  all cores
+	  core displayed  */
+	int32_t core[2];
+};
 
 // target_type.h contains the full definitionof struct targe_type
 struct target
@@ -110,7 +124,7 @@ struct target
 	const char *cmd_name;				/* tcl Name of target */
 	int target_number;					/* DO NOT USE!  field to be removed in 2010 */
 	struct jtag_tap *tap;					/* where on the jtag chain is this */
-	int coreid;							/* which device on the TAP? */
+	int32_t coreid;							/* which device on the TAP? */
 	const char *variant;				/* what variant of this chip is it? */
 
 	/**
@@ -166,6 +180,20 @@ struct target
 	struct rtos *rtos;					/* Instance of Real Time Operating System support */
 	bool rtos_auto_detect;				/* A flag that indicates that the RTOS has been specified as "auto" 
 	                                     * and must be detected when symbols are offered */
+
+	int smp;								/*  add some target attributes for smp support */
+	struct target_list *head;
+	/*  the gdb service is there in case of smp , we have only one gdb server
+	 *  for all smp target
+	 *  the target attached to the gdb is changing dynamically by changing
+	 *  gdb_service->target pointer */
+	struct gdb_service *gdb_service;
+};
+
+
+struct target_list {
+	struct target *target;
+	struct target_list *next;
 };
 
 /** Returns the instance-specific name of the specified target. */