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/export.h> 14 #include <linux/slab.h> 15 #include <linux/input.h> 16 #include <linux/module.h> 17 #include <linux/mutex.h> 18 #include <linux/spinlock.h> 19 #include <linux/jiffies.h> 20 #include <linux/fixp-arith.h> 21 22 MODULE_LICENSE("GPL"); 23 MODULE_AUTHOR("Anssi Hannula <anssi.hannula@gmail.com>"); 24 MODULE_DESCRIPTION("Force feedback support for memoryless devices"); 25 26 /* Number of effects handled with memoryless devices */ 27 #define FF_MEMLESS_EFFECTS 16 28 29 /* Envelope update interval in ms */ 30 #define FF_ENVELOPE_INTERVAL 50 31 32 #define FF_EFFECT_STARTED 0 33 #define FF_EFFECT_PLAYING 1 34 #define FF_EFFECT_ABORTING 2 35 36 struct ml_effect_state { 37 struct ff_effect *effect; 38 unsigned long flags; /* effect state (STARTED, PLAYING, etc) */ 39 int count; /* loop count of the effect */ 40 unsigned long play_at; /* start time */ 41 unsigned long stop_at; /* stop time */ 42 unsigned long adj_at; /* last time the effect was sent */ 43 }; 44 45 struct ml_device { 46 void *private; 47 struct ml_effect_state states[FF_MEMLESS_EFFECTS]; 48 int gain; 49 struct timer_list timer; 50 struct input_dev *dev; 51 52 int (*play_effect)(struct input_dev *dev, void *data, 53 struct ff_effect *effect); 54 }; 55 56 static const struct ff_envelope *get_envelope(const struct ff_effect *effect) 57 { 58 static const struct ff_envelope empty_envelope; 59 60 switch (effect->type) { 61 case FF_PERIODIC: 62 return &effect->u.periodic.envelope; 63 64 case FF_CONSTANT: 65 return &effect->u.constant.envelope; 66 67 default: 68 return &empty_envelope; 69 } 70 } 71 72 /* 73 * Check for the next time envelope requires an update on memoryless devices 74 */ 75 static unsigned long calculate_next_time(struct ml_effect_state *state) 76 { 77 const struct ff_envelope *envelope = get_envelope(state->effect); 78 unsigned long attack_stop, fade_start, next_fade; 79 80 if (envelope->attack_length) { 81 attack_stop = state->play_at + 82 msecs_to_jiffies(envelope->attack_length); 83 if (time_before(state->adj_at, attack_stop)) 84 return state->adj_at + 85 msecs_to_jiffies(FF_ENVELOPE_INTERVAL); 86 } 87 88 if (state->effect->replay.length) { 89 if (envelope->fade_length) { 90 /* check when fading should start */ 91 fade_start = state->stop_at - 92 msecs_to_jiffies(envelope->fade_length); 93 94 if (time_before(state->adj_at, fade_start)) 95 return fade_start; 96 97 /* already fading, advance to next checkpoint */ 98 next_fade = state->adj_at + 99 msecs_to_jiffies(FF_ENVELOPE_INTERVAL); 100 if (time_before(next_fade, state->stop_at)) 101 return next_fade; 102 } 103 104 return state->stop_at; 105 } 106 107 return state->play_at; 108 } 109 110 static void ml_schedule_timer(struct ml_device *ml) 111 { 112 struct ml_effect_state *state; 113 unsigned long now = jiffies; 114 unsigned long earliest = 0; 115 unsigned long next_at; 116 int events = 0; 117 int i; 118 119 pr_debug("calculating next timer\n"); 120 121 for (i = 0; i < FF_MEMLESS_EFFECTS; i++) { 122 123 state = &ml->states[i]; 124 125 if (!test_bit(FF_EFFECT_STARTED, &state->flags)) 126 continue; 127 128 if (test_bit(FF_EFFECT_PLAYING, &state->flags)) 129 next_at = calculate_next_time(state); 130 else 131 next_at = state->play_at; 132 133 if (time_before_eq(now, next_at) && 134 (++events == 1 || time_before(next_at, earliest))) 135 earliest = next_at; 136 } 137 138 if (!events) { 139 pr_debug("no actions\n"); 140 timer_delete(&ml->timer); 141 } else { 142 pr_debug("timer set\n"); 143 mod_timer(&ml->timer, earliest); 144 } 145 } 146 147 /* 148 * Apply an envelope to a value 149 */ 150 static int apply_envelope(struct ml_effect_state *state, int value, 151 struct ff_envelope *envelope) 152 { 153 struct ff_effect *effect = state->effect; 154 unsigned long now = jiffies; 155 int time_from_level; 156 int time_of_envelope; 157 int envelope_level; 158 int difference; 159 160 if (envelope->attack_length && 161 time_before(now, 162 state->play_at + msecs_to_jiffies(envelope->attack_length))) { 163 pr_debug("value = 0x%x, attack_level = 0x%x\n", 164 value, envelope->attack_level); 165 time_from_level = jiffies_to_msecs(now - state->play_at); 166 time_of_envelope = envelope->attack_length; 167 envelope_level = min_t(u16, envelope->attack_level, 0x7fff); 168 169 } else if (envelope->fade_length && effect->replay.length && 170 time_after(now, 171 state->stop_at - msecs_to_jiffies(envelope->fade_length)) && 172 time_before(now, state->stop_at)) { 173 time_from_level = jiffies_to_msecs(state->stop_at - now); 174 time_of_envelope = envelope->fade_length; 175 envelope_level = min_t(u16, envelope->fade_level, 0x7fff); 176 } else 177 return value; 178 179 difference = abs(value) - envelope_level; 180 181 pr_debug("difference = %d\n", difference); 182 pr_debug("time_from_level = 0x%x\n", time_from_level); 183 pr_debug("time_of_envelope = 0x%x\n", time_of_envelope); 184 185 difference = difference * time_from_level / time_of_envelope; 186 187 pr_debug("difference = %d\n", difference); 188 189 return value < 0 ? 190 -(difference + envelope_level) : (difference + envelope_level); 191 } 192 193 /* 194 * Return the type the effect has to be converted into (memless devices) 195 */ 196 static int get_compatible_type(struct ff_device *ff, int effect_type) 197 { 198 199 if (test_bit(effect_type, ff->ffbit)) 200 return effect_type; 201 202 if (effect_type == FF_PERIODIC && test_bit(FF_RUMBLE, ff->ffbit)) 203 return FF_RUMBLE; 204 205 pr_err("invalid type in get_compatible_type()\n"); 206 207 return 0; 208 } 209 210 /* 211 * Only left/right direction should be used (under/over 0x8000) for 212 * forward/reverse motor direction (to keep calculation fast & simple). 213 */ 214 static u16 ml_calculate_direction(u16 direction, u16 force, 215 u16 new_direction, u16 new_force) 216 { 217 if (!force) 218 return new_direction; 219 if (!new_force) 220 return direction; 221 return (((u32)(direction >> 1) * force + 222 (new_direction >> 1) * new_force) / 223 (force + new_force)) << 1; 224 } 225 226 #define FRAC_N 8 227 static inline s16 fixp_new16(s16 a) 228 { 229 return ((s32)a) >> (16 - FRAC_N); 230 } 231 232 static inline s16 fixp_mult(s16 a, s16 b) 233 { 234 a = ((s32)a * 0x100) / 0x7fff; 235 return ((s32)(a * b)) >> FRAC_N; 236 } 237 238 /* 239 * Combine two effects and apply gain. 240 */ 241 static 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 s16 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_sin16(i), level) * gain / 0xffff; 257 y = fixp_mult(-fixp_cos16(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 */ 324 static 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 388 static 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 401 static void ml_effect_timer(struct timer_list *t) 402 { 403 struct ml_device *ml = timer_container_of(ml, t, timer); 404 struct input_dev *dev = ml->dev; 405 406 pr_debug("timer: updating effects\n"); 407 408 guard(spinlock_irqsave)(&dev->event_lock); 409 ml_play_effects(ml); 410 } 411 412 /* 413 * Sets requested gain for FF effects. Called with dev->event_lock held. 414 */ 415 static void ml_ff_set_gain(struct input_dev *dev, u16 gain) 416 { 417 struct ml_device *ml = dev->ff->private; 418 int i; 419 420 ml->gain = gain; 421 422 for (i = 0; i < FF_MEMLESS_EFFECTS; i++) 423 __clear_bit(FF_EFFECT_PLAYING, &ml->states[i].flags); 424 425 ml_play_effects(ml); 426 } 427 428 /* 429 * Start/stop specified FF effect. Called with dev->event_lock held. 430 */ 431 static int ml_ff_playback(struct input_dev *dev, int effect_id, int value) 432 { 433 struct ml_device *ml = dev->ff->private; 434 struct ml_effect_state *state = &ml->states[effect_id]; 435 436 if (value > 0) { 437 pr_debug("initiated play\n"); 438 439 __set_bit(FF_EFFECT_STARTED, &state->flags); 440 state->count = value; 441 state->play_at = jiffies + 442 msecs_to_jiffies(state->effect->replay.delay); 443 state->stop_at = state->play_at + 444 msecs_to_jiffies(state->effect->replay.length); 445 state->adj_at = state->play_at; 446 447 } else { 448 pr_debug("initiated stop\n"); 449 450 if (test_bit(FF_EFFECT_PLAYING, &state->flags)) 451 __set_bit(FF_EFFECT_ABORTING, &state->flags); 452 else 453 __clear_bit(FF_EFFECT_STARTED, &state->flags); 454 } 455 456 ml_play_effects(ml); 457 458 return 0; 459 } 460 461 static int ml_ff_upload(struct input_dev *dev, 462 struct ff_effect *effect, struct ff_effect *old) 463 { 464 struct ml_device *ml = dev->ff->private; 465 struct ml_effect_state *state = &ml->states[effect->id]; 466 467 guard(spinlock_irq)(&dev->event_lock); 468 469 if (test_bit(FF_EFFECT_STARTED, &state->flags)) { 470 __clear_bit(FF_EFFECT_PLAYING, &state->flags); 471 state->play_at = jiffies + 472 msecs_to_jiffies(state->effect->replay.delay); 473 state->stop_at = state->play_at + 474 msecs_to_jiffies(state->effect->replay.length); 475 state->adj_at = state->play_at; 476 ml_schedule_timer(ml); 477 } 478 479 return 0; 480 } 481 482 static void ml_ff_destroy(struct ff_device *ff) 483 { 484 struct ml_device *ml = ff->private; 485 486 /* 487 * Even though we stop all playing effects when tearing down 488 * an input device (via input_device_flush() that calls into 489 * input_ff_flush() that stops and erases all effects), we 490 * do not actually stop the timer, and therefore we should 491 * do it here. 492 */ 493 timer_delete_sync(&ml->timer); 494 495 kfree(ml->private); 496 } 497 498 /** 499 * input_ff_create_memless() - create memoryless force-feedback device 500 * @dev: input device supporting force-feedback 501 * @data: driver-specific data to be passed into @play_effect 502 * @play_effect: driver-specific method for playing FF effect 503 */ 504 int input_ff_create_memless(struct input_dev *dev, void *data, 505 int (*play_effect)(struct input_dev *, void *, struct ff_effect *)) 506 { 507 struct ff_device *ff; 508 int error; 509 int i; 510 511 struct ml_device *ml __free(kfree) = kzalloc(sizeof(*ml), GFP_KERNEL); 512 if (!ml) 513 return -ENOMEM; 514 515 ml->dev = dev; 516 ml->private = data; 517 ml->play_effect = play_effect; 518 ml->gain = 0xffff; 519 timer_setup(&ml->timer, ml_effect_timer, 0); 520 521 set_bit(FF_GAIN, dev->ffbit); 522 523 error = input_ff_create(dev, FF_MEMLESS_EFFECTS); 524 if (error) 525 return error; 526 527 ff = dev->ff; 528 ff->upload = ml_ff_upload; 529 ff->playback = ml_ff_playback; 530 ff->set_gain = ml_ff_set_gain; 531 ff->destroy = ml_ff_destroy; 532 533 /* we can emulate periodic effects with RUMBLE */ 534 if (test_bit(FF_RUMBLE, ff->ffbit)) { 535 set_bit(FF_PERIODIC, dev->ffbit); 536 set_bit(FF_SINE, dev->ffbit); 537 set_bit(FF_TRIANGLE, dev->ffbit); 538 set_bit(FF_SQUARE, dev->ffbit); 539 } 540 541 for (i = 0; i < FF_MEMLESS_EFFECTS; i++) 542 ml->states[i].effect = &ff->effects[i]; 543 544 ff->private = no_free_ptr(ml); 545 546 return 0; 547 } 548 EXPORT_SYMBOL_GPL(input_ff_create_memless); 549