multiple mass storage (#2535)

* driver

* plumbing

* guard adjust

* read correct lun

* de-template

* rearrange

* guard

* guard

* memory

* headers

* memory

Co-authored-by: Matthew Kennedy <makenne@microsoft.com>

firmware/controllers/engine_controller.cpp

@@ -709,7 +709,7 @@ void initEngineContoller(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX)
  * UNUSED_SIZE constants.
  */
 #ifndef RAM_UNUSED_SIZE
-#define RAM_UNUSED_SIZE 3050
+#define RAM_UNUSED_SIZE 2850
 #endif
 #ifndef CCM_UNUSED_SIZE
 #define CCM_UNUSED_SIZE 2000

firmware/hw_layer/mass_storage/mass_storage.mk

@@ -6,4 +6,6 @@ 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/compressed_block_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 \

firmware/hw_layer/mass_storage/mass_storage_device.cpp

@@ -0,0 +1,220 @@
+/**
+ * @file	mass_storage_device.cpp
+ *
+ * @date April 9, 2021
+ * @author Matthew Kennedy, (c) 2021
+ *
+ * This file implements a USB mass storage device with multiple LUNs, so multiple drives can be mounted at once.
+ */
+
+#include "mass_storage_device.h"
+
+#if HAL_USE_USB_MSD
+
+#define MSD_REQ_RESET                   0xFF
+#define MSD_REQ_GET_MAX_LUN             0xFE
+
+#define MSD_CBW_SIGNATURE               0x43425355
+#define MSD_CSW_SIGNATURE               0x53425355
+
+#define CBW_FLAGS_RESERVED_MASK         0b01111111
+#define CBW_LUN_RESERVED_MASK           0b11110000
+#define CBW_CMD_LEN_RESERVED_MASK       0b11000000
+
+#define CSW_STATUS_PASSED               0x00
+#define CSW_STATUS_FAILED               0x01
+#define CSW_STATUS_PHASE_ERROR          0x02
+
+#define MSD_SETUP_WORD(setup, index) (uint16_t)(((uint16_t)setup[index+1] << 8)\
+                                                | (setup[index] & 0x00FF))
+
+#define MSD_SETUP_VALUE(setup)  MSD_SETUP_WORD(setup, 2)
+#define MSD_SETUP_INDEX(setup)  MSD_SETUP_WORD(setup, 4)
+#define MSD_SETUP_LENGTH(setup) MSD_SETUP_WORD(setup, 6)
+
+static uint32_t scsi_transport_transmit(const SCSITransport *transport,
+										const uint8_t *data, size_t len) {
+	usb_scsi_transport_handler_t *trp = reinterpret_cast<usb_scsi_transport_handler_t*>(transport->handler);
+	msg_t status = usbTransmit(trp->usbp, trp->ep, data, len);
+	if (MSG_OK == status)
+		return len;
+	else
+		return 0;
+}
+
+static uint32_t scsi_transport_receive(const SCSITransport *transport,
+										uint8_t *data, size_t len) {
+	usb_scsi_transport_handler_t *trp = reinterpret_cast<usb_scsi_transport_handler_t*>(transport->handler);
+	msg_t status = usbReceive(trp->usbp, trp->ep, data, len);
+	if (MSG_RESET != status)
+		return status;
+	else
+		return 0;
+}
+
+MassStorageController::MassStorageController(USBDriver* usb)
+	: ThreadController("MSD", MSD_THD_PRIO)
+	, m_usb(usb) {
+	for (size_t i = 0; i < USB_MSD_LUN_COUNT; i++) {
+		scsiObjectInit(&m_luns[i].target);
+	}
+
+	m_scsiTransportHandler.usbp = usb;
+	m_scsiTransportHandler.ep = USB_MSD_DATA_EP;
+	m_scsiTransport.handler  = &m_scsiTransportHandler;
+	m_scsiTransport.transmit = scsi_transport_transmit;
+	m_scsiTransport.receive  = scsi_transport_receive;
+}
+
+void MassStorageController::ThreadTask() {
+	while (!chThdShouldTerminateX()) {
+		const msg_t status = usbReceive(m_usb, USB_MSD_DATA_EP, (uint8_t*)&m_cbw, sizeof(m_cbw));
+
+		if (MSG_RESET == status) {
+			chThdSleepMilliseconds(50);
+			continue;
+		}
+
+		if (cbwValid(m_cbw, status) && cbwMeaningful(m_cbw)) {
+			chibios_rt::MutexLocker lock(m_lunMutex);
+
+			auto target = &m_luns[m_cbw.lun].target;
+			if (SCSI_SUCCESS == scsiExecCmd(target, m_cbw.cmd_data)) {
+				sendCsw(CSW_STATUS_PASSED, 0);
+			} else {
+				sendCsw(CSW_STATUS_FAILED, scsiResidue(target));
+			}
+		} else {
+			// ignore incorrect CBW
+		}
+	}
+}
+
+/*static*/ bool MassStorageController::cbwValid(const msd_cbw_t& cbw, msg_t recvd) {
+	// check valid size
+	if (sizeof(msd_cbw_t) != recvd) {
+		return false;
+	}
+
+	// Check valid signature
+	if (cbw.signature != MSD_CBW_SIGNATURE) {
+		return false;
+	}
+
+	return true;
+}
+
+/*static*/ bool MassStorageController::cbwMeaningful(const msd_cbw_t& cbw) {
+	if ((cbw.cmd_len & CBW_CMD_LEN_RESERVED_MASK) != 0) {
+		return false;
+	}
+
+	if ((cbw.flags & CBW_FLAGS_RESERVED_MASK) != 0) {
+		return false;
+	}
+
+	if (cbw.lun >= USB_MSD_LUN_COUNT) {
+		return false;
+	}
+
+	return true;
+}
+
+void MassStorageController::sendCsw(uint8_t status, uint32_t residue) {
+	m_csw.signature = MSD_CSW_SIGNATURE;
+	m_csw.data_residue = residue;
+	m_csw.tag = m_cbw.tag;
+	m_csw.status = status;
+
+	usbTransmit(m_usb, USB_MSD_DATA_EP, (uint8_t*)&m_csw, sizeof(m_csw));
+}
+
+/**
+ * @brief   Hardcoded default SCSI inquiry response structure.
+ */
+static const scsi_inquiry_response_t default_scsi_inquiry_response = {
+    0x00,           /* direct access block device     */
+    0x80,           /* removable                      */
+    0x04,           /* SPC-2                          */
+    0x02,           /* response data format           */
+    0x20,           /* response has 0x20 + 4 bytes    */
+    0x00,
+    0x00,
+    0x00,
+    "Chibios",
+    "Mass Storage",
+    {'v',CH_KERNEL_MAJOR+'0','.',CH_KERNEL_MINOR+'0'}
+};
+
+/**
+ * @brief   Hardcoded default SCSI unit serial number inquiry response structure.
+ */
+static const scsi_unit_serial_number_inquiry_response_t default_scsi_unit_serial_number_inquiry_response =
+{
+    0x00,
+    0x80,
+    0x00,
+    0x08,
+    "0000000"
+};
+
+void MassStorageController::attachLun(uint8_t lunIndex,
+						BaseBlockDevice *blkdev, uint8_t *blkbuf,
+						const scsi_inquiry_response_t *inquiry,
+						const scsi_unit_serial_number_inquiry_response_t *serialInquiry) {
+	chibios_rt::MutexLocker lock(m_lunMutex);
+
+	auto& lun = m_luns[lunIndex];
+
+
+	if (NULL == inquiry) {
+		lun.config.inquiry_response = &default_scsi_inquiry_response;
+	}
+	else {
+		lun.config.inquiry_response = inquiry;
+	}
+	if (NULL == serialInquiry) {
+		lun.config.unit_serial_number_inquiry_response = &default_scsi_unit_serial_number_inquiry_response;
+	}
+	else {
+		lun.config.unit_serial_number_inquiry_response = serialInquiry;
+	}
+	lun.config.blkbuf = blkbuf;
+	lun.config.blkdev = blkdev;
+	lun.config.transport = &m_scsiTransport;
+
+	scsiStart(&lun.target, &lun.config);
+}
+
+extern "C" bool msd_request_hook_new(USBDriver *usbp) {
+	/* check that the request is for interface 0.*/
+	if (MSD_SETUP_INDEX(usbp->setup) != 0) {
+		return false;
+	}
+
+	if (usbp->setup[0] == (USB_RTYPE_TYPE_CLASS | USB_RTYPE_RECIPIENT_INTERFACE | USB_RTYPE_DIR_HOST2DEV)
+		&& usbp->setup[1] == MSD_REQ_RESET) {
+		/* Bulk-Only Mass Storage Reset (class-specific request)
+		This request is used to reset the mass storage device and its associated interface.
+		This class-specific request shall ready the device for the next CBW from the host. */
+		/* Do any special reset code here. */
+		/* The device shall NAK the status stage of the device request until
+		* the Bulk-Only Mass Storage Reset is complete.
+		* NAK EP1 in and out */
+		// usbp->otg->ie[1].DIEPCTL = DIEPCTL_SNAK;
+		// usbp->otg->oe[1].DOEPCTL = DOEPCTL_SNAK;
+		/* response to this request using EP0 */
+		usbSetupTransfer(usbp, 0, 0, NULL);
+		return true;
+	} else if (usbp->setup[0] == (USB_RTYPE_TYPE_CLASS | USB_RTYPE_RECIPIENT_INTERFACE | USB_RTYPE_DIR_DEV2HOST)
+		&& usbp->setup[1] == MSD_REQ_GET_MAX_LUN) {
+		/* Return the maximum supported LUN. */
+		static uint8_t zero = USB_MSD_LUN_COUNT - 1;
+		usbSetupTransfer(usbp, &zero, 1, NULL);
+		return true;
+	}
+
+	return false;
+}
+
+#endif // HAL_USE_USB_MSD

