From 1be7163408cc6420d85bf990a2dae46c559a12b1 Mon Sep 17 00:00:00 2001 From: Drasko DRASKOVIC Date: Thu, 7 Jul 2011 17:41:20 +0200 Subject: [PATCH] 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. --- src/target/mips32.c | 106 +++++++++++++++++++++++++++++++++++++ src/target/mips32.h | 5 ++ src/target/mips32_pracc.c | 95 +++++++++++++++++++++++++++++++++ src/target/mips32_pracc.h | 41 +++++++++++++-- src/target/mips_m4k.c | 108 +++++++++++++++++++++++++++++++++++++- src/target/mips_m4k.h | 5 ++ 6 files changed, 355 insertions(+), 5 deletions(-) diff --git a/src/target/mips32.c b/src/target/mips32.c index 65ea6e8591..d7f001e1f5 100644 --- a/src/target/mips32.c +++ b/src/target/mips32.c @@ -7,6 +7,9 @@ * Copyright (C) 2007,2008 Øyvind Harboe * * oyvind.harboe@zylin.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 * @@ -758,3 +761,106 @@ int mips32_blank_check_memory(struct target *target, return ERROR_OK; } + +static int mips32_verify_pointer(struct command_context *cmd_ctx, + struct mips32_common *mips32) +{ + if (mips32->common_magic != MIPS32_COMMON_MAGIC) { + command_print(cmd_ctx, "target is not an MIPS32"); + return ERROR_TARGET_INVALID; + } + return ERROR_OK; +} + +/** + * MIPS32 targets expose command interface + * to manipulate CP0 registers + */ +COMMAND_HANDLER(mips32_handle_cp0_command) +{ + int retval; + struct target *target = get_current_target(CMD_CTX); + struct mips32_common *mips32 = target_to_mips32(target); + struct mips_ejtag *ejtag_info = &mips32->ejtag_info; + + + retval = mips32_verify_pointer(CMD_CTX, mips32); + if (retval != ERROR_OK) + return retval; + + if (target->state != TARGET_HALTED) + { + command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME); + return ERROR_OK; + } + + /* two or more argument, access a single register/select (write if third argument is given) */ + if (CMD_ARGC < 2) + { + command_print(CMD_CTX, "command requires more arguments."); + } + else + { + uint32_t cp0_reg, cp0_sel; + COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], cp0_reg); + COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], cp0_sel); + + if (CMD_ARGC == 2) + { + uint32_t value; + + if ((retval = mips32_cp0_read(ejtag_info, &value, cp0_reg, cp0_sel)) != ERROR_OK) + { + command_print(CMD_CTX, + "couldn't access reg %" PRIi32, + cp0_reg); + return ERROR_OK; + } + if ((retval = jtag_execute_queue()) != ERROR_OK) + { + return retval; + } + + command_print(CMD_CTX, "cp0 reg %" PRIi32 ", select %" PRIi32 ": %8.8" PRIx32, + cp0_reg, cp0_sel, value); + } + else if (CMD_ARGC == 3) + { + uint32_t value; + COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], value); + if ((retval = mips32_cp0_write(ejtag_info, value, cp0_reg, cp0_sel)) != ERROR_OK) + { + command_print(CMD_CTX, + "couldn't access cp0 reg %" PRIi32 ", select %" PRIi32, + cp0_reg, cp0_sel); + return ERROR_OK; + } + command_print(CMD_CTX, "cp0 reg %" PRIi32 ", select %" PRIi32 ": %8.8" PRIx32, + cp0_reg, cp0_sel, value); + } + } + + return ERROR_OK; +} + +static const struct command_registration mips32_exec_command_handlers[] = { + { + .name = "cp0", + .handler = mips32_handle_cp0_command, + .mode = COMMAND_EXEC, + .usage = "regnum select [value]", + .help = "display/modify cp0 register", + }, + COMMAND_REGISTRATION_DONE +}; + +const struct command_registration mips32_command_handlers[] = { + { + .name = "mips32", + .mode = COMMAND_ANY, + .help = "mips32 command group", + .chain = mips32_exec_command_handlers, + }, + COMMAND_REGISTRATION_DONE +}; + diff --git a/src/target/mips32.h b/src/target/mips32.h index 4f0f0ef073..8b21b0ac3b 100644 --- a/src/target/mips32.h +++ b/src/target/mips32.h @@ -4,6 +4,9 @@ * * * Copyright (C) 2008 by David T.L. Wong * * * + * 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 * @@ -149,6 +152,8 @@ struct mips32_algorithm #define MIPS32_SDBBP 0x7000003F #define MIPS16_SDBBP 0xE801 +extern const struct command_registration mips32_command_handlers[]; + int mips32_arch_state(struct target *target); int mips32_init_arch_info(struct target *target, diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c index cb8665c3aa..1986e5ca21 100644 --- a/src/target/mips32_pracc.c +++ b/src/target/mips32_pracc.c @@ -6,6 +6,9 @@ * * * Copyright (C) 2009 by David N. Claffey * * * + * 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) diff --git a/src/target/mips32_pracc.h b/src/target/mips32_pracc.h index b207a5be56..0c106bab55 100644 --- a/src/target/mips32_pracc.h +++ b/src/target/mips32_pracc.h @@ -4,6 +4,9 @@ * * * Copyright (C) 2008 by David T.L. Wong * * * + * 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 * @@ -35,9 +38,9 @@ #define MIPS32_PRACC_PARAM_OUT_SIZE 0x1000 #define MIPS32_FASTDATA_HANDLER_SIZE 0x80 -#define UPPER16(uint32_t) (uint32_t >> 16) -#define LOWER16(uint32_t) (uint32_t & 0xFFFF) -#define NEG16(v) (((~(v)) + 1) & 0xFFFF) +#define UPPER16(uint32_t) (uint32_t >> 16) +#define LOWER16(uint32_t) (uint32_t & 0xFFFF) +#define NEG16(v) (((~(v)) + 1) & 0xFFFF) /*#define NEG18(v) (((~(v)) + 1) & 0x3FFFF)*/ int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, @@ -54,4 +57,36 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, int code_len, const uint32_ int num_param_in, uint32_t *param_in, int num_param_out, uint32_t *param_out, int cycle); +/** + * \b mips32_cp0_read + * + * Simulates mfc0 ASM instruction (Move From C0), + * i.e. implements copro C0 Register read. + * + * @param[in] ejtag_info + * @param[in] val Storage to hold read value + * @param[in] cp0_reg Number of copro C0 register we want to read + * @param[in] cp0_sel Select for the given C0 register + * + * @return ERROR_OK on Sucess, ERROR_FAIL otherwise + */ +int mips32_cp0_read(struct mips_ejtag *ejtag_info, + uint32_t *val, uint32_t cp0_reg, uint32_t cp0_sel); + +/** + * \b mips32_cp0_write + * + * Simulates mtc0 ASM instruction (Move To C0), + * i.e. implements copro C0 Register read. + * + * @param[in] ejtag_info + * @param[in] val Value to be written + * @param[in] cp0_reg Number of copro C0 register we want to write to + * @param[in] cp0_sel Select for the given C0 register + * + * @return ERROR_OK on Sucess, ERROR_FAIL otherwise + */ +int mips32_cp0_write(struct mips_ejtag *ejtag_info, + uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel); + #endif diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c index 822d0c8541..350ed5139c 100644 --- a/src/target/mips_m4k.c +++ b/src/target/mips_m4k.c @@ -6,6 +6,9 @@ * * * Copyright (C) 2009 by David N. Claffey * * * + * 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 * @@ -1078,13 +1081,13 @@ static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address, return ERROR_FAIL; } - uint32_t i, t32; + uint32_t i, t32; for(i = 0; i < (count*4); i += 4) { t32 = target_buffer_get_u32(target,&buffer[i]); h_u32_to_le(&t[i], t32); } - + retval = mips32_pracc_fastdata_xfer(ejtag_info, mips32->fast_data_area, write_t, address, count, (uint32_t*) (void *)t); @@ -1101,6 +1104,106 @@ static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address, return retval; } +static int mips_m4k_verify_pointer(struct command_context *cmd_ctx, + struct mips_m4k_common *mips_m4k) +{ + if (mips_m4k->common_magic != MIPSM4K_COMMON_MAGIC) { + command_print(cmd_ctx, "target is not an MIPS_M4K"); + return ERROR_TARGET_INVALID; + } + return ERROR_OK; +} + +COMMAND_HANDLER(mips_m4k_handle_cp0_command) +{ + int retval; + struct target *target = get_current_target(CMD_CTX); + struct mips_m4k_common *mips_m4k = target_to_m4k(target); + struct mips_ejtag *ejtag_info = &mips_m4k->mips32.ejtag_info; + + retval = mips_m4k_verify_pointer(CMD_CTX, mips_m4k); + if (retval != ERROR_OK) + return retval; + + if (target->state != TARGET_HALTED) + { + command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME); + return ERROR_OK; + } + + /* two or more argument, access a single register/select (write if third argument is given) */ + if (CMD_ARGC < 2) + { + command_print(CMD_CTX, "command requires more arguments."); + } + else + { + uint32_t cp0_reg, cp0_sel; + COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], cp0_reg); + COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], cp0_sel); + + if (CMD_ARGC == 2) + { + uint32_t value; + + if ((retval = mips32_cp0_read(ejtag_info, &value, cp0_reg, cp0_sel)) != ERROR_OK) + { + command_print(CMD_CTX, + "couldn't access reg %" PRIi32, + cp0_reg); + return ERROR_OK; + } + if ((retval = jtag_execute_queue()) != ERROR_OK) + { + return retval; + } + + command_print(CMD_CTX, "cp0 reg %" PRIi32 ", select %" PRIi32 ": %8.8" PRIx32, + cp0_reg, cp0_sel, value); + } + else if (CMD_ARGC == 3) + { + uint32_t value; + COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], value); + if ((retval = mips32_cp0_write(ejtag_info, value, cp0_reg, cp0_sel)) != ERROR_OK) + { + command_print(CMD_CTX, + "couldn't access cp0 reg %" PRIi32 ", select %" PRIi32, + cp0_reg, cp0_sel); + return ERROR_OK; + } + command_print(CMD_CTX, "cp0 reg %" PRIi32 ", select %" PRIi32 ": %8.8" PRIx32, + cp0_reg, cp0_sel, value); + } + } + + return ERROR_OK; +} + +static const struct command_registration mips_m4k_exec_command_handlers[] = { + { + .name = "cp0", + .handler = mips_m4k_handle_cp0_command, + .mode = COMMAND_EXEC, + .usage = "regnum [value]", + .help = "display/modify cp0 register", + }, + COMMAND_REGISTRATION_DONE +}; + +const struct command_registration mips_m4k_command_handlers[] = { + { + .chain = mips32_command_handlers, + }, + { + .name = "mips_m4k", + .mode = COMMAND_ANY, + .help = "mips_m4k command group", + .chain = mips_m4k_exec_command_handlers, + }, + COMMAND_REGISTRATION_DONE +}; + struct target_type mips_m4k_target = { .name = "mips_m4k", @@ -1133,6 +1236,7 @@ struct target_type mips_m4k_target = .add_watchpoint = mips_m4k_add_watchpoint, .remove_watchpoint = mips_m4k_remove_watchpoint, + .commands = mips_m4k_command_handlers, .target_create = mips_m4k_target_create, .init_target = mips_m4k_init_target, .examine = mips_m4k_examine, diff --git a/src/target/mips_m4k.h b/src/target/mips_m4k.h index 85d4c0a5c8..11a936fc63 100644 --- a/src/target/mips_m4k.h +++ b/src/target/mips_m4k.h @@ -4,6 +4,9 @@ * * * Copyright (C) 2008 by David T.L. Wong * * * + * 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 * @@ -43,4 +46,6 @@ target_to_m4k(struct target *target) struct mips_m4k_common, mips32); } +extern const struct command_registration mips_m4k_command_handlers[]; + #endif /*MIPS_M4K_H*/ -- 2.30.2