/*
 * system.c:	implement the routes used to access memory under
 *				DEBUG state.
 *
 * Copyright (C) 2004, OPEN-JTAG, All rights reserved.
 */

#include <windows.h>
#include "../types.h"
#include "../xjerr.h"
#include "../tapctrl.h"
#include "arm7tdmi.h"

/*
 * To meet the dynamic timing requirements of the memory system, any attempt to 
 * access system state must occur synchronously to it. The ARM7TDMI core must 
 * be forced to synchronize back to MCLK. This is controlled by bit 33 (BREAKPT)
 * of scan chain 1. 
 *
 * After a system-speed instruction has been scanned into the data bus and 
 * clocked into the pipeline, the RESTART instruction must be loaded into the TAP
 * controller. This causes the ARM7TDMI core to behave as follows:
 *	1. The ARM7TDMI core automatically synchronizes back to MCLK;
 *	2. It executes the instruction at system speed;
 *	3. It re-enters debug state.
 */




/*
 * arm7tdmi_mem_rd8() - 
 *		used to read memory at 8-bit width
 *
 *		@addr: address of memory to be read from
 *		@buf:  buf used to store the content read from memory
 *		@len:  lenngth in terms of 8-bit byte to be read
 */
int arm7tdmi_mem_rd8(u32 addr, u8 *buf, int len)
{
	int status;
	int counter;
	u32 r0, r1;
	u32 shift_in[2];
	u32 shift_out[2];

	if(buf == NULL)
		return XJERR_INVALID_PARAMATER;

	//In DEBUG state?
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE)
		return XJERR_TARGET_RUNNING;
	
	//Select scan chain 1
	status = arm7tdmi_connect_scanchain(1);
	if(status != XJ_OK)
		return status;

	//Backup R0 & R1 first
	arm7tdmi_core_rd_reg(ARM7TDMI_R0, &r0);
	arm7tdmi_core_rd_reg(ARM7TDMI_R1, &r1);


	//Read memory ...
	for(counter = 0; counter < len; counter++){
		//put the address to be read to RO
		arm7tdmi_core_wri_reg(ARM7TDMI_R0, addr);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		/* 
		 * Set bit 33 of scan chain 1, next instruct 
		 * will be executed at system speed.
		 */
		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_SYSTEM_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//LDRB R1, [R0] = 0xE5D01000
		shift_in[0] = 0xE5D01000;				
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		tapctrl_acs_ireg(JTAG_BYPASS);
		tapctrl_acs_ireg(JTAG_RESTART);
		tapctrl_acs_ireg(JTAG_INTEST);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//STRB R1, [R1] = 0xE5C11000
		shift_in[0] = 0xE5C11000;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//NOP x 3
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);
		arm7tdmi_acs_sc1(shift_in, shift_out);
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//Update buffer and address
		*buf = (u8)shift_out[0];
		buf += 1;
		addr += 1;

	}

	//Restore R0 & R1 before exit
	arm7tdmi_core_wri_reg(ARM7TDMI_R0, r0);
	arm7tdmi_core_wri_reg(ARM7TDMI_R1, r1);

	return XJ_OK;
}


/*
 * arm7tdmi_mem_wri8() - 
 *		used to wri memory at 8-bit width
 *
 *		@addr: destination address of memory to be written
 *		@buf:  point to the content to be written
 *		@len:  lenngth in terms of 8-bit byte to be written
 */
int arm7tdmi_mem_wri8(u32 addr, const u8 *buf, int len)
{
	int status;
	int counter;
	u32 r0, r1;
	u32 shift_in[2];
	u32 shift_out[2];
	
	if(buf == NULL)
		return XJERR_INVALID_PARAMATER;

	//In DEBUG state?
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE)
		return XJERR_TARGET_RUNNING;
	
	//Select scan chain 1
	status = arm7tdmi_connect_scanchain(1);
	if(status != XJ_OK)
		return status;

	//Backup R0 & R1 first
	arm7tdmi_core_rd_reg(ARM7TDMI_R0, &r0);
	arm7tdmi_core_rd_reg(ARM7TDMI_R1, &r1);


	//Write memory ...
	for(counter = 0; counter < len; counter++){
		//put the address into R0
		arm7tdmi_core_wri_reg(ARM7TDMI_R0, addr);

		//put the new value into R1
		arm7tdmi_core_wri_reg(ARM7TDMI_R1, *buf);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		/* 
		 * Set bit 33 of scan chain 1, next instruct 
		 * will be executed at system speed.
		 */
		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_SYSTEM_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//STRB R1, [R0] = 0xE5C01000
		shift_in[0] = 0xE5C01000;				
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		tapctrl_acs_ireg(JTAG_BYPASS);
		tapctrl_acs_ireg(JTAG_RESTART);
		tapctrl_acs_ireg(JTAG_INTEST);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);
		
		//Update address and buffer
		addr += 1;
		buf += 1;
	}


	//Restore R0 & R1 before exit
	arm7tdmi_core_wri_reg(ARM7TDMI_R0, r0);
	arm7tdmi_core_wri_reg(ARM7TDMI_R1, r1);

	return XJ_OK;
}


/*
 * arm7tdmi_mem_rd16() - 
 *		used to read memory at 16-bit width
 *
 *		@addr: address of memory to be read from
 *		@buf:  buf used to store the content read from memory
 *		@len:  lenngth in terms of 16-bit byte to be read
 */
int arm7tdmi_mem_rd16(u32 addr, u16 *buf, int len)
{
	int status;
	int counter;
	u32 r0, r1;
	u32 shift_in[2];
	u32 shift_out[2];

	if(buf == NULL)
		return XJERR_INVALID_PARAMATER;

	//In DEBUG state?
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE)
		return XJERR_TARGET_RUNNING;
	
	//Select scan chain 1
	status = arm7tdmi_connect_scanchain(1);
	if(status != XJ_OK)
		return status;

	/* 
	 * Shape the address to make sure that the least significant 
	 * 1 bit of addr is zero.
	 */
	addr &= 0xFFFFFFFE;

	//Backup R0 & R1 first
	arm7tdmi_core_rd_reg(ARM7TDMI_R0, &r0);
	arm7tdmi_core_rd_reg(ARM7TDMI_R1, &r1);

	//Read memory ...
	for(counter = 0; counter < len; counter++){
		//put the address to be read to RO
		arm7tdmi_core_wri_reg(ARM7TDMI_R0, addr);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		/* 
		 * Set bit 33 of scan chain 1, next instruct 
		 * will be executed at system speed.
		 */
		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_SYSTEM_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//LDRH R1, [R0] = 0xE1D010B0
		shift_in[0] = 0xE1D010B0;				
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		tapctrl_acs_ireg(JTAG_BYPASS);
		tapctrl_acs_ireg(JTAG_RESTART);
		tapctrl_acs_ireg(JTAG_INTEST);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//STRH R1, [R1] = 0xE1C110B0
		shift_in[0] = 0xE1C110B0;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//NOP x 3
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);
		arm7tdmi_acs_sc1(shift_in, shift_out);
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//Update buffer and address
		*buf = (u16)shift_out[0];
		buf += 1;
		addr += 2;
	}

	//Restore R0 & R1 before exit
	arm7tdmi_core_wri_reg(ARM7TDMI_R0, r0);
	arm7tdmi_core_wri_reg(ARM7TDMI_R1, r1);

	return XJ_OK;
}


/*
 * arm7tdmi_mem_wri16() - 
 *		used to wri memory at 16-bit width
 *
 *		@addr: destination address of memory to be written
 *		@buf:  point to the content to be written
 *		@len:  lenngth in terms of 16-bit byte to be written
 */
int arm7tdmi_mem_wri16(u32 addr, const u16 *buf, int len)
{
	int status;
	int counter;
	u32 r0, r1;
	u32 shift_in[2];
	u32 shift_out[2];

	if(buf == NULL)
		return XJERR_INVALID_PARAMATER;

	//In DEBUG state?
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE)
		return XJERR_TARGET_RUNNING;
	
	//Select scan chain 1
	status = arm7tdmi_connect_scanchain(1);
	if(status != XJ_OK)
		return status;

	/* 
	 * Shape the address to make sure that the least significant 
	 * 1 bit of addr is zero.
	 */
	addr &= 0xFFFFFFFE;

	//Backup R0 & R1 first
	arm7tdmi_core_rd_reg(ARM7TDMI_R0, &r0);
	arm7tdmi_core_rd_reg(ARM7TDMI_R1, &r1);


	//Write memory ...
	for(counter = 0; counter < len; counter++){
		//put the address into R0
		arm7tdmi_core_wri_reg(ARM7TDMI_R0, addr);

		//put the new value into R1
		arm7tdmi_core_wri_reg(ARM7TDMI_R1, *buf);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		/* 
		 * Set bit 33 of scan chain 1, next instruct 
		 * will be executed at system speed.
		 */
		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_SYSTEM_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//STRH R1, [R0] = 0xE1C010B0
		shift_in[0] = 0xE1C010B0;				
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		tapctrl_acs_ireg(JTAG_BYPASS);
		tapctrl_acs_ireg(JTAG_RESTART);
		tapctrl_acs_ireg(JTAG_INTEST);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);
		
		//Update address and buffer
		addr += 2;
		buf += 1;
	}


	//Restore R0 & R1 before exit
	arm7tdmi_core_wri_reg(ARM7TDMI_R0, r0);
	arm7tdmi_core_wri_reg(ARM7TDMI_R1, r1);

	return XJ_OK;
}


/*
 * arm7tdmi_mem_rd32() - 
 *		used to read memory at 32-bit width
 *
 *		@addr: address of memory to be read from
 *		@buf:  buf used to store the content read from memory
 *		@len:  lenngth in terms of 32-bit byte to be read
 */
int arm7tdmi_mem_rd32(u32 addr, u32 *buf, int len)
{
	int status;
	int counter;
	u32 r0, r1;
	u32 shift_in[2];
	u32 shift_out[2];

	if(buf == NULL)
		return XJERR_INVALID_PARAMATER;

	//In DEBUG state?
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE)
		return XJERR_TARGET_RUNNING;
	
	//Select scan chain 1
	status = arm7tdmi_connect_scanchain(1);
	if(status != XJ_OK)
		return status;

	/* 
	 * Shape the address to make sure that the least significant 
	 * 2 bits of addr are zero.
	 */
	addr &= 0xFFFFFFFC;

	//Backup R0 & R1 first
	arm7tdmi_core_rd_reg(ARM7TDMI_R0, &r0);
	arm7tdmi_core_rd_reg(ARM7TDMI_R1, &r1);

	//Read memory ...
	for(counter = 0; counter < len; counter++){
		//put the address to be read to RO
		arm7tdmi_core_wri_reg(ARM7TDMI_R0, addr);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		/* 
		 * Set bit 33 of scan chain 1, next instruct 
		 * will be executed at system speed.
		 */
		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_SYSTEM_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//LDR R1, [R0] = 0xE5901000
		shift_in[0] = 0xE5901000;				
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		tapctrl_acs_ireg(JTAG_BYPASS);
		tapctrl_acs_ireg(JTAG_RESTART);
		tapctrl_acs_ireg(JTAG_INTEST);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//STR R1, [R1] = 0xE5811000
		shift_in[0] = 0xE5811000;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//NOP x 3
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);
		arm7tdmi_acs_sc1(shift_in, shift_out);
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//Update buffer and address
		*buf = shift_out[0];
		buf += 1;
		addr += 4;
	}

	//Restore R0 & R1 before exit
	arm7tdmi_core_wri_reg(ARM7TDMI_R0, r0);
	arm7tdmi_core_wri_reg(ARM7TDMI_R1, r1);

	return XJ_OK;
}


/*
 * arm7tdmi_mem_wri32() - 
 *		used to wri memory at 32-bit width
 *
 *		@addr: destination address of memory to be written
 *		@buf:  point to the content to be written
 *		@len:  lenngth in terms of 32-bit byte to be written
 */
int arm7tdmi_mem_wri32(u32 addr, const u32 *buf, int len)
{
	int status;
	int counter;
	u32 r0, r1;
	u32 shift_in[2];
	u32 shift_out[2];

	if(buf == NULL)
		return XJERR_INVALID_PARAMATER;

	//In DEBUG state?
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE)
		return XJERR_TARGET_RUNNING;
	
	//Select scan chain 1
	status = arm7tdmi_connect_scanchain(1);
	if(status != XJ_OK)
		return status;

	/* 
	 * Shape the address to make sure that the least significant 
	 * 2 bits of addr are zero.
	 */
	addr &= 0xFFFFFFFC;

	//Backup R0 & R1 first
	arm7tdmi_core_rd_reg(ARM7TDMI_R0, &r0);
	arm7tdmi_core_rd_reg(ARM7TDMI_R1, &r1);


	//Write memory ...
	for(counter = 0; counter < len; counter++){
		//put the address into R0
		arm7tdmi_core_wri_reg(ARM7TDMI_R0, addr);

		//put the new value into R1
		arm7tdmi_core_wri_reg(ARM7TDMI_R1, *buf);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		/* 
		 * Set bit 33 of scan chain 1, next instruct 
		 * will be executed at system speed.
		 */
		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_SYSTEM_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		//STR R1, [R0] = 0xE5801000
		shift_in[0] = 0xE5801000;				
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);

		tapctrl_acs_ireg(JTAG_BYPASS);
		tapctrl_acs_ireg(JTAG_RESTART);
		tapctrl_acs_ireg(JTAG_INTEST);

		//NOP
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);
		
		//Update address and buffer
		addr += 4;
		buf += 1;
	}


	//Restore R0 & R1 before exit
	arm7tdmi_core_wri_reg(ARM7TDMI_R0, r0);
	arm7tdmi_core_wri_reg(ARM7TDMI_R1, r1);

	return XJ_OK;
}