[Linuxconsole-commit]CVS: ruby/linux/drivers/input/joystick/iforce iforce-ff.c,NONE,1.7

[Vojtech P. <vo...@us...>]

Update of /cvsroot/linuxconsole/ruby/linux/drivers/input/joystick/iforce
In directory usw-pr-cvs1:/tmp/cvs-serv7222

Added Files:
      Tag: 1.7
	iforce-ff.c 
Log Message:
Moved.

--- NEW FILE: iforce-ff.c ---
/*
 * $Id: iforce-ff.c,v 1.7 2002/01/22 20:33:25 vojtech Exp $
 *
 *  Copyright (c) 2000-2001 Vojtech Pavlik <vo...@uc...>
 *  Copyright (c) 2001 Johann Deneux <de...@if...>
 *
 *  USB/RS232 I-Force joysticks and wheels.
 */

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * Should you need to contact me, the author, you can do so either by
 * e-mail - mail your message to <vo...@uc...>, or by paper mail:
 * Vojtech Pavlik, Simunkova 1594, Prague 8, 182 00 Czech Republic
 */

#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) {
		down(&iforce->mem_mutex);
		if (allocate_resource(&(iforce->device_memory), mod_chunk, 2,
			iforce->device_memory.start, iforce->device_memory.end, 2L,
			NULL, NULL)) {
			up(&iforce->mem_mutex);
			return -ENOMEM;
		}
		up(&iforce->mem_mutex);
	}

	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("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) {
		down(&iforce->mem_mutex);
		if (allocate_resource(&(iforce->device_memory), mod_chunk, 0x0c,
			iforce->device_memory.start, iforce->device_memory.end, 2L,
			NULL, NULL)) {
			up(&iforce->mem_mutex);
			return -ENOMEM;
		}
		up(&iforce->mem_mutex);
	}

	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 shape of the force
 */

static int make_shape_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) {
		down(&iforce->mem_mutex);
		if (allocate_resource(&(iforce->device_memory), mod_chunk, 0x0e,
			iforce->device_memory.start, iforce->device_memory.end, 2L,
			NULL, NULL)) {
			up(&iforce->mem_mutex);
			return -ENOMEM;
		}
		up(&iforce->mem_mutex);
	}

	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_SHAPE, data);

	return 0;
}

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

