/*
 * target/arm7tdmi/arm7tdmi: implements arm7tdmi target
 *
 * Copyright (C) 2003 2004, Rongkai zhan <zhanrk@163.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/* $Id$ */

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>

#include "jtager.h"
#include "jtag.h"
#include "target.h"

extern core_register_t arm7tdmi_regs[];
extern ice_register_t  arm7tdmi_ice_regs[];

target_t arm7tdmi_target = {
	.type		= TARGET_TYPE_ARM7TDMI,
	.mode		= TARGET_MODE_ARM,
	.status 	= TARGET_STATUS_RUNNING,
	.halt_reason 	= TARGET_HALTED_NONE,
	
	/*
	 * These two pointers are initialized in the function arm7tdmi_setup()
	 */
	.regs = NULL,
	.ice_regs = NULL,

	/* BYPASS register always output 0 */
	.bypass 	= {ARM7TDMI_BYPASS_REG_BITNR, 0, 0},
	.idcode 	= {ARM7TDMI_IDCODE_REG_BITNR, 0, 0},
	/* INSTRUCTION register always output b0001 during CAPTURE-DR stage */
	.instruction 	= {ARM7TDMI_INSTRUCTION_REG_BITNR, 0xffffffff, 0x01},
	/*
	 * The scan path select register always
	 * output b1000 during CAPTURE-DR stage.
	 */
	.scanpath = {ARM7TDMI_SCANPATH_REG_BITNR, 0x0, 0x08}, 
	
	/* scan chains of ARM7TDMI */
	.sc_num 	= 3, /* scan chain 0, 1, 2 */
	.active_sc 	= 0,
	.sc = {
		{"ARM7TDMI CPU core logic",
		 ARM7TDMI_SCANCHAIN0_BITNR, {0, }, {0, }, NULL},
		{"ARM7TDMI CPU core debug subset",
		 ARM7TDMI_SCANCHAIN1_BITNR, {0, }, {0, }, NULL},
		{"ARM7TDMI EmbeddedICE-RT logic",
		 ARM7TDMI_SCANCHAIN2_BITNR, {0, }, {0, }, NULL}
	},
	.private = NULL,
};

/*
 * arm7tdmi_halt - Halt the ARM7TDMI target and make it into the debug mode
 */
int arm7tdmi_halt(void)
{
	u32 status;
	int success, retval;
	int temp = 0;

	if (target->status != TARGET_STATUS_RUNNING) {
		printf("The target cpu has been halted!\n");
		return 0;
	}
	
	/*
	 * We halt the target by setting the DBGRQ bit (bit[1]) of the debug
	 * control register of ARM7TDMI EmbeddedICE-RT logic. We also set the
	 * INTDIS bit (bit[2]) of the ICE debug control register for disabling
	 * the interrupt on the target.
	 *
	 * NOTICE: Do not set DBGACK bit, please. The reason is:
	 * When a system-speed access from debug state occurs, the core will
	 * temporarily drop out of debug state, so DBGACK might go LOW.
	 * But if set the DBGACK bit of the debug control registerl,
	 * the DBGACK signal will always go HIGH.
	 */
	/* select scan chain 2 -- EmbeddedICE-RT */
	retval = jtag_select_scanchain(2);
	if (retval)
		return retval;
	retval = jtag_write_ireg(JTAG_INTEST); /* internal test mode */
	if (retval)
		return retval;
	
	/* set DBGRQ bit and disable interrupts */
	retval = arm7tdmi_ice_write(ARM7TDMI_ICE_DBGCTL, 0x6);
	if (retval)
		return retval;

	/* After sending DBGRQ, Run-Test/Idle state must be entered: */
	retval = jtag_write_ireg(JTAG_RESTART);
	if (retval)
		return retval;
	
	/* Read debug status register to see whether the DBGACK signal
	 * is asserted. If the DBGACK siganl goes HIGH, then the target
	 * has entered the debug state.
	 */
	success = 0;
	printf("Requesting HALT target ... ");
	while (temp++ < 10) {
		retval = arm7tdmi_ice_read(ARM7TDMI_ICE_DBGSTAT, &status);
		if (retval)
			return retval;
		
		if (status & 0x01) {
			/* success */
			success = 1;
			break;
		}
		//jtag_write_ireg(JTAG_RESTART);
		printf(".");
		usleep(100);
	}

	/* Whatever happens, the DBGRQ bit flag must be cleared */
	arm7tdmi_ice_write(ARM7TDMI_ICE_DBGCTL, 0x00);
	
	if (success) {
		printf("[OK]\n");
		target->status = TARGET_STATUS_HALTED;
		target->halt_reason = TARGET_HALTED_BY_DBGRQ;
		
		/*
		 * When the bit[4] of the debug status register of ARM7TDMI
		 * EmbeddedICE-RT logic is HIGH, the ARM7TDMI core has
		 * entered the debug state from THUMB mode. Otherwise the
		 * ARM7TDMI core has entered the debug state from ARM mode.
		 */
		printf("The target is halted in ");
		if (status & 0x10) {
			printf("THUMB mode.\n");
			target->mode = TARGET_MODE_THUMB;
		} else {
			printf("ARM mode.\n");
			target->mode = TARGET_MODE_ARM;
		}
		retval = 0;
		//retval = arm7tdmi_regs_read();
	} else {
		/* make the TAP controller into the Run-Test/Idle state */
		jtag_write_ireg(JTAG_RESTART);
		
		printf("[FAILED]\n");	
		retval = -ERR_TARGET_HALT_FAILED;
	}

	return retval;
} /* end of target_halt() */

