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