kernel/2.6.29.6-aldebaran-rt/drivers/input/joystick/iforce/iforce-ff.c

   1 /*
   2  *  Copyright (c) 2000-2002 Vojtech Pavlik <vojtech@ucw.cz>
   3  *  Copyright (c) 2001-2002, 2007 Johann Deneux <johann.deneux@gmail.com>
   4  *
   5  *  USB/RS232 I-Force joysticks and wheels.
   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  * Should you need to contact me, the author, you can do so either by
  24  * e-mail - mail your message to <vojtech@ucw.cz>, or by paper mail:
  25  * Vojtech Pavlik, Simunkova 1594, Prague 8, 182 00 Czech Republic
  26  */
  27
  28 #include "iforce.h"
  29
  30 /*
  31  * Set the magnitude of a constant force effect
  32  * Return error code
  33  *
  34  * Note: caller must ensure exclusive access to device
  35  */
  36
  37 static int make_magnitude_modifier(struct iforce* iforce,
  38         struct resource* mod_chunk, int no_alloc, __s16 level)
  39 {
  40         unsigned char data[3];
  41
  42         if (!no_alloc) {
  43                 mutex_lock(&iforce->mem_mutex);
  44                 if (allocate_resource(&(iforce->device_memory), mod_chunk, 2,
  45                         iforce->device_memory.start, iforce->device_memory.end, 2L,
  46                         NULL, NULL)) {
  47                         mutex_unlock(&iforce->mem_mutex);
  48                         return -ENOSPC;
  49                 }
  50                 mutex_unlock(&iforce->mem_mutex);
  51         }
  52
  53         data[0] = LO(mod_chunk->start);
  54         data[1] = HI(mod_chunk->start);
  55         data[2] = HIFIX80(level);
  56
  57         iforce_send_packet(iforce, FF_CMD_MAGNITUDE, data);
  58
  59         iforce_dump_packet("magnitude: ", FF_CMD_MAGNITUDE, data);
  60         return 0;
  61 }
  62
  63 /*
  64  * Upload the component of an effect dealing with the period, phase and magnitude
  65  */
  66
  67 static int make_period_modifier(struct iforce* iforce,
  68         struct resource* mod_chunk, int no_alloc,
  69         __s16 magnitude, __s16 offset, u16 period, u16 phase)
  70 {
  71         unsigned char data[7];
  72
  73         period = TIME_SCALE(period);
  74
  75         if (!no_alloc) {
  76                 mutex_lock(&iforce->mem_mutex);
  77                 if (allocate_resource(&(iforce->device_memory), mod_chunk, 0x0c,
  78                         iforce->device_memory.start, iforce->device_memory.end, 2L,
  79                         NULL, NULL)) {
  80                         mutex_unlock(&iforce->mem_mutex);
  81                         return -ENOSPC;
  82                 }
  83                 mutex_unlock(&iforce->mem_mutex);
  84         }
  85
  86         data[0] = LO(mod_chunk->start);
  87         data[1] = HI(mod_chunk->start);
  88
  89         data[2] = HIFIX80(magnitude);
  90         data[3] = HIFIX80(offset);
  91         data[4] = HI(phase);
  92
  93         data[5] = LO(period);
  94         data[6] = HI(period);
  95
  96         iforce_send_packet(iforce, FF_CMD_PERIOD, data);
  97
  98         return 0;
  99 }
 100
 101 /*
 102  * Uploads the part of an effect setting the envelope of the force
 103  */
 104
 105 static int make_envelope_modifier(struct iforce* iforce,
 106         struct resource* mod_chunk, int no_alloc,
 107         u16 attack_duration, __s16 initial_level,
 108         u16 fade_duration, __s16 final_level)
 109 {
 110         unsigned char data[8];
 111
 112         attack_duration = TIME_SCALE(attack_duration);
 113         fade_duration = TIME_SCALE(fade_duration);
 114
 115         if (!no_alloc) {
 116                 mutex_lock(&iforce->mem_mutex);
 117                 if (allocate_resource(&(iforce->device_memory), mod_chunk, 0x0e,
 118                         iforce->device_memory.start, iforce->device_memory.end, 2L,
 119                         NULL, NULL)) {
 120                         mutex_unlock(&iforce->mem_mutex);
 121                         return -ENOSPC;
 122                 }
 123                 mutex_unlock(&iforce->mem_mutex);
 124         }
 125
 126         data[0] = LO(mod_chunk->start);
 127         data[1] = HI(mod_chunk->start);
 128
 129         data[2] = LO(attack_duration);
 130         data[3] = HI(attack_duration);
 131         data[4] = HI(initial_level);
 132
 133         data[5] = LO(fade_duration);
 134         data[6] = HI(fade_duration);
 135         data[7] = HI(final_level);
 136
 137         iforce_send_packet(iforce, FF_CMD_ENVELOPE, data);
 138
 139         return 0;
 140 }
 141
 142 /*
 143  * Component of spring, friction, inertia... effects
 144  */
 145
 146 static int make_condition_modifier(struct iforce* iforce,
 147         struct resource* mod_chunk, int no_alloc,
 148         __u16 rsat, __u16 lsat, __s16 rk, __s16 lk, u16 db, __s16 center)
 149 {
 150         unsigned char data[10];
 151
 152         if (!no_alloc) {
 153                 mutex_lock(&iforce->mem_mutex);
 154                 if (allocate_resource(&(iforce->device_memory), mod_chunk, 8,
 155                         iforce->device_memory.start, iforce->device_memory.end, 2L,
 156                         NULL, NULL)) {
 157                         mutex_unlock(&iforce->mem_mutex);
 158                         return -ENOSPC;
 159                 }
 160                 mutex_unlock(&iforce->mem_mutex);
 161         }
 162
 163         data[0] = LO(mod_chunk->start);
 164         data[1] = HI(mod_chunk->start);
 165
 166         data[2] = (100 * rk) >> 15;     /* Dangerous: the sign is extended by gcc on plateforms providing an arith shift */
 167         data[3] = (100 * lk) >> 15; /* This code is incorrect on cpus lacking arith shift */
 168
 169         center = (500 * center) >> 15;
 170         data[4] = LO(center);
 171         data[5] = HI(center);
 172
 173         db = (1000 * db) >> 16;
 174         data[6] = LO(db);
 175         data[7] = HI(db);
 176
 177         data[8] = (100 * rsat) >> 16;
 178         data[9] = (100 * lsat) >> 16;
 179
 180         iforce_send_packet(iforce, FF_CMD_CONDITION, data);
 181         iforce_dump_packet("condition", FF_CMD_CONDITION, data);
 182
 183         return 0;
 184 }
 185
 186 static unsigned char find_button(struct iforce *iforce, signed short button)
 187 {
 188         int i;
 189
 190         for (i = 1; iforce->type->btn[i] >= 0; i++)
 191                 if (iforce->type->btn[i] == button)
 192                         return i + 1;
 193         return 0;
 194 }
 195
 196 /*
 197  * Analyse the changes in an effect, and tell if we need to send an condition
 198  * parameter packet
 199  */
 200 static int need_condition_modifier(struct iforce *iforce,
 201                                    struct ff_effect *old,
 202                                    struct ff_effect *new)
 203 {
 204         int ret = 0;
 205         int i;
 206
 207         if (new->type != FF_SPRING && new->type != FF_FRICTION) {
 208                 dev_warn(&iforce->dev->dev, "bad effect type in %s\n",
 209                          __func__);
 210                 return 0;
 211         }
 212
 213         for (i = 0; i < 2; i++) {
 214                 ret |= old->u.condition[i].right_saturation != new->u.condition[i].right_saturation
 215                         || old->u.condition[i].left_saturation != new->u.condition[i].left_saturation
 216                         || old->u.condition[i].right_coeff != new->u.condition[i].right_coeff
 217                         || old->u.condition[i].left_coeff != new->u.condition[i].left_coeff
 218                         || old->u.condition[i].deadband != new->u.condition[i].deadband
 219                         || old->u.condition[i].center != new->u.condition[i].center;
 220         }
 221         return ret;
 222 }
 223
 224 /*
 225  * Analyse the changes in an effect, and tell if we need to send a magnitude
 226  * parameter packet
 227  */
 228 static int need_magnitude_modifier(struct iforce *iforce,
 229                                    struct ff_effect *old,
 230                                    struct ff_effect *effect)
 231 {
 232         if (effect->type != FF_CONSTANT) {
 233                 dev_warn(&iforce->dev->dev, "bad effect type in %s\n",
 234                          __func__);
 235                 return 0;
 236         }
 237
 238         return old->u.constant.level != effect->u.constant.level;
 239 }
 240
 241 /*
 242  * Analyse the changes in an effect, and tell if we need to send an envelope
 243  * parameter packet
 244  */
 245 static int need_envelope_modifier(struct iforce *iforce, struct ff_effect *old,
 246                                   struct ff_effect *effect)
 247 {
 248         switch (effect->type) {
 249         case FF_CONSTANT:
 250                 if (old->u.constant.envelope.attack_length != effect->u.constant.envelope.attack_length
 251                 || old->u.constant.envelope.attack_level != effect->u.constant.envelope.attack_level
 252                 || old->u.constant.envelope.fade_length != effect->u.constant.envelope.fade_length
 253                 || old->u.constant.envelope.fade_level != effect->u.constant.envelope.fade_level)
 254                         return 1;
 255                 break;
 256
 257         case FF_PERIODIC:
 258                 if (old->u.periodic.envelope.attack_length != effect->u.periodic.envelope.attack_length
 259                 || old->u.periodic.envelope.attack_level != effect->u.periodic.envelope.attack_level
 260                 || old->u.periodic.envelope.fade_length != effect->u.periodic.envelope.fade_length
 261                 || old->u.periodic.envelope.fade_level != effect->u.periodic.envelope.fade_level)
 262                         return 1;
 263                 break;
 264
 265         default:
 266                 dev_warn(&iforce->dev->dev, "bad effect type in %s\n",
 267                          __func__);
 268         }
 269
 270         return 0;
 271 }
 272
 273 /*
 274  * Analyse the changes in an effect, and tell if we need to send a periodic
 275  * parameter effect
 276  */
 277 static int need_period_modifier(struct iforce *iforce, struct ff_effect *old,
 278                                 struct ff_effect *new)
 279 {
 280         if (new->type != FF_PERIODIC) {
 281                 dev_warn(&iforce->dev->dev, "bad effect type in %s\n",
 282                          __func__);
 283                 return 0;
 284         }
 285         return (old->u.periodic.period != new->u.periodic.period
 286                 || old->u.periodic.magnitude != new->u.periodic.magnitude
 287                 || old->u.periodic.offset != new->u.periodic.offset
 288                 || old->u.periodic.phase != new->u.periodic.phase);
 289 }
 290
 291 /*
 292  * Analyse the changes in an effect, and tell if we need to send an effect
 293  * packet
 294  */
 295 static int need_core(struct ff_effect *old, struct ff_effect *new)
 296 {
 297         if (old->direction != new->direction
 298                 || old->trigger.button != new->trigger.button
 299                 || old->trigger.interval != new->trigger.interval
 300                 || old->replay.length != new->replay.length
 301                 || old->replay.delay != new->replay.delay)
 302                 return 1;
 303
 304         return 0;
 305 }
 306 /*
 307  * Send the part common to all effects to the device
 308  */
 309 static int make_core(struct iforce* iforce, u16 id, u16 mod_id1, u16 mod_id2,
 310         u8 effect_type, u8 axes, u16 duration, u16 delay, u16 button,
 311         u16 interval, u16 direction)
 312 {
 313         unsigned char data[14];
 314
 315         duration = TIME_SCALE(duration);
 316         delay    = TIME_SCALE(delay);
 317         interval = TIME_SCALE(interval);
 318
 319         data[0]  = LO(id);
 320         data[1]  = effect_type;
 321         data[2]  = LO(axes) | find_button(iforce, button);
 322
 323         data[3]  = LO(duration);
 324         data[4]  = HI(duration);
 325
 326         data[5]  = HI(direction);
 327
 328         data[6]  = LO(interval);
 329         data[7]  = HI(interval);
 330
 331         data[8]  = LO(mod_id1);
 332         data[9]  = HI(mod_id1);
 333         data[10] = LO(mod_id2);
 334         data[11] = HI(mod_id2);
 335
 336         data[12] = LO(delay);
 337         data[13] = HI(delay);
 338
 339         /* Stop effect */
 340 /*      iforce_control_playback(iforce, id, 0);*/
 341
 342         iforce_send_packet(iforce, FF_CMD_EFFECT, data);
 343
 344         /* If needed, restart effect */
 345         if (test_bit(FF_CORE_SHOULD_PLAY, iforce->core_effects[id].flags)) {
 346                 /* BUG: perhaps we should replay n times, instead of 1. But we do not know n */
 347                 iforce_control_playback(iforce, id, 1);
 348         }
 349
 350         return 0;
 351 }
 352
 353 /*
 354  * Upload a periodic effect to the device
 355  * See also iforce_upload_constant.
 356  */
 357 int iforce_upload_periodic(struct iforce *iforce, struct ff_effect *effect, struct ff_effect *old)
 358 {
 359         u8 wave_code;
 360         int core_id = effect->id;
 361         struct iforce_core_effect* core_effect = iforce->core_effects + core_id;
 362         struct resource* mod1_chunk = &(iforce->core_effects[core_id].mod1_chunk);
 363         struct resource* mod2_chunk = &(iforce->core_effects[core_id].mod2_chunk);
 364         int param1_err = 1;
 365         int param2_err = 1;
 366         int core_err = 0;
 367
 368         if (!old || need_period_modifier(iforce, old, effect)) {
 369                 param1_err = make_period_modifier(iforce, mod1_chunk,
 370                         old != NULL,
 371                         effect->u.periodic.magnitude, effect->u.periodic.offset,
 372                         effect->u.periodic.period, effect->u.periodic.phase);
 373                 if (param1_err)
 374                         return param1_err;
 375                 set_bit(FF_MOD1_IS_USED, core_effect->flags);
 376         }
 377
 378         if (!old || need_envelope_modifier(iforce, old, effect)) {
 379                 param2_err = make_envelope_modifier(iforce, mod2_chunk,
 380                         old !=NULL,
 381                         effect->u.periodic.envelope.attack_length,
 382                         effect->u.periodic.envelope.attack_level,
 383                         effect->u.periodic.envelope.fade_length,
 384                         effect->u.periodic.envelope.fade_level);
 385                 if (param2_err)
 386                         return param2_err;
 387                 set_bit(FF_MOD2_IS_USED, core_effect->flags);
 388         }
 389
 390         switch (effect->u.periodic.waveform) {
 391                 case FF_SQUARE:         wave_code = 0x20; break;
 392                 case FF_TRIANGLE:       wave_code = 0x21; break;
 393                 case FF_SINE:           wave_code = 0x22; break;
 394                 case FF_SAW_UP:         wave_code = 0x23; break;
 395                 case FF_SAW_DOWN:       wave_code = 0x24; break;
 396                 default:                wave_code = 0x20; break;
 397         }
 398
 399         if (!old || need_core(old, effect)) {
 400                 core_err = make_core(iforce, effect->id,
 401                         mod1_chunk->start,
 402                         mod2_chunk->start,
 403                         wave_code,
 404                         0x20,
 405                         effect->replay.length,
 406                         effect->replay.delay,
 407                         effect->trigger.button,
 408                         effect->trigger.interval,
 409                         effect->direction);
 410         }
 411
 412         /* If one of the parameter creation failed, we already returned an
 413          * error code.
 414          * If the core creation failed, we return its error code.
 415          * Else: if one parameter at least was created, we return 0
 416          *       else we return 1;
 417          */
 418         return core_err < 0 ? core_err : (param1_err && param2_err);
 419 }
 420
 421 /*
 422  * Upload a constant force effect
 423  * Return value:
 424  *  <0 Error code
 425  *  0 Ok, effect created or updated
 426  *  1 effect did not change since last upload, and no packet was therefore sent
 427  */
 428 int iforce_upload_constant(struct iforce *iforce, struct ff_effect *effect, struct ff_effect *old)
 429 {
 430         int core_id = effect->id;
 431         struct iforce_core_effect* core_effect = iforce->core_effects + core_id;
 432         struct resource* mod1_chunk = &(iforce->core_effects[core_id].mod1_chunk);
 433         struct resource* mod2_chunk = &(iforce->core_effects[core_id].mod2_chunk);
 434         int param1_err = 1;
 435         int param2_err = 1;
 436         int core_err = 0;
 437
 438         if (!old || need_magnitude_modifier(iforce, old, effect)) {
 439                 param1_err = make_magnitude_modifier(iforce, mod1_chunk,
 440                         old != NULL,
 441                         effect->u.constant.level);
 442                 if (param1_err)
 443                         return param1_err;
 444                 set_bit(FF_MOD1_IS_USED, core_effect->flags);
 445         }
 446
 447         if (!old || need_envelope_modifier(iforce, old, effect)) {
 448                 param2_err = make_envelope_modifier(iforce, mod2_chunk,
 449                         old != NULL,
 450                         effect->u.constant.envelope.attack_length,
 451                         effect->u.constant.envelope.attack_level,
 452                         effect->u.constant.envelope.fade_length,
 453                         effect->u.constant.envelope.fade_level);
 454                 if (param2_err)
 455                         return param2_err;
 456                 set_bit(FF_MOD2_IS_USED, core_effect->flags);
 457         }
 458
 459         if (!old || need_core(old, effect)) {
 460                 core_err = make_core(iforce, effect->id,
 461                         mod1_chunk->start,
 462                         mod2_chunk->start,
 463                         0x00,
 464                         0x20,
 465                         effect->replay.length,
 466                         effect->replay.delay,
 467                         effect->trigger.button,
 468                         effect->trigger.interval,
 469                         effect->direction);
 470         }
 471
 472         /* If one of the parameter creation failed, we already returned an
 473          * error code.
 474          * If the core creation failed, we return its error code.
 475          * Else: if one parameter at least was created, we return 0
 476          *       else we return 1;
 477          */
 478         return core_err < 0 ? core_err : (param1_err && param2_err);
 479 }
 480
 481 /*
 482  * Upload an condition effect. Those are for example friction, inertia, springs...
 483  */
 484 int iforce_upload_condition(struct iforce *iforce, struct ff_effect *effect, struct ff_effect *old)
 485 {
 486         int core_id = effect->id;
 487         struct iforce_core_effect* core_effect = iforce->core_effects + core_id;
 488         struct resource* mod1_chunk = &(core_effect->mod1_chunk);
 489         struct resource* mod2_chunk = &(core_effect->mod2_chunk);
 490         u8 type;
 491         int param_err = 1;
 492         int core_err = 0;
 493
 494         switch (effect->type) {
 495                 case FF_SPRING: type = 0x40; break;
 496                 case FF_DAMPER: type = 0x41; break;
 497                 default: return -1;
 498         }
 499
 500         if (!old || need_condition_modifier(iforce, old, effect)) {
 501                 param_err = make_condition_modifier(iforce, mod1_chunk,
 502                         old != NULL,
 503                         effect->u.condition[0].right_saturation,
 504                         effect->u.condition[0].left_saturation,
 505                         effect->u.condition[0].right_coeff,
 506                         effect->u.condition[0].left_coeff,
 507                         effect->u.condition[0].deadband,
 508                         effect->u.condition[0].center);
 509                 if (param_err)
 510                         return param_err;
 511                 set_bit(FF_MOD1_IS_USED, core_effect->flags);
 512
 513                 param_err = make_condition_modifier(iforce, mod2_chunk,
 514                         old != NULL,
 515                         effect->u.condition[1].right_saturation,
 516                         effect->u.condition[1].left_saturation,
 517                         effect->u.condition[1].right_coeff,
 518                         effect->u.condition[1].left_coeff,
 519                         effect->u.condition[1].deadband,
 520                         effect->u.condition[1].center);
 521                 if (param_err)
 522                         return param_err;
 523                 set_bit(FF_MOD2_IS_USED, core_effect->flags);
 524
 525         }
 526
 527         if (!old || need_core(old, effect)) {
 528                 core_err = make_core(iforce, effect->id,
 529                         mod1_chunk->start, mod2_chunk->start,
 530                         type, 0xc0,
 531                         effect->replay.length, effect->replay.delay,
 532                         effect->trigger.button, effect->trigger.interval,
 533                         effect->direction);
 534         }
 535
 536         /* If the parameter creation failed, we already returned an
 537          * error code.
 538          * If the core creation failed, we return its error code.
 539          * Else: if a parameter  was created, we return 0
 540          *       else we return 1;
 541          */
 542         return core_err < 0 ? core_err : param_err;
 543 }