mips32: Added CP0 coprocessor R/W routines

This patch adds MIPS32 CP0 coprocessor R/W routines,
as well as adequate commands to use these routines via
telnet interface.

Now is becomes possible to affect CP0 internal registers
and configure CPU directly from OpenOCD.

1 files changed, 95 insertions, 0 deletions

diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index cb8665c3..1986e5ca 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -6,6 +6,9 @@
  *                                                                         *
  *   Copyright (C) 2009 by David N. Claffey <dnclaffey@gmail.com>          *
  *                                                                         *
+ *   Copyright (C) 2011 by Drasko DRASKOVIC                                *
+ *   drasko.draskovic@gmail.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     *
@@ -568,6 +571,98 @@ static int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info, uint32_t addr,
 	return retval;
 }
 
+int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_reg, uint32_t cp0_sel)
+{
+	/**
+	 * Do not make this code static, but regenerate it every time,
+	 * as 5th element has to be changed to add parameters
+	 */
+	uint32_t code[] = {
+															/* start: */
+		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
+		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),			/* $15 = MIPS32_PRACC_STACK */
+		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
+		MIPS32_SW(8,0,15),									/* sw $8,($15) */
+		MIPS32_SW(9,0,15),									/* sw $9,($15) */
+
+		/* 5 */ MIPS32_MFC0(8,0,0),							/* move COP0 [cp0_reg select] to $8 */
+
+		MIPS32_LUI(9,UPPER16(MIPS32_PRACC_PARAM_OUT)),		/* $11 = MIPS32_PRACC_PARAM_OUT */
+		MIPS32_ORI(9,9,LOWER16(MIPS32_PRACC_PARAM_OUT)),
+		MIPS32_SW(8,0,9),									/* sw $8,0($9) */
+
+		MIPS32_LW(9,0,15),									/* lw $9,($15) */
+		MIPS32_LW(8,0,15),									/* lw $8,($15) */
+		MIPS32_B(NEG16(12)),								/* b start */
+		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
+	};
+
+	/**
+	 * Note that our input parametes cp0_reg and cp0_sel
+	 * are numbers (not gprs) which make part of mfc0 instruction opcode.
+	 *
+	 * These are not fix, but can be different for each mips32_cp0_read() function call,
+	 * and that is why we must insert them directly into opcode,
+	 * i.e. we can not pass it on EJTAG microprogram stack (via param_in),
+	 * and put them into the gprs later from MIPS32_PRACC_STACK
+	 * because mfc0 do not use gpr as a parameter for the cp0_reg and select part,
+	 * but plain (immediate) number.
+	 *
+	 * MIPS32_MTC0 is implemented via MIPS32_R_INST macro.
+	 * In order to insert our parameters, we must change rd and funct fields.
+	 */
+	code[5] |= (cp0_reg << 11) | cp0_sel;  /* change rd and funct of MIPS32_R_INST macro */
+
+	/* TODO remove array */
+	uint32_t *param_out = val;
+	int retval;
+
+	retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, 0, NULL, 1, param_out, 1);
+
+	return retval;
+}
+
+int mips32_cp0_write(struct mips_ejtag *ejtag_info,
+											uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel)
+{
+	uint32_t code[] = {
+															/* start: */
+		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
+		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),			/* $15 = MIPS32_PRACC_STACK */
+		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
+		MIPS32_SW(8,0,15),									/* sw $8,($15) */
+		MIPS32_SW(9,0,15),									/* sw $9,($15) */
+
+		MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),		/* $8 = MIPS32_PRACC_PARAM_IN */
+		MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
+		MIPS32_LW(9,0,8),									/* Load write val to $9 */
+
+		/* 8 */ MIPS32_MTC0(9,0,0),							/* move $9 to COP0 [cp0_reg select] */
+
+		MIPS32_LW(9,0,15),									/* lw $9,($15) */
+		MIPS32_LW(8,0,15),									/* lw $8,($15) */
+		MIPS32_B(NEG16(12)),								/* b start */
+		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
+	};
+
+	/**
+	 * Note that MIPS32_MTC0 macro is implemented via MIPS32_R_INST macro.
+	 * In order to insert our parameters, we must change rd and funct fields.
+	 */
+	code[8] |= (cp0_reg << 11) | cp0_sel;  /* change rd and funct fields of MIPS32_R_INST macro */
+
+	/* TODO remove array */
+	uint32_t *param_in = malloc(1 * sizeof(uint32_t));
+	int retval;
+	param_in[0] = val;
+
+	retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, 1, param_in, 0, NULL, 1);
+
+	free(param_in);
+
+	return retval;
+}
+
 int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
 {
 	switch (size)