Merge pull request #3 from dorindabassey/vsound_test

Sound: Handle control Queue

Cargo.lock

@@ -1111,6 +1111,7 @@ dependencies = [
  "vhost",
  "vhost-user-backend",
  "virtio-bindings 0.2.0",
+ "virtio-queue",
  "vm-memory",
  "vmm-sys-util",
 ]

crates/sound/Cargo.toml

@@ -22,6 +22,7 @@ thiserror = "1.0"
 vhost = { version = "0.6", features = ["vhost-user-slave"] }
 vhost-user-backend = "0.8"
 virtio-bindings = "0.2"
+virtio-queue = "0.7"
 vm-memory = "0.10"
 vmm-sys-util = "0.11"
 pipewire = { version = "0.6.0", optional = true }

crates/sound/src/lib.rs

@@ -30,6 +30,26 @@ pub enum Error {
     SoundReqMissingData,
     #[error("Audio backend not supported")]
     AudioBackendNotSupported,
+    #[error("Descriptor not found")]
+    DescriptorNotFound,
+    #[error("Descriptor read failed")]
+    DescriptorReadFailed,
+    #[error("Descriptor write failed")]
+    DescriptorWriteFailed,
+    #[error("Isufficient descriptor size, required: {0}, found: {1}")]
+    InsufficientDescriptorSize(usize, usize),
+    #[error("Failed to send notification")]
+    SendNotificationFailed,
+    #[error("Invalid descriptor count {0}")]
+    UnexpectedDescriptorCount(usize),
+    #[error("Invalid descriptor size, expected: {0}, found: {1}")]
+    UnexpectedDescriptorSize(usize, usize),
+    #[error("Invalid descriptor size, expected at least: {0}, found: {1}")]
+    UnexpectedMinimumDescriptorSize(usize, usize),
+    #[error("Received unexpected readable descriptor at index {0}")]
+    UnexpectedReadableDescriptor(usize),
+    #[error("Received unexpected write only descriptor at index {0}")]
+    UnexpectedWriteOnlyDescriptor(usize),
 }
 
 impl std::convert::From<Error> for IoError {

crates/sound/src/vhu_sound.rs

@@ -1,22 +1,39 @@
 // SPDX-License-Identifier: Apache-2.0 or BSD-3-Clause
 
+use std::mem::size_of;
+use std::sync::RwLock;
+use std::{io::Result as IoResult, u16, u32, u64, u8};
+
+use log::{error, debug};
+use vhost::vhost_user::message::{VhostUserProtocolFeatures, VhostUserVirtioFeatures};
+use vhost_user_backend::{VhostUserBackend, VringRwLock, VringT};
+use virtio_bindings::bindings::{
+    virtio_config::VIRTIO_F_NOTIFY_ON_EMPTY, virtio_config::VIRTIO_F_VERSION_1,
+    virtio_ring::VIRTIO_RING_F_EVENT_IDX, virtio_ring::VIRTIO_RING_F_INDIRECT_DESC,
+};
+use virtio_queue::{DescriptorChain, QueueOwnedT};
+use vm_memory::{Bytes, ByteValued, GuestAddressSpace, GuestMemoryAtomic, GuestMemoryMmap, GuestMemoryLoadGuard};
+use vmm_sys_util::{
+    epoll::EventSet,
+    eventfd::{EventFd, EFD_NONBLOCK},
+};
+
 use crate::audio_backends::{alloc_audio_backend, AudioBackend};
 use crate::virtio_sound::*;
 use crate::{Error, Result, SoundConfig};
 
-use std::sync::RwLock;
-use std::{io::Result as IoResult, u16, u32, u64, u8};
-use vhost::vhost_user::message::{VhostUserProtocolFeatures, VhostUserVirtioFeatures};
-use vhost_user_backend::{VhostUserBackend, VringRwLock};
-use virtio_bindings::bindings::{
-    virtio_config::VIRTIO_F_NOTIFY_ON_EMPTY, virtio_config::VIRTIO_F_VERSION_1,
-    virtio_ring::VIRTIO_RING_F_EVENT_IDX, virtio_ring::VIRTIO_RING_F_INDIRECT_DESC,
-};
-use vm_memory::{ByteValued, GuestMemoryAtomic, GuestMemoryMmap};
-use vmm_sys_util::{
-    epoll::EventSet,
-    eventfd::{EventFd, EFD_NONBLOCK},
-};
+pub const SUPPORTED_FORMATS: u64 = 1 << VIRTIO_SND_PCM_FMT_U8
+    | 1 << VIRTIO_SND_PCM_FMT_S16
+    | 1 << VIRTIO_SND_PCM_FMT_S24
+    | 1 << VIRTIO_SND_PCM_FMT_S32;
+
+pub const SUPPORTED_RATES: u64 = 1 << VIRTIO_SND_PCM_RATE_8000
+    | 1 << VIRTIO_SND_PCM_RATE_11025
+    | 1 << VIRTIO_SND_PCM_RATE_16000
+    | 1 << VIRTIO_SND_PCM_RATE_22050
+    | 1 << VIRTIO_SND_PCM_RATE_32000
+    | 1 << VIRTIO_SND_PCM_RATE_44100
+    | 1 << VIRTIO_SND_PCM_RATE_48000;
 
 struct VhostUserSoundThread {
     mem: Option<GuestMemoryAtomic<GuestMemoryMmap>>,
@@ -50,6 +67,7 @@ impl VhostUserSoundThread {
     }
 
     fn update_memory(&mut self, mem: GuestMemoryAtomic<GuestMemoryMmap>) -> IoResult<()> {
+        debug!("update memory");
         self.mem = Some(mem);
         Ok(())
     }
@@ -57,17 +75,223 @@ impl VhostUserSoundThread {
     fn handle_event(&self, device_event: u16, vrings: &[VringRwLock]) -> IoResult<bool> {
         let vring = &vrings[device_event as usize];
         let queue_idx = self.queue_indexes[device_event as usize];
+        debug!("handle event call queue: {}", queue_idx);
 
         match queue_idx {
-            CONTROL_QUEUE_IDX => self.process_control(vring),
-            EVENT_QUEUE_IDX => self.process_event(vring),
-            TX_QUEUE_IDX => self.process_tx(vring),
-            RX_QUEUE_IDX => self.process_rx(vring),
-            _ => Err(Error::HandleUnknownEvent.into()),
+            CONTROL_QUEUE_IDX => {
+                debug!("control queue: {}", CONTROL_QUEUE_IDX);
+                let vring = &vrings[0];
+                if self.event_idx {
+                    // vm-virtio's Queue implementation only checks avail_index
+                    // once, so to properly support EVENT_IDX we need to keep
+                    // calling process_request_queue() until it stops finding
+                    // new requests on the queue.
+                    loop {
+                        vring.disable_notification().unwrap();
+                        self.process_control(vring)?;
+                        if !vring.enable_notification().unwrap() {
+                            break;
+                        }
+                    }
+                } else {
+                    // Without EVENT_IDX, a single call is enough.
+                    self.process_control(vring)?;
+                }
+            }
+            EVENT_QUEUE_IDX => {
+                self.process_event(vring)?;
+            }
+            TX_QUEUE_IDX => {
+                self.process_tx(vring)?;
+            }
+            RX_QUEUE_IDX => {
+                self.process_rx(vring)?;
+            }
+            _ => {
+                return Err(Error::HandleUnknownEvent.into());
+            }
         }
+        Ok(false)
     }
 
-    fn process_control(&self, _vring: &VringRwLock) -> IoResult<bool> {
+    /// Process the messages in the vring and dispatch replies
+    fn process_control(&self, vring: &VringRwLock) -> Result<bool> {
+        let requests: Vec<SndDescriptorChain> = vring
+            .get_mut()
+            .get_queue_mut()
+            .iter(self.mem.as_ref().unwrap().memory())
+            .map_err(|_| Error::DescriptorNotFound)?
+            .collect();
+
+        debug!("Requests to process: {}", requests.len());
+        if requests.is_empty() {
+            debug!("yes, it's empty");
+            return Ok(true);
+        }
+        //iterate over each sound request
+        for desc_chain in requests {
+            let descriptors: Vec<_> = desc_chain.clone().collect();
+            debug!("Sound request with n descriptors: {}", descriptors.len());
+
+            let desc_request = descriptors[0];
+            if desc_request.is_write_only() {
+                return Err(Error::UnexpectedWriteOnlyDescriptor(0));
+            }
+            let read_desc_len: usize = desc_request.len() as usize;
+            let header_size = size_of::<VirtioSoundHeader>();
+            if (read_desc_len as usize) < header_size {
+                return Err(Error::UnexpectedMinimumDescriptorSize(
+                    header_size,
+                    read_desc_len,
+                ));
+            }
+            let hdr_request = desc_chain
+                .memory()
+                .read_obj::<VirtioSoundHeader>(desc_request.addr())
+                .map_err(|_| Error::DescriptorReadFailed)?;
+
+            let desc_response = descriptors[1];
+            if !desc_response.is_write_only() {
+                return Err(Error::UnexpectedReadableDescriptor(1));
+            }
+
+            let response = VirtioSoundHeader { code: VIRTIO_SND_S_OK.into(), };
+
+            let mut len = desc_response.len() as u32;
+            let request_type = hdr_request.code.to_native();
+            match request_type {
+                VIRTIO_SND_R_JACK_INFO => todo!(),
+                VIRTIO_SND_R_PCM_INFO => {
+                    if descriptors.len() != 3 {
+                        return Err(Error::UnexpectedDescriptorCount(descriptors.len()));
+                    }
+                    let desc_pcm = descriptors[2];
+                    if !desc_pcm.is_write_only() {
+                        return Err(Error::UnexpectedReadableDescriptor(2));
+                    }
+                    let query_info = desc_chain
+                        .memory()
+                        .read_obj::<VirtioSoundQueryInfo>(desc_request.addr())
+                        .map_err(|_| Error::DescriptorReadFailed)?;
+
+                    let start_id: usize = u32::from(query_info.start_id) as usize;
+                    let count: usize = u32::from(query_info.count) as usize;
+                    let mut pcm_info = vec![VirtioSoundPcmInfo::default(); count];
+                    for pcm in &mut pcm_info {
+                        pcm.hdr.hda_fn_nid = 0.into();
+                        pcm.features = 0.into();
+                        pcm.formats = SUPPORTED_FORMATS.into();
+                        pcm.rates = SUPPORTED_RATES.into();
+                        pcm.direction = VIRTIO_SND_D_OUTPUT;
+                        pcm.channels_min = 1;
+                        pcm.channels_max = 6;
+                        pcm.padding = [0; 5];
+                    }
+                    if start_id + count > pcm_info.len() {
+                        error!(
+                            "start_id({}) + count({}) must be smaller than the number of streams ({})",
+                            start_id,
+                            count,
+                            pcm_info.len()
+                        );
+                        desc_chain
+                            .memory()
+                            .write_obj(VIRTIO_SND_S_BAD_MSG, desc_response.addr())
+                            .map_err(|_| Error::DescriptorWriteFailed)?;
+                    }
+                    desc_chain
+                        .memory()
+                        .write_obj(response, desc_response.addr())
+                        .map_err(|_| Error::DescriptorWriteFailed)?;
+                    //let mut len = desc_response.len() as u32;
+
+                    for i in start_id..(start_id + count) {
+                        desc_chain
+                            .memory()
+                            .write_slice(pcm_info[i].as_slice(), desc_pcm.addr())
+                            .map_err(|_| Error::DescriptorWriteFailed)?;
+                    }
+                    len += desc_pcm.len();
+                },
+                VIRTIO_SND_R_CHMAP_INFO => todo!(),
+                VIRTIO_SND_R_JACK_REMAP => todo!(),
+                VIRTIO_SND_R_PCM_SET_PARAMS => {
+                    if descriptors.len() != 2 {
+                        return Err(Error::UnexpectedDescriptorCount(descriptors.len()));
+                    }
+
+                    let set_params = desc_chain
+                        .memory()
+                        .read_obj::<VirtioSndPcmSetParams>(desc_request.addr())
+                        .map_err(|_| Error::DescriptorReadFailed)?;
+                    let stream_id: usize = u32::from(set_params.hdr.stream_id) as usize;
+                    let buffer_bytes: u32 = set_params.buffer_bytes.into();
+                    let period_bytes: u32 = set_params.period_bytes.into();
+                    let features: u32 = set_params.features.into();
+
+                    dbg!("stream_id: {}", stream_id );
+                    dbg!("set params format: {}", set_params.format);
+                    dbg!("set params rate: {} ", set_params.rate);
+                    dbg!("set params channels: {} ", set_params.channels);
+                    if features != 0 {
+                        error!("No feature is supported");
+                        desc_chain
+                            .memory()
+                            .write_obj(VIRTIO_SND_S_NOT_SUPP, desc_response.addr())
+                            .map_err(|_| Error::DescriptorWriteFailed)?;
+                    }
+                    if buffer_bytes % period_bytes != 0 {
+                        error!("buffer_bytes({}) must be dividable by period_bytes({})",
+                            buffer_bytes, period_bytes);
+                            desc_chain
+                                .memory()
+                                .write_obj(VIRTIO_SND_S_BAD_MSG, desc_response.addr())
+                                .map_err(|_| Error::DescriptorWriteFailed)?;
+                    }
+                    desc_chain
+                        .memory()
+                        .write_obj(response, desc_response.addr())
+                        .map_err(|_| Error::DescriptorWriteFailed)?;
+                    len = desc_response.len() as u32;
+                },
+                VIRTIO_SND_R_PCM_PREPARE
+                | VIRTIO_SND_R_PCM_START
+                | VIRTIO_SND_R_PCM_STOP
+                | VIRTIO_SND_R_PCM_RELEASE => {
+                    let pcm_hdr = desc_chain
+                        .memory()
+                        .read_obj::<VirtioSoundPcmHeader>(desc_request.addr())
+                        .map_err(|_| Error::DescriptorReadFailed)?;
+                    let stream_id: usize = u32::from(pcm_hdr.stream_id) as usize;
+                    dbg!("stream_id: {}", stream_id );
+
+                    desc_chain
+                        .memory()
+                        .write_obj(response, desc_response.addr())
+                        .map_err(|_| Error::DescriptorWriteFailed)?;
+                    len = desc_response.len() as u32;
+                },
+                _ => {
+                    error!(
+                        "virtio-snd: Unknown control queue message code: {}",
+                        request_type
+                    );
+                }
+            };
+            if vring
+                .add_used(desc_chain.head_index(), len)
+                .is_err()
+            {
+                error!("Couldn't return used descriptors to the ring");
+            }
+        }
+        // Send notification once all the requests are processed
+        debug!("Sending processed request notification");
+        vring
+            .signal_used_queue()
+            .map_err(|_| Error::SendNotificationFailed)?;
+        debug!("Process control queue finished");
+
         Ok(false)
     }
 
@@ -82,6 +306,7 @@ impl VhostUserSoundThread {
     fn process_rx(&self, _vring: &VringRwLock) -> IoResult<bool> {
         Ok(false)
     }
+
 }
 
 pub struct VhostUserSoundBackend {
@@ -91,6 +316,8 @@ pub struct VhostUserSoundBackend {
     _audio_backend: RwLock<Box<dyn AudioBackend + Send + Sync>>,
 }
 
+type SndDescriptorChain = DescriptorChain<GuestMemoryLoadGuard<GuestMemoryMmap<()>>>;
+
 impl VhostUserSoundBackend {
     pub fn new(config: SoundConfig) -> Result<Self> {
         let threads = if config.multi_thread {

crates/sound/src/virtio_sound.rs

@@ -48,8 +48,8 @@ pub const VIRTIO_SND_S_IO_ERR: u32 = 0x8003;
 
 // device data flow directions
 
-pub const VIRTIO_SND_D_OUTPUT: u32 = 0;
-pub const VIRTIO_SND_D_INPUT: u32 = 1;
+pub const VIRTIO_SND_D_OUTPUT: u8 = 0;
+pub const VIRTIO_SND_D_INPUT: u8 = 1;
 
 // supported jack features