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