www.pudn.com > jtag_src.rar > arm7tdmi.c

/* 
 * arm7tdmi.c:	implement the core routes for ARM7TDMI 
 * 
 * Copyright (C) 2004, OPEN-JTAG, All rights reserved. 
 */ 
 
#include  
#include "../tool.h" 
#include "../xjerr.h" 
#include "../tapctrl.h" 
#include "arm7tdmi.h" 
#include "ice.h" 
 
 
 
//Used to record some important staus of the arm7tdmi 
arm7tdmi_status_t arm7tdmi_status; 
static arm7tdmi_breakpt_list_t *arm7tdmi_breakpt_head = NULL; 
static arm7tdmi_watchpt_status_t arm7tdmi_watchpt_status = {0, 0}; 
 
/* 
 * arm7tdmi_init() - 
 *		Used to initialize the arm7tdmi's core status. 
 *		Please call this route before starting any debug 
 *		operations. 
 */ 
int arm7tdmi_init(void) 
{ 
	arm7tdmi_status.by = -1; 
	arm7tdmi_status.scanchain = 3;				//On reset, scan chain 3 is selected by default. 
	arm7tdmi_status.from = ARM7TDMI_FROM_ARM; 
	arm7tdmi_status.state = ARM7TDMI_SYSTEM_STATE; 
	arm7tdmi_status.endian = ARM7TDMI_LITTLE_END; 
	 
	return XJ_OK;	 
} 
 
 
/* 
 * arm7tdmi_connect_scanchain() -  
 *		Used to select specific scan chian of arm7tmi and put 
 *		the selected scan chain into test state. Before returning, 
 *		the avtive scan chain is updated. 
 * 
 *		@sc_num: the scan chain to be selected. 
 */ 
int arm7tdmi_connect_scanchain(int sc_num) 
{ 
	int status; 
	int shift_out; 
 
	if(sc_num < 0 || sc_num > ARM7TDMI_NUMOF_SCANCHAIN) 
		return XJ_ERROR; 
 
	/* 
	 * In SYSTEM state, select scan chain 0 and 1 is not allowed. Select scan 
	 * chain 0 or 1 when in SYSTEM state will affect the debug and put the debug 
	 * into unpredictable situation. 
	 */ 
	if( (arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE) && (sc_num != 2) ) 
		return XJERR_TARGET_RUNNING; 
 
	status = tapctrl_acs_ireg(JTAG_SCAN_N); 
	if(status != XJ_OK) 
		return status; 
 
	status = tapctrl_acs_dreg(ARM7TDMI_REGLEN_SCSEL, &sc_num, &shift_out); 
	if(status != XJ_OK) 
		return status; 
	if(shift_out != 0x8)			//shift out should be b1000 
		return XJERR_SCANCHAIN_FAIL; 
 
	//Enter INTEST state 
	status = tapctrl_acs_ireg(JTAG_INTEST); 
	if(status != XJ_OK) 
		return status; 
	 
	//Update the active scan chain 
	arm7tdmi_status.scanchain = sc_num; 
 
	return XJ_OK; 
} 
 
 
/* 
 * arm7tdmi_acs_sc1() -  
 *		Used to acces scan chain 1. By calling this route, ARM/THUMB  
 *		instructions can be scaned into the databus and clocked into 
 *		the pipeline. When scan chain 1 is selected and put into  
 *		INTEST state, everytime enter the RUN-TEST/IDLE state, a DCLK 
 *		is generated. Please make sure scan chain 1 is selected and  
 *		put it into INTEST state before calling this route. 
 *  
 *		Due to the reverse bit sequence of scan chain 1 cells, 
 *		all the data shifted in and out from scan chain 1 should 
 *		be reversed. 
 * 
 *		@shift_in:  data shifted into scan chain 1 
 *		@shift_out: data shifted out from scan chain 1 
 */ 
int arm7tdmi_acs_sc1(u32 *shift_in, u32 *shift_out) 
{ 
	int status; 
	u32 in_temp[2]; 
	u32 out_temp[2]; 
 
	//In DEBUG state? 
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE) 
		return XJERR_TARGET_RUNNING; 
	 
	//Scan chain 1 selected? 
	if(arm7tdmi_status.scanchain != 1) 
		return XJERR_SC1_NOT_SELECTED; 
	 
	tool_reverse_bit_order(ARM7TDMI_REGLEN_SC1, shift_in, in_temp);		//Revert shifted in 
	status = tapctrl_acs_dreg(ARM7TDMI_REGLEN_SC1, in_temp, out_temp); 
	tool_reverse_bit_order(ARM7TDMI_REGLEN_SC1, out_temp, shift_out);	//Revert shifted out 
	 
	return XJ_OK; 
} 
 
 
/* 
 * arm7tdmi_system_reste() - 
 *		This route use nRESET to reset the system. 
 *		To reset the system by nRESET, there must have a 
 *		connection between the jtag interface and nRESET 
 */ 
