Fixing low-voltage flashing on STM32F7 parts. (#567)

* fixing low voltage flashing for STM32F7
* Refactoring duplicated code

flashloaders/stm32f7.s

@@ -0,0 +1,33 @@
+.global start
+.syntax unified
+
+@ r0 = source
+@ r1 = target
+@ r2 = wordcount
+@ r3 = flash_base
+@ r4 = temp
+
+start:
+    ldr     r3, flash_base
+next:
+    cbz     r2, done
+    ldr     r4, [r0]
+    str     r4, [r1]
+    dsb     sy
+
+wait:
+    ldrh    r4, [r3, #0x0e]
+    tst.w   r4, #1
+    bne     wait
+
+    add     r0, #4
+    add     r1, #4
+    sub     r2, #1
+    b       next
+done:
+    bkpt
+
+.align 2
+
+flash_base:
+	.word 0x40023c00

flashloaders/stm32f7lv.s

@@ -0,0 +1,34 @@
+.global start
+.syntax unified
+
+@ r0 = source
+@ r1 = target
+@ r2 = wordcount
+@ r3 = flash_base
+@ r4 = temp
+
+start:
+    lsls    r2, r2, #2
+    ldr     r3, flash_base
+next:
+    cbz     r2, done
+    ldrb    r4, [r0]
+    strb    r4, [r1]
+    dsb     sy
+
+wait:
+    ldrh    r4, [r3, #0x0e]
+    tst.w   r4, #1
+    bne     wait
+
+    add     r0, #1
+    add     r1, #1
+    sub     r2, #1
+    b       next
+done:
+    bkpt
+
+.align 2
+
+flash_base:
+	.word 0x40023c00

src/flash_loader.c

@@ -156,6 +156,7 @@ static const uint8_t loader_code_stm32vl[] = {
     };
 
 	static const uint8_t loader_code_stm32f7[] = {
+        // flashloaders/stm32f7.s
         0x08, 0x4b,
         0x72, 0xb1,
         0x04, 0x68,
@@ -172,6 +173,30 @@ static const uint8_t loader_code_stm32vl[] = {
         0x00, 0x3c, 0x02, 0x40,
     };
 
+    static const uint8_t loader_code_stm32f7_lv[] = {
+        // flashloaders/stm32f7lv.s
+        0x92, 0x00,             //      lsls    r2, r2, #2
+        0x09, 0x4b,             //      ldr r3, [pc, #36]   ; (0x20000028 <flash_base>)
+                                // next:
+        0x72, 0xb1,             //      cbz     r2, 24 <done>
+        0x04, 0x78,             //      ldrb    r4, [r0, #0]
+        0x0c, 0x70,             //      strb    r4, [r1, #0]
+        0xbf, 0xf3, 0x4f, 0x8f, //      dsb sy
+                                // wait:
+        0xdc, 0x89,             //      ldrh    r4, [r3, #14]
+        0x14, 0xf0, 0x01, 0x0f, //      tst.w   r4, #1
+        0xfb, 0xd1,             //      bne.n   e <wait>
+        0x00, 0xf1, 0x01, 0x00, //      add     r0, r0, #1
+        0x01, 0xf1, 0x01, 0x01, //      add     r1, r1, #1
+        0xa2, 0xf1, 0x01, 0x02, //      sub     r2, r2, #1
+        0xef, 0xe7,             //      b       next
+                                // done:
+        0x00, 0xbe,             //      bkpt
+        0x00, 0xbf,             //      nop
+                                // flash_base:
+        0x00, 0x3c, 0x02, 0x40  //      .word   0x40023c00
+    };
+
 
 
 int stlink_flash_loader_init(stlink_t *sl, flash_loader_t *fl)
@@ -191,6 +216,37 @@ int stlink_flash_loader_init(stlink_t *sl, flash_loader_t *fl)
 	return 0;
 }
 
+static int loader_v_dependent_assignment(stlink_t *sl, 
+                                         const uint8_t **loader_code, size_t *loader_size,
+                                         const uint8_t *high_v_loader, size_t high_v_loader_size,
+                                         const uint8_t *low_v_loader, size_t low_v_loader_size)
+{
+    int retval = 0;
+
+    if( sl->version.stlink_v == 1 ) {
+        printf("STLINK V1 cannot read voltage, defaulting to 32-bit writes\n");
+        *loader_code = high_v_loader;
+        *loader_size = high_v_loader_size;
+    }
+    else {
+        int voltage = stlink_target_voltage(sl);
+        if (voltage == -1) {
+            retval = -1;
+            printf("Failed to read Target voltage\n");
+        } 
+        else {
+            if (voltage > 2700) {
+                *loader_code = high_v_loader;
+                *loader_size = high_v_loader_size;
+            } else {
+                *loader_code = low_v_loader;
+                *loader_size = low_v_loader_size;
+            }
+        }
+    }
+    return retval;
+}
+
 int stlink_flash_loader_write_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size)
 {
     const uint8_t* loader_code;
@@ -222,29 +278,25 @@ int stlink_flash_loader_write_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t*
 		sl->chip_id == STLINK_CHIPID_STM32_F413   ||
 		sl->chip_id == STLINK_CHIPID_STM32_F446
 		) {
-        if( sl->version.stlink_v == 1 ) {
+        int retval;
-            printf("STLINK V1 cannot read voltage, defaulting to 32-bit writes on F4 devices\n");
+        retval = loader_v_dependent_assignment(sl,
-            loader_code = loader_code_stm32f4;
+                                               &loader_code, &loader_size,
-            loader_size = sizeof(loader_code_stm32f4);
+                                               loader_code_stm32f4, sizeof(loader_code_stm32f4),
-        }
+                                               loader_code_stm32f4_lv, sizeof(loader_code_stm32f4_lv));
-        else {
+        if (retval == -1) {
-            int voltage = stlink_target_voltage(sl);
+            return retval;
-            if (voltage == -1) {
-                printf("Failed to read Target voltage\n");
-                return voltage;
-            } else if (voltage > 2700) {
-                loader_code = loader_code_stm32f4;
-                loader_size = sizeof(loader_code_stm32f4);
-            } else {
-                loader_code = loader_code_stm32f4_lv;
-                loader_size = sizeof(loader_code_stm32f4_lv);
-            }
         }
     } else if (sl->core_id == STM32F7_CORE_ID ||
                sl->chip_id == STLINK_CHIPID_STM32_F7 ||
                sl->chip_id == STLINK_CHIPID_STM32_F7XXXX) {
-        loader_code = loader_code_stm32f7;
+        int retval;
-        loader_size = sizeof(loader_code_stm32f7);
+        retval = loader_v_dependent_assignment(sl,
+                                               &loader_code, &loader_size,
+                                               loader_code_stm32f7, sizeof(loader_code_stm32f7),
+                                               loader_code_stm32f7_lv, sizeof(loader_code_stm32f7_lv));
+        if (retval == -1) {
+            return retval;
+        }
     } else if (sl->chip_id == STLINK_CHIPID_STM32_F0 || sl->chip_id == STLINK_CHIPID_STM32_F04 || sl->chip_id == STLINK_CHIPID_STM32_F0_CAN || sl->chip_id == STLINK_CHIPID_STM32_F0_SMALL || sl->chip_id == STLINK_CHIPID_STM32_F09X) {
         loader_code = loader_code_stm32f0;
         loader_size = sizeof(loader_code_stm32f0);