3 files changed, 23 insertions, 17 deletions
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 5dd6d7a..65a5278 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -238,23 +238,7 @@ static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
 static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
-	uint32_t dscr;
-
-	/* Read DSCR */
-	int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	if (ERROR_OK != retval)
-		return retval;
-
-	/* clear bitfield */
-	dscr &= ~bit_mask;
-	/* put new value */
-	dscr |= value & bit_mask;
-
-	/* write new DSCR */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-	return retval;
+	return armv8_set_dbgreg_bits(armv8, CPUV8_DBG_DSCR, bit_mask, value);
 }
 
 static struct target *get_aarch64(struct target *target, int32_t coreid)
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 00ab6ed..2f1d5c1 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1292,3 +1292,24 @@ int armv8_get_gdb_reg_list(struct target *target,
 		}
 	}
 }
+
+int armv8_set_dbgreg_bits(struct armv8_common *armv8, unsigned int reg, unsigned long mask, unsigned long value)
+{
+	uint32_t tmp;
+
+	/* Read register */
+	int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + reg, &tmp);
+	if (ERROR_OK != retval)
+		return retval;
+
+	/* clear bitfield */
+	tmp &= ~mask;
+	/* put new value */
+	tmp |= value & mask;
+
+	/* write new value */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + reg, tmp);
+	return retval;
+}
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 3b2fc59..1cb3a3b 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -307,6 +307,7 @@ static inline unsigned int armv8_curel_from_core_mode(enum arm_mode core_mode)
 }
 
 void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64);
+int armv8_set_dbgreg_bits(struct armv8_common *armv8, unsigned int reg, unsigned long mask, unsigned long value);
 
 extern const struct command_registration armv8_command_handlers[];