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