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