1 /*
2 * Copyright (c) 2018-2019 PHYTEC Messtechnik GmbH
3 * Copyright (c) 2023 Nordic Semiconductor ASA
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 /*
9 * This file is based on DAP.c from CMSIS-DAP Source (Revision: V2.0.0)
10 * https://github.com/ARM-software/CMSIS_5/tree/develop/CMSIS/DAP/Firmware
11 * Copyright (c) 2013-2017 ARM Limited. All rights reserved.
12 * SPDX-License-Identifier: Apache-2.0
13 */
14
15 #include <string.h>
16 #include <zephyr/kernel.h>
17 #include <zephyr/device.h>
18 #include <zephyr/init.h>
19 #include <zephyr/sys/byteorder.h>
20 #include <zephyr/drivers/swdp.h>
21 #include <stdint.h>
22
23 #include <cmsis_dap.h>
24
25 #include <zephyr/logging/log.h>
26 LOG_MODULE_REGISTER(dap, CONFIG_DAP_LOG_LEVEL);
27
28 #define DAP_STATE_CONNECTED 0
29
30 struct dap_context {
31 struct device *swdp_dev;
32 atomic_t state;
33 uint8_t debug_port;
34 uint8_t capabilities;
35 uint16_t pkt_size;
36 struct {
37 /* Idle cycles after transfer */
38 uint8_t idle_cycles;
39 /* Number of retries after WAIT response */
40 uint16_t retry_count;
41 /* Number of retries if read value does not match */
42 uint16_t match_retry;
43 /* Match Mask */
44 uint32_t match_mask;
45 } transfer;
46 };
47
48 static struct dap_context dap_ctx[1];
49
50 BUILD_ASSERT(sizeof(CONFIG_CMSIS_DAP_PROBE_VENDOR) <=
51 MIN(CONFIG_CMSIS_DAP_PACKET_SIZE - 2, UINT8_MAX - 2),
52 "PROBE_VENDOR string is too long.");
53 BUILD_ASSERT(sizeof(CONFIG_CMSIS_DAP_PROBE_NAME) <=
54 MIN(CONFIG_CMSIS_DAP_PACKET_SIZE - 2, UINT8_MAX - 2),
55 "PROBE_NAME string is too long.");
56 BUILD_ASSERT(sizeof(CONFIG_CMSIS_DAP_BOARD_VENDOR) <=
57 MIN(CONFIG_CMSIS_DAP_PACKET_SIZE - 2, UINT8_MAX - 2),
58 "BOARD_VENDOR string is too long.");
59 BUILD_ASSERT(sizeof(CONFIG_CMSIS_DAP_BOARD_NAME) <=
60 MIN(CONFIG_CMSIS_DAP_PACKET_SIZE - 2, UINT8_MAX - 2),
61 "BOARD_NAME string is too long.");
62 BUILD_ASSERT(sizeof(CONFIG_CMSIS_DAP_DEVICE_VENDOR) <=
63 MIN(CONFIG_CMSIS_DAP_PACKET_SIZE - 2, UINT8_MAX - 2),
64 "DEVICE_VENDOR string is too long.");
65 BUILD_ASSERT(sizeof(CONFIG_CMSIS_DAP_DEVICE_NAME) <=
66 MIN(CONFIG_CMSIS_DAP_PACKET_SIZE - 2, UINT8_MAX - 2),
67 "DEVICE_NAME string is too long.");
68
69 /* Get DAP Information */
dap_info(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)70 static uint16_t dap_info(struct dap_context *const ctx,
71 const uint8_t *const request,
72 uint8_t *const response)
73 {
74 uint8_t *info = response + 1;
75 uint8_t id = request[0];
76 uint8_t length = 0U;
77
78 switch (id) {
79 case DAP_ID_VENDOR:
80 LOG_DBG("ID_VENDOR");
81 memcpy(info, CONFIG_CMSIS_DAP_PROBE_VENDOR,
82 sizeof(CONFIG_CMSIS_DAP_PROBE_VENDOR));
83 length = sizeof(CONFIG_CMSIS_DAP_PROBE_VENDOR);
84 break;
85 case DAP_ID_PRODUCT:
86 LOG_DBG("ID_PRODUCT");
87 memcpy(info, CONFIG_CMSIS_DAP_PROBE_NAME,
88 sizeof(CONFIG_CMSIS_DAP_PROBE_NAME));
89 length = sizeof(CONFIG_CMSIS_DAP_PROBE_NAME);
90 break;
91 case DAP_ID_SER_NUM:
92 /* optional to implement */
93 LOG_DBG("ID_SER_NUM unsupported");
94 break;
95 case DAP_ID_FW_VER:
96 LOG_DBG("ID_FW_VER");
97 memcpy(info, DAP_FW_VER, sizeof(DAP_FW_VER));
98 length = sizeof(DAP_FW_VER);
99 break;
100 case DAP_ID_DEVICE_VENDOR:
101 LOG_DBG("ID_DEVICE_VENDOR");
102 memcpy(info, CONFIG_CMSIS_DAP_DEVICE_VENDOR,
103 sizeof(CONFIG_CMSIS_DAP_DEVICE_VENDOR));
104 length = sizeof(CONFIG_CMSIS_DAP_DEVICE_VENDOR);
105 break;
106 case DAP_ID_DEVICE_NAME:
107 LOG_DBG("ID_DEVICE_NAME");
108 memcpy(info, CONFIG_CMSIS_DAP_DEVICE_NAME,
109 sizeof(CONFIG_CMSIS_DAP_DEVICE_NAME));
110 length = sizeof(CONFIG_CMSIS_DAP_DEVICE_NAME);
111 break;
112 case DAP_ID_BOARD_VENDOR:
113 LOG_DBG("ID_BOARD_VENDOR");
114 memcpy(info, CONFIG_CMSIS_DAP_BOARD_VENDOR,
115 sizeof(CONFIG_CMSIS_DAP_BOARD_VENDOR));
116 length = sizeof(CONFIG_CMSIS_DAP_BOARD_VENDOR);
117 break;
118 case DAP_ID_BOARD_NAME:
119 memcpy(info, CONFIG_CMSIS_DAP_BOARD_NAME,
120 sizeof(CONFIG_CMSIS_DAP_BOARD_NAME));
121 length = sizeof(CONFIG_CMSIS_DAP_BOARD_NAME);
122 LOG_DBG("ID_BOARD_NAME");
123 break;
124 case DAP_ID_PRODUCT_FW_VER:
125 /* optional to implement */
126 LOG_DBG("ID_PRODUCT_FW_VER unsupported");
127 break;
128 case DAP_ID_CAPABILITIES:
129 info[0] = ctx->capabilities;
130 LOG_DBG("ID_CAPABILITIES 0x%0x", info[0]);
131 length = 1U;
132 break;
133 case DAP_ID_TIMESTAMP_CLOCK:
134 LOG_DBG("ID_TIMESTAMP_CLOCK unsupported");
135 break;
136 case DAP_ID_UART_RX_BUFFER_SIZE:
137 LOG_DBG("ID_UART_RX_BUFFER_SIZE unsupported");
138 break;
139 case DAP_ID_UART_TX_BUFFER_SIZE:
140 LOG_DBG("ID_UART_TX_BUFFER_SIZE unsupported");
141 break;
142 case DAP_ID_SWO_BUFFER_SIZE:
143 LOG_DBG("ID_SWO_BUFFER_SIZE unsupported");
144 break;
145 case DAP_ID_PACKET_SIZE:
146 LOG_DBG("ID_PACKET_SIZE");
147 sys_put_le16(ctx->pkt_size, &info[0]);
148 length = 2U;
149 break;
150 case DAP_ID_PACKET_COUNT:
151 LOG_DBG("ID_PACKET_COUNT");
152 info[0] = CONFIG_CMSIS_DAP_PACKET_COUNT;
153 length = 1U;
154 break;
155 default:
156 LOG_DBG("unsupported ID");
157 break;
158 }
159
160 response[0] = length;
161
162 return length + 1U;
163 }
164
165 /* Process Host Status command and prepare response */
dap_host_status(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)166 static uint16_t dap_host_status(struct dap_context *const ctx,
167 const uint8_t *const request,
168 uint8_t *const response)
169 {
170 switch (request[0]) {
171 case DAP_DEBUGGER_CONNECTED:
172 if (request[1]) {
173 LOG_INF("Debugger connected");
174 } else {
175 LOG_INF("Debugger disconnected");
176 }
177 break;
178 case DAP_TARGET_RUNNING:
179 LOG_DBG("unsupported");
180 break;
181 default:
182 *response = DAP_ERROR;
183 return 1U;
184 }
185
186 response[0] = DAP_OK;
187 return 1U;
188 }
189
190 /* Process Connect command and prepare response */
dap_connect(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)191 static uint16_t dap_connect(struct dap_context *const ctx,
192 const uint8_t *const request,
193 uint8_t *const response)
194 {
195 const struct swdp_api *api = ctx->swdp_dev->api;
196 uint8_t port;
197
198 if (request[0] == DAP_PORT_AUTODETECT) {
199 port = DAP_PORT_SWD;
200 } else {
201 port = request[0];
202 }
203
204 switch (port) {
205 case DAP_PORT_SWD:
206 LOG_INF("port swd");
207 ctx->debug_port = DAP_PORT_SWD;
208
209 if (atomic_test_and_set_bit(&ctx->state,
210 DAP_STATE_CONNECTED)) {
211 LOG_ERR("DAP device is already connected");
212 break;
213 }
214
215 api->swdp_port_on(ctx->swdp_dev);
216 break;
217 case DAP_PORT_JTAG:
218 LOG_ERR("port unsupported");
219 port = DAP_ERROR;
220 break;
221 default:
222 LOG_DBG("port disabled");
223 port = DAP_PORT_DISABLED;
224 break;
225 }
226
227 response[0] = port;
228 return 1U;
229 }
230
231 /* Process Disconnect command and prepare response */
dap_disconnect(struct dap_context * const ctx,uint8_t * const response)232 static uint16_t dap_disconnect(struct dap_context *const ctx,
233 uint8_t *const response)
234 {
235 const struct swdp_api *api = ctx->swdp_dev->api;
236
237 LOG_DBG("");
238
239 ctx->debug_port = DAP_PORT_DISABLED;
240
241 if (atomic_test_bit(&ctx->state, DAP_STATE_CONNECTED)) {
242 api->swdp_port_off(ctx->swdp_dev);
243 } else {
244 LOG_WRN("DAP device is not connected");
245 }
246
247 response[0] = DAP_OK;
248 atomic_clear_bit(&ctx->state, DAP_STATE_CONNECTED);
249
250 return 1;
251 }
252
253 /* Process Delay command and prepare response */
dap_delay(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)254 static uint16_t dap_delay(struct dap_context *const ctx,
255 const uint8_t *const request,
256 uint8_t *const response)
257 {
258 uint16_t delay = sys_get_le16(&request[0]);
259
260 LOG_DBG("dap delay %u ms", delay);
261
262 k_busy_wait(delay * USEC_PER_MSEC);
263 response[0] = DAP_OK;
264
265 return 1U;
266 }
267
268 /* Process Reset Target command and prepare response */
dap_reset_target(struct dap_context * const ctx,uint8_t * const response)269 static uint16_t dap_reset_target(struct dap_context *const ctx,
270 uint8_t *const response)
271 {
272 response[0] = DAP_OK;
273 response[1] = 0U;
274 LOG_WRN("unsupported");
275
276 return 2;
277 }
278
279 /* Process SWJ Pins command and prepare response */
dap_swj_pins(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)280 static uint16_t dap_swj_pins(struct dap_context *const ctx,
281 const uint8_t *const request,
282 uint8_t *const response)
283 {
284 const struct swdp_api *api = ctx->swdp_dev->api;
285 uint8_t value = request[0];
286 uint8_t select = request[1];
287 uint32_t wait = sys_get_le32(&request[2]);
288 k_timepoint_t end = sys_timepoint_calc(K_USEC(wait));
289 uint8_t state;
290
291 if (!atomic_test_bit(&ctx->state, DAP_STATE_CONNECTED)) {
292 LOG_ERR("DAP device is not connected");
293 response[0] = DAP_ERROR;
294 return 1U;
295 }
296
297 /* Skip if nothing selected. */
298 if (select) {
299 api->swdp_set_pins(ctx->swdp_dev, select, value);
300 }
301
302 do {
303 api->swdp_get_pins(ctx->swdp_dev, &state);
304 LOG_INF("select 0x%02x, value 0x%02x, wait %u, state 0x%02x",
305 select, value, wait, state);
306 if ((value & select) == (state & select)) {
307 LOG_DBG("swdp_get_pins succeeded before timeout");
308 break;
309 }
310 } while (!sys_timepoint_expired(end));
311
312 response[0] = state;
313
314 return sizeof(state);
315 }
316
317 /* Process SWJ Clock command and prepare response */
dap_swj_clock(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)318 static uint16_t dap_swj_clock(struct dap_context *const ctx,
319 const uint8_t *const request,
320 uint8_t *const response)
321 {
322 const struct swdp_api *api = ctx->swdp_dev->api;
323 uint32_t clk = sys_get_le32(&request[0]);
324
325 LOG_DBG("clock %d", clk);
326
327 if (atomic_test_bit(&ctx->state, DAP_STATE_CONNECTED)) {
328 if (clk) {
329 api->swdp_set_clock(ctx->swdp_dev, clk);
330 response[0] = DAP_OK;
331 } else {
332 response[0] = DAP_ERROR;
333 }
334 } else {
335 LOG_WRN("DAP device is not connected");
336 response[0] = DAP_OK;
337 }
338
339 return 1U;
340 }
341
342 /* Process SWJ Sequence command and prepare response */
dap_swj_sequence(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)343 static uint16_t dap_swj_sequence(struct dap_context *const ctx,
344 const uint8_t *const request,
345 uint8_t *const response)
346 {
347 const struct swdp_api *api = ctx->swdp_dev->api;
348 uint16_t count = request[0];
349
350 LOG_DBG("count %u", count);
351
352 if (count == 0U) {
353 count = 256U;
354 }
355
356 if (!atomic_test_bit(&ctx->state, DAP_STATE_CONNECTED)) {
357 LOG_ERR("DAP device is not connected");
358 response[0] = DAP_ERROR;
359 return 1U;
360 }
361
362 api->swdp_output_sequence(ctx->swdp_dev, count, &request[1]);
363 response[0] = DAP_OK;
364
365 return 1U;
366 }
367
368 /* Process SWD Configure command and prepare response */
dap_swdp_configure(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)369 static uint16_t dap_swdp_configure(struct dap_context *const ctx,
370 const uint8_t *const request,
371 uint8_t *const response)
372 {
373 const struct swdp_api *api = ctx->swdp_dev->api;
374 uint8_t turnaround = (request[0] & 0x03U) + 1U;
375 bool data_phase = (request[0] & 0x04U) ? true : false;
376
377 if (!atomic_test_bit(&ctx->state, DAP_STATE_CONNECTED)) {
378 LOG_ERR("DAP device is not connected");
379 response[0] = DAP_ERROR;
380 return 1U;
381 }
382
383 api->swdp_configure(ctx->swdp_dev, turnaround, data_phase);
384 response[0] = DAP_OK;
385
386 return 1U;
387 }
388
389 /* Process Transfer Configure command and prepare response */
dap_transfer_cfg(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)390 static uint16_t dap_transfer_cfg(struct dap_context *const ctx,
391 const uint8_t *const request,
392 uint8_t *const response)
393 {
394
395 ctx->transfer.idle_cycles = request[0];
396 ctx->transfer.retry_count = sys_get_le16(&request[1]);
397 ctx->transfer.match_retry = sys_get_le16(&request[3]);
398 LOG_DBG("idle_cycles %d, retry_count %d, match_retry %d",
399 ctx->transfer.idle_cycles,
400 ctx->transfer.retry_count,
401 ctx->transfer.match_retry);
402
403 response[0] = DAP_OK;
404 return 1U;
405 }
406
do_swdp_transfer(struct dap_context * const ctx,const uint8_t req_val,uint32_t * data)407 static inline uint8_t do_swdp_transfer(struct dap_context *const ctx,
408 const uint8_t req_val,
409 uint32_t *data)
410 {
411 const struct swdp_api *api = ctx->swdp_dev->api;
412 uint32_t retry = ctx->transfer.retry_count;
413 uint8_t rspns_val;
414
415 do {
416 api->swdp_transfer(ctx->swdp_dev,
417 req_val,
418 data,
419 ctx->transfer.idle_cycles,
420 &rspns_val);
421 } while ((rspns_val == SWDP_ACK_WAIT) && retry--);
422
423 return rspns_val;
424 }
425
swdp_transfer_match(struct dap_context * const ctx,const uint8_t req_val,const uint32_t match_val)426 static uint8_t swdp_transfer_match(struct dap_context *const ctx,
427 const uint8_t req_val,
428 const uint32_t match_val)
429 {
430 uint32_t match_retry = ctx->transfer.match_retry;
431 uint32_t data;
432 uint8_t rspns_val;
433
434 if (req_val & SWDP_REQUEST_APnDP) {
435 /* Post AP read, result will be returned on the next transfer */
436 rspns_val = do_swdp_transfer(ctx, req_val, NULL);
437 if (rspns_val != SWDP_ACK_OK) {
438 return rspns_val;
439 }
440 }
441
442 do {
443 /*
444 * Read register until its value matches
445 * or retry counter expires
446 */
447 rspns_val = do_swdp_transfer(ctx, req_val, &data);
448 if (rspns_val != SWDP_ACK_OK) {
449 return rspns_val;
450 }
451
452 } while (((data & ctx->transfer.match_mask) != match_val) &&
453 match_retry--);
454
455 if ((data & ctx->transfer.match_mask) != match_val) {
456 rspns_val |= DAP_TRANSFER_MISMATCH;
457 }
458
459 return rspns_val;
460 }
461
462 /*
463 * Process SWD Transfer command and prepare response
464 * pyOCD counterpart is _encode_transfer_data.
465 * Packet format: one byte DAP_index (ignored)
466 * one bytes transfer_count
467 * following by number transfer_count pairs of
468 * one byte request (register)
469 * four byte data (for write request only)
470 */
dap_swdp_transfer(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)471 static uint16_t dap_swdp_transfer(struct dap_context *const ctx,
472 const uint8_t *const request,
473 uint8_t *const response)
474 {
475 uint8_t *rspns_buf;
476 const uint8_t *req_buf;
477 uint8_t rspns_cnt = 0;
478 uint8_t rspns_val = 0;
479 bool post_read = false;
480 uint32_t check_write = 0;
481 uint8_t req_cnt;
482 uint8_t req_val;
483 uint32_t match_val;
484 uint32_t data;
485
486 /* Ignore DAP index request[0] */
487 req_cnt = request[1];
488 req_buf = request + sizeof(req_cnt) + 1;
489 rspns_buf = response + (sizeof(rspns_cnt) + sizeof(rspns_val));
490
491 for (; req_cnt; req_cnt--) {
492 req_val = *req_buf++;
493 if (req_val & SWDP_REQUEST_RnW) {
494 /* Read register */
495 if (post_read) {
496 /*
497 * Read was posted before, read previous AP
498 * data or post next AP read.
499 */
500 if ((req_val & (SWDP_REQUEST_APnDP |
501 DAP_TRANSFER_MATCH_VALUE)) !=
502 SWDP_REQUEST_APnDP) {
503 req_val = DP_RDBUFF | SWDP_REQUEST_RnW;
504 post_read = false;
505 }
506
507 rspns_val = do_swdp_transfer(ctx, req_val, &data);
508 if (rspns_val != SWDP_ACK_OK) {
509 break;
510 }
511
512 /* Store previous AP data */
513 sys_put_le32(data, rspns_buf);
514 rspns_buf += sizeof(data);
515 }
516 if (req_val & DAP_TRANSFER_MATCH_VALUE) {
517 LOG_INF("match value read");
518 /* Read with value match */
519 match_val = sys_get_le32(req_buf);
520 req_buf += sizeof(match_val);
521
522 rspns_val = swdp_transfer_match(ctx, req_val, match_val);
523 if (rspns_val != SWDP_ACK_OK) {
524 break;
525 }
526
527 } else if (req_val & SWDP_REQUEST_APnDP) {
528 /* Normal read */
529 if (!post_read) {
530 /* Post AP read */
531 rspns_val = do_swdp_transfer(ctx, req_val, NULL);
532 if (rspns_val != SWDP_ACK_OK) {
533 break;
534 }
535 post_read = true;
536 }
537 } else {
538 /* Read DP register */
539 rspns_val = do_swdp_transfer(ctx, req_val, &data);
540 if (rspns_val != SWDP_ACK_OK) {
541 break;
542 }
543 /* Store data */
544 sys_put_le32(data, rspns_buf);
545 rspns_buf += sizeof(data);
546 }
547 check_write = 0U;
548 } else {
549 /* Write register */
550 if (post_read) {
551 /* Read previous data */
552 rspns_val = do_swdp_transfer(ctx,
553 DP_RDBUFF | SWDP_REQUEST_RnW,
554 &data);
555 if (rspns_val != SWDP_ACK_OK) {
556 break;
557 }
558
559 /* Store previous data */
560 sys_put_le32(data, rspns_buf);
561 rspns_buf += sizeof(data);
562 post_read = false;
563 }
564 /* Load data */
565 data = sys_get_le32(req_buf);
566 req_buf += sizeof(data);
567 if (req_val & DAP_TRANSFER_MATCH_MASK) {
568 /* Write match mask */
569 ctx->transfer.match_mask = data;
570 rspns_val = SWDP_ACK_OK;
571 } else {
572 /* Write DP/AP register */
573 rspns_val = do_swdp_transfer(ctx, req_val, &data);
574 if (rspns_val != SWDP_ACK_OK) {
575 break;
576 }
577
578 check_write = 1U;
579 }
580 }
581 rspns_cnt++;
582 }
583
584 if (rspns_val == SWDP_ACK_OK) {
585 if (post_read) {
586 /* Read previous data */
587 rspns_val = do_swdp_transfer(ctx,
588 DP_RDBUFF | SWDP_REQUEST_RnW,
589 &data);
590 if (rspns_val != SWDP_ACK_OK) {
591 goto end;
592 }
593
594 /* Store previous data */
595 sys_put_le32(data, rspns_buf);
596 rspns_buf += sizeof(data);
597 } else if (check_write) {
598 /* Check last write */
599 rspns_val = do_swdp_transfer(ctx,
600 DP_RDBUFF | SWDP_REQUEST_RnW,
601 NULL);
602 }
603 }
604
605 end:
606 response[0] = rspns_cnt;
607 response[1] = rspns_val;
608
609 return (rspns_buf - response);
610 }
611
612 /* Delegate DAP Transfer command */
dap_transfer(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)613 static uint16_t dap_transfer(struct dap_context *const ctx,
614 const uint8_t *const request,
615 uint8_t *const response)
616 {
617 uint16_t retval;
618
619 if (!atomic_test_bit(&ctx->state, DAP_STATE_CONNECTED)) {
620 LOG_ERR("DAP device is not connected");
621 response[0] = DAP_ERROR;
622 return 1U;
623 }
624
625 switch (ctx->debug_port) {
626 case DAP_PORT_SWD:
627 retval = dap_swdp_transfer(ctx, request, response);
628 break;
629 case DAP_PORT_JTAG:
630 default:
631 LOG_ERR("port unsupported");
632 response[0] = DAP_ERROR;
633 retval = 1U;
634 }
635
636 return retval;
637 }
638
dap_swdp_sequence(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)639 static uint16_t dap_swdp_sequence(struct dap_context *const ctx,
640 const uint8_t *const request,
641 uint8_t *const response)
642 {
643 const struct swdp_api *api = ctx->swdp_dev->api;
644 const uint8_t *request_data = request + 1;
645 uint8_t *response_data = response + 1;
646 uint8_t count = request[0];
647 uint8_t num_cycles;
648 uint32_t num_bytes;
649 bool input;
650
651 switch (ctx->debug_port) {
652 case DAP_PORT_SWD:
653 response[0] = DAP_OK;
654 break;
655 case DAP_PORT_JTAG:
656 default:
657 LOG_ERR("port unsupported");
658 response[0] = DAP_ERROR;
659 return 1U;
660 }
661
662 for (size_t i = 0; i < count; ++i) {
663 input = *request_data & BIT(7);
664 num_cycles = *request_data & BIT_MASK(7);
665 num_bytes = (num_cycles + 7) >> 3; /* rounded up to full bytes */
666
667 if (num_cycles == 0) {
668 num_cycles = 64;
669 }
670
671 request_data += 1;
672
673 if (input) {
674 api->swdp_input_sequence(ctx->swdp_dev, num_cycles, response_data);
675 response_data += num_bytes;
676 } else {
677 api->swdp_output_sequence(ctx->swdp_dev, num_cycles, request_data);
678 request_data += num_bytes;
679 }
680 }
681
682 return response_data - response;
683 }
684
685 /*
686 * Process SWD DAP_TransferBlock command and prepare response.
687 * pyOCD counterpart is _encode_transfer_block_data.
688 * Packet format: one byte DAP_index (ignored)
689 * two bytes transfer_count
690 * one byte block_request (register)
691 * data[transfer_count * sizeof(uint32_t)]
692 */
dap_swdp_transferblock(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)693 static uint16_t dap_swdp_transferblock(struct dap_context *const ctx,
694 const uint8_t *const request,
695 uint8_t *const response)
696 {
697 uint32_t data;
698 uint8_t *rspns_buf;
699 const uint8_t *req_buf;
700 uint16_t rspns_cnt = 0;
701 uint16_t req_cnt;
702 uint8_t rspns_val = 0;
703 uint8_t req_val;
704
705 req_cnt = sys_get_le16(&request[1]);
706 req_val = request[3];
707 req_buf = request + (sizeof(req_cnt) + sizeof(req_val) + 1);
708 rspns_buf = response + (sizeof(rspns_cnt) + sizeof(rspns_val));
709
710 if (req_cnt == 0U) {
711 goto end;
712 }
713
714 if (req_val & SWDP_REQUEST_RnW) {
715 /* Read register block */
716 if (req_val & SWDP_REQUEST_APnDP) {
717 /* Post AP read */
718 rspns_val = do_swdp_transfer(ctx, req_val, NULL);
719 if (rspns_val != SWDP_ACK_OK) {
720 goto end;
721 }
722 }
723
724 while (req_cnt--) {
725 /* Read DP/AP register */
726 if ((req_cnt == 0U) &&
727 (req_val & SWDP_REQUEST_APnDP)) {
728 /* Last AP read */
729 req_val = DP_RDBUFF | SWDP_REQUEST_RnW;
730 }
731
732 rspns_val = do_swdp_transfer(ctx, req_val, &data);
733 if (rspns_val != SWDP_ACK_OK) {
734 goto end;
735 }
736
737 /* Store data */
738 sys_put_le32(data, rspns_buf);
739 rspns_buf += sizeof(data);
740 rspns_cnt++;
741 }
742 } else {
743 /* Write register block */
744 while (req_cnt--) {
745 /* Load data */
746 data = sys_get_le32(req_buf);
747 req_buf += sizeof(data);
748 /* Write DP/AP register */
749 rspns_val = do_swdp_transfer(ctx, req_val, &data);
750 if (rspns_val != SWDP_ACK_OK) {
751 goto end;
752 }
753
754 rspns_cnt++;
755 }
756 /* Check last write */
757 rspns_val = do_swdp_transfer(ctx, DP_RDBUFF | SWDP_REQUEST_RnW, NULL);
758 }
759
760 end:
761 sys_put_le16(rspns_cnt, &response[0]);
762 response[2] = rspns_val;
763
764 LOG_DBG("Received %u, to transmit %u, response count %u",
765 req_buf - request,
766 rspns_buf - response,
767 rspns_cnt * 4);
768
769 return (rspns_buf - response);
770 }
771
772 /* Delegate Transfer Block command */
dap_transferblock(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)773 static uint16_t dap_transferblock(struct dap_context *const ctx,
774 const uint8_t *const request,
775 uint8_t *const response)
776 {
777 uint16_t retval;
778
779 if (!atomic_test_bit(&ctx->state, DAP_STATE_CONNECTED)) {
780 LOG_ERR("DAP device is not connected");
781 /* Clear response count */
782 sys_put_le16(0U, &response[0]);
783 /* Clear DAP response (ACK) value */
784 response[2] = 0U;
785 return 3U;
786 }
787
788 switch (ctx->debug_port) {
789 case DAP_PORT_SWD:
790 retval = dap_swdp_transferblock(ctx, request, response);
791 break;
792 case DAP_PORT_JTAG:
793 default:
794 LOG_ERR("port unsupported");
795 /* Clear response count */
796 sys_put_le16(0U, &response[0]);
797 /* Clear DAP response (ACK) value */
798 response[2] = 0U;
799 retval = 3U;
800 }
801
802 return retval;
803 }
804
805 /* Process SWD Write ABORT command and prepare response */
dap_swdp_writeabort(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)806 static uint16_t dap_swdp_writeabort(struct dap_context *const ctx,
807 const uint8_t *const request,
808 uint8_t *const response)
809 {
810 const struct swdp_api *api = ctx->swdp_dev->api;
811 /* Load data (Ignore DAP index in request[0]) */
812 uint32_t data = sys_get_le32(&request[1]);
813
814 /* Write Abort register */
815 api->swdp_transfer(ctx->swdp_dev, DP_ABORT, &data,
816 ctx->transfer.idle_cycles, NULL);
817
818 response[0] = DAP_OK;
819 return 1U;
820 }
821
822 /* Delegate DAP Write ABORT command */
dap_writeabort(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)823 static uint16_t dap_writeabort(struct dap_context *const ctx,
824 const uint8_t *const request,
825 uint8_t *const response)
826 {
827 uint16_t retval;
828
829 if (!atomic_test_bit(&ctx->state, DAP_STATE_CONNECTED)) {
830 LOG_ERR("DAP device is not connected");
831 response[0] = DAP_ERROR;
832 return 1U;
833 }
834
835 switch (ctx->debug_port) {
836 case DAP_PORT_SWD:
837 retval = dap_swdp_writeabort(ctx, request, response);
838 break;
839 case DAP_PORT_JTAG:
840 default:
841 LOG_ERR("port unsupported");
842 response[0] = DAP_ERROR;
843 retval = 1U;
844 }
845 return retval;
846 }
847
848 /* Process DAP Vendor command request */
dap_process_vendor_cmd(struct dap_context * const ctx,const uint8_t * const request,uint8_t * const response)849 static uint16_t dap_process_vendor_cmd(struct dap_context *const ctx,
850 const uint8_t *const request,
851 uint8_t *const response)
852 {
853 response[0] = ID_DAP_INVALID;
854 return 1U;
855 }
856
857 /*
858 * Process DAP command request and prepare response
859 * request: pointer to request data
860 * response: pointer to response data
861 * return: number of bytes in response
862 *
863 * All the subsequent command functions have the same parameter
864 * and return value structure.
865 */
dap_process_cmd(struct dap_context * const ctx,const uint8_t * request,uint8_t * response)866 static uint16_t dap_process_cmd(struct dap_context *const ctx,
867 const uint8_t *request,
868 uint8_t *response)
869 {
870 uint16_t retval;
871
872 LOG_HEXDUMP_DBG(request, 8, "req");
873
874 if ((*request >= ID_DAP_VENDOR0) && (*request <= ID_DAP_VENDOR31)) {
875 return dap_process_vendor_cmd(ctx, request, response);
876 }
877
878 *response++ = *request;
879 LOG_DBG("request 0x%02x", *request);
880
881 switch (*request++) {
882 case ID_DAP_INFO:
883 retval = dap_info(ctx, request, response);
884 break;
885 case ID_DAP_HOST_STATUS:
886 retval = dap_host_status(ctx, request, response);
887 break;
888 case ID_DAP_CONNECT:
889 retval = dap_connect(ctx, request, response);
890 break;
891 case ID_DAP_DISCONNECT:
892 retval = dap_disconnect(ctx, response);
893 break;
894 case ID_DAP_DELAY:
895 retval = dap_delay(ctx, request, response);
896 break;
897 case ID_DAP_RESET_TARGET:
898 retval = dap_reset_target(ctx, response);
899 break;
900 case ID_DAP_SWJ_PINS:
901 retval = dap_swj_pins(ctx, request, response);
902 break;
903 case ID_DAP_SWJ_CLOCK:
904 retval = dap_swj_clock(ctx, request, response);
905 break;
906 case ID_DAP_SWJ_SEQUENCE:
907 retval = dap_swj_sequence(ctx, request, response);
908 break;
909 case ID_DAP_SWDP_CONFIGURE:
910 retval = dap_swdp_configure(ctx, request, response);
911 break;
912 case ID_DAP_SWDP_SEQUENCE:
913 retval = dap_swdp_sequence(ctx, request, response);
914 break;
915 case ID_DAP_JTAG_SEQUENCE:
916 LOG_ERR("JTAG sequence unsupported");
917 retval = 1;
918 *response = DAP_ERROR;
919 break;
920 case ID_DAP_JTAG_CONFIGURE:
921 LOG_ERR("JTAG configure unsupported");
922 retval = 1;
923 *response = DAP_ERROR;
924 break;
925 case ID_DAP_JTAG_IDCODE:
926 LOG_ERR("JTAG IDCODE unsupported");
927 retval = 1;
928 *response = DAP_ERROR;
929 break;
930 case ID_DAP_TRANSFER_CONFIGURE:
931 retval = dap_transfer_cfg(ctx, request, response);
932 break;
933 case ID_DAP_TRANSFER:
934 retval = dap_transfer(ctx, request, response);
935 break;
936 case ID_DAP_TRANSFER_BLOCK:
937 retval = dap_transferblock(ctx, request, response);
938 break;
939 case ID_DAP_WRITE_ABORT:
940 retval = dap_writeabort(ctx, request, response);
941 break;
942 case ID_DAP_SWO_TRANSPORT:
943 LOG_ERR("SWO Transport unsupported");
944 retval = 1;
945 *response = DAP_ERROR;
946 break;
947 case ID_DAP_SWO_MODE:
948 LOG_ERR("SWO Mode unsupported");
949 retval = 1;
950 *response = DAP_ERROR;
951 break;
952 case ID_DAP_SWO_BAUDRATE:
953 LOG_ERR("SWO Baudrate unsupported");
954 retval = 1;
955 *response = DAP_ERROR;
956 break;
957 case ID_DAP_SWO_CONTROL:
958 LOG_ERR("SWO Control unsupported");
959 retval = 1;
960 *response = DAP_ERROR;
961 break;
962 case ID_DAP_SWO_STATUS:
963 LOG_ERR("SWO Status unsupported");
964 retval = 1;
965 *response = DAP_ERROR;
966 break;
967 case ID_DAP_SWO_DATA:
968 LOG_ERR("SWO Data unsupported");
969 retval = 1;
970 *response = DAP_ERROR;
971 break;
972 case ID_DAP_UART_TRANSPORT:
973 LOG_ERR("UART Transport unsupported");
974 retval = 1;
975 *response = DAP_ERROR;
976 break;
977 case ID_DAP_UART_CONFIGURE:
978 LOG_ERR("UART Configure unsupported");
979 retval = 1;
980 *response = DAP_ERROR;
981 break;
982 case ID_DAP_UART_CONTROL:
983 LOG_ERR("UART Control unsupported");
984 retval = 1;
985 *response = DAP_ERROR;
986 break;
987 case ID_DAP_UART_STATUS:
988 LOG_ERR("UART Status unsupported");
989 retval = 1;
990 *response = DAP_ERROR;
991 break;
992 case ID_DAP_UART_TRANSFER:
993 LOG_ERR("UART Transfer unsupported");
994 retval = 1;
995 *response = DAP_ERROR;
996 break;
997
998 default:
999 *(response - 1) = ID_DAP_INVALID;
1000 return 1U;
1001 }
1002
1003 return (1U + retval);
1004 }
1005
1006 /*
1007 * Execute DAP command (process request and prepare response)
1008 * request: pointer to request data
1009 * response: pointer to response data
1010 * return: number of bytes in response
1011 */
dap_execute_cmd(const uint8_t * request,uint8_t * response)1012 uint32_t dap_execute_cmd(const uint8_t *request,
1013 uint8_t *response)
1014 {
1015 uint32_t retval;
1016 uint16_t n;
1017 uint8_t count;
1018
1019 if (request[0] == ID_DAP_EXECUTE_COMMANDS) {
1020 /* copy command and increment */
1021 *response++ = *request++;
1022 count = request[0];
1023 request += sizeof(count);
1024 response[0] = count;
1025 response += sizeof(count);
1026 retval = sizeof(count) + 1U;
1027 LOG_WRN("(untested) ID DAP EXECUTE_COMMANDS count %u", count);
1028 while (count--) {
1029 n = dap_process_cmd(&dap_ctx[0], request, response);
1030 retval += n;
1031 request += n;
1032 response += n;
1033 }
1034 return retval;
1035 }
1036
1037 return dap_process_cmd(&dap_ctx[0], request, response);
1038 }
1039
dap_setup(const struct device * const dev)1040 int dap_setup(const struct device *const dev)
1041 {
1042 dap_ctx[0].swdp_dev = (void *)dev;
1043
1044 if (!device_is_ready(dap_ctx[0].swdp_dev)) {
1045 LOG_ERR("SWD driver not ready");
1046 return -ENODEV;
1047 }
1048
1049 /* Default settings */
1050 dap_ctx[0].pkt_size = CONFIG_CMSIS_DAP_PACKET_SIZE;
1051 dap_ctx[0].debug_port = 0U;
1052 dap_ctx[0].transfer.idle_cycles = 0U;
1053 dap_ctx[0].transfer.retry_count = 100U;
1054 dap_ctx[0].transfer.match_retry = 0U;
1055 dap_ctx[0].transfer.match_mask = 0U;
1056 dap_ctx[0].capabilities = DAP_SUPPORTS_ATOMIC_COMMANDS |
1057 DAP_DP_SUPPORTS_SWD;
1058
1059 return 0;
1060 }
1061