firmware/hw_layer/mass_storage/mass_storage_device.h

@@ -0,0 +1,54 @@
+/**
+ * @file	mass_storage_device.h
+ *
+ * @date April 9, 2021
+ * @author Matthew Kennedy, (c) 2021
+ *
+ * This file implements a USB mass storage device with multiple LUNs, so multiple drives can be mounted at once.
+ */
+
+#include "ch.hpp"
+#include "hal.h"
+#include "hal_usb_msd.h"
+#include "thread_controller.h"
+
+#if HAL_USE_USB_MSD
+
+class MassStorageController : public ThreadController<USB_MSD_THREAD_WA_SIZE> {
+public:
+	MassStorageController(USBDriver* usb);
+
+	void attachLun(uint8_t lunIndex, BaseBlockDevice *blkdev, uint8_t *blkbuf,
+					const scsi_inquiry_response_t *inquiry,
+					const scsi_unit_serial_number_inquiry_response_t *serialInquiry);
+protected:
+	void ThreadTask() override;
+
+private:
+	static bool cbwValid(const msd_cbw_t& cbw, msg_t status);
+	static bool cbwMeaningful(const msd_cbw_t& cbw);
+
+	void sendCsw(uint8_t status, uint32_t residue);
+
+	usbmsdstate_t m_state;
+
+	USBDriver* const m_usb;
+
+	// temporary storage
+	msd_cbw_t m_cbw;
+	msd_csw_t m_csw;
+
+	SCSITransport m_scsiTransport;
+	usb_scsi_transport_handler_t m_scsiTransportHandler;
+
+	chibios_rt::Mutex m_lunMutex;
+
+	struct LunEntry {
+		SCSITarget target;
+		SCSITargetConfig config;
+	};
+
+	LunEntry m_luns[USB_MSD_LUN_COUNT];
+};
+
+#endif // HAL_USE_USB_MSD