int arm7tdmi_system_reset(void) 
{ 
	return XJ_OK; 
} 
 
 
/* 
 * arm7tdmi_enter_dbgstat() -  
 *		This route is used to force the target enter DEBUG state by  
 *		forcing DEBUGRQ signal HIGH. The value of PC when entering 
 *		DEBUG state is recorded.  
 * 
 *		The obtained value of PC indicates the next instruction to  
 *		be executed on exit from DEBUG state, which is important and 
 *		critcal to resume the normal execution of program under debug. 
 *		It is for later use to return from the DEBUG state back to  
 *		the SYSTEM state. 
 * 
 *		Please modify this function carefully. 
 *  
 *		@pc:	used to return the value of PC which indicates the 
 *				next instruction to be execute on exit from DEBUG 
 *				state. 
 */ 
int arm7tdmi_enter_dbgstat(u32 *pc) 
{ 
	int status; 
	u32 r0; 
	u32 dbgstat; 
	u32 shift_in[2]; 
	u32 shift_out[2]; 
 
	if(pc == NULL) 
		return XJERR_INVALID_PARAMATER; 
 
	//Check whether already in DEBUG state? 
	if(arm7tdmi_status.state == ARM7TDMI_DEBUG_STATE) 
		return XJERR_TARGET_HALTED; 
 
	//Enter DEBUG state by forcing DEBUGRQ signal HIGH  
	 
	//Select scan chain 2 
	status = arm7tdmi_connect_scanchain(2); 
	if(status != XJ_OK) 
		return status; 
	 
	//Write DEBUG control register 
	status = arm7tdmi_ice_write(ARM7TDMI_ICE_DBGCTRL, 0x6); 
	status = tapctrl_runtest(); 
 
	//Check the DBGACK signal to judge whether operation success? 
	status = arm7tdmi_ice_read(ARM7TDMI_ICE_DBGSTAT, &dbgstat); 
	if(status != XJ_OK) 
		return status; 
	if( (dbgstat & 0x1) == 0 ) 
		return XJERR_ENTDBG_FAIL; 
 
	//Successful, update the arm7tdmi status 
	arm7tdmi_status.by = ARM7TDMI_BY_BREAKPT; 
	arm7tdmi_status.state = ARM7TDMI_DEBUG_STATE; 
	 
	if (dbgstat & 0x10) 
		arm7tdmi_status.from = ARM7TDMI_FROM_THUMB; 
	else 
		arm7tdmi_status.from = ARM7TDMI_FROM_ARM; 
 
	/* 
	 * Don't forget to clear the DEBUG control register to  
	 * avoid enter DEBUG state. This is necessary. 
	 */ 
	arm7tdmi_ice_write(ARM7TDMI_ICE_DBGCTRL, 0x0); 
 
 
	/* 
	 * Try to obtain the value of PC when entering DEBUG state. The obtained 
	 * value of PC indicates the next instruction to be executed on exit from 
	 * DEBUG state. 
	 */ 
 
	//Select scan chain 1 
	status = arm7tdmi_connect_scanchain(1); 
	if(status != XJ_OK) 
		return status; 
 
	if(arm7tdmi_status.from == ARM7TDMI_FROM_ARM){    //Enter DEBUG from ARM state 
 
		//Step 1 - Read R0 
 
		//STR R0, [R0] = 0xE5800000					 
		shift_in[0] = 0xE5800000;					//Instruction 1 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruction 2 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruction 3 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP; 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
		r0 = shift_out[0]; 
 
		//Step 2 - Move PC to R0 
 
		//MOV R0, PC = 0xE1A0000F 
		shift_in[0] = 0xE1A0000F;					//Instruction 4 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out);	 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 3 - Read the value of PC from R0 
 
		//STR R0, [R0] = 0xE5800000 
		shift_in[0] = 0xE5800000;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
		*pc = shift_out[0]; 
 
		/*  
		 * Entry into DEBUG state through a debug request is similar to a  
		 * breakpoint. However, unlike a breakpoint, the last instruction  
		 * has completed execution and so must not be refetched on exit  
		 * from DEBUG state. You can assume that entry to DEBUG state adds 
		 * three addresses to the PC and every instruction executed in DEBUG 
		 * state adds one address. To move the value of PC to R0, 4 instructions 
		 * have been executed in DEBUG state. So, the value of PC which  
		 * indicates the next instruction should be executed on exit from  
		 * DEBUG state is 
		 *				pc - 4 x (3 + 4) 
		 */ 
		*pc -= 28; 
 
	}else{		//Enter DEBUG from THUMB state 
		 
		//Step 1 - Read R0  
 
		//STR R0, [R0] = 0x60006000 
		shift_in[0] = 0x60006000;					//Instruction 1 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					//Instruction 2 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					//Instruction 3 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//R0 
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
		r0 = shift_out[0]; 
 
		//Step 2 - Move PC to R0 
 
		//MOV R0, PC = 0x46784678 
		shift_in[0] = 0x46784678;					//Instruction 4			 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 3 - Read the value of PC from R0 
 
		//STR R0, [R0] = 0x60006000 
		shift_in[0] = 0x60006000;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
		*pc = shift_out[0]; 
 
		/*  
		 * Entry into DEBUG state through a debug request is similar to a  
		 * breakpoint. However, unlike a breakpoint, the last instruction  
		 * has completed execution and so must not be refetched on exit  
		 * from DEBUG state. You can assume that entry to DEBUG state adds 
		 * three addresses to the PC and every instruction executed in DEBUG 
		 * state adds one address. To move the value of PC to R0, 4 instructions 
		 * have been executed in DEBUG state. So, the value of PC which  
		 * indicates the next instruction should be executed on exit from  
		 * DEBUG state is 
		 *				pc - 2 x (3 + 4) 
		 */ 
		*pc -= 14; 
 
		//Step4 - Switch from THUMB state to ARM state 
 
		/* 
		 * When enter DEBUG state from THUBM state, we make ARM7TDMI enter ARM  
		 * state before any other debug operation is performed. Before leave the 
		 * DEBUG state, we make ARM7TDMI return back to THUMB state before the 
		 * normal operation is resumed. 
		 */ 
 
		//BX PC = 0x47784778 
		shift_in[0] = 0x47784778;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
	} 
 
	//Last Step - Restore R0 
	arm7tdmi_core_wri_reg(ARM7TDMI_R0, r0); 
 
	return XJ_OK; 
} 
 
 
/*  
 * arm7tdmi_check_dbgstat() - 
 *		After some breakpts or watchpts are setting, this route is used 
 *		to check the DEBUG status register to check whether the ARM7TDMI 
 *		enter DEBUG state by breakpt/watchpt.  
 * 
 *		When DBGACK flag is not set, XJ_ERROR is returned. When DBGACK  
 *		flag is set, it means the ARM7TDMI is in DEBUG state. Next, try 
 *		to obtain the value of PC when entering the DEBUG state.  
 * 
 *		The obtained value of PC indicates the next instruction to be  
 *		executed on exit from DEBUG state, which is important and 
 *		critcal to resume the normal execution of program under debug. 
 *		It is for later use to return from the DEBUG state back to  
 *		the SYSTEM state. 
 * 
 *		Please modify this function carefully. 
 * 
 *		@pc:	used to return the value of PC which indicates the next  
 *				instruction to be execute on exit from DEBUG state. 
 */ 
