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


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



/*
 * ARM7TDMI inlcudes 18 regisgers: R0-R14, PC, CPSR & SPSR
 * Plz note that SPSR is unavailable when the ARM7TDMI is in
 * the System/User state. 
 */
u32 arm7tdmi_register[18][2] = {
	{0x0, 0}, {0x0, 0}, {0x0, 0}, {0x0, 0}, {0x0, 0}, {0x0, 0},
	{0x0, 0}, {0x0, 0}, {0x0, 0}, {0x0, 0}, {0x0, 0}, {0x0, 0},
	{0x0, 0}, {0x0, 0}, {0x0, 0}
};



static int arm7tdmi_core_rd_r0(u32 *r0)
{
	u32 shift_in[2];
	u32 shift_out[2];

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

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

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

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

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

	*r0 = shift_out[0];

	return XJ_OK;
}


static int arm7tdmi_core_wri_r0(u32 r0)
{
	u32 shift_in[2];
	u32 shift_out[2];

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

	/* 
	 * Step 1: clear R0
	 * MOV R0, #0 = 0xE3A00000
	 */
	shift_in[0] = 0xE3A00000;
	shift_in[1] = ARM7TDMI_DEBUG_SPEED;
	arm7tdmi_acs_sc1(shift_in, shift_out);

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

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


	/* 
	 * Step 2: set R0
	 * LDR R0, [R0] = 0xE5900000
	 */
	shift_in[0] = 0xE5900000;
	shift_in[1] = ARM7TDMI_DEBUG_SPEED;
	arm7tdmi_acs_sc1(shift_in, shift_out);

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

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

	shift_in[0] = r0;				//New value
	shift_in[1] = ARM7TDMI_DEBUG_SPEED;
	arm7tdmi_acs_sc1(shift_in, shift_out);

	shift_in[0] = ARM7TDMI_NOP;
	shift_in[1] = ARM7TDMI_DEBUG_SPEED;
	arm7tdmi_acs_sc1(shift_in, shift_out);
	
	return XJ_OK;
}


//Read a specific core register
int arm7tdmi_core_rd_reg(int reg_idx, u32 *cur_val)
{
	int status;
	u32 r0;
	u32 cpsr;
	u32 shift_in[2];
	u32 shift_out[2];

	if(cur_val == NULL)
		return XJERR_INVALID_PARAMATER;

	//In DEBUG state?
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE)
		return XJERR_TARGET_RUNNING;
	
	//Check the validity of the register according to the register index
	if(reg_idx < ARM7TDMI_R0 || reg_idx > ARM7TDMI_SPSR)
		return XJERR_INVALID_COREREG;

	//Select scan chain 1
	if(arm7tdmi_status.scanchain != 1){
		status = arm7tdmi_connect_scanchain(1);
		if(status != XJ_OK)
			return status;
	}

	//Anyway, read the value of R0 first.
	arm7tdmi_core_rd_r0(&r0);
	
	//If R0 is the target register, return directly
	if(reg_idx == ARM7TDMI_R0){
		*cur_val = r0;
		return XJ_OK;
	}

	//Execute all the instructions at the DEBUG speed
	shift_in[1] = ARM7TDMI_DEBUG_SPEED;

	if(reg_idx == ARM7TDMI_CPSR){			//Read CPSR
		// MRS R0, CPSR = 0xE10F0000
		shift_in[0] = 0xE10F0000;	
	}else if(reg_idx == ARM7TDMI_SPSR){		//Read SPSR
		// Make sure SPSR is available
		arm7tdmi_core_rd_reg(ARM7TDMI_CPSR, &cpsr);
		if( (cpsr & 0x1F) == 0x10 || (cpsr & 0x1F) == 0x1F )
			return XJERR_COREREG_UNAVAILABLE;

		// MRS R0, SPSR = 0xE14F0000
		shift_in[0] = 0xE14F0000;
	}else{									//Read R1-R15
		// MOV R0, Rx = 0xE1A0000x
		shift_in[0] = (0xE1A00000 | reg_idx);
	}

	//Move the value of the target register to R0
	arm7tdmi_acs_sc1(shift_in, shift_out);

	/*
	 * Next, use STR R0, [R0] to make the value of R0 apear
	 * on the data bus
	 */
	//STR R0, [R0] = 0xE5800000, fetch cycle of STR R0, [R0]
	shift_in[0] = 0xE5800000;				
	arm7tdmi_acs_sc1(shift_in, shift_out);

	//NOP, decode cycle of STR R0, [R0]
	shift_in[0] = ARM7TDMI_NOP;
	arm7tdmi_acs_sc1(shift_in, shift_out);

	//NOP, execute cycle STR R0, [R0]
	shift_in[0] = ARM7TDMI_NOP;
	arm7tdmi_acs_sc1(shift_in, shift_out);

	//NOP, now the value of R0 appears on the data bus
	shift_in[0] = ARM7TDMI_NOP;
	arm7tdmi_acs_sc1(shift_in, shift_out);

	*cur_val = shift_out[0];

	//Restore R0 before return
	arm7tdmi_core_wri_r0(r0);

	return XJ_OK;
}