firmware/hw_layer/mass_storage/mass_storage_init.cpp

@@ -0,0 +1,110 @@
+#include "mass_storage_init.h"
+#include "mass_storage_device.h"
+#include "null_device.h"
+
+#if HAL_USE_USB_MSD
+
+#if EFI_EMBED_INI_MSD
+	#ifdef EFI_USE_COMPRESSED_INI_MSD
+		#include "compressed_block_device.h"
+		#include "ramdisk_image_compressed.h"
+
+		static CompressedBlockDevice cbd;
+	#else
+		#include "ramdisk.h"
+		#include "ramdisk_image.h"
+
+		static RamDisk ramdisk;
+	#endif
+
+	// If the ramdisk image told us not to use it, don't use it.
+	#ifdef RAMDISK_INVALID
+		#undef EFI_EMBED_INI_MSD
+		#define EFI_EMBED_INI_MSD FALSE
+	#endif
+#endif
+
+#if STM32_USB_USE_OTG2
+  USBDriver *usb_driver = &USBD2;
+#else
+  USBDriver *usb_driver = &USBD1;
+#endif
+
+// One block buffer per LUN
+static NO_CACHE uint8_t blkbuf0[MMCSD_BLOCK_SIZE];
+static NO_CACHE uint8_t blkbuf1[MMCSD_BLOCK_SIZE];
+
+static MassStorageController msd(usb_driver);
+
+static const scsi_inquiry_response_t iniDriveInquiry = {
+    0x00,           /* direct access block device     */
+    0x80,           /* removable                      */
+    0x04,           /* SPC-2                          */
+    0x02,           /* response data format           */
+    0x20,           /* response has 0x20 + 4 bytes    */
+    0x00,
+    0x00,
+    0x00,
+    "rusEFI",
+    "INI Drive",
+    {'v',CH_KERNEL_MAJOR+'0','.',CH_KERNEL_MINOR+'0'}
+};
+
+static const scsi_inquiry_response_t sdCardInquiry = {
+    0x00,           /* direct access block device     */
+    0x80,           /* removable                      */
+    0x04,           /* SPC-2                          */
+    0x02,           /* response data format           */
+    0x20,           /* response has 0x20 + 4 bytes    */
+    0x00,
+    0x00,
+    0x00,
+    "rusEFI",
+    "SD Card",
+    {'v',CH_KERNEL_MAJOR+'0','.',CH_KERNEL_MINOR+'0'}
+};
+
+void attachMsdSdCard(BaseBlockDevice* blkdev) {
+	msd.attachLun(1, blkdev, blkbuf1, &sdCardInquiry, nullptr);
+}
+
+static BaseBlockDevice* getRamdiskDevice() {
+#if EFI_EMBED_INI_MSD
+#ifdef EFI_USE_COMPRESSED_INI_MSD
+	uzlib_init();
+	compressedBlockDeviceObjectInit(&cbd);
+	compressedBlockDeviceStart(&cbd, ramdisk_image_gz, sizeof(ramdisk_image_gz));
+
+	return (BaseBlockDevice*)&cbd;
+#else // not EFI_USE_COMPRESSED_INI_MSD
+	ramdiskObjectInit(&ramdisk);
+
+	constexpr size_t ramdiskSize = sizeof(ramdisk_image);
+	constexpr size_t blockSize = 512;
+	constexpr size_t blockCount = ramdiskSize / blockSize;
+
+	// Ramdisk should be a round number of blocks
+	static_assert(ramdiskSize % blockSize == 0);
+
+	ramdiskStart(&ramdisk, const_cast<uint8_t*>(ramdisk_image), blockSize, blockCount, /*readonly =*/ true);
+
+	return (BaseBlockDevice*)&ramdisk;
+#endif // EFI_USE_COMPRESSED_INI_MSD
+#else // not EFI_EMBED_INI_MSD
+	// No embedded ini file, just mount the null device instead
+	return (BaseBlockDevice*)&ND1;
+#endif
+}
+
+void initUsbMsd() {
+	// Attach the ini ramdisk
+	msd.attachLun(0, getRamdiskDevice(), blkbuf0, &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, blkbuf1, &sdCardInquiry, nullptr);
+
+	// start the mass storage thread
+	msd.Start();
+}
+
+#endif // HAL_USE_USB_MSD

