Pavel Chromy style fixes.

git-svn-id: svn://svn.berlios.de/openocd/trunk@344 b42882b7-edfa-0310-969c-e2dbd0fdcd60

3 files changed, 1147 insertions, 1148 deletions

diff --git a/src/flash/at91sam7.h b/src/flash/at91sam7.h
index 43961d5e..4ef154c9 100644
--- a/src/flash/at91sam7.h
+++ b/src/flash/at91sam7.h
@@ -1,100 +1,100 @@
-/***************************************************************************
- *   Copyright (C) 2006 by Magnus Lundin                                   *
- *   lundin@mlu.mine.nu                                                   *
- *                                                                         *
- *   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.             *
- ***************************************************************************/
-#ifndef AT91SAM7_H
-#define AT91SAM7_H
-
-#include "flash.h"
-#include "target.h"
-
-typedef struct at91sam7_flash_bank_s
-{
-	u32 working_area;
-	u32 working_area_size;
-
-	/* chip id register */
-	u32 cidr;
-	u16 cidr_ext;
-	u16 cidr_nvptyp;
-	u16 cidr_arch;
-	u16 cidr_sramsiz;
-	u16 cidr_nvpsiz;
-	u16 cidr_nvpsiz2;
-	u16 cidr_eproc;
-	u16 cidr_version;
-	char * target_name;
-
-	/* flash geometry */
-	u16 num_pages;
-	u16 pagesize;
-	u16 pages_in_lockregion;
-	u8 num_erase_regions;
-	u8 num_planes;
-	u32 *erase_region_info;
-
-	/* nv memory bits */
-	u16 num_lockbits;
-	u16 lockbits[4];
-	u16 num_nvmbits;
-	u16 nvmbits;
-	u8  securitybit;
-	u8  flashmode[4];         /* 0: not init, 1: fmcn for nvbits (1uS), 2: fmcn for flash (1.5uS) */
-
-	/* main clock status */
-	u8  mck_valid;
-	u32 mck_freq;
-	
-	int probed;
-	
-} at91sam7_flash_bank_t;
-
-/* AT91SAM7 control registers */
-#define DBGU_CIDR 0xFFFFF240
-#define CKGR_MCFR 0xFFFFFC24
-#define CKGR_MCFR_MAINRDY  0x10000
-#define CKGR_PLLR 0xFFFFFC2c
-#define CKGR_PLLR_DIV 0xff
-#define CKGR_PLLR_MUL 0x07ff0000
-#define PMC_MCKR  0xFFFFFC30
-#define PMC_MCKR_CSS  0x03
-#define PMC_MCKR_PRES 0x1c
-
-/* Flash Controller Commands */
-#define  WP   0x01
-#define  SLB  0x02
-#define  WPL  0x03
-#define  CLB  0x04
-#define  EA   0x08
-#define  SGPB 0x0B
-#define  CGPB 0x0D
-#define  SSB  0x0F
-
-/* MC_FSR bit definitions */
-#define        MC_FSR_FRDY 1
-#define        MC_FSR_EOL 2
-
-/* AT91SAM7 constants */
-#define RC_FREQ  32000
-
-/*  FLASH_TIMING_MODES */
-#define  FMR_TIMING_NONE    0
-#define  FMR_TIMING_NVBITS  1
-#define  FMR_TIMING_FLASH   2
-
-#endif /* AT91SAM7_H */
+/***************************************************************************

+ *   Copyright (C) 2006 by Magnus Lundin                                   *

+ *   lundin@mlu.mine.nu                                                    *

+ *                                                                         *

+ *   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.             *

+ ***************************************************************************/

+#ifndef AT91SAM7_H

+#define AT91SAM7_H

+

+#include "flash.h"

+#include "target.h"

+

+typedef struct at91sam7_flash_bank_s

+{

+	u32 working_area;

+	u32 working_area_size;

+

+	/* chip id register */

+	u32 cidr;

+	u16 cidr_ext;

+	u16 cidr_nvptyp;

+	u16 cidr_arch;

+	u16 cidr_sramsiz;

+	u16 cidr_nvpsiz;

+	u16 cidr_nvpsiz2;

+	u16 cidr_eproc;

+	u16 cidr_version;

+	char * target_name;

+

+	/* flash geometry */

+	u16 num_pages;

+	u16 pagesize;

+	u16 pages_in_lockregion;

+	u8 num_erase_regions;

+	u8 num_planes;

+	u32 *erase_region_info;

+

+	/* nv memory bits */

+	u16 num_lockbits;

+	u16 lockbits[4];

+	u16 num_nvmbits;

+	u16 nvmbits;

+	u8  securitybit;

+	u8  flashmode[4];         /* 0: not init, 1: fmcn for nvbits (1uS), 2: fmcn for flash (1.5uS) */

+

+	/* main clock status */

+	u8  mck_valid;

+	u32 mck_freq;

+	

+	int probed;

+	

+} at91sam7_flash_bank_t;

+

+/* AT91SAM7 control registers */

+#define DBGU_CIDR 0xFFFFF240

+#define CKGR_MCFR 0xFFFFFC24

+#define CKGR_MCFR_MAINRDY  0x10000

+#define CKGR_PLLR 0xFFFFFC2c

+#define CKGR_PLLR_DIV 0xff

+#define CKGR_PLLR_MUL 0x07ff0000

+#define PMC_MCKR  0xFFFFFC30

+#define PMC_MCKR_CSS  0x03

+#define PMC_MCKR_PRES 0x1c

+

+/* Flash Controller Commands */

+#define  WP   0x01

+#define  SLB  0x02

+#define  WPL  0x03

+#define  CLB  0x04

+#define  EA   0x08

+#define  SGPB 0x0B

+#define  CGPB 0x0D

+#define  SSB  0x0F

+

+/* MC_FSR bit definitions */

+#define        MC_FSR_FRDY 1

+#define        MC_FSR_EOL 2

+

+/* AT91SAM7 constants */

+#define RC_FREQ  32000

+

+/*  FLASH_TIMING_MODES */

+#define  FMR_TIMING_NONE    0

+#define  FMR_TIMING_NVBITS  1

+#define  FMR_TIMING_FLASH   2

+

+#endif /* AT91SAM7_H */

