virtio-console: Initial multiport protocol support

This makes the device act as a multiport console.
This doesn't introduce new functionality, the number of ports remains
fixed at 1 port (for the terminal with stdin/stdout).

Signed-off-by: Matej Hrica <[email protected]>

Author: mtjhrc

src/devices/src/virtio/console/console_control.rs

@@ -0,0 +1,126 @@
+use std::collections::VecDeque;
+use std::ops::Deref;
+use std::sync::{Arc, Mutex};
+
+use libc::EFD_NONBLOCK;
+use utils::eventfd::EventFd;
+use vm_memory::{ByteValued, GuestMemoryMmap};
+
+use crate::virtio::console::defs::control_event::{
+    VIRTIO_CONSOLE_CONSOLE_PORT, VIRTIO_CONSOLE_PORT_ADD, VIRTIO_CONSOLE_RESIZE,
+};
+
+#[derive(Copy, Clone, Debug, Default)]
+#[repr(C, packed(4))]
+pub struct VirtioConsoleControl {
+    /// Port number
+    pub id: u32,
+    /// The kind of control event
+    pub event: u16,
+    /// Extra information for the event
+    pub value: u16,
+}
+
+// Safe because it only has data and has no implicit padding.
+// But NOTE that this relies on CPU being little endian, to have correct semantics
+unsafe impl ByteValued for VirtioConsoleControl {}
+
+#[derive(Copy, Clone, Debug, Default)]
+#[repr(C, packed)]
+pub struct VirtioConsoleResize {
+    // NOTE: the order of these fields in the actual kernel implementation and in the spec are swapped,
+    // we follow the order in the kernel to get it working correctly
+    pub rows: u16,
+    pub cols: u16,
+}
+
+// Safe because it only has data and has no implicit padding.
+// but NOTE, that we rely on CPU being little endian, for the values to be correct
+unsafe impl ByteValued for VirtioConsoleResize {}
+
+pub enum Payload {
+    ConsoleControl(VirtioConsoleControl),
+    Bytes(Vec<u8>),
+}
+
+impl Deref for Payload {
+    type Target = [u8];
+
+    fn deref(&self) -> &Self::Target {
+        match self {
+            Payload::ConsoleControl(b) => b.as_slice(),
+            Payload::Bytes(b) => b.as_slice(),
+        }
+    }
+}
+
+// Utility for sending commands into control rx queue
+pub struct ConsoleControl {
+    queue: Mutex<VecDeque<Payload>>,
+    queue_evt: EventFd,
+}
+
+impl ConsoleControl {
+    pub fn new() -> Arc<Self> {
+        Arc::new(Self {
+            queue: Default::default(),
+            queue_evt: EventFd::new(EFD_NONBLOCK).unwrap(),
+        })
+    }
+
+    pub fn mark_console_port(&self, _mem: &GuestMemoryMmap, port_id: u32) {
+        self.push_msg(VirtioConsoleControl {
+            id: port_id,
+            event: VIRTIO_CONSOLE_CONSOLE_PORT,
+            value: 1,
+        })
+    }
+
+    pub fn console_resize(&self, port_id: u32, new_size: VirtioConsoleResize) {
+        let mut buf = Vec::new();
+        buf.extend(
+            VirtioConsoleControl {
+                id: port_id,
+                event: VIRTIO_CONSOLE_RESIZE,
+                value: 0,
+            }
+            .as_slice(),
+        );
+        buf.extend(new_size.as_slice());
+        self.push_vec(buf)
+    }
+
+    /// Adds another port with the specified port_id
+    pub fn port_add(&self, port_id: u32) {
+        self.push_msg(VirtioConsoleControl {
+            id: port_id,
+            event: VIRTIO_CONSOLE_PORT_ADD,
+            value: 0,
+        })
+    }
+
+    pub fn queue_pop(&self) -> Option<Payload> {
+        let mut queue = self.queue.lock().expect("Poisoned lock");
+        queue.pop_front()
+    }
+
+    pub fn queue_evt(&self) -> &EventFd {
+        &self.queue_evt
+    }
+
+    fn push_msg(&self, msg: VirtioConsoleControl) {
+        let mut queue = self.queue.lock().expect("Poisoned lock");
+        queue.push_back(Payload::ConsoleControl(msg));
+        if let Err(e) = self.queue_evt.write(1) {
+            log::trace!("ConsoleControl failed to write to notify {e}")
+        }
+    }
+
+    fn push_vec(&self, buf: Vec<u8>) {
+        let mut queue = self.queue.lock().expect("Poisoned lock");
+        queue.push_back(Payload::Bytes(buf));
+        if let Err(e) = self.queue_evt.write(1) {
+            log::trace!("ConsoleControl failed to write to notify {e}")
+        }
+    }
+}

