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
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/*
241 * Combine two effects and apply gain.
242 */
243static void ml_combine_effects(struct ff_effect *effect,
244                   struct ml_effect_state *state,
245                   int gain)
246{
247    struct ff_effect *new = state->effect;
248    unsigned int strong, weak, i;
249    int x, y;
250    fixp_t level;
251
252    switch (new->type) {
253    case FF_CONSTANT:
254        i = new->direction * 360 / 0xffff;
255        level = fixp_new16(apply_envelope(state,
256                    new->u.constant.level,
257                    &new->u.constant.envelope));
258        x = fixp_mult(fixp_sin(i), level) * gain / 0xffff;
259        y = fixp_mult(-fixp_cos(i), level) * gain / 0xffff;
260        /*
261         * here we abuse ff_ramp to hold x and y of constant force
262         * If in future any driver wants something else than x and y
263         * in s8, this should be changed to something more generic
264         */
265        effect->u.ramp.start_level =
266            clamp_val(effect->u.ramp.start_level + x, -0x80, 0x7f);
267        effect->u.ramp.end_level =
268            clamp_val(effect->u.ramp.end_level + y, -0x80, 0x7f);
269        break;
270
271    case FF_RUMBLE:
272        strong = (u32)new->u.rumble.strong_magnitude * gain / 0xffff;
273        weak = (u32)new->u.rumble.weak_magnitude * gain / 0xffff;
274
275        if (effect->u.rumble.strong_magnitude + strong)
276            effect->direction = ml_calculate_direction(
277                effect->direction,
278                effect->u.rumble.strong_magnitude,
279                new->direction, strong);
280        else if (effect->u.rumble.weak_magnitude + weak)
281            effect->direction = ml_calculate_direction(
282                effect->direction,
283                effect->u.rumble.weak_magnitude,
284                new->direction, weak);
285        else
286            effect->direction = 0;
287        effect->u.rumble.strong_magnitude =
288            min(strong + effect->u.rumble.strong_magnitude,
289                0xffffU);
290        effect->u.rumble.weak_magnitude =
291            min(weak + effect->u.rumble.weak_magnitude, 0xffffU);
292        break;
293
294    case FF_PERIODIC:
295        i = apply_envelope(state, abs(new->u.periodic.magnitude),
296                   &new->u.periodic.envelope);
297
298        /* here we also scale it 0x7fff => 0xffff */
299        i = i * gain / 0x7fff;
300
301        if (effect->u.rumble.strong_magnitude + i)
302            effect->direction = ml_calculate_direction(
303                effect->direction,
304                effect->u.rumble.strong_magnitude,
305                new->direction, i);
306        else
307            effect->direction = 0;
308        effect->u.rumble.strong_magnitude =
309            min(i + effect->u.rumble.strong_magnitude, 0xffffU);
310        effect->u.rumble.weak_magnitude =
311            min(i + effect->u.rumble.weak_magnitude, 0xffffU);
312        break;
313
314    default:
315        pr_err("invalid type in ml_combine_effects()\n");
316        break;
317    }
318
319}
320
321
322/*
323 * Because memoryless devices have only one effect per effect type active
324 * at one time we have to combine multiple effects into one
325 */
326static int ml_get_combo_effect(struct ml_device *ml,
327                   unsigned long *effect_handled,
328                   struct ff_effect *combo_effect)
329{
330    struct ff_effect *effect;
331    struct ml_effect_state *state;
332    int effect_type;
333    int i;
334
335    memset(combo_effect, 0, sizeof(struct ff_effect));
336
337    for (i = 0; i < FF_MEMLESS_EFFECTS; i++) {
338        if (__test_and_set_bit(i, effect_handled))
339            continue;
340
341        state = &ml->states[i];
342        effect = state->effect;
343
344        if (!test_bit(FF_EFFECT_STARTED, &state->flags))
345            continue;
346
347        if (time_before(jiffies, state->play_at))
348            continue;
349
350        /*
351         * here we have started effects that are either
352         * currently playing (and may need be aborted)
353         * or need to start playing.
354         */
355        effect_type = get_compatible_type(ml->dev->ff, effect->type);
356        if (combo_effect->type != effect_type) {
357            if (combo_effect->type != 0) {
358                __clear_bit(i, effect_handled);
359                continue;
360            }
361            combo_effect->type = effect_type;
362        }
363
364        if (__test_and_clear_bit(FF_EFFECT_ABORTING, &state->flags)) {
365            __clear_bit(FF_EFFECT_PLAYING, &state->flags);
366            __clear_bit(FF_EFFECT_STARTED, &state->flags);
367        } else if (effect->replay.length &&
368               time_after_eq(jiffies, state->stop_at)) {
369
370            __clear_bit(FF_EFFECT_PLAYING, &state->flags);
371
372            if (--state->count <= 0) {
373                __clear_bit(FF_EFFECT_STARTED, &state->flags);
374            } else {
375                state->play_at = jiffies +
376                    msecs_to_jiffies(effect->replay.delay);
377                state->stop_at = state->play_at +
378                    msecs_to_jiffies(effect->replay.length);
379            }
380        } else {
381            __set_bit(FF_EFFECT_PLAYING, &state->flags);
382            state->adj_at = jiffies;
383            ml_combine_effects(combo_effect, state, ml->gain);
384        }
385    }
386
387    return combo_effect->type != 0;
388}
389
390static void ml_play_effects(struct ml_device *ml)
391{
392    struct ff_effect effect;
393    DECLARE_BITMAP(handled_bm, FF_MEMLESS_EFFECTS);
394
395    memset(handled_bm, 0, sizeof(handled_bm));
396
397    while (ml_get_combo_effect(ml, handled_bm, &effect))
398        ml->play_effect(ml->dev, ml->private, &effect);
399
400    ml_schedule_timer(ml);
401}
402
403static void ml_effect_timer(unsigned long timer_data)
404{
405    struct input_dev *dev = (struct input_dev *)timer_data;
406    struct ml_device *ml = dev->ff->private;
407    unsigned long flags;
408
409    pr_debug("timer: updating effects\n");
410
411    spin_lock_irqsave(&dev->event_lock, flags);
412    ml_play_effects(ml);
413    spin_unlock_irqrestore(&dev->event_lock, flags);
414}
415
416/*
417 * Sets requested gain for FF effects. Called with dev->event_lock held.
418 */
419static void ml_ff_set_gain(struct input_dev *dev, u16 gain)
420{
421    struct ml_device *ml = dev->ff->private;
422    int i;
423
424    ml->gain = gain;
425
426    for (i = 0; i < FF_MEMLESS_EFFECTS; i++)
427        __clear_bit(FF_EFFECT_PLAYING, &ml->states[i].flags);
428
429    ml_play_effects(ml);
430}
431
432/*
433 * Start/stop specified FF effect. Called with dev->event_lock held.
434 */
435static int ml_ff_playback(struct input_dev *dev, int effect_id, int value)
436{
437    struct ml_device *ml = dev->ff->private;
438    struct ml_effect_state *state = &ml->states[effect_id];
439
440    if (value > 0) {
441        pr_debug("initiated play\n");
442
443        __set_bit(FF_EFFECT_STARTED, &state->flags);
444        state->count = value;
445        state->play_at = jiffies +
446                 msecs_to_jiffies(state->effect->replay.delay);
447        state->stop_at = state->play_at +
448                 msecs_to_jiffies(state->effect->replay.length);
449        state->adj_at = state->play_at;
450
451    } else {
452        pr_debug("initiated stop\n");
453
454        if (test_bit(FF_EFFECT_PLAYING, &state->flags))
455            __set_bit(FF_EFFECT_ABORTING, &state->flags);
456        else
457            __clear_bit(FF_EFFECT_STARTED, &state->flags);
458    }
459
460    ml_play_effects(ml);
461
462    return 0;
463}
464
465static int ml_ff_upload(struct input_dev *dev,
466            struct ff_effect *effect, struct ff_effect *old)
467{
468    struct ml_device *ml = dev->ff->private;
469    struct ml_effect_state *state = &ml->states[effect->id];
470
471    spin_lock_irq(&dev->event_lock);
472
473    if (test_bit(FF_EFFECT_STARTED, &state->flags)) {
474        __clear_bit(FF_EFFECT_PLAYING, &state->flags);
475        state->play_at = jiffies +
476                 msecs_to_jiffies(state->effect->replay.delay);
477        state->stop_at = state->play_at +
478                 msecs_to_jiffies(state->effect->replay.length);
479        state->adj_at = state->play_at;
480        ml_schedule_timer(ml);
481    }
482
483    spin_unlock_irq(&dev->event_lock);
484
485    return 0;
486}
487
488static void ml_ff_destroy(struct ff_device *ff)
489{
490    struct ml_device *ml = ff->private;
491
492    kfree(ml->private);
493}
494
495/**
496 * input_ff_create_memless() - create memoryless force-feedback device
497 * @dev: input device supporting force-feedback
498 * @data: driver-specific data to be passed into @play_effect
499 * @play_effect: driver-specific method for playing FF effect
500 */
501int input_ff_create_memless(struct input_dev *dev, void *data,
502        int (*play_effect)(struct input_dev *, void *, struct ff_effect *))
503{
504    struct ml_device *ml;
505    struct ff_device *ff;
506    int error;
507    int i;
508
509    ml = kzalloc(sizeof(struct ml_device), GFP_KERNEL);
510    if (!ml)
511        return -ENOMEM;
512
513    ml->dev = dev;
514    ml->private = data;
515    ml->play_effect = play_effect;
516    ml->gain = 0xffff;
517    setup_timer(&ml->timer, ml_effect_timer, (unsigned long)dev);
518
519    set_bit(FF_GAIN, dev->ffbit);
520
521    error = input_ff_create(dev, FF_MEMLESS_EFFECTS);
522    if (error) {
523        kfree(ml);
524        return error;
525    }
526
527    ff = dev->ff;
528    ff->private = ml;
529    ff->upload = ml_ff_upload;
530    ff->playback = ml_ff_playback;
531    ff->set_gain = ml_ff_set_gain;
532    ff->destroy = ml_ff_destroy;
533
534    /* we can emulate periodic effects with RUMBLE */
535    if (test_bit(FF_RUMBLE, ff->ffbit)) {
536        set_bit(FF_PERIODIC, dev->ffbit);
537        set_bit(FF_SINE, dev->ffbit);
538        set_bit(FF_TRIANGLE, dev->ffbit);
539        set_bit(FF_SQUARE, dev->ffbit);
540    }
541
542    for (i = 0; i < FF_MEMLESS_EFFECTS; i++)
543        ml->states[i].effect = &ff->effects[i];
544
545    return 0;
546}
547EXPORT_SYMBOL_GPL(input_ff_create_memless);
548

Archive Download this file



interactive