diff --git a/src/jtag/bitq.c b/src/jtag/bitq.c
index dd9acdf3..4e53bc55 100644
--- a/src/jtag/bitq.c
+++ b/src/jtag/bitq.c
@@ -1,366 +1,366 @@
-/***************************************************************************
- *   Copyright (C) 2007 by Pavel Chromy                                    *
- *   chromy@asix.cz                                                        *
- *                                                                         *
- *   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 "bitq.h"
-
-/* project specific includes */
-#include "log.h"
-#include "types.h"
-#include "jtag.h"
-#include "configuration.h"
-
-/* system includes */
-#include <string.h>
-#include <stdlib.h>
-#include <unistd.h>
-
-#include <sys/time.h>
-#include <time.h>
-
-
-bitq_interface_t *bitq_interface; /* low level bit queue interface */
-
-bitq_state_t bitq_in_state; /* state of input queue */
-
-u8 *bitq_in_buffer; /* buffer dynamically reallocated as needed */
-unsigned long bitq_in_bufsize=32; /* min. buffer size */
-
-
-/*
- * input queue processing does not use jtag_read_buffer() to avoid unnecessary overhead
- * also the buffer for incomming data is reallocated only if necessary
- * no parameters, makes use of stored state information
- */ 
-void bitq_in_proc(void)
-{
-	/* static information preserved between calls to increase performance */
-	static u8 *in_buff; /* pointer to buffer for scanned data */
-	static int in_idx; /* index of byte being scanned */
-	static u8 in_mask; /* mask of next bit to be scanned */
-
-	scan_field_t *field;
-	int tdo;
-	
-	/* loop through the queue */
-	while (bitq_in_state.cmd) {
-		/* only JTAG_SCAN command may return data */
-		if (bitq_in_state.cmd->type==JTAG_SCAN) {
-			/* loop through the fields */
-			while (bitq_in_state.field_idx<bitq_in_state.cmd->cmd.scan->num_fields) {
-
-				field=&bitq_in_state.cmd->cmd.scan->fields[bitq_in_state.field_idx];
-				if ( field->in_value || field->in_handler) {
-
-					if (bitq_in_state.bit_pos==0) {
-						/* initialize field scanning */
-						in_mask=0x01;
-						in_idx=0;
-						if (field->in_value) in_buff=field->in_value;
-						else {
-							/* buffer reallocation needed? */
-							if (field->num_bits>bitq_in_bufsize*8) {
-								/* buffer previously allocated? */
-								if (bitq_in_buffer!=NULL) {
-									/* free it */
-									free(bitq_in_buffer);
-									bitq_in_buffer=NULL;
-								}
-								/* double the buffer size until it fits */
-								while (field->num_bits>bitq_in_bufsize*8) bitq_in_bufsize*=2;
-							}
-							/* if necessary, allocate buffer and check for malloc error */ 
-							if (bitq_in_buffer==NULL && (bitq_in_buffer=malloc(bitq_in_bufsize))==NULL) {
-								ERROR("malloc error");
-								exit(-1);                
-							}
-							in_buff=(void *)bitq_in_buffer;
-						}
-					}
-
-					/* field scanning */
-					while (bitq_in_state.bit_pos<field->num_bits) {
-						if ((tdo=bitq_interface->in())<0) {
-#ifdef _DEBUG_JTAG_IO_
-							DEBUG("bitq in EOF");
-#endif
-							return;
-						}
-						if (in_mask==0x01) in_buff[in_idx]=0;
-						if (tdo) in_buff[in_idx]|=in_mask;
-						if (in_mask==0x80) {
-							in_mask=0x01;
-							in_idx++;
-						}
-						else in_mask<<=1;
-						bitq_in_state.bit_pos++;
-					}
-					
-
-					if (field->in_handler && bitq_in_state.status==ERROR_OK) {
-						bitq_in_state.status=(*field->in_handler)(in_buff, field->in_handler_priv, field);
-					}
-
-				}
-				
-				bitq_in_state.field_idx++; /* advance to next field */
-				bitq_in_state.bit_pos=0; /* start next field from the first bit */
-			}
-
-		}
-		bitq_in_state.cmd=bitq_in_state.cmd->next; /* advance to next command */
-		bitq_in_state.field_idx=0; /* preselect first field */
-	}
-}
-
-
-
-void bitq_io(int tms, int tdi, int tdo_req)
-{
-	bitq_interface->out(tms, tdi, tdo_req);
-	/* check and process the input queue */ 
-	if (bitq_interface->in_rdy()) bitq_in_proc();
-}
-
-
-void bitq_end_state(enum tap_state state)
-{
-	if (state==-1) return;
-	if (tap_move_map[state]==-1) {
-		ERROR("BUG: %i is not a valid end state", state);
-		exit(-1);
-	}
-	end_state = state;
-}
-
-
-void bitq_state_move(enum tap_state new_state)
-{
-	int i=0;
-	u8 tms_scan;
-
-	if (tap_move_map[cur_state]==-1 || tap_move_map[new_state]==-1) {
-		ERROR("TAP move from or to unstable state");
-		exit(-1);
-	}
-	
-	tms_scan=TAP_MOVE(cur_state, new_state);
-
-	for (i=0; i<7; i++) {
-		bitq_io(tms_scan&1, 0, 0);
-		tms_scan>>=1;
-	}
-
-	cur_state = new_state;
-}
-
-
-void bitq_path_move(pathmove_command_t *cmd)
-{
-	int i;
-
-	for (i=0; i<=cmd->num_states; i++) {
-		if (tap_transitions[cur_state].low == cmd->path[i]) bitq_io(0, 0, 0);
-		else if (tap_transitions[cur_state].high == cmd->path[i]) bitq_io(1, 0, 0);
-		else {
-			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[i]]);
-			exit(-1);
-		}
-
-		cur_state = cmd->path[i];
-	}
-
-	end_state = cur_state;
-}
-
-
-void bitq_runtest(int num_cycles)
-{
-	int i;
-
-	/* only do a state_move when we're not already in RTI */
-	if (cur_state != TAP_RTI) bitq_state_move(TAP_RTI);
-
-	/* execute num_cycles */
-	for (i = 0; i < num_cycles; i++)
-		bitq_io(0, 0, 0);
-
-	/* finish in end_state */
-	if (cur_state != end_state) bitq_state_move(end_state);
-}
-
-
-void bitq_scan_field(scan_field_t *field, int pause)
-{
-	int bit_cnt;
-	int tdo_req;
-
-	u8 *out_ptr;
-	u8 out_mask;
-	
-	if ( field->in_value || field->in_handler) tdo_req=1;
-	else tdo_req=0;
-
-	if (field->out_value==NULL) {
-		/* just send zeros and request data from TDO */
-		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--)
-			bitq_io(0, 0, tdo_req);
-		bitq_io(pause, 0, tdo_req);
-	}
-	else {
-		/* send data, and optionally request TDO */
-		out_mask=0x01;
-		out_ptr=field->out_value;
-		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--) {
-			bitq_io(0, ((*out_ptr)&out_mask)!=0, tdo_req);
-			if (out_mask==0x80) {
-				out_mask=0x01;
-				out_ptr++;
-			}
-			else out_mask<<=1;
-		}
-		bitq_io(pause, ((*out_ptr)&out_mask)!=0, tdo_req);
-	}
-
-	if (pause) {
-		bitq_io(0,0,0);
-		if (cur_state==TAP_SI) cur_state=TAP_PI;
-		else if (cur_state==TAP_SD) cur_state=TAP_PD;
-	}
-}
-
-
-void bitq_scan(scan_command_t *cmd)
-{
-	int i;
-
-	if (cmd->ir_scan) bitq_state_move(TAP_SI);
-	else bitq_state_move(TAP_SD);
-
-	for (i=0; i < cmd->num_fields-1; i++)
-		bitq_scan_field(&cmd->fields[i], 0);
-	bitq_scan_field(&cmd->fields[i], 1);
-}
-
-
-int bitq_execute_queue(void)
-{
-	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
-	
-	bitq_in_state.cmd = jtag_command_queue;
-	bitq_in_state.field_idx = 0;
-	bitq_in_state.bit_pos = 0;
-	bitq_in_state.status = ERROR_OK;
-
-	while (cmd) {
-
-		switch (cmd->type) {
-
-			case JTAG_END_STATE:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
-#endif
-				bitq_end_state(cmd->cmd.end_state->end_state);
-				break;
-
-			case JTAG_RESET:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-#endif
-				bitq_interface->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-				if (bitq_interface->in_rdy()) bitq_in_proc();
-				break;
-
-			case JTAG_RUNTEST:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
-#endif
-				bitq_end_state(cmd->cmd.runtest->end_state);
-				bitq_runtest(cmd->cmd.runtest->num_cycles);
-				break;
-
-			case JTAG_STATEMOVE:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
-#endif
-				bitq_end_state(cmd->cmd.statemove->end_state);
-				bitq_state_move(end_state); /* uncoditional TAP move */
-				break;
-
-			case JTAG_PATHMOVE:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
-#endif
-				bitq_path_move(cmd->cmd.pathmove);
-				break;
-
-			case JTAG_SCAN:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("scan end in %i", cmd->cmd.scan->end_state);
-				if (cmd->cmd.scan->ir_scan) DEBUG("scan ir");
-				else DEBUG("scan dr");
-#endif
-				bitq_end_state(cmd->cmd.scan->end_state);
-				bitq_scan(cmd->cmd.scan);
-				if (cur_state != end_state) bitq_state_move(end_state);
-				break;
-
-			case JTAG_SLEEP:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("sleep %i", cmd->cmd.sleep->us);
-#endif
-				bitq_interface->sleep(cmd->cmd.sleep->us);
-				if (bitq_interface->in_rdy()) bitq_in_proc();
-				break;
-
-			default:
-				ERROR("BUG: unknown JTAG command type encountered");
-				exit(-1);
-		}
-
-		cmd = cmd->next;
-	}
-
-	bitq_interface->flush();
-	bitq_in_proc();
-
-	if (bitq_in_state.cmd) {
-		ERROR("missing data from bitq interface");
-		return ERROR_JTAG_QUEUE_FAILED;
-	}
-	if (bitq_interface->in()>=0) {
-		ERROR("extra data from bitq interface");
-		return ERROR_JTAG_QUEUE_FAILED;
-	}
-
-	return bitq_in_state.status;
-}
-
-
-void bitq_cleanup(void)
-{
-	if (bitq_in_buffer!=NULL)
-	{
-		free(bitq_in_buffer);
-		bitq_in_buffer=NULL;
-	}
-}
+/***************************************************************************

+ *   Copyright (C) 2007 by Pavel Chromy                                    *

+ *   chromy@asix.cz                                                        *

+ *                                                                         *

+ *   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 "bitq.h"

+

+/* project specific includes */

+#include "log.h"

