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
get_envelope(const struct ff_effect * effect)55 static const struct ff_envelope *get_envelope(const struct ff_effect *effect)
56 {
57 static const struct ff_envelope empty_envelope;
58
59 switch (effect->type) {
60 case FF_PERIODIC:
61 return &effect->u.periodic.envelope;
62
63 case FF_CONSTANT:
64 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 */
calculate_next_time(struct ml_effect_state * state)74 static unsigned long calculate_next_time(struct ml_effect_state *state)
75 {
76 const struct ff_envelope *envelope = get_envelope(state->effect);
77 unsigned long attack_stop, fade_start, next_fade;
78
79 if (envelope->attack_length) {
80 attack_stop = state->play_at +
81 msecs_to_jiffies(envelope->attack_length);
82 if (time_before(state->adj_at, attack_stop))
83 return state->adj_at +
84 msecs_to_jiffies(FF_ENVELOPE_INTERVAL);
85 }
86
87 if (state->effect->replay.length) {
88 if (envelope->fade_length) {
89 /* check when fading should start */
90 fade_start = state->stop_at -
91 msecs_to_jiffies(envelope->fade_length);
92
93 if (time_before(state->adj_at, fade_start))
94 return fade_start;
95
96 /* already fading, advance to next checkpoint */
97 next_fade = state->adj_at +
98 msecs_to_jiffies(FF_ENVELOPE_INTERVAL);
99 if (time_before(next_fade, state->stop_at))
100 return next_fade;
101 }
102
103 return state->stop_at;
104 }
105
106 return state->play_at;
107 }
108
ml_schedule_timer(struct ml_device * ml)109 static void ml_schedule_timer(struct ml_device *ml)
110 {
111 struct ml_effect_state *state;
112 unsigned long now = jiffies;
113 unsigned long earliest = 0;
114 unsigned long next_at;
115 int events = 0;
116 int i;
117
118 pr_debug("calculating next timer\n");
119
120 for (i = 0; i < FF_MEMLESS_EFFECTS; i++) {
121
122 state = &ml->states[i];
123
124 if (!test_bit(FF_EFFECT_STARTED, &state->flags))
125 continue;
126
127 if (test_bit(FF_EFFECT_PLAYING, &state->flags))
128 next_at = calculate_next_time(state);
129 else
130 next_at = state->play_at;
131
132 if (time_before_eq(now, next_at) &&
133 (++events == 1 || time_before(next_at, earliest)))
134 earliest = next_at;
135 }
136
137 if (!events) {
138 pr_debug("no actions\n");
139 del_timer(&ml->timer);
140 } else {
141 pr_debug("timer set\n");
142 mod_timer(&ml->timer, earliest);
143 }
144 }
145
146 /*
147 * Apply an envelope to a value
148 */
apply_envelope(struct ml_effect_state * state,int value,struct ff_envelope * envelope)149 static int apply_envelope(struct ml_effect_state *state, int value,
150 struct ff_envelope *envelope)
151 {
152 struct ff_effect *effect = state->effect;
153 unsigned long now = jiffies;
154 int time_from_level;
155 int time_of_envelope;
156 int envelope_level;
157 int difference;
158
159 if (envelope->attack_length &&
160 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 time_from_level = jiffies_to_msecs(now - state->play_at);
165 time_of_envelope = envelope->attack_length;
166 envelope_level = min_t(u16, envelope->attack_level, 0x7fff);
167
168 } else if (envelope->fade_length && effect->replay.length &&
169 time_after(now,
170 state->stop_at - msecs_to_jiffies(envelope->fade_length)) &&
171 time_before(now, state->stop_at)) {
172 time_from_level = jiffies_to_msecs(state->stop_at - now);
173 time_of_envelope = envelope->fade_length;
174 envelope_level = min_t(u16, envelope->fade_level, 0x7fff);
175 } else
176 return value;
177
178 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 difference = difference * time_from_level / time_of_envelope;
185
186 pr_debug("difference = %d\n", difference);
187
188 return value < 0 ?
189 -(difference + envelope_level) : (difference + envelope_level);
190 }
191
192 /*
193 * Return the type the effect has to be converted into (memless devices)
194 */
get_compatible_type(struct ff_device * ff,int effect_type)195 static int get_compatible_type(struct ff_device *ff, int effect_type)
196 {
197
198 if (test_bit(effect_type, ff->ffbit))
199 return effect_type;
200
201 if (effect_type == FF_PERIODIC && test_bit(FF_RUMBLE, ff->ffbit))
202 return FF_RUMBLE;
203
204 pr_err("invalid type in get_compatible_type()\n");
205
206 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 */
ml_calculate_direction(u16 direction,u16 force,u16 new_direction,u16 new_force)213 static u16 ml_calculate_direction(u16 direction, u16 force,
214 u16 new_direction, u16 new_force)
215 {
216 if (!force)
217 return new_direction;
218 if (!new_force)
219 return direction;
220 return (((u32)(direction >> 1) * force +
221 (new_direction >> 1) * new_force) /
222 (force + new_force)) << 1;
223 }
224
225 #define FRAC_N 8
fixp_new16(s16 a)226 static inline s16 fixp_new16(s16 a)
227 {
228 return ((s32)a) >> (16 - FRAC_N);
229 }
230
fixp_mult(s16 a,s16 b)231 static inline s16 fixp_mult(s16 a, s16 b)
232 {
233 a = ((s32)a * 0x100) / 0x7fff;
234 return ((s32)(a * b)) >> FRAC_N;
235 }
236
237 /*
238 * Combine two effects and apply gain.
239 */
ml_combine_effects(struct ff_effect * effect,struct ml_effect_state * state,int gain)240 static void ml_combine_effects(struct ff_effect *effect,
241 struct ml_effect_state *state,
242 int gain)
243 {
244 struct ff_effect *new = state->effect;
245 unsigned int strong, weak, i;
246 int x, y;
247 s16 level;
248
249 switch (new->type) {
250 case FF_CONSTANT:
251 i = new->direction * 360 / 0xffff;
252 level = fixp_new16(apply_envelope(state,
253 new->u.constant.level,
254 &new->u.constant.envelope));
255 x = fixp_mult(fixp_sin16(i), level) * gain / 0xffff;
256 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 effect->u.ramp.start_level =
263 clamp_val(effect->u.ramp.start_level + x, -0x80, 0x7f);
264 effect->u.ramp.end_level =
265 clamp_val(effect->u.ramp.end_level + y, -0x80, 0x7f);
266 break;
267
268 case FF_RUMBLE:
269 strong = (u32)new->u.rumble.strong_magnitude * gain / 0xffff;
270 weak = (u32)new->u.rumble.weak_magnitude * gain / 0xffff;
271
272 if (effect->u.rumble.strong_magnitude + strong)
273 effect->direction = ml_calculate_direction(
274 effect->direction,
275 effect->u.rumble.strong_magnitude,
276 new->direction, strong);
277 else if (effect->u.rumble.weak_magnitude + weak)
278 effect->direction = ml_calculate_direction(
279 effect->direction,
280 effect->u.rumble.weak_magnitude,
281 new->direction, weak);
282 else
283 effect->direction = 0;
284 effect->u.rumble.strong_magnitude =
285 min(strong + effect->u.rumble.strong_magnitude,
286 0xffffU);
287 effect->u.rumble.weak_magnitude =
288 min(weak + effect->u.rumble.weak_magnitude, 0xffffU);
289 break;
290
291 case FF_PERIODIC:
292 i = apply_envelope(state, abs(new->u.periodic.magnitude),
293 &new->u.periodic.envelope);
294
295 /* here we also scale it 0x7fff => 0xffff */
296 i = i * gain / 0x7fff;
297
298 if (effect->u.rumble.strong_magnitude + i)
299 effect->direction = ml_calculate_direction(
300 effect->direction,
301 effect->u.rumble.strong_magnitude,
302 new->direction, i);
303 else
304 effect->direction = 0;
305 effect->u.rumble.strong_magnitude =
306 min(i + effect->u.rumble.strong_magnitude, 0xffffU);
307 effect->u.rumble.weak_magnitude =
308 min(i + effect->u.rumble.weak_magnitude, 0xffffU);
309 break;
310
311 default:
312 pr_err("invalid type in ml_combine_effects()\n");
313 break;
314 }
315
316 }
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 */
ml_get_combo_effect(struct ml_device * ml,unsigned long * effect_handled,struct ff_effect * combo_effect)323 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 memset(combo_effect, 0, sizeof(struct ff_effect));
333
334 for (i = 0; i < FF_MEMLESS_EFFECTS; i++) {
335 if (__test_and_set_bit(i, effect_handled))
336 continue;
337
338 state = &ml->states[i];
339 effect = state->effect;
340
341 if (!test_bit(FF_EFFECT_STARTED, &state->flags))
342 continue;
343
344 if (time_before(jiffies, state->play_at))
345 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 effect_type = get_compatible_type(ml->dev->ff, effect->type);
353 if (combo_effect->type != effect_type) {
354 if (combo_effect->type != 0) {
355 __clear_bit(i, effect_handled);
356 continue;
357 }
358 combo_effect->type = effect_type;
359 }
360
361 if (__test_and_clear_bit(FF_EFFECT_ABORTING, &state->flags)) {
362 __clear_bit(FF_EFFECT_PLAYING, &state->flags);
363 __clear_bit(FF_EFFECT_STARTED, &state->flags);
364 } else if (effect->replay.length &&
365 time_after_eq(jiffies, state->stop_at)) {
366
367 __clear_bit(FF_EFFECT_PLAYING, &state->flags);
368
369 if (--state->count <= 0) {
370 __clear_bit(FF_EFFECT_STARTED, &state->flags);
371 } else {
372 state->play_at = jiffies +
373 msecs_to_jiffies(effect->replay.delay);
374 state->stop_at = state->play_at +
375 msecs_to_jiffies(effect->replay.length);
376 }
377 } else {
378 __set_bit(FF_EFFECT_PLAYING, &state->flags);
379 state->adj_at = jiffies;
380 ml_combine_effects(combo_effect, state, ml->gain);
381 }
382 }
383
384 return combo_effect->type != 0;
385 }
386
ml_play_effects(struct ml_device * ml)387 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 memset(handled_bm, 0, sizeof(handled_bm));
393
394 while (ml_get_combo_effect(ml, handled_bm, &effect))
395 ml->play_effect(ml->dev, ml->private, &effect);
396
397 ml_schedule_timer(ml);
398 }
399
ml_effect_timer(struct timer_list * t)400 static void ml_effect_timer(struct timer_list *t)
401 {
402 struct ml_device *ml = from_timer(ml, t, timer);
403 struct input_dev *dev = ml->dev;
404 unsigned long flags;
405
406 pr_debug("timer: updating effects\n");
407
408 spin_lock_irqsave(&dev->event_lock, flags);
409 ml_play_effects(ml);
410 spin_unlock_irqrestore(&dev->event_lock, flags);
411 }
412
413 /*
414 * Sets requested gain for FF effects. Called with dev->event_lock held.
415 */
ml_ff_set_gain(struct input_dev * dev,u16 gain)416 static void ml_ff_set_gain(struct input_dev *dev, u16 gain)
417 {
418 struct ml_device *ml = dev->ff->private;
419 int i;
420
421 ml->gain = gain;
422
423 for (i = 0; i < FF_MEMLESS_EFFECTS; i++)
424 __clear_bit(FF_EFFECT_PLAYING, &ml->states[i].flags);
425
426 ml_play_effects(ml);
427 }
428
429 /*
430 * Start/stop specified FF effect. Called with dev->event_lock held.
431 */
ml_ff_playback(struct input_dev * dev,int effect_id,int value)432 static int ml_ff_playback(struct input_dev *dev, int effect_id, int value)
433 {
434 struct ml_device *ml = dev->ff->private;
435 struct ml_effect_state *state = &ml->states[effect_id];
436
437 if (value > 0) {
438 pr_debug("initiated play\n");
439
440 __set_bit(FF_EFFECT_STARTED, &state->flags);
441 state->count = value;
442 state->play_at = jiffies +
443 msecs_to_jiffies(state->effect->replay.delay);
444 state->stop_at = state->play_at +
445 msecs_to_jiffies(state->effect->replay.length);
446 state->adj_at = state->play_at;
447
448 } else {
449 pr_debug("initiated stop\n");
450
451 if (test_bit(FF_EFFECT_PLAYING, &state->flags))
452 __set_bit(FF_EFFECT_ABORTING, &state->flags);
453 else
454 __clear_bit(FF_EFFECT_STARTED, &state->flags);
455 }
456
457 ml_play_effects(ml);
458
459 return 0;
460 }
461
ml_ff_upload(struct input_dev * dev,struct ff_effect * effect,struct ff_effect * old)462 static int ml_ff_upload(struct input_dev *dev,
463 struct ff_effect *effect, struct ff_effect *old)
464 {
465 struct ml_device *ml = dev->ff->private;
466 struct ml_effect_state *state = &ml->states[effect->id];
467
468 spin_lock_irq(&dev->event_lock);
469
470 if (test_bit(FF_EFFECT_STARTED, &state->flags)) {
471 __clear_bit(FF_EFFECT_PLAYING, &state->flags);
472 state->play_at = jiffies +
473 msecs_to_jiffies(state->effect->replay.delay);
474 state->stop_at = state->play_at +
475 msecs_to_jiffies(state->effect->replay.length);
476 state->adj_at = state->play_at;
477 ml_schedule_timer(ml);
478 }
479
480 spin_unlock_irq(&dev->event_lock);
481
482 return 0;
483 }
484
ml_ff_destroy(struct ff_device * ff)485 static void ml_ff_destroy(struct ff_device *ff)
486 {
487 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 del_timer_sync(&ml->timer);
497
498 kfree(ml->private);
499 }
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 */
input_ff_create_memless(struct input_dev * dev,void * data,int (* play_effect)(struct input_dev *,void *,struct ff_effect *))507 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 ml = kzalloc(sizeof(struct ml_device), GFP_KERNEL);
516 if (!ml)
517 return -ENOMEM;
518
519 ml->dev = dev;
520 ml->private = data;
521 ml->play_effect = play_effect;
522 ml->gain = 0xffff;
523 timer_setup(&ml->timer, ml_effect_timer, 0);
524
525 set_bit(FF_GAIN, dev->ffbit);
526
527 error = input_ff_create(dev, FF_MEMLESS_EFFECTS);
528 if (error) {
529 kfree(ml);
530 return error;
531 }
532
533 ff = dev->ff;
534 ff->private = ml;
535 ff->upload = ml_ff_upload;
536 ff->playback = ml_ff_playback;
537 ff->set_gain = ml_ff_set_gain;
538 ff->destroy = ml_ff_destroy;
539
540 /* we can emulate periodic effects with RUMBLE */
541 if (test_bit(FF_RUMBLE, ff->ffbit)) {
542 set_bit(FF_PERIODIC, dev->ffbit);
543 set_bit(FF_SINE, dev->ffbit);
544 set_bit(FF_TRIANGLE, dev->ffbit);
545 set_bit(FF_SQUARE, dev->ffbit);
546 }
547
548 for (i = 0; i < FF_MEMLESS_EFFECTS; i++)
549 ml->states[i].effect = &ff->effects[i];
550
551 return 0;
552 }
553 EXPORT_SYMBOL_GPL(input_ff_create_memless);
554