From 1f3c16aea85f0f57a5d4e57731b56bdf2765b3db Mon Sep 17 00:00:00 2001
From: bunnie <bunnie@kosagi.com>
Date: Wed, 11 Jan 2023 03:55:04 +0800
Subject: [PATCH] add ReadFormatCapacities

this is required by Windows for the driver to work.
---
 .../scsi/commands/read_format_capacities.rs   | 21 +++++++++++++++++--
 .../firmware/usbd_scsi/src/scsi/scsi.rs       | 13 ++++++++++++
 2 files changed, 32 insertions(+), 2 deletions(-)

diff --git a/services/usb-device-xous/stm32-usb.rs/firmware/usbd_scsi/src/scsi/commands/read_format_capacities.rs b/services/usb-device-xous/stm32-usb.rs/firmware/usbd_scsi/src/scsi/commands/read_format_capacities.rs
index cef9baeb4..b8c607ef6 100644
--- a/services/usb-device-xous/stm32-usb.rs/firmware/usbd_scsi/src/scsi/commands/read_format_capacities.rs
+++ b/services/usb-device-xous/stm32-usb.rs/firmware/usbd_scsi/src/scsi/commands/read_format_capacities.rs
@@ -16,7 +16,24 @@ pub struct ReadFormatCapacitiesCommand {
     #[pkd(7, 0, 7, 8)]
     pub allocation_length: u16,
     
-    #[pkd(7, 0, 11, 11)]
+    #[pkd(7, 0, 9, 9)]
     pub control: Control,
 }
-impl ParsePackedStruct for ReadFormatCapacitiesCommand {}
\ No newline at end of file
+impl ParsePackedStruct for ReadFormatCapacitiesCommand {}
+
+#[derive(Clone, Copy, Eq, PartialEq, Debug, Packed)]
+#[packed(big_endian, lsb0)]
+pub struct ReadFormatCapacitiesResponse {
+    #[pkd(7, 0, 3, 3)]
+    pub capacity_list_length: u8,
+
+    #[pkd(7, 0, 4, 7)]
+    pub number_of_blocks: u32,
+
+    #[pkd(1, 0, 8, 8)]
+    pub descriptor_code: u8,
+
+    #[pkd(7, 0, 9, 11)]
+    pub block_length: u32,
+}
+impl ParsePackedStruct for ReadFormatCapacitiesResponse {}
\ No newline at end of file
diff --git a/services/usb-device-xous/stm32-usb.rs/firmware/usbd_scsi/src/scsi/scsi.rs b/services/usb-device-xous/stm32-usb.rs/firmware/usbd_scsi/src/scsi/scsi.rs
index 6f860e856..afc0e7714 100644
--- a/services/usb-device-xous/stm32-usb.rs/firmware/usbd_scsi/src/scsi/scsi.rs
+++ b/services/usb-device-xous/stm32-usb.rs/firmware/usbd_scsi/src/scsi/scsi.rs
@@ -251,6 +251,19 @@ impl<B: UsbBus, BD: BlockDevice> Scsi<'_, B, BD> {
                 }
             },
 
+            Command::ReadFormatCapacities(_rfc) => {
+                log::info!("got rfc");
+                let rfc_resp = ReadFormatCapacitiesResponse {
+                    capacity_list_length: 1 * 8, // 1 entry by 8 bytes
+                    number_of_blocks: self.block_device.max_lba(),
+                    descriptor_code: 1, // 1 - unformatted media; 2 - formatted media; 3 - no cartridge in drive
+                    block_length: BD::BLOCK_BYTES as u32,
+                };
+                let mut buf = self.inner.take_buffer_space(ReadFormatCapacitiesResponse::BYTES)?;
+                rfc_resp.pack(&mut buf)?;
+                Done
+            },
+
             _ => Err(Error::UnhandledOpCode)?,
         })
     }