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

Linking to existing account procedure

If you already have an account and want to add another login method you MUST first sign in with your existing account and then change URL to read https://review.openocd.org/login/?link to get to this page again but this time it'll work for linking. Thank you.

SSH host keys fingerprints

1024 SHA256:YKx8b7u5ZWdcbp7/4AeXNaqElP49m6QrwfXaqQGJAOk gerrit-code-review@openocd.zylin.com (DSA)
384 SHA256:jHIbSQa4REvwCFG4cq5LBlBLxmxSqelQPem/EXIrxjk gerrit-code-review@openocd.org (ECDSA)
521 SHA256:UAOPYkU9Fjtcao0Ul/Rrlnj/OsQvt+pgdYSZ4jOYdgs gerrit-code-review@openocd.org (ECDSA)
256 SHA256:A13M5QlnozFOvTllybRZH6vm7iSt0XLxbA48yfc2yfY gerrit-code-review@openocd.org (ECDSA)
256 SHA256:spYMBqEYoAOtK7yZBrcwE8ZpYt6b68Cfh9yEVetvbXg gerrit-code-review@openocd.org (ED25519)
+--[ED25519 256]--+
|=..              |
|+o..   .         |
|*.o   . .        |
|+B . . .         |
|Bo. = o S        |
|Oo.+ + =         |
|oB=.* = . o      |
| =+=.+   + E     |
|. .=o   . o      |
+----[SHA256]-----+
2048 SHA256:0Onrb7/PHjpo6iVZ7xQX2riKN83FJ3KGU0TvI0TaFG4 gerrit-code-review@openocd.zylin.com (RSA)