int arm7tdmi_check_dbgstat(u32 *pc) 
{ 
	int status; 
	u32 r0; 
	u32 dbgstat; 
	u32 shift_in[2]; 
	u32 shift_out[2]; 
 
	if(pc == NULL) 
		return XJERR_INVALID_PARAMATER; 
 
	//In DEBUG state? 
	if(arm7tdmi_status.state == ARM7TDMI_DEBUG_STATE) 
		return XJERR_TARGET_HALTED; 
 
	//Select scan chain 2 
	status = arm7tdmi_connect_scanchain(2); 
	if(status != XJ_OK) 
		return status; 
 
	//Check DEBUG status register 
	arm7tdmi_ice_read(ARM7TDMI_ICE_DBGSTAT, &dbgstat); 
	if(dbgstat & 0x1){ 
		arm7tdmi_status.state = ARM7TDMI_DEBUG_STATE; 
		if (dbgstat & 0x10) 
			arm7tdmi_status.from = ARM7TDMI_FROM_THUMB; 
		else 
			arm7tdmi_status.from = ARM7TDMI_FROM_ARM; 
	}else 
		return XJERR_TARGET_RUNNING; 
 
	/* 
	 * Try to obtain the value of PC when entering DEBUG state. The obtained 
	 * value of PC indicates the next instruction to be executed on exit from 
	 * DEBUG state. 
	 */ 
 
	//Select scan chain 1 
	status = arm7tdmi_connect_scanchain(1); 
	if(status != XJ_OK) 
		return status; 
 
 
	if(arm7tdmi_status.from == ARM7TDMI_FROM_ARM){	//Enter DEBUG from ARM state 
 
		//Step 1 - Read R0  
 
		//STR R0, [R0] = 0xE5800000					 
		shift_in[0] = 0xE5800000;					//Instruction 1 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		/*  
		 * After the ARM7TDMI core has entered debug state from breakpt/watchpt, 
		 * the first time the 33rd bit is captured and scanned out, its value  
		 * tells the debugger if the core entered debug state due to a breakpoint 
		 * (bit 33 clear) or a watchpoint (bit 33 set). 
		 */ 
		if(shift_out[1] & 0x1) 
			arm7tdmi_status.by = ARM7TDMI_BY_WATCHPT; 
		else 
			arm7tdmi_status.by = ARM7TDMI_BY_BREAKPT; 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruction 2 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruction 3 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP; 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
		r0 = shift_out[0]; 
 
		//Step 2 - Move PC to R0 
 
		//MOV R0, PC = 0xE1A0000F 
		shift_in[0] = 0xE1A0000F;					//Instruction 4 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out);	 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 3 - Read the value of PC from R0 
 
		//STR R0, [R0] = 0xE5800000 
		shift_in[0] = 0xE5800000;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
		*pc = shift_out[0]; 
 
		/* 
		 * Entry into DEBUG state from a breakpoint/watchpoint advances the  
		 * PC by four addresses. Each instruction executed in DEBUG state  
		 * advances the PC by one address.  
		 * 
		 * To move the value of PC to R0, 4 instructions have been executed in D 
		 * EBUG state. So,  the value of PC which indicates the next instruction 
		 * should be executed on exit from DEBUG state is 
		 *			pc - 4 x (4 + 4) 
		 */ 
		*pc -= 32; 
 
	}else{		//Enter DEBUG from THUMB state 
 
		//Step 1 - Read R0  
 
		//STR R0, [R0] = 0x60006000 
		shift_in[0] = 0x60006000;					//Instruction 1 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		/*  
		 * After the ARM7TDMI core has entered debug state from breakpt/watchpt, 
		 * the first time the 33rd bit is captured and scanned out, its value  
		 * tells the debugger if the core entered debug state due to a breakpoint 
		 * (bit 33 clear) or a watchpoint (bit 33 set). 
		 */ 
		if(shift_out[1] & 0x1) 
			arm7tdmi_status.by = ARM7TDMI_BY_WATCHPT; 
		else 
			arm7tdmi_status.by = ARM7TDMI_BY_BREAKPT; 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					//Instruction 2 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					//Instruction 3 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//R0 
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
		r0 = shift_out[0]; 
 
		//Step 2 - Move PC to R0 
 
		//MOV R0, PC = 0x46784678 
		shift_in[0] = 0x46784678;					//Instruction 4			 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 3 - Read the value of PC from R0 
 
		//STR R0, [R0] = 0x60006000 
		shift_in[0] = 0x60006000;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
		*pc = shift_out[0]; 
 
		/* 
		 * Entry into DEBUG state from a breakpoint/watchpoint advances the  
		 * PC by four addresses. Each instruction executed in DEBUG state  
		 * advances the PC by one address.  
		 * 
		 * To move the value of PC to R0, 4 instructions have been executed in D 
		 * EBUG state. So,  the value of PC which indicates the next instruction 
		 * should be executed on exit from DEBUG state is 
		 *			pc - 2 x (4 + 4) 
		 */	 
		*pc -= 16; 
 
		//Step4 - Switch from THUMB state to ARM state 
 
		/* 
		 * When enter DEBUG state, we make ARM7TDMI enter ARM state before any 
		 * further debug is performed. When leave from DEBUG state, we make 
		 * ARM7TDMI return to THUMB state before the normal operation is resumed. 
		 */ 
 
		//BX PC = 0x47784778 
		shift_in[0] = 0x47784778;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
	} 
 
	//LastStep - Restore R0 
	arm7tdmi_core_wri_reg(ARM7TDMI_R0, r0); 
 
	return XJ_OK; 
} 
 
 
/* 
 * arm7tdmi_exit_dbgstat() -  
 *		This route is used to exit from the DEBUG state and return to the 
 *		normal SYSTEM state. The input pc indicates the next instruction 
 *		to be exectued on exit from DEBUG state. After some operations in 
 *		the DEBUG state, this route can be used to exit from the DEBUG state 
 *		and resume the execution of program under debug. 
 * 
 *		Please modify this function carefully. 
 * 
 *		@pc:	the value of pc which indicates the next instruction to 
 *				be executed on exit from DEBUG state. 
 */ 
