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