target/armv7m: rework Cortex-M register handling part 3

Move primask/basepri/faultmask/control packing/unpacking from
cortex_m.c and hla_target.c to armv7m.c armv7m_read_core_reg()
and armv7m_write_core_reg() where also the FP 32/64-bit registers
conversion takes place.

Introduce a new hidden register ARMV7M_PMSK_BPRI_FLTMSK_CTRL
for packing/unpacking of special registers in the register cache.

The new packing/unpacking is endianess safe.

While on it improve returned error codes and LOG_ messages.

Just minimal changes in cortex_m.c and hla_target.c, will be
consolidated in the next patch.

Change-Id: Id51e764e243e54b5fdaadf2a202eee7c4bc729fe
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/5863
Tested-by: jenkins
Reviewed-by: Christopher Head <chead@zaber.com>
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index 177437391c3b698b339b6f0b8509c84ff7d5f81b..7e9b72a8e8c4b8f0d1aba14f69d30a8786d97744 100644 (file)
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -14,6 +14,9 @@
  *   Copyright (C) 2018 by Liviu Ionescu                                   *
  *   <ilg@livius.net>                                                      *
  *                                                                         *
+ *   Copyright (C) 2019 by Tomas Vanek                                     *
+ *   vanekt@fbl.cz                                                         *
+ *                                                                         *
  *   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     *
