rlink.c

一个烧写FLASH的软件

} tap_state_queue;



static
int
dtc_queue_init(void) {
	dtc_queue.rq_head = NULL;
	dtc_queue.rq_tail = NULL;
	dtc_queue.cmd_index = 0;
	dtc_queue.reply_index = 0;
	return(0);
}


static
inline
dtc_reply_queue_entry_t *
dtc_queue_enqueue_reply(
	enum scan_type	type,
	u8				*buffer,
	int				size,
	int				offset,
	int				length,
	jtag_command_t	*cmd
) {
	dtc_reply_queue_entry_t	*rq_entry;

	rq_entry = malloc(sizeof(dtc_reply_queue_entry_t));
	if(rq_entry != NULL) {
		rq_entry->scan.type = type;
		rq_entry->scan.buffer = buffer;
		rq_entry->scan.size = size;
		rq_entry->scan.offset = offset;
		rq_entry->scan.length = length;
		rq_entry->cmd = cmd;
		rq_entry->next = NULL;

		if(dtc_queue.rq_head == NULL)
			dtc_queue.rq_head = rq_entry;
		else
			dtc_queue.rq_tail->next = rq_entry;

		dtc_queue.rq_tail = rq_entry;
	}

	return(rq_entry);
}


/*
 * Running the queue means that any pending command buffer is run and any reply data dealt with.  The command buffer is then cleared for subsequent processing.
 * The queue is automatically run by append when it is necessary to get space for the append.
*/

static
int
dtc_queue_run(void) {
	dtc_reply_queue_entry_t	*rq_p, *rq_next;
	int			retval;
	int			usb_err;
	int			bit_cnt;
	int			x;
	u8			*dtc_p, *tdo_p;
	u8			dtc_mask, tdo_mask;
	u8			reply_buffer[USB_EP2IN_SIZE];

	retval = ERROR_OK;

	if(dtc_queue.cmd_index < 1) return(retval);

	dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = DTC_CMD_STOP;

	/* run the cmd */
	if(dtc_queue.rq_head == NULL) {
		usb_err = dtc_run_download(pHDev,
			dtc_queue.cmd_buffer, dtc_queue.cmd_index,
			NULL, 0
		);	
		if(usb_err < 0) {
			LOG_ERROR("dtc_run_download: %s\n", usb_strerror());
			exit(1);
		}
	} else {
		usb_err = dtc_run_download(pHDev,
			dtc_queue.cmd_buffer, dtc_queue.cmd_index,
			reply_buffer, dtc_queue.reply_index
		);	
		if(usb_err < 0) {
			LOG_ERROR("dtc_run_download: %s\n", usb_strerror());
			exit(1);
		} else {
			/* process the reply, which empties the reply queue and frees its entries */
			dtc_p = reply_buffer;

			/* The rigamarole with the masks and doing it bit-by-bit is due to the fact that the scan buffer is LSb-first and the DTC code is MSb-first for hardware reasons.   It was that or craft a function to do the reversal, and that wouldn't work with bit-stuffing (supplying extra bits to use mostly byte operations), or any other scheme which would throw the byte alignment off. */

			for(
				rq_p = dtc_queue.rq_head;
				rq_p != NULL;
				rq_p = rq_next
			) {
				tdo_p = rq_p->scan.buffer + (rq_p->scan.offset / 8);
				tdo_mask = 1 << (rq_p->scan.offset % 8);


				bit_cnt = rq_p->scan.length;
				if(bit_cnt >= 8) {
					/* bytes */

					dtc_mask = 1 << (8 - 1);

					for(
						;
						bit_cnt;
						bit_cnt--
					) {
						if(*dtc_p & dtc_mask) {
							*tdo_p |= tdo_mask;
						} else {
							*tdo_p &=~ tdo_mask;
						}
						
						dtc_mask >>= 1;
						if(dtc_mask == 0) {
							dtc_p++;
							dtc_mask = 1 << (8 - 1);
						}

						tdo_mask <<= 1;
						if(tdo_mask == 0) {
							tdo_p++;
							tdo_mask = 1;
						}
					}
				} else {
					/*  extra bits or last bit */

					x = *dtc_p++;
					if((
						rq_p->scan.type == SCAN_IN
					) && (
						rq_p->scan.offset != rq_p->scan.size - 1
					)) {
						/* extra bits were sent as a full byte with padding on the end */
						dtc_mask = 1 << (8 - 1);
					} else {
						dtc_mask = 1 << (bit_cnt - 1);
					}

					for(
						;
						bit_cnt;
						bit_cnt--
					) {
						if(x & dtc_mask) {
							*tdo_p |= tdo_mask;
						} else {
							*tdo_p &=~ tdo_mask;
						}

						dtc_mask >>= 1;

						tdo_mask <<= 1;
						if(tdo_mask == 0) {
							tdo_p++;
							tdo_mask = 1;
						}
								
					}
				}

				if((rq_p->scan.offset + rq_p->scan.length) >= rq_p->scan.size) {
					/* feed scan buffer back into openocd and free it */
					if(jtag_read_buffer(rq_p->scan.buffer, rq_p->cmd->cmd.scan) != ERROR_OK) {
						 retval = ERROR_JTAG_QUEUE_FAILED;
					}
					free(rq_p->scan.buffer);
				}

				rq_next = rq_p->next;
				free(rq_p);
			}
			dtc_queue.rq_head = NULL;
			dtc_queue.rq_tail = NULL;
		}

	}


	/* reset state for new appends */
	dtc_queue.cmd_index = 0;
	dtc_queue.reply_index = 0;

	return(retval);
}



