SD card block read prefetcher

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

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<BlockCache*>(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<BlockCache*>(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<BlockCache*>(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<BlockCache*>(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<BlockCache*>(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<BlockCache*>(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<BlockCache*>(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<BlockCache*>(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<BlockCache*>(instance)->readThread(); }, this);
+}
+
+#endif // HAL_USE_USB_MSD

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<Handle*, handleCount> m_requests;
+	chibios_rt::Mailbox<Handle*, handleCount> m_completed;
+	chibios_rt::Mailbox<Handle*, handleCount> 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

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 \

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<BaseBlockDevice*>(&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();