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