?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
ipdbg: fix double free of virtual-ir data
[openocd.git] / src / jtag / drivers / stlink_usb.c
blobb14fbf1f38ec5c87c6ab13a8f676bca58284e5af
1 // SPDX-License-Identifier: GPL-2.0-or-later
2
3 /***************************************************************************
4 * Copyright (C) 2020 by Tarek Bochkati *
5 * Tarek Bochkati <tarek.bouchkati@gmail.com> *
6 * *
7 * SWIM contributions by Ake Rehnman *
8 * Copyright (C) 2017 Ake Rehnman *
9 * ake.rehnman(at)gmail.com *
10 * *
11 * Copyright (C) 2011-2012 by Mathias Kuester *
12 * Mathias Kuester <kesmtp@freenet.de> *
13 * *
14 * Copyright (C) 2012 by Spencer Oliver *
15 * spen@spen-soft.co.uk *
16 * *
17 * This code is based on https://github.com/texane/stlink *
18 ***************************************************************************/
19
20 #ifdef HAVE_CONFIG_H
21 #include "config.h"
22 #endif
23
24 /* project specific includes */
25 #include <helper/align.h>
26 #include <helper/binarybuffer.h>
27 #include <helper/bits.h>
28 #include <helper/system.h>
29 #include <helper/time_support.h>
30 #include <jtag/adapter.h>
31 #include <jtag/interface.h>
32 #include <jtag/hla/hla_layout.h>
33 #include <jtag/hla/hla_transport.h>
34 #include <jtag/hla/hla_interface.h>
35 #include <jtag/swim.h>
36 #include <target/arm_adi_v5.h>
37 #include <target/target.h>
38 #include <transport/transport.h>
39
40 #include <target/cortex_m.h>
41
42 #include <helper/system.h>
43
44 #ifdef HAVE_ARPA_INET_H
45 #include <arpa/inet.h>
46 #endif
47
48 #ifdef HAVE_NETINET_TCP_H
49 #include <netinet/tcp.h>
50 #endif
51
52 #include "libusb_helper.h"
53
54 #ifdef HAVE_LIBUSB1
55 #define USE_LIBUSB_ASYNCIO
56 #endif
57
58 #define STLINK_SERIAL_LEN 24
59
60 #define ENDPOINT_IN 0x80
61 #define ENDPOINT_OUT 0x00
62
63 #define STLINK_WRITE_TIMEOUT (LIBUSB_TIMEOUT_MS)
64 #define STLINK_READ_TIMEOUT (LIBUSB_TIMEOUT_MS)
65
66 #define STLINK_RX_EP (1|ENDPOINT_IN)
67 #define STLINK_TX_EP (2|ENDPOINT_OUT)
68 #define STLINK_TRACE_EP (3|ENDPOINT_IN)
69
70 #define STLINK_V2_1_TX_EP (1|ENDPOINT_OUT)
71 #define STLINK_V2_1_TRACE_EP (2|ENDPOINT_IN)
72
73 #define STLINK_SG_SIZE (31)
74 #define STLINK_DATA_SIZE (6144)
75 #define STLINK_CMD_SIZE_V2 (16)
76 #define STLINK_CMD_SIZE_V1 (10)
77
78 #define STLINK_V1_PID (0x3744)
79 #define STLINK_V2_PID (0x3748)
80 #define STLINK_V2_1_PID (0x374B)
81 #define STLINK_V2_1_NO_MSD_PID (0x3752)
82 #define STLINK_V3_USBLOADER_PID (0x374D)
83 #define STLINK_V3E_PID (0x374E)
84 #define STLINK_V3S_PID (0x374F)
85 #define STLINK_V3_2VCP_PID (0x3753)
86 #define STLINK_V3E_NO_MSD_PID (0x3754)
87 #define STLINK_V3P_USBLOADER_PID (0x3755)
88 #define STLINK_V3P_PID (0x3757)
89
90 /*
91 * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and
92 * this limits the bulk packet size and the 8bit read/writes to max 64 bytes.
93 * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes from FW V3J6.
94 *
95 * For 16 and 32bit read/writes stlink handles USB packet split and the limit
96 * is the internal buffer size of 6144 bytes.
97 * TODO: override ADIv5 layer's tar_autoincr_block that limits the transfer
98 * to 1024 or 4096 bytes
99 */
100 #define STLINK_MAX_RW8 (64)
101 #define STLINKV3_MAX_RW8 (512)
102 #define STLINK_MAX_RW16_32 STLINK_DATA_SIZE
103 #define STLINK_SWIM_DATA_SIZE STLINK_DATA_SIZE
104
105 /* "WAIT" responses will be retried (with exponential backoff) at
106 * most this many times before failing to caller.
107 */
108 #define MAX_WAIT_RETRIES 8
109
110 /* HLA is currently limited at AP#0 and no control on CSW */
111 #define STLINK_HLA_AP_NUM 0
112 #define STLINK_HLA_CSW 0
113
114 enum stlink_jtag_api_version {
115 STLINK_JTAG_API_V1 = 1,
116 STLINK_JTAG_API_V2,
117 STLINK_JTAG_API_V3,
118 };
119
120 enum stlink_mode {
121 STLINK_MODE_UNKNOWN = 0,
122 STLINK_MODE_DFU,
123 STLINK_MODE_MASS,
124 STLINK_MODE_DEBUG_JTAG,
125 STLINK_MODE_DEBUG_SWD,
126 STLINK_MODE_DEBUG_SWIM
127 };
128
129 /** */
130 struct stlink_usb_version {
131 /** */
132 int stlink;
133 /** */
134 int jtag;
135 /** */
136 int swim;
137 /** jtag api version supported */
138 enum stlink_jtag_api_version jtag_api;
139 /** one bit for each feature supported. See macros STLINK_F_* */
140 uint32_t flags;
141 };
142
143 struct stlink_usb_priv_s {
144 /** */
145 struct libusb_device_handle *fd;
146 /** */
147 struct libusb_transfer *trans;
148 };
149
150 struct stlink_tcp_version {
151 uint32_t api;
152 uint32_t major;
153 uint32_t minor;
154 uint32_t build;
155 };
156
157 struct stlink_tcp_priv_s {
158 /** */
159 int fd;
160 /** */
161 bool connected;
162 /** */
163 uint32_t device_id;
164 /** */
165 uint32_t connect_id;
166 /** */
167 uint8_t *send_buf;
168 /** */
169 uint8_t *recv_buf;
170 /** */
171 struct stlink_tcp_version version;
172 };
173
174 struct stlink_backend_s {
175 /** */
176 int (*open)(void *handle, struct hl_interface_param_s *param);
177 /** */
178 int (*close)(void *handle);
179 /** */
180 int (*xfer_noerrcheck)(void *handle, const uint8_t *buf, int size);
181 /** */
182 int (*read_trace)(void *handle, const uint8_t *buf, int size);
183 };
184
185 /* TODO: make queue size dynamic */
186 /* TODO: don't allocate queue for HLA */
187 #define MAX_QUEUE_DEPTH (4096)
188
189 enum queue_cmd {
190 CMD_DP_READ = 1,
191 CMD_DP_WRITE,
192
193 CMD_AP_READ,
194 CMD_AP_WRITE,
195
196 /*
197 * encode the bytes size in the enum's value. This makes easy to extract it
198 * with a simple logic AND, by using the macro CMD_MEM_AP_2_SIZE() below
199 */
200 CMD_MEM_AP_READ8 = 0x10 + 1,
201 CMD_MEM_AP_READ16 = 0x10 + 2,
202 CMD_MEM_AP_READ32 = 0x10 + 4,
203
204 CMD_MEM_AP_WRITE8 = 0x20 + 1,
205 CMD_MEM_AP_WRITE16 = 0x20 + 2,
206 CMD_MEM_AP_WRITE32 = 0x20 + 4,
207 };
208
209 #define CMD_MEM_AP_2_SIZE(cmd) ((cmd) & 7)
210
211 struct dap_queue {
212 enum queue_cmd cmd;
213 union {
214 struct dp_r {
215 unsigned int reg;
216 struct adiv5_dap *dap;
217 uint32_t *p_data;
218 } dp_r;
219 struct dp_w {
220 unsigned int reg;
221 struct adiv5_dap *dap;
222 uint32_t data;
223 } dp_w;
224 struct ap_r {
225 unsigned int reg;
226 struct adiv5_ap *ap;
227 uint32_t *p_data;
228 } ap_r;
229 struct ap_w {
230 unsigned int reg;
231 struct adiv5_ap *ap;
232 uint32_t data;
233 bool changes_csw_default;
234 } ap_w;
235 struct mem_ap {
236 uint32_t addr;
237 struct adiv5_ap *ap;
238 union {
239 uint32_t *p_data;
240 uint32_t data;
241 };
242 uint32_t csw;
243 } mem_ap;
244 };
245 };
246
247 /** */
248 struct stlink_usb_handle_s {
249 /** */
250 struct stlink_backend_s *backend;
251 /** */
252 union {
253 struct stlink_usb_priv_s usb_backend_priv;
254 struct stlink_tcp_priv_s tcp_backend_priv;
255 };
256 /** */
257 uint8_t rx_ep;
258 /** */
259 uint8_t tx_ep;
260 /** */
261 uint8_t trace_ep;
262 /** */
263 uint8_t *cmdbuf;
264 /** */
265 uint8_t cmdidx;
266 /** */
267 uint8_t direction;
268 /** */
269 uint8_t *databuf;
270 /** */
271 uint32_t max_mem_packet;
272 /** */
273 enum stlink_mode st_mode;
274 /** */
275 struct stlink_usb_version version;
276 /** */
277 uint16_t vid;
278 /** */
279 uint16_t pid;
280 /** */
281 struct {
282 /** whether SWO tracing is enabled or not */
283 bool enabled;
284 /** trace module source clock */
285 uint32_t source_hz;
286 } trace;
287 /** reconnect is needed next time we try to query the
288 * status */
289 bool reconnect_pending;
290 /** queue of dap_direct operations */
291 struct dap_queue queue[MAX_QUEUE_DEPTH];
292 /** first element available in the queue */
293 unsigned int queue_index;
294 };
295
296 /** */
297 static inline int stlink_usb_open(void *handle, struct hl_interface_param_s *param)
298 {
299 struct stlink_usb_handle_s *h = handle;
300 return h->backend->open(handle, param);
301 }
302
303 /** */
304 static inline int stlink_usb_close(void *handle)
305 {
306 struct stlink_usb_handle_s *h = handle;
307 return h->backend->close(handle);
308 }
309 /** */
310 static inline int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
311 {
312 struct stlink_usb_handle_s *h = handle;
313 return h->backend->xfer_noerrcheck(handle, buf, size);
314 }
315
316 #define STLINK_SWIM_ERR_OK 0x00
317 #define STLINK_SWIM_BUSY 0x01
318 #define STLINK_DEBUG_ERR_OK 0x80
319 #define STLINK_DEBUG_ERR_FAULT 0x81
320 #define STLINK_SWD_AP_WAIT 0x10
321 #define STLINK_SWD_AP_FAULT 0x11
322 #define STLINK_SWD_AP_ERROR 0x12
323 #define STLINK_SWD_AP_PARITY_ERROR 0x13
324 #define STLINK_JTAG_GET_IDCODE_ERROR 0x09
325 #define STLINK_JTAG_WRITE_ERROR 0x0c
326 #define STLINK_JTAG_WRITE_VERIF_ERROR 0x0d
327 #define STLINK_SWD_DP_WAIT 0x14
328 #define STLINK_SWD_DP_FAULT 0x15
329 #define STLINK_SWD_DP_ERROR 0x16
330 #define STLINK_SWD_DP_PARITY_ERROR 0x17
331
332 #define STLINK_SWD_AP_WDATA_ERROR 0x18
333 #define STLINK_SWD_AP_STICKY_ERROR 0x19
334 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
335
336 #define STLINK_BAD_AP_ERROR 0x1d
337
338 #define STLINK_CORE_RUNNING 0x80
339 #define STLINK_CORE_HALTED 0x81
340 #define STLINK_CORE_STAT_UNKNOWN -1
341
342 #define STLINK_GET_VERSION 0xF1
343 #define STLINK_DEBUG_COMMAND 0xF2
344 #define STLINK_DFU_COMMAND 0xF3
345 #define STLINK_SWIM_COMMAND 0xF4
346 #define STLINK_GET_CURRENT_MODE 0xF5
347 #define STLINK_GET_TARGET_VOLTAGE 0xF7
348
349 #define STLINK_DEV_DFU_MODE 0x00
350 #define STLINK_DEV_MASS_MODE 0x01
351 #define STLINK_DEV_DEBUG_MODE 0x02
352 #define STLINK_DEV_SWIM_MODE 0x03
353 #define STLINK_DEV_BOOTLOADER_MODE 0x04
354 #define STLINK_DEV_UNKNOWN_MODE -1
355
356 #define STLINK_DFU_EXIT 0x07
357
358 /*
359 STLINK_SWIM_ENTER_SEQ
360 1.3ms low then 750Hz then 1.5kHz
361
362 STLINK_SWIM_GEN_RST
363 STM8 DM pulls reset pin low 50us
364
365 STLINK_SWIM_SPEED
366 uint8_t (0=low|1=high)
367
368 STLINK_SWIM_WRITEMEM
369 uint16_t length
370 uint32_t address
371
372 STLINK_SWIM_RESET
373 send synchronization seq (16us low, response 64 clocks low)
374 */
375 #define STLINK_SWIM_ENTER 0x00
376 #define STLINK_SWIM_EXIT 0x01
377 #define STLINK_SWIM_READ_CAP 0x02
378 #define STLINK_SWIM_SPEED 0x03
379 #define STLINK_SWIM_ENTER_SEQ 0x04
380 #define STLINK_SWIM_GEN_RST 0x05
381 #define STLINK_SWIM_RESET 0x06
382 #define STLINK_SWIM_ASSERT_RESET 0x07
383 #define STLINK_SWIM_DEASSERT_RESET 0x08
384 #define STLINK_SWIM_READSTATUS 0x09
385 #define STLINK_SWIM_WRITEMEM 0x0a
386 #define STLINK_SWIM_READMEM 0x0b
387 #define STLINK_SWIM_READBUF 0x0c
388
389 #define STLINK_DEBUG_GETSTATUS 0x01
390 #define STLINK_DEBUG_FORCEDEBUG 0x02
391 #define STLINK_DEBUG_APIV1_RESETSYS 0x03
392 #define STLINK_DEBUG_APIV1_READALLREGS 0x04
393 #define STLINK_DEBUG_APIV1_READREG 0x05
394 #define STLINK_DEBUG_APIV1_WRITEREG 0x06
395 #define STLINK_DEBUG_READMEM_32BIT 0x07
396 #define STLINK_DEBUG_WRITEMEM_32BIT 0x08
397 #define STLINK_DEBUG_RUNCORE 0x09
398 #define STLINK_DEBUG_STEPCORE 0x0a
399 #define STLINK_DEBUG_APIV1_SETFP 0x0b
400 #define STLINK_DEBUG_READMEM_8BIT 0x0c
401 #define STLINK_DEBUG_WRITEMEM_8BIT 0x0d
402 #define STLINK_DEBUG_APIV1_CLEARFP 0x0e
403 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG 0x0f
404 #define STLINK_DEBUG_APIV1_SETWATCHPOINT 0x10
405
406 #define STLINK_DEBUG_ENTER_JTAG_RESET 0x00
407 #define STLINK_DEBUG_ENTER_SWD_NO_RESET 0xa3
408 #define STLINK_DEBUG_ENTER_JTAG_NO_RESET 0xa4
409
410 #define STLINK_DEBUG_APIV1_ENTER 0x20
411 #define STLINK_DEBUG_EXIT 0x21
412 #define STLINK_DEBUG_READCOREID 0x22
413
414 #define STLINK_DEBUG_APIV2_ENTER 0x30
415 #define STLINK_DEBUG_APIV2_READ_IDCODES 0x31
416 #define STLINK_DEBUG_APIV2_RESETSYS 0x32
417 #define STLINK_DEBUG_APIV2_READREG 0x33
418 #define STLINK_DEBUG_APIV2_WRITEREG 0x34
419 #define STLINK_DEBUG_APIV2_WRITEDEBUGREG 0x35
420 #define STLINK_DEBUG_APIV2_READDEBUGREG 0x36
421
422 #define STLINK_DEBUG_APIV2_READALLREGS 0x3A
423 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
424 #define STLINK_DEBUG_APIV2_DRIVE_NRST 0x3C
425
426 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
427
428 #define STLINK_DEBUG_APIV2_START_TRACE_RX 0x40
429 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX 0x41
430 #define STLINK_DEBUG_APIV2_GET_TRACE_NB 0x42
431 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ 0x43
432 #define STLINK_DEBUG_APIV2_JTAG_SET_FREQ 0x44
433 #define STLINK_DEBUG_APIV2_READ_DAP_REG 0x45
434 #define STLINK_DEBUG_APIV2_WRITE_DAP_REG 0x46
435 #define STLINK_DEBUG_APIV2_READMEM_16BIT 0x47
436 #define STLINK_DEBUG_APIV2_WRITEMEM_16BIT 0x48
437
438 #define STLINK_DEBUG_APIV2_INIT_AP 0x4B
439 #define STLINK_DEBUG_APIV2_CLOSE_AP_DBG 0x4C
440
441 #define STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC 0x50
442 #define STLINK_DEBUG_APIV2_RW_MISC_OUT 0x51
443 #define STLINK_DEBUG_APIV2_RW_MISC_IN 0x52
444
445 #define STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC 0x54
446
447 #define STLINK_APIV3_SET_COM_FREQ 0x61
448 #define STLINK_APIV3_GET_COM_FREQ 0x62
449
450 #define STLINK_APIV3_GET_VERSION_EX 0xFB
451
452 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW 0x00
453 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH 0x01
454 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
455
456 #define STLINK_DEBUG_PORT_ACCESS 0xffff
457
458 #define STLINK_TRACE_SIZE 4096
459 #define STLINK_TRACE_MAX_HZ 2250000
460 #define STLINK_V3_TRACE_MAX_HZ 24000000
461
462 #define STLINK_V3_MAX_FREQ_NB 10
463
464 #define REQUEST_SENSE 0x03
465 #define REQUEST_SENSE_LENGTH 18
466
467 /* STLINK TCP commands */
468 #define STLINK_TCP_CMD_REFRESH_DEVICE_LIST 0x00
469 #define STLINK_TCP_CMD_GET_NB_DEV 0x01
470 #define STLINK_TCP_CMD_GET_DEV_INFO 0x02
471 #define STLINK_TCP_CMD_OPEN_DEV 0x03
472 #define STLINK_TCP_CMD_CLOSE_DEV 0x04
473 #define STLINK_TCP_CMD_SEND_USB_CMD 0x05
474 #define STLINK_TCP_CMD_GET_SERVER_VERSION 0x06
475 #define STLINK_TCP_CMD_GET_NB_OF_DEV_CLIENTS 0x07
476
477 /* STLINK TCP constants */
478 #define OPENOCD_STLINK_TCP_API_VERSION 1
479 #define STLINK_TCP_REQUEST_WRITE 0
480 #define STLINK_TCP_REQUEST_READ 1
481 #define STLINK_TCP_REQUEST_READ_SWO 3
482 #define STLINK_TCP_SS_SIZE 4
483 #define STLINK_TCP_USB_CMD_SIZE 32
484 #define STLINK_TCP_SERIAL_SIZE 32
485 #define STLINK_TCP_SEND_BUFFER_SIZE 10240
486 #define STLINK_TCP_RECV_BUFFER_SIZE 10240
487
488 /* STLINK TCP command status */
489 #define STLINK_TCP_SS_OK 0x00000001
490 #define STLINK_TCP_SS_MEMORY_PROBLEM 0x00001000
491 #define STLINK_TCP_SS_TIMEOUT 0x00001001
492 #define STLINK_TCP_SS_BAD_PARAMETER 0x00001002
493 #define STLINK_TCP_SS_OPEN_ERR 0x00001003
494 #define STLINK_TCP_SS_TRUNCATED_DATA 0x00001052
495 #define STLINK_TCP_SS_CMD_NOT_AVAILABLE 0x00001053
496 #define STLINK_TCP_SS_TCP_ERROR 0x00002001
497 #define STLINK_TCP_SS_TCP_CANT_CONNECT 0x00002002
498 #define STLINK_TCP_SS_TCP_CLOSE_ERROR 0x00002003
499 #define STLINK_TCP_SS_TCP_BUSY 0x00002004
500 #define STLINK_TCP_SS_WIN32_ERROR 0x00010000
501
502 /*
503 * Map the relevant features, quirks and workaround for specific firmware
504 * version of stlink
505 */
506 #define STLINK_F_HAS_TRACE BIT(0) /* v2>=j13 || v3 */
507 #define STLINK_F_HAS_GETLASTRWSTATUS2 BIT(1) /* v2>=j15 || v3 */
508 #define STLINK_F_HAS_SWD_SET_FREQ BIT(2) /* v2>=j22 */
509 #define STLINK_F_HAS_JTAG_SET_FREQ BIT(3) /* v2>=j24 */
510 #define STLINK_F_QUIRK_JTAG_DP_READ BIT(4) /* v2>=j24 && v2<j32 */
511 #define STLINK_F_HAS_DAP_REG BIT(5) /* v2>=j24 || v3 */
512 #define STLINK_F_HAS_MEM_16BIT BIT(6) /* v2>=j26 || v3 */
513 #define STLINK_F_HAS_AP_INIT BIT(7) /* v2>=j28 || v3 */
514 #define STLINK_F_FIX_CLOSE_AP BIT(8) /* v2>=j29 || v3 */
515 #define STLINK_F_HAS_DPBANKSEL BIT(9) /* v2>=j32 || v3>=j2 */
516 #define STLINK_F_HAS_RW8_512BYTES BIT(10) /* v3>=j6 */
517
518 /* aliases */
519 #define STLINK_F_HAS_TARGET_VOLT STLINK_F_HAS_TRACE
520 #define STLINK_F_HAS_FPU_REG STLINK_F_HAS_GETLASTRWSTATUS2
521 #define STLINK_F_HAS_MEM_WR_NO_INC STLINK_F_HAS_MEM_16BIT
522 #define STLINK_F_HAS_MEM_RD_NO_INC STLINK_F_HAS_DPBANKSEL
523 #define STLINK_F_HAS_RW_MISC STLINK_F_HAS_DPBANKSEL
524 #define STLINK_F_HAS_CSW STLINK_F_HAS_DPBANKSEL
525
526 #define STLINK_REGSEL_IS_FPU(x) ((x) > 0x1F)
527
528 struct speed_map {
529 int speed;
530 int speed_divisor;
531 };
532
533 /* SWD clock speed */
534 static const struct speed_map stlink_khz_to_speed_map_swd[] = {
535 {4000, 0},
536 {1800, 1}, /* default */
537 {1200, 2},
538 {950, 3},
539 {480, 7},
540 {240, 15},
541 {125, 31},
542 {100, 40},
543 {50, 79},
544 {25, 158},
545 {15, 265},
546 {5, 798}
547 };
548
549 /* JTAG clock speed */
550 static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
551 {9000, 4},
552 {4500, 8},
553 {2250, 16},
554 {1125, 32}, /* default */
555 {562, 64},
556 {281, 128},
557 {140, 256}
558 };
559
560 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
561 static int stlink_swim_status(void *handle);
562 static void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size);
563 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map);
564 static int stlink_speed(void *handle, int khz, bool query);
565 static int stlink_usb_open_ap(void *handle, unsigned short apsel);
566
567 /** */
568 static unsigned int stlink_usb_block(void *handle)
569 {
570 struct stlink_usb_handle_s *h = handle;
571
572 assert(handle);
573
574 if (h->version.flags & STLINK_F_HAS_RW8_512BYTES)
575 return STLINKV3_MAX_RW8;
576 else
577 return STLINK_MAX_RW8;
578 }
579
580 #ifdef USE_LIBUSB_ASYNCIO
581
582 static LIBUSB_CALL void sync_transfer_cb(struct libusb_transfer *transfer)
583 {
584 int *completed = transfer->user_data;
585 *completed = 1;
586 /* caller interprets result and frees transfer */
587 }
588
589
590 static void sync_transfer_wait_for_completion(struct libusb_transfer *transfer)
591 {
592 int r, *completed = transfer->user_data;
593
594 while (!*completed) {
595 r = jtag_libusb_handle_events_completed(completed);
596 if (r < 0) {
597 if (r == LIBUSB_ERROR_INTERRUPTED)
598 continue;
599 libusb_cancel_transfer(transfer);
600 continue;
601 }
602 }
603 }
604
605
606 static int transfer_error_status(const struct libusb_transfer *transfer)
607 {
608 int r = 0;
609
610 switch (transfer->status) {
611 case LIBUSB_TRANSFER_COMPLETED:
612 r = 0;
613 break;
614 case LIBUSB_TRANSFER_TIMED_OUT:
615 r = LIBUSB_ERROR_TIMEOUT;
616 break;
617 case LIBUSB_TRANSFER_STALL:
618 r = LIBUSB_ERROR_PIPE;
619 break;
620 case LIBUSB_TRANSFER_OVERFLOW:
621 r = LIBUSB_ERROR_OVERFLOW;
622 break;
623 case LIBUSB_TRANSFER_NO_DEVICE:
624 r = LIBUSB_ERROR_NO_DEVICE;
625 break;
626 case LIBUSB_TRANSFER_ERROR:
627 case LIBUSB_TRANSFER_CANCELLED:
628 r = LIBUSB_ERROR_IO;
629 break;
630 default:
631 r = LIBUSB_ERROR_OTHER;
632 break;
633 }
634
635 return r;
636 }
637
638 struct jtag_xfer {
639 int ep;
640 uint8_t *buf;
641 size_t size;
642 /* Internal */
643 int retval;
644 int completed;
645 size_t transfer_size;
646 struct libusb_transfer *transfer;
647 };
648
649 static int jtag_libusb_bulk_transfer_n(
650 struct libusb_device_handle *dev_handle,
651 struct jtag_xfer *transfers,
652 size_t n_transfers,
653 int timeout)
654 {
655 int retval = 0;
656 int returnval = ERROR_OK;
657
658
659 for (size_t i = 0; i < n_transfers; ++i) {
660 transfers[i].retval = 0;
661 transfers[i].completed = 0;
662 transfers[i].transfer_size = 0;
663 transfers[i].transfer = libusb_alloc_transfer(0);
664
665 if (!transfers[i].transfer) {
666 for (size_t j = 0; j < i; ++j)
667 libusb_free_transfer(transfers[j].transfer);
668
669 LOG_DEBUG("ERROR, failed to alloc usb transfers");
670 for (size_t k = 0; k < n_transfers; ++k)
671 transfers[k].retval = LIBUSB_ERROR_NO_MEM;
672 return ERROR_FAIL;
673 }
674 }
675
676 for (size_t i = 0; i < n_transfers; ++i) {
677 libusb_fill_bulk_transfer(
678 transfers[i].transfer,
679 dev_handle,
680 transfers[i].ep, transfers[i].buf, transfers[i].size,
681 sync_transfer_cb, &transfers[i].completed, timeout);
682 transfers[i].transfer->type = LIBUSB_TRANSFER_TYPE_BULK;
683
684 retval = libusb_submit_transfer(transfers[i].transfer);
685 if (retval < 0) {
686 LOG_DEBUG("ERROR, failed to submit transfer %zu, error %d", i, retval);
687
688 /* Probably no point continuing to submit transfers once a submission fails.
689 * As a result, tag all remaining transfers as errors.
690 */
691 for (size_t j = i; j < n_transfers; ++j)
692 transfers[j].retval = retval;
693
694 returnval = ERROR_FAIL;
695 break;
696 }
697 }
698
699 /* Wait for every submitted USB transfer to complete.
700 */
701 for (size_t i = 0; i < n_transfers; ++i) {
702 if (transfers[i].retval == 0) {
703 sync_transfer_wait_for_completion(transfers[i].transfer);
704
705 retval = transfer_error_status(transfers[i].transfer);
706 if (retval) {
707 returnval = ERROR_FAIL;
708 transfers[i].retval = retval;
709 LOG_DEBUG("ERROR, transfer %zu failed, error %d", i, retval);
710 } else {
711 /* Assuming actual_length is only valid if there is no transfer error.
712 */
713 transfers[i].transfer_size = transfers[i].transfer->actual_length;
714 }
715 }
716
717 libusb_free_transfer(transfers[i].transfer);
718 transfers[i].transfer = NULL;
719 }
720
721 return returnval;
722 }
723
724 #endif
725
726
727 /** */
728 static int stlink_usb_xfer_v1_get_status(void *handle)
729 {
730 struct stlink_usb_handle_s *h = handle;
731 int tr, ret;
732
733 assert(handle);
734
735 /* read status */
736 memset(h->cmdbuf, 0, STLINK_SG_SIZE);
737
738 ret = jtag_libusb_bulk_read(h->usb_backend_priv.fd, h->rx_ep, (char *)h->cmdbuf, 13,
739 STLINK_READ_TIMEOUT, &tr);
740 if (ret || tr != 13)
741 return ERROR_FAIL;
742
743 uint32_t t1;
744
745 t1 = buf_get_u32(h->cmdbuf, 0, 32);
746
747 /* check for USBS */
748 if (t1 != 0x53425355)
749 return ERROR_FAIL;
750 /*
751 * CSW status:
752 * 0 success
753 * 1 command failure
754 * 2 phase error
755 */
756 if (h->cmdbuf[12] != 0)
757 return ERROR_FAIL;
758
759 return ERROR_OK;
760 }
761
762 #ifdef USE_LIBUSB_ASYNCIO
763 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
764 {
765 struct stlink_usb_handle_s *h = handle;
766
767 assert(handle);
768
769 size_t n_transfers = 0;
770 struct jtag_xfer transfers[2];
771
772 memset(transfers, 0, sizeof(transfers));
773
774 transfers[0].ep = h->tx_ep;
775 transfers[0].buf = h->cmdbuf;
776 transfers[0].size = cmdsize;
777
778 ++n_transfers;
779
780 if (h->direction == h->tx_ep && size) {
781 transfers[1].ep = h->tx_ep;
782 transfers[1].buf = (uint8_t *)buf;
783 transfers[1].size = size;
784
785 ++n_transfers;
786 } else if (h->direction == h->rx_ep && size) {
787 transfers[1].ep = h->rx_ep;
788 transfers[1].buf = (uint8_t *)buf;
789 transfers[1].size = size;
790
791 ++n_transfers;
792 }
793
794 return jtag_libusb_bulk_transfer_n(
795 h->usb_backend_priv.fd,
796 transfers,
797 n_transfers,
798 STLINK_WRITE_TIMEOUT);
799 }
800 #else
801 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
802 {
803 struct stlink_usb_handle_s *h = handle;
804 int tr, ret;
805
806 assert(handle);
807
808 ret = jtag_libusb_bulk_write(h->usb_backend_priv.fd, h->tx_ep, (char *)h->cmdbuf,
809 cmdsize, STLINK_WRITE_TIMEOUT, &tr);
810 if (ret || tr != cmdsize)
811 return ERROR_FAIL;
812
813 if (h->direction == h->tx_ep && size) {
814 ret = jtag_libusb_bulk_write(h->usb_backend_priv.fd, h->tx_ep, (char *)buf,
815 size, STLINK_WRITE_TIMEOUT, &tr);
816 if (ret || tr != size) {
817 LOG_DEBUG("bulk write failed");
818 return ERROR_FAIL;
819 }
820 } else if (h->direction == h->rx_ep && size) {
821 ret = jtag_libusb_bulk_read(h->usb_backend_priv.fd, h->rx_ep, (char *)buf,
822 size, STLINK_READ_TIMEOUT, &tr);
823 if (ret || tr != size) {
824 LOG_DEBUG("bulk read failed");
825 return ERROR_FAIL;
826 }
827 }
828
829 return ERROR_OK;
830 }
831 #endif
832
833 /** */
834 static int stlink_usb_xfer_v1_get_sense(void *handle)
835 {
836 int res;
837 struct stlink_usb_handle_s *h = handle;
838
839 assert(handle);
840
841 stlink_usb_init_buffer(handle, h->rx_ep, 16);
842
843 h->cmdbuf[h->cmdidx++] = REQUEST_SENSE;
844 h->cmdbuf[h->cmdidx++] = 0;
845 h->cmdbuf[h->cmdidx++] = 0;
846 h->cmdbuf[h->cmdidx++] = 0;
847 h->cmdbuf[h->cmdidx++] = REQUEST_SENSE_LENGTH;
848
849 res = stlink_usb_xfer_rw(handle, REQUEST_SENSE_LENGTH, h->databuf, 16);
850
851 if (res != ERROR_OK)
852 return res;
853
854 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK)
855 return ERROR_FAIL;
856
857 return ERROR_OK;
858 }
859
860 /** */
861 static int stlink_usb_usb_read_trace(void *handle, const uint8_t *buf, int size)
862 {
863 struct stlink_usb_handle_s *h = handle;
864 int tr, ret;
865
866 ret = jtag_libusb_bulk_read(h->usb_backend_priv.fd, h->trace_ep, (char *)buf, size,
867 STLINK_READ_TIMEOUT, &tr);
868 if (ret || tr != size) {
869 LOG_ERROR("bulk trace read failed");
870 return ERROR_FAIL;
871 }
872
873 return ERROR_OK;
874 }
875
876 /*
877 transfers block in cmdbuf
878 <size> indicates number of bytes in the following
879 data phase.
880 Ignore the (eventual) error code in the received packet.
881 */
882 static int stlink_usb_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
883 {
884 int err, cmdsize = STLINK_CMD_SIZE_V2;
885 struct stlink_usb_handle_s *h = handle;
886
887 assert(handle);
888
889 if (h->version.stlink == 1) {
890 cmdsize = STLINK_SG_SIZE;
891 /* put length in bCBWCBLength */
892 h->cmdbuf[14] = h->cmdidx-15;
893 }
894
895 err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
896
897 if (err != ERROR_OK)
898 return err;
899
900 if (h->version.stlink == 1) {
901 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK) {
902 /* check csw status */
903 if (h->cmdbuf[12] == 1) {
904 LOG_DEBUG("get sense");
905 if (stlink_usb_xfer_v1_get_sense(handle) != ERROR_OK)
906 return ERROR_FAIL;
907 }
908 return ERROR_FAIL;
909 }
910 }
911
912 return ERROR_OK;
913 }
914
915
916 static int stlink_tcp_send_cmd(void *handle, int send_size, int recv_size, bool check_tcp_status)
917 {
918 struct stlink_usb_handle_s *h = handle;
919
920 assert(handle);
921
922 /* send the TCP command */
923 int sent_size = send(h->tcp_backend_priv.fd, (void *)h->tcp_backend_priv.send_buf, send_size, 0);
924 if (sent_size != send_size) {
925 LOG_ERROR("failed to send USB CMD");
926 if (sent_size == -1)
927 LOG_DEBUG("socket send error: %s (errno %d)", strerror(errno), errno);
928 else
929 LOG_DEBUG("sent size %d (expected %d)", sent_size, send_size);
930 return ERROR_FAIL;
931 }
932
933 /* read the TCP response */
934 int retval = ERROR_OK;
935 int remaining_bytes = recv_size;
936 uint8_t *recv_buf = h->tcp_backend_priv.recv_buf;
937 const int64_t timeout = timeval_ms() + 1000; /* 1 second */
938
939 while (remaining_bytes > 0) {
940 if (timeval_ms() > timeout) {
941 LOG_DEBUG("received size %d (expected %d)", recv_size - remaining_bytes, recv_size);
942 retval = ERROR_TIMEOUT_REACHED;
943 break;
944 }
945
946 keep_alive();
947 int received = recv(h->tcp_backend_priv.fd, (void *)recv_buf, remaining_bytes, 0);
948
949 if (received == -1) {
950 LOG_DEBUG("socket recv error: %s (errno %d)", strerror(errno), errno);
951 retval = ERROR_FAIL;
952 break;
953 }
954
955 recv_buf += received;
956 remaining_bytes -= received;
957 }
958
959 if (retval != ERROR_OK) {
960 LOG_ERROR("failed to receive USB CMD response");
961 return retval;
962 }
963
964 if (check_tcp_status) {
965 uint32_t tcp_ss = le_to_h_u32(h->tcp_backend_priv.recv_buf);
966 if (tcp_ss != STLINK_TCP_SS_OK) {
967 if (tcp_ss == STLINK_TCP_SS_TCP_BUSY) {
968 LOG_DEBUG("TCP busy");
969 return ERROR_WAIT;
970 }
971
972 LOG_ERROR("TCP error status 0x%X", tcp_ss);
973 return ERROR_FAIL;
974 }
975 }
976
977 return ERROR_OK;
978 }
979
980 /** */
981 static int stlink_tcp_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
982 {
983 struct stlink_usb_handle_s *h = handle;
984
985 int send_size = STLINK_TCP_USB_CMD_SIZE;
986 int recv_size = STLINK_TCP_SS_SIZE;
987
988 assert(handle);
989
990 /* prepare the TCP command */
991 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_SEND_USB_CMD;
992 memset(&h->tcp_backend_priv.send_buf[1], 0, 3); /* reserved for alignment and future use, must be zero */
993 h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.connect_id);
994 /* tcp_backend_priv.send_buf[8..23] already contains the constructed stlink command */
995 h->tcp_backend_priv.send_buf[24] = h->direction;
996 memset(&h->tcp_backend_priv.send_buf[25], 0, 3); /* reserved for alignment and future use, must be zero */
997
998 h_u32_to_le(&h->tcp_backend_priv.send_buf[28], size);
999
1000 /*
1001 * if the xfer is a write request (tx_ep)
1002 * > then buf content will be copied
1003 * into &cmdbuf[32].
1004 * else : the xfer is a read or trace read request (rx_ep or trace_ep)
1005 * > the buf content will be filled from &databuf[4].
1006 *
1007 * note : if h->direction is trace_ep, h->cmdbuf is zeros.
1008 */
1009
1010 if (h->direction == h->tx_ep) { /* STLINK_TCP_REQUEST_WRITE */
1011 send_size += size;
1012 if (send_size > STLINK_TCP_SEND_BUFFER_SIZE) {
1013 LOG_ERROR("STLINK_TCP command buffer overflow");
1014 return ERROR_FAIL;
1015 }
1016 memcpy(&h->tcp_backend_priv.send_buf[32], buf, size);
1017 } else { /* STLINK_TCP_REQUEST_READ or STLINK_TCP_REQUEST_READ_SWO */
1018 recv_size += size;
1019 if (recv_size > STLINK_TCP_RECV_BUFFER_SIZE) {
1020 LOG_ERROR("STLINK_TCP data buffer overflow");
1021 return ERROR_FAIL;
1022 }
1023 }
1024
1025 int ret = stlink_tcp_send_cmd(h, send_size, recv_size, true);
1026 if (ret != ERROR_OK)
1027 return ret;
1028
1029 if (h->direction != h->tx_ep) {
1030 /* the read data is located in tcp_backend_priv.recv_buf[4] */
1031 /* most of the case it will be copying the data from tcp_backend_priv.recv_buf[4]
1032 * to handle->cmd_buff which are the same, so let's avoid unnecessary copying */
1033 if (buf != &h->tcp_backend_priv.recv_buf[4])
1034 memcpy((uint8_t *)buf, &h->tcp_backend_priv.recv_buf[4], size);
1035 }
1036
1037 return ERROR_OK;
1038 }
1039
1040 /** */
1041 static int stlink_tcp_read_trace(void *handle, const uint8_t *buf, int size)
1042 {
1043 struct stlink_usb_handle_s *h = handle;
1044
1045 stlink_usb_init_buffer(h, h->trace_ep, 0);
1046 return stlink_tcp_xfer_noerrcheck(handle, buf, size);
1047 }
1048
1049 /**
1050 Converts an STLINK status code held in the first byte of a response
1051 to an openocd error, logs any error/wait status as debug output.
1052 */
1053 static int stlink_usb_error_check(void *handle)
1054 {
1055 struct stlink_usb_handle_s *h = handle;
1056
1057 assert(handle);
1058
1059 if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1060 switch (h->databuf[0]) {
1061 case STLINK_SWIM_ERR_OK:
1062 return ERROR_OK;
1063 case STLINK_SWIM_BUSY:
1064 return ERROR_WAIT;
1065 default:
1066 LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
1067 return ERROR_FAIL;
1068 }
1069 }
1070
1071 /* TODO: no error checking yet on api V1 */
1072 if (h->version.jtag_api == STLINK_JTAG_API_V1)
1073 h->databuf[0] = STLINK_DEBUG_ERR_OK;
1074
1075 switch (h->databuf[0]) {
1076 case STLINK_DEBUG_ERR_OK:
1077 return ERROR_OK;
1078 case STLINK_DEBUG_ERR_FAULT:
1079 LOG_DEBUG("SWD fault response (0x%x)", STLINK_DEBUG_ERR_FAULT);
1080 return ERROR_FAIL;
1081 case STLINK_SWD_AP_WAIT:
1082 LOG_DEBUG("wait status SWD_AP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
1083 return ERROR_WAIT;
1084 case STLINK_SWD_DP_WAIT:
1085 LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_DP_WAIT);
1086 return ERROR_WAIT;
1087 case STLINK_JTAG_GET_IDCODE_ERROR:
1088 LOG_DEBUG("STLINK_JTAG_GET_IDCODE_ERROR");
1089 return ERROR_FAIL;
1090 case STLINK_JTAG_WRITE_ERROR:
1091 LOG_DEBUG("Write error");
1092 return ERROR_FAIL;
1093 case STLINK_JTAG_WRITE_VERIF_ERROR:
1094 LOG_DEBUG("Write verify error, ignoring");
1095 return ERROR_OK;
1096 case STLINK_SWD_AP_FAULT:
1097 /* git://git.ac6.fr/openocd commit 657e3e885b9ee10
1098 * returns ERROR_OK with the comment:
1099 * Change in error status when reading outside RAM.
1100 * This fix allows CDT plugin to visualize memory.
1101 */
1102 LOG_DEBUG("STLINK_SWD_AP_FAULT");
1103 return ERROR_FAIL;
1104 case STLINK_SWD_AP_ERROR:
1105 LOG_DEBUG("STLINK_SWD_AP_ERROR");
1106 return ERROR_FAIL;
1107 case STLINK_SWD_AP_PARITY_ERROR:
1108 LOG_DEBUG("STLINK_SWD_AP_PARITY_ERROR");
1109 return ERROR_FAIL;
1110 case STLINK_SWD_DP_FAULT:
1111 LOG_DEBUG("STLINK_SWD_DP_FAULT");
1112 return ERROR_FAIL;
1113 case STLINK_SWD_DP_ERROR:
1114 LOG_DEBUG("STLINK_SWD_DP_ERROR");
1115 return ERROR_FAIL;
1116 case STLINK_SWD_DP_PARITY_ERROR:
1117 LOG_DEBUG("STLINK_SWD_DP_PARITY_ERROR");
1118 return ERROR_FAIL;
1119 case STLINK_SWD_AP_WDATA_ERROR:
1120 LOG_DEBUG("STLINK_SWD_AP_WDATA_ERROR");
1121 return ERROR_FAIL;
1122 case STLINK_SWD_AP_STICKY_ERROR:
1123 LOG_DEBUG("STLINK_SWD_AP_STICKY_ERROR");
1124 return ERROR_FAIL;
1125 case STLINK_SWD_AP_STICKYORUN_ERROR:
1126 LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR");
1127 return ERROR_FAIL;
1128 case STLINK_BAD_AP_ERROR:
1129 LOG_DEBUG("STLINK_BAD_AP_ERROR");
1130 return ERROR_FAIL;
1131 default:
1132 LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
1133 return ERROR_FAIL;
1134 }
1135 }
1136
1137 /*
1138 * Wrapper around stlink_usb_xfer_noerrcheck()
1139 * to check the error code in the received packet
1140 */
1141 static int stlink_usb_xfer_errcheck(void *handle, const uint8_t *buf, int size)
1142 {
1143 int retval;
1144
1145 assert(size > 0);
1146
1147 retval = stlink_usb_xfer_noerrcheck(handle, buf, size);
1148 if (retval != ERROR_OK)
1149 return retval;
1150
1151 return stlink_usb_error_check(handle);
1152 }
1153
1154 /** Issue an STLINK command via USB transfer, with retries on any wait status responses.
1155
1156 Works for commands where the STLINK_DEBUG status is returned in the first
1157 byte of the response packet. For SWIM a SWIM_READSTATUS is requested instead.
1158
1159 Returns an openocd result code.
1160 */
1161 static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
1162 {
1163 int retries = 0;
1164 int res;
1165 struct stlink_usb_handle_s *h = handle;
1166
1167 while (1) {
1168 if ((h->st_mode != STLINK_MODE_DEBUG_SWIM) || !retries) {
1169 res = stlink_usb_xfer_noerrcheck(handle, buf, size);
1170 if (res != ERROR_OK)
1171 return res;
1172 }
1173
1174 if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1175 res = stlink_swim_status(handle);
1176 if (res != ERROR_OK)
1177 return res;
1178 }
1179
1180 res = stlink_usb_error_check(handle);
1181 if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1182 unsigned int delay_us = (1<<retries++) * 1000;
1183 LOG_DEBUG("stlink_cmd_allow_retry ERROR_WAIT, retry %d, delaying %u microseconds", retries, delay_us);
1184 usleep(delay_us);
1185 continue;
1186 }
1187 return res;
1188 }
1189 }
1190
1191 /** */
1192 static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
1193 {
1194 struct stlink_usb_handle_s *h = handle;
1195
1196 assert(handle);
1197
1198 assert(h->version.flags & STLINK_F_HAS_TRACE);
1199
1200 return h->backend->read_trace(handle, buf, size);
1201 }
1202
1203 /*
1204 this function writes transfer length in
1205 the right place in the cb
1206 */
1207 static void stlink_usb_set_cbw_transfer_datalength(void *handle, uint32_t size)
1208 {
1209 struct stlink_usb_handle_s *h = handle;
1210
1211 buf_set_u32(h->cmdbuf+8, 0, 32, size);
1212 }
1213
1214 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
1215 {
1216 struct stlink_usb_handle_s *h = handle;
1217
1218 /* fill the send buffer */
1219 strcpy((char *)h->cmdbuf, "USBC");
1220 h->cmdidx += 4;
1221 /* csw tag not used */
1222 buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, 0);
1223 h->cmdidx += 4;
1224 /* cbw data transfer length (in the following data phase in or out) */
1225 buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
1226 h->cmdidx += 4;
1227 /* cbw flags */
1228 h->cmdbuf[h->cmdidx++] = (direction == h->rx_ep ? ENDPOINT_IN : ENDPOINT_OUT);
1229 h->cmdbuf[h->cmdidx++] = 0; /* lun */
1230 /* cdb clength (is filled in at xfer) */
1231 h->cmdbuf[h->cmdidx++] = 0;
1232 }
1233
1234 /** */
1235 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size)
1236 {
1237 struct stlink_usb_handle_s *h = handle;
1238
1239 h->direction = direction;
1240
1241 h->cmdidx = 0;
1242
1243 memset(h->cmdbuf, 0, STLINK_SG_SIZE);
1244 memset(h->databuf, 0, STLINK_DATA_SIZE);
1245
1246 if (h->version.stlink == 1)
1247 stlink_usb_xfer_v1_create_cmd(handle, direction, size);
1248 }
1249
1250 /** */
1251 static int stlink_usb_version(void *handle)
1252 {
1253 int res;
1254 uint32_t flags;
1255 uint16_t version;
1256 uint8_t v, x, y, jtag, swim, msd, bridge = 0;
1257 char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */
1258 char *p;
1259 struct stlink_usb_handle_s *h = handle;
1260
1261 assert(handle);
1262
1263 stlink_usb_init_buffer(handle, h->rx_ep, 6);
1264
1265 h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION;
1266
1267 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 6);
1268
1269 if (res != ERROR_OK)
1270 return res;
1271
1272 version = be_to_h_u16(h->databuf);
1273 v = (version >> 12) & 0x0f;
1274 x = (version >> 6) & 0x3f;
1275 y = version & 0x3f;
1276
1277 h->vid = le_to_h_u16(h->databuf + 2);
1278 h->pid = le_to_h_u16(h->databuf + 4);
1279
1280 switch (h->pid) {
1281 case STLINK_V2_1_PID:
1282 case STLINK_V2_1_NO_MSD_PID:
1283 if ((x <= 22 && y == 7) || (x >= 25 && y >= 7 && y <= 12)) {
1284 /* MxSy : STM8 V2.1 - SWIM only */
1285 msd = x;
1286 swim = y;
1287 jtag = 0;
1288 } else {
1289 /* JxMy : STM32 V2.1 - JTAG/SWD only */
1290 jtag = x;
1291 msd = y;
1292 swim = 0;
1293 }
1294 break;
1295 default:
1296 jtag = x;
1297 swim = y;
1298 msd = 0;
1299 break;
1300 }
1301
1302 /* STLINK-V3 & STLINK-V3P require a specific command */
1303 if (v >= 3 && x == 0 && y == 0) {
1304 stlink_usb_init_buffer(handle, h->rx_ep, 16);
1305
1306 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_VERSION_EX;
1307
1308 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 12);
1309 if (res != ERROR_OK)
1310 return res;
1311
1312 v = h->databuf[0];
1313 swim = h->databuf[1];
1314 jtag = h->databuf[2];
1315 msd = h->databuf[3];
1316 bridge = h->databuf[4];
1317 h->vid = le_to_h_u16(h->databuf + 8);
1318 h->pid = le_to_h_u16(h->databuf + 10);
1319 }
1320
1321 h->version.stlink = v;
1322 h->version.jtag = jtag;
1323 h->version.swim = swim;
1324
1325 flags = 0;
1326 switch (h->version.stlink) {
1327 case 1:
1328 /* ST-LINK/V1 from J11 switch to api-v2 (and support SWD) */
1329 if (h->version.jtag >= 11)
1330 h->version.jtag_api = STLINK_JTAG_API_V2;
1331 else
1332 h->version.jtag_api = STLINK_JTAG_API_V1;
1333
1334 break;
1335 case 2:
1336 /* all ST-LINK/V2 and ST-Link/V2.1 use api-v2 */
1337 h->version.jtag_api = STLINK_JTAG_API_V2;
1338
1339 /* API for trace from J13 */
1340 /* API for target voltage from J13 */
1341 if (h->version.jtag >= 13)
1342 flags |= STLINK_F_HAS_TRACE;
1343
1344 /* preferred API to get last R/W status from J15 */
1345 if (h->version.jtag >= 15)
1346 flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
1347
1348 /* API to set SWD frequency from J22 */
1349 if (h->version.jtag >= 22)
1350 flags |= STLINK_F_HAS_SWD_SET_FREQ;
1351
1352 /* API to set JTAG frequency from J24 */
1353 /* API to access DAP registers from J24 */
1354 if (h->version.jtag >= 24) {
1355 flags |= STLINK_F_HAS_JTAG_SET_FREQ;
1356 flags |= STLINK_F_HAS_DAP_REG;
1357 }
1358
1359 /* Quirk for read DP in JTAG mode (V2 only) from J24, fixed in J32 */
1360 if (h->version.jtag >= 24 && h->version.jtag < 32)
1361 flags |= STLINK_F_QUIRK_JTAG_DP_READ;
1362
1363 /* API to read/write memory at 16 bit from J26 */
1364 /* API to write memory without address increment from J26 */
1365 if (h->version.jtag >= 26)
1366 flags |= STLINK_F_HAS_MEM_16BIT;
1367
1368 /* API required to init AP before any AP access from J28 */
1369 if (h->version.jtag >= 28)
1370 flags |= STLINK_F_HAS_AP_INIT;
1371
1372 /* API required to return proper error code on close AP from J29 */
1373 if (h->version.jtag >= 29)
1374 flags |= STLINK_F_FIX_CLOSE_AP;
1375
1376 /* Banked regs (DPv1 & DPv2) support from V2J32 */
1377 /* API to read memory without address increment from V2J32 */
1378 /* Memory R/W supports CSW from V2J32 */
1379 if (h->version.jtag >= 32)
1380 flags |= STLINK_F_HAS_DPBANKSEL;
1381
1382 break;
1383 case 3:
1384 /* all STLINK-V3 use api-v3 */
1385 h->version.jtag_api = STLINK_JTAG_API_V3;
1386
1387 /* STLINK-V3 is a superset of ST-LINK/V2 */
1388
1389 /* API for trace */
1390 /* API for target voltage */
1391 flags |= STLINK_F_HAS_TRACE;
1392
1393 /* preferred API to get last R/W status */
1394 flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
1395
1396 /* API to access DAP registers */
1397 flags |= STLINK_F_HAS_DAP_REG;
1398
1399 /* API to read/write memory at 16 bit */
1400 /* API to write memory without address increment */
1401 flags |= STLINK_F_HAS_MEM_16BIT;
1402
1403 /* API required to init AP before any AP access */
1404 flags |= STLINK_F_HAS_AP_INIT;
1405
1406 /* API required to return proper error code on close AP */
1407 flags |= STLINK_F_FIX_CLOSE_AP;
1408
1409 /* Banked regs (DPv1 & DPv2) support from V3J2 */
1410 /* API to read memory without address increment from V3J2 */
1411 /* Memory R/W supports CSW from V3J2 */
1412 if (h->version.jtag >= 2)
1413 flags |= STLINK_F_HAS_DPBANKSEL;
1414
1415 /* 8bit read/write max packet size 512 bytes from V3J6 */
1416 if (h->version.jtag >= 6)
1417 flags |= STLINK_F_HAS_RW8_512BYTES;
1418
1419 break;
1420 case 4:
1421 /* STLINK-V3P use api-v3 */
1422 h->version.jtag_api = STLINK_JTAG_API_V3;
1423
1424 /* STLINK-V3P is a superset of ST-LINK/V3 */
1425
1426 /* API for trace */
1427 /* API for target voltage */
1428 flags |= STLINK_F_HAS_TRACE;
1429
1430 /* preferred API to get last R/W status */
1431 flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
1432
1433 /* API to access DAP registers */
1434 flags |= STLINK_F_HAS_DAP_REG;
1435
1436 /* API to read/write memory at 16 bit */
1437 /* API to write memory without address increment */
1438 flags |= STLINK_F_HAS_MEM_16BIT;
1439
1440 /* API required to init AP before any AP access */
1441 flags |= STLINK_F_HAS_AP_INIT;
1442
1443 /* API required to return proper error code on close AP */
1444 flags |= STLINK_F_FIX_CLOSE_AP;
1445
1446 /* Banked regs (DPv1 & DPv2) support */
1447 /* API to read memory without address increment */
1448 /* Memory R/W supports CSW */
1449 flags |= STLINK_F_HAS_DPBANKSEL;
1450
1451 /* 8bit read/write max packet size 512 bytes */
1452 flags |= STLINK_F_HAS_RW8_512BYTES;
1453
1454 break;
1455 default:
1456 break;
1457 }
1458 h->version.flags = flags;
1459
1460 p = v_str;
1461 p += sprintf(p, "V%d", v);
1462 if (jtag || !msd)
1463 p += sprintf(p, "J%d", jtag);
1464 if (msd)
1465 p += sprintf(p, "M%d", msd);
1466 if (bridge)
1467 p += sprintf(p, "B%d", bridge);
1468 if (swim || !msd)
1469 sprintf(p, "S%d", swim);
1470
1471 LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
1472 v_str,
1473 h->version.jtag_api,
1474 h->vid,
1475 h->pid);
1476
1477 return ERROR_OK;
1478 }
1479
1480 static int stlink_usb_check_voltage(void *handle, float *target_voltage)
1481 {
1482 struct stlink_usb_handle_s *h = handle;
1483 uint32_t adc_results[2];
1484
1485 /* no error message, simply quit with error */
1486 if (!(h->version.flags & STLINK_F_HAS_TARGET_VOLT))
1487 return ERROR_COMMAND_NOTFOUND;
1488
1489 stlink_usb_init_buffer(handle, h->rx_ep, 8);
1490
1491 h->cmdbuf[h->cmdidx++] = STLINK_GET_TARGET_VOLTAGE;
1492
1493 int result = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1494
1495 if (result != ERROR_OK)
1496 return result;
1497
1498 /* convert result */
1499 adc_results[0] = le_to_h_u32(h->databuf);
1500 adc_results[1] = le_to_h_u32(h->databuf + 4);
1501
1502 *target_voltage = 0;
1503
1504 if (adc_results[0])
1505 *target_voltage = 2 * ((float)adc_results[1]) * (float)(1.2 / adc_results[0]);
1506
1507 LOG_INFO("Target voltage: %f", (double)*target_voltage);
1508
1509 return ERROR_OK;
1510 }
1511
1512 static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
1513 {
1514 struct stlink_usb_handle_s *h = handle;
1515
1516 assert(handle);
1517
1518 if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
1519 return ERROR_COMMAND_NOTFOUND;
1520
1521 stlink_usb_init_buffer(handle, h->rx_ep, 2);
1522
1523 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1524 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
1525 h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1526 h->cmdidx += 2;
1527
1528 int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1529
1530 if (result != ERROR_OK)
1531 return result;
1532
1533 return ERROR_OK;
1534 }
1535
1536 static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor)
1537 {
1538 struct stlink_usb_handle_s *h = handle;
1539
1540 assert(handle);
1541
1542 if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
1543 return ERROR_COMMAND_NOTFOUND;
1544
1545 stlink_usb_init_buffer(handle, h->rx_ep, 2);
1546
1547 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1548 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
1549 h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1550 h->cmdidx += 2;
1551
1552 int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1553
1554 if (result != ERROR_OK)
1555 return result;
1556
1557 return ERROR_OK;
1558 }
1559
1560 /** */
1561 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
1562 {
1563 int res;
1564 struct stlink_usb_handle_s *h = handle;
1565
1566 assert(handle);
1567
1568 stlink_usb_init_buffer(handle, h->rx_ep, 2);
1569
1570 h->cmdbuf[h->cmdidx++] = STLINK_GET_CURRENT_MODE;
1571
1572 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
1573
1574 if (res != ERROR_OK)
1575 return res;
1576
1577 *mode = h->databuf[0];
1578
1579 return ERROR_OK;
1580 }
1581
1582 /** */
1583 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
1584 {
1585 int rx_size = 0;
1586 struct stlink_usb_handle_s *h = handle;
1587
1588 assert(handle);
1589
1590 /* on api V2 we are able the read the latest command
1591 * status
1592 * TODO: we need the test on api V1 too
1593 */
1594 if (h->version.jtag_api != STLINK_JTAG_API_V1)
1595 rx_size = 2;
1596
1597 stlink_usb_init_buffer(handle, h->rx_ep, rx_size);
1598
1599 switch (type) {
1600 case STLINK_MODE_DEBUG_JTAG:
1601 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1602 if (h->version.jtag_api == STLINK_JTAG_API_V1)
1603 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1604 else
1605 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1606 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG_NO_RESET;
1607 break;
1608 case STLINK_MODE_DEBUG_SWD:
1609 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1610 if (h->version.jtag_api == STLINK_JTAG_API_V1)
1611 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1612 else
1613 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1614 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD_NO_RESET;
1615 break;
1616 case STLINK_MODE_DEBUG_SWIM:
1617 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1618 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
1619 /* swim enter does not return any response or status */
1620 return stlink_usb_xfer_noerrcheck(handle, h->databuf, 0);
1621 case STLINK_MODE_DFU:
1622 case STLINK_MODE_MASS:
1623 default:
1624 return ERROR_FAIL;
1625 }
1626
1627 return stlink_cmd_allow_retry(handle, h->databuf, rx_size);
1628 }
1629
1630 /** */
1631 static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
1632 {
1633 int res;
1634 struct stlink_usb_handle_s *h = handle;
1635
1636 assert(handle);
1637
1638 /* command with no reply, use a valid endpoint but zero size */
1639 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1640
1641 switch (type) {
1642 case STLINK_MODE_DEBUG_JTAG:
1643 case STLINK_MODE_DEBUG_SWD:
1644 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1645 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_EXIT;
1646 break;
1647 case STLINK_MODE_DEBUG_SWIM:
1648 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1649 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_EXIT;
1650 break;
1651 case STLINK_MODE_DFU:
1652 h->cmdbuf[h->cmdidx++] = STLINK_DFU_COMMAND;
1653 h->cmdbuf[h->cmdidx++] = STLINK_DFU_EXIT;
1654 break;
1655 case STLINK_MODE_MASS:
1656 default:
1657 return ERROR_FAIL;
1658 }
1659
1660 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 0);
1661
1662 if (res != ERROR_OK)
1663 return res;
1664
1665 return ERROR_OK;
1666 }
1667
1668 static int stlink_usb_assert_srst(void *handle, int srst);
1669
1670 static enum stlink_mode stlink_get_mode(enum hl_transports t)
1671 {
1672 switch (t) {
1673 case HL_TRANSPORT_SWD:
1674 return STLINK_MODE_DEBUG_SWD;
1675 case HL_TRANSPORT_JTAG:
1676 return STLINK_MODE_DEBUG_JTAG;
1677 default:
1678 return STLINK_MODE_UNKNOWN;
1679 }
1680 }
1681
1682 /** */
1683 static int stlink_usb_exit_mode(void *handle)
1684 {
1685 int res;
1686 uint8_t mode;
1687 enum stlink_mode emode;
1688
1689 assert(handle);
1690
1691 res = stlink_usb_current_mode(handle, &mode);
1692
1693 if (res != ERROR_OK)
1694 return res;
1695
1696 LOG_DEBUG("MODE: 0x%02X", mode);
1697
1698 /* try to exit current mode */
1699 switch (mode) {
1700 case STLINK_DEV_DFU_MODE:
1701 emode = STLINK_MODE_DFU;
1702 break;
1703 case STLINK_DEV_DEBUG_MODE:
1704 emode = STLINK_MODE_DEBUG_SWD;
1705 break;
1706 case STLINK_DEV_SWIM_MODE:
1707 emode = STLINK_MODE_DEBUG_SWIM;
1708 break;
1709 case STLINK_DEV_BOOTLOADER_MODE:
1710 case STLINK_DEV_MASS_MODE:
1711 default:
1712 emode = STLINK_MODE_UNKNOWN;
1713 break;
1714 }
1715
1716 if (emode != STLINK_MODE_UNKNOWN)
1717 return stlink_usb_mode_leave(handle, emode);
1718
1719 return ERROR_OK;
1720 }
1721
1722 /** */
1723 static int stlink_usb_init_mode(void *handle, bool connect_under_reset, int initial_interface_speed)
1724 {
1725 int res;
1726 uint8_t mode;
1727 enum stlink_mode emode;
1728 struct stlink_usb_handle_s *h = handle;
1729
1730 assert(handle);
1731
1732 res = stlink_usb_exit_mode(handle);
1733 if (res != ERROR_OK)
1734 return res;
1735
1736 res = stlink_usb_current_mode(handle, &mode);
1737
1738 if (res != ERROR_OK)
1739 return res;
1740
1741 /* we check the target voltage here as an aid to debugging connection problems.
1742 * the stlink requires the target Vdd to be connected for reliable debugging.
1743 * this cmd is supported in all modes except DFU
1744 */
1745 if (mode != STLINK_DEV_DFU_MODE) {
1746
1747 float target_voltage;
1748
1749 /* check target voltage (if supported) */
1750 res = stlink_usb_check_voltage(h, &target_voltage);
1751
1752 if (res != ERROR_OK) {
1753 if (res != ERROR_COMMAND_NOTFOUND)
1754 LOG_ERROR("voltage check failed");
1755 /* attempt to continue as it is not a catastrophic failure */
1756 } else {
1757 /* check for a sensible target voltage, operating range is 1.65-5.5v
1758 * according to datasheet */
1759 if (target_voltage < 1.5)
1760 LOG_ERROR("target voltage may be too low for reliable debugging");
1761 }
1762 }
1763
1764 LOG_DEBUG("MODE: 0x%02X", mode);
1765
1766 /* set selected mode */
1767 emode = h->st_mode;
1768
1769 if (emode == STLINK_MODE_UNKNOWN) {
1770 LOG_ERROR("selected mode (transport) not supported");
1771 return ERROR_FAIL;
1772 }
1773
1774 /* set the speed before entering the mode, as the chip discovery phase should be done at this speed too */
1775 if (emode == STLINK_MODE_DEBUG_JTAG) {
1776 if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
1777 stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
1778 stlink_speed(h, initial_interface_speed, false);
1779 }
1780 } else if (emode == STLINK_MODE_DEBUG_SWD) {
1781 if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
1782 stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
1783 stlink_speed(h, initial_interface_speed, false);
1784 }
1785 }
1786
1787 if (h->version.jtag_api == STLINK_JTAG_API_V3 &&
1788 (emode == STLINK_MODE_DEBUG_JTAG || emode == STLINK_MODE_DEBUG_SWD)) {
1789 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
1790
1791 stlink_get_com_freq(h, (emode == STLINK_MODE_DEBUG_JTAG), map);
1792 stlink_dump_speed_map(map, ARRAY_SIZE(map));
1793 stlink_speed(h, initial_interface_speed, false);
1794 }
1795
1796 /* preliminary SRST assert:
1797 * We want SRST is asserted before activating debug signals (mode_enter).
1798 * As the required mode has not been set, the adapter may not know what pin to use.
1799 * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
1800 * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
1801 * after power on, SWIM_RST stays unchanged */
1802 if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
1803 stlink_usb_assert_srst(handle, 0);
1804 /* do not check the return status here, we will
1805 proceed and enter the desired mode below
1806 and try asserting srst again. */
1807
1808 res = stlink_usb_mode_enter(handle, emode);
1809 if (res != ERROR_OK)
1810 return res;
1811
1812 /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
1813 if (connect_under_reset) {
1814 res = stlink_usb_assert_srst(handle, 0);
1815 if (res != ERROR_OK)
1816 return res;
1817 }
1818
1819 res = stlink_usb_current_mode(handle, &mode);
1820
1821 if (res != ERROR_OK)
1822 return res;
1823
1824 LOG_DEBUG("MODE: 0x%02X", mode);
1825
1826 return ERROR_OK;
1827 }
1828
1829 /* request status from last swim request */
1830 static int stlink_swim_status(void *handle)
1831 {
1832 struct stlink_usb_handle_s *h = handle;
1833 int res;
1834
1835 stlink_usb_init_buffer(handle, h->rx_ep, 4);
1836 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1837 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
1838 /* error is checked by the caller */
1839 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1840 if (res != ERROR_OK)
1841 return res;
1842 return ERROR_OK;
1843 }
1844 /*
1845 the purpose of this function is unknown...
1846 capabilities? anyway for swim v6 it returns
1847 0001020600000000
1848 */
1849 __attribute__((unused))
1850 static int stlink_swim_cap(void *handle, uint8_t *cap)
1851 {
1852 struct stlink_usb_handle_s *h = handle;
1853 int res;
1854
1855 stlink_usb_init_buffer(handle, h->rx_ep, 8);
1856 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1857 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
1858 h->cmdbuf[h->cmdidx++] = 0x01;
1859 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1860 if (res != ERROR_OK)
1861 return res;
1862 memcpy(cap, h->databuf, 8);
1863 return ERROR_OK;
1864 }
1865
1866 /* debug dongle assert/deassert sreset line */
1867 static int stlink_swim_assert_reset(void *handle, int reset)
1868 {
1869 struct stlink_usb_handle_s *h = handle;
1870 int res;
1871
1872 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1873 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1874 if (!reset)
1875 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
1876 else
1877 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
1878 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1879 if (res != ERROR_OK)
1880 return res;
1881 return ERROR_OK;
1882 }
1883
1884 /*
1885 send swim enter seq
1886 1.3ms low then 750Hz then 1.5kHz
1887 */
1888 static int stlink_swim_enter(void *handle)
1889 {
1890 struct stlink_usb_handle_s *h = handle;
1891 int res;
1892
1893 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1894 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1895 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
1896 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1897 if (res != ERROR_OK)
1898 return res;
1899 return ERROR_OK;
1900 }
1901
1902 /* switch high/low speed swim */
1903 static int stlink_swim_speed(void *handle, int speed)
1904 {
1905 struct stlink_usb_handle_s *h = handle;
1906 int res;
1907
1908 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1909 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1910 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
1911 if (speed)
1912 h->cmdbuf[h->cmdidx++] = 1;
1913 else
1914 h->cmdbuf[h->cmdidx++] = 0;
1915 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1916 if (res != ERROR_OK)
1917 return res;
1918 return ERROR_OK;
1919 }
1920
1921 /*
1922 initiate srst from swim.
1923 nrst is pulled low for 50us.
1924 */
1925 static int stlink_swim_generate_rst(void *handle)
1926 {
1927 struct stlink_usb_handle_s *h = handle;
1928 int res;
1929
1930 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1931 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1932 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
1933 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1934 if (res != ERROR_OK)
1935 return res;
1936 return ERROR_OK;
1937 }
1938
1939 /*
1940 send resynchronize sequence
1941 swim is pulled low for 16us
1942 reply is 64 clks low
1943 */
1944 static int stlink_swim_resync(void *handle)
1945 {
1946 struct stlink_usb_handle_s *h = handle;
1947 int res;
1948
1949 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1950 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1951 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
1952 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1953 if (res != ERROR_OK)
1954 return res;
1955 return ERROR_OK;
1956 }
1957
1958 static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
1959 {
1960 struct stlink_usb_handle_s *h = handle;
1961 int res;
1962 unsigned int i;
1963 unsigned int datalen = 0;
1964 int cmdsize = STLINK_CMD_SIZE_V2;
1965
1966 if (len > STLINK_SWIM_DATA_SIZE)
1967 return ERROR_FAIL;
1968
1969 if (h->version.stlink == 1)
1970 cmdsize = STLINK_SG_SIZE;
1971
1972 stlink_usb_init_buffer(handle, h->tx_ep, 0);
1973 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1974 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
1975 h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1976 h->cmdidx += 2;
1977 h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1978 h->cmdidx += 4;
1979 for (i = 0; i < len; i++) {
1980 if (h->cmdidx == cmdsize)
1981 h->databuf[datalen++] = *(data++);
1982 else
1983 h->cmdbuf[h->cmdidx++] = *(data++);
1984 }
1985 if (h->version.stlink == 1)
1986 stlink_usb_set_cbw_transfer_datalength(handle, datalen);
1987
1988 res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
1989 if (res != ERROR_OK)
1990 return res;
1991 return ERROR_OK;
1992 }
1993
1994 static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
1995 {
1996 struct stlink_usb_handle_s *h = handle;
1997 int res;
1998
1999 if (len > STLINK_SWIM_DATA_SIZE)
2000 return ERROR_FAIL;
2001
2002 stlink_usb_init_buffer(handle, h->rx_ep, 0);
2003 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
2004 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
2005 h_u16_to_be(h->cmdbuf+h->cmdidx, len);
2006 h->cmdidx += 2;
2007 h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
2008 h->cmdidx += 4;
2009 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
2010 if (res != ERROR_OK)
2011 return res;
2012
2013 stlink_usb_init_buffer(handle, h->rx_ep, len);
2014 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
2015 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
2016 res = stlink_usb_xfer_noerrcheck(handle, data, len);
2017 if (res != ERROR_OK)
2018 return res;
2019
2020 return ERROR_OK;
2021 }
2022
2023 /** */
2024 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
2025 {
2026 int res, offset;
2027 struct stlink_usb_handle_s *h = handle;
2028
2029 assert(handle);
2030
2031 /* there is no swim read core id cmd */
2032 if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
2033 *idcode = 0;
2034 return ERROR_OK;
2035 }
2036
2037 stlink_usb_init_buffer(handle, h->rx_ep, 12);
2038
2039 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2040 if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2041 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
2042
2043 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
2044 offset = 0;
2045 } else {
2046 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_IDCODES;
2047
2048 res = stlink_usb_xfer_errcheck(handle, h->databuf, 12);
2049 offset = 4;
2050 }
2051
2052 if (res != ERROR_OK)
2053 return res;
2054
2055 *idcode = le_to_h_u32(h->databuf + offset);
2056
2057 LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
2058
2059 return ERROR_OK;
2060 }
2061
2062 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
2063 {
2064 struct stlink_usb_handle_s *h = handle;
2065 int res;
2066
2067 assert(handle);
2068
2069 stlink_usb_init_buffer(handle, h->rx_ep, 8);
2070
2071 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2072 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
2073 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2074 h->cmdidx += 4;
2075
2076 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
2077 if (res != ERROR_OK)
2078 return res;
2079
2080 *val = le_to_h_u32(h->databuf + 4);
2081 return ERROR_OK;
2082 }
2083
2084 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
2085 {
2086 struct stlink_usb_handle_s *h = handle;
2087
2088 assert(handle);
2089
2090 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2091
2092 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2093 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2094 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
2095 else
2096 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
2097 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2098 h->cmdidx += 4;
2099 h_u32_to_le(h->cmdbuf+h->cmdidx, val);
2100 h->cmdidx += 4;
2101
2102 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2103 }
2104
2105 /** */
2106 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
2107 {
2108 struct stlink_usb_handle_s *h = handle;
2109
2110 assert(handle);
2111
2112 if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
2113 int res;
2114
2115 stlink_usb_init_buffer(handle, h->rx_ep, 10);
2116
2117 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2118 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
2119
2120 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
2121 if (res != ERROR_OK)
2122 return res;
2123
2124 size_t bytes_avail = le_to_h_u16(h->databuf);
2125 *size = bytes_avail < *size ? bytes_avail : *size;
2126
2127 if (*size > 0) {
2128 res = stlink_usb_read_trace(handle, buf, *size);
2129 if (res != ERROR_OK)
2130 return res;
2131 return ERROR_OK;
2132 }
2133 }
2134 *size = 0;
2135 return ERROR_OK;
2136 }
2137
2138 static enum target_state stlink_usb_v2_get_status(void *handle)
2139 {
2140 int result;
2141 uint32_t status;
2142
2143 result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
2144 if (result != ERROR_OK)
2145 return TARGET_UNKNOWN;
2146
2147 if (status & S_HALT)
2148 return TARGET_HALTED;
2149 else if (status & S_RESET_ST)
2150 return TARGET_RESET;
2151
2152 return TARGET_RUNNING;
2153 }
2154
2155 /** */
2156 static enum target_state stlink_usb_state(void *handle)
2157 {
2158 int res;
2159 struct stlink_usb_handle_s *h = handle;
2160
2161 assert(handle);
2162
2163 if (h->reconnect_pending) {
2164 LOG_INFO("Previous state query failed, trying to reconnect");
2165 res = stlink_usb_mode_enter(handle, h->st_mode);
2166 if (res != ERROR_OK)
2167 return TARGET_UNKNOWN;
2168
2169 h->reconnect_pending = false;
2170 }
2171
2172 if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2173 res = stlink_usb_v2_get_status(handle);
2174 if (res == TARGET_UNKNOWN)
2175 h->reconnect_pending = true;
2176 return res;
2177 }
2178
2179 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2180
2181 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2182 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
2183
2184 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
2185
2186 if (res != ERROR_OK)
2187 return TARGET_UNKNOWN;
2188
2189 if (h->databuf[0] == STLINK_CORE_RUNNING)
2190 return TARGET_RUNNING;
2191 if (h->databuf[0] == STLINK_CORE_HALTED)
2192 return TARGET_HALTED;
2193
2194 h->reconnect_pending = true;
2195
2196 return TARGET_UNKNOWN;
2197 }
2198
2199 static int stlink_usb_assert_srst(void *handle, int srst)
2200 {
2201 struct stlink_usb_handle_s *h = handle;
2202
2203 assert(handle);
2204
2205 if (h->st_mode == STLINK_MODE_DEBUG_SWIM)
2206 return stlink_swim_assert_reset(handle, srst);
2207
2208 if (h->version.stlink == 1)
2209 return ERROR_COMMAND_NOTFOUND;
2210
2211 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2212
2213 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2214 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
2215 h->cmdbuf[h->cmdidx++] = srst;
2216
2217 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2218 }
2219
2220 /** */
2221 static void stlink_usb_trace_disable(void *handle)
2222 {
2223 int res = ERROR_OK;
2224 struct stlink_usb_handle_s *h = handle;
2225
2226 assert(handle);
2227
2228 assert(h->version.flags & STLINK_F_HAS_TRACE);
2229
2230 LOG_DEBUG("Tracing: disable");
2231
2232 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2233 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2234 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
2235 res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2236
2237 if (res == ERROR_OK)
2238 h->trace.enabled = false;
2239 }
2240
2241
2242 /** */
2243 static int stlink_usb_trace_enable(void *handle)
2244 {
2245 int res;
2246 struct stlink_usb_handle_s *h = handle;
2247
2248 assert(handle);
2249
2250 if (h->version.flags & STLINK_F_HAS_TRACE) {
2251 stlink_usb_init_buffer(handle, h->rx_ep, 10);
2252
2253 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2254 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
2255 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
2256 h->cmdidx += 2;
2257 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
2258 h->cmdidx += 4;
2259
2260 res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2261
2262 if (res == ERROR_OK) {
2263 h->trace.enabled = true;
2264 LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
2265 }
2266 } else {
2267 LOG_ERROR("Tracing is not supported by this version.");
2268 res = ERROR_FAIL;
2269 }
2270
2271 return res;
2272 }
2273
2274 /** */
2275 static int stlink_usb_reset(void *handle)
2276 {
2277 struct stlink_usb_handle_s *h = handle;
2278 int retval;
2279
2280 assert(handle);
2281
2282 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2283
2284 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2285
2286 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2287 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
2288 else
2289 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
2290
2291 retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
2292 if (retval != ERROR_OK)
2293 return retval;
2294
2295 if (h->trace.enabled) {
2296 stlink_usb_trace_disable(h);
2297 return stlink_usb_trace_enable(h);
2298 }
2299
2300 return ERROR_OK;
2301 }
2302
2303 /** */
2304 static int stlink_usb_run(void *handle)
2305 {
2306 int res;
2307 struct stlink_usb_handle_s *h = handle;
2308
2309 assert(handle);
2310
2311 if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2312 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
2313
2314 return res;
2315 }
2316
2317 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2318
2319 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2320 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
2321
2322 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2323 }
2324
2325 /** */
2326 static int stlink_usb_halt(void *handle)
2327 {
2328 int res;
2329 struct stlink_usb_handle_s *h = handle;
2330
2331 assert(handle);
2332
2333 if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2334 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
2335
2336 return res;
2337 }
2338
2339 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2340
2341 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2342 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
2343
2344 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2345 }
2346
2347 /** */
2348 static int stlink_usb_step(void *handle)
2349 {
2350 struct stlink_usb_handle_s *h = handle;
2351
2352 assert(handle);
2353
2354 if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2355 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
2356 * that the Cortex-M3 currently does. */
2357 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
2358 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
2359 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
2360 }
2361
2362 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2363
2364 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2365 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
2366
2367 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2368 }
2369
2370 /** */
2371 static int stlink_usb_read_regs(void *handle)
2372 {
2373 int res;
2374 struct stlink_usb_handle_s *h = handle;
2375
2376 assert(handle);
2377
2378 stlink_usb_init_buffer(handle, h->rx_ep, 88);
2379
2380 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2381 if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2382
2383 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
2384 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 84);
2385 /* regs data from offset 0 */
2386 } else {
2387 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
2388 res = stlink_usb_xfer_errcheck(handle, h->databuf, 88);
2389 /* status at offset 0, regs data from offset 4 */
2390 }
2391
2392 return res;
2393 }
2394
2395 /** */
2396 static int stlink_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
2397 {
2398 int res;
2399 struct stlink_usb_handle_s *h = handle;
2400
2401 assert(handle);
2402
2403 if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
2404 res = stlink_usb_write_debug_reg(h, DCB_DCRSR, regsel & 0x7f);
2405 if (res != ERROR_OK)
2406 return res;
2407
2408 /* FIXME: poll DHCSR.S_REGRDY before read DCRDR */
2409 return stlink_usb_v2_read_debug_reg(h, DCB_DCRDR, val);
2410 }
2411
2412 stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
2413
2414 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2415 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2416 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
2417 else
2418 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
2419 h->cmdbuf[h->cmdidx++] = regsel;
2420
2421 if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2422 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
2423 if (res != ERROR_OK)
2424 return res;
2425 *val = le_to_h_u32(h->databuf);
2426 return ERROR_OK;
2427 } else {
2428 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
2429 if (res != ERROR_OK)
2430 return res;
2431 *val = le_to_h_u32(h->databuf + 4);
2432 return ERROR_OK;
2433 }
2434 }
2435
2436 /** */
2437 static int stlink_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
2438 {
2439 struct stlink_usb_handle_s *h = handle;
2440
2441 assert(handle);
2442
2443 if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
2444 int res = stlink_usb_write_debug_reg(h, DCB_DCRDR, val);
2445 if (res != ERROR_OK)
2446 return res;
2447
2448 return stlink_usb_write_debug_reg(h, DCB_DCRSR, DCRSR_WNR | (regsel & 0x7f));
2449 /* FIXME: poll DHCSR.S_REGRDY after write DCRSR */
2450 }
2451
2452 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2453
2454 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2455 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2456 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
2457 else
2458 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
2459 h->cmdbuf[h->cmdidx++] = regsel;
2460 h_u32_to_le(h->cmdbuf+h->cmdidx, val);
2461 h->cmdidx += 4;
2462
2463 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2464 }
2465
2466 static int stlink_usb_get_rw_status(void *handle)
2467 {
2468 struct stlink_usb_handle_s *h = handle;
2469
2470 assert(handle);
2471
2472 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2473 return ERROR_OK;
2474
2475 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2476
2477 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2478 if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
2479 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
2480 return stlink_usb_xfer_errcheck(handle, h->databuf, 12);
2481 } else {
2482 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
2483 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2484 }
2485 }
2486
2487 /** */
2488 static int stlink_usb_read_mem8(void *handle, uint8_t ap_num, uint32_t csw,
2489 uint32_t addr, uint16_t len, uint8_t *buffer)
2490 {
2491 int res;
2492 uint16_t read_len = len;
2493 struct stlink_usb_handle_s *h = handle;
2494
2495 assert(handle);
2496
2497 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2498 return ERROR_COMMAND_NOTFOUND;
2499
2500 /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2501 if (len > stlink_usb_block(h)) {
2502 LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
2503 return ERROR_FAIL;
2504 }
2505
2506 stlink_usb_init_buffer(handle, h->rx_ep, read_len);
2507
2508 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2509 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
2510 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2511 h->cmdidx += 4;
2512 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2513 h->cmdidx += 2;
2514 h->cmdbuf[h->cmdidx++] = ap_num;
2515 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2516 h->cmdidx += 3;
2517
2518 /* we need to fix read length for single bytes */
2519 if (read_len == 1)
2520 read_len++;
2521
2522 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, read_len);
2523
2524 if (res != ERROR_OK)
2525 return res;
2526
2527 memcpy(buffer, h->databuf, len);
2528
2529 return stlink_usb_get_rw_status(handle);
2530 }
2531
2532 /** */
2533 static int stlink_usb_write_mem8(void *handle, uint8_t ap_num, uint32_t csw,
2534 uint32_t addr, uint16_t len, const uint8_t *buffer)
2535 {
2536 int res;
2537 struct stlink_usb_handle_s *h = handle;
2538
2539 assert(handle);
2540
2541 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2542 return ERROR_COMMAND_NOTFOUND;
2543
2544 /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2545 if (len > stlink_usb_block(h)) {
2546 LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
2547 return ERROR_FAIL;
2548 }
2549
2550 stlink_usb_init_buffer(handle, h->tx_ep, len);
2551
2552 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2553 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
2554 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2555 h->cmdidx += 4;
2556 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2557 h->cmdidx += 2;
2558 h->cmdbuf[h->cmdidx++] = ap_num;
2559 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2560 h->cmdidx += 3;
2561
2562 res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2563
2564 if (res != ERROR_OK)
2565 return res;
2566
2567 return stlink_usb_get_rw_status(handle);
2568 }
2569
2570 /** */
2571 static int stlink_usb_read_mem16(void *handle, uint8_t ap_num, uint32_t csw,
2572 uint32_t addr, uint16_t len, uint8_t *buffer)
2573 {
2574 int res;
2575 struct stlink_usb_handle_s *h = handle;
2576
2577 assert(handle);
2578
2579 if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2580 return ERROR_COMMAND_NOTFOUND;
2581
2582 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2583 return ERROR_COMMAND_NOTFOUND;
2584
2585 if (len > STLINK_MAX_RW16_32) {
2586 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2587 return ERROR_FAIL;
2588 }
2589
2590 /* data must be a multiple of 2 and half-word aligned */
2591 if (len % 2 || addr % 2) {
2592 LOG_DEBUG("Invalid data alignment");
2593 return ERROR_TARGET_UNALIGNED_ACCESS;
2594 }
2595
2596 stlink_usb_init_buffer(handle, h->rx_ep, len);
2597
2598 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2599 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
2600 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2601 h->cmdidx += 4;
2602 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2603 h->cmdidx += 2;
2604 h->cmdbuf[h->cmdidx++] = ap_num;
2605 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2606 h->cmdidx += 3;
2607
2608 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2609
2610 if (res != ERROR_OK)
2611 return res;
2612
2613 memcpy(buffer, h->databuf, len);
2614
2615 return stlink_usb_get_rw_status(handle);
2616 }
2617
2618 /** */
2619 static int stlink_usb_write_mem16(void *handle, uint8_t ap_num, uint32_t csw,
2620 uint32_t addr, uint16_t len, const uint8_t *buffer)
2621 {
2622 int res;
2623 struct stlink_usb_handle_s *h = handle;
2624
2625 assert(handle);
2626
2627 if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2628 return ERROR_COMMAND_NOTFOUND;
2629
2630 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2631 return ERROR_COMMAND_NOTFOUND;
2632
2633 if (len > STLINK_MAX_RW16_32) {
2634 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2635 return ERROR_FAIL;
2636 }
2637
2638 /* data must be a multiple of 2 and half-word aligned */
2639 if (len % 2 || addr % 2) {
2640 LOG_DEBUG("Invalid data alignment");
2641 return ERROR_TARGET_UNALIGNED_ACCESS;
2642 }
2643
2644 stlink_usb_init_buffer(handle, h->tx_ep, len);
2645
2646 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2647 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
2648 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2649 h->cmdidx += 4;
2650 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2651 h->cmdidx += 2;
2652 h->cmdbuf[h->cmdidx++] = ap_num;
2653 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2654 h->cmdidx += 3;
2655
2656 res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2657
2658 if (res != ERROR_OK)
2659 return res;
2660
2661 return stlink_usb_get_rw_status(handle);
2662 }
2663
2664 /** */
2665 static int stlink_usb_read_mem32(void *handle, uint8_t ap_num, uint32_t csw,
2666 uint32_t addr, uint16_t len, uint8_t *buffer)
2667 {
2668 int res;
2669 struct stlink_usb_handle_s *h = handle;
2670
2671 assert(handle);
2672
2673 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2674 return ERROR_COMMAND_NOTFOUND;
2675
2676 if (len > STLINK_MAX_RW16_32) {
2677 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2678 return ERROR_FAIL;
2679 }
2680
2681 /* data must be a multiple of 4 and word aligned */
2682 if (len % 4 || addr % 4) {
2683 LOG_DEBUG("Invalid data alignment");
2684 return ERROR_TARGET_UNALIGNED_ACCESS;
2685 }
2686
2687 stlink_usb_init_buffer(handle, h->rx_ep, len);
2688
2689 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2690 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
2691 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2692 h->cmdidx += 4;
2693 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2694 h->cmdidx += 2;
2695 h->cmdbuf[h->cmdidx++] = ap_num;
2696 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2697 h->cmdidx += 3;
2698
2699 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2700
2701 if (res != ERROR_OK)
2702 return res;
2703
2704 memcpy(buffer, h->databuf, len);
2705
2706 return stlink_usb_get_rw_status(handle);
2707 }
2708
2709 /** */
2710 static int stlink_usb_write_mem32(void *handle, uint8_t ap_num, uint32_t csw,
2711 uint32_t addr, uint16_t len, const uint8_t *buffer)
2712 {
2713 int res;
2714 struct stlink_usb_handle_s *h = handle;
2715
2716 assert(handle);
2717
2718 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2719 return ERROR_COMMAND_NOTFOUND;
2720
2721 if (len > STLINK_MAX_RW16_32) {
2722 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2723 return ERROR_FAIL;
2724 }
2725
2726 /* data must be a multiple of 4 and word aligned */
2727 if (len % 4 || addr % 4) {
2728 LOG_DEBUG("Invalid data alignment");
2729 return ERROR_TARGET_UNALIGNED_ACCESS;
2730 }
2731
2732 stlink_usb_init_buffer(handle, h->tx_ep, len);
2733
2734 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2735 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
2736 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2737 h->cmdidx += 4;
2738 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2739 h->cmdidx += 2;
2740 h->cmdbuf[h->cmdidx++] = ap_num;
2741 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2742 h->cmdidx += 3;
2743
2744 res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2745
2746 if (res != ERROR_OK)
2747 return res;
2748
2749 return stlink_usb_get_rw_status(handle);
2750 }
2751
2752 static int stlink_usb_read_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t csw,
2753 uint32_t addr, uint16_t len, uint8_t *buffer)
2754 {
2755 struct stlink_usb_handle_s *h = handle;
2756
2757 assert(handle != NULL);
2758
2759 if (!(h->version.flags & STLINK_F_HAS_MEM_RD_NO_INC))
2760 return ERROR_COMMAND_NOTFOUND;
2761
2762 if (len > STLINK_MAX_RW16_32) {
2763 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2764 return ERROR_FAIL;
2765 }
2766
2767 /* data must be a multiple of 4 and word aligned */
2768 if (len % 4 || addr % 4) {
2769 LOG_DEBUG("Invalid data alignment");
2770 return ERROR_TARGET_UNALIGNED_ACCESS;
2771 }
2772
2773 stlink_usb_init_buffer(handle, h->rx_ep, len);
2774
2775 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2776 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC;
2777 h_u32_to_le(h->cmdbuf + h->cmdidx, addr);
2778 h->cmdidx += 4;
2779 h_u16_to_le(h->cmdbuf + h->cmdidx, len);
2780 h->cmdidx += 2;
2781 h->cmdbuf[h->cmdidx++] = ap_num;
2782 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2783 h->cmdidx += 3;
2784
2785 int retval = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2786 if (retval != ERROR_OK)
2787 return retval;
2788
2789 memcpy(buffer, h->databuf, len);
2790
2791 return stlink_usb_get_rw_status(handle);
2792 }
2793
2794 static int stlink_usb_write_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t csw,
2795 uint32_t addr, uint16_t len, const uint8_t *buffer)
2796 {
2797 struct stlink_usb_handle_s *h = handle;
2798
2799 assert(handle != NULL);
2800
2801 if (!(h->version.flags & STLINK_F_HAS_MEM_WR_NO_INC))
2802 return ERROR_COMMAND_NOTFOUND;
2803
2804 if (len > STLINK_MAX_RW16_32) {
2805 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2806 return ERROR_FAIL;
2807 }
2808
2809 /* data must be a multiple of 4 and word aligned */
2810 if (len % 4 || addr % 4) {
2811 LOG_DEBUG("Invalid data alignment");
2812 return ERROR_TARGET_UNALIGNED_ACCESS;
2813 }
2814
2815 stlink_usb_init_buffer(handle, h->tx_ep, len);
2816
2817 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2818 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC;
2819 h_u32_to_le(h->cmdbuf + h->cmdidx, addr);
2820 h->cmdidx += 4;
2821 h_u16_to_le(h->cmdbuf + h->cmdidx, len);
2822 h->cmdidx += 2;
2823 h->cmdbuf[h->cmdidx++] = ap_num;
2824 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2825 h->cmdidx += 3;
2826
2827 int retval = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2828 if (retval != ERROR_OK)
2829 return retval;
2830
2831 return stlink_usb_get_rw_status(handle);
2832 }
2833
2834 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
2835 {
2836 uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
2837 if (max_tar_block == 0)
2838 max_tar_block = 4;
2839 return max_tar_block;
2840 }
2841
2842 static int stlink_usb_read_ap_mem(void *handle, uint8_t ap_num, uint32_t csw,
2843 uint32_t addr, uint32_t size, uint32_t count, uint8_t *buffer)
2844 {
2845 int retval = ERROR_OK;
2846 uint32_t bytes_remaining;
2847 int retries = 0;
2848 struct stlink_usb_handle_s *h = handle;
2849
2850 /* calculate byte count */
2851 count *= size;
2852
2853 /* switch to 8 bit if stlink does not support 16 bit memory read */
2854 if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2855 size = 1;
2856
2857 while (count) {
2858 bytes_remaining = (size != 1) ?
2859 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2860
2861 if (count < bytes_remaining)
2862 bytes_remaining = count;
2863
2864 /*
2865 * all stlink support 8/32bit memory read/writes and only from
2866 * stlink V2J26 there is support for 16 bit memory read/write.
2867 * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2868 * as 8bit access.
2869 */
2870 if (size != 1) {
2871 /* When in jtag mode the stlink uses the auto-increment functionality.
2872 * However it expects us to pass the data correctly, this includes
2873 * alignment and any page boundaries. We already do this as part of the
2874 * adi_v5 implementation, but the stlink is a hla adapter and so this
2875 * needs implementing manually.
2876 * currently this only affects jtag mode, according to ST they do single
2877 * access in SWD mode - but this may change and so we do it for both modes */
2878
2879 /* we first need to check for any unaligned bytes */
2880 if (addr & (size - 1)) {
2881 uint32_t head_bytes = size - (addr & (size - 1));
2882 retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, head_bytes, buffer);
2883 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2884 usleep((1 << retries++) * 1000);
2885 continue;
2886 }
2887 if (retval != ERROR_OK)
2888 return retval;
2889 buffer += head_bytes;
2890 addr += head_bytes;
2891 count -= head_bytes;
2892 bytes_remaining -= head_bytes;
2893 }
2894
2895 if (bytes_remaining & (size - 1))
2896 retval = stlink_usb_read_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer);
2897 else if (size == 2)
2898 retval = stlink_usb_read_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
2899 else
2900 retval = stlink_usb_read_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer);
2901 } else {
2902 retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer);
2903 }
2904
2905 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2906 usleep((1 << retries++) * 1000);
2907 continue;
2908 }
2909 if (retval != ERROR_OK)
2910 return retval;
2911
2912 buffer += bytes_remaining;
2913 addr += bytes_remaining;
2914 count -= bytes_remaining;
2915 }
2916
2917 return retval;
2918 }
2919
2920 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
2921 uint32_t count, uint8_t *buffer)
2922 {
2923 return stlink_usb_read_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW,
2924 addr, size, count, buffer);
2925 }
2926
2927 static int stlink_usb_write_ap_mem(void *handle, uint8_t ap_num, uint32_t csw,
2928 uint32_t addr, uint32_t size, uint32_t count, const uint8_t *buffer)
2929 {
2930 int retval = ERROR_OK;
2931 uint32_t bytes_remaining;
2932 int retries = 0;
2933 struct stlink_usb_handle_s *h = handle;
2934
2935 /* calculate byte count */
2936 count *= size;
2937
2938 /* switch to 8 bit if stlink does not support 16 bit memory read */
2939 if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2940 size = 1;
2941
2942 while (count) {
2943
2944 bytes_remaining = (size != 1) ?
2945 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2946
2947 if (count < bytes_remaining)
2948 bytes_remaining = count;
2949
2950 /*
2951 * all stlink support 8/32bit memory read/writes and only from
2952 * stlink V2J26 there is support for 16 bit memory read/write.
2953 * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2954 * as 8bit access.
2955 */
2956 if (size != 1) {
2957
2958 /* When in jtag mode the stlink uses the auto-increment functionality.
2959 * However it expects us to pass the data correctly, this includes
2960 * alignment and any page boundaries. We already do this as part of the
2961 * adi_v5 implementation, but the stlink is a hla adapter and so this
2962 * needs implementing manually.
2963 * currently this only affects jtag mode, according to ST they do single
2964 * access in SWD mode - but this may change and so we do it for both modes */
2965
2966 /* we first need to check for any unaligned bytes */
2967 if (addr & (size - 1)) {
2968
2969 uint32_t head_bytes = size - (addr & (size - 1));
2970 retval = stlink_usb_write_mem8(handle, ap_num, csw, addr, head_bytes, buffer);
2971 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2972 usleep((1<<retries++) * 1000);
2973 continue;
2974 }
2975 if (retval != ERROR_OK)
2976 return retval;
2977 buffer += head_bytes;
2978 addr += head_bytes;
2979 count -= head_bytes;
2980 bytes_remaining -= head_bytes;
2981 }
2982
2983 if (bytes_remaining & (size - 1))
2984 retval = stlink_usb_write_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer);
2985 else if (size == 2)
2986 retval = stlink_usb_write_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
2987 else
2988 retval = stlink_usb_write_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer);
2989
2990 } else
2991 retval = stlink_usb_write_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer);
2992 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2993 usleep((1<<retries++) * 1000);
2994 continue;
2995 }
2996 if (retval != ERROR_OK)
2997 return retval;
2998
2999 buffer += bytes_remaining;
3000 addr += bytes_remaining;
3001 count -= bytes_remaining;
3002 }
3003
3004 return retval;
3005 }
3006
3007 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
3008 uint32_t count, const uint8_t *buffer)
3009 {
3010 return stlink_usb_write_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW,
3011 addr, size, count, buffer);
3012 }
3013
3014 /** */
3015 static int stlink_usb_override_target(const char *targetname)
3016 {
3017 return !strcmp(targetname, "cortex_m");
3018 }
3019
3020 static int stlink_speed_swim(void *handle, int khz, bool query)
3021 {
3022 int retval;
3023
3024 /*
3025 we only have low and high speed...
3026 before changing speed the SWIM_CSR HS bit
3027 must be updated
3028 */
3029 if (!query) {
3030 retval = stlink_swim_speed(handle, (khz < SWIM_FREQ_HIGH) ? 0 : 1);
3031 if (retval != ERROR_OK)
3032 LOG_ERROR("Unable to set adapter speed");
3033 }
3034
3035 return (khz < SWIM_FREQ_HIGH) ? SWIM_FREQ_LOW : SWIM_FREQ_HIGH;
3036 }
3037
3038 static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
3039 {
3040 unsigned int i;
3041 int speed_index = -1;
3042 int speed_diff = INT_MAX;
3043 int last_valid_speed = -1;
3044 bool match = true;
3045
3046 for (i = 0; i < map_size; i++) {
3047 if (!map[i].speed)
3048 continue;
3049 last_valid_speed = i;
3050 if (khz == map[i].speed) {
3051 speed_index = i;
3052 break;
3053 } else {
3054 int current_diff = khz - map[i].speed;
3055 /* get abs value for comparison */
3056 current_diff = (current_diff > 0) ? current_diff : -current_diff;
3057 if ((current_diff < speed_diff) && khz >= map[i].speed) {
3058 speed_diff = current_diff;
3059 speed_index = i;
3060 }
3061 }
3062 }
3063
3064 if (speed_index == -1) {
3065 /* this will only be here if we cannot match the slow speed.
3066 * use the slowest speed we support.*/
3067 speed_index = last_valid_speed;
3068 match = false;
3069 } else if (i == map_size)
3070 match = false;
3071
3072 if (!match && query) {
3073 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz",
3074 khz, map[speed_index].speed);
3075 }
3076
3077 return speed_index;
3078 }
3079
3080 static int stlink_speed_swd(void *handle, int khz, bool query)
3081 {
3082 int speed_index;
3083 struct stlink_usb_handle_s *h = handle;
3084
3085 /* old firmware cannot change it */
3086 if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
3087 return khz;
3088
3089 speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
3090 ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
3091
3092 if (!query) {
3093 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
3094 if (result != ERROR_OK) {
3095 LOG_ERROR("Unable to set adapter speed");
3096 return khz;
3097 }
3098 }
3099
3100 return stlink_khz_to_speed_map_swd[speed_index].speed;
3101 }
3102
3103 static int stlink_speed_jtag(void *handle, int khz, bool query)
3104 {
3105 int speed_index;
3106 struct stlink_usb_handle_s *h = handle;
3107
3108 /* old firmware cannot change it */
3109 if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
3110 return khz;
3111
3112 speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
3113 ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
3114
3115 if (!query) {
3116 int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
3117 if (result != ERROR_OK) {
3118 LOG_ERROR("Unable to set adapter speed");
3119 return khz;
3120 }
3121 }
3122
3123 return stlink_khz_to_speed_map_jtag[speed_index].speed;
3124 }
3125
3126 static void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
3127 {
3128 unsigned int i;
3129
3130 LOG_DEBUG("Supported clock speeds are:");
3131 for (i = 0; i < map_size; i++)
3132 if (map[i].speed)
3133 LOG_DEBUG("%d kHz", map[i].speed);
3134 }
3135
3136 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
3137 {
3138 struct stlink_usb_handle_s *h = handle;
3139 int i;
3140
3141 if (h->version.jtag_api != STLINK_JTAG_API_V3) {
3142 LOG_ERROR("Unknown command");
3143 return 0;
3144 }
3145
3146 stlink_usb_init_buffer(handle, h->rx_ep, 16);
3147
3148 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3149 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
3150 h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
3151
3152 int res = stlink_usb_xfer_errcheck(handle, h->databuf, 52);
3153
3154 int size = h->databuf[8];
3155
3156 if (size > STLINK_V3_MAX_FREQ_NB)
3157 size = STLINK_V3_MAX_FREQ_NB;
3158
3159 for (i = 0; i < size; i++) {
3160 map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
3161 map[i].speed_divisor = i;
3162 }
3163
3164 /* set to zero all the next entries */
3165 for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
3166 map[i].speed = 0;
3167
3168 return res;
3169 }
3170
3171 static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
3172 {
3173 struct stlink_usb_handle_s *h = handle;
3174
3175 if (h->version.jtag_api != STLINK_JTAG_API_V3) {
3176 LOG_ERROR("Unknown command");
3177 return 0;
3178 }
3179
3180 stlink_usb_init_buffer(handle, h->rx_ep, 16);
3181
3182 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3183 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
3184 h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
3185 h->cmdbuf[h->cmdidx++] = 0;
3186
3187 h_u32_to_le(&h->cmdbuf[4], frequency);
3188
3189 return stlink_usb_xfer_errcheck(handle, h->databuf, 8);
3190 }
3191
3192 static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
3193 {
3194 struct stlink_usb_handle_s *h = handle;
3195 int speed_index;
3196 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
3197
3198 stlink_get_com_freq(h, is_jtag, map);
3199
3200 speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
3201
3202 if (!query) {
3203 int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
3204 if (result != ERROR_OK) {
3205 LOG_ERROR("Unable to set adapter speed");
3206 return khz;
3207 }
3208 }
3209 return map[speed_index].speed;
3210 }
3211
3212 static int stlink_speed(void *handle, int khz, bool query)
3213 {
3214 struct stlink_usb_handle_s *h = handle;
3215
3216 if (!handle)
3217 return khz;
3218
3219 switch (h->st_mode) {
3220 case STLINK_MODE_DEBUG_SWIM:
3221 return stlink_speed_swim(handle, khz, query);
3222 case STLINK_MODE_DEBUG_SWD:
3223 if (h->version.jtag_api == STLINK_JTAG_API_V3)
3224 return stlink_speed_v3(handle, false, khz, query);
3225 else
3226 return stlink_speed_swd(handle, khz, query);
3227 break;
3228 case STLINK_MODE_DEBUG_JTAG:
3229 if (h->version.jtag_api == STLINK_JTAG_API_V3)
3230 return stlink_speed_v3(handle, true, khz, query);
3231 else
3232 return stlink_speed_jtag(handle, khz, query);
3233 break;
3234 default:
3235 break;
3236 }
3237
3238 return khz;
3239 }
3240
3241 /** */
3242 static int stlink_usb_usb_close(void *handle)
3243 {
3244 struct stlink_usb_handle_s *h = handle;
3245
3246 if (!h)
3247 return ERROR_OK;
3248
3249 if (h->usb_backend_priv.fd) {
3250 stlink_usb_exit_mode(h);
3251 /* do not check return code, it prevent
3252 us from closing jtag_libusb */
3253 jtag_libusb_close(h->usb_backend_priv.fd);
3254 }
3255
3256 free(h->cmdbuf);
3257 free(h->databuf);
3258
3259 return ERROR_OK;
3260 }
3261
3262 /** */
3263 static int stlink_tcp_close(void *handle)
3264 {
3265 struct stlink_usb_handle_s *h = handle;
3266
3267 if (!h)
3268 return ERROR_OK;
3269
3270 int ret = ERROR_OK;
3271 if (h->tcp_backend_priv.connected) {
3272 if (h->tcp_backend_priv.connect_id) {
3273 stlink_usb_exit_mode(h);
3274
3275 /* close the stlink */
3276 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_CLOSE_DEV;
3277 memset(&h->tcp_backend_priv.send_buf[1], 0, 4); /* reserved */
3278 h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.connect_id);
3279 ret = stlink_tcp_send_cmd(h, 8, 4, true);
3280 if (ret != ERROR_OK)
3281 LOG_ERROR("cannot close the STLINK");
3282 }
3283
3284 if (close_socket(h->tcp_backend_priv.fd) != 0)
3285 LOG_ERROR("error closing the socket, errno: %s", strerror(errno));
3286 }
3287
3288 free(h->tcp_backend_priv.send_buf);
3289 free(h->tcp_backend_priv.recv_buf);
3290
3291 return ret;
3292 }
3293
3294 /** */
3295 static int stlink_close(void *handle)
3296 {
3297 if (handle) {
3298 struct stlink_usb_handle_s *h = handle;
3299
3300 stlink_usb_close(handle);
3301
3302 free(h);
3303 }
3304
3305 return ERROR_OK;
3306 }
3307
3308 /* Compute ST-Link serial number from the device descriptor
3309 * this function will help to work-around a bug in old ST-Link/V2 DFU
3310 * the buggy DFU returns an incorrect serial in the USB descriptor
3311 * example for the following serial "57FF72067265575742132067"
3312 * - the correct descriptor serial is:
3313 * 0x32, 0x03, 0x35, 0x00, 0x37, 0x00, 0x46, 0x00, 0x46, 0x00, 0x37, 0x00, 0x32, 0x00 ...
3314 * this contains the length (0x32 = 50), the type (0x3 = DT_STRING) and the serial in unicode format
3315 * the serial part is: 0x0035, 0x0037, 0x0046, 0x0046, 0x0037, 0x0032 ... >> 57FF72 ...
3316 * this format could be read correctly by 'libusb_get_string_descriptor_ascii'
3317 * so this case is managed by libusb_helper::string_descriptor_equal
3318 * - the buggy DFU is not doing any unicode conversion and returns a raw serial data in the descriptor
3319 * 0x1a, 0x03, 0x57, 0x00, 0xFF, 0x00, 0x72, 0x00 ...
3320 * >> 57 FF 72 ...
3321 * based on the length (0x1a = 26) we could easily decide if we have to fixup the serial
3322 * and then we have just to convert the raw data into printable characters using sprintf
3323 */
3324 static char *stlink_usb_get_alternate_serial(struct libusb_device_handle *device,
3325 struct libusb_device_descriptor *dev_desc)
3326 {
3327 int usb_retval;
3328 unsigned char desc_serial[(STLINK_SERIAL_LEN + 1) * 2];
3329
3330 if (dev_desc->iSerialNumber == 0)
3331 return NULL;
3332
3333 /* get the LANGID from String Descriptor Zero */
3334 usb_retval = libusb_get_string_descriptor(device, 0, 0, desc_serial,
3335 sizeof(desc_serial));
3336
3337 if (usb_retval < LIBUSB_SUCCESS) {
3338 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
3339 libusb_error_name(usb_retval), usb_retval);
3340 return NULL;
3341 } else if (usb_retval < 4) {
3342 /* the size should be least 4 bytes to contain a minimum of 1 supported LANGID */
3343 LOG_ERROR("could not get the LANGID");
3344 return NULL;
3345 }
3346
3347 uint32_t langid = desc_serial[2] | (desc_serial[3] << 8);
3348
3349 /* get the serial */
3350 usb_retval = libusb_get_string_descriptor(device, dev_desc->iSerialNumber,
3351 langid, desc_serial, sizeof(desc_serial));
3352
3353 unsigned char len = desc_serial[0];
3354
3355 if (usb_retval < LIBUSB_SUCCESS) {
3356 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
3357 libusb_error_name(usb_retval), usb_retval);
3358 return NULL;
3359 } else if (desc_serial[1] != LIBUSB_DT_STRING || len > usb_retval) {
3360 LOG_ERROR("invalid string in ST-LINK USB serial descriptor");
3361 return NULL;
3362 }
3363
3364 if (len == ((STLINK_SERIAL_LEN + 1) * 2)) {
3365 /* good ST-Link adapter, this case is managed by
3366 * libusb::libusb_get_string_descriptor_ascii */
3367 return NULL;
3368 } else if (len != ((STLINK_SERIAL_LEN / 2 + 1) * 2)) {
3369 LOG_ERROR("unexpected serial length (%d) in descriptor", len);
3370 return NULL;
3371 }
3372
3373 /* else (len == 26) => buggy ST-Link */
3374
3375 char *alternate_serial = malloc((STLINK_SERIAL_LEN + 1) * sizeof(char));
3376 if (!alternate_serial)
3377 return NULL;
3378
3379 for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
3380 sprintf(alternate_serial + i, "%02X", desc_serial[i + 2]);
3381
3382 alternate_serial[STLINK_SERIAL_LEN] = '\0';
3383
3384 return alternate_serial;
3385 }
3386
3387 /** */
3388 static int stlink_usb_usb_open(void *handle, struct hl_interface_param_s *param)
3389 {
3390 struct stlink_usb_handle_s *h = handle;
3391 int err, retry_count = 1;
3392
3393 h->cmdbuf = malloc(STLINK_SG_SIZE);
3394 h->databuf = malloc(STLINK_DATA_SIZE);
3395
3396 if (!h->cmdbuf || !h->databuf)
3397 return ERROR_FAIL;
3398
3399 /*
3400 On certain host USB configurations(e.g. MacBook Air)
3401 STLINKv2 dongle seems to have its FW in a funky state if,
3402 after plugging it in, you try to use openocd with it more
3403 then once (by launching and closing openocd). In cases like
3404 that initial attempt to read the FW info via
3405 stlink_usb_version will fail and the device has to be reset
3406 in order to become operational.
3407 */
3408 do {
3409 if (jtag_libusb_open(param->vid, param->pid, NULL,
3410 &h->usb_backend_priv.fd, stlink_usb_get_alternate_serial) != ERROR_OK) {
3411 LOG_ERROR("open failed");
3412 return ERROR_FAIL;
3413 }
3414
3415 jtag_libusb_set_configuration(h->usb_backend_priv.fd, 0);
3416
3417 if (libusb_claim_interface(h->usb_backend_priv.fd, 0) != ERROR_OK) {
3418 LOG_DEBUG("claim interface failed");
3419 return ERROR_FAIL;
3420 }
3421
3422 /* RX EP is common for all versions */
3423 h->rx_ep = STLINK_RX_EP;
3424
3425 uint16_t pid;
3426 if (jtag_libusb_get_pid(libusb_get_device(h->usb_backend_priv.fd), &pid) != ERROR_OK) {
3427 LOG_DEBUG("libusb_get_pid failed");
3428 return ERROR_FAIL;
3429 }
3430
3431 /* wrap version for first read */
3432 switch (pid) {
3433 case STLINK_V1_PID:
3434 h->version.stlink = 1;
3435 h->tx_ep = STLINK_TX_EP;
3436 break;
3437 case STLINK_V3_USBLOADER_PID:
3438 case STLINK_V3E_PID:
3439 case STLINK_V3S_PID:
3440 case STLINK_V3_2VCP_PID:
3441 case STLINK_V3E_NO_MSD_PID:
3442 case STLINK_V3P_USBLOADER_PID:
3443 case STLINK_V3P_PID:
3444 h->version.stlink = 3;
3445 h->tx_ep = STLINK_V2_1_TX_EP;
3446 h->trace_ep = STLINK_V2_1_TRACE_EP;
3447 break;
3448 case STLINK_V2_1_PID:
3449 case STLINK_V2_1_NO_MSD_PID:
3450 h->version.stlink = 2;
3451 h->tx_ep = STLINK_V2_1_TX_EP;
3452 h->trace_ep = STLINK_V2_1_TRACE_EP;
3453 break;
3454 default:
3455 /* fall through - we assume V2 to be the default version*/
3456 case STLINK_V2_PID:
3457 h->version.stlink = 2;
3458 h->tx_ep = STLINK_TX_EP;
3459 h->trace_ep = STLINK_TRACE_EP;
3460 break;
3461 }
3462
3463 /* get the device version */
3464 err = stlink_usb_version(h);
3465
3466 if (err == ERROR_OK) {
3467 break;
3468 } else if (h->version.stlink == 1 ||
3469 retry_count == 0) {
3470 LOG_ERROR("read version failed");
3471 return ERROR_FAIL;
3472 } else {
3473 err = libusb_release_interface(h->usb_backend_priv.fd, 0);
3474 if (err != ERROR_OK) {
3475 LOG_ERROR("release interface failed");
3476 return ERROR_FAIL;
3477 }
3478
3479 err = libusb_reset_device(h->usb_backend_priv.fd);
3480 if (err != ERROR_OK) {
3481 LOG_ERROR("reset device failed");
3482 return ERROR_FAIL;
3483 }
3484
3485 jtag_libusb_close(h->usb_backend_priv.fd);
3486 /*
3487 Give the device one second to settle down and
3488 reenumerate.
3489 */
3490 usleep(1 * 1000 * 1000);
3491 retry_count--;
3492 }
3493 } while (1);
3494
3495 return ERROR_OK;
3496 }
3497
3498 /** */
3499 static int stlink_tcp_open(void *handle, struct hl_interface_param_s *param)
3500 {
3501 struct stlink_usb_handle_s *h = handle;
3502 int ret;
3503
3504 /* SWIM is not supported using stlink-server */
3505 if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
3506 LOG_ERROR("stlink-server does not support SWIM mode");
3507 return ERROR_FAIL;
3508 }
3509
3510 h->tcp_backend_priv.send_buf = malloc(STLINK_TCP_SEND_BUFFER_SIZE);
3511 h->tcp_backend_priv.recv_buf = malloc(STLINK_TCP_RECV_BUFFER_SIZE);
3512
3513 if (!h->tcp_backend_priv.send_buf || !h->tcp_backend_priv.recv_buf)
3514 return ERROR_FAIL;
3515
3516 h->cmdbuf = &h->tcp_backend_priv.send_buf[8];
3517 h->databuf = &h->tcp_backend_priv.recv_buf[4];
3518
3519 /* configure directions */
3520 h->rx_ep = STLINK_TCP_REQUEST_READ;
3521 h->tx_ep = STLINK_TCP_REQUEST_WRITE;
3522 h->trace_ep = STLINK_TCP_REQUEST_READ_SWO;
3523
3524 h->tcp_backend_priv.fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
3525 h->tcp_backend_priv.connected = false;
3526 h->tcp_backend_priv.device_id = 0;
3527 h->tcp_backend_priv.connect_id = 0;
3528
3529 if (h->tcp_backend_priv.fd == -1) {
3530 LOG_ERROR("error creating the socket, errno: %s", strerror(errno));
3531 return ERROR_FAIL;
3532 }
3533
3534 struct sockaddr_in serv;
3535 memset(&serv, 0, sizeof(struct sockaddr_in));
3536 serv.sin_family = AF_INET;
3537 serv.sin_port = htons(param->stlink_tcp_port);
3538 serv.sin_addr.s_addr = inet_addr("127.0.0.1");
3539
3540 LOG_DEBUG("socket : %x", h->tcp_backend_priv.fd);
3541
3542 int optval = 1;
3543 if (setsockopt(h->tcp_backend_priv.fd, IPPROTO_TCP, TCP_NODELAY, (const void *)&optval, sizeof(int)) == -1) {
3544 LOG_ERROR("cannot set sock option 'TCP_NODELAY', errno: %s", strerror(errno));
3545 return ERROR_FAIL;
3546 }
3547
3548 optval = STLINK_TCP_RECV_BUFFER_SIZE;
3549 if (setsockopt(h->tcp_backend_priv.fd, SOL_SOCKET, SO_RCVBUF, (const void *)&optval, sizeof(int)) == -1) {
3550 LOG_ERROR("cannot set sock option 'SO_RCVBUF', errno: %s", strerror(errno));
3551 return ERROR_FAIL;
3552 }
3553
3554 optval = STLINK_TCP_SEND_BUFFER_SIZE;
3555 if (setsockopt(h->tcp_backend_priv.fd, SOL_SOCKET, SO_SNDBUF, (const void *)&optval, sizeof(int)) == -1) {
3556 LOG_ERROR("cannot set sock option 'SO_SNDBUF', errno: %s", strerror(errno));
3557 return ERROR_FAIL;
3558 }
3559
3560 if (connect(h->tcp_backend_priv.fd, (const struct sockaddr *)&serv, sizeof(serv)) == -1) {
3561 LOG_ERROR("cannot connect to stlink server, errno: %s", strerror(errno));
3562 return ERROR_FAIL;
3563 }
3564
3565 h->tcp_backend_priv.connected = true;
3566
3567 LOG_INFO("connected to stlink-server");
3568
3569 /* print stlink-server version */
3570 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_SERVER_VERSION;
3571 h->tcp_backend_priv.send_buf[1] = OPENOCD_STLINK_TCP_API_VERSION;
3572 memset(&h->tcp_backend_priv.send_buf[2], 0, 2); /* reserved */
3573 ret = stlink_tcp_send_cmd(h, 4, 16, false);
3574 if (ret != ERROR_OK) {
3575 LOG_ERROR("cannot get the stlink-server version");
3576 return ERROR_FAIL;
3577 }
3578
3579 h->tcp_backend_priv.version.api = le_to_h_u32(&h->tcp_backend_priv.recv_buf[0]);
3580 h->tcp_backend_priv.version.major = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
3581 h->tcp_backend_priv.version.minor = le_to_h_u32(&h->tcp_backend_priv.recv_buf[8]);
3582 h->tcp_backend_priv.version.build = le_to_h_u32(&h->tcp_backend_priv.recv_buf[12]);
3583 LOG_INFO("stlink-server API v%d, version %d.%d.%d",
3584 h->tcp_backend_priv.version.api,
3585 h->tcp_backend_priv.version.major,
3586 h->tcp_backend_priv.version.minor,
3587 h->tcp_backend_priv.version.build);
3588
3589 /* in stlink-server API v1 sending more than 1428 bytes will cause stlink-server
3590 * to crash in windows: select a safe default value (1K) */
3591 if (h->tcp_backend_priv.version.api < 2)
3592 h->max_mem_packet = (1 << 10);
3593
3594 /* refresh stlink list (re-enumerate) */
3595 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_REFRESH_DEVICE_LIST;
3596 h->tcp_backend_priv.send_buf[1] = 0; /* don't clear the list, just refresh it */
3597 ret = stlink_tcp_send_cmd(h, 2, 4, true);
3598 if (ret != ERROR_OK)
3599 return ret;
3600
3601 /* get the number of connected stlinks */
3602 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_NB_DEV;
3603 ret = stlink_tcp_send_cmd(h, 1, 4, false);
3604 if (ret != ERROR_OK)
3605 return ret;
3606
3607 uint32_t connected_stlinks = le_to_h_u32(h->tcp_backend_priv.recv_buf);
3608
3609 if (connected_stlinks == 0) {
3610 LOG_ERROR("no ST-LINK detected");
3611 return ERROR_FAIL;
3612 }
3613
3614 LOG_DEBUG("%d ST-LINK detected", connected_stlinks);
3615
3616 if (connected_stlinks > 255) {
3617 LOG_WARNING("STLink server cannot handle more than 255 ST-LINK connected");
3618 connected_stlinks = 255;
3619 }
3620
3621 /* list all connected ST-Link and seek for the requested vid:pid and serial */
3622 char serial[STLINK_TCP_SERIAL_SIZE + 1] = {0};
3623 uint8_t stlink_used;
3624 bool stlink_id_matched = false;
3625 const char *adapter_serial = adapter_get_required_serial();
3626 bool stlink_serial_matched = !adapter_serial;
3627
3628 for (uint32_t stlink_id = 0; stlink_id < connected_stlinks; stlink_id++) {
3629 /* get the stlink info */
3630 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_DEV_INFO;
3631 h->tcp_backend_priv.send_buf[1] = (uint8_t)stlink_id;
3632 memset(&h->tcp_backend_priv.send_buf[2], 0, 2); /* reserved */
3633 h_u32_to_le(&h->tcp_backend_priv.send_buf[4], 41); /* size of TDeviceInfo2 */
3634 ret = stlink_tcp_send_cmd(h, 8, 45, true);
3635 if (ret != ERROR_OK)
3636 return ret;
3637
3638 h->tcp_backend_priv.device_id = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
3639 memcpy(serial, &h->tcp_backend_priv.recv_buf[8], STLINK_TCP_SERIAL_SIZE);
3640 h->vid = le_to_h_u16(&h->tcp_backend_priv.recv_buf[40]);
3641 h->pid = le_to_h_u16(&h->tcp_backend_priv.recv_buf[42]);
3642 stlink_used = h->tcp_backend_priv.recv_buf[44];
3643
3644 /* check the vid:pid */
3645 for (int i = 0; param->vid[i]; i++) {
3646 if (param->vid[i] == h->vid && param->pid[i] == h->pid) {
3647 stlink_id_matched = true;
3648 break;
3649 }
3650 }
3651
3652 if (!stlink_id_matched)
3653 continue;
3654
3655 /* check the serial if specified */
3656 if (adapter_serial) {
3657 /* ST-Link server fixes the buggy serial returned by old ST-Link DFU
3658 * for further details refer to stlink_usb_get_alternate_serial
3659 * so if the user passes the buggy serial, we need to fix it before
3660 * comparing with the serial returned by ST-Link server */
3661 if (strlen(adapter_serial) == STLINK_SERIAL_LEN / 2) {
3662 char fixed_serial[STLINK_SERIAL_LEN + 1];
3663
3664 for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
3665 sprintf(fixed_serial + i, "%02X", adapter_serial[i / 2]);
3666
3667 fixed_serial[STLINK_SERIAL_LEN] = '\0';
3668
3669 stlink_serial_matched = strcmp(fixed_serial, serial) == 0;
3670 } else {
3671 stlink_serial_matched = strcmp(adapter_serial, serial) == 0;
3672 }
3673 }
3674
3675 if (!stlink_serial_matched)
3676 LOG_DEBUG("Device serial number '%s' doesn't match requested serial '%s'",
3677 serial, adapter_serial);
3678 else /* exit the search loop if there is match */
3679 break;
3680 }
3681
3682 if (!stlink_id_matched) {
3683 LOG_ERROR("ST-LINK open failed (vid/pid mismatch)");
3684 return ERROR_FAIL;
3685 }
3686
3687 if (!stlink_serial_matched) {
3688 LOG_ERROR("ST-LINK open failed (serial mismatch)");
3689 return ERROR_FAIL;
3690 }
3691
3692 /* check if device is 'exclusively' used by another application */
3693 if (stlink_used) {
3694 LOG_ERROR("the selected device is already used");
3695 return ERROR_FAIL;
3696 }
3697
3698 LOG_DEBUG("transport: vid: 0x%04x pid: 0x%04x serial: %s", h->vid, h->pid, serial);
3699
3700 /* now let's open the stlink */
3701 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_OPEN_DEV;
3702 memset(&h->tcp_backend_priv.send_buf[1], 0, 4); /* reserved */
3703 h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.device_id);
3704 ret = stlink_tcp_send_cmd(h, 8, 8, true);
3705 if (ret != ERROR_OK)
3706 return ret;
3707
3708 h->tcp_backend_priv.connect_id = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
3709
3710 /* get stlink version */
3711 return stlink_usb_version(h);
3712 }
3713
3714 static struct stlink_backend_s stlink_usb_backend = {
3715 .open = stlink_usb_usb_open,
3716 .close = stlink_usb_usb_close,
3717 .xfer_noerrcheck = stlink_usb_usb_xfer_noerrcheck,
3718 .read_trace = stlink_usb_usb_read_trace,
3719 };
3720
3721 static struct stlink_backend_s stlink_tcp_backend = {
3722 .open = stlink_tcp_open,
3723 .close = stlink_tcp_close,
3724 .xfer_noerrcheck = stlink_tcp_xfer_noerrcheck,
3725 .read_trace = stlink_tcp_read_trace,
3726 };
3727
3728 static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode, void **fd)
3729 {
3730 struct stlink_usb_handle_s *h;
3731
3732 LOG_DEBUG("stlink_open");
3733
3734 h = calloc(1, sizeof(struct stlink_usb_handle_s));
3735
3736 if (!h) {
3737 LOG_DEBUG("malloc failed");
3738 return ERROR_FAIL;
3739 }
3740
3741 h->st_mode = mode;
3742
3743 for (unsigned i = 0; param->vid[i]; i++) {
3744 LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
3745 h->st_mode, param->vid[i], param->pid[i],
3746 adapter_get_required_serial() ? adapter_get_required_serial() : "");
3747 }
3748
3749 if (param->use_stlink_tcp)
3750 h->backend = &stlink_tcp_backend;
3751 else
3752 h->backend = &stlink_usb_backend;
3753
3754 if (stlink_usb_open(h, param) != ERROR_OK)
3755 goto error_open;
3756
3757 /* check if mode is supported */
3758 int err = ERROR_OK;
3759
3760 switch (h->st_mode) {
3761 case STLINK_MODE_DEBUG_SWD:
3762 if (h->version.jtag_api == STLINK_JTAG_API_V1)
3763 err = ERROR_FAIL;
3764 /* fall-through */
3765 case STLINK_MODE_DEBUG_JTAG:
3766 if (h->version.jtag == 0)
3767 err = ERROR_FAIL;
3768 break;
3769 case STLINK_MODE_DEBUG_SWIM:
3770 if (h->version.swim == 0)
3771 err = ERROR_FAIL;
3772 break;
3773 default:
3774 err = ERROR_FAIL;
3775 break;
3776 }
3777
3778 if (err != ERROR_OK) {
3779 LOG_ERROR("mode (transport) not supported by device");
3780 goto error_open;
3781 }
3782
3783 /* initialize the debug hardware */
3784 err = stlink_usb_init_mode(h, param->connect_under_reset, param->initial_interface_speed);
3785
3786 if (err != ERROR_OK) {
3787 LOG_ERROR("init mode failed (unable to connect to the target)");
3788 goto error_open;
3789 }
3790
3791 if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
3792 err = stlink_swim_enter(h);
3793 if (err != ERROR_OK) {
3794 LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
3795 goto error_open;
3796 }
3797 *fd = h;
3798 h->max_mem_packet = STLINK_SWIM_DATA_SIZE;
3799 return ERROR_OK;
3800 }
3801
3802 /* set max_mem_packet if it was not set by the low-level interface */
3803 if (h->max_mem_packet == 0) {
3804 /* get cpuid, so we can determine the max page size
3805 * start with a safe default */
3806 h->max_mem_packet = (1 << 10);
3807
3808 uint8_t buffer[4];
3809 stlink_usb_open_ap(h, STLINK_HLA_AP_NUM);
3810 err = stlink_usb_read_mem32(h, STLINK_HLA_AP_NUM, STLINK_HLA_CSW, CPUID, 4, buffer);
3811 if (err == ERROR_OK) {
3812 uint32_t cpuid = le_to_h_u32(buffer);
3813 int i = (cpuid >> 4) & 0xf;
3814 if (i == 4 || i == 3) {
3815 /* Cortex-M3/M4 has 4096 bytes autoincrement range */
3816 h->max_mem_packet = (1 << 12);
3817 }
3818 }
3819
3820 LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
3821 }
3822
3823 *fd = h;
3824
3825 return ERROR_OK;
3826
3827 error_open:
3828 stlink_close(h);
3829 return ERROR_FAIL;
3830 }
3831
3832 static int stlink_usb_hl_open(struct hl_interface_param_s *param, void **fd)
3833 {
3834 return stlink_open(param, stlink_get_mode(param->transport), fd);
3835 }
3836
3837 static int stlink_config_trace(void *handle, bool enabled,
3838 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
3839 unsigned int *trace_freq, unsigned int traceclkin_freq,
3840 uint16_t *prescaler)
3841 {
3842 struct stlink_usb_handle_s *h = handle;
3843
3844 if (!(h->version.flags & STLINK_F_HAS_TRACE)) {
3845 LOG_ERROR("The attached ST-LINK version doesn't support trace");
3846 return ERROR_FAIL;
3847 }
3848
3849 if (!enabled) {
3850 stlink_usb_trace_disable(h);
3851 return ERROR_OK;
3852 }
3853
3854 assert(trace_freq);
3855 assert(prescaler);
3856
3857 if (pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART) {
3858 LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
3859 return ERROR_FAIL;
3860 }
3861
3862 unsigned int max_trace_freq = (h->version.stlink >= 3) ?
3863 STLINK_V3_TRACE_MAX_HZ : STLINK_TRACE_MAX_HZ;
3864
3865 /* Only concern ourselves with the frequency if the STlink is processing it. */
3866 if (*trace_freq > max_trace_freq) {
3867 LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
3868 max_trace_freq);
3869 return ERROR_FAIL;
3870 }
3871
3872 if (!*trace_freq)
3873 *trace_freq = max_trace_freq;
3874
3875 unsigned int presc = (traceclkin_freq + *trace_freq / 2) / *trace_freq;
3876 if (presc == 0 || presc > TPIU_ACPR_MAX_SWOSCALER + 1) {
3877 LOG_ERROR("SWO frequency is not suitable. Please choose a different "
3878 "frequency.");
3879 return ERROR_FAIL;
3880 }
3881
3882 /* Probe's UART speed must be within 3% of the TPIU's SWO baud rate. */
3883 unsigned int max_deviation = (traceclkin_freq * 3) / 100;
3884 if (presc * *trace_freq < traceclkin_freq - max_deviation ||
3885 presc * *trace_freq > traceclkin_freq + max_deviation) {
3886 LOG_ERROR("SWO frequency is not suitable. Please choose a different "
3887 "frequency.");
3888 return ERROR_FAIL;
3889 }
3890
3891 *prescaler = presc;
3892
3893 stlink_usb_trace_disable(h);
3894
3895 h->trace.source_hz = *trace_freq;
3896
3897 return stlink_usb_trace_enable(h);
3898 }
3899
3900 /** */
3901 static int stlink_usb_init_access_port(void *handle, unsigned char ap_num)
3902 {
3903 struct stlink_usb_handle_s *h = handle;
3904
3905 assert(handle);
3906
3907 if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3908 return ERROR_COMMAND_NOTFOUND;
3909
3910 LOG_DEBUG_IO("init ap_num = %d", ap_num);
3911 stlink_usb_init_buffer(handle, h->rx_ep, 16);
3912 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3913 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_INIT_AP;
3914 h->cmdbuf[h->cmdidx++] = ap_num;
3915
3916 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3917 }
3918
3919 /** */
3920 static int stlink_usb_close_access_port(void *handle, unsigned char ap_num)
3921 {
3922 struct stlink_usb_handle_s *h = handle;
3923
3924 assert(handle);
3925
3926 if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3927 return ERROR_COMMAND_NOTFOUND;
3928
3929 LOG_DEBUG_IO("close ap_num = %d", ap_num);
3930 stlink_usb_init_buffer(handle, h->rx_ep, 16);
3931 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3932 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_CLOSE_AP_DBG;
3933 h->cmdbuf[h->cmdidx++] = ap_num;
3934
3935 /* ignore incorrectly returned error on bogus FW */
3936 if (h->version.flags & STLINK_F_FIX_CLOSE_AP)
3937 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3938 else
3939 return stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
3940
3941 }
3942
3943 static int stlink_usb_rw_misc_out(void *handle, uint32_t items, const uint8_t *buffer)
3944 {
3945 struct stlink_usb_handle_s *h = handle;
3946 unsigned int buflen = ALIGN_UP(items, 4) + 4 * items;
3947
3948 LOG_DEBUG_IO("%s(%" PRIu32 ")", __func__, items);
3949
3950 assert(handle != NULL);
3951
3952 if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
3953 return ERROR_COMMAND_NOTFOUND;
3954
3955 stlink_usb_init_buffer(handle, h->tx_ep, buflen);
3956
3957 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3958 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RW_MISC_OUT;
3959 h_u32_to_le(&h->cmdbuf[2], items);
3960
3961 return stlink_usb_xfer_noerrcheck(handle, buffer, buflen);
3962 }
3963
3964 static int stlink_usb_rw_misc_in(void *handle, uint32_t items, uint8_t *buffer)
3965 {
3966 struct stlink_usb_handle_s *h = handle;
3967 unsigned int buflen = 2 * 4 * items;
3968
3969 LOG_DEBUG_IO("%s(%" PRIu32 ")", __func__, items);
3970
3971 assert(handle != NULL);
3972
3973 if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
3974 return ERROR_COMMAND_NOTFOUND;
3975
3976 stlink_usb_init_buffer(handle, h->rx_ep, buflen);
3977
3978 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3979 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RW_MISC_IN;
3980
3981 int res = stlink_usb_xfer_noerrcheck(handle, h->databuf, buflen);
3982 if (res != ERROR_OK)
3983 return res;
3984
3985 memcpy(buffer, h->databuf, buflen);
3986
3987 return ERROR_OK;
3988 }
3989
3990 /** */
3991 static int stlink_read_dap_register(void *handle, unsigned short dap_port,
3992 unsigned short addr, uint32_t *val)
3993 {
3994 struct stlink_usb_handle_s *h = handle;
3995 int retval;
3996
3997 assert(handle);
3998
3999 if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
4000 return ERROR_COMMAND_NOTFOUND;
4001
4002 stlink_usb_init_buffer(handle, h->rx_ep, 16);
4003 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
4004 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_DAP_REG;
4005 h_u16_to_le(&h->cmdbuf[2], dap_port);
4006 h_u16_to_le(&h->cmdbuf[4], addr);
4007
4008 retval = stlink_usb_xfer_errcheck(handle, h->databuf, 8);
4009 *val = le_to_h_u32(h->databuf + 4);
4010 LOG_DEBUG_IO("dap_port_read = %d, addr = 0x%x, value = 0x%" PRIx32, dap_port, addr, *val);
4011 return retval;
4012 }
4013
4014 /** */
4015 static int stlink_write_dap_register(void *handle, unsigned short dap_port,
4016 unsigned short addr, uint32_t val)
4017 {
4018 struct stlink_usb_handle_s *h = handle;
4019
4020 assert(handle);
4021
4022 if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
4023 return ERROR_COMMAND_NOTFOUND;
4024
4025 LOG_DEBUG_IO("dap_write port = %d, addr = 0x%x, value = 0x%" PRIx32, dap_port, addr, val);
4026 stlink_usb_init_buffer(handle, h->rx_ep, 16);
4027 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
4028 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITE_DAP_REG;
4029 h_u16_to_le(&h->cmdbuf[2], dap_port);
4030 h_u16_to_le(&h->cmdbuf[4], addr);
4031 h_u32_to_le(&h->cmdbuf[6], val);
4032 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
4033 }
4034
4035 /** */
4036 struct hl_layout_api_s stlink_usb_layout_api = {
4037 /** */
4038 .open = stlink_usb_hl_open,
4039 /** */
4040 .close = stlink_close,
4041 /** */
4042 .idcode = stlink_usb_idcode,
4043 /** */
4044 .state = stlink_usb_state,
4045 /** */
4046 .reset = stlink_usb_reset,
4047 /** */
4048 .assert_srst = stlink_usb_assert_srst,
4049 /** */
4050 .run = stlink_usb_run,
4051 /** */
4052 .halt = stlink_usb_halt,
4053 /** */
4054 .step = stlink_usb_step,
4055 /** */
4056 .read_regs = stlink_usb_read_regs,
4057 /** */
4058 .read_reg = stlink_usb_read_reg,
4059 /** */
4060 .write_reg = stlink_usb_write_reg,
4061 /** */
4062 .read_mem = stlink_usb_read_mem,
4063 /** */
4064 .write_mem = stlink_usb_write_mem,
4065 /** */
4066 .write_debug_reg = stlink_usb_write_debug_reg,
4067 /** */
4068 .override_target = stlink_usb_override_target,
4069 /** */
4070 .speed = stlink_speed,
4071 /** */
4072 .config_trace = stlink_config_trace,
4073 /** */
4074 .poll_trace = stlink_usb_trace_read,
4075 };
4076
4077 /*****************************************************************************
4078 * DAP direct interface
4079 */
4080
4081 static struct stlink_usb_handle_s *stlink_dap_handle;
4082 static struct hl_interface_param_s stlink_dap_param;
4083 static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1);
4084 static uint32_t last_csw_default[DP_APSEL_MAX + 1];
4085 static int stlink_dap_error = ERROR_OK;
4086
4087 /** */
4088 static int stlink_dap_record_error(int error)
4089 {
4090 if (stlink_dap_error == ERROR_OK)
4091 stlink_dap_error = error;
4092 return ERROR_OK;
4093 }
4094
4095 /** */
4096 static int stlink_dap_get_and_clear_error(void)
4097 {
4098 int retval = stlink_dap_error;
4099 stlink_dap_error = ERROR_OK;
4100 return retval;
4101 }
4102
4103 static int stlink_dap_get_error(void)
4104 {
4105 return stlink_dap_error;
4106 }
4107
4108 static int stlink_usb_open_ap(void *handle, unsigned short apsel)
4109 {
4110 struct stlink_usb_handle_s *h = handle;
4111 int retval;
4112
4113 /* nothing to do on old versions */
4114 if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
4115 return ERROR_OK;
4116
4117 if (apsel > DP_APSEL_MAX)
4118 return ERROR_FAIL;
4119
4120 if (test_bit(apsel, opened_ap))
4121 return ERROR_OK;
4122
4123 retval = stlink_usb_init_access_port(h, apsel);
4124 if (retval != ERROR_OK)
4125 return retval;
4126
4127 LOG_DEBUG("AP %d enabled", apsel);
4128 set_bit(apsel, opened_ap);
4129 last_csw_default[apsel] = 0;
4130 return ERROR_OK;
4131 }
4132
4133 static int stlink_dap_open_ap(unsigned short apsel)
4134 {
4135 return stlink_usb_open_ap(stlink_dap_handle, apsel);
4136 }
4137
4138 /** */
4139 static int stlink_dap_closeall_ap(void)
4140 {
4141 int retval, apsel;
4142
4143 /* nothing to do on old versions */
4144 if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT))
4145 return ERROR_OK;
4146
4147 for (apsel = 0; apsel <= DP_APSEL_MAX; apsel++) {
4148 if (!test_bit(apsel, opened_ap))
4149 continue;
4150 retval = stlink_usb_close_access_port(stlink_dap_handle, apsel);
4151 if (retval != ERROR_OK)
4152 return retval;
4153 clear_bit(apsel, opened_ap);
4154 }
4155 return ERROR_OK;
4156 }
4157
4158 /** */
4159 static int stlink_dap_reinit_interface(void)
4160 {
4161 int retval;
4162
4163 /*
4164 * On JTAG only, it should be enough to call stlink_usb_reset(). But on
4165 * some firmware version it does not work as expected, and there is no
4166 * equivalent for SWD.
4167 * At least for now, to reset the interface quit from JTAG/SWD mode then
4168 * select the mode again.
4169 */
4170
4171 if (!stlink_dap_handle->reconnect_pending) {
4172 stlink_dap_handle->reconnect_pending = true;
4173 stlink_usb_mode_leave(stlink_dap_handle, stlink_dap_handle->st_mode);
4174 }
4175
4176 retval = stlink_usb_mode_enter(stlink_dap_handle, stlink_dap_handle->st_mode);
4177 if (retval != ERROR_OK)
4178 return retval;
4179
4180 stlink_dap_handle->reconnect_pending = false;
4181 /* on new FW, calling mode-leave closes all the opened AP; reopen them! */
4182 if (stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT)
4183 for (unsigned int apsel = 0; apsel <= DP_APSEL_MAX; apsel++)
4184 if (test_bit(apsel, opened_ap)) {
4185 clear_bit(apsel, opened_ap);
4186 stlink_dap_open_ap(apsel);
4187 }
4188 return ERROR_OK;
4189 }
4190
4191 /** */
4192 static int stlink_dap_op_connect(struct adiv5_dap *dap)
4193 {
4194 uint32_t idcode;
4195 int retval;
4196
4197 LOG_INFO("stlink_dap_op_connect(%sconnect)", dap->do_reconnect ? "re" : "");
4198
4199 /* Check if we should reset srst already when connecting, but not if reconnecting. */
4200 if (!dap->do_reconnect) {
4201 enum reset_types jtag_reset_config = jtag_get_reset_config();
4202
4203 if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
4204 if (jtag_reset_config & RESET_SRST_NO_GATING)
4205 adapter_assert_reset();
4206 else
4207 LOG_WARNING("\'srst_nogate\' reset_config option is required");
4208 }
4209 }
4210
4211 dap->do_reconnect = false;
4212 dap_invalidate_cache(dap);
4213 for (unsigned int i = 0; i <= DP_APSEL_MAX; i++)
4214 last_csw_default[i] = 0;
4215
4216 retval = dap_dp_init(dap);
4217 if (retval != ERROR_OK) {
4218 dap->do_reconnect = true;
4219 return retval;
4220 }
4221
4222 retval = stlink_usb_idcode(stlink_dap_handle, &idcode);
4223 if (retval == ERROR_OK)
4224 LOG_INFO("%s %#8.8" PRIx32,
4225 (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) ? "JTAG IDCODE" : "SWD DPIDR",
4226 idcode);
4227 else
4228 dap->do_reconnect = true;
4229
4230 return retval;
4231 }
4232
4233 /** */
4234 static int stlink_dap_check_reconnect(struct adiv5_dap *dap)
4235 {
4236 int retval;
4237
4238 if (!dap->do_reconnect)
4239 return ERROR_OK;
4240
4241 retval = stlink_dap_reinit_interface();
4242 if (retval != ERROR_OK)
4243 return retval;
4244
4245 return stlink_dap_op_connect(dap);
4246 }
4247
4248 /** */
4249 static int stlink_dap_op_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
4250 {
4251 /* Ignore the request */
4252 return ERROR_OK;
4253 }
4254
4255 /** */
4256 static int stlink_dap_dp_read(struct adiv5_dap *dap, unsigned int reg, uint32_t *data)
4257 {
4258 uint32_t dummy;
4259 int retval;
4260
4261 if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
4262 if (reg & 0x000000F0) {
4263 LOG_ERROR("Banked DP registers not supported in current STLink FW");
4264 return ERROR_COMMAND_NOTFOUND;
4265 }
4266
4267 data = data ? data : &dummy;
4268 if (stlink_dap_handle->version.flags & STLINK_F_QUIRK_JTAG_DP_READ
4269 && stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) {
4270 /* Quirk required in JTAG. Read RDBUFF to get the data */
4271 retval = stlink_read_dap_register(stlink_dap_handle,
4272 STLINK_DEBUG_PORT_ACCESS, reg, &dummy);
4273 if (retval == ERROR_OK)
4274 retval = stlink_read_dap_register(stlink_dap_handle,
4275 STLINK_DEBUG_PORT_ACCESS, DP_RDBUFF, data);
4276 } else {
4277 retval = stlink_read_dap_register(stlink_dap_handle,
4278 STLINK_DEBUG_PORT_ACCESS, reg, data);
4279 }
4280
4281 return retval;
4282 }
4283
4284 /** */
4285 static int stlink_dap_dp_write(struct adiv5_dap *dap, unsigned int reg, uint32_t data)
4286 {
4287 int retval;
4288
4289 if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
4290 if (reg & 0x000000F0) {
4291 LOG_ERROR("Banked DP registers not supported in current STLink FW");
4292 return ERROR_COMMAND_NOTFOUND;
4293 }
4294
4295 if (reg == DP_SELECT && (data & DP_SELECT_DPBANK) != 0) {
4296 /* ignored if STLINK_F_HAS_DPBANKSEL, not properly managed otherwise */
4297 LOG_DEBUG("Ignoring DPBANKSEL while write SELECT");
4298 data &= ~DP_SELECT_DPBANK;
4299 }
4300
4301 /* ST-Link does not like that we set CORUNDETECT */
4302 if (reg == DP_CTRL_STAT)
4303 data &= ~CORUNDETECT;
4304
4305 retval = stlink_write_dap_register(stlink_dap_handle,
4306 STLINK_DEBUG_PORT_ACCESS, reg, data);
4307 return retval;
4308 }
4309
4310 /** */
4311 static int stlink_dap_ap_read(struct adiv5_ap *ap, unsigned int reg, uint32_t *data)
4312 {
4313 struct adiv5_dap *dap = ap->dap;
4314 uint32_t dummy;
4315 int retval;
4316
4317 if (is_adiv6(dap)) {
4318 static bool error_flagged;
4319 if (!error_flagged)
4320 LOG_ERROR("ADIv6 dap not supported by stlink dap-direct mode");
4321 error_flagged = true;
4322 return ERROR_FAIL;
4323 }
4324
4325 if (reg != ADIV5_AP_REG_IDR) {
4326 retval = stlink_dap_open_ap(ap->ap_num);
4327 if (retval != ERROR_OK)
4328 return retval;
4329 }
4330 data = data ? data : &dummy;
4331 retval = stlink_read_dap_register(stlink_dap_handle, ap->ap_num, reg,
4332 data);
4333 dap->stlink_flush_ap_write = false;
4334 return retval;
4335 }
4336
4337 /** */
4338 static int stlink_dap_ap_write(struct adiv5_ap *ap, unsigned int reg, uint32_t data)
4339 {
4340 struct adiv5_dap *dap = ap->dap;
4341 int retval;
4342
4343 if (is_adiv6(dap)) {
4344 static bool error_flagged;
4345 if (!error_flagged)
4346 LOG_ERROR("ADIv6 dap not supported by stlink dap-direct mode");
4347 error_flagged = true;
4348 return ERROR_FAIL;
4349 }
4350
4351 retval = stlink_dap_open_ap(ap->ap_num);
4352 if (retval != ERROR_OK)
4353 return retval;
4354
4355 retval = stlink_write_dap_register(stlink_dap_handle, ap->ap_num, reg,
4356 data);
4357 dap->stlink_flush_ap_write = true;
4358 return retval;
4359 }
4360
4361 /** */
4362 static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
4363 {
4364 LOG_WARNING("stlink_dap_op_queue_ap_abort()");
4365 return ERROR_OK;
4366 }
4367
4368 #define RW_MISC_CMD_ADDRESS 1
4369 #define RW_MISC_CMD_WRITE 2
4370 #define RW_MISC_CMD_READ 3
4371 #define RW_MISC_CMD_APNUM 5
4372
4373 static int stlink_usb_misc_rw_segment(void *handle, const struct dap_queue *q, unsigned int len, unsigned int items)
4374 {
4375 uint8_t buf[2 * 4 * items];
4376
4377 LOG_DEBUG("Queue: %u commands in %u items", len, items);
4378
4379 uint32_t ap_num = DP_APSEL_INVALID;
4380 unsigned int cmd_index = 0;
4381 unsigned int val_index = ALIGN_UP(items, 4);
4382 for (unsigned int i = 0; i < len; i++) {
4383 if (ap_num != q[i].mem_ap.ap->ap_num) {
4384 ap_num = q[i].mem_ap.ap->ap_num;
4385 buf[cmd_index++] = RW_MISC_CMD_APNUM;
4386 h_u32_to_le(&buf[val_index], ap_num);
4387 val_index += 4;
4388 }
4389
4390 switch (q[i].cmd) {
4391 case CMD_MEM_AP_READ32:
4392 buf[cmd_index++] = RW_MISC_CMD_READ;
4393 h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
4394 val_index += 4;
4395 break;
4396 case CMD_MEM_AP_WRITE32:
4397 buf[cmd_index++] = RW_MISC_CMD_ADDRESS;
4398 h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
4399 val_index += 4;
4400 buf[cmd_index++] = RW_MISC_CMD_WRITE;
4401 h_u32_to_le(&buf[val_index], q[i].mem_ap.data);
4402 val_index += 4;
4403 break;
4404 default:
4405 /* Not supposed to happen */
4406 return ERROR_FAIL;
4407 }
4408 }
4409 /* pad after last command */
4410 while (!IS_ALIGNED(cmd_index, 4))
4411 buf[cmd_index++] = 0;
4412
4413 int retval = stlink_usb_rw_misc_out(handle, items, buf);
4414 if (retval != ERROR_OK)
4415 return retval;
4416
4417 retval = stlink_usb_rw_misc_in(handle, items, buf);
4418 if (retval != ERROR_OK)
4419 return retval;
4420
4421 ap_num = DP_APSEL_INVALID;
4422 val_index = 0;
4423 unsigned int err_index = 4 * items;
4424 for (unsigned int i = 0; i < len; i++) {
4425 uint32_t errcode = le_to_h_u32(&buf[err_index]);
4426 if (errcode != STLINK_DEBUG_ERR_OK) {
4427 LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
4428 return ERROR_FAIL;
4429 }
4430 if (ap_num != q[i].mem_ap.ap->ap_num) {
4431 ap_num = q[i].mem_ap.ap->ap_num;
4432 err_index += 4;
4433 val_index += 4;
4434 errcode = le_to_h_u32(&buf[err_index]);
4435 if (errcode != STLINK_DEBUG_ERR_OK) {
4436 LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
4437 return ERROR_FAIL;
4438 }
4439 }
4440
4441 if (q[i].cmd == CMD_MEM_AP_READ32) {
4442 *q[i].mem_ap.p_data = le_to_h_u32(&buf[val_index]);
4443 } else { /* q[i]->cmd == CMD_MEM_AP_WRITE32 */
4444 err_index += 4;
4445 val_index += 4;
4446 errcode = le_to_h_u32(&buf[err_index]);
4447 if (errcode != STLINK_DEBUG_ERR_OK) {
4448 LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
4449 return ERROR_FAIL;
4450 }
4451 }
4452 err_index += 4;
4453 val_index += 4;
4454 }
4455
4456 return ERROR_OK;
4457 }
4458
4459 static int stlink_usb_buf_rw_segment(void *handle, const struct dap_queue *q, unsigned int count)
4460 {
4461 uint32_t bufsize = count * CMD_MEM_AP_2_SIZE(q[0].cmd);
4462 uint8_t buf[bufsize];
4463 uint8_t ap_num = q[0].mem_ap.ap->ap_num;
4464 uint32_t addr = q[0].mem_ap.addr;
4465 uint32_t csw = q[0].mem_ap.csw;
4466
4467 int retval = stlink_dap_open_ap(ap_num);
4468 if (retval != ERROR_OK)
4469 return retval;
4470
4471 switch (q[0].cmd) {
4472 case CMD_MEM_AP_WRITE8:
4473 for (unsigned int i = 0; i < count; i++)
4474 buf[i] = q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 3);
4475 return stlink_usb_write_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4476
4477 case CMD_MEM_AP_WRITE16:
4478 for (unsigned int i = 0; i < count; i++)
4479 h_u16_to_le(&buf[2 * i], q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 2));
4480 return stlink_usb_write_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4481
4482 case CMD_MEM_AP_WRITE32:
4483 for (unsigned int i = 0; i < count; i++)
4484 h_u32_to_le(&buf[4 * i], q[i].mem_ap.data);
4485 if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
4486 return stlink_usb_write_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4487 else
4488 return stlink_usb_write_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4489
4490 case CMD_MEM_AP_READ8:
4491 retval = stlink_usb_read_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4492 if (retval == ERROR_OK)
4493 for (unsigned int i = 0; i < count; i++)
4494 *q[i].mem_ap.p_data = buf[i] << 8 * (q[i].mem_ap.addr & 3);
4495 return retval;
4496
4497 case CMD_MEM_AP_READ16:
4498 retval = stlink_usb_read_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4499 if (retval == ERROR_OK)
4500 for (unsigned int i = 0; i < count; i++)
4501 *q[i].mem_ap.p_data = le_to_h_u16(&buf[2 * i]) << 8 * (q[i].mem_ap.addr & 2);
4502 return retval;
4503
4504 case CMD_MEM_AP_READ32:
4505 if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
4506 retval = stlink_usb_read_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4507 else
4508 retval = stlink_usb_read_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4509 if (retval == ERROR_OK)
4510 for (unsigned int i = 0; i < count; i++)
4511 *q[i].mem_ap.p_data = le_to_h_u32(&buf[4 * i]);
4512 return retval;
4513
4514 default:
4515 return ERROR_FAIL;
4516 };
4517 }
4518
4519 /* TODO: recover these values with cmd STLINK_DEBUG_APIV2_RW_MISC_GET_MAX (0x53) */
4520 #define STLINK_V2_RW_MISC_SIZE (64)
4521 #define STLINK_V3_RW_MISC_SIZE (1227)
4522
4523 static int stlink_usb_count_misc_rw_queue(void *handle, const struct dap_queue *q, unsigned int len,
4524 unsigned int *pkt_items)
4525 {
4526 struct stlink_usb_handle_s *h = handle;
4527 unsigned int i, items = 0;
4528 uint32_t ap_num = DP_APSEL_INVALID;
4529 unsigned int misc_max_items = (h->version.stlink == 2) ? STLINK_V2_RW_MISC_SIZE : STLINK_V3_RW_MISC_SIZE;
4530
4531 if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
4532 return 0;
4533 /*
4534 * Before stlink-server API v3, RW_MISC sequence doesn't lock the st-link,
4535 * so are not safe in shared mode.
4536 * Don't use it with TCP backend to prevent any issue in case of sharing.
4537 * This further degrades the performance, on top of TCP server overhead.
4538 */
4539 if (h->backend == &stlink_tcp_backend && h->tcp_backend_priv.version.api < 3)
4540 return 0;
4541
4542 for (i = 0; i < len; i++) {
4543 if (q[i].cmd != CMD_MEM_AP_READ32 && q[i].cmd != CMD_MEM_AP_WRITE32)
4544 break;
4545 unsigned int count = 1;
4546 if (ap_num != q[i].mem_ap.ap->ap_num) {
4547 count++;
4548 ap_num = q[i].mem_ap.ap->ap_num;
4549 }
4550 if (q[i].cmd == CMD_MEM_AP_WRITE32)
4551 count++;
4552 if (items + count > misc_max_items)
4553 break;
4554 items += count;
4555 }
4556
4557 *pkt_items = items;
4558
4559 return i;
4560 }
4561
4562 static int stlink_usb_count_buf_rw_queue(const struct dap_queue *q, unsigned int len)
4563 {
4564 uint32_t incr = CMD_MEM_AP_2_SIZE(q[0].cmd);
4565 unsigned int len_max;
4566
4567 if (incr == 1)
4568 len_max = stlink_usb_block(stlink_dap_handle);
4569 else
4570 len_max = STLINK_MAX_RW16_32 / incr;
4571
4572 /* check for no address increment, 32 bits only */
4573 if (len > 1 && incr == 4 && q[0].mem_ap.addr == q[1].mem_ap.addr)
4574 incr = 0;
4575
4576 if (len > len_max)
4577 len = len_max;
4578
4579 for (unsigned int i = 1; i < len; i++)
4580 if (q[i].cmd != q[0].cmd ||
4581 q[i].mem_ap.ap != q[0].mem_ap.ap ||
4582 q[i].mem_ap.csw != q[0].mem_ap.csw ||
4583 q[i].mem_ap.addr != q[i - 1].mem_ap.addr + incr)
4584 return i;
4585
4586 return len;
4587 }
4588
4589 static int stlink_usb_mem_rw_queue(void *handle, const struct dap_queue *q, unsigned int len, unsigned int *skip)
4590 {
4591 unsigned int count, misc_items = 0;
4592 int retval;
4593
4594 unsigned int count_misc = stlink_usb_count_misc_rw_queue(handle, q, len, &misc_items);
4595 unsigned int count_buf = stlink_usb_count_buf_rw_queue(q, len);
4596
4597 if (count_misc > count_buf) {
4598 count = count_misc;
4599 retval = stlink_usb_misc_rw_segment(handle, q, count, misc_items);
4600 } else {
4601 count = count_buf;
4602 retval = stlink_usb_buf_rw_segment(handle, q, count_buf);
4603 }
4604 if (retval != ERROR_OK)
4605 return retval;
4606
4607 *skip = count;
4608 return ERROR_OK;
4609 }
4610
4611 static void stlink_dap_run_internal(struct adiv5_dap *dap)
4612 {
4613 int retval = stlink_dap_check_reconnect(dap);
4614 if (retval != ERROR_OK) {
4615 stlink_dap_handle->queue_index = 0;
4616 stlink_dap_record_error(retval);
4617 return;
4618 }
4619
4620 unsigned int i = stlink_dap_handle->queue_index;
4621 struct dap_queue *q = &stlink_dap_handle->queue[0];
4622
4623 while (i && stlink_dap_get_error() == ERROR_OK) {
4624 unsigned int skip = 1;
4625
4626 switch (q->cmd) {
4627 case CMD_DP_READ:
4628 retval = stlink_dap_dp_read(q->dp_r.dap, q->dp_r.reg, q->dp_r.p_data);
4629 break;
4630 case CMD_DP_WRITE:
4631 retval = stlink_dap_dp_write(q->dp_w.dap, q->dp_w.reg, q->dp_w.data);
4632 break;
4633 case CMD_AP_READ:
4634 retval = stlink_dap_ap_read(q->ap_r.ap, q->ap_r.reg, q->ap_r.p_data);
4635 break;
4636 case CMD_AP_WRITE:
4637 /* ignore increment packed, not supported */
4638 if (q->ap_w.reg == ADIV5_MEM_AP_REG_CSW)
4639 q->ap_w.data &= ~CSW_ADDRINC_PACKED;
4640 retval = stlink_dap_ap_write(q->ap_w.ap, q->ap_w.reg, q->ap_w.data);
4641 break;
4642
4643 case CMD_MEM_AP_READ8:
4644 case CMD_MEM_AP_READ16:
4645 case CMD_MEM_AP_READ32:
4646 case CMD_MEM_AP_WRITE8:
4647 case CMD_MEM_AP_WRITE16:
4648 case CMD_MEM_AP_WRITE32:
4649 retval = stlink_usb_mem_rw_queue(stlink_dap_handle, q, i, &skip);
4650 break;
4651
4652 default:
4653 LOG_ERROR("ST-Link: Unknown queue command %d", q->cmd);
4654 retval = ERROR_FAIL;
4655 break;
4656 }
4657 stlink_dap_record_error(retval);
4658 q += skip;
4659 i -= skip;
4660 }
4661
4662 stlink_dap_handle->queue_index = 0;
4663 }
4664
4665 /** */
4666 static int stlink_dap_run_finalize(struct adiv5_dap *dap)
4667 {
4668 uint32_t ctrlstat, pwrmask;
4669 int retval, saved_retval;
4670
4671 /* Here no LOG_DEBUG. This is called continuously! */
4672
4673 /*
4674 * ST-Link returns immediately after a DAP write, without waiting for it
4675 * to complete.
4676 * Run a dummy read to DP_RDBUFF, as suggested in
4677 * http://infocenter.arm.com/help/topic/com.arm.doc.faqs/ka16363.html
4678 */
4679 if (dap->stlink_flush_ap_write) {
4680 dap->stlink_flush_ap_write = false;
4681 retval = stlink_dap_dp_read(dap, DP_RDBUFF, NULL);
4682 if (retval != ERROR_OK) {
4683 dap->do_reconnect = true;
4684 return retval;
4685 }
4686 }
4687
4688 saved_retval = stlink_dap_get_and_clear_error();
4689
4690 retval = stlink_dap_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
4691 if (retval != ERROR_OK) {
4692 LOG_ERROR("Fail reading CTRL/STAT register. Force reconnect");
4693 dap->do_reconnect = true;
4694 return retval;
4695 }
4696
4697 if (ctrlstat & SSTICKYERR) {
4698 if (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG)
4699 retval = stlink_dap_dp_write(dap, DP_CTRL_STAT,
4700 ctrlstat & (dap->dp_ctrl_stat | SSTICKYERR));
4701 else
4702 retval = stlink_dap_dp_write(dap, DP_ABORT, STKERRCLR);
4703 if (retval != ERROR_OK) {
4704 dap->do_reconnect = true;
4705 return retval;
4706 }
4707 }
4708
4709 /* check for power lost */
4710 pwrmask = dap->dp_ctrl_stat & (CDBGPWRUPREQ | CSYSPWRUPREQ);
4711 if ((ctrlstat & pwrmask) != pwrmask)
4712 dap->do_reconnect = true;
4713
4714 return saved_retval;
4715 }
4716
4717 static int stlink_dap_op_queue_run(struct adiv5_dap *dap)
4718 {
4719 stlink_dap_run_internal(dap);
4720 return stlink_dap_run_finalize(dap);
4721 }
4722
4723 /** */
4724 static void stlink_dap_op_quit(struct adiv5_dap *dap)
4725 {
4726 int retval;
4727
4728 retval = stlink_dap_closeall_ap();
4729 if (retval != ERROR_OK)
4730 LOG_ERROR("Error closing APs");
4731 }
4732
4733 static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned int reg,
4734 uint32_t *data)
4735 {
4736 if (stlink_dap_get_error() != ERROR_OK)
4737 return ERROR_OK;
4738
4739 unsigned int i = stlink_dap_handle->queue_index++;
4740 struct dap_queue *q = &stlink_dap_handle->queue[i];
4741 q->cmd = CMD_DP_READ;
4742 q->dp_r.reg = reg;
4743 q->dp_r.dap = dap;
4744 q->dp_r.p_data = data;
4745
4746 if (i == MAX_QUEUE_DEPTH - 1)
4747 stlink_dap_run_internal(dap);
4748
4749 return ERROR_OK;
4750 }
4751
4752 static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned int reg,
4753 uint32_t data)
4754 {
4755 if (stlink_dap_get_error() != ERROR_OK)
4756 return ERROR_OK;
4757
4758 unsigned int i = stlink_dap_handle->queue_index++;
4759 struct dap_queue *q = &stlink_dap_handle->queue[i];
4760 q->cmd = CMD_DP_WRITE;
4761 q->dp_w.reg = reg;
4762 q->dp_w.dap = dap;
4763 q->dp_w.data = data;
4764
4765 if (i == MAX_QUEUE_DEPTH - 1)
4766 stlink_dap_run_internal(dap);
4767
4768 return ERROR_OK;
4769 }
4770
4771 static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned int reg,
4772 uint32_t *data)
4773 {
4774 if (stlink_dap_get_error() != ERROR_OK)
4775 return ERROR_OK;
4776
4777 unsigned int i = stlink_dap_handle->queue_index++;
4778 struct dap_queue *q = &stlink_dap_handle->queue[i];
4779
4780 /* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_RD_NO_INC
4781 * and STLINK_F_HAS_RW_MISC */
4782 if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) &&
4783 (reg == ADIV5_MEM_AP_REG_DRW || reg == ADIV5_MEM_AP_REG_BD0 || reg == ADIV5_MEM_AP_REG_BD1 ||
4784 reg == ADIV5_MEM_AP_REG_BD2 || reg == ADIV5_MEM_AP_REG_BD3)) {
4785 /* de-queue previous write-TAR */
4786 struct dap_queue *prev_q = q - 1;
4787 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_TAR) {
4788 stlink_dap_handle->queue_index = i;
4789 i--;
4790 q = prev_q;
4791 prev_q--;
4792 }
4793 /* de-queue previous write-CSW if it didn't changed ap->csw_default */
4794 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_CSW &&
4795 !prev_q->ap_w.changes_csw_default) {
4796 stlink_dap_handle->queue_index = i;
4797 q = prev_q;
4798 }
4799
4800 switch (ap->csw_value & CSW_SIZE_MASK) {
4801 case CSW_8BIT:
4802 q->cmd = CMD_MEM_AP_READ8;
4803 break;
4804 case CSW_16BIT:
4805 q->cmd = CMD_MEM_AP_READ16;
4806 break;
4807 case CSW_32BIT:
4808 q->cmd = CMD_MEM_AP_READ32;
4809 break;
4810 default:
4811 LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK);
4812 stlink_dap_record_error(ERROR_FAIL);
4813 return ERROR_FAIL;
4814 }
4815
4816 q->mem_ap.addr = (reg == ADIV5_MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c));
4817 q->mem_ap.ap = ap;
4818 q->mem_ap.p_data = data;
4819 q->mem_ap.csw = ap->csw_default;
4820
4821 /* force TAR and CSW update */
4822 ap->tar_valid = false;
4823 ap->csw_value = 0;
4824 } else {
4825 q->cmd = CMD_AP_READ;
4826 q->ap_r.reg = reg;
4827 q->ap_r.ap = ap;
4828 q->ap_r.p_data = data;
4829 }
4830
4831 if (i == MAX_QUEUE_DEPTH - 1)
4832 stlink_dap_run_internal(ap->dap);
4833
4834 return ERROR_OK;
4835 }
4836
4837 static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned int reg,
4838 uint32_t data)
4839 {
4840 if (stlink_dap_get_error() != ERROR_OK)
4841 return ERROR_OK;
4842
4843 unsigned int i = stlink_dap_handle->queue_index++;
4844 struct dap_queue *q = &stlink_dap_handle->queue[i];
4845
4846 /* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_WR_NO_INC
4847 * and STLINK_F_HAS_RW_MISC */
4848 if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) &&
4849 (reg == ADIV5_MEM_AP_REG_DRW || reg == ADIV5_MEM_AP_REG_BD0 || reg == ADIV5_MEM_AP_REG_BD1 ||
4850 reg == ADIV5_MEM_AP_REG_BD2 || reg == ADIV5_MEM_AP_REG_BD3)) {
4851 /* de-queue previous write-TAR */
4852 struct dap_queue *prev_q = q - 1;
4853 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_TAR) {
4854 stlink_dap_handle->queue_index = i;
4855 i--;
4856 q = prev_q;
4857 prev_q--;
4858 }
4859 /* de-queue previous write-CSW if it didn't changed ap->csw_default */
4860 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_CSW &&
4861 !prev_q->ap_w.changes_csw_default) {
4862 stlink_dap_handle->queue_index = i;
4863 q = prev_q;
4864 }
4865
4866 switch (ap->csw_value & CSW_SIZE_MASK) {
4867 case CSW_8BIT:
4868 q->cmd = CMD_MEM_AP_WRITE8;
4869 break;
4870 case CSW_16BIT:
4871 q->cmd = CMD_MEM_AP_WRITE16;
4872 break;
4873 case CSW_32BIT:
4874 q->cmd = CMD_MEM_AP_WRITE32;
4875 break;
4876 default:
4877 LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK);
4878 stlink_dap_record_error(ERROR_FAIL);
4879 return ERROR_FAIL;
4880 }
4881
4882 q->mem_ap.addr = (reg == ADIV5_MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c));
4883 q->mem_ap.ap = ap;
4884 q->mem_ap.data = data;
4885 q->mem_ap.csw = ap->csw_default;
4886
4887 /* force TAR and CSW update */
4888 ap->tar_valid = false;
4889 ap->csw_value = 0;
4890 } else {
4891 q->cmd = CMD_AP_WRITE;
4892 q->ap_w.reg = reg;
4893 q->ap_w.ap = ap;
4894 q->ap_w.data = data;
4895 uint8_t ap_num = ap->ap_num;
4896 if (reg == ADIV5_MEM_AP_REG_CSW && ap->csw_default != last_csw_default[ap_num]) {
4897 q->ap_w.changes_csw_default = true;
4898 last_csw_default[ap_num] = ap->csw_default;
4899 } else {
4900 q->ap_w.changes_csw_default = false;
4901 }
4902 }
4903
4904 if (i == MAX_QUEUE_DEPTH - 1)
4905 stlink_dap_run_internal(ap->dap);
4906
4907 return ERROR_OK;
4908 }
4909
4910 static int stlink_swim_op_srst(void)
4911 {
4912 return stlink_swim_generate_rst(stlink_dap_handle);
4913 }
4914
4915 static int stlink_swim_op_read_mem(uint32_t addr, uint32_t size,
4916 uint32_t count, uint8_t *buffer)
4917 {
4918 int retval;
4919 uint32_t bytes_remaining;
4920
4921 LOG_DEBUG_IO("read at 0x%08" PRIx32 " len %" PRIu32 "*0x%08" PRIx32, addr, size, count);
4922 count *= size;
4923
4924 while (count) {
4925 bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count;
4926 retval = stlink_swim_readbytes(stlink_dap_handle, addr, bytes_remaining, buffer);
4927 if (retval != ERROR_OK)
4928 return retval;
4929
4930 buffer += bytes_remaining;
4931 addr += bytes_remaining;
4932 count -= bytes_remaining;
4933 }
4934
4935 return ERROR_OK;
4936 }
4937
4938 static int stlink_swim_op_write_mem(uint32_t addr, uint32_t size,
4939 uint32_t count, const uint8_t *buffer)
4940 {
4941 int retval;
4942 uint32_t bytes_remaining;
4943
4944 LOG_DEBUG_IO("write at 0x%08" PRIx32 " len %" PRIu32 "*0x%08" PRIx32, addr, size, count);
4945 count *= size;
4946
4947 while (count) {
4948 bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count;
4949 retval = stlink_swim_writebytes(stlink_dap_handle, addr, bytes_remaining, buffer);
4950 if (retval != ERROR_OK)
4951 return retval;
4952
4953 buffer += bytes_remaining;
4954 addr += bytes_remaining;
4955 count -= bytes_remaining;
4956 }
4957
4958 return ERROR_OK;
4959 }
4960
4961 static int stlink_swim_op_reconnect(void)
4962 {
4963 int retval;
4964
4965 retval = stlink_usb_mode_enter(stlink_dap_handle, STLINK_MODE_DEBUG_SWIM);
4966 if (retval != ERROR_OK)
4967 return retval;
4968
4969 return stlink_swim_resync(stlink_dap_handle);
4970 }
4971
4972 static int stlink_dap_config_trace(bool enabled,
4973 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
4974 unsigned int *trace_freq, unsigned int traceclkin_freq,
4975 uint16_t *prescaler)
4976 {
4977 return stlink_config_trace(stlink_dap_handle, enabled, pin_protocol,
4978 port_size, trace_freq, traceclkin_freq,
4979 prescaler);
4980 }
4981
4982 static int stlink_dap_trace_read(uint8_t *buf, size_t *size)
4983 {
4984 return stlink_usb_trace_read(stlink_dap_handle, buf, size);
4985 }
4986
4987 /** */
4988 COMMAND_HANDLER(stlink_dap_vid_pid)
4989 {
4990 unsigned int i, max_usb_ids = HLA_MAX_USB_IDS;
4991
4992 if (CMD_ARGC > max_usb_ids * 2) {
4993 LOG_WARNING("ignoring extra IDs in vid_pid "
4994 "(maximum is %d pairs)", max_usb_ids);
4995 CMD_ARGC = max_usb_ids * 2;
4996 }
4997 if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
4998 LOG_WARNING("incomplete vid_pid configuration directive");
4999 return ERROR_COMMAND_SYNTAX_ERROR;
5000 }
5001 for (i = 0; i < CMD_ARGC; i += 2) {
5002 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], stlink_dap_param.vid[i / 2]);
5003 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], stlink_dap_param.pid[i / 2]);
5004 }
5005
5006 /* null termination */
5007 stlink_dap_param.vid[i / 2] = stlink_dap_param.pid[i / 2] = 0;
5008
5009 return ERROR_OK;
5010 }
5011
5012 /** */
5013 COMMAND_HANDLER(stlink_dap_backend_command)
5014 {
5015 /* default values */
5016 bool use_stlink_tcp = false;
5017 uint16_t stlink_tcp_port = 7184;
5018
5019 if (CMD_ARGC == 0 || CMD_ARGC > 2)
5020 return ERROR_COMMAND_SYNTAX_ERROR;
5021 else if (strcmp(CMD_ARGV[0], "usb") == 0) {
5022 if (CMD_ARGC > 1)
5023 return ERROR_COMMAND_SYNTAX_ERROR;
5024 /* else use_stlink_tcp = false (already the case ) */
5025 } else if (strcmp(CMD_ARGV[0], "tcp") == 0) {
5026 use_stlink_tcp = true;
5027 if (CMD_ARGC == 2)
5028 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], stlink_tcp_port);
5029 } else
5030 return ERROR_COMMAND_SYNTAX_ERROR;
5031
5032 stlink_dap_param.use_stlink_tcp = use_stlink_tcp;
5033 stlink_dap_param.stlink_tcp_port = stlink_tcp_port;
5034
5035 return ERROR_OK;
5036 }
5037
5038 #define BYTES_PER_LINE 16
5039 COMMAND_HANDLER(stlink_dap_cmd_command)
5040 {
5041 unsigned int rx_n, tx_n;
5042 struct stlink_usb_handle_s *h = stlink_dap_handle;
5043
5044 if (CMD_ARGC < 2)
5045 return ERROR_COMMAND_SYNTAX_ERROR;
5046
5047 COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], rx_n);
5048 tx_n = CMD_ARGC - 1;
5049 if (tx_n > STLINK_SG_SIZE || rx_n > STLINK_DATA_SIZE) {
5050 LOG_ERROR("max %x byte sent and %d received", STLINK_SG_SIZE, STLINK_DATA_SIZE);
5051 return ERROR_COMMAND_SYNTAX_ERROR;
5052 }
5053
5054 stlink_usb_init_buffer(h, h->rx_ep, rx_n);
5055
5056 for (unsigned int i = 0; i < tx_n; i++) {
5057 uint8_t byte;
5058 COMMAND_PARSE_NUMBER(u8, CMD_ARGV[i + 1], byte);
5059 h->cmdbuf[h->cmdidx++] = byte;
5060 }
5061
5062 int retval = stlink_usb_xfer_noerrcheck(h, h->databuf, rx_n);
5063 if (retval != ERROR_OK) {
5064 LOG_ERROR("Error %d", retval);
5065 return retval;
5066 }
5067
5068 for (unsigned int i = 0; i < rx_n; i++)
5069 command_print_sameline(CMD, "0x%02x%c", h->databuf[i],
5070 ((i == (rx_n - 1)) || ((i % BYTES_PER_LINE) == (BYTES_PER_LINE - 1))) ? '\n' : ' ');
5071
5072 return ERROR_OK;
5073 }
5074
5075 /** */
5076 static const struct command_registration stlink_dap_subcommand_handlers[] = {
5077 {
5078 .name = "vid_pid",
5079 .handler = stlink_dap_vid_pid,
5080 .mode = COMMAND_CONFIG,
5081 .help = "USB VID and PID of the adapter",
5082 .usage = "(vid pid)+",
5083 },
5084 {
5085 .name = "backend",
5086 .handler = &stlink_dap_backend_command,
5087 .mode = COMMAND_CONFIG,
5088 .help = "select which ST-Link backend to use",
5089 .usage = "usb | tcp [port]",
5090 },
5091 {
5092 .name = "cmd",
5093 .handler = stlink_dap_cmd_command,
5094 .mode = COMMAND_EXEC,
5095 .help = "send arbitrary command",
5096 .usage = "rx_n (tx_byte)+",
5097 },
5098 COMMAND_REGISTRATION_DONE
5099 };
5100
5101 /** */
5102 static const struct command_registration stlink_dap_command_handlers[] = {
5103 {
5104 .name = "st-link",
5105 .mode = COMMAND_ANY,
5106 .help = "perform st-link management",
5107 .chain = stlink_dap_subcommand_handlers,
5108 .usage = "",
5109 },
5110 COMMAND_REGISTRATION_DONE
5111 };
5112
5113 /** */
5114 static int stlink_dap_init(void)
5115 {
5116 enum reset_types jtag_reset_config = jtag_get_reset_config();
5117 enum stlink_mode mode;
5118 int retval;
5119
5120 LOG_DEBUG("stlink_dap_init()");
5121
5122 if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
5123 if (jtag_reset_config & RESET_SRST_NO_GATING)
5124 stlink_dap_param.connect_under_reset = true;
5125 else
5126 LOG_WARNING("\'srst_nogate\' reset_config option is required");
5127 }
5128
5129 if (transport_is_dapdirect_swd())
5130 mode = STLINK_MODE_DEBUG_SWD;
5131 else if (transport_is_dapdirect_jtag())
5132 mode = STLINK_MODE_DEBUG_JTAG;
5133 else if (transport_is_swim())
5134 mode = STLINK_MODE_DEBUG_SWIM;
5135 else {
5136 LOG_ERROR("Unsupported transport");
5137 return ERROR_FAIL;
5138 }
5139
5140 retval = stlink_open(&stlink_dap_param, mode, (void **)&stlink_dap_handle);
5141 if (retval != ERROR_OK)
5142 return retval;
5143
5144 if ((mode != STLINK_MODE_DEBUG_SWIM) &&
5145 !(stlink_dap_handle->version.flags & STLINK_F_HAS_DAP_REG)) {
5146 LOG_ERROR("ST-Link version does not support DAP direct transport");
5147 return ERROR_FAIL;
5148 }
5149 return ERROR_OK;
5150 }
5151
5152 /** */
5153 static int stlink_dap_quit(void)
5154 {
5155 LOG_DEBUG("stlink_dap_quit()");
5156
5157 return stlink_close(stlink_dap_handle);
5158 }
5159
5160 /** */
5161 static int stlink_dap_reset(int req_trst, int req_srst)
5162 {
5163 LOG_DEBUG("stlink_dap_reset(%d)", req_srst);
5164 return stlink_usb_assert_srst(stlink_dap_handle,
5165 req_srst ? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
5166 : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH);
5167 }
5168
5169 /** */
5170 static int stlink_dap_speed(int speed)
5171 {
5172 if (speed == 0) {
5173 LOG_ERROR("RTCK not supported. Set nonzero adapter_khz.");
5174 return ERROR_JTAG_NOT_IMPLEMENTED;
5175 }
5176
5177 stlink_dap_param.initial_interface_speed = speed;
5178 stlink_speed(stlink_dap_handle, speed, false);
5179 return ERROR_OK;
5180 }
5181
5182 /** */
5183 static int stlink_dap_khz(int khz, int *jtag_speed)
5184 {
5185 if (khz == 0) {
5186 LOG_ERROR("RCLK not supported");
5187 return ERROR_FAIL;
5188 }
5189
5190 *jtag_speed = stlink_speed(stlink_dap_handle, khz, true);
5191 return ERROR_OK;
5192 }
5193
5194 /** */
5195 static int stlink_dap_speed_div(int speed, int *khz)
5196 {
5197 *khz = speed;
5198 return ERROR_OK;
5199 }
5200
5201 static const struct dap_ops stlink_dap_ops = {
5202 .connect = stlink_dap_op_connect,
5203 .send_sequence = stlink_dap_op_send_sequence,
5204 .queue_dp_read = stlink_dap_op_queue_dp_read,
5205 .queue_dp_write = stlink_dap_op_queue_dp_write,
5206 .queue_ap_read = stlink_dap_op_queue_ap_read,
5207 .queue_ap_write = stlink_dap_op_queue_ap_write,
5208 .queue_ap_abort = stlink_dap_op_queue_ap_abort,
5209 .run = stlink_dap_op_queue_run,
5210 .sync = NULL, /* optional */
5211 .quit = stlink_dap_op_quit, /* optional */
5212 };
5213
5214 static const struct swim_driver stlink_swim_ops = {
5215 .srst = stlink_swim_op_srst,
5216 .read_mem = stlink_swim_op_read_mem,
5217 .write_mem = stlink_swim_op_write_mem,
5218 .reconnect = stlink_swim_op_reconnect,
5219 };
5220
5221 static const char *const stlink_dap_transport[] = { "dapdirect_swd", "dapdirect_jtag", "swim", NULL };
5222
5223 struct adapter_driver stlink_dap_adapter_driver = {
5224 .name = "st-link",
5225 .transports = stlink_dap_transport,
5226 .commands = stlink_dap_command_handlers,
5227
5228 .init = stlink_dap_init,
5229 .quit = stlink_dap_quit,
5230 .reset = stlink_dap_reset,
5231 .speed = stlink_dap_speed,
5232 .khz = stlink_dap_khz,
5233 .speed_div = stlink_dap_speed_div,
5234 .config_trace = stlink_dap_config_trace,
5235 .poll_trace = stlink_dap_trace_read,
5236
5237 .dap_jtag_ops = &stlink_dap_ops,
5238 .dap_swd_ops = &stlink_dap_ops,
5239 .swim_ops = &stlink_swim_ops,
5240 };
Official OpenOCD Mirror