git.gag.com Git - fw/openocd/blob - 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         buf_set_u32(ir_out_val, 0, tap->ir_length, instr);
 319
 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();
 324
 325         free(ir_out_val);
 326
 327         return retval;
 328 }
 329
 330 static int ipdbg_shift_vir(struct ipdbg_hub *hub)
 331 {
 332         if (!hub)
 333                 return ERROR_FAIL;
 334
 335         if (!hub->virtual_ir)
 336                 return ERROR_OK;
 337
 338         int retval = ipdbg_shift_instr(hub, hub->virtual_ir->instruction);
 339         if (retval != ERROR_OK)
 340                 return retval;
 341
 342         struct jtag_tap *tap = hub->tap;
 343         if (!tap)
 344                 return ERROR_FAIL;
 345
 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);
 348
 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();
 353
 354         free(dr_out_val);
 355
 356         return retval;
 357 }
 358
 359 static int ipdbg_shift_data(struct ipdbg_hub *hub, uint32_t dn_data, uint32_t *up_data)
 360 {
 361         if (!hub)
 362                 return ERROR_FAIL;
 363
 364         struct jtag_tap *tap = hub->tap;
 365         if (!tap)
 366                 return ERROR_FAIL;
 367
 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;
 371
 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();
 376
 377         if (up_data && retval == ERROR_OK)
 378                 *up_data = buf_get_u32(dr_in_val, 0, hub->data_register_length);
 379
 380         free(dr_out_val);
 381         free(dr_in_val);
 382
 383         return retval;
 384 }
 385
 386 static int ipdbg_distribute_data_from_hub(struct ipdbg_hub *hub, uint32_t up)
 387 {
 388         const bool valid_up_data = up & hub->valid_mask;
 389         if (!valid_up_data)
 390                 return ERROR_OK;
 391
 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);
 397                 return ERROR_OK;
 398         }
 399
 400         struct connection *conn = hub->connections[tool];
 401         if (conn) {
 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)
 406                                 return retval;
 407                 }
 408                 ipdbg_append_to_fifo(&connection->up_fifo, up);
 409         }
 410         return ERROR_OK;
 411 }
 412
 413 static int ipdbg_jtag_transfer_byte(struct ipdbg_hub *hub, size_t tool, struct ipdbg_connection *connection)
 414 {
 415         uint32_t dn = hub->valid_mask | ((tool & hub->tool_mask) << 8) |
 416                                 (0x00fful & ipdbg_get_from_fifo(&connection->dn_fifo));
 417         uint32_t up = 0;
 418         int ret = ipdbg_shift_data(hub, dn, &up);
 419         if (ret != ERROR_OK)
 420                 return ret;
 421
 422         ret = ipdbg_distribute_data_from_hub(hub, up);
 423         if (ret != ERROR_OK)
 424                 return ret;
 425
 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);
 429         }
 430
 431         hub->last_dn_tool = tool;
 432
 433         return ERROR_OK;
 434 }
 435
 436 static int ipdbg_polling_callback(void *priv)
 437 {
 438         struct ipdbg_hub *hub = priv;
 439
 440         int ret = ipdbg_shift_vir(hub);
 441         if (ret != ERROR_OK)
 442                 return ret;
 443
 444         ret = ipdbg_shift_instr(hub, hub->user_instruction);
 445         if (ret != ERROR_OK)
 446                 return ret;
 447
 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);
 456                                 if (ret != ERROR_OK)
 457                                         return ret;
 458                                 ++num_transfers;
 459                         }
 460                 }
 461         }
 462
 463         /* some transfers to get data from jtag-hub in case there is no dn data */
 464         while (num_transfers++ < hub->max_tools) {
 465                 uint32_t dn = 0;
 466                 uint32_t up = 0;
 467
 468                 int retval = ipdbg_shift_data(hub, dn, &up);
 469                 if (retval != ERROR_OK)
 470                         return ret;
 471
 472                 retval = ipdbg_distribute_data_from_hub(hub, up);
 473                 if (retval != ERROR_OK)
 474                         return ret;
 475         }
 476
 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)
 484                                 return retval;
 485                 }
 486         }
 487
 488         return ERROR_OK;
 489 }
 490
 491 static int ipdbg_start_polling(struct ipdbg_service *service, struct connection *connection)
 492 {
 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 */
 498                 return ERROR_OK;
 499         }
 500
 501         const uint32_t reset_hub = hub->valid_mask | ((hub->max_tools) << 8);
 502
 503         int ret = ipdbg_shift_vir(hub);
 504         if (ret != ERROR_OK)
 505                 return ret;
 506
 507         ret = ipdbg_shift_instr(hub, hub->user_instruction);
 508         if (ret != ERROR_OK)
 509                 return ret;
 510
 511         ret = ipdbg_shift_data(hub, reset_hub, NULL);
 512         hub->last_dn_tool = hub->tool_mask;
 513         hub->dn_xoff = 0;
 514         if (ret != ERROR_OK)
 515                 return ret;
 516
 517         LOG_INFO("IPDBG start_polling");
 518
 519         const int time_ms = 20;
 520         const int periodic = 1;
 521         return target_register_timer_callback(ipdbg_polling_callback, time_ms, periodic, hub);
 522 }
 523
 524 static int ipdbg_stop_polling(struct ipdbg_service *service)
 525 {
 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");
 531
 532                 return target_unregister_timer_callback(ipdbg_polling_callback, hub);
 533         }
 534
 535         return ERROR_OK;
 536 }
 537
 538 static int ipdbg_on_new_connection(struct connection *connection)
 539 {
 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);
 545
 546         int retval = ipdbg_start_polling(service, connection);
 547         if (retval != ERROR_OK) {
 548                 LOG_ERROR("BUG: ipdbg_start_polling failed");
 549                 return retval;
 550         }
 551
 552         struct ipdbg_connection *conn = connection->priv;
 553         conn->closed = false;
 554
 555         LOG_INFO("New IPDBG Connection");
 556
 557         return ERROR_OK;
 558 }
 559
 560 static int ipdbg_on_connection_input(struct connection *connection)
 561 {
 562         struct ipdbg_connection *conn = connection->priv;
 563         struct ipdbg_fifo *fifo = &conn->dn_fifo;
 564
 565         if (ipdbg_fifo_is_full(fifo))
 566                 return ERROR_OK;
 567
 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) {
 571                 if (bytes_read < 0)
 572                         LOG_ERROR("error during read: %s", strerror(errno));
 573                 return ERROR_SERVER_REMOTE_CLOSED;
 574         }
 575
 576         fifo->count += bytes_read;
 577
 578         return ERROR_OK;
 579 }
 580
 581 static int ipdbg_on_connection_closed(struct connection *connection)
 582 {
 583         struct ipdbg_connection *conn = connection->priv;
 584         conn->closed = true;
 585         LOG_INFO("Closed IPDBG Connection");
 586
 587         return ipdbg_stop_polling(connection->service->priv);
 588 }
 589
 590 static const struct service_driver ipdbg_service_driver = {
 591         .name = "ipdbg",
 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,
 597 };
 598
 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)
 601 {
 602         LOG_INFO("starting ipdbg service on port %d for tool %d", port, tool);
 603
 604         struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
 605         if (hub) {
 606                 free(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");
 609                         return ERROR_FAIL;
 610                 }
 611         } else {
 612                 int retval = ipdbg_create_hub(tap, user_instruction, data_register_length, virtual_ir, &hub);
 613                 if (retval != ERROR_OK) {
 614                         free(virtual_ir);
 615                         return retval;
 616                 }
 617         }
 618
 619         struct ipdbg_service *service = NULL;
 620         int retval = ipdbg_create_service(hub, tool, &service, port);
 621
 622         if (retval != ERROR_OK || !service) {
 623                 if (hub->active_services == 0 && hub->active_connections == 0)
 624                         ipdbg_free_hub(hub);
 625                 return ERROR_FAIL;
 626         }
 627
 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)
 634                         ipdbg_add_hub(hub);
 635                 hub->active_services++;
 636         } else {
 637                 if (hub->active_services == 0 && hub->active_connections == 0)
 638                         ipdbg_free_hub(hub);
 639                 free(service);
 640         }
 641
 642         return retval;
 643 }
 644
 645 static int ipdbg_stop(struct jtag_tap *tap, uint32_t user_instruction,
 646                         struct ipdbg_virtual_ir_info *virtual_ir, uint8_t tool)
 647 {
 648         struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
 649         free(virtual_ir);
 650         if (!hub)
 651                 return ERROR_FAIL;
 652
 653         struct ipdbg_service *service = ipdbg_find_service(hub, tool);
 654         if (!service)
 655                 return ERROR_FAIL;
 656
 657         int retval = ipdbg_remove_service(service);
 658         if (retval != ERROR_OK) {
 659                 LOG_ERROR("BUG: ipdbg_remove_service failed");
 660                 return retval;
 661         }
 662
 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");
 670                 return retval;
 671         }
 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");
 677                         return retval;
 678                 }
 679                 ipdbg_free_hub(hub);
 680         }
 681         return ERROR_OK;
 682 }
 683
 684 COMMAND_HANDLER(handle_ipdbg_command)
 685 {
 686         struct jtag_tap *tap = NULL;
 687         uint16_t port = 4242;
 688         uint8_t tool = 1;
 689         uint32_t user_instruction = 0x00;
 690         uint8_t data_register_length = IPDBG_MAX_DR_LENGTH;
 691         bool start = true;
 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;
 698
 699         if ((CMD_ARGC < IPDBG_MIN_NUM_OF_OPTIONS) || (CMD_ARGC > IPDBG_MAX_NUM_OF_OPTIONS))
 700                 return ERROR_COMMAND_SYNTAX_ERROR;
 701
 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");
 706                                 return ERROR_FAIL;
 707                         }
 708                         tap = jtag_tap_by_string(CMD_ARGV[i + 1]);
 709                         if (!tap) {
 710                                 command_print(CMD, "Tap %s unknown", CMD_ARGV[i + 1]);
 711                                 return ERROR_FAIL;
 712                         }
 713                         ++i;
 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);
 722                                 return ERROR_FAIL;
 723                         }
 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) {
 734                         start = false;
 735                 } else if (strcmp(CMD_ARGV[i], "-start") == 0) {
 736                         start = true;
 737                 } else {
 738                         command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
 739                         return ERROR_FAIL;
 740                 }
 741         }
 742
 743         if (!tap) {
 744                 command_print(CMD, "no valid tap selected");
 745                 return ERROR_FAIL;
 746         }
 747
 748         if (!hub_configured) {
 749                 command_print(CMD, "hub not configured correctly");
 750                 return ERROR_FAIL;
 751         }
 752
 753         if (tool >= ipdbg_max_tools_from_data_register_length(data_register_length)) {
 754                 command_print(CMD, "Tool: %d is invalid", tool);
 755                 return ERROR_FAIL;
 756         }
 757
 758         if (has_virtual_ir) {
 759                 virtual_ir = calloc(1, sizeof(struct ipdbg_virtual_ir_info));
 760                 if (!virtual_ir) {
 761                         LOG_ERROR("Out of memory");
 762                         return ERROR_FAIL;
 763                 }
 764                 virtual_ir->instruction = virtual_ir_instruction;
 765                 virtual_ir->length      = virtual_ir_length;
 766                 virtual_ir->value       = virtual_ir_value;
 767         }
 768
 769         if (start)
 770                 return ipdbg_start(port, tap, user_instruction, data_register_length, virtual_ir, tool);
 771         else
 772                 return ipdbg_stop(tap, user_instruction, virtual_ir, tool);
 773 }
 774
 775 static const struct command_registration ipdbg_command_handlers[] = {
 776         {
 777                 .name = "ipdbg",
 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]]]]",
 783         },
 784         COMMAND_REGISTRATION_DONE
 785 };
 786
 787 int ipdbg_register_commands(struct command_context *cmd_ctx)
 788 {
 789         return register_commands(cmd_ctx, NULL, ipdbg_command_handlers);
 790 }