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 /*
  28  * The smbus protocol can read only 32 bytes max at a time.
  29  * But this should be fine for i2c/spi as well.
  30  */
  31 #define F54_REPORT_DATA_SIZE    32
  32
  33 /* F54 commands */
  34 #define F54_GET_REPORT          1
  35 #define F54_FORCE_CAL           2
  36
  37 /* F54 capabilities */
  38 #define F54_CAP_BASELINE        (1 << 2)
  39 #define F54_CAP_IMAGE8          (1 << 3)
  40 #define F54_CAP_IMAGE16         (1 << 6)
  41
  42 /**
  43  * enum rmi_f54_report_type - RMI4 F54 report types
  44  *
  45  * @F54_8BIT_IMAGE:     Normalized 8-Bit Image Report. The capacitance variance
  46  *                      from baseline for each pixel.
  47  *
  48  * @F54_16BIT_IMAGE:    Normalized 16-Bit Image Report. The capacitance variance
  49  *                      from baseline for each pixel.
  50  *
  51  * @F54_RAW_16BIT_IMAGE:
  52  *                      Raw 16-Bit Image Report. The raw capacitance for each
  53  *                      pixel.
  54  *
  55  * @F54_TRUE_BASELINE:  True Baseline Report. The baseline capacitance for each
  56  *                      pixel.
  57  *
  58  * @F54_FULL_RAW_CAP:   Full Raw Capacitance Report. The raw capacitance with
  59  *                      low reference set to its minimum value and high
  60  *                      reference set to its maximum value.
  61  *
  62  * @F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
  63  *                      Full Raw Capacitance with Receiver Offset Removed
  64  *                      Report. Set Low reference to its minimum value and high
  65  *                      references to its maximum value, then report the raw
  66  *                      capacitance for each pixel.
  67  */
  68 enum rmi_f54_report_type {
  69         F54_REPORT_NONE = 0,
  70         F54_8BIT_IMAGE = 1,
  71         F54_16BIT_IMAGE = 2,
  72         F54_RAW_16BIT_IMAGE = 3,
  73         F54_TRUE_BASELINE = 9,
  74         F54_FULL_RAW_CAP = 19,
  75         F54_FULL_RAW_CAP_RX_OFFSET_REMOVED = 20,
  76         F54_MAX_REPORT_TYPE,
  77 };
  78
  79 static const char * const rmi_f54_report_type_names[] = {
  80         [F54_REPORT_NONE]               = "Unknown",
  81         [F54_8BIT_IMAGE]                = "Normalized 8-Bit Image",
  82         [F54_16BIT_IMAGE]               = "Normalized 16-Bit Image",
  83         [F54_RAW_16BIT_IMAGE]           = "Raw 16-Bit Image",
  84         [F54_TRUE_BASELINE]             = "True Baseline",
  85         [F54_FULL_RAW_CAP]              = "Full Raw Capacitance",
  86         [F54_FULL_RAW_CAP_RX_OFFSET_REMOVED]
  87                                         = "Full Raw Capacitance RX Offset Removed",
  88 };
  89
  90 struct f54_data {
  91         struct rmi_function *fn;
  92
  93         u8 num_rx_electrodes;
  94         u8 num_tx_electrodes;
  95         u8 capabilities;
  96         u16 clock_rate;
  97         u8 family;
  98
  99         enum rmi_f54_report_type report_type;
 100         u8 *report_data;
 101         int report_size;
 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         u32 sequence;
 120         int input;
 121         enum rmi_f54_report_type inputs[F54_MAX_REPORT_TYPE];
 122 };
 123
 124 /*
 125  * Basic checks on report_type to ensure we write a valid type
 126  * to the sensor.
 127  */
 128 static bool is_f54_report_type_valid(struct f54_data *f54,
 129                                      enum rmi_f54_report_type reptype)
 130 {
 131         switch (reptype) {
 132         case F54_8BIT_IMAGE:
 133                 return f54->capabilities & F54_CAP_IMAGE8;
 134         case F54_16BIT_IMAGE:
 135         case F54_RAW_16BIT_IMAGE:
 136                 return f54->capabilities & F54_CAP_IMAGE16;
 137         case F54_TRUE_BASELINE:
 138                 return f54->capabilities & F54_CAP_IMAGE16;
 139         case F54_FULL_RAW_CAP:
 140         case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
 141                 return true;
 142         default:
 143                 return false;
 144         }
 145 }
 146
 147 static enum rmi_f54_report_type rmi_f54_get_reptype(struct f54_data *f54,
 148                                                 unsigned int i)
 149 {
 150         if (i >= F54_MAX_REPORT_TYPE)
 151                 return F54_REPORT_NONE;
 152
 153         return f54->inputs[i];
 154 }
 155
 156 static void rmi_f54_create_input_map(struct f54_data *f54)
 157 {
 158         int i = 0;
 159         enum rmi_f54_report_type reptype;
 160
 161         for (reptype = 1; reptype < F54_MAX_REPORT_TYPE; reptype++) {
 162                 if (!is_f54_report_type_valid(f54, reptype))
 163                         continue;
 164
 165                 f54->inputs[i++] = reptype;
 166         }
 167
 168         /* Remaining values are zero via kzalloc */
 169 }
 170
 171 static int rmi_f54_request_report(struct rmi_function *fn, u8 report_type)
 172 {
 173         struct f54_data *f54 = dev_get_drvdata(&fn->dev);
 174         struct rmi_device *rmi_dev = fn->rmi_dev;
 175         int error;
 176
 177         /* Write Report Type into F54_AD_Data0 */
 178         if (f54->report_type != report_type) {
 179                 error = rmi_write(rmi_dev, f54->fn->fd.data_base_addr,
 180                                   report_type);
 181                 if (error)
 182                         return error;
 183                 f54->report_type = report_type;
 184         }
 185
 186         /*
 187          * Small delay after disabling interrupts to avoid race condition
 188          * in firmare. This value is a bit higher than absolutely necessary.
 189          * Should be removed once issue is resolved in firmware.
 190          */
 191         usleep_range(2000, 3000);
 192
 193         mutex_lock(&f54->data_mutex);
 194
 195         error = rmi_write(rmi_dev, fn->fd.command_base_addr, F54_GET_REPORT);
 196         if (error < 0)
 197                 goto unlock;
 198
 199         init_completion(&f54->cmd_done);
 200
 201         f54->is_busy = 1;
 202         f54->timeout = jiffies + msecs_to_jiffies(100);
 203
 204         queue_delayed_work(f54->workqueue, &f54->work, 0);
 205
 206 unlock:
 207         mutex_unlock(&f54->data_mutex);
 208
 209         return error;
 210 }
 211
 212 static size_t rmi_f54_get_report_size(struct f54_data *f54)
 213 {
 214         struct rmi_device *rmi_dev = f54->fn->rmi_dev;
 215         struct rmi_driver_data *drv_data = dev_get_drvdata(&rmi_dev->dev);
 216         u8 rx = drv_data->num_rx_electrodes ? : f54->num_rx_electrodes;
 217         u8 tx = drv_data->num_tx_electrodes ? : f54->num_tx_electrodes;
 218         size_t size;
 219
 220         switch (rmi_f54_get_reptype(f54, f54->input)) {
 221         case F54_8BIT_IMAGE:
 222                 size = rx * tx;
 223                 break;
 224         case F54_16BIT_IMAGE:
 225         case F54_RAW_16BIT_IMAGE:
 226         case F54_TRUE_BASELINE:
 227         case F54_FULL_RAW_CAP:
 228         case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
 229                 size = sizeof(u16) * rx * tx;
 230                 break;
 231         default:
 232                 size = 0;
 233         }
 234
 235         return size;
 236 }
 237
 238 static int rmi_f54_get_pixel_fmt(enum rmi_f54_report_type reptype, u32 *pixfmt)
 239 {
 240         int ret = 0;
 241
 242         switch (reptype) {
 243         case F54_8BIT_IMAGE:
 244                 *pixfmt = V4L2_TCH_FMT_DELTA_TD08;
 245                 break;
 246
 247         case F54_16BIT_IMAGE:
 248                 *pixfmt = V4L2_TCH_FMT_DELTA_TD16;
 249                 break;
 250
 251         case F54_RAW_16BIT_IMAGE:
 252         case F54_TRUE_BASELINE:
 253         case F54_FULL_RAW_CAP:
 254         case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
 255                 *pixfmt = V4L2_TCH_FMT_TU16;
 256                 break;
 257
 258         case F54_REPORT_NONE:
 259         case F54_MAX_REPORT_TYPE:
 260                 ret = -EINVAL;
 261                 break;
 262         }
 263
 264         return ret;
 265 }
 266
 267 static const struct v4l2_file_operations rmi_f54_video_fops = {
 268         .owner = THIS_MODULE,
 269         .open = v4l2_fh_open,
 270         .release = vb2_fop_release,
 271         .unlocked_ioctl = video_ioctl2,
 272         .read = vb2_fop_read,
 273         .mmap = vb2_fop_mmap,
 274         .poll = vb2_fop_poll,
 275 };
 276
 277 static int rmi_f54_queue_setup(struct vb2_queue *q, unsigned int *nbuffers,
 278                                unsigned int *nplanes, unsigned int sizes[],
 279                                struct device *alloc_devs[])
 280 {
 281         struct f54_data *f54 = q->drv_priv;
 282
 283         if (*nplanes)
 284                 return sizes[0] < rmi_f54_get_report_size(f54) ? -EINVAL : 0;
 285
 286         *nplanes = 1;
 287         sizes[0] = rmi_f54_get_report_size(f54);
 288
 289         return 0;
 290 }
 291
 292 static void rmi_f54_buffer_queue(struct vb2_buffer *vb)
 293 {
 294         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 295         struct f54_data *f54 = vb2_get_drv_priv(vb->vb2_queue);
 296         u16 *ptr;
 297         enum vb2_buffer_state state;
 298         enum rmi_f54_report_type reptype;
 299         int ret;
 300
 301         mutex_lock(&f54->status_mutex);
 302
 303         vb2_set_plane_payload(vb, 0, 0);
 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         vb->timestamp = ktime_get_ns();
 351         vbuf->field = V4L2_FIELD_NONE;
 352         vbuf->sequence = f54->sequence++;
 353         vb2_buffer_done(vb, state);
 354         mutex_unlock(&f54->status_mutex);
 355 }
 356
 357 static void rmi_f54_stop_streaming(struct vb2_queue *q)
 358 {
 359         struct f54_data *f54 = vb2_get_drv_priv(q);
 360
 361         f54->sequence = 0;
 362 }
 363
 364 /* V4L2 structures */
 365 static const struct vb2_ops rmi_f54_queue_ops = {
 366         .queue_setup            = rmi_f54_queue_setup,
 367         .buf_queue              = rmi_f54_buffer_queue,
 368         .stop_streaming         = rmi_f54_stop_streaming,
 369         .wait_prepare           = vb2_ops_wait_prepare,
 370         .wait_finish            = vb2_ops_wait_finish,
 371 };
 372
 373 static const struct vb2_queue rmi_f54_queue = {
 374         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
 375         .io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ,
 376         .buf_struct_size = sizeof(struct vb2_v4l2_buffer),
 377         .ops = &rmi_f54_queue_ops,
 378         .mem_ops = &vb2_vmalloc_memops,
 379         .timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC,
 380 };
 381
 382 static int rmi_f54_vidioc_querycap(struct file *file, void *priv,
 383                                    struct v4l2_capability *cap)
 384 {
 385         struct f54_data *f54 = video_drvdata(file);
 386
 387         strlcpy(cap->driver, F54_NAME, sizeof(cap->driver));
 388         strlcpy(cap->card, SYNAPTICS_INPUT_DEVICE_NAME, sizeof(cap->card));
 389         snprintf(cap->bus_info, sizeof(cap->bus_info),
 390                 "rmi4:%s", dev_name(&f54->fn->dev));
 391
 392         return 0;
 393 }
 394
 395 static int rmi_f54_vidioc_enum_input(struct file *file, void *priv,
 396                                      struct v4l2_input *i)
 397 {
 398         struct f54_data *f54 = video_drvdata(file);
 399         enum rmi_f54_report_type reptype;
 400
 401         reptype = rmi_f54_get_reptype(f54, i->index);
 402         if (reptype == F54_REPORT_NONE)
 403                 return -EINVAL;
 404
 405         i->type = V4L2_INPUT_TYPE_TOUCH;
 406
 407         strlcpy(i->name, rmi_f54_report_type_names[reptype], sizeof(i->name));
 408         return 0;
 409 }
 410
 411 static int rmi_f54_set_input(struct f54_data *f54, unsigned int i)
 412 {
 413         struct rmi_device *rmi_dev = f54->fn->rmi_dev;
 414         struct rmi_driver_data *drv_data = dev_get_drvdata(&rmi_dev->dev);
 415         u8 rx = drv_data->num_rx_electrodes ? : f54->num_rx_electrodes;
 416         u8 tx = drv_data->num_tx_electrodes ? : f54->num_tx_electrodes;
 417         struct v4l2_pix_format *f = &f54->format;
 418         enum rmi_f54_report_type reptype;
 419         int ret;
 420
 421         reptype = rmi_f54_get_reptype(f54, i);
 422         if (reptype == F54_REPORT_NONE)
 423                 return -EINVAL;
 424
 425         ret = rmi_f54_get_pixel_fmt(reptype, &f->pixelformat);
 426         if (ret)
 427                 return ret;
 428
 429         f54->input = i;
 430
 431         f->width = rx;
 432         f->height = tx;
 433         f->field = V4L2_FIELD_NONE;
 434         f->colorspace = V4L2_COLORSPACE_RAW;
 435         f->bytesperline = f->width * sizeof(u16);
 436         f->sizeimage = f->width * f->height * sizeof(u16);
 437
 438         return 0;
 439 }
 440
 441 static int rmi_f54_vidioc_s_input(struct file *file, void *priv, unsigned int i)
 442 {
 443         return rmi_f54_set_input(video_drvdata(file), i);
 444 }
 445
 446 static int rmi_f54_vidioc_g_input(struct file *file, void *priv,
 447                                   unsigned int *i)
 448 {
 449         struct f54_data *f54 = video_drvdata(file);
 450
 451         *i = f54->input;
 452
 453         return 0;
 454 }
 455
 456 static int rmi_f54_vidioc_fmt(struct file *file, void *priv,
 457                               struct v4l2_format *f)
 458 {
 459         struct f54_data *f54 = video_drvdata(file);
 460
 461         f->fmt.pix = f54->format;
 462
 463         return 0;
 464 }
 465
 466 static int rmi_f54_vidioc_enum_fmt(struct file *file, void *priv,
 467                                    struct v4l2_fmtdesc *fmt)
 468 {
 469         struct f54_data *f54 = video_drvdata(file);
 470
 471         if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 472                 return -EINVAL;
 473
 474         if (fmt->index)
 475                 return -EINVAL;
 476
 477         fmt->pixelformat = f54->format.pixelformat;
 478
 479         return 0;
 480 }
 481
 482 static 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
 494 static 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
 518 static 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
 527 static 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         int report_size;
 533         u8 command;
 534         int error;
 535         int i;
 536
 537         report_size = rmi_f54_get_report_size(f54);
 538         if (report_size == 0) {
 539                 dev_err(&fn->dev, "Bad report size, report type=%d\n",
 540                                 f54->report_type);
 541                 error = -EINVAL;
 542                 goto error;     /* retry won't help */
 543         }
 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         for (i = 0; i < report_size; i += F54_REPORT_DATA_SIZE) {
 569                 int size = min(F54_REPORT_DATA_SIZE, report_size - i);
 570
 571                 fifo[0] = i & 0xff;
 572                 fifo[1] = i >> 8;
 573                 error = rmi_write_block(fn->rmi_dev,
 574                                         fn->fd.data_base_addr + F54_FIFO_OFFSET,
 575                                         fifo, sizeof(fifo));
 576                 if (error) {
 577                         dev_err(&fn->dev, "Failed to set fifo start offset\n");
 578                         goto abort;
 579                 }
 580
 581                 error = rmi_read_block(fn->rmi_dev, fn->fd.data_base_addr +
 582                                        F54_REPORT_DATA_OFFSET,
 583                                        f54->report_data + i, size);
 584                 if (error) {
 585                         dev_err(&fn->dev, "%s: read [%d bytes] returned %d\n",
 586                                 __func__, size, error);
 587                         goto abort;
 588                 }
 589         }
 590
 591 abort:
 592         f54->report_size = error ? 0 : report_size;
 593 error:
 594         if (error)
 595                 report_size = 0;
 596
 597         if (report_size == 0 && !error) {
 598                 queue_delayed_work(f54->workqueue, &f54->work,
 599                                    msecs_to_jiffies(1));
 600         } else {
 601                 f54->is_busy = false;
 602                 complete(&f54->cmd_done);
 603         }
 604
 605         mutex_unlock(&f54->data_mutex);
 606 }
 607
 608 static int rmi_f54_config(struct rmi_function *fn)
 609 {
 610         struct rmi_driver *drv = fn->rmi_dev->driver;
 611
 612         drv->clear_irq_bits(fn->rmi_dev, fn->irq_mask);
 613
 614         return 0;
 615 }
 616
 617 static int rmi_f54_detect(struct rmi_function *fn)
 618 {
 619         int error;
 620         struct f54_data *f54;
 621         u8 buf[6];
 622
 623         f54 = dev_get_drvdata(&fn->dev);
 624
 625         error = rmi_read_block(fn->rmi_dev, fn->fd.query_base_addr,
 626                                buf, sizeof(buf));
 627         if (error) {
 628                 dev_err(&fn->dev, "%s: Failed to query F54 properties\n",
 629                         __func__);
 630                 return error;
 631         }
 632
 633         f54->num_rx_electrodes = buf[0];
 634         f54->num_tx_electrodes = buf[1];
 635         f54->capabilities = buf[2];
 636         f54->clock_rate = buf[3] | (buf[4] << 8);
 637         f54->family = buf[5];
 638
 639         rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 num_rx_electrodes: %d\n",
 640                 f54->num_rx_electrodes);
 641         rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 num_tx_electrodes: %d\n",
 642                 f54->num_tx_electrodes);
 643         rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 capabilities: 0x%x\n",
 644                 f54->capabilities);
 645         rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 clock rate: 0x%x\n",
 646                 f54->clock_rate);
 647         rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 family: 0x%x\n",
 648                 f54->family);
 649
 650         f54->is_busy = false;
 651
 652         return 0;
 653 }
 654
 655 static int rmi_f54_probe(struct rmi_function *fn)
 656 {
 657         struct f54_data *f54;
 658         int ret;
 659         u8 rx, tx;
 660
 661         f54 = devm_kzalloc(&fn->dev, sizeof(struct f54_data), GFP_KERNEL);
 662         if (!f54)
 663                 return -ENOMEM;
 664
 665         f54->fn = fn;
 666         dev_set_drvdata(&fn->dev, f54);
 667
 668         ret = rmi_f54_detect(fn);
 669         if (ret)
 670                 return ret;
 671
 672         mutex_init(&f54->data_mutex);
 673         mutex_init(&f54->status_mutex);
 674
 675         rx = f54->num_rx_electrodes;
 676         tx = f54->num_tx_electrodes;
 677         f54->report_data = devm_kzalloc(&fn->dev,
 678                                         array3_size(tx, rx, sizeof(u16)),
 679                                         GFP_KERNEL);
 680         if (f54->report_data == NULL)
 681                 return -ENOMEM;
 682
 683         INIT_DELAYED_WORK(&f54->work, rmi_f54_work);
 684
 685         f54->workqueue = create_singlethread_workqueue("rmi4-poller");
 686         if (!f54->workqueue)
 687                 return -ENOMEM;
 688
 689         rmi_f54_create_input_map(f54);
 690         rmi_f54_set_input(f54, 0);
 691
 692         /* register video device */
 693         strlcpy(f54->v4l2.name, F54_NAME, sizeof(f54->v4l2.name));
 694         ret = v4l2_device_register(&fn->dev, &f54->v4l2);
 695         if (ret) {
 696                 dev_err(&fn->dev, "Unable to register video dev.\n");
 697                 goto remove_wq;
 698         }
 699
 700         /* initialize the queue */
 701         mutex_init(&f54->lock);
 702         f54->queue = rmi_f54_queue;
 703         f54->queue.drv_priv = f54;
 704         f54->queue.lock = &f54->lock;
 705         f54->queue.dev = &fn->dev;
 706
 707         ret = vb2_queue_init(&f54->queue);
 708         if (ret)
 709                 goto remove_v4l2;
 710
 711         f54->vdev = rmi_f54_video_device;
 712         f54->vdev.v4l2_dev = &f54->v4l2;
 713         f54->vdev.lock = &f54->lock;
 714         f54->vdev.vfl_dir = VFL_DIR_RX;
 715         f54->vdev.queue = &f54->queue;
 716         video_set_drvdata(&f54->vdev, f54);
 717
 718         ret = video_register_device(&f54->vdev, VFL_TYPE_TOUCH, -1);
 719         if (ret) {
 720                 dev_err(&fn->dev, "Unable to register video subdevice.");
 721                 goto remove_v4l2;
 722         }
 723
 724         return 0;
 725
 726 remove_v4l2:
 727         v4l2_device_unregister(&f54->v4l2);
 728 remove_wq:
 729         cancel_delayed_work_sync(&f54->work);
 730         flush_workqueue(f54->workqueue);
 731         destroy_workqueue(f54->workqueue);
 732         return ret;
 733 }
 734
 735 static void rmi_f54_remove(struct rmi_function *fn)
 736 {
 737         struct f54_data *f54 = dev_get_drvdata(&fn->dev);
 738
 739         video_unregister_device(&f54->vdev);
 740         v4l2_device_unregister(&f54->v4l2);
 741         destroy_workqueue(f54->workqueue);
 742 }
 743
 744 struct rmi_function_handler rmi_f54_handler = {
 745         .driver = {
 746                 .name = F54_NAME,
 747         },
 748         .func = 0x54,
 749         .probe = rmi_f54_probe,
 750         .config = rmi_f54_config,
 751         .remove = rmi_f54_remove,
 752 };