dev/block: nvme hello world
This commit is contained in:
parent
2b70a35882
commit
d1fe89c134
@ -2,7 +2,7 @@
|
||||
use core::fmt;
|
||||
|
||||
use acpi_lib::mcfg::McfgEntry;
|
||||
use alloc::{rc::Rc, vec::Vec};
|
||||
use alloc::{boxed::Box, rc::Rc, vec::Vec};
|
||||
use bitflags::bitflags;
|
||||
use device_api::Device;
|
||||
use kernel_util::sync::IrqSafeSpinlock;
|
||||
@ -14,7 +14,10 @@ pub use space::{
|
||||
ecam::PciEcam, PciConfigSpace, PciConfigurationSpace, PciLegacyConfigurationSpace,
|
||||
};
|
||||
|
||||
use crate::mem::{address::FromRaw, PhysicalAddress};
|
||||
use crate::{
|
||||
device::nvme,
|
||||
mem::{address::FromRaw, PhysicalAddress},
|
||||
};
|
||||
|
||||
bitflags! {
|
||||
/// Command register of the PCI configuration space
|
||||
@ -241,6 +244,13 @@ impl PciConfigurationSpace for PciConfigSpace {
|
||||
_ => todo!(),
|
||||
}
|
||||
}
|
||||
|
||||
fn write_u32(&self, offset: usize, value: u32) {
|
||||
match self {
|
||||
Self::Ecam(ecam) => ecam.write_u32(offset, value),
|
||||
_ => todo!(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn setup_bus_device(device: &mut PciBusDevice) -> Result<(), Error> {
|
||||
@ -259,6 +269,13 @@ fn setup_bus_device(device: &mut PciBusDevice) -> Result<(), Error> {
|
||||
|
||||
// Match by class
|
||||
match (config.class_code(), config.subclass(), config.prog_if()) {
|
||||
(0x01, 0x08, 0x02) => {
|
||||
let dev = Box::leak(Box::new(nvme::NvmeController::from_pci_bus(&device.info)?));
|
||||
|
||||
unsafe {
|
||||
dev.init()?;
|
||||
}
|
||||
}
|
||||
_ => {
|
||||
debugln!(" -> No driver");
|
||||
}
|
||||
|
||||
@ -17,6 +17,11 @@ impl PciConfigurationSpace for PciEcam {
|
||||
assert_eq!(offset & 3, 0);
|
||||
unsafe { ((self.mapping.address() + offset) as *const u32).read_volatile() }
|
||||
}
|
||||
|
||||
fn write_u32(&self, offset: usize, value: u32) {
|
||||
assert_eq!(offset & 3, 0);
|
||||
unsafe { ((self.mapping.address() + offset) as *mut u32).write_volatile(value) }
|
||||
}
|
||||
}
|
||||
|
||||
impl PciEcam {
|
||||
|
||||
@ -16,6 +16,20 @@ macro_rules! pci_config_field_getter {
|
||||
};
|
||||
}
|
||||
|
||||
macro_rules! pci_config_field_setter {
|
||||
($self:ident, u32, $offset:expr, $value:expr) => {
|
||||
$self.write_u32($offset, $value)
|
||||
};
|
||||
|
||||
($self:ident, u16, $offset:expr, $value:expr) => {{
|
||||
$self.write_u16($offset, $value)
|
||||
}};
|
||||
|
||||
($self:ident, u8, $offset:expr, $value:expr) => {
|
||||
$self.write_u8($offset, $value)
|
||||
};
|
||||
}
|
||||
|
||||
macro_rules! pci_config_field {
|
||||
(
|
||||
$offset:expr => $ty:ident,
|
||||
@ -29,8 +43,8 @@ macro_rules! pci_config_field {
|
||||
|
||||
$(
|
||||
$(#[$setter_meta])*
|
||||
fn $setter(&self, _value: $ty) {
|
||||
todo!()
|
||||
fn $setter(&self, value: $ty) {
|
||||
pci_config_field_setter!(self, $ty, $offset, value)
|
||||
}
|
||||
)?
|
||||
};
|
||||
@ -67,6 +81,13 @@ pub trait PciConfigurationSpace {
|
||||
/// The `offset` must be u32-aligned.
|
||||
fn read_u32(&self, offset: usize) -> u32;
|
||||
|
||||
/// Writes a 32-bit value to the device configuration space.
|
||||
///
|
||||
/// # Note
|
||||
///
|
||||
/// The `offset` must be u32-aligned.
|
||||
fn write_u32(&self, offset: usize, value: u32);
|
||||
|
||||
/// Reads a 16-bit value from the device configuration space.
|
||||
///
|
||||
/// # Note
|
||||
@ -78,12 +99,31 @@ pub trait PciConfigurationSpace {
|
||||
(value >> ((offset & 3) * 8)) as u16
|
||||
}
|
||||
|
||||
/// Reads a byte from the device configuration space.
|
||||
/// Reads a byte from the device configuration space
|
||||
fn read_u8(&self, offset: usize) -> u8 {
|
||||
let value = self.read_u32(offset & !3);
|
||||
(value >> ((offset & 3) * 8)) as u8
|
||||
}
|
||||
|
||||
/// Writes a 16-bit value to the device configuration space.
|
||||
///
|
||||
/// # Note
|
||||
///
|
||||
/// The `offset` must be u16-aligned.
|
||||
fn write_u16(&self, offset: usize, value: u16) {
|
||||
let shift = ((offset >> 1) & 1) << 4;
|
||||
assert_eq!(offset & 1, 0);
|
||||
let mut tmp = self.read_u32(offset & !3);
|
||||
tmp &= !(0xFFFF << shift);
|
||||
tmp |= (value as u32) << shift;
|
||||
self.write_u32(offset & !3, tmp);
|
||||
}
|
||||
|
||||
/// Writes a byte to the device configuration space
|
||||
fn write_u8(&self, offset: usize, value: u16) {
|
||||
todo!()
|
||||
}
|
||||
|
||||
/// Returns `true` if the device is present on the bus (i.e. configuration space is not filled
|
||||
/// with only 1's)
|
||||
fn is_valid(&self) -> bool {
|
||||
|
||||
@ -10,6 +10,8 @@ pub mod devtree;
|
||||
#[cfg(not(target_arch = "aarch64"))]
|
||||
pub mod bus;
|
||||
|
||||
pub mod nvme;
|
||||
|
||||
pub mod display;
|
||||
pub mod power;
|
||||
pub mod serial;
|
||||
|
||||
36
src/device/nvme/command.rs
Normal file
36
src/device/nvme/command.rs
Normal file
@ -0,0 +1,36 @@
|
||||
use super::queue::SubmissionQueueEntry;
|
||||
|
||||
pub trait Command {
|
||||
fn fill_sqe(&self, sqe: &mut SubmissionQueueEntry);
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
#[repr(u32)]
|
||||
pub enum IdentifyCommand {
|
||||
Controller = 0x01,
|
||||
}
|
||||
|
||||
#[repr(C)]
|
||||
pub struct IdentifyControllerResponse {
|
||||
pub pci_vid: u16,
|
||||
pub pci_ssvid: u16,
|
||||
pub serial_number: [u8; 20],
|
||||
pub model_number: [u8; 40],
|
||||
pub firmware_rev: u64,
|
||||
}
|
||||
|
||||
pub enum AdminCommand {
|
||||
Identify { nsid: u32, cns: IdentifyCommand },
|
||||
}
|
||||
|
||||
impl Command for AdminCommand {
|
||||
fn fill_sqe(&self, sqe: &mut SubmissionQueueEntry) {
|
||||
match self {
|
||||
AdminCommand::Identify { nsid, cns } => {
|
||||
sqe.command.set_opcode(0x06);
|
||||
sqe.nsid = *nsid;
|
||||
sqe.command_specific[0] = *cns as u32;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
261
src/device/nvme/mod.rs
Normal file
261
src/device/nvme/mod.rs
Normal file
@ -0,0 +1,261 @@
|
||||
#![allow(missing_docs)]
|
||||
use core::{mem::size_of, ptr::null_mut, time::Duration};
|
||||
|
||||
use abi::error::Error;
|
||||
use bytemuck::{Pod, Zeroable};
|
||||
use device_api::Device;
|
||||
use kernel_util::{sync::IrqSafeSpinlock, util::OneTimeInit};
|
||||
use static_assertions::{const_assert, const_assert_eq};
|
||||
use tock_registers::{
|
||||
interfaces::{ReadWriteable, Readable, Writeable},
|
||||
register_bitfields, register_structs,
|
||||
registers::{ReadOnly, ReadWrite, WriteOnly},
|
||||
};
|
||||
|
||||
use crate::{
|
||||
arch::{x86_64::mem::table::L3, Architecture, ARCHITECTURE},
|
||||
debug,
|
||||
device::{
|
||||
bus::pci::{PciBaseAddress, PciCommandRegister, PciConfigurationSpace},
|
||||
nvme::{
|
||||
command::{AdminCommand, IdentifyCommand},
|
||||
queue::{CompletionQueueEntry, SubmissionQueueEntry},
|
||||
},
|
||||
},
|
||||
mem::{
|
||||
address::{AsPhysicalAddress, FromRaw, IntoRaw},
|
||||
device::DeviceMemoryIo,
|
||||
phys,
|
||||
pointer::{PhysicalRef, PhysicalRefMut},
|
||||
table::EntryLevel,
|
||||
PhysicalAddress,
|
||||
},
|
||||
task::runtime,
|
||||
};
|
||||
|
||||
use self::queue::QueuePair;
|
||||
|
||||
use super::bus::pci::{FromPciBus, PciDeviceInfo};
|
||||
|
||||
mod command;
|
||||
mod queue;
|
||||
|
||||
register_bitfields! {
|
||||
u32,
|
||||
CC [
|
||||
IOCQES OFFSET(20) NUMBITS(4) [],
|
||||
IOSQES OFFSET(16) NUMBITS(4) [],
|
||||
AMS OFFSET(11) NUMBITS(3) [],
|
||||
MPS OFFSET(7) NUMBITS(4) [],
|
||||
CSS OFFSET(4) NUMBITS(3) [
|
||||
NvmCommandSet = 0
|
||||
],
|
||||
ENABLE OFFSET(0) NUMBITS(1) [],
|
||||
],
|
||||
CSTS [
|
||||
CFS OFFSET(1) NUMBITS(1) [],
|
||||
RDY OFFSET(0) NUMBITS(1) [],
|
||||
],
|
||||
AQA [
|
||||
/// Admin Completion Queue Size in entries - 1
|
||||
ACQS OFFSET(16) NUMBITS(12) [],
|
||||
/// Admin Submission Queue Size in entries - 1
|
||||
ASQS OFFSET(0) NUMBITS(12) [],
|
||||
]
|
||||
}
|
||||
|
||||
register_bitfields! {
|
||||
u64,
|
||||
CAP [
|
||||
/// Maximum Queue Entries Supported - 1. i.e., 0 means maximum queue len of 1, 1 = 2 etc.
|
||||
MQES OFFSET(0) NUMBITS(16) [],
|
||||
/// Timeout. Represents the worst-case time the host software should wait for CSTS.RDY to
|
||||
/// change its state.
|
||||
TO OFFSET(24) NUMBITS(8) [],
|
||||
/// Doorbell stride. Stride in bytes = pow(2, 2 + DSTRD).
|
||||
DSTRD OFFSET(32) NUMBITS(4) [],
|
||||
/// NVM Subsystem Reset Supported (see NVMe BS Section 3.7.1)
|
||||
NSSRS OFFSET(36) NUMBITS(1) [],
|
||||
/// Controller supports one or more I/O command sets
|
||||
CSS_IO_COMMANDS OFFSET(43) NUMBITS(1) [],
|
||||
/// Controller only supports admin commands and no I/O commands
|
||||
CSS_ADMIN_ONLY OFFSET(44) NUMBITS(1) [],
|
||||
/// Memory page size minimum (bytes = pow(2, 12 + MPSMIN))
|
||||
MPSMIN OFFSET(48) NUMBITS(4) [],
|
||||
/// Memory page size maximum -|-
|
||||
MPSMAX OFFSET(52) NUMBITS(4) [],
|
||||
]
|
||||
}
|
||||
|
||||
register_structs! {
|
||||
#[allow(non_snake_case)]
|
||||
Regs {
|
||||
(0x00 => CAP: ReadOnly<u64, CAP::Register>),
|
||||
(0x08 => VS: ReadOnly<u32>),
|
||||
(0x0C => INTMS: WriteOnly<u32>),
|
||||
(0x10 => INTMC: WriteOnly<u32>),
|
||||
(0x14 => CC: ReadWrite<u32, CC::Register>),
|
||||
(0x18 => _0),
|
||||
(0x1C => CSTS: ReadOnly<u32, CSTS::Register>),
|
||||
(0x20 => _1),
|
||||
(0x24 => AQA: ReadWrite<u32, AQA::Register>),
|
||||
(0x28 => ASQ: ReadWrite<u64>),
|
||||
(0x30 => ACQ: ReadWrite<u64>),
|
||||
(0x38 => _2),
|
||||
(0x2000 => @END),
|
||||
}
|
||||
}
|
||||
|
||||
pub struct NvmeController {
|
||||
regs: IrqSafeSpinlock<DeviceMemoryIo<'static, Regs>>,
|
||||
admin_q: OneTimeInit<IrqSafeSpinlock<QueuePair<'static>>>,
|
||||
}
|
||||
|
||||
impl Regs {
|
||||
unsafe fn doorbell_ptr(&self, shift: u64, completion: bool, queue_index: usize) -> *mut u32 {
|
||||
let doorbell_base = (self as *const Regs as *mut Regs).addr() + 0x1000;
|
||||
let offset = (queue_index << shift) + completion as usize * 4;
|
||||
(doorbell_base + offset) as *mut u32
|
||||
}
|
||||
}
|
||||
|
||||
impl NvmeController {
|
||||
async fn late_init(&'static self) {
|
||||
let mut admin_q = self.admin_q.get().lock();
|
||||
|
||||
// Identify
|
||||
let page = phys::alloc_page().unwrap();
|
||||
let mut ranges = [page];
|
||||
|
||||
debugln!("output range = {:#x}", ranges[0]);
|
||||
admin_q
|
||||
.perform_request(
|
||||
AdminCommand::Identify {
|
||||
nsid: 0,
|
||||
cns: IdentifyCommand::Controller,
|
||||
},
|
||||
&ranges,
|
||||
)
|
||||
.await;
|
||||
|
||||
let pref = unsafe { PhysicalRef::<command::IdentifyControllerResponse>::map(page) };
|
||||
|
||||
fn cstr(s: &[u8]) -> &str {
|
||||
let i = s
|
||||
.iter()
|
||||
.position(|x| *x == 0 || *x == b' ')
|
||||
.unwrap_or_else(|| s.len());
|
||||
core::str::from_utf8(&s[..i]).unwrap()
|
||||
}
|
||||
|
||||
let sn = cstr(&pref.serial_number);
|
||||
let mn = cstr(&pref.model_number);
|
||||
|
||||
debugln!("Serial Number: {:?}", sn);
|
||||
debugln!("Model Number: {:?}", mn);
|
||||
|
||||
loop {}
|
||||
}
|
||||
}
|
||||
|
||||
impl Device for NvmeController {
|
||||
unsafe fn init(&'static self) -> Result<(), Error> {
|
||||
let regs = self.regs.lock();
|
||||
|
||||
let min_page_size = 1usize << (12 + regs.CAP.read(CAP::MPSMIN));
|
||||
let max_page_size = 1usize << (12 + regs.CAP.read(CAP::MPSMAX));
|
||||
|
||||
if min_page_size > 4096 {
|
||||
panic!();
|
||||
}
|
||||
|
||||
if regs.CAP.read(CAP::CSS_IO_COMMANDS) != 1 {
|
||||
panic!();
|
||||
}
|
||||
|
||||
let timeout = Duration::from_millis(regs.CAP.read(CAP::TO) * 500);
|
||||
debugln!("Worst-case timeout: {:?}", timeout);
|
||||
|
||||
while regs.CSTS.matches_any(CSTS::RDY::SET) {
|
||||
core::hint::spin_loop();
|
||||
}
|
||||
|
||||
let queue_slots = 32;
|
||||
if queue_slots > regs.CAP.read(CAP::MQES) + 1 {
|
||||
todo!(
|
||||
"queue_slots too big, max = {}",
|
||||
regs.CAP.read(CAP::MQES) + 1
|
||||
);
|
||||
}
|
||||
|
||||
// Setup the admin queue (index 0)
|
||||
let doorbell_shift = regs.CAP.read(CAP::DSTRD) + 2;
|
||||
let admin_sq_doorbell = unsafe { regs.doorbell_ptr(doorbell_shift, false, 0) };
|
||||
let admin_cq_doorbell = unsafe { regs.doorbell_ptr(doorbell_shift, true, 0) };
|
||||
let mut admin_q =
|
||||
QueuePair::new(queue_slots as usize, admin_sq_doorbell, admin_cq_doorbell).unwrap();
|
||||
|
||||
regs.AQA
|
||||
.modify(AQA::ASQS.val(queue_slots as u32 - 1) + AQA::ACQS.val(queue_slots as u32 - 1));
|
||||
regs.ASQ.set(admin_q.sq_physical_pointer().into_raw());
|
||||
regs.ACQ.set(admin_q.cq_physical_pointer().into_raw());
|
||||
|
||||
// Configure the controller
|
||||
const IOSQES: u32 = size_of::<SubmissionQueueEntry>().ilog2();
|
||||
const IOCQES: u32 = size_of::<CompletionQueueEntry>().ilog2();
|
||||
|
||||
regs.CC.modify(
|
||||
CC::IOCQES.val(IOCQES)
|
||||
+ CC::IOSQES.val(IOSQES)
|
||||
+ CC::MPS.val(0)
|
||||
+ CC::CSS::NvmCommandSet,
|
||||
);
|
||||
|
||||
// Enable the controller
|
||||
regs.CC.modify(CC::ENABLE::SET);
|
||||
|
||||
debugln!("Reset the controller");
|
||||
|
||||
while !regs.CSTS.matches_any(CSTS::RDY::SET + CSTS::CFS::SET) {
|
||||
core::hint::spin_loop();
|
||||
}
|
||||
|
||||
if regs.CSTS.matches_any(CSTS::CFS::SET) {
|
||||
todo!("CFS set after reset!");
|
||||
}
|
||||
|
||||
self.admin_q.init(IrqSafeSpinlock::new(admin_q));
|
||||
|
||||
// Schedule late_init task
|
||||
runtime::spawn(self.late_init())?;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn display_name(&self) -> &'static str {
|
||||
"NVM Express Controller"
|
||||
}
|
||||
}
|
||||
|
||||
impl FromPciBus for NvmeController {
|
||||
fn from_pci_bus(info: &PciDeviceInfo) -> Result<Self, Error> {
|
||||
let PciBaseAddress::Memory(bar0) = info.config_space.bar(0).unwrap() else {
|
||||
panic!();
|
||||
};
|
||||
|
||||
let mut cmd = PciCommandRegister::from_bits_retain(info.config_space.command());
|
||||
cmd &= !(PciCommandRegister::DISABLE_INTERRUPTS | PciCommandRegister::ENABLE_IO);
|
||||
cmd |= PciCommandRegister::ENABLE_MEMORY | PciCommandRegister::BUS_MASTER;
|
||||
info.config_space.set_command(cmd.bits());
|
||||
|
||||
let regs = unsafe { DeviceMemoryIo::<Regs>::map(PhysicalAddress::from_raw(bar0)) }?;
|
||||
|
||||
// Disable the controller
|
||||
regs.CC.modify(CC::ENABLE::CLEAR);
|
||||
|
||||
Ok(Self {
|
||||
regs: IrqSafeSpinlock::new(regs),
|
||||
admin_q: OneTimeInit::new(),
|
||||
})
|
||||
}
|
||||
}
|
||||
334
src/device/nvme/queue.rs
Normal file
334
src/device/nvme/queue.rs
Normal file
@ -0,0 +1,334 @@
|
||||
use core::{
|
||||
mem::size_of,
|
||||
pin::Pin,
|
||||
ptr::null_mut,
|
||||
task::{Context, Poll},
|
||||
};
|
||||
|
||||
use abi::error::Error;
|
||||
use bytemuck::{Pod, Zeroable};
|
||||
use futures_util::Future;
|
||||
use static_assertions::const_assert;
|
||||
|
||||
use crate::{
|
||||
arch::x86_64::mem::table::L3,
|
||||
mem::{
|
||||
address::{AsPhysicalAddress, IntoRaw},
|
||||
phys,
|
||||
pointer::PhysicalRefMut,
|
||||
table::EntryLevel,
|
||||
PhysicalAddress,
|
||||
},
|
||||
proc,
|
||||
};
|
||||
|
||||
use super::command::Command;
|
||||
|
||||
#[derive(Zeroable, Pod, Clone, Copy, Debug)]
|
||||
#[repr(C)]
|
||||
pub struct PhysicalRegionPage(u64);
|
||||
|
||||
// Bits:
|
||||
//
|
||||
// 16..32 - CID. Command identifier
|
||||
// 14..16 - PSDT. PRP or SGL for data transfer.
|
||||
// 0b00 - PRP used
|
||||
// 0b01 - SGL used. Not implemented
|
||||
// 0b10 - SGL used. Not implemented
|
||||
// 0b11 - Reserved
|
||||
// 10..14 - Reserved
|
||||
// 08..10 - FUSE. Fused Operation
|
||||
// 00..08 - OPC. Opcode
|
||||
#[derive(Zeroable, Pod, Clone, Copy, Debug)]
|
||||
#[repr(C)]
|
||||
pub struct CommandDword0(u32);
|
||||
|
||||
#[derive(Zeroable, Pod, Clone, Copy, Debug)]
|
||||
#[repr(C)]
|
||||
pub struct SubmissionQueueEntry {
|
||||
pub command: CommandDword0, // 0
|
||||
pub nsid: u32, // 1
|
||||
_0: [u32; 2], // 2, 3
|
||||
pub metadata_pointer: u64, // 4, 5
|
||||
pub data_pointer: [PhysicalRegionPage; 2], // 6, 7, 8, 9
|
||||
pub command_specific: [u32; 6], // 10, 11, 12, 13, 14, 15
|
||||
}
|
||||
|
||||
#[repr(C)]
|
||||
pub struct CompletionQueueEntry {
|
||||
dw: [u32; 4],
|
||||
}
|
||||
|
||||
pub struct Queue<'a, T> {
|
||||
data: PhysicalRefMut<'a, [T]>,
|
||||
|
||||
mask: usize,
|
||||
head: usize,
|
||||
tail: usize,
|
||||
phase: bool,
|
||||
|
||||
head_doorbell: *mut u32,
|
||||
tail_doorbell: *mut u32,
|
||||
}
|
||||
|
||||
unsafe impl<'a, T> Sync for Queue<'a, T> {}
|
||||
unsafe impl<'a, T> Send for Queue<'a, T> {}
|
||||
|
||||
// TODO PageBox<T>?
|
||||
pub struct QueuePair<'a> {
|
||||
base: PhysicalAddress,
|
||||
page_count: usize,
|
||||
|
||||
sq: Queue<'a, SubmissionQueueEntry>,
|
||||
cq: Queue<'a, CompletionQueueEntry>,
|
||||
}
|
||||
|
||||
const_assert!(size_of::<CompletionQueueEntry>().is_power_of_two());
|
||||
|
||||
impl PhysicalRegionPage {
|
||||
pub const fn null() -> Self {
|
||||
Self(0)
|
||||
}
|
||||
|
||||
pub const fn with_addr(addr: u64) -> Self {
|
||||
Self(addr)
|
||||
}
|
||||
}
|
||||
|
||||
impl CommandDword0 {
|
||||
pub fn set_command_id(&mut self, id: u16) {
|
||||
self.0 &= !(0xFFFF << 16);
|
||||
self.0 |= (id as u32) << 16;
|
||||
}
|
||||
|
||||
pub fn set_opcode(&mut self, opcode: u8) {
|
||||
self.0 &= !0xFF;
|
||||
self.0 |= opcode as u32;
|
||||
}
|
||||
}
|
||||
|
||||
impl CompletionQueueEntry {
|
||||
pub fn phase(&self) -> bool {
|
||||
self.dw[3] & (1 << 16) != 0
|
||||
}
|
||||
|
||||
pub fn subqueue_id(&self) -> u32 {
|
||||
self.dw[2] >> 16
|
||||
}
|
||||
|
||||
pub fn subqueue_head(&self) -> usize {
|
||||
(self.dw[2] & 0xFFFF) as _
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, T> Queue<'a, T> {
|
||||
pub unsafe fn from_raw_parts(
|
||||
base: PhysicalAddress,
|
||||
len: usize,
|
||||
head_doorbell: *mut u32,
|
||||
tail_doorbell: *mut u32,
|
||||
phase: bool,
|
||||
) -> Self {
|
||||
// Submission queues have tail doorbells, completion queues have head doorbells
|
||||
assert!(
|
||||
(head_doorbell.is_null() && !tail_doorbell.is_null())
|
||||
|| (!head_doorbell.is_null() && tail_doorbell.is_null())
|
||||
);
|
||||
|
||||
Self {
|
||||
data: PhysicalRefMut::map_slice(base, len),
|
||||
mask: len - 1,
|
||||
head: 0,
|
||||
tail: 0,
|
||||
head_doorbell,
|
||||
tail_doorbell,
|
||||
phase,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn physical_pointer(&self) -> PhysicalAddress {
|
||||
unsafe { self.data.as_physical_address() }
|
||||
}
|
||||
|
||||
pub fn enqueue(&mut self, item: T) -> usize {
|
||||
let index = self.tail;
|
||||
self.data[self.tail] = item;
|
||||
self.phase ^= self.set_tail(self.next_index(self.tail));
|
||||
index
|
||||
}
|
||||
|
||||
pub fn at_head(&self, offset: usize) -> (&T, bool) {
|
||||
let index = (self.head + offset) & self.mask;
|
||||
let expected_phase = self.phase ^ (index < self.head);
|
||||
(&self.data[index], expected_phase)
|
||||
}
|
||||
|
||||
pub fn take(&mut self, count: usize) {
|
||||
let index = (self.head + count) & self.mask;
|
||||
self.phase ^= self.set_head(index);
|
||||
}
|
||||
|
||||
pub fn take_until(&mut self, new_head: usize) {
|
||||
self.phase ^= self.set_head(new_head);
|
||||
}
|
||||
|
||||
fn next_index(&self, index: usize) -> usize {
|
||||
(index + 1) & self.mask
|
||||
}
|
||||
|
||||
fn set_tail(&mut self, new_tail: usize) -> bool {
|
||||
let wrapped = new_tail < self.tail;
|
||||
|
||||
self.tail = new_tail;
|
||||
|
||||
if !self.tail_doorbell.is_null() {
|
||||
unsafe {
|
||||
self.tail_doorbell
|
||||
.write_volatile(self.tail.try_into().unwrap());
|
||||
}
|
||||
}
|
||||
|
||||
wrapped
|
||||
}
|
||||
|
||||
fn set_head(&mut self, new_head: usize) -> bool {
|
||||
let wrapped = new_head < self.head;
|
||||
|
||||
self.head = new_head;
|
||||
|
||||
if !self.head_doorbell.is_null() {
|
||||
unsafe {
|
||||
self.head_doorbell
|
||||
.write_volatile(self.head.try_into().unwrap());
|
||||
}
|
||||
}
|
||||
|
||||
wrapped
|
||||
}
|
||||
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.head == self.tail
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a> QueuePair<'a> {
|
||||
pub fn new(
|
||||
capacity: usize,
|
||||
sq_doorbell: *mut u32,
|
||||
cq_doorbell: *mut u32,
|
||||
) -> Result<Self, Error> {
|
||||
let sq_size = capacity * size_of::<SubmissionQueueEntry>();
|
||||
let cq_size = capacity * size_of::<CompletionQueueEntry>();
|
||||
|
||||
let page_count = L3::page_count(sq_size) + L3::page_count(cq_size);
|
||||
let base = phys::alloc_pages_contiguous(page_count)?;
|
||||
|
||||
let sq_base = base;
|
||||
let cq_base = base.add(L3::align_up(sq_size));
|
||||
|
||||
debugln!(
|
||||
"Allocated queue pair: sq={:x?}, cq={:x?} ({} pages)",
|
||||
sq_base..sq_base.add(sq_size),
|
||||
cq_base..cq_base.add(cq_size),
|
||||
page_count
|
||||
);
|
||||
|
||||
let sq = unsafe { Queue::from_raw_parts(sq_base, capacity, null_mut(), sq_doorbell, true) };
|
||||
let cq = unsafe { Queue::from_raw_parts(cq_base, capacity, cq_doorbell, null_mut(), true) };
|
||||
|
||||
Ok(Self {
|
||||
base,
|
||||
page_count,
|
||||
sq,
|
||||
cq,
|
||||
})
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn sq_physical_pointer(&self) -> PhysicalAddress {
|
||||
self.sq.physical_pointer()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn cq_physical_pointer(&self) -> PhysicalAddress {
|
||||
self.cq.physical_pointer()
|
||||
}
|
||||
|
||||
pub fn submit<C: Command>(
|
||||
&mut self,
|
||||
command: C,
|
||||
ranges: &[PhysicalAddress],
|
||||
) -> Result<(), Error> {
|
||||
let index = self.sq.tail;
|
||||
let mut sqe = SubmissionQueueEntry::zeroed();
|
||||
|
||||
sqe.command.set_command_id(index.try_into().unwrap());
|
||||
if ranges.len() != 1 {
|
||||
todo!();
|
||||
}
|
||||
sqe.data_pointer[0] = PhysicalRegionPage::with_addr(ranges[0].into_raw());
|
||||
sqe.data_pointer[1] = PhysicalRegionPage::null();
|
||||
|
||||
command.fill_sqe(&mut sqe);
|
||||
|
||||
self.sq.enqueue(sqe);
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn advance_head(&mut self, new_head: usize) {
|
||||
self.sq.take_until(new_head);
|
||||
}
|
||||
|
||||
pub fn process_completions(&mut self) -> usize {
|
||||
let mut i = 0;
|
||||
loop {
|
||||
let (packet, expected_phase) = self.cq.at_head(i);
|
||||
let packet_phase = packet.phase();
|
||||
|
||||
if packet_phase != expected_phase {
|
||||
break;
|
||||
}
|
||||
|
||||
i += 1;
|
||||
|
||||
let sqid = packet.subqueue_id();
|
||||
// TODO
|
||||
assert_eq!(sqid, 0);
|
||||
|
||||
let sqhd = packet.subqueue_head();
|
||||
self.advance_head(sqhd);
|
||||
}
|
||||
|
||||
if i != 0 {
|
||||
self.cq.take(i);
|
||||
}
|
||||
|
||||
i
|
||||
}
|
||||
|
||||
pub async fn perform_request<C: Command>(&mut self, command: C, ranges: &[PhysicalAddress]) {
|
||||
self.submit(command, ranges).unwrap();
|
||||
|
||||
while self.process_completions() == 0 {
|
||||
core::hint::spin_loop();
|
||||
}
|
||||
}
|
||||
|
||||
pub fn wait_all_complete(&'a self, index: usize) -> impl Future<Output = ()> + 'a {
|
||||
struct F<'q> {
|
||||
queue: &'q QueuePair<'q>,
|
||||
index: usize,
|
||||
}
|
||||
|
||||
impl<'q> Future for F<'q> {
|
||||
type Output = ();
|
||||
|
||||
fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
|
||||
todo!()
|
||||
}
|
||||
}
|
||||
|
||||
F { queue: self, index }
|
||||
}
|
||||
}
|
||||
@ -25,6 +25,7 @@ fn setup_root() -> Result<NodeRef, Error> {
|
||||
/// initialization has finished.
|
||||
pub fn kinit() -> Result<(), Error> {
|
||||
infoln!("In main");
|
||||
loop {}
|
||||
|
||||
#[cfg(feature = "fb_console")]
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user