1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Driver for ST MIPID02 CSI-2 to PARALLEL bridge 4 * 5 * Copyright (C) STMicroelectronics SA 2019 6 * Authors: Mickael Guene <mickael.guene@st.com> 7 * for STMicroelectronics. 8 * 9 * 10 */ 11 12 #include <linux/clk.h> 13 #include <linux/delay.h> 14 #include <linux/gpio/consumer.h> 15 #include <linux/i2c.h> 16 #include <linux/module.h> 17 #include <linux/of_graph.h> 18 #include <linux/regulator/consumer.h> 19 #include <media/v4l2-async.h> 20 #include <media/v4l2-ctrls.h> 21 #include <media/v4l2-device.h> 22 #include <media/v4l2-fwnode.h> 23 #include <media/v4l2-subdev.h> 24 25 #define MIPID02_CLK_LANE_WR_REG1 0x01 26 #define MIPID02_CLK_LANE_REG1 0x02 27 #define MIPID02_CLK_LANE_REG3 0x04 28 #define MIPID02_DATA_LANE0_REG1 0x05 29 #define MIPID02_DATA_LANE0_REG2 0x06 30 #define MIPID02_DATA_LANE1_REG1 0x09 31 #define MIPID02_DATA_LANE1_REG2 0x0a 32 #define MIPID02_MODE_REG1 0x14 33 #define MIPID02_MODE_REG2 0x15 34 #define MIPID02_DATA_ID_RREG 0x17 35 #define MIPID02_DATA_SELECTION_CTRL 0x19 36 #define MIPID02_PIX_WIDTH_CTRL 0x1e 37 #define MIPID02_PIX_WIDTH_CTRL_EMB 0x1f 38 39 /* Bits definition for MIPID02_CLK_LANE_REG1 */ 40 #define CLK_ENABLE BIT(0) 41 /* Bits definition for MIPID02_CLK_LANE_REG3 */ 42 #define CLK_MIPI_CSI BIT(1) 43 /* Bits definition for MIPID02_DATA_LANE0_REG1 */ 44 #define DATA_ENABLE BIT(0) 45 /* Bits definition for MIPID02_DATA_LANEx_REG2 */ 46 #define DATA_MIPI_CSI BIT(0) 47 /* Bits definition for MIPID02_MODE_REG1 */ 48 #define MODE_DATA_SWAP BIT(2) 49 #define MODE_NO_BYPASS BIT(6) 50 /* Bits definition for MIPID02_MODE_REG2 */ 51 #define MODE_HSYNC_ACTIVE_HIGH BIT(1) 52 #define MODE_VSYNC_ACTIVE_HIGH BIT(2) 53 /* Bits definition for MIPID02_DATA_SELECTION_CTRL */ 54 #define SELECTION_MANUAL_DATA BIT(2) 55 #define SELECTION_MANUAL_WIDTH BIT(3) 56 57 static const u32 mipid02_supported_fmt_codes[] = { 58 MEDIA_BUS_FMT_SBGGR8_1X8, MEDIA_BUS_FMT_SGBRG8_1X8, 59 MEDIA_BUS_FMT_SGRBG8_1X8, MEDIA_BUS_FMT_SRGGB8_1X8, 60 MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SGBRG10_1X10, 61 MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SRGGB10_1X10, 62 MEDIA_BUS_FMT_SBGGR12_1X12, MEDIA_BUS_FMT_SGBRG12_1X12, 63 MEDIA_BUS_FMT_SGRBG12_1X12, MEDIA_BUS_FMT_SRGGB12_1X12, 64 MEDIA_BUS_FMT_UYVY8_1X16, MEDIA_BUS_FMT_BGR888_1X24 65 }; 66 67 /* regulator supplies */ 68 static const char * const mipid02_supply_name[] = { 69 "VDDE", /* 1.8V digital I/O supply */ 70 "VDDIN", /* 1V8 voltage regulator supply */ 71 }; 72 73 #define MIPID02_NUM_SUPPLIES ARRAY_SIZE(mipid02_supply_name) 74 75 #define MIPID02_SINK_0 0 76 #define MIPID02_SINK_1 1 77 #define MIPID02_SOURCE 2 78 #define MIPID02_PAD_NB 3 79 80 struct mipid02_dev { 81 struct i2c_client *i2c_client; 82 struct regulator_bulk_data supplies[MIPID02_NUM_SUPPLIES]; 83 struct v4l2_subdev sd; 84 struct media_pad pad[MIPID02_PAD_NB]; 85 struct clk *xclk; 86 struct gpio_desc *reset_gpio; 87 /* endpoints info */ 88 struct v4l2_fwnode_endpoint rx; 89 u64 link_frequency; 90 struct v4l2_fwnode_endpoint tx; 91 /* remote source */ 92 struct v4l2_async_subdev asd; 93 struct v4l2_async_notifier notifier; 94 struct v4l2_subdev *s_subdev; 95 /* registers */ 96 struct { 97 u8 clk_lane_reg1; 98 u8 data_lane0_reg1; 99 u8 data_lane1_reg1; 100 u8 mode_reg1; 101 u8 mode_reg2; 102 u8 data_id_rreg; 103 u8 pix_width_ctrl; 104 u8 pix_width_ctrl_emb; 105 } r; 106 /* lock to protect all members below */ 107 struct mutex lock; 108 bool streaming; 109 struct v4l2_mbus_framefmt fmt; 110 }; 111 112 static int bpp_from_code(__u32 code) 113 { 114 switch (code) { 115 case MEDIA_BUS_FMT_SBGGR8_1X8: 116 case MEDIA_BUS_FMT_SGBRG8_1X8: 117 case MEDIA_BUS_FMT_SGRBG8_1X8: 118 case MEDIA_BUS_FMT_SRGGB8_1X8: 119 return 8; 120 case MEDIA_BUS_FMT_SBGGR10_1X10: 121 case MEDIA_BUS_FMT_SGBRG10_1X10: 122 case MEDIA_BUS_FMT_SGRBG10_1X10: 123 case MEDIA_BUS_FMT_SRGGB10_1X10: 124 return 10; 125 case MEDIA_BUS_FMT_SBGGR12_1X12: 126 case MEDIA_BUS_FMT_SGBRG12_1X12: 127 case MEDIA_BUS_FMT_SGRBG12_1X12: 128 case MEDIA_BUS_FMT_SRGGB12_1X12: 129 return 12; 130 case MEDIA_BUS_FMT_UYVY8_1X16: 131 return 16; 132 case MEDIA_BUS_FMT_BGR888_1X24: 133 return 24; 134 default: 135 return 0; 136 } 137 } 138 139 static u8 data_type_from_code(__u32 code) 140 { 141 switch (code) { 142 case MEDIA_BUS_FMT_SBGGR8_1X8: 143 case MEDIA_BUS_FMT_SGBRG8_1X8: 144 case MEDIA_BUS_FMT_SGRBG8_1X8: 145 case MEDIA_BUS_FMT_SRGGB8_1X8: 146 return 0x2a; 147 case MEDIA_BUS_FMT_SBGGR10_1X10: 148 case MEDIA_BUS_FMT_SGBRG10_1X10: 149 case MEDIA_BUS_FMT_SGRBG10_1X10: 150 case MEDIA_BUS_FMT_SRGGB10_1X10: 151 return 0x2b; 152 case MEDIA_BUS_FMT_SBGGR12_1X12: 153 case MEDIA_BUS_FMT_SGBRG12_1X12: 154 case MEDIA_BUS_FMT_SGRBG12_1X12: 155 case MEDIA_BUS_FMT_SRGGB12_1X12: 156 return 0x2c; 157 case MEDIA_BUS_FMT_UYVY8_1X16: 158 return 0x1e; 159 case MEDIA_BUS_FMT_BGR888_1X24: 160 return 0x24; 161 default: 162 return 0; 163 } 164 } 165 166 static void init_format(struct v4l2_mbus_framefmt *fmt) 167 { 168 fmt->code = MEDIA_BUS_FMT_SBGGR8_1X8; 169 fmt->field = V4L2_FIELD_NONE; 170 fmt->colorspace = V4L2_COLORSPACE_SRGB; 171 fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(V4L2_COLORSPACE_SRGB); 172 fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE; 173 fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(V4L2_COLORSPACE_SRGB); 174 fmt->width = 640; 175 fmt->height = 480; 176 } 177 178 static __u32 get_fmt_code(__u32 code) 179 { 180 unsigned int i; 181 182 for (i = 0; i < ARRAY_SIZE(mipid02_supported_fmt_codes); i++) { 183 if (code == mipid02_supported_fmt_codes[i]) 184 return code; 185 } 186 187 return mipid02_supported_fmt_codes[0]; 188 } 189 190 static __u32 serial_to_parallel_code(__u32 serial) 191 { 192 if (serial == MEDIA_BUS_FMT_UYVY8_1X16) 193 return MEDIA_BUS_FMT_UYVY8_2X8; 194 if (serial == MEDIA_BUS_FMT_BGR888_1X24) 195 return MEDIA_BUS_FMT_BGR888_3X8; 196 197 return serial; 198 } 199 200 static inline struct mipid02_dev *to_mipid02_dev(struct v4l2_subdev *sd) 201 { 202 return container_of(sd, struct mipid02_dev, sd); 203 } 204 205 static int mipid02_read_reg(struct mipid02_dev *bridge, u16 reg, u8 *val) 206 { 207 struct i2c_client *client = bridge->i2c_client; 208 struct i2c_msg msg[2]; 209 u8 buf[2]; 210 int ret; 211 212 buf[0] = reg >> 8; 213 buf[1] = reg & 0xff; 214 215 msg[0].addr = client->addr; 216 msg[0].flags = client->flags; 217 msg[0].buf = buf; 218 msg[0].len = sizeof(buf); 219 220 msg[1].addr = client->addr; 221 msg[1].flags = client->flags | I2C_M_RD; 222 msg[1].buf = val; 223 msg[1].len = 1; 224 225 ret = i2c_transfer(client->adapter, msg, 2); 226 if (ret < 0) { 227 dev_dbg(&client->dev, "%s: %x i2c_transfer, reg: %x => %d\n", 228 __func__, client->addr, reg, ret); 229 return ret; 230 } 231 232 return 0; 233 } 234 235 static int mipid02_write_reg(struct mipid02_dev *bridge, u16 reg, u8 val) 236 { 237 struct i2c_client *client = bridge->i2c_client; 238 struct i2c_msg msg; 239 u8 buf[3]; 240 int ret; 241 242 buf[0] = reg >> 8; 243 buf[1] = reg & 0xff; 244 buf[2] = val; 245 246 msg.addr = client->addr; 247 msg.flags = client->flags; 248 msg.buf = buf; 249 msg.len = sizeof(buf); 250 251 ret = i2c_transfer(client->adapter, &msg, 1); 252 if (ret < 0) { 253 dev_dbg(&client->dev, "%s: i2c_transfer, reg: %x => %d\n", 254 __func__, reg, ret); 255 return ret; 256 } 257 258 return 0; 259 } 260 261 static int mipid02_get_regulators(struct mipid02_dev *bridge) 262 { 263 unsigned int i; 264 265 for (i = 0; i < MIPID02_NUM_SUPPLIES; i++) 266 bridge->supplies[i].supply = mipid02_supply_name[i]; 267 268 return devm_regulator_bulk_get(&bridge->i2c_client->dev, 269 MIPID02_NUM_SUPPLIES, 270 bridge->supplies); 271 } 272 273 static void mipid02_apply_reset(struct mipid02_dev *bridge) 274 { 275 gpiod_set_value_cansleep(bridge->reset_gpio, 0); 276 usleep_range(5000, 10000); 277 gpiod_set_value_cansleep(bridge->reset_gpio, 1); 278 usleep_range(5000, 10000); 279 gpiod_set_value_cansleep(bridge->reset_gpio, 0); 280 usleep_range(5000, 10000); 281 } 282 283 static int mipid02_set_power_on(struct mipid02_dev *bridge) 284 { 285 struct i2c_client *client = bridge->i2c_client; 286 int ret; 287 288 ret = clk_prepare_enable(bridge->xclk); 289 if (ret) { 290 dev_err(&client->dev, "%s: failed to enable clock\n", __func__); 291 return ret; 292 } 293 294 ret = regulator_bulk_enable(MIPID02_NUM_SUPPLIES, 295 bridge->supplies); 296 if (ret) { 297 dev_err(&client->dev, "%s: failed to enable regulators\n", 298 __func__); 299 goto xclk_off; 300 } 301 302 if (bridge->reset_gpio) { 303 dev_dbg(&client->dev, "apply reset"); 304 mipid02_apply_reset(bridge); 305 } else { 306 dev_dbg(&client->dev, "don't apply reset"); 307 usleep_range(5000, 10000); 308 } 309 310 return 0; 311 312 xclk_off: 313 clk_disable_unprepare(bridge->xclk); 314 return ret; 315 } 316 317 static void mipid02_set_power_off(struct mipid02_dev *bridge) 318 { 319 regulator_bulk_disable(MIPID02_NUM_SUPPLIES, bridge->supplies); 320 clk_disable_unprepare(bridge->xclk); 321 } 322 323 static int mipid02_detect(struct mipid02_dev *bridge) 324 { 325 u8 reg; 326 327 /* 328 * There is no version registers. Just try to read register 329 * MIPID02_CLK_LANE_WR_REG1. 330 */ 331 return mipid02_read_reg(bridge, MIPID02_CLK_LANE_WR_REG1, ®); 332 } 333 334 static u32 mipid02_get_link_freq_from_cid_pixel_rate(struct mipid02_dev *bridge, 335 struct v4l2_subdev *subdev) 336 { 337 struct v4l2_fwnode_endpoint *ep = &bridge->rx; 338 struct v4l2_ctrl *ctrl; 339 u32 pixel_clock; 340 u32 bpp = bpp_from_code(bridge->fmt.code); 341 342 ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE); 343 if (!ctrl) 344 return 0; 345 pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl); 346 347 return pixel_clock * bpp / (2 * ep->bus.mipi_csi2.num_data_lanes); 348 } 349 350 /* 351 * We need to know link frequency to setup clk_lane_reg1 timings. Link frequency 352 * will be computed using connected device V4L2_CID_PIXEL_RATE, bit per pixel 353 * and number of lanes. 354 */ 355 static int mipid02_configure_from_rx_speed(struct mipid02_dev *bridge) 356 { 357 struct i2c_client *client = bridge->i2c_client; 358 struct v4l2_subdev *subdev = bridge->s_subdev; 359 u32 link_freq; 360 361 link_freq = mipid02_get_link_freq_from_cid_pixel_rate(bridge, subdev); 362 if (!link_freq) { 363 dev_err(&client->dev, "Failed to detect link frequency"); 364 return -EINVAL; 365 } 366 367 dev_dbg(&client->dev, "detect link_freq = %d Hz", link_freq); 368 bridge->r.clk_lane_reg1 |= (2000000000 / link_freq) << 2; 369 370 return 0; 371 } 372 373 static int mipid02_configure_clk_lane(struct mipid02_dev *bridge) 374 { 375 struct i2c_client *client = bridge->i2c_client; 376 struct v4l2_fwnode_endpoint *ep = &bridge->rx; 377 bool *polarities = ep->bus.mipi_csi2.lane_polarities; 378 379 /* midid02 doesn't support clock lane remapping */ 380 if (ep->bus.mipi_csi2.clock_lane != 0) { 381 dev_err(&client->dev, "clk lane must be map to lane 0\n"); 382 return -EINVAL; 383 } 384 bridge->r.clk_lane_reg1 |= (polarities[0] << 1) | CLK_ENABLE; 385 386 return 0; 387 } 388 389 static int mipid02_configure_data0_lane(struct mipid02_dev *bridge, int nb, 390 bool are_lanes_swap, bool *polarities) 391 { 392 bool are_pin_swap = are_lanes_swap ? polarities[2] : polarities[1]; 393 394 if (nb == 1 && are_lanes_swap) 395 return 0; 396 397 /* 398 * data lane 0 as pin swap polarity reversed compared to clock and 399 * data lane 1 400 */ 401 if (!are_pin_swap) 402 bridge->r.data_lane0_reg1 = 1 << 1; 403 bridge->r.data_lane0_reg1 |= DATA_ENABLE; 404 405 return 0; 406 } 407 408 static int mipid02_configure_data1_lane(struct mipid02_dev *bridge, int nb, 409 bool are_lanes_swap, bool *polarities) 410 { 411 bool are_pin_swap = are_lanes_swap ? polarities[1] : polarities[2]; 412 413 if (nb == 1 && !are_lanes_swap) 414 return 0; 415 416 if (are_pin_swap) 417 bridge->r.data_lane1_reg1 = 1 << 1; 418 bridge->r.data_lane1_reg1 |= DATA_ENABLE; 419 420 return 0; 421 } 422 423 static int mipid02_configure_from_rx(struct mipid02_dev *bridge) 424 { 425 struct v4l2_fwnode_endpoint *ep = &bridge->rx; 426 bool are_lanes_swap = ep->bus.mipi_csi2.data_lanes[0] == 2; 427 bool *polarities = ep->bus.mipi_csi2.lane_polarities; 428 int nb = ep->bus.mipi_csi2.num_data_lanes; 429 int ret; 430 431 ret = mipid02_configure_clk_lane(bridge); 432 if (ret) 433 return ret; 434 435 ret = mipid02_configure_data0_lane(bridge, nb, are_lanes_swap, 436 polarities); 437 if (ret) 438 return ret; 439 440 ret = mipid02_configure_data1_lane(bridge, nb, are_lanes_swap, 441 polarities); 442 if (ret) 443 return ret; 444 445 bridge->r.mode_reg1 |= are_lanes_swap ? MODE_DATA_SWAP : 0; 446 bridge->r.mode_reg1 |= (nb - 1) << 1; 447 448 return mipid02_configure_from_rx_speed(bridge); 449 } 450 451 static int mipid02_configure_from_tx(struct mipid02_dev *bridge) 452 { 453 struct v4l2_fwnode_endpoint *ep = &bridge->tx; 454 455 bridge->r.pix_width_ctrl = ep->bus.parallel.bus_width; 456 bridge->r.pix_width_ctrl_emb = ep->bus.parallel.bus_width; 457 if (ep->bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) 458 bridge->r.mode_reg2 |= MODE_HSYNC_ACTIVE_HIGH; 459 if (ep->bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) 460 bridge->r.mode_reg2 |= MODE_VSYNC_ACTIVE_HIGH; 461 462 return 0; 463 } 464 465 static int mipid02_configure_from_code(struct mipid02_dev *bridge) 466 { 467 u8 data_type; 468 469 bridge->r.data_id_rreg = 0; 470 data_type = data_type_from_code(bridge->fmt.code); 471 if (!data_type) 472 return -EINVAL; 473 bridge->r.data_id_rreg = data_type; 474 475 return 0; 476 } 477 478 static int mipid02_stream_disable(struct mipid02_dev *bridge) 479 { 480 struct i2c_client *client = bridge->i2c_client; 481 int ret; 482 483 /* Disable all lanes */ 484 ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG1, 0); 485 if (ret) 486 goto error; 487 ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG1, 0); 488 if (ret) 489 goto error; 490 ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG1, 0); 491 if (ret) 492 goto error; 493 error: 494 if (ret) 495 dev_err(&client->dev, "failed to stream off %d", ret); 496 497 return ret; 498 } 499 500 static int mipid02_stream_enable(struct mipid02_dev *bridge) 501 { 502 struct i2c_client *client = bridge->i2c_client; 503 int ret = -EINVAL; 504 505 if (!bridge->s_subdev) 506 goto error; 507 508 memset(&bridge->r, 0, sizeof(bridge->r)); 509 /* build registers content */ 510 ret = mipid02_configure_from_rx(bridge); 511 if (ret) 512 goto error; 513 ret = mipid02_configure_from_tx(bridge); 514 if (ret) 515 goto error; 516 ret = mipid02_configure_from_code(bridge); 517 if (ret) 518 goto error; 519 520 /* write mipi registers */ 521 ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG1, 522 bridge->r.clk_lane_reg1); 523 if (ret) 524 goto error; 525 ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG3, CLK_MIPI_CSI); 526 if (ret) 527 goto error; 528 ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG1, 529 bridge->r.data_lane0_reg1); 530 if (ret) 531 goto error; 532 ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG2, 533 DATA_MIPI_CSI); 534 if (ret) 535 goto error; 536 ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG1, 537 bridge->r.data_lane1_reg1); 538 if (ret) 539 goto error; 540 ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG2, 541 DATA_MIPI_CSI); 542 if (ret) 543 goto error; 544 ret = mipid02_write_reg(bridge, MIPID02_MODE_REG1, 545 MODE_NO_BYPASS | bridge->r.mode_reg1); 546 if (ret) 547 goto error; 548 ret = mipid02_write_reg(bridge, MIPID02_MODE_REG2, 549 bridge->r.mode_reg2); 550 if (ret) 551 goto error; 552 ret = mipid02_write_reg(bridge, MIPID02_DATA_ID_RREG, 553 bridge->r.data_id_rreg); 554 if (ret) 555 goto error; 556 ret = mipid02_write_reg(bridge, MIPID02_DATA_SELECTION_CTRL, 557 SELECTION_MANUAL_DATA | SELECTION_MANUAL_WIDTH); 558 if (ret) 559 goto error; 560 ret = mipid02_write_reg(bridge, MIPID02_PIX_WIDTH_CTRL, 561 bridge->r.pix_width_ctrl); 562 if (ret) 563 goto error; 564 ret = mipid02_write_reg(bridge, MIPID02_PIX_WIDTH_CTRL_EMB, 565 bridge->r.pix_width_ctrl_emb); 566 if (ret) 567 goto error; 568 569 return 0; 570 571 error: 572 dev_err(&client->dev, "failed to stream on %d", ret); 573 mipid02_stream_disable(bridge); 574 575 return ret; 576 } 577 578 static int mipid02_s_stream(struct v4l2_subdev *sd, int enable) 579 { 580 struct mipid02_dev *bridge = to_mipid02_dev(sd); 581 struct i2c_client *client = bridge->i2c_client; 582 int ret = 0; 583 584 dev_dbg(&client->dev, "%s : requested %d / current = %d", __func__, 585 enable, bridge->streaming); 586 mutex_lock(&bridge->lock); 587 588 if (bridge->streaming == enable) 589 goto out; 590 591 ret = enable ? mipid02_stream_enable(bridge) : 592 mipid02_stream_disable(bridge); 593 if (!ret) 594 bridge->streaming = enable; 595 596 out: 597 dev_dbg(&client->dev, "%s current now = %d / %d", __func__, 598 bridge->streaming, ret); 599 mutex_unlock(&bridge->lock); 600 601 return ret; 602 } 603 604 static int mipid02_enum_mbus_code(struct v4l2_subdev *sd, 605 struct v4l2_subdev_pad_config *cfg, 606 struct v4l2_subdev_mbus_code_enum *code) 607 { 608 struct mipid02_dev *bridge = to_mipid02_dev(sd); 609 int ret = 0; 610 611 switch (code->pad) { 612 case MIPID02_SINK_0: 613 if (code->index >= ARRAY_SIZE(mipid02_supported_fmt_codes)) 614 ret = -EINVAL; 615 else 616 code->code = mipid02_supported_fmt_codes[code->index]; 617 break; 618 case MIPID02_SOURCE: 619 if (code->index == 0) 620 code->code = serial_to_parallel_code(bridge->fmt.code); 621 else 622 ret = -EINVAL; 623 break; 624 default: 625 ret = -EINVAL; 626 } 627 628 return ret; 629 } 630 631 static int mipid02_get_fmt(struct v4l2_subdev *sd, 632 struct v4l2_subdev_pad_config *cfg, 633 struct v4l2_subdev_format *format) 634 { 635 struct v4l2_mbus_framefmt *mbus_fmt = &format->format; 636 struct mipid02_dev *bridge = to_mipid02_dev(sd); 637 struct i2c_client *client = bridge->i2c_client; 638 struct v4l2_mbus_framefmt *fmt; 639 640 dev_dbg(&client->dev, "%s probe %d", __func__, format->pad); 641 642 if (format->pad >= MIPID02_PAD_NB) 643 return -EINVAL; 644 /* second CSI-2 pad not yet supported */ 645 if (format->pad == MIPID02_SINK_1) 646 return -EINVAL; 647 648 if (format->which == V4L2_SUBDEV_FORMAT_TRY) 649 fmt = v4l2_subdev_get_try_format(&bridge->sd, cfg, format->pad); 650 else 651 fmt = &bridge->fmt; 652 653 mutex_lock(&bridge->lock); 654 655 *mbus_fmt = *fmt; 656 /* code may need to be converted for source */ 657 if (format->pad == MIPID02_SOURCE) 658 mbus_fmt->code = serial_to_parallel_code(mbus_fmt->code); 659 660 mutex_unlock(&bridge->lock); 661 662 return 0; 663 } 664 665 static void mipid02_set_fmt_source(struct v4l2_subdev *sd, 666 struct v4l2_subdev_pad_config *cfg, 667 struct v4l2_subdev_format *format) 668 { 669 struct mipid02_dev *bridge = to_mipid02_dev(sd); 670 671 /* source pad mirror active sink pad */ 672 format->format = bridge->fmt; 673 /* but code may need to be converted */ 674 format->format.code = serial_to_parallel_code(format->format.code); 675 676 /* only apply format for V4L2_SUBDEV_FORMAT_TRY case */ 677 if (format->which != V4L2_SUBDEV_FORMAT_TRY) 678 return; 679 680 *v4l2_subdev_get_try_format(sd, cfg, format->pad) = format->format; 681 } 682 683 static void mipid02_set_fmt_sink(struct v4l2_subdev *sd, 684 struct v4l2_subdev_pad_config *cfg, 685 struct v4l2_subdev_format *format) 686 { 687 struct mipid02_dev *bridge = to_mipid02_dev(sd); 688 struct v4l2_mbus_framefmt *fmt; 689 690 format->format.code = get_fmt_code(format->format.code); 691 692 if (format->which == V4L2_SUBDEV_FORMAT_TRY) 693 fmt = v4l2_subdev_get_try_format(sd, cfg, format->pad); 694 else 695 fmt = &bridge->fmt; 696 697 *fmt = format->format; 698 } 699 700 static int mipid02_set_fmt(struct v4l2_subdev *sd, 701 struct v4l2_subdev_pad_config *cfg, 702 struct v4l2_subdev_format *format) 703 { 704 struct mipid02_dev *bridge = to_mipid02_dev(sd); 705 struct i2c_client *client = bridge->i2c_client; 706 int ret = 0; 707 708 dev_dbg(&client->dev, "%s for %d", __func__, format->pad); 709 710 if (format->pad >= MIPID02_PAD_NB) 711 return -EINVAL; 712 /* second CSI-2 pad not yet supported */ 713 if (format->pad == MIPID02_SINK_1) 714 return -EINVAL; 715 716 mutex_lock(&bridge->lock); 717 718 if (bridge->streaming) { 719 ret = -EBUSY; 720 goto error; 721 } 722 723 if (format->pad == MIPID02_SOURCE) 724 mipid02_set_fmt_source(sd, cfg, format); 725 else 726 mipid02_set_fmt_sink(sd, cfg, format); 727 728 error: 729 mutex_unlock(&bridge->lock); 730 731 return ret; 732 } 733 734 static const struct v4l2_subdev_video_ops mipid02_video_ops = { 735 .s_stream = mipid02_s_stream, 736 }; 737 738 static const struct v4l2_subdev_pad_ops mipid02_pad_ops = { 739 .enum_mbus_code = mipid02_enum_mbus_code, 740 .get_fmt = mipid02_get_fmt, 741 .set_fmt = mipid02_set_fmt, 742 }; 743 744 static const struct v4l2_subdev_ops mipid02_subdev_ops = { 745 .video = &mipid02_video_ops, 746 .pad = &mipid02_pad_ops, 747 }; 748 749 static const struct media_entity_operations mipid02_subdev_entity_ops = { 750 .link_validate = v4l2_subdev_link_validate, 751 }; 752 753 static int mipid02_async_bound(struct v4l2_async_notifier *notifier, 754 struct v4l2_subdev *s_subdev, 755 struct v4l2_async_subdev *asd) 756 { 757 struct mipid02_dev *bridge = to_mipid02_dev(notifier->sd); 758 struct i2c_client *client = bridge->i2c_client; 759 int source_pad; 760 int ret; 761 762 dev_dbg(&client->dev, "sensor_async_bound call %p", s_subdev); 763 764 source_pad = media_entity_get_fwnode_pad(&s_subdev->entity, 765 s_subdev->fwnode, 766 MEDIA_PAD_FL_SOURCE); 767 if (source_pad < 0) { 768 dev_err(&client->dev, "Couldn't find output pad for subdev %s\n", 769 s_subdev->name); 770 return source_pad; 771 } 772 773 ret = media_create_pad_link(&s_subdev->entity, source_pad, 774 &bridge->sd.entity, 0, 775 MEDIA_LNK_FL_ENABLED | 776 MEDIA_LNK_FL_IMMUTABLE); 777 if (ret) { 778 dev_err(&client->dev, "Couldn't create media link %d", ret); 779 return ret; 780 } 781 782 bridge->s_subdev = s_subdev; 783 784 return 0; 785 } 786 787 static void mipid02_async_unbind(struct v4l2_async_notifier *notifier, 788 struct v4l2_subdev *s_subdev, 789 struct v4l2_async_subdev *asd) 790 { 791 struct mipid02_dev *bridge = to_mipid02_dev(notifier->sd); 792 793 bridge->s_subdev = NULL; 794 } 795 796 static const struct v4l2_async_notifier_operations mipid02_notifier_ops = { 797 .bound = mipid02_async_bound, 798 .unbind = mipid02_async_unbind, 799 }; 800 801 static int mipid02_parse_rx_ep(struct mipid02_dev *bridge) 802 { 803 struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY }; 804 struct i2c_client *client = bridge->i2c_client; 805 struct device_node *ep_node; 806 int ret; 807 808 /* parse rx (endpoint 0) */ 809 ep_node = of_graph_get_endpoint_by_regs(bridge->i2c_client->dev.of_node, 810 0, 0); 811 if (!ep_node) { 812 dev_err(&client->dev, "unable to find port0 ep"); 813 ret = -EINVAL; 814 goto error; 815 } 816 817 ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep); 818 if (ret) { 819 dev_err(&client->dev, "Could not parse v4l2 endpoint %d\n", 820 ret); 821 goto error_of_node_put; 822 } 823 824 /* do some sanity checks */ 825 if (ep.bus.mipi_csi2.num_data_lanes > 2) { 826 dev_err(&client->dev, "max supported data lanes is 2 / got %d", 827 ep.bus.mipi_csi2.num_data_lanes); 828 ret = -EINVAL; 829 goto error_of_node_put; 830 } 831 832 /* register it for later use */ 833 bridge->rx = ep; 834 835 /* register async notifier so we get noticed when sensor is connected */ 836 bridge->asd.match.fwnode = 837 fwnode_graph_get_remote_port_parent(of_fwnode_handle(ep_node)); 838 bridge->asd.match_type = V4L2_ASYNC_MATCH_FWNODE; 839 of_node_put(ep_node); 840 841 v4l2_async_notifier_init(&bridge->notifier); 842 ret = v4l2_async_notifier_add_subdev(&bridge->notifier, &bridge->asd); 843 if (ret) { 844 dev_err(&client->dev, "fail to register asd to notifier %d", 845 ret); 846 fwnode_handle_put(bridge->asd.match.fwnode); 847 return ret; 848 } 849 bridge->notifier.ops = &mipid02_notifier_ops; 850 851 ret = v4l2_async_subdev_notifier_register(&bridge->sd, 852 &bridge->notifier); 853 if (ret) 854 v4l2_async_notifier_cleanup(&bridge->notifier); 855 856 return ret; 857 858 error_of_node_put: 859 of_node_put(ep_node); 860 error: 861 862 return ret; 863 } 864 865 static int mipid02_parse_tx_ep(struct mipid02_dev *bridge) 866 { 867 struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_PARALLEL }; 868 struct i2c_client *client = bridge->i2c_client; 869 struct device_node *ep_node; 870 int ret; 871 872 /* parse tx (endpoint 2) */ 873 ep_node = of_graph_get_endpoint_by_regs(bridge->i2c_client->dev.of_node, 874 2, 0); 875 if (!ep_node) { 876 dev_err(&client->dev, "unable to find port1 ep"); 877 ret = -EINVAL; 878 goto error; 879 } 880 881 ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep); 882 if (ret) { 883 dev_err(&client->dev, "Could not parse v4l2 endpoint\n"); 884 goto error_of_node_put; 885 } 886 887 of_node_put(ep_node); 888 bridge->tx = ep; 889 890 return 0; 891 892 error_of_node_put: 893 of_node_put(ep_node); 894 error: 895 896 return -EINVAL; 897 } 898 899 static int mipid02_probe(struct i2c_client *client) 900 { 901 struct device *dev = &client->dev; 902 struct mipid02_dev *bridge; 903 u32 clk_freq; 904 int ret; 905 906 bridge = devm_kzalloc(dev, sizeof(*bridge), GFP_KERNEL); 907 if (!bridge) 908 return -ENOMEM; 909 910 init_format(&bridge->fmt); 911 912 bridge->i2c_client = client; 913 v4l2_i2c_subdev_init(&bridge->sd, client, &mipid02_subdev_ops); 914 915 /* got and check clock */ 916 bridge->xclk = devm_clk_get(dev, "xclk"); 917 if (IS_ERR(bridge->xclk)) { 918 dev_err(dev, "failed to get xclk\n"); 919 return PTR_ERR(bridge->xclk); 920 } 921 922 clk_freq = clk_get_rate(bridge->xclk); 923 if (clk_freq < 6000000 || clk_freq > 27000000) { 924 dev_err(dev, "xclk freq must be in 6-27 Mhz range. got %d Hz\n", 925 clk_freq); 926 return -EINVAL; 927 } 928 929 bridge->reset_gpio = devm_gpiod_get_optional(dev, "reset", 930 GPIOD_OUT_HIGH); 931 932 ret = mipid02_get_regulators(bridge); 933 if (ret) { 934 dev_err(dev, "failed to get regulators %d", ret); 935 return ret; 936 } 937 938 mutex_init(&bridge->lock); 939 bridge->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; 940 bridge->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; 941 bridge->sd.entity.ops = &mipid02_subdev_entity_ops; 942 bridge->pad[0].flags = MEDIA_PAD_FL_SINK; 943 bridge->pad[1].flags = MEDIA_PAD_FL_SINK; 944 bridge->pad[2].flags = MEDIA_PAD_FL_SOURCE; 945 ret = media_entity_pads_init(&bridge->sd.entity, MIPID02_PAD_NB, 946 bridge->pad); 947 if (ret) { 948 dev_err(&client->dev, "pads init failed %d", ret); 949 goto mutex_cleanup; 950 } 951 952 /* enable clock, power and reset device if available */ 953 ret = mipid02_set_power_on(bridge); 954 if (ret) 955 goto entity_cleanup; 956 957 ret = mipid02_detect(bridge); 958 if (ret) { 959 dev_err(&client->dev, "failed to detect mipid02 %d", ret); 960 goto power_off; 961 } 962 963 ret = mipid02_parse_tx_ep(bridge); 964 if (ret) { 965 dev_err(&client->dev, "failed to parse tx %d", ret); 966 goto power_off; 967 } 968 969 ret = mipid02_parse_rx_ep(bridge); 970 if (ret) { 971 dev_err(&client->dev, "failed to parse rx %d", ret); 972 goto power_off; 973 } 974 975 ret = v4l2_async_register_subdev(&bridge->sd); 976 if (ret < 0) { 977 dev_err(&client->dev, "v4l2_async_register_subdev failed %d", 978 ret); 979 goto unregister_notifier; 980 } 981 982 dev_info(&client->dev, "mipid02 device probe successfully"); 983 984 return 0; 985 986 unregister_notifier: 987 v4l2_async_notifier_unregister(&bridge->notifier); 988 v4l2_async_notifier_cleanup(&bridge->notifier); 989 power_off: 990 mipid02_set_power_off(bridge); 991 entity_cleanup: 992 media_entity_cleanup(&bridge->sd.entity); 993 mutex_cleanup: 994 mutex_destroy(&bridge->lock); 995 996 return ret; 997 } 998 999 static int mipid02_remove(struct i2c_client *client) 1000 { 1001 struct v4l2_subdev *sd = i2c_get_clientdata(client); 1002 struct mipid02_dev *bridge = to_mipid02_dev(sd); 1003 1004 v4l2_async_notifier_unregister(&bridge->notifier); 1005 v4l2_async_notifier_cleanup(&bridge->notifier); 1006 v4l2_async_unregister_subdev(&bridge->sd); 1007 mipid02_set_power_off(bridge); 1008 media_entity_cleanup(&bridge->sd.entity); 1009 mutex_destroy(&bridge->lock); 1010 1011 return 0; 1012 } 1013 1014 static const struct of_device_id mipid02_dt_ids[] = { 1015 { .compatible = "st,st-mipid02" }, 1016 { /* sentinel */ } 1017 }; 1018 MODULE_DEVICE_TABLE(of, mipid02_dt_ids); 1019 1020 static struct i2c_driver mipid02_i2c_driver = { 1021 .driver = { 1022 .name = "st-mipid02", 1023 .of_match_table = mipid02_dt_ids, 1024 }, 1025 .probe_new = mipid02_probe, 1026 .remove = mipid02_remove, 1027 }; 1028 1029 module_i2c_driver(mipid02_i2c_driver); 1030 1031 MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>"); 1032 MODULE_DESCRIPTION("STMicroelectronics MIPID02 CSI-2 bridge driver"); 1033 MODULE_LICENSE("GPL v2"); 1034