From dccbf7d88d05a1f7a22f164ef149777718a399ed Mon Sep 17 00:00:00 2001 From: Paul Fertser Date: Sun, 1 Apr 2012 15:18:02 +0200 Subject: [PATCH] armv7m: add FPU registers support This patch adds the fpv4-sp-d16 registers to the armv7m register set. The work is inspired by Mathias K but takes a different approach: instead of having both double and single presicion registers in the cache this patch works only with the doubles and counts on GDB to split the data in halves whenever needed. Tested with HLA only (on an STM32F334 disco board). Currently this patch makes all ARMv7-M targets report an FPU-enabled target description to GDB. It shouldn't harm if the user is not trying to access non-existing FPU. However, the plan is to make this depend on actual FPU presence later. Change-Id: Ifcc72c80ef745230c42e4dc3995f792753fc4e7a Signed-off-by: Mathias K [fercerpav@gmail.com: rework to fit target description framework] Signed-off-by: Paul Fertser Reviewed-on: http://openocd.zylin.com/514 Tested-by: jenkins Reviewed-by: Peter Stuge Reviewed-by: Spencer Oliver --- src/target/arm.h | 2 +- src/target/arm7_9_common.c | 9 ++-- src/target/arm_dpm.c | 2 +- src/target/armv4_5.c | 4 +- src/target/armv7m.c | 91 +++++++++++++++++++++++++++++++------- src/target/cortex_m.c | 46 +++++++++++++++++++ src/target/hla_target.c | 26 +++-------- src/target/xscale.c | 2 +- 8 files changed, 137 insertions(+), 45 deletions(-) diff --git a/src/target/arm.h b/src/target/arm.h index b93952c0ee..27636cc3df 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 7f0e3adb38..617ee78406 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 b8f392cddf..5df625f035 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 69674cbf1e..e75fe99c4d 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 99af3a8777..ee96f0e2b3 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 d3b8f4e080..b194c33e6f 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 35ff8248b9..f778d23aa7 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 898d080ef0..82e41600eb 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"); -- 2.30.2