diff --git a/src/target/arm.h b/src/target/arm.h index b93952c0e..27636cc3d 100644 --- a/src/target/arm.h +++ b/src/target/arm.h @@ -154,7 +154,7 @@ struct arm { int (*read_core_reg)(struct target *target, struct reg *reg, int num, enum arm_mode mode); int (*write_core_reg)(struct target *target, struct reg *reg, - int num, enum arm_mode mode, uint32_t value); + int num, enum arm_mode mode, uint8_t *value); /** Read coprocessor register. */ int (*mrc)(struct target *target, int cpnum, diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c index 7f0e3adb3..617ee7840 100644 --- a/src/target/arm7_9_common.c +++ b/src/target/arm7_9_common.c @@ -2033,7 +2033,7 @@ static int arm7_9_read_core_reg(struct target *target, struct reg *r, } static int arm7_9_write_core_reg(struct target *target, struct reg *r, - int num, enum arm_mode mode, uint32_t value) + int num, enum arm_mode mode, uint8_t *value) { uint32_t reg[16]; struct arm_reg *areg = r->arch_info; @@ -2058,7 +2058,7 @@ static int arm7_9_write_core_reg(struct target *target, struct reg *r, if ((num >= 0) && (num <= 15)) { /* write a normal core register */ - reg[num] = value; + reg[num] = buf_get_u32(value, 0, 32); arm7_9->write_core_regs(target, 1 << num, reg); } else { @@ -2067,11 +2067,12 @@ static int arm7_9_write_core_reg(struct target *target, struct reg *r, */ int spsr = (areg->mode != ARM_MODE_ANY); + uint32_t t = buf_get_u32(value, 0, 32); /* if we're writing the CPSR, mask the T bit */ if (!spsr) - value &= ~0x20; + t &= ~0x20; - arm7_9->write_xpsr(target, value, spsr); + arm7_9->write_xpsr(target, t, spsr); } r->valid = 1; diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c index b8f392cdd..5df625f03 100644 --- a/src/target/arm_dpm.c +++ b/src/target/arm_dpm.c @@ -571,7 +571,7 @@ fail: } static int arm_dpm_write_core_reg(struct target *target, struct reg *r, - int regnum, enum arm_mode mode, uint32_t value) + int regnum, enum arm_mode mode, uint8_t *value) { struct arm_dpm *dpm = target_to_arm(target)->dpm; int retval; diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c index 69674cbf1..e75fe99c4 100644 --- a/src/target/armv4_5.c +++ b/src/target/armv4_5.c @@ -555,8 +555,10 @@ static int armv4_5_set_core_reg(struct reg *reg, uint8_t *buf) LOG_DEBUG("changing ARM core mode to '%s'", arm_mode_name(value & 0x1f)); value &= ~((1 << 24) | (1 << 5)); + uint8_t t[4]; + buf_set_u32(t, 0, 32, value); armv4_5_target->write_core_reg(target, reg, - 16, ARM_MODE_ANY, value); + 16, ARM_MODE_ANY, t); } } else { buf_set_u32(reg->value, 0, 32, value); diff --git a/src/target/armv7m.c b/src/target/armv7m.c index 99af3a877..ee96f0e2b 100644 --- a/src/target/armv7m.c +++ b/src/target/armv7m.c @@ -110,6 +110,25 @@ static const struct { { ARMV7M_BASEPRI, "basepri", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" }, { ARMV7M_FAULTMASK, "faultmask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" }, { ARMV7M_CONTROL, "control", 2, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" }, + + { ARMV7M_D0, "d0", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D1, "d1", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D2, "d2", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D3, "d3", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D4, "d4", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D5, "d5", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D6, "d6", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D7, "d7", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D8, "d8", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D9, "d9", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D10, "d10", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D11, "d11", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D12, "d12", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D13, "d13", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D14, "d14", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D15, "d15", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + + { ARMV7M_FPSCR, "fpscr", 32, REG_TYPE_INT, "float", "org.gnu.gdb.arm.vfp" }, }; #define ARMV7M_NUM_REGS ARRAY_SIZE(armv7m_regs) @@ -131,8 +150,8 @@ int armv7m_restore_context(struct target *target) for (i = ARMV7M_NUM_REGS - 1; i >= 0; i--) { if (cache->reg_list[i].dirty) { - uint32_t value = buf_get_u32(cache->reg_list[i].value, 0, 32); - armv7m->arm.write_core_reg(target, &cache->reg_list[i], i, ARM_MODE_ANY, value); + armv7m->arm.write_core_reg(target, &cache->reg_list[i], i, + ARM_MODE_ANY, cache->reg_list[i].value); } } @@ -179,12 +198,11 @@ static int armv7m_set_core_reg(struct reg *reg, uint8_t *buf) { struct arm_reg *armv7m_reg = reg->arch_info; struct target *target = armv7m_reg->target; - uint32_t value = buf_get_u32(buf, 0, 32); if (target->state != TARGET_HALTED) return ERROR_TARGET_NOT_HALTED; - buf_set_u32(reg->value, 0, 32, value); + buf_cpy(buf, reg->value, reg->size); reg->dirty = 1; reg->valid = 1; @@ -202,10 +220,28 @@ static int armv7m_read_core_reg(struct target *target, struct reg *r, assert(num < (int)armv7m->arm.core_cache->num_regs); armv7m_core_reg = armv7m->arm.core_cache->reg_list[num].arch_info; - retval = armv7m->load_core_reg_u32(target, - armv7m_core_reg->num, ®_value); - buf_set_u32(armv7m->arm.core_cache->reg_list[num].value, 0, 32, reg_value); + if ((armv7m_core_reg->num >= ARMV7M_D0) && (armv7m_core_reg->num <= ARMV7M_D15)) { + /* map D0..D15 to S0..S31 */ + size_t regidx = ARMV7M_S0 + 2 * (armv7m_core_reg->num - ARMV7M_D0); + retval = armv7m->load_core_reg_u32(target, regidx, ®_value); + if (retval != ERROR_OK) + return retval; + buf_set_u32(armv7m->arm.core_cache->reg_list[num].value, + 0, 32, reg_value); + retval = armv7m->load_core_reg_u32(target, regidx + 1, ®_value); + if (retval != ERROR_OK) + return retval; + buf_set_u32(armv7m->arm.core_cache->reg_list[num].value + 4, + 0, 32, reg_value); + } else { + retval = armv7m->load_core_reg_u32(target, + armv7m_core_reg->num, ®_value); + if (retval != ERROR_OK) + return retval; + buf_set_u32(armv7m->arm.core_cache->reg_list[num].value, 0, 32, reg_value); + } + armv7m->arm.core_cache->reg_list[num].valid = 1; armv7m->arm.core_cache->reg_list[num].dirty = 0; @@ -213,7 +249,7 @@ static int armv7m_read_core_reg(struct target *target, struct reg *r, } static int armv7m_write_core_reg(struct target *target, struct reg *r, - int num, enum arm_mode mode, uint32_t value) + int num, enum arm_mode mode, uint8_t *value) { int retval; struct arm_reg *armv7m_core_reg; @@ -222,20 +258,38 @@ static int armv7m_write_core_reg(struct target *target, struct reg *r, assert(num < (int)armv7m->arm.core_cache->num_regs); armv7m_core_reg = armv7m->arm.core_cache->reg_list[num].arch_info; - retval = armv7m->store_core_reg_u32(target, - armv7m_core_reg->num, - value); - if (retval != ERROR_OK) { - LOG_ERROR("JTAG failure"); - armv7m->arm.core_cache->reg_list[num].dirty = armv7m->arm.core_cache->reg_list[num].valid; - return ERROR_JTAG_DEVICE_ERROR; + + if ((armv7m_core_reg->num >= ARMV7M_D0) && (armv7m_core_reg->num <= ARMV7M_D15)) { + /* map D0..D15 to S0..S31 */ + size_t regidx = ARMV7M_S0 + 2 * (armv7m_core_reg->num - ARMV7M_D0); + + uint32_t t = buf_get_u32(value, 0, 32); + retval = armv7m->store_core_reg_u32(target, regidx, t); + if (retval != ERROR_OK) + goto out_error; + + t = buf_get_u32(value + 4, 0, 32); + retval = armv7m->store_core_reg_u32(target, regidx + 1, t); + if (retval != ERROR_OK) + goto out_error; + } else { + uint32_t t = buf_get_u32(value, 0, 32); + + LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", num, t); + retval = armv7m->store_core_reg_u32(target, armv7m_core_reg->num, t); + if (retval != ERROR_OK) + goto out_error; } - LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", num, value); armv7m->arm.core_cache->reg_list[num].valid = 1; armv7m->arm.core_cache->reg_list[num].dirty = 0; return ERROR_OK; + +out_error: + LOG_ERROR("Error setting register"); + armv7m->arm.core_cache->reg_list[num].dirty = armv7m->arm.core_cache->reg_list[num].valid; + return ERROR_JTAG_DEVICE_ERROR; } /** @@ -533,7 +587,10 @@ struct reg_cache *armv7m_build_reg_cache(struct target *target) reg_list[i].name = armv7m_regs[i].name; reg_list[i].size = armv7m_regs[i].bits; - reg_list[i].value = calloc(1, 4); + size_t storage_size = DIV_ROUND_UP(armv7m_regs[i].bits, 8); + if (storage_size < 4) + storage_size = 4; + reg_list[i].value = calloc(1, storage_size); reg_list[i].dirty = 0; reg_list[i].valid = 0; reg_list[i].type = &armv7m_reg_type; diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c index d3b8f4e08..b194c33e6 100644 --- a/src/target/cortex_m.c +++ b/src/target/cortex_m.c @@ -1498,6 +1498,29 @@ static int cortex_m_load_core_reg_u32(struct target *target, LOG_DEBUG("load from core reg %i value 0x%" PRIx32 "", (int)num, *value); break; + case ARMV7M_FPSCR: + /* Floating-point Status and Registers */ + retval = target_write_u32(target, DCB_DCRSR, 0x21); + if (retval != ERROR_OK) + return retval; + retval = target_read_u32(target, DCB_DCRDR, value); + if (retval != ERROR_OK) + return retval; + LOG_DEBUG("load from FPSCR value 0x%" PRIx32, *value); + break; + + case ARMV7M_S0 ... ARMV7M_S31: + /* Floating-point Status and Registers */ + retval = target_write_u32(target, DCB_DCRSR, num - ARMV7M_S0 + 0x40); + if (retval != ERROR_OK) + return retval; + retval = target_read_u32(target, DCB_DCRDR, value); + if (retval != ERROR_OK) + return retval; + LOG_DEBUG("load from FPU reg S%d value 0x%" PRIx32, + (int)(num - ARMV7M_S0), *value); + break; + case ARMV7M_PRIMASK: case ARMV7M_BASEPRI: case ARMV7M_FAULTMASK: @@ -1561,6 +1584,29 @@ static int cortex_m_store_core_reg_u32(struct target *target, LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value); break; + case ARMV7M_FPSCR: + /* Floating-point Status and Registers */ + retval = target_write_u32(target, DCB_DCRDR, value); + if (retval != ERROR_OK) + return retval; + retval = target_write_u32(target, DCB_DCRSR, 0x21 | (1<<16)); + if (retval != ERROR_OK) + return retval; + LOG_DEBUG("write FPSCR value 0x%" PRIx32, value); + break; + + case ARMV7M_S0 ... ARMV7M_S31: + /* Floating-point Status and Registers */ + retval = target_write_u32(target, DCB_DCRDR, value); + if (retval != ERROR_OK) + return retval; + retval = target_write_u32(target, DCB_DCRSR, (num - ARMV7M_S0 + 0x40) | (1<<16)); + if (retval != ERROR_OK) + return retval; + LOG_DEBUG("write FPU reg S%d value 0x%" PRIx32, + (int)(num - ARMV7M_S0), value); + break; + case ARMV7M_PRIMASK: case ARMV7M_BASEPRI: case ARMV7M_FAULTMASK: diff --git a/src/target/hla_target.c b/src/target/hla_target.c index 35ff8248b..f778d23aa 100644 --- a/src/target/hla_target.c +++ b/src/target/hla_target.c @@ -75,11 +75,6 @@ static int adapter_load_core_reg_u32(struct target *target, LOG_DEBUG("load from core reg %i value 0x%" PRIx32 "", (int)num, *value); break; - case ARMV7M_FPSID: - case ARMV7M_FPEXC: - *value = 0; - break; - case ARMV7M_FPSCR: /* Floating-point Status and Registers */ retval = target_write_u32(target, ARMV7M_SCS_DCRSR, 33); @@ -88,7 +83,7 @@ static int adapter_load_core_reg_u32(struct target *target, retval = target_read_u32(target, ARMV7M_SCS_DCRDR, value); if (retval != ERROR_OK) return retval; - LOG_DEBUG("load from core reg %i value 0x%" PRIx32 "", (int)num, *value); + LOG_DEBUG("load from FPSCR value 0x%" PRIx32, *value); break; case ARMV7M_S0 ... ARMV7M_S31: @@ -99,11 +94,8 @@ static int adapter_load_core_reg_u32(struct target *target, retval = target_read_u32(target, ARMV7M_SCS_DCRDR, value); if (retval != ERROR_OK) return retval; - LOG_DEBUG("load from core reg %i value 0x%" PRIx32 "", (int)num, *value); - break; - - case ARMV7M_D0 ... ARMV7M_D15: - value = 0; + LOG_DEBUG("load from FPU reg S%d value 0x%" PRIx32, + (int)(num - ARMV7M_S0), *value); break; case ARMV7M_PRIMASK: @@ -176,10 +168,6 @@ static int adapter_store_core_reg_u32(struct target *target, LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value); break; - case ARMV7M_FPSID: - case ARMV7M_FPEXC: - break; - case ARMV7M_FPSCR: /* Floating-point Status and Registers */ retval = target_write_u32(target, ARMV7M_SCS_DCRDR, value); @@ -188,7 +176,7 @@ static int adapter_store_core_reg_u32(struct target *target, retval = target_write_u32(target, ARMV7M_SCS_DCRSR, 33 | (1<<16)); if (retval != ERROR_OK) return retval; - LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value); + LOG_DEBUG("write FPSCR value 0x%" PRIx32, value); break; case ARMV7M_S0 ... ARMV7M_S31: @@ -199,10 +187,8 @@ static int adapter_store_core_reg_u32(struct target *target, retval = target_write_u32(target, ARMV7M_SCS_DCRSR, (num-ARMV7M_S0+64) | (1<<16)); if (retval != ERROR_OK) return retval; - LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value); - break; - - case ARMV7M_D0 ... ARMV7M_D15: + LOG_DEBUG("write FPU reg S%d value 0x%" PRIx32, + (int)(num - ARMV7M_S0), value); break; case ARMV7M_PRIMASK: diff --git a/src/target/xscale.c b/src/target/xscale.c index 898d080ef..82e41600e 100644 --- a/src/target/xscale.c +++ b/src/target/xscale.c @@ -1621,7 +1621,7 @@ static int xscale_read_core_reg(struct target *target, struct reg *r, } static int xscale_write_core_reg(struct target *target, struct reg *r, - int num, enum arm_mode mode, uint32_t value) + int num, enum arm_mode mode, uint8_t *value) { /** \todo add debug handler support for core register writes */ LOG_ERROR("not implemented");