diff --git a/ion/src/device/flash.cpp b/ion/src/device/flash.cpp index b0260845c..b5b0bae8e 100644 --- a/ion/src/device/flash.cpp +++ b/ion/src/device/flash.cpp @@ -45,11 +45,141 @@ static void close() { } } -static void typed_memcpy(uint8_t * source, uint8_t * destination, size_t length) { - MemoryAccessType * src = reinterpret_cast(source); - MemoryAccessType * dst = reinterpret_cast(destination); - for (size_t i=0; i +static inline T * align(void * input) { + size_t k = clog2(sizeof(T)); + return reinterpret_cast(reinterpret_cast(input) & ~((1< +static inline T eat(void * ptr) { + T * pointer = *reinterpret_cast(ptr); + T result = *pointer; + *reinterpret_cast(ptr) = pointer+1; + return result; +} + +static inline ptrdiff_t byte_offset(void * p1, void * p2) { + return reinterpret_cast(p2) - reinterpret_cast(p1); +} + +template +static inline T min(T i, T j) { + return (i| + * |-- HeaderDelta ->| + */ + + MemoryAccessType * alignedDestination = align(destination); + ptrdiff_t headerDelta = byte_offset(alignedDestination, destination); + assert(headerDelta >= 0 && headerDelta < static_cast(sizeof(MemoryAccessType))); + + if (headerDelta > 0) { + // At this point, alignedDestination < destination + // We'll then retrieve the current value at alignedDestination, fill it with + // bytes from source, and write it back at alignedDestination. + + // First, retrieve the current value at alignedDestination + MemoryAccessType header = *alignedDestination; + + // Then copy headerLength bytes from source and put them in the header + uint8_t * headerStart = reinterpret_cast(&header); + // Here's where source data shall start being copied in the header + uint8_t * headerDataStart = headerStart + headerDelta; + // And here's where it should end + uint8_t * headerDataEnd = min( + headerStart + sizeof(MemoryAccessType), // Either at the end of the header + headerDataStart + length // or whenever src runs out of data + ); + for (uint8_t * h = headerDataStart; h(&source); + } + + // Then eventually write the header back into the aligned destination + *alignedDestination++ = header; + } + + /* Step 2 - Copy the bulk of the data + * At this point, we can use aligned MemoryAccessType pointers. */ + + MemoryAccessType * lastAlignedDestination = align(destination + length); + while (alignedDestination < lastAlignedDestination) { + *alignedDestination++ = eat(&source); + } + + /* Step 3 - Copy a footer if needed + * Some unaligned data can be pending at the end. Let's take care of it like + * we did for the header. + * + * _alignedDst _Destination+length + * | | + * --+--------+--------+--------+--------+--------+--------+-- + * | || | | | || | + *---+--------+--------+--------+--------+--------+--------+-- + * |<------------ Footer ------------->| + * |- footerLength ->| + */ + + ptrdiff_t footerLength = byte_offset(alignedDestination, destination + length); + assert(footerLength < static_cast(sizeof(MemoryAccessType))); + if (footerLength > 0) { + assert(alignedDestination == lastAlignedDestination); + + // First, retrieve the current value at alignedDestination + MemoryAccessType footer = *alignedDestination; + + /* Then copy footerLength bytes from source and put them at the beginning of + * the footer */ + uint8_t * footerPointer = reinterpret_cast(&footer); + for (ptrdiff_t i=0; i(&source); + } + + // Then eventually write the footer back into the aligned destination + *alignedDestination = footer; } } @@ -68,7 +198,6 @@ int SectorAtAddress(uint32_t address) { return -1; } - void MassErase() { open(); FLASH.CR()->setMER(true); @@ -93,7 +222,7 @@ void EraseSector(int i) { void WriteMemory(uint8_t * source, uint8_t * destination, size_t length) { open(); FLASH.CR()->setPG(true); - typed_memcpy(source, destination, length); + flash_memcpy(source, destination, length); wait(); FLASH.CR()->setPG(false); close();