ipdbg: improve ipdbg-host speed

By queuing multiple jtag transfers the connection speed between
JTAG-Host and JTAG-Hub is improved. This is due to much less
calls to OS functions. An improvement of about x30 has been
measured with ftdi-based jtag adapters

For this to work the JTAG-Host server needs to know if flow control
is enabled on the JTAG-Hub ports. This is possible with newer
JTAG-Hub/JtagCDC. For old JTAG-Hubs the queuing is not enabled so
this change is backwards compatible.

Change-Id: I8a5108adbe2a2c1e3d3620b5c9ff77a546bfc14e
Signed-off-by: Daniel Anselmi <danselmi@gmx.ch>
Reviewed-on: https://review.openocd.org/c/openocd/+/7978
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

diff --git a/src/server/ipdbg.c b/src/server/ipdbg.c
index c1bdb2939555bb8a1543dbb768d6be08ab80d66d..0733230322456a1b5de5a20feffb5d2bfb111b39 100644 (file)
--- a/src/server/ipdbg.c
+++ b/src/server/ipdbg.c
@@ -20,6 +20,17 @@
 #define IPDBG_MIN_DR_LENGTH 11
 #define IPDBG_MAX_DR_LENGTH 13
 #define IPDBG_TCP_PORT_STR_MAX_LENGTH 6
