diff options
| -rw-r--r-- | src/jtag/zy1000/zy1000.c | 155 |
1 files changed, 67 insertions, 88 deletions
diff --git a/src/jtag/zy1000/zy1000.c b/src/jtag/zy1000/zy1000.c index a844dc3a..6f75e59b 100644 --- a/src/jtag/zy1000/zy1000.c +++ b/src/jtag/zy1000/zy1000.c @@ -492,103 +492,93 @@ static void shiftValueInnerFlip(const tap_state_t state, const tap_state_t endSt } #endif -static void gotoEndState(tap_state_t end_state) +// here we shuffle N bits out/in +static __inline void scanBits(const uint8_t *out_value, uint8_t *in_value, int num_bits, bool pause, tap_state_t shiftState, tap_state_t end_state) { - setCurrentState(end_state); -} - -static __inline void scanFields(int num_fields, const struct scan_field *fields, tap_state_t shiftState, int pause) -{ - int i; - int j; - int k; - - for (i = 0; i < num_fields; i++) + tap_state_t pause_state = shiftState; + for (int j = 0; j < num_bits; j += 32) { - cyg_uint32 value; - - uint8_t *inBuffer = NULL; - - - // figure out where to store the input data - int num_bits = fields[i].num_bits; - if (fields[i].in_value != NULL) + int k = num_bits - j; + if (k > 32) { - inBuffer = fields[i].in_value; + k = 32; + /* we have more to shift out */ + } else if (pause) + { + /* this was the last to shift out this time */ + pause_state = end_state; } - // here we shuffle N bits out/in - j = 0; - while (j < num_bits) + // we have (num_bits + 7)/8 bytes of bits to toggle out. + // bits are pushed out LSB to MSB + cyg_uint32 value; + value = 0; + if (out_value != NULL) { - tap_state_t pause_state; - int l; - k = num_bits-j; - pause_state = (shiftState == TAP_DRSHIFT)?TAP_DRSHIFT:TAP_IRSHIFT; - if (k > 32) - { - k = 32; - /* we have more to shift out */ - } else if (pause&&(i == num_fields-1)) + for (int l = 0; l < k; l += 8) { - /* this was the last to shift out this time */ - pause_state = (shiftState==TAP_DRSHIFT)?TAP_DRPAUSE:TAP_IRPAUSE; + value|=out_value[(j + l)/8]<<l; } + } + /* mask away unused bits for easier debugging */ + if (k < 32) + { + value&=~(((uint32_t)0xffffffff) << k); + } else + { + /* Shifting by >= 32 is not defined by the C standard + * and will in fact shift by &0x1f bits on nios */ + } - // we have (num_bits + 7)/8 bytes of bits to toggle out. - // bits are pushed out LSB to MSB - value = 0; - if (fields[i].out_value != NULL) - { - for (l = 0; l < k; l += 8) - { - value|=fields[i].out_value[(j + l)/8]<<l; - } - } - /* mask away unused bits for easier debugging */ - if (k < 32) - { - value&=~(((uint32_t)0xffffffff) << k); - } else - { - /* Shifting by >= 32 is not defined by the C standard - * and will in fact shift by &0x1f bits on nios */ - } + shiftValueInner(shiftState, pause_state, k, value); - shiftValueInner(shiftState, pause_state, k, value); + if (in_value != NULL) + { + // data in, LSB to MSB + value = getShiftValue(); + // we're shifting in data to MSB, shift data to be aligned for returning the value + value >>= 32-k; - if (inBuffer != NULL) + for (int l = 0; l < k; l += 8) { - // data in, LSB to MSB - value = getShiftValue(); - // we're shifting in data to MSB, shift data to be aligned for returning the value - value >>= 32-k; - - for (l = 0; l < k; l += 8) - { - inBuffer[(j + l)/8]=(value >> l)&0xff; - } + in_value[(j + l)/8]=(value >> l)&0xff; } - j += k; } } } +static __inline void scanFields(int num_fields, const struct scan_field *fields, tap_state_t shiftState, tap_state_t end_state) +{ + for (int i = 0; i < num_fields; i++) + { + scanBits(fields[i].out_value, + fields[i].in_value, + fields[i].num_bits, + (i == num_fields-1), + shiftState, + end_state); + } +} + int interface_jtag_add_ir_scan(struct jtag_tap *active, const struct scan_field *fields, tap_state_t state) { int scan_size = 0; struct jtag_tap *tap, *nextTap; + tap_state_t pause_state = TAP_IRSHIFT; for (tap = jtag_tap_next_enabled(NULL); tap!= NULL; tap = nextTap) { nextTap = jtag_tap_next_enabled(tap); - bool pause = (nextTap==NULL); + if (nextTap==NULL) + { + pause_state = state; + } scan_size = tap->ir_length; /* search the list */ if (tap == active) { - scanFields(1, fields, TAP_IRSHIFT, pause); + scanFields(1, fields, TAP_IRSHIFT, pause_state); /* update device information */ buf_cpy(fields[0].out_value, tap->cur_instr, scan_size); @@ -597,12 +587,11 @@ int interface_jtag_add_ir_scan(struct jtag_tap *active, const struct scan_field { /* if a device isn't listed, set it to BYPASS */ assert(scan_size <= 32); - shiftValueInner(TAP_IRSHIFT, pause?TAP_IRPAUSE:TAP_IRSHIFT, scan_size, 0xffffffff); + shiftValueInner(TAP_IRSHIFT, pause_state, scan_size, 0xffffffff); tap->bypass = 1; } } - gotoEndState(state); return ERROR_OK; } @@ -613,51 +602,41 @@ int interface_jtag_add_ir_scan(struct jtag_tap *active, const struct scan_field int interface_jtag_add_plain_ir_scan(int num_bits, const uint8_t *out_bits, uint8_t *in_bits, tap_state_t state) { - struct scan_field field; - field.num_bits = num_bits; - field.out_value = out_bits; - field.in_value = in_bits; - - scanFields(1, &field, TAP_IRSHIFT, 1); - gotoEndState(state); - + scanBits(out_bits, in_bits, num_bits, true, TAP_IRSHIFT, state); return ERROR_OK; } int interface_jtag_add_dr_scan(struct jtag_tap *active, int num_fields, const struct scan_field *fields, tap_state_t state) { struct jtag_tap *tap, *nextTap; + tap_state_t pause_state = TAP_DRSHIFT; for (tap = jtag_tap_next_enabled(NULL); tap!= NULL; tap = nextTap) { nextTap = jtag_tap_next_enabled(tap); - bool pause = (nextTap==NULL); + if (nextTap==NULL) + { + pause_state = state; + } /* Find a range of fields to write to this tap */ if (tap == active) { assert(!tap->bypass); - scanFields(num_fields, fields, TAP_DRSHIFT, pause); + scanFields(num_fields, fields, TAP_DRSHIFT, pause_state); } else { /* Shift out a 0 for disabled tap's */ assert(tap->bypass); - shiftValueInner(TAP_DRSHIFT, pause?TAP_DRPAUSE:TAP_DRSHIFT, 1, 0); + shiftValueInner(TAP_DRSHIFT, pause_state, 1, 0); } } - gotoEndState(state); return ERROR_OK; } int interface_jtag_add_plain_dr_scan(int num_bits, const uint8_t *out_bits, uint8_t *in_bits, tap_state_t state) { - struct scan_field field; - field.num_bits = num_bits; - field.out_value = out_bits; - field.in_value = in_bits; - - scanFields(1, &field, TAP_DRSHIFT, 1); - gotoEndState(state); + scanBits(out_bits, in_bits, num_bits, true, TAP_DRSHIFT, state); return ERROR_OK; } |