rtos : smp support

Change-Id: I583cddf5e62ed77f108786a085569ab8699ad50d
Signed-off-by: Michel JAOUEN <michel.jaouen@stericsson.com>
Reviewed-on: http://openocd.zylin.com/344
Tested-by: jenkins
Reviewed-by: Evan Hunter <evan@ozhiker.com>
Reviewed-by: Øyvind Harboe <oyvindharboe@gmail.com>

4 files changed, 36 insertions, 17 deletions

diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 78a7e9d2..81502570 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -49,6 +49,13 @@ static struct rtos_type *rtos_types[] =
 
 int rtos_thread_packet(struct connection *connection, char *packet, int packet_size);
 
+int rtos_smp_init(struct target *target)
+{
+	if (target->rtos->type->smp_init)
+		return target->rtos->type->smp_init(target);
+	return ERROR_TARGET_INIT_FAILED;
+}
+
 
 int rtos_create(Jim_GetOptInfo *goi, struct target * target)
 {
@@ -437,10 +444,11 @@ int rtos_get_gdb_reg_list(struct connection *connection)
 {
 	struct target *target = get_target_from_connection(connection);
 	int64_t current_threadid = target->rtos->current_threadid;
-	if ( ( target->rtos != NULL ) &&
-		 ( current_threadid != -1 ) &&
-		 ( current_threadid != 0 ) &&
-		 ( current_threadid != target->rtos->current_thread ) )
+	if ((target->rtos != NULL) &&
+		 (current_threadid != -1) &&
+		 (current_threadid != 0) &&
+		 ((current_threadid != target->rtos->current_thread) ||
+		 (target->smp))) /* in smp several current thread are possible */
 	{
 		char * hex_reg_list;
 		target->rtos->type->get_thread_reg_list( target->rtos, current_threadid, &hex_reg_list );
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index df0812fc..890a99e0 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -75,9 +75,11 @@ struct rtos_type
 	char * name;
 	int (*detect_rtos)                 ( struct target* target );
 	int (*create)                      ( struct target* target );
+	int (*smp_init)(struct target *target);
 	int (*update_threads)              ( struct rtos*   rtos );
 	int (*get_thread_reg_list)         ( struct rtos *rtos, int64_t thread_id, char ** hex_reg_list );
 	int (*get_symbol_list_to_lookup)   (symbol_table_elem_t * symbol_list[] );
+	int (*clean)(struct target *target);
 };
 
 
@@ -105,5 +107,6 @@ int rtos_try_next( struct target * target );
 int gdb_thread_packet(struct connection *connection, char *packet, int packet_size);
 int rtos_get_gdb_reg_list(struct connection *connection);
 int rtos_update_threads( struct target *target );
+int rtos_smp_init(struct target *target);
 
 #endif // RTOS_H
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 70915556..8d84a991 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -836,6 +836,11 @@ static int gdb_new_connection(struct connection *connection)
 	breakpoint_clear_target(gdb_service->target);
 	watchpoint_clear_target(gdb_service->target);
 
+	/* clean previous rtos session if supported*/
+	if ((gdb_service->target->rtos) &&
+			(gdb_service->target->rtos->type->clean))
+		gdb_service->target->rtos->type->clean(gdb_service->target);
+
 	/* remove the initial ACK from the incoming buffer */
 	if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
 		return retval;
@@ -869,9 +874,9 @@ static int gdb_new_connection(struct connection *connection)
 
 	gdb_actual_connections++;
 	LOG_DEBUG("New GDB Connection: %d, Target %s, state: %s",
-		  gdb_actual_connections,
-		  target_name(gdb_service->target),
-		  target_state_name(gdb_service->target));
+			gdb_actual_connections,
+			target_name(gdb_service->target),
+			target_state_name(gdb_service->target));
 
 	/* DANGER! If we fail subsequently, we must remove this handler,
 	 * otherwise we occasionally see crashes as the timer can invoke the
@@ -2256,6 +2261,7 @@ static int gdb_input_inner(struct connection *connection)
 				case 'c':
 				case 's':
 					{
+						gdb_thread_packet(connection, packet, packet_size);
 						log_add_callback(gdb_log_callback, connection);
 
 						if (gdb_con->mem_write_error)
diff --git a/src/target/target.c b/src/target/target.c
index c3d65e33..e53d6f84 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -5119,11 +5119,12 @@ 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;
-    curr = (struct target_list*) NULL;
-	head = (struct target_list*) NULL;
-	
+	struct target *target = (struct target *) NULL;
+	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
@@ -5139,7 +5140,6 @@ static int jim_target_smp(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 		LOG_DEBUG("%s ",targetname);
 		if (target)
 		{
-			struct target_list *new;
 			new=malloc(sizeof(struct target_list));
 			new->target = target;
 			new->next = (struct target_list*)NULL;
@@ -5160,11 +5160,13 @@ static int jim_target_smp(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 
     while(curr!=(struct target_list *)NULL)
 	{
-    target=curr->target;
-	target->smp = 1;
-	target->head = head;
-	curr=curr->next;
+		target = curr->target;
+		target->smp = 1;
+		target->head = head;
+		curr = curr->next;
 	}
+	if (target->rtos)
+		retval = rtos_smp_init(head->target);
 	return retval;
 }