drivers/input/rmi4/rmi_f54.c

   1 /*
   2  * Copyright (c) 2012-2015 Synaptics Incorporated
   3  *
   4  * This program is free software; you can redistribute it and/or modify it
   5  * under the terms of the GNU General Public License version 2 as published by
   6  * the Free Software Foundation.
   7  */
   8
   9 #include <linux/kernel.h>
  10 #include <linux/rmi.h>
  11 #include <linux/input.h>
  12 #include <linux/slab.h>
  13 #include <linux/delay.h>
  14 #include <linux/i2c.h>
  15 #include <media/v4l2-device.h>
  16 #include <media/v4l2-ioctl.h>
  17 #include <media/videobuf2-v4l2.h>
  18 #include <media/videobuf2-vmalloc.h>
  19 #include "rmi_driver.h"
  20
  21 #define F54_NAME                "rmi4_f54"
  22
  23 /* F54 data offsets */
  24 #define F54_REPORT_DATA_OFFSET  3
  25 #define F54_FIFO_OFFSET         1
  26 #define F54_NUM_TX_OFFSET       1
  27 #define F54_NUM_RX_OFFSET       0
  28
  29 /* F54 commands */
  30 #define F54_GET_REPORT          1
  31 #define F54_FORCE_CAL           2
  32
  33 /* Fixed sizes of reports */
  34 #define F54_QUERY_LEN                   27
  35
  36 /* F54 capabilities */
  37 #define F54_CAP_BASELINE        (1 << 2)
  38 #define F54_CAP_IMAGE8          (1 << 3)
  39 #define F54_CAP_IMAGE16         (1 << 6)
  40
  41 /**
  42  * enum rmi_f54_report_type - RMI4 F54 report types
  43  *
  44  * @F54_8BIT_IMAGE:     Normalized 8-Bit Image Report. The capacitance variance
  45  *                      from baseline for each pixel.
  46  *
  47  * @F54_16BIT_IMAGE:    Normalized 16-Bit Image Report. The capacitance variance
  48  *                      from baseline for each pixel.
  49  *
  50  * @F54_RAW_16BIT_IMAGE:
  51  *                      Raw 16-Bit Image Report. The raw capacitance for each
  52  *                      pixel.
  53  *
  54  * @F54_TRUE_BASELINE:  True Baseline Report. The baseline capacitance for each
  55  *                      pixel.
  56  *
  57  * @F54_FULL_RAW_CAP:   Full Raw Capacitance Report. The raw capacitance with
  58  *                      low reference set to its minimum value and high
  59  *                      reference set to its maximum value.
  60  *
  61  * @F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
  62  *                      Full Raw Capacitance with Receiver Offset Removed
  63  *                      Report. Set Low reference to its minimum value and high
  64  *                      references to its maximum value, then report the raw
  65  *                      capacitance for each pixel.
  66  */
  67 enum rmi_f54_report_type {
  68         F54_REPORT_NONE = 0,
  69         F54_8BIT_IMAGE = 1,
  70         F54_16BIT_IMAGE = 2,
  71         F54_RAW_16BIT_IMAGE = 3,
  72         F54_TRUE_BASELINE = 9,
  73         F54_FULL_RAW_CAP = 19,
  74         F54_FULL_RAW_CAP_RX_OFFSET_REMOVED = 20,
  75         F54_MAX_REPORT_TYPE,
  76 };
  77
  78 const char *rmi_f54_report_type_names[] = {
  79         [F54_REPORT_NONE]               = "Unknown",
  80         [F54_8BIT_IMAGE]                = "Normalized 8-Bit Image",
  81         [F54_16BIT_IMAGE]               = "Normalized 16-Bit Image",
  82         [F54_RAW_16BIT_IMAGE]           = "Raw 16-Bit Image",
  83         [F54_TRUE_BASELINE]             = "True Baseline",
  84         [F54_FULL_RAW_CAP]              = "Full Raw Capacitance",
  85         [F54_FULL_RAW_CAP_RX_OFFSET_REMOVED]
  86                                         = "Full Raw Capacitance RX Offset Removed",
  87 };
  88
  89 struct rmi_f54_reports {
  90         int start;
  91         int size;
  92 };
  93
  94 struct f54_data {
  95         struct rmi_function *fn;
  96
  97         u8 qry[F54_QUERY_LEN];
  98         u8 num_rx_electrodes;
  99         u8 num_tx_electrodes;
 100         u8 capabilities;
 101         u16 clock_rate;
 102         u8 family;
 103
 104         enum rmi_f54_report_type report_type;
 105         u8 *report_data;
 106         int report_size;
 107         struct rmi_f54_reports standard_report[2];
 108
 109         bool is_busy;
 110         struct mutex status_mutex;
 111         struct mutex data_mutex;
 112
 113         struct workqueue_struct *workqueue;
 114         struct delayed_work work;
 115         unsigned long timeout;
 116
 117         struct completion cmd_done;
 118
 119         /* V4L2 support */
 120         struct v4l2_device v4l2;
 121         struct v4l2_pix_format format;
 122         struct video_device vdev;
 123         struct vb2_queue queue;
 124         struct mutex lock;
 125         int input;
 126         enum rmi_f54_report_type inputs[F54_MAX_REPORT_TYPE];
 127 };
 128
 129 /*
 130  * Basic checks on report_type to ensure we write a valid type
 131  * to the sensor.
 132  */
 133 static bool is_f54_report_type_valid(struct f54_data *f54,
 134                                      enum rmi_f54_report_type reptype)
 135 {
 136         switch (reptype) {
 137         case F54_8BIT_IMAGE:
 138                 return f54->capabilities & F54_CAP_IMAGE8;
 139         case F54_16BIT_IMAGE:
 140         case F54_RAW_16BIT_IMAGE:
 141                 return f54->capabilities & F54_CAP_IMAGE16;
 142         case F54_TRUE_BASELINE:
 143                 return f54->capabilities & F54_CAP_IMAGE16;
 144         case F54_FULL_RAW_CAP:
 145         case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
 146                 return true;
 147         default:
 148                 return false;
 149         }
 150 }
 151
 152 static enum rmi_f54_report_type rmi_f54_get_reptype(struct f54_data *f54,
 153                                                 unsigned int i)
 154 {
 155         if (i >= F54_MAX_REPORT_TYPE)
 156                 return F54_REPORT_NONE;
 157
 158         return f54->inputs[i];
 159 }
 160
 161 static void rmi_f54_create_input_map(struct f54_data *f54)
 162 {
 163         int i = 0;
 164         enum rmi_f54_report_type reptype;
 165
 166         for (reptype = 1; reptype < F54_MAX_REPORT_TYPE; reptype++) {
 167                 if (!is_f54_report_type_valid(f54, reptype))
 168                         continue;
 169
 170                 f54->inputs[i++] = reptype;
 171         }
 172
 173         /* Remaining values are zero via kzalloc */
 174 }
 175
 176 static int rmi_f54_request_report(struct rmi_function *fn, u8 report_type)
 177 {
 178         struct f54_data *f54 = dev_get_drvdata(&fn->dev);
 179         struct rmi_device *rmi_dev = fn->rmi_dev;
 180         int error;
 181
 182         /* Write Report Type into F54_AD_Data0 */
 183         if (f54->report_type != report_type) {
 184                 error = rmi_write(rmi_dev, f54->fn->fd.data_base_addr,
 185                                   report_type);
 186                 if (error)
 187                         return error;
 188                 f54->report_type = report_type;
 189         }
 190
 191         /*
 192          * Small delay after disabling interrupts to avoid race condition
 193          * in firmare. This value is a bit higher than absolutely necessary.
 194          * Should be removed once issue is resolved in firmware.
 195          */
 196         usleep_range(2000, 3000);
 197
 198         mutex_lock(&f54->data_mutex);
 199
 200         error = rmi_write(rmi_dev, fn->fd.command_base_addr, F54_GET_REPORT);
 201         if (error < 0)
 202                 return error;
 203
 204         init_completion(&f54->cmd_done);
 205
 206         f54->is_busy = 1;
 207         f54->timeout = jiffies + msecs_to_jiffies(100);
 208
 209         queue_delayed_work(f54->workqueue, &f54->work, 0);
 210
 211         mutex_unlock(&f54->data_mutex);
 212
 213         return 0;
 214 }
 215
 216 static size_t rmi_f54_get_report_size(struct f54_data *f54)
 217 {
 218         u8 rx = f54->num_rx_electrodes ? : f54->num_rx_electrodes;
 219         u8 tx = f54->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
 240 static 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
 269 static 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
 279 static 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
 294 static 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
 347 data_done:
 348         mutex_unlock(&f54->data_mutex);
 349 done:
 350         vb2_buffer_done(vb, state);
 351         mutex_unlock(&f54->status_mutex);
 352 }
 353
 354 /* V4L2 structures */
 355 static 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
 362 static 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
 372 static 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
 385 static 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
 401 static int rmi_f54_set_input(struct f54_data *f54, unsigned int i)
 402 {
 403         struct v4l2_pix_format *f = &f54->format;
 404         enum rmi_f54_report_type reptype;
 405         int ret;
 406
 407         reptype = rmi_f54_get_reptype(f54, i);
 408         if (reptype == F54_REPORT_NONE)
 409                 return -EINVAL;
 410
 411         ret = rmi_f54_get_pixel_fmt(reptype, &f->pixelformat);
 412         if (ret)
 413                 return ret;
 414
 415         f54->input = i;
 416
 417         f->width = f54->num_rx_electrodes;
 418         f->height = f54->num_tx_electrodes;
 419         f->field = V4L2_FIELD_NONE;
 420         f->colorspace = V4L2_COLORSPACE_RAW;
 421         f->bytesperline = f->width * sizeof(u16);
 422         f->sizeimage = f->width * f->height * sizeof(u16);
 423
 424         return 0;
 425 }
 426
 427 static int rmi_f54_vidioc_s_input(struct file *file, void *priv, unsigned int i)
 428 {
 429         return rmi_f54_set_input(video_drvdata(file), i);
 430 }
 431
 432 static int rmi_f54_vidioc_g_input(struct file *file, void *priv,
 433                                   unsigned int *i)
 434 {
 435         struct f54_data *f54 = video_drvdata(file);
 436
 437         *i = f54->input;
 438
 439         return 0;
 440 }
 441
 442 static int rmi_f54_vidioc_fmt(struct file *file, void *priv,
 443                               struct v4l2_format *f)
 444 {
 445         struct f54_data *f54 = video_drvdata(file);
 446
 447         f->fmt.pix = f54->format;
 448
 449         return 0;
 450 }
 451
 452 static int rmi_f54_vidioc_enum_fmt(struct file *file, void *priv,
 453                                    struct v4l2_fmtdesc *fmt)
 454 {
 455         if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 456                 return -EINVAL;
 457
 458         switch (fmt->index) {
 459         case 0:
 460                 fmt->pixelformat = V4L2_TCH_FMT_DELTA_TD16;
 461                 break;
 462
 463         case 1:
 464                 fmt->pixelformat = V4L2_TCH_FMT_DELTA_TD08;
 465                 break;
 466
 467         case 2:
 468                 fmt->pixelformat = V4L2_TCH_FMT_TU16;
 469                 break;
 470
 471         default:
 472                 return -EINVAL;
 473         }
 474
 475         return 0;
 476 }
 477
 478 static int rmi_f54_vidioc_g_parm(struct file *file, void *fh,
 479                                  struct v4l2_streamparm *a)
 480 {
 481         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 482                 return -EINVAL;
 483
 484         a->parm.capture.readbuffers = 1;
 485         a->parm.capture.timeperframe.numerator = 1;
 486         a->parm.capture.timeperframe.denominator = 10;
 487         return 0;
 488 }
 489
 490 static const struct v4l2_ioctl_ops rmi_f54_video_ioctl_ops = {
 491         .vidioc_querycap        = rmi_f54_vidioc_querycap,
 492
 493         .vidioc_enum_fmt_vid_cap = rmi_f54_vidioc_enum_fmt,
 494         .vidioc_s_fmt_vid_cap   = rmi_f54_vidioc_fmt,
 495         .vidioc_g_fmt_vid_cap   = rmi_f54_vidioc_fmt,
 496         .vidioc_try_fmt_vid_cap = rmi_f54_vidioc_fmt,
 497         .vidioc_g_parm          = rmi_f54_vidioc_g_parm,
 498
 499         .vidioc_enum_input      = rmi_f54_vidioc_enum_input,
 500         .vidioc_g_input         = rmi_f54_vidioc_g_input,
 501         .vidioc_s_input         = rmi_f54_vidioc_s_input,
 502
 503         .vidioc_reqbufs         = vb2_ioctl_reqbufs,
 504         .vidioc_create_bufs     = vb2_ioctl_create_bufs,
 505         .vidioc_querybuf        = vb2_ioctl_querybuf,
 506         .vidioc_qbuf            = vb2_ioctl_qbuf,
 507         .vidioc_dqbuf           = vb2_ioctl_dqbuf,
 508         .vidioc_expbuf          = vb2_ioctl_expbuf,
 509
 510         .vidioc_streamon        = vb2_ioctl_streamon,
 511         .vidioc_streamoff       = vb2_ioctl_streamoff,
 512 };
 513
 514 static const struct video_device rmi_f54_video_device = {
 515         .name = "Synaptics RMI4",
 516         .fops = &rmi_f54_video_fops,
 517         .ioctl_ops = &rmi_f54_video_ioctl_ops,
 518         .release = video_device_release_empty,
 519         .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TOUCH |
 520                        V4L2_CAP_READWRITE | V4L2_CAP_STREAMING,
 521 };
 522
 523 static void rmi_f54_work(struct work_struct *work)
 524 {
 525         struct f54_data *f54 = container_of(work, struct f54_data, work.work);
 526         struct rmi_function *fn = f54->fn;
 527         u8 fifo[2];
 528         struct rmi_f54_reports *report;
 529         int report_size;
 530         u8 command;
 531         u8 *data;
 532         int error;
 533
 534         data = f54->report_data;
 535         report_size = rmi_f54_get_report_size(f54);
 536         if (report_size == 0) {
 537                 dev_err(&fn->dev, "Bad report size, report type=%d\n",
 538                                 f54->report_type);
 539                 error = -EINVAL;
 540                 goto error;     /* retry won't help */
 541         }
 542         f54->standard_report[0].size = report_size;
 543         report = f54->standard_report;
 544
 545         mutex_lock(&f54->data_mutex);
 546
 547         /*
 548          * Need to check if command has completed.
 549          * If not try again later.
 550          */
 551         error = rmi_read(fn->rmi_dev, f54->fn->fd.command_base_addr,
 552                          &command);
 553         if (error) {
 554                 dev_err(&fn->dev, "Failed to read back command\n");
 555                 goto error;
 556         }
 557         if (command & F54_GET_REPORT) {
 558                 if (time_after(jiffies, f54->timeout)) {
 559                         dev_err(&fn->dev, "Get report command timed out\n");
 560                         error = -ETIMEDOUT;
 561                 }
 562                 report_size = 0;
 563                 goto error;
 564         }
 565
 566         rmi_dbg(RMI_DEBUG_FN, &fn->dev, "Get report command completed, reading data\n");
 567
 568         report_size = 0;
 569         for (; report->size; report++) {
 570                 fifo[0] = report->start & 0xff;
 571                 fifo[1] = (report->start >> 8) & 0xff;
 572                 error = rmi_write_block(fn->rmi_dev,
 573                                         fn->fd.data_base_addr + F54_FIFO_OFFSET,
 574                                         fifo, sizeof(fifo));
 575                 if (error) {
 576                         dev_err(&fn->dev, "Failed to set fifo start offset\n");
 577                         goto abort;
 578                 }
 579
 580                 error = rmi_read_block(fn->rmi_dev, fn->fd.data_base_addr +
 581                                        F54_REPORT_DATA_OFFSET, data,
 582                                        report->size);
 583                 if (error) {
 584                         dev_err(&fn->dev, "%s: read [%d bytes] returned %d\n",
 585                                 __func__, report->size, error);
 586                         goto abort;
 587                 }
 588                 data += report->size;
 589                 report_size += report->size;
 590         }
 591
 592 abort:
 593         f54->report_size = error ? 0 : report_size;
 594 error:
 595         if (error)
 596                 report_size = 0;
 597
 598         if (report_size == 0 && !error) {
 599                 queue_delayed_work(f54->workqueue, &f54->work,
 600                                    msecs_to_jiffies(1));
 601         } else {
 602                 f54->is_busy = false;
 603                 complete(&f54->cmd_done);
 604         }
 605
 606         mutex_unlock(&f54->data_mutex);
 607 }
 608
 609 static int rmi_f54_attention(struct rmi_function *fn, unsigned long *irqbits)
 610 {
 611         return 0;
 612 }
 613
 614 static int rmi_f54_config(struct rmi_function *fn)
 615 {
 616         struct rmi_driver *drv = fn->rmi_dev->driver;
 617
 618         drv->set_irq_bits(fn->rmi_dev, fn->irq_mask);
 619
 620         return 0;
 621 }
 622
 623 static int rmi_f54_detect(struct rmi_function *fn)
 624 {
 625         int error;
 626         struct f54_data *f54;
 627
 628         f54 = dev_get_drvdata(&fn->dev);
 629
 630         error = rmi_read_block(fn->rmi_dev, fn->fd.query_base_addr,
 631                                &f54->qry, sizeof(f54->qry));
 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 = f54->qry[0];
 639         f54->num_tx_electrodes = f54->qry[1];
 640         f54->capabilities = f54->qry[2];
 641         f54->clock_rate = f54->qry[3] | (f54->qry[4] << 8);
 642         f54->family = f54->qry[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
 660 static 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                                         sizeof(u16) * tx * rx,
 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
 730 remove_v4l2:
 731         v4l2_device_unregister(&f54->v4l2);
 732 remove_wq:
 733         cancel_delayed_work_sync(&f54->work);
 734         flush_workqueue(f54->workqueue);
 735         destroy_workqueue(f54->workqueue);
 736         return ret;
 737 }
 738
 739 static 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
 747 struct 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         .attention = rmi_f54_attention,
 755         .remove = rmi_f54_remove,
 756 };