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