From bd3472751be353b9e220bfa2fa59bacb4d6b5b57 Mon Sep 17 00:00:00 2001
From: Karl Palsson <karlp@tweak.net.au>
Date: Wed, 12 Oct 2011 20:40:24 +0000
Subject: [PATCH] Support arm core cpuid register decoding

---
 src/stlink-common.c | 17 +++++++++++++++--
 src/stlink-common.h | 11 +++++++++++
 src/stlink-usb.c    |  3 ++-
 src/test_usb.c      |  8 ++++++--
 4 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/src/stlink-common.c b/src/stlink-common.c
index 0aadb3d..707316c 100644
--- a/src/stlink-common.c
+++ b/src/stlink-common.c
@@ -258,7 +258,6 @@ static void disable_flash_read_protection(stlink_t *sl) {
 void stlink_close(stlink_t *sl) {
     D(sl, "\n*** stlink_close ***\n");
     sl->backend->close(sl);
-
     free(sl);
 }
 
@@ -299,10 +298,24 @@ uint16_t stlink_chip_id(stlink_t *sl) {
     return chip_id;
 }
 
+/**
+ * Cortex m3 tech ref manual, CPUID register description
+ * @param sl stlink context
+ * @param cpuid pointer to the result object
+ */
+void stlink_cpu_id(stlink_t *sl, cortex_m3_cpuid_t *cpuid) {
+    stlink_read_mem32(sl, CM3_REG_CPUID, 4);
+    uint32_t raw = read_uint32(sl->q_buf, 0);
+    cpuid->implementer_id = (raw >> 24) & 0x7f;
+    cpuid->variant = (raw >> 20) & 0xf;
+    cpuid->part = (raw >> 4) & 0xfff;
+    cpuid->revision = raw & 0xf;
+    return;
+}
+
 void stlink_reset(stlink_t *sl) {
     D(sl, "\n*** stlink_reset ***\n");
     sl->backend->reset(sl);
-
 }
 
 void stlink_run(stlink_t *sl) {
diff --git a/src/stlink-common.h b/src/stlink-common.h
index 7c3c0cb..b357c7a 100644
--- a/src/stlink-common.h
+++ b/src/stlink-common.h
@@ -71,6 +71,9 @@ extern "C" {
 #define STLINK_SWD_ENTER 0x30
 #define STLINK_SWD_READCOREID 0x32  // TBD
 
+// cortex m3 technical reference manual
+#define CM3_REG_CPUID 0xE000ED00
+
     typedef struct {
         uint32_t r[16];
         uint32_t xpsr;
@@ -81,6 +84,13 @@ extern "C" {
     } reg;
 
     typedef uint32_t stm32_addr_t;
+    
+    typedef struct _cortex_m3_cpuid_ {
+        uint16_t implementer_id;
+        uint16_t variant;
+        uint16_t part;
+        uint8_t revision;
+    } cortex_m3_cpuid_t;
 
     typedef struct stlink_version_ {
         uint32_t stlink_v;
@@ -197,6 +207,7 @@ extern "C" {
     
     // PUBLIC
     uint16_t stlink_chip_id(stlink_t *sl);
+    void stlink_cpu_id(stlink_t *sl, cortex_m3_cpuid_t *cpuid);
 
     // privates, publics, the rest....
     // TODO sort what is private, and what is not
diff --git a/src/stlink-usb.c b/src/stlink-usb.c
index 16f9343..895b368 100644
--- a/src/stlink-usb.c
+++ b/src/stlink-usb.c
@@ -528,4 +528,5 @@ on_error:
     if (sl != NULL) free(sl);
     if (slu != NULL) free(slu);
     return 0;
-}
\ No newline at end of file
+}
+
diff --git a/src/test_usb.c b/src/test_usb.c
index 0c4232f..3d31453 100644
--- a/src/test_usb.c
+++ b/src/test_usb.c
@@ -23,8 +23,12 @@ int main(int ac, char** av) {
         printf("-- mode after entering swd mode: %d\n", stlink_current_mode(sl));
 
         printf("-- chip id: %#x\n", stlink_chip_id(sl));
-        printf("-- core_id\n");
-        stlink_core_id(sl);
+        printf("-- core_id: %#x\n", stlink_core_id(sl));
+
+        cortex_m3_cpuid_t cpuid;
+        stlink_cpu_id(sl, &cpuid);
+        printf("cpuid:impl_id = %0#x, variant = %#x\n", cpuid.implementer_id, cpuid.variant);
+        printf("cpuid:part = %#x, rev = %#x\n", cpuid.part, cpuid.revision);
 
         printf("-- read_sram\n");
         static const uint32_t sram_base = 0x8000000;
-- 
2.30.2