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