Support Lelo F1s V3 as Harmony protocol

crates/buttplug_server/src/device/protocol_impl/lelo_harmony.rs

@@ -7,32 +7,30 @@
 
 use crate::device::{
   hardware::{
-    Hardware,
-    HardwareCommand,
-    HardwareEvent,
-    HardwareSubscribeCmd,
-    HardwareUnsubscribeCmd,
+    Hardware, HardwareCommand, HardwareEvent, HardwareSubscribeCmd, HardwareUnsubscribeCmd,
     HardwareWriteCmd,
   },
   protocol::{
-    ProtocolHandler,
-    ProtocolIdentifier,
-    ProtocolInitializer,
-    generic_protocol_initializer_setup,
+    ProtocolHandler, ProtocolIdentifier, ProtocolInitializer, generic_protocol_initializer_setup,
   },
 };
 use async_trait::async_trait;
 use buttplug_core::errors::ButtplugDeviceError;
+use buttplug_core::util::async_manager;
 use buttplug_server_device_config::Endpoint;
 use buttplug_server_device_config::{
-  ProtocolCommunicationSpecifier,
-  ServerDeviceDefinition,
-  UserDeviceIdentifier,
+  ProtocolCommunicationSpecifier, ServerDeviceDefinition, UserDeviceIdentifier,
+};
+use std::{
+  collections::HashMap,
+  sync::{Arc, Mutex},
+  time::Duration,
 };
-use std::sync::Arc;
 use uuid::{Uuid, uuid};
 
 const LELO_HARMONY_PROTOCOL_UUID: Uuid = uuid!("220e180a-e6d5-4fd1-963e-43a6f990b717");
+const LELO_HARMONY_F1SV3_VARIANT: &str = "f1sv3";
+const LELO_F1SV3_DEFAULT_IDLE_STOP_TIMEOUT_MS: u32 = 800;
 generic_protocol_initializer_setup!(LeloHarmony, "lelo-harmony");
 
 #[derive(Default)]
