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 crate::device::{
9 hardware::{Hardware, HardwareCommand, HardwareEvent, HardwareSubscribeCmd, HardwareWriteCmd},
10 protocol::{
11 ProtocolHandler,
12 ProtocolIdentifier,
13 ProtocolInitializer,
14 generic_protocol_initializer_setup,
15 },
16};
17use async_trait::async_trait;
18use buttplug_core::{errors::ButtplugDeviceError, util::sleep};
19use buttplug_server_device_config::{
20 Endpoint,
21 ProtocolCommunicationSpecifier,
22 ServerDeviceDefinition,
23 UserDeviceIdentifier,
24};
25use futures::FutureExt;
26use std::{
27 sync::{
28 Arc,
29 atomic::{AtomicU8, Ordering},
30 },
31 time::Duration,
32};
33use tokio::select;
34use uuid::{Uuid, uuid};
35
36const FREDORCH_COMMAND_TIMEOUT_MS: u64 = 500;
37
38const CRC_HI: [u8; 256] = [
39 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0, 193, 129, 64, 1, 192, 128, 65, 0, 193, 129,
40 64, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0, 193, 129, 64, 0, 193, 129, 64, 1, 192,
41 128, 65, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0, 193, 129, 64, 1, 192, 128, 65, 0,
42 193, 129, 64, 0, 193, 129, 64, 1, 192, 128, 65, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128,
43 65, 0, 193, 129, 64, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0, 193, 129, 64, 1, 192,
44 128, 65, 0, 193, 129, 64, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0, 193, 129, 64, 0,
45 193, 129, 64, 1, 192, 128, 65, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0, 193, 129,
46 64, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0, 193, 129, 64, 1, 192, 128, 65, 0, 193,
47 129, 64, 0, 193, 129, 64, 1, 192, 128, 65, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0,
48 193, 129, 64, 1, 192, 128, 65, 0, 193, 129, 64, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128,
49 65, 0, 193, 129, 64, 0, 193, 129, 64, 1, 192, 128, 65, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192,
50 128, 65, 0, 193, 129, 64,
51];
52const CRC_LO: [u8; 256] = [
53 0, 192, 193, 1, 195, 3, 2, 194, 198, 6, 7, 199, 5, 197, 196, 4, 204, 12, 13, 205, 15, 207, 206,
54 14, 10, 202, 203, 11, 201, 9, 8, 200, 216, 24, 25, 217, 27, 219, 218, 26, 30, 222, 223, 31, 221,
55 29, 28, 220, 20, 212, 213, 21, 215, 23, 22, 214, 210, 18, 19, 211, 17, 209, 208, 16, 240, 48, 49,
56 241, 51, 243, 242, 50, 54, 246, 247, 55, 245, 53, 52, 244, 60, 252, 253, 61, 255, 63, 62, 254,
57 250, 58, 59, 251, 57, 249, 248, 56, 40, 232, 233, 41, 235, 43, 42, 234, 238, 46, 47, 239, 45,
58 237, 236, 44, 228, 36, 37, 229, 39, 231, 230, 38, 34, 226, 227, 35, 225, 33, 32, 224, 160, 96,
59 97, 161, 99, 163, 162, 98, 102, 166, 167, 103, 165, 101, 100, 164, 108, 172, 173, 109, 175, 111,
60 110, 174, 170, 106, 107, 171, 105, 169, 168, 104, 120, 184, 185, 121, 187, 123, 122, 186, 190,
61 126, 127, 191, 125, 189, 188, 124, 180, 116, 117, 181, 119, 183, 182, 118, 114, 178, 179, 115,
62 177, 113, 112, 176, 80, 144, 145, 81, 147, 83, 82, 146, 150, 86, 87, 151, 85, 149, 148, 84, 156,
63 92, 93, 157, 95, 159, 158, 94, 90, 154, 155, 91, 153, 89, 88, 152, 136, 72, 73, 137, 75, 139,
64 138, 74, 78, 142, 143, 79, 141, 77, 76, 140, 68, 132, 133, 69, 135, 71, 70, 134, 130, 66, 67,
65 131, 65, 129, 128, 64,
66];
67pub fn crc16(data: &[u8]) -> [u8; 2] {
68 let mut n: u8 = 255;
69 let mut o: u8 = 255;
70 for val in data {
71 let a: u8 = n ^ val;
72 n = o ^ CRC_HI[a as usize];
73 o = CRC_LO[a as usize]
74 }
75 [n, o]
76}
77
78const FREDORCH_PROTOCOL_UUID: Uuid = uuid!("f9a83f46-0af5-4766-84f0-a1cca6614115");
79
80generic_protocol_initializer_setup!(Fredorch, "fredorch");
81
82#[derive(Default)]
83pub struct FredorchInitializer {}
84
85#[async_trait]
86impl ProtocolInitializer for FredorchInitializer {
87 async fn initialize(
88 &mut self,
89 hardware: Arc<Hardware>,
90 _: &ServerDeviceDefinition,
91 ) -> Result<Arc<dyn ProtocolHandler>, ButtplugDeviceError> {
92 let mut event_receiver = hardware.event_stream();
93 hardware
94 .subscribe(&HardwareSubscribeCmd::new(
95 FREDORCH_PROTOCOL_UUID,
96 Endpoint::Rx,
97 ))
98 .await?;
99
100 let init: Vec<(String, Vec<u8>)> = vec![
101 (
102 "Set the device to program mode".to_owned(),
103 vec![0x01, 0x06, 0x00, 0x64, 0x00, 0x01],
104 ),
105 (
106 "Set the program mode to record".to_owned(),
107 vec![0x01, 0x06, 0x00, 0x69, 0x00, 0x00],
108 ),
109 (
110 "Program the device to move to position 0 at speed 5".to_owned(),
111 vec![
112 0x01, 0x10, 0x00, 0x6b, 0x00, 0x05, 0x0a, 0x00, 0x05, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00,
113 0x00, 0x01,
114 ],
115 ),
116 (
117 "Run the program".to_owned(),
118 vec![0x01, 0x06, 0x00, 0x69, 0x00, 0x01],
119 ),
120 (
121 "Set the program to repeat".to_owned(),
122 vec![0x01, 0x06, 0x00, 0x6a, 0x00, 0x01],
123 ),
124 ];
125
126 // expect 0, 1, 0, 1, 1 on connect
127 select! {
128 event = event_receiver.recv().fuse() => {
129 if let Ok(HardwareEvent::Notification(_, _, n)) = event {
130 debug!("Fredorch: wake up - received {:?}", n);
131 } else {
132 return Err(
133 ButtplugDeviceError::ProtocolSpecificError(
134 "Fredorch".to_owned(),
135 "Fredorch Device disconnected while initialising.".to_owned(),
136 )
137 );
138 }
139 }
140 _ = sleep(Duration::from_millis(FREDORCH_COMMAND_TIMEOUT_MS)).fuse() => {
141 // Or not?
142 }
143 }
144
145 for mut data in init {
146 let crc = crc16(&data.1);
147 data.1.push(crc[0]);
148 data.1.push(crc[1]);
149 debug!("Fredorch: {} - sent {:?}", data.0, data.1);
150 hardware
151 .write_value(&HardwareWriteCmd::new(
152 &[FREDORCH_PROTOCOL_UUID],
153 Endpoint::Tx,
154 data.1.clone(),
155 false,
156 ))
157 .await?;
158
159 select! {
160 event = event_receiver.recv().fuse() => {
161 if let Ok(HardwareEvent::Notification(_, _, n)) = event {
162 debug!("Fredorch: {} - received {:?}", data.0, n);
163 } else {
164 return Err(
165 ButtplugDeviceError::ProtocolSpecificError(
166 "Fredorch".to_owned(),
167 "Fredorch Device disconnected while initialising.".to_owned(),
168 )
169 );
170 }
171 }
172 _ = sleep(Duration::from_millis(FREDORCH_COMMAND_TIMEOUT_MS)).fuse() => {
173 return Err(
174 ButtplugDeviceError::ProtocolSpecificError(
175 "Fredorch".to_owned(),
176 "Fredorch Device timed out while initialising.".to_owned(),
177 )
178 );
179 }
180 }
181 }
182
183 Ok(Arc::new(Fredorch::default()))
184 }
185}
186
187const SPEED_MATRIX: [[u32; 20]; 15] = [
188 // distance, speed 1-20
189 /* 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 */
190 /* 1*/
191 [
192 1000, 800, 400, 235, 200, 172, 155, 92, 60, 45, 38, 34, 32, 28, 27, 26, 25, 24, 23, 22,
193 ],
194 /* 2*/
195 [
196 1500, 1000, 800, 680, 600, 515, 425, 265, 165, 115, 80, 70, 50, 48, 45, 35, 34, 33, 32, 30,
197 ],
198 /* 3*/
199 [
200 2500, 2310, 1135, 925, 792, 695, 565, 380, 218, 155, 105, 82, 70, 68, 65, 60, 48, 45, 43, 40,
201 ],
202 /* 4*/
203 [
204 3000, 2800, 1500, 1155, 965, 810, 690, 465, 260, 195, 140, 110, 85, 75, 74, 73, 70, 65, 60, 55,
205 ],
206 /* 5*/
207 [
208 3400, 3232, 2305, 1380, 1200, 1165, 972, 565, 328, 235, 162, 132, 98, 78, 75, 74, 73, 72, 71,
209 70,
210 ],
211 /* 6*/
212 [
213 3500, 3350, 2500, 1640, 1250, 1210, 1010, 645, 385, 275, 175, 160, 115, 95, 91, 90, 85, 80, 77,
214 75,
215 ],
216 /* 7*/
217 [
218 3600, 3472, 2980, 2060, 1560, 1275, 1132, 738, 430, 310, 230, 170, 128, 122, 110, 108, 105,
219 103, 101, 100,
220 ],
221 /* 8*/
222 [
223 3800, 3500, 3055, 2105, 1740, 1370, 1290, 830, 490, 355, 235, 195, 150, 140, 135, 132, 130,
224 125, 120, 119,
225 ],
226 /* 9*/
227 [
228 3900, 3518, 3190, 2315, 2045, 1510, 1442, 1045, 552, 392, 280, 225, 172, 145, 140, 138, 135,
229 134, 132, 130,
230 ],
231 /*10*/
232 [
233 6000, 5755, 3240, 2530, 2135, 1605, 1500, 1200, 595, 425, 285, 245, 175, 170, 160, 155, 150,
234 145, 142, 140,
235 ],
236 /*11*/
237 [
238 6428, 5872, 3335, 2780, 2270, 1782, 1590, 1310, 648, 470, 315, 255, 182, 180, 175, 172, 170,
239 162, 160, 155,
240 ],
241 /*12*/
242 [
243 6730, 5950, 3490, 2995, 2395, 1890, 1650, 1350, 700, 500, 350, 290, 220, 190, 185, 180, 175,
244 170, 165, 160,
245 ],
246 /*13*/
247 [
248 6962, 6122, 3880, 3205, 2465, 1900, 1700, 1400, 835, 545, 375, 310, 228, 195, 190, 185, 182,
249 181, 180, 175,
250 ],
251 /*14*/
252 [
253 7945, 6365, 4130, 3470, 2505, 1910, 1755, 1510, 855, 580, 400, 330, 235, 210, 205, 200, 195,
254 190, 185, 180,
255 ],
256 /*15*/
257 [
258 8048, 7068, 4442, 3708, 2668, 1930, 1800, 1520, 878, 618, 428, 365, 260, 255, 250, 240, 230,
259 220, 210, 200,
260 ],
261];
262
263fn calculate_speed(distance: u32, duration: u32) -> u8 {
264 let distance = distance.clamp(0, 15);
265 if distance == 0 {
266 return 0;
267 }
268
269 let mut speed = 1;
270 while speed < 20 {
271 if SPEED_MATRIX[distance as usize - 1][speed as usize - 1] < duration {
272 return speed;
273 }
274 speed += 1;
275 }
276 speed
277}
278
279#[derive(Default)]
280pub struct Fredorch {
281 previous_position: Arc<AtomicU8>,
282}
283
284impl ProtocolHandler for Fredorch {
285 fn handle_position_with_duration_cmd(
286 &self,
287 _feature_index: u32,
288 _feature_id: Uuid,
289 position: u32,
290 duration: u32,
291 ) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
292 let previous_position = self.previous_position.load(Ordering::Relaxed);
293 let distance = (previous_position as i32 - position as i32).unsigned_abs();
294 // The Fredorch only has 15 positions, but scales them to 0-150
295 let pos = (position * 10) as u8;
296
297 let speed = calculate_speed(distance, duration);
298 let mut data: Vec<u8> = vec![
299 0x01, 0x10, 0x00, 0x6B, 0x00, 0x05, 0x0a, 0x00, speed, 0x00, speed, 0x00, pos, 0x00, pos,
300 0x00, 0x01,
301 ];
302 let crc = crc16(&data);
303 data.push(crc[0]);
304 data.push(crc[1]);
305 self
306 .previous_position
307 .store(position as u8, Ordering::Relaxed);
308 Ok(vec![
309 HardwareWriteCmd::new(&[FREDORCH_PROTOCOL_UUID], Endpoint::Tx, data, false).into(),
310 ])
311 }
312
313 // TODO: Something is off... I think we need to program in both directions independently
314 fn handle_output_oscillate_cmd(
315 &self,
316 _feature_index: u32,
317 _feature_id: Uuid,
318 speed: u32,
319 ) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
320 // If we ever get oscillate with range, these should be loaded from the last set range
321 let min_pos = if speed == 0 { 0 } else { 0 };
322 let max_pos = if speed == 0 { 0 } else { 15 };
323 let mut data: Vec<u8> = vec![
324 0x01,
325 0x10,
326 0x00,
327 0x6B,
328 0x00,
329 0x05,
330 0x0a,
331 0x00,
332 speed as u8,
333 0x00,
334 speed as u8,
335 0x00,
336 min_pos * 15,
337 0x00,
338 max_pos * 15,
339 0x00,
340 0x01,
341 ];
342 let crc = crc16(&data);
343 data.push(crc[0]);
344 data.push(crc[1]);
345 Ok(vec![
346 HardwareWriteCmd::new(&[FREDORCH_PROTOCOL_UUID], Endpoint::Tx, data, false).into(),
347 ])
348 }
349}