diff options
Diffstat (limited to 'meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src')
4 files changed, 0 insertions, 1192 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 deleted file mode 100644 index 177c76cc..00000000 --- a/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/backend.rs +++ /dev/null @@ -1,170 +0,0 @@ -// 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 deleted file mode 100644 index 228266f6..00000000 --- a/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/can.rs +++ /dev/null @@ -1,264 +0,0 @@ -// 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 deleted file mode 100644 index e89934ea..00000000 --- a/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/main.rs +++ /dev/null @@ -1,26 +0,0 @@ -// 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 deleted file mode 100644 index 6aa12488..00000000 --- a/meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/vhu_can.rs +++ /dev/null @@ -1,732 +0,0 @@ -// 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() - } -} - |