jtag.c

/***************************************************************************
 *   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                                                      *
 *                                                                         *
 *   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 "replacements.h"

#include "jtag.h"

#include "command.h"
#include "log.h"

#include "stdlib.h"
#include "string.h"
#include <unistd.h>

/* note that this is not marked as static as it must be available from outside jtag.c for those
   that implement the jtag_xxx() minidriver layer
*/
int jtag_error=ERROR_OK;

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;

char* jtag_event_strings[] =
{
	"JTAG controller reset (RESET or TRST)"
};

const Jim_Nvp nvp_jtag_tap_event[] = {
	{ .value = JTAG_TAP_EVENT_ENABLE,       .name = "tap-enable" },
	{ .value = JTAG_TAP_EVENT_DISABLE,      .name = "tap-disable" },

	{ .name = NULL, .value = -1 }
};

int jtag_trst = 0;
int jtag_srst = 0;

jtag_command_t *jtag_command_queue = NULL;
jtag_command_t **last_comand_pointer = &jtag_command_queue;
static jtag_tap_t *jtag_all_taps = NULL;

enum reset_types jtag_reset_config = RESET_NONE;
tap_state_t cmd_queue_end_state = TAP_RESET;
tap_state_t cmd_queue_cur_state = TAP_RESET;

int jtag_verify_capture_ir = 1;

/* how long the OpenOCD should wait before attempting JTAG communication after reset lines deasserted (in ms) */
int jtag_nsrst_delay = 0; /* default to no nSRST delay */
int jtag_ntrst_delay = 0; /* default to no nTRST delay */

/* maximum number of JTAG devices expected in the chain
 */
#define JTAG_MAX_CHAIN_SIZE 20

/* callbacks to inform high-level handlers about JTAG state changes */
jtag_event_callback_t *jtag_event_callbacks;

/* speed in kHz*/
static int speed_khz = 0;
/* flag if the kHz speed was defined */
static int hasKHz = 0;

/* jtag interfaces (parport, FTDI-USB, TI-USB, ...)
 */

#if BUILD_ECOSBOARD == 1
	extern jtag_interface_t zy1000_interface;
#endif

#if BUILD_PARPORT == 1
	extern jtag_interface_t parport_interface;
#endif

#if BUILD_DUMMY == 1
	extern jtag_interface_t dummy_interface;
#endif

#if BUILD_FT2232_FTD2XX == 1
	extern jtag_interface_t ft2232_interface;
#endif

#if BUILD_FT2232_LIBFTDI == 1
	extern jtag_interface_t ft2232_interface;
#endif

#if BUILD_AMTJTAGACCEL == 1
	extern jtag_interface_t amt_jtagaccel_interface;
#endif

#if BUILD_EP93XX == 1
	extern jtag_interface_t ep93xx_interface;
#endif

#if BUILD_AT91RM9200 == 1
	extern jtag_interface_t at91rm9200_interface;
#endif

#if BUILD_GW16012 == 1
	extern jtag_interface_t gw16012_interface;
#endif

#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
	extern jtag_interface_t presto_interface;
#endif

#if BUILD_USBPROG == 1
	extern jtag_interface_t usbprog_interface;
#endif

#if BUILD_JLINK == 1
	extern jtag_interface_t jlink_interface;
#endif

#if BUILD_VSLLINK == 1
	extern jtag_interface_t vsllink_interface;
#endif

#if BUILD_RLINK == 1
	extern jtag_interface_t rlink_interface;
#endif

#if BUILD_ARMJTAGEW == 1
	extern jtag_interface_t armjtagew_interface;
#endif