/*
 * arm7tdmi_exec_instruction - Execute an instruction on the ARM7TDMI core
 * 	 		       while in debug state by scan chain 1
 * @writein: an array with 2 elements, writein[0] contains the instruction
 * 	     opcode to be executed, (writein[1] & 0x01) represents the
 * 	     value of BREAKPT signal.
 * @readout: used to return the data read out from scan chain 1
 * 
 * Return Value: the value of BREAKPT signal.
 *
 * NOTE: We assume that the target has been halted - the target has
 * been in debug state. And scan chain 1 has been selected, the INTEST
 * instruction has been written into the instruction register.
 */
int arm7tdmi_exec_instruction(u32 *writein, u32 *readout)
{
	u32 indata[2] = {0, 0};
	u32 outdata[2] = {0, 0};
	int bitnr = arm7tdmi_target.sc[1].bitnr; /* 33 bits */
	int retval;
	
	/* reverse the bit order of the data to be written in */
	reverse_bit_order(bitnr, (u8 *)writein, (u8 *)indata);
	
	/* 
	 * OK, write in and read out
	 * read or write JTAG data register will always change its state
	 * from Update-DR to Run-Test/Idle. Therefore, it will also pulse
	 * DCLK signal.
	 */
	retval = jtag_rw_dreg(bitnr, indata, outdata);
	if (retval)
		return retval;

	/* reverse the bit order of the data read out */
	reverse_bit_order(bitnr, (u8 *)outdata, (u8 *)readout);

	return retval;
} /* end of arm7tdmi_exec_instruction() */

/*
 * target_restart - Exit from the debug state, and return to
 * 		    the normal system state.
 */