@@ -108,6 +111,15 @@ static const struct {
        { ARMV7M_MSP, "msp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" },
        { ARMV7M_PSP, "psp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" },
 
+       /* A working register for packing/unpacking special regs, hidden from gdb */
+       { ARMV7M_PMSK_BPRI_FLTMSK_CTRL, "pmsk_bpri_fltmsk_ctrl", 32, REG_TYPE_INT, NULL, NULL },
+
+       /* WARNING: If you use armv7m_write_core_reg() on one of 4 following
+        * special registers, the new data go to ARMV7M_PMSK_BPRI_FLTMSK_CTRL
+        * cache only and are not flushed to CPU HW register.
+        * To trigger write to CPU HW register, add
+        *              armv7m_write_core_reg(,,ARMV7M_PMSK_BPRI_FLTMSK_CTRL,);
+        */
        { ARMV7M_PRIMASK, "primask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
        { 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" },
@@ -150,6 +162,9 @@ int armv7m_restore_context(struct target *target)
        if (armv7m->pre_restore_context)
                armv7m->pre_restore_context(target);
 
+       /* The descending order of register writes is crucial for correct
+        * packing of ARMV7M_PMSK_BPRI_FLTMSK_CTRL!
+        * See also comments in the register table above */
        for (i = cache->num_regs - 1; i >= 0; i--) {
                if (cache->reg_list[i].dirty) {
                        armv7m->arm.write_core_reg(target, &cache->reg_list[i], i,
@@ -225,23 +240,45 @@ static uint32_t armv7m_map_id_to_regsel(unsigned int arm_reg_id)
                 */
                return arm_reg_id;
 
+       case ARMV7M_PMSK_BPRI_FLTMSK_CTRL:
+               return ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL;
+
        case ARMV7M_FPSCR:
                return ARMV7M_REGSEL_FPSCR;
 
        case ARMV7M_D0 ... ARMV7M_D15:
                return ARMV7M_REGSEL_S0 + 2 * (arm_reg_id - ARMV7M_D0);
 
-       /* TODO: remove. This is temporary hack until packing/unpacking
-        * of special regs is moved to armv7m.c */
+       default:
+               LOG_ERROR("Bad register ID %u", arm_reg_id);
+               return arm_reg_id;
+       }
+}
+
+static bool armv7m_map_reg_packing(unsigned int arm_reg_id,
+                                       unsigned int *reg32_id, uint32_t *offset)
+{
+       switch (arm_reg_id) {
+
        case ARMV7M_PRIMASK:
+               *reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+               *offset = 0;
+               return true;
        case ARMV7M_BASEPRI:
+               *reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+               *offset = 1;
+               return true;
        case ARMV7M_FAULTMASK:
+               *reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+               *offset = 2;
+               return true;
        case ARMV7M_CONTROL:
-               return arm_reg_id;
+               *reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
+               *offset = 3;
+               return true;
 
        default:
-               LOG_ERROR("Bad register ID %u", arm_reg_id);
-               return arm_reg_id;
+               return false;
        }
 }
 
@@ -250,85 +287,134 @@ static int armv7m_read_core_reg(struct target *target, struct reg *r,
 {
        uint32_t reg_value;
        int retval;
-       struct arm_reg *armv7m_core_reg;
        struct armv7m_common *armv7m = target_to_armv7m(target);
 
        assert(num < (int)armv7m->arm.core_cache->num_regs);
        assert(num == (int)r->number);
 
-       armv7m_core_reg = armv7m->arm.core_cache->reg_list[num].arch_info;
+       /* If a code calls read_reg, it expects the cache is no more dirty.
+        * Clear the dirty flag regardless of the later read succeeds or not
+        * to prevent unwanted cache flush after a read error */
+       r->dirty = false;
 
-       uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
+       if (r->size <= 8) {
+               /* any 8-bit or shorter register is packed */
+               uint32_t offset = 0;    /* silence false gcc warning */
+               unsigned int reg32_id;
+
+               bool is_packed = armv7m_map_reg_packing(num, &reg32_id, &offset);
+               assert(is_packed);
+               struct reg *r32 = &armv7m->arm.core_cache->reg_list[reg32_id];
+
+               /* Read 32-bit container register if not cached */
+               if (!r32->valid) {
+                       retval = armv7m_read_core_reg(target, r32, reg32_id, mode);
+                       if (retval != ERROR_OK)
+                               return retval;
+               }
+
+               /* Copy required bits of 32-bit container register */
+               buf_cpy(r32->value + offset, r->value, r->size);
 
-       if ((armv7m_core_reg->num >= ARMV7M_D0) && (armv7m_core_reg->num <= ARMV7M_D15)) {
-               /* map D0..D15 to S0..S31 */
-               retval = armv7m->load_core_reg_u32(target, regsel, &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, regsel + 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,
-                                                  regsel, &reg_value);
+               assert(r->size == 32 || r->size == 64);
+
+               struct arm_reg *armv7m_core_reg = r->arch_info;
+               uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
+
+               retval = armv7m->load_core_reg_u32(target, regsel, &reg_value);
                if (retval != ERROR_OK)
                        return retval;
-               buf_set_u32(armv7m->arm.core_cache->reg_list[num].value, 0, 32, reg_value);
+               buf_set_u32(r->value, 0, 32, reg_value);
+
+               if (r->size == 64) {
+                       retval = armv7m->load_core_reg_u32(target, regsel + 1, &reg_value);
+                       if (retval != ERROR_OK) {
+                               r->valid = false;
+                               return retval;
+                       }
+                       buf_set_u32(r->value + 4, 0, 32, reg_value);
+
+                       uint64_t q = buf_get_u64(r->value, 0, 64);
+                       LOG_DEBUG("read %s value 0x%016" PRIx64, r->name, q);
+               } else {
+                       LOG_DEBUG("read %s value 0x%08" PRIx32, r->name, reg_value);
+               }
        }
 
-       armv7m->arm.core_cache->reg_list[num].valid = true;
-       armv7m->arm.core_cache->reg_list[num].dirty = false;
+       r->valid = true;
 
-       return retval;
+       return ERROR_OK;
 }
 
 static int armv7m_write_core_reg(struct target *target, struct reg *r,
        int num, enum arm_mode mode, uint8_t *value)
 {
        int retval;
-       struct arm_reg *armv7m_core_reg;
+       uint32_t t;
        struct armv7m_common *armv7m = target_to_armv7m(target);
 
        assert(num < (int)armv7m->arm.core_cache->num_regs);
        assert(num == (int)r->number);
 
-       armv7m_core_reg = armv7m->arm.core_cache->reg_list[num].arch_info;
+       if (value != r->value) {
+               /* If we are not flushing the cache, store the new value to the cache */
+               buf_cpy(value, r->value, r->size);
+       }
 
-       uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
+       if (r->size <= 8) {
+               /* any 8-bit or shorter register is packed */
+               uint32_t offset = 0;    /* silence false gcc warning */
+               unsigned int reg32_id;
 
-       if ((armv7m_core_reg->num >= ARMV7M_D0) && (armv7m_core_reg->num <= ARMV7M_D15)) {
-               /* map D0..D15 to S0..S31 */
-               uint32_t t = buf_get_u32(value, 0, 32);
-               retval = armv7m->store_core_reg_u32(target, regsel, t);
-               if (retval != ERROR_OK)
-                       goto out_error;
+               bool is_packed = armv7m_map_reg_packing(num, &reg32_id, &offset);
+               assert(is_packed);
+               struct reg *r32 = &armv7m->arm.core_cache->reg_list[reg32_id];
+
+               if (!r32->valid) {
+                       /* Before merging with other parts ensure the 32-bit register is valid */
+                       retval = armv7m_read_core_reg(target, r32, reg32_id, mode);
+                       if (retval != ERROR_OK)
+                               return retval;
+               }
+
+               /* Write a part to the 32-bit container register */
+               buf_cpy(value, r32->value + offset, r->size);
+               r32->dirty = true;
 
-               t = buf_get_u32(value + 4, 0, 32);
-               retval = armv7m->store_core_reg_u32(target, regsel + 1, t);
-               if (retval != ERROR_OK)
-                       goto out_error;
        } else {
-               uint32_t t = buf_get_u32(value, 0, 32);
+               assert(r->size == 32 || r->size == 64);
+
+               struct arm_reg *armv7m_core_reg = r->arch_info;
+               uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
 
-               LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", num, t);
+               t = buf_get_u32(value, 0, 32);
                retval = armv7m->store_core_reg_u32(target, regsel, t);
                if (retval != ERROR_OK)
                        goto out_error;
+
+               if (r->size == 64) {
+                       t = buf_get_u32(value + 4, 0, 32);
+                       retval = armv7m->store_core_reg_u32(target, regsel + 1, t);
+                       if (retval != ERROR_OK)
+                               goto out_error;
+
+                       uint64_t q = buf_get_u64(value, 0, 64);
+                       LOG_DEBUG("write %s value 0x%016" PRIx64, r->name, q);
+               } else {
+                       LOG_DEBUG("write %s value 0x%08" PRIx32, r->name, t);
+               }
        }
 
-       armv7m->arm.core_cache->reg_list[num].valid = true;
-       armv7m->arm.core_cache->reg_list[num].dirty = false;
+       r->valid = true;
+       r->dirty = false;
 
        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;
+       r->dirty = true;
+       LOG_ERROR("Error setting register %s", r->name);
+       return retval;
 }
 
 /**
@@ -661,6 +747,7 @@ struct reg_cache *armv7m_build_reg_cache(struct target *target)
                reg_list[i].value = calloc(1, storage_size);
                reg_list[i].dirty = false;
                reg_list[i].valid = false;
+               reg_list[i].hidden = i == ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
                reg_list[i].type = &armv7m_reg_type;
                reg_list[i].arch_info = &arch_info[i];
 
@@ -669,6 +756,9 @@ struct reg_cache *armv7m_build_reg_cache(struct target *target)
                reg_list[i].exist = true;
                reg_list[i].caller_save = true; /* gdb defaults to true */
 
+               if (reg_list[i].hidden)
+                       continue;
+
                feature = calloc(1, sizeof(struct reg_feature));
                if (feature) {
                        feature->name = armv7m_regs[i].feature;

diff --git a/src/target/armv7m.h b/src/target/armv7m.h
index bd10905b84f6e95fd8f92a858d69794a91b7451a..db6f8bc5246c1ce8ce9f55c57a7ac61d97df0778 100644 (file)
--- a/src/target/armv7m.h
+++ b/src/target/armv7m.h
@@ -128,6 +128,16 @@ enum {
        ARMV7M_PSP = ARMV7M_REGSEL_PSP,
 
        /* following indices are arbitrary, do not match DCRSR.REGSEL selectors */
+
+       /* working register for packing/unpacking special regs, hidden from gdb */
+       ARMV7M_PMSK_BPRI_FLTMSK_CTRL,
+
+       /* WARNING: If you use armv7m_write_core_reg() on one of 4 following
+        * special registers, the new data go to ARMV7M_PMSK_BPRI_FLTMSK_CTRL
+        * cache only and are not flushed to CPU HW register.
+        * To trigger write to CPU HW register, add
+        *              armv7m_write_core_reg(,,ARMV7M_PMSK_BPRI_FLTMSK_CTRL,);
+        */
        ARMV7M_PRIMASK,
        ARMV7M_BASEPRI,
        ARMV7M_FAULTMASK,

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 4df903e661e210c29df09b9db3f8a96c33cbc772..00e7c3a5750a2cfeabd9b7403f6f5f1a5fd4422b 100644 (file)
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1646,35 +1646,12 @@ static int cortex_m_load_core_reg_u32(struct target *target,
                                  (int)(regsel - ARMV7M_REGSEL_S0), *value);
                        break;
 
-               case ARMV7M_PRIMASK:
-               case ARMV7M_BASEPRI:
-               case ARMV7M_FAULTMASK:
-               case ARMV7M_CONTROL:
-                       /* Cortex-M3 packages these four registers as bitfields
-                        * in one Debug Core register.  So say r0 and r2 docs;
-                        * it was removed from r1 docs, but still works.
-                        */
-                       cortexm_dap_read_coreregister_u32(target, value, ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL);
-
-                       switch (regsel) {
-                               case ARMV7M_PRIMASK:
-                                       *value = buf_get_u32((uint8_t *)value, 0, 1);
-                                       break;
-
-                               case ARMV7M_BASEPRI:
-                                       *value = buf_get_u32((uint8_t *)value, 8, 8);
-                                       break;
-
-                               case ARMV7M_FAULTMASK:
-                                       *value = buf_get_u32((uint8_t *)value, 16, 1);
-                                       break;
-
-                               case ARMV7M_CONTROL:
-                                       *value = buf_get_u32((uint8_t *)value, 24, 3);
-                                       break;
-                       }
+               case ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL:
+                       retval = cortexm_dap_read_coreregister_u32(target, value, ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL);
+                       if (retval != ERROR_OK)
+                               return retval;
 
-                       LOG_DEBUG("load from special reg %" PRIu32 " value 0x%" PRIx32 "", regsel, *value);
+                       LOG_DEBUG("load from special reg PRIMASK/BASEPRI/FAULTMASK/CONTROL value 0x%" PRIx32, *value);
                        break;
 
                default:
@@ -1688,7 +1665,6 @@ static int cortex_m_store_core_reg_u32(struct target *target,
                uint32_t regsel, uint32_t value)
 {
        int retval;
-       uint32_t reg;
        struct armv7m_common *armv7m = target_to_armv7m(target);
 
        switch (regsel) {
@@ -1728,37 +1704,10 @@ static int cortex_m_store_core_reg_u32(struct target *target,
                                  (int)(regsel - ARMV7M_REGSEL_S0), value);
                        break;
 
-               case ARMV7M_PRIMASK:
-               case ARMV7M_BASEPRI:
-               case ARMV7M_FAULTMASK:
-               case ARMV7M_CONTROL:
-                       /* Cortex-M3 packages these four registers as bitfields
-                        * in one Debug Core register.  So say r0 and r2 docs;
-                        * it was removed from r1 docs, but still works.
-                        */
-                       cortexm_dap_read_coreregister_u32(target, &reg, ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL);
-
-                       switch (regsel) {
-                               case ARMV7M_PRIMASK:
-                                       buf_set_u32((uint8_t *)&reg, 0, 1, value);
-                                       break;
-
-                               case ARMV7M_BASEPRI:
-                                       buf_set_u32((uint8_t *)&reg, 8, 8, value);
-                                       break;
-
-                               case ARMV7M_FAULTMASK:
-                                       buf_set_u32((uint8_t *)&reg, 16, 1, value);
-                                       break;
-
-                               case ARMV7M_CONTROL:
-                                       buf_set_u32((uint8_t *)&reg, 24, 3, value);
-                                       break;
-                       }
-
-                       cortexm_dap_write_coreregister_u32(target, reg, ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL);
+               case ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL:
+                       cortexm_dap_write_coreregister_u32(target, value, ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL);
 
-                       LOG_DEBUG("write special reg %" PRIu32 " value 0x%" PRIx32 " ", regsel, value);
+                       LOG_DEBUG("write special reg PRIMASK/BASEPRI/FAULTMASK/CONTROL value 0x%" PRIx32, value);
                        break;
 
                default:

diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index f012664d7bad0ab81ff0d2607e327206ce5bce77..725c2d2feb54338438e3830f9685a3e977361e32 100644 (file)
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -97,38 +97,12 @@ static int adapter_load_core_reg_u32(struct target *target,
                          (int)(regsel - ARMV7M_REGSEL_S0), *value);
                break;
 
-       case ARMV7M_PRIMASK:
-       case ARMV7M_BASEPRI:
-       case ARMV7M_FAULTMASK:
-       case ARMV7M_CONTROL:
-               /* Cortex-M3 packages these four registers as bitfields
-                * in one Debug Core register.  So say r0 and r2 docs;
-                * it was removed from r1 docs, but still works.
-                */
+       case ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL:
                retval = adapter->layout->api->read_reg(adapter->handle, ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL, value);
                if (retval != ERROR_OK)
                        return retval;
 
-               switch (regsel) {
-               case ARMV7M_PRIMASK:
-                       *value = buf_get_u32((uint8_t *) value, 0, 1);
-                       break;
-
-               case ARMV7M_BASEPRI:
-                       *value = buf_get_u32((uint8_t *) value, 8, 8);
-                       break;
-
-               case ARMV7M_FAULTMASK:
-                       *value = buf_get_u32((uint8_t *) value, 16, 1);
-                       break;
-
-               case ARMV7M_CONTROL:
-                       *value = buf_get_u32((uint8_t *) value, 24, 3);
-                       break;
-               }
-
-               LOG_DEBUG("load from special reg %" PRIu32 " value 0x%" PRIx32 "",
-                         regsel, *value);
+               LOG_DEBUG("load from special reg PRIMASK/BASEPRI/FAULTMASK/CONTROL value 0x%" PRIx32, *value);
                break;
 
        default:
@@ -142,7 +116,6 @@ static int adapter_store_core_reg_u32(struct target *target,
                uint32_t regsel, uint32_t value)
 {
        int retval;
-       uint32_t reg;
        struct armv7m_common *armv7m = target_to_armv7m(target);
        struct hl_interface_s *adapter = target_to_adapter(target);
 
@@ -186,38 +159,10 @@ static int adapter_store_core_reg_u32(struct target *target,
                          (int)(regsel - ARMV7M_REGSEL_S0), value);
                break;
 
-       case ARMV7M_PRIMASK:
-       case ARMV7M_BASEPRI:
-       case ARMV7M_FAULTMASK:
-       case ARMV7M_CONTROL:
-               /* Cortex-M3 packages these four registers as bitfields
-                * in one Debug Core register.  So say r0 and r2 docs;
-                * it was removed from r1 docs, but still works.
-                */
-
-               adapter->layout->api->read_reg(adapter->handle, ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL, &reg);
-
-               switch (regsel) {
-               case ARMV7M_PRIMASK:
-                       buf_set_u32((uint8_t *) &reg, 0, 1, value);
-                       break;
-
-               case ARMV7M_BASEPRI:
-                       buf_set_u32((uint8_t *) &reg, 8, 8, value);
-                       break;
-
-               case ARMV7M_FAULTMASK:
-                       buf_set_u32((uint8_t *) &reg, 16, 1, value);
-                       break;
-
-               case ARMV7M_CONTROL:
-                       buf_set_u32((uint8_t *) &reg, 24, 3, value);
-                       break;
-               }
-
-               adapter->layout->api->write_reg(adapter->handle, ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL, reg);
+       case ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL:
+               adapter->layout->api->write_reg(adapter->handle, ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL, value);
 
-               LOG_DEBUG("write special reg %" PRIu32 " value 0x%" PRIx32 " ", regsel, value);
+               LOG_DEBUG("write special reg PRIMASK/BASEPRI/FAULTMASK/CONTROL value 0x%" PRIx32, value);
                break;
 
        default: