/*
 * arm7tdmi.c:	implement the core routes for ARM7TDMI
 *
 * Copyright (C) 2004, OPEN-JTAG, All rights reserved.
 */

#include <windows.h>
#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.