diff options
Diffstat (limited to 'meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src')
4 files changed, 1192 insertions, 0 deletions
diff --git a/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/backend.rs b/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/backend.rs new file mode 100644 index 00000000..177c76cc --- /dev/null +++ b/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/backend.rs @@ -0,0 +1,170 @@ +// VIRTIO CAN Emulation via vhost-user +// +// Copyright 2023 VIRTUAL OPEN SYSTEMS SAS. All Rights Reserved. +// Timos Ampelikiotis <t.ampelikiotis@virtualopensystems.com> +// +// SPDX-License-Identifier: Apache-2.0 or BSD-3-Clause + +use log::{error, info, warn}; +use std::process::exit; +use std::sync::{Arc, RwLock}; +use std::thread::{spawn, JoinHandle}; + +use clap::Parser; +use thiserror::Error as ThisError; +use vhost::{vhost_user, vhost_user::Listener}; +use vhost_user_backend::VhostUserDaemon; +use vm_memory::{GuestMemoryAtomic, GuestMemoryMmap}; + +use crate::can::{CanController}; +use crate::vhu_can::VhostUserCanBackend; + +pub(crate) type Result<T> = std::result::Result<T, Error>; + +#[derive(Debug, ThisError)] +/// Errors related to low level CAN helpers +pub(crate) enum Error { + #[error("Invalid socket count: {0}")] + SocketCountInvalid(usize), + #[error("Failed to join threads")] + FailedJoiningThreads, + #[error("Could not create can controller: {0}")] + CouldNotCreateCanController(crate::can::Error), + #[error("Could not create can backend: {0}")] + CouldNotCreateBackend(crate::vhu_can::Error), + #[error("Could not create daemon: {0}")] + CouldNotCreateDaemon(vhost_user_backend::Error), +} + +#[derive(Parser, Debug)] +#[clap(author, version, about, long_about = None)] +struct CanArgs { + /// Location of vhost-user Unix domain socket. This is suffixed by 0,1,2..socket_count-1. + #[clap(short, long)] + socket_path: String, + + /// A can device name to be used for reading (ex. vcan, can0, can1, ... etc.) + #[clap(short = 'i', long)] + can_in: String, + + /// A can device name to be used for writing (ex. vcan, can0, can1, ... etc.) + #[clap(short = 'o', long)] + can_out: String, + + /// Number of guests (sockets) to connect to. + #[clap(short = 'c', long, default_value_t = 1)] + socket_count: u32, +} + +#[derive(PartialEq, Debug)] +struct CanConfiguration { + socket_path: String, + socket_count: u32, + can_in: String, + can_out: String, +} + +impl TryFrom<CanArgs> for CanConfiguration { + type Error = Error; + + fn try_from(args: CanArgs) -> Result<Self> { + + if args.socket_count == 0 { + return Err(Error::SocketCountInvalid(0)); + } + + let can_in = args.can_in.trim().to_string(); + let can_out = args.can_out.trim().to_string(); + + Ok(CanConfiguration { + socket_path: args.socket_path, + socket_count: args.socket_count, + can_in, + can_out, + }) + } +} + +fn start_backend(args: CanArgs) -> Result<()> { + + println!("start_backend function!\n"); + + let config = CanConfiguration::try_from(args).unwrap(); + let mut handles = Vec::new(); + + for _ in 0..config.socket_count { + let socket = config.socket_path.to_owned(); + let can_in = config.can_in.to_owned(); + let can_out = config.can_out.to_owned(); + + let handle: JoinHandle<Result<()>> = spawn(move || loop { + // A separate thread is spawned for each socket and can connect to a separate guest. + // These are run in an infinite loop to not require the daemon to be restarted once a + // guest exits. + // + // There isn't much value in complicating code here to return an error from the + // threads, and so the code uses unwrap() instead. The panic on a thread won't cause + // trouble to other threads/guests or the main() function and should be safe for the + // daemon. + + let controller = + CanController::new(can_in.clone(), can_out.clone()).map_err(Error::CouldNotCreateCanController)?; + let shared_controller_1 = Arc::new(RwLock::new(controller)); + let shared_controller_2 = shared_controller_1.clone(); + let vu_can_backend = Arc::new(RwLock::new( + VhostUserCanBackend::new(shared_controller_1).map_err(Error::CouldNotCreateBackend)?, + )); + let _read_hanlde = CanController::start_read_thread(shared_controller_2); + + let mut daemon = VhostUserDaemon::new( + String::from("vhost-device-can-backend"), + vu_can_backend.clone(), + GuestMemoryAtomic::new(GuestMemoryMmap::new()), + ) + .map_err(Error::CouldNotCreateDaemon)?; + + /* Start the read thread -- need to handle it after termination */ + let vring_workers = daemon.get_epoll_handlers(); + vu_can_backend.read() + .unwrap() + .set_vring_worker(&vring_workers[0]); + + let listener = Listener::new(socket.clone(), true).unwrap(); + daemon.start(listener).unwrap(); + + match daemon.wait() { + Ok(()) => { + info!("Stopping cleanly."); + } + Err(vhost_user_backend::Error::HandleRequest( + vhost_user::Error::PartialMessage | vhost_user::Error::Disconnected, + )) => { + info!("vhost-user connection closed with partial message. If the VM is shutting down, this is expected behavior; otherwise, it might be a bug."); + } + Err(e) => { + warn!("Error running daemon: {:?}", e); + } + } + + // No matter the result, we need to shut down the worker thread. + vu_can_backend.read().unwrap().exit_event.write(1).unwrap(); + }); + + handles.push(handle); + } + + for handle in handles { + handle.join().map_err(|_| Error::FailedJoiningThreads)??; + } + + Ok(()) +} + +pub(crate) fn can_init() { + env_logger::init(); + println!("Can_init function!"); + if let Err(e) = start_backend(CanArgs::parse()) { + error!("{e}"); + exit(1); + } +} diff --git a/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/can.rs b/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/can.rs new file mode 100644 index 00000000..228266f6 --- /dev/null +++ b/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/can.rs @@ -0,0 +1,264 @@ +// CAN backend device +// +// Copyright 2023 VIRTUAL OPEN SYSTEMS SAS. All Rights Reserved. +// Timos Ampelikiotis <t.ampelikiotis@virtualopensystems.com> +// +// SPDX-License-Identifier: Apache-2.0 or BSD-3-Clause + +use log::{warn, error}; +use std::sync::{Arc, RwLock}; + +use thiserror::Error as ThisError; +use vm_memory::{ByteValued, Le16}; + +extern crate socketcan; +use socketcan::{ + CanFdSocket, CanFdFrame, CanAnyFrame, EmbeddedFrame, Socket, + Frame, StandardId, +}; + +use std::thread::{JoinHandle, spawn}; +use vmm_sys_util::eventfd::{EFD_NONBLOCK, EventFd}; + +extern crate queues; +use queues::*; + +use crate::vhu_can::{VirtioCanFrame, VIRTIO_CAN_STATUS_OK}; + +type Result<T> = std::result::Result<T, Error>; + +#[derive(Copy, Clone, Debug, PartialEq, ThisError)] +/// Errors related to low level gpio helpers +pub(crate) enum Error { + //#[error("Can not enabled yet")] + //CanNotEnabled, + #[error("Can open socket operation failed")] + CanSocketFailed, + #[error("Can write socket operation failed")] + CanSocketWriteFailed, + #[error("Can read socket operation failed")] + CanSocketReadFailed, + #[error("Pop can element operation failed")] + CanPopFailed, + #[error("Creating Eventfd for CAN events failed")] + CanEventFdFailed, + #[error("CanQueueFailed")] + CanQueueFailed, +} + +/* CAN flags to determine type of CAN Id */ +pub(crate) const VIRTIO_CAN_FLAGS_EXTENDED: u32 = 0x8000; +pub(crate) const VIRTIO_CAN_FLAGS_FD: u32 = 0x4000; +pub(crate) const VIRTIO_CAN_FLAGS_RTR: u32 = 0x2000; + +pub(crate) const VIRTIO_CAN_TX: u16 = 0x0001; +pub(crate) const VIRTIO_CAN_RX: u16 = 0x0101; + +pub(crate) const CAN_EFF_FLAG: u32 = 0x80000000; /* EFF/SFF is set in the MSB */ +pub(crate) const CAN_RTR_FLAG: u32 = 0x40000000; /* remote transmission request */ +pub(crate) const CAN_ERR_FLAG: u32 = 0x20000000; /* error message frame */ + +pub(crate) const CAN_SFF_MASK: u32 = 0x000007FF; /* standard frame format (SFF) */ +pub(crate) const CAN_EFF_MASK: u32 = 0x1FFFFFFF; /* extended frame format (EFF) */ + +//pub(crate) const CAN_FRMF_BRS: u32 = 0x01; /* bit rate switch (2nd bitrate for data) */ +//pub(crate) const CAN_FRMF_ESI: u32 = 0x02; /* error state ind. of transmitting node */ +pub(crate) const CAN_FRMF_TYPE_FD: u32 = 0x10; /* internal bit ind. of CAN FD frame */ +pub(crate) const CAN_ERR_BUSOFF: u32 = 0x00000040; /* bus off */ + +/// Virtio Can Configuration +#[derive(Copy, Clone, Debug, Default, PartialEq)] +#[repr(C)] +pub(crate) struct VirtioCanConfig { + /* CAN controller status */ + pub(crate) status: Le16, +} + +// SAFETY: The layout of the structure is fixed and can be initialized by +// reading its content from byte array. +unsafe impl ByteValued for VirtioCanConfig {} + +#[derive(Debug)] +pub(crate) struct CanController { + config: VirtioCanConfig, + pub can_in_name: String, + pub can_out_name: String, + can_out_socket: CanFdSocket, + pub rx_event_fd: EventFd, + rx_fifo: Queue<VirtioCanFrame>, +} + +impl CanController { + // Creates a new controller corresponding to `device`. + pub(crate) fn new(can_in_name: String, can_out_name: String) -> Result<CanController> { + + let can_in_name = can_in_name.to_owned(); + println!("can_in_name: {:?}", can_in_name); + + let can_out_name = can_out_name.to_owned(); + println!("can_out_name: {:?}", can_out_name); + + let can_out_socket = Self::open_can_sockets(can_out_name.clone()); + + let rx_fifo = Queue::new(); + + let rx_efd = EventFd::new(EFD_NONBLOCK).map_err(|_| Error::CanEventFdFailed)?; + + Ok(CanController { + config: VirtioCanConfig { + status: 0x0.into(), + }, + can_in_name, + can_out_name, + can_out_socket, + rx_event_fd: rx_efd, + rx_fifo + }) + } + + pub fn print_can_frame (canframe: VirtioCanFrame) { + println!("canframe.msg_type 0x{:x}", canframe.msg_type.to_native()); + println!("canframe.can_id 0x{:x}", canframe.can_id.to_native()); + println!("canframe.length {}", canframe.length.to_native()); + println!("canframe.flags 0x{:x}", canframe.flags.to_native()); + if canframe.length.to_native() == 0 { + println!("[]"); + return; + } + print!("["); + let last_elem = canframe.length.to_native() as usize - 1; + for (index, sdu) in canframe.sdu.iter().enumerate() { + print!("0x{:x}, ", sdu); + if index == last_elem { + print!("0x{:x}", sdu); + break; + } + } + println!("]"); + } + + /* FIXME: This thread is not handle after termination */ + pub fn start_read_thread (controller: Arc<RwLock<CanController>>) -> JoinHandle<Result<()>> { + spawn(move || { + CanController::read_can_socket(controller) + } + ) + } + + pub fn push(&mut self, rx_elem: VirtioCanFrame) -> Result<()> { + match self.rx_fifo.add(rx_elem) { + Ok(_) => Ok(()), // Successfully added, so return Ok(()) + _ => Err(Error::CanQueueFailed), // Handle other errors + } + } + + pub fn pop(&mut self) -> Result<VirtioCanFrame> { + match self.rx_fifo.remove() { + Ok(item) => Ok(item), + _ => Err(Error::CanPopFailed), + } + } + + fn open_can_sockets (can_out_name: String) -> CanFdSocket { + let can_out_socket = match CanFdSocket::open(&can_out_name) { + Ok(socket) => socket, + Err(_) => { + warn!("Error opening CAN socket"); + panic!("Failed to open CAN socket."); + //return Err(Error::CanSocketFailed); + } + }; + + can_out_socket + } + + pub fn read_can_socket (controller: Arc<RwLock<CanController>>) -> Result<()> { + let can_in_name = &controller.read().unwrap().can_in_name.clone(); + dbg!("Start reading from {} socket!", &can_in_name); + let socket = match CanFdSocket::open(&can_in_name) { + Ok(socket) => socket, + Err(_) => { + warn!("Error opening CAN socket"); + return Err(Error::CanSocketFailed); + } + }; + + // Receive CAN messages + loop { + if let Ok(frame) = socket.read_frame() { + + let mut controller = controller.write().unwrap(); + match frame { + CanAnyFrame::Normal(frame) => { + // Regular CAN frame + println!("Received CAN message: {:?}", frame); + } + CanAnyFrame::Fd(frame) => { + // CAN FD frame + println!("Received CAN FD message: {:?}", frame); + + let read_can_frame = VirtioCanFrame { + msg_type: VIRTIO_CAN_RX.into(), + can_id: frame.raw_id().into(), + length: (frame.data().len() as u16).into(), + reserved: 0.into(), + flags: frame.id_flags().bits().into(), + sdu: { + let mut sdu_data: [u8; 64] = [0; 64]; + for i in 0..frame.data().len() { + sdu_data[i] = frame.data()[i]; + } + sdu_data + }, + }; + + match controller.push(read_can_frame) { + Ok(_) => warn!("New Can frame was received"), + Err(_) => { + warn!("Error read/push CAN frame"); + return Err(Error::CanSocketReadFailed); + } + } + } + CanAnyFrame::Remote(frame) => { + // Remote CAN frame + println!("Received Remote CAN message: {:?}", frame); + } + CanAnyFrame::Error(frame) => { + // Error frame + println!("Received Error frame: {:?}", frame); + } + } + + controller.rx_event_fd.write(1).unwrap(); + } + } + } + + pub(crate) fn config(&self) -> &VirtioCanConfig { + log::trace!("Get config\n"); + &self.config + } + + pub(crate) fn operation(&self, tx_request: VirtioCanFrame) -> Result<u8> { + log::trace!("Can operation\n"); + + // Create a CAN frame with a specific CAN-ID and the data buffer + let can_id = StandardId::new(tx_request.can_id.to_native().try_into().unwrap()).unwrap(); + let data_len = tx_request.length.to_native() as usize; + + let data: Vec<u8> = tx_request.sdu.iter().cloned().take(data_len).collect(); + let frame = CanFdFrame::new(can_id, &data).unwrap(); + + // Send the CAN frame + let write_result = self.can_out_socket.write_frame(&frame); + match write_result { + Ok(_) => Ok(VIRTIO_CAN_STATUS_OK), + Err(_) => { + warn!("Error write CAN socket"); + Err(Error::CanSocketWriteFailed) + } + } + } +} + diff --git a/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/main.rs b/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/main.rs new file mode 100644 index 00000000..e89934ea --- /dev/null +++ b/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/main.rs @@ -0,0 +1,26 @@ +// VIRTIO CAN Emulation via vhost-user +// +// Copyright 2023 VIRTUAL OPEN SYSTEMS SAS. All Rights Reserved. +// Timos Ampelikiotis <t.ampelikiotis@virtualopensystems.com> +// +// SPDX-License-Identifier: Apache-2.0 or BSD-3-Clause + +#[cfg(target_env = "gnu")] +mod backend; +#[cfg(target_env = "gnu")] +mod can; +#[cfg(target_env = "gnu")] +mod vhu_can; + +#[cfg(target_env = "gnu")] +fn main() { + println!("Hello to main vhost-user-can"); + backend::can_init() +} + +// Rust vmm container (https://github.com/rust-vmm/rust-vmm-container) doesn't +// have tools to do a musl build at the moment, and adding that support is +// tricky as well to the container. Skip musl builds until the time pre-built +// libgpiod library is available for musl. +#[cfg(target_env = "musl")] +fn main() {} diff --git a/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/vhu_can.rs b/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/vhu_can.rs new file mode 100644 index 00000000..6aa12488 --- /dev/null +++ b/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/vhu_can.rs @@ -0,0 +1,732 @@ +// vhost device can +// +// Copyright 2023 VIRTUAL OPEN SYSTEMS SAS. All Rights Reserved. +// Timos Ampelikiotis <t.ampelikiotis@virtualopensystems.com> +// +// SPDX-License-Identifier: Apache-2.0 or BSD-3-Clause + +use log::{warn, error}; +use std::mem::size_of; +use std::slice::from_raw_parts; +use std::sync::{Arc, RwLock}; +use std::{ + convert, + io::{self, Result as IoResult}, +}; +use std::os::fd::AsRawFd; +use thiserror::Error as ThisError; +use vhost::vhost_user::message::{VhostUserProtocolFeatures, VhostUserVirtioFeatures}; +use vhost_user_backend::{VhostUserBackendMut, VringRwLock, VringT}; +use virtio_bindings::bindings::virtio_config::{VIRTIO_F_NOTIFY_ON_EMPTY, VIRTIO_F_VERSION_1}; +use virtio_bindings::bindings::virtio_ring::{ + VIRTIO_RING_F_EVENT_IDX, VIRTIO_RING_F_INDIRECT_DESC, +}; +use virtio_queue::{DescriptorChain, QueueOwnedT}; +use vm_memory::{ + ByteValued, Bytes, GuestAddressSpace, GuestMemoryAtomic, GuestMemoryLoadGuard, + GuestMemoryMmap, Le16, Le32, +}; +use vmm_sys_util::epoll::EventSet; +use vmm_sys_util::eventfd::{EventFd, EFD_NONBLOCK}; +use crate::can::{ + CanController, CAN_EFF_FLAG, CAN_RTR_FLAG, CAN_ERR_FLAG, CAN_SFF_MASK, + CAN_EFF_MASK, VIRTIO_CAN_FLAGS_FD, VIRTIO_CAN_FLAGS_RTR, + VIRTIO_CAN_FLAGS_EXTENDED, VIRTIO_CAN_TX, VIRTIO_CAN_RX, + CAN_FRMF_TYPE_FD, CAN_ERR_BUSOFF, +}; +use vhost_user_backend::VringEpollHandler; + +/// Feature bit numbers +pub const VIRTIO_CAN_F_CAN_CLASSIC: u16 = 0; +pub const VIRTIO_CAN_F_CAN_FD: u16 = 1; +//pub const VIRTIO_CAN_F_LATE_TX_ACK: u16 = 2; +pub const VIRTIO_CAN_F_RTR_FRAMES: u16 = 3; + +/// Possible values of the status field +pub const VIRTIO_CAN_STATUS_OK: u8 = 0x0; +pub const VIRTIO_CAN_STATUS_ERR: u8 = 0x1; + +/// CAN Control messages +const VIRTIO_CAN_SET_CTRL_MODE_START: u16 = 0x0201; +const VIRTIO_CAN_SET_CTRL_MODE_STOP: u16 = 0x0202; + +/// Virtio configuration +const QUEUE_SIZE: usize = 64; +const NUM_QUEUES: usize = 3; + +/// Queues +const TX_QUEUE: u16 = 0; +const RX_QUEUE: u16 = 1; +const CTRL_QUEUE: u16 = 2; +const BACKEND_EFD: u16 = 4; + +type Result<T> = std::result::Result<T, Error>; + +#[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, + #[error("Unknown can message type: {0}")] + UnexpectedCanMsgType(u16), + #[error("RTR frames not negotiated")] + UnexpectedRtrFlag, + #[error("Can FD frames not negotiated")] + UnexpectedFdFlag, + #[error("Classic CAN frames not negotiated")] + UnexpectedClassicFlag, +} + +impl convert::From<Error> for io::Error { + fn from(e: Error) -> Self { + io::Error::new(io::ErrorKind::Other, e) + } +} + +/// Virtio CAN 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 VirtioCanTxResponse { + result: i8, +} +// SAFETY: The layout of the structure is fixed and can be initialized by +// reading its content from byte array. +unsafe impl ByteValued for VirtioCanTxResponse {} + +#[derive(Copy, Clone, Debug)] +#[repr(C)] +pub struct VirtioCanFrame { + pub msg_type: Le16, + pub length: Le16, /* 0..8 CC, 0..64 CANFD, 0..2048 CANXL, 12 bits */ + pub reserved: Le32, /* May be needed in part for CAN XL priority */ + pub flags: Le32, + pub can_id: Le32, + pub sdu: [u8; 64], +} + +impl Default for VirtioCanFrame { + fn default() -> Self { + VirtioCanFrame { + msg_type: Le16::default(), + length: Le16::default(), + reserved: Le32::default(), + flags: Le32::default(), + can_id: Le32::default(), + sdu: [0; 64], // Initialize "asd" with default value (0 in this case) + } + } +} + +// SAFETY: The layout of the structure is fixed and can be initialized by +// reading its content from byte array. +unsafe impl ByteValued for VirtioCanFrame {} + +#[derive(Copy, Clone, Default)] +#[repr(C)] +struct VirtioCanCtrlRequest { + msg_type: Le16, +} +// SAFETY: The layout of the structure is fixed and can be initialized by +// reading its content from byte array. +unsafe impl ByteValued for VirtioCanCtrlRequest {} + +#[derive(Copy, Clone, Default)] +#[repr(C)] +struct VirtioCanCtrlResponse { + result: i8, +} +// SAFETY: The layout of the structure is fixed and can be initialized by +// reading its content from byte array. +unsafe impl ByteValued for VirtioCanCtrlResponse {} + +pub(crate) struct VhostUserCanBackend { + controller: Arc<RwLock<CanController>>, + acked_features: u64, + event_idx: bool, + pub(crate) exit_event: EventFd, + mem: Option<GuestMemoryAtomic<GuestMemoryMmap>>, +} + +type CanDescriptorChain = DescriptorChain<GuestMemoryLoadGuard<GuestMemoryMmap<()>>>; + +impl VhostUserCanBackend { + pub(crate) fn new(controller: Arc<RwLock<CanController>>) -> Result<Self> { + Ok(VhostUserCanBackend { + controller: controller, + event_idx: false, + acked_features: 0x0, + exit_event: EventFd::new(EFD_NONBLOCK).map_err(|_| Error::EventFdFailed)?, + mem: None, + }) + } + + fn check_features (&self, features: u16) -> bool { + (self.acked_features & (1 << features)) != 0 + } + + fn process_ctrl_requests( + &self, + requests: Vec<CanDescriptorChain>, + vring: &VringRwLock + ) -> Result<bool> { + dbg!("process_ctrl_requests"); + + if requests.is_empty() { + return Ok(true); + } + + for desc_chain in requests { + let descriptors: Vec<_> = desc_chain.clone().collect(); + + if descriptors.len() < 1 { + warn!("Error::UnexpectedDescriptorCount"); + return Err(Error::UnexpectedDescriptorCount(descriptors.len())); + } + + println!("descriptors.len(): {:?}", descriptors.len()); + + let desc_request = descriptors[0]; + if desc_request.is_write_only() { + warn!("Error::UnexpectedWriteOnlyDescriptor"); + return Err(Error::UnexpectedWriteOnlyDescriptor(0)); + } + + if desc_request.len() as usize != size_of::<VirtioCanCtrlRequest>() { + println!("UnexpectedDescriptorSize, len = {:?}", desc_request.len()); + return Err(Error::UnexpectedDescriptorSize( + size_of::<VirtioCanCtrlRequest>(), + desc_request.len(), + )); + } + + let request = desc_chain + .memory() + .read_obj::<VirtioCanCtrlRequest>(desc_request.addr()) + .map_err(|_| Error::DescriptorReadFailed)?; + + + match request.msg_type.into() { + VIRTIO_CAN_SET_CTRL_MODE_START => { + println!("VIRTIO_CAN_SET_CTRL_MODE_START"); + //vcan->busoff = false; + Ok(()) + } + VIRTIO_CAN_SET_CTRL_MODE_STOP => { + println!("VIRTIO_CAN_SET_CTRL_MODE_STOP"); + //vcan->busoff = false; + Ok(()) + } + _ => { + println!("Ctrl queue: msg type 0x{:?} unknown", request.msg_type); + return Err(Error::HandleEventUnknown.into()) + }, + }?; + + let desc_response = descriptors[1]; + if !desc_response.is_write_only() { + println!("This is not wirtable"); + return Err(Error::UnexpectedReadableDescriptor(1)); + } + + let response = VIRTIO_CAN_STATUS_OK; + + 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() { + println!("Couldn't return used descriptors to the ring"); + warn!("Couldn't return used descriptors to the ring"); + } + } + + Ok(true) + } + + fn process_tx_requests( + &self, + requests: Vec<CanDescriptorChain>, + vring: &VringRwLock + ) -> Result<bool> { + dbg!("process_tx_requests"); + + if requests.is_empty() { + return Ok(true); + } + + println!("requests.len: {:?}", requests.len()); + for desc_chain in requests { + let descriptors: Vec<_> = desc_chain.clone().collect(); + + if descriptors.len() != 2 { + println!("Error::UnexpectedDescriptorCount"); + return Err(Error::UnexpectedDescriptorCount(descriptors.len())); + } + + let desc_request = descriptors[0]; + if desc_request.is_write_only() { + println!("Error::UnexpectedReadOnlyDescriptor"); + return Err(Error::UnexpectedWriteOnlyDescriptor(0)); + } + + if desc_request.len() as usize != size_of::<VirtioCanFrame>() { + println!("Tx UnexpectedDescriptorSize, len = {:?}", desc_request.len()); + //return Err(Error::UnexpectedDescriptorSize( + // size_of::<VirtioCanFrame>(), + // desc_request.len(), + //)); + } + + let request = desc_chain + .memory() + .read_obj::<VirtioCanFrame>(desc_request.addr()) + .map_err(|_| Error::DescriptorReadFailed)?; + + CanController::print_can_frame(request); + + let msg_type = request.msg_type.to_native(); + let mut can_id = request.can_id.to_native(); + let mut flags = request.flags.to_native(); + let mut length = request.length.to_native(); + + if msg_type != VIRTIO_CAN_TX { + warn!("TX: Message type 0x{:x} unknown\n", msg_type); + return Err(Error::UnexpectedCanMsgType(msg_type)); + } + + if (flags & VIRTIO_CAN_FLAGS_FD) != 0 { + if length > 64 { + println!("Cut sdu_len from {:?} to 64\n", request.length); + length = 64; + } + } else { + if length > 8 { + println!("Cut sdu_len from {:?} to 8\n", request.length); + length = 8; + } + } + + /* + * Copy Virtio frame structure to qemu frame structure and + * check while doing this whether the frame type was negotiated + */ + if (flags & VIRTIO_CAN_FLAGS_EXTENDED) != 0 { + flags &= CAN_EFF_MASK; + flags |= CAN_EFF_FLAG; + } else { + flags &= CAN_SFF_MASK; + } + + if (flags & VIRTIO_CAN_FLAGS_RTR) != 0 { + if !self.check_features(VIRTIO_CAN_F_CAN_CLASSIC) || + !self.check_features(VIRTIO_CAN_F_RTR_FRAMES) { + warn!("TX: RTR frames not negotiated"); + return Err(Error::UnexpectedRtrFlag); + } + can_id |= flags | CAN_RTR_FLAG; + } + + if (flags & VIRTIO_CAN_FLAGS_FD) != 0 { + if !self.check_features(VIRTIO_CAN_F_CAN_FD) { + warn!("TX: FD frames not negotiated\n"); + return Err(Error::UnexpectedFdFlag); + } + flags |= CAN_FRMF_TYPE_FD; + } else { + if !self.check_features(VIRTIO_CAN_F_CAN_CLASSIC) { + warn!("TX: Classic frames not negotiated\n"); + return Err(Error::UnexpectedClassicFlag); + } + flags = 0; + } + + let mut corrected_request = VirtioCanFrame::default(); + corrected_request.msg_type = msg_type.into(); + corrected_request.can_id = can_id.into(); + corrected_request.flags = flags.into(); + corrected_request.length = length.into(); + corrected_request.sdu.copy_from_slice(&request.sdu[0..64]); + + let desc_response = descriptors[1]; + if !desc_response.is_write_only() { + println!("Error::UnexpectedWriteOnlyDescriptor"); + return Err(Error::UnexpectedReadableDescriptor(1)); + } + + let response = match self.controller.write().unwrap().operation(corrected_request) { + Ok(result) => { + result + } + Err(_) => { + warn!("We got an error from controller send func"); + VIRTIO_CAN_STATUS_ERR + } + }; + + 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() { + println!("Couldn't return used descriptors to the ring"); + warn!("Couldn't return used descriptors to the ring"); + } + } + + Ok(true) + } + + fn process_rx_requests( + &mut self, + requests: Vec<CanDescriptorChain>, + vring: &VringRwLock + ) -> Result<bool> { + dbg!("process_rx_requests"); + + if requests.is_empty() { + return Ok(true); + } + + let desc_chain = &requests[0]; + let descriptors: Vec<_> = desc_chain.clone().collect(); + + if descriptors.len() != 1 { + println!("Error::UnexpectedDescriptorCount"); + return Err(Error::UnexpectedDescriptorCount(descriptors.len())); + } + + let desc_response = descriptors[0]; + if !desc_response.is_write_only() { + return Err(Error::UnexpectedReadableDescriptor(1)); + } + + let mut response = match self.controller.write().unwrap().pop() { + Ok(item) => item, + Err(_) => return Err(Error::HandleEventUnknown), + }; + + CanController::print_can_frame(response); + + if (response.can_id.to_native() & CAN_ERR_FLAG) != 0 { + if (response.can_id.to_native() & CAN_ERR_BUSOFF) != 0 { + warn!("Got BusOff error frame, device does a local bus off\n"); + //vcan->busoff = true; + } else { + println!("Dropping error frame 0x{:x}\n", response.can_id.to_native()); + } + return Ok(true); + } + + let mut can_rx = VirtioCanFrame::default(); + can_rx.msg_type = VIRTIO_CAN_RX.into(); + can_rx.can_id = response.can_id; + can_rx.length = response.length; + can_rx.flags = (can_rx.flags.to_native() | VIRTIO_CAN_FLAGS_FD).into(); + + if (response.flags.to_native() & CAN_FRMF_TYPE_FD) != 0 { + if !self.check_features(VIRTIO_CAN_F_CAN_FD) { + warn!("Drop non-supported CAN FD frame"); + return Err(Error::UnexpectedFdFlag); + } + } else { + if !self.check_features(VIRTIO_CAN_F_CAN_CLASSIC) { + warn!("Drop non-supported CAN classic frame"); + return Err(Error::UnexpectedClassicFlag); + } + } + if (response.can_id.to_native() & CAN_RTR_FLAG) != 0 && + !self.check_features(VIRTIO_CAN_F_RTR_FRAMES) { + warn!("Drop non-supported RTR frame"); + return Err(Error::UnexpectedRtrFlag); + } + + if (response.can_id.to_native() & CAN_EFF_FLAG) != 0 { + can_rx.flags = VIRTIO_CAN_FLAGS_EXTENDED.into(); + can_rx.can_id = (response.can_id.to_native() & CAN_EFF_MASK).into(); + } else { + can_rx.can_id = (response.can_id.to_native() & CAN_SFF_MASK).into(); + } + if (response.can_id.to_native() & CAN_RTR_FLAG) != 0 { + can_rx.flags = (can_rx.flags.to_native() & VIRTIO_CAN_FLAGS_RTR).into(); + } + + // HACK AHEAD: Vcan can not be comfigured as CANFD interface, but possible + // to configure its MTU to 64 bytes. So if a messages bigger than 8 bytes + // is being received we consider it as CANFD message. + let can_in_name = self.controller.read().unwrap().can_in_name.clone(); + if self.check_features(VIRTIO_CAN_F_CAN_FD) && + response.length.to_native() > 8 && can_in_name == "vcan0" { + response.flags = (response.flags.to_native() | CAN_FRMF_TYPE_FD).into(); + warn!("\n\n\nCANFD VCAN0\n\n"); + } + + if (response.flags.to_native() & CAN_FRMF_TYPE_FD) != 0 { + can_rx.flags = (can_rx.flags.to_native() | VIRTIO_CAN_FLAGS_FD).into(); + if response.length.to_native() > 64 { + warn!("%s(): Cut length from {} to 64\n", response.length.to_native()); + can_rx.length = 64.into(); + } + } else { + if response.length.to_native() > 8 { + warn!("%s(): Cut length from {} to 8\n", response.length.to_native()); + can_rx.length = 8.into(); + } + } + + can_rx.sdu.copy_from_slice(&response.sdu[0..64]); + CanController::print_can_frame(can_rx); + + desc_chain + .memory() + .write_slice(can_rx.as_slice(), desc_response.addr()) + .map_err(|_| Error::DescriptorWriteFailed)?; + + if vring.add_used(desc_chain.head_index(), desc_response.len()).is_err() { + warn!("Couldn't return used descriptors to the ring"); + } + + Ok(true) + } + + /// Process the messages in the vring and dispatch replies + fn process_ctrl_queue(&mut self, vring: &VringRwLock) -> Result<()> { + dbg!("process_ctrl_queue"); + let requests: Vec<_> = vring + .get_mut() + .get_queue_mut() + .iter(self.mem.as_ref().unwrap().memory()) + .map_err(|_| Error::DescriptorNotFound)? + .collect(); + + if self.process_ctrl_requests(requests, vring)? { + // Send notification once all the requests are processed + vring + .signal_used_queue() + .map_err(|_| Error::NotificationFailed)?; + } + Ok(()) + } + + /// Process the messages in the vring and dispatch replies + fn process_tx_queue(&self, vring: &VringRwLock) -> Result<()> { + dbg!("process_tx_queue"); + let requests: Vec<_> = vring + .get_mut() + .get_queue_mut() + .iter(self.mem.as_ref().unwrap().memory()) + .map_err(|_| Error::DescriptorNotFound)? + .collect(); + + if self.process_tx_requests(requests, vring)? { + // Send notification once all the requests are processed + vring + .signal_used_queue() + .map_err(|_| { + println!("signal_used_queue error"); + Error::NotificationFailed + })?; + } + + Ok(()) + } + + /// Process the messages in the vring and dispatch replies + fn process_rx_queue(&mut self, vring: &VringRwLock) -> Result<()> { + dbg!("process_rx_queue"); + let requests: Vec<_> = vring + .get_mut() + .get_queue_mut() + .iter(self.mem.as_ref().unwrap().memory()) + .map_err(|_| Error::DescriptorNotFound)? + .collect(); + + if self.process_rx_requests(requests, vring)? { + // Send notification once all the requests are processed + vring + .signal_used_queue() + .map_err(|_| { + println!("NotificationFailed"); + Error::NotificationFailed + })?; + } + Ok(()) + } + + fn process_rx_queue_dump(&mut self, _vring: &VringRwLock) -> Result<()> { + dbg!("Do nothing, if you reach that point!"); + Ok(()) + } + + /// Set self's VringWorker. + pub(crate) fn set_vring_worker( + &self, + vring_worker: &Arc<VringEpollHandler<Arc<RwLock<VhostUserCanBackend>>, VringRwLock, ()>>, + ) { + let rx_event_fd = self.controller.read().unwrap().rx_event_fd.as_raw_fd(); + vring_worker + .register_listener( + rx_event_fd, + EventSet::IN, + //u64::from(BACKEND_EFD)) + 4u64 as u64) + .unwrap(); + } +} + +/// VhostUserBackendMut trait methods +impl VhostUserBackendMut<VringRwLock, ()> + for VhostUserCanBackend +{ + fn num_queues(&self) -> usize { + println!("num_queues: {:?}", NUM_QUEUES); + NUM_QUEUES + } + + fn max_queue_size(&self) -> usize { + println!("max_queue_size: {:?}", QUEUE_SIZE); + QUEUE_SIZE + } + + fn features(&self) -> u64 { + // this matches the current libvhost defaults except VHOST_F_LOG_ALL + let features = 1 << VIRTIO_F_VERSION_1 + | 1 << VIRTIO_F_NOTIFY_ON_EMPTY + | 1 << VIRTIO_RING_F_EVENT_IDX + | 1 << VIRTIO_CAN_F_CAN_CLASSIC + | 1 << VIRTIO_CAN_F_CAN_FD + | 1 << VIRTIO_RING_F_INDIRECT_DESC + | VhostUserVirtioFeatures::PROTOCOL_FEATURES.bits(); + + println!("vhu_can->features: {:x}", features); + features + } + + fn acked_features(&mut self, _features: u64) { + println!("\nacked_features: 0x{:x}\n", _features); + self.acked_features = _features; + } + + fn protocol_features(&self) -> VhostUserProtocolFeatures { + let protocol_features = VhostUserProtocolFeatures::MQ + | VhostUserProtocolFeatures::CONFIG + | VhostUserProtocolFeatures::REPLY_ACK; + //| VhostUserProtocolFeatures::STATUS + + println!("protocol_features: {:x}", protocol_features); + protocol_features + } + + fn get_config(&self, offset: u32, size: u32) -> Vec<u8> { + // SAFETY: The layout of the structure is fixed and can be initialized by + // reading its content from byte array. + dbg!("vhu_can->get_config"); + unsafe { + from_raw_parts( + self.controller.write().unwrap() + .config() + .as_slice() + .as_ptr() + .offset(offset as isize) as *const _ as *const _, + size as usize, + ) + .to_vec() + } + } + + fn set_event_idx(&mut self, enabled: bool) { + dbg!(self.event_idx = enabled); + } + + fn update_memory(&mut self, mem: GuestMemoryAtomic<GuestMemoryMmap>) -> IoResult<()> { + dbg!("update_memory\n"); + self.mem = Some(mem); + Ok(()) + } + + fn handle_event( + &mut self, + device_event: u16, + evset: EventSet, + vrings: &[VringRwLock], + _thread_id: usize, + ) -> IoResult<bool> { + dbg!("\nhandle_event:"); + + if evset != EventSet::IN { + return Err(Error::HandleEventNotEpollIn.into()); + } + if device_event == RX_QUEUE { + println!("RX_QUEUE\n"); + return Ok(false); + }; + let vring = if device_event != BACKEND_EFD { + &vrings[device_event as usize] + } else { + println!("BACKEND_EFD\n"); + let _ = self.controller.write().unwrap().rx_event_fd.read(); + &vrings[RX_QUEUE as usize] + }; + 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(); + //match queue_idx { + match device_event { + CTRL_QUEUE => self.process_ctrl_queue(vring), + TX_QUEUE => self.process_tx_queue(vring), + RX_QUEUE => self.process_rx_queue_dump(vring), + BACKEND_EFD => self.process_rx_queue(vring), + _ => Err(Error::HandleEventUnknown.into()), + }?; + if !vring.enable_notification().unwrap() { + break; + } + } + } else { + // Without EVENT_IDX, a single call is enough. + match device_event { + CTRL_QUEUE => self.process_ctrl_queue(vring), + TX_QUEUE => self.process_tx_queue(vring), + RX_QUEUE => self.process_rx_queue_dump(vring), + BACKEND_EFD => self.process_rx_queue(vring), + _ => Err(Error::HandleEventUnknown.into()), + }?; + } + Ok(false) + } + + fn exit_event(&self, _thread_index: usize) -> Option<EventFd> { + dbg!("exit_event\n"); + self.exit_event.try_clone().ok() + } +} + |