drivers/media/platform/ti-vpe/cal.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * TI CAL camera interface driver
   4  *
   5  * Copyright (c) 2015 Texas Instruments Inc.
   6  * Benoit Parrot, <bparrot@ti.com>
   7  */
   8
   9 #include <linux/interrupt.h>
  10 #include <linux/io.h>
  11 #include <linux/ioctl.h>
  12 #include <linux/module.h>
  13 #include <linux/platform_device.h>
  14 #include <linux/delay.h>
  15 #include <linux/pm_runtime.h>
  16 #include <linux/slab.h>
  17 #include <linux/mfd/syscon.h>
  18 #include <linux/regmap.h>
  19 #include <linux/videodev2.h>
  20 #include <linux/of_device.h>
  21 #include <linux/of_graph.h>
  22
  23 #include <media/v4l2-fwnode.h>
  24 #include <media/v4l2-async.h>
  25 #include <media/v4l2-common.h>
  26 #include <media/v4l2-ctrls.h>
  27 #include <media/v4l2-device.h>
  28 #include <media/v4l2-event.h>
  29 #include <media/v4l2-ioctl.h>
  30 #include <media/v4l2-fh.h>
  31 #include <media/videobuf2-core.h>
  32 #include <media/videobuf2-dma-contig.h>
  33 #include "cal_regs.h"
  34
  35 #define CAL_MODULE_NAME "cal"
  36
  37 #define MAX_WIDTH_BYTES (8192 * 8)
  38 #define MAX_HEIGHT_LINES 16383
  39
  40 #define CAL_VERSION "0.1.0"
  41
  42 MODULE_DESCRIPTION("TI CAL driver");
  43 MODULE_AUTHOR("Benoit Parrot, <bparrot@ti.com>");
  44 MODULE_LICENSE("GPL v2");
  45 MODULE_VERSION(CAL_VERSION);
  46
  47 static unsigned video_nr = -1;
  48 module_param(video_nr, uint, 0644);
  49 MODULE_PARM_DESC(video_nr, "videoX start number, -1 is autodetect");
  50
  51 static unsigned debug;
  52 module_param(debug, uint, 0644);
  53 MODULE_PARM_DESC(debug, "activates debug info");
  54
  55 /* timeperframe: min/max and default */
  56 static const struct v4l2_fract
  57         tpf_default = {.numerator = 1001,       .denominator = 30000};
  58
  59 #define cal_dbg(level, caldev, fmt, arg...)     \
  60                 v4l2_dbg(level, debug, &caldev->v4l2_dev, fmt, ##arg)
  61 #define cal_info(caldev, fmt, arg...)   \
  62                 v4l2_info(&caldev->v4l2_dev, fmt, ##arg)
  63 #define cal_err(caldev, fmt, arg...)    \
  64                 v4l2_err(&caldev->v4l2_dev, fmt, ##arg)
  65
  66 #define ctx_dbg(level, ctx, fmt, arg...)        \
  67                 v4l2_dbg(level, debug, &ctx->v4l2_dev, fmt, ##arg)
  68 #define ctx_info(ctx, fmt, arg...)      \
  69                 v4l2_info(&ctx->v4l2_dev, fmt, ##arg)
  70 #define ctx_err(ctx, fmt, arg...)       \
  71                 v4l2_err(&ctx->v4l2_dev, fmt, ##arg)
  72
  73 #define CAL_NUM_INPUT 1
  74 #define CAL_NUM_CONTEXT 2
  75
  76 #define reg_read(dev, offset) ioread32(dev->base + offset)
  77 #define reg_write(dev, offset, val) iowrite32(val, dev->base + offset)
  78
  79 #define reg_read_field(dev, offset, mask) get_field(reg_read(dev, offset), \
  80                                                     mask)
  81 #define reg_write_field(dev, offset, field, mask) { \
  82         u32 val = reg_read(dev, offset); \
  83         set_field(&val, field, mask); \
  84         reg_write(dev, offset, val); }
  85
  86 /* ------------------------------------------------------------------
  87  *      Basic structures
  88  * ------------------------------------------------------------------
  89  */
  90
  91 struct cal_fmt {
  92         u32     fourcc;
  93         u32     code;
  94         /* Bits per pixel */
  95         u8      bpp;
  96 };
  97
  98 static struct cal_fmt cal_formats[] = {
  99         {
 100                 .fourcc         = V4L2_PIX_FMT_YUYV,
 101                 .code           = MEDIA_BUS_FMT_YUYV8_2X8,
 102                 .bpp            = 16,
 103         }, {
 104                 .fourcc         = V4L2_PIX_FMT_UYVY,
 105                 .code           = MEDIA_BUS_FMT_UYVY8_2X8,
 106                 .bpp            = 16,
 107         }, {
 108                 .fourcc         = V4L2_PIX_FMT_YVYU,
 109                 .code           = MEDIA_BUS_FMT_YVYU8_2X8,
 110                 .bpp            = 16,
 111         }, {
 112                 .fourcc         = V4L2_PIX_FMT_VYUY,
 113                 .code           = MEDIA_BUS_FMT_VYUY8_2X8,
 114                 .bpp            = 16,
 115         }, {
 116                 .fourcc         = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
 117                 .code           = MEDIA_BUS_FMT_RGB565_2X8_LE,
 118                 .bpp            = 16,
 119         }, {
 120                 .fourcc         = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
 121                 .code           = MEDIA_BUS_FMT_RGB565_2X8_BE,
 122                 .bpp            = 16,
 123         }, {
 124                 .fourcc         = V4L2_PIX_FMT_RGB555, /* gggbbbbb arrrrrgg */
 125                 .code           = MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
 126                 .bpp            = 16,
 127         }, {
 128                 .fourcc         = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */
 129                 .code           = MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE,
 130                 .bpp            = 16,
 131         }, {
 132                 .fourcc         = V4L2_PIX_FMT_RGB24, /* rgb */
 133                 .code           = MEDIA_BUS_FMT_RGB888_2X12_LE,
 134                 .bpp            = 24,
 135         }, {
 136                 .fourcc         = V4L2_PIX_FMT_BGR24, /* bgr */
 137                 .code           = MEDIA_BUS_FMT_RGB888_2X12_BE,
 138                 .bpp            = 24,
 139         }, {
 140                 .fourcc         = V4L2_PIX_FMT_RGB32, /* argb */
 141                 .code           = MEDIA_BUS_FMT_ARGB8888_1X32,
 142                 .bpp            = 32,
 143         }, {
 144                 .fourcc         = V4L2_PIX_FMT_SBGGR8,
 145                 .code           = MEDIA_BUS_FMT_SBGGR8_1X8,
 146                 .bpp            = 8,
 147         }, {
 148                 .fourcc         = V4L2_PIX_FMT_SGBRG8,
 149                 .code           = MEDIA_BUS_FMT_SGBRG8_1X8,
 150                 .bpp            = 8,
 151         }, {
 152                 .fourcc         = V4L2_PIX_FMT_SGRBG8,
 153                 .code           = MEDIA_BUS_FMT_SGRBG8_1X8,
 154                 .bpp            = 8,
 155         }, {
 156                 .fourcc         = V4L2_PIX_FMT_SRGGB8,
 157                 .code           = MEDIA_BUS_FMT_SRGGB8_1X8,
 158                 .bpp            = 8,
 159         }, {
 160                 .fourcc         = V4L2_PIX_FMT_SBGGR10,
 161                 .code           = MEDIA_BUS_FMT_SBGGR10_1X10,
 162                 .bpp            = 10,
 163         }, {
 164                 .fourcc         = V4L2_PIX_FMT_SGBRG10,
 165                 .code           = MEDIA_BUS_FMT_SGBRG10_1X10,
 166                 .bpp            = 10,
 167         }, {
 168                 .fourcc         = V4L2_PIX_FMT_SGRBG10,
 169                 .code           = MEDIA_BUS_FMT_SGRBG10_1X10,
 170                 .bpp            = 10,
 171         }, {
 172                 .fourcc         = V4L2_PIX_FMT_SRGGB10,
 173                 .code           = MEDIA_BUS_FMT_SRGGB10_1X10,
 174                 .bpp            = 10,
 175         }, {
 176                 .fourcc         = V4L2_PIX_FMT_SBGGR12,
 177                 .code           = MEDIA_BUS_FMT_SBGGR12_1X12,
 178                 .bpp            = 12,
 179         }, {
 180                 .fourcc         = V4L2_PIX_FMT_SGBRG12,
 181                 .code           = MEDIA_BUS_FMT_SGBRG12_1X12,
 182                 .bpp            = 12,
 183         }, {
 184                 .fourcc         = V4L2_PIX_FMT_SGRBG12,
 185                 .code           = MEDIA_BUS_FMT_SGRBG12_1X12,
 186                 .bpp            = 12,
 187         }, {
 188                 .fourcc         = V4L2_PIX_FMT_SRGGB12,
 189                 .code           = MEDIA_BUS_FMT_SRGGB12_1X12,
 190                 .bpp            = 12,
 191         },
 192 };
 193
 194 /*  Print Four-character-code (FOURCC) */
 195 static char *fourcc_to_str(u32 fmt)
 196 {
 197         static char code[5];
 198
 199         code[0] = (unsigned char)(fmt & 0xff);
 200         code[1] = (unsigned char)((fmt >> 8) & 0xff);
 201         code[2] = (unsigned char)((fmt >> 16) & 0xff);
 202         code[3] = (unsigned char)((fmt >> 24) & 0xff);
 203         code[4] = '\0';
 204
 205         return code;
 206 }
 207
 208 /* buffer for one video frame */
 209 struct cal_buffer {
 210         /* common v4l buffer stuff -- must be first */
 211         struct vb2_v4l2_buffer  vb;
 212         struct list_head        list;
 213         const struct cal_fmt    *fmt;
 214 };
 215
 216 struct cal_dmaqueue {
 217         struct list_head        active;
 218
 219         /* Counters to control fps rate */
 220         int                     frame;
 221         int                     ini_jiffies;
 222 };
 223
 224 struct cc_data {
 225         void __iomem            *base;
 226         struct resource         *res;
 227
 228         struct platform_device *pdev;
 229 };
 230
 231 /* CTRL_CORE_CAMERRX_CONTROL register field id */
 232 enum cal_camerarx_field {
 233         F_CTRLCLKEN,
 234         F_CAMMODE,
 235         F_LANEENABLE,
 236         F_CSI_MODE,
 237
 238         F_MAX_FIELDS,
 239 };
 240
 241 struct cal_csi2_phy {
 242         struct regmap_field *fields[F_MAX_FIELDS];
 243         struct reg_field *base_fields;
 244         const int num_lanes;
 245 };
 246
 247 struct cal_data {
 248         const int num_csi2_phy;
 249         struct cal_csi2_phy *csi2_phy_core;
 250
 251         const unsigned int flags;
 252 };
 253
 254 static struct reg_field dra72x_ctrl_core_csi0_reg_fields[F_MAX_FIELDS] = {
 255         [F_CTRLCLKEN] = REG_FIELD(0, 10, 10),
 256         [F_CAMMODE] = REG_FIELD(0, 11, 12),
 257         [F_LANEENABLE] = REG_FIELD(0, 13, 16),
 258         [F_CSI_MODE] = REG_FIELD(0, 17, 17),
 259 };
 260
 261 static struct reg_field dra72x_ctrl_core_csi1_reg_fields[F_MAX_FIELDS] = {
 262         [F_CTRLCLKEN] = REG_FIELD(0, 0, 0),
 263         [F_CAMMODE] = REG_FIELD(0, 1, 2),
 264         [F_LANEENABLE] = REG_FIELD(0, 3, 4),
 265         [F_CSI_MODE] = REG_FIELD(0, 5, 5),
 266 };
 267
 268 static struct cal_csi2_phy dra72x_cal_csi_phy[] = {
 269         {
 270                 .base_fields = dra72x_ctrl_core_csi0_reg_fields,
 271                 .num_lanes = 4,
 272         },
 273         {
 274                 .base_fields = dra72x_ctrl_core_csi1_reg_fields,
 275                 .num_lanes = 2,
 276         },
 277 };
 278
 279 static const struct cal_data dra72x_cal_data = {
 280         .csi2_phy_core = dra72x_cal_csi_phy,
 281         .num_csi2_phy = ARRAY_SIZE(dra72x_cal_csi_phy),
 282 };
 283
 284 static const struct cal_data dra72x_es1_cal_data = {
 285         .csi2_phy_core = dra72x_cal_csi_phy,
 286         .num_csi2_phy = ARRAY_SIZE(dra72x_cal_csi_phy),
 287         .flags = DRA72_CAL_PRE_ES2_LDO_DISABLE,
 288 };
 289
 290 static struct reg_field dra76x_ctrl_core_csi0_reg_fields[F_MAX_FIELDS] = {
 291         [F_CTRLCLKEN] = REG_FIELD(0, 8, 8),
 292         [F_CAMMODE] = REG_FIELD(0, 9, 10),
 293         [F_CSI_MODE] = REG_FIELD(0, 11, 11),
 294         [F_LANEENABLE] = REG_FIELD(0, 27, 31),
 295 };
 296
 297 static struct reg_field dra76x_ctrl_core_csi1_reg_fields[F_MAX_FIELDS] = {
 298         [F_CTRLCLKEN] = REG_FIELD(0, 0, 0),
 299         [F_CAMMODE] = REG_FIELD(0, 1, 2),
 300         [F_CSI_MODE] = REG_FIELD(0, 3, 3),
 301         [F_LANEENABLE] = REG_FIELD(0, 24, 26),
 302 };
 303
 304 static struct cal_csi2_phy dra76x_cal_csi_phy[] = {
 305         {
 306                 .base_fields = dra76x_ctrl_core_csi0_reg_fields,
 307                 .num_lanes = 5,
 308         },
 309         {
 310                 .base_fields = dra76x_ctrl_core_csi1_reg_fields,
 311                 .num_lanes = 3,
 312         },
 313 };
 314
 315 static const struct cal_data dra76x_cal_data = {
 316         .csi2_phy_core = dra76x_cal_csi_phy,
 317         .num_csi2_phy = ARRAY_SIZE(dra76x_cal_csi_phy),
 318 };
 319
 320 static struct reg_field am654_ctrl_core_csi0_reg_fields[F_MAX_FIELDS] = {
 321         [F_CTRLCLKEN] = REG_FIELD(0, 15, 15),
 322         [F_CAMMODE] = REG_FIELD(0, 24, 25),
 323         [F_LANEENABLE] = REG_FIELD(0, 0, 4),
 324 };
 325
 326 static struct cal_csi2_phy am654_cal_csi_phy[] = {
 327         {
 328                 .base_fields = am654_ctrl_core_csi0_reg_fields,
 329                 .num_lanes = 5,
 330         },
 331 };
 332
 333 static const struct cal_data am654_cal_data = {
 334         .csi2_phy_core = am654_cal_csi_phy,
 335         .num_csi2_phy = ARRAY_SIZE(am654_cal_csi_phy),
 336 };
 337
 338 /*
 339  * there is one cal_dev structure in the driver, it is shared by
 340  * all instances.
 341  */
 342 struct cal_dev {
 343         int                     irq;
 344         void __iomem            *base;
 345         struct resource         *res;
 346         struct platform_device  *pdev;
 347         struct v4l2_device      v4l2_dev;
 348
 349         /* Controller flags for special cases */
 350         unsigned int            flags;
 351
 352         const struct cal_data   *data;
 353
 354         /* Control Module handle */
 355         struct regmap           *syscon_camerrx;
 356         u32                     syscon_camerrx_offset;
 357
 358         /* Camera Core Module handle */
 359         struct cc_data          *cc[CAL_NUM_CSI2_PORTS];
 360
 361         struct cal_ctx          *ctx[CAL_NUM_CONTEXT];
 362 };
 363
 364 /*
 365  * There is one cal_ctx structure for each camera core context.
 366  */
 367 struct cal_ctx {
 368         struct v4l2_device      v4l2_dev;
 369         struct v4l2_ctrl_handler ctrl_handler;
 370         struct video_device     vdev;
 371         struct v4l2_async_notifier notifier;
 372         struct v4l2_subdev      *sensor;
 373         struct v4l2_fwnode_endpoint     endpoint;
 374
 375         struct v4l2_async_subdev asd;
 376
 377         struct v4l2_fh          fh;
 378         struct cal_dev          *dev;
 379         struct cc_data          *cc;
 380
 381         /* v4l2_ioctl mutex */
 382         struct mutex            mutex;
 383         /* v4l2 buffers lock */
 384         spinlock_t              slock;
 385
 386         /* Several counters */
 387         unsigned long           jiffies;
 388
 389         struct cal_dmaqueue     vidq;
 390
 391         /* Input Number */
 392         int                     input;
 393
 394         /* video capture */
 395         const struct cal_fmt    *fmt;
 396         /* Used to store current pixel format */
 397         struct v4l2_format              v_fmt;
 398         /* Used to store current mbus frame format */
 399         struct v4l2_mbus_framefmt       m_fmt;
 400
 401         /* Current subdev enumerated format */
 402         struct cal_fmt          *active_fmt[ARRAY_SIZE(cal_formats)];
 403         int                     num_active_fmt;
 404
 405         struct v4l2_fract       timeperframe;
 406         unsigned int            sequence;
 407         unsigned int            external_rate;
 408         struct vb2_queue        vb_vidq;
 409         unsigned int            seq_count;
 410         unsigned int            csi2_port;
 411         unsigned int            virtual_channel;
 412
 413         /* Pointer pointing to current v4l2_buffer */
 414         struct cal_buffer       *cur_frm;
 415         /* Pointer pointing to next v4l2_buffer */
 416         struct cal_buffer       *next_frm;
 417 };
 418
 419 static const struct cal_fmt *find_format_by_pix(struct cal_ctx *ctx,
 420                                                 u32 pixelformat)
 421 {
 422         const struct cal_fmt *fmt;
 423         unsigned int k;
 424
 425         for (k = 0; k < ctx->num_active_fmt; k++) {
 426                 fmt = ctx->active_fmt[k];
 427                 if (fmt->fourcc == pixelformat)
 428                         return fmt;
 429         }
 430
 431         return NULL;
 432 }
 433
 434 static const struct cal_fmt *find_format_by_code(struct cal_ctx *ctx,
 435                                                  u32 code)
 436 {
 437         const struct cal_fmt *fmt;
 438         unsigned int k;
 439
 440         for (k = 0; k < ctx->num_active_fmt; k++) {
 441                 fmt = ctx->active_fmt[k];
 442                 if (fmt->code == code)
 443                         return fmt;
 444         }
 445
 446         return NULL;
 447 }
 448
 449 static inline struct cal_ctx *notifier_to_ctx(struct v4l2_async_notifier *n)
 450 {
 451         return container_of(n, struct cal_ctx, notifier);
 452 }
 453
 454 static inline int get_field(u32 value, u32 mask)
 455 {
 456         return (value & mask) >> __ffs(mask);
 457 }
 458
 459 static inline void set_field(u32 *valp, u32 field, u32 mask)
 460 {
 461         u32 val = *valp;
 462
 463         val &= ~mask;
 464         val |= (field << __ffs(mask)) & mask;
 465         *valp = val;
 466 }
 467
 468 static u32 cal_data_get_phy_max_lanes(struct cal_ctx *ctx)
 469 {
 470         struct cal_dev *dev = ctx->dev;
 471         u32 phy_id = ctx->csi2_port - 1;
 472
 473         return dev->data->csi2_phy_core[phy_id].num_lanes;
 474 }
 475
 476 static u32 cal_data_get_num_csi2_phy(struct cal_dev *dev)
 477 {
 478         return dev->data->num_csi2_phy;
 479 }
 480
 481 static int cal_camerarx_regmap_init(struct cal_dev *dev)
 482 {
 483         struct reg_field *field;
 484         struct cal_csi2_phy *phy;
 485         int i, j;
 486
 487         if (!dev->data)
 488                 return -EINVAL;
 489
 490         for (i = 0; i < cal_data_get_num_csi2_phy(dev); i++) {
 491                 phy = &dev->data->csi2_phy_core[i];
 492                 for (j = 0; j < F_MAX_FIELDS; j++) {
 493                         field = &phy->base_fields[j];
 494                         /*
 495                          * Here we update the reg offset with the
 496                          * value found in DT
 497                          */
 498                         field->reg = dev->syscon_camerrx_offset;
 499                         phy->fields[j] =
 500                                 devm_regmap_field_alloc(&dev->pdev->dev,
 501                                                         dev->syscon_camerrx,
 502                                                         *field);
 503                         if (IS_ERR(phy->fields[j])) {
 504                                 cal_err(dev, "Unable to allocate regmap fields\n");
 505                                 return PTR_ERR(phy->fields[j]);
 506                         }
 507                 }
 508         }
 509         return 0;
 510 }
 511
 512 static const struct regmap_config cal_regmap_config = {
 513         .reg_bits = 32,
 514         .val_bits = 32,
 515         .reg_stride = 4,
 516 };
 517
 518 static struct regmap *cal_get_camerarx_regmap(struct cal_dev *dev)
 519 {
 520         struct platform_device *pdev = dev->pdev;
 521         struct regmap *regmap;
 522         void __iomem *base;
 523         u32 reg_io_width;
 524         struct regmap_config r_config = cal_regmap_config;
 525         struct resource *res;
 526
 527         res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 528                                            "camerrx_control");
 529         base = devm_ioremap_resource(&pdev->dev, res);
 530         if (IS_ERR(base)) {
 531                 cal_err(dev, "failed to ioremap\n");
 532                 return ERR_CAST(base);
 533         }
 534
 535         cal_dbg(1, dev, "ioresource %s at %pa - %pa\n",
 536                 res->name, &res->start, &res->end);
 537
 538         reg_io_width = 4;
 539         r_config.reg_stride = reg_io_width;
 540         r_config.val_bits = reg_io_width * 8;
 541         r_config.max_register = resource_size(res) - reg_io_width;
 542
 543         regmap = regmap_init_mmio(NULL, base, &r_config);
 544         if (IS_ERR(regmap))
 545                 pr_err("regmap init failed\n");
 546
 547         return regmap;
 548 }
 549
 550 /*
 551  * Control Module CAMERARX block access
 552  */
 553 static void camerarx_phy_enable(struct cal_ctx *ctx)
 554 {
 555         struct cal_csi2_phy *phy;
 556         u32 phy_id = ctx->csi2_port - 1;
 557         u32 max_lanes;
 558
 559         phy = &ctx->dev->data->csi2_phy_core[phy_id];
 560         regmap_field_write(phy->fields[F_CAMMODE], 0);
 561         /* Always enable all lanes at the phy control level */
 562         max_lanes = (1 << cal_data_get_phy_max_lanes(ctx)) - 1;
 563         regmap_field_write(phy->fields[F_LANEENABLE], max_lanes);
 564         /* F_CSI_MODE is not present on every architecture */
 565         if (phy->fields[F_CSI_MODE])
 566                 regmap_field_write(phy->fields[F_CSI_MODE], 1);
 567         regmap_field_write(phy->fields[F_CTRLCLKEN], 1);
 568 }
 569
 570 static void camerarx_phy_disable(struct cal_ctx *ctx)
 571 {
 572         struct cal_csi2_phy *phy;
 573         u32 phy_id = ctx->csi2_port - 1;
 574
 575         phy = &ctx->dev->data->csi2_phy_core[phy_id];
 576         regmap_field_write(phy->fields[F_CTRLCLKEN], 0);
 577 }
 578
 579 /*
 580  * Camera Instance access block
 581  */
 582 static struct cc_data *cc_create(struct cal_dev *dev, unsigned int core)
 583 {
 584         struct platform_device *pdev = dev->pdev;
 585         struct cc_data *cc;
 586
 587         cc = devm_kzalloc(&pdev->dev, sizeof(*cc), GFP_KERNEL);
 588         if (!cc)
 589                 return ERR_PTR(-ENOMEM);
 590
 591         cc->res = platform_get_resource_byname(pdev,
 592                                                IORESOURCE_MEM,
 593                                                (core == 0) ?
 594                                                 "cal_rx_core0" :
 595                                                 "cal_rx_core1");
 596         cc->base = devm_ioremap_resource(&pdev->dev, cc->res);
 597         if (IS_ERR(cc->base)) {
 598                 cal_err(dev, "failed to ioremap\n");
 599                 return ERR_CAST(cc->base);
 600         }
 601
 602         cal_dbg(1, dev, "ioresource %s at %pa - %pa\n",
 603                 cc->res->name, &cc->res->start, &cc->res->end);
 604
 605         return cc;
 606 }
 607
 608 /*
 609  * Get Revision and HW info
 610  */
 611 static void cal_get_hwinfo(struct cal_dev *dev)
 612 {
 613         u32 revision = 0;
 614         u32 hwinfo = 0;
 615
 616         revision = reg_read(dev, CAL_HL_REVISION);
 617         cal_dbg(3, dev, "CAL_HL_REVISION = 0x%08x (expecting 0x40000200)\n",
 618                 revision);
 619
 620         hwinfo = reg_read(dev, CAL_HL_HWINFO);
 621         cal_dbg(3, dev, "CAL_HL_HWINFO = 0x%08x (expecting 0xA3C90469)\n",
 622                 hwinfo);
 623 }
 624
 625 /*
 626  *   Errata i913: CSI2 LDO Needs to be disabled when module is powered on
 627  *
 628  *   Enabling CSI2 LDO shorts it to core supply. It is crucial the 2 CSI2
 629  *   LDOs on the device are disabled if CSI-2 module is powered on
 630  *   (0x4845 B304 | 0x4845 B384 [28:27] = 0x1) or in ULPS (0x4845 B304
 631  *   | 0x4845 B384 [28:27] = 0x2) mode. Common concerns include: high
 632  *   current draw on the module supply in active mode.
 633  *
 634  *   Errata does not apply when CSI-2 module is powered off
 635  *   (0x4845 B304 | 0x4845 B384 [28:27] = 0x0).
 636  *
 637  * SW Workaround:
 638  *      Set the following register bits to disable the LDO,
 639  *      which is essentially CSI2 REG10 bit 6:
 640  *
 641  *              Core 0:  0x4845 B828 = 0x0000 0040
 642  *              Core 1:  0x4845 B928 = 0x0000 0040
 643  */
 644 static void i913_errata(struct cal_dev *dev, unsigned int port)
 645 {
 646         u32 reg10 = reg_read(dev->cc[port], CAL_CSI2_PHY_REG10);
 647
 648         set_field(&reg10, CAL_CSI2_PHY_REG0_HSCLOCKCONFIG_DISABLE,
 649                   CAL_CSI2_PHY_REG10_I933_LDO_DISABLE_MASK);
 650
 651         cal_dbg(1, dev, "CSI2_%d_REG10 = 0x%08x\n", port, reg10);
 652         reg_write(dev->cc[port], CAL_CSI2_PHY_REG10, reg10);
 653 }
 654
 655 static int cal_runtime_get(struct cal_dev *dev)
 656 {
 657         int r;
 658
 659         r = pm_runtime_get_sync(&dev->pdev->dev);
 660
 661         if (dev->flags & DRA72_CAL_PRE_ES2_LDO_DISABLE) {
 662                 /*
 663                  * Apply errata on both port eveytime we (re-)enable
 664                  * the clock
 665                  */
 666                 i913_errata(dev, 0);
 667                 i913_errata(dev, 1);
 668         }
 669
 670         return r;
 671 }
 672
 673 static inline void cal_runtime_put(struct cal_dev *dev)
 674 {
 675         pm_runtime_put_sync(&dev->pdev->dev);
 676 }
 677
 678 static void cal_quickdump_regs(struct cal_dev *dev)
 679 {
 680         cal_info(dev, "CAL Registers @ 0x%pa:\n", &dev->res->start);
 681         print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 4,
 682                        (__force const void *)dev->base,
 683                        resource_size(dev->res), false);
 684
 685         if (dev->ctx[0]) {
 686                 cal_info(dev, "CSI2 Core 0 Registers @ %pa:\n",
 687                          &dev->ctx[0]->cc->res->start);
 688                 print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 4,
 689                                (__force const void *)dev->ctx[0]->cc->base,
 690                                resource_size(dev->ctx[0]->cc->res),
 691                                false);
 692         }
 693
 694         if (dev->ctx[1]) {
 695                 cal_info(dev, "CSI2 Core 1 Registers @ %pa:\n",
 696                          &dev->ctx[1]->cc->res->start);
 697                 print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 4,
 698                                (__force const void *)dev->ctx[1]->cc->base,
 699                                resource_size(dev->ctx[1]->cc->res),
 700                                false);
 701         }
 702 }
 703
 704 /*
 705  * Enable the expected IRQ sources
 706  */
 707 static void enable_irqs(struct cal_ctx *ctx)
 708 {
 709         /* Enable IRQ_WDMA_END 0/1 */
 710         reg_write_field(ctx->dev,
 711                         CAL_HL_IRQENABLE_SET(2),
 712                         CAL_HL_IRQ_ENABLE,
 713                         CAL_HL_IRQ_MASK(ctx->csi2_port));
 714         /* Enable IRQ_WDMA_START 0/1 */
 715         reg_write_field(ctx->dev,
 716                         CAL_HL_IRQENABLE_SET(3),
 717                         CAL_HL_IRQ_ENABLE,
 718                         CAL_HL_IRQ_MASK(ctx->csi2_port));
 719         /* Todo: Add VC_IRQ and CSI2_COMPLEXIO_IRQ handling */
 720         reg_write(ctx->dev, CAL_CSI2_VC_IRQENABLE(1), 0xFF000000);
 721 }
 722
 723 static void disable_irqs(struct cal_ctx *ctx)
 724 {
 725         /* Disable IRQ_WDMA_END 0/1 */
 726         reg_write_field(ctx->dev,
 727                         CAL_HL_IRQENABLE_CLR(2),
 728                         CAL_HL_IRQ_CLEAR,
 729                         CAL_HL_IRQ_MASK(ctx->csi2_port));
 730         /* Disable IRQ_WDMA_START 0/1 */
 731         reg_write_field(ctx->dev,
 732                         CAL_HL_IRQENABLE_CLR(3),
 733                         CAL_HL_IRQ_CLEAR,
 734                         CAL_HL_IRQ_MASK(ctx->csi2_port));
 735         /* Todo: Add VC_IRQ and CSI2_COMPLEXIO_IRQ handling */
 736         reg_write(ctx->dev, CAL_CSI2_VC_IRQENABLE(1), 0);
 737 }
 738
 739 static void csi2_phy_config(struct cal_ctx *ctx);
 740
 741 static void csi2_phy_init(struct cal_ctx *ctx)
 742 {
 743         int i;
 744         u32 val;
 745
 746         /* Steps
 747          *  1. Configure D-PHY mode and enable required lanes
 748          *  2. Reset complex IO - Wait for completion of reset
 749          *          Note if the external sensor is not sending byte clock,
 750          *          the reset will timeout
 751          *  3 Program Stop States
 752          *      A. Program THS_TERM, THS_SETTLE, etc... Timings parameters
 753          *              in terms of DDR clock periods
 754          *      B. Enable stop state transition timeouts
 755          *  4.Force FORCERXMODE
 756          *      D. Enable pull down using pad control
 757          *      E. Power up PHY
 758          *      F. Wait for power up completion
 759          *      G. Wait for all enabled lane to reach stop state
 760          *      H. Disable pull down using pad control
 761          */
 762
 763         /* 1. Configure D-PHY mode and enable required lanes */
 764         camerarx_phy_enable(ctx);
 765
 766         /* 2. Reset complex IO - Do not wait for reset completion */
 767         val = reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port));
 768         set_field(&val, CAL_CSI2_COMPLEXIO_CFG_RESET_CTRL_OPERATIONAL,
 769                   CAL_CSI2_COMPLEXIO_CFG_RESET_CTRL_MASK);
 770         reg_write(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port), val);
 771         ctx_dbg(3, ctx, "CAL_CSI2_COMPLEXIO_CFG(%d) = 0x%08x De-assert Complex IO Reset\n",
 772                 ctx->csi2_port,
 773                 reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port)));
 774
 775         /* Dummy read to allow SCP to complete */
 776         val = reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port));
 777
 778         /* 3.A. Program Phy Timing Parameters */
 779         csi2_phy_config(ctx);
 780
 781         /* 3.B. Program Stop States */
 782         val = reg_read(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port));
 783         set_field(&val, CAL_GEN_ENABLE,
 784                   CAL_CSI2_TIMING_STOP_STATE_X16_IO1_MASK);
 785         set_field(&val, CAL_GEN_DISABLE,
 786                   CAL_CSI2_TIMING_STOP_STATE_X4_IO1_MASK);
 787         set_field(&val, 407, CAL_CSI2_TIMING_STOP_STATE_COUNTER_IO1_MASK);
 788         reg_write(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port), val);
 789         ctx_dbg(3, ctx, "CAL_CSI2_TIMING(%d) = 0x%08x Stop States\n",
 790                 ctx->csi2_port,
 791                 reg_read(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port)));
 792
 793         /* 4. Force FORCERXMODE */
 794         val = reg_read(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port));
 795         set_field(&val, CAL_GEN_ENABLE,
 796                   CAL_CSI2_TIMING_FORCE_RX_MODE_IO1_MASK);
 797         reg_write(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port), val);
 798         ctx_dbg(3, ctx, "CAL_CSI2_TIMING(%d) = 0x%08x Force RXMODE\n",
 799                 ctx->csi2_port,
 800                 reg_read(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port)));
 801
 802         /* E. Power up the PHY using the complex IO */
 803         val = reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port));
 804         set_field(&val, CAL_CSI2_COMPLEXIO_CFG_PWR_CMD_STATE_ON,
 805                   CAL_CSI2_COMPLEXIO_CFG_PWR_CMD_MASK);
 806         reg_write(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port), val);
 807
 808         /* F. Wait for power up completion */
 809         for (i = 0; i < 10; i++) {
 810                 if (reg_read_field(ctx->dev,
 811                                    CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port),
 812                                    CAL_CSI2_COMPLEXIO_CFG_PWR_STATUS_MASK) ==
 813                     CAL_CSI2_COMPLEXIO_CFG_PWR_STATUS_STATE_ON)
 814                         break;
 815                 usleep_range(1000, 1100);
 816         }
 817         ctx_dbg(3, ctx, "CAL_CSI2_COMPLEXIO_CFG(%d) = 0x%08x Powered UP %s\n",
 818                 ctx->csi2_port,
 819                 reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port)),
 820                 (i >= 10) ? "(timeout)" : "");
 821 }
 822
 823 static void csi2_wait_for_phy(struct cal_ctx *ctx)
 824 {
 825         int i;
 826
 827         /* Steps
 828          *  2. Wait for completion of reset
 829          *          Note if the external sensor is not sending byte clock,
 830          *          the reset will timeout
 831          *  4.Force FORCERXMODE
 832          *      G. Wait for all enabled lane to reach stop state
 833          *      H. Disable pull down using pad control
 834          */
 835
 836         /* 2. Wait for reset completion */
 837         for (i = 0; i < 250; i++) {
 838                 if (reg_read_field(ctx->dev,
 839                                    CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port),
 840                                    CAL_CSI2_COMPLEXIO_CFG_RESET_DONE_MASK) ==
 841                     CAL_CSI2_COMPLEXIO_CFG_RESET_DONE_RESETCOMPLETED)
 842                         break;
 843                 usleep_range(1000, 1100);
 844         }
 845         ctx_dbg(3, ctx, "CAL_CSI2_COMPLEXIO_CFG(%d) = 0x%08x Complex IO Reset Done (%d) %s\n",
 846                 ctx->csi2_port,
 847                 reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port)), i,
 848                 (i >= 250) ? "(timeout)" : "");
 849
 850         /* 4. G. Wait for all enabled lane to reach stop state */
 851         for (i = 0; i < 10; i++) {
 852                 if (reg_read_field(ctx->dev,
 853                                    CAL_CSI2_TIMING(ctx->csi2_port),
 854                                    CAL_CSI2_TIMING_FORCE_RX_MODE_IO1_MASK) ==
 855                     CAL_GEN_DISABLE)
 856                         break;
 857                 usleep_range(1000, 1100);
 858         }
 859         ctx_dbg(3, ctx, "CAL_CSI2_TIMING(%d) = 0x%08x Stop State Reached %s\n",
 860                 ctx->csi2_port,
 861                 reg_read(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port)),
 862                 (i >= 10) ? "(timeout)" : "");
 863
 864         ctx_dbg(1, ctx, "CSI2_%d_REG1 = 0x%08x (Bit(31,28) should be set!)\n",
 865                 (ctx->csi2_port - 1), reg_read(ctx->cc, CAL_CSI2_PHY_REG1));
 866 }
 867
 868 static void csi2_phy_deinit(struct cal_ctx *ctx)
 869 {
 870         int i;
 871         u32 val;
 872
 873         /* Power down the PHY using the complex IO */
 874         val = reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port));
 875         set_field(&val, CAL_CSI2_COMPLEXIO_CFG_PWR_CMD_STATE_OFF,
 876                   CAL_CSI2_COMPLEXIO_CFG_PWR_CMD_MASK);
 877         reg_write(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port), val);
 878
 879         /* Wait for power down completion */
 880         for (i = 0; i < 10; i++) {
 881                 if (reg_read_field(ctx->dev,
 882                                    CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port),
 883                                    CAL_CSI2_COMPLEXIO_CFG_PWR_STATUS_MASK) ==
 884                     CAL_CSI2_COMPLEXIO_CFG_PWR_STATUS_STATE_OFF)
 885                         break;
 886                 usleep_range(1000, 1100);
 887         }
 888         ctx_dbg(3, ctx, "CAL_CSI2_COMPLEXIO_CFG(%d) = 0x%08x Powered Down %s\n",
 889                 ctx->csi2_port,
 890                 reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port)),
 891                 (i >= 10) ? "(timeout)" : "");
 892
 893         /* Assert Comple IO Reset */
 894         val = reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port));
 895         set_field(&val, CAL_CSI2_COMPLEXIO_CFG_RESET_CTRL,
 896                   CAL_CSI2_COMPLEXIO_CFG_RESET_CTRL_MASK);
 897         reg_write(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port), val);
 898
 899         /* Wait for power down completion */
 900         for (i = 0; i < 10; i++) {
 901                 if (reg_read_field(ctx->dev,
 902                                    CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port),
 903                                    CAL_CSI2_COMPLEXIO_CFG_RESET_DONE_MASK) ==
 904                     CAL_CSI2_COMPLEXIO_CFG_RESET_DONE_RESETONGOING)
 905                         break;
 906                 usleep_range(1000, 1100);
 907         }
 908         ctx_dbg(3, ctx, "CAL_CSI2_COMPLEXIO_CFG(%d) = 0x%08x Complex IO in Reset (%d) %s\n",
 909                 ctx->csi2_port,
 910                 reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port)), i,
 911                 (i >= 10) ? "(timeout)" : "");
 912
 913         /* Disable the phy */
 914         camerarx_phy_disable(ctx);
 915 }
 916
 917 static void csi2_lane_config(struct cal_ctx *ctx)
 918 {
 919         u32 val = reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port));
 920         u32 lane_mask = CAL_CSI2_COMPLEXIO_CFG_CLOCK_POSITION_MASK;
 921         u32 polarity_mask = CAL_CSI2_COMPLEXIO_CFG_CLOCK_POL_MASK;
 922         struct v4l2_fwnode_bus_mipi_csi2 *mipi_csi2 =
 923                 &ctx->endpoint.bus.mipi_csi2;
 924         int lane;
 925
 926         set_field(&val, mipi_csi2->clock_lane + 1, lane_mask);
 927         set_field(&val, mipi_csi2->lane_polarities[0], polarity_mask);
 928         for (lane = 0; lane < mipi_csi2->num_data_lanes; lane++) {
 929                 /*
 930                  * Every lane are one nibble apart starting with the
 931                  * clock followed by the data lanes so shift masks by 4.
 932                  */
 933                 lane_mask <<= 4;
 934                 polarity_mask <<= 4;
 935                 set_field(&val, mipi_csi2->data_lanes[lane] + 1, lane_mask);
 936                 set_field(&val, mipi_csi2->lane_polarities[lane + 1],
 937                           polarity_mask);
 938         }
 939
 940         reg_write(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port), val);
 941         ctx_dbg(3, ctx, "CAL_CSI2_COMPLEXIO_CFG(%d) = 0x%08x\n",
 942                 ctx->csi2_port, val);
 943 }
 944
 945 static void csi2_ppi_enable(struct cal_ctx *ctx)
 946 {
 947         reg_write_field(ctx->dev, CAL_CSI2_PPI_CTRL(ctx->csi2_port),
 948                         CAL_GEN_ENABLE, CAL_CSI2_PPI_CTRL_IF_EN_MASK);
 949 }
 950
 951 static void csi2_ppi_disable(struct cal_ctx *ctx)
 952 {
 953         reg_write_field(ctx->dev, CAL_CSI2_PPI_CTRL(ctx->csi2_port),
 954                         CAL_GEN_DISABLE, CAL_CSI2_PPI_CTRL_IF_EN_MASK);
 955 }
 956
 957 static void csi2_ctx_config(struct cal_ctx *ctx)
 958 {
 959         u32 val;
 960
 961         val = reg_read(ctx->dev, CAL_CSI2_CTX0(ctx->csi2_port));
 962         set_field(&val, ctx->csi2_port, CAL_CSI2_CTX_CPORT_MASK);
 963         /*
 964          * DT type: MIPI CSI-2 Specs
 965          *   0x1: All - DT filter is disabled
 966          *  0x24: RGB888 1 pixel  = 3 bytes
 967          *  0x2B: RAW10  4 pixels = 5 bytes
 968          *  0x2A: RAW8   1 pixel  = 1 byte
 969          *  0x1E: YUV422 2 pixels = 4 bytes
 970          */
 971         set_field(&val, 0x1, CAL_CSI2_CTX_DT_MASK);
 972         /* Virtual Channel from the CSI2 sensor usually 0! */
 973         set_field(&val, ctx->virtual_channel, CAL_CSI2_CTX_VC_MASK);
 974         /* NUM_LINES_PER_FRAME => 0 means auto detect */
 975         set_field(&val, 0, CAL_CSI2_CTX_LINES_MASK);
 976         set_field(&val, CAL_CSI2_CTX_ATT_PIX, CAL_CSI2_CTX_ATT_MASK);
 977         set_field(&val, CAL_CSI2_CTX_PACK_MODE_LINE,
 978                   CAL_CSI2_CTX_PACK_MODE_MASK);
 979         reg_write(ctx->dev, CAL_CSI2_CTX0(ctx->csi2_port), val);
 980         ctx_dbg(3, ctx, "CAL_CSI2_CTX0(%d) = 0x%08x\n", ctx->csi2_port,
 981                 reg_read(ctx->dev, CAL_CSI2_CTX0(ctx->csi2_port)));
 982 }
 983
 984 static void pix_proc_config(struct cal_ctx *ctx)
 985 {
 986         u32 val, extract, pack;
 987
 988         switch (ctx->fmt->bpp) {
 989         case 8:
 990                 extract = CAL_PIX_PROC_EXTRACT_B8;
 991                 pack = CAL_PIX_PROC_PACK_B8;
 992                 break;
 993         case 10:
 994                 extract = CAL_PIX_PROC_EXTRACT_B10_MIPI;
 995                 pack = CAL_PIX_PROC_PACK_B16;
 996                 break;
 997         case 12:
 998                 extract = CAL_PIX_PROC_EXTRACT_B12_MIPI;
 999                 pack = CAL_PIX_PROC_PACK_B16;
1000                 break;
1001         case 16:
1002                 extract = CAL_PIX_PROC_EXTRACT_B16_LE;
1003                 pack = CAL_PIX_PROC_PACK_B16;
1004                 break;
1005         default:
1006                 /*
1007                  * If you see this warning then it means that you added
1008                  * some new entry in the cal_formats[] array with a different
1009                  * bit per pixel values then the one supported below.
1010                  * Either add support for the new bpp value below or adjust
1011                  * the new entry to use one of the value below.
1012                  *
1013                  * Instead of failing here just use 8 bpp as a default.
1014                  */
1015                 dev_warn_once(&ctx->dev->pdev->dev,
1016                               "%s:%d:%s: bpp:%d unsupported! Overwritten with 8.\n",
1017                               __FILE__, __LINE__, __func__, ctx->fmt->bpp);
1018                 extract = CAL_PIX_PROC_EXTRACT_B8;
1019                 pack = CAL_PIX_PROC_PACK_B8;
1020                 break;
1021         }
1022
1023         val = reg_read(ctx->dev, CAL_PIX_PROC(ctx->csi2_port));
1024         set_field(&val, extract, CAL_PIX_PROC_EXTRACT_MASK);
1025         set_field(&val, CAL_PIX_PROC_DPCMD_BYPASS, CAL_PIX_PROC_DPCMD_MASK);
1026         set_field(&val, CAL_PIX_PROC_DPCME_BYPASS, CAL_PIX_PROC_DPCME_MASK);
1027         set_field(&val, pack, CAL_PIX_PROC_PACK_MASK);
1028         set_field(&val, ctx->csi2_port, CAL_PIX_PROC_CPORT_MASK);
1029         set_field(&val, CAL_GEN_ENABLE, CAL_PIX_PROC_EN_MASK);
1030         reg_write(ctx->dev, CAL_PIX_PROC(ctx->csi2_port), val);
1031         ctx_dbg(3, ctx, "CAL_PIX_PROC(%d) = 0x%08x\n", ctx->csi2_port,
1032                 reg_read(ctx->dev, CAL_PIX_PROC(ctx->csi2_port)));
1033 }
1034
1035 static void cal_wr_dma_config(struct cal_ctx *ctx,
1036                               unsigned int width, unsigned int height)
1037 {
1038         u32 val;
1039
1040         val = reg_read(ctx->dev, CAL_WR_DMA_CTRL(ctx->csi2_port));
1041         set_field(&val, ctx->csi2_port, CAL_WR_DMA_CTRL_CPORT_MASK);
1042         set_field(&val, height, CAL_WR_DMA_CTRL_YSIZE_MASK);
1043         set_field(&val, CAL_WR_DMA_CTRL_DTAG_PIX_DAT,
1044                   CAL_WR_DMA_CTRL_DTAG_MASK);
1045         set_field(&val, CAL_WR_DMA_CTRL_MODE_CONST,
1046                   CAL_WR_DMA_CTRL_MODE_MASK);
1047         set_field(&val, CAL_WR_DMA_CTRL_PATTERN_LINEAR,
1048                   CAL_WR_DMA_CTRL_PATTERN_MASK);
1049         set_field(&val, CAL_GEN_ENABLE, CAL_WR_DMA_CTRL_STALL_RD_MASK);
1050         reg_write(ctx->dev, CAL_WR_DMA_CTRL(ctx->csi2_port), val);
1051         ctx_dbg(3, ctx, "CAL_WR_DMA_CTRL(%d) = 0x%08x\n", ctx->csi2_port,
1052                 reg_read(ctx->dev, CAL_WR_DMA_CTRL(ctx->csi2_port)));
1053
1054         /*
1055          * width/16 not sure but giving it a whirl.
1056          * zero does not work right
1057          */
1058         reg_write_field(ctx->dev,
1059                         CAL_WR_DMA_OFST(ctx->csi2_port),
1060                         (width / 16),
1061                         CAL_WR_DMA_OFST_MASK);
1062         ctx_dbg(3, ctx, "CAL_WR_DMA_OFST(%d) = 0x%08x\n", ctx->csi2_port,
1063                 reg_read(ctx->dev, CAL_WR_DMA_OFST(ctx->csi2_port)));
1064
1065         val = reg_read(ctx->dev, CAL_WR_DMA_XSIZE(ctx->csi2_port));
1066         /* 64 bit word means no skipping */
1067         set_field(&val, 0, CAL_WR_DMA_XSIZE_XSKIP_MASK);
1068         /*
1069          * (width*8)/64 this should be size of an entire line
1070          * in 64bit word but 0 means all data until the end
1071          * is detected automagically
1072          */
1073         set_field(&val, (width / 8), CAL_WR_DMA_XSIZE_MASK);
1074         reg_write(ctx->dev, CAL_WR_DMA_XSIZE(ctx->csi2_port), val);
1075         ctx_dbg(3, ctx, "CAL_WR_DMA_XSIZE(%d) = 0x%08x\n", ctx->csi2_port,
1076                 reg_read(ctx->dev, CAL_WR_DMA_XSIZE(ctx->csi2_port)));
1077
1078         val = reg_read(ctx->dev, CAL_CTRL);
1079         set_field(&val, CAL_CTRL_BURSTSIZE_BURST128, CAL_CTRL_BURSTSIZE_MASK);
1080         set_field(&val, 0xF, CAL_CTRL_TAGCNT_MASK);
1081         set_field(&val, CAL_CTRL_POSTED_WRITES_NONPOSTED,
1082                   CAL_CTRL_POSTED_WRITES_MASK);
1083         set_field(&val, 0xFF, CAL_CTRL_MFLAGL_MASK);
1084         set_field(&val, 0xFF, CAL_CTRL_MFLAGH_MASK);
1085         reg_write(ctx->dev, CAL_CTRL, val);
1086         ctx_dbg(3, ctx, "CAL_CTRL = 0x%08x\n", reg_read(ctx->dev, CAL_CTRL));
1087 }
1088
1089 static void cal_wr_dma_addr(struct cal_ctx *ctx, unsigned int dmaaddr)
1090 {
1091         reg_write(ctx->dev, CAL_WR_DMA_ADDR(ctx->csi2_port), dmaaddr);
1092 }
1093
1094 /*
1095  * TCLK values are OK at their reset values
1096  */
1097 #define TCLK_TERM       0
1098 #define TCLK_MISS       1
1099 #define TCLK_SETTLE     14
1100
1101 static void csi2_phy_config(struct cal_ctx *ctx)
1102 {
1103         unsigned int reg0, reg1;
1104         unsigned int ths_term, ths_settle;
1105         unsigned int csi2_ddrclk_khz;
1106         struct v4l2_fwnode_bus_mipi_csi2 *mipi_csi2 =
1107                         &ctx->endpoint.bus.mipi_csi2;
1108         u32 num_lanes = mipi_csi2->num_data_lanes;
1109
1110         /* DPHY timing configuration */
1111         /* CSI-2 is DDR and we only count used lanes. */
1112         csi2_ddrclk_khz = ctx->external_rate / 1000
1113                 / (2 * num_lanes) * ctx->fmt->bpp;
1114         ctx_dbg(1, ctx, "csi2_ddrclk_khz: %d\n", csi2_ddrclk_khz);
1115
1116         /* THS_TERM: Programmed value = floor(20 ns/DDRClk period) */
1117         ths_term = 20 * csi2_ddrclk_khz / 1000000;
1118         ctx_dbg(1, ctx, "ths_term: %d (0x%02x)\n", ths_term, ths_term);
1119
1120         /* THS_SETTLE: Programmed value = floor(105 ns/DDRClk period) + 4 */
1121         ths_settle = (105 * csi2_ddrclk_khz / 1000000) + 4;
1122         ctx_dbg(1, ctx, "ths_settle: %d (0x%02x)\n", ths_settle, ths_settle);
1123
1124         reg0 = reg_read(ctx->cc, CAL_CSI2_PHY_REG0);
1125         set_field(&reg0, CAL_CSI2_PHY_REG0_HSCLOCKCONFIG_DISABLE,
1126                   CAL_CSI2_PHY_REG0_HSCLOCKCONFIG_MASK);
1127         set_field(&reg0, ths_term, CAL_CSI2_PHY_REG0_THS_TERM_MASK);
1128         set_field(&reg0, ths_settle, CAL_CSI2_PHY_REG0_THS_SETTLE_MASK);
1129
1130         ctx_dbg(1, ctx, "CSI2_%d_REG0 = 0x%08x\n", (ctx->csi2_port - 1), reg0);
1131         reg_write(ctx->cc, CAL_CSI2_PHY_REG0, reg0);
1132
1133         reg1 = reg_read(ctx->cc, CAL_CSI2_PHY_REG1);
1134         set_field(&reg1, TCLK_TERM, CAL_CSI2_PHY_REG1_TCLK_TERM_MASK);
1135         set_field(&reg1, 0xb8, CAL_CSI2_PHY_REG1_DPHY_HS_SYNC_PATTERN_MASK);
1136         set_field(&reg1, TCLK_MISS, CAL_CSI2_PHY_REG1_CTRLCLK_DIV_FACTOR_MASK);
1137         set_field(&reg1, TCLK_SETTLE, CAL_CSI2_PHY_REG1_TCLK_SETTLE_MASK);
1138
1139         ctx_dbg(1, ctx, "CSI2_%d_REG1 = 0x%08x\n", (ctx->csi2_port - 1), reg1);
1140         reg_write(ctx->cc, CAL_CSI2_PHY_REG1, reg1);
1141 }
1142
1143 static int cal_get_external_info(struct cal_ctx *ctx)
1144 {
1145         struct v4l2_ctrl *ctrl;
1146
1147         if (!ctx->sensor)
1148                 return -ENODEV;
1149
1150         ctrl = v4l2_ctrl_find(ctx->sensor->ctrl_handler, V4L2_CID_PIXEL_RATE);
1151         if (!ctrl) {
1152                 ctx_err(ctx, "no pixel rate control in subdev: %s\n",
1153                         ctx->sensor->name);
1154                 return -EPIPE;
1155         }
1156
1157         ctx->external_rate = v4l2_ctrl_g_ctrl_int64(ctrl);
1158         ctx_dbg(3, ctx, "sensor Pixel Rate: %d\n", ctx->external_rate);
1159
1160         return 0;
1161 }
1162
1163 static inline void cal_schedule_next_buffer(struct cal_ctx *ctx)
1164 {
1165         struct cal_dmaqueue *dma_q = &ctx->vidq;
1166         struct cal_buffer *buf;
1167         unsigned long addr;
1168
1169         buf = list_entry(dma_q->active.next, struct cal_buffer, list);
1170         ctx->next_frm = buf;
1171         list_del(&buf->list);
1172
1173         addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
1174         cal_wr_dma_addr(ctx, addr);
1175 }
1176
1177 static inline void cal_process_buffer_complete(struct cal_ctx *ctx)
1178 {
1179         ctx->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns();
1180         ctx->cur_frm->vb.field = ctx->m_fmt.field;
1181         ctx->cur_frm->vb.sequence = ctx->sequence++;
1182
1183         vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
1184         ctx->cur_frm = ctx->next_frm;
1185 }
1186
1187 #define isvcirqset(irq, vc, ff) (irq & \
1188         (CAL_CSI2_VC_IRQENABLE_ ##ff ##_IRQ_##vc ##_MASK))
1189
1190 #define isportirqset(irq, port) (irq & CAL_HL_IRQ_MASK(port))
1191
1192 static irqreturn_t cal_irq(int irq_cal, void *data)
1193 {
1194         struct cal_dev *dev = (struct cal_dev *)data;
1195         struct cal_ctx *ctx;
1196         struct cal_dmaqueue *dma_q;
1197         u32 irqst2, irqst3;
1198
1199         /* Check which DMA just finished */
1200         irqst2 = reg_read(dev, CAL_HL_IRQSTATUS(2));
1201         if (irqst2) {
1202                 /* Clear Interrupt status */
1203                 reg_write(dev, CAL_HL_IRQSTATUS(2), irqst2);
1204
1205                 /* Need to check both port */
1206                 if (isportirqset(irqst2, 1)) {
1207                         ctx = dev->ctx[0];
1208
1209                         if (ctx->cur_frm != ctx->next_frm)
1210                                 cal_process_buffer_complete(ctx);
1211                 }
1212
1213                 if (isportirqset(irqst2, 2)) {
1214                         ctx = dev->ctx[1];
1215
1216                         if (ctx->cur_frm != ctx->next_frm)
1217                                 cal_process_buffer_complete(ctx);
1218                 }
1219         }
1220
1221         /* Check which DMA just started */
1222         irqst3 = reg_read(dev, CAL_HL_IRQSTATUS(3));
1223         if (irqst3) {
1224                 /* Clear Interrupt status */
1225                 reg_write(dev, CAL_HL_IRQSTATUS(3), irqst3);
1226
1227                 /* Need to check both port */
1228                 if (isportirqset(irqst3, 1)) {
1229                         ctx = dev->ctx[0];
1230                         dma_q = &ctx->vidq;
1231
1232                         spin_lock(&ctx->slock);
1233                         if (!list_empty(&dma_q->active) &&
1234                             ctx->cur_frm == ctx->next_frm)
1235                                 cal_schedule_next_buffer(ctx);
1236                         spin_unlock(&ctx->slock);
1237                 }
1238
1239                 if (isportirqset(irqst3, 2)) {
1240                         ctx = dev->ctx[1];
1241                         dma_q = &ctx->vidq;
1242
1243                         spin_lock(&ctx->slock);
1244                         if (!list_empty(&dma_q->active) &&
1245                             ctx->cur_frm == ctx->next_frm)
1246                                 cal_schedule_next_buffer(ctx);
1247                         spin_unlock(&ctx->slock);
1248                 }
1249         }
1250
1251         return IRQ_HANDLED;
1252 }
1253
1254 /*
1255  * video ioctls
1256  */
1257 static int cal_querycap(struct file *file, void *priv,
1258                         struct v4l2_capability *cap)
1259 {
1260         struct cal_ctx *ctx = video_drvdata(file);
1261
1262         strscpy(cap->driver, CAL_MODULE_NAME, sizeof(cap->driver));
1263         strscpy(cap->card, CAL_MODULE_NAME, sizeof(cap->card));
1264
1265         snprintf(cap->bus_info, sizeof(cap->bus_info),
1266                  "platform:%s", ctx->v4l2_dev.name);
1267         return 0;
1268 }
1269
1270 static int cal_enum_fmt_vid_cap(struct file *file, void  *priv,
1271                                 struct v4l2_fmtdesc *f)
1272 {
1273         struct cal_ctx *ctx = video_drvdata(file);
1274         const struct cal_fmt *fmt = NULL;
1275
1276         if (f->index >= ctx->num_active_fmt)
1277                 return -EINVAL;
1278
1279         fmt = ctx->active_fmt[f->index];
1280
1281         f->pixelformat = fmt->fourcc;
1282         f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1283         return 0;
1284 }
1285
1286 static int __subdev_get_format(struct cal_ctx *ctx,
1287                                struct v4l2_mbus_framefmt *fmt)
1288 {
1289         struct v4l2_subdev_format sd_fmt;
1290         struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
1291         int ret;
1292
1293         sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1294         sd_fmt.pad = 0;
1295
1296         ret = v4l2_subdev_call(ctx->sensor, pad, get_fmt, NULL, &sd_fmt);
1297         if (ret)
1298                 return ret;
1299
1300         *fmt = *mbus_fmt;
1301
1302         ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__,
1303                 fmt->width, fmt->height, fmt->code);
1304
1305         return 0;
1306 }
1307
1308 static int __subdev_set_format(struct cal_ctx *ctx,
1309                                struct v4l2_mbus_framefmt *fmt)
1310 {
1311         struct v4l2_subdev_format sd_fmt;
1312         struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
1313         int ret;
1314
1315         sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1316         sd_fmt.pad = 0;
1317         *mbus_fmt = *fmt;
1318
1319         ret = v4l2_subdev_call(ctx->sensor, pad, set_fmt, NULL, &sd_fmt);
1320         if (ret)
1321                 return ret;
1322
1323         ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__,
1324                 fmt->width, fmt->height, fmt->code);
1325
1326         return 0;
1327 }
1328
1329 static int cal_calc_format_size(struct cal_ctx *ctx,
1330                                 const struct cal_fmt *fmt,
1331                                 struct v4l2_format *f)
1332 {
1333         u32 bpl, max_width;
1334
1335         if (!fmt) {
1336                 ctx_dbg(3, ctx, "No cal_fmt provided!\n");
1337                 return -EINVAL;
1338         }
1339
1340         /*
1341          * Maximum width is bound by the DMA max width in bytes.
1342          * We need to recalculate the actual maxi width depending on the
1343          * number of bytes per pixels required.
1344          */
1345         max_width = MAX_WIDTH_BYTES / (ALIGN(fmt->bpp, 8) >> 3);
1346         v4l_bound_align_image(&f->fmt.pix.width, 48, max_width, 2,
1347                               &f->fmt.pix.height, 32, MAX_HEIGHT_LINES, 0, 0);
1348
1349         bpl = (f->fmt.pix.width * ALIGN(fmt->bpp, 8)) >> 3;
1350         f->fmt.pix.bytesperline = ALIGN(bpl, 16);
1351
1352         f->fmt.pix.sizeimage = f->fmt.pix.height *
1353                                f->fmt.pix.bytesperline;
1354
1355         ctx_dbg(3, ctx, "%s: fourcc: %s size: %dx%d bpl:%d img_size:%d\n",
1356                 __func__, fourcc_to_str(f->fmt.pix.pixelformat),
1357                 f->fmt.pix.width, f->fmt.pix.height,
1358                 f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
1359
1360         return 0;
1361 }
1362
1363 static int cal_g_fmt_vid_cap(struct file *file, void *priv,
1364                              struct v4l2_format *f)
1365 {
1366         struct cal_ctx *ctx = video_drvdata(file);
1367
1368         *f = ctx->v_fmt;
1369
1370         return 0;
1371 }
1372
1373 static int cal_try_fmt_vid_cap(struct file *file, void *priv,
1374                                struct v4l2_format *f)
1375 {
1376         struct cal_ctx *ctx = video_drvdata(file);
1377         const struct cal_fmt *fmt;
1378         struct v4l2_subdev_frame_size_enum fse;
1379         int ret, found;
1380
1381         fmt = find_format_by_pix(ctx, f->fmt.pix.pixelformat);
1382         if (!fmt) {
1383                 ctx_dbg(3, ctx, "Fourcc format (0x%08x) not found.\n",
1384                         f->fmt.pix.pixelformat);
1385
1386                 /* Just get the first one enumerated */
1387                 fmt = ctx->active_fmt[0];
1388                 f->fmt.pix.pixelformat = fmt->fourcc;
1389         }
1390
1391         f->fmt.pix.field = ctx->v_fmt.fmt.pix.field;
1392
1393         /* check for/find a valid width/height */
1394         ret = 0;
1395         found = false;
1396         fse.pad = 0;
1397         fse.code = fmt->code;
1398         fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1399         for (fse.index = 0; ; fse.index++) {
1400                 ret = v4l2_subdev_call(ctx->sensor, pad, enum_frame_size,
1401                                        NULL, &fse);
1402                 if (ret)
1403                         break;
1404
1405                 if ((f->fmt.pix.width == fse.max_width) &&
1406                     (f->fmt.pix.height == fse.max_height)) {
1407                         found = true;
1408                         break;
1409                 } else if ((f->fmt.pix.width >= fse.min_width) &&
1410                          (f->fmt.pix.width <= fse.max_width) &&
1411                          (f->fmt.pix.height >= fse.min_height) &&
1412                          (f->fmt.pix.height <= fse.max_height)) {
1413                         found = true;
1414                         break;
1415                 }
1416         }
1417
1418         if (!found) {
1419                 /* use existing values as default */
1420                 f->fmt.pix.width = ctx->v_fmt.fmt.pix.width;
1421                 f->fmt.pix.height =  ctx->v_fmt.fmt.pix.height;
1422         }
1423
1424         /*
1425          * Use current colorspace for now, it will get
1426          * updated properly during s_fmt
1427          */
1428         f->fmt.pix.colorspace = ctx->v_fmt.fmt.pix.colorspace;
1429         return cal_calc_format_size(ctx, fmt, f);
1430 }
1431
1432 static int cal_s_fmt_vid_cap(struct file *file, void *priv,
1433                              struct v4l2_format *f)
1434 {
1435         struct cal_ctx *ctx = video_drvdata(file);
1436         struct vb2_queue *q = &ctx->vb_vidq;
1437         const struct cal_fmt *fmt;
1438         struct v4l2_mbus_framefmt mbus_fmt;
1439         int ret;
1440
1441         if (vb2_is_busy(q)) {
1442                 ctx_dbg(3, ctx, "%s device busy\n", __func__);
1443                 return -EBUSY;
1444         }
1445
1446         ret = cal_try_fmt_vid_cap(file, priv, f);
1447         if (ret < 0)
1448                 return ret;
1449
1450         fmt = find_format_by_pix(ctx, f->fmt.pix.pixelformat);
1451
1452         v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, fmt->code);
1453
1454         ret = __subdev_set_format(ctx, &mbus_fmt);
1455         if (ret)
1456                 return ret;
1457
1458         /* Just double check nothing has gone wrong */
1459         if (mbus_fmt.code != fmt->code) {
1460                 ctx_dbg(3, ctx,
1461                         "%s subdev changed format on us, this should not happen\n",
1462                         __func__);
1463                 return -EINVAL;
1464         }
1465
1466         v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt);
1467         ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1468         ctx->v_fmt.fmt.pix.pixelformat  = fmt->fourcc;
1469         cal_calc_format_size(ctx, fmt, &ctx->v_fmt);
1470         ctx->fmt = fmt;
1471         ctx->m_fmt = mbus_fmt;
1472         *f = ctx->v_fmt;
1473
1474         return 0;
1475 }
1476
1477 static int cal_enum_framesizes(struct file *file, void *fh,
1478                                struct v4l2_frmsizeenum *fsize)
1479 {
1480         struct cal_ctx *ctx = video_drvdata(file);
1481         const struct cal_fmt *fmt;
1482         struct v4l2_subdev_frame_size_enum fse;
1483         int ret;
1484
1485         /* check for valid format */
1486         fmt = find_format_by_pix(ctx, fsize->pixel_format);
1487         if (!fmt) {
1488                 ctx_dbg(3, ctx, "Invalid pixel code: %x\n",
1489                         fsize->pixel_format);
1490                 return -EINVAL;
1491         }
1492
1493         fse.index = fsize->index;
1494         fse.pad = 0;
1495         fse.code = fmt->code;
1496         fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1497
1498         ret = v4l2_subdev_call(ctx->sensor, pad, enum_frame_size, NULL, &fse);
1499         if (ret)
1500                 return ret;
1501
1502         ctx_dbg(1, ctx, "%s: index: %d code: %x W:[%d,%d] H:[%d,%d]\n",
1503                 __func__, fse.index, fse.code, fse.min_width, fse.max_width,
1504                 fse.min_height, fse.max_height);
1505
1506         fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1507         fsize->discrete.width = fse.max_width;
1508         fsize->discrete.height = fse.max_height;
1509
1510         return 0;
1511 }
1512
1513 static int cal_enum_input(struct file *file, void *priv,
1514                           struct v4l2_input *inp)
1515 {
1516         if (inp->index >= CAL_NUM_INPUT)
1517                 return -EINVAL;
1518
1519         inp->type = V4L2_INPUT_TYPE_CAMERA;
1520         sprintf(inp->name, "Camera %u", inp->index);
1521         return 0;
1522 }
1523
1524 static int cal_g_input(struct file *file, void *priv, unsigned int *i)
1525 {
1526         struct cal_ctx *ctx = video_drvdata(file);
1527
1528         *i = ctx->input;
1529         return 0;
1530 }
1531
1532 static int cal_s_input(struct file *file, void *priv, unsigned int i)
1533 {
1534         struct cal_ctx *ctx = video_drvdata(file);
1535
1536         if (i >= CAL_NUM_INPUT)
1537                 return -EINVAL;
1538
1539         ctx->input = i;
1540         return 0;
1541 }
1542
1543 /* timeperframe is arbitrary and continuous */
1544 static int cal_enum_frameintervals(struct file *file, void *priv,
1545                                    struct v4l2_frmivalenum *fival)
1546 {
1547         struct cal_ctx *ctx = video_drvdata(file);
1548         const struct cal_fmt *fmt;
1549         struct v4l2_subdev_frame_interval_enum fie = {
1550                 .index = fival->index,
1551                 .width = fival->width,
1552                 .height = fival->height,
1553                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1554         };
1555         int ret;
1556
1557         fmt = find_format_by_pix(ctx, fival->pixel_format);
1558         if (!fmt)
1559                 return -EINVAL;
1560
1561         fie.code = fmt->code;
1562         ret = v4l2_subdev_call(ctx->sensor, pad, enum_frame_interval,
1563                                NULL, &fie);
1564         if (ret)
1565                 return ret;
1566         fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
1567         fival->discrete = fie.interval;
1568
1569         return 0;
1570 }
1571
1572 /*
1573  * Videobuf operations
1574  */
1575 static int cal_queue_setup(struct vb2_queue *vq,
1576                            unsigned int *nbuffers, unsigned int *nplanes,
1577                            unsigned int sizes[], struct device *alloc_devs[])
1578 {
1579         struct cal_ctx *ctx = vb2_get_drv_priv(vq);
1580         unsigned size = ctx->v_fmt.fmt.pix.sizeimage;
1581
1582         if (vq->num_buffers + *nbuffers < 3)
1583                 *nbuffers = 3 - vq->num_buffers;
1584
1585         if (*nplanes) {
1586                 if (sizes[0] < size)
1587                         return -EINVAL;
1588                 size = sizes[0];
1589         }
1590
1591         *nplanes = 1;
1592         sizes[0] = size;
1593
1594         ctx_dbg(3, ctx, "nbuffers=%d, size=%d\n", *nbuffers, sizes[0]);
1595
1596         return 0;
1597 }
1598
1599 static int cal_buffer_prepare(struct vb2_buffer *vb)
1600 {
1601         struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1602         struct cal_buffer *buf = container_of(vb, struct cal_buffer,
1603                                               vb.vb2_buf);
1604         unsigned long size;
1605
1606         if (WARN_ON(!ctx->fmt))
1607                 return -EINVAL;
1608
1609         size = ctx->v_fmt.fmt.pix.sizeimage;
1610         if (vb2_plane_size(vb, 0) < size) {
1611                 ctx_err(ctx,
1612                         "data will not fit into plane (%lu < %lu)\n",
1613                         vb2_plane_size(vb, 0), size);
1614                 return -EINVAL;
1615         }
1616
1617         vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);
1618         return 0;
1619 }
1620
1621 static void cal_buffer_queue(struct vb2_buffer *vb)
1622 {
1623         struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1624         struct cal_buffer *buf = container_of(vb, struct cal_buffer,
1625                                               vb.vb2_buf);
1626         struct cal_dmaqueue *vidq = &ctx->vidq;
1627         unsigned long flags = 0;
1628
1629         /* recheck locking */
1630         spin_lock_irqsave(&ctx->slock, flags);
1631         list_add_tail(&buf->list, &vidq->active);
1632         spin_unlock_irqrestore(&ctx->slock, flags);
1633 }
1634
1635 static int cal_start_streaming(struct vb2_queue *vq, unsigned int count)
1636 {
1637         struct cal_ctx *ctx = vb2_get_drv_priv(vq);
1638         struct cal_dmaqueue *dma_q = &ctx->vidq;
1639         struct cal_buffer *buf, *tmp;
1640         unsigned long addr = 0;
1641         unsigned long flags;
1642         int ret;
1643
1644         spin_lock_irqsave(&ctx->slock, flags);
1645         if (list_empty(&dma_q->active)) {
1646                 spin_unlock_irqrestore(&ctx->slock, flags);
1647                 ctx_dbg(3, ctx, "buffer queue is empty\n");
1648                 return -EIO;
1649         }
1650
1651         buf = list_entry(dma_q->active.next, struct cal_buffer, list);
1652         ctx->cur_frm = buf;
1653         ctx->next_frm = buf;
1654         list_del(&buf->list);
1655         spin_unlock_irqrestore(&ctx->slock, flags);
1656
1657         addr = vb2_dma_contig_plane_dma_addr(&ctx->cur_frm->vb.vb2_buf, 0);
1658         ctx->sequence = 0;
1659
1660         ret = cal_get_external_info(ctx);
1661         if (ret < 0)
1662                 goto err;
1663
1664         ret = v4l2_subdev_call(ctx->sensor, core, s_power, 1);
1665         if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV) {
1666                 ctx_err(ctx, "power on failed in subdev\n");
1667                 goto err;
1668         }
1669
1670         cal_runtime_get(ctx->dev);
1671
1672         csi2_ctx_config(ctx);
1673         pix_proc_config(ctx);
1674         cal_wr_dma_config(ctx, ctx->v_fmt.fmt.pix.bytesperline,
1675                           ctx->v_fmt.fmt.pix.height);
1676         csi2_lane_config(ctx);
1677
1678         enable_irqs(ctx);
1679         csi2_phy_init(ctx);
1680
1681         ret = v4l2_subdev_call(ctx->sensor, video, s_stream, 1);
1682         if (ret) {
1683                 v4l2_subdev_call(ctx->sensor, core, s_power, 0);
1684                 ctx_err(ctx, "stream on failed in subdev\n");
1685                 cal_runtime_put(ctx->dev);
1686                 goto err;
1687         }
1688
1689         csi2_wait_for_phy(ctx);
1690         cal_wr_dma_addr(ctx, addr);
1691         csi2_ppi_enable(ctx);
1692
1693         if (debug >= 4)
1694                 cal_quickdump_regs(ctx->dev);
1695
1696         return 0;
1697
1698 err:
1699         spin_lock_irqsave(&ctx->slock, flags);
1700         vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
1701         ctx->cur_frm = NULL;
1702         ctx->next_frm = NULL;
1703         list_for_each_entry_safe(buf, tmp, &dma_q->active, list) {
1704                 list_del(&buf->list);
1705                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
1706         }
1707         spin_unlock_irqrestore(&ctx->slock, flags);
1708         return ret;
1709 }
1710
1711 static void cal_stop_streaming(struct vb2_queue *vq)
1712 {
1713         struct cal_ctx *ctx = vb2_get_drv_priv(vq);
1714         struct cal_dmaqueue *dma_q = &ctx->vidq;
1715         struct cal_buffer *buf, *tmp;
1716         unsigned long flags;
1717         int ret;
1718
1719         csi2_ppi_disable(ctx);
1720         disable_irqs(ctx);
1721         csi2_phy_deinit(ctx);
1722
1723         if (v4l2_subdev_call(ctx->sensor, video, s_stream, 0))
1724                 ctx_err(ctx, "stream off failed in subdev\n");
1725
1726         ret = v4l2_subdev_call(ctx->sensor, core, s_power, 0);
1727         if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
1728                 ctx_err(ctx, "power off failed in subdev\n");
1729
1730         /* Release all active buffers */
1731         spin_lock_irqsave(&ctx->slock, flags);
1732         list_for_each_entry_safe(buf, tmp, &dma_q->active, list) {
1733                 list_del(&buf->list);
1734                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
1735         }
1736
1737         if (ctx->cur_frm == ctx->next_frm) {
1738                 vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR);
1739         } else {
1740                 vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR);
1741                 vb2_buffer_done(&ctx->next_frm->vb.vb2_buf,
1742                                 VB2_BUF_STATE_ERROR);
1743         }
1744         ctx->cur_frm = NULL;
1745         ctx->next_frm = NULL;
1746         spin_unlock_irqrestore(&ctx->slock, flags);
1747
1748         cal_runtime_put(ctx->dev);
1749 }
1750
1751 static const struct vb2_ops cal_video_qops = {
1752         .queue_setup            = cal_queue_setup,
1753         .buf_prepare            = cal_buffer_prepare,
1754         .buf_queue              = cal_buffer_queue,
1755         .start_streaming        = cal_start_streaming,
1756         .stop_streaming         = cal_stop_streaming,
1757         .wait_prepare           = vb2_ops_wait_prepare,
1758         .wait_finish            = vb2_ops_wait_finish,
1759 };
1760
1761 static const struct v4l2_file_operations cal_fops = {
1762         .owner          = THIS_MODULE,
1763         .open           = v4l2_fh_open,
1764         .release        = vb2_fop_release,
1765         .read           = vb2_fop_read,
1766         .poll           = vb2_fop_poll,
1767         .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
1768         .mmap           = vb2_fop_mmap,
1769 };
1770
1771 static const struct v4l2_ioctl_ops cal_ioctl_ops = {
1772         .vidioc_querycap      = cal_querycap,
1773         .vidioc_enum_fmt_vid_cap  = cal_enum_fmt_vid_cap,
1774         .vidioc_g_fmt_vid_cap     = cal_g_fmt_vid_cap,
1775         .vidioc_try_fmt_vid_cap   = cal_try_fmt_vid_cap,
1776         .vidioc_s_fmt_vid_cap     = cal_s_fmt_vid_cap,
1777         .vidioc_enum_framesizes   = cal_enum_framesizes,
1778         .vidioc_reqbufs       = vb2_ioctl_reqbufs,
1779         .vidioc_create_bufs   = vb2_ioctl_create_bufs,
1780         .vidioc_prepare_buf   = vb2_ioctl_prepare_buf,
1781         .vidioc_querybuf      = vb2_ioctl_querybuf,
1782         .vidioc_qbuf          = vb2_ioctl_qbuf,
1783         .vidioc_dqbuf         = vb2_ioctl_dqbuf,
1784         .vidioc_expbuf        = vb2_ioctl_expbuf,
1785         .vidioc_enum_input    = cal_enum_input,
1786         .vidioc_g_input       = cal_g_input,
1787         .vidioc_s_input       = cal_s_input,
1788         .vidioc_enum_frameintervals = cal_enum_frameintervals,
1789         .vidioc_streamon      = vb2_ioctl_streamon,
1790         .vidioc_streamoff     = vb2_ioctl_streamoff,
1791         .vidioc_log_status    = v4l2_ctrl_log_status,
1792         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1793         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1794 };
1795
1796 static const struct video_device cal_videodev = {
1797         .name           = CAL_MODULE_NAME,
1798         .fops           = &cal_fops,
1799         .ioctl_ops      = &cal_ioctl_ops,
1800         .minor          = -1,
1801         .release        = video_device_release_empty,
1802         .device_caps    = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
1803                           V4L2_CAP_READWRITE,
1804 };
1805
1806 /* -----------------------------------------------------------------
1807  *      Initialization and module stuff
1808  * ------------------------------------------------------------------
1809  */
1810 static int cal_complete_ctx(struct cal_ctx *ctx);
1811
1812 static int cal_async_bound(struct v4l2_async_notifier *notifier,
1813                            struct v4l2_subdev *subdev,
1814                            struct v4l2_async_subdev *asd)
1815 {
1816         struct cal_ctx *ctx = notifier_to_ctx(notifier);
1817         struct v4l2_subdev_mbus_code_enum mbus_code;
1818         int ret = 0;
1819         int i, j, k;
1820
1821         if (ctx->sensor) {
1822                 ctx_info(ctx, "Rejecting subdev %s (Already set!!)",
1823                          subdev->name);
1824                 return 0;
1825         }
1826
1827         ctx->sensor = subdev;
1828         ctx_dbg(1, ctx, "Using sensor %s for capture\n", subdev->name);
1829
1830         /* Enumerate sub device formats and enable all matching local formats */
1831         ctx->num_active_fmt = 0;
1832         for (j = 0, i = 0; ret != -EINVAL; ++j) {
1833                 struct cal_fmt *fmt;
1834
1835                 memset(&mbus_code, 0, sizeof(mbus_code));
1836                 mbus_code.index = j;
1837                 mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1838                 ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
1839                                        NULL, &mbus_code);
1840                 if (ret)
1841                         continue;
1842
1843                 ctx_dbg(2, ctx,
1844                         "subdev %s: code: %04x idx: %d\n",
1845                         subdev->name, mbus_code.code, j);
1846
1847                 for (k = 0; k < ARRAY_SIZE(cal_formats); k++) {
1848                         fmt = &cal_formats[k];
1849
1850                         if (mbus_code.code == fmt->code) {
1851                                 ctx->active_fmt[i] = fmt;
1852                                 ctx_dbg(2, ctx,
1853                                         "matched fourcc: %s: code: %04x idx: %d\n",
1854                                         fourcc_to_str(fmt->fourcc),
1855                                         fmt->code, i);
1856                                 ctx->num_active_fmt = ++i;
1857                         }
1858                 }
1859         }
1860
1861         if (i == 0) {
1862                 ctx_err(ctx, "No suitable format reported by subdev %s\n",
1863                         subdev->name);
1864                 return -EINVAL;
1865         }
1866
1867         cal_complete_ctx(ctx);
1868
1869         return 0;
1870 }
1871
1872 static int cal_async_complete(struct v4l2_async_notifier *notifier)
1873 {
1874         struct cal_ctx *ctx = notifier_to_ctx(notifier);
1875         const struct cal_fmt *fmt;
1876         struct v4l2_mbus_framefmt mbus_fmt;
1877         int ret;
1878
1879         ret = __subdev_get_format(ctx, &mbus_fmt);
1880         if (ret)
1881                 return ret;
1882
1883         fmt = find_format_by_code(ctx, mbus_fmt.code);
1884         if (!fmt) {
1885                 ctx_dbg(3, ctx, "mbus code format (0x%08x) not found.\n",
1886                         mbus_fmt.code);
1887                 return -EINVAL;
1888         }
1889
1890         /* Save current subdev format */
1891         v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt);
1892         ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1893         ctx->v_fmt.fmt.pix.pixelformat  = fmt->fourcc;
1894         cal_calc_format_size(ctx, fmt, &ctx->v_fmt);
1895         ctx->fmt = fmt;
1896         ctx->m_fmt = mbus_fmt;
1897
1898         return 0;
1899 }
1900
1901 static const struct v4l2_async_notifier_operations cal_async_ops = {
1902         .bound = cal_async_bound,
1903         .complete = cal_async_complete,
1904 };
1905
1906 static int cal_complete_ctx(struct cal_ctx *ctx)
1907 {
1908         struct video_device *vfd;
1909         struct vb2_queue *q;
1910         int ret;
1911
1912         ctx->timeperframe = tpf_default;
1913         ctx->external_rate = 192000000;
1914
1915         /* initialize locks */
1916         spin_lock_init(&ctx->slock);
1917         mutex_init(&ctx->mutex);
1918
1919         /* initialize queue */
1920         q = &ctx->vb_vidq;
1921         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1922         q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ;
1923         q->drv_priv = ctx;
1924         q->buf_struct_size = sizeof(struct cal_buffer);
1925         q->ops = &cal_video_qops;
1926         q->mem_ops = &vb2_dma_contig_memops;
1927         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1928         q->lock = &ctx->mutex;
1929         q->min_buffers_needed = 3;
1930         q->dev = ctx->v4l2_dev.dev;
1931
1932         ret = vb2_queue_init(q);
1933         if (ret)
1934                 return ret;
1935
1936         /* init video dma queues */
1937         INIT_LIST_HEAD(&ctx->vidq.active);
1938
1939         vfd = &ctx->vdev;
1940         *vfd = cal_videodev;
1941         vfd->v4l2_dev = &ctx->v4l2_dev;
1942         vfd->queue = q;
1943
1944         /*
1945          * Provide a mutex to v4l2 core. It will be used to protect
1946          * all fops and v4l2 ioctls.
1947          */
1948         vfd->lock = &ctx->mutex;
1949         video_set_drvdata(vfd, ctx);
1950
1951         ret = video_register_device(vfd, VFL_TYPE_GRABBER, video_nr);
1952         if (ret < 0)
1953                 return ret;
1954
1955         v4l2_info(&ctx->v4l2_dev, "V4L2 device registered as %s\n",
1956                   video_device_node_name(vfd));
1957
1958         return 0;
1959 }
1960
1961 static struct device_node *
1962 of_get_next_port(const struct device_node *parent,
1963                  struct device_node *prev)
1964 {
1965         struct device_node *port = NULL;
1966
1967         if (!parent)
1968                 return NULL;
1969
1970         if (!prev) {
1971                 struct device_node *ports;
1972                 /*
1973                  * It's the first call, we have to find a port subnode
1974                  * within this node or within an optional 'ports' node.
1975                  */
1976                 ports = of_get_child_by_name(parent, "ports");
1977                 if (ports)
1978                         parent = ports;
1979
1980                 port = of_get_child_by_name(parent, "port");
1981
1982                 /* release the 'ports' node */
1983                 of_node_put(ports);
1984         } else {
1985                 struct device_node *ports;
1986
1987                 ports = of_get_parent(prev);
1988                 if (!ports)
1989                         return NULL;
1990
1991                 do {
1992                         port = of_get_next_child(ports, prev);
1993                         if (!port) {
1994                                 of_node_put(ports);
1995                                 return NULL;
1996                         }
1997                         prev = port;
1998                 } while (!of_node_name_eq(port, "port"));
1999                 of_node_put(ports);
2000         }
2001
2002         return port;
2003 }
2004
2005 static struct device_node *
2006 of_get_next_endpoint(const struct device_node *parent,
2007                      struct device_node *prev)
2008 {
2009         struct device_node *ep = NULL;
2010
2011         if (!parent)
2012                 return NULL;
2013
2014         do {
2015                 ep = of_get_next_child(parent, prev);
2016                 if (!ep)
2017                         return NULL;
2018                 prev = ep;
2019         } while (!of_node_name_eq(ep, "endpoint"));
2020
2021         return ep;
2022 }
2023
2024 static int of_cal_create_instance(struct cal_ctx *ctx, int inst)
2025 {
2026         struct platform_device *pdev = ctx->dev->pdev;
2027         struct device_node *ep_node, *port, *sensor_node, *parent;
2028         struct v4l2_fwnode_endpoint *endpoint;
2029         struct v4l2_async_subdev *asd;
2030         u32 regval = 0;
2031         int ret, index, found_port = 0, lane;
2032
2033         parent = pdev->dev.of_node;
2034
2035         asd = &ctx->asd;
2036         endpoint = &ctx->endpoint;
2037
2038         ep_node = NULL;
2039         port = NULL;
2040         sensor_node = NULL;
2041         ret = -EINVAL;
2042
2043         ctx_dbg(3, ctx, "Scanning Port node for csi2 port: %d\n", inst);
2044         for (index = 0; index < CAL_NUM_CSI2_PORTS; index++) {
2045                 port = of_get_next_port(parent, port);
2046                 if (!port) {
2047                         ctx_dbg(1, ctx, "No port node found for csi2 port:%d\n",
2048                                 index);
2049                         goto cleanup_exit;
2050                 }
2051
2052                 /* Match the slice number with <REG> */
2053                 of_property_read_u32(port, "reg", &regval);
2054                 ctx_dbg(3, ctx, "port:%d inst:%d <reg>:%d\n",
2055                         index, inst, regval);
2056                 if ((regval == inst) && (index == inst)) {
2057                         found_port = 1;
2058                         break;
2059                 }
2060         }
2061
2062         if (!found_port) {
2063                 ctx_dbg(1, ctx, "No port node matches csi2 port:%d\n",
2064                         inst);
2065                 goto cleanup_exit;
2066         }
2067
2068         ctx_dbg(3, ctx, "Scanning sub-device for csi2 port: %d\n",
2069                 inst);
2070
2071         ep_node = of_get_next_endpoint(port, ep_node);
2072         if (!ep_node) {
2073                 ctx_dbg(3, ctx, "can't get next endpoint\n");
2074                 goto cleanup_exit;
2075         }
2076
2077         sensor_node = of_graph_get_remote_port_parent(ep_node);
2078         if (!sensor_node) {
2079                 ctx_dbg(3, ctx, "can't get remote parent\n");
2080                 goto cleanup_exit;
2081         }
2082         asd->match_type = V4L2_ASYNC_MATCH_FWNODE;
2083         asd->match.fwnode = of_fwnode_handle(sensor_node);
2084
2085         v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), endpoint);
2086
2087         if (endpoint->bus_type != V4L2_MBUS_CSI2_DPHY) {
2088                 ctx_err(ctx, "Port:%d sub-device %pOFn is not a CSI2 device\n",
2089                         inst, sensor_node);
2090                 goto cleanup_exit;
2091         }
2092
2093         /* Store Virtual Channel number */
2094         ctx->virtual_channel = endpoint->base.id;
2095
2096         ctx_dbg(3, ctx, "Port:%d v4l2-endpoint: CSI2\n", inst);
2097         ctx_dbg(3, ctx, "Virtual Channel=%d\n", ctx->virtual_channel);
2098         ctx_dbg(3, ctx, "flags=0x%08x\n", endpoint->bus.mipi_csi2.flags);
2099         ctx_dbg(3, ctx, "clock_lane=%d\n", endpoint->bus.mipi_csi2.clock_lane);
2100         ctx_dbg(3, ctx, "num_data_lanes=%d\n",
2101                 endpoint->bus.mipi_csi2.num_data_lanes);
2102         ctx_dbg(3, ctx, "data_lanes= <\n");
2103         for (lane = 0; lane < endpoint->bus.mipi_csi2.num_data_lanes; lane++)
2104                 ctx_dbg(3, ctx, "\t%d\n",
2105                         endpoint->bus.mipi_csi2.data_lanes[lane]);
2106         ctx_dbg(3, ctx, "\t>\n");
2107
2108         ctx_dbg(1, ctx, "Port: %d found sub-device %pOFn\n",
2109                 inst, sensor_node);
2110
2111         v4l2_async_notifier_init(&ctx->notifier);
2112
2113         ret = v4l2_async_notifier_add_subdev(&ctx->notifier, asd);
2114         if (ret) {
2115                 ctx_err(ctx, "Error adding asd\n");
2116                 goto cleanup_exit;
2117         }
2118
2119         ctx->notifier.ops = &cal_async_ops;
2120         ret = v4l2_async_notifier_register(&ctx->v4l2_dev,
2121                                            &ctx->notifier);
2122         if (ret) {
2123                 ctx_err(ctx, "Error registering async notifier\n");
2124                 v4l2_async_notifier_cleanup(&ctx->notifier);
2125                 ret = -EINVAL;
2126         }
2127
2128         /*
2129          * On success we need to keep reference on sensor_node, or
2130          * if notifier_cleanup was called above, sensor_node was
2131          * already put.
2132          */
2133         sensor_node = NULL;
2134
2135 cleanup_exit:
2136         of_node_put(sensor_node);
2137         of_node_put(ep_node);
2138         of_node_put(port);
2139
2140         return ret;
2141 }
2142
2143 static struct cal_ctx *cal_create_instance(struct cal_dev *dev, int inst)
2144 {
2145         struct cal_ctx *ctx;
2146         struct v4l2_ctrl_handler *hdl;
2147         int ret;
2148
2149         ctx = devm_kzalloc(&dev->pdev->dev, sizeof(*ctx), GFP_KERNEL);
2150         if (!ctx)
2151                 return NULL;
2152
2153         /* save the cal_dev * for future ref */
2154         ctx->dev = dev;
2155
2156         snprintf(ctx->v4l2_dev.name, sizeof(ctx->v4l2_dev.name),
2157                  "%s-%03d", CAL_MODULE_NAME, inst);
2158         ret = v4l2_device_register(&dev->pdev->dev, &ctx->v4l2_dev);
2159         if (ret)
2160                 goto err_exit;
2161
2162         hdl = &ctx->ctrl_handler;
2163         ret = v4l2_ctrl_handler_init(hdl, 11);
2164         if (ret) {
2165                 ctx_err(ctx, "Failed to init ctrl handler\n");
2166                 goto unreg_dev;
2167         }
2168         ctx->v4l2_dev.ctrl_handler = hdl;
2169
2170         /* Make sure Camera Core H/W register area is available */
2171         ctx->cc = dev->cc[inst];
2172
2173         /* Store the instance id */
2174         ctx->csi2_port = inst + 1;
2175
2176         ret = of_cal_create_instance(ctx, inst);
2177         if (ret) {
2178                 ret = -EINVAL;
2179                 goto free_hdl;
2180         }
2181         return ctx;
2182
2183 free_hdl:
2184         v4l2_ctrl_handler_free(hdl);
2185 unreg_dev:
2186         v4l2_device_unregister(&ctx->v4l2_dev);
2187 err_exit:
2188         return NULL;
2189 }
2190
2191 static const struct of_device_id cal_of_match[];
2192
2193 static int cal_probe(struct platform_device *pdev)
2194 {
2195         struct cal_dev *dev;
2196         struct cal_ctx *ctx;
2197         struct device_node *parent = pdev->dev.of_node;
2198         struct regmap *syscon_camerrx = NULL;
2199         u32 syscon_camerrx_offset = 0;
2200         int ret;
2201         int irq;
2202         int i;
2203
2204         dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
2205         if (!dev)
2206                 return -ENOMEM;
2207
2208         dev->data = of_device_get_match_data(&pdev->dev);
2209         if (!dev->data) {
2210                 dev_err(&pdev->dev, "Could not get feature data based on compatible version\n");
2211                 return -ENODEV;
2212         }
2213
2214         dev->flags = dev->data->flags;
2215
2216         /* set pseudo v4l2 device name so we can use v4l2_printk */
2217         strscpy(dev->v4l2_dev.name, CAL_MODULE_NAME,
2218                 sizeof(dev->v4l2_dev.name));
2219
2220         /* save pdev pointer */
2221         dev->pdev = pdev;
2222
2223         syscon_camerrx = syscon_regmap_lookup_by_phandle(parent,
2224                                                          "ti,camerrx-control");
2225         ret = of_property_read_u32_index(parent, "ti,camerrx-control", 1,
2226                                          &syscon_camerrx_offset);
2227         if (IS_ERR(syscon_camerrx))
2228                 ret = PTR_ERR(syscon_camerrx);
2229         if (ret) {
2230                 dev_warn(&pdev->dev, "failed to get ti,camerrx-control: %d\n",
2231                          ret);
2232
2233                 /*
2234                  * Backward DTS compatibility.
2235                  * If syscon entry is not present then check if the
2236                  * camerrx_control resource is present.
2237                  */
2238                 syscon_camerrx = cal_get_camerarx_regmap(dev);
2239                 if (IS_ERR(syscon_camerrx)) {
2240                         dev_err(&pdev->dev, "failed to get camerrx_control regmap\n");
2241                         return PTR_ERR(syscon_camerrx);
2242                 }
2243                 /* In this case the base already point to the direct
2244                  * CM register so no need for an offset
2245                  */
2246                 syscon_camerrx_offset = 0;
2247         }
2248
2249         dev->syscon_camerrx = syscon_camerrx;
2250         dev->syscon_camerrx_offset = syscon_camerrx_offset;
2251         ret = cal_camerarx_regmap_init(dev);
2252         if (ret)
2253                 return ret;
2254
2255         dev->res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
2256                                                 "cal_top");
2257         dev->base = devm_ioremap_resource(&pdev->dev, dev->res);
2258         if (IS_ERR(dev->base))
2259                 return PTR_ERR(dev->base);
2260
2261         cal_dbg(1, dev, "ioresource %s at %pa - %pa\n",
2262                 dev->res->name, &dev->res->start, &dev->res->end);
2263
2264         irq = platform_get_irq(pdev, 0);
2265         cal_dbg(1, dev, "got irq# %d\n", irq);
2266         ret = devm_request_irq(&pdev->dev, irq, cal_irq, 0, CAL_MODULE_NAME,
2267                                dev);
2268         if (ret)
2269                 return ret;
2270
2271         platform_set_drvdata(pdev, dev);
2272
2273         dev->cc[0] = cc_create(dev, 0);
2274         if (IS_ERR(dev->cc[0]))
2275                 return PTR_ERR(dev->cc[0]);
2276
2277         if (cal_data_get_num_csi2_phy(dev) > 1) {
2278                 dev->cc[1] = cc_create(dev, 1);
2279                 if (IS_ERR(dev->cc[1]))
2280                         return PTR_ERR(dev->cc[1]);
2281         } else {
2282                 dev->cc[1] = NULL;
2283         }
2284
2285         dev->ctx[0] = NULL;
2286         dev->ctx[1] = NULL;
2287
2288         dev->ctx[0] = cal_create_instance(dev, 0);
2289         if (cal_data_get_num_csi2_phy(dev) > 1)
2290                 dev->ctx[1] = cal_create_instance(dev, 1);
2291         if (!dev->ctx[0] && !dev->ctx[1]) {
2292                 cal_err(dev, "Neither port is configured, no point in staying up\n");
2293                 return -ENODEV;
2294         }
2295
2296         pm_runtime_enable(&pdev->dev);
2297
2298         ret = cal_runtime_get(dev);
2299         if (ret)
2300                 goto runtime_disable;
2301
2302         /* Just check we can actually access the module */
2303         cal_get_hwinfo(dev);
2304
2305         cal_runtime_put(dev);
2306
2307         return 0;
2308
2309 runtime_disable:
2310         pm_runtime_disable(&pdev->dev);
2311         for (i = 0; i < CAL_NUM_CONTEXT; i++) {
2312                 ctx = dev->ctx[i];
2313                 if (ctx) {
2314                         v4l2_async_notifier_unregister(&ctx->notifier);
2315                         v4l2_async_notifier_cleanup(&ctx->notifier);
2316                         v4l2_ctrl_handler_free(&ctx->ctrl_handler);
2317                         v4l2_device_unregister(&ctx->v4l2_dev);
2318                 }
2319         }
2320
2321         return ret;
2322 }
2323
2324 static int cal_remove(struct platform_device *pdev)
2325 {
2326         struct cal_dev *dev =
2327                 (struct cal_dev *)platform_get_drvdata(pdev);
2328         struct cal_ctx *ctx;
2329         int i;
2330
2331         cal_dbg(1, dev, "Removing %s\n", CAL_MODULE_NAME);
2332
2333         cal_runtime_get(dev);
2334
2335         for (i = 0; i < CAL_NUM_CONTEXT; i++) {
2336                 ctx = dev->ctx[i];
2337                 if (ctx) {
2338                         ctx_dbg(1, ctx, "unregistering %s\n",
2339                                 video_device_node_name(&ctx->vdev));
2340                         camerarx_phy_disable(ctx);
2341                         v4l2_async_notifier_unregister(&ctx->notifier);
2342                         v4l2_async_notifier_cleanup(&ctx->notifier);
2343                         v4l2_ctrl_handler_free(&ctx->ctrl_handler);
2344                         v4l2_device_unregister(&ctx->v4l2_dev);
2345                         video_unregister_device(&ctx->vdev);
2346                 }
2347         }
2348
2349         cal_runtime_put(dev);
2350         pm_runtime_disable(&pdev->dev);
2351
2352         return 0;
2353 }
2354
2355 #if defined(CONFIG_OF)
2356 static const struct of_device_id cal_of_match[] = {
2357         {
2358                 .compatible = "ti,dra72-cal",
2359                 .data = (void *)&dra72x_cal_data,
2360         },
2361         {
2362                 .compatible = "ti,dra72-pre-es2-cal",
2363                 .data = (void *)&dra72x_es1_cal_data,
2364         },
2365         {
2366                 .compatible = "ti,dra76-cal",
2367                 .data = (void *)&dra76x_cal_data,
2368         },
2369         {
2370                 .compatible = "ti,am654-cal",
2371                 .data = (void *)&am654_cal_data,
2372         },
2373         {},
2374 };
2375 MODULE_DEVICE_TABLE(of, cal_of_match);
2376 #endif
2377
2378 static struct platform_driver cal_pdrv = {
2379         .probe          = cal_probe,
2380         .remove         = cal_remove,
2381         .driver         = {
2382                 .name   = CAL_MODULE_NAME,
2383                 .of_match_table = of_match_ptr(cal_of_match),
2384         },
2385 };
2386
2387 module_platform_driver(cal_pdrv);