Line data Source code
1 : // SPDX-License-Identifier: GPL-2.0-or-later
2 : /*
3 : * Force feedback support for memoryless devices
4 : *
5 : * Copyright (c) 2006 Anssi Hannula <anssi.hannula@gmail.com>
6 : * Copyright (c) 2006 Dmitry Torokhov <dtor@mail.ru>
7 : */
8 :
9 : /*
10 : */
11 :
12 : /* #define DEBUG */
13 :
14 : #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
15 :
16 : #include <linux/slab.h>
17 : #include <linux/input.h>
18 : #include <linux/module.h>
19 : #include <linux/mutex.h>
20 : #include <linux/spinlock.h>
21 : #include <linux/jiffies.h>
22 : #include <linux/fixp-arith.h>
23 :
24 : MODULE_LICENSE("GPL");
25 : MODULE_AUTHOR("Anssi Hannula <anssi.hannula@gmail.com>");
26 : MODULE_DESCRIPTION("Force feedback support for memoryless devices");
27 :
28 : /* Number of effects handled with memoryless devices */
29 : #define FF_MEMLESS_EFFECTS 16
30 :
31 : /* Envelope update interval in ms */
32 : #define FF_ENVELOPE_INTERVAL 50
33 :
34 : #define FF_EFFECT_STARTED 0
35 : #define FF_EFFECT_PLAYING 1
36 : #define FF_EFFECT_ABORTING 2
37 :
38 : struct ml_effect_state {
39 : struct ff_effect *effect;
40 : unsigned long flags; /* effect state (STARTED, PLAYING, etc) */
41 : int count; /* loop count of the effect */
42 : unsigned long play_at; /* start time */
43 : unsigned long stop_at; /* stop time */
44 : unsigned long adj_at; /* last time the effect was sent */
45 : };
46 :
47 : struct ml_device {
48 : void *private;
49 : struct ml_effect_state states[FF_MEMLESS_EFFECTS];
50 : int gain;
51 : struct timer_list timer;
52 : struct input_dev *dev;
53 :
54 : int (*play_effect)(struct input_dev *dev, void *data,
55 : struct ff_effect *effect);
56 : };
57 :
58 : static const struct ff_envelope *get_envelope(const struct ff_effect *effect)
59 : {
60 : static const struct ff_envelope empty_envelope;
61 :
62 0 : switch (effect->type) {
63 : case FF_PERIODIC:
64 0 : return &effect->u.periodic.envelope;
65 :
66 : case FF_CONSTANT:
67 0 : return &effect->u.constant.envelope;
68 :
69 : default:
70 : return &empty_envelope;
71 : }
72 : }
73 :
74 : /*
75 : * Check for the next time envelope requires an update on memoryless devices
76 : */
77 0 : static unsigned long calculate_next_time(struct ml_effect_state *state)
78 : {
79 0 : const struct ff_envelope *envelope = get_envelope(state->effect);
80 : unsigned long attack_stop, fade_start, next_fade;
81 :
82 0 : if (envelope->attack_length) {
83 0 : attack_stop = state->play_at +
84 0 : msecs_to_jiffies(envelope->attack_length);
85 0 : if (time_before(state->adj_at, attack_stop))
86 0 : return state->adj_at +
87 0 : msecs_to_jiffies(FF_ENVELOPE_INTERVAL);
88 : }
89 :
90 0 : if (state->effect->replay.length) {
91 0 : if (envelope->fade_length) {
92 : /* check when fading should start */
93 0 : fade_start = state->stop_at -
94 0 : msecs_to_jiffies(envelope->fade_length);
95 :
96 0 : if (time_before(state->adj_at, fade_start))
97 : return fade_start;
98 :
99 : /* already fading, advance to next checkpoint */
100 0 : next_fade = state->adj_at +
101 0 : msecs_to_jiffies(FF_ENVELOPE_INTERVAL);
102 0 : if (time_before(next_fade, state->stop_at))
103 : return next_fade;
104 : }
105 :
106 0 : return state->stop_at;
107 : }
108 :
109 0 : return state->play_at;
110 : }
111 :
112 0 : static void ml_schedule_timer(struct ml_device *ml)
113 : {
114 : struct ml_effect_state *state;
115 0 : unsigned long now = jiffies;
116 0 : unsigned long earliest = 0;
117 : unsigned long next_at;
118 0 : int events = 0;
119 : int i;
120 :
121 : pr_debug("calculating next timer\n");
122 :
123 0 : for (i = 0; i < FF_MEMLESS_EFFECTS; i++) {
124 :
125 0 : state = &ml->states[i];
126 :
127 0 : if (!test_bit(FF_EFFECT_STARTED, &state->flags))
128 0 : continue;
129 :
130 0 : if (test_bit(FF_EFFECT_PLAYING, &state->flags))
131 0 : next_at = calculate_next_time(state);
132 : else
133 0 : next_at = state->play_at;
134 :
135 0 : if (time_before_eq(now, next_at) &&
136 0 : (++events == 1 || time_before(next_at, earliest)))
137 : earliest = next_at;
138 : }
139 :
140 0 : if (!events) {
141 : pr_debug("no actions\n");
142 0 : del_timer(&ml->timer);
143 : } else {
144 : pr_debug("timer set\n");
145 0 : mod_timer(&ml->timer, earliest);
146 : }
147 0 : }
148 :
149 : /*
150 : * Apply an envelope to a value
151 : */
152 0 : static int apply_envelope(struct ml_effect_state *state, int value,
153 : struct ff_envelope *envelope)
154 : {
155 0 : struct ff_effect *effect = state->effect;
156 0 : unsigned long now = jiffies;
157 : int time_from_level;
158 : int time_of_envelope;
159 : int envelope_level;
160 : int difference;
161 :
162 0 : if (envelope->attack_length &&
163 0 : time_before(now,
164 : state->play_at + msecs_to_jiffies(envelope->attack_length))) {
165 : pr_debug("value = 0x%x, attack_level = 0x%x\n",
166 : value, envelope->attack_level);
167 0 : time_from_level = jiffies_to_msecs(now - state->play_at);
168 0 : time_of_envelope = envelope->attack_length;
169 0 : envelope_level = min_t(u16, envelope->attack_level, 0x7fff);
170 :
171 0 : } else if (envelope->fade_length && effect->replay.length &&
172 0 : time_after(now,
173 : state->stop_at - msecs_to_jiffies(envelope->fade_length)) &&
174 0 : time_before(now, state->stop_at)) {
175 0 : time_from_level = jiffies_to_msecs(state->stop_at - now);
176 0 : time_of_envelope = envelope->fade_length;
177 0 : envelope_level = min_t(u16, envelope->fade_level, 0x7fff);
178 : } else
179 : return value;
180 :
181 0 : difference = abs(value) - envelope_level;
182 :
183 : pr_debug("difference = %d\n", difference);
184 : pr_debug("time_from_level = 0x%x\n", time_from_level);
185 : pr_debug("time_of_envelope = 0x%x\n", time_of_envelope);
186 :
187 0 : difference = difference * time_from_level / time_of_envelope;
188 :
189 : pr_debug("difference = %d\n", difference);
190 :
191 : return value < 0 ?
192 0 : -(difference + envelope_level) : (difference + envelope_level);
193 : }
194 :
195 : /*
196 : * Return the type the effect has to be converted into (memless devices)
197 : */
198 0 : static int get_compatible_type(struct ff_device *ff, int effect_type)
199 : {
200 :
201 0 : if (test_bit(effect_type, ff->ffbit))
202 : return effect_type;
203 :
204 0 : if (effect_type == FF_PERIODIC && test_bit(FF_RUMBLE, ff->ffbit))
205 : return FF_RUMBLE;
206 :
207 0 : pr_err("invalid type in get_compatible_type()\n");
208 :
209 0 : return 0;
210 : }
211 :
212 : /*
213 : * Only left/right direction should be used (under/over 0x8000) for
214 : * forward/reverse motor direction (to keep calculation fast & simple).
215 : */
216 : static u16 ml_calculate_direction(u16 direction, u16 force,
217 : u16 new_direction, u16 new_force)
218 : {
219 0 : if (!force)
220 : return new_direction;
221 0 : if (!new_force)
222 : return direction;
223 0 : return (((u32)(direction >> 1) * force +
224 0 : (new_direction >> 1) * new_force) /
225 0 : (force + new_force)) << 1;
226 : }
227 :
228 : #define FRAC_N 8
229 : static inline s16 fixp_new16(s16 a)
230 : {
231 0 : return ((s32)a) >> (16 - FRAC_N);
232 : }
233 :
234 : static inline s16 fixp_mult(s16 a, s16 b)
235 : {
236 0 : a = ((s32)a * 0x100) / 0x7fff;
237 0 : return ((s32)(a * b)) >> FRAC_N;
238 : }
239 :
240 : /*
241 : * Combine two effects and apply gain.
242 : */
243 0 : static void ml_combine_effects(struct ff_effect *effect,
244 : struct ml_effect_state *state,
245 : int gain)
246 : {
247 0 : struct ff_effect *new = state->effect;
248 : unsigned int strong, weak, i;
249 : int x, y;
250 : s16 level;
251 :
252 0 : switch (new->type) {
253 : case FF_CONSTANT:
254 0 : i = new->direction * 360 / 0xffff;
255 0 : level = fixp_new16(apply_envelope(state,
256 0 : new->u.constant.level,
257 : &new->u.constant.envelope));
258 0 : x = fixp_mult(fixp_sin16(i), level) * gain / 0xffff;
259 0 : y = fixp_mult(-fixp_cos16(i), level) * gain / 0xffff;
260 : /*
261 : * here we abuse ff_ramp to hold x and y of constant force
262 : * If in future any driver wants something else than x and y
263 : * in s8, this should be changed to something more generic
264 : */
265 0 : effect->u.ramp.start_level =
266 0 : clamp_val(effect->u.ramp.start_level + x, -0x80, 0x7f);
267 0 : effect->u.ramp.end_level =
268 0 : clamp_val(effect->u.ramp.end_level + y, -0x80, 0x7f);
269 0 : break;
270 :
271 : case FF_RUMBLE:
272 0 : strong = (u32)new->u.rumble.strong_magnitude * gain / 0xffff;
273 0 : weak = (u32)new->u.rumble.weak_magnitude * gain / 0xffff;
274 :
275 0 : if (effect->u.rumble.strong_magnitude + strong)
276 0 : effect->direction = ml_calculate_direction(
277 0 : effect->direction,
278 : effect->u.rumble.strong_magnitude,
279 0 : new->direction, strong);
280 0 : else if (effect->u.rumble.weak_magnitude + weak)
281 0 : effect->direction = ml_calculate_direction(
282 0 : effect->direction,
283 : effect->u.rumble.weak_magnitude,
284 0 : new->direction, weak);
285 : else
286 0 : effect->direction = 0;
287 0 : effect->u.rumble.strong_magnitude =
288 0 : min(strong + effect->u.rumble.strong_magnitude,
289 : 0xffffU);
290 0 : effect->u.rumble.weak_magnitude =
291 0 : min(weak + effect->u.rumble.weak_magnitude, 0xffffU);
292 0 : break;
293 :
294 : case FF_PERIODIC:
295 0 : i = apply_envelope(state, abs(new->u.periodic.magnitude),
296 : &new->u.periodic.envelope);
297 :
298 : /* here we also scale it 0x7fff => 0xffff */
299 0 : i = i * gain / 0x7fff;
300 :
301 0 : if (effect->u.rumble.strong_magnitude + i)
302 0 : effect->direction = ml_calculate_direction(
303 0 : effect->direction,
304 : effect->u.rumble.strong_magnitude,
305 0 : new->direction, i);
306 : else
307 0 : effect->direction = 0;
308 0 : effect->u.rumble.strong_magnitude =
309 0 : min(i + effect->u.rumble.strong_magnitude, 0xffffU);
310 0 : effect->u.rumble.weak_magnitude =
311 0 : min(i + effect->u.rumble.weak_magnitude, 0xffffU);
312 0 : break;
313 :
314 : default:
315 0 : pr_err("invalid type in ml_combine_effects()\n");
316 0 : break;
317 : }
318 :
319 0 : }
320 :
321 :
322 : /*
323 : * Because memoryless devices have only one effect per effect type active
324 : * at one time we have to combine multiple effects into one
325 : */
326 0 : static int ml_get_combo_effect(struct ml_device *ml,
327 : unsigned long *effect_handled,
328 : struct ff_effect *combo_effect)
329 : {
330 : struct ff_effect *effect;
331 : struct ml_effect_state *state;
332 : int effect_type;
333 : int i;
334 :
335 0 : memset(combo_effect, 0, sizeof(struct ff_effect));
336 :
337 0 : for (i = 0; i < FF_MEMLESS_EFFECTS; i++) {
338 0 : if (__test_and_set_bit(i, effect_handled))
339 0 : continue;
340 :
341 0 : state = &ml->states[i];
342 0 : effect = state->effect;
343 :
344 0 : if (!test_bit(FF_EFFECT_STARTED, &state->flags))
345 0 : continue;
346 :
347 0 : if (time_before(jiffies, state->play_at))
348 0 : continue;
349 :
350 : /*
351 : * here we have started effects that are either
352 : * currently playing (and may need be aborted)
353 : * or need to start playing.
354 : */
355 0 : effect_type = get_compatible_type(ml->dev->ff, effect->type);
356 0 : if (combo_effect->type != effect_type) {
357 0 : if (combo_effect->type != 0) {
358 0 : __clear_bit(i, effect_handled);
359 0 : continue;
360 : }
361 0 : combo_effect->type = effect_type;
362 : }
363 :
364 0 : if (__test_and_clear_bit(FF_EFFECT_ABORTING, &state->flags)) {
365 0 : __clear_bit(FF_EFFECT_PLAYING, &state->flags);
366 0 : __clear_bit(FF_EFFECT_STARTED, &state->flags);
367 0 : } else if (effect->replay.length &&
368 0 : time_after_eq(jiffies, state->stop_at)) {
369 :
370 0 : __clear_bit(FF_EFFECT_PLAYING, &state->flags);
371 :
372 0 : if (--state->count <= 0) {
373 0 : __clear_bit(FF_EFFECT_STARTED, &state->flags);
374 : } else {
375 0 : state->play_at = jiffies +
376 0 : msecs_to_jiffies(effect->replay.delay);
377 0 : state->stop_at = state->play_at +
378 0 : msecs_to_jiffies(effect->replay.length);
379 : }
380 : } else {
381 0 : __set_bit(FF_EFFECT_PLAYING, &state->flags);
382 0 : state->adj_at = jiffies;
383 0 : ml_combine_effects(combo_effect, state, ml->gain);
384 : }
385 : }
386 :
387 0 : return combo_effect->type != 0;
388 : }
389 :
390 0 : static void ml_play_effects(struct ml_device *ml)
391 : {
392 : struct ff_effect effect;
393 : DECLARE_BITMAP(handled_bm, FF_MEMLESS_EFFECTS);
394 :
395 0 : memset(handled_bm, 0, sizeof(handled_bm));
396 :
397 0 : while (ml_get_combo_effect(ml, handled_bm, &effect))
398 0 : ml->play_effect(ml->dev, ml->private, &effect);
399 :
400 0 : ml_schedule_timer(ml);
401 0 : }
402 :
403 0 : static void ml_effect_timer(struct timer_list *t)
404 : {
405 0 : struct ml_device *ml = from_timer(ml, t, timer);
406 0 : struct input_dev *dev = ml->dev;
407 : unsigned long flags;
408 :
409 : pr_debug("timer: updating effects\n");
410 :
411 0 : spin_lock_irqsave(&dev->event_lock, flags);
412 0 : ml_play_effects(ml);
413 0 : spin_unlock_irqrestore(&dev->event_lock, flags);
414 0 : }
415 :
416 : /*
417 : * Sets requested gain for FF effects. Called with dev->event_lock held.
418 : */
419 0 : static void ml_ff_set_gain(struct input_dev *dev, u16 gain)
420 : {
421 0 : struct ml_device *ml = dev->ff->private;
422 : int i;
423 :
424 0 : ml->gain = gain;
425 :
426 0 : for (i = 0; i < FF_MEMLESS_EFFECTS; i++)
427 0 : __clear_bit(FF_EFFECT_PLAYING, &ml->states[i].flags);
428 :
429 0 : ml_play_effects(ml);
430 0 : }
431 :
432 : /*
433 : * Start/stop specified FF effect. Called with dev->event_lock held.
434 : */
435 0 : static int ml_ff_playback(struct input_dev *dev, int effect_id, int value)
436 : {
437 0 : struct ml_device *ml = dev->ff->private;
438 0 : struct ml_effect_state *state = &ml->states[effect_id];
439 :
440 0 : if (value > 0) {
441 : pr_debug("initiated play\n");
442 :
443 0 : __set_bit(FF_EFFECT_STARTED, &state->flags);
444 0 : state->count = value;
445 0 : state->play_at = jiffies +
446 0 : msecs_to_jiffies(state->effect->replay.delay);
447 0 : state->stop_at = state->play_at +
448 0 : msecs_to_jiffies(state->effect->replay.length);
449 0 : state->adj_at = state->play_at;
450 :
451 : } else {
452 : pr_debug("initiated stop\n");
453 :
454 0 : if (test_bit(FF_EFFECT_PLAYING, &state->flags))
455 0 : __set_bit(FF_EFFECT_ABORTING, &state->flags);
456 : else
457 0 : __clear_bit(FF_EFFECT_STARTED, &state->flags);
458 : }
459 :
460 0 : ml_play_effects(ml);
461 :
462 0 : return 0;
463 : }
464 :
465 0 : static int ml_ff_upload(struct input_dev *dev,
466 : struct ff_effect *effect, struct ff_effect *old)
467 : {
468 0 : struct ml_device *ml = dev->ff->private;
469 0 : struct ml_effect_state *state = &ml->states[effect->id];
470 :
471 0 : spin_lock_irq(&dev->event_lock);
472 :
473 0 : if (test_bit(FF_EFFECT_STARTED, &state->flags)) {
474 0 : __clear_bit(FF_EFFECT_PLAYING, &state->flags);
475 0 : state->play_at = jiffies +
476 0 : msecs_to_jiffies(state->effect->replay.delay);
477 0 : state->stop_at = state->play_at +
478 0 : msecs_to_jiffies(state->effect->replay.length);
479 0 : state->adj_at = state->play_at;
480 0 : ml_schedule_timer(ml);
481 : }
482 :
483 0 : spin_unlock_irq(&dev->event_lock);
484 :
485 0 : return 0;
486 : }
487 :
488 0 : static void ml_ff_destroy(struct ff_device *ff)
489 : {
490 0 : struct ml_device *ml = ff->private;
491 :
492 : /*
493 : * Even though we stop all playing effects when tearing down
494 : * an input device (via input_device_flush() that calls into
495 : * input_ff_flush() that stops and erases all effects), we
496 : * do not actually stop the timer, and therefore we should
497 : * do it here.
498 : */
499 0 : del_timer_sync(&ml->timer);
500 :
501 0 : kfree(ml->private);
502 0 : }
503 :
504 : /**
505 : * input_ff_create_memless() - create memoryless force-feedback device
506 : * @dev: input device supporting force-feedback
507 : * @data: driver-specific data to be passed into @play_effect
508 : * @play_effect: driver-specific method for playing FF effect
509 : */
510 0 : int input_ff_create_memless(struct input_dev *dev, void *data,
511 : int (*play_effect)(struct input_dev *, void *, struct ff_effect *))
512 : {
513 : struct ml_device *ml;
514 : struct ff_device *ff;
515 : int error;
516 : int i;
517 :
518 0 : ml = kzalloc(sizeof(struct ml_device), GFP_KERNEL);
519 0 : if (!ml)
520 : return -ENOMEM;
521 :
522 0 : ml->dev = dev;
523 0 : ml->private = data;
524 0 : ml->play_effect = play_effect;
525 0 : ml->gain = 0xffff;
526 0 : timer_setup(&ml->timer, ml_effect_timer, 0);
527 :
528 0 : set_bit(FF_GAIN, dev->ffbit);
529 :
530 0 : error = input_ff_create(dev, FF_MEMLESS_EFFECTS);
531 0 : if (error) {
532 0 : kfree(ml);
533 0 : return error;
534 : }
535 :
536 0 : ff = dev->ff;
537 0 : ff->private = ml;
538 0 : ff->upload = ml_ff_upload;
539 0 : ff->playback = ml_ff_playback;
540 0 : ff->set_gain = ml_ff_set_gain;
541 0 : ff->destroy = ml_ff_destroy;
542 :
543 : /* we can emulate periodic effects with RUMBLE */
544 0 : if (test_bit(FF_RUMBLE, ff->ffbit)) {
545 0 : set_bit(FF_PERIODIC, dev->ffbit);
546 0 : set_bit(FF_SINE, dev->ffbit);
547 0 : set_bit(FF_TRIANGLE, dev->ffbit);
548 0 : set_bit(FF_SQUARE, dev->ffbit);
549 : }
550 :
551 0 : for (i = 0; i < FF_MEMLESS_EFFECTS; i++)
552 0 : ml->states[i].effect = &ff->effects[i];
553 :
554 : return 0;
555 : }
556 : EXPORT_SYMBOL_GPL(input_ff_create_memless);
|