static int make_interactive_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) {
		down(&iforce->mem_mutex);
		if (allocate_resource(&(iforce->device_memory), mod_chunk, 8,
			iforce->device_memory.start, iforce->device_memory.end, 2L,
			NULL, NULL)) {
			up(&iforce->mem_mutex);
			return -ENOMEM;
		}
		up(&iforce->mem_mutex);
	}

	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_INTERACT, data);
	iforce_dump_packet("interactive", FF_CMD_INTERACT, 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 interactive 
 * parameter packet
 */
static int need_interactive_modifier(struct iforce* iforce, struct ff_effect* new)
{
	int id = new->id;
	struct ff_effect* old = &iforce->core_effects[id].effect;

	if (new->type != FF_SPRING && new->type != FF_FRICTION) {
		printk(KERN_WARNING "iforce.c: bad effect type in need_interactive_modifier\n");
		return FALSE;
	}

	return (old->u.interactive.right_saturation != new->u.interactive.right_saturation
		|| old->u.interactive.left_saturation != new->u.interactive.left_saturation
		|| old->u.interactive.right_coeff != new->u.interactive.right_coeff
		|| old->u.interactive.left_coeff != new->u.interactive.left_coeff
		|| old->u.interactive.deadband != new->u.interactive.deadband
		|| old->u.interactive.center != new->u.interactive.center);
}

/*
 * 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* effect)
{
	int id = effect->id;
	struct ff_effect* old = &iforce->core_effects[id].effect;

	if (effect->type != FF_CONSTANT) {
		printk(KERN_WARNING "iforce.c: bad effect type in need_shape_modifier\n");
		return FALSE;
	}

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

/*
 * Analyse the changes in an effect, and tell if we need to send a shape
 * parameter packet
 */
static int need_shape_modifier(struct iforce* iforce, struct ff_effect* effect)
{
	int id = effect->id;
	struct ff_effect* old = &iforce->core_effects[id].effect;

	switch (effect->type) {
	case FF_CONSTANT:
		if (old->u.constant.shape.attack_length != effect->u.constant.shape.attack_length
		|| old->u.constant.shape.attack_level != effect->u.constant.shape.attack_level
		|| old->u.constant.shape.fade_length != effect->u.constant.shape.fade_length
		|| old->u.constant.shape.fade_level != effect->u.constant.shape.fade_level)
			return TRUE;
		break;

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

	default:
		printk(KERN_WARNING "iforce.c: bad effect type in need_shape_modifier\n");
	}

	return FALSE;
}

/*
 * 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* new)
{
	int id = new->id;
	struct ff_effect* old = &iforce->core_effects[id].effect;

	if (new->type != FF_PERIODIC) {
		printk(KERN_WARNING "iforce.c: bad effect type in need_periodic_modifier\n");
		return FALSE;
	}

	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 iforce* iforce, struct ff_effect* new)
{
	int id = new->id;
	struct ff_effect* old = &iforce->core_effects[id].effect;

	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 TRUE;

	return FALSE;
}
/*
 * 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, int is_update)
{
	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 (!is_update || need_period_modifier(iforce, effect)) {
		param1_err = make_period_modifier(iforce, mod1_chunk, 
			is_update,
			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 (!is_update || need_shape_modifier(iforce, effect)) {
		param2_err = make_shape_modifier(iforce, mod2_chunk,
			is_update,
			effect->u.periodic.shape.attack_length,
			effect->u.periodic.shape.attack_level,
			effect->u.periodic.shape.fade_length,
			effect->u.periodic.shape.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 (!is_update || need_core(iforce, 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, int is_update)
{
	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 (!is_update || need_magnitude_modifier(iforce, effect)) {
		param1_err = make_magnitude_modifier(iforce, mod1_chunk,
			is_update,
			effect->u.constant.level);
		if (param1_err) return param1_err;
		set_bit(FF_MOD1_IS_USED, core_effect->flags);
	}

	if (!is_update || need_shape_modifier(iforce, effect)) {
		param2_err = make_shape_modifier(iforce, mod2_chunk,
			is_update,
			effect->u.constant.shape.attack_length,
			effect->u.constant.shape.attack_level,
			effect->u.constant.shape.fade_length,
			effect->u.constant.shape.fade_level);
		if (param2_err) return param2_err;
		set_bit(FF_MOD2_IS_USED, core_effect->flags);
	}

	if (!is_update || need_core(iforce, 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 interactive effect. Those are for example friction, inertia, springs...
 */
int iforce_upload_interactive(struct iforce* iforce, struct ff_effect* effect, int is_update)
{
	int core_id = effect->id;
	struct iforce_core_effect* core_effect = iforce->core_effects + core_id;
	struct resource* mod_chunk = &(core_effect->mod1_chunk);
	u8 type, axes;
	u16 mod1, mod2, direction;
	int param_err = 1;
	int core_err = 0;

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

	if (!is_update || need_interactive_modifier(iforce, effect)) {
		param_err = make_interactive_modifier(iforce, mod_chunk,
			is_update,
			effect->u.interactive.right_saturation,
			effect->u.interactive.left_saturation,
			effect->u.interactive.right_coeff,
			effect->u.interactive.left_coeff,
			effect->u.interactive.deadband,
			effect->u.interactive.center);
		if (param_err) return param_err;
		set_bit(FF_MOD1_IS_USED, core_effect->flags);
	}

	switch ((test_bit(ABS_X, &effect->u.interactive.axis) ||
		test_bit(ABS_WHEEL, &effect->u.interactive.axis)) |
		(!!test_bit(ABS_Y, &effect->u.interactive.axis) << 1)) {

		case 0: /* Only one axis, choose orientation */
			mod1 = mod_chunk->start;
			mod2 = 0xffff;
			direction = effect->direction;
			axes = 0x20;
			break;

		case 1:	/* Only X axis */
			mod1 = mod_chunk->start;
			mod2 = 0xffff;
			direction = 0x5a00;
			axes = 0x40;
			break;

		case 2: /* Only Y axis */
			mod1 = 0xffff;
			mod2 = mod_chunk->start;
			direction = 0xb400;
			axes = 0x80;
			break;

		case 3:	/* Both X and Y axes */
			/* TODO: same setting for both axes is not mandatory */
			mod1 = mod_chunk->start;
			mod2 = mod_chunk->start;
			direction = 0x6000;
			axes = 0xc0;
			break;

		default:
			return -1;
	}

	if (!is_update || need_core(iforce, effect)) {
		core_err = make_core(iforce, effect->id,
			mod1, mod2,
			type, axes,
			effect->replay.length, effect->replay.delay,
			effect->trigger.button, effect->trigger.interval,
			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;
}