mips32: Added CP0 coprocessor R/W routines
authorDrasko DRASKOVIC <drasko.draskovic@gmail.com>
Thu, 7 Jul 2011 15:41:20 +0000 (17:41 +0200)
committerØyvind Harboe <oyvind.harboe@zylin.com>
Tue, 9 Aug 2011 21:17:28 +0000 (23:17 +0200)
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
src/target/mips32.h
src/target/mips32_pracc.c
src/target/mips32_pracc.h
src/target/mips_m4k.c
src/target/mips_m4k.h

index 65ea6e8..d7f001e 100644 (file)
@@ -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
+};
+
index 4f0f0ef..8b21b0a 100644 (file)
@@ -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,
index cb8665c..1986e5c 100644 (file)
@@ -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)
index b207a5b..0c106ba 100644 (file)
@@ -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
index 822d0c8..350ed51 100644 (file)
@@ -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     *
@@ -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,
index 85d4c0a..11a936f 100644 (file)
@@ -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*/