firmware/hw_layer/mass_storage/mass_storage_init.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#include "hal.h"
+
+#if HAL_USE_USB_MSD
+void initUsbMsd();
+void attachMsdSdCard(BaseBlockDevice* blkdev);
+#endif

firmware/hw_layer/mass_storage/null_device.cpp

@@ -10,23 +10,7 @@
 
 #include "hal.h"
 
-#if EFI_EMBED_INI_MSD
+#include "global.h"
-#include "ramdisk.h"
-#include "compressed_block_device.h"
-
-#ifdef EFI_USE_COMPRESSED_INI_MSD
-#include "ramdisk_image_compressed.h"
-#else
-#include "ramdisk_image.h"
-#endif
-
-// If the ramdisk image told us not to use it, don't use it.
-#ifdef RAMDISK_INVALID
-#undef EFI_EMBED_INI_MSD
-#define EFI_EMBED_INI_MSD FALSE
-#endif
-
-#endif
 
 #include <cstring>
 
@@ -83,44 +67,5 @@ static const struct BaseBlockDeviceVMT ndVmt = {
 	nd_get_info
 };
 
-#if EFI_EMBED_INI_MSD
-#ifdef EFI_USE_COMPRESSED_INI_MSD
-static CompressedBlockDevice cbd;
-#else
-static RamDisk ramdisk;
-#endif
-#else
 // This device is always ready and has no state
