X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fcortex_m.c;h=38ed4c3a1f5f6ab32ed6460266482d89336331ab;hp=d3b8f4e080f5c18583a191024e398e4b62990e8d;hb=217403ce096627fec035b79f9464e5bfc09e8f06;hpb=24e99ac6d9a8a7672da832ec54e015b66aef246c

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index d3b8f4e080..38ed4c3a1f 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:
@@ -1839,6 +1885,17 @@ int cortex_m_examine(struct target *target)
 			armv7m->arm.is_armv6m = true;
 		}
 
+		if (armv7m->fp_feature != FPv4_SP &&
+		    armv7m->arm.core_cache->num_regs > ARMV7M_NUM_CORE_REGS_NOFP) {
+			/* free unavailable FPU registers */
+			size_t idx;
+			for (idx = ARMV7M_NUM_CORE_REGS_NOFP;
+			     idx < armv7m->arm.core_cache->num_regs;
+			     idx++)
+				free(armv7m->arm.core_cache->reg_list[idx].value);
+			armv7m->arm.core_cache->num_regs = ARMV7M_NUM_CORE_REGS_NOFP;
+		}
+
 		if (i == 4 || i == 3) {
 			/* Cortex-M3/M4 has 4096 bytes autoincrement range */
 			armv7m->dap.tar_autoincr_block = (1 << 12);