vhost-user-i2c: Process virtio requests

This patch implements process_queue() to process incoming requests.

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

1 files changed, 132 insertions, 2 deletions

diff --git a/src/vhu_i2c.rs b/src/vhu_i2c.rs
index 5854ba0..b023372 100644
--- a/src/vhu_i2c.rs
+++ b/src/vhu_i2c.rs
@@ -7,11 +7,12 @@
 
 use std::sync::{Arc, RwLock};
 use std::{convert, error, fmt, io};
+use std::mem::{size_of};
 use vhost::vhost_user::message::*;
 use vhost_user_backend::{VhostUserBackend, Vring};
 use virtio_bindings::bindings::virtio_net::*;
 use virtio_bindings::bindings::virtio_ring::{VIRTIO_RING_F_EVENT_IDX, VIRTIO_RING_F_INDIRECT_DESC};
-use vm_memory::{GuestMemoryAtomic, GuestMemoryMmap};
+use vm_memory::{Bytes, ByteValued, GuestMemoryAtomic, GuestMemoryMmap, Le16, Le32};
 use crate::i2c::*;
 
 const QUEUE_SIZE: usize = 1024;
@@ -49,6 +50,29 @@ impl convert::From<Error> for io::Error {
     }
 }
 
+
+// I2C definitions from Virtio Spec
+
+// The final status written by the device
+const VIRTIO_I2C_MSG_OK: u8 = 0;
+const VIRTIO_I2C_MSG_ERR: u8 = 1;
+
+#[derive(Copy, Clone, Default)]
+#[repr(C)]
+struct VirtioI2cOutHdr {
+    addr: Le16,
+    padding: Le16,
+    flags: Le32
+}
+unsafe impl ByteValued for VirtioI2cOutHdr {}
+
+#[derive(Copy, Clone, Default)]
+#[repr(C)]
+struct VirtioI2cInHdr {
+    status: u8
+}
+unsafe impl ByteValued for VirtioI2cInHdr {}
+
 pub struct VhostUserI2c {
     backend: I2cBackend,
     event_idx: bool,
@@ -68,8 +92,114 @@ impl VhostUserI2c {
     // Process the messages in the vring and dispatch replies
     fn process_queue(
         &self,
-        _vring: &mut Vring,
+        vring: &mut Vring,
     ) -> Result<bool> {
+        let mut reqs: Vec<I2cReq> = Vec::new();
+        let mut index = 0;
+
+        // Iterate over each I2C request and push it to "reqs" vector.
+        while let Some(mut desc_chain) = vring.mut_queue().iter().unwrap().next() {
+            let desc_out_hdr = desc_chain.next().unwrap();
+            let desc_buf = desc_chain.next().unwrap();
+            let desc_in_hdr = desc_chain.next().unwrap();
+            let mut flags: u16 = 0;
+
+            if desc_in_hdr.has_next() {
+                return Err(Error::UnexpectedDescriptorFound.into());
+            }
+
+            if desc_out_hdr.is_write_only() {
+                return Err(Error::UnexpectedWriteOnlyDescriptor.into());
+            }
+
+            if desc_out_hdr.len() as usize != size_of::<VirtioI2cOutHdr>() {
+                return Err(Error::UnexpectedDescriptorSize.into());
+            }
+
+            if !desc_in_hdr.is_write_only() {
+                return Err(Error::UnexpectedReadDescriptor.into());
+            }
+
+            if desc_in_hdr.len() as usize != size_of::<u8>() {
+                return Err(Error::UnexpectedDescriptorSize.into());
+            }
+
+            if desc_buf.len() == 0 {
+                return Err(Error::UnexpectedDescriptorSize.into());
+            }
+
+            let mut buf: Vec<u8> = vec![0; desc_buf.len() as usize];
+
+            if desc_buf.is_write_only() {
+                flags = I2C_M_RD;
+            } else {
+                desc_chain
+                    .memory()
+                    .read(&mut buf, desc_buf.addr()).unwrap();
+            }
+
+            let out_hdr = desc_chain
+                .memory()
+                .read_obj::<VirtioI2cOutHdr>(desc_out_hdr.addr()).unwrap();
+
+            reqs.push(
+                I2cReq {
+                    addr: out_hdr.addr.to_native() >> 1,
+                    flags: flags,
+                    len: desc_buf.len() as u16,
+                    buf: buf
+                }
+                );
+
+            index += 1;
+        }
+
+        if index == 0 {
+            return Ok(true);
+        }
+
+        // Roll back to the initial position, as we need to go over them again
+        for _ in 0..index {
+            vring.mut_queue().go_to_previous_position();
+        }
+
+        let in_hdr = match vi2c_xfer(&self.backend, &mut reqs) {
+            true => VirtioI2cInHdr { status: VIRTIO_I2C_MSG_OK },
+            false => VirtioI2cInHdr { status: VIRTIO_I2C_MSG_ERR },
+        };
+
+        index = 0;
+        while let Some(mut desc_chain) = vring.mut_queue().iter().unwrap().next() {
+            let _unused = desc_chain.next().unwrap();
+            let desc_buf = desc_chain.next().unwrap();
+            let desc_in_hdr = desc_chain.next().unwrap();
+            let mut len = size_of::<VirtioI2cInHdr>() as u32;
+
+            // Write the data read from the I2C client
+            if desc_buf.is_write_only() {
+                desc_chain
+                    .memory()
+                    .write(&reqs[index].buf, desc_buf.addr()).unwrap();
+            }
+
+            // Write the transfer status
+            desc_chain
+                .memory()
+                .write_obj::<VirtioI2cInHdr>(in_hdr, desc_in_hdr.addr()).unwrap();
+
+            if in_hdr.status == VIRTIO_I2C_MSG_OK {
+                len += desc_buf.len();
+            }
+
+            if vring.mut_queue().add_used(desc_chain.head_index(), len).is_err() {
+                println!("Couldn't return used descriptors to the ring");
+            }
+
+            index += 1;
+        }
+
+        // Send notification once all the requests are processed
+        vring.signal_used_queue().unwrap();
         Ok(true)
     }
 }