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