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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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