linux/drivers/input/rmi4/rmi_f54.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * Copyright (c) 2012-2015 Synaptics Incorporated
   4 * Copyright (C) 2016 Zodiac Inflight Innovations
   5 */
   6
   7#include <linux/kernel.h>
   8#include <linux/rmi.h>
   9#include <linux/input.h>
  10#include <linux/slab.h>
  11#include <linux/delay.h>
  12#include <linux/i2c.h>
  13#include <media/v4l2-device.h>
  14#include <media/v4l2-ioctl.h>
  15#include <media/videobuf2-v4l2.h>
  16#include <media/videobuf2-vmalloc.h>
  17#include "rmi_driver.h"
  18
  19#define F54_NAME                "rmi4_f54"
  20
  21/* F54 data offsets */
  22#define F54_REPORT_DATA_OFFSET  3
  23#define F54_FIFO_OFFSET         1
  24#define F54_NUM_TX_OFFSET       1
  25#define F54_NUM_RX_OFFSET       0
  26
  27/* F54 commands */
  28#define F54_GET_REPORT          1
  29#define F54_FORCE_CAL           2
  30
  31/* F54 capabilities */
  32#define F54_CAP_BASELINE        (1 << 2)
  33#define F54_CAP_IMAGE8          (1 << 3)
  34#define F54_CAP_IMAGE16         (1 << 6)
  35
  36/**
  37 * enum rmi_f54_report_type - RMI4 F54 report types
  38 *
  39 * @F54_8BIT_IMAGE:     Normalized 8-Bit Image Report. The capacitance variance
  40 *                      from baseline for each pixel.
  41 *
  42 * @F54_16BIT_IMAGE:    Normalized 16-Bit Image Report. The capacitance variance
  43 *                      from baseline for each pixel.
  44 *
  45 * @F54_RAW_16BIT_IMAGE:
  46 *                      Raw 16-Bit Image Report. The raw capacitance for each
  47 *                      pixel.
  48 *
  49 * @F54_TRUE_BASELINE:  True Baseline Report. The baseline capacitance for each
  50 *                      pixel.
  51 *
  52 * @F54_FULL_RAW_CAP:   Full Raw Capacitance Report. The raw capacitance with
  53 *                      low reference set to its minimum value and high
  54 *                      reference set to its maximum value.
  55 *
  56 * @F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
  57 *                      Full Raw Capacitance with Receiver Offset Removed
  58 *                      Report. Set Low reference to its minimum value and high
  59 *                      references to its maximum value, then report the raw
  60 *                      capacitance for each pixel.
  61 */
  62enum rmi_f54_report_type {
  63        F54_REPORT_NONE = 0,
  64        F54_8BIT_IMAGE = 1,
  65        F54_16BIT_IMAGE = 2,
  66        F54_RAW_16BIT_IMAGE = 3,
  67        F54_TRUE_BASELINE = 9,
  68        F54_FULL_RAW_CAP = 19,
  69        F54_FULL_RAW_CAP_RX_OFFSET_REMOVED = 20,
  70        F54_MAX_REPORT_TYPE,
  71};
  72
  73static const char * const rmi_f54_report_type_names[] = {
  74        [F54_REPORT_NONE]               = "Unknown",
  75        [F54_8BIT_IMAGE]                = "Normalized 8-Bit Image",
  76        [F54_16BIT_IMAGE]               = "Normalized 16-Bit Image",
  77        [F54_RAW_16BIT_IMAGE]           = "Raw 16-Bit Image",
  78        [F54_TRUE_BASELINE]             = "True Baseline",
  79        [F54_FULL_RAW_CAP]              = "Full Raw Capacitance",
  80        [F54_FULL_RAW_CAP_RX_OFFSET_REMOVED]
  81                                        = "Full Raw Capacitance RX Offset Removed",
  82};
  83
  84struct rmi_f54_reports {
  85        int start;
  86        int size;
  87};
  88
  89struct f54_data {
  90        struct rmi_function *fn;
  91
  92        u8 num_rx_electrodes;
  93        u8 num_tx_electrodes;
  94        u8 capabilities;
  95        u16 clock_rate;
  96        u8 family;
  97
  98        enum rmi_f54_report_type report_type;
  99        u8 *report_data;
 100        int report_size;
 101        struct rmi_f54_reports standard_report[2];
 102
 103        bool is_busy;
 104        struct mutex status_mutex;
 105        struct mutex data_mutex;
 106
 107        struct workqueue_struct *workqueue;
 108        struct delayed_work work;
 109        unsigned long timeout;
 110
 111        struct completion cmd_done;
 112
 113        /* V4L2 support */
 114        struct v4l2_device v4l2;
 115        struct v4l2_pix_format format;
 116        struct video_device vdev;
 117        struct vb2_queue queue;
 118        struct mutex lock;
 119        int input;
 120        enum rmi_f54_report_type inputs[F54_MAX_REPORT_TYPE];
 121};
 122
 123/*
 124 * Basic checks on report_type to ensure we write a valid type
 125 * to the sensor.
 126 */
 127static bool is_f54_report_type_valid(struct f54_data *f54,
 128                                     enum rmi_f54_report_type reptype)
 129{
 130        switch (reptype) {
 131        case F54_8BIT_IMAGE:
 132                return f54->capabilities & F54_CAP_IMAGE8;
 133        case F54_16BIT_IMAGE:
 134        case F54_RAW_16BIT_IMAGE:
 135                return f54->capabilities & F54_CAP_IMAGE16;
 136        case F54_TRUE_BASELINE:
 137                return f54->capabilities & F54_CAP_IMAGE16;
 138        case F54_FULL_RAW_CAP:
 139        case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
 140                return true;
 141        default:
 142                return false;
 143        }
 144}
 145
 146static enum rmi_f54_report_type rmi_f54_get_reptype(struct f54_data *f54,
 147                                                unsigned int i)
 148{
 149        if (i >= F54_MAX_REPORT_TYPE)
 150                return F54_REPORT_NONE;
 151
 152        return f54->inputs[i];
 153}
 154
 155static void rmi_f54_create_input_map(struct f54_data *f54)
 156{
 157        int i = 0;
 158        enum rmi_f54_report_type reptype;
 159
 160        for (reptype = 1; reptype < F54_MAX_REPORT_TYPE; reptype++) {
 161                if (!is_f54_report_type_valid(f54, reptype))
 162                        continue;
 163
 164                f54->inputs[i++] = reptype;
 165        }
 166
 167        /* Remaining values are zero via kzalloc */
 168}
 169
 170static int rmi_f54_request_report(struct rmi_function *fn, u8 report_type)
 171{
 172        struct f54_data *f54 = dev_get_drvdata(&fn->dev);
 173        struct rmi_device *rmi_dev = fn->rmi_dev;
 174        int error;
 175
 176        /* Write Report Type into F54_AD_Data0 */
 177        if (f54->report_type != report_type) {
 178                error = rmi_write(rmi_dev, f54->fn->fd.data_base_addr,
 179                                  report_type);
 180                if (error)
 181                        return error;
 182                f54->report_type = report_type;
 183        }
 184
 185        /*
 186         * Small delay after disabling interrupts to avoid race condition
 187         * in firmare. This value is a bit higher than absolutely necessary.
 188         * Should be removed once issue is resolved in firmware.
 189         */
 190        usleep_range(2000, 3000);
 191
 192        mutex_lock(&f54->data_mutex);
 193
 194        error = rmi_write(rmi_dev, fn->fd.command_base_addr, F54_GET_REPORT);
 195        if (error < 0)
 196                goto unlock;
 197
 198        init_completion(&f54->cmd_done);
 199
 200        f54->is_busy = 1;
 201        f54->timeout = jiffies + msecs_to_jiffies(100);
 202
 203        queue_delayed_work(f54->workqueue, &f54->work, 0);
 204
 205unlock:
 206        mutex_unlock(&f54->data_mutex);
 207
 208        return error;
 209}
 210
 211static size_t rmi_f54_get_report_size(struct f54_data *f54)
 212{
 213        struct rmi_device *rmi_dev = f54->fn->rmi_dev;
 214        struct rmi_driver_data *drv_data = dev_get_drvdata(&rmi_dev->dev);
 215        u8 rx = drv_data->num_rx_electrodes ? : f54->num_rx_electrodes;
 216        u8 tx = drv_data->num_tx_electrodes ? : f54->num_tx_electrodes;
 217        size_t size;
 218
 219        switch (rmi_f54_get_reptype(f54, f54->input)) {
 220        case F54_8BIT_IMAGE:
 221                size = rx * tx;
 222                break;
 223        case F54_16BIT_IMAGE:
 224        case F54_RAW_16BIT_IMAGE:
 225        case F54_TRUE_BASELINE:
 226        case F54_FULL_RAW_CAP:
 227        case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
 228                size = sizeof(u16) * rx * tx;
 229                break;
 230        default:
 231                size = 0;
 232        }
 233
 234        return size;
 235}
 236
 237static int rmi_f54_get_pixel_fmt(enum rmi_f54_report_type reptype, u32 *pixfmt)
 238{
 239        int ret = 0;
 240
 241        switch (reptype) {
 242        case F54_8BIT_IMAGE:
 243                *pixfmt = V4L2_TCH_FMT_DELTA_TD08;
 244                break;
 245
 246        case F54_16BIT_IMAGE:
 247                *pixfmt = V4L2_TCH_FMT_DELTA_TD16;
 248                break;
 249
 250        case F54_RAW_16BIT_IMAGE:
 251        case F54_TRUE_BASELINE:
 252        case F54_FULL_RAW_CAP:
 253        case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
 254                *pixfmt = V4L2_TCH_FMT_TU16;
 255                break;
 256
 257        case F54_REPORT_NONE:
 258        case F54_MAX_REPORT_TYPE:
 259                ret = -EINVAL;
 260                break;
 261        }
 262
 263        return ret;
 264}
 265
 266static const struct v4l2_file_operations rmi_f54_video_fops = {
 267        .owner = THIS_MODULE,
 268        .open = v4l2_fh_open,
 269        .release = vb2_fop_release,
 270        .unlocked_ioctl = video_ioctl2,
 271        .read = vb2_fop_read,
 272        .mmap = vb2_fop_mmap,
 273        .poll = vb2_fop_poll,
 274};
 275
 276static int rmi_f54_queue_setup(struct vb2_queue *q, unsigned int *nbuffers,
 277                               unsigned int *nplanes, unsigned int sizes[],
 278                               struct device *alloc_devs[])
 279{
 280        struct f54_data *f54 = q->drv_priv;
 281
 282        if (*nplanes)
 283                return sizes[0] < rmi_f54_get_report_size(f54) ? -EINVAL : 0;
 284
 285        *nplanes = 1;
 286        sizes[0] = rmi_f54_get_report_size(f54);
 287
 288        return 0;
 289}
 290
 291static void rmi_f54_buffer_queue(struct vb2_buffer *vb)
 292{
 293        struct f54_data *f54 = vb2_get_drv_priv(vb->vb2_queue);
 294        u16 *ptr;
 295        enum vb2_buffer_state state;
 296        enum rmi_f54_report_type reptype;
 297        int ret;
 298
 299        mutex_lock(&f54->status_mutex);
 300
 301        reptype = rmi_f54_get_reptype(f54, f54->input);
 302        if (reptype == F54_REPORT_NONE) {
 303                state = VB2_BUF_STATE_ERROR;
 304                goto done;
 305        }
 306
 307        if (f54->is_busy) {
 308                state = VB2_BUF_STATE_ERROR;
 309                goto done;
 310        }
 311
 312        ret = rmi_f54_request_report(f54->fn, reptype);
 313        if (ret) {
 314                dev_err(&f54->fn->dev, "Error requesting F54 report\n");
 315                state = VB2_BUF_STATE_ERROR;
 316                goto done;
 317        }
 318
 319        /* get frame data */
 320        mutex_lock(&f54->data_mutex);
 321
 322        while (f54->is_busy) {
 323                mutex_unlock(&f54->data_mutex);
 324                if (!wait_for_completion_timeout(&f54->cmd_done,
 325                                                 msecs_to_jiffies(1000))) {
 326                        dev_err(&f54->fn->dev, "Timed out\n");
 327                        state = VB2_BUF_STATE_ERROR;
 328                        goto done;
 329                }
 330                mutex_lock(&f54->data_mutex);
 331        }
 332
 333        ptr = vb2_plane_vaddr(vb, 0);
 334        if (!ptr) {
 335                dev_err(&f54->fn->dev, "Error acquiring frame ptr\n");
 336                state = VB2_BUF_STATE_ERROR;
 337                goto data_done;
 338        }
 339
 340        memcpy(ptr, f54->report_data, f54->report_size);
 341        vb2_set_plane_payload(vb, 0, rmi_f54_get_report_size(f54));
 342        state = VB2_BUF_STATE_DONE;
 343
 344data_done:
 345        mutex_unlock(&f54->data_mutex);
 346done:
 347        vb2_buffer_done(vb, state);
 348        mutex_unlock(&f54->status_mutex);
 349}
 350
 351/* V4L2 structures */
 352static const struct vb2_ops rmi_f54_queue_ops = {
 353        .queue_setup            = rmi_f54_queue_setup,
 354        .buf_queue              = rmi_f54_buffer_queue,
 355        .wait_prepare           = vb2_ops_wait_prepare,
 356        .wait_finish            = vb2_ops_wait_finish,
 357};
 358
 359static const struct vb2_queue rmi_f54_queue = {
 360        .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
 361        .io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ,
 362        .buf_struct_size = sizeof(struct vb2_buffer),
 363        .ops = &rmi_f54_queue_ops,
 364        .mem_ops = &vb2_vmalloc_memops,
 365        .timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC,
 366        .min_buffers_needed = 1,
 367};
 368
 369static int rmi_f54_vidioc_querycap(struct file *file, void *priv,
 370                                   struct v4l2_capability *cap)
 371{
 372        struct f54_data *f54 = video_drvdata(file);
 373
 374        strlcpy(cap->driver, F54_NAME, sizeof(cap->driver));
 375        strlcpy(cap->card, SYNAPTICS_INPUT_DEVICE_NAME, sizeof(cap->card));
 376        snprintf(cap->bus_info, sizeof(cap->bus_info),
 377                "rmi4:%s", dev_name(&f54->fn->dev));
 378
 379        return 0;
 380}
 381
 382static int rmi_f54_vidioc_enum_input(struct file *file, void *priv,
 383                                     struct v4l2_input *i)
 384{
 385        struct f54_data *f54 = video_drvdata(file);
 386        enum rmi_f54_report_type reptype;
 387
 388        reptype = rmi_f54_get_reptype(f54, i->index);
 389        if (reptype == F54_REPORT_NONE)
 390                return -EINVAL;
 391
 392        i->type = V4L2_INPUT_TYPE_TOUCH;
 393
 394        strlcpy(i->name, rmi_f54_report_type_names[reptype], sizeof(i->name));
 395        return 0;
 396}
 397
 398static int rmi_f54_set_input(struct f54_data *f54, unsigned int i)
 399{
 400        struct rmi_device *rmi_dev = f54->fn->rmi_dev;
 401        struct rmi_driver_data *drv_data = dev_get_drvdata(&rmi_dev->dev);
 402        u8 rx = drv_data->num_rx_electrodes ? : f54->num_rx_electrodes;
 403        u8 tx = drv_data->num_tx_electrodes ? : f54->num_tx_electrodes;
 404        struct v4l2_pix_format *f = &f54->format;
 405        enum rmi_f54_report_type reptype;
 406        int ret;
 407
 408        reptype = rmi_f54_get_reptype(f54, i);
 409        if (reptype == F54_REPORT_NONE)
 410                return -EINVAL;
 411
 412        ret = rmi_f54_get_pixel_fmt(reptype, &f->pixelformat);
 413        if (ret)
 414                return ret;
 415
 416        f54->input = i;
 417
 418        f->width = rx;
 419        f->height = tx;
 420        f->field = V4L2_FIELD_NONE;
 421        f->colorspace = V4L2_COLORSPACE_RAW;
 422        f->bytesperline = f->width * sizeof(u16);
 423        f->sizeimage = f->width * f->height * sizeof(u16);
 424
 425        return 0;
 426}
 427
 428static int rmi_f54_vidioc_s_input(struct file *file, void *priv, unsigned int i)
 429{
 430        return rmi_f54_set_input(video_drvdata(file), i);
 431}
 432
 433static int rmi_f54_vidioc_g_input(struct file *file, void *priv,
 434                                  unsigned int *i)
 435{
 436        struct f54_data *f54 = video_drvdata(file);
 437
 438        *i = f54->input;
 439
 440        return 0;
 441}
 442
 443static int rmi_f54_vidioc_fmt(struct file *file, void *priv,
 444                              struct v4l2_format *f)
 445{
 446        struct f54_data *f54 = video_drvdata(file);
 447
 448        f->fmt.pix = f54->format;
 449
 450        return 0;
 451}
 452
 453static int rmi_f54_vidioc_enum_fmt(struct file *file, void *priv,
 454                                   struct v4l2_fmtdesc *fmt)
 455{
 456        struct f54_data *f54 = video_drvdata(file);
 457
 458        if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 459                return -EINVAL;
 460
 461        if (fmt->index)
 462                return -EINVAL;
 463
 464        fmt->pixelformat = f54->format.pixelformat;
 465
 466        return 0;
 467}
 468
 469static int rmi_f54_vidioc_g_parm(struct file *file, void *fh,
 470                                 struct v4l2_streamparm *a)
 471{
 472        if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 473                return -EINVAL;
 474
 475        a->parm.capture.readbuffers = 1;
 476        a->parm.capture.timeperframe.numerator = 1;
 477        a->parm.capture.timeperframe.denominator = 10;
 478        return 0;
 479}
 480
 481static const struct v4l2_ioctl_ops rmi_f54_video_ioctl_ops = {
 482        .vidioc_querycap        = rmi_f54_vidioc_querycap,
 483
 484        .vidioc_enum_fmt_vid_cap = rmi_f54_vidioc_enum_fmt,
 485        .vidioc_s_fmt_vid_cap   = rmi_f54_vidioc_fmt,
 486        .vidioc_g_fmt_vid_cap   = rmi_f54_vidioc_fmt,
 487        .vidioc_try_fmt_vid_cap = rmi_f54_vidioc_fmt,
 488        .vidioc_g_parm          = rmi_f54_vidioc_g_parm,
 489
 490        .vidioc_enum_input      = rmi_f54_vidioc_enum_input,
 491        .vidioc_g_input         = rmi_f54_vidioc_g_input,
 492        .vidioc_s_input         = rmi_f54_vidioc_s_input,
 493
 494        .vidioc_reqbufs         = vb2_ioctl_reqbufs,
 495        .vidioc_create_bufs     = vb2_ioctl_create_bufs,
 496        .vidioc_querybuf        = vb2_ioctl_querybuf,
 497        .vidioc_qbuf            = vb2_ioctl_qbuf,
 498        .vidioc_dqbuf           = vb2_ioctl_dqbuf,
 499        .vidioc_expbuf          = vb2_ioctl_expbuf,
 500
 501        .vidioc_streamon        = vb2_ioctl_streamon,
 502        .vidioc_streamoff       = vb2_ioctl_streamoff,
 503};
 504
 505static const struct video_device rmi_f54_video_device = {
 506        .name = "Synaptics RMI4",
 507        .fops = &rmi_f54_video_fops,
 508        .ioctl_ops = &rmi_f54_video_ioctl_ops,
 509        .release = video_device_release_empty,
 510        .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TOUCH |
 511                       V4L2_CAP_READWRITE | V4L2_CAP_STREAMING,
 512};
 513
 514static void rmi_f54_work(struct work_struct *work)
 515{
 516        struct f54_data *f54 = container_of(work, struct f54_data, work.work);
 517        struct rmi_function *fn = f54->fn;
 518        u8 fifo[2];
 519        struct rmi_f54_reports *report;
 520        int report_size;
 521        u8 command;
 522        u8 *data;
 523        int error;
 524
 525        data = f54->report_data;
 526        report_size = rmi_f54_get_report_size(f54);
 527        if (report_size == 0) {
 528                dev_err(&fn->dev, "Bad report size, report type=%d\n",
 529                                f54->report_type);
 530                error = -EINVAL;
 531                goto error;     /* retry won't help */
 532        }
 533        f54->standard_report[0].size = report_size;
 534        report = f54->standard_report;
 535
 536        mutex_lock(&f54->data_mutex);
 537
 538        /*
 539         * Need to check if command has completed.
 540         * If not try again later.
 541         */
 542        error = rmi_read(fn->rmi_dev, f54->fn->fd.command_base_addr,
 543                         &command);
 544        if (error) {
 545                dev_err(&fn->dev, "Failed to read back command\n");
 546                goto error;
 547        }
 548        if (command & F54_GET_REPORT) {
 549                if (time_after(jiffies, f54->timeout)) {
 550                        dev_err(&fn->dev, "Get report command timed out\n");
 551                        error = -ETIMEDOUT;
 552                }
 553                report_size = 0;
 554                goto error;
 555        }
 556
 557        rmi_dbg(RMI_DEBUG_FN, &fn->dev, "Get report command completed, reading data\n");
 558
 559        report_size = 0;
 560        for (; report->size; report++) {
 561                fifo[0] = report->start & 0xff;
 562                fifo[1] = (report->start >> 8) & 0xff;
 563                error = rmi_write_block(fn->rmi_dev,
 564                                        fn->fd.data_base_addr + F54_FIFO_OFFSET,
 565                                        fifo, sizeof(fifo));
 566                if (error) {
 567                        dev_err(&fn->dev, "Failed to set fifo start offset\n");
 568                        goto abort;
 569                }
 570
 571                error = rmi_read_block(fn->rmi_dev, fn->fd.data_base_addr +
 572                                       F54_REPORT_DATA_OFFSET, data,
 573                                       report->size);
 574                if (error) {
 575                        dev_err(&fn->dev, "%s: read [%d bytes] returned %d\n",
 576                                __func__, report->size, error);
 577                        goto abort;
 578                }
 579                data += report->size;
 580                report_size += report->size;
 581        }
 582
 583abort:
 584        f54->report_size = error ? 0 : report_size;
 585error:
 586        if (error)
 587                report_size = 0;
 588
 589        if (report_size == 0 && !error) {
 590                queue_delayed_work(f54->workqueue, &f54->work,
 591                                   msecs_to_jiffies(1));
 592        } else {
 593                f54->is_busy = false;
 594                complete(&f54->cmd_done);
 595        }
 596
 597        mutex_unlock(&f54->data_mutex);
 598}
 599
 600static int rmi_f54_config(struct rmi_function *fn)
 601{
 602        struct rmi_driver *drv = fn->rmi_dev->driver;
 603
 604        drv->set_irq_bits(fn->rmi_dev, fn->irq_mask);
 605
 606        return 0;
 607}
 608
 609static int rmi_f54_detect(struct rmi_function *fn)
 610{
 611        int error;
 612        struct f54_data *f54;
 613        u8 buf[6];
 614
 615        f54 = dev_get_drvdata(&fn->dev);
 616
 617        error = rmi_read_block(fn->rmi_dev, fn->fd.query_base_addr,
 618                               buf, sizeof(buf));
 619        if (error) {
 620                dev_err(&fn->dev, "%s: Failed to query F54 properties\n",
 621                        __func__);
 622                return error;
 623        }
 624
 625        f54->num_rx_electrodes = buf[0];
 626        f54->num_tx_electrodes = buf[1];
 627        f54->capabilities = buf[2];
 628        f54->clock_rate = buf[3] | (buf[4] << 8);
 629        f54->family = buf[5];
 630
 631        rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 num_rx_electrodes: %d\n",
 632                f54->num_rx_electrodes);
 633        rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 num_tx_electrodes: %d\n",
 634                f54->num_tx_electrodes);
 635        rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 capabilities: 0x%x\n",
 636                f54->capabilities);
 637        rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 clock rate: 0x%x\n",
 638                f54->clock_rate);
 639        rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 family: 0x%x\n",
 640                f54->family);
 641
 642        f54->is_busy = false;
 643
 644        return 0;
 645}
 646
 647static int rmi_f54_probe(struct rmi_function *fn)
 648{
 649        struct f54_data *f54;
 650        int ret;
 651        u8 rx, tx;
 652
 653        f54 = devm_kzalloc(&fn->dev, sizeof(struct f54_data), GFP_KERNEL);
 654        if (!f54)
 655                return -ENOMEM;
 656
 657        f54->fn = fn;
 658        dev_set_drvdata(&fn->dev, f54);
 659
 660        ret = rmi_f54_detect(fn);
 661        if (ret)
 662                return ret;
 663
 664        mutex_init(&f54->data_mutex);
 665        mutex_init(&f54->status_mutex);
 666
 667        rx = f54->num_rx_electrodes;
 668        tx = f54->num_tx_electrodes;
 669        f54->report_data = devm_kzalloc(&fn->dev,
 670                                        array3_size(tx, rx, sizeof(u16)),
 671                                        GFP_KERNEL);
 672        if (f54->report_data == NULL)
 673                return -ENOMEM;
 674
 675        INIT_DELAYED_WORK(&f54->work, rmi_f54_work);
 676
 677        f54->workqueue = create_singlethread_workqueue("rmi4-poller");
 678        if (!f54->workqueue)
 679                return -ENOMEM;
 680
 681        rmi_f54_create_input_map(f54);
 682        rmi_f54_set_input(f54, 0);
 683
 684        /* register video device */
 685        strlcpy(f54->v4l2.name, F54_NAME, sizeof(f54->v4l2.name));
 686        ret = v4l2_device_register(&fn->dev, &f54->v4l2);
 687        if (ret) {
 688                dev_err(&fn->dev, "Unable to register video dev.\n");
 689                goto remove_wq;
 690        }
 691
 692        /* initialize the queue */
 693        mutex_init(&f54->lock);
 694        f54->queue = rmi_f54_queue;
 695        f54->queue.drv_priv = f54;
 696        f54->queue.lock = &f54->lock;
 697        f54->queue.dev = &fn->dev;
 698
 699        ret = vb2_queue_init(&f54->queue);
 700        if (ret)
 701                goto remove_v4l2;
 702
 703        f54->vdev = rmi_f54_video_device;
 704        f54->vdev.v4l2_dev = &f54->v4l2;
 705        f54->vdev.lock = &f54->lock;
 706        f54->vdev.vfl_dir = VFL_DIR_RX;
 707        f54->vdev.queue = &f54->queue;
 708        video_set_drvdata(&f54->vdev, f54);
 709
 710        ret = video_register_device(&f54->vdev, VFL_TYPE_TOUCH, -1);
 711        if (ret) {
 712                dev_err(&fn->dev, "Unable to register video subdevice.");
 713                goto remove_v4l2;
 714        }
 715
 716        return 0;
 717
 718remove_v4l2:
 719        v4l2_device_unregister(&f54->v4l2);
 720remove_wq:
 721        cancel_delayed_work_sync(&f54->work);
 722        flush_workqueue(f54->workqueue);
 723        destroy_workqueue(f54->workqueue);
 724        return ret;
 725}
 726
 727static void rmi_f54_remove(struct rmi_function *fn)
 728{
 729        struct f54_data *f54 = dev_get_drvdata(&fn->dev);
 730
 731        video_unregister_device(&f54->vdev);
 732        v4l2_device_unregister(&f54->v4l2);
 733}
 734
 735struct rmi_function_handler rmi_f54_handler = {
 736        .driver = {
 737                .name = F54_NAME,
 738        },
 739        .func = 0x54,
 740        .probe = rmi_f54_probe,
 741        .config = rmi_f54_config,
 742        .remove = rmi_f54_remove,
 743};
 744