1 /* 2 * Force feedback support for memoryless devices 3 * 4 * Copyright (c) 2006 Anssi Hannula <anssi.hannula@gmail.com> 5 * Copyright (c) 2006 Dmitry Torokhov <dtor@mail.ru> 6 */ 7 8 /* 9 * This program is free software; you can redistribute it and/or modify 10 * it under the terms of the GNU General Public License as published by 11 * the Free Software Foundation; either version 2 of the License, or 12 * (at your option) any later version. 13 * 14 * This program is distributed in the hope that it will be useful, 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 * GNU General Public License for more details. 18 * 19 * You should have received a copy of the GNU General Public License 20 * along with this program; if not, write to the Free Software 21 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 22 */ 23 24 /* #define DEBUG */ 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 36 MODULE_LICENSE("GPL"); 37 MODULE_AUTHOR("Anssi Hannula <anssi.hannula@gmail.com>"); 38 MODULE_DESCRIPTION("Force feedback support for memoryless devices"); 39 40 /* Number of effects handled with memoryless devices */ 41 #define FF_MEMLESS_EFFECTS 16 42 43 /* Envelope update interval in ms */ 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 50 struct ml_effect_state { 51 struct ff_effect *effect; 52 unsigned long flags; /* effect state (STARTED, PLAYING, etc) */ 53 int count; /* loop count of the effect */ 54 unsigned long play_at; /* start time */ 55 unsigned long stop_at; /* stop time */ 56 unsigned long adj_at; /* last time the effect was sent */ 57 }; 58 59 struct 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 70 static 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 * Check for the next time envelope requires an update on memoryless devices 88 */ 89 static 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 /* check when fading should start */ 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 /* already fading, advance to next checkpoint */ 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 124 static 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 * Apply an envelope to a value 163 */ 164 static 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 * Return the type the effect has to be converted into (memless devices) 209 */ 210 static 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 * Only left/right direction should be used (under/over 0x8000) for 226 * forward/reverse motor direction (to keep calculation fast & simple). 227 */ 228 static 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 241 static inline s16 fixp_new16(s16 a) 242 { 243 return ((s32)a) >> (16 - FRAC_N); 244 } 245 246 static 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 * Combine two effects and apply gain. 254 */ 255 static 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 * here we abuse ff_ramp to hold x and y of constant force 274 * If in future any driver wants something else than x and y 275 * in s8, this should be changed to something more generic 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 /* here we also scale it 0x7fff => 0xffff */ 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 * Because memoryless devices have only one effect per effect type active 336 * at one time we have to combine multiple effects into one 337 */ 338 static 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 * here we have started effects that are either 364 * currently playing (and may need be aborted) 365 * or need to start playing. 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 402 static 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 415 static void ml_effect_timer(struct timer_list *t) 416 { 417 struct ml_device *ml = from_timer(ml, t, timer); 418 struct input_dev *dev = ml->dev; 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 * Sets requested gain for FF effects. Called with dev->event_lock held. 430 */ 431 static 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 * Start/stop specified FF effect. Called with dev->event_lock held. 446 */ 447 static 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 477 static 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 500 static 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 * input_ff_create_memless() - create memoryless force-feedback device 509 * @dev: input device supporting force-feedback 510 * @data: driver-specific data to be passed into @play_effect 511 * @play_effect: driver-specific method for playing FF effect 512 */ 513 int 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 timer_setup(&ml->timer, ml_effect_timer, 0); 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 /* we can emulate periodic effects with RUMBLE */ 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 } 559 EXPORT_SYMBOL_GPL(input_ff_create_memless); 560