+#define IPDBG_SCRATCH_MEMORY_SIZE 1024
+#define IPDBG_EMPTY_DOWN_TRANSFERS 1024
+#define IPDBG_CONSECUTIVE_UP_TRANSFERS 1024
+
+#if IPDBG_SCRATCH_MEMORY_SIZE < IPDBG_EMPTY_DOWN_TRANSFERS
+#error "scratch Memory must be at least IPDBG_EMPTY_DOWN_TRANSFERS"
+#endif
+
+#if IPDBG_SCRATCH_MEMORY_SIZE < IPDBG_CONSECUTIVE_UP_TRANSFERS
+#error "scratch Memory must be at least IPDBG_CONSECUTIVE_UP_TRANSFERS"
+#endif
 
 /* private connection data for IPDBG */
 struct ipdbg_fifo {
@@ -48,6 +59,13 @@ struct ipdbg_virtual_ir_info {
        uint32_t value;
 };
 
+struct ipdbg_hub_scratch_memory {
+       uint8_t *dr_out_vals;
+       uint8_t *dr_in_vals;
+       uint8_t *vir_out_val;
+       struct scan_field *fields;
+};
+
 struct ipdbg_hub {
        uint32_t user_instruction;
        uint32_t max_tools;
@@ -62,7 +80,9 @@ struct ipdbg_hub {
        struct connection **connections;
        uint8_t data_register_length;
        uint8_t dn_xoff;
+       uint8_t flow_control_enabled;
        struct ipdbg_virtual_ir_info *virtual_ir;
+       struct ipdbg_hub_scratch_memory scratch_memory;
 };
 
 static struct ipdbg_hub *ipdbg_first_hub;
@@ -236,20 +256,28 @@ static int ipdbg_create_hub(struct jtag_tap *tap, uint32_t user_instruction, uin
 {
        *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;
-       }
+       if (!new_hub)
+               goto mem_err_hub;
 
+       const size_t dreg_buffer_size = DIV_ROUND_UP(data_register_length, 8);
        new_hub->max_tools = ipdbg_max_tools_from_data_register_length(data_register_length);
+
+       new_hub->scratch_memory.dr_out_vals = calloc(IPDBG_SCRATCH_MEMORY_SIZE, dreg_buffer_size);
+       new_hub->scratch_memory.dr_in_vals = calloc(IPDBG_SCRATCH_MEMORY_SIZE, dreg_buffer_size);
+       new_hub->scratch_memory.fields = calloc(IPDBG_SCRATCH_MEMORY_SIZE, sizeof(struct scan_field));
        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;
-       }
+
+       if (virtual_ir)
+               new_hub->scratch_memory.vir_out_val = calloc(1, DIV_ROUND_UP(virtual_ir->length, 8));
+
+       if (!new_hub->scratch_memory.dr_out_vals || !new_hub->scratch_memory.dr_in_vals ||
+               !new_hub->scratch_memory.fields || (virtual_ir && !new_hub->scratch_memory.vir_out_val) ||
+               !new_hub->connections)
+               goto mem_err2;
+
+       if (virtual_ir)
+               buf_set_u32(new_hub->scratch_memory.vir_out_val, 0, virtual_ir->length, virtual_ir->value);
+
        new_hub->tap                  = tap;
        new_hub->user_instruction     = user_instruction;
        new_hub->data_register_length = data_register_length;
@@ -260,8 +288,19 @@ static int ipdbg_create_hub(struct jtag_tap *tap, uint32_t user_instruction, uin
        new_hub->virtual_ir           = virtual_ir;
 
        *hub = new_hub;
-
        return ERROR_OK;
+
+mem_err2:
+       free(new_hub->scratch_memory.vir_out_val);
+       free(new_hub->connections);
+       free(new_hub->scratch_memory.fields);
+       free(new_hub->scratch_memory.dr_in_vals);
+       free(new_hub->scratch_memory.dr_out_vals);
+       free(new_hub);
+mem_err_hub:
+       free(virtual_ir);
+       LOG_ERROR("Out of memory");
+       return ERROR_FAIL;
 }
 
 static void ipdbg_free_hub(struct ipdbg_hub *hub)
@@ -270,6 +309,10 @@ static void ipdbg_free_hub(struct ipdbg_hub *hub)
                return;
        free(hub->connections);
        free(hub->virtual_ir);
+       free(hub->scratch_memory.dr_out_vals);
+       free(hub->scratch_memory.dr_in_vals);
+       free(hub->scratch_memory.fields);
+       free(hub->scratch_memory.vir_out_val);
        free(hub);
 }
 
@@ -348,20 +391,11 @@ static int ipdbg_shift_vir(struct ipdbg_hub *hub)
        if (!tap)
                return ERROR_FAIL;
 
-       uint8_t *dr_out_val = calloc(DIV_ROUND_UP(hub->virtual_ir->length, 8), 1);
-       if (!dr_out_val) {
-               LOG_ERROR("Out of memory");
-               return ERROR_FAIL;
-       }
-       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);
+       ipdbg_init_scan_field(hub->scratch_memory.fields, NULL,
+               hub->virtual_ir->length, hub->scratch_memory.vir_out_val);
+       jtag_add_dr_scan(tap, 1, hub->scratch_memory.fields, TAP_IDLE);
        retval = jtag_execute_queue();
 
-       free(dr_out_val);
-
        return retval;
 }
 
@@ -374,33 +408,15 @@ static int ipdbg_shift_data(struct ipdbg_hub *hub, uint32_t dn_data, uint32_t *u
        if (!tap)
                return ERROR_FAIL;
 
-       uint8_t *dr_out_val = calloc(DIV_ROUND_UP(hub->data_register_length, 8), 1);
-       if (!dr_out_val) {
-               LOG_ERROR("Out of memory");
-               return ERROR_FAIL;
-       }
-       buf_set_u32(dr_out_val, 0, hub->data_register_length, dn_data);
-
-       uint8_t *dr_in_val = NULL;
-       if (up_data) {
-               dr_in_val = calloc(DIV_ROUND_UP(hub->data_register_length, 8), 1);
-               if (!dr_in_val) {
-                       LOG_ERROR("Out of memory");
-                       free(dr_out_val);
-                       return ERROR_FAIL;
-               }
-       }
+       buf_set_u32(hub->scratch_memory.dr_out_vals, 0, hub->data_register_length, dn_data);
 
-       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);
+       ipdbg_init_scan_field(hub->scratch_memory.fields, hub->scratch_memory.dr_in_vals,
+                                               hub->data_register_length, hub->scratch_memory.dr_out_vals);
+       jtag_add_dr_scan(tap, 1, hub->scratch_memory.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);
+               *up_data = buf_get_u32(hub->scratch_memory.dr_in_vals, 0, hub->data_register_length);
 
        return retval;
 }
