From: Antonio Borneo <borneo.antonio@gmail.com>
Date: Thu, 28 Feb 2019 09:28:26 +0000 (+0100)
Subject: stlink: fix handling of DPv1 and DPv2 banked registers
X-Git-Tag: v0.11.0-rc1~518
X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=commitdiff_plain;h=d2308da6e9adc21acd8428afec770670e57bea25

stlink: fix handling of DPv1 and DPv2 banked registers

Arm DPv1 and DPv2 support banked registers by setting the bank in
field DPBANKSEL of register DP_SELECT.
Old ST-Link firmware don't support banked registers and setting a
bank other than bank zero on DPv1 or DPv2 cause issues in the
firmware because it cannot set back bank zero to read CTRL/STAT.
New ST-Link firmware mask away DPBANKSEL bits while writing in
DP_SELECT but support banked register using the same packed method
used by OpenOCD:
	#define BANK_REG(bank, reg) (((bank) << 4) | (reg))

Add a new macro STLINK_F_HAS_DPBANKSEL for firmware that support
arm DPv1 and DPv2, plus trigger an error if banked registers are
requested on old firmware.
Prevent changing DPBANKSEL on old firmware.
Log a debug message when changing DPBANKSEL will be ignored.

Change-Id: Iaa592517831d63f8da2290db54f6b32504e3081b
Signed-off-by: Antonio Borneo <borneo.antonio@gmail.com>
Reviewed-on: http://openocd.zylin.com/4978
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index e121eb42b3..5c904d4ffb 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -316,6 +316,7 @@ enum stlink_mode {
 #define STLINK_F_HAS_DAP_REG            BIT(5)
 #define STLINK_F_QUIRK_JTAG_DP_READ     BIT(6)
 #define STLINK_F_HAS_AP_INIT            BIT(7)
+#define STLINK_F_HAS_DPBANKSEL          BIT(8)
 
 /* aliases */
 #define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
@@ -1021,6 +1022,10 @@ static int stlink_usb_version(void *handle)
 		if (h->version.jtag >= 28)
 			flags |= STLINK_F_HAS_AP_INIT;
 
+		/* Banked regs (DPv1 & DPv2) support from V2J32 */
+		if (h->version.jtag >= 32)
+			flags |= STLINK_F_HAS_DPBANKSEL;
+
 		break;
 	case 3:
 		/* all STLINK-V3 use api-v3 */
@@ -1044,6 +1049,10 @@ static int stlink_usb_version(void *handle)
 		/* API required to init AP before any AP access */
 		flags |= STLINK_F_HAS_AP_INIT;
 
+		/* Banked regs (DPv1 & DPv2) support from V3J2 */
+		if (h->version.jtag >= 2)
+			flags |= STLINK_F_HAS_DPBANKSEL;
+
 		break;
 	default:
 		break;
@@ -3259,6 +3268,12 @@ static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 	uint32_t dummy;
 	int retval;
 
+	if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
+		if (reg & 0x000000F0) {
+			LOG_ERROR("Banked DP registers not supported in current STLink FW");
+			return ERROR_COMMAND_NOTFOUND;
+		}
+
 	retval = stlink_dap_check_reconnect(dap);
 	if (retval != ERROR_OK)
 		return retval;
@@ -3286,6 +3301,18 @@ static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 {
 	int retval;
 
+	if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
+		if (reg & 0x000000F0) {
+			LOG_ERROR("Banked DP registers not supported in current STLink FW");
+			return ERROR_COMMAND_NOTFOUND;
+		}
+
+	if (reg == DP_SELECT && (data & DP_SELECT_DPBANK) != 0) {
+		/* ignored if STLINK_F_HAS_DPBANKSEL, not properly managed otherwise */
+		LOG_DEBUG("Ignoring DPBANKSEL while write SELECT");
+		data &= ~DP_SELECT_DPBANK;
+	}
+
 	retval = stlink_dap_check_reconnect(dap);
 	if (retval != ERROR_OK)
 		return retval;