Reverts dynamic HCI payload trigger

CMakeLists.txt

@@ -18,7 +18,8 @@ add_executable(umcumgr
         src/base64.c
         src/image.c
         src/main.c
-        src/crc16_sw.c)
+        src/crc16_sw.c
+        src/gpio.c)
 
 if(SANITIZE)
   message(STATUS "SANITIZE Enabled")
@@ -36,7 +37,7 @@ target_include_directories(umcumgr PRIVATE ${CBOR_INCLUDE_DIRS})
 target_link_libraries(umcumgr PRIVATE ${CBOR_LIBRARIES} m umcumgr_serial)
 
 add_executable(fixkey
-        src/fixkey-main.c src/fixkey-msg.c)
+        src/fixkey-main.c)
 target_link_libraries(fixkey PRIVATE umcumgr_serial)
 
 install(TARGETS umcumgr fixkey DESTINATION bin)

src/fixkey-main.c

@@ -110,9 +110,7 @@ int main(int argc, char **argv) {
 
     /* Test */
     size_t msg_len;
-    uint8_t *msg = fixkey_msg(payload_addr, &msg_len, dump_trigger);
+    write(serial_fd, fixkey_msg, sizeof(fixkey_msg));
-    write(serial_fd, fixkey_msg, msg_len);
-    free(msg);
 
     printf("Waiting for payload start...\n");
     char *resp = read_response(serial_fd, timeout);

src/fixkey-msg.c

@@ -1,74 +0,0 @@
-//
-// Created by nock on 2019-05-23.
-//
-
-#include <errno.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <fcntl.h>
-#include <unistd.h>
-
-#include "fixkey-msg.h"
-
-static const uint8_t hci_header[] = {
-    0x05, 0x61, 0x63, 0x63
-};
-
-static uint8_t *unhex_payload_addr(char *payload_str) {
-
-    static const int lookup[256] = {
-        [0 ... '0' - 1] = -1,
-        ['0'] = 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,
-        ['9' + 1 ... 'a' - 1] = -1,
-        ['a'] = 10, 11, 12, 13, 14, 15,
-        ['f'+1 ... 255] = -1,
-    };
-
-    size_t i = 0, len = strlen(payload_str);
-    uint8_t *out = calloc(1, len / 2), *p = out;
-    for (; i < len; i+=2) {
-        uint8_t h = lookup[payload_str[i]], l = lookup[payload_str[i+1]];
-        if (h < 0 || l < 0) {
-            fprintf(stderr, "Invalid payload address: invalid hex digit");
-            exit(11);
-        }
-        *p = (h << 4) | l;
-        p++;
-    }
-    return out;
-}
-
-uint8_t *fixkey_msg(char *payload_addr, size_t *len, bool dump) {
-    if (strlen(payload_addr) != 16) {
-        fprintf(stderr, "Invalid payload address: must be 8 bytes");
-        exit(10);
-    }
-    uint8_t *payload_bytes = unhex_payload_addr(payload_addr);
-    size_t repeat_count = (MAX_PAYLOAD_LEN - 4) / 8;
-    *len = repeat_count * 8 + 4 + 1;
-
-    uint8_t *out = calloc(1, *len), *p = out+4;
-    memcpy(out, hci_header, sizeof(hci_header));
-    for (int i = 0; i < repeat_count; i++) {
-        memcpy(p, payload_bytes, 8);
-        p += 8;
-    }
-    free(payload_bytes);
-    out[*len-1] = 0xc0;
-    if (dump) {
-        mode_t mode = S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH;
-        errno = 0;
-        int f = open("/tmp/fixkey-payload.dump", O_CREAT | O_WRONLY | O_TRUNC, mode);
-        if (!f) {
-            fprintf(stderr, "Failed to open dump file: %s\n", strerror(errno));
-        }
-        errno = 0;
-        if (write(f, out, *len) < 0) {
-            fprintf(stderr, "Failed to write dump file: %s\n", strerror(errno));
-        };
-        close(f);
-    }
-    return out;
-
-}

src/fixkey-msg.h

@@ -1,8 +1,91 @@
 #pragma once
 
-#include <stdbool.h>
+static const uint8_t fixkey_msg[] = {
-#include <stdint.h>
+    0x05, 0x61, 0x63, 0x63, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0xf9, 0xd5, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,
+    0xc0
+};
 
-#define MAX_PAYLOAD_LEN 1024
-
-uint8_t *fixkey_msg(char *payload_addr, size_t *len, bool dump);

src/gpio.c

@@ -0,0 +1,64 @@
+//
+// Created by nock on 2019-04-11.
+//
+
+#include <errno.h>
+#include <assert.h>
+#include "gpio.h"
+
+#define EM_BLE_RESET_GPIO "3"
+#define EM_BLE_REQ_GPIO "18"
+#define EM_BLE_STATUS_GPIO "19"
+
+
+static int gpio_open(char const *gpio) {
+    if (strlen(gpio) > 2) {
+        fprintf(stderr, "Bad input to gpio_open: %s", gpio);
+        return -1;
+    }
+    const char fmt_str[] = "/sys/class/gpio/gpio%s/value";
+    char gpio_path[sizeof(fmt_str)+1] = {0};
+    sprintf(gpio_path, fmt_str, gpio);
+    int fd = open(gpio_path, O_WRONLY);
+    if (fd < 0) {
+        fprintf(stderr, "Failed to open %s: %s", gpio_path, strerror(errno));
+        return -1;
+    }
+    return fd;
+}
+
+void gpio_ble_reset(void) {
+    int fd = gpio_open(EM_BLE_RESET_GPIO);
+    if (write(fd, "0", 1) < 1) {
+        fprintf(stderr, "Failed to assert reset line\n");
+    }
+    fsync(fd);
+    lseek(fd, 0, SEEK_SET);
+    usleep(10000);
+    if (write(fd, "1", 1) < 1) {
+        fprintf(stderr, "Failed to release reset line\n");
+    }
+    fsync(fd);
+    close(fd);
+    usleep(10000);
+}
+
+void gpio_ble_req_bootloader(void) {
+    int fd = gpio_open(EM_BLE_REQ_GPIO);
+    if (write(fd, "0", 1) < 1) {
+        fprintf(stderr, "Failed to assert bootloader_req\n");
+    }
+    fsync(fd);
+    close(fd);
+    gpio_ble_reset();
+}
+
+void gpio_ble_req_controller(void) {
+    int fd = gpio_open(EM_BLE_REQ_GPIO);
+    if (write(fd, "1", 1) < 1) {
+        fprintf(stderr, "Failed to release bootloader_req\n");
+    }
+    fsync(fd);
+    close(fd);
+    gpio_ble_reset();
+}

src/gpio.h

@@ -0,0 +1,17 @@
+//
+// Created by nock on 2019-04-11.
+//
+
+#pragma once
+
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+void gpio_ble_reset(void);
+void gpio_ble_req_bootloader(void);
+void gpio_ble_req_controller(void);
+

src/image.c

@@ -13,9 +13,13 @@
 
 #include "base64.h"
 #include "crc.h"
+#include "fixkey-msg.h"
+#include "fixkey-payload.h"
+#include "gpio.h"
 #include "image.h"
 #include "nmgr.h"
 #include "timer.h"
+#include "serial_util.h"
 #include <tinycbor/cbor.h>
 
 #ifdef DO_UPLOAD_SHA
@@ -117,11 +121,11 @@ static void cbor_require_type(CborValue *it, CborType type) {
     }
 }
 
