Quelle  iforce-ff.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 *  Copyright (c) 2000-2002 Vojtech Pavlik <vojtech@ucw.cz>
 *  Copyright (c) 2001-2002, 2007 Johann Deneux <johann.deneux@gmail.com>
 *
 *  USB/RS232 I-Force joysticks and wheels.
 */

#include "iforce.h"

/*
 * Set the magnitude of a constant force effect
 * Return error code
 *
 * Note: caller must ensure exclusive access to device
 */

static int make_magnitude_modifier(struct iforce* iforce,
 struct resource* mod_chunk, int no_alloc, __s16 level)
{
 unsigned char data[3];

 if (!no_alloc) {
  guard(mutex)(&iforce->mem_mutex);

  if (allocate_resource(&iforce->device_memory, mod_chunk, 2,
          iforce->device_memory.start,
          iforce->device_memory.end,
          2L, NULL, NULL))
   return -ENOSPC;
 }

 data[0] = LO(mod_chunk->start);
 data[1] = HI(mod_chunk->start);
 data[2] = HIFIX80(level);

 iforce_send_packet(iforce, FF_CMD_MAGNITUDE, data);

 iforce_dump_packet(iforce, "magnitude", FF_CMD_MAGNITUDE, data);
 return 0;
}

/*
 * Upload the component of an effect dealing with the period, phase and magnitude
 */

static int make_period_modifier(struct iforce* iforce,
 struct resource* mod_chunk, int no_alloc,
 __s16 magnitude, __s16 offset, u16 period, u16 phase)
{
 unsigned char data[7];

 period = TIME_SCALE(period);

 if (!no_alloc) {
  guard(mutex)(&iforce->mem_mutex);

  if (allocate_resource(&iforce->device_memory, mod_chunk, 0x0c,
          iforce->device_memory.start,
          iforce->device_memory.end,
          2L, NULL, NULL))
   return -ENOSPC;
 }

 data[0] = LO(mod_chunk->start);
 data[1] = HI(mod_chunk->start);

 data[2] = HIFIX80(magnitude);
 data[3] = HIFIX80(offset);
 data[4] = HI(phase);

 data[5] = LO(period);
 data[6] = HI(period);

 iforce_send_packet(iforce, FF_CMD_PERIOD, data);

 return 0;
}

/*
 * Uploads the part of an effect setting the envelope of the force
 */

static int make_envelope_modifier(struct iforce* iforce,
 struct resource* mod_chunk, int no_alloc,
 u16 attack_duration, __s16 initial_level,
 u16 fade_duration, __s16 final_level)
{
 unsigned char data[8];

 attack_duration = TIME_SCALE(attack_duration);
 fade_duration = TIME_SCALE(fade_duration);

 if (!no_alloc) {
  guard(mutex)(&iforce->mem_mutex);

  if (allocate_resource(&(iforce->device_memory), mod_chunk, 0x0e,
          iforce->device_memory.start,
          iforce->device_memory.end,
          2L, NULL, NULL))
   return -ENOSPC;
 }

 data[0] = LO(mod_chunk->start);
 data[1] = HI(mod_chunk->start);

 data[2] = LO(attack_duration);
 data[3] = HI(attack_duration);
 data[4] = HI(initial_level);

 data[5] = LO(fade_duration);
 data[6] = HI(fade_duration);
 data[7] = HI(final_level);

 iforce_send_packet(iforce, FF_CMD_ENVELOPE, data);

 return 0;
}

/*
 * Component of spring, friction, inertia... effects
 */

static int make_condition_modifier(struct iforce* iforce,
 struct resource* mod_chunk, int no_alloc,
 __u16 rsat, __u16 lsat, __s16 rk, __s16 lk, u16 db, __s16 center)
{
 unsigned char data[10];

 if (!no_alloc) {
  guard(mutex)(&iforce->mem_mutex);

  if (allocate_resource(&(iforce->device_memory), mod_chunk, 8,
          iforce->device_memory.start,
          iforce->device_memory.end,
          2L, NULL, NULL))
   return -ENOSPC;
 }

 data[0] = LO(mod_chunk->start);
 data[1] = HI(mod_chunk->start);

 data[2] = (100 * rk) >> 15; /* Dangerous: the sign is extended by gcc on plateforms providing an arith shift */
 data[3] = (100 * lk) >> 15; /* This code is incorrect on cpus lacking arith shift */

 center = (500 * center) >> 15;
 data[4] = LO(center);
 data[5] = HI(center);

 db = (1000 * db) >> 16;
 data[6] = LO(db);
 data[7] = HI(db);

 data[8] = (100 * rsat) >> 16;
 data[9] = (100 * lsat) >> 16;

 iforce_send_packet(iforce, FF_CMD_CONDITION, data);
 iforce_dump_packet(iforce, "condition", FF_CMD_CONDITION, data);

 return 0;
}

