drivers/media/platform/qcom/camss/camss-vfe.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * camss-vfe.c
   4  *
   5  * Qualcomm MSM Camera Subsystem - VFE (Video Front End) Module
   6  *
   7  * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
   8  * Copyright (C) 2015-2018 Linaro Ltd.
   9  */
  10 #include <linux/clk.h>
  11 #include <linux/completion.h>
  12 #include <linux/interrupt.h>
  13 #include <linux/iommu.h>
  14 #include <linux/mutex.h>
  15 #include <linux/of.h>
  16 #include <linux/platform_device.h>
  17 #include <linux/pm_runtime.h>
  18 #include <linux/spinlock_types.h>
  19 #include <linux/spinlock.h>
  20 #include <media/media-entity.h>
  21 #include <media/v4l2-device.h>
  22 #include <media/v4l2-subdev.h>
  23
  24 #include "camss-vfe.h"
  25 #include "camss.h"
  26
  27 #define MSM_VFE_NAME "msm_vfe"
  28
  29 #define vfe_line_array(ptr_line)        \
  30         ((const struct vfe_line (*)[]) &(ptr_line[-(ptr_line->id)]))
  31
  32 #define to_vfe(ptr_line)        \
  33         container_of(vfe_line_array(ptr_line), struct vfe_device, line)
  34
  35 /* VFE reset timeout */
  36 #define VFE_RESET_TIMEOUT_MS 50
  37 /* VFE halt timeout */
  38 #define VFE_HALT_TIMEOUT_MS 100
  39 /* Max number of frame drop updates per frame */
  40 #define VFE_FRAME_DROP_UPDATES 5
  41 /* Frame drop value. NOTE: VAL + UPDATES should not exceed 31 */
  42 #define VFE_FRAME_DROP_VAL 20
  43
  44 #define VFE_NEXT_SOF_MS 500
  45
  46 #define SCALER_RATIO_MAX 16
  47
  48 struct vfe_format {
  49         u32 code;
  50         u8 bpp;
  51 };
  52
  53 static const struct vfe_format formats_rdi_8x16[] = {
  54         { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
  55         { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
  56         { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
  57         { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
  58         { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
  59         { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
  60         { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
  61         { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
  62         { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
  63         { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
  64         { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
  65         { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
  66         { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
  67         { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
  68         { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
  69         { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
  70         { MEDIA_BUS_FMT_Y10_1X10, 10 },
  71 };
  72
  73 static const struct vfe_format formats_pix_8x16[] = {
  74         { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
  75         { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
  76         { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
  77         { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
  78 };
  79
  80 static const struct vfe_format formats_rdi_8x96[] = {
  81         { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
  82         { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
  83         { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
  84         { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
  85         { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
  86         { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
  87         { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
  88         { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
  89         { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
  90         { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
  91         { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
  92         { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
  93         { MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 16 },
  94         { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
  95         { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
  96         { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
  97         { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
  98         { MEDIA_BUS_FMT_SBGGR14_1X14, 14 },
  99         { MEDIA_BUS_FMT_SGBRG14_1X14, 14 },
 100         { MEDIA_BUS_FMT_SGRBG14_1X14, 14 },
 101         { MEDIA_BUS_FMT_SRGGB14_1X14, 14 },
 102         { MEDIA_BUS_FMT_Y10_1X10, 10 },
 103         { MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 16 },
 104 };
 105
 106 static const struct vfe_format formats_pix_8x96[] = {
 107         { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
 108         { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
 109         { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
 110         { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
 111 };
 112
 113 /*
 114  * vfe_get_bpp - map media bus format to bits per pixel
 115  * @formats: supported media bus formats array
 116  * @nformats: size of @formats array
 117  * @code: media bus format code
 118  *
 119  * Return number of bits per pixel
 120  */
 121 static u8 vfe_get_bpp(const struct vfe_format *formats,
 122                       unsigned int nformats, u32 code)
 123 {
 124         unsigned int i;
 125
 126         for (i = 0; i < nformats; i++)
 127                 if (code == formats[i].code)
 128                         return formats[i].bpp;
 129
 130         WARN(1, "Unknown format\n");
 131
 132         return formats[0].bpp;
 133 }
 134
 135 static u32 vfe_find_code(u32 *code, unsigned int n_code,
 136                          unsigned int index, u32 req_code)
 137 {
 138         int i;
 139
 140         if (!req_code && (index >= n_code))
 141                 return 0;
 142
 143         for (i = 0; i < n_code; i++)
 144                 if (req_code) {
 145                         if (req_code == code[i])
 146                                 return req_code;
 147                 } else {
 148                         if (i == index)
 149                                 return code[i];
 150                 }
 151
 152         return code[0];
 153 }
 154
 155 static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 156                             unsigned int index, u32 src_req_code)
 157 {
 158         struct vfe_device *vfe = to_vfe(line);
 159
 160         if (vfe->camss->version == CAMSS_8x16)
 161                 switch (sink_code) {
 162                 case MEDIA_BUS_FMT_YUYV8_2X8:
 163                 {
 164                         u32 src_code[] = {
 165                                 MEDIA_BUS_FMT_YUYV8_2X8,
 166                                 MEDIA_BUS_FMT_YUYV8_1_5X8,
 167                         };
 168
 169                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 170                                              index, src_req_code);
 171                 }
 172                 case MEDIA_BUS_FMT_YVYU8_2X8:
 173                 {
 174                         u32 src_code[] = {
 175                                 MEDIA_BUS_FMT_YVYU8_2X8,
 176                                 MEDIA_BUS_FMT_YVYU8_1_5X8,
 177                         };
 178
 179                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 180                                              index, src_req_code);
 181                 }
 182                 case MEDIA_BUS_FMT_UYVY8_2X8:
 183                 {
 184                         u32 src_code[] = {
 185                                 MEDIA_BUS_FMT_UYVY8_2X8,
 186                                 MEDIA_BUS_FMT_UYVY8_1_5X8,
 187                         };
 188
 189                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 190                                              index, src_req_code);
 191                 }
 192                 case MEDIA_BUS_FMT_VYUY8_2X8:
 193                 {
 194                         u32 src_code[] = {
 195                                 MEDIA_BUS_FMT_VYUY8_2X8,
 196                                 MEDIA_BUS_FMT_VYUY8_1_5X8,
 197                         };
 198
 199                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 200                                              index, src_req_code);
 201                 }
 202                 default:
 203                         if (index > 0)
 204                                 return 0;
 205
 206                         return sink_code;
 207                 }
 208         else if (vfe->camss->version == CAMSS_8x96)
 209                 switch (sink_code) {
 210                 case MEDIA_BUS_FMT_YUYV8_2X8:
 211                 {
 212                         u32 src_code[] = {
 213                                 MEDIA_BUS_FMT_YUYV8_2X8,
 214                                 MEDIA_BUS_FMT_YVYU8_2X8,
 215                                 MEDIA_BUS_FMT_UYVY8_2X8,
 216                                 MEDIA_BUS_FMT_VYUY8_2X8,
 217                                 MEDIA_BUS_FMT_YUYV8_1_5X8,
 218                         };
 219
 220                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 221                                              index, src_req_code);
 222                 }
 223                 case MEDIA_BUS_FMT_YVYU8_2X8:
 224                 {
 225                         u32 src_code[] = {
 226                                 MEDIA_BUS_FMT_YVYU8_2X8,
 227                                 MEDIA_BUS_FMT_YUYV8_2X8,
 228                                 MEDIA_BUS_FMT_UYVY8_2X8,
 229                                 MEDIA_BUS_FMT_VYUY8_2X8,
 230                                 MEDIA_BUS_FMT_YVYU8_1_5X8,
 231                         };
 232
 233                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 234                                              index, src_req_code);
 235                 }
 236                 case MEDIA_BUS_FMT_UYVY8_2X8:
 237                 {
 238                         u32 src_code[] = {
 239                                 MEDIA_BUS_FMT_UYVY8_2X8,
 240                                 MEDIA_BUS_FMT_YUYV8_2X8,
 241                                 MEDIA_BUS_FMT_YVYU8_2X8,
 242                                 MEDIA_BUS_FMT_VYUY8_2X8,
 243                                 MEDIA_BUS_FMT_UYVY8_1_5X8,
 244                         };
 245
 246                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 247                                              index, src_req_code);
 248                 }
 249                 case MEDIA_BUS_FMT_VYUY8_2X8:
 250                 {
 251                         u32 src_code[] = {
 252                                 MEDIA_BUS_FMT_VYUY8_2X8,
 253                                 MEDIA_BUS_FMT_YUYV8_2X8,
 254                                 MEDIA_BUS_FMT_YVYU8_2X8,
 255                                 MEDIA_BUS_FMT_UYVY8_2X8,
 256                                 MEDIA_BUS_FMT_VYUY8_1_5X8,
 257                         };
 258
 259                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 260                                              index, src_req_code);
 261                 }
 262                 default:
 263                         if (index > 0)
 264                                 return 0;
 265
 266                         return sink_code;
 267                 }
 268         else
 269                 return 0;
 270 }
 271
 272 /*
 273  * vfe_reset - Trigger reset on VFE module and wait to complete
 274  * @vfe: VFE device
 275  *
 276  * Return 0 on success or a negative error code otherwise
 277  */
 278 static int vfe_reset(struct vfe_device *vfe)
 279 {
 280         unsigned long time;
 281
 282         reinit_completion(&vfe->reset_complete);
 283
 284         vfe->ops->global_reset(vfe);
 285
 286         time = wait_for_completion_timeout(&vfe->reset_complete,
 287                 msecs_to_jiffies(VFE_RESET_TIMEOUT_MS));
 288         if (!time) {
 289                 dev_err(vfe->camss->dev, "VFE reset timeout\n");
 290                 return -EIO;
 291         }
 292
 293         return 0;
 294 }
 295
 296 /*
 297  * vfe_halt - Trigger halt on VFE module and wait to complete
 298  * @vfe: VFE device
 299  *
 300  * Return 0 on success or a negative error code otherwise
 301  */
 302 static int vfe_halt(struct vfe_device *vfe)
 303 {
 304         unsigned long time;
 305
 306         reinit_completion(&vfe->halt_complete);
 307
 308         vfe->ops->halt_request(vfe);
 309
 310         time = wait_for_completion_timeout(&vfe->halt_complete,
 311                 msecs_to_jiffies(VFE_HALT_TIMEOUT_MS));
 312         if (!time) {
 313                 dev_err(vfe->camss->dev, "VFE halt timeout\n");
 314                 return -EIO;
 315         }
 316
 317         return 0;
 318 }
 319
 320 static void vfe_init_outputs(struct vfe_device *vfe)
 321 {
 322         int i;
 323
 324         for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
 325                 struct vfe_output *output = &vfe->line[i].output;
 326
 327                 output->state = VFE_OUTPUT_OFF;
 328                 output->buf[0] = NULL;
 329                 output->buf[1] = NULL;
 330                 INIT_LIST_HEAD(&output->pending_bufs);
 331         }
 332 }
 333
 334 static void vfe_reset_output_maps(struct vfe_device *vfe)
 335 {
 336         int i;
 337
 338         for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++)
 339                 vfe->wm_output_map[i] = VFE_LINE_NONE;
 340 }
 341
 342 static void vfe_output_init_addrs(struct vfe_device *vfe,
 343                                   struct vfe_output *output, u8 sync)
 344 {
 345         u32 ping_addr;
 346         u32 pong_addr;
 347         unsigned int i;
 348
 349         output->active_buf = 0;
 350
 351         for (i = 0; i < output->wm_num; i++) {
 352                 if (output->buf[0])
 353                         ping_addr = output->buf[0]->addr[i];
 354                 else
 355                         ping_addr = 0;
 356
 357                 if (output->buf[1])
 358                         pong_addr = output->buf[1]->addr[i];
 359                 else
 360                         pong_addr = ping_addr;
 361
 362                 vfe->ops->wm_set_ping_addr(vfe, output->wm_idx[i], ping_addr);
 363                 vfe->ops->wm_set_pong_addr(vfe, output->wm_idx[i], pong_addr);
 364                 if (sync)
 365                         vfe->ops->bus_reload_wm(vfe, output->wm_idx[i]);
 366         }
 367 }
 368
 369 static void vfe_output_update_ping_addr(struct vfe_device *vfe,
 370                                         struct vfe_output *output, u8 sync)
 371 {
 372         u32 addr;
 373         unsigned int i;
 374
 375         for (i = 0; i < output->wm_num; i++) {
 376                 if (output->buf[0])
 377                         addr = output->buf[0]->addr[i];
 378                 else
 379                         addr = 0;
 380
 381                 vfe->ops->wm_set_ping_addr(vfe, output->wm_idx[i], addr);
 382                 if (sync)
 383                         vfe->ops->bus_reload_wm(vfe, output->wm_idx[i]);
 384         }
 385 }
 386
 387 static void vfe_output_update_pong_addr(struct vfe_device *vfe,
 388                                         struct vfe_output *output, u8 sync)
 389 {
 390         u32 addr;
 391         unsigned int i;
 392
 393         for (i = 0; i < output->wm_num; i++) {
 394                 if (output->buf[1])
 395                         addr = output->buf[1]->addr[i];
 396                 else
 397                         addr = 0;
 398
 399                 vfe->ops->wm_set_pong_addr(vfe, output->wm_idx[i], addr);
 400                 if (sync)
 401                         vfe->ops->bus_reload_wm(vfe, output->wm_idx[i]);
 402         }
 403
 404 }
 405
 406 static int vfe_reserve_wm(struct vfe_device *vfe, enum vfe_line_id line_id)
 407 {
 408         int ret = -EBUSY;
 409         int i;
 410
 411         for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++) {
 412                 if (vfe->wm_output_map[i] == VFE_LINE_NONE) {
 413                         vfe->wm_output_map[i] = line_id;
 414                         ret = i;
 415                         break;
 416                 }
 417         }
 418
 419         return ret;
 420 }
 421
 422 static int vfe_release_wm(struct vfe_device *vfe, u8 wm)
 423 {
 424         if (wm >= ARRAY_SIZE(vfe->wm_output_map))
 425                 return -EINVAL;
 426
 427         vfe->wm_output_map[wm] = VFE_LINE_NONE;
 428
 429         return 0;
 430 }
 431
 432 static void vfe_output_frame_drop(struct vfe_device *vfe,
 433                                   struct vfe_output *output,
 434                                   u32 drop_pattern)
 435 {
 436         u8 drop_period;
 437         unsigned int i;
 438
 439         /* We need to toggle update period to be valid on next frame */
 440         output->drop_update_idx++;
 441         output->drop_update_idx %= VFE_FRAME_DROP_UPDATES;
 442         drop_period = VFE_FRAME_DROP_VAL + output->drop_update_idx;
 443
 444         for (i = 0; i < output->wm_num; i++) {
 445                 vfe->ops->wm_set_framedrop_period(vfe, output->wm_idx[i],
 446                                                   drop_period);
 447                 vfe->ops->wm_set_framedrop_pattern(vfe, output->wm_idx[i],
 448                                                    drop_pattern);
 449         }
 450         vfe->ops->reg_update(vfe,
 451                              container_of(output, struct vfe_line, output)->id);
 452 }
 453
 454 static struct camss_buffer *vfe_buf_get_pending(struct vfe_output *output)
 455 {
 456         struct camss_buffer *buffer = NULL;
 457
 458         if (!list_empty(&output->pending_bufs)) {
 459                 buffer = list_first_entry(&output->pending_bufs,
 460                                           struct camss_buffer,
 461                                           queue);
 462                 list_del(&buffer->queue);
 463         }
 464
 465         return buffer;
 466 }
 467
 468 /*
 469  * vfe_buf_add_pending - Add output buffer to list of pending
 470  * @output: VFE output
 471  * @buffer: Video buffer
 472  */
 473 static void vfe_buf_add_pending(struct vfe_output *output,
 474                                 struct camss_buffer *buffer)
 475 {
 476         INIT_LIST_HEAD(&buffer->queue);
 477         list_add_tail(&buffer->queue, &output->pending_bufs);
 478 }
 479
 480 /*
 481  * vfe_buf_flush_pending - Flush all pending buffers.
 482  * @output: VFE output
 483  * @state: vb2 buffer state
 484  */
 485 static void vfe_buf_flush_pending(struct vfe_output *output,
 486                                   enum vb2_buffer_state state)
 487 {
 488         struct camss_buffer *buf;
 489         struct camss_buffer *t;
 490
 491         list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
 492                 vb2_buffer_done(&buf->vb.vb2_buf, state);
 493                 list_del(&buf->queue);
 494         }
 495 }
 496
 497 static void vfe_buf_update_wm_on_next(struct vfe_device *vfe,
 498                                       struct vfe_output *output)
 499 {
 500         switch (output->state) {
 501         case VFE_OUTPUT_CONTINUOUS:
 502                 vfe_output_frame_drop(vfe, output, 3);
 503                 break;
 504         case VFE_OUTPUT_SINGLE:
 505         default:
 506                 dev_err_ratelimited(vfe->camss->dev,
 507                                     "Next buf in wrong state! %d\n",
 508                                     output->state);
 509                 break;
 510         }
 511 }
 512
 513 static void vfe_buf_update_wm_on_last(struct vfe_device *vfe,
 514                                       struct vfe_output *output)
 515 {
 516         switch (output->state) {
 517         case VFE_OUTPUT_CONTINUOUS:
 518                 output->state = VFE_OUTPUT_SINGLE;
 519                 vfe_output_frame_drop(vfe, output, 1);
 520                 break;
 521         case VFE_OUTPUT_SINGLE:
 522                 output->state = VFE_OUTPUT_STOPPING;
 523                 vfe_output_frame_drop(vfe, output, 0);
 524                 break;
 525         default:
 526                 dev_err_ratelimited(vfe->camss->dev,
 527                                     "Last buff in wrong state! %d\n",
 528                                     output->state);
 529                 break;
 530         }
 531 }
 532
 533 static void vfe_buf_update_wm_on_new(struct vfe_device *vfe,
 534                                      struct vfe_output *output,
 535                                      struct camss_buffer *new_buf)
 536 {
 537         int inactive_idx;
 538
 539         switch (output->state) {
 540         case VFE_OUTPUT_SINGLE:
 541                 inactive_idx = !output->active_buf;
 542
 543                 if (!output->buf[inactive_idx]) {
 544                         output->buf[inactive_idx] = new_buf;
 545
 546                         if (inactive_idx)
 547                                 vfe_output_update_pong_addr(vfe, output, 0);
 548                         else
 549                                 vfe_output_update_ping_addr(vfe, output, 0);
 550
 551                         vfe_output_frame_drop(vfe, output, 3);
 552                         output->state = VFE_OUTPUT_CONTINUOUS;
 553                 } else {
 554                         vfe_buf_add_pending(output, new_buf);
 555                         dev_err_ratelimited(vfe->camss->dev,
 556                                             "Inactive buffer is busy\n");
 557                 }
 558                 break;
 559
 560         case VFE_OUTPUT_IDLE:
 561                 if (!output->buf[0]) {
 562                         output->buf[0] = new_buf;
 563
 564                         vfe_output_init_addrs(vfe, output, 1);
 565
 566                         vfe_output_frame_drop(vfe, output, 1);
 567                         output->state = VFE_OUTPUT_SINGLE;
 568                 } else {
 569                         vfe_buf_add_pending(output, new_buf);
 570                         dev_err_ratelimited(vfe->camss->dev,
 571                                             "Output idle with buffer set!\n");
 572                 }
 573                 break;
 574
 575         case VFE_OUTPUT_CONTINUOUS:
 576         default:
 577                 vfe_buf_add_pending(output, new_buf);
 578                 break;
 579         }
 580 }
 581
 582 static int vfe_get_output(struct vfe_line *line)
 583 {
 584         struct vfe_device *vfe = to_vfe(line);
 585         struct vfe_output *output;
 586         struct v4l2_format *f = &line->video_out.active_fmt;
 587         unsigned long flags;
 588         int i;
 589         int wm_idx;
 590
 591         spin_lock_irqsave(&vfe->output_lock, flags);
 592
 593         output = &line->output;
 594         if (output->state != VFE_OUTPUT_OFF) {
 595                 dev_err(vfe->camss->dev, "Output is running\n");
 596                 goto error;
 597         }
 598         output->state = VFE_OUTPUT_RESERVED;
 599
 600         output->active_buf = 0;
 601
 602         switch (f->fmt.pix_mp.pixelformat) {
 603         case V4L2_PIX_FMT_NV12:
 604         case V4L2_PIX_FMT_NV21:
 605         case V4L2_PIX_FMT_NV16:
 606         case V4L2_PIX_FMT_NV61:
 607                 output->wm_num = 2;
 608                 break;
 609         default:
 610                 output->wm_num = 1;
 611                 break;
 612         }
 613
 614         for (i = 0; i < output->wm_num; i++) {
 615                 wm_idx = vfe_reserve_wm(vfe, line->id);
 616                 if (wm_idx < 0) {
 617                         dev_err(vfe->camss->dev, "Can not reserve wm\n");
 618                         goto error_get_wm;
 619                 }
 620                 output->wm_idx[i] = wm_idx;
 621         }
 622
 623         output->drop_update_idx = 0;
 624
 625         spin_unlock_irqrestore(&vfe->output_lock, flags);
 626
 627         return 0;
 628
 629 error_get_wm:
 630         for (i--; i >= 0; i--)
 631                 vfe_release_wm(vfe, output->wm_idx[i]);
 632         output->state = VFE_OUTPUT_OFF;
 633 error:
 634         spin_unlock_irqrestore(&vfe->output_lock, flags);
 635
 636         return -EINVAL;
 637 }
 638
 639 static int vfe_put_output(struct vfe_line *line)
 640 {
 641         struct vfe_device *vfe = to_vfe(line);
 642         struct vfe_output *output = &line->output;
 643         unsigned long flags;
 644         unsigned int i;
 645
 646         spin_lock_irqsave(&vfe->output_lock, flags);
 647
 648         for (i = 0; i < output->wm_num; i++)
 649                 vfe_release_wm(vfe, output->wm_idx[i]);
 650
 651         output->state = VFE_OUTPUT_OFF;
 652
 653         spin_unlock_irqrestore(&vfe->output_lock, flags);
 654         return 0;
 655 }
 656
 657 static int vfe_enable_output(struct vfe_line *line)
 658 {
 659         struct vfe_device *vfe = to_vfe(line);
 660         struct vfe_output *output = &line->output;
 661         const struct vfe_hw_ops *ops = vfe->ops;
 662         unsigned long flags;
 663         unsigned int i;
 664         u16 ub_size;
 665
 666         ub_size = ops->get_ub_size(vfe->id);
 667         if (!ub_size)
 668                 return -EINVAL;
 669
 670         spin_lock_irqsave(&vfe->output_lock, flags);
 671
 672         ops->reg_update_clear(vfe, line->id);
 673
 674         if (output->state != VFE_OUTPUT_RESERVED) {
 675                 dev_err(vfe->camss->dev, "Output is not in reserved state %d\n",
 676                         output->state);
 677                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 678                 return -EINVAL;
 679         }
 680         output->state = VFE_OUTPUT_IDLE;
 681
 682         output->buf[0] = vfe_buf_get_pending(output);
 683         output->buf[1] = vfe_buf_get_pending(output);
 684
 685         if (!output->buf[0] && output->buf[1]) {
 686                 output->buf[0] = output->buf[1];
 687                 output->buf[1] = NULL;
 688         }
 689
 690         if (output->buf[0])
 691                 output->state = VFE_OUTPUT_SINGLE;
 692
 693         if (output->buf[1])
 694                 output->state = VFE_OUTPUT_CONTINUOUS;
 695
 696         switch (output->state) {
 697         case VFE_OUTPUT_SINGLE:
 698                 vfe_output_frame_drop(vfe, output, 1);
 699                 break;
 700         case VFE_OUTPUT_CONTINUOUS:
 701                 vfe_output_frame_drop(vfe, output, 3);
 702                 break;
 703         default:
 704                 vfe_output_frame_drop(vfe, output, 0);
 705                 break;
 706         }
 707
 708         output->sequence = 0;
 709         output->wait_sof = 0;
 710         output->wait_reg_update = 0;
 711         reinit_completion(&output->sof);
 712         reinit_completion(&output->reg_update);
 713
 714         vfe_output_init_addrs(vfe, output, 0);
 715
 716         if (line->id != VFE_LINE_PIX) {
 717                 ops->set_cgc_override(vfe, output->wm_idx[0], 1);
 718                 ops->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 1);
 719                 ops->bus_connect_wm_to_rdi(vfe, output->wm_idx[0], line->id);
 720                 ops->wm_set_subsample(vfe, output->wm_idx[0]);
 721                 ops->set_rdi_cid(vfe, line->id, 0);
 722                 ops->wm_set_ub_cfg(vfe, output->wm_idx[0],
 723                                    (ub_size + 1) * output->wm_idx[0], ub_size);
 724                 ops->wm_frame_based(vfe, output->wm_idx[0], 1);
 725                 ops->wm_enable(vfe, output->wm_idx[0], 1);
 726                 ops->bus_reload_wm(vfe, output->wm_idx[0]);
 727         } else {
 728                 ub_size /= output->wm_num;
 729                 for (i = 0; i < output->wm_num; i++) {
 730                         ops->set_cgc_override(vfe, output->wm_idx[i], 1);
 731                         ops->wm_set_subsample(vfe, output->wm_idx[i]);
 732                         ops->wm_set_ub_cfg(vfe, output->wm_idx[i],
 733                                            (ub_size + 1) * output->wm_idx[i],
 734                                            ub_size);
 735                         ops->wm_line_based(vfe, output->wm_idx[i],
 736                                         &line->video_out.active_fmt.fmt.pix_mp,
 737                                         i, 1);
 738                         ops->wm_enable(vfe, output->wm_idx[i], 1);
 739                         ops->bus_reload_wm(vfe, output->wm_idx[i]);
 740                 }
 741                 ops->enable_irq_pix_line(vfe, 0, line->id, 1);
 742                 ops->set_module_cfg(vfe, 1);
 743                 ops->set_camif_cfg(vfe, line);
 744                 ops->set_realign_cfg(vfe, line, 1);
 745                 ops->set_xbar_cfg(vfe, output, 1);
 746                 ops->set_demux_cfg(vfe, line);
 747                 ops->set_scale_cfg(vfe, line);
 748                 ops->set_crop_cfg(vfe, line);
 749                 ops->set_clamp_cfg(vfe);
 750                 ops->set_camif_cmd(vfe, 1);
 751         }
 752
 753         ops->reg_update(vfe, line->id);
 754
 755         spin_unlock_irqrestore(&vfe->output_lock, flags);
 756
 757         return 0;
 758 }
 759
 760 static int vfe_disable_output(struct vfe_line *line)
 761 {
 762         struct vfe_device *vfe = to_vfe(line);
 763         struct vfe_output *output = &line->output;
 764         const struct vfe_hw_ops *ops = vfe->ops;
 765         unsigned long flags;
 766         unsigned long time;
 767         unsigned int i;
 768
 769         spin_lock_irqsave(&vfe->output_lock, flags);
 770
 771         output->wait_sof = 1;
 772         spin_unlock_irqrestore(&vfe->output_lock, flags);
 773
 774         time = wait_for_completion_timeout(&output->sof,
 775                                            msecs_to_jiffies(VFE_NEXT_SOF_MS));
 776         if (!time)
 777                 dev_err(vfe->camss->dev, "VFE sof timeout\n");
 778
 779         spin_lock_irqsave(&vfe->output_lock, flags);
 780         for (i = 0; i < output->wm_num; i++)
 781                 ops->wm_enable(vfe, output->wm_idx[i], 0);
 782
 783         ops->reg_update(vfe, line->id);
 784         output->wait_reg_update = 1;
 785         spin_unlock_irqrestore(&vfe->output_lock, flags);
 786
 787         time = wait_for_completion_timeout(&output->reg_update,
 788                                            msecs_to_jiffies(VFE_NEXT_SOF_MS));
 789         if (!time)
 790                 dev_err(vfe->camss->dev, "VFE reg update timeout\n");
 791
 792         spin_lock_irqsave(&vfe->output_lock, flags);
 793
 794         if (line->id != VFE_LINE_PIX) {
 795                 ops->wm_frame_based(vfe, output->wm_idx[0], 0);
 796                 ops->bus_disconnect_wm_from_rdi(vfe, output->wm_idx[0],
 797                                                 line->id);
 798                 ops->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 0);
 799                 ops->set_cgc_override(vfe, output->wm_idx[0], 0);
 800                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 801         } else {
 802                 for (i = 0; i < output->wm_num; i++) {
 803                         ops->wm_line_based(vfe, output->wm_idx[i], NULL, i, 0);
 804                         ops->set_cgc_override(vfe, output->wm_idx[i], 0);
 805                 }
 806
 807                 ops->enable_irq_pix_line(vfe, 0, line->id, 0);
 808                 ops->set_module_cfg(vfe, 0);
 809                 ops->set_realign_cfg(vfe, line, 0);
 810                 ops->set_xbar_cfg(vfe, output, 0);
 811
 812                 ops->set_camif_cmd(vfe, 0);
 813                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 814
 815                 ops->camif_wait_for_stop(vfe, vfe->camss->dev);
 816         }
 817
 818         return 0;
 819 }
 820
 821 /*
 822  * vfe_enable - Enable streaming on VFE line
 823  * @line: VFE line
 824  *
 825  * Return 0 on success or a negative error code otherwise
 826  */
 827 static int vfe_enable(struct vfe_line *line)
 828 {
 829         struct vfe_device *vfe = to_vfe(line);
 830         int ret;
 831
 832         mutex_lock(&vfe->stream_lock);
 833
 834         if (!vfe->stream_count) {
 835                 vfe->ops->enable_irq_common(vfe);
 836
 837                 vfe->ops->bus_enable_wr_if(vfe, 1);
 838
 839                 vfe->ops->set_qos(vfe);
 840
 841                 vfe->ops->set_ds(vfe);
 842         }
 843
 844         vfe->stream_count++;
 845
 846         mutex_unlock(&vfe->stream_lock);
 847
 848         ret = vfe_get_output(line);
 849         if (ret < 0)
 850                 goto error_get_output;
 851
 852         ret = vfe_enable_output(line);
 853         if (ret < 0)
 854                 goto error_enable_output;
 855
 856         vfe->was_streaming = 1;
 857
 858         return 0;
 859
 860
 861 error_enable_output:
 862         vfe_put_output(line);
 863
 864 error_get_output:
 865         mutex_lock(&vfe->stream_lock);
 866
 867         if (vfe->stream_count == 1)
 868                 vfe->ops->bus_enable_wr_if(vfe, 0);
 869
 870         vfe->stream_count--;
 871
 872         mutex_unlock(&vfe->stream_lock);
 873
 874         return ret;
 875 }
 876
 877 /*
 878  * vfe_disable - Disable streaming on VFE line
 879  * @line: VFE line
 880  *
 881  * Return 0 on success or a negative error code otherwise
 882  */
 883 static int vfe_disable(struct vfe_line *line)
 884 {
 885         struct vfe_device *vfe = to_vfe(line);
 886
 887         vfe_disable_output(line);
 888
 889         vfe_put_output(line);
 890
 891         mutex_lock(&vfe->stream_lock);
 892
 893         if (vfe->stream_count == 1)
 894                 vfe->ops->bus_enable_wr_if(vfe, 0);
 895
 896         vfe->stream_count--;
 897
 898         mutex_unlock(&vfe->stream_lock);
 899
 900         return 0;
 901 }
 902
 903 /*
 904  * vfe_isr_sof - Process start of frame interrupt
 905  * @vfe: VFE Device
 906  * @line_id: VFE line
 907  */
 908 static void vfe_isr_sof(struct vfe_device *vfe, enum vfe_line_id line_id)
 909 {
 910         struct vfe_output *output;
 911         unsigned long flags;
 912
 913         spin_lock_irqsave(&vfe->output_lock, flags);
 914         output = &vfe->line[line_id].output;
 915         if (output->wait_sof) {
 916                 output->wait_sof = 0;
 917                 complete(&output->sof);
 918         }
 919         spin_unlock_irqrestore(&vfe->output_lock, flags);
 920 }
 921
 922 /*
 923  * vfe_isr_reg_update - Process reg update interrupt
 924  * @vfe: VFE Device
 925  * @line_id: VFE line
 926  */
 927 static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
 928 {
 929         struct vfe_output *output;
 930         unsigned long flags;
 931
 932         spin_lock_irqsave(&vfe->output_lock, flags);
 933         vfe->ops->reg_update_clear(vfe, line_id);
 934
 935         output = &vfe->line[line_id].output;
 936
 937         if (output->wait_reg_update) {
 938                 output->wait_reg_update = 0;
 939                 complete(&output->reg_update);
 940                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 941                 return;
 942         }
 943
 944         if (output->state == VFE_OUTPUT_STOPPING) {
 945                 /* Release last buffer when hw is idle */
 946                 if (output->last_buffer) {
 947                         vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
 948                                         VB2_BUF_STATE_DONE);
 949                         output->last_buffer = NULL;
 950                 }
 951                 output->state = VFE_OUTPUT_IDLE;
 952
 953                 /* Buffers received in stopping state are queued in */
 954                 /* dma pending queue, start next capture here */
 955
 956                 output->buf[0] = vfe_buf_get_pending(output);
 957                 output->buf[1] = vfe_buf_get_pending(output);
 958
 959                 if (!output->buf[0] && output->buf[1]) {
 960                         output->buf[0] = output->buf[1];
 961                         output->buf[1] = NULL;
 962                 }
 963
 964                 if (output->buf[0])
 965                         output->state = VFE_OUTPUT_SINGLE;
 966
 967                 if (output->buf[1])
 968                         output->state = VFE_OUTPUT_CONTINUOUS;
 969
 970                 switch (output->state) {
 971                 case VFE_OUTPUT_SINGLE:
 972                         vfe_output_frame_drop(vfe, output, 2);
 973                         break;
 974                 case VFE_OUTPUT_CONTINUOUS:
 975                         vfe_output_frame_drop(vfe, output, 3);
 976                         break;
 977                 default:
 978                         vfe_output_frame_drop(vfe, output, 0);
 979                         break;
 980                 }
 981
 982                 vfe_output_init_addrs(vfe, output, 1);
 983         }
 984
 985         spin_unlock_irqrestore(&vfe->output_lock, flags);
 986 }
 987
 988 /*
 989  * vfe_isr_wm_done - Process write master done interrupt
 990  * @vfe: VFE Device
 991  * @wm: Write master id
 992  */
 993 static void vfe_isr_wm_done(struct vfe_device *vfe, u8 wm)
 994 {
 995         struct camss_buffer *ready_buf;
 996         struct vfe_output *output;
 997         dma_addr_t *new_addr;
 998         unsigned long flags;
 999         u32 active_index;
1000         u64 ts = ktime_get_ns();
1001         unsigned int i;
1002
1003         active_index = vfe->ops->wm_get_ping_pong_status(vfe, wm);
1004
1005         spin_lock_irqsave(&vfe->output_lock, flags);
1006
1007         if (vfe->wm_output_map[wm] == VFE_LINE_NONE) {
1008                 dev_err_ratelimited(vfe->camss->dev,
1009                                     "Received wm done for unmapped index\n");
1010                 goto out_unlock;
1011         }
1012         output = &vfe->line[vfe->wm_output_map[wm]].output;
1013
1014         if (output->active_buf == active_index) {
1015                 dev_err_ratelimited(vfe->camss->dev,
1016                                     "Active buffer mismatch!\n");
1017                 goto out_unlock;
1018         }
1019         output->active_buf = active_index;
1020
1021         ready_buf = output->buf[!active_index];
1022         if (!ready_buf) {
1023                 dev_err_ratelimited(vfe->camss->dev,
1024                                     "Missing ready buf %d %d!\n",
1025                                     !active_index, output->state);
1026                 goto out_unlock;
1027         }
1028
1029         ready_buf->vb.vb2_buf.timestamp = ts;
1030         ready_buf->vb.sequence = output->sequence++;
1031
1032         /* Get next buffer */
1033         output->buf[!active_index] = vfe_buf_get_pending(output);
1034         if (!output->buf[!active_index]) {
1035                 /* No next buffer - set same address */
1036                 new_addr = ready_buf->addr;
1037                 vfe_buf_update_wm_on_last(vfe, output);
1038         } else {
1039                 new_addr = output->buf[!active_index]->addr;
1040                 vfe_buf_update_wm_on_next(vfe, output);
1041         }
1042
1043         if (active_index)
1044                 for (i = 0; i < output->wm_num; i++)
1045                         vfe->ops->wm_set_ping_addr(vfe, output->wm_idx[i],
1046                                                    new_addr[i]);
1047         else
1048                 for (i = 0; i < output->wm_num; i++)
1049                         vfe->ops->wm_set_pong_addr(vfe, output->wm_idx[i],
1050                                                    new_addr[i]);
1051
1052         spin_unlock_irqrestore(&vfe->output_lock, flags);
1053
1054         if (output->state == VFE_OUTPUT_STOPPING)
1055                 output->last_buffer = ready_buf;
1056         else
1057                 vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
1058
1059         return;
1060
1061 out_unlock:
1062         spin_unlock_irqrestore(&vfe->output_lock, flags);
1063 }
1064
1065 /*
1066  * vfe_isr_wm_done - Process composite image done interrupt
1067  * @vfe: VFE Device
1068  * @comp: Composite image id
1069  */
1070 static void vfe_isr_comp_done(struct vfe_device *vfe, u8 comp)
1071 {
1072         unsigned int i;
1073
1074         for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++)
1075                 if (vfe->wm_output_map[i] == VFE_LINE_PIX) {
1076                         vfe_isr_wm_done(vfe, i);
1077                         break;
1078                 }
1079 }
1080
1081 static inline void vfe_isr_reset_ack(struct vfe_device *vfe)
1082 {
1083         complete(&vfe->reset_complete);
1084 }
1085
1086 static inline void vfe_isr_halt_ack(struct vfe_device *vfe)
1087 {
1088         complete(&vfe->halt_complete);
1089         vfe->ops->halt_clear(vfe);
1090 }
1091
1092 /*
1093  * vfe_set_clock_rates - Calculate and set clock rates on VFE module
1094  * @vfe: VFE device
1095  *
1096  * Return 0 on success or a negative error code otherwise
1097  */
1098 static int vfe_set_clock_rates(struct vfe_device *vfe)
1099 {
1100         struct device *dev = vfe->camss->dev;
1101         u32 pixel_clock[MSM_VFE_LINE_NUM];
1102         int i, j;
1103         int ret;
1104
1105         for (i = VFE_LINE_RDI0; i <= VFE_LINE_PIX; i++) {
1106                 ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
1107                                             &pixel_clock[i]);
1108                 if (ret)
1109                         pixel_clock[i] = 0;
1110         }
1111
1112         for (i = 0; i < vfe->nclocks; i++) {
1113                 struct camss_clock *clock = &vfe->clock[i];
1114
1115                 if (!strcmp(clock->name, "vfe0") ||
1116                     !strcmp(clock->name, "vfe1")) {
1117                         u64 min_rate = 0;
1118                         long rate;
1119
1120                         for (j = VFE_LINE_RDI0; j <= VFE_LINE_PIX; j++) {
1121                                 u32 tmp;
1122                                 u8 bpp;
1123
1124                                 if (j == VFE_LINE_PIX) {
1125                                         tmp = pixel_clock[j];
1126                                 } else {
1127                                         struct vfe_line *l = &vfe->line[j];
1128
1129                                         bpp = vfe_get_bpp(l->formats,
1130                                                 l->nformats,
1131                                                 l->fmt[MSM_VFE_PAD_SINK].code);
1132                                         tmp = pixel_clock[j] * bpp / 64;
1133                                 }
1134
1135                                 if (min_rate < tmp)
1136                                         min_rate = tmp;
1137                         }
1138
1139                         camss_add_clock_margin(&min_rate);
1140
1141                         for (j = 0; j < clock->nfreqs; j++)
1142                                 if (min_rate < clock->freq[j])
1143                                         break;
1144
1145                         if (j == clock->nfreqs) {
1146                                 dev_err(dev,
1147                                         "Pixel clock is too high for VFE");
1148                                 return -EINVAL;
1149                         }
1150
1151                         /* if sensor pixel clock is not available */
1152                         /* set highest possible VFE clock rate */
1153                         if (min_rate == 0)
1154                                 j = clock->nfreqs - 1;
1155
1156                         rate = clk_round_rate(clock->clk, clock->freq[j]);
1157                         if (rate < 0) {
1158                                 dev_err(dev, "clk round rate failed: %ld\n",
1159                                         rate);
1160                                 return -EINVAL;
1161                         }
1162
1163                         ret = clk_set_rate(clock->clk, rate);
1164                         if (ret < 0) {
1165                                 dev_err(dev, "clk set rate failed: %d\n", ret);
1166                                 return ret;
1167                         }
1168                 }
1169         }
1170
1171         return 0;
1172 }
1173
1174 /*
1175  * vfe_check_clock_rates - Check current clock rates on VFE module
1176  * @vfe: VFE device
1177  *
1178  * Return 0 if current clock rates are suitable for a new pipeline
1179  * or a negative error code otherwise
1180  */
1181 static int vfe_check_clock_rates(struct vfe_device *vfe)
1182 {
1183         u32 pixel_clock[MSM_VFE_LINE_NUM];
1184         int i, j;
1185         int ret;
1186
1187         for (i = VFE_LINE_RDI0; i <= VFE_LINE_PIX; i++) {
1188                 ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
1189                                             &pixel_clock[i]);
1190                 if (ret)
1191                         pixel_clock[i] = 0;
1192         }
1193
1194         for (i = 0; i < vfe->nclocks; i++) {
1195                 struct camss_clock *clock = &vfe->clock[i];
1196
1197                 if (!strcmp(clock->name, "vfe0") ||
1198                     !strcmp(clock->name, "vfe1")) {
1199                         u64 min_rate = 0;
1200                         unsigned long rate;
1201
1202                         for (j = VFE_LINE_RDI0; j <= VFE_LINE_PIX; j++) {
1203                                 u32 tmp;
1204                                 u8 bpp;
1205
1206                                 if (j == VFE_LINE_PIX) {
1207                                         tmp = pixel_clock[j];
1208                                 } else {
1209                                         struct vfe_line *l = &vfe->line[j];
1210
1211                                         bpp = vfe_get_bpp(l->formats,
1212                                                 l->nformats,
1213                                                 l->fmt[MSM_VFE_PAD_SINK].code);
1214                                         tmp = pixel_clock[j] * bpp / 64;
1215                                 }
1216
1217                                 if (min_rate < tmp)
1218                                         min_rate = tmp;
1219                         }
1220
1221                         camss_add_clock_margin(&min_rate);
1222
1223                         rate = clk_get_rate(clock->clk);
1224                         if (rate < min_rate)
1225                                 return -EBUSY;
1226                 }
1227         }
1228
1229         return 0;
1230 }
1231
1232 /*
1233  * vfe_get - Power up and reset VFE module
1234  * @vfe: VFE Device
1235  *
1236  * Return 0 on success or a negative error code otherwise
1237  */
1238 static int vfe_get(struct vfe_device *vfe)
1239 {
1240         int ret;
1241
1242         mutex_lock(&vfe->power_lock);
1243
1244         if (vfe->power_count == 0) {
1245                 ret = camss_pm_domain_on(vfe->camss, vfe->id);
1246                 if (ret < 0)
1247                         goto error_pm_domain;
1248
1249                 ret = pm_runtime_get_sync(vfe->camss->dev);
1250                 if (ret < 0)
1251                         goto error_pm_runtime_get;
1252
1253                 ret = vfe_set_clock_rates(vfe);
1254                 if (ret < 0)
1255                         goto error_clocks;
1256
1257                 ret = camss_enable_clocks(vfe->nclocks, vfe->clock,
1258                                           vfe->camss->dev);
1259                 if (ret < 0)
1260                         goto error_clocks;
1261
1262                 ret = vfe_reset(vfe);
1263                 if (ret < 0)
1264                         goto error_reset;
1265
1266                 vfe_reset_output_maps(vfe);
1267
1268                 vfe_init_outputs(vfe);
1269         } else {
1270                 ret = vfe_check_clock_rates(vfe);
1271                 if (ret < 0)
1272                         goto error_clocks;
1273         }
1274         vfe->power_count++;
1275
1276         mutex_unlock(&vfe->power_lock);
1277
1278         return 0;
1279
1280 error_reset:
1281         camss_disable_clocks(vfe->nclocks, vfe->clock);
1282
1283 error_clocks:
1284         pm_runtime_put_sync(vfe->camss->dev);
1285
1286 error_pm_runtime_get:
1287         camss_pm_domain_off(vfe->camss, vfe->id);
1288
1289 error_pm_domain:
1290         mutex_unlock(&vfe->power_lock);
1291
1292         return ret;
1293 }
1294
1295 /*
1296  * vfe_put - Power down VFE module
1297  * @vfe: VFE Device
1298  */
1299 static void vfe_put(struct vfe_device *vfe)
1300 {
1301         mutex_lock(&vfe->power_lock);
1302
1303         if (vfe->power_count == 0) {
1304                 dev_err(vfe->camss->dev, "vfe power off on power_count == 0\n");
1305                 goto exit;
1306         } else if (vfe->power_count == 1) {
1307                 if (vfe->was_streaming) {
1308                         vfe->was_streaming = 0;
1309                         vfe_halt(vfe);
1310                 }
1311                 camss_disable_clocks(vfe->nclocks, vfe->clock);
1312                 pm_runtime_put_sync(vfe->camss->dev);
1313                 camss_pm_domain_off(vfe->camss, vfe->id);
1314         }
1315
1316         vfe->power_count--;
1317
1318 exit:
1319         mutex_unlock(&vfe->power_lock);
1320 }
1321
1322 /*
1323  * vfe_queue_buffer - Add empty buffer
1324  * @vid: Video device structure
1325  * @buf: Buffer to be enqueued
1326  *
1327  * Add an empty buffer - depending on the current number of buffers it will be
1328  * put in pending buffer queue or directly given to the hardware to be filled.
1329  *
1330  * Return 0 on success or a negative error code otherwise
1331  */
1332 static int vfe_queue_buffer(struct camss_video *vid,
1333                             struct camss_buffer *buf)
1334 {
1335         struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
1336         struct vfe_device *vfe = to_vfe(line);
1337         struct vfe_output *output;
1338         unsigned long flags;
1339
1340         output = &line->output;
1341
1342         spin_lock_irqsave(&vfe->output_lock, flags);
1343
1344         vfe_buf_update_wm_on_new(vfe, output, buf);
1345
1346         spin_unlock_irqrestore(&vfe->output_lock, flags);
1347
1348         return 0;
1349 }
1350
1351 /*
1352  * vfe_flush_buffers - Return all vb2 buffers
1353  * @vid: Video device structure
1354  * @state: vb2 buffer state of the returned buffers
1355  *
1356  * Return all buffers to vb2. This includes queued pending buffers (still
1357  * unused) and any buffers given to the hardware but again still not used.
1358  *
1359  * Return 0 on success or a negative error code otherwise
1360  */
1361 static int vfe_flush_buffers(struct camss_video *vid,
1362                              enum vb2_buffer_state state)
1363 {
1364         struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
1365         struct vfe_device *vfe = to_vfe(line);
1366         struct vfe_output *output;
1367         unsigned long flags;
1368
1369         output = &line->output;
1370
1371         spin_lock_irqsave(&vfe->output_lock, flags);
1372
1373         vfe_buf_flush_pending(output, state);
1374
1375         if (output->buf[0])
1376                 vb2_buffer_done(&output->buf[0]->vb.vb2_buf, state);
1377
1378         if (output->buf[1])
1379                 vb2_buffer_done(&output->buf[1]->vb.vb2_buf, state);
1380
1381         if (output->last_buffer) {
1382                 vb2_buffer_done(&output->last_buffer->vb.vb2_buf, state);
1383                 output->last_buffer = NULL;
1384         }
1385
1386         spin_unlock_irqrestore(&vfe->output_lock, flags);
1387
1388         return 0;
1389 }
1390
1391 /*
1392  * vfe_set_power - Power on/off VFE module
1393  * @sd: VFE V4L2 subdevice
1394  * @on: Requested power state
1395  *
1396  * Return 0 on success or a negative error code otherwise
1397  */
1398 static int vfe_set_power(struct v4l2_subdev *sd, int on)
1399 {
1400         struct vfe_line *line = v4l2_get_subdevdata(sd);
1401         struct vfe_device *vfe = to_vfe(line);
1402         int ret;
1403
1404         if (on) {
1405                 ret = vfe_get(vfe);
1406                 if (ret < 0)
1407                         return ret;
1408
1409                 vfe->ops->hw_version_read(vfe, vfe->camss->dev);
1410         } else {
1411                 vfe_put(vfe);
1412         }
1413
1414         return 0;
1415 }
1416
1417 /*
1418  * vfe_set_stream - Enable/disable streaming on VFE module
1419  * @sd: VFE V4L2 subdevice
1420  * @enable: Requested streaming state
1421  *
1422  * Main configuration of VFE module is triggered here.
1423  *
1424  * Return 0 on success or a negative error code otherwise
1425  */
1426 static int vfe_set_stream(struct v4l2_subdev *sd, int enable)
1427 {
1428         struct vfe_line *line = v4l2_get_subdevdata(sd);
1429         struct vfe_device *vfe = to_vfe(line);
1430         int ret;
1431
1432         if (enable) {
1433                 ret = vfe_enable(line);
1434                 if (ret < 0)
1435                         dev_err(vfe->camss->dev,
1436                                 "Failed to enable vfe outputs\n");
1437         } else {
1438                 ret = vfe_disable(line);
1439                 if (ret < 0)
1440                         dev_err(vfe->camss->dev,
1441                                 "Failed to disable vfe outputs\n");
1442         }
1443
1444         return ret;
1445 }
1446
1447 /*
1448  * __vfe_get_format - Get pointer to format structure
1449  * @line: VFE line
1450  * @cfg: V4L2 subdev pad configuration
1451  * @pad: pad from which format is requested
1452  * @which: TRY or ACTIVE format
1453  *
1454  * Return pointer to TRY or ACTIVE format structure
1455  */
1456 static struct v4l2_mbus_framefmt *
1457 __vfe_get_format(struct vfe_line *line,
1458                  struct v4l2_subdev_pad_config *cfg,
1459                  unsigned int pad,
1460                  enum v4l2_subdev_format_whence which)
1461 {
1462         if (which == V4L2_SUBDEV_FORMAT_TRY)
1463                 return v4l2_subdev_get_try_format(&line->subdev, cfg, pad);
1464
1465         return &line->fmt[pad];
1466 }
1467
1468 /*
1469  * __vfe_get_compose - Get pointer to compose selection structure
1470  * @line: VFE line
1471  * @cfg: V4L2 subdev pad configuration
1472  * @which: TRY or ACTIVE format
1473  *
1474  * Return pointer to TRY or ACTIVE compose rectangle structure
1475  */
1476 static struct v4l2_rect *
1477 __vfe_get_compose(struct vfe_line *line,
1478                   struct v4l2_subdev_pad_config *cfg,
1479                   enum v4l2_subdev_format_whence which)
1480 {
1481         if (which == V4L2_SUBDEV_FORMAT_TRY)
1482                 return v4l2_subdev_get_try_compose(&line->subdev, cfg,
1483                                                    MSM_VFE_PAD_SINK);
1484
1485         return &line->compose;
1486 }
1487
1488 /*
1489  * __vfe_get_crop - Get pointer to crop selection structure
1490  * @line: VFE line
1491  * @cfg: V4L2 subdev pad configuration
1492  * @which: TRY or ACTIVE format
1493  *
1494  * Return pointer to TRY or ACTIVE crop rectangle structure
1495  */
1496 static struct v4l2_rect *
1497 __vfe_get_crop(struct vfe_line *line,
1498                struct v4l2_subdev_pad_config *cfg,
1499                enum v4l2_subdev_format_whence which)
1500 {
1501         if (which == V4L2_SUBDEV_FORMAT_TRY)
1502                 return v4l2_subdev_get_try_crop(&line->subdev, cfg,
1503                                                 MSM_VFE_PAD_SRC);
1504
1505         return &line->crop;
1506 }
1507
1508 /*
1509  * vfe_try_format - Handle try format by pad subdev method
1510  * @line: VFE line
1511  * @cfg: V4L2 subdev pad configuration
1512  * @pad: pad on which format is requested
1513  * @fmt: pointer to v4l2 format structure
1514  * @which: wanted subdev format
1515  */
1516 static void vfe_try_format(struct vfe_line *line,
1517                            struct v4l2_subdev_pad_config *cfg,
1518                            unsigned int pad,
1519                            struct v4l2_mbus_framefmt *fmt,
1520                            enum v4l2_subdev_format_whence which)
1521 {
1522         unsigned int i;
1523         u32 code;
1524
1525         switch (pad) {
1526         case MSM_VFE_PAD_SINK:
1527                 /* Set format on sink pad */
1528
1529                 for (i = 0; i < line->nformats; i++)
1530                         if (fmt->code == line->formats[i].code)
1531                                 break;
1532
1533                 /* If not found, use UYVY as default */
1534                 if (i >= line->nformats)
1535                         fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
1536
1537                 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
1538                 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
1539
1540                 fmt->field = V4L2_FIELD_NONE;
1541                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
1542
1543                 break;
1544
1545         case MSM_VFE_PAD_SRC:
1546                 /* Set and return a format same as sink pad */
1547                 code = fmt->code;
1548
1549                 *fmt = *__vfe_get_format(line, cfg, MSM_VFE_PAD_SINK, which);
1550
1551                 fmt->code = vfe_src_pad_code(line, fmt->code, 0, code);
1552
1553                 if (line->id == VFE_LINE_PIX) {
1554                         struct v4l2_rect *rect;
1555
1556                         rect = __vfe_get_crop(line, cfg, which);
1557
1558                         fmt->width = rect->width;
1559                         fmt->height = rect->height;
1560                 }
1561
1562                 break;
1563         }
1564
1565         fmt->colorspace = V4L2_COLORSPACE_SRGB;
1566 }
1567
1568 /*
1569  * vfe_try_compose - Handle try compose selection by pad subdev method
1570  * @line: VFE line
1571  * @cfg: V4L2 subdev pad configuration
1572  * @rect: pointer to v4l2 rect structure
1573  * @which: wanted subdev format
1574  */
1575 static void vfe_try_compose(struct vfe_line *line,
1576                             struct v4l2_subdev_pad_config *cfg,
1577                             struct v4l2_rect *rect,
1578                             enum v4l2_subdev_format_whence which)
1579 {
1580         struct v4l2_mbus_framefmt *fmt;
1581
1582         fmt = __vfe_get_format(line, cfg, MSM_VFE_PAD_SINK, which);
1583
1584         if (rect->width > fmt->width)
1585                 rect->width = fmt->width;
1586
1587         if (rect->height > fmt->height)
1588                 rect->height = fmt->height;
1589
1590         if (fmt->width > rect->width * SCALER_RATIO_MAX)
1591                 rect->width = (fmt->width + SCALER_RATIO_MAX - 1) /
1592                                                         SCALER_RATIO_MAX;
1593
1594         rect->width &= ~0x1;
1595
1596         if (fmt->height > rect->height * SCALER_RATIO_MAX)
1597                 rect->height = (fmt->height + SCALER_RATIO_MAX - 1) /
1598                                                         SCALER_RATIO_MAX;
1599
1600         if (rect->width < 16)
1601                 rect->width = 16;
1602
1603         if (rect->height < 4)
1604                 rect->height = 4;
1605 }
1606
1607 /*
1608  * vfe_try_crop - Handle try crop selection by pad subdev method
1609  * @line: VFE line
1610  * @cfg: V4L2 subdev pad configuration
1611  * @rect: pointer to v4l2 rect structure
1612  * @which: wanted subdev format
1613  */
1614 static void vfe_try_crop(struct vfe_line *line,
1615                          struct v4l2_subdev_pad_config *cfg,
1616                          struct v4l2_rect *rect,
1617                          enum v4l2_subdev_format_whence which)
1618 {
1619         struct v4l2_rect *compose;
1620
1621         compose = __vfe_get_compose(line, cfg, which);
1622
1623         if (rect->width > compose->width)
1624                 rect->width = compose->width;
1625
1626         if (rect->width + rect->left > compose->width)
1627                 rect->left = compose->width - rect->width;
1628
1629         if (rect->height > compose->height)
1630                 rect->height = compose->height;
1631
1632         if (rect->height + rect->top > compose->height)
1633                 rect->top = compose->height - rect->height;
1634
1635         /* wm in line based mode writes multiple of 16 horizontally */
1636         rect->left += (rect->width & 0xf) >> 1;
1637         rect->width &= ~0xf;
1638
1639         if (rect->width < 16) {
1640                 rect->left = 0;
1641                 rect->width = 16;
1642         }
1643
1644         if (rect->height < 4) {
1645                 rect->top = 0;
1646                 rect->height = 4;
1647         }
1648 }
1649
1650 /*
1651  * vfe_enum_mbus_code - Handle pixel format enumeration
1652  * @sd: VFE V4L2 subdevice
1653  * @cfg: V4L2 subdev pad configuration
1654  * @code: pointer to v4l2_subdev_mbus_code_enum structure
1655  *
1656  * return -EINVAL or zero on success
1657  */
1658 static int vfe_enum_mbus_code(struct v4l2_subdev *sd,
1659                               struct v4l2_subdev_pad_config *cfg,
1660                               struct v4l2_subdev_mbus_code_enum *code)
1661 {
1662         struct vfe_line *line = v4l2_get_subdevdata(sd);
1663
1664         if (code->pad == MSM_VFE_PAD_SINK) {
1665                 if (code->index >= line->nformats)
1666                         return -EINVAL;
1667
1668                 code->code = line->formats[code->index].code;
1669         } else {
1670                 struct v4l2_mbus_framefmt *sink_fmt;
1671
1672                 sink_fmt = __vfe_get_format(line, cfg, MSM_VFE_PAD_SINK,
1673                                             code->which);
1674
1675                 code->code = vfe_src_pad_code(line, sink_fmt->code,
1676                                               code->index, 0);
1677                 if (!code->code)
1678                         return -EINVAL;
1679         }
1680
1681         return 0;
1682 }
1683
1684 /*
1685  * vfe_enum_frame_size - Handle frame size enumeration
1686  * @sd: VFE V4L2 subdevice
1687  * @cfg: V4L2 subdev pad configuration
1688  * @fse: pointer to v4l2_subdev_frame_size_enum structure
1689  *
1690  * Return -EINVAL or zero on success
1691  */
1692 static int vfe_enum_frame_size(struct v4l2_subdev *sd,
1693                                struct v4l2_subdev_pad_config *cfg,
1694                                struct v4l2_subdev_frame_size_enum *fse)
1695 {
1696         struct vfe_line *line = v4l2_get_subdevdata(sd);
1697         struct v4l2_mbus_framefmt format;
1698
1699         if (fse->index != 0)
1700                 return -EINVAL;
1701
1702         format.code = fse->code;
1703         format.width = 1;
1704         format.height = 1;
1705         vfe_try_format(line, cfg, fse->pad, &format, fse->which);
1706         fse->min_width = format.width;
1707         fse->min_height = format.height;
1708
1709         if (format.code != fse->code)
1710                 return -EINVAL;
1711
1712         format.code = fse->code;
1713         format.width = -1;
1714         format.height = -1;
1715         vfe_try_format(line, cfg, fse->pad, &format, fse->which);
1716         fse->max_width = format.width;
1717         fse->max_height = format.height;
1718
1719         return 0;
1720 }
1721
1722 /*
1723  * vfe_get_format - Handle get format by pads subdev method
1724  * @sd: VFE V4L2 subdevice
1725  * @cfg: V4L2 subdev pad configuration
1726  * @fmt: pointer to v4l2 subdev format structure
1727  *
1728  * Return -EINVAL or zero on success
1729  */
1730 static int vfe_get_format(struct v4l2_subdev *sd,
1731                           struct v4l2_subdev_pad_config *cfg,
1732                           struct v4l2_subdev_format *fmt)
1733 {
1734         struct vfe_line *line = v4l2_get_subdevdata(sd);
1735         struct v4l2_mbus_framefmt *format;
1736
1737         format = __vfe_get_format(line, cfg, fmt->pad, fmt->which);
1738         if (format == NULL)
1739                 return -EINVAL;
1740
1741         fmt->format = *format;
1742
1743         return 0;
1744 }
1745
1746 static int vfe_set_selection(struct v4l2_subdev *sd,
1747                              struct v4l2_subdev_pad_config *cfg,
1748                              struct v4l2_subdev_selection *sel);
1749
1750 /*
1751  * vfe_set_format - Handle set format by pads subdev method
1752  * @sd: VFE V4L2 subdevice
1753  * @cfg: V4L2 subdev pad configuration
1754  * @fmt: pointer to v4l2 subdev format structure
1755  *
1756  * Return -EINVAL or zero on success
1757  */
1758 static int vfe_set_format(struct v4l2_subdev *sd,
1759                           struct v4l2_subdev_pad_config *cfg,
1760                           struct v4l2_subdev_format *fmt)
1761 {
1762         struct vfe_line *line = v4l2_get_subdevdata(sd);
1763         struct v4l2_mbus_framefmt *format;
1764
1765         format = __vfe_get_format(line, cfg, fmt->pad, fmt->which);
1766         if (format == NULL)
1767                 return -EINVAL;
1768
1769         vfe_try_format(line, cfg, fmt->pad, &fmt->format, fmt->which);
1770         *format = fmt->format;
1771
1772         if (fmt->pad == MSM_VFE_PAD_SINK) {
1773                 struct v4l2_subdev_selection sel = { 0 };
1774                 int ret;
1775
1776                 /* Propagate the format from sink to source */
1777                 format = __vfe_get_format(line, cfg, MSM_VFE_PAD_SRC,
1778                                           fmt->which);
1779
1780                 *format = fmt->format;
1781                 vfe_try_format(line, cfg, MSM_VFE_PAD_SRC, format,
1782                                fmt->which);
1783
1784                 if (line->id != VFE_LINE_PIX)
1785                         return 0;
1786
1787                 /* Reset sink pad compose selection */
1788                 sel.which = fmt->which;
1789                 sel.pad = MSM_VFE_PAD_SINK;
1790                 sel.target = V4L2_SEL_TGT_COMPOSE;
1791                 sel.r.width = fmt->format.width;
1792                 sel.r.height = fmt->format.height;
1793                 ret = vfe_set_selection(sd, cfg, &sel);
1794                 if (ret < 0)
1795                         return ret;
1796         }
1797
1798         return 0;
1799 }
1800
1801 /*
1802  * vfe_get_selection - Handle get selection by pads subdev method
1803  * @sd: VFE V4L2 subdevice
1804  * @cfg: V4L2 subdev pad configuration
1805  * @sel: pointer to v4l2 subdev selection structure
1806  *
1807  * Return -EINVAL or zero on success
1808  */
1809 static int vfe_get_selection(struct v4l2_subdev *sd,
1810                              struct v4l2_subdev_pad_config *cfg,
1811                              struct v4l2_subdev_selection *sel)
1812 {
1813         struct vfe_line *line = v4l2_get_subdevdata(sd);
1814         struct v4l2_subdev_format fmt = { 0 };
1815         struct v4l2_rect *rect;
1816         int ret;
1817
1818         if (line->id != VFE_LINE_PIX)
1819                 return -EINVAL;
1820
1821         if (sel->pad == MSM_VFE_PAD_SINK)
1822                 switch (sel->target) {
1823                 case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1824                         fmt.pad = sel->pad;
1825                         fmt.which = sel->which;
1826                         ret = vfe_get_format(sd, cfg, &fmt);
1827                         if (ret < 0)
1828                                 return ret;
1829
1830                         sel->r.left = 0;
1831                         sel->r.top = 0;
1832                         sel->r.width = fmt.format.width;
1833                         sel->r.height = fmt.format.height;
1834                         break;
1835                 case V4L2_SEL_TGT_COMPOSE:
1836                         rect = __vfe_get_compose(line, cfg, sel->which);
1837                         if (rect == NULL)
1838                                 return -EINVAL;
1839
1840                         sel->r = *rect;
1841                         break;
1842                 default:
1843                         return -EINVAL;
1844                 }
1845         else if (sel->pad == MSM_VFE_PAD_SRC)
1846                 switch (sel->target) {
1847                 case V4L2_SEL_TGT_CROP_BOUNDS:
1848                         rect = __vfe_get_compose(line, cfg, sel->which);
1849                         if (rect == NULL)
1850                                 return -EINVAL;
1851
1852                         sel->r.left = rect->left;
1853                         sel->r.top = rect->top;
1854                         sel->r.width = rect->width;
1855                         sel->r.height = rect->height;
1856                         break;
1857                 case V4L2_SEL_TGT_CROP:
1858                         rect = __vfe_get_crop(line, cfg, sel->which);
1859                         if (rect == NULL)
1860                                 return -EINVAL;
1861
1862                         sel->r = *rect;
1863                         break;
1864                 default:
1865                         return -EINVAL;
1866                 }
1867
1868         return 0;
1869 }
1870
1871 /*
1872  * vfe_set_selection - Handle set selection by pads subdev method
1873  * @sd: VFE V4L2 subdevice
1874  * @cfg: V4L2 subdev pad configuration
1875  * @sel: pointer to v4l2 subdev selection structure
1876  *
1877  * Return -EINVAL or zero on success
1878  */
1879 static int vfe_set_selection(struct v4l2_subdev *sd,
1880                              struct v4l2_subdev_pad_config *cfg,
1881                              struct v4l2_subdev_selection *sel)
1882 {
1883         struct vfe_line *line = v4l2_get_subdevdata(sd);
1884         struct v4l2_rect *rect;
1885         int ret;
1886
1887         if (line->id != VFE_LINE_PIX)
1888                 return -EINVAL;
1889
1890         if (sel->target == V4L2_SEL_TGT_COMPOSE &&
1891                 sel->pad == MSM_VFE_PAD_SINK) {
1892                 struct v4l2_subdev_selection crop = { 0 };
1893
1894                 rect = __vfe_get_compose(line, cfg, sel->which);
1895                 if (rect == NULL)
1896                         return -EINVAL;
1897
1898                 vfe_try_compose(line, cfg, &sel->r, sel->which);
1899                 *rect = sel->r;
1900
1901                 /* Reset source crop selection */
1902                 crop.which = sel->which;
1903                 crop.pad = MSM_VFE_PAD_SRC;
1904                 crop.target = V4L2_SEL_TGT_CROP;
1905                 crop.r = *rect;
1906                 ret = vfe_set_selection(sd, cfg, &crop);
1907         } else if (sel->target == V4L2_SEL_TGT_CROP &&
1908                 sel->pad == MSM_VFE_PAD_SRC) {
1909                 struct v4l2_subdev_format fmt = { 0 };
1910
1911                 rect = __vfe_get_crop(line, cfg, sel->which);
1912                 if (rect == NULL)
1913                         return -EINVAL;
1914
1915                 vfe_try_crop(line, cfg, &sel->r, sel->which);
1916                 *rect = sel->r;
1917
1918                 /* Reset source pad format width and height */
1919                 fmt.which = sel->which;
1920                 fmt.pad = MSM_VFE_PAD_SRC;
1921                 ret = vfe_get_format(sd, cfg, &fmt);
1922                 if (ret < 0)
1923                         return ret;
1924
1925                 fmt.format.width = rect->width;
1926                 fmt.format.height = rect->height;
1927                 ret = vfe_set_format(sd, cfg, &fmt);
1928         } else {
1929                 ret = -EINVAL;
1930         }
1931
1932         return ret;
1933 }
1934
1935 /*
1936  * vfe_init_formats - Initialize formats on all pads
1937  * @sd: VFE V4L2 subdevice
1938  * @fh: V4L2 subdev file handle
1939  *
1940  * Initialize all pad formats with default values.
1941  *
1942  * Return 0 on success or a negative error code otherwise
1943  */
1944 static int vfe_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1945 {
1946         struct v4l2_subdev_format format = {
1947                 .pad = MSM_VFE_PAD_SINK,
1948                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
1949                               V4L2_SUBDEV_FORMAT_ACTIVE,
1950                 .format = {
1951                         .code = MEDIA_BUS_FMT_UYVY8_2X8,
1952                         .width = 1920,
1953                         .height = 1080
1954                 }
1955         };
1956
1957         return vfe_set_format(sd, fh ? fh->pad : NULL, &format);
1958 }
1959
1960 /*
1961  * msm_vfe_subdev_init - Initialize VFE device structure and resources
1962  * @vfe: VFE device
1963  * @res: VFE module resources table
1964  *
1965  * Return 0 on success or a negative error code otherwise
1966  */
1967 int msm_vfe_subdev_init(struct camss *camss, struct vfe_device *vfe,
1968                         const struct resources *res, u8 id)
1969 {
1970         struct device *dev = camss->dev;
1971         struct platform_device *pdev = to_platform_device(dev);
1972         struct resource *r;
1973         int i, j;
1974         int ret;
1975
1976         vfe->isr_ops.reset_ack = vfe_isr_reset_ack;
1977         vfe->isr_ops.halt_ack = vfe_isr_halt_ack;
1978         vfe->isr_ops.reg_update = vfe_isr_reg_update;
1979         vfe->isr_ops.sof = vfe_isr_sof;
1980         vfe->isr_ops.comp_done = vfe_isr_comp_done;
1981         vfe->isr_ops.wm_done = vfe_isr_wm_done;
1982
1983         if (camss->version == CAMSS_8x16)
1984                 vfe->ops = &vfe_ops_4_1;
1985         else if (camss->version == CAMSS_8x96)
1986                 vfe->ops = &vfe_ops_4_7;
1987         else
1988                 return -EINVAL;
1989
1990         /* Memory */
1991
1992         r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
1993         vfe->base = devm_ioremap_resource(dev, r);
1994         if (IS_ERR(vfe->base)) {
1995                 dev_err(dev, "could not map memory\n");
1996                 return PTR_ERR(vfe->base);
1997         }
1998
1999         /* Interrupt */
2000
2001         r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
2002                                          res->interrupt[0]);
2003         if (!r) {
2004                 dev_err(dev, "missing IRQ\n");
2005                 return -EINVAL;
2006         }
2007
2008         vfe->irq = r->start;
2009         snprintf(vfe->irq_name, sizeof(vfe->irq_name), "%s_%s%d",
2010                  dev_name(dev), MSM_VFE_NAME, vfe->id);
2011         ret = devm_request_irq(dev, vfe->irq, vfe->ops->isr,
2012                                IRQF_TRIGGER_RISING, vfe->irq_name, vfe);
2013         if (ret < 0) {
2014                 dev_err(dev, "request_irq failed: %d\n", ret);
2015                 return ret;
2016         }
2017
2018         /* Clocks */
2019
2020         vfe->nclocks = 0;
2021         while (res->clock[vfe->nclocks])
2022                 vfe->nclocks++;
2023
2024         vfe->clock = devm_kcalloc(dev, vfe->nclocks, sizeof(*vfe->clock),
2025                                   GFP_KERNEL);
2026         if (!vfe->clock)
2027                 return -ENOMEM;
2028
2029         for (i = 0; i < vfe->nclocks; i++) {
2030                 struct camss_clock *clock = &vfe->clock[i];
2031
2032                 clock->clk = devm_clk_get(dev, res->clock[i]);
2033                 if (IS_ERR(clock->clk))
2034                         return PTR_ERR(clock->clk);
2035
2036                 clock->name = res->clock[i];
2037
2038                 clock->nfreqs = 0;
2039                 while (res->clock_rate[i][clock->nfreqs])
2040                         clock->nfreqs++;
2041
2042                 if (!clock->nfreqs) {
2043                         clock->freq = NULL;
2044                         continue;
2045                 }
2046
2047                 clock->freq = devm_kcalloc(dev,
2048                                            clock->nfreqs,
2049                                            sizeof(*clock->freq),
2050                                            GFP_KERNEL);
2051                 if (!clock->freq)
2052                         return -ENOMEM;
2053
2054                 for (j = 0; j < clock->nfreqs; j++)
2055                         clock->freq[j] = res->clock_rate[i][j];
2056         }
2057
2058         mutex_init(&vfe->power_lock);
2059         vfe->power_count = 0;
2060
2061         mutex_init(&vfe->stream_lock);
2062         vfe->stream_count = 0;
2063
2064         spin_lock_init(&vfe->output_lock);
2065
2066         vfe->camss = camss;
2067         vfe->id = id;
2068         vfe->reg_update = 0;
2069
2070         for (i = VFE_LINE_RDI0; i <= VFE_LINE_PIX; i++) {
2071                 struct vfe_line *l = &vfe->line[i];
2072
2073                 l->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
2074                 l->video_out.camss = camss;
2075                 l->id = i;
2076                 init_completion(&l->output.sof);
2077                 init_completion(&l->output.reg_update);
2078
2079                 if (camss->version == CAMSS_8x16) {
2080                         if (i == VFE_LINE_PIX) {
2081                                 l->formats = formats_pix_8x16;
2082                                 l->nformats = ARRAY_SIZE(formats_pix_8x16);
2083                         } else {
2084                                 l->formats = formats_rdi_8x16;
2085                                 l->nformats = ARRAY_SIZE(formats_rdi_8x16);
2086                         }
2087                 } else if (camss->version == CAMSS_8x96) {
2088                         if (i == VFE_LINE_PIX) {
2089                                 l->formats = formats_pix_8x96;
2090                                 l->nformats = ARRAY_SIZE(formats_pix_8x96);
2091                         } else {
2092                                 l->formats = formats_rdi_8x96;
2093                                 l->nformats = ARRAY_SIZE(formats_rdi_8x96);
2094                         }
2095                 } else {
2096                         return -EINVAL;
2097                 }
2098         }
2099
2100         init_completion(&vfe->reset_complete);
2101         init_completion(&vfe->halt_complete);
2102
2103         return 0;
2104 }
2105
2106 /*
2107  * msm_vfe_get_vfe_id - Get VFE HW module id
2108  * @entity: Pointer to VFE media entity structure
2109  * @id: Return CSID HW module id here
2110  */
2111 void msm_vfe_get_vfe_id(struct media_entity *entity, u8 *id)
2112 {
2113         struct v4l2_subdev *sd;
2114         struct vfe_line *line;
2115         struct vfe_device *vfe;
2116
2117         sd = media_entity_to_v4l2_subdev(entity);
2118         line = v4l2_get_subdevdata(sd);
2119         vfe = to_vfe(line);
2120
2121         *id = vfe->id;
2122 }
2123
2124 /*
2125  * msm_vfe_get_vfe_line_id - Get VFE line id by media entity
2126  * @entity: Pointer to VFE media entity structure
2127  * @id: Return VFE line id here
2128  */
2129 void msm_vfe_get_vfe_line_id(struct media_entity *entity, enum vfe_line_id *id)
2130 {
2131         struct v4l2_subdev *sd;
2132         struct vfe_line *line;
2133
2134         sd = media_entity_to_v4l2_subdev(entity);
2135         line = v4l2_get_subdevdata(sd);
2136
2137         *id = line->id;
2138 }
2139
2140 /*
2141  * vfe_link_setup - Setup VFE connections
2142  * @entity: Pointer to media entity structure
2143  * @local: Pointer to local pad
2144  * @remote: Pointer to remote pad
2145  * @flags: Link flags
2146  *
2147  * Return 0 on success
2148  */
2149 static int vfe_link_setup(struct media_entity *entity,
2150                           const struct media_pad *local,
2151                           const struct media_pad *remote, u32 flags)
2152 {
2153         if (flags & MEDIA_LNK_FL_ENABLED)
2154                 if (media_entity_remote_pad(local))
2155                         return -EBUSY;
2156
2157         return 0;
2158 }
2159
2160 static const struct v4l2_subdev_core_ops vfe_core_ops = {
2161         .s_power = vfe_set_power,
2162 };
2163
2164 static const struct v4l2_subdev_video_ops vfe_video_ops = {
2165         .s_stream = vfe_set_stream,
2166 };
2167
2168 static const struct v4l2_subdev_pad_ops vfe_pad_ops = {
2169         .enum_mbus_code = vfe_enum_mbus_code,
2170         .enum_frame_size = vfe_enum_frame_size,
2171         .get_fmt = vfe_get_format,
2172         .set_fmt = vfe_set_format,
2173         .get_selection = vfe_get_selection,
2174         .set_selection = vfe_set_selection,
2175 };
2176
2177 static const struct v4l2_subdev_ops vfe_v4l2_ops = {
2178         .core = &vfe_core_ops,
2179         .video = &vfe_video_ops,
2180         .pad = &vfe_pad_ops,
2181 };
2182
2183 static const struct v4l2_subdev_internal_ops vfe_v4l2_internal_ops = {
2184         .open = vfe_init_formats,
2185 };
2186
2187 static const struct media_entity_operations vfe_media_ops = {
2188         .link_setup = vfe_link_setup,
2189         .link_validate = v4l2_subdev_link_validate,
2190 };
2191
2192 static const struct camss_video_ops camss_vfe_video_ops = {
2193         .queue_buffer = vfe_queue_buffer,
2194         .flush_buffers = vfe_flush_buffers,
2195 };
2196
2197 void msm_vfe_stop_streaming(struct vfe_device *vfe)
2198 {
2199         int i;
2200
2201         for (i = 0; i < ARRAY_SIZE(vfe->line); i++)
2202                 msm_video_stop_streaming(&vfe->line[i].video_out);
2203 }
2204
2205 /*
2206  * msm_vfe_register_entities - Register subdev node for VFE module
2207  * @vfe: VFE device
2208  * @v4l2_dev: V4L2 device
2209  *
2210  * Initialize and register a subdev node for the VFE module. Then
2211  * call msm_video_register() to register the video device node which
2212  * will be connected to this subdev node. Then actually create the
2213  * media link between them.
2214  *
2215  * Return 0 on success or a negative error code otherwise
2216  */
2217 int msm_vfe_register_entities(struct vfe_device *vfe,
2218                               struct v4l2_device *v4l2_dev)
2219 {
2220         struct device *dev = vfe->camss->dev;
2221         struct v4l2_subdev *sd;
2222         struct media_pad *pads;
2223         struct camss_video *video_out;
2224         int ret;
2225         int i;
2226
2227         for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
2228                 char name[32];
2229
2230                 sd = &vfe->line[i].subdev;
2231                 pads = vfe->line[i].pads;
2232                 video_out = &vfe->line[i].video_out;
2233
2234                 v4l2_subdev_init(sd, &vfe_v4l2_ops);
2235                 sd->internal_ops = &vfe_v4l2_internal_ops;
2236                 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
2237                 if (i == VFE_LINE_PIX)
2238                         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
2239                                  MSM_VFE_NAME, vfe->id, "pix");
2240                 else
2241                         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s%d",
2242                                  MSM_VFE_NAME, vfe->id, "rdi", i);
2243
2244                 v4l2_set_subdevdata(sd, &vfe->line[i]);
2245
2246                 ret = vfe_init_formats(sd, NULL);
2247                 if (ret < 0) {
2248                         dev_err(dev, "Failed to init format: %d\n", ret);
2249                         goto error_init;
2250                 }
2251
2252                 pads[MSM_VFE_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
2253                 pads[MSM_VFE_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
2254
2255                 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
2256                 sd->entity.ops = &vfe_media_ops;
2257                 ret = media_entity_pads_init(&sd->entity, MSM_VFE_PADS_NUM,
2258                                              pads);
2259                 if (ret < 0) {
2260                         dev_err(dev, "Failed to init media entity: %d\n", ret);
2261                         goto error_init;
2262                 }
2263
2264                 ret = v4l2_device_register_subdev(v4l2_dev, sd);
2265                 if (ret < 0) {
2266                         dev_err(dev, "Failed to register subdev: %d\n", ret);
2267                         goto error_reg_subdev;
2268                 }
2269
2270                 video_out->ops = &camss_vfe_video_ops;
2271                 video_out->bpl_alignment = 8;
2272                 video_out->line_based = 0;
2273                 if (i == VFE_LINE_PIX) {
2274                         video_out->bpl_alignment = 16;
2275                         video_out->line_based = 1;
2276                 }
2277                 snprintf(name, ARRAY_SIZE(name), "%s%d_%s%d",
2278                          MSM_VFE_NAME, vfe->id, "video", i);
2279                 ret = msm_video_register(video_out, v4l2_dev, name,
2280                                          i == VFE_LINE_PIX ? 1 : 0);
2281                 if (ret < 0) {
2282                         dev_err(dev, "Failed to register video node: %d\n",
2283                                 ret);
2284                         goto error_reg_video;
2285                 }
2286
2287                 ret = media_create_pad_link(
2288                                 &sd->entity, MSM_VFE_PAD_SRC,
2289                                 &video_out->vdev.entity, 0,
2290                                 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
2291                 if (ret < 0) {
2292                         dev_err(dev, "Failed to link %s->%s entities: %d\n",
2293                                 sd->entity.name, video_out->vdev.entity.name,
2294                                 ret);
2295                         goto error_link;
2296                 }
2297         }
2298
2299         return 0;
2300
2301 error_link:
2302         msm_video_unregister(video_out);
2303
2304 error_reg_video:
2305         v4l2_device_unregister_subdev(sd);
2306
2307 error_reg_subdev:
2308         media_entity_cleanup(&sd->entity);
2309
2310 error_init:
2311         for (i--; i >= 0; i--) {
2312                 sd = &vfe->line[i].subdev;
2313                 video_out = &vfe->line[i].video_out;
2314
2315                 msm_video_unregister(video_out);
2316                 v4l2_device_unregister_subdev(sd);
2317                 media_entity_cleanup(&sd->entity);
2318         }
2319
2320         return ret;
2321 }
2322
2323 /*
2324  * msm_vfe_unregister_entities - Unregister VFE module subdev node
2325  * @vfe: VFE device
2326  */
2327 void msm_vfe_unregister_entities(struct vfe_device *vfe)
2328 {
2329         int i;
2330
2331         mutex_destroy(&vfe->power_lock);
2332         mutex_destroy(&vfe->stream_lock);
2333
2334         for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
2335                 struct v4l2_subdev *sd = &vfe->line[i].subdev;
2336                 struct camss_video *video_out = &vfe->line[i].video_out;
2337
2338                 msm_video_unregister(video_out);
2339                 v4l2_device_unregister_subdev(sd);
2340                 media_entity_cleanup(&sd->entity);
2341         }
2342 }