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