1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
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
41#define FF_MEMLESS_EFFECTS 16
42
43
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;
53 int count;
54 unsigned long play_at;
55 unsigned long stop_at;
56 unsigned long adj_at;
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
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
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
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
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
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
226
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
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
274
275
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
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
336
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
364
365
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
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
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
509
510
511
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
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