gpio: Process virtio requests

This patch implements process_queue() to process incoming requests.

Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>

gpio/Cargo.toml

@@ -20,6 +20,7 @@ thiserror = "1.0"
 vhost = { version = "0.4", features = ["vhost-user-slave"] }
 vhost-user-backend = "0.3"
 virtio-bindings = ">=0.1"
+virtio-queue = "0.2"
 vm-memory = "0.7"
 vmm-sys-util = "=0.9.0"
 

gpio/src/vhu_gpio.rs

@@ -5,6 +5,9 @@
 //
 // SPDX-License-Identifier: Apache-2.0
 
+use log::error;
+use std::mem::size_of;
+use std::slice::from_raw_parts;
 use std::{convert, io};
 
 use thiserror::Error as ThisError;
@@ -14,11 +17,18 @@ use virtio_bindings::bindings::virtio_net::{VIRTIO_F_NOTIFY_ON_EMPTY, VIRTIO_F_V
 use virtio_bindings::bindings::virtio_ring::{
     VIRTIO_RING_F_EVENT_IDX, VIRTIO_RING_F_INDIRECT_DESC,
 };
-use vm_memory::{GuestMemoryAtomic, GuestMemoryMmap};
+use virtio_queue::DescriptorChain;
+use vm_memory::{
+    ByteValued, Bytes, GuestMemoryAtomic, GuestMemoryLoadGuard, GuestMemoryMmap, Le16, Le32,
+};
 use vmm_sys_util::epoll::EventSet;
 use vmm_sys_util::eventfd::{EventFd, EFD_NONBLOCK};
 
-use crate::gpio::{GpioController, GpioDevice};
+use crate::gpio::{GpioController, GpioDevice, VirtioGpioConfig};
+
+/// Possible values of the status field
+const VIRTIO_GPIO_STATUS_OK: u8 = 0x0;
+const VIRTIO_GPIO_STATUS_ERR: u8 = 0x1;
 
 const QUEUE_SIZE: usize = 256;
 const NUM_QUEUES: usize = 2;
@@ -29,13 +39,31 @@ const REQUEST_QUEUE: u16 = 0;
 type Result<T> = std::result::Result<T, Error>;
 type VhostUserBackendResult<T> = std::result::Result<T, std::io::Error>;
 
-#[derive(Debug, ThisError)]
+#[derive(Copy, Clone, Debug, PartialEq, ThisError)]
 /// Errors related to vhost-device-gpio-daemon.
 pub(crate) enum Error {
     #[error("Failed to handle event, didn't match EPOLLIN")]
     HandleEventNotEpollIn,
     #[error("Failed to handle unknown event")]
     HandleEventUnknown,
+    #[error("Received unexpected write only descriptor at index {0}")]
+    UnexpectedWriteOnlyDescriptor(usize),
+    #[error("Received unexpected readable descriptor at index {0}")]
+    UnexpectedReadableDescriptor(usize),
+    #[error("Invalid descriptor count {0}")]
+    UnexpectedDescriptorCount(usize),
+    #[error("Invalid descriptor size, expected: {0}, found: {1}")]
+    UnexpectedDescriptorSize(usize, u32),
+    #[error("Descriptor not found")]
+    DescriptorNotFound,
+    #[error("Descriptor read failed")]
+    DescriptorReadFailed,
+    #[error("Descriptor write failed")]
+    DescriptorWriteFailed,
+    #[error("Failed to send notification")]
+    NotificationFailed,
+    #[error("Failed to create new EventFd")]
+    EventFdFailed,
 }
 
 impl convert::From<Error> for io::Error {
@@ -44,23 +72,132 @@ impl convert::From<Error> for io::Error {
     }
 }
 
+/// Virtio GPIO Request / Response messages
+///
+/// The response message is a stream of bytes, where first byte represents the
+/// status, and rest is message specific data.
+#[derive(Copy, Clone, Default)]
+#[repr(C)]
+struct VirtioGpioRequest {
+    rtype: Le16,
+    gpio: Le16,
+    value: Le32,
+}
+unsafe impl ByteValued for VirtioGpioRequest {}
+
 pub(crate) struct VhostUserGpioBackend<D: GpioDevice> {
     controller: GpioController<D>,
     event_idx: bool,
     pub(crate) exit_event: EventFd,
 }
 
+type GpioDescriptorChain = DescriptorChain<GuestMemoryLoadGuard<GuestMemoryMmap<()>>>;
+
 impl<D: GpioDevice> VhostUserGpioBackend<D> {
     pub(crate) fn new(controller: GpioController<D>) -> Result<Self> {
         Ok(VhostUserGpioBackend {
             controller,
             event_idx: false,
-            exit_event: EventFd::new(EFD_NONBLOCK).expect("Creating exit eventfd"),
+            exit_event: EventFd::new(EFD_NONBLOCK).map_err(|_| Error::EventFdFailed)?,
         })
     }
 
+    /// Process the requests in the request queue
+    fn process_requests(
+        &self,
+        requests: Vec<GpioDescriptorChain>,
+        vring: &VringRwLock,
+    ) -> Result<bool> {
+        if requests.is_empty() {
+            return Ok(true);
+        }
+
+        for desc_chain in requests {
+            let descriptors: Vec<_> = desc_chain.clone().collect();
+            if descriptors.len() != 2 {
+                return Err(Error::UnexpectedDescriptorCount(descriptors.len()));
+            }
+
+            let desc_request = descriptors[0];
+            if desc_request.is_write_only() {
+                return Err(Error::UnexpectedWriteOnlyDescriptor(0));
+            }
+
+            if desc_request.len() as usize != size_of::<VirtioGpioRequest>() {
+                return Err(Error::UnexpectedDescriptorSize(
+                    size_of::<VirtioGpioRequest>(),
+                    desc_request.len(),
+                ));
+            }
+
+            let request = desc_chain
+                .memory()
+                .read_obj::<VirtioGpioRequest>(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 = match self.controller.operation(
+                request.rtype.to_native(),
+                request.gpio.to_native(),
+                request.value.to_native(),
+            ) {
+                Ok(mut data) => {
+                    if data.len() != (desc_response.len() - 1) as usize {
+                        error!(
+                            "Invalid response size, expected {}, received {}",
+                            desc_response.len(),
+                            data.len()
+                        );
+                        vec![VIRTIO_GPIO_STATUS_ERR, 0]
+                    } else {
+                        let mut buf = vec![VIRTIO_GPIO_STATUS_OK];
+                        buf.append(&mut data);
+                        buf
+                    }
+                }
+
+                Err(x) => {
+                    error!("{:?}", x);
+                    vec![VIRTIO_GPIO_STATUS_ERR, 0]
+                }
+            };
+
+            desc_chain
+                .memory()
+                .write_slice(response.as_slice(), desc_response.addr())
+                .map_err(|_| Error::DescriptorWriteFailed)?;
+
+            if vring
+                .add_used(desc_chain.head_index(), desc_response.len())
+                .is_err()
+            {
+                error!("Couldn't return used descriptors to the ring");
+            }
+        }
+
+        Ok(true)
+    }
+
     /// Process the messages in the vring and dispatch replies
-    fn process_request_queue(&self, _vring: &VringRwLock) -> Result<()> {
+    fn process_request_queue(&self, vring: &VringRwLock) -> Result<()> {
+        let requests: Vec<_> = vring
+            .get_mut()
+            .get_queue_mut()
+            .iter()
+            .map_err(|_| Error::DescriptorNotFound)?
+            .collect();
+
+        if self.process_requests(requests, vring)? {
+            // Send notification once all the requests are processed
+            vring
+                .signal_used_queue()
+                .map_err(|_| Error::NotificationFailed)?;
+        }
+
         Ok(())
     }
 }
@@ -87,7 +224,17 @@ impl<D: 'static + GpioDevice + Sync + Send> VhostUserBackendMut<VringRwLock, ()>
     }
 
     fn protocol_features(&self) -> VhostUserProtocolFeatures {
-        VhostUserProtocolFeatures::MQ
+        VhostUserProtocolFeatures::MQ | VhostUserProtocolFeatures::CONFIG
+    }
+
+    fn get_config(&self, _offset: u32, _size: u32) -> Vec<u8> {
+        unsafe {
+            from_raw_parts(
+                self.controller.get_config() as *const _ as *const _,
+                size_of::<VirtioGpioConfig>(),
+            )
+            .to_vec()
+        }
     }
 
     fn set_event_idx(&mut self, enabled: bool) {