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 <kesmtp@freenet.de>
[fercerpav@gmail.com: rework to fit target description framework]
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/514
Tested-by: jenkins
Reviewed-by: Peter Stuge <peter@stuge.se>
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>

diff --git a/src/target/arm.h b/src/target/arm.h
index b93952c0eef908f6e0726f4012eede2fdb58f03e..27636cc3df9658c0f40dd15d57dccbbf5a8782b6 100644 (file)
--- 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 7f0e3adb38826aa8d2b43a1d8308db7cef99aa31..617ee78406224922c3833e54017b8a383c75dbf2 100644 (file)
--- 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 b8f392cddf669bbe39b3170ef302ddaa6d73d8ec..5df625f035fc974a3fa4a17bcac4fc8f5d577996 100644 (file)
--- 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 69674cbf1e319be71e24fefff161c4f32f2d86c2..e75fe99c4dd9b0c71a2c2da5870fe2769ee9e6d7 100644 (file)
--- 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 99af3a8777e6703954dd9ee5507517d5e8940815..ee96f0e2b36648bbeb9965652ead8f2da7117866 100644 (file)
--- 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, &reg_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, &reg_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, &reg_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, &reg_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 d3b8f4e080f5c18583a191024e398e4b62990e8d..b194c33e6f1baa07e9f83a481391e5250cf87219 100644 (file)
--- 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 35ff8248b929d74bc6dbb33da48b95c764d50644..f778d23aa7cdf3a84c43c9da1089fd9b468ea619 100644 (file)
--- 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 898d080ef05adb23c6a29cb6ca122647d2703919..82e41600eb2a6b34d790ce21a05005550a69faad 100644 (file)
--- 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");