scmi: add eventqueue notification

eventqueue notification for sensor device is not implemented before.
It adds a initial version to implement this feature.

Now input event can be captured and dealt.
At the beginning, we open all scmidevices which can do notification.
For example:
open /dev/iio:deviceX during VuScmiBackend initializtion.

And in function register_device, we create "device_identify" for each devices.
If this device can do notificaiton,
insert this identify into ScmiHandler event_fds hashmap.
After that, register all those devices with an increasing device_event.

For this reason, we need to change SensorT trait.
And add Option<File> which is used for those notifiable Sensor in trait SensorT.
For instance, iio devices support notification if "/dev/iio:devixeX" exist,
which should implement them.

Signed-off-by: Junnan Wu <junnan01.wu@samsung.com>

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<File>,
+
+    /// 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<RawFd> {
+        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<RawFd> {
+        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)]

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<RawFd> {
+        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 {

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/<channel>_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<ChanScanType> {
+        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/<channel>_type"
+    scan_type: Option<ChanScanType>,
+}
+
+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<RawFd> {
+        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<F: FromStr>(path: &Path) -> Result<Option<F>, 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<Axis>) {
@@ -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,
+            }
+        );
+    }
 }

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}"))?;

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<MessageValue>;
 #[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<u8>,
@@ -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<MessageValues, ScmiDeviceError>;
+
+    // Get device notify fd
+    fn get_notify_fd(&self) -> Option<RawFd>;
+
+    // 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<MessageValues, ScmiDeviceError>;
 }
 
 type DeviceList = Vec<Box<dyn ScmiDevice>>;
@@ -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<dyn ScmiDevice>) {
+    // Return device_index and protocol of this device in backend
+    fn insert(&mut self, mut device: Box<dyn ScmiDevice>) -> 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<MessageValues, ScmiDeviceError>;
 
+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<Mutex<HashMap<u16, DeviceIdentify>>>,
+}
+
+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<ScmiResponse> {
+        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<NParameters> {
         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<dyn ScmiDevice>) {
-        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);
+            }
+        }
+    }
 }

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<Arc<VringEpollHandler<Arc<RwLock<VuScmiBackend>>>>>,
+    ) {
+        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<ScmiDescriptorChain>,
@@ -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");