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)
 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         }
 294
 295         if (csi2_track_errors)
 296                 csi2_isr_handle_errors(csi2, status);
 297 }
 298
 299 void csi2_set_buffer(struct csi2_device *csi2, unsigned int channel,
 300                      dma_addr_t dmaaddr, unsigned int stride, unsigned int size)
 301 {
 302         u64 addr = dmaaddr;
 303         /*
 304          * ADDRESS0 must be written last as it triggers the double buffering
 305          * mechanism for all buffer registers within the hardware.
 306          */
 307         addr >>= 4;
 308         csi2_reg_write(csi2, CSI2_CH_LENGTH(channel), size >> 4);
 309         csi2_reg_write(csi2, CSI2_CH_STRIDE(channel), stride >> 4);
 310         csi2_reg_write(csi2, CSI2_CH_ADDR1(channel), addr >> 32);
 311         csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), addr & 0xffffffff);
 312 }
 313
 314 void csi2_set_compression(struct csi2_device *csi2, unsigned int channel,
 315                           enum csi2_compression_mode mode, unsigned int shift,
 316                           unsigned int offset)
 317 {
 318         u32 compression = 0;
 319
 320         set_field(&compression, COMP_OFFSET_MASK, offset);
 321         set_field(&compression, COMP_SHIFT_MASK, shift);
 322         set_field(&compression, COMP_MODE_MASK, mode);
 323         csi2_reg_write(csi2, CSI2_CH_COMP_CTRL(channel), compression);
 324 }
 325
 326 static int csi2_get_vc_dt_fallback(struct csi2_device *csi2,
 327                                    unsigned int channel, u8 *vc, u8 *dt)
 328 {
 329         struct v4l2_subdev *sd = &csi2->sd;
 330         struct v4l2_subdev_state *state;
 331         struct v4l2_mbus_framefmt *fmt;
 332         const struct cfe_fmt *cfe_fmt;
 333
 334         state = v4l2_subdev_get_locked_active_state(sd);
 335
 336         /* Without Streams API, the channel number matches the sink pad */
 337         fmt = v4l2_subdev_get_pad_format(sd, state, channel);
 338         if (!fmt)
 339                 return -EINVAL;
 340
 341         cfe_fmt = find_format_by_code(fmt->code);
 342         if (!cfe_fmt)
 343                 return -EINVAL;
 344
 345         *vc = 0;
 346         *dt = cfe_fmt->csi_dt;
 347
 348         return 0;
 349 }
 350
 351 static int csi2_get_vc_dt(struct csi2_device *csi2, unsigned int channel,
 352                           u8 *vc, u8 *dt)
 353 {
 354         struct v4l2_mbus_frame_desc remote_desc;
 355         const struct media_pad *remote_pad;
 356         struct v4l2_subdev *source_sd;
 357         int ret;
 358
 359         /* Without Streams API, the channel number matches the sink pad */
 360         remote_pad = media_pad_remote_pad_first(&csi2->pad[channel]);
 361         if (!remote_pad)
 362                 return -EPIPE;
 363
 364         source_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
 365
 366         ret = v4l2_subdev_call(source_sd, pad, get_frame_desc,
 367                                remote_pad->index, &remote_desc);
 368         if (ret == -ENOIOCTLCMD) {
 369                 csi2_dbg("source does not support get_frame_desc, use fallback\n");
 370                 return csi2_get_vc_dt_fallback(csi2, channel, vc, dt);
 371         } else if (ret) {
 372                 csi2_err("Failed to get frame descriptor\n");
 373                 return ret;
 374         }
 375
 376         if (remote_desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
 377                 csi2_err("Frame descriptor does not describe CSI-2 link");
 378                 return -EINVAL;
 379         }
 380
 381         if (remote_desc.num_entries != 1) {
 382                 csi2_err("Frame descriptor does not have a single entry");
 383                 return -EINVAL;
 384         }
 385
 386         *vc = remote_desc.entry[0].bus.csi2.vc;
 387         *dt = remote_desc.entry[0].bus.csi2.dt;
 388
 389         return 0;
 390 }
 391
 392 void csi2_start_channel(struct csi2_device *csi2, unsigned int channel,
 393                         enum csi2_mode mode, bool auto_arm,
 394                         bool pack_bytes, unsigned int width,
 395                         unsigned int height)
 396 {
 397         u32 ctrl;
 398         int ret;
 399         u8 vc, dt;
 400
 401         ret = csi2_get_vc_dt(csi2, channel, &vc, &dt);
 402         if (ret)
 403                 return;
 404
 405         csi2_dbg("%s [%u]\n", __func__, channel);
 406
 407         csi2_reg_write(csi2, CSI2_CH_CTRL(channel), 0);
 408         csi2_reg_write(csi2, CSI2_CH_DEBUG(channel), 0);
 409         csi2_reg_write(csi2, CSI2_STATUS, IRQ_CH_MASK(channel));
 410
 411         /* Enable channel and FS/FE interrupts. */
 412         ctrl = DMA_EN | IRQ_EN_FS | IRQ_EN_FE_ACK | PACK_LINE;
 413         /* PACK_BYTES ensures no striding for embedded data. */
 414         if (pack_bytes)
 415                 ctrl |= PACK_BYTES;
 416
 417         if (auto_arm)
 418                 ctrl |= AUTO_ARM;
 419
 420         if (width && height) {
 421                 set_field(&ctrl, mode, CH_MODE_MASK);
 422                 csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel),
 423                                (height << 16) | width);
 424         } else {
 425                 set_field(&ctrl, 0x0, CH_MODE_MASK);
 426                 csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel), 0);
 427         }
 428
 429         set_field(&ctrl, vc, VC_MASK);
 430         set_field(&ctrl, dt, DT_MASK);
 431         csi2_reg_write(csi2, CSI2_CH_CTRL(channel), ctrl);
 432         csi2->num_lines[channel] = height;
 433 }
 434
 435 void csi2_stop_channel(struct csi2_device *csi2, unsigned int channel)
 436 {
 437         csi2_dbg("%s [%u]\n", __func__, channel);
 438
 439         /* Channel disable.  Use FORCE to allow stopping mid-frame. */
 440         csi2_reg_write(csi2, CSI2_CH_CTRL(channel), FORCE);
 441         /* Latch the above change by writing to the ADDR0 register. */
 442         csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), 0);
 443         /* Write this again, the HW needs it! */
 444         csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), 0);
 445 }
 446
 447 void csi2_open_rx(struct csi2_device *csi2)
 448 {
 449         csi2_reg_write(csi2, CSI2_IRQ_MASK,
 450                        csi2_track_errors ? CSI2_IRQ_MASK_IRQ_ALL : 0);
 451
 452         dphy_start(&csi2->dphy);
 453
 454         csi2_reg_write(csi2, CSI2_CTRL,
 455                        csi2->multipacket_line ? 0 : EOP_IS_EOL);
 456 }
 457
 458 void csi2_close_rx(struct csi2_device *csi2)
 459 {
 460         dphy_stop(&csi2->dphy);
 461
 462         csi2_reg_write(csi2, CSI2_IRQ_MASK, 0);
 463 }
 464
 465 static struct csi2_device *to_csi2_device(struct v4l2_subdev *subdev)
 466 {
 467         return container_of(subdev, struct csi2_device, sd);
 468 }
 469
 470 static int csi2_init_cfg(struct v4l2_subdev *sd,
 471                          struct v4l2_subdev_state *state)
 472 {
 473         struct v4l2_mbus_framefmt *fmt;
 474
 475         for (unsigned int i = 0; i < CSI2_NUM_CHANNELS; ++i) {
 476                 const struct v4l2_mbus_framefmt *def_fmt;
 477
 478                 /* CSI2_CH1_EMBEDDED */
 479                 if (i == 1)
 480                         def_fmt = &cfe_default_meta_format;
 481                 else
 482                         def_fmt = &cfe_default_format;
 483
 484                 fmt = v4l2_subdev_get_pad_format(sd, state, i);
 485                 *fmt = *def_fmt;
 486
 487                 fmt = v4l2_subdev_get_pad_format(sd, state, i + CSI2_NUM_CHANNELS);
 488                 *fmt = *def_fmt;
 489         }
 490
 491         return 0;
 492 }
 493
 494 static int csi2_pad_set_fmt(struct v4l2_subdev *sd,
 495                             struct v4l2_subdev_state *state,
 496                             struct v4l2_subdev_format *format)
 497 {
 498         if (format->pad < CSI2_NUM_CHANNELS) {
 499                 /*
 500                  * Store the sink pad format and propagate it to the source pad.
 501                  */
 502
 503                 struct v4l2_mbus_framefmt *fmt;
 504
 505                 fmt = v4l2_subdev_get_pad_format(sd, state, format->pad);
 506                 if (!fmt)
 507                         return -EINVAL;
 508
 509                 *fmt = format->format;
 510
 511                 fmt = v4l2_subdev_get_pad_format(sd, state,
 512                         format->pad + CSI2_NUM_CHANNELS);
 513                 if (!fmt)
 514                         return -EINVAL;
 515
 516                 format->format.field = V4L2_FIELD_NONE;
 517
 518                 *fmt = format->format;
 519         } else {
 520                 /*
 521                  * Only allow changing the source pad mbus code.
 522                  */
 523
 524                 struct v4l2_mbus_framefmt *sink_fmt, *source_fmt;
 525                 u32 sink_code;
 526                 u32 code;
 527
 528                 sink_fmt = v4l2_subdev_get_pad_format(sd, state,
 529                         format->pad - CSI2_NUM_CHANNELS);
 530                 if (!sink_fmt)
 531                         return -EINVAL;
 532
 533                 source_fmt = v4l2_subdev_get_pad_format(sd, state, format->pad);
 534                 if (!source_fmt)
 535                         return -EINVAL;
 536
 537                 sink_code = sink_fmt->code;
 538                 code = format->format.code;
 539
 540                 /*
 541                  * If the source code from the user does not match the code in
 542                  * the sink pad, check that the source code matches either the
 543                  * 16-bit version or the compressed version of the sink code.
 544                  */
 545
 546                 if (code != sink_code &&
 547                     (code == cfe_find_16bit_code(sink_code) ||
 548                      code == cfe_find_compressed_code(sink_code)))
 549                         source_fmt->code = code;
 550
 551                 format->format.code = source_fmt->code;
 552         }
 553
 554         return 0;
 555 }
 556
 557 static int csi2_link_validate(struct v4l2_subdev *sd, struct media_link *link,
 558                               struct v4l2_subdev_format *source_fmt,
 559                               struct v4l2_subdev_format *sink_fmt)
 560 {
 561         struct csi2_device *csi2 = to_csi2_device(sd);
 562
 563         csi2_dbg("%s: link \"%s\":%u -> \"%s\":%u\n", __func__,
 564                  link->source->entity->name, link->source->index,
 565                  link->sink->entity->name, link->sink->index);
 566
 567         if ((link->source->entity == &csi2->sd.entity &&
 568              link->source->index == 1) ||
 569             (link->sink->entity == &csi2->sd.entity &&
 570              link->sink->index == 1)) {
 571                 csi2_dbg("Ignore metadata pad for now\n");
 572                 return 0;
 573         }
 574
 575         /* The width, height and code must match. */
 576         if (source_fmt->format.width != sink_fmt->format.width ||
 577             source_fmt->format.width != sink_fmt->format.width ||
 578             source_fmt->format.code != sink_fmt->format.code) {
 579                 csi2_err("%s: format does not match (source %ux%u 0x%x, sink %ux%u 0x%x)\n",
 580                          __func__,
 581                          source_fmt->format.width, source_fmt->format.height,
 582                          source_fmt->format.code,
 583                          sink_fmt->format.width, sink_fmt->format.height,
 584                          sink_fmt->format.code);
 585                 return -EPIPE;
 586         }
 587
 588         return 0;
 589 }
 590
 591 static const struct v4l2_subdev_pad_ops csi2_subdev_pad_ops = {
 592         .init_cfg = csi2_init_cfg,
 593         .get_fmt = v4l2_subdev_get_fmt,
 594         .set_fmt = csi2_pad_set_fmt,
 595         .link_validate = csi2_link_validate,
 596 };
 597
 598 static const struct media_entity_operations csi2_entity_ops = {
 599         .link_validate = v4l2_subdev_link_validate,
 600 };
 601
 602 static const struct v4l2_subdev_ops csi2_subdev_ops = {
 603         .pad = &csi2_subdev_pad_ops,
 604 };
 605
 606 int csi2_init(struct csi2_device *csi2, struct dentry *debugfs)
 607 {
 608         unsigned int i, ret;
 609
 610         spin_lock_init(&csi2->errors_lock);
 611
 612         csi2->dphy.dev = csi2->v4l2_dev->dev;
 613         dphy_probe(&csi2->dphy);
 614
 615         debugfs_create_file("csi2_regs", 0444, debugfs, csi2, &csi2_regs_fops);
 616
 617         if (csi2_track_errors)
 618                 debugfs_create_file("csi2_errors", 0444, debugfs, csi2,
 619                                     &csi2_errors_fops);
 620
 621         for (i = 0; i < CSI2_NUM_CHANNELS * 2; i++)
 622                 csi2->pad[i].flags = i < CSI2_NUM_CHANNELS ?
 623                                      MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
 624
 625         ret = media_entity_pads_init(&csi2->sd.entity, ARRAY_SIZE(csi2->pad),
 626                                      csi2->pad);
 627         if (ret)
 628                 return ret;
 629
 630         /* Initialize subdev */
 631         v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
 632         csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
 633         csi2->sd.entity.ops = &csi2_entity_ops;
 634         csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
 635         csi2->sd.owner = THIS_MODULE;
 636         snprintf(csi2->sd.name, sizeof(csi2->sd.name), "csi2");
 637
 638         ret = v4l2_subdev_init_finalize(&csi2->sd);
 639         if (ret)
 640                 goto err_entity_cleanup;
 641
 642         ret = v4l2_device_register_subdev(csi2->v4l2_dev, &csi2->sd);
 643         if (ret) {
 644                 csi2_err("Failed register csi2 subdev (%d)\n", ret);
 645                 goto err_subdev_cleanup;
 646         }
 647
 648         return 0;
 649
 650 err_subdev_cleanup:
 651         v4l2_subdev_cleanup(&csi2->sd);
 652 err_entity_cleanup:
 653         media_entity_cleanup(&csi2->sd.entity);
 654
 655         return ret;
 656 }
 657
 658 void csi2_uninit(struct csi2_device *csi2)
 659 {
 660         v4l2_device_unregister_subdev(&csi2->sd);
 661         v4l2_subdev_cleanup(&csi2->sd);
 662         media_entity_cleanup(&csi2->sd.entity);
 663 }