diff --git a/Cargo.lock b/Cargo.lock index 34c418d..9c447be 100644 --- a/Cargo.lock +++ b/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", diff --git a/Cargo.toml b/Cargo.toml index 9ff2bd8..94c83a7 100644 --- a/Cargo.toml +++ b/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" diff --git a/src/main.rs b/src/main.rs index 5c4bdea..03c90fc 100644 --- a/src/main.rs +++ b/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; @@ -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_19, - p.PIN_16, + p.PIN_18, // clk + p.PIN_19, // mosi + 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; diff --git a/src/scsi/mod.rs b/src/scsi/mod.rs index 4889bf4..464140a 100644 --- a/src/scsi/mod.rs +++ b/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>> { - sdcard: SdCard<'c>, +pub struct MassStorageClass<'d, D: Driver<'d>> { + sdcard: SdCard, bulk_out: D::EndpointOut, bulk_in: D::EndpointIn, } -impl<'d, 'c, D: Driver<'d>> MassStorageClass<'d, 'c, D> { - pub fn new(builder: &mut Builder<'d, D>, sdcard: SdCard<'c>) -> Self { +impl<'d, D: Driver<'d>> MassStorageClass<'d, D> { + 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]; - if let Ok(n) = self.bulk_out.read(&mut cbw_buf).await { - if let Some(cbw) = CommandBlockWrapper::parse(&cbw_buf[..n]) { - self.handle_command(&cbw.CBWCB).await; + loop { + let mut cbw_buf = [0u8; 31]; + if let Ok(n) = self.bulk_out.read(&mut cbw_buf).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 = 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 = 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 = 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)] diff --git a/src/storage.rs b/src/storage.rs index 685267a..991367c 100644 --- a/src/storage.rs +++ b/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>, - root: Option>, } -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) { - let vol = self.volume.as_mut().unwrap(); - let root = vol.open_root_dir().unwrap(); - self.root = Some(root); + pub async fn open_volume(&mut self) -> Result, ()> { + if self.is_attached() { + return Ok(self + .volume_mgr + .open_volume(VolumeIdx(0)) + .await + .map_err(|_| ())?); + } + Err(()) } - async fn get_volume(&'a mut self) { - let vol = self.volume_mgr.open_volume(VolumeIdx(0)).await.unwrap(); - self.volume = Some(vol); + pub fn size(&self) -> u64 { + let mut result = 0; + + 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 } } diff --git a/src/usb.rs b/src/usb.rs index 1659839..68e96e9 100644 --- a/src/usb.rs +++ b/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"); diff --git a/src/utils.rs b/src/utils.rs new file mode 100644 index 0000000..62683c6 --- /dev/null +++ b/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 + }} +}