From df4a7b054b1e1b387aaaf1218ce279acfe44aa34 Mon Sep 17 00:00:00 2001 From: Jeremy Soller Date: Sat, 24 Aug 2019 08:37:38 -0600 Subject: [PATCH] nvmed: Identify controller and namespaces --- nvmed/src/nvme.rs | 246 +++++++++++++++++++++++++++++++++++++--------- 1 file changed, 202 insertions(+), 44 deletions(-) diff --git a/nvmed/src/nvme.rs b/nvmed/src/nvme.rs index 793c32a2da..0dcf29cd82 100644 --- a/nvmed/src/nvme.rs +++ b/nvmed/src/nvme.rs @@ -1,7 +1,8 @@ -use std::thread; +use std::{mem, thread}; use syscall::io::{Dma, Io, Mmio}; use syscall::error::Result; +#[derive(Clone, Copy)] #[repr(packed)] pub struct NvmeCmd { /// Opcode @@ -38,7 +39,7 @@ impl NvmeCmd { opcode: 5, flags: 0, cid: cid, - nsid: 0xFFFFFFFF, + nsid: 0, _rsvd: 0, mptr: 0, dptr: [ptr as u64, 0], @@ -56,7 +57,7 @@ impl NvmeCmd { opcode: 1, flags: 0, cid: cid, - nsid: 0xFFFFFFFF, + nsid: 0, _rsvd: 0, mptr: 0, dptr: [ptr as u64, 0], @@ -69,12 +70,30 @@ impl NvmeCmd { } } + pub fn identify_namespace(cid: u16, ptr: usize, nsid: u32) -> Self { + Self { + opcode: 6, + flags: 0, + cid: cid, + nsid: nsid, + _rsvd: 0, + mptr: 0, + dptr: [ptr as u64, 0], + cdw10: 0, + cdw11: 0, + cdw12: 0, + cdw13: 0, + cdw14: 0, + cdw15: 0, + } + } + pub fn identify_controller(cid: u16, ptr: usize) -> Self { Self { opcode: 6, flags: 0, cid: cid, - nsid: 0xFFFFFFFF, + nsid: 0, _rsvd: 0, mptr: 0, dptr: [ptr as u64, 0], @@ -105,12 +124,12 @@ impl NvmeCmd { } } - pub fn io_read(cid: u16, lba: u64, count: u16, ptr: usize) -> Self { + pub fn io_read(cid: u16, nsid: u32, lba: u64, count: u16, ptr: usize) -> Self { Self { opcode: 2, flags: 1 << 6, cid: cid, - nsid: 0xFFFFFFFF, + nsid: nsid, _rsvd: 0, mptr: 0, dptr: [ptr as u64, (count as u64) << 9], @@ -123,12 +142,12 @@ impl NvmeCmd { } } - pub fn io_write(cid: u16, lba: u64, count: u16, ptr: usize) -> Self { + pub fn io_write(cid: u16, nsid: u32, lba: u64, count: u16, ptr: usize) -> Self { Self { opcode: 1, flags: 1 << 6, cid: cid, - nsid: 0xFFFFFFFF, + nsid: nsid, _rsvd: 0, mptr: 0, dptr: [ptr as u64, (count as u64) << 9], @@ -142,6 +161,7 @@ impl NvmeCmd { } } +#[derive(Clone, Copy, Debug)] #[repr(packed)] pub struct NvmeComp { command_specific: u32, @@ -182,19 +202,77 @@ pub struct NvmeRegs { cmbsz: Mmio, } +pub struct NvmeCmdQueue { + data: Dma<[NvmeCmd; 64]>, + i: usize, +} + +impl NvmeCmdQueue { + fn new() -> Result { + Ok(Self { + data: Dma::zeroed()?, + i: 0, + }) + } + + fn submit(&mut self, entry: NvmeCmd) -> usize { + self.data[self.i] = entry; + self.i = (self.i + 1) % self.data.len(); + self.i + } +} + +pub struct NvmeCompQueue { + data: Dma<[NvmeComp; 256]>, + i: usize, + phase: bool, +} + +impl NvmeCompQueue { + fn new() -> Result { + Ok(Self { + data: Dma::zeroed()?, + i: 0, + phase: true, + }) + } + + fn complete(&mut self) -> Option<(usize, NvmeComp)> { + let entry = self.data[self.i]; + if ((entry.status & 1) == 1) == self.phase { + self.i = (self.i + 1) % self.data.len(); + if self.i == 0 { + self.phase = ! self.phase; + } + Some((self.i, entry)) + } else { + None + } + } + + fn complete_spin(&mut self) -> (usize, NvmeComp) { + loop { + if let Some(some) = self.complete() { + return some; + } else { + thread::yield_now(); + } + } + } +} + pub struct Nvme { regs: &'static mut NvmeRegs, - submission_queues: [Dma<[NvmeCmd; 64]>; 2], - completion_queues: [Dma<[NvmeComp; 256]>; 2], - + submission_queues: [NvmeCmdQueue; 2], + completion_queues: [NvmeCompQueue; 2], } impl Nvme { pub fn new(address: usize) -> Result { Ok(Nvme { regs: unsafe { &mut *(address as *mut NvmeRegs) }, - submission_queues: [Dma::zeroed()?, Dma::zeroed()?], - completion_queues: [Dma::zeroed()?, Dma::zeroed()?], + submission_queues: [NvmeCmdQueue::new()?, NvmeCmdQueue::new()?], + completion_queues: [NvmeCompQueue::new()?, NvmeCompQueue::new()?], }) } @@ -203,7 +281,6 @@ impl Nvme { let addr = (self.regs as *mut _ as usize) + 0x1000 + index * (4 << dstrd); - println!("doorbell {:X}", addr); &mut *(addr as *mut Mmio) } @@ -225,19 +302,21 @@ impl Nvme { self.regs.cc.writef(1, false); for (qid, queue) in self.completion_queues.iter().enumerate() { - println!(" - completion queue {}: {:X}, {}", qid, queue.physical(), queue.len()); + let data = &queue.data; + println!(" - completion queue {}: {:X}, {}", qid, data.physical(), data.len()); } for (qid, queue) in self.submission_queues.iter().enumerate() { - println!(" - submission queue {}: {:X}, {}", qid, queue.physical(), queue.len()); + let data = &queue.data; + println!(" - submission queue {}: {:X}, {}", qid, data.physical(), data.len()); } { let asq = &self.submission_queues[0]; let acq = &self.completion_queues[0]; - self.regs.aqa.write(((acq.len() as u32) << 16) | (asq.len() as u32)); - self.regs.asq.write(asq.physical() as u64); - self.regs.acq.write(acq.physical() as u64); + self.regs.aqa.write(((acq.data.len() as u32) << 16) | (asq.data.len() as u32)); + self.regs.asq.write(asq.data.physical() as u64); + self.regs.acq.write(acq.data.physical() as u64); // Set IOCQES, IOSQES, AMS, MPS, and CSS let mut cc = self.regs.cc.read(); @@ -254,39 +333,118 @@ impl Nvme { thread::yield_now(); } - let nsids: Dma<[u16; 2048]> = Dma::zeroed().unwrap(); - - println!(" - Attempting to retrieve namespace ID list"); { - let qid = 0; - let queue = &mut self.submission_queues[qid]; - let cid = 0; - let cmd = &mut queue[cid]; + let data: Dma<[u8; 4096]> = Dma::zeroed().unwrap(); - *cmd = NvmeCmd::identify_namespace_list(cid as u16, nsids.physical(), 0); - - self.submission_queue_tail(qid as u16, (cid as u16) + 1); - } - - println!(" - Waiting to retrieve namespace ID list"); - { - let qid = 0; - let queue = &self.completion_queues[qid]; - let cid = 0; - let comp = &queue[cid]; - - while comp.status & 1 == 0 { - thread::yield_now(); + println!(" - Attempting to identify controller"); + { + let qid = 0; + let queue = &mut self.submission_queues[qid]; + let cid = queue.i as u16; + let entry = NvmeCmd::identify_controller(cid, data.physical()); + let tail = queue.submit(entry); + self.submission_queue_tail(qid as u16, tail as u16); } - self.completion_queue_head(qid as u16, (cid as u16) + 1); + println!(" - Waiting to identify controller"); + { + let qid = 0; + let queue = &mut self.completion_queues[qid]; + let (head, entry) = queue.complete_spin(); + self.completion_queue_head(qid as u16, head as u16); + } + + println!(" - Dumping identify controller"); + + let mut serial = String::new(); + for &b in &data[4..24] { + if b == 0 { + break; + } + serial.push(b as char); + } + println!(" - Serial: {}", serial); + + let mut model = String::new(); + for &b in &data[24..64] { + if b == 0 { + break; + } + model.push(b as char); + } + println!(" - Model: {}", model); + + let mut firmware = String::new(); + for &b in &data[64..72] { + if b == 0 { + break; + } + firmware.push(b as char); + } + println!(" - Firmware: {}", firmware); + } + + let mut nsids = Vec::new(); + { + let data: Dma<[u32; 1024]> = Dma::zeroed().unwrap(); + + println!(" - Attempting to retrieve namespace ID list"); + { + let qid = 0; + let queue = &mut self.submission_queues[qid]; + let cid = queue.i as u16; + let entry = NvmeCmd::identify_namespace_list(cid, data.physical(), 0); + let tail = queue.submit(entry); + self.submission_queue_tail(qid as u16, tail as u16); + } + + println!(" - Waiting to retrieve namespace ID list"); + { + let qid = 0; + let queue = &mut self.completion_queues[qid]; + let (head, entry) = queue.complete_spin(); + self.completion_queue_head(qid as u16, head as u16); + } + + println!(" - Dumping namespace ID list"); + for &nsid in data.iter() { + if nsid != 0 { + println!(" - {}", nsid); + nsids.push(nsid); + } + } } - println!(" - Dumping namespace ID list"); for &nsid in nsids.iter() { - if nsid != 0 { - println!("{:X}", nsid); + let data: Dma<[u8; 4096]> = Dma::zeroed().unwrap(); + + println!(" - Attempting to identify namespace {}", nsid); + { + let qid = 0; + let queue = &mut self.submission_queues[qid]; + let cid = queue.i as u16; + let entry = NvmeCmd::identify_namespace(cid, data.physical(), nsid); + let tail = queue.submit(entry); + self.submission_queue_tail(qid as u16, tail as u16); } + + println!(" - Waiting to identify namespace {}", nsid); + { + let qid = 0; + let queue = &mut self.completion_queues[qid]; + let (head, entry) = queue.complete_spin(); + self.completion_queue_head(qid as u16, head as u16); + } + + println!(" - Dumping identify namespace"); + + let size = *(data.as_ptr().offset(0) as *const u64); + println!(" - Size: {}", size); + + let capacity = *(data.as_ptr().offset(8) as *const u64); + println!(" - Capacity: {}", capacity); + + //TODO: Read block size } } }