Properly detect and warn if multiple stlinky magic bytes are detected

[fw/stlink] / src / st-term.c

diff --git a/src/st-term.c b/src/st-term.c
index 9be3263afde7d8d4b8771810d2c4825a7fa2f4a1..a6a97fc1c4e6988fc2970c0c651c3262c3e91c3b 100644 (file)
--- a/src/st-term.c
+++ b/src/st-term.c
@@ -1,4 +1,5 @@
 #include <stdio.h>
+#include <stdlib.h>
 /* According to POSIX.1-2001 */
 #include <sys/select.h>
 #include <string.h>
@@ -12,6 +13,11 @@
 
 #define STLINKY_MAGIC 0xDEADF00D
 
+#define READ_UINT32_LE(buf)  ((uint32_t) (   buf[0]         \
+                                           | buf[1] <<  8   \
+                                           | buf[2] << 16   \
+                                           | buf[3] << 24))
+
 struct stlinky {
        stlink_t *sl;
        uint32_t off;
@@ -24,20 +30,29 @@ struct stlinky*  stlinky_detect(stlink_t* sl)
 {
        static const uint32_t sram_base = 0x20000000;
        struct stlinky* st = malloc(sizeof(struct stlinky));
+       int multiple=0;
        st->sl = sl;
-       printf("sram: 0x%x bytes @ 0x%x\n", sl->sram_base, sl->sram_size);
+       printf("sram: 0x%x bytes @ 0x%zx\n", sl->sram_base, sl->sram_size);
        uint32_t off;
        for (off = 0; off < sl->sram_size; off += 4) {
                stlink_read_mem32(sl, sram_base + off, 4);
-               if ( STLINKY_MAGIC== *(uint32_t*) sl->q_buf)
+               if (STLINKY_MAGIC == READ_UINT32_LE(sl->q_buf))
                {
+                       if (multiple > 0)
+                               printf("WARNING: another ");
                        printf("stlinky detected at 0x%x\n", sram_base + off);
                        st->off = sram_base + off;
                        stlink_read_mem32(sl, st->off + 4, 4);
                        st->bufsize = (size_t) *(unsigned char*) sl->q_buf;
                        printf("stlinky buffer size 0x%zu \n", st->bufsize);
-                       return st;
+                       multiple++;
+               }
+       }
+       if (multiple > 0) {
+               if (multiple > 1) {
+                       printf("Using last stlinky structure detected\n");
                }
+               return st;
        }
        return NULL;
 }
@@ -120,6 +135,7 @@ static int keep_running = 1;
 static int sigcount=0;
 void cleanup(int dummy)
 {
+       (void) dummy;
        sigcount++;
        keep_running = 0;
        printf("\n\nGot a signal\n");