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

   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
  18static 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
  48static 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
  86static 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
 127static 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
 167static 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 */
 181static 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 */
 209static 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 */
 226static 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 */
 258static 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 */
 276static 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 */
 290static 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 */
 338int 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 */
 409int 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 */
 465int 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}
 525