WIP, sd fails to init

Cargo.lock

@@ -679,6 +679,7 @@ dependencies = [
  "defmt 0.3.100",
  "document-features",
  "embassy-time-driver",
+ "embassy-time-queue-utils",
  "embedded-hal 0.2.7",
  "embedded-hal 1.0.0",
  "embedded-hal-async",

Cargo.toml

@@ -12,7 +12,7 @@ opt-level = "z"
 
 [features]
 default = ["rp235x", "defmt"]
-rp2040 = ["embassy-rp/rp2040"]
+# rp2040 = ["embassy-rp/rp2040"]
 rp235x = ["embassy-rp/rp235xb"]
 trouble = ["dep:bt-hci", "dep:cyw43", "dep:cyw43-pio", "dep:trouble-host"]
 defmt = [
@@ -44,7 +44,7 @@ embassy-rp = { version = "0.4.0", features = [
   "binary-info",
 ] }
 embassy-futures = "0.1.1"
-embassy-time = "0.4.0"
+embassy-time = { version = "0.4.0", features = ["generic-queue-8"] }
 embassy-embedded-hal = "0.3.1"
 embassy-sync = "0.7"
 embassy-usb = "0.4.0"

src/main.rs

@@ -37,6 +37,7 @@ use display::display_handler;
 mod scsi;
 mod storage;
 mod usb;
+mod utils;
 
 embassy_rp::bind_interrupts!(struct Irqs {
     I2C1_IRQ => i2c::InterruptHandler<I2C1>;
@@ -65,23 +66,29 @@ async fn main(_spawner: Spawner) {
     let sdcard = {
         let mut config = spi::Config::default();
         config.frequency = 400_000;
-        let spi = Spi::new(
+        let mut spi = Spi::new(
             p.SPI0,
-            p.PIN_18,
+            p.PIN_18, // clk
-            p.PIN_19,
+            p.PIN_19, // mosi
-            p.PIN_16,
+            p.PIN_16, // miso
             p.DMA_CH2,
             p.DMA_CH3,
             config.clone(),
         );
-        let cs = Output::new(p.PIN_5, Level::High);
+        let cs = Output::new(p.PIN_17, Level::High);
+        let det = Input::new(p.PIN_22, Pull::None);
 
         let device = ExclusiveDevice::new(spi, cs, Delay).unwrap();
+        Timer::after_millis(500).await;
         let sdcard = SdmmcSdCard::new(device, Delay);
+        while sdcard.num_bytes().await.is_err() {
+            Timer::after_millis(250).await;
+            defmt::error!("Sd init failed, trying again");
+        }
 
         config.frequency = 32_000_000;
         sdcard.spi(|dev| dev.bus_mut().set_config(&config));
-        SdCard::new(sdcard, Input::new(p.PIN_22, Pull::None))
+        SdCard::new(sdcard, det)
     };
 
     usb_handler(usb, sdcard).await;

src/scsi/mod.rs

@@ -1,20 +1,22 @@
+use crate::format;
 use embassy_usb::driver::{Driver, EndpointIn, EndpointOut};
 use embassy_usb::types::StringIndex;
 use embassy_usb::{Builder, Config};
+use heapless::Vec;
 
 mod scsi_types;
 use scsi_types::*;
 
 use crate::storage::SdCard;
 
-pub struct MassStorageClass<'d, 'c, D: Driver<'d>> {
+pub struct MassStorageClass<'d, D: Driver<'d>> {
-    sdcard: SdCard<'c>,
+    sdcard: SdCard,
     bulk_out: D::EndpointOut,
     bulk_in: D::EndpointIn,
 }
 
-impl<'d, 'c, D: Driver<'d>> MassStorageClass<'d, 'c, D> {
+impl<'d, D: Driver<'d>> MassStorageClass<'d, D> {
-    pub fn new(builder: &mut Builder<'d, D>, sdcard: SdCard<'c>) -> Self {
+    pub fn new(builder: &mut Builder<'d, D>, sdcard: SdCard) -> Self {
         let mut function = builder.function(0x08, 0x06, 0x50); // Mass Storage class
         let mut interface = function.interface();
         let mut alt = interface.alt_setting(0x08, 0x06, 0x50, None);
@@ -30,26 +32,118 @@ impl<'d, 'c, D: Driver<'d>> MassStorageClass<'d, 'c, D> {
     }
 
     pub async fn poll(&mut self) {
-        let mut cbw_buf = [0u8; 31];
+        loop {
-        if let Ok(n) = self.bulk_out.read(&mut cbw_buf).await {
+            let mut cbw_buf = [0u8; 31];
-            if let Some(cbw) = CommandBlockWrapper::parse(&cbw_buf[..n]) {
+            if let Ok(n) = self.bulk_out.read(&mut cbw_buf).await {
-                self.handle_command(&cbw.CBWCB).await;
+                if n == 31 {
+                    if let Some(cbw) = CommandBlockWrapper::parse(&cbw_buf[..n]) {
+                        // TODO: validate cbw
+                        if self.handle_command(&cbw.CBWCB).await.is_ok() {
+                            self.send_csw_success(cbw.dCBWTag).await
+                        } else {
+                            self.send_csw_success(cbw.dCBWTag).await
+                        }
+                    }
+                }
             }
         }
     }
 
-    async fn handle_command(&self, cbw: &[u8]) {
+    async fn handle_command(&mut self, cbw: &[u8]) -> Result<(), ()> {
         match parse_cb(cbw) {
             ScsiCommand::Unknown => {
                 #[cfg(feature = "defmt")]
                 defmt::info!("Got unexpected scsi command: {}", cbw);
+                Err(())
             }
             ScsiCommand::Inquiry {
                 evpd,
                 page_code,
                 alloc_len,
-            } => todo!(),
+            } => {
-            ScsiCommand::TestUnitReady => todo!(),
+                #[cfg(feature = "defmt")]
+                defmt::info!(
+                    "SCSI INQUIRY: evpd={}, page_code=0x{:02x}, alloc_len={}",
+                    evpd,
+                    page_code,
+                    alloc_len
+                );
+
+                let mut response: Vec<u8, 64> = Vec::new();
+                if !evpd {
+                    response.push(0x00).unwrap(); // Direct-access block device
+                    response.push(0x80).unwrap(); // Removable
+                    response.push(0x05).unwrap(); // SPC-3 compliance
+                    response.push(0x02).unwrap(); // Response data format
+                    response.push(0).unwrap(); // Additional length
+
+                    // Vendor ID (8 bytes)
+                    response.extend_from_slice(b"RUSTUSB ").unwrap();
+                    // Product ID (16 bytes)
+                    response.extend_from_slice(b"Mass Storage    ").unwrap();
+
+                    // Product Revision (4 bytes): encode volume size in GB
+                    let size_bytes = self.sdcard.size();
+                    let size_gb = ((size_bytes + 500_000_000) / 1_000_000_000) as u32;
+                    let rev_str = format!(4, "{}", size_gb);
+
+                    let rev_bytes = rev_str.as_bytes();
+                    response
+                        .extend_from_slice(&[
+                            *rev_bytes.get(0).unwrap_or(&b'0'),
+                            *rev_bytes.get(1).unwrap_or(&b'0'),
+                            *rev_bytes.get(2).unwrap_or(&b'0'),
+                            *rev_bytes.get(3).unwrap_or(&b'0'),
+                        ])
+                        .unwrap();
+
+                    // Now fix up the Additional Length
+                    let addl_len = response.len() - 5;
+                    response[4] = addl_len as u8;
+                } else {
+                    match page_code {
+                        0x00 => {
+                            response
+                                .extend_from_slice(&[0x00, 0x00, 0x00, 0x03, 0x00, 0x80, 0x83])
+                                .unwrap();
+                        }
+                        0x80 => {
+                            let serial = b"RUST1234";
+                            let mut data: Vec<u8, 64> = Vec::new();
+                            data.extend_from_slice(&[0x00, 0x80, 0x00, serial.len() as u8])
+                                .unwrap();
+                            data.extend_from_slice(serial).unwrap();
+                        }
+                        0x83 => {
+                            let id = b"RUSTVOL1";
+                            let mut data: Vec<u8, 64> = Vec::new();
+                            data.extend_from_slice(&[
+                                0x00,
+                                0x83, // Page code
+                                0x00,
+                                (4 + id.len()) as u8, // Length
+                                0x02,                 // ASCII identifier
+                                0x01,                 // Identifier type
+                                0x00,                 // Reserved
+                                id.len() as u8,
+                            ])
+                            .unwrap();
+                            data.extend_from_slice(id).unwrap();
+                        }
+                        _ => (),
+                    }
+                };
+
+                let len = core::cmp::min(alloc_len as usize, response.len());
+                self.bulk_in.write(&response[..len]).await.map_err(|_| ())
+            }
+            ScsiCommand::TestUnitReady => {
+                if self.sdcard.is_attached() {
+                    Ok(())
+                } else {
+                    Err(())
+                }
+            }
             ScsiCommand::RequestSense { desc, alloc_len } => todo!(),
             ScsiCommand::ModeSense6 {
                 dbd,
@@ -65,13 +159,42 @@ impl<'d, 'c, D: Driver<'d>> MassStorageClass<'d, 'c, D> {
                 subpage_code,
                 alloc_len,
             } => todo!(),
-            ScsiCommand::ReadCapacity10 => todo!(),
+            ScsiCommand::ReadCapacity10 => {
+                const block_size: u64 = 512;
+                let total_blocks = self.sdcard.size() / block_size;
+                defmt::info!("total size: {}", self.sdcard.size());
+
+                let last_lba = total_blocks - 1;
+
+                let mut resp = [0u8; 8];
+                resp[0..4].copy_from_slice(&(last_lba as u32).to_be_bytes());
+                resp[4..8].copy_from_slice(&(block_size as u32).to_be_bytes());
+
+                self.bulk_in.write(&resp).await.map_err(|_| ())
+            }
             ScsiCommand::ReadCapacity16 { alloc_len } => todo!(),
             ScsiCommand::Read { lba, len } => todo!(),
             ScsiCommand::Write { lba, len } => todo!(),
             ScsiCommand::ReadFormatCapacities { alloc_len } => todo!(),
         }
     }
+
+    pub async fn send_csw_success(&mut self, tag: u32) {
+        self.send_csw(tag, 0x00, 0).await;
+    }
+
+    pub async fn send_csw_fail(&mut self, tag: u32) {
+        self.send_csw(tag, 0x01, 0).await; // 0x01 = Command Failed
+    }
+
+    pub async fn send_csw(&mut self, tag: u32, status: u8, residue: u32) {
+        let mut csw = [0u8; 13];
+        csw[0..4].copy_from_slice(&0x53425355u32.to_le_bytes()); // Signature "USBS"
+        csw[4..8].copy_from_slice(&tag.to_le_bytes());
+        csw[8..12].copy_from_slice(&residue.to_le_bytes());
+        csw[12] = status;
+        let _ = self.bulk_in.write(&csw).await;
+    }
 }
 
 #[repr(C, packed)]

src/storage.rs

@@ -1,10 +1,11 @@
+use embassy_futures::block_on;
 use embassy_rp::gpio::{Input, Output};
 use embassy_rp::peripherals::SPI0;
 use embassy_rp::spi::{Async, Spi};
 use embedded_hal_bus::spi::ExclusiveDevice;
 use embedded_sdmmc;
 use embedded_sdmmc::asynchronous::{
-    Directory, SdCard as SdmmcSdCard, Volume, VolumeIdx, VolumeManager,
+    BlockCount, BlockDevice, Directory, SdCard as SdmmcSdCard, Volume, VolumeIdx, VolumeManager,
 };
 use embedded_sdmmc::blocking::{TimeSource, Timestamp};
 
@@ -25,15 +26,18 @@ impl TimeSource for DummyTimeSource {
     }
 }
 
-pub struct SdCard<'a> {
+pub struct SdCard {
     det: Input<'static>,
     volume_mgr: VolMgr,
-    volume: Option<Vol<'a>>,
-    root: Option<Dir<'a>>,
 }
 
-impl<'a> SdCard<'a> {
+impl SdCard {
     pub fn new(sdcard: SD, det: Input<'static>) -> Self {
+        block_on(sdcard.get_card_type()).unwrap();
+        defmt::info!(
+            "Card size is {} bytes",
+            block_on(sdcard.num_bytes()).unwrap()
+        );
         let volume_mgr = VolumeManager::<_, _, MAX_DIRS, MAX_FILES, MAX_VOLUMES>::new_with_limits(
             sdcard,
             DummyTimeSource {},
@@ -42,25 +46,45 @@ impl<'a> SdCard<'a> {
         Self {
             det: det,
             volume_mgr,
-            volume: None,
-            root: None,
         }
     }
 
     /// Returns true if an SD card is inserted.
     /// The DET pin is active-low via mechanical switch in the socket.
-    fn attached(&self) -> bool {
+    pub fn is_attached(&self) -> bool {
         self.det.is_low()
     }
 
-    async fn get_root(&'a mut self) {
+    pub async fn open_volume(&mut self) -> Result<Vol<'_>, ()> {
-        let vol = self.volume.as_mut().unwrap();
+        if self.is_attached() {
-        let root = vol.open_root_dir().unwrap();
+            return Ok(self
-        self.root = Some(root);
+                .volume_mgr
+                .open_volume(VolumeIdx(0))
+                .await
+                .map_err(|_| ())?);
+        }
+        Err(())
     }
 
-    async fn get_volume(&'a mut self) {
+    pub fn size(&self) -> u64 {
-        let vol = self.volume_mgr.open_volume(VolumeIdx(0)).await.unwrap();
+        let mut result = 0;
-        self.volume = Some(vol);
+
+        self.volume_mgr.device(|sd| {
+            result = block_on(sd.num_bytes()).unwrap_or(0);
+            DummyTimeSource {}
+        });
+
+        result
+    }
+
+    pub fn blocks(&self) -> u32 {
+        let mut result = 0;
+
+        self.volume_mgr.device(|sd| {
+            result = block_on(sd.num_blocks()).unwrap_or(BlockCount(0)).0;
+            DummyTimeSource {}
+        });
+
+        result
     }
 }

src/usb.rs

@@ -9,7 +9,7 @@ use embassy_rp::{
 use embassy_time::Delay;
 use embassy_usb::{Builder, Config};
 
-pub async fn usb_handler(driver: Driver<'static, USB>, mut sdcard: SdCard<'_>) {
+pub async fn usb_handler(driver: Driver<'static, USB>, sdcard: SdCard) {
     let mut config = Config::new(0xc0de, 0xcafe);
     config.manufacturer = Some("LegitCamper");
     config.product = Some("PicoCalc");

src/utils.rs

@@ -0,0 +1,11 @@
+#[macro_export]
+macro_rules! format {
+    ($len:literal, $($arg:tt)*) => {{
+        use heapless::String;
+        use core::fmt::Write;
+
+        let mut s: String<$len> = String::new();
+        let _ = write!(&mut s, $($arg)*);
+        s
+    }}
+}