meta-egvirt/recipes-extended/vhost-device-can/vhost-device-can-0.1.0/src/can.rs

   1 // CAN backend device
   2 //
   3 // Copyright 2023 VIRTUAL OPEN SYSTEMS SAS. All Rights Reserved.
   4 //          Timos Ampelikiotis <t.ampelikiotis@virtualopensystems.com>
   5 //
   6 // SPDX-License-Identifier: Apache-2.0 or BSD-3-Clause
   7
   8 use log::{warn, error};
   9 use std::sync::{Arc, RwLock};
  10
  11 use thiserror::Error as ThisError;
  12 use vm_memory::{ByteValued, Le16};
  13
  14 extern crate socketcan;
  15 use socketcan::{
  16         CanFdSocket, CanFdFrame, CanAnyFrame, EmbeddedFrame, Socket,
  17         Frame, StandardId,
  18 };
  19
  20 use std::thread::{JoinHandle, spawn};
  21 use vmm_sys_util::eventfd::{EFD_NONBLOCK, EventFd};
  22
  23 extern crate queues;
  24 use queues::*;
  25
  26 use crate::vhu_can::{VirtioCanFrame, VIRTIO_CAN_STATUS_OK};
  27
  28 type Result<T> = std::result::Result<T, Error>;
  29
  30 #[derive(Copy, Clone, Debug, PartialEq, ThisError)]
  31 /// Errors related to low level gpio helpers
  32 pub(crate) enum Error {
  33     //#[error("Can not enabled yet")]
  34     //CanNotEnabled,
  35     #[error("Can open socket operation failed")]
  36     CanSocketFailed,
  37     #[error("Can write socket operation failed")]
  38     CanSocketWriteFailed,
  39     #[error("Can read socket operation failed")]
  40     CanSocketReadFailed,
  41     #[error("Pop can element operation failed")]
  42     CanPopFailed,
  43     #[error("Creating Eventfd for CAN events failed")]
  44         CanEventFdFailed,
  45     #[error("CanQueueFailed")]
  46     CanQueueFailed,
  47 }
  48
  49 /* CAN flags to determine type of CAN Id */
  50 pub(crate) const VIRTIO_CAN_FLAGS_EXTENDED: u32 = 0x8000;
  51 pub(crate) const VIRTIO_CAN_FLAGS_FD: u32 = 0x4000;
  52 pub(crate) const VIRTIO_CAN_FLAGS_RTR: u32 = 0x2000;
  53
  54 pub(crate) const VIRTIO_CAN_TX: u16 = 0x0001;
  55 pub(crate) const VIRTIO_CAN_RX: u16 = 0x0101;
  56
  57 pub(crate) const CAN_EFF_FLAG: u32 = 0x80000000; /* EFF/SFF is set in the MSB */
  58 pub(crate) const CAN_RTR_FLAG: u32 = 0x40000000; /* remote transmission request */
  59 pub(crate) const CAN_ERR_FLAG: u32 = 0x20000000; /* error message frame */
  60
  61 pub(crate) const CAN_SFF_MASK: u32 = 0x000007FF; /* standard frame format (SFF) */
  62 pub(crate) const CAN_EFF_MASK: u32 = 0x1FFFFFFF; /* extended frame format (EFF) */
  63
  64 //pub(crate) const CAN_FRMF_BRS: u32 = 0x01; /* bit rate switch (2nd bitrate for data) */
  65 //pub(crate) const CAN_FRMF_ESI: u32 = 0x02; /* error state ind. of transmitting node */
  66 pub(crate) const CAN_FRMF_TYPE_FD: u32 = 0x10; /* internal bit ind. of CAN FD frame */
  67 pub(crate) const CAN_ERR_BUSOFF: u32 = 0x00000040; /* bus off */
  68
  69 /// Virtio Can Configuration
  70 #[derive(Copy, Clone, Debug, Default, PartialEq)]
  71 #[repr(C)]
  72 pub(crate) struct VirtioCanConfig {
  73         /* CAN controller status */
  74     pub(crate) status: Le16,
  75 }
  76
  77 // SAFETY: The layout of the structure is fixed and can be initialized by
  78 // reading its content from byte array.
  79 unsafe impl ByteValued for VirtioCanConfig {}
  80
  81 #[derive(Debug)]
  82 pub(crate) struct CanController {
  83     config: VirtioCanConfig,
  84     pub can_in_name: String,
  85     pub can_out_name: String,
  86         can_out_socket: CanFdSocket,
  87         pub rx_event_fd: EventFd,
  88         rx_fifo: Queue<VirtioCanFrame>,
  89 }
  90
  91 impl CanController {
  92     // Creates a new controller corresponding to `device`.
  93     pub(crate) fn new(can_in_name: String, can_out_name: String) -> Result<CanController> {
  94
  95         let can_in_name = can_in_name.to_owned();
  96                 println!("can_in_name: {:?}", can_in_name);
  97
  98         let can_out_name = can_out_name.to_owned();
  99                 println!("can_out_name: {:?}", can_out_name);
 100
 101                 let can_out_socket = Self::open_can_sockets(can_out_name.clone());
 102
 103                 let rx_fifo = Queue::new();
 104
 105                 let rx_efd = EventFd::new(EFD_NONBLOCK).map_err(|_| Error::CanEventFdFailed)?;
 106
 107         Ok(CanController {
 108             config: VirtioCanConfig {
 109                 status: 0x0.into(),
 110             },
 111             can_in_name,
 112             can_out_name,
 113                         can_out_socket,
 114                         rx_event_fd: rx_efd,
 115                         rx_fifo
 116         })
 117     }
 118
 119         pub fn print_can_frame (canframe: VirtioCanFrame) {
 120                 println!("canframe.msg_type 0x{:x}", canframe.msg_type.to_native());
 121         println!("canframe.can_id 0x{:x}", canframe.can_id.to_native());
 122         println!("canframe.length {}", canframe.length.to_native());
 123         println!("canframe.flags 0x{:x}", canframe.flags.to_native());
 124                 if canframe.length.to_native() == 0 {
 125                         println!("[]");
 126                         return;
 127                 }
 128         print!("[");
 129         let last_elem = canframe.length.to_native() as usize - 1;
 130         for (index, sdu) in canframe.sdu.iter().enumerate() {
 131             print!("0x{:x}, ", sdu);
 132           if index == last_elem {
 133               print!("0x{:x}", sdu);
 134                           break;
 135           }
 136         }
 137         println!("]");
 138         }
 139
 140         /* FIXME: This thread is not handle after termination */
 141         pub fn start_read_thread (controller: Arc<RwLock<CanController>>) -> JoinHandle<Result<()>> {
 142                 spawn(move || {
 143                                 CanController::read_can_socket(controller)
 144                         }
 145                 )
 146         }
 147
 148     pub fn push(&mut self, rx_elem: VirtioCanFrame) -> Result<()> {
 149                 match self.rx_fifo.add(rx_elem) {
 150                         Ok(_) => Ok(()), // Successfully added, so return Ok(())
 151                         _ => Err(Error::CanQueueFailed), // Handle other errors
 152                 }
 153     }
 154
 155     pub fn pop(&mut self) -> Result<VirtioCanFrame> {
 156                 match self.rx_fifo.remove() {
 157             Ok(item) => Ok(item),
 158             _ => Err(Error::CanPopFailed),
 159         }
 160     }
 161
 162         fn open_can_sockets (can_out_name: String) -> CanFdSocket {
 163             let can_out_socket = match CanFdSocket::open(&can_out_name) {
 164                         Ok(socket) => socket,
 165                 Err(_) => {
 166                     warn!("Error opening CAN socket");
 167                     panic!("Failed to open CAN socket.");
 168                                 //return Err(Error::CanSocketFailed);
 169                 }
 170             };
 171
 172                 can_out_socket
 173         }
 174
 175         pub fn read_can_socket (controller: Arc<RwLock<CanController>>) -> Result<()> {
 176                 let can_in_name = &controller.read().unwrap().can_in_name.clone();
 177                 dbg!("Start reading from {} socket!", &can_in_name);
 178             let socket = match CanFdSocket::open(&can_in_name) {
 179                         Ok(socket) => socket,
 180                 Err(_) => {
 181                     warn!("Error opening CAN socket");
 182                                 return Err(Error::CanSocketFailed);
 183                 }
 184             };
 185
 186                 // Receive CAN messages
 187             loop {
 188                         if let Ok(frame) = socket.read_frame() {
 189
 190                                 let mut controller = controller.write().unwrap();
 191                             match frame {
 192                                         CanAnyFrame::Normal(frame) => {
 193                                     // Regular CAN frame
 194                                     println!("Received CAN message: {:?}", frame);
 195                                 }
 196                                 CanAnyFrame::Fd(frame) => {
 197                                     // CAN FD frame
 198                                     println!("Received CAN FD message: {:?}", frame);
 199
 200                                                 let read_can_frame = VirtioCanFrame {
 201                                                         msg_type: VIRTIO_CAN_RX.into(),
 202                                                         can_id: frame.raw_id().into(),
 203                                                         length: (frame.data().len() as u16).into(),
 204                                                         reserved: 0.into(),
 205                                                         flags: frame.id_flags().bits().into(),
 206                                                         sdu: {
 207                                                                 let mut sdu_data: [u8; 64] = [0; 64];
 208                                                                 for i in 0..frame.data().len() {
 209                                                                     sdu_data[i] = frame.data()[i];
 210                                                                 }
 211                                                             sdu_data
 212                                                         },
 213                                                 };
 214
 215                                                 match controller.push(read_can_frame) {
 216                                                         Ok(_) => warn!("New Can frame was received"),
 217                                             Err(_) => {
 218                                                 warn!("Error read/push CAN frame");
 219                                                                 return Err(Error::CanSocketReadFailed);
 220                                             }
 221                                         }
 222                                 }
 223                                 CanAnyFrame::Remote(frame) => {
 224                                     // Remote CAN frame
 225                                     println!("Received Remote CAN message: {:?}", frame);
 226                                 }
 227                                 CanAnyFrame::Error(frame) => {
 228                                     // Error frame
 229                                     println!("Received Error frame: {:?}", frame);
 230                                 }
 231                             }
 232
 233                                 controller.rx_event_fd.write(1).unwrap();
 234                 }
 235             }
 236         }
 237
 238     pub(crate) fn config(&self) -> &VirtioCanConfig {
 239                 log::trace!("Get config\n");
 240         &self.config
 241     }
 242
 243     pub(crate) fn operation(&self, tx_request: VirtioCanFrame) -> Result<u8> {
 244                 log::trace!("Can operation\n");
 245
 246             // Create a CAN frame with a specific CAN-ID and the data buffer
 247                 let can_id = StandardId::new(tx_request.can_id.to_native().try_into().unwrap()).unwrap();
 248                 let data_len = tx_request.length.to_native() as usize;
 249
 250                 let data: Vec<u8> = tx_request.sdu.iter().cloned().take(data_len).collect();
 251             let frame = CanFdFrame::new(can_id, &data).unwrap();
 252
 253             // Send the CAN frame
 254             let write_result = self.can_out_socket.write_frame(&frame);
 255             match write_result {
 256                         Ok(_) => Ok(VIRTIO_CAN_STATUS_OK),
 257                 Err(_) => {
 258                     warn!("Error write CAN socket");
 259                                 Err(Error::CanSocketWriteFailed)
 260                 }
 261             }
 262     }
 263 }
 264