@@ -43,7 +41,7 @@ impl ProtocolInitializer for LeloHarmonyInitializer {
   async fn initialize(
     &mut self,
     hardware: Arc<Hardware>,
-    _: &ServerDeviceDefinition,
+    def: &ServerDeviceDefinition,
   ) -> Result<Arc<dyn ProtocolHandler>, ButtplugDeviceError> {
     // The Lelo Harmony has a very specific pairing flow:
     // * First the device is turned on in BLE mode (long press)
@@ -72,6 +70,13 @@ impl ProtocolInitializer for LeloHarmonyInitializer {
           )
         } else if !n.is_empty() && n[0] == 1u8 && n[1..].iter().all(|b| *b == 0u8) {
           debug!("Lelo Harmony is authorised!");
+          if def
+            .protocol_variant()
+            .as_deref()
+            .is_some_and(|variant| variant == LELO_HARMONY_F1SV3_VARIANT)
+          {
+            return Ok(Arc::new(LeloHarmony::f1sv3_harmony(hardware.clone(), def)));
+          }
           return Ok(Arc::new(LeloHarmony::default()));
         } else {
           debug!("Lelo Harmony gave us a password: {:?}", n);
@@ -109,35 +114,181 @@ impl ProtocolInitializer for LeloHarmonyInitializer {
   }
 }
 
+pub struct LeloHarmony {
+  output_endpoint: Endpoint,
+  use_zero_pattern_for_stop: bool,
+  write_with_response: bool,
+  idle_stop_timeout: Option<Duration>,
+  hardware: Option<Arc<Hardware>>,
+  state: Arc<Mutex<HashMap<u32, MotorState>>>,
+}
+
 #[derive(Default)]
-pub struct LeloHarmony {}
+struct MotorState {
+  has_seen_nonzero: bool,
+  generation: u64,
+}
+
+impl Default for LeloHarmony {
+  fn default() -> Self {
+    Self::new(Endpoint::Tx, false, false, None, None)
+  }
+}
 
 impl LeloHarmony {
+  pub(super) fn f1sv3(hardware: Arc<Hardware>, def: &ServerDeviceDefinition) -> Self {
+    Self::new(
+      Endpoint::TxVibrate,
+      true,
+      true,
+      Self::idle_stop_timeout(def),
+      Some(hardware),
+    )
+  }
+
+  fn f1sv3_harmony(hardware: Arc<Hardware>, def: &ServerDeviceDefinition) -> Self {
+    Self::new(
+      Endpoint::Tx,
+      true,
+      true,
+      Self::idle_stop_timeout(def),
+      Some(hardware),
+    )
+  }
+
+  fn idle_stop_timeout(def: &ServerDeviceDefinition) -> Option<Duration> {
+    def.vibrate_smoothing_enabled().then(|| {
+      Duration::from_millis(
+        def
+          .vibrate_smoothing_idle_stop_ms()
+          .unwrap_or(LELO_F1SV3_DEFAULT_IDLE_STOP_TIMEOUT_MS) as u64,
+      )
+    })
+  }
+
+  fn new(
+    output_endpoint: Endpoint,
+    use_zero_pattern_for_stop: bool,
+    write_with_response: bool,
+    idle_stop_timeout: Option<Duration>,
+    hardware: Option<Arc<Hardware>>,
+  ) -> Self {
+    Self {
+      output_endpoint,
+      use_zero_pattern_for_stop,
+      write_with_response,
+      idle_stop_timeout,
+      hardware,
+      state: Arc::new(Mutex::new(HashMap::new())),
+    }
+  }
+
+  fn command_for_speed(
+    &self,
+    feature_id: Uuid,
+    feature_index: u32,
+    speed: u32,
+  ) -> HardwareWriteCmd {
+    let pattern = if self.use_zero_pattern_for_stop && speed == 0 {
+      0x00
+    } else {
+      0x08
+    };
+    HardwareWriteCmd::new(
+      &[feature_id],
+      self.output_endpoint,
+      vec![
+        0x0a,
+        0x12,
+        feature_index as u8 + 1,
+        pattern,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        speed as u8,
+        0x00,
+      ],
+      self.write_with_response,
+    )
+  }
+
+  fn maybe_defer_stop(
+    &self,
+    feature_index: u32,
+    feature_id: Uuid,
+    speed: u32,
+  ) -> Result<bool, ButtplugDeviceError> {
+    if speed != 0 {
+      if self.idle_stop_timeout.is_some() {
+        let mut state = self.state.lock().map_err(|_| {
+          ButtplugDeviceError::ProtocolSpecificError(
+            "LeloHarmony".to_owned(),
+            "Lelo Harmony motor state lock failed".to_owned(),
+          )
+        })?;
+        let motor_state = state.entry(feature_index).or_default();
+        motor_state.has_seen_nonzero = true;
+        motor_state.generation += 1;
+      }
+      return Ok(false);
+    }
+
+    let Some(idle_stop_timeout) = self.idle_stop_timeout else {
+      return Ok(false);
+    };
+    let Some(hardware) = self.hardware.clone() else {
+      return Ok(false);
+    };
+
+    let mut state = self.state.lock().map_err(|_| {
+      ButtplugDeviceError::ProtocolSpecificError(
+        "LeloHarmony".to_owned(),
+        "Lelo Harmony motor state lock failed".to_owned(),
+      )
+    })?;
+    let motor_state = state.entry(feature_index).or_default();
+    if !motor_state.has_seen_nonzero {
+      return Ok(true);
+    }
+
+    motor_state.generation += 1;
+    let generation = motor_state.generation;
+    let stop_cmd = self.command_for_speed(feature_id, feature_index, 0);
+
+    let state = self.state.clone();
+    buttplug_core::spawn!("LeloHarmonyF1sV3DelayedStop", async move {
+      async_manager::sleep(idle_stop_timeout).await;
+      let should_stop = state
+        .lock()
+        .ok()
+        .and_then(|state| {
+          state
+            .get(&feature_index)
+            .map(|state| state.generation == generation)
+        })
+        .unwrap_or(false);
+      if should_stop {
+        let _ = hardware.write_value(&stop_cmd).await;
+      }
+    });
+
+    Ok(true)
+  }
+
   fn handle_input_cmd(
     &self,
     feature_index: u32,
     feature_id: Uuid,
     speed: u32,
   ) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
+    if self.maybe_defer_stop(feature_index, feature_id, speed)? {
+      return Ok(vec![]);
+    }
     Ok(vec![
-      HardwareWriteCmd::new(
-        &[feature_id],
-        Endpoint::Tx,
-        vec![
-          0x0a,
-          0x12,
-          feature_index as u8 + 1,
-          0x08,
-          0x00,
-          0x00,
-          0x00,
-          0x00,
-          speed as u8,
-          0x00,
-        ],
-        false,
-      )
-      .into(),
+      self
+        .command_for_speed(feature_id, feature_index, speed)
+        .into(),
     ])
   }
 }
