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

/* [<][>][^][v][top][bottom][index][help] */

DEFINITIONS

This source file includes following definitions.
  1. vfe_get_bpp
  2. vfe_find_code
  3. vfe_src_pad_code
  4. vfe_reset
  5. vfe_halt
  6. vfe_init_outputs
  7. vfe_reset_output_maps
  8. vfe_output_init_addrs
  9. vfe_output_update_ping_addr
  10. vfe_output_update_pong_addr
  11. vfe_reserve_wm
  12. vfe_release_wm
  13. vfe_output_frame_drop
  14. vfe_buf_get_pending
  15. vfe_buf_add_pending
  16. vfe_buf_flush_pending
  17. vfe_buf_update_wm_on_next
  18. vfe_buf_update_wm_on_last
  19. vfe_buf_update_wm_on_new
  20. vfe_get_output
  21. vfe_put_output
  22. vfe_enable_output
  23. vfe_disable_output
  24. vfe_enable
  25. vfe_disable
  26. vfe_isr_sof
  27. vfe_isr_reg_update
  28. vfe_isr_wm_done
  29. vfe_isr_comp_done
  30. vfe_isr_reset_ack
  31. vfe_isr_halt_ack
  32. vfe_set_clock_rates
  33. vfe_check_clock_rates
  34. vfe_get
  35. vfe_put
  36. vfe_queue_buffer
  37. vfe_flush_buffers
  38. vfe_set_power
  39. vfe_set_stream
  40. __vfe_get_format
  41. __vfe_get_compose
  42. __vfe_get_crop
  43. vfe_try_format
  44. vfe_try_compose
  45. vfe_try_crop
  46. vfe_enum_mbus_code
  47. vfe_enum_frame_size
  48. vfe_get_format
  49. vfe_set_format
  50. vfe_get_selection
  51. vfe_set_selection
  52. vfe_init_formats
  53. msm_vfe_subdev_init
  54. msm_vfe_get_vfe_id
  55. msm_vfe_get_vfe_line_id
  56. vfe_link_setup
  57. msm_vfe_stop_streaming
  58. msm_vfe_register_entities
  59. msm_vfe_unregister_entities

   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 2
  41 /* Frame drop value. VAL + UPDATES - 1 should not exceed 31 */
  42 #define VFE_FRAME_DROP_VAL 30
  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         struct media_entity *sensor;
 663         unsigned long flags;
 664         unsigned int frame_skip = 0;
 665         unsigned int i;
 666         u16 ub_size;
 667 
 668         ub_size = ops->get_ub_size(vfe->id);
 669         if (!ub_size)
 670                 return -EINVAL;
 671 
 672         sensor = camss_find_sensor(&line->subdev.entity);
 673         if (sensor) {
 674                 struct v4l2_subdev *subdev =
 675                                         media_entity_to_v4l2_subdev(sensor);
 676 
 677                 v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
 678                 /* Max frame skip is 29 frames */
 679                 if (frame_skip > VFE_FRAME_DROP_VAL - 1)
 680                         frame_skip = VFE_FRAME_DROP_VAL - 1;
 681         }
 682 
 683         spin_lock_irqsave(&vfe->output_lock, flags);
 684 
 685         ops->reg_update_clear(vfe, line->id);
 686 
 687         if (output->state != VFE_OUTPUT_RESERVED) {
 688                 dev_err(vfe->camss->dev, "Output is not in reserved state %d\n",
 689                         output->state);
 690                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 691                 return -EINVAL;
 692         }
 693         output->state = VFE_OUTPUT_IDLE;
 694 
 695         output->buf[0] = vfe_buf_get_pending(output);
 696         output->buf[1] = vfe_buf_get_pending(output);
 697 
 698         if (!output->buf[0] && output->buf[1]) {
 699                 output->buf[0] = output->buf[1];
 700                 output->buf[1] = NULL;
 701         }
 702 
 703         if (output->buf[0])
 704                 output->state = VFE_OUTPUT_SINGLE;
 705 
 706         if (output->buf[1])
 707                 output->state = VFE_OUTPUT_CONTINUOUS;
 708 
 709         switch (output->state) {
 710         case VFE_OUTPUT_SINGLE:
 711                 vfe_output_frame_drop(vfe, output, 1 << frame_skip);
 712                 break;
 713         case VFE_OUTPUT_CONTINUOUS:
 714                 vfe_output_frame_drop(vfe, output, 3 << frame_skip);
 715                 break;
 716         default:
 717                 vfe_output_frame_drop(vfe, output, 0);
 718                 break;
 719         }
 720 
 721         output->sequence = 0;
 722         output->wait_sof = 0;
 723         output->wait_reg_update = 0;
 724         reinit_completion(&output->sof);
 725         reinit_completion(&output->reg_update);
 726 
 727         vfe_output_init_addrs(vfe, output, 0);
 728 
 729         if (line->id != VFE_LINE_PIX) {
 730                 ops->set_cgc_override(vfe, output->wm_idx[0], 1);
 731                 ops->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 1);
 732                 ops->bus_connect_wm_to_rdi(vfe, output->wm_idx[0], line->id);
 733                 ops->wm_set_subsample(vfe, output->wm_idx[0]);
 734                 ops->set_rdi_cid(vfe, line->id, 0);
 735                 ops->wm_set_ub_cfg(vfe, output->wm_idx[0],
 736                                    (ub_size + 1) * output->wm_idx[0], ub_size);
 737                 ops->wm_frame_based(vfe, output->wm_idx[0], 1);
 738                 ops->wm_enable(vfe, output->wm_idx[0], 1);
 739                 ops->bus_reload_wm(vfe, output->wm_idx[0]);
 740         } else {
 741                 ub_size /= output->wm_num;
 742                 for (i = 0; i < output->wm_num; i++) {
 743                         ops->set_cgc_override(vfe, output->wm_idx[i], 1);
 744                         ops->wm_set_subsample(vfe, output->wm_idx[i]);
 745                         ops->wm_set_ub_cfg(vfe, output->wm_idx[i],
 746                                            (ub_size + 1) * output->wm_idx[i],
 747                                            ub_size);
 748                         ops->wm_line_based(vfe, output->wm_idx[i],
 749                                         &line->video_out.active_fmt.fmt.pix_mp,
 750                                         i, 1);
 751                         ops->wm_enable(vfe, output->wm_idx[i], 1);
 752                         ops->bus_reload_wm(vfe, output->wm_idx[i]);
 753                 }
 754                 ops->enable_irq_pix_line(vfe, 0, line->id, 1);
 755                 ops->set_module_cfg(vfe, 1);
 756                 ops->set_camif_cfg(vfe, line);
 757                 ops->set_realign_cfg(vfe, line, 1);
 758                 ops->set_xbar_cfg(vfe, output, 1);
 759                 ops->set_demux_cfg(vfe, line);
 760                 ops->set_scale_cfg(vfe, line);
 761                 ops->set_crop_cfg(vfe, line);
 762                 ops->set_clamp_cfg(vfe);
 763                 ops->set_camif_cmd(vfe, 1);
 764         }
 765 
 766         ops->reg_update(vfe, line->id);
 767 
 768         spin_unlock_irqrestore(&vfe->output_lock, flags);
 769 
 770         return 0;
 771 }
 772 
 773 static int vfe_disable_output(struct vfe_line *line)
 774 {
 775         struct vfe_device *vfe = to_vfe(line);
 776         struct vfe_output *output = &line->output;
 777         const struct vfe_hw_ops *ops = vfe->ops;
 778         unsigned long flags;
 779         unsigned long time;
 780         unsigned int i;
 781 
 782         spin_lock_irqsave(&vfe->output_lock, flags);
 783 
 784         output->wait_sof = 1;
 785         spin_unlock_irqrestore(&vfe->output_lock, flags);
 786 
 787         time = wait_for_completion_timeout(&output->sof,
 788                                            msecs_to_jiffies(VFE_NEXT_SOF_MS));
 789         if (!time)
 790                 dev_err(vfe->camss->dev, "VFE sof timeout\n");
 791 
 792         spin_lock_irqsave(&vfe->output_lock, flags);
 793         for (i = 0; i < output->wm_num; i++)
 794                 ops->wm_enable(vfe, output->wm_idx[i], 0);
 795 
 796         ops->reg_update(vfe, line->id);
 797         output->wait_reg_update = 1;
 798         spin_unlock_irqrestore(&vfe->output_lock, flags);
 799 
 800         time = wait_for_completion_timeout(&output->reg_update,
 801                                            msecs_to_jiffies(VFE_NEXT_SOF_MS));
 802         if (!time)
 803                 dev_err(vfe->camss->dev, "VFE reg update timeout\n");
 804 
 805         spin_lock_irqsave(&vfe->output_lock, flags);
 806 
 807         if (line->id != VFE_LINE_PIX) {
 808                 ops->wm_frame_based(vfe, output->wm_idx[0], 0);
 809                 ops->bus_disconnect_wm_from_rdi(vfe, output->wm_idx[0],
 810                                                 line->id);
 811                 ops->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 0);
 812                 ops->set_cgc_override(vfe, output->wm_idx[0], 0);
 813                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 814         } else {
 815                 for (i = 0; i < output->wm_num; i++) {
 816                         ops->wm_line_based(vfe, output->wm_idx[i], NULL, i, 0);
 817                         ops->set_cgc_override(vfe, output->wm_idx[i], 0);
 818                 }
 819 
 820                 ops->enable_irq_pix_line(vfe, 0, line->id, 0);
 821                 ops->set_module_cfg(vfe, 0);
 822                 ops->set_realign_cfg(vfe, line, 0);
 823                 ops->set_xbar_cfg(vfe, output, 0);
 824 
 825                 ops->set_camif_cmd(vfe, 0);
 826                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 827 
 828                 ops->camif_wait_for_stop(vfe, vfe->camss->dev);
 829         }
 830 
 831         return 0;
 832 }
 833 
 834 /*
 835  * vfe_enable - Enable streaming on VFE line
 836  * @line: VFE line
 837  *
 838  * Return 0 on success or a negative error code otherwise
 839  */
 840 static int vfe_enable(struct vfe_line *line)
 841 {
 842         struct vfe_device *vfe = to_vfe(line);
 843         int ret;
 844 
 845         mutex_lock(&vfe->stream_lock);
 846 
 847         if (!vfe->stream_count) {
 848                 vfe->ops->enable_irq_common(vfe);
 849 
 850                 vfe->ops->bus_enable_wr_if(vfe, 1);
 851 
 852                 vfe->ops->set_qos(vfe);
 853 
 854                 vfe->ops->set_ds(vfe);
 855         }
 856 
 857         vfe->stream_count++;
 858 
 859         mutex_unlock(&vfe->stream_lock);
 860 
 861         ret = vfe_get_output(line);
 862         if (ret < 0)
 863                 goto error_get_output;
 864 
 865         ret = vfe_enable_output(line);
 866         if (ret < 0)
 867                 goto error_enable_output;
 868 
 869         vfe->was_streaming = 1;
 870 
 871         return 0;
 872 
 873 
 874 error_enable_output:
 875         vfe_put_output(line);
 876 
 877 error_get_output:
 878         mutex_lock(&vfe->stream_lock);
 879 
 880         if (vfe->stream_count == 1)
 881                 vfe->ops->bus_enable_wr_if(vfe, 0);
 882 
 883         vfe->stream_count--;
 884 
 885         mutex_unlock(&vfe->stream_lock);
 886 
 887         return ret;
 888 }
 889 
 890 /*
 891  * vfe_disable - Disable streaming on VFE line
 892  * @line: VFE line
 893  *
 894  * Return 0 on success or a negative error code otherwise
 895  */
 896 static int vfe_disable(struct vfe_line *line)
 897 {
 898         struct vfe_device *vfe = to_vfe(line);
 899 
 900         vfe_disable_output(line);
 901 
 902         vfe_put_output(line);
 903 
 904         mutex_lock(&vfe->stream_lock);
 905 
 906         if (vfe->stream_count == 1)
 907                 vfe->ops->bus_enable_wr_if(vfe, 0);
 908 
 909         vfe->stream_count--;
 910 
 911         mutex_unlock(&vfe->stream_lock);
 912 
 913         return 0;
 914 }
 915 
 916 /*
 917  * vfe_isr_sof - Process start of frame interrupt
 918  * @vfe: VFE Device
 919  * @line_id: VFE line
 920  */
 921 static void vfe_isr_sof(struct vfe_device *vfe, enum vfe_line_id line_id)
 922 {
 923         struct vfe_output *output;
 924         unsigned long flags;
 925 
 926         spin_lock_irqsave(&vfe->output_lock, flags);
 927         output = &vfe->line[line_id].output;
 928         if (output->wait_sof) {
 929                 output->wait_sof = 0;
 930                 complete(&output->sof);
 931         }
 932         spin_unlock_irqrestore(&vfe->output_lock, flags);
 933 }
 934 
 935 /*
 936  * vfe_isr_reg_update - Process reg update interrupt
 937  * @vfe: VFE Device
 938  * @line_id: VFE line
 939  */
 940 static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
 941 {
 942         struct vfe_output *output;
 943         unsigned long flags;
 944 
 945         spin_lock_irqsave(&vfe->output_lock, flags);
 946         vfe->ops->reg_update_clear(vfe, line_id);
 947 
 948         output = &vfe->line[line_id].output;
 949 
 950         if (output->wait_reg_update) {
 951                 output->wait_reg_update = 0;
 952                 complete(&output->reg_update);
 953                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 954                 return;
 955         }
 956 
 957         if (output->state == VFE_OUTPUT_STOPPING) {
 958                 /* Release last buffer when hw is idle */
 959                 if (output->last_buffer) {
 960                         vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
 961                                         VB2_BUF_STATE_DONE);
 962                         output->last_buffer = NULL;
 963                 }
 964                 output->state = VFE_OUTPUT_IDLE;
 965 
 966                 /* Buffers received in stopping state are queued in */
 967                 /* dma pending queue, start next capture here */
 968 
 969                 output->buf[0] = vfe_buf_get_pending(output);
 970                 output->buf[1] = vfe_buf_get_pending(output);
 971 
 972                 if (!output->buf[0] && output->buf[1]) {
 973                         output->buf[0] = output->buf[1];
 974                         output->buf[1] = NULL;
 975                 }
 976 
 977                 if (output->buf[0])
 978                         output->state = VFE_OUTPUT_SINGLE;
 979 
 980                 if (output->buf[1])
 981                         output->state = VFE_OUTPUT_CONTINUOUS;
 982 
 983                 switch (output->state) {
 984                 case VFE_OUTPUT_SINGLE:
 985                         vfe_output_frame_drop(vfe, output, 2);
 986                         break;
 987                 case VFE_OUTPUT_CONTINUOUS:
 988                         vfe_output_frame_drop(vfe, output, 3);
 989                         break;
 990                 default:
 991                         vfe_output_frame_drop(vfe, output, 0);
 992                         break;
 993                 }
 994 
 995                 vfe_output_init_addrs(vfe, output, 1);
 996         }
 997 
 998         spin_unlock_irqrestore(&vfe->output_lock, flags);
 999 }
