diff options
Diffstat (limited to 'drivers/media/i2c/st-mipid02.c')
| -rw-r--r-- | drivers/media/i2c/st-mipid02.c | 1127 |
1 files changed, 1127 insertions, 0 deletions
diff --git a/drivers/media/i2c/st-mipid02.c b/drivers/media/i2c/st-mipid02.c new file mode 100644 index 0000000000..dab1478711 --- /dev/null +++ b/drivers/media/i2c/st-mipid02.c @@ -0,0 +1,1127 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Driver for ST MIPID02 CSI-2 to PARALLEL bridge + * + * Copyright (C) STMicroelectronics SA 2019 + * Authors: Mickael Guene <mickael.guene@st.com> + * for STMicroelectronics. + * + * + */ + +#include <linux/clk.h> +#include <linux/delay.h> +#include <linux/gpio/consumer.h> +#include <linux/i2c.h> +#include <linux/module.h> +#include <linux/of_graph.h> +#include <linux/regulator/consumer.h> +#include <media/v4l2-async.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-fwnode.h> +#include <media/v4l2-subdev.h> + +#define MIPID02_CLK_LANE_WR_REG1 0x01 +#define MIPID02_CLK_LANE_REG1 0x02 +#define MIPID02_CLK_LANE_REG3 0x04 +#define MIPID02_DATA_LANE0_REG1 0x05 +#define MIPID02_DATA_LANE0_REG2 0x06 +#define MIPID02_DATA_LANE1_REG1 0x09 +#define MIPID02_DATA_LANE1_REG2 0x0a +#define MIPID02_MODE_REG1 0x14 +#define MIPID02_MODE_REG2 0x15 +#define MIPID02_DATA_ID_RREG 0x17 +#define MIPID02_DATA_SELECTION_CTRL 0x19 +#define MIPID02_PIX_WIDTH_CTRL 0x1e +#define MIPID02_PIX_WIDTH_CTRL_EMB 0x1f + +/* Bits definition for MIPID02_CLK_LANE_REG1 */ +#define CLK_ENABLE BIT(0) +/* Bits definition for MIPID02_CLK_LANE_REG3 */ +#define CLK_MIPI_CSI BIT(1) +/* Bits definition for MIPID02_DATA_LANE0_REG1 */ +#define DATA_ENABLE BIT(0) +/* Bits definition for MIPID02_DATA_LANEx_REG2 */ +#define DATA_MIPI_CSI BIT(0) +/* Bits definition for MIPID02_MODE_REG1 */ +#define MODE_DATA_SWAP BIT(2) +#define MODE_NO_BYPASS BIT(6) +/* Bits definition for MIPID02_MODE_REG2 */ +#define MODE_HSYNC_ACTIVE_HIGH BIT(1) +#define MODE_VSYNC_ACTIVE_HIGH BIT(2) +#define MODE_PCLK_SAMPLE_RISING BIT(3) +/* Bits definition for MIPID02_DATA_SELECTION_CTRL */ +#define SELECTION_MANUAL_DATA BIT(2) +#define SELECTION_MANUAL_WIDTH BIT(3) + +static const u32 mipid02_supported_fmt_codes[] = { + MEDIA_BUS_FMT_SBGGR8_1X8, MEDIA_BUS_FMT_SGBRG8_1X8, + MEDIA_BUS_FMT_SGRBG8_1X8, MEDIA_BUS_FMT_SRGGB8_1X8, + MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SGBRG10_1X10, + MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SRGGB10_1X10, + MEDIA_BUS_FMT_SBGGR12_1X12, MEDIA_BUS_FMT_SGBRG12_1X12, + MEDIA_BUS_FMT_SGRBG12_1X12, MEDIA_BUS_FMT_SRGGB12_1X12, + MEDIA_BUS_FMT_YUYV8_1X16, MEDIA_BUS_FMT_YVYU8_1X16, + MEDIA_BUS_FMT_UYVY8_1X16, MEDIA_BUS_FMT_VYUY8_1X16, + MEDIA_BUS_FMT_RGB565_1X16, MEDIA_BUS_FMT_BGR888_1X24, + MEDIA_BUS_FMT_RGB565_2X8_LE, MEDIA_BUS_FMT_RGB565_2X8_BE, + MEDIA_BUS_FMT_YUYV8_2X8, MEDIA_BUS_FMT_YVYU8_2X8, + MEDIA_BUS_FMT_UYVY8_2X8, MEDIA_BUS_FMT_VYUY8_2X8, + MEDIA_BUS_FMT_JPEG_1X8 +}; + +/* regulator supplies */ +static const char * const mipid02_supply_name[] = { + "VDDE", /* 1.8V digital I/O supply */ + "VDDIN", /* 1V8 voltage regulator supply */ +}; + +#define MIPID02_NUM_SUPPLIES ARRAY_SIZE(mipid02_supply_name) + +#define MIPID02_SINK_0 0 +#define MIPID02_SINK_1 1 +#define MIPID02_SOURCE 2 +#define MIPID02_PAD_NB 3 + +struct mipid02_dev { + struct i2c_client *i2c_client; + struct regulator_bulk_data supplies[MIPID02_NUM_SUPPLIES]; + struct v4l2_subdev sd; + struct media_pad pad[MIPID02_PAD_NB]; + struct clk *xclk; + struct gpio_desc *reset_gpio; + /* endpoints info */ + struct v4l2_fwnode_endpoint rx; + u64 link_frequency; + struct v4l2_fwnode_endpoint tx; + /* remote source */ + struct v4l2_async_notifier notifier; + struct v4l2_subdev *s_subdev; + /* registers */ + struct { + u8 clk_lane_reg1; + u8 data_lane0_reg1; + u8 data_lane1_reg1; + u8 mode_reg1; + u8 mode_reg2; + u8 data_selection_ctrl; + u8 data_id_rreg; + u8 pix_width_ctrl; + u8 pix_width_ctrl_emb; + } r; + /* lock to protect all members below */ + struct mutex lock; + bool streaming; + struct v4l2_mbus_framefmt fmt; +}; + +static int bpp_from_code(__u32 code) +{ + switch (code) { + case MEDIA_BUS_FMT_SBGGR8_1X8: + case MEDIA_BUS_FMT_SGBRG8_1X8: + case MEDIA_BUS_FMT_SGRBG8_1X8: + case MEDIA_BUS_FMT_SRGGB8_1X8: + return 8; + case MEDIA_BUS_FMT_SBGGR10_1X10: + case MEDIA_BUS_FMT_SGBRG10_1X10: + case MEDIA_BUS_FMT_SGRBG10_1X10: + case MEDIA_BUS_FMT_SRGGB10_1X10: + return 10; + case MEDIA_BUS_FMT_SBGGR12_1X12: + case MEDIA_BUS_FMT_SGBRG12_1X12: + case MEDIA_BUS_FMT_SGRBG12_1X12: + case MEDIA_BUS_FMT_SRGGB12_1X12: + return 12; + case MEDIA_BUS_FMT_YUYV8_1X16: + case MEDIA_BUS_FMT_YVYU8_1X16: + case MEDIA_BUS_FMT_UYVY8_1X16: + case MEDIA_BUS_FMT_VYUY8_1X16: + case MEDIA_BUS_FMT_RGB565_1X16: + case MEDIA_BUS_FMT_YUYV8_2X8: + case MEDIA_BUS_FMT_YVYU8_2X8: + case MEDIA_BUS_FMT_UYVY8_2X8: + case MEDIA_BUS_FMT_VYUY8_2X8: + case MEDIA_BUS_FMT_RGB565_2X8_LE: + case MEDIA_BUS_FMT_RGB565_2X8_BE: + return 16; + case MEDIA_BUS_FMT_BGR888_1X24: + return 24; + default: + return 0; + } +} + +static u8 data_type_from_code(__u32 code) +{ + switch (code) { + case MEDIA_BUS_FMT_SBGGR8_1X8: + case MEDIA_BUS_FMT_SGBRG8_1X8: + case MEDIA_BUS_FMT_SGRBG8_1X8: + case MEDIA_BUS_FMT_SRGGB8_1X8: + return 0x2a; + case MEDIA_BUS_FMT_SBGGR10_1X10: + case MEDIA_BUS_FMT_SGBRG10_1X10: + case MEDIA_BUS_FMT_SGRBG10_1X10: + case MEDIA_BUS_FMT_SRGGB10_1X10: + return 0x2b; + case MEDIA_BUS_FMT_SBGGR12_1X12: + case MEDIA_BUS_FMT_SGBRG12_1X12: + case MEDIA_BUS_FMT_SGRBG12_1X12: + case MEDIA_BUS_FMT_SRGGB12_1X12: + return 0x2c; + case MEDIA_BUS_FMT_YUYV8_1X16: + case MEDIA_BUS_FMT_YVYU8_1X16: + case MEDIA_BUS_FMT_UYVY8_1X16: + case MEDIA_BUS_FMT_VYUY8_1X16: + case MEDIA_BUS_FMT_YUYV8_2X8: + case MEDIA_BUS_FMT_YVYU8_2X8: + case MEDIA_BUS_FMT_UYVY8_2X8: + case MEDIA_BUS_FMT_VYUY8_2X8: + return 0x1e; + case MEDIA_BUS_FMT_BGR888_1X24: + return 0x24; + case MEDIA_BUS_FMT_RGB565_1X16: + case MEDIA_BUS_FMT_RGB565_2X8_LE: + case MEDIA_BUS_FMT_RGB565_2X8_BE: + return 0x22; + default: + return 0; + } +} + +static void init_format(struct v4l2_mbus_framefmt *fmt) +{ + fmt->code = MEDIA_BUS_FMT_SBGGR8_1X8; + fmt->field = V4L2_FIELD_NONE; + fmt->colorspace = V4L2_COLORSPACE_SRGB; + fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(V4L2_COLORSPACE_SRGB); + fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE; + fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(V4L2_COLORSPACE_SRGB); + fmt->width = 640; + fmt->height = 480; +} + +static __u32 get_fmt_code(__u32 code) +{ + unsigned int i; + + for (i = 0; i < ARRAY_SIZE(mipid02_supported_fmt_codes); i++) { + if (code == mipid02_supported_fmt_codes[i]) + return code; + } + + return mipid02_supported_fmt_codes[0]; +} + +static __u32 serial_to_parallel_code(__u32 serial) +{ + if (serial == MEDIA_BUS_FMT_RGB565_1X16) + return MEDIA_BUS_FMT_RGB565_2X8_LE; + if (serial == MEDIA_BUS_FMT_YUYV8_1X16) + return MEDIA_BUS_FMT_YUYV8_2X8; + if (serial == MEDIA_BUS_FMT_YVYU8_1X16) + return MEDIA_BUS_FMT_YVYU8_2X8; + if (serial == MEDIA_BUS_FMT_UYVY8_1X16) + return MEDIA_BUS_FMT_UYVY8_2X8; + if (serial == MEDIA_BUS_FMT_VYUY8_1X16) + return MEDIA_BUS_FMT_VYUY8_2X8; + if (serial == MEDIA_BUS_FMT_BGR888_1X24) + return MEDIA_BUS_FMT_BGR888_3X8; + + return serial; +} + +static inline struct mipid02_dev *to_mipid02_dev(struct v4l2_subdev *sd) +{ + return container_of(sd, struct mipid02_dev, sd); +} + +static int mipid02_read_reg(struct mipid02_dev *bridge, u16 reg, u8 *val) +{ + struct i2c_client *client = bridge->i2c_client; + struct i2c_msg msg[2]; + u8 buf[2]; + int ret; + + buf[0] = reg >> 8; + buf[1] = reg & 0xff; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].buf = buf; + msg[0].len = sizeof(buf); + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].buf = val; + msg[1].len = 1; + + ret = i2c_transfer(client->adapter, msg, 2); + if (ret < 0) { + dev_dbg(&client->dev, "%s: %x i2c_transfer, reg: %x => %d\n", + __func__, client->addr, reg, ret); + return ret; + } + + return 0; +} + +static int mipid02_write_reg(struct mipid02_dev *bridge, u16 reg, u8 val) +{ + struct i2c_client *client = bridge->i2c_client; + struct i2c_msg msg; + u8 buf[3]; + int ret; + + buf[0] = reg >> 8; + buf[1] = reg & 0xff; + buf[2] = val; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.buf = buf; + msg.len = sizeof(buf); + + ret = i2c_transfer(client->adapter, &msg, 1); + if (ret < 0) { + dev_dbg(&client->dev, "%s: i2c_transfer, reg: %x => %d\n", + __func__, reg, ret); + return ret; + } + + return 0; +} + +static int mipid02_get_regulators(struct mipid02_dev *bridge) +{ + unsigned int i; + + for (i = 0; i < MIPID02_NUM_SUPPLIES; i++) + bridge->supplies[i].supply = mipid02_supply_name[i]; + + return devm_regulator_bulk_get(&bridge->i2c_client->dev, + MIPID02_NUM_SUPPLIES, + bridge->supplies); +} + +static void mipid02_apply_reset(struct mipid02_dev *bridge) +{ + gpiod_set_value_cansleep(bridge->reset_gpio, 0); + usleep_range(5000, 10000); + gpiod_set_value_cansleep(bridge->reset_gpio, 1); + usleep_range(5000, 10000); + gpiod_set_value_cansleep(bridge->reset_gpio, 0); + usleep_range(5000, 10000); +} + +static int mipid02_set_power_on(struct mipid02_dev *bridge) +{ + struct i2c_client *client = bridge->i2c_client; + int ret; + + ret = clk_prepare_enable(bridge->xclk); + if (ret) { + dev_err(&client->dev, "%s: failed to enable clock\n", __func__); + return ret; + } + + ret = regulator_bulk_enable(MIPID02_NUM_SUPPLIES, + bridge->supplies); + if (ret) { + dev_err(&client->dev, "%s: failed to enable regulators\n", + __func__); + goto xclk_off; + } + + if (bridge->reset_gpio) { + dev_dbg(&client->dev, "apply reset"); + mipid02_apply_reset(bridge); + } else { + dev_dbg(&client->dev, "don't apply reset"); + usleep_range(5000, 10000); + } + + return 0; + +xclk_off: + clk_disable_unprepare(bridge->xclk); + return ret; +} + +static void mipid02_set_power_off(struct mipid02_dev *bridge) +{ + regulator_bulk_disable(MIPID02_NUM_SUPPLIES, bridge->supplies); + clk_disable_unprepare(bridge->xclk); +} + +static int mipid02_detect(struct mipid02_dev *bridge) +{ + u8 reg; + + /* + * There is no version registers. Just try to read register + * MIPID02_CLK_LANE_WR_REG1. + */ + return mipid02_read_reg(bridge, MIPID02_CLK_LANE_WR_REG1, ®); +} + +static u32 mipid02_get_link_freq_from_cid_link_freq(struct mipid02_dev *bridge, + struct v4l2_subdev *subdev) +{ + struct v4l2_querymenu qm = {.id = V4L2_CID_LINK_FREQ, }; + struct v4l2_ctrl *ctrl; + int ret; + + ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_LINK_FREQ); + if (!ctrl) + return 0; + qm.index = v4l2_ctrl_g_ctrl(ctrl); + + ret = v4l2_querymenu(subdev->ctrl_handler, &qm); + if (ret) + return 0; + + return qm.value; +} + +static u32 mipid02_get_link_freq_from_cid_pixel_rate(struct mipid02_dev *bridge, + struct v4l2_subdev *subdev) +{ + struct v4l2_fwnode_endpoint *ep = &bridge->rx; + struct v4l2_ctrl *ctrl; + u32 pixel_clock; + u32 bpp = bpp_from_code(bridge->fmt.code); + + ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE); + if (!ctrl) + return 0; + pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl); + + return pixel_clock * bpp / (2 * ep->bus.mipi_csi2.num_data_lanes); +} + +/* + * We need to know link frequency to setup clk_lane_reg1 timings. Link frequency + * will be computed using connected device V4L2_CID_PIXEL_RATE, bit per pixel + * and number of lanes. + */ +static int mipid02_configure_from_rx_speed(struct mipid02_dev *bridge) +{ + struct i2c_client *client = bridge->i2c_client; + struct v4l2_subdev *subdev = bridge->s_subdev; + u32 link_freq; + + link_freq = mipid02_get_link_freq_from_cid_link_freq(bridge, subdev); + if (!link_freq) { + link_freq = mipid02_get_link_freq_from_cid_pixel_rate(bridge, + subdev); + if (!link_freq) { + dev_err(&client->dev, "Failed to get link frequency"); + return -EINVAL; + } + } + + dev_dbg(&client->dev, "detect link_freq = %d Hz", link_freq); + bridge->r.clk_lane_reg1 |= (2000000000 / link_freq) << 2; + + return 0; +} + +static int mipid02_configure_clk_lane(struct mipid02_dev *bridge) +{ + struct i2c_client *client = bridge->i2c_client; + struct v4l2_fwnode_endpoint *ep = &bridge->rx; + bool *polarities = ep->bus.mipi_csi2.lane_polarities; + + /* midid02 doesn't support clock lane remapping */ + if (ep->bus.mipi_csi2.clock_lane != 0) { + dev_err(&client->dev, "clk lane must be map to lane 0\n"); + return -EINVAL; + } + bridge->r.clk_lane_reg1 |= (polarities[0] << 1) | CLK_ENABLE; + + return 0; +} + +static int mipid02_configure_data0_lane(struct mipid02_dev *bridge, int nb, + bool are_lanes_swap, bool *polarities) +{ + bool are_pin_swap = are_lanes_swap ? polarities[2] : polarities[1]; + + if (nb == 1 && are_lanes_swap) + return 0; + + /* + * data lane 0 as pin swap polarity reversed compared to clock and + * data lane 1 + */ + if (!are_pin_swap) + bridge->r.data_lane0_reg1 = 1 << 1; + bridge->r.data_lane0_reg1 |= DATA_ENABLE; + + return 0; +} + +static int mipid02_configure_data1_lane(struct mipid02_dev *bridge, int nb, + bool are_lanes_swap, bool *polarities) +{ + bool are_pin_swap = are_lanes_swap ? polarities[1] : polarities[2]; + + if (nb == 1 && !are_lanes_swap) + return 0; + + if (are_pin_swap) + bridge->r.data_lane1_reg1 = 1 << 1; + bridge->r.data_lane1_reg1 |= DATA_ENABLE; + + return 0; +} + +static int mipid02_configure_from_rx(struct mipid02_dev *bridge) +{ + struct v4l2_fwnode_endpoint *ep = &bridge->rx; + bool are_lanes_swap = ep->bus.mipi_csi2.data_lanes[0] == 2; + bool *polarities = ep->bus.mipi_csi2.lane_polarities; + int nb = ep->bus.mipi_csi2.num_data_lanes; + int ret; + + ret = mipid02_configure_clk_lane(bridge); + if (ret) + return ret; + + ret = mipid02_configure_data0_lane(bridge, nb, are_lanes_swap, + polarities); + if (ret) + return ret; + + ret = mipid02_configure_data1_lane(bridge, nb, are_lanes_swap, + polarities); + if (ret) + return ret; + + bridge->r.mode_reg1 |= are_lanes_swap ? MODE_DATA_SWAP : 0; + bridge->r.mode_reg1 |= (nb - 1) << 1; + + return mipid02_configure_from_rx_speed(bridge); +} + +static int mipid02_configure_from_tx(struct mipid02_dev *bridge) +{ + struct v4l2_fwnode_endpoint *ep = &bridge->tx; + + bridge->r.data_selection_ctrl = SELECTION_MANUAL_WIDTH; + bridge->r.pix_width_ctrl = ep->bus.parallel.bus_width; + bridge->r.pix_width_ctrl_emb = ep->bus.parallel.bus_width; + if (ep->bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) + bridge->r.mode_reg2 |= MODE_HSYNC_ACTIVE_HIGH; + if (ep->bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) + bridge->r.mode_reg2 |= MODE_VSYNC_ACTIVE_HIGH; + if (ep->bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_RISING) + bridge->r.mode_reg2 |= MODE_PCLK_SAMPLE_RISING; + + return 0; +} + +static int mipid02_configure_from_code(struct mipid02_dev *bridge) +{ + u8 data_type; + + bridge->r.data_id_rreg = 0; + + if (bridge->fmt.code != MEDIA_BUS_FMT_JPEG_1X8) { + bridge->r.data_selection_ctrl |= SELECTION_MANUAL_DATA; + + data_type = data_type_from_code(bridge->fmt.code); + if (!data_type) + return -EINVAL; + bridge->r.data_id_rreg = data_type; + } + + return 0; +} + +static int mipid02_stream_disable(struct mipid02_dev *bridge) +{ + struct i2c_client *client = bridge->i2c_client; + int ret = -EINVAL; + + if (!bridge->s_subdev) + goto error; + + ret = v4l2_subdev_call(bridge->s_subdev, video, s_stream, 0); + if (ret) + goto error; + + /* Disable all lanes */ + ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG1, 0); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG1, 0); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG1, 0); + if (ret) + goto error; +error: + if (ret) + dev_err(&client->dev, "failed to stream off %d", ret); + + return ret; +} + +static int mipid02_stream_enable(struct mipid02_dev *bridge) +{ + struct i2c_client *client = bridge->i2c_client; + int ret = -EINVAL; + + if (!bridge->s_subdev) + goto error; + + memset(&bridge->r, 0, sizeof(bridge->r)); + /* build registers content */ + ret = mipid02_configure_from_rx(bridge); + if (ret) + goto error; + ret = mipid02_configure_from_tx(bridge); + if (ret) + goto error; + ret = mipid02_configure_from_code(bridge); + if (ret) + goto error; + + /* write mipi registers */ + ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG1, + bridge->r.clk_lane_reg1); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG3, CLK_MIPI_CSI); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG1, + bridge->r.data_lane0_reg1); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG2, + DATA_MIPI_CSI); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG1, + bridge->r.data_lane1_reg1); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG2, + DATA_MIPI_CSI); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_MODE_REG1, + MODE_NO_BYPASS | bridge->r.mode_reg1); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_MODE_REG2, + bridge->r.mode_reg2); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_DATA_ID_RREG, + bridge->r.data_id_rreg); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_DATA_SELECTION_CTRL, + bridge->r.data_selection_ctrl); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_PIX_WIDTH_CTRL, + bridge->r.pix_width_ctrl); + if (ret) + goto error; + ret = mipid02_write_reg(bridge, MIPID02_PIX_WIDTH_CTRL_EMB, + bridge->r.pix_width_ctrl_emb); + if (ret) + goto error; + + ret = v4l2_subdev_call(bridge->s_subdev, video, s_stream, 1); + if (ret) + goto error; + + return 0; + +error: + dev_err(&client->dev, "failed to stream on %d", ret); + mipid02_stream_disable(bridge); + + return ret; +} + +static int mipid02_s_stream(struct v4l2_subdev *sd, int enable) +{ + struct mipid02_dev *bridge = to_mipid02_dev(sd); + struct i2c_client *client = bridge->i2c_client; + int ret = 0; + + dev_dbg(&client->dev, "%s : requested %d / current = %d", __func__, + enable, bridge->streaming); + mutex_lock(&bridge->lock); + + if (bridge->streaming == enable) + goto out; + + ret = enable ? mipid02_stream_enable(bridge) : + mipid02_stream_disable(bridge); + if (!ret) + bridge->streaming = enable; + +out: + dev_dbg(&client->dev, "%s current now = %d / %d", __func__, + bridge->streaming, ret); + mutex_unlock(&bridge->lock); + + return ret; +} + +static int mipid02_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state, + struct v4l2_subdev_mbus_code_enum *code) +{ + struct mipid02_dev *bridge = to_mipid02_dev(sd); + int ret = 0; + + switch (code->pad) { + case MIPID02_SINK_0: + if (code->index >= ARRAY_SIZE(mipid02_supported_fmt_codes)) + ret = -EINVAL; + else + code->code = mipid02_supported_fmt_codes[code->index]; + break; + case MIPID02_SOURCE: + if (code->index == 0) + code->code = serial_to_parallel_code(bridge->fmt.code); + else + ret = -EINVAL; + break; + default: + ret = -EINVAL; + } + + return ret; +} + +static int mipid02_get_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state, + struct v4l2_subdev_format *format) +{ + struct v4l2_mbus_framefmt *mbus_fmt = &format->format; + struct mipid02_dev *bridge = to_mipid02_dev(sd); + struct i2c_client *client = bridge->i2c_client; + struct v4l2_mbus_framefmt *fmt; + + dev_dbg(&client->dev, "%s probe %d", __func__, format->pad); + + if (format->pad >= MIPID02_PAD_NB) + return -EINVAL; + /* second CSI-2 pad not yet supported */ + if (format->pad == MIPID02_SINK_1) + return -EINVAL; + + if (format->which == V4L2_SUBDEV_FORMAT_TRY) + fmt = v4l2_subdev_get_try_format(&bridge->sd, sd_state, + format->pad); + else + fmt = &bridge->fmt; + + mutex_lock(&bridge->lock); + + *mbus_fmt = *fmt; + /* code may need to be converted for source */ + if (format->pad == MIPID02_SOURCE) + mbus_fmt->code = serial_to_parallel_code(mbus_fmt->code); + + mutex_unlock(&bridge->lock); + + return 0; +} + +static void mipid02_set_fmt_source(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state, + struct v4l2_subdev_format *format) +{ + struct mipid02_dev *bridge = to_mipid02_dev(sd); + + /* source pad mirror sink pad */ + if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) + format->format = bridge->fmt; + else + format->format = *v4l2_subdev_get_try_format(sd, sd_state, + MIPID02_SINK_0); + + /* but code may need to be converted */ + format->format.code = serial_to_parallel_code(format->format.code); + + /* only apply format for V4L2_SUBDEV_FORMAT_TRY case */ + if (format->which != V4L2_SUBDEV_FORMAT_TRY) + return; + + *v4l2_subdev_get_try_format(sd, sd_state, MIPID02_SOURCE) = + format->format; +} + +static void mipid02_set_fmt_sink(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state, + struct v4l2_subdev_format *format) +{ + struct mipid02_dev *bridge = to_mipid02_dev(sd); + struct v4l2_subdev_format source_fmt; + struct v4l2_mbus_framefmt *fmt; + + format->format.code = get_fmt_code(format->format.code); + + if (format->which == V4L2_SUBDEV_FORMAT_TRY) + fmt = v4l2_subdev_get_try_format(sd, sd_state, format->pad); + else + fmt = &bridge->fmt; + + *fmt = format->format; + + /* + * Propagate the format change to the source pad, taking + * care not to update the format pointer given back to user + */ + source_fmt = *format; + mipid02_set_fmt_source(sd, sd_state, &source_fmt); +} + +static int mipid02_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_state *sd_state, + struct v4l2_subdev_format *format) +{ + struct mipid02_dev *bridge = to_mipid02_dev(sd); + struct i2c_client *client = bridge->i2c_client; + int ret = 0; + + dev_dbg(&client->dev, "%s for %d", __func__, format->pad); + + if (format->pad >= MIPID02_PAD_NB) + return -EINVAL; + /* second CSI-2 pad not yet supported */ + if (format->pad == MIPID02_SINK_1) + return -EINVAL; + + mutex_lock(&bridge->lock); + + if (bridge->streaming) { + ret = -EBUSY; + goto error; + } + + if (format->pad == MIPID02_SOURCE) + mipid02_set_fmt_source(sd, sd_state, format); + else + mipid02_set_fmt_sink(sd, sd_state, format); + +error: + mutex_unlock(&bridge->lock); + + return ret; +} + +static const struct v4l2_subdev_video_ops mipid02_video_ops = { + .s_stream = mipid02_s_stream, +}; + +static const struct v4l2_subdev_pad_ops mipid02_pad_ops = { + .enum_mbus_code = mipid02_enum_mbus_code, + .get_fmt = mipid02_get_fmt, + .set_fmt = mipid02_set_fmt, +}; + +static const struct v4l2_subdev_ops mipid02_subdev_ops = { + .video = &mipid02_video_ops, + .pad = &mipid02_pad_ops, +}; + +static const struct media_entity_operations mipid02_subdev_entity_ops = { + .link_validate = v4l2_subdev_link_validate, +}; + +static int mipid02_async_bound(struct v4l2_async_notifier *notifier, + struct v4l2_subdev *s_subdev, + struct v4l2_async_connection *asd) +{ + struct mipid02_dev *bridge = to_mipid02_dev(notifier->sd); + struct i2c_client *client = bridge->i2c_client; + int source_pad; + int ret; + + dev_dbg(&client->dev, "sensor_async_bound call %p", s_subdev); + + source_pad = media_entity_get_fwnode_pad(&s_subdev->entity, + s_subdev->fwnode, + MEDIA_PAD_FL_SOURCE); + if (source_pad < 0) { + dev_err(&client->dev, "Couldn't find output pad for subdev %s\n", + s_subdev->name); + return source_pad; + } + + ret = media_create_pad_link(&s_subdev->entity, source_pad, + &bridge->sd.entity, 0, + MEDIA_LNK_FL_ENABLED | + MEDIA_LNK_FL_IMMUTABLE); + if (ret) { + dev_err(&client->dev, "Couldn't create media link %d", ret); + return ret; + } + + bridge->s_subdev = s_subdev; + + return 0; +} + +static void mipid02_async_unbind(struct v4l2_async_notifier *notifier, + struct v4l2_subdev *s_subdev, + struct v4l2_async_connection *asd) +{ + struct mipid02_dev *bridge = to_mipid02_dev(notifier->sd); + + bridge->s_subdev = NULL; +} + +static const struct v4l2_async_notifier_operations mipid02_notifier_ops = { + .bound = mipid02_async_bound, + .unbind = mipid02_async_unbind, +}; + +static int mipid02_parse_rx_ep(struct mipid02_dev *bridge) +{ + struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY }; + struct i2c_client *client = bridge->i2c_client; + struct v4l2_async_connection *asd; + struct device_node *ep_node; + int ret; + + /* parse rx (endpoint 0) */ + ep_node = of_graph_get_endpoint_by_regs(bridge->i2c_client->dev.of_node, + 0, 0); + if (!ep_node) { + dev_err(&client->dev, "unable to find port0 ep"); + ret = -EINVAL; + goto error; + } + + ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep); + if (ret) { + dev_err(&client->dev, "Could not parse v4l2 endpoint %d\n", + ret); + goto error_of_node_put; + } + + /* do some sanity checks */ + if (ep.bus.mipi_csi2.num_data_lanes > 2) { + dev_err(&client->dev, "max supported data lanes is 2 / got %d", + ep.bus.mipi_csi2.num_data_lanes); + ret = -EINVAL; + goto error_of_node_put; + } + + /* register it for later use */ + bridge->rx = ep; + + /* register async notifier so we get noticed when sensor is connected */ + v4l2_async_subdev_nf_init(&bridge->notifier, &bridge->sd); + asd = v4l2_async_nf_add_fwnode_remote(&bridge->notifier, + of_fwnode_handle(ep_node), + struct v4l2_async_connection); + of_node_put(ep_node); + + if (IS_ERR(asd)) { + dev_err(&client->dev, "fail to register asd to notifier %ld", + PTR_ERR(asd)); + return PTR_ERR(asd); + } + bridge->notifier.ops = &mipid02_notifier_ops; + + ret = v4l2_async_nf_register(&bridge->notifier); + if (ret) + v4l2_async_nf_cleanup(&bridge->notifier); + + return ret; + +error_of_node_put: + of_node_put(ep_node); +error: + + return ret; +} + +static int mipid02_parse_tx_ep(struct mipid02_dev *bridge) +{ + struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_PARALLEL }; + struct i2c_client *client = bridge->i2c_client; + struct device_node *ep_node; + int ret; + + /* parse tx (endpoint 2) */ + ep_node = of_graph_get_endpoint_by_regs(bridge->i2c_client->dev.of_node, + 2, 0); + if (!ep_node) { + dev_err(&client->dev, "unable to find port1 ep"); + ret = -EINVAL; + goto error; + } + + ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep); + if (ret) { + dev_err(&client->dev, "Could not parse v4l2 endpoint\n"); + goto error_of_node_put; + } + + of_node_put(ep_node); + bridge->tx = ep; + + return 0; + +error_of_node_put: + of_node_put(ep_node); +error: + + return -EINVAL; +} + +static int mipid02_probe(struct i2c_client *client) +{ + struct device *dev = &client->dev; + struct mipid02_dev *bridge; + u32 clk_freq; + int ret; + + bridge = devm_kzalloc(dev, sizeof(*bridge), GFP_KERNEL); + if (!bridge) + return -ENOMEM; + + init_format(&bridge->fmt); + + bridge->i2c_client = client; + v4l2_i2c_subdev_init(&bridge->sd, client, &mipid02_subdev_ops); + + /* got and check clock */ + bridge->xclk = devm_clk_get(dev, "xclk"); + if (IS_ERR(bridge->xclk)) { + dev_err(dev, "failed to get xclk\n"); + return PTR_ERR(bridge->xclk); + } + + clk_freq = clk_get_rate(bridge->xclk); + if (clk_freq < 6000000 || clk_freq > 27000000) { + dev_err(dev, "xclk freq must be in 6-27 Mhz range. got %d Hz\n", + clk_freq); + return -EINVAL; + } + + bridge->reset_gpio = devm_gpiod_get_optional(dev, "reset", + GPIOD_OUT_HIGH); + + if (IS_ERR(bridge->reset_gpio)) { + dev_err(dev, "failed to get reset GPIO\n"); + return PTR_ERR(bridge->reset_gpio); + } + + ret = mipid02_get_regulators(bridge); + if (ret) { + dev_err(dev, "failed to get regulators %d", ret); + return ret; + } + + mutex_init(&bridge->lock); + bridge->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; + bridge->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; + bridge->sd.entity.ops = &mipid02_subdev_entity_ops; + bridge->pad[0].flags = MEDIA_PAD_FL_SINK; + bridge->pad[1].flags = MEDIA_PAD_FL_SINK; + bridge->pad[2].flags = MEDIA_PAD_FL_SOURCE; + ret = media_entity_pads_init(&bridge->sd.entity, MIPID02_PAD_NB, + bridge->pad); + if (ret) { + dev_err(&client->dev, "pads init failed %d", ret); + goto mutex_cleanup; + } + + /* enable clock, power and reset device if available */ + ret = mipid02_set_power_on(bridge); + if (ret) + goto entity_cleanup; + + ret = mipid02_detect(bridge); + if (ret) { + dev_err(&client->dev, "failed to detect mipid02 %d", ret); + goto power_off; + } + + ret = mipid02_parse_tx_ep(bridge); + if (ret) { + dev_err(&client->dev, "failed to parse tx %d", ret); + goto power_off; + } + + ret = mipid02_parse_rx_ep(bridge); + if (ret) { + dev_err(&client->dev, "failed to parse rx %d", ret); + goto power_off; + } + + ret = v4l2_async_register_subdev(&bridge->sd); + if (ret < 0) { + dev_err(&client->dev, "v4l2_async_register_subdev failed %d", + ret); + goto unregister_notifier; + } + + dev_info(&client->dev, "mipid02 device probe successfully"); + + return 0; + +unregister_notifier: + v4l2_async_nf_unregister(&bridge->notifier); + v4l2_async_nf_cleanup(&bridge->notifier); +power_off: + mipid02_set_power_off(bridge); +entity_cleanup: + media_entity_cleanup(&bridge->sd.entity); +mutex_cleanup: + mutex_destroy(&bridge->lock); + + return ret; +} + +static void mipid02_remove(struct i2c_client *client) +{ + struct v4l2_subdev *sd = i2c_get_clientdata(client); + struct mipid02_dev *bridge = to_mipid02_dev(sd); + + v4l2_async_nf_unregister(&bridge->notifier); + v4l2_async_nf_cleanup(&bridge->notifier); + v4l2_async_unregister_subdev(&bridge->sd); + mipid02_set_power_off(bridge); + media_entity_cleanup(&bridge->sd.entity); + mutex_destroy(&bridge->lock); +} + +static const struct of_device_id mipid02_dt_ids[] = { + { .compatible = "st,st-mipid02" }, + { /* sentinel */ } +}; +MODULE_DEVICE_TABLE(of, mipid02_dt_ids); + +static struct i2c_driver mipid02_i2c_driver = { + .driver = { + .name = "st-mipid02", + .of_match_table = mipid02_dt_ids, + }, + .probe = mipid02_probe, + .remove = mipid02_remove, +}; + +module_i2c_driver(mipid02_i2c_driver); + +MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics MIPID02 CSI-2 bridge driver"); +MODULE_LICENSE("GPL v2"); |