Buttplug sex toy control library
buttplug.io
1// Buttplug Rust Source Code File - See https://buttplug.io for more info.
2//
3// Copyright 2016-2024 Nonpolynomial Labs LLC. All rights reserved.
4//
5// Licensed under the BSD 3-Clause license. See LICENSE file in the project root
6// for full license information.
7
8use buttplug_core::{errors::ButtplugDeviceError, message::OutputType};
9use buttplug_server_device_config::{
10 Endpoint,
11 ProtocolCommunicationSpecifier,
12 ServerDeviceDefinition,
13 UserDeviceIdentifier,
14};
15
16use crate::device::{
17 hardware::{Hardware, HardwareCommand, HardwareReadCmd, HardwareWriteCmd},
18 protocol::{
19 ProtocolHandler,
20 ProtocolIdentifier,
21 ProtocolInitializer,
22 generic_protocol_initializer_setup,
23 },
24};
25use async_trait::async_trait;
26use std::{
27 collections::HashMap,
28 sync::{
29 Arc,
30 atomic::{AtomicU8, Ordering},
31 },
32};
33use uuid::{Uuid, uuid};
34
35generic_protocol_initializer_setup!(SenseeV2, "sensee-v2");
36
37const SENSEE_V2_PROTOCOL_UUID: Uuid = uuid!("6e68d015-6e83-484b-9dbc-de7684cf8c29");
38
39#[derive(Default)]
40pub struct SenseeV2Initializer {}
41
42#[async_trait]
43impl ProtocolInitializer for SenseeV2Initializer {
44 async fn initialize(
45 &mut self,
46 hardware: Arc<Hardware>,
47 device_definition: &ServerDeviceDefinition,
48 ) -> Result<Arc<dyn ProtocolHandler>, ButtplugDeviceError> {
49 let res = hardware
50 .read_value(&HardwareReadCmd::new(
51 SENSEE_V2_PROTOCOL_UUID,
52 Endpoint::Tx,
53 128,
54 500,
55 ))
56 .await?;
57 info!("Sensee model data: {:X?}", res.data());
58
59 let device_type = if res.data().len() >= 6 && res.data()[6] != 0x00 {
60 res.data()[6]
61 } else {
62 0x65
63 };
64
65 let feature_map = |output_type| {
66 let mut map = HashMap::new();
67 device_definition
68 .features()
69 .iter()
70 .enumerate()
71 .for_each(|(i, x)| {
72 if let Some(output_map) = x.output()
73 && output_map.contains(output_type)
74 {
75 map.insert(i as u32, AtomicU8::new(0));
76 }
77 });
78 map
79 };
80
81 let vibe_map = feature_map(OutputType::Vibrate);
82 let thrust_map = feature_map(OutputType::Oscillate);
83 let suck_map = feature_map(OutputType::Constrict);
84
85 Ok(Arc::new(SenseeV2::new(
86 device_type,
87 vibe_map,
88 thrust_map,
89 suck_map,
90 )))
91 }
92}
93
94pub struct SenseeV2 {
95 device_type: u8,
96 vibe_map: HashMap<u32, AtomicU8>,
97 thrust_map: HashMap<u32, AtomicU8>,
98 suck_map: HashMap<u32, AtomicU8>,
99}
100
101fn make_cmd(dtype: u8, func: u8, cmd: Vec<u8>) -> Vec<u8> {
102 let mut out = vec![0x55, 0xAA, 0xF0]; // fixed start code
103 out.push(0x02); // version
104 out.push(0x00); // package numer?
105 out.push(0x04 + cmd.len() as u8); // Data length
106 out.push(dtype); // Device type - always 0x66?
107 out.push(func); // Function code
108 out.extend(cmd);
109
110 let cdc = vec![0, 0];
111 // ToDo: CDC not yet used
112 out.extend(cdc);
113
114 out
115}
116
117impl SenseeV2 {
118 fn new(
119 device_type: u8,
120 vibe_map: HashMap<u32, AtomicU8>,
121 thrust_map: HashMap<u32, AtomicU8>,
122 suck_map: HashMap<u32, AtomicU8>,
123 ) -> Self {
124 Self {
125 device_type,
126 vibe_map,
127 thrust_map,
128 suck_map,
129 }
130 }
131
132 fn compile_command(&self) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
133 let mut data = vec![];
134 data.push(
135 if !self.vibe_map.is_empty() { 1 } else { 0 }
136 + if !self.thrust_map.is_empty() { 1 } else { 0 }
137 + if !self.suck_map.is_empty() { 1 } else { 0 } as u8,
138 );
139 let mut data_add = |i, m: &HashMap<u32, AtomicU8>| {
140 if !m.is_empty() {
141 data.push(i);
142 data.push(m.len() as u8);
143 for (i, (_, v)) in m.iter().enumerate() {
144 data.push((i + 1) as u8);
145 data.push(v.load(Ordering::Relaxed));
146 }
147 }
148 };
149 data_add(0, &self.vibe_map);
150 data_add(1, &self.thrust_map);
151 data_add(2, &self.suck_map);
152
153 Ok(vec![
154 HardwareWriteCmd::new(
155 &[SENSEE_V2_PROTOCOL_UUID],
156 Endpoint::Tx,
157 make_cmd(self.device_type, 0xf1, data),
158 false,
159 )
160 .into(),
161 ])
162 }
163}
164
165impl ProtocolHandler for SenseeV2 {
166 fn handle_output_vibrate_cmd(
167 &self,
168 feature_index: u32,
169 _feature_id: Uuid,
170 speed: u32,
171 ) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
172 self
173 .vibe_map
174 .get(&feature_index)
175 .unwrap()
176 .store(speed as u8, Ordering::Relaxed);
177 self.compile_command()
178 }
179
180 fn handle_output_oscillate_cmd(
181 &self,
182 feature_index: u32,
183 _feature_id: Uuid,
184 speed: u32,
185 ) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
186 self
187 .thrust_map
188 .get(&feature_index)
189 .unwrap()
190 .store(speed as u8, Ordering::Relaxed);
191 self.compile_command()
192 }
193
194 fn handle_output_constrict_cmd(
195 &self,
196 feature_index: u32,
197 _feature_id: Uuid,
198 level: u32,
199 ) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
200 self
201 .suck_map
202 .get(&feature_index)
203 .unwrap()
204 .store(level as u8, Ordering::Relaxed);
205 self.compile_command()
206 }
207}