1 /*
2 * $Id$
3 *
4 * Copyright (C) 2002, 2003 ETC s.r.o.
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
19 * 02111-1307, USA.
20 *
21 * Written by Marcel Telka <marcel@telka.sk>, 2002, 2003.
22 *
23 */
24
25 #include <sysdep.h>
26
27 #include <inttypes.h>
28 #include <stdlib.h>
29 #include <string.h>
30
31 #include <urjtag/error.h>
32 #include <urjtag/log.h>
33 #include <urjtag/tap_register.h>
34
35 urj_tap_register_t *
urj_tap_register_alloc(int len)36 urj_tap_register_alloc (int len)
37 {
38 urj_tap_register_t *tr;
39
40 if (len < 1)
41 {
42 urj_error_set (URJ_ERROR_INVALID, "len < 1");
43 return NULL;
44 }
45
46 tr = malloc (sizeof (urj_tap_register_t));
47 if (!tr)
48 {
49 urj_error_set (URJ_ERROR_OUT_OF_MEMORY, "malloc(%zd) fails",
50 sizeof (urj_tap_register_t));
51 return NULL;
52 }
53
54 tr->data = malloc (len);
55 if (!tr->data)
56 {
57 free (tr);
58 urj_error_set (URJ_ERROR_OUT_OF_MEMORY, "malloc(%zd) fails",
59 (size_t) len);
60 return NULL;
61 }
62
63 memset (tr->data, 0, len);
64
65 tr->string = malloc (len + 1);
66 if (!tr->string)
67 {
68 free (tr->data);
69 free (tr);
70 urj_error_set (URJ_ERROR_OUT_OF_MEMORY, "malloc(%zd) fails",
71 (size_t) (len + 1));
72 return NULL;
73 }
74
75 tr->len = len;
76 tr->string[len] = '\0';
77
78 return tr;
79 }
80
81 urj_tap_register_t *
urj_tap_register_realloc(urj_tap_register_t * tr,int new_len)82 urj_tap_register_realloc (urj_tap_register_t *tr, int new_len)
83 {
84 if (!tr)
85 return urj_tap_register_alloc (new_len);
86
87 if (new_len < 1)
88 {
89 urj_error_set (URJ_ERROR_INVALID, "new_len < 1");
90 return NULL;
91 }
92
93 tr->data = realloc (tr->data, new_len);
94
95 if (!tr->data)
96 {
97 urj_error_set (URJ_ERROR_OUT_OF_MEMORY, "realloc(%d) fails",
98 new_len);
99 return NULL;
100 }
101
102 if (tr->len < new_len)
103 memset (tr->data + tr->len, 0, (new_len - tr->len));
104
105 tr->len = new_len;
106
107 return tr;
108 }
109
110 urj_tap_register_t *
urj_tap_register_duplicate(const urj_tap_register_t * tr)111 urj_tap_register_duplicate (const urj_tap_register_t *tr)
112 {
113 if (!tr)
114 {
115 urj_error_set (URJ_ERROR_INVALID, "tr == NULL");
116 return NULL;
117 }
118
119 return urj_tap_register_init (urj_tap_register_alloc (tr->len),
120 urj_tap_register_get_string (tr));
121 }
122
123 void
urj_tap_register_free(urj_tap_register_t * tr)124 urj_tap_register_free (urj_tap_register_t *tr)
125 {
126 if (tr)
127 {
128 free (tr->data);
129 free (tr->string);
130 }
131 free (tr);
132 }
133
134 urj_tap_register_t *
urj_tap_register_fill(urj_tap_register_t * tr,int val)135 urj_tap_register_fill (urj_tap_register_t *tr, int val)
136 {
137 if (tr)
138 memset (tr->data, val & 1, tr->len);
139
140 return tr;
141 }
142
143 int
urj_tap_register_set_string(urj_tap_register_t * tr,const char * str)144 urj_tap_register_set_string (urj_tap_register_t *tr, const char *str)
145 {
146 if (!tr)
147 {
148 urj_error_set (URJ_ERROR_INVALID, "tr == NULL");
149 return URJ_STATUS_FAIL;
150 }
151
152 if (strncmp (str, "0x", 2) == 0)
153 {
154 /* Hex values */
155 uint64_t val;
156
157 if (sscanf (str, "%"PRIX64, &val) != 1)
158 {
159 urj_error_set (URJ_ERROR_SYNTAX,
160 _("invalid hex string '%s'"),
161 str);
162 return URJ_STATUS_FAIL;
163 }
164 return urj_tap_register_set_value (tr, val);
165 }
166 else
167 {
168 /* Bit string */
169 unsigned int bit;
170
171 if (strspn (str, "01") != strlen (str))
172 {
173 urj_error_set (URJ_ERROR_SYNTAX,
174 _("bit patterns should be 0s and 1s, not '%s'"),
175 str);
176 return URJ_STATUS_FAIL;
177 }
178 else if (tr->len != strlen (str))
179 {
180 urj_error_set (URJ_ERROR_OUT_OF_BOUNDS,
181 _("register length %d mismatch: %zd"),
182 tr->len, strlen (str));
183 return URJ_STATUS_FAIL;
184 }
185
186 for (bit = 0; str[bit]; ++bit)
187 tr->data[tr->len - 1 - bit] = (str[bit] == '1');
188
189 return URJ_STATUS_OK;
190 }
191 }
192
193 int
urj_tap_register_set_string_bit_range(urj_tap_register_t * tr,const char * str,int msb,int lsb)194 urj_tap_register_set_string_bit_range (urj_tap_register_t *tr, const char *str, int msb, int lsb)
195 {
196 int step = msb >= lsb ? 1 : -1;
197 int len = msb >= lsb ? msb - lsb + 1 : lsb - msb + 1;
198 int sidx;
199
200 if (!tr)
201 {
202 urj_error_set (URJ_ERROR_INVALID, "tr == NULL");
203 return URJ_STATUS_FAIL;
204 }
205
206 if (msb > tr->len - 1 || lsb > tr->len - 1 || msb < 0 || lsb < 0)
207 {
208 urj_error_set (URJ_ERROR_OUT_OF_BOUNDS,
209 _("register %d:%d will not fit in %d bits"),
210 msb, lsb, tr->len);
211 return URJ_STATUS_FAIL;
212 }
213
214 if (strncmp (str, "0x", 2) == 0)
215 {
216 /* Hex values */
217 uint64_t val;
218
219 //printf("in urj_tap_register_set_string_bit_range(%s) hexmode\n", str);
220 if (sscanf (str, "%"PRIX64, &val) != 1)
221 {
222 urj_error_set (URJ_ERROR_SYNTAX,
223 _("invalid hex string '%s'"),
224 str);
225 return URJ_STATUS_FAIL;
226 }
227 return urj_tap_register_set_value_bit_range (tr, val, msb, lsb);
228 }
229 else
230 {
231 /* Bit string */
232 int bit;
233
234 //printf("in urj_tap_register_set_string_bit_range(%s, %d, %d) binmode len=%d step=%d\n", str, msb, lsb, len, step);
235 if (strspn (str, "01") != strlen (str))
236 {
237 urj_error_set (URJ_ERROR_SYNTAX,
238 _("bit patterns should be 0s and 1s, not '%s'"),
239 str);
240 return URJ_STATUS_FAIL;
241 }
242 else if (len != strlen (str))
243 {
244 urj_error_set (URJ_ERROR_OUT_OF_BOUNDS,
245 _("register subfield length %d mismatch: %zd"),
246 len, strlen (str));
247 return URJ_STATUS_FAIL;
248 }
249
250 for (sidx = 0, bit = msb; bit*step >= lsb*step; bit -= step, sidx++)
251 {
252 tr->data[bit] = (str[sidx] == '1');
253 }
254
255 return URJ_STATUS_OK;
256 }
257 }
258
259 int
urj_tap_register_set_value_bit_range(urj_tap_register_t * tr,uint64_t val,int msb,int lsb)260 urj_tap_register_set_value_bit_range (urj_tap_register_t *tr, uint64_t val, int msb, int lsb)
261 {
262 int bit;
263 int step = msb >= lsb ? 1 : -1;
264
265 if (!tr)
266 {
267 urj_error_set (URJ_ERROR_INVALID, "tr == NULL");
268 return URJ_STATUS_FAIL;
269 }
270
271 if (msb > tr->len - 1 || lsb > tr->len - 1 || msb < 0 || lsb < 0)
272 {
273 urj_error_set (URJ_ERROR_OUT_OF_BOUNDS,
274 _("register %d:%d will not fit in %d bits"),
275 msb, lsb, tr->len);
276 return URJ_STATUS_FAIL;
277 }
278
279 for (bit = lsb; bit * step <= msb * step; bit += step)
280 {
281 tr->data[bit] = !!(val & 1);
282 val >>= 1;
283 }
284
285 return URJ_STATUS_OK;
286 }
287
288 int
urj_tap_register_set_value(urj_tap_register_t * tr,uint64_t val)289 urj_tap_register_set_value (urj_tap_register_t *tr, uint64_t val)
290 {
291 return urj_tap_register_set_value_bit_range (tr, val, tr->len - 1, 0);
292 }
293
294 const char *
urj_tap_register_get_string_bit_range(const urj_tap_register_t * tr,int msb,int lsb)295 urj_tap_register_get_string_bit_range (const urj_tap_register_t *tr, int msb, int lsb)
296 {
297 int bit;
298 int string_idx;
299 int step = msb >= lsb ? 1 : -1;
300
301 if (!tr)
302 {
303 urj_error_set (URJ_ERROR_INVALID, "tr == NULL");
304 return NULL;
305 }
306
307 if (msb > tr->len - 1 || lsb > tr->len - 1 || msb < 0 || lsb < 0)
308 {
309 urj_error_set (URJ_ERROR_INVALID, "msb or lsb out of range");
310 return NULL;
311 }
312
313 for (bit = msb, string_idx = 0; bit * step >= lsb * step; bit -= step, string_idx++)
314 {
315 tr->string[string_idx] = (tr->data[bit] & 1) ? '1' : '0';
316 }
317 tr->string[string_idx] = '\0';
318
319 return tr->string;
320 }
321
322 const char *
urj_tap_register_get_string(const urj_tap_register_t * tr)323 urj_tap_register_get_string (const urj_tap_register_t *tr)
324 {
325 int i;
326
327 if (!tr)
328 {
329 urj_error_set (URJ_ERROR_INVALID, "tr == NULL");
330 return NULL;
331 }
332
333 for (i = 0; i < tr->len; i++)
334 tr->string[tr->len - 1 - i] = (tr->data[i] & 1) ? '1' : '0';
335
336 return tr->string;
337 }
338
339 uint64_t
urj_tap_register_get_value_bit_range(const urj_tap_register_t * tr,int msb,int lsb)340 urj_tap_register_get_value_bit_range (const urj_tap_register_t *tr, int msb, int lsb)
341 {
342 int bit;
343 uint64_t l, b;
344 int step = msb >= lsb ? 1 : -1;
345
346 if (!tr)
347 return 0;
348
349 if (msb > tr->len - 1 || lsb > tr->len - 1 || msb < 0 || lsb < 0)
350 return 0;
351
352 l = 0;
353 b = 1;
354 for (bit = lsb; bit * step <= msb * step; bit += step)
355 {
356 if (tr->data[bit] & 1)
357 l |= b;
358 b <<= 1;
359 }
360
361 return l;
362 }
363
364 uint64_t
urj_tap_register_get_value(const urj_tap_register_t * tr)365 urj_tap_register_get_value (const urj_tap_register_t *tr)
366 {
367 return urj_tap_register_get_value_bit_range (tr, tr->len - 1, 0);
368 }
369
370 int
urj_tap_register_all_bits_same_value(const urj_tap_register_t * tr)371 urj_tap_register_all_bits_same_value (const urj_tap_register_t *tr)
372 {
373 int i, value;
374 if (!tr)
375 return -1;
376 if (tr->len < 0)
377 return -1;
378
379 /* Return -1 if any of the bits in the register
380 * differs from the others; the value otherwise. */
381
382 value = tr->data[0] & 1;
383
384 for (i = 1; i < tr->len; i++)
385 {
386 if ((tr->data[i] & 1) != value)
387 return -1;
388 }
389 return value;
390 }
391
392 urj_tap_register_t *
urj_tap_register_init(urj_tap_register_t * tr,const char * value)393 urj_tap_register_init (urj_tap_register_t *tr, const char *value)
394 {
395 int i;
396
397 const char *p;
398
399 if (!value || !tr)
400 return tr;
401
402 p = strchr (value, '\0');
403
404 for (i = 0; i < tr->len; i++)
405 {
406 if (p == value)
407 tr->data[i] = 0;
408 else
409 {
410 p--;
411 tr->data[i] = (*p == '0') ? 0 : 1;
412 }
413 }
414
415 return tr;
416 }
417
418 int
urj_tap_register_compare(const urj_tap_register_t * tr,const urj_tap_register_t * tr2)419 urj_tap_register_compare (const urj_tap_register_t *tr,
420 const urj_tap_register_t *tr2)
421 {
422 int i;
423
424 if (!tr && !tr2)
425 return 0;
426
427 if (!tr || !tr2)
428 return 1;
429
430 if (tr->len != tr2->len)
431 return 1;
432
433 for (i = 0; i < tr->len; i++)
434 if (tr->data[i] != tr2->data[i])
435 return 1;
436
437 return 0;
438 }
439
440 int
urj_tap_register_match(const urj_tap_register_t * tr,const char * expr)441 urj_tap_register_match (const urj_tap_register_t *tr, const char *expr)
442 {
443 int i;
444 const char *s;
445
446 if (!tr || !expr || (tr->len != strlen (expr)))
447 return 0;
448
449 s = urj_tap_register_get_string (tr);
450
451 for (i = 0; i < tr->len; i++)
452 if ((expr[i] != '?') && (expr[i] != s[i]))
453 return 0;
454
455 return 1;
456 }
457
458 urj_tap_register_t *
urj_tap_register_inc(urj_tap_register_t * tr)459 urj_tap_register_inc (urj_tap_register_t *tr)
460 {
461 int i;
462
463 if (!tr)
464 return NULL;
465
466 for (i = 0; i < tr->len; i++)
467 {
468 tr->data[i] ^= 1;
469
470 if (tr->data[i] == 1)
471 break;
472 }
473
474 return tr;
475 }
476
477 urj_tap_register_t *
urj_tap_register_dec(urj_tap_register_t * tr)478 urj_tap_register_dec (urj_tap_register_t *tr)
479 {
480 int i;
481
482 if (!tr)
483 return NULL;
484
485 for (i = 0; i < tr->len; i++)
486 {
487 tr->data[i] ^= 1;
488
489 if (tr->data[i] == 0)
490 break;
491 }
492
493 return tr;
494 }
495
496 urj_tap_register_t *
urj_tap_register_shift_right(urj_tap_register_t * tr,int shift)497 urj_tap_register_shift_right (urj_tap_register_t *tr, int shift)
498 {
499 int i;
500
501 if (!tr)
502 return NULL;
503
504 if (shift < 1)
505 return tr;
506
507 for (i = 0; i < tr->len; i++)
508 {
509 if (i + shift < tr->len)
510 tr->data[i] = tr->data[i + shift];
511 else
512 tr->data[i] = 0;
513 }
514
515 return tr;
516 }
517
518 urj_tap_register_t *
urj_tap_register_shift_left(urj_tap_register_t * tr,int shift)519 urj_tap_register_shift_left (urj_tap_register_t *tr, int shift)
520 {
521 int i;
522
523 if (!tr)
524 return NULL;
525
526 if (shift < 1)
527 return tr;
528
529 for (i = tr->len - 1; i >= 0; i--)
530 {
531 if (i - shift >= 0)
532 tr->data[i] = tr->data[i - shift];
533 else
534 tr->data[i] = 0;
535 }
536
537 return tr;
538 }
539