fix: Add support for Lelo Harmony style F1SV3s

Fixes #679
buttplugio · Dec 4, 2024 · c2ce686 · c2ce686
1 parent f192b8d
commit c2ce686
Show file tree

Hide file tree

Showing 3 changed files with 53 additions and 10 deletions.
diff --git a/buttplug/buttplug-device-config/build-config/buttplug-device-config-v3.json b/buttplug/buttplug-device-config/build-config/buttplug-device-config-v3.json
@@ -6773,7 +6773,9 @@
               "0000fff0-0000-1000-8000-00805f9b34fb": {
                 "tx": "0000fff1-0000-1000-8000-00805f9b34fb",
                 "whitelist": "00000a10-0000-1000-8000-00805f9b34fb",
-                "rx": "00000a04-0000-1000-8000-00805f9b34fb"
+                "rx": "00000a04-0000-1000-8000-00805f9b34fb",
+                "txvibrate": "0000fff2-0000-1000-8000-00805f9b34fb",
+                "generic0": "00000a11-0000-1000-8000-00805f9b34fb"
               }
             }
           }

diff --git a/buttplug/buttplug-device-config/device-config-v3/buttplug-device-config-v3.yml b/buttplug/buttplug-device-config/device-config-v3/buttplug-device-config-v3.yml
@@ -3863,6 +3863,8 @@ protocols:
               tx: 0000fff1-0000-1000-8000-00805f9b34fb
               whitelist: 00000a10-0000-1000-8000-00805f9b34fb
               rx: 00000a04-0000-1000-8000-00805f9b34fb
+              txvibrate: 0000fff2-0000-1000-8000-00805f9b34fb
+              generic0: 00000a11-0000-1000-8000-00805f9b34fb
   lelo-harmony:
     defaults:
       name: Lelo Tiani Harmony

diff --git a/buttplug/src/server/device/protocol/lelof1sv2.rs b/buttplug/src/server/device/protocol/lelof1sv2.rs
@@ -43,6 +43,10 @@ impl ProtocolInitializer for LeloF1sV2Initializer {
     hardware: Arc<Hardware>,
     _: &UserDeviceDefinition,
   ) -> Result<Arc<dyn ProtocolHandler>, ButtplugDeviceError> {
+
+    let use_harmony= !hardware.endpoints().contains(&Endpoint::Whitelist);
+    let sec_endpoint = if use_harmony { Endpoint::Generic0 } else { Endpoint::Whitelist };
+
     // The Lelo F1s V2 has a very specific pairing flow:
     // * First the device is turned on in BLE mode (long press)
     // * Then the security endpoint (Whitelist) needs to be read (which we can do via subscribe)
@@ -55,7 +59,7 @@ impl ProtocolInitializer for LeloF1sV2Initializer {
     // * If it returns 0x00,00,00,00,00,00,00,00 the connection is authorised
     let mut event_receiver = hardware.event_stream();
     hardware
-      .subscribe(&HardwareSubscribeCmd::new(Endpoint::Whitelist))
+      .subscribe(&HardwareSubscribeCmd::new(sec_endpoint))
       .await?;
     let noauth: Vec<u8> = vec![0; 8];
     let authed: Vec<u8> = vec![1, 0, 0, 0, 0, 0, 0, 0];
@@ -69,20 +73,20 @@ impl ProtocolInitializer for LeloF1sV2Initializer {
           )
         } else if n.eq(&authed) {
           debug!("Lelo F1s V2 is authorised!");
-          return Ok(Arc::new(LeloF1sV2::default()));
+          return Ok(Arc::new(LeloF1sV2::new(use_harmony)));
         } else {
           debug!("Lelo F1s V2 gave us a password: {:?}", n);
           // Can't send whilst subscribed
           hardware
-            .unsubscribe(&HardwareUnsubscribeCmd::new(Endpoint::Whitelist))
+            .unsubscribe(&HardwareUnsubscribeCmd::new(sec_endpoint))
             .await?;
           // Send with response
           hardware
-            .write_value(&HardwareWriteCmd::new(Endpoint::Whitelist, n, true))
+            .write_value(&HardwareWriteCmd::new(sec_endpoint, n, true))
             .await?;
           // Get back to the loop
           hardware
-            .subscribe(&HardwareSubscribeCmd::new(Endpoint::Whitelist))
+            .subscribe(&HardwareSubscribeCmd::new(sec_endpoint))
             .await?;
         }
       } else {
@@ -95,28 +99,63 @@ impl ProtocolInitializer for LeloF1sV2Initializer {
   }
 }
 
-#[derive(Default)]
-pub struct LeloF1sV2 {}
+pub struct LeloF1sV2 {
+  use_harmony: bool
+}
+
+impl LeloF1sV2 {
+  fn new(use_harmony: bool) -> Self {
+    Self { use_harmony }
+  }
+}
 
 impl ProtocolHandler for LeloF1sV2 {
   fn keepalive_strategy(&self) -> super::ProtocolKeepaliveStrategy {
     super::ProtocolKeepaliveStrategy::RepeatLastPacketStrategy
   }
 
   fn needs_full_command_set(&self) -> bool {
-    true
+    !self.use_harmony
   }
 
   fn handle_scalar_cmd(
     &self,
     cmds: &[Option<(ActuatorType, u32)>],
   ) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
+    if self.use_harmony
+    {
+      let mut cmd_vec: Vec<HardwareCommand> = vec![];
+      for (i, cmd) in cmds.iter().enumerate() {
+        if let Some(pair) = cmd {
+          cmd_vec.push(
+            HardwareWriteCmd::new(
+              Endpoint::TxVibrate,
+              vec![
+                0x0a,
+                0x12,
+                i as u8 + 1,
+                0x08,
+                0x00,
+                0x00,
+                0x00,
+                0x00,
+                pair.1 as u8,
+                0x00,
+              ],
+              false,
+            )
+                .into(),
+          );
+        }
+      }
+      return Ok(cmd_vec);
+    }
     let mut cmd_vec = vec![0x1];
     for cmd in cmds.iter() {
       cmd_vec.push(cmd.expect("LeloF1s should always send all values").1 as u8);
     }
     Ok(vec![
-      HardwareWriteCmd::new(Endpoint::Tx, cmd_vec, false).into()
+      HardwareWriteCmd::new(Endpoint::Tx, cmd_vec, true).into()
     ])
   }
 }