usb: device_next: accept padded Mass Storage CDBs

Universal Serial Bus Mass Storage Specification For Bootability requires
that device shall accept Bulk-Only-Transport bCBWCBLength of 12 for all
commands documented in the specification. Allow padding for commands
documented in Bootability specification, but leave others intact.

This fixes Request Sense on Windows 11 which previously failed due to
bCBWCBLength being 12 instead of 6.

Signed-off-by: Tomasz Moń <tomasz.mon@nordicsemi.no>

Author: tmon-nordic

subsys/usb/device_next/class/usbd_msc.c

@@ -304,8 +304,10 @@ static void msc_process_cbw(struct msc_bot_ctx *ctx)
 	struct scsi_ctx *lun = &ctx->luns[ctx->cbw.bCBWLUN];
 	bool cmd_is_data_read, cmd_is_data_write;
 	size_t data_len;
+	int cb_len;
 
-	data_len = scsi_cmd(lun, ctx->cbw.CBWCB, ctx->cbw.bCBWCBLength, ctx->scsi_buf);
+	cb_len = scsi_usb_boot_cmd_len(ctx->cbw.CBWCB, ctx->cbw.bCBWCBLength);
+	data_len = scsi_cmd(lun, ctx->cbw.CBWCB, cb_len, ctx->scsi_buf);
 	ctx->scsi_bytes = data_len;
 	ctx->scsi_offset = 0;
 	cmd_is_data_read = scsi_cmd_is_data_read(lun);

subsys/usb/device_next/class/usbd_msc_scsi.c

@@ -806,6 +806,29 @@ SCSI_CMD_HANDLER(MODE_SENSE_10)
 	return good(ctx, length);
 }
 
+int scsi_usb_boot_cmd_len(const uint8_t *cb, int len)
+{
+	/* Universal Serial Bus Mass Storage Specification For Bootability
+	 * requires device to accept CBW padded to 12 bytes for commands
+	 * documented in Bootability specification. Windows 11 uses padding
+	 * for REQUEST SENSE command.
+	 */
+	if (len != 12) {
+		return len;
+	}
+
+	switch (cb[0]) {
+	case TEST_UNIT_READY:	return sizeof(SCSI_CMD_STRUCT(TEST_UNIT_READY));
+	case REQUEST_SENSE:	return sizeof(SCSI_CMD_STRUCT(REQUEST_SENSE));
+	case INQUIRY:		return sizeof(SCSI_CMD_STRUCT(INQUIRY));
+	case READ_CAPACITY_10:	return sizeof(SCSI_CMD_STRUCT(READ_CAPACITY_10));
+	case READ_10:		return sizeof(SCSI_CMD_STRUCT(READ_10));
+	case WRITE_10:		return sizeof(SCSI_CMD_STRUCT(WRITE_10));
+	case MODE_SENSE_10:	return sizeof(SCSI_CMD_STRUCT(MODE_SENSE_10));
+	default:		return len;
+	}
+}
+
 size_t scsi_cmd(struct scsi_ctx *ctx, const uint8_t *cb, int len,
 		uint8_t data_in_buf[static CONFIG_USBD_MSC_SCSI_BUFFER_SIZE])
 {

subsys/usb/device_next/class/usbd_msc_scsi.h

@@ -84,6 +84,7 @@ struct scsi_ctx {
 void scsi_init(struct scsi_ctx *ctx, const char *disk, const char *vendor,
 	       const char *product, const char *revision);
 void scsi_reset(struct scsi_ctx *ctx);
+int scsi_usb_boot_cmd_len(const uint8_t *cb, int len);
 size_t scsi_cmd(struct scsi_ctx *ctx, const uint8_t *cb, int len,
 		uint8_t data_in_buf[static CONFIG_USBD_MSC_SCSI_BUFFER_SIZE]);