static
int
tap_state_queue_init(void) {
	tap_state_queue.length = 0;
	tap_state_queue.buffer = 0;
	return(0);
}


static
int
tap_state_queue_run(void) {
	int	i;
	int	bits;
	u8	byte;
	int	retval;

	retval = 0;
	if(!tap_state_queue.length) return(retval);
	bits = 1;
	byte = 0;
	for(i = tap_state_queue.length; i--;) {
		
		byte <<= 1;
		if(tap_state_queue.buffer & 1) {
			byte |= 1;
		}
		if((bits >= 8) || !i) {
			byte <<= (8 - bits);

			/* make sure there's room for stop, byte op, and one byte */
			if(dtc_queue.cmd_index >= (sizeof(dtc_queue.cmd_buffer) - (1 + 1 + 1))) {
				dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
					DTC_CMD_STOP;
				dtc_queue_run();
			}

#ifdef USE_HARDWARE_SHIFTER_FOR_TMS
			if(bits == 8) {
				dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
					DTC_CMD_SHIFT_TMS_BYTES(1);
			} else {
#endif
				dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
					DTC_CMD_SHIFT_TMS_BITS(bits);
#ifdef USE_HARDWARE_SHIFTER_FOR_TMS
			}
#endif

			dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
				byte;

			byte = 0;
			bits = 1;
		} else {
			bits++;
		}

		tap_state_queue.buffer >>= 1;
	}
	retval = tap_state_queue_init();
	return(retval);
}


static
int
tap_state_queue_append(
	u8	tms
) {
	int	retval;

	if(tap_state_queue.length >= sizeof(tap_state_queue.buffer) * 8) {
		retval = tap_state_queue_run();
		if(retval != 0) return(retval);
	}

	if(tms) {
		tap_state_queue.buffer |= (1 << tap_state_queue.length);
	}
	tap_state_queue.length++;

	return(0);
}


static
void rlink_end_state(enum tap_state state)
{
	if (tap_move_map[state] != -1)
		end_state = state;
	else
	{
		LOG_ERROR("BUG: %i is not a valid end state", state);
		exit(-1);
	}
}


static
void rlink_state_move(void) {

	int i=0, tms=0;
	u8 tms_scan = TAP_MOVE(cur_state, end_state);

	for (i = 0; i < 7; i++)
	{
		tms = (tms_scan >> i) & 1;
		tap_state_queue_append(tms);
	}

	cur_state = end_state;
}