jtag_interface_t *jtag_interfaces[] = {
#if BUILD_ECOSBOARD == 1
	&zy1000_interface,
#endif
#if BUILD_PARPORT == 1
	&parport_interface,
#endif
#if BUILD_DUMMY == 1
	&dummy_interface,
#endif
#if BUILD_FT2232_FTD2XX == 1
	&ft2232_interface,
#endif
#if BUILD_FT2232_LIBFTDI == 1
	&ft2232_interface,
#endif
#if BUILD_AMTJTAGACCEL == 1
	&amt_jtagaccel_interface,
#endif
#if BUILD_EP93XX == 1
	&ep93xx_interface,
#endif
#if BUILD_AT91RM9200 == 1
	&at91rm9200_interface,
#endif
#if BUILD_GW16012 == 1
	&gw16012_interface,
#endif
#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
	&presto_interface,
#endif
#if BUILD_USBPROG == 1
	&usbprog_interface,
#endif
#if BUILD_JLINK == 1
	&jlink_interface,
#endif
#if BUILD_VSLLINK == 1
	&vsllink_interface,
#endif
#if BUILD_RLINK == 1
	&rlink_interface,
#endif
#if BUILD_ARMJTAGEW == 1
	&armjtagew_interface,
#endif
	NULL,
};

jtag_interface_t *jtag = NULL;

/* configuration */
jtag_interface_t *jtag_interface = NULL;
int jtag_speed = 0;

/* forward declarations */
void jtag_add_pathmove(int num_states, tap_state_t *path);
void jtag_add_runtest(int num_cycles, tap_state_t endstate);
void jtag_add_end_state(tap_state_t endstate);
void jtag_add_sleep(u32 us);
int jtag_execute_queue(void);
tap_state_t tap_state_by_name(const char *name);

/* jtag commands */
int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *argv);

int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

jtag_tap_t *jtag_AllTaps(void)
{
	return jtag_all_taps;
};

int jtag_NumTotalTaps(void)
{
	jtag_tap_t *t;
	int n;

	n = 0;
	t = jtag_AllTaps();
	while(t){
		n++;
		t = t->next_tap;
	}
	return n;
}

int jtag_NumEnabledTaps(void)
{
	jtag_tap_t *t;
	int n;

	n = 0;
	t = jtag_AllTaps();
	while(t){
		if( t->enabled ){
			n++;
		}
		t = t->next_tap;
	}
	return n;
}

jtag_tap_t *jtag_TapByString( const char *s )
{
	jtag_tap_t *t;
	char *cp;

	t = jtag_AllTaps();
	/* try name first */
	while(t){
		if( 0 == strcmp( t->dotted_name, s ) ){
			break;
		} else {
			t = t->next_tap;
		}
	}
	/* backup plan is by number */
	if( t == NULL ){
		/* ok - is "s" a number? */
		int n;
		n = strtol( s, &cp, 0 );
		if( (s != cp) && (*cp == 0) ){
			/* Then it is... */
			t = jtag_TapByAbsPosition(n);
		}
	}
	return t;
}

jtag_tap_t * jtag_TapByJimObj( Jim_Interp *interp, Jim_Obj *o )
{
	jtag_tap_t *t;
	const char *cp;

	cp = Jim_GetString( o, NULL );
	if(cp == NULL){
		cp = "(unknown)";
		t = NULL;
	}  else {
		t = jtag_TapByString( cp );
	}
	if( t == NULL ){
		Jim_SetResult_sprintf(interp,"Tap: %s is unknown", cp );
	}
	return t;
}

/* returns a pointer to the n-th device in the scan chain */
jtag_tap_t * jtag_TapByAbsPosition( int n )
{
	int orig_n;
	jtag_tap_t *t;

	orig_n = n;
	t = jtag_AllTaps();

	while( t && (n > 0)) {
		n--;
		t = t->next_tap;
	}
	return t;
}

int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv)
{
	jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;

	if (callback == NULL)
	{
		return ERROR_INVALID_ARGUMENTS;
	}

	if (*callbacks_p)
	{
		while ((*callbacks_p)->next)
			callbacks_p = &((*callbacks_p)->next);
		callbacks_p = &((*callbacks_p)->next);
	}

	(*callbacks_p) = malloc(sizeof(jtag_event_callback_t));
	(*callbacks_p)->callback = callback;
	(*callbacks_p)->priv = priv;
	(*callbacks_p)->next = NULL;

	return ERROR_OK;
}

