ipu6-isys-csi2.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * Copyright (C) 2013--2024 Intel Corporation
  4 */
  5
  6#include <linux/atomic.h>
  7#include <linux/bitfield.h>
  8#include <linux/bits.h>
  9#include <linux/delay.h>
 10#include <linux/device.h>
 11#include <linux/err.h>
 12#include <linux/io.h>
 13#include <linux/minmax.h>
 14#include <linux/sprintf.h>
 15
 16#include <media/media-entity.h>
 17#include <media/v4l2-ctrls.h>
 18#include <media/v4l2-device.h>
 19#include <media/v4l2-event.h>
 20#include <media/v4l2-subdev.h>
 21
 22#include "ipu6-bus.h"
 23#include "ipu6-isys.h"
 24#include "ipu6-isys-csi2.h"
 25#include "ipu6-isys-subdev.h"
 26#include "ipu6-platform-isys-csi2-reg.h"
 27
 28static const u32 csi2_supported_codes[] = {
 29	MEDIA_BUS_FMT_RGB565_1X16,
 30	MEDIA_BUS_FMT_RGB888_1X24,
 31	MEDIA_BUS_FMT_UYVY8_1X16,
 32	MEDIA_BUS_FMT_YUYV8_1X16,
 33	MEDIA_BUS_FMT_SBGGR10_1X10,
 34	MEDIA_BUS_FMT_SGBRG10_1X10,
 35	MEDIA_BUS_FMT_SGRBG10_1X10,
 36	MEDIA_BUS_FMT_SRGGB10_1X10,
 37	MEDIA_BUS_FMT_SBGGR12_1X12,
 38	MEDIA_BUS_FMT_SGBRG12_1X12,
 39	MEDIA_BUS_FMT_SGRBG12_1X12,
 40	MEDIA_BUS_FMT_SRGGB12_1X12,
 41	MEDIA_BUS_FMT_SBGGR8_1X8,
 42	MEDIA_BUS_FMT_SGBRG8_1X8,
 43	MEDIA_BUS_FMT_SGRBG8_1X8,
 44	MEDIA_BUS_FMT_SRGGB8_1X8,
 45	MEDIA_BUS_FMT_Y8_1X8,
 46	MEDIA_BUS_FMT_Y10_1X10,
 47	MEDIA_BUS_FMT_Y12_1X12,
 48	MEDIA_BUS_FMT_Y16_1X16,
 49	MEDIA_BUS_FMT_META_8,
 50	MEDIA_BUS_FMT_META_10,
 51	MEDIA_BUS_FMT_META_12,
 52	MEDIA_BUS_FMT_META_16,
 53	MEDIA_BUS_FMT_META_24,
 54	0
 55};
 56
 57/*
 58 * Strings corresponding to CSI-2 receiver errors are here.
 59 * Corresponding macros are defined in the header file.
 60 */
 61static const struct ipu6_csi2_error dphy_rx_errors[] = {
 62	{ "Single packet header error corrected", true },
 63	{ "Multiple packet header errors detected", true },
 64	{ "Payload checksum (CRC) error", true },
 65	{ "Transfer FIFO overflow", false },
 66	{ "Reserved short packet data type detected", true },
 67	{ "Reserved long packet data type detected", true },
 68	{ "Incomplete long packet detected", false },
 69	{ "Frame sync error", false },
 70	{ "Line sync error", false },
 71	{ "DPHY recoverable synchronization error", true },
 72	{ "DPHY fatal error", false },
 73	{ "DPHY elastic FIFO overflow", false },
 74	{ "Inter-frame short packet discarded", true },
 75	{ "Inter-frame long packet discarded", true },
 76	{ "MIPI pktgen overflow", false },
 77	{ "MIPI pktgen data loss", false },
 78	{ "FIFO overflow", false },
 79	{ "Lane deskew", false },
 80	{ "SOT sync error", false },
 81	{ "HSIDLE detected", false }
 82};
 83
 84s64 ipu6_isys_csi2_get_link_freq(struct ipu6_isys_csi2 *csi2)
 85{
 86	struct media_pad *src_pad;
 87
 88	if (!csi2)
 89		return -EINVAL;
 90
 91	src_pad = media_entity_remote_source_pad_unique(&csi2->asd.sd.entity);
 92	if (IS_ERR(src_pad)) {
 93		dev_err(&csi2->isys->adev->auxdev.dev,
 94			"can't get source pad of %s (%pe)\n",
 95			csi2->asd.sd.name, src_pad);
 96		return PTR_ERR(src_pad);
 97	}
 98
 99	return v4l2_get_link_freq(src_pad, 0, 0);
100}
101
102static int csi2_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
103				struct v4l2_event_subscription *sub)
104{
105	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
106	struct ipu6_isys_csi2 *csi2 = to_ipu6_isys_csi2(asd);
107	struct device *dev = &csi2->isys->adev->auxdev.dev;
108
109	dev_dbg(dev, "csi2 subscribe event(type %u id %u)\n",
110		sub->type, sub->id);
111
112	switch (sub->type) {
113	case V4L2_EVENT_FRAME_SYNC:
114		return v4l2_event_subscribe(fh, sub, 10, NULL);
115	case V4L2_EVENT_CTRL:
116		return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
117	default:
118		return -EINVAL;
119	}
120}
121
122static const struct v4l2_subdev_core_ops csi2_sd_core_ops = {
123	.subscribe_event = csi2_subscribe_event,
124	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
125};
126
127/*
128 * The input system CSI2+ receiver has several
129 * parameters affecting the receiver timings. These depend
130 * on the MIPI bus frequency F in Hz (sensor transmitter rate)
131 * as follows:
132 *	register value = (A/1e9 + B * UI) / COUNT_ACC
133 * where
134 *	UI = 1 / (2 * F) in seconds
135 *	COUNT_ACC = counter accuracy in seconds
136 *	COUNT_ACC = 0.125 ns = 1 / 8 ns, ACCINV = 8.
137 *
138 * A and B are coefficients from the table below,
139 * depending whether the register minimum or maximum value is
140 * calculated.
141 *				       Minimum     Maximum
142 * Clock lane			       A     B     A     B
143 * reg_rx_csi_dly_cnt_termen_clane     0     0    38     0
144 * reg_rx_csi_dly_cnt_settle_clane    95    -8   300   -16
145 * Data lanes
146 * reg_rx_csi_dly_cnt_termen_dlane0    0     0    35     4
147 * reg_rx_csi_dly_cnt_settle_dlane0   85    -2   145    -6
148 * reg_rx_csi_dly_cnt_termen_dlane1    0     0    35     4
149 * reg_rx_csi_dly_cnt_settle_dlane1   85    -2   145    -6
150 * reg_rx_csi_dly_cnt_termen_dlane2    0     0    35     4
151 * reg_rx_csi_dly_cnt_settle_dlane2   85    -2   145    -6
152 * reg_rx_csi_dly_cnt_termen_dlane3    0     0    35     4
153 * reg_rx_csi_dly_cnt_settle_dlane3   85    -2   145    -6
154 *
155 * We use the minimum values of both A and B.
156 */
157
158#define DIV_SHIFT	8
159#define CSI2_ACCINV	8
160
161static u32 calc_timing(s32 a, s32 b, s64 link_freq, s32 accinv)
162{
163	return accinv * a + (accinv * b * (500000000 >> DIV_SHIFT)
164			     / (s32)(link_freq >> DIV_SHIFT));
165}
166
167static int
168ipu6_isys_csi2_calc_timing(struct ipu6_isys_csi2 *csi2,
169			   struct ipu6_isys_csi2_timing *timing, s32 accinv)
170{
171	struct device *dev = &csi2->isys->adev->auxdev.dev;
172	s64 link_freq;
173
174	link_freq = ipu6_isys_csi2_get_link_freq(csi2);
175	if (link_freq < 0)
176		return link_freq;
177
178	timing->ctermen = calc_timing(CSI2_CSI_RX_DLY_CNT_TERMEN_CLANE_A,
179				      CSI2_CSI_RX_DLY_CNT_TERMEN_CLANE_B,
180				      link_freq, accinv);
181	timing->csettle = calc_timing(CSI2_CSI_RX_DLY_CNT_SETTLE_CLANE_A,
182				      CSI2_CSI_RX_DLY_CNT_SETTLE_CLANE_B,
183				      link_freq, accinv);
184	timing->dtermen = calc_timing(CSI2_CSI_RX_DLY_CNT_TERMEN_DLANE_A,
185				      CSI2_CSI_RX_DLY_CNT_TERMEN_DLANE_B,
186				      link_freq, accinv);
187	timing->dsettle = calc_timing(CSI2_CSI_RX_DLY_CNT_SETTLE_DLANE_A,
188				      CSI2_CSI_RX_DLY_CNT_SETTLE_DLANE_B,
189				      link_freq, accinv);
190
191	dev_dbg(dev, "ctermen %u csettle %u dtermen %u dsettle %u\n",
192		timing->ctermen, timing->csettle,
193		timing->dtermen, timing->dsettle);
194
195	return 0;
196}
197
198void ipu6_isys_register_errors(struct ipu6_isys_csi2 *csi2)
199{
200	u32 irq = readl(csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
201			CSI_PORT_REG_BASE_IRQ_STATUS_OFFSET);
202	struct ipu6_isys *isys = csi2->isys;
203	u32 mask;
204
205	mask = isys->pdata->ipdata->csi2.irq_mask;
206	writel(irq & mask, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
207	       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
208	csi2->receiver_errors |= irq & mask;
209}
210
211void ipu6_isys_csi2_error(struct ipu6_isys_csi2 *csi2)
212{
213	struct device *dev = &csi2->isys->adev->auxdev.dev;
214	const struct ipu6_csi2_error *errors;
215	u32 status;
216	u32 i;
217
218	/* register errors once more in case of interrupts are disabled */
219	ipu6_isys_register_errors(csi2);
220	status = csi2->receiver_errors;
221	csi2->receiver_errors = 0;
222	errors = dphy_rx_errors;
223
224	for (i = 0; i < CSI_RX_NUM_ERRORS_IN_IRQ; i++) {
225		if (status & BIT(i))
226			dev_err_ratelimited(dev, "csi2-%i error: %s\n",
227					    csi2->port, errors[i].error_string);
228	}
229}
230
231static int ipu6_isys_csi2_set_stream(struct v4l2_subdev *sd,
232				     const struct ipu6_isys_csi2_timing *timing,
233				     unsigned int nlanes, int enable)
234{
235	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
236	struct ipu6_isys_csi2 *csi2 = to_ipu6_isys_csi2(asd);
237	struct ipu6_isys *isys = csi2->isys;
238	struct device *dev = &isys->adev->auxdev.dev;
239	struct ipu6_isys_csi2_config cfg;
240	unsigned int nports;
241	int ret = 0;
242	u32 mask = 0;
243	u32 i;
244
245	dev_dbg(dev, "stream %s CSI2-%u with %u lanes\n", enable ? "on" : "off",
246		csi2->port, nlanes);
247
248	cfg.port = csi2->port;
249	cfg.nlanes = nlanes;
250
251	mask = isys->pdata->ipdata->csi2.irq_mask;
252	nports = isys->pdata->ipdata->csi2.nports;
253
254	if (!enable) {
255		writel(0, csi2->base + CSI_REG_CSI_FE_ENABLE);
256		writel(0, csi2->base + CSI_REG_PPI2CSI_ENABLE);
257
258		writel(0,
259		       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
260		       CSI_PORT_REG_BASE_IRQ_ENABLE_OFFSET);
261		writel(mask,
262		       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
263		       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
264		writel(0,
265		       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
266		       CSI_PORT_REG_BASE_IRQ_ENABLE_OFFSET);
267		writel(0xffffffff,
268		       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
269		       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
270
271		isys->phy_set_power(isys, &cfg, timing, false);
272
273		writel(0, isys->pdata->base + CSI_REG_HUB_FW_ACCESS_PORT
274		       (isys->pdata->ipdata->csi2.fw_access_port_ofs,
275			csi2->port));
276		writel(0, isys->pdata->base +
277		       CSI_REG_HUB_DRV_ACCESS_PORT(csi2->port));
278
279		return ret;
280	}
281
282	/* reset port reset */
283	writel(0x1, csi2->base + CSI_REG_PORT_GPREG_SRST);
284	usleep_range(100, 200);
285	writel(0x0, csi2->base + CSI_REG_PORT_GPREG_SRST);
286
287	/* enable port clock */
288	for (i = 0; i < nports; i++) {
289		writel(1, isys->pdata->base + CSI_REG_HUB_DRV_ACCESS_PORT(i));
290		writel(1, isys->pdata->base + CSI_REG_HUB_FW_ACCESS_PORT
291		       (isys->pdata->ipdata->csi2.fw_access_port_ofs, i));
292	}
293
294	/* enable all error related irq */
295	writel(mask,
296	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
297	       CSI_PORT_REG_BASE_IRQ_STATUS_OFFSET);
298	writel(mask,
299	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
300	       CSI_PORT_REG_BASE_IRQ_MASK_OFFSET);
301	writel(mask,
302	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
303	       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
304	writel(mask,
305	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
306	       CSI_PORT_REG_BASE_IRQ_LEVEL_NOT_PULSE_OFFSET);
307	writel(mask,
308	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
309	       CSI_PORT_REG_BASE_IRQ_ENABLE_OFFSET);
310
311	/*
312	 * Using event from firmware instead of irq to handle CSI2 sync event
313	 * which can reduce system wakeups. If CSI2 sync irq enabled, we need
314	 * disable the firmware CSI2 sync event to avoid duplicate handling.
315	 */
316	writel(0xffffffff, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
317	       CSI_PORT_REG_BASE_IRQ_STATUS_OFFSET);
318	writel(0, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
319	       CSI_PORT_REG_BASE_IRQ_MASK_OFFSET);
320	writel(0xffffffff, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
321	       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
322	writel(0, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
323	       CSI_PORT_REG_BASE_IRQ_LEVEL_NOT_PULSE_OFFSET);
324	writel(0xffffffff, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
325	       CSI_PORT_REG_BASE_IRQ_ENABLE_OFFSET);
326
327	/* configure to enable FE and PPI2CSI */
328	writel(0, csi2->base + CSI_REG_CSI_FE_MODE);
329	writel(CSI_SENSOR_INPUT, csi2->base + CSI_REG_CSI_FE_MUX_CTRL);
330	writel(CSI_CNTR_SENSOR_LINE_ID | CSI_CNTR_SENSOR_FRAME_ID,
331	       csi2->base + CSI_REG_CSI_FE_SYNC_CNTR_SEL);
332	writel(FIELD_PREP(PPI_INTF_CONFIG_NOF_ENABLED_DLANES_MASK, nlanes - 1),
333	       csi2->base + CSI_REG_PPI2CSI_CONFIG_PPI_INTF);
334
335	writel(1, csi2->base + CSI_REG_PPI2CSI_ENABLE);
336	writel(1, csi2->base + CSI_REG_CSI_FE_ENABLE);
337
338	ret = isys->phy_set_power(isys, &cfg, timing, true);
339	if (ret)
340		dev_err(dev, "csi-%d phy power up failed %d\n", csi2->port,
341			ret);
342
343	return ret;
344}
345
346static int ipu6_isys_csi2_enable_streams(struct v4l2_subdev *sd,
347					 struct v4l2_subdev_state *state,
348					 u32 pad, u64 streams_mask)
349{
350	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
351	struct ipu6_isys_csi2 *csi2 = to_ipu6_isys_csi2(asd);
352	struct ipu6_isys_csi2_timing timing = { };
353	struct v4l2_subdev *remote_sd;
354	struct media_pad *remote_pad;
355	u64 sink_streams;
356	int ret;
357
358	remote_pad = media_pad_remote_pad_first(&sd->entity.pads[CSI2_PAD_SINK]);
359	remote_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
360
361	sink_streams =
362		v4l2_subdev_state_xlate_streams(state, pad, CSI2_PAD_SINK,
363						&streams_mask);
364
365	ret = ipu6_isys_csi2_calc_timing(csi2, &timing, CSI2_ACCINV);
366	if (ret)
367		return ret;
368
369	ret = ipu6_isys_csi2_set_stream(sd, &timing, csi2->nlanes, true);
370	if (ret)
371		return ret;
372
373	ret = v4l2_subdev_enable_streams(remote_sd, remote_pad->index,
374					 sink_streams);
375	if (ret) {
376		ipu6_isys_csi2_set_stream(sd, NULL, 0, false);
377		return ret;
378	}
379
380	return 0;
381}
382
383static int ipu6_isys_csi2_disable_streams(struct v4l2_subdev *sd,
384					  struct v4l2_subdev_state *state,
385					  u32 pad, u64 streams_mask)
386{
387	struct v4l2_subdev *remote_sd;
388	struct media_pad *remote_pad;
389	u64 sink_streams;
390
391	sink_streams =
392		v4l2_subdev_state_xlate_streams(state, pad, CSI2_PAD_SINK,
393						&streams_mask);
394
395	remote_pad = media_pad_remote_pad_first(&sd->entity.pads[CSI2_PAD_SINK]);
396	remote_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
397
398	ipu6_isys_csi2_set_stream(sd, NULL, 0, false);
399
400	v4l2_subdev_disable_streams(remote_sd, remote_pad->index, sink_streams);
401
402	return 0;
403}
404
405static int ipu6_isys_csi2_set_sel(struct v4l2_subdev *sd,
406				  struct v4l2_subdev_state *state,
407				  struct v4l2_subdev_selection *sel)
408{
409	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
410	struct device *dev = &asd->isys->adev->auxdev.dev;
411	struct v4l2_mbus_framefmt *sink_ffmt;
412	struct v4l2_mbus_framefmt *src_ffmt;
413	struct v4l2_rect *crop;
414
415	if (sel->pad == CSI2_PAD_SINK || sel->target != V4L2_SEL_TGT_CROP)
416		return -EINVAL;
417
418	sink_ffmt = v4l2_subdev_state_get_opposite_stream_format(state,
419								 sel->pad,
420								 sel->stream);
421	if (!sink_ffmt)
422		return -EINVAL;
423
424	src_ffmt = v4l2_subdev_state_get_format(state, sel->pad, sel->stream);
425	if (!src_ffmt)
426		return -EINVAL;
427
428	crop = v4l2_subdev_state_get_crop(state, sel->pad, sel->stream);
429	if (!crop)
430		return -EINVAL;
431
432	/* Only vertical cropping is supported */
433	sel->r.left = 0;
434	sel->r.width = sink_ffmt->width;
435	/* Non-bayer formats can't be single line cropped */
436	if (!ipu6_isys_is_bayer_format(sink_ffmt->code))
437		sel->r.top &= ~1;
438	sel->r.height = clamp(sel->r.height & ~1, IPU6_ISYS_MIN_HEIGHT,
439			      sink_ffmt->height - sel->r.top);
440	*crop = sel->r;
441
442	/* update source pad format */
443	src_ffmt->width = sel->r.width;
444	src_ffmt->height = sel->r.height;
445	if (ipu6_isys_is_bayer_format(sink_ffmt->code))
446		src_ffmt->code = ipu6_isys_convert_bayer_order(sink_ffmt->code,
447							       sel->r.left,
448							       sel->r.top);
449	dev_dbg(dev, "set crop for %s sel: %d,%d,%d,%d code: 0x%x\n",
450		sd->name, sel->r.left, sel->r.top, sel->r.width, sel->r.height,
451		src_ffmt->code);
452
453	return 0;
454}
455
456static int ipu6_isys_csi2_get_sel(struct v4l2_subdev *sd,
457				  struct v4l2_subdev_state *state,
458				  struct v4l2_subdev_selection *sel)
459{
460	struct v4l2_mbus_framefmt *sink_ffmt;
461	struct v4l2_rect *crop;
462	int ret = 0;
463
464	if (sd->entity.pads[sel->pad].flags & MEDIA_PAD_FL_SINK)
465		return -EINVAL;
466
467	sink_ffmt = v4l2_subdev_state_get_opposite_stream_format(state,
468								 sel->pad,
469								 sel->stream);
470	if (!sink_ffmt)
471		return -EINVAL;
472
473	crop = v4l2_subdev_state_get_crop(state, sel->pad, sel->stream);
474	if (!crop)
475		return -EINVAL;
476
477	switch (sel->target) {
478	case V4L2_SEL_TGT_CROP_DEFAULT:
479	case V4L2_SEL_TGT_CROP_BOUNDS:
480		sel->r.left = 0;
481		sel->r.top = 0;
482		sel->r.width = sink_ffmt->width;
483		sel->r.height = sink_ffmt->height;
484		break;
485	case V4L2_SEL_TGT_CROP:
486		sel->r = *crop;
487		break;
488	default:
489		ret = -EINVAL;
490	}
491
492	return ret;
493}
494
495static const struct v4l2_subdev_pad_ops csi2_sd_pad_ops = {
496	.get_fmt = v4l2_subdev_get_fmt,
497	.set_fmt = ipu6_isys_subdev_set_fmt,
498	.get_selection = ipu6_isys_csi2_get_sel,
499	.set_selection = ipu6_isys_csi2_set_sel,
500	.enum_mbus_code = ipu6_isys_subdev_enum_mbus_code,
501	.set_routing = ipu6_isys_subdev_set_routing,
502	.enable_streams = ipu6_isys_csi2_enable_streams,
503	.disable_streams = ipu6_isys_csi2_disable_streams,
504};
505
506static const struct v4l2_subdev_ops csi2_sd_ops = {
507	.core = &csi2_sd_core_ops,
508	.pad = &csi2_sd_pad_ops,
509};
510
511static const struct media_entity_operations csi2_entity_ops = {
512	.link_validate = v4l2_subdev_link_validate,
513	.has_pad_interdep = v4l2_subdev_has_pad_interdep,
514};
515
516void ipu6_isys_csi2_cleanup(struct ipu6_isys_csi2 *csi2)
517{
518	if (!csi2->isys)
519		return;
520
521	v4l2_device_unregister_subdev(&csi2->asd.sd);
522	v4l2_subdev_cleanup(&csi2->asd.sd);
523	ipu6_isys_subdev_cleanup(&csi2->asd);
524	csi2->isys = NULL;
525}
526
527int ipu6_isys_csi2_init(struct ipu6_isys_csi2 *csi2,
528			struct ipu6_isys *isys,
529			void __iomem *base, unsigned int index)
530{
531	struct device *dev = &isys->adev->auxdev.dev;
532	int ret;
533
534	csi2->isys = isys;
535	csi2->base = base;
536	csi2->port = index;
537
538	csi2->asd.sd.entity.ops = &csi2_entity_ops;
539	csi2->asd.isys = isys;
540	ret = ipu6_isys_subdev_init(&csi2->asd, &csi2_sd_ops, 0,
541				    NR_OF_CSI2_SINK_PADS, NR_OF_CSI2_SRC_PADS);
542	if (ret)
543		goto fail;
544
545	csi2->asd.source = IPU6_FW_ISYS_STREAM_SRC_CSI2_PORT0 + index;
546	csi2->asd.supported_codes = csi2_supported_codes;
547	snprintf(csi2->asd.sd.name, sizeof(csi2->asd.sd.name),
548		 IPU6_ISYS_ENTITY_PREFIX " CSI2 %u", index);
549	v4l2_set_subdevdata(&csi2->asd.sd, &csi2->asd);
550	ret = v4l2_subdev_init_finalize(&csi2->asd.sd);
551	if (ret) {
552		dev_err(dev, "failed to init v4l2 subdev\n");
553		goto fail;
554	}
555
556	ret = v4l2_device_register_subdev(&isys->v4l2_dev, &csi2->asd.sd);
557	if (ret) {
558		dev_err(dev, "failed to register v4l2 subdev\n");
559		goto fail;
560	}
561
562	return 0;
563
564fail:
565	ipu6_isys_csi2_cleanup(csi2);
566
567	return ret;
568}
569
570void ipu6_isys_csi2_sof_event_by_stream(struct ipu6_isys_stream *stream)
571{
572	struct video_device *vdev = stream->asd->sd.devnode;
573	struct device *dev = &stream->isys->adev->auxdev.dev;
574	struct ipu6_isys_csi2 *csi2 = ipu6_isys_subdev_to_csi2(stream->asd);
575	struct v4l2_event ev = {
576		.type = V4L2_EVENT_FRAME_SYNC,
577	};
578
579	ev.u.frame_sync.frame_sequence = atomic_fetch_inc(&stream->sequence);
580	v4l2_event_queue(vdev, &ev);
581
582	dev_dbg(dev, "sof_event::csi2-%i sequence: %i, vc: %d\n",
583		csi2->port, ev.u.frame_sync.frame_sequence, stream->vc);
584}
585
586void ipu6_isys_csi2_eof_event_by_stream(struct ipu6_isys_stream *stream)
587{
588	struct device *dev = &stream->isys->adev->auxdev.dev;
589	struct ipu6_isys_csi2 *csi2 = ipu6_isys_subdev_to_csi2(stream->asd);
590	u32 frame_sequence = atomic_read(&stream->sequence);
591
592	dev_dbg(dev, "eof_event::csi2-%i sequence: %i\n",
593		csi2->port, frame_sequence);
594}
595
596int ipu6_isys_csi2_get_remote_desc(u32 source_stream,
597				   struct ipu6_isys_csi2 *csi2,
598				   struct media_entity *source_entity,
599				   struct v4l2_mbus_frame_desc_entry *entry)
600{
601	struct v4l2_mbus_frame_desc_entry *desc_entry = NULL;
602	struct device *dev = &csi2->isys->adev->auxdev.dev;
603	struct v4l2_mbus_frame_desc desc;
604	struct v4l2_subdev *source;
605	struct media_pad *pad;
606	unsigned int i;
607	int ret;
608
609	source = media_entity_to_v4l2_subdev(source_entity);
610	if (!source)
611		return -EPIPE;
612
613	pad = media_pad_remote_pad_first(&csi2->asd.pad[CSI2_PAD_SINK]);
614	if (!pad)
615		return -EPIPE;
616
617	ret = v4l2_subdev_call(source, pad, get_frame_desc, pad->index, &desc);
618	if (ret)
619		return ret;
620
621	if (desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
622		dev_err(dev, "Unsupported frame descriptor type\n");
623		return -EINVAL;
624	}
625
626	for (i = 0; i < desc.num_entries; i++) {
627		if (source_stream == desc.entry[i].stream) {
628			desc_entry = &desc.entry[i];
629			break;
630		}
631	}
632
633	if (!desc_entry) {
634		dev_err(dev, "Failed to find stream %u from remote subdev\n",
635			source_stream);
636		return -EINVAL;
637	}
638
639	if (desc_entry->bus.csi2.vc >= NR_OF_CSI2_VC) {
640		dev_err(dev, "invalid vc %d\n", desc_entry->bus.csi2.vc);
641		return -EINVAL;
642	}
643
644	*entry = *desc_entry;
645
646	return 0;
647}