From 4d336e8ffb7507f893640d344326b46f4bd8762d Mon Sep 17 00:00:00 2001 From: Antonio Borneo Date: Sun, 25 Oct 2020 17:35:45 +0100 Subject: [PATCH] stlink: handle read/write FPU registers in HLA API Old stlink firmware in stlink V1 and stlink V2 pre-J15 do not handle FPU registers in the read_reg() and write_reg() API. Add code to be compatible with the new API of OpenOCD. Change-Id: Ib0439c5294b6911ea75efe8c7fa085b014317a4b Signed-off-by: Antonio Borneo Reviewed-on: http://openocd.zylin.com/5883 Tested-by: jenkins Reviewed-by: Tomas Vanek --- src/jtag/drivers/stlink_usb.c | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) 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; -- 2.30.2