int arm7tdmi_exit_dbgstat(u32 pc) 
{ 
	int status; 
	u32 r0; 
	u32 shift_in[2]; 
	u32 shift_out[2]; 
 
	//In SYSTEM state? 
	if(arm7tdmi_status.state == ARM7TDMI_SYSTEM_STATE) 
		return XJ_OK; 
	 
	//Clear the DEBUGRQ flag in DBGCTRL register  
	status = arm7tdmi_connect_scanchain(2); 
	if(status != XJ_OK) 
		return status; 
 
	status = arm7tdmi_ice_write(ARM7TDMI_ICE_DBGCTRL, 0x0); 
	if(status != XJ_OK) 
		return status; 
 
	//Select scan chain 1 
	status = arm7tdmi_connect_scanchain(1); 
	if(status != XJ_OK) 
		return status; 
 
	if(arm7tdmi_status.from == ARM7TDMI_FROM_ARM){	//Enter DEBUG from ARM state 
 
		//Step 1 - Read R0 & Write the new value of PC to R0 
 
		pc &= 0xFFFFFFFC;	//Align 
		status = arm7tdmi_core_rd_reg(ARM7TDMI_R0, &r0); 
		status = arm7tdmi_core_wri_reg(ARM7TDMI_R0, pc); 
 
		//Step 2 - Move R0 to PC 
 
		//MOV PC, R0 = 0xE1A0F000					 
		shift_in[0] = 0xE1A0F000;				 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruct 1 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruct 2 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 3 - Restore R0 
 
		//MOV R0, #0 = 0xE3A00000 
		shift_in[0] = 0xE3A00000;					//Instruct 3 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruct 4 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruct 5 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//LDR R0, [R0] = 0xE5900000 
		shift_in[0] = 0xE5900000;					//Instruct 6 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruct 7 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruct 8 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//R0 - put the value of R0 to the data bus 
		shift_in[0] = r0; 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Write the value obtained from data bus to R0 
		shift_in[0] = ARM7TDMI_NOP; 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 4 - Set SYSTEM speed flag 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					//Instruct 9	 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP + SYSTEM SPEED 
		shift_in[0] = ARM7TDMI_NOP;					//Instruct 10		 
		shift_in[1] = ARM7TDMI_SYSTEM_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		/* 
		 * After move the new value to register PC, 10 instructions have been 
		 * executed so far. To exit from DEBUG state and resume the normal  
		 * operation of the program under debug, 10 instructions backwards. 
		 */ 
 
		//B -10 = 0xEAFFFFF6 
		shift_in[0] = 0xEAFFFFF6;					//Final branch 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
	}else{	//Enter DEBUG from THUMB state 
 
		//Step 1 - Read R0 & Write the new value of PC to R0 
 
		pc &= 0xFFFFFFFE;	//Align 
		status = arm7tdmi_core_rd_reg(ARM7TDMI_R0, &r0); 
		status = arm7tdmi_core_wri_reg(ARM7TDMI_R0, pc + 1); 
 
		//Step 2 - Switch from ARM state back to THUMB state first 
 
		//BX R0 = 0xE12FFF10 
		shift_in[0] = 0xE12FFF10; 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP 
		shift_in[0] = ARM7TDMI_NOP;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 3 - Sbbtract R0 by 1 
		 
		//SUB R0, #1 = 0x38013801 
		shift_in[0] = 0x38013801;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 4 - Move R0 to PC 
		 
		//MOV PC, R0 = 0x46874687 
		shift_in[0] = 0x46874687;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;						//Instruct 1 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;						//Instruct 2 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 5 - Restore R0 
 
		//MOV R0, #0 = 0x20002000 
		shift_in[0] = 0x20002000;						//Instruct 3				 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;						//Instruct 4				 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;						//Instruct 5			 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//LDR R0, [R0] = 0x68006800 
		shift_in[0] = 0x68006800;						//Instruct 6			 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;						//Instruct 7				 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;						//Instruct 8					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//R1 - put the value of R0 to the data bus 
		shift_in[0] = r0;									 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Write the value obtained from data bus to R0  
		shift_in[0] = 0x1C001C00;					 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//Step 6 - Set SYSTEM speed flag 
 
		//NOP  
		shift_in[0] = 0x1C001C00;						//Instruct 9 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		//NOP  
		shift_in[0] = 0x1C001C00;						//Instruct 10 
		shift_in[1] = ARM7TDMI_SYSTEM_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
 
		/* 
		 * After move the new value to register PC, 10 instructions have been 
		 * executed so far. To exit from DEBUG state and resume the normal  
		 * operation of the program under debug, 10 instructions backwards. 
		 */ 
 
		//B -10 = 0xE7F6E7F6 
		shift_in[0] = 0xE7F6E7F6;						//Final branch 
		shift_in[1] = ARM7TDMI_DEBUG_SPEED; 
		arm7tdmi_acs_sc1(shift_in, shift_out); 
	} 
	 
	/* 
	 * Last Step - Use RESTART to Exit DEBUG state and  
	 *			   return back to SYSTEM state 
	 */ 
	status = tapctrl_acs_ireg(JTAG_BYPASS); 
	status = tapctrl_acs_ireg(JTAG_RESTART); 
	status = tapctrl_runtest(); 
 
	//Update the ARM7TDMI status 
	arm7tdmi_status.state = ARM7TDMI_SYSTEM_STATE; 
	arm7tdmi_status.by = -1; 
	arm7tdmi_status.scanchain = -1;	 //For safe purpose 
	 
	return XJ_OK; 
} 
 
 
/*  
 * arm7tdmi_set_swbreakpt() - 
 *		Used to set a software break point at a specific address. 
 * 
 *		ARM7TDMI supports two watchpoint units. Normally, WP0 for break point  
 *		and WP1 for watch point. If WP0 is used for hardware break point, then  
 *		no software break point can be supported. If WP0 is used for software  
 *		break point, it can support any number of software breakpoint. 
 * 
 *		@addr: address of software break point. 
 */ 
