X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fjtag%2Fdrivers%2Fstlink_usb.c;fp=src%2Fjtag%2Fdrivers%2Fstlink_usb.c;h=4545bcba02089ca65be3d71a0dc39df1f6188084;hp=1d38103a2f3cfc289fd75ef05e4277026c14af3d;hb=4d336e8ffb7507f893640d344326b46f4bd8762d;hpb=be631eed16bd9ef2b735fd64650ca7283d2ca361

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 1d38103a2f..4545bcba02 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -324,6 +324,9 @@ struct stlink_usb_handle_s {
 
 /* aliases */
 #define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
+#define STLINK_F_HAS_FPU_REG            STLINK_F_HAS_GETLASTRWSTATUS2
+
+#define STLINK_REGSEL_IS_FPU(x)         ((x) > 0x1F)
 
 struct speed_map {
 	int speed;
@@ -2023,6 +2026,15 @@ static int stlink_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
 
 	assert(handle != NULL);
 
+	if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
+		res = stlink_usb_write_debug_reg(h, DCB_DCRSR, regsel & 0x7f);
+		if (res != ERROR_OK)
+			return res;
+
+		/* FIXME: poll DHCSR.S_REGRDY before read DCRDR */
+		return stlink_usb_v2_read_debug_reg(h, DCB_DCRDR, val);
+	}
+
 	stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -2054,6 +2066,15 @@ static int stlink_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
 
 	assert(handle != NULL);
 
+	if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
+		int res = stlink_usb_write_debug_reg(h, DCB_DCRDR, val);
+		if (res != ERROR_OK)
+			return res;
+
+		return stlink_usb_write_debug_reg(h, DCB_DCRSR, DCRSR_WnR | (regsel & 0x7f));
+		/* FIXME: poll DHCSR.S_REGRDY after write DCRSR */
+	}
+
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;