[PATCH 01/24] Add ff-memless-next module

[Michal Malý]

Add ff-memless-next module

Signed-off-by: Michal Malý <madcatxster@xxxxxxxxxxxxxxxxxx>
Tested-by: Elias Vanderstuyft <elias.vds@xxxxxxxxx>
---
 drivers/input/Kconfig                 |   11 +
 drivers/input/Makefile                |    1 +
 drivers/input/ff-memless-next.c       | 1036 +++++++++++++++++++++++++++++++++
 include/linux/input/ff-memless-next.h |  149 +++++
 4 files changed, 1197 insertions(+)
 create mode 100644 drivers/input/ff-memless-next.c
 create mode 100644 include/linux/input/ff-memless-next.h

diff --git a/drivers/input/Kconfig b/drivers/input/Kconfig
index a11ff74..3780962 100644
--- a/drivers/input/Kconfig
+++ b/drivers/input/Kconfig
@@ -38,6 +38,17 @@ config INPUT_FF_MEMLESS
 	  To compile this driver as a module, choose M here: the
 	  module will be called ff-memless.
 
+config INPUT_FF_MEMLESS_NEXT
+	tristate "New version of support for memoryless force-feedback devices"
+	help
+	  Say Y here to enable a new version of support for memoryless force
+	  feedback devices.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ff-memless-next.
+
 config INPUT_POLLDEV
 	tristate "Polled input device skeleton"
 	help
diff --git a/drivers/input/Makefile b/drivers/input/Makefile
index 5ca3f63..b4f11f5 100644
--- a/drivers/input/Makefile
+++ b/drivers/input/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_INPUT)		+= input-core.o
 input-core-y := input.o input-compat.o input-mt.o ff-core.o
 
 obj-$(CONFIG_INPUT_FF_MEMLESS)	+= ff-memless.o
+obj-$(CONFIG_INPUT_FF_MEMLESS_NEXT)	+= ff-memless-next.o
 obj-$(CONFIG_INPUT_POLLDEV)	+= input-polldev.o
 obj-$(CONFIG_INPUT_SPARSEKMAP)	+= sparse-keymap.o
 obj-$(CONFIG_INPUT_MATRIXKMAP)	+= matrix-keymap.o
