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 /* VFE reset timeout */
  30 #define VFE_RESET_TIMEOUT_MS 50
  31
  32 #define SCALER_RATIO_MAX 16
  33
  34 struct vfe_format {
  35         u32 code;
  36         u8 bpp;
  37 };
  38
  39 static const struct vfe_format formats_rdi_8x16[] = {
  40         { MEDIA_BUS_FMT_UYVY8_1X16, 8 },
  41         { MEDIA_BUS_FMT_VYUY8_1X16, 8 },
  42         { MEDIA_BUS_FMT_YUYV8_1X16, 8 },
  43         { MEDIA_BUS_FMT_YVYU8_1X16, 8 },
  44         { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
  45         { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
  46         { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
  47         { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
  48         { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
  49         { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
  50         { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
  51         { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
  52         { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
  53         { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
  54         { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
  55         { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
  56         { MEDIA_BUS_FMT_Y10_1X10, 10 },
  57 };
  58
  59 static const struct vfe_format formats_pix_8x16[] = {
  60         { MEDIA_BUS_FMT_UYVY8_1X16, 8 },
  61         { MEDIA_BUS_FMT_VYUY8_1X16, 8 },
  62         { MEDIA_BUS_FMT_YUYV8_1X16, 8 },
  63         { MEDIA_BUS_FMT_YVYU8_1X16, 8 },
  64 };
  65
  66 static const struct vfe_format formats_rdi_8x96[] = {
  67         { MEDIA_BUS_FMT_UYVY8_1X16, 8 },
  68         { MEDIA_BUS_FMT_VYUY8_1X16, 8 },
  69         { MEDIA_BUS_FMT_YUYV8_1X16, 8 },
  70         { MEDIA_BUS_FMT_YVYU8_1X16, 8 },
  71         { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
  72         { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
  73         { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
  74         { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
  75         { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
  76         { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
  77         { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
  78         { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
  79         { MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 16 },
  80         { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
  81         { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
  82         { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
  83         { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
  84         { MEDIA_BUS_FMT_SBGGR14_1X14, 14 },
  85         { MEDIA_BUS_FMT_SGBRG14_1X14, 14 },
  86         { MEDIA_BUS_FMT_SGRBG14_1X14, 14 },
  87         { MEDIA_BUS_FMT_SRGGB14_1X14, 14 },
  88         { MEDIA_BUS_FMT_Y10_1X10, 10 },
  89         { MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 16 },
  90 };
  91
  92 static const struct vfe_format formats_pix_8x96[] = {
  93         { MEDIA_BUS_FMT_UYVY8_1X16, 8 },
  94         { MEDIA_BUS_FMT_VYUY8_1X16, 8 },
  95         { MEDIA_BUS_FMT_YUYV8_1X16, 8 },
  96         { MEDIA_BUS_FMT_YVYU8_1X16, 8 },
  97 };
  98
  99 static const struct vfe_format formats_rdi_845[] = {
 100         { MEDIA_BUS_FMT_UYVY8_1X16, 8 },
 101         { MEDIA_BUS_FMT_VYUY8_1X16, 8 },
 102         { MEDIA_BUS_FMT_YUYV8_1X16, 8 },
 103         { MEDIA_BUS_FMT_YVYU8_1X16, 8 },
 104         { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
 105         { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
 106         { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
 107         { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
 108         { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
 109         { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
 110         { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
 111         { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
 112         { MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 16 },
 113         { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
 114         { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
 115         { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
 116         { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
 117         { MEDIA_BUS_FMT_SBGGR14_1X14, 14 },
 118         { MEDIA_BUS_FMT_SGBRG14_1X14, 14 },
 119         { MEDIA_BUS_FMT_SGRBG14_1X14, 14 },
 120         { MEDIA_BUS_FMT_SRGGB14_1X14, 14 },
 121         { MEDIA_BUS_FMT_Y8_1X8, 8 },
 122         { MEDIA_BUS_FMT_Y10_1X10, 10 },
 123         { MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 16 },
 124 };
 125
 126 /*
 127  * vfe_get_bpp - map media bus format to bits per pixel
 128  * @formats: supported media bus formats array
 129  * @nformats: size of @formats array
 130  * @code: media bus format code
 131  *
 132  * Return number of bits per pixel
 133  */
 134 static u8 vfe_get_bpp(const struct vfe_format *formats,
 135                       unsigned int nformats, u32 code)
 136 {
 137         unsigned int i;
 138
 139         for (i = 0; i < nformats; i++)
 140                 if (code == formats[i].code)
 141                         return formats[i].bpp;
 142
 143         WARN(1, "Unknown format\n");
 144
 145         return formats[0].bpp;
 146 }
 147
 148 static u32 vfe_find_code(u32 *code, unsigned int n_code,
 149                          unsigned int index, u32 req_code)
 150 {
 151         int i;
 152
 153         if (!req_code && (index >= n_code))
 154                 return 0;
 155
 156         for (i = 0; i < n_code; i++)
 157                 if (req_code) {
 158                         if (req_code == code[i])
 159                                 return req_code;
 160                 } else {
 161                         if (i == index)
 162                                 return code[i];
 163                 }
 164
 165         return code[0];
 166 }
 167
 168 static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 169                             unsigned int index, u32 src_req_code)
 170 {
 171         struct vfe_device *vfe = to_vfe(line);
 172
 173         switch (vfe->camss->res->version) {
 174         case CAMSS_8x16:
 175                 switch (sink_code) {
 176                 case MEDIA_BUS_FMT_YUYV8_1X16:
 177                 {
 178                         u32 src_code[] = {
 179                                 MEDIA_BUS_FMT_YUYV8_1X16,
 180                                 MEDIA_BUS_FMT_YUYV8_1_5X8,
 181                         };
 182
 183                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 184                                              index, src_req_code);
 185                 }
 186                 case MEDIA_BUS_FMT_YVYU8_1X16:
 187                 {
 188                         u32 src_code[] = {
 189                                 MEDIA_BUS_FMT_YVYU8_1X16,
 190                                 MEDIA_BUS_FMT_YVYU8_1_5X8,
 191                         };
 192
 193                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 194                                              index, src_req_code);
 195                 }
 196                 case MEDIA_BUS_FMT_UYVY8_1X16:
 197                 {
 198                         u32 src_code[] = {
 199                                 MEDIA_BUS_FMT_UYVY8_1X16,
 200                                 MEDIA_BUS_FMT_UYVY8_1_5X8,
 201                         };
 202
 203                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 204                                              index, src_req_code);
 205                 }
 206                 case MEDIA_BUS_FMT_VYUY8_1X16:
 207                 {
 208                         u32 src_code[] = {
 209                                 MEDIA_BUS_FMT_VYUY8_1X16,
 210                                 MEDIA_BUS_FMT_VYUY8_1_5X8,
 211                         };
 212
 213                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 214                                              index, src_req_code);
 215                 }
 216                 default:
 217                         if (index > 0)
 218                                 return 0;
 219
 220                         return sink_code;
 221                 }
 222                 break;
 223         case CAMSS_8x96:
 224         case CAMSS_660:
 225         case CAMSS_845:
 226         case CAMSS_8250:
 227                 switch (sink_code) {
 228                 case MEDIA_BUS_FMT_YUYV8_1X16:
 229                 {
 230                         u32 src_code[] = {
 231                                 MEDIA_BUS_FMT_YUYV8_1X16,
 232                                 MEDIA_BUS_FMT_YVYU8_1X16,
 233                                 MEDIA_BUS_FMT_UYVY8_1X16,
 234                                 MEDIA_BUS_FMT_VYUY8_1X16,
 235                                 MEDIA_BUS_FMT_YUYV8_1_5X8,
 236                         };
 237
 238                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 239                                              index, src_req_code);
 240                 }
 241                 case MEDIA_BUS_FMT_YVYU8_1X16:
 242                 {
 243                         u32 src_code[] = {
 244                                 MEDIA_BUS_FMT_YVYU8_1X16,
 245                                 MEDIA_BUS_FMT_YUYV8_1X16,
 246                                 MEDIA_BUS_FMT_UYVY8_1X16,
 247                                 MEDIA_BUS_FMT_VYUY8_1X16,
 248                                 MEDIA_BUS_FMT_YVYU8_1_5X8,
 249                         };
 250
 251                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 252                                              index, src_req_code);
 253                 }
 254                 case MEDIA_BUS_FMT_UYVY8_1X16:
 255                 {
 256                         u32 src_code[] = {
 257                                 MEDIA_BUS_FMT_UYVY8_1X16,
 258                                 MEDIA_BUS_FMT_YUYV8_1X16,
 259                                 MEDIA_BUS_FMT_YVYU8_1X16,
 260                                 MEDIA_BUS_FMT_VYUY8_1X16,
 261                                 MEDIA_BUS_FMT_UYVY8_1_5X8,
 262                         };
 263
 264                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 265                                              index, src_req_code);
 266                 }
 267                 case MEDIA_BUS_FMT_VYUY8_1X16:
 268                 {
 269                         u32 src_code[] = {
 270                                 MEDIA_BUS_FMT_VYUY8_1X16,
 271                                 MEDIA_BUS_FMT_YUYV8_1X16,
 272                                 MEDIA_BUS_FMT_YVYU8_1X16,
 273                                 MEDIA_BUS_FMT_UYVY8_1X16,
 274                                 MEDIA_BUS_FMT_VYUY8_1_5X8,
 275                         };
 276
 277                         return vfe_find_code(src_code, ARRAY_SIZE(src_code),
 278                                              index, src_req_code);
 279                 }
 280                 default:
 281                         if (index > 0)
 282                                 return 0;
 283
 284                         return sink_code;
 285                 }
 286                 break;
 287         }
 288         return 0;
 289 }
 290
 291 int vfe_reset(struct vfe_device *vfe)
 292 {
 293         unsigned long time;
 294
 295         reinit_completion(&vfe->reset_complete);
 296
 297         vfe->ops->global_reset(vfe);
 298
 299         time = wait_for_completion_timeout(&vfe->reset_complete,
 300                 msecs_to_jiffies(VFE_RESET_TIMEOUT_MS));
 301         if (!time) {
 302                 dev_err(vfe->camss->dev, "VFE reset timeout\n");
 303                 return -EIO;
 304         }
 305
 306         return 0;
 307 }
 308
 309 static void vfe_init_outputs(struct vfe_device *vfe)
 310 {
 311         int i;
 312
 313         for (i = 0; i < vfe->line_num; i++) {
 314                 struct vfe_output *output = &vfe->line[i].output;
 315
 316                 output->state = VFE_OUTPUT_OFF;
 317                 output->buf[0] = NULL;
 318                 output->buf[1] = NULL;
 319                 INIT_LIST_HEAD(&output->pending_bufs);
 320         }
 321 }
 322
 323 static void vfe_reset_output_maps(struct vfe_device *vfe)
 324 {
 325         int i;
 326
 327         for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++)
 328                 vfe->wm_output_map[i] = VFE_LINE_NONE;
 329 }
 330
 331 int vfe_reserve_wm(struct vfe_device *vfe, enum vfe_line_id line_id)
 332 {
 333         int ret = -EBUSY;
 334         int i;
 335
 336         for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++) {
 337                 if (vfe->wm_output_map[i] == VFE_LINE_NONE) {
 338                         vfe->wm_output_map[i] = line_id;
 339                         ret = i;
 340                         break;
 341                 }
 342         }
 343
 344         return ret;
 345 }
 346
 347 int vfe_release_wm(struct vfe_device *vfe, u8 wm)
 348 {
 349         if (wm >= ARRAY_SIZE(vfe->wm_output_map))
 350                 return -EINVAL;
 351
 352         vfe->wm_output_map[wm] = VFE_LINE_NONE;
 353
 354         return 0;
 355 }
 356
 357 struct camss_buffer *vfe_buf_get_pending(struct vfe_output *output)
 358 {
 359         struct camss_buffer *buffer = NULL;
 360
 361         if (!list_empty(&output->pending_bufs)) {
 362                 buffer = list_first_entry(&output->pending_bufs,
 363                                           struct camss_buffer,
 364                                           queue);
 365                 list_del(&buffer->queue);
 366         }
 367
 368         return buffer;
 369 }
 370
 371 void vfe_buf_add_pending(struct vfe_output *output,
 372                          struct camss_buffer *buffer)
 373 {
 374         INIT_LIST_HEAD(&buffer->queue);
 375         list_add_tail(&buffer->queue, &output->pending_bufs);
 376 }
 377
 378 /*
 379  * vfe_buf_flush_pending - Flush all pending buffers.
 380  * @output: VFE output
 381  * @state: vb2 buffer state
 382  */
 383 static void vfe_buf_flush_pending(struct vfe_output *output,
 384                                   enum vb2_buffer_state state)
 385 {
 386         struct camss_buffer *buf;
 387         struct camss_buffer *t;
 388
 389         list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
 390                 vb2_buffer_done(&buf->vb.vb2_buf, state);
 391                 list_del(&buf->queue);
 392         }
 393 }
 394
 395 int vfe_put_output(struct vfe_line *line)
 396 {
 397         struct vfe_device *vfe = to_vfe(line);
 398         struct vfe_output *output = &line->output;
 399         unsigned long flags;
 400         unsigned int i;
 401
 402         spin_lock_irqsave(&vfe->output_lock, flags);
 403
 404         for (i = 0; i < output->wm_num; i++)
 405                 vfe_release_wm(vfe, output->wm_idx[i]);
 406
 407         output->state = VFE_OUTPUT_OFF;
 408
 409         spin_unlock_irqrestore(&vfe->output_lock, flags);
 410         return 0;
 411 }
 412
 413 static int vfe_disable_output(struct vfe_line *line)
 414 {
 415         struct vfe_device *vfe = to_vfe(line);
 416         struct vfe_output *output = &line->output;
 417         unsigned long flags;
 418         unsigned int i;
 419
 420         spin_lock_irqsave(&vfe->output_lock, flags);
 421         for (i = 0; i < output->wm_num; i++)
 422                 vfe->ops->vfe_wm_stop(vfe, output->wm_idx[i]);
 423         output->gen2.active_num = 0;
 424         spin_unlock_irqrestore(&vfe->output_lock, flags);
 425
 426         return vfe_reset(vfe);
 427 }
 428
 429 /*
 430  * vfe_disable - Disable streaming on VFE line
 431  * @line: VFE line
 432  *
 433  * Return 0 on success or a negative error code otherwise
 434  */
 435 int vfe_disable(struct vfe_line *line)
 436 {
 437         struct vfe_device *vfe = to_vfe(line);
 438         int ret;
 439
 440         ret = vfe_disable_output(line);
 441         if (ret)
 442                 goto error;
 443
 444         vfe_put_output(line);
 445
 446         mutex_lock(&vfe->stream_lock);
 447
 448         vfe->stream_count--;
 449
 450         mutex_unlock(&vfe->stream_lock);
 451
 452 error:
 453         return ret;
 454 }
 455
 456 /**
 457  * vfe_isr_comp_done() - Process composite image done interrupt
 458  * @vfe: VFE Device
 459  * @comp: Composite image id
 460  */
 461 void vfe_isr_comp_done(struct vfe_device *vfe, u8 comp)
 462 {
 463         unsigned int i;
 464
 465         for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++)
 466                 if (vfe->wm_output_map[i] == VFE_LINE_PIX) {
 467                         vfe->isr_ops.wm_done(vfe, i);
 468                         break;
 469                 }
 470 }
 471
 472 void vfe_isr_reset_ack(struct vfe_device *vfe)
 473 {
 474         complete(&vfe->reset_complete);
 475 }
 476
 477 static int vfe_match_clock_names(struct vfe_device *vfe,
 478                                  struct camss_clock *clock)
 479 {
 480         char vfe_name[7]; /* vfeXXX\0 */
 481         char vfe_lite_name[12]; /* vfe_liteXXX\0 */
 482
 483         snprintf(vfe_name, sizeof(vfe_name), "vfe%d", vfe->id);
 484         snprintf(vfe_lite_name, sizeof(vfe_lite_name), "vfe_lite%d", vfe->id);
 485
 486         return (!strcmp(clock->name, vfe_name) ||
 487                 !strcmp(clock->name, vfe_lite_name) ||
 488                 !strcmp(clock->name, "vfe_lite"));
 489 }
 490
 491 /*
 492  * vfe_set_clock_rates - Calculate and set clock rates on VFE module
 493  * @vfe: VFE device
 494  *
 495  * Return 0 on success or a negative error code otherwise
 496  */
 497 static int vfe_set_clock_rates(struct vfe_device *vfe)
 498 {
 499         struct device *dev = vfe->camss->dev;
 500         u64 pixel_clock[VFE_LINE_NUM_MAX];
 501         int i, j;
 502         int ret;
 503
 504         for (i = VFE_LINE_RDI0; i < vfe->line_num; i++) {
 505                 ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
 506                                             &pixel_clock[i]);
 507                 if (ret)
 508                         pixel_clock[i] = 0;
 509         }
 510
 511         for (i = 0; i < vfe->nclocks; i++) {
 512                 struct camss_clock *clock = &vfe->clock[i];
 513
 514                 if (vfe_match_clock_names(vfe, clock)) {
 515                         u64 min_rate = 0;
 516                         long rate;
 517
 518                         for (j = VFE_LINE_RDI0; j < vfe->line_num; j++) {
 519                                 u32 tmp;
 520                                 u8 bpp;
 521
 522                                 if (j == VFE_LINE_PIX) {
 523                                         tmp = pixel_clock[j];
 524                                 } else {
 525                                         struct vfe_line *l = &vfe->line[j];
 526
 527                                         bpp = vfe_get_bpp(l->formats,
 528                                                 l->nformats,
 529                                                 l->fmt[MSM_VFE_PAD_SINK].code);
 530                                         tmp = pixel_clock[j] * bpp / 64;
 531                                 }
 532
 533                                 if (min_rate < tmp)
 534                                         min_rate = tmp;
 535                         }
 536
 537                         camss_add_clock_margin(&min_rate);
 538
 539                         for (j = 0; j < clock->nfreqs; j++)
 540                                 if (min_rate < clock->freq[j])
 541                                         break;
 542
 543                         if (j == clock->nfreqs) {
 544                                 dev_err(dev,
 545                                         "Pixel clock is too high for VFE");
 546                                 return -EINVAL;
 547                         }
 548
 549                         /* if sensor pixel clock is not available */
 550                         /* set highest possible VFE clock rate */
 551                         if (min_rate == 0)
 552                                 j = clock->nfreqs - 1;
 553
 554                         rate = clk_round_rate(clock->clk, clock->freq[j]);
 555                         if (rate < 0) {
 556                                 dev_err(dev, "clk round rate failed: %ld\n",
 557                                         rate);
 558                                 return -EINVAL;
 559                         }
 560
 561                         ret = clk_set_rate(clock->clk, rate);
 562                         if (ret < 0) {
 563                                 dev_err(dev, "clk set rate failed: %d\n", ret);
 564                                 return ret;
 565                         }
 566                 }
 567         }
 568
 569         return 0;
 570 }
 571
 572 /*
 573  * vfe_check_clock_rates - Check current clock rates on VFE module
 574  * @vfe: VFE device
 575  *
 576  * Return 0 if current clock rates are suitable for a new pipeline
 577  * or a negative error code otherwise
 578  */
 579 static int vfe_check_clock_rates(struct vfe_device *vfe)
 580 {
 581         u64 pixel_clock[VFE_LINE_NUM_MAX];
 582         int i, j;
 583         int ret;
 584
 585         for (i = VFE_LINE_RDI0; i < vfe->line_num; i++) {
 586                 ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
 587                                             &pixel_clock[i]);
 588                 if (ret)
 589                         pixel_clock[i] = 0;
 590         }
 591
 592         for (i = 0; i < vfe->nclocks; i++) {
 593                 struct camss_clock *clock = &vfe->clock[i];
 594
 595                 if (vfe_match_clock_names(vfe, clock)) {
 596                         u64 min_rate = 0;
 597                         unsigned long rate;
 598
 599                         for (j = VFE_LINE_RDI0; j < vfe->line_num; j++) {
 600                                 u32 tmp;
 601                                 u8 bpp;
 602
 603                                 if (j == VFE_LINE_PIX) {
 604                                         tmp = pixel_clock[j];
 605                                 } else {
 606                                         struct vfe_line *l = &vfe->line[j];
 607
 608                                         bpp = vfe_get_bpp(l->formats,
 609                                                 l->nformats,
 610                                                 l->fmt[MSM_VFE_PAD_SINK].code);
 611                                         tmp = pixel_clock[j] * bpp / 64;
 612                                 }
 613
 614                                 if (min_rate < tmp)
 615                                         min_rate = tmp;
 616                         }
 617
 618                         camss_add_clock_margin(&min_rate);
 619
 620                         rate = clk_get_rate(clock->clk);
 621                         if (rate < min_rate)
 622                                 return -EBUSY;
 623                 }
 624         }
 625
 626         return 0;
 627 }
 628
 629 /*
 630  * vfe_get - Power up and reset VFE module
 631  * @vfe: VFE Device
 632  *
 633  * Return 0 on success or a negative error code otherwise
 634  */
 635 int vfe_get(struct vfe_device *vfe)
 636 {
 637         int ret;
 638
 639         mutex_lock(&vfe->power_lock);
 640
 641         if (vfe->power_count == 0) {
 642                 ret = vfe->ops->pm_domain_on(vfe);
 643                 if (ret < 0)
 644                         goto error_pm_domain;
 645
 646                 ret = pm_runtime_resume_and_get(vfe->camss->dev);
 647                 if (ret < 0)
 648                         goto error_domain_off;
 649
 650                 ret = vfe_set_clock_rates(vfe);
 651                 if (ret < 0)
 652                         goto error_pm_runtime_get;
 653
 654                 ret = camss_enable_clocks(vfe->nclocks, vfe->clock,
 655                                           vfe->camss->dev);
 656                 if (ret < 0)
 657                         goto error_pm_runtime_get;
 658
 659                 ret = vfe_reset(vfe);
 660                 if (ret < 0)
 661                         goto error_reset;
 662
 663                 vfe_reset_output_maps(vfe);
 664
 665                 vfe_init_outputs(vfe);
 666
 667                 vfe->ops->hw_version(vfe);
 668         } else {
 669                 ret = vfe_check_clock_rates(vfe);
 670                 if (ret < 0)
 671                         goto error_pm_domain;
 672         }
 673         vfe->power_count++;
 674
 675         mutex_unlock(&vfe->power_lock);
 676
 677         return 0;
 678
 679 error_reset:
 680         camss_disable_clocks(vfe->nclocks, vfe->clock);
 681
 682 error_pm_runtime_get:
 683         pm_runtime_put_sync(vfe->camss->dev);
 684 error_domain_off:
 685         vfe->ops->pm_domain_off(vfe);
 686
 687 error_pm_domain:
 688         mutex_unlock(&vfe->power_lock);
 689
 690         return ret;
 691 }
 692
 693 /*
 694  * vfe_put - Power down VFE module
 695  * @vfe: VFE Device
 696  */
 697 void vfe_put(struct vfe_device *vfe)
 698 {
 699         mutex_lock(&vfe->power_lock);
 700
 701         if (vfe->power_count == 0) {
 702                 dev_err(vfe->camss->dev, "vfe power off on power_count == 0\n");
 703                 goto exit;
 704         } else if (vfe->power_count == 1) {
 705                 if (vfe->was_streaming) {
 706                         vfe->was_streaming = 0;
 707                         vfe->ops->vfe_halt(vfe);
 708                 }
 709                 camss_disable_clocks(vfe->nclocks, vfe->clock);
 710                 pm_runtime_put_sync(vfe->camss->dev);
 711                 vfe->ops->pm_domain_off(vfe);
 712         }
 713
 714         vfe->power_count--;
 715
 716 exit:
 717         mutex_unlock(&vfe->power_lock);
 718 }
 719
 720 /*
 721  * vfe_flush_buffers - Return all vb2 buffers
 722  * @vid: Video device structure
 723  * @state: vb2 buffer state of the returned buffers
 724  *
 725  * Return all buffers to vb2. This includes queued pending buffers (still
 726  * unused) and any buffers given to the hardware but again still not used.
 727  *
 728  * Return 0 on success or a negative error code otherwise
 729  */
 730 int vfe_flush_buffers(struct camss_video *vid,
 731                       enum vb2_buffer_state state)
 732 {
 733         struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
 734         struct vfe_device *vfe = to_vfe(line);
 735         struct vfe_output *output;
 736         unsigned long flags;
 737
 738         output = &line->output;
 739
 740         spin_lock_irqsave(&vfe->output_lock, flags);
 741
 742         vfe_buf_flush_pending(output, state);
 743
 744         if (output->buf[0])
 745                 vb2_buffer_done(&output->buf[0]->vb.vb2_buf, state);
 746
 747         if (output->buf[1])
 748                 vb2_buffer_done(&output->buf[1]->vb.vb2_buf, state);
 749
 750         if (output->last_buffer) {
 751                 vb2_buffer_done(&output->last_buffer->vb.vb2_buf, state);
 752                 output->last_buffer = NULL;
 753         }
 754
 755         spin_unlock_irqrestore(&vfe->output_lock, flags);
 756
 757         return 0;
 758 }
 759
 760 /*
 761  * vfe_set_power - Power on/off VFE module
 762  * @sd: VFE V4L2 subdevice
 763  * @on: Requested power state
 764  *
 765  * Return 0 on success or a negative error code otherwise
 766  */
 767 static int vfe_set_power(struct v4l2_subdev *sd, int on)
 768 {
 769         struct vfe_line *line = v4l2_get_subdevdata(sd);
 770         struct vfe_device *vfe = to_vfe(line);
 771         int ret;
 772
 773         if (on) {
 774                 ret = vfe_get(vfe);
 775                 if (ret < 0)
 776                         return ret;
 777         } else {
 778                 vfe_put(vfe);
 779         }
 780
 781         return 0;
 782 }
 783
 784 /*
 785  * vfe_set_stream - Enable/disable streaming on VFE module
 786  * @sd: VFE V4L2 subdevice
 787  * @enable: Requested streaming state
 788  *
 789  * Main configuration of VFE module is triggered here.
 790  *
 791  * Return 0 on success or a negative error code otherwise
 792  */
 793 static int vfe_set_stream(struct v4l2_subdev *sd, int enable)
 794 {
 795         struct vfe_line *line = v4l2_get_subdevdata(sd);
 796         struct vfe_device *vfe = to_vfe(line);
 797         int ret;
 798
 799         if (enable) {
 800                 line->output.state = VFE_OUTPUT_RESERVED;
 801                 ret = vfe->ops->vfe_enable(line);
 802                 if (ret < 0)
 803                         dev_err(vfe->camss->dev,
 804                                 "Failed to enable vfe outputs\n");
 805         } else {
 806                 ret = vfe->ops->vfe_disable(line);
 807                 if (ret < 0)
 808                         dev_err(vfe->camss->dev,
 809                                 "Failed to disable vfe outputs\n");
 810         }
 811
 812         return ret;
 813 }
 814
 815 /*
 816  * __vfe_get_format - Get pointer to format structure
 817  * @line: VFE line
 818  * @cfg: V4L2 subdev pad configuration
 819  * @pad: pad from which format is requested
 820  * @which: TRY or ACTIVE format
 821  *
 822  * Return pointer to TRY or ACTIVE format structure
 823  */
 824 static struct v4l2_mbus_framefmt *
 825 __vfe_get_format(struct vfe_line *line,
 826                  struct v4l2_subdev_state *sd_state,
 827                  unsigned int pad,
 828                  enum v4l2_subdev_format_whence which)
 829 {
 830         if (which == V4L2_SUBDEV_FORMAT_TRY)
 831                 return v4l2_subdev_get_try_format(&line->subdev, sd_state,
 832                                                   pad);
 833
 834         return &line->fmt[pad];
 835 }
 836
 837 /*
 838  * __vfe_get_compose - Get pointer to compose selection structure
 839  * @line: VFE line
 840  * @cfg: V4L2 subdev pad configuration
 841  * @which: TRY or ACTIVE format
 842  *
 843  * Return pointer to TRY or ACTIVE compose rectangle structure
 844  */
 845 static struct v4l2_rect *
 846 __vfe_get_compose(struct vfe_line *line,
 847                   struct v4l2_subdev_state *sd_state,
 848                   enum v4l2_subdev_format_whence which)
 849 {
 850         if (which == V4L2_SUBDEV_FORMAT_TRY)
 851                 return v4l2_subdev_get_try_compose(&line->subdev, sd_state,
 852                                                    MSM_VFE_PAD_SINK);
 853
 854         return &line->compose;
 855 }
 856
 857 /*
 858  * __vfe_get_crop - Get pointer to crop selection structure
 859  * @line: VFE line
 860  * @cfg: V4L2 subdev pad configuration
 861  * @which: TRY or ACTIVE format
 862  *
 863  * Return pointer to TRY or ACTIVE crop rectangle structure
 864  */
 865 static struct v4l2_rect *
 866 __vfe_get_crop(struct vfe_line *line,
 867                struct v4l2_subdev_state *sd_state,
 868                enum v4l2_subdev_format_whence which)
 869 {
 870         if (which == V4L2_SUBDEV_FORMAT_TRY)
 871                 return v4l2_subdev_get_try_crop(&line->subdev, sd_state,
 872                                                 MSM_VFE_PAD_SRC);
 873
 874         return &line->crop;
 875 }
 876
 877 /*
 878  * vfe_try_format - Handle try format by pad subdev method
 879  * @line: VFE line
 880  * @cfg: V4L2 subdev pad configuration
 881  * @pad: pad on which format is requested
 882  * @fmt: pointer to v4l2 format structure
 883  * @which: wanted subdev format
 884  */
 885 static void vfe_try_format(struct vfe_line *line,
 886                            struct v4l2_subdev_state *sd_state,
 887                            unsigned int pad,
 888                            struct v4l2_mbus_framefmt *fmt,
 889                            enum v4l2_subdev_format_whence which)
 890 {
 891         unsigned int i;
 892         u32 code;
 893
 894         switch (pad) {
 895         case MSM_VFE_PAD_SINK:
 896                 /* Set format on sink pad */
 897
 898                 for (i = 0; i < line->nformats; i++)
 899                         if (fmt->code == line->formats[i].code)
 900                                 break;
 901
 902                 /* If not found, use UYVY as default */
 903                 if (i >= line->nformats)
 904                         fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
 905
 906                 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
 907                 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 908
 909                 fmt->field = V4L2_FIELD_NONE;
 910                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
 911
 912                 break;
 913
 914         case MSM_VFE_PAD_SRC:
 915                 /* Set and return a format same as sink pad */
 916                 code = fmt->code;
 917
 918                 *fmt = *__vfe_get_format(line, sd_state, MSM_VFE_PAD_SINK,
 919                                          which);
 920
 921                 fmt->code = vfe_src_pad_code(line, fmt->code, 0, code);
 922
 923                 if (line->id == VFE_LINE_PIX) {
 924                         struct v4l2_rect *rect;
 925
 926                         rect = __vfe_get_crop(line, sd_state, which);
 927
 928                         fmt->width = rect->width;
 929                         fmt->height = rect->height;
 930                 }
 931
 932                 break;
 933         }
 934
 935         fmt->colorspace = V4L2_COLORSPACE_SRGB;
 936 }
 937
 938 /*
 939  * vfe_try_compose - Handle try compose selection by pad subdev method
 940  * @line: VFE line
 941  * @cfg: V4L2 subdev pad configuration
 942  * @rect: pointer to v4l2 rect structure
 943  * @which: wanted subdev format
 944  */
 945 static void vfe_try_compose(struct vfe_line *line,
 946                             struct v4l2_subdev_state *sd_state,
 947                             struct v4l2_rect *rect,
 948                             enum v4l2_subdev_format_whence which)
 949 {
 950         struct v4l2_mbus_framefmt *fmt;
 951
 952         fmt = __vfe_get_format(line, sd_state, MSM_VFE_PAD_SINK, which);
 953
 954         if (rect->width > fmt->width)
 955                 rect->width = fmt->width;
 956
 957         if (rect->height > fmt->height)
 958                 rect->height = fmt->height;
 959
 960         if (fmt->width > rect->width * SCALER_RATIO_MAX)
 961                 rect->width = (fmt->width + SCALER_RATIO_MAX - 1) /
 962                                                         SCALER_RATIO_MAX;
 963
 964         rect->width &= ~0x1;
 965
 966         if (fmt->height > rect->height * SCALER_RATIO_MAX)
 967                 rect->height = (fmt->height + SCALER_RATIO_MAX - 1) /
 968                                                         SCALER_RATIO_MAX;
 969
 970         if (rect->width < 16)
 971                 rect->width = 16;
 972
 973         if (rect->height < 4)
 974                 rect->height = 4;
 975 }
 976
 977 /*
 978  * vfe_try_crop - Handle try crop selection by pad subdev method
 979  * @line: VFE line
 980  * @cfg: V4L2 subdev pad configuration
 981  * @rect: pointer to v4l2 rect structure
 982  * @which: wanted subdev format
 983  */
 984 static void vfe_try_crop(struct vfe_line *line,
 985                          struct v4l2_subdev_state *sd_state,
 986                          struct v4l2_rect *rect,
 987                          enum v4l2_subdev_format_whence which)
 988 {
 989         struct v4l2_rect *compose;
 990
 991         compose = __vfe_get_compose(line, sd_state, which);
 992
 993         if (rect->width > compose->width)
 994                 rect->width = compose->width;
 995
 996         if (rect->width + rect->left > compose->width)
 997                 rect->left = compose->width - rect->width;
 998
 999         if (rect->height > compose->height)
1000                 rect->height = compose->height;
1001
1002         if (rect->height + rect->top > compose->height)
1003                 rect->top = compose->height - rect->height;
1004
1005         /* wm in line based mode writes multiple of 16 horizontally */
1006         rect->left += (rect->width & 0xf) >> 1;
1007         rect->width &= ~0xf;
1008
1009         if (rect->width < 16) {
1010                 rect->left = 0;
1011                 rect->width = 16;
1012         }
1013
1014         if (rect->height < 4) {
1015                 rect->top = 0;
1016                 rect->height = 4;
1017         }
1018 }
1019
1020 /*
1021  * vfe_enum_mbus_code - Handle pixel format enumeration
1022  * @sd: VFE V4L2 subdevice
1023  * @cfg: V4L2 subdev pad configuration
1024  * @code: pointer to v4l2_subdev_mbus_code_enum structure
1025  *
1026  * return -EINVAL or zero on success
1027  */
1028 static int vfe_enum_mbus_code(struct v4l2_subdev *sd,
1029                               struct v4l2_subdev_state *sd_state,
1030                               struct v4l2_subdev_mbus_code_enum *code)
1031 {
1032         struct vfe_line *line = v4l2_get_subdevdata(sd);
1033
1034         if (code->pad == MSM_VFE_PAD_SINK) {
1035                 if (code->index >= line->nformats)
1036                         return -EINVAL;
1037
1038                 code->code = line->formats[code->index].code;
1039         } else {
1040                 struct v4l2_mbus_framefmt *sink_fmt;
1041
1042                 sink_fmt = __vfe_get_format(line, sd_state, MSM_VFE_PAD_SINK,
1043                                             code->which);
1044
1045                 code->code = vfe_src_pad_code(line, sink_fmt->code,
1046                                               code->index, 0);
1047                 if (!code->code)
1048                         return -EINVAL;
1049         }
1050
1051         return 0;
1052 }
1053
1054 /*
1055  * vfe_enum_frame_size - Handle frame size enumeration
1056  * @sd: VFE V4L2 subdevice
1057  * @cfg: V4L2 subdev pad configuration
1058  * @fse: pointer to v4l2_subdev_frame_size_enum structure
1059  *
1060  * Return -EINVAL or zero on success
1061  */
1062 static int vfe_enum_frame_size(struct v4l2_subdev *sd,
1063                                struct v4l2_subdev_state *sd_state,
1064                                struct v4l2_subdev_frame_size_enum *fse)
1065 {
1066         struct vfe_line *line = v4l2_get_subdevdata(sd);
1067         struct v4l2_mbus_framefmt format;
1068
1069         if (fse->index != 0)
1070                 return -EINVAL;
1071
1072         format.code = fse->code;
1073         format.width = 1;
1074         format.height = 1;
1075         vfe_try_format(line, sd_state, fse->pad, &format, fse->which);
1076         fse->min_width = format.width;
1077         fse->min_height = format.height;
1078
1079         if (format.code != fse->code)
1080                 return -EINVAL;
1081
1082         format.code = fse->code;
1083         format.width = -1;
1084         format.height = -1;
1085         vfe_try_format(line, sd_state, fse->pad, &format, fse->which);
1086         fse->max_width = format.width;
1087         fse->max_height = format.height;
1088
1089         return 0;
1090 }
1091
1092 /*
1093  * vfe_get_format - Handle get format by pads subdev method
1094  * @sd: VFE V4L2 subdevice
1095  * @cfg: V4L2 subdev pad configuration
1096  * @fmt: pointer to v4l2 subdev format structure
1097  *
1098  * Return -EINVAL or zero on success
1099  */
1100 static int vfe_get_format(struct v4l2_subdev *sd,
1101                           struct v4l2_subdev_state *sd_state,
1102                           struct v4l2_subdev_format *fmt)
1103 {
1104         struct vfe_line *line = v4l2_get_subdevdata(sd);
1105         struct v4l2_mbus_framefmt *format;
1106
1107         format = __vfe_get_format(line, sd_state, fmt->pad, fmt->which);
1108         if (format == NULL)
1109                 return -EINVAL;
1110
1111         fmt->format = *format;
1112
1113         return 0;
1114 }
1115
1116 static int vfe_set_selection(struct v4l2_subdev *sd,
1117                              struct v4l2_subdev_state *sd_state,
1118                              struct v4l2_subdev_selection *sel);
1119
1120 /*
1121  * vfe_set_format - Handle set format by pads subdev method
1122  * @sd: VFE V4L2 subdevice
1123  * @cfg: V4L2 subdev pad configuration
1124  * @fmt: pointer to v4l2 subdev format structure
1125  *
1126  * Return -EINVAL or zero on success
1127  */
1128 static int vfe_set_format(struct v4l2_subdev *sd,
1129                           struct v4l2_subdev_state *sd_state,
1130                           struct v4l2_subdev_format *fmt)
1131 {
1132         struct vfe_line *line = v4l2_get_subdevdata(sd);
1133         struct v4l2_mbus_framefmt *format;
1134
1135         format = __vfe_get_format(line, sd_state, fmt->pad, fmt->which);
1136         if (format == NULL)
1137                 return -EINVAL;
1138
1139         vfe_try_format(line, sd_state, fmt->pad, &fmt->format, fmt->which);
1140         *format = fmt->format;
1141
1142         if (fmt->pad == MSM_VFE_PAD_SINK) {
1143                 struct v4l2_subdev_selection sel = { 0 };
1144                 int ret;
1145
1146                 /* Propagate the format from sink to source */
1147                 format = __vfe_get_format(line, sd_state, MSM_VFE_PAD_SRC,
1148                                           fmt->which);
1149
1150                 *format = fmt->format;
1151                 vfe_try_format(line, sd_state, MSM_VFE_PAD_SRC, format,
1152                                fmt->which);
1153
1154                 if (line->id != VFE_LINE_PIX)
1155                         return 0;
1156
1157                 /* Reset sink pad compose selection */
1158                 sel.which = fmt->which;
1159                 sel.pad = MSM_VFE_PAD_SINK;
1160                 sel.target = V4L2_SEL_TGT_COMPOSE;
1161                 sel.r.width = fmt->format.width;
1162                 sel.r.height = fmt->format.height;
1163                 ret = vfe_set_selection(sd, sd_state, &sel);
1164                 if (ret < 0)
1165                         return ret;
1166         }
1167
1168         return 0;
1169 }
1170
1171 /*
1172  * vfe_get_selection - Handle get selection by pads subdev method
1173  * @sd: VFE V4L2 subdevice
1174  * @cfg: V4L2 subdev pad configuration
1175  * @sel: pointer to v4l2 subdev selection structure
1176  *
1177  * Return -EINVAL or zero on success
1178  */
1179 static int vfe_get_selection(struct v4l2_subdev *sd,
1180                              struct v4l2_subdev_state *sd_state,
1181                              struct v4l2_subdev_selection *sel)
1182 {
1183         struct vfe_line *line = v4l2_get_subdevdata(sd);
1184         struct v4l2_subdev_format fmt = { 0 };
1185         struct v4l2_rect *rect;
1186         int ret;
1187
1188         if (line->id != VFE_LINE_PIX)
1189                 return -EINVAL;
1190
1191         if (sel->pad == MSM_VFE_PAD_SINK)
1192                 switch (sel->target) {
1193                 case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1194                         fmt.pad = sel->pad;
1195                         fmt.which = sel->which;
1196                         ret = vfe_get_format(sd, sd_state, &fmt);
1197                         if (ret < 0)
1198                                 return ret;
1199
1200                         sel->r.left = 0;
1201                         sel->r.top = 0;
1202                         sel->r.width = fmt.format.width;
1203                         sel->r.height = fmt.format.height;
1204                         break;
1205                 case V4L2_SEL_TGT_COMPOSE:
1206                         rect = __vfe_get_compose(line, sd_state, sel->which);
1207                         if (rect == NULL)
1208                                 return -EINVAL;
1209
1210                         sel->r = *rect;
1211                         break;
1212                 default:
1213                         return -EINVAL;
1214                 }
1215         else if (sel->pad == MSM_VFE_PAD_SRC)
1216                 switch (sel->target) {
1217                 case V4L2_SEL_TGT_CROP_BOUNDS:
1218                         rect = __vfe_get_compose(line, sd_state, sel->which);
1219                         if (rect == NULL)
1220                                 return -EINVAL;
1221
1222                         sel->r.left = rect->left;
1223                         sel->r.top = rect->top;
1224                         sel->r.width = rect->width;
1225                         sel->r.height = rect->height;
1226                         break;
1227                 case V4L2_SEL_TGT_CROP:
1228                         rect = __vfe_get_crop(line, sd_state, sel->which);
1229                         if (rect == NULL)
1230                                 return -EINVAL;
1231
1232                         sel->r = *rect;
1233                         break;
1234                 default:
1235                         return -EINVAL;
1236                 }
1237
1238         return 0;
1239 }
1240
1241 /*
1242  * vfe_set_selection - Handle set selection by pads subdev method
1243  * @sd: VFE V4L2 subdevice
1244  * @cfg: V4L2 subdev pad configuration
1245  * @sel: pointer to v4l2 subdev selection structure
1246  *
1247  * Return -EINVAL or zero on success
1248  */
1249 static int vfe_set_selection(struct v4l2_subdev *sd,
1250                              struct v4l2_subdev_state *sd_state,
1251                              struct v4l2_subdev_selection *sel)
1252 {
1253         struct vfe_line *line = v4l2_get_subdevdata(sd);
1254         struct v4l2_rect *rect;
1255         int ret;
1256
1257         if (line->id != VFE_LINE_PIX)
1258                 return -EINVAL;
1259
1260         if (sel->target == V4L2_SEL_TGT_COMPOSE &&
1261                 sel->pad == MSM_VFE_PAD_SINK) {
1262                 struct v4l2_subdev_selection crop = { 0 };
1263
1264                 rect = __vfe_get_compose(line, sd_state, sel->which);
1265                 if (rect == NULL)
1266                         return -EINVAL;
1267
1268                 vfe_try_compose(line, sd_state, &sel->r, sel->which);
1269                 *rect = sel->r;
1270
1271                 /* Reset source crop selection */
1272                 crop.which = sel->which;
1273                 crop.pad = MSM_VFE_PAD_SRC;
1274                 crop.target = V4L2_SEL_TGT_CROP;
1275                 crop.r = *rect;
1276                 ret = vfe_set_selection(sd, sd_state, &crop);
1277         } else if (sel->target == V4L2_SEL_TGT_CROP &&
1278                 sel->pad == MSM_VFE_PAD_SRC) {
1279                 struct v4l2_subdev_format fmt = { 0 };
1280
1281                 rect = __vfe_get_crop(line, sd_state, sel->which);
1282                 if (rect == NULL)
1283                         return -EINVAL;
1284
1285                 vfe_try_crop(line, sd_state, &sel->r, sel->which);
1286                 *rect = sel->r;
1287
1288                 /* Reset source pad format width and height */
1289                 fmt.which = sel->which;
1290                 fmt.pad = MSM_VFE_PAD_SRC;
1291                 ret = vfe_get_format(sd, sd_state, &fmt);
1292                 if (ret < 0)
1293                         return ret;
1294
1295                 fmt.format.width = rect->width;
1296                 fmt.format.height = rect->height;
1297                 ret = vfe_set_format(sd, sd_state, &fmt);
1298         } else {
1299                 ret = -EINVAL;
1300         }
1301
1302         return ret;
1303 }
1304
1305 /*
1306  * vfe_init_formats - Initialize formats on all pads
1307  * @sd: VFE V4L2 subdevice
1308  * @fh: V4L2 subdev file handle
1309  *
1310  * Initialize all pad formats with default values.
1311  *
1312  * Return 0 on success or a negative error code otherwise
1313  */
1314 static int vfe_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1315 {
1316         struct v4l2_subdev_format format = {
1317                 .pad = MSM_VFE_PAD_SINK,
1318                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
1319                               V4L2_SUBDEV_FORMAT_ACTIVE,
1320                 .format = {
1321                         .code = MEDIA_BUS_FMT_UYVY8_1X16,
1322                         .width = 1920,
1323                         .height = 1080
1324                 }
1325         };
1326
1327         return vfe_set_format(sd, fh ? fh->state : NULL, &format);
1328 }
1329
1330 /*
1331  * msm_vfe_subdev_init - Initialize VFE device structure and resources
1332  * @vfe: VFE device
1333  * @res: VFE module resources table
1334  *
1335  * Return 0 on success or a negative error code otherwise
1336  */
1337 int msm_vfe_subdev_init(struct camss *camss, struct vfe_device *vfe,
1338                         const struct camss_subdev_resources *res, u8 id)
1339 {
1340         struct device *dev = camss->dev;
1341         struct platform_device *pdev = to_platform_device(dev);
1342         int i, j;
1343         int ret;
1344
1345         vfe->ops = res->ops;
1346
1347         if (!res->line_num)
1348                 return -EINVAL;
1349
1350         vfe->line_num = res->line_num;
1351         vfe->ops->subdev_init(dev, vfe);
1352
1353         /* Memory */
1354
1355         vfe->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
1356         if (IS_ERR(vfe->base)) {
1357                 dev_err(dev, "could not map memory\n");
1358                 return PTR_ERR(vfe->base);
1359         }
1360
1361         /* Interrupt */
1362
1363         ret = platform_get_irq_byname(pdev, res->interrupt[0]);
1364         if (ret < 0)
1365                 return ret;
1366
1367         vfe->irq = ret;
1368         snprintf(vfe->irq_name, sizeof(vfe->irq_name), "%s_%s%d",
1369                  dev_name(dev), MSM_VFE_NAME, id);
1370         ret = devm_request_irq(dev, vfe->irq, vfe->ops->isr,
1371                                IRQF_TRIGGER_RISING, vfe->irq_name, vfe);
1372         if (ret < 0) {
1373                 dev_err(dev, "request_irq failed: %d\n", ret);
1374                 return ret;
1375         }
1376
1377         /* Clocks */
1378
1379         vfe->nclocks = 0;
1380         while (res->clock[vfe->nclocks])
1381                 vfe->nclocks++;
1382
1383         vfe->clock = devm_kcalloc(dev, vfe->nclocks, sizeof(*vfe->clock),
1384                                   GFP_KERNEL);
1385         if (!vfe->clock)
1386                 return -ENOMEM;
1387
1388         for (i = 0; i < vfe->nclocks; i++) {
1389                 struct camss_clock *clock = &vfe->clock[i];
1390
1391                 clock->clk = devm_clk_get(dev, res->clock[i]);
1392                 if (IS_ERR(clock->clk))
1393                         return PTR_ERR(clock->clk);
1394
1395                 clock->name = res->clock[i];
1396
1397                 clock->nfreqs = 0;
1398                 while (res->clock_rate[i][clock->nfreqs])
1399                         clock->nfreqs++;
1400
1401                 if (!clock->nfreqs) {
1402                         clock->freq = NULL;
1403                         continue;
1404                 }
1405
1406                 clock->freq = devm_kcalloc(dev,
1407                                            clock->nfreqs,
1408                                            sizeof(*clock->freq),
1409                                            GFP_KERNEL);
1410                 if (!clock->freq)
1411                         return -ENOMEM;
1412
1413                 for (j = 0; j < clock->nfreqs; j++)
1414                         clock->freq[j] = res->clock_rate[i][j];
1415         }
1416
1417         mutex_init(&vfe->power_lock);
1418         vfe->power_count = 0;
1419
1420         mutex_init(&vfe->stream_lock);
1421         vfe->stream_count = 0;
1422
1423         spin_lock_init(&vfe->output_lock);
1424
1425         vfe->camss = camss;
1426         vfe->id = id;
1427         vfe->reg_update = 0;
1428
1429         for (i = VFE_LINE_RDI0; i < vfe->line_num; i++) {
1430                 struct vfe_line *l = &vfe->line[i];
1431
1432                 l->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1433                 l->video_out.camss = camss;
1434                 l->id = i;
1435                 init_completion(&l->output.sof);
1436                 init_completion(&l->output.reg_update);
1437
1438                 switch (camss->res->version) {
1439                 case CAMSS_8x16:
1440                         if (i == VFE_LINE_PIX) {
1441                                 l->formats = formats_pix_8x16;
1442                                 l->nformats = ARRAY_SIZE(formats_pix_8x16);
1443                         } else {
1444                                 l->formats = formats_rdi_8x16;
1445                                 l->nformats = ARRAY_SIZE(formats_rdi_8x16);
1446                         }
1447                         break;
1448                 case CAMSS_8x96:
1449                 case CAMSS_660:
1450                         if (i == VFE_LINE_PIX) {
1451                                 l->formats = formats_pix_8x96;
1452                                 l->nformats = ARRAY_SIZE(formats_pix_8x96);
1453                         } else {
1454                                 l->formats = formats_rdi_8x96;
1455                                 l->nformats = ARRAY_SIZE(formats_rdi_8x96);
1456                         }
1457                         break;
1458                 case CAMSS_845:
1459                 case CAMSS_8250:
1460                         l->formats = formats_rdi_845;
1461                         l->nformats = ARRAY_SIZE(formats_rdi_845);
1462                         break;
1463                 }
1464         }
1465
1466         init_completion(&vfe->reset_complete);
1467         init_completion(&vfe->halt_complete);
1468
1469         return 0;
1470 }
1471
1472 /*
1473  * vfe_link_setup - Setup VFE connections
1474  * @entity: Pointer to media entity structure
1475  * @local: Pointer to local pad
1476  * @remote: Pointer to remote pad
1477  * @flags: Link flags
1478  *
1479  * Return 0 on success
1480  */
1481 static int vfe_link_setup(struct media_entity *entity,
1482                           const struct media_pad *local,
1483                           const struct media_pad *remote, u32 flags)
1484 {
1485         if (flags & MEDIA_LNK_FL_ENABLED)
1486                 if (media_pad_remote_pad_first(local))
1487                         return -EBUSY;
1488
1489         return 0;
1490 }
1491
1492 static const struct v4l2_subdev_core_ops vfe_core_ops = {
1493         .s_power = vfe_set_power,
1494 };
1495
1496 static const struct v4l2_subdev_video_ops vfe_video_ops = {
1497         .s_stream = vfe_set_stream,
1498 };
1499
1500 static const struct v4l2_subdev_pad_ops vfe_pad_ops = {
1501         .enum_mbus_code = vfe_enum_mbus_code,
1502         .enum_frame_size = vfe_enum_frame_size,
1503         .get_fmt = vfe_get_format,
1504         .set_fmt = vfe_set_format,
1505         .get_selection = vfe_get_selection,
1506         .set_selection = vfe_set_selection,
1507 };
1508
1509 static const struct v4l2_subdev_ops vfe_v4l2_ops = {
1510         .core = &vfe_core_ops,
1511         .video = &vfe_video_ops,
1512         .pad = &vfe_pad_ops,
1513 };
1514
1515 static const struct v4l2_subdev_internal_ops vfe_v4l2_internal_ops = {
1516         .open = vfe_init_formats,
1517 };
1518
1519 static const struct media_entity_operations vfe_media_ops = {
1520         .link_setup = vfe_link_setup,
1521         .link_validate = v4l2_subdev_link_validate,
1522 };
1523
1524 /*
1525  * msm_vfe_register_entities - Register subdev node for VFE module
1526  * @vfe: VFE device
1527  * @v4l2_dev: V4L2 device
1528  *
1529  * Initialize and register a subdev node for the VFE module. Then
1530  * call msm_video_register() to register the video device node which
1531  * will be connected to this subdev node. Then actually create the
1532  * media link between them.
1533  *
1534  * Return 0 on success or a negative error code otherwise
1535  */
1536 int msm_vfe_register_entities(struct vfe_device *vfe,
1537                               struct v4l2_device *v4l2_dev)
1538 {
1539         struct device *dev = vfe->camss->dev;
1540         struct v4l2_subdev *sd;
1541         struct media_pad *pads;
1542         struct camss_video *video_out;
1543         int ret;
1544         int i;
1545
1546         for (i = 0; i < vfe->line_num; i++) {
1547                 char name[32];
1548
1549                 sd = &vfe->line[i].subdev;
1550                 pads = vfe->line[i].pads;
1551                 video_out = &vfe->line[i].video_out;
1552
1553                 v4l2_subdev_init(sd, &vfe_v4l2_ops);
1554                 sd->internal_ops = &vfe_v4l2_internal_ops;
1555                 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1556                 if (i == VFE_LINE_PIX)
1557                         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
1558                                  MSM_VFE_NAME, vfe->id, "pix");
1559                 else
1560                         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s%d",
1561                                  MSM_VFE_NAME, vfe->id, "rdi", i);
1562
1563                 v4l2_set_subdevdata(sd, &vfe->line[i]);
1564
1565                 ret = vfe_init_formats(sd, NULL);
1566                 if (ret < 0) {
1567                         dev_err(dev, "Failed to init format: %d\n", ret);
1568                         goto error_init;
1569                 }
1570
1571                 pads[MSM_VFE_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1572                 pads[MSM_VFE_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
1573
1574                 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
1575                 sd->entity.ops = &vfe_media_ops;
1576                 ret = media_entity_pads_init(&sd->entity, MSM_VFE_PADS_NUM,
1577                                              pads);
1578                 if (ret < 0) {
1579                         dev_err(dev, "Failed to init media entity: %d\n", ret);
1580                         goto error_init;
1581                 }
1582
1583                 ret = v4l2_device_register_subdev(v4l2_dev, sd);
1584                 if (ret < 0) {
1585                         dev_err(dev, "Failed to register subdev: %d\n", ret);
1586                         goto error_reg_subdev;
1587                 }
1588
1589                 video_out->ops = &vfe->video_ops;
1590                 if (vfe->camss->res->version == CAMSS_845 ||
1591                     vfe->camss->res->version == CAMSS_8250)
1592                         video_out->bpl_alignment = 16;
1593                 else
1594                         video_out->bpl_alignment = 8;
1595                 video_out->line_based = 0;
1596                 if (i == VFE_LINE_PIX) {
1597                         video_out->bpl_alignment = 16;
1598                         video_out->line_based = 1;
1599                 }
1600                 snprintf(name, ARRAY_SIZE(name), "%s%d_%s%d",
1601                          MSM_VFE_NAME, vfe->id, "video", i);
1602                 ret = msm_video_register(video_out, v4l2_dev, name,
1603                                          i == VFE_LINE_PIX ? 1 : 0);
1604                 if (ret < 0) {
1605                         dev_err(dev, "Failed to register video node: %d\n",
1606                                 ret);
1607                         goto error_reg_video;
1608                 }
1609
1610                 ret = media_create_pad_link(
1611                                 &sd->entity, MSM_VFE_PAD_SRC,
1612                                 &video_out->vdev.entity, 0,
1613                                 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1614                 if (ret < 0) {
1615                         dev_err(dev, "Failed to link %s->%s entities: %d\n",
1616                                 sd->entity.name, video_out->vdev.entity.name,
1617                                 ret);
1618                         goto error_link;
1619                 }
1620         }
1621
1622         return 0;
1623
1624 error_link:
1625         msm_video_unregister(video_out);
1626
1627 error_reg_video:
1628         v4l2_device_unregister_subdev(sd);
1629
1630 error_reg_subdev:
1631         media_entity_cleanup(&sd->entity);
1632
1633 error_init:
1634         for (i--; i >= 0; i--) {
1635                 sd = &vfe->line[i].subdev;
1636                 video_out = &vfe->line[i].video_out;
1637
1638                 msm_video_unregister(video_out);
1639                 v4l2_device_unregister_subdev(sd);
1640                 media_entity_cleanup(&sd->entity);
1641         }
1642
1643         return ret;
1644 }
1645
1646 /*
1647  * msm_vfe_unregister_entities - Unregister VFE module subdev node
1648  * @vfe: VFE device
1649  */
1650 void msm_vfe_unregister_entities(struct vfe_device *vfe)
1651 {
1652         int i;
1653
1654         mutex_destroy(&vfe->power_lock);
1655         mutex_destroy(&vfe->stream_lock);
1656
1657         for (i = 0; i < vfe->line_num; i++) {
1658                 struct v4l2_subdev *sd = &vfe->line[i].subdev;
1659                 struct camss_video *video_out = &vfe->line[i].video_out;
1660
1661                 msm_video_unregister(video_out);
1662                 v4l2_device_unregister_subdev(sd);
1663                 media_entity_cleanup(&sd->entity);
1664         }
1665 }