jtag.c

/***************************************************************************
 *   Copyright (C) 2005 by Dominic Rath                                    *
 *   Dominic.Rath@gmx.de                                                   *
 *                                                                         *
 *   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 "interpreter.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; 


char* tap_state_strings[16] =
{
	"tlr", 
	"sds", "cd", "sd", "e1d", "pd", "e2d", "ud",
	"rti",
	"sis", "ci", "si", "e1i", "pi", "e2i", "ui"
};

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;

/* tap_move[i][j]: tap movement command to go from state i to state j
 * 0: Test-Logic-Reset
 * 1: Run-Test/Idle
 * 2: Shift-DR
 * 3: Pause-DR
 * 4: Shift-IR
 * 5: Pause-IR
 * 
 * SD->SD and SI->SI have to be caught in interface specific code
 */
u8 tap_move[6][6] =
{
/*	  TLR   RTI   SD    PD    SI    PI             */
	{0x7f, 0x00, 0x17, 0x0a, 0x1b, 0x16},	/* TLR */
	{0x7f, 0x00, 0x25, 0x05, 0x2b, 0x0b},	/* RTI */
	{0x7f, 0x31, 0x00, 0x01, 0x0f, 0x2f},	/* SD  */
	{0x7f, 0x30, 0x20, 0x17, 0x1e, 0x2f},	/* PD  */
	{0x7f, 0x31, 0x07, 0x17, 0x00, 0x01},	/* SI  */
	{0x7f, 0x30, 0x1c, 0x17, 0x20, 0x2f}	/* PI  */
};

int tap_move_map[16] = {
	0, -1, -1,  2, -1,  3, -1, -1,
	1, -1, -1,  4, -1,  5, -1, -1
};

tap_transition_t tap_transitions[16] =
{
	{TAP_TLR, TAP_RTI},		/* TLR */
	{TAP_SIS, TAP_CD},		/* SDS */
	{TAP_E1D, TAP_SD},		/* CD  */
	{TAP_E1D, TAP_SD},		/* SD  */
	{TAP_UD,  TAP_PD}, 		/* E1D */
	{TAP_E2D, TAP_PD},		/* PD  */
	{TAP_UD,  TAP_SD},		/* E2D */
	{TAP_SDS, TAP_RTI},		/* UD  */
	{TAP_SDS, TAP_RTI},		/* RTI */
	{TAP_TLR, TAP_CI},		/* SIS */
	{TAP_E1I, TAP_SI},		/* CI  */
	{TAP_E1I, TAP_SI},		/* SI  */
	{TAP_UI,  TAP_PI}, 		/* E1I */
	{TAP_E2I, TAP_PI},		/* PI  */
	{TAP_UI,  TAP_SI},		/* E2I */
	{TAP_SDS, TAP_RTI}		/* UI  */
};

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

/* kludge!!!! these are just global variables that the
 * interface use internally. They really belong
 * inside the drivers, but we don't want to break
 * linking the drivers!!!!
 */
enum tap_state end_state = TAP_TLR;
enum tap_state cur_state = TAP_TLR;
int jtag_trst = 0;
int jtag_srst = 0;

jtag_command_t *jtag_command_queue = NULL;
jtag_command_t **last_comand_pointer = &jtag_command_queue;
jtag_device_t *jtag_devices = NULL;
int jtag_num_devices = 0;
int jtag_ir_scan_size = 0;
enum reset_types jtag_reset_config = RESET_NONE;
enum tap_state cmd_queue_end_state = TAP_TLR;
enum tap_state cmd_queue_cur_state = TAP_TLR;

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 speed1 = 0, speed2 = 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 eCosBoard_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

jtag_interface_t *jtag_interfaces[] = {
#if BUILD_ECOSBOARD == 1
	&eCosBoard_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
	NULL,
};

jtag_interface_t *jtag = NULL;

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


/* forward declarations */
void jtag_add_pathmove(int num_states, enum tap_state *path);
void jtag_add_runtest(int num_cycles, enum tap_state endstate);
void jtag_add_end_state(enum tap_state endstate);
void jtag_add_sleep(u32 us);
int jtag_execute_queue(void);
int jtag_cancel_queue(void);

/* 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 handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

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

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;
}

/* returns a pointer to the n-th device in the scan chain */
jtag_device_t* jtag_get_device(int num)
{
	jtag_device_t *device = jtag_devices;
	int i = 0;

	while (device)
	{
		if (num == i)
			return device;
		device = device->next;
		i++;
	}
	
	LOG_ERROR("jtag device number %d not defined", num);
	exit(-1);
}