int jtag_unregister_event_callback(int (*callback)(enum jtag_event event, void *priv))
{
	jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;

	if (callback == NULL)
	{
		return ERROR_INVALID_ARGUMENTS;
	}

	while (*callbacks_p)
	{
		jtag_event_callback_t **next = &((*callbacks_p)->next);
		if ((*callbacks_p)->callback == callback)
		{
			free(*callbacks_p);
			*callbacks_p = *next;
		}
		callbacks_p = next;
	}

	return ERROR_OK;
}

int jtag_call_event_callbacks(enum jtag_event event)
{
	jtag_event_callback_t *callback = jtag_event_callbacks;

	LOG_DEBUG("jtag event: %s", jtag_event_strings[event]);

	while (callback)
	{
		callback->callback(event, callback->priv);
		callback = callback->next;
	}

	return ERROR_OK;
}

/* returns a pointer to the pointer of the last command in queue
 * this may be a pointer to the root pointer (jtag_command_queue)
 * or to the next member of the last but one command
 */
jtag_command_t** jtag_get_last_command_p(void)
{
/*	jtag_command_t *cmd = jtag_command_queue;

	if (cmd)
		while (cmd->next)
			cmd = cmd->next;
	else
		return &jtag_command_queue;

	return &cmd->next;*/

	return last_comand_pointer;
}

