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