1
2
3
4
5
6
7
8
17
18
19
20
21
22
23
24
25
26
27
28
29
34
35
40
41
48
49
54
55
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
92
93
97
98
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
174
175
185
186
187
188
189
193
194
195
196
197
198
199
200
201
202
203
204
205
206
210
211
217
218
219
220
221
237
238
249
250
251
252
253
254
258
259
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
292
293
294
300
301
302
303
304
305
306
307
308
309
313
314
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
347
348
349
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
424
425
426
435
436
437
438
439
448
449
450
451
452
453
454
455
456
457
458
459
460
467
468
469
470
471
472
473
474
475
476
477
478
479
480
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
607
608
609
610
611
612
613
614
615
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
685
686
687
688
689
690
691
692
693
694
695
696
697
698
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
759
760
761
762
763
767
768
769
770
771
773
777
778
779
780
781
782
783
784
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
815
816
817
818
819
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
848
849
850
851
852
853
854
855
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
884
885
886
887
888
904
905
906
907
908
909
910
914
915
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
941
942
943
944
945
962
963
964
965
966
967
968
969
973
974
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1060
1061
1062
1063
1064
1068
1069
1070
1071
1075
1079
1083
1084
1085
1086
1087
1096
1097
1101
1102
1106
1107
1111
1112
1113
1114
1118
1119
1120
1121
1122
1123
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
#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 <pld/pld.h>
#include "ipdbg.h"
7 includes
#define IPDBG_BUFFER_SIZE 16384
#define IPDBG_MIN_NUM_OF_CREATE_OPTIONS 3
#define IPDBG_MAX_NUM_OF_CREATE_OPTIONS 10
#define IPDBG_NUM_OF_START_OPTIONS 4
#define IPDBG_NUM_OF_STOP_OPTIONS 2
#define IPDBG_NUM_OF_QUEUE_OPTIONS 2
#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
10 defines
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_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;
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;
char *name;
size_t using_queue_size;
struct ipdbg_hub *next;
struct jtag_tap *tap;
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;
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;
}if (bytes_written != fifo->count) { ... }
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;
while (data_register_length--)
max_tools *= 2;
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;
}for (service = ipdbg_first_service; service; service = service->next) { ... }
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;
}if (ipdbg_first_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;
}if (!*service) { ... }
(*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;
}if (service == ipdbg_first_service) { ... }
for (struct ipdbg_service *iservice = ipdbg_first_service; iservice->next; iservice = iservice->next) {
if (service == iservice->next) {
iservice->next = service->next;
return ERROR_OK;
}if (service == iservice->next) { ... }
}for (struct ipdbg_service *iservice = ipdbg_first_service; iservice->next; iservice = iservice->next) { ... }
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;
}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)) { ... }
}if (hub->tap == tap && hub->user_instruction == user_instruction) { ... }
}for (hub = ipdbg_first_hub; hub; hub = hub->next) { ... }
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;
}if (ipdbg_first_hub) { ... } else {
ipdbg_first_hub = hub;
}else { ... }
}{ ... }
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;
}if (hub == ipdbg_first_hub) { ... }
for (struct ipdbg_hub *ihub = ipdbg_first_hub; ihub->next; ihub = ihub->next) {
if (hub == ihub->next) {
ihub->next = hub->next;
return ERROR_OK;
}if (hub == ihub->next) { ... }
}for (struct ipdbg_hub *ihub = ipdbg_first_hub; ihub->next; ihub = ihub->next) { ... }
return ERROR_FAIL;
}{ ... }
static void ipdbg_free_hub(struct ipdbg_hub *hub)
{
if (!hub)
return;
free(hub->connections);
free(hub->virtual_ir);
free(hub->name);
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);
}{ ... }
static struct ipdbg_hub *ipdbg_allocate_hub(uint8_t data_register_length, struct ipdbg_virtual_ir_info *virtual_ir,
const char *name)
{
struct ipdbg_hub *new_hub = calloc(1, sizeof(struct ipdbg_hub));
if (!new_hub) {
free(virtual_ir);
LOG_ERROR("Out of memory");
return NULL;
}if (!new_hub) { ... }
new_hub->name = strdup(name);
if (!new_hub->name) {
free(new_hub);
free(virtual_ir);
LOG_ERROR("Out of memory");
return NULL;
}if (!new_hub->name) { ... }
const size_t dreg_buffer_size = DIV_ROUND_UP(data_register_length, 8);
uint32_t 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(max_tools, sizeof(struct connection *));
if (virtual_ir) {
new_hub->virtual_ir = virtual_ir;
new_hub->scratch_memory.vir_out_val = calloc(1, DIV_ROUND_UP(virtual_ir->length, 8));
}if (virtual_ir) { ... }
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) {
ipdbg_free_hub(new_hub);
LOG_ERROR("Out of memory");
return NULL;
}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) { ... }
return new_hub;
}{ ... }
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) {
return ERROR_OK;
}if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) == instr) { ... }
uint8_t *ir_out_val = calloc(DIV_ROUND_UP(tap->ir_length, 8), 1);
if (!ir_out_val) {
LOG_ERROR("Out of memory");
return ERROR_FAIL;
}if (!ir_out_val) { ... }
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;
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();
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;
buf_set_u32(hub->scratch_memory.dr_out_vals, 0, hub->data_register_length, dn_data);
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(hub->scratch_memory.dr_in_vals, 0, hub->data_register_length);
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;
}if (tool == hub->tool_mask) { ... }
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;
}if (ipdbg_fifo_is_full(&connection->up_fifo)) { ... }
ipdbg_append_to_fifo(&connection->up_fifo, up);
}if (conn) { ... }
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);
}if ((rx_data & hub->xoff_mask) && hub->last_dn_tool != hub->max_tools) { ... }
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 < hub->using_queue_size; ++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);
}for (size_t i = 0; i < hub->using_queue_size; ++i) { ... }
int retval = jtag_execute_queue();
if (retval == ERROR_OK) {
uint32_t up_data;
for (size_t i = 0; i < hub->using_queue_size; ++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) {
/* ... */
ipdbg_check_for_xoff(hub, hub->max_tools, up_data);
}if (i == 0) { ... }
}for (size_t i = 0; i < hub->using_queue_size; ++i) { ... }
}if (retval == ERROR_OK) { ... }
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) |
(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;
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 < hub->using_queue_size) ?
connection->dn_fifo.count : hub->using_queue_size;
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);
}for (size_t i = 0; i < num_tx; ++i) { ... }
int retval = jtag_execute_queue();
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) {
/* ... */
ipdbg_check_for_xoff(hub, tool, up_data);
}if (i == 0) { ... }
}for (size_t i = 0; i < num_tx; ++i) { ... }
}if (retval == ERROR_OK) { ... }
return retval;
}{ ... }
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;
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)) {
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;
}while (((hub->dn_xoff & BIT(tool)) == 0) && !ipdbg_fifo_is_empty(&connection->dn_fifo)) { ... }
}if (conn && conn->priv) { ... }
}for (size_t tool = 0; tool < hub->max_tools; ++tool) { ... }
ret = ipdbg_shift_empty_data(hub);
if (ret != ERROR_OK)
return ret;
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;
}if (conn && conn->priv) { ... }
}for (size_t tool = 0; tool < hub->max_tools; ++tool) { ... }
return ERROR_OK;
}{ ... }
static int ipdbg_get_flow_control_info_from_hub(struct ipdbg_hub *hub)
{
uint32_t up_data;
/* ... */
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;
if (tool == hub->tool_mask)
hub->flow_control_enabled = up_data & 0x007f;
else
ipdbg_distribute_data_from_hub(hub, up_data);
}if (valid_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;
hub->connections[service->tool] = connection;
hub->active_connections++;
if (hub->active_connections > 1) {
return ERROR_OK;
}if (hub->active_connections > 1) { ... }
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;
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;
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);
}if (hub->active_connections == 0) { ... }
return ERROR_OK;
}{ ... }
static int ipdbg_on_new_connection(struct connection *connection)
{
struct ipdbg_service *service = connection->service->priv;
connection->priv = &service->connection;
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;
}if (retval != ERROR_OK) { ... }
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;
}if (bytes_read <= 0) { ... }
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 const struct service_driver ipdbg_service_driver = {
.name = "ipdbg",
.new_connection_during_keep_alive_handler = NULL,
.new_connection_handler = ipdbg_on_new_connection,
.input_handler = ipdbg_on_connection_input,
.connection_closed_handler = ipdbg_on_connection_closed,
.keep_client_alive_handler = NULL,
...};
static struct ipdbg_hub *ipdbg_get_hub_by_name(const char *name)
{
struct ipdbg_hub *hub = NULL;
for (hub = ipdbg_first_hub; hub; hub = hub->next) {
if (strcmp(hub->name, name) == 0)
break;
}for (hub = ipdbg_first_hub; hub; hub = hub->next) { ... }
return hub;
}{ ... };
static int ipdbg_stop_service(struct ipdbg_service *service)
{
int retval = ipdbg_remove_service(service);
if (retval != ERROR_OK) {
LOG_ERROR("BUG: ipdbg_remove_service failed");
return retval;
}if (retval != ERROR_OK) { ... }
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);
/* ... */
if (retval != ERROR_OK) {
LOG_ERROR("BUG: remove_service failed");
return retval;
}if (retval != ERROR_OK) { ... }
return ERROR_OK;
}{ ... }
int ipdbg_server_free(void)
{
int retval = ERROR_OK;
for (struct ipdbg_hub *hub = ipdbg_first_hub; hub;) {
for (uint8_t tool = 0; tool < hub->max_tools; ++tool) {
struct ipdbg_service *service = ipdbg_find_service(hub, tool);
if (service) {
int new_retval = ipdbg_stop_service(service);
if (new_retval != ERROR_OK)
retval = new_retval;
hub->active_services--;
}if (service) { ... }
}for (uint8_t tool = 0; tool < hub->max_tools; ++tool) { ... }
struct ipdbg_hub *next_hub = hub->next;
int new_retval = ipdbg_remove_hub(hub);
if (new_retval != ERROR_OK)
retval = new_retval;
ipdbg_free_hub(hub);
hub = next_hub;
}for (struct ipdbg_hub *hub = ipdbg_first_hub; hub;) { ... }
return retval;
}{ ... }
static int ipdbg_start(struct ipdbg_hub *hub, uint16_t port, uint8_t tool)
{
LOG_INFO("starting ipdbg service on port %d for tool %d", port, tool);
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;
}if (retval != ERROR_OK || !service) { ... }
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_service_driver, port_str_buffer, 1, service);
if (retval != ERROR_OK) {
free(service);
return retval;
}if (retval != ERROR_OK) { ... }
ipdbg_add_service(service);
hub->active_services++;
return ERROR_OK;
}{ ... }
COMMAND_HANDLER(handle_ipdbg_start_command)
{
struct ipdbg_hub *hub = CMD_DATA;
uint16_t port = 4242;
uint8_t tool = 1;
if (CMD_ARGC > IPDBG_NUM_OF_START_OPTIONS)
return ERROR_COMMAND_SYNTAX_ERROR;
for (unsigned int i = 0; i < CMD_ARGC; ++i) {
if (strcmp(CMD_ARGV[i], "-port") == 0) {
COMMAND_PARSE_ADDITIONAL_NUMBER(u16, i, port, "port number");
}if (strcmp(CMD_ARGV[i], "-port") == 0) { ... } else if (strcmp(CMD_ARGV[i], "-tool") == 0) {
COMMAND_PARSE_ADDITIONAL_NUMBER(u8, i, tool, "tool");
}else if (strcmp(CMD_ARGV[i], "-tool") == 0) { ... } else {
command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
return ERROR_FAIL;
}else { ... }
}for (unsigned int i = 0; i < CMD_ARGC; ++i) { ... }
return ipdbg_start(hub, port, tool);
}{ ... }
static int ipdbg_stop(struct ipdbg_hub *hub, uint8_t tool)
{
struct ipdbg_service *service = ipdbg_find_service(hub, tool);
if (!service) {
LOG_ERROR("No service for hub '%s'/tool %d found", hub->name, tool);
return ERROR_FAIL;
}if (!service) { ... }
int retval = ipdbg_stop_service(service);
hub->active_services--;
LOG_INFO("stopped ipdbg service for tool %d", tool);
return retval;
}{ ... }
COMMAND_HANDLER(handle_ipdbg_stop_command)
{
struct ipdbg_hub *hub = CMD_DATA;
uint8_t tool = 1;
if (CMD_ARGC > IPDBG_NUM_OF_STOP_OPTIONS)
return ERROR_COMMAND_SYNTAX_ERROR;
for (unsigned int i = 0; i < CMD_ARGC; ++i) {
if (strcmp(CMD_ARGV[i], "-tool") == 0) {
COMMAND_PARSE_ADDITIONAL_NUMBER(u8, i, tool, "tool");
}if (strcmp(CMD_ARGV[i], "-tool") == 0) { ... } else {
command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
return ERROR_FAIL;
}else { ... }
}for (unsigned int i = 0; i < CMD_ARGC; ++i) { ... }
return ipdbg_stop(hub, tool);
}{ ... }
static const struct command_registration ipdbg_hostserver_subcommand_handlers[] = {
{
.name = "start",
.mode = COMMAND_EXEC,
.handler = handle_ipdbg_start_command,
.help = "Starts a IPDBG Host server.",
.usage = "-tool number -port port"
...}, {
.name = "stop",
.mode = COMMAND_EXEC,
.handler = handle_ipdbg_stop_command,
.help = "Stops a IPDBG Host server.",
.usage = "-tool number"
...},
COMMAND_REGISTRATION_DONE
...};
static COMMAND_HELPER(ipdbg_config_queuing, struct ipdbg_hub *hub, unsigned int size)
{
if (!hub)
return ERROR_FAIL;
if (hub->active_connections) {
command_print(CMD, "Configuration change not allowed when hub has active connections");
return ERROR_FAIL;
}if (hub->active_connections) { ... }
if (size == 0 || size > IPDBG_SCRATCH_MEMORY_SIZE) {
command_print(CMD, "queuing size out of range! Must be 0 < size <= %d", IPDBG_SCRATCH_MEMORY_SIZE);
return ERROR_COMMAND_ARGUMENT_INVALID;
}if (size == 0 || size > IPDBG_SCRATCH_MEMORY_SIZE) { ... }
hub->using_queue_size = size;
return ERROR_OK;
}{ ... }
COMMAND_HANDLER(handle_ipdbg_cfg_queuing_command)
{
struct ipdbg_hub *hub = CMD_DATA;
unsigned int size;
if (CMD_ARGC != IPDBG_NUM_OF_QUEUE_OPTIONS)
return ERROR_COMMAND_SYNTAX_ERROR;
for (unsigned int i = 0; i < CMD_ARGC; ++i) {
if (strcmp(CMD_ARGV[i], "-size") == 0) {
COMMAND_PARSE_ADDITIONAL_NUMBER(uint, i, size, "size");
}if (strcmp(CMD_ARGV[i], "-size") == 0) { ... } else {
command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
return ERROR_FAIL;
}else { ... }
}for (unsigned int i = 0; i < CMD_ARGC; ++i) { ... }
return CALL_COMMAND_HANDLER(ipdbg_config_queuing, hub, size);
}{ ... }
static const struct command_registration ipdbg_hub_subcommand_handlers[] = {
{
.name = "ipdbg",
.mode = COMMAND_EXEC,
.help = "IPDBG Hub commands.",
.usage = "",
.chain = ipdbg_hostserver_subcommand_handlers
...},
{
.name = "queuing",
.handler = handle_ipdbg_cfg_queuing_command,
.mode = COMMAND_ANY,
.help = "configures queuing between IPDBG Host and Hub.",
.usage = "-size size",
...},
COMMAND_REGISTRATION_DONE
...};
static int ipdbg_register_hub_command(struct ipdbg_hub *hub, struct command_invocation *cmd)
{
Jim_Interp *interp = CMD_CTX->interp;
Jim_Cmd *jcmd = Jim_GetCommand(interp, Jim_NewStringObj(interp, hub->name, -1), JIM_NONE);
if (jcmd) {
LOG_ERROR("cannot create Hub because a command with name '%s' already exists", hub->name);
return ERROR_FAIL;
}if (jcmd) { ... }
const struct command_registration obj_commands[] = {
{
.name = hub->name,
.mode = COMMAND_EXEC,
.help = "IPDBG Hub command group.",
.usage = "",
.chain = ipdbg_hub_subcommand_handlers
...},
COMMAND_REGISTRATION_DONE
...};
return register_commands_with_data(CMD_CTX, NULL, obj_commands, 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, const char *name, struct command_invocation *cmd)
{
struct ipdbg_hub *new_hub = ipdbg_allocate_hub(data_register_length, virtual_ir, name);
if (!new_hub)
return ERROR_FAIL;
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;
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->max_tools = ipdbg_max_tools_from_data_register_length(data_register_length);
new_hub->using_queue_size = IPDBG_SCRATCH_MEMORY_SIZE;
int retval = ipdbg_register_hub_command(new_hub, cmd);
if (retval != ERROR_OK) {
LOG_ERROR("Creating hub failed");
ipdbg_free_hub(new_hub);
return ERROR_FAIL;
}if (retval != ERROR_OK) { ... }
ipdbg_add_hub(new_hub);
return ERROR_OK;
}{ ... }
COMMAND_HANDLER(handle_ipdbg_create_hub_command)
{
struct jtag_tap *tap = NULL;
uint32_t user_instruction = 0x00;
uint8_t data_register_length = IPDBG_MAX_DR_LENGTH;
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;
int user_num = 1;
bool hub_configured = false;
if (CMD_ARGC < IPDBG_MIN_NUM_OF_CREATE_OPTIONS || CMD_ARGC > IPDBG_MAX_NUM_OF_CREATE_OPTIONS)
return ERROR_COMMAND_SYNTAX_ERROR;
const char *hub_name = CMD_ARGV[0];
for (unsigned int i = 1; 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 name given");
return ERROR_FAIL;
}if (i + 1 >= CMD_ARGC || CMD_ARGV[i + 1][0] == '-') { ... }
tap = jtag_tap_by_string(CMD_ARGV[i + 1]);
if (!tap) {
command_print(CMD, "Tap %s unknown", CMD_ARGV[i + 1]);
return ERROR_FAIL;
}if (!tap) { ... }
++i;
}if (strcmp(CMD_ARGV[i], "-tap") == 0) { ... } else if (strcmp(CMD_ARGV[i], "-ir") == 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;
}if (data_register_length < IPDBG_MIN_DR_LENGTH || data_register_length > IPDBG_MAX_DR_LENGTH) { ... }
}else if (strcmp(CMD_ARGV[i], "-ir") == 0) { ... } else if (strcmp(CMD_ARGV[i], "-pld") == 0) {
++i;
if (i >= CMD_ARGC || CMD_ARGV[i][0] == '-')
return ERROR_COMMAND_SYNTAX_ERROR;
struct pld_device *device = get_pld_device_by_name_or_numstr(CMD_ARGV[i]);
if (!device || !device->driver) {
command_print(CMD, "pld device '#%s' is out of bounds or unknown", CMD_ARGV[i]);
return ERROR_FAIL;
}if (!device || !device->driver) { ... }
COMMAND_PARSE_OPTIONAL_NUMBER(int, i, user_num);
struct pld_ipdbg_hub pld_hub;
struct pld_driver *driver = device->driver;
if (!driver->get_ipdbg_hub) {
command_print(CMD, "pld driver has no ipdbg support");
return ERROR_FAIL;
}if (!driver->get_ipdbg_hub) { ... }
if (driver->get_ipdbg_hub(user_num, device, &pld_hub) != ERROR_OK) {
command_print(CMD, "unable to retrieve hub from pld driver");
return ERROR_FAIL;
}if (driver->get_ipdbg_hub(user_num, device, &pld_hub) != ERROR_OK) { ... }
if (!pld_hub.tap) {
command_print(CMD, "no tap received from pld driver");
return ERROR_FAIL;
}if (!pld_hub.tap) { ... }
hub_configured = true;
user_instruction = pld_hub.user_ir_code;
tap = pld_hub.tap;
}else if (strcmp(CMD_ARGV[i], "-pld") == 0) { ... } 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], "-vir") == 0) { ... } else {
command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
return ERROR_FAIL;
}else { ... }
}for (unsigned int i = 1; i < CMD_ARGC; ++i) { ... }
if (!tap) {
command_print(CMD, "no valid tap selected");
return ERROR_FAIL;
}if (!tap) { ... }
if (!hub_configured) {
command_print(CMD, "hub not configured correctly");
return ERROR_FAIL;
}if (!hub_configured) { ... }
if (ipdbg_get_hub_by_name(hub_name)) {
LOG_ERROR("IPDBG hub with name '%s' already exists", hub_name);
return ERROR_FAIL;
}if (ipdbg_get_hub_by_name(hub_name)) { ... }
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;
}if (!virtual_ir) { ... }
virtual_ir->instruction = virtual_ir_instruction;
virtual_ir->length = virtual_ir_length;
virtual_ir->value = virtual_ir_value;
}if (has_virtual_ir) { ... }
if (ipdbg_find_hub(tap, user_instruction, virtual_ir)) {
LOG_ERROR("IPDBG hub for given TAP and user-instruction already exists");
free(virtual_ir);
return ERROR_FAIL;
}if (ipdbg_find_hub(tap, user_instruction, virtual_ir)) { ... }
return ipdbg_create_hub(tap, user_instruction, data_register_length, virtual_ir, hub_name, cmd);
}{ ... }
static const struct command_registration ipdbg_config_command_handlers[] = {
{
.name = "create-hub",
.mode = COMMAND_ANY,
.handler = handle_ipdbg_create_hub_command,
.help = "create a IPDBG Hub",
.usage = "name.ipdbghub (-tap device.tap -ir ir_value [dr_length] |"
" -pld name.pld [user]) [-vir [vir_value [length [instr_code]]]]",
...},
COMMAND_REGISTRATION_DONE
...};
static const struct command_registration ipdbg_command_handlers[] = {
{
.name = "ipdbg",
.mode = COMMAND_ANY,
.help = "IPDBG Hub/Host commands.",
.usage = "",
.chain = ipdbg_config_command_handlers,
...},
COMMAND_REGISTRATION_DONE
...};
int ipdbg_register_commands(struct command_context *cmd_ctx)
{
return register_commands(cmd_ctx, NULL, ipdbg_command_handlers);
}{ ... }