SD card read prefetcher (#3814)

* block cache

* efi::size

* extract function and prefetch at start

* comments

* s

* s

* bool result

* no prints

* refactoring

* enable only on some ECU

* normalize

* adjust defines

* is_protected

* naming, comment

* cleanup

* typo

* priority

* mem

* not that mem

firmware/config/stm32f4ems/efifeatures.h

@@ -255,13 +255,14 @@
 #define EFI_CONSOLE_USB_DEVICE SDU1
 
 #if defined(EFI_HAS_EXT_SDRAM)
-    #define ENABLE_PERF_TRACE TRUE
-    #define LUA_USER_HEAP (1 * 1024 * 1024)
-    #define LUA_SYSTEM_HEAP (1 * 1024 * 1024)
+	#define ENABLE_PERF_TRACE TRUE
+	#define LUA_USER_HEAP (1 * 1024 * 1024)
+	#define LUA_SYSTEM_HEAP (1 * 1024 * 1024)
+	#define EFI_USE_COMPRESSED_INI_MSD
 #elif defined(EFI_IS_F42x)
-    // F42x has more memory, so we can:
-    //  - use compressed USB MSD image (requires 32k of memory)
-    //  - use perf trace (requires ~16k of memory)
+	// F42x has more memory, so we can:
+	//  - use compressed USB MSD image (requires 32k of memory)
+	//  - use perf trace (requires ~16k of memory)
 	#define EFI_USE_COMPRESSED_INI_MSD
 	#define ENABLE_PERF_TRACE TRUE
 

firmware/controllers/thread_priority.h

@@ -43,6 +43,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,197 @@
+
+#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->deviceMutex);
+
+	// ask the underlying device, otherwise false
+	auto backing = bc->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->deviceMutex);
+
+	// ask the underlying device, otherwise true
+	auto backing = bc->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->deviceMutex);
+
+	if (auto backing = bc->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->deviceMutex);
+
+	if (auto backing = bc->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 (!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->backing) {
+		return HAL_FAILED;
+	}
+
+	chibios_rt::MutexLocker lock(bc->deviceMutex);
+	return bc->backing->vmt->write(bc->backing, startblk, buffer, n);
+}
+
+bool BlockCache::fetchBlock(uint32_t blockId) {
+	chibios_rt::MutexLocker lock(deviceMutex);
+
+	auto result = backing->vmt->read(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, 512);
+
+		// 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->backing) {
+		return HAL_FAILED;
+	}
+
+	chibios_rt::MutexLocker lock(bc->deviceMutex);
+	return bc->backing->vmt->sync(bc->backing);
+}
+
+static bool get_info(void* instance, BlockDeviceInfo* bdip) {
+	BlockCache* bc = reinterpret_cast<BlockCache*>(instance);
+
+	if (!bc->backing) {
+		return HAL_FAILED;
+	}
+
+	chibios_rt::MutexLocker lock(bc->deviceMutex);
+	return bc->backing->vmt->get_info(bc->backing, bdip);
+}
+
+static const BaseBlockDeviceVMT blockCacheVmt = {
+	(size_t)0, // instanceOffset
+	is_inserted,
+	is_protected,
+	connect,
+	disconnect,
+	read,
+	write,
+	sync,
+	get_info,
+};
+
+BlockCache::BlockCache() {
+	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->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,50 @@
+
+#pragma once
+
+#include "ch.hpp"
+#include "hal.h"
+
+#if HAL_USE_USB_MSD
+
+struct BlockCache {
+	const BaseBlockDeviceVMT* vmt;
+	_base_block_device_data
+
+	BaseBlockDevice* backing;
+	chibios_rt::Mutex deviceMutex;
+
+	BlockCache();
+
+	void start(BaseBlockDevice* backing);
+
+	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 m_cachedBlockData[512];
+
+	// 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

@@ -6,6 +6,7 @@ HW_MASS_STORAGE_SRC_C = $(PROJECT_DIR)/ChibiOS-Contrib/os/various/lib_scsi.c \
 ALLINC += $(PROJECT_DIR)/ext/uzlib/src
 
 ALLCPPSRC += $(PROJECT_DIR)/hw_layer/mass_storage/null_device.cpp \
+			 $(PROJECT_DIR)/hw_layer/mass_storage/block_cache.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 \

firmware/hw_layer/mass_storage/mass_storage_init.cpp

@@ -2,6 +2,7 @@
 
 #include "mass_storage_init.h"
 #include "mass_storage_device.h"
+#include "block_cache.h"
 #include "null_device.h"
 
 #if HAL_USE_USB_MSD
@@ -72,8 +73,12 @@ static const scsi_inquiry_response_t sdCardInquiry = {
     {'v',CH_KERNEL_MAJOR+'0','.',CH_KERNEL_MINOR+'0'}
 };
 
+static BlockCache sdReadPrefetch;
+
 void attachMsdSdCard(BaseBlockDevice* blkdev) {
-	msd.attachLun(1, blkdev, blkbuf1, &sdCardInquiry, nullptr);
+	// Start the prefetcher
+	sdReadPrefetch.start(blkdev);
+	msd.attachLun(1, reinterpret_cast<BaseBlockDevice*>(&sdReadPrefetch), blkbuf1, &sdCardInquiry, nullptr);
 
 #if EFI_TUNER_STUDIO
 	// SD MSD attached, enable indicator in TS