Code Review / openocd.git / blob
? search:


8a9028bd1ea8ad125198a082c750758b3b9e804a
[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 (is_adiv6(dap)) {
4290 static bool error_flagged;
4291 if (!error_flagged)
4292 LOG_ERROR("ADIv6 dap not supported by stlink dap-direct mode");
4293 error_flagged = true;
4294 return ERROR_FAIL;
4295 }
4296
4297 if (reg != ADIV5_AP_REG_IDR) {
4298 retval = stlink_dap_open_ap(ap->ap_num);
4299 if (retval != ERROR_OK)
4300 return retval;
4301 }
4302 data = data ? data : &dummy;
4303 retval = stlink_read_dap_register(stlink_dap_handle, ap->ap_num, reg,
4304 data);
4305 dap->stlink_flush_ap_write = false;
4306 return retval;
4307 }
4308
4309 /** */
4310 static int stlink_dap_ap_write(struct adiv5_ap *ap, unsigned int reg, uint32_t data)
4311 {
4312 struct adiv5_dap *dap = ap->dap;
4313 int retval;
4314
4315 if (is_adiv6(dap)) {
4316 static bool error_flagged;
4317 if (!error_flagged)
4318 LOG_ERROR("ADIv6 dap not supported by stlink dap-direct mode");
4319 error_flagged = true;
4320 return ERROR_FAIL;
4321 }
4322
4323 retval = stlink_dap_open_ap(ap->ap_num);
4324 if (retval != ERROR_OK)
4325 return retval;
4326
4327 retval = stlink_write_dap_register(stlink_dap_handle, ap->ap_num, reg,
4328 data);
4329 dap->stlink_flush_ap_write = true;
4330 return retval;
4331 }
4332
4333 /** */
4334 static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
4335 {
4336 LOG_WARNING("stlink_dap_op_queue_ap_abort()");
4337 return ERROR_OK;
4338 }
4339
4340 #define RW_MISC_CMD_ADDRESS 1
4341 #define RW_MISC_CMD_WRITE 2
4342 #define RW_MISC_CMD_READ 3
4343 #define RW_MISC_CMD_APNUM 5
4344
4345 static int stlink_usb_misc_rw_segment(void *handle, const struct dap_queue *q, unsigned int len, unsigned int items)
4346 {
4347 uint8_t buf[2 * 4 * items];
4348
4349 LOG_DEBUG("Queue: %u commands in %u items", len, items);
4350
4351 int ap_num = DP_APSEL_INVALID;
4352 unsigned int cmd_index = 0;
4353 unsigned int val_index = ALIGN_UP(items, 4);
4354 for (unsigned int i = 0; i < len; i++) {
4355 if (ap_num != q[i].mem_ap.ap->ap_num) {
4356 ap_num = q[i].mem_ap.ap->ap_num;
4357 buf[cmd_index++] = RW_MISC_CMD_APNUM;
4358 h_u32_to_le(&buf[val_index], ap_num);
4359 val_index += 4;
4360 }
4361
4362 switch (q[i].cmd) {
4363 case CMD_MEM_AP_READ32:
4364 buf[cmd_index++] = RW_MISC_CMD_READ;
4365 h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
4366 val_index += 4;
4367 break;
4368 case CMD_MEM_AP_WRITE32:
4369 buf[cmd_index++] = RW_MISC_CMD_ADDRESS;
4370 h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
4371 val_index += 4;
4372 buf[cmd_index++] = RW_MISC_CMD_WRITE;
4373 h_u32_to_le(&buf[val_index], q[i].mem_ap.data);
4374 val_index += 4;
4375 break;
4376 default:
4377 /* Not supposed to happen */
4378 return ERROR_FAIL;
4379 }
4380 }
4381 /* pad after last command */
4382 while (!IS_ALIGNED(cmd_index, 4))
4383 buf[cmd_index++] = 0;
4384
4385 int retval = stlink_usb_rw_misc_out(handle, items, buf);
4386 if (retval != ERROR_OK)
4387 return retval;
4388
4389 retval = stlink_usb_rw_misc_in(handle, items, buf);
4390 if (retval != ERROR_OK)
4391 return retval;
4392
4393 ap_num = DP_APSEL_INVALID;
4394 val_index = 0;
4395 unsigned int err_index = 4 * items;
4396 for (unsigned int i = 0; i < len; i++) {
4397 uint32_t errcode = le_to_h_u32(&buf[err_index]);
4398 if (errcode != STLINK_DEBUG_ERR_OK) {
4399 LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
4400 return ERROR_FAIL;
4401 }
4402 if (ap_num != q[i].mem_ap.ap->ap_num) {
4403 ap_num = q[i].mem_ap.ap->ap_num;
4404 err_index += 4;
4405 val_index += 4;
4406 errcode = le_to_h_u32(&buf[err_index]);
4407 if (errcode != STLINK_DEBUG_ERR_OK) {
4408 LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
4409 return ERROR_FAIL;
4410 }
4411 }
4412
4413 if (q[i].cmd == CMD_MEM_AP_READ32) {
4414 *q[i].mem_ap.p_data = le_to_h_u32(&buf[val_index]);
4415 } else { /* q[i]->cmd == CMD_MEM_AP_WRITE32 */
4416 err_index += 4;
4417 val_index += 4;
4418 errcode = le_to_h_u32(&buf[err_index]);
4419 if (errcode != STLINK_DEBUG_ERR_OK) {
4420 LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
4421 return ERROR_FAIL;
4422 }
4423 }
4424 err_index += 4;
4425 val_index += 4;
4426 }
4427
4428 return ERROR_OK;
4429 }
4430
4431 static int stlink_usb_buf_rw_segment(void *handle, const struct dap_queue *q, unsigned int count)
4432 {
4433 uint32_t bufsize = count * CMD_MEM_AP_2_SIZE(q[0].cmd);
4434 uint8_t buf[bufsize];
4435 uint8_t ap_num = q[0].mem_ap.ap->ap_num;
4436 uint32_t addr = q[0].mem_ap.addr;
4437 uint32_t csw = q[0].mem_ap.csw;
4438
4439 int retval = stlink_dap_open_ap(ap_num);
4440 if (retval != ERROR_OK)
4441 return retval;
4442
4443 switch (q[0].cmd) {
4444 case CMD_MEM_AP_WRITE8:
4445 for (unsigned int i = 0; i < count; i++)
4446 buf[i] = q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 3);
4447 return stlink_usb_write_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4448
4449 case CMD_MEM_AP_WRITE16:
4450 for (unsigned int i = 0; i < count; i++)
4451 h_u16_to_le(&buf[2 * i], q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 2));
4452 return stlink_usb_write_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4453
4454 case CMD_MEM_AP_WRITE32:
4455 for (unsigned int i = 0; i < count; i++)
4456 h_u32_to_le(&buf[4 * i], q[i].mem_ap.data);
4457 if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
4458 return stlink_usb_write_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4459 else
4460 return stlink_usb_write_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4461
4462 case CMD_MEM_AP_READ8:
4463 retval = stlink_usb_read_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4464 if (retval == ERROR_OK)
4465 for (unsigned int i = 0; i < count; i++)
4466 *q[i].mem_ap.p_data = buf[i] << 8 * (q[i].mem_ap.addr & 3);
4467 return retval;
4468
4469 case CMD_MEM_AP_READ16:
4470 retval = stlink_usb_read_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4471 if (retval == ERROR_OK)
4472 for (unsigned int i = 0; i < count; i++)
4473 *q[i].mem_ap.p_data = le_to_h_u16(&buf[2 * i]) << 8 * (q[i].mem_ap.addr & 2);
4474 return retval;
4475
4476 case CMD_MEM_AP_READ32:
4477 if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
4478 retval = stlink_usb_read_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4479 else
4480 retval = stlink_usb_read_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4481 if (retval == ERROR_OK)
4482 for (unsigned int i = 0; i < count; i++)
4483 *q[i].mem_ap.p_data = le_to_h_u32(&buf[4 * i]);
4484 return retval;
4485
4486 default:
4487 return ERROR_FAIL;
4488 };
4489 }
4490
4491 /* TODO: recover these values with cmd STLINK_DEBUG_APIV2_RW_MISC_GET_MAX (0x53) */
4492 #define STLINK_V2_RW_MISC_SIZE (64)
4493 #define STLINK_V3_RW_MISC_SIZE (1227)
4494
4495 static int stlink_usb_count_misc_rw_queue(void *handle, const struct dap_queue *q, unsigned int len,
4496 unsigned int *pkt_items)
4497 {
4498 struct stlink_usb_handle_s *h = handle;
4499 unsigned int i, items = 0;
4500 int ap_num = DP_APSEL_INVALID;
4501 unsigned int misc_max_items = (h->version.stlink == 2) ? STLINK_V2_RW_MISC_SIZE : STLINK_V3_RW_MISC_SIZE;
4502
4503 if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
4504 return 0;
4505 /*
4506 * Before stlink-server API v3, RW_MISC sequence doesn't lock the st-link,
4507 * so are not safe in shared mode.
4508 * Don't use it with TCP backend to prevent any issue in case of sharing.
4509 * This further degrades the performance, on top of TCP server overhead.
4510 */
4511 if (h->backend == &stlink_tcp_backend && h->tcp_backend_priv.version.api < 3)
4512 return 0;
4513
4514 for (i = 0; i < len; i++) {
4515 if (q[i].cmd != CMD_MEM_AP_READ32 && q[i].cmd != CMD_MEM_AP_WRITE32)
4516 break;
4517 unsigned int count = 1;
4518 if (ap_num != q[i].mem_ap.ap->ap_num) {
4519 count++;
4520 ap_num = q[i].mem_ap.ap->ap_num;
4521 }
4522 if (q[i].cmd == CMD_MEM_AP_WRITE32)
4523 count++;
4524 if (items + count > misc_max_items)
4525 break;
4526 items += count;
4527 }
4528
4529 *pkt_items = items;
4530
4531 return i;
4532 }
4533
4534 static int stlink_usb_count_buf_rw_queue(const struct dap_queue *q, unsigned int len)
4535 {
4536 uint32_t incr = CMD_MEM_AP_2_SIZE(q[0].cmd);
4537 unsigned int len_max;
4538
4539 if (incr == 1)
4540 len_max = stlink_usb_block(stlink_dap_handle);
4541 else
4542 len_max = STLINK_MAX_RW16_32 / incr;
4543
4544 /* check for no address increment, 32 bits only */
4545 if (len > 1 && incr == 4 && q[0].mem_ap.addr == q[1].mem_ap.addr)
4546 incr = 0;
4547
4548 if (len > len_max)
4549 len = len_max;
4550
4551 for (unsigned int i = 1; i < len; i++)
4552 if (q[i].cmd != q[0].cmd ||
4553 q[i].mem_ap.ap != q[0].mem_ap.ap ||
4554 q[i].mem_ap.csw != q[0].mem_ap.csw ||
4555 q[i].mem_ap.addr != q[i - 1].mem_ap.addr + incr)
4556 return i;
4557
4558 return len;
4559 }
4560
4561 static int stlink_usb_mem_rw_queue(void *handle, const struct dap_queue *q, unsigned int len, unsigned int *skip)
4562 {
4563 unsigned int count, misc_items = 0;
4564 int retval;
4565
4566 unsigned int count_misc = stlink_usb_count_misc_rw_queue(handle, q, len, &misc_items);
4567 unsigned int count_buf = stlink_usb_count_buf_rw_queue(q, len);
4568
4569 if (count_misc > count_buf) {
4570 count = count_misc;
4571 retval = stlink_usb_misc_rw_segment(handle, q, count, misc_items);
4572 } else {
4573 count = count_buf;
4574 retval = stlink_usb_buf_rw_segment(handle, q, count_buf);
4575 }
4576 if (retval != ERROR_OK)
4577 return retval;
4578
4579 *skip = count;
4580 return ERROR_OK;
4581 }
4582
4583 static void stlink_dap_run_internal(struct adiv5_dap *dap)
4584 {
4585 int retval = stlink_dap_check_reconnect(dap);
4586 if (retval != ERROR_OK) {
4587 stlink_dap_handle->queue_index = 0;
4588 stlink_dap_record_error(retval);
4589 return;
4590 }
4591
4592 unsigned int i = stlink_dap_handle->queue_index;
4593 struct dap_queue *q = &stlink_dap_handle->queue[0];
4594
4595 while (i && stlink_dap_get_error() == ERROR_OK) {
4596 unsigned int skip = 1;
4597
4598 switch (q->cmd) {
4599 case CMD_DP_READ:
4600 retval = stlink_dap_dp_read(q->dp_r.dap, q->dp_r.reg, q->dp_r.p_data);
4601 break;
4602 case CMD_DP_WRITE:
4603 retval = stlink_dap_dp_write(q->dp_w.dap, q->dp_w.reg, q->dp_w.data);
4604 break;
4605 case CMD_AP_READ:
4606 retval = stlink_dap_ap_read(q->ap_r.ap, q->ap_r.reg, q->ap_r.p_data);
4607 break;
4608 case CMD_AP_WRITE:
4609 /* ignore increment packed, not supported */
4610 if (q->ap_w.reg == ADIV5_MEM_AP_REG_CSW)
4611 q->ap_w.data &= ~CSW_ADDRINC_PACKED;
4612 retval = stlink_dap_ap_write(q->ap_w.ap, q->ap_w.reg, q->ap_w.data);
4613 break;
4614
4615 case CMD_MEM_AP_READ8:
4616 case CMD_MEM_AP_READ16:
4617 case CMD_MEM_AP_READ32:
4618 case CMD_MEM_AP_WRITE8:
4619 case CMD_MEM_AP_WRITE16:
4620 case CMD_MEM_AP_WRITE32:
4621 retval = stlink_usb_mem_rw_queue(stlink_dap_handle, q, i, &skip);
4622 break;
4623
4624 default:
4625 LOG_ERROR("ST-Link: Unknown queue command %d", q->cmd);
4626 retval = ERROR_FAIL;
4627 break;
4628 }
4629 stlink_dap_record_error(retval);
4630 q += skip;
4631 i -= skip;
4632 }
4633
4634 stlink_dap_handle->queue_index = 0;
4635 }
4636
4637 /** */
4638 static int stlink_dap_run_finalize(struct adiv5_dap *dap)
4639 {
4640 uint32_t ctrlstat, pwrmask;
4641 int retval, saved_retval;
4642
4643 /* Here no LOG_DEBUG. This is called continuously! */
4644
4645 /*
4646 * ST-Link returns immediately after a DAP write, without waiting for it
4647 * to complete.
4648 * Run a dummy read to DP_RDBUFF, as suggested in
4649 * http://infocenter.arm.com/help/topic/com.arm.doc.faqs/ka16363.html
4650 */
4651 if (dap->stlink_flush_ap_write) {
4652 dap->stlink_flush_ap_write = false;
4653 retval = stlink_dap_dp_read(dap, DP_RDBUFF, NULL);
4654 if (retval != ERROR_OK) {
4655 dap->do_reconnect = true;
4656 return retval;
4657 }
4658 }
4659
4660 saved_retval = stlink_dap_get_and_clear_error();
4661
4662 retval = stlink_dap_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
4663 if (retval != ERROR_OK) {
4664 LOG_ERROR("Fail reading CTRL/STAT register. Force reconnect");
4665 dap->do_reconnect = true;
4666 return retval;
4667 }
4668
4669 if (ctrlstat & SSTICKYERR) {
4670 if (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG)
4671 retval = stlink_dap_dp_write(dap, DP_CTRL_STAT,
4672 ctrlstat & (dap->dp_ctrl_stat | SSTICKYERR));
4673 else
4674 retval = stlink_dap_dp_write(dap, DP_ABORT, STKERRCLR);
4675 if (retval != ERROR_OK) {
4676 dap->do_reconnect = true;
4677 return retval;
4678 }
4679 }
4680
4681 /* check for power lost */
4682 pwrmask = dap->dp_ctrl_stat & (CDBGPWRUPREQ | CSYSPWRUPREQ);
4683 if ((ctrlstat & pwrmask) != pwrmask)
4684 dap->do_reconnect = true;
4685
4686 return saved_retval;
4687 }
4688
4689 static int stlink_dap_op_queue_run(struct adiv5_dap *dap)
4690 {
4691 stlink_dap_run_internal(dap);
4692 return stlink_dap_run_finalize(dap);
4693 }
4694
4695 /** */
4696 static void stlink_dap_op_quit(struct adiv5_dap *dap)
4697 {
4698 int retval;
4699
4700 retval = stlink_dap_closeall_ap();
4701 if (retval != ERROR_OK)
4702 LOG_ERROR("Error closing APs");
4703 }
4704
4705 static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned int reg,
4706 uint32_t *data)
4707 {
4708 if (stlink_dap_get_error() != ERROR_OK)
4709 return ERROR_OK;
4710
4711 unsigned int i = stlink_dap_handle->queue_index++;
4712 struct dap_queue *q = &stlink_dap_handle->queue[i];
4713 q->cmd = CMD_DP_READ;
4714 q->dp_r.reg = reg;
4715 q->dp_r.dap = dap;
4716 q->dp_r.p_data = data;
4717
4718 if (i == MAX_QUEUE_DEPTH - 1)
4719 stlink_dap_run_internal(dap);
4720
4721 return ERROR_OK;
4722 }
4723
4724 static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned int reg,
4725 uint32_t data)
4726 {
4727 if (stlink_dap_get_error() != ERROR_OK)
4728 return ERROR_OK;
4729
4730 unsigned int i = stlink_dap_handle->queue_index++;
4731 struct dap_queue *q = &stlink_dap_handle->queue[i];
4732 q->cmd = CMD_DP_WRITE;
4733 q->dp_w.reg = reg;
4734 q->dp_w.dap = dap;
4735 q->dp_w.data = data;
4736
4737 if (i == MAX_QUEUE_DEPTH - 1)
4738 stlink_dap_run_internal(dap);
4739
4740 return ERROR_OK;
4741 }
4742
4743 static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned int reg,
4744 uint32_t *data)
4745 {
4746 if (stlink_dap_get_error() != ERROR_OK)
4747 return ERROR_OK;
4748
4749 unsigned int i = stlink_dap_handle->queue_index++;
4750 struct dap_queue *q = &stlink_dap_handle->queue[i];
4751
4752 /* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_RD_NO_INC
4753 * and STLINK_F_HAS_RW_MISC */
4754 if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) &&
4755 (reg == ADIV5_MEM_AP_REG_DRW || reg == ADIV5_MEM_AP_REG_BD0 || reg == ADIV5_MEM_AP_REG_BD1 ||
4756 reg == ADIV5_MEM_AP_REG_BD2 || reg == ADIV5_MEM_AP_REG_BD3)) {
4757 /* de-queue previous write-TAR */
4758 struct dap_queue *prev_q = q - 1;
4759 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_TAR) {
4760 stlink_dap_handle->queue_index = i;
4761 i--;
4762 q = prev_q;
4763 prev_q--;
4764 }
4765 /* de-queue previous write-CSW if it didn't changed ap->csw_default */
4766 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_CSW &&
4767 !prev_q->ap_w.changes_csw_default) {
4768 stlink_dap_handle->queue_index = i;
4769 q = prev_q;
4770 }
4771
4772 switch (ap->csw_value & CSW_SIZE_MASK) {
4773 case CSW_8BIT:
4774 q->cmd = CMD_MEM_AP_READ8;
4775 break;
4776 case CSW_16BIT:
4777 q->cmd = CMD_MEM_AP_READ16;
4778 break;
4779 case CSW_32BIT:
4780 q->cmd = CMD_MEM_AP_READ32;
4781 break;
4782 default:
4783 LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK);
4784 stlink_dap_record_error(ERROR_FAIL);
4785 return ERROR_FAIL;
4786 }
4787
4788 q->mem_ap.addr = (reg == ADIV5_MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c));
4789 q->mem_ap.ap = ap;
4790 q->mem_ap.p_data = data;
4791 q->mem_ap.csw = ap->csw_default;
4792
4793 /* force TAR and CSW update */
4794 ap->tar_valid = false;
4795 ap->csw_value = 0;
4796 } else {
4797 q->cmd = CMD_AP_READ;
4798 q->ap_r.reg = reg;
4799 q->ap_r.ap = ap;
4800 q->ap_r.p_data = data;
4801 }
4802
4803 if (i == MAX_QUEUE_DEPTH - 1)
4804 stlink_dap_run_internal(ap->dap);
4805
4806 return ERROR_OK;
4807 }
4808
4809 static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned int reg,
4810 uint32_t data)
4811 {
4812 if (stlink_dap_get_error() != ERROR_OK)
4813 return ERROR_OK;
4814
4815 unsigned int i = stlink_dap_handle->queue_index++;
4816 struct dap_queue *q = &stlink_dap_handle->queue[i];
4817
4818 /* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_WR_NO_INC
4819 * and STLINK_F_HAS_RW_MISC */
4820 if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) &&
4821 (reg == ADIV5_MEM_AP_REG_DRW || reg == ADIV5_MEM_AP_REG_BD0 || reg == ADIV5_MEM_AP_REG_BD1 ||
4822 reg == ADIV5_MEM_AP_REG_BD2 || reg == ADIV5_MEM_AP_REG_BD3)) {
4823 /* de-queue previous write-TAR */
4824 struct dap_queue *prev_q = q - 1;
4825 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_TAR) {
4826 stlink_dap_handle->queue_index = i;
4827 i--;
4828 q = prev_q;
4829 prev_q--;
4830 }
4831 /* de-queue previous write-CSW if it didn't changed ap->csw_default */
4832 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_CSW &&
4833 !prev_q->ap_w.changes_csw_default) {
4834 stlink_dap_handle->queue_index = i;
4835 q = prev_q;
4836 }
4837
4838 switch (ap->csw_value & CSW_SIZE_MASK) {
4839 case CSW_8BIT:
4840 q->cmd = CMD_MEM_AP_WRITE8;
4841 break;
4842 case CSW_16BIT:
4843 q->cmd = CMD_MEM_AP_WRITE16;
4844 break;
4845 case CSW_32BIT:
4846 q->cmd = CMD_MEM_AP_WRITE32;
4847 break;
4848 default:
4849 LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK);
4850 stlink_dap_record_error(ERROR_FAIL);
4851 return ERROR_FAIL;
4852 }
4853
4854 q->mem_ap.addr = (reg == ADIV5_MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c));
4855 q->mem_ap.ap = ap;
4856 q->mem_ap.data = data;
4857 q->mem_ap.csw = ap->csw_default;
4858
4859 /* force TAR and CSW update */
4860 ap->tar_valid = false;
4861 ap->csw_value = 0;
4862 } else {
4863 q->cmd = CMD_AP_WRITE;
4864 q->ap_w.reg = reg;
4865 q->ap_w.ap = ap;
4866 q->ap_w.data = data;
4867 if (reg == ADIV5_MEM_AP_REG_CSW && ap->csw_default != last_csw_default[ap->ap_num]) {
4868 q->ap_w.changes_csw_default = true;
4869 last_csw_default[ap->ap_num] = ap->csw_default;
4870 } else {
4871 q->ap_w.changes_csw_default = false;
4872 }
4873 }
4874
4875 if (i == MAX_QUEUE_DEPTH - 1)
4876 stlink_dap_run_internal(ap->dap);
4877
4878 return ERROR_OK;
4879 }
4880
4881 static int stlink_swim_op_srst(void)
4882 {
4883 return stlink_swim_generate_rst(stlink_dap_handle);
4884 }
4885
4886 static int stlink_swim_op_read_mem(uint32_t addr, uint32_t size,
4887 uint32_t count, uint8_t *buffer)
4888 {
4889 int retval;
4890 uint32_t bytes_remaining;
4891
4892 LOG_DEBUG_IO("read at 0x%08" PRIx32 " len %" PRIu32 "*0x%08" PRIx32, addr, size, count);
4893 count *= size;
4894
4895 while (count) {
4896 bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count;
4897 retval = stlink_swim_readbytes(stlink_dap_handle, addr, bytes_remaining, buffer);
4898 if (retval != ERROR_OK)
4899 return retval;
4900
4901 buffer += bytes_remaining;
4902 addr += bytes_remaining;
4903 count -= bytes_remaining;
4904 }
4905
4906 return ERROR_OK;
4907 }
4908
4909 static int stlink_swim_op_write_mem(uint32_t addr, uint32_t size,
4910 uint32_t count, const uint8_t *buffer)
4911 {
4912 int retval;
4913 uint32_t bytes_remaining;
4914
4915 LOG_DEBUG_IO("write at 0x%08" PRIx32 " len %" PRIu32 "*0x%08" PRIx32, addr, size, count);
4916 count *= size;
4917
4918 while (count) {
4919 bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count;
4920 retval = stlink_swim_writebytes(stlink_dap_handle, addr, bytes_remaining, buffer);
4921 if (retval != ERROR_OK)
4922 return retval;
4923
4924 buffer += bytes_remaining;
4925 addr += bytes_remaining;
4926 count -= bytes_remaining;
4927 }
4928
4929 return ERROR_OK;
4930 }
4931
4932 static int stlink_swim_op_reconnect(void)
4933 {
4934 int retval;
4935
4936 retval = stlink_usb_mode_enter(stlink_dap_handle, STLINK_MODE_DEBUG_SWIM);
4937 if (retval != ERROR_OK)
4938 return retval;
4939
4940 return stlink_swim_resync(stlink_dap_handle);
4941 }
4942
4943 static int stlink_dap_config_trace(bool enabled,
4944 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
4945 unsigned int *trace_freq, unsigned int traceclkin_freq,
4946 uint16_t *prescaler)
4947 {
4948 return stlink_config_trace(stlink_dap_handle, enabled, pin_protocol,
4949 port_size, trace_freq, traceclkin_freq,
4950 prescaler);
4951 }
4952
4953 static int stlink_dap_trace_read(uint8_t *buf, size_t *size)
4954 {
4955 return stlink_usb_trace_read(stlink_dap_handle, buf, size);
4956 }
4957
4958 /** */
4959 COMMAND_HANDLER(stlink_dap_vid_pid)
4960 {
4961 unsigned int i, max_usb_ids = HLA_MAX_USB_IDS;
4962
4963 if (CMD_ARGC > max_usb_ids * 2) {
4964 LOG_WARNING("ignoring extra IDs in vid_pid "
4965 "(maximum is %d pairs)", max_usb_ids);
4966 CMD_ARGC = max_usb_ids * 2;
4967 }
4968 if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
4969 LOG_WARNING("incomplete vid_pid configuration directive");
4970 return ERROR_COMMAND_SYNTAX_ERROR;
4971 }
4972 for (i = 0; i < CMD_ARGC; i += 2) {
4973 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], stlink_dap_param.vid[i / 2]);
4974 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], stlink_dap_param.pid[i / 2]);
4975 }
4976
4977 /* null termination */
4978 stlink_dap_param.vid[i / 2] = stlink_dap_param.pid[i / 2] = 0;
4979
4980 return ERROR_OK;
4981 }
4982
4983 /** */
4984 COMMAND_HANDLER(stlink_dap_backend_command)
4985 {
4986 /* default values */
4987 bool use_stlink_tcp = false;
4988 uint16_t stlink_tcp_port = 7184;
4989
4990 if (CMD_ARGC == 0 || CMD_ARGC > 2)
4991 return ERROR_COMMAND_SYNTAX_ERROR;
4992 else if (strcmp(CMD_ARGV[0], "usb") == 0) {
4993 if (CMD_ARGC > 1)
4994 return ERROR_COMMAND_SYNTAX_ERROR;
4995 /* else use_stlink_tcp = false (already the case ) */
4996 } else if (strcmp(CMD_ARGV[0], "tcp") == 0) {
4997 use_stlink_tcp = true;
4998 if (CMD_ARGC == 2)
4999 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], stlink_tcp_port);
5000 } else
5001 return ERROR_COMMAND_SYNTAX_ERROR;
5002
5003 stlink_dap_param.use_stlink_tcp = use_stlink_tcp;
5004 stlink_dap_param.stlink_tcp_port = stlink_tcp_port;
5005
5006 return ERROR_OK;
5007 }
5008
5009 #define BYTES_PER_LINE 16
5010 COMMAND_HANDLER(stlink_dap_cmd_command)
5011 {
5012 unsigned int rx_n, tx_n;
5013 struct stlink_usb_handle_s *h = stlink_dap_handle;
5014
5015 if (CMD_ARGC < 2)
5016 return ERROR_COMMAND_SYNTAX_ERROR;
5017
5018 COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], rx_n);
5019 tx_n = CMD_ARGC - 1;
5020 if (tx_n > STLINK_SG_SIZE || rx_n > STLINK_DATA_SIZE) {
5021 LOG_ERROR("max %x byte sent and %d received", STLINK_SG_SIZE, STLINK_DATA_SIZE);
5022 return ERROR_COMMAND_SYNTAX_ERROR;
5023 }
5024
5025 stlink_usb_init_buffer(h, h->rx_ep, rx_n);
5026
5027 for (unsigned int i = 0; i < tx_n; i++) {
5028 uint8_t byte;
5029 COMMAND_PARSE_NUMBER(u8, CMD_ARGV[i + 1], byte);
5030 h->cmdbuf[h->cmdidx++] = byte;
5031 }
5032
5033 int retval = stlink_usb_xfer_noerrcheck(h, h->databuf, rx_n);
5034 if (retval != ERROR_OK) {
5035 LOG_ERROR("Error %d", retval);
5036 return retval;
5037 }
5038
5039 for (unsigned int i = 0; i < rx_n; i++)
5040 command_print_sameline(CMD, "0x%02x%c", h->databuf[i],
5041 ((i == (rx_n - 1)) || ((i % BYTES_PER_LINE) == (BYTES_PER_LINE - 1))) ? '\n' : ' ');
5042
5043 return ERROR_OK;
5044 }
5045
5046 /** */
5047 static const struct command_registration stlink_dap_subcommand_handlers[] = {
5048 {
5049 .name = "vid_pid",
5050 .handler = stlink_dap_vid_pid,
5051 .mode = COMMAND_CONFIG,
5052 .help = "USB VID and PID of the adapter",
5053 .usage = "(vid pid)+",
5054 },
5055 {
5056 .name = "backend",
5057 .handler = &stlink_dap_backend_command,
5058 .mode = COMMAND_CONFIG,
5059 .help = "select which ST-Link backend to use",
5060 .usage = "usb | tcp [port]",
5061 },
5062 {
5063 .name = "cmd",
5064 .handler = stlink_dap_cmd_command,
5065 .mode = COMMAND_EXEC,
5066 .help = "send arbitrary command",
5067 .usage = "rx_n (tx_byte)+",
5068 },
5069 COMMAND_REGISTRATION_DONE
5070 };
5071
5072 /** */
5073 static const struct command_registration stlink_dap_command_handlers[] = {
5074 {
5075 .name = "st-link",
5076 .mode = COMMAND_ANY,
5077 .help = "perform st-link management",
5078 .chain = stlink_dap_subcommand_handlers,
5079 .usage = "",
5080 },
5081 COMMAND_REGISTRATION_DONE
5082 };
5083
5084 /** */
5085 static int stlink_dap_init(void)
5086 {
5087 enum reset_types jtag_reset_config = jtag_get_reset_config();
5088 enum stlink_mode mode;
5089 int retval;
5090
5091 LOG_DEBUG("stlink_dap_init()");
5092
5093 if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
5094 if (jtag_reset_config & RESET_SRST_NO_GATING)
5095 stlink_dap_param.connect_under_reset = true;
5096 else
5097 LOG_WARNING("\'srst_nogate\' reset_config option is required");
5098 }
5099
5100 if (transport_is_dapdirect_swd())
5101 mode = STLINK_MODE_DEBUG_SWD;
5102 else if (transport_is_dapdirect_jtag())
5103 mode = STLINK_MODE_DEBUG_JTAG;
5104 else if (transport_is_swim())
5105 mode = STLINK_MODE_DEBUG_SWIM;
5106 else {
5107 LOG_ERROR("Unsupported transport");
5108 return ERROR_FAIL;
5109 }
5110
5111 retval = stlink_open(&stlink_dap_param, mode, (void **)&stlink_dap_handle);
5112 if (retval != ERROR_OK)
5113 return retval;
5114
5115 if ((mode != STLINK_MODE_DEBUG_SWIM) &&
5116 !(stlink_dap_handle->version.flags & STLINK_F_HAS_DAP_REG)) {
5117 LOG_ERROR("ST-Link version does not support DAP direct transport");
5118 return ERROR_FAIL;
5119 }
5120 return ERROR_OK;
5121 }
5122
5123 /** */
5124 static int stlink_dap_quit(void)
5125 {
5126 LOG_DEBUG("stlink_dap_quit()");
5127
5128 return stlink_close(stlink_dap_handle);
5129 }
5130
5131 /** */
5132 static int stlink_dap_reset(int req_trst, int req_srst)
5133 {
5134 LOG_DEBUG("stlink_dap_reset(%d)", req_srst);
5135 return stlink_usb_assert_srst(stlink_dap_handle,
5136 req_srst ? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
5137 : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH);
5138 }
5139
5140 /** */
5141 static int stlink_dap_speed(int speed)
5142 {
5143 if (speed == 0) {
5144 LOG_ERROR("RTCK not supported. Set nonzero adapter_khz.");
5145 return ERROR_JTAG_NOT_IMPLEMENTED;
5146 }
5147
5148 stlink_dap_param.initial_interface_speed = speed;
5149 stlink_speed(stlink_dap_handle, speed, false);
5150 return ERROR_OK;
5151 }
5152
5153 /** */
5154 static int stlink_dap_khz(int khz, int *jtag_speed)
5155 {
5156 if (khz == 0) {
5157 LOG_ERROR("RCLK not supported");
5158 return ERROR_FAIL;
5159 }
5160
5161 *jtag_speed = stlink_speed(stlink_dap_handle, khz, true);
5162 return ERROR_OK;
5163 }
5164
5165 /** */
5166 static int stlink_dap_speed_div(int speed, int *khz)
5167 {
5168 *khz = speed;
5169 return ERROR_OK;
5170 }
5171
5172 static const struct dap_ops stlink_dap_ops = {
5173 .connect = stlink_dap_op_connect,
5174 .send_sequence = stlink_dap_op_send_sequence,
5175 .queue_dp_read = stlink_dap_op_queue_dp_read,
5176 .queue_dp_write = stlink_dap_op_queue_dp_write,
5177 .queue_ap_read = stlink_dap_op_queue_ap_read,
5178 .queue_ap_write = stlink_dap_op_queue_ap_write,
5179 .queue_ap_abort = stlink_dap_op_queue_ap_abort,
5180 .run = stlink_dap_op_queue_run,
5181 .sync = NULL, /* optional */
5182 .quit = stlink_dap_op_quit, /* optional */
5183 };
5184
5185 static const struct swim_driver stlink_swim_ops = {
5186 .srst = stlink_swim_op_srst,
5187 .read_mem = stlink_swim_op_read_mem,
5188 .write_mem = stlink_swim_op_write_mem,
5189 .reconnect = stlink_swim_op_reconnect,
5190 };
5191
5192 static const char *const stlink_dap_transport[] = { "dapdirect_swd", "dapdirect_jtag", "swim", NULL };
5193
5194 struct adapter_driver stlink_dap_adapter_driver = {
5195 .name = "st-link",
5196 .transports = stlink_dap_transport,
5197 .commands = stlink_dap_command_handlers,
5198
5199 .init = stlink_dap_init,
5200 .quit = stlink_dap_quit,
5201 .reset = stlink_dap_reset,
5202 .speed = stlink_dap_speed,
5203 .khz = stlink_dap_khz,
5204 .speed_div = stlink_dap_speed_div,
5205 .config_trace = stlink_dap_config_trace,
5206 .poll_trace = stlink_dap_trace_read,
5207
5208 .dap_jtag_ops = &stlink_dap_ops,
5209 .dap_swd_ops = &stlink_dap_ops,
5210 .swim_ops = &stlink_swim_ops,
5211 };
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)