diff options
Diffstat (limited to 'drivers/staging/media/imx')
| -rw-r--r-- | drivers/staging/media/imx/Kconfig | 21 | ||||
| -rw-r--r-- | drivers/staging/media/imx/Makefile | 12 | ||||
| -rw-r--r-- | drivers/staging/media/imx/TODO | 23 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-ic-common.c | 113 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-ic-prp.c | 518 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-ic-prpencvf.c | 1309 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-ic.h | 38 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-media-capture.c | 775 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-media-csi.c | 1817 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-media-dev.c | 667 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-media-fim.c | 494 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-media-internal-sd.c | 349 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-media-of.c | 270 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-media-utils.c | 896 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-media-vdic.c | 1009 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx-media.h | 325 | ||||
| -rw-r--r-- | drivers/staging/media/imx/imx6-mipi-csi2.c | 698 |
17 files changed, 9334 insertions, 0 deletions
diff --git a/drivers/staging/media/imx/Kconfig b/drivers/staging/media/imx/Kconfig new file mode 100644 index 000000000000..7eff50bcea39 --- /dev/null +++ b/drivers/staging/media/imx/Kconfig @@ -0,0 +1,21 @@ +config VIDEO_IMX_MEDIA + tristate "i.MX5/6 V4L2 media core driver" + depends on MEDIA_CONTROLLER && VIDEO_V4L2 && ARCH_MXC && IMX_IPUV3_CORE + select V4L2_FWNODE + ---help--- + Say yes here to enable support for video4linux media controller + driver for the i.MX5/6 SOC. + +if VIDEO_IMX_MEDIA +menu "i.MX5/6 Media Sub devices" + +config VIDEO_IMX_CSI + tristate "i.MX5/6 Camera Sensor Interface driver" + depends on VIDEO_IMX_MEDIA && VIDEO_DEV && I2C + select VIDEOBUF2_DMA_CONTIG + default y + ---help--- + A video4linux camera sensor interface driver for i.MX5/6. + +endmenu +endif diff --git a/drivers/staging/media/imx/Makefile b/drivers/staging/media/imx/Makefile new file mode 100644 index 000000000000..3569625b6305 --- /dev/null +++ b/drivers/staging/media/imx/Makefile @@ -0,0 +1,12 @@ +imx-media-objs := imx-media-dev.o imx-media-internal-sd.o imx-media-of.o +imx-media-common-objs := imx-media-utils.o imx-media-fim.o +imx-media-ic-objs := imx-ic-common.o imx-ic-prp.o imx-ic-prpencvf.o + +obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media.o +obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-common.o +obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-capture.o +obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-vdic.o +obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-ic.o + +obj-$(CONFIG_VIDEO_IMX_CSI) += imx-media-csi.o +obj-$(CONFIG_VIDEO_IMX_CSI) += imx6-mipi-csi2.o diff --git a/drivers/staging/media/imx/TODO b/drivers/staging/media/imx/TODO new file mode 100644 index 000000000000..0bee3132b26f --- /dev/null +++ b/drivers/staging/media/imx/TODO @@ -0,0 +1,23 @@ + +- Clean up and move the ov5642 subdev driver to drivers/media/i2c, or + merge support for OV5642 into drivers/media/i2c/ov5640.c, and create + the binding docs for it. + +- The Frame Interval Monitor could be exported to v4l2-core for + general use. + +- At driver load time, the device-tree node that is the original source + (the "sensor"), is parsed to record its media bus configuration, and + this info is required in imx-media-csi.c to setup the CSI. + Laurent Pinchart argues that instead the CSI subdev should call its + neighbor's g_mbus_config op (which should be propagated if necessary) + to get this info. However Hans Verkuil is planning to remove the + g_mbus_config op. For now this driver uses the parsed DT mbus config + method until this issue is resolved. + +- This media driver supports inheriting V4L2 controls to the + video capture devices, from the subdevices in the capture device's + pipeline. The controls for each capture device are updated in the + link_notify callback when the pipeline is modified. It should be + decided whether this feature is useful enough to make it generally + available by exporting to v4l2-core. diff --git a/drivers/staging/media/imx/imx-ic-common.c b/drivers/staging/media/imx/imx-ic-common.c new file mode 100644 index 000000000000..cfdd4900a3be --- /dev/null +++ b/drivers/staging/media/imx/imx-ic-common.c @@ -0,0 +1,113 @@ +/* + * V4L2 Image Converter Subdev for Freescale i.MX5/6 SOC + * + * Copyright (c) 2014-2016 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/module.h> +#include <linux/platform_device.h> +#include <media/v4l2-device.h> +#include <media/v4l2-subdev.h> +#include "imx-media.h" +#include "imx-ic.h" + +#define IC_TASK_PRP IC_NUM_TASKS +#define IC_NUM_OPS (IC_NUM_TASKS + 1) + +static struct imx_ic_ops *ic_ops[IC_NUM_OPS] = { + [IC_TASK_PRP] = &imx_ic_prp_ops, + [IC_TASK_ENCODER] = &imx_ic_prpencvf_ops, + [IC_TASK_VIEWFINDER] = &imx_ic_prpencvf_ops, +}; + +static int imx_ic_probe(struct platform_device *pdev) +{ + struct imx_media_internal_sd_platformdata *pdata; + struct imx_ic_priv *priv; + int ret; + + priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + platform_set_drvdata(pdev, &priv->sd); + priv->dev = &pdev->dev; + + /* get our ipu_id, grp_id and IC task id */ + pdata = priv->dev->platform_data; + priv->ipu_id = pdata->ipu_id; + switch (pdata->grp_id) { + case IMX_MEDIA_GRP_ID_IC_PRP: + priv->task_id = IC_TASK_PRP; + break; + case IMX_MEDIA_GRP_ID_IC_PRPENC: + priv->task_id = IC_TASK_ENCODER; + break; + case IMX_MEDIA_GRP_ID_IC_PRPVF: + priv->task_id = IC_TASK_VIEWFINDER; + break; + default: + return -EINVAL; + } + + v4l2_subdev_init(&priv->sd, ic_ops[priv->task_id]->subdev_ops); + v4l2_set_subdevdata(&priv->sd, priv); + priv->sd.internal_ops = ic_ops[priv->task_id]->internal_ops; + priv->sd.entity.ops = ic_ops[priv->task_id]->entity_ops; + priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER; + priv->sd.dev = &pdev->dev; + priv->sd.owner = THIS_MODULE; + priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS; + priv->sd.grp_id = pdata->grp_id; + strncpy(priv->sd.name, pdata->sd_name, sizeof(priv->sd.name)); + + ret = ic_ops[priv->task_id]->init(priv); + if (ret) + return ret; + + ret = v4l2_async_register_subdev(&priv->sd); + if (ret) + ic_ops[priv->task_id]->remove(priv); + + return ret; +} + +static int imx_ic_remove(struct platform_device *pdev) +{ + struct v4l2_subdev *sd = platform_get_drvdata(pdev); + struct imx_ic_priv *priv = container_of(sd, struct imx_ic_priv, sd); + + v4l2_info(sd, "Removing\n"); + + ic_ops[priv->task_id]->remove(priv); + + v4l2_async_unregister_subdev(sd); + media_entity_cleanup(&sd->entity); + + return 0; +} + +static const struct platform_device_id imx_ic_ids[] = { + { .name = "imx-ipuv3-ic" }, + { }, +}; +MODULE_DEVICE_TABLE(platform, imx_ic_ids); + +static struct platform_driver imx_ic_driver = { + .probe = imx_ic_probe, + .remove = imx_ic_remove, + .id_table = imx_ic_ids, + .driver = { + .name = "imx-ipuv3-ic", + }, +}; +module_platform_driver(imx_ic_driver); + +MODULE_DESCRIPTION("i.MX IC subdev driver"); +MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("platform:imx-ipuv3-ic"); diff --git a/drivers/staging/media/imx/imx-ic-prp.c b/drivers/staging/media/imx/imx-ic-prp.c new file mode 100644 index 000000000000..c2bb5ef2acb4 --- /dev/null +++ b/drivers/staging/media/imx/imx-ic-prp.c @@ -0,0 +1,518 @@ +/* + * V4L2 Capture IC Preprocess Subdev for Freescale i.MX5/6 SOC + * + * This subdevice handles capture of video frames from the CSI or VDIC, + * which are routed directly to the Image Converter preprocess tasks, + * for resizing, colorspace conversion, and rotation. + * + * Copyright (c) 2012-2017 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/delay.h> +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/spinlock.h> +#include <linux/timer.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-ioctl.h> +#include <media/v4l2-subdev.h> +#include <media/imx.h> +#include "imx-media.h" +#include "imx-ic.h" + +/* + * Min/Max supported width and heights. + */ +#define MIN_W 176 +#define MIN_H 144 +#define MAX_W 4096 +#define MAX_H 4096 +#define W_ALIGN 4 /* multiple of 16 pixels */ +#define H_ALIGN 1 /* multiple of 2 lines */ +#define S_ALIGN 1 /* multiple of 2 */ + +struct prp_priv { + struct imx_media_dev *md; + struct imx_ic_priv *ic_priv; + struct media_pad pad[PRP_NUM_PADS]; + + /* lock to protect all members below */ + struct mutex lock; + + /* IPU units we require */ + struct ipu_soc *ipu; + + struct v4l2_subdev *src_sd; + struct v4l2_subdev *sink_sd_prpenc; + struct v4l2_subdev *sink_sd_prpvf; + + /* the CSI id at link validate */ + int csi_id; + + struct v4l2_mbus_framefmt format_mbus; + struct v4l2_fract frame_interval; + + int stream_count; +}; + +static inline struct prp_priv *sd_to_priv(struct v4l2_subdev *sd) +{ + struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd); + + return ic_priv->prp_priv; +} + +static int prp_start(struct prp_priv *priv) +{ + struct imx_ic_priv *ic_priv = priv->ic_priv; + bool src_is_vdic; + + priv->ipu = priv->md->ipu[ic_priv->ipu_id]; + + /* set IC to receive from CSI or VDI depending on source */ + src_is_vdic = !!(priv->src_sd->grp_id & IMX_MEDIA_GRP_ID_VDIC); + + ipu_set_ic_src_mux(priv->ipu, priv->csi_id, src_is_vdic); + + return 0; +} + +static void prp_stop(struct prp_priv *priv) +{ +} + +static struct v4l2_mbus_framefmt * +__prp_get_fmt(struct prp_priv *priv, struct v4l2_subdev_pad_config *cfg, + unsigned int pad, enum v4l2_subdev_format_whence which) +{ + struct imx_ic_priv *ic_priv = priv->ic_priv; + + if (which == V4L2_SUBDEV_FORMAT_TRY) + return v4l2_subdev_get_try_format(&ic_priv->sd, cfg, pad); + else + return &priv->format_mbus; +} + +/* + * V4L2 subdev operations. + */ + +static int prp_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_mbus_code_enum *code) +{ + struct prp_priv *priv = sd_to_priv(sd); + struct v4l2_mbus_framefmt *infmt; + int ret = 0; + + mutex_lock(&priv->lock); + + switch (code->pad) { + case PRP_SINK_PAD: + ret = imx_media_enum_ipu_format(&code->code, code->index, + CS_SEL_ANY); + break; + case PRP_SRC_PAD_PRPENC: + case PRP_SRC_PAD_PRPVF: + if (code->index != 0) { + ret = -EINVAL; + goto out; + } + infmt = __prp_get_fmt(priv, cfg, PRP_SINK_PAD, code->which); + code->code = infmt->code; + break; + default: + ret = -EINVAL; + } +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_get_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct prp_priv *priv = sd_to_priv(sd); + struct v4l2_mbus_framefmt *fmt; + int ret = 0; + + if (sdformat->pad >= PRP_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + + fmt = __prp_get_fmt(priv, cfg, sdformat->pad, sdformat->which); + if (!fmt) { + ret = -EINVAL; + goto out; + } + + sdformat->format = *fmt; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct prp_priv *priv = sd_to_priv(sd); + struct v4l2_mbus_framefmt *fmt, *infmt; + const struct imx_media_pixfmt *cc; + int ret = 0; + u32 code; + + if (sdformat->pad >= PRP_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + + if (priv->stream_count > 0) { + ret = -EBUSY; + goto out; + } + + infmt = __prp_get_fmt(priv, cfg, PRP_SINK_PAD, sdformat->which); + + switch (sdformat->pad) { + case PRP_SINK_PAD: + v4l_bound_align_image(&sdformat->format.width, MIN_W, MAX_W, + W_ALIGN, &sdformat->format.height, + MIN_H, MAX_H, H_ALIGN, S_ALIGN); + + cc = imx_media_find_ipu_format(sdformat->format.code, + CS_SEL_ANY); + if (!cc) { + imx_media_enum_ipu_format(&code, 0, CS_SEL_ANY); + cc = imx_media_find_ipu_format(code, CS_SEL_ANY); + sdformat->format.code = cc->codes[0]; + } + + imx_media_fill_default_mbus_fields(&sdformat->format, infmt, + true); + break; + case PRP_SRC_PAD_PRPENC: + case PRP_SRC_PAD_PRPVF: + /* Output pads mirror input pad */ + sdformat->format = *infmt; + break; + } + + fmt = __prp_get_fmt(priv, cfg, sdformat->pad, sdformat->which); + *fmt = sdformat->format; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_link_setup(struct media_entity *entity, + const struct media_pad *local, + const struct media_pad *remote, u32 flags) +{ + struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); + struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd); + struct prp_priv *priv = ic_priv->prp_priv; + struct v4l2_subdev *remote_sd; + int ret = 0; + + dev_dbg(ic_priv->dev, "link setup %s -> %s", remote->entity->name, + local->entity->name); + + remote_sd = media_entity_to_v4l2_subdev(remote->entity); + + mutex_lock(&priv->lock); + + if (local->flags & MEDIA_PAD_FL_SINK) { + if (flags & MEDIA_LNK_FL_ENABLED) { + if (priv->src_sd) { + ret = -EBUSY; + goto out; + } + if (priv->sink_sd_prpenc && (remote_sd->grp_id & + IMX_MEDIA_GRP_ID_VDIC)) { + ret = -EINVAL; + goto out; + } + priv->src_sd = remote_sd; + } else { + priv->src_sd = NULL; + } + + goto out; + } + + /* this is a source pad */ + if (flags & MEDIA_LNK_FL_ENABLED) { + switch (local->index) { + case PRP_SRC_PAD_PRPENC: + if (priv->sink_sd_prpenc) { + ret = -EBUSY; + goto out; + } + if (priv->src_sd && (priv->src_sd->grp_id & + IMX_MEDIA_GRP_ID_VDIC)) { + ret = -EINVAL; + goto out; + } + priv->sink_sd_prpenc = remote_sd; + break; + case PRP_SRC_PAD_PRPVF: + if (priv->sink_sd_prpvf) { + ret = -EBUSY; + goto out; + } + priv->sink_sd_prpvf = remote_sd; + break; + default: + ret = -EINVAL; + } + } else { + switch (local->index) { + case PRP_SRC_PAD_PRPENC: + priv->sink_sd_prpenc = NULL; + break; + case PRP_SRC_PAD_PRPVF: + priv->sink_sd_prpvf = NULL; + break; + default: + ret = -EINVAL; + } + } + +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_link_validate(struct v4l2_subdev *sd, + struct media_link *link, + struct v4l2_subdev_format *source_fmt, + struct v4l2_subdev_format *sink_fmt) +{ + struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd); + struct prp_priv *priv = ic_priv->prp_priv; + struct imx_media_subdev *csi; + int ret; + + ret = v4l2_subdev_link_validate_default(sd, link, + source_fmt, sink_fmt); + if (ret) + return ret; + + csi = imx_media_find_upstream_subdev(priv->md, &ic_priv->sd.entity, + IMX_MEDIA_GRP_ID_CSI); + if (IS_ERR(csi)) + csi = NULL; + + mutex_lock(&priv->lock); + + if (priv->src_sd->grp_id & IMX_MEDIA_GRP_ID_VDIC) { + /* + * the ->PRPENC link cannot be enabled if the source + * is the VDIC + */ + if (priv->sink_sd_prpenc) + ret = -EINVAL; + goto out; + } else { + /* the source is a CSI */ + if (!csi) { + ret = -EINVAL; + goto out; + } + } + + if (csi) { + switch (csi->sd->grp_id) { + case IMX_MEDIA_GRP_ID_CSI0: + priv->csi_id = 0; + break; + case IMX_MEDIA_GRP_ID_CSI1: + priv->csi_id = 1; + break; + default: + ret = -EINVAL; + } + } else { + priv->csi_id = 0; + } + +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_s_stream(struct v4l2_subdev *sd, int enable) +{ + struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd); + struct prp_priv *priv = ic_priv->prp_priv; + int ret = 0; + + mutex_lock(&priv->lock); + + if (!priv->src_sd || (!priv->sink_sd_prpenc && !priv->sink_sd_prpvf)) { + ret = -EPIPE; + goto out; + } + + /* + * enable/disable streaming only if stream_count is + * going from 0 to 1 / 1 to 0. + */ + if (priv->stream_count != !enable) + goto update_count; + + dev_dbg(ic_priv->dev, "stream %s\n", enable ? "ON" : "OFF"); + + if (enable) + ret = prp_start(priv); + else + prp_stop(priv); + if (ret) + goto out; + + /* start/stop upstream */ + ret = v4l2_subdev_call(priv->src_sd, video, s_stream, enable); + ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0; + if (ret) { + if (enable) + prp_stop(priv); + goto out; + } + +update_count: + priv->stream_count += enable ? 1 : -1; + if (priv->stream_count < 0) + priv->stream_count = 0; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_g_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct prp_priv *priv = sd_to_priv(sd); + + if (fi->pad >= PRP_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + fi->interval = priv->frame_interval; + mutex_unlock(&priv->lock); + + return 0; +} + +static int prp_s_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct prp_priv *priv = sd_to_priv(sd); + + if (fi->pad >= PRP_NUM_PADS) + return -EINVAL; + + /* No limits on frame interval */ + mutex_lock(&priv->lock); + priv->frame_interval = fi->interval; + mutex_unlock(&priv->lock); + + return 0; +} + +/* + * retrieve our pads parsed from the OF graph by the media device + */ +static int prp_registered(struct v4l2_subdev *sd) +{ + struct prp_priv *priv = sd_to_priv(sd); + int i, ret; + u32 code; + + /* get media device */ + priv->md = dev_get_drvdata(sd->v4l2_dev->dev); + + for (i = 0; i < PRP_NUM_PADS; i++) { + priv->pad[i].flags = (i == PRP_SINK_PAD) ? + MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE; + } + + /* init default frame interval */ + priv->frame_interval.numerator = 1; + priv->frame_interval.denominator = 30; + + /* set a default mbus format */ + imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV); + ret = imx_media_init_mbus_fmt(&priv->format_mbus, 640, 480, code, + V4L2_FIELD_NONE, NULL); + if (ret) + return ret; + + return media_entity_pads_init(&sd->entity, PRP_NUM_PADS, priv->pad); +} + +static const struct v4l2_subdev_pad_ops prp_pad_ops = { + .enum_mbus_code = prp_enum_mbus_code, + .get_fmt = prp_get_fmt, + .set_fmt = prp_set_fmt, + .link_validate = prp_link_validate, +}; + +static const struct v4l2_subdev_video_ops prp_video_ops = { + .g_frame_interval = prp_g_frame_interval, + .s_frame_interval = prp_s_frame_interval, + .s_stream = prp_s_stream, +}; + +static const struct media_entity_operations prp_entity_ops = { + .link_setup = prp_link_setup, + .link_validate = v4l2_subdev_link_validate, +}; + +static const struct v4l2_subdev_ops prp_subdev_ops = { + .video = &prp_video_ops, + .pad = &prp_pad_ops, +}; + +static const struct v4l2_subdev_internal_ops prp_internal_ops = { + .registered = prp_registered, +}; + +static int prp_init(struct imx_ic_priv *ic_priv) +{ + struct prp_priv *priv; + + priv = devm_kzalloc(ic_priv->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + mutex_init(&priv->lock); + ic_priv->prp_priv = priv; + priv->ic_priv = ic_priv; + + return 0; +} + +static void prp_remove(struct imx_ic_priv *ic_priv) +{ + struct prp_priv *priv = ic_priv->prp_priv; + + mutex_destroy(&priv->lock); +} + +struct imx_ic_ops imx_ic_prp_ops = { + .subdev_ops = &prp_subdev_ops, + .internal_ops = &prp_internal_ops, + .entity_ops = &prp_entity_ops, + .init = prp_init, + .remove = prp_remove, +}; diff --git a/drivers/staging/media/imx/imx-ic-prpencvf.c b/drivers/staging/media/imx/imx-ic-prpencvf.c new file mode 100644 index 000000000000..ed363fe3b3d0 --- /dev/null +++ b/drivers/staging/media/imx/imx-ic-prpencvf.c @@ -0,0 +1,1309 @@ +/* + * V4L2 Capture IC Preprocess Subdev for Freescale i.MX5/6 SOC + * + * This subdevice handles capture of video frames from the CSI or VDIC, + * which are routed directly to the Image Converter preprocess tasks, + * for resizing, colorspace conversion, and rotation. + * + * Copyright (c) 2012-2017 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/delay.h> +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/spinlock.h> +#include <linux/timer.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-ioctl.h> +#include <media/v4l2-mc.h> +#include <media/v4l2-subdev.h> +#include <media/imx.h> +#include "imx-media.h" +#include "imx-ic.h" + +/* + * Min/Max supported width and heights. + * + * We allow planar output, so we have to align width at the source pad + * by 16 pixels to meet IDMAC alignment requirements for possible planar + * output. + * + * TODO: move this into pad format negotiation, if capture device + * has not requested a planar format, we should allow 8 pixel + * alignment at the source pad. + */ +#define MIN_W_SINK 176 +#define MIN_H_SINK 144 +#define MAX_W_SINK 4096 +#define MAX_H_SINK 4096 +#define W_ALIGN_SINK 3 /* multiple of 8 pixels */ +#define H_ALIGN_SINK 1 /* multiple of 2 lines */ + +#define MAX_W_SRC 1024 +#define MAX_H_SRC 1024 +#define W_ALIGN_SRC 4 /* multiple of 16 pixels */ +#define H_ALIGN_SRC 1 /* multiple of 2 lines */ + +#define S_ALIGN 1 /* multiple of 2 */ + +struct prp_priv { + struct imx_media_dev *md; + struct imx_ic_priv *ic_priv; + struct media_pad pad[PRPENCVF_NUM_PADS]; + /* the video device at output pad */ + struct imx_media_video_dev *vdev; + + /* lock to protect all members below */ + struct mutex lock; + + /* IPU units we require */ + struct ipu_soc *ipu; + struct ipu_ic *ic; + struct ipuv3_channel *out_ch; + struct ipuv3_channel *rot_in_ch; + struct ipuv3_channel *rot_out_ch; + + /* active vb2 buffers to send to video dev sink */ + struct imx_media_buffer *active_vb2_buf[2]; + struct imx_media_dma_buf underrun_buf; + + int ipu_buf_num; /* ipu double buffer index: 0-1 */ + + /* the sink for the captured frames */ + struct media_entity *sink; + /* the source subdev */ + struct v4l2_subdev *src_sd; + + struct v4l2_mbus_framefmt format_mbus[PRPENCVF_NUM_PADS]; + const struct imx_media_pixfmt *cc[PRPENCVF_NUM_PADS]; + struct v4l2_fract frame_interval; + + struct imx_media_dma_buf rot_buf[2]; + + /* controls */ + struct v4l2_ctrl_handler ctrl_hdlr; + int rotation; /* degrees */ + bool hflip; + bool vflip; + + /* derived from rotation, hflip, vflip controls */ + enum ipu_rotate_mode rot_mode; + + spinlock_t irqlock; /* protect eof_irq handler */ + + struct timer_list eof_timeout_timer; + int eof_irq; + int nfb4eof_irq; + + int stream_count; + bool last_eof; /* waiting for last EOF at stream off */ + bool nfb4eof; /* NFB4EOF encountered during streaming */ + struct completion last_eof_comp; +}; + +static const struct prp_channels { + u32 out_ch; + u32 rot_in_ch; + u32 rot_out_ch; +} prp_channel[] = { + [IC_TASK_ENCODER] = { + .out_ch = IPUV3_CHANNEL_IC_PRP_ENC_MEM, + .rot_in_ch = IPUV3_CHANNEL_MEM_ROT_ENC, + .rot_out_ch = IPUV3_CHANNEL_ROT_ENC_MEM, + }, + [IC_TASK_VIEWFINDER] = { + .out_ch = IPUV3_CHANNEL_IC_PRP_VF_MEM, + .rot_in_ch = IPUV3_CHANNEL_MEM_ROT_VF, + .rot_out_ch = IPUV3_CHANNEL_ROT_VF_MEM, + }, +}; + +static inline struct prp_priv *sd_to_priv(struct v4l2_subdev *sd) +{ + struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd); + + return ic_priv->task_priv; +} + +static void prp_put_ipu_resources(struct prp_priv *priv) +{ + if (!IS_ERR_OR_NULL(priv->ic)) + ipu_ic_put(priv->ic); + priv->ic = NULL; + + if (!IS_ERR_OR_NULL(priv->out_ch)) + ipu_idmac_put(priv->out_ch); + priv->out_ch = NULL; + + if (!IS_ERR_OR_NULL(priv->rot_in_ch)) + ipu_idmac_put(priv->rot_in_ch); + priv->rot_in_ch = NULL; + + if (!IS_ERR_OR_NULL(priv->rot_out_ch)) + ipu_idmac_put(priv->rot_out_ch); + priv->rot_out_ch = NULL; +} + +static int prp_get_ipu_resources(struct prp_priv *priv) +{ + struct imx_ic_priv *ic_priv = priv->ic_priv; + int ret, task = ic_priv->task_id; + + priv->ipu = priv->md->ipu[ic_priv->ipu_id]; + + priv->ic = ipu_ic_get(priv->ipu, task); + if (IS_ERR(priv->ic)) { + v4l2_err(&ic_priv->sd, "failed to get IC\n"); + ret = PTR_ERR(priv->ic); + goto out; + } + + priv->out_ch = ipu_idmac_get(priv->ipu, + prp_channel[task].out_ch); + if (IS_ERR(priv->out_ch)) { + v4l2_err(&ic_priv->sd, "could not get IDMAC channel %u\n", + prp_channel[task].out_ch); + ret = PTR_ERR(priv->out_ch); + goto out; + } + + priv->rot_in_ch = ipu_idmac_get(priv->ipu, + prp_channel[task].rot_in_ch); + if (IS_ERR(priv->rot_in_ch)) { + v4l2_err(&ic_priv->sd, "could not get IDMAC channel %u\n", + prp_channel[task].rot_in_ch); + ret = PTR_ERR(priv->rot_in_ch); + goto out; + } + + priv->rot_out_ch = ipu_idmac_get(priv->ipu, + prp_channel[task].rot_out_ch); + if (IS_ERR(priv->rot_out_ch)) { + v4l2_err(&ic_priv->sd, "could not get IDMAC channel %u\n", + prp_channel[task].rot_out_ch); + ret = PTR_ERR(priv->rot_out_ch); + goto out; + } + + return 0; +out: + prp_put_ipu_resources(priv); + return ret; +} + +static void prp_vb2_buf_done(struct prp_priv *priv, struct ipuv3_channel *ch) +{ + struct imx_media_video_dev *vdev = priv->vdev; + struct imx_media_buffer *done, *next; + struct vb2_buffer *vb; + dma_addr_t phys; + + done = priv->active_vb2_buf[priv->ipu_buf_num]; + if (done) { + vb = &done->vbuf.vb2_buf; + vb->timestamp = ktime_get_ns(); + vb2_buffer_done(vb, priv->nfb4eof ? + VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE); + } + + priv->nfb4eof = false; + + /* get next queued buffer */ + next = imx_media_capture_device_next_buf(vdev); + if (next) { + phys = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0); + priv->active_vb2_buf[priv->ipu_buf_num] = next; + } else { + phys = priv->underrun_buf.phys; + priv->active_vb2_buf[priv->ipu_buf_num] = NULL; + } + + if (ipu_idmac_buffer_is_ready(ch, priv->ipu_buf_num)) + ipu_idmac_clear_buffer(ch, priv->ipu_buf_num); + + ipu_cpmem_set_buffer(ch, priv->ipu_buf_num, phys); +} + +static irqreturn_t prp_eof_interrupt(int irq, void *dev_id) +{ + struct prp_priv *priv = dev_id; + struct ipuv3_channel *channel; + + spin_lock(&priv->irqlock); + + if (priv->last_eof) { + complete(&priv->last_eof_comp); + priv->last_eof = false; + goto unlock; + } + + channel = (ipu_rot_mode_is_irt(priv->rot_mode)) ? + priv->rot_out_ch : priv->out_ch; + + prp_vb2_buf_done(priv, channel); + + /* select new IPU buf */ + ipu_idmac_select_buffer(channel, priv->ipu_buf_num); + /* toggle IPU double-buffer index */ + priv->ipu_buf_num ^= 1; + + /* bump the EOF timeout timer */ + mod_timer(&priv->eof_timeout_timer, + jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT)); + +unlock: + spin_unlock(&priv->irqlock); + return IRQ_HANDLED; +} + +static irqreturn_t prp_nfb4eof_interrupt(int irq, void *dev_id) +{ + struct prp_priv *priv = dev_id; + struct imx_ic_priv *ic_priv = priv->ic_priv; + + spin_lock(&priv->irqlock); + + /* + * this is not an unrecoverable error, just mark + * the next captured frame with vb2 error flag. + */ + priv->nfb4eof = true; + + v4l2_err(&ic_priv->sd, "NFB4EOF\n"); + + spin_unlock(&priv->irqlock); + + return IRQ_HANDLED; +} + +/* + * EOF timeout timer function. + */ +/* + * EOF timeout timer function. This is an unrecoverable condition + * without a stream restart. + */ +static void prp_eof_timeout(unsigned long data) +{ + struct prp_priv *priv = (struct prp_priv *)data; + struct imx_media_video_dev *vdev = priv->vdev; + struct imx_ic_priv *ic_priv = priv->ic_priv; + + v4l2_err(&ic_priv->sd, "EOF timeout\n"); + + /* signal a fatal error to capture device */ + imx_media_capture_device_error(vdev); +} + +static void prp_setup_vb2_buf(struct prp_priv *priv, dma_addr_t *phys) +{ + struct imx_media_video_dev *vdev = priv->vdev; + struct imx_media_buffer *buf; + int i; + + for (i = 0; i < 2; i++) { + buf = imx_media_capture_device_next_buf(vdev); + if (buf) { + priv->active_vb2_buf[i] = buf; + phys[i] = vb2_dma_contig_plane_dma_addr( + &buf->vbuf.vb2_buf, 0); + } else { + priv->active_vb2_buf[i] = NULL; + phys[i] = priv->underrun_buf.phys; + } + } +} + +static void prp_unsetup_vb2_buf(struct prp_priv *priv, + enum vb2_buffer_state return_status) +{ + struct imx_media_buffer *buf; + int i; + + /* return any remaining active frames with return_status */ + for (i = 0; i < 2; i++) { + buf = priv->active_vb2_buf[i]; + if (buf) { + struct vb2_buffer *vb = &buf->vbuf.vb2_buf; + + vb->timestamp = ktime_get_ns(); + vb2_buffer_done(vb, return_status); + } + } +} + +static int prp_setup_channel(struct prp_priv *priv, + struct ipuv3_channel *channel, + enum ipu_rotate_mode rot_mode, + dma_addr_t addr0, dma_addr_t addr1, + bool rot_swap_width_height) +{ + struct imx_media_video_dev *vdev = priv->vdev; + const struct imx_media_pixfmt *outcc; + struct v4l2_mbus_framefmt *infmt; + unsigned int burst_size; + struct ipu_image image; + int ret; + + infmt = &priv->format_mbus[PRPENCVF_SINK_PAD]; + outcc = vdev->cc; + + ipu_cpmem_zero(channel); + + memset(&image, 0, sizeof(image)); + image.pix = vdev->fmt.fmt.pix; + image.rect.width = image.pix.width; + image.rect.height = image.pix.height; + + if (rot_swap_width_height) { + swap(image.pix.width, image.pix.height); + swap(image.rect.width, image.rect.height); + /* recalc stride using swapped width */ + image.pix.bytesperline = outcc->planar ? + image.pix.width : + (image.pix.width * outcc->bpp) >> 3; + } + + image.phys0 = addr0; + image.phys1 = addr1; + + ret = ipu_cpmem_set_image(channel, &image); + if (ret) + return ret; + + if (channel == priv->rot_in_ch || + channel == priv->rot_out_ch) { + burst_size = 8; + ipu_cpmem_set_block_mode(channel); + } else { + burst_size = (image.pix.width & 0xf) ? 8 : 16; + } + + ipu_cpmem_set_burstsize(channel, burst_size); + + if (rot_mode) + ipu_cpmem_set_rotation(channel, rot_mode); + + if (image.pix.field == V4L2_FIELD_NONE && + V4L2_FIELD_HAS_BOTH(infmt->field) && + channel == priv->out_ch) + ipu_cpmem_interlaced_scan(channel, image.pix.bytesperline); + + ret = ipu_ic_task_idma_init(priv->ic, channel, + image.pix.width, image.pix.height, + burst_size, rot_mode); + if (ret) + return ret; + + ipu_cpmem_set_axi_id(channel, 1); + + ipu_idmac_set_double_buffer(channel, true); + + return 0; +} + +static int prp_setup_rotation(struct prp_priv *priv) +{ + struct imx_media_video_dev *vdev = priv->vdev; + struct imx_ic_priv *ic_priv = priv->ic_priv; + const struct imx_media_pixfmt *outcc, *incc; + struct v4l2_mbus_framefmt *infmt; + struct v4l2_pix_format *outfmt; + dma_addr_t phys[2]; + int ret; + + infmt = &priv->format_mbus[PRPENCVF_SINK_PAD]; + outfmt = &vdev->fmt.fmt.pix; + incc = priv->cc[PRPENCVF_SINK_PAD]; + outcc = vdev->cc; + + ret = imx_media_alloc_dma_buf(priv->md, &priv->rot_buf[0], + outfmt->sizeimage); + if (ret) { + v4l2_err(&ic_priv->sd, "failed to alloc rot_buf[0], %d\n", ret); + return ret; + } + ret = imx_media_alloc_dma_buf(priv->md, &priv->rot_buf[1], + outfmt->sizeimage); + if (ret) { + v4l2_err(&ic_priv->sd, "failed to alloc rot_buf[1], %d\n", ret); + goto free_rot0; + } + + ret = ipu_ic_task_init(priv->ic, + infmt->width, infmt->height, + outfmt->height, outfmt->width, + incc->cs, outcc->cs); + if (ret) { + v4l2_err(&ic_priv->sd, "ipu_ic_task_init failed, %d\n", ret); + goto free_rot1; + } + + /* init the IC-PRP-->MEM IDMAC channel */ + ret = prp_setup_channel(priv, priv->out_ch, IPU_ROTATE_NONE, + priv->rot_buf[0].phys, priv->rot_buf[1].phys, + true); + if (ret) { + v4l2_err(&ic_priv->sd, + "prp_setup_channel(out_ch) failed, %d\n", ret); + goto free_rot1; + } + + /* init the MEM-->IC-PRP ROT IDMAC channel */ + ret = prp_setup_channel(priv, priv->rot_in_ch, priv->rot_mode, + priv->rot_buf[0].phys, priv->rot_buf[1].phys, + true); + if (ret) { + v4l2_err(&ic_priv->sd, + "prp_setup_channel(rot_in_ch) failed, %d\n", ret); + goto free_rot1; + } + + prp_setup_vb2_buf(priv, phys); + + /* init the destination IC-PRP ROT-->MEM IDMAC channel */ + ret = prp_setup_channel(priv, priv->rot_out_ch, IPU_ROTATE_NONE, + phys[0], phys[1], + false); + if (ret) { + v4l2_err(&ic_priv->sd, + "prp_setup_channel(rot_out_ch) failed, %d\n", ret); + goto unsetup_vb2; + } + + /* now link IC-PRP-->MEM to MEM-->IC-PRP ROT */ + ipu_idmac_link(priv->out_ch, priv->rot_in_ch); + + /* enable the IC */ + ipu_ic_enable(priv->ic); + + /* set buffers ready */ + ipu_idmac_select_buffer(priv->out_ch, 0); + ipu_idmac_select_buffer(priv->out_ch, 1); + ipu_idmac_select_buffer(priv->rot_out_ch, 0); + ipu_idmac_select_buffer(priv->rot_out_ch, 1); + + /* enable the channels */ + ipu_idmac_enable_channel(priv->out_ch); + ipu_idmac_enable_channel(priv->rot_in_ch); + ipu_idmac_enable_channel(priv->rot_out_ch); + + /* and finally enable the IC PRP task */ + ipu_ic_task_enable(priv->ic); + + return 0; + +unsetup_vb2: + prp_unsetup_vb2_buf(priv, VB2_BUF_STATE_QUEUED); +free_rot1: + imx_media_free_dma_buf(priv->md, &priv->rot_buf[1]); +free_rot0: + imx_media_free_dma_buf(priv->md, &priv->rot_buf[0]); + return ret; +} + +static void prp_unsetup_rotation(struct prp_priv *priv) +{ + ipu_ic_task_disable(priv->ic); + + ipu_idmac_disable_channel(priv->out_ch); + ipu_idmac_disable_channel(priv->rot_in_ch); + ipu_idmac_disable_channel(priv->rot_out_ch); + + ipu_idmac_unlink(priv->out_ch, priv->rot_in_ch); + + ipu_ic_disable(priv->ic); + + imx_media_free_dma_buf(priv->md, &priv->rot_buf[0]); + imx_media_free_dma_buf(priv->md, &priv->rot_buf[1]); +} + +static int prp_setup_norotation(struct prp_priv *priv) +{ + struct imx_media_video_dev *vdev = priv->vdev; + struct imx_ic_priv *ic_priv = priv->ic_priv; + const struct imx_media_pixfmt *outcc, *incc; + struct v4l2_mbus_framefmt *infmt; + struct v4l2_pix_format *outfmt; + dma_addr_t phys[2]; + int ret; + + infmt = &priv->format_mbus[PRPENCVF_SINK_PAD]; + outfmt = &vdev->fmt.fmt.pix; + incc = priv->cc[PRPENCVF_SINK_PAD]; + outcc = vdev->cc; + + ret = ipu_ic_task_init(priv->ic, + infmt->width, infmt->height, + outfmt->width, outfmt->height, + incc->cs, outcc->cs); + if (ret) { + v4l2_err(&ic_priv->sd, "ipu_ic_task_init failed, %d\n", ret); + return ret; + } + + prp_setup_vb2_buf(priv, phys); + + /* init the IC PRP-->MEM IDMAC channel */ + ret = prp_setup_channel(priv, priv->out_ch, priv->rot_mode, + phys[0], phys[1], false); + if (ret) { + v4l2_err(&ic_priv->sd, + "prp_setup_channel(out_ch) failed, %d\n", ret); + goto unsetup_vb2; + } + + ipu_cpmem_dump(priv->out_ch); + ipu_ic_dump(priv->ic); + ipu_dump(priv->ipu); + + ipu_ic_enable(priv->ic); + + /* set buffers ready */ + ipu_idmac_select_buffer(priv->out_ch, 0); + ipu_idmac_select_buffer(priv->out_ch, 1); + + /* enable the channels */ + ipu_idmac_enable_channel(priv->out_ch); + + /* enable the IC task */ + ipu_ic_task_enable(priv->ic); + + return 0; + +unsetup_vb2: + prp_unsetup_vb2_buf(priv, VB2_BUF_STATE_QUEUED); + return ret; +} + +static void prp_unsetup_norotation(struct prp_priv *priv) +{ + ipu_ic_task_disable(priv->ic); + ipu_idmac_disable_channel(priv->out_ch); + ipu_ic_disable(priv->ic); +} + +static void prp_unsetup(struct prp_priv *priv, + enum vb2_buffer_state state) +{ + if (ipu_rot_mode_is_irt(priv->rot_mode)) + prp_unsetup_rotation(priv); + else + prp_unsetup_norotation(priv); + + prp_unsetup_vb2_buf(priv, state); +} + +static int prp_start(struct prp_priv *priv) +{ + struct imx_ic_priv *ic_priv = priv->ic_priv; + struct imx_media_video_dev *vdev = priv->vdev; + struct v4l2_pix_format *outfmt; + int ret; + + ret = prp_get_ipu_resources(priv); + if (ret) + return ret; + + outfmt = &vdev->fmt.fmt.pix; + + ret = imx_media_alloc_dma_buf(priv->md, &priv->underrun_buf, + outfmt->sizeimage); + if (ret) + goto out_put_ipu; + + priv->ipu_buf_num = 0; + + /* init EOF completion waitq */ + init_completion(&priv->last_eof_comp); + priv->last_eof = false; + priv->nfb4eof = false; + + if (ipu_rot_mode_is_irt(priv->rot_mode)) + ret = prp_setup_rotation(priv); + else + ret = prp_setup_norotation(priv); + if (ret) + goto out_free_underrun; + + priv->nfb4eof_irq = ipu_idmac_channel_irq(priv->ipu, + priv->out_ch, + IPU_IRQ_NFB4EOF); + ret = devm_request_irq(ic_priv->dev, priv->nfb4eof_irq, + prp_nfb4eof_interrupt, 0, + "imx-ic-prp-nfb4eof", priv); + if (ret) { + v4l2_err(&ic_priv->sd, + "Error registering NFB4EOF irq: %d\n", ret); + goto out_unsetup; + } + + if (ipu_rot_mode_is_irt(priv->rot_mode)) + priv->eof_irq = ipu_idmac_channel_irq( + priv->ipu, priv->rot_out_ch, IPU_IRQ_EOF); + else + priv->eof_irq = ipu_idmac_channel_irq( + priv->ipu, priv->out_ch, IPU_IRQ_EOF); + + ret = devm_request_irq(ic_priv->dev, priv->eof_irq, + prp_eof_interrupt, 0, + "imx-ic-prp-eof", priv); + if (ret) { + v4l2_err(&ic_priv->sd, + "Error registering eof irq: %d\n", ret); + goto out_free_nfb4eof_irq; + } + + /* start the EOF timeout timer */ + mod_timer(&priv->eof_timeout_timer, + jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT)); + + return 0; + +out_free_nfb4eof_irq: + devm_free_irq(ic_priv->dev, priv->nfb4eof_irq, priv); +out_unsetup: + prp_unsetup(priv, VB2_BUF_STATE_QUEUED); +out_free_underrun: + imx_media_free_dma_buf(priv->md, &priv->underrun_buf); +out_put_ipu: + prp_put_ipu_resources(priv); + return ret; +} + +static void prp_stop(struct prp_priv *priv) +{ + struct imx_ic_priv *ic_priv = priv->ic_priv; + unsigned long flags; + int ret; + + /* mark next EOF interrupt as the last before stream off */ + spin_lock_irqsave(&priv->irqlock, flags); + priv->last_eof = true; + spin_unlock_irqrestore(&priv->irqlock, flags); + + /* + * and then wait for interrupt handler to mark completion. + */ + ret = wait_for_completion_timeout( + &priv->last_eof_comp, + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT)); + if (ret == 0) + v4l2_warn(&ic_priv->sd, "wait last EOF timeout\n"); + + devm_free_irq(ic_priv->dev, priv->eof_irq, priv); + devm_free_irq(ic_priv->dev, priv->nfb4eof_irq, priv); + + prp_unsetup(priv, VB2_BUF_STATE_ERROR); + + imx_media_free_dma_buf(priv->md, &priv->underrun_buf); + + /* cancel the EOF timeout timer */ + del_timer_sync(&priv->eof_timeout_timer); + + prp_put_ipu_resources(priv); +} + +static struct v4l2_mbus_framefmt * +__prp_get_fmt(struct prp_priv *priv, struct v4l2_subdev_pad_config *cfg, + unsigned int pad, enum v4l2_subdev_format_whence which) +{ + struct imx_ic_priv *ic_priv = priv->ic_priv; + + if (which == V4L2_SUBDEV_FORMAT_TRY) + return v4l2_subdev_get_try_format(&ic_priv->sd, cfg, pad); + else + return &priv->format_mbus[pad]; +} + +/* + * Applies IC resizer and IDMAC alignment restrictions to output + * rectangle given the input rectangle, and depending on given + * rotation mode. + * + * The IC resizer cannot downsize more than 4:1. Note also that + * for 90 or 270 rotation, _both_ output width and height must + * be aligned by W_ALIGN_SRC, because the intermediate rotation + * buffer swaps output width/height, and the final output buffer + * does not. + * + * Returns true if the output rectangle was modified. + */ +static bool prp_bound_align_output(struct v4l2_mbus_framefmt *outfmt, + struct v4l2_mbus_framefmt *infmt, + enum ipu_rotate_mode rot_mode) +{ + u32 orig_width = outfmt->width; + u32 orig_height = outfmt->height; + + if (ipu_rot_mode_is_irt(rot_mode)) + v4l_bound_align_image(&outfmt->width, + infmt->height / 4, MAX_H_SRC, + W_ALIGN_SRC, + &outfmt->height, + infmt->width / 4, MAX_W_SRC, + W_ALIGN_SRC, S_ALIGN); + else + v4l_bound_align_image(&outfmt->width, + infmt->width / 4, MAX_W_SRC, + W_ALIGN_SRC, + &outfmt->height, + infmt->height / 4, MAX_H_SRC, + H_ALIGN_SRC, S_ALIGN); + + return outfmt->width != orig_width || outfmt->height != orig_height; +} + +/* + * V4L2 subdev operations. + */ + +static int prp_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_mbus_code_enum *code) +{ + if (code->pad >= PRPENCVF_NUM_PADS) + return -EINVAL; + + return imx_media_enum_ipu_format(&code->code, code->index, CS_SEL_ANY); +} + +static int prp_get_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct prp_priv *priv = sd_to_priv(sd); + struct v4l2_mbus_framefmt *fmt; + int ret = 0; + + if (sdformat->pad >= PRPENCVF_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + + fmt = __prp_get_fmt(priv, cfg, sdformat->pad, sdformat->which); + if (!fmt) { + ret = -EINVAL; + goto out; + } + + sdformat->format = *fmt; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static void prp_try_fmt(struct prp_priv *priv, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat, + const struct imx_media_pixfmt **cc) +{ + struct v4l2_mbus_framefmt *infmt; + + *cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_ANY); + if (!*cc) { + u32 code; + + imx_media_enum_ipu_format(&code, 0, CS_SEL_ANY); + *cc = imx_media_find_ipu_format(code, CS_SEL_ANY); + sdformat->format.code = (*cc)->codes[0]; + } + + infmt = __prp_get_fmt(priv, cfg, PRPENCVF_SINK_PAD, sdformat->which); + + if (sdformat->pad == PRPENCVF_SRC_PAD) { + if (sdformat->format.field != V4L2_FIELD_NONE) + sdformat->format.field = infmt->field; + + prp_bound_align_output(&sdformat->format, infmt, + priv->rot_mode); + + /* propagate colorimetry from sink */ + sdformat->format.colorspace = infmt->colorspace; + sdformat->format.xfer_func = infmt->xfer_func; + sdformat->format.quantization = infmt->quantization; + sdformat->format.ycbcr_enc = infmt->ycbcr_enc; + } else { + v4l_bound_align_image(&sdformat->format.width, + MIN_W_SINK, MAX_W_SINK, W_ALIGN_SINK, + &sdformat->format.height, + MIN_H_SINK, MAX_H_SINK, H_ALIGN_SINK, + S_ALIGN); + + imx_media_fill_default_mbus_fields(&sdformat->format, infmt, + true); + } +} + +static int prp_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct prp_priv *priv = sd_to_priv(sd); + struct imx_media_video_dev *vdev = priv->vdev; + const struct imx_media_pixfmt *cc; + struct v4l2_pix_format vdev_fmt; + struct v4l2_mbus_framefmt *fmt; + int ret = 0; + + if (sdformat->pad >= PRPENCVF_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + + if (priv->stream_count > 0) { + ret = -EBUSY; + goto out; + } + + prp_try_fmt(priv, cfg, sdformat, &cc); + + fmt = __prp_get_fmt(priv, cfg, sdformat->pad, sdformat->which); + *fmt = sdformat->format; + + /* propagate a default format to source pad */ + if (sdformat->pad == PRPENCVF_SINK_PAD) { + const struct imx_media_pixfmt *outcc; + struct v4l2_mbus_framefmt *outfmt; + struct v4l2_subdev_format format; + + format.pad = PRPENCVF_SRC_PAD; + format.which = sdformat->which; + format.format = sdformat->format; + prp_try_fmt(priv, cfg, &format, &outcc); + + outfmt = __prp_get_fmt(priv, cfg, PRPENCVF_SRC_PAD, + sdformat->which); + *outfmt = format.format; + if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE) + priv->cc[PRPENCVF_SRC_PAD] = outcc; + } + + if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) + goto out; + + priv->cc[sdformat->pad] = cc; + + /* propagate output pad format to capture device */ + imx_media_mbus_fmt_to_pix_fmt(&vdev_fmt, + &priv->format_mbus[PRPENCVF_SRC_PAD], + priv->cc[PRPENCVF_SRC_PAD]); + mutex_unlock(&priv->lock); + imx_media_capture_device_set_format(vdev, &vdev_fmt); + + return 0; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_enum_frame_size(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_frame_size_enum *fse) +{ + struct prp_priv *priv = sd_to_priv(sd); + struct v4l2_subdev_format format = {0}; + const struct imx_media_pixfmt *cc; + int ret = 0; + + if (fse->pad >= PRPENCVF_NUM_PADS || fse->index != 0) + return -EINVAL; + + mutex_lock(&priv->lock); + + format.pad = fse->pad; + format.which = fse->which; + format.format.code = fse->code; + format.format.width = 1; + format.format.height = 1; + prp_try_fmt(priv, cfg, &format, &cc); + fse->min_width = format.format.width; + fse->min_height = format.format.height; + + if (format.format.code != fse->code) { + ret = -EINVAL; + goto out; + } + + format.format.code = fse->code; + format.format.width = -1; + format.format.height = -1; + prp_try_fmt(priv, cfg, &format, &cc); + fse->max_width = format.format.width; + fse->max_height = format.format.height; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_link_setup(struct media_entity *entity, + const struct media_pad *local, + const struct media_pad *remote, u32 flags) +{ + struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); + struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd); + struct prp_priv *priv = ic_priv->task_priv; + struct v4l2_subdev *remote_sd; + int ret = 0; + + dev_dbg(ic_priv->dev, "link setup %s -> %s", remote->entity->name, + local->entity->name); + + mutex_lock(&priv->lock); + + if (local->flags & MEDIA_PAD_FL_SINK) { + if (!is_media_entity_v4l2_subdev(remote->entity)) { + ret = -EINVAL; + goto out; + } + + remote_sd = media_entity_to_v4l2_subdev(remote->entity); + + if (flags & MEDIA_LNK_FL_ENABLED) { + if (priv->src_sd) { + ret = -EBUSY; + goto out; + } + priv->src_sd = remote_sd; + } else { + priv->src_sd = NULL; + } + + goto out; + } + + /* this is the source pad */ + + /* the remote must be the device node */ + if (!is_media_entity_v4l2_video_device(remote->entity)) { + ret = -EINVAL; + goto out; + } + + if (flags & MEDIA_LNK_FL_ENABLED) { + if (priv->sink) { + ret = -EBUSY; + goto out; + } + } else { + priv->sink = NULL; + goto out; + } + + priv->sink = remote->entity; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_s_ctrl(struct v4l2_ctrl *ctrl) +{ + struct prp_priv *priv = container_of(ctrl->handler, + struct prp_priv, ctrl_hdlr); + struct imx_ic_priv *ic_priv = priv->ic_priv; + enum ipu_rotate_mode rot_mode; + int rotation, ret = 0; + bool hflip, vflip; + + mutex_lock(&priv->lock); + + rotation = priv->rotation; + hflip = priv->hflip; + vflip = priv->vflip; + + switch (ctrl->id) { + case V4L2_CID_HFLIP: + hflip = (ctrl->val == 1); + break; + case V4L2_CID_VFLIP: + vflip = (ctrl->val == 1); + break; + case V4L2_CID_ROTATE: + rotation = ctrl->val; + break; + default: + v4l2_err(&ic_priv->sd, "Invalid control\n"); + ret = -EINVAL; + goto out; + } + + ret = ipu_degrees_to_rot_mode(&rot_mode, rotation, hflip, vflip); + if (ret) + goto out; + + if (rot_mode != priv->rot_mode) { + struct v4l2_mbus_framefmt outfmt, infmt; + + /* can't change rotation mid-streaming */ + if (priv->stream_count > 0) { + ret = -EBUSY; + goto out; + } + + outfmt = priv->format_mbus[PRPENCVF_SRC_PAD]; + infmt = priv->format_mbus[PRPENCVF_SINK_PAD]; + + if (prp_bound_align_output(&outfmt, &infmt, rot_mode)) { + ret = -EINVAL; + goto out; + } + + priv->rot_mode = rot_mode; + priv->rotation = rotation; + priv->hflip = hflip; + priv->vflip = vflip; + } + +out: + mutex_unlock(&priv->lock); + return ret; +} + +static const struct v4l2_ctrl_ops prp_ctrl_ops = { + .s_ctrl = prp_s_ctrl, +}; + +static int prp_init_controls(struct prp_priv *priv) +{ + struct imx_ic_priv *ic_priv = priv->ic_priv; + struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr; + int ret; + + v4l2_ctrl_handler_init(hdlr, 3); + + v4l2_ctrl_new_std(hdlr, &prp_ctrl_ops, V4L2_CID_HFLIP, + 0, 1, 1, 0); + v4l2_ctrl_new_std(hdlr, &prp_ctrl_ops, V4L2_CID_VFLIP, + 0, 1, 1, 0); + v4l2_ctrl_new_std(hdlr, &prp_ctrl_ops, V4L2_CID_ROTATE, + 0, 270, 90, 0); + + ic_priv->sd.ctrl_handler = hdlr; + + if (hdlr->error) { + ret = hdlr->error; + goto out_free; + } + + v4l2_ctrl_handler_setup(hdlr); + return 0; + +out_free: + v4l2_ctrl_handler_free(hdlr); + return ret; +} + +static int prp_s_stream(struct v4l2_subdev *sd, int enable) +{ + struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd); + struct prp_priv *priv = ic_priv->task_priv; + int ret = 0; + + mutex_lock(&priv->lock); + + if (!priv->src_sd || !priv->sink) { + ret = -EPIPE; + goto out; + } + + /* + * enable/disable streaming only if stream_count is + * going from 0 to 1 / 1 to 0. + */ + if (priv->stream_count != !enable) + goto update_count; + + dev_dbg(ic_priv->dev, "stream %s\n", enable ? "ON" : "OFF"); + + if (enable) + ret = prp_start(priv); + else + prp_stop(priv); + if (ret) + goto out; + + /* start/stop upstream */ + ret = v4l2_subdev_call(priv->src_sd, video, s_stream, enable); + ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0; + if (ret) { + if (enable) + prp_stop(priv); + goto out; + } + +update_count: + priv->stream_count += enable ? 1 : -1; + if (priv->stream_count < 0) + priv->stream_count = 0; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int prp_g_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct prp_priv *priv = sd_to_priv(sd); + + if (fi->pad >= PRPENCVF_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + fi->interval = priv->frame_interval; + mutex_unlock(&priv->lock); + + return 0; +} + +static int prp_s_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct prp_priv *priv = sd_to_priv(sd); + + if (fi->pad >= PRPENCVF_NUM_PADS) + return -EINVAL; + + /* No limits on frame interval */ + mutex_lock(&priv->lock); + priv->frame_interval = fi->interval; + mutex_unlock(&priv->lock); + + return 0; +} + +/* + * retrieve our pads parsed from the OF graph by the media device + */ +static int prp_registered(struct v4l2_subdev *sd) +{ + struct prp_priv *priv = sd_to_priv(sd); + int i, ret; + u32 code; + + /* get media device */ + priv->md = dev_get_drvdata(sd->v4l2_dev->dev); + + for (i = 0; i < PRPENCVF_NUM_PADS; i++) { + priv->pad[i].flags = (i == PRPENCVF_SINK_PAD) ? + MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE; + + /* set a default mbus format */ + imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV); + ret = imx_media_init_mbus_fmt(&priv->format_mbus[i], + 640, 480, code, V4L2_FIELD_NONE, + &priv->cc[i]); + if (ret) + return ret; + } + + /* init default frame interval */ + priv->frame_interval.numerator = 1; + priv->frame_interval.denominator = 30; + + ret = media_entity_pads_init(&sd->entity, PRPENCVF_NUM_PADS, + priv->pad); + if (ret) + return ret; + + ret = imx_media_capture_device_register(priv->vdev); + if (ret) + return ret; + + ret = imx_media_add_video_device(priv->md, priv->vdev); + if (ret) + goto unreg; + + ret = prp_init_controls(priv); + if (ret) + goto unreg; + + return 0; +unreg: + imx_media_capture_device_unregister(priv->vdev); + return ret; +} + +static void prp_unregistered(struct v4l2_subdev *sd) +{ + struct prp_priv *priv = sd_to_priv(sd); + + imx_media_capture_device_unregister(priv->vdev); + v4l2_ctrl_handler_free(&priv->ctrl_hdlr); +} + +static const struct v4l2_subdev_pad_ops prp_pad_ops = { + .enum_mbus_code = prp_enum_mbus_code, + .enum_frame_size = prp_enum_frame_size, + .get_fmt = prp_get_fmt, + .set_fmt = prp_set_fmt, +}; + +static const struct v4l2_subdev_video_ops prp_video_ops = { + .g_frame_interval = prp_g_frame_interval, + .s_frame_interval = prp_s_frame_interval, + .s_stream = prp_s_stream, +}; + +static const struct media_entity_operations prp_entity_ops = { + .link_setup = prp_link_setup, + .link_validate = v4l2_subdev_link_validate, +}; + +static const struct v4l2_subdev_ops prp_subdev_ops = { + .video = &prp_video_ops, + .pad = &prp_pad_ops, +}; + +static const struct v4l2_subdev_internal_ops prp_internal_ops = { + .registered = prp_registered, + .unregistered = prp_unregistered, +}; + +static int prp_init(struct imx_ic_priv *ic_priv) +{ + struct prp_priv *priv; + + priv = devm_kzalloc(ic_priv->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + ic_priv->task_priv = priv; + priv->ic_priv = ic_priv; + + spin_lock_init(&priv->irqlock); + init_timer(&priv->eof_timeout_timer); + priv->eof_timeout_timer.data = (unsigned long)priv; + priv->eof_timeout_timer.function = prp_eof_timeout; + + priv->vdev = imx_media_capture_device_init(&ic_priv->sd, + PRPENCVF_SRC_PAD); + if (IS_ERR(priv->vdev)) + return PTR_ERR(priv->vdev); + + mutex_init(&priv->lock); + + return 0; +} + +static void prp_remove(struct imx_ic_priv *ic_priv) +{ + struct prp_priv *priv = ic_priv->task_priv; + + mutex_destroy(&priv->lock); + imx_media_capture_device_remove(priv->vdev); +} + +struct imx_ic_ops imx_ic_prpencvf_ops = { + .subdev_ops = &prp_subdev_ops, + .internal_ops = &prp_internal_ops, + .entity_ops = &prp_entity_ops, + .init = prp_init, + .remove = prp_remove, +}; diff --git a/drivers/staging/media/imx/imx-ic.h b/drivers/staging/media/imx/imx-ic.h new file mode 100644 index 000000000000..6b2267bda8ab --- /dev/null +++ b/drivers/staging/media/imx/imx-ic.h @@ -0,0 +1,38 @@ +/* + * V4L2 Image Converter Subdev for Freescale i.MX5/6 SOC + * + * Copyright (c) 2016 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#ifndef _IMX_IC_H +#define _IMX_IC_H + +#include <media/v4l2-subdev.h> + +struct imx_ic_priv { + struct device *dev; + struct v4l2_subdev sd; + int ipu_id; + int task_id; + void *prp_priv; + void *task_priv; +}; + +struct imx_ic_ops { + const struct v4l2_subdev_ops *subdev_ops; + const struct v4l2_subdev_internal_ops *internal_ops; + const struct media_entity_operations *entity_ops; + + int (*init)(struct imx_ic_priv *ic_priv); + void (*remove)(struct imx_ic_priv *ic_priv); +}; + +extern struct imx_ic_ops imx_ic_prp_ops; +extern struct imx_ic_ops imx_ic_prpencvf_ops; +extern struct imx_ic_ops imx_ic_pp_ops; + +#endif diff --git a/drivers/staging/media/imx/imx-media-capture.c b/drivers/staging/media/imx/imx-media-capture.c new file mode 100644 index 000000000000..ddab4c249da2 --- /dev/null +++ b/drivers/staging/media/imx/imx-media-capture.c @@ -0,0 +1,775 @@ +/* + * Video Capture Subdev for Freescale i.MX5/6 SOC + * + * Copyright (c) 2012-2016 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/delay.h> +#include <linux/fs.h> +#include <linux/module.h> +#include <linux/of_platform.h> +#include <linux/pinctrl/consumer.h> +#include <linux/platform_device.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/spinlock.h> +#include <linux/timer.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-event.h> +#include <media/v4l2-fwnode.h> +#include <media/v4l2-ioctl.h> +#include <media/v4l2-mc.h> +#include <media/v4l2-subdev.h> +#include <media/videobuf2-dma-contig.h> +#include <video/imx-ipu-v3.h> +#include <media/imx.h> +#include "imx-media.h" + +struct capture_priv { + struct imx_media_video_dev vdev; + + struct v4l2_subdev *src_sd; + int src_sd_pad; + struct device *dev; + + struct imx_media_dev *md; + + struct media_pad vdev_pad; + + struct mutex mutex; /* capture device mutex */ + + /* the videobuf2 queue */ + struct vb2_queue q; + /* list of ready imx_media_buffer's from q */ + struct list_head ready_q; + /* protect ready_q */ + spinlock_t q_lock; + + /* controls inherited from subdevs */ + struct v4l2_ctrl_handler ctrl_hdlr; + + /* misc status */ + bool stop; /* streaming is stopping */ +}; + +#define to_capture_priv(v) container_of(v, struct capture_priv, vdev) + +/* In bytes, per queue */ +#define VID_MEM_LIMIT SZ_64M + +static struct vb2_ops capture_qops; + +/* + * Video ioctls follow + */ + +static int vidioc_querycap(struct file *file, void *fh, + struct v4l2_capability *cap) +{ + struct capture_priv *priv = video_drvdata(file); + + strncpy(cap->driver, "imx-media-capture", sizeof(cap->driver) - 1); + strncpy(cap->card, "imx-media-capture", sizeof(cap->card) - 1); + snprintf(cap->bus_info, sizeof(cap->bus_info), + "platform:%s", priv->src_sd->name); + + return 0; +} + +static int capture_enum_framesizes(struct file *file, void *fh, + struct v4l2_frmsizeenum *fsize) +{ + struct capture_priv *priv = video_drvdata(file); + const struct imx_media_pixfmt *cc; + struct v4l2_subdev_frame_size_enum fse = { + .index = fsize->index, + .pad = priv->src_sd_pad, + .which = V4L2_SUBDEV_FORMAT_ACTIVE, + }; + int ret; + + cc = imx_media_find_format(fsize->pixel_format, CS_SEL_ANY, true); + if (!cc) + return -EINVAL; + + fse.code = cc->codes[0]; + + ret = v4l2_subdev_call(priv->src_sd, pad, enum_frame_size, NULL, &fse); + if (ret) + return ret; + + if (fse.min_width == fse.max_width && + fse.min_height == fse.max_height) { + fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE; + fsize->discrete.width = fse.min_width; + fsize->discrete.height = fse.min_height; + } else { + fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS; + fsize->stepwise.min_width = fse.min_width; + fsize->stepwise.max_width = fse.max_width; + fsize->stepwise.min_height = fse.min_height; + fsize->stepwise.max_height = fse.max_height; + fsize->stepwise.step_width = 1; + fsize->stepwise.step_height = 1; + } + + return 0; +} + +static int capture_enum_frameintervals(struct file *file, void *fh, + struct v4l2_frmivalenum *fival) +{ + struct capture_priv *priv = video_drvdata(file); + const struct imx_media_pixfmt *cc; + struct v4l2_subdev_frame_interval_enum fie = { + .index = fival->index, + .pad = priv->src_sd_pad, + .width = fival->width, + .height = fival->height, + .which = V4L2_SUBDEV_FORMAT_ACTIVE, + }; + int ret; + + cc = imx_media_find_format(fival->pixel_format, CS_SEL_ANY, true); + if (!cc) + return -EINVAL; + + fie.code = cc->codes[0]; + + ret = v4l2_subdev_call(priv->src_sd, pad, enum_frame_interval, NULL, &fie); + if (ret) + return ret; + + fival->type = V4L2_FRMIVAL_TYPE_DISCRETE; + fival->discrete = fie.interval; + + return 0; +} + +static int capture_enum_fmt_vid_cap(struct file *file, void *fh, + struct v4l2_fmtdesc *f) +{ + struct capture_priv *priv = video_drvdata(file); + const struct imx_media_pixfmt *cc_src; + struct v4l2_subdev_format fmt_src; + u32 fourcc; + int ret; + + fmt_src.pad = priv->src_sd_pad; + fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE; + ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src); + if (ret) { + v4l2_err(priv->src_sd, "failed to get src_sd format\n"); + return ret; + } + + cc_src = imx_media_find_ipu_format(fmt_src.format.code, CS_SEL_ANY); + if (!cc_src) + cc_src = imx_media_find_mbus_format(fmt_src.format.code, + CS_SEL_ANY, true); + if (!cc_src) + return -EINVAL; + + if (cc_src->bayer) { + if (f->index != 0) + return -EINVAL; + fourcc = cc_src->fourcc; + } else { + u32 cs_sel = (cc_src->cs == IPUV3_COLORSPACE_YUV) ? + CS_SEL_YUV : CS_SEL_RGB; + + ret = imx_media_enum_format(&fourcc, f->index, cs_sel); + if (ret) + return ret; + } + + f->pixelformat = fourcc; + + return 0; +} + +static int capture_g_fmt_vid_cap(struct file *file, void *fh, + struct v4l2_format *f) +{ + struct capture_priv *priv = video_drvdata(file); + + *f = priv->vdev.fmt; + + return 0; +} + +static int capture_try_fmt_vid_cap(struct file *file, void *fh, + struct v4l2_format *f) +{ + struct capture_priv *priv = video_drvdata(file); + struct v4l2_subdev_format fmt_src; + const struct imx_media_pixfmt *cc, *cc_src; + int ret; + + fmt_src.pad = priv->src_sd_pad; + fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE; + ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src); + if (ret) + return ret; + + cc_src = imx_media_find_ipu_format(fmt_src.format.code, CS_SEL_ANY); + if (!cc_src) + cc_src = imx_media_find_mbus_format(fmt_src.format.code, + CS_SEL_ANY, true); + if (!cc_src) + return -EINVAL; + + if (cc_src->bayer) { + cc = cc_src; + } else { + u32 fourcc, cs_sel; + + cs_sel = (cc_src->cs == IPUV3_COLORSPACE_YUV) ? + CS_SEL_YUV : CS_SEL_RGB; + fourcc = f->fmt.pix.pixelformat; + + cc = imx_media_find_format(fourcc, cs_sel, false); + if (!cc) { + imx_media_enum_format(&fourcc, 0, cs_sel); + cc = imx_media_find_format(fourcc, cs_sel, false); + } + } + + imx_media_mbus_fmt_to_pix_fmt(&f->fmt.pix, &fmt_src.format, cc); + + return 0; +} + +static int capture_s_fmt_vid_cap(struct file *file, void *fh, + struct v4l2_format *f) +{ + struct capture_priv *priv = video_drvdata(file); + int ret; + + if (vb2_is_busy(&priv->q)) { + v4l2_err(priv->src_sd, "%s queue busy\n", __func__); + return -EBUSY; + } + + ret = capture_try_fmt_vid_cap(file, priv, f); + if (ret) + return ret; + + priv->vdev.fmt.fmt.pix = f->fmt.pix; + priv->vdev.cc = imx_media_find_format(f->fmt.pix.pixelformat, + CS_SEL_ANY, true); + + return 0; +} + +static int capture_querystd(struct file *file, void *fh, v4l2_std_id *std) +{ + struct capture_priv *priv = video_drvdata(file); + + return v4l2_subdev_call(priv->src_sd, video, querystd, std); +} + +static int capture_g_std(struct file *file, void *fh, v4l2_std_id *std) +{ + struct capture_priv *priv = video_drvdata(file); + + return v4l2_subdev_call(priv->src_sd, video, g_std, std); +} + +static int capture_s_std(struct file *file, void *fh, v4l2_std_id std) +{ + struct capture_priv *priv = video_drvdata(file); + + if (vb2_is_busy(&priv->q)) + return -EBUSY; + + return v4l2_subdev_call(priv->src_sd, video, s_std, std); +} + +static int capture_g_parm(struct file *file, void *fh, + struct v4l2_streamparm *a) +{ + struct capture_priv *priv = video_drvdata(file); + struct v4l2_subdev_frame_interval fi; + int ret; + + if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) + return -EINVAL; + + memset(&fi, 0, sizeof(fi)); + fi.pad = priv->src_sd_pad; + ret = v4l2_subdev_call(priv->src_sd, video, g_frame_interval, &fi); + if (ret < 0) + return ret; + + a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME; + a->parm.capture.timeperframe = fi.interval; + + return 0; +} + +static int capture_s_parm(struct file *file, void *fh, + struct v4l2_streamparm *a) +{ + struct capture_priv *priv = video_drvdata(file); + struct v4l2_subdev_frame_interval fi; + int ret; + + if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) + return -EINVAL; + + memset(&fi, 0, sizeof(fi)); + fi.pad = priv->src_sd_pad; + fi.interval = a->parm.capture.timeperframe; + ret = v4l2_subdev_call(priv->src_sd, video, s_frame_interval, &fi); + if (ret < 0) + return ret; + + a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME; + a->parm.capture.timeperframe = fi.interval; + + return 0; +} + +static const struct v4l2_ioctl_ops capture_ioctl_ops = { + .vidioc_querycap = vidioc_querycap, + + .vidioc_enum_framesizes = capture_enum_framesizes, + .vidioc_enum_frameintervals = capture_enum_frameintervals, + + .vidioc_enum_fmt_vid_cap = capture_enum_fmt_vid_cap, + .vidioc_g_fmt_vid_cap = capture_g_fmt_vid_cap, + .vidioc_try_fmt_vid_cap = capture_try_fmt_vid_cap, + .vidioc_s_fmt_vid_cap = capture_s_fmt_vid_cap, + + .vidioc_querystd = capture_querystd, + .vidioc_g_std = capture_g_std, + .vidioc_s_std = capture_s_std, + + .vidioc_g_parm = capture_g_parm, + .vidioc_s_parm = capture_s_parm, + + .vidioc_reqbufs = vb2_ioctl_reqbufs, + .vidioc_create_bufs = vb2_ioctl_create_bufs, + .vidioc_prepare_buf = vb2_ioctl_prepare_buf, + .vidioc_querybuf = vb2_ioctl_querybuf, + .vidioc_qbuf = vb2_ioctl_qbuf, + .vidioc_dqbuf = vb2_ioctl_dqbuf, + .vidioc_expbuf = vb2_ioctl_expbuf, + .vidioc_streamon = vb2_ioctl_streamon, + .vidioc_streamoff = vb2_ioctl_streamoff, +}; + +/* + * Queue operations + */ + +static int capture_queue_setup(struct vb2_queue *vq, + unsigned int *nbuffers, + unsigned int *nplanes, + unsigned int sizes[], + struct device *alloc_devs[]) +{ + struct capture_priv *priv = vb2_get_drv_priv(vq); + struct v4l2_pix_format *pix = &priv->vdev.fmt.fmt.pix; + unsigned int count = *nbuffers; + + if (vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) + return -EINVAL; + + if (*nplanes) { + if (*nplanes != 1 || sizes[0] < pix->sizeimage) + return -EINVAL; + count += vq->num_buffers; + } + + count = min_t(__u32, VID_MEM_LIMIT / pix->sizeimage, count); + + if (*nplanes) + *nbuffers = (count < vq->num_buffers) ? 0 : + count - vq->num_buffers; + else + *nbuffers = count; + + *nplanes = 1; + sizes[0] = pix->sizeimage; + + return 0; +} + +static int capture_buf_init(struct vb2_buffer *vb) +{ + struct imx_media_buffer *buf = to_imx_media_vb(vb); + + INIT_LIST_HEAD(&buf->list); + + return 0; +} + +static int capture_buf_prepare(struct vb2_buffer *vb) +{ + struct vb2_queue *vq = vb->vb2_queue; + struct capture_priv *priv = vb2_get_drv_priv(vq); + struct v4l2_pix_format *pix = &priv->vdev.fmt.fmt.pix; + + if (vb2_plane_size(vb, 0) < pix->sizeimage) { + v4l2_err(priv->src_sd, + "data will not fit into plane (%lu < %lu)\n", + vb2_plane_size(vb, 0), (long)pix->sizeimage); + return -EINVAL; + } + + vb2_set_plane_payload(vb, 0, pix->sizeimage); + + return 0; +} + +static void capture_buf_queue(struct vb2_buffer *vb) +{ + struct capture_priv *priv = vb2_get_drv_priv(vb->vb2_queue); + struct imx_media_buffer *buf = to_imx_media_vb(vb); + unsigned long flags; + + spin_lock_irqsave(&priv->q_lock, flags); + + list_add_tail(&buf->list, &priv->ready_q); + + spin_unlock_irqrestore(&priv->q_lock, flags); +} + +static int capture_start_streaming(struct vb2_queue *vq, unsigned int count) +{ + struct capture_priv *priv = vb2_get_drv_priv(vq); + struct imx_media_buffer *buf, *tmp; + unsigned long flags; + int ret; + + if (vb2_is_streaming(vq)) + return 0; + + ret = imx_media_pipeline_set_stream(priv->md, &priv->src_sd->entity, + true); + if (ret) { + v4l2_err(priv->src_sd, "pipeline start failed with %d\n", ret); + goto return_bufs; + } + + priv->stop = false; + + return 0; + +return_bufs: + spin_lock_irqsave(&priv->q_lock, flags); + list_for_each_entry_safe(buf, tmp, &priv->ready_q, list) { + list_del(&buf->list); + vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_QUEUED); + } + spin_unlock_irqrestore(&priv->q_lock, flags); + return ret; +} + +static void capture_stop_streaming(struct vb2_queue *vq) +{ + struct capture_priv *priv = vb2_get_drv_priv(vq); + struct imx_media_buffer *frame; + unsigned long flags; + int ret; + + if (!vb2_is_streaming(vq)) + return; + + spin_lock_irqsave(&priv->q_lock, flags); + priv->stop = true; + spin_unlock_irqrestore(&priv->q_lock, flags); + + ret = imx_media_pipeline_set_stream(priv->md, &priv->src_sd->entity, + false); + if (ret) + v4l2_warn(priv->src_sd, "pipeline stop failed with %d\n", ret); + + /* release all active buffers */ + spin_lock_irqsave(&priv->q_lock, flags); + while (!list_empty(&priv->ready_q)) { + frame = list_entry(priv->ready_q.next, + struct imx_media_buffer, list); + list_del(&frame->list); + vb2_buffer_done(&frame->vbuf.vb2_buf, VB2_BUF_STATE_ERROR); + } + spin_unlock_irqrestore(&priv->q_lock, flags); +} + +static struct vb2_ops capture_qops = { + .queue_setup = capture_queue_setup, + .buf_init = capture_buf_init, + .buf_prepare = capture_buf_prepare, + .buf_queue = capture_buf_queue, + .wait_prepare = vb2_ops_wait_prepare, + .wait_finish = vb2_ops_wait_finish, + .start_streaming = capture_start_streaming, + .stop_streaming = capture_stop_streaming, +}; + +/* + * File operations + */ +static int capture_open(struct file *file) +{ + struct capture_priv *priv = video_drvdata(file); + struct video_device *vfd = priv->vdev.vfd; + int ret; + + if (mutex_lock_interruptible(&priv->mutex)) + return -ERESTARTSYS; + + ret = v4l2_fh_open(file); + if (ret) + v4l2_err(priv->src_sd, "v4l2_fh_open failed\n"); + + ret = v4l2_pipeline_pm_use(&vfd->entity, 1); + if (ret) + v4l2_fh_release(file); + + mutex_unlock(&priv->mutex); + return ret; +} + +static int capture_release(struct file *file) +{ + struct capture_priv *priv = video_drvdata(file); + struct video_device *vfd = priv->vdev.vfd; + struct vb2_queue *vq = &priv->q; + int ret = 0; + + mutex_lock(&priv->mutex); + + if (file->private_data == vq->owner) { + vb2_queue_release(vq); + vq->owner = NULL; + } + + v4l2_pipeline_pm_use(&vfd->entity, 0); + + v4l2_fh_release(file); + mutex_unlock(&priv->mutex); + return ret; +} + +static const struct v4l2_file_operations capture_fops = { + .owner = THIS_MODULE, + .open = capture_open, + .release = capture_release, + .poll = vb2_fop_poll, + .unlocked_ioctl = video_ioctl2, + .mmap = vb2_fop_mmap, +}; + +static struct video_device capture_videodev = { + .fops = &capture_fops, + .ioctl_ops = &capture_ioctl_ops, + .minor = -1, + .release = video_device_release, + .vfl_dir = VFL_DIR_RX, + .tvnorms = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM, + .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING, +}; + +void imx_media_capture_device_set_format(struct imx_media_video_dev *vdev, + struct v4l2_pix_format *pix) +{ + struct capture_priv *priv = to_capture_priv(vdev); + + mutex_lock(&priv->mutex); + priv->vdev.fmt.fmt.pix = *pix; + priv->vdev.cc = imx_media_find_format(pix->pixelformat, CS_SEL_ANY, + true); + mutex_unlock(&priv->mutex); +} +EXPORT_SYMBOL_GPL(imx_media_capture_device_set_format); + +struct imx_media_buffer * +imx_media_capture_device_next_buf(struct imx_media_video_dev *vdev) +{ + struct capture_priv *priv = to_capture_priv(vdev); + struct imx_media_buffer *buf = NULL; + unsigned long flags; + + spin_lock_irqsave(&priv->q_lock, flags); + + /* get next queued buffer */ + if (!list_empty(&priv->ready_q)) { + buf = list_entry(priv->ready_q.next, struct imx_media_buffer, + list); + list_del(&buf->list); + } + + spin_unlock_irqrestore(&priv->q_lock, flags); + + return buf; +} +EXPORT_SYMBOL_GPL(imx_media_capture_device_next_buf); + +void imx_media_capture_device_error(struct imx_media_video_dev *vdev) +{ + struct capture_priv *priv = to_capture_priv(vdev); + struct vb2_queue *vq = &priv->q; + unsigned long flags; + + if (!vb2_is_streaming(vq)) + return; + + spin_lock_irqsave(&priv->q_lock, flags); + vb2_queue_error(vq); + spin_unlock_irqrestore(&priv->q_lock, flags); +} +EXPORT_SYMBOL_GPL(imx_media_capture_device_error); + +int imx_media_capture_device_register(struct imx_media_video_dev *vdev) +{ + struct capture_priv *priv = to_capture_priv(vdev); + struct v4l2_subdev *sd = priv->src_sd; + struct video_device *vfd = vdev->vfd; + struct vb2_queue *vq = &priv->q; + struct v4l2_subdev_format fmt_src; + int ret; + + /* get media device */ + priv->md = dev_get_drvdata(sd->v4l2_dev->dev); + + vfd->v4l2_dev = sd->v4l2_dev; + + ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1); + if (ret) { + v4l2_err(sd, "Failed to register video device\n"); + return ret; + } + + vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + vq->io_modes = VB2_MMAP | VB2_DMABUF; + vq->drv_priv = priv; + vq->buf_struct_size = sizeof(struct imx_media_buffer); + vq->ops = &capture_qops; + vq->mem_ops = &vb2_dma_contig_memops; + vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; + vq->lock = &priv->mutex; + vq->min_buffers_needed = 2; + vq->dev = priv->dev; + + ret = vb2_queue_init(vq); + if (ret) { + v4l2_err(sd, "vb2_queue_init failed\n"); + goto unreg; + } + + INIT_LIST_HEAD(&priv->ready_q); + + priv->vdev_pad.flags = MEDIA_PAD_FL_SINK; + ret = media_entity_pads_init(&vfd->entity, 1, &priv->vdev_pad); + if (ret) { + v4l2_err(sd, "failed to init dev pad\n"); + goto unreg; + } + + /* create the link from the src_sd devnode pad to device node */ + ret = media_create_pad_link(&sd->entity, priv->src_sd_pad, + &vfd->entity, 0, 0); + if (ret) { + v4l2_err(sd, "failed to create link to device node\n"); + goto unreg; + } + + /* setup default format */ + fmt_src.pad = priv->src_sd_pad; + fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE; + v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt_src); + if (ret) { + v4l2_err(sd, "failed to get src_sd format\n"); + goto unreg; + } + + vdev->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + imx_media_mbus_fmt_to_pix_fmt(&vdev->fmt.fmt.pix, + &fmt_src.format, NULL); + vdev->cc = imx_media_find_format(vdev->fmt.fmt.pix.pixelformat, + CS_SEL_ANY, false); + + v4l2_info(sd, "Registered %s as /dev/%s\n", vfd->name, + video_device_node_name(vfd)); + + vfd->ctrl_handler = &priv->ctrl_hdlr; + + return 0; +unreg: + video_unregister_device(vfd); + return ret; +} +EXPORT_SYMBOL_GPL(imx_media_capture_device_register); + +void imx_media_capture_device_unregister(struct imx_media_video_dev *vdev) +{ + struct capture_priv *priv = to_capture_priv(vdev); + struct video_device *vfd = priv->vdev.vfd; + + mutex_lock(&priv->mutex); + + if (video_is_registered(vfd)) { + video_unregister_device(vfd); + media_entity_cleanup(&vfd->entity); + } + + mutex_unlock(&priv->mutex); +} +EXPORT_SYMBOL_GPL(imx_media_capture_device_unregister); + +struct imx_media_video_dev * +imx_media_capture_device_init(struct v4l2_subdev *src_sd, int pad) +{ + struct capture_priv *priv; + struct video_device *vfd; + + priv = devm_kzalloc(src_sd->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return ERR_PTR(-ENOMEM); + + priv->src_sd = src_sd; + priv->src_sd_pad = pad; + priv->dev = src_sd->dev; + + mutex_init(&priv->mutex); + spin_lock_init(&priv->q_lock); + + snprintf(capture_videodev.name, sizeof(capture_videodev.name), + "%s capture", src_sd->name); + + vfd = video_device_alloc(); + if (!vfd) + return ERR_PTR(-ENOMEM); + + *vfd = capture_videodev; + vfd->lock = &priv->mutex; + vfd->queue = &priv->q; + priv->vdev.vfd = vfd; + + video_set_drvdata(vfd, priv); + + v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0); + + return &priv->vdev; +} +EXPORT_SYMBOL_GPL(imx_media_capture_device_init); + +void imx_media_capture_device_remove(struct imx_media_video_dev *vdev) +{ + struct capture_priv *priv = to_capture_priv(vdev); + + v4l2_ctrl_handler_free(&priv->ctrl_hdlr); +} +EXPORT_SYMBOL_GPL(imx_media_capture_device_remove); + +MODULE_DESCRIPTION("i.MX5/6 v4l2 video capture interface driver"); +MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>"); +MODULE_LICENSE("GPL"); diff --git a/drivers/staging/media/imx/imx-media-csi.c b/drivers/staging/media/imx/imx-media-csi.c new file mode 100644 index 000000000000..a2d26693912e --- /dev/null +++ b/drivers/staging/media/imx/imx-media-csi.c @@ -0,0 +1,1817 @@ +/* + * V4L2 Capture CSI Subdev for Freescale i.MX5/6 SOC + * + * Copyright (c) 2014-2017 Mentor Graphics Inc. + * Copyright (C) 2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/delay.h> +#include <linux/gcd.h> +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/pinctrl/consumer.h> +#include <linux/platform_device.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-event.h> +#include <media/v4l2-fwnode.h> +#include <media/v4l2-mc.h> +#include <media/v4l2-subdev.h> +#include <media/videobuf2-dma-contig.h> +#include <video/imx-ipu-v3.h> +#include <media/imx.h> +#include "imx-media.h" + +/* + * Min/Max supported width and heights. + * + * We allow planar output, so we have to align width by 16 pixels + * to meet IDMAC alignment requirements. + * + * TODO: move this into pad format negotiation, if capture device + * has not requested planar formats, we should allow 8 pixel + * alignment. + */ +#define MIN_W 176 +#define MIN_H 144 +#define MAX_W 4096 +#define MAX_H 4096 +#define W_ALIGN 4 /* multiple of 16 pixels */ +#define H_ALIGN 1 /* multiple of 2 lines */ +#define S_ALIGN 1 /* multiple of 2 */ + +/* + * struct csi_skip_desc - CSI frame skipping descriptor + * @keep - number of frames kept per max_ratio frames + * @max_ratio - width of skip_smfc, written to MAX_RATIO bitfield + * @skip_smfc - skip pattern written to the SKIP_SMFC bitfield + */ +struct csi_skip_desc { + u8 keep; + u8 max_ratio; + u8 skip_smfc; +}; + +struct csi_priv { + struct device *dev; + struct ipu_soc *ipu; + struct imx_media_dev *md; + struct v4l2_subdev sd; + struct media_pad pad[CSI_NUM_PADS]; + /* the video device at IDMAC output pad */ + struct imx_media_video_dev *vdev; + struct imx_media_fim *fim; + int csi_id; + int smfc_id; + + /* lock to protect all members below */ + struct mutex lock; + + int active_output_pad; + + struct ipuv3_channel *idmac_ch; + struct ipu_smfc *smfc; + struct ipu_csi *csi; + + struct v4l2_mbus_framefmt format_mbus[CSI_NUM_PADS]; + const struct imx_media_pixfmt *cc[CSI_NUM_PADS]; + struct v4l2_fract frame_interval[CSI_NUM_PADS]; + struct v4l2_rect crop; + struct v4l2_rect compose; + const struct csi_skip_desc *skip; + + /* active vb2 buffers to send to video dev sink */ + struct imx_media_buffer *active_vb2_buf[2]; + struct imx_media_dma_buf underrun_buf; + + int ipu_buf_num; /* ipu double buffer index: 0-1 */ + + /* the sink for the captured frames */ + struct media_entity *sink; + enum ipu_csi_dest dest; + /* the source subdev */ + struct v4l2_subdev *src_sd; + + /* the mipi virtual channel number at link validate */ + int vc_num; + + /* the attached sensor at stream on */ + struct imx_media_subdev *sensor; + + spinlock_t irqlock; /* protect eof_irq handler */ + struct timer_list eof_timeout_timer; + int eof_irq; + int nfb4eof_irq; + + struct v4l2_ctrl_handler ctrl_hdlr; + + int stream_count; /* streaming counter */ + bool last_eof; /* waiting for last EOF at stream off */ + bool nfb4eof; /* NFB4EOF encountered during streaming */ + struct completion last_eof_comp; +}; + +static inline struct csi_priv *sd_to_dev(struct v4l2_subdev *sdev) +{ + return container_of(sdev, struct csi_priv, sd); +} + +static void csi_idmac_put_ipu_resources(struct csi_priv *priv) +{ + if (!IS_ERR_OR_NULL(priv->idmac_ch)) + ipu_idmac_put(priv->idmac_ch); + priv->idmac_ch = NULL; + + if (!IS_ERR_OR_NULL(priv->smfc)) + ipu_smfc_put(priv->smfc); + priv->smfc = NULL; +} + +static int csi_idmac_get_ipu_resources(struct csi_priv *priv) +{ + int ch_num, ret; + + ch_num = IPUV3_CHANNEL_CSI0 + priv->smfc_id; + + priv->smfc = ipu_smfc_get(priv->ipu, ch_num); + if (IS_ERR(priv->smfc)) { + v4l2_err(&priv->sd, "failed to get SMFC\n"); + ret = PTR_ERR(priv->smfc); + goto out; + } + + priv->idmac_ch = ipu_idmac_get(priv->ipu, ch_num); + if (IS_ERR(priv->idmac_ch)) { + v4l2_err(&priv->sd, "could not get IDMAC channel %u\n", + ch_num); + ret = PTR_ERR(priv->idmac_ch); + goto out; + } + + return 0; +out: + csi_idmac_put_ipu_resources(priv); + return ret; +} + +static void csi_vb2_buf_done(struct csi_priv *priv) +{ + struct imx_media_video_dev *vdev = priv->vdev; + struct imx_media_buffer *done, *next; + struct vb2_buffer *vb; + dma_addr_t phys; + + done = priv->active_vb2_buf[priv->ipu_buf_num]; + if (done) { + vb = &done->vbuf.vb2_buf; + vb->timestamp = ktime_get_ns(); + vb2_buffer_done(vb, priv->nfb4eof ? + VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE); + } + + priv->nfb4eof = false; + + /* get next queued buffer */ + next = imx_media_capture_device_next_buf(vdev); + if (next) { + phys = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0); + priv->active_vb2_buf[priv->ipu_buf_num] = next; + } else { + phys = priv->underrun_buf.phys; + priv->active_vb2_buf[priv->ipu_buf_num] = NULL; + } + + if (ipu_idmac_buffer_is_ready(priv->idmac_ch, priv->ipu_buf_num)) + ipu_idmac_clear_buffer(priv->idmac_ch, priv->ipu_buf_num); + + ipu_cpmem_set_buffer(priv->idmac_ch, priv->ipu_buf_num, phys); +} + +static irqreturn_t csi_idmac_eof_interrupt(int irq, void *dev_id) +{ + struct csi_priv *priv = dev_id; + + spin_lock(&priv->irqlock); + + if (priv->last_eof) { + complete(&priv->last_eof_comp); + priv->last_eof = false; + goto unlock; + } + + if (priv->fim) { + struct timespec cur_ts; + + ktime_get_ts(&cur_ts); + /* call frame interval monitor */ + imx_media_fim_eof_monitor(priv->fim, &cur_ts); + } + + csi_vb2_buf_done(priv); + + /* select new IPU buf */ + ipu_idmac_select_buffer(priv->idmac_ch, priv->ipu_buf_num); + /* toggle IPU double-buffer index */ + priv->ipu_buf_num ^= 1; + + /* bump the EOF timeout timer */ + mod_timer(&priv->eof_timeout_timer, + jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT)); + +unlock: + spin_unlock(&priv->irqlock); + return IRQ_HANDLED; +} + +static irqreturn_t csi_idmac_nfb4eof_interrupt(int irq, void *dev_id) +{ + struct csi_priv *priv = dev_id; + + spin_lock(&priv->irqlock); + + /* + * this is not an unrecoverable error, just mark + * the next captured frame with vb2 error flag. + */ + priv->nfb4eof = true; + + v4l2_err(&priv->sd, "NFB4EOF\n"); + + spin_unlock(&priv->irqlock); + + return IRQ_HANDLED; +} + +/* + * EOF timeout timer function. This is an unrecoverable condition + * without a stream restart. + */ +static void csi_idmac_eof_timeout(unsigned long data) +{ + struct csi_priv *priv = (struct csi_priv *)data; + struct imx_media_video_dev *vdev = priv->vdev; + + v4l2_err(&priv->sd, "EOF timeout\n"); + + /* signal a fatal error to capture device */ + imx_media_capture_device_error(vdev); +} + +static void csi_idmac_setup_vb2_buf(struct csi_priv *priv, dma_addr_t *phys) +{ + struct imx_media_video_dev *vdev = priv->vdev; + struct imx_media_buffer *buf; + int i; + + for (i = 0; i < 2; i++) { + buf = imx_media_capture_device_next_buf(vdev); + if (buf) { + priv->active_vb2_buf[i] = buf; + phys[i] = vb2_dma_contig_plane_dma_addr( + &buf->vbuf.vb2_buf, 0); + } else { + priv->active_vb2_buf[i] = NULL; + phys[i] = priv->underrun_buf.phys; + } + } +} + +static void csi_idmac_unsetup_vb2_buf(struct csi_priv *priv, + enum vb2_buffer_state return_status) +{ + struct imx_media_buffer *buf; + int i; + + /* return any remaining active frames with return_status */ + for (i = 0; i < 2; i++) { + buf = priv->active_vb2_buf[i]; + if (buf) { + struct vb2_buffer *vb = &buf->vbuf.vb2_buf; + + vb->timestamp = ktime_get_ns(); + vb2_buffer_done(vb, return_status); + } + } +} + +/* init the SMFC IDMAC channel */ +static int csi_idmac_setup_channel(struct csi_priv *priv) +{ + struct imx_media_video_dev *vdev = priv->vdev; + struct v4l2_fwnode_endpoint *sensor_ep; + struct v4l2_mbus_framefmt *infmt; + struct ipu_image image; + u32 passthrough_bits; + dma_addr_t phys[2]; + bool passthrough; + u32 burst_size; + int ret; + + infmt = &priv->format_mbus[CSI_SINK_PAD]; + sensor_ep = &priv->sensor->sensor_ep; + + ipu_cpmem_zero(priv->idmac_ch); + + memset(&image, 0, sizeof(image)); + image.pix = vdev->fmt.fmt.pix; + image.rect.width = image.pix.width; + image.rect.height = image.pix.height; + + csi_idmac_setup_vb2_buf(priv, phys); + + image.phys0 = phys[0]; + image.phys1 = phys[1]; + + /* + * Check for conditions that require the IPU to handle the + * data internally as generic data, aka passthrough mode: + * - raw bayer formats + * - the sensor bus is 16-bit parallel + */ + switch (image.pix.pixelformat) { + case V4L2_PIX_FMT_SBGGR8: + case V4L2_PIX_FMT_SGBRG8: + case V4L2_PIX_FMT_SGRBG8: + case V4L2_PIX_FMT_SRGGB8: + burst_size = 8; + passthrough = true; + passthrough_bits = 8; + break; + case V4L2_PIX_FMT_SBGGR16: + case V4L2_PIX_FMT_SGBRG16: + case V4L2_PIX_FMT_SGRBG16: + case V4L2_PIX_FMT_SRGGB16: + burst_size = 4; + passthrough = true; + passthrough_bits = 16; + break; + case V4L2_PIX_FMT_YUV420: + case V4L2_PIX_FMT_NV12: + burst_size = (image.pix.width & 0x3f) ? + ((image.pix.width & 0x1f) ? + ((image.pix.width & 0xf) ? 8 : 16) : 32) : 64; + passthrough = (sensor_ep->bus_type != V4L2_MBUS_CSI2 && + sensor_ep->bus.parallel.bus_width >= 16); + passthrough_bits = 16; + break; + case V4L2_PIX_FMT_YUYV: + case V4L2_PIX_FMT_UYVY: + burst_size = (image.pix.width & 0x1f) ? + ((image.pix.width & 0xf) ? 8 : 16) : 32; + passthrough = (sensor_ep->bus_type != V4L2_MBUS_CSI2 && + sensor_ep->bus.parallel.bus_width >= 16); + passthrough_bits = 16; + break; + default: + burst_size = (image.pix.width & 0xf) ? 8 : 16; + passthrough = (sensor_ep->bus_type != V4L2_MBUS_CSI2 && + sensor_ep->bus.parallel.bus_width >= 16); + passthrough_bits = 16; + break; + } + + if (passthrough) { + ipu_cpmem_set_resolution(priv->idmac_ch, image.rect.width, + image.rect.height); + ipu_cpmem_set_stride(priv->idmac_ch, image.pix.bytesperline); + ipu_cpmem_set_buffer(priv->idmac_ch, 0, image.phys0); + ipu_cpmem_set_buffer(priv->idmac_ch, 1, image.phys1); + ipu_cpmem_set_format_passthrough(priv->idmac_ch, + passthrough_bits); + } else { + ret = ipu_cpmem_set_image(priv->idmac_ch, &image); + if (ret) + goto unsetup_vb2; + } + + ipu_cpmem_set_burstsize(priv->idmac_ch, burst_size); + + /* + * Set the channel for the direct CSI-->memory via SMFC + * use-case to very high priority, by enabling the watermark + * signal in the SMFC, enabling WM in the channel, and setting + * the channel priority to high. + * + * Refer to the i.mx6 rev. D TRM Table 36-8: Calculated priority + * value. + * + * The WM's are set very low by intention here to ensure that + * the SMFC FIFOs do not overflow. + */ + ipu_smfc_set_watermark(priv->smfc, 0x02, 0x01); + ipu_cpmem_set_high_priority(priv->idmac_ch); + ipu_idmac_enable_watermark(priv->idmac_ch, true); + ipu_cpmem_set_axi_id(priv->idmac_ch, 0); + + burst_size = passthrough ? + (burst_size >> 3) - 1 : (burst_size >> 2) - 1; + + ipu_smfc_set_burstsize(priv->smfc, burst_size); + + if (image.pix.field == V4L2_FIELD_NONE && + V4L2_FIELD_HAS_BOTH(infmt->field)) + ipu_cpmem_interlaced_scan(priv->idmac_ch, + image.pix.bytesperline); + + ipu_idmac_set_double_buffer(priv->idmac_ch, true); + + return 0; + +unsetup_vb2: + csi_idmac_unsetup_vb2_buf(priv, VB2_BUF_STATE_QUEUED); + return ret; +} + +static void csi_idmac_unsetup(struct csi_priv *priv, + enum vb2_buffer_state state) +{ + ipu_idmac_disable_channel(priv->idmac_ch); + ipu_smfc_disable(priv->smfc); + + csi_idmac_unsetup_vb2_buf(priv, state); +} + +static int csi_idmac_setup(struct csi_priv *priv) +{ + int ret; + + ret = csi_idmac_setup_channel(priv); + if (ret) + return ret; + + ipu_cpmem_dump(priv->idmac_ch); + ipu_dump(priv->ipu); + + ipu_smfc_enable(priv->smfc); + + /* set buffers ready */ + ipu_idmac_select_buffer(priv->idmac_ch, 0); + ipu_idmac_select_buffer(priv->idmac_ch, 1); + + /* enable the channels */ + ipu_idmac_enable_channel(priv->idmac_ch); + + return 0; +} + +static int csi_idmac_start(struct csi_priv *priv) +{ + struct imx_media_video_dev *vdev = priv->vdev; + struct v4l2_pix_format *outfmt; + int ret; + + ret = csi_idmac_get_ipu_resources(priv); + if (ret) + return ret; + + ipu_smfc_map_channel(priv->smfc, priv->csi_id, priv->vc_num); + + outfmt = &vdev->fmt.fmt.pix; + + ret = imx_media_alloc_dma_buf(priv->md, &priv->underrun_buf, + outfmt->sizeimage); + if (ret) + goto out_put_ipu; + + priv->ipu_buf_num = 0; + + /* init EOF completion waitq */ + init_completion(&priv->last_eof_comp); + priv->last_eof = false; + priv->nfb4eof = false; + + ret = csi_idmac_setup(priv); + if (ret) { + v4l2_err(&priv->sd, "csi_idmac_setup failed: %d\n", ret); + goto out_free_dma_buf; + } + + priv->nfb4eof_irq = ipu_idmac_channel_irq(priv->ipu, + priv->idmac_ch, + IPU_IRQ_NFB4EOF); + ret = devm_request_irq(priv->dev, priv->nfb4eof_irq, + csi_idmac_nfb4eof_interrupt, 0, + "imx-smfc-nfb4eof", priv); + if (ret) { + v4l2_err(&priv->sd, + "Error registering NFB4EOF irq: %d\n", ret); + goto out_unsetup; + } + + priv->eof_irq = ipu_idmac_channel_irq(priv->ipu, priv->idmac_ch, + IPU_IRQ_EOF); + + ret = devm_request_irq(priv->dev, priv->eof_irq, + csi_idmac_eof_interrupt, 0, + "imx-smfc-eof", priv); + if (ret) { + v4l2_err(&priv->sd, + "Error registering eof irq: %d\n", ret); + goto out_free_nfb4eof_irq; + } + + /* start the EOF timeout timer */ + mod_timer(&priv->eof_timeout_timer, + jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT)); + + return 0; + +out_free_nfb4eof_irq: + devm_free_irq(priv->dev, priv->nfb4eof_irq, priv); +out_unsetup: + csi_idmac_unsetup(priv, VB2_BUF_STATE_QUEUED); +out_free_dma_buf: + imx_media_free_dma_buf(priv->md, &priv->underrun_buf); +out_put_ipu: + csi_idmac_put_ipu_resources(priv); + return ret; +} + +static void csi_idmac_stop(struct csi_priv *priv) +{ + unsigned long flags; + int ret; + + /* mark next EOF interrupt as the last before stream off */ + spin_lock_irqsave(&priv->irqlock, flags); + priv->last_eof = true; + spin_unlock_irqrestore(&priv->irqlock, flags); + + /* + * and then wait for interrupt handler to mark completion. + */ + ret = wait_for_completion_timeout( + &priv->last_eof_comp, msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT)); + if (ret == 0) + v4l2_warn(&priv->sd, "wait last EOF timeout\n"); + + devm_free_irq(priv->dev, priv->eof_irq, priv); + devm_free_irq(priv->dev, priv->nfb4eof_irq, priv); + + csi_idmac_unsetup(priv, VB2_BUF_STATE_ERROR); + + imx_media_free_dma_buf(priv->md, &priv->underrun_buf); + + /* cancel the EOF timeout timer */ + del_timer_sync(&priv->eof_timeout_timer); + + csi_idmac_put_ipu_resources(priv); +} + +/* Update the CSI whole sensor and active windows */ +static int csi_setup(struct csi_priv *priv) +{ + struct v4l2_mbus_framefmt *infmt, *outfmt; + struct v4l2_mbus_config sensor_mbus_cfg; + struct v4l2_fwnode_endpoint *sensor_ep; + struct v4l2_mbus_framefmt if_fmt; + + infmt = &priv->format_mbus[CSI_SINK_PAD]; + outfmt = &priv->format_mbus[priv->active_output_pad]; + sensor_ep = &priv->sensor->sensor_ep; + + /* compose mbus_config from sensor endpoint */ + sensor_mbus_cfg.type = sensor_ep->bus_type; + sensor_mbus_cfg.flags = (sensor_ep->bus_type == V4L2_MBUS_CSI2) ? + sensor_ep->bus.mipi_csi2.flags : + sensor_ep->bus.parallel.flags; + + /* + * we need to pass input sensor frame to CSI interface, but + * with translated field type from output format + */ + if_fmt = *infmt; + if_fmt.field = outfmt->field; + + ipu_csi_set_window(priv->csi, &priv->crop); + + ipu_csi_set_downsize(priv->csi, + priv->crop.width == 2 * priv->compose.width, + priv->crop.height == 2 * priv->compose.height); + + ipu_csi_init_interface(priv->csi, &sensor_mbus_cfg, &if_fmt); + + ipu_csi_set_dest(priv->csi, priv->dest); + + if (priv->dest == IPU_CSI_DEST_IDMAC) + ipu_csi_set_skip_smfc(priv->csi, priv->skip->skip_smfc, + priv->skip->max_ratio - 1, 0); + + ipu_csi_dump(priv->csi); + + return 0; +} + +static int csi_start(struct csi_priv *priv) +{ + struct v4l2_fract *output_fi, *input_fi; + u32 bad_frames = 0; + int ret; + + if (!priv->sensor) { + v4l2_err(&priv->sd, "no sensor attached\n"); + return -EINVAL; + } + + output_fi = &priv->frame_interval[priv->active_output_pad]; + input_fi = &priv->frame_interval[CSI_SINK_PAD]; + + ret = v4l2_subdev_call(priv->sensor->sd, sensor, + g_skip_frames, &bad_frames); + if (!ret && bad_frames) { + u32 delay_usec; + + /* + * This sensor has bad frames when it is turned on, + * add a delay to avoid them before enabling the CSI + * hardware. Especially for sensors with a bt.656 interface, + * any shifts in the SAV/EAV sync codes will cause the CSI + * to lose vert/horiz sync. + */ + delay_usec = DIV_ROUND_UP_ULL( + (u64)USEC_PER_SEC * input_fi->numerator * bad_frames, + input_fi->denominator); + usleep_range(delay_usec, delay_usec + 1000); + } + + if (priv->dest == IPU_CSI_DEST_IDMAC) { + ret = csi_idmac_start(priv); + if (ret) + return ret; + } + + ret = csi_setup(priv); + if (ret) + goto idmac_stop; + + /* start the frame interval monitor */ + if (priv->fim && priv->dest == IPU_CSI_DEST_IDMAC) { + ret = imx_media_fim_set_stream(priv->fim, output_fi, true); + if (ret) + goto idmac_stop; + } + + ret = ipu_csi_enable(priv->csi); + if (ret) { + v4l2_err(&priv->sd, "CSI enable error: %d\n", ret); + goto fim_off; + } + + return 0; + +fim_off: + if (priv->fim && priv->dest == IPU_CSI_DEST_IDMAC) + imx_media_fim_set_stream(priv->fim, NULL, false); +idmac_stop: + if (priv->dest == IPU_CSI_DEST_IDMAC) + csi_idmac_stop(priv); + return ret; +} + +static void csi_stop(struct csi_priv *priv) +{ + if (priv->dest == IPU_CSI_DEST_IDMAC) { + csi_idmac_stop(priv); + + /* stop the frame interval monitor */ + if (priv->fim) + imx_media_fim_set_stream(priv->fim, NULL, false); + } + + ipu_csi_disable(priv->csi); +} + +static const struct csi_skip_desc csi_skip[12] = { + { 1, 1, 0x00 }, /* Keep all frames */ + { 5, 6, 0x10 }, /* Skip every sixth frame */ + { 4, 5, 0x08 }, /* Skip every fifth frame */ + { 3, 4, 0x04 }, /* Skip every fourth frame */ + { 2, 3, 0x02 }, /* Skip every third frame */ + { 3, 5, 0x0a }, /* Skip frames 1 and 3 of every 5 */ + { 1, 2, 0x01 }, /* Skip every second frame */ + { 2, 5, 0x0b }, /* Keep frames 1 and 4 of every 5 */ + { 1, 3, 0x03 }, /* Keep one in three frames */ + { 1, 4, 0x07 }, /* Keep one in four frames */ + { 1, 5, 0x0f }, /* Keep one in five frames */ + { 1, 6, 0x1f }, /* Keep one in six frames */ +}; + +static void csi_apply_skip_interval(const struct csi_skip_desc *skip, + struct v4l2_fract *interval) +{ + unsigned int div; + + interval->numerator *= skip->max_ratio; + interval->denominator *= skip->keep; + + /* Reduce fraction to lowest terms */ + div = gcd(interval->numerator, interval->denominator); + if (div > 1) { + interval->numerator /= div; + interval->denominator /= div; + } +} + +/* + * Find the skip pattern to produce the output frame interval closest to the + * requested one, for the given input frame interval. Updates the output frame + * interval to the exact value. + */ +static const struct csi_skip_desc *csi_find_best_skip(struct v4l2_fract *in, + struct v4l2_fract *out) +{ + const struct csi_skip_desc *skip = &csi_skip[0], *best_skip = skip; + u32 min_err = UINT_MAX; + u64 want_us; + int i; + + /* Default to 1:1 ratio */ + if (out->numerator == 0 || out->denominator == 0 || + in->numerator == 0 || in->denominator == 0) { + *out = *in; + return best_skip; + } + + want_us = div_u64((u64)USEC_PER_SEC * out->numerator, out->denominator); + + /* Find the reduction closest to the requested time per frame */ + for (i = 0; i < ARRAY_SIZE(csi_skip); i++, skip++) { + u64 tmp, err; + + tmp = div_u64((u64)USEC_PER_SEC * in->numerator * + skip->max_ratio, in->denominator * skip->keep); + + err = abs((s64)tmp - want_us); + if (err < min_err) { + min_err = err; + best_skip = skip; + } + } + + *out = *in; + csi_apply_skip_interval(best_skip, out); + + return best_skip; +} + +/* + * V4L2 subdev operations. + */ + +static int csi_g_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + + if (fi->pad >= CSI_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + + fi->interval = priv->frame_interval[fi->pad]; + + mutex_unlock(&priv->lock); + + return 0; +} + +static int csi_s_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_fract *input_fi; + int ret = 0; + + mutex_lock(&priv->lock); + + input_fi = &priv->frame_interval[CSI_SINK_PAD]; + + switch (fi->pad) { + case CSI_SINK_PAD: + /* No limits on input frame interval */ + /* Reset output intervals and frame skipping ratio to 1:1 */ + priv->frame_interval[CSI_SRC_PAD_IDMAC] = fi->interval; + priv->frame_interval[CSI_SRC_PAD_DIRECT] = fi->interval; + priv->skip = &csi_skip[0]; + break; + case CSI_SRC_PAD_IDMAC: + /* + * frame interval at IDMAC output pad depends on input + * interval, modified by frame skipping. + */ + priv->skip = csi_find_best_skip(input_fi, &fi->interval); + break; + case CSI_SRC_PAD_DIRECT: + /* + * frame interval at DIRECT output pad is same as input + * interval. + */ + fi->interval = *input_fi; + break; + default: + ret = -EINVAL; + goto out; + } + + priv->frame_interval[fi->pad] = fi->interval; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int csi_s_stream(struct v4l2_subdev *sd, int enable) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + int ret = 0; + + mutex_lock(&priv->lock); + + if (!priv->src_sd || !priv->sink) { + ret = -EPIPE; + goto out; + } + + /* + * enable/disable streaming only if stream_count is + * going from 0 to 1 / 1 to 0. + */ + if (priv->stream_count != !enable) + goto update_count; + + if (enable) { + /* upstream must be started first, before starting CSI */ + ret = v4l2_subdev_call(priv->src_sd, video, s_stream, 1); + ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0; + if (ret) + goto out; + + dev_dbg(priv->dev, "stream ON\n"); + ret = csi_start(priv); + if (ret) { + v4l2_subdev_call(priv->src_sd, video, s_stream, 0); + goto out; + } + } else { + dev_dbg(priv->dev, "stream OFF\n"); + /* CSI must be stopped first, then stop upstream */ + csi_stop(priv); + v4l2_subdev_call(priv->src_sd, video, s_stream, 0); + } + +update_count: + priv->stream_count += enable ? 1 : -1; + if (priv->stream_count < 0) + priv->stream_count = 0; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int csi_link_setup(struct media_entity *entity, + const struct media_pad *local, + const struct media_pad *remote, u32 flags) +{ + struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); + struct csi_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_subdev *remote_sd; + int ret = 0; + + dev_dbg(priv->dev, "link setup %s -> %s\n", remote->entity->name, + local->entity->name); + + mutex_lock(&priv->lock); + + if (local->flags & MEDIA_PAD_FL_SINK) { + if (!is_media_entity_v4l2_subdev(remote->entity)) { + ret = -EINVAL; + goto out; + } + + remote_sd = media_entity_to_v4l2_subdev(remote->entity); + + if (flags & MEDIA_LNK_FL_ENABLED) { + if (priv->src_sd) { + ret = -EBUSY; + goto out; + } + priv->src_sd = remote_sd; + } else { + priv->src_sd = NULL; + } + + goto out; + } + + /* this is a source pad */ + + if (flags & MEDIA_LNK_FL_ENABLED) { + if (priv->sink) { + ret = -EBUSY; + goto out; + } + } else { + v4l2_ctrl_handler_free(&priv->ctrl_hdlr); + v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0); + priv->sink = NULL; + goto out; + } + + /* record which output pad is now active */ + priv->active_output_pad = local->index; + + /* set CSI destination */ + if (local->index == CSI_SRC_PAD_IDMAC) { + if (!is_media_entity_v4l2_video_device(remote->entity)) { + ret = -EINVAL; + goto out; + } + + if (priv->fim) { + ret = imx_media_fim_add_controls(priv->fim); + if (ret) + goto out; + } + + priv->dest = IPU_CSI_DEST_IDMAC; + } else { + if (!is_media_entity_v4l2_subdev(remote->entity)) { + ret = -EINVAL; + goto out; + } + + remote_sd = media_entity_to_v4l2_subdev(remote->entity); + switch (remote_sd->grp_id) { + case IMX_MEDIA_GRP_ID_VDIC: + priv->dest = IPU_CSI_DEST_VDIC; + break; + case IMX_MEDIA_GRP_ID_IC_PRP: + priv->dest = IPU_CSI_DEST_IC; + break; + default: + ret = -EINVAL; + goto out; + } + } + + priv->sink = remote->entity; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int csi_link_validate(struct v4l2_subdev *sd, + struct media_link *link, + struct v4l2_subdev_format *source_fmt, + struct v4l2_subdev_format *sink_fmt) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_fwnode_endpoint *sensor_ep; + const struct imx_media_pixfmt *incc; + struct imx_media_subdev *sensor; + bool is_csi2; + int ret; + + ret = v4l2_subdev_link_validate_default(sd, link, + source_fmt, sink_fmt); + if (ret) + return ret; + + sensor = __imx_media_find_sensor(priv->md, &priv->sd.entity); + if (IS_ERR(sensor)) { + v4l2_err(&priv->sd, "no sensor attached\n"); + return PTR_ERR(priv->sensor); + } + + mutex_lock(&priv->lock); + + priv->sensor = sensor; + sensor_ep = &priv->sensor->sensor_ep; + is_csi2 = (sensor_ep->bus_type == V4L2_MBUS_CSI2); + incc = priv->cc[CSI_SINK_PAD]; + + if (priv->dest != IPU_CSI_DEST_IDMAC && + (incc->bayer || (!is_csi2 && + sensor_ep->bus.parallel.bus_width >= 16))) { + v4l2_err(&priv->sd, + "bayer/16-bit parallel buses must go to IDMAC pad\n"); + ret = -EINVAL; + goto out; + } + + if (is_csi2) { + int vc_num = 0; + /* + * NOTE! It seems the virtual channels from the mipi csi-2 + * receiver are used only for routing by the video mux's, + * or for hard-wired routing to the CSI's. Once the stream + * enters the CSI's however, they are treated internally + * in the IPU as virtual channel 0. + */ +#if 0 + mutex_unlock(&priv->lock); + vc_num = imx_media_find_mipi_csi2_channel(priv->md, + &priv->sd.entity); + if (vc_num < 0) + return vc_num; + mutex_lock(&priv->lock); +#endif + ipu_csi_set_mipi_datatype(priv->csi, vc_num, + &priv->format_mbus[CSI_SINK_PAD]); + } + + /* select either parallel or MIPI-CSI2 as input to CSI */ + ipu_set_csi_src_mux(priv->ipu, priv->csi_id, is_csi2); +out: + mutex_unlock(&priv->lock); + return ret; +} + +static struct v4l2_mbus_framefmt * +__csi_get_fmt(struct csi_priv *priv, struct v4l2_subdev_pad_config *cfg, + unsigned int pad, enum v4l2_subdev_format_whence which) +{ + if (which == V4L2_SUBDEV_FORMAT_TRY) + return v4l2_subdev_get_try_format(&priv->sd, cfg, pad); + else + return &priv->format_mbus[pad]; +} + +static struct v4l2_rect * +__csi_get_crop(struct csi_priv *priv, struct v4l2_subdev_pad_config *cfg, + enum v4l2_subdev_format_whence which) +{ + if (which == V4L2_SUBDEV_FORMAT_TRY) + return v4l2_subdev_get_try_crop(&priv->sd, cfg, CSI_SINK_PAD); + else + return &priv->crop; +} + +static struct v4l2_rect * +__csi_get_compose(struct csi_priv *priv, struct v4l2_subdev_pad_config *cfg, + enum v4l2_subdev_format_whence which) +{ + if (which == V4L2_SUBDEV_FORMAT_TRY) + return v4l2_subdev_get_try_compose(&priv->sd, cfg, + CSI_SINK_PAD); + else + return &priv->compose; +} + +static void csi_try_crop(struct csi_priv *priv, + struct v4l2_rect *crop, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_mbus_framefmt *infmt, + struct imx_media_subdev *sensor) +{ + struct v4l2_fwnode_endpoint *sensor_ep; + + sensor_ep = &sensor->sensor_ep; + + crop->width = min_t(__u32, infmt->width, crop->width); + if (crop->left + crop->width > infmt->width) + crop->left = infmt->width - crop->width; + /* adjust crop left/width to h/w alignment restrictions */ + crop->left &= ~0x3; + crop->width &= ~0x7; + + /* + * FIXME: not sure why yet, but on interlaced bt.656, + * changing the vertical cropping causes loss of vertical + * sync, so fix it to NTSC/PAL active lines. NTSC contains + * 2 extra lines of active video that need to be cropped. + */ + if (sensor_ep->bus_type == V4L2_MBUS_BT656 && + (V4L2_FIELD_HAS_BOTH(infmt->field) || + infmt->field == V4L2_FIELD_ALTERNATE)) { + crop->height = infmt->height; + crop->top = (infmt->height == 480) ? 2 : 0; + } else { + crop->height = min_t(__u32, infmt->height, crop->height); + if (crop->top + crop->height > infmt->height) + crop->top = infmt->height - crop->height; + } +} + +static int csi_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_mbus_code_enum *code) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + const struct imx_media_pixfmt *incc; + struct v4l2_mbus_framefmt *infmt; + int ret = 0; + + mutex_lock(&priv->lock); + + infmt = __csi_get_fmt(priv, cfg, CSI_SINK_PAD, code->which); + incc = imx_media_find_mbus_format(infmt->code, CS_SEL_ANY, true); + + switch (code->pad) { + case CSI_SINK_PAD: + ret = imx_media_enum_mbus_format(&code->code, code->index, + CS_SEL_ANY, true); + break; + case CSI_SRC_PAD_DIRECT: + case CSI_SRC_PAD_IDMAC: + if (incc->bayer) { + if (code->index != 0) { + ret = -EINVAL; + goto out; + } + code->code = infmt->code; + } else { + u32 cs_sel = (incc->cs == IPUV3_COLORSPACE_YUV) ? + CS_SEL_YUV : CS_SEL_RGB; + ret = imx_media_enum_ipu_format(&code->code, + code->index, + cs_sel); + } + break; + default: + ret = -EINVAL; + } + +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int csi_enum_frame_size(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_frame_size_enum *fse) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_rect *crop; + int ret = 0; + + if (fse->pad >= CSI_NUM_PADS || + fse->index > (fse->pad == CSI_SINK_PAD ? 0 : 3)) + return -EINVAL; + + mutex_lock(&priv->lock); + + if (fse->pad == CSI_SINK_PAD) { + fse->min_width = MIN_W; + fse->max_width = MAX_W; + fse->min_height = MIN_H; + fse->max_height = MAX_H; + } else { + crop = __csi_get_crop(priv, cfg, fse->which); + + fse->min_width = fse->max_width = fse->index & 1 ? + crop->width / 2 : crop->width; + fse->min_height = fse->max_height = fse->index & 2 ? + crop->height / 2 : crop->height; + } + + mutex_unlock(&priv->lock); + return ret; +} + +static int csi_enum_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_frame_interval_enum *fie) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_fract *input_fi; + struct v4l2_rect *crop; + int ret = 0; + + if (fie->pad >= CSI_NUM_PADS || + fie->index >= (fie->pad != CSI_SRC_PAD_IDMAC ? + 1 : ARRAY_SIZE(csi_skip))) + return -EINVAL; + + mutex_lock(&priv->lock); + + input_fi = &priv->frame_interval[CSI_SINK_PAD]; + crop = __csi_get_crop(priv, cfg, fie->which); + + if ((fie->width != crop->width && fie->width != crop->width / 2) || + (fie->height != crop->height && fie->height != crop->height / 2)) { + ret = -EINVAL; + goto out; + } + + fie->interval = *input_fi; + + if (fie->pad == CSI_SRC_PAD_IDMAC) + csi_apply_skip_interval(&csi_skip[fie->index], + &fie->interval); + +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int csi_get_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_mbus_framefmt *fmt; + int ret = 0; + + if (sdformat->pad >= CSI_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + + fmt = __csi_get_fmt(priv, cfg, sdformat->pad, sdformat->which); + if (!fmt) { + ret = -EINVAL; + goto out; + } + + sdformat->format = *fmt; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static void csi_try_fmt(struct csi_priv *priv, + struct imx_media_subdev *sensor, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat, + struct v4l2_rect *crop, + struct v4l2_rect *compose, + const struct imx_media_pixfmt **cc) +{ + const struct imx_media_pixfmt *incc; + struct v4l2_mbus_framefmt *infmt; + u32 code; + + infmt = __csi_get_fmt(priv, cfg, CSI_SINK_PAD, sdformat->which); + + switch (sdformat->pad) { + case CSI_SRC_PAD_DIRECT: + case CSI_SRC_PAD_IDMAC: + incc = imx_media_find_mbus_format(infmt->code, + CS_SEL_ANY, true); + + sdformat->format.width = compose->width; + sdformat->format.height = compose->height; + + if (incc->bayer) { + sdformat->format.code = infmt->code; + *cc = incc; + } else { + u32 cs_sel = (incc->cs == IPUV3_COLORSPACE_YUV) ? + CS_SEL_YUV : CS_SEL_RGB; + + *cc = imx_media_find_ipu_format(sdformat->format.code, + cs_sel); + if (!*cc) { + imx_media_enum_ipu_format(&code, 0, cs_sel); + *cc = imx_media_find_ipu_format(code, cs_sel); + sdformat->format.code = (*cc)->codes[0]; + } + } + + if (sdformat->pad == CSI_SRC_PAD_DIRECT || + sdformat->format.field != V4L2_FIELD_NONE) + sdformat->format.field = infmt->field; + + /* + * translate V4L2_FIELD_ALTERNATE to SEQ_TB or SEQ_BT + * depending on input height (assume NTSC top-bottom + * order if 480 lines, otherwise PAL bottom-top order). + */ + if (sdformat->format.field == V4L2_FIELD_ALTERNATE) { + sdformat->format.field = (infmt->height == 480) ? + V4L2_FIELD_SEQ_TB : V4L2_FIELD_SEQ_BT; + } + + /* propagate colorimetry from sink */ + sdformat->format.colorspace = infmt->colorspace; + sdformat->format.xfer_func = infmt->xfer_func; + sdformat->format.quantization = infmt->quantization; + sdformat->format.ycbcr_enc = infmt->ycbcr_enc; + break; + case CSI_SINK_PAD: + v4l_bound_align_image(&sdformat->format.width, MIN_W, MAX_W, + W_ALIGN, &sdformat->format.height, + MIN_H, MAX_H, H_ALIGN, S_ALIGN); + + /* Reset crop and compose rectangles */ + crop->left = 0; + crop->top = 0; + crop->width = sdformat->format.width; + crop->height = sdformat->format.height; + csi_try_crop(priv, crop, cfg, &sdformat->format, sensor); + compose->left = 0; + compose->top = 0; + compose->width = crop->width; + compose->height = crop->height; + + *cc = imx_media_find_mbus_format(sdformat->format.code, + CS_SEL_ANY, true); + if (!*cc) { + imx_media_enum_mbus_format(&code, 0, + CS_SEL_ANY, false); + *cc = imx_media_find_mbus_format(code, + CS_SEL_ANY, false); + sdformat->format.code = (*cc)->codes[0]; + } + + imx_media_fill_default_mbus_fields( + &sdformat->format, infmt, + priv->active_output_pad == CSI_SRC_PAD_DIRECT); + break; + } +} + +static int csi_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + struct imx_media_video_dev *vdev = priv->vdev; + const struct imx_media_pixfmt *cc; + struct imx_media_subdev *sensor; + struct v4l2_pix_format vdev_fmt; + struct v4l2_mbus_framefmt *fmt; + struct v4l2_rect *crop, *compose; + int ret = 0; + + if (sdformat->pad >= CSI_NUM_PADS) + return -EINVAL; + + sensor = imx_media_find_sensor(priv->md, &priv->sd.entity); + if (IS_ERR(sensor)) { + v4l2_err(&priv->sd, "no sensor attached\n"); + return PTR_ERR(sensor); + } + + mutex_lock(&priv->lock); + + if (priv->stream_count > 0) { + ret = -EBUSY; + goto out; + } + + crop = __csi_get_crop(priv, cfg, sdformat->which); + compose = __csi_get_compose(priv, cfg, sdformat->which); + + csi_try_fmt(priv, sensor, cfg, sdformat, crop, compose, &cc); + + fmt = __csi_get_fmt(priv, cfg, sdformat->pad, sdformat->which); + *fmt = sdformat->format; + + if (sdformat->pad == CSI_SINK_PAD) { + int pad; + + /* propagate format to source pads */ + for (pad = CSI_SINK_PAD + 1; pad < CSI_NUM_PADS; pad++) { + const struct imx_media_pixfmt *outcc; + struct v4l2_mbus_framefmt *outfmt; + struct v4l2_subdev_format format; + + format.pad = pad; + format.which = sdformat->which; + format.format = sdformat->format; + csi_try_fmt(priv, sensor, cfg, &format, NULL, compose, + &outcc); + + outfmt = __csi_get_fmt(priv, cfg, pad, sdformat->which); + *outfmt = format.format; + + if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE) + priv->cc[pad] = outcc; + } + } + + if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) + goto out; + + priv->cc[sdformat->pad] = cc; + + /* propagate IDMAC output pad format to capture device */ + imx_media_mbus_fmt_to_pix_fmt(&vdev_fmt, + &priv->format_mbus[CSI_SRC_PAD_IDMAC], + priv->cc[CSI_SRC_PAD_IDMAC]); + mutex_unlock(&priv->lock); + imx_media_capture_device_set_format(vdev, &vdev_fmt); + + return 0; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int csi_get_selection(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_selection *sel) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_mbus_framefmt *infmt; + struct v4l2_rect *crop, *compose; + int ret = 0; + + if (sel->pad != CSI_SINK_PAD) + return -EINVAL; + + mutex_lock(&priv->lock); + + infmt = __csi_get_fmt(priv, cfg, CSI_SINK_PAD, sel->which); + crop = __csi_get_crop(priv, cfg, sel->which); + compose = __csi_get_compose(priv, cfg, sel->which); + + switch (sel->target) { + case V4L2_SEL_TGT_CROP_BOUNDS: + sel->r.left = 0; + sel->r.top = 0; + sel->r.width = infmt->width; + sel->r.height = infmt->height; + break; + case V4L2_SEL_TGT_CROP: + sel->r = *crop; + break; + case V4L2_SEL_TGT_COMPOSE_BOUNDS: + sel->r.left = 0; + sel->r.top = 0; + sel->r.width = crop->width; + sel->r.height = crop->height; + break; + case V4L2_SEL_TGT_COMPOSE: + sel->r = *compose; + break; + default: + ret = -EINVAL; + } + + mutex_unlock(&priv->lock); + return ret; +} + +static int csi_set_scale(u32 *compose, u32 crop, u32 flags) +{ + if ((flags & (V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE)) == + (V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE) && + *compose != crop && *compose != crop / 2) + return -ERANGE; + + if (*compose <= crop / 2 || + (*compose < crop * 3 / 4 && !(flags & V4L2_SEL_FLAG_GE)) || + (*compose < crop && (flags & V4L2_SEL_FLAG_LE))) + *compose = crop / 2; + else + *compose = crop; + + return 0; +} + +static int csi_set_selection(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_selection *sel) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_mbus_framefmt *infmt; + struct v4l2_rect *crop, *compose; + struct imx_media_subdev *sensor; + int pad, ret = 0; + + if (sel->pad != CSI_SINK_PAD) + return -EINVAL; + + sensor = imx_media_find_sensor(priv->md, &priv->sd.entity); + if (IS_ERR(sensor)) { + v4l2_err(&priv->sd, "no sensor attached\n"); + return PTR_ERR(sensor); + } + + mutex_lock(&priv->lock); + + if (priv->stream_count > 0) { + ret = -EBUSY; + goto out; + } + + infmt = __csi_get_fmt(priv, cfg, CSI_SINK_PAD, sel->which); + crop = __csi_get_crop(priv, cfg, sel->which); + compose = __csi_get_compose(priv, cfg, sel->which); + + switch (sel->target) { + case V4L2_SEL_TGT_CROP: + /* + * Modifying the crop rectangle always changes the format on + * the source pads. If the KEEP_CONFIG flag is set, just return + * the current crop rectangle. + */ + if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) { + sel->r = priv->crop; + if (sel->which == V4L2_SUBDEV_FORMAT_TRY) + *crop = sel->r; + goto out; + } + + csi_try_crop(priv, &sel->r, cfg, infmt, sensor); + + *crop = sel->r; + + /* Reset scaling to 1:1 */ + compose->width = crop->width; + compose->height = crop->height; + break; + case V4L2_SEL_TGT_COMPOSE: + /* + * Modifying the compose rectangle always changes the format on + * the source pads. If the KEEP_CONFIG flag is set, just return + * the current compose rectangle. + */ + if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) { + sel->r = priv->compose; + if (sel->which == V4L2_SUBDEV_FORMAT_TRY) + *compose = sel->r; + goto out; + } + + sel->r.left = 0; + sel->r.top = 0; + ret = csi_set_scale(&sel->r.width, crop->width, sel->flags); + if (ret) + goto out; + ret = csi_set_scale(&sel->r.height, crop->height, sel->flags); + if (ret) + goto out; + + *compose = sel->r; + break; + default: + ret = -EINVAL; + goto out; + } + + /* Reset source pads to sink compose rectangle */ + for (pad = CSI_SINK_PAD + 1; pad < CSI_NUM_PADS; pad++) { + struct v4l2_mbus_framefmt *outfmt; + + outfmt = __csi_get_fmt(priv, cfg, pad, sel->which); + outfmt->width = compose->width; + outfmt->height = compose->height; + } + +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int csi_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh, + struct v4l2_event_subscription *sub) +{ + if (sub->type != V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR) + return -EINVAL; + if (sub->id != 0) + return -EINVAL; + + return v4l2_event_subscribe(fh, sub, 0, NULL); +} + +static int csi_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh, + struct v4l2_event_subscription *sub) +{ + return v4l2_event_unsubscribe(fh, sub); +} + +/* + * retrieve our pads parsed from the OF graph by the media device + */ +static int csi_registered(struct v4l2_subdev *sd) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + int i, ret; + u32 code; + + /* get media device */ + priv->md = dev_get_drvdata(sd->v4l2_dev->dev); + + /* get handle to IPU CSI */ + priv->csi = ipu_csi_get(priv->ipu, priv->csi_id); + if (IS_ERR(priv->csi)) { + v4l2_err(&priv->sd, "failed to get CSI%d\n", priv->csi_id); + return PTR_ERR(priv->csi); + } + + for (i = 0; i < CSI_NUM_PADS; i++) { + priv->pad[i].flags = (i == CSI_SINK_PAD) ? + MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE; + + code = 0; + if (i != CSI_SINK_PAD) + imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV); + + /* set a default mbus format */ + ret = imx_media_init_mbus_fmt(&priv->format_mbus[i], + 640, 480, code, V4L2_FIELD_NONE, + &priv->cc[i]); + if (ret) + goto put_csi; + + /* init default frame interval */ + priv->frame_interval[i].numerator = 1; + priv->frame_interval[i].denominator = 30; + } + + /* disable frame skipping */ + priv->skip = &csi_skip[0]; + + /* init default crop and compose rectangle sizes */ + priv->crop.width = 640; + priv->crop.height = 480; + priv->compose.width = 640; + priv->compose.height = 480; + + priv->fim = imx_media_fim_init(&priv->sd); + if (IS_ERR(priv->fim)) { + ret = PTR_ERR(priv->fim); + goto put_csi; + } + + ret = media_entity_pads_init(&sd->entity, CSI_NUM_PADS, priv->pad); + if (ret) + goto free_fim; + + ret = imx_media_capture_device_register(priv->vdev); + if (ret) + goto free_fim; + + ret = imx_media_add_video_device(priv->md, priv->vdev); + if (ret) + goto unreg; + + return 0; +unreg: + imx_media_capture_device_unregister(priv->vdev); +free_fim: + if (priv->fim) + imx_media_fim_free(priv->fim); +put_csi: + ipu_csi_put(priv->csi); + return ret; +} + +static void csi_unregistered(struct v4l2_subdev *sd) +{ + struct csi_priv *priv = v4l2_get_subdevdata(sd); + + imx_media_capture_device_unregister(priv->vdev); + + if (priv->fim) + imx_media_fim_free(priv->fim); + + if (!IS_ERR_OR_NULL(priv->csi)) + ipu_csi_put(priv->csi); +} + +static const struct media_entity_operations csi_entity_ops = { + .link_setup = csi_link_setup, + .link_validate = v4l2_subdev_link_validate, +}; + +static const struct v4l2_subdev_core_ops csi_core_ops = { + .subscribe_event = csi_subscribe_event, + .unsubscribe_event = csi_unsubscribe_event, +}; + +static const struct v4l2_subdev_video_ops csi_video_ops = { + .g_frame_interval = csi_g_frame_interval, + .s_frame_interval = csi_s_frame_interval, + .s_stream = csi_s_stream, +}; + +static const struct v4l2_subdev_pad_ops csi_pad_ops = { + .enum_mbus_code = csi_enum_mbus_code, + .enum_frame_size = csi_enum_frame_size, + .enum_frame_interval = csi_enum_frame_interval, + .get_fmt = csi_get_fmt, + .set_fmt = csi_set_fmt, + .get_selection = csi_get_selection, + .set_selection = csi_set_selection, + .link_validate = csi_link_validate, +}; + +static const struct v4l2_subdev_ops csi_subdev_ops = { + .core = &csi_core_ops, + .video = &csi_video_ops, + .pad = &csi_pad_ops, +}; + +static const struct v4l2_subdev_internal_ops csi_internal_ops = { + .registered = csi_registered, + .unregistered = csi_unregistered, +}; + +static int imx_csi_probe(struct platform_device *pdev) +{ + struct ipu_client_platformdata *pdata; + struct pinctrl *pinctrl; + struct csi_priv *priv; + int ret; + + priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + platform_set_drvdata(pdev, &priv->sd); + priv->dev = &pdev->dev; + + ret = dma_set_coherent_mask(priv->dev, DMA_BIT_MASK(32)); + if (ret) + return ret; + + /* get parent IPU */ + priv->ipu = dev_get_drvdata(priv->dev->parent); + + /* get our CSI id */ + pdata = priv->dev->platform_data; + priv->csi_id = pdata->csi; + priv->smfc_id = (priv->csi_id == 0) ? 0 : 2; + + init_timer(&priv->eof_timeout_timer); + priv->eof_timeout_timer.data = (unsigned long)priv; + priv->eof_timeout_timer.function = csi_idmac_eof_timeout; + spin_lock_init(&priv->irqlock); + + v4l2_subdev_init(&priv->sd, &csi_subdev_ops); + v4l2_set_subdevdata(&priv->sd, priv); + priv->sd.internal_ops = &csi_internal_ops; + priv->sd.entity.ops = &csi_entity_ops; + priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; + priv->sd.dev = &pdev->dev; + priv->sd.fwnode = of_fwnode_handle(pdata->of_node); + priv->sd.owner = THIS_MODULE; + priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS; + priv->sd.grp_id = priv->csi_id ? + IMX_MEDIA_GRP_ID_CSI1 : IMX_MEDIA_GRP_ID_CSI0; + imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name), + priv->sd.grp_id, ipu_get_num(priv->ipu)); + + priv->vdev = imx_media_capture_device_init(&priv->sd, + CSI_SRC_PAD_IDMAC); + if (IS_ERR(priv->vdev)) + return PTR_ERR(priv->vdev); + + mutex_init(&priv->lock); + + v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0); + priv->sd.ctrl_handler = &priv->ctrl_hdlr; + + /* + * The IPUv3 driver did not assign an of_node to this + * device. As a result, pinctrl does not automatically + * configure our pin groups, so we need to do that manually + * here, after setting this device's of_node. + */ + priv->dev->of_node = pdata->of_node; + pinctrl = devm_pinctrl_get_select_default(priv->dev); + + ret = v4l2_async_register_subdev(&priv->sd); + if (ret) + goto free; + + return 0; +free: + v4l2_ctrl_handler_free(&priv->ctrl_hdlr); + mutex_destroy(&priv->lock); + imx_media_capture_device_remove(priv->vdev); + return ret; +} + +static int imx_csi_remove(struct platform_device *pdev) +{ + struct v4l2_subdev *sd = platform_get_drvdata(pdev); + struct csi_priv *priv = sd_to_dev(sd); + + v4l2_ctrl_handler_free(&priv->ctrl_hdlr); + mutex_destroy(&priv->lock); + imx_media_capture_device_remove(priv->vdev); + v4l2_async_unregister_subdev(sd); + media_entity_cleanup(&sd->entity); + + return 0; +} + +static const struct platform_device_id imx_csi_ids[] = { + { .name = "imx-ipuv3-csi" }, + { }, +}; +MODULE_DEVICE_TABLE(platform, imx_csi_ids); + +static struct platform_driver imx_csi_driver = { + .probe = imx_csi_probe, + .remove = imx_csi_remove, + .id_table = imx_csi_ids, + .driver = { + .name = "imx-ipuv3-csi", + }, +}; +module_platform_driver(imx_csi_driver); + +MODULE_DESCRIPTION("i.MX CSI subdev driver"); +MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("platform:imx-ipuv3-csi"); diff --git a/drivers/staging/media/imx/imx-media-dev.c b/drivers/staging/media/imx/imx-media-dev.c new file mode 100644 index 000000000000..48cbc7716758 --- /dev/null +++ b/drivers/staging/media/imx/imx-media-dev.c @@ -0,0 +1,667 @@ +/* + * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC + * + * Copyright (c) 2016 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/delay.h> +#include <linux/fs.h> +#include <linux/module.h> +#include <linux/of_platform.h> +#include <linux/pinctrl/consumer.h> +#include <linux/platform_device.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/spinlock.h> +#include <linux/timer.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-event.h> +#include <media/v4l2-ioctl.h> +#include <media/v4l2-mc.h> +#include <video/imx-ipu-v3.h> +#include <media/imx.h> +#include "imx-media.h" + +static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n) +{ + return container_of(n, struct imx_media_dev, subdev_notifier); +} + +/* + * Find a subdev by device node or device name. This is called during + * driver load to form the async subdev list and bind them. + */ +struct imx_media_subdev * +imx_media_find_async_subdev(struct imx_media_dev *imxmd, + struct device_node *np, + const char *devname) +{ + struct fwnode_handle *fwnode = np ? of_fwnode_handle(np) : NULL; + struct imx_media_subdev *imxsd; + int i; + + for (i = 0; i < imxmd->subdev_notifier.num_subdevs; i++) { + imxsd = &imxmd->subdev[i]; + switch (imxsd->asd.match_type) { + case V4L2_ASYNC_MATCH_FWNODE: + if (fwnode && imxsd->asd.match.fwnode.fwnode == fwnode) + return imxsd; + break; + case V4L2_ASYNC_MATCH_DEVNAME: + if (devname && + !strcmp(imxsd->asd.match.device_name.name, devname)) + return imxsd; + break; + default: + break; + } + } + + return NULL; +} + + +/* + * Adds a subdev to the async subdev list. If np is non-NULL, adds + * the async as a V4L2_ASYNC_MATCH_FWNODE match type, otherwise as + * a V4L2_ASYNC_MATCH_DEVNAME match type using the dev_name of the + * given platform_device. This is called during driver load when + * forming the async subdev list. + */ +struct imx_media_subdev * +imx_media_add_async_subdev(struct imx_media_dev *imxmd, + struct device_node *np, + struct platform_device *pdev) +{ + struct imx_media_subdev *imxsd; + struct v4l2_async_subdev *asd; + const char *devname = NULL; + int sd_idx; + + mutex_lock(&imxmd->mutex); + + if (pdev) + devname = dev_name(&pdev->dev); + + /* return NULL if this subdev already added */ + if (imx_media_find_async_subdev(imxmd, np, devname)) { + dev_dbg(imxmd->md.dev, "%s: already added %s\n", + __func__, np ? np->name : devname); + imxsd = NULL; + goto out; + } + + sd_idx = imxmd->subdev_notifier.num_subdevs; + if (sd_idx >= IMX_MEDIA_MAX_SUBDEVS) { + dev_err(imxmd->md.dev, "%s: too many subdevs! can't add %s\n", + __func__, np ? np->name : devname); + imxsd = ERR_PTR(-ENOSPC); + goto out; + } + + imxsd = &imxmd->subdev[sd_idx]; + + asd = &imxsd->asd; + if (np) { + asd->match_type = V4L2_ASYNC_MATCH_FWNODE; + asd->match.fwnode.fwnode = of_fwnode_handle(np); + } else { + asd->match_type = V4L2_ASYNC_MATCH_DEVNAME; + strncpy(imxsd->devname, devname, sizeof(imxsd->devname)); + asd->match.device_name.name = imxsd->devname; + imxsd->pdev = pdev; + } + + imxmd->async_ptrs[sd_idx] = asd; + imxmd->subdev_notifier.num_subdevs++; + + dev_dbg(imxmd->md.dev, "%s: added %s, match type %s\n", + __func__, np ? np->name : devname, np ? "FWNODE" : "DEVNAME"); + +out: + mutex_unlock(&imxmd->mutex); + return imxsd; +} + +/* + * Adds an imx-media link to a subdev pad's link list. This is called + * during driver load when forming the links between subdevs. + * + * @pad: the local pad + * @remote_node: the device node of the remote subdev + * @remote_devname: the device name of the remote subdev + * @local_pad: local pad index + * @remote_pad: remote pad index + */ +int imx_media_add_pad_link(struct imx_media_dev *imxmd, + struct imx_media_pad *pad, + struct device_node *remote_node, + const char *remote_devname, + int local_pad, int remote_pad) +{ + struct imx_media_link *link; + int link_idx, ret = 0; + + mutex_lock(&imxmd->mutex); + + link_idx = pad->num_links; + if (link_idx >= IMX_MEDIA_MAX_LINKS) { + dev_err(imxmd->md.dev, "%s: too many links!\n", __func__); + ret = -ENOSPC; + goto out; + } + + link = &pad->link[link_idx]; + + link->remote_sd_node = remote_node; + if (remote_devname) + strncpy(link->remote_devname, remote_devname, + sizeof(link->remote_devname)); + + link->local_pad = local_pad; + link->remote_pad = remote_pad; + + pad->num_links++; +out: + mutex_unlock(&imxmd->mutex); + return ret; +} + +/* + * get IPU from this CSI and add it to the list of IPUs + * the media driver will control. + */ +static int imx_media_get_ipu(struct imx_media_dev *imxmd, + struct v4l2_subdev *csi_sd) +{ + struct ipu_soc *ipu; + int ipu_id; + + ipu = dev_get_drvdata(csi_sd->dev->parent); + if (!ipu) { + v4l2_err(&imxmd->v4l2_dev, + "CSI %s has no parent IPU!\n", csi_sd->name); + return -ENODEV; + } + + ipu_id = ipu_get_num(ipu); + if (ipu_id > 1) { + v4l2_err(&imxmd->v4l2_dev, "invalid IPU id %d!\n", ipu_id); + return -ENODEV; + } + + if (!imxmd->ipu[ipu_id]) + imxmd->ipu[ipu_id] = ipu; + + return 0; +} + +/* async subdev bound notifier */ +static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier, + struct v4l2_subdev *sd, + struct v4l2_async_subdev *asd) +{ + struct imx_media_dev *imxmd = notifier2dev(notifier); + struct device_node *np = to_of_node(sd->fwnode); + struct imx_media_subdev *imxsd; + int ret = 0; + + mutex_lock(&imxmd->mutex); + + imxsd = imx_media_find_async_subdev(imxmd, np, dev_name(sd->dev)); + if (!imxsd) { + ret = -EINVAL; + goto out; + } + + if (sd->grp_id & IMX_MEDIA_GRP_ID_CSI) { + ret = imx_media_get_ipu(imxmd, sd); + if (ret) + goto out_unlock; + } else if (sd->entity.function == MEDIA_ENT_F_VID_MUX) { + /* this is a video mux */ + sd->grp_id = IMX_MEDIA_GRP_ID_VIDMUX; + } else if (imxsd->num_sink_pads == 0) { + /* + * this is an original source of video frames, it + * could be a camera sensor, an analog decoder, or + * a bridge device (HDMI -> MIPI CSI-2 for example). + * This group ID is used to locate the entity that + * is the original source of video in a pipeline. + */ + sd->grp_id = IMX_MEDIA_GRP_ID_SENSOR; + } + + /* attach the subdev */ + imxsd->sd = sd; +out: + if (ret) + v4l2_warn(&imxmd->v4l2_dev, + "Received unknown subdev %s\n", sd->name); + else + v4l2_info(&imxmd->v4l2_dev, + "Registered subdev %s\n", sd->name); + +out_unlock: + mutex_unlock(&imxmd->mutex); + return ret; +} + +/* + * Create a single source->sink media link given a subdev and a single + * link from one of its source pads. Called after all subdevs have + * registered. + */ +static int imx_media_create_link(struct imx_media_dev *imxmd, + struct imx_media_subdev *src, + struct imx_media_link *link) +{ + struct imx_media_subdev *sink; + u16 source_pad, sink_pad; + int ret; + + sink = imx_media_find_async_subdev(imxmd, link->remote_sd_node, + link->remote_devname); + if (!sink) { + v4l2_warn(&imxmd->v4l2_dev, "%s: no sink for %s:%d\n", + __func__, src->sd->name, link->local_pad); + return 0; + } + + source_pad = link->local_pad; + sink_pad = link->remote_pad; + + v4l2_info(&imxmd->v4l2_dev, "%s: %s:%d -> %s:%d\n", __func__, + src->sd->name, source_pad, sink->sd->name, sink_pad); + + ret = media_create_pad_link(&src->sd->entity, source_pad, + &sink->sd->entity, sink_pad, 0); + if (ret) + v4l2_err(&imxmd->v4l2_dev, + "create_pad_link failed: %d\n", ret); + + return ret; +} + +/* + * create the media links from all imx-media pads and their links. + * Called after all subdevs have registered. + */ +static int imx_media_create_links(struct imx_media_dev *imxmd) +{ + struct imx_media_subdev *imxsd; + struct imx_media_link *link; + struct imx_media_pad *pad; + int num_pads, i, j, k; + int ret = 0; + + for (i = 0; i < imxmd->num_subdevs; i++) { + imxsd = &imxmd->subdev[i]; + num_pads = imxsd->num_sink_pads + imxsd->num_src_pads; + + for (j = 0; j < num_pads; j++) { + pad = &imxsd->pad[j]; + + /* only create the source->sink links */ + if (!(pad->pad.flags & MEDIA_PAD_FL_SOURCE)) + continue; + + for (k = 0; k < pad->num_links; k++) { + link = &pad->link[k]; + + ret = imx_media_create_link(imxmd, imxsd, link); + if (ret) + goto out; + } + } + } + +out: + return ret; +} + +/* + * adds given video device to given imx-media source pad vdev list. + * Continues upstream from the pad entity's sink pads. + */ +static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd, + struct imx_media_video_dev *vdev, + struct media_pad *srcpad) +{ + struct media_entity *entity = srcpad->entity; + struct imx_media_subdev *imxsd; + struct imx_media_pad *imxpad; + struct media_link *link; + struct v4l2_subdev *sd; + int i, vdev_idx, ret; + + /* skip this entity if not a v4l2_subdev */ + if (!is_media_entity_v4l2_subdev(entity)) + return 0; + + sd = media_entity_to_v4l2_subdev(entity); + imxsd = imx_media_find_subdev_by_sd(imxmd, sd); + if (IS_ERR(imxsd)) + return PTR_ERR(imxsd); + + imxpad = &imxsd->pad[srcpad->index]; + vdev_idx = imxpad->num_vdevs; + + /* just return if we've been here before */ + for (i = 0; i < vdev_idx; i++) + if (vdev == imxpad->vdev[i]) + return 0; + + if (vdev_idx >= IMX_MEDIA_MAX_VDEVS) { + dev_err(imxmd->md.dev, "can't add %s to pad %s:%u\n", + vdev->vfd->entity.name, entity->name, srcpad->index); + return -ENOSPC; + } + + dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n", + vdev->vfd->entity.name, entity->name, srcpad->index); + imxpad->vdev[vdev_idx] = vdev; + imxpad->num_vdevs++; + + /* move upstream from this entity's sink pads */ + for (i = 0; i < entity->num_pads; i++) { + struct media_pad *pad = &entity->pads[i]; + + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + continue; + + list_for_each_entry(link, &entity->links, list) { + if (link->sink != pad) + continue; + ret = imx_media_add_vdev_to_pad(imxmd, vdev, + link->source); + if (ret) + return ret; + } + } + + return 0; +} + +/* form the vdev lists in all imx-media source pads */ +static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd) +{ + struct imx_media_video_dev *vdev; + struct media_link *link; + int i, ret; + + for (i = 0; i < imxmd->num_vdevs; i++) { + vdev = imxmd->vdev[i]; + link = list_first_entry(&vdev->vfd->entity.links, + struct media_link, list); + ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source); + if (ret) + break; + } + + return ret; +} + +/* async subdev complete notifier */ +static int imx_media_probe_complete(struct v4l2_async_notifier *notifier) +{ + struct imx_media_dev *imxmd = notifier2dev(notifier); + int i, ret; + + mutex_lock(&imxmd->mutex); + + /* make sure all subdevs were bound */ + for (i = 0; i < imxmd->num_subdevs; i++) { + if (!imxmd->subdev[i].sd) { + v4l2_err(&imxmd->v4l2_dev, "unbound subdev!\n"); + ret = -ENODEV; + goto unlock; + } + } + + ret = imx_media_create_links(imxmd); + if (ret) + goto unlock; + + ret = imx_media_create_pad_vdev_lists(imxmd); + if (ret) + goto unlock; + + ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev); +unlock: + mutex_unlock(&imxmd->mutex); + if (ret) + return ret; + + return media_device_register(&imxmd->md); +} + +/* + * adds controls to a video device from an entity subdevice. + * Continues upstream from the entity's sink pads. + */ +static int imx_media_inherit_controls(struct imx_media_dev *imxmd, + struct video_device *vfd, + struct media_entity *entity) +{ + int i, ret = 0; + + if (is_media_entity_v4l2_subdev(entity)) { + struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); + + dev_dbg(imxmd->md.dev, + "adding controls to %s from %s\n", + vfd->entity.name, sd->entity.name); + + ret = v4l2_ctrl_add_handler(vfd->ctrl_handler, + sd->ctrl_handler, + NULL); + if (ret) + return ret; + } + + /* move upstream */ + for (i = 0; i < entity->num_pads; i++) { + struct media_pad *pad, *spad = &entity->pads[i]; + + if (!(spad->flags & MEDIA_PAD_FL_SINK)) + continue; + + pad = media_entity_remote_pad(spad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + continue; + + ret = imx_media_inherit_controls(imxmd, vfd, pad->entity); + if (ret) + break; + } + + return ret; +} + +static int imx_media_link_notify(struct media_link *link, u32 flags, + unsigned int notification) +{ + struct media_entity *source = link->source->entity; + struct imx_media_subdev *imxsd; + struct imx_media_pad *imxpad; + struct imx_media_dev *imxmd; + struct video_device *vfd; + struct v4l2_subdev *sd; + int i, pad_idx, ret; + + ret = v4l2_pipeline_link_notify(link, flags, notification); + if (ret) + return ret; + + /* don't bother if source is not a subdev */ + if (!is_media_entity_v4l2_subdev(source)) + return 0; + + sd = media_entity_to_v4l2_subdev(source); + pad_idx = link->source->index; + + imxmd = dev_get_drvdata(sd->v4l2_dev->dev); + + imxsd = imx_media_find_subdev_by_sd(imxmd, sd); + if (IS_ERR(imxsd)) + return PTR_ERR(imxsd); + imxpad = &imxsd->pad[pad_idx]; + + /* + * Before disabling a link, reset controls for all video + * devices reachable from this link. + * + * After enabling a link, refresh controls for all video + * devices reachable from this link. + */ + if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH && + !(flags & MEDIA_LNK_FL_ENABLED)) { + for (i = 0; i < imxpad->num_vdevs; i++) { + vfd = imxpad->vdev[i]->vfd; + dev_dbg(imxmd->md.dev, + "reset controls for %s\n", + vfd->entity.name); + v4l2_ctrl_handler_free(vfd->ctrl_handler); + v4l2_ctrl_handler_init(vfd->ctrl_handler, 0); + } + } else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH && + (link->flags & MEDIA_LNK_FL_ENABLED)) { + for (i = 0; i < imxpad->num_vdevs; i++) { + vfd = imxpad->vdev[i]->vfd; + dev_dbg(imxmd->md.dev, + "refresh controls for %s\n", + vfd->entity.name); + ret = imx_media_inherit_controls(imxmd, vfd, + &vfd->entity); + if (ret) + break; + } + } + + return ret; +} + +static const struct media_device_ops imx_media_md_ops = { + .link_notify = imx_media_link_notify, +}; + +static int imx_media_probe(struct platform_device *pdev) +{ + struct device *dev = &pdev->dev; + struct device_node *node = dev->of_node; + struct imx_media_subdev *csi[4] = {0}; + struct imx_media_dev *imxmd; + int ret; + + imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL); + if (!imxmd) + return -ENOMEM; + + dev_set_drvdata(dev, imxmd); + + strlcpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model)); + imxmd->md.ops = &imx_media_md_ops; + imxmd->md.dev = dev; + + mutex_init(&imxmd->mutex); + + imxmd->v4l2_dev.mdev = &imxmd->md; + strlcpy(imxmd->v4l2_dev.name, "imx-media", + sizeof(imxmd->v4l2_dev.name)); + + media_device_init(&imxmd->md); + + ret = v4l2_device_register(dev, &imxmd->v4l2_dev); + if (ret < 0) { + v4l2_err(&imxmd->v4l2_dev, + "Failed to register v4l2_device: %d\n", ret); + goto cleanup; + } + + dev_set_drvdata(imxmd->v4l2_dev.dev, imxmd); + + ret = imx_media_of_parse(imxmd, &csi, node); + if (ret) { + v4l2_err(&imxmd->v4l2_dev, + "imx_media_of_parse failed with %d\n", ret); + goto unreg_dev; + } + + ret = imx_media_add_internal_subdevs(imxmd, csi); + if (ret) { + v4l2_err(&imxmd->v4l2_dev, + "add_internal_subdevs failed with %d\n", ret); + goto unreg_dev; + } + + /* no subdevs? just bail */ + imxmd->num_subdevs = imxmd->subdev_notifier.num_subdevs; + if (imxmd->num_subdevs == 0) { + ret = -ENODEV; + goto unreg_dev; + } + + /* prepare the async subdev notifier and register it */ + imxmd->subdev_notifier.subdevs = imxmd->async_ptrs; + imxmd->subdev_notifier.bound = imx_media_subdev_bound; + imxmd->subdev_notifier.complete = imx_media_probe_complete; + ret = v4l2_async_notifier_register(&imxmd->v4l2_dev, + &imxmd->subdev_notifier); + if (ret) { + v4l2_err(&imxmd->v4l2_dev, + "v4l2_async_notifier_register failed with %d\n", ret); + goto del_int; + } + + return 0; + +del_int: + imx_media_remove_internal_subdevs(imxmd); +unreg_dev: + v4l2_device_unregister(&imxmd->v4l2_dev); +cleanup: + media_device_cleanup(&imxmd->md); + return ret; +} + +static int imx_media_remove(struct platform_device *pdev) +{ + struct imx_media_dev *imxmd = + (struct imx_media_dev *)platform_get_drvdata(pdev); + + v4l2_info(&imxmd->v4l2_dev, "Removing imx-media\n"); + + v4l2_async_notifier_unregister(&imxmd->subdev_notifier); + imx_media_remove_internal_subdevs(imxmd); + v4l2_device_unregister(&imxmd->v4l2_dev); + media_device_unregister(&imxmd->md); + media_device_cleanup(&imxmd->md); + + return 0; +} + +static const struct of_device_id imx_media_dt_ids[] = { + { .compatible = "fsl,imx-capture-subsystem" }, + { /* sentinel */ } +}; +MODULE_DEVICE_TABLE(of, imx_media_dt_ids); + +static struct platform_driver imx_media_pdrv = { + .probe = imx_media_probe, + .remove = imx_media_remove, + .driver = { + .name = "imx-media", + .of_match_table = imx_media_dt_ids, + }, +}; + +module_platform_driver(imx_media_pdrv); + +MODULE_DESCRIPTION("i.MX5/6 v4l2 media controller driver"); +MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>"); +MODULE_LICENSE("GPL"); diff --git a/drivers/staging/media/imx/imx-media-fim.c b/drivers/staging/media/imx/imx-media-fim.c new file mode 100644 index 000000000000..47275ef803f3 --- /dev/null +++ b/drivers/staging/media/imx/imx-media-fim.c @@ -0,0 +1,494 @@ +/* + * Frame Interval Monitor. + * + * Copyright (c) 2016 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/delay.h> +#include <linux/irq.h> +#include <linux/module.h> +#include <linux/platform_device.h> +#include <linux/slab.h> +#include <linux/spinlock.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-subdev.h> +#include <media/imx.h> +#include "imx-media.h" + +enum { + FIM_CL_ENABLE = 0, + FIM_CL_NUM, + FIM_CL_TOLERANCE_MIN, + FIM_CL_TOLERANCE_MAX, + FIM_CL_NUM_SKIP, + FIM_NUM_CONTROLS, +}; + +enum { + FIM_CL_ICAP_EDGE = 0, + FIM_CL_ICAP_CHANNEL, + FIM_NUM_ICAP_CONTROLS, +}; + +#define FIM_CL_ENABLE_DEF 0 /* FIM disabled by default */ +#define FIM_CL_NUM_DEF 8 /* average 8 frames */ +#define FIM_CL_NUM_SKIP_DEF 2 /* skip 2 frames after restart */ +#define FIM_CL_TOLERANCE_MIN_DEF 50 /* usec */ +#define FIM_CL_TOLERANCE_MAX_DEF 0 /* no max tolerance (unbounded) */ + +struct imx_media_fim { + struct imx_media_dev *md; + + /* the owning subdev of this fim instance */ + struct v4l2_subdev *sd; + + /* FIM's control handler */ + struct v4l2_ctrl_handler ctrl_handler; + + /* control clusters */ + struct v4l2_ctrl *ctrl[FIM_NUM_CONTROLS]; + struct v4l2_ctrl *icap_ctrl[FIM_NUM_ICAP_CONTROLS]; + + spinlock_t lock; /* protect control values */ + + /* current control values */ + bool enabled; + int num_avg; + int num_skip; + unsigned long tolerance_min; /* usec */ + unsigned long tolerance_max; /* usec */ + /* input capture method of measuring FI */ + int icap_channel; + int icap_flags; + + int counter; + struct timespec last_ts; + unsigned long sum; /* usec */ + unsigned long nominal; /* usec */ + + struct completion icap_first_event; + bool stream_on; +}; + +#define icap_enabled(fim) ((fim)->icap_flags != IRQ_TYPE_NONE) + +static void update_fim_nominal(struct imx_media_fim *fim, + const struct v4l2_fract *fi) +{ + if (fi->denominator == 0) { + dev_dbg(fim->sd->dev, "no frame interval, FIM disabled\n"); + fim->enabled = false; + return; + } + + fim->nominal = DIV_ROUND_CLOSEST_ULL(1000000ULL * (u64)fi->numerator, + fi->denominator); + + dev_dbg(fim->sd->dev, "FI=%lu usec\n", fim->nominal); +} + +static void reset_fim(struct imx_media_fim *fim, bool curval) +{ + struct v4l2_ctrl *icap_chan = fim->icap_ctrl[FIM_CL_ICAP_CHANNEL]; + struct v4l2_ctrl *icap_edge = fim->icap_ctrl[FIM_CL_ICAP_EDGE]; + struct v4l2_ctrl *en = fim->ctrl[FIM_CL_ENABLE]; + struct v4l2_ctrl *num = fim->ctrl[FIM_CL_NUM]; + struct v4l2_ctrl *skip = fim->ctrl[FIM_CL_NUM_SKIP]; + struct v4l2_ctrl *tol_min = fim->ctrl[FIM_CL_TOLERANCE_MIN]; + struct v4l2_ctrl *tol_max = fim->ctrl[FIM_CL_TOLERANCE_MAX]; + + if (curval) { + fim->enabled = en->cur.val; + fim->icap_flags = icap_edge->cur.val; + fim->icap_channel = icap_chan->cur.val; + fim->num_avg = num->cur.val; + fim->num_skip = skip->cur.val; + fim->tolerance_min = tol_min->cur.val; + fim->tolerance_max = tol_max->cur.val; + } else { + fim->enabled = en->val; + fim->icap_flags = icap_edge->val; + fim->icap_channel = icap_chan->val; + fim->num_avg = num->val; + fim->num_skip = skip->val; + fim->tolerance_min = tol_min->val; + fim->tolerance_max = tol_max->val; + } + + /* disable tolerance range if max <= min */ + if (fim->tolerance_max <= fim->tolerance_min) + fim->tolerance_max = 0; + + /* num_skip must be >= 1 if input capture not used */ + if (!icap_enabled(fim)) + fim->num_skip = max_t(int, fim->num_skip, 1); + + fim->counter = -fim->num_skip; + fim->sum = 0; +} + +static void send_fim_event(struct imx_media_fim *fim, unsigned long error) +{ + static const struct v4l2_event ev = { + .type = V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR, + }; + + v4l2_subdev_notify_event(fim->sd, &ev); +} + +/* + * Monitor an averaged frame interval. If the average deviates too much + * from the nominal frame rate, send the frame interval error event. The + * frame intervals are averaged in order to quiet noise from + * (presumably random) interrupt latency. + */ +static void frame_interval_monitor(struct imx_media_fim *fim, + struct timespec *ts) +{ + unsigned long interval, error, error_avg; + bool send_event = false; + struct timespec diff; + + if (!fim->enabled || ++fim->counter <= 0) + goto out_update_ts; + + diff = timespec_sub(*ts, fim->last_ts); + interval = diff.tv_sec * 1000 * 1000 + diff.tv_nsec / 1000; + error = abs(interval - fim->nominal); + + if (fim->tolerance_max && error >= fim->tolerance_max) { + dev_dbg(fim->sd->dev, + "FIM: %lu ignored, out of tolerance bounds\n", + error); + fim->counter--; + goto out_update_ts; + } + + fim->sum += error; + + if (fim->counter == fim->num_avg) { + error_avg = DIV_ROUND_CLOSEST(fim->sum, fim->num_avg); + + if (error_avg > fim->tolerance_min) + send_event = true; + + dev_dbg(fim->sd->dev, "FIM: error: %lu usec%s\n", + error_avg, send_event ? " (!!!)" : ""); + + fim->counter = 0; + fim->sum = 0; + } + +out_update_ts: + fim->last_ts = *ts; + if (send_event) + send_fim_event(fim, error_avg); +} + +#ifdef CONFIG_IMX_GPT_ICAP +/* + * Input Capture method of measuring frame intervals. Not subject + * to interrupt latency. + */ +static void fim_input_capture_handler(int channel, void *dev_id, + struct timespec *ts) +{ + struct imx_media_fim *fim = dev_id; + unsigned long flags; + + spin_lock_irqsave(&fim->lock, flags); + + frame_interval_monitor(fim, ts); + + if (!completion_done(&fim->icap_first_event)) + complete(&fim->icap_first_event); + + spin_unlock_irqrestore(&fim->lock, flags); +} + +static int fim_request_input_capture(struct imx_media_fim *fim) +{ + init_completion(&fim->icap_first_event); + + return mxc_request_input_capture(fim->icap_channel, + fim_input_capture_handler, + fim->icap_flags, fim); +} + +static void fim_free_input_capture(struct imx_media_fim *fim) +{ + mxc_free_input_capture(fim->icap_channel, fim); +} + +#else /* CONFIG_IMX_GPT_ICAP */ + +static int fim_request_input_capture(struct imx_media_fim *fim) +{ + return 0; +} + +static void fim_free_input_capture(struct imx_media_fim *fim) +{ +} + +#endif /* CONFIG_IMX_GPT_ICAP */ + +/* + * In case we are monitoring the first frame interval after streamon + * (when fim->num_skip = 0), we need a valid fim->last_ts before we + * can begin. This only applies to the input capture method. It is not + * possible to accurately measure the first FI after streamon using the + * EOF method, so fim->num_skip minimum is set to 1 in that case, so this + * function is a noop when the EOF method is used. + */ +static void fim_acquire_first_ts(struct imx_media_fim *fim) +{ + unsigned long ret; + + if (!fim->enabled || fim->num_skip > 0) + return; + + ret = wait_for_completion_timeout( + &fim->icap_first_event, + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT)); + if (ret == 0) + v4l2_warn(fim->sd, "wait first icap event timeout\n"); +} + +/* FIM Controls */ +static int fim_s_ctrl(struct v4l2_ctrl *ctrl) +{ + struct imx_media_fim *fim = container_of(ctrl->handler, + struct imx_media_fim, + ctrl_handler); + unsigned long flags; + int ret = 0; + + spin_lock_irqsave(&fim->lock, flags); + + switch (ctrl->id) { + case V4L2_CID_IMX_FIM_ENABLE: + break; + case V4L2_CID_IMX_FIM_ICAP_EDGE: + if (fim->stream_on) + ret = -EBUSY; + break; + default: + ret = -EINVAL; + } + + if (!ret) + reset_fim(fim, false); + + spin_unlock_irqrestore(&fim->lock, flags); + return ret; +} + +static const struct v4l2_ctrl_ops fim_ctrl_ops = { + .s_ctrl = fim_s_ctrl, +}; + +static const struct v4l2_ctrl_config fim_ctrl[] = { + [FIM_CL_ENABLE] = { + .ops = &fim_ctrl_ops, + .id = V4L2_CID_IMX_FIM_ENABLE, + .name = "FIM Enable", + .type = V4L2_CTRL_TYPE_BOOLEAN, + .def = FIM_CL_ENABLE_DEF, + .min = 0, + .max = 1, + .step = 1, + }, + [FIM_CL_NUM] = { + .ops = &fim_ctrl_ops, + .id = V4L2_CID_IMX_FIM_NUM, + .name = "FIM Num Average", + .type = V4L2_CTRL_TYPE_INTEGER, + .def = FIM_CL_NUM_DEF, + .min = 1, /* no averaging */ + .max = 64, /* average 64 frames */ + .step = 1, + }, + [FIM_CL_TOLERANCE_MIN] = { + .ops = &fim_ctrl_ops, + .id = V4L2_CID_IMX_FIM_TOLERANCE_MIN, + .name = "FIM Tolerance Min", + .type = V4L2_CTRL_TYPE_INTEGER, + .def = FIM_CL_TOLERANCE_MIN_DEF, + .min = 2, + .max = 200, + .step = 1, + }, + [FIM_CL_TOLERANCE_MAX] = { + .ops = &fim_ctrl_ops, + .id = V4L2_CID_IMX_FIM_TOLERANCE_MAX, + .name = "FIM Tolerance Max", + .type = V4L2_CTRL_TYPE_INTEGER, + .def = FIM_CL_TOLERANCE_MAX_DEF, + .min = 0, + .max = 500, + .step = 1, + }, + [FIM_CL_NUM_SKIP] = { + .ops = &fim_ctrl_ops, + .id = V4L2_CID_IMX_FIM_NUM_SKIP, + .name = "FIM Num Skip", + .type = V4L2_CTRL_TYPE_INTEGER, + .def = FIM_CL_NUM_SKIP_DEF, + .min = 0, /* skip no frames */ + .max = 256, /* skip 256 frames */ + .step = 1, + }, +}; + +static const struct v4l2_ctrl_config fim_icap_ctrl[] = { + [FIM_CL_ICAP_EDGE] = { + .ops = &fim_ctrl_ops, + .id = V4L2_CID_IMX_FIM_ICAP_EDGE, + .name = "FIM Input Capture Edge", + .type = V4L2_CTRL_TYPE_INTEGER, + .def = IRQ_TYPE_NONE, /* input capture disabled by default */ + .min = IRQ_TYPE_NONE, + .max = IRQ_TYPE_EDGE_BOTH, + .step = 1, + }, + [FIM_CL_ICAP_CHANNEL] = { + .ops = &fim_ctrl_ops, + .id = V4L2_CID_IMX_FIM_ICAP_CHANNEL, + .name = "FIM Input Capture Channel", + .type = V4L2_CTRL_TYPE_INTEGER, + .def = 0, + .min = 0, + .max = 1, + .step = 1, + }, +}; + +static int init_fim_controls(struct imx_media_fim *fim) +{ + struct v4l2_ctrl_handler *hdlr = &fim->ctrl_handler; + int i, ret; + + v4l2_ctrl_handler_init(hdlr, FIM_NUM_CONTROLS + FIM_NUM_ICAP_CONTROLS); + + for (i = 0; i < FIM_NUM_CONTROLS; i++) + fim->ctrl[i] = v4l2_ctrl_new_custom(hdlr, + &fim_ctrl[i], + NULL); + for (i = 0; i < FIM_NUM_ICAP_CONTROLS; i++) + fim->icap_ctrl[i] = v4l2_ctrl_new_custom(hdlr, + &fim_icap_ctrl[i], + NULL); + if (hdlr->error) { + ret = hdlr->error; + goto err_free; + } + + v4l2_ctrl_cluster(FIM_NUM_CONTROLS, fim->ctrl); + v4l2_ctrl_cluster(FIM_NUM_ICAP_CONTROLS, fim->icap_ctrl); + + return 0; +err_free: + v4l2_ctrl_handler_free(hdlr); + return ret; +} + +/* + * Monitor frame intervals via EOF interrupt. This method is + * subject to uncertainty errors introduced by interrupt latency. + * + * This is a noop if the Input Capture method is being used, since + * the frame_interval_monitor() is called by the input capture event + * callback handler in that case. + */ +void imx_media_fim_eof_monitor(struct imx_media_fim *fim, struct timespec *ts) +{ + unsigned long flags; + + spin_lock_irqsave(&fim->lock, flags); + + if (!icap_enabled(fim)) + frame_interval_monitor(fim, ts); + + spin_unlock_irqrestore(&fim->lock, flags); +} +EXPORT_SYMBOL_GPL(imx_media_fim_eof_monitor); + +/* Called by the subdev in its s_stream callback */ +int imx_media_fim_set_stream(struct imx_media_fim *fim, + const struct v4l2_fract *fi, + bool on) +{ + unsigned long flags; + int ret = 0; + + v4l2_ctrl_lock(fim->ctrl[FIM_CL_ENABLE]); + + if (fim->stream_on == on) + goto out; + + if (on) { + spin_lock_irqsave(&fim->lock, flags); + reset_fim(fim, true); + update_fim_nominal(fim, fi); + spin_unlock_irqrestore(&fim->lock, flags); + + if (icap_enabled(fim)) { + ret = fim_request_input_capture(fim); + if (ret) + goto out; + fim_acquire_first_ts(fim); + } + } else { + if (icap_enabled(fim)) + fim_free_input_capture(fim); + } + + fim->stream_on = on; +out: + v4l2_ctrl_unlock(fim->ctrl[FIM_CL_ENABLE]); + return ret; +} +EXPORT_SYMBOL_GPL(imx_media_fim_set_stream); + +int imx_media_fim_add_controls(struct imx_media_fim *fim) +{ + /* add the FIM controls to the calling subdev ctrl handler */ + return v4l2_ctrl_add_handler(fim->sd->ctrl_handler, + &fim->ctrl_handler, NULL); +} +EXPORT_SYMBOL_GPL(imx_media_fim_add_controls); + +/* Called by the subdev in its subdev registered callback */ +struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd) +{ + struct imx_media_fim *fim; + int ret; + + fim = devm_kzalloc(sd->dev, sizeof(*fim), GFP_KERNEL); + if (!fim) + return ERR_PTR(-ENOMEM); + + /* get media device */ + fim->md = dev_get_drvdata(sd->v4l2_dev->dev); + fim->sd = sd; + + spin_lock_init(&fim->lock); + + ret = init_fim_controls(fim); + if (ret) + return ERR_PTR(ret); + + return fim; +} +EXPORT_SYMBOL_GPL(imx_media_fim_init); + +void imx_media_fim_free(struct imx_media_fim *fim) +{ + v4l2_ctrl_handler_free(&fim->ctrl_handler); +} +EXPORT_SYMBOL_GPL(imx_media_fim_free); diff --git a/drivers/staging/media/imx/imx-media-internal-sd.c b/drivers/staging/media/imx/imx-media-internal-sd.c new file mode 100644 index 000000000000..cdfbf40dfcbe --- /dev/null +++ b/drivers/staging/media/imx/imx-media-internal-sd.c @@ -0,0 +1,349 @@ +/* + * Media driver for Freescale i.MX5/6 SOC + * + * Adds the internal subdevices and the media links between them. + * + * Copyright (c) 2016 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/platform_device.h> +#include "imx-media.h" + +enum isd_enum { + isd_csi0 = 0, + isd_csi1, + isd_vdic, + isd_ic_prp, + isd_ic_prpenc, + isd_ic_prpvf, + num_isd, +}; + +static const struct internal_subdev_id { + enum isd_enum index; + const char *name; + u32 grp_id; +} isd_id[num_isd] = { + [isd_csi0] = { + .index = isd_csi0, + .grp_id = IMX_MEDIA_GRP_ID_CSI0, + .name = "imx-ipuv3-csi", + }, + [isd_csi1] = { + .index = isd_csi1, + .grp_id = IMX_MEDIA_GRP_ID_CSI1, + .name = "imx-ipuv3-csi", + }, + [isd_vdic] = { + .index = isd_vdic, + .grp_id = IMX_MEDIA_GRP_ID_VDIC, + .name = "imx-ipuv3-vdic", + }, + [isd_ic_prp] = { + .index = isd_ic_prp, + .grp_id = IMX_MEDIA_GRP_ID_IC_PRP, + .name = "imx-ipuv3-ic", + }, + [isd_ic_prpenc] = { + .index = isd_ic_prpenc, + .grp_id = IMX_MEDIA_GRP_ID_IC_PRPENC, + .name = "imx-ipuv3-ic", + }, + [isd_ic_prpvf] = { + .index = isd_ic_prpvf, + .grp_id = IMX_MEDIA_GRP_ID_IC_PRPVF, + .name = "imx-ipuv3-ic", + }, +}; + +struct internal_link { + const struct internal_subdev_id *remote_id; + int remote_pad; +}; + +struct internal_pad { + bool devnode; /* does this pad link to a device node */ + struct internal_link link[IMX_MEDIA_MAX_LINKS]; +}; + +static const struct internal_subdev { + const struct internal_subdev_id *id; + struct internal_pad pad[IMX_MEDIA_MAX_PADS]; + int num_sink_pads; + int num_src_pads; +} internal_subdev[num_isd] = { + [isd_csi0] = { + .id = &isd_id[isd_csi0], + .num_sink_pads = CSI_NUM_SINK_PADS, + .num_src_pads = CSI_NUM_SRC_PADS, + .pad[CSI_SRC_PAD_DIRECT] = { + .link = { + { + .remote_id = &isd_id[isd_ic_prp], + .remote_pad = PRP_SINK_PAD, + }, { + .remote_id = &isd_id[isd_vdic], + .remote_pad = VDIC_SINK_PAD_DIRECT, + }, + }, + }, + .pad[CSI_SRC_PAD_IDMAC] = { + .devnode = true, + }, + }, + + [isd_csi1] = { + .id = &isd_id[isd_csi1], + .num_sink_pads = CSI_NUM_SINK_PADS, + .num_src_pads = CSI_NUM_SRC_PADS, + .pad[CSI_SRC_PAD_DIRECT] = { + .link = { + { + .remote_id = &isd_id[isd_ic_prp], + .remote_pad = PRP_SINK_PAD, + }, { + .remote_id = &isd_id[isd_vdic], + .remote_pad = VDIC_SINK_PAD_DIRECT, + }, + }, + }, + .pad[CSI_SRC_PAD_IDMAC] = { + .devnode = true, + }, + }, + + [isd_vdic] = { + .id = &isd_id[isd_vdic], + .num_sink_pads = VDIC_NUM_SINK_PADS, + .num_src_pads = VDIC_NUM_SRC_PADS, + .pad[VDIC_SINK_PAD_IDMAC] = { + .devnode = true, + }, + .pad[VDIC_SRC_PAD_DIRECT] = { + .link = { + { + .remote_id = &isd_id[isd_ic_prp], + .remote_pad = PRP_SINK_PAD, + }, + }, + }, + }, + + [isd_ic_prp] = { + .id = &isd_id[isd_ic_prp], + .num_sink_pads = PRP_NUM_SINK_PADS, + .num_src_pads = PRP_NUM_SRC_PADS, + .pad[PRP_SRC_PAD_PRPENC] = { + .link = { + { + .remote_id = &isd_id[isd_ic_prpenc], + .remote_pad = 0, + }, + }, + }, + .pad[PRP_SRC_PAD_PRPVF] = { + .link = { + { + .remote_id = &isd_id[isd_ic_prpvf], + .remote_pad = 0, + }, + }, + }, + }, + + [isd_ic_prpenc] = { + .id = &isd_id[isd_ic_prpenc], + .num_sink_pads = PRPENCVF_NUM_SINK_PADS, + .num_src_pads = PRPENCVF_NUM_SRC_PADS, + .pad[PRPENCVF_SRC_PAD] = { + .devnode = true, + }, + }, + + [isd_ic_prpvf] = { + .id = &isd_id[isd_ic_prpvf], + .num_sink_pads = PRPENCVF_NUM_SINK_PADS, + .num_src_pads = PRPENCVF_NUM_SRC_PADS, + .pad[PRPENCVF_SRC_PAD] = { + .devnode = true, + }, + }, +}; + +/* form a device name given a group id and ipu id */ +static inline void isd_id_to_devname(char *devname, int sz, + const struct internal_subdev_id *id, + int ipu_id) +{ + int pdev_id = ipu_id * num_isd + id->index; + + snprintf(devname, sz, "%s.%d", id->name, pdev_id); +} + +/* adds the links from given internal subdev */ +static int add_internal_links(struct imx_media_dev *imxmd, + const struct internal_subdev *isd, + struct imx_media_subdev *imxsd, + int ipu_id) +{ + int i, num_pads, ret; + + num_pads = isd->num_sink_pads + isd->num_src_pads; + + for (i = 0; i < num_pads; i++) { + const struct internal_pad *intpad = &isd->pad[i]; + struct imx_media_pad *pad = &imxsd->pad[i]; + int j; + + /* init the pad flags for this internal subdev */ + pad->pad.flags = (i < isd->num_sink_pads) ? + MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE; + /* export devnode pad flag to the subdevs */ + pad->devnode = intpad->devnode; + + for (j = 0; ; j++) { + const struct internal_link *link; + char remote_devname[32]; + + link = &intpad->link[j]; + + if (!link->remote_id) + break; + + isd_id_to_devname(remote_devname, + sizeof(remote_devname), + link->remote_id, ipu_id); + + ret = imx_media_add_pad_link(imxmd, pad, + NULL, remote_devname, + i, link->remote_pad); + if (ret) + return ret; + } + } + + return 0; +} + +/* register an internal subdev as a platform device */ +static struct imx_media_subdev * +add_internal_subdev(struct imx_media_dev *imxmd, + const struct internal_subdev *isd, + int ipu_id) +{ + struct imx_media_internal_sd_platformdata pdata; + struct platform_device_info pdevinfo = {0}; + struct imx_media_subdev *imxsd; + struct platform_device *pdev; + + pdata.grp_id = isd->id->grp_id; + + /* the id of IPU this subdev will control */ + pdata.ipu_id = ipu_id; + + /* create subdev name */ + imx_media_grp_id_to_sd_name(pdata.sd_name, sizeof(pdata.sd_name), + pdata.grp_id, ipu_id); + + pdevinfo.name = isd->id->name; + pdevinfo.id = ipu_id * num_isd + isd->id->index; + pdevinfo.parent = imxmd->md.dev; + pdevinfo.data = &pdata; + pdevinfo.size_data = sizeof(pdata); + pdevinfo.dma_mask = DMA_BIT_MASK(32); + + pdev = platform_device_register_full(&pdevinfo); + if (IS_ERR(pdev)) + return ERR_CAST(pdev); + + imxsd = imx_media_add_async_subdev(imxmd, NULL, pdev); + if (IS_ERR(imxsd)) + return imxsd; + + imxsd->num_sink_pads = isd->num_sink_pads; + imxsd->num_src_pads = isd->num_src_pads; + + return imxsd; +} + +/* adds the internal subdevs in one ipu */ +static int add_ipu_internal_subdevs(struct imx_media_dev *imxmd, + struct imx_media_subdev *csi0, + struct imx_media_subdev *csi1, + int ipu_id) +{ + enum isd_enum i; + int ret; + + for (i = 0; i < num_isd; i++) { + const struct internal_subdev *isd = &internal_subdev[i]; + struct imx_media_subdev *imxsd; + + /* + * the CSIs are represented in the device-tree, so those + * devices are added already, and are added to the async + * subdev list by of_parse_subdev(), so we are given those + * subdevs as csi0 and csi1. + */ + switch (isd->id->grp_id) { + case IMX_MEDIA_GRP_ID_CSI0: + imxsd = csi0; + break; + case IMX_MEDIA_GRP_ID_CSI1: + imxsd = csi1; + break; + default: + imxsd = add_internal_subdev(imxmd, isd, ipu_id); + break; + } + + if (IS_ERR(imxsd)) + return PTR_ERR(imxsd); + + /* add the links from this subdev */ + if (imxsd) { + ret = add_internal_links(imxmd, isd, imxsd, ipu_id); + if (ret) + return ret; + } + } + + return 0; +} + +int imx_media_add_internal_subdevs(struct imx_media_dev *imxmd, + struct imx_media_subdev *csi[4]) +{ + int ret; + + ret = add_ipu_internal_subdevs(imxmd, csi[0], csi[1], 0); + if (ret) + goto remove; + + ret = add_ipu_internal_subdevs(imxmd, csi[2], csi[3], 1); + if (ret) + goto remove; + + return 0; + +remove: + imx_media_remove_internal_subdevs(imxmd); + return ret; +} + +void imx_media_remove_internal_subdevs(struct imx_media_dev *imxmd) +{ + struct imx_media_subdev *imxsd; + int i; + + for (i = 0; i < imxmd->subdev_notifier.num_subdevs; i++) { + imxsd = &imxmd->subdev[i]; + if (!imxsd->pdev) + continue; + platform_device_unregister(imxsd->pdev); + } +} diff --git a/drivers/staging/media/imx/imx-media-of.c b/drivers/staging/media/imx/imx-media-of.c new file mode 100644 index 000000000000..b026fe66467c --- /dev/null +++ b/drivers/staging/media/imx/imx-media-of.c @@ -0,0 +1,270 @@ +/* + * Media driver for Freescale i.MX5/6 SOC + * + * Open Firmware parsing. + * + * Copyright (c) 2016 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/of_platform.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-fwnode.h> +#include <media/v4l2-subdev.h> +#include <media/videobuf2-dma-contig.h> +#include <linux/of_graph.h> +#include <video/imx-ipu-v3.h> +#include "imx-media.h" + +static int of_add_pad_link(struct imx_media_dev *imxmd, + struct imx_media_pad *pad, + struct device_node *local_sd_node, + struct device_node *remote_sd_node, + int local_pad, int remote_pad) +{ + dev_dbg(imxmd->md.dev, "%s: adding %s:%d -> %s:%d\n", __func__, + local_sd_node->name, local_pad, + remote_sd_node->name, remote_pad); + + return imx_media_add_pad_link(imxmd, pad, remote_sd_node, NULL, + local_pad, remote_pad); +} + +static void of_parse_sensor(struct imx_media_dev *imxmd, + struct imx_media_subdev *sensor, + struct device_node *sensor_np) +{ + struct device_node *endpoint; + + endpoint = of_graph_get_next_endpoint(sensor_np, NULL); + if (endpoint) { + v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint), + &sensor->sensor_ep); + of_node_put(endpoint); + } +} + +static int of_get_port_count(const struct device_node *np) +{ + struct device_node *ports, *child; + int num = 0; + + /* check if this node has a ports subnode */ + ports = of_get_child_by_name(np, "ports"); + if (ports) + np = ports; + + for_each_child_of_node(np, child) + if (of_node_cmp(child->name, "port") == 0) + num++; + + of_node_put(ports); + return num; +} + +/* + * find the remote device node and remote port id (remote pad #) + * given local endpoint node + */ +static void of_get_remote_pad(struct device_node *epnode, + struct device_node **remote_node, + int *remote_pad) +{ + struct device_node *rp, *rpp; + struct device_node *remote; + + rp = of_graph_get_remote_port(epnode); + rpp = of_graph_get_remote_port_parent(epnode); + + if (of_device_is_compatible(rpp, "fsl,imx6q-ipu")) { + /* the remote is one of the CSI ports */ + remote = rp; + *remote_pad = 0; + of_node_put(rpp); + } else { + remote = rpp; + if (of_property_read_u32(rp, "reg", remote_pad)) + *remote_pad = 0; + of_node_put(rp); + } + + if (!of_device_is_available(remote)) { + of_node_put(remote); + *remote_node = NULL; + } else { + *remote_node = remote; + } +} + +static struct imx_media_subdev * +of_parse_subdev(struct imx_media_dev *imxmd, struct device_node *sd_np, + bool is_csi_port) +{ + struct imx_media_subdev *imxsd; + int i, num_pads, ret; + + if (!of_device_is_available(sd_np)) { + dev_dbg(imxmd->md.dev, "%s: %s not enabled\n", __func__, + sd_np->name); + return NULL; + } + + /* register this subdev with async notifier */ + imxsd = imx_media_add_async_subdev(imxmd, sd_np, NULL); + if (IS_ERR_OR_NULL(imxsd)) + return imxsd; + + if (is_csi_port) { + /* + * the ipu-csi has one sink port and two source ports. + * The source ports are not represented in the device tree, + * but are described by the internal pads and links later. + */ + num_pads = CSI_NUM_PADS; + imxsd->num_sink_pads = CSI_NUM_SINK_PADS; + } else if (of_device_is_compatible(sd_np, "fsl,imx6-mipi-csi2")) { + num_pads = of_get_port_count(sd_np); + /* the mipi csi2 receiver has only one sink port */ + imxsd->num_sink_pads = 1; + } else if (of_device_is_compatible(sd_np, "video-mux")) { + num_pads = of_get_port_count(sd_np); + /* for the video mux, all but the last port are sinks */ + imxsd->num_sink_pads = num_pads - 1; + } else { + num_pads = of_get_port_count(sd_np); + if (num_pads != 1) { + dev_warn(imxmd->md.dev, + "%s: unknown device %s with %d ports\n", + __func__, sd_np->name, num_pads); + return NULL; + } + + /* + * we got to this node from this single source port, + * there are no sink pads. + */ + imxsd->num_sink_pads = 0; + } + + if (imxsd->num_sink_pads >= num_pads) + return ERR_PTR(-EINVAL); + + imxsd->num_src_pads = num_pads - imxsd->num_sink_pads; + + dev_dbg(imxmd->md.dev, "%s: %s has %d pads (%d sink, %d src)\n", + __func__, sd_np->name, num_pads, + imxsd->num_sink_pads, imxsd->num_src_pads); + + /* + * With no sink, this subdev node is the original source + * of video, parse it's media bus for use by the pipeline. + */ + if (imxsd->num_sink_pads == 0) + of_parse_sensor(imxmd, imxsd, sd_np); + + for (i = 0; i < num_pads; i++) { + struct device_node *epnode = NULL, *port, *remote_np; + struct imx_media_subdev *remote_imxsd; + struct imx_media_pad *pad; + int remote_pad; + + /* init this pad */ + pad = &imxsd->pad[i]; + pad->pad.flags = (i < imxsd->num_sink_pads) ? + MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE; + + if (is_csi_port) + port = (i < imxsd->num_sink_pads) ? sd_np : NULL; + else + port = of_graph_get_port_by_id(sd_np, i); + if (!port) + continue; + + for_each_child_of_node(port, epnode) { + of_get_remote_pad(epnode, &remote_np, &remote_pad); + if (!remote_np) + continue; + + ret = of_add_pad_link(imxmd, pad, sd_np, remote_np, + i, remote_pad); + if (ret) { + imxsd = ERR_PTR(ret); + break; + } + + if (i < imxsd->num_sink_pads) { + /* follow sink endpoints upstream */ + remote_imxsd = of_parse_subdev(imxmd, + remote_np, + false); + if (IS_ERR(remote_imxsd)) { + imxsd = remote_imxsd; + break; + } + } + + of_node_put(remote_np); + } + + if (port != sd_np) + of_node_put(port); + if (IS_ERR(imxsd)) { + of_node_put(remote_np); + of_node_put(epnode); + break; + } + } + + return imxsd; +} + +int imx_media_of_parse(struct imx_media_dev *imxmd, + struct imx_media_subdev *(*csi)[4], + struct device_node *np) +{ + struct imx_media_subdev *lcsi; + struct device_node *csi_np; + u32 ipu_id, csi_id; + int i, ret; + + for (i = 0; ; i++) { + csi_np = of_parse_phandle(np, "ports", i); + if (!csi_np) + break; + + lcsi = of_parse_subdev(imxmd, csi_np, true); + if (IS_ERR(lcsi)) { + ret = PTR_ERR(lcsi); + goto err_put; + } + + ret = of_property_read_u32(csi_np, "reg", &csi_id); + if (ret) { + dev_err(imxmd->md.dev, + "%s: csi port missing reg property!\n", + __func__); + goto err_put; + } + + ipu_id = of_alias_get_id(csi_np->parent, "ipu"); + of_node_put(csi_np); + + if (ipu_id > 1 || csi_id > 1) { + dev_err(imxmd->md.dev, + "%s: invalid ipu/csi id (%u/%u)\n", + __func__, ipu_id, csi_id); + return -EINVAL; + } + + (*csi)[ipu_id * 2 + csi_id] = lcsi; + } + + return 0; +err_put: + of_node_put(csi_np); + return ret; +} diff --git a/drivers/staging/media/imx/imx-media-utils.c b/drivers/staging/media/imx/imx-media-utils.c new file mode 100644 index 000000000000..59523872a886 --- /dev/null +++ b/drivers/staging/media/imx/imx-media-utils.c @@ -0,0 +1,896 @@ +/* + * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC + * + * Copyright (c) 2016 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/module.h> +#include "imx-media.h" + +/* + * List of supported pixel formats for the subdevs. + * + * In all of these tables, the non-mbus formats (with no + * mbus codes) must all fall at the end of the table. + */ + +static const struct imx_media_pixfmt yuv_formats[] = { + { + .fourcc = V4L2_PIX_FMT_UYVY, + .codes = { + MEDIA_BUS_FMT_UYVY8_2X8, + MEDIA_BUS_FMT_UYVY8_1X16 + }, + .cs = IPUV3_COLORSPACE_YUV, + .bpp = 16, + }, { + .fourcc = V4L2_PIX_FMT_YUYV, + .codes = { + MEDIA_BUS_FMT_YUYV8_2X8, + MEDIA_BUS_FMT_YUYV8_1X16 + }, + .cs = IPUV3_COLORSPACE_YUV, + .bpp = 16, + }, + /*** + * non-mbus YUV formats start here. NOTE! when adding non-mbus + * formats, NUM_NON_MBUS_YUV_FORMATS must be updated below. + ***/ + { + .fourcc = V4L2_PIX_FMT_YUV420, + .cs = IPUV3_COLORSPACE_YUV, + .bpp = 12, + .planar = true, + }, { + .fourcc = V4L2_PIX_FMT_YVU420, + .cs = IPUV3_COLORSPACE_YUV, + .bpp = 12, + .planar = true, + }, { + .fourcc = V4L2_PIX_FMT_YUV422P, + .cs = IPUV3_COLORSPACE_YUV, + .bpp = 16, + .planar = true, + }, { + .fourcc = V4L2_PIX_FMT_NV12, + .cs = IPUV3_COLORSPACE_YUV, + .bpp = 12, + .planar = true, + }, { + .fourcc = V4L2_PIX_FMT_NV16, + .cs = IPUV3_COLORSPACE_YUV, + .bpp = 16, + .planar = true, + }, +}; + +#define NUM_NON_MBUS_YUV_FORMATS 5 +#define NUM_YUV_FORMATS ARRAY_SIZE(yuv_formats) +#define NUM_MBUS_YUV_FORMATS (NUM_YUV_FORMATS - NUM_NON_MBUS_YUV_FORMATS) + +static const struct imx_media_pixfmt rgb_formats[] = { + { + .fourcc = V4L2_PIX_FMT_RGB565, + .codes = {MEDIA_BUS_FMT_RGB565_2X8_LE}, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 16, + }, { + .fourcc = V4L2_PIX_FMT_RGB24, + .codes = { + MEDIA_BUS_FMT_RGB888_1X24, + MEDIA_BUS_FMT_RGB888_2X12_LE + }, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 24, + }, { + .fourcc = V4L2_PIX_FMT_RGB32, + .codes = {MEDIA_BUS_FMT_ARGB8888_1X32}, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 32, + .ipufmt = true, + }, + /*** raw bayer formats start here ***/ + { + .fourcc = V4L2_PIX_FMT_SBGGR8, + .codes = {MEDIA_BUS_FMT_SBGGR8_1X8}, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 8, + .bayer = true, + }, { + .fourcc = V4L2_PIX_FMT_SGBRG8, + .codes = {MEDIA_BUS_FMT_SGBRG8_1X8}, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 8, + .bayer = true, + }, { + .fourcc = V4L2_PIX_FMT_SGRBG8, + .codes = {MEDIA_BUS_FMT_SGRBG8_1X8}, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 8, + .bayer = true, + }, { + .fourcc = V4L2_PIX_FMT_SRGGB8, + .codes = {MEDIA_BUS_FMT_SRGGB8_1X8}, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 8, + .bayer = true, + }, { + .fourcc = V4L2_PIX_FMT_SBGGR16, + .codes = { + MEDIA_BUS_FMT_SBGGR10_1X10, + MEDIA_BUS_FMT_SBGGR12_1X12, + MEDIA_BUS_FMT_SBGGR14_1X14, + MEDIA_BUS_FMT_SBGGR16_1X16 + }, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 16, + .bayer = true, + }, { + .fourcc = V4L2_PIX_FMT_SGBRG16, + .codes = { + MEDIA_BUS_FMT_SGBRG10_1X10, + MEDIA_BUS_FMT_SGBRG12_1X12, + MEDIA_BUS_FMT_SGBRG14_1X14, + MEDIA_BUS_FMT_SGBRG16_1X16, + }, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 16, + .bayer = true, + }, { + .fourcc = V4L2_PIX_FMT_SGRBG16, + .codes = { + MEDIA_BUS_FMT_SGRBG10_1X10, + MEDIA_BUS_FMT_SGRBG12_1X12, + MEDIA_BUS_FMT_SGRBG14_1X14, + MEDIA_BUS_FMT_SGRBG16_1X16, + }, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 16, + .bayer = true, + }, { + .fourcc = V4L2_PIX_FMT_SRGGB16, + .codes = { + MEDIA_BUS_FMT_SRGGB10_1X10, + MEDIA_BUS_FMT_SRGGB12_1X12, + MEDIA_BUS_FMT_SRGGB14_1X14, + MEDIA_BUS_FMT_SRGGB16_1X16, + }, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 16, + .bayer = true, + }, + /*** + * non-mbus RGB formats start here. NOTE! when adding non-mbus + * formats, NUM_NON_MBUS_RGB_FORMATS must be updated below. + ***/ + { + .fourcc = V4L2_PIX_FMT_BGR24, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 24, + }, { + .fourcc = V4L2_PIX_FMT_BGR32, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 32, + }, +}; + +#define NUM_NON_MBUS_RGB_FORMATS 2 +#define NUM_RGB_FORMATS ARRAY_SIZE(rgb_formats) +#define NUM_MBUS_RGB_FORMATS (NUM_RGB_FORMATS - NUM_NON_MBUS_RGB_FORMATS) + +static const struct imx_media_pixfmt ipu_yuv_formats[] = { + { + .fourcc = V4L2_PIX_FMT_YUV32, + .codes = {MEDIA_BUS_FMT_AYUV8_1X32}, + .cs = IPUV3_COLORSPACE_YUV, + .bpp = 32, + .ipufmt = true, + }, +}; + +#define NUM_IPU_YUV_FORMATS ARRAY_SIZE(ipu_yuv_formats) + +static const struct imx_media_pixfmt ipu_rgb_formats[] = { + { + .fourcc = V4L2_PIX_FMT_RGB32, + .codes = {MEDIA_BUS_FMT_ARGB8888_1X32}, + .cs = IPUV3_COLORSPACE_RGB, + .bpp = 32, + .ipufmt = true, + }, +}; + +#define NUM_IPU_RGB_FORMATS ARRAY_SIZE(ipu_rgb_formats) + +static void init_mbus_colorimetry(struct v4l2_mbus_framefmt *mbus, + const struct imx_media_pixfmt *fmt) +{ + mbus->colorspace = (fmt->cs == IPUV3_COLORSPACE_RGB) ? + V4L2_COLORSPACE_SRGB : V4L2_COLORSPACE_SMPTE170M; + mbus->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(mbus->colorspace); + mbus->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(mbus->colorspace); + mbus->quantization = + V4L2_MAP_QUANTIZATION_DEFAULT(fmt->cs == IPUV3_COLORSPACE_RGB, + mbus->colorspace, + mbus->ycbcr_enc); +} + +static const struct imx_media_pixfmt *find_format(u32 fourcc, + u32 code, + enum codespace_sel cs_sel, + bool allow_non_mbus, + bool allow_bayer) +{ + const struct imx_media_pixfmt *array, *fmt, *ret = NULL; + u32 array_size; + int i, j; + + switch (cs_sel) { + case CS_SEL_YUV: + array_size = NUM_YUV_FORMATS; + array = yuv_formats; + break; + case CS_SEL_RGB: + array_size = NUM_RGB_FORMATS; + array = rgb_formats; + break; + case CS_SEL_ANY: + array_size = NUM_YUV_FORMATS + NUM_RGB_FORMATS; + array = yuv_formats; + break; + default: + return NULL; + } + + for (i = 0; i < array_size; i++) { + if (cs_sel == CS_SEL_ANY && i >= NUM_YUV_FORMATS) + fmt = &rgb_formats[i - NUM_YUV_FORMATS]; + else + fmt = &array[i]; + + if ((!allow_non_mbus && fmt->codes[0] == 0) || + (!allow_bayer && fmt->bayer)) + continue; + + if (fourcc && fmt->fourcc == fourcc) { + ret = fmt; + goto out; + } + + for (j = 0; code && fmt->codes[j]; j++) { + if (code == fmt->codes[j]) { + ret = fmt; + goto out; + } + } + } + +out: + return ret; +} + +static int enum_format(u32 *fourcc, u32 *code, u32 index, + enum codespace_sel cs_sel, + bool allow_non_mbus, + bool allow_bayer) +{ + const struct imx_media_pixfmt *fmt; + u32 mbus_yuv_sz = NUM_MBUS_YUV_FORMATS; + u32 mbus_rgb_sz = NUM_MBUS_RGB_FORMATS; + u32 yuv_sz = NUM_YUV_FORMATS; + u32 rgb_sz = NUM_RGB_FORMATS; + + switch (cs_sel) { + case CS_SEL_YUV: + if (index >= yuv_sz || + (!allow_non_mbus && index >= mbus_yuv_sz)) + return -EINVAL; + fmt = &yuv_formats[index]; + break; + case CS_SEL_RGB: + if (index >= rgb_sz || + (!allow_non_mbus && index >= mbus_rgb_sz)) + return -EINVAL; + fmt = &rgb_formats[index]; + if (!allow_bayer && fmt->bayer) + return -EINVAL; + break; + case CS_SEL_ANY: + if (!allow_non_mbus) { + if (index >= mbus_yuv_sz) { + index -= mbus_yuv_sz; + if (index >= mbus_rgb_sz) + return -EINVAL; + fmt = &rgb_formats[index]; + if (!allow_bayer && fmt->bayer) + return -EINVAL; + } else { + fmt = &yuv_formats[index]; + } + } else { + if (index >= yuv_sz + rgb_sz) + return -EINVAL; + if (index >= yuv_sz) { + fmt = &rgb_formats[index - yuv_sz]; + if (!allow_bayer && fmt->bayer) + return -EINVAL; + } else { + fmt = &yuv_formats[index]; + } + } + break; + default: + return -EINVAL; + } + + if (fourcc) + *fourcc = fmt->fourcc; + if (code) + *code = fmt->codes[0]; + + return 0; +} + +const struct imx_media_pixfmt * +imx_media_find_format(u32 fourcc, enum codespace_sel cs_sel, bool allow_bayer) +{ + return find_format(fourcc, 0, cs_sel, true, allow_bayer); +} +EXPORT_SYMBOL_GPL(imx_media_find_format); + +int imx_media_enum_format(u32 *fourcc, u32 index, enum codespace_sel cs_sel) +{ + return enum_format(fourcc, NULL, index, cs_sel, true, false); +} +EXPORT_SYMBOL_GPL(imx_media_enum_format); + +const struct imx_media_pixfmt * +imx_media_find_mbus_format(u32 code, enum codespace_sel cs_sel, + bool allow_bayer) +{ + return find_format(0, code, cs_sel, false, allow_bayer); +} +EXPORT_SYMBOL_GPL(imx_media_find_mbus_format); + +int imx_media_enum_mbus_format(u32 *code, u32 index, enum codespace_sel cs_sel, + bool allow_bayer) +{ + return enum_format(NULL, code, index, cs_sel, false, allow_bayer); +} +EXPORT_SYMBOL_GPL(imx_media_enum_mbus_format); + +const struct imx_media_pixfmt * +imx_media_find_ipu_format(u32 code, enum codespace_sel cs_sel) +{ + const struct imx_media_pixfmt *array, *fmt, *ret = NULL; + u32 array_size; + int i, j; + + switch (cs_sel) { + case CS_SEL_YUV: + array_size = NUM_IPU_YUV_FORMATS; + array = ipu_yuv_formats; + break; + case CS_SEL_RGB: + array_size = NUM_IPU_RGB_FORMATS; + array = ipu_rgb_formats; + break; + case CS_SEL_ANY: + array_size = NUM_IPU_YUV_FORMATS + NUM_IPU_RGB_FORMATS; + array = ipu_yuv_formats; + break; + default: + return NULL; + } + + for (i = 0; i < array_size; i++) { + if (cs_sel == CS_SEL_ANY && i >= NUM_IPU_YUV_FORMATS) + fmt = &ipu_rgb_formats[i - NUM_IPU_YUV_FORMATS]; + else + fmt = &array[i]; + + for (j = 0; code && fmt->codes[j]; j++) { + if (code == fmt->codes[j]) { + ret = fmt; + goto out; + } + } + } + +out: + return ret; +} +EXPORT_SYMBOL_GPL(imx_media_find_ipu_format); + +int imx_media_enum_ipu_format(u32 *code, u32 index, enum codespace_sel cs_sel) +{ + switch (cs_sel) { + case CS_SEL_YUV: + if (index >= NUM_IPU_YUV_FORMATS) + return -EINVAL; + *code = ipu_yuv_formats[index].codes[0]; + break; + case CS_SEL_RGB: + if (index >= NUM_IPU_RGB_FORMATS) + return -EINVAL; + *code = ipu_rgb_formats[index].codes[0]; + break; + case CS_SEL_ANY: + if (index >= NUM_IPU_YUV_FORMATS + NUM_IPU_RGB_FORMATS) + return -EINVAL; + if (index >= NUM_IPU_YUV_FORMATS) { + index -= NUM_IPU_YUV_FORMATS; + *code = ipu_rgb_formats[index].codes[0]; + } else { + *code = ipu_yuv_formats[index].codes[0]; + } + break; + default: + return -EINVAL; + } + + return 0; +} +EXPORT_SYMBOL_GPL(imx_media_enum_ipu_format); + +int imx_media_init_mbus_fmt(struct v4l2_mbus_framefmt *mbus, + u32 width, u32 height, u32 code, u32 field, + const struct imx_media_pixfmt **cc) +{ + const struct imx_media_pixfmt *lcc; + + mbus->width = width; + mbus->height = height; + mbus->field = field; + if (code == 0) + imx_media_enum_mbus_format(&code, 0, CS_SEL_YUV, false); + lcc = imx_media_find_mbus_format(code, CS_SEL_ANY, false); + if (!lcc) { + lcc = imx_media_find_ipu_format(code, CS_SEL_ANY); + if (!lcc) + return -EINVAL; + } + + mbus->code = code; + init_mbus_colorimetry(mbus, lcc); + if (cc) + *cc = lcc; + + return 0; +} +EXPORT_SYMBOL_GPL(imx_media_init_mbus_fmt); + +/* + * Check whether the field and colorimetry parameters in tryfmt are + * uninitialized, and if so fill them with the values from fmt, + * or if tryfmt->colorspace has been initialized, all the default + * colorimetry params can be derived from tryfmt->colorspace. + * + * tryfmt->code must be set on entry. + * + * If this format is destined to be routed through the Image Converter, + * quantization and Y`CbCr encoding must be fixed. The IC expects and + * produces fixed quantization and Y`CbCr encoding at its input and output + * (full range for RGB, limited range for YUV, and V4L2_YCBCR_ENC_601). + */ +void imx_media_fill_default_mbus_fields(struct v4l2_mbus_framefmt *tryfmt, + struct v4l2_mbus_framefmt *fmt, + bool ic_route) +{ + const struct imx_media_pixfmt *cc; + bool is_rgb = false; + + cc = imx_media_find_mbus_format(tryfmt->code, CS_SEL_ANY, true); + if (!cc) + cc = imx_media_find_ipu_format(tryfmt->code, CS_SEL_ANY); + if (cc && cc->cs != IPUV3_COLORSPACE_YUV) + is_rgb = true; + + /* fill field if necessary */ + if (tryfmt->field == V4L2_FIELD_ANY) + tryfmt->field = fmt->field; + + /* fill colorimetry if necessary */ + if (tryfmt->colorspace == V4L2_COLORSPACE_DEFAULT) { + tryfmt->colorspace = fmt->colorspace; + tryfmt->xfer_func = fmt->xfer_func; + tryfmt->ycbcr_enc = fmt->ycbcr_enc; + tryfmt->quantization = fmt->quantization; + } else { + if (tryfmt->xfer_func == V4L2_XFER_FUNC_DEFAULT) { + tryfmt->xfer_func = + V4L2_MAP_XFER_FUNC_DEFAULT(tryfmt->colorspace); + } + if (tryfmt->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT) { + tryfmt->ycbcr_enc = + V4L2_MAP_YCBCR_ENC_DEFAULT(tryfmt->colorspace); + } + if (tryfmt->quantization == V4L2_QUANTIZATION_DEFAULT) { + tryfmt->quantization = + V4L2_MAP_QUANTIZATION_DEFAULT( + is_rgb, tryfmt->colorspace, + tryfmt->ycbcr_enc); + } + } + + if (ic_route) { + tryfmt->quantization = is_rgb ? + V4L2_QUANTIZATION_FULL_RANGE : + V4L2_QUANTIZATION_LIM_RANGE; + tryfmt->ycbcr_enc = V4L2_YCBCR_ENC_601; + } +} +EXPORT_SYMBOL_GPL(imx_media_fill_default_mbus_fields); + +int imx_media_mbus_fmt_to_pix_fmt(struct v4l2_pix_format *pix, + struct v4l2_mbus_framefmt *mbus, + const struct imx_media_pixfmt *cc) +{ + u32 stride; + + if (!cc) { + cc = imx_media_find_ipu_format(mbus->code, CS_SEL_ANY); + if (!cc) + cc = imx_media_find_mbus_format(mbus->code, CS_SEL_ANY, + true); + if (!cc) + return -EINVAL; + } + + /* + * TODO: the IPU currently does not support the AYUV32 format, + * so until it does convert to a supported YUV format. + */ + if (cc->ipufmt && cc->cs == IPUV3_COLORSPACE_YUV) { + u32 code; + + imx_media_enum_mbus_format(&code, 0, CS_SEL_YUV, false); + cc = imx_media_find_mbus_format(code, CS_SEL_YUV, false); + } + + stride = cc->planar ? mbus->width : (mbus->width * cc->bpp) >> 3; + + pix->width = mbus->width; + pix->height = mbus->height; + pix->pixelformat = cc->fourcc; + pix->colorspace = mbus->colorspace; + pix->xfer_func = mbus->xfer_func; + pix->ycbcr_enc = mbus->ycbcr_enc; + pix->quantization = mbus->quantization; + pix->field = mbus->field; + pix->bytesperline = stride; + pix->sizeimage = (pix->width * pix->height * cc->bpp) >> 3; + + return 0; +} +EXPORT_SYMBOL_GPL(imx_media_mbus_fmt_to_pix_fmt); + +int imx_media_mbus_fmt_to_ipu_image(struct ipu_image *image, + struct v4l2_mbus_framefmt *mbus) +{ + int ret; + + memset(image, 0, sizeof(*image)); + + ret = imx_media_mbus_fmt_to_pix_fmt(&image->pix, mbus, NULL); + if (ret) + return ret; + + image->rect.width = mbus->width; + image->rect.height = mbus->height; + + return 0; +} +EXPORT_SYMBOL_GPL(imx_media_mbus_fmt_to_ipu_image); + +int imx_media_ipu_image_to_mbus_fmt(struct v4l2_mbus_framefmt *mbus, + struct ipu_image *image) +{ + const struct imx_media_pixfmt *fmt; + + fmt = imx_media_find_format(image->pix.pixelformat, CS_SEL_ANY, true); + if (!fmt) + return -EINVAL; + + memset(mbus, 0, sizeof(*mbus)); + mbus->width = image->pix.width; + mbus->height = image->pix.height; + mbus->code = fmt->codes[0]; + mbus->field = image->pix.field; + mbus->colorspace = image->pix.colorspace; + mbus->xfer_func = image->pix.xfer_func; + mbus->ycbcr_enc = image->pix.ycbcr_enc; + mbus->quantization = image->pix.quantization; + + return 0; +} +EXPORT_SYMBOL_GPL(imx_media_ipu_image_to_mbus_fmt); + +void imx_media_free_dma_buf(struct imx_media_dev *imxmd, + struct imx_media_dma_buf *buf) +{ + if (buf->virt) + dma_free_coherent(imxmd->md.dev, buf->len, + buf->virt, buf->phys); + + buf->virt = NULL; + buf->phys = 0; +} +EXPORT_SYMBOL_GPL(imx_media_free_dma_buf); + +int imx_media_alloc_dma_buf(struct imx_media_dev *imxmd, + struct imx_media_dma_buf *buf, + int size) +{ + imx_media_free_dma_buf(imxmd, buf); + + buf->len = PAGE_ALIGN(size); + buf->virt = dma_alloc_coherent(imxmd->md.dev, buf->len, &buf->phys, + GFP_DMA | GFP_KERNEL); + if (!buf->virt) { + dev_err(imxmd->md.dev, "failed to alloc dma buffer\n"); + return -ENOMEM; + } + + return 0; +} +EXPORT_SYMBOL_GPL(imx_media_alloc_dma_buf); + +/* form a subdev name given a group id and ipu id */ +void imx_media_grp_id_to_sd_name(char *sd_name, int sz, u32 grp_id, int ipu_id) +{ + int id; + + switch (grp_id) { + case IMX_MEDIA_GRP_ID_CSI0...IMX_MEDIA_GRP_ID_CSI1: + id = (grp_id >> IMX_MEDIA_GRP_ID_CSI_BIT) - 1; + snprintf(sd_name, sz, "ipu%d_csi%d", ipu_id + 1, id); + break; + case IMX_MEDIA_GRP_ID_VDIC: + snprintf(sd_name, sz, "ipu%d_vdic", ipu_id + 1); + break; + case IMX_MEDIA_GRP_ID_IC_PRP: + snprintf(sd_name, sz, "ipu%d_ic_prp", ipu_id + 1); + break; + case IMX_MEDIA_GRP_ID_IC_PRPENC: + snprintf(sd_name, sz, "ipu%d_ic_prpenc", ipu_id + 1); + break; + case IMX_MEDIA_GRP_ID_IC_PRPVF: + snprintf(sd_name, sz, "ipu%d_ic_prpvf", ipu_id + 1); + break; + default: + break; + } +} +EXPORT_SYMBOL_GPL(imx_media_grp_id_to_sd_name); + +struct imx_media_subdev * +imx_media_find_subdev_by_sd(struct imx_media_dev *imxmd, + struct v4l2_subdev *sd) +{ + struct imx_media_subdev *imxsd; + int i; + + for (i = 0; i < imxmd->num_subdevs; i++) { + imxsd = &imxmd->subdev[i]; + if (sd == imxsd->sd) + return imxsd; + } + + return ERR_PTR(-ENODEV); +} +EXPORT_SYMBOL_GPL(imx_media_find_subdev_by_sd); + +struct imx_media_subdev * +imx_media_find_subdev_by_id(struct imx_media_dev *imxmd, u32 grp_id) +{ + struct imx_media_subdev *imxsd; + int i; + + for (i = 0; i < imxmd->num_subdevs; i++) { + imxsd = &imxmd->subdev[i]; + if (imxsd->sd && imxsd->sd->grp_id == grp_id) + return imxsd; + } + + return ERR_PTR(-ENODEV); +} +EXPORT_SYMBOL_GPL(imx_media_find_subdev_by_id); + +/* + * Adds a video device to the master video device list. This is called by + * an async subdev that owns a video device when it is registered. + */ +int imx_media_add_video_device(struct imx_media_dev *imxmd, + struct imx_media_video_dev *vdev) +{ + int vdev_idx, ret = 0; + + mutex_lock(&imxmd->mutex); + + vdev_idx = imxmd->num_vdevs; + if (vdev_idx >= IMX_MEDIA_MAX_VDEVS) { + dev_err(imxmd->md.dev, + "%s: too many video devices! can't add %s\n", + __func__, vdev->vfd->name); + ret = -ENOSPC; + goto out; + } + + imxmd->vdev[vdev_idx] = vdev; + imxmd->num_vdevs++; +out: + mutex_unlock(&imxmd->mutex); + return ret; +} +EXPORT_SYMBOL_GPL(imx_media_add_video_device); + +/* + * Search upstream or downstream for a subdevice in the current pipeline + * with given grp_id, starting from start_entity. Returns the subdev's + * source/sink pad that it was reached from. Must be called with + * mdev->graph_mutex held. + */ +static struct media_pad * +find_pipeline_pad(struct imx_media_dev *imxmd, + struct media_entity *start_entity, + u32 grp_id, bool upstream) +{ + struct media_entity *me = start_entity; + struct media_pad *pad = NULL; + struct v4l2_subdev *sd; + int i; + + for (i = 0; i < me->num_pads; i++) { + struct media_pad *spad = &me->pads[i]; + + if ((upstream && !(spad->flags & MEDIA_PAD_FL_SINK)) || + (!upstream && !(spad->flags & MEDIA_PAD_FL_SOURCE))) + continue; + + pad = media_entity_remote_pad(spad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + continue; + + sd = media_entity_to_v4l2_subdev(pad->entity); + if (sd->grp_id & grp_id) + return pad; + + return find_pipeline_pad(imxmd, pad->entity, grp_id, upstream); + } + + return NULL; +} + +/* + * Search upstream for a subdev in the current pipeline with + * given grp_id. Must be called with mdev->graph_mutex held. + */ +static struct v4l2_subdev * +find_upstream_subdev(struct imx_media_dev *imxmd, + struct media_entity *start_entity, + u32 grp_id) +{ + struct v4l2_subdev *sd; + struct media_pad *pad; + + if (is_media_entity_v4l2_subdev(start_entity)) { + sd = media_entity_to_v4l2_subdev(start_entity); + if (sd->grp_id & grp_id) + return sd; + } + + pad = find_pipeline_pad(imxmd, start_entity, grp_id, true); + + return pad ? media_entity_to_v4l2_subdev(pad->entity) : NULL; +} + + +/* + * Find the upstream mipi-csi2 virtual channel reached from the given + * start entity in the current pipeline. + * Must be called with mdev->graph_mutex held. + */ +int imx_media_find_mipi_csi2_channel(struct imx_media_dev *imxmd, + struct media_entity *start_entity) +{ + struct media_pad *pad; + int ret = -EPIPE; + + pad = find_pipeline_pad(imxmd, start_entity, IMX_MEDIA_GRP_ID_CSI2, + true); + if (pad) { + ret = pad->index - 1; + dev_dbg(imxmd->md.dev, "found vc%d from %s\n", + ret, start_entity->name); + } + + return ret; +} +EXPORT_SYMBOL_GPL(imx_media_find_mipi_csi2_channel); + +/* + * Find a subdev reached upstream from the given start entity in + * the current pipeline. + * Must be called with mdev->graph_mutex held. + */ +struct imx_media_subdev * +imx_media_find_upstream_subdev(struct imx_media_dev *imxmd, + struct media_entity *start_entity, + u32 grp_id) +{ + struct v4l2_subdev *sd; + + sd = find_upstream_subdev(imxmd, start_entity, grp_id); + if (!sd) + return ERR_PTR(-ENODEV); + + return imx_media_find_subdev_by_sd(imxmd, sd); +} +EXPORT_SYMBOL_GPL(imx_media_find_upstream_subdev); + +struct imx_media_subdev * +__imx_media_find_sensor(struct imx_media_dev *imxmd, + struct media_entity *start_entity) +{ + return imx_media_find_upstream_subdev(imxmd, start_entity, + IMX_MEDIA_GRP_ID_SENSOR); +} +EXPORT_SYMBOL_GPL(__imx_media_find_sensor); + +struct imx_media_subdev * +imx_media_find_sensor(struct imx_media_dev *imxmd, + struct media_entity *start_entity) +{ + struct imx_media_subdev *sensor; + + mutex_lock(&imxmd->md.graph_mutex); + sensor = __imx_media_find_sensor(imxmd, start_entity); + mutex_unlock(&imxmd->md.graph_mutex); + + return sensor; +} +EXPORT_SYMBOL_GPL(imx_media_find_sensor); + +/* + * Turn current pipeline streaming on/off starting from entity. + */ +int imx_media_pipeline_set_stream(struct imx_media_dev *imxmd, + struct media_entity *entity, + bool on) +{ + struct v4l2_subdev *sd; + int ret = 0; + + if (!is_media_entity_v4l2_subdev(entity)) + return -EINVAL; + sd = media_entity_to_v4l2_subdev(entity); + + mutex_lock(&imxmd->md.graph_mutex); + + if (on) { + ret = __media_pipeline_start(entity, &imxmd->pipe); + if (ret) + goto out; + ret = v4l2_subdev_call(sd, video, s_stream, 1); + if (ret) + __media_pipeline_stop(entity); + } else { + v4l2_subdev_call(sd, video, s_stream, 0); + if (entity->pipe) + __media_pipeline_stop(entity); + } + +out: + mutex_unlock(&imxmd->md.graph_mutex); + return ret; +} +EXPORT_SYMBOL_GPL(imx_media_pipeline_set_stream); + +MODULE_DESCRIPTION("i.MX5/6 v4l2 media controller driver"); +MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>"); +MODULE_LICENSE("GPL"); diff --git a/drivers/staging/media/imx/imx-media-vdic.c b/drivers/staging/media/imx/imx-media-vdic.c new file mode 100644 index 000000000000..7eabdc4aa79f --- /dev/null +++ b/drivers/staging/media/imx/imx-media-vdic.c @@ -0,0 +1,1009 @@ +/* + * V4L2 Deinterlacer Subdev for Freescale i.MX5/6 SOC + * + * Copyright (c) 2017 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/delay.h> +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/platform_device.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/timer.h> +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-ioctl.h> +#include <media/v4l2-mc.h> +#include <media/v4l2-subdev.h> +#include <media/imx.h> +#include "imx-media.h" + +/* + * This subdev implements two different video pipelines: + * + * CSI -> VDIC + * + * In this pipeline, the CSI sends a single interlaced field F(n-1) + * directly to the VDIC (and optionally the following field F(n) + * can be sent to memory via IDMAC channel 13). This pipeline only works + * in VDIC's high motion mode, which only requires a single field for + * processing. The other motion modes (low and medium) require three + * fields, so this pipeline does not work in those modes. Also, it is + * not clear how this pipeline can deal with the various field orders + * (sequential BT/TB, interlaced BT/TB). + * + * MEM -> CH8,9,10 -> VDIC + * + * In this pipeline, previous field F(n-1), current field F(n), and next + * field F(n+1) are transferred to the VDIC via IDMAC channels 8,9,10. + * These memory buffers can come from a video output or mem2mem device. + * All motion modes are supported by this pipeline. + * + * The "direct" CSI->VDIC pipeline requires no DMA, but it can only be + * used in high motion mode. + */ + +struct vdic_priv; + +struct vdic_pipeline_ops { + int (*setup)(struct vdic_priv *priv); + void (*start)(struct vdic_priv *priv); + void (*stop)(struct vdic_priv *priv); + void (*disable)(struct vdic_priv *priv); +}; + +/* + * Min/Max supported width and heights. + */ +#define MIN_W 176 +#define MIN_H 144 +#define MAX_W_VDIC 968 +#define MAX_H_VDIC 2048 +#define W_ALIGN 4 /* multiple of 16 pixels */ +#define H_ALIGN 1 /* multiple of 2 lines */ +#define S_ALIGN 1 /* multiple of 2 */ + +struct vdic_priv { + struct device *dev; + struct ipu_soc *ipu; + struct imx_media_dev *md; + struct v4l2_subdev sd; + struct media_pad pad[VDIC_NUM_PADS]; + int ipu_id; + + /* lock to protect all members below */ + struct mutex lock; + + /* IPU units we require */ + struct ipu_vdi *vdi; + + int active_input_pad; + + struct ipuv3_channel *vdi_in_ch_p; /* F(n-1) transfer channel */ + struct ipuv3_channel *vdi_in_ch; /* F(n) transfer channel */ + struct ipuv3_channel *vdi_in_ch_n; /* F(n+1) transfer channel */ + + /* pipeline operations */ + struct vdic_pipeline_ops *ops; + + /* current and previous input buffers indirect path */ + struct imx_media_buffer *curr_in_buf; + struct imx_media_buffer *prev_in_buf; + + /* + * translated field type, input line stride, and field size + * for indirect path + */ + u32 fieldtype; + u32 in_stride; + u32 field_size; + + /* the source (a video device or subdev) */ + struct media_entity *src; + /* the sink that will receive the progressive out buffers */ + struct v4l2_subdev *sink_sd; + + struct v4l2_mbus_framefmt format_mbus[VDIC_NUM_PADS]; + const struct imx_media_pixfmt *cc[VDIC_NUM_PADS]; + struct v4l2_fract frame_interval[VDIC_NUM_PADS]; + + /* the video device at IDMAC input pad */ + struct imx_media_video_dev *vdev; + + bool csi_direct; /* using direct CSI->VDIC->IC pipeline */ + + /* motion select control */ + struct v4l2_ctrl_handler ctrl_hdlr; + enum ipu_motion_sel motion; + + int stream_count; +}; + +static void vdic_put_ipu_resources(struct vdic_priv *priv) +{ + if (!IS_ERR_OR_NULL(priv->vdi_in_ch_p)) + ipu_idmac_put(priv->vdi_in_ch_p); + priv->vdi_in_ch_p = NULL; + + if (!IS_ERR_OR_NULL(priv->vdi_in_ch)) + ipu_idmac_put(priv->vdi_in_ch); + priv->vdi_in_ch = NULL; + + if (!IS_ERR_OR_NULL(priv->vdi_in_ch_n)) + ipu_idmac_put(priv->vdi_in_ch_n); + priv->vdi_in_ch_n = NULL; + + if (!IS_ERR_OR_NULL(priv->vdi)) + ipu_vdi_put(priv->vdi); + priv->vdi = NULL; +} + +static int vdic_get_ipu_resources(struct vdic_priv *priv) +{ + int ret, err_chan; + + priv->ipu = priv->md->ipu[priv->ipu_id]; + + priv->vdi = ipu_vdi_get(priv->ipu); + if (IS_ERR(priv->vdi)) { + v4l2_err(&priv->sd, "failed to get VDIC\n"); + ret = PTR_ERR(priv->vdi); + goto out; + } + + if (!priv->csi_direct) { + priv->vdi_in_ch_p = ipu_idmac_get(priv->ipu, + IPUV3_CHANNEL_MEM_VDI_PREV); + if (IS_ERR(priv->vdi_in_ch_p)) { + err_chan = IPUV3_CHANNEL_MEM_VDI_PREV; + ret = PTR_ERR(priv->vdi_in_ch_p); + goto out_err_chan; + } + + priv->vdi_in_ch = ipu_idmac_get(priv->ipu, + IPUV3_CHANNEL_MEM_VDI_CUR); + if (IS_ERR(priv->vdi_in_ch)) { + err_chan = IPUV3_CHANNEL_MEM_VDI_CUR; + ret = PTR_ERR(priv->vdi_in_ch); + goto out_err_chan; + } + + priv->vdi_in_ch_n = ipu_idmac_get(priv->ipu, + IPUV3_CHANNEL_MEM_VDI_NEXT); + if (IS_ERR(priv->vdi_in_ch_n)) { + err_chan = IPUV3_CHANNEL_MEM_VDI_NEXT; + ret = PTR_ERR(priv->vdi_in_ch_n); + goto out_err_chan; + } + } + + return 0; + +out_err_chan: + v4l2_err(&priv->sd, "could not get IDMAC channel %u\n", err_chan); +out: + vdic_put_ipu_resources(priv); + return ret; +} + +/* + * This function is currently unused, but will be called when the + * output/mem2mem device at the IDMAC input pad sends us a new + * buffer. It kicks off the IDMAC read channels to bring in the + * buffer fields from memory and begin the conversions. + */ +static void __maybe_unused prepare_vdi_in_buffers(struct vdic_priv *priv, + struct imx_media_buffer *curr) +{ + dma_addr_t prev_phys, curr_phys, next_phys; + struct imx_media_buffer *prev; + struct vb2_buffer *curr_vb, *prev_vb; + u32 fs = priv->field_size; + u32 is = priv->in_stride; + + /* current input buffer is now previous */ + priv->prev_in_buf = priv->curr_in_buf; + priv->curr_in_buf = curr; + prev = priv->prev_in_buf ? priv->prev_in_buf : curr; + + prev_vb = &prev->vbuf.vb2_buf; + curr_vb = &curr->vbuf.vb2_buf; + + switch (priv->fieldtype) { + case V4L2_FIELD_SEQ_TB: + prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0); + curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs; + next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0); + break; + case V4L2_FIELD_SEQ_BT: + prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + fs; + curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0); + next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs; + break; + case V4L2_FIELD_INTERLACED_BT: + prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + is; + curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0); + next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is; + break; + default: + /* assume V4L2_FIELD_INTERLACED_TB */ + prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0); + curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is; + next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0); + break; + } + + ipu_cpmem_set_buffer(priv->vdi_in_ch_p, 0, prev_phys); + ipu_cpmem_set_buffer(priv->vdi_in_ch, 0, curr_phys); + ipu_cpmem_set_buffer(priv->vdi_in_ch_n, 0, next_phys); + + ipu_idmac_select_buffer(priv->vdi_in_ch_p, 0); + ipu_idmac_select_buffer(priv->vdi_in_ch, 0); + ipu_idmac_select_buffer(priv->vdi_in_ch_n, 0); +} + +static int setup_vdi_channel(struct vdic_priv *priv, + struct ipuv3_channel *channel, + dma_addr_t phys0, dma_addr_t phys1) +{ + struct imx_media_video_dev *vdev = priv->vdev; + unsigned int burst_size; + struct ipu_image image; + int ret; + + ipu_cpmem_zero(channel); + + memset(&image, 0, sizeof(image)); + image.pix = vdev->fmt.fmt.pix; + /* one field to VDIC channels */ + image.pix.height /= 2; + image.rect.width = image.pix.width; + image.rect.height = image.pix.height; + image.phys0 = phys0; + image.phys1 = phys1; + + ret = ipu_cpmem_set_image(channel, &image); + if (ret) + return ret; + + burst_size = (image.pix.width & 0xf) ? 8 : 16; + ipu_cpmem_set_burstsize(channel, burst_size); + + ipu_cpmem_set_axi_id(channel, 1); + + ipu_idmac_set_double_buffer(channel, false); + + return 0; +} + +static int vdic_setup_direct(struct vdic_priv *priv) +{ + /* set VDIC to receive from CSI for direct path */ + ipu_fsu_link(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT, + IPUV3_CHANNEL_CSI_VDI_PREV); + + return 0; +} + +static void vdic_start_direct(struct vdic_priv *priv) +{ +} + +static void vdic_stop_direct(struct vdic_priv *priv) +{ +} + +static void vdic_disable_direct(struct vdic_priv *priv) +{ + ipu_fsu_unlink(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT, + IPUV3_CHANNEL_CSI_VDI_PREV); +} + +static int vdic_setup_indirect(struct vdic_priv *priv) +{ + struct v4l2_mbus_framefmt *infmt; + const struct imx_media_pixfmt *incc; + int in_size, ret; + + infmt = &priv->format_mbus[VDIC_SINK_PAD_IDMAC]; + incc = priv->cc[VDIC_SINK_PAD_IDMAC]; + + in_size = (infmt->width * incc->bpp * infmt->height) >> 3; + + /* 1/2 full image size */ + priv->field_size = in_size / 2; + priv->in_stride = incc->planar ? + infmt->width : (infmt->width * incc->bpp) >> 3; + + priv->prev_in_buf = NULL; + priv->curr_in_buf = NULL; + + priv->fieldtype = infmt->field; + + /* init the vdi-in channels */ + ret = setup_vdi_channel(priv, priv->vdi_in_ch_p, 0, 0); + if (ret) + return ret; + ret = setup_vdi_channel(priv, priv->vdi_in_ch, 0, 0); + if (ret) + return ret; + return setup_vdi_channel(priv, priv->vdi_in_ch_n, 0, 0); +} + +static void vdic_start_indirect(struct vdic_priv *priv) +{ + /* enable the channels */ + ipu_idmac_enable_channel(priv->vdi_in_ch_p); + ipu_idmac_enable_channel(priv->vdi_in_ch); + ipu_idmac_enable_channel(priv->vdi_in_ch_n); +} + +static void vdic_stop_indirect(struct vdic_priv *priv) +{ + /* disable channels */ + ipu_idmac_disable_channel(priv->vdi_in_ch_p); + ipu_idmac_disable_channel(priv->vdi_in_ch); + ipu_idmac_disable_channel(priv->vdi_in_ch_n); +} + +static void vdic_disable_indirect(struct vdic_priv *priv) +{ +} + +static struct vdic_pipeline_ops direct_ops = { + .setup = vdic_setup_direct, + .start = vdic_start_direct, + .stop = vdic_stop_direct, + .disable = vdic_disable_direct, +}; + +static struct vdic_pipeline_ops indirect_ops = { + .setup = vdic_setup_indirect, + .start = vdic_start_indirect, + .stop = vdic_stop_indirect, + .disable = vdic_disable_indirect, +}; + +static int vdic_start(struct vdic_priv *priv) +{ + struct v4l2_mbus_framefmt *infmt; + int ret; + + infmt = &priv->format_mbus[priv->active_input_pad]; + + priv->ops = priv->csi_direct ? &direct_ops : &indirect_ops; + + ret = vdic_get_ipu_resources(priv); + if (ret) + return ret; + + /* + * init the VDIC. + * + * note we don't give infmt->code to ipu_vdi_setup(). The VDIC + * only supports 4:2:2 or 4:2:0, and this subdev will only + * negotiate 4:2:2 at its sink pads. + */ + ipu_vdi_setup(priv->vdi, MEDIA_BUS_FMT_UYVY8_2X8, + infmt->width, infmt->height); + ipu_vdi_set_field_order(priv->vdi, V4L2_STD_UNKNOWN, infmt->field); + ipu_vdi_set_motion(priv->vdi, priv->motion); + + ret = priv->ops->setup(priv); + if (ret) + goto out_put_ipu; + + ipu_vdi_enable(priv->vdi); + + priv->ops->start(priv); + + return 0; + +out_put_ipu: + vdic_put_ipu_resources(priv); + return ret; +} + +static void vdic_stop(struct vdic_priv *priv) +{ + priv->ops->stop(priv); + ipu_vdi_disable(priv->vdi); + priv->ops->disable(priv); + + vdic_put_ipu_resources(priv); +} + +/* + * V4L2 subdev operations. + */ + +static int vdic_s_ctrl(struct v4l2_ctrl *ctrl) +{ + struct vdic_priv *priv = container_of(ctrl->handler, + struct vdic_priv, ctrl_hdlr); + enum ipu_motion_sel motion; + int ret = 0; + + mutex_lock(&priv->lock); + + switch (ctrl->id) { + case V4L2_CID_DEINTERLACING_MODE: + motion = ctrl->val; + if (motion != priv->motion) { + /* can't change motion control mid-streaming */ + if (priv->stream_count > 0) { + ret = -EBUSY; + goto out; + } + priv->motion = motion; + } + break; + default: + v4l2_err(&priv->sd, "Invalid control\n"); + ret = -EINVAL; + } + +out: + mutex_unlock(&priv->lock); + return ret; +} + +static const struct v4l2_ctrl_ops vdic_ctrl_ops = { + .s_ctrl = vdic_s_ctrl, +}; + +static const char * const vdic_ctrl_motion_menu[] = { + "No Motion Compensation", + "Low Motion", + "Medium Motion", + "High Motion", +}; + +static int vdic_init_controls(struct vdic_priv *priv) +{ + struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr; + int ret; + + v4l2_ctrl_handler_init(hdlr, 1); + + v4l2_ctrl_new_std_menu_items(hdlr, &vdic_ctrl_ops, + V4L2_CID_DEINTERLACING_MODE, + HIGH_MOTION, 0, HIGH_MOTION, + vdic_ctrl_motion_menu); + + priv->sd.ctrl_handler = hdlr; + + if (hdlr->error) { + ret = hdlr->error; + goto out_free; + } + + v4l2_ctrl_handler_setup(hdlr); + return 0; + +out_free: + v4l2_ctrl_handler_free(hdlr); + return ret; +} + +static int vdic_s_stream(struct v4l2_subdev *sd, int enable) +{ + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_subdev *src_sd = NULL; + int ret = 0; + + mutex_lock(&priv->lock); + + if (!priv->src || !priv->sink_sd) { + ret = -EPIPE; + goto out; + } + + if (priv->csi_direct) + src_sd = media_entity_to_v4l2_subdev(priv->src); + + /* + * enable/disable streaming only if stream_count is + * going from 0 to 1 / 1 to 0. + */ + if (priv->stream_count != !enable) + goto update_count; + + dev_dbg(priv->dev, "stream %s\n", enable ? "ON" : "OFF"); + + if (enable) + ret = vdic_start(priv); + else + vdic_stop(priv); + if (ret) + goto out; + + if (src_sd) { + /* start/stop upstream */ + ret = v4l2_subdev_call(src_sd, video, s_stream, enable); + ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0; + if (ret) { + if (enable) + vdic_stop(priv); + goto out; + } + } + +update_count: + priv->stream_count += enable ? 1 : -1; + if (priv->stream_count < 0) + priv->stream_count = 0; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static struct v4l2_mbus_framefmt * +__vdic_get_fmt(struct vdic_priv *priv, struct v4l2_subdev_pad_config *cfg, + unsigned int pad, enum v4l2_subdev_format_whence which) +{ + if (which == V4L2_SUBDEV_FORMAT_TRY) + return v4l2_subdev_get_try_format(&priv->sd, cfg, pad); + else + return &priv->format_mbus[pad]; +} + +static int vdic_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_mbus_code_enum *code) +{ + if (code->pad >= VDIC_NUM_PADS) + return -EINVAL; + + return imx_media_enum_ipu_format(&code->code, code->index, CS_SEL_YUV); +} + +static int vdic_get_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_mbus_framefmt *fmt; + int ret = 0; + + if (sdformat->pad >= VDIC_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + + fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which); + if (!fmt) { + ret = -EINVAL; + goto out; + } + + sdformat->format = *fmt; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static void vdic_try_fmt(struct vdic_priv *priv, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat, + const struct imx_media_pixfmt **cc) +{ + struct v4l2_mbus_framefmt *infmt; + + *cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_YUV); + if (!*cc) { + u32 code; + + imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV); + *cc = imx_media_find_ipu_format(code, CS_SEL_YUV); + sdformat->format.code = (*cc)->codes[0]; + } + + infmt = __vdic_get_fmt(priv, cfg, priv->active_input_pad, + sdformat->which); + + switch (sdformat->pad) { + case VDIC_SRC_PAD_DIRECT: + sdformat->format = *infmt; + /* output is always progressive! */ + sdformat->format.field = V4L2_FIELD_NONE; + break; + case VDIC_SINK_PAD_DIRECT: + case VDIC_SINK_PAD_IDMAC: + v4l_bound_align_image(&sdformat->format.width, + MIN_W, MAX_W_VDIC, W_ALIGN, + &sdformat->format.height, + MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN); + + imx_media_fill_default_mbus_fields(&sdformat->format, infmt, + true); + + /* input must be interlaced! Choose SEQ_TB if not */ + if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field)) + sdformat->format.field = V4L2_FIELD_SEQ_TB; + break; + } +} + +static int vdic_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + const struct imx_media_pixfmt *cc; + struct v4l2_mbus_framefmt *fmt; + int ret = 0; + + if (sdformat->pad >= VDIC_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + + if (priv->stream_count > 0) { + ret = -EBUSY; + goto out; + } + + vdic_try_fmt(priv, cfg, sdformat, &cc); + + fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which); + *fmt = sdformat->format; + + /* propagate format to source pad */ + if (sdformat->pad == VDIC_SINK_PAD_DIRECT || + sdformat->pad == VDIC_SINK_PAD_IDMAC) { + const struct imx_media_pixfmt *outcc; + struct v4l2_mbus_framefmt *outfmt; + struct v4l2_subdev_format format; + + format.pad = VDIC_SRC_PAD_DIRECT; + format.which = sdformat->which; + format.format = sdformat->format; + vdic_try_fmt(priv, cfg, &format, &outcc); + + outfmt = __vdic_get_fmt(priv, cfg, VDIC_SRC_PAD_DIRECT, + sdformat->which); + *outfmt = format.format; + if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE) + priv->cc[VDIC_SRC_PAD_DIRECT] = outcc; + } + + if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE) + priv->cc[sdformat->pad] = cc; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int vdic_link_setup(struct media_entity *entity, + const struct media_pad *local, + const struct media_pad *remote, u32 flags) +{ + struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_subdev *remote_sd; + int ret = 0; + + dev_dbg(priv->dev, "link setup %s -> %s", remote->entity->name, + local->entity->name); + + mutex_lock(&priv->lock); + + if (local->flags & MEDIA_PAD_FL_SOURCE) { + if (!is_media_entity_v4l2_subdev(remote->entity)) { + ret = -EINVAL; + goto out; + } + + remote_sd = media_entity_to_v4l2_subdev(remote->entity); + + if (flags & MEDIA_LNK_FL_ENABLED) { + if (priv->sink_sd) { + ret = -EBUSY; + goto out; + } + priv->sink_sd = remote_sd; + } else { + priv->sink_sd = NULL; + } + + goto out; + } + + /* this is a sink pad */ + + if (flags & MEDIA_LNK_FL_ENABLED) { + if (priv->src) { + ret = -EBUSY; + goto out; + } + } else { + priv->src = NULL; + goto out; + } + + if (local->index == VDIC_SINK_PAD_IDMAC) { + struct imx_media_video_dev *vdev = priv->vdev; + + if (!is_media_entity_v4l2_video_device(remote->entity)) { + ret = -EINVAL; + goto out; + } + if (!vdev) { + ret = -ENODEV; + goto out; + } + + priv->csi_direct = false; + } else { + if (!is_media_entity_v4l2_subdev(remote->entity)) { + ret = -EINVAL; + goto out; + } + + remote_sd = media_entity_to_v4l2_subdev(remote->entity); + + /* direct pad must connect to a CSI */ + if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_CSI) || + remote->index != CSI_SRC_PAD_DIRECT) { + ret = -EINVAL; + goto out; + } + + priv->csi_direct = true; + } + + priv->src = remote->entity; + /* record which input pad is now active */ + priv->active_input_pad = local->index; +out: + mutex_unlock(&priv->lock); + return ret; +} + +static int vdic_link_validate(struct v4l2_subdev *sd, + struct media_link *link, + struct v4l2_subdev_format *source_fmt, + struct v4l2_subdev_format *sink_fmt) +{ + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + int ret; + + ret = v4l2_subdev_link_validate_default(sd, link, + source_fmt, sink_fmt); + if (ret) + return ret; + + mutex_lock(&priv->lock); + + if (priv->csi_direct && priv->motion != HIGH_MOTION) { + v4l2_err(&priv->sd, + "direct CSI pipeline requires high motion\n"); + ret = -EINVAL; + } + + mutex_unlock(&priv->lock); + return ret; +} + +static int vdic_g_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + + if (fi->pad >= VDIC_NUM_PADS) + return -EINVAL; + + mutex_lock(&priv->lock); + + fi->interval = priv->frame_interval[fi->pad]; + + mutex_unlock(&priv->lock); + + return 0; +} + +static int vdic_s_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fi) +{ + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + struct v4l2_fract *input_fi, *output_fi; + int ret = 0; + + mutex_lock(&priv->lock); + + input_fi = &priv->frame_interval[priv->active_input_pad]; + output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT]; + + switch (fi->pad) { + case VDIC_SINK_PAD_DIRECT: + case VDIC_SINK_PAD_IDMAC: + /* No limits on input frame interval */ + /* Reset output interval */ + *output_fi = fi->interval; + if (priv->csi_direct) + output_fi->denominator *= 2; + break; + case VDIC_SRC_PAD_DIRECT: + /* + * frame rate at output pad is double input + * rate when using direct CSI->VDIC pipeline. + * + * TODO: implement VDIC frame skipping + */ + fi->interval = *input_fi; + if (priv->csi_direct) + fi->interval.denominator *= 2; + break; + default: + ret = -EINVAL; + goto out; + } + + priv->frame_interval[fi->pad] = fi->interval; +out: + mutex_unlock(&priv->lock); + return ret; +} + +/* + * retrieve our pads parsed from the OF graph by the media device + */ +static int vdic_registered(struct v4l2_subdev *sd) +{ + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + int i, ret; + u32 code; + + /* get media device */ + priv->md = dev_get_drvdata(sd->v4l2_dev->dev); + + for (i = 0; i < VDIC_NUM_PADS; i++) { + priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ? + MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK; + + code = 0; + if (i != VDIC_SINK_PAD_IDMAC) + imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV); + + /* set a default mbus format */ + ret = imx_media_init_mbus_fmt(&priv->format_mbus[i], + 640, 480, code, V4L2_FIELD_NONE, + &priv->cc[i]); + if (ret) + return ret; + + /* init default frame interval */ + priv->frame_interval[i].numerator = 1; + priv->frame_interval[i].denominator = 30; + if (i == VDIC_SRC_PAD_DIRECT) + priv->frame_interval[i].denominator *= 2; + } + + priv->active_input_pad = VDIC_SINK_PAD_DIRECT; + + ret = vdic_init_controls(priv); + if (ret) + return ret; + + ret = media_entity_pads_init(&sd->entity, VDIC_NUM_PADS, priv->pad); + if (ret) + v4l2_ctrl_handler_free(&priv->ctrl_hdlr); + + return ret; +} + +static void vdic_unregistered(struct v4l2_subdev *sd) +{ + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + + v4l2_ctrl_handler_free(&priv->ctrl_hdlr); +} + +static const struct v4l2_subdev_pad_ops vdic_pad_ops = { + .enum_mbus_code = vdic_enum_mbus_code, + .get_fmt = vdic_get_fmt, + .set_fmt = vdic_set_fmt, + .link_validate = vdic_link_validate, +}; + +static const struct v4l2_subdev_video_ops vdic_video_ops = { + .g_frame_interval = vdic_g_frame_interval, + .s_frame_interval = vdic_s_frame_interval, + .s_stream = vdic_s_stream, +}; + +static const struct media_entity_operations vdic_entity_ops = { + .link_setup = vdic_link_setup, + .link_validate = v4l2_subdev_link_validate, +}; + +static const struct v4l2_subdev_ops vdic_subdev_ops = { + .video = &vdic_video_ops, + .pad = &vdic_pad_ops, +}; + +static const struct v4l2_subdev_internal_ops vdic_internal_ops = { + .registered = vdic_registered, + .unregistered = vdic_unregistered, +}; + +static int imx_vdic_probe(struct platform_device *pdev) +{ + struct imx_media_internal_sd_platformdata *pdata; + struct vdic_priv *priv; + int ret; + + priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + platform_set_drvdata(pdev, &priv->sd); + priv->dev = &pdev->dev; + + pdata = priv->dev->platform_data; + priv->ipu_id = pdata->ipu_id; + + v4l2_subdev_init(&priv->sd, &vdic_subdev_ops); + v4l2_set_subdevdata(&priv->sd, priv); + priv->sd.internal_ops = &vdic_internal_ops; + priv->sd.entity.ops = &vdic_entity_ops; + priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; + priv->sd.dev = &pdev->dev; + priv->sd.owner = THIS_MODULE; + priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE; + /* get our group id */ + priv->sd.grp_id = pdata->grp_id; + strncpy(priv->sd.name, pdata->sd_name, sizeof(priv->sd.name)); + + mutex_init(&priv->lock); + + ret = v4l2_async_register_subdev(&priv->sd); + if (ret) + goto free; + + return 0; +free: + mutex_destroy(&priv->lock); + return ret; +} + +static int imx_vdic_remove(struct platform_device *pdev) +{ + struct v4l2_subdev *sd = platform_get_drvdata(pdev); + struct vdic_priv *priv = v4l2_get_subdevdata(sd); + + v4l2_info(sd, "Removing\n"); + + v4l2_async_unregister_subdev(sd); + mutex_destroy(&priv->lock); + media_entity_cleanup(&sd->entity); + + return 0; +} + +static const struct platform_device_id imx_vdic_ids[] = { + { .name = "imx-ipuv3-vdic" }, + { }, +}; +MODULE_DEVICE_TABLE(platform, imx_vdic_ids); + +static struct platform_driver imx_vdic_driver = { + .probe = imx_vdic_probe, + .remove = imx_vdic_remove, + .id_table = imx_vdic_ids, + .driver = { + .name = "imx-ipuv3-vdic", + }, +}; +module_platform_driver(imx_vdic_driver); + +MODULE_DESCRIPTION("i.MX VDIC subdev driver"); +MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("platform:imx-ipuv3-vdic"); diff --git a/drivers/staging/media/imx/imx-media.h b/drivers/staging/media/imx/imx-media.h new file mode 100644 index 000000000000..d409170632bd --- /dev/null +++ b/drivers/staging/media/imx/imx-media.h @@ -0,0 +1,325 @@ +/* + * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC + * + * Copyright (c) 2016 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#ifndef _IMX_MEDIA_H +#define _IMX_MEDIA_H + +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-fwnode.h> +#include <media/v4l2-subdev.h> +#include <media/videobuf2-dma-contig.h> +#include <video/imx-ipu-v3.h> + +/* + * This is somewhat arbitrary, but we need at least: + * - 4 video devices per IPU + * - 3 IC subdevs per IPU + * - 1 VDIC subdev per IPU + * - 2 CSI subdevs per IPU + * - 1 mipi-csi2 receiver subdev + * - 2 video-mux subdevs + * - 2 camera sensor subdevs per IPU (1 parallel, 1 mipi-csi2) + * + */ +/* max video devices */ +#define IMX_MEDIA_MAX_VDEVS 8 +/* max subdevices */ +#define IMX_MEDIA_MAX_SUBDEVS 32 +/* max pads per subdev */ +#define IMX_MEDIA_MAX_PADS 16 +/* max links per pad */ +#define IMX_MEDIA_MAX_LINKS 8 + +/* + * Pad definitions for the subdevs with multiple source or + * sink pads + */ + +/* ipu_csi */ +enum { + CSI_SINK_PAD = 0, + CSI_SRC_PAD_DIRECT, + CSI_SRC_PAD_IDMAC, + CSI_NUM_PADS, +}; + +#define CSI_NUM_SINK_PADS 1 +#define CSI_NUM_SRC_PADS 2 + +/* ipu_vdic */ +enum { + VDIC_SINK_PAD_DIRECT = 0, + VDIC_SINK_PAD_IDMAC, + VDIC_SRC_PAD_DIRECT, + VDIC_NUM_PADS, +}; + +#define VDIC_NUM_SINK_PADS 2 +#define VDIC_NUM_SRC_PADS 1 + +/* ipu_ic_prp */ +enum { + PRP_SINK_PAD = 0, + PRP_SRC_PAD_PRPENC, + PRP_SRC_PAD_PRPVF, + PRP_NUM_PADS, +}; + +#define PRP_NUM_SINK_PADS 1 +#define PRP_NUM_SRC_PADS 2 + +/* ipu_ic_prpencvf */ +enum { + PRPENCVF_SINK_PAD = 0, + PRPENCVF_SRC_PAD, + PRPENCVF_NUM_PADS, +}; + +#define PRPENCVF_NUM_SINK_PADS 1 +#define PRPENCVF_NUM_SRC_PADS 1 + +/* How long to wait for EOF interrupts in the buffer-capture subdevs */ +#define IMX_MEDIA_EOF_TIMEOUT 1000 + +struct imx_media_pixfmt { + u32 fourcc; + u32 codes[4]; + int bpp; /* total bpp */ + enum ipu_color_space cs; + bool planar; /* is a planar format */ + bool bayer; /* is a raw bayer format */ + bool ipufmt; /* is one of the IPU internal formats */ +}; + +struct imx_media_buffer { + struct vb2_v4l2_buffer vbuf; /* v4l buffer must be first */ + struct list_head list; +}; + +struct imx_media_video_dev { + struct video_device *vfd; + + /* the user format */ + struct v4l2_format fmt; + const struct imx_media_pixfmt *cc; +}; + +static inline struct imx_media_buffer *to_imx_media_vb(struct vb2_buffer *vb) +{ + struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); + + return container_of(vbuf, struct imx_media_buffer, vbuf); +} + +struct imx_media_link { + struct device_node *remote_sd_node; + char remote_devname[32]; + int local_pad; + int remote_pad; +}; + +struct imx_media_pad { + struct media_pad pad; + struct imx_media_link link[IMX_MEDIA_MAX_LINKS]; + bool devnode; /* does this pad link to a device node */ + int num_links; + + /* + * list of video devices that can be reached from this pad, + * list is only valid for source pads. + */ + struct imx_media_video_dev *vdev[IMX_MEDIA_MAX_VDEVS]; + int num_vdevs; +}; + +struct imx_media_internal_sd_platformdata { + char sd_name[V4L2_SUBDEV_NAME_SIZE]; + u32 grp_id; + int ipu_id; +}; + +struct imx_media_subdev { + struct v4l2_async_subdev asd; + struct v4l2_subdev *sd; /* set when bound */ + + struct imx_media_pad pad[IMX_MEDIA_MAX_PADS]; + int num_sink_pads; + int num_src_pads; + + /* the platform device if this is an internal subdev */ + struct platform_device *pdev; + /* the devname is needed for async devname match */ + char devname[32]; + + /* if this is a sensor */ + struct v4l2_fwnode_endpoint sensor_ep; +}; + +struct imx_media_dev { + struct media_device md; + struct v4l2_device v4l2_dev; + + /* the pipeline object */ + struct media_pipeline pipe; + + struct mutex mutex; /* protect elements below */ + + /* master subdevice list */ + struct imx_media_subdev subdev[IMX_MEDIA_MAX_SUBDEVS]; + int num_subdevs; + + /* master video device list */ + struct imx_media_video_dev *vdev[IMX_MEDIA_MAX_VDEVS]; + int num_vdevs; + + /* IPUs this media driver control, valid after subdevs bound */ + struct ipu_soc *ipu[2]; + + /* for async subdev registration */ + struct v4l2_async_subdev *async_ptrs[IMX_MEDIA_MAX_SUBDEVS]; + struct v4l2_async_notifier subdev_notifier; +}; + +enum codespace_sel { + CS_SEL_YUV = 0, + CS_SEL_RGB, + CS_SEL_ANY, +}; + +const struct imx_media_pixfmt * +imx_media_find_format(u32 fourcc, enum codespace_sel cs_sel, bool allow_bayer); +int imx_media_enum_format(u32 *fourcc, u32 index, enum codespace_sel cs_sel); +const struct imx_media_pixfmt * +imx_media_find_mbus_format(u32 code, enum codespace_sel cs_sel, + bool allow_bayer); +int imx_media_enum_mbus_format(u32 *code, u32 index, enum codespace_sel cs_sel, + bool allow_bayer); +const struct imx_media_pixfmt * +imx_media_find_ipu_format(u32 code, enum codespace_sel cs_sel); +int imx_media_enum_ipu_format(u32 *code, u32 index, enum codespace_sel cs_sel); + +int imx_media_init_mbus_fmt(struct v4l2_mbus_framefmt *mbus, + u32 width, u32 height, u32 code, u32 field, + const struct imx_media_pixfmt **cc); +void imx_media_fill_default_mbus_fields(struct v4l2_mbus_framefmt *tryfmt, + struct v4l2_mbus_framefmt *fmt, + bool ic_route); +int imx_media_mbus_fmt_to_pix_fmt(struct v4l2_pix_format *pix, + struct v4l2_mbus_framefmt *mbus, + const struct imx_media_pixfmt *cc); +int imx_media_mbus_fmt_to_ipu_image(struct ipu_image *image, + struct v4l2_mbus_framefmt *mbus); +int imx_media_ipu_image_to_mbus_fmt(struct v4l2_mbus_framefmt *mbus, + struct ipu_image *image); + +struct imx_media_subdev * +imx_media_find_async_subdev(struct imx_media_dev *imxmd, + struct device_node *np, + const char *devname); +struct imx_media_subdev * +imx_media_add_async_subdev(struct imx_media_dev *imxmd, + struct device_node *np, + struct platform_device *pdev); +int imx_media_add_pad_link(struct imx_media_dev *imxmd, + struct imx_media_pad *pad, + struct device_node *remote_node, + const char *remote_devname, + int local_pad, int remote_pad); + +void imx_media_grp_id_to_sd_name(char *sd_name, int sz, + u32 grp_id, int ipu_id); + +int imx_media_add_internal_subdevs(struct imx_media_dev *imxmd, + struct imx_media_subdev *csi[4]); +void imx_media_remove_internal_subdevs(struct imx_media_dev *imxmd); + +struct imx_media_subdev * +imx_media_find_subdev_by_sd(struct imx_media_dev *imxmd, + struct v4l2_subdev *sd); +struct imx_media_subdev * +imx_media_find_subdev_by_id(struct imx_media_dev *imxmd, + u32 grp_id); +int imx_media_add_video_device(struct imx_media_dev *imxmd, + struct imx_media_video_dev *vdev); +int imx_media_find_mipi_csi2_channel(struct imx_media_dev *imxmd, + struct media_entity *start_entity); +struct imx_media_subdev * +imx_media_find_upstream_subdev(struct imx_media_dev *imxmd, + struct media_entity *start_entity, + u32 grp_id); +struct imx_media_subdev * +__imx_media_find_sensor(struct imx_media_dev *imxmd, + struct media_entity *start_entity); +struct imx_media_subdev * +imx_media_find_sensor(struct imx_media_dev *imxmd, + struct media_entity *start_entity); + +struct imx_media_dma_buf { + void *virt; + dma_addr_t phys; + unsigned long len; +}; + +void imx_media_free_dma_buf(struct imx_media_dev *imxmd, + struct imx_media_dma_buf *buf); +int imx_media_alloc_dma_buf(struct imx_media_dev *imxmd, + struct imx_media_dma_buf *buf, + int size); + +int imx_media_pipeline_set_stream(struct imx_media_dev *imxmd, + struct media_entity *entity, + bool on); + +/* imx-media-fim.c */ +struct imx_media_fim; +void imx_media_fim_eof_monitor(struct imx_media_fim *fim, struct timespec *ts); +int imx_media_fim_set_stream(struct imx_media_fim *fim, + const struct v4l2_fract *frame_interval, + bool on); +int imx_media_fim_add_controls(struct imx_media_fim *fim); +struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd); +void imx_media_fim_free(struct imx_media_fim *fim); + +/* imx-media-of.c */ +struct imx_media_subdev * +imx_media_of_find_subdev(struct imx_media_dev *imxmd, + struct device_node *np, + const char *name); +int imx_media_of_parse(struct imx_media_dev *dev, + struct imx_media_subdev *(*csi)[4], + struct device_node *np); + +/* imx-media-capture.c */ +struct imx_media_video_dev * +imx_media_capture_device_init(struct v4l2_subdev *src_sd, int pad); +void imx_media_capture_device_remove(struct imx_media_video_dev *vdev); +int imx_media_capture_device_register(struct imx_media_video_dev *vdev); +void imx_media_capture_device_unregister(struct imx_media_video_dev *vdev); +struct imx_media_buffer * +imx_media_capture_device_next_buf(struct imx_media_video_dev *vdev); +void imx_media_capture_device_set_format(struct imx_media_video_dev *vdev, + struct v4l2_pix_format *pix); +void imx_media_capture_device_error(struct imx_media_video_dev *vdev); + +/* subdev group ids */ +#define IMX_MEDIA_GRP_ID_SENSOR (1 << 8) +#define IMX_MEDIA_GRP_ID_VIDMUX (1 << 9) +#define IMX_MEDIA_GRP_ID_CSI2 (1 << 10) +#define IMX_MEDIA_GRP_ID_CSI_BIT 11 +#define IMX_MEDIA_GRP_ID_CSI (0x3 << IMX_MEDIA_GRP_ID_CSI_BIT) +#define IMX_MEDIA_GRP_ID_CSI0 (1 << IMX_MEDIA_GRP_ID_CSI_BIT) +#define IMX_MEDIA_GRP_ID_CSI1 (2 << IMX_MEDIA_GRP_ID_CSI_BIT) +#define IMX_MEDIA_GRP_ID_VDIC (1 << 13) +#define IMX_MEDIA_GRP_ID_IC_PRP (1 << 14) +#define IMX_MEDIA_GRP_ID_IC_PRPENC (1 << 15) +#define IMX_MEDIA_GRP_ID_IC_PRPVF (1 << 16) + +#endif diff --git a/drivers/staging/media/imx/imx6-mipi-csi2.c b/drivers/staging/media/imx/imx6-mipi-csi2.c new file mode 100644 index 000000000000..5061f3f524fd --- /dev/null +++ b/drivers/staging/media/imx/imx6-mipi-csi2.c @@ -0,0 +1,698 @@ +/* + * MIPI CSI-2 Receiver Subdev for Freescale i.MX6 SOC. + * + * Copyright (c) 2012-2017 Mentor Graphics Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ +#include <linux/clk.h> +#include <linux/interrupt.h> +#include <linux/io.h> +#include <linux/iopoll.h> +#include <linux/irq.h> +#include <linux/module.h> +#include <linux/of_graph.h> +#include <linux/platform_device.h> +#include <media/v4l2-device.h> +#include <media/v4l2-fwnode.h> +#include <media/v4l2-subdev.h> +#include "imx-media.h" + +/* + * there must be 5 pads: 1 input pad from sensor, and + * the 4 virtual channel output pads + */ +#define CSI2_SINK_PAD 0 +#define CSI2_NUM_SINK_PADS 1 +#define CSI2_NUM_SRC_PADS 4 +#define CSI2_NUM_PADS 5 + +/* + * The default maximum bit-rate per lane in Mbps, if the + * source subdev does not provide V4L2_CID_LINK_FREQ. + */ +#define CSI2_DEFAULT_MAX_MBPS 849 + +struct csi2_dev { + struct device *dev; + struct v4l2_subdev sd; + struct media_pad pad[CSI2_NUM_PADS]; + struct clk *dphy_clk; + struct clk *pllref_clk; + struct clk *pix_clk; /* what is this? */ + void __iomem *base; + struct v4l2_fwnode_bus_mipi_csi2 bus; + + /* lock to protect all members below */ + struct mutex lock; + + struct v4l2_mbus_framefmt format_mbus; + + int stream_count; + struct v4l2_subdev *src_sd; + bool sink_linked[CSI2_NUM_SRC_PADS]; +}; + +#define DEVICE_NAME "imx6-mipi-csi2" + +/* Register offsets */ +#define CSI2_VERSION 0x000 +#define CSI2_N_LANES 0x004 +#define CSI2_PHY_SHUTDOWNZ 0x008 +#define CSI2_DPHY_RSTZ 0x00c +#define CSI2_RESETN 0x010 +#define CSI2_PHY_STATE 0x014 +#define PHY_STOPSTATEDATA_BIT 4 +#define PHY_STOPSTATEDATA(n) BIT(PHY_STOPSTATEDATA_BIT + (n)) +#define PHY_RXCLKACTIVEHS BIT(8) +#define PHY_RXULPSCLKNOT BIT(9) +#define PHY_STOPSTATECLK BIT(10) +#define CSI2_DATA_IDS_1 0x018 +#define CSI2_DATA_IDS_2 0x01c +#define CSI2_ERR1 0x020 +#define CSI2_ERR2 0x024 +#define CSI2_MSK1 0x028 +#define CSI2_MSK2 0x02c +#define CSI2_PHY_TST_CTRL0 0x030 +#define PHY_TESTCLR BIT(0) +#define PHY_TESTCLK BIT(1) +#define CSI2_PHY_TST_CTRL1 0x034 +#define PHY_TESTEN BIT(16) +/* + * i.MX CSI2IPU Gasket registers follow. The CSI2IPU gasket is + * not part of the MIPI CSI-2 core, but its registers fall in the + * same register map range. + */ +#define CSI2IPU_GASKET 0xf00 +#define CSI2IPU_YUV422_YUYV BIT(2) + +static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev) +{ + return container_of(sdev, struct csi2_dev, sd); +} + +/* + * The required sequence of MIPI CSI-2 startup as specified in the i.MX6 + * reference manual is as follows: + * + * 1. Deassert presetn signal (global reset). + * It's not clear what this "global reset" signal is (maybe APB + * global reset), but in any case this step would be probably + * be carried out during driver load in csi2_probe(). + * + * 2. Configure MIPI Camera Sensor to put all Tx lanes in LP-11 state. + * This must be carried out by the MIPI sensor's s_power(ON) subdev + * op. + * + * 3. D-PHY initialization. + * 4. CSI2 Controller programming (Set N_LANES, deassert PHY_SHUTDOWNZ, + * deassert PHY_RSTZ, deassert CSI2_RESETN). + * 5. Read the PHY status register (PHY_STATE) to confirm that all data and + * clock lanes of the D-PHY are in LP-11 state. + * 6. Configure the MIPI Camera Sensor to start transmitting a clock on the + * D-PHY clock lane. + * 7. CSI2 Controller programming - Read the PHY status register (PHY_STATE) + * to confirm that the D-PHY is receiving a clock on the D-PHY clock lane. + * + * All steps 3 through 7 are carried out by csi2_s_stream(ON) here. Step + * 6 is accomplished by calling the source subdev's s_stream(ON) between + * steps 5 and 7. + */ + +static void csi2_enable(struct csi2_dev *csi2, bool enable) +{ + if (enable) { + writel(0x1, csi2->base + CSI2_PHY_SHUTDOWNZ); + writel(0x1, csi2->base + CSI2_DPHY_RSTZ); + writel(0x1, csi2->base + CSI2_RESETN); + } else { + writel(0x0, csi2->base + CSI2_PHY_SHUTDOWNZ); + writel(0x0, csi2->base + CSI2_DPHY_RSTZ); + writel(0x0, csi2->base + CSI2_RESETN); + } +} + +static void csi2_set_lanes(struct csi2_dev *csi2) +{ + int lanes = csi2->bus.num_data_lanes; + + writel(lanes - 1, csi2->base + CSI2_N_LANES); +} + +static void dw_mipi_csi2_phy_write(struct csi2_dev *csi2, + u32 test_code, u32 test_data) +{ + /* Clear PHY test interface */ + writel(PHY_TESTCLR, csi2->base + CSI2_PHY_TST_CTRL0); + writel(0x0, csi2->base + CSI2_PHY_TST_CTRL1); + writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0); + + /* Raise test interface strobe signal */ + writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0); + + /* Configure address write on falling edge and lower strobe signal */ + writel(PHY_TESTEN | test_code, csi2->base + CSI2_PHY_TST_CTRL1); + writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0); + + /* Configure data write on rising edge and raise strobe signal */ + writel(test_data, csi2->base + CSI2_PHY_TST_CTRL1); + writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0); + + /* Clear strobe signal */ + writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0); +} + +/* + * This table is based on the table documented at + * https://community.nxp.com/docs/DOC-94312. It assumes + * a 27MHz D-PHY pll reference clock. + */ +static const struct { + u32 max_mbps; + u32 hsfreqrange_sel; +} hsfreq_map[] = { + { 90, 0x00}, {100, 0x20}, {110, 0x40}, {125, 0x02}, + {140, 0x22}, {150, 0x42}, {160, 0x04}, {180, 0x24}, + {200, 0x44}, {210, 0x06}, {240, 0x26}, {250, 0x46}, + {270, 0x08}, {300, 0x28}, {330, 0x48}, {360, 0x2a}, + {400, 0x4a}, {450, 0x0c}, {500, 0x2c}, {550, 0x0e}, + {600, 0x2e}, {650, 0x10}, {700, 0x30}, {750, 0x12}, + {800, 0x32}, {850, 0x14}, {900, 0x34}, {950, 0x54}, + {1000, 0x74}, +}; + +static int max_mbps_to_hsfreqrange_sel(u32 max_mbps) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(hsfreq_map); i++) + if (hsfreq_map[i].max_mbps > max_mbps) + return hsfreq_map[i].hsfreqrange_sel; + + return -EINVAL; +} + +static int csi2_dphy_init(struct csi2_dev *csi2) +{ + struct v4l2_ctrl *ctrl; + u32 mbps_per_lane; + int sel; + + ctrl = v4l2_ctrl_find(csi2->src_sd->ctrl_handler, + V4L2_CID_LINK_FREQ); + if (!ctrl) + mbps_per_lane = CSI2_DEFAULT_MAX_MBPS; + else + mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val], + USEC_PER_SEC); + + sel = max_mbps_to_hsfreqrange_sel(mbps_per_lane); + if (sel < 0) + return sel; + + dw_mipi_csi2_phy_write(csi2, 0x44, sel); + + return 0; +} + +/* + * Waits for ultra-low-power state on D-PHY clock lane. This is currently + * unused and may not be needed at all, but keep around just in case. + */ +static int __maybe_unused csi2_dphy_wait_ulp(struct csi2_dev *csi2) +{ + u32 reg; + int ret; + + /* wait for ULP on clock lane */ + ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg, + !(reg & PHY_RXULPSCLKNOT), 0, 500000); + if (ret) { + v4l2_err(&csi2->sd, "ULP timeout, phy_state = 0x%08x\n", reg); + return ret; + } + + /* wait until no errors on bus */ + ret = readl_poll_timeout(csi2->base + CSI2_ERR1, reg, + reg == 0x0, 0, 500000); + if (ret) { + v4l2_err(&csi2->sd, "stable bus timeout, err1 = 0x%08x\n", reg); + return ret; + } + + return 0; +} + +/* Waits for low-power LP-11 state on data and clock lanes. */ +static int csi2_dphy_wait_stopstate(struct csi2_dev *csi2) +{ + u32 mask, reg; + int ret; + + mask = PHY_STOPSTATECLK | + ((csi2->bus.num_data_lanes - 1) << PHY_STOPSTATEDATA_BIT); + + ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg, + (reg & mask) == mask, 0, 500000); + if (ret) { + v4l2_err(&csi2->sd, "LP-11 timeout, phy_state = 0x%08x\n", reg); + return ret; + } + + return 0; +} + +/* Wait for active clock on the clock lane. */ +static int csi2_dphy_wait_clock_lane(struct csi2_dev *csi2) +{ + u32 reg; + int ret; + + ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg, + (reg & PHY_RXCLKACTIVEHS), 0, 500000); + if (ret) { + v4l2_err(&csi2->sd, "clock lane timeout, phy_state = 0x%08x\n", + reg); + return ret; + } + + return 0; +} + +/* Setup the i.MX CSI2IPU Gasket */ +static void csi2ipu_gasket_init(struct csi2_dev *csi2) +{ + u32 reg = 0; + + switch (csi2->format_mbus.code) { + case MEDIA_BUS_FMT_YUYV8_2X8: + case MEDIA_BUS_FMT_YUYV8_1X16: + reg = CSI2IPU_YUV422_YUYV; + break; + default: + break; + } + + writel(reg, csi2->base + CSI2IPU_GASKET); +} + +static int csi2_start(struct csi2_dev *csi2) +{ + int ret; + + ret = clk_prepare_enable(csi2->pix_clk); + if (ret) + return ret; + + /* setup the gasket */ + csi2ipu_gasket_init(csi2); + + /* Step 3 */ + ret = csi2_dphy_init(csi2); + if (ret) + goto err_disable_clk; + + /* Step 4 */ + csi2_set_lanes(csi2); + csi2_enable(csi2, true); + + /* Step 5 */ + ret = csi2_dphy_wait_stopstate(csi2); + if (ret) + goto err_assert_reset; + + /* Step 6 */ + ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1); + ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0; + if (ret) + goto err_assert_reset; + + /* Step 7 */ + ret = csi2_dphy_wait_clock_lane(csi2); + if (ret) + goto err_stop_upstream; + + return 0; + +err_stop_upstream: + v4l2_subdev_call(csi2->src_sd, video, s_stream, 0); +err_assert_reset: + csi2_enable(csi2, false); +err_disable_clk: + clk_disable_unprepare(csi2->pix_clk); + return ret; +} + +static void csi2_stop(struct csi2_dev *csi2) +{ + /* stop upstream */ + v4l2_subdev_call(csi2->src_sd, video, s_stream, 0); + + csi2_enable(csi2, false); + clk_disable_unprepare(csi2->pix_clk); +} + +/* + * V4L2 subdev operations. + */ + +static int csi2_s_stream(struct v4l2_subdev *sd, int enable) +{ + struct csi2_dev *csi2 = sd_to_dev(sd); + int i, ret = 0; + + mutex_lock(&csi2->lock); + + if (!csi2->src_sd) { + ret = -EPIPE; + goto out; + } + + for (i = 0; i < CSI2_NUM_SRC_PADS; i++) { + if (csi2->sink_linked[i]) + break; + } + if (i >= CSI2_NUM_SRC_PADS) { + ret = -EPIPE; + goto out; + } + + /* + * enable/disable streaming only if stream_count is + * going from 0 to 1 / 1 to 0. + */ + if (csi2->stream_count != !enable) + goto update_count; + + dev_dbg(csi2->dev, "stream %s\n", enable ? "ON" : "OFF"); + if (enable) + ret = csi2_start(csi2); + else + csi2_stop(csi2); + if (ret) + goto out; + +update_count: + csi2->stream_count += enable ? 1 : -1; + if (csi2->stream_count < 0) + csi2->stream_count = 0; +out: + mutex_unlock(&csi2->lock); + return ret; +} + +static int csi2_link_setup(struct media_entity *entity, + const struct media_pad *local, + const struct media_pad *remote, u32 flags) +{ + struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); + struct csi2_dev *csi2 = sd_to_dev(sd); + struct v4l2_subdev *remote_sd; + int ret = 0; + + dev_dbg(csi2->dev, "link setup %s -> %s", remote->entity->name, + local->entity->name); + + remote_sd = media_entity_to_v4l2_subdev(remote->entity); + + mutex_lock(&csi2->lock); + + if (local->flags & MEDIA_PAD_FL_SOURCE) { + if (flags & MEDIA_LNK_FL_ENABLED) { + if (csi2->sink_linked[local->index - 1]) { + ret = -EBUSY; + goto out; + } + csi2->sink_linked[local->index - 1] = true; + } else { + csi2->sink_linked[local->index - 1] = false; + } + } else { + if (flags & MEDIA_LNK_FL_ENABLED) { + if (csi2->src_sd) { + ret = -EBUSY; + goto out; + } + csi2->src_sd = remote_sd; + } else { + csi2->src_sd = NULL; + } + } + +out: + mutex_unlock(&csi2->lock); + return ret; +} + +static int csi2_get_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct csi2_dev *csi2 = sd_to_dev(sd); + struct v4l2_mbus_framefmt *fmt; + + mutex_lock(&csi2->lock); + + if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) + fmt = v4l2_subdev_get_try_format(&csi2->sd, cfg, + sdformat->pad); + else + fmt = &csi2->format_mbus; + + sdformat->format = *fmt; + + mutex_unlock(&csi2->lock); + + return 0; +} + +static int csi2_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *sdformat) +{ + struct csi2_dev *csi2 = sd_to_dev(sd); + int ret = 0; + + if (sdformat->pad >= CSI2_NUM_PADS) + return -EINVAL; + + mutex_lock(&csi2->lock); + + if (csi2->stream_count > 0) { + ret = -EBUSY; + goto out; + } + + /* Output pads mirror active input pad, no limits on input pads */ + if (sdformat->pad != CSI2_SINK_PAD) + sdformat->format = csi2->format_mbus; + + if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) + cfg->try_fmt = sdformat->format; + else + csi2->format_mbus = sdformat->format; +out: + mutex_unlock(&csi2->lock); + return ret; +} + +/* + * retrieve our pads parsed from the OF graph by the media device + */ +static int csi2_registered(struct v4l2_subdev *sd) +{ + struct csi2_dev *csi2 = sd_to_dev(sd); + int i, ret; + + for (i = 0; i < CSI2_NUM_PADS; i++) { + csi2->pad[i].flags = (i == CSI2_SINK_PAD) ? + MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE; + } + + /* set a default mbus format */ + ret = imx_media_init_mbus_fmt(&csi2->format_mbus, + 640, 480, 0, V4L2_FIELD_NONE, NULL); + if (ret) + return ret; + + return media_entity_pads_init(&sd->entity, CSI2_NUM_PADS, csi2->pad); +} + +static const struct media_entity_operations csi2_entity_ops = { + .link_setup = csi2_link_setup, + .link_validate = v4l2_subdev_link_validate, +}; + +static const struct v4l2_subdev_video_ops csi2_video_ops = { + .s_stream = csi2_s_stream, +}; + +static const struct v4l2_subdev_pad_ops csi2_pad_ops = { + .get_fmt = csi2_get_fmt, + .set_fmt = csi2_set_fmt, +}; + +static const struct v4l2_subdev_ops csi2_subdev_ops = { + .video = &csi2_video_ops, + .pad = &csi2_pad_ops, +}; + +static const struct v4l2_subdev_internal_ops csi2_internal_ops = { + .registered = csi2_registered, +}; + +static int csi2_parse_endpoints(struct csi2_dev *csi2) +{ + struct device_node *node = csi2->dev->of_node; + struct device_node *epnode; + struct v4l2_fwnode_endpoint ep; + + epnode = of_graph_get_endpoint_by_regs(node, 0, -1); + if (!epnode) { + v4l2_err(&csi2->sd, "failed to get sink endpoint node\n"); + return -EINVAL; + } + + v4l2_fwnode_endpoint_parse(of_fwnode_handle(epnode), &ep); + of_node_put(epnode); + + if (ep.bus_type != V4L2_MBUS_CSI2) { + v4l2_err(&csi2->sd, "invalid bus type, must be MIPI CSI2\n"); + return -EINVAL; + } + + csi2->bus = ep.bus.mipi_csi2; + + dev_dbg(csi2->dev, "data lanes: %d\n", csi2->bus.num_data_lanes); + dev_dbg(csi2->dev, "flags: 0x%08x\n", csi2->bus.flags); + return 0; +} + +static int csi2_probe(struct platform_device *pdev) +{ + struct csi2_dev *csi2; + struct resource *res; + int ret; + + csi2 = devm_kzalloc(&pdev->dev, sizeof(*csi2), GFP_KERNEL); + if (!csi2) + return -ENOMEM; + + csi2->dev = &pdev->dev; + + v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops); + v4l2_set_subdevdata(&csi2->sd, &pdev->dev); + csi2->sd.internal_ops = &csi2_internal_ops; + csi2->sd.entity.ops = &csi2_entity_ops; + csi2->sd.dev = &pdev->dev; + csi2->sd.owner = THIS_MODULE; + csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE; + strcpy(csi2->sd.name, DEVICE_NAME); + csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; + csi2->sd.grp_id = IMX_MEDIA_GRP_ID_CSI2; + + ret = csi2_parse_endpoints(csi2); + if (ret) + return ret; + + csi2->pllref_clk = devm_clk_get(&pdev->dev, "ref"); + if (IS_ERR(csi2->pllref_clk)) { + v4l2_err(&csi2->sd, "failed to get pll reference clock\n"); + ret = PTR_ERR(csi2->pllref_clk); + return ret; + } + + csi2->dphy_clk = devm_clk_get(&pdev->dev, "dphy"); + if (IS_ERR(csi2->dphy_clk)) { + v4l2_err(&csi2->sd, "failed to get dphy clock\n"); + ret = PTR_ERR(csi2->dphy_clk); + return ret; + } + + csi2->pix_clk = devm_clk_get(&pdev->dev, "pix"); + if (IS_ERR(csi2->pix_clk)) { + v4l2_err(&csi2->sd, "failed to get pixel clock\n"); + ret = PTR_ERR(csi2->pix_clk); + return ret; + } + + res = platform_get_resource(pdev, IORESOURCE_MEM, 0); + if (!res) { + v4l2_err(&csi2->sd, "failed to get platform resources\n"); + return -ENODEV; + } + + csi2->base = devm_ioremap(&pdev->dev, res->start, PAGE_SIZE); + if (!csi2->base) { + v4l2_err(&csi2->sd, "failed to map CSI-2 registers\n"); + return -ENOMEM; + } + + mutex_init(&csi2->lock); + + ret = clk_prepare_enable(csi2->pllref_clk); + if (ret) { + v4l2_err(&csi2->sd, "failed to enable pllref_clk\n"); + goto rmmutex; + } + + ret = clk_prepare_enable(csi2->dphy_clk); + if (ret) { + v4l2_err(&csi2->sd, "failed to enable dphy_clk\n"); + goto pllref_off; + } + + platform_set_drvdata(pdev, &csi2->sd); + + ret = v4l2_async_register_subdev(&csi2->sd); + if (ret) + goto dphy_off; + + return 0; + +dphy_off: + clk_disable_unprepare(csi2->dphy_clk); +pllref_off: + clk_disable_unprepare(csi2->pllref_clk); +rmmutex: + mutex_destroy(&csi2->lock); + return ret; +} + +static int csi2_remove(struct platform_device *pdev) +{ + struct v4l2_subdev *sd = platform_get_drvdata(pdev); + struct csi2_dev *csi2 = sd_to_dev(sd); + + v4l2_async_unregister_subdev(sd); + clk_disable_unprepare(csi2->dphy_clk); + clk_disable_unprepare(csi2->pllref_clk); + mutex_destroy(&csi2->lock); + media_entity_cleanup(&sd->entity); + + return 0; +} + +static const struct of_device_id csi2_dt_ids[] = { + { .compatible = "fsl,imx6-mipi-csi2", }, + { /* sentinel */ } +}; +MODULE_DEVICE_TABLE(of, csi2_dt_ids); + +static struct platform_driver csi2_driver = { + .driver = { + .name = DEVICE_NAME, + .of_match_table = csi2_dt_ids, + }, + .probe = csi2_probe, + .remove = csi2_remove, +}; + +module_platform_driver(csi2_driver); + +MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver"); +MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>"); +MODULE_LICENSE("GPL"); |