drivers/media/platform/msm/camss-8x16/csid.c

   1 /*
   2  * csid.c
   3  *
   4  * Qualcomm MSM Camera Subsystem - CSID Module
   5  *
   6  * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
   7  * Copyright (C) 2015-2016 Linaro Ltd.
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License version 2 and
  11  * only version 2 as published by the Free Software Foundation.
  12  *
  13  * This program is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  */
  18 #include <linux/clk.h>
  19 #include <linux/completion.h>
  20 #include <linux/interrupt.h>
  21 #include <linux/of.h>
  22 #include <linux/platform_device.h>
  23 #include <linux/regulator/consumer.h>
  24 #include <media/media-entity.h>
  25 #include <media/v4l2-device.h>
  26 #include <media/v4l2-subdev.h>
  27
  28 #include "csid.h"
  29 #include "camss.h"
  30
  31 #define MSM_CSID_NAME "msm_csid"
  32
  33 #define CAMSS_CSID_HW_VERSION           0x0
  34 #define CAMSS_CSID_CORE_CTRL_0          0x004
  35 #define CAMSS_CSID_CORE_CTRL_1          0x008
  36 #define CAMSS_CSID_RST_CMD              0x00c
  37 #define CAMSS_CSID_CID_LUT_VC_n(n)      (0x010 + 0x4 * (n))
  38 #define CAMSS_CSID_CID_n_CFG(n)         (0x020 + 0x4 * (n))
  39 #define CAMSS_CSID_IRQ_CLEAR_CMD        0x060
  40 #define CAMSS_CSID_IRQ_MASK             0x064
  41 #define CAMSS_CSID_IRQ_STATUS           0x068
  42 #define CAMSS_CSID_TG_CTRL              0x0a0
  43 #define CAMSS_CSID_TG_VC_CFG            0x0a4
  44 #define CAMSS_CSID_TG_VC_CFG_H_BLANKING         0x3ff
  45 #define CAMSS_CSID_TG_VC_CFG_V_BLANKING         0x7f
  46 #define CAMSS_CSID_TG_DT_n_CGG_0(n)     (0x0ac + 0xc * (n))
  47 #define CAMSS_CSID_TG_DT_n_CGG_1(n)     (0x0b0 + 0xc * (n))
  48 #define CAMSS_CSID_TG_DT_n_CGG_2(n)     (0x0b4 + 0xc * (n))
  49
  50 /*
  51  * csid_isr - CSID module interrupt handler
  52  * @irq: Interrupt line
  53  * @dev: CSID device
  54  *
  55  * Return IRQ_HANDLED on success
  56  */
  57 static irqreturn_t csid_isr(int irq, void *dev)
  58 {
  59         struct csid_device *csid = dev;
  60         u32 value;
  61
  62         value = readl(csid->base + CAMSS_CSID_IRQ_STATUS);
  63         writel(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
  64
  65         if ((value >> 11) & 0x1)
  66                 complete(&csid->reset_complete);
  67
  68         return IRQ_HANDLED;
  69 }
  70
  71 /*
  72  * csid_enable_clocks - Enable clocks for CSID module and
  73  * set clock rates where needed
  74  * @csid: CSID device
  75  *
  76  * Return 0 on success or a negative error code otherwise
  77  */
  78 static int csid_enable_clocks(int nclocks, struct clk **clock, s32 *clock_rate)
  79 {
  80         long clk_rate;
  81         int ret;
  82         int i;
  83
  84         for (i = 0; i < nclocks; i++) {
  85                 if (clock_rate[i]) {
  86                         clk_rate = clk_round_rate(clock[i], clock_rate[i]);
  87                         if (clk_rate < 0) {
  88                                 pr_err("clock round rate failed\n");
  89                                 ret = clk_rate;
  90                                 goto error;
  91                         }
  92                         ret = clk_set_rate(clock[i], clk_rate);
  93                         if (ret < 0) {
  94                                 pr_err("clock set rate failed\n");
  95                                 goto error;
  96                         }
  97                 }
  98                 ret = clk_prepare_enable(clock[i]);
  99                 if (ret) {
 100                         pr_err("clock enable failed\n");
 101                         goto error;
 102                 }
 103         }
 104
 105         return 0;
 106
 107 error:
 108         for (i--; i >= 0; i--)
 109                 clk_disable_unprepare(clock[i]);
 110
 111         return ret;
 112 }
 113
 114 /*
 115  * csid_disable_clocks - Disable clocks for CSID module
 116  * @csid: CSID device
 117  */
 118 static void csid_disable_clocks(int nclocks, struct clk **clock)
 119 {
 120         int i;
 121
 122         for (i = nclocks - 1; i >= 0; i--)
 123                 clk_disable_unprepare(clock[i]);
 124 }
 125
 126 /*
 127  * csid_set_power - Power on/off CSID module
 128  * @sd: CSID V4L2 subdevice
 129  * @on: Requested power state
 130  *
 131  * Return 0 on success or a negative error code otherwise
 132  */
 133 static int csid_set_power(struct v4l2_subdev *sd, int on)
 134 {
 135         struct csid_device *csid = v4l2_get_subdevdata(sd);
 136         int ret;
 137
 138         dev_err(csid->camss->dev, "%s: Enter, csid%d on = %d\n",
 139                 __func__, csid->id, on);
 140
 141         if (on) {
 142                 u32 hw_version;
 143
 144                 ret = regulator_enable(csid->vdda);
 145                 if (ret < 0)
 146                         return ret;
 147
 148                 ret = csid_enable_clocks(csid->nclocks, csid->clock,
 149                                          csid->clock_rate);
 150                 if (ret < 0)
 151                         return ret;
 152
 153                 enable_irq(csid->irq);
 154
 155                 hw_version = readl(csid->base + CAMSS_CSID_HW_VERSION);
 156                 dev_err(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
 157         } else {
 158                 disable_irq(csid->irq);
 159
 160                 csid_disable_clocks(csid->nclocks, csid->clock);
 161
 162                 ret = regulator_disable(csid->vdda);
 163                 if (ret < 0)
 164                         return ret;
 165         }
 166
 167         dev_err(csid->camss->dev, "%s: Exit, csid%d on = %d\n",
 168                 __func__, csid->id, on);
 169
 170         return 0;
 171 }
 172
 173 #define DATA_TYPE_YUV422_8BIT 0x1e
 174
 175 /*
 176  * csid_get_data_type - map media but format to data type
 177  * @fmt media bus format code
 178  *
 179  * Return data type code
 180  */
 181 static u8 csid_get_data_type(u32 fmt)
 182 {
 183         switch (fmt) {
 184         case MEDIA_BUS_FMT_UYVY8_2X8:
 185                 return DATA_TYPE_YUV422_8BIT;
 186         }
 187
 188         return 0;
 189 }
 190
 191 #define DECODE_FORMAT_UNCOMPRESSED_8_BIT 0x1
 192
 193 /*
 194  * csid_get_decode_format - map media but format to decode format
 195  * @fmt media bus format code
 196  *
 197  * Return decode format code
 198  */
 199 static u8 csid_get_decode_format(u32 fmt)
 200 {
 201         switch (fmt) {
 202         case MEDIA_BUS_FMT_UYVY8_2X8:
 203                 return DECODE_FORMAT_UNCOMPRESSED_8_BIT;
 204         }
 205
 206         return 0;
 207 }
 208
 209 /*
 210  * csid_set_stream - Enable/disable streaming on CSID module
 211  * @sd: CSID V4L2 subdevice
 212  * @enable: Requested streaming state
 213  *
 214  * Main configuration of CSID module is also done here.
 215  *
 216  * Return 0 on success or a negative error code otherwise
 217  */
 218 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 219 {
 220         struct csid_device *csid = v4l2_get_subdevdata(sd);
 221         struct csid_testgen_config *tg = &csid->testgen;
 222
 223         dev_err(csid->camss->dev, "%s: Enter, csid%d enable = %d\n",
 224                 __func__, csid->id, enable);
 225
 226         if (enable) {
 227                 u8 vc = 0; /* TODO: How to get this from sensor? */
 228                 u8 cid = vc * 4;
 229                 u8 dt, dt_shift, df;
 230                 u32 val;
 231                 int ret;
 232
 233                 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
 234                 if (ret < 0) {
 235                         dev_err(csid->camss->dev,
 236                                 "could not sync v4l2 controls\n");
 237                         return ret;
 238                 }
 239
 240                 if (!tg->enabled &&
 241                     !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) {
 242                         return -ENOLINK;
 243                 }
 244
 245                 /* Reset */
 246                 writel(0x7FFF, csid->base + CAMSS_CSID_RST_CMD);
 247                 wait_for_completion(&csid->reset_complete);
 248
 249                 dt = csid_get_data_type(csid->fmt[MSM_CSID_PAD_SRC].code);
 250
 251                 if (tg->enabled) {
 252                         /* Config Test Generator */
 253                         u32 num_bytes_per_line =
 254                                         csid->fmt[MSM_CSID_PAD_SRC].width * 2;
 255                         u32 num_lines = csid->fmt[MSM_CSID_PAD_SRC].height;
 256
 257                         /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
 258                         /* 1:0 VC */
 259                         val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
 260                               ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
 261                         writel(val, csid->base + CAMSS_CSID_TG_VC_CFG);
 262
 263                         /* 28:16 bytes per lines, 12:0 num of lines */
 264                         val = ((num_bytes_per_line & 0x1FFF) << 16) |
 265                               (num_lines & 0x1FFF);
 266                         writel(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_0(0));
 267
 268                         /* 5:0 data type */
 269                         val = dt;
 270                         writel(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_1(0));
 271
 272                         /* 2:0 output random */
 273                         val = tg->payload_mode;
 274                         writel(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_2(0));
 275                 } else {
 276                         struct csid_phy_config *phy = &csid->phy;
 277
 278                         val = phy->lane_cnt - 1;
 279                         val |= phy->lane_assign << 4;
 280
 281                         writel(val, csid->base + CAMSS_CSID_CORE_CTRL_0);
 282
 283                         val = phy->csiphy_id << 17;
 284                         val |= 0x9;
 285
 286                         writel(val, csid->base + CAMSS_CSID_CORE_CTRL_1);
 287                 }
 288
 289                 /* Config LUT */
 290
 291                 dt_shift = (cid % 4) * 8;
 292                 df = csid_get_decode_format(csid->fmt[MSM_CSID_PAD_SINK].code);
 293
 294                 val = readl(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
 295                 val &= ~(0xff << dt_shift);
 296                 val |= dt << dt_shift;
 297                 writel(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
 298
 299                 val = (df << 4) | 0x3;
 300                 writel(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
 301
 302                 if (tg->enabled) {
 303                         val = 0x00a06437;
 304                         writel(val, csid->base + CAMSS_CSID_TG_CTRL);
 305                 }
 306         } else {
 307                 if (tg->enabled) {
 308                         u32 val = 0x00a06436;
 309                         writel(val, csid->base + CAMSS_CSID_TG_CTRL);
 310                 }
 311         }
 312
 313         return 0;
 314 }
 315
 316 /*
 317  * __csid_get_format - Get pointer to format structure
 318  * @csid: CSID device
 319  * @cfg: V4L2 subdev pad configuration
 320  * @pad: pad from which format is requested
 321  * @which: TRY or ACTIVE format
 322  *
 323  * Return pointer to TRY or ACTIVE format structure
 324  */
 325 static struct v4l2_mbus_framefmt *
 326 __csid_get_format(struct csid_device *csid,
 327                   struct v4l2_subdev_pad_config *cfg,
 328                   unsigned int pad,
 329                   enum v4l2_subdev_format_whence which)
 330 {
 331         if (which == V4L2_SUBDEV_FORMAT_TRY)
 332                 return v4l2_subdev_get_try_format(&csid->subdev, cfg, pad);
 333
 334         return &csid->fmt[pad];
 335 }
 336
 337 /*
 338  * csid_get_format - Handle get format by pads subdev method
 339  * @sd: CSID V4L2 subdevice
 340  * @cfg: V4L2 subdev pad configuration
 341  * @fmt: pointer to v4l2 subdev format structure
 342  *
 343  * Return -EINVAL or zero on success
 344  */
 345 static int csid_get_format(struct v4l2_subdev *sd,
 346                            struct v4l2_subdev_pad_config *cfg,
 347                            struct v4l2_subdev_format *fmt)
 348 {
 349         struct csid_device *csid = v4l2_get_subdevdata(sd);
 350         struct v4l2_mbus_framefmt *format;
 351
 352         format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
 353         if (format == NULL)
 354                 return -EINVAL;
 355
 356         fmt->format = *format;
 357
 358         return 0;
 359 }
 360
 361 /*
 362  * csid_set_format - Handle set format by pads subdev method
 363  * @sd: CSID V4L2 subdevice
 364  * @cfg: V4L2 subdev pad configuration
 365  * @fmt: pointer to v4l2 subdev format structure
 366  *
 367  * Return -EINVAL or zero on success
 368  */
 369 static int csid_set_format(struct v4l2_subdev *sd,
 370                            struct v4l2_subdev_pad_config *cfg,
 371                            struct v4l2_subdev_format *fmt)
 372 {
 373         struct csid_device *csid = v4l2_get_subdevdata(sd);
 374         struct v4l2_mbus_framefmt *format;
 375
 376         if (fmt->pad == MSM_CSID_PAD_SINK) {
 377                 /* Set format on sink pad */
 378                 format = __csid_get_format(csid, cfg, fmt->pad,
 379                                            fmt->which);
 380                 if (format == NULL)
 381                         return -EINVAL;
 382
 383                 *format = fmt->format;
 384
 385                 /* Reset format on source pad */
 386                 format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SRC,
 387                                            fmt->which);
 388                 if (format == NULL)
 389                         return -EINVAL;
 390
 391                 *format = fmt->format;
 392         } else {
 393                 if (media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) {
 394                         /* CSID is linked to CSIPHY */
 395                         /* Reset format on source pad to sink pad format */
 396
 397                         format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SINK,
 398                                                    fmt->which);
 399                         if (format == NULL)
 400                                 return -EINVAL;
 401
 402                         fmt->format = *format;
 403
 404                         format = __csid_get_format(csid, cfg, fmt->pad,
 405                                                    fmt->which);
 406                         if (format == NULL)
 407                                 return -EINVAL;
 408
 409                         *format = fmt->format;
 410                 } else {
 411                         /* CSID is not linked to CSIPHY */
 412                         /* Set format on source pad to allow */
 413                         /* test generator usage */
 414
 415                         format = __csid_get_format(csid, cfg, fmt->pad,
 416                                                    fmt->which);
 417                         if (format == NULL)
 418                                 return -EINVAL;
 419
 420                         /* Accept only YUV422 format */
 421                         fmt->format.code = MEDIA_BUS_FMT_UYVY8_2X8;
 422                         *format = fmt->format;
 423                 }
 424         }
 425
 426         return 0;
 427 }
 428
 429 static const char * const csid_test_pattern_menu[] = {
 430         "Disabled",
 431         "Incrementing",
 432         "Alternating 55/AA",
 433         "All Zeros",
 434         "All Ones",
 435         "Random Data",
 436 };
 437
 438 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
 439 {
 440         struct csid_testgen_config *tg = &csid->testgen;
 441
 442         /* If CSID is linked to CSIPHY, do not allow to enable test generator */
 443         if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
 444                 return -EBUSY;
 445
 446         tg->enabled = !!value;
 447
 448         switch (value) {
 449         case 1:
 450                 tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING;
 451                 break;
 452         case 2:
 453                 tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA;
 454                 break;
 455         case 3:
 456                 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES;
 457                 break;
 458         case 4:
 459                 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES;
 460                 break;
 461         case 5:
 462                 tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM;
 463                 break;
 464         }
 465
 466         return 0;
 467 }
 468
 469 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
 470 {
 471         struct csid_device *csid = container_of(ctrl->handler,
 472                                                 struct csid_device, ctrls);
 473         int ret = -EINVAL;
 474
 475         switch (ctrl->id) {
 476         case V4L2_CID_TEST_PATTERN:
 477                 ret = csid_set_test_pattern(csid, ctrl->val);
 478                 break;
 479         }
 480
 481         return ret;
 482 }
 483
 484 static struct v4l2_ctrl_ops csid_ctrl_ops = {
 485         .s_ctrl = csid_s_ctrl,
 486 };
 487
 488 /*
 489  * msm_csid_subdev_init - Initialize CSID device structure and resources
 490  * @csid: CSID device
 491  * @camss: Camera sub-system structure
 492  * @res: CSID module resources table
 493  * @id: CSID module id
 494  *
 495  * Return 0 on success or a negative error code otherwise
 496  */
 497 int msm_csid_subdev_init(struct csid_device *csid, struct camss *camss,
 498                          struct resources *res, u8 id)
 499 {
 500         struct device *dev = camss->dev;
 501         struct platform_device *pdev = container_of(dev, struct platform_device, dev);
 502         struct resource *r;
 503         int i;
 504         int ret;
 505
 506         csid->camss = camss;
 507
 508         csid->id = id;
 509
 510         /* Memory */
 511
 512         r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
 513         csid->base = devm_ioremap_resource(dev, r);
 514         if (IS_ERR(csid->base)) {
 515                 dev_err(dev, "could not map memory\n");
 516                 return PTR_ERR(csid->base);
 517         }
 518
 519         /* Interrupt */
 520
 521         r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, res->interrupt[0]);
 522         csid->irq = r->start;
 523         if (IS_ERR_VALUE(csid->irq))
 524                 return csid->irq;
 525
 526         ret = devm_request_irq(dev, csid->irq, csid_isr,
 527                 IRQF_TRIGGER_RISING, dev_name(dev), csid);
 528         if (ret < 0) {
 529                 dev_err(dev, "request_irq failed\n");
 530                 return ret;
 531         }
 532
 533         disable_irq(csid->irq);
 534
 535         /* Clocks */
 536
 537         i = 0;
 538         csid->nclocks = 0;
 539         while (res->clock[i++])
 540                 csid->nclocks++;
 541
 542         csid->clock = devm_kzalloc(dev, csid->nclocks * sizeof(*csid->clock),
 543                                     GFP_KERNEL);
 544         if (!csid->clock) {
 545                 dev_err(dev, "could not allocate memory\n");
 546                 return -ENOMEM;
 547         }
 548
 549         csid->clock_rate = devm_kzalloc(dev, csid->nclocks *
 550                                         sizeof(*csid->clock_rate), GFP_KERNEL);
 551         if (!csid->clock_rate) {
 552                 dev_err(dev, "could not allocate memory\n");
 553                 return -ENOMEM;
 554         }
 555
 556         for (i = 0; i < csid->nclocks; i++) {
 557                 csid->clock[i] = devm_clk_get(dev, res->clock[i]);
 558                 if (IS_ERR(csid->clock[i]))
 559                         return PTR_ERR(csid->clock[i]);
 560                 csid->clock_rate[i] = res->clock_rate[i];
 561         }
 562
 563         /* Regulator */
 564
 565         csid->vdda = devm_regulator_get(dev, res->regulator[0]);
 566         if (IS_ERR(csid->vdda)) {
 567                 dev_err(dev, "could not get regulator\n");
 568                 return PTR_ERR(csid->vdda);
 569         }
 570
 571         init_completion(&csid->reset_complete);
 572
 573         return 0;
 574 }
 575
 576 /*
 577  * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
 578  * @lane_cfg - CSI2 lane configuration
 579  *
 580  * Return lane assign
 581  */
 582 static u32 csid_get_lane_assign(struct camss_csiphy_lanes_cfg *lanecfg)
 583 {
 584         u32 lane_assign = 0;
 585         int i;
 586
 587         for (i = 0; i < lanecfg->num_data; i++)
 588                 lane_assign |= lanecfg->data[i].pos << (i * 4);
 589
 590         return lane_assign;
 591 }
 592
 593 /*
 594  * csid_link_setup - Setup CSID connections
 595  * @entity: Pointer to media entity structure
 596  * @local: Pointer to local pad
 597  * @remote: Pointer to remote pad
 598  * @flags: Link flags
 599  *
 600  * Rreturn 0 on success
 601  */
 602 static int csid_link_setup(struct media_entity *entity,
 603                            const struct media_pad *local,
 604                            const struct media_pad *remote, u32 flags)
 605 {
 606         if ((local->flags & MEDIA_PAD_FL_SINK) &&
 607             (flags & MEDIA_LNK_FL_ENABLED)) {
 608                 struct v4l2_subdev *sd;
 609                 struct csid_device *csid;
 610                 struct csiphy_device *csiphy;
 611                 struct camss_csiphy_lanes_cfg *lanecfg;
 612
 613                 sd = container_of(entity, struct v4l2_subdev, entity);
 614                 csid = v4l2_get_subdevdata(sd);
 615
 616                 /* If test generator is enabled
 617                  * do not allow a link from CSIPHY to CSID */
 618                 if (csid->testgen_mode->cur.val != 0)
 619                         return -EBUSY;
 620
 621                 sd = container_of(remote->entity, struct v4l2_subdev, entity);
 622                 csiphy = v4l2_get_subdevdata(sd);
 623
 624                 /* If a sensor is not linked to CSIPHY
 625                  * do no allow a link from CSIPHY to CSID */
 626                 if (!csiphy->cfg.csi2)
 627                         return -EPERM;
 628
 629                 csid->phy.csiphy_id = csiphy->id;
 630
 631                 lanecfg = &csiphy->cfg.csi2->lanecfg;
 632                 csid->phy.lane_cnt = lanecfg->num_data;
 633                 csid->phy.lane_assign = csid_get_lane_assign(lanecfg);
 634         }
 635
 636         return 0;
 637 }
 638
 639 static const struct v4l2_subdev_core_ops csid_core_ops = {
 640         .s_power = csid_set_power,
 641 };
 642
 643 static const struct v4l2_subdev_video_ops csid_video_ops = {
 644         .s_stream = csid_set_stream,
 645 };
 646
 647 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
 648         .get_fmt = csid_get_format,
 649         .set_fmt = csid_set_format,
 650 };
 651
 652 static const struct v4l2_subdev_ops csid_v4l2_ops = {
 653         .core = &csid_core_ops,
 654         .video = &csid_video_ops,
 655         .pad = &csid_pad_ops,
 656 };
 657
 658 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops;
 659
 660 static const struct media_entity_operations csid_media_ops = {
 661         .link_setup = csid_link_setup,
 662         .link_validate = v4l2_subdev_link_validate,
 663 };
 664
 665 /*
 666  * msm_csid_register_entities - Register subdev node for CSID module
 667  * @csid: CSID device
 668  * @v4l2_dev: V4L2 device
 669  *
 670  * Return 0 on success or a negative error code otherwise
 671  */
 672 int msm_csid_register_entities(struct csid_device *csid,
 673                                struct v4l2_device *v4l2_dev)
 674 {
 675         struct v4l2_subdev *sd = &csid->subdev;
 676         struct media_pad *pads = csid->pads;
 677         int ret;
 678
 679         v4l2_subdev_init(sd, &csid_v4l2_ops);
 680         sd->internal_ops = &csid_v4l2_internal_ops;
 681         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 682         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
 683                  MSM_CSID_NAME, csid->id);
 684         v4l2_set_subdevdata(sd, csid);
 685
 686         v4l2_ctrl_handler_init(&csid->ctrls, 1);
 687         csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
 688                                 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
 689                                 ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0,
 690                                 csid_test_pattern_menu);
 691
 692         if (csid->ctrls.error) {
 693                 dev_err(csid->camss->dev, "failed to init ctrl: %d\n",
 694                         csid->ctrls.error);
 695                 ret = csid->ctrls.error;
 696                 goto free_ctrl;
 697         }
 698
 699         csid->subdev.ctrl_handler = &csid->ctrls;
 700
 701         pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 702         pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
 703
 704         sd->entity.ops = &csid_media_ops;
 705         ret = media_entity_init(&sd->entity, MSM_CSID_PADS_NUM, pads, 0);
 706         if (ret < 0) {
 707                 dev_err(csid->camss->dev, "failed to init media entity");
 708                 goto free_ctrl;
 709         }
 710
 711         ret = v4l2_device_register_subdev(v4l2_dev, sd);
 712         if (ret < 0) {
 713                 dev_err(csid->camss->dev, "failed to register subdev");
 714                 goto media_cleanup;
 715         }
 716
 717         return 0;
 718
 719 media_cleanup:
 720         media_entity_cleanup(&sd->entity);
 721 free_ctrl:
 722         v4l2_ctrl_handler_free(&csid->ctrls);
 723
 724         return ret;
 725 }
 726
 727 /*
 728  * msm_csid_unregister_entities - Unregister CSID module subdev node
 729  * @csid: CSID device
 730  */
 731 void msm_csid_unregister_entities(struct csid_device *csid)
 732 {
 733         v4l2_device_unregister_subdev(&csid->subdev);
 734         v4l2_ctrl_handler_free(&csid->ctrls);
 735 }