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 }; 753