stlink: handle read/write FPU registers in HLA API

[openocd.git] / src / jtag / drivers / stlink_usb.c

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 48db3e611e462a625515d64f3cc21b43a0fd2a43..4545bcba02089ca65be3d71a0dc39df1f6188084 100644 (file)
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -299,6 +299,7 @@ struct stlink_usb_handle_s {
 
 #define STLINK_TRACE_SIZE               4096
 #define STLINK_TRACE_MAX_HZ             2000000
+#define STLINK_V3_TRACE_MAX_HZ          24000000
 
 #define STLINK_V3_MAX_FREQ_NB               10
 
@@ -323,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;
@@ -2015,13 +2019,22 @@ static int stlink_usb_read_regs(void *handle)
 }
 
 /** */
-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;
@@ -2029,7 +2042,7 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
                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);
@@ -2047,12 +2060,21 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 }
 
 /** */
-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;
@@ -2060,7 +2082,7 @@ static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
                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;
 
@@ -2988,7 +3010,6 @@ static int stlink_config_trace(void *handle, bool enabled,
                uint16_t *prescaler)
 {
        struct stlink_usb_handle_s *h = handle;
-       uint16_t presc;
 
        if (enabled && (!(h->version.flags & STLINK_F_HAS_TRACE) ||
                        pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART)) {
@@ -2996,24 +3017,32 @@ static int stlink_config_trace(void *handle, bool enabled,
                return ERROR_FAIL;
        }
 
+       unsigned int max_trace_freq = (h->version.stlink == 3) ?
+                       STLINK_V3_TRACE_MAX_HZ : STLINK_TRACE_MAX_HZ;
+
        /* Only concern ourselves with the frequency if the STlink is processing it. */
-       if (enabled && *trace_freq > STLINK_TRACE_MAX_HZ) {
+       if (enabled && *trace_freq > max_trace_freq) {
                LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
-                         STLINK_TRACE_MAX_HZ);
+                         max_trace_freq);
                return ERROR_FAIL;
        }
 
        stlink_usb_trace_disable(h);
 
        if (!*trace_freq)
-               *trace_freq = STLINK_TRACE_MAX_HZ;
-
-       presc = traceclkin_freq / *trace_freq;
+               *trace_freq = max_trace_freq;
 
-       if (traceclkin_freq % *trace_freq > 0)
-               presc++;
+       unsigned int presc = (traceclkin_freq + *trace_freq / 2) / *trace_freq;
+       if (presc == 0 || presc > TPIU_ACPR_MAX_SWOSCALER + 1) {
+               LOG_ERROR("SWO frequency is not suitable. Please choose a different "
+                       "frequency.");
+               return ERROR_FAIL;
+       }
 
-       if (presc > TPIU_ACPR_MAX_SWOSCALER) {
+       /* Probe's UART speed must be within 3% of the TPIU's SWO baud rate. */
+       unsigned int max_deviation = (traceclkin_freq * 3) / 100;
+       if (presc * *trace_freq < traceclkin_freq - max_deviation ||
+                       presc * *trace_freq > traceclkin_freq + max_deviation) {
                LOG_ERROR("SWO frequency is not suitable. Please choose a different "
                        "frequency.");
                return ERROR_FAIL;