void* cmd_queue_alloc(size_t size)
{
	cmd_queue_page_t **p_page = &cmd_queue_pages;
	int offset;
	u8 *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=(u8 *)((*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;
}

static void jtag_prelude1(void)
{
	if (jtag_trst == 1)
	{
		LOG_WARNING("JTAG command queued, while TRST is low (TAP in reset)");
		jtag_error=ERROR_JTAG_TRST_ASSERTED;
		return;
	}

	if (cmd_queue_end_state == TAP_RESET)
		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
}

static void jtag_prelude(tap_state_t state)
{
	jtag_prelude1();

	if (state != TAP_INVALID)
		jtag_add_end_state(state);

	cmd_queue_cur_state = cmd_queue_end_state;
}

void jtag_add_ir_scan(int num_fields, scan_field_t *fields, tap_state_t state)
{
	int retval;

	jtag_prelude(state);

	retval=interface_jtag_add_ir_scan(num_fields, fields, cmd_queue_end_state);
	if (retval!=ERROR_OK)
		jtag_error=retval;
}

int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
{
	jtag_command_t **last_cmd;
	jtag_tap_t *tap;
	int j;
	int x;
	int nth_tap;
	int scan_size = 0;

	last_cmd = jtag_get_last_command_p();

	/* allocate memory for a new list member */
	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
	(*last_cmd)->next = NULL;
	last_comand_pointer = &((*last_cmd)->next);
	(*last_cmd)->type = JTAG_SCAN;

	/* allocate memory for ir scan command */
	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
	(*last_cmd)->cmd.scan->ir_scan = 1;
	x = jtag_NumEnabledTaps();
	(*last_cmd)->cmd.scan->num_fields = x;	/* one field per device */
	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(x  * sizeof(scan_field_t));
	(*last_cmd)->cmd.scan->end_state = state;

	nth_tap = -1;
	tap = NULL;
	for(;;){
		int found = 0;

		/* do this here so it is not forgotten */
		tap = jtag_NextEnabledTap(tap);
		if( tap == NULL ){
			break;
		}
		nth_tap++;
		scan_size = tap->ir_length;
		(*last_cmd)->cmd.scan->fields[nth_tap].tap = tap;
		(*last_cmd)->cmd.scan->fields[nth_tap].num_bits = scan_size;
		(*last_cmd)->cmd.scan->fields[nth_tap].in_value = NULL;
		(*last_cmd)->cmd.scan->fields[nth_tap].in_handler = NULL;	/* disable verification by default */

		/* search the list */
		for (j = 0; j < num_fields; j++)
		{
			if (tap == fields[j].tap)
			{
				found = 1;
				(*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
				(*last_cmd)->cmd.scan->fields[nth_tap].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);

				if (jtag_verify_capture_ir)
				{
					if (fields[j].in_handler==NULL)
					{
						jtag_set_check_value((*last_cmd)->cmd.scan->fields+nth_tap, tap->expected, tap->expected_mask, NULL);
					} else
					{
						(*last_cmd)->cmd.scan->fields[nth_tap].in_handler = fields[j].in_handler;
						(*last_cmd)->cmd.scan->fields[nth_tap].in_handler_priv = fields[j].in_handler_priv;
						(*last_cmd)->cmd.scan->fields[nth_tap].in_check_value = tap->expected;
						(*last_cmd)->cmd.scan->fields[nth_tap].in_check_mask = tap->expected_mask;
					}
				}

				tap->bypass = 0;
				break;
			}
		}

		if (!found)
		{
			/* if a tap isn't listed, set it to BYPASS */
			(*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
			(*last_cmd)->cmd.scan->fields[nth_tap].out_mask = NULL;
			tap->bypass = 1;
		}

		/* update device information */
		buf_cpy((*last_cmd)->cmd.scan->fields[nth_tap].out_value, tap->cur_instr, scan_size);
	}

	return ERROR_OK;
}

void jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, tap_state_t state)
{
	int retval;

	jtag_prelude(state);

	retval=interface_jtag_add_plain_ir_scan(num_fields, fields, cmd_queue_end_state);
	if (retval!=ERROR_OK)
		jtag_error=retval;
}

int MINIDRIVER(interface_jtag_add_plain_ir_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
{
	int i;
	jtag_command_t **last_cmd;

	last_cmd = jtag_get_last_command_p();

	/* allocate memory for a new list member */
	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
	(*last_cmd)->next = NULL;
	last_comand_pointer = &((*last_cmd)->next);
	(*last_cmd)->type = JTAG_SCAN;

	/* allocate memory for ir scan command */
	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
	(*last_cmd)->cmd.scan->ir_scan = 1;
	(*last_cmd)->cmd.scan->num_fields = num_fields;
	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
	(*last_cmd)->cmd.scan->end_state = state;

	for( i = 0 ; i < num_fields ; i++ ){
		int num_bits = fields[i].num_bits;
		int num_bytes = CEIL(fields[i].num_bits, 8);
		(*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
		(*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
		(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
		(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
		(*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
		(*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
		(*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
		(*last_cmd)->cmd.scan->fields[i].in_handler = NULL;
		(*last_cmd)->cmd.scan->fields[i].in_handler_priv = NULL;
	}
	return ERROR_OK;
}

void jtag_add_dr_scan(int num_fields, scan_field_t *fields, tap_state_t state)
{
	int retval;

	jtag_prelude(state);

	retval=interface_jtag_add_dr_scan(num_fields, fields, cmd_queue_end_state);
	if (retval!=ERROR_OK)
		jtag_error=retval;
}

int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
{
	int j;
	int nth_tap;
	int bypass_devices = 0;
	int field_count = 0;
	int scan_size;

	jtag_command_t **last_cmd = jtag_get_last_command_p();
	jtag_tap_t *tap;

	/* count devices in bypass */
	tap = NULL;
	bypass_devices = 0;
	for(;;){
		tap = jtag_NextEnabledTap(tap);
		if( tap == NULL ){
			break;
		}
		if( tap->bypass ){
			bypass_devices++;
		}
	}

	/* allocate memory for a new list member */
	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
	last_comand_pointer = &((*last_cmd)->next);
	(*last_cmd)->next = NULL;
	(*last_cmd)->type = JTAG_SCAN;

	/* allocate memory for dr scan command */
	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
	(*last_cmd)->cmd.scan->ir_scan = 0;
	(*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
	(*last_cmd)->cmd.scan->end_state = state;

	tap = NULL;
	nth_tap = -1;
	for(;;){
		nth_tap++;
		tap = jtag_NextEnabledTap(tap);
		if( tap == NULL ){
			break;
		}
		int found = 0;
		(*last_cmd)->cmd.scan->fields[field_count].tap = tap;

		for (j = 0; j < num_fields; j++)
		{
			if (tap == fields[j].tap)
			{
				found = 1;
				scan_size = fields[j].num_bits;
				(*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
				(*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
				(*last_cmd)->cmd.scan->fields[field_count].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
				(*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;
				(*last_cmd)->cmd.scan->fields[field_count].in_check_value = fields[j].in_check_value;
				(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = fields[j].in_check_mask;
				(*last_cmd)->cmd.scan->fields[field_count].in_handler = fields[j].in_handler;
				(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = fields[j].in_handler_priv;
			}
		}
		if (!found)
		{
#ifdef _DEBUG_JTAG_IO_
			/* if a device isn't listed, the BYPASS register should be selected */
			if (! tap->bypass)
			{
				LOG_ERROR("BUG: no scan data for a device not in BYPASS");
				exit(-1);
			}
#endif
			/* program the scan field to 1 bit length, and ignore it's value */
			(*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
			(*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
			(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
		}
		else
		{
#ifdef _DEBUG_JTAG_IO_
			/* if a device is listed, the BYPASS register must not be selected */
			if (tap->bypass)
			{
				LOG_ERROR("BUG: scan data for a device in BYPASS");
				exit(-1);
			}
#endif
		}
	}
	return ERROR_OK;
}

void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
		int num_fields,
		const int *num_bits,
		const u32 *value,
		tap_state_t end_state)
{
	int nth_tap;
	int field_count = 0;
	int scan_size;
	int bypass_devices = 0;

	jtag_command_t **last_cmd = jtag_get_last_command_p();
	jtag_tap_t *tap;

	/* count devices in bypass */
	tap = NULL;
	bypass_devices = 0;
	for(;;){
		tap = jtag_NextEnabledTap(tap);
		if( tap == NULL ){
			break;
		}
		if( tap->bypass ){
			bypass_devices++;
		}
	}

	/* allocate memory for a new list member */
	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
	last_comand_pointer = &((*last_cmd)->next);
	(*last_cmd)->next = NULL;
	(*last_cmd)->type = JTAG_SCAN;

	/* allocate memory for dr scan command */
	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
	(*last_cmd)->cmd.scan->ir_scan = 0;
	(*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
	(*last_cmd)->cmd.scan->end_state = end_state;

	tap = NULL;
	nth_tap = -1;
	for(;;){
		tap = jtag_NextEnabledTap(tap);
		if( tap == NULL ){
			break;
		}
		nth_tap++;
		(*last_cmd)->cmd.scan->fields[field_count].tap = tap;

		if (tap == target_tap)
		{
			int j;
#ifdef _DEBUG_JTAG_IO_
			/* if a device is listed, the BYPASS register must not be selected */
			if (tap->bypass)
			{
				LOG_ERROR("BUG: scan data for a device in BYPASS");
				exit(-1);
			}
#endif
			for (j = 0; j < num_fields; j++)
			{
				u8 out_value[4];
				scan_size = num_bits[j];
				buf_set_u32(out_value, 0, scan_size, value[j]);
				(*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
				(*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
				(*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
				(*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
				(*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
				(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
				(*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
				(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
			}
		} else
		{
#ifdef _DEBUG_JTAG_IO_
			/* if a device isn't listed, the BYPASS register should be selected */
			if (! tap->bypass)
			{
				LOG_ERROR("BUG: no scan data for a device not in BYPASS");
				exit(-1);
			}
#endif
			/* program the scan field to 1 bit length, and ignore it's value */
			(*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
			(*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
			(*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
			(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
		}
	}
}

void jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, tap_state_t state)
{
	int retval;

	jtag_prelude(state);

	retval=interface_jtag_add_plain_dr_scan(num_fields, fields, cmd_queue_end_state);
	if (retval!=ERROR_OK)
		jtag_error=retval;
}

int MINIDRIVER(interface_jtag_add_plain_dr_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
{
	int i;
	jtag_command_t **last_cmd = jtag_get_last_command_p();

	/* allocate memory for a new list member */
	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
	last_comand_pointer = &((*last_cmd)->next);
	(*last_cmd)->next = NULL;
	(*last_cmd)->type = JTAG_SCAN;

	/* allocate memory for scan command */
	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
	(*last_cmd)->cmd.scan->ir_scan = 0;
	(*last_cmd)->cmd.scan->num_fields = num_fields;
	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
	(*last_cmd)->cmd.scan->end_state = state;

	for (i = 0; i < num_fields; i++)
	{
		int num_bits = fields[i].num_bits;
		int num_bytes = CEIL(fields[i].num_bits, 8);
		(*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
		(*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
		(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
		(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
		(*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
		(*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
		(*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
		(*last_cmd)->cmd.scan->fields[i].in_handler = fields[i].in_handler;
		(*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[i].in_handler_priv;
	}

	return ERROR_OK;
}

void jtag_add_tlr(void)
{
	jtag_prelude(TAP_RESET);

	int retval;
	retval=interface_jtag_add_tlr();
	if (retval!=ERROR_OK)
		jtag_error=retval;
}

int MINIDRIVER(interface_jtag_add_tlr)(void)
{
	tap_state_t state = TAP_RESET;
	jtag_command_t **last_cmd = jtag_get_last_command_p();

	/* allocate memory for a new list member */
	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
	last_comand_pointer = &((*last_cmd)->next);
	(*last_cmd)->next = NULL;
	(*last_cmd)->type = JTAG_STATEMOVE;

	(*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
	(*last_cmd)->cmd.statemove->end_state = state;

	return ERROR_OK;
}

void jtag_add_pathmove(int num_states, tap_state_t *path)
{
	tap_state_t cur_state=cmd_queue_cur_state;
	int i;
	int retval;

	/* the last state has to be a stable state */
	if (!tap_is_state_stable(path[num_states - 1]))
	{
		LOG_ERROR("BUG: TAP path doesn't finish in a stable state");
		exit(-1);
	}

	for (i=0; i<num_states; i++)
	{
		if (path[i] == TAP_RESET)
		{
			LOG_ERROR("BUG: TAP_RESET is not a valid state for pathmove sequences");
			exit(-1);
		}
		if ( tap_state_transition(cur_state, true)  != path[i]
		  && tap_state_transition(cur_state, false) != path[i])
		{
			LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(cur_state), tap_state_name(path[i]));
			exit(-1);
		}
		cur_state = path[i];
	}

	jtag_prelude1();

	retval=interface_jtag_add_pathmove(num_states, path);
	cmd_queue_cur_state = path[num_states - 1];
	if (retval!=ERROR_OK)
		jtag_error=retval;
}

int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, tap_state_t *path)
{
	jtag_command_t **last_cmd = jtag_get_last_command_p();
	int i;

	/* allocate memory for a new list member */
	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
	last_comand_pointer = &((*last_cmd)->next);
	(*last_cmd)->next = NULL;
	(*last_cmd)->type = JTAG_PATHMOVE;

	(*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));
	(*last_cmd)->cmd.pathmove->num_states = num_states;
	(*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(tap_state_t) * num_states);

	for (i = 0; i < num_states; i++)
		(*last_cmd)->cmd.pathmove->path[i] = path[i];

	return ERROR_OK;
}