@@ -432,6 +448,60 @@ static int ipdbg_distribute_data_from_hub(struct ipdbg_hub *hub, uint32_t up)
        return ERROR_OK;
 }
 
+static void ipdbg_check_for_xoff(struct ipdbg_hub *hub, size_t tool,
+                                                               uint32_t rx_data)
+{
+       if ((rx_data & 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;
+}
+
+static int ipdbg_shift_empty_data(struct ipdbg_hub *hub)
+{
+       if (!hub)
+               return ERROR_FAIL;
+
+       struct jtag_tap *tap = hub->tap;
+       if (!tap)
+               return ERROR_FAIL;
+
+       const size_t dreg_buffer_size = DIV_ROUND_UP(hub->data_register_length, 8);
+       memset(hub->scratch_memory.dr_out_vals, 0, dreg_buffer_size);
+       for (size_t i = 0; i < IPDBG_EMPTY_DOWN_TRANSFERS; ++i) {
+               ipdbg_init_scan_field(hub->scratch_memory.fields + i,
+                                                               hub->scratch_memory.dr_in_vals + i * dreg_buffer_size,
+                                                               hub->data_register_length,
+                                                               hub->scratch_memory.dr_out_vals);
+               jtag_add_dr_scan(tap, 1, hub->scratch_memory.fields + i, TAP_IDLE);
+       }
+
+       int retval = jtag_execute_queue();
+
+       if (retval == ERROR_OK) {
+               uint32_t up_data;
+               for (size_t i = 0; i < IPDBG_EMPTY_DOWN_TRANSFERS; ++i) {
+                       up_data = buf_get_u32(hub->scratch_memory.dr_in_vals +
+                                                                       i * dreg_buffer_size, 0,
+                                                                       hub->data_register_length);
+                       int rv = ipdbg_distribute_data_from_hub(hub, up_data);
+                       if (rv != ERROR_OK)
+                               retval = rv;
+
+                       if (i == 0) {
+                               /* check if xoff sent is only needed on the first transfer which
+                                  may contain the xoff of the prev down transfer.
+                               */
+                               ipdbg_check_for_xoff(hub, hub->max_tools, up_data);
+                       }
+               }
+       }
+
+       return retval;
+}
+
 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) |
@@ -445,14 +515,63 @@ static int ipdbg_jtag_transfer_byte(struct ipdbg_hub *hub, size_t tool, struct i
        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);
+       ipdbg_check_for_xoff(hub, tool, up);
+
+       return ERROR_OK;
+}
+
+static int ipdbg_jtag_transfer_bytes(struct ipdbg_hub *hub,
+                       size_t tool, struct ipdbg_connection *connection)
+{
+       if (!hub)
+               return ERROR_FAIL;
+
+       struct jtag_tap *tap = hub->tap;
+       if (!tap)
+               return ERROR_FAIL;
+
+       const size_t dreg_buffer_size = DIV_ROUND_UP(hub->data_register_length, 8);
+       size_t num_tx = (connection->dn_fifo.count < IPDBG_CONSECUTIVE_UP_TRANSFERS) ?
+               connection->dn_fifo.count : IPDBG_CONSECUTIVE_UP_TRANSFERS;
+
+       for (size_t i = 0; i < num_tx; ++i) {
+               uint32_t dn_data = hub->valid_mask | ((tool & hub->tool_mask) << 8) |
+                       (0x00fful & ipdbg_get_from_fifo(&connection->dn_fifo));
+               buf_set_u32(hub->scratch_memory.dr_out_vals + i * dreg_buffer_size, 0,
+                                       hub->data_register_length, dn_data);
+
+               ipdbg_init_scan_field(hub->scratch_memory.fields + i,
+                                                               hub->scratch_memory.dr_in_vals +
+                                                                       i * dreg_buffer_size,
+                                                               hub->data_register_length,
+                                                               hub->scratch_memory.dr_out_vals +
+                                                                       i * dreg_buffer_size);
+               jtag_add_dr_scan(tap, 1, hub->scratch_memory.fields + i, TAP_IDLE);
        }
 
-       hub->last_dn_tool = tool;
+       int retval = jtag_execute_queue();
 
-       return ERROR_OK;
+       if (retval == ERROR_OK) {
+               uint32_t up_data;
+               for (size_t i = 0; i < num_tx; ++i) {
+                       up_data = buf_get_u32(hub->scratch_memory.dr_in_vals +
+                                                                       i * dreg_buffer_size,
+                                                                       0, hub->data_register_length);
+                       int rv = ipdbg_distribute_data_from_hub(hub, up_data);
+                       if (rv != ERROR_OK)
+                               retval = rv;
+                       if (i == 0) {
+                               /* check if xoff sent is only needed on the first transfer which
+                                  may contain the xoff of the prev down transfer.
+                                  No checks for this channel because this function is only
+                                  called for channels without enabled flow control.
+                               */
+                               ipdbg_check_for_xoff(hub, tool, up_data);
+                       }
+               }
+       }
+
+       return retval;
 }
 
 static int ipdbg_polling_callback(void *priv)