static int arm7tdmi_set_swbreakpt(u32 addr) 
{ 
	int status; 
	u32 temp; 
	u32 instruct; 
	u32 bit_pattern = 0xDEEEDEEE; 
	arm7tdmi_breakpt_list_t *breakpt_new; 
 
 
	//In DEBUG state? 
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE) 
		return XJERR_TARGET_RUNNING; 
	 
	status = arm7tdmi_connect_scanchain(2); 
	if(status != XJ_OK) 
		return status; 
 
	/* 
	 * Check the possibility. 
	 * If watchpt-0 is already used as a hardware break point, 
	 * return ERROR. 
	 */ 
	if( (arm7tdmi_breakpt_head != NULL) &&  
		arm7tdmi_breakpt_head->type == HARDWARE_BREAKPT) 
		return XJERR_SET_BREAKPT_FAIL; 
 
 
	//First disable WP0  
	arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLVAL, 0x0); 
 
	/* 
	 * Try to set it as software break point by replacing the instruction 
	 * at addr by a specific bit pattern 
	 */ 
	status = arm7tdmi_connect_scanchain(1); 
 
	arm7tdmi_mem_rd32(addr, &instruct, 1);		 
	arm7tdmi_mem_wri32(addr, &bit_pattern, 1); 
	arm7tdmi_mem_rd32(addr, &temp, 1);	 
 
	status = arm7tdmi_connect_scanchain(2); 
 
	//Fail, the target instruction can't be replaced 
	if(temp != bit_pattern){		 
		arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLMSK, 0x0F7); 
		arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLVAL, 0x100); 
		return XJERR_SET_BREAKPT_FAIL; 
	} 
 
	//Succesful 
	breakpt_new = (arm7tdmi_breakpt_list_t*)malloc(sizeof(arm7tdmi_breakpt_list_t)); 
	if(breakpt_new == NULL){ 
		arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLMSK, 0x0F7); 
		arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLVAL, 0x100); 
		return XJ_ERROR; 
	} 
 
	breakpt_new->address = addr; 
	breakpt_new->instruct = instruct; 
	breakpt_new->type = SOFTWARE_BREAKPT; 
	breakpt_new->next = NULL; 
	 
	breakpt_new->next = arm7tdmi_breakpt_head; 
	arm7tdmi_breakpt_head = breakpt_new; 
 
	//Configure WP0 
	arm7tdmi_ice_write(ARM7TDMI_WP0_ADDRMSK, 0xFFFFFFFF); 
	arm7tdmi_ice_write(ARM7TDMI_WP0_DATAVAL, bit_pattern); 
	arm7tdmi_ice_write(ARM7TDMI_WP0_DATAMSK, 0x00000000); 
	arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLMSK, 0x0F7); 
	arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLVAL, 0x102);		//nOPC = 0, Enable WP0 b4 exit 
	 
	return XJ_OK; 
}  
 
 
/*  
 * arm7tdmi_set_hwbreakpt() - 
 *		Used to set a hardware break point at a specific address. 
 * 
 *		ARM7TDMI supports two watchpoint units. Normally, WP0 for break point  
 *		and WP1 for watch point. If WP0 is used for hardware break point, then  
 *		no software break point can be supported. If WP0 is used for software  
 *		break point, it can support any number of software breakpoint. 
 * 
 *		@addr: address of hardware break point. 
 */ 
