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

Linking to existing account procedure

If you already have an account and want to add another login method you MUST first sign in with your existing account and then change URL to read https://review.openocd.org/login/?link to get to this page again but this time it'll work for linking. Thank you.

SSH host keys fingerprints

1024 SHA256:YKx8b7u5ZWdcbp7/4AeXNaqElP49m6QrwfXaqQGJAOk gerrit-code-review@openocd.zylin.com (DSA)
384 SHA256:jHIbSQa4REvwCFG4cq5LBlBLxmxSqelQPem/EXIrxjk gerrit-code-review@openocd.org (ECDSA)
521 SHA256:UAOPYkU9Fjtcao0Ul/Rrlnj/OsQvt+pgdYSZ4jOYdgs gerrit-code-review@openocd.org (ECDSA)
256 SHA256:A13M5QlnozFOvTllybRZH6vm7iSt0XLxbA48yfc2yfY gerrit-code-review@openocd.org (ECDSA)
256 SHA256:spYMBqEYoAOtK7yZBrcwE8ZpYt6b68Cfh9yEVetvbXg gerrit-code-review@openocd.org (ED25519)
+--[ED25519 256]--+
|=..              |
|+o..   .         |
|*.o   . .        |
|+B . . .         |
|Bo. = o S        |
|Oo.+ + =         |
|oB=.* = . o      |
| =+=.+   + E     |
|. .=o   . o      |
+----[SHA256]-----+
2048 SHA256:0Onrb7/PHjpo6iVZ7xQX2riKN83FJ3KGU0TvI0TaFG4 gerrit-code-review@openocd.zylin.com (RSA)