diff --git a/drivers/storage/nvmed/src/nvme/cmd.rs b/drivers/storage/nvmed/src/nvme/cmd.rs index b3567d9923..80aad3d250 100644 --- a/drivers/storage/nvmed/src/nvme/cmd.rs +++ b/drivers/storage/nvmed/src/nvme/cmd.rs @@ -159,4 +159,22 @@ impl NvmeCmd { cdw15: 0, } } + + pub fn set_features_num_queues(cid: u16, nsid: u32, ncqr: u16, nsqr: u16) -> Self { + Self { + opcode: 9, // Set Features + flags: 0, + cid, + nsid, + _rsvd: 0, + mptr: 0, + dptr: [0, 0], + cdw10: 0x07, // Feature ID: Number of Queues + cdw11: ((ncqr as u32) << 16) | (nsqr as u32), + cdw12: 0, + cdw13: 0, + cdw14: 0, + cdw15: 0, + } + } } diff --git a/drivers/storage/nvmed/src/nvme/mod.rs b/drivers/storage/nvmed/src/nvme/mod.rs index 682ee93329..2bd517e5ea 100644 --- a/drivers/storage/nvmed/src/nvme/mod.rs +++ b/drivers/storage/nvmed/src/nvme/mod.rs @@ -423,21 +423,36 @@ impl Nvme { namespaces.insert(nsid, self.identify_namespace(nsid).await); } - // TODO: Multiple queues - let cq = self.create_io_completion_queue(1, Some(0)).await; - log::trace!("created compq"); - let sq = self.create_io_submission_queue(1, 1).await; - log::trace!("created subq"); - self.thread_ctxts - .read() - .get(&0) - .unwrap() - .lock() - .queues - .borrow_mut() - .insert(1, (sq, cq)); - self.sq_ivs.write().insert(1, 0); - self.cq_ivs.write().insert(1, 0); + // NVMe Set Features / Number of Queues (Feature ID 0x07). + // Linux 7.1 reference: drivers/nvme/host/core.c nvme_set_queue_count(). + // Request max I/O queues from controller, allocate up to + // min(requested, allocated, CPU_COUNT) queue pairs. + let nvme_queue_count = self + .submit_and_complete_admin_command(|cid| { + NvmeCmd::set_features_num_queues(cid, 0, 64, 64) + }) + .await; + let ncqr = nvme_queue_count.command_specific; + let nsqr = ncqr >> 16; + let ncqr = ncqr & 0xFFFF; + let n_queues = (nsqr.min(ncqr) as usize).min(8).max(1); + log::info!("nvmed: controller supports {} SQ / {} CQ, creating {} I/O queue pairs", nsqr, ncqr, n_queues); + + for qid in 1..=n_queues as u16 { + let cq = self.create_io_completion_queue(qid, Some((qid - 1) as u16 % 4)).await; + let sq = self.create_io_submission_queue(qid, qid).await; + log::trace!("nvmed: created I/O queue pair qid={}", qid); + self.thread_ctxts + .read() + .get(&0) + .unwrap() + .lock() + .queues + .borrow_mut() + .insert(qid, (sq, cq)); + self.sq_ivs.write().insert(qid, (qid - 1) as u16 % 4); + self.cq_ivs.write().insert(qid, (qid - 1) as u16 % 4); + } namespaces }