static int arm7tdmi_set_hwbreakpt(u32 addr) 
{ 
	int status;	 
	arm7tdmi_breakpt_list_t *breakpt_new; 
 
	//In DEBUG state? 
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE) 
		return XJERR_TARGET_RUNNING; 
	 
	status = arm7tdmi_connect_scanchain(2); 
	if(status != XJ_OK) 
		return status; 
 
	/* 
	 * Check the possibility. 
	 * If watchpt-0 is already used as a hardware break point, 
	 * return ERROR. 
	 */ 
	if(arm7tdmi_breakpt_head != NULL) 
		return XJERR_SET_BREAKPT_FAIL; 
 
	//Add this hardware break point to the list of break point. 
	breakpt_new = (arm7tdmi_breakpt_list_t*)malloc(sizeof(arm7tdmi_breakpt_list_t)); 
	if(breakpt_new == NULL) 
		return XJ_ERROR; 
 
	breakpt_new->address = addr; 
	breakpt_new->instruct = 0; 
	breakpt_new->type = HARDWARE_BREAKPT; 
	breakpt_new->next = NULL; 
 
	arm7tdmi_breakpt_head = breakpt_new; 
 
 
	//Configure WP0 
	arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLVAL, 0x0);		//Disable WP0 during the setting 
	arm7tdmi_ice_write(ARM7TDMI_WP0_ADDRVAL, addr); 
	arm7tdmi_ice_write(ARM7TDMI_WP0_ADDRMSK, 0x0); 
	arm7tdmi_ice_write(ARM7TDMI_WP0_DATAMSK, 0xFFFFFFFF); 
	arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLMSK, 0x0F7); 
	arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLVAL, 0x100);	//nOPC = 0, Enable WP0 b4 exit 
	 
	return XJ_OK; 
} 
 
 
/*  
 * arm7tdmi_set_breakpt() - 
 *		Used to set a break point at a specific address. 
 *		We prefer software break point. Everytime, we will try to 
 *		set it as a software break point first. If fail, then try 
 *		to set it as a hardware break point.		 
 * 
 *		@addr: address of break point. 
 */ 
