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
21MODULE_LICENSE("GPL");
22MODULE_AUTHOR("Anssi Hannula <anssi.hannula@gmail.com>");
23MODULE_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
35struct 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
44struct 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
55static 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 */
74static 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
109static 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 */
149static 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 */
195static 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 */
213static 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
226static inline s16 fixp_new16(s16 a)
227{
228	return ((s32)a) >> (16 - FRAC_N);
229}
230
231static 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 */
240static 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 */
323static 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
387static 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
400static 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 */
416static 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 */
432static 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
462static 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
485static 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 */
507int 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}
553EXPORT_SYMBOL_GPL(input_ff_create_memless);
554