Fix GAP8 flashing problems

src/deck/drivers/src/aideck.c

@@ -54,6 +54,7 @@
 #include "queue.h"
 #include "stm32fxxx.h"
 #include "system.h"
+#include "buf2buf.h"
 
 #include "cpx_internal_router.h"
 #include "cpx_external_router.h"
@@ -111,73 +112,86 @@ void cpxBootloaderMessage(const CPXPacket_t * packet) {
   xEventGroupSetBits(bootloaderSync, CPX_WAIT_FOR_BOOTLOADER_REPLY);
 }
 
-static CPXPacket_t bootPacket;
+static CPXPacket_t txPacket;
 
 #define FLASH_BUFFER_SIZE 64
 static uint8_t flashBuffer[FLASH_BUFFER_SIZE];
-static int flashBufferIndex = 0;
+static Buf2bufContext_t gap8BufContext;
 
-static bool gap8DeckFlasherWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t *buffer, const DeckMemDef_t* memDef) {
+static void sendFlashInit(const uint32_t fwSize) {
+  GAP8BlCmdPacket_t* gap8BlPacket = (GAP8BlCmdPacket_t*)txPacket.data;
 
-  cpxInitRoute(CPX_T_STM32, CPX_T_GAP8, CPX_F_BOOTLOADER, &bootPacket.route);
+  gap8BlPacket->cmd = GAP8_BL_CMD_START_WRITE;
+  gap8BlPacket->startAddress = 0x40000;
+  gap8BlPacket->writeSize = fwSize;
+  txPacket.dataLength = sizeof(GAP8BlCmdPacket_t);
+  bool writeOk = cpxSendPacketBlockingTimeout(&txPacket, M2T(GAP8_MAX_MEM_WRITE_TIMEOUT_MS));
+  ASSERT(writeOk);
+}
 
-  if (memAddr == 0) {
-    GAP8BlCmdPacket_t* gap8BlPacket = (GAP8BlCmdPacket_t*)bootPacket.data;
+static void sendFlashBuffer(const uint32_t size) {
+  memcpy(&txPacket.data, flashBuffer, size);
+  txPacket.dataLength = size;
+  bool writeOk = cpxSendPacketBlockingTimeout(&txPacket, M2T(GAP8_MAX_MEM_WRITE_TIMEOUT_MS));
+  ASSERT(writeOk);
+}
 
-    gap8BlPacket->cmd = GAP8_BL_CMD_START_WRITE;
-    gap8BlPacket->startAddress = 0x40000;
-    gap8BlPacket->writeSize = *(memDef->newFwSizeP);
-    bootPacket.dataLength = sizeof(GAP8BlCmdPacket_t);
-    bool writeOk = cpxSendPacketBlockingTimeout(&bootPacket, M2T(GAP8_MAX_MEM_WRITE_TIMEOUT_MS));
-    ASSERT(writeOk);
-  }
+static void sendFlashMd5Request(const uint32_t fwSize) {
+  GAP8BlCmdPacket_t* gap8BlPacket = (GAP8BlCmdPacket_t*)txPacket.data;
+  gap8BlPacket->cmd = GAP8_BL_CMD_MD5;
+  gap8BlPacket->startAddress = 0x40000;
+  gap8BlPacket->writeSize = fwSize;
+  txPacket.dataLength = sizeof(GAP8BlCmdPacket_t);
+  bool writeOk = cpxSendPacketBlockingTimeout(&txPacket, M2T(GAP8_MAX_MEM_WRITE_TIMEOUT_MS));
+  ASSERT(writeOk);
+}
+
+static void waitForCpxResponse() {
+  EventBits_t bits = xEventGroupWaitBits(bootloaderSync,
+                      CPX_WAIT_FOR_BOOTLOADER_REPLY,
+                      pdTRUE,  // Clear bits before returning
+                      pdFALSE, // Wait for any bit
+                      M2T(GAP8_MAX_MEM_VERIFY_TIMEOUT_MS));
+  bool flashWritten = (bits & CPX_WAIT_FOR_BOOTLOADER_REPLY);
+  ASSERT(flashWritten);
+}
+
+static bool gap8DeckFlasherWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t *buffer, const DeckMemDef_t* memDef) {
+  cpxInitRoute(CPX_T_STM32, CPX_T_GAP8, CPX_F_BOOTLOADER, &txPacket.route);
+
+  const uint32_t fwSize = *(memDef->newFwSizeP);
 
   // The GAP8 can only flash data in multiples of 4 bytes,
   // buffering will guard against this and also speed things up.
   // The full binary that will be flashed is multiple of 4.
 
-  uint32_t sizeLeftToBufferFull = sizeof(flashBuffer) - flashBufferIndex;
-  uint32_t sizeAbleToBuffer = sizeLeftToBufferFull < writeLen ? sizeLeftToBufferFull : writeLen;
-  uint32_t lastAddressToWrite = memAddr + sizeAbleToBuffer;
-
-  memcpy(&flashBuffer[flashBufferIndex], buffer, sizeAbleToBuffer);
-  flashBufferIndex += sizeAbleToBuffer;
-
-  if (flashBufferIndex == sizeof(flashBuffer) || lastAddressToWrite == *(memDef->newFwSizeP)) {
-    memcpy(&bootPacket.data, flashBuffer, flashBufferIndex);
-    bootPacket.dataLength = flashBufferIndex;
+  const bool isFirstBuf = (memAddr == 0);
+  if (isFirstBuf) {
+    sendFlashInit(fwSize);
+    buf2bufInit(&gap8BufContext, flashBuffer, FLASH_BUFFER_SIZE);
+  }
 
-    bool writeOk = cpxSendPacketBlockingTimeout(&bootPacket, M2T(GAP8_MAX_MEM_WRITE_TIMEOUT_MS));
-    ASSERT(writeOk);
+  buf2bufAddInBuf(&gap8BufContext, buffer, writeLen);
+  ASSERT(buf2bufBytesAdded(&gap8BufContext) <= fwSize);
+  while(buf2bufConsumeInBuf(&gap8BufContext)) {
+    sendFlashBuffer(FLASH_BUFFER_SIZE);
+  }
+  buf2bufReleaseInBuf(&gap8BufContext);
 
-    flashBufferIndex = 0;
-    int sizeLeftToBuffer = writeLen - sizeLeftToBufferFull;
-    if (sizeLeftToBuffer > 0) {
-      memcpy(&flashBuffer[flashBufferIndex], &buffer[sizeLeftToBufferFull], sizeLeftToBuffer);
-      flashBufferIndex += sizeLeftToBuffer;
+  const bool isLastBuf = (buf2bufBytesConsumed(&gap8BufContext) == fwSize);
+  if (isLastBuf) {
+    uint32_t size = buf2bufReleaseOutBuf(&gap8BufContext);
+    if (size > 0) {
+      sendFlashBuffer(size);
     }
-  }
+    ASSERT(buf2bufBytesAdded(&gap8BufContext) == buf2bufBytesConsumed(&gap8BufContext));
 
-  if (memAddr + writeLen == *(memDef->newFwSizeP)) {
     // Request the MD5 checksum of the flashed data. This is only done
     // for synchronizing and making sure everything has been written,
     // we do not care about the results.
-    GAP8BlCmdPacket_t* gap8BlPacket = (GAP8BlCmdPacket_t*)bootPacket.data;
-    gap8BlPacket->cmd = GAP8_BL_CMD_MD5;
-    gap8BlPacket->startAddress = 0x40000;
-    gap8BlPacket->writeSize = *(memDef->newFwSizeP);
-    bootPacket.dataLength = sizeof(GAP8BlCmdPacket_t);
-    bool writeOk = cpxSendPacketBlockingTimeout(&bootPacket, M2T(GAP8_MAX_MEM_WRITE_TIMEOUT_MS));
-    ASSERT(writeOk);
-
-    EventBits_t bits = xEventGroupWaitBits(bootloaderSync,
-                        CPX_WAIT_FOR_BOOTLOADER_REPLY,
-                        pdTRUE,  // Clear bits before returning
-                        pdFALSE, // Wait for any bit
-                        M2T(GAP8_MAX_MEM_VERIFY_TIMEOUT_MS));
-    bool flashWritten = (bits & CPX_WAIT_FOR_BOOTLOADER_REPLY);
-    ASSERT(flashWritten);
-}
+    sendFlashMd5Request(fwSize);
+    waitForCpxResponse();
+  }
 
   return true;
 }
@@ -186,14 +200,14 @@ static bool gap8DeckFlasherWrite(const uint32_t memAddr, const uint8_t writeLen,
 static bool isGap8InBootloaderMode = false;
 
 static void resetGap8ToBootloader() {
-  cpxInitRoute(CPX_T_STM32, CPX_T_ESP32, CPX_F_SYSTEM, &bootPacket.route);
+  cpxInitRoute(CPX_T_STM32, CPX_T_ESP32, CPX_F_SYSTEM, &txPacket.route);
 
-  ESP32SysPacket_t* esp32SysPacket = (ESP32SysPacket_t*)bootPacket.data;
+  ESP32SysPacket_t* esp32SysPacket = (ESP32SysPacket_t*)txPacket.data;
 
   esp32SysPacket->cmd = ESP32_SYS_CMD_RESET_GAP8;
-  bootPacket.dataLength = sizeof(ESP32SysPacket_t);
+  txPacket.dataLength = sizeof(ESP32SysPacket_t);
 
-  cpxSendPacketBlocking(&bootPacket);
+  cpxSendPacketBlocking(&txPacket);
   // This should be handled on RX on CPX instead
   vTaskDelay(100);
   isGap8InBootloaderMode = true;

src/utils/interface/buf2buf.h

@@ -0,0 +1,120 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2022 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ * buf2buf.h - utility for copying buffers when transferring data from one buffer size to another.
+ * Incoming buffers have one size while outgoing buffers have a different size.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef struct {
+    const uint8_t* inBuf;
+    uint32_t inBufIndex;
+    uint32_t inBufSize;
+    uint32_t added;
+
+    uint8_t* outBuf;
+    uint32_t outBufIndex;
+    uint32_t outBufSize;
+    uint32_t consumed;
+} Buf2bufContext_t;
+
+/**
+ * @brief Initialize a context
+ *
+ * This function must be called first to initialize the context.
+ * A pointer to the out-buffer will be stored in the context and the out-buffer should not be modified until released.
+ *
+ * @param context A new context
+ * @param outBuf Pointer to the out buffer (reused over and over)
+ * @param outBufSize The size of the out buffer
+ */
+void buf2bufInit(Buf2bufContext_t* context, uint8_t* outBuf, const uint32_t outBufSize);
+
+/**
+ * @brief Add a new in-buffer
+ *
+ * Call this function when a new in-buffer is available. The pointer to the in-buffer will be stored in the context
+ * and the in-buffer should not be modified until released by a call to buf2bufReleaseInBuf().
+ *
+ * @param context The context
+ * @param inBuf Pointer to the in-buffer
+ * @param inBufSize The size of the in-buffer
+ */
+void buf2bufAddInBuf(Buf2bufContext_t* context, const uint8_t* inBuf, const uint32_t inBufSize);
+
+/**
+ * @brief Consume data from the in-buffer
+ *
+ * Copy data from the in-buffer to the out-buffer, may have to be called multiple times if the out-buffer can not
+ * hold all the data from the in-buffer. If this function returns true, the out-buffer is full and is ready to be
+ * emptied by the application code. This function should be called repeatedly until it returns false which indicates
+ * that the in-buffer is fully consumed.
+ *
+ * @param context The context
+ * @return true The out buffer is full and ready to be used
+ * @return false The in buffer is fully consumed
+ */
+bool buf2bufConsumeInBuf(Buf2bufContext_t* context);
+
+/**
+ * @brief Release the in-buffer
+ *
+ * Call when in-buffer has been fully consumed to release the in-buffer.
+ *
+ * @param context The context
+ */
+void buf2bufReleaseInBuf(Buf2bufContext_t* context);
+
+/**
+ * @brief Release the out-buffer
+ *
+ * Will release the out-buffer and return the number of bytes of data that remains to be emptied by the application
+ * code.
+ *
+ * @param context
+ * @return uint32_t
+ */
+uint32_t buf2bufReleaseOutBuf(Buf2bufContext_t* context);
+
+/**
+ * @brief The total number of bytes added by in-buffers
+ *
+ * @param context The context
+ * @return uint32_t The number of bytes
+ */
+static inline uint32_t buf2bufBytesAdded(const Buf2bufContext_t* context) {
+    return context->added;
+}
+
+/**
+ * @brief The total number of bytes consumed from in-buffers
+ *
+ * @param context The context
+ * @return uint32_t The number of bytes
+ */
+static inline uint32_t buf2bufBytesConsumed(const Buf2bufContext_t* context) {
+    return context->consumed;
+}

src/utils/src/Kbuild

@@ -5,7 +5,7 @@ obj-y += cpuid.o
 obj-y += crc32.o
 obj-y += debug.o
 obj-y += eprintf.o
-
+obj-y += buf2buf.o
 ### Implementations for abort(), malloc() and free(), needed to interact with apps written in C++
 obj-y += abort.o
 obj-y += malloc.o