1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
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
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
|
/***************************************************************************
* Copyright (C) 2005 by Dominic Rath *
* Dominic.Rath@gmx.de *
* *
* Copyright (C) 2007,2008 Øyvind Harboe *
* oyvind.harboe@zylin.com *
* *
* Copyright (C) 2009 SoftPLC Corporation *
* http://softplc.com *
* dick@softplc.com *
* *
* Copyright (C) 2009 Zachary T Welch *
* zw@superlucidity.net *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "commands.h"
typedef struct cmd_queue_page_s
{
void *address;
size_t used;
struct cmd_queue_page_s *next;
} cmd_queue_page_t;
#define CMD_QUEUE_PAGE_SIZE (1024 * 1024)
static cmd_queue_page_t *cmd_queue_pages = NULL;
jtag_command_t *jtag_command_queue = NULL;
static jtag_command_t **next_command_pointer = &jtag_command_queue;
void jtag_queue_command(jtag_command_t * cmd)
{
// this command goes on the end, so ensure the queue terminates
cmd->next = NULL;
jtag_command_t **last_cmd = next_command_pointer;
assert(NULL != last_cmd);
assert(NULL == *last_cmd);
*last_cmd = cmd;
// store location where the next command pointer will be stored
next_command_pointer = &cmd->next;
}
void* cmd_queue_alloc(size_t size)
{
cmd_queue_page_t **p_page = &cmd_queue_pages;
int offset;
uint8_t *t;
/*
* WARNING:
* We align/round the *SIZE* per below
* so that all pointers returned by
* this function are reasonably well
* aligned.
*
* If we did not, then an "odd-length" request would cause the
* *next* allocation to be at an *odd* address, and because
* this function has the same type of api as malloc() - we
* must also return pointers that have the same type of
* alignment.
*
* What I do not/have is a reasonable portable means
* to align by...
*
* The solution here, is based on these suggestions.
* http://gcc.gnu.org/ml/gcc-help/2008-12/msg00041.html
*
*/
union worse_case_align {
int i;
long l;
float f;
void *v;
};
#define ALIGN_SIZE (sizeof(union worse_case_align))
/* The alignment process. */
size = (size + ALIGN_SIZE -1) & (~(ALIGN_SIZE-1));
/* Done... */
if (*p_page)
{
while ((*p_page)->next)
p_page = &((*p_page)->next);
if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)
p_page = &((*p_page)->next);
}
if (!*p_page)
{
*p_page = malloc(sizeof(cmd_queue_page_t));
(*p_page)->used = 0;
(*p_page)->address = malloc(CMD_QUEUE_PAGE_SIZE);
(*p_page)->next = NULL;
}
offset = (*p_page)->used;
(*p_page)->used += size;
t=(uint8_t *)((*p_page)->address);
return t + offset;
}
void cmd_queue_free(void)
{
cmd_queue_page_t *page = cmd_queue_pages;
while (page)
{
cmd_queue_page_t *last = page;
free(page->address);
page = page->next;
free(last);
}
cmd_queue_pages = NULL;
}
void jtag_command_queue_reset(void)
{
cmd_queue_free();
jtag_command_queue = NULL;
next_command_pointer = &jtag_command_queue;
}
enum scan_type jtag_scan_type(const scan_command_t *cmd)
{
int i;
int type = 0;
for (i = 0; i < cmd->num_fields; i++)
{
if (cmd->fields[i].in_value)
type |= SCAN_IN;
if (cmd->fields[i].out_value)
type |= SCAN_OUT;
}
return type;
}
int jtag_scan_size(const scan_command_t *cmd)
{
int bit_count = 0;
int i;
/* count bits in scan command */
for (i = 0; i < cmd->num_fields; i++)
{
bit_count += cmd->fields[i].num_bits;
}
return bit_count;
}
int jtag_build_buffer(const scan_command_t *cmd, uint8_t **buffer)
{
int bit_count = 0;
int i;
bit_count = jtag_scan_size(cmd);
*buffer = calloc(1,CEIL(bit_count, 8));
bit_count = 0;
#ifdef _DEBUG_JTAG_IO_
LOG_DEBUG("%s num_fields: %i", cmd->ir_scan ? "IRSCAN" : "DRSCAN", cmd->num_fields);
#endif
for (i = 0; i < cmd->num_fields; i++)
{
if (cmd->fields[i].out_value)
{
#ifdef _DEBUG_JTAG_IO_
char* char_buf = buf_to_str(cmd->fields[i].out_value, (cmd->fields[i].num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : cmd->fields[i].num_bits, 16);
#endif
buf_set_buf(cmd->fields[i].out_value, 0, *buffer, bit_count, cmd->fields[i].num_bits);
#ifdef _DEBUG_JTAG_IO_
LOG_DEBUG("fields[%i].out_value[%i]: 0x%s", i, cmd->fields[i].num_bits, char_buf);
free(char_buf);
#endif
}
else
{
#ifdef _DEBUG_JTAG_IO_
LOG_DEBUG("fields[%i].out_value[%i]: NULL", i, cmd->fields[i].num_bits);
#endif
}
bit_count += cmd->fields[i].num_bits;
}
#ifdef _DEBUG_JTAG_IO_
//LOG_DEBUG("bit_count totalling: %i", bit_count );
#endif
return bit_count;
}
int jtag_read_buffer(uint8_t *buffer, const scan_command_t *cmd)
{
int i;
int bit_count = 0;
int retval;
/* we return ERROR_OK, unless a check fails, or a handler reports a problem */
retval = ERROR_OK;
for (i = 0; i < cmd->num_fields; i++)
{
/* if neither in_value nor in_handler
* are specified we don't have to examine this field
*/
if (cmd->fields[i].in_value)
{
int num_bits = cmd->fields[i].num_bits;
uint8_t *captured = buf_set_buf(buffer, bit_count, malloc(CEIL(num_bits, 8)), 0, num_bits);
#ifdef _DEBUG_JTAG_IO_
char *char_buf = buf_to_str(captured, (num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : num_bits, 16);
LOG_DEBUG("fields[%i].in_value[%i]: 0x%s", i, num_bits, char_buf);
free(char_buf);
#endif
if (cmd->fields[i].in_value)
{
buf_cpy(captured, cmd->fields[i].in_value, num_bits);
}
free(captured);
}
bit_count += cmd->fields[i].num_bits;
}
return retval;
}
|