diff --git a/drivers/input/ff-memless-next.c b/drivers/input/ff-memless-next.c
new file mode 100644
index 0000000..f343865
--- /dev/null
+++ b/drivers/input/ff-memless-next.c
@@ -0,0 +1,1036 @@
+/*
+ * Force feedback support for memoryless devices
+ *
+ * This module is based on "ff-memless" orignally written by Anssi Hannula.
+ * It is extended to support all force feedback effects currently supported
+ * by the Linux input stack.
+ * Logic of emulation of FF_RUMBLE through FF_PERIODIC provided by
+ * Elias Vanderstuyft <elias.vds@xxxxxxxxx>
+ *
+ * Copyright(c) 2014 Michal "MadCatX" Maly <madcatxster@xxxxxxxxxxxxxxxxxx>
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/jiffies.h>
+#include <linux/fixp-arith.h>
+#include <linux/input/ff-memless-next.h>
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Michal \"MadCatX\" Maly");
+MODULE_DESCRIPTION("Force feedback support for memoryless force feedback devices");
+
+#define FF_MAX_EFFECTS 16
+#define FF_MIN_EFFECT_LENGTH ((1000 / CONFIG_HZ) + 1)
+#define FF_EFFECT_STARTED 1
+#define FF_EFFECT_PLAYING 2
+#define FF_EFFECT_EMULATED 3
+
+enum mlnx_emulate {
+	EMUL_NOTHING,	/* Do not emulate anything */
+	EMUL_RUMBLE,	/* Emulate FF_RUMBLE with FF_PERIODIC */
+	EMUL_PERIODIC	/* Emulate FF_PERIODIC with FF_RUMBLE */
+};
+
+struct mlnx_effect {
+	struct ff_effect effect;
+	unsigned long flags;
+	unsigned long begin_at;
+	unsigned long stop_at;
+	unsigned long updated_at;
+	unsigned long attack_stop;
+	unsigned long fade_begin;
+	int repeat;
+};
+
+struct mlnx_device {
+	u8 combinable_playing;
+	u8 rumble_playing;
+	unsigned long update_rate_jiffies;
+	void *private;
+	struct mlnx_effect effects[FF_MAX_EFFECTS];
+	u16 gain;
+	struct timer_list timer;
+	struct input_dev *dev;
+	enum mlnx_emulate emul;
+
+	int (*control_effect)(struct input_dev *, void *,
+			      const struct mlnx_effect_command *);
+};
+
+static s32 mlnx_calculate_x_force(const s32 level,
+						  const u16 direction)
+{
+	s32 new = (level * -fixp_sin(direction)) >> FRAC_N;
+	pr_debug("x force: %d\n", new);
+	return new;
+}
+
+static s32 mlnx_calculate_y_force(const s32 level,
+						  const u16 direction)
+{
+	s32 new = (level * fixp_cos(direction)) >> FRAC_N;
+	pr_debug("y force: %d\n", new);
+	return new;
+}
+
+static bool mlnx_is_combinable(const struct ff_effect *effect)
+{
+	switch (effect->type) {
+	case FF_CONSTANT:
+	case FF_PERIODIC:
+	case FF_RAMP:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool mlnx_is_conditional(const struct ff_effect *effect)
+{
+	switch (effect->type) {
+	case FF_DAMPER:
+	case FF_FRICTION:
+	case FF_INERTIA:
+	case FF_SPRING:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void mlnx_clr_emulated(struct mlnx_effect *mlnxeff)
+{
+	__clear_bit(FF_EFFECT_EMULATED, &mlnxeff->flags);
+}
+
+static void mlnx_clr_playing(struct mlnx_effect *mlnxeff)
+{
+	__clear_bit(FF_EFFECT_PLAYING, &mlnxeff->flags);
+}
+
+static void mlnx_clr_started(struct mlnx_effect *mlnxeff)
+{
+	__clear_bit(FF_EFFECT_STARTED, &mlnxeff->flags);
+}
+
+static bool mlnx_is_emulated(const struct mlnx_effect *mlnxeff)
+{
+	return test_bit(FF_EFFECT_EMULATED, &mlnxeff->flags);
+}
+
+static bool mlnx_is_rumble(const struct ff_effect *effect)
+{
+	return effect->type == FF_RUMBLE;
+}
+
+static bool mlnx_is_playing(const struct mlnx_effect *mlnxeff)
+{
+	return test_bit(FF_EFFECT_PLAYING, &mlnxeff->flags);
+}
+
+static bool mlnx_is_started(const struct mlnx_effect *mlnxeff)
+{
+	return test_bit(FF_EFFECT_STARTED, &mlnxeff->flags);
+}
+
+static bool mlnx_test_set_playing(struct mlnx_effect *mlnxeff)
+{
+	return test_and_set_bit(FF_EFFECT_PLAYING, &mlnxeff->flags);
+}
+
+static const struct ff_envelope *mlnx_get_envelope(const struct ff_effect *effect)
+{
+	static const struct ff_envelope empty;
+
+	switch (effect->type) {
+	case FF_CONSTANT:
+		return &effect->u.constant.envelope;
+	case FF_PERIODIC:
+		return &effect->u.periodic.envelope;
+	case FF_RAMP:
+		return &effect->u.ramp.envelope;
+	default:
+		return &empty;
+	}
+}
+
+/* Some devices might have a limit on how many uncombinable effects
+ * can be played at once */
+static int mlnx_upload_conditional(struct mlnx_device *mlnxdev,
+				   const struct ff_effect *effect)
+{
+	struct mlnx_effect_command ecmd = {
+		.cmd = MLNX_UPLOAD_UNCOMB,
+		.u.uncomb.id = effect->id,
+		.u.uncomb.effect = effect
+	};
+	return mlnxdev->control_effect(mlnxdev->dev, mlnxdev->private, &ecmd);
+}
+
+static int mlnx_erase_conditional(struct mlnx_device *mlnxdev,
+				  const struct ff_effect *effect)
+{
+	struct mlnx_effect_command ecmd = {
+		.cmd = MLNX_ERASE_UNCOMB,
+		.u.uncomb.id = effect->id,
+		.u.uncomb.effect = effect
+	};
+	return mlnxdev->control_effect(mlnxdev->dev, mlnxdev->private, &ecmd);
+}
+
+static void mlnx_set_envelope_times(struct mlnx_effect *mlnxeff)
+{
+	const struct ff_effect *effect = &mlnxeff->effect;
+	const struct ff_envelope *envelope = mlnx_get_envelope(effect);
+
+	if (envelope->attack_length) {
+		unsigned long j = msecs_to_jiffies(envelope->attack_length);
+		mlnxeff->attack_stop = mlnxeff->begin_at + j;
+	}
+	if (effect->replay.length && envelope->fade_length) {
+		unsigned long j = msecs_to_jiffies(envelope->fade_length);
+		mlnxeff->fade_begin = mlnxeff->stop_at - j;
+	}
+}
+
+static void mlnx_set_trip_times(struct mlnx_effect *mlnxeff,
+				const unsigned long now)
+{
+	const struct ff_effect *effect = &mlnxeff->effect;
+	const u16 replay_delay = effect->replay.delay;
+	const u16 replay_length = effect->replay.length;
+
+	mlnxeff->begin_at = now + msecs_to_jiffies(replay_delay);
+	mlnxeff->stop_at = mlnxeff->begin_at + msecs_to_jiffies(replay_length);
+	mlnxeff->updated_at = mlnxeff->begin_at;
+}
+
+static void mlnx_start_effect(struct mlnx_effect *mlnxeff)
+{
+	const unsigned long now = jiffies;
+
+	mlnx_set_trip_times(mlnxeff, now);
+	mlnx_set_envelope_times(mlnxeff);
+	__set_bit(FF_EFFECT_STARTED, &mlnxeff->flags);
+}
+
+static void mlnx_stop_effect(struct mlnx_device *mlnxdev,
+			     const struct mlnx_effect *mlnxeff)
+{
+	switch (mlnxeff->effect.type) {
+	case FF_PERIODIC:
+		if (mlnx_is_emulated(mlnxeff)) {
+			if (--mlnxdev->rumble_playing == 0) {
+				const struct mlnx_effect_command c = {
+					.cmd = MLNX_STOP_RUMBLE
+				};
+				mlnxdev->control_effect(mlnxdev->dev,
+							mlnxdev->private, &c);
+			}
+			return;
+		}
+	case FF_CONSTANT:
+	case FF_RAMP:
+		if (--mlnxdev->combinable_playing == 0) {
+			const struct mlnx_effect_command c = {
+				.cmd = MLNX_STOP_COMBINED
+			};
+			mlnxdev->control_effect(mlnxdev->dev, mlnxdev->private,
+						&c);
+		}
+		return;
+	case FF_RUMBLE:
+		if (mlnx_is_emulated(mlnxeff)) {
+			if (--mlnxdev->combinable_playing == 0) {
+				const struct mlnx_effect_command c = {
+					.cmd = MLNX_STOP_COMBINED
+				};
+				mlnxdev->control_effect(mlnxdev->dev,
+							mlnxdev->private, &c);
+			}
+		} else {
+			if (--mlnxdev->rumble_playing == 0) {
+				const struct mlnx_effect_command c = {
+					.cmd = MLNX_STOP_RUMBLE
+				};
+				mlnxdev->control_effect(mlnxdev->dev,
+							mlnxdev->private, &c);
+			}
+		}
+		return;
+	case FF_DAMPER:
+	case FF_FRICTION:
+	case FF_INERTIA:
+	case FF_SPRING:
+	{
+		const struct mlnx_effect_command c = {
+			.cmd = MLNX_STOP_UNCOMB,
+			.u.uncomb.id = mlnxeff->effect.id,
+			.u.uncomb.effect = &mlnxeff->effect
+		};
+		mlnxdev->control_effect(mlnxdev->dev, mlnxdev->private, &c);
+		return;
+	}
+	default:
+		return;
+	}
+}
+
+static int mlnx_restart_effect(struct mlnx_device *mlnxdev,
+			       struct mlnx_effect *mlnxeff)
+{
+	const struct ff_effect *effect = &mlnxeff->effect;
+	const unsigned long now = jiffies;
+
+	if (mlnx_is_combinable(effect)) {
+		if (effect->replay.delay)
+			mlnx_stop_effect(mlnxdev, mlnxeff);
+		else {
+			if (mlnx_is_emulated(mlnxeff))
+				mlnxdev->rumble_playing--;
+			else
+				mlnxdev->combinable_playing--;
+		}
+	} else if (mlnx_is_rumble(effect)) {
+		if (effect->replay.delay)
+			mlnx_stop_effect(mlnxdev, mlnxeff);
+		else {
+			if (mlnx_is_emulated(mlnxeff))
+				mlnxdev->combinable_playing--;
+			else
+				mlnxdev->rumble_playing--;
+		}
+	} else if (mlnx_is_conditional(effect)) {
+		int ret;
+		if (effect->replay.delay)
+			mlnx_stop_effect(mlnxdev, mlnxeff);
+
+		ret = mlnx_upload_conditional(mlnxdev, &mlnxeff->effect);
+		if (ret)
+			return ret;
+	}
+
+	mlnx_set_trip_times(mlnxeff, now);
+	mlnx_set_envelope_times(mlnxeff);
+
+	return 0;
+}
+
+static s32 mlnx_apply_envelope(const struct mlnx_effect *mlnxeff,
+			       const s32 level)
+{
+	const struct ff_effect *effect = &mlnxeff->effect;
+	const struct ff_envelope *envelope = mlnx_get_envelope(effect);
+	const unsigned long now = jiffies;
+	const s32 alevel = abs(level);
+
+	/* Effect has an envelope with nonzero attack time */
+	if (envelope->attack_length && time_before(now, mlnxeff->attack_stop)) {
+		const s32 clength = jiffies_to_msecs(now - mlnxeff->begin_at);
+		const s32 alength = envelope->attack_length;
+		const s32 dlevel = (alevel - envelope->attack_level)
+				 * clength / alength;
+		return level < 0 ? -(dlevel + envelope->attack_level) :
+				    (dlevel + envelope->attack_level);
+	} else if (envelope->fade_length && time_before_eq(mlnxeff->fade_begin, now)) {
+		const s32 clength = jiffies_to_msecs(now - mlnxeff->fade_begin);
+		const s32 flength = envelope->fade_length;
+		const s32 dlevel = (envelope->fade_level - alevel)
+				 * clength / flength;
+		return level < 0 ? -(dlevel + alevel) : (dlevel + alevel);
+	}
+
+	return level;
+}
+
+static s32 mlnx_calculate_periodic(struct mlnx_effect *mlnxeff, const s32 level)
+{
+	const struct ff_effect *effect = &mlnxeff->effect;
+	const unsigned long now = jiffies;
+	const u16 period = effect->u.periodic.period;
+	const u16 phase = effect->u.periodic.phase;
+	const s16 offset = effect->u.periodic.offset;
+	s32 new = level;
+	u16 t = (jiffies_to_msecs(now - mlnxeff->begin_at) + phase) % period;
+
+	switch (effect->u.periodic.waveform) {
+	case FF_SINE:
+	{
+		u16 degrees = (360 * t) / period;
+		new = ((level * fixp_sin(degrees)) >> FRAC_N) + offset;
+		break;
+	}
+	case FF_SQUARE:
+	{
+		u16 degrees = (360 * t) / period;
+		new = level * (degrees < 180 ? 1 : -1) + offset;
+		break;
+	}
+	case FF_SAW_UP:
+		new = 2 * level * t / period - level + offset;
+		break;
+	case FF_SAW_DOWN:
+		new = level - 2 * level * t / period + offset;
+		break;
+	case FF_TRIANGLE:
+	{
+		new = (2 * abs(level - (2 * level * t) / period));
+		new = new - abs(level) + offset;
+		break;
+	}
+	case FF_CUSTOM:
+		pr_debug("Custom waveform is not handled by this driver\n");
+		return level;
+	default:
+		pr_err("Invalid waveform\n");
+		return level;
+	}
+
+	/* Ensure that the offset did not make the value exceed s16 range */
+	new = clamp(new, -0x7fff, 0x7fff);
+	pr_debug("level: %d, t: %u\n", new, t);
+	return new;
+}
+
+static s32 mlnx_calculate_ramp(const struct mlnx_effect *mlnxeff)
+{
+	const struct ff_effect *effect = &mlnxeff->effect;
+	const struct ff_envelope *envelope = mlnx_get_envelope(effect);
+	const unsigned long now = jiffies;
+	const u16 length = effect->replay.length;
+	const s16 mean = (effect->u.ramp.start_level + effect->u.ramp.end_level) / 2;
+	const u16 t = jiffies_to_msecs(now - mlnxeff->begin_at);
+	s32 start = effect->u.ramp.start_level;
+	s32 end = effect->u.ramp.end_level;
+	s32 new;
+
+	if (envelope->attack_length && time_before(now, mlnxeff->attack_stop)) {
+		const s32 clength = jiffies_to_msecs(now - mlnxeff->begin_at);
+		const s32 alength = envelope->attack_length;
+		s32 attack_level;
+		if (end > start)
+			attack_level = mean - envelope->attack_level;
+		else
+			attack_level = mean + envelope->attack_level;
+		start = (start - attack_level) * clength / alength
+		      + attack_level;
+	} else if (envelope->fade_length && time_before_eq(mlnxeff->fade_begin, now)) {
+		const s32 clength = jiffies_to_msecs(now - mlnxeff->fade_begin);
+		const s32 flength = envelope->fade_length;
+		s32 fade_level;
+		if (end > start)
+			fade_level = mean + envelope->fade_level;
+		else
+			fade_level = mean - envelope->fade_level;
+		end = (fade_level - end) * clength / flength + end;
+	}
+
+	new = ((end - start) * t) / length + start;
+	new = clamp(new, -0x7fff, 0x7fff);
+	pr_debug("RAMP level: %d, t: %u\n", new, t);
+	return new;
+}
+
+static void mlnx_destroy(struct ff_device *dev)
+{
+	struct mlnx_device *mlnxdev = dev->private;
+	del_timer_sync(&mlnxdev->timer);
+
+	kfree(mlnxdev->private);
+}
+
+static unsigned long mlnx_get_envelope_update_time(const struct mlnx_effect *mlnxeff,
+						   const unsigned long update_rate_jiffies)
+{
+	const struct ff_effect *effect = &mlnxeff->effect;
+	const struct ff_envelope *envelope = mlnx_get_envelope(effect);
+	const unsigned long now = jiffies;
+	unsigned long fade_next;
+
+	/* Effect has an envelope with nonzero attack time */
+	if (envelope->attack_length && time_before(now, mlnxeff->attack_stop)) {
+		if (time_before(mlnxeff->updated_at + update_rate_jiffies, mlnxeff->attack_stop))
+			return mlnxeff->updated_at + update_rate_jiffies;
+
+		return mlnxeff->attack_stop;
+	}
+
+	/* Effect has an envelope with nonzero fade time */
+	if (mlnxeff->effect.replay.length) {
+		if (!envelope->fade_length)
+			return mlnxeff->stop_at;
+
+		/* Schedule the next update when the fade begins */
+		if (time_before(mlnxeff->updated_at, mlnxeff->fade_begin))
+			return mlnxeff->fade_begin;
+
+		/* Already fading */
+		fade_next = mlnxeff->updated_at + update_rate_jiffies;
+
+		/* Schedule update when the effect stops */
+		if (time_after(fade_next, mlnxeff->stop_at))
+			return mlnxeff->stop_at;
+		/* Schedule update at the next checkpoint */
+			return fade_next;
+	}
+
+	/* There is no envelope */
+
+	/* Prevent the effect from being started twice */
+	if (mlnxeff->begin_at == now && mlnx_is_playing(mlnxeff))
+		return now - 1;
+
+	return mlnxeff->begin_at;
+}
+
+static unsigned long mlnx_get_update_time(struct mlnx_effect *mlnxeff,
+				const unsigned long update_rate_jiffies)
+{
+	const unsigned long now = jiffies;
+	unsigned long time, update_periodic;
+
+	switch (mlnxeff->effect.type) {
+	/* Constant effect does not change with time, but it can have
+	 * an envelope and a duration */
+	case FF_CONSTANT:
+		return mlnx_get_envelope_update_time(mlnxeff,
+						     update_rate_jiffies);
+	/* Periodic and ramp effects have to be periodically updated */
+	case FF_PERIODIC:
+	case FF_RAMP:
+		time = mlnx_get_envelope_update_time(mlnxeff,
+						     update_rate_jiffies);
+		if (mlnx_is_emulated(mlnxeff))
+			update_periodic = mlnxeff->stop_at;
+		else
+			update_periodic = mlnxeff->updated_at +
+					  update_rate_jiffies;
+
+		/* Periodic effect has to be updated earlier than envelope
+		 * or envelope update time is in the past */
+		if (time_before(update_periodic, time) || time_before(time, now))
+			return update_periodic;
+		/* Envelope needs to be updated */
+		return time;
+	case FF_RUMBLE:
+		if (mlnx_is_emulated(mlnxeff))
+			return mlnxeff->updated_at + update_rate_jiffies;
+	case FF_DAMPER:
+	case FF_FRICTION:
+	case FF_INERTIA:
+	case FF_SPRING:
+	default:
+		if (time_after_eq(mlnxeff->begin_at, now))
+			return mlnxeff->begin_at;
+
+		return mlnxeff->stop_at;
+	}
+}
+
+static void mlnx_schedule_playback(struct mlnx_device *mlnxdev)
+{
+	struct mlnx_effect *mlnxeff;
+	const unsigned long now = jiffies;
+	int events = 0;
+	int i;
+	unsigned long earliest = 0;
+	unsigned long time;
+
+	/* Iterate over all effects and determine the earliest
+	 * time when we have to attend to any */
+	for (i = 0; i < FF_MAX_EFFECTS; i++) {
+		mlnxeff = &mlnxdev->effects[i];
+
+		if (!mlnx_is_started(mlnxeff))
+			continue; /* Effect is not started, skip it */
+
+		if (mlnx_is_playing(mlnxeff))
+			time = mlnx_get_update_time(mlnxeff,
+						mlnxdev->update_rate_jiffies);
+		else
+			time = mlnxeff->begin_at;
+
+		pr_debug("Update time for effect %d: %lu\n", i, time);
+
+		/* Scheduled time is in the future and is either
+		 * before the current earliest time or it is
+		 * the first valid time value in this pass */
+		if (time_before_eq(now, time) &&
+		    (++events == 1 || time_before(time, earliest)))
+			earliest = time;
+	}
+
+	if (events) {
+		pr_debug("Events: %d, earliest: %lu\n", events, earliest);
+		mod_timer(&mlnxdev->timer, earliest);
+	} else {
+		pr_debug("No events, deactivating timer\n");
+		del_timer(&mlnxdev->timer);
+	}
+}
+
+static u16 mlnx_calculate_rumble_direction(const u32 total_mag, const u16 total_dir,
+					   const u32 new_mag, const u16 new_dir)
+{
+	if (!new_mag)
+		return total_dir;
+	if (!total_mag)
+		return new_dir;
+	return (((total_dir >> 1) * total_mag +
+		(new_dir >> 1) * new_mag) /
+		(total_mag + new_mag)) << 1;
+}
+
+static void mlnx_add_force(struct mlnx_effect *mlnxeff, s32 *cfx, s32 *cfy,
+			   const u16 gain)
+{
+	const struct ff_effect *effect = &mlnxeff->effect;
+	u16 direction;
+	s32 level;
+
+	pr_debug("Processing effect type %d, ID %d\n",
+		 mlnxeff->effect.type, mlnxeff->effect.id);
+
+	direction = mlnxeff->effect.direction * 360 / 0xffff;
+	pr_debug("Direction deg: %u\n", direction);
+
+	switch (mlnxeff->effect.type) {
+	case FF_CONSTANT:
+		level = mlnx_apply_envelope(mlnxeff, effect->u.constant.level);
+		break;
+	case FF_PERIODIC:
+		level = mlnx_apply_envelope(mlnxeff,
+					    effect->u.periodic.magnitude);
+		level = mlnx_calculate_periodic(mlnxeff, level);
+		break;
+	case FF_RAMP:
+		level = mlnx_calculate_ramp(mlnxeff);
+		break;
+	default:
+		pr_err("Effect %d not handled by mlnx_add_force\n",
+		       mlnxeff->effect.type);
+		return;
+	}
+
+	*cfx += mlnx_calculate_x_force(level, direction) * gain / 0xffff;
+	*cfy += mlnx_calculate_y_force(level, direction) * gain / 0xffff;
+}
+
+static void mlnx_add_rumble(const struct mlnx_effect *mlnxeff, u32 *strong_mag,
+			    u32 *weak_mag, u16 *strong_dir,
+			    u16 *weak_dir, const u16 gain)
+{
+	const struct ff_effect *eff = &mlnxeff->effect;
+	const struct ff_rumble_effect *reff = &mlnxeff->effect.u.rumble;
+	const u32 new_strong_mag = (u32)reff->strong_magnitude * gain / 0xffffU;
+	const u32 new_weak_mag = (u32)reff->weak_magnitude * gain / 0xffffU;
+
+	*strong_dir = mlnx_calculate_rumble_direction(*strong_mag, *strong_dir,
+						      new_strong_mag,
+						      eff->direction);
+	*weak_dir = mlnx_calculate_rumble_direction(*weak_mag, *weak_dir,
+						    new_weak_mag,
+						    eff->direction);
+	*strong_mag += new_strong_mag;
+	*weak_mag += new_weak_mag;
+}
+
+static void mlnx_add_emul_periodic(const struct mlnx_effect *mlnxeff,
+				   u32 *strong_mag, u32 *weak_mag,
+				   u16 *strong_dir, u16 *weak_dir,
+				   const u16 gain)
+{
+	const struct ff_effect *eff = &mlnxeff->effect;
+	const u32 level = (u32)abs(mlnx_apply_envelope(mlnxeff,
+						  eff->u.periodic.magnitude)) * gain / 0x7fffU;
+
+	*strong_dir = mlnx_calculate_rumble_direction(*strong_mag, *strong_dir,
+						      level, eff->direction);
+	*weak_dir = mlnx_calculate_rumble_direction(*weak_mag, *weak_dir,
+						    level, eff->direction);
+
+	*strong_mag += level;
+	*weak_mag += level;
+}
+
+static void mlnx_add_emul_rumble(const struct mlnx_effect *mlnxeff, s32 *cfx,
+				 s32 *cfy, const u16 gain,
+				 const unsigned long now,
+				 const unsigned long update_rate_jiffies)
+{
+	const struct ff_effect *effect = &mlnxeff->effect;
+	const u16 strong = effect->u.rumble.strong_magnitude;
+	const u16 weak = effect->u.rumble.weak_magnitude;
+	/* To calculate 't', we pretend that mlnxeff->begin_at == 0, thus t == now.  */
+	/* This will synchronise all simultaneously playing emul rumble effects,     */
+	/* otherwise non-deterministic phase-inversions could occur depending on     */
+	/* upload time, which could lead to undesired cancellation of these effects. */
+	const unsigned long t = now % (4UL * update_rate_jiffies);
+	s32 level = 0;
+	bool direction_up;
+	bool direction_left;
+
+	if (strong)
+		level += (strong / 4) * (t < 2UL * update_rate_jiffies ? 1 : -1);
+	if (weak)
+		level += (weak / 4) * (t < 2UL * update_rate_jiffies ?
+					(t < 1UL * update_rate_jiffies ? 1 : -1) :
+					(t < 3UL * update_rate_jiffies ? 1 : -1));
+	direction_up = (effect->direction > 0x3fffU && effect->direction <= 0xbfffU);
+	direction_left = (effect->direction <= 0x7fffU);
+
+	pr_debug("Emulated cf: %d, t: %lu, n: %lu, begin: %lu, diff: %lu j: %lu\n",
+		 level, t, now, mlnxeff->begin_at, now - mlnxeff->begin_at,
+		 update_rate_jiffies);
+	level = (level * gain) / 0xffff;
+	*cfx += direction_left ? -level : level;
+	*cfy += direction_up ? -level : level;
+}
+
+static void mlnx_play_effects(struct mlnx_device *mlnxdev)
+{
+	const u16 gain = mlnxdev->gain;
+	const unsigned long now = jiffies;
+	int i;
+	s32 cfx = 0;
+	s32 cfy = 0;
+	u32 strong_mag = 0;
+	u32 weak_mag = 0;
+	u16 strong_dir = 0;
+	u16 weak_dir = 0;
+
+	for (i = 0; i < FF_MAX_EFFECTS; i++) {
+		struct mlnx_effect *mlnxeff = &mlnxdev->effects[i];
+
+		if (!mlnx_is_started(mlnxeff)) {
+			pr_debug("Effect %hd/%d not started\n",
+				 mlnxeff->effect.id, i);
+			continue;
+		}
+
+		if (time_before(now, mlnxeff->begin_at)) {
+			pr_debug("Effect %hd/%d begins at a later time\n",
+				 mlnxeff->effect.id, i);
+			continue;
+		}
+
+		if (time_before_eq(mlnxeff->stop_at, now) && mlnxeff->effect.replay.length) {
+			pr_debug("Effect %hd/%d has to be stopped\n",
+				 mlnxeff->effect.id, i);
+
+			mlnx_clr_playing(mlnxeff);
+			if (--mlnxeff->repeat > 0)
+				mlnx_restart_effect(mlnxdev, mlnxeff);
+			else {
+				mlnx_stop_effect(mlnxdev, mlnxeff);
+				mlnx_clr_started(mlnxeff);
+				mlnx_clr_emulated(mlnxeff);
+				if (mlnx_is_conditional(&mlnxeff->effect))
+					mlnx_erase_conditional(mlnxdev, &mlnxeff->effect);
+			}
+
+			continue;
+		}
+
+		switch (mlnxeff->effect.type) {
+		case FF_PERIODIC:
+			if (mlnxdev->emul == EMUL_PERIODIC) {
+				if (!mlnx_test_set_playing(mlnxeff)) {
+					mlnxdev->rumble_playing++;
+					pr_debug("Starting emul periodic, total rumble %u\n",
+						 mlnxdev->rumble_playing);
+				}
+				__set_bit(FF_EFFECT_EMULATED, &mlnxeff->flags);
+				mlnx_add_emul_periodic(mlnxeff, &strong_mag, &weak_mag,
+						       &strong_dir, &weak_dir, gain);
+				break;
+			}
+		case FF_CONSTANT:
+		case FF_RAMP:
+			if (!mlnx_test_set_playing(mlnxeff)) {
+				mlnxdev->combinable_playing++;
+				pr_debug("Starting combinable effect, total %u\n",
+					 mlnxdev->combinable_playing);
+			}
+			mlnx_add_force(mlnxeff, &cfx, &cfy, gain);
+			break;
+		case FF_RUMBLE:
+			if (mlnxdev->emul == EMUL_RUMBLE) {
+				if (!mlnx_test_set_playing(mlnxeff)) {
+					mlnxdev->combinable_playing++;
+					pr_debug("Starting emul rumble, total comb %u\n",
+						 mlnxdev->combinable_playing);
+				}
+				__set_bit(FF_EFFECT_EMULATED, &mlnxeff->flags);
+				mlnx_add_emul_rumble(mlnxeff, &cfx, &cfy, gain, now,
+						     mlnxdev->update_rate_jiffies);
+			} else {
+				if (!mlnx_test_set_playing(mlnxeff)) {
+					mlnxdev->rumble_playing++;
+					pr_debug("Starting rumble effect, total %u\n",
+						 mlnxdev->rumble_playing);
+				}
+				mlnx_add_rumble(mlnxeff, &strong_mag, &weak_mag,
+						&strong_dir, &weak_dir, gain);
+			}
+			break;
+		case FF_DAMPER:
+		case FF_FRICTION:
+		case FF_INERTIA:
+		case FF_SPRING:
+			if (!mlnx_test_set_playing(mlnxeff)) {
+				const struct mlnx_effect_command ecmd = {
+					.cmd = MLNX_START_UNCOMB,
+					.u.uncomb.id = i,
+					.u.uncomb.effect = &mlnxeff->effect
+				};
+				mlnxdev->control_effect(mlnxdev->dev,
+						      mlnxdev->private, &ecmd);
+			}
+			break;
+		default:
+			pr_debug("Unhandled type of effect\n");
+		}
+		mlnxeff->updated_at = now;
+	}
+
+	if (mlnxdev->combinable_playing) {
+		const struct mlnx_effect_command ecmd = {
+			.cmd = MLNX_START_COMBINED,
+			.u.simple_force = {
+				.x = clamp(cfx, -0x7fff, 0x7fff),
+				.y = clamp(cfy, -0x7fff, 0x7fff)
+			}
+		};
+		mlnxdev->control_effect(mlnxdev->dev, mlnxdev->private, &ecmd);
+	}
+	if (mlnxdev->rumble_playing) {
+		const struct mlnx_effect_command ecmd = {
+			.cmd = MLNX_START_RUMBLE,
+			.u.rumble_force = {
+				.strong = clamp(strong_mag, (u32)0, (u32)0xffffU),
+				.weak = clamp(weak_mag, (u32)0, (u32)0xffffU),
+				.strong_dir = clamp(strong_dir, (u16)0, (u16)0xffffU),
+				.weak_dir = clamp(weak_dir, (u16)0, (u16)0xffffU)
+			}
+		};
+		mlnxdev->control_effect(mlnxdev->dev, mlnxdev->private, &ecmd);
+	}
+
+	mlnx_schedule_playback(mlnxdev);
+}
+
+static void mlnx_set_gain(struct input_dev *dev, u16 gain)
+{
+	struct mlnx_device *mlnxdev = dev->ff->private;
+	int i;
+
+	mlnxdev->gain = gain;
+
+	for (i = 0; i < FF_MAX_EFFECTS; i++) {
+		struct mlnx_effect *eff = &mlnxdev->effects[i];
+		if (eff == NULL)
+			continue;
+		if (mlnx_is_playing(eff)) {
+			if (mlnx_is_combinable(&eff->effect)) {
+				mlnx_clr_playing(eff);
+				if (mlnx_is_emulated(eff))
+					--mlnxdev->rumble_playing;
+				else
+					--mlnxdev->combinable_playing;
+			} else if (mlnx_is_rumble(&eff->effect)) {
+				mlnx_clr_playing(eff);
+				if (mlnx_is_emulated(eff))
+					--mlnxdev->combinable_playing;
+				else
+					--mlnxdev->rumble_playing;
+			}
+		}
+	}
+
+	mlnx_play_effects(mlnxdev);
+}
+
+static int mlnx_startstop(struct input_dev *dev, int effect_id, int repeat)
+{
+	struct mlnx_device *mlnxdev = dev->ff->private;
+	struct mlnx_effect *mlnxeff = &mlnxdev->effects[effect_id];
+	int ret;
+
+	if (repeat > 0) {
+		pr_debug("Starting effect ID %d\n", effect_id);
+		mlnxeff->repeat = repeat;
+
+		if (!mlnx_is_started(mlnxeff)) {
+			/* Check that device has a free effect slot */
+			if (mlnx_is_conditional(&mlnxeff->effect)) {
+				ret = mlnx_upload_conditional(mlnxdev, &mlnxeff->effect);
+				if (ret) {
+					/* Device effect slots are all occupied */
+					pr_debug("No free effect slot for EID %d\n", effect_id);
+					return ret;
+				}
+			}
+			mlnx_start_effect(mlnxeff);
+		}
+	} else {
+		pr_debug("Stopping effect ID %d\n", effect_id);
+		if (mlnx_is_started(mlnxeff)) {
+			if (mlnx_is_playing(mlnxeff)) {
+				mlnx_clr_playing(mlnxeff);
+				mlnx_stop_effect(mlnxdev, mlnxeff);
+			}
+			mlnx_clr_started(mlnxeff);
+			mlnx_clr_emulated(mlnxeff);
+
+			if (mlnx_is_conditional(&mlnxeff->effect))
+				return mlnx_erase_conditional(mlnxdev, &mlnxeff->effect);
+		} else {
+			pr_debug("Effect ID %d already stopped\n", effect_id);
+			return 0;
+		}
+	}
+	mlnx_play_effects(mlnxdev);
+
+	return 0;
+}
+
+static void mlnx_timer_fired(unsigned long data)
+{
+	struct input_dev *dev = (struct input_dev *)data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->event_lock, flags);
+	mlnx_play_effects(dev->ff->private);
+	spin_unlock_irqrestore(&dev->event_lock, flags);
+}
+
+static int mlnx_upload(struct input_dev *dev, struct ff_effect *effect,
+		       struct ff_effect *old)
+{
+	struct mlnx_device *mlnxdev = dev->ff->private;
+	struct mlnx_effect *mlnxeff = &mlnxdev->effects[effect->id];
+	const u16 length = effect->replay.length;
+	const u16 delay = effect->replay.delay;
+	int ret, fade_from;
+
+	/* Effect's timing is below kernel timer resolution */
+	if (length && length < FF_MIN_EFFECT_LENGTH)
+		effect->replay.length = FF_MIN_EFFECT_LENGTH;
+	if (delay && delay < FF_MIN_EFFECT_LENGTH)
+		effect->replay.delay = FF_MIN_EFFECT_LENGTH;
+
+	/* Periodic effects must have a non-zero period */
+	if (effect->type == FF_PERIODIC) {
+		if (!effect->u.periodic.period)
+			return -EINVAL;
+	}
+	/* Ramp effects cannot be infinite */
+	if (effect->type == FF_RAMP && !length)
+		return -EINVAL;
+
+	if (mlnx_is_combinable(effect)) {
+		const struct ff_envelope *envelope = mlnx_get_envelope(effect);
+
+		/* Infinite effects cannot fade */
+		if (!length && envelope->fade_length > 0)
+			return -EINVAL;
+		/* Fade length cannot be greater than effect duration */
+		fade_from = (int)length - (int)envelope->fade_length;
+		if (fade_from < 0)
+			return -EINVAL;
+		/* Envelope cannot start fading before it finishes attacking */
+		if (fade_from < envelope->attack_length && fade_from > 0)
+			return -EINVAL;
+	}
+
+
+	spin_lock_irq(&dev->event_lock);
+	mlnxeff->effect = *effect; /* Keep internal copy of the effect */
+	/* Check if the effect being modified is playing */
+	if (mlnx_is_started(mlnxeff)) {
+		if (mlnx_is_playing(mlnxeff)) {
+			mlnx_clr_playing(mlnxeff);
+			ret = mlnx_restart_effect(mlnxdev, mlnxeff);
+
+			if (ret) {
+				/* Restore the original effect */
+				if (old)
+					mlnxeff->effect = *old;
+				spin_unlock_irq(&dev->event_lock);
+				return ret;
+			}
+		}
+
+		mlnx_schedule_playback(mlnxdev);
+	}
+
+	spin_unlock_irq(&dev->event_lock);
+
+	return 0;
+}
+
+int input_ff_create_mlnx(struct input_dev *dev, void *data,
+			 int (*control_effect)(struct input_dev *, void *, const struct mlnx_effect_command *),
+			 const u16 update_rate)
+{
+	struct mlnx_device *mlnxdev;
+	int ret;
+
+	mlnxdev = kzalloc(sizeof(*mlnxdev), GFP_KERNEL);
+	if (!mlnxdev)
+		return -ENOMEM;
+
+	mlnxdev->dev = dev;
+	mlnxdev->private = data;
+	mlnxdev->control_effect = control_effect;
+	mlnxdev->gain = 0xffff;
+	mlnxdev->update_rate_jiffies = update_rate < FF_MIN_EFFECT_LENGTH ?
+				       FF_MIN_EFFECT_LENGTH : update_rate;
+	input_set_capability(dev, EV_FF, FF_GAIN);
+	setup_timer(&mlnxdev->timer, mlnx_timer_fired, (unsigned long)dev);
+
+	/* Set up effect emulation if needed */
+	if (test_bit(FF_PERIODIC, dev->ffbit) &&
+	    !test_bit(FF_RUMBLE, dev->ffbit)) {
+		set_bit(FF_RUMBLE, dev->ffbit);
+		mlnxdev->emul = EMUL_RUMBLE;
+		pr_debug("Emulating RUMBLE with PERIODIC\n");
+	} else if (test_bit(FF_RUMBLE, dev->ffbit) &&
+		   !test_bit(FF_PERIODIC, dev->ffbit)) {
+		set_bit(FF_PERIODIC, dev->ffbit);
+		set_bit(FF_SINE, dev->ffbit);
+		set_bit(FF_SQUARE, dev->ffbit);
+		set_bit(FF_TRIANGLE, dev->ffbit);
+		set_bit(FF_SAW_DOWN, dev->ffbit);
+		set_bit(FF_SAW_UP, dev->ffbit);
+		mlnxdev->emul = EMUL_PERIODIC;
+		pr_debug("Emulating PERIODIC with RUMBLE\n");
+	} else {
+		mlnxdev->emul = EMUL_NOTHING;
+		pr_debug("No effect emulation is necessary\n");
+	}
+
+	ret = input_ff_create(dev, FF_MAX_EFFECTS);
+	if (ret) {
+		kfree(mlnxdev);
+		return ret;
+	}
+
+
+	dev->ff->private = mlnxdev;
+	dev->ff->upload = mlnx_upload;
+	dev->ff->set_gain = mlnx_set_gain;
+	dev->ff->destroy = mlnx_destroy;
+	dev->ff->playback = mlnx_startstop;
+
+	pr_debug("Device successfully registered.\n");
+	return 0;
+}
+EXPORT_SYMBOL_GPL(input_ff_create_mlnx);
diff --git a/include/linux/input/ff-memless-next.h b/include/linux/input/ff-memless-next.h
new file mode 100644
index 0000000..2f15897
--- /dev/null
+++ b/include/linux/input/ff-memless-next.h
@@ -0,0 +1,149 @@
+#include <linux/input.h>
+
+/** DEFINITION OF TERMS
+ *
+ * Combined effect - An effect whose force is a superposition of forces
+ *                   generated by all effects that can be added together.
+ *                   Only one combined effect can be playing at a time.
+ *                   Effects that can be added together to create a combined
+ *                   effect are FF_CONSTANT, FF_PERIODIC and FF_RAMP.
+ * Uncombinable effect - An effect that cannot be combined with another effect.
+ *                       All conditional effects - FF_DAMPER, FF_FRICTION,
+ *                       FF_INERTIA and FF_SPRING are uncombinable.
+ *                       Number of uncombinable effects playing simultaneously
+ *                       depends on the capabilities of the hardware.
+ * Rumble effect - An effect generated by device's rumble motors instead of
+ *                 force feedback actuators.
+ *
+ *
+ * HANDLING OF UNCOMBINABLE EFFECTS
+ *
+ * Uncombinable effects cannot be combined together into just one effect, at
+ * least not in a clear and obvious manner. Therefore these effects have to
+ * be handled individually by ff-memless-next. Handling of these effects is
+ * left entirely to the hardware-specific driver, ff-memless-next merely
+ * passes these effects to the hardware-specific driver at appropriate time.
+ * ff-memless-next provides the UPLOAD command to notify the hardware-specific
+ * driver that the userspace is about to request playback of an uncombinable
+ * effect. The hardware-specific driver shall take all steps needed to make
+ * the device ready to play the effect when it receives the UPLOAD command.
+ * The actual playback shall commence when START_UNCOMB command is received.
+ * Opposite to the UPLOAD command is the ERASE command which tells
+ * the hardware-specific driver that the playback has finished and that
+ * the effect will not be restarted. STOP_UNCOMB command tells
+ * the hardware-specific driver that the playback shall stop but the device
+ * shall still be ready to resume the playback immediately.
+ *
+ * In case it is not possible to make the device ready to play an uncombinable
+ * effect (all hardware effect slots are occupied), the hardware-specific
+ * driver may return an error when it receives an UPLOAD command. If the
+ * hardware-specific driver returns 0, the upload is considered successful.
+ * START_UNCOMB and STOP_UNCOMB commands cannot fail and the device must always
+ * start the playback of the requested effect if the UPLOAD command of the
+ * respective effect has been successful. ff-memless-next will never send
+ * a START/STOP_UNCOMB command for an effect that has not been uploaded
+ * successfully, nor will it send an ERASE command for an effect that is
+ * playing (= has been started with START_UNCOMB command).
+ */
+
+enum mlnx_commands {
+	/* Start or update a combined effect. This command is sent whenever
+	 * a FF_CONSTANT, FF_PERIODIC or FF_RAMP is started, stopped or
+	 * updated by userspace, when the applied envelopes are recalculated
+	 * or when periodic effects are recalculated. */
+	MLNX_START_COMBINED,
+	/* Stop combined effect. This command is sent when all combinable
+	 * effects are stopped. */
+	MLNX_STOP_COMBINED,
+	/* Start or update a rumble effect. This command is sent whenever
+	 * a FF_RUMBLE effect is started or when its magnitudes or directions
+	 * change. */
+	MLNX_START_RUMBLE,
+	/* Stop a rumble effect. This command is sent when all FF_RUMBLE
+	 * effects are stopped. */
+	MLNX_STOP_RUMBLE,
+	/* Start or update an uncombinable effect. This command is sent
+	 * whenever an uncombinable effect is started or updated. */
+	MLNX_START_UNCOMB,
+	/* Stop uncombinable effect. This command is sent when an uncombinable
+	 * effect is stopped. */
+	MLNX_STOP_UNCOMB,
+	/* Upload uncombinable effect to device. This command is sent when the
+	 * effect is started from userspace. It is up to the hardware-specific
+	 * driver to handle this situation.
+	 */
+	MLNX_UPLOAD_UNCOMB,
+	/* Remove uncombinable effect from device, This command is sent when
+	 * and uncombinable effect has finished playing and will not be
+	 * restarted.
+	 */
+	MLNX_ERASE_UNCOMB
+};
+
+/** struct mlnx_simple_force - holds constant forces along X and Y axis
+ * @x: Force along X axis. Negative value denotes force pulling to the left,
+ *     positive value denotes force pulling to the right.
+ * @y: Force along Y axis. Negative value denotes force denotes force pulling
+ *     away from the user, positive value denotes force pulling towards
+ *     the user.
+ */
+struct mlnx_simple_force {
+	const s32 x;
+	const s32 y;
+};
+
+/** struct mlnx_rumble_force - holds information about rumble effect
+ * @strong: Magnitude of the strong vibration.
+ * @weak: Magnitude of the weak vibration.
+ * @strong_dir: Direction of the strong vibration expressed in the same way
+ *              as the direction of force feedback effect in struct ff_effect.
+ * @weak_dir: Direction of the weak vibration, same as above applies.
+ */
+struct mlnx_rumble_force {
+	const u32 strong;
+	const u32 weak;
+	const u16 strong_dir;
+	const u16 weak_dir;
+};
+
+/** struct mlnx_uncomb_effect - holds information about uncombinable effect
+ * @id: Id of the effect assigned by ff-core.
+ * @effect: Pointer to the uncombinable effect stored in ff-memless-next module
+ *          Hardware-specific driver must not alter this.
+ */
+struct mlnx_uncomb_effect {
+	const int id;
+	const struct ff_effect *effect;
+};
+
+/** struct mlnx_commands - describes what action shall the force feedback
+ *                         device perform
+ * @cmd: Type of the action.
+ * @u: Data associated with the action.
+ */
+struct mlnx_effect_command {
+	const enum mlnx_commands cmd;
+	union {
+		const struct mlnx_simple_force simple_force;
+		const struct mlnx_rumble_force rumble_force;
+		const struct mlnx_uncomb_effect uncomb;
+	} u;
+};
+
+/** input_ff_create_mlnx() - Register a device within ff-memless-next and
+ *                           the kernel force feedback system
+ * @dev: Pointer to the struct input_dev associated with the device.
+ * @data: Any device-specific data that shall be passed to the callback.
+ *        function called by ff-memless-next when a force feedback action
+ *        shall be performed.
+ * @control_effect: Pointer to the callback function.
+ * @update_date: Delay in milliseconds between two recalculations of periodic
+ *               effects, ramp effects and envelopes. Note that this value will
+ *               never be lower than (CONFIG_HZ / 1000) + 1 regardless of the
+ *               value specified here. This is not a "hard" rate limiter.
+ *               Userspace still can submit effects at a rate faster than
+ *               this value.
+ */
+int input_ff_create_mlnx(struct input_dev *dev, void *data,
+			 int (*control_effect)(struct input_dev *, void *, const struct mlnx_effect_command *),
+			 const u16 update_rate);
-- 
1.9.1


--
To unsubscribe from this list: send the line "unsubscribe linux-kernel" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at  http://vger.kernel.org/majordomo-info.html
Please read the FAQ at  http://www.tux.org/lkml/