| // Copyright 2022 The Fuchsia Authors. All rights reserved. | |
| // Use of this source code is governed by a BSD-style license that can be | |
| // found in the LICENSE file. | |
| #include <lib/storage/storage.h> | |
| #ifdef ENABLE_FIRMWARE_STORAGE_LOG | |
| #define firmware_storage_log printf | |
| #else | |
| #define firmware_storage_log(...) | |
| #endif | |
| #define FIRMWARE_STORAGE_MIN(a, b) (((a) < (b)) ? (a) : (b)) | |
| #define FIRMWARE_STORAGE_MAX(a, b) (((a) > (b)) ? (a) : (b)) | |
| #define FIRMWARE_STORAGE_IS_ALIGNED(a, b) ((((uintptr_t)(a)) % (uintptr_t)(b)) == 0) | |
| #define FIRMWARE_STORAGE_ROUNDUP(a, b) \ | |
| ({ \ | |
| const __typeof((a)) _a = (a); \ | |
| const __typeof((b)) _b = (b); \ | |
| ((_a + _b - 1) / _b * _b); \ | |
| }) | |
| #define FIRMWARE_STORAGE_ROUNDDOWN(a, b) \ | |
| ({ \ | |
| const __typeof((a)) _a = (a); \ | |
| const __typeof((b)) _b = (b); \ | |
| _a - (_a % _b); \ | |
| }) | |
| static bool ReadWrapper(FuchsiaFirmwareStorage* ops, size_t offset, size_t size, void* dst) { | |
| size_t offset_blocks = offset / ops->block_size; | |
| size_t size_blocks = size / ops->block_size; | |
| if (offset_blocks + size_blocks > ops->total_blocks) { | |
| firmware_storage_log("Read overflow\n"); | |
| return false; | |
| } | |
| return ops->read(ops->ctx, offset_blocks, size_blocks, dst); | |
| } | |
| static bool WriteWrapper(FuchsiaFirmwareStorage* ops, size_t offset, size_t size, const void* src) { | |
| size_t offset_blocks = offset / ops->block_size; | |
| size_t size_blocks = size / ops->block_size; | |
| if (offset_blocks + size_blocks > ops->total_blocks) { | |
| firmware_storage_log("Write overflow\n"); | |
| return false; | |
| } | |
| return ops->write(ops->ctx, offset_blocks, size_blocks, src); | |
| } | |
| // First, relax the `size` alignment requirement, assume aligned offset and buffer: | |
| // offset | |
| // |~~~~~~~~~size~~~~~~~~~| | |
| // |---------|---------|---------| | |
| static bool ReadWithAlignedOffsetAndDma(FuchsiaFirmwareStorage* ops, size_t offset, size_t size, | |
| void* dst) { | |
| // Read the aligned part. | |
| size_t aligned_read_size = FIRMWARE_STORAGE_ROUNDDOWN(size, ops->block_size); | |
| if (aligned_read_size && !ReadWrapper(ops, offset, aligned_read_size, dst)) { | |
| firmware_storage_log("Failed to read aligned part\n"); | |
| return false; | |
| } | |
| if (aligned_read_size == size) { | |
| return true; | |
| } | |
| // Read the remaining unaligned part into the scratch buffer | |
| if (!ReadWrapper(ops, offset + aligned_read_size, ops->block_size, ops->scratch_buffer)) { | |
| firmware_storage_log("Failed to read unaligned part\n"); | |
| return false; | |
| } | |
| memcpy((uint8_t*)dst + aligned_read_size, ops->scratch_buffer, size - aligned_read_size); | |
| return true; | |
| } | |
| // Second, relax both the `offset` and `size` alignment requirement, assume aligned buffer. | |
| static bool ReadWithAlignedDma(FuchsiaFirmwareStorage* ops, size_t offset, size_t size, void* dst) { | |
| if (FIRMWARE_STORAGE_IS_ALIGNED(offset, ops->block_size)) { | |
| return ReadWithAlignedOffsetAndDma(ops, offset, size, dst); | |
| } | |
| size_t aligned_offset = FIRMWARE_STORAGE_ROUNDDOWN(offset, ops->block_size); | |
| // Case 1: | |
| // |~~unaligned_read~~| | |
| // |~~~~~~~~~~~~~size~~~~~~~~~~~~| | |
| // offset | |
| // |----------------------|---------------------| | |
| // aligned_offset | |
| // | |
| // Case 2: | |
| // |~~unaligned_read~~| | |
| // |~~~~~~~~size~~~~~~| | |
| // offset | |
| // |-------------------------|------------------------| | |
| // aligned_offset | |
| size_t unaligned_read = FIRMWARE_STORAGE_MIN(size, ops->block_size - (offset - aligned_offset)); | |
| size_t new_offset = offset + unaligned_read; | |
| size_t new_size = size - unaligned_read; | |
| // Read the aligned part first, if there is any, so that we can safely reuse the scratch buffer. | |
| if (new_size) { | |
| uint8_t* next = (uint8_t*)dst + unaligned_read; | |
| // If the new output address is still buffer aligned, read into it. | |
| if (FIRMWARE_STORAGE_IS_ALIGNED(next, FUCHSIA_FIRMWARE_STORAGE_BUFFER_ALIGNMENT)) { | |
| if (!ReadWithAlignedOffsetAndDma(ops, new_offset, new_size, next)) { | |
| return false; | |
| } | |
| } else { | |
| // Otherwise, read into `dst` which is assumed buffer aligned and memmove to correct position. | |
| if (!ReadWithAlignedOffsetAndDma(ops, new_offset, new_size, dst)) { | |
| return false; | |
| } | |
| memmove(next, dst, new_size); | |
| } | |
| } | |
| // Read the unaligned part to scratch buffer. | |
| if (!ReadWrapper(ops, aligned_offset, ops->block_size, ops->scratch_buffer)) { | |
| firmware_storage_log("Failed to read unaligned part\n"); | |
| return false; | |
| } | |
| memcpy(dst, ops->scratch_buffer + (offset - aligned_offset), unaligned_read); | |
| return true; | |
| } | |
| static uint8_t dma_aligned_scratch_buffer[FUCHSIA_FIRMWARE_STORAGE_BUFFER_ALIGNMENT] | |
| __attribute__((__aligned__(FUCHSIA_FIRMWARE_STORAGE_BUFFER_ALIGNMENT))); | |
| // Finally, relax all of `offset`, `size` and buffer alignment requirement | |
| bool FuchsiaFirmwareStorageRead(FuchsiaFirmwareStorage* ops, size_t offset, size_t size, | |
| void* dst) { | |
| uintptr_t dst_addr = (uintptr_t)dst; | |
| if (FIRMWARE_STORAGE_IS_ALIGNED(dst_addr, FUCHSIA_FIRMWARE_STORAGE_BUFFER_ALIGNMENT)) { | |
| return ReadWithAlignedDma(ops, offset, size, dst); | |
| } | |
| // Case 1: | |
| // |~~~~~~~to_read~~~~~~| | |
| // |~~~~~~~~~~~~~dst buffer~~~~~~~~~~~~| | |
| // |----------------------|---------------------| | |
| // buffer alignment size | |
| // | |
| // Case 2: | |
| // |~~~~to_read~~~~~~| | |
| // |~~~~dst buffer~~~| | |
| // |----------------------|---------------------| | |
| // buffer alignment size | |
| size_t to_read = FIRMWARE_STORAGE_MIN( | |
| size, | |
| FIRMWARE_STORAGE_ROUNDUP(dst_addr, FUCHSIA_FIRMWARE_STORAGE_BUFFER_ALIGNMENT) - dst_addr); | |
| if (!ReadWithAlignedDma(ops, offset, to_read, dma_aligned_scratch_buffer)) { | |
| return false; | |
| } | |
| memcpy(dst, dma_aligned_scratch_buffer, to_read); | |
| return (to_read == size) || | |
| ReadWithAlignedDma(ops, offset + to_read, size - to_read, (uint8_t*)dst + to_read); | |
| } | |
| // A write API that relaxes the `size` alignment requirement, assume aligned offset and DMA: | |
| // offset | |
| // |~~~~~~~~~size~~~~~~~~~| | |
| // |---------|---------|---------| | |
| static bool WriteWithAlignedOffsetAndDma(FuchsiaFirmwareStorage* ops, size_t offset, size_t size, | |
| const void* src) { | |
| // Write the aligned part. | |
| size_t aligned_write_size = FIRMWARE_STORAGE_ROUNDDOWN(size, ops->block_size); | |
| if (aligned_write_size && !WriteWrapper(ops, offset, aligned_write_size, src)) { | |
| firmware_storage_log("Failed to write aligned part\n"); | |
| return false; | |
| } | |
| if (aligned_write_size == size) { | |
| return true; | |
| } | |
| // Perform read-copy-write for the unaligned part. | |
| // Read the block of the unaligned part into the scratch buffer | |
| if (!ReadWrapper(ops, offset + aligned_write_size, ops->block_size, ops->scratch_buffer)) { | |
| firmware_storage_log("Failed to read unaligned part\n"); | |
| return false; | |
| } | |
| memcpy(ops->scratch_buffer, (const uint8_t*)src + aligned_write_size, size - aligned_write_size); | |
| if (!WriteWrapper(ops, offset + aligned_write_size, ops->block_size, ops->scratch_buffer)) { | |
| firmware_storage_log("Failed to write unaligned part\n"); | |
| return false; | |
| } | |
| return true; | |
| } | |
| // There's not much optimization we can do other than simply copying chunk by chunk. | |
| // If `src` can be modified, we can optimize the number of write calls similar to read, see | |
| // implementation of `FuchsiaFirmwareStorageWrite()` below | |
| bool FuchsiaFirmwareStorageWriteConst(FuchsiaFirmwareStorage* ops, size_t offset, size_t size, | |
| const void* src) { | |
| // Round down to block boundary and copy-write block by block. | |
| size_t curr_block_offset = FIRMWARE_STORAGE_ROUNDDOWN(offset, ops->block_size); | |
| const uint8_t* data = (const uint8_t*)src; | |
| while (size) { | |
| // If at any point `data` becomes buffer aligned and `offset` becomes block aligned, write | |
| // directly. | |
| if (FIRMWARE_STORAGE_IS_ALIGNED((uintptr_t)data, FUCHSIA_FIRMWARE_STORAGE_BUFFER_ALIGNMENT) && | |
| FIRMWARE_STORAGE_IS_ALIGNED(offset, ops->block_size)) { | |
| return WriteWithAlignedOffsetAndDma(ops, offset, size, data); | |
| } | |
| // Calculate the range (in absolute offset) for copying the data into the scratch buffer. | |
| // The larger between the current block start and `offset`. | |
| size_t copy_start = FIRMWARE_STORAGE_MAX(curr_block_offset, offset); | |
| // The smaller between current block end and `src` end. | |
| size_t copy_end = FIRMWARE_STORAGE_MIN(offset + size, curr_block_offset + ops->block_size); | |
| size_t to_copy = copy_end - copy_start; | |
| if (to_copy < ops->block_size) { | |
| // Partial copy. Some data on the storage needs to be kept. | |
| if (!ReadWrapper(ops, curr_block_offset, ops->block_size, ops->scratch_buffer)) { | |
| firmware_storage_log("Failed to read block\n"); | |
| return false; | |
| } | |
| } | |
| memcpy(ops->scratch_buffer + copy_start - curr_block_offset, data, to_copy); | |
| if (!WriteWrapper(ops, curr_block_offset, ops->block_size, ops->scratch_buffer)) { | |
| firmware_storage_log("Failed to write block\n"); | |
| return false; | |
| } | |
| curr_block_offset += ops->block_size; // Move to the next block. | |
| size -= to_copy; | |
| offset += to_copy; | |
| data += to_copy; | |
| } | |
| return true; | |
| } | |
| // Circular rotation of memory data to the left. | |
| static void RotateMemoryLeft(uint8_t* mem, size_t size, size_t rotate); | |
| // A write API that relaxes the `size` and `offset` alignment requirement and only assumes aligned | |
| // buffer. The `src` buffer needs to be non-const because it needs to be temporarily modified | |
| // internally. | |
| static bool WriteWithAlignedDma(FuchsiaFirmwareStorage* ops, size_t offset, size_t size, | |
| void* src) { | |
| if (FIRMWARE_STORAGE_IS_ALIGNED(offset, ops->block_size)) { | |
| return WriteWithAlignedOffsetAndDma(ops, offset, size, src); | |
| } | |
| size_t aligned_offset = FIRMWARE_STORAGE_ROUNDDOWN(offset, ops->block_size); | |
| // Case 1: | |
| // |~~unaligned_write~| | |
| // |~~~~~~~~~~~~~size~~~~~~~~~~~~| | |
| // offset | |
| // |----------------------|---------------------| | |
| // aligned_offset | |
| // | |
| // Case 2: | |
| // |~~unaligned_write~| | |
| // |~~~~~~~~size~~~~~~| | |
| // offset | |
| // |-------------------------|------------------------| | |
| // aligned_offset | |
| size_t unaligned_write = FIRMWARE_STORAGE_MIN(size, ops->block_size - (offset - aligned_offset)); | |
| size_t new_offset = offset + unaligned_write; | |
| size_t new_size = size - unaligned_write; | |
| // Write the aligned part first so that scratch buffer can be re-used | |
| if (new_size) { | |
| uint8_t* next = (uint8_t*)src + unaligned_write; | |
| // If the new data address is still buffer aligned, write it directly. | |
| if (FIRMWARE_STORAGE_IS_ALIGNED(next, FUCHSIA_FIRMWARE_STORAGE_BUFFER_ALIGNMENT)) { | |
| if (!WriteWithAlignedOffsetAndDma(ops, new_offset, new_size, next)) { | |
| return false; | |
| } | |
| } else { | |
| // Otherwise, rotate `src` left by `unaligned_write`, so that the data to write starts | |
| // at `src` which is assumed buffer aligned. | |
| RotateMemoryLeft(src, size, unaligned_write); | |
| bool res = WriteWithAlignedOffsetAndDma(ops, new_offset, new_size, src); | |
| RotateMemoryLeft(src, size, new_size); // Rotate back to restore the data. | |
| if (!res) { | |
| return false; | |
| } | |
| } | |
| } | |
| // Read-copy-write the unaligned part | |
| if (!ReadWrapper(ops, aligned_offset, ops->block_size, ops->scratch_buffer)) { | |
| firmware_storage_log("Failed to read unaligned part\n"); | |
| return false; | |
| } | |
| memcpy(ops->scratch_buffer + (offset - aligned_offset), src, unaligned_write); | |
| if (!WriteWrapper(ops, aligned_offset, ops->block_size, ops->scratch_buffer)) { | |
| firmware_storage_log("Failed to write unaligned part\n"); | |
| return false; | |
| } | |
| return true; | |
| } | |
| // A Write API that relaxes all of `offset`, `size` and buffer alignment requirement | |
| bool FuchsiaFirmwareStorageWrite(FuchsiaFirmwareStorage* ops, size_t offset, size_t size, | |
| void* src) { | |
| uintptr_t src_addr = (uintptr_t)src; | |
| if (FIRMWARE_STORAGE_IS_ALIGNED(src_addr, FUCHSIA_FIRMWARE_STORAGE_BUFFER_ALIGNMENT)) { | |
| return WriteWithAlignedDma(ops, offset, size, src); | |
| } | |
| // Case 1: | |
| // |~~~~~~~to_write~~~~~| | |
| // |~~~~~~~~~~~~~src buffer~~~~~~~~~~~~| | |
| // |----------------------|---------------------| | |
| // buffer alignment size | |
| // | |
| // Case 2: | |
| // |~~~~to_write~~~~~| | |
| // |~~~~src buffer~~~| | |
| // |----------------------|---------------------| | |
| // buffer alignment size | |
| size_t to_write = FIRMWARE_STORAGE_MIN( | |
| size, | |
| FIRMWARE_STORAGE_ROUNDUP(src_addr, FUCHSIA_FIRMWARE_STORAGE_BUFFER_ALIGNMENT) - src_addr); | |
| memcpy(dma_aligned_scratch_buffer, src, to_write); | |
| if (!WriteWithAlignedDma(ops, offset, to_write, dma_aligned_scratch_buffer)) { | |
| return false; | |
| } | |
| return (to_write == size) || | |
| WriteWithAlignedDma(ops, offset + to_write, size - to_write, (uint8_t*)src + to_write); | |
| } | |
| // Reverse a memory specified by [start, end) | |
| static void ReverseMemory(uint8_t* start, uint8_t* end) { | |
| while (start < end) { | |
| uint8_t temp = *start; | |
| *(start++) = *(--end); | |
| *end = temp; | |
| } | |
| } | |
| static void RotateMemoryLeft(uint8_t* mem, size_t size, size_t rotate) { | |
| ReverseMemory(mem, mem + rotate); // Reverse the left part. | |
| ReverseMemory(mem + rotate, mem + size); // Reverse the right part. | |
| ReverseMemory(mem, mem + size); // Reverse the entire array. | |
| } |