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