register.c

linux下的jtag仿真器程序

		spsr->value = 0;
	}
	
	/* 
	 * restore R0 register
	 */
	arm9tdmi_r0_write(arm9tdmi_target.regs[0].value);
	
	/*
	 * Finally, correct the value of PC to the real halted point.
	 * Entry into debug state by external DBGRQ will advance
	 * the PC by three addresses or 12 bytes. Reading out the
	 * value of R15 needs to execute two instructions in debug
	 * state, which will advance the PC by two addresses or 8
	 * bytes.
	 */
	if (target->halt_reason == TARGET_HALTED_BY_DBGRQ)
		target->regs[15].value -= (3 + 2) * 4;
	else
		target->regs[15].value -= (4 + 2) * 4;
	reg_pc->value = target->regs[15].value;

	return 0;
} /* end of arm9tdmi_get_core_state(...) */

/*
 * arm9tdmi_register_read - read some specific register
 */
int arm9tdmi_register_read(core_register_t *reg)
{
	scan_chain_t *sc1 = &target->sc[1];
	u32 r0_val; /* the value of R0 */
	char *tmp;
	u32 reg_num;
	int retval;

	if (!reg || !reg->name)
		return -ERR_TARGET_UNKNOWN_REG;
	else if (target->status == TARGET_STATUS_RUNNING)
		return -ERR_TARGET_IS_RUNNING;
	else if (target->mode == TARGET_MODE_THUMB)
		return -ERR_TARGET_IN_THUMB_MODE;
	else if (strcasecmp(reg->name, "SPSR") == 0) {
		/* Only if we have an valid CPSR register value, we can
		 * read/write SPSR register value.
		 */
		if (reg_index("CPSR")->value == 0)
			return -ERR_TARGET_INVALID_CPSR;
		/* no SPSR register in user mode */
		if ((reg_index("CPSR")->value & 0x1F) == 0x10)
			return -ERR_TARGET_NO_SPSR;
	}
	
	/*
	 * Select scan chain 1, and use INTEST instruction to make scan
	 * chain 1 into the internal test mode.
	 */
	retval = jtag_select_scanchain(1);
	if (retval)
		return retval;
	retval = jtag_write_ireg(JTAG_INTEST);
	if (retval)
		return retval;

	/*
	 * If the register to read is not R0, then we first
	 * save the content of R0 register.
	 */
	r0_val = arm9tdmi_r0_read();
	if (strcasecmp(reg->name, "R0") == 0) {
		/* just return directly */
		reg->value = r0_val;
		return 0;
	}
	
	/*
	 * We first copy the content of Source Register to R0.
	 * NOTE: This will clobber R0 register.
	 */
	if (strcasecmp(reg->name, "CPSR") == 0) {
		/* MRS R0, CPSR = 0xE10F0000 */
		sc1->writein[0] = 0xE10F0000;
	} else if (strcasecmp(reg->name, "SPSR") == 0) {
		/* MRS R0, SPSR = 0xE14F0000 */
		sc1->writein[0] = 0xE14F0000;
	} else if (strcasecmp(reg->name, "PC") == 0) {
		/* MOV R0, PC = 0xE1A0000F */
		sc1->writein[0] = 0xE1A0000F;
	} else { /* R1 - R15 */
		/* MOV R0, Rxx = 0xE1A0000x */
		sc1->writein[0] = 0xE1A00000;

		tmp = reg->name + 1;
		str2int(tmp, &reg_num);
		/* set the Rm bits (bit[3:0]) of the OPCODE of
		 * the "MOV R0, Rxx" instruction
		 */
		sc1->writein[0] |= (reg_num & 0x0f);
	}
	sc1->writein[1] = 0;
	arm9tdmi_exec_instruction(sc1->writein, sc1->readout, DEBUG_SPEED);

	sc1->writein[0] = 0xE5800000; /* STR R0, [R0] */
	arm9tdmi_exec_instruction(sc1->writein, sc1->readout, DEBUG_SPEED);

	sc1->writein[0] = ARM_NOP; /* NOP */
	arm9tdmi_exec_instruction(sc1->writein, sc1->readout, DEBUG_SPEED);

	/* Execute "STR R0, [R0]" -- read out R0
	 * This is 1st cycle of STR instruction.
	 * In the 2nd cycle, R0's value will be visible on the data bus.
	 */
	sc1->writein[0] = ARM_NOP; /* NOP */
	arm9tdmi_exec_instruction(sc1->writein, sc1->readout, DEBUG_SPEED);

	/* 2nd cycle of STR instruction */
	sc1->writein[0] = ARM_NOP; /* NOP */
	arm9tdmi_exec_instruction(sc1->writein, sc1->readout, DEBUG_SPEED);
	
	/*
	 * OK, we get the value we want ...
	 */
	reg->value = sc1->readout[0];

	/*
	 * Finally, restore R0 register ...
	 */
	arm9tdmi_r0_write(r0_val);
	return 0;
} /* end of arm9tdmi_register_read(...) */

