diff --git a/vhost-device-scmi/src/devices/common.rs b/vhost-device-scmi/src/devices/common.rs index 4b259ba..9408cb9 100644 --- a/vhost-device-scmi/src/devices/common.rs +++ b/vhost-device-scmi/src/devices/common.rs @@ -12,6 +12,8 @@ use std::collections::{HashMap, HashSet}; use std::ffi::OsString; use std::fmt::Write; +use std::fs::File; +use std::os::unix::io::RawFd; use itertools::Itertools; use log::debug; @@ -21,7 +23,7 @@ use crate::scmi::{ self, DeviceResult, MessageId, MessageValue, MessageValues, ProtocolId, ScmiDevice, ScmiDeviceError, MAX_SIMPLE_STRING_LENGTH, SENSOR_AXIS_DESCRIPTION_GET, SENSOR_CONFIG_GET, SENSOR_CONFIG_SET, SENSOR_CONTINUOUS_UPDATE_NOTIFY, SENSOR_DESCRIPTION_GET, SENSOR_PROTOCOL_ID, - SENSOR_READING_GET, + SENSOR_READING_GET, SENSOR_UPDATE, }; use super::{fake, iio}; @@ -230,6 +232,15 @@ pub struct Sensor { /// Sensors can be enabled and disabled using SCMI. [Sensor]s created /// using [Sensor::new] are disabled initially. enabled: bool, + + /// Sensor notification can be enabled or disabled when frontend sends SENSOR_CONTINUOUS_UPDATE_NOTIFY. + notify_enabled: bool, + /// If this sensor supports notifying the frontend actively, it should record + /// notification device file here. (e.g. For iio device, the file is /dev/iio:deviceX) + pub notify_dev: Option, + + /// Sensor id, to identify the sensor in notification lookup. + pub sensor_id: usize, } impl Sensor { @@ -237,6 +248,9 @@ impl Sensor { Self { name: properties.get("name").map(|s| (*s).to_owned()), enabled: false, + notify_enabled: false, + notify_dev: None, + sensor_id: 0, } } } @@ -322,7 +336,7 @@ pub trait SensorT: Send { } else { self.format_unit(0) }; - // During initialization, sensor name has be set. + // During initialization, sensor name has been set. let name = self.sensor().name.clone().unwrap(); let values: MessageValues = vec![ // attributes low @@ -401,6 +415,15 @@ pub trait SensorT: Send { return Result::Err(ScmiDeviceError::UnsupportedRequest); } self.sensor_mut().enabled = config != 0; + + if self.sensor().enabled { + self.notify_status_set(true) + .map_err(|_| ScmiDeviceError::GenericError)?; + } else { + self.notify_status_set(false) + .map_err(|_| ScmiDeviceError::GenericError)?; + } + debug!("Sensor enabled: {}", self.sensor().enabled); Ok(vec![]) } @@ -440,19 +463,93 @@ pub trait SensorT: Send { self.config_set(config) } SENSOR_CONTINUOUS_UPDATE_NOTIFY => { - // Linux VIRTIO SCMI insists on this. - // We can accept it and ignore it, the sensor will be still working. - Ok(vec![]) + // Linux VIRTIO SCMI insists on handling this. + match parameters[0].get_unsigned() { + 1 => { + self.sensor_mut().notify_enabled = true; + Ok(vec![MessageValue::Signed(1)]) + } + 0 => { + self.sensor_mut().notify_enabled = false; + Ok(vec![MessageValue::Signed(0)]) + } + _ => Result::Err(ScmiDeviceError::InvalidParameters), + } } SENSOR_READING_GET => { if !self.sensor().enabled { return Result::Err(ScmiDeviceError::NotEnabled); } - self.reading_get() + if self.sensor().notify_enabled { + self.notify_status_set(false) + .map_err(|_| ScmiDeviceError::GenericError)?; + } + let ret = self.reading_get(); + if self.sensor().notify_enabled { + self.notify_status_set(true) + .map_err(|_| ScmiDeviceError::GenericError)?; + } + ret } _ => Result::Err(ScmiDeviceError::UnsupportedRequest), } } + + /// Returns the notification messages from the device. + /// + /// Usually need to redefine this. Different sensors may have different ways to + /// get notifications. + fn reading_update(&mut self, _device_index: u32) -> DeviceResult { + Ok(vec![]) + } + + /// Enable/Disable Sensor notify function. + /// + /// Usually need to redefine this. + /// Different sensors require different configuration to enable/disable notifications. + fn notify_status_set(&self, _enabled: bool) -> Result<(), DeviceError> { + Ok(()) + } + + /// Get notify device fd + /// + /// This fd is used for getting notifications. + /// It should be registered in epoll handler. + fn get_notify_fd(&self) -> Option { + None + } + + /// Set the device id. + /// + /// Usually no need to redefine this. + /// Sensor id is increasing during registration. + fn set_id(&mut self, id: usize) { + self.sensor_mut().sensor_id = id; + } + + /// Get id of this device. + /// + /// Usually no need to redefine this. + fn get_id(&self) -> usize { + self.sensor().sensor_id + } + + /// Get a nofication message value from this sensor. + /// + /// The default implementation supports only SENSOR_UPDATE notification. + fn notify(&mut self, device_index: u32, message_id: MessageId) -> DeviceResult { + match message_id { + SENSOR_UPDATE => { + // Read pending notifications, to prevent spamming the frontend with EVENT:IN interrupts. + let ret = self.reading_update(device_index); + if !self.sensor().enabled || !self.sensor().notify_enabled { + return Result::Err(ScmiDeviceError::NotEnabled); + } + ret + } + _ => Result::Err(ScmiDeviceError::UnsupportedNotify), + } + } } // It's possible to impl ScmiDevice for SensorT but it is not very useful @@ -472,6 +569,22 @@ impl ScmiDevice for SensorDevice { fn handle(&mut self, message_id: MessageId, parameters: &[MessageValue]) -> DeviceResult { self.0.handle(message_id, parameters) } + + fn get_notify_fd(&self) -> Option { + self.0.get_notify_fd() + } + + fn set_id(&mut self, id: usize) { + self.0.set_id(id) + } + + fn get_id(&self) -> usize { + self.0.get_id() + } + + fn notify(&mut self, device_index: u32, message_id: MessageId) -> DeviceResult { + self.0.notify(device_index, message_id) + } } #[cfg(test)] diff --git a/vhost-device-scmi/src/devices/fake.rs b/vhost-device-scmi/src/devices/fake.rs index de786d3..f863497 100644 --- a/vhost-device-scmi/src/devices/fake.rs +++ b/vhost-device-scmi/src/devices/fake.rs @@ -9,9 +9,9 @@ //! arranging SCMI virtualization setup without the need to bind real host //! devices. -use crate::scmi::{self, DeviceResult, MessageValue}; - use super::common::{DeviceProperties, MaybeDevice, Sensor, SensorDevice, SensorT}; +use crate::scmi::{self, DeviceResult, MessageValue}; +use std::os::unix::io::RawFd; pub struct FakeSensor { sensor: Sensor, @@ -54,6 +54,25 @@ impl SensorT for FakeSensor { } Ok(result) } + + fn get_notify_fd(&self) -> Option { + Some(1) + } + + fn reading_update(&mut self, device_index: u32) -> DeviceResult { + let value = self.value; + self.value = self.value.overflowing_add(1).0; + let mut result = vec![]; + result.push(MessageValue::Unsigned(0)); + result.push(MessageValue::Unsigned(device_index)); + for i in 0..3 { + result.push(MessageValue::Signed(i32::from(value) + 100 * i)); + result.push(MessageValue::Signed(0)); + result.push(MessageValue::Signed(0)); + result.push(MessageValue::Signed(0)); + } + Ok(result) + } } impl FakeSensor { diff --git a/vhost-device-scmi/src/devices/iio.rs b/vhost-device-scmi/src/devices/iio.rs index 4ce817e..d7c52fb 100644 --- a/vhost-device-scmi/src/devices/iio.rs +++ b/vhost-device-scmi/src/devices/iio.rs @@ -12,7 +12,11 @@ use std::cmp::{max, min}; use std::ffi::{OsStr, OsString}; use std::fs; -use std::io::ErrorKind; +use std::fs::File; +use std::io::{ErrorKind, Read}; +use std::os::unix::io::AsRawFd; +use std::os::unix::io::RawFd; + use std::path::{Path, PathBuf}; use std::str::FromStr; @@ -161,6 +165,82 @@ const UNIT_MAPPING: &[UnitMapping] = &[ const IIO_DEFAULT_NAME: &str = "iio"; +#[derive(PartialEq, Debug, Clone, Copy)] +enum IioEndian { + IioBe, + IioLe, +} + +/// Representation of an IIO channel axis's scan type. +/// It is read from sysfs "scan_element/_type" +/// +/// Used also for scalar values. +#[derive(PartialEq, Debug, Clone, Copy)] +struct ChanScanType { + sign: char, + realbits: u8, + storagebits: u8, + shift: u8, + repeat: u8, + endianness: IioEndian, +} + +impl ChanScanType { + /// Construct a ChanScanType from fs value + /// + /// The channel scan type follows the rule + /// If repeat > 1, "%s:%c%d/%dX%d>>%u\n" + /// Else, "%s:%c%d/%d>>%u\n". + /// For more details, see kernel "drivers/iio/industrialio-buffer.c" + fn new(value: String) -> Option { + let error_message = "Error format from iio device!"; + let endianness = match &value[0..2] { + "le" => IioEndian::IioLe, + "be" => IioEndian::IioBe, + _ => panic!("{}", error_message), + }; + let sign = match &value[3..4] { + "s" => 's', + "u" => 'u', + _ => panic!("{}", error_message), + }; + let index_split = value.find('/').expect(error_message); + let index_shift = value.find('>').expect(error_message); + + let realbits: u8 = value[4..index_split].parse().expect(error_message); + let (storagebits, repeat, shift): (u8, u8, u8) = match value.find('X') { + Some(index_repeat) => ( + value[index_split + 1..index_repeat] + .parse() + .expect(error_message), + value[index_repeat + 1..index_shift] + .parse() + .expect(error_message), + value[index_shift + 2..value.len() - 1] + .parse() + .expect(error_message), + ), + None => ( + value[index_split + 1..index_shift] + .parse() + .expect(error_message), + 0, + value[index_shift + 2..value.len() - 1] + .parse() + .expect(error_message), + ), + }; + Some(ChanScanType { + sign, + realbits, + storagebits, + shift, + repeat, + endianness, + }) + } +} + /// Representation of an IIO channel axis. /// /// Used also for scalar values. @@ -175,6 +255,35 @@ struct Axis { /// sufficiently accurate SCMI value that is represented by an integer (not /// a float) + decadic exponent. custom_exponent: i8, + /// Channel scan type, necessary if the sensor supports notifications. + /// The data from /dev/iio:deviceX will be formatted according to this. + /// The ChanScanType is parsed from "scan_elements/_type" + scan_type: Option, +} + +impl Axis { + fn new(path: OsString, unit_exponent: i8, custom_exponent: i8) -> Axis { + let scan_path = Path::new(&path).parent().unwrap().join("scan_elements"); + let mut scan_name = path.clone(); + scan_name.push("_type"); + let scan_type_path = scan_path.join(Path::new(&scan_name).file_name().unwrap()); + if scan_type_path.is_file() { + let scan_type = fs::read_to_string(scan_type_path).unwrap(); + Axis { + path, + unit_exponent, + custom_exponent, + scan_type: ChanScanType::new(scan_type), + } + } else { + Axis { + path, + unit_exponent, + custom_exponent, + scan_type: None, + } + } + } } /// Particular IIO sensor specification. @@ -238,6 +347,15 @@ impl SensorT for IIOSensor { } axes.sort_by(|a1, a2| a1.path.cmp(&a2.path)); self.axes = axes; + + // If /dev/iio:deviceX exists, it means that this device can notify + // Open it and store it in SensorT. + let path_split: Vec<_> = self.path.to_str().unwrap().split('/').collect(); + let dev_path = "/dev/".to_owned() + path_split[path_split.len() - 1]; + if Path::new(&dev_path.clone()).exists() { + self.sensor_mut().notify_dev = Some(File::open(dev_path.clone()).unwrap()); + } + Ok(()) } @@ -287,6 +405,105 @@ impl SensorT for IIOSensor { } Ok(result) } + + fn get_notify_fd(&self) -> Option { + self.sensor.notify_dev.as_ref().map(|fd| fd.as_raw_fd()) + } + + fn reading_update(&mut self, device_index: u32) -> DeviceResult { + let mut result = vec![]; + // The buffer length should correspond to the IIO device type. + // The type is available from /sys/bus/iio/devices/iio:deviceX/scan_elements/in_XXX_type. + // For example, if the content of in_XXX_type is le:s16/16>>0, each value is a little endian + // signed 16-bit integer. For a 3-axes sensor with [x, y, z, (t)] values, i.e. the 3 axes plus an + // optional timestamp, we need 6 or 8 bytes buffer. + // Currently, the only supported type is "le:s16/16>>0". + let scan_type = self.axes[0].scan_type.unwrap(); + + let signed = scan_type.sign == 's'; + let le_endian = scan_type.endianness == IioEndian::IioLe; + if !signed || !le_endian { + error!("Unsupported notification format: {:?}", scan_type); + return Err(ScmiDeviceError::GenericError); + } + + let sample_byte = (scan_type.realbits as f64 / 8_f64).ceil() as usize; + let sample_buffer_len = sample_byte * self.axes.len(); + let mut buffer = vec![0u8; sample_buffer_len]; + let mut file = self.sensor().notify_dev.as_ref().unwrap(); + + match file.read(&mut buffer) { + Ok(len) => { + if len > 0 { + result.push(MessageValue::Unsigned(0)); // Agentid + result.push(MessageValue::Unsigned(device_index)); // Sensorid + + // If SCMI sensor_attributes_low Bit[9] ("Timestamp supported") is set + // then the sensor can provide timestamped values and the timestamp + // should be read (and reported below), for example: + /* + let _time_stamp = i16::from_le_bytes( + buffer[self.axes.len() * 2..self.axes.len() * 2 + 2] + .try_into() + .unwrap(), + ); + */ + for i in 0..self.axes.len() { + let value = + i16::from_le_bytes(buffer[i * 2..i * 2 + 2].try_into().unwrap()); + let value_i64 = self + .deal_axis_raw_data(value as i64, &self.axes[i]) + .unwrap(); + let sensor_value_low = (value_i64 & 0xffff_ffff) as i32; + let sensor_value_high = (value_i64 >> 32) as i32; + result.push(MessageValue::Signed(sensor_value_low)); + result.push(MessageValue::Signed(sensor_value_high)); + // Timestamp, currently not provided: + result.push(MessageValue::Unsigned(0)); + result.push(MessageValue::Unsigned(0)); + } + } + } + Err(_) => { + return Err(ScmiDeviceError::GenericError); + } + } + Ok(result) + } + + fn notify_status_set(&self, enabled: bool) -> Result<(), DeviceError> { + let path_split: Vec<_> = self.path.to_str().unwrap().split('/').collect(); + let iio_name = path_split[path_split.len() - 1]; + let buffer_enable = format!("/sys/bus/iio/devices/{}/buffer/enable", iio_name); + let mut scan_enable = vec![]; + for i in 0..self.number_of_axes() { + scan_enable.push(format!( + "/sys/bus/iio/devices/{}/scan_elements/{}_{}_en", + iio_name, + self.channel.clone().into_string().unwrap(), + self.axis_name_suffix(i).to_lowercase() + )); + } + match enabled { + true => { + for scan_path in scan_enable { + fs::write(scan_path.clone(), "1") + .map_err(|e| DeviceError::IOError(scan_path.into(), e))?; + } + fs::write(buffer_enable.clone(), "1") + .map_err(|e| DeviceError::IOError(buffer_enable.into(), e))?; + } + false => { + fs::write(buffer_enable.clone(), "0") + .map_err(|e| DeviceError::IOError(buffer_enable.into(), e))?; + for scan_path in scan_enable { + fs::write(scan_path.clone(), "0") + .map_err(|e| DeviceError::IOError(scan_path.into(), e))?; + } + } + }; + Ok(()) + } } fn read_number_from_file(path: &Path) -> Result, ScmiDeviceError> { @@ -390,11 +607,11 @@ impl IIOSensor { // To get meaningful integer values, we must adjust exponent to // the provided scale if any. let custom_exponent = self.custom_exponent(path, unit_exponent); - axes.push(Axis { - path: OsString::from(path), + axes.push(Axis::new( + OsString::from(path), unit_exponent, custom_exponent, - }); + )); } fn register_iio_file(&mut self, file: fs::DirEntry, axes: &mut Vec) { @@ -432,6 +649,11 @@ impl IIOSensor { for value_path in [ Path::new(&(String::from(path.to_str().unwrap()) + "_" + name)), &Path::new(&path).parent().unwrap().join(name), + Path::new(&format!( + "{}_{}", + &String::from(path.to_str().unwrap())[..path.len() - 2], + name + )), ] .iter() { @@ -811,6 +1033,34 @@ mod tests { assert_eq!(result.get(3).unwrap(), &MessageValue::Unsigned(0)); } + #[test] + fn test_iio_reading_scalar_multiple_axis() { + let directory = IIODirectory::new(&[ + ("in_accel_x_raw", "205\n"), + ("in_accel_y_raw", "-392\n"), + ("in_accel_z_raw", "16518\n"), + ("in_accel_scale", "0.000598205\n"), + ]); + let mut sensor = make_iio_sensor(&directory, "in_accel".to_owned(), None); + sensor.initialize().unwrap(); + let result = sensor.reading_get().unwrap(); + assert_eq!(result.len(), 12); + assert_eq!(result.first().unwrap(), &MessageValue::Signed(1226)); + assert_eq!(result.get(4).unwrap(), &MessageValue::Signed(-2345)); + assert_eq!(result.get(8).unwrap(), &MessageValue::Signed(98812)); + for i in 0..12 { + if i % 4 == 2 || i % 4 == 3 { + assert_eq!(result.get(i).unwrap(), &MessageValue::Unsigned(0)); + } + if i != 5 && i % 4 == 1 { + assert_eq!(result.get(i).unwrap(), &MessageValue::Signed(0)); + } + if i == 5 { + assert_eq!(result.get(i).unwrap(), &MessageValue::Signed(-1)); + } + } + } + #[test] fn test_iio_reading_axes() { let directory = IIODirectory::new(&[ @@ -839,4 +1089,41 @@ mod tests { } } } + + #[test] + fn test_scan_type_parse() { + assert_eq!( + ChanScanType::new(String::from("le:s16/16>>0\n")).unwrap(), + ChanScanType { + sign: 's', + realbits: 16, + storagebits: 16, + shift: 0, + repeat: 0, + endianness: IioEndian::IioLe, + } + ); + assert_eq!( + ChanScanType::new(String::from("be:u24/28>>5\n")).unwrap(), + ChanScanType { + sign: 'u', + realbits: 24, + storagebits: 28, + shift: 5, + repeat: 0, + endianness: IioEndian::IioBe, + } + ); + assert_eq!( + ChanScanType::new(String::from("le:s12/16X4>>3\n")).unwrap(), + ChanScanType { + sign: 's', + realbits: 12, + storagebits: 16, + shift: 3, + repeat: 4, + endianness: IioEndian::IioLe, + } + ); + } } diff --git a/vhost-device-scmi/src/main.rs b/vhost-device-scmi/src/main.rs index 476e164..e390f1e 100644 --- a/vhost-device-scmi/src/main.rs +++ b/vhost-device-scmi/src/main.rs @@ -126,6 +126,11 @@ fn start_backend(config: VuScmiConfig) -> Result<()> { ) .unwrap(); + // Register devices such as "/dev/iio:deviceX" which can actively notify the frontend to epoll the handler. + // Then once there is data coming from these devices, an event will be created. (device_event=3) + let handlers = daemon.get_epoll_handlers(); + backend.read().unwrap().register_device_event_fd(handlers); + daemon .serve(&config.socket_path) .map_err(|e| format!("{e}"))?; diff --git a/vhost-device-scmi/src/scmi.rs b/vhost-device-scmi/src/scmi.rs index 3653528..c097ad5 100644 --- a/vhost-device-scmi/src/scmi.rs +++ b/vhost-device-scmi/src/scmi.rs @@ -13,6 +13,7 @@ use std::{ cmp::min, collections::HashMap, + os::unix::io::RawFd, sync::{Arc, Mutex}, }; @@ -23,6 +24,7 @@ use thiserror::Error as ThisError; use crate::devices::common::DeviceError; pub type MessageHeader = u32; +use super::vhu_scmi::NOTIFY_ALLOW_START_FD; pub const MAX_SIMPLE_STRING_LENGTH: usize = 16; // incl. NULL terminator @@ -56,8 +58,9 @@ pub type MessageValues = Vec; #[derive(Debug, PartialEq)] enum MessageType { // 4-bit unsigned integer - Command, // 0 - Unsupported, // anything else + Command, // 0 + Notification = 3, // Notifications have a message type of 3 + Unsupported, // anything else } pub type MessageId = u8; pub type ProtocolId = u8; @@ -126,10 +129,19 @@ impl From<&MessageValues> for Response { } } +impl Response { + // Notification is different from general response, it doesn't need ReturnStatus. + pub fn from_notification(value: &MessageValues) -> Self { + Self { + values: value.to_vec(), + } + } +} + /// SCMI response in SCMI representation byte. /// /// Use [ScmiResponse::from] function to construct it. -#[derive(Debug)] +#[derive(Debug, PartialEq)] pub struct ScmiResponse { header: MessageHeader, ret_bytes: Vec, @@ -247,6 +259,8 @@ pub const SENSOR_AXIS_DESCRIPTION_GET: MessageId = 0x7; pub const SENSOR_CONFIG_GET: MessageId = 0x9; pub const SENSOR_CONFIG_SET: MessageId = 0xA; pub const SENSOR_CONTINUOUS_UPDATE_NOTIFY: MessageId = 0xB; +// Notification message ids: +pub const SENSOR_UPDATE: MessageId = 0x1; #[allow(dead_code)] pub const SENSOR_UNIT_NONE: u8 = 0; @@ -550,6 +564,8 @@ pub enum ScmiDeviceError { NotEnabled, #[error("Unsupported request")] UnsupportedRequest, + #[error("Unsupported notify")] + UnsupportedNotify, } /// The highest representation of an SCMI device. @@ -575,6 +591,26 @@ pub trait ScmiDevice: Send { message_id: MessageId, parameters: &[MessageValue], ) -> Result; + + // Get device notify fd + fn get_notify_fd(&self) -> Option; + + // Set id for this ScmiDeivce + fn set_id(&mut self, id: usize); + + // Get id of this ScmiDevice + fn get_id(&self) -> usize; + + /// Get notify message from this device. + /// + /// Usually need to redefine it for different protocols. + /// device_index is the index of this device in its protocol list. + /// message_id is the notification message id to be used. + fn notify( + &mut self, + device_index: u32, + message_id: MessageId, + ) -> Result; } type DeviceList = Vec>; @@ -591,7 +627,8 @@ impl DeviceMap { // SENSOR_DESCRIPTION_GET supports -- the upper 16 bits of the response. const MAX_NUMBER_OF_PROTOCOL_DEVICES: usize = 0xFFFF; - fn insert(&mut self, device: Box) { + // Return device_index and protocol of this device in backend + fn insert(&mut self, mut device: Box) -> DeviceIdentify { let mut device_map = self.0.lock().unwrap(); let devices = device_map.entry(device.protocol()).or_default(); if devices.len() >= Self::MAX_NUMBER_OF_PROTOCOL_DEVICES { @@ -600,7 +637,10 @@ impl DeviceMap { device.protocol() ); } + device.set_id(devices.len()); + let device_identify = (device.protocol(), device.get_id()); devices.push(device); + device_identify } fn number_of_devices(&self, protocol_id: ProtocolId) -> u16 { @@ -629,9 +669,39 @@ impl DeviceMap { pub type DeviceResult = Result; +pub type DeviceIdentify = (ProtocolId, usize); + +/// EventfdMap is a structure used to construct the relationship between device_event and DeviceIdentify +/// Once a device supports notification, it should insert a key-value to this hashmap +/// "device_event" is automatically assigned according to "available device event", +/// then function handle_event can find the device via this hashmap. +struct EventfdMap { + // Next available device_event, it should be initialized with NOTIFY_ALLOW_START_FD + available_device_event: u16, + // Hashmap of device_event -> (ProtocolID, DeviceID) for device lookup + map: Arc>>, +} + +impl EventfdMap { + fn new() -> Self { + Self { + available_device_event: NOTIFY_ALLOW_START_FD, + map: Arc::new(Mutex::new(HashMap::new())), + } + } + + // If this device has eventfd for notification, insert it into map with a device_eventfd + fn insert(&mut self, device_identify: DeviceIdentify) { + let mut map = self.map.lock().unwrap(); + map.insert(self.available_device_event, device_identify); + self.available_device_event += 1; + } +} + pub struct ScmiHandler { handlers: HandlerMap, devices: DeviceMap, + event_fds: EventfdMap, } impl ScmiHandler { @@ -646,6 +716,7 @@ impl ScmiHandler { Self { handlers: HandlerMap::new(), devices: DeviceMap::new(), + event_fds: EventfdMap::new(), } } @@ -666,11 +737,71 @@ impl ScmiHandler { } _ => Response::from(ReturnStatus::NotSupported), }, + MessageType::Notification => Response::from(ReturnStatus::NotSupported), MessageType::Unsupported => Response::from(ReturnStatus::NotSupported), }; ScmiResponse::from(request.header, response) } + pub fn get_device_eventfd_list(&self) -> Vec<(RawFd, u16)> { + let mut list = vec![]; + for (eventfd, (protocol_id, device_index)) in self.event_fds.map.lock().unwrap().iter() { + if let Some(devices) = self.devices.0.lock().unwrap().get_mut(protocol_id) { + if let Some(device) = devices.get_mut(*device_index) { + list.push((device.get_notify_fd().unwrap(), *eventfd)); + } + } + } + list + } + + pub fn get_max_eventfd(&self) -> u16 { + self.event_fds.available_device_event - 1 + } + + /// According to device_event, find out the device which will do notification. + /// Then call its notify function to return a ScmiResponse. + /// Now only SENSOR PROTOCOL can do notification. + /// And it supports only the `SENSOR_UPDATE` message. + pub fn notify(&mut self, device_event: u16) -> Option { + let event_fds_locked = self.event_fds.map.lock().unwrap(); + let device_identify = event_fds_locked.get(&device_event).unwrap(); + let mut devices_locked = self.devices.0.lock().unwrap(); + let devices = devices_locked.get_mut(&device_identify.0).unwrap(); + let dev = devices.get_mut(device_identify.1).unwrap(); + + if dev.protocol() == SENSOR_PROTOCOL_ID { + let device_index: u32 = dev.get_id() as u32; + match dev.notify(device_index, SENSOR_UPDATE) { + Ok(message_values) => { + if !message_values.is_empty() { + // message_id=0x1 [7:0] + // message_type = 0x3 [9:8] + // protocol_id=0x15; [17:10] + // 0x1 | (0x3<<8) | (0x15<<10) + let notify_header: MessageHeader = (SENSOR_UPDATE as u32) + | ((MessageType::Notification as u32) << 8) + | ((SENSOR_PROTOCOL_ID as u32) << 10); + + Some(ScmiResponse::from( + notify_header, + Response::from_notification(&message_values), + )) + } else { + error!("No data from device!"); + None + } + } + Err(_) => { + error!("Fail to read from device!"); + None + } + } + } else { + None + } + } + pub fn number_of_parameters(&self, request: &ScmiRequest) -> Option { self.request_handler(request).map(|info| { info.parameters @@ -720,8 +851,13 @@ impl ScmiHandler { .expect("Impossibly large number of SCMI protocols") } + // If a device can notify, record its event_fd, which will be assigned to the device later. pub fn register_device(&mut self, device: Box) { - self.devices.insert(device); + let register_event = device.get_notify_fd().is_some(); + let device_identify = self.devices.insert(device); + if register_event { + self.event_fds.insert(device_identify); + } } fn handle_device( @@ -749,6 +885,10 @@ impl ScmiHandler { info!("Unsupported request for {}", device_index); Response::from(ReturnStatus::NotSupported) } + ScmiDeviceError::UnsupportedNotify => { + info!("Unsupported notify for {}", device_index); + Response::from(ReturnStatus::NotSupported) + } ScmiDeviceError::GenericError => { warn!("Device error in {}", device_index); Response::from(ReturnStatus::GenericError) @@ -1343,6 +1483,30 @@ mod tests { ); } + fn check_notify_enabled(sensor: u32, notify_enabled: bool, handler: &mut ScmiHandler) { + let notify_enabled_flag = u32::from(notify_enabled); + let parameters = vec![ + MessageValue::Unsigned(sensor), + MessageValue::Unsigned(notify_enabled_flag), + ]; + let result = vec![MessageValue::Unsigned(notify_enabled_flag)]; + test_message_with_handler( + SENSOR_PROTOCOL_ID, + SENSOR_CONTINUOUS_UPDATE_NOTIFY, + parameters, + ReturnStatus::Success, + result, + handler, + ); + } + + #[test] + fn test_sensor_continuous_update_notify() { + let mut handler = make_handler(); + check_notify_enabled(0, true, &mut handler); + check_notify_enabled(0, false, &mut handler) + } + #[test] fn test_sensor_reading_get() { let mut handler = make_handler(); @@ -1377,4 +1541,39 @@ mod tests { } } } + + #[test] + fn test_sensor_reading_update() { + let mut handler = ScmiHandler::new(); + for sensor_id in 0..2 { + let properties = + DeviceProperties::new(vec![("name".to_owned(), format!("fake{}", sensor_id))]); + let fake_sensor = FakeSensor::new_device(&properties).unwrap(); + handler.register_device(fake_sensor); + enable_sensor(sensor_id, true, &mut handler); + check_notify_enabled(sensor_id, true, &mut handler); + } + + for iteration in 0..2 { + for sensor_id in 0..2 { + let notification = handler.notify(NOTIFY_ALLOW_START_FD + sensor_id).unwrap(); + let notify_header: MessageHeader = (SENSOR_UPDATE as u32) + | ((MessageType::Notification as u32) << 8) + | ((SENSOR_PROTOCOL_ID as u32) << 10); + let mut result = vec![]; + result.push(MessageValue::Unsigned(0)); + result.push(MessageValue::Unsigned(sensor_id as u32)); + for i in 0..3 { + result.push(MessageValue::Signed(iteration + 100 * i)); + result.push(MessageValue::Signed(0)); + result.push(MessageValue::Signed(0)); + result.push(MessageValue::Signed(0)); + } + let response = + ScmiResponse::from(notify_header, Response::from_notification(&result)); + + assert_eq!(notification, response); + } + } + } } diff --git a/vhost-device-scmi/src/vhu_scmi.rs b/vhost-device-scmi/src/vhu_scmi.rs index 9c50948..a3669b7 100644 --- a/vhost-device-scmi/src/vhu_scmi.rs +++ b/vhost-device-scmi/src/vhu_scmi.rs @@ -9,8 +9,10 @@ use log::{debug, error, warn}; use std::io; use std::io::Result as IoResult; use std::mem::size_of; +use std::sync::{Arc, RwLock}; use thiserror::Error as ThisError; use vhost::vhost_user::message::{VhostUserProtocolFeatures, VhostUserVirtioFeatures}; +use vhost_user_backend::VringEpollHandler; 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::{ @@ -34,6 +36,9 @@ const NUM_QUEUES: usize = 2; const COMMAND_QUEUE: u16 = 0; const EVENT_QUEUE: u16 = 1; +// The first allow fd for notification device. +// [0...NUM_QUEUES] is already used by COMMAND_QUEUE, EVENT_QUEUE, exit +pub const NOTIFY_ALLOW_START_FD: u16 = 3; const VIRTIO_SCMI_F_P2A_CHANNELS: u16 = 0; @@ -57,6 +62,8 @@ pub enum VuScmiError { InsufficientDescriptorSize(usize, usize), #[error("Failed to send notification")] SendNotificationFailed, + #[error("Failed to get notification data from scmi device")] + GetNotificationFailed, #[error("Invalid descriptor count {0}")] UnexpectedDescriptorCount(usize), #[error("Invalid descriptor size, expected: {0}, found: {1}")] @@ -132,6 +139,20 @@ impl VuScmiBackend { }) } + /// Registers all the devices that can provide notifications + /// Create a hashmap to store the relationship about device's notify_fd and scmibackend eventfd. + pub fn register_device_event_fd( + &self, + handlers: Vec>>>>, + ) { + let eventfd_list = self.scmi_handler.get_device_eventfd_list(); + for (device_notify_fd, device_event) in eventfd_list { + handlers[0] + .register_listener(device_notify_fd, EventSet::IN, device_event as u64) + .unwrap(); + } + } + pub fn process_requests( &mut self, requests: Vec, @@ -326,6 +347,67 @@ impl VuScmiBackend { debug!("Processing event queue finished"); Ok(()) } + + fn notify_event_queue(&mut self, vring: &VringRwLock, device_event: u16) -> Result<()> { + if self.event_descriptors.is_empty() { + vring + .signal_used_queue() + .map_err(|_| VuScmiError::DescriptorNotFound)?; + return Ok(()); + } + + let desc_chain = &self.event_descriptors.pop().unwrap(); + let descriptors: Vec<_> = desc_chain.clone().collect(); + + if descriptors.len() != 1 { + return Err(VuScmiError::UnexpectedDescriptorCount(descriptors.len())); + } + let desc = descriptors[0]; + if !desc.is_write_only() { + return Err(VuScmiError::UnexpectedReadableDescriptor(0)); + } + debug!( + "SCMI event response avail descriptor length: {}", + desc.len() + ); + debug!("Calling SCMI notify"); + if let Some(mut response) = self.scmi_handler.notify(device_event) { + let write_desc_len: usize = desc.len() as usize; + if response.len() > write_desc_len { + error!( + "Response of length {} cannot fit into the descriptor size {}", + response.len(), + write_desc_len + ); + response = response.communication_error(); + if response.len() > write_desc_len { + return Err(VuScmiError::InsufficientDescriptorSize( + response.len(), + write_desc_len, + )); + } + } + desc_chain + .memory() + .write_slice(response.as_slice(), desc.addr()) + .map_err(|_| VuScmiError::DescriptorWriteFailed)?; + + if vring + .add_used(desc_chain.head_index(), response.len() as u32) + .is_err() + { + log::error!("Failed to send notification data to the ring"); + return Err(VuScmiError::SendNotificationFailed); + } + + let _ = vring + .signal_used_queue() + .map_err(|_| VuScmiError::SendNotificationFailed); + } else { + debug!("Get Notification Failed!"); + } + Ok(()) + } } /// VhostUserBackend trait methods @@ -382,6 +464,8 @@ impl VhostUserBackendMut for VuScmiBackend { return Err(VuScmiError::HandleEventNotEpollIn.into()); } + let max_device_event = self.scmi_handler.get_max_eventfd(); + match device_event { COMMAND_QUEUE => { let vring = &vrings[COMMAND_QUEUE as usize]; @@ -426,8 +510,15 @@ impl VhostUserBackendMut for VuScmiBackend { } _ => { - warn!("unhandled device_event: {}", device_event); - return Err(VuScmiError::HandleEventUnknownEvent.into()); + if device_event >= NOTIFY_ALLOW_START_FD && device_event <= max_device_event { + let vring = &vrings[EVENT_QUEUE as usize]; + vring.disable_notification().unwrap(); + self.notify_event_queue(vring, device_event)?; + vring.enable_notification().unwrap(); + } else { + warn!("unhandled device_event: {}", device_event); + return Err(VuScmiError::HandleEventUnknownEvent.into()); + } } } debug!("Handle event finished");