diff --git a/drivers/media/platform/rockchip/cif/capture.c b/drivers/media/platform/rockchip/cif/capture.c index 5b7e269e79bc..fdf8f1334649 100644 --- a/drivers/media/platform/rockchip/cif/capture.c +++ b/drivers/media/platform/rockchip/cif/capture.c @@ -1357,9 +1357,17 @@ static int rkcif_assign_new_buffer_oneframe(struct rkcif_stream *stream, static void rkcif_s_rx_buffer(struct rkcif_device *dev, struct rkisp_rx_buf *dbufs) { - struct media_pad *pad = media_entity_remote_pad(&dev->sditf->pads); + struct media_pad *pad = NULL; struct v4l2_subdev *sd; + if (dev->sditf[0]) { + if (dev->sditf[0]->is_combine_mode) + pad = media_entity_remote_pad(&dev->sditf[0]->pads[1]); + else + pad = media_entity_remote_pad(&dev->sditf[0]->pads[0]); + } else { + return; + } if (pad) sd = media_entity_to_v4l2_subdev(pad->entity); else @@ -2091,6 +2099,8 @@ static int rkcif_csi_channel_init(struct rkcif_stream *stream, channel->crop_st_x = stream->crop[CROP_SRC_ACT].left; channel->crop_st_y = stream->crop[CROP_SRC_ACT].top; + if (dev->sditf_cnt > 1 && dev->sditf_cnt <= RKCIF_MAX_SDITF) + channel->crop_st_y *= dev->sditf_cnt; channel->width = stream->crop[CROP_SRC_ACT].width; channel->height = stream->crop[CROP_SRC_ACT].height; } else { @@ -2099,6 +2109,9 @@ static int rkcif_csi_channel_init(struct rkcif_stream *stream, channel->crop_en = 0; } + if (dev->sditf_cnt > 1 && dev->sditf_cnt <= RKCIF_MAX_SDITF) + channel->height *= dev->sditf_cnt; + fmt = find_output_fmt(stream, stream->pixm.pixelformat); if (!fmt) { v4l2_err(&dev->v4l2_dev, "can not find output format: 0x%x", @@ -2483,7 +2496,7 @@ static int rkcif_csi_channel_set_v1(struct rkcif_stream *stream, unsigned int val = 0x0; struct rkcif_device *dev = stream->cifdev; struct rkcif_stream *detect_stream = &dev->stream[0]; - struct sditf_priv *priv = dev->sditf; + struct sditf_priv *priv = dev->sditf[0]; unsigned int wait_line = 0x3fff; unsigned int dma_en = 0; @@ -3065,10 +3078,19 @@ static int rkcif_create_dummy_buf(struct rkcif_stream *stream) struct rkcif_device *dev = stream->cifdev; struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf; int ret = 0; + u32 height = 0; + + if (stream->crop_enable) + height = stream->crop[CROP_SRC_ACT].height; + else + height = stream->pixm.height; + + if (dev->sditf_cnt > 1 && dev->sditf_cnt <= RKCIF_MAX_SDITF) + height *= dev->sditf_cnt; /* get a maximum plane size */ dummy_buf->size = max3(stream->pixm.plane_fmt[0].bytesperline * - stream->pixm.height, + height, stream->pixm.plane_fmt[1].sizeimage, stream->pixm.plane_fmt[2].sizeimage); /* @@ -3325,6 +3347,16 @@ void rkcif_do_stop_stream(struct rkcif_stream *stream, v4l2_err(v4l2_dev, "pipeline close failed error:%d\n", ret); pm_runtime_put_sync(dev->dev); v4l2_pipeline_pm_put(&node->vdev.entity); + if (dev->sditf_cnt > 1) { + for (i = 0; i < dev->sditf_cnt; i++) + ret |= v4l2_subdev_call(dev->sditf[i]->sensor_sd, + core, + s_power, + 0); + if (ret < 0) + v4l2_err(v4l2_dev, "set power off fail, ret %d\n", + ret); + } if (dev->hdr.hdr_mode == HDR_X2) { if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_READY && dev->stream[RKCIF_STREAM_MIPI_ID1].state == RKCIF_STATE_READY) { @@ -4230,6 +4262,7 @@ int rkcif_do_start_stream(struct rkcif_stream *stream, unsigned int mode) struct rkmodule_hdr_cfg hdr_cfg; int rkmodule_stream_seq = RKMODULE_START_STREAM_DEFAULT; int ret; + int i = 0; v4l2_info(&dev->v4l2_dev, "stream[%d] start streaming\n", stream->id); @@ -4310,6 +4343,16 @@ int rkcif_do_start_stream(struct rkcif_stream *stream, unsigned int mode) ret); goto destroy_buf; } + if (dev->sditf_cnt > 1) { + for (i = 0; i < dev->sditf_cnt; i++) + ret |= v4l2_subdev_call(dev->sditf[i]->sensor_sd, + core, + s_power, + 1); + if (ret < 0) + v4l2_err(v4l2_dev, "set power on fail, ret %d\n", + ret); + } ret = dev->pipe.open(&dev->pipe, &node->vdev.entity, true); if (ret < 0) { v4l2_err(v4l2_dev, "open cif pipeline failed %d\n", @@ -4547,6 +4590,9 @@ int rkcif_set_fmt(struct rkcif_stream *stream, } } + if (dev->sditf_cnt > 1 && dev->sditf_cnt <= RKCIF_MAX_SDITF) + height *= dev->sditf_cnt; + extend_line->pixm.height = height + RKMODULE_EXTEND_LINE; /* compact mode need bytesperline 4bytes align, @@ -7261,7 +7307,7 @@ static void rkcif_modify_line_int(struct rkcif_stream *stream, bool en) static void rkcif_detect_wake_up_mode_change(struct rkcif_stream *stream) { struct rkcif_device *cif_dev = stream->cifdev; - struct sditf_priv *priv = cif_dev->sditf; + struct sditf_priv *priv = cif_dev->sditf[0]; if (!priv || priv->toisp_inf.link_mode != TOISP_NONE) return; @@ -7561,7 +7607,7 @@ void rkcif_irq_handle_toisp(struct rkcif_device *cif_dev, unsigned int intstat_g { int i = 0; bool to_check = false; - struct sditf_priv *priv = cif_dev->sditf; + struct sditf_priv *priv = cif_dev->sditf[0]; if (!priv || priv->toisp_inf.link_mode == TOISP_NONE) return; @@ -8454,7 +8500,7 @@ int rkcif_sditf_disconnect(struct video_device *vdev) struct media_link *link; int ret; - link = list_first_entry(&cifdev->sditf->sd.entity.links, struct media_link, list); + link = list_first_entry(&cifdev->sditf[0]->sd.entity.links, struct media_link, list); ret = media_entity_setup_link(link, 0); if (ret) dev_err(cifdev->dev, "failed to disable link of sditf with isp"); diff --git a/drivers/media/platform/rockchip/cif/dev.c b/drivers/media/platform/rockchip/cif/dev.c index f28808002169..1c7b50a3ef17 100644 --- a/drivers/media/platform/rockchip/cif/dev.c +++ b/drivers/media/platform/rockchip/cif/dev.c @@ -112,7 +112,7 @@ static ssize_t rkcif_store_line_int_num(struct device *dev, const char *buf, size_t len) { struct rkcif_device *cif_dev = (struct rkcif_device *)dev_get_drvdata(dev); - struct sditf_priv *priv = cif_dev->sditf; + struct sditf_priv *priv = cif_dev->sditf[0]; int val = 0; int ret = 0; @@ -987,6 +987,18 @@ static int rkcif_pipeline_set_stream(struct rkcif_pipeline *p, bool on) if (on && ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV) goto err_stream_off; } + + if (cif_dev->sditf_cnt > 1) { + for (i = 0; i < cif_dev->sditf_cnt; i++) { + ret = v4l2_subdev_call(cif_dev->sditf[i]->sensor_sd, + video, + s_stream, + on); + if (on && ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV) + goto err_stream_off; + } + } + if (on) rkcif_set_sensor_streamon_in_sync_mode(cif_dev); } else { @@ -1040,6 +1052,16 @@ static int rkcif_pipeline_set_stream(struct rkcif_pipeline *p, bool on) if (on && ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV) goto err_stream_off; } + if (cif_dev->sditf_cnt > 1) { + for (i = 0; i < cif_dev->sditf_cnt; i++) { + ret = v4l2_subdev_call(cif_dev->sditf[i]->sensor_sd, + video, + s_stream, + on); + if (on && ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV) + goto err_stream_off; + } + } if (on) rkcif_set_sensor_streamon_in_sync_mode(cif_dev); @@ -1231,8 +1253,9 @@ static int _set_pipeline_default_fmt(struct rkcif_device *dev) static int subdev_asyn_register_itf(struct rkcif_device *dev) { - struct sditf_priv *sditf = dev->sditf; + struct sditf_priv *sditf = NULL; int ret = 0; + int i = 0; ret = rkcif_update_sensor_info(&dev->stream[0]); if (ret) { @@ -1240,8 +1263,12 @@ static int subdev_asyn_register_itf(struct rkcif_device *dev) "There is not terminal subdev, not synchronized with ISP\n"); return 0; } - if (sditf) - ret = v4l2_async_register_subdev_sensor_common(&sditf->sd); + + for (i = 0; i < dev->sditf_cnt; i++) { + sditf = dev->sditf[i]; + if (sditf && (!sditf->is_combine_mode)) + ret = v4l2_async_register_subdev_sensor_common(&sditf->sd); + } return ret; } @@ -1700,6 +1727,7 @@ int rkcif_plat_init(struct rkcif_device *cif_dev, struct device_node *node, int cif_dev->isr_hdl = rkcif_irq_handler; cif_dev->id_use_cnt = 0; cif_dev->sync_type = NO_SYNC_MODE; + cif_dev->sditf_cnt = 0; if (cif_dev->chip_id == CHIP_RV1126_CIF_LITE) cif_dev->isr_hdl = rkcif_irq_lite_handler; diff --git a/drivers/media/platform/rockchip/cif/dev.h b/drivers/media/platform/rockchip/cif/dev.h index 26705a7c1980..23998519f964 100644 --- a/drivers/media/platform/rockchip/cif/dev.h +++ b/drivers/media/platform/rockchip/cif/dev.h @@ -578,6 +578,7 @@ static inline struct vb2_queue *to_vb2_queue(struct file *file) #define CIF_SCALE_CH3_VDEV_NAME CIF_DRIVER_NAME "_scale_ch3" #define RKCIF_SCALE_ENUM_SIZE_MAX 3 +#define RKCIF_MAX_SDITF 4 enum scale_ch_sw { SCALE_MIPI0_ID0, @@ -708,7 +709,7 @@ struct rkcif_device { irqreturn_t (*isr_hdl)(int irq, struct rkcif_device *cif_dev); int inf_id; - struct sditf_priv *sditf; + struct sditf_priv *sditf[RKCIF_MAX_SDITF]; struct proc_dir_entry *proc_dir; struct rkcif_irq_stats irq_stats; spinlock_t hdr_lock; /* lock for hdr buf sync */ @@ -728,6 +729,7 @@ struct rkcif_device { bool iommu_en; bool is_use_dummybuf; int sync_type; + int sditf_cnt; }; extern struct platform_driver rkcif_plat_drv; diff --git a/drivers/media/platform/rockchip/cif/subdev-itf.c b/drivers/media/platform/rockchip/cif/subdev-itf.c index 3f23231d4274..a0e8cc631522 100644 --- a/drivers/media/platform/rockchip/cif/subdev-itf.c +++ b/drivers/media/platform/rockchip/cif/subdev-itf.c @@ -82,6 +82,11 @@ static int sditf_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id, if (cif_dev->active_sensor) { sensor_sd = cif_dev->active_sensor->sd; return v4l2_subdev_call(sensor_sd, pad, get_mbus_config, 0, config); + } else { + config->type = V4L2_MBUS_CSI2_DPHY; + config->flags = V4L2_MBUS_CSI2_CHANNEL_0 | + V4L2_MBUS_CSI2_CONTINUOUS_CLOCK; + return 0; } return -EINVAL; @@ -128,7 +133,7 @@ static int sditf_get_set_fmt(struct v4l2_subdev *sd, pixm.height = priv->cap_info.height; v4l2_dbg(3, rkcif_debug, &cif_dev->v4l2_dev, "%s, width %d, height %d, hdr mode %d\n", - __func__, fmt->format.width, fmt->format.width, priv->hdr_cfg.hdr_mode); + __func__, fmt->format.width, fmt->format.height, priv->hdr_cfg.hdr_mode); if (priv->hdr_cfg.hdr_mode == NO_HDR) { rkcif_set_fmt(&cif_dev->stream[0], &pixm, false); } else if (priv->hdr_cfg.hdr_mode == HDR_X2) { @@ -139,6 +144,39 @@ static int sditf_get_set_fmt(struct v4l2_subdev *sd, rkcif_set_fmt(&cif_dev->stream[1], &pixm, false); rkcif_set_fmt(&cif_dev->stream[2], &pixm, false); } + } else { + if (priv->sensor_sd) { + fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE; + fmt->pad = 0; + ret = v4l2_subdev_call(priv->sensor_sd, pad, get_fmt, NULL, fmt); + if (ret) { + v4l2_err(&priv->sd, + "%s: get sensor format failed\n", __func__); + return ret; + } + + input_sel.target = V4L2_SEL_TGT_CROP_BOUNDS; + input_sel.which = V4L2_SUBDEV_FORMAT_ACTIVE; + input_sel.pad = 0; + ret = v4l2_subdev_call(priv->sensor_sd, + pad, get_selection, NULL, + &input_sel); + if (!ret) { + fmt->format.width = input_sel.r.width; + fmt->format.height = input_sel.r.height; + } + priv->cap_info.width = fmt->format.width; + priv->cap_info.height = fmt->format.height; + pixm.pixelformat = rkcif_mbus_pixelcode_to_v4l2(fmt->format.code); + pixm.width = priv->cap_info.width; + pixm.height = priv->cap_info.height; + } else { + fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE; + fmt->pad = 0; + fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10; + fmt->format.width = 640; + fmt->format.height = 480; + } } return 0; @@ -217,6 +255,8 @@ static long sditf_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg) mode = (struct rkisp_vicap_mode *)arg; memcpy(&priv->mode, mode, sizeof(*mode)); sditf_reinit_mode(priv, &priv->mode); + mode->input.merge_num = cif_dev->sditf_cnt; + mode->input.index = priv->combine_index; return 0; case RKISP_VICAP_CMD_INIT_BUF: pbuf_num = (int *)arg; @@ -249,6 +289,7 @@ static long sditf_compat_ioctl32(struct v4l2_subdev *sd, struct rkcif_device *cif_dev = priv->cif_dev; struct v4l2_subdev *sensor_sd; struct rkisp_vicap_mode *mode; + struct rkmodule_hdr_cfg *hdr_cfg; int buf_num; int ret = 0; @@ -271,6 +312,18 @@ static long sditf_compat_ioctl32(struct v4l2_subdev *sd, return -EFAULT; ret = sditf_ioctl(sd, cmd, &buf_num); return ret; + case RKMODULE_GET_HDR_CFG: + hdr_cfg = kzalloc(sizeof(*hdr_cfg), GFP_KERNEL); + if (!hdr_cfg) { + ret = -ENOMEM; + return ret; + } + if (copy_from_user(hdr_cfg, up, sizeof(*hdr_cfg))) { + kfree(hdr_cfg); + return -EFAULT; + } + ret = sditf_ioctl(sd, cmd, hdr_cfg); + return ret; default: break; } @@ -530,8 +583,61 @@ static int rkcif_sditf_attach_cifdev(struct sditf_priv *sditf) return -EINVAL; } - cif_dev->sditf = sditf; + cif_dev->sditf[cif_dev->sditf_cnt] = sditf; sditf->cif_dev = cif_dev; + cif_dev->sditf_cnt++; + + return 0; +} + +struct sensor_async_subdev { + struct v4l2_async_subdev asd; + struct v4l2_mbus_config mbus; + int lanes; +}; + +static int sditf_fwnode_parse(struct device *dev, + struct v4l2_fwnode_endpoint *vep, + struct v4l2_async_subdev *asd) +{ + struct sensor_async_subdev *s_asd = + container_of(asd, struct sensor_async_subdev, asd); + struct v4l2_mbus_config *config = &s_asd->mbus; + + if (vep->base.port != 0) { + dev_err(dev, "sditf has only port 0\n"); + return -EINVAL; + } + + if (vep->bus_type == V4L2_MBUS_CSI2_DPHY || + vep->bus_type == V4L2_MBUS_CSI2_CPHY) { + config->type = vep->bus_type; + config->flags = vep->bus.mipi_csi2.flags; + s_asd->lanes = vep->bus.mipi_csi2.num_data_lanes; + } else if (vep->bus_type == V4L2_MBUS_CCP2) { + config->type = vep->bus_type; + s_asd->lanes = vep->bus.mipi_csi1.data_lane; + } else { + dev_err(dev, "type is not supported\n"); + return -EINVAL; + } + + switch (s_asd->lanes) { + case 1: + config->flags |= V4L2_MBUS_CSI2_1_LANE; + break; + case 2: + config->flags |= V4L2_MBUS_CSI2_2_LANE; + break; + case 3: + config->flags |= V4L2_MBUS_CSI2_3_LANE; + break; + case 4: + config->flags |= V4L2_MBUS_CSI2_4_LANE; + break; + default: + return -EINVAL; + } return 0; } @@ -568,16 +674,108 @@ static const struct v4l2_ctrl_ops rkcif_sditf_ctrl_ops = { .g_volatile_ctrl = rkcif_sditf_get_ctrl, }; +static int sditf_notifier_bound(struct v4l2_async_notifier *notifier, + struct v4l2_subdev *subdev, + struct v4l2_async_subdev *asd) +{ + struct sditf_priv *sditf = container_of(notifier, + struct sditf_priv, notifier); + struct media_entity *source_entity, *sink_entity; + int ret = 0; + + sditf->sensor_sd = subdev; + + if (sditf->num_sensors == 1) { + v4l2_err(subdev, + "%s: the num of subdev is beyond %d\n", + __func__, sditf->num_sensors); + return -EBUSY; + } + + if (sditf->sd.entity.pads[0].flags & MEDIA_PAD_FL_SINK) { + source_entity = &subdev->entity; + sink_entity = &sditf->sd.entity; + + ret = media_create_pad_link(source_entity, + 0, + sink_entity, + 0, + MEDIA_LNK_FL_ENABLED); + if (ret) + v4l2_err(&sditf->sd, "failed to create link for %s\n", + sditf->sensor_sd->name); + } + sditf->sensor_sd = subdev; + ++sditf->num_sensors; + + v4l2_err(subdev, "Async registered subdev\n"); + + return 0; +} + +static void sditf_notifier_unbind(struct v4l2_async_notifier *notifier, + struct v4l2_subdev *sd, + struct v4l2_async_subdev *asd) +{ + struct sditf_priv *sditf = container_of(notifier, + struct sditf_priv, + notifier); + + sditf->sensor_sd = NULL; +} + +static const struct v4l2_async_notifier_operations sditf_notifier_ops = { + .bound = sditf_notifier_bound, + .unbind = sditf_notifier_unbind, +}; + +static int sditf_subdev_notifier(struct sditf_priv *sditf) +{ + struct v4l2_async_notifier *ntf = &sditf->notifier; + int ret; + + v4l2_async_notifier_init(ntf); + + ret = v4l2_async_notifier_parse_fwnode_endpoints_by_port( + sditf->dev, &sditf->notifier, + sizeof(struct sensor_async_subdev), 0, + sditf_fwnode_parse); + if (ret < 0) + return ret; + + sditf->sd.subdev_notifier = &sditf->notifier; + sditf->notifier.ops = &sditf_notifier_ops; + + ret = v4l2_async_subdev_notifier_register(&sditf->sd, &sditf->notifier); + if (ret) { + v4l2_err(&sditf->sd, + "failed to register async notifier : %d\n", + ret); + v4l2_async_notifier_cleanup(&sditf->notifier); + return ret; + } + + return v4l2_async_register_subdev(&sditf->sd); +} + static int rkcif_subdev_media_init(struct sditf_priv *priv) { struct rkcif_device *cif_dev = priv->cif_dev; struct v4l2_ctrl_handler *handler = &priv->ctrl_handler; unsigned long flags = V4L2_CTRL_FLAG_VOLATILE; int ret; + int pad_num = 0; - priv->pads.flags = MEDIA_PAD_FL_SOURCE; + if (priv->is_combine_mode) { + priv->pads[0].flags = MEDIA_PAD_FL_SINK; + priv->pads[1].flags = MEDIA_PAD_FL_SOURCE; + pad_num = 2; + } else { + priv->pads[0].flags = MEDIA_PAD_FL_SOURCE; + pad_num = 1; + } priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_COMPOSER; - ret = media_entity_pads_init(&priv->sd.entity, 1, &priv->pads); + ret = media_entity_pads_init(&priv->sd.entity, pad_num, priv->pads); if (ret < 0) return ret; @@ -604,6 +802,8 @@ static int rkcif_subdev_media_init(struct sditf_priv *priv) priv->toisp_inf.ch_info[0].is_valid = false; priv->toisp_inf.ch_info[1].is_valid = false; priv->toisp_inf.ch_info[2].is_valid = false; + if (priv->is_combine_mode) + sditf_subdev_notifier(priv); return 0; } @@ -612,6 +812,7 @@ static int rkcif_subdev_probe(struct platform_device *pdev) struct device *dev = &pdev->dev; struct v4l2_subdev *sd; struct sditf_priv *priv; + struct device_node *node = dev->of_node; int ret; priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL); @@ -627,7 +828,19 @@ static int rkcif_subdev_probe(struct platform_device *pdev) platform_set_drvdata(pdev, &sd->entity); - rkcif_sditf_attach_cifdev(priv); + ret = rkcif_sditf_attach_cifdev(priv); + if (ret < 0) + return ret; + + ret = of_property_read_u32(node, + "rockchip,combine-index", + &priv->combine_index); + if (ret) { + priv->is_combine_mode = false; + priv->combine_index = 0; + } else { + priv->is_combine_mode = true; + } ret = rkcif_subdev_media_init(priv); if (ret < 0) return ret; diff --git a/drivers/media/platform/rockchip/cif/subdev-itf.h b/drivers/media/platform/rockchip/cif/subdev-itf.h index 893e5ed97e37..e0701ffae824 100644 --- a/drivers/media/platform/rockchip/cif/subdev-itf.h +++ b/drivers/media/platform/rockchip/cif/subdev-itf.h @@ -56,8 +56,9 @@ struct toisp_info { struct sditf_priv { struct device *dev; + struct v4l2_async_notifier notifier; struct v4l2_subdev sd; - struct media_pad pads; + struct media_pad pads[2]; struct rkcif_device *cif_dev; struct rkmodule_hdr_cfg hdr_cfg; struct capture_info cap_info; @@ -65,7 +66,11 @@ struct sditf_priv { struct toisp_info toisp_inf; struct v4l2_ctrl *pixel_rate; struct v4l2_ctrl_handler ctrl_handler; + struct v4l2_subdev *sensor_sd; int buf_num; + int num_sensors; + int combine_index; + bool is_combine_mode; }; extern struct platform_driver rkcif_subdev_driver;