/*
 * LDR R0, [R0] = 0xE5900000
 * OPCODE: cond = AL (1110), always executed.
 * 		    bit[27:26] = 01, fixed.
 * 		    I bit (bit[25]) = 0, Offset is an immediate value.
 * 		    P bit (bit[24]) = 1, Pre: add offset before transfer.
 * 		    U bit (bit[23]) = 1, Up: Add offset to base.
 * 		    B bit (bit[22]) = 0, transfer word quantity.
 * 		    W bit(bit[21]) = 0, No write-back to base.
 * 		    L bit(bit[20]) = 1, Store instruction.
 * 		    Rn (bit[19:16]) = 0000, R0 is the Base Register.
 * 		    Rd (bit[15:12]) = 0000, R0 is the Source Register.
 * 		    Offset (bit[11:0]) = 0x0.
 * 1110 0101 1001 0000 0000 0000 0000 0000 = 0xE5900000
 */
int arm9tdmi_register_write(core_register_t *reg, u32 value)
{
	scan_chain_t *sc1 = &target->sc[1];
	u32 r0_val = 0; /* the value of R0 */
	u32 data;
	char *tmp;
	u32 reg_num;
	int retval;
	

	if (!reg || !reg->name)
		return -ERR_TARGET_UNKNOWN_REG;
	else if (target->status == TARGET_STATUS_RUNNING)
		return -ERR_TARGET_IS_RUNNING;
	else if (target->mode == TARGET_MODE_THUMB)
		return -ERR_TARGET_IN_THUMB_MODE;
	else if (strcasecmp(reg->name, "SPSR") == 0) {
		/* Only if we have an valid CPSR register value, we can
		 * read/write SPSR register value.
		 */
		if (reg_index("CPSR")->value == 0)
			return -ERR_TARGET_INVALID_CPSR;
		/* no SPSR register in user mode */
		if ((reg_index("CPSR")->value & 0x1F) == 0x10)
			return -ERR_TARGET_NO_SPSR;
	}
	
	/*
	 * Select scan chain 1, and use INTEST instruction to make scan
	 * chain 1 into the internal test mode.
	 */
	retval = jtag_select_scanchain(1);
	if (retval)
		return retval;
	retval = jtag_write_ireg(JTAG_INTEST);
	if (retval)
		return retval;

	/*
	 * Step 1: If the register to write is not R0, then
	 * we first save the content of R0 register.
	 */
	if (strcasecmp(reg->name, "R0") != 0)
		r0_val = arm9tdmi_r0_read();

	/*
	 * Step 2: write the new value into R0 register.
	 * And if R0 is the destination register, just return directly.
	 */
	data = value;
	if (strcasecmp(reg->name, "CPSR") == 0) {
		data &= 0xF00000FF;
		data &= (~(0x00000020)); /* never change T bit (bit[5]) */
		if ((reg->value & 0x0000001F) == 0x10) {
			/* in User mode : never change the
			 * control bits of CPSR (bit[7:0])
			 */
			data &= 0xFFFFFF00;
		}
	} else if (strcasecmp(reg->name, "SPSR") == 0) {
		data &= 0xF00000FF;
		data &= (~(0x00000020)); /* never change T bit (bit[5]) */
	}
	reg->value = data; /* update our copy */

	arm9tdmi_r0_write(data); /* write new value into R0 */
	if (strcasecmp(reg->name, "R0") == 0)
		return 0;

	/*
	 * Step 3: move the value of R0 to the dest register.
	 */
	if (strcasecmp(reg->name, "CPSR") == 0) {
		/* MSR CPSR, R0 */
		sc1->writein[0] = 0xE129F000;
	} else if (strcasecmp(reg->name, "SPSR") == 0) {
		/* MSR SPSR, R0 */
		sc1->writein[0] = 0xE169F000;
	} else if (strcasecmp(reg->name, "PC") == 0) {
		/* MOV R15, R0 */
		sc1->writein[0] = 0xE1A0F000;
	} else { /* R1 - R15 */
		/* MOV Rxx, R0 */
		sc1->writein[0] = 0xE1A00000;
		tmp = reg->name + 1;
		str2int(tmp, &reg_num);
		/* set the Rd bits (bit[15:12]) of the OPCODE of
		 * the "MOV Rxx, R0" instruction
		 */
		sc1->writein[0] |= ((reg_num << 12) & 0x0000F000);
	}
	sc1->writein[1] = 0;
	arm9tdmi_exec_instruction(sc1->writein, sc1->readout, DEBUG_SPEED);

	sc1->writein[0] = ARM_NOP; /* NOP */
	arm9tdmi_exec_instruction(sc1->writein, sc1->readout, DEBUG_SPEED);

	sc1->writein[0] = ARM_NOP; /* NOP */
	arm9tdmi_exec_instruction(sc1->writein, sc1->readout, DEBUG_SPEED);

	sc1->writein[0] = ARM_NOP; /* NOP */
	arm9tdmi_exec_instruction(sc1->writein, sc1->readout, DEBUG_SPEED);

	/*
	 * Finally, restore R0 register
	 */
	arm9tdmi_r0_write(r0_val);

	return 0;
} /* end of arm9tdmi_register_write(...) */