drivers/input/rmi4/rmi_f54.c

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