+#include "types.h"

+#include "jtag.h"

+#include "configuration.h"

+

+/* system includes */

+#include <string.h>

+#include <stdlib.h>

+#include <unistd.h>

+

+#include <sys/time.h>

+#include <time.h>

+

+

+bitq_interface_t *bitq_interface; /* low level bit queue interface */

+

+bitq_state_t bitq_in_state; /* state of input queue */

+

+u8 *bitq_in_buffer; /* buffer dynamically reallocated as needed */

+unsigned long bitq_in_bufsize=32; /* min. buffer size */

+

+

+/*

+ * input queue processing does not use jtag_read_buffer() to avoid unnecessary overhead

+ * also the buffer for incomming data is reallocated only if necessary

+ * no parameters, makes use of stored state information

+ */

+void bitq_in_proc(void)

+{

+	/* static information preserved between calls to increase performance */

+	static u8 *in_buff; /* pointer to buffer for scanned data */

+	static int in_idx; /* index of byte being scanned */

+	static u8 in_mask; /* mask of next bit to be scanned */

+

+	scan_field_t *field;

+	int tdo;

+

+	/* loop through the queue */

+	while (bitq_in_state.cmd) {

+		/* only JTAG_SCAN command may return data */

+		if (bitq_in_state.cmd->type==JTAG_SCAN) {

+			/* loop through the fields */

+			while (bitq_in_state.field_idx<bitq_in_state.cmd->cmd.scan->num_fields) {

+

+				field=&bitq_in_state.cmd->cmd.scan->fields[bitq_in_state.field_idx];

+				if ( field->in_value || field->in_handler) {

+

+					if (bitq_in_state.bit_pos==0) {

+						/* initialize field scanning */

+						in_mask=0x01;

+						in_idx=0;

+						if (field->in_value) in_buff=field->in_value;

+						else {

+							/* buffer reallocation needed? */

+							if (field->num_bits>bitq_in_bufsize*8) {

+								/* buffer previously allocated? */

+								if (bitq_in_buffer!=NULL) {

+									/* free it */

+									free(bitq_in_buffer);

+									bitq_in_buffer=NULL;

+								}

+								/* double the buffer size until it fits */

+								while (field->num_bits>bitq_in_bufsize*8) bitq_in_bufsize*=2;

+							}

+							/* if necessary, allocate buffer and check for malloc error */

+							if (bitq_in_buffer==NULL && (bitq_in_buffer=malloc(bitq_in_bufsize))==NULL) {

+								ERROR("malloc error");

+								exit(-1);

+							}

+							in_buff=(void *)bitq_in_buffer;

+						}

+					}

+

+					/* field scanning */

+					while (bitq_in_state.bit_pos<field->num_bits) {

+						if ((tdo=bitq_interface->in())<0) {

+#ifdef _DEBUG_JTAG_IO_

+							DEBUG("bitq in EOF");

+#endif

+							return;

+						}

+						if (in_mask==0x01) in_buff[in_idx]=0;

+						if (tdo) in_buff[in_idx]|=in_mask;

+						if (in_mask==0x80) {

+							in_mask=0x01;

+							in_idx++;

+						}

+						else in_mask<<=1;

+						bitq_in_state.bit_pos++;

+					}

+

+

+					if (field->in_handler && bitq_in_state.status==ERROR_OK) {

+						bitq_in_state.status=(*field->in_handler)(in_buff, field->in_handler_priv, field);

+					}

+

+				}

+

+				bitq_in_state.field_idx++; /* advance to next field */

+				bitq_in_state.bit_pos=0; /* start next field from the first bit */

+			}

+

+		}

+		bitq_in_state.cmd=bitq_in_state.cmd->next; /* advance to next command */

+		bitq_in_state.field_idx=0; /* preselect first field */

+	}

+}

+

+

+

+void bitq_io(int tms, int tdi, int tdo_req)

+{

+	bitq_interface->out(tms, tdi, tdo_req);

+	/* check and process the input queue */

+	if (bitq_interface->in_rdy()) bitq_in_proc();

+}

+

+

+void bitq_end_state(enum tap_state state)

+{

+	if (state==-1) return;

+	if (tap_move_map[state]==-1) {

+		ERROR("BUG: %i is not a valid end state", state);

+		exit(-1);

+	}

+	end_state = state;

+}

+

+

+void bitq_state_move(enum tap_state new_state)

+{

+	int i=0;

+	u8 tms_scan;

+

+	if (tap_move_map[cur_state]==-1 || tap_move_map[new_state]==-1) {

+		ERROR("TAP move from or to unstable state");

+		exit(-1);

+	}

+

+	tms_scan=TAP_MOVE(cur_state, new_state);

+

+	for (i=0; i<7; i++) {

+		bitq_io(tms_scan&1, 0, 0);

+		tms_scan>>=1;

+	}

+

+	cur_state = new_state;

+}

+

+

+void bitq_path_move(pathmove_command_t *cmd)

+{

+	int i;

+

+	for (i=0; i<=cmd->num_states; i++) {

+		if (tap_transitions[cur_state].low == cmd->path[i]) bitq_io(0, 0, 0);

+		else if (tap_transitions[cur_state].high == cmd->path[i]) bitq_io(1, 0, 0);

+		else {

+			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[i]]);

+			exit(-1);

+		}

+

+		cur_state = cmd->path[i];

+	}

+

+	end_state = cur_state;

+}

+

+

+void bitq_runtest(int num_cycles)

+{

+	int i;

+

+	/* only do a state_move when we're not already in RTI */

+	if (cur_state != TAP_RTI) bitq_state_move(TAP_RTI);

+

+	/* execute num_cycles */

+	for (i = 0; i < num_cycles; i++)

+		bitq_io(0, 0, 0);

+

+	/* finish in end_state */

+	if (cur_state != end_state) bitq_state_move(end_state);

+}

+

+

+void bitq_scan_field(scan_field_t *field, int pause)

+{

+	int bit_cnt;

+	int tdo_req;

+

+	u8 *out_ptr;

+	u8 out_mask;

+

+	if ( field->in_value || field->in_handler) tdo_req=1;

+	else tdo_req=0;

+

+	if (field->out_value==NULL) {

+		/* just send zeros and request data from TDO */

+		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--)

+			bitq_io(0, 0, tdo_req);

+		bitq_io(pause, 0, tdo_req);

+	}

+	else {

+		/* send data, and optionally request TDO */

+		out_mask=0x01;

+		out_ptr=field->out_value;

+		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--) {

+			bitq_io(0, ((*out_ptr)&out_mask)!=0, tdo_req);

+			if (out_mask==0x80) {

+				out_mask=0x01;

+				out_ptr++;

+			}

+			else out_mask<<=1;

+		}

+		bitq_io(pause, ((*out_ptr)&out_mask)!=0, tdo_req);

+	}

+

+	if (pause) {

+		bitq_io(0,0,0);

+		if (cur_state==TAP_SI) cur_state=TAP_PI;

+		else if (cur_state==TAP_SD) cur_state=TAP_PD;

+	}

+}

+

+

+void bitq_scan(scan_command_t *cmd)

+{

+	int i;

+

+	if (cmd->ir_scan) bitq_state_move(TAP_SI);

+	else bitq_state_move(TAP_SD);

+

+	for (i=0; i < cmd->num_fields-1; i++)

+		bitq_scan_field(&cmd->fields[i], 0);

+	bitq_scan_field(&cmd->fields[i], 1);

+}

+

+

+int bitq_execute_queue(void)

+{

+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */

+

+	bitq_in_state.cmd = jtag_command_queue;

+	bitq_in_state.field_idx = 0;

+	bitq_in_state.bit_pos = 0;

+	bitq_in_state.status = ERROR_OK;

+

+	while (cmd) {

+

+		switch (cmd->type) {

+

+			case JTAG_END_STATE:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);

+#endif

+				bitq_end_state(cmd->cmd.end_state->end_state);

+				break;

+

+			case JTAG_RESET:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);

+#endif

+				bitq_interface->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);

+				if (bitq_interface->in_rdy()) bitq_in_proc();

+				break;

+

+			case JTAG_RUNTEST:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);

+#endif

+				bitq_end_state(cmd->cmd.runtest->end_state);

+				bitq_runtest(cmd->cmd.runtest->num_cycles);

+				break;

+

+			case JTAG_STATEMOVE:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);

+#endif

+				bitq_end_state(cmd->cmd.statemove->end_state);

+				bitq_state_move(end_state); /* uncoditional TAP move */

+				break;

+

