target/arm_adi_v5,arm_dap: introduce multidrop_targetsel and its configuration

Add multidrop_targetsel to struct adiv5_dap.
Add option -dp-id and -instance-id to dap create command.
Add convenience function dap_is_multidrop()

Change-Id: Ibb93abb5f50b3665c320a10c1497421035762134
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: https://review.openocd.org/c/openocd/+/6140
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

3 files changed, 79 insertions, 1 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index a6da167..85dc432 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -4404,6 +4404,20 @@ A DAP may also provide optional @var{configparams}:
 register during initial examination and when checking the sticky error bit.
 This bit is normally checked after setting the CSYSPWRUPREQ bit, but some
 devices do not set the ack bit until sometime later.
+
+@item @code{-dp-id} @var{number}
+@*Debug port identification number for SWD DPv2 multidrop.
+The @var{number} is written to bits 0..27 of DP TARGETSEL during DP selection.
+To find the id number of a single connected device read DP TARGETID:
+@code{device.dap dpreg 0x24}
+Use bits 0..27 of TARGETID.
+
+@item @code{-instance-id} @var{number}
+@*Instance identification number for SWD DPv2 multidrop.
+The @var{number} is written to bits 28..31 of DP TARGETSEL during DP selection.
+To find the instance number of a single connected device read DP DLPIDR:
+@code{device.dap dpreg 0x34}
+The instance number is in bits 28..31 of DLPIDR value.
 @end itemize
 @end deffn
 
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index fa0a78a..1c96dc8 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -97,6 +97,11 @@
 #define DP_APSEL_MAX        (255)
 #define DP_APSEL_INVALID    (-1)
 
+#define DP_TARGETSEL_INVALID 0xFFFFFFFFU
+#define DP_TARGETSEL_DPID_MASK 0x0FFFFFFFU
+#define DP_TARGETSEL_INSTANCEID_MASK 0xF0000000U
+#define DP_TARGETSEL_INSTANCEID_SHIFT 28
+
 
 /* MEM-AP register addresses */
 #define MEM_AP_REG_CSW		0x00
@@ -324,6 +329,13 @@ struct adiv5_dap {
 	/** Flag saying whether to ignore the syspwrupack flag in DAP. Some devices
 	 *  do not set this bit until later in the bringup sequence */
 	bool ignore_syspwrupack;
+
+	/** Value to select DP in SWD multidrop mode or DP_TARGETSEL_INVALID */
+	uint32_t multidrop_targetsel;
+	/** TPARTNO and TDESIGNER fields of multidrop_targetsel have been configured */
+	bool multidrop_dp_id_valid;
+	/** TINSTANCE field of multidrop_targetsel has been configured */
+	bool multidrop_instance_id_valid;
 };
 
 /**
@@ -610,6 +622,12 @@ static inline struct adiv5_ap *dap_ap(struct adiv5_dap *dap, uint8_t ap_num)
 	return &dap->ap[ap_num];
 }
 
+/** Check if SWD multidrop configuration is valid */
+static inline bool dap_is_multidrop(struct adiv5_dap *dap)
+{
+	return dap->multidrop_dp_id_valid && dap->multidrop_instance_id_valid;
+}
+
 /* Lookup CoreSight component */
 int dap_lookup_cs_component(struct adiv5_ap *ap,
 			target_addr_t dbgbase, uint8_t type, target_addr_t *addr, int32_t *idx);
diff --git a/src/target/arm_dap.c b/src/target/arm_dap.c
index 0eb55a9..bde1dfb 100644
--- a/src/target/arm_dap.c
+++ b/src/target/arm_dap.c
@@ -155,11 +155,15 @@ int dap_cleanup_all(void)
 enum dap_cfg_param {
 	CFG_CHAIN_POSITION,
 	CFG_IGNORE_SYSPWRUPACK,
+	CFG_DP_ID,
+	CFG_INSTANCE_ID,
 };
 
 static const struct jim_nvp nvp_config_opts[] = {
-	{ .name = "-chain-position",   .value = CFG_CHAIN_POSITION },
+	{ .name = "-chain-position",     .value = CFG_CHAIN_POSITION },
 	{ .name = "-ignore-syspwrupack", .value = CFG_IGNORE_SYSPWRUPACK },
+	{ .name = "-dp-id",              .value = CFG_DP_ID },
+	{ .name = "-instance-id",        .value = CFG_INSTANCE_ID },
 	{ .name = NULL, .value = -1 }
 };
 
@@ -197,6 +201,48 @@ static int dap_configure(struct jim_getopt_info *goi, struct arm_dap_object *dap
 		case CFG_IGNORE_SYSPWRUPACK:
 			dap->dap.ignore_syspwrupack = true;
 			break;
+		case CFG_DP_ID: {
+			jim_wide w;
+			e = jim_getopt_wide(goi, &w);
+			if (e != JIM_OK) {
+				Jim_SetResultFormatted(goi->interp,
+						"create %s: bad parameter %s",
+						dap->name, n->name);
+				return JIM_ERR;
+			}
+			if (w < 0 || w > DP_TARGETSEL_DPID_MASK) {
+				Jim_SetResultFormatted(goi->interp,
+						"create %s: %s out of range",
+						dap->name, n->name);
+				return JIM_ERR;
+			}
+			dap->dap.multidrop_targetsel =
+				(dap->dap.multidrop_targetsel & DP_TARGETSEL_INSTANCEID_MASK)
+				| (w & DP_TARGETSEL_DPID_MASK);
+			dap->dap.multidrop_dp_id_valid = true;
+			break;
+		}
+		case CFG_INSTANCE_ID: {
+			jim_wide w;
+			e = jim_getopt_wide(goi, &w);
+			if (e != JIM_OK) {
+				Jim_SetResultFormatted(goi->interp,
+						"create %s: bad parameter %s",
+						dap->name, n->name);
+				return JIM_ERR;
+			}
+			if (w < 0 || w > 15) {
+				Jim_SetResultFormatted(goi->interp,
+						"create %s: %s out of range",
+						dap->name, n->name);
+				return JIM_ERR;
+			}
+			dap->dap.multidrop_targetsel =
+				(dap->dap.multidrop_targetsel & DP_TARGETSEL_DPID_MASK)
+				| ((w << DP_TARGETSEL_INSTANCEID_SHIFT) & DP_TARGETSEL_INSTANCEID_MASK);
+			dap->dap.multidrop_instance_id_valid = true;
+			break;
+		}
 		default:
 			break;
 		}