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