+			case JTAG_PATHMOVE:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);

+#endif

+				bitq_path_move(cmd->cmd.pathmove);

+				break;

+

+			case JTAG_SCAN:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("scan end in %i", cmd->cmd.scan->end_state);

+				if (cmd->cmd.scan->ir_scan) DEBUG("scan ir");

+				else DEBUG("scan dr");

+#endif

+				bitq_end_state(cmd->cmd.scan->end_state);

+				bitq_scan(cmd->cmd.scan);

+				if (cur_state != end_state) bitq_state_move(end_state);

+				break;

+

+			case JTAG_SLEEP:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("sleep %i", cmd->cmd.sleep->us);

+#endif

+				bitq_interface->sleep(cmd->cmd.sleep->us);

+				if (bitq_interface->in_rdy()) bitq_in_proc();

+				break;

+

+			default:

+				ERROR("BUG: unknown JTAG command type encountered");

+				exit(-1);

+		}

+

+		cmd = cmd->next;

+	}

+

+	bitq_interface->flush();

+	bitq_in_proc();

+

+	if (bitq_in_state.cmd) {

+		ERROR("missing data from bitq interface");

+		return ERROR_JTAG_QUEUE_FAILED;

+	}

+	if (bitq_interface->in()>=0) {

+		ERROR("extra data from bitq interface");

+		return ERROR_JTAG_QUEUE_FAILED;

+	}

+

+	return bitq_in_state.status;

+}

+

+

+void bitq_cleanup(void)

+{

+	if (bitq_in_buffer!=NULL)

+	{

+		free(bitq_in_buffer);

+		bitq_in_buffer=NULL;

+	}

+}