int arm7tdmi_set_breakpt(u32 addr) 
{ 
	int status; 
	 
	status = arm7tdmi_set_swbreakpt(addr); 
 
	if(status == XJ_OK) 
		return XJ_OK; 
	else 
		status = arm7tdmi_set_hwbreakpt(addr); 
 
	return status; 
} 
 
 
/* 
 * arm7tdmi_clr_breakpt() - 
 *		Used to clear a break point located at a specific address 
 * 
 *		@addr: address of the break point to be cleared. 
 */ 
int arm7tdmi_clr_breakpt(u32 addr) 
{ 
	int status; 
	int type; 
	u32 instruct; 
	arm7tdmi_breakpt_list_t *breakpt_temp; 
	arm7tdmi_breakpt_list_t *breakpt_prev; 
 
	//In DEBUG state? 
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE) 
		return XJERR_TARGET_RUNNING; 
	 
	status = arm7tdmi_connect_scanchain(2); 
	if(status != XJ_OK) 
		return status; 
 
	//Find the target break point first 
	breakpt_prev = NULL; 
	for(breakpt_temp = arm7tdmi_breakpt_head; breakpt_temp != NULL; breakpt_temp = breakpt_temp->next){ 
		if(breakpt_temp->address == addr){ 
			if(breakpt_prev == NULL){ 
				arm7tdmi_breakpt_head = breakpt_temp->next; 
			}else{ 
				breakpt_prev->next = breakpt_temp->next; 
			} 
			break; 
		} 
		breakpt_prev = breakpt_temp; 
	} 
 
	//Found?? 
	if(breakpt_temp == NULL) 
		return XJERR_CLR_BREAKPT_FAIL; 
 
	type = breakpt_temp->type; 
	instruct = breakpt_temp->instruct; 
	free(breakpt_temp);								//Delete breakpt_temp 
 
	if(type == SOFTWARE_BREAKPT){					//Software break point 
		status = arm7tdmi_connect_scanchain(1); 
		arm7tdmi_mem_wri32(addr, &instruct, 1); 
	}else{											//Hardware break point 
		status = arm7tdmi_connect_scanchain(2); 
		arm7tdmi_ice_write(ARM7TDMI_WP0_CTRLVAL, 0x0); 
	} 
 
	return XJ_OK; 
}  
 
 
/*  
 * arm7tdmi_set_watchpt() - 
 *		Used to set a watch point at a specific address. 
 * 
 *		ARM7TDMI supports two watchpoint units. Normally, WP0 for break point  
 *		and WP1 for watch point.  
 * 
 *		@addr: address of watch point to be set. 
 */ 
