cortex_m: allow setting debug ap during create

This patch adds a Cortex-M private configuration option
that allows setting the acess point during target
creation. This circumvents situations in hybrid systems
when the correct access point can not be automatically
detected.

Change-Id: If313a5250e6e66509bb9080f3498feab7781dced
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3639
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>

6 files changed, 66 insertions, 5 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 7c2152b1..b47d2c44 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -4152,6 +4152,10 @@ The value should normally correspond to a static mapping for the
 scan and after a reset. A manual call to arp_examine is required to
 access the target for debugging.
 
+@item @code{-ap-num} @var{ap_number} -- set DAP access port for target,
+@var{ap_number} is the numeric index of the DAP AP the target is connected to.
+Use this option with systems where multiple, independent cores are connected
+to separate access ports of the same DAP.
 @end itemize
 @end deffn
 
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 39f2b399..eafc2ddc 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -1359,6 +1359,41 @@ static int dap_info_command(struct command_context *cmd_ctx,
 	return ERROR_OK;
 }
 
+int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
+{
+	struct adiv5_private_config *pc;
+	const char *arg;
+	jim_wide ap_num;
+	int e;
+
+	/* check if argv[0] is for us */
+	arg = Jim_GetString(goi->argv[0], NULL);
+	if (strcmp(arg, "-ap-num"))
+		return JIM_CONTINUE;
+
+	e = Jim_GetOpt_String(goi, &arg, NULL);
+	if (e != JIM_OK)
+		return e;
+
+	if (goi->argc == 0) {
+		Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv, "-ap-num ?ap-number? ...");
+		return JIM_ERR;
+	}
+
+	e = Jim_GetOpt_Wide(goi, &ap_num);
+	if (e != JIM_OK)
+		return e;
+
+	if (target->private_config == NULL) {
+		pc = calloc(1, sizeof(struct adiv5_private_config));
+		target->private_config = pc;
+		pc->ap_num = ap_num;
+	}
+
+
+	return JIM_OK;
+}
+
 COMMAND_HANDLER(handle_dap_info_command)
 {
 	struct target *target = get_current_target(CMD_CTX);
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 3220d8b6..bf9cb5cc 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -504,4 +504,10 @@ int dap_to_jtag(struct target *target);
 
 extern const struct command_registration dap_command_handlers[];
 
+struct adiv5_private_config {
+	int ap_num;
+};
+
+extern int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi);
+
 #endif /* OPENOCD_TARGET_ARM_ADI_V5_H */
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index f674cc52..36a77467 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1709,6 +1709,7 @@ void cortex_m_deinit_target(struct target *target)
 	cortex_m_dwt_free(target);
 	armv7m_free_reg_cache(target);
 
+	free(target->private_config);
 	free(cortex_m);
 }
 
@@ -1912,11 +1913,15 @@ int cortex_m_examine(struct target *target)
 			return retval;
 		}
 
-		/* Search for the MEM-AP */
-		retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7m->debug_ap);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("Could not find MEM-AP to control the core");
-			return retval;
+		if (cortex_m->apsel < 0) {
+			/* Search for the MEM-AP */
+			retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7m->debug_ap);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Could not find MEM-AP to control the core");
+				return retval;
+			}
+		} else {
+			armv7m->debug_ap = dap_ap(swjdp, cortex_m->apsel);
 		}
 
 		/* Leave (only) generic DAP stuff for debugport_init(); */
@@ -2180,6 +2185,13 @@ static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
 	cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
 	cortex_m_init_arch_info(target, cortex_m, target->tap);
 
+	if (target->private_config != NULL) {
+		struct adiv5_private_config *pc =
+				(struct adiv5_private_config *)target->private_config;
+		cortex_m->apsel = pc->ap_num;
+	} else
+		cortex_m->apsel = -1;
+
 	return ERROR_OK;
 }
 
@@ -2441,6 +2453,7 @@ struct target_type cortexm_target = {
 
 	.commands = cortex_m_command_handlers,
 	.target_create = cortex_m_target_create,
+	.target_jim_configure = adiv5_jim_configure,
 	.init_target = cortex_m_init_target,
 	.examine = cortex_m_examine,
 	.deinit_target = cortex_m_deinit_target,
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index eabaac49..3d9714b9 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -188,6 +188,8 @@ struct cortex_m_common {
 	enum cortex_m_isrmasking_mode isrmasking_mode;
 
 	struct armv7m_common armv7m;
+
+	int apsel;
 };
 
 static inline struct cortex_m_common *
diff --git a/src/target/target.h b/src/target/target.h
index 7f6ac148..e86b700a 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -173,6 +173,7 @@ struct target {
 	struct debug_msg_receiver *dbgmsg;	/* list of debug message receivers */
 	uint32_t dbg_msg_enabled;			/* debug message status */
 	void *arch_info;					/* architecture specific information */
+	void *private_config;				/* pointer to target specific config data (for jim_configure hook) */
 	struct target *next;				/* next target in list */
 
 	int display;						/* display async info in telnet session. Do not display