diff --git a/src/jtag/usbprog.c b/src/jtag/usbprog.c
index ab4fe77a..2ff785ab 100644
--- a/src/jtag/usbprog.c
+++ b/src/jtag/usbprog.c
@@ -1,682 +1,681 @@
-/***************************************************************************
- *   Copyright (C) 2007 by Benedikt Sauter sauter@ixbat.de		           *
- *   based on Dominic Rath's amt_jtagaccel.c			                   *
- *									                                       * 
- *   usbprog is a free programming adapter. You can easily install	       *
- *   different firmware versions from an "online pool" over USB.	       * 
- *   The adapter can be used for programming and debugging AVR and ARM     *
- *   processors, as USB to RS232 converter, as JTAG interface or as        *
- *   simple I/O interface (5 lines).					                   *
- *									                                       *
- *   http://www.embedded-projects.net/usbprog				               *
- *                                                                         *
- *   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 <usb.h>
-
-/* system includes */
-
-#include "log.h"
-
-#define VID 0x1781
-#define PID 0x0c63
-
-/* Pins at usbprog */
-#define TDO_BIT         0
-#define TDI_BIT         3
-#define TCK_BIT         2
-#define TMS_BIT         1
-
-int usbprog_execute_queue(void);
-int usbprog_speed(int speed);
-int usbprog_register_commands(struct command_context_s *cmd_ctx);
-int usbprog_init(void);
-int usbprog_quit(void);
-
-void usbprog_end_state(enum tap_state state);
-void usbprog_state_move(void);
-void usbprog_path_move(pathmove_command_t *cmd);
-void usbprog_runtest(int num_cycles);
-void usbprog_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size);
-
-jtag_interface_t usbprog_interface = 
-{
-	.name = "usbprog",
-	.execute_queue = usbprog_execute_queue,
-	.speed = usbprog_speed,	
-	.register_commands = usbprog_register_commands,
-	.init = usbprog_init,
-	.quit = usbprog_quit
-};
-
-#define UNKOWN_COMMAND  0x00
-#define PORT_DIRECTION  0x01
-#define PORT_SET        0x02
-#define PORT_GET        0x03
-#define PORT_SETBIT     0x04
-#define PORT_GETBIT     0x05
-#define WRITE_TDI     	0x06
-#define READ_TDO     	0x07
-#define WRITE_AND_READ 	0x08
-#define WRITE_TMS     	0x09
-#define WRITE_TMS_CHAIN 0x0A
-
-struct usbprog_jtag 
-{
-	struct usb_dev_handle* usb_handle;
-};
-
-struct usbprog_jtag * usbprog_jtag_handle;
-
-struct usbprog_jtag* usbprog_jtag_open();
-void usbprog_jtag_close(struct usbprog_jtag *usbprog_jtag);
-void usbprog_jtag_init(struct usbprog_jtag *usbprog_jtag);
-unsigned char usbprog_jtag_message(struct usbprog_jtag *usbprog_jtag, char *msg, int msglen);
-
-void usbprog_jtag_read_tdo(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);
-void usbprog_jtag_write_tdi(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);
-void usbprog_jtag_write_and_read(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);
-void usbprog_jtag_write_tms(struct usbprog_jtag *usbprog_jtag, char tms_scan);
-
-char tms_chain[64];
-int tms_chain_index;
-void usbprog_jtag_tms_collect(char tms_scan);
-void usbprog_jtag_tms_send(struct usbprog_jtag *usbprog_jtag);
-
-void usbprog_write(int tck, int tms, int tdi);
-void usbprog_reset(int trst, int srst);
-
-void usbprog_jtag_set_direction(struct usbprog_jtag *usbprog_jtag, unsigned char direction);
-void usbprog_jtag_write_slice(struct usbprog_jtag *usbprog_jtag,unsigned char value);
-unsigned char usbprog_jtag_get_port(struct usbprog_jtag *usbprog_jtag);
-void usbprog_jtag_set_bit(struct usbprog_jtag *usbprog_jtag,int bit, int value);
-int usbprog_jtag_get_bit(struct usbprog_jtag *usbprog_jtag, int bit);
-
-int usbprog_speed(int speed)
-{
-	return ERROR_OK;
-}
-
-int usbprog_register_commands(struct command_context_s *cmd_ctx)
-{
-	return ERROR_OK;
-}
-
-int usbprog_execute_queue(void)
-{
-	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
-	int scan_size;
-	enum scan_type type;
-	u8 *buffer;
-	
-	while (cmd)
-	{
-		switch (cmd->type)
-		{
-			case JTAG_END_STATE:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
-#endif
-				if (cmd->cmd.end_state->end_state != -1)
-					usbprog_end_state(cmd->cmd.end_state->end_state);
-				break;
-			case JTAG_RESET:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-#endif
-				if (cmd->cmd.reset->trst == 1)
-				{
-					cur_state = TAP_TLR;
-				}
-				usbprog_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-				break;
-			case JTAG_RUNTEST:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
-#endif
-				if (cmd->cmd.runtest->end_state != -1)
-					usbprog_end_state(cmd->cmd.runtest->end_state);
-				usbprog_runtest(cmd->cmd.runtest->num_cycles);
-				break;
-			case JTAG_STATEMOVE:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
-#endif
-				if (cmd->cmd.statemove->end_state != -1)
-					usbprog_end_state(cmd->cmd.statemove->end_state);
-				usbprog_state_move();
-				break;
-			case JTAG_PATHMOVE:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, 
-					cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
-#endif
-				usbprog_path_move(cmd->cmd.pathmove);
-				break;
-			case JTAG_SCAN:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("scan end in %i", cmd->cmd.scan->end_state);
-#endif
-				if (cmd->cmd.scan->end_state != -1)
-					usbprog_end_state(cmd->cmd.scan->end_state);
-				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
-				type = jtag_scan_type(cmd->cmd.scan);
-				usbprog_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
-				if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)
-					return ERROR_JTAG_QUEUE_FAILED;
-				if (buffer)
-					free(buffer);
-				break;
-			case JTAG_SLEEP:
-#ifdef _DEBUG_JTAG_IO_
-				DEBUG("sleep %i", cmd->cmd.sleep->us);
-#endif
-				jtag_sleep(cmd->cmd.sleep->us);
-					break;
-			default:
-				ERROR("BUG: unknown JTAG command type encountered");
-				exit(-1);
-		}
-			
-		cmd = cmd->next;
-	}
-	
-	return ERROR_OK;
-}
-
-int usbprog_init(void)
-{
-	usbprog_jtag_handle = usbprog_jtag_open();
-	
-	tms_chain_index = 0;
-	if (usbprog_jtag_handle == 0)
-	{
-		ERROR("Can't find USB JTAG Interface! Please check connection and permissions.");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-	
-	INFO("USB JTAG Interface ready!");
-	
-	usbprog_jtag_init(usbprog_jtag_handle);
-	usbprog_reset(0, 0);
-	usbprog_write(0, 0, 0);
-	
-	return ERROR_OK;
-}
-
-int usbprog_quit(void)
-{
-	return ERROR_OK;
-}
-
-/*************** jtag execute commands **********************/
-void usbprog_end_state(enum tap_state state)
-{
-	if (tap_move_map[state] != -1)
-		end_state = state;
-	else
-	{
-		ERROR("BUG: %i is not a valid end state", state);
-		exit(-1);
-	}
-}
-
-void usbprog_state_move(void)
-{	
-	int i = 0, tms = 0;
-	u8 tms_scan = TAP_MOVE(cur_state, end_state);
-	
-	usbprog_jtag_write_tms(usbprog_jtag_handle, (char)tms_scan);
-	for (i = 0; i < 7; i++)
-	{
-		tms = (tms_scan >> i) & 1;
-	}
-	
-	cur_state = end_state;
-}
-
-void usbprog_path_move(pathmove_command_t *cmd)
-{
-	int num_states = cmd->num_states;
-	int state_count;
-	
-	state_count = 0;
-	while (num_states)
-	{
-		if (tap_transitions[cur_state].low == cmd->path[state_count])
-		{
-			//INFO("1");
-			usbprog_write(0, 0, 0);
-			usbprog_write(1, 0, 0);
-		}
-		else if (tap_transitions[cur_state].high == cmd->path[state_count])
-		{
-			//INFO("2");
-			usbprog_write(0, 1, 0);
-			usbprog_write(1, 1, 0);
-		}
-		else
-		{
-			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[state_count]]);
-			exit(-1);
-		}
-		
-		cur_state = cmd->path[state_count];
-		state_count++;
-		num_states--;
-	}
-	
-	end_state = cur_state;
-}
-
-void usbprog_runtest(int num_cycles)
-{
-	int i;
-	
-	/* only do a state_move when we're not already in RTI */
-	if (cur_state != TAP_RTI)
-	{
-		usbprog_end_state(TAP_RTI);
-		usbprog_state_move();
-	}
-	
-	/* execute num_cycles */
-	if (num_cycles > 0)
-	{
-		usbprog_jtag_tms_send(usbprog_jtag_handle);
-		usbprog_write(0, 0, 0);
-	}
-	else
-	{
-		usbprog_jtag_tms_send(usbprog_jtag_handle);
-		//INFO("NUM CYCLES %i",num_cycles);
-	}
-	
-	for (i = 0; i < num_cycles; i++)
-	{
-		usbprog_write(1, 0, 0);
-		usbprog_write(0, 0, 0);
-	}
-	
-	/* finish in end_state */
-	/*
-	usbprog_end_state(saved_end_state);
-	if (cur_state != end_state)
-		usbprog_state_move();
-	*/
-}
-
-void usbprog_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
-{
-	enum tap_state saved_end_state = end_state;
-	
-	if (ir_scan)
-		usbprog_end_state(TAP_SI);
-	else
-		usbprog_end_state(TAP_SD);
-	
-	//usbprog_jtag_tms_send(usbprog_jtag_handle);
-	
-	usbprog_state_move();
-	usbprog_end_state(saved_end_state);
-	
-	usbprog_jtag_tms_send(usbprog_jtag_handle);
-	
-	if (type == SCAN_OUT)
-	{
-		usbprog_jtag_write_tdi(usbprog_jtag_handle,buffer, scan_size);
-	}
-	if (type == SCAN_IN)
-	{
-		usbprog_jtag_read_tdo(usbprog_jtag_handle,buffer, scan_size);
-	}
-	if (type == SCAN_IO)
-	{
-		usbprog_jtag_write_and_read(usbprog_jtag_handle,buffer, scan_size);
-	}
-	
-	if (ir_scan)
-		cur_state = TAP_PI;
-	else
-		cur_state = TAP_PD;
-	
-	if (cur_state != end_state)
-		usbprog_state_move();
-}
-
-/*************** jtag wrapper functions *********************/
-
-void usbprog_write(int tck, int tms, int tdi)
-{
-	unsigned char output_value=0x00;
-	
-	if (tms)
-		output_value |= (1<<TMS_BIT);
-	if (tdi)
-		output_value |= (1<<TDI_BIT);
-	if (tck)
-		output_value |= (1<<TCK_BIT);
-	
-	usbprog_jtag_write_slice(usbprog_jtag_handle,output_value);
-}
-
-/* (1) assert or (0) deassert reset lines */
-void usbprog_reset(int trst, int srst)
-{
-	DEBUG("trst: %i, srst: %i", trst, srst);
-	
-	if (trst)
-		usbprog_jtag_set_bit(usbprog_jtag_handle, 5, 0);
-	else
-		usbprog_jtag_set_bit(usbprog_jtag_handle, 5, 1);
-	
-	if (srst)
-		usbprog_jtag_set_bit(usbprog_jtag_handle, 4, 0);
-	else
-		usbprog_jtag_set_bit(usbprog_jtag_handle, 4, 1);
-}
-
-/*************** jtag lowlevel functions ********************/
-
-struct usb_bus *busses;
-
-struct usbprog_jtag* usbprog_jtag_open()
-{
-	struct usb_bus *bus;
-	struct usb_device *dev;
-	
-	struct usbprog_jtag *tmp;
-	
-	tmp = (struct usbprog_jtag*)malloc(sizeof(struct usbprog_jtag));
-	
-	usb_set_debug(10);	
-	usb_init();
-	usb_find_busses();
-	usb_find_devices();
-	
-	busses = usb_get_busses();
-	
-	/* find usbprog_jtag device in usb bus */
-	
-	for (bus = busses; bus; bus = bus->next)
-	{
-		for (dev = bus->devices; dev; dev = dev->next)
-		{
-			/* condition for sucessfully hit (too bad, I only check the vendor id)*/
-			if (dev->descriptor.idVendor == VID && dev->descriptor.idProduct == PID)
-			{
-				tmp->usb_handle = usb_open(dev);
-				usb_set_configuration(tmp->usb_handle, 1);
-				usb_claim_interface(tmp->usb_handle, 0);
-				usb_set_altinterface(tmp->usb_handle, 0);
-				return tmp;
-			}
-		} 
-	}
-	return 0;
-}
-
-void usbprog_jtag_close(struct usbprog_jtag *usbprog_jtag)
-{
-	usb_close(usbprog_jtag->usb_handle);
-	free(usbprog_jtag);
-}
-
-unsigned char usbprog_jtag_message(struct usbprog_jtag *usbprog_jtag, char *msg, int msglen)
-{
-	int res = usb_bulk_write(usbprog_jtag->usb_handle, 3, msg,msglen, 100);
-	if ((msg[0] == 2) || (msg[0] == 1) || (msg[0] == 4) || (msg[0] == 0) || \
-			(msg[0] == 6) || (msg[0] == 0x0A) || (msg[0] == 9))
-		return 1;  
-	if (res == msglen)
-	{
-		//INFO("HALLLLOOO %i",(int)msg[0]);
-		res =  usb_bulk_read(usbprog_jtag->usb_handle, 0x82, msg, 2, 100);
-		if (res > 0)
-			return (unsigned char)msg[1];
-		else 
-			return -1;
-	}
-	else
-		return -1;
-	return 0;
-}
-
-void usbprog_jtag_init(struct usbprog_jtag *usbprog_jtag)
-{
-	usbprog_jtag_set_direction(usbprog_jtag, 0xFE);
-}
-
-void usbprog_jtag_write_and_read(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)
-{
-	char tmp[64];	// fastes packet size for usb controller
-	int send_bits, bufindex = 0, fillindex = 0, i, loops;
-	
-	char swap;
-	// 61 byte can be transfered (488 bit)
-	
-	while (size > 0)
-	{  
-		if (size > 488)
-		{
-			send_bits = 488;
-			size = size - 488;
-			loops = 61;
-		}
-		else
-		{
-			send_bits = size;
-			loops = size / 8;
-			loops++;
-			size = 0;
-		}
-		tmp[0] = WRITE_AND_READ;
-		tmp[1] = (char)(send_bits >> 8);	// high 
-		tmp[2] = (char)(send_bits);			// low
-		i = 0; 
-		
-		for (i = 0; i < loops; i++)
-		{
-			tmp[3 + i] = buffer[bufindex];
-			bufindex++;
-		}
-		
-		if (usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 64, 1000) == 64)
-		{
-			//INFO("HALLLLOOO2 %i",(int)tmp[0]);
-			usleep(1);
-			int timeout = 0;
-			while (usb_bulk_read(usbprog_jtag->usb_handle, 0x82, tmp, 64, 1000) < 1)
-			{
-				timeout++;
-				if (timeout > 10)
-					break;
-			}	
-			
-			for (i = 0; i < loops; i++)
-			{
-				swap =  tmp[3 + i];
-				buffer[fillindex++] = swap;
-			} 
-		}
-	}
-}
-
-void usbprog_jtag_read_tdo(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)
-{
-	char tmp[64];	// fastes packet size for usb controller
-	int send_bits, fillindex = 0, i, loops;
-
-	char swap;
-	// 61 byte can be transfered (488 bit)
-    
-	while (size > 0)
-	{  
-		if (size > 488)
-		{
-			send_bits = 488;
-			size = size - 488;
-			loops = 61;
-		}
-		else
-		{
-			send_bits = size;
-			loops = size / 8;
-			loops++;
-			size = 0;
-		}
-		tmp[0] = WRITE_AND_READ;
-		tmp[1] = (char)(send_bits >> 8);	// high 
-		tmp[2] = (char)(send_bits);			// low
-    
-		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 3, 1000);
-    
-		//INFO("HALLLLOOO3 %i",(int)tmp[0]);
-		int timeout = 0;
-		usleep(1);
-		while (usb_bulk_read(usbprog_jtag->usb_handle, 0x82, tmp, 64, 10) < 1)
-		{
-			timeout++;
-			if (timeout > 10)
-				break;
-		}
-
-		for (i = 0; i < loops; i++)
-		{
-			swap = tmp[3 + i];
-			buffer[fillindex++] = swap;
-		} 
-	}
-}
-
-void usbprog_jtag_write_tdi(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)
-{
-	char tmp[64];	// fastes packet size for usb controller
-	int send_bits, bufindex = 0, i, loops;
-
-	// 61 byte can be transfered (488 bit)
-	while (size > 0)
-	{
-		if (size > 488)
-		{
-			send_bits = 488;
-			size = size - 488;
-			loops = 61;
-		}
-		else
-		{
-			send_bits = size;
-			loops = size/8;
-			//if(loops==0)
-			loops++;
-			size = 0;
-		}
-		tmp[0] = WRITE_TDI;
-		tmp[1] = (char)(send_bits >> 8);	// high 
-		tmp[2] = (char)(send_bits);			// low
-		i = 0; 
-		
-		for (i = 0; i < loops; i++)
-		{
-			tmp[3 + i] = buffer[bufindex];
-			bufindex++;
-		}
-		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 64, 1000);
-	}
-}
-
-void usbprog_jtag_write_tms(struct usbprog_jtag *usbprog_jtag, char tms_scan)
-{
-	usbprog_jtag_tms_collect(tms_scan);
-}
-
-void usbprog_jtag_set_direction(struct usbprog_jtag *usbprog_jtag, unsigned char direction)
-{
-	char tmp[2];
-	tmp[0] = PORT_DIRECTION;
-	tmp[1] = (char)direction;
-	usbprog_jtag_message(usbprog_jtag, tmp, 2);
-}
-
-void usbprog_jtag_write_slice(struct usbprog_jtag *usbprog_jtag,unsigned char value)
-{
-	char tmp[2];
-	tmp[0] = PORT_SET;
-	tmp[1] = (char)value;
-	usbprog_jtag_message(usbprog_jtag, tmp, 2);
-}
-
-unsigned char usbprog_jtag_get_port(struct usbprog_jtag *usbprog_jtag)
-{
-	char tmp[2];
-	tmp[0] = PORT_GET;
-	tmp[1] = 0x00;
-	return usbprog_jtag_message(usbprog_jtag, tmp, 2);
-}
-
-void usbprog_jtag_set_bit(struct usbprog_jtag *usbprog_jtag,int bit, int value)
-{
-	char tmp[3];
-	tmp[0] = PORT_SETBIT;
-	tmp[1] = (char)bit;
-	if (value == 1)  
-		tmp[2] = 0x01;
-	else
-		tmp[2] = 0x00;
-	usbprog_jtag_message(usbprog_jtag, tmp, 3);
-}
-
-int usbprog_jtag_get_bit(struct usbprog_jtag *usbprog_jtag, int bit)
-{
-	char tmp[2];
-	tmp[0] = PORT_GETBIT;
-	tmp[1] = (char)bit;
-	
-	if (usbprog_jtag_message(usbprog_jtag, tmp, 2) > 0)
-		return 1;
-	else
-		return 0;
-}
-
-void usbprog_jtag_tms_collect(char tms_scan)
-{
-	tms_chain[tms_chain_index] = tms_scan;
-	tms_chain_index++;
-}
-
-void usbprog_jtag_tms_send(struct usbprog_jtag *usbprog_jtag)
-{
-	int i;
-	//INFO("TMS SEND");
-	if (tms_chain_index > 0)
-	{
-		char tmp[tms_chain_index + 2];
-		tmp[0] = WRITE_TMS_CHAIN;
-		tmp[1] = (char)(tms_chain_index);
-		for (i = 0; i < tms_chain_index + 1; i++)
-			tmp[2 + i] = tms_chain[i];
-		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, tms_chain_index + 2, 1000);
-		tms_chain_index = 0;
-	}
-}
+/***************************************************************************

+ *   Copyright (C) 2007 by Benedikt Sauter sauter@ixbat.de                 *

+ *   based on Dominic Rath's amt_jtagaccel.c                               *

+ *                                                                         *

+ *   usbprog is a free programming adapter. You can easily install         *

+ *   different firmware versions from an "online pool" over USB.           *

+ *   The adapter can be used for programming and debugging AVR and ARM     *

+ *   processors, as USB to RS232 converter, as JTAG interface or as        *

+ *   simple I/O interface (5 lines).                                       *

+ *                                                                         *

+ *   http://www.embedded-projects.net/usbprog                              *

+ *                                                                         *

+ *   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 <usb.h>

+

+/* system includes */

