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