void* cmd_queue_alloc(size_t size)
{
	cmd_queue_page_t **p_page = &cmd_queue_pages;
	int offset;
	u8 *t;

	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()
{
	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()
{
	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_TLR)
		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
}

static void jtag_prelude(enum tap_state state)
{
	jtag_prelude1();
	
	if (state != -1)
		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, enum tap_state 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, enum tap_state state)
{	
	jtag_command_t **last_cmd;
	jtag_device_t *device;
	int i, j;
	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;
	(*last_cmd)->cmd.scan->num_fields = jtag_num_devices;	/* one field per device */
	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(jtag_num_devices * sizeof(scan_field_t));
	(*last_cmd)->cmd.scan->end_state = state;

	for (i = 0; i < jtag_num_devices; i++)
	{
		int found = 0;
		device = jtag_get_device(i);
		scan_size = device->ir_length;
		(*last_cmd)->cmd.scan->fields[i].device = i;
		(*last_cmd)->cmd.scan->fields[i].num_bits = scan_size;
		(*last_cmd)->cmd.scan->fields[i].in_value = NULL;
		(*last_cmd)->cmd.scan->fields[i].in_handler = NULL;	/* disable verification by default */

		/* search the list */
		for (j = 0; j < num_fields; j++)
		{
			if (i == fields[j].device)
			{
				found = 1;
				(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
				(*last_cmd)->cmd.scan->fields[i].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+i, device->expected, device->expected_mask, NULL);
					} else
					{
						(*last_cmd)->cmd.scan->fields[i].in_handler = fields[j].in_handler;
						(*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[j].in_handler_priv;
						(*last_cmd)->cmd.scan->fields[i].in_check_value = device->expected; 
						(*last_cmd)->cmd.scan->fields[i].in_check_mask = device->expected_mask;
					}
				}
				
				device->bypass = 0;
				break;
			}
		}
	
		if (!found)
		{
			/* if a device isn't listed, set it to BYPASS */
			(*last_cmd)->cmd.scan->fields[i].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
			(*last_cmd)->cmd.scan->fields[i].out_mask = NULL;
			device->bypass = 1;
			
		}
		
		/* update device information */
		buf_cpy((*last_cmd)->cmd.scan->fields[i].out_value, jtag_get_device(i)->cur_instr, scan_size);
	}
	
	return ERROR_OK;
}

void jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state 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, enum tap_state 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].device = fields[i].device;
		(*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, enum tap_state 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, enum tap_state state)
{
	int i, j;
	int bypass_devices = 0;
	int field_count = 0;
	int scan_size;

	jtag_command_t **last_cmd = jtag_get_last_command_p();
	jtag_device_t *device = jtag_devices;

	/* count devices in bypass */
	while (device)
	{
		if (device->bypass)
			bypass_devices++;
		device = device->next;
	}
	
	/* 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;
	
	for (i = 0; i < jtag_num_devices; i++)
	{
		int found = 0;
		(*last_cmd)->cmd.scan->fields[field_count].device = i;
	
		for (j = 0; j < num_fields; j++)
		{
			if (i == fields[j].device)
			{
				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 (!jtag_get_device(i)->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 (jtag_get_device(i)->bypass)
			{
				LOG_ERROR("BUG: scan data for a device in BYPASS");
				exit(-1);
			}
#endif
		}
	}
	return ERROR_OK;
}

void MINIDRIVER(interface_jtag_add_dr_out)(int device_num, 
		int num_fields,
		const int *num_bits,
		const u32 *value,
		enum tap_state end_state)
{
	int i;
	int field_count = 0;
	int scan_size;
	int bypass_devices = 0;

	jtag_command_t **last_cmd = jtag_get_last_command_p();
	jtag_device_t *device = jtag_devices;
	/* count devices in bypass */
	while (device)
	{
		if (device->bypass)
			bypass_devices++;
		device = device->next;
	}
	
	/* 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;
	
	for (i = 0; i < jtag_num_devices; i++)
	{
		(*last_cmd)->cmd.scan->fields[field_count].device = i;
	
		if (i == device_num)
		{
			int j;
#ifdef _DEBUG_JTAG_IO_
			/* if a device is listed, the BYPASS register must not be selected */
			if (jtag_get_device(i)->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 (!jtag_get_device(i)->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, enum tap_state 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, enum tap_state 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].device = fields[i].device;
		(*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()
{
	jtag_prelude(TAP_TLR);
	
	int retval;
	retval=interface_jtag_add_tlr();
	if (retval!=ERROR_OK)
		jtag_error=retval;
}

int MINIDRIVER(interface_jtag_add_tlr)()
{
	enum tap_state state = TAP_TLR;
	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, enum tap_state *path)
{
	enum tap_state cur_state=cmd_queue_cur_state;
	int i;
	int retval;

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

	for (i=0; i<num_states; i++)
	{
		if ((tap_transitions[cur_state].low != path[i])&&
				(tap_transitions[cur_state].high != path[i]))
		{
			LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[path[i]]);
			exit(-1);
		}
		cur_state = path[i];
	}
	
	jtag_prelude1();
	
	cmd_queue_cur_state = path[num_states - 1];

	retval=interface_jtag_add_pathmove(num_states, path);
	if (retval!=ERROR_OK)
		jtag_error=retval;
}


int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, enum tap_state *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(enum tap_state) * num_states);
	
	for (i = 0; i < num_states; i++)
		(*last_cmd)->cmd.pathmove->path[i] = path[i];
	
	return ERROR_OK;
}