int arm7tdmi_set_watchpt(u32 addr) 
{ 
	int status; 
 
	//Is WP1 available? 
	if(arm7tdmi_watchpt_status.busy == 1) 
		return XJERR_SET_WATCHPT_FAIL; 
 
	//In DEBUG state? 
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE) 
		return XJERR_TARGET_RUNNING; 
	 
	status = arm7tdmi_connect_scanchain(2); 
	if(status != XJ_OK) 
		return status; 
 
	//Configure WP1 
	arm7tdmi_ice_write(ARM7TDMI_WP1_CTRLVAL, 0x0);		//Disable WP1 during the setting 
	arm7tdmi_ice_write(ARM7TDMI_WP1_ADDRVAL, addr); 
	arm7tdmi_ice_write(ARM7TDMI_WP1_ADDRMSK, 0x0); 
	arm7tdmi_ice_write(ARM7TDMI_WP1_DATAMSK, 0xFFFFFFFF); 
	arm7tdmi_ice_write(ARM7TDMI_WP1_CTRLMSK, 0x0F7); 
	arm7tdmi_ice_write(ARM7TDMI_WP1_CTRLVAL, 0x108);	//nOPC = 1, Enable WP1 b4 exit 
 
	//Update the status of WP1 
	arm7tdmi_watchpt_status.busy = 1; 
	arm7tdmi_watchpt_status.addr = addr; 
	 
	return XJ_OK; 
} 
 
 
/*  
 * arm7tdmi_clr_watchpt() - 
 *		Used to clear a watch point at a specific address. 
 * 
 *		ARM7TDMI supports two watchpoint units. Normally, WP0 for break point  
 *		and WP1 for watch point.  
 * 
 *		@addr: address of watch point to be cleared. 
 */ 
int arm7tdmi_clr_watchpt(u32 addr) 
{ 
	int status; 
 
	//Match? 
	if( (arm7tdmi_watchpt_status.busy == 0) || (arm7tdmi_watchpt_status.addr != addr) ) 
		return XJERR_SET_WATCHPT_FAIL; 
 
	//In DEBUG state? 
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE) 
		return XJERR_TARGET_RUNNING; 
	 
	status = arm7tdmi_connect_scanchain(2); 
	if(status != XJ_OK) 
		return status; 
 
	//Disable WP1 
	arm7tdmi_ice_write(ARM7TDMI_WP1_CTRLVAL, 0x0);		 
 
	//Update the status of WP1 
	arm7tdmi_watchpt_status.busy = 0; 
	arm7tdmi_watchpt_status.addr = 0;	 
 
	return XJ_OK; 
}