@@ -161,3 +312,50 @@ impl ProtocolHandler for LeloHarmony {
     self.handle_input_cmd(feature_index, feature_id, speed)
   }
 }
+
+#[cfg(test)]
+mod test {
+  use super::*;
+
+  #[test]
+  fn uses_configured_output_endpoint_for_vibration() {
+    let handler = LeloHarmony::new(Endpoint::TxVibrate, false, false, None, None);
+    let commands = handler
+      .handle_output_vibrate_cmd(1, uuid!("00000000-0000-0000-0000-000000000001"), 50)
+      .expect("Command should build");
+
+    assert_eq!(commands.len(), 1);
+    match &commands[0] {
+      HardwareCommand::Write(cmd) => {
+        assert_eq!(cmd.endpoint(), Endpoint::TxVibrate);
+        assert_eq!(
+          cmd.data(),
+          &[0x0a, 0x12, 0x02, 0x08, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00]
+        );
+        assert!(!cmd.write_with_response());
+      }
+      _ => panic!("Expected write command"),
+    }
+  }
+
+  #[test]
+  fn can_use_zero_pattern_for_stop() {
+    let handler = LeloHarmony::new(Endpoint::TxVibrate, true, true, None, None);
+    let commands = handler
+      .handle_output_vibrate_cmd(0, uuid!("00000000-0000-0000-0000-000000000001"), 0)
+      .expect("Command should build");
+
+    assert_eq!(commands.len(), 1);
+    match &commands[0] {
+      HardwareCommand::Write(cmd) => {
+        assert_eq!(cmd.endpoint(), Endpoint::TxVibrate);
+        assert_eq!(
+          cmd.data(),
+          &[0x0a, 0x12, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]
+        );
+        assert!(cmd.write_with_response());
+      }
+      _ => panic!("Expected write command"),
+    }
+  }
+}

crates/buttplug_server/src/device/protocol_impl/lelof1sv2.rs

@@ -8,26 +8,17 @@
 use super::{lelo_harmony::LeloHarmony, lelof1s::LeloF1s};
 use crate::device::{
   hardware::{
-    Hardware,
-    HardwareEvent,
-    HardwareSubscribeCmd,
-    HardwareUnsubscribeCmd,
-    HardwareWriteCmd,
+    Hardware, HardwareEvent, HardwareSubscribeCmd, HardwareUnsubscribeCmd, HardwareWriteCmd,
   },
   protocol::{
-    ProtocolHandler,
-    ProtocolIdentifier,
-    ProtocolInitializer,
-    generic_protocol_initializer_setup,
+    ProtocolHandler, ProtocolIdentifier, ProtocolInitializer, generic_protocol_initializer_setup,
   },
 };
 use async_trait::async_trait;
 use buttplug_core::errors::ButtplugDeviceError;
 use buttplug_server_device_config::Endpoint;
 use buttplug_server_device_config::{
-  ProtocolCommunicationSpecifier,
-  ServerDeviceDefinition,
-  UserDeviceIdentifier,
+  ProtocolCommunicationSpecifier, ServerDeviceDefinition, UserDeviceIdentifier,
 };
 use std::sync::Arc;
 use uuid::{Uuid, uuid};
@@ -43,7 +34,7 @@ impl ProtocolInitializer for LeloF1sV2Initializer {
   async fn initialize(
     &mut self,
     hardware: Arc<Hardware>,
-    _: &ServerDeviceDefinition,
+    def: &ServerDeviceDefinition,
   ) -> Result<Arc<dyn ProtocolHandler>, ButtplugDeviceError> {
     let use_harmony = !hardware.endpoints().contains(&Endpoint::Whitelist);
     let sec_endpoint = if use_harmony {
@@ -82,7 +73,7 @@ impl ProtocolInitializer for LeloF1sV2Initializer {
         } else if n.eq(&authed) {
           debug!("Lelo F1s V2 is authorised!");
           if use_harmony {
-            return Ok(Arc::new(LeloHarmony::default()));
+            return Ok(Arc::new(LeloHarmony::f1sv3(hardware.clone(), def)));
           } else {
             return Ok(Arc::new(LeloF1s::new(true)));
           }