int MINIDRIVER(interface_jtag_add_runtest)(int num_cycles, enum tap_state state)
{
	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_cmd)->next = NULL;
	last_comand_pointer = &((*last_cmd)->next);
	(*last_cmd)->type = JTAG_RUNTEST;

	(*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));
	(*last_cmd)->cmd.runtest->num_cycles = num_cycles;
	(*last_cmd)->cmd.runtest->end_state = state;
	
	return ERROR_OK;
}

void jtag_add_runtest(int num_cycles, enum tap_state state)
{
	int retval;
	
	jtag_prelude(state);
	
	/* executed by sw or hw fifo */
	retval=interface_jtag_add_runtest(num_cycles, cmd_queue_end_state);
	if (retval!=ERROR_OK)
		jtag_error=retval;
}

void jtag_add_reset(int req_tlr_or_trst, int req_srst)
{
	int trst_with_tlr = 0;
	int retval;
	
	/* FIX!!! there are *many* different cases here. A better
	 * approach is needed for legal combinations of transitions...
	 */
	if ((jtag_reset_config & RESET_HAS_SRST)&&
			(jtag_reset_config & RESET_HAS_TRST)&& 
			((jtag_reset_config & RESET_SRST_PULLS_TRST)==0))
	{
		if (((req_tlr_or_trst&&!jtag_trst)||
				(!req_tlr_or_trst&&jtag_trst))&&
				((req_srst&&!jtag_srst)||
						(!req_srst&&jtag_srst)))
		{
			/* FIX!!! srst_pulls_trst allows 1,1 => 0,0 transition.... */
			//LOG_ERROR("BUG: transition of req_tlr_or_trst and req_srst in the same jtag_add_reset() call is undefined");
		}
	}
	
	/* Make sure that jtag_reset_config allows the requested reset */
	/* if SRST pulls TRST, we can't fulfill srst == 1 with trst == 0 */
	if (((jtag_reset_config & RESET_SRST_PULLS_TRST) && (req_srst == 1)) && (!req_tlr_or_trst))
	{
		LOG_ERROR("BUG: requested reset would assert trst");
		jtag_error=ERROR_FAIL;
		return;
	}
		
	/* if TRST pulls SRST, we reset with TAP T-L-R */
	if (((jtag_reset_config & RESET_TRST_PULLS_SRST) && (req_tlr_or_trst)) && (req_srst == 0))
	{
		trst_with_tlr = 1;
	}
	
	if (req_srst && !(jtag_reset_config & RESET_HAS_SRST))
	{
		LOG_ERROR("BUG: requested SRST assertion, but the current configuration doesn't support this");
		jtag_error=ERROR_FAIL;
		return;
	}
	
	if (req_tlr_or_trst)
	{
		if (!trst_with_tlr && (jtag_reset_config & RESET_HAS_TRST))
		{
			jtag_trst = 1;
		} else
		{
			trst_with_tlr = 1;
		}
	} else
	{
		jtag_trst = 0;
	}
	
	jtag_srst = req_srst;

	retval = interface_jtag_add_reset(jtag_trst, jtag_srst);
	if (retval!=ERROR_OK)
	{
		jtag_error=retval;
		return;
	}

	if (jtag_srst)
	{
		LOG_DEBUG("SRST line asserted");
	}
	else
	{
		LOG_DEBUG("SRST line released");
		if (jtag_nsrst_delay)
			jtag_add_sleep(jtag_nsrst_delay * 1000);
	}