1000 
1001 /*
1002  * vfe_isr_wm_done - Process write master done interrupt
1003  * @vfe: VFE Device
1004  * @wm: Write master id
1005  */
1006 static void vfe_isr_wm_done(struct vfe_device *vfe, u8 wm)
1007 {
1008         struct camss_buffer *ready_buf;
1009         struct vfe_output *output;
1010         dma_addr_t *new_addr;
1011         unsigned long flags;
1012         u32 active_index;
1013         u64 ts = ktime_get_ns();
1014         unsigned int i;
1015 
1016         active_index = vfe->ops->wm_get_ping_pong_status(vfe, wm);
1017 
1018         spin_lock_irqsave(&vfe->output_lock, flags);
1019 
1020         if (vfe->wm_output_map[wm] == VFE_LINE_NONE) {
1021                 dev_err_ratelimited(vfe->camss->dev,
1022                                     "Received wm done for unmapped index\n");
1023                 goto out_unlock;
1024         }
1025         output = &vfe->line[vfe->wm_output_map[wm]].output;
1026 
1027         if (output->active_buf == active_index) {
1028                 dev_err_ratelimited(vfe->camss->dev,
1029                                     "Active buffer mismatch!\n");
1030                 goto out_unlock;
1031         }
1032         output->active_buf = active_index;
1033 
1034         ready_buf = output->buf[!active_index];
1035         if (!ready_buf) {
1036                 dev_err_ratelimited(vfe->camss->dev,
1037                                     "Missing ready buf %d %d!\n",
1038                                     !active_index, output->state);
1039                 goto out_unlock;
1040         }
1041 
1042         ready_buf->vb.vb2_buf.timestamp = ts;
1043         ready_buf->vb.sequence = output->sequence++;
1044 
1045         /* Get next buffer */
1046         output->buf[!active_index] = vfe_buf_get_pending(output);
1047         if (!output->buf[!active_index]) {
1048                 /* No next buffer - set same address */
1049                 new_addr = ready_buf->addr;
1050                 vfe_buf_update_wm_on_last(vfe, output);
1051         } else {
1052                 new_addr = output->buf[!active_index]->addr;
1053                 vfe_buf_update_wm_on_next(vfe, output);
1054         }
1055 
1056         if (active_index)
1057                 for (i = 0; i < output->wm_num; i++)
1058                         vfe->ops->wm_set_ping_addr(vfe, output->wm_idx[i],
1059                                                    new_addr[i]);
1060         else
1061                 for (i = 0; i < output->wm_num; i++)
1062                         vfe->ops->wm_set_pong_addr(vfe, output->wm_idx[i],
1063                                                    new_addr[i]);
1064 
1065         spin_unlock_irqrestore(&vfe->output_lock, flags);
1066 
1067         if (output->state == VFE_OUTPUT_STOPPING)
1068                 output->last_buffer = ready_buf;
1069         else
1070                 vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
1071 
1072         return;
1073 
1074 out_unlock:
1075         spin_unlock_irqrestore(&vfe->output_lock, flags);
1076 }
1077 
1078 /*
1079  * vfe_isr_wm_done - Process composite image done interrupt
1080  * @vfe: VFE Device
1081  * @comp: Composite image id
1082  */
1083 static void vfe_isr_comp_done(struct vfe_device *vfe, u8 comp)
1084 {
1085         unsigned int i;
1086 
1087         for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++)
1088                 if (vfe->wm_output_map[i] == VFE_LINE_PIX) {
1089                         vfe_isr_wm_done(vfe, i);
1090                         break;
1091                 }
1092 }
1093 
1094 static inline void vfe_isr_reset_ack(struct vfe_device *vfe)
1095 {
1096         complete(&vfe->reset_complete);
1097 }
1098 
1099 static inline void vfe_isr_halt_ack(struct vfe_device *vfe)
1100 {
1101         complete(&vfe->halt_complete);
1102         vfe->ops->halt_clear(vfe);
1103 }
1104 
1105 /*
1106  * vfe_set_clock_rates - Calculate and set clock rates on VFE module
1107  * @vfe: VFE device
1108  *
1109  * Return 0 on success or a negative error code otherwise
1110  */
1111 static int vfe_set_clock_rates(struct vfe_device *vfe)
1112 {
1113         struct device *dev = vfe->camss->dev;
1114         u32 pixel_clock[MSM_VFE_LINE_NUM];
1115         int i, j;
1116         int ret;
1117 
1118         for (i = VFE_LINE_RDI0; i <= VFE_LINE_PIX; i++) {
1119                 ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
1120                                             &pixel_clock[i]);
1121                 if (ret)
1122                         pixel_clock[i] = 0;
1123         }
1124 
1125         for (i = 0; i < vfe->nclocks; i++) {
1126                 struct camss_clock *clock = &vfe->clock[i];
1127 
1128                 if (!strcmp(clock->name, "vfe0") ||
1129                     !strcmp(clock->name, "vfe1")) {
1130                         u64 min_rate = 0;
1131                         long rate;
1132 
1133                         for (j = VFE_LINE_RDI0; j <= VFE_LINE_PIX; j++) {
1134                                 u32 tmp;
1135                                 u8 bpp;
1136 
1137                                 if (j == VFE_LINE_PIX) {
1138                                         tmp = pixel_clock[j];
1139                                 } else {
1140                                         struct vfe_line *l = &vfe->line[j];
1141 
1142                                         bpp = vfe_get_bpp(l->formats,
1143                                                 l->nformats,
1144                                                 l->fmt[MSM_VFE_PAD_SINK].code);
1145                                         tmp = pixel_clock[j] * bpp / 64;
1146                                 }
1147 
1148                                 if (min_rate < tmp)
1149                                         min_rate = tmp;
1150                         }
1151 
1152                         camss_add_clock_margin(&min_rate);
1153 
1154                         for (j = 0; j < clock->nfreqs; j++)
1155                                 if (min_rate < clock->freq[j])
1156                                         break;
1157 
1158                         if (j == clock->nfreqs) {
1159                                 dev_err(dev,
1160                                         "Pixel clock is too high for VFE");
1161                                 return -EINVAL;
1162                         }
1163 
1164                         /* if sensor pixel clock is not available */
1165                         /* set highest possible VFE clock rate */
1166                         if (min_rate == 0)
1167                                 j = clock->nfreqs - 1;
1168 
1169                         rate = clk_round_rate(clock->clk, clock->freq[j]);
1170                         if (rate < 0) {
1171                                 dev_err(dev, "clk round rate failed: %ld\n",
1172                                         rate);
1173                                 return -EINVAL;
1174                         }
1175 
1176                         ret = clk_set_rate(clock->clk, rate);
1177                         if (ret < 0) {
1178                                 dev_err(dev, "clk set rate failed: %d\n", ret);
1179                                 return ret;
1180                         }
1181                 }
1182         }
1183 
1184         return 0;
1185 }
1186 
1187 /*
1188  * vfe_check_clock_rates - Check current clock rates on VFE module
1189  * @vfe: VFE device
1190  *
1191  * Return 0 if current clock rates are suitable for a new pipeline
1192  * or a negative error code otherwise
1193  */
1194 static int vfe_check_clock_rates(struct vfe_device *vfe)
1195 {
1196         u32 pixel_clock[MSM_VFE_LINE_NUM];
1197         int i, j;
1198         int ret;
1199 
1200         for (i = VFE_LINE_RDI0; i <= VFE_LINE_PIX; i++) {
1201                 ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
1202                                             &pixel_clock[i]);
1203                 if (ret)
1204                         pixel_clock[i] = 0;
1205         }
1206 
1207         for (i = 0; i < vfe->nclocks; i++) {
1208                 struct camss_clock *clock = &vfe->clock[i];
1209 
1210                 if (!strcmp(clock->name, "vfe0") ||
1211                     !strcmp(clock->name, "vfe1")) {
1212                         u64 min_rate = 0;
1213                         unsigned long rate;
1214 
1215                         for (j = VFE_LINE_RDI0; j <= VFE_LINE_PIX; j++) {
1216                                 u32 tmp;
1217                                 u8 bpp;
1218 
1219                                 if (j == VFE_LINE_PIX) {
1220                                         tmp = pixel_clock[j];
1221                                 } else {
1222                                         struct vfe_line *l = &vfe->line[j];
1223 
1224                                         bpp = vfe_get_bpp(l->formats,
1225                                                 l->nformats,
1226                                                 l->fmt[MSM_VFE_PAD_SINK].code);
1227                                         tmp = pixel_clock[j] * bpp / 64;
1228                                 }
1229 
1230                                 if (min_rate < tmp)
1231                                         min_rate = tmp;
1232                         }
1233 
1234                         camss_add_clock_margin(&min_rate);
1235 
1236                         rate = clk_get_rate(clock->clk);
1237                         if (rate < min_rate)
1238                                 return -EBUSY;
1239                 }
1240         }
1241 
1242         return 0;
1243 }
1244 
1245 /*
1246  * vfe_get - Power up and reset VFE module
1247  * @vfe: VFE Device
1248  *
1249  * Return 0 on success or a negative error code otherwise
1250  */
1251 static int vfe_get(struct vfe_device *vfe)
1252 {
1253         int ret;
1254 
1255         mutex_lock(&vfe->power_lock);
1256 
1257         if (vfe->power_count == 0) {
1258                 ret = camss_pm_domain_on(vfe->camss, vfe->id);
1259                 if (ret < 0)
1260                         goto error_pm_domain;
1261 
1262                 ret = pm_runtime_get_sync(vfe->camss->dev);
1263                 if (ret < 0)
1264                         goto error_pm_runtime_get;
1265 
1266                 ret = vfe_set_clock_rates(vfe);
1267                 if (ret < 0)
1268                         goto error_clocks;
1269 
1270                 ret = camss_enable_clocks(vfe->nclocks, vfe->clock,
1271                                           vfe->camss->dev);
1272                 if (ret < 0)
1273                         goto error_clocks;
1274 
1275                 ret = vfe_reset(vfe);
1276                 if (ret < 0)
1277                         goto error_reset;
1278 
1279                 vfe_reset_output_maps(vfe);
1280 
1281                 vfe_init_outputs(vfe);
1282         } else {
1283                 ret = vfe_check_clock_rates(vfe);
1284                 if (ret < 0)
1285                         goto error_clocks;
1286         }
1287         vfe->power_count++;
1288 
1289         mutex_unlock(&vfe->power_lock);
1290 
1291         return 0;
1292 
1293 error_reset:
1294         camss_disable_clocks(vfe->nclocks, vfe->clock);
1295 
1296 error_clocks:
1297         pm_runtime_put_sync(vfe->camss->dev);
1298 
1299 error_pm_runtime_get:
1300         camss_pm_domain_off(vfe->camss, vfe->id);
1301 
1302 error_pm_domain:
1303         mutex_unlock(&vfe->power_lock);
1304 
1305         return ret;
1306 }
1307 
1308 /*
1309  * vfe_put - Power down VFE module
1310  * @vfe: VFE Device
1311  */
1312 static void vfe_put(struct vfe_device *vfe)
1313 {
1314         mutex_lock(&vfe->power_lock);
1315 
1316         if (vfe->power_count == 0) {
1317                 dev_err(vfe->camss->dev, "vfe power off on power_count == 0\n");
1318                 goto exit;
1319         } else if (vfe->power_count == 1) {
1320                 if (vfe->was_streaming) {
1321                         vfe->was_streaming = 0;
1322                         vfe_halt(vfe);
1323                 }
1324                 camss_disable_clocks(vfe->nclocks, vfe->clock);
1325                 pm_runtime_put_sync(vfe->camss->dev);
1326                 camss_pm_domain_off(vfe->camss, vfe->id);
1327         }
1328 
1329         vfe->power_count--;
1330 
1331 exit:
1332         mutex_unlock(&vfe->power_lock);
1333 }
1334 
1335 /*
1336  * vfe_queue_buffer - Add empty buffer
1337  * @vid: Video device structure
1338  * @buf: Buffer to be enqueued
1339  *
1340  * Add an empty buffer - depending on the current number of buffers it will be
1341  * put in pending buffer queue or directly given to the hardware to be filled.
1342  *
1343  * Return 0 on success or a negative error code otherwise
1344  */
1345 static int vfe_queue_buffer(struct camss_video *vid,
1346                             struct camss_buffer *buf)
1347 {
1348         struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
1349         struct vfe_device *vfe = to_vfe(line);
1350         struct vfe_output *output;
1351         unsigned long flags;
1352 
1353         output = &line->output;
1354 
1355         spin_lock_irqsave(&vfe->output_lock, flags);
1356 
1357         vfe_buf_update_wm_on_new(vfe, output, buf);
1358 
1359         spin_unlock_irqrestore(&vfe->output_lock, flags);
1360 
1361         return 0;
1362 }
1363 
1364 /*
1365  * vfe_flush_buffers - Return all vb2 buffers
1366  * @vid: Video device structure
1367  * @state: vb2 buffer state of the returned buffers
1368  *
1369  * Return all buffers to vb2. This includes queued pending buffers (still
1370  * unused) and any buffers given to the hardware but again still not used.
1371  *
1372  * Return 0 on success or a negative error code otherwise
1373  */
1374 static int vfe_flush_buffers(struct camss_video *vid,
1375                              enum vb2_buffer_state state)
1376 {
1377         struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
1378         struct vfe_device *vfe = to_vfe(line);
1379         struct vfe_output *output;
1380         unsigned long flags;
1381 
1382         output = &line->output;
1383 
1384         spin_lock_irqsave(&vfe->output_lock, flags);
1385 
1386         vfe_buf_flush_pending(output, state);
1387 
1388         if (output->buf[0])
1389                 vb2_buffer_done(&output->buf[0]->vb.vb2_buf, state);
1390 
1391         if (output->buf[1])
1392                 vb2_buffer_done(&output->buf[1]->vb.vb2_buf, state);
1393 
1394         if (output->last_buffer) {
1395                 vb2_buffer_done(&output->last_buffer->vb.vb2_buf, state);
1396                 output->last_buffer = NULL;
1397         }
1398 
1399         spin_unlock_irqrestore(&vfe->output_lock, flags);
1400 
1401         return 0;
1402 }
1403 
1404 /*
1405  * vfe_set_power - Power on/off VFE module
1406  * @sd: VFE V4L2 subdevice
1407  * @on: Requested power state
1408  *
1409  * Return 0 on success or a negative error code otherwise
1410  */
1411 static int vfe_set_power(struct v4l2_subdev *sd, int on)
1412 {
1413         struct vfe_line *line = v4l2_get_subdevdata(sd);
1414         struct vfe_device *vfe = to_vfe(line);
1415         int ret;
1416 
1417         if (on) {
1418                 ret = vfe_get(vfe);
1419                 if (ret < 0)
1420                         return ret;
1421 
1422                 vfe->ops->hw_version_read(vfe, vfe->camss->dev);
1423         } else {
1424                 vfe_put(vfe);
1425         }
1426 
1427         return 0;
1428 }
1429 
1430 /*
1431  * vfe_set_stream - Enable/disable streaming on VFE module
1432  * @sd: VFE V4L2 subdevice
1433  * @enable: Requested streaming state
1434  *
1435  * Main configuration of VFE module is triggered here.
1436  *
1437  * Return 0 on success or a negative error code otherwise
1438  */
1439 static int vfe_set_stream(struct v4l2_subdev *sd, int enable)
1440 {
1441         struct vfe_line *line = v4l2_get_subdevdata(sd);
1442         struct vfe_device *vfe = to_vfe(line);
1443         int ret;
1444 
1445         if (enable) {
1446                 ret = vfe_enable(line);
1447                 if (ret < 0)
1448                         dev_err(vfe->camss->dev,
1449                                 "Failed to enable vfe outputs\n");
1450         } else {
1451                 ret = vfe_disable(line);
1452                 if (ret < 0)
1453                         dev_err(vfe->camss->dev,
1454                                 "Failed to disable vfe outputs\n");
1455         }
1456 
1457         return ret;
1458 }
1459 
1460 /*
1461  * __vfe_get_format - Get pointer to format structure
1462  * @line: VFE line
1463  * @cfg: V4L2 subdev pad configuration
1464  * @pad: pad from which format is requested
1465  * @which: TRY or ACTIVE format
1466  *
1467  * Return pointer to TRY or ACTIVE format structure
1468  */
1469 static struct v4l2_mbus_framefmt *
1470 __vfe_get_format(struct vfe_line *line,
1471                  struct v4l2_subdev_pad_config *cfg,
1472                  unsigned int pad,
1473                  enum v4l2_subdev_format_whence which)
1474 {
1475         if (which == V4L2_SUBDEV_FORMAT_TRY)
1476                 return v4l2_subdev_get_try_format(&line->subdev, cfg, pad);
1477 
1478         return &line->fmt[pad];
1479 }
1480 
1481 /*
1482  * __vfe_get_compose - Get pointer to compose selection structure
1483  * @line: VFE line
1484  * @cfg: V4L2 subdev pad configuration
1485  * @which: TRY or ACTIVE format
1486  *
1487  * Return pointer to TRY or ACTIVE compose rectangle structure
1488  */
1489 static struct v4l2_rect *
1490 __vfe_get_compose(struct vfe_line *line,
1491                   struct v4l2_subdev_pad_config *cfg,
1492                   enum v4l2_subdev_format_whence which)
1493 {
1494         if (which == V4L2_SUBDEV_FORMAT_TRY)
1495                 return v4l2_subdev_get_try_compose(&line->subdev, cfg,
1496                                                    MSM_VFE_PAD_SINK);
1497 
1498         return &line->compose;
1499 }
1500 
1501 /*
1502  * __vfe_get_crop - Get pointer to crop selection structure
1503  * @line: VFE line
1504  * @cfg: V4L2 subdev pad configuration
1505  * @which: TRY or ACTIVE format
1506  *
1507  * Return pointer to TRY or ACTIVE crop rectangle structure
1508  */
1509 static struct v4l2_rect *
1510 __vfe_get_crop(struct vfe_line *line,
1511                struct v4l2_subdev_pad_config *cfg,
1512                enum v4l2_subdev_format_whence which)
1513 {
1514         if (which == V4L2_SUBDEV_FORMAT_TRY)
1515                 return v4l2_subdev_get_try_crop(&line->subdev, cfg,
1516                                                 MSM_VFE_PAD_SRC);
1517 
1518         return &line->crop;
1519 }
1520 
1521 /*
1522  * vfe_try_format - Handle try format by pad subdev method
1523  * @line: VFE line
1524  * @cfg: V4L2 subdev pad configuration
1525  * @pad: pad on which format is requested
1526  * @fmt: pointer to v4l2 format structure
1527  * @which: wanted subdev format
1528  */
1529 static void vfe_try_format(struct vfe_line *line,
1530                            struct v4l2_subdev_pad_config *cfg,
1531                            unsigned int pad,
1532                            struct v4l2_mbus_framefmt *fmt,
1533                            enum v4l2_subdev_format_whence which)
1534 {
1535         unsigned int i;
1536         u32 code;
1537 
1538         switch (pad) {
1539         case MSM_VFE_PAD_SINK:
1540                 /* Set format on sink pad */
1541 
1542                 for (i = 0; i < line->nformats; i++)
1543                         if (fmt->code == line->formats[i].code)
1544                                 break;
1545 
1546                 /* If not found, use UYVY as default */
1547                 if (i >= line->nformats)
1548                         fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
1549 
1550                 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
1551                 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
1552 
1553                 fmt->field = V4L2_FIELD_NONE;
1554                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
1555 
1556                 break;
1557 
1558         case MSM_VFE_PAD_SRC:
1559                 /* Set and return a format same as sink pad */
1560                 code = fmt->code;
1561 
1562                 *fmt = *__vfe_get_format(line, cfg, MSM_VFE_PAD_SINK, which);
1563 
1564                 fmt->code = vfe_src_pad_code(line, fmt->code, 0, code);
1565 
1566                 if (line->id == VFE_LINE_PIX) {
1567                         struct v4l2_rect *rect;
1568 
1569                         rect = __vfe_get_crop(line, cfg, which);
1570 
1571                         fmt->width = rect->width;
1572                         fmt->height = rect->height;
1573                 }
1574 
1575                 break;
1576         }
1577 
1578         fmt->colorspace = V4L2_COLORSPACE_SRGB;
1579 }
1580 
1581 /*
1582  * vfe_try_compose - Handle try compose selection by pad subdev method
1583  * @line: VFE line
1584  * @cfg: V4L2 subdev pad configuration
1585  * @rect: pointer to v4l2 rect structure
1586  * @which: wanted subdev format
1587  */
1588 static void vfe_try_compose(struct vfe_line *line,
1589                             struct v4l2_subdev_pad_config *cfg,
1590                             struct v4l2_rect *rect,
1591                             enum v4l2_subdev_format_whence which)
1592 {
1593         struct v4l2_mbus_framefmt *fmt;
1594 
1595         fmt = __vfe_get_format(line, cfg, MSM_VFE_PAD_SINK, which);
1596 
1597         if (rect->width > fmt->width)
1598                 rect->width = fmt->width;
1599 
1600         if (rect->height > fmt->height)
1601                 rect->height = fmt->height;
1602 
1603         if (fmt->width > rect->width * SCALER_RATIO_MAX)
1604                 rect->width = (fmt->width + SCALER_RATIO_MAX - 1) /
1605                                                         SCALER_RATIO_MAX;
1606 
1607         rect->width &= ~0x1;
1608 
1609         if (fmt->height > rect->height * SCALER_RATIO_MAX)
1610                 rect->height = (fmt->height + SCALER_RATIO_MAX - 1) /
1611                                                         SCALER_RATIO_MAX;
1612 
1613         if (rect->width < 16)
1614                 rect->width = 16;
1615 
1616         if (rect->height < 4)
1617                 rect->height = 4;
1618 }
1619 
1620 /*
1621  * vfe_try_crop - Handle try crop selection by pad subdev method
1622  * @line: VFE line
1623  * @cfg: V4L2 subdev pad configuration
1624  * @rect: pointer to v4l2 rect structure
1625  * @which: wanted subdev format
1626  */
1627 static void vfe_try_crop(struct vfe_line *line,
1628                          struct v4l2_subdev_pad_config *cfg,
1629                          struct v4l2_rect *rect,
1630                          enum v4l2_subdev_format_whence which)
1631 {
1632         struct v4l2_rect *compose;
1633 
1634         compose = __vfe_get_compose(line, cfg, which);
1635 
1636         if (rect->width > compose->width)
1637                 rect->width = compose->width;
1638 
1639         if (rect->width + rect->left > compose->width)
1640                 rect->left = compose->width - rect->width;
1641 
1642         if (rect->height > compose->height)
1643                 rect->height = compose->height;
1644 
1645         if (rect->height + rect->top > compose->height)
1646                 rect->top = compose->height - rect->height;
1647 
1648         /* wm in line based mode writes multiple of 16 horizontally */
1649         rect->left += (rect->width & 0xf) >> 1;
1650         rect->width &= ~0xf;
1651 
1652         if (rect->width < 16) {
1653                 rect->left = 0;
1654                 rect->width = 16;
1655         }
1656 
1657         if (rect->height < 4) {
1658                 rect->top = 0;
1659                 rect->height = 4;
1660         }
1661 }
1662 
1663 /*
1664  * vfe_enum_mbus_code - Handle pixel format enumeration
1665  * @sd: VFE V4L2 subdevice
1666  * @cfg: V4L2 subdev pad configuration
1667  * @code: pointer to v4l2_subdev_mbus_code_enum structure
1668  *
1669  * return -EINVAL or zero on success
1670  */
1671 static int vfe_enum_mbus_code(struct v4l2_subdev *sd,
1672                               struct v4l2_subdev_pad_config *cfg,
1673                               struct v4l2_subdev_mbus_code_enum *code)
1674 {
1675         struct vfe_line *line = v4l2_get_subdevdata(sd);
1676 
1677         if (code->pad == MSM_VFE_PAD_SINK) {
1678                 if (code->index >= line->nformats)
1679                         return -EINVAL;
1680 
1681                 code->code = line->formats[code->index].code;
1682         } else {
1683                 struct v4l2_mbus_framefmt *sink_fmt;
1684 
1685                 sink_fmt = __vfe_get_format(line, cfg, MSM_VFE_PAD_SINK,
1686                                             code->which);
1687 
1688                 code->code = vfe_src_pad_code(line, sink_fmt->code,
1689                                               code->index, 0);
1690                 if (!code->code)
1691                         return -EINVAL;
1692         }
1693 
1694         return 0;
1695 }
1696 
1697 /*
1698  * vfe_enum_frame_size - Handle frame size enumeration
1699  * @sd: VFE V4L2 subdevice
1700  * @cfg: V4L2 subdev pad configuration
1701  * @fse: pointer to v4l2_subdev_frame_size_enum structure
1702  *
1703  * Return -EINVAL or zero on success
1704  */
1705 static int vfe_enum_frame_size(struct v4l2_subdev *sd,
1706                                struct v4l2_subdev_pad_config *cfg,
1707                                struct v4l2_subdev_frame_size_enum *fse)
1708 {
1709         struct vfe_line *line = v4l2_get_subdevdata(sd);
1710         struct v4l2_mbus_framefmt format;
1711 
1712         if (fse->index != 0)
1713                 return -EINVAL;
1714 
1715         format.code = fse->code;
1716         format.width = 1;
1717         format.height = 1;
1718         vfe_try_format(line, cfg, fse->pad, &format, fse->which);
1719         fse->min_width = format.width;
1720         fse->min_height = format.height;
1721 
1722         if (format.code != fse->code)
1723                 return -EINVAL;
1724 
1725         format.code = fse->code;
1726         format.width = -1;
1727         format.height = -1;
1728         vfe_try_format(line, cfg, fse->pad, &format, fse->which);
1729         fse->max_width = format.width;
1730         fse->max_height = format.height;
1731 
1732         return 0;
1733 }
1734 
1735 /*
1736  * vfe_get_format - Handle get format by pads subdev method
1737  * @sd: VFE V4L2 subdevice
1738  * @cfg: V4L2 subdev pad configuration
1739  * @fmt: pointer to v4l2 subdev format structure
1740  *
1741  * Return -EINVAL or zero on success
1742  */
1743 static int vfe_get_format(struct v4l2_subdev *sd,
1744                           struct v4l2_subdev_pad_config *cfg,
1745                           struct v4l2_subdev_format *fmt)
1746 {
1747         struct vfe_line *line = v4l2_get_subdevdata(sd);
1748         struct v4l2_mbus_framefmt *format;
1749 
1750         format = __vfe_get_format(line, cfg, fmt->pad, fmt->which);
1751         if (format == NULL)
1752                 return -EINVAL;
1753 
1754         fmt->format = *format;
1755 
1756         return 0;
1757 }
1758 
1759 static int vfe_set_selection(struct v4l2_subdev *sd,
1760                              struct v4l2_subdev_pad_config *cfg,
1761                              struct v4l2_subdev_selection *sel);
1762 
1763 /*
1764  * vfe_set_format - Handle set format by pads subdev method
1765  * @sd: VFE V4L2 subdevice
1766  * @cfg: V4L2 subdev pad configuration
1767  * @fmt: pointer to v4l2 subdev format structure
1768  *
1769  * Return -EINVAL or zero on success
1770  */
1771 static int vfe_set_format(struct v4l2_subdev *sd,
1772                           struct v4l2_subdev_pad_config *cfg,
1773                           struct v4l2_subdev_format *fmt)
1774 {
1775         struct vfe_line *line = v4l2_get_subdevdata(sd);
1776         struct v4l2_mbus_framefmt *format;
1777 
1778         format = __vfe_get_format(line, cfg, fmt->pad, fmt->which);
1779         if (format == NULL)
1780                 return -EINVAL;
1781 
1782         vfe_try_format(line, cfg, fmt->pad, &fmt->format, fmt->which);
1783         *format = fmt->format;
1784 
1785         if (fmt->pad == MSM_VFE_PAD_SINK) {
1786                 struct v4l2_subdev_selection sel = { 0 };
1787                 int ret;
1788 
1789                 /* Propagate the format from sink to source */
1790                 format = __vfe_get_format(line, cfg, MSM_VFE_PAD_SRC,
1791                                           fmt->which);
1792 
1793                 *format = fmt->format;
1794                 vfe_try_format(line, cfg, MSM_VFE_PAD_SRC, format,
1795                                fmt->which);
1796 
1797                 if (line->id != VFE_LINE_PIX)
1798                         return 0;
1799 
1800                 /* Reset sink pad compose selection */
1801                 sel.which = fmt->which;
1802                 sel.pad = MSM_VFE_PAD_SINK;
1803                 sel.target = V4L2_SEL_TGT_COMPOSE;
1804                 sel.r.width = fmt->format.width;
1805                 sel.r.height = fmt->format.height;
1806                 ret = vfe_set_selection(sd, cfg, &sel);
1807                 if (ret < 0)
1808                         return ret;
1809         }
1810 
1811         return 0;
1812 }
1813 
1814 /*
1815  * vfe_get_selection - Handle get selection by pads subdev method
1816  * @sd: VFE V4L2 subdevice
1817  * @cfg: V4L2 subdev pad configuration
1818  * @sel: pointer to v4l2 subdev selection structure
1819  *
1820  * Return -EINVAL or zero on success
1821  */
1822 static int vfe_get_selection(struct v4l2_subdev *sd,
1823                              struct v4l2_subdev_pad_config *cfg,
1824                              struct v4l2_subdev_selection *sel)
1825 {
1826         struct vfe_line *line = v4l2_get_subdevdata(sd);
1827         struct v4l2_subdev_format fmt = { 0 };
1828         struct v4l2_rect *rect;
1829         int ret;
1830 
1831         if (line->id != VFE_LINE_PIX)
1832                 return -EINVAL;
1833 
1834         if (sel->pad == MSM_VFE_PAD_SINK)
1835                 switch (sel->target) {
1836                 case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1837                         fmt.pad = sel->pad;
1838                         fmt.which = sel->which;
1839                         ret = vfe_get_format(sd, cfg, &fmt);
1840                         if (ret < 0)
1841                                 return ret;
1842 
1843                         sel->r.left = 0;
1844                         sel->r.top = 0;
1845                         sel->r.width = fmt.format.width;
1846                         sel->r.height = fmt.format.height;
1847                         break;
1848                 case V4L2_SEL_TGT_COMPOSE:
1849                         rect = __vfe_get_compose(line, cfg, sel->which);
1850                         if (rect == NULL)
1851                                 return -EINVAL;
1852 
1853                         sel->r = *rect;
1854                         break;
1855                 default:
1856                         return -EINVAL;
1857                 }
1858         else if (sel->pad == MSM_VFE_PAD_SRC)
1859                 switch (sel->target) {
1860                 case V4L2_SEL_TGT_CROP_BOUNDS:
1861                         rect = __vfe_get_compose(line, cfg, sel->which);
1862                         if (rect == NULL)
1863                                 return -EINVAL;
1864 
1865                         sel->r.left = rect->left;
1866                         sel->r.top = rect->top;
1867                         sel->r.width = rect->width;
1868                         sel->r.height = rect->height;
1869                         break;
1870                 case V4L2_SEL_TGT_CROP:
1871                         rect = __vfe_get_crop(line, cfg, sel->which);
1872                         if (rect == NULL)
1873                                 return -EINVAL;
1874 
1875                         sel->r = *rect;
1876                         break;
1877                 default:
1878                         return -EINVAL;
1879                 }
1880 
1881         return 0;
1882 }
1883 
1884 /*
1885  * vfe_set_selection - Handle set selection by pads subdev method
1886  * @sd: VFE V4L2 subdevice
1887  * @cfg: V4L2 subdev pad configuration
1888  * @sel: pointer to v4l2 subdev selection structure
1889  *
1890  * Return -EINVAL or zero on success
1891  */
1892 static int vfe_set_selection(struct v4l2_subdev *sd,
1893                              struct v4l2_subdev_pad_config *cfg,
1894                              struct v4l2_subdev_selection *sel)
1895 {
1896         struct vfe_line *line = v4l2_get_subdevdata(sd);
1897         struct v4l2_rect *rect;
1898         int ret;
1899 
1900         if (line->id != VFE_LINE_PIX)
1901                 return -EINVAL;
1902 
1903         if (sel->target == V4L2_SEL_TGT_COMPOSE &&
1904                 sel->pad == MSM_VFE_PAD_SINK) {
1905                 struct v4l2_subdev_selection crop = { 0 };
1906 
1907                 rect = __vfe_get_compose(line, cfg, sel->which);
1908                 if (rect == NULL)
1909                         return -EINVAL;
1910 
1911                 vfe_try_compose(line, cfg, &sel->r, sel->which);
1912                 *rect = sel->r;
1913 
1914                 /* Reset source crop selection */
1915                 crop.which = sel->which;
1916                 crop.pad = MSM_VFE_PAD_SRC;
1917                 crop.target = V4L2_SEL_TGT_CROP;
1918                 crop.r = *rect;
1919                 ret = vfe_set_selection(sd, cfg, &crop);
1920         } else if (sel->target == V4L2_SEL_TGT_CROP &&
1921                 sel->pad == MSM_VFE_PAD_SRC) {
1922                 struct v4l2_subdev_format fmt = { 0 };
1923 
1924                 rect = __vfe_get_crop(line, cfg, sel->which);
1925                 if (rect == NULL)
1926                         return -EINVAL;
1927 
1928                 vfe_try_crop(line, cfg, &sel->r, sel->which);
1929                 *rect = sel->r;
1930 
1931                 /* Reset source pad format width and height */
1932                 fmt.which = sel->which;
1933                 fmt.pad = MSM_VFE_PAD_SRC;
1934                 ret = vfe_get_format(sd, cfg, &fmt);
1935                 if (ret < 0)
1936                         return ret;
1937 
1938                 fmt.format.width = rect->width;
1939                 fmt.format.height = rect->height;
1940                 ret = vfe_set_format(sd, cfg, &fmt);
1941         } else {
1942                 ret = -EINVAL;
1943         }
1944 
1945         return ret;
1946 }
1947 
1948 /*
1949  * vfe_init_formats - Initialize formats on all pads
1950  * @sd: VFE V4L2 subdevice
1951  * @fh: V4L2 subdev file handle
1952  *
1953  * Initialize all pad formats with default values.
1954  *
1955  * Return 0 on success or a negative error code otherwise
1956  */
1957 static int vfe_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1958 {
1959         struct v4l2_subdev_format format = {
1960                 .pad = MSM_VFE_PAD_SINK,
1961                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
1962                               V4L2_SUBDEV_FORMAT_ACTIVE,
1963                 .format = {
1964                         .code = MEDIA_BUS_FMT_UYVY8_2X8,
1965                         .width = 1920,
1966                         .height = 1080
1967                 }
1968         };
1969 
1970         return vfe_set_format(sd, fh ? fh->pad : NULL, &format);
1971 }
1972 
1973 /*
1974  * msm_vfe_subdev_init - Initialize VFE device structure and resources
1975  * @vfe: VFE device
1976  * @res: VFE module resources table
1977  *
1978  * Return 0 on success or a negative error code otherwise
1979  */
1980 int msm_vfe_subdev_init(struct camss *camss, struct vfe_device *vfe,
1981                         const struct resources *res, u8 id)
1982 {
1983         struct device *dev = camss->dev;
1984         struct platform_device *pdev = to_platform_device(dev);
1985         struct resource *r;
1986         int i, j;
1987         int ret;
1988 
1989         vfe->isr_ops.reset_ack = vfe_isr_reset_ack;
1990         vfe->isr_ops.halt_ack = vfe_isr_halt_ack;
1991         vfe->isr_ops.reg_update = vfe_isr_reg_update;
1992         vfe->isr_ops.sof = vfe_isr_sof;
1993         vfe->isr_ops.comp_done = vfe_isr_comp_done;
1994         vfe->isr_ops.wm_done = vfe_isr_wm_done;
1995 
1996         if (camss->version == CAMSS_8x16)
1997                 vfe->ops = &vfe_ops_4_1;
1998         else if (camss->version == CAMSS_8x96)
1999                 vfe->ops = &vfe_ops_4_7;
2000         else
2001                 return -EINVAL;
2002 
2003         /* Memory */
2004 
2005         r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
2006         vfe->base = devm_ioremap_resource(dev, r);
2007         if (IS_ERR(vfe->base)) {
2008                 dev_err(dev, "could not map memory\n");
2009                 return PTR_ERR(vfe->base);
2010         }
2011 
2012         /* Interrupt */
2013 
2014         r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
2015                                          res->interrupt[0]);
2016         if (!r) {
2017                 dev_err(dev, "missing IRQ\n");
2018                 return -EINVAL;
2019         }
2020 
2021         vfe->irq = r->start;
2022         snprintf(vfe->irq_name, sizeof(vfe->irq_name), "%s_%s%d",
2023                  dev_name(dev), MSM_VFE_NAME, vfe->id);
2024         ret = devm_request_irq(dev, vfe->irq, vfe->ops->isr,
2025                                IRQF_TRIGGER_RISING, vfe->irq_name, vfe);
2026         if (ret < 0) {
2027                 dev_err(dev, "request_irq failed: %d\n", ret);
2028                 return ret;
2029         }
2030 
2031         /* Clocks */
2032 
2033         vfe->nclocks = 0;
2034         while (res->clock[vfe->nclocks])
2035                 vfe->nclocks++;
2036 
2037         vfe->clock = devm_kcalloc(dev, vfe->nclocks, sizeof(*vfe->clock),
2038                                   GFP_KERNEL);
2039         if (!vfe->clock)
2040                 return -ENOMEM;
2041 
2042         for (i = 0; i < vfe->nclocks; i++) {
2043                 struct camss_clock *clock = &vfe->clock[i];
2044 
2045                 clock->clk = devm_clk_get(dev, res->clock[i]);
2046                 if (IS_ERR(clock->clk))
2047                         return PTR_ERR(clock->clk);
2048 
2049                 clock->name = res->clock[i];
2050 
2051                 clock->nfreqs = 0;
2052                 while (res->clock_rate[i][clock->nfreqs])
2053                         clock->nfreqs++;
2054 
2055                 if (!clock->nfreqs) {
2056                         clock->freq = NULL;
2057                         continue;
2058                 }
2059 
2060                 clock->freq = devm_kcalloc(dev,
2061                                            clock->nfreqs,
2062                                            sizeof(*clock->freq),
2063                                            GFP_KERNEL);
2064                 if (!clock->freq)
2065                         return -ENOMEM;
2066 
2067                 for (j = 0; j < clock->nfreqs; j++)
2068                         clock->freq[j] = res->clock_rate[i][j];
2069         }
2070 
2071         mutex_init(&vfe->power_lock);
2072         vfe->power_count = 0;
2073 
2074         mutex_init(&vfe->stream_lock);
2075         vfe->stream_count = 0;
2076 
2077         spin_lock_init(&vfe->output_lock);
2078 
2079         vfe->camss = camss;
2080         vfe->id = id;
2081         vfe->reg_update = 0;
2082 
2083         for (i = VFE_LINE_RDI0; i <= VFE_LINE_PIX; i++) {
2084                 struct vfe_line *l = &vfe->line[i];
2085 
2086                 l->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
2087                 l->video_out.camss = camss;
2088                 l->id = i;
2089                 init_completion(&l->output.sof);
2090                 init_completion(&l->output.reg_update);
2091 
2092                 if (camss->version == CAMSS_8x16) {
2093                         if (i == VFE_LINE_PIX) {
2094                                 l->formats = formats_pix_8x16;
2095                                 l->nformats = ARRAY_SIZE(formats_pix_8x16);
2096                         } else {
2097                                 l->formats = formats_rdi_8x16;
2098                                 l->nformats = ARRAY_SIZE(formats_rdi_8x16);
2099                         }
2100                 } else if (camss->version == CAMSS_8x96) {
2101                         if (i == VFE_LINE_PIX) {
2102                                 l->formats = formats_pix_8x96;
2103                                 l->nformats = ARRAY_SIZE(formats_pix_8x96);
2104                         } else {
2105                                 l->formats = formats_rdi_8x96;
2106                                 l->nformats = ARRAY_SIZE(formats_rdi_8x96);
2107                         }
2108                 } else {
2109                         return -EINVAL;
2110                 }
2111         }
2112 
2113         init_completion(&vfe->reset_complete);
2114         init_completion(&vfe->halt_complete);
2115 
2116         return 0;
2117 }
2118 
2119 /*
2120  * msm_vfe_get_vfe_id - Get VFE HW module id
2121  * @entity: Pointer to VFE media entity structure
2122  * @id: Return CSID HW module id here
2123  */
2124 void msm_vfe_get_vfe_id(struct media_entity *entity, u8 *id)
2125 {
2126         struct v4l2_subdev *sd;
2127         struct vfe_line *line;
2128         struct vfe_device *vfe;
2129 
2130         sd = media_entity_to_v4l2_subdev(entity);
2131         line = v4l2_get_subdevdata(sd);
2132         vfe = to_vfe(line);
2133 
2134         *id = vfe->id;
2135 }
2136 
2137 /*
2138  * msm_vfe_get_vfe_line_id - Get VFE line id by media entity
2139  * @entity: Pointer to VFE media entity structure
2140  * @id: Return VFE line id here
2141  */
2142 void msm_vfe_get_vfe_line_id(struct media_entity *entity, enum vfe_line_id *id)
2143 {
2144         struct v4l2_subdev *sd;
2145         struct vfe_line *line;
2146 
2147         sd = media_entity_to_v4l2_subdev(entity);
2148         line = v4l2_get_subdevdata(sd);
2149 
2150         *id = line->id;
2151 }
2152 
2153 /*
2154  * vfe_link_setup - Setup VFE connections
2155  * @entity: Pointer to media entity structure
2156  * @local: Pointer to local pad
2157  * @remote: Pointer to remote pad
2158  * @flags: Link flags
2159  *
2160  * Return 0 on success
2161  */
2162 static int vfe_link_setup(struct media_entity *entity,
2163                           const struct media_pad *local,
2164                           const struct media_pad *remote, u32 flags)
2165 {
2166         if (flags & MEDIA_LNK_FL_ENABLED)
2167                 if (media_entity_remote_pad(local))
2168                         return -EBUSY;
2169 
2170         return 0;
2171 }
2172 
2173 static const struct v4l2_subdev_core_ops vfe_core_ops = {
2174         .s_power = vfe_set_power,
2175 };
2176 
2177 static const struct v4l2_subdev_video_ops vfe_video_ops = {
2178         .s_stream = vfe_set_stream,
2179 };
2180 
2181 static const struct v4l2_subdev_pad_ops vfe_pad_ops = {
2182         .enum_mbus_code = vfe_enum_mbus_code,
2183         .enum_frame_size = vfe_enum_frame_size,
2184         .get_fmt = vfe_get_format,
2185         .set_fmt = vfe_set_format,
2186         .get_selection = vfe_get_selection,
2187         .set_selection = vfe_set_selection,
2188 };
2189 
2190 static const struct v4l2_subdev_ops vfe_v4l2_ops = {
2191         .core = &vfe_core_ops,
2192         .video = &vfe_video_ops,
2193         .pad = &vfe_pad_ops,
2194 };
2195 
2196 static const struct v4l2_subdev_internal_ops vfe_v4l2_internal_ops = {
2197         .open = vfe_init_formats,
2198 };
2199 
2200 static const struct media_entity_operations vfe_media_ops = {
2201         .link_setup = vfe_link_setup,
2202         .link_validate = v4l2_subdev_link_validate,
2203 };
2204 
2205 static const struct camss_video_ops camss_vfe_video_ops = {
2206         .queue_buffer = vfe_queue_buffer,
2207         .flush_buffers = vfe_flush_buffers,
2208 };
2209 
2210 void msm_vfe_stop_streaming(struct vfe_device *vfe)
2211 {
2212         int i;
2213 
2214         for (i = 0; i < ARRAY_SIZE(vfe->line); i++)
2215                 msm_video_stop_streaming(&vfe->line[i].video_out);
2216 }
2217 
2218 /*
2219  * msm_vfe_register_entities - Register subdev node for VFE module
2220  * @vfe: VFE device
2221  * @v4l2_dev: V4L2 device
2222  *
2223  * Initialize and register a subdev node for the VFE module. Then
2224  * call msm_video_register() to register the video device node which
2225  * will be connected to this subdev node. Then actually create the
2226  * media link between them.
2227  *
2228  * Return 0 on success or a negative error code otherwise
2229  */
2230 int msm_vfe_register_entities(struct vfe_device *vfe,
2231                               struct v4l2_device *v4l2_dev)
2232 {
2233         struct device *dev = vfe->camss->dev;
2234         struct v4l2_subdev *sd;
2235         struct media_pad *pads;
2236         struct camss_video *video_out;
2237         int ret;
2238         int i;
2239 
2240         for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
2241                 char name[32];
2242 
2243                 sd = &vfe->line[i].subdev;
2244                 pads = vfe->line[i].pads;
2245                 video_out = &vfe->line[i].video_out;
2246 
2247                 v4l2_subdev_init(sd, &vfe_v4l2_ops);
2248                 sd->internal_ops = &vfe_v4l2_internal_ops;
2249                 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
2250                 if (i == VFE_LINE_PIX)
2251                         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
2252                                  MSM_VFE_NAME, vfe->id, "pix");
2253                 else
2254                         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s%d",
2255                                  MSM_VFE_NAME, vfe->id, "rdi", i);
2256 
2257                 v4l2_set_subdevdata(sd, &vfe->line[i]);
2258 
2259                 ret = vfe_init_formats(sd, NULL);
2260                 if (ret < 0) {
2261                         dev_err(dev, "Failed to init format: %d\n", ret);
2262                         goto error_init;
2263                 }
2264 
2265                 pads[MSM_VFE_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
2266                 pads[MSM_VFE_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
2267 
2268                 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
2269                 sd->entity.ops = &vfe_media_ops;
2270                 ret = media_entity_pads_init(&sd->entity, MSM_VFE_PADS_NUM,
2271                                              pads);
2272                 if (ret < 0) {
2273                         dev_err(dev, "Failed to init media entity: %d\n", ret);
2274                         goto error_init;
2275                 }
2276 
2277                 ret = v4l2_device_register_subdev(v4l2_dev, sd);
2278                 if (ret < 0) {
2279                         dev_err(dev, "Failed to register subdev: %d\n", ret);
2280                         goto error_reg_subdev;
2281                 }
2282 
2283                 video_out->ops = &camss_vfe_video_ops;
2284                 video_out->bpl_alignment = 8;
2285                 video_out->line_based = 0;
2286                 if (i == VFE_LINE_PIX) {
2287                         video_out->bpl_alignment = 16;
2288                         video_out->line_based = 1;
2289                 }
2290                 snprintf(name, ARRAY_SIZE(name), "%s%d_%s%d",
2291                          MSM_VFE_NAME, vfe->id, "video", i);
2292                 ret = msm_video_register(video_out, v4l2_dev, name,
2293                                          i == VFE_LINE_PIX ? 1 : 0);
2294                 if (ret < 0) {
2295                         dev_err(dev, "Failed to register video node: %d\n",
2296                                 ret);
2297                         goto error_reg_video;
2298                 }
2299 
2300                 ret = media_create_pad_link(
2301                                 &sd->entity, MSM_VFE_PAD_SRC,
2302                                 &video_out->vdev.entity, 0,
2303                                 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
2304                 if (ret < 0) {
2305                         dev_err(dev, "Failed to link %s->%s entities: %d\n",
2306                                 sd->entity.name, video_out->vdev.entity.name,
2307                                 ret);
2308                         goto error_link;
2309                 }
2310         }
2311 
2312         return 0;
2313 
2314 error_link:
2315         msm_video_unregister(video_out);
2316 
2317 error_reg_video:
2318         v4l2_device_unregister_subdev(sd);
2319 
2320 error_reg_subdev:
2321         media_entity_cleanup(&sd->entity);
2322 
2323 error_init:
2324         for (i--; i >= 0; i--) {
2325                 sd = &vfe->line[i].subdev;
2326                 video_out = &vfe->line[i].video_out;
2327 
2328                 msm_video_unregister(video_out);
2329                 v4l2_device_unregister_subdev(sd);
2330                 media_entity_cleanup(&sd->entity);
2331         }
2332 
2333         return ret;
2334 }
2335 
2336 /*
2337  * msm_vfe_unregister_entities - Unregister VFE module subdev node
2338  * @vfe: VFE device
2339  */
2340 void msm_vfe_unregister_entities(struct vfe_device *vfe)
2341 {
2342         int i;
2343 
2344         mutex_destroy(&vfe->power_lock);
2345         mutex_destroy(&vfe->stream_lock);
2346 
2347         for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
2348                 struct v4l2_subdev *sd = &vfe->line[i].subdev;
2349                 struct camss_video *video_out = &vfe->line[i].video_out;
2350 
2351                 msm_video_unregister(video_out);
2352                 v4l2_device_unregister_subdev(sd);
2353                 media_entity_cleanup(&sd->entity);
2354         }
2355 }

/* [<][>][^][v][top][bottom][index][help] */