xref: /linux/drivers/media/platform/raspberrypi/rp1-cfe/csi2.c (revision fcb3ad4366b9c810cbb9da34c076a9a52d8aa1e0)
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 }
587