diff options
Diffstat (limited to 'drivers/media/platform/stm32/stm32-dcmi.c')
| -rw-r--r-- | drivers/media/platform/stm32/stm32-dcmi.c | 323 | 
1 files changed, 267 insertions, 56 deletions
diff --git a/drivers/media/platform/stm32/stm32-dcmi.c b/drivers/media/platform/stm32/stm32-dcmi.c index d855e9c09c08..9392e3409fba 100644 --- a/drivers/media/platform/stm32/stm32-dcmi.c +++ b/drivers/media/platform/stm32/stm32-dcmi.c @@ -100,10 +100,10 @@ enum state {  #define OVERRUN_ERROR_THRESHOLD	3  struct dcmi_graph_entity { -	struct device_node *node; -  	struct v4l2_async_subdev asd; -	struct v4l2_subdev *subdev; + +	struct device_node *remote_node; +	struct v4l2_subdev *source;  };  struct dcmi_format { @@ -169,6 +169,10 @@ struct stm32_dcmi {  	/* Ensure DMA operations atomicity */  	struct mutex			dma_lock; + +	struct media_device		mdev; +	struct media_pad		vid_cap_pad; +	struct media_pipeline		pipeline;  };  static inline struct stm32_dcmi *notifier_to_dcmi(struct v4l2_async_notifier *n) @@ -580,6 +584,144 @@ static void dcmi_buf_queue(struct vb2_buffer *vb)  	spin_unlock_irq(&dcmi->irqlock);  } +static struct media_entity *dcmi_find_source(struct stm32_dcmi *dcmi) +{ +	struct media_entity *entity = &dcmi->vdev->entity; +	struct media_pad *pad; + +	/* Walk searching for entity having no sink */ +	while (1) { +		pad = &entity->pads[0]; +		if (!(pad->flags & MEDIA_PAD_FL_SINK)) +			break; + +		pad = media_entity_remote_pad(pad); +		if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) +			break; + +		entity = pad->entity; +	} + +	return entity; +} + +static int dcmi_pipeline_s_fmt(struct stm32_dcmi *dcmi, +			       struct v4l2_subdev_pad_config *pad_cfg, +			       struct v4l2_subdev_format *format) +{ +	struct media_entity *entity = &dcmi->entity.source->entity; +	struct v4l2_subdev *subdev; +	struct media_pad *sink_pad = NULL; +	struct media_pad *src_pad = NULL; +	struct media_pad *pad = NULL; +	struct v4l2_subdev_format fmt = *format; +	bool found = false; +	int ret; + +	/* +	 * Starting from sensor subdevice, walk within +	 * pipeline and set format on each subdevice +	 */ +	while (1) { +		unsigned int i; + +		/* Search if current entity has a source pad */ +		for (i = 0; i < entity->num_pads; i++) { +			pad = &entity->pads[i]; +			if (pad->flags & MEDIA_PAD_FL_SOURCE) { +				src_pad = pad; +				found = true; +				break; +			} +		} +		if (!found) +			break; + +		subdev = media_entity_to_v4l2_subdev(entity); + +		/* Propagate format on sink pad if any, otherwise source pad */ +		if (sink_pad) +			pad = sink_pad; + +		dev_dbg(dcmi->dev, "\"%s\":%d pad format set to 0x%x %ux%u\n", +			subdev->name, pad->index, format->format.code, +			format->format.width, format->format.height); + +		fmt.pad = pad->index; +		ret = v4l2_subdev_call(subdev, pad, set_fmt, pad_cfg, &fmt); +		if (ret < 0) { +			dev_err(dcmi->dev, "%s: Failed to set format 0x%x %ux%u on \"%s\":%d pad (%d)\n", +				__func__, format->format.code, +				format->format.width, format->format.height, +				subdev->name, pad->index, ret); +			return ret; +		} + +		if (fmt.format.code != format->format.code || +		    fmt.format.width != format->format.width || +		    fmt.format.height != format->format.height) { +			dev_dbg(dcmi->dev, "\"%s\":%d pad format has been changed to 0x%x %ux%u\n", +				subdev->name, pad->index, fmt.format.code, +				fmt.format.width, fmt.format.height); +		} + +		/* Walk to next entity */ +		sink_pad = media_entity_remote_pad(src_pad); +		if (!sink_pad || !is_media_entity_v4l2_subdev(sink_pad->entity)) +			break; + +		entity = sink_pad->entity; +	} +	*format = fmt; + +	return 0; +} + +static int dcmi_pipeline_s_stream(struct stm32_dcmi *dcmi, int state) +{ +	struct media_entity *entity = &dcmi->vdev->entity; +	struct v4l2_subdev *subdev; +	struct media_pad *pad; +	int ret; + +	/* Start/stop all entities within pipeline */ +	while (1) { +		pad = &entity->pads[0]; +		if (!(pad->flags & MEDIA_PAD_FL_SINK)) +			break; + +		pad = media_entity_remote_pad(pad); +		if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) +			break; + +		entity = pad->entity; +		subdev = media_entity_to_v4l2_subdev(entity); + +		ret = v4l2_subdev_call(subdev, video, s_stream, state); +		if (ret < 0 && ret != -ENOIOCTLCMD) { +			dev_err(dcmi->dev, "%s: \"%s\" failed to %s streaming (%d)\n", +				__func__, subdev->name, +				state ? "start" : "stop", ret); +			return ret; +		} + +		dev_dbg(dcmi->dev, "\"%s\" is %s\n", +			subdev->name, state ? "started" : "stopped"); +	} + +	return 0; +} + +static int dcmi_pipeline_start(struct stm32_dcmi *dcmi) +{ +	return dcmi_pipeline_s_stream(dcmi, 1); +} + +static void dcmi_pipeline_stop(struct stm32_dcmi *dcmi) +{ +	dcmi_pipeline_s_stream(dcmi, 0); +} +  static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)  {  	struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq); @@ -594,14 +736,17 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)  		goto err_release_buffers;  	} -	/* Enable stream on the sub device */ -	ret = v4l2_subdev_call(dcmi->entity.subdev, video, s_stream, 1); -	if (ret && ret != -ENOIOCTLCMD) { -		dev_err(dcmi->dev, "%s: Failed to start streaming, subdev streamon error", -			__func__); +	ret = media_pipeline_start(&dcmi->vdev->entity, &dcmi->pipeline); +	if (ret < 0) { +		dev_err(dcmi->dev, "%s: Failed to start streaming, media pipeline start error (%d)\n", +			__func__, ret);  		goto err_pm_put;  	} +	ret = dcmi_pipeline_start(dcmi); +	if (ret) +		goto err_media_pipeline_stop; +  	spin_lock_irq(&dcmi->irqlock);  	/* Set bus width */ @@ -673,7 +818,7 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)  	if (ret) {  		dev_err(dcmi->dev, "%s: Start streaming failed, cannot start capture\n",  			__func__); -		goto err_subdev_streamoff; +		goto err_pipeline_stop;  	}  	/* Enable interruptions */ @@ -684,8 +829,11 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)  	return 0; -err_subdev_streamoff: -	v4l2_subdev_call(dcmi->entity.subdev, video, s_stream, 0); +err_pipeline_stop: +	dcmi_pipeline_stop(dcmi); + +err_media_pipeline_stop: +	media_pipeline_stop(&dcmi->vdev->entity);  err_pm_put:  	pm_runtime_put(dcmi->dev); @@ -710,13 +858,10 @@ static void dcmi_stop_streaming(struct vb2_queue *vq)  {  	struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);  	struct dcmi_buf *buf, *node; -	int ret; -	/* Disable stream on the sub device */ -	ret = v4l2_subdev_call(dcmi->entity.subdev, video, s_stream, 0); -	if (ret && ret != -ENOIOCTLCMD) -		dev_err(dcmi->dev, "%s: Failed to stop streaming, subdev streamoff error (%d)\n", -			__func__, ret); +	dcmi_pipeline_stop(dcmi); + +	media_pipeline_stop(&dcmi->vdev->entity);  	spin_lock_irq(&dcmi->irqlock); @@ -857,7 +1002,7 @@ static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,  	}  	v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code); -	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, set_fmt, +	ret = v4l2_subdev_call(dcmi->entity.source, pad, set_fmt,  			       &pad_cfg, &format);  	if (ret < 0)  		return ret; @@ -934,8 +1079,7 @@ static int dcmi_set_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f)  	mf->width = sd_framesize.width;  	mf->height = sd_framesize.height; -	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, -			       set_fmt, NULL, &format); +	ret = dcmi_pipeline_s_fmt(dcmi, NULL, &format);  	if (ret < 0)  		return ret; @@ -991,7 +1135,7 @@ static int dcmi_get_sensor_format(struct stm32_dcmi *dcmi,  	};  	int ret; -	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, get_fmt, NULL, &fmt); +	ret = v4l2_subdev_call(dcmi->entity.source, pad, get_fmt, NULL, &fmt);  	if (ret)  		return ret; @@ -1020,7 +1164,7 @@ static int dcmi_set_sensor_format(struct stm32_dcmi *dcmi,  	}  	v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code); -	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, set_fmt, +	ret = v4l2_subdev_call(dcmi->entity.source, pad, set_fmt,  			       &pad_cfg, &format);  	if (ret < 0)  		return ret; @@ -1043,7 +1187,7 @@ static int dcmi_get_sensor_bounds(struct stm32_dcmi *dcmi,  	/*  	 * Get sensor bounds first  	 */ -	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, get_selection, +	ret = v4l2_subdev_call(dcmi->entity.source, pad, get_selection,  			       NULL, &bounds);  	if (!ret)  		*r = bounds.r; @@ -1224,7 +1368,7 @@ static int dcmi_enum_framesizes(struct file *file, void *fh,  	fse.code = sd_fmt->mbus_code; -	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, enum_frame_size, +	ret = v4l2_subdev_call(dcmi->entity.source, pad, enum_frame_size,  			       NULL, &fse);  	if (ret)  		return ret; @@ -1241,7 +1385,7 @@ static int dcmi_g_parm(struct file *file, void *priv,  {  	struct stm32_dcmi *dcmi = video_drvdata(file); -	return v4l2_g_parm_cap(video_devdata(file), dcmi->entity.subdev, p); +	return v4l2_g_parm_cap(video_devdata(file), dcmi->entity.source, p);  }  static int dcmi_s_parm(struct file *file, void *priv, @@ -1249,7 +1393,7 @@ static int dcmi_s_parm(struct file *file, void *priv,  {  	struct stm32_dcmi *dcmi = video_drvdata(file); -	return v4l2_s_parm_cap(video_devdata(file), dcmi->entity.subdev, p); +	return v4l2_s_parm_cap(video_devdata(file), dcmi->entity.source, p);  }  static int dcmi_enum_frameintervals(struct file *file, void *fh, @@ -1271,7 +1415,7 @@ static int dcmi_enum_frameintervals(struct file *file, void *fh,  	fie.code = sd_fmt->mbus_code; -	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, +	ret = v4l2_subdev_call(dcmi->entity.source, pad,  			       enum_frame_interval, NULL, &fie);  	if (ret)  		return ret; @@ -1291,7 +1435,7 @@ MODULE_DEVICE_TABLE(of, stm32_dcmi_of_match);  static int dcmi_open(struct file *file)  {  	struct stm32_dcmi *dcmi = video_drvdata(file); -	struct v4l2_subdev *sd = dcmi->entity.subdev; +	struct v4l2_subdev *sd = dcmi->entity.source;  	int ret;  	if (mutex_lock_interruptible(&dcmi->lock)) @@ -1322,7 +1466,7 @@ unlock:  static int dcmi_release(struct file *file)  {  	struct stm32_dcmi *dcmi = video_drvdata(file); -	struct v4l2_subdev *sd = dcmi->entity.subdev; +	struct v4l2_subdev *sd = dcmi->entity.source;  	bool fh_singular;  	int ret; @@ -1409,6 +1553,12 @@ static int dcmi_set_default_fmt(struct stm32_dcmi *dcmi)  	return 0;  } +/* + * FIXME: For the time being we only support subdevices + * which expose RGB & YUV "parallel form" mbus code (_2X8). + * Nevertheless, this allows to support serial source subdevices + * and serial to parallel bridges which conform to this. + */  static const struct dcmi_format dcmi_formats[] = {  	{  		.fourcc = V4L2_PIX_FMT_RGB565, @@ -1433,7 +1583,7 @@ static int dcmi_formats_init(struct stm32_dcmi *dcmi)  {  	const struct dcmi_format *sd_fmts[ARRAY_SIZE(dcmi_formats)];  	unsigned int num_fmts = 0, i, j; -	struct v4l2_subdev *subdev = dcmi->entity.subdev; +	struct v4l2_subdev *subdev = dcmi->entity.source;  	struct v4l2_subdev_mbus_code_enum mbus_code = {  		.which = V4L2_SUBDEV_FORMAT_ACTIVE,  	}; @@ -1447,12 +1597,20 @@ static int dcmi_formats_init(struct stm32_dcmi *dcmi)  			/* Code supported, have we got this fourcc yet? */  			for (j = 0; j < num_fmts; j++)  				if (sd_fmts[j]->fourcc == -						dcmi_formats[i].fourcc) +						dcmi_formats[i].fourcc) {  					/* Already available */ +					dev_dbg(dcmi->dev, "Skipping fourcc/code: %4.4s/0x%x\n", +						(char *)&sd_fmts[j]->fourcc, +						mbus_code.code);  					break; -			if (j == num_fmts) +				} +			if (j == num_fmts) {  				/* New */  				sd_fmts[num_fmts++] = dcmi_formats + i; +				dev_dbg(dcmi->dev, "Supported fourcc/code: %4.4s/0x%x\n", +					(char *)&sd_fmts[num_fmts - 1]->fourcc, +					sd_fmts[num_fmts - 1]->mbus_code); +			}  		}  		mbus_code.index++;  	} @@ -1479,7 +1637,7 @@ static int dcmi_formats_init(struct stm32_dcmi *dcmi)  static int dcmi_framesizes_init(struct stm32_dcmi *dcmi)  {  	unsigned int num_fsize = 0; -	struct v4l2_subdev *subdev = dcmi->entity.subdev; +	struct v4l2_subdev *subdev = dcmi->entity.source;  	struct v4l2_subdev_frame_size_enum fse = {  		.which = V4L2_SUBDEV_FORMAT_ACTIVE,  		.code = dcmi->sd_format->mbus_code, @@ -1526,7 +1684,20 @@ static int dcmi_graph_notify_complete(struct v4l2_async_notifier *notifier)  	struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);  	int ret; -	dcmi->vdev->ctrl_handler = dcmi->entity.subdev->ctrl_handler; +	/* +	 * Now that the graph is complete, +	 * we search for the source subdevice +	 * in order to expose it through V4L2 interface +	 */ +	dcmi->entity.source = +		media_entity_to_v4l2_subdev(dcmi_find_source(dcmi)); +	if (!dcmi->entity.source) { +		dev_err(dcmi->dev, "Source subdevice not found\n"); +		return -ENODEV; +	} + +	dcmi->vdev->ctrl_handler = dcmi->entity.source->ctrl_handler; +  	ret = dcmi_formats_init(dcmi);  	if (ret) {  		dev_err(dcmi->dev, "No supported mediabus format found\n"); @@ -1551,14 +1722,6 @@ static int dcmi_graph_notify_complete(struct v4l2_async_notifier *notifier)  		return ret;  	} -	ret = video_register_device(dcmi->vdev, VFL_TYPE_GRABBER, -1); -	if (ret) { -		dev_err(dcmi->dev, "Failed to register video device\n"); -		return ret; -	} - -	dev_dbg(dcmi->dev, "Device registered as %s\n", -		video_device_node_name(dcmi->vdev));  	return 0;  } @@ -1579,12 +1742,31 @@ static int dcmi_graph_notify_bound(struct v4l2_async_notifier *notifier,  				   struct v4l2_async_subdev *asd)  {  	struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier); +	unsigned int ret; +	int src_pad; -	dev_dbg(dcmi->dev, "Subdev %s bound\n", subdev->name); +	dev_dbg(dcmi->dev, "Subdev \"%s\" bound\n", subdev->name); -	dcmi->entity.subdev = subdev; +	/* +	 * Link this sub-device to DCMI, it could be +	 * a parallel camera sensor or a bridge +	 */ +	src_pad = media_entity_get_fwnode_pad(&subdev->entity, +					      subdev->fwnode, +					      MEDIA_PAD_FL_SOURCE); + +	ret = media_create_pad_link(&subdev->entity, src_pad, +				    &dcmi->vdev->entity, 0, +				    MEDIA_LNK_FL_IMMUTABLE | +				    MEDIA_LNK_FL_ENABLED); +	if (ret) +		dev_err(dcmi->dev, "Failed to create media pad link with subdev \"%s\"\n", +			subdev->name); +	else +		dev_dbg(dcmi->dev, "DCMI is now linked to \"%s\"\n", +			subdev->name); -	return 0; +	return ret;  }  static const struct v4l2_async_notifier_operations dcmi_graph_notify_ops = { @@ -1608,7 +1790,7 @@ static int dcmi_graph_parse(struct stm32_dcmi *dcmi, struct device_node *node)  		return -EINVAL;  	/* Remote node to connect */ -	dcmi->entity.node = remote; +	dcmi->entity.remote_node = remote;  	dcmi->entity.asd.match_type = V4L2_ASYNC_MATCH_FWNODE;  	dcmi->entity.asd.match.fwnode = of_fwnode_handle(remote);  	return 0; @@ -1631,7 +1813,7 @@ static int dcmi_graph_init(struct stm32_dcmi *dcmi)  					     &dcmi->entity.asd);  	if (ret) {  		dev_err(dcmi->dev, "Failed to add subdev notifier\n"); -		of_node_put(dcmi->entity.node); +		of_node_put(dcmi->entity.remote_node);  		return ret;  	} @@ -1679,7 +1861,6 @@ static int dcmi_probe(struct platform_device *pdev)  	np = of_graph_get_next_endpoint(np, NULL);  	if (!np) {  		dev_err(&pdev->dev, "Could not find the endpoint\n"); -		of_node_put(np);  		return -ENODEV;  	} @@ -1699,11 +1880,8 @@ static int dcmi_probe(struct platform_device *pdev)  	dcmi->bus.data_shift = ep.bus.parallel.data_shift;  	irq = platform_get_irq(pdev, 0); -	if (irq <= 0) { -		if (irq != -EPROBE_DEFER) -			dev_err(&pdev->dev, "Could not get irq\n"); +	if (irq <= 0)  		return irq ? irq : -ENXIO; -	}  	dcmi->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);  	if (!dcmi->res) { @@ -1751,10 +1929,19 @@ static int dcmi_probe(struct platform_device *pdev)  	q = &dcmi->queue; +	dcmi->v4l2_dev.mdev = &dcmi->mdev; + +	/* Initialize media device */ +	strscpy(dcmi->mdev.model, DRV_NAME, sizeof(dcmi->mdev.model)); +	snprintf(dcmi->mdev.bus_info, sizeof(dcmi->mdev.bus_info), +		 "platform:%s", DRV_NAME); +	dcmi->mdev.dev = &pdev->dev; +	media_device_init(&dcmi->mdev); +  	/* Initialize the top-level structure */  	ret = v4l2_device_register(&pdev->dev, &dcmi->v4l2_dev);  	if (ret) -		goto err_dma_release; +		goto err_media_device_cleanup;  	dcmi->vdev = video_device_alloc();  	if (!dcmi->vdev) { @@ -1774,6 +1961,25 @@ static int dcmi_probe(struct platform_device *pdev)  				  V4L2_CAP_READWRITE;  	video_set_drvdata(dcmi->vdev, dcmi); +	/* Media entity pads */ +	dcmi->vid_cap_pad.flags = MEDIA_PAD_FL_SINK; +	ret = media_entity_pads_init(&dcmi->vdev->entity, +				     1, &dcmi->vid_cap_pad); +	if (ret) { +		dev_err(dcmi->dev, "Failed to init media entity pad\n"); +		goto err_device_release; +	} +	dcmi->vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT; + +	ret = video_register_device(dcmi->vdev, VFL_TYPE_GRABBER, -1); +	if (ret) { +		dev_err(dcmi->dev, "Failed to register video device\n"); +		goto err_media_entity_cleanup; +	} + +	dev_dbg(dcmi->dev, "Device registered as %s\n", +		video_device_node_name(dcmi->vdev)); +  	/* Buffer queue */  	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;  	q->io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF; @@ -1789,12 +1995,12 @@ static int dcmi_probe(struct platform_device *pdev)  	ret = vb2_queue_init(q);  	if (ret < 0) {  		dev_err(&pdev->dev, "Failed to initialize vb2 queue\n"); -		goto err_device_release; +		goto err_media_entity_cleanup;  	}  	ret = dcmi_graph_init(dcmi);  	if (ret < 0) -		goto err_device_release; +		goto err_media_entity_cleanup;  	/* Reset device */  	ret = reset_control_assert(dcmi->rstc); @@ -1821,11 +2027,14 @@ static int dcmi_probe(struct platform_device *pdev)  err_cleanup:  	v4l2_async_notifier_cleanup(&dcmi->notifier); +err_media_entity_cleanup: +	media_entity_cleanup(&dcmi->vdev->entity);  err_device_release:  	video_device_release(dcmi->vdev);  err_device_unregister:  	v4l2_device_unregister(&dcmi->v4l2_dev); -err_dma_release: +err_media_device_cleanup: +	media_device_cleanup(&dcmi->mdev);  	dma_release_channel(dcmi->dma_chan);  	return ret; @@ -1839,7 +2048,9 @@ static int dcmi_remove(struct platform_device *pdev)  	v4l2_async_notifier_unregister(&dcmi->notifier);  	v4l2_async_notifier_cleanup(&dcmi->notifier); +	media_entity_cleanup(&dcmi->vdev->entity);  	v4l2_device_unregister(&dcmi->v4l2_dev); +	media_device_cleanup(&dcmi->mdev);  	dma_release_channel(dcmi->dma_chan);  |