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