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 /* 10 */ 11 12 /* #define DEBUG */ 13 14 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt 15 16 #include <linux/slab.h> 17 #include <linux/input.h> 18 #include <linux/module.h> 19 #include <linux/mutex.h> 20 #include <linux/spinlock.h> 21 #include <linux/jiffies.h> 22 #include <linux/fixp-arith.h> 23 24 MODULE_LICENSE("GPL"); 25 MODULE_AUTHOR("Anssi Hannula <anssi.hannula@gmail.com>"); 26 MODULE_DESCRIPTION("Force feedback support for memoryless devices"); 27 28 /* Number of effects handled with memoryless devices */ 29 #define FF_MEMLESS_EFFECTS 16 30 31 /* Envelope update interval in ms */ 32 #define FF_ENVELOPE_INTERVAL 50 33 34 #define FF_EFFECT_STARTED 0 35 #define FF_EFFECT_PLAYING 1 36 #define FF_EFFECT_ABORTING 2 37 38 struct ml_effect_state { 39 struct ff_effect *effect; 40 unsigned long flags; /* effect state (STARTED, PLAYING, etc) */ 41 int count; /* loop count of the effect */ 42 unsigned long play_at; /* start time */ 43 unsigned long stop_at; /* stop time */ 44 unsigned long adj_at; /* last time the effect was sent */ 45 }; 46 47 struct ml_device { 48 void *private; 49 struct ml_effect_state states[FF_MEMLESS_EFFECTS]; 50 int gain; 51 struct timer_list timer; 52 struct input_dev *dev; 53 54 int (*play_effect)(struct input_dev *dev, void *data, 55 struct ff_effect *effect); 56 }; 57 58 static const struct ff_envelope *get_envelope(const struct ff_effect *effect) 59 { 60 static const struct ff_envelope empty_envelope; 61 62 switch (effect->type) { 63 case FF_PERIODIC: 64 return &effect->u.periodic.envelope; 65 66 case FF_CONSTANT: 67 return &effect->u.constant.envelope; 68 69 default: 70 return &empty_envelope; 71 } 72 } 73 74 /* 75 * Check for the next time envelope requires an update on memoryless devices 76 */ 77 static unsigned long calculate_next_time(struct ml_effect_state *state) 78 { 79 const struct ff_envelope *envelope = get_envelope(state->effect); 80 unsigned long attack_stop, fade_start, next_fade; 81 82 if (envelope->attack_length) { 83 attack_stop = state->play_at + 84 msecs_to_jiffies(envelope->attack_length); 85 if (time_before(state->adj_at, attack_stop)) 86 return state->adj_at + 87 msecs_to_jiffies(FF_ENVELOPE_INTERVAL); 88 } 89 90 if (state->effect->replay.length) { 91 if (envelope->fade_length) { 92 /* check when fading should start */ 93 fade_start = state->stop_at - 94 msecs_to_jiffies(envelope->fade_length); 95 96 if (time_before(state->adj_at, fade_start)) 97 return fade_start; 98 99 /* already fading, advance to next checkpoint */ 100 next_fade = state->adj_at + 101 msecs_to_jiffies(FF_ENVELOPE_INTERVAL); 102 if (time_before(next_fade, state->stop_at)) 103 return next_fade; 104 } 105 106 return state->stop_at; 107 } 108 109 return state->play_at; 110 } 111 112 static void ml_schedule_timer(struct ml_device *ml) 113 { 114 struct ml_effect_state *state; 115 unsigned long now = jiffies; 116 unsigned long earliest = 0; 117 unsigned long next_at; 118 int events = 0; 119 int i; 120 121 pr_debug("calculating next timer\n"); 122 123 for (i = 0; i < FF_MEMLESS_EFFECTS; i++) { 124 125 state = &ml->states[i]; 126 127 if (!test_bit(FF_EFFECT_STARTED, &state->flags)) 128 continue; 129 130 if (test_bit(FF_EFFECT_PLAYING, &state->flags)) 131 next_at = calculate_next_time(state); 132 else 133 next_at = state->play_at; 134 135 if (time_before_eq(now, next_at) && 136 (++events == 1 || time_before(next_at, earliest))) 137 earliest = next_at; 138 } 139 140 if (!events) { 141 pr_debug("no actions\n"); 142 del_timer(&ml->timer); 143 } else { 144 pr_debug("timer set\n"); 145 mod_timer(&ml->timer, earliest); 146 } 147 } 148 149 /* 150 * Apply an envelope to a value 151 */ 152 static int apply_envelope(struct ml_effect_state *state, int value, 153 struct ff_envelope *envelope) 154 { 155 struct ff_effect *effect = state->effect; 156 unsigned long now = jiffies; 157 int time_from_level; 158 int time_of_envelope; 159 int envelope_level; 160 int difference; 161 162 if (envelope->attack_length && 163 time_before(now, 164 state->play_at + msecs_to_jiffies(envelope->attack_length))) { 165 pr_debug("value = 0x%x, attack_level = 0x%x\n", 166 value, envelope->attack_level); 167 time_from_level = jiffies_to_msecs(now - state->play_at); 168 time_of_envelope = envelope->attack_length; 169 envelope_level = min_t(u16, envelope->attack_level, 0x7fff); 170 171 } else if (envelope->fade_length && effect->replay.length && 172 time_after(now, 173 state->stop_at - msecs_to_jiffies(envelope->fade_length)) && 174 time_before(now, state->stop_at)) { 175 time_from_level = jiffies_to_msecs(state->stop_at - now); 176 time_of_envelope = envelope->fade_length; 177 envelope_level = min_t(u16, envelope->fade_level, 0x7fff); 178 } else 179 return value; 180 181 difference = abs(value) - envelope_level; 182 183 pr_debug("difference = %d\n", difference); 184 pr_debug("time_from_level = 0x%x\n", time_from_level); 185 pr_debug("time_of_envelope = 0x%x\n", time_of_envelope); 186 187 difference = difference * time_from_level / time_of_envelope; 188 189 pr_debug("difference = %d\n", difference); 190 191 return value < 0 ? 192 -(difference + envelope_level) : (difference + envelope_level); 193 } 194 195 /* 196 * Return the type the effect has to be converted into (memless devices) 197 */ 198 static int get_compatible_type(struct ff_device *ff, int effect_type) 199 { 200 201 if (test_bit(effect_type, ff->ffbit)) 202 return effect_type; 203 204 if (effect_type == FF_PERIODIC && test_bit(FF_RUMBLE, ff->ffbit)) 205 return FF_RUMBLE; 206 207 pr_err("invalid type in get_compatible_type()\n"); 208 209 return 0; 210 } 211 212 /* 213 * Only left/right direction should be used (under/over 0x8000) for 214 * forward/reverse motor direction (to keep calculation fast & simple). 215 */ 216 static u16 ml_calculate_direction(u16 direction, u16 force, 217 u16 new_direction, u16 new_force) 218 { 219 if (!force) 220 return new_direction; 221 if (!new_force) 222 return direction; 223 return (((u32)(direction >> 1) * force + 224 (new_direction >> 1) * new_force) / 225 (force + new_force)) << 1; 226 } 227 228 #define FRAC_N 8 229 static inline s16 fixp_new16(s16 a) 230 { 231 return ((s32)a) >> (16 - FRAC_N); 232 } 233 234 static inline s16 fixp_mult(s16 a, s16 b) 235 { 236 a = ((s32)a * 0x100) / 0x7fff; 237 return ((s32)(a * b)) >> FRAC_N; 238 } 239 240 /* 241 * Combine two effects and apply gain. 242 */ 243 static 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 s16 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_sin16(i), level) * gain / 0xffff; 259 y = fixp_mult(-fixp_cos16(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 */ 326 static 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 390 static 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 403 static void ml_effect_timer(struct timer_list *t) 404 { 405 struct ml_device *ml = from_timer(ml, t, timer); 406 struct input_dev *dev = ml->dev; 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 */ 419 static 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 */ 435 static 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 465 static 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 488 static 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 */ 501 int 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 timer_setup(&ml->timer, ml_effect_timer, 0); 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 } 547 EXPORT_SYMBOL_GPL(input_ff_create_memless); 548