1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * Driver for Cadence MIPI-CSI2 RX Controller v1.3 4 * 5 * Copyright (C) 2017 Cadence Design Systems Inc. 6 */ 7 8 #include <linux/bitfield.h> 9 #include <linux/clk.h> 10 #include <linux/delay.h> 11 #include <linux/export.h> 12 #include <linux/io.h> 13 #include <linux/iopoll.h> 14 #include <linux/module.h> 15 #include <linux/of.h> 16 #include <linux/of_graph.h> 17 #include <linux/phy/phy.h> 18 #include <linux/platform_device.h> 19 #include <linux/reset.h> 20 #include <linux/slab.h> 21 22 #include <media/cadence/cdns-csi2rx.h> 23 #include <media/v4l2-ctrls.h> 24 #include <media/v4l2-device.h> 25 #include <media/v4l2-fwnode.h> 26 #include <media/v4l2-subdev.h> 27 28 #define CSI2RX_DEVICE_CFG_REG 0x000 29 30 #define CSI2RX_SOFT_RESET_REG 0x004 31 #define CSI2RX_SOFT_RESET_PROTOCOL BIT(1) 32 #define CSI2RX_SOFT_RESET_FRONT BIT(0) 33 34 #define CSI2RX_STATIC_CFG_REG 0x008 35 #define CSI2RX_STATIC_CFG_DLANE_MAP(llane, plane) ((plane) << (16 + (llane) * 4)) 36 #define CSI2RX_STATIC_CFG_LANES_MASK GENMASK(11, 8) 37 38 #define CSI2RX_DPHY_LANE_CTRL_REG 0x40 39 #define CSI2RX_DPHY_CL_RST BIT(16) 40 #define CSI2RX_DPHY_DL_RST(i) BIT((i) + 12) 41 #define CSI2RX_DPHY_CL_EN BIT(4) 42 #define CSI2RX_DPHY_DL_EN(i) BIT(i) 43 44 #define CSI2RX_STREAM_BASE(n) (((n) + 1) * 0x100) 45 46 #define CSI2RX_STREAM_CTRL_REG(n) (CSI2RX_STREAM_BASE(n) + 0x000) 47 #define CSI2RX_STREAM_CTRL_SOFT_RST BIT(4) 48 #define CSI2RX_STREAM_CTRL_STOP BIT(1) 49 #define CSI2RX_STREAM_CTRL_START BIT(0) 50 51 #define CSI2RX_STREAM_STATUS_REG(n) (CSI2RX_STREAM_BASE(n) + 0x004) 52 #define CSI2RX_STREAM_STATUS_RDY BIT(31) 53 54 #define CSI2RX_STREAM_DATA_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x008) 55 #define CSI2RX_STREAM_DATA_CFG_VC_SELECT(n) BIT((n) + 16) 56 57 #define CSI2RX_STREAM_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x00c) 58 #define CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF BIT(8) 59 #define CSI2RX_STREAM_CFG_NUM_PIXELS_MASK GENMASK(5, 4) 60 #define CSI2RX_STREAM_CFG_NUM_PIXELS(n) ((n) >> 1U) 61 62 #define CSI2RX_LANES_MAX 4 63 #define CSI2RX_STREAMS_MAX 4 64 65 #define CSI2RX_ERROR_IRQS_REG 0x28 66 #define CSI2RX_ERROR_IRQS_MASK_REG 0x2C 67 68 #define CSI2RX_STREAM3_FIFO_OVERFLOW_IRQ BIT(19) 69 #define CSI2RX_STREAM2_FIFO_OVERFLOW_IRQ BIT(18) 70 #define CSI2RX_STREAM1_FIFO_OVERFLOW_IRQ BIT(17) 71 #define CSI2RX_STREAM0_FIFO_OVERFLOW_IRQ BIT(16) 72 #define CSI2RX_FRONT_TRUNC_HDR_IRQ BIT(12) 73 #define CSI2RX_PROT_TRUNCATED_PACKET_IRQ BIT(11) 74 #define CSI2RX_FRONT_LP_NO_PAYLOAD_IRQ BIT(10) 75 #define CSI2RX_SP_INVALID_RCVD_IRQ BIT(9) 76 #define CSI2RX_DATA_ID_IRQ BIT(7) 77 #define CSI2RX_HEADER_CORRECTED_ECC_IRQ BIT(6) 78 #define CSI2RX_HEADER_ECC_IRQ BIT(5) 79 #define CSI2RX_PAYLOAD_CRC_IRQ BIT(4) 80 81 #define CSI2RX_ECC_ERRORS GENMASK(7, 4) 82 #define CSI2RX_PACKET_ERRORS GENMASK(12, 9) 83 84 enum csi2rx_pads { 85 CSI2RX_PAD_SINK, 86 CSI2RX_PAD_SOURCE_STREAM0, 87 CSI2RX_PAD_SOURCE_STREAM1, 88 CSI2RX_PAD_SOURCE_STREAM2, 89 CSI2RX_PAD_SOURCE_STREAM3, 90 CSI2RX_PAD_MAX, 91 }; 92 93 struct csi2rx_fmt { 94 u32 code; 95 /* width of a single pixel on CSI-2 bus */ 96 u8 bpp; 97 /* max pixels per clock supported on output bus */ 98 u8 max_pixels; 99 }; 100 101 struct csi2rx_event { 102 u32 mask; 103 const char *name; 104 }; 105 106 static const struct csi2rx_event csi2rx_events[] = { 107 { CSI2RX_STREAM3_FIFO_OVERFLOW_IRQ, "Overflow of the Stream 3 FIFO detected" }, 108 { CSI2RX_STREAM2_FIFO_OVERFLOW_IRQ, "Overflow of the Stream 2 FIFO detected" }, 109 { CSI2RX_STREAM1_FIFO_OVERFLOW_IRQ, "Overflow of the Stream 1 FIFO detected" }, 110 { CSI2RX_STREAM0_FIFO_OVERFLOW_IRQ, "Overflow of the Stream 0 FIFO detected" }, 111 { CSI2RX_FRONT_TRUNC_HDR_IRQ, "A truncated header [short or long] has been received" }, 112 { CSI2RX_PROT_TRUNCATED_PACKET_IRQ, "A truncated long packet has been received" }, 113 { CSI2RX_FRONT_LP_NO_PAYLOAD_IRQ, "A truncated long packet has been received. No payload" }, 114 { CSI2RX_SP_INVALID_RCVD_IRQ, "A reserved or invalid short packet has been received" }, 115 { CSI2RX_DATA_ID_IRQ, "Data ID error in the header packet" }, 116 { CSI2RX_HEADER_CORRECTED_ECC_IRQ, "ECC error detected and corrected" }, 117 { CSI2RX_HEADER_ECC_IRQ, "Unrecoverable ECC error" }, 118 { CSI2RX_PAYLOAD_CRC_IRQ, "CRC error" }, 119 }; 120 121 #define CSI2RX_NUM_EVENTS ARRAY_SIZE(csi2rx_events) 122 123 struct csi2rx_priv { 124 struct device *dev; 125 unsigned int count; 126 int error_irq; 127 128 void __iomem *base; 129 struct clk *sys_clk; 130 struct clk *p_clk; 131 struct clk *pixel_clk[CSI2RX_STREAMS_MAX]; 132 struct reset_control *sys_rst; 133 struct reset_control *p_rst; 134 struct reset_control *pixel_rst[CSI2RX_STREAMS_MAX]; 135 struct phy *dphy; 136 137 u8 num_pixels[CSI2RX_STREAMS_MAX]; 138 u32 vc_select[CSI2RX_STREAMS_MAX]; 139 u8 lanes[CSI2RX_LANES_MAX]; 140 u8 num_lanes; 141 u8 max_lanes; 142 u8 max_streams; 143 bool has_internal_dphy; 144 u32 events[CSI2RX_NUM_EVENTS]; 145 146 struct v4l2_subdev subdev; 147 struct v4l2_async_notifier notifier; 148 struct media_pad pads[CSI2RX_PAD_MAX]; 149 150 /* Remote source */ 151 struct v4l2_subdev *source_subdev; 152 int source_pad; 153 }; 154 155 static const struct csi2rx_fmt formats[] = { 156 { .code = MEDIA_BUS_FMT_YUYV8_1X16, .bpp = 16, .max_pixels = 2, }, 157 { .code = MEDIA_BUS_FMT_UYVY8_1X16, .bpp = 16, .max_pixels = 2, }, 158 { .code = MEDIA_BUS_FMT_YVYU8_1X16, .bpp = 16, .max_pixels = 2, }, 159 { .code = MEDIA_BUS_FMT_VYUY8_1X16, .bpp = 16, .max_pixels = 2, }, 160 { .code = MEDIA_BUS_FMT_SBGGR8_1X8, .bpp = 8, .max_pixels = 4, }, 161 { .code = MEDIA_BUS_FMT_SGBRG8_1X8, .bpp = 8, .max_pixels = 4, }, 162 { .code = MEDIA_BUS_FMT_SGRBG8_1X8, .bpp = 8, .max_pixels = 4, }, 163 { .code = MEDIA_BUS_FMT_SRGGB8_1X8, .bpp = 8, .max_pixels = 4, }, 164 { .code = MEDIA_BUS_FMT_Y8_1X8, .bpp = 8, .max_pixels = 4, }, 165 { .code = MEDIA_BUS_FMT_SBGGR10_1X10, .bpp = 10, .max_pixels = 2, }, 166 { .code = MEDIA_BUS_FMT_SGBRG10_1X10, .bpp = 10, .max_pixels = 2, }, 167 { .code = MEDIA_BUS_FMT_SGRBG10_1X10, .bpp = 10, .max_pixels = 2, }, 168 { .code = MEDIA_BUS_FMT_SRGGB10_1X10, .bpp = 10, .max_pixels = 2, }, 169 { .code = MEDIA_BUS_FMT_RGB565_1X16, .bpp = 16, .max_pixels = 1, }, 170 { .code = MEDIA_BUS_FMT_RGB888_1X24, .bpp = 24, .max_pixels = 1, }, 171 { .code = MEDIA_BUS_FMT_BGR888_1X24, .bpp = 24, .max_pixels = 1, }, 172 }; 173 174 static void csi2rx_configure_error_irq_mask(void __iomem *base, 175 struct csi2rx_priv *csi2rx) 176 { 177 u32 error_irq_mask = 0; 178 179 error_irq_mask |= CSI2RX_ECC_ERRORS; 180 error_irq_mask |= CSI2RX_PACKET_ERRORS; 181 182 /* 183 * Iterate through all source pads and check if they are linked 184 * to an active remote pad. If an active remote pad is found, 185 * calculate the corresponding bit position and set it in 186 * mask, enabling the stream overflow error in the mask. 187 */ 188 for (int i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) { 189 struct media_pad *remote_pad; 190 191 remote_pad = media_pad_remote_pad_first(&csi2rx->pads[i]); 192 if (remote_pad) { 193 int pad = i - CSI2RX_PAD_SOURCE_STREAM0; 194 u32 bit_mask = CSI2RX_STREAM0_FIFO_OVERFLOW_IRQ << pad; 195 196 error_irq_mask |= bit_mask; 197 } 198 } 199 200 writel(error_irq_mask, base + CSI2RX_ERROR_IRQS_MASK_REG); 201 } 202 203 static irqreturn_t csi2rx_irq_handler(int irq, void *dev_id) 204 { 205 struct csi2rx_priv *csi2rx = dev_id; 206 int i; 207 u32 error_status, error_mask; 208 209 error_status = readl(csi2rx->base + CSI2RX_ERROR_IRQS_REG); 210 error_mask = readl(csi2rx->base + CSI2RX_ERROR_IRQS_MASK_REG); 211 212 for (i = 0; i < CSI2RX_NUM_EVENTS; i++) 213 if ((error_status & csi2rx_events[i].mask) && 214 (error_mask & csi2rx_events[i].mask)) 215 csi2rx->events[i]++; 216 217 writel(error_status, csi2rx->base + CSI2RX_ERROR_IRQS_REG); 218 219 return IRQ_HANDLED; 220 } 221 222 static const struct csi2rx_fmt *csi2rx_get_fmt_by_code(u32 code) 223 { 224 unsigned int i; 225 226 for (i = 0; i < ARRAY_SIZE(formats); i++) 227 if (formats[i].code == code) 228 return &formats[i]; 229 230 return NULL; 231 } 232 233 static int csi2rx_get_frame_desc_from_source(struct csi2rx_priv *csi2rx, 234 struct v4l2_mbus_frame_desc *fd) 235 { 236 struct media_pad *remote_pad; 237 238 remote_pad = media_entity_remote_source_pad_unique(&csi2rx->subdev.entity); 239 if (IS_ERR(remote_pad)) { 240 dev_err(csi2rx->dev, "No remote pad found for sink\n"); 241 return PTR_ERR(remote_pad); 242 } 243 244 return v4l2_subdev_call(csi2rx->source_subdev, pad, get_frame_desc, 245 remote_pad->index, fd); 246 } 247 248 static inline 249 struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev) 250 { 251 return container_of(subdev, struct csi2rx_priv, subdev); 252 } 253 254 static void csi2rx_reset(struct csi2rx_priv *csi2rx) 255 { 256 unsigned int i; 257 258 /* Reset module */ 259 writel(CSI2RX_SOFT_RESET_PROTOCOL | CSI2RX_SOFT_RESET_FRONT, 260 csi2rx->base + CSI2RX_SOFT_RESET_REG); 261 /* Reset individual streams. */ 262 for (i = 0; i < csi2rx->max_streams; i++) { 263 writel(CSI2RX_STREAM_CTRL_SOFT_RST, 264 csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); 265 } 266 267 usleep_range(10, 20); 268 269 /* Clear resets */ 270 writel(0, csi2rx->base + CSI2RX_SOFT_RESET_REG); 271 for (i = 0; i < csi2rx->max_streams; i++) 272 writel(0, csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); 273 } 274 275 static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx) 276 { 277 union phy_configure_opts opts = { }; 278 struct phy_configure_opts_mipi_dphy *cfg = &opts.mipi_dphy; 279 struct v4l2_mbus_framefmt *framefmt; 280 struct v4l2_subdev_state *state; 281 const struct csi2rx_fmt *fmt; 282 struct v4l2_subdev_route *route; 283 int source_pad = csi2rx->source_pad; 284 struct media_pad *pad = &csi2rx->source_subdev->entity.pads[source_pad]; 285 s64 link_freq; 286 int ret; 287 u32 bpp; 288 289 state = v4l2_subdev_get_locked_active_state(&csi2rx->subdev); 290 291 /* 292 * For multi-stream transmitters there is no single pixel rate. 293 * 294 * In multistream usecase pass bpp as 0 so that v4l2_get_link_freq() 295 * returns an error if it falls back to V4L2_CID_PIXEL_RATE. 296 */ 297 if (state->routing.num_routes > 1) { 298 bpp = 0; 299 } else { 300 route = &state->routing.routes[0]; 301 framefmt = v4l2_subdev_state_get_format(state, CSI2RX_PAD_SINK, 302 route->sink_stream); 303 if (!framefmt) { 304 dev_err(csi2rx->dev, "Did not find active sink format\n"); 305 return -EINVAL; 306 } 307 308 fmt = csi2rx_get_fmt_by_code(framefmt->code); 309 bpp = fmt->bpp; 310 } 311 312 link_freq = v4l2_get_link_freq(pad, bpp, 2 * csi2rx->num_lanes); 313 if (link_freq < 0) { 314 dev_err(csi2rx->dev, "Unable to calculate link frequency\n"); 315 return link_freq; 316 } 317 318 ret = phy_mipi_dphy_get_default_config_for_hsclk(link_freq, 319 csi2rx->num_lanes, cfg); 320 if (ret) 321 return ret; 322 323 ret = phy_power_on(csi2rx->dphy); 324 if (ret) 325 return ret; 326 327 ret = phy_configure(csi2rx->dphy, &opts); 328 if (ret) { 329 phy_power_off(csi2rx->dphy); 330 return ret; 331 } 332 333 return 0; 334 } 335 336 static int csi2rx_start(struct csi2rx_priv *csi2rx) 337 { 338 unsigned int i; 339 unsigned long lanes_used = 0; 340 u32 reg; 341 int ret; 342 343 csi2rx_reset(csi2rx); 344 345 if (csi2rx->error_irq >= 0) 346 csi2rx_configure_error_irq_mask(csi2rx->base, csi2rx); 347 348 reg = csi2rx->num_lanes << 8; 349 for (i = 0; i < csi2rx->num_lanes; i++) { 350 reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, csi2rx->lanes[i]); 351 set_bit(csi2rx->lanes[i], &lanes_used); 352 } 353 354 /* 355 * Even the unused lanes need to be mapped. In order to avoid 356 * to map twice to the same physical lane, keep the lanes used 357 * in the previous loop, and only map unused physical lanes to 358 * the rest of our logical lanes. 359 */ 360 for (i = csi2rx->num_lanes; i < csi2rx->max_lanes; i++) { 361 unsigned int idx = find_first_zero_bit(&lanes_used, 362 csi2rx->max_lanes); 363 set_bit(idx, &lanes_used); 364 reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, i + 1); 365 } 366 367 writel(reg, csi2rx->base + CSI2RX_STATIC_CFG_REG); 368 369 /* Enable DPHY clk and data lanes. */ 370 if (csi2rx->dphy) { 371 reg = CSI2RX_DPHY_CL_EN | CSI2RX_DPHY_CL_RST; 372 for (i = 0; i < csi2rx->num_lanes; i++) { 373 reg |= CSI2RX_DPHY_DL_EN(csi2rx->lanes[i] - 1); 374 reg |= CSI2RX_DPHY_DL_RST(csi2rx->lanes[i] - 1); 375 } 376 377 writel(reg, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG); 378 379 ret = csi2rx_configure_ext_dphy(csi2rx); 380 if (ret) { 381 dev_err(csi2rx->dev, 382 "Failed to configure external DPHY: %d\n", ret); 383 return ret; 384 } 385 } 386 387 /* 388 * Create a static mapping between the CSI virtual channels 389 * and the output stream. 390 * 391 * This should be enhanced, but v4l2 lacks the support for 392 * changing that mapping dynamically. 393 * 394 * We also cannot enable and disable independent streams here, 395 * hence the reference counting. 396 */ 397 for (i = 0; i < csi2rx->max_streams; i++) { 398 writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF | 399 FIELD_PREP(CSI2RX_STREAM_CFG_NUM_PIXELS_MASK, 400 csi2rx->num_pixels[i]), 401 csi2rx->base + CSI2RX_STREAM_CFG_REG(i)); 402 403 writel(csi2rx->vc_select[i], 404 csi2rx->base + CSI2RX_STREAM_DATA_CFG_REG(i)); 405 406 writel(CSI2RX_STREAM_CTRL_START, 407 csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); 408 } 409 410 411 return 0; 412 } 413 414 static void csi2rx_stop(struct csi2rx_priv *csi2rx) 415 { 416 unsigned int i; 417 u32 val; 418 int ret; 419 420 writel(0, csi2rx->base + CSI2RX_ERROR_IRQS_MASK_REG); 421 422 for (i = 0; i < csi2rx->max_streams; i++) { 423 writel(CSI2RX_STREAM_CTRL_STOP, 424 csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); 425 426 ret = readl_relaxed_poll_timeout(csi2rx->base + 427 CSI2RX_STREAM_STATUS_REG(i), 428 val, 429 !(val & CSI2RX_STREAM_STATUS_RDY), 430 10, 10000); 431 if (ret) 432 dev_warn(csi2rx->dev, 433 "Failed to stop streaming on pad%u\n", i); 434 } 435 436 if (csi2rx->dphy) { 437 writel(0, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG); 438 439 if (phy_power_off(csi2rx->dphy)) 440 dev_warn(csi2rx->dev, "Couldn't power off DPHY\n"); 441 } 442 } 443 444 static int csi2rx_log_status(struct v4l2_subdev *sd) 445 { 446 struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(sd); 447 unsigned int i; 448 449 for (i = 0; i < CSI2RX_NUM_EVENTS; i++) { 450 if (csi2rx->events[i]) 451 dev_info(csi2rx->dev, "%s events: %d\n", 452 csi2rx_events[i].name, 453 csi2rx->events[i]); 454 } 455 456 return 0; 457 } 458 459 static void csi2rx_update_vc_select(struct csi2rx_priv *csi2rx, 460 struct v4l2_subdev_state *state) 461 { 462 struct v4l2_mbus_frame_desc fd = {0}; 463 struct v4l2_subdev_route *route; 464 unsigned int i; 465 int ret; 466 467 ret = csi2rx_get_frame_desc_from_source(csi2rx, &fd); 468 if (ret || fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) { 469 dev_dbg(csi2rx->dev, 470 "Failed to get source frame desc, allowing only VC=0\n"); 471 for (i = 0; i < CSI2RX_STREAMS_MAX; i++) 472 csi2rx->vc_select[i] = CSI2RX_STREAM_DATA_CFG_VC_SELECT(0); 473 return; 474 } 475 476 /* If source provides per-stream VC info, use it to filter by VC */ 477 memset(csi2rx->vc_select, 0, sizeof(csi2rx->vc_select)); 478 479 for_each_active_route(&state->routing, route) { 480 u32 cdns_stream = route->source_pad - CSI2RX_PAD_SOURCE_STREAM0; 481 482 for (i = 0; i < fd.num_entries; i++) { 483 if (fd.entry[i].stream != route->sink_stream) 484 continue; 485 486 csi2rx->vc_select[cdns_stream] |= 487 CSI2RX_STREAM_DATA_CFG_VC_SELECT(fd.entry[i].bus.csi2.vc); 488 } 489 } 490 } 491 492 static int csi2rx_enable_streams(struct v4l2_subdev *subdev, 493 struct v4l2_subdev_state *state, u32 pad, 494 u64 streams_mask) 495 { 496 struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); 497 u64 sink_streams; 498 int ret; 499 500 sink_streams = v4l2_subdev_state_xlate_streams(state, pad, 501 CSI2RX_PAD_SINK, 502 &streams_mask); 503 504 /* 505 * If we're not the first users, there's no need to 506 * enable the whole controller. 507 */ 508 if (!csi2rx->count) { 509 ret = pm_runtime_resume_and_get(csi2rx->dev); 510 if (ret < 0) 511 goto err; 512 513 csi2rx_update_vc_select(csi2rx, state); 514 515 ret = csi2rx_start(csi2rx); 516 if (ret) 517 goto err_put_pm; 518 } 519 520 /* Start streaming on the source */ 521 ret = v4l2_subdev_enable_streams(csi2rx->source_subdev, csi2rx->source_pad, 522 sink_streams); 523 if (ret) { 524 dev_err(csi2rx->dev, 525 "Failed to start streams %#llx on subdev\n", 526 sink_streams); 527 goto err_stop_csi; 528 } 529 530 csi2rx->count++; 531 return 0; 532 533 err_stop_csi: 534 if (!csi2rx->count) 535 csi2rx_stop(csi2rx); 536 err_put_pm: 537 if (!csi2rx->count) 538 pm_runtime_put(csi2rx->dev); 539 err: 540 return ret; 541 } 542 543 static int csi2rx_disable_streams(struct v4l2_subdev *subdev, 544 struct v4l2_subdev_state *state, u32 pad, 545 u64 streams_mask) 546 { 547 struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); 548 u64 sink_streams; 549 550 sink_streams = v4l2_subdev_state_xlate_streams(state, pad, 551 CSI2RX_PAD_SINK, 552 &streams_mask); 553 554 if (v4l2_subdev_disable_streams(csi2rx->source_subdev, 555 csi2rx->source_pad, sink_streams)) { 556 dev_err(csi2rx->dev, "Couldn't disable our subdev\n"); 557 } 558 559 csi2rx->count--; 560 561 /* Let the last user turn off the lights. */ 562 if (!csi2rx->count) { 563 csi2rx_stop(csi2rx); 564 pm_runtime_put(csi2rx->dev); 565 } 566 567 return 0; 568 } 569 570 static int csi2rx_enum_mbus_code(struct v4l2_subdev *subdev, 571 struct v4l2_subdev_state *state, 572 struct v4l2_subdev_mbus_code_enum *code_enum) 573 { 574 if (code_enum->index >= ARRAY_SIZE(formats)) 575 return -EINVAL; 576 577 code_enum->code = formats[code_enum->index].code; 578 579 return 0; 580 } 581 582 static int _csi2rx_set_routing(struct v4l2_subdev *subdev, 583 struct v4l2_subdev_state *state, 584 struct v4l2_subdev_krouting *routing) 585 { 586 static const struct v4l2_mbus_framefmt format = { 587 .width = 640, 588 .height = 480, 589 .code = MEDIA_BUS_FMT_UYVY8_1X16, 590 .field = V4L2_FIELD_NONE, 591 .colorspace = V4L2_COLORSPACE_SRGB, 592 .ycbcr_enc = V4L2_YCBCR_ENC_601, 593 .quantization = V4L2_QUANTIZATION_LIM_RANGE, 594 .xfer_func = V4L2_XFER_FUNC_SRGB, 595 }; 596 int ret; 597 598 ret = v4l2_subdev_routing_validate(subdev, routing, 599 V4L2_SUBDEV_ROUTING_ONLY_1_TO_1); 600 if (ret) 601 return ret; 602 603 return v4l2_subdev_set_routing_with_fmt(subdev, state, routing, &format); 604 } 605 606 static int csi2rx_set_routing(struct v4l2_subdev *subdev, 607 struct v4l2_subdev_state *state, 608 enum v4l2_subdev_format_whence which, 609 struct v4l2_subdev_krouting *routing) 610 { 611 struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); 612 int ret; 613 614 if (which == V4L2_SUBDEV_FORMAT_ACTIVE && csi2rx->count) 615 return -EBUSY; 616 617 ret = _csi2rx_set_routing(subdev, state, routing); 618 if (ret) 619 return ret; 620 621 return 0; 622 } 623 624 static int csi2rx_set_fmt(struct v4l2_subdev *subdev, 625 struct v4l2_subdev_state *state, 626 struct v4l2_subdev_format *format) 627 { 628 struct v4l2_mbus_framefmt *fmt; 629 630 /* No transcoding, source and sink formats must match. */ 631 if (format->pad != CSI2RX_PAD_SINK) 632 return v4l2_subdev_get_fmt(subdev, state, format); 633 634 if (!csi2rx_get_fmt_by_code(format->format.code)) 635 format->format.code = formats[0].code; 636 637 format->format.field = V4L2_FIELD_NONE; 638 639 /* Set sink format */ 640 fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream); 641 *fmt = format->format; 642 643 /* Propagate to source format */ 644 fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad, 645 format->stream); 646 if (!fmt) 647 return -EINVAL; 648 649 *fmt = format->format; 650 651 return 0; 652 } 653 654 static int csi2rx_init_state(struct v4l2_subdev *subdev, 655 struct v4l2_subdev_state *state) 656 { 657 struct v4l2_subdev_route routes[] = { 658 { 659 .sink_pad = CSI2RX_PAD_SINK, 660 .sink_stream = 0, 661 .source_pad = CSI2RX_PAD_SOURCE_STREAM0, 662 .source_stream = 0, 663 .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE, 664 }, 665 }; 666 667 struct v4l2_subdev_krouting routing = { 668 .num_routes = ARRAY_SIZE(routes), 669 .routes = routes, 670 }; 671 672 return _csi2rx_set_routing(subdev, state, &routing); 673 } 674 675 int cdns_csi2rx_negotiate_ppc(struct v4l2_subdev *subdev, unsigned int pad, 676 u8 *ppc) 677 { 678 struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); 679 const struct csi2rx_fmt *csi_fmt; 680 struct v4l2_subdev_route *route; 681 struct v4l2_subdev_state *state; 682 struct v4l2_mbus_framefmt *fmt; 683 int ret = 0; 684 685 if (!ppc || pad < CSI2RX_PAD_SOURCE_STREAM0 || pad >= CSI2RX_PAD_MAX) 686 return -EINVAL; 687 688 state = v4l2_subdev_lock_and_get_active_state(subdev); 689 /* Check all streams on requested pad */ 690 for_each_active_route(&state->routing, route) { 691 if (route->source_pad != pad) 692 continue; 693 694 fmt = v4l2_subdev_state_get_format(state, route->source_pad, 695 route->source_stream); 696 if (!fmt) { 697 ret = -EPIPE; 698 *ppc = 1; 699 break; 700 } 701 702 csi_fmt = csi2rx_get_fmt_by_code(fmt->code); 703 if (!csi_fmt) { 704 ret = -EINVAL; 705 *ppc = 1; 706 break; 707 } 708 709 /* Reduce requested PPC if it is too high for this stream */ 710 *ppc = min(*ppc, csi_fmt->max_pixels); 711 } 712 v4l2_subdev_unlock_state(state); 713 714 csi2rx->num_pixels[pad - CSI2RX_PAD_SOURCE_STREAM0] = 715 CSI2RX_STREAM_CFG_NUM_PIXELS(*ppc); 716 717 return ret; 718 } 719 EXPORT_SYMBOL_FOR_MODULES(cdns_csi2rx_negotiate_ppc, "j721e-csi2rx"); 720 721 static const struct v4l2_subdev_pad_ops csi2rx_pad_ops = { 722 .enum_mbus_code = csi2rx_enum_mbus_code, 723 .get_fmt = v4l2_subdev_get_fmt, 724 .set_fmt = csi2rx_set_fmt, 725 .get_frame_desc = v4l2_subdev_get_frame_desc_passthrough, 726 .set_routing = csi2rx_set_routing, 727 .enable_streams = csi2rx_enable_streams, 728 .disable_streams = csi2rx_disable_streams, 729 }; 730 731 static const struct v4l2_subdev_core_ops csi2rx_core_ops = { 732 .log_status = csi2rx_log_status, 733 }; 734 735 static const struct v4l2_subdev_ops csi2rx_subdev_ops = { 736 .core = &csi2rx_core_ops, 737 .pad = &csi2rx_pad_ops, 738 }; 739 740 static const struct v4l2_subdev_internal_ops csi2rx_internal_ops = { 741 .init_state = csi2rx_init_state, 742 }; 743 744 static const struct media_entity_operations csi2rx_media_ops = { 745 .link_validate = v4l2_subdev_link_validate, 746 .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1, 747 .has_pad_interdep = v4l2_subdev_has_pad_interdep, 748 }; 749 750 static int csi2rx_async_bound(struct v4l2_async_notifier *notifier, 751 struct v4l2_subdev *s_subdev, 752 struct v4l2_async_connection *asd) 753 { 754 struct v4l2_subdev *subdev = notifier->sd; 755 struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); 756 757 csi2rx->source_pad = media_entity_get_fwnode_pad(&s_subdev->entity, 758 asd->match.fwnode, 759 MEDIA_PAD_FL_SOURCE); 760 if (csi2rx->source_pad < 0) { 761 dev_err(csi2rx->dev, "Couldn't find output pad for subdev %s\n", 762 s_subdev->name); 763 return csi2rx->source_pad; 764 } 765 766 csi2rx->source_subdev = s_subdev; 767 768 dev_dbg(csi2rx->dev, "Bound %s pad: %d\n", s_subdev->name, 769 csi2rx->source_pad); 770 771 return media_create_pad_link(&csi2rx->source_subdev->entity, 772 csi2rx->source_pad, 773 &csi2rx->subdev.entity, 0, 774 MEDIA_LNK_FL_ENABLED | 775 MEDIA_LNK_FL_IMMUTABLE); 776 } 777 778 static const struct v4l2_async_notifier_operations csi2rx_notifier_ops = { 779 .bound = csi2rx_async_bound, 780 }; 781 782 static int csi2rx_get_resources(struct csi2rx_priv *csi2rx, 783 struct platform_device *pdev) 784 { 785 unsigned char i; 786 u32 dev_cfg; 787 int ret; 788 789 csi2rx->base = devm_platform_ioremap_resource(pdev, 0); 790 if (IS_ERR(csi2rx->base)) 791 return PTR_ERR(csi2rx->base); 792 793 csi2rx->sys_clk = devm_clk_get(&pdev->dev, "sys_clk"); 794 if (IS_ERR(csi2rx->sys_clk)) { 795 dev_err(&pdev->dev, "Couldn't get sys clock\n"); 796 return PTR_ERR(csi2rx->sys_clk); 797 } 798 799 csi2rx->p_clk = devm_clk_get(&pdev->dev, "p_clk"); 800 if (IS_ERR(csi2rx->p_clk)) { 801 dev_err(&pdev->dev, "Couldn't get P clock\n"); 802 return PTR_ERR(csi2rx->p_clk); 803 } 804 805 csi2rx->sys_rst = devm_reset_control_get_optional_exclusive(&pdev->dev, 806 "sys"); 807 if (IS_ERR(csi2rx->sys_rst)) 808 return PTR_ERR(csi2rx->sys_rst); 809 810 csi2rx->p_rst = devm_reset_control_get_optional_exclusive(&pdev->dev, 811 "reg_bank"); 812 if (IS_ERR(csi2rx->p_rst)) 813 return PTR_ERR(csi2rx->p_rst); 814 815 csi2rx->dphy = devm_phy_optional_get(&pdev->dev, "dphy"); 816 if (IS_ERR(csi2rx->dphy)) { 817 dev_err(&pdev->dev, "Couldn't get external D-PHY\n"); 818 return PTR_ERR(csi2rx->dphy); 819 } 820 821 ret = clk_prepare_enable(csi2rx->p_clk); 822 if (ret) { 823 dev_err(&pdev->dev, "Couldn't prepare and enable P clock\n"); 824 return ret; 825 } 826 827 dev_cfg = readl(csi2rx->base + CSI2RX_DEVICE_CFG_REG); 828 clk_disable_unprepare(csi2rx->p_clk); 829 830 csi2rx->max_lanes = dev_cfg & 7; 831 if (csi2rx->max_lanes > CSI2RX_LANES_MAX) { 832 dev_err(&pdev->dev, "Invalid number of lanes: %u\n", 833 csi2rx->max_lanes); 834 return -EINVAL; 835 } 836 837 csi2rx->max_streams = (dev_cfg >> 4) & 7; 838 if (csi2rx->max_streams > CSI2RX_STREAMS_MAX) { 839 dev_err(&pdev->dev, "Invalid number of streams: %u\n", 840 csi2rx->max_streams); 841 return -EINVAL; 842 } 843 844 csi2rx->has_internal_dphy = dev_cfg & BIT(3) ? true : false; 845 846 /* 847 * FIXME: Once we'll have internal D-PHY support, the check 848 * will need to be removed. 849 */ 850 if (!csi2rx->dphy && csi2rx->has_internal_dphy) { 851 dev_err(&pdev->dev, "Internal D-PHY not supported yet\n"); 852 return -EINVAL; 853 } 854 855 for (i = 0; i < csi2rx->max_streams; i++) { 856 char name[16]; 857 858 snprintf(name, sizeof(name), "pixel_if%u_clk", i); 859 csi2rx->pixel_clk[i] = devm_clk_get(&pdev->dev, name); 860 if (IS_ERR(csi2rx->pixel_clk[i])) { 861 dev_err(&pdev->dev, "Couldn't get clock %s\n", name); 862 return PTR_ERR(csi2rx->pixel_clk[i]); 863 } 864 865 snprintf(name, sizeof(name), "pixel_if%u", i); 866 csi2rx->pixel_rst[i] = 867 devm_reset_control_get_optional_exclusive(&pdev->dev, 868 name); 869 if (IS_ERR(csi2rx->pixel_rst[i])) 870 return PTR_ERR(csi2rx->pixel_rst[i]); 871 } 872 873 return 0; 874 } 875 876 static int csi2rx_parse_dt(struct csi2rx_priv *csi2rx) 877 { 878 struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 }; 879 struct v4l2_async_connection *asd; 880 struct fwnode_handle *fwh; 881 struct device_node *ep; 882 int ret; 883 884 ep = of_graph_get_endpoint_by_regs(csi2rx->dev->of_node, 0, 0); 885 if (!ep) 886 return -EINVAL; 887 888 fwh = of_fwnode_handle(ep); 889 ret = v4l2_fwnode_endpoint_parse(fwh, &v4l2_ep); 890 if (ret) { 891 dev_err(csi2rx->dev, "Could not parse v4l2 endpoint\n"); 892 of_node_put(ep); 893 return ret; 894 } 895 896 if (v4l2_ep.bus_type != V4L2_MBUS_CSI2_DPHY) { 897 dev_err(csi2rx->dev, "Unsupported media bus type: 0x%x\n", 898 v4l2_ep.bus_type); 899 of_node_put(ep); 900 return -EINVAL; 901 } 902 903 memcpy(csi2rx->lanes, v4l2_ep.bus.mipi_csi2.data_lanes, 904 sizeof(csi2rx->lanes)); 905 csi2rx->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes; 906 if (csi2rx->num_lanes > csi2rx->max_lanes) { 907 dev_err(csi2rx->dev, "Unsupported number of data-lanes: %d\n", 908 csi2rx->num_lanes); 909 of_node_put(ep); 910 return -EINVAL; 911 } 912 913 v4l2_async_subdev_nf_init(&csi2rx->notifier, &csi2rx->subdev); 914 915 asd = v4l2_async_nf_add_fwnode_remote(&csi2rx->notifier, fwh, 916 struct v4l2_async_connection); 917 of_node_put(ep); 918 if (IS_ERR(asd)) { 919 v4l2_async_nf_cleanup(&csi2rx->notifier); 920 return PTR_ERR(asd); 921 } 922 923 csi2rx->notifier.ops = &csi2rx_notifier_ops; 924 925 ret = v4l2_async_nf_register(&csi2rx->notifier); 926 if (ret) 927 v4l2_async_nf_cleanup(&csi2rx->notifier); 928 929 return ret; 930 } 931 932 static int csi2rx_probe(struct platform_device *pdev) 933 { 934 struct csi2rx_priv *csi2rx; 935 unsigned int i; 936 int ret; 937 938 csi2rx = kzalloc_obj(*csi2rx); 939 if (!csi2rx) 940 return -ENOMEM; 941 platform_set_drvdata(pdev, csi2rx); 942 csi2rx->dev = &pdev->dev; 943 944 ret = csi2rx_get_resources(csi2rx, pdev); 945 if (ret) 946 goto err_free_priv; 947 948 ret = csi2rx_parse_dt(csi2rx); 949 if (ret) 950 goto err_free_priv; 951 952 csi2rx->subdev.owner = THIS_MODULE; 953 csi2rx->subdev.dev = &pdev->dev; 954 v4l2_subdev_init(&csi2rx->subdev, &csi2rx_subdev_ops); 955 csi2rx->subdev.internal_ops = &csi2rx_internal_ops; 956 v4l2_set_subdevdata(&csi2rx->subdev, &pdev->dev); 957 snprintf(csi2rx->subdev.name, sizeof(csi2rx->subdev.name), 958 "%s.%s", KBUILD_MODNAME, dev_name(&pdev->dev)); 959 960 /* Create our media pads */ 961 csi2rx->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; 962 csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK; 963 for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) 964 csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE; 965 csi2rx->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE | 966 V4L2_SUBDEV_FL_STREAMS; 967 csi2rx->subdev.entity.ops = &csi2rx_media_ops; 968 969 ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX, 970 csi2rx->pads); 971 if (ret) 972 goto err_cleanup; 973 974 csi2rx->error_irq = platform_get_irq_byname_optional(pdev, "error_irq"); 975 976 if (csi2rx->error_irq < 0) { 977 dev_dbg(csi2rx->dev, "Optional interrupt not defined, proceeding without it\n"); 978 } else { 979 ret = devm_request_irq(csi2rx->dev, csi2rx->error_irq, 980 csi2rx_irq_handler, 0, 981 dev_name(&pdev->dev), csi2rx); 982 if (ret) { 983 dev_err(csi2rx->dev, 984 "Unable to request interrupt: %d\n", ret); 985 goto err_cleanup; 986 } 987 } 988 989 ret = v4l2_subdev_init_finalize(&csi2rx->subdev); 990 if (ret) 991 goto err_cleanup; 992 993 pm_runtime_enable(csi2rx->dev); 994 ret = v4l2_async_register_subdev(&csi2rx->subdev); 995 if (ret < 0) 996 goto err_free_state; 997 998 dev_info(&pdev->dev, 999 "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n", 1000 csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams, 1001 csi2rx->dphy ? "external" : 1002 csi2rx->has_internal_dphy ? "internal" : "no"); 1003 1004 return 0; 1005 1006 err_free_state: 1007 v4l2_subdev_cleanup(&csi2rx->subdev); 1008 pm_runtime_disable(csi2rx->dev); 1009 err_cleanup: 1010 v4l2_async_nf_unregister(&csi2rx->notifier); 1011 v4l2_async_nf_cleanup(&csi2rx->notifier); 1012 media_entity_cleanup(&csi2rx->subdev.entity); 1013 err_free_priv: 1014 kfree(csi2rx); 1015 return ret; 1016 } 1017 1018 static void csi2rx_remove(struct platform_device *pdev) 1019 { 1020 struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev); 1021 1022 v4l2_async_nf_unregister(&csi2rx->notifier); 1023 v4l2_async_nf_cleanup(&csi2rx->notifier); 1024 v4l2_async_unregister_subdev(&csi2rx->subdev); 1025 v4l2_subdev_cleanup(&csi2rx->subdev); 1026 media_entity_cleanup(&csi2rx->subdev.entity); 1027 pm_runtime_disable(csi2rx->dev); 1028 kfree(csi2rx); 1029 } 1030 1031 static int csi2rx_runtime_suspend(struct device *dev) 1032 { 1033 struct csi2rx_priv *csi2rx = dev_get_drvdata(dev); 1034 1035 reset_control_assert(csi2rx->sys_rst); 1036 clk_disable_unprepare(csi2rx->sys_clk); 1037 1038 for (unsigned int i = 0; i < csi2rx->max_streams; i++) { 1039 reset_control_assert(csi2rx->pixel_rst[i]); 1040 clk_disable_unprepare(csi2rx->pixel_clk[i]); 1041 } 1042 1043 reset_control_assert(csi2rx->p_rst); 1044 clk_disable_unprepare(csi2rx->p_clk); 1045 1046 return 0; 1047 } 1048 1049 static int csi2rx_runtime_resume(struct device *dev) 1050 { 1051 struct csi2rx_priv *csi2rx = dev_get_drvdata(dev); 1052 unsigned int i; 1053 int ret; 1054 1055 ret = clk_prepare_enable(csi2rx->p_clk); 1056 if (ret) 1057 return ret; 1058 1059 reset_control_deassert(csi2rx->p_rst); 1060 1061 for (i = 0; i < csi2rx->max_streams; i++) { 1062 ret = clk_prepare_enable(csi2rx->pixel_clk[i]); 1063 if (ret) 1064 goto err_disable_pixclk; 1065 1066 reset_control_deassert(csi2rx->pixel_rst[i]); 1067 } 1068 1069 ret = clk_prepare_enable(csi2rx->sys_clk); 1070 if (ret) 1071 goto err_disable_pixclk; 1072 1073 reset_control_deassert(csi2rx->sys_rst); 1074 1075 return 0; 1076 1077 err_disable_pixclk: 1078 while (i--) { 1079 reset_control_assert(csi2rx->pixel_rst[i]); 1080 clk_disable_unprepare(csi2rx->pixel_clk[i]); 1081 } 1082 1083 reset_control_assert(csi2rx->p_rst); 1084 clk_disable_unprepare(csi2rx->p_clk); 1085 1086 return ret; 1087 } 1088 1089 static const struct dev_pm_ops csi2rx_pm_ops = { 1090 RUNTIME_PM_OPS(csi2rx_runtime_suspend, csi2rx_runtime_resume, NULL) 1091 }; 1092 1093 static const struct of_device_id csi2rx_of_table[] = { 1094 { .compatible = "starfive,jh7110-csi2rx" }, 1095 { .compatible = "cdns,csi2rx" }, 1096 { }, 1097 }; 1098 MODULE_DEVICE_TABLE(of, csi2rx_of_table); 1099 1100 static struct platform_driver csi2rx_driver = { 1101 .probe = csi2rx_probe, 1102 .remove = csi2rx_remove, 1103 1104 .driver = { 1105 .name = "cdns-csi2rx", 1106 .of_match_table = csi2rx_of_table, 1107 .pm = &csi2rx_pm_ops, 1108 }, 1109 }; 1110 module_platform_driver(csi2rx_driver); 1111 MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>"); 1112 MODULE_DESCRIPTION("Cadence CSI2-RX controller"); 1113 MODULE_LICENSE("GPL"); 1114