From a71f48db1055dd7cbd73260e089a63e86afaebb8 Mon Sep 17 00:00:00 2001
From: Michael Pratt <michael@pratt.im>
Date: Sun, 8 Jul 2012 21:04:30 -0400
Subject: [PATCH] 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 | 17 +++++++++++++++++
 src/stlink-common.h |  6 ++++++
 src/stlink-usb.c    | 34 ++++++++++++++++++++++++++++++++++
 3 files changed, 57 insertions(+)

diff --git a/src/stlink-common.c b/src/stlink-common.c
index a4e2a53..359d39a 100644
--- 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 ee59094..c982b3d 100644
--- 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 a25f7ec..397723c 100644
--- 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,
-- 
2.30.2