static
void rlink_path_move(pathmove_command_t *cmd)
{
	int num_states = cmd->num_states;
	int state_count;
	int tms = 0;

	state_count = 0;
	while (num_states)
	{
		if (tap_transitions[cur_state].low == cmd->path[state_count])
		{
			tms = 0;
		}
		else if (tap_transitions[cur_state].high == cmd->path[state_count])
		{
			tms = 1;
		}
		else
		{
			LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", jtag_state_name(cur_state), jtag_state_name(cmd->path[state_count]));
			exit(-1);
		}

		tap_state_queue_append(tms);

		cur_state = cmd->path[state_count];
		state_count++;
		num_states--;
	}

	end_state = cur_state;
}


static
void rlink_runtest(int num_cycles)
{
	int i;

	enum tap_state saved_end_state = end_state;

	/* only do a state_move when we're not already in RTI */
	if (cur_state != TAP_IDLE)
	{
		rlink_end_state(TAP_IDLE);
		rlink_state_move();
	}

	/* execute num_cycles */
	for (i = 0; i < num_cycles; i++)
	{
		tap_state_queue_append(0);
	}

	/* finish in end_state */
	rlink_end_state(saved_end_state);
	if (cur_state != end_state)
		rlink_state_move();
}


/* (1) assert or (0) deassert reset lines */
static
void rlink_reset(int trst, int srst)
{
	u8			bitmap;
	int			usb_err;

	bitmap = ((~(ST7_PA_NLINE_DRIVER_ENABLE)) & ST7_PA_NUNASSERTED);

	if(trst) {
		bitmap &= ~ST7_PA_NJTAG_TRST;
	}
	if(srst) {
		bitmap &= ~ST7_PA_NRLINK_RST;
	}

	usb_err = ep1_generic_commandl(
		pHDev, 6,
		 
		EP1_CMD_MEMORY_WRITE,
			ST7_PADR >> 8,
			ST7_PADR,
			1,
			bitmap,
		EP1_CMD_DTC_GET_CACHED_STATUS
	);
	if(usb_err < 0) {
		LOG_ERROR("%s: %s\n", __func__, usb_strerror());
		exit(1);
	}

	usb_err = usb_bulk_read(
		pHDev, USB_EP1IN_ADDR,
		&bitmap, 1,
		USB_TIMEOUT_MS
	);
	if(usb_err < 1) {
		LOG_ERROR("%s: %s\n", __func__, usb_strerror());
		exit(1);
	}
}


