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