drivers/input/joystick/iforce/iforce-ff.c

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

DEFINITIONS