-static NullDevice nd = { &ndVmt, BLK_READY };
+NullDevice ND1 = { &ndVmt, BLK_READY };
-#endif
-
-#if HAL_USE_USB_MSD
-void msdMountNullDevice(USBMassStorageDriver* msdp, USBDriver *usbp, uint8_t* blkbuf, const scsi_inquiry_response_t* inquiry) {
-	// TODO: implement multi-LUN so we can mount the ini image and SD card at the same time
-
-#if EFI_EMBED_INI_MSD
-#ifdef EFI_USE_COMPRESSED_INI_MSD
-	uzlib_init();
-	compressedBlockDeviceObjectInit(&cbd);
-	compressedBlockDeviceStart(&cbd, ramdisk_image_gz, sizeof(ramdisk_image_gz));
-	msdStart(msdp, usbp, (BaseBlockDevice*)&cbd, blkbuf, inquiry, nullptr);
-#else // not EFI_USE_COMPRESSED_INI_MSD
-	ramdiskObjectInit(&ramdisk);
-
-	constexpr size_t ramdiskSize = sizeof(ramdisk_image);
-	constexpr size_t blockSize = 512;
-	constexpr size_t blockCount = ramdiskSize / blockSize;
-
-	// Ramdisk should be a round number of blocks
-	static_assert(ramdiskSize % blockSize == 0);
-
-	ramdiskStart(&ramdisk, const_cast<uint8_t*>(ramdisk_image), blockSize, blockCount, /*readonly =*/ true);
-
-	msdStart(msdp, usbp, (BaseBlockDevice*)&ramdisk, blkbuf, inquiry, nullptr);
-#endif // EFI_USE_COMPRESSED_INI_MSD
-#else // not EFI_EMBED_INI_MSD
-	// No embedded ini file, just mount the null device instead
-	msdStart(msdp, usbp, (BaseBlockDevice*)&nd, blkbuf, inquiry, nullptr);
-#endif
-}
-#endif

firmware/hw_layer/mass_storage/null_device.h

@@ -10,6 +10,9 @@
 
 #include "hal.h"
 
-#if HAL_USE_USB_MSD
+struct NullDevice {
-void msdMountNullDevice(USBMassStorageDriver* msdp, USBDriver *usbp, uint8_t* blkbuf, const scsi_inquiry_response_t* inquiry);
+	const struct BaseBlockDeviceVMT *vmt;
-#endif
+	_base_block_device_data
+};
+
+extern NullDevice ND1;

firmware/hw_layer/mmc_card.cpp

@@ -26,7 +26,7 @@
 #include "engine_configuration.h"
 #include "status_loop.h"
 #include "buffered_writer.h"
-#include "null_device.h"
+#include "mass_storage_init.h"
 #include "thread_priority.h"
 
 #include "rtc_helper.h"
