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

   1 /*
   2  * ispif.c
   3  *
   4  * Qualcomm MSM Camera Subsystem - ISPIF Module
   5  *
   6  * Copyright (c) 2013-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/iopoll.h>
  22 #include <linux/platform_device.h>
  23 #include <media/media-entity.h>
  24 #include <media/v4l2-device.h>
  25 #include <media/v4l2-subdev.h>
  26
  27 #include "ispif.h"
  28 #include "camss.h"
  29
  30 #define MSM_ISPIF_NAME "msm_ispif"
  31
  32 #define ISPIF_RST_CMD_0                 0x008
  33 #define ISPIF_IRQ_GLOBAL_CLEAR_CMD      0x01c
  34 #define ISPIF_VFE_m_CTRL_0(m)           (0x200 + 0x200 * (m))
  35 #define ISPIF_VFE_m_CTRL_0_PIX0_LINE_BUF_EN     (1 << 6)
  36 #define ISPIF_VFE_m_IRQ_MASK_0(m)       (0x208 + 0x200 * (m))
  37 #define ISPIF_VFE_m_IRQ_MASK_0_ENABLE   0x0a493249
  38 #define ISPIF_VFE_m_IRQ_MASK_1(m)       (0x20c + 0x200 * (m))
  39 #define ISPIF_VFE_m_IRQ_MASK_1_ENABLE   0x02493249
  40 #define ISPIF_VFE_m_IRQ_MASK_2(m)       (0x210 + 0x200 * (m))
  41 #define ISPIF_VFE_m_IRQ_MASK_2_ENABLE   0x00001249
  42 #define ISPIF_VFE_m_IRQ_STATUS_0(m)     (0x21c + 0x200 * (m))
  43 #define ISPIF_VFE_m_IRQ_STATUS_1(m)     (0x220 + 0x200 * (m))
  44 #define ISPIF_VFE_m_IRQ_STATUS_2(m)     (0x224 + 0x200 * (m))
  45 #define ISPIF_VFE_m_IRQ_CLEAR_0(m)      (0x230 + 0x200 * (m))
  46 #define ISPIF_VFE_m_IRQ_CLEAR_1(m)      (0x234 + 0x200 * (m))
  47 #define ISPIF_VFE_m_IRQ_CLEAR_2(m)      (0x238 + 0x200 * (m))
  48 #define ISPIF_VFE_m_INTF_INPUT_SEL(m)   (0x244 + 0x200 * (m))
  49 #define ISPIF_VFE_m_INTF_CMD_0(m)       (0x248 + 0x200 * (m))
  50 #define ISPIF_VFE_m_INTF_CMD_1(m)       (0x24c + 0x200 * (m))
  51 #define ISPIF_VFE_m_PIX_INTF_n_CID_MASK(m, n)   (0x254 + 0x200 * (m) + 0x4 * (n))
  52 #define ISPIF_VFE_m_RDI_INTF_n_CID_MASK(m, n)   (0x264 + 0x200 * (m) + 0x4 * (n))
  53 #define ISPIF_VFE_m_PIX_INTF_n_STATUS(m, n)     (0x2c0 + 0x200 * (m) + 0x4 * (n))
  54 #define ISPIF_VFE_m_RDI_INTF_n_STATUS(m, n)     (0x2d0 + 0x200 * (m) + 0x4 * (n))
  55
  56 #define CSI_RDI_CLK_MUX_SEL             0x008
  57
  58 #define ISPIF_TIMEOUT_SLEEP_US          1000
  59 #define ISPIF_TIMEOUT_ALL_US            1000000
  60
  61 enum ispif_intf {
  62         PIX0,
  63         RDI0,
  64         PIX1,
  65         RDI1,
  66         RDI2
  67 };
  68
  69 enum ispif_intf_cmd {
  70         CMD_DISABLE_FRAME_BOUNDARY = 0x0,
  71         CMD_ENABLE_FRAME_BOUNDARY = 0x1,
  72         CMD_DISABLE_IMMEDIATELY = 0x2,
  73         CMD_ALL_DISABLE_IMMEDIATELY = 0xaaaaaaaa,
  74         CMD_ALL_NO_CHANGE = 0xffffffff,
  75 };
  76
  77 /*
  78  * ispif_isr - ISPIF module interrupt handler
  79  * @irq: Interrupt line
  80  * @dev: ISPIF device
  81  *
  82  * Return IRQ_HANDLED on success
  83  */
  84 static irqreturn_t ispif_isr(int irq, void *dev)
  85 {
  86         struct ispif_device *ispif = dev;
  87         u32 value0, value1, value2;
  88
  89         value0 = readl(ispif->base + ISPIF_VFE_m_IRQ_STATUS_0(0));
  90         value1 = readl(ispif->base + ISPIF_VFE_m_IRQ_STATUS_1(0));
  91         value2 = readl(ispif->base + ISPIF_VFE_m_IRQ_STATUS_2(0));
  92
  93         writel(value0, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_0(0));
  94         writel(value1, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_1(0));
  95         writel(value2, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_2(0));
  96
  97         wmb();
  98         writel(0x1, ispif->base + ISPIF_IRQ_GLOBAL_CLEAR_CMD);
  99         wmb();
 100
 101         if ((value0 >> 27) & 0x1)
 102                 complete(&ispif->reset_complete);
 103
 104         return IRQ_HANDLED;
 105 }
 106
 107 /*
 108  * ispif_enable_clocks - Enable clocks for ISPIF module
 109  *
 110  * Return 0 on success or a negative error code otherwise
 111  */
 112 static int ispif_enable_clocks(int nclocks, struct clk **clock,
 113                                u8 *clock_for_reset, u8 reset)
 114 {
 115         int ret;
 116         int i;
 117
 118         for (i = 0; i < nclocks; i++) {
 119                 if (clock_for_reset[i] == reset) {
 120                         ret = clk_prepare_enable(clock[i]);
 121                         if (ret) {
 122                                 pr_err("clock enable failed\n");
 123                                 goto error;
 124                         }
 125                 }
 126         }
 127
 128         return 0;
 129
 130 error:
 131         for (i--; i >= 0; i--)
 132                 if (clock_for_reset[i] == reset)
 133                         clk_disable_unprepare(clock[i]);
 134
 135         return ret;
 136 }
 137
 138 /*
 139  * ispif_disable_clocks - Disable clocks for ISPIF module
 140  */
 141 static void ispif_disable_clocks(int nclocks, struct clk **clock,
 142                                  u8 *clock_for_reset, u8 reset)
 143 {
 144         int i;
 145
 146         for (i = nclocks - 1; i >= 0; i--)
 147                 if (clock_for_reset[i] == reset)
 148                         clk_disable_unprepare(clock[i]);
 149 }
 150
 151 /*
 152  * ispif_set_power - Power on/off ISPIF module
 153  * @sd: ISPIF V4L2 subdevice
 154  * @on: Requested power state
 155  *
 156  * Return 0 on success or a negative error code otherwise
 157  */
 158 static int ispif_set_power(struct v4l2_subdev *sd, int on)
 159 {
 160         struct ispif_device *ispif = v4l2_get_subdevdata(sd);
 161         int ret = 0;
 162
 163         dev_err(ispif->camss->dev, "%s: Enter, on = %d\n",
 164                 __func__, on);
 165
 166         if (on)
 167                 ret = ispif_enable_clocks(ispif->nclocks, ispif->clock,
 168                                           ispif->clock_for_reset, 0);
 169         else
 170                 ispif_disable_clocks(ispif->nclocks, ispif->clock,
 171                                      ispif->clock_for_reset, 0);
 172
 173         dev_err(ispif->camss->dev, "%s: Exit, on = %d\n",
 174                 __func__, on);
 175
 176         return ret;
 177 }
 178
 179 static int ispif_reset(struct ispif_device *ispif)
 180 {
 181         int ret;
 182
 183         ret = ispif_enable_clocks(ispif->nclocks, ispif->clock,
 184                                   ispif->clock_for_reset, 1);
 185         if (ret < 0)
 186                 goto exit;
 187
 188         writel(0xfe0f1fff, ispif->base + ISPIF_RST_CMD_0);
 189         wait_for_completion(&ispif->reset_complete);
 190
 191         ispif_disable_clocks(ispif->nclocks, ispif->clock,
 192                              ispif->clock_for_reset, 1);
 193
 194 exit:
 195         return ret;
 196 }
 197
 198 static void ispif_reset_sw(struct ispif_device *ispif, u8 vfe)
 199 {
 200
 201         writel_relaxed(ISPIF_VFE_m_CTRL_0_PIX0_LINE_BUF_EN,
 202                        ispif->base + ISPIF_VFE_m_CTRL_0(vfe));
 203         writel_relaxed(0, ispif->base + ISPIF_VFE_m_IRQ_MASK_0(vfe));
 204         writel_relaxed(0, ispif->base + ISPIF_VFE_m_IRQ_MASK_1(vfe));
 205         writel_relaxed(0, ispif->base + ISPIF_VFE_m_IRQ_MASK_2(vfe));
 206         writel_relaxed(0xffffffff, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_0(vfe));
 207         writel_relaxed(0xffffffff, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_1(vfe));
 208         writel_relaxed(0xffffffff, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_2(vfe));
 209
 210         writel_relaxed(0, ispif->base + ISPIF_VFE_m_INTF_INPUT_SEL(vfe));
 211
 212         writel_relaxed(CMD_ALL_NO_CHANGE,
 213                        ispif->base + ISPIF_VFE_m_INTF_CMD_0(vfe));
 214         writel_relaxed(CMD_ALL_NO_CHANGE,
 215                        ispif->base + ISPIF_VFE_m_INTF_CMD_1(vfe));
 216
 217         writel_relaxed(0,
 218                        ispif->base + ISPIF_VFE_m_PIX_INTF_n_CID_MASK(vfe, 0));
 219         writel_relaxed(0,
 220                        ispif->base + ISPIF_VFE_m_PIX_INTF_n_CID_MASK(vfe, 1));
 221         writel_relaxed(0,
 222                        ispif->base + ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 0));
 223         writel_relaxed(0,
 224                        ispif->base + ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 1));
 225         writel_relaxed(0,
 226                        ispif->base + ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 2));
 227
 228         wmb();
 229         writel_relaxed(0x1, ispif->base + ISPIF_IRQ_GLOBAL_CLEAR_CMD);
 230         wmb();
 231 }
 232
 233 static void ispif_select_clk_mux(struct ispif_device *ispif,
 234                                  enum ispif_intf intf, u8 csid, u8 vfe)
 235 {
 236         u32 val = 0;
 237
 238         switch (intf) {
 239         case PIX0:
 240                 val = readl_relaxed(ispif->base_clk_mux);
 241                 val &= ~(0xf << (vfe * 8));
 242                 val |= (csid << (vfe * 8));
 243                 writel_relaxed(val, ispif->base_clk_mux);
 244                 break;
 245
 246         case RDI0:
 247                 val = readl_relaxed(ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
 248                 val &= ~(0xf << (vfe * 12));
 249                 val |= (csid << (vfe * 12));
 250                 writel_relaxed(val, ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
 251                 break;
 252
 253         case PIX1:
 254                 val = readl_relaxed(ispif->base_clk_mux);
 255                 val &= ~(0xf << (4 + (vfe * 8)));
 256                 val |= (csid << (4 + (vfe * 8)));
 257                 writel_relaxed(val, ispif->base_clk_mux);
 258                 break;
 259
 260         case RDI1:
 261                 val = readl_relaxed(ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
 262                 val &= ~(0xf << (4 + (vfe * 12)));
 263                 val |= (csid << (4 + (vfe * 12)));
 264                 writel_relaxed(val, ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
 265                 break;
 266
 267         case RDI2:
 268                 val = readl_relaxed(ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
 269                 val &= ~(0xf << (8 + (vfe * 12)));
 270                 val |= (csid << (8 + (vfe * 12)));
 271                 writel_relaxed(val, ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
 272                 break;
 273         }
 274
 275         mb();
 276 }
 277
 278 static int ispif_validate_intf_status(struct ispif_device *ispif,
 279                                       enum ispif_intf intf, u8 vfe)
 280 {
 281         int ret = 0;
 282         u32 val;
 283
 284         switch (intf) {
 285         case PIX0:
 286                 val = readl_relaxed(ispif->base +
 287                         ISPIF_VFE_m_PIX_INTF_n_STATUS(vfe, 0));
 288                 break;
 289         case RDI0:
 290                 val = readl_relaxed(ispif->base +
 291                         ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 0));
 292                 break;
 293         case PIX1:
 294                 val = readl_relaxed(ispif->base +
 295                         ISPIF_VFE_m_PIX_INTF_n_STATUS(vfe, 1));
 296                 break;
 297         case RDI1:
 298                 val = readl_relaxed(ispif->base +
 299                         ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 1));
 300                 break;
 301         case RDI2:
 302                 val = readl_relaxed(ispif->base +
 303                         ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 2));
 304                 break;
 305         }
 306
 307         if ((val & 0xf) != 0xf)
 308                 ret = -EBUSY;
 309
 310         return ret;
 311 }
 312
 313 static void ispif_select_csid(struct ispif_device *ispif,
 314                               enum ispif_intf intf, u8 csid, u8 vfe)
 315 {
 316         u32 val;
 317
 318         val = readl_relaxed(ispif->base + ISPIF_VFE_m_INTF_INPUT_SEL(vfe));
 319         switch (intf) {
 320         case PIX0:
 321                 val &= ~(BIT(1) | BIT(0));
 322                 val |= csid;
 323                 break;
 324         case RDI0:
 325                 val &= ~(BIT(5) | BIT(4));
 326                 val |= (csid << 4);
 327                 break;
 328         case PIX1:
 329                 val &= ~(BIT(9) | BIT(8));
 330                 val |= (csid << 8);
 331                 break;
 332         case RDI1:
 333                 val &= ~(BIT(13) | BIT(12));
 334                 val |= (csid << 12);
 335                 break;
 336         case RDI2:
 337                 val &= ~(BIT(21) | BIT(20));
 338                 val |= (csid << 20);
 339                 break;
 340         }
 341
 342         wmb();
 343         writel_relaxed(val, ispif->base + ISPIF_VFE_m_INTF_INPUT_SEL(vfe));
 344         wmb();
 345 }
 346
 347 static void ispif_enable_cid(struct ispif_device *ispif, enum ispif_intf intf,
 348                              u16 cid_mask, u8 vfe, u8 enable)
 349 {
 350         u32 addr, val;
 351
 352         switch (intf) {
 353         case PIX0:
 354                 addr = ISPIF_VFE_m_PIX_INTF_n_CID_MASK(vfe, 0);
 355                 break;
 356         case RDI0:
 357                 addr = ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 0);
 358                 break;
 359         case PIX1:
 360                 addr = ISPIF_VFE_m_PIX_INTF_n_CID_MASK(vfe, 1);
 361                 break;
 362         case RDI1:
 363                 addr = ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 1);
 364                 break;
 365         case RDI2:
 366                 addr = ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 2);
 367                 break;
 368         }
 369
 370         val = readl_relaxed(ispif->base + addr);
 371         if (enable)
 372                 val |= cid_mask;
 373         else
 374                 val &= ~cid_mask;
 375
 376         wmb();
 377         writel_relaxed(val, ispif->base + addr);
 378         wmb();
 379 }
 380
 381 static void ispif_config_irq(struct ispif_device *ispif, u8 vfe)
 382 {
 383         u32 val;
 384
 385         val = ISPIF_VFE_m_IRQ_MASK_0_ENABLE;
 386         writel(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_0(vfe));
 387         writel(val, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_0(vfe));
 388         val = ISPIF_VFE_m_IRQ_MASK_1_ENABLE;
 389         writel(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_1(vfe));
 390         writel(val, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_1(vfe));
 391         val = ISPIF_VFE_m_IRQ_MASK_2_ENABLE;
 392         writel(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_2(vfe));
 393         writel(val, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_2(vfe));
 394         wmb();
 395         writel(0x1, ispif->base + ISPIF_IRQ_GLOBAL_CLEAR_CMD);
 396         wmb();
 397 }
 398
 399 static void ispif_intf_cmd(struct ispif_device *ispif, u8 cmd,
 400                            enum ispif_intf intf, u8 vfe, u8 vc)
 401 {
 402         u32 val = CMD_ALL_NO_CHANGE;
 403
 404         if (intf == RDI2) {
 405                 val &= ~(0x3 << (vc * 2 + 8));
 406                 val |= (cmd << (vc * 2 + 8));
 407                 wmb();
 408                 writel_relaxed(val, ispif->base + ISPIF_VFE_m_INTF_CMD_1(vfe));
 409                 wmb();
 410         } else {
 411                 val &= ~(0x3 << (vc * 2 + intf * 8));
 412                 val |= (cmd << (vc * 2 + intf * 8));
 413                 wmb();
 414                 writel_relaxed(val, ispif->base + ISPIF_VFE_m_INTF_CMD_0(vfe));
 415                 wmb();
 416         }
 417 }
 418
 419 /*
 420  * ispif_set_stream - Enable/disable streaming on ISPIF module
 421  * @sd: ISPIF V4L2 subdevice
 422  * @enable: Requested streaming state
 423  *
 424  * Main configuration of ISPIF module is also done here.
 425  *
 426  * Return 0 on success or a negative error code otherwise
 427  */
 428 static int ispif_set_stream(struct v4l2_subdev *sd, int enable)
 429 {
 430         struct ispif_device *ispif = v4l2_get_subdevdata(sd);
 431         enum ispif_intf ispif_intf = RDI0;
 432         u8 vfe = 0;
 433         u8 vc = 0; /* TODO: How to get this from sensor? */
 434         u8 cid = vc * 4;
 435
 436         int ret;
 437
 438         dev_err(ispif->camss->dev, "%s: Enter, enable = %d\n",
 439                 __func__, enable);
 440
 441         if (enable) {
 442                 u8 csid = ispif->csid_id;
 443
 444                 if (!media_entity_remote_pad(
 445                                         &ispif->pads[MSM_ISPIF_PAD_SINK])) {
 446                         return -ENOLINK;
 447                 }
 448
 449                 /* Reset */
 450
 451                 ret = ispif_reset(ispif);
 452                 if (ret < 0)
 453                         return ret;
 454
 455                 /* Config */
 456
 457                 ispif_reset_sw(ispif, vfe);
 458
 459                 ispif_select_clk_mux(ispif, ispif_intf, csid, vfe);
 460
 461                 ret = ispif_validate_intf_status(ispif, ispif_intf, vfe);
 462                 if (ret < 0)
 463                         return ret;
 464
 465                 ispif_select_csid(ispif, ispif_intf, csid, vfe);
 466
 467                 ispif_enable_cid(ispif, ispif_intf, 1 << cid, vfe, 1);
 468
 469                 ispif_config_irq(ispif, vfe);
 470
 471                 ispif_intf_cmd(ispif, CMD_ENABLE_FRAME_BOUNDARY, ispif_intf, vfe, vc);
 472         } else {
 473                 u32 stop_flag = 0;
 474
 475                 ispif_intf_cmd(ispif, CMD_DISABLE_FRAME_BOUNDARY, ispif_intf, vfe, vc);
 476
 477                 ret = readl_poll_timeout(ispif->base + ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 0),
 478                                          stop_flag,
 479                                          (stop_flag & 0xf) == 0xf,
 480                                          ISPIF_TIMEOUT_SLEEP_US,
 481                                          ISPIF_TIMEOUT_ALL_US);
 482                 if (ret < 0)
 483                         return ret;
 484
 485                 ispif_enable_cid(ispif, ispif_intf, 1 << cid, vfe, 0);
 486         }
 487
 488         return 0;
 489 }
 490
 491 /*
 492  * msm_ispif_subdev_init - Initialize ISPIF device structure and resources
 493  * @ispif: ISPIF device
 494  * @camss: Camera sub-system structure
 495  * @res: ISPIF module resources table
 496  *
 497  * Return 0 on success or a negative error code otherwise
 498  */
 499 int msm_ispif_subdev_init(struct ispif_device *ispif, struct camss *camss,
 500                           struct resources_ispif *res)
 501 {
 502         struct device *dev = camss->dev;
 503         struct platform_device *pdev = container_of(dev, struct platform_device, dev);
 504         struct resource *r;
 505         int i;
 506         int ret;
 507
 508         ispif->camss = camss;
 509
 510         /* Memory */
 511
 512         r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
 513         ispif->base = devm_ioremap_resource(dev, r);
 514         if (IS_ERR(ispif->base)) {
 515                 dev_err(dev, "could not map memory\n");
 516                 return PTR_ERR(ispif->base);
 517         }
 518
 519         r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[1]);
 520         ispif->base_clk_mux = devm_ioremap_resource(dev, r);
 521         if (IS_ERR(ispif->base_clk_mux)) {
 522                 dev_err(dev, "could not map memory\n");
 523                 return PTR_ERR(ispif->base_clk_mux);
 524         }
 525
 526         /* Interrupt */
 527
 528         r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, res->interrupt);
 529         ispif->irq = r->start;
 530         if (IS_ERR_VALUE(ispif->irq))
 531                 return ispif->irq;
 532
 533         ret = devm_request_irq(dev, ispif->irq, ispif_isr,
 534                                IRQF_TRIGGER_RISING, "ispif", ispif);
 535         if (ret < 0) {
 536                 dev_err(dev, "request_irq failed\n");
 537                 return ret;
 538         }
 539
 540         /* Clocks */
 541
 542         i = 0;
 543         ispif->nclocks = 0;
 544         while (res->clock[i++])
 545                 ispif->nclocks++;
 546
 547         ispif->clock = devm_kzalloc(dev, ispif->nclocks * sizeof(*ispif->clock),
 548                                     GFP_KERNEL);
 549         if (!ispif->clock) {
 550                 dev_err(dev, "could not allocate memory\n");
 551                 return -ENOMEM;
 552         }
 553
 554         ispif->clock_for_reset = devm_kzalloc(dev, ispif->nclocks *
 555                         sizeof(*ispif->clock_for_reset), GFP_KERNEL);
 556         if (!ispif->clock_for_reset) {
 557                 dev_err(dev, "could not allocate memory\n");
 558                 return -ENOMEM;
 559         }
 560
 561         for (i = 0; i < ispif->nclocks; i++) {
 562                 ispif->clock[i] = devm_clk_get(dev, res->clock[i]);
 563                 if (IS_ERR(ispif->clock[i]))
 564                         return PTR_ERR(ispif->clock[i]);
 565                 ispif->clock_for_reset[i] = res->clock_for_reset[i];
 566         }
 567
 568         init_completion(&ispif->reset_complete);
 569
 570         return 0;
 571 }
 572
 573 static int ispif_link_setup(struct media_entity *entity,
 574                            const struct media_pad *local,
 575                            const struct media_pad *remote, u32 flags)
 576 {
 577         if ((local->flags & MEDIA_PAD_FL_SINK) &&
 578             (flags & MEDIA_LNK_FL_ENABLED)) {
 579                 struct v4l2_subdev *sd;
 580                 struct ispif_device *ispif;
 581                 struct csid_device *csid;
 582
 583                 sd = container_of(entity, struct v4l2_subdev, entity);
 584                 ispif = v4l2_get_subdevdata(sd);
 585
 586                 sd = container_of(remote->entity, struct v4l2_subdev, entity);
 587                 csid = v4l2_get_subdevdata(sd);
 588
 589                 ispif->csid_id = csid->id;
 590         }
 591
 592         return 0;
 593 }
 594
 595 static const struct v4l2_subdev_core_ops ispif_core_ops = {
 596         .s_power = ispif_set_power,
 597 };
 598
 599 static const struct v4l2_subdev_video_ops ispif_video_ops = {
 600         .s_stream = ispif_set_stream,
 601 };
 602
 603 static const struct v4l2_subdev_pad_ops ispif_pad_ops;
 604
 605 static const struct v4l2_subdev_ops ispif_v4l2_ops = {
 606         .core = &ispif_core_ops,
 607         .video = &ispif_video_ops,
 608         .pad = &ispif_pad_ops,
 609 };
 610
 611 static const struct v4l2_subdev_internal_ops ispif_v4l2_internal_ops;
 612
 613 static const struct media_entity_operations ispif_media_ops = {
 614         .link_setup = ispif_link_setup,
 615 };
 616
 617 /*
 618  * msm_ispif_register_entities - Register subdev node for ISPIF module
 619  * @ispif: ISPIF device
 620  * @v4l2_dev: V4L2 device
 621  *
 622  * Return 0 on success or a negative error code otherwise
 623  */
 624 int msm_ispif_register_entities(struct ispif_device *ispif,
 625                                 struct v4l2_device *v4l2_dev)
 626 {
 627         struct v4l2_subdev *sd = &ispif->subdev;
 628         struct media_pad *pads = ispif->pads;
 629         int ret;
 630
 631         v4l2_subdev_init(sd, &ispif_v4l2_ops);
 632         sd->internal_ops = &ispif_v4l2_internal_ops;
 633         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 634         snprintf(sd->name, ARRAY_SIZE(sd->name), MSM_ISPIF_NAME);
 635         v4l2_set_subdevdata(sd, ispif);
 636
 637         pads[MSM_ISPIF_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 638         pads[MSM_ISPIF_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
 639
 640         sd->entity.ops = &ispif_media_ops;
 641         ret = media_entity_init(&sd->entity, MSM_ISPIF_PADS_NUM, pads, 0);
 642         if (ret < 0) {
 643                 pr_err("Fail to init media entity");
 644                 return ret;
 645         }
 646
 647         ret = v4l2_device_register_subdev(v4l2_dev, sd);
 648         if (ret < 0) {
 649                 pr_err("Fail to register subdev");
 650                 media_entity_cleanup(&sd->entity);
 651         }
 652
 653         return ret;
 654 }
 655
 656 /*
 657  * msm_ispif_unregister_entities - Unregister ISPIF module subdev node
 658  * @ispif: ISPIF device
 659  */
 660 void msm_ispif_unregister_entities(struct ispif_device *ispif)
 661 {
 662         v4l2_device_unregister_subdev(&ispif->subdev);
 663 }