cs40l50-vibra.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * CS40L50 Advanced Haptic Driver with waveform memory,
  4 * integrated DSP, and closed-loop algorithms
  5 *
  6 * Copyright 2024 Cirrus Logic, Inc.
  7 *
  8 * Author: James Ogletree <james.ogletree@cirrus.com>
  9 */
 10
 11#include <linux/bitfield.h>
 12#include <linux/input.h>
 13#include <linux/mfd/cs40l50.h>
 14#include <linux/platform_device.h>
 15#include <linux/pm_runtime.h>
 16
 17/* Wavetables */
 18#define CS40L50_RAM_INDEX_START		0x1000000
 19#define CS40L50_RAM_INDEX_END		0x100007F
 20#define CS40L50_RTH_INDEX_START		0x1400000
 21#define CS40L50_RTH_INDEX_END		0x1400001
 22#define CS40L50_ROM_INDEX_START		0x1800000
 23#define CS40L50_ROM_INDEX_END		0x180001A
 24#define CS40L50_TYPE_PCM		8
 25#define CS40L50_TYPE_PWLE		12
 26#define CS40L50_PCM_ID			0x0
 27#define CS40L50_OWT_CUSTOM_DATA_SIZE	2
 28#define CS40L50_CUSTOM_DATA_MASK	0xFFFFU
 29
 30/* DSP */
 31#define CS40L50_GPIO_BASE		0x2804140
 32#define CS40L50_OWT_BASE		0x2805C34
 33#define CS40L50_OWT_SIZE		0x2805C38
 34#define CS40L50_OWT_NEXT		0x2805C3C
 35#define CS40L50_EFFECTS_MAX		1
 36
 37/* GPIO */
 38#define CS40L50_GPIO_NUM_MASK		GENMASK(14, 12)
 39#define CS40L50_GPIO_EDGE_MASK		BIT(15)
 40#define CS40L50_GPIO_MAPPING_NONE	0
 41#define CS40L50_GPIO_DISABLE		0x1FF
 42
 43enum cs40l50_bank_type {
 44	CS40L50_WVFRM_BANK_RAM,
 45	CS40L50_WVFRM_BANK_ROM,
 46	CS40L50_WVFRM_BANK_OWT,
 47	CS40L50_WVFRM_BANK_NUM,
 48};
 49
 50/* Describes an area in DSP memory populated by effects */
 51struct cs40l50_bank {
 52	enum cs40l50_bank_type type;
 53	u32 base_index;
 54	u32 max_index;
 55};
 56
 57struct cs40l50_effect {
 58	enum cs40l50_bank_type type;
 59	struct list_head list;
 60	u32 gpio_reg;
 61	u32 index;
 62	int id;
 63};
 64
 65/* Describes haptic interface of loaded DSP firmware */
 66struct cs40l50_vibra_dsp {
 67	struct cs40l50_bank *banks;
 68	u32 gpio_base_reg;
 69	u32 owt_offset_reg;
 70	u32 owt_size_reg;
 71	u32 owt_base_reg;
 72	u32 push_owt_cmd;
 73	u32 delete_owt_cmd;
 74	u32 stop_cmd;
 75	int (*write)(struct device *dev, struct regmap *regmap, u32 val);
 76};
 77
 78/* Describes configuration and state of haptic operations */
 79struct cs40l50_vibra {
 80	struct device *dev;
 81	struct regmap *regmap;
 82	struct input_dev *input;
 83	struct workqueue_struct *vib_wq;
 84	struct list_head effect_head;
 85	struct cs40l50_vibra_dsp dsp;
 86};
 87
 88struct cs40l50_work {
 89	struct cs40l50_vibra *vib;
 90	struct ff_effect *effect;
 91	struct work_struct work;
 92	s16 *custom_data;
 93	int custom_len;
 94	int count;
 95	int error;
 96};
 97
 98static struct cs40l50_bank cs40l50_banks[] = {
 99	{
100		.type =		CS40L50_WVFRM_BANK_RAM,
101		.base_index =	CS40L50_RAM_INDEX_START,
102		.max_index =	CS40L50_RAM_INDEX_END,
103	},
104	{
105		.type =		CS40L50_WVFRM_BANK_ROM,
106		.base_index =	CS40L50_ROM_INDEX_START,
107		.max_index =	CS40L50_ROM_INDEX_END,
108	},
109	{
110		.type =		CS40L50_WVFRM_BANK_OWT,
111		.base_index =	CS40L50_RTH_INDEX_START,
112		.max_index =	CS40L50_RTH_INDEX_END,
113	},
114};
115
116static struct cs40l50_vibra_dsp cs40l50_dsp = {
117	.banks =		cs40l50_banks,
118	.gpio_base_reg =	CS40L50_GPIO_BASE,
119	.owt_base_reg =		CS40L50_OWT_BASE,
120	.owt_offset_reg =	CS40L50_OWT_NEXT,
121	.owt_size_reg =		CS40L50_OWT_SIZE,
122	.push_owt_cmd =		CS40L50_OWT_PUSH,
123	.delete_owt_cmd =	CS40L50_OWT_DELETE,
124	.stop_cmd =		CS40L50_STOP_PLAYBACK,
125	.write =		cs40l50_dsp_write,
126};
127
128static struct cs40l50_effect *cs40l50_find_effect(int id, struct list_head *effect_head)
129{
130	struct cs40l50_effect *effect;
131
132	list_for_each_entry(effect, effect_head, list)
133		if (effect->id == id)
134			return effect;
135
136	return NULL;
137}
138
139static int cs40l50_effect_bank_set(struct cs40l50_work *work_data,
140				   struct cs40l50_effect *effect)
141{
142	s16 bank_type = work_data->custom_data[0] & CS40L50_CUSTOM_DATA_MASK;
143
144	if (bank_type >= CS40L50_WVFRM_BANK_NUM) {
145		dev_err(work_data->vib->dev, "Invalid bank (%d)\n", bank_type);
146		return -EINVAL;
147	}
148
149	if (work_data->custom_len > CS40L50_OWT_CUSTOM_DATA_SIZE)
150		effect->type = CS40L50_WVFRM_BANK_OWT;
151	else
152		effect->type = bank_type;
153
154	return 0;
155}
156
157static int cs40l50_effect_index_set(struct cs40l50_work *work_data,
158				    struct cs40l50_effect *effect)
159{
160	struct cs40l50_vibra *vib = work_data->vib;
161	struct cs40l50_effect *owt_effect;
162	u32 base_index, max_index;
163
164	base_index = vib->dsp.banks[effect->type].base_index;
165	max_index = vib->dsp.banks[effect->type].max_index;
166
167	effect->index = base_index;
168
169	switch (effect->type) {
170	case CS40L50_WVFRM_BANK_OWT:
171		list_for_each_entry(owt_effect, &vib->effect_head, list)
172			if (owt_effect->type == CS40L50_WVFRM_BANK_OWT)
173				effect->index++;
174		break;
175	case CS40L50_WVFRM_BANK_ROM:
176	case CS40L50_WVFRM_BANK_RAM:
177		effect->index += work_data->custom_data[1] & CS40L50_CUSTOM_DATA_MASK;
178		break;
179	default:
180		dev_err(vib->dev, "Bank type %d not supported\n", effect->type);
181		return -EINVAL;
182	}
183
184	if (effect->index > max_index || effect->index < base_index) {
185		dev_err(vib->dev, "Index out of bounds: %u\n", effect->index);
186		return -ENOSPC;
187	}
188
189	return 0;
190}
191
192static int cs40l50_effect_gpio_mapping_set(struct cs40l50_work *work_data,
193					   struct cs40l50_effect *effect)
194{
195	u16 gpio_edge, gpio_num, button = work_data->effect->trigger.button;
196	struct cs40l50_vibra *vib = work_data->vib;
197
198	if (button) {
199		gpio_num = FIELD_GET(CS40L50_GPIO_NUM_MASK, button);
200		gpio_edge = FIELD_GET(CS40L50_GPIO_EDGE_MASK, button);
201		effect->gpio_reg = vib->dsp.gpio_base_reg + (gpio_num * 8) - gpio_edge;
202
203		return regmap_write(vib->regmap, effect->gpio_reg, button);
204	}
205
206	effect->gpio_reg = CS40L50_GPIO_MAPPING_NONE;
207
208	return 0;
209}
210
211struct cs40l50_owt_header {
212	u32 type;
213	u32 data_words;
214	u32 offset;
215} __packed;
216
217static int cs40l50_upload_owt(struct cs40l50_work *work_data)
218{
219	u8 *new_owt_effect_data __free(kfree) = NULL;
220	struct cs40l50_vibra *vib = work_data->vib;
221	size_t len = work_data->custom_len * 2;
222	struct cs40l50_owt_header header;
223	u32 offset, size;
224	int error;
225
226	error = regmap_read(vib->regmap, vib->dsp.owt_size_reg, &size);
227	if (error)
228		return error;
229
230	if ((size * sizeof(u32)) < sizeof(header) + len) {
231		dev_err(vib->dev, "No space in open wavetable for effect\n");
232		return -ENOSPC;
233	}
234
235	header.type = work_data->custom_data[0] == CS40L50_PCM_ID ? CS40L50_TYPE_PCM :
236								    CS40L50_TYPE_PWLE;
237	header.offset = sizeof(header) / sizeof(u32);
238	header.data_words = len / sizeof(u32);
239
240	new_owt_effect_data = kmalloc(sizeof(header) + len, GFP_KERNEL);
241	if (!new_owt_effect_data)
242		return -ENOMEM;
243
244	memcpy(new_owt_effect_data, &header, sizeof(header));
245	memcpy(new_owt_effect_data + sizeof(header), work_data->custom_data, len);
246
247	error = regmap_read(vib->regmap, vib->dsp.owt_offset_reg, &offset);
248	if (error)
249		return error;
250
251	error = regmap_bulk_write(vib->regmap, vib->dsp.owt_base_reg +
252				  (offset * sizeof(u32)), new_owt_effect_data,
253				  sizeof(header) + len);
254	if (error)
255		return error;
256
257	error = vib->dsp.write(vib->dev, vib->regmap, vib->dsp.push_owt_cmd);
258	if (error)
259		return error;
260
261	return 0;
262}
263
264static void cs40l50_add_worker(struct work_struct *work)
265{
266	struct cs40l50_work *work_data = container_of(work, struct cs40l50_work, work);
267	struct cs40l50_vibra *vib = work_data->vib;
268	struct cs40l50_effect *effect;
269	bool is_new = false;
270	int error;
271
272	error = pm_runtime_resume_and_get(vib->dev);
273	if (error)
274		goto err_exit;
275
276	/* Update effect if already uploaded, otherwise create new effect */
277	effect = cs40l50_find_effect(work_data->effect->id, &vib->effect_head);
278	if (!effect) {
279		effect = kzalloc(sizeof(*effect), GFP_KERNEL);
280		if (!effect) {
281			error = -ENOMEM;
282			goto err_pm;
283		}
284
285		effect->id = work_data->effect->id;
286		is_new = true;
287	}
288
289	error = cs40l50_effect_bank_set(work_data, effect);
290	if (error)
291		goto err_free;
292
293	error = cs40l50_effect_index_set(work_data, effect);
294	if (error)
295		goto err_free;
296
297	error = cs40l50_effect_gpio_mapping_set(work_data, effect);
298	if (error)
299		goto err_free;
300
301	if (effect->type == CS40L50_WVFRM_BANK_OWT)
302		error = cs40l50_upload_owt(work_data);
303err_free:
304	if (is_new) {
305		if (error)
306			kfree(effect);
307		else
308			list_add(&effect->list, &vib->effect_head);
309	}
310err_pm:
311	pm_runtime_mark_last_busy(vib->dev);
312	pm_runtime_put_autosuspend(vib->dev);
313err_exit:
314	work_data->error = error;
315}
316
317static int cs40l50_add(struct input_dev *dev, struct ff_effect *effect,
318		       struct ff_effect *old)
319{
320	struct ff_periodic_effect *periodic = &effect->u.periodic;
321	struct cs40l50_vibra *vib = input_get_drvdata(dev);
322	struct cs40l50_work work_data;
323
324	if (effect->type != FF_PERIODIC || periodic->waveform != FF_CUSTOM) {
325		dev_err(vib->dev, "Type (%#X) or waveform (%#X) unsupported\n",
326			effect->type, periodic->waveform);
327		return -EINVAL;
328	}
329
330	work_data.custom_data = memdup_array_user(effect->u.periodic.custom_data,
331						  effect->u.periodic.custom_len,
332						  sizeof(s16));
333	if (IS_ERR(work_data.custom_data))
334		return PTR_ERR(work_data.custom_data);
335
336	work_data.custom_len = effect->u.periodic.custom_len;
337	work_data.vib = vib;
338	work_data.effect = effect;
339	INIT_WORK_ONSTACK(&work_data.work, cs40l50_add_worker);
340
341	/* Push to the workqueue to serialize with playbacks */
342	queue_work(vib->vib_wq, &work_data.work);
343	flush_work(&work_data.work);
344	destroy_work_on_stack(&work_data.work);
345
346	kfree(work_data.custom_data);
347
348	return work_data.error;
349}
350
351static void cs40l50_start_worker(struct work_struct *work)
352{
353	struct cs40l50_work *work_data = container_of(work, struct cs40l50_work, work);
354	struct cs40l50_vibra *vib = work_data->vib;
355	struct cs40l50_effect *start_effect;
356
357	if (pm_runtime_resume_and_get(vib->dev) < 0)
358		goto err_free;
359
360	start_effect = cs40l50_find_effect(work_data->effect->id, &vib->effect_head);
361	if (start_effect) {
362		while (--work_data->count >= 0) {
363			vib->dsp.write(vib->dev, vib->regmap, start_effect->index);
364			usleep_range(work_data->effect->replay.length,
365				     work_data->effect->replay.length + 100);
366		}
367	} else {
368		dev_err(vib->dev, "Effect to play not found\n");
369	}
370
371	pm_runtime_mark_last_busy(vib->dev);
372	pm_runtime_put_autosuspend(vib->dev);
373err_free:
374	kfree(work_data);
375}
376
377static void cs40l50_stop_worker(struct work_struct *work)
378{
379	struct cs40l50_work *work_data = container_of(work, struct cs40l50_work, work);
380	struct cs40l50_vibra *vib = work_data->vib;
381
382	if (pm_runtime_resume_and_get(vib->dev) < 0)
383		return;
384
385	vib->dsp.write(vib->dev, vib->regmap, vib->dsp.stop_cmd);
386
387	pm_runtime_mark_last_busy(vib->dev);
388	pm_runtime_put_autosuspend(vib->dev);
389
390	kfree(work_data);
391}
392
393static int cs40l50_playback(struct input_dev *dev, int effect_id, int val)
394{
395	struct cs40l50_vibra *vib = input_get_drvdata(dev);
396	struct cs40l50_work *work_data;
397
398	work_data = kzalloc(sizeof(*work_data), GFP_ATOMIC);
399	if (!work_data)
400		return -ENOMEM;
401
402	work_data->vib = vib;
403
404	if (val > 0) {
405		work_data->effect = &dev->ff->effects[effect_id];
406		work_data->count = val;
407		INIT_WORK(&work_data->work, cs40l50_start_worker);
408	} else {
409		/* Stop the amplifier as device drives only one effect */
410		INIT_WORK(&work_data->work, cs40l50_stop_worker);
411	}
412
413	queue_work(vib->vib_wq, &work_data->work);
414
415	return 0;
416}
417
418static void cs40l50_erase_worker(struct work_struct *work)
419{
420	struct cs40l50_work *work_data = container_of(work, struct cs40l50_work, work);
421	struct cs40l50_effect *erase_effect, *owt_effect;
422	struct cs40l50_vibra *vib = work_data->vib;
423	int error;
424
425	error = pm_runtime_resume_and_get(vib->dev);
426	if (error)
427		goto err_exit;
428
429	erase_effect = cs40l50_find_effect(work_data->effect->id, &vib->effect_head);
430	if (!erase_effect) {
431		dev_err(vib->dev, "Effect to erase not found\n");
432		error = -EINVAL;
433		goto err_pm;
434	}
435
436	if (erase_effect->gpio_reg != CS40L50_GPIO_MAPPING_NONE) {
437		error = regmap_write(vib->regmap, erase_effect->gpio_reg,
438				     CS40L50_GPIO_DISABLE);
439		if (error)
440			goto err_pm;
441	}
442
443	if (erase_effect->type == CS40L50_WVFRM_BANK_OWT) {
444		error = vib->dsp.write(vib->dev, vib->regmap,
445				       vib->dsp.delete_owt_cmd |
446				       (erase_effect->index & 0xFF));
447		if (error)
448			goto err_pm;
449
450		list_for_each_entry(owt_effect, &vib->effect_head, list)
451			if (owt_effect->type == CS40L50_WVFRM_BANK_OWT &&
452			    owt_effect->index > erase_effect->index)
453				owt_effect->index--;
454	}
455
456	list_del(&erase_effect->list);
457	kfree(erase_effect);
458err_pm:
459	pm_runtime_mark_last_busy(vib->dev);
460	pm_runtime_put_autosuspend(vib->dev);
461err_exit:
462	work_data->error = error;
463}
464
465static int cs40l50_erase(struct input_dev *dev, int effect_id)
466{
467	struct cs40l50_vibra *vib = input_get_drvdata(dev);
468	struct cs40l50_work work_data;
469
470	work_data.vib = vib;
471	work_data.effect = &dev->ff->effects[effect_id];
472
473	INIT_WORK_ONSTACK(&work_data.work, cs40l50_erase_worker);
474
475	/* Push to workqueue to serialize with playbacks */
476	queue_work(vib->vib_wq, &work_data.work);
477	flush_work(&work_data.work);
478	destroy_work_on_stack(&work_data.work);
479
480	return work_data.error;
481}
482
483static void cs40l50_remove_wq(void *data)
484{
485	destroy_workqueue(data);
486}
487
488static int cs40l50_vibra_probe(struct platform_device *pdev)
489{
490	struct cs40l50 *cs40l50 = dev_get_drvdata(pdev->dev.parent);
491	struct cs40l50_vibra *vib;
492	int error;
493
494	vib = devm_kzalloc(pdev->dev.parent, sizeof(*vib), GFP_KERNEL);
495	if (!vib)
496		return -ENOMEM;
497
498	vib->dev = cs40l50->dev;
499	vib->regmap = cs40l50->regmap;
500	vib->dsp = cs40l50_dsp;
501
502	vib->input = devm_input_allocate_device(vib->dev);
503	if (!vib->input)
504		return -ENOMEM;
505
506	vib->input->id.product = cs40l50->devid;
507	vib->input->id.version = cs40l50->revid;
508	vib->input->name = "cs40l50_vibra";
509
510	input_set_drvdata(vib->input, vib);
511	input_set_capability(vib->input, EV_FF, FF_PERIODIC);
512	input_set_capability(vib->input, EV_FF, FF_CUSTOM);
513
514	error = input_ff_create(vib->input, CS40L50_EFFECTS_MAX);
515	if (error) {
516		dev_err(vib->dev, "Failed to create input device\n");
517		return error;
518	}
519
520	vib->input->ff->upload = cs40l50_add;
521	vib->input->ff->playback = cs40l50_playback;
522	vib->input->ff->erase = cs40l50_erase;
523
524	INIT_LIST_HEAD(&vib->effect_head);
525
526	vib->vib_wq = alloc_ordered_workqueue("vib_wq", WQ_HIGHPRI);
527	if (!vib->vib_wq)
528		return -ENOMEM;
529
530	error = devm_add_action_or_reset(vib->dev, cs40l50_remove_wq, vib->vib_wq);
531	if (error)
532		return error;
533
534	error = input_register_device(vib->input);
535	if (error)
536		return error;
537
538	return 0;
539}
540
541static const struct platform_device_id cs40l50_vibra_id_match[] = {
542	{ "cs40l50-vibra", },
543	{}
544};
545MODULE_DEVICE_TABLE(platform, cs40l50_vibra_id_match);
546
547static struct platform_driver cs40l50_vibra_driver = {
548	.probe		= cs40l50_vibra_probe,
549	.id_table	= cs40l50_vibra_id_match,
550	.driver		= {
551		.name	= "cs40l50-vibra",
552	},
553};
554module_platform_driver(cs40l50_vibra_driver);
555
556MODULE_DESCRIPTION("CS40L50 Advanced Haptic Driver");
557MODULE_AUTHOR("James Ogletree, Cirrus Logic Inc. <james.ogletree@cirrus.com>");
558MODULE_LICENSE("GPL");