diff --git a/hal_st/stm32fxxx/FlashInternalStm.cpp b/hal_st/stm32fxxx/FlashInternalStm.cpp index 21fb49f5..d5cd6571 100644 --- a/hal_st/stm32fxxx/FlashInternalStm.cpp +++ b/hal_st/stm32fxxx/FlashInternalStm.cpp @@ -12,10 +12,10 @@ namespace hal { HAL_FLASH_Unlock(); -#if defined(STM32WB) || defined(STM32G4) || defined(STM32G0) +#if defined(STM32WBA) + AlignedWriteBuffer(buffer, address); +#elif defined(STM32WB) || defined(STM32G4) || defined(STM32G0) AlignedWriteBuffer(buffer, address); -#elif defined(STM32WBA) - AlignedWriteBuffer(buffer, address); #else uint32_t word; while (buffer.size() >= sizeof(word) && ((address & (sizeof(word) - 1)) == 0)) @@ -30,8 +30,6 @@ namespace hal #if defined(STM32F0) || defined(STM32F3) AlignedWriteBuffer(buffer, address); -#elif defined(STM32WBA) - AlignedWriteBuffer(buffer, address); #elif !defined(STM32WB) && !defined(STM32G4) && !defined(STM32G0) && !defined(STM32WBA) for (uint8_t byte : buffer) { @@ -107,6 +105,29 @@ namespace hal } } +#ifdef STM32WBA + const uint8_t alignment = sizeof(uint64_t) * 2; + void FlashInternalStmBase::AlignedWriteBuffer(infra::ConstByteRange buffer, uint32_t address) + { + services::FlashAlign::WithAlignment flashAlign; + flashAlign.Align(address, buffer); + + services::FlashAlign::Chunk* chunk = flashAlign.First(); + auto dataSize = chunk->data.size(); + while (chunk != nullptr) + { + really_assert(chunk->data.size() % sizeof(alignment) == 0); + auto fullAddress = reinterpret_cast(flashMemory.begin() + chunk->alignedAddress); + + uint32_t addr = reinterpret_cast(chunk->data.begin()); + auto result = HAL_FLASH_Program(FLASH_TYPEPROGRAM_QUADWORD, fullAddress, addr); + really_assert(result == HAL_OK); + fullAddress += alignment; + chunk = flashAlign.Next(); + } + } +#endif + FlashInternalStm::FlashInternalStm(infra::MemoryRange sectorSizes, infra::ConstByteRange flashMemory) : FlashInternalStmBase(flashMemory) , sectorSizes(sectorSizes) diff --git a/hal_st/stm32fxxx/FlashInternalStm.hpp b/hal_st/stm32fxxx/FlashInternalStm.hpp index 6b65c1ab..dfebf11e 100644 --- a/hal_st/stm32fxxx/FlashInternalStm.hpp +++ b/hal_st/stm32fxxx/FlashInternalStm.hpp @@ -21,6 +21,9 @@ namespace hal private: template void AlignedWriteBuffer(infra::ConstByteRange buffer, uint32_t address); +#ifdef STM32WBA + void AlignedWriteBuffer(infra::ConstByteRange buffer, uint32_t address); +#endif private: infra::ConstByteRange flashMemory; diff --git a/hal_st/synchronous_stm32fxxx/SynchronousFlashInternalStm.cpp b/hal_st/synchronous_stm32fxxx/SynchronousFlashInternalStm.cpp index 825a856a..10e4c51a 100644 --- a/hal_st/synchronous_stm32fxxx/SynchronousFlashInternalStm.cpp +++ b/hal_st/synchronous_stm32fxxx/SynchronousFlashInternalStm.cpp @@ -11,7 +11,9 @@ namespace hal { HAL_FLASH_Unlock(); -#if defined(STM32WB) || defined(STM32G4) || defined(STM32G0) +#if defined(STM32WBA) + AlignedWriteBuffer(buffer, address); +#elif defined(STM32WB) || defined(STM32G4) || defined(STM32G0) AlignedWriteBuffer(buffer, address); #elif defined(STM32WBA) AlignedWriteBuffer(buffer, address); @@ -29,9 +31,7 @@ namespace hal #if defined(STM32F0) || defined(STM32F3) AlignedWriteBuffer(buffer, address); -#elif defined(STM32WBA) - AlignedWriteBuffer(buffer, address); -#elif !defined(STM32WB) && !defined(STM32G4) && !defined(STM32G0) +#elif !defined(STM32WB) && !defined(STM32G4) && !defined(STM32G0) && !defined(STM32WBA) for (uint8_t byte : buffer) { auto result = HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, reinterpret_cast(flashMemory.begin() + address), byte); @@ -101,6 +101,29 @@ namespace hal } } +#ifdef STM32WBA + const uint8_t alignment = sizeof(uint64_t) * 2; + void SynchronousFlashInternalStmBase::AlignedWriteBuffer(infra::ConstByteRange buffer, uint32_t address) + { + services::FlashAlign::WithAlignment flashAlign; + flashAlign.Align(address, buffer); + + services::FlashAlign::Chunk* chunk = flashAlign.First(); + auto dataSize = chunk->data.size(); + while (chunk != nullptr) + { + really_assert(chunk->data.size() % sizeof(alignment) == 0); + auto fullAddress = reinterpret_cast(flashMemory.begin() + chunk->alignedAddress); + + uint32_t addr = reinterpret_cast(chunk->data.begin()); + auto result = HAL_FLASH_Program(FLASH_TYPEPROGRAM_QUADWORD, fullAddress, addr); + really_assert(result == HAL_OK); + fullAddress += alignment; + chunk = flashAlign.Next(); + } + } +#endif + SynchronousFlashInternalStm::SynchronousFlashInternalStm(infra::MemoryRange sectorSizes, infra::ConstByteRange flashMemory) : SynchronousFlashInternalStmBase(flashMemory) , sectorSizes(sectorSizes) diff --git a/hal_st/synchronous_stm32fxxx/SynchronousFlashInternalStm.hpp b/hal_st/synchronous_stm32fxxx/SynchronousFlashInternalStm.hpp index 60fad350..5d825d4d 100644 --- a/hal_st/synchronous_stm32fxxx/SynchronousFlashInternalStm.hpp +++ b/hal_st/synchronous_stm32fxxx/SynchronousFlashInternalStm.hpp @@ -20,6 +20,9 @@ namespace hal private: template void AlignedWriteBuffer(infra::ConstByteRange buffer, uint32_t address); +#ifdef STM32WBA + void AlignedWriteBuffer(infra::ConstByteRange buffer, uint32_t address); +#endif private: infra::ConstByteRange flashMemory;