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