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.

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);

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);

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,