Root/drivers/input/ff-memless.c

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        case FF_CONSTANT:
78            return &effect->u.constant.envelope;
79        default:
80            return &empty_envelope;
81    }
82}
83
84/*
85 * Check for the next time envelope requires an update on memoryless devices
86 */
87static unsigned long calculate_next_time(struct ml_effect_state *state)
88{
89    const struct ff_envelope *envelope = get_envelope(state->effect);
90    unsigned long attack_stop, fade_start, next_fade;
91
92    if (envelope->attack_length) {
93        attack_stop = state->play_at +
94            msecs_to_jiffies(envelope->attack_length);
95        if (time_before(state->adj_at, attack_stop))
96            return state->adj_at +
97                    msecs_to_jiffies(FF_ENVELOPE_INTERVAL);
98    }
99
100    if (state->effect->replay.length) {
101        if (envelope->fade_length) {
102            /* check when fading should start */
103            fade_start = state->stop_at -
104                    msecs_to_jiffies(envelope->fade_length);
105
106            if (time_before(state->adj_at, fade_start))
107                return fade_start;
108
109            /* already fading, advance to next checkpoint */
110            next_fade = state->adj_at +
111                    msecs_to_jiffies(FF_ENVELOPE_INTERVAL);
112            if (time_before(next_fade, state->stop_at))
113                return next_fade;
114        }
115
116        return state->stop_at;
117    }
118
119    return state->play_at;
120}
121
122static void ml_schedule_timer(struct ml_device *ml)
123{
124    struct ml_effect_state *state;
125    unsigned long now = jiffies;
126    unsigned long earliest = 0;
127    unsigned long next_at;
128    int events = 0;
129    int i;
130
131    pr_debug("calculating next timer\n");
132
133    for (i = 0; i < FF_MEMLESS_EFFECTS; i++) {
134
135        state = &ml->states[i];
136
137        if (!test_bit(FF_EFFECT_STARTED, &state->flags))
138            continue;
139
140        if (test_bit(FF_EFFECT_PLAYING, &state->flags))
141            next_at = calculate_next_time(state);
142        else
143            next_at = state->play_at;
144
145        if (time_before_eq(now, next_at) &&
146            (++events == 1 || time_before(next_at, earliest)))
147            earliest = next_at;
148    }
149
150    if (!events) {
151        pr_debug("no actions\n");
152        del_timer(&ml->timer);
153    } else {
154        pr_debug("timer set\n");
155        mod_timer(&ml->timer, earliest);
156    }
157}
158
159/*
160 * Apply an envelope to a value
161 */
162static int apply_envelope(struct ml_effect_state *state, int value,
163              struct ff_envelope *envelope)
164{
165    struct ff_effect *effect = state->effect;
166    unsigned long now = jiffies;
167    int time_from_level;
168    int time_of_envelope;
169    int envelope_level;
170    int difference;
171
172    if (envelope->attack_length &&
173        time_before(now,
174            state->play_at + msecs_to_jiffies(envelope->attack_length))) {
175        pr_debug("value = 0x%x, attack_level = 0x%x\n",
176             value, envelope->attack_level);
177        time_from_level = jiffies_to_msecs(now - state->play_at);
178        time_of_envelope = envelope->attack_length;
179        envelope_level = min_t(u16, envelope->attack_level, 0x7fff);
180
181    } else if (envelope->fade_length && effect->replay.length &&
182           time_after(now,
183                  state->stop_at - msecs_to_jiffies(envelope->fade_length)) &&
184           time_before(now, state->stop_at)) {
185        time_from_level = jiffies_to_msecs(state->stop_at - now);
186        time_of_envelope = envelope->fade_length;
187        envelope_level = min_t(u16, envelope->fade_level, 0x7fff);
188    } else
189        return value;
190
191    difference = abs(value) - envelope_level;
192
193    pr_debug("difference = %d\n", difference);
194    pr_debug("time_from_level = 0x%x\n", time_from_level);
195    pr_debug("time_of_envelope = 0x%x\n", time_of_envelope);
196
197    difference = difference * time_from_level / time_of_envelope;
198
199    pr_debug("difference = %d\n", difference);
200
201    return value < 0 ?
202        -(difference + envelope_level) : (difference + envelope_level);
203}
204
205/*
206 * Return the type the effect has to be converted into (memless devices)
207 */
208static int get_compatible_type(struct ff_device *ff, int effect_type)
209{
210
211    if (test_bit(effect_type, ff->ffbit))
212        return effect_type;
213
214    if (effect_type == FF_PERIODIC && test_bit(FF_RUMBLE, ff->ffbit))
215        return FF_RUMBLE;
216
217    pr_err("invalid type in get_compatible_type()\n");
218
219    return 0;
220}
221
222/*
223 * Only left/right direction should be used (under/over 0x8000) for
224 * forward/reverse motor direction (to keep calculation fast & simple).
225 */
226static u16 ml_calculate_direction(u16 direction, u16 force,
227                  u16 new_direction, u16 new_force)
228{
229    if (!force)
230        return new_direction;
231    if (!new_force)
232        return direction;
233    return (((u32)(direction >> 1) * force +
234         (new_direction >> 1) * new_force) /
235        (force + new_force)) << 1;
236}
237
238/*
239 * Combine two effects and apply gain.
240 */
241static void ml_combine_effects(struct ff_effect *effect,
242                   struct ml_effect_state *state,
243                   int gain)
244{
245    struct ff_effect *new = state->effect;
246    unsigned int strong, weak, i;
247    int x, y;
248    fixp_t level;
249
250    switch (new->type) {
251    case FF_CONSTANT:
252        i = new->direction * 360 / 0xffff;
253        level = fixp_new16(apply_envelope(state,
254                    new->u.constant.level,
255                    &new->u.constant.envelope));
256        x = fixp_mult(fixp_sin(i), level) * gain / 0xffff;
257        y = fixp_mult(-fixp_cos(i), level) * gain / 0xffff;
258        /*
259         * here we abuse ff_ramp to hold x and y of constant force
260         * If in future any driver wants something else than x and y
261         * in s8, this should be changed to something more generic
262         */
263        effect->u.ramp.start_level =
264            clamp_val(effect->u.ramp.start_level + x, -0x80, 0x7f);
265        effect->u.ramp.end_level =
266            clamp_val(effect->u.ramp.end_level + y, -0x80, 0x7f);
267        break;
268
269    case FF_RUMBLE:
270        strong = (u32)new->u.rumble.strong_magnitude * gain / 0xffff;
271        weak = (u32)new->u.rumble.weak_magnitude * gain / 0xffff;
272
273        if (effect->u.rumble.strong_magnitude + strong)
274            effect->direction = ml_calculate_direction(
275                effect->direction,
276                effect->u.rumble.strong_magnitude,
277                new->direction, strong);
278        else if (effect->u.rumble.weak_magnitude + weak)
279            effect->direction = ml_calculate_direction(
280                effect->direction,
281                effect->u.rumble.weak_magnitude,
282                new->direction, weak);
283        else
284            effect->direction = 0;
285        effect->u.rumble.strong_magnitude =
286            min(strong + effect->u.rumble.strong_magnitude,
287                0xffffU);
288        effect->u.rumble.weak_magnitude =
289            min(weak + effect->u.rumble.weak_magnitude, 0xffffU);
290        break;
291
292    case FF_PERIODIC:
293        i = apply_envelope(state, abs(new->u.periodic.magnitude),
294                   &new->u.periodic.envelope);
295
296        /* here we also scale it 0x7fff => 0xffff */
297        i = i * gain / 0x7fff;
298
299        if (effect->u.rumble.strong_magnitude + i)
300            effect->direction = ml_calculate_direction(
301                effect->direction,
302                effect->u.rumble.strong_magnitude,
303                new->direction, i);
304        else
305            effect->direction = 0;
306        effect->u.rumble.strong_magnitude =
307            min(i + effect->u.rumble.strong_magnitude, 0xffffU);
308        effect->u.rumble.weak_magnitude =
309            min(i + effect->u.rumble.weak_magnitude, 0xffffU);
310        break;
311
312    default:
313        pr_err("invalid type in ml_combine_effects()\n");
314        break;
315    }
316
317}
318
319
320/*
321 * Because memoryless devices have only one effect per effect type active
322 * at one time we have to combine multiple effects into one
323 */
324static int ml_get_combo_effect(struct ml_device *ml,
325                   unsigned long *effect_handled,
326                   struct ff_effect *combo_effect)
327{
328    struct ff_effect *effect;
329    struct ml_effect_state *state;
330    int effect_type;
331    int i;
332
333    memset(combo_effect, 0, sizeof(struct ff_effect));
334
335    for (i = 0; i < FF_MEMLESS_EFFECTS; i++) {
336        if (__test_and_set_bit(i, effect_handled))
337            continue;
338
339        state = &ml->states[i];
340        effect = state->effect;
341
342        if (!test_bit(FF_EFFECT_STARTED, &state->flags))
343            continue;
344
345        if (time_before(jiffies, state->play_at))
346            continue;
347
348        /*
349         * here we have started effects that are either
350         * currently playing (and may need be aborted)
351         * or need to start playing.
352         */
353        effect_type = get_compatible_type(ml->dev->ff, effect->type);
354        if (combo_effect->type != effect_type) {
355            if (combo_effect->type != 0) {
356                __clear_bit(i, effect_handled);
357                continue;
358            }
359            combo_effect->type = effect_type;
360        }
361
362        if (__test_and_clear_bit(FF_EFFECT_ABORTING, &state->flags)) {
363            __clear_bit(FF_EFFECT_PLAYING, &state->flags);
364            __clear_bit(FF_EFFECT_STARTED, &state->flags);
365        } else if (effect->replay.length &&
366               time_after_eq(jiffies, state->stop_at)) {
367
368            __clear_bit(FF_EFFECT_PLAYING, &state->flags);
369
370            if (--state->count <= 0) {
371                __clear_bit(FF_EFFECT_STARTED, &state->flags);
372            } else {
373                state->play_at = jiffies +
374                    msecs_to_jiffies(effect->replay.delay);
375                state->stop_at = state->play_at +
376                    msecs_to_jiffies(effect->replay.length);
377            }
378        } else {
379            __set_bit(FF_EFFECT_PLAYING, &state->flags);
380            state->adj_at = jiffies;
381            ml_combine_effects(combo_effect, state, ml->gain);
382        }
383    }
384
385    return combo_effect->type != 0;
386}
387
388static void ml_play_effects(struct ml_device *ml)
389{
390    struct ff_effect effect;
391    DECLARE_BITMAP(handled_bm, FF_MEMLESS_EFFECTS);
392
393    memset(handled_bm, 0, sizeof(handled_bm));
394
395    while (ml_get_combo_effect(ml, handled_bm, &effect))
396        ml->play_effect(ml->dev, ml->private, &effect);
397
398    ml_schedule_timer(ml);
399}
400
401static void ml_effect_timer(unsigned long timer_data)
402{
403    struct input_dev *dev = (struct input_dev *)timer_data;
404    struct ml_device *ml = dev->ff->private;
405    unsigned long flags;
406
407    pr_debug("timer: updating effects\n");
408
409    spin_lock_irqsave(&dev->event_lock, flags);
410    ml_play_effects(ml);
411    spin_unlock_irqrestore(&dev->event_lock, flags);
412}
413
414/*
415 * Sets requested gain for FF effects. Called with dev->event_lock held.
416 */
417static void ml_ff_set_gain(struct input_dev *dev, u16 gain)
418{
419    struct ml_device *ml = dev->ff->private;
420    int i;
421
422    ml->gain = gain;
423
424    for (i = 0; i < FF_MEMLESS_EFFECTS; i++)
425        __clear_bit(FF_EFFECT_PLAYING, &ml->states[i].flags);
426
427    ml_play_effects(ml);
428}
429
430/*
431 * Start/stop specified FF effect. Called with dev->event_lock held.
432 */
433static int ml_ff_playback(struct input_dev *dev, int effect_id, int value)
434{
435    struct ml_device *ml = dev->ff->private;
436    struct ml_effect_state *state = &ml->states[effect_id];
437
438    if (value > 0) {
439        pr_debug("initiated play\n");
440
441        __set_bit(FF_EFFECT_STARTED, &state->flags);
442        state->count = value;
443        state->play_at = jiffies +
444                 msecs_to_jiffies(state->effect->replay.delay);
445        state->stop_at = state->play_at +
446                 msecs_to_jiffies(state->effect->replay.length);
447        state->adj_at = state->play_at;
448
449    } else {
450        pr_debug("initiated stop\n");
451
452        if (test_bit(FF_EFFECT_PLAYING, &state->flags))
453            __set_bit(FF_EFFECT_ABORTING, &state->flags);
454        else
455            __clear_bit(FF_EFFECT_STARTED, &state->flags);
456    }
457
458    ml_play_effects(ml);
459
460    return 0;
461}
462
463static int ml_ff_upload(struct input_dev *dev,
464            struct ff_effect *effect, struct ff_effect *old)
465{
466    struct ml_device *ml = dev->ff->private;
467    struct ml_effect_state *state = &ml->states[effect->id];
468
469    spin_lock_irq(&dev->event_lock);
470
471    if (test_bit(FF_EFFECT_STARTED, &state->flags)) {
472        __clear_bit(FF_EFFECT_PLAYING, &state->flags);
473        state->play_at = jiffies +
474                 msecs_to_jiffies(state->effect->replay.delay);
475        state->stop_at = state->play_at +
476                 msecs_to_jiffies(state->effect->replay.length);
477        state->adj_at = state->play_at;
478        ml_schedule_timer(ml);
479    }
480
481    spin_unlock_irq(&dev->event_lock);
482
483    return 0;
484}
485
486static void ml_ff_destroy(struct ff_device *ff)
487{
488    struct ml_device *ml = ff->private;
489
490    kfree(ml->private);
491}
492
493/**
494 * input_ff_create_memless() - create memoryless force-feedback device
495 * @dev: input device supporting force-feedback
496 * @data: driver-specific data to be passed into @play_effect
497 * @play_effect: driver-specific method for playing FF effect
498 */
499int input_ff_create_memless(struct input_dev *dev, void *data,
500        int (*play_effect)(struct input_dev *, void *, struct ff_effect *))
501{
502    struct ml_device *ml;
503    struct ff_device *ff;
504    int error;
505    int i;
506
507    ml = kzalloc(sizeof(struct ml_device), GFP_KERNEL);
508    if (!ml)
509        return -ENOMEM;
510
511    ml->dev = dev;
512    ml->private = data;
513    ml->play_effect = play_effect;
514    ml->gain = 0xffff;
515    setup_timer(&ml->timer, ml_effect_timer, (unsigned long)dev);
516
517    set_bit(FF_GAIN, dev->ffbit);
518
519    error = input_ff_create(dev, FF_MEMLESS_EFFECTS);
520    if (error) {
521        kfree(ml);
522        return error;
523    }
524
525    ff = dev->ff;
526    ff->private = ml;
527    ff->upload = ml_ff_upload;
528    ff->playback = ml_ff_playback;
529    ff->set_gain = ml_ff_set_gain;
530    ff->destroy = ml_ff_destroy;
531
532    /* we can emulate periodic effects with RUMBLE */
533    if (test_bit(FF_RUMBLE, ff->ffbit)) {
534        set_bit(FF_PERIODIC, dev->ffbit);
535        set_bit(FF_SINE, dev->ffbit);
536        set_bit(FF_TRIANGLE, dev->ffbit);
537        set_bit(FF_SQUARE, dev->ffbit);
538    }
539
540    for (i = 0; i < FF_MEMLESS_EFFECTS; i++)
541        ml->states[i].effect = &ff->effects[i];
542
543    return 0;
544}
545EXPORT_SYMBOL_GPL(input_ff_create_memless);
546

Archive Download this file

Branches:
ben-wpan
ben-wpan-stefan
javiroman/ks7010
jz-2.6.34
jz-2.6.34-rc5
jz-2.6.34-rc6
jz-2.6.34-rc7
jz-2.6.35
jz-2.6.36
jz-2.6.37
jz-2.6.38
jz-2.6.39
jz-3.0
jz-3.1
jz-3.11
jz-3.12
jz-3.13
jz-3.15
jz-3.16
jz-3.18-dt
jz-3.2
jz-3.3
jz-3.4
jz-3.5
jz-3.6
jz-3.6-rc2-pwm
jz-3.9
jz-3.9-clk
jz-3.9-rc8
jz47xx
jz47xx-2.6.38
master

Tags:
od-2011-09-04
od-2011-09-18
v2.6.34-rc5
v2.6.34-rc6
v2.6.34-rc7
v3.9



interactive