Branch data 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 : 0 : static const struct ff_envelope *get_envelope(const struct ff_effect *effect)
59 : : {
60 : 0 : static const struct ff_envelope empty_envelope;
61 : :
62 : 0 : switch (effect->type) {
63 : 0 : case FF_PERIODIC:
64 : 0 : return &effect->u.periodic.envelope;
65 : :
66 : 0 : 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 : 0 : 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 : : 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 : : 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 : 0 : struct ml_effect_state *state;
115 : 0 : unsigned long now = jiffies;
116 : 0 : unsigned long earliest = 0;
117 : 0 : unsigned long next_at;
118 : 0 : int events = 0;
119 : 0 : int i;
120 : :
121 : 0 : 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 : 0 : pr_debug("no actions\n");
142 : 0 : del_timer(&ml->timer);
143 : : } else {
144 : 0 : 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 : 0 : int time_from_level;
158 : 0 : int time_of_envelope;
159 : 0 : int envelope_level;
160 : 0 : 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 : 0 : 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 : 0 : return value;
180 : :
181 : 0 : difference = abs(value) - envelope_level;
182 : :
183 : 0 : pr_debug("difference = %d\n", difference);
184 : 0 : pr_debug("time_from_level = 0x%x\n", time_from_level);
185 : 0 : pr_debug("time_of_envelope = 0x%x\n", time_of_envelope);
186 : :
187 : 0 : difference = difference * time_from_level / time_of_envelope;
188 : :
189 : 0 : pr_debug("difference = %d\n", difference);
190 : :
191 : 0 : 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 : 0 : 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 : 0 : static inline s16 fixp_new16(s16 a)
230 : : {
231 : 0 : return ((s32)a) >> (16 - FRAC_N);
232 : : }
233 : :
234 : 0 : 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 : 0 : unsigned int strong, weak, i;
249 : 0 : int x, y;
250 : 0 : s16 level;
251 : :
252 [ # # # # ]: 0 : switch (new->type) {
253 : 0 : 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 : 0 : 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 : 0 : 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 : 0 : 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 : 0 : struct ff_effect *effect;
331 : 0 : struct ml_effect_state *state;
332 : 0 : int effect_type;
333 : 0 : 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 : 0 : struct ff_effect effect;
393 : 0 : 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 : 0 : unsigned long flags;
408 : :
409 : 0 : 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 : 0 : 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 : 0 : 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 : 0 : 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 : 0 : struct ml_device *ml;
514 : 0 : struct ff_device *ff;
515 : 0 : int error;
516 : 0 : 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);
|