int arm7tdmi_restart(void)
{
	core_register_t *reg_pc = reg_index("PC");
	core_register_t *reg_r0 = reg_index("R0");

	if (target->status == TARGET_STATUS_RUNNING)
		return -ERR_TARGET_IS_RUNNING;

	if (target->mode == TARGET_MODE_THUMB) {
		printf("Error: %s: THUMB mode is not yet implemented.\n",
			__FUNCTION__);
		return -1;
	}

	/* Cancel the DBGRQ and enable interrupts */
	jtag_select_scanchain(2);
	arm7tdmi_ice_write(0x00, 0x00);
	jtag_write_ireg(JTAG_RESTART);

	/* Select scan chain 1 and use INTEST instruction */
	jtag_select_scanchain(1);
	jtag_write_ireg(JTAG_INTEST);

	/*
	 * First, we recover the internal state of ARM7TDMI core.
	 * Use the following instructions sequence to recover R0 and PC:
	 * LDR R0, [R0] ; scan the value of PC into R0
	 * MOV PC, R0  ; update PC of the core
	 * LDR R0, [R0] ; scan in the value of R0 register
	 */

	/* LDR R0, [R0] = 0xE5900000       - restore PC */
	target->sc[1].writein[0] = 0xE5900000;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* NOP */
	target->sc[1].writein[0] = 0xE1A00000;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* NOP 
	 * 1st instruction is executed while fetching 3rd instruction
	 */
	target->sc[1].writein[0] = 0xE1A00000;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* NOP                           -  scan in the value of PC */
	target->sc[1].writein[0] = reg_pc->value;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* MOV PC, R0 = 0xE1A0F000       - put R0 into PC */
	target->sc[1].writein[0] = 0xE1A0F000;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* NOP */
	target->sc[1].writein[0] = 0xE1A00000;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* NOP 
	 * 1st instruction "MOV PC, R0" is executed,
	 * while fetching 3rd instruction
	 */
	target->sc[1].writein[0] = 0xE1A00000;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);
	
	/* LDR R0, [R0] = 0xE5900000            - restore R0 */
	target->sc[1].writein[0] = 0xE5900000;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* NOP */
	target->sc[1].writein[0] = 0xE1A00000;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* NOP */
	target->sc[1].writein[0] = 0xE1A00000;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* scan in the value of R0 register */
	target->sc[1].writein[0] = reg_r0->value;
	target->sc[1].writein[1] = DEBUG_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* 
	 * The penultimate instruction must be scanned in with bit33 set HIGH.
	 * insert NOP instruction: MOV R0, R0 = 0xE1A00000
	 */
	target->sc[1].writein[0] = 0xE1A00000;
	target->sc[1].writein[1] = SYSTEM_SPEED;
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/*
	 * The final instruction is the branch instruction and it is
	 * scanned in with bit 33 set LOW. For small branches, you also
	 * can replace the final branch instruction with a substract
	 * instruction, with the PC as the destination operand.
	 * B -6 = 0xEAFFFFFA
	 */
	if (target->halt_reason == TARGET_HALTED_BY_BREAKPT)
		target->sc[1].writein[0] = 0xEAFFFFFB; /* opcode of "B -7" */
	else
		target->sc[1].writein[0] = 0xEAFFFFFA; /* opcode of "B -6" */
	target->sc[1].writein[1] = DEBUG_SPEED;	
	arm7tdmi_exec_instruction(target->sc[1].writein, target->sc[1].readout);

	/* Write RESTART instruction into the TAP controller.
	 * When the state machine enters the Run-Test/Idle state,
	 * the ARM7TDMI core will revert back to system mode,
	 * and it will resynchronize clock to MCLK.
	 */
	jtag_write_ireg(JTAG_RESTART);

	return 0;
} /* end of arm7tdmi_restart() */

struct target_operation arm7tdmi_tops = {
	/* ARM7TDMI EmbeddedICE-RT logic register read/write operations */
	.ice_read 	= arm7tdmi_ice_read,
	.ice_write 	= arm7tdmi_ice_write,

	/* ARM7TDMI cpu core operations */
	.halt		= arm7tdmi_halt,
	.restart	= arm7tdmi_restart,

	/* ARM7TDMI core registers read/write operations */
	.get_core_state = arm7tdmi_get_core_state,
	.register_read 	= arm7tdmi_register_read,
	.register_write	= arm7tdmi_register_write,

	/* ARM7TDMI target memory read/write operations */
	.mem_read16 	= arm7tdmi_memory_read16,
	.mem_write16 	= arm7tdmi_memory_write16,
	.mem_read32 	= arm7tdmi_memory_read32,
	.mem_write32 	= arm7tdmi_memory_write32,
};

int arm7tdmi_setup(void)
{
	arm7tdmi_target.regs = arm7tdmi_regs;
	arm7tdmi_target.ice_regs = arm7tdmi_ice_regs;

	target = &arm7tdmi_target;
	t_op = &arm7tdmi_tops;
	return 0;
}