@@ -70,15 +70,6 @@ spi_device_e mmcSpiDevice = SPI_NONE;
 #define LS_RESPONSE "ls_result"
 #define FILE_LIST_MAX_COUNT 20
 
-#if HAL_USE_USB_MSD
-#include "hal_usb_msd.h"
-#if STM32_USB_USE_OTG2
-  USBDriver *usb_driver = &USBD2;
-#else
-  USBDriver *usb_driver = &USBD1;
-#endif
-#endif /* HAL_USE_USB_MSD */
-
 static THD_WORKING_AREA(mmcThreadStack, 3 * UTILITY_THREAD_STACK_SIZE);		// MMC monitor thread
 
 #if HAL_USE_MMC_SPI
@@ -325,21 +316,6 @@ static void mmcUnMount(void) {
 }
 
 #if HAL_USE_USB_MSD
-static NO_CACHE uint8_t blkbuf[MMCSD_BLOCK_SIZE];
-
-static const scsi_inquiry_response_t scsi_inquiry_response = {
-    0x00,           /* direct access block device     */
-    0x80,           /* removable                      */
-    0x04,           /* SPC-2                          */
-    0x02,           /* response data format           */
-    0x20,           /* response has 0x20 + 4 bytes    */
-    0x00,
-    0x00,
-    0x00,
-    "rusEFI",
-    "SD Card",
-    {'v',CH_KERNEL_MAJOR+'0','.',CH_KERNEL_MINOR+'0'}
-};
 
 static chibios_rt::BinarySemaphore usbConnectedSemaphore(/* taken =*/ true);
 
@@ -431,14 +407,11 @@ static bool mountMmc() {
 	// mount the null device and try to mount the filesystem ourselves
 	if (cardBlockDevice && hasUsb) {
 		// Mount the real card to USB
-		msdStart(&USBMSD1, usb_driver, cardBlockDevice, blkbuf, &scsi_inquiry_response, NULL);
+		attachMsdSdCard(cardBlockDevice);
 
 		sdStatus = SD_STATE_MSD;
 		// At this point we're done: don't try to write files ourselves
 		return false;
-	} else {
-		// Mount a  "no media" device to USB
-		msdMountNullDevice(&USBMSD1, usb_driver, blkbuf, &scsi_inquiry_response);
 	}
 #endif
 

firmware/hw_layer/ports/rusefi_halconf.h

@@ -32,6 +32,8 @@
 // If USB and File logging, enable USB Mass Storage & community
 #define HAL_USE_USB_MSD (EFI_FILE_LOGGING && EFI_USB_SERIAL)
 #define HAL_USE_COMMUNITY (EFI_FILE_LOGGING && EFI_USB_SERIAL)
+#define USB_MSD_LUN_COUNT 2
+
 // only the MSD driver requires USB_USE_WAIT
 #define USB_USE_WAIT (EFI_FILE_LOGGING && EFI_USB_SERIAL)
 

firmware/hw_layer/ports/stm32/serial_over_usb/usbcfg.c

@@ -422,11 +422,13 @@ static void sof_handler(USBDriver *usbp) {
   osalSysUnlockFromISR();
 }
 
+bool msd_request_hook_new(USBDriver *usbp);
+
 // We need a custom hook to handle both MSD and CDC at the same time
 static bool hybridRequestHook(USBDriver *usbp) {
 #if HAL_USE_USB_MSD
 	// Try the MSD driver first
-	if (msd_request_hook(usbp)) {
+	if (msd_request_hook_new(usbp)) {
 		return true;
 	}
 #endif // HAL_USE_USB_MSD

firmware/rusefi.cpp

@@ -125,6 +125,7 @@
 #include "mpu_util.h"
 #include "tunerstudio.h"
 #include "mmc_card.h"
+#include "mass_storage_init.h"
 #include "trigger_emulator_algo.h"
 
 #if EFI_HD44780_LCD
@@ -201,6 +202,10 @@ void runRusEfi(void) {
 	startUsbConsole();
 #endif
 
+#if HAL_USE_USB_MSD
+	initUsbMsd();
+#endif
+
 	/**
 	 * Next we should initialize serial port console, it's important to know what's going on
 	 */