Add IPDBG JtagHost functionality to OpenOCD

IPDBG are utilities to debug IP-cores. It uses JTAG for
transport to/from the FPGA. The different UIs use TCP/IP
as transport. The JtagHost makes the bridge between these
two.

Comparable to the bridge between GDB and the in-circuit-
debugging-unit of a micro controller.

Change-Id: Ib1bc10dcbd4ea426e492bb7b2d85c1ed1b7a8d5a
Signed-off-by: Daniel Anselmi <danselmi@gmx.ch>
Reviewed-on: http://openocd.zylin.com/5938
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

diff --git a/doc/openocd.texi b/doc/openocd.texi
index a8a413c4f461ffc66926d0677bc839832e51038c..6614f4832fb5e57f91c4ab123fb5f27591170ede 100644 (file)
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -10585,6 +10585,49 @@ If @emph{xsvfdump} shows a file is using those opcodes, it
 probably will not be usable with other XSVF tools.
 
 
+@section IPDBG: JTAG-Host server
+@cindex IPDBG JTAG-Host server
+@cindex IPDBG
+
+IPDBG is a set of tools to debug IP-Cores. It comprises, among others, a logic analyzer and an arbitrary
+waveform generator. These are synthesize-able hardware descriptions of
+logic circuits in addition to software for control, visualization and further analysis.
+In a session using JTAG for its transport protocol, OpenOCD supports the function
+of a JTAG-Host. The JTAG-Host is needed to connect the circuit over JTAG to the
+control-software. For more details see @url{http://ipdbg.org}.
+
+@deffn {Command} {ipdbg} [@option{-start|-stop}] @option{-tap @var{tapname}} @option{-hub @var{ir_value} [@var{dr_length}]} [@option{-port @var{number}}] [@option{-tool @var{number}}] [@option{-vir [@var{vir_value} [@var{length} [@var{instr_code}]]]}]
+Starts or stops a IPDBG JTAG-Host server. Arguments can be specified in any order.
+
+Command options:
+@itemize @bullet
+@item @option{-start|-stop} starts or stops a IPDBG JTAG-Host server (default: start).
+@item @option{-tap @var{tapname}} targeting the TAP @var{tapname}.
+@item @option{-hub @var{ir_value}} states that the JTAG hub is
+reachable with dr-scans while the JTAG instruction register has the value @var{ir_value}.
+@item @option{-port @var{number}} tcp port number where the JTAG-Host is listening.
+@item @option{-tool @var{number}} number of the tool/feature. These corresponds to the ports "data_(up/down)_(0..6)" at the JtagHub.
+@item @option{-vir [@var{vir_value} [@var{length} [@var{instr_code}]]]} On some devices, the user data-register is only reachable if there is a
+specific value in a second dr. This second dr is called vir (virtual ir). With this parameter given, the IPDBG satisfies this condition prior an
+access to the IPDBG-Hub. The value shifted into the vir is given by the first parameter @var{vir_value} (default: 0x11). The second
+parameter @var{length} is the length of the vir data register (default: 5). With the @var{instr_code} (default: 0x00e) parameter the ir value to
+shift data through vir can be configured.
+@end itemize
+@end deffn
+
+Examples:
+@example
+ipdbg -start -tap xc6s.tap -hub 0x02 -port 4242 -tool 4
+@end example
+Starts a server listening on tcp-port 4242 which connects to tool 4.
+The connection is through the TAP of a Xilinx Spartan 6 on USER1 instruction (tested with a papillion pro board).
+
+@example
+ipdbg -start -tap 10m50.tap -hub 0x00C -vir -port 60000 -tool 1
+@end example
+Starts a server listening on tcp-port 60000 which connects to tool 1 (data_up_1/data_down_1).
+The connection is through the TAP of a Intel MAX10 virtual jtag component (sld_instance_index is 0; sld_ir_width is smaller than 5).
+
 @node Utility Commands
 @chapter Utility Commands
 @cindex Utility Commands

diff --git a/src/helper/command.h b/src/helper/command.h
index 68f4c14fe116ff405374dce390929f51b966c40c..068df9d0e3d4ff2006f18b2a8b82ff9617f899a5 100644 (file)
--- a/src/helper/command.h
+++ b/src/helper/command.h
@@ -426,6 +426,48 @@ DECLARE_PARSE_WRAPPER(_target_addr, target_addr_t);
 #define COMMAND_PARSE_ADDRESS(in, out) \
        COMMAND_PARSE_NUMBER(target_addr, in, out)
 
+/**
+ * @brief parses the command argument at position @a argn into @a out
+ * as a @a type, or prints a command error referring to @a name_str
+ * and passes the error code to the caller. @a argn will be incremented
+ * if no error occurred. Otherwise the calling function will return
+ * the error code produced by the parsing function.
+ *
+ * This function may cause the calling function to return immediately,
+ * so it should be used carefully to avoid leaking resources.  In most
+ * situations, parsing should be completed in full before proceeding
+ * to allocate resources, and this strategy will most prevents leaks.
+ */
+#define COMMAND_PARSE_ADDITIONAL_NUMBER(type, argn, out, name_str) \
+       do { \
+               if (argn+1 >= CMD_ARGC || CMD_ARGV[argn+1][0] == '-') { \
+                       command_print(CMD, "no " name_str " given"); \
+                       return ERROR_FAIL; \
+               } \
+               ++argn; \
+               COMMAND_PARSE_NUMBER(type, CMD_ARGV[argn], out); \
+       } while (0)
+
+/**
+ * @brief parses the command argument at position @a argn into @a out
+ * as a @a type if the argument @a argn does not start with '-'.
+ * and passes the error code to the caller. @a argn will be incremented
+ * if no error occurred. Otherwise the calling function will return
+ * the error code produced by the parsing function.
+ *
+ * This function may cause the calling function to return immediately,
+ * so it should be used carefully to avoid leaking resources.  In most
+ * situations, parsing should be completed in full before proceeding
+ * to allocate resources, and this strategy will most prevents leaks.
+ */
+#define COMMAND_PARSE_OPTIONAL_NUMBER(type, argn, out) \
+       do { \
+               if (argn+1 < CMD_ARGC && CMD_ARGV[argn+1][0] != '-') { \
+                       ++argn; \
+                       COMMAND_PARSE_NUMBER(type, CMD_ARGV[argn], out); \
+               } \
+       } while (0)
+
 /**
  * Parse the string @c as a binary parameter, storing the boolean value
  * in @c out.  The strings @c on and @c off are used to match different

diff --git a/src/jtag/core.c b/src/jtag/core.c
index 37924aad03e27f917018006a05845073d720ac0a..147df28549be301b7779658d0063cfb12c86ab68 100644 (file)
--- a/src/jtag/core.c
+++ b/src/jtag/core.c
@@ -45,6 +45,9 @@
 #include "svf/svf.h"
 #include "xsvf/xsvf.h"
 
+/* ipdbg are utilities to debug IP-cores. It uses JTAG for transport. */
+#include "server/ipdbg.h"
+
 /** The number of JTAG queue flushes (for profiling and debugging purposes). */
 static int jtag_flush_queue_count;
 
@@ -1975,7 +1978,12 @@ static int jtag_select(struct command_context *ctx)
        if (retval != ERROR_OK)
                return retval;
 
-       return xsvf_register_commands(ctx);
+       retval = xsvf_register_commands(ctx);
+
+       if (retval != ERROR_OK)
+               return retval;
+
+       return ipdbg_register_commands(ctx);
 }
 
 static struct transport jtag_transport = {

diff --git a/src/server/Makefile.am b/src/server/Makefile.am
index d270ee2811f4cd5a19c0d7d919629d75ec8b4f3c..5f7469a8495db91f793037aa88f2e3046ea5fd7d 100644 (file)
--- a/src/server/Makefile.am
+++ b/src/server/Makefile.am
@@ -10,7 +10,9 @@ noinst_LTLIBRARIES += %D%/libserver.la
        %D%/tcl_server.c \
        %D%/tcl_server.h \
        %D%/rtt_server.c \
-       %D%/rtt_server.h
+       %D%/rtt_server.h \
+       %D%/ipdbg.c \
+       %D%/ipdbg.h
 
 %C%_libserver_la_CFLAGS = $(AM_CFLAGS)
 if IS_MINGW

diff --git a/src/server/ipdbg.c b/src/server/ipdbg.c
new file mode 100644 (file)
index 0000000..ec2fae8
--- /dev/null
+++ b/src/server/ipdbg.c
@@ -0,0 +1,782 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/* Copyright (C) 2020 by Daniel Anselmi <danselmi@gmx.ch> */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/bits.h>
+#include <helper/time_support.h>
+#include <jtag/jtag.h>
+#include <server/server.h>
+#include <target/target.h>
+
+#include "ipdbg.h"
+
+#define IPDBG_BUFFER_SIZE 16384
+#define IPDBG_MIN_NUM_OF_OPTIONS 4
+#define IPDBG_MAX_NUM_OF_OPTIONS 14
+#define IPDBG_MIN_DR_LENGTH 11
+#define IPDBG_MAX_DR_LENGTH 13
+#define IPDBG_TCP_PORT_STR_MAX_LENGTH 6
+
+/* private connection data for IPDBG */
+struct ipdbg_fifo {
+       size_t count;
+       size_t rd_idx;
+       char buffer[IPDBG_BUFFER_SIZE];
+};
+
+struct ipdbg_connection {
+       struct ipdbg_fifo dn_fifo;
+       struct ipdbg_fifo up_fifo;
+       bool closed;
+};
+
+struct ipdbg_service {
+       struct ipdbg_hub *hub;
+       struct ipdbg_service *next;
+       uint16_t port;
+       struct ipdbg_connection connection;
+       uint8_t tool;
+};
+
+struct ipdbg_virtual_ir_info {
+       uint32_t instruction;
+       uint32_t length;
+       uint32_t value;
+};
+
+struct ipdbg_hub {
+       uint32_t user_instruction;
+       uint32_t max_tools;
+       uint32_t active_connections;
+       uint32_t active_services;
+       uint32_t valid_mask;
+       uint32_t xoff_mask;
+       uint32_t tool_mask;
+       uint32_t last_dn_tool;
+       struct ipdbg_hub *next;
+       struct jtag_tap *tap;
+       struct connection **connections;
+       uint8_t data_register_length;
+       uint8_t dn_xoff;
+       struct ipdbg_virtual_ir_info *virtual_ir;
+};
+
+static struct ipdbg_hub *ipdbg_first_hub;
+
+static struct ipdbg_service *ipdbg_first_service;
+
+static void ipdbg_init_fifo(struct ipdbg_fifo *fifo)
+{
+       fifo->count = 0;
+       fifo->rd_idx = 0;
+}
+
+static bool ipdbg_fifo_is_empty(struct ipdbg_fifo *fifo)
+{
+       return fifo->count == 0;
+}
+
+static bool ipdbg_fifo_is_full(struct ipdbg_fifo *fifo)
+{
+       return fifo->count == IPDBG_BUFFER_SIZE;
+}
+
+static void ipdbg_zero_rd_idx(struct ipdbg_fifo *fifo)
+{
+       if (fifo->rd_idx == 0)
+               return;
+
+       size_t ri = fifo->rd_idx;
+       for (size_t idx = 0 ; idx < fifo->count ; ++idx)
+               fifo->buffer[idx] = fifo->buffer[ri++];
+       fifo->rd_idx = 0;
+}
+
+static void ipdbg_append_to_fifo(struct ipdbg_fifo *fifo, char data)
+{
+       if (ipdbg_fifo_is_full(fifo))
+               return;
+
+       ipdbg_zero_rd_idx(fifo);
+       fifo->buffer[fifo->count++] = data;
+}
+
+static char ipdbg_get_from_fifo(struct ipdbg_fifo *fifo)
+{
+       if (ipdbg_fifo_is_empty(fifo))
+               return 0;
+
+       fifo->count--;
+       return fifo->buffer[fifo->rd_idx++];
+}
+
+static int ipdbg_move_buffer_to_connection(struct connection *conn, struct ipdbg_fifo *fifo)
+{
+       if (ipdbg_fifo_is_empty(fifo))
+               return ERROR_OK;
+
+       struct ipdbg_connection *connection = conn->priv;
+       if (connection->closed)
+               return ERROR_SERVER_REMOTE_CLOSED;
+
+       ipdbg_zero_rd_idx(fifo);
+       size_t bytes_written = connection_write(conn, fifo->buffer, fifo->count);
+       if (bytes_written != fifo->count) {
+               LOG_ERROR("error during write: %zu != %zu", bytes_written, fifo->count);
+               connection->closed = true;
+               return ERROR_SERVER_REMOTE_CLOSED;
+       }
+
+       fifo->count -= bytes_written;
+
+       return ERROR_OK;
+}
+
+static int ipdbg_max_tools_from_data_register_length(uint8_t data_register_length)
+{
+       int max_tools = 1;
+       data_register_length -= 10; /* 8 bit payload, 1 xoff-flag, 1 valid-flag; remaining bits used to select tool*/
+       while (data_register_length--)
+               max_tools *= 2;
+
+       /* last tool is used to reset JtagCDC and transfer "XON" to host*/
+       return max_tools - 1;
+}
+
+static struct ipdbg_service *ipdbg_find_service(struct ipdbg_hub *hub, uint8_t tool)
+{
+       struct ipdbg_service *service;
+       for (service = ipdbg_first_service ; service ; service = service->next) {
+               if (service->hub == hub && service->tool == tool)
+                       break;
+       }
+       return service;
+}
+
+static void ipdbg_add_service(struct ipdbg_service *service)
+{
+       struct ipdbg_service *iservice;
+       if (ipdbg_first_service) {
+               for (iservice = ipdbg_first_service ; iservice->next; iservice = iservice->next)
+                       ;
+               iservice->next = service;
+       } else
+               ipdbg_first_service = service;
+}
+
+static int ipdbg_create_service(struct ipdbg_hub *hub, uint8_t tool, struct ipdbg_service **service, uint16_t port)
+{
+       *service = calloc(1, sizeof(struct ipdbg_service));
+       if (!*service) {
+               LOG_ERROR("Out of memory");
+               return ERROR_FAIL;
+       }
+
+       (*service)->hub = hub;
+       (*service)->tool = tool;
+       (*service)->port = port;
+
+       return ERROR_OK;
+}
+
+static int ipdbg_remove_service(struct ipdbg_service *service)
+{
+       if (!ipdbg_first_service)
+               return ERROR_FAIL;
+
+       if (service == ipdbg_first_service) {
+               ipdbg_first_service = ipdbg_first_service->next;
+               return ERROR_OK;
+       }
+
+       for (struct ipdbg_service *iservice = ipdbg_first_service ; iservice->next ; iservice = iservice->next) {
+               if (service == iservice->next) {
+                       iservice->next = service->next;
+                       return ERROR_OK;
+               }
+       }
+       return ERROR_FAIL;
+}
+
+static struct ipdbg_hub *ipdbg_find_hub(struct jtag_tap *tap,
+                               uint32_t user_instruction, struct ipdbg_virtual_ir_info *virtual_ir)
+{
+       struct ipdbg_hub *hub = NULL;
+       for (hub = ipdbg_first_hub ; hub ; hub = hub->next) {
+               if (hub->tap == tap && hub->user_instruction == user_instruction) {
+                       if ((!virtual_ir && !hub->virtual_ir) ||
+                                (virtual_ir && hub->virtual_ir &&
+                                 virtual_ir->instruction == hub->virtual_ir->instruction &&
+                                 virtual_ir->length == hub->virtual_ir->length &&
+                                 virtual_ir->value == hub->virtual_ir->value)) {
+                               break;
+                       }
+               }
+       }
+       return hub;
+}
+
+static void ipdbg_add_hub(struct ipdbg_hub *hub)
+{
+       struct ipdbg_hub *ihub;
+       if (ipdbg_first_hub) {
+               for (ihub = ipdbg_first_hub ; ihub->next; ihub = ihub->next)
+                       ;
+               ihub->next = hub;
+       } else
+               ipdbg_first_hub = hub;
+}
+
+static int ipdbg_create_hub(struct jtag_tap *tap, uint32_t user_instruction, uint8_t data_register_length,
+                                         struct ipdbg_virtual_ir_info *virtual_ir, struct ipdbg_hub **hub)
+{
+       *hub = NULL;
+       struct ipdbg_hub *new_hub = calloc(1, sizeof(struct ipdbg_hub));
+       if (!new_hub) {
+               free(virtual_ir);
+               LOG_ERROR("Out of memory");
+               return ERROR_FAIL;
+       }
+
+       new_hub->max_tools = ipdbg_max_tools_from_data_register_length(data_register_length);
+       new_hub->connections = calloc(new_hub->max_tools, sizeof(struct connection *));
+       if (!new_hub->connections) {
+               free(virtual_ir);
+               free(new_hub);
+               LOG_ERROR("Out of memory");
+               return ERROR_FAIL;
+       }
+       new_hub->tap                  = tap;
+       new_hub->user_instruction     = user_instruction;
+       new_hub->data_register_length = data_register_length;
+       new_hub->valid_mask           = BIT(data_register_length - 1);
+       new_hub->xoff_mask            = BIT(data_register_length - 2);
+       new_hub->tool_mask            = (new_hub->xoff_mask - 1) >> 8;
+       new_hub->last_dn_tool         = new_hub->tool_mask;
+       new_hub->virtual_ir           = virtual_ir;
+
+       *hub = new_hub;
+
+       return ERROR_OK;
+}
+
+static void ipdbg_free_hub(struct ipdbg_hub *hub)
+{
+       if (!hub)
+               return;
+       free(hub->connections);
+       free(hub->virtual_ir);
+       free(hub);
+}
+
+static int ipdbg_remove_hub(struct ipdbg_hub *hub)
+{
+       if (!ipdbg_first_hub)
+               return ERROR_FAIL;
+       if (hub == ipdbg_first_hub) {
+               ipdbg_first_hub = ipdbg_first_hub->next;
+               return ERROR_OK;
+       }
+
+       for (struct ipdbg_hub *ihub = ipdbg_first_hub ; ihub->next ; ihub = ihub->next) {
+               if (hub == ihub->next) {
+                       ihub->next = hub->next;
+                       return ERROR_OK;
+               }
+       }
+
+       return ERROR_FAIL;
+}
+
+static void ipdbg_init_scan_field(struct scan_field *fields, uint8_t *in_value, int num_bits, const uint8_t *out_value)
+{
+       fields->check_mask = NULL;
+       fields->check_value = NULL;
+       fields->in_value = in_value;
+       fields->num_bits = num_bits;
+       fields->out_value = out_value;
+}
+
+static int ipdbg_shift_instr(struct ipdbg_hub *hub, uint32_t instr)
+{
+       if (!hub)
+               return ERROR_FAIL;
+
+       struct jtag_tap *tap = hub->tap;
+       if (!tap)
+               return ERROR_FAIL;
+
+       if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) == instr) {
+               /* there is already the requested instruction in the ir */
+               return ERROR_OK;
+       }
+
+       uint8_t *ir_out_val = calloc(DIV_ROUND_UP(tap->ir_length, 8), 1);
+       buf_set_u32(ir_out_val, 0, tap->ir_length, instr);
+
+       struct scan_field fields;
+       ipdbg_init_scan_field(&fields, NULL, tap->ir_length, ir_out_val);
+       jtag_add_ir_scan(tap, &fields, TAP_IDLE);
+       int retval = jtag_execute_queue();
+
+       free(ir_out_val);
+
+       return retval;
+}
+
+static int ipdbg_shift_vir(struct ipdbg_hub *hub)
+{
+       if (!hub)
+               return ERROR_FAIL;
+
+       if (!hub->virtual_ir)
+               return ERROR_OK;
+
+       int retval = ipdbg_shift_instr(hub, hub->virtual_ir->instruction);
+       if (retval != ERROR_OK)
+               return retval;
+
+       struct jtag_tap *tap = hub->tap;
+       if (!tap)
+               return ERROR_FAIL;
+
+       uint8_t *dr_out_val = calloc(DIV_ROUND_UP(hub->virtual_ir->length, 8), 1);
+       buf_set_u32(dr_out_val, 0, hub->virtual_ir->length, hub->virtual_ir->value);
+
+       struct scan_field fields;
+       ipdbg_init_scan_field(&fields, NULL, hub->virtual_ir->length, dr_out_val);
+       jtag_add_dr_scan(tap, 1, &fields, TAP_IDLE);
+       retval = jtag_execute_queue();
+
+       free(dr_out_val);
+
+       return retval;
+}
+
+static int ipdbg_shift_data(struct ipdbg_hub *hub, uint32_t dn_data, uint32_t *up_data)
+{
+       if (!hub)
+               return ERROR_FAIL;
+
+       struct jtag_tap *tap = hub->tap;
+       if (!tap)
+               return ERROR_FAIL;
+
+       uint8_t *dr_out_val = calloc(DIV_ROUND_UP(hub->data_register_length, 8), 1);
+       buf_set_u32(dr_out_val, 0, hub->data_register_length, dn_data);
+       uint8_t *dr_in_val = up_data ? calloc(DIV_ROUND_UP(hub->data_register_length, 8), 1) : NULL;
+
+       struct scan_field fields;
+       ipdbg_init_scan_field(&fields, dr_in_val, hub->data_register_length, dr_out_val);
+       jtag_add_dr_scan(tap, 1, &fields, TAP_IDLE);
+       int retval = jtag_execute_queue();
+
+       if (up_data && retval == ERROR_OK)
+               *up_data = buf_get_u32(dr_in_val, 0, hub->data_register_length);
+
+       free(dr_out_val);
+       free(dr_in_val);
+
+       return retval;
+}
+
+static int ipdbg_distribute_data_from_hub(struct ipdbg_hub *hub, uint32_t up)
+{
+       const bool valid_up_data = up & hub->valid_mask;
+       if (!valid_up_data)
+               return ERROR_OK;
+
+       const size_t tool = (up >> 8) & hub->tool_mask;
+       if (tool == hub->tool_mask) {
+               const uint8_t xon_cmd = up & 0x00ff;
+               hub->dn_xoff &= ~xon_cmd;
+               LOG_INFO("received xon cmd: %d\n", xon_cmd);
+               return ERROR_OK;
+       }
+
+       struct connection *conn = hub->connections[tool];
+       if (conn) {
+               struct ipdbg_connection *connection = conn->priv;
+               if (ipdbg_fifo_is_full(&connection->up_fifo)) {
+                       int retval = ipdbg_move_buffer_to_connection(conn, &connection->up_fifo);
+                       if (retval != ERROR_OK)
+                               return retval;
+               }
+               ipdbg_append_to_fifo(&connection->up_fifo, up);
+       }
+       return ERROR_OK;
+}
+
+static int ipdbg_jtag_transfer_byte(struct ipdbg_hub *hub, size_t tool, struct ipdbg_connection *connection)
+{
+       uint32_t dn = hub->valid_mask | ((tool & hub->tool_mask) << 8) |
+                               (0x00fful & ipdbg_get_from_fifo(&connection->dn_fifo));
+       uint32_t up = 0;
+       int ret = ipdbg_shift_data(hub, dn, &up);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = ipdbg_distribute_data_from_hub(hub, up);
+       if (ret != ERROR_OK)
+               return ret;
+
+       if ((up & hub->xoff_mask) && (hub->last_dn_tool != hub->max_tools)) {
+               hub->dn_xoff |= BIT(hub->last_dn_tool);
+               LOG_INFO("tool %d sent xoff", hub->last_dn_tool);
+       }
+
+       hub->last_dn_tool = tool;
+
+       return ERROR_OK;
+}
+
+static int ipdbg_polling_callback(void *priv)
+{
+       struct ipdbg_hub *hub = priv;
+
+       int ret = ipdbg_shift_vir(hub);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = ipdbg_shift_instr(hub, hub->user_instruction);
+       if (ret != ERROR_OK)
+               return ret;
+
+       /* transfer dn buffers to jtag-hub */
+       unsigned int num_transfers = 0;
+       for (size_t tool = 0 ; tool < hub->max_tools ; ++tool) {
+               struct connection *conn = hub->connections[tool];
+               if (conn && conn->priv) {
+                       struct ipdbg_connection *connection = conn->priv;
+                       while (((hub->dn_xoff & BIT(tool)) == 0) && !ipdbg_fifo_is_empty(&connection->dn_fifo)) {
+                               ret = ipdbg_jtag_transfer_byte(hub, tool, connection);
+                               if (ret != ERROR_OK)
+                                       return ret;
+                               ++num_transfers;
+                       }
+               }
+       }
+
+       /* some transfers to get data from jtag-hub in case there is no dn data */
+       while (num_transfers++ < hub->max_tools) {
+               uint32_t dn = 0;
+               uint32_t up = 0;
+
+               int retval = ipdbg_shift_data(hub, dn, &up);
+               if (retval != ERROR_OK)
+                       return ret;
+
+               retval = ipdbg_distribute_data_from_hub(hub, up);
+               if (retval != ERROR_OK)
+                       return ret;
+       }
+
+       /* write from up fifos to sockets */
+       for (size_t tool = 0 ; tool < hub->max_tools ; ++tool) {
+               struct connection *conn = hub->connections[tool];
+               if (conn && conn->priv) {
+                       struct ipdbg_connection *connection = conn->priv;
+                       int retval = ipdbg_move_buffer_to_connection(conn, &connection->up_fifo);
+                       if (retval != ERROR_OK)
+                               return retval;
+               }
+       }
+
+       return ERROR_OK;
+}
+
+static int ipdbg_start_polling(struct ipdbg_service *service, struct connection *connection)
+{
+       struct ipdbg_hub *hub = service->hub;
+       hub->connections[service->tool] = connection;
+       hub->active_connections++;
+       if (hub->active_connections > 1) {
+               /* hub is already initialized */
+               return ERROR_OK;
+       }
+
+       const uint32_t reset_hub = hub->valid_mask | ((hub->max_tools) << 8);
+
+       int ret = ipdbg_shift_vir(hub);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = ipdbg_shift_instr(hub, hub->user_instruction);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = ipdbg_shift_data(hub, reset_hub, NULL);
+       hub->last_dn_tool = hub->tool_mask;
+       hub->dn_xoff = 0;
+       if (ret != ERROR_OK)
+               return ret;
+
+       LOG_INFO("IPDBG start_polling");
+
+       const int time_ms = 20;
+       const int periodic = 1;
+       return target_register_timer_callback(ipdbg_polling_callback, time_ms, periodic, hub);
+}
+
+static int ipdbg_stop_polling(struct ipdbg_service *service)
+{
+       struct ipdbg_hub *hub = service->hub;
+       hub->connections[service->tool] = NULL;
+       hub->active_connections--;
+       if (hub->active_connections == 0) {
+               LOG_INFO("IPDBG stop_polling");
+
+               return target_unregister_timer_callback(ipdbg_polling_callback, hub);
+       }
+
+       return ERROR_OK;
+}
+
+static int ipdbg_on_new_connection(struct connection *connection)
+{
+       struct ipdbg_service *service = connection->service->priv;
+       connection->priv = &service->connection;
+       /* initialize ipdbg connection information */
+       ipdbg_init_fifo(&service->connection.up_fifo);
+       ipdbg_init_fifo(&service->connection.dn_fifo);
+
+       int retval = ipdbg_start_polling(service, connection);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("BUG: ipdbg_start_polling failed");
+               return retval;
+       }
+
+       struct ipdbg_connection *conn = connection->priv;
+       conn->closed = false;
+
+       LOG_INFO("New IPDBG Connection");
+
+       return ERROR_OK;
+}
+
+static int ipdbg_on_connection_input(struct connection *connection)
+{
+       struct ipdbg_connection *conn = connection->priv;
+       struct ipdbg_fifo *fifo = &conn->dn_fifo;
+
+       if (ipdbg_fifo_is_full(fifo))
+               return ERROR_OK;
+
+       ipdbg_zero_rd_idx(fifo);
+       int bytes_read = connection_read(connection, fifo->buffer + fifo->count, IPDBG_BUFFER_SIZE - fifo->count);
+       if (bytes_read <= 0) {
+               if (bytes_read < 0)
+                       LOG_ERROR("error during read: %s", strerror(errno));
+               return ERROR_SERVER_REMOTE_CLOSED;
+       }
+
+       fifo->count += bytes_read;
+
+       return ERROR_OK;
+}
+
+static int ipdbg_on_connection_closed(struct connection *connection)
+{
+       struct ipdbg_connection *conn = connection->priv;
+       conn->closed = true;
+       LOG_INFO("Closed IPDBG Connection");
+
+       return ipdbg_stop_polling(connection->service->priv);
+}
+
+static int ipdbg_start(uint16_t port, struct jtag_tap *tap, uint32_t user_instruction,
+                                       uint8_t data_register_length, struct ipdbg_virtual_ir_info *virtual_ir, uint8_t tool)
+{
+       LOG_INFO("starting ipdbg service on port %d for tool %d", port, tool);
+
+       struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
+       if (hub) {
+               free(virtual_ir);
+               if (hub->data_register_length != data_register_length) {
+                       LOG_DEBUG("hub must have the same data_register_length for all tools");
+                       return ERROR_FAIL;
+               }
+       } else {
+               int retval = ipdbg_create_hub(tap, user_instruction, data_register_length, virtual_ir, &hub);
+               if (retval != ERROR_OK) {
+                       free(virtual_ir);
+                       return retval;
+               }
+       }
+
+       struct ipdbg_service *service = NULL;
+       int retval = ipdbg_create_service(hub, tool, &service, port);
+
+       if (retval != ERROR_OK || !service) {
+               if (hub->active_services == 0 && hub->active_connections == 0)
+                       ipdbg_free_hub(hub);
+               return ERROR_FAIL;
+       }
+
+       char port_str_buffer[IPDBG_TCP_PORT_STR_MAX_LENGTH];
+       snprintf(port_str_buffer, IPDBG_TCP_PORT_STR_MAX_LENGTH, "%u", port);
+       retval = add_service("ipdbg", port_str_buffer, 1, &ipdbg_on_new_connection,
+               &ipdbg_on_connection_input, &ipdbg_on_connection_closed, service);
+       if (retval == ERROR_OK) {
+               ipdbg_add_service(service);
+               if (hub->active_services == 0 && hub->active_connections == 0)
+                       ipdbg_add_hub(hub);
+               hub->active_services++;
+       } else {
+               if (hub->active_services == 0 && hub->active_connections == 0)
+                       ipdbg_free_hub(hub);
+               free(service);
+       }
+
+       return retval;
+}
+
+static int ipdbg_stop(struct jtag_tap *tap, uint32_t user_instruction,
+                       struct ipdbg_virtual_ir_info *virtual_ir, uint8_t tool)
+{
+       struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
+       free(virtual_ir);
+       if (!hub)
+               return ERROR_FAIL;
+
+       struct ipdbg_service *service = ipdbg_find_service(hub, tool);
+       if (!service)
+               return ERROR_FAIL;
+
+       int retval = ipdbg_remove_service(service);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("BUG: ipdbg_remove_service failed");
+               return retval;
+       }
+
+       char port_str_buffer[IPDBG_TCP_PORT_STR_MAX_LENGTH];
+       snprintf(port_str_buffer, IPDBG_TCP_PORT_STR_MAX_LENGTH, "%u", service->port);
+       retval = remove_service("ipdbg", port_str_buffer);
+       /* The ipdbg_service structure is freed by server.c:remove_service().
+          There the "priv" pointer is freed.*/
+       if (retval != ERROR_OK) {
+               LOG_ERROR("BUG: remove_service failed");
+               return retval;
+       }
+       hub->active_services--;
+       if (hub->active_connections == 0 && hub->active_services == 0) {
+               retval = ipdbg_remove_hub(hub);
+               if (retval != ERROR_OK) {
+                       LOG_ERROR("BUG: ipdbg_remove_hub failed");
+                       return retval;
+               }
+               ipdbg_free_hub(hub);
+       }
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_ipdbg_command)
+{
+       struct jtag_tap *tap = NULL;
+       uint16_t port = 4242;
+       uint8_t tool = 1;
+       uint32_t user_instruction = 0x00;
+       uint8_t data_register_length = IPDBG_MAX_DR_LENGTH;
+       bool start = true;
+       bool hub_configured = false;
+       bool has_virtual_ir = false;
+       uint32_t virtual_ir_instruction = 0x00e;
+       uint32_t virtual_ir_length = 5;
+       uint32_t virtual_ir_value = 0x11;
+       struct ipdbg_virtual_ir_info *virtual_ir = NULL;
+
+       if ((CMD_ARGC < IPDBG_MIN_NUM_OF_OPTIONS) || (CMD_ARGC > IPDBG_MAX_NUM_OF_OPTIONS))
+               return ERROR_COMMAND_SYNTAX_ERROR;
+
+       for (unsigned int i = 0; i < CMD_ARGC; ++i) {
+               if (strcmp(CMD_ARGV[i], "-tap") == 0) {
+                       if (i + 1 >= CMD_ARGC || CMD_ARGV[i + 1][0] == '-') {
+                               command_print(CMD, "no TAP given");
+                               return ERROR_FAIL;
+                       }
+                       tap = jtag_tap_by_string(CMD_ARGV[i + 1]);
+                       if (!tap) {
+                               command_print(CMD, "Tap %s unknown", CMD_ARGV[i + 1]);
+                               return ERROR_FAIL;
+                       }
+                       ++i;
+               } else if (strcmp(CMD_ARGV[i], "-hub") == 0) {
+                       COMMAND_PARSE_ADDITIONAL_NUMBER(u32, i, user_instruction, "ir_value to select hub");
+                       hub_configured = true;
+                       COMMAND_PARSE_OPTIONAL_NUMBER(u8, i, data_register_length);
+                       if (data_register_length < IPDBG_MIN_DR_LENGTH ||
+                               data_register_length > IPDBG_MAX_DR_LENGTH) {
+                               command_print(CMD, "length of \"user\"-data register must be at least %d and at most %d.",
+                                                       IPDBG_MIN_DR_LENGTH, IPDBG_MAX_DR_LENGTH);
+                               return ERROR_FAIL;
+                       }
+               } else if (strcmp(CMD_ARGV[i], "-vir") == 0) {
+                       COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_value);
+                       COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_length);
+                       COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_instruction);
+                       has_virtual_ir = true;
+               } else if (strcmp(CMD_ARGV[i], "-port") == 0) {
+                       COMMAND_PARSE_ADDITIONAL_NUMBER(u16, i, port, "port number");
+               } else if (strcmp(CMD_ARGV[i], "-tool") == 0) {
+                       COMMAND_PARSE_ADDITIONAL_NUMBER(u8, i, tool, "tool");
+               } else if (strcmp(CMD_ARGV[i], "-stop") == 0) {
+                       start = false;
+               } else if (strcmp(CMD_ARGV[i], "-start") == 0) {
+                       start = true;
+               } else {
+                       command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
+                       return ERROR_FAIL;
+               }
+       }
+
+       if (!tap) {
+               command_print(CMD, "no valid tap selected");
+               return ERROR_FAIL;
+       }
+
+       if (!hub_configured) {
+               command_print(CMD, "hub not configured correctly");
+               return ERROR_FAIL;
+       }
+
+       if (tool >= ipdbg_max_tools_from_data_register_length(data_register_length)) {
+               command_print(CMD, "Tool: %d is invalid", tool);
+               return ERROR_FAIL;
+       }
+
+       if (has_virtual_ir) {
+               virtual_ir = calloc(1, sizeof(struct ipdbg_virtual_ir_info));
+               if (!virtual_ir) {
+                       LOG_ERROR("Out of memory");
+                       return ERROR_FAIL;
+               }
+               virtual_ir->instruction = virtual_ir_instruction;
+               virtual_ir->length      = virtual_ir_length;
+               virtual_ir->value       = virtual_ir_value;
+       }
+
+       if (start)
+               return ipdbg_start(port, tap, user_instruction, data_register_length, virtual_ir, tool);
+       else
+               return ipdbg_stop(tap, user_instruction, virtual_ir, tool);
+}
+
+static const struct command_registration ipdbg_command_handlers[] = {
+       {
+               .name = "ipdbg",
+               .handler = handle_ipdbg_command,
+               .mode = COMMAND_EXEC,
+               .help = "Starts or stops an IPDBG JTAG-Host server.",
+               .usage = "[-start|-stop] -tap device.tap -hub ir_value [dr_length]"
+                                " [-port number] [-tool number] [-vir [vir_value [length [instr_code]]]]",
+       },
+       COMMAND_REGISTRATION_DONE
+};
+
+int ipdbg_register_commands(struct command_context *cmd_ctx)
+{
+       return register_commands(cmd_ctx, NULL, ipdbg_command_handlers);
+}

diff --git a/src/server/ipdbg.h b/src/server/ipdbg.h
new file mode 100644 (file)
index 0000000..6b70545
--- /dev/null
+++ b/src/server/ipdbg.h
@@ -0,0 +1,11 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/* Copyright (C) 2020 by Daniel Anselmi <danselmi@gmx.ch> */
+
+#ifndef OPENOCD_IPDBG_IPDBG_H
+#define OPENOCD_IPDBG_IPDBG_H
+
+#include <helper/command.h>
+
+int ipdbg_register_commands(struct command_context *cmd_ctx);
+
+#endif /* OPENOCD_IPDBG_IPDBG_H */