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

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * PiSP Front End driver.
   4  * Copyright (c) 2021 Raspberry Pi Ltd.
   5  *
   6  */
   7
   8 #include <linux/bitops.h>
   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 "pisp_fe.h"
  17 #include "cfe.h"
  18
  19 #define FE_VERSION              0x000
  20 #define FE_CONTROL              0x004
  21 #define FE_STATUS               0x008
  22 #define FE_FRAME_STATUS         0x00c
  23 #define FE_ERROR_STATUS         0x010
  24 #define FE_OUTPUT_STATUS        0x014
  25 #define FE_INT_EN               0x018
  26 #define FE_INT_STATUS           0x01c
  27
  28 /* CONTROL */
  29 #define FE_CONTROL_QUEUE        BIT(0)
  30 #define FE_CONTROL_ABORT        BIT(1)
  31 #define FE_CONTROL_RESET        BIT(2)
  32 #define FE_CONTROL_LATCH_REGS   BIT(3)
  33
  34 /* INT_EN / INT_STATUS */
  35 #define FE_INT_EOF              BIT(0)
  36 #define FE_INT_SOF              BIT(1)
  37 #define FE_INT_LINES0           BIT(8)
  38 #define FE_INT_LINES1           BIT(9)
  39 #define FE_INT_STATS            BIT(16)
  40 #define FE_INT_QREADY           BIT(24)
  41
  42 /* STATUS */
  43 #define FE_STATUS_QUEUED        BIT(0)
  44 #define FE_STATUS_WAITING       BIT(1)
  45 #define FE_STATUS_ACTIVE        BIT(2)
  46
  47 #define PISP_FE_CONFIG_BASE_OFFSET      0x0040
  48
  49 #define PISP_FE_ENABLE_STATS_CLUSTER \
  50         (PISP_FE_ENABLE_STATS_CROP | PISP_FE_ENABLE_DECIMATE    | \
  51          PISP_FE_ENABLE_BLC        | PISP_FE_ENABLE_CDAF_STATS  | \
  52          PISP_FE_ENABLE_AWB_STATS  | PISP_FE_ENABLE_RGBY        | \
  53          PISP_FE_ENABLE_LSC        | PISP_FE_ENABLE_AGC_STATS)
  54
  55 #define PISP_FE_ENABLE_OUTPUT_CLUSTER(i)                                \
  56         ((PISP_FE_ENABLE_CROP0     | PISP_FE_ENABLE_DOWNSCALE0 |        \
  57           PISP_FE_ENABLE_COMPRESS0 | PISP_FE_ENABLE_OUTPUT0) << (4 * (i)))
  58
  59 struct pisp_fe_config_param {
  60         u32 dirty_flags;
  61         u32 dirty_flags_extra;
  62         size_t offset;
  63         size_t size;
  64 };
  65
  66 static const struct pisp_fe_config_param pisp_fe_config_map[] = {
  67         /* *_dirty_flag_extra types */
  68         { 0, PISP_FE_DIRTY_GLOBAL,     offsetof(struct pisp_fe_config, global),
  69                                         sizeof(struct pisp_fe_global_config)         },
  70         { 0, PISP_FE_DIRTY_FLOATING,   offsetof(struct pisp_fe_config, floating_stats),
  71                                         sizeof(struct pisp_fe_floating_stats_config) },
  72         { 0, PISP_FE_DIRTY_OUTPUT_AXI, offsetof(struct pisp_fe_config, output_axi),
  73                                         sizeof(struct pisp_fe_output_axi_config)     },
  74         /* *_dirty_flag types */
  75         { PISP_FE_ENABLE_INPUT,      0, offsetof(struct pisp_fe_config, input),
  76                                         sizeof(struct pisp_fe_input_config)          },
  77         { PISP_FE_ENABLE_DECOMPRESS, 0, offsetof(struct pisp_fe_config, decompress),
  78                                         sizeof(struct pisp_decompress_config)        },
  79         { PISP_FE_ENABLE_DECOMPAND,  0, offsetof(struct pisp_fe_config, decompand),
  80                                         sizeof(struct pisp_fe_decompand_config)      },
  81         { PISP_FE_ENABLE_BLA,        0, offsetof(struct pisp_fe_config, bla),
  82                                         sizeof(struct pisp_bla_config)               },
  83         { PISP_FE_ENABLE_DPC,        0, offsetof(struct pisp_fe_config, dpc),
  84                                         sizeof(struct pisp_fe_dpc_config)            },
  85         { PISP_FE_ENABLE_STATS_CROP, 0, offsetof(struct pisp_fe_config, stats_crop),
  86                                         sizeof(struct pisp_fe_crop_config)           },
  87         { PISP_FE_ENABLE_BLC,        0, offsetof(struct pisp_fe_config, blc),
  88                                         sizeof(struct pisp_bla_config)               },
  89         { PISP_FE_ENABLE_CDAF_STATS, 0, offsetof(struct pisp_fe_config, cdaf_stats),
  90                                         sizeof(struct pisp_fe_cdaf_stats_config)     },
  91         { PISP_FE_ENABLE_AWB_STATS,  0, offsetof(struct pisp_fe_config, awb_stats),
  92                                         sizeof(struct pisp_fe_awb_stats_config)      },
  93         { PISP_FE_ENABLE_RGBY,       0, offsetof(struct pisp_fe_config, rgby),
  94                                         sizeof(struct pisp_fe_rgby_config)           },
  95         { PISP_FE_ENABLE_LSC,        0, offsetof(struct pisp_fe_config, lsc),
  96                                         sizeof(struct pisp_fe_lsc_config)            },
  97         { PISP_FE_ENABLE_AGC_STATS,  0, offsetof(struct pisp_fe_config, agc_stats),
  98                                         sizeof(struct pisp_agc_statistics)           },
  99         { PISP_FE_ENABLE_CROP0,      0, offsetof(struct pisp_fe_config, ch[0].crop),
 100                                         sizeof(struct pisp_fe_crop_config)           },
 101         { PISP_FE_ENABLE_DOWNSCALE0, 0, offsetof(struct pisp_fe_config, ch[0].downscale),
 102                                         sizeof(struct pisp_fe_downscale_config)      },
 103         { PISP_FE_ENABLE_COMPRESS0,  0, offsetof(struct pisp_fe_config, ch[0].compress),
 104                                         sizeof(struct pisp_compress_config)          },
 105         { PISP_FE_ENABLE_OUTPUT0,    0, offsetof(struct pisp_fe_config, ch[0].output),
 106                                         sizeof(struct pisp_fe_output_config)         },
 107         { PISP_FE_ENABLE_CROP1,      0, offsetof(struct pisp_fe_config, ch[1].crop),
 108                                         sizeof(struct pisp_fe_crop_config)           },
 109         { PISP_FE_ENABLE_DOWNSCALE1, 0, offsetof(struct pisp_fe_config, ch[1].downscale),
 110                                         sizeof(struct pisp_fe_downscale_config)      },
 111         { PISP_FE_ENABLE_COMPRESS1,  0, offsetof(struct pisp_fe_config, ch[1].compress),
 112                                         sizeof(struct pisp_compress_config)          },
 113         { PISP_FE_ENABLE_OUTPUT1,    0, offsetof(struct pisp_fe_config, ch[1].output),
 114                                         sizeof(struct pisp_fe_output_config)         },
 115 };
 116
 117 #define pisp_fe_dbg_verbose(fmt, arg...)                        \
 118         do {                                                    \
 119                 if (cfe_debug_verbose)                          \
 120                         dev_dbg(fe->v4l2_dev->dev, fmt, ##arg); \
 121         } while (0)
 122 #define pisp_fe_dbg(fmt, arg...) dev_dbg(fe->v4l2_dev->dev, fmt, ##arg)
 123 #define pisp_fe_info(fmt, arg...) dev_info(fe->v4l2_dev->dev, fmt, ##arg)
 124 #define pisp_fe_err(fmt, arg...) dev_err(fe->v4l2_dev->dev, fmt, ##arg)
 125
 126 static inline u32 pisp_fe_reg_read(struct pisp_fe_device *fe, u32 offset)
 127 {
 128         return readl(fe->base + offset);
 129 }
 130
 131 static inline void pisp_fe_reg_write(struct pisp_fe_device *fe, u32 offset,
 132                                      u32 val)
 133 {
 134         writel(val, fe->base + offset);
 135         pisp_fe_dbg_verbose("fe: write 0x%04x -> 0x%03x\n", val, offset);
 136 }
 137
 138 static inline void pisp_fe_reg_write_relaxed(struct pisp_fe_device *fe, u32 offset,
 139                                              u32 val)
 140 {
 141         writel_relaxed(val, fe->base + offset);
 142         pisp_fe_dbg_verbose("fe: write 0x%04x -> 0x%03x\n", val, offset);
 143 }
 144
 145 static int pisp_regs_show(struct seq_file *s, void *data)
 146 {
 147         struct pisp_fe_device *fe = s->private;
 148         int ret;
 149
 150         ret = pm_runtime_resume_and_get(fe->v4l2_dev->dev);
 151         if (ret)
 152                 return ret;
 153
 154         pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_LATCH_REGS);
 155
 156 #define DUMP(reg) seq_printf(s, #reg " \t0x%08x\n", pisp_fe_reg_read(fe, reg))
 157         DUMP(FE_VERSION);
 158         DUMP(FE_CONTROL);
 159         DUMP(FE_STATUS);
 160         DUMP(FE_FRAME_STATUS);
 161         DUMP(FE_ERROR_STATUS);
 162         DUMP(FE_OUTPUT_STATUS);
 163         DUMP(FE_INT_EN);
 164         DUMP(FE_INT_STATUS);
 165 #undef DUMP
 166
 167         pm_runtime_put(fe->v4l2_dev->dev);
 168
 169         return 0;
 170 }
 171
 172 DEFINE_SHOW_ATTRIBUTE(pisp_regs);
 173
 174 static void pisp_config_write(struct pisp_fe_device *fe,
 175                               struct pisp_fe_config *config,
 176                               unsigned int start_offset,
 177                               unsigned int size)
 178 {
 179         const unsigned int max_offset =
 180                 offsetof(struct pisp_fe_config, ch[PISP_FE_NUM_OUTPUTS]);
 181         unsigned int i, end_offset;
 182         u32 *cfg = (u32 *)config;
 183
 184         start_offset = min(start_offset, max_offset);
 185         end_offset = min(start_offset + size, max_offset);
 186
 187         cfg += start_offset >> 2;
 188         for (i = start_offset; i < end_offset; i += 4, cfg++)
 189                 pisp_fe_reg_write_relaxed(fe, PISP_FE_CONFIG_BASE_OFFSET + i,
 190                                           *cfg);
 191 }
 192
 193 void pisp_fe_isr(struct pisp_fe_device *fe, bool *sof, bool *eof)
 194 {
 195         u32 status, int_status, out_status, frame_status, error_status;
 196         unsigned int i;
 197
 198         pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_LATCH_REGS);
 199         status = pisp_fe_reg_read(fe, FE_STATUS);
 200         out_status = pisp_fe_reg_read(fe, FE_OUTPUT_STATUS);
 201         frame_status = pisp_fe_reg_read(fe, FE_FRAME_STATUS);
 202         error_status = pisp_fe_reg_read(fe, FE_ERROR_STATUS);
 203
 204         int_status = pisp_fe_reg_read(fe, FE_INT_STATUS);
 205         pisp_fe_reg_write(fe, FE_INT_STATUS, int_status);
 206
 207         pisp_fe_dbg_verbose("%s: status 0x%x out 0x%x frame 0x%x error 0x%x int 0x%x\n",
 208                 __func__, status, out_status, frame_status, error_status,
 209                 int_status);
 210
 211         /* We do not report interrupts for the input/stream pad. */
 212         for (i = 0; i < FE_NUM_PADS - 1; i++) {
 213                 sof[i] = !!(int_status & FE_INT_SOF);
 214                 eof[i] = !!(int_status & FE_INT_EOF);
 215         }
 216 }
 217
 218 static bool pisp_fe_validate_output(struct pisp_fe_config const *cfg,
 219                                     unsigned int c, struct v4l2_format const *f)
 220 {
 221         unsigned int wbytes;
 222
 223         wbytes = cfg->ch[c].output.format.width;
 224         if (cfg->ch[c].output.format.format & PISP_IMAGE_FORMAT_BPS_MASK)
 225                 wbytes *= 2;
 226
 227         /* Check output image dimensions are nonzero and not too big */
 228         if (cfg->ch[c].output.format.width < 2 ||
 229             cfg->ch[c].output.format.height < 2 ||
 230             cfg->ch[c].output.format.height > f->fmt.pix.height ||
 231             cfg->ch[c].output.format.stride > f->fmt.pix.bytesperline ||
 232             wbytes > f->fmt.pix.bytesperline)
 233                 return false;
 234
 235         /* Check for zero-sized crops, which could cause lockup */
 236         if ((cfg->global.enables & PISP_FE_ENABLE_CROP(c)) &&
 237             ((cfg->ch[c].crop.offset_x >= (cfg->input.format.width & ~1) ||
 238               cfg->ch[c].crop.offset_y >= cfg->input.format.height ||
 239               cfg->ch[c].crop.width < 2 ||
 240               cfg->ch[c].crop.height < 2)))
 241                 return false;
 242
 243         if ((cfg->global.enables & PISP_FE_ENABLE_DOWNSCALE(c)) &&
 244             (cfg->ch[c].downscale.output_width < 2 ||
 245              cfg->ch[c].downscale.output_height < 2))
 246                 return false;
 247
 248         return true;
 249 }
 250
 251 static bool pisp_fe_validate_stats(struct pisp_fe_config const *cfg)
 252 {
 253         /* Check for zero-sized crop, which could cause lockup */
 254         return (!(cfg->global.enables & PISP_FE_ENABLE_STATS_CROP) ||
 255                 (cfg->stats_crop.offset_x < (cfg->input.format.width & ~1) &&
 256                  cfg->stats_crop.offset_y < cfg->input.format.height &&
 257                  cfg->stats_crop.width >= 2 &&
 258                  cfg->stats_crop.height >= 2));
 259 }
 260
 261 int pisp_fe_validate_config(struct pisp_fe_device *fe,
 262                             struct pisp_fe_config *cfg,
 263                             struct v4l2_format const *f0,
 264                             struct v4l2_format const *f1)
 265 {
 266         unsigned int i;
 267
 268         /*
 269          * Check the input is enabled, streaming and has nonzero size;
 270          * to avoid cases where the hardware might lock up or try to
 271          * read inputs from memory (which this driver doesn't support).
 272          */
 273         if (!(cfg->global.enables & PISP_FE_ENABLE_INPUT) ||
 274             cfg->input.streaming != 1 || cfg->input.format.width < 2 ||
 275             cfg->input.format.height < 2) {
 276                 pisp_fe_err("%s: Input config not valid", __func__);
 277                 return -EINVAL;
 278         }
 279
 280         for (i = 0; i < PISP_FE_NUM_OUTPUTS; i++) {
 281                 if (!(cfg->global.enables & PISP_FE_ENABLE_OUTPUT(i))) {
 282                         if (cfg->global.enables &
 283                                         PISP_FE_ENABLE_OUTPUT_CLUSTER(i)) {
 284                                 pisp_fe_err("%s: Output %u not valid",
 285                                             __func__, i);
 286                                 return -EINVAL;
 287                         }
 288                         continue;
 289                 }
 290
 291                 if (!pisp_fe_validate_output(cfg, i, i ? f1 : f0))
 292                         return -EINVAL;
 293         }
 294
 295         if ((cfg->global.enables & PISP_FE_ENABLE_STATS_CLUSTER) &&
 296             !pisp_fe_validate_stats(cfg)) {
 297                 pisp_fe_err("%s: Stats config not valid", __func__);
 298                 return -EINVAL;
 299         }
 300
 301         return 0;
 302 }
 303
 304 void pisp_fe_submit_job(struct pisp_fe_device *fe, struct vb2_buffer **vb2_bufs,
 305                         struct pisp_fe_config *cfg)
 306 {
 307         unsigned int i;
 308         u64 addr;
 309         u32 status;
 310
 311         /*
 312          * Check output buffers exist and outputs are correctly configured.
 313          * If valid, set the buffer's DMA address; otherwise disable.
 314          */
 315         for (i = 0; i < PISP_FE_NUM_OUTPUTS; i++) {
 316                 struct vb2_buffer *buf = vb2_bufs[FE_OUTPUT0_PAD + i];
 317
 318                 if (!(cfg->global.enables & PISP_FE_ENABLE_OUTPUT(i)))
 319                         continue;
 320
 321                 addr = vb2_dma_contig_plane_dma_addr(buf, 0);
 322                 cfg->output_buffer[i].addr_lo = addr & 0xffffffff;
 323                 cfg->output_buffer[i].addr_hi = addr >> 32;
 324         }
 325
 326         if (vb2_bufs[FE_STATS_PAD]) {
 327                 addr = vb2_dma_contig_plane_dma_addr(vb2_bufs[FE_STATS_PAD], 0);
 328                 cfg->stats_buffer.addr_lo = addr & 0xffffffff;
 329                 cfg->stats_buffer.addr_hi = addr >> 32;
 330         }
 331
 332         /* Set up ILINES interrupts 3/4 of the way down each output */
 333         cfg->ch[0].output.ilines =
 334                 max(0x80u, (3u * cfg->ch[0].output.format.height) >> 2);
 335         cfg->ch[1].output.ilines =
 336                 max(0x80u, (3u * cfg->ch[1].output.format.height) >> 2);
 337
 338         /*
 339          * The hardware must have consumed the previous config by now.
 340          * This read of status also serves as a memory barrier before the
 341          * sequence of relaxed writes which follow.
 342          */
 343         status = pisp_fe_reg_read(fe, FE_STATUS);
 344         pisp_fe_dbg_verbose("%s: status = 0x%x\n", __func__, status);
 345         if (WARN_ON(status & FE_STATUS_QUEUED))
 346                 return;
 347
 348         /*
 349          * Unconditionally write buffers, global and input parameters.
 350          * Write cropping and output parameters whenever they are enabled.
 351          * Selectively write other parameters that have been marked as
 352          * changed through the dirty flags.
 353          */
 354         pisp_config_write(fe, cfg, 0,
 355                           offsetof(struct pisp_fe_config, decompress));
 356         cfg->dirty_flags_extra &= ~PISP_FE_DIRTY_GLOBAL;
 357         cfg->dirty_flags &= ~PISP_FE_ENABLE_INPUT;
 358         cfg->dirty_flags |= (cfg->global.enables &
 359                              (PISP_FE_ENABLE_STATS_CROP        |
 360                               PISP_FE_ENABLE_OUTPUT_CLUSTER(0) |
 361                               PISP_FE_ENABLE_OUTPUT_CLUSTER(1)));
 362         for (i = 0; i < ARRAY_SIZE(pisp_fe_config_map); i++) {
 363                 const struct pisp_fe_config_param *p = &pisp_fe_config_map[i];
 364
 365                 if (cfg->dirty_flags & p->dirty_flags ||
 366                     cfg->dirty_flags_extra & p->dirty_flags_extra)
 367                         pisp_config_write(fe, cfg, p->offset, p->size);
 368         }
 369
 370         /* This final non-relaxed write serves as a memory barrier */
 371         pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_QUEUE);
 372 }
 373
 374 void pisp_fe_start(struct pisp_fe_device *fe)
 375 {
 376         pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_RESET);
 377         pisp_fe_reg_write(fe, FE_INT_STATUS, ~0);
 378         pisp_fe_reg_write(fe, FE_INT_EN, FE_INT_EOF | FE_INT_SOF | FE_INT_LINES0 | FE_INT_LINES1);
 379         fe->inframe_count = 0;
 380 }
 381
 382 void pisp_fe_stop(struct pisp_fe_device *fe)
 383 {
 384         pisp_fe_reg_write(fe, FE_INT_EN, 0);
 385         pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_ABORT);
 386         usleep_range(1000, 2000);
 387         WARN_ON(pisp_fe_reg_read(fe, FE_STATUS));
 388         pisp_fe_reg_write(fe, FE_INT_STATUS, ~0);
 389 }
 390
 391 static struct pisp_fe_device *to_pisp_fe_device(struct v4l2_subdev *subdev)
 392 {
 393         return container_of(subdev, struct pisp_fe_device, sd);
 394 }
 395
 396 static int pisp_fe_init_cfg(struct v4l2_subdev *sd,
 397                             struct v4l2_subdev_state *state)
 398 {
 399         struct v4l2_mbus_framefmt *fmt;
 400
 401         fmt = v4l2_subdev_get_pad_format(sd, state, FE_STREAM_PAD);
 402         *fmt = cfe_default_format;
 403         fmt->code = MEDIA_BUS_FMT_SRGGB16_1X16;
 404
 405         fmt = v4l2_subdev_get_pad_format(sd, state, FE_CONFIG_PAD);
 406         *fmt = cfe_default_meta_format;
 407         fmt->code = MEDIA_BUS_FMT_FIXED;
 408
 409         fmt = v4l2_subdev_get_pad_format(sd, state, FE_OUTPUT0_PAD);
 410         *fmt = cfe_default_format;
 411         fmt->code = MEDIA_BUS_FMT_SRGGB16_1X16;
 412
 413         fmt = v4l2_subdev_get_pad_format(sd, state, FE_OUTPUT1_PAD);
 414         *fmt = cfe_default_format;
 415         fmt->code = MEDIA_BUS_FMT_SRGGB16_1X16;
 416
 417         fmt = v4l2_subdev_get_pad_format(sd, state, FE_STATS_PAD);
 418         *fmt = cfe_default_meta_format;
 419         fmt->code = MEDIA_BUS_FMT_FIXED;
 420
 421         return 0;
 422 }
 423
 424 static int pisp_fe_pad_set_fmt(struct v4l2_subdev *sd,
 425                                struct v4l2_subdev_state *state,
 426                                struct v4l2_subdev_format *format)
 427 {
 428         struct v4l2_mbus_framefmt *fmt;
 429         const struct cfe_fmt *cfe_fmt;
 430
 431         /* TODO: format propagation to source pads */
 432         /* TODO: format validation */
 433
 434         switch (format->pad) {
 435         case FE_STREAM_PAD:
 436         case FE_OUTPUT0_PAD:
 437         case FE_OUTPUT1_PAD:
 438                 cfe_fmt = find_format_by_code(format->format.code);
 439                 if (!cfe_fmt || !(cfe_fmt->flags & CFE_FORMAT_FLAG_FE_OUT))
 440                         cfe_fmt = find_format_by_code(MEDIA_BUS_FMT_SRGGB16_1X16);
 441
 442                 format->format.code = cfe_fmt->code;
 443
 444                 break;
 445
 446         case FE_STATS_PAD:
 447         case FE_CONFIG_PAD:
 448                 format->format.code = MEDIA_BUS_FMT_FIXED;
 449                 break;
 450         }
 451
 452         fmt = v4l2_subdev_get_pad_format(sd, state, format->pad);
 453         *fmt = format->format;
 454
 455         return 0;
 456 }
 457
 458 static int pisp_fe_link_validate(struct v4l2_subdev *sd,
 459                                  struct media_link *link,
 460                                  struct v4l2_subdev_format *source_fmt,
 461                                  struct v4l2_subdev_format *sink_fmt)
 462 {
 463         struct pisp_fe_device *fe = to_pisp_fe_device(sd);
 464
 465         pisp_fe_dbg("%s: link \"%s\":%u -> \"%s\":%u\n", __func__,
 466                     link->source->entity->name, link->source->index,
 467                     link->sink->entity->name, link->sink->index);
 468
 469         /* The width, height and code must match. */
 470         if (source_fmt->format.width != sink_fmt->format.width ||
 471             source_fmt->format.width != sink_fmt->format.width ||
 472             source_fmt->format.code != sink_fmt->format.code) {
 473                 pisp_fe_err("%s: format does not match (source %ux%u 0x%x, sink %ux%u 0x%x)\n",
 474                             __func__,
 475                              source_fmt->format.width,
 476                              source_fmt->format.height,
 477                              source_fmt->format.code,
 478                              sink_fmt->format.width,
 479                              sink_fmt->format.height,
 480                              sink_fmt->format.code);
 481                 return -EPIPE;
 482         }
 483
 484         return 0;
 485 }
 486
 487 static const struct v4l2_subdev_pad_ops pisp_fe_subdev_pad_ops = {
 488         .init_cfg = pisp_fe_init_cfg,
 489         .get_fmt = v4l2_subdev_get_fmt,
 490         .set_fmt = pisp_fe_pad_set_fmt,
 491         .link_validate = pisp_fe_link_validate,
 492 };
 493
 494 static const struct media_entity_operations pisp_fe_entity_ops = {
 495         .link_validate = v4l2_subdev_link_validate,
 496 };
 497
 498 static const struct v4l2_subdev_ops pisp_fe_subdev_ops = {
 499         .pad = &pisp_fe_subdev_pad_ops,
 500 };
 501
 502 int pisp_fe_init(struct pisp_fe_device *fe, struct dentry *debugfs)
 503 {
 504         int ret;
 505
 506         debugfs_create_file("pisp_regs", 0444, debugfs, fe, &pisp_regs_fops);
 507
 508         fe->hw_revision = pisp_fe_reg_read(fe, FE_VERSION);
 509         pisp_fe_info("PiSP FE HW v%u.%u\n",
 510                      (fe->hw_revision >> 24) & 0xff,
 511                      (fe->hw_revision >> 20) & 0x0f);
 512
 513         fe->pad[FE_STREAM_PAD].flags =
 514                 MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
 515         fe->pad[FE_CONFIG_PAD].flags = MEDIA_PAD_FL_SINK;
 516         fe->pad[FE_OUTPUT0_PAD].flags = MEDIA_PAD_FL_SOURCE;
 517         fe->pad[FE_OUTPUT1_PAD].flags = MEDIA_PAD_FL_SOURCE;
 518         fe->pad[FE_STATS_PAD].flags = MEDIA_PAD_FL_SOURCE;
 519
 520         ret = media_entity_pads_init(&fe->sd.entity, ARRAY_SIZE(fe->pad),
 521                                      fe->pad);
 522         if (ret)
 523                 return ret;
 524
 525         /* Initialize subdev */
 526         v4l2_subdev_init(&fe->sd, &pisp_fe_subdev_ops);
 527         fe->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
 528         fe->sd.entity.ops = &pisp_fe_entity_ops;
 529         fe->sd.entity.name = "pisp-fe";
 530         fe->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
 531         fe->sd.owner = THIS_MODULE;
 532         snprintf(fe->sd.name, sizeof(fe->sd.name), "pisp-fe");
 533
 534         ret = v4l2_subdev_init_finalize(&fe->sd);
 535         if (ret)
 536                 goto err_entity_cleanup;
 537
 538         ret = v4l2_device_register_subdev(fe->v4l2_dev, &fe->sd);
 539         if (ret) {
 540                 pisp_fe_err("Failed register pisp fe subdev (%d)\n", ret);
 541                 goto err_subdev_cleanup;
 542         }
 543
 544         /* Must be in IDLE state (STATUS == 0) here. */
 545         WARN_ON(pisp_fe_reg_read(fe, FE_STATUS));
 546
 547         return 0;
 548
 549 err_subdev_cleanup:
 550         v4l2_subdev_cleanup(&fe->sd);
 551 err_entity_cleanup:
 552         media_entity_cleanup(&fe->sd.entity);
 553
 554         return ret;
 555 }
 556
 557 void pisp_fe_uninit(struct pisp_fe_device *fe)
 558 {
 559         v4l2_device_unregister_subdev(&fe->sd);
 560         v4l2_subdev_cleanup(&fe->sd);
 561         media_entity_cleanup(&fe->sd.entity);
 562 }