+

+#include "log.h"

+

+#define VID 0x1781

+#define PID 0x0c63

+

+/* Pins at usbprog */

+#define TDO_BIT         0

+#define TDI_BIT         3

+#define TCK_BIT         2

+#define TMS_BIT         1

+

+int usbprog_execute_queue(void);

+int usbprog_speed(int speed);

+int usbprog_register_commands(struct command_context_s *cmd_ctx);

+int usbprog_init(void);

+int usbprog_quit(void);

+

+void usbprog_end_state(enum tap_state state);

+void usbprog_state_move(void);

+void usbprog_path_move(pathmove_command_t *cmd);

+void usbprog_runtest(int num_cycles);

+void usbprog_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size);

+

+jtag_interface_t usbprog_interface =

+{

+	.name = "usbprog",

+	.execute_queue = usbprog_execute_queue,

+	.speed = usbprog_speed,

+	.register_commands = usbprog_register_commands,

+	.init = usbprog_init,

+	.quit = usbprog_quit

+};

+

+#define UNKOWN_COMMAND  0x00

+#define PORT_DIRECTION  0x01

+#define PORT_SET        0x02

+#define PORT_GET        0x03

+#define PORT_SETBIT     0x04

+#define PORT_GETBIT     0x05

+#define WRITE_TDI     	0x06

+#define READ_TDO     	0x07

+#define WRITE_AND_READ 	0x08

+#define WRITE_TMS     	0x09

+#define WRITE_TMS_CHAIN 0x0A

+

+struct usbprog_jtag

+{

+	struct usb_dev_handle* usb_handle;

+};

+

+struct usbprog_jtag * usbprog_jtag_handle;

+

+struct usbprog_jtag* usbprog_jtag_open();

+void usbprog_jtag_close(struct usbprog_jtag *usbprog_jtag);

+void usbprog_jtag_init(struct usbprog_jtag *usbprog_jtag);

+unsigned char usbprog_jtag_message(struct usbprog_jtag *usbprog_jtag, char *msg, int msglen);

+

+void usbprog_jtag_read_tdo(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);

+void usbprog_jtag_write_tdi(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);

+void usbprog_jtag_write_and_read(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);

+void usbprog_jtag_write_tms(struct usbprog_jtag *usbprog_jtag, char tms_scan);

+

+char tms_chain[64];

+int tms_chain_index;

+void usbprog_jtag_tms_collect(char tms_scan);

+void usbprog_jtag_tms_send(struct usbprog_jtag *usbprog_jtag);

+

+void usbprog_write(int tck, int tms, int tdi);

+void usbprog_reset(int trst, int srst);

+

