+/** */
+static int stlink_configure_target_trace_port(void *handle)
+{
+ int res;
+ uint32_t reg;
+ struct stlink_usb_handle_s *h;
+
+ assert(handle != NULL);
+
+ h = (struct stlink_usb_handle_s *)handle;
+
+ /* configure the TPI */
+
+ /* enable the trace subsystem */
+ res = stlink_usb_v2_read_debug_reg(handle, DCB_DEMCR, ®);
+ if (res != ERROR_OK)
+ goto out;
+ res = stlink_usb_write_debug_reg(handle, DCB_DEMCR, TRCENA|reg);
+ if (res != ERROR_OK)
+ goto out;
+ /* set the TPI clock prescaler */
+ res = stlink_usb_write_debug_reg(handle, TPI_ACPR, h->trace.prescale);
+ if (res != ERROR_OK)
+ goto out;
+ /* select the pin protocol. The STLinkv2 only supports asynchronous
+ * UART emulation (NRZ) mode, so that's what we pick. */
+ res = stlink_usb_write_debug_reg(handle, TPI_SPPR, 0b10);
+ if (res != ERROR_OK)
+ goto out;
+ /* disable continuous formatting */
+ res = stlink_usb_write_debug_reg(handle, TPI_FFCR, (1<<8));
+ if (res != ERROR_OK)
+ goto out;
+
+ /* configure the ITM */
+
+ /* unlock access to the ITM registers */
+ res = stlink_usb_write_debug_reg(handle, ITM_LAR, 0xC5ACCE55);
+ if (res != ERROR_OK)
+ goto out;
+ /* enable trace with ATB ID 1 */
+ res = stlink_usb_write_debug_reg(handle, ITM_TCR, (1<<16)|(1<<0)|(1<<2));
+ if (res != ERROR_OK)
+ goto out;
+ /* trace privilege */
+ res = stlink_usb_write_debug_reg(handle, ITM_TPR, 1);
+ if (res != ERROR_OK)
+ goto out;
+ /* trace port enable (port 0) */
+ res = stlink_usb_write_debug_reg(handle, ITM_TER, (1<<0));
+ if (res != ERROR_OK)
+ goto out;
+
+ res = ERROR_OK;
+out:
+ return res;
+}
+
+/** */
+static void stlink_usb_trace_disable(void *handle)
+{
+ int res = ERROR_OK;
+ struct stlink_usb_handle_s *h;
+
+ assert(handle != NULL);
+
+ h = (struct stlink_usb_handle_s *)handle;
+
+ assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION);
+
+ LOG_DEBUG("Tracing: disable\n");
+
+ stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
+ res = stlink_usb_xfer(handle, h->databuf, 2);
+
+ if (res == ERROR_OK)
+ h->trace.enabled = false;
+}
+
+
+/** */
+static int stlink_usb_trace_enable(void *handle)
+{
+ int res;
+ struct stlink_usb_handle_s *h;
+
+ assert(handle != NULL);
+
+ h = (struct stlink_usb_handle_s *)handle;
+
+ if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
+ uint32_t trace_hz;
+
+ res = stlink_configure_target_trace_port(handle);
+ if (res != ERROR_OK)
+ LOG_ERROR("Unable to configure tracing on target\n");
+
+ trace_hz = h->trace.prescale > 0 ?
+ h->trace.source_hz / (h->trace.prescale + 1) :
+ h->trace.source_hz;
+
+ stlink_usb_init_buffer(handle, STLINK_RX_EP, 10);
+
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
+ h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
+ h->cmdidx += 2;
+ h_u32_to_le(h->cmdbuf+h->cmdidx, trace_hz);
+ h->cmdidx += 4;
+
+ res = stlink_usb_xfer(handle, h->databuf, 2);
+
+ if (res == ERROR_OK) {
+ h->trace.enabled = true;
+ LOG_DEBUG("Tracing: recording at %uHz\n", trace_hz);
+ }
+ } else {
+ LOG_ERROR("Tracing is not supported by this version.");
+ res = ERROR_FAIL;
+ }
+
+ return res;
+}
+