diff --git a/src/deck/drivers/src/aideck.c b/src/deck/drivers/src/aideck.c index f003536604..35e64f36ed 100644 --- a/src/deck/drivers/src/aideck.c +++ b/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; diff --git a/src/utils/interface/buf2buf.h b/src/utils/interface/buf2buf.h new file mode 100644 index 0000000000..05916bdc7c --- /dev/null +++ b/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 . + * + * + * 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 +#include + +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; +} diff --git a/src/utils/src/Kbuild b/src/utils/src/Kbuild index 9135d3dcf4..67f84ac2e7 100644 --- a/src/utils/src/Kbuild +++ b/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 diff --git a/src/utils/src/buf2buf.c b/src/utils/src/buf2buf.c new file mode 100644 index 0000000000..cde9088930 --- /dev/null +++ b/src/utils/src/buf2buf.c @@ -0,0 +1,110 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( 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 . + */ + +#include +#include +#include "buf2buf.h" + +void buf2bufInit(Buf2bufContext_t* context, uint8_t* outBuf, const uint32_t outBufSize) { + context->outBuf = outBuf; + context->outBufSize = outBufSize; + context->outBufIndex = 0; + + context->inBuf = 0; + context->inBufSize = 0; + context->inBufIndex = 0; + + context->consumed = 0; + context->added = 0; +} + + +void buf2bufAddInBuf(Buf2bufContext_t* context, const uint8_t* inBuf, const uint32_t inBufSize) { + context->inBuf = inBuf; + context->inBufSize = inBufSize; + context->inBufIndex = 0; + context->added += inBufSize; +} + + +bool buf2bufConsumeInBuf(Buf2bufContext_t* context) { + const uint32_t dataLeftIn = context->inBufSize - context->inBufIndex; + const uint32_t spaceLeftOut = context->outBufSize - context->outBufIndex; + + uint32_t copySize = dataLeftIn; + if (spaceLeftOut < dataLeftIn) { + copySize = spaceLeftOut; + } + + memcpy(&context->outBuf[context->outBufIndex], &context->inBuf[context->inBufIndex], copySize); + context->inBufIndex += copySize; + context->outBufIndex += copySize; + context->consumed += copySize; + + const bool isOutBufFull = (context->outBufIndex == context->outBufSize); + if (isOutBufFull) { + context->outBufIndex = 0; + } + + return isOutBufFull; +} + + +void buf2bufReleaseInBuf(Buf2bufContext_t* context) { + context->inBuf = 0; + context->inBufSize = 0; + context->inBufIndex = 0; +} + + +uint32_t buf2bufReleaseOutBuf(Buf2bufContext_t* context) { + context->outBuf = 0; + + return context->outBufIndex; +} + + +#define OUTBUFSIZE 1000 +static uint8_t outBuf[OUTBUFSIZE]; +static Buf2bufContext_t context; +void handleInBuffer(const uint32_t memAddr, const uint8_t inBufDataLen, const uint8_t *inBuf, const uint32_t totSize) { + const bool isFirstBuf = (memAddr == 0); + if (isFirstBuf) { + buf2bufInit(&context, outBuf, OUTBUFSIZE); + } + + buf2bufAddInBuf(&context, inBuf, inBufDataLen); + while(buf2bufConsumeInBuf(&context)) { + // send(outbuf, OUTBUFSIZE); + } + buf2bufReleaseInBuf(&context); + + const bool isLastBuf = (memAddr + inBufDataLen >= totSize); + if (isLastBuf) { + uint32_t size = buf2bufReleaseOutBuf(&context); + if (size > 0) { + // send(outbuf, size); + } + } +} diff --git a/test/utils/src/test_buf2buf.c b/test/utils/src/test_buf2buf.c new file mode 100644 index 0000000000..f240114ebf --- /dev/null +++ b/test/utils/src/test_buf2buf.c @@ -0,0 +1,184 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * 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 . + * + * Unit tests for buf2buf + */ + +// Module under test +#include "buf2buf.h" + +#include "unity.h" +#include + +static Buf2bufContext_t context; + +#define IN_BUF_SIZE 100 +static uint8_t inBuf[IN_BUF_SIZE]; + +#define OUT_BUF_SIZE 100 +static uint8_t outBuf[OUT_BUF_SIZE]; + +// Helpers + +static void validateStdUseCase(uint32_t inBufSize, uint32_t nrOfInBufs, uint32_t outBufSize); + +void setUp(void) { + for (int i = 0; i < IN_BUF_SIZE; i++) { + inBuf[i] = i + 1; + } + + memset(outBuf, 0, sizeof(outBuf)); + memset(&context, 0, sizeof(context)); +} + +void tearDown(void) {} + +void testThatASmallInBufferIsNotFillingLargeOutBuffer() { + // Fixture + buf2bufInit(&context, outBuf, 10); + + // Test + buf2bufAddInBuf(&context, inBuf, 4); + bool actual = buf2bufConsumeInBuf(&context); + + // Assert + TEST_ASSERT_FALSE(actual); +} + +void testThatEqualSizeInAndOutBufferFillsOutBuffer() { + // Fixture + buf2bufInit(&context, outBuf, 10); + + // Test + buf2bufAddInBuf(&context, inBuf, 10); + bool actual = buf2bufConsumeInBuf(&context); + + // Assert + TEST_ASSERT_TRUE(actual); +} + +void testThatALargeInBufferFillsSmallerOutBuffer() { + // Fixture + buf2bufInit(&context, outBuf, 10); + + // Test + buf2bufAddInBuf(&context, inBuf, 15); + bool actual = buf2bufConsumeInBuf(&context); + + // Assert + TEST_ASSERT_TRUE(actual); +} + +void testThatMultipleSmallInBufferFillsOutBuffer() { + // Fixture + buf2bufInit(&context, outBuf, 10); + + // Test + buf2bufAddInBuf(&context, inBuf, 7); + bool actual1 = buf2bufConsumeInBuf(&context); + + buf2bufAddInBuf(&context, inBuf, 7); + bool actual2 = buf2bufConsumeInBuf(&context); + + // Assert + TEST_ASSERT_FALSE(actual1); + TEST_ASSERT_TRUE(actual2); +} + +void testThatOutBufferWithDataReportsSizeWhenReleased() { + // Fixture + buf2bufInit(&context, outBuf, 10); + + buf2bufAddInBuf(&context, inBuf, 4); + buf2bufConsumeInBuf(&context); + + buf2bufAddInBuf(&context, inBuf, 4); + buf2bufConsumeInBuf(&context); + + // Test + uint32_t actual = buf2bufReleaseOutBuf(&context); + + // Assert + TEST_ASSERT_EQUAL_UINT32(8, actual); +} + +void testStdUseCaseWithVariousConfigurations() { + // Same buffer size + validateStdUseCase(5, 5, 5); + + // Smaller in buffer than out buffer + validateStdUseCase(5, 10, 7); + + // Our buffer multiple size of in buffer + validateStdUseCase(4, 5, 8); + + // In buffer multiple size of out buffer + validateStdUseCase(8, 5, 4); + + // Larger in buffer than out buffer + validateStdUseCase(7, 10, 5); + + // Last in buffer overflows into new out buffer + validateStdUseCase(5, 2, 7); + + // Last in buffer fits in out buffer + validateStdUseCase(5, 3, 8); +} + +// Helpers ------------------------------- + +static void validateStdUseCase(uint32_t inBufSize, uint32_t nrOfInBufs, uint32_t outBufSize) { + TEST_ASSERT_LESS_OR_EQUAL(IN_BUF_SIZE, inBufSize); + TEST_ASSERT_LESS_OR_EQUAL(OUT_BUF_SIZE, outBufSize); + + // Buffers for concatenating all in and out buffers, will be compared at the end + uint8_t concatInBufs[inBufSize * nrOfInBufs]; + uint8_t concatOutBufs[inBufSize * nrOfInBufs]; + + uint32_t inIndex = 0; + uint32_t outIndex = 0; + + buf2bufInit(&context, outBuf, outBufSize); + + // Simulate a bunch of in buffers + for (uint32_t i = 0; i < nrOfInBufs; i++) { + memcpy(&concatInBufs[inIndex], inBuf, inBufSize); + inIndex += inBufSize; + + buf2bufAddInBuf(&context, inBuf, inBufSize); + while(buf2bufConsumeInBuf(&context)) { + // Store out buffers + memcpy(&concatOutBufs[outIndex], outBuf, outBufSize); + outIndex += outBufSize; + } + buf2bufReleaseInBuf(&context); + } + + // Store the last potential out buffer + uint32_t size = buf2bufReleaseOutBuf(&context); + memcpy(&concatOutBufs[outIndex], outBuf, size); + outIndex += size; + + TEST_ASSERT_EQUAL_UINT32(inIndex, outIndex); + TEST_ASSERT_EQUAL_UINT8_ARRAY(concatInBufs, concatOutBufs, inIndex); +}