/* 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;
}
/** */
-static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
+static int stlink_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
{
int res;
struct stlink_usb_handle_s *h = handle;
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;
h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
else
h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
- h->cmdbuf[h->cmdidx++] = num;
+ h->cmdbuf[h->cmdidx++] = regsel;
if (h->version.jtag_api == STLINK_JTAG_API_V1) {
res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
}
/** */
-static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
+static int stlink_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
{
struct stlink_usb_handle_s *h = handle;
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;
h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
else
h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
- h->cmdbuf[h->cmdidx++] = num;
+ h->cmdbuf[h->cmdidx++] = regsel;
h_u32_to_le(h->cmdbuf+h->cmdidx, val);
h->cmdidx += 4;