drivers/media/i2c/st-mipid02.c

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