- uint32_t scan_size_bits, scan_size_bytes, bits_last_scan;
- uint32_t scans_max_payload, bytecount;
- uint8_t *tdi_buffer_start = NULL, *tdi_buffer = NULL;
- uint8_t *tdo_buffer_start = NULL, *tdo_buffer = NULL;
-
- uint8_t first_tms_count, first_tms_sequence;
- uint8_t last_tms_count, last_tms_sequence;
-
- uint8_t tms_count_pause, tms_sequence_pause;
- uint8_t tms_count_resume, tms_sequence_resume;
-
- uint8_t tms_count_start, tms_sequence_start;
- uint8_t tms_count_end, tms_sequence_end;
-
- enum scan_type type;
- int ret;
-
- /* Determine scan size */
- scan_size_bits = jtag_scan_size(cmd->cmd.scan);
- scan_size_bytes = DIV_ROUND_UP(scan_size_bits, 8);
-
- /* Determine scan type (IN/OUT/IO) */
- type = jtag_scan_type(cmd->cmd.scan);
-
- /* Determine number of scan commands */
- scans_max_payload = scan_size_bytes / 58;
-
- /* Determine size of last shift command */
- bits_last_scan = scan_size_bits - (scans_max_payload * 58 * 8);
-
- /* Allocate TDO buffer if required */
- if ((type == SCAN_IN) || (type == SCAN_IO)) {
- tdo_buffer_start = calloc(sizeof(uint8_t), scan_size_bytes);
-
- if (tdo_buffer_start == NULL) {
- return ERROR_FAIL;
- }
-
- tdo_buffer = tdo_buffer_start;
- }
-
- /* Fill TDI buffer if required */
- if ((type == SCAN_OUT) || (type == SCAN_IO)) {
- jtag_build_buffer(cmd->cmd.scan, &tdi_buffer_start);
- tdi_buffer = tdi_buffer_start;
- }
-
- /* Get TAP state transitions */
- if (cmd->cmd.scan->ir_scan) {
- ulink_set_end_state(TAP_IRSHIFT);
- first_tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
- first_tms_sequence = tap_get_tms_path(tap_get_state(), tap_get_end_state());
-
- tap_set_state(TAP_IRSHIFT);
- tap_set_end_state(cmd->cmd.scan->end_state);
- last_tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
- last_tms_sequence = tap_get_tms_path(tap_get_state(), tap_get_end_state());
-
- /* TAP state transitions for split scans */
- tms_count_pause = tap_get_tms_path_len(TAP_IRSHIFT, TAP_IRPAUSE);
- tms_sequence_pause = tap_get_tms_path(TAP_IRSHIFT, TAP_IRPAUSE);
- tms_count_resume = tap_get_tms_path_len(TAP_IRPAUSE, TAP_IRSHIFT);
- tms_sequence_resume = tap_get_tms_path(TAP_IRPAUSE, TAP_IRSHIFT);
- }
- else {
- ulink_set_end_state(TAP_DRSHIFT);
- first_tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
- first_tms_sequence = tap_get_tms_path(tap_get_state(), tap_get_end_state());
-
- tap_set_state(TAP_DRSHIFT);
- tap_set_end_state(cmd->cmd.scan->end_state);
- last_tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
- last_tms_sequence = tap_get_tms_path(tap_get_state(), tap_get_end_state());
-
- /* TAP state transitions for split scans */
- tms_count_pause = tap_get_tms_path_len(TAP_DRSHIFT, TAP_DRPAUSE);
- tms_sequence_pause = tap_get_tms_path(TAP_DRSHIFT, TAP_DRPAUSE);
- tms_count_resume = tap_get_tms_path_len(TAP_DRPAUSE, TAP_DRSHIFT);
- tms_sequence_resume = tap_get_tms_path(TAP_DRPAUSE, TAP_DRSHIFT);
- }
-
- /* Generate scan commands with full payload */
- bytecount = scan_size_bytes;
- while (bytecount > 0) {
- if (bytecount == scan_size_bytes) {
- /* This is the first scan */
- tms_count_start = first_tms_count;
- tms_sequence_start = first_tms_sequence;
- }
- else {
- /* Resume from previous scan */
- tms_count_start = tms_count_resume;
- tms_sequence_start = tms_sequence_resume;
- }
-
- if (bytecount > 58) { /* Full scan, at least one scan will follow */
- tms_count_end = tms_count_pause;
- tms_sequence_end = tms_sequence_pause;
-
- ret = ulink_append_scan_cmd(device, type, 58 * 8, tdi_buffer,
- tdo_buffer_start, tdo_buffer, tms_count_start, tms_sequence_start,
- tms_count_end, tms_sequence_end, cmd, false);
-
- bytecount -= 58;
-
- /* Update TDI and TDO buffer pointers */
- if (tdi_buffer_start != NULL) {
- tdi_buffer += 58;
- }
- if (tdo_buffer_start != NULL) {
- tdo_buffer += 58;
- }
- }
- else if (bytecount == 58) { /* Full scan, no further scans */
- tms_count_end = last_tms_count;
- tms_sequence_end = last_tms_sequence;
-
- ret = ulink_append_scan_cmd(device, type, 58 * 8, tdi_buffer,
- tdo_buffer_start, tdo_buffer, tms_count_start, tms_sequence_start,
- tms_count_end, tms_sequence_end, cmd, true);
-
- bytecount = 0;
- }
- else { /* Scan with less than maximum payload, no further scans */
- tms_count_end = last_tms_count;
- tms_sequence_end = last_tms_sequence;
-
- ret = ulink_append_scan_cmd(device, type, bits_last_scan, tdi_buffer,
- tdo_buffer_start, tdo_buffer, tms_count_start, tms_sequence_start,
- tms_count_end, tms_sequence_end, cmd, true);
-
- bytecount = 0;
- }
-
- if (ret != ERROR_OK) {
- free(tdi_buffer_start);
- return ret;
- }
- }
-
- free(tdi_buffer_start);
-
- /* Set current state to the end state requested by the command */
- tap_set_state(cmd->cmd.scan->end_state);
-
- return ERROR_OK;