static unsigned char find_button(struct iforce *iforce, signed short button)
{
 int i;

 for (i = 1; iforce->type->btn[i] >= 0; i++)
  if (iforce->type->btn[i] == button)
   return i + 1;
 return 0;
}

/*
 * Analyse the changes in an effect, and tell if we need to send an condition
 * parameter packet
 */
static int need_condition_modifier(struct iforce *iforce,
       struct ff_effect *old,
       struct ff_effect *new)
{
 int ret = 0;
 int i;

 if (new->type != FF_SPRING && new->type != FF_FRICTION) {
  dev_warn(&iforce->dev->dev, "bad effect type in %s\n",
    __func__);
  return 0;
 }

 for (i = 0; i < 2; i++) {
  ret |= old->u.condition[i].right_saturation != new->u.condition[i].right_saturation
   || old->u.condition[i].left_saturation != new->u.condition[i].left_saturation
   || old->u.condition[i].right_coeff != new->u.condition[i].right_coeff
   || old->u.condition[i].left_coeff != new->u.condition[i].left_coeff
   || old->u.condition[i].deadband != new->u.condition[i].deadband
   || old->u.condition[i].center != new->u.condition[i].center;
 }
 return ret;
}

/*
 * Analyse the changes in an effect, and tell if we need to send a magnitude
 * parameter packet
 */
static int need_magnitude_modifier(struct iforce *iforce,
       struct ff_effect *old,
       struct ff_effect *effect)
{
 if (effect->type != FF_CONSTANT) {
  dev_warn(&iforce->dev->dev, "bad effect type in %s\n",
    __func__);
  return 0;
 }

 return old->u.constant.level != effect->u.constant.level;
}

/*
 * Analyse the changes in an effect, and tell if we need to send an envelope
 * parameter packet
 */
static int need_envelope_modifier(struct iforce *iforce, struct ff_effect *old,
      struct ff_effect *effect)
{
 switch (effect->type) {
 case FF_CONSTANT:
  if (old->u.constant.envelope.attack_length != effect->u.constant.envelope.attack_length
  || old->u.constant.envelope.attack_level != effect->u.constant.envelope.attack_level
  || old->u.constant.envelope.fade_length != effect->u.constant.envelope.fade_length
  || old->u.constant.envelope.fade_level != effect->u.constant.envelope.fade_level)
   return 1;
  break;

 case FF_PERIODIC:
  if (old->u.periodic.envelope.attack_length != effect->u.periodic.envelope.attack_length
  || old->u.periodic.envelope.attack_level != effect->u.periodic.envelope.attack_level
  || old->u.periodic.envelope.fade_length != effect->u.periodic.envelope.fade_length
  || old->u.periodic.envelope.fade_level != effect->u.periodic.envelope.fade_level)
   return 1;
  break;

 default:
  dev_warn(&iforce->dev->dev, "bad effect type in %s\n",
    __func__);
 }

 return 0;
}

/*
 * Analyse the changes in an effect, and tell if we need to send a periodic
 * parameter effect
 */
static int need_period_modifier(struct iforce *iforce, struct ff_effect *old,
    struct ff_effect *new)
{
 if (new->type != FF_PERIODIC) {
  dev_warn(&iforce->dev->dev, "bad effect type in %s\n",
    __func__);
  return 0;
 }
 return (old->u.periodic.period != new->u.periodic.period
  || old->u.periodic.magnitude != new->u.periodic.magnitude
  || old->u.periodic.offset != new->u.periodic.offset
  || old->u.periodic.phase != new->u.periodic.phase);
}

/*
 * Analyse the changes in an effect, and tell if we need to send an effect
 * packet
 */