@@ -468,33 +587,25 @@ static int ipdbg_polling_callback(void *priv)
                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 (hub->flow_control_enabled & BIT(tool))
+                                       ret = ipdbg_jtag_transfer_byte(hub, tool, connection);
+                               else
+                                       ret = ipdbg_jtag_transfer_bytes(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;
-       }
+       ret = ipdbg_shift_empty_data(hub);
+       if (ret != ERROR_OK)
+               return ret;
 
        /* write from up fifos to sockets */
        for (size_t tool = 0; tool < hub->max_tools; ++tool) {
@@ -510,6 +621,33 @@ static int ipdbg_polling_callback(void *priv)
        return ERROR_OK;
 }
 
+static int ipdbg_get_flow_control_info_from_hub(struct ipdbg_hub *hub)
+{
+       uint32_t up_data;
+
+       /* on older implementations the flow_control_enable_word is not sent to us.
+               so we don't know -> assume it's enabled on all channels */
+       hub->flow_control_enabled = 0x7f;
+
+       int ret = ipdbg_shift_data(hub, 0UL, &up_data);
+       if (ret != ERROR_OK)
+               return ret;
+
+       const bool valid_up_data = up_data & hub->valid_mask;
+       if (valid_up_data) {
+               const size_t tool = (up_data >> 8) & hub->tool_mask;
+               /* the first valid data from hub is flow_control_enable_word */
+               if (tool == hub->tool_mask)
+                       hub->flow_control_enabled = up_data & 0x007f;
+               else
+                       ipdbg_distribute_data_from_hub(hub, up_data);
+       }
+
+       LOG_INFO("Flow control enabled on IPDBG JTAG Hub: 0x%02x", hub->flow_control_enabled);
+
+       return ERROR_OK;
+}
+
 static int ipdbg_start_polling(struct ipdbg_service *service, struct connection *connection)
 {
        struct ipdbg_hub *hub = service->hub;
@@ -536,6 +674,10 @@ static int ipdbg_start_polling(struct ipdbg_service *service, struct connection
        if (ret != ERROR_OK)
                return ret;
 
+       ret = ipdbg_get_flow_control_info_from_hub(hub);
+       if (ret != ERROR_OK)
+               return ret;
+
        LOG_INFO("IPDBG start_polling");
 
        const int time_ms = 20;