static
int
rlink_scan(
	jtag_command_t	*cmd,
	enum scan_type	type,
	u8			*buffer,
	int			scan_size
) {
	int			ir_scan;
	enum tap_state	saved_end_state;
	int			byte_bits;
	int			extra_bits;
	int			chunk_bits;
	int			chunk_bytes;
	int			x;

	int			tdi_bit_offset;
	u8			tdi_mask, *tdi_p;
	u8			dtc_mask;

	if(scan_size < 1) {
		LOG_ERROR("scan_size cannot be less than 1 bit\n");
		exit(1);
	}

	ir_scan = cmd->cmd.scan->ir_scan;

	/* Move to the proper state before starting to shift TDI/TDO. */
	if (!(
		(!ir_scan && (cur_state == TAP_DRSHIFT))
		||
		(ir_scan && (cur_state == TAP_IRSHIFT))
	)) {
		saved_end_state = end_state;
		rlink_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
		rlink_state_move();
		rlink_end_state(saved_end_state);
	}

	tap_state_queue_run();


#if 0
	printf("scan_size = %d, type=0x%x\n", scan_size, type);
	{
		int   i;

		/* clear unused bits in scan buffer for ease of debugging */
		/* (it makes diffing output easier) */
		buffer[scan_size / 8] &= ((1 << ((scan_size - 1) % 8) + 1) - 1);

		printf("before scan:");
		for(i = 0; i < (scan_size + 7) / 8; i++) {
			printf(" %02x", buffer[i]);
		}
		printf("\n");
	}
#endif

	/* The number of bits that can be shifted as complete bytes */
	byte_bits = (int)(scan_size - 1) / 8 * 8;
	/* The number of bits left over, not counting the last bit */
	extra_bits = (scan_size - 1) - byte_bits;

	tdi_bit_offset = 0;
	tdi_p = buffer;
	tdi_mask = 1;

	if(extra_bits && (type == SCAN_OUT)) {
		/* Schedule any extra bits into the DTC command buffer, padding as needed */
		/* For SCAN_OUT, this comes before the full bytes so the (leading) padding bits will fall off the end */
		/* make sure there's room for stop, byte op, and one byte */
		if(
			(dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1 + 1))
		) {
			dtc_queue_run();
		}

		x = 0;
		dtc_mask = 1 << (extra_bits - 1);
	
		while(extra_bits--) {
			if(*tdi_p & tdi_mask) {
				x |= dtc_mask;
			}

			dtc_mask >>= 1;

			tdi_mask <<= 1;
			if(tdi_mask == 0) {
				tdi_p++;
				tdi_mask = 1;
			}
		}

		dtc_queue.cmd_buffer[dtc_queue.cmd_index++] =
			DTC_CMD_SHIFT_TDI_BYTES(1);

		dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
	}

	/* Loop scheduling full bytes into the DTC command buffer */
	while(byte_bits) {
		if(type == SCAN_IN) {
			/* make sure there's room for stop and byte op */
			x = (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1));
		} else {
			/* make sure there's room for stop, byte op, and at least one byte */
			x = (dtc_queue.cmd_index >= sizeof(dtc_queue.cmd_buffer) - (1 + 1 + 1));
		}

		if(type != SCAN_OUT) {
			/* make sure there's room for at least one reply byte */
			x |= (dtc_queue.reply_index >= USB_EP2IN_SIZE - (1));
		}

		if(x) {
			dtc_queue_run();
		}

		chunk_bits = byte_bits;
		/* we can only use up to 16 bytes at a time */
		if(chunk_bits > (16 * 8)) chunk_bits = (16 * 8);

		if(type != SCAN_IN) {
			/* how much is there room for, considering stop and byte op? */
			x = (sizeof(dtc_queue.cmd_buffer) - (dtc_queue.cmd_index + 1 + 1)) * 8;
			if(chunk_bits > x) chunk_bits = x;
		}

		if(type != SCAN_OUT) {
			/* how much is there room for in the reply buffer? */
			x = (USB_EP2IN_SIZE - dtc_queue.reply_index) * 8;
			if(chunk_bits > x) chunk_bits = x;
		}

		/* so the loop will end */
		byte_bits -= chunk_bits;

		if(type != SCAN_OUT) {
			if(dtc_queue_enqueue_reply(
				type, buffer, scan_size, tdi_bit_offset,
				chunk_bits,
				cmd
			) == NULL) {
				LOG_ERROR("enqueuing DTC reply entry: %s\n", strerror(errno));
				exit(1);
			}
			
			tdi_bit_offset += chunk_bits;
		}

		/* chunk_bits is a multiple of 8, so there are no rounding issues. */
		chunk_bytes = chunk_bits / 8;

		switch(type) {
			case SCAN_IN:
				x = DTC_CMD_SHIFT_TDO_BYTES(chunk_bytes);
				break;
			case SCAN_OUT:
				x = DTC_CMD_SHIFT_TDI_BYTES(chunk_bytes);
				break;
			default:
				x = DTC_CMD_SHIFT_TDIO_BYTES(chunk_bytes);
				break;
		}
		dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;

		if(type != SCAN_IN) {
			x = 0;
			dtc_mask = 1 << (8 - 1);
		
			while(chunk_bits--) {
				if(*tdi_p & tdi_mask) {
					x |= dtc_mask;
				}
	
				dtc_mask >>= 1;
				if(dtc_mask == 0) {
					dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = x;
					dtc_queue.reply_index++;
					x = 0;
					dtc_mask = 1 << (8 - 1);
				}
	
				tdi_mask <<= 1;
				if(tdi_mask == 0) {
					tdi_p++;