static int need_core(struct ff_effect *old, struct ff_effect *new)
{
 if (old->direction != new->direction
  || old->trigger.button != new->trigger.button
  || old->trigger.interval != new->trigger.interval
  || old->replay.length != new->replay.length
  || old->replay.delay != new->replay.delay)
  return 1;

 return 0;
}
/*
 * Send the part common to all effects to the device
 */
static int make_core(struct iforce* iforce, u16 id, u16 mod_id1, u16 mod_id2,
 u8 effect_type, u8 axes, u16 duration, u16 delay, u16 button,
 u16 interval, u16 direction)
{
 unsigned char data[14];

 duration = TIME_SCALE(duration);
 delay    = TIME_SCALE(delay);
 interval = TIME_SCALE(interval);

 data[0]  = LO(id);
 data[1]  = effect_type;
 data[2]  = LO(axes) | find_button(iforce, button);

 data[3]  = LO(duration);
 data[4]  = HI(duration);

 data[5]  = HI(direction);

 data[6]  = LO(interval);
 data[7]  = HI(interval);

 data[8]  = LO(mod_id1);
 data[9]  = HI(mod_id1);
 data[10] = LO(mod_id2);
 data[11] = HI(mod_id2);

 data[12] = LO(delay);
 data[13] = HI(delay);

 /* Stop effect */
/* iforce_control_playback(iforce, id, 0);*/

 iforce_send_packet(iforce, FF_CMD_EFFECT, data);

 /* If needed, restart effect */
 if (test_bit(FF_CORE_SHOULD_PLAY, iforce->core_effects[id].flags)) {
  /* BUG: perhaps we should replay n times, instead of 1. But we do not know n */
  iforce_control_playback(iforce, id, 1);
 }

 return 0;
}

/*
 * Upload a periodic effect to the device
 * See also iforce_upload_constant.
 */
int iforce_upload_periodic(struct iforce *iforce, struct ff_effect *effect, struct ff_effect *old)
{
 u8 wave_code;
 int core_id = effect->id;
 struct iforce_core_effect* core_effect = iforce->core_effects + core_id;
 struct resource* mod1_chunk = &(iforce->core_effects[core_id].mod1_chunk);
 struct resource* mod2_chunk = &(iforce->core_effects[core_id].mod2_chunk);
 int param1_err = 1;
 int param2_err = 1;
 int core_err = 0;

 if (!old || need_period_modifier(iforce, old, effect)) {
  param1_err = make_period_modifier(iforce, mod1_chunk,
   old != NULL,
   effect->u.periodic.magnitude, effect->u.periodic.offset,
   effect->u.periodic.period, effect->u.periodic.phase);
  if (param1_err)
   return param1_err;
  set_bit(FF_MOD1_IS_USED, core_effect->flags);
 }

 if (!old || need_envelope_modifier(iforce, old, effect)) {
  param2_err = make_envelope_modifier(iforce, mod2_chunk,
   old !=NULL,
   effect->u.periodic.envelope.attack_length,
   effect->u.periodic.envelope.attack_level,
   effect->u.periodic.envelope.fade_length,
   effect->u.periodic.envelope.fade_level);
  if (param2_err)
   return param2_err;
  set_bit(FF_MOD2_IS_USED, core_effect->flags);
 }

 switch (effect->u.periodic.waveform) {
 case FF_SQUARE:  wave_code = 0x20; break;
 case FF_TRIANGLE: wave_code = 0x21; break;
 case FF_SINE:  wave_code = 0x22; break;
 case FF_SAW_UP:  wave_code = 0x23; break;
 case FF_SAW_DOWN: wave_code = 0x24; break;
 default:  wave_code = 0x20; break;
 }

 if (!old || need_core(old, effect)) {
  core_err = make_core(iforce, effect->id,
   mod1_chunk->start,
   mod2_chunk->start,
   wave_code,
   0x20,
   effect->replay.length,
   effect->replay.delay,
   effect->trigger.button,
   effect->trigger.interval,
   effect->direction);
 }

 /* If one of the parameter creation failed, we already returned an
  * error code.
  * If the core creation failed, we return its error code.
  * Else: if one parameter at least was created, we return 0
  *       else we return 1;
 */
 return core_err < 0 ? core_err : (param1_err && param2_err);
}

