drivers/input/misc/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 <[email protected]>
   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
  43 enum 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 */
  51 struct cs40l50_bank {
  52         enum cs40l50_bank_type type;
  53         u32 base_index;
  54         u32 max_index;
  55 };
  56
  57 struct 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 */
  66 struct 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 */
  79 struct 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
  88 struct 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
  98 static 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
 116 static 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
 128 static 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
 139 static 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
 157 static 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
 192 static 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
 211 struct cs40l50_owt_header {
 212         u32 type;
 213         u32 data_words;
 214         u32 offset;
 215 } __packed;
 216
 217 static 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
 262 static 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);
 301 err_free:
 302         if (is_new) {
 303                 if (error)
 304                         kfree(effect);
 305                 else
 306                         list_add(&effect->list, &vib->effect_head);
 307         }
 308 err_pm:
 309         pm_runtime_mark_last_busy(vib->dev);
 310         pm_runtime_put_autosuspend(vib->dev);
 311 err_exit:
 312         work_data->error = error;
 313 }
 314
 315 static 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
 349 static 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);
 371 err_free:
 372         kfree(work_data);
 373 }
 374
 375 static 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
 391 static 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
 416 static 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);
 456 err_pm:
 457         pm_runtime_mark_last_busy(vib->dev);
 458         pm_runtime_put_autosuspend(vib->dev);
 459 err_exit:
 460         work_data->error = error;
 461 }
 462
 463 static 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
 481 static void cs40l50_remove_wq(void *data)
 482 {
 483         flush_workqueue(data);
 484         destroy_workqueue(data);
 485 }
 486
 487 static 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
 540 static const struct platform_device_id cs40l50_vibra_id_match[] = {
 541         { "cs40l50-vibra", },
 542         {}
 543 };
 544 MODULE_DEVICE_TABLE(platform, cs40l50_vibra_id_match);
 545
 546 static 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 };
 553 module_platform_driver(cs40l50_vibra_driver);
 554
 555 MODULE_DESCRIPTION("CS40L50 Advanced Haptic Driver");
 556 MODULE_AUTHOR("James Ogletree, Cirrus Logic Inc. <[email protected]>");
 557 MODULE_LICENSE("GPL");