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 set_stream(struct v4l2_subdev *sd, int enable) 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 device *dev = &csi2->isys->adev->auxdev.dev; 353 struct ipu6_isys_csi2_timing timing = { }; 354 unsigned int nlanes; 355 int ret; 356 357 dev_dbg(dev, "csi2 stream %s callback\n", enable ? "on" : "off"); 358 359 if (!enable) { 360 csi2->stream_count--; 361 if (csi2->stream_count) 362 return 0; 363 364 ipu6_isys_csi2_set_stream(sd, &timing, 0, enable); 365 return 0; 366 } 367 368 if (csi2->stream_count) { 369 csi2->stream_count++; 370 return 0; 371 } 372 373 nlanes = csi2->nlanes; 374 375 ret = ipu6_isys_csi2_calc_timing(csi2, &timing, CSI2_ACCINV); 376 if (ret) 377 return ret; 378 379 ret = ipu6_isys_csi2_set_stream(sd, &timing, nlanes, enable); 380 if (ret) 381 return ret; 382 383 csi2->stream_count++; 384 385 return 0; 386 } 387 388 static int ipu6_isys_csi2_set_sel(struct v4l2_subdev *sd, 389 struct v4l2_subdev_state *state, 390 struct v4l2_subdev_selection *sel) 391 { 392 struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd); 393 struct device *dev = &asd->isys->adev->auxdev.dev; 394 struct v4l2_mbus_framefmt *sink_ffmt; 395 struct v4l2_mbus_framefmt *src_ffmt; 396 struct v4l2_rect *crop; 397 398 if (sel->pad == CSI2_PAD_SINK || sel->target != V4L2_SEL_TGT_CROP) 399 return -EINVAL; 400 401 sink_ffmt = v4l2_subdev_state_get_opposite_stream_format(state, 402 sel->pad, 403 sel->stream); 404 if (!sink_ffmt) 405 return -EINVAL; 406 407 src_ffmt = v4l2_subdev_state_get_format(state, sel->pad, sel->stream); 408 if (!src_ffmt) 409 return -EINVAL; 410 411 crop = v4l2_subdev_state_get_crop(state, sel->pad, sel->stream); 412 if (!crop) 413 return -EINVAL; 414 415 /* Only vertical cropping is supported */ 416 sel->r.left = 0; 417 sel->r.width = sink_ffmt->width; 418 /* Non-bayer formats can't be single line cropped */ 419 if (!ipu6_isys_is_bayer_format(sink_ffmt->code)) 420 sel->r.top &= ~1; 421 sel->r.height = clamp(sel->r.height & ~1, IPU6_ISYS_MIN_HEIGHT, 422 sink_ffmt->height - sel->r.top); 423 *crop = sel->r; 424 425 /* update source pad format */ 426 src_ffmt->width = sel->r.width; 427 src_ffmt->height = sel->r.height; 428 if (ipu6_isys_is_bayer_format(sink_ffmt->code)) 429 src_ffmt->code = ipu6_isys_convert_bayer_order(sink_ffmt->code, 430 sel->r.left, 431 sel->r.top); 432 dev_dbg(dev, "set crop for %s sel: %d,%d,%d,%d code: 0x%x\n", 433 sd->name, sel->r.left, sel->r.top, sel->r.width, sel->r.height, 434 src_ffmt->code); 435 436 return 0; 437 } 438 439 static int ipu6_isys_csi2_get_sel(struct v4l2_subdev *sd, 440 struct v4l2_subdev_state *state, 441 struct v4l2_subdev_selection *sel) 442 { 443 struct v4l2_mbus_framefmt *sink_ffmt; 444 struct v4l2_rect *crop; 445 int ret = 0; 446 447 if (sd->entity.pads[sel->pad].flags & MEDIA_PAD_FL_SINK) 448 return -EINVAL; 449 450 sink_ffmt = v4l2_subdev_state_get_opposite_stream_format(state, 451 sel->pad, 452 sel->stream); 453 if (!sink_ffmt) 454 return -EINVAL; 455 456 crop = v4l2_subdev_state_get_crop(state, sel->pad, sel->stream); 457 if (!crop) 458 return -EINVAL; 459 460 switch (sel->target) { 461 case V4L2_SEL_TGT_CROP_DEFAULT: 462 case V4L2_SEL_TGT_CROP_BOUNDS: 463 sel->r.left = 0; 464 sel->r.top = 0; 465 sel->r.width = sink_ffmt->width; 466 sel->r.height = sink_ffmt->height; 467 break; 468 case V4L2_SEL_TGT_CROP: 469 sel->r = *crop; 470 break; 471 default: 472 ret = -EINVAL; 473 } 474 475 return ret; 476 } 477 478 static const struct v4l2_subdev_video_ops csi2_sd_video_ops = { 479 .s_stream = set_stream, 480 }; 481 482 static const struct v4l2_subdev_pad_ops csi2_sd_pad_ops = { 483 .get_fmt = v4l2_subdev_get_fmt, 484 .set_fmt = ipu6_isys_subdev_set_fmt, 485 .get_selection = ipu6_isys_csi2_get_sel, 486 .set_selection = ipu6_isys_csi2_set_sel, 487 .enum_mbus_code = ipu6_isys_subdev_enum_mbus_code, 488 .set_routing = ipu6_isys_subdev_set_routing, 489 }; 490 491 static const struct v4l2_subdev_ops csi2_sd_ops = { 492 .core = &csi2_sd_core_ops, 493 .video = &csi2_sd_video_ops, 494 .pad = &csi2_sd_pad_ops, 495 }; 496 497 static const struct media_entity_operations csi2_entity_ops = { 498 .link_validate = v4l2_subdev_link_validate, 499 .has_pad_interdep = v4l2_subdev_has_pad_interdep, 500 }; 501 502 void ipu6_isys_csi2_cleanup(struct ipu6_isys_csi2 *csi2) 503 { 504 if (!csi2->isys) 505 return; 506 507 v4l2_device_unregister_subdev(&csi2->asd.sd); 508 v4l2_subdev_cleanup(&csi2->asd.sd); 509 ipu6_isys_subdev_cleanup(&csi2->asd); 510 csi2->isys = NULL; 511 } 512 513 int ipu6_isys_csi2_init(struct ipu6_isys_csi2 *csi2, 514 struct ipu6_isys *isys, 515 void __iomem *base, unsigned int index) 516 { 517 struct device *dev = &isys->adev->auxdev.dev; 518 int ret; 519 520 csi2->isys = isys; 521 csi2->base = base; 522 csi2->port = index; 523 524 csi2->asd.sd.entity.ops = &csi2_entity_ops; 525 csi2->asd.isys = isys; 526 ret = ipu6_isys_subdev_init(&csi2->asd, &csi2_sd_ops, 0, 527 NR_OF_CSI2_SINK_PADS, NR_OF_CSI2_SRC_PADS); 528 if (ret) 529 goto fail; 530 531 csi2->asd.source = IPU6_FW_ISYS_STREAM_SRC_CSI2_PORT0 + index; 532 csi2->asd.supported_codes = csi2_supported_codes; 533 snprintf(csi2->asd.sd.name, sizeof(csi2->asd.sd.name), 534 IPU6_ISYS_ENTITY_PREFIX " CSI2 %u", index); 535 v4l2_set_subdevdata(&csi2->asd.sd, &csi2->asd); 536 ret = v4l2_subdev_init_finalize(&csi2->asd.sd); 537 if (ret) { 538 dev_err(dev, "failed to init v4l2 subdev\n"); 539 goto fail; 540 } 541 542 ret = v4l2_device_register_subdev(&isys->v4l2_dev, &csi2->asd.sd); 543 if (ret) { 544 dev_err(dev, "failed to register v4l2 subdev\n"); 545 goto fail; 546 } 547 548 return 0; 549 550 fail: 551 ipu6_isys_csi2_cleanup(csi2); 552 553 return ret; 554 } 555 556 void ipu6_isys_csi2_sof_event_by_stream(struct ipu6_isys_stream *stream) 557 { 558 struct video_device *vdev = stream->asd->sd.devnode; 559 struct device *dev = &stream->isys->adev->auxdev.dev; 560 struct ipu6_isys_csi2 *csi2 = ipu6_isys_subdev_to_csi2(stream->asd); 561 struct v4l2_event ev = { 562 .type = V4L2_EVENT_FRAME_SYNC, 563 }; 564 565 ev.u.frame_sync.frame_sequence = atomic_fetch_inc(&stream->sequence); 566 v4l2_event_queue(vdev, &ev); 567 568 dev_dbg(dev, "sof_event::csi2-%i sequence: %i, vc: %d\n", 569 csi2->port, ev.u.frame_sync.frame_sequence, stream->vc); 570 } 571 572 void ipu6_isys_csi2_eof_event_by_stream(struct ipu6_isys_stream *stream) 573 { 574 struct device *dev = &stream->isys->adev->auxdev.dev; 575 struct ipu6_isys_csi2 *csi2 = ipu6_isys_subdev_to_csi2(stream->asd); 576 u32 frame_sequence = atomic_read(&stream->sequence); 577 578 dev_dbg(dev, "eof_event::csi2-%i sequence: %i\n", 579 csi2->port, frame_sequence); 580 } 581 582 int ipu6_isys_csi2_get_remote_desc(u32 source_stream, 583 struct ipu6_isys_csi2 *csi2, 584 struct media_entity *source_entity, 585 struct v4l2_mbus_frame_desc_entry *entry) 586 { 587 struct v4l2_mbus_frame_desc_entry *desc_entry = NULL; 588 struct device *dev = &csi2->isys->adev->auxdev.dev; 589 struct v4l2_mbus_frame_desc desc; 590 struct v4l2_subdev *source; 591 struct media_pad *pad; 592 unsigned int i; 593 int ret; 594 595 source = media_entity_to_v4l2_subdev(source_entity); 596 if (!source) 597 return -EPIPE; 598 599 pad = media_pad_remote_pad_first(&csi2->asd.pad[CSI2_PAD_SINK]); 600 if (!pad) 601 return -EPIPE; 602 603 ret = v4l2_subdev_call(source, pad, get_frame_desc, pad->index, &desc); 604 if (ret) 605 return ret; 606 607 if (desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) { 608 dev_err(dev, "Unsupported frame descriptor type\n"); 609 return -EINVAL; 610 } 611 612 for (i = 0; i < desc.num_entries; i++) { 613 if (source_stream == desc.entry[i].stream) { 614 desc_entry = &desc.entry[i]; 615 break; 616 } 617 } 618 619 if (!desc_entry) { 620 dev_err(dev, "Failed to find stream %u from remote subdev\n", 621 source_stream); 622 return -EINVAL; 623 } 624 625 if (desc_entry->bus.csi2.vc >= NR_OF_CSI2_VC) { 626 dev_err(dev, "invalid vc %d\n", desc_entry->bus.csi2.vc); 627 return -EINVAL; 628 } 629 630 *entry = *desc_entry; 631 632 return 0; 633 } 634 635 void ipu6_isys_set_csi2_streams_status(struct ipu6_isys_video *av, bool status) 636 { 637 struct ipu6_isys_stream *stream = av->stream; 638 struct v4l2_subdev *sd = &stream->asd->sd; 639 struct v4l2_subdev_state *state; 640 struct media_pad *r_pad; 641 unsigned int i; 642 u32 r_stream; 643 644 r_pad = media_pad_remote_pad_first(&av->pad); 645 r_stream = ipu6_isys_get_src_stream_by_src_pad(sd, r_pad->index); 646 647 state = v4l2_subdev_lock_and_get_active_state(sd); 648 649 for (i = 0; i < state->stream_configs.num_configs; i++) { 650 struct v4l2_subdev_stream_config *cfg = 651 &state->stream_configs.configs[i]; 652 653 if (cfg->pad == r_pad->index && r_stream == cfg->stream) { 654 dev_dbg(&av->isys->adev->auxdev.dev, 655 "%s: pad:%u, stream:%u, status:%u\n", 656 sd->entity.name, r_pad->index, r_stream, 657 status); 658 cfg->enabled = status; 659 } 660 } 661 662 v4l2_subdev_unlock_state(state); 663 } 664