src/devices/src/virtio/console/device.rs

@@ -2,6 +2,7 @@ use std::cmp;
 use std::collections::VecDeque;
 use std::io;
 use std::io::Write;
+use std::mem::{size_of, size_of_val};
 use std::os::unix::io::{AsRawFd, RawFd};
 use std::result;
 use std::sync::atomic::{AtomicUsize, Ordering};
@@ -16,14 +17,23 @@ use super::super::{
     ActivateError, ActivateResult, ConsoleError, DeviceState, Queue as VirtQueue, VirtioDevice,
     VIRTIO_MMIO_INT_CONFIG, VIRTIO_MMIO_INT_VRING,
 };
-use super::{defs, defs::uapi};
+use super::{defs, defs::control_event, defs::uapi};
 use crate::legacy::Gic;
+use crate::virtio::console::console_control::{
+    ConsoleControl, VirtioConsoleControl, VirtioConsoleResize,
+};
+use crate::virtio::console::defs::NUM_PORTS;
 use crate::Error as DeviceError;
 
 pub(crate) const RXQ_INDEX: usize = 0;
 pub(crate) const TXQ_INDEX: usize = 1;
-pub(crate) const AVAIL_FEATURES: u64 =
-    1 << uapi::VIRTIO_CONSOLE_F_SIZE as u64 | 1 << uapi::VIRTIO_F_VERSION_1 as u64;
+
+pub(crate) const CONTROL_RXQ_INDEX: usize = 2;
+pub(crate) const CONTROL_TXQ_INDEX: usize = 3;
+
+pub(crate) const AVAIL_FEATURES: u64 = 1 << uapi::VIRTIO_CONSOLE_F_SIZE as u64
+    | 1 << uapi::VIRTIO_CONSOLE_F_MULTIPORT as u64
+    | 1 << uapi::VIRTIO_F_VERSION_1 as u64;
 
 pub(crate) fn get_win_size() -> (u16, u16) {
     #[repr(C)]
@@ -60,20 +70,16 @@ impl VirtioConsoleConfig {
         VirtioConsoleConfig {
             cols,
             rows,
-            max_nr_ports: 1u32,
+            max_nr_ports: NUM_PORTS as u32,
             emerg_wr: 0u32,
         }
     }
-
-    pub fn update_console_size(&mut self, cols: u16, rows: u16) {
-        self.cols = cols;
-        self.rows = rows;
-    }
 }
 
 pub struct Console {
     pub(crate) queues: Vec<VirtQueue>,
     pub(crate) queue_events: Vec<EventFd>,
+    pub(crate) control: Arc<ConsoleControl>,
     pub(crate) avail_features: u64,
     pub(crate) acked_features: u64,
     pub(crate) interrupt_status: Arc<AtomicUsize>,
@@ -107,6 +113,7 @@ impl Console {
         let config = VirtioConsoleConfig::new(cols, rows);
 
         Ok(Console {
+            control: ConsoleControl::new(),
             queues,
             queue_events,
             avail_features: AVAIL_FEATURES,
@@ -185,9 +192,110 @@ impl Console {
     }
 
     pub fn update_console_size(&mut self, cols: u16, rows: u16) {
-        debug!("update_console_size: {} {}", cols, rows);
-        self.config.update_console_size(cols, rows);
-        self.signal_config_update().unwrap();
+        log::debug!("update_console_size: {} {}", cols, rows);
+        // Note that we currently only support resizing on the first/main console
+        self.control
+            .console_resize(0, VirtioConsoleResize { rows, cols });
+    }
+
+    pub(crate) fn process_control_rx(&mut self) -> bool {
+        log::trace!("process_control_rx");
+        let DeviceState::Activated(ref mem) = self.device_state else {
+            unreachable!()
+        };
+        let mut raise_irq = false;
+
+        while let Some(head) = self.queues[CONTROL_RXQ_INDEX].pop(mem) {
+            if let Some(buf) = self.control.queue_pop() {
+                match mem.write(&buf, head.addr) {
+                    Ok(n) => {
+                        if n != buf.len() {
+                            log::error!("process_control_rx: partial write");
+                        }
+                        raise_irq = true;
+                        log::trace!("process_control_rx wrote {n}");
+                        self.queues[CONTROL_RXQ_INDEX].add_used(mem, head.index, n as u32);
+                    }
+                    Err(e) => {
+                        log::error!("process_control_rx failed to write: {e}");
+                    }
+                }
+            } else {
+                self.queues[CONTROL_RXQ_INDEX].undo_pop();
+                break;
+            }
+        }
+        raise_irq
+    }
+
+    pub(crate) fn process_control_tx(&mut self) -> bool {
+        let mem = match self.device_state {
+            DeviceState::Activated(ref mem) => mem,
+            // This should never happen, it's been already validated in the event handler.
+            DeviceState::Inactive => unreachable!(),
+        };
+
+        let tx_queue = &mut self.queues[CONTROL_TXQ_INDEX];
+        let mut raise_irq = false;
+
+        while let Some(head) = tx_queue.pop(mem) {
+            raise_irq = true;
+
+            let cmd: VirtioConsoleControl = match mem.read_obj(head.addr) {
+                Ok(cmd) => cmd,
+                Err(e) => {
+                    log::error!(
+                    "Failed to read VirtioConsoleControl struct: {e:?}, struct len = {len}, head.len = {head_len}",
+                    len = size_of::<VirtioConsoleControl>(),
+                    head_len = head.len,
+                );
+                    continue;
+                }
+            };
+            tx_queue.add_used(mem, head.index, size_of_val(&cmd) as u32);
+
+            log::trace!("VirtioConsoleControl cmd: {cmd:?}");
+            match cmd.event {
+                control_event::VIRTIO_CONSOLE_DEVICE_READY => {
+                    log::debug!(
+                        "Device is ready: initialization {}",
+                        if cmd.value == 1 { "ok" } else { "failed" }
+                    );
+                    self.control.port_add(0);
+                }
+                control_event::VIRTIO_CONSOLE_PORT_READY => {
+                    if cmd.value != 1 {
+                        log::error!("Port initialization failed: {:?}", cmd);
+                        continue;
+                    }
+                    if cmd.id == 0 {
+                        self.control.mark_console_port(mem, 0);
+                    }
+                }
+                control_event::VIRTIO_CONSOLE_PORT_OPEN => {
+                    let opened = match cmd.value {
+                        0 => false,
+                        1 => true,
+                        _ => {
+                            log::error!(
+                                "Invalid value ({}) for VIRTIO_CONSOLE_PORT_OPEN on port {}",
+                                cmd.value,
+                                cmd.id
+                            );
+                            continue;
+                        }
+                    };
+
+                    if !opened {
+                        log::debug!("Guest closed port {}", cmd.id);
+                        continue;
+                    }
+                }
+                _ => log::warn!("Unknown console control event {:x}", cmd.event),
+            }
+        }
+
+        raise_irq
     }
 
     pub(crate) fn process_rx(&mut self) -> bool {