diff --git a/firmware/controllers/thread_priority.h b/firmware/controllers/thread_priority.h index ae25cfc82a..4ebebe1829 100644 --- a/firmware/controllers/thread_priority.h +++ b/firmware/controllers/thread_priority.h @@ -47,6 +47,7 @@ // USB mass storage #define MSD_THD_PRIO LOWPRIO + 20 +#define MSD_CACHE_PRIO MSD_THD_PRIO - 1 // Lua interpreter must be lowest priority, as the user's code may get stuck in an infinite loop #define PRIO_LUA LOWPRIO + 10 diff --git a/firmware/hw_layer/mass_storage/block_cache.cpp b/firmware/hw_layer/mass_storage/block_cache.cpp new file mode 100644 index 0000000000..a34bbde598 --- /dev/null +++ b/firmware/hw_layer/mass_storage/block_cache.cpp @@ -0,0 +1,196 @@ +#include "pch.h" + +#include "block_cache.h" + +#if HAL_USE_USB_MSD + +static bool is_inserted(void* instance) { + BlockCache* bc = reinterpret_cast(instance); + + chibios_rt::MutexLocker lock(bc->m_deviceMutex); + + // ask the underlying device, otherwise false + auto backing = bc->m_backing; + return backing ? backing->vmt->is_inserted(backing) : false; +} + +static bool is_protected(void* instance) { + BlockCache* bc = reinterpret_cast(instance); + + chibios_rt::MutexLocker lock(bc->m_deviceMutex); + + // ask the underlying device, otherwise true + auto backing = bc->m_backing; + return backing ? backing->vmt->is_protected(backing) : true; +} + +static bool connect(void* instance) { + BlockCache* bc = reinterpret_cast(instance); + + chibios_rt::MutexLocker lock(bc->m_deviceMutex); + + if (auto backing = bc->m_backing) { + return backing->vmt->connect(backing); + } + + return HAL_FAILED; +} + +static bool disconnect(void* instance) { + BlockCache* bc = reinterpret_cast(instance); + + chibios_rt::MutexLocker lock(bc->m_deviceMutex); + + if (auto backing = bc->m_backing) { + return backing->vmt->disconnect(backing); + } + + return HAL_FAILED; +} + +static bool read(void* instance, uint32_t startblk, uint8_t* buffer, uint32_t n) { + if (n != 1) { + return HAL_FAILED; + } + + BlockCache* bc = reinterpret_cast(instance); + + return bc->read(startblk, buffer); +} + +bool BlockCache::read(uint32_t startblk, uint8_t* buffer) { + if (!m_backing) { + return HAL_FAILED; + } + + Handle* h; + m_free.fetch(&h, TIME_INFINITE); + + // copy request information + h->startblk = startblk; + h->buffer = buffer; + + // make the request + m_requests.post(h, TIME_INFINITE); + // wait for it to complete + m_completed.fetch(&h, TIME_INFINITE); + + // stash the result + auto result = h->result; + + // return the handle to the free list + m_free.post(h, TIME_INFINITE); + + return result; +} + +static bool write(void* instance, uint32_t startblk, const uint8_t* buffer, uint32_t n) { + BlockCache* bc = reinterpret_cast(instance); + + if (!bc->m_backing) { + return HAL_FAILED; + } + + chibios_rt::MutexLocker lock(bc->m_deviceMutex); + return bc->m_backing->vmt->write(bc->m_backing, startblk, buffer, n); +} + +bool BlockCache::fetchBlock(uint32_t blockId) { + chibios_rt::MutexLocker lock(m_deviceMutex); + + auto result = m_backing->vmt->read(m_backing, blockId, m_cachedBlockData, 1); + + if (result == HAL_SUCCESS) { + // read succeeded, mark cache as valid + m_cachedBlockId = blockId; + } else { + // read failed, invalidate cache + m_cachedBlockId = -1; + } + + return result; +} + +void BlockCache::readThread() { + while (true) { + Handle* h; + + // Wait for a request to come in + m_requests.fetch(&h, TIME_INFINITE); + + auto startblk = h->startblk; + + // Did we prefetch the wrong block? + if (startblk != m_cachedBlockId) { + // Cache miss, fetch the correct block + h->result = fetchBlock(startblk); + } else { + // Cache hit, the correct block is already loaded! + h->result = HAL_SUCCESS; + } + + // Copy from the cache to the output buffer + memcpy(h->buffer, m_cachedBlockData, MMCSD_BLOCK_SIZE); + + // return the completed request + m_completed.post(h, TIME_INFINITE); + + // Now that we have returned the requested block, prefetch the next block + startblk++; + fetchBlock(startblk); + } +} + +static bool sync(void* instance) { + BlockCache* bc = reinterpret_cast(instance); + + if (!bc->m_backing) { + return HAL_FAILED; + } + + chibios_rt::MutexLocker lock(bc->m_deviceMutex); + return bc->m_backing->vmt->sync(bc->m_backing); +} + +static bool get_info(void* instance, BlockDeviceInfo* bdip) { + BlockCache* bc = reinterpret_cast(instance); + + if (!bc->m_backing) { + return HAL_FAILED; + } + + chibios_rt::MutexLocker lock(bc->m_deviceMutex); + return bc->m_backing->vmt->get_info(bc->m_backing, bdip); +} + +static const BaseBlockDeviceVMT blockCacheVmt = { + (size_t)0, // instanceOffset + is_inserted, + is_protected, + connect, + disconnect, + read, + write, + sync, + get_info, +}; + +BlockCache::BlockCache(uint8_t* cachedBlockData) : m_cachedBlockData(cachedBlockData) { + vmt = &blockCacheVmt; + + // push all in to the free buffer + for (int i = 0; i < efi::size(m_handles); i++) { + m_free.post(&m_handles[i], TIME_INFINITE); + } +} + +void BlockCache::start(BaseBlockDevice* backing) { + this->m_backing = backing; + + // prefetch block 0 + fetchBlock(0); + + chThdCreateStatic(waRead, sizeof(waRead), MSD_CACHE_PRIO, [](void* instance) { reinterpret_cast(instance)->readThread(); }, this); +} + +#endif // HAL_USE_USB_MSD diff --git a/firmware/hw_layer/mass_storage/block_cache.h b/firmware/hw_layer/mass_storage/block_cache.h new file mode 100644 index 0000000000..6b5c06939b --- /dev/null +++ b/firmware/hw_layer/mass_storage/block_cache.h @@ -0,0 +1,49 @@ +#pragma once + +#include "ch.hpp" +#include "hal.h" + +#if HAL_USE_USB_MSD + +struct BlockCache { + const BaseBlockDeviceVMT* vmt; + _base_block_device_data + + BaseBlockDevice* m_backing; + chibios_rt::Mutex m_deviceMutex; + + BlockCache(uint8_t* cachedBlockData); + + void start(BaseBlockDevice* cachedBlockData); + + bool read(uint32_t startblk, uint8_t* buffer); + +private: + bool fetchBlock(uint32_t blockId); + + struct Handle { + // Read request parameters + uint32_t startblk; + uint8_t* buffer; + + // Returned result + bool result; + }; + + static constexpr int handleCount = 1; + + chibios_rt::Mailbox m_requests; + chibios_rt::Mailbox m_completed; + chibios_rt::Mailbox m_free; + + Handle m_handles[handleCount]; + + int32_t m_cachedBlockId = -1; + uint8_t* const m_cachedBlockData; + + // Worker thread that performs actual read operations in the background + void readThread(); + THD_WORKING_AREA(waRead, USB_MSD_THREAD_WA_SIZE); +}; + +#endif // HAL_USE_USB_MSD diff --git a/firmware/hw_layer/mass_storage/mass_storage.mk b/firmware/hw_layer/mass_storage/mass_storage.mk index 9854997552..143338ac64 100644 --- a/firmware/hw_layer/mass_storage/mass_storage.mk +++ b/firmware/hw_layer/mass_storage/mass_storage.mk @@ -9,3 +9,4 @@ ALLCPPSRC += $(PROJECT_DIR)/hw_layer/mass_storage/null_device.cpp \ $(PROJECT_DIR)/hw_layer/mass_storage/compressed_block_device.cpp \ $(PROJECT_DIR)/hw_layer/mass_storage/mass_storage_device.cpp \ $(PROJECT_DIR)/hw_layer/mass_storage/mass_storage_init.cpp \ + $(PROJECT_DIR)/hw_layer/mass_storage/block_cache.cpp \ diff --git a/firmware/hw_layer/mass_storage/mass_storage_init.cpp b/firmware/hw_layer/mass_storage/mass_storage_init.cpp index 07a22a2aa4..57e5b6c1d0 100644 --- a/firmware/hw_layer/mass_storage/mass_storage_init.cpp +++ b/firmware/hw_layer/mass_storage/mass_storage_init.cpp @@ -3,6 +3,7 @@ #include "mass_storage_init.h" #include "mass_storage_device.h" #include "null_device.h" +#include "block_cache.h" #if HAL_USE_USB_MSD @@ -40,7 +41,10 @@ // One block buffer per LUN static NO_CACHE uint8_t blkbufIni[MMCSD_BLOCK_SIZE]; -static SDMMC_MEMORY(MMCSD_BLOCK_SIZE) uint8_t blkbufSdmmc[MMCSD_BLOCK_SIZE]; +static struct { + uint8_t sdmmc[MMCSD_BLOCK_SIZE]; + uint8_t prefetch[MMCSD_BLOCK_SIZE]; +} sdBuffers SDMMC_MEMORY(2 * MMCSD_BLOCK_SIZE); static CCM_OPTIONAL MassStorageController msd(usb_driver); @@ -72,8 +76,12 @@ static const scsi_inquiry_response_t sdCardInquiry = { {'v',CH_KERNEL_MAJOR+'0','.',CH_KERNEL_MINOR+'0'} }; +static BlockCache sdReadPrefetch(sdBuffers.prefetch); + void attachMsdSdCard(BaseBlockDevice* blkdev) { - msd.attachLun(1, blkdev, blkbufSdmmc, &sdCardInquiry, nullptr); + // Start the prefetcher + sdReadPrefetch.start(blkdev); + msd.attachLun(1, reinterpret_cast(&sdReadPrefetch), sdBuffers.sdmmc, &sdCardInquiry, nullptr); #if EFI_TUNER_STUDIO // SD MSD attached, enable indicator in TS @@ -114,9 +122,9 @@ void initUsbMsd() { // SRAM, but excluded from the cache #ifdef STM32H7XX { - void* base = &blkbufSdmmc; - static_assert(sizeof(blkbufSdmmc) == 512); - uint32_t size = MPU_RASR_SIZE_512; + void* base = &sdBuffers; + static_assert(sizeof(sdBuffers) == 1024); + uint32_t size = MPU_RASR_SIZE_1K; mpuConfigureRegion(MPU_REGION_4, base, @@ -137,7 +145,7 @@ void initUsbMsd() { msd.attachLun(0, getRamdiskDevice(), blkbufIni, &iniDriveInquiry, nullptr); // attach a null device in place of the SD card for now - the SD thread may replace it later - msd.attachLun(1, (BaseBlockDevice*)&ND1, blkbufSdmmc, &sdCardInquiry, nullptr); + msd.attachLun(1, (BaseBlockDevice*)&ND1, sdBuffers.sdmmc, &sdCardInquiry, nullptr); // start the mass storage thread msd.start();