2 * Force feedback support for memoryless devices
4 * Copyright (c) 2006 Anssi Hannula <anssi.hannula@gmail.com>
5 * Copyright (c) 2006 Dmitry Torokhov <dtor@mail.ru>
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.
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.
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
26 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
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>
35 #include "fixp-arith.h"
37 MODULE_LICENSE("GPL");
38 MODULE_AUTHOR("Anssi Hannula <anssi.hannula@gmail.com>");
39 MODULE_DESCRIPTION("Force feedback support for memoryless devices");
41 /* Number of effects handled with memoryless devices */
42 #define FF_MEMLESS_EFFECTS 16
44 /* Envelope update interval in ms */
45 #define FF_ENVELOPE_INTERVAL 50
47 #define FF_EFFECT_STARTED 0
48 #define FF_EFFECT_PLAYING 1
49 #define FF_EFFECT_ABORTING 2
51 struct ml_effect_state
{
52 struct ff_effect
*effect
;
53 unsigned long flags
; /* effect state (STARTED, PLAYING, etc) */
54 int count
; /* loop count of the effect */
55 unsigned long play_at
; /* start time */
56 unsigned long stop_at
; /* stop time */
57 unsigned long adj_at
; /* last time the effect was sent */
62 struct ml_effect_state states
[FF_MEMLESS_EFFECTS
];
64 struct timer_list timer
;
65 struct input_dev
*dev
;
67 int (*play_effect
)(struct input_dev
*dev
, void *data
,
68 struct ff_effect
*effect
);
71 static const struct ff_envelope
*get_envelope(const struct ff_effect
*effect
)
73 static const struct ff_envelope empty_envelope
;
75 switch (effect
->type
) {
77 return &effect
->u
.periodic
.envelope
;
79 return &effect
->u
.constant
.envelope
;
81 return &empty_envelope
;
86 * Check for the next time envelope requires an update on memoryless devices
88 static unsigned long calculate_next_time(struct ml_effect_state
*state
)
90 const struct ff_envelope
*envelope
= get_envelope(state
->effect
);
91 unsigned long attack_stop
, fade_start
, next_fade
;
93 if (envelope
->attack_length
) {
94 attack_stop
= state
->play_at
+
95 msecs_to_jiffies(envelope
->attack_length
);
96 if (time_before(state
->adj_at
, attack_stop
))
97 return state
->adj_at
+
98 msecs_to_jiffies(FF_ENVELOPE_INTERVAL
);
101 if (state
->effect
->replay
.length
) {
102 if (envelope
->fade_length
) {
103 /* check when fading should start */
104 fade_start
= state
->stop_at
-
105 msecs_to_jiffies(envelope
->fade_length
);
107 if (time_before(state
->adj_at
, fade_start
))
110 /* already fading, advance to next checkpoint */
111 next_fade
= state
->adj_at
+
112 msecs_to_jiffies(FF_ENVELOPE_INTERVAL
);
113 if (time_before(next_fade
, state
->stop_at
))
117 return state
->stop_at
;
120 return state
->play_at
;
123 static void ml_schedule_timer(struct ml_device
*ml
)
125 struct ml_effect_state
*state
;
126 unsigned long now
= jiffies
;
127 unsigned long earliest
= 0;
128 unsigned long next_at
;
132 pr_debug("calculating next timer\n");
134 for (i
= 0; i
< FF_MEMLESS_EFFECTS
; i
++) {
136 state
= &ml
->states
[i
];
138 if (!test_bit(FF_EFFECT_STARTED
, &state
->flags
))
141 if (test_bit(FF_EFFECT_PLAYING
, &state
->flags
))
142 next_at
= calculate_next_time(state
);
144 next_at
= state
->play_at
;
146 if (time_before_eq(now
, next_at
) &&
147 (++events
== 1 || time_before(next_at
, earliest
)))
152 pr_debug("no actions\n");
153 del_timer(&ml
->timer
);
155 pr_debug("timer set\n");
156 mod_timer(&ml
->timer
, earliest
);
161 * Apply an envelope to a value
163 static int apply_envelope(struct ml_effect_state
*state
, int value
,
164 struct ff_envelope
*envelope
)
166 struct ff_effect
*effect
= state
->effect
;
167 unsigned long now
= jiffies
;
169 int time_of_envelope
;
173 if (envelope
->attack_length
&&
175 state
->play_at
+ msecs_to_jiffies(envelope
->attack_length
))) {
176 pr_debug("value = 0x%x, attack_level = 0x%x\n",
177 value
, envelope
->attack_level
);
178 time_from_level
= jiffies_to_msecs(now
- state
->play_at
);
179 time_of_envelope
= envelope
->attack_length
;
180 envelope_level
= min_t(__s16
, envelope
->attack_level
, 0x7fff);
182 } else if (envelope
->fade_length
&& effect
->replay
.length
&&
184 state
->stop_at
- msecs_to_jiffies(envelope
->fade_length
)) &&
185 time_before(now
, state
->stop_at
)) {
186 time_from_level
= jiffies_to_msecs(state
->stop_at
- now
);
187 time_of_envelope
= envelope
->fade_length
;
188 envelope_level
= min_t(__s16
, envelope
->fade_level
, 0x7fff);
192 difference
= abs(value
) - envelope_level
;
194 pr_debug("difference = %d\n", difference
);
195 pr_debug("time_from_level = 0x%x\n", time_from_level
);
196 pr_debug("time_of_envelope = 0x%x\n", time_of_envelope
);
198 difference
= difference
* time_from_level
/ time_of_envelope
;
200 pr_debug("difference = %d\n", difference
);
203 -(difference
+ envelope_level
) : (difference
+ envelope_level
);
207 * Return the type the effect has to be converted into (memless devices)
209 static int get_compatible_type(struct ff_device
*ff
, int effect_type
)
212 if (test_bit(effect_type
, ff
->ffbit
))
215 if (effect_type
== FF_PERIODIC
&& test_bit(FF_RUMBLE
, ff
->ffbit
))
218 pr_err("invalid type in get_compatible_type()\n");
224 * Only left/right direction should be used (under/over 0x8000) for
225 * forward/reverse motor direction (to keep calculation fast & simple).
227 static u16
ml_calculate_direction(u16 direction
, u16 force
,
228 u16 new_direction
, u16 new_force
)
231 return new_direction
;
234 return (((u32
)(direction
>> 1) * force
+
235 (new_direction
>> 1) * new_force
) /
236 (force
+ new_force
)) << 1;
240 * Combine two effects and apply gain.
242 static void ml_combine_effects(struct ff_effect
*effect
,
243 struct ml_effect_state
*state
,
246 struct ff_effect
*new = state
->effect
;
247 unsigned int strong
, weak
, i
;
253 i
= new->direction
* 360 / 0xffff;
254 level
= fixp_new16(apply_envelope(state
,
255 new->u
.constant
.level
,
256 &new->u
.constant
.envelope
));
257 x
= fixp_mult(fixp_sin(i
), level
) * gain
/ 0xffff;
258 y
= fixp_mult(-fixp_cos(i
), level
) * gain
/ 0xffff;
260 * here we abuse ff_ramp to hold x and y of constant force
261 * If in future any driver wants something else than x and y
262 * in s8, this should be changed to something more generic
264 effect
->u
.ramp
.start_level
=
265 clamp_val(effect
->u
.ramp
.start_level
+ x
, -0x80, 0x7f);
266 effect
->u
.ramp
.end_level
=
267 clamp_val(effect
->u
.ramp
.end_level
+ y
, -0x80, 0x7f);
271 strong
= (u32
)new->u
.rumble
.strong_magnitude
* gain
/ 0xffff;
272 weak
= (u32
)new->u
.rumble
.weak_magnitude
* gain
/ 0xffff;
274 if (effect
->u
.rumble
.strong_magnitude
+ strong
)
275 effect
->direction
= ml_calculate_direction(
277 effect
->u
.rumble
.strong_magnitude
,
278 new->direction
, strong
);
279 else if (effect
->u
.rumble
.weak_magnitude
+ weak
)
280 effect
->direction
= ml_calculate_direction(
282 effect
->u
.rumble
.weak_magnitude
,
283 new->direction
, weak
);
285 effect
->direction
= 0;
286 effect
->u
.rumble
.strong_magnitude
=
287 min(strong
+ effect
->u
.rumble
.strong_magnitude
,
289 effect
->u
.rumble
.weak_magnitude
=
290 min(weak
+ effect
->u
.rumble
.weak_magnitude
, 0xffffU
);
294 i
= apply_envelope(state
, abs(new->u
.periodic
.magnitude
),
295 &new->u
.periodic
.envelope
);
297 /* here we also scale it 0x7fff => 0xffff */
298 i
= i
* gain
/ 0x7fff;
300 if (effect
->u
.rumble
.strong_magnitude
+ i
)
301 effect
->direction
= ml_calculate_direction(
303 effect
->u
.rumble
.strong_magnitude
,
306 effect
->direction
= 0;
307 effect
->u
.rumble
.strong_magnitude
=
308 min(i
+ effect
->u
.rumble
.strong_magnitude
, 0xffffU
);
309 effect
->u
.rumble
.weak_magnitude
=
310 min(i
+ effect
->u
.rumble
.weak_magnitude
, 0xffffU
);
314 pr_err("invalid type in ml_combine_effects()\n");
322 * Because memoryless devices have only one effect per effect type active
323 * at one time we have to combine multiple effects into one
325 static int ml_get_combo_effect(struct ml_device
*ml
,
326 unsigned long *effect_handled
,
327 struct ff_effect
*combo_effect
)
329 struct ff_effect
*effect
;
330 struct ml_effect_state
*state
;
334 memset(combo_effect
, 0, sizeof(struct ff_effect
));
336 for (i
= 0; i
< FF_MEMLESS_EFFECTS
; i
++) {
337 if (__test_and_set_bit(i
, effect_handled
))
340 state
= &ml
->states
[i
];
341 effect
= state
->effect
;
343 if (!test_bit(FF_EFFECT_STARTED
, &state
->flags
))
346 if (time_before(jiffies
, state
->play_at
))
350 * here we have started effects that are either
351 * currently playing (and may need be aborted)
352 * or need to start playing.
354 effect_type
= get_compatible_type(ml
->dev
->ff
, effect
->type
);
355 if (combo_effect
->type
!= effect_type
) {
356 if (combo_effect
->type
!= 0) {
357 __clear_bit(i
, effect_handled
);
360 combo_effect
->type
= effect_type
;
363 if (__test_and_clear_bit(FF_EFFECT_ABORTING
, &state
->flags
)) {
364 __clear_bit(FF_EFFECT_PLAYING
, &state
->flags
);
365 __clear_bit(FF_EFFECT_STARTED
, &state
->flags
);
366 } else if (effect
->replay
.length
&&
367 time_after_eq(jiffies
, state
->stop_at
)) {
369 __clear_bit(FF_EFFECT_PLAYING
, &state
->flags
);
371 if (--state
->count
<= 0) {
372 __clear_bit(FF_EFFECT_STARTED
, &state
->flags
);
374 state
->play_at
= jiffies
+
375 msecs_to_jiffies(effect
->replay
.delay
);
376 state
->stop_at
= state
->play_at
+
377 msecs_to_jiffies(effect
->replay
.length
);
380 __set_bit(FF_EFFECT_PLAYING
, &state
->flags
);
381 state
->adj_at
= jiffies
;
382 ml_combine_effects(combo_effect
, state
, ml
->gain
);
386 return combo_effect
->type
!= 0;
389 static void ml_play_effects(struct ml_device
*ml
)
391 struct ff_effect effect
;
392 DECLARE_BITMAP(handled_bm
, FF_MEMLESS_EFFECTS
);
394 memset(handled_bm
, 0, sizeof(handled_bm
));
396 while (ml_get_combo_effect(ml
, handled_bm
, &effect
))
397 ml
->play_effect(ml
->dev
, ml
->private, &effect
);
399 ml_schedule_timer(ml
);
402 static void ml_effect_timer(unsigned long timer_data
)
404 struct input_dev
*dev
= (struct input_dev
*)timer_data
;
405 struct ml_device
*ml
= dev
->ff
->private;
408 pr_debug("timer: updating effects\n");
410 spin_lock_irqsave(&dev
->event_lock
, flags
);
412 spin_unlock_irqrestore(&dev
->event_lock
, flags
);
416 * Sets requested gain for FF effects. Called with dev->event_lock held.
418 static void ml_ff_set_gain(struct input_dev
*dev
, u16 gain
)
420 struct ml_device
*ml
= dev
->ff
->private;
425 for (i
= 0; i
< FF_MEMLESS_EFFECTS
; i
++)
426 __clear_bit(FF_EFFECT_PLAYING
, &ml
->states
[i
].flags
);
432 * Start/stop specified FF effect. Called with dev->event_lock held.
434 static int ml_ff_playback(struct input_dev
*dev
, int effect_id
, int value
)
436 struct ml_device
*ml
= dev
->ff
->private;
437 struct ml_effect_state
*state
= &ml
->states
[effect_id
];
440 pr_debug("initiated play\n");
442 __set_bit(FF_EFFECT_STARTED
, &state
->flags
);
443 state
->count
= value
;
444 state
->play_at
= jiffies
+
445 msecs_to_jiffies(state
->effect
->replay
.delay
);
446 state
->stop_at
= state
->play_at
+
447 msecs_to_jiffies(state
->effect
->replay
.length
);
448 state
->adj_at
= state
->play_at
;
451 pr_debug("initiated stop\n");
453 if (test_bit(FF_EFFECT_PLAYING
, &state
->flags
))
454 __set_bit(FF_EFFECT_ABORTING
, &state
->flags
);
456 __clear_bit(FF_EFFECT_STARTED
, &state
->flags
);
464 static int ml_ff_upload(struct input_dev
*dev
,
465 struct ff_effect
*effect
, struct ff_effect
*old
)
467 struct ml_device
*ml
= dev
->ff
->private;
468 struct ml_effect_state
*state
= &ml
->states
[effect
->id
];
470 spin_lock_irq(&dev
->event_lock
);
472 if (test_bit(FF_EFFECT_STARTED
, &state
->flags
)) {
473 __clear_bit(FF_EFFECT_PLAYING
, &state
->flags
);
474 state
->play_at
= jiffies
+
475 msecs_to_jiffies(state
->effect
->replay
.delay
);
476 state
->stop_at
= state
->play_at
+
477 msecs_to_jiffies(state
->effect
->replay
.length
);
478 state
->adj_at
= state
->play_at
;
479 ml_schedule_timer(ml
);
482 spin_unlock_irq(&dev
->event_lock
);
487 static void ml_ff_destroy(struct ff_device
*ff
)
489 struct ml_device
*ml
= ff
->private;
495 * input_ff_create_memless() - create memoryless force-feedback device
496 * @dev: input device supporting force-feedback
497 * @data: driver-specific data to be passed into @play_effect
498 * @play_effect: driver-specific method for playing FF effect
500 int input_ff_create_memless(struct input_dev
*dev
, void *data
,
501 int (*play_effect
)(struct input_dev
*, void *, struct ff_effect
*))
503 struct ml_device
*ml
;
504 struct ff_device
*ff
;
508 ml
= kzalloc(sizeof(struct ml_device
), GFP_KERNEL
);
514 ml
->play_effect
= play_effect
;
516 setup_timer(&ml
->timer
, ml_effect_timer
, (unsigned long)dev
);
518 set_bit(FF_GAIN
, dev
->ffbit
);
520 error
= input_ff_create(dev
, FF_MEMLESS_EFFECTS
);
528 ff
->upload
= ml_ff_upload
;
529 ff
->playback
= ml_ff_playback
;
530 ff
->set_gain
= ml_ff_set_gain
;
531 ff
->destroy
= ml_ff_destroy
;
533 /* we can emulate periodic effects with RUMBLE */
534 if (test_bit(FF_RUMBLE
, ff
->ffbit
)) {
535 set_bit(FF_PERIODIC
, dev
->ffbit
);
536 set_bit(FF_SINE
, dev
->ffbit
);
537 set_bit(FF_TRIANGLE
, dev
->ffbit
);
538 set_bit(FF_SQUARE
, dev
->ffbit
);
541 for (i
= 0; i
< FF_MEMLESS_EFFECTS
; i
++)
542 ml
->states
[i
].effect
= &ff
->effects
[i
];
546 EXPORT_SYMBOL_GPL(input_ff_create_memless
);