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

   1 /*
   2  * camss-csid.c
   3  *
   4  * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
   5  *
   6  * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
   7  * Copyright (C) 2015-2017 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/kernel.h>
  22 #include <linux/of.h>
  23 #include <linux/platform_device.h>
  24 #include <linux/regulator/consumer.h>
  25 #include <media/media-entity.h>
  26 #include <media/v4l2-device.h>
  27 #include <media/v4l2-subdev.h>
  28
  29 #include "camss-csid.h"
  30 #include "camss.h"
  31
  32 #define MSM_CSID_NAME "msm_csid"
  33
  34 #define CAMSS_CSID_HW_VERSION           0x0
  35 #define CAMSS_CSID_CORE_CTRL_0          0x004
  36 #define CAMSS_CSID_CORE_CTRL_1          0x008
  37 #define CAMSS_CSID_RST_CMD              0x00c
  38 #define CAMSS_CSID_CID_LUT_VC_n(n)      (0x010 + 0x4 * (n))
  39 #define CAMSS_CSID_CID_n_CFG(n)         (0x020 + 0x4 * (n))
  40 #define CAMSS_CSID_IRQ_CLEAR_CMD        0x060
  41 #define CAMSS_CSID_IRQ_MASK             0x064
  42 #define CAMSS_CSID_IRQ_STATUS           0x068
  43 #define CAMSS_CSID_TG_CTRL              0x0a0
  44 #define CAMSS_CSID_TG_CTRL_DISABLE      0xa06436
  45 #define CAMSS_CSID_TG_CTRL_ENABLE       0xa06437
  46 #define CAMSS_CSID_TG_VC_CFG            0x0a4
  47 #define CAMSS_CSID_TG_VC_CFG_H_BLANKING         0x3ff
  48 #define CAMSS_CSID_TG_VC_CFG_V_BLANKING         0x7f
  49 #define CAMSS_CSID_TG_DT_n_CGG_0(n)     (0x0ac + 0xc * (n))
  50 #define CAMSS_CSID_TG_DT_n_CGG_1(n)     (0x0b0 + 0xc * (n))
  51 #define CAMSS_CSID_TG_DT_n_CGG_2(n)     (0x0b4 + 0xc * (n))
  52
  53 #define DATA_TYPE_EMBEDDED_DATA_8BIT    0x12
  54 #define DATA_TYPE_YUV422_8BIT           0x1e
  55 #define DATA_TYPE_RAW_6BIT              0x28
  56 #define DATA_TYPE_RAW_8BIT              0x2a
  57 #define DATA_TYPE_RAW_10BIT             0x2b
  58 #define DATA_TYPE_RAW_12BIT             0x2c
  59
  60 #define DECODE_FORMAT_UNCOMPRESSED_6_BIT        0x0
  61 #define DECODE_FORMAT_UNCOMPRESSED_8_BIT        0x1
  62 #define DECODE_FORMAT_UNCOMPRESSED_10_BIT       0x2
  63 #define DECODE_FORMAT_UNCOMPRESSED_12_BIT       0x3
  64
  65 #define CSID_RESET_TIMEOUT_MS 500
  66
  67 struct csid_fmts {
  68         u32 code;
  69         u8 data_type;
  70         u8 decode_format;
  71         u8 bpp;
  72         u8 spp; /* bus samples per pixel */
  73 };
  74
  75 static const struct csid_fmts csid_input_fmts[] = {
  76         {
  77                 MEDIA_BUS_FMT_UYVY8_2X8,
  78                 DATA_TYPE_YUV422_8BIT,
  79                 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
  80                 8,
  81                 2,
  82         },
  83         {
  84                 MEDIA_BUS_FMT_VYUY8_2X8,
  85                 DATA_TYPE_YUV422_8BIT,
  86                 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
  87                 8,
  88                 2,
  89         },
  90         {
  91                 MEDIA_BUS_FMT_YUYV8_2X8,
  92                 DATA_TYPE_YUV422_8BIT,
  93                 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
  94                 8,
  95                 2,
  96         },
  97         {
  98                 MEDIA_BUS_FMT_YVYU8_2X8,
  99                 DATA_TYPE_YUV422_8BIT,
 100                 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
 101                 8,
 102                 2,
 103         },
 104         {
 105                 MEDIA_BUS_FMT_SBGGR8_1X8,
 106                 DATA_TYPE_RAW_8BIT,
 107                 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
 108                 8,
 109                 1,
 110         },
 111         {
 112                 MEDIA_BUS_FMT_SGBRG8_1X8,
 113                 DATA_TYPE_RAW_8BIT,
 114                 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
 115                 8,
 116                 1,
 117         },
 118         {
 119                 MEDIA_BUS_FMT_SGRBG8_1X8,
 120                 DATA_TYPE_RAW_8BIT,
 121                 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
 122                 8,
 123                 1,
 124         },
 125         {
 126                 MEDIA_BUS_FMT_SRGGB8_1X8,
 127                 DATA_TYPE_RAW_8BIT,
 128                 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
 129                 8,
 130                 1,
 131         },
 132         {
 133                 MEDIA_BUS_FMT_SBGGR10_1X10,
 134                 DATA_TYPE_RAW_10BIT,
 135                 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
 136                 10,
 137                 1,
 138         },
 139         {
 140                 MEDIA_BUS_FMT_SGBRG10_1X10,
 141                 DATA_TYPE_RAW_10BIT,
 142                 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
 143                 10,
 144                 1,
 145         },
 146         {
 147                 MEDIA_BUS_FMT_SGRBG10_1X10,
 148                 DATA_TYPE_RAW_10BIT,
 149                 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
 150                 10,
 151                 1,
 152         },
 153         {
 154                 MEDIA_BUS_FMT_SRGGB10_1X10,
 155                 DATA_TYPE_RAW_10BIT,
 156                 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
 157                 10,
 158                 1,
 159         },
 160         {
 161                 MEDIA_BUS_FMT_SBGGR12_1X12,
 162                 DATA_TYPE_RAW_12BIT,
 163                 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
 164                 12,
 165                 1,
 166         },
 167         {
 168                 MEDIA_BUS_FMT_SGBRG12_1X12,
 169                 DATA_TYPE_RAW_12BIT,
 170                 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
 171                 12,
 172                 1,
 173         },
 174         {
 175                 MEDIA_BUS_FMT_SGRBG12_1X12,
 176                 DATA_TYPE_RAW_12BIT,
 177                 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
 178                 12,
 179                 1,
 180         },
 181         {
 182                 MEDIA_BUS_FMT_SRGGB12_1X12,
 183                 DATA_TYPE_RAW_12BIT,
 184                 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
 185                 12,
 186                 1,
 187         }
 188 };
 189
 190 static const struct csid_fmts *csid_get_fmt_entry(u32 code)
 191 {
 192         unsigned int i;
 193
 194         for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
 195                 if (code == csid_input_fmts[i].code)
 196                         return &csid_input_fmts[i];
 197
 198         WARN(1, "Unknown format\n");
 199
 200         return &csid_input_fmts[0];
 201 }
 202
 203 /*
 204  * csid_isr - CSID module interrupt handler
 205  * @irq: Interrupt line
 206  * @dev: CSID device
 207  *
 208  * Return IRQ_HANDLED on success
 209  */
 210 static irqreturn_t csid_isr(int irq, void *dev)
 211 {
 212         struct csid_device *csid = dev;
 213         u32 value;
 214
 215         value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS);
 216         writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
 217
 218         if ((value >> 11) & 0x1)
 219                 complete(&csid->reset_complete);
 220
 221         return IRQ_HANDLED;
 222 }
 223
 224 /*
 225  * csid_set_clock_rates - Calculate and set clock rates on CSID module
 226  * @csiphy: CSID device
 227  */
 228 static int csid_set_clock_rates(struct csid_device *csid)
 229 {
 230         struct device *dev = to_device_index(csid, csid->id);
 231         u32 pixel_clock;
 232         int i, j;
 233         int ret;
 234
 235         ret = camss_get_pixel_clock(&csid->subdev.entity, &pixel_clock);
 236         if (ret)
 237                 pixel_clock = 0;
 238
 239         for (i = 0; i < csid->nclocks; i++) {
 240                 struct camss_clock *clock = &csid->clock[i];
 241
 242                 if (!strcmp(clock->name, "csi0") ||
 243                         !strcmp(clock->name, "csi1")) {
 244                         u8 bpp = csid_get_fmt_entry(
 245                                 csid->fmt[MSM_CSIPHY_PAD_SINK].code)->bpp;
 246                         u8 num_lanes = csid->phy.lane_cnt;
 247                         u64 min_rate = pixel_clock * bpp / (2 * num_lanes * 4);
 248                         long rate;
 249
 250                         camss_add_clock_margin(&min_rate);
 251
 252                         for (j = 0; j < clock->nfreqs; j++)
 253                                 if (min_rate < clock->freq[j])
 254                                         break;
 255
 256                         if (j == clock->nfreqs) {
 257                                 dev_err(dev,
 258                                         "Pixel clock is too high for CSID\n");
 259                                 return -EINVAL;
 260                         }
 261
 262                         /* if sensor pixel clock is not available */
 263                         /* set highest possible CSID clock rate */
 264                         if (min_rate == 0)
 265                                 j = clock->nfreqs - 1;
 266
 267                         rate = clk_round_rate(clock->clk, clock->freq[j]);
 268                         if (rate < 0) {
 269                                 dev_err(dev, "clk round rate failed: %ld\n",
 270                                         rate);
 271                                 return -EINVAL;
 272                         }
 273
 274                         ret = clk_set_rate(clock->clk, rate);
 275                         if (ret < 0) {
 276                                 dev_err(dev, "clk set rate failed: %d\n", ret);
 277                                 return ret;
 278                         }
 279                 }
 280         }
 281
 282         return 0;
 283 }
 284
 285 /*
 286  * csid_reset - Trigger reset on CSID module and wait to complete
 287  * @csid: CSID device
 288  *
 289  * Return 0 on success or a negative error code otherwise
 290  */
 291 static int csid_reset(struct csid_device *csid)
 292 {
 293         unsigned long time;
 294
 295         reinit_completion(&csid->reset_complete);
 296
 297         writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
 298
 299         time = wait_for_completion_timeout(&csid->reset_complete,
 300                 msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
 301         if (!time) {
 302                 dev_err(to_device_index(csid, csid->id),
 303                         "CSID reset timeout\n");
 304                 return -EIO;
 305         }
 306
 307         return 0;
 308 }
 309
 310 /*
 311  * csid_set_power - Power on/off CSID module
 312  * @sd: CSID V4L2 subdevice
 313  * @on: Requested power state
 314  *
 315  * Return 0 on success or a negative error code otherwise
 316  */
 317 static int csid_set_power(struct v4l2_subdev *sd, int on)
 318 {
 319         struct csid_device *csid = v4l2_get_subdevdata(sd);
 320         struct device *dev = to_device_index(csid, csid->id);
 321         int ret;
 322
 323         if (on) {
 324                 u32 hw_version;
 325
 326                 ret = regulator_enable(csid->vdda);
 327                 if (ret < 0)
 328                         return ret;
 329
 330                 ret = csid_set_clock_rates(csid);
 331                 if (ret < 0) {
 332                         regulator_disable(csid->vdda);
 333                         return ret;
 334                 }
 335
 336                 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
 337                 if (ret < 0) {
 338                         regulator_disable(csid->vdda);
 339                         return ret;
 340                 }
 341
 342                 enable_irq(csid->irq);
 343
 344                 ret = csid_reset(csid);
 345                 if (ret < 0) {
 346                         disable_irq(csid->irq);
 347                         camss_disable_clocks(csid->nclocks, csid->clock);
 348                         regulator_disable(csid->vdda);
 349                         return ret;
 350                 }
 351
 352                 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
 353                 dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version);
 354         } else {
 355                 disable_irq(csid->irq);
 356                 camss_disable_clocks(csid->nclocks, csid->clock);
 357                 ret = regulator_disable(csid->vdda);
 358         }
 359
 360         return ret;
 361 }
 362
 363 /*
 364  * csid_set_stream - Enable/disable streaming on CSID module
 365  * @sd: CSID V4L2 subdevice
 366  * @enable: Requested streaming state
 367  *
 368  * Main configuration of CSID module is also done here.
 369  *
 370  * Return 0 on success or a negative error code otherwise
 371  */
 372 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 373 {
 374         struct csid_device *csid = v4l2_get_subdevdata(sd);
 375         struct csid_testgen_config *tg = &csid->testgen;
 376         u32 val;
 377
 378         if (enable) {
 379                 u8 vc = 0; /* Virtual Channel 0 */
 380                 u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
 381                 u8 dt, dt_shift, df;
 382                 int ret;
 383
 384                 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
 385                 if (ret < 0) {
 386                         dev_err(to_device_index(csid, csid->id),
 387                                 "could not sync v4l2 controls: %d\n", ret);
 388                         return ret;
 389                 }
 390
 391                 if (!tg->enabled &&
 392                     !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
 393                         return -ENOLINK;
 394
 395                 if (tg->enabled) {
 396                         /* Config Test Generator */
 397                         struct v4l2_mbus_framefmt *f =
 398                                         &csid->fmt[MSM_CSID_PAD_SRC];
 399                         u8 bpp = csid_get_fmt_entry(f->code)->bpp;
 400                         u8 spp = csid_get_fmt_entry(f->code)->spp;
 401                         u32 num_bytes_per_line = f->width * bpp * spp / 8;
 402                         u32 num_lines = f->height;
 403
 404                         /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
 405                         /* 1:0 VC */
 406                         val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
 407                               ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
 408                         writel_relaxed(val, csid->base + CAMSS_CSID_TG_VC_CFG);
 409
 410                         /* 28:16 bytes per lines, 12:0 num of lines */
 411                         val = ((num_bytes_per_line & 0x1fff) << 16) |
 412                               (num_lines & 0x1fff);
 413                         writel_relaxed(val, csid->base +
 414                                        CAMSS_CSID_TG_DT_n_CGG_0(0));
 415
 416                         dt = csid_get_fmt_entry(
 417                                 csid->fmt[MSM_CSID_PAD_SRC].code)->data_type;
 418
 419                         /* 5:0 data type */
 420                         val = dt;
 421                         writel_relaxed(val, csid->base +
 422                                        CAMSS_CSID_TG_DT_n_CGG_1(0));
 423
 424                         /* 2:0 output test pattern */
 425                         val = tg->payload_mode;
 426                         writel_relaxed(val, csid->base +
 427                                        CAMSS_CSID_TG_DT_n_CGG_2(0));
 428
 429                         df = csid_get_fmt_entry(
 430                                 csid->fmt[MSM_CSID_PAD_SRC].code)->decode_format;
 431                 } else {
 432                         struct csid_phy_config *phy = &csid->phy;
 433
 434                         val = phy->lane_cnt - 1;
 435                         val |= phy->lane_assign << 4;
 436
 437                         writel_relaxed(val,
 438                                        csid->base + CAMSS_CSID_CORE_CTRL_0);
 439
 440                         val = phy->csiphy_id << 17;
 441                         val |= 0x9;
 442
 443                         writel_relaxed(val,
 444                                        csid->base + CAMSS_CSID_CORE_CTRL_1);
 445
 446                         dt = csid_get_fmt_entry(
 447                                 csid->fmt[MSM_CSID_PAD_SINK].code)->data_type;
 448                         df = csid_get_fmt_entry(
 449                                 csid->fmt[MSM_CSID_PAD_SINK].code)->decode_format;
 450                 }
 451
 452                 /* Config LUT */
 453
 454                 dt_shift = (cid % 4) * 8;
 455
 456                 val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
 457                 val &= ~(0xff << dt_shift);
 458                 val |= dt << dt_shift;
 459                 writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
 460
 461                 val = (df << 4) | 0x3;
 462                 writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
 463
 464                 if (tg->enabled) {
 465                         val = CAMSS_CSID_TG_CTRL_ENABLE;
 466                         writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
 467                 }
 468         } else {
 469                 if (tg->enabled) {
 470                         val = CAMSS_CSID_TG_CTRL_DISABLE;
 471                         writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
 472                 }
 473         }
 474
 475         return 0;
 476 }
 477
 478 /*
 479  * __csid_get_format - Get pointer to format structure
 480  * @csid: CSID device
 481  * @cfg: V4L2 subdev pad configuration
 482  * @pad: pad from which format is requested
 483  * @which: TRY or ACTIVE format
 484  *
 485  * Return pointer to TRY or ACTIVE format structure
 486  */
 487 static struct v4l2_mbus_framefmt *
 488 __csid_get_format(struct csid_device *csid,
 489                   struct v4l2_subdev_pad_config *cfg,
 490                   unsigned int pad,
 491                   enum v4l2_subdev_format_whence which)
 492 {
 493         if (which == V4L2_SUBDEV_FORMAT_TRY)
 494                 return v4l2_subdev_get_try_format(&csid->subdev, cfg, pad);
 495
 496         return &csid->fmt[pad];
 497 }
 498
 499 /*
 500  * csid_try_format - Handle try format by pad subdev method
 501  * @csid: CSID device
 502  * @cfg: V4L2 subdev pad configuration
 503  * @pad: pad on which format is requested
 504  * @fmt: pointer to v4l2 format structure
 505  * @which: wanted subdev format
 506  */
 507 static void csid_try_format(struct csid_device *csid,
 508                             struct v4l2_subdev_pad_config *cfg,
 509                             unsigned int pad,
 510                             struct v4l2_mbus_framefmt *fmt,
 511                             enum v4l2_subdev_format_whence which)
 512 {
 513         unsigned int i;
 514
 515         switch (pad) {
 516         case MSM_CSID_PAD_SINK:
 517                 /* Set format on sink pad */
 518
 519                 for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
 520                         if (fmt->code == csid_input_fmts[i].code)
 521                                 break;
 522
 523                 /* If not found, use UYVY as default */
 524                 if (i >= ARRAY_SIZE(csid_input_fmts))
 525                         fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 526
 527                 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
 528                 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 529
 530                 fmt->field = V4L2_FIELD_NONE;
 531                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
 532
 533                 break;
 534
 535         case MSM_CSID_PAD_SRC:
 536                 if (csid->testgen_mode->cur.val == 0) {
 537                         /* Test generator is disabled, keep pad formats */
 538                         /* in sync - set and return a format same as sink pad */
 539                         struct v4l2_mbus_framefmt format;
 540
 541                         format = *__csid_get_format(csid, cfg,
 542                                                     MSM_CSID_PAD_SINK, which);
 543                         *fmt = format;
 544                 } else {
 545                         /* Test generator is enabled, set format on source*/
 546                         /* pad to allow test generator usage */
 547
 548                         for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
 549                                 if (csid_input_fmts[i].code == fmt->code)
 550                                         break;
 551
 552                         /* If not found, use UYVY as default */
 553                         if (i >= ARRAY_SIZE(csid_input_fmts))
 554                                 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 555
 556                         fmt->width = clamp_t(u32, fmt->width, 1, 8191);
 557                         fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 558
 559                         fmt->field = V4L2_FIELD_NONE;
 560                 }
 561                 break;
 562         }
 563
 564         fmt->colorspace = V4L2_COLORSPACE_SRGB;
 565 }
 566
 567 /*
 568  * csid_enum_mbus_code - Handle pixel format enumeration
 569  * @sd: CSID V4L2 subdevice
 570  * @cfg: V4L2 subdev pad configuration
 571  * @code: pointer to v4l2_subdev_mbus_code_enum structure
 572  * return -EINVAL or zero on success
 573  */
 574 static int csid_enum_mbus_code(struct v4l2_subdev *sd,
 575                                struct v4l2_subdev_pad_config *cfg,
 576                                struct v4l2_subdev_mbus_code_enum *code)
 577 {
 578         struct csid_device *csid = v4l2_get_subdevdata(sd);
 579         struct v4l2_mbus_framefmt *format;
 580
 581         if (code->pad == MSM_CSID_PAD_SINK) {
 582                 if (code->index >= ARRAY_SIZE(csid_input_fmts))
 583                         return -EINVAL;
 584
 585                 code->code = csid_input_fmts[code->index].code;
 586         } else {
 587                 if (csid->testgen_mode->cur.val == 0) {
 588                         if (code->index > 0)
 589                                 return -EINVAL;
 590
 591                         format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SINK,
 592                                                    code->which);
 593
 594                         code->code = format->code;
 595                 } else {
 596                         if (code->index >= ARRAY_SIZE(csid_input_fmts))
 597                                 return -EINVAL;
 598
 599                         code->code = csid_input_fmts[code->index].code;
 600                 }
 601         }
 602
 603         return 0;
 604 }
 605
 606 /*
 607  * csid_enum_frame_size - Handle frame size enumeration
 608  * @sd: CSID V4L2 subdevice
 609  * @cfg: V4L2 subdev pad configuration
 610  * @fse: pointer to v4l2_subdev_frame_size_enum structure
 611  * return -EINVAL or zero on success
 612  */
 613 static int csid_enum_frame_size(struct v4l2_subdev *sd,
 614                                 struct v4l2_subdev_pad_config *cfg,
 615                                 struct v4l2_subdev_frame_size_enum *fse)
 616 {
 617         struct csid_device *csid = v4l2_get_subdevdata(sd);
 618         struct v4l2_mbus_framefmt format;
 619
 620         if (fse->index != 0)
 621                 return -EINVAL;
 622
 623         format.code = fse->code;
 624         format.width = 1;
 625         format.height = 1;
 626         csid_try_format(csid, cfg, fse->pad, &format, fse->which);
 627         fse->min_width = format.width;
 628         fse->min_height = format.height;
 629
 630         if (format.code != fse->code)
 631                 return -EINVAL;
 632
 633         format.code = fse->code;
 634         format.width = -1;
 635         format.height = -1;
 636         csid_try_format(csid, cfg, fse->pad, &format, fse->which);
 637         fse->max_width = format.width;
 638         fse->max_height = format.height;
 639
 640         return 0;
 641 }
 642
 643 /*
 644  * csid_get_format - Handle get format by pads subdev method
 645  * @sd: CSID V4L2 subdevice
 646  * @cfg: V4L2 subdev pad configuration
 647  * @fmt: pointer to v4l2 subdev format structure
 648  *
 649  * Return -EINVAL or zero on success
 650  */
 651 static int csid_get_format(struct v4l2_subdev *sd,
 652                            struct v4l2_subdev_pad_config *cfg,
 653                            struct v4l2_subdev_format *fmt)
 654 {
 655         struct csid_device *csid = v4l2_get_subdevdata(sd);
 656         struct v4l2_mbus_framefmt *format;
 657
 658         format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
 659         if (format == NULL)
 660                 return -EINVAL;
 661
 662         fmt->format = *format;
 663
 664         return 0;
 665 }
 666
 667 /*
 668  * csid_set_format - Handle set format by pads subdev method
 669  * @sd: CSID V4L2 subdevice
 670  * @cfg: V4L2 subdev pad configuration
 671  * @fmt: pointer to v4l2 subdev format structure
 672  *
 673  * Return -EINVAL or zero on success
 674  */
 675 static int csid_set_format(struct v4l2_subdev *sd,
 676                            struct v4l2_subdev_pad_config *cfg,
 677                            struct v4l2_subdev_format *fmt)
 678 {
 679         struct csid_device *csid = v4l2_get_subdevdata(sd);
 680         struct v4l2_mbus_framefmt *format;
 681
 682         format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
 683         if (format == NULL)
 684                 return -EINVAL;
 685
 686         csid_try_format(csid, cfg, fmt->pad, &fmt->format, fmt->which);
 687         *format = fmt->format;
 688
 689         /* Propagate the format from sink to source */
 690         if (fmt->pad == MSM_CSID_PAD_SINK) {
 691                 format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SRC,
 692                                            fmt->which);
 693
 694                 *format = fmt->format;
 695                 csid_try_format(csid, cfg, MSM_CSID_PAD_SRC, format,
 696                                 fmt->which);
 697         }
 698
 699         return 0;
 700 }
 701
 702 /*
 703  * csid_init_formats - Initialize formats on all pads
 704  * @sd: CSID V4L2 subdevice
 705  * @fh: V4L2 subdev file handle
 706  *
 707  * Initialize all pad formats with default values.
 708  *
 709  * Return 0 on success or a negative error code otherwise
 710  */
 711 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 712 {
 713         struct v4l2_subdev_format format = {
 714                 .pad = MSM_CSID_PAD_SINK,
 715                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
 716                               V4L2_SUBDEV_FORMAT_ACTIVE,
 717                 .format = {
 718                         .code = MEDIA_BUS_FMT_UYVY8_2X8,
 719                         .width = 1920,
 720                         .height = 1080
 721                 }
 722         };
 723
 724         return csid_set_format(sd, fh ? fh->pad : NULL, &format);
 725 }
 726
 727 static const char * const csid_test_pattern_menu[] = {
 728         "Disabled",
 729         "Incrementing",
 730         "Alternating 0x55/0xAA",
 731         "All Zeros 0x00",
 732         "All Ones 0xFF",
 733         "Pseudo-random Data",
 734 };
 735
 736 /*
 737  * csid_set_test_pattern - Set test generator's pattern mode
 738  * @csid: CSID device
 739  * @value: desired test pattern mode
 740  *
 741  * Return 0 on success or a negative error code otherwise
 742  */
 743 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
 744 {
 745         struct csid_testgen_config *tg = &csid->testgen;
 746
 747         /* If CSID is linked to CSIPHY, do not allow to enable test generator */
 748         if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
 749                 return -EBUSY;
 750
 751         tg->enabled = !!value;
 752
 753         switch (value) {
 754         case 1:
 755                 tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING;
 756                 break;
 757         case 2:
 758                 tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA;
 759                 break;
 760         case 3:
 761                 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES;
 762                 break;
 763         case 4:
 764                 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES;
 765                 break;
 766         case 5:
 767                 tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM;
 768                 break;
 769         }
 770
 771         return 0;
 772 }
 773
 774 /*
 775  * csid_s_ctrl - Handle set control subdev method
 776  * @ctrl: pointer to v4l2 control structure
 777  *
 778  * Return 0 on success or a negative error code otherwise
 779  */
 780 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
 781 {
 782         struct csid_device *csid = container_of(ctrl->handler,
 783                                                 struct csid_device, ctrls);
 784         int ret = -EINVAL;
 785
 786         switch (ctrl->id) {
 787         case V4L2_CID_TEST_PATTERN:
 788                 ret = csid_set_test_pattern(csid, ctrl->val);
 789                 break;
 790         }
 791
 792         return ret;
 793 }
 794
 795 static const struct v4l2_ctrl_ops csid_ctrl_ops = {
 796         .s_ctrl = csid_s_ctrl,
 797 };
 798
 799 /*
 800  * msm_csid_subdev_init - Initialize CSID device structure and resources
 801  * @csid: CSID device
 802  * @res: CSID module resources table
 803  * @id: CSID module id
 804  *
 805  * Return 0 on success or a negative error code otherwise
 806  */
 807 int msm_csid_subdev_init(struct csid_device *csid,
 808                          const struct resources *res, u8 id)
 809 {
 810         struct device *dev = to_device_index(csid, id);
 811         struct platform_device *pdev = to_platform_device(dev);
 812         struct resource *r;
 813         int i, j;
 814         int ret;
 815
 816         csid->id = id;
 817
 818         /* Memory */
 819
 820         r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
 821         csid->base = devm_ioremap_resource(dev, r);
 822         if (IS_ERR(csid->base)) {
 823                 dev_err(dev, "could not map memory\n");
 824                 return PTR_ERR(csid->base);
 825         }
 826
 827         /* Interrupt */
 828
 829         r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
 830                                          res->interrupt[0]);
 831         if (!r) {
 832                 dev_err(dev, "missing IRQ\n");
 833                 return -EINVAL;
 834         }
 835
 836         csid->irq = r->start;
 837         snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
 838                  dev_name(dev), MSM_CSID_NAME, csid->id);
 839         ret = devm_request_irq(dev, csid->irq, csid_isr,
 840                 IRQF_TRIGGER_RISING, csid->irq_name, csid);
 841         if (ret < 0) {
 842                 dev_err(dev, "request_irq failed: %d\n", ret);
 843                 return ret;
 844         }
 845
 846         disable_irq(csid->irq);
 847
 848         /* Clocks */
 849
 850         csid->nclocks = 0;
 851         while (res->clock[csid->nclocks])
 852                 csid->nclocks++;
 853
 854         csid->clock = devm_kzalloc(dev, csid->nclocks * sizeof(*csid->clock),
 855                                     GFP_KERNEL);
 856         if (!csid->clock)
 857                 return -ENOMEM;
 858
 859         for (i = 0; i < csid->nclocks; i++) {
 860                 struct camss_clock *clock = &csid->clock[i];
 861
 862                 clock->clk = devm_clk_get(dev, res->clock[i]);
 863                 if (IS_ERR(clock->clk))
 864                         return PTR_ERR(clock->clk);
 865
 866                 clock->name = res->clock[i];
 867
 868                 clock->nfreqs = 0;
 869                 while (res->clock_rate[i][clock->nfreqs])
 870                         clock->nfreqs++;
 871
 872                 if (!clock->nfreqs) {
 873                         clock->freq = NULL;
 874                         continue;
 875                 }
 876
 877                 clock->freq = devm_kzalloc(dev, clock->nfreqs *
 878                                            sizeof(*clock->freq), GFP_KERNEL);
 879                 if (!clock->freq)
 880                         return -ENOMEM;
 881
 882                 for (j = 0; j < clock->nfreqs; j++)
 883                         clock->freq[j] = res->clock_rate[i][j];
 884         }
 885
 886         /* Regulator */
 887
 888         csid->vdda = devm_regulator_get(dev, res->regulator[0]);
 889         if (IS_ERR(csid->vdda)) {
 890                 dev_err(dev, "could not get regulator\n");
 891                 return PTR_ERR(csid->vdda);
 892         }
 893
 894         init_completion(&csid->reset_complete);
 895
 896         return 0;
 897 }
 898
 899 /*
 900  * msm_csid_get_csid_id - Get CSID HW module id
 901  * @entity: Pointer to CSID media entity structure
 902  * @id: Return CSID HW module id here
 903  */
 904 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
 905 {
 906         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 907         struct csid_device *csid = v4l2_get_subdevdata(sd);
 908
 909         *id = csid->id;
 910 }
 911
 912 /*
 913  * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
 914  * @lane_cfg - CSI2 lane configuration
 915  *
 916  * Return lane assign
 917  */
 918 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
 919 {
 920         u32 lane_assign = 0;
 921         int i;
 922
 923         for (i = 0; i < lane_cfg->num_data; i++)
 924                 lane_assign |= lane_cfg->data[i].pos << (i * 4);
 925
 926         return lane_assign;
 927 }
 928
 929 /*
 930  * csid_link_setup - Setup CSID connections
 931  * @entity: Pointer to media entity structure
 932  * @local: Pointer to local pad
 933  * @remote: Pointer to remote pad
 934  * @flags: Link flags
 935  *
 936  * Return 0 on success
 937  */
 938 static int csid_link_setup(struct media_entity *entity,
 939                            const struct media_pad *local,
 940                            const struct media_pad *remote, u32 flags)
 941 {
 942         if (flags & MEDIA_LNK_FL_ENABLED)
 943                 if (media_entity_remote_pad(local))
 944                         return -EBUSY;
 945
 946         if ((local->flags & MEDIA_PAD_FL_SINK) &&
 947             (flags & MEDIA_LNK_FL_ENABLED)) {
 948                 struct v4l2_subdev *sd;
 949                 struct csid_device *csid;
 950                 struct csiphy_device *csiphy;
 951                 struct csiphy_lanes_cfg *lane_cfg;
 952                 struct v4l2_subdev_format format = { 0 };
 953
 954                 sd = media_entity_to_v4l2_subdev(entity);
 955                 csid = v4l2_get_subdevdata(sd);
 956
 957                 /* If test generator is enabled */
 958                 /* do not allow a link from CSIPHY to CSID */
 959                 if (csid->testgen_mode->cur.val != 0)
 960                         return -EBUSY;
 961
 962                 sd = media_entity_to_v4l2_subdev(remote->entity);
 963                 csiphy = v4l2_get_subdevdata(sd);
 964
 965                 /* If a sensor is not linked to CSIPHY */
 966                 /* do no allow a link from CSIPHY to CSID */
 967                 if (!csiphy->cfg.csi2)
 968                         return -EPERM;
 969
 970                 csid->phy.csiphy_id = csiphy->id;
 971
 972                 lane_cfg = &csiphy->cfg.csi2->lane_cfg;
 973                 csid->phy.lane_cnt = lane_cfg->num_data;
 974                 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
 975
 976                 /* Reset format on source pad to sink pad format */
 977                 format.pad = MSM_CSID_PAD_SRC;
 978                 format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 979                 csid_set_format(&csid->subdev, NULL, &format);
 980         }
 981
 982         return 0;
 983 }
 984
 985 static const struct v4l2_subdev_core_ops csid_core_ops = {
 986         .s_power = csid_set_power,
 987 };
 988
 989 static const struct v4l2_subdev_video_ops csid_video_ops = {
 990         .s_stream = csid_set_stream,
 991 };
 992
 993 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
 994         .enum_mbus_code = csid_enum_mbus_code,
 995         .enum_frame_size = csid_enum_frame_size,
 996         .get_fmt = csid_get_format,
 997         .set_fmt = csid_set_format,
 998 };
 999
1000 static const struct v4l2_subdev_ops csid_v4l2_ops = {
1001         .core = &csid_core_ops,
1002         .video = &csid_video_ops,
1003         .pad = &csid_pad_ops,
1004 };
1005
1006 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
1007         .open = csid_init_formats,
1008 };
1009
1010 static const struct media_entity_operations csid_media_ops = {
1011         .link_setup = csid_link_setup,
1012         .link_validate = v4l2_subdev_link_validate,
1013 };
1014
1015 /*
1016  * msm_csid_register_entity - Register subdev node for CSID module
1017  * @csid: CSID device
1018  * @v4l2_dev: V4L2 device
1019  *
1020  * Return 0 on success or a negative error code otherwise
1021  */
1022 int msm_csid_register_entity(struct csid_device *csid,
1023                              struct v4l2_device *v4l2_dev)
1024 {
1025         struct v4l2_subdev *sd = &csid->subdev;
1026         struct media_pad *pads = csid->pads;
1027         struct device *dev = to_device_index(csid, csid->id);
1028         int ret;
1029
1030         v4l2_subdev_init(sd, &csid_v4l2_ops);
1031         sd->internal_ops = &csid_v4l2_internal_ops;
1032         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1033         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
1034                  MSM_CSID_NAME, csid->id);
1035         v4l2_set_subdevdata(sd, csid);
1036
1037         ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
1038         if (ret < 0) {
1039                 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
1040                 return ret;
1041         }
1042
1043         csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
1044                                 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
1045                                 ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0,
1046                                 csid_test_pattern_menu);
1047
1048         if (csid->ctrls.error) {
1049                 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
1050                 ret = csid->ctrls.error;
1051                 goto free_ctrl;
1052         }
1053
1054         csid->subdev.ctrl_handler = &csid->ctrls;
1055
1056         ret = csid_init_formats(sd, NULL);
1057         if (ret < 0) {
1058                 dev_err(dev, "Failed to init format: %d\n", ret);
1059                 goto free_ctrl;
1060         }
1061
1062         pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1063         pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
1064
1065         sd->entity.function = MEDIA_ENT_F_IO_V4L;
1066         sd->entity.ops = &csid_media_ops;
1067         ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
1068         if (ret < 0) {
1069                 dev_err(dev, "Failed to init media entity: %d\n", ret);
1070                 goto free_ctrl;
1071         }
1072
1073         ret = v4l2_device_register_subdev(v4l2_dev, sd);
1074         if (ret < 0) {
1075                 dev_err(dev, "Failed to register subdev: %d\n", ret);
1076                 goto media_cleanup;
1077         }
1078
1079         return 0;
1080
1081 media_cleanup:
1082         media_entity_cleanup(&sd->entity);
1083 free_ctrl:
1084         v4l2_ctrl_handler_free(&csid->ctrls);
1085
1086         return ret;
1087 }
1088
1089 /*
1090  * msm_csid_unregister_entity - Unregister CSID module subdev node
1091  * @csid: CSID device
1092  */
1093 void msm_csid_unregister_entity(struct csid_device *csid)
1094 {
1095         v4l2_device_unregister_subdev(&csid->subdev);
1096         media_entity_cleanup(&csid->subdev.entity);
1097         v4l2_ctrl_handler_free(&csid->ctrls);
1098 }