Code Review / openocd.git / blob
? search:
summary | shortlog | log | commit | commitdiff | review | tree
history | raw | HEAD
stlink: collapse consecutive mem AP r/w in a single command
[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/binarybuffer.h>
37 #include <helper/bits.h>
38 #include <helper/system.h>
39 #include <jtag/interface.h>
40 #include <jtag/hla/hla_layout.h>
41 #include <jtag/hla/hla_transport.h>
42 #include <jtag/hla/hla_interface.h>
43 #include <jtag/swim.h>
44 #include <target/arm_adi_v5.h>
45 #include <target/target.h>
46 #include <transport/transport.h>
47
48 #include <target/cortex_m.h>
49
50 #include <helper/system.h>
51
52 #ifdef HAVE_ARPA_INET_H
53 #include <arpa/inet.h>
54 #endif
55
56 #ifdef HAVE_NETINET_TCP_H
57 #include <netinet/tcp.h>
58 #endif
59
60 #include "libusb_helper.h"
61
62 #ifdef HAVE_LIBUSB1
63 #define USE_LIBUSB_ASYNCIO
64 #endif
65
66 #define STLINK_SERIAL_LEN 24
67
68 #define ENDPOINT_IN 0x80
69 #define ENDPOINT_OUT 0x00
70
71 #define STLINK_WRITE_TIMEOUT 1000
72 #define STLINK_READ_TIMEOUT 1000
73
74 #define STLINK_RX_EP (1|ENDPOINT_IN)
75 #define STLINK_TX_EP (2|ENDPOINT_OUT)
76 #define STLINK_TRACE_EP (3|ENDPOINT_IN)
77
78 #define STLINK_V2_1_TX_EP (1|ENDPOINT_OUT)
79 #define STLINK_V2_1_TRACE_EP (2|ENDPOINT_IN)
80
81 #define STLINK_SG_SIZE (31)
82 #define STLINK_DATA_SIZE (6144)
83 #define STLINK_CMD_SIZE_V2 (16)
84 #define STLINK_CMD_SIZE_V1 (10)
85
86 #define STLINK_V1_PID (0x3744)
87 #define STLINK_V2_PID (0x3748)
88 #define STLINK_V2_1_PID (0x374B)
89 #define STLINK_V2_1_NO_MSD_PID (0x3752)
90 #define STLINK_V3_USBLOADER_PID (0x374D)
91 #define STLINK_V3E_PID (0x374E)
92 #define STLINK_V3S_PID (0x374F)
93 #define STLINK_V3_2VCP_PID (0x3753)
94 #define STLINK_V3E_NO_MSD_PID (0x3754)
95
96 /*
97 * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and
98 * this limits the bulk packet size and the 8bit read/writes to max 64 bytes.
99 * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes from FW V3J6.
100 *
101 * For 16 and 32bit read/writes stlink handles USB packet split and the limit
102 * is the internal buffer size of 6144 bytes.
103 * TODO: override ADIv5 layer's tar_autoincr_block that limits the transfer
104 * to 1024 or 4096 bytes
105 */
106 #define STLINK_MAX_RW8 (64)
107 #define STLINKV3_MAX_RW8 (512)
108 #define STLINK_MAX_RW16_32 STLINK_DATA_SIZE
109 #define STLINK_SWIM_DATA_SIZE STLINK_DATA_SIZE
110
111 /* "WAIT" responses will be retried (with exponential backoff) at
112 * most this many times before failing to caller.
113 */
114 #define MAX_WAIT_RETRIES 8
115
116 /* HLA is currently limited at AP#0 and no control on CSW */
117 #define STLINK_HLA_AP_NUM 0
118 #define STLINK_HLA_CSW 0
119
120 enum stlink_jtag_api_version {
121 STLINK_JTAG_API_V1 = 1,
122 STLINK_JTAG_API_V2,
123 STLINK_JTAG_API_V3,
124 };
125
126 enum stlink_mode {
127 STLINK_MODE_UNKNOWN = 0,
128 STLINK_MODE_DFU,
129 STLINK_MODE_MASS,
130 STLINK_MODE_DEBUG_JTAG,
131 STLINK_MODE_DEBUG_SWD,
132 STLINK_MODE_DEBUG_SWIM
133 };
134
135 /** */
136 struct stlink_usb_version {
137 /** */
138 int stlink;
139 /** */
140 int jtag;
141 /** */
142 int swim;
143 /** jtag api version supported */
144 enum stlink_jtag_api_version jtag_api;
145 /** one bit for each feature supported. See macros STLINK_F_* */
146 uint32_t flags;
147 };
148
149 struct stlink_usb_priv_s {
150 /** */
151 struct libusb_device_handle *fd;
152 /** */
153 struct libusb_transfer *trans;
154 };
155
156 struct stlink_tcp_priv_s {
157 /** */
158 int fd;
159 /** */
160 bool connected;
161 /** */
162 uint32_t device_id;
163 /** */
164 uint32_t connect_id;
165 /** */
166 uint8_t *send_buf;
167 /** */
168 uint8_t *recv_buf;
169 };
170
171 struct stlink_backend_s {
172 /** */
173 int (*open)(void *handle, struct hl_interface_param_s *param);
174 /** */
175 int (*close)(void *handle);
176 /** */
177 int (*xfer_noerrcheck)(void *handle, const uint8_t *buf, int size);
178 /** */
179 int (*read_trace)(void *handle, const uint8_t *buf, int size);
180 };
181
182 /* TODO: make queue size dynamic */
183 /* TODO: don't allocate queue for HLA */
184 #define MAX_QUEUE_DEPTH (4096)
185
186 enum queue_cmd {
187 CMD_DP_READ = 1,
188 CMD_DP_WRITE,
189
190 CMD_AP_READ,
191 CMD_AP_WRITE,
192
193 /*
194 * encode the bytes size in the enum's value. This makes easy to extract it
195 * with a simple logic AND, by using the macro CMD_MEM_AP_2_SIZE() below
196 */
197 CMD_MEM_AP_READ8 = 0x10 + 1,
198 CMD_MEM_AP_READ16 = 0x10 + 2,
199 CMD_MEM_AP_READ32 = 0x10 + 4,
200
201 CMD_MEM_AP_WRITE8 = 0x20 + 1,
202 CMD_MEM_AP_WRITE16 = 0x20 + 2,
203 CMD_MEM_AP_WRITE32 = 0x20 + 4,
204 };
205
206 #define CMD_MEM_AP_2_SIZE(cmd) ((cmd) & 7)
207
208 struct dap_queue {
209 enum queue_cmd cmd;
210 union {
211 struct dp_r {
212 unsigned int reg;
213 struct adiv5_dap *dap;
214 uint32_t *p_data;
215 } dp_r;
216 struct dp_w {
217 unsigned int reg;
218 struct adiv5_dap *dap;
219 uint32_t data;
220 } dp_w;
221 struct ap_r {
222 unsigned int reg;
223 struct adiv5_ap *ap;
224 uint32_t *p_data;
225 } ap_r;
226 struct ap_w {
227 unsigned int reg;
228 struct adiv5_ap *ap;
229 uint32_t data;
230 } ap_w;
231 struct mem_ap {
232 uint32_t addr;
233 struct adiv5_ap *ap;
234 union {
235 uint32_t *p_data;
236 uint32_t data;
237 };
238 uint32_t csw;
239 } mem_ap;
240 };
241 };
242
243 /** */
244 struct stlink_usb_handle_s {
245 /** */
246 struct stlink_backend_s *backend;
247 /** */
248 union {
249 struct stlink_usb_priv_s usb_backend_priv;
250 struct stlink_tcp_priv_s tcp_backend_priv;
251 };
252 /** */
253 uint8_t rx_ep;
254 /** */
255 uint8_t tx_ep;
256 /** */
257 uint8_t trace_ep;
258 /** */
259 uint8_t *cmdbuf;
260 /** */
261 uint8_t cmdidx;
262 /** */
263 uint8_t direction;
264 /** */
265 uint8_t *databuf;
266 /** */
267 uint32_t max_mem_packet;
268 /** */
269 enum stlink_mode st_mode;
270 /** */
271 struct stlink_usb_version version;
272 /** */
273 uint16_t vid;
274 /** */
275 uint16_t pid;
276 /** */
277 struct {
278 /** whether SWO tracing is enabled or not */
279 bool enabled;
280 /** trace module source clock */
281 uint32_t source_hz;
282 } trace;
283 /** reconnect is needed next time we try to query the
284 * status */
285 bool reconnect_pending;
286 /** queue of dap_direct operations */
287 struct dap_queue queue[MAX_QUEUE_DEPTH];
288 /** first element available in the queue */
289 unsigned int queue_index;
290 };
291
292 /** */
293 static inline int stlink_usb_open(void *handle, struct hl_interface_param_s *param)
294 {
295 struct stlink_usb_handle_s *h = handle;
296 return h->backend->open(handle, param);
297 }
298
299 /** */
300 static inline int stlink_usb_close(void *handle)
301 {
302 struct stlink_usb_handle_s *h = handle;
303 return h->backend->close(handle);
304 }
305 /** */
306 static inline int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
307 {
308 struct stlink_usb_handle_s *h = handle;
309 return h->backend->xfer_noerrcheck(handle, buf, size);
310 }
311
312 #define STLINK_SWIM_ERR_OK 0x00
313 #define STLINK_SWIM_BUSY 0x01
314 #define STLINK_DEBUG_ERR_OK 0x80
315 #define STLINK_DEBUG_ERR_FAULT 0x81
316 #define STLINK_SWD_AP_WAIT 0x10
317 #define STLINK_SWD_AP_FAULT 0x11
318 #define STLINK_SWD_AP_ERROR 0x12
319 #define STLINK_SWD_AP_PARITY_ERROR 0x13
320 #define STLINK_JTAG_GET_IDCODE_ERROR 0x09
321 #define STLINK_JTAG_WRITE_ERROR 0x0c
322 #define STLINK_JTAG_WRITE_VERIF_ERROR 0x0d
323 #define STLINK_SWD_DP_WAIT 0x14
324 #define STLINK_SWD_DP_FAULT 0x15
325 #define STLINK_SWD_DP_ERROR 0x16
326 #define STLINK_SWD_DP_PARITY_ERROR 0x17
327
328 #define STLINK_SWD_AP_WDATA_ERROR 0x18
329 #define STLINK_SWD_AP_STICKY_ERROR 0x19
330 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
331
332 #define STLINK_BAD_AP_ERROR 0x1d
333
334 #define STLINK_CORE_RUNNING 0x80
335 #define STLINK_CORE_HALTED 0x81
336 #define STLINK_CORE_STAT_UNKNOWN -1
337
338 #define STLINK_GET_VERSION 0xF1
339 #define STLINK_DEBUG_COMMAND 0xF2
340 #define STLINK_DFU_COMMAND 0xF3
341 #define STLINK_SWIM_COMMAND 0xF4
342 #define STLINK_GET_CURRENT_MODE 0xF5
343 #define STLINK_GET_TARGET_VOLTAGE 0xF7
344
345 #define STLINK_DEV_DFU_MODE 0x00
346 #define STLINK_DEV_MASS_MODE 0x01
347 #define STLINK_DEV_DEBUG_MODE 0x02
348 #define STLINK_DEV_SWIM_MODE 0x03
349 #define STLINK_DEV_BOOTLOADER_MODE 0x04
350 #define STLINK_DEV_UNKNOWN_MODE -1
351
352 #define STLINK_DFU_EXIT 0x07
353
354 /*
355 STLINK_SWIM_ENTER_SEQ
356 1.3ms low then 750Hz then 1.5kHz
357
358 STLINK_SWIM_GEN_RST
359 STM8 DM pulls reset pin low 50us
360
361 STLINK_SWIM_SPEED
362 uint8_t (0=low|1=high)
363
364 STLINK_SWIM_WRITEMEM
365 uint16_t length
366 uint32_t address
367
368 STLINK_SWIM_RESET
369 send synchronization seq (16us low, response 64 clocks low)
370 */
371 #define STLINK_SWIM_ENTER 0x00
372 #define STLINK_SWIM_EXIT 0x01
373 #define STLINK_SWIM_READ_CAP 0x02
374 #define STLINK_SWIM_SPEED 0x03
375 #define STLINK_SWIM_ENTER_SEQ 0x04
376 #define STLINK_SWIM_GEN_RST 0x05
377 #define STLINK_SWIM_RESET 0x06
378 #define STLINK_SWIM_ASSERT_RESET 0x07
379 #define STLINK_SWIM_DEASSERT_RESET 0x08
380 #define STLINK_SWIM_READSTATUS 0x09
381 #define STLINK_SWIM_WRITEMEM 0x0a
382 #define STLINK_SWIM_READMEM 0x0b
383 #define STLINK_SWIM_READBUF 0x0c
384
385 #define STLINK_DEBUG_GETSTATUS 0x01
386 #define STLINK_DEBUG_FORCEDEBUG 0x02
387 #define STLINK_DEBUG_APIV1_RESETSYS 0x03
388 #define STLINK_DEBUG_APIV1_READALLREGS 0x04
389 #define STLINK_DEBUG_APIV1_READREG 0x05
390 #define STLINK_DEBUG_APIV1_WRITEREG 0x06
391 #define STLINK_DEBUG_READMEM_32BIT 0x07
392 #define STLINK_DEBUG_WRITEMEM_32BIT 0x08
393 #define STLINK_DEBUG_RUNCORE 0x09
394 #define STLINK_DEBUG_STEPCORE 0x0a
395 #define STLINK_DEBUG_APIV1_SETFP 0x0b
396 #define STLINK_DEBUG_READMEM_8BIT 0x0c
397 #define STLINK_DEBUG_WRITEMEM_8BIT 0x0d
398 #define STLINK_DEBUG_APIV1_CLEARFP 0x0e
399 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG 0x0f
400 #define STLINK_DEBUG_APIV1_SETWATCHPOINT 0x10
401
402 #define STLINK_DEBUG_ENTER_JTAG_RESET 0x00
403 #define STLINK_DEBUG_ENTER_SWD_NO_RESET 0xa3
404 #define STLINK_DEBUG_ENTER_JTAG_NO_RESET 0xa4
405
406 #define STLINK_DEBUG_APIV1_ENTER 0x20
407 #define STLINK_DEBUG_EXIT 0x21
408 #define STLINK_DEBUG_READCOREID 0x22
409
410 #define STLINK_DEBUG_APIV2_ENTER 0x30
411 #define STLINK_DEBUG_APIV2_READ_IDCODES 0x31
412 #define STLINK_DEBUG_APIV2_RESETSYS 0x32
413 #define STLINK_DEBUG_APIV2_READREG 0x33
414 #define STLINK_DEBUG_APIV2_WRITEREG 0x34
415 #define STLINK_DEBUG_APIV2_WRITEDEBUGREG 0x35
416 #define STLINK_DEBUG_APIV2_READDEBUGREG 0x36
417
418 #define STLINK_DEBUG_APIV2_READALLREGS 0x3A
419 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
420 #define STLINK_DEBUG_APIV2_DRIVE_NRST 0x3C
421
422 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
423
424 #define STLINK_DEBUG_APIV2_START_TRACE_RX 0x40
425 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX 0x41
426 #define STLINK_DEBUG_APIV2_GET_TRACE_NB 0x42
427 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ 0x43
428 #define STLINK_DEBUG_APIV2_JTAG_SET_FREQ 0x44
429 #define STLINK_DEBUG_APIV2_READ_DAP_REG 0x45
430 #define STLINK_DEBUG_APIV2_WRITE_DAP_REG 0x46
431 #define STLINK_DEBUG_APIV2_READMEM_16BIT 0x47
432 #define STLINK_DEBUG_APIV2_WRITEMEM_16BIT 0x48
433
434 #define STLINK_DEBUG_APIV2_INIT_AP 0x4B
435 #define STLINK_DEBUG_APIV2_CLOSE_AP_DBG 0x4C
436
437 #define STLINK_APIV3_SET_COM_FREQ 0x61
438 #define STLINK_APIV3_GET_COM_FREQ 0x62
439
440 #define STLINK_APIV3_GET_VERSION_EX 0xFB
441
442 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW 0x00
443 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH 0x01
444 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
445
446 #define STLINK_DEBUG_PORT_ACCESS 0xffff
447
448 #define STLINK_TRACE_SIZE 4096
449 #define STLINK_TRACE_MAX_HZ 2000000
450 #define STLINK_V3_TRACE_MAX_HZ 24000000
451
452 #define STLINK_V3_MAX_FREQ_NB 10
453
454 #define REQUEST_SENSE 0x03
455 #define REQUEST_SENSE_LENGTH 18
456
457 /* STLINK TCP commands */
458 #define STLINK_TCP_CMD_REFRESH_DEVICE_LIST 0x00
459 #define STLINK_TCP_CMD_GET_NB_DEV 0x01
460 #define STLINK_TCP_CMD_GET_DEV_INFO 0x02
461 #define STLINK_TCP_CMD_OPEN_DEV 0x03
462 #define STLINK_TCP_CMD_CLOSE_DEV 0x04
463 #define STLINK_TCP_CMD_SEND_USB_CMD 0x05
464 #define STLINK_TCP_CMD_GET_SERVER_VERSION 0x06
465 #define STLINK_TCP_CMD_GET_NB_OF_DEV_CLIENTS 0x07
466
467 /* STLINK TCP constants */
468 #define OPENOCD_STLINK_TCP_API_VERSION 1
469 #define STLINK_TCP_REQUEST_WRITE 0
470 #define STLINK_TCP_REQUEST_READ 1
471 #define STLINK_TCP_REQUEST_READ_SWO 3
472 #define STLINK_TCP_SS_SIZE 4
473 #define STLINK_TCP_USB_CMD_SIZE 32
474 #define STLINK_TCP_SERIAL_SIZE 32
475 #define STLINK_TCP_SEND_BUFFER_SIZE 10240
476 #define STLINK_TCP_RECV_BUFFER_SIZE 10240
477
478 /* STLINK TCP command status */
479 #define STLINK_TCP_SS_OK 0x00000001
480 #define STLINK_TCP_SS_MEMORY_PROBLEM 0x00001000
481 #define STLINK_TCP_SS_TIMEOUT 0x00001001
482 #define STLINK_TCP_SS_BAD_PARAMETER 0x00001002
483 #define STLINK_TCP_SS_OPEN_ERR 0x00001003
484 #define STLINK_TCP_SS_TRUNCATED_DATA 0x00001052
485 #define STLINK_TCP_SS_CMD_NOT_AVAILABLE 0x00001053
486 #define STLINK_TCP_SS_TCP_ERROR 0x00002001
487 #define STLINK_TCP_SS_TCP_CANT_CONNECT 0x00002002
488 #define STLINK_TCP_SS_WIN32_ERROR 0x00010000
489
490 /*
491 * Map the relevant features, quirks and workaround for specific firmware
492 * version of stlink
493 */
494 #define STLINK_F_HAS_TRACE BIT(0) /* v2>=j13 || v3 */
495 #define STLINK_F_HAS_GETLASTRWSTATUS2 BIT(1) /* v2>=j15 || v3 */
496 #define STLINK_F_HAS_SWD_SET_FREQ BIT(2) /* v2>=j22 */
497 #define STLINK_F_HAS_JTAG_SET_FREQ BIT(3) /* v2>=j24 */
498 #define STLINK_F_QUIRK_JTAG_DP_READ BIT(4) /* v2>=j24 && v2<j32 */
499 #define STLINK_F_HAS_DAP_REG BIT(5) /* v2>=j24 || v3 */
500 #define STLINK_F_HAS_MEM_16BIT BIT(6) /* v2>=j26 || v3 */
501 #define STLINK_F_HAS_AP_INIT BIT(7) /* v2>=j28 || v3 */
502 #define STLINK_F_FIX_CLOSE_AP BIT(8) /* v2>=j29 || v3 */
503 #define STLINK_F_HAS_DPBANKSEL BIT(9) /* v2>=j32 || v3>=j2 */
504 #define STLINK_F_HAS_RW8_512BYTES BIT(10) /* v3>=j6 */
505
506 /* aliases */
507 #define STLINK_F_HAS_TARGET_VOLT STLINK_F_HAS_TRACE
508 #define STLINK_F_HAS_FPU_REG STLINK_F_HAS_GETLASTRWSTATUS2
509 #define STLINK_F_HAS_CSW STLINK_F_HAS_DPBANKSEL
510
511 #define STLINK_REGSEL_IS_FPU(x) ((x) > 0x1F)
512
513 struct speed_map {
514 int speed;
515 int speed_divisor;
516 };
517
518 /* SWD clock speed */
519 static const struct speed_map stlink_khz_to_speed_map_swd[] = {
520 {4000, 0},
521 {1800, 1}, /* default */
522 {1200, 2},
523 {950, 3},
524 {480, 7},
525 {240, 15},
526 {125, 31},
527 {100, 40},
528 {50, 79},
529 {25, 158},
530 {15, 265},
531 {5, 798}
532 };
533
534 /* JTAG clock speed */
535 static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
536 {9000, 4},
537 {4500, 8},
538 {2250, 16},
539 {1125, 32}, /* default */
540 {562, 64},
541 {281, 128},
542 {140, 256}
543 };
544
545 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
546 static int stlink_swim_status(void *handle);
547 static void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size);
548 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map);
549 static int stlink_speed(void *handle, int khz, bool query);
550 static int stlink_usb_open_ap(void *handle, unsigned short apsel);
551
552 /** */
553 static unsigned int stlink_usb_block(void *handle)
554 {
555 struct stlink_usb_handle_s *h = handle;
556
557 assert(handle);
558
559 if (h->version.flags & STLINK_F_HAS_RW8_512BYTES)
560 return STLINKV3_MAX_RW8;
561 else
562 return STLINK_MAX_RW8;
563 }
564
565 #ifdef USE_LIBUSB_ASYNCIO
566
567 static LIBUSB_CALL void sync_transfer_cb(struct libusb_transfer *transfer)
568 {
569 int *completed = transfer->user_data;
570 *completed = 1;
571 /* caller interprets result and frees transfer */
572 }
573
574
575 static void sync_transfer_wait_for_completion(struct libusb_transfer *transfer)
576 {
577 int r, *completed = transfer->user_data;
578
579 while (!*completed) {
580 r = jtag_libusb_handle_events_completed(completed);
581 if (r < 0) {
582 if (r == LIBUSB_ERROR_INTERRUPTED)
583 continue;
584 libusb_cancel_transfer(transfer);
585 continue;
586 }
587 }
588 }
589
590
591 static int transfer_error_status(const struct libusb_transfer *transfer)
592 {
593 int r = 0;
594
595 switch (transfer->status) {
596 case LIBUSB_TRANSFER_COMPLETED:
597 r = 0;
598 break;
599 case LIBUSB_TRANSFER_TIMED_OUT:
600 r = LIBUSB_ERROR_TIMEOUT;
601 break;
602 case LIBUSB_TRANSFER_STALL:
603 r = LIBUSB_ERROR_PIPE;
604 break;
605 case LIBUSB_TRANSFER_OVERFLOW:
606 r = LIBUSB_ERROR_OVERFLOW;
607 break;
608 case LIBUSB_TRANSFER_NO_DEVICE:
609 r = LIBUSB_ERROR_NO_DEVICE;
610 break;
611 case LIBUSB_TRANSFER_ERROR:
612 case LIBUSB_TRANSFER_CANCELLED:
613 r = LIBUSB_ERROR_IO;
614 break;
615 default:
616 r = LIBUSB_ERROR_OTHER;
617 break;
618 }
619
620 return r;
621 }
622
623 struct jtag_xfer {
624 int ep;
625 uint8_t *buf;
626 size_t size;
627 /* Internal */
628 int retval;
629 int completed;
630 size_t transfer_size;
631 struct libusb_transfer *transfer;
632 };
633
634 static int jtag_libusb_bulk_transfer_n(
635 struct libusb_device_handle *dev_handle,
636 struct jtag_xfer *transfers,
637 size_t n_transfers,
638 int timeout)
639 {
640 int retval = 0;
641 int returnval = ERROR_OK;
642
643
644 for (size_t i = 0; i < n_transfers; ++i) {
645 transfers[i].retval = 0;
646 transfers[i].completed = 0;
647 transfers[i].transfer_size = 0;
648 transfers[i].transfer = libusb_alloc_transfer(0);
649
650 if (!transfers[i].transfer) {
651 for (size_t j = 0; j < i; ++j)
652 libusb_free_transfer(transfers[j].transfer);
653
654 LOG_DEBUG("ERROR, failed to alloc usb transfers");
655 for (size_t k = 0; k < n_transfers; ++k)
656 transfers[k].retval = LIBUSB_ERROR_NO_MEM;
657 return ERROR_FAIL;
658 }
659 }
660
661 for (size_t i = 0; i < n_transfers; ++i) {
662 libusb_fill_bulk_transfer(
663 transfers[i].transfer,
664 dev_handle,
665 transfers[i].ep, transfers[i].buf, transfers[i].size,
666 sync_transfer_cb, &transfers[i].completed, timeout);
667 transfers[i].transfer->type = LIBUSB_TRANSFER_TYPE_BULK;
668
669 retval = libusb_submit_transfer(transfers[i].transfer);
670 if (retval < 0) {
671 LOG_DEBUG("ERROR, failed to submit transfer %zu, error %d", i, retval);
672
673 /* Probably no point continuing to submit transfers once a submission fails.
674 * As a result, tag all remaining transfers as errors.
675 */
676 for (size_t j = i; j < n_transfers; ++j)
677 transfers[j].retval = retval;
678
679 returnval = ERROR_FAIL;
680 break;
681 }
682 }
683
684 /* Wait for every submitted USB transfer to complete.
685 */
686 for (size_t i = 0; i < n_transfers; ++i) {
687 if (transfers[i].retval == 0) {
688 sync_transfer_wait_for_completion(transfers[i].transfer);
689
690 retval = transfer_error_status(transfers[i].transfer);
691 if (retval) {
692 returnval = ERROR_FAIL;
693 transfers[i].retval = retval;
694 LOG_DEBUG("ERROR, transfer %zu failed, error %d", i, retval);
695 } else {
696 /* Assuming actual_length is only valid if there is no transfer error.
697 */
698 transfers[i].transfer_size = transfers[i].transfer->actual_length;
699 }
700 }
701
702 libusb_free_transfer(transfers[i].transfer);
703 transfers[i].transfer = NULL;
704 }
705
706 return returnval;
707 }
708
709 #endif
710
711
712 /** */
713 static int stlink_usb_xfer_v1_get_status(void *handle)
714 {
715 struct stlink_usb_handle_s *h = handle;
716 int tr, ret;
717
718 assert(handle);
719
720 /* read status */
721 memset(h->cmdbuf, 0, STLINK_SG_SIZE);
722
723 ret = jtag_libusb_bulk_read(h->usb_backend_priv.fd, h->rx_ep, (char *)h->cmdbuf, 13,
724 STLINK_READ_TIMEOUT, &tr);
725 if (ret || tr != 13)
726 return ERROR_FAIL;
727
728 uint32_t t1;
729
730 t1 = buf_get_u32(h->cmdbuf, 0, 32);
731
732 /* check for USBS */
733 if (t1 != 0x53425355)
734 return ERROR_FAIL;
735 /*
736 * CSW status:
737 * 0 success
738 * 1 command failure
739 * 2 phase error
740 */
741 if (h->cmdbuf[12] != 0)
742 return ERROR_FAIL;
743
744 return ERROR_OK;
745 }
746
747 #ifdef USE_LIBUSB_ASYNCIO
748 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
749 {
750 struct stlink_usb_handle_s *h = handle;
751
752 assert(handle);
753
754 size_t n_transfers = 0;
755 struct jtag_xfer transfers[2];
756
757 memset(transfers, 0, sizeof(transfers));
758
759 transfers[0].ep = h->tx_ep;
760 transfers[0].buf = h->cmdbuf;
761 transfers[0].size = cmdsize;
762
763 ++n_transfers;
764
765 if (h->direction == h->tx_ep && size) {
766 transfers[1].ep = h->tx_ep;
767 transfers[1].buf = (uint8_t *)buf;
768 transfers[1].size = size;
769
770 ++n_transfers;
771 } else if (h->direction == h->rx_ep && size) {
772 transfers[1].ep = h->rx_ep;
773 transfers[1].buf = (uint8_t *)buf;
774 transfers[1].size = size;
775
776 ++n_transfers;
777 }
778
779 return jtag_libusb_bulk_transfer_n(
780 h->usb_backend_priv.fd,
781 transfers,
782 n_transfers,
783 STLINK_WRITE_TIMEOUT);
784 }
785 #else
786 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
787 {
788 struct stlink_usb_handle_s *h = handle;
789 int tr, ret;
790
791 assert(handle);
792
793 ret = jtag_libusb_bulk_write(h->usb_backend_priv.fd, h->tx_ep, (char *)h->cmdbuf,
794 cmdsize, STLINK_WRITE_TIMEOUT, &tr);
795 if (ret || tr != cmdsize)
796 return ERROR_FAIL;
797
798 if (h->direction == h->tx_ep && size) {
799 ret = jtag_libusb_bulk_write(h->usb_backend_priv.fd, h->tx_ep, (char *)buf,
800 size, STLINK_WRITE_TIMEOUT, &tr);
801 if (ret || tr != size) {
802 LOG_DEBUG("bulk write failed");
803 return ERROR_FAIL;
804 }
805 } else if (h->direction == h->rx_ep && size) {
806 ret = jtag_libusb_bulk_read(h->usb_backend_priv.fd, h->rx_ep, (char *)buf,
807 size, STLINK_READ_TIMEOUT, &tr);
808 if (ret || tr != size) {
809 LOG_DEBUG("bulk read failed");
810 return ERROR_FAIL;
811 }
812 }
813
814 return ERROR_OK;
815 }
816 #endif
817
818 /** */
819 static int stlink_usb_xfer_v1_get_sense(void *handle)
820 {
821 int res;
822 struct stlink_usb_handle_s *h = handle;
823
824 assert(handle);
825
826 stlink_usb_init_buffer(handle, h->rx_ep, 16);
827
828 h->cmdbuf[h->cmdidx++] = REQUEST_SENSE;
829 h->cmdbuf[h->cmdidx++] = 0;
830 h->cmdbuf[h->cmdidx++] = 0;
831 h->cmdbuf[h->cmdidx++] = 0;
832 h->cmdbuf[h->cmdidx++] = REQUEST_SENSE_LENGTH;
833
834 res = stlink_usb_xfer_rw(handle, REQUEST_SENSE_LENGTH, h->databuf, 16);
835
836 if (res != ERROR_OK)
837 return res;
838
839 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK)
840 return ERROR_FAIL;
841
842 return ERROR_OK;
843 }
844
845 /** */
846 static int stlink_usb_usb_read_trace(void *handle, const uint8_t *buf, int size)
847 {
848 struct stlink_usb_handle_s *h = handle;
849 int tr, ret;
850
851 ret = jtag_libusb_bulk_read(h->usb_backend_priv.fd, h->trace_ep, (char *)buf, size,
852 STLINK_READ_TIMEOUT, &tr);
853 if (ret || tr != size) {
854 LOG_ERROR("bulk trace read failed");
855 return ERROR_FAIL;
856 }
857
858 return ERROR_OK;
859 }
860
861 /*
862 transfers block in cmdbuf
863 <size> indicates number of bytes in the following
864 data phase.
865 Ignore the (eventual) error code in the received packet.
866 */
867 static int stlink_usb_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
868 {
869 int err, cmdsize = STLINK_CMD_SIZE_V2;
870 struct stlink_usb_handle_s *h = handle;
871
872 assert(handle);
873
874 if (h->version.stlink == 1) {
875 cmdsize = STLINK_SG_SIZE;
876 /* put length in bCBWCBLength */
877 h->cmdbuf[14] = h->cmdidx-15;
878 }
879
880 err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
881
882 if (err != ERROR_OK)
883 return err;
884
885 if (h->version.stlink == 1) {
886 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK) {
887 /* check csw status */
888 if (h->cmdbuf[12] == 1) {
889 LOG_DEBUG("get sense");
890 if (stlink_usb_xfer_v1_get_sense(handle) != ERROR_OK)
891 return ERROR_FAIL;
892 }
893 return ERROR_FAIL;
894 }
895 }
896
897 return ERROR_OK;
898 }
899
900
901 static int stlink_tcp_send_cmd(void *handle, int send_size, int recv_size, bool check_tcp_status)
902 {
903 struct stlink_usb_handle_s *h = handle;
904
905 assert(handle);
906
907 /* send the TCP command */
908 int sent_size = send(h->tcp_backend_priv.fd, (void *)h->tcp_backend_priv.send_buf, send_size, 0);
909 if (sent_size != send_size) {
910 LOG_ERROR("failed to send USB CMD");
911 if (sent_size == -1)
912 LOG_DEBUG("socket send error: %s (errno %d)", strerror(errno), errno);
913 else
914 LOG_DEBUG("sent size %d (expected %d)", sent_size, send_size);
915 return ERROR_FAIL;
916 }
917
918 keep_alive();
919
920 /* read the TCP response */
921 int received_size = recv(h->tcp_backend_priv.fd, (void *)h->tcp_backend_priv.recv_buf, recv_size, 0);
922 if (received_size != recv_size) {
923 LOG_ERROR("failed to receive USB CMD response");
924 if (received_size == -1)
925 LOG_DEBUG("socket recv error: %s (errno %d)", strerror(errno), errno);
926 else
927 LOG_DEBUG("received size %d (expected %d)", received_size, recv_size);
928 return ERROR_FAIL;
929 }
930
931 if (check_tcp_status) {
932 uint32_t tcp_ss = le_to_h_u32(h->tcp_backend_priv.recv_buf);
933 if (tcp_ss != STLINK_TCP_SS_OK) {
934 LOG_ERROR("TCP error status 0x%X", tcp_ss);
935 return ERROR_FAIL;
936 }
937 }
938
939 return ERROR_OK;
940 }
941
942 /** */
943 static int stlink_tcp_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
944 {
945 struct stlink_usb_handle_s *h = handle;
946
947 int send_size = STLINK_TCP_USB_CMD_SIZE;
948 int recv_size = STLINK_TCP_SS_SIZE;
949
950 assert(handle);
951
952 /* prepare the TCP command */
953 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_SEND_USB_CMD;
954 memset(&h->tcp_backend_priv.send_buf[1], 0, 3); /* reserved for alignment and future use, must be zero */
955 h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.connect_id);
956 /* tcp_backend_priv.send_buf[8..23] already contains the constructed stlink command */
957 h->tcp_backend_priv.send_buf[24] = h->direction;
958 memset(&h->tcp_backend_priv.send_buf[25], 0, 3); /* reserved for alignment and future use, must be zero */
959
960 h_u32_to_le(&h->tcp_backend_priv.send_buf[28], size);
961
962 /*
963 * if the xfer is a write request (tx_ep)
964 * > then buf content will be copied
965 * into &cmdbuf[32].
966 * else : the xfer is a read or trace read request (rx_ep or trace_ep)
967 * > the buf content will be filled from &databuf[4].
968 *
969 * note : if h->direction is trace_ep, h->cmdbuf is zeros.
970 */
971
972 if (h->direction == h->tx_ep) { /* STLINK_TCP_REQUEST_WRITE */
973 send_size += size;
974 if (send_size > STLINK_TCP_SEND_BUFFER_SIZE) {
975 LOG_ERROR("STLINK_TCP command buffer overflow");
976 return ERROR_FAIL;
977 }
978 memcpy(&h->tcp_backend_priv.send_buf[32], buf, size);
979 } else { /* STLINK_TCP_REQUEST_READ or STLINK_TCP_REQUEST_READ_SWO */
980 recv_size += size;
981 if (recv_size > STLINK_TCP_RECV_BUFFER_SIZE) {
982 LOG_ERROR("STLINK_TCP data buffer overflow");
983 return ERROR_FAIL;
984 }
985 }
986
987 int ret = stlink_tcp_send_cmd(h, send_size, recv_size, true);
988 if (ret != ERROR_OK)
989 return ret;
990
991 if (h->direction != h->tx_ep) {
992 /* the read data is located in tcp_backend_priv.recv_buf[4] */
993 /* most of the case it will be copying the data from tcp_backend_priv.recv_buf[4]
994 * to handle->cmd_buff which are the same, so let's avoid unnecessary copying */
995 if (buf != &h->tcp_backend_priv.recv_buf[4])
996 memcpy((uint8_t *)buf, &h->tcp_backend_priv.recv_buf[4], size);
997 }
998
999 return ERROR_OK;
1000 }
1001
1002 /** */
1003 static int stlink_tcp_read_trace(void *handle, const uint8_t *buf, int size)
1004 {
1005 struct stlink_usb_handle_s *h = handle;
1006
1007 stlink_usb_init_buffer(h, h->trace_ep, 0);
1008 return stlink_tcp_xfer_noerrcheck(handle, buf, size);
1009 }
1010
1011 /**
1012 Converts an STLINK status code held in the first byte of a response
1013 to an openocd error, logs any error/wait status as debug output.
1014 */
1015 static int stlink_usb_error_check(void *handle)
1016 {
1017 struct stlink_usb_handle_s *h = handle;
1018
1019 assert(handle);
1020
1021 if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1022 switch (h->databuf[0]) {
1023 case STLINK_SWIM_ERR_OK:
1024 return ERROR_OK;
1025 case STLINK_SWIM_BUSY:
1026 return ERROR_WAIT;
1027 default:
1028 LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
1029 return ERROR_FAIL;
1030 }
1031 }
1032
1033 /* TODO: no error checking yet on api V1 */
1034 if (h->version.jtag_api == STLINK_JTAG_API_V1)
1035 h->databuf[0] = STLINK_DEBUG_ERR_OK;
1036
1037 switch (h->databuf[0]) {
1038 case STLINK_DEBUG_ERR_OK:
1039 return ERROR_OK;
1040 case STLINK_DEBUG_ERR_FAULT:
1041 LOG_DEBUG("SWD fault response (0x%x)", STLINK_DEBUG_ERR_FAULT);
1042 return ERROR_FAIL;
1043 case STLINK_SWD_AP_WAIT:
1044 LOG_DEBUG("wait status SWD_AP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
1045 return ERROR_WAIT;
1046 case STLINK_SWD_DP_WAIT:
1047 LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_DP_WAIT);
1048 return ERROR_WAIT;
1049 case STLINK_JTAG_GET_IDCODE_ERROR:
1050 LOG_DEBUG("STLINK_JTAG_GET_IDCODE_ERROR");
1051 return ERROR_FAIL;
1052 case STLINK_JTAG_WRITE_ERROR:
1053 LOG_DEBUG("Write error");
1054 return ERROR_FAIL;
1055 case STLINK_JTAG_WRITE_VERIF_ERROR:
1056 LOG_DEBUG("Write verify error, ignoring");
1057 return ERROR_OK;
1058 case STLINK_SWD_AP_FAULT:
1059 /* git://git.ac6.fr/openocd commit 657e3e885b9ee10
1060 * returns ERROR_OK with the comment:
1061 * Change in error status when reading outside RAM.
1062 * This fix allows CDT plugin to visualize memory.
1063 */
1064 LOG_DEBUG("STLINK_SWD_AP_FAULT");
1065 return ERROR_FAIL;
1066 case STLINK_SWD_AP_ERROR:
1067 LOG_DEBUG("STLINK_SWD_AP_ERROR");
1068 return ERROR_FAIL;
1069 case STLINK_SWD_AP_PARITY_ERROR:
1070 LOG_DEBUG("STLINK_SWD_AP_PARITY_ERROR");
1071 return ERROR_FAIL;
1072 case STLINK_SWD_DP_FAULT:
1073 LOG_DEBUG("STLINK_SWD_DP_FAULT");
1074 return ERROR_FAIL;
1075 case STLINK_SWD_DP_ERROR:
1076 LOG_DEBUG("STLINK_SWD_DP_ERROR");
1077 return ERROR_FAIL;
1078 case STLINK_SWD_DP_PARITY_ERROR:
1079 LOG_DEBUG("STLINK_SWD_DP_PARITY_ERROR");
1080 return ERROR_FAIL;
1081 case STLINK_SWD_AP_WDATA_ERROR:
1082 LOG_DEBUG("STLINK_SWD_AP_WDATA_ERROR");
1083 return ERROR_FAIL;
1084 case STLINK_SWD_AP_STICKY_ERROR:
1085 LOG_DEBUG("STLINK_SWD_AP_STICKY_ERROR");
1086 return ERROR_FAIL;
1087 case STLINK_SWD_AP_STICKYORUN_ERROR:
1088 LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR");
1089 return ERROR_FAIL;
1090 case STLINK_BAD_AP_ERROR:
1091 LOG_DEBUG("STLINK_BAD_AP_ERROR");
1092 return ERROR_FAIL;
1093 default:
1094 LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
1095 return ERROR_FAIL;
1096 }
1097 }
1098
1099 /*
1100 * Wrapper around stlink_usb_xfer_noerrcheck()
1101 * to check the error code in the received packet
1102 */
1103 static int stlink_usb_xfer_errcheck(void *handle, const uint8_t *buf, int size)
1104 {
1105 int retval;
1106
1107 assert(size > 0);
1108
1109 retval = stlink_usb_xfer_noerrcheck(handle, buf, size);
1110 if (retval != ERROR_OK)
1111 return retval;
1112
1113 return stlink_usb_error_check(handle);
1114 }
1115
1116 /** Issue an STLINK command via USB transfer, with retries on any wait status responses.
1117
1118 Works for commands where the STLINK_DEBUG status is returned in the first
1119 byte of the response packet. For SWIM a SWIM_READSTATUS is requested instead.
1120
1121 Returns an openocd result code.
1122 */
1123 static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
1124 {
1125 int retries = 0;
1126 int res;
1127 struct stlink_usb_handle_s *h = handle;
1128
1129 while (1) {
1130 if ((h->st_mode != STLINK_MODE_DEBUG_SWIM) || !retries) {
1131 res = stlink_usb_xfer_noerrcheck(handle, buf, size);
1132 if (res != ERROR_OK)
1133 return res;
1134 }
1135
1136 if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1137 res = stlink_swim_status(handle);
1138 if (res != ERROR_OK)
1139 return res;
1140 }
1141
1142 res = stlink_usb_error_check(handle);
1143 if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1144 unsigned int delay_us = (1<<retries++) * 1000;
1145 LOG_DEBUG("stlink_cmd_allow_retry ERROR_WAIT, retry %d, delaying %u microseconds", retries, delay_us);
1146 usleep(delay_us);
1147 continue;
1148 }
1149 return res;
1150 }
1151 }
1152
1153 /** */
1154 static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
1155 {
1156 struct stlink_usb_handle_s *h = handle;
1157
1158 assert(handle);
1159
1160 assert(h->version.flags & STLINK_F_HAS_TRACE);
1161
1162 return h->backend->read_trace(handle, buf, size);
1163 }
1164
1165 /*
1166 this function writes transfer length in
1167 the right place in the cb
1168 */
1169 static void stlink_usb_set_cbw_transfer_datalength(void *handle, uint32_t size)
1170 {
1171 struct stlink_usb_handle_s *h = handle;
1172
1173 buf_set_u32(h->cmdbuf+8, 0, 32, size);
1174 }
1175
1176 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
1177 {
1178 struct stlink_usb_handle_s *h = handle;
1179
1180 /* fill the send buffer */
1181 strcpy((char *)h->cmdbuf, "USBC");
1182 h->cmdidx += 4;
1183 /* csw tag not used */
1184 buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, 0);
1185 h->cmdidx += 4;
1186 /* cbw data transfer length (in the following data phase in or out) */
1187 buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
1188 h->cmdidx += 4;
1189 /* cbw flags */
1190 h->cmdbuf[h->cmdidx++] = (direction == h->rx_ep ? ENDPOINT_IN : ENDPOINT_OUT);
1191 h->cmdbuf[h->cmdidx++] = 0; /* lun */
1192 /* cdb clength (is filled in at xfer) */
1193 h->cmdbuf[h->cmdidx++] = 0;
1194 }
1195
1196 /** */
1197 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size)
1198 {
1199 struct stlink_usb_handle_s *h = handle;
1200
1201 h->direction = direction;
1202
1203 h->cmdidx = 0;
1204
1205 memset(h->cmdbuf, 0, STLINK_SG_SIZE);
1206 memset(h->databuf, 0, STLINK_DATA_SIZE);
1207
1208 if (h->version.stlink == 1)
1209 stlink_usb_xfer_v1_create_cmd(handle, direction, size);
1210 }
1211
1212 /** */
1213 static int stlink_usb_version(void *handle)
1214 {
1215 int res;
1216 uint32_t flags;
1217 uint16_t version;
1218 uint8_t v, x, y, jtag, swim, msd, bridge = 0;
1219 char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */
1220 char *p;
1221 struct stlink_usb_handle_s *h = handle;
1222
1223 assert(handle);
1224
1225 stlink_usb_init_buffer(handle, h->rx_ep, 6);
1226
1227 h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION;
1228
1229 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 6);
1230
1231 if (res != ERROR_OK)
1232 return res;
1233
1234 version = be_to_h_u16(h->databuf);
1235 v = (version >> 12) & 0x0f;
1236 x = (version >> 6) & 0x3f;
1237 y = version & 0x3f;
1238
1239 h->vid = le_to_h_u16(h->databuf + 2);
1240 h->pid = le_to_h_u16(h->databuf + 4);
1241
1242 switch (h->pid) {
1243 case STLINK_V2_1_PID:
1244 case STLINK_V2_1_NO_MSD_PID:
1245 if ((x <= 22 && y == 7) || (x >= 25 && y >= 7 && y <= 12)) {
1246 /* MxSy : STM8 V2.1 - SWIM only */
1247 msd = x;
1248 swim = y;
1249 jtag = 0;
1250 } else {
1251 /* JxMy : STM32 V2.1 - JTAG/SWD only */
1252 jtag = x;
1253 msd = y;
1254 swim = 0;
1255 }
1256 break;
1257 default:
1258 jtag = x;
1259 swim = y;
1260 msd = 0;
1261 break;
1262 }
1263
1264 /* STLINK-V3 requires a specific command */
1265 if (v == 3 && x == 0 && y == 0) {
1266 stlink_usb_init_buffer(handle, h->rx_ep, 16);
1267
1268 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_VERSION_EX;
1269
1270 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 12);
1271 if (res != ERROR_OK)
1272 return res;
1273
1274 v = h->databuf[0];
1275 swim = h->databuf[1];
1276 jtag = h->databuf[2];
1277 msd = h->databuf[3];
1278 bridge = h->databuf[4];
1279 h->vid = le_to_h_u16(h->databuf + 8);
1280 h->pid = le_to_h_u16(h->databuf + 10);
1281 }
1282
1283 h->version.stlink = v;
1284 h->version.jtag = jtag;
1285 h->version.swim = swim;
1286
1287 flags = 0;
1288 switch (h->version.stlink) {
1289 case 1:
1290 /* ST-LINK/V1 from J11 switch to api-v2 (and support SWD) */
1291 if (h->version.jtag >= 11)
1292 h->version.jtag_api = STLINK_JTAG_API_V2;
1293 else
1294 h->version.jtag_api = STLINK_JTAG_API_V1;
1295
1296 break;
1297 case 2:
1298 /* all ST-LINK/V2 and ST-Link/V2.1 use api-v2 */
1299 h->version.jtag_api = STLINK_JTAG_API_V2;
1300
1301 /* API for trace from J13 */
1302 /* API for target voltage from J13 */
1303 if (h->version.jtag >= 13)
1304 flags |= STLINK_F_HAS_TRACE;
1305
1306 /* preferred API to get last R/W status from J15 */
1307 if (h->version.jtag >= 15)
1308 flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
1309
1310 /* API to set SWD frequency from J22 */
1311 if (h->version.jtag >= 22)
1312 flags |= STLINK_F_HAS_SWD_SET_FREQ;
1313
1314 /* API to set JTAG frequency from J24 */
1315 /* API to access DAP registers from J24 */
1316 if (h->version.jtag >= 24) {
1317 flags |= STLINK_F_HAS_JTAG_SET_FREQ;
1318 flags |= STLINK_F_HAS_DAP_REG;
1319 }
1320
1321 /* Quirk for read DP in JTAG mode (V2 only) from J24, fixed in J32 */
1322 if (h->version.jtag >= 24 && h->version.jtag < 32)
1323 flags |= STLINK_F_QUIRK_JTAG_DP_READ;
1324
1325 /* API to read/write memory at 16 bit from J26 */
1326 if (h->version.jtag >= 26)
1327 flags |= STLINK_F_HAS_MEM_16BIT;
1328
1329 /* API required to init AP before any AP access from J28 */
1330 if (h->version.jtag >= 28)
1331 flags |= STLINK_F_HAS_AP_INIT;
1332
1333 /* API required to return proper error code on close AP from J29 */
1334 if (h->version.jtag >= 29)
1335 flags |= STLINK_F_FIX_CLOSE_AP;
1336
1337 /* Banked regs (DPv1 & DPv2) support from V2J32 */
1338 /* Memory R/W supports CSW from V2J32 */
1339 if (h->version.jtag >= 32)
1340 flags |= STLINK_F_HAS_DPBANKSEL;
1341
1342 break;
1343 case 3:
1344 /* all STLINK-V3 use api-v3 */
1345 h->version.jtag_api = STLINK_JTAG_API_V3;
1346
1347 /* STLINK-V3 is a superset of ST-LINK/V2 */
1348
1349 /* API for trace */
1350 /* API for target voltage */
1351 flags |= STLINK_F_HAS_TRACE;
1352
1353 /* preferred API to get last R/W status */
1354 flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
1355
1356 /* API to access DAP registers */
1357 flags |= STLINK_F_HAS_DAP_REG;
1358
1359 /* API to read/write memory at 16 bit */
1360 flags |= STLINK_F_HAS_MEM_16BIT;
1361
1362 /* API required to init AP before any AP access */
1363 flags |= STLINK_F_HAS_AP_INIT;
1364
1365 /* API required to return proper error code on close AP */
1366 flags |= STLINK_F_FIX_CLOSE_AP;
1367
1368 /* Banked regs (DPv1 & DPv2) support from V3J2 */
1369 /* Memory R/W supports CSW from V3J2 */
1370 if (h->version.jtag >= 2)
1371 flags |= STLINK_F_HAS_DPBANKSEL;
1372
1373 /* 8bit read/write max packet size 512 bytes from V3J6 */
1374 if (h->version.jtag >= 6)
1375 flags |= STLINK_F_HAS_RW8_512BYTES;
1376
1377 break;
1378 default:
1379 break;
1380 }
1381 h->version.flags = flags;
1382
1383 p = v_str;
1384 p += sprintf(p, "V%d", v);
1385 if (jtag || !msd)
1386 p += sprintf(p, "J%d", jtag);
1387 if (msd)
1388 p += sprintf(p, "M%d", msd);
1389 if (bridge)
1390 p += sprintf(p, "B%d", bridge);
1391 if (swim || !msd)
1392 sprintf(p, "S%d", swim);
1393
1394 LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
1395 v_str,
1396 h->version.jtag_api,
1397 h->vid,
1398 h->pid);
1399
1400 return ERROR_OK;
1401 }
1402
1403 static int stlink_usb_check_voltage(void *handle, float *target_voltage)
1404 {
1405 struct stlink_usb_handle_s *h = handle;
1406 uint32_t adc_results[2];
1407
1408 /* no error message, simply quit with error */
1409 if (!(h->version.flags & STLINK_F_HAS_TARGET_VOLT))
1410 return ERROR_COMMAND_NOTFOUND;
1411
1412 stlink_usb_init_buffer(handle, h->rx_ep, 8);
1413
1414 h->cmdbuf[h->cmdidx++] = STLINK_GET_TARGET_VOLTAGE;
1415
1416 int result = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1417
1418 if (result != ERROR_OK)
1419 return result;
1420
1421 /* convert result */
1422 adc_results[0] = le_to_h_u32(h->databuf);
1423 adc_results[1] = le_to_h_u32(h->databuf + 4);
1424
1425 *target_voltage = 0;
1426
1427 if (adc_results[0])
1428 *target_voltage = 2 * ((float)adc_results[1]) * (float)(1.2 / adc_results[0]);
1429
1430 LOG_INFO("Target voltage: %f", (double)*target_voltage);
1431
1432 return ERROR_OK;
1433 }
1434
1435 static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
1436 {
1437 struct stlink_usb_handle_s *h = handle;
1438
1439 assert(handle);
1440
1441 if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
1442 return ERROR_COMMAND_NOTFOUND;
1443
1444 stlink_usb_init_buffer(handle, h->rx_ep, 2);
1445
1446 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1447 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
1448 h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1449 h->cmdidx += 2;
1450
1451 int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1452
1453 if (result != ERROR_OK)
1454 return result;
1455
1456 return ERROR_OK;
1457 }
1458
1459 static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor)
1460 {
1461 struct stlink_usb_handle_s *h = handle;
1462
1463 assert(handle);
1464
1465 if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
1466 return ERROR_COMMAND_NOTFOUND;
1467
1468 stlink_usb_init_buffer(handle, h->rx_ep, 2);
1469
1470 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1471 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
1472 h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1473 h->cmdidx += 2;
1474
1475 int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1476
1477 if (result != ERROR_OK)
1478 return result;
1479
1480 return ERROR_OK;
1481 }
1482
1483 /** */
1484 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
1485 {
1486 int res;
1487 struct stlink_usb_handle_s *h = handle;
1488
1489 assert(handle);
1490
1491 stlink_usb_init_buffer(handle, h->rx_ep, 2);
1492
1493 h->cmdbuf[h->cmdidx++] = STLINK_GET_CURRENT_MODE;
1494
1495 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
1496
1497 if (res != ERROR_OK)
1498 return res;
1499
1500 *mode = h->databuf[0];
1501
1502 return ERROR_OK;
1503 }
1504
1505 /** */
1506 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
1507 {
1508 int rx_size = 0;
1509 struct stlink_usb_handle_s *h = handle;
1510
1511 assert(handle);
1512
1513 /* on api V2 we are able the read the latest command
1514 * status
1515 * TODO: we need the test on api V1 too
1516 */
1517 if (h->version.jtag_api != STLINK_JTAG_API_V1)
1518 rx_size = 2;
1519
1520 stlink_usb_init_buffer(handle, h->rx_ep, rx_size);
1521
1522 switch (type) {
1523 case STLINK_MODE_DEBUG_JTAG:
1524 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1525 if (h->version.jtag_api == STLINK_JTAG_API_V1)
1526 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1527 else
1528 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1529 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG_NO_RESET;
1530 break;
1531 case STLINK_MODE_DEBUG_SWD:
1532 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1533 if (h->version.jtag_api == STLINK_JTAG_API_V1)
1534 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1535 else
1536 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1537 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD_NO_RESET;
1538 break;
1539 case STLINK_MODE_DEBUG_SWIM:
1540 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1541 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
1542 /* swim enter does not return any response or status */
1543 return stlink_usb_xfer_noerrcheck(handle, h->databuf, 0);
1544 case STLINK_MODE_DFU:
1545 case STLINK_MODE_MASS:
1546 default:
1547 return ERROR_FAIL;
1548 }
1549
1550 return stlink_cmd_allow_retry(handle, h->databuf, rx_size);
1551 }
1552
1553 /** */
1554 static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
1555 {
1556 int res;
1557 struct stlink_usb_handle_s *h = handle;
1558
1559 assert(handle);
1560
1561 /* command with no reply, use a valid endpoint but zero size */
1562 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1563
1564 switch (type) {
1565 case STLINK_MODE_DEBUG_JTAG:
1566 case STLINK_MODE_DEBUG_SWD:
1567 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1568 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_EXIT;
1569 break;
1570 case STLINK_MODE_DEBUG_SWIM:
1571 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1572 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_EXIT;
1573 break;
1574 case STLINK_MODE_DFU:
1575 h->cmdbuf[h->cmdidx++] = STLINK_DFU_COMMAND;
1576 h->cmdbuf[h->cmdidx++] = STLINK_DFU_EXIT;
1577 break;
1578 case STLINK_MODE_MASS:
1579 default:
1580 return ERROR_FAIL;
1581 }
1582
1583 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 0);
1584
1585 if (res != ERROR_OK)
1586 return res;
1587
1588 return ERROR_OK;
1589 }
1590
1591 static int stlink_usb_assert_srst(void *handle, int srst);
1592
1593 static enum stlink_mode stlink_get_mode(enum hl_transports t)
1594 {
1595 switch (t) {
1596 case HL_TRANSPORT_SWD:
1597 return STLINK_MODE_DEBUG_SWD;
1598 case HL_TRANSPORT_JTAG:
1599 return STLINK_MODE_DEBUG_JTAG;
1600 default:
1601 return STLINK_MODE_UNKNOWN;
1602 }
1603 }
1604
1605 /** */
1606 static int stlink_usb_exit_mode(void *handle)
1607 {
1608 int res;
1609 uint8_t mode;
1610 enum stlink_mode emode;
1611
1612 assert(handle);
1613
1614 res = stlink_usb_current_mode(handle, &mode);
1615
1616 if (res != ERROR_OK)
1617 return res;
1618
1619 LOG_DEBUG("MODE: 0x%02X", mode);
1620
1621 /* try to exit current mode */
1622 switch (mode) {
1623 case STLINK_DEV_DFU_MODE:
1624 emode = STLINK_MODE_DFU;
1625 break;
1626 case STLINK_DEV_DEBUG_MODE:
1627 emode = STLINK_MODE_DEBUG_SWD;
1628 break;
1629 case STLINK_DEV_SWIM_MODE:
1630 emode = STLINK_MODE_DEBUG_SWIM;
1631 break;
1632 case STLINK_DEV_BOOTLOADER_MODE:
1633 case STLINK_DEV_MASS_MODE:
1634 default:
1635 emode = STLINK_MODE_UNKNOWN;
1636 break;
1637 }
1638
1639 if (emode != STLINK_MODE_UNKNOWN)
1640 return stlink_usb_mode_leave(handle, emode);
1641
1642 return ERROR_OK;
1643 }
1644
1645 /** */
1646 static int stlink_usb_init_mode(void *handle, bool connect_under_reset, int initial_interface_speed)
1647 {
1648 int res;
1649 uint8_t mode;
1650 enum stlink_mode emode;
1651 struct stlink_usb_handle_s *h = handle;
1652
1653 assert(handle);
1654
1655 res = stlink_usb_exit_mode(handle);
1656 if (res != ERROR_OK)
1657 return res;
1658
1659 res = stlink_usb_current_mode(handle, &mode);
1660
1661 if (res != ERROR_OK)
1662 return res;
1663
1664 /* we check the target voltage here as an aid to debugging connection problems.
1665 * the stlink requires the target Vdd to be connected for reliable debugging.
1666 * this cmd is supported in all modes except DFU
1667 */
1668 if (mode != STLINK_DEV_DFU_MODE) {
1669
1670 float target_voltage;
1671
1672 /* check target voltage (if supported) */
1673 res = stlink_usb_check_voltage(h, &target_voltage);
1674
1675 if (res != ERROR_OK) {
1676 if (res != ERROR_COMMAND_NOTFOUND)
1677 LOG_ERROR("voltage check failed");
1678 /* attempt to continue as it is not a catastrophic failure */
1679 } else {
1680 /* check for a sensible target voltage, operating range is 1.65-5.5v
1681 * according to datasheet */
1682 if (target_voltage < 1.5)
1683 LOG_ERROR("target voltage may be too low for reliable debugging");
1684 }
1685 }
1686
1687 LOG_DEBUG("MODE: 0x%02X", mode);
1688
1689 /* set selected mode */
1690 emode = h->st_mode;
1691
1692 if (emode == STLINK_MODE_UNKNOWN) {
1693 LOG_ERROR("selected mode (transport) not supported");
1694 return ERROR_FAIL;
1695 }
1696
1697 /* set the speed before entering the mode, as the chip discovery phase should be done at this speed too */
1698 if (emode == STLINK_MODE_DEBUG_JTAG) {
1699 if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
1700 stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
1701 stlink_speed(h, initial_interface_speed, false);
1702 }
1703 } else if (emode == STLINK_MODE_DEBUG_SWD) {
1704 if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
1705 stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
1706 stlink_speed(h, initial_interface_speed, false);
1707 }
1708 }
1709
1710 if (h->version.jtag_api == STLINK_JTAG_API_V3 &&
1711 (emode == STLINK_MODE_DEBUG_JTAG || emode == STLINK_MODE_DEBUG_SWD)) {
1712 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
1713
1714 stlink_get_com_freq(h, (emode == STLINK_MODE_DEBUG_JTAG), map);
1715 stlink_dump_speed_map(map, ARRAY_SIZE(map));
1716 stlink_speed(h, initial_interface_speed, false);
1717 }
1718
1719 /* preliminary SRST assert:
1720 * We want SRST is asserted before activating debug signals (mode_enter).
1721 * As the required mode has not been set, the adapter may not know what pin to use.
1722 * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
1723 * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
1724 * after power on, SWIM_RST stays unchanged */
1725 if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
1726 stlink_usb_assert_srst(handle, 0);
1727 /* do not check the return status here, we will
1728 proceed and enter the desired mode below
1729 and try asserting srst again. */
1730
1731 res = stlink_usb_mode_enter(handle, emode);
1732 if (res != ERROR_OK)
1733 return res;
1734
1735 /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
1736 if (connect_under_reset) {
1737 res = stlink_usb_assert_srst(handle, 0);
1738 if (res != ERROR_OK)
1739 return res;
1740 }
1741
1742 res = stlink_usb_current_mode(handle, &mode);
1743
1744 if (res != ERROR_OK)
1745 return res;
1746
1747 LOG_DEBUG("MODE: 0x%02X", mode);
1748
1749 return ERROR_OK;
1750 }
1751
1752 /* request status from last swim request */
1753 static int stlink_swim_status(void *handle)
1754 {
1755 struct stlink_usb_handle_s *h = handle;
1756 int res;
1757
1758 stlink_usb_init_buffer(handle, h->rx_ep, 4);
1759 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1760 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
1761 /* error is checked by the caller */
1762 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1763 if (res != ERROR_OK)
1764 return res;
1765 return ERROR_OK;
1766 }
1767 /*
1768 the purpose of this function is unknown...
1769 capabilities? anyway for swim v6 it returns
1770 0001020600000000
1771 */
1772 __attribute__((unused))
1773 static int stlink_swim_cap(void *handle, uint8_t *cap)
1774 {
1775 struct stlink_usb_handle_s *h = handle;
1776 int res;
1777
1778 stlink_usb_init_buffer(handle, h->rx_ep, 8);
1779 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1780 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
1781 h->cmdbuf[h->cmdidx++] = 0x01;
1782 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1783 if (res != ERROR_OK)
1784 return res;
1785 memcpy(cap, h->databuf, 8);
1786 return ERROR_OK;
1787 }
1788
1789 /* debug dongle assert/deassert sreset line */
1790 static int stlink_swim_assert_reset(void *handle, int reset)
1791 {
1792 struct stlink_usb_handle_s *h = handle;
1793 int res;
1794
1795 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1796 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1797 if (!reset)
1798 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
1799 else
1800 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
1801 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1802 if (res != ERROR_OK)
1803 return res;
1804 return ERROR_OK;
1805 }
1806
1807 /*
1808 send swim enter seq
1809 1.3ms low then 750Hz then 1.5kHz
1810 */
1811 static int stlink_swim_enter(void *handle)
1812 {
1813 struct stlink_usb_handle_s *h = handle;
1814 int res;
1815
1816 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1817 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1818 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
1819 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1820 if (res != ERROR_OK)
1821 return res;
1822 return ERROR_OK;
1823 }
1824
1825 /* switch high/low speed swim */
1826 static int stlink_swim_speed(void *handle, int speed)
1827 {
1828 struct stlink_usb_handle_s *h = handle;
1829 int res;
1830
1831 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1832 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1833 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
1834 if (speed)
1835 h->cmdbuf[h->cmdidx++] = 1;
1836 else
1837 h->cmdbuf[h->cmdidx++] = 0;
1838 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1839 if (res != ERROR_OK)
1840 return res;
1841 return ERROR_OK;
1842 }
1843
1844 /*
1845 initiate srst from swim.
1846 nrst is pulled low for 50us.
1847 */
1848 static int stlink_swim_generate_rst(void *handle)
1849 {
1850 struct stlink_usb_handle_s *h = handle;
1851 int res;
1852
1853 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1854 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1855 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
1856 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1857 if (res != ERROR_OK)
1858 return res;
1859 return ERROR_OK;
1860 }
1861
1862 /*
1863 send resynchronize sequence
1864 swim is pulled low for 16us
1865 reply is 64 clks low
1866 */
1867 static int stlink_swim_resync(void *handle)
1868 {
1869 struct stlink_usb_handle_s *h = handle;
1870 int res;
1871
1872 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1873 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1874 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
1875 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1876 if (res != ERROR_OK)
1877 return res;
1878 return ERROR_OK;
1879 }
1880
1881 static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
1882 {
1883 struct stlink_usb_handle_s *h = handle;
1884 int res;
1885 unsigned int i;
1886 unsigned int datalen = 0;
1887 int cmdsize = STLINK_CMD_SIZE_V2;
1888
1889 if (len > STLINK_SWIM_DATA_SIZE)
1890 return ERROR_FAIL;
1891
1892 if (h->version.stlink == 1)
1893 cmdsize = STLINK_SG_SIZE;
1894
1895 stlink_usb_init_buffer(handle, h->tx_ep, 0);
1896 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1897 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
1898 h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1899 h->cmdidx += 2;
1900 h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1901 h->cmdidx += 4;
1902 for (i = 0; i < len; i++) {
1903 if (h->cmdidx == cmdsize)
1904 h->databuf[datalen++] = *(data++);
1905 else
1906 h->cmdbuf[h->cmdidx++] = *(data++);
1907 }
1908 if (h->version.stlink == 1)
1909 stlink_usb_set_cbw_transfer_datalength(handle, datalen);
1910
1911 res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
1912 if (res != ERROR_OK)
1913 return res;
1914 return ERROR_OK;
1915 }
1916
1917 static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
1918 {
1919 struct stlink_usb_handle_s *h = handle;
1920 int res;
1921
1922 if (len > STLINK_SWIM_DATA_SIZE)
1923 return ERROR_FAIL;
1924
1925 stlink_usb_init_buffer(handle, h->rx_ep, 0);
1926 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1927 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
1928 h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1929 h->cmdidx += 2;
1930 h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1931 h->cmdidx += 4;
1932 res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1933 if (res != ERROR_OK)
1934 return res;
1935
1936 stlink_usb_init_buffer(handle, h->rx_ep, len);
1937 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1938 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
1939 res = stlink_usb_xfer_noerrcheck(handle, data, len);
1940 if (res != ERROR_OK)
1941 return res;
1942
1943 return ERROR_OK;
1944 }
1945
1946 /** */
1947 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
1948 {
1949 int res, offset;
1950 struct stlink_usb_handle_s *h = handle;
1951
1952 assert(handle);
1953
1954 /* there is no swim read core id cmd */
1955 if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1956 *idcode = 0;
1957 return ERROR_OK;
1958 }
1959
1960 stlink_usb_init_buffer(handle, h->rx_ep, 12);
1961
1962 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1963 if (h->version.jtag_api == STLINK_JTAG_API_V1) {
1964 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
1965
1966 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1967 offset = 0;
1968 } else {
1969 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_IDCODES;
1970
1971 res = stlink_usb_xfer_errcheck(handle, h->databuf, 12);
1972 offset = 4;
1973 }
1974
1975 if (res != ERROR_OK)
1976 return res;
1977
1978 *idcode = le_to_h_u32(h->databuf + offset);
1979
1980 LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
1981
1982 return ERROR_OK;
1983 }
1984
1985 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
1986 {
1987 struct stlink_usb_handle_s *h = handle;
1988 int res;
1989
1990 assert(handle);
1991
1992 stlink_usb_init_buffer(handle, h->rx_ep, 8);
1993
1994 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1995 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
1996 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1997 h->cmdidx += 4;
1998
1999 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
2000 if (res != ERROR_OK)
2001 return res;
2002
2003 *val = le_to_h_u32(h->databuf + 4);
2004 return ERROR_OK;
2005 }
2006
2007 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
2008 {
2009 struct stlink_usb_handle_s *h = handle;
2010
2011 assert(handle);
2012
2013 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2014
2015 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2016 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2017 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
2018 else
2019 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
2020 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2021 h->cmdidx += 4;
2022 h_u32_to_le(h->cmdbuf+h->cmdidx, val);
2023 h->cmdidx += 4;
2024
2025 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2026 }
2027
2028 /** */
2029 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
2030 {
2031 struct stlink_usb_handle_s *h = handle;
2032
2033 assert(handle);
2034
2035 if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
2036 int res;
2037
2038 stlink_usb_init_buffer(handle, h->rx_ep, 10);
2039
2040 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2041 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
2042
2043 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
2044 if (res != ERROR_OK)
2045 return res;
2046
2047 size_t bytes_avail = le_to_h_u16(h->databuf);
2048 *size = bytes_avail < *size ? bytes_avail : *size;
2049
2050 if (*size > 0) {
2051 res = stlink_usb_read_trace(handle, buf, *size);
2052 if (res != ERROR_OK)
2053 return res;
2054 return ERROR_OK;
2055 }
2056 }
2057 *size = 0;
2058 return ERROR_OK;
2059 }
2060
2061 static enum target_state stlink_usb_v2_get_status(void *handle)
2062 {
2063 int result;
2064 uint32_t status;
2065
2066 result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
2067 if (result != ERROR_OK)
2068 return TARGET_UNKNOWN;
2069
2070 if (status & S_HALT)
2071 return TARGET_HALTED;
2072 else if (status & S_RESET_ST)
2073 return TARGET_RESET;
2074
2075 return TARGET_RUNNING;
2076 }
2077
2078 /** */
2079 static enum target_state stlink_usb_state(void *handle)
2080 {
2081 int res;
2082 struct stlink_usb_handle_s *h = handle;
2083
2084 assert(handle);
2085
2086 if (h->reconnect_pending) {
2087 LOG_INFO("Previous state query failed, trying to reconnect");
2088 res = stlink_usb_mode_enter(handle, h->st_mode);
2089 if (res != ERROR_OK)
2090 return TARGET_UNKNOWN;
2091
2092 h->reconnect_pending = false;
2093 }
2094
2095 if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2096 res = stlink_usb_v2_get_status(handle);
2097 if (res == TARGET_UNKNOWN)
2098 h->reconnect_pending = true;
2099 return res;
2100 }
2101
2102 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2103
2104 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2105 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
2106
2107 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
2108
2109 if (res != ERROR_OK)
2110 return TARGET_UNKNOWN;
2111
2112 if (h->databuf[0] == STLINK_CORE_RUNNING)
2113 return TARGET_RUNNING;
2114 if (h->databuf[0] == STLINK_CORE_HALTED)
2115 return TARGET_HALTED;
2116
2117 h->reconnect_pending = true;
2118
2119 return TARGET_UNKNOWN;
2120 }
2121
2122 static int stlink_usb_assert_srst(void *handle, int srst)
2123 {
2124 struct stlink_usb_handle_s *h = handle;
2125
2126 assert(handle);
2127
2128 if (h->st_mode == STLINK_MODE_DEBUG_SWIM)
2129 return stlink_swim_assert_reset(handle, srst);
2130
2131 if (h->version.stlink == 1)
2132 return ERROR_COMMAND_NOTFOUND;
2133
2134 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2135
2136 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2137 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
2138 h->cmdbuf[h->cmdidx++] = srst;
2139
2140 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2141 }
2142
2143 /** */
2144 static void stlink_usb_trace_disable(void *handle)
2145 {
2146 int res = ERROR_OK;
2147 struct stlink_usb_handle_s *h = handle;
2148
2149 assert(handle);
2150
2151 assert(h->version.flags & STLINK_F_HAS_TRACE);
2152
2153 LOG_DEBUG("Tracing: disable");
2154
2155 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2156 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2157 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
2158 res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2159
2160 if (res == ERROR_OK)
2161 h->trace.enabled = false;
2162 }
2163
2164
2165 /** */
2166 static int stlink_usb_trace_enable(void *handle)
2167 {
2168 int res;
2169 struct stlink_usb_handle_s *h = handle;
2170
2171 assert(handle);
2172
2173 if (h->version.flags & STLINK_F_HAS_TRACE) {
2174 stlink_usb_init_buffer(handle, h->rx_ep, 10);
2175
2176 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2177 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
2178 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
2179 h->cmdidx += 2;
2180 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
2181 h->cmdidx += 4;
2182
2183 res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2184
2185 if (res == ERROR_OK) {
2186 h->trace.enabled = true;
2187 LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
2188 }
2189 } else {
2190 LOG_ERROR("Tracing is not supported by this version.");
2191 res = ERROR_FAIL;
2192 }
2193
2194 return res;
2195 }
2196
2197 /** */
2198 static int stlink_usb_reset(void *handle)
2199 {
2200 struct stlink_usb_handle_s *h = handle;
2201 int retval;
2202
2203 assert(handle);
2204
2205 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2206
2207 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2208
2209 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2210 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
2211 else
2212 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
2213
2214 retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
2215 if (retval != ERROR_OK)
2216 return retval;
2217
2218 if (h->trace.enabled) {
2219 stlink_usb_trace_disable(h);
2220 return stlink_usb_trace_enable(h);
2221 }
2222
2223 return ERROR_OK;
2224 }
2225
2226 /** */
2227 static int stlink_usb_run(void *handle)
2228 {
2229 int res;
2230 struct stlink_usb_handle_s *h = handle;
2231
2232 assert(handle);
2233
2234 if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2235 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
2236
2237 return res;
2238 }
2239
2240 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2241
2242 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2243 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
2244
2245 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2246 }
2247
2248 /** */
2249 static int stlink_usb_halt(void *handle)
2250 {
2251 int res;
2252 struct stlink_usb_handle_s *h = handle;
2253
2254 assert(handle);
2255
2256 if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2257 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
2258
2259 return res;
2260 }
2261
2262 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2263
2264 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2265 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
2266
2267 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2268 }
2269
2270 /** */
2271 static int stlink_usb_step(void *handle)
2272 {
2273 struct stlink_usb_handle_s *h = handle;
2274
2275 assert(handle);
2276
2277 if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2278 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
2279 * that the Cortex-M3 currently does. */
2280 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
2281 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
2282 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
2283 }
2284
2285 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2286
2287 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2288 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
2289
2290 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2291 }
2292
2293 /** */
2294 static int stlink_usb_read_regs(void *handle)
2295 {
2296 int res;
2297 struct stlink_usb_handle_s *h = handle;
2298
2299 assert(handle);
2300
2301 stlink_usb_init_buffer(handle, h->rx_ep, 88);
2302
2303 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2304 if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2305
2306 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
2307 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 84);
2308 /* regs data from offset 0 */
2309 } else {
2310 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
2311 res = stlink_usb_xfer_errcheck(handle, h->databuf, 88);
2312 /* status at offset 0, regs data from offset 4 */
2313 }
2314
2315 return res;
2316 }
2317
2318 /** */
2319 static int stlink_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
2320 {
2321 int res;
2322 struct stlink_usb_handle_s *h = handle;
2323
2324 assert(handle);
2325
2326 if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
2327 res = stlink_usb_write_debug_reg(h, DCB_DCRSR, regsel & 0x7f);
2328 if (res != ERROR_OK)
2329 return res;
2330
2331 /* FIXME: poll DHCSR.S_REGRDY before read DCRDR */
2332 return stlink_usb_v2_read_debug_reg(h, DCB_DCRDR, val);
2333 }
2334
2335 stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
2336
2337 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2338 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2339 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
2340 else
2341 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
2342 h->cmdbuf[h->cmdidx++] = regsel;
2343
2344 if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2345 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
2346 if (res != ERROR_OK)
2347 return res;
2348 *val = le_to_h_u32(h->databuf);
2349 return ERROR_OK;
2350 } else {
2351 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
2352 if (res != ERROR_OK)
2353 return res;
2354 *val = le_to_h_u32(h->databuf + 4);
2355 return ERROR_OK;
2356 }
2357 }
2358
2359 /** */
2360 static int stlink_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
2361 {
2362 struct stlink_usb_handle_s *h = handle;
2363
2364 assert(handle);
2365
2366 if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
2367 int res = stlink_usb_write_debug_reg(h, DCB_DCRDR, val);
2368 if (res != ERROR_OK)
2369 return res;
2370
2371 return stlink_usb_write_debug_reg(h, DCB_DCRSR, DCRSR_WNR | (regsel & 0x7f));
2372 /* FIXME: poll DHCSR.S_REGRDY after write DCRSR */
2373 }
2374
2375 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2376
2377 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2378 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2379 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
2380 else
2381 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
2382 h->cmdbuf[h->cmdidx++] = regsel;
2383 h_u32_to_le(h->cmdbuf+h->cmdidx, val);
2384 h->cmdidx += 4;
2385
2386 return stlink_cmd_allow_retry(handle, h->databuf, 2);
2387 }
2388
2389 static int stlink_usb_get_rw_status(void *handle)
2390 {
2391 struct stlink_usb_handle_s *h = handle;
2392
2393 assert(handle);
2394
2395 if (h->version.jtag_api == STLINK_JTAG_API_V1)
2396 return ERROR_OK;
2397
2398 stlink_usb_init_buffer(handle, h->rx_ep, 2);
2399
2400 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2401 if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
2402 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
2403 return stlink_usb_xfer_errcheck(handle, h->databuf, 12);
2404 } else {
2405 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
2406 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2407 }
2408 }
2409
2410 /** */
2411 static int stlink_usb_read_mem8(void *handle, uint8_t ap_num, uint32_t csw,
2412 uint32_t addr, uint16_t len, uint8_t *buffer)
2413 {
2414 int res;
2415 uint16_t read_len = len;
2416 struct stlink_usb_handle_s *h = handle;
2417
2418 assert(handle);
2419
2420 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2421 return ERROR_COMMAND_NOTFOUND;
2422
2423 /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2424 if (len > stlink_usb_block(h)) {
2425 LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
2426 return ERROR_FAIL;
2427 }
2428
2429 stlink_usb_init_buffer(handle, h->rx_ep, read_len);
2430
2431 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2432 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
2433 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2434 h->cmdidx += 4;
2435 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2436 h->cmdidx += 2;
2437 h->cmdbuf[h->cmdidx++] = ap_num;
2438 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2439 h->cmdidx += 3;
2440
2441 /* we need to fix read length for single bytes */
2442 if (read_len == 1)
2443 read_len++;
2444
2445 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, read_len);
2446
2447 if (res != ERROR_OK)
2448 return res;
2449
2450 memcpy(buffer, h->databuf, len);
2451
2452 return stlink_usb_get_rw_status(handle);
2453 }
2454
2455 /** */
2456 static int stlink_usb_write_mem8(void *handle, uint8_t ap_num, uint32_t csw,
2457 uint32_t addr, uint16_t len, const uint8_t *buffer)
2458 {
2459 int res;
2460 struct stlink_usb_handle_s *h = handle;
2461
2462 assert(handle);
2463
2464 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2465 return ERROR_COMMAND_NOTFOUND;
2466
2467 /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2468 if (len > stlink_usb_block(h)) {
2469 LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
2470 return ERROR_FAIL;
2471 }
2472
2473 stlink_usb_init_buffer(handle, h->tx_ep, len);
2474
2475 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2476 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
2477 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2478 h->cmdidx += 4;
2479 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2480 h->cmdidx += 2;
2481 h->cmdbuf[h->cmdidx++] = ap_num;
2482 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2483 h->cmdidx += 3;
2484
2485 res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2486
2487 if (res != ERROR_OK)
2488 return res;
2489
2490 return stlink_usb_get_rw_status(handle);
2491 }
2492
2493 /** */
2494 static int stlink_usb_read_mem16(void *handle, uint8_t ap_num, uint32_t csw,
2495 uint32_t addr, uint16_t len, uint8_t *buffer)
2496 {
2497 int res;
2498 struct stlink_usb_handle_s *h = handle;
2499
2500 assert(handle);
2501
2502 if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2503 return ERROR_COMMAND_NOTFOUND;
2504
2505 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2506 return ERROR_COMMAND_NOTFOUND;
2507
2508 if (len > STLINK_MAX_RW16_32) {
2509 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2510 return ERROR_FAIL;
2511 }
2512
2513 /* data must be a multiple of 2 and half-word aligned */
2514 if (len % 2 || addr % 2) {
2515 LOG_DEBUG("Invalid data alignment");
2516 return ERROR_TARGET_UNALIGNED_ACCESS;
2517 }
2518
2519 stlink_usb_init_buffer(handle, h->rx_ep, len);
2520
2521 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2522 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
2523 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2524 h->cmdidx += 4;
2525 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2526 h->cmdidx += 2;
2527 h->cmdbuf[h->cmdidx++] = ap_num;
2528 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2529 h->cmdidx += 3;
2530
2531 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2532
2533 if (res != ERROR_OK)
2534 return res;
2535
2536 memcpy(buffer, h->databuf, len);
2537
2538 return stlink_usb_get_rw_status(handle);
2539 }
2540
2541 /** */
2542 static int stlink_usb_write_mem16(void *handle, uint8_t ap_num, uint32_t csw,
2543 uint32_t addr, uint16_t len, const uint8_t *buffer)
2544 {
2545 int res;
2546 struct stlink_usb_handle_s *h = handle;
2547
2548 assert(handle);
2549
2550 if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2551 return ERROR_COMMAND_NOTFOUND;
2552
2553 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2554 return ERROR_COMMAND_NOTFOUND;
2555
2556 if (len > STLINK_MAX_RW16_32) {
2557 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2558 return ERROR_FAIL;
2559 }
2560
2561 /* data must be a multiple of 2 and half-word aligned */
2562 if (len % 2 || addr % 2) {
2563 LOG_DEBUG("Invalid data alignment");
2564 return ERROR_TARGET_UNALIGNED_ACCESS;
2565 }
2566
2567 stlink_usb_init_buffer(handle, h->tx_ep, len);
2568
2569 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2570 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
2571 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2572 h->cmdidx += 4;
2573 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2574 h->cmdidx += 2;
2575 h->cmdbuf[h->cmdidx++] = ap_num;
2576 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2577 h->cmdidx += 3;
2578
2579 res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2580
2581 if (res != ERROR_OK)
2582 return res;
2583
2584 return stlink_usb_get_rw_status(handle);
2585 }
2586
2587 /** */
2588 static int stlink_usb_read_mem32(void *handle, uint8_t ap_num, uint32_t csw,
2589 uint32_t addr, uint16_t len, uint8_t *buffer)
2590 {
2591 int res;
2592 struct stlink_usb_handle_s *h = handle;
2593
2594 assert(handle);
2595
2596 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2597 return ERROR_COMMAND_NOTFOUND;
2598
2599 if (len > STLINK_MAX_RW16_32) {
2600 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2601 return ERROR_FAIL;
2602 }
2603
2604 /* data must be a multiple of 4 and word aligned */
2605 if (len % 4 || addr % 4) {
2606 LOG_DEBUG("Invalid data alignment");
2607 return ERROR_TARGET_UNALIGNED_ACCESS;
2608 }
2609
2610 stlink_usb_init_buffer(handle, h->rx_ep, len);
2611
2612 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2613 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
2614 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2615 h->cmdidx += 4;
2616 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2617 h->cmdidx += 2;
2618 h->cmdbuf[h->cmdidx++] = ap_num;
2619 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2620 h->cmdidx += 3;
2621
2622 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2623
2624 if (res != ERROR_OK)
2625 return res;
2626
2627 memcpy(buffer, h->databuf, len);
2628
2629 return stlink_usb_get_rw_status(handle);
2630 }
2631
2632 /** */
2633 static int stlink_usb_write_mem32(void *handle, uint8_t ap_num, uint32_t csw,
2634 uint32_t addr, uint16_t len, const uint8_t *buffer)
2635 {
2636 int res;
2637 struct stlink_usb_handle_s *h = handle;
2638
2639 assert(handle);
2640
2641 if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2642 return ERROR_COMMAND_NOTFOUND;
2643
2644 if (len > STLINK_MAX_RW16_32) {
2645 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2646 return ERROR_FAIL;
2647 }
2648
2649 /* data must be a multiple of 4 and word aligned */
2650 if (len % 4 || addr % 4) {
2651 LOG_DEBUG("Invalid data alignment");
2652 return ERROR_TARGET_UNALIGNED_ACCESS;
2653 }
2654
2655 stlink_usb_init_buffer(handle, h->tx_ep, len);
2656
2657 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2658 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
2659 h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2660 h->cmdidx += 4;
2661 h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2662 h->cmdidx += 2;
2663 h->cmdbuf[h->cmdidx++] = ap_num;
2664 h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2665 h->cmdidx += 3;
2666
2667 res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2668
2669 if (res != ERROR_OK)
2670 return res;
2671
2672 return stlink_usb_get_rw_status(handle);
2673 }
2674
2675 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
2676 {
2677 uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
2678 if (max_tar_block == 0)
2679 max_tar_block = 4;
2680 return max_tar_block;
2681 }
2682
2683 static int stlink_usb_read_ap_mem(void *handle, uint8_t ap_num, uint32_t csw,
2684 uint32_t addr, uint32_t size, uint32_t count, uint8_t *buffer)
2685 {
2686 int retval = ERROR_OK;
2687 uint32_t bytes_remaining;
2688 int retries = 0;
2689 struct stlink_usb_handle_s *h = handle;
2690
2691 /* calculate byte count */
2692 count *= size;
2693
2694 /* switch to 8 bit if stlink does not support 16 bit memory read */
2695 if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2696 size = 1;
2697
2698 while (count) {
2699 bytes_remaining = (size != 1) ?
2700 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2701
2702 if (count < bytes_remaining)
2703 bytes_remaining = count;
2704
2705 /*
2706 * all stlink support 8/32bit memory read/writes and only from
2707 * stlink V2J26 there is support for 16 bit memory read/write.
2708 * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2709 * as 8bit access.
2710 */
2711 if (size != 1) {
2712 /* When in jtag mode the stlink uses the auto-increment functionality.
2713 * However it expects us to pass the data correctly, this includes
2714 * alignment and any page boundaries. We already do this as part of the
2715 * adi_v5 implementation, but the stlink is a hla adapter and so this
2716 * needs implementing manually.
2717 * currently this only affects jtag mode, according to ST they do single
2718 * access in SWD mode - but this may change and so we do it for both modes */
2719
2720 /* we first need to check for any unaligned bytes */
2721 if (addr & (size - 1)) {
2722 uint32_t head_bytes = size - (addr & (size - 1));
2723 retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, head_bytes, buffer);
2724 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2725 usleep((1 << retries++) * 1000);
2726 continue;
2727 }
2728 if (retval != ERROR_OK)
2729 return retval;
2730 buffer += head_bytes;
2731 addr += head_bytes;
2732 count -= head_bytes;
2733 bytes_remaining -= head_bytes;
2734 }
2735
2736 if (bytes_remaining & (size - 1))
2737 retval = stlink_usb_read_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer);
2738 else if (size == 2)
2739 retval = stlink_usb_read_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
2740 else
2741 retval = stlink_usb_read_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer);
2742 } else {
2743 retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer);
2744 }
2745
2746 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2747 usleep((1 << retries++) * 1000);
2748 continue;
2749 }
2750 if (retval != ERROR_OK)
2751 return retval;
2752
2753 buffer += bytes_remaining;
2754 addr += bytes_remaining;
2755 count -= bytes_remaining;
2756 }
2757
2758 return retval;
2759 }
2760
2761 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
2762 uint32_t count, uint8_t *buffer)
2763 {
2764 return stlink_usb_read_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW,
2765 addr, size, count, buffer);
2766 }
2767
2768 static int stlink_usb_write_ap_mem(void *handle, uint8_t ap_num, uint32_t csw,
2769 uint32_t addr, uint32_t size, uint32_t count, const uint8_t *buffer)
2770 {
2771 int retval = ERROR_OK;
2772 uint32_t bytes_remaining;
2773 int retries = 0;
2774 struct stlink_usb_handle_s *h = handle;
2775
2776 /* calculate byte count */
2777 count *= size;
2778
2779 /* switch to 8 bit if stlink does not support 16 bit memory read */
2780 if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2781 size = 1;
2782
2783 while (count) {
2784
2785 bytes_remaining = (size != 1) ?
2786 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2787
2788 if (count < bytes_remaining)
2789 bytes_remaining = count;
2790
2791 /*
2792 * all stlink support 8/32bit memory read/writes and only from
2793 * stlink V2J26 there is support for 16 bit memory read/write.
2794 * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2795 * as 8bit access.
2796 */
2797 if (size != 1) {
2798
2799 /* When in jtag mode the stlink uses the auto-increment functionality.
2800 * However it expects us to pass the data correctly, this includes
2801 * alignment and any page boundaries. We already do this as part of the
2802 * adi_v5 implementation, but the stlink is a hla adapter and so this
2803 * needs implementing manually.
2804 * currently this only affects jtag mode, according to ST they do single
2805 * access in SWD mode - but this may change and so we do it for both modes */
2806
2807 /* we first need to check for any unaligned bytes */
2808 if (addr & (size - 1)) {
2809
2810 uint32_t head_bytes = size - (addr & (size - 1));
2811 retval = stlink_usb_write_mem8(handle, ap_num, csw, addr, head_bytes, buffer);
2812 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2813 usleep((1<<retries++) * 1000);
2814 continue;
2815 }
2816 if (retval != ERROR_OK)
2817 return retval;
2818 buffer += head_bytes;
2819 addr += head_bytes;
2820 count -= head_bytes;
2821 bytes_remaining -= head_bytes;
2822 }
2823
2824 if (bytes_remaining & (size - 1))
2825 retval = stlink_usb_write_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer);
2826 else if (size == 2)
2827 retval = stlink_usb_write_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
2828 else
2829 retval = stlink_usb_write_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer);
2830
2831 } else
2832 retval = stlink_usb_write_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer);
2833 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2834 usleep((1<<retries++) * 1000);
2835 continue;
2836 }
2837 if (retval != ERROR_OK)
2838 return retval;
2839
2840 buffer += bytes_remaining;
2841 addr += bytes_remaining;
2842 count -= bytes_remaining;
2843 }
2844
2845 return retval;
2846 }
2847
2848 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
2849 uint32_t count, const uint8_t *buffer)
2850 {
2851 return stlink_usb_write_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW,
2852 addr, size, count, buffer);
2853 }
2854
2855 /** */
2856 static int stlink_usb_override_target(const char *targetname)
2857 {
2858 return !strcmp(targetname, "cortex_m");
2859 }
2860
2861 static int stlink_speed_swim(void *handle, int khz, bool query)
2862 {
2863 int retval;
2864
2865 /*
2866 we only have low and high speed...
2867 before changing speed the SWIM_CSR HS bit
2868 must be updated
2869 */
2870 if (!query) {
2871 retval = stlink_swim_speed(handle, (khz < SWIM_FREQ_HIGH) ? 0 : 1);
2872 if (retval != ERROR_OK)
2873 LOG_ERROR("Unable to set adapter speed");
2874 }
2875
2876 return (khz < SWIM_FREQ_HIGH) ? SWIM_FREQ_LOW : SWIM_FREQ_HIGH;
2877 }
2878
2879 static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
2880 {
2881 unsigned int i;
2882 int speed_index = -1;
2883 int speed_diff = INT_MAX;
2884 int last_valid_speed = -1;
2885 bool match = true;
2886
2887 for (i = 0; i < map_size; i++) {
2888 if (!map[i].speed)
2889 continue;
2890 last_valid_speed = i;
2891 if (khz == map[i].speed) {
2892 speed_index = i;
2893 break;
2894 } else {
2895 int current_diff = khz - map[i].speed;
2896 /* get abs value for comparison */
2897 current_diff = (current_diff > 0) ? current_diff : -current_diff;
2898 if ((current_diff < speed_diff) && khz >= map[i].speed) {
2899 speed_diff = current_diff;
2900 speed_index = i;
2901 }
2902 }
2903 }
2904
2905 if (speed_index == -1) {
2906 /* this will only be here if we cannot match the slow speed.
2907 * use the slowest speed we support.*/
2908 speed_index = last_valid_speed;
2909 match = false;
2910 } else if (i == map_size)
2911 match = false;
2912
2913 if (!match && query) {
2914 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz",
2915 khz, map[speed_index].speed);
2916 }
2917
2918 return speed_index;
2919 }
2920
2921 static int stlink_speed_swd(void *handle, int khz, bool query)
2922 {
2923 int speed_index;
2924 struct stlink_usb_handle_s *h = handle;
2925
2926 /* old firmware cannot change it */
2927 if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
2928 return khz;
2929
2930 speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
2931 ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
2932
2933 if (!query) {
2934 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
2935 if (result != ERROR_OK) {
2936 LOG_ERROR("Unable to set adapter speed");
2937 return khz;
2938 }
2939 }
2940
2941 return stlink_khz_to_speed_map_swd[speed_index].speed;
2942 }
2943
2944 static int stlink_speed_jtag(void *handle, int khz, bool query)
2945 {
2946 int speed_index;
2947 struct stlink_usb_handle_s *h = handle;
2948
2949 /* old firmware cannot change it */
2950 if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
2951 return khz;
2952
2953 speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
2954 ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
2955
2956 if (!query) {
2957 int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
2958 if (result != ERROR_OK) {
2959 LOG_ERROR("Unable to set adapter speed");
2960 return khz;
2961 }
2962 }
2963
2964 return stlink_khz_to_speed_map_jtag[speed_index].speed;
2965 }
2966
2967 static void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
2968 {
2969 unsigned int i;
2970
2971 LOG_DEBUG("Supported clock speeds are:");
2972 for (i = 0; i < map_size; i++)
2973 if (map[i].speed)
2974 LOG_DEBUG("%d kHz", map[i].speed);
2975 }
2976
2977 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
2978 {
2979 struct stlink_usb_handle_s *h = handle;
2980 int i;
2981
2982 if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2983 LOG_ERROR("Unknown command");
2984 return 0;
2985 }
2986
2987 stlink_usb_init_buffer(handle, h->rx_ep, 16);
2988
2989 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2990 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
2991 h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2992
2993 int res = stlink_usb_xfer_errcheck(handle, h->databuf, 52);
2994
2995 int size = h->databuf[8];
2996
2997 if (size > STLINK_V3_MAX_FREQ_NB)
2998 size = STLINK_V3_MAX_FREQ_NB;
2999
3000 for (i = 0; i < size; i++) {
3001 map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
3002 map[i].speed_divisor = i;
3003 }
3004
3005 /* set to zero all the next entries */
3006 for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
3007 map[i].speed = 0;
3008
3009 return res;
3010 }
3011
3012 static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
3013 {
3014 struct stlink_usb_handle_s *h = handle;
3015
3016 if (h->version.jtag_api != STLINK_JTAG_API_V3) {
3017 LOG_ERROR("Unknown command");
3018 return 0;
3019 }
3020
3021 stlink_usb_init_buffer(handle, h->rx_ep, 16);
3022
3023 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3024 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
3025 h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
3026 h->cmdbuf[h->cmdidx++] = 0;
3027
3028 h_u32_to_le(&h->cmdbuf[4], frequency);
3029
3030 return stlink_usb_xfer_errcheck(handle, h->databuf, 8);
3031 }
3032
3033 static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
3034 {
3035 struct stlink_usb_handle_s *h = handle;
3036 int speed_index;
3037 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
3038
3039 stlink_get_com_freq(h, is_jtag, map);
3040
3041 speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
3042
3043 if (!query) {
3044 int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
3045 if (result != ERROR_OK) {
3046 LOG_ERROR("Unable to set adapter speed");
3047 return khz;
3048 }
3049 }
3050 return map[speed_index].speed;
3051 }
3052
3053 static int stlink_speed(void *handle, int khz, bool query)
3054 {
3055 struct stlink_usb_handle_s *h = handle;
3056
3057 if (!handle)
3058 return khz;
3059
3060 switch (h->st_mode) {
3061 case STLINK_MODE_DEBUG_SWIM:
3062 return stlink_speed_swim(handle, khz, query);
3063 case STLINK_MODE_DEBUG_SWD:
3064 if (h->version.jtag_api == STLINK_JTAG_API_V3)
3065 return stlink_speed_v3(handle, false, khz, query);
3066 else
3067 return stlink_speed_swd(handle, khz, query);
3068 break;
3069 case STLINK_MODE_DEBUG_JTAG:
3070 if (h->version.jtag_api == STLINK_JTAG_API_V3)
3071 return stlink_speed_v3(handle, true, khz, query);
3072 else
3073 return stlink_speed_jtag(handle, khz, query);
3074 break;
3075 default:
3076 break;
3077 }
3078
3079 return khz;
3080 }
3081
3082 /** */
3083 static int stlink_usb_usb_close(void *handle)
3084 {
3085 struct stlink_usb_handle_s *h = handle;
3086
3087 if (!h)
3088 return ERROR_OK;
3089
3090 if (h->usb_backend_priv.fd) {
3091 stlink_usb_exit_mode(h);
3092 /* do not check return code, it prevent
3093 us from closing jtag_libusb */
3094 jtag_libusb_close(h->usb_backend_priv.fd);
3095 }
3096
3097 free(h->cmdbuf);
3098 free(h->databuf);
3099
3100 return ERROR_OK;
3101 }
3102
3103 /** */
3104 static int stlink_tcp_close(void *handle)
3105 {
3106 struct stlink_usb_handle_s *h = handle;
3107
3108 if (!h)
3109 return ERROR_OK;
3110
3111 int ret = ERROR_OK;
3112 if (h->tcp_backend_priv.connected) {
3113 if (h->tcp_backend_priv.connect_id) {
3114 stlink_usb_exit_mode(h);
3115
3116 /* close the stlink */
3117 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_CLOSE_DEV;
3118 memset(&h->tcp_backend_priv.send_buf[1], 0, 4); /* reserved */
3119 h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.connect_id);
3120 ret = stlink_tcp_send_cmd(h, 8, 4, true);
3121 if (ret != ERROR_OK)
3122 LOG_ERROR("cannot close the STLINK");
3123 }
3124
3125 if (close_socket(h->tcp_backend_priv.fd) != 0)
3126 LOG_ERROR("error closing the socket, errno: %s", strerror(errno));
3127 }
3128
3129 free(h->tcp_backend_priv.send_buf);
3130 free(h->tcp_backend_priv.recv_buf);
3131
3132 return ret;
3133 }
3134
3135 /** */
3136 static int stlink_close(void *handle)
3137 {
3138 if (handle) {
3139 struct stlink_usb_handle_s *h = handle;
3140
3141 stlink_usb_close(handle);