+void usbprog_jtag_set_direction(struct usbprog_jtag *usbprog_jtag, unsigned char direction);

+void usbprog_jtag_write_slice(struct usbprog_jtag *usbprog_jtag,unsigned char value);

+unsigned char usbprog_jtag_get_port(struct usbprog_jtag *usbprog_jtag);

+void usbprog_jtag_set_bit(struct usbprog_jtag *usbprog_jtag,int bit, int value);

+int usbprog_jtag_get_bit(struct usbprog_jtag *usbprog_jtag, int bit);

+

+int usbprog_speed(int speed)

+{

+	return ERROR_OK;

+}

+

+int usbprog_register_commands(struct command_context_s *cmd_ctx)

+{

+	return ERROR_OK;

+}

+

+int usbprog_execute_queue(void)

+{

+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */

+	int scan_size;

+	enum scan_type type;

+	u8 *buffer;

+

+	while (cmd)

+	{

+		switch (cmd->type)

+		{

+			case JTAG_END_STATE:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);

+#endif

+				if (cmd->cmd.end_state->end_state != -1)

+					usbprog_end_state(cmd->cmd.end_state->end_state);

+				break;

+			case JTAG_RESET:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);

+#endif

+				if (cmd->cmd.reset->trst == 1)

+				{

+					cur_state = TAP_TLR;

+				}

+				usbprog_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);

+				break;

+			case JTAG_RUNTEST:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);

+#endif

+				if (cmd->cmd.runtest->end_state != -1)

+					usbprog_end_state(cmd->cmd.runtest->end_state);

+				usbprog_runtest(cmd->cmd.runtest->num_cycles);

+				break;

+			case JTAG_STATEMOVE:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);

+#endif

+				if (cmd->cmd.statemove->end_state != -1)

+					usbprog_end_state(cmd->cmd.statemove->end_state);

+				usbprog_state_move();

+				break;

+			case JTAG_PATHMOVE:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states,

+					cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);

+#endif

+				usbprog_path_move(cmd->cmd.pathmove);

+				break;

+			case JTAG_SCAN:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("scan end in %i", cmd->cmd.scan->end_state);

+#endif

+				if (cmd->cmd.scan->end_state != -1)

+					usbprog_end_state(cmd->cmd.scan->end_state);

+				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);

+				type = jtag_scan_type(cmd->cmd.scan);

+				usbprog_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);

+				if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)

+					return ERROR_JTAG_QUEUE_FAILED;

+				if (buffer)

+					free(buffer);

+				break;

+			case JTAG_SLEEP:

+#ifdef _DEBUG_JTAG_IO_

+				DEBUG("sleep %i", cmd->cmd.sleep->us);

+#endif

+				jtag_sleep(cmd->cmd.sleep->us);

+					break;

+			default:

+				ERROR("BUG: unknown JTAG command type encountered");

+				exit(-1);

+		}

+

+		cmd = cmd->next;

+	}

+

+	return ERROR_OK;

+}

+

+int usbprog_init(void)

+{

+	usbprog_jtag_handle = usbprog_jtag_open();

+

+	tms_chain_index = 0;

+	if (usbprog_jtag_handle == 0)

+	{

+		ERROR("Can't find USB JTAG Interface! Please check connection and permissions.");

+		return ERROR_JTAG_INIT_FAILED;

+	}

+

+	INFO("USB JTAG Interface ready!");

+

+	usbprog_jtag_init(usbprog_jtag_handle);

+	usbprog_reset(0, 0);

+	usbprog_write(0, 0, 0);

+

+	return ERROR_OK;

+}

+

+int usbprog_quit(void)

+{

+	return ERROR_OK;

+}

+

+/*************** jtag execute commands **********************/

+void usbprog_end_state(enum tap_state state)

+{

+	if (tap_move_map[state] != -1)

+		end_state = state;

+	else

+	{

+		ERROR("BUG: %i is not a valid end state", state);

+		exit(-1);

+	}

+}

+

+void usbprog_state_move(void)

+{

+	int i = 0, tms = 0;

+	u8 tms_scan = TAP_MOVE(cur_state, end_state);

+

+	usbprog_jtag_write_tms(usbprog_jtag_handle, (char)tms_scan);

+	for (i = 0; i < 7; i++)

+	{

+		tms = (tms_scan >> i) & 1;

+	}

+

+	cur_state = end_state;

+}

+

+void usbprog_path_move(pathmove_command_t *cmd)

+{

+	int num_states = cmd->num_states;

+	int state_count;

+

+	state_count = 0;

+	while (num_states)

+	{

+		if (tap_transitions[cur_state].low == cmd->path[state_count])

+		{

+			//INFO("1");

+			usbprog_write(0, 0, 0);

+			usbprog_write(1, 0, 0);

+		}

+		else if (tap_transitions[cur_state].high == cmd->path[state_count])

+		{

+			//INFO("2");

+			usbprog_write(0, 1, 0);

+			usbprog_write(1, 1, 0);

+		}

+		else

+		{

+			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[state_count]]);

+			exit(-1);

+		}

+

+		cur_state = cmd->path[state_count];

+		state_count++;

+		num_states--;

+	}

+

+	end_state = cur_state;

+}

+

+void usbprog_runtest(int num_cycles)

+{

+	int i;

+

+	/* only do a state_move when we're not already in RTI */

+	if (cur_state != TAP_RTI)

+	{

+		usbprog_end_state(TAP_RTI);

+		usbprog_state_move();

+	}

+

+	/* execute num_cycles */

+	if (num_cycles > 0)

+	{

+		usbprog_jtag_tms_send(usbprog_jtag_handle);

+		usbprog_write(0, 0, 0);

+	}

+	else

+	{

+		usbprog_jtag_tms_send(usbprog_jtag_handle);

+		//INFO("NUM CYCLES %i",num_cycles);

+	}

+

+	for (i = 0; i < num_cycles; i++)

+	{

+		usbprog_write(1, 0, 0);

+		usbprog_write(0, 0, 0);

+	}

+

+	/* finish in end_state */

+	/*

+	usbprog_end_state(saved_end_state);

+	if (cur_state != end_state)

+		usbprog_state_move();

+	*/

+}

+

+void usbprog_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)

+{

+	enum tap_state saved_end_state = end_state;

+

+	if (ir_scan)

+		usbprog_end_state(TAP_SI);

+	else

+		usbprog_end_state(TAP_SD);

+

+	//usbprog_jtag_tms_send(usbprog_jtag_handle);

+

+	usbprog_state_move();

+	usbprog_end_state(saved_end_state);

+

+	usbprog_jtag_tms_send(usbprog_jtag_handle);

+

+	if (type == SCAN_OUT)

+	{

+		usbprog_jtag_write_tdi(usbprog_jtag_handle,buffer, scan_size);

+	}

+	if (type == SCAN_IN)

+	{

+		usbprog_jtag_read_tdo(usbprog_jtag_handle,buffer, scan_size);

+	}

+	if (type == SCAN_IO)

+	{

+		usbprog_jtag_write_and_read(usbprog_jtag_handle,buffer, scan_size);

+	}

+

+	if (ir_scan)

+		cur_state = TAP_PI;

+	else

+		cur_state = TAP_PD;

+

+	if (cur_state != end_state)

+		usbprog_state_move();

+}

+

+/*************** jtag wrapper functions *********************/

+

+void usbprog_write(int tck, int tms, int tdi)

+{

+	unsigned char output_value=0x00;

+

+	if (tms)

+		output_value |= (1<<TMS_BIT);

+	if (tdi)

+		output_value |= (1<<TDI_BIT);

+	if (tck)

+		output_value |= (1<<TCK_BIT);

+

+	usbprog_jtag_write_slice(usbprog_jtag_handle,output_value);

+}

+

+/* (1) assert or (0) deassert reset lines */

+void usbprog_reset(int trst, int srst)

+{

+	DEBUG("trst: %i, srst: %i", trst, srst);

+

+	if (trst)

+		usbprog_jtag_set_bit(usbprog_jtag_handle, 5, 0);

+	else

+		usbprog_jtag_set_bit(usbprog_jtag_handle, 5, 1);

+

+	if (srst)

+		usbprog_jtag_set_bit(usbprog_jtag_handle, 4, 0);

+	else

+		usbprog_jtag_set_bit(usbprog_jtag_handle, 4, 1);

+}

+

+/*************** jtag lowlevel functions ********************/

+

+struct usb_bus *busses;

+

+struct usbprog_jtag* usbprog_jtag_open()

