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 <borneo.antonio@gmail.com>
Reviewed-on: http://openocd.zylin.com/5883
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 1d38103a2f3cfc289fd75ef05e4277026c14af3d..4545bcba02089ca65be3d71a0dc39df1f6188084 100644 (file)
--- 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;