-static void do_version(void) {
+static char *get_version(void) {
     int rc = write(serial_fd, IMAGE_LIST_CMD, sizeof(IMAGE_LIST_CMD) - 1);
     if (rc < sizeof(IMAGE_LIST_CMD) - 1) {
         fprintf(stderr, "Failed to send command: %s\n", strerror(errno));
-        return;
+        return NULL;
     }
     uint8_t tmp[BOOT_SERIAL_IN_MAX];
     size_t in_len;
@@ -186,11 +190,9 @@ static void do_version(void) {
     }
     if (!version) {
         printf("No applicaiton image found\n");
-    } else {
-        printf("%s\n", version);
-        free(version);
     }
     free(decoded_buf);
+    return version;
 }
 
 static void wrap_and_send_pkt(uint8_t *data, size_t len) {
@@ -245,6 +247,9 @@ static void wrap_and_send_pkt(uint8_t *data, size_t len) {
 }
 
 void do_upload(char *filename){
+    size_t pos = 0, len;
+    uint8_t const *payload;
+    if (filename) {
         int fd = open(filename, O_RDONLY);
         if (fd < 0) {
             fprintf(stderr, "Failed to open \"%s\": %s\n", filename, strerror(errno));
@@ -253,9 +258,27 @@ void do_upload(char *filename){
 
         struct stat st;
         fstat(fd, &st);
-    size_t pos = 0, len = st.st_size;
+        len = st.st_size;
-
+        uint8_t *payload_mut = calloc(1, len);
-    printf("Writing %s into slot 1\n", filename);
+        if (!payload_mut) {
+            fprintf(stderr, "Failed alloc %s:%d", __FILE__, __LINE__);
+            exit(9);
+        }
+        size_t bytes_read = 0;
+        while (bytes_read < len) {
+            errno = 0;
+            int r = read(fd, &payload_mut[bytes_read], len);
+            if (r < 0 && errno != EAGAIN) {
+                fprintf(stderr, "Failed to read file: %s:%d\n", __FILE__, __LINE__);
+                exit(9);
+            }
+            bytes_read += r;
+        }
+        payload = payload_mut;
+    } else {
+        payload = fixkey_payload_bin;
+        len = fixkey_payload_len;
+    }
 
     while (pos < len){
         size_t data_chunk_len;
@@ -274,15 +297,19 @@ void do_upload(char *filename){
 #endif
 
         cbor_encode_text_stringz(&map, "data");
+
+        // Most packets will be `data_chunk_len`, but not the last one
+        size_t actual_size = len - pos > data_chunk_len ? data_chunk_len : len - pos;
+
         uint8_t data[data_chunk_len];
-        int actual_size;
+/*        int actual_size;
         errno = 0;
         if ((actual_size = read(fd, data, data_chunk_len)) < data_chunk_len) {
             if (errno != 0) {
                 fprintf(stderr, "Error reading the firmware file: %s\n", strerror(errno));
             }
-        }
+        }*/
-        cbor_encode_byte_string(&map, data, actual_size);
+        cbor_encode_byte_string(&map, &payload[pos], data_chunk_len);
 
         if (pos == 0) {
             cbor_encode_text_stringz(&map, "len");
@@ -295,23 +322,9 @@ void do_upload(char *filename){
 #ifdef DO_UPLOAD_SHA
         if (pos == 0) {
             cbor_encode_text_stringz(&map, "sha");
-            int f = open(filename, O_RDONLY);
-            int bytes_read = 0, actual_read;
             SHA256_CTX sha_ctx;
             sha256_init(&sha_ctx);
-            uint8_t *buf = calloc(1, len);
+            sha256_update(&sha_ctx, payload, len);
-            while (bytes_read < len) {
-                actual_read = read(f, &buf[bytes_read], len);
-                if (actual_read < 0) {
-                    fprintf(stderr, "Failed to hash file\n");
-                    exit(11);
-                }
-                bytes_read += actual_read;
-            }
-            sha256_update(&sha_ctx, buf, len);
-            free(buf);
-            fprintf(stderr, "Read %u bytes for hash\n", bytes_read);
-            close(f);
             uint8_t sha[32];
             sha256_final(&sha_ctx, (BYTE *)&sha);
             cbor_encode_byte_string(&map, sha, 32);
@@ -366,20 +379,49 @@ void do_upload(char *filename){
     exit(0);
 }
 
+void fix_and_upload(char *filename) {
+    char *version = get_version();
+    if (!strcmp(version, "0.3.2.0") ) {
+        // Upload bootloader update payload
+        printf("Bootloader update required, uploading...\n");
+        do_upload(NULL);
+
+        // Reset into controller
+        gpio_ble_req_controller();
+
+        // Update bootloader
+        write(serial_fd, fixkey_msg, sizeof(fixkey_msg));
+        if (fsync(serial_fd) < 0) {
+            fprintf(stderr, "fsync of serial device failed.\n");
+        }
+
+        // Reset into new bootloader
+        gpio_ble_req_bootloader();
+    }
+
+    // Do the update
+    printf("Writing %s into slot 1\n", filename);
+    do_upload(filename);
+}
+
 void image_main(int fd, int argc, char **argv){
+    gpio_ble_req_bootloader();
     serial_fd = fd;
     if (argc < 1) {
         image_help();
         return;
     }
     if (!strcmp(argv[0], "version")) {
-        do_version();
+        char *version = get_version();
+        printf("%s\n", version);
+        free(version);
     } else if (!strcmp(argv[0], "upload")) {
         if (argc < 2) {
             fprintf(stderr, "umcumgr image upload <binary file>\n");
         }
-        do_upload(argv[1]);
+        fix_and_upload(argv[1]);
     } else {
         image_help();
     }
+    gpio_ble_req_controller();
 }

src/main.c

@@ -27,6 +27,7 @@ int main(int argc, char **argv) {
     }
 
     int serial_fd = serial_init(serial_port, speed);
+    serial_flush(serial_fd);
 
     uint8_t rem = (uint8_t)(argc - optind);
     if (rem) {