1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Copyright (c) 2000-2002 Vojtech Pavlik <vojtech@ucw.cz> |
4 | * Copyright (c) 2001-2002, 2007 Johann Deneux <johann.deneux@gmail.com> |
5 | * |
6 | * USB/RS232 I-Force joysticks and wheels. |
7 | */ |
8 | |
9 | #include "iforce.h" |
10 | |
11 | /* |
12 | * Set the magnitude of a constant force effect |
13 | * Return error code |
14 | * |
15 | * Note: caller must ensure exclusive access to device |
16 | */ |
17 | |
18 | static int make_magnitude_modifier(struct iforce* iforce, |
19 | struct resource* mod_chunk, int no_alloc, __s16 level) |
20 | { |
21 | unsigned char data[3]; |
22 | |
23 | if (!no_alloc) { |
24 | mutex_lock(&iforce->mem_mutex); |
25 | if (allocate_resource(root: &(iforce->device_memory), new: mod_chunk, size: 2, |
26 | min: iforce->device_memory.start, max: iforce->device_memory.end, align: 2L, |
27 | NULL, NULL)) { |
28 | mutex_unlock(lock: &iforce->mem_mutex); |
29 | return -ENOSPC; |
30 | } |
31 | mutex_unlock(lock: &iforce->mem_mutex); |
32 | } |
33 | |
34 | data[0] = LO(mod_chunk->start); |
35 | data[1] = HI(mod_chunk->start); |
36 | data[2] = HIFIX80(level); |
37 | |
38 | iforce_send_packet(iforce, FF_CMD_MAGNITUDE, data); |
39 | |
40 | iforce_dump_packet(iforce, msg: "magnitude" , FF_CMD_MAGNITUDE, data); |
41 | return 0; |
42 | } |
43 | |
44 | /* |
45 | * Upload the component of an effect dealing with the period, phase and magnitude |
46 | */ |
47 | |
48 | static int make_period_modifier(struct iforce* iforce, |
49 | struct resource* mod_chunk, int no_alloc, |
50 | __s16 magnitude, __s16 offset, u16 period, u16 phase) |
51 | { |
52 | unsigned char data[7]; |
53 | |
54 | period = TIME_SCALE(period); |
55 | |
56 | if (!no_alloc) { |
57 | mutex_lock(&iforce->mem_mutex); |
58 | if (allocate_resource(root: &(iforce->device_memory), new: mod_chunk, size: 0x0c, |
59 | min: iforce->device_memory.start, max: iforce->device_memory.end, align: 2L, |
60 | NULL, NULL)) { |
61 | mutex_unlock(lock: &iforce->mem_mutex); |
62 | return -ENOSPC; |
63 | } |
64 | mutex_unlock(lock: &iforce->mem_mutex); |
65 | } |
66 | |
67 | data[0] = LO(mod_chunk->start); |
68 | data[1] = HI(mod_chunk->start); |
69 | |
70 | data[2] = HIFIX80(magnitude); |
71 | data[3] = HIFIX80(offset); |
72 | data[4] = HI(phase); |
73 | |
74 | data[5] = LO(period); |
75 | data[6] = HI(period); |
76 | |
77 | iforce_send_packet(iforce, FF_CMD_PERIOD, data); |
78 | |
79 | return 0; |
80 | } |
81 | |
82 | /* |
83 | * Uploads the part of an effect setting the envelope of the force |
84 | */ |
85 | |
86 | static int make_envelope_modifier(struct iforce* iforce, |
87 | struct resource* mod_chunk, int no_alloc, |
88 | u16 attack_duration, __s16 initial_level, |
89 | u16 fade_duration, __s16 final_level) |
90 | { |
91 | unsigned char data[8]; |
92 | |
93 | attack_duration = TIME_SCALE(attack_duration); |
94 | fade_duration = TIME_SCALE(fade_duration); |
95 | |
96 | if (!no_alloc) { |
97 | mutex_lock(&iforce->mem_mutex); |
98 | if (allocate_resource(root: &(iforce->device_memory), new: mod_chunk, size: 0x0e, |
99 | min: iforce->device_memory.start, max: iforce->device_memory.end, align: 2L, |
100 | NULL, NULL)) { |
101 | mutex_unlock(lock: &iforce->mem_mutex); |
102 | return -ENOSPC; |
103 | } |
104 | mutex_unlock(lock: &iforce->mem_mutex); |
105 | } |
106 | |
107 | data[0] = LO(mod_chunk->start); |
108 | data[1] = HI(mod_chunk->start); |
109 | |
110 | data[2] = LO(attack_duration); |
111 | data[3] = HI(attack_duration); |
112 | data[4] = HI(initial_level); |
113 | |
114 | data[5] = LO(fade_duration); |
115 | data[6] = HI(fade_duration); |
116 | data[7] = HI(final_level); |
117 | |
118 | iforce_send_packet(iforce, FF_CMD_ENVELOPE, data); |
119 | |
120 | return 0; |
121 | } |
122 | |
123 | /* |
124 | * Component of spring, friction, inertia... effects |
125 | */ |
126 | |
127 | static int make_condition_modifier(struct iforce* iforce, |
128 | struct resource* mod_chunk, int no_alloc, |
129 | __u16 rsat, __u16 lsat, __s16 rk, __s16 lk, u16 db, __s16 center) |
130 | { |
131 | unsigned char data[10]; |
132 | |
133 | if (!no_alloc) { |
134 | mutex_lock(&iforce->mem_mutex); |
135 | if (allocate_resource(root: &(iforce->device_memory), new: mod_chunk, size: 8, |
136 | min: iforce->device_memory.start, max: iforce->device_memory.end, align: 2L, |
137 | NULL, NULL)) { |
138 | mutex_unlock(lock: &iforce->mem_mutex); |
139 | return -ENOSPC; |
140 | } |
141 | mutex_unlock(lock: &iforce->mem_mutex); |
142 | } |
143 | |
144 | data[0] = LO(mod_chunk->start); |
145 | data[1] = HI(mod_chunk->start); |
146 | |
147 | data[2] = (100 * rk) >> 15; /* Dangerous: the sign is extended by gcc on plateforms providing an arith shift */ |
148 | data[3] = (100 * lk) >> 15; /* This code is incorrect on cpus lacking arith shift */ |
149 | |
150 | center = (500 * center) >> 15; |
151 | data[4] = LO(center); |
152 | data[5] = HI(center); |
153 | |
154 | db = (1000 * db) >> 16; |
155 | data[6] = LO(db); |
156 | data[7] = HI(db); |
157 | |
158 | data[8] = (100 * rsat) >> 16; |
159 | data[9] = (100 * lsat) >> 16; |
160 | |
161 | iforce_send_packet(iforce, FF_CMD_CONDITION, data); |
162 | iforce_dump_packet(iforce, msg: "condition" , FF_CMD_CONDITION, data); |
163 | |
164 | return 0; |
165 | } |
166 | |
167 | static unsigned char find_button(struct iforce *iforce, signed short button) |
168 | { |
169 | int i; |
170 | |
171 | for (i = 1; iforce->type->btn[i] >= 0; i++) |
172 | if (iforce->type->btn[i] == button) |
173 | return i + 1; |
174 | return 0; |
175 | } |
176 | |
177 | /* |
178 | * Analyse the changes in an effect, and tell if we need to send an condition |
179 | * parameter packet |
180 | */ |
181 | static int need_condition_modifier(struct iforce *iforce, |
182 | struct ff_effect *old, |
183 | struct ff_effect *new) |
184 | { |
185 | int ret = 0; |
186 | int i; |
187 | |
188 | if (new->type != FF_SPRING && new->type != FF_FRICTION) { |
189 | dev_warn(&iforce->dev->dev, "bad effect type in %s\n" , |
190 | __func__); |
191 | return 0; |
192 | } |
193 | |
194 | for (i = 0; i < 2; i++) { |
195 | ret |= old->u.condition[i].right_saturation != new->u.condition[i].right_saturation |
196 | || old->u.condition[i].left_saturation != new->u.condition[i].left_saturation |
197 | || old->u.condition[i].right_coeff != new->u.condition[i].right_coeff |
198 | || old->u.condition[i].left_coeff != new->u.condition[i].left_coeff |
199 | || old->u.condition[i].deadband != new->u.condition[i].deadband |
200 | || old->u.condition[i].center != new->u.condition[i].center; |
201 | } |
202 | return ret; |
203 | } |
204 | |
205 | /* |
206 | * Analyse the changes in an effect, and tell if we need to send a magnitude |
207 | * parameter packet |
208 | */ |
209 | static int need_magnitude_modifier(struct iforce *iforce, |
210 | struct ff_effect *old, |
211 | struct ff_effect *effect) |
212 | { |
213 | if (effect->type != FF_CONSTANT) { |
214 | dev_warn(&iforce->dev->dev, "bad effect type in %s\n" , |
215 | __func__); |
216 | return 0; |
217 | } |
218 | |
219 | return old->u.constant.level != effect->u.constant.level; |
220 | } |
221 | |
222 | /* |
223 | * Analyse the changes in an effect, and tell if we need to send an envelope |
224 | * parameter packet |
225 | */ |
226 | static int need_envelope_modifier(struct iforce *iforce, struct ff_effect *old, |
227 | struct ff_effect *effect) |
228 | { |
229 | switch (effect->type) { |
230 | case FF_CONSTANT: |
231 | if (old->u.constant.envelope.attack_length != effect->u.constant.envelope.attack_length |
232 | || old->u.constant.envelope.attack_level != effect->u.constant.envelope.attack_level |
233 | || old->u.constant.envelope.fade_length != effect->u.constant.envelope.fade_length |
234 | || old->u.constant.envelope.fade_level != effect->u.constant.envelope.fade_level) |
235 | return 1; |
236 | break; |
237 | |
238 | case FF_PERIODIC: |
239 | if (old->u.periodic.envelope.attack_length != effect->u.periodic.envelope.attack_length |
240 | || old->u.periodic.envelope.attack_level != effect->u.periodic.envelope.attack_level |
241 | || old->u.periodic.envelope.fade_length != effect->u.periodic.envelope.fade_length |
242 | || old->u.periodic.envelope.fade_level != effect->u.periodic.envelope.fade_level) |
243 | return 1; |
244 | break; |
245 | |
246 | default: |
247 | dev_warn(&iforce->dev->dev, "bad effect type in %s\n" , |
248 | __func__); |
249 | } |
250 | |
251 | return 0; |
252 | } |
253 | |
254 | /* |
255 | * Analyse the changes in an effect, and tell if we need to send a periodic |
256 | * parameter effect |
257 | */ |
258 | static int need_period_modifier(struct iforce *iforce, struct ff_effect *old, |
259 | struct ff_effect *new) |
260 | { |
261 | if (new->type != FF_PERIODIC) { |
262 | dev_warn(&iforce->dev->dev, "bad effect type in %s\n" , |
263 | __func__); |
264 | return 0; |
265 | } |
266 | return (old->u.periodic.period != new->u.periodic.period |
267 | || old->u.periodic.magnitude != new->u.periodic.magnitude |
268 | || old->u.periodic.offset != new->u.periodic.offset |
269 | || old->u.periodic.phase != new->u.periodic.phase); |
270 | } |
271 | |
272 | /* |
273 | * Analyse the changes in an effect, and tell if we need to send an effect |
274 | * packet |
275 | */ |
276 | static int need_core(struct ff_effect *old, struct ff_effect *new) |
277 | { |
278 | if (old->direction != new->direction |
279 | || old->trigger.button != new->trigger.button |
280 | || old->trigger.interval != new->trigger.interval |
281 | || old->replay.length != new->replay.length |
282 | || old->replay.delay != new->replay.delay) |
283 | return 1; |
284 | |
285 | return 0; |
286 | } |
287 | /* |
288 | * Send the part common to all effects to the device |
289 | */ |
290 | static int make_core(struct iforce* iforce, u16 id, u16 mod_id1, u16 mod_id2, |
291 | u8 effect_type, u8 axes, u16 duration, u16 delay, u16 button, |
292 | u16 interval, u16 direction) |
293 | { |
294 | unsigned char data[14]; |
295 | |
296 | duration = TIME_SCALE(duration); |
297 | delay = TIME_SCALE(delay); |
298 | interval = TIME_SCALE(interval); |
299 | |
300 | data[0] = LO(id); |
301 | data[1] = effect_type; |
302 | data[2] = LO(axes) | find_button(iforce, button); |
303 | |
304 | data[3] = LO(duration); |
305 | data[4] = HI(duration); |
306 | |
307 | data[5] = HI(direction); |
308 | |
309 | data[6] = LO(interval); |
310 | data[7] = HI(interval); |
311 | |
312 | data[8] = LO(mod_id1); |
313 | data[9] = HI(mod_id1); |
314 | data[10] = LO(mod_id2); |
315 | data[11] = HI(mod_id2); |
316 | |
317 | data[12] = LO(delay); |
318 | data[13] = HI(delay); |
319 | |
320 | /* Stop effect */ |
321 | /* iforce_control_playback(iforce, id, 0);*/ |
322 | |
323 | iforce_send_packet(iforce, FF_CMD_EFFECT, data); |
324 | |
325 | /* If needed, restart effect */ |
326 | if (test_bit(FF_CORE_SHOULD_PLAY, iforce->core_effects[id].flags)) { |
327 | /* BUG: perhaps we should replay n times, instead of 1. But we do not know n */ |
328 | iforce_control_playback(iforce, id, 1); |
329 | } |
330 | |
331 | return 0; |
332 | } |
333 | |
334 | /* |
335 | * Upload a periodic effect to the device |
336 | * See also iforce_upload_constant. |
337 | */ |
338 | int iforce_upload_periodic(struct iforce *iforce, struct ff_effect *effect, struct ff_effect *old) |
339 | { |
340 | u8 wave_code; |
341 | int core_id = effect->id; |
342 | struct iforce_core_effect* core_effect = iforce->core_effects + core_id; |
343 | struct resource* mod1_chunk = &(iforce->core_effects[core_id].mod1_chunk); |
344 | struct resource* mod2_chunk = &(iforce->core_effects[core_id].mod2_chunk); |
345 | int param1_err = 1; |
346 | int param2_err = 1; |
347 | int core_err = 0; |
348 | |
349 | if (!old || need_period_modifier(iforce, old, new: effect)) { |
350 | param1_err = make_period_modifier(iforce, mod_chunk: mod1_chunk, |
351 | no_alloc: old != NULL, |
352 | magnitude: effect->u.periodic.magnitude, offset: effect->u.periodic.offset, |
353 | period: effect->u.periodic.period, phase: effect->u.periodic.phase); |
354 | if (param1_err) |
355 | return param1_err; |
356 | set_bit(FF_MOD1_IS_USED, addr: core_effect->flags); |
357 | } |
358 | |
359 | if (!old || need_envelope_modifier(iforce, old, effect)) { |
360 | param2_err = make_envelope_modifier(iforce, mod_chunk: mod2_chunk, |
361 | no_alloc: old !=NULL, |
362 | attack_duration: effect->u.periodic.envelope.attack_length, |
363 | initial_level: effect->u.periodic.envelope.attack_level, |
364 | fade_duration: effect->u.periodic.envelope.fade_length, |
365 | final_level: effect->u.periodic.envelope.fade_level); |
366 | if (param2_err) |
367 | return param2_err; |
368 | set_bit(FF_MOD2_IS_USED, addr: core_effect->flags); |
369 | } |
370 | |
371 | switch (effect->u.periodic.waveform) { |
372 | case FF_SQUARE: wave_code = 0x20; break; |
373 | case FF_TRIANGLE: wave_code = 0x21; break; |
374 | case FF_SINE: wave_code = 0x22; break; |
375 | case FF_SAW_UP: wave_code = 0x23; break; |
376 | case FF_SAW_DOWN: wave_code = 0x24; break; |
377 | default: wave_code = 0x20; break; |
378 | } |
379 | |
380 | if (!old || need_core(old, new: effect)) { |
381 | core_err = make_core(iforce, id: effect->id, |
382 | mod_id1: mod1_chunk->start, |
383 | mod_id2: mod2_chunk->start, |
384 | effect_type: wave_code, |
385 | axes: 0x20, |
386 | duration: effect->replay.length, |
387 | delay: effect->replay.delay, |
388 | button: effect->trigger.button, |
389 | interval: effect->trigger.interval, |
390 | direction: effect->direction); |
391 | } |
392 | |
393 | /* If one of the parameter creation failed, we already returned an |
394 | * error code. |
395 | * If the core creation failed, we return its error code. |
396 | * Else: if one parameter at least was created, we return 0 |
397 | * else we return 1; |
398 | */ |
399 | return core_err < 0 ? core_err : (param1_err && param2_err); |
400 | } |
401 | |
402 | /* |
403 | * Upload a constant force effect |
404 | * Return value: |
405 | * <0 Error code |
406 | * 0 Ok, effect created or updated |
407 | * 1 effect did not change since last upload, and no packet was therefore sent |
408 | */ |
409 | int iforce_upload_constant(struct iforce *iforce, struct ff_effect *effect, struct ff_effect *old) |
410 | { |
411 | int core_id = effect->id; |
412 | struct iforce_core_effect* core_effect = iforce->core_effects + core_id; |
413 | struct resource* mod1_chunk = &(iforce->core_effects[core_id].mod1_chunk); |
414 | struct resource* mod2_chunk = &(iforce->core_effects[core_id].mod2_chunk); |
415 | int param1_err = 1; |
416 | int param2_err = 1; |
417 | int core_err = 0; |
418 | |
419 | if (!old || need_magnitude_modifier(iforce, old, effect)) { |
420 | param1_err = make_magnitude_modifier(iforce, mod_chunk: mod1_chunk, |
421 | no_alloc: old != NULL, |
422 | level: effect->u.constant.level); |
423 | if (param1_err) |
424 | return param1_err; |
425 | set_bit(FF_MOD1_IS_USED, addr: core_effect->flags); |
426 | } |
427 | |
428 | if (!old || need_envelope_modifier(iforce, old, effect)) { |
429 | param2_err = make_envelope_modifier(iforce, mod_chunk: mod2_chunk, |
430 | no_alloc: old != NULL, |
431 | attack_duration: effect->u.constant.envelope.attack_length, |
432 | initial_level: effect->u.constant.envelope.attack_level, |
433 | fade_duration: effect->u.constant.envelope.fade_length, |
434 | final_level: effect->u.constant.envelope.fade_level); |
435 | if (param2_err) |
436 | return param2_err; |
437 | set_bit(FF_MOD2_IS_USED, addr: core_effect->flags); |
438 | } |
439 | |
440 | if (!old || need_core(old, new: effect)) { |
441 | core_err = make_core(iforce, id: effect->id, |
442 | mod_id1: mod1_chunk->start, |
443 | mod_id2: mod2_chunk->start, |
444 | effect_type: 0x00, |
445 | axes: 0x20, |
446 | duration: effect->replay.length, |
447 | delay: effect->replay.delay, |
448 | button: effect->trigger.button, |
449 | interval: effect->trigger.interval, |
450 | direction: effect->direction); |
451 | } |
452 | |
453 | /* If one of the parameter creation failed, we already returned an |
454 | * error code. |
455 | * If the core creation failed, we return its error code. |
456 | * Else: if one parameter at least was created, we return 0 |
457 | * else we return 1; |
458 | */ |
459 | return core_err < 0 ? core_err : (param1_err && param2_err); |
460 | } |
461 | |
462 | /* |
463 | * Upload an condition effect. Those are for example friction, inertia, springs... |
464 | */ |
465 | int iforce_upload_condition(struct iforce *iforce, struct ff_effect *effect, struct ff_effect *old) |
466 | { |
467 | int core_id = effect->id; |
468 | struct iforce_core_effect* core_effect = iforce->core_effects + core_id; |
469 | struct resource* mod1_chunk = &(core_effect->mod1_chunk); |
470 | struct resource* mod2_chunk = &(core_effect->mod2_chunk); |
471 | u8 type; |
472 | int param_err = 1; |
473 | int core_err = 0; |
474 | |
475 | switch (effect->type) { |
476 | case FF_SPRING: type = 0x40; break; |
477 | case FF_DAMPER: type = 0x41; break; |
478 | default: return -1; |
479 | } |
480 | |
481 | if (!old || need_condition_modifier(iforce, old, new: effect)) { |
482 | param_err = make_condition_modifier(iforce, mod_chunk: mod1_chunk, |
483 | no_alloc: old != NULL, |
484 | rsat: effect->u.condition[0].right_saturation, |
485 | lsat: effect->u.condition[0].left_saturation, |
486 | rk: effect->u.condition[0].right_coeff, |
487 | lk: effect->u.condition[0].left_coeff, |
488 | db: effect->u.condition[0].deadband, |
489 | center: effect->u.condition[0].center); |
490 | if (param_err) |
491 | return param_err; |
492 | set_bit(FF_MOD1_IS_USED, addr: core_effect->flags); |
493 | |
494 | param_err = make_condition_modifier(iforce, mod_chunk: mod2_chunk, |
495 | no_alloc: old != NULL, |
496 | rsat: effect->u.condition[1].right_saturation, |
497 | lsat: effect->u.condition[1].left_saturation, |
498 | rk: effect->u.condition[1].right_coeff, |
499 | lk: effect->u.condition[1].left_coeff, |
500 | db: effect->u.condition[1].deadband, |
501 | center: effect->u.condition[1].center); |
502 | if (param_err) |
503 | return param_err; |
504 | set_bit(FF_MOD2_IS_USED, addr: core_effect->flags); |
505 | |
506 | } |
507 | |
508 | if (!old || need_core(old, new: effect)) { |
509 | core_err = make_core(iforce, id: effect->id, |
510 | mod_id1: mod1_chunk->start, mod_id2: mod2_chunk->start, |
511 | effect_type: type, axes: 0xc0, |
512 | duration: effect->replay.length, delay: effect->replay.delay, |
513 | button: effect->trigger.button, interval: effect->trigger.interval, |
514 | direction: effect->direction); |
515 | } |
516 | |
517 | /* If the parameter creation failed, we already returned an |
518 | * error code. |
519 | * If the core creation failed, we return its error code. |
520 | * Else: if a parameter was created, we return 0 |
521 | * else we return 1; |
522 | */ |
523 | return core_err < 0 ? core_err : param_err; |
524 | } |
525 | |