1 /* SPDX-License-Identifier: GPL-2.0-or-later */
2 /* Copyright (C) 2020 by Daniel Anselmi <danselmi@gmx.ch> */
8 #include <helper/bits.h>
9 #include <helper/time_support.h>
10 #include <jtag/jtag.h>
11 #include <server/server.h>
12 #include <target/target.h>
16 #define IPDBG_BUFFER_SIZE 16384
17 #define IPDBG_MIN_NUM_OF_OPTIONS 4
18 #define IPDBG_MAX_NUM_OF_OPTIONS 14
19 #define IPDBG_MIN_DR_LENGTH 11
20 #define IPDBG_MAX_DR_LENGTH 13
21 #define IPDBG_TCP_PORT_STR_MAX_LENGTH 6
23 /* private connection data for IPDBG */
27 char buffer[IPDBG_BUFFER_SIZE];
30 struct ipdbg_connection {
31 struct ipdbg_fifo dn_fifo;
32 struct ipdbg_fifo up_fifo;
36 struct ipdbg_service {
37 struct ipdbg_hub *hub;
38 struct ipdbg_service *next;
40 struct ipdbg_connection connection;
44 struct ipdbg_virtual_ir_info {
51 uint32_t user_instruction;
53 uint32_t active_connections;
54 uint32_t active_services;
58 uint32_t last_dn_tool;
59 struct ipdbg_hub *next;
61 struct connection **connections;
62 uint8_t data_register_length;
64 struct ipdbg_virtual_ir_info *virtual_ir;
67 static struct ipdbg_hub *ipdbg_first_hub;
69 static struct ipdbg_service *ipdbg_first_service;
71 static void ipdbg_init_fifo(struct ipdbg_fifo *fifo)
77 static bool ipdbg_fifo_is_empty(struct ipdbg_fifo *fifo)
79 return fifo->count == 0;
82 static bool ipdbg_fifo_is_full(struct ipdbg_fifo *fifo)
84 return fifo->count == IPDBG_BUFFER_SIZE;
87 static void ipdbg_zero_rd_idx(struct ipdbg_fifo *fifo)
89 if (fifo->rd_idx == 0)
92 size_t ri = fifo->rd_idx;
93 for (size_t idx = 0 ; idx < fifo->count ; ++idx)
94 fifo->buffer[idx] = fifo->buffer[ri++];
98 static void ipdbg_append_to_fifo(struct ipdbg_fifo *fifo, char data)
100 if (ipdbg_fifo_is_full(fifo))
103 ipdbg_zero_rd_idx(fifo);
104 fifo->buffer[fifo->count++] = data;
107 static char ipdbg_get_from_fifo(struct ipdbg_fifo *fifo)
109 if (ipdbg_fifo_is_empty(fifo))
113 return fifo->buffer[fifo->rd_idx++];
116 static int ipdbg_move_buffer_to_connection(struct connection *conn, struct ipdbg_fifo *fifo)
118 if (ipdbg_fifo_is_empty(fifo))
121 struct ipdbg_connection *connection = conn->priv;
122 if (connection->closed)
123 return ERROR_SERVER_REMOTE_CLOSED;
125 ipdbg_zero_rd_idx(fifo);
126 size_t bytes_written = connection_write(conn, fifo->buffer, fifo->count);
127 if (bytes_written != fifo->count) {
128 LOG_ERROR("error during write: %zu != %zu", bytes_written, fifo->count);
129 connection->closed = true;
130 return ERROR_SERVER_REMOTE_CLOSED;
133 fifo->count -= bytes_written;
138 static int ipdbg_max_tools_from_data_register_length(uint8_t data_register_length)
141 data_register_length -= 10; /* 8 bit payload, 1 xoff-flag, 1 valid-flag; remaining bits used to select tool*/
142 while (data_register_length--)
145 /* last tool is used to reset JtagCDC and transfer "XON" to host*/
146 return max_tools - 1;
149 static struct ipdbg_service *ipdbg_find_service(struct ipdbg_hub *hub, uint8_t tool)
151 struct ipdbg_service *service;
152 for (service = ipdbg_first_service ; service ; service = service->next) {
153 if (service->hub == hub && service->tool == tool)
159 static void ipdbg_add_service(struct ipdbg_service *service)
161 struct ipdbg_service *iservice;
162 if (ipdbg_first_service) {
163 for (iservice = ipdbg_first_service ; iservice->next; iservice = iservice->next)
165 iservice->next = service;
167 ipdbg_first_service = service;
170 static int ipdbg_create_service(struct ipdbg_hub *hub, uint8_t tool, struct ipdbg_service **service, uint16_t port)
172 *service = calloc(1, sizeof(struct ipdbg_service));
174 LOG_ERROR("Out of memory");
178 (*service)->hub = hub;
179 (*service)->tool = tool;
180 (*service)->port = port;
185 static int ipdbg_remove_service(struct ipdbg_service *service)
187 if (!ipdbg_first_service)
190 if (service == ipdbg_first_service) {
191 ipdbg_first_service = ipdbg_first_service->next;
195 for (struct ipdbg_service *iservice = ipdbg_first_service ; iservice->next ; iservice = iservice->next) {
196 if (service == iservice->next) {
197 iservice->next = service->next;
204 static struct ipdbg_hub *ipdbg_find_hub(struct jtag_tap *tap,
205 uint32_t user_instruction, struct ipdbg_virtual_ir_info *virtual_ir)
207 struct ipdbg_hub *hub = NULL;
208 for (hub = ipdbg_first_hub ; hub ; hub = hub->next) {
209 if (hub->tap == tap && hub->user_instruction == user_instruction) {
210 if ((!virtual_ir && !hub->virtual_ir) ||
211 (virtual_ir && hub->virtual_ir &&
212 virtual_ir->instruction == hub->virtual_ir->instruction &&
213 virtual_ir->length == hub->virtual_ir->length &&
214 virtual_ir->value == hub->virtual_ir->value)) {
222 static void ipdbg_add_hub(struct ipdbg_hub *hub)
224 struct ipdbg_hub *ihub;
225 if (ipdbg_first_hub) {
226 for (ihub = ipdbg_first_hub ; ihub->next; ihub = ihub->next)
230 ipdbg_first_hub = hub;
233 static int ipdbg_create_hub(struct jtag_tap *tap, uint32_t user_instruction, uint8_t data_register_length,
234 struct ipdbg_virtual_ir_info *virtual_ir, struct ipdbg_hub **hub)
237 struct ipdbg_hub *new_hub = calloc(1, sizeof(struct ipdbg_hub));
240 LOG_ERROR("Out of memory");
244 new_hub->max_tools = ipdbg_max_tools_from_data_register_length(data_register_length);
245 new_hub->connections = calloc(new_hub->max_tools, sizeof(struct connection *));
246 if (!new_hub->connections) {
249 LOG_ERROR("Out of memory");
253 new_hub->user_instruction = user_instruction;
254 new_hub->data_register_length = data_register_length;
255 new_hub->valid_mask = BIT(data_register_length - 1);
256 new_hub->xoff_mask = BIT(data_register_length - 2);
257 new_hub->tool_mask = (new_hub->xoff_mask - 1) >> 8;
258 new_hub->last_dn_tool = new_hub->tool_mask;
259 new_hub->virtual_ir = virtual_ir;
266 static void ipdbg_free_hub(struct ipdbg_hub *hub)
270 free(hub->connections);
271 free(hub->virtual_ir);
275 static int ipdbg_remove_hub(struct ipdbg_hub *hub)
277 if (!ipdbg_first_hub)
279 if (hub == ipdbg_first_hub) {
280 ipdbg_first_hub = ipdbg_first_hub->next;
284 for (struct ipdbg_hub *ihub = ipdbg_first_hub ; ihub->next ; ihub = ihub->next) {
285 if (hub == ihub->next) {
286 ihub->next = hub->next;
294 static void ipdbg_init_scan_field(struct scan_field *fields, uint8_t *in_value, int num_bits, const uint8_t *out_value)
296 fields->check_mask = NULL;
297 fields->check_value = NULL;
298 fields->in_value = in_value;
299 fields->num_bits = num_bits;
300 fields->out_value = out_value;
303 static int ipdbg_shift_instr(struct ipdbg_hub *hub, uint32_t instr)
308 struct jtag_tap *tap = hub->tap;
312 if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) == instr) {
313 /* there is already the requested instruction in the ir */
317 uint8_t *ir_out_val = calloc(DIV_ROUND_UP(tap->ir_length, 8), 1);
318 buf_set_u32(ir_out_val, 0, tap->ir_length, instr);
320 struct scan_field fields;
321 ipdbg_init_scan_field(&fields, NULL, tap->ir_length, ir_out_val);
322 jtag_add_ir_scan(tap, &fields, TAP_IDLE);
323 int retval = jtag_execute_queue();
330 static int ipdbg_shift_vir(struct ipdbg_hub *hub)
335 if (!hub->virtual_ir)
338 int retval = ipdbg_shift_instr(hub, hub->virtual_ir->instruction);
339 if (retval != ERROR_OK)
342 struct jtag_tap *tap = hub->tap;
346 uint8_t *dr_out_val = calloc(DIV_ROUND_UP(hub->virtual_ir->length, 8), 1);
347 buf_set_u32(dr_out_val, 0, hub->virtual_ir->length, hub->virtual_ir->value);
349 struct scan_field fields;
350 ipdbg_init_scan_field(&fields, NULL, hub->virtual_ir->length, dr_out_val);
351 jtag_add_dr_scan(tap, 1, &fields, TAP_IDLE);
352 retval = jtag_execute_queue();
359 static int ipdbg_shift_data(struct ipdbg_hub *hub, uint32_t dn_data, uint32_t *up_data)
364 struct jtag_tap *tap = hub->tap;
368 uint8_t *dr_out_val = calloc(DIV_ROUND_UP(hub->data_register_length, 8), 1);
369 buf_set_u32(dr_out_val, 0, hub->data_register_length, dn_data);
370 uint8_t *dr_in_val = up_data ? calloc(DIV_ROUND_UP(hub->data_register_length, 8), 1) : NULL;
372 struct scan_field fields;
373 ipdbg_init_scan_field(&fields, dr_in_val, hub->data_register_length, dr_out_val);
374 jtag_add_dr_scan(tap, 1, &fields, TAP_IDLE);
375 int retval = jtag_execute_queue();
377 if (up_data && retval == ERROR_OK)
378 *up_data = buf_get_u32(dr_in_val, 0, hub->data_register_length);
386 static int ipdbg_distribute_data_from_hub(struct ipdbg_hub *hub, uint32_t up)
388 const bool valid_up_data = up & hub->valid_mask;
392 const size_t tool = (up >> 8) & hub->tool_mask;
393 if (tool == hub->tool_mask) {
394 const uint8_t xon_cmd = up & 0x00ff;
395 hub->dn_xoff &= ~xon_cmd;
396 LOG_INFO("received xon cmd: %d\n", xon_cmd);
400 struct connection *conn = hub->connections[tool];
402 struct ipdbg_connection *connection = conn->priv;
403 if (ipdbg_fifo_is_full(&connection->up_fifo)) {
404 int retval = ipdbg_move_buffer_to_connection(conn, &connection->up_fifo);
405 if (retval != ERROR_OK)
408 ipdbg_append_to_fifo(&connection->up_fifo, up);
413 static int ipdbg_jtag_transfer_byte(struct ipdbg_hub *hub, size_t tool, struct ipdbg_connection *connection)
415 uint32_t dn = hub->valid_mask | ((tool & hub->tool_mask) << 8) |
416 (0x00fful & ipdbg_get_from_fifo(&connection->dn_fifo));
418 int ret = ipdbg_shift_data(hub, dn, &up);
422 ret = ipdbg_distribute_data_from_hub(hub, up);
426 if ((up & hub->xoff_mask) && (hub->last_dn_tool != hub->max_tools)) {
427 hub->dn_xoff |= BIT(hub->last_dn_tool);
428 LOG_INFO("tool %d sent xoff", hub->last_dn_tool);
431 hub->last_dn_tool = tool;
436 static int ipdbg_polling_callback(void *priv)
438 struct ipdbg_hub *hub = priv;
440 int ret = ipdbg_shift_vir(hub);
444 ret = ipdbg_shift_instr(hub, hub->user_instruction);
448 /* transfer dn buffers to jtag-hub */
449 unsigned int num_transfers = 0;
450 for (size_t tool = 0 ; tool < hub->max_tools ; ++tool) {
451 struct connection *conn = hub->connections[tool];
452 if (conn && conn->priv) {
453 struct ipdbg_connection *connection = conn->priv;
454 while (((hub->dn_xoff & BIT(tool)) == 0) && !ipdbg_fifo_is_empty(&connection->dn_fifo)) {
455 ret = ipdbg_jtag_transfer_byte(hub, tool, connection);
463 /* some transfers to get data from jtag-hub in case there is no dn data */
464 while (num_transfers++ < hub->max_tools) {
468 int retval = ipdbg_shift_data(hub, dn, &up);
469 if (retval != ERROR_OK)
472 retval = ipdbg_distribute_data_from_hub(hub, up);
473 if (retval != ERROR_OK)
477 /* write from up fifos to sockets */
478 for (size_t tool = 0 ; tool < hub->max_tools ; ++tool) {
479 struct connection *conn = hub->connections[tool];
480 if (conn && conn->priv) {
481 struct ipdbg_connection *connection = conn->priv;
482 int retval = ipdbg_move_buffer_to_connection(conn, &connection->up_fifo);
483 if (retval != ERROR_OK)
491 static int ipdbg_start_polling(struct ipdbg_service *service, struct connection *connection)
493 struct ipdbg_hub *hub = service->hub;
494 hub->connections[service->tool] = connection;
495 hub->active_connections++;
496 if (hub->active_connections > 1) {
497 /* hub is already initialized */
501 const uint32_t reset_hub = hub->valid_mask | ((hub->max_tools) << 8);
503 int ret = ipdbg_shift_vir(hub);
507 ret = ipdbg_shift_instr(hub, hub->user_instruction);
511 ret = ipdbg_shift_data(hub, reset_hub, NULL);
512 hub->last_dn_tool = hub->tool_mask;
517 LOG_INFO("IPDBG start_polling");
519 const int time_ms = 20;
520 const int periodic = 1;
521 return target_register_timer_callback(ipdbg_polling_callback, time_ms, periodic, hub);
524 static int ipdbg_stop_polling(struct ipdbg_service *service)
526 struct ipdbg_hub *hub = service->hub;
527 hub->connections[service->tool] = NULL;
528 hub->active_connections--;
529 if (hub->active_connections == 0) {
530 LOG_INFO("IPDBG stop_polling");
532 return target_unregister_timer_callback(ipdbg_polling_callback, hub);
538 static int ipdbg_on_new_connection(struct connection *connection)
540 struct ipdbg_service *service = connection->service->priv;
541 connection->priv = &service->connection;
542 /* initialize ipdbg connection information */
543 ipdbg_init_fifo(&service->connection.up_fifo);
544 ipdbg_init_fifo(&service->connection.dn_fifo);
546 int retval = ipdbg_start_polling(service, connection);
547 if (retval != ERROR_OK) {
548 LOG_ERROR("BUG: ipdbg_start_polling failed");
552 struct ipdbg_connection *conn = connection->priv;
553 conn->closed = false;
555 LOG_INFO("New IPDBG Connection");
560 static int ipdbg_on_connection_input(struct connection *connection)
562 struct ipdbg_connection *conn = connection->priv;
563 struct ipdbg_fifo *fifo = &conn->dn_fifo;
565 if (ipdbg_fifo_is_full(fifo))
568 ipdbg_zero_rd_idx(fifo);
569 int bytes_read = connection_read(connection, fifo->buffer + fifo->count, IPDBG_BUFFER_SIZE - fifo->count);
570 if (bytes_read <= 0) {
572 LOG_ERROR("error during read: %s", strerror(errno));
573 return ERROR_SERVER_REMOTE_CLOSED;
576 fifo->count += bytes_read;
581 static int ipdbg_on_connection_closed(struct connection *connection)
583 struct ipdbg_connection *conn = connection->priv;
585 LOG_INFO("Closed IPDBG Connection");
587 return ipdbg_stop_polling(connection->service->priv);
590 static const struct service_driver ipdbg_service_driver = {
592 .new_connection_during_keep_alive_handler = NULL,
593 .new_connection_handler = ipdbg_on_new_connection,
594 .input_handler = ipdbg_on_connection_input,
595 .connection_closed_handler = ipdbg_on_connection_closed,
596 .keep_client_alive_handler = NULL,
599 static int ipdbg_start(uint16_t port, struct jtag_tap *tap, uint32_t user_instruction,
600 uint8_t data_register_length, struct ipdbg_virtual_ir_info *virtual_ir, uint8_t tool)
602 LOG_INFO("starting ipdbg service on port %d for tool %d", port, tool);
604 struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
607 if (hub->data_register_length != data_register_length) {
608 LOG_DEBUG("hub must have the same data_register_length for all tools");
612 int retval = ipdbg_create_hub(tap, user_instruction, data_register_length, virtual_ir, &hub);
613 if (retval != ERROR_OK) {
619 struct ipdbg_service *service = NULL;
620 int retval = ipdbg_create_service(hub, tool, &service, port);
622 if (retval != ERROR_OK || !service) {
623 if (hub->active_services == 0 && hub->active_connections == 0)
628 char port_str_buffer[IPDBG_TCP_PORT_STR_MAX_LENGTH];
629 snprintf(port_str_buffer, IPDBG_TCP_PORT_STR_MAX_LENGTH, "%u", port);
630 retval = add_service(&ipdbg_service_driver, port_str_buffer, 1, service);
631 if (retval == ERROR_OK) {
632 ipdbg_add_service(service);
633 if (hub->active_services == 0 && hub->active_connections == 0)
635 hub->active_services++;
637 if (hub->active_services == 0 && hub->active_connections == 0)
645 static int ipdbg_stop(struct jtag_tap *tap, uint32_t user_instruction,
646 struct ipdbg_virtual_ir_info *virtual_ir, uint8_t tool)
648 struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
653 struct ipdbg_service *service = ipdbg_find_service(hub, tool);
657 int retval = ipdbg_remove_service(service);
658 if (retval != ERROR_OK) {
659 LOG_ERROR("BUG: ipdbg_remove_service failed");
663 char port_str_buffer[IPDBG_TCP_PORT_STR_MAX_LENGTH];
664 snprintf(port_str_buffer, IPDBG_TCP_PORT_STR_MAX_LENGTH, "%u", service->port);
665 retval = remove_service("ipdbg", port_str_buffer);
666 /* The ipdbg_service structure is freed by server.c:remove_service().
667 There the "priv" pointer is freed.*/
668 if (retval != ERROR_OK) {
669 LOG_ERROR("BUG: remove_service failed");
672 hub->active_services--;
673 if (hub->active_connections == 0 && hub->active_services == 0) {
674 retval = ipdbg_remove_hub(hub);
675 if (retval != ERROR_OK) {
676 LOG_ERROR("BUG: ipdbg_remove_hub failed");
684 COMMAND_HANDLER(handle_ipdbg_command)
686 struct jtag_tap *tap = NULL;
687 uint16_t port = 4242;
689 uint32_t user_instruction = 0x00;
690 uint8_t data_register_length = IPDBG_MAX_DR_LENGTH;
692 bool hub_configured = false;
693 bool has_virtual_ir = false;
694 uint32_t virtual_ir_instruction = 0x00e;
695 uint32_t virtual_ir_length = 5;
696 uint32_t virtual_ir_value = 0x11;
697 struct ipdbg_virtual_ir_info *virtual_ir = NULL;
699 if ((CMD_ARGC < IPDBG_MIN_NUM_OF_OPTIONS) || (CMD_ARGC > IPDBG_MAX_NUM_OF_OPTIONS))
700 return ERROR_COMMAND_SYNTAX_ERROR;
702 for (unsigned int i = 0; i < CMD_ARGC; ++i) {
703 if (strcmp(CMD_ARGV[i], "-tap") == 0) {
704 if (i + 1 >= CMD_ARGC || CMD_ARGV[i + 1][0] == '-') {
705 command_print(CMD, "no TAP given");
708 tap = jtag_tap_by_string(CMD_ARGV[i + 1]);
710 command_print(CMD, "Tap %s unknown", CMD_ARGV[i + 1]);
714 } else if (strcmp(CMD_ARGV[i], "-hub") == 0) {
715 COMMAND_PARSE_ADDITIONAL_NUMBER(u32, i, user_instruction, "ir_value to select hub");
716 hub_configured = true;
717 COMMAND_PARSE_OPTIONAL_NUMBER(u8, i, data_register_length);
718 if (data_register_length < IPDBG_MIN_DR_LENGTH ||
719 data_register_length > IPDBG_MAX_DR_LENGTH) {
720 command_print(CMD, "length of \"user\"-data register must be at least %d and at most %d.",
721 IPDBG_MIN_DR_LENGTH, IPDBG_MAX_DR_LENGTH);
724 } else if (strcmp(CMD_ARGV[i], "-vir") == 0) {
725 COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_value);
726 COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_length);
727 COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_instruction);
728 has_virtual_ir = true;
729 } else if (strcmp(CMD_ARGV[i], "-port") == 0) {
730 COMMAND_PARSE_ADDITIONAL_NUMBER(u16, i, port, "port number");
731 } else if (strcmp(CMD_ARGV[i], "-tool") == 0) {
732 COMMAND_PARSE_ADDITIONAL_NUMBER(u8, i, tool, "tool");
733 } else if (strcmp(CMD_ARGV[i], "-stop") == 0) {
735 } else if (strcmp(CMD_ARGV[i], "-start") == 0) {
738 command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
744 command_print(CMD, "no valid tap selected");
748 if (!hub_configured) {
749 command_print(CMD, "hub not configured correctly");
753 if (tool >= ipdbg_max_tools_from_data_register_length(data_register_length)) {
754 command_print(CMD, "Tool: %d is invalid", tool);
758 if (has_virtual_ir) {
759 virtual_ir = calloc(1, sizeof(struct ipdbg_virtual_ir_info));
761 LOG_ERROR("Out of memory");
764 virtual_ir->instruction = virtual_ir_instruction;
765 virtual_ir->length = virtual_ir_length;
766 virtual_ir->value = virtual_ir_value;
770 return ipdbg_start(port, tap, user_instruction, data_register_length, virtual_ir, tool);
772 return ipdbg_stop(tap, user_instruction, virtual_ir, tool);
775 static const struct command_registration ipdbg_command_handlers[] = {
778 .handler = handle_ipdbg_command,
779 .mode = COMMAND_EXEC,
780 .help = "Starts or stops an IPDBG JTAG-Host server.",
781 .usage = "[-start|-stop] -tap device.tap -hub ir_value [dr_length]"
782 " [-port number] [-tool number] [-vir [vir_value [length [instr_code]]]]",
784 COMMAND_REGISTRATION_DONE
787 int ipdbg_register_commands(struct command_context *cmd_ctx)
789 return register_commands(cmd_ctx, NULL, ipdbg_command_handlers);