1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Force feedback support for memoryless devices 4 * 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 ": " fmt 12 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 21 MODULE_LICENSE("GPL"); 22 MODULE_AUTHOR("Anssi Hannula <anssi.hannula@gmail.com>"); 23 MODULE_DESCRIPTION("Force feedback support for memoryless devices"); 24 25 /* Number of effects handled with memoryless devices */ 26 #define FF_MEMLESS_EFFECTS 16 27 28 /* Envelope update interval in ms */ 29 #define FF_ENVELOPE_INTERVAL 50 30 31 #define FF_EFFECT_STARTED 0 32 #define FF_EFFECT_PLAYING 1 33 #define FF_EFFECT_ABORTING 2 34 35 struct 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 44 struct 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 55 static 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 devices 73 */ 74 static 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 109 static 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 else 130 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 value 148 */ 149 static 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 } else 176 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 */ 195 static 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) for 211 * forward/reverse motor direction (to keep calculation fast & simple). 212 */ 213 static 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 8 226 static inline s16 fixp_new16(s16 a) 227 { 228 return ((s32)a) >> (16 - FRAC_N); 229 } 230 231 static 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 */ 240 static 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 force 259 * If in future any driver wants something else than x and y 260 * in s8, this should be changed to something more generic 261 */ 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 else 283 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 else 304 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 active 321 * at one time we have to combine multiple effects into one 322 */ 323 static 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 either 349 * 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 387 static 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 400 static 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 */ 416 static 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 */ 432 static 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 else 454 __clear_bit(FF_EFFECT_STARTED, &state->flags); 455 } 456 457 ml_play_effects(ml); 458 459 return 0; 460 } 461 462 static 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 485 static 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 down 491 * an input device (via input_device_flush() that calls into 492 * input_ff_flush() that stops and erases all effects), we 493 * do not actually stop the timer, and therefore we should 494 * 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 device 503 * @dev: input device supporting force-feedback 504 * @data: driver-specific data to be passed into @play_effect 505 * @play_effect: driver-specific method for playing FF effect 506 */ 507 int 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 } 553 EXPORT_SYMBOL_GPL(input_ff_create_memless); 554