Read Cortex M4F floating point registers

Since the ST-LINK does not seem to support reading these registers, I
have implemented functions that will manually request these registers
and add them to the reg struct.

As of now, these functions are just backend and are not integrated into
anything, however I have verified that they work with the STM32F407
DISCOVERY board.

diff --git a/src/stlink-common.c b/src/stlink-common.c
index a4e2a53700d422e64de52f1f1987e7ce65506961..359d39adc691231e5115a43e9bc16267bf6ad43c 100644 (file)
--- a/src/stlink-common.c
+++ b/src/stlink-common.c
@@ -577,6 +577,11 @@ void stlink_read_all_regs(stlink_t *sl, reg *regp) {
     sl->backend->read_all_regs(sl, regp);
 }
 
+void stlink_read_all_unsupported_regs(stlink_t *sl, reg *regp) {
+    DLOG("*** stlink_read_all_unsupported_regs ***\n");
+    sl->backend->read_all_unsupported_regs(sl, regp);
+}
+
 void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx) {
     DLOG("*** stlink_write_reg\n");
     sl->backend->write_reg(sl, reg, idx);
@@ -594,6 +599,18 @@ void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
     sl->backend->read_reg(sl, r_idx, regp);
 }
 
+void stlink_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
+    DLOG("*** stlink_read_unsupported_reg\n");
+    DLOG(" (%d) ***\n", r_idx);
+
+    if (r_idx != 0x21 || r_idx < 0x40 || r_idx > 0x5f) {
+        fprintf(stderr, "Error: register address must be in [0x21, 0x40..0x5f]\n");
+        return;
+    }
+
+    sl->backend->read_unsupported_reg(sl, r_idx, regp);
+}
+
 unsigned int is_core_halted(stlink_t *sl) {
     /* return non zero if core is halted */
     stlink_status(sl);

diff --git a/src/stlink-common.h b/src/stlink-common.h
index ee59094b66d854c56a9ee99e64592d6ede0ea3db..c982b3df608cd6272bf3f800ac6b37a5be3f06e4 100644 (file)
--- a/src/stlink-common.h
+++ b/src/stlink-common.h
@@ -238,11 +238,13 @@ extern "C" {
     
     typedef struct {
         uint32_t r[16];
+        uint32_t s[32];
         uint32_t xpsr;
         uint32_t main_sp;
         uint32_t process_sp;
         uint32_t rw;
         uint32_t rw2;
+        uint32_t fpscr;
     } reg;
 
     typedef uint32_t stm32_addr_t;
@@ -295,6 +297,8 @@ extern "C" {
         void (*write_mem8) (stlink_t *sl, uint32_t addr, uint16_t len);
         void (*read_all_regs) (stlink_t *sl, reg * regp);
         void (*read_reg) (stlink_t *sl, int r_idx, reg * regp);
+        void (*read_all_unsupported_regs) (stlink_t *sl, reg *regp);
+        void (*read_unsupported_reg) (stlink_t *sl, int r_idx, reg *regp);
         void (*write_reg) (stlink_t *sl, uint32_t reg, int idx);
         void (*step) (stlink_t * stl);
         int (*current_mode) (stlink_t * stl);
@@ -360,7 +364,9 @@ extern "C" {
     void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len);
     void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len);
     void stlink_read_all_regs(stlink_t *sl, reg *regp);
+    void stlink_read_all_unsupported_regs(stlink_t *sl, reg *regp);
     void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp);
+    void stlink_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp);
     void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx);
     void stlink_step(stlink_t *sl);
     int stlink_current_mode(stlink_t *sl);

diff --git a/src/stlink-usb.c b/src/stlink-usb.c
index a25f7ec55d5b6815f28ab0e4ae242079eef842d4..397723c253c0298829e8b8c4d8749129f46085b3 100644 (file)
--- a/src/stlink-usb.c
+++ b/src/stlink-usb.c
@@ -576,6 +576,38 @@ void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
     }
 }
 
+void _stlink_usb_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
+    uint32_t r;
+
+    sl->q_buf[0] = (unsigned char) r_idx;
+    for (int i = 1; i < 4; i++) {
+        sl->q_buf[i] = 0;
+    }
+
+    _stlink_usb_write_mem32(sl, 0xE000EDF4, 4);
+    _stlink_usb_read_mem32(sl, 0xE000EDF8, 4);
+
+    r = read_uint32(sl->q_buf, 0);
+    DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
+
+    switch (r_idx) {
+        case 0x21:
+            regp->fpscr = r;
+            break;
+        default:
+            regp->s[r_idx - 0x40] = r;
+            break;
+    }
+}
+
+void _stlink_usb_read_all_unsupported_regs(stlink_t *sl, reg *regp) {
+    _stlink_usb_read_unsupported_reg(sl, 0x21, regp);
+
+    for (int i = 0; i < 32; i++) {
+        _stlink_usb_read_unsupported_reg(sl, 0x40+i, regp);
+    }
+}
+
 void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const data = sl->q_buf;
@@ -616,6 +648,8 @@ stlink_backend_t _stlink_usb_backend = {
     _stlink_usb_write_mem8,
     _stlink_usb_read_all_regs,
     _stlink_usb_read_reg,
+    _stlink_usb_read_all_unsupported_regs,
+    _stlink_usb_read_unsupported_reg,
     _stlink_usb_write_reg,
     _stlink_usb_step,
     _stlink_usb_current_mode,