/*
 * Upload a constant force effect
 * Return value:
 *  <0 Error code
 *  0 Ok, effect created or updated
 *  1 effect did not change since last upload, and no packet was therefore sent
 */
int iforce_upload_constant(struct iforce *iforce, struct ff_effect *effect, struct ff_effect *old)
{
 int core_id = effect->id;
 struct iforce_core_effect* core_effect = iforce->core_effects + core_id;
 struct resource* mod1_chunk = &(iforce->core_effects[core_id].mod1_chunk);
 struct resource* mod2_chunk = &(iforce->core_effects[core_id].mod2_chunk);
 int param1_err = 1;
 int param2_err = 1;
 int core_err = 0;

 if (!old || need_magnitude_modifier(iforce, old, effect)) {
  param1_err = make_magnitude_modifier(iforce, mod1_chunk,
   old != NULL,
   effect->u.constant.level);
  if (param1_err)
   return param1_err;
  set_bit(FF_MOD1_IS_USED, core_effect->flags);
 }

 if (!old || need_envelope_modifier(iforce, old, effect)) {
  param2_err = make_envelope_modifier(iforce, mod2_chunk,
   old != NULL,
   effect->u.constant.envelope.attack_length,
   effect->u.constant.envelope.attack_level,
   effect->u.constant.envelope.fade_length,
   effect->u.constant.envelope.fade_level);
  if (param2_err)
   return param2_err;
  set_bit(FF_MOD2_IS_USED, core_effect->flags);
 }

 if (!old || need_core(old, effect)) {
  core_err = make_core(iforce, effect->id,
   mod1_chunk->start,
   mod2_chunk->start,
   0x00,
   0x20,
   effect->replay.length,
   effect->replay.delay,
   effect->trigger.button,
   effect->trigger.interval,
   effect->direction);
 }

 /* If one of the parameter creation failed, we already returned an
  * error code.
  * If the core creation failed, we return its error code.
  * Else: if one parameter at least was created, we return 0
  *       else we return 1;
 */
 return core_err < 0 ? core_err : (param1_err && param2_err);
}

/*
 * Upload an condition effect. Those are for example friction, inertia, springs...
 */
int iforce_upload_condition(struct iforce *iforce, struct ff_effect *effect, struct ff_effect *old)
{
 int core_id = effect->id;
 struct iforce_core_effect* core_effect = iforce->core_effects + core_id;
 struct resource* mod1_chunk = &(core_effect->mod1_chunk);
 struct resource* mod2_chunk = &(core_effect->mod2_chunk);
 u8 type;
 int param_err = 1;
 int core_err = 0;

 switch (effect->type) {
 case FF_SPRING: type = 0x40; break;
 case FF_DAMPER: type = 0x41; break;
 default: return -1;
 }

 if (!old || need_condition_modifier(iforce, old, effect)) {
  param_err = make_condition_modifier(iforce, mod1_chunk,
   old != NULL,
   effect->u.condition[0].right_saturation,
   effect->u.condition[0].left_saturation,
   effect->u.condition[0].right_coeff,
   effect->u.condition[0].left_coeff,
   effect->u.condition[0].deadband,
   effect->u.condition[0].center);
  if (param_err)
   return param_err;
  set_bit(FF_MOD1_IS_USED, core_effect->flags);

  param_err = make_condition_modifier(iforce, mod2_chunk,
   old != NULL,
   effect->u.condition[1].right_saturation,
   effect->u.condition[1].left_saturation,
   effect->u.condition[1].right_coeff,
   effect->u.condition[1].left_coeff,
   effect->u.condition[1].deadband,
   effect->u.condition[1].center);
  if (param_err)
   return param_err;
  set_bit(FF_MOD2_IS_USED, core_effect->flags);

 }

 if (!old || need_core(old, effect)) {
  core_err = make_core(iforce, effect->id,
   mod1_chunk->start, mod2_chunk->start,
   type, 0xc0,
   effect->replay.length, effect->replay.delay,
   effect->trigger.button, effect->trigger.interval,
   effect->direction);
 }

 /* If the parameter creation failed, we already returned an
  * error code.
  * If the core creation failed, we return its error code.
  * Else: if a parameter  was created, we return 0
  *       else we return 1;
 */
 return core_err < 0 ? core_err : param_err;
}