+{

+	struct usb_bus *bus;

+	struct usb_device *dev;

+

+	struct usbprog_jtag *tmp;

+

+	tmp = (struct usbprog_jtag*)malloc(sizeof(struct usbprog_jtag));

+

+	usb_set_debug(10);

+	usb_init();

+	usb_find_busses();

+	usb_find_devices();

+

+	busses = usb_get_busses();

+

+	/* find usbprog_jtag device in usb bus */

+

+	for (bus = busses; bus; bus = bus->next)

+	{

+		for (dev = bus->devices; dev; dev = dev->next)

+		{

+			/* condition for sucessfully hit (too bad, I only check the vendor id)*/

+			if (dev->descriptor.idVendor == VID && dev->descriptor.idProduct == PID)

+			{

+				tmp->usb_handle = usb_open(dev);

+				usb_set_configuration(tmp->usb_handle, 1);

+				usb_claim_interface(tmp->usb_handle, 0);

+				usb_set_altinterface(tmp->usb_handle, 0);

+				return tmp;

+			}

+		}

+	}

+	return 0;

+}

+

+void usbprog_jtag_close(struct usbprog_jtag *usbprog_jtag)

+{

+	usb_close(usbprog_jtag->usb_handle);

+	free(usbprog_jtag);

+}

+

+unsigned char usbprog_jtag_message(struct usbprog_jtag *usbprog_jtag, char *msg, int msglen)

+{

+	int res = usb_bulk_write(usbprog_jtag->usb_handle, 3, msg,msglen, 100);

+	if ((msg[0] == 2) || (msg[0] == 1) || (msg[0] == 4) || (msg[0] == 0) || \

+			(msg[0] == 6) || (msg[0] == 0x0A) || (msg[0] == 9))

+		return 1;

+	if (res == msglen)

+	{

+		//INFO("HALLLLOOO %i",(int)msg[0]);

+		res =  usb_bulk_read(usbprog_jtag->usb_handle, 0x82, msg, 2, 100);

+		if (res > 0)

+			return (unsigned char)msg[1];

+		else

+			return -1;

+	}

+	else

+		return -1;

+	return 0;

+}

+

+void usbprog_jtag_init(struct usbprog_jtag *usbprog_jtag)

+{

+	usbprog_jtag_set_direction(usbprog_jtag, 0xFE);

+}

+

+void usbprog_jtag_write_and_read(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)

+{

+	char tmp[64];	/* fastes packet size for usb controller */

+	int send_bits, bufindex = 0, fillindex = 0, i, loops;

+

+	char swap;

+	/* 61 byte can be transfered (488 bit) */

+

+	while (size > 0)

+	{

+		if (size > 488)

+		{

+			send_bits = 488;

+			size = size - 488;

+			loops = 61;

+		}

+		else

+		{

+			send_bits = size;

+			loops = size / 8;

+			loops++;

+			size = 0;

+		}

+		tmp[0] = WRITE_AND_READ;

+		tmp[1] = (char)(send_bits >> 8);	/* high */

+		tmp[2] = (char)(send_bits);			/* low */

+		i = 0;

+

+		for (i = 0; i < loops; i++)

+		{

+			tmp[3 + i] = buffer[bufindex];

+			bufindex++;

+		}

+

+		if (usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 64, 1000) == 64)

+		{

+			//INFO("HALLLLOOO2 %i",(int)tmp[0]);

+			usleep(1);

+			int timeout = 0;

+			while (usb_bulk_read(usbprog_jtag->usb_handle, 0x82, tmp, 64, 1000) < 1)

+			{

+				timeout++;

+				if (timeout > 10)

+					break;

+			}

+

+			for (i = 0; i < loops; i++)

+			{

+				swap =  tmp[3 + i];

+				buffer[fillindex++] = swap;

+			}

+		}

+	}

+}

+

+void usbprog_jtag_read_tdo(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)

+{

+	char tmp[64];	/* fastes packet size for usb controller */

+	int send_bits, fillindex = 0, i, loops;

+

+	char swap;

+	/* 61 byte can be transfered (488 bit) */

+

+	while (size > 0)

+	{

+		if (size > 488)

+		{

+			send_bits = 488;

+			size = size - 488;

+			loops = 61;

+		}

+		else

+		{

+			send_bits = size;

+			loops = size / 8;

+			loops++;

+			size = 0;

+		}

+		tmp[0] = WRITE_AND_READ;

+		tmp[1] = (char)(send_bits >> 8);	/* high */

+		tmp[2] = (char)(send_bits);			/* low */

+

+		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 3, 1000);

+

+		//INFO("HALLLLOOO3 %i",(int)tmp[0]);

+		int timeout = 0;

+		usleep(1);

+		while (usb_bulk_read(usbprog_jtag->usb_handle, 0x82, tmp, 64, 10) < 1)

+		{

+			timeout++;

+			if (timeout > 10)

+				break;

+		}

+

+		for (i = 0; i < loops; i++)

+		{

+			swap = tmp[3 + i];

+			buffer[fillindex++] = swap;

+		}

+	}

+}

+

+void usbprog_jtag_write_tdi(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)

+{

+	char tmp[64];	/* fastes packet size for usb controller */

+	int send_bits, bufindex = 0, i, loops;

+

+	/* 61 byte can be transfered (488 bit) */

+	while (size > 0)

+	{

+		if (size > 488)

+		{

+			send_bits = 488;

+			size = size - 488;

+			loops = 61;

+		}

+		else

+		{

+			send_bits = size;

+			loops = size/8;

+			//if(loops==0)

+			loops++;

+			size = 0;

+		}

+		tmp[0] = WRITE_TDI;

+		tmp[1] = (char)(send_bits >> 8);	/* high */

+		tmp[2] = (char)(send_bits);			/* low */

+		i = 0;

+

+		for (i = 0; i < loops; i++)

+		{

+			tmp[3 + i] = buffer[bufindex];

+			bufindex++;

+		}

+		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 64, 1000);

+	}

+}

+

+void usbprog_jtag_write_tms(struct usbprog_jtag *usbprog_jtag, char tms_scan)

+{

+	usbprog_jtag_tms_collect(tms_scan);

+}

+

+void usbprog_jtag_set_direction(struct usbprog_jtag *usbprog_jtag, unsigned char direction)

+{

+	char tmp[2];

+	tmp[0] = PORT_DIRECTION;

+	tmp[1] = (char)direction;

+	usbprog_jtag_message(usbprog_jtag, tmp, 2);

+}

+

+void usbprog_jtag_write_slice(struct usbprog_jtag *usbprog_jtag,unsigned char value)

+{

+	char tmp[2];

+	tmp[0] = PORT_SET;

+	tmp[1] = (char)value;

+	usbprog_jtag_message(usbprog_jtag, tmp, 2);

+}

+

+unsigned char usbprog_jtag_get_port(struct usbprog_jtag *usbprog_jtag)

+{

+	char tmp[2];

+	tmp[0] = PORT_GET;

+	tmp[1] = 0x00;

+	return usbprog_jtag_message(usbprog_jtag, tmp, 2);

+}

+

+void usbprog_jtag_set_bit(struct usbprog_jtag *usbprog_jtag,int bit, int value)

+{

+	char tmp[3];

+	tmp[0] = PORT_SETBIT;

+	tmp[1] = (char)bit;

+	if (value == 1)

+		tmp[2] = 0x01;

+	else

+		tmp[2] = 0x00;

+	usbprog_jtag_message(usbprog_jtag, tmp, 3);

+}

+

+int usbprog_jtag_get_bit(struct usbprog_jtag *usbprog_jtag, int bit)

+{

+	char tmp[2];

+	tmp[0] = PORT_GETBIT;

+	tmp[1] = (char)bit;

+

+	if (usbprog_jtag_message(usbprog_jtag, tmp, 2) > 0)

+		return 1;

+	else

+		return 0;

+}

+

+void usbprog_jtag_tms_collect(char tms_scan)

+{

+	tms_chain[tms_chain_index] = tms_scan;

+	tms_chain_index++;

+}

+

+void usbprog_jtag_tms_send(struct usbprog_jtag *usbprog_jtag)

+{

+	int i;

+	//INFO("TMS SEND");

+	if (tms_chain_index > 0)

+	{

+		char tmp[tms_chain_index + 2];

+		tmp[0] = WRITE_TMS_CHAIN;

+		tmp[1] = (char)(tms_chain_index);

+		for (i = 0; i < tms_chain_index + 1; i++)

+			tmp[2 + i] = tms_chain[i];

+		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, tms_chain_index + 2, 1000);

+		tms_chain_index = 0;

+	}

+}