src/server/ipdbg.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /* Copyright (C) 2020 by Daniel Anselmi <danselmi@gmx.ch> */
   3
   4 #ifdef HAVE_CONFIG_H
   5 #include "config.h"
   6 #endif
   7
   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>
  13
  14 #include "ipdbg.h"
  15
  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
  22
  23 /* private connection data for IPDBG */
  24 struct ipdbg_fifo {
  25         size_t count;
  26         size_t rd_idx;
  27         char buffer[IPDBG_BUFFER_SIZE];
  28 };
  29
  30 struct ipdbg_connection {
  31         struct ipdbg_fifo dn_fifo;
  32         struct ipdbg_fifo up_fifo;
  33         bool closed;
  34 };
  35
  36 struct ipdbg_service {
  37         struct ipdbg_hub *hub;
  38         struct ipdbg_service *next;
  39         uint16_t port;
  40         struct ipdbg_connection connection;
  41         uint8_t tool;
  42 };
  43
  44 struct ipdbg_virtual_ir_info {
  45         uint32_t instruction;
  46         uint32_t length;
  47         uint32_t value;
  48 };
  49
  50 struct ipdbg_hub {
  51         uint32_t user_instruction;
  52         uint32_t max_tools;
  53         uint32_t active_connections;
  54         uint32_t active_services;
  55         uint32_t valid_mask;
  56         uint32_t xoff_mask;
  57         uint32_t tool_mask;
  58         uint32_t last_dn_tool;
  59         struct ipdbg_hub *next;
  60         struct jtag_tap *tap;
  61         struct connection **connections;
  62         uint8_t data_register_length;
  63         uint8_t dn_xoff;
  64         struct ipdbg_virtual_ir_info *virtual_ir;
  65 };
  66
  67 static struct ipdbg_hub *ipdbg_first_hub;
  68
  69 static struct ipdbg_service *ipdbg_first_service;
  70
  71 static void ipdbg_init_fifo(struct ipdbg_fifo *fifo)
  72 {
  73         fifo->count = 0;
  74         fifo->rd_idx = 0;
  75 }
  76
  77 static bool ipdbg_fifo_is_empty(struct ipdbg_fifo *fifo)
  78 {
  79         return fifo->count == 0;
  80 }
  81
  82 static bool ipdbg_fifo_is_full(struct ipdbg_fifo *fifo)
  83 {
  84         return fifo->count == IPDBG_BUFFER_SIZE;
  85 }
  86
  87 static void ipdbg_zero_rd_idx(struct ipdbg_fifo *fifo)
  88 {
  89         if (fifo->rd_idx == 0)
  90                 return;
  91
  92         size_t ri = fifo->rd_idx;
  93         for (size_t idx = 0; idx < fifo->count; ++idx)
  94                 fifo->buffer[idx] = fifo->buffer[ri++];
  95         fifo->rd_idx = 0;
  96 }
  97
  98 static void ipdbg_append_to_fifo(struct ipdbg_fifo *fifo, char data)
  99 {
 100         if (ipdbg_fifo_is_full(fifo))
 101                 return;
 102
 103         ipdbg_zero_rd_idx(fifo);
 104         fifo->buffer[fifo->count++] = data;
 105 }
 106
 107 static char ipdbg_get_from_fifo(struct ipdbg_fifo *fifo)
 108 {
 109         if (ipdbg_fifo_is_empty(fifo))
 110                 return 0;
 111
 112         fifo->count--;
 113         return fifo->buffer[fifo->rd_idx++];
 114 }
 115
 116 static int ipdbg_move_buffer_to_connection(struct connection *conn, struct ipdbg_fifo *fifo)
 117 {
 118         if (ipdbg_fifo_is_empty(fifo))
 119                 return ERROR_OK;
 120
 121         struct ipdbg_connection *connection = conn->priv;
 122         if (connection->closed)
 123                 return ERROR_SERVER_REMOTE_CLOSED;
 124
 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;
 131         }
 132
 133         fifo->count -= bytes_written;
 134
 135         return ERROR_OK;
 136 }
 137
 138 static int ipdbg_max_tools_from_data_register_length(uint8_t data_register_length)
 139 {
 140         int max_tools = 1;
 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--)
 143                 max_tools *= 2;
 144
 145         /* last tool is used to reset JtagCDC and transfer "XON" to host*/
 146         return max_tools - 1;
 147 }
 148
 149 static struct ipdbg_service *ipdbg_find_service(struct ipdbg_hub *hub, uint8_t tool)
 150 {
 151         struct ipdbg_service *service;
 152         for (service = ipdbg_first_service; service; service = service->next) {
 153                 if (service->hub == hub && service->tool == tool)
 154                         break;
 155         }
 156         return service;
 157 }
 158
 159 static void ipdbg_add_service(struct ipdbg_service *service)
 160 {
 161         struct ipdbg_service *iservice;
 162         if (ipdbg_first_service) {
 163                 for (iservice = ipdbg_first_service; iservice->next; iservice = iservice->next)
 164                         ;
 165                 iservice->next = service;
 166         } else
 167                 ipdbg_first_service = service;
 168 }
 169
 170 static int ipdbg_create_service(struct ipdbg_hub *hub, uint8_t tool, struct ipdbg_service **service, uint16_t port)
 171 {
 172         *service = calloc(1, sizeof(struct ipdbg_service));
 173         if (!*service) {
 174                 LOG_ERROR("Out of memory");
 175                 return ERROR_FAIL;
 176         }
 177
 178         (*service)->hub = hub;
 179         (*service)->tool = tool;
 180         (*service)->port = port;
 181
 182         return ERROR_OK;
 183 }
 184
 185 static int ipdbg_remove_service(struct ipdbg_service *service)
 186 {
 187         if (!ipdbg_first_service)
 188                 return ERROR_FAIL;
 189
 190         if (service == ipdbg_first_service) {
 191                 ipdbg_first_service = ipdbg_first_service->next;
 192                 return ERROR_OK;
 193         }
 194
 195         for (struct ipdbg_service *iservice = ipdbg_first_service; iservice->next; iservice = iservice->next) {
 196                 if (service == iservice->next) {
 197                         iservice->next = service->next;
 198                         return ERROR_OK;
 199                 }
 200         }
 201         return ERROR_FAIL;
 202 }
 203
 204 static struct ipdbg_hub *ipdbg_find_hub(struct jtag_tap *tap,
 205                                 uint32_t user_instruction, struct ipdbg_virtual_ir_info *virtual_ir)
 206 {
 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)) {
 215                                 break;
 216                         }
 217                 }
 218         }
 219         return hub;
 220 }
 221
 222 static void ipdbg_add_hub(struct ipdbg_hub *hub)
 223 {
 224         struct ipdbg_hub *ihub;
 225         if (ipdbg_first_hub) {
 226                 for (ihub = ipdbg_first_hub; ihub->next; ihub = ihub->next)
 227                         ;
 228                 ihub->next = hub;
 229         } else
 230                 ipdbg_first_hub = hub;
 231 }
 232
 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)
 235 {
 236         *hub = NULL;
 237         struct ipdbg_hub *new_hub = calloc(1, sizeof(struct ipdbg_hub));
 238         if (!new_hub) {
 239                 free(virtual_ir);
 240                 LOG_ERROR("Out of memory");
 241                 return ERROR_FAIL;
 242         }
 243
 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) {
 247                 free(virtual_ir);
 248                 free(new_hub);
 249                 LOG_ERROR("Out of memory");
 250                 return ERROR_FAIL;
 251         }
 252         new_hub->tap                  = tap;
 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;
 260
 261         *hub = new_hub;
 262
 263         return ERROR_OK;
 264 }
 265
 266 static void ipdbg_free_hub(struct ipdbg_hub *hub)
 267 {
 268         if (!hub)
 269                 return;
 270         free(hub->connections);
 271         free(hub->virtual_ir);
 272         free(hub);
 273 }
 274
 275 static int ipdbg_remove_hub(struct ipdbg_hub *hub)
 276 {
 277         if (!ipdbg_first_hub)
 278                 return ERROR_FAIL;
 279         if (hub == ipdbg_first_hub) {
 280                 ipdbg_first_hub = ipdbg_first_hub->next;
 281                 return ERROR_OK;
 282         }
 283
 284         for (struct ipdbg_hub *ihub = ipdbg_first_hub; ihub->next; ihub = ihub->next) {
 285                 if (hub == ihub->next) {
 286                         ihub->next = hub->next;
 287                         return ERROR_OK;
 288                 }
 289         }
 290
 291         return ERROR_FAIL;
 292 }
 293
 294 static void ipdbg_init_scan_field(struct scan_field *fields, uint8_t *in_value, int num_bits, const uint8_t *out_value)
 295 {
 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;
 301 }
 302
 303 static int ipdbg_shift_instr(struct ipdbg_hub *hub, uint32_t instr)
 304 {
 305         if (!hub)
 306                 return ERROR_FAIL;
 307
 308         struct jtag_tap *tap = hub->tap;
 309         if (!tap)
 310                 return ERROR_FAIL;
 311
 312         if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) == instr) {
 313                 /* there is already the requested instruction in the ir */
 314                 return ERROR_OK;
 315         }
 316
 317         uint8_t *ir_out_val = calloc(DIV_ROUND_UP(tap->ir_length, 8), 1);
 318         if (!ir_out_val) {
 319                 LOG_ERROR("Out of memory");
 320                 return ERROR_FAIL;
 321         }
 322         buf_set_u32(ir_out_val, 0, tap->ir_length, instr);
 323
 324         struct scan_field fields;
 325         ipdbg_init_scan_field(&fields, NULL, tap->ir_length, ir_out_val);
 326         jtag_add_ir_scan(tap, &fields, TAP_IDLE);
 327         int retval = jtag_execute_queue();
 328
 329         free(ir_out_val);
 330
 331         return retval;
 332 }
 333
 334 static int ipdbg_shift_vir(struct ipdbg_hub *hub)
 335 {
 336         if (!hub)
 337                 return ERROR_FAIL;
 338
 339         if (!hub->virtual_ir)
 340                 return ERROR_OK;
 341
 342         int retval = ipdbg_shift_instr(hub, hub->virtual_ir->instruction);
 343         if (retval != ERROR_OK)
 344                 return retval;
 345
 346         struct jtag_tap *tap = hub->tap;
 347         if (!tap)
 348                 return ERROR_FAIL;
 349
 350         uint8_t *dr_out_val = calloc(DIV_ROUND_UP(hub->virtual_ir->length, 8), 1);
 351         if (!dr_out_val) {
 352                 LOG_ERROR("Out of memory");
 353                 return ERROR_FAIL;
 354         }
 355         buf_set_u32(dr_out_val, 0, hub->virtual_ir->length, hub->virtual_ir->value);
 356
 357         struct scan_field fields;
 358         ipdbg_init_scan_field(&fields, NULL, hub->virtual_ir->length, dr_out_val);
 359         jtag_add_dr_scan(tap, 1, &fields, TAP_IDLE);
 360         retval = jtag_execute_queue();
 361
 362         free(dr_out_val);
 363
 364         return retval;
 365 }
 366
 367 static int ipdbg_shift_data(struct ipdbg_hub *hub, uint32_t dn_data, uint32_t *up_data)
 368 {
 369         if (!hub)
 370                 return ERROR_FAIL;
 371
 372         struct jtag_tap *tap = hub->tap;
 373         if (!tap)
 374                 return ERROR_FAIL;
 375
 376         uint8_t *dr_out_val = calloc(DIV_ROUND_UP(hub->data_register_length, 8), 1);
 377         if (!dr_out_val) {
 378                 LOG_ERROR("Out of memory");
 379                 return ERROR_FAIL;
 380         }
 381         buf_set_u32(dr_out_val, 0, hub->data_register_length, dn_data);
 382
 383         uint8_t *dr_in_val = NULL;
 384         if (up_data) {
 385                 dr_in_val = calloc(DIV_ROUND_UP(hub->data_register_length, 8), 1);
 386                 if (!dr_in_val) {
 387                         LOG_ERROR("Out of memory");
 388                         free(dr_out_val);
 389                         return ERROR_FAIL;
 390                 }
 391         }
 392
 393         struct scan_field fields;
 394         ipdbg_init_scan_field(&fields, dr_in_val, hub->data_register_length, dr_out_val);
 395         jtag_add_dr_scan(tap, 1, &fields, TAP_IDLE);
 396         int retval = jtag_execute_queue();
 397
 398         if (up_data && retval == ERROR_OK)
 399                 *up_data = buf_get_u32(dr_in_val, 0, hub->data_register_length);
 400
 401         free(dr_out_val);
 402         free(dr_in_val);
 403
 404         return retval;
 405 }
 406
 407 static int ipdbg_distribute_data_from_hub(struct ipdbg_hub *hub, uint32_t up)
 408 {
 409         const bool valid_up_data = up & hub->valid_mask;
 410         if (!valid_up_data)
 411                 return ERROR_OK;
 412
 413         const size_t tool = (up >> 8) & hub->tool_mask;
 414         if (tool == hub->tool_mask) {
 415                 const uint8_t xon_cmd = up & 0x00ff;
 416                 hub->dn_xoff &= ~xon_cmd;
 417                 LOG_INFO("received xon cmd: %d\n", xon_cmd);
 418                 return ERROR_OK;
 419         }
 420
 421         struct connection *conn = hub->connections[tool];
 422         if (conn) {
 423                 struct ipdbg_connection *connection = conn->priv;
 424                 if (ipdbg_fifo_is_full(&connection->up_fifo)) {
 425                         int retval = ipdbg_move_buffer_to_connection(conn, &connection->up_fifo);
 426                         if (retval != ERROR_OK)
 427                                 return retval;
 428                 }
 429                 ipdbg_append_to_fifo(&connection->up_fifo, up);
 430         }
 431         return ERROR_OK;
 432 }
 433
 434 static int ipdbg_jtag_transfer_byte(struct ipdbg_hub *hub, size_t tool, struct ipdbg_connection *connection)
 435 {
 436         uint32_t dn = hub->valid_mask | ((tool & hub->tool_mask) << 8) |
 437                                 (0x00fful & ipdbg_get_from_fifo(&connection->dn_fifo));
 438         uint32_t up = 0;
 439         int ret = ipdbg_shift_data(hub, dn, &up);
 440         if (ret != ERROR_OK)
 441                 return ret;
 442
 443         ret = ipdbg_distribute_data_from_hub(hub, up);
 444         if (ret != ERROR_OK)
 445                 return ret;
 446
 447         if ((up & hub->xoff_mask) && (hub->last_dn_tool != hub->max_tools)) {
 448                 hub->dn_xoff |= BIT(hub->last_dn_tool);
 449                 LOG_INFO("tool %d sent xoff", hub->last_dn_tool);
 450         }
 451
 452         hub->last_dn_tool = tool;
 453
 454         return ERROR_OK;
 455 }
 456
 457 static int ipdbg_polling_callback(void *priv)
 458 {
 459         struct ipdbg_hub *hub = priv;
 460
 461         int ret = ipdbg_shift_vir(hub);
 462         if (ret != ERROR_OK)
 463                 return ret;
 464
 465         ret = ipdbg_shift_instr(hub, hub->user_instruction);
 466         if (ret != ERROR_OK)
 467                 return ret;
 468
 469         /* transfer dn buffers to jtag-hub */
 470         unsigned int num_transfers = 0;
 471         for (size_t tool = 0; tool < hub->max_tools; ++tool) {
 472                 struct connection *conn = hub->connections[tool];
 473                 if (conn && conn->priv) {
 474                         struct ipdbg_connection *connection = conn->priv;
 475                         while (((hub->dn_xoff & BIT(tool)) == 0) && !ipdbg_fifo_is_empty(&connection->dn_fifo)) {
 476                                 ret = ipdbg_jtag_transfer_byte(hub, tool, connection);
 477                                 if (ret != ERROR_OK)
 478                                         return ret;
 479                                 ++num_transfers;
 480                         }
 481                 }
 482         }
 483
 484         /* some transfers to get data from jtag-hub in case there is no dn data */
 485         while (num_transfers++ < hub->max_tools) {
 486                 uint32_t dn = 0;
 487                 uint32_t up = 0;
 488
 489                 int retval = ipdbg_shift_data(hub, dn, &up);
 490                 if (retval != ERROR_OK)
 491                         return ret;
 492
 493                 retval = ipdbg_distribute_data_from_hub(hub, up);
 494                 if (retval != ERROR_OK)
 495                         return ret;
 496         }
 497
 498         /* write from up fifos to sockets */
 499         for (size_t tool = 0; tool < hub->max_tools; ++tool) {
 500                 struct connection *conn = hub->connections[tool];
 501                 if (conn && conn->priv) {
 502                         struct ipdbg_connection *connection = conn->priv;
 503                         int retval = ipdbg_move_buffer_to_connection(conn, &connection->up_fifo);
 504                         if (retval != ERROR_OK)
 505                                 return retval;
 506                 }
 507         }
 508
 509         return ERROR_OK;
 510 }
 511
 512 static int ipdbg_start_polling(struct ipdbg_service *service, struct connection *connection)
 513 {
 514         struct ipdbg_hub *hub = service->hub;
 515         hub->connections[service->tool] = connection;
 516         hub->active_connections++;
 517         if (hub->active_connections > 1) {
 518                 /* hub is already initialized */
 519                 return ERROR_OK;
 520         }
 521
 522         const uint32_t reset_hub = hub->valid_mask | ((hub->max_tools) << 8);
 523
 524         int ret = ipdbg_shift_vir(hub);
 525         if (ret != ERROR_OK)
 526                 return ret;
 527
 528         ret = ipdbg_shift_instr(hub, hub->user_instruction);
 529         if (ret != ERROR_OK)
 530                 return ret;
 531
 532         ret = ipdbg_shift_data(hub, reset_hub, NULL);
 533         hub->last_dn_tool = hub->tool_mask;
 534         hub->dn_xoff = 0;
 535         if (ret != ERROR_OK)
 536                 return ret;
 537
 538         LOG_INFO("IPDBG start_polling");
 539
 540         const int time_ms = 20;
 541         const int periodic = 1;
 542         return target_register_timer_callback(ipdbg_polling_callback, time_ms, periodic, hub);
 543 }
 544
 545 static int ipdbg_stop_polling(struct ipdbg_service *service)
 546 {
 547         struct ipdbg_hub *hub = service->hub;
 548         hub->connections[service->tool] = NULL;
 549         hub->active_connections--;
 550         if (hub->active_connections == 0) {
 551                 LOG_INFO("IPDBG stop_polling");
 552
 553                 return target_unregister_timer_callback(ipdbg_polling_callback, hub);
 554         }
 555
 556         return ERROR_OK;
 557 }
 558
 559 static int ipdbg_on_new_connection(struct connection *connection)
 560 {
 561         struct ipdbg_service *service = connection->service->priv;
 562         connection->priv = &service->connection;
 563         /* initialize ipdbg connection information */
 564         ipdbg_init_fifo(&service->connection.up_fifo);
 565         ipdbg_init_fifo(&service->connection.dn_fifo);
 566
 567         int retval = ipdbg_start_polling(service, connection);
 568         if (retval != ERROR_OK) {
 569                 LOG_ERROR("BUG: ipdbg_start_polling failed");
 570                 return retval;
 571         }
 572
 573         struct ipdbg_connection *conn = connection->priv;
 574         conn->closed = false;
 575
 576         LOG_INFO("New IPDBG Connection");
 577
 578         return ERROR_OK;
 579 }
 580
 581 static int ipdbg_on_connection_input(struct connection *connection)
 582 {
 583         struct ipdbg_connection *conn = connection->priv;
 584         struct ipdbg_fifo *fifo = &conn->dn_fifo;
 585
 586         if (ipdbg_fifo_is_full(fifo))
 587                 return ERROR_OK;
 588
 589         ipdbg_zero_rd_idx(fifo);
 590         int bytes_read = connection_read(connection, fifo->buffer + fifo->count, IPDBG_BUFFER_SIZE - fifo->count);
 591         if (bytes_read <= 0) {
 592                 if (bytes_read < 0)
 593                         LOG_ERROR("error during read: %s", strerror(errno));
 594                 return ERROR_SERVER_REMOTE_CLOSED;
 595         }
 596
 597         fifo->count += bytes_read;
 598
 599         return ERROR_OK;
 600 }
 601
 602 static int ipdbg_on_connection_closed(struct connection *connection)
 603 {
 604         struct ipdbg_connection *conn = connection->priv;
 605         conn->closed = true;
 606         LOG_INFO("Closed IPDBG Connection");
 607
 608         return ipdbg_stop_polling(connection->service->priv);
 609 }
 610
 611 static const struct service_driver ipdbg_service_driver = {
 612         .name = "ipdbg",
 613         .new_connection_during_keep_alive_handler = NULL,
 614         .new_connection_handler = ipdbg_on_new_connection,
 615         .input_handler = ipdbg_on_connection_input,
 616         .connection_closed_handler = ipdbg_on_connection_closed,
 617         .keep_client_alive_handler = NULL,
 618 };
 619
 620 static int ipdbg_start(uint16_t port, struct jtag_tap *tap, uint32_t user_instruction,
 621                                         uint8_t data_register_length, struct ipdbg_virtual_ir_info *virtual_ir, uint8_t tool)
 622 {
 623         LOG_INFO("starting ipdbg service on port %d for tool %d", port, tool);
 624
 625         struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
 626         if (hub) {
 627                 free(virtual_ir);
 628                 if (hub->data_register_length != data_register_length) {
 629                         LOG_DEBUG("hub must have the same data_register_length for all tools");
 630                         return ERROR_FAIL;
 631                 }
 632         } else {
 633                 int retval = ipdbg_create_hub(tap, user_instruction, data_register_length, virtual_ir, &hub);
 634                 if (retval != ERROR_OK) {
 635                         free(virtual_ir);
 636                         return retval;
 637                 }
 638         }
 639
 640         struct ipdbg_service *service = NULL;
 641         int retval = ipdbg_create_service(hub, tool, &service, port);
 642
 643         if (retval != ERROR_OK || !service) {
 644                 if (hub->active_services == 0 && hub->active_connections == 0)
 645                         ipdbg_free_hub(hub);
 646                 return ERROR_FAIL;
 647         }
 648
 649         char port_str_buffer[IPDBG_TCP_PORT_STR_MAX_LENGTH];
 650         snprintf(port_str_buffer, IPDBG_TCP_PORT_STR_MAX_LENGTH, "%u", port);
 651         retval = add_service(&ipdbg_service_driver, port_str_buffer, 1, service);
 652         if (retval == ERROR_OK) {
 653                 ipdbg_add_service(service);
 654                 if (hub->active_services == 0 && hub->active_connections == 0)
 655                         ipdbg_add_hub(hub);
 656                 hub->active_services++;
 657         } else {
 658                 if (hub->active_services == 0 && hub->active_connections == 0)
 659                         ipdbg_free_hub(hub);
 660                 free(service);
 661         }
 662
 663         return retval;
 664 }
 665
 666 static int ipdbg_stop(struct jtag_tap *tap, uint32_t user_instruction,
 667                         struct ipdbg_virtual_ir_info *virtual_ir, uint8_t tool)
 668 {
 669         struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
 670         free(virtual_ir);
 671         if (!hub)
 672                 return ERROR_FAIL;
 673
 674         struct ipdbg_service *service = ipdbg_find_service(hub, tool);
 675         if (!service)
 676                 return ERROR_FAIL;
 677
 678         int retval = ipdbg_remove_service(service);
 679         if (retval != ERROR_OK) {
 680                 LOG_ERROR("BUG: ipdbg_remove_service failed");
 681                 return retval;
 682         }
 683
 684         char port_str_buffer[IPDBG_TCP_PORT_STR_MAX_LENGTH];
 685         snprintf(port_str_buffer, IPDBG_TCP_PORT_STR_MAX_LENGTH, "%u", service->port);
 686         retval = remove_service("ipdbg", port_str_buffer);
 687         /* The ipdbg_service structure is freed by server.c:remove_service().
 688            There the "priv" pointer is freed.*/
 689         if (retval != ERROR_OK) {
 690                 LOG_ERROR("BUG: remove_service failed");
 691                 return retval;
 692         }
 693         hub->active_services--;
 694         if (hub->active_connections == 0 && hub->active_services == 0) {
 695                 retval = ipdbg_remove_hub(hub);
 696                 if (retval != ERROR_OK) {
 697                         LOG_ERROR("BUG: ipdbg_remove_hub failed");
 698                         return retval;
 699                 }
 700                 ipdbg_free_hub(hub);
 701         }
 702         return ERROR_OK;
 703 }
 704
 705 COMMAND_HANDLER(handle_ipdbg_command)
 706 {
 707         struct jtag_tap *tap = NULL;
 708         uint16_t port = 4242;
 709         uint8_t tool = 1;
 710         uint32_t user_instruction = 0x00;
 711         uint8_t data_register_length = IPDBG_MAX_DR_LENGTH;
 712         bool start = true;
 713         bool hub_configured = false;
 714         bool has_virtual_ir = false;
 715         uint32_t virtual_ir_instruction = 0x00e;
 716         uint32_t virtual_ir_length = 5;
 717         uint32_t virtual_ir_value = 0x11;
 718         struct ipdbg_virtual_ir_info *virtual_ir = NULL;
 719
 720         if ((CMD_ARGC < IPDBG_MIN_NUM_OF_OPTIONS) || (CMD_ARGC > IPDBG_MAX_NUM_OF_OPTIONS))
 721                 return ERROR_COMMAND_SYNTAX_ERROR;
 722
 723         for (unsigned int i = 0; i < CMD_ARGC; ++i) {
 724                 if (strcmp(CMD_ARGV[i], "-tap") == 0) {
 725                         if (i + 1 >= CMD_ARGC || CMD_ARGV[i + 1][0] == '-') {
 726                                 command_print(CMD, "no TAP given");
 727                                 return ERROR_FAIL;
 728                         }
 729                         tap = jtag_tap_by_string(CMD_ARGV[i + 1]);
 730                         if (!tap) {
 731                                 command_print(CMD, "Tap %s unknown", CMD_ARGV[i + 1]);
 732                                 return ERROR_FAIL;
 733                         }
 734                         ++i;
 735                 } else if (strcmp(CMD_ARGV[i], "-hub") == 0) {
 736                         COMMAND_PARSE_ADDITIONAL_NUMBER(u32, i, user_instruction, "ir_value to select hub");
 737                         hub_configured = true;
 738                         COMMAND_PARSE_OPTIONAL_NUMBER(u8, i, data_register_length);
 739                         if (data_register_length < IPDBG_MIN_DR_LENGTH ||
 740                                 data_register_length > IPDBG_MAX_DR_LENGTH) {
 741                                 command_print(CMD, "length of \"user\"-data register must be at least %d and at most %d.",
 742                                                         IPDBG_MIN_DR_LENGTH, IPDBG_MAX_DR_LENGTH);
 743                                 return ERROR_FAIL;
 744                         }
 745                 } else if (strcmp(CMD_ARGV[i], "-vir") == 0) {
 746                         COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_value);
 747                         COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_length);
 748                         COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_instruction);
 749                         has_virtual_ir = true;
 750                 } else if (strcmp(CMD_ARGV[i], "-port") == 0) {
 751                         COMMAND_PARSE_ADDITIONAL_NUMBER(u16, i, port, "port number");
 752                 } else if (strcmp(CMD_ARGV[i], "-tool") == 0) {
 753                         COMMAND_PARSE_ADDITIONAL_NUMBER(u8, i, tool, "tool");
 754                 } else if (strcmp(CMD_ARGV[i], "-stop") == 0) {
 755                         start = false;
 756                 } else if (strcmp(CMD_ARGV[i], "-start") == 0) {
 757                         start = true;
 758                 } else {
 759                         command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
 760                         return ERROR_FAIL;
 761                 }
 762         }
 763
 764         if (!tap) {
 765                 command_print(CMD, "no valid tap selected");
 766                 return ERROR_FAIL;
 767         }
 768
 769         if (!hub_configured) {
 770                 command_print(CMD, "hub not configured correctly");
 771                 return ERROR_FAIL;
 772         }
 773
 774         if (tool >= ipdbg_max_tools_from_data_register_length(data_register_length)) {
 775                 command_print(CMD, "Tool: %d is invalid", tool);
 776                 return ERROR_FAIL;
 777         }
 778
 779         if (has_virtual_ir) {
 780                 virtual_ir = calloc(1, sizeof(struct ipdbg_virtual_ir_info));
 781                 if (!virtual_ir) {
 782                         LOG_ERROR("Out of memory");
 783                         return ERROR_FAIL;
 784                 }
 785                 virtual_ir->instruction = virtual_ir_instruction;
 786                 virtual_ir->length      = virtual_ir_length;
 787                 virtual_ir->value       = virtual_ir_value;
 788         }
 789
 790         if (start)
 791                 return ipdbg_start(port, tap, user_instruction, data_register_length, virtual_ir, tool);
 792         else
 793                 return ipdbg_stop(tap, user_instruction, virtual_ir, tool);
 794 }
 795
 796 static const struct command_registration ipdbg_command_handlers[] = {
 797         {
 798                 .name = "ipdbg",
 799                 .handler = handle_ipdbg_command,
 800                 .mode = COMMAND_EXEC,
 801                 .help = "Starts or stops an IPDBG JTAG-Host server.",
 802                 .usage = "[-start|-stop] -tap device.tap -hub ir_value [dr_length]"
 803                                  " [-port number] [-tool number] [-vir [vir_value [length [instr_code]]]]",
 804         },
 805         COMMAND_REGISTRATION_DONE
 806 };
 807
 808 int ipdbg_register_commands(struct command_context *cmd_ctx)
 809 {
 810         return register_commands(cmd_ctx, NULL, ipdbg_command_handlers);
 811 }