//Write a specific core register
int arm7tdmi_core_wri_reg(int reg_idx, u32 new_val)
{
	int status;
	u32 r0;
	u32 cpsr;
	u32 shift_in[2];
	u32 shift_out[2];

	//In DEBUG state?
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE)
		return XJERR_TARGET_RUNNING;
	
	//Check the validity of the register according to the register index
	if(reg_idx < ARM7TDMI_R0 || reg_idx > ARM7TDMI_SPSR)
		return XJERR_INVALID_COREREG;

	//Select scan chain 1
	if(arm7tdmi_status.scanchain != 1){
		status = arm7tdmi_connect_scanchain(1);
		if(status != XJ_OK)
			return status;
	}


	if(reg_idx == ARM7TDMI_R0){			//If R0 is the target, write and return
		arm7tdmi_core_wri_r0(new_val);
		return XJ_OK;
	}else{								//Else, backup R0 and write the new value to R0
		arm7tdmi_core_rd_r0(&r0);
		arm7tdmi_core_wri_r0(new_val);
	}
	
	//Execute all the instructions at the DEBUG speed
	shift_in[1] = ARM7TDMI_DEBUG_SPEED;

	if(reg_idx == ARM7TDMI_CPSR){		//WriteCPSR
		// MSR CPSR, R0 = 0xE12FF000
		shift_in[0] = 0xE12FF000;
	}else if(reg_idx == ARM7TDMI_SPSR){	//Write SPSR
		// Make sure SPSR is available
		arm7tdmi_core_rd_reg(ARM7TDMI_CPSR, &cpsr);
		if( (cpsr & 0x1F) == 0x10 || (cpsr & 0x1F) == 0x1F )
			return XJERR_COREREG_UNAVAILABLE;

		// MSR SPSR, R0 = 0xE16FF000
		shift_in[0] = 0xE16FF000;
	}else{								//Write R1-R15
		//MOV R?, R0 = 0xE1A0?000
		shift_in[0] = 0xE1A00000;		
		shift_in[0] |= ((reg_idx - ARM7TDMI_R0)*0x1000);
	}

	arm7tdmi_acs_sc1(shift_in, shift_out);

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

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

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

	//Restore R0 before return
	arm7tdmi_core_wri_r0(r0);

	return XJ_OK;
}


//Read all the core registers
int arm7tdmi_core_rd_all(void)
{
	int index;
	int status;
	u32 cpsr;
	u32 spsr;
	u32 shift_in[2];
	u32 shift_out[2];
	
	//In DEBUG state?
	if(arm7tdmi_status.state != ARM7TDMI_DEBUG_STATE)
		return XJERR_TARGET_RUNNING;
	
	//Select scan chain 1
	if(arm7tdmi_status.scanchain != 1){
		status = arm7tdmi_connect_scanchain(1);
		if(status != XJ_OK)
			return status;
	}
	

	//1. Read R0-R15

	//STM R0, {R0-R15} = 0xE880FFFF
	shift_in[0] = 0xE880FFFF;
	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);

	for(index = 0; index <= 15; index++){
		shift_in[0] = ARM7TDMI_NOP;
		shift_in[1] = ARM7TDMI_DEBUG_SPEED;
		arm7tdmi_acs_sc1(shift_in, shift_out);
		arm7tdmi_register[index][0] = shift_out[0];
	}

	//2. Read CPSR and SPSR
	status = arm7tdmi_core_rd_reg(ARM7TDMI_CPSR, &cpsr);
	if(status == XJ_OK)
		arm7tdmi_register[ARM7TDMI_CPSR][0] = cpsr;

	status = arm7tdmi_core_rd_reg(ARM7TDMI_SPSR, &spsr);
	if(status == XJ_OK)
		arm7tdmi_register[ARM7TDMI_SPSR][0] = spsr;

	for(index = 0; index <=17; index++)
		printf("0x%0.8x\n", arm7tdmi_register[index][0]);

	return XJ_OK;
}