drivers/media/platform/raspberrypi/rp1_cfe/csi2.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * RP1 CSI-2 Driver
   4  *
   5  * Copyright (C) 2021 - Raspberry Pi Ltd.
   6  *
   7  */
   8
   9 #include <linux/delay.h>
  10 #include <linux/moduleparam.h>
  11 #include <linux/pm_runtime.h>
  12 #include <linux/seq_file.h>
  13
  14 #include <media/videobuf2-dma-contig.h>
  15
  16 #include "csi2.h"
  17 #include "cfe.h"
  18
  19 static bool csi2_track_errors;
  20 module_param_named(track_csi2_errors, csi2_track_errors, bool, 0);
  21 MODULE_PARM_DESC(track_csi2_errors, "track csi-2 errors");
  22
  23 #define csi2_dbg_verbose(fmt, arg...)                             \
  24         do {                                                      \
  25                 if (cfe_debug_verbose)                            \
  26                         dev_dbg(csi2->v4l2_dev->dev, fmt, ##arg); \
  27         } while (0)
  28 #define csi2_dbg(fmt, arg...) dev_dbg(csi2->v4l2_dev->dev, fmt, ##arg)
  29 #define csi2_info(fmt, arg...) dev_info(csi2->v4l2_dev->dev, fmt, ##arg)
  30 #define csi2_err(fmt, arg...) dev_err(csi2->v4l2_dev->dev, fmt, ##arg)
  31
  32 /* CSI2-DMA registers */
  33 #define CSI2_STATUS             0x000
  34 #define CSI2_QOS                0x004
  35 #define CSI2_DISCARDS_OVERFLOW  0x008
  36 #define CSI2_DISCARDS_INACTIVE  0x00c
  37 #define CSI2_DISCARDS_UNMATCHED 0x010
  38 #define CSI2_DISCARDS_LEN_LIMIT 0x014
  39
  40 #define CSI2_DISCARDS_AMOUNT_SHIFT      0
  41 #define CSI2_DISCARDS_AMOUNT_MASK       GENMASK(23, 0)
  42 #define CSI2_DISCARDS_DT_SHIFT          24
  43 #define CSI2_DISCARDS_DT_MASK           GENMASK(29, 24)
  44 #define CSI2_DISCARDS_VC_SHIFT          30
  45 #define CSI2_DISCARDS_VC_MASK           GENMASK(31, 30)
  46
  47 #define CSI2_LLEV_PANICS        0x018
  48 #define CSI2_ULEV_PANICS        0x01c
  49 #define CSI2_IRQ_MASK           0x020
  50 #define CSI2_IRQ_MASK_IRQ_OVERFLOW              BIT(0)
  51 #define CSI2_IRQ_MASK_IRQ_DISCARD_OVERFLOW      BIT(1)
  52 #define CSI2_IRQ_MASK_IRQ_DISCARD_LENGTH_LIMIT  BIT(2)
  53 #define CSI2_IRQ_MASK_IRQ_DISCARD_UNMATCHED     BIT(3)
  54 #define CSI2_IRQ_MASK_IRQ_DISCARD_INACTIVE      BIT(4)
  55 #define CSI2_IRQ_MASK_IRQ_ALL                                              \
  56         (CSI2_IRQ_MASK_IRQ_OVERFLOW | CSI2_IRQ_MASK_IRQ_DISCARD_OVERFLOW | \
  57          CSI2_IRQ_MASK_IRQ_DISCARD_LENGTH_LIMIT |                          \
  58          CSI2_IRQ_MASK_IRQ_DISCARD_UNMATCHED |                             \
  59          CSI2_IRQ_MASK_IRQ_DISCARD_INACTIVE)
  60
  61 #define CSI2_CTRL               0x024
  62 #define CSI2_CH_CTRL(x)         ((x) * 0x40 + 0x28)
  63 #define CSI2_CH_ADDR0(x)        ((x) * 0x40 + 0x2c)
  64 #define CSI2_CH_ADDR1(x)        ((x) * 0x40 + 0x3c)
  65 #define CSI2_CH_STRIDE(x)       ((x) * 0x40 + 0x30)
  66 #define CSI2_CH_LENGTH(x)       ((x) * 0x40 + 0x34)
  67 #define CSI2_CH_DEBUG(x)        ((x) * 0x40 + 0x38)
  68 #define CSI2_CH_FRAME_SIZE(x)   ((x) * 0x40 + 0x40)
  69 #define CSI2_CH_COMP_CTRL(x)    ((x) * 0x40 + 0x44)
  70 #define CSI2_CH_FE_FRAME_ID(x)  ((x) * 0x40 + 0x48)
  71
  72 /* CSI2_STATUS */
  73 #define IRQ_FS(x)               (BIT(0) << (x))
  74 #define IRQ_FE(x)               (BIT(4) << (x))
  75 #define IRQ_FE_ACK(x)           (BIT(8) << (x))
  76 #define IRQ_LE(x)               (BIT(12) << (x))
  77 #define IRQ_LE_ACK(x)           (BIT(16) << (x))
  78 #define IRQ_CH_MASK(x)          (IRQ_FS(x) | IRQ_FE(x) | IRQ_FE_ACK(x) | IRQ_LE(x) | IRQ_LE_ACK(x))
  79 #define IRQ_OVERFLOW            BIT(20)
  80 #define IRQ_DISCARD_OVERFLOW    BIT(21)
  81 #define IRQ_DISCARD_LEN_LIMIT   BIT(22)
  82 #define IRQ_DISCARD_UNMATCHED   BIT(23)
  83 #define IRQ_DISCARD_INACTIVE    BIT(24)
  84
  85 /* CSI2_CTRL */
  86 #define EOP_IS_EOL              BIT(0)
  87
  88 /* CSI2_CH_CTRL */
  89 #define DMA_EN                  BIT(0)
  90 #define FORCE                   BIT(3)
  91 #define AUTO_ARM                BIT(4)
  92 #define IRQ_EN_FS               BIT(13)
  93 #define IRQ_EN_FE               BIT(14)
  94 #define IRQ_EN_FE_ACK           BIT(15)
  95 #define IRQ_EN_LE               BIT(16)
  96 #define IRQ_EN_LE_ACK           BIT(17)
  97 #define FLUSH_FE                BIT(28)
  98 #define PACK_LINE               BIT(29)
  99 #define PACK_BYTES              BIT(30)
 100 #define CH_MODE_MASK            GENMASK(2, 1)
 101 #define VC_MASK                 GENMASK(6, 5)
 102 #define DT_MASK                 GENMASK(12, 7)
 103 #define LC_MASK                 GENMASK(27, 18)
 104
 105 /* CHx_COMPRESSION_CONTROL */
 106 #define COMP_OFFSET_MASK        GENMASK(15, 0)
 107 #define COMP_SHIFT_MASK         GENMASK(19, 16)
 108 #define COMP_MODE_MASK          GENMASK(25, 24)
 109
 110 static inline u32 csi2_reg_read(struct csi2_device *csi2, u32 offset)
 111 {
 112         return readl(csi2->base + offset);
 113 }
 114
 115 static inline void csi2_reg_write(struct csi2_device *csi2, u32 offset, u32 val)
 116 {
 117         writel(val, csi2->base + offset);
 118         csi2_dbg_verbose("csi2: write 0x%04x -> 0x%03x\n", val, offset);
 119 }
 120
 121 static inline void set_field(u32 *valp, u32 field, u32 mask)
 122 {
 123         u32 val = *valp;
 124
 125         val &= ~mask;
 126         val |= (field << __ffs(mask)) & mask;
 127         *valp = val;
 128 }
 129
 130 static int csi2_regs_show(struct seq_file *s, void *data)
 131 {
 132         struct csi2_device *csi2 = s->private;
 133         unsigned int i;
 134         int ret;
 135
 136         ret = pm_runtime_resume_and_get(csi2->v4l2_dev->dev);
 137         if (ret)
 138                 return ret;
 139
 140 #define DUMP(reg) seq_printf(s, #reg " \t0x%08x\n", csi2_reg_read(csi2, reg))
 141 #define DUMP_CH(idx, reg) seq_printf(s, #reg "(%u) \t0x%08x\n", idx, csi2_reg_read(csi2, reg(idx)))
 142
 143         DUMP(CSI2_STATUS);
 144         DUMP(CSI2_DISCARDS_OVERFLOW);
 145         DUMP(CSI2_DISCARDS_INACTIVE);
 146         DUMP(CSI2_DISCARDS_UNMATCHED);
 147         DUMP(CSI2_DISCARDS_LEN_LIMIT);
 148         DUMP(CSI2_LLEV_PANICS);
 149         DUMP(CSI2_ULEV_PANICS);
 150         DUMP(CSI2_IRQ_MASK);
 151         DUMP(CSI2_CTRL);
 152
 153         for (i = 0; i < CSI2_NUM_CHANNELS; ++i) {
 154                 DUMP_CH(i, CSI2_CH_CTRL);
 155                 DUMP_CH(i, CSI2_CH_ADDR0);
 156                 DUMP_CH(i, CSI2_CH_ADDR1);
 157                 DUMP_CH(i, CSI2_CH_STRIDE);
 158                 DUMP_CH(i, CSI2_CH_LENGTH);
 159                 DUMP_CH(i, CSI2_CH_DEBUG);
 160                 DUMP_CH(i, CSI2_CH_FRAME_SIZE);
 161                 DUMP_CH(i, CSI2_CH_COMP_CTRL);
 162                 DUMP_CH(i, CSI2_CH_FE_FRAME_ID);
 163         }
 164
 165 #undef DUMP
 166 #undef DUMP_CH
 167
 168         pm_runtime_put(csi2->v4l2_dev->dev);
 169
 170         return 0;
 171 }
 172
 173 DEFINE_SHOW_ATTRIBUTE(csi2_regs);
 174
 175 static int csi2_errors_show(struct seq_file *s, void *data)
 176 {
 177         struct csi2_device *csi2 = s->private;
 178         unsigned long flags;
 179         u32 discards_table[DISCARDS_TABLE_NUM_VCS][DISCARDS_TABLE_NUM_ENTRIES];
 180         u32 discards_dt_table[DISCARDS_TABLE_NUM_ENTRIES];
 181         u32 overflows;
 182
 183         spin_lock_irqsave(&csi2->errors_lock, flags);
 184
 185         memcpy(discards_table, csi2->discards_table, sizeof(discards_table));
 186         memcpy(discards_dt_table, csi2->discards_dt_table,
 187                sizeof(discards_dt_table));
 188         overflows = csi2->overflows;
 189
 190         csi2->overflows = 0;
 191         memset(csi2->discards_table, 0, sizeof(discards_table));
 192         memset(csi2->discards_dt_table, 0, sizeof(discards_dt_table));
 193
 194         spin_unlock_irqrestore(&csi2->errors_lock, flags);
 195
 196         seq_printf(s, "Overflows %u\n", overflows);
 197         seq_puts(s, "Discards:\n");
 198         seq_puts(s, "VC            OVLF        LEN  UNMATCHED   INACTIVE\n");
 199
 200         for (unsigned int vc = 0; vc < DISCARDS_TABLE_NUM_VCS; ++vc) {
 201                 seq_printf(s, "%u       %10u %10u %10u %10u\n", vc,
 202                            discards_table[vc][DISCARDS_TABLE_OVERFLOW],
 203                            discards_table[vc][DISCARDS_TABLE_LENGTH_LIMIT],
 204                            discards_table[vc][DISCARDS_TABLE_UNMATCHED],
 205                            discards_table[vc][DISCARDS_TABLE_INACTIVE]);
 206         }
 207
 208         seq_printf(s, "Last DT %10u %10u %10u %10u\n",
 209                    discards_dt_table[DISCARDS_TABLE_OVERFLOW],
 210                    discards_dt_table[DISCARDS_TABLE_LENGTH_LIMIT],
 211                    discards_dt_table[DISCARDS_TABLE_UNMATCHED],
 212                    discards_dt_table[DISCARDS_TABLE_INACTIVE]);
 213
 214         return 0;
 215 }
 216
 217 DEFINE_SHOW_ATTRIBUTE(csi2_errors);
 218
 219 static void csi2_isr_handle_errors(struct csi2_device *csi2, u32 status)
 220 {
 221         spin_lock(&csi2->errors_lock);
 222
 223         if (status & IRQ_OVERFLOW)
 224                 csi2->overflows++;
 225
 226         for (unsigned int i = 0; i < DISCARDS_TABLE_NUM_ENTRIES; ++i) {
 227                 static const u32 discard_bits[] = {
 228                         IRQ_DISCARD_OVERFLOW,
 229                         IRQ_DISCARD_LEN_LIMIT,
 230                         IRQ_DISCARD_UNMATCHED,
 231                         IRQ_DISCARD_INACTIVE,
 232                 };
 233                 static const u8 discard_regs[] = {
 234                         CSI2_DISCARDS_OVERFLOW,
 235                         CSI2_DISCARDS_LEN_LIMIT,
 236                         CSI2_DISCARDS_UNMATCHED,
 237                         CSI2_DISCARDS_INACTIVE,
 238                 };
 239                 u32 amount;
 240                 u8 dt, vc;
 241                 u32 v;
 242
 243                 if (!(status & discard_bits[i]))
 244                         continue;
 245
 246                 v = csi2_reg_read(csi2, discard_regs[i]);
 247                 csi2_reg_write(csi2, discard_regs[i], 0);
 248
 249                 amount = (v & CSI2_DISCARDS_AMOUNT_MASK) >>
 250                          CSI2_DISCARDS_AMOUNT_SHIFT;
 251                 dt = (v & CSI2_DISCARDS_DT_MASK) >> CSI2_DISCARDS_DT_SHIFT;
 252                 vc = (v & CSI2_DISCARDS_VC_MASK) >> CSI2_DISCARDS_VC_SHIFT;
 253
 254                 csi2->discards_table[vc][i] += amount;
 255                 csi2->discards_dt_table[i] = dt;
 256         }
 257
 258         spin_unlock(&csi2->errors_lock);
 259 }
 260
 261 void csi2_isr(struct csi2_device *csi2, bool *sof, bool *eof, bool *lci)
 262 {
 263         unsigned int i;
 264         u32 status;
 265
 266         status = csi2_reg_read(csi2, CSI2_STATUS);
 267         csi2_dbg_verbose("ISR: STA: 0x%x\n", status);
 268
 269         /* Write value back to clear the interrupts */
 270         csi2_reg_write(csi2, CSI2_STATUS, status);
 271
 272         for (i = 0; i < CSI2_NUM_CHANNELS; i++) {
 273                 u32 dbg;
 274
 275                 if ((status & IRQ_CH_MASK(i)) == 0)
 276                         continue;
 277
 278                 dbg = csi2_reg_read(csi2, CSI2_CH_DEBUG(i));
 279
 280                 csi2_dbg_verbose("ISR: [%u], %s%s%s%s%s frame: %u line: %u\n",
 281                                  i, (status & IRQ_FS(i)) ? "FS " : "",
 282                                  (status & IRQ_FE(i)) ? "FE " : "",
 283                                  (status & IRQ_FE_ACK(i)) ? "FE_ACK " : "",
 284                                  (status & IRQ_LE(i)) ? "LE " : "",
 285                                  (status & IRQ_LE_ACK(i)) ? "LE_ACK " : "",
 286                                  dbg >> 16,
 287                                  csi2->num_lines[i] ?
 288                                          ((dbg & 0xffff) % csi2->num_lines[i]) :
 289                                          0);
 290
 291                 sof[i] = !!(status & IRQ_FS(i));
 292                 eof[i] = !!(status & IRQ_FE_ACK(i));
 293                 lci[i] = !!(status & IRQ_LE_ACK(i));
 294         }
 295
 296         if (csi2_track_errors)
 297                 csi2_isr_handle_errors(csi2, status);
 298 }
 299
 300 void csi2_set_buffer(struct csi2_device *csi2, unsigned int channel,
 301                      dma_addr_t dmaaddr, unsigned int stride, unsigned int size)
 302 {
 303         u64 addr = dmaaddr;
 304         /*
 305          * ADDRESS0 must be written last as it triggers the double buffering
 306          * mechanism for all buffer registers within the hardware.
 307          */
 308         addr >>= 4;
 309         csi2_reg_write(csi2, CSI2_CH_LENGTH(channel), size >> 4);
 310         csi2_reg_write(csi2, CSI2_CH_STRIDE(channel), stride >> 4);
 311         csi2_reg_write(csi2, CSI2_CH_ADDR1(channel), addr >> 32);
 312         csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), addr & 0xffffffff);
 313 }
 314
 315 void csi2_set_compression(struct csi2_device *csi2, unsigned int channel,
 316                           enum csi2_compression_mode mode, unsigned int shift,
 317                           unsigned int offset)
 318 {
 319         u32 compression = 0;
 320
 321         set_field(&compression, COMP_OFFSET_MASK, offset);
 322         set_field(&compression, COMP_SHIFT_MASK, shift);
 323         set_field(&compression, COMP_MODE_MASK, mode);
 324         csi2_reg_write(csi2, CSI2_CH_COMP_CTRL(channel), compression);
 325 }
 326
 327 void csi2_start_channel(struct csi2_device *csi2, unsigned int channel,
 328                         u16 dt, enum csi2_mode mode, bool auto_arm,
 329                         bool pack_bytes, unsigned int width,
 330                         unsigned int height)
 331 {
 332         u32 ctrl;
 333
 334         csi2_dbg("%s [%u]\n", __func__, channel);
 335
 336         /*
 337          * Disable the channel, but ensure N != 0!  Otherwise we end up with a
 338          * spurious LE + LE_ACK interrupt when re-enabling the channel.
 339          */
 340         csi2_reg_write(csi2, CSI2_CH_CTRL(channel), 0x100 << __ffs(LC_MASK));
 341         csi2_reg_write(csi2, CSI2_CH_DEBUG(channel), 0);
 342         csi2_reg_write(csi2, CSI2_STATUS, IRQ_CH_MASK(channel));
 343
 344         /* Enable channel and FS/FE/LE interrupts. */
 345         ctrl = DMA_EN | IRQ_EN_FS | IRQ_EN_FE_ACK | IRQ_EN_LE_ACK | PACK_LINE;
 346         /* PACK_BYTES ensures no striding for embedded data. */
 347         if (pack_bytes)
 348                 ctrl |= PACK_BYTES;
 349
 350         if (auto_arm)
 351                 ctrl |= AUTO_ARM;
 352
 353         if (width && height) {
 354                 int line_int_freq = height >> 2;
 355
 356                 line_int_freq = min(max(0x80, line_int_freq), 0x3ff);
 357                 set_field(&ctrl, line_int_freq, LC_MASK);
 358                 set_field(&ctrl, mode, CH_MODE_MASK);
 359                 csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel),
 360                                (height << 16) | width);
 361         } else {
 362                 /*
 363                  * Do not disable line interrupts for the embedded data channel,
 364                  * set it to the maximum value.  This avoids spamming the ISR
 365                  * with spurious line interrupts.
 366                  */
 367                 set_field(&ctrl, 0x3ff, LC_MASK);
 368                 set_field(&ctrl, 0x00, CH_MODE_MASK);
 369                 csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel), 0);
 370         }
 371
 372         set_field(&ctrl, dt, DT_MASK);
 373         csi2_reg_write(csi2, CSI2_CH_CTRL(channel), ctrl);
 374         csi2->num_lines[channel] = height;
 375 }
 376
 377 void csi2_stop_channel(struct csi2_device *csi2, unsigned int channel)
 378 {
 379         csi2_dbg("%s [%u]\n", __func__, channel);
 380
 381         /* Channel disable.  Use FORCE to allow stopping mid-frame. */
 382         csi2_reg_write(csi2, CSI2_CH_CTRL(channel),
 383                        (0x100 << __ffs(LC_MASK)) | FORCE);
 384         /* Latch the above change by writing to the ADDR0 register. */
 385         csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), 0);
 386         /* Write this again, the HW needs it! */
 387         csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), 0);
 388 }
 389
 390 void csi2_open_rx(struct csi2_device *csi2)
 391 {
 392         csi2_reg_write(csi2, CSI2_IRQ_MASK,
 393                        csi2_track_errors ? CSI2_IRQ_MASK_IRQ_ALL : 0);
 394
 395         dphy_start(&csi2->dphy);
 396
 397         csi2_reg_write(csi2, CSI2_CTRL,
 398                        csi2->multipacket_line ? 0 : EOP_IS_EOL);
 399 }
 400
 401 void csi2_close_rx(struct csi2_device *csi2)
 402 {
 403         dphy_stop(&csi2->dphy);
 404
 405         csi2_reg_write(csi2, CSI2_IRQ_MASK, 0);
 406 }
 407
 408 static struct csi2_device *to_csi2_device(struct v4l2_subdev *subdev)
 409 {
 410         return container_of(subdev, struct csi2_device, sd);
 411 }
 412
 413 static int csi2_init_cfg(struct v4l2_subdev *sd,
 414                          struct v4l2_subdev_state *state)
 415 {
 416         struct v4l2_mbus_framefmt *fmt;
 417
 418         for (unsigned int i = 0; i < CSI2_NUM_CHANNELS; ++i) {
 419                 const struct v4l2_mbus_framefmt *def_fmt;
 420
 421                 /* CSI2_CH1_EMBEDDED */
 422                 if (i == 1)
 423                         def_fmt = &cfe_default_meta_format;
 424                 else
 425                         def_fmt = &cfe_default_format;
 426
 427                 fmt = v4l2_subdev_get_pad_format(sd, state, i);
 428                 *fmt = *def_fmt;
 429
 430                 fmt = v4l2_subdev_get_pad_format(sd, state, i + CSI2_NUM_CHANNELS);
 431                 *fmt = *def_fmt;
 432         }
 433
 434         return 0;
 435 }
 436
 437 static int csi2_pad_set_fmt(struct v4l2_subdev *sd,
 438                             struct v4l2_subdev_state *state,
 439                             struct v4l2_subdev_format *format)
 440 {
 441         if (format->pad < CSI2_NUM_CHANNELS) {
 442                 /*
 443                  * Store the sink pad format and propagate it to the source pad.
 444                  */
 445
 446                 struct v4l2_mbus_framefmt *fmt;
 447
 448                 fmt = v4l2_subdev_get_pad_format(sd, state, format->pad);
 449                 if (!fmt)
 450                         return -EINVAL;
 451
 452                 *fmt = format->format;
 453
 454                 fmt = v4l2_subdev_get_pad_format(sd, state,
 455                         format->pad + CSI2_NUM_CHANNELS);
 456                 if (!fmt)
 457                         return -EINVAL;
 458
 459                 format->format.field = V4L2_FIELD_NONE;
 460
 461                 *fmt = format->format;
 462         } else {
 463                 /*
 464                  * Only allow changing the source pad mbus code.
 465                  */
 466
 467                 struct v4l2_mbus_framefmt *sink_fmt, *source_fmt;
 468                 u32 sink_code;
 469                 u32 code;
 470
 471                 sink_fmt = v4l2_subdev_get_pad_format(sd, state,
 472                         format->pad - CSI2_NUM_CHANNELS);
 473                 if (!sink_fmt)
 474                         return -EINVAL;
 475
 476                 source_fmt = v4l2_subdev_get_pad_format(sd, state, format->pad);
 477                 if (!source_fmt)
 478                         return -EINVAL;
 479
 480                 sink_code = sink_fmt->code;
 481                 code = format->format.code;
 482
 483                 /*
 484                  * If the source code from the user does not match the code in
 485                  * the sink pad, check that the source code matches either the
 486                  * 16-bit version or the compressed version of the sink code.
 487                  */
 488
 489                 if (code != sink_code &&
 490                     (code == cfe_find_16bit_code(sink_code) ||
 491                      code == cfe_find_compressed_code(sink_code)))
 492                         source_fmt->code = code;
 493
 494                 format->format.code = source_fmt->code;
 495         }
 496
 497         return 0;
 498 }
 499
 500 static int csi2_link_validate(struct v4l2_subdev *sd, struct media_link *link,
 501                               struct v4l2_subdev_format *source_fmt,
 502                               struct v4l2_subdev_format *sink_fmt)
 503 {
 504         struct csi2_device *csi2 = to_csi2_device(sd);
 505
 506         csi2_dbg("%s: link \"%s\":%u -> \"%s\":%u\n", __func__,
 507                  link->source->entity->name, link->source->index,
 508                  link->sink->entity->name, link->sink->index);
 509
 510         if ((link->source->entity == &csi2->sd.entity &&
 511              link->source->index == 1) ||
 512             (link->sink->entity == &csi2->sd.entity &&
 513              link->sink->index == 1)) {
 514                 csi2_dbg("Ignore metadata pad for now\n");
 515                 return 0;
 516         }
 517
 518         /* The width, height and code must match. */
 519         if (source_fmt->format.width != sink_fmt->format.width ||
 520             source_fmt->format.width != sink_fmt->format.width ||
 521             source_fmt->format.code != sink_fmt->format.code) {
 522                 csi2_err("%s: format does not match (source %ux%u 0x%x, sink %ux%u 0x%x)\n",
 523                          __func__,
 524                          source_fmt->format.width, source_fmt->format.height,
 525                          source_fmt->format.code,
 526                          sink_fmt->format.width, sink_fmt->format.height,
 527                          sink_fmt->format.code);
 528                 return -EPIPE;
 529         }
 530
 531         return 0;
 532 }
 533
 534 static const struct v4l2_subdev_pad_ops csi2_subdev_pad_ops = {
 535         .init_cfg = csi2_init_cfg,
 536         .get_fmt = v4l2_subdev_get_fmt,
 537         .set_fmt = csi2_pad_set_fmt,
 538         .link_validate = csi2_link_validate,
 539 };
 540
 541 static const struct media_entity_operations csi2_entity_ops = {
 542         .link_validate = v4l2_subdev_link_validate,
 543 };
 544
 545 static const struct v4l2_subdev_ops csi2_subdev_ops = {
 546         .pad = &csi2_subdev_pad_ops,
 547 };
 548
 549 int csi2_init(struct csi2_device *csi2, struct dentry *debugfs)
 550 {
 551         unsigned int i, ret;
 552
 553         spin_lock_init(&csi2->errors_lock);
 554
 555         csi2->dphy.dev = csi2->v4l2_dev->dev;
 556         dphy_probe(&csi2->dphy);
 557
 558         debugfs_create_file("csi2_regs", 0444, debugfs, csi2, &csi2_regs_fops);
 559
 560         if (csi2_track_errors)
 561                 debugfs_create_file("csi2_errors", 0444, debugfs, csi2,
 562                                     &csi2_errors_fops);
 563
 564         for (i = 0; i < CSI2_NUM_CHANNELS * 2; i++)
 565                 csi2->pad[i].flags = i < CSI2_NUM_CHANNELS ?
 566                                      MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
 567
 568         ret = media_entity_pads_init(&csi2->sd.entity, ARRAY_SIZE(csi2->pad),
 569                                      csi2->pad);
 570         if (ret)
 571                 return ret;
 572
 573         /* Initialize subdev */
 574         v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
 575         csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
 576         csi2->sd.entity.ops = &csi2_entity_ops;
 577         csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
 578         csi2->sd.owner = THIS_MODULE;
 579         snprintf(csi2->sd.name, sizeof(csi2->sd.name), "csi2");
 580
 581         ret = v4l2_subdev_init_finalize(&csi2->sd);
 582         if (ret)
 583                 goto err_entity_cleanup;
 584
 585         ret = v4l2_device_register_subdev(csi2->v4l2_dev, &csi2->sd);
 586         if (ret) {
 587                 csi2_err("Failed register csi2 subdev (%d)\n", ret);
 588                 goto err_subdev_cleanup;
 589         }
 590
 591         return 0;
 592
 593 err_subdev_cleanup:
 594         v4l2_subdev_cleanup(&csi2->sd);
 595 err_entity_cleanup:
 596         media_entity_cleanup(&csi2->sd.entity);
 597
 598         return ret;
 599 }
 600
 601 void csi2_uninit(struct csi2_device *csi2)
 602 {
 603         v4l2_device_unregister_subdev(&csi2->sd);
 604         v4l2_subdev_cleanup(&csi2->sd);
 605         media_entity_cleanup(&csi2->sd.entity);
 606 }