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
242	memcpy(new_owt_effect_data, &header, sizeof(header));
243	memcpy(new_owt_effect_data + sizeof(header), work_data->custom_data, len);
244
245	error = regmap_read(vib->regmap, vib->dsp.owt_offset_reg, &offset);
246	if (error)
247		return error;
248
249	error = regmap_bulk_write(vib->regmap, vib->dsp.owt_base_reg +
250				  (offset * sizeof(u32)), new_owt_effect_data,
251				  sizeof(header) + len);
252	if (error)
253		return error;
254
255	error = vib->dsp.write(vib->dev, vib->regmap, vib->dsp.push_owt_cmd);
256	if (error)
257		return error;
258
259	return 0;
260}
261
262static void cs40l50_add_worker(struct work_struct *work)
263{
264	struct cs40l50_work *work_data = container_of(work, struct cs40l50_work, work);
265	struct cs40l50_vibra *vib = work_data->vib;
266	struct cs40l50_effect *effect;
267	bool is_new = false;
268	int error;
269
270	error = pm_runtime_resume_and_get(vib->dev);
271	if (error)
272		goto err_exit;
273
274	/* Update effect if already uploaded, otherwise create new effect */
275	effect = cs40l50_find_effect(work_data->effect->id, &vib->effect_head);
276	if (!effect) {
277		effect = kzalloc(sizeof(*effect), GFP_KERNEL);
278		if (!effect) {
279			error = -ENOMEM;
280			goto err_pm;
281		}
282
283		effect->id = work_data->effect->id;
284		is_new = true;
285	}
286
287	error = cs40l50_effect_bank_set(work_data, effect);
288	if (error)
289		goto err_free;
290
291	error = cs40l50_effect_index_set(work_data, effect);
292	if (error)
293		goto err_free;
294
295	error = cs40l50_effect_gpio_mapping_set(work_data, effect);
296	if (error)
297		goto err_free;
298
299	if (effect->type == CS40L50_WVFRM_BANK_OWT)
300		error = cs40l50_upload_owt(work_data);
301err_free:
302	if (is_new) {
303		if (error)
304			kfree(effect);
305		else
306			list_add(&effect->list, &vib->effect_head);
307	}
308err_pm:
309	pm_runtime_mark_last_busy(vib->dev);
310	pm_runtime_put_autosuspend(vib->dev);
311err_exit:
312	work_data->error = error;
313}
314
315static int cs40l50_add(struct input_dev *dev, struct ff_effect *effect,
316		       struct ff_effect *old)
317{
318	struct ff_periodic_effect *periodic = &effect->u.periodic;
319	struct cs40l50_vibra *vib = input_get_drvdata(dev);
320	struct cs40l50_work work_data;
321
322	if (effect->type != FF_PERIODIC || periodic->waveform != FF_CUSTOM) {
323		dev_err(vib->dev, "Type (%#X) or waveform (%#X) unsupported\n",
324			effect->type, periodic->waveform);
325		return -EINVAL;
326	}
327
328	work_data.custom_data = memdup_array_user(effect->u.periodic.custom_data,
329						  effect->u.periodic.custom_len,
330						  sizeof(s16));
331	if (IS_ERR(work_data.custom_data))
332		return PTR_ERR(work_data.custom_data);
333
334	work_data.custom_len = effect->u.periodic.custom_len;
335	work_data.vib = vib;
336	work_data.effect = effect;
337	INIT_WORK_ONSTACK(&work_data.work, cs40l50_add_worker);
338
339	/* Push to the workqueue to serialize with playbacks */
340	queue_work(vib->vib_wq, &work_data.work);
341	flush_work(&work_data.work);
342	destroy_work_on_stack(&work_data.work);
343
344	kfree(work_data.custom_data);
345
346	return work_data.error;
347}
348
349static void cs40l50_start_worker(struct work_struct *work)
350{
351	struct cs40l50_work *work_data = container_of(work, struct cs40l50_work, work);
352	struct cs40l50_vibra *vib = work_data->vib;
353	struct cs40l50_effect *start_effect;
354
355	if (pm_runtime_resume_and_get(vib->dev) < 0)
356		goto err_free;
357
358	start_effect = cs40l50_find_effect(work_data->effect->id, &vib->effect_head);
359	if (start_effect) {
360		while (--work_data->count >= 0) {
361			vib->dsp.write(vib->dev, vib->regmap, start_effect->index);
362			usleep_range(work_data->effect->replay.length,
363				     work_data->effect->replay.length + 100);
364		}
365	} else {
366		dev_err(vib->dev, "Effect to play not found\n");
367	}
368
369	pm_runtime_mark_last_busy(vib->dev);
370	pm_runtime_put_autosuspend(vib->dev);
371err_free:
372	kfree(work_data);
373}
374
375static void cs40l50_stop_worker(struct work_struct *work)
376{
377	struct cs40l50_work *work_data = container_of(work, struct cs40l50_work, work);
378	struct cs40l50_vibra *vib = work_data->vib;
379
380	if (pm_runtime_resume_and_get(vib->dev) < 0)
381		return;
382
383	vib->dsp.write(vib->dev, vib->regmap, vib->dsp.stop_cmd);
384
385	pm_runtime_mark_last_busy(vib->dev);
386	pm_runtime_put_autosuspend(vib->dev);
387
388	kfree(work_data);
389}
390
391static int cs40l50_playback(struct input_dev *dev, int effect_id, int val)
392{
393	struct cs40l50_vibra *vib = input_get_drvdata(dev);
394	struct cs40l50_work *work_data;
395
396	work_data = kzalloc(sizeof(*work_data), GFP_ATOMIC);
397	if (!work_data)
398		return -ENOMEM;
399
400	work_data->vib = vib;
401
402	if (val > 0) {
403		work_data->effect = &dev->ff->effects[effect_id];
404		work_data->count = val;
405		INIT_WORK(&work_data->work, cs40l50_start_worker);
406	} else {
407		/* Stop the amplifier as device drives only one effect */
408		INIT_WORK(&work_data->work, cs40l50_stop_worker);
409	}
410
411	queue_work(vib->vib_wq, &work_data->work);
412
413	return 0;
414}
415
416static void cs40l50_erase_worker(struct work_struct *work)
417{
418	struct cs40l50_work *work_data = container_of(work, struct cs40l50_work, work);
419	struct cs40l50_effect *erase_effect, *owt_effect;
420	struct cs40l50_vibra *vib = work_data->vib;
421	int error;
422
423	error = pm_runtime_resume_and_get(vib->dev);
424	if (error)
425		goto err_exit;
426
427	erase_effect = cs40l50_find_effect(work_data->effect->id, &vib->effect_head);
428	if (!erase_effect) {
429		dev_err(vib->dev, "Effect to erase not found\n");
430		error = -EINVAL;
431		goto err_pm;
432	}
433
434	if (erase_effect->gpio_reg != CS40L50_GPIO_MAPPING_NONE) {
435		error = regmap_write(vib->regmap, erase_effect->gpio_reg,
436				     CS40L50_GPIO_DISABLE);
437		if (error)
438			goto err_pm;
439	}
440
441	if (erase_effect->type == CS40L50_WVFRM_BANK_OWT) {
442		error = vib->dsp.write(vib->dev, vib->regmap,
443				       vib->dsp.delete_owt_cmd |
444				       (erase_effect->index & 0xFF));
445		if (error)
446			goto err_pm;
447
448		list_for_each_entry(owt_effect, &vib->effect_head, list)
449			if (owt_effect->type == CS40L50_WVFRM_BANK_OWT &&
450			    owt_effect->index > erase_effect->index)
451				owt_effect->index--;
452	}
453
454	list_del(&erase_effect->list);
455	kfree(erase_effect);
456err_pm:
457	pm_runtime_mark_last_busy(vib->dev);
458	pm_runtime_put_autosuspend(vib->dev);
459err_exit:
460	work_data->error = error;
461}
462
463static int cs40l50_erase(struct input_dev *dev, int effect_id)
464{
465	struct cs40l50_vibra *vib = input_get_drvdata(dev);
466	struct cs40l50_work work_data;
467
468	work_data.vib = vib;
469	work_data.effect = &dev->ff->effects[effect_id];
470
471	INIT_WORK_ONSTACK(&work_data.work, cs40l50_erase_worker);
472
473	/* Push to workqueue to serialize with playbacks */
474	queue_work(vib->vib_wq, &work_data.work);
475	flush_work(&work_data.work);
476	destroy_work_on_stack(&work_data.work);
477
478	return work_data.error;
479}
480
481static void cs40l50_remove_wq(void *data)
482{
483	flush_workqueue(data);
484	destroy_workqueue(data);
485}
486
487static int cs40l50_vibra_probe(struct platform_device *pdev)
488{
489	struct cs40l50 *cs40l50 = dev_get_drvdata(pdev->dev.parent);
490	struct cs40l50_vibra *vib;
491	int error;
492
493	vib = devm_kzalloc(pdev->dev.parent, sizeof(*vib), GFP_KERNEL);
494	if (!vib)
495		return -ENOMEM;
496
497	vib->dev = cs40l50->dev;
498	vib->regmap = cs40l50->regmap;
499	vib->dsp = cs40l50_dsp;
500
501	vib->input = devm_input_allocate_device(vib->dev);
502	if (!vib->input)
503		return -ENOMEM;
504
505	vib->input->id.product = cs40l50->devid;
506	vib->input->id.version = cs40l50->revid;
507	vib->input->name = "cs40l50_vibra";
508
509	input_set_drvdata(vib->input, vib);
510	input_set_capability(vib->input, EV_FF, FF_PERIODIC);
511	input_set_capability(vib->input, EV_FF, FF_CUSTOM);
512
513	error = input_ff_create(vib->input, CS40L50_EFFECTS_MAX);
514	if (error) {
515		dev_err(vib->dev, "Failed to create input device\n");
516		return error;
517	}
518
519	vib->input->ff->upload = cs40l50_add;
520	vib->input->ff->playback = cs40l50_playback;
521	vib->input->ff->erase = cs40l50_erase;
522
523	INIT_LIST_HEAD(&vib->effect_head);
524
525	vib->vib_wq = alloc_ordered_workqueue("vib_wq", WQ_HIGHPRI);
526	if (!vib->vib_wq)
527		return -ENOMEM;
528
529	error = devm_add_action_or_reset(vib->dev, cs40l50_remove_wq, vib->vib_wq);
530	if (error)
531		return error;
532
533	error = input_register_device(vib->input);
534	if (error)
535		return error;
536
537	return 0;
538}
539
540static const struct platform_device_id cs40l50_vibra_id_match[] = {
541	{ "cs40l50-vibra", },
542	{}
543};
544MODULE_DEVICE_TABLE(platform, cs40l50_vibra_id_match);
545
546static struct platform_driver cs40l50_vibra_driver = {
547	.probe		= cs40l50_vibra_probe,
548	.id_table	= cs40l50_vibra_id_match,
549	.driver		= {
550		.name	= "cs40l50-vibra",
551	},
552};
553module_platform_driver(cs40l50_vibra_driver);
554
555MODULE_DESCRIPTION("CS40L50 Advanced Haptic Driver");
556MODULE_AUTHOR("James Ogletree, Cirrus Logic Inc. <james.ogletree@cirrus.com>");
557MODULE_LICENSE("GPL");