1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <string.h>
4 #include <stdint.h>
5 #include <limits.h>
6
7 #if !defined(_MSC_VER)
8 #include <sys/time.h>
9 #endif
10
11 #include <sys/types.h>
12 #include <errno.h>
13 #include <unistd.h>
14
15 #if defined(_WIN32)
16 #include <win32_socket.h>
17 #endif
18
19 #include <stlink.h>
20 #include <helper.h>
21 #include "usb.h"
22
23 enum SCSI_Generic_Direction {SG_DXFER_TO_DEV = 0, SG_DXFER_FROM_DEV = 0x80};
24
le_to_h_u32(const uint8_t * buf)25 static inline uint32_t le_to_h_u32(const uint8_t* buf) {
26 return((uint32_t)((uint32_t)buf[0] | (uint32_t)buf[1] << 8 | (uint32_t)buf[2] << 16 | (uint32_t)buf[3] << 24));
27 }
28
_stlink_match_speed_map(const uint32_t * map,unsigned int map_size,uint32_t khz)29 static int _stlink_match_speed_map(const uint32_t *map, unsigned int map_size, uint32_t khz) {
30 unsigned int i;
31 int speed_index = -1;
32 int speed_diff = INT_MAX;
33 int last_valid_speed = -1;
34 bool match = true;
35
36 for (i = 0; i < map_size; i++) {
37 if (!map[i]) { continue; }
38
39 last_valid_speed = i;
40
41 if (khz == map[i]) {
42 speed_index = i;
43 break;
44 } else {
45 int current_diff = khz - map[i];
46 // get abs value for comparison
47 current_diff = (current_diff > 0) ? current_diff : -current_diff;
48
49 if (current_diff < speed_diff) {
50 speed_diff = current_diff;
51 speed_index = i;
52 }
53 }
54 }
55
56 if (speed_index == -1) {
57 // This will only be here if we cannot match the slow speed.
58 // Use the slowest speed we support.
59 speed_index = last_valid_speed;
60 match = false;
61 } else if (i == map_size) {
62 match = false;
63 }
64
65 if (!match) {
66 ILOG("Unable to match requested speed %d kHz, using %d kHz\n", khz, map[speed_index]);
67 }
68
69 return(speed_index);
70 }
71
_stlink_usb_close(stlink_t * sl)72 void _stlink_usb_close(stlink_t* sl) {
73 if (!sl) { return; }
74
75 struct stlink_libusb * const handle = sl->backend_data;
76
77 // maybe we couldn't even get the usb device?
78 if (handle != NULL) {
79 if (handle->usb_handle != NULL) { libusb_close(handle->usb_handle); }
80
81 libusb_exit(handle->libusb_ctx);
82 free(handle);
83 }
84 }
85
send_recv(struct stlink_libusb * handle,int terminate,unsigned char * txbuf,size_t txsize,unsigned char * rxbuf,size_t rxsize)86 ssize_t send_recv(struct stlink_libusb* handle, int terminate,
87 unsigned char* txbuf, size_t txsize, unsigned char* rxbuf, size_t rxsize) {
88 // Note: txbuf and rxbuf can point to the same area
89 int res = 0;
90 int t;
91
92 t = libusb_bulk_transfer(handle->usb_handle, handle->ep_req, txbuf, (int)txsize, &res, 3000);
93
94 if (t) {
95 printf("[!] send_recv send request failed: %s\n", libusb_error_name(t));
96 return(-1);
97 } else if ((size_t)res != txsize) {
98 printf("[!] send_recv send request wrote %u bytes (instead of %u).\n",
99 (unsigned int)res, (unsigned int)txsize);
100 }
101
102 if (rxsize != 0) {
103 t = libusb_bulk_transfer(handle->usb_handle, handle->ep_rep, rxbuf, (int)rxsize, &res, 3000);
104
105 if (t) {
106 printf("[!] send_recv read reply failed: %s\n", libusb_error_name(t));
107 return(-1);
108 }
109 }
110
111 if ((handle->protocoll == 1) && terminate) {
112 // read the SG reply
113 unsigned char sg_buf[13];
114 t = libusb_bulk_transfer(handle->usb_handle, handle->ep_rep, sg_buf, 13, &res, 3000);
115
116 if (t) {
117 printf("[!] send_recv read storage failed: %s\n", libusb_error_name(t));
118 return(-1);
119 }
120
121 // The STLink doesn't seem to evaluate the sequence number.
122 handle->sg_transfer_idx++;
123 }
124
125 return(res);
126 }
127
send_only(struct stlink_libusb * handle,int terminate,unsigned char * txbuf,size_t txsize)128 static inline int send_only(struct stlink_libusb* handle, int terminate,
129 unsigned char* txbuf, size_t txsize) {
130 return((int)send_recv(handle, terminate, txbuf, txsize, NULL, 0));
131 }
132
133
fill_command(stlink_t * sl,enum SCSI_Generic_Direction dir,uint32_t len)134 static int fill_command(stlink_t * sl, enum SCSI_Generic_Direction dir, uint32_t len) {
135 struct stlink_libusb * const slu = sl->backend_data;
136 unsigned char* const cmd = sl->c_buf;
137 int i = 0;
138 memset(cmd, 0, sizeof(sl->c_buf));
139
140 if (slu->protocoll == 1) {
141 cmd[i++] = 'U';
142 cmd[i++] = 'S';
143 cmd[i++] = 'B';
144 cmd[i++] = 'C';
145 write_uint32(&cmd[i], slu->sg_transfer_idx);
146 write_uint32(&cmd[i + 4], len);
147 i += 8;
148 cmd[i++] = (dir == SG_DXFER_FROM_DEV) ? 0x80 : 0;
149 cmd[i++] = 0; // logical unit
150 cmd[i++] = 0xa; // command length
151 }
152
153 return(i);
154 }
155
_stlink_usb_version(stlink_t * sl)156 int _stlink_usb_version(stlink_t *sl) {
157 struct stlink_libusb * const slu = sl->backend_data;
158 unsigned char* const data = sl->q_buf;
159 unsigned char* const cmd = sl->c_buf;
160 ssize_t size;
161 uint32_t rep_len = 6;
162 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
163
164 cmd[i++] = STLINK_GET_VERSION;
165
166 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
167
168 if (size == -1) {
169 printf("[!] send_recv STLINK_GET_VERSION\n");
170 return((int)size);
171 }
172
173 /* STLINK-V3 requires a specific command */
174 if (sl->version.stlink_v == 3) {
175 rep_len = 12;
176 i = fill_command(sl, SG_DXFER_FROM_DEV, 16);
177 cmd[i++] = STLINK_APIV3_GET_VERSION_EX;
178
179 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
180
181 if (size != (ssize_t)rep_len) {
182 printf("[!] send_recv STLINK_APIV3_GET_VERSION_EX\n");
183 return((int)size);
184 }
185 }
186
187 return(0);
188 }
189
_stlink_usb_target_voltage(stlink_t * sl)190 int32_t _stlink_usb_target_voltage(stlink_t *sl) {
191 struct stlink_libusb * const slu = sl->backend_data;
192 unsigned char* const rdata = sl->q_buf;
193 unsigned char* const cmd = sl->c_buf;
194 ssize_t size;
195 uint32_t rep_len = 8;
196 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
197 uint32_t factor, reading;
198 int voltage;
199
200 cmd[i++] = STLINK_GET_TARGET_VOLTAGE;
201
202 size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
203
204 if (size == -1) {
205 printf("[!] send_recv STLINK_GET_TARGET_VOLTAGE\n");
206 return(-1);
207 } else if (size != 8) {
208 printf("[!] wrong length STLINK_GET_TARGET_VOLTAGE\n");
209 return(-1);
210 }
211
212 factor = (rdata[3] << 24) | (rdata[2] << 16) | (rdata[1] << 8) | (rdata[0] << 0);
213 reading = (rdata[7] << 24) | (rdata[6] << 16) | (rdata[5] << 8) | (rdata[4] << 0);
214 voltage = 2400 * reading / factor;
215
216 return(voltage);
217 }
218
_stlink_usb_read_debug32(stlink_t * sl,uint32_t addr,uint32_t * data)219 int _stlink_usb_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data) {
220 struct stlink_libusb * const slu = sl->backend_data;
221 unsigned char* const rdata = sl->q_buf;
222 unsigned char* const cmd = sl->c_buf;
223 ssize_t size;
224 const int rep_len = 8;
225
226 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
227 cmd[i++] = STLINK_DEBUG_COMMAND;
228 cmd[i++] = STLINK_JTAG_READDEBUG_32BIT;
229 write_uint32(&cmd[i], addr);
230 size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
231
232 if (size == -1) {
233 printf("[!] send_recv STLINK_JTAG_READDEBUG_32BIT\n");
234 return((int)size);
235 }
236
237 *data = read_uint32(rdata, 4);
238
239 return(0);
240 }
241
_stlink_usb_write_debug32(stlink_t * sl,uint32_t addr,uint32_t data)242 int _stlink_usb_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
243 struct stlink_libusb * const slu = sl->backend_data;
244 unsigned char* const rdata = sl->q_buf;
245 unsigned char* const cmd = sl->c_buf;
246 ssize_t size;
247 const int rep_len = 2;
248
249 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
250 cmd[i++] = STLINK_DEBUG_COMMAND;
251 cmd[i++] = STLINK_JTAG_WRITEDEBUG_32BIT;
252 write_uint32(&cmd[i], addr);
253 write_uint32(&cmd[i + 4], data);
254 size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
255
256 if (size == -1) {
257 printf("[!] send_recv STLINK_JTAG_WRITEDEBUG_32BIT\n");
258 return((int)size);
259 }
260
261 return(0);
262 }
263
_stlink_usb_get_rw_status(stlink_t * sl)264 int _stlink_usb_get_rw_status(stlink_t *sl) {
265 if (sl->version.jtag_api == STLINK_JTAG_API_V1) { return(0); }
266
267 unsigned char* const rdata = sl->q_buf;
268 struct stlink_libusb * const slu = sl->backend_data;
269 unsigned char* const cmd = sl->c_buf;
270 int i;
271 int16_t ret = 0;
272
273 i = fill_command(sl, SG_DXFER_FROM_DEV, 12);
274 cmd[i++] = STLINK_DEBUG_COMMAND;
275
276 if (sl->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
277 cmd[i++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
278 ret = send_recv(slu, 1, cmd, slu->cmd_len, rdata, 12);
279 } else {
280 cmd[i++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
281 ret = send_recv(slu, 1, cmd, slu->cmd_len, rdata, 2);
282 }
283
284 if (ret < 0) { return(-1); }
285
286 return(0);
287 }
288
_stlink_usb_write_mem32(stlink_t * sl,uint32_t addr,uint16_t len)289 int _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
290 struct stlink_libusb * const slu = sl->backend_data;
291 unsigned char* const data = sl->q_buf;
292 unsigned char* const cmd = sl->c_buf;
293 int i, ret;
294
295 i = fill_command(sl, SG_DXFER_TO_DEV, len);
296 cmd[i++] = STLINK_DEBUG_COMMAND;
297 cmd[i++] = STLINK_DEBUG_WRITEMEM_32BIT;
298 write_uint32(&cmd[i], addr);
299 write_uint16(&cmd[i + 4], len);
300 ret = send_only(slu, 0, cmd, slu->cmd_len);
301
302 if (ret == -1) { return(ret); }
303
304 ret = send_only(slu, 1, data, len);
305
306 if (ret == -1) { return(ret); }
307
308 return(_stlink_usb_get_rw_status(sl));
309 }
310
_stlink_usb_write_mem8(stlink_t * sl,uint32_t addr,uint16_t len)311 int _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
312 struct stlink_libusb * const slu = sl->backend_data;
313 unsigned char* const data = sl->q_buf;
314 unsigned char* const cmd = sl->c_buf;
315 int i, ret;
316
317 i = fill_command(sl, SG_DXFER_TO_DEV, 0);
318 cmd[i++] = STLINK_DEBUG_COMMAND;
319 cmd[i++] = STLINK_DEBUG_WRITEMEM_8BIT;
320 write_uint32(&cmd[i], addr);
321 write_uint16(&cmd[i + 4], len);
322 ret = send_only(slu, 0, cmd, slu->cmd_len);
323
324 if (ret == -1) { return(ret); }
325
326 ret = send_only(slu, 1, data, len);
327
328 if (ret == -1) { return(ret); }
329
330 return(0);
331 }
332
333
_stlink_usb_current_mode(stlink_t * sl)334 int _stlink_usb_current_mode(stlink_t * sl) {
335 struct stlink_libusb * const slu = sl->backend_data;
336 unsigned char* const cmd = sl->c_buf;
337 unsigned char* const data = sl->q_buf;
338 ssize_t size;
339 int rep_len = 2;
340 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
341
342 cmd[i++] = STLINK_GET_CURRENT_MODE;
343 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
344
345 if (size == -1) {
346 printf("[!] send_recv STLINK_GET_CURRENT_MODE\n");
347 return(-1);
348 }
349
350 return(sl->q_buf[0]);
351 }
352
_stlink_usb_core_id(stlink_t * sl)353 int _stlink_usb_core_id(stlink_t * sl) {
354 struct stlink_libusb * const slu = sl->backend_data;
355 unsigned char* const cmd = sl->c_buf;
356 unsigned char* const data = sl->q_buf;
357 ssize_t size;
358 int offset, rep_len = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 12;
359 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
360
361 cmd[i++] = STLINK_DEBUG_COMMAND;
362
363 if (sl->version.jtag_api == STLINK_JTAG_API_V1) {
364 cmd[i++] = STLINK_DEBUG_READCOREID;
365 offset = 0;
366 } else {
367 cmd[i++] = STLINK_DEBUG_APIV2_READ_IDCODES;
368 offset = 4;
369 }
370
371 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
372
373 if (size == -1) {
374 printf("[!] send_recv STLINK_DEBUG_READCOREID\n");
375 return(-1);
376 }
377
378 sl->core_id = read_uint32(data, offset);
379
380 return(0);
381 }
382
_stlink_usb_status_v2(stlink_t * sl)383 int _stlink_usb_status_v2(stlink_t *sl) {
384 int result;
385 uint32_t status = 0;
386
387 result = _stlink_usb_read_debug32(sl, STLINK_REG_DHCSR, &status);
388 DLOG("core status: %08X\n", status);
389
390 if (result != 0) {
391 sl->core_stat = TARGET_UNKNOWN;
392 } else {
393 if (status & STLINK_REG_DHCSR_C_HALT) {
394 sl->core_stat = TARGET_HALTED;
395 } else if (status & STLINK_REG_DHCSR_S_RESET_ST) {
396 sl->core_stat = TARGET_RESET;
397 } else {
398 sl->core_stat = TARGET_RUNNING;
399 }
400 }
401
402 return(result);
403 }
404
_stlink_usb_status(stlink_t * sl)405 int _stlink_usb_status(stlink_t * sl) {
406 if (sl->version.jtag_api != STLINK_JTAG_API_V1) { return(_stlink_usb_status_v2(sl)); }
407
408 struct stlink_libusb * const slu = sl->backend_data;
409 unsigned char* const data = sl->q_buf;
410 unsigned char* const cmd = sl->c_buf;
411 ssize_t size;
412 int rep_len = 2;
413 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
414
415 cmd[i++] = STLINK_DEBUG_COMMAND;
416 cmd[i++] = STLINK_DEBUG_GETSTATUS;
417 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
418
419 if (size == -1) {
420 printf("[!] send_recv STLINK_DEBUG_GETSTATUS\n");
421 return((int)size);
422 }
423
424 sl->q_len = (int)size;
425
426 if (sl->q_len > 1) {
427 if (sl->q_buf[0] == STLINK_CORE_RUNNING) {
428 sl->core_stat = TARGET_RUNNING;
429 } else if (sl->q_buf[0] == STLINK_CORE_HALTED) {
430 sl->core_stat = TARGET_HALTED;
431 } else {
432 sl->core_stat = TARGET_UNKNOWN;
433 }
434 } else {
435 sl->core_stat = TARGET_UNKNOWN;
436 }
437
438 return(0);
439 }
440
_stlink_usb_force_debug(stlink_t * sl)441 int _stlink_usb_force_debug(stlink_t *sl) {
442 struct stlink_libusb *slu = sl->backend_data;
443
444 int res;
445
446 if (sl->version.jtag_api != STLINK_JTAG_API_V1) {
447 res = _stlink_usb_write_debug32(sl, STLINK_REG_DHCSR, STLINK_REG_DHCSR_DBGKEY | STLINK_REG_DHCSR_C_HALT | STLINK_REG_DHCSR_C_DEBUGEN);
448 return(res);
449 }
450
451 unsigned char* const data = sl->q_buf;
452 unsigned char* const cmd = sl->c_buf;
453 ssize_t size;
454 int rep_len = 2;
455 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
456
457 cmd[i++] = STLINK_DEBUG_COMMAND;
458 cmd[i++] = STLINK_DEBUG_FORCEDEBUG;
459 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
460
461 if (size == -1) {
462 printf("[!] send_recv STLINK_DEBUG_FORCEDEBUG\n");
463 return((int)size);
464 }
465
466 return(0);
467 }
468
_stlink_usb_enter_swd_mode(stlink_t * sl)469 int _stlink_usb_enter_swd_mode(stlink_t * sl) {
470 struct stlink_libusb * const slu = sl->backend_data;
471 unsigned char* const cmd = sl->c_buf;
472 ssize_t size;
473 unsigned char* const data = sl->q_buf;
474 const uint32_t rep_len = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 0 : 2;
475 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
476
477 cmd[i++] = STLINK_DEBUG_COMMAND;
478 // select correct API-Version for entering SWD mode: V1 API (0x20) or V2 API (0x30).
479 cmd[i++] = sl->version.jtag_api == STLINK_JTAG_API_V1 ? STLINK_DEBUG_APIV1_ENTER : STLINK_DEBUG_APIV2_ENTER;
480 cmd[i++] = STLINK_DEBUG_ENTER_SWD;
481 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
482
483 if (size == -1) {
484 printf("[!] send_recv STLINK_DEBUG_ENTER\n");
485 return((int)size);
486 }
487
488 return(0);
489 }
490
_stlink_usb_exit_dfu_mode(stlink_t * sl)491 int _stlink_usb_exit_dfu_mode(stlink_t* sl) {
492 struct stlink_libusb * const slu = sl->backend_data;
493 unsigned char* const cmd = sl->c_buf;
494 ssize_t size;
495 int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
496
497 cmd[i++] = STLINK_DFU_COMMAND;
498 cmd[i++] = STLINK_DFU_EXIT;
499 size = send_only(slu, 1, cmd, slu->cmd_len);
500
501 if (size == -1) {
502 printf("[!] send_recv STLINK_DFU_EXIT\n");
503 return((int)size);
504 }
505
506 return(0);
507 }
508
509
_stlink_usb_reset(stlink_t * sl)510 int _stlink_usb_reset(stlink_t * sl) {
511 struct stlink_libusb * const slu = sl->backend_data;
512 unsigned char* const data = sl->q_buf;
513 unsigned char* const cmd = sl->c_buf;
514 ssize_t size;
515 int i, rep_len = 2;
516
517 // send reset command
518 i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
519 cmd[i++] = STLINK_DEBUG_COMMAND;
520
521 if (sl->version.jtag_api == STLINK_JTAG_API_V1) {
522 cmd[i++] = STLINK_DEBUG_APIV1_RESETSYS;
523 } else {
524 cmd[i++] = STLINK_DEBUG_APIV2_RESETSYS;
525 }
526
527 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
528
529 if (size == -1) {
530 printf("[!] send_recv STLINK_DEBUG_RESETSYS\n");
531 return((int)size);
532 }
533
534 return(0);
535 }
536
_stlink_usb_jtag_reset(stlink_t * sl,int value)537 int _stlink_usb_jtag_reset(stlink_t * sl, int value) {
538 struct stlink_libusb * const slu = sl->backend_data;
539 unsigned char* const data = sl->q_buf;
540 unsigned char* const cmd = sl->c_buf;
541 ssize_t size;
542 int rep_len = 2;
543 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
544
545 cmd[i++] = STLINK_DEBUG_COMMAND;
546 cmd[i++] = STLINK_JTAG_DRIVE_NRST;
547 cmd[i++] = value;
548 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
549
550 if (size == -1) {
551 printf("[!] send_recv STLINK_JTAG_DRIVE_NRST\n");
552 return((int)size);
553 }
554
555 return(0);
556 }
557
558
_stlink_usb_step(stlink_t * sl)559 int _stlink_usb_step(stlink_t* sl) {
560 struct stlink_libusb * const slu = sl->backend_data;
561
562 if (sl->version.jtag_api != STLINK_JTAG_API_V1) {
563 // emulates the JTAG v1 API by using DHCSR
564 _stlink_usb_write_debug32(sl, STLINK_REG_DHCSR, STLINK_REG_DHCSR_DBGKEY | STLINK_REG_DHCSR_C_HALT |
565 STLINK_REG_DHCSR_C_MASKINTS | STLINK_REG_DHCSR_C_DEBUGEN);
566 _stlink_usb_write_debug32(sl, STLINK_REG_DHCSR, STLINK_REG_DHCSR_DBGKEY | STLINK_REG_DHCSR_C_STEP |
567 STLINK_REG_DHCSR_C_MASKINTS | STLINK_REG_DHCSR_C_DEBUGEN);
568 return _stlink_usb_write_debug32(sl, STLINK_REG_DHCSR, STLINK_REG_DHCSR_DBGKEY | STLINK_REG_DHCSR_C_HALT |
569 STLINK_REG_DHCSR_C_DEBUGEN);
570 }
571
572 unsigned char* const data = sl->q_buf;
573 unsigned char* const cmd = sl->c_buf;
574 ssize_t size;
575 int rep_len = 2;
576 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
577
578 cmd[i++] = STLINK_DEBUG_COMMAND;
579 cmd[i++] = STLINK_DEBUG_STEPCORE;
580 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
581
582 if (size == -1) {
583 printf("[!] send_recv STLINK_DEBUG_STEPCORE\n");
584 return((int)size);
585 }
586
587 return(0);
588 }
589
590 /**
591 * This seems to do a good job of restarting things from the beginning?
592 * @param sl
593 * @param type
594 */
_stlink_usb_run(stlink_t * sl,enum run_type type)595 int _stlink_usb_run(stlink_t* sl, enum run_type type) {
596 struct stlink_libusb * const slu = sl->backend_data;
597
598 int res;
599
600 if (sl->version.jtag_api != STLINK_JTAG_API_V1) {
601 res = _stlink_usb_write_debug32(sl, STLINK_REG_DHCSR,
602 STLINK_REG_DHCSR_DBGKEY | STLINK_REG_DHCSR_C_DEBUGEN |
603 ((type==RUN_FLASH_LOADER)?STLINK_REG_DHCSR_C_MASKINTS:0));
604 return(res);
605 }
606
607
608 unsigned char* const data = sl->q_buf;
609 unsigned char* const cmd = sl->c_buf;
610 ssize_t size;
611 int rep_len = 2;
612 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
613
614 cmd[i++] = STLINK_DEBUG_COMMAND;
615 cmd[i++] = STLINK_DEBUG_RUNCORE;
616 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
617
618 if (size == -1) {
619 printf("[!] send_recv STLINK_DEBUG_RUNCORE\n");
620 return((int)size);
621 }
622
623 return(0);
624 }
625
_stlink_usb_set_swdclk(stlink_t * sl,int clk_freq)626 int _stlink_usb_set_swdclk(stlink_t* sl, int clk_freq) {
627 struct stlink_libusb * const slu = sl->backend_data;
628 unsigned char* const data = sl->q_buf;
629 unsigned char* const cmd = sl->c_buf;
630 ssize_t size;
631 int rep_len = 2;
632 int i;
633
634 // clock speed only supported by stlink/v2 and for firmware >= 22
635 if (sl->version.stlink_v == 2 && sl->version.jtag_v >= 22) {
636 uint16_t clk_divisor;
637 if (clk_freq) {
638 const uint32_t map[] = {5, 15, 25, 50, 100, 125, 240, 480, 950, 1200, 1800, 4000};
639 int speed_index = _stlink_match_speed_map(map, STLINK_ARRAY_SIZE(map), clk_freq);
640 switch (map[speed_index]) {
641 case 5: clk_divisor = STLINK_SWDCLK_5KHZ_DIVISOR; break;
642 case 15: clk_divisor = STLINK_SWDCLK_15KHZ_DIVISOR; break;
643 case 25: clk_divisor = STLINK_SWDCLK_25KHZ_DIVISOR; break;
644 case 50: clk_divisor = STLINK_SWDCLK_50KHZ_DIVISOR; break;
645 case 100: clk_divisor = STLINK_SWDCLK_100KHZ_DIVISOR; break;
646 case 125: clk_divisor = STLINK_SWDCLK_125KHZ_DIVISOR; break;
647 case 240: clk_divisor = STLINK_SWDCLK_240KHZ_DIVISOR; break;
648 case 480: clk_divisor = STLINK_SWDCLK_480KHZ_DIVISOR; break;
649 case 950: clk_divisor = STLINK_SWDCLK_950KHZ_DIVISOR; break;
650 case 1200: clk_divisor = STLINK_SWDCLK_1P2MHZ_DIVISOR; break;
651 default:
652 case 1800: clk_divisor = STLINK_SWDCLK_1P8MHZ_DIVISOR; break;
653 case 4000: clk_divisor = STLINK_SWDCLK_4MHZ_DIVISOR; break;
654 }
655 } else
656 clk_divisor = STLINK_SWDCLK_1P8MHZ_DIVISOR;
657
658 i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
659
660 cmd[i++] = STLINK_DEBUG_COMMAND;
661 cmd[i++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
662 cmd[i++] = clk_divisor & 0xFF;
663 cmd[i++] = (clk_divisor >> 8) & 0xFF;
664 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
665
666 if (size == -1) {
667 printf("[!] send_recv STLINK_DEBUG_APIV2_SWD_SET_FREQ\n");
668 return((int)size);
669 }
670
671 return(0);
672 } else if (sl->version.stlink_v == 3) {
673 int speed_index;
674 uint32_t map[STLINK_V3_MAX_FREQ_NB];
675 i = fill_command(sl, SG_DXFER_FROM_DEV, 16);
676
677 cmd[i++] = STLINK_DEBUG_COMMAND;
678 cmd[i++] = STLINK_APIV3_GET_COM_FREQ;
679 cmd[i++] = 0; // SWD mode
680 size = send_recv(slu, 1, cmd, slu->cmd_len, data, 52);
681
682 if (size == -1) {
683 printf("[!] send_recv STLINK_APIV3_GET_COM_FREQ\n");
684 return((int)size);
685 }
686
687 int speeds_size = data[8];
688 if (speeds_size > STLINK_V3_MAX_FREQ_NB) {
689 speeds_size = STLINK_V3_MAX_FREQ_NB;
690 }
691
692 for (i = 0; i < speeds_size; i++) map[i] = le_to_h_u32(&data[12 + 4 * i]);
693
694 // Set to zero all the next entries
695 for (i = speeds_size; i < STLINK_V3_MAX_FREQ_NB; i++) map[i] = 0;
696
697 if (!clk_freq) clk_freq = 1800; // set default frequency
698 speed_index = _stlink_match_speed_map(map, STLINK_ARRAY_SIZE(map), clk_freq);
699
700 i = fill_command(sl, SG_DXFER_FROM_DEV, 16);
701
702 cmd[i++] = STLINK_DEBUG_COMMAND;
703 cmd[i++] = STLINK_APIV3_SET_COM_FREQ;
704 cmd[i++] = 0; // SWD mode
705 cmd[i++] = 0;
706 cmd[i++] = (uint8_t)((map[speed_index] >> 0) & 0xFF);
707 cmd[i++] = (uint8_t)((map[speed_index] >> 8) & 0xFF);
708 cmd[i++] = (uint8_t)((map[speed_index] >> 16) & 0xFF);
709 cmd[i++] = (uint8_t)((map[speed_index] >> 24) & 0xFF);
710
711 size = send_recv(slu, 1, cmd, slu->cmd_len, data, 8);
712
713 if (size == -1) {
714 printf("[!] send_recv STLINK_APIV3_SET_COM_FREQ\n");
715 return((int)size);
716 }
717
718 return(0);
719 } else if (clk_freq) {
720 WLOG("ST-Link firmware does not support frequency setup\n");
721 }
722
723 return(-1);
724 }
725
_stlink_usb_exit_debug_mode(stlink_t * sl)726 int _stlink_usb_exit_debug_mode(stlink_t *sl) {
727 struct stlink_libusb * const slu = sl->backend_data;
728 unsigned char* const cmd = sl->c_buf;
729 ssize_t size;
730 int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
731
732 cmd[i++] = STLINK_DEBUG_COMMAND;
733 cmd[i++] = STLINK_DEBUG_EXIT;
734
735 size = send_only(slu, 1, cmd, slu->cmd_len);
736
737 if (size == -1) {
738 printf("[!] send_only STLINK_DEBUG_EXIT\n");
739 return((int)size);
740 }
741
742 return(0);
743 }
744
_stlink_usb_read_mem32(stlink_t * sl,uint32_t addr,uint16_t len)745 int _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
746 struct stlink_libusb * const slu = sl->backend_data;
747 unsigned char* const data = sl->q_buf;
748 unsigned char* const cmd = sl->c_buf;
749 ssize_t size;
750 int i = fill_command(sl, SG_DXFER_FROM_DEV, len);
751
752 cmd[i++] = STLINK_DEBUG_COMMAND;
753 cmd[i++] = STLINK_DEBUG_READMEM_32BIT;
754 write_uint32(&cmd[i], addr);
755 write_uint16(&cmd[i + 4], len);
756 size = send_recv(slu, 1, cmd, slu->cmd_len, data, len);
757
758 if (size == -1) {
759 printf("[!] send_recv STLINK_DEBUG_READMEM_32BIT\n");
760 return((int)size);
761 }
762
763 sl->q_len = (int)size;
764 stlink_print_data(sl);
765
766 return(0);
767 }
768
_stlink_usb_read_all_regs(stlink_t * sl,struct stlink_reg * regp)769 int _stlink_usb_read_all_regs(stlink_t *sl, struct stlink_reg *regp) {
770 struct stlink_libusb * const slu = sl->backend_data;
771 unsigned char* const cmd = sl->c_buf;
772 unsigned char* const data = sl->q_buf;
773 ssize_t size;
774 uint32_t rep_len = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 84 : 88;
775 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
776
777 cmd[i++] = STLINK_DEBUG_COMMAND;
778
779 if (sl->version.jtag_api == STLINK_JTAG_API_V1) {
780 cmd[i++] = STLINK_DEBUG_APIV1_READALLREGS;
781 } else {
782 cmd[i++] = STLINK_DEBUG_APIV2_READALLREGS;
783 }
784
785 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
786
787 if (size == -1) {
788 printf("[!] send_recv STLINK_DEBUG_READALLREGS\n");
789 return((int)size);
790 }
791
792 /* V1: regs data from offset 0 */
793 /* V2: status at offset 0, regs data from offset 4 */
794 int reg_offset = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 0 : 4;
795 sl->q_len = (int)size;
796 stlink_print_data(sl);
797
798 for (i = 0; i < 16; i++) regp->r[i] = read_uint32(sl->q_buf, reg_offset + i * 4);
799
800 regp->xpsr = read_uint32(sl->q_buf, reg_offset + 64);
801 regp->main_sp = read_uint32(sl->q_buf, reg_offset + 68);
802 regp->process_sp = read_uint32(sl->q_buf, reg_offset + 72);
803 regp->rw = read_uint32(sl->q_buf, reg_offset + 76);
804 regp->rw2 = read_uint32(sl->q_buf, reg_offset + 80);
805
806 if (sl->verbose < 2) { return(0); }
807
808 DLOG("xpsr = 0x%08x\n", regp->xpsr);
809 DLOG("main_sp = 0x%08x\n", regp->main_sp);
810 DLOG("process_sp = 0x%08x\n", regp->process_sp);
811 DLOG("rw = 0x%08x\n", regp->rw);
812 DLOG("rw2 = 0x%08x\n", regp->rw2);
813
814 return(0);
815 }
816
_stlink_usb_read_reg(stlink_t * sl,int r_idx,struct stlink_reg * regp)817 int _stlink_usb_read_reg(stlink_t *sl, int r_idx, struct stlink_reg *regp) {
818 struct stlink_libusb * const slu = sl->backend_data;
819 unsigned char* const data = sl->q_buf;
820 unsigned char* const cmd = sl->c_buf;
821 ssize_t size;
822 uint32_t r;
823 uint32_t rep_len = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8;
824 int reg_offset = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 0 : 4;
825 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
826
827 cmd[i++] = STLINK_DEBUG_COMMAND;
828
829 if (sl->version.jtag_api == STLINK_JTAG_API_V1) {
830 cmd[i++] = STLINK_DEBUG_APIV1_READREG;
831 } else {
832 cmd[i++] = STLINK_DEBUG_APIV2_READREG;
833 }
834
835 cmd[i++] = (uint8_t)r_idx;
836 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
837
838 if (size == -1) {
839 printf("[!] send_recv STLINK_DEBUG_READREG\n");
840 return((int)size);
841 }
842
843 sl->q_len = (int)size;
844 stlink_print_data(sl);
845 r = read_uint32(sl->q_buf, reg_offset);
846 DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
847
848 switch (r_idx) {
849 case 16:
850 regp->xpsr = r;
851 break;
852 case 17:
853 regp->main_sp = r;
854 break;
855 case 18:
856 regp->process_sp = r;
857 break;
858 case 19:
859 regp->rw = r; // XXX ?(primask, basemask etc.)
860 break;
861 case 20:
862 regp->rw2 = r; // XXX ?(primask, basemask etc.)
863 break;
864 default:
865 regp->r[r_idx] = r;
866 }
867
868 return(0);
869 }
870
871 /* See section C1.6 of the ARMv7-M Architecture Reference Manual */
_stlink_usb_read_unsupported_reg(stlink_t * sl,int r_idx,struct stlink_reg * regp)872 int _stlink_usb_read_unsupported_reg(stlink_t *sl, int r_idx, struct stlink_reg *regp) {
873 uint32_t r;
874 int ret;
875
876 sl->q_buf[0] = (unsigned char)r_idx;
877
878 for (int i = 1; i < 4; i++) sl->q_buf[i] = 0;
879
880 ret = _stlink_usb_write_mem32(sl, STLINK_REG_DCRSR, 4);
881
882 if (ret == -1) { return(ret); }
883
884 ret = _stlink_usb_read_mem32(sl, STLINK_REG_DCRDR, 4);
885
886 if (ret == -1) { return(ret); }
887
888 r = read_uint32(sl->q_buf, 0);
889 DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
890
891 switch (r_idx) {
892 case 0x14:
893 regp->primask = (uint8_t)(r & 0xFF);
894 regp->basepri = (uint8_t)((r >> 8) & 0xFF);
895 regp->faultmask = (uint8_t)((r >> 16) & 0xFF);
896 regp->control = (uint8_t)((r >> 24) & 0xFF);
897 break;
898 case 0x21:
899 regp->fpscr = r;
900 break;
901 default:
902 regp->s[r_idx - 0x40] = r;
903 break;
904 }
905
906 return(0);
907 }
908
_stlink_usb_read_all_unsupported_regs(stlink_t * sl,struct stlink_reg * regp)909 int _stlink_usb_read_all_unsupported_regs(stlink_t *sl, struct stlink_reg *regp) {
910 int ret;
911
912 ret = _stlink_usb_read_unsupported_reg(sl, 0x14, regp);
913
914 if (ret == -1) { return(ret); }
915
916 ret = _stlink_usb_read_unsupported_reg(sl, 0x21, regp);
917
918 if (ret == -1) { return(ret); }
919
920 for (int i = 0; i < 32; i++) {
921 ret = _stlink_usb_read_unsupported_reg(sl, 0x40 + i, regp);
922
923 if (ret == -1) { return(ret); }
924 }
925
926 return(0);
927 }
928
929 /* See section C1.6 of the ARMv7-M Architecture Reference Manual */
_stlink_usb_write_unsupported_reg(stlink_t * sl,uint32_t val,int r_idx,struct stlink_reg * regp)930 int _stlink_usb_write_unsupported_reg(stlink_t *sl, uint32_t val, int r_idx, struct stlink_reg *regp) {
931 int ret;
932
933 if (r_idx >= 0x1C && r_idx <= 0x1F) { // primask, basepri, faultmask, or control
934 /* These are held in the same register */
935 ret = _stlink_usb_read_unsupported_reg(sl, 0x14, regp);
936
937 if (ret == -1) { return(ret); }
938
939 val = (uint8_t)(val >> 24);
940
941 switch (r_idx) {
942 case 0x1C: /* control */
943 val = (((uint32_t)val) << 24) |
944 (((uint32_t)regp->faultmask) << 16) |
945 (((uint32_t)regp->basepri) << 8) |
946 ((uint32_t)regp->primask);
947 break;
948 case 0x1D: /* faultmask */
949 val = (((uint32_t)regp->control) << 24) |
950 (((uint32_t)val) << 16) |
951 (((uint32_t)regp->basepri) << 8) |
952 ((uint32_t)regp->primask);
953 break;
954 case 0x1E: /* basepri */
955 val = (((uint32_t)regp->control) << 24) |
956 (((uint32_t)regp->faultmask) << 16) |
957 (((uint32_t)val) << 8) |
958 ((uint32_t)regp->primask);
959 break;
960 case 0x1F: /* primask */
961 val = (((uint32_t)regp->control) << 24) |
962 (((uint32_t)regp->faultmask) << 16) |
963 (((uint32_t)regp->basepri) << 8) |
964 ((uint32_t)val);
965 break;
966 }
967
968 r_idx = 0x14;
969 }
970
971 write_uint32(sl->q_buf, val);
972
973 ret = _stlink_usb_write_mem32(sl, STLINK_REG_DCRDR, 4);
974
975 if (ret == -1) { return(ret); }
976
977 sl->q_buf[0] = (unsigned char)r_idx;
978 sl->q_buf[1] = 0;
979 sl->q_buf[2] = 0x01;
980 sl->q_buf[3] = 0;
981
982 return(_stlink_usb_write_mem32(sl, STLINK_REG_DCRSR, 4));
983 }
984
_stlink_usb_write_reg(stlink_t * sl,uint32_t reg,int idx)985 int _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
986 struct stlink_libusb * const slu = sl->backend_data;
987 unsigned char* const data = sl->q_buf;
988 unsigned char* const cmd = sl->c_buf;
989 ssize_t size;
990 uint32_t rep_len = 2;
991 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
992
993 cmd[i++] = STLINK_DEBUG_COMMAND;
994
995 if (sl->version.jtag_api == STLINK_JTAG_API_V1) {
996 cmd[i++] = STLINK_DEBUG_APIV1_WRITEREG;
997 } else {
998 cmd[i++] = STLINK_DEBUG_APIV2_WRITEREG;
999 }
1000
1001 cmd[i++] = idx;
1002 write_uint32(&cmd[i], reg);
1003 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
1004
1005 if (size == -1) {
1006 printf("[!] send_recv STLINK_DEBUG_WRITEREG\n");
1007 return((int)size);
1008 }
1009
1010 sl->q_len = (int)size;
1011 stlink_print_data(sl);
1012
1013 return(0);
1014 }
1015
_stlink_usb_enable_trace(stlink_t * sl,uint32_t frequency)1016 int _stlink_usb_enable_trace(stlink_t* sl, uint32_t frequency) {
1017 struct stlink_libusb * const slu = sl->backend_data;
1018 unsigned char* const data = sl->q_buf;
1019 unsigned char* const cmd = sl->c_buf;
1020 ssize_t size;
1021 uint32_t rep_len = 2;
1022
1023 int i = fill_command(sl, SG_DXFER_TO_DEV, rep_len);
1024 cmd[i++] = STLINK_DEBUG_COMMAND;
1025 cmd[i++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
1026 write_uint16(&cmd[i + 0], 2 * STLINK_TRACE_BUF_LEN);
1027 write_uint32(&cmd[i + 2], frequency);
1028
1029 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
1030
1031 if (size == -1) {
1032 printf("[!] send_only STLINK_DEBUG_APIV2_START_TRACE_RX\n");
1033 return((int)size);
1034 }
1035
1036 sl->q_len = (int)size;
1037 stlink_print_data(sl);
1038
1039 return(0);
1040 }
1041
_stlink_usb_disable_trace(stlink_t * sl)1042 int _stlink_usb_disable_trace(stlink_t* sl) {
1043 struct stlink_libusb * const slu = sl->backend_data;
1044 unsigned char* const data = sl->q_buf;
1045 unsigned char* const cmd = sl->c_buf;
1046 ssize_t size;
1047 uint32_t rep_len = 2;
1048
1049 int i = fill_command(sl, SG_DXFER_TO_DEV, rep_len);
1050 cmd[i++] = STLINK_DEBUG_COMMAND;
1051 cmd[i++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
1052
1053 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
1054
1055 if (size == -1) {
1056 printf("[!] send_only STLINK_DEBUG_APIV2_STOP_TRACE_RX\n");
1057 return((int)size);
1058 }
1059
1060 sl->q_len = (int)size;
1061 stlink_print_data(sl);
1062
1063 return(0);
1064 }
1065
_stlink_usb_read_trace(stlink_t * sl,uint8_t * buf,size_t size)1066 int _stlink_usb_read_trace(stlink_t* sl, uint8_t* buf, size_t size) {
1067 struct stlink_libusb * const slu = sl->backend_data;
1068 unsigned char* const data = sl->q_buf;
1069 unsigned char* const cmd = sl->c_buf;
1070 uint32_t rep_len = 2;
1071 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
1072
1073 cmd[i++] = STLINK_DEBUG_COMMAND;
1074 cmd[i++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
1075 ssize_t send_size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
1076
1077 if (send_size == -1) {
1078 printf("[!] send_recv STLINK_DEBUG_APIV2_GET_TRACE_NB\n");
1079 return((int)send_size);
1080 }
1081 if (send_size != 2) {
1082 printf("[!] send_recv STLINK_DEBUG_APIV2_GET_TRACE_NB %d\n", (int)send_size);
1083 return -1;
1084 }
1085
1086 uint16_t trace_count = read_uint16(sl->q_buf, 0);
1087
1088 if (trace_count > size) {
1089 ELOG("read_trace insufficient buffer length\n");
1090 return -1;
1091 }
1092
1093 if (trace_count != 0) {
1094 int res = 0;
1095 int t = libusb_bulk_transfer(slu->usb_handle, slu->ep_trace, buf, trace_count, &res, 3000);
1096
1097 if (t || res != (int)trace_count) {
1098 ELOG("read_trace read error %d\n", t);
1099 return(-1);
1100 }
1101 }
1102
1103 return trace_count;
1104 }
1105
1106 static stlink_backend_t _stlink_usb_backend = {
1107 _stlink_usb_close,
1108 _stlink_usb_exit_debug_mode,
1109 _stlink_usb_enter_swd_mode,
1110 NULL, // don't enter_jtag_mode here...
1111 _stlink_usb_exit_dfu_mode,
1112 _stlink_usb_core_id,
1113 _stlink_usb_reset,
1114 _stlink_usb_jtag_reset,
1115 _stlink_usb_run,
1116 _stlink_usb_status,
1117 _stlink_usb_version,
1118 _stlink_usb_read_debug32,
1119 _stlink_usb_read_mem32,
1120 _stlink_usb_write_debug32,
1121 _stlink_usb_write_mem32,
1122 _stlink_usb_write_mem8,
1123 _stlink_usb_read_all_regs,
1124 _stlink_usb_read_reg,
1125 _stlink_usb_read_all_unsupported_regs,
1126 _stlink_usb_read_unsupported_reg,
1127 _stlink_usb_write_unsupported_reg,
1128 _stlink_usb_write_reg,
1129 _stlink_usb_step,
1130 _stlink_usb_current_mode,
1131 _stlink_usb_force_debug,
1132 _stlink_usb_target_voltage,
1133 _stlink_usb_set_swdclk,
1134 _stlink_usb_enable_trace,
1135 _stlink_usb_disable_trace,
1136 _stlink_usb_read_trace
1137 };
1138
1139 /* return the length of serial or (0) in case of errors */
stlink_serial(struct libusb_device_handle * handle,struct libusb_device_descriptor * desc,char * serial)1140 size_t stlink_serial(struct libusb_device_handle *handle, struct libusb_device_descriptor *desc, char *serial) {
1141 unsigned char desc_serial[(STLINK_SERIAL_LENGTH) * 2];
1142
1143 /* truncate the string in the serial buffer */
1144 serial[0] = '\0';
1145
1146 /* get the LANGID from String Descriptor Zero */
1147 int ret = libusb_get_string_descriptor(handle, 0, 0, desc_serial, sizeof(desc_serial));
1148 if (ret < 4) return 0;
1149
1150 uint32_t langid = desc_serial[2] | (desc_serial[3] << 8);
1151
1152 /* get the serial */
1153 ret = libusb_get_string_descriptor(handle, desc->iSerialNumber, langid, desc_serial,
1154 sizeof(desc_serial));
1155 if (ret < 0) return 0; // could not read serial
1156
1157 unsigned char len = desc_serial[0];
1158
1159 if (len == ((STLINK_SERIAL_LENGTH + 1) * 2)) { /* len == 50 */
1160 /* good ST-Link adapter */
1161 ret = libusb_get_string_descriptor_ascii(
1162 handle, desc->iSerialNumber, (unsigned char *)serial, STLINK_SERIAL_BUFFER_SIZE);
1163 if (ret < 0) return 0;
1164 } else if (len == ((STLINK_SERIAL_LENGTH / 2 + 1) * 2)) { /* len == 26 */
1165 /* fix-up the buggy serial */
1166 for (unsigned int i = 0; i < STLINK_SERIAL_LENGTH; i += 2)
1167 sprintf(serial + i, "%02X", desc_serial[i + 2]);
1168 serial[STLINK_SERIAL_LENGTH] = '\0';
1169 } else {
1170 return 0;
1171 }
1172
1173 return strlen(serial);
1174 }
1175
stlink_open_usb(enum ugly_loglevel verbose,enum connect_type connect,char serial[STLINK_SERIAL_BUFFER_SIZE],int freq)1176 stlink_t *stlink_open_usb(enum ugly_loglevel verbose, enum connect_type connect, char serial[STLINK_SERIAL_BUFFER_SIZE], int freq) {
1177 stlink_t* sl = NULL;
1178 struct stlink_libusb* slu = NULL;
1179 int ret = -1;
1180 int config;
1181
1182 sl = calloc(1, sizeof(stlink_t));
1183 slu = calloc(1, sizeof(struct stlink_libusb));
1184
1185 if (sl == NULL) { goto on_malloc_error; }
1186
1187 if (slu == NULL) { goto on_malloc_error; }
1188
1189 ugly_init(verbose);
1190 sl->backend = &_stlink_usb_backend;
1191 sl->backend_data = slu;
1192
1193 sl->core_stat = TARGET_UNKNOWN;
1194
1195 if (libusb_init(&(slu->libusb_ctx))) {
1196 WLOG("failed to init libusb context, wrong version of libraries?\n");
1197 goto on_error;
1198 }
1199
1200 #if LIBUSB_API_VERSION < 0x01000106
1201 libusb_set_debug(slu->libusb_ctx, ugly_libusb_log_level(verbose));
1202 #else
1203 libusb_set_option(slu->libusb_ctx, LIBUSB_OPTION_LOG_LEVEL, ugly_libusb_log_level(verbose));
1204 #endif
1205
1206 libusb_device **list;
1207 // TODO: We should use ssize_t and use it as a counter if > 0.
1208 // As per libusb API: ssize_t libusb_get_device_list (libusb_context *ctx, libusb_device ***list)
1209 int cnt = (int)libusb_get_device_list(slu->libusb_ctx, &list);
1210 struct libusb_device_descriptor desc;
1211 int devBus = 0;
1212 int devAddr = 0;
1213
1214 // TODO: Reading a environment variable in a usb open function is not very nice, this should
1215 // be refactored and moved into the CLI tools, and instead of giving USB_BUS:USB_ADDR a real
1216 // stlink serial string should be passed to this function. Probably people are using this
1217 // but this is very odd because as programmer can change to multiple busses and it is better
1218 // to detect them based on serial.
1219 char *device = getenv("STLINK_DEVICE");
1220
1221 if (device) {
1222 char *c = strchr(device, ':');
1223
1224 if (c == NULL) {
1225 WLOG("STLINK_DEVICE must be <USB_BUS>:<USB_ADDR> format\n");
1226 goto on_error;
1227 }
1228
1229 devBus = atoi(device);
1230 *c++ = 0;
1231 devAddr = atoi(c);
1232 ILOG("bus %03d dev %03d\n", devBus, devAddr);
1233 }
1234
1235 while (cnt--) {
1236 struct libusb_device_handle *handle;
1237
1238 libusb_get_device_descriptor(list[cnt], &desc);
1239
1240 if (desc.idVendor != STLINK_USB_VID_ST) { continue; }
1241
1242 if (devBus && devAddr) {
1243 if ((libusb_get_bus_number(list[cnt]) != devBus) ||
1244 (libusb_get_device_address(list[cnt]) != devAddr)) {
1245 continue;
1246 }
1247 }
1248
1249 ret = libusb_open(list[cnt], &handle);
1250
1251 if (ret) { continue; } // could not open device
1252
1253 size_t serial_len = stlink_serial(handle, &desc, sl->serial);
1254
1255 libusb_close(handle);
1256
1257 if (serial_len != STLINK_SERIAL_LENGTH) { continue; } // could not read the serial
1258
1259 // if no serial provided, or if serial match device, fixup version and protocol
1260 if (((serial == NULL) || (*serial == 0)) || (memcmp(serial, &sl->serial, STLINK_SERIAL_LENGTH) == 0)) {
1261 if (STLINK_V1_USB_PID(desc.idProduct)) {
1262 slu->protocoll = 1;
1263 sl->version.stlink_v = 1;
1264 } else if (STLINK_V2_USB_PID(desc.idProduct) || STLINK_V2_1_USB_PID(desc.idProduct)) {
1265 sl->version.stlink_v = 2;
1266 } else if (STLINK_V3_USB_PID(desc.idProduct)) {
1267 sl->version.stlink_v = 3;
1268 }
1269
1270 break;
1271 }
1272 }
1273
1274 if (cnt < 0) {
1275 WLOG ("Couldn't find %s ST-Link devices\n", (devBus && devAddr) ? "matched" : "any");
1276 goto on_error;
1277 } else {
1278 ret = libusb_open(list[cnt], &slu->usb_handle);
1279
1280 if (ret != 0) {
1281 WLOG("Error %d (%s) opening ST-Link v%d device %03d:%03d\n", ret,
1282 strerror(errno),
1283 sl->version.stlink_v,
1284 libusb_get_bus_number(list[cnt]),
1285 libusb_get_device_address(list[cnt]));
1286 libusb_free_device_list(list, 1);
1287 goto on_error;
1288 }
1289 }
1290
1291 libusb_free_device_list(list, 1);
1292
1293 if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) {
1294 ret = libusb_detach_kernel_driver(slu->usb_handle, 0);
1295
1296 if (ret < 0) {
1297 WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-ret));
1298 goto on_libusb_error;
1299 }
1300 }
1301
1302 if (libusb_get_configuration(slu->usb_handle, &config)) {
1303 // this may fail for a previous configured device
1304 WLOG("libusb_get_configuration()\n");
1305 goto on_libusb_error;
1306 }
1307
1308 if (config != 1) {
1309 printf("setting new configuration (%d -> 1)\n", config);
1310
1311 if (libusb_set_configuration(slu->usb_handle, 1)) {
1312 // this may fail for a previous configured device
1313 WLOG("libusb_set_configuration() failed\n");
1314 goto on_libusb_error;
1315 }
1316 }
1317
1318 if (libusb_claim_interface(slu->usb_handle, 0)) {
1319 WLOG("Stlink usb device found, but unable to claim (probably already in use?)\n");
1320 goto on_libusb_error;
1321 }
1322
1323 // TODO: Could use the scanning technique from STM8 code here...
1324 slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
1325
1326 if (desc.idProduct == STLINK_USB_PID_STLINK_NUCLEO ||
1327 desc.idProduct == STLINK_USB_PID_STLINK_32L_AUDIO ||
1328 desc.idProduct == STLINK_USB_PID_STLINK_V2_1 ||
1329 desc.idProduct == STLINK_USB_PID_STLINK_V3_USBLOADER ||
1330 desc.idProduct == STLINK_USB_PID_STLINK_V3E_PID ||
1331 desc.idProduct == STLINK_USB_PID_STLINK_V3S_PID ||
1332 desc.idProduct == STLINK_USB_PID_STLINK_V3_2VCP_PID) {
1333 slu->ep_req = 1 /* ep req */ | LIBUSB_ENDPOINT_OUT;
1334 slu->ep_trace = 2 | LIBUSB_ENDPOINT_IN;
1335 } else {
1336 slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
1337 slu->ep_trace = 3 | LIBUSB_ENDPOINT_IN;
1338 }
1339
1340 slu->sg_transfer_idx = 0;
1341 slu->cmd_len = (slu->protocoll == 1) ? STLINK_SG_SIZE : STLINK_CMD_SIZE;
1342
1343 // initialize stlink version (sl->version)
1344 stlink_version(sl);
1345
1346 if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
1347 // this seems to work, and is unnecessary information for the user.
1348 // demoted to debug -- REW
1349 DLOG("-- exit_dfu_mode\n");
1350 stlink_exit_dfu_mode(sl);
1351 }
1352
1353 sl->freq = freq;
1354 // set the speed before entering the mode as the chip discovery phase
1355 // should be done at this speed too
1356 // set the stlink clock speed (default is 1800kHz)
1357 DLOG("JTAG/SWD freq set to %d\n", freq);
1358 stlink_set_swdclk(sl, freq);
1359
1360 stlink_target_connect(sl, connect);
1361 return(sl);
1362
1363 on_libusb_error:
1364 stlink_close(sl);
1365 return(NULL);
1366
1367 on_error:
1368
1369 if (slu->libusb_ctx) { libusb_exit(slu->libusb_ctx); }
1370
1371 on_malloc_error:
1372
1373 if (sl != NULL) { free(sl); }
1374
1375 if (slu != NULL) { free(slu); }
1376
1377 return(NULL);
1378 }
1379
stlink_probe_usb_devs(libusb_device ** devs,stlink_t ** sldevs[],enum connect_type connect,int freq)1380 static size_t stlink_probe_usb_devs(libusb_device **devs, stlink_t **sldevs[], enum connect_type connect, int freq) {
1381 stlink_t **_sldevs;
1382 libusb_device *dev;
1383 int i = 0;
1384 size_t slcnt = 0;
1385 size_t slcur = 0;
1386
1387 /* Count STLINKs */
1388 while ((dev = devs[i++]) != NULL) {
1389 struct libusb_device_descriptor desc;
1390 int ret = libusb_get_device_descriptor(dev, &desc);
1391
1392 if (ret < 0) {
1393 WLOG("failed to get libusb device descriptor (libusb error: %d)\n", ret);
1394 break;
1395 }
1396
1397 if (desc.idVendor != STLINK_USB_VID_ST) { continue; }
1398
1399 if (!STLINK_SUPPORTED_USB_PID(desc.idProduct)) {
1400 WLOG("skipping ST device : %#04x:%#04x)\n", desc.idVendor, desc.idProduct);
1401 continue;
1402 }
1403
1404 slcnt++;
1405 }
1406
1407 _sldevs = calloc(slcnt, sizeof(stlink_t *)); // allocate list of pointers
1408
1409 if (!_sldevs) {
1410 *sldevs = NULL;
1411 return(0);
1412 }
1413
1414 /* Open STLINKS and attach them to list */
1415 i = 0;
1416
1417 while ((dev = devs[i++]) != NULL) {
1418 struct libusb_device_descriptor desc;
1419 int ret = libusb_get_device_descriptor(dev, &desc);
1420
1421 if (ret < 0) {
1422 WLOG("failed to get libusb device descriptor (libusb error: %d)\n", ret);
1423 break;
1424 }
1425
1426 if (!STLINK_SUPPORTED_USB_PID(desc.idProduct)) { continue; }
1427
1428 struct libusb_device_handle* handle;
1429 char serial[STLINK_SERIAL_BUFFER_SIZE] = {0, };
1430
1431 ret = libusb_open(dev, &handle);
1432
1433 if (ret < 0) {
1434 if (ret == LIBUSB_ERROR_ACCESS) {
1435 ELOG("Could not open USB device %#06x:%#06x, access error.\n", desc.idVendor, desc.idProduct);
1436 } else {
1437 ELOG("Failed to open USB device %#06x:%#06x, libusb error: %d)\n", desc.idVendor, desc.idProduct, ret);
1438 }
1439
1440 break;
1441 }
1442
1443 size_t serial_len = stlink_serial(handle, &desc, serial);
1444
1445 libusb_close(handle);
1446
1447 if (serial_len != STLINK_SERIAL_LENGTH) { continue; }
1448
1449 stlink_t *sl = stlink_open_usb(0, connect, serial, freq);
1450
1451 if (!sl) {
1452 ELOG("Failed to open USB device %#06x:%#06x\n", desc.idVendor, desc.idProduct);
1453 continue;
1454 }
1455
1456 _sldevs[slcur++] = sl;
1457 }
1458
1459 *sldevs = _sldevs;
1460
1461 return(slcur);
1462 }
1463
stlink_probe_usb(stlink_t ** stdevs[],enum connect_type connect,int freq)1464 size_t stlink_probe_usb(stlink_t **stdevs[], enum connect_type connect, int freq) {
1465 libusb_device **devs;
1466 stlink_t **sldevs;
1467
1468 size_t slcnt = 0;
1469 int r;
1470 ssize_t cnt;
1471
1472 r = libusb_init(NULL);
1473
1474 if (r < 0) { return(0); }
1475
1476 cnt = libusb_get_device_list(NULL, &devs);
1477
1478 if (cnt < 0) { return(0); }
1479
1480 slcnt = stlink_probe_usb_devs(devs, &sldevs, connect, freq);
1481 libusb_free_device_list(devs, 1);
1482
1483 libusb_exit(NULL);
1484
1485 *stdevs = sldevs;
1486
1487 return(slcnt);
1488 }
1489
stlink_probe_usb_free(stlink_t *** stdevs,size_t size)1490 void stlink_probe_usb_free(stlink_t ***stdevs, size_t size) {
1491 if (stdevs == NULL || *stdevs == NULL || size == 0) { return; }
1492
1493 for (size_t n = 0; n < size; n++) { stlink_close((*stdevs)[n]); }
1494
1495 free(*stdevs);
1496 *stdevs = NULL;
1497 }
1498