drivers/media/virtio/virtio_video_caps.c - third_party/linux - Git at Google

 // SPDX-License-Identifier: GPL-2.0+
 /* Driver for virtio video device.
  *
  * Copyright 2019 OpenSynergy GmbH.
  *
  * 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.
  *
  * This program is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  */

 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-dma-sg.h>

 #include "virtio_video.h"

 static void virtio_video_free_frame_rates(struct video_format_frame *frame)
 {
 	if (!frame)
 		return;

 	kfree(frame->frame_rates);
 }

 static void virtio_video_free_frames(struct video_format *fmt)
 {
 	size_t idx = 0;

 	if (!fmt)
 		return;

 	for (idx = 0; idx < fmt->desc.num_frames; idx++)
 		virtio_video_free_frame_rates(&fmt->frames[idx]);
 	kfree(fmt->frames);
 }

 static void virtio_video_free_fmt(struct list_head *fmts_list)
 {
 	struct video_format *fmt = NULL;
 	struct video_format *tmp = NULL;

 	list_for_each_entry_safe(fmt, tmp, fmts_list, formats_list_entry) {
 		list_del(&fmt->formats_list_entry);
 		virtio_video_free_frames(fmt);
 		kfree(fmt);
 	}
 }

 static void virtio_video_free_fmts(struct virtio_video_device *vvd)
 {
 	virtio_video_free_fmt(&vvd->input_fmt_list);
 	virtio_video_free_fmt(&vvd->output_fmt_list);
 }

 static void assign_format_range(struct virtio_video_format_range *d_range,
 				struct virtio_video_format_range *s_range)
 {
 	d_range->min = le32_to_cpu(s_range->min);
 	d_range->max = le32_to_cpu(s_range->max);
 	d_range->step = le32_to_cpu(s_range->step);
 }

 static size_t
 virtio_video_parse_virtio_frame_rate(struct virtio_video_device *vvd,
 				     struct virtio_video_format_range *f_rate,
 				     void *buf)
 {
 	struct virtio_video_format_range *virtio_frame_rate = NULL;
 	size_t frame_rate_size = sizeof(struct virtio_video_format_range);

 	if (!f_rate || !buf || !vvd)
 		return 0;

 	virtio_frame_rate = buf;
 	assign_format_range(f_rate, virtio_frame_rate);
 	return frame_rate_size;
 }

 static size_t virtio_video_parse_virtio_frame(struct virtio_video_device *vvd,
 					      struct video_format_frame *frm,
 					      void *buf)
 {
 	struct virtio_video *vv = NULL;
 	struct virtio_video_format_frame *virtio_frame = NULL;
 	struct virtio_video_format_frame *frame = &frm->frame;
 	struct virtio_video_format_range *rate = NULL;
 	size_t idx, offset = 0;
 	size_t extra_size = 0;

 	if (!frame || !buf || !vvd)
 		return 0;

 	vv = vvd->vv;
 	virtio_frame = buf;

 	assign_format_range(&frame->width, &virtio_frame->width);
 	assign_format_range(&frame->height, &virtio_frame->height);

 	frame->num_rates = le32_to_cpu(virtio_frame->num_rates);
 	frm->frame_rates =  kcalloc(frame->num_rates,
 				    sizeof(struct virtio_video_format_range),
 				    GFP_KERNEL);

 	offset = sizeof(struct virtio_video_format_frame);
 	for (idx = 0; idx < frame->num_rates; idx++) {
 		rate = &frm->frame_rates[idx];
 		extra_size =
 			virtio_video_parse_virtio_frame_rate(vvd, rate,
 							     buf + offset);
 		if (extra_size == 0) {
 			kfree(frm->frame_rates);
 			v4l2_err(&vv->v4l2_dev, "failed to parse frame rate\n");
 			return 0;
 		}
 		offset += extra_size;
 	}

 	return offset;
 }

 static size_t virtio_video_parse_virtio_fmt(struct virtio_video_device *vvd,
 					    struct video_format *fmt, void *buf)
 {
 	struct virtio_video *vv = NULL;
 	struct virtio_video_format_desc *virtio_fmt_desc = NULL;
 	struct virtio_video_format_desc *fmt_desc = NULL;
 	struct video_format_frame *frame = NULL;
 	size_t idx, offset = 0;
 	size_t extra_size = 0;

 	if (!fmt || !buf || !vvd)
 		return 0;

 	vv = vvd->vv;
 	virtio_fmt_desc = buf;
 	fmt_desc = &fmt->desc;

 	fmt_desc->format =
 		virtio_video_format_to_v4l2
 		(le32_to_cpu(virtio_fmt_desc->format));
 	fmt_desc->mask = le64_to_cpu(virtio_fmt_desc->mask);
 	fmt_desc->planes_layout = le32_to_cpu(virtio_fmt_desc->planes_layout);

 	fmt_desc->num_frames = le32_to_cpu(virtio_fmt_desc->num_frames);
 	fmt->frames = kcalloc(fmt_desc->num_frames,
 			      sizeof(struct video_format_frame),
 			      GFP_KERNEL);

 	offset = sizeof(struct virtio_video_format_desc);
 	for (idx = 0; idx < fmt_desc->num_frames; idx++) {
 		frame = &fmt->frames[idx];
 		extra_size =
 			virtio_video_parse_virtio_frame(vvd, frame,
 							buf + offset);
 		if (extra_size == 0) {
 			kfree(fmt->frames);
 			v4l2_err(&vv->v4l2_dev, "failed to parse frame\n");
 			return 0;
 		}
 		offset += extra_size;
 	}

 	return offset;
 }

 int virtio_video_parse_virtio_capability(struct virtio_video_device *vvd,
 					    void *input_buf, void *output_buf)
 {
 	struct virtio_video *vv = NULL;
 	struct virtio_video_query_capability_resp *input_resp = input_buf;
 	struct virtio_video_query_capability_resp *output_resp = output_buf;
 	int fmt_idx = 0;
 	size_t offset = 0;
 	struct video_format *fmt = NULL;

 	if (!input_buf || !output_buf || !vvd)
 		return -1;

 	vv = vvd->vv;

 	if (le32_to_cpu(input_resp->num_descs) <= 0 ||
 	    le32_to_cpu(output_resp->num_descs) <= 0) {
 		v4l2_err(&vv->v4l2_dev, "invalid capability response\n");
 		return -1;
 	}

 	vvd->num_input_fmts = le32_to_cpu(input_resp->num_descs);
 	offset = sizeof(struct virtio_video_query_capability_resp);

 	for (fmt_idx = 0; fmt_idx < vvd->num_input_fmts; fmt_idx++) {
 		size_t fmt_size = 0;

 		fmt = kzalloc(sizeof(*fmt), GFP_KERNEL);
 		if (!fmt) {
 			virtio_video_free_fmts(vvd);
 			return -1;
 		}

 		fmt_size = virtio_video_parse_virtio_fmt(vvd, fmt,
 							 input_buf + offset);
 		if (fmt_size == 0) {
 			v4l2_err(&vv->v4l2_dev, "failed to parse input fmt\n");
 			virtio_video_free_fmts(vvd);
 			kfree(fmt);
 			return -1;
 		}
 		offset += fmt_size;
 		list_add(&fmt->formats_list_entry, &vvd->input_fmt_list);
 	}

 	vvd->num_output_fmts = le32_to_cpu(output_resp->num_descs);
 	offset = sizeof(struct virtio_video_query_capability_resp);

 	for (fmt_idx = 0; fmt_idx < vvd->num_output_fmts; fmt_idx++) {
 		size_t fmt_size = 0;

 		fmt = kzalloc(sizeof(*fmt), GFP_KERNEL);
 		if (!fmt) {
 			virtio_video_free_fmts(vvd);
 			return -1;
 		}

 		fmt_size = virtio_video_parse_virtio_fmt(vvd, fmt,
 							 output_buf + offset);
 		if (fmt_size == 0) {
 			v4l2_err(&vv->v4l2_dev, "failed to parse output fmt\n");
 			virtio_video_free_fmts(vvd);
 			kfree(fmt);
 			return -1;
 		}
 		offset += fmt_size;
 		list_add(&fmt->formats_list_entry, &vvd->output_fmt_list);
 	}
 	return 0;
 }

 void virtio_video_clean_capability(struct virtio_video_device *vvd)
 {
 	if (!vvd)
 		return;
 	virtio_video_free_fmts(vvd);
 }

 static void
 virtio_video_free_control_fmt_data(struct video_control_fmt_data *data)
 {
 	if (!data)
 		return;

 	kfree(data->entries);
 	kfree(data);
 }

 static void virtio_video_free_control_formats(struct virtio_video_device *vvd)
 {
 	struct video_control_format *c_fmt = NULL;
 	struct video_control_format *tmp = NULL;

 	list_for_each_entry_safe(c_fmt, tmp, &vvd->controls_fmt_list,
 				 controls_list_entry) {
 		list_del(&c_fmt->controls_list_entry);
 		virtio_video_free_control_fmt_data(c_fmt->profile);
 		virtio_video_free_control_fmt_data(c_fmt->level);
 		kfree(c_fmt);
 	}
 }

 static int virtio_video_parse_control_levels(struct virtio_video_device *vvd,
 					     struct video_control_format *fmt)
 {
 	int idx, ret = 0;
 	struct virtio_video_query_control_resp *resp_buf = NULL;
 	struct virtio_video_query_control_resp_level *l_resp_buf = NULL;
 	struct virtio_video *vv = NULL;
 	uint32_t virtio_format, num_levels, mask = 0;
 	uint32_t *virtio_levels = NULL;
 	struct video_control_fmt_data *level = NULL;
 	int max = 0, min = UINT_MAX;
 	size_t resp_size;

 	if (!vvd)
 		return -EINVAL;

 	vv = vvd->vv;
 	resp_size = vv->max_resp_len;

 	virtio_format = virtio_video_v4l2_format_to_virtio(fmt->format);

 	resp_buf = kzalloc(resp_size, GFP_KERNEL);
 	if (IS_ERR(resp_buf)) {
 		ret = PTR_ERR(resp_buf);
 		goto err;
 	}

 	vv->got_control = false;
 	ret = virtio_video_query_control_level(vv, resp_buf, resp_size,
 					       virtio_format);
 	if (ret) {
 		v4l2_err(&vv->v4l2_dev, "failed to query level\n");
 		goto err;
 	}

 	ret = wait_event_timeout(vv->wq, vv->got_control, 5 * HZ);
 	if (ret == 0) {
 		v4l2_err(&vv->v4l2_dev,
 			 "timed out waiting for query level\n");
 		ret = -EIO;
 		goto err;
 	}

 	ret = 0;
 	l_resp_buf = (void *)((char *)resp_buf + sizeof(*resp_buf));
 	num_levels = le32_to_cpu(l_resp_buf->num);
 	if (num_levels == 0)
 		goto err;

 	fmt->level = kzalloc(sizeof(*level), GFP_KERNEL);
 	if (!fmt->level) {
 		ret = -ENOMEM;
 		goto err;
 	}

 	level = fmt->level;
 	level->entries = kcalloc(num_levels, sizeof(uint32_t), GFP_KERNEL);
 	if (!level->entries) {
 		ret = -ENOMEM;
 		goto err;
 	}

 	virtio_levels = (void *)((char *)l_resp_buf + sizeof(*l_resp_buf));

 	for (idx = 0; idx < num_levels; idx++) {
 		level->entries[idx] =
 			virtio_video_level_to_v4l2
 			(le32_to_cpu(virtio_levels[idx]));

 		mask = mask | (1 << level->entries[idx]);
 		if (level->entries[idx] > max)
 			max = level->entries[idx];
 		if (level->entries[idx] < min)
 			min = level->entries[idx];
 	}
 	level->min = min;
 	level->max = max;
 	level->num = num_levels;
 	level->skip_mask = ~mask;
 err:
 	kfree(resp_buf);
 	return ret;
 }

 static int virtio_video_parse_control_profiles(struct virtio_video_device *vvd,
 					       struct video_control_format *fmt)
 {
 	int idx, ret = 0;
 	struct virtio_video_query_control_resp *resp_buf = NULL;
 	struct virtio_video_query_control_resp_profile *p_resp_buf = NULL;
 	struct virtio_video *vv = NULL;
 	uint32_t virtio_format, num_profiles, mask = 0;
 	uint32_t *virtio_profiles = NULL;
 	struct video_control_fmt_data *profile = NULL;
 	int max = 0, min = UINT_MAX;
 	size_t resp_size;

 	if (!vvd)
 		return -EINVAL;

 	vv = vvd->vv;
 	resp_size = vv->max_resp_len;
 	virtio_format = virtio_video_v4l2_format_to_virtio(fmt->format);
 	resp_buf = kzalloc(resp_size, GFP_KERNEL);
 	if (IS_ERR(resp_buf)) {
 		ret = PTR_ERR(resp_buf);
 		goto err;
 	}

 	vv->got_control = false;
 	ret = virtio_video_query_control_profile(vv, resp_buf, resp_size,
 						 virtio_format);
 	if (ret) {
 		v4l2_err(&vv->v4l2_dev, "failed to query profile\n");
 		goto err;
 	}

 	ret = wait_event_timeout(vv->wq, vv->got_control, 5 * HZ);
 	if (ret == 0) {
 		v4l2_err(&vv->v4l2_dev,
 			 "timed out waiting for query profile\n");
 		ret = -EIO;
 		goto err;
 	}

 	ret = 0;
 	p_resp_buf = (void *)((char *)resp_buf + sizeof(*resp_buf));
 	num_profiles = le32_to_cpu(p_resp_buf->num);
 	if (num_profiles == 0)
 		goto err;

 	fmt->profile = kzalloc(sizeof(*profile), GFP_KERNEL);
 	if (!fmt->profile) {
 		ret = -ENOMEM;
 		goto err;
 	}

 	profile = fmt->profile;
 	profile->entries = kcalloc(num_profiles, sizeof(uint32_t), GFP_KERNEL);
 	if (!profile->entries) {
 		ret = -ENOMEM;
 		goto err;
 	}

 	virtio_profiles = (void *)((char *)p_resp_buf + sizeof(*p_resp_buf));

 	for (idx = 0; idx < num_profiles; idx++) {
 		profile->entries[idx] =
 			virtio_video_profile_to_v4l2
 			(le32_to_cpu(virtio_profiles[idx]));

 		mask = mask | (1 << profile->entries[idx]);
 		if (profile->entries[idx] > max)
 			max = profile->entries[idx];
 		if (profile->entries[idx] < min)
 			min = profile->entries[idx];
 	}
 	profile->min = min;
 	profile->max = max;
 	profile->num = num_profiles;
 	profile->skip_mask = ~mask;
 err:
 	kfree(resp_buf);
 	return ret;
 }

 int virtio_video_parse_virtio_control(struct virtio_video_device *vvd)
 {
 	struct video_format *fmt = NULL;
 	struct video_control_format *c_fmt = NULL;
 	struct virtio_video *vv = NULL;
 	uint32_t virtio_format;
 	int ret = 0;

 	if (!vvd)
 		return -EINVAL;

 	vv = vvd->vv;

 	list_for_each_entry(fmt, &vvd->output_fmt_list, formats_list_entry) {
 		virtio_format =
 			virtio_video_v4l2_format_to_virtio(fmt->desc.format);
 		if (virtio_format < VIRTIO_VIDEO_FORMAT_CODED_MIN ||
 		    virtio_format > VIRTIO_VIDEO_FORMAT_CODED_MAX)
 			continue;

 		c_fmt = kzalloc(sizeof(*c_fmt), GFP_KERNEL);
 		if (!c_fmt) {
 			virtio_video_free_control_formats(vvd);
 			return -1;
 		}

 		c_fmt->format = fmt->desc.format;
 		ret = virtio_video_parse_control_profiles(vvd, c_fmt);
 		if (ret) {
 			virtio_video_free_control_formats(vvd);
 			kfree(c_fmt);
 			v4l2_err(&vv->v4l2_dev,
 				 "failed to parse control profile\n");
 			goto err;
 		}
 		ret = virtio_video_parse_control_levels(vvd, c_fmt);
 		if (ret) {
 			virtio_video_free_control_formats(vvd);
 			kfree(c_fmt);
 			v4l2_err(&vv->v4l2_dev,
 				 "failed to parse control level\n");
 			goto err;
 		}
 		list_add(&c_fmt->controls_list_entry, &vvd->controls_fmt_list);
 	}
 	return 0;
 err:
 	return ret;
 }

 void virtio_video_clean_control(struct virtio_video_device *vvd)
 {
 	if (!vvd)
 		return;

 	virtio_video_free_control_formats(vvd);
 }
	// SPDX-License-Identifier: GPL-2.0+
	/* Driver for virtio video device.
	*
	* Copyright 2019 OpenSynergy GmbH.
	*
	* 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.
	*
	* This program is distributed in the hope that it will be useful,
	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	* GNU General Public License for more details.
	*
	* You should have received a copy of the GNU General Public License
	* along with this program; if not, see <http://www.gnu.org/licenses/>.
	*/

	#include <media/v4l2-ioctl.h>
	#include <media/videobuf2-dma-sg.h>

	#include "virtio_video.h"

	static void virtio_video_free_frame_rates(struct video_format_frame *frame)
	{
	if (!frame)
	return;

	kfree(frame->frame_rates);
	}

	static void virtio_video_free_frames(struct video_format *fmt)
	{
	size_t idx = 0;

	if (!fmt)
	return;

	for (idx = 0; idx < fmt->desc.num_frames; idx++)
	virtio_video_free_frame_rates(&fmt->frames[idx]);
	kfree(fmt->frames);
	}

	static void virtio_video_free_fmt(struct list_head *fmts_list)
	{
	struct video_format *fmt = NULL;
	struct video_format *tmp = NULL;

	list_for_each_entry_safe(fmt, tmp, fmts_list, formats_list_entry) {
	list_del(&fmt->formats_list_entry);
	virtio_video_free_frames(fmt);
	kfree(fmt);
	}
	}

	static void virtio_video_free_fmts(struct virtio_video_device *vvd)
	{
	virtio_video_free_fmt(&vvd->input_fmt_list);
	virtio_video_free_fmt(&vvd->output_fmt_list);
	}

	static void assign_format_range(struct virtio_video_format_range *d_range,
	struct virtio_video_format_range *s_range)
	{
	d_range->min = le32_to_cpu(s_range->min);
	d_range->max = le32_to_cpu(s_range->max);
	d_range->step = le32_to_cpu(s_range->step);
	}

	static size_t
	virtio_video_parse_virtio_frame_rate(struct virtio_video_device *vvd,
	struct virtio_video_format_range *f_rate,
	void *buf)
	{
	struct virtio_video_format_range *virtio_frame_rate = NULL;
	size_t frame_rate_size = sizeof(struct virtio_video_format_range);

	if (!f_rate \|\| !buf \|\| !vvd)
	return 0;

	virtio_frame_rate = buf;
	assign_format_range(f_rate, virtio_frame_rate);
	return frame_rate_size;
	}

	static size_t virtio_video_parse_virtio_frame(struct virtio_video_device *vvd,
	struct video_format_frame *frm,
	void *buf)
	{
	struct virtio_video *vv = NULL;
	struct virtio_video_format_frame *virtio_frame = NULL;
	struct virtio_video_format_frame *frame = &frm->frame;
	struct virtio_video_format_range *rate = NULL;
	size_t idx, offset = 0;
	size_t extra_size = 0;

	if (!frame \|\| !buf \|\| !vvd)
	return 0;

	vv = vvd->vv;
	virtio_frame = buf;

	assign_format_range(&frame->width, &virtio_frame->width);
	assign_format_range(&frame->height, &virtio_frame->height);

	frame->num_rates = le32_to_cpu(virtio_frame->num_rates);
	frm->frame_rates = kcalloc(frame->num_rates,
	sizeof(struct virtio_video_format_range),
	GFP_KERNEL);

	offset = sizeof(struct virtio_video_format_frame);
	for (idx = 0; idx < frame->num_rates; idx++) {
	rate = &frm->frame_rates[idx];
	extra_size =
	virtio_video_parse_virtio_frame_rate(vvd, rate,
	buf + offset);
	if (extra_size == 0) {
	kfree(frm->frame_rates);
	v4l2_err(&vv->v4l2_dev, "failed to parse frame rate\n");
	return 0;
	}
	offset += extra_size;
	}

	return offset;
	}

	static size_t virtio_video_parse_virtio_fmt(struct virtio_video_device *vvd,
	struct video_format fmt, void buf)
	{
	struct virtio_video *vv = NULL;
	struct virtio_video_format_desc *virtio_fmt_desc = NULL;
	struct virtio_video_format_desc *fmt_desc = NULL;
	struct video_format_frame *frame = NULL;
	size_t idx, offset = 0;
	size_t extra_size = 0;

	if (!fmt \|\| !buf \|\| !vvd)
	return 0;

	vv = vvd->vv;
	virtio_fmt_desc = buf;
	fmt_desc = &fmt->desc;

	fmt_desc->format =
	virtio_video_format_to_v4l2
	(le32_to_cpu(virtio_fmt_desc->format));
	fmt_desc->mask = le64_to_cpu(virtio_fmt_desc->mask);
	fmt_desc->planes_layout = le32_to_cpu(virtio_fmt_desc->planes_layout);

	fmt_desc->num_frames = le32_to_cpu(virtio_fmt_desc->num_frames);
	fmt->frames = kcalloc(fmt_desc->num_frames,
	sizeof(struct video_format_frame),
	GFP_KERNEL);

	offset = sizeof(struct virtio_video_format_desc);
	for (idx = 0; idx < fmt_desc->num_frames; idx++) {
	frame = &fmt->frames[idx];
	extra_size =
	virtio_video_parse_virtio_frame(vvd, frame,
	buf + offset);
	if (extra_size == 0) {
	kfree(fmt->frames);
	v4l2_err(&vv->v4l2_dev, "failed to parse frame\n");
	return 0;
	}
	offset += extra_size;
	}

	return offset;
	}

	int virtio_video_parse_virtio_capability(struct virtio_video_device *vvd,
	void input_buf, void output_buf)
	{
	struct virtio_video *vv = NULL;
	struct virtio_video_query_capability_resp *input_resp = input_buf;
	struct virtio_video_query_capability_resp *output_resp = output_buf;
	int fmt_idx = 0;
	size_t offset = 0;
	struct video_format *fmt = NULL;

	if (!input_buf \|\| !output_buf \|\| !vvd)
	return -1;

	vv = vvd->vv;

	if (le32_to_cpu(input_resp->num_descs) <= 0 \|\|
	le32_to_cpu(output_resp->num_descs) <= 0) {
	v4l2_err(&vv->v4l2_dev, "invalid capability response\n");
	return -1;
	}

	vvd->num_input_fmts = le32_to_cpu(input_resp->num_descs);
	offset = sizeof(struct virtio_video_query_capability_resp);

	for (fmt_idx = 0; fmt_idx < vvd->num_input_fmts; fmt_idx++) {
	size_t fmt_size = 0;

	fmt = kzalloc(sizeof(*fmt), GFP_KERNEL);
	if (!fmt) {
	virtio_video_free_fmts(vvd);
	return -1;
	}

	fmt_size = virtio_video_parse_virtio_fmt(vvd, fmt,
	input_buf + offset);
	if (fmt_size == 0) {
	v4l2_err(&vv->v4l2_dev, "failed to parse input fmt\n");
	virtio_video_free_fmts(vvd);
	kfree(fmt);
	return -1;
	}
	offset += fmt_size;
	list_add(&fmt->formats_list_entry, &vvd->input_fmt_list);
	}

	vvd->num_output_fmts = le32_to_cpu(output_resp->num_descs);
	offset = sizeof(struct virtio_video_query_capability_resp);

	for (fmt_idx = 0; fmt_idx < vvd->num_output_fmts; fmt_idx++) {
	size_t fmt_size = 0;

	fmt = kzalloc(sizeof(*fmt), GFP_KERNEL);
	if (!fmt) {
	virtio_video_free_fmts(vvd);
	return -1;
	}

	fmt_size = virtio_video_parse_virtio_fmt(vvd, fmt,
	output_buf + offset);
	if (fmt_size == 0) {
	v4l2_err(&vv->v4l2_dev, "failed to parse output fmt\n");
	virtio_video_free_fmts(vvd);
	kfree(fmt);
	return -1;
	}
	offset += fmt_size;
	list_add(&fmt->formats_list_entry, &vvd->output_fmt_list);
	}
	return 0;
	}

	void virtio_video_clean_capability(struct virtio_video_device *vvd)
	{
	if (!vvd)
	return;
	virtio_video_free_fmts(vvd);
	}

	static void
	virtio_video_free_control_fmt_data(struct video_control_fmt_data *data)
	{
	if (!data)
	return;

	kfree(data->entries);
	kfree(data);
	}

	static void virtio_video_free_control_formats(struct virtio_video_device *vvd)
	{
	struct video_control_format *c_fmt = NULL;
	struct video_control_format *tmp = NULL;

	list_for_each_entry_safe(c_fmt, tmp, &vvd->controls_fmt_list,
	controls_list_entry) {
	list_del(&c_fmt->controls_list_entry);
	virtio_video_free_control_fmt_data(c_fmt->profile);
	virtio_video_free_control_fmt_data(c_fmt->level);
	kfree(c_fmt);
	}
	}

	static int virtio_video_parse_control_levels(struct virtio_video_device *vvd,
	struct video_control_format *fmt)
	{
	int idx, ret = 0;
	struct virtio_video_query_control_resp *resp_buf = NULL;
	struct virtio_video_query_control_resp_level *l_resp_buf = NULL;
	struct virtio_video *vv = NULL;
	uint32_t virtio_format, num_levels, mask = 0;
	uint32_t *virtio_levels = NULL;
	struct video_control_fmt_data *level = NULL;
	int max = 0, min = UINT_MAX;
	size_t resp_size;

	if (!vvd)
	return -EINVAL;

	vv = vvd->vv;
	resp_size = vv->max_resp_len;

	virtio_format = virtio_video_v4l2_format_to_virtio(fmt->format);

	resp_buf = kzalloc(resp_size, GFP_KERNEL);
	if (IS_ERR(resp_buf)) {
	ret = PTR_ERR(resp_buf);
	goto err;
	}

	vv->got_control = false;
	ret = virtio_video_query_control_level(vv, resp_buf, resp_size,
	virtio_format);
	if (ret) {
	v4l2_err(&vv->v4l2_dev, "failed to query level\n");
	goto err;
	}

	ret = wait_event_timeout(vv->wq, vv->got_control, 5 * HZ);
	if (ret == 0) {
	v4l2_err(&vv->v4l2_dev,
	"timed out waiting for query level\n");
	ret = -EIO;
	goto err;
	}

	ret = 0;
	l_resp_buf = (void )((char )resp_buf + sizeof(*resp_buf));
	num_levels = le32_to_cpu(l_resp_buf->num);
	if (num_levels == 0)
	goto err;

	fmt->level = kzalloc(sizeof(*level), GFP_KERNEL);
	if (!fmt->level) {
	ret = -ENOMEM;
	goto err;
	}

	level = fmt->level;
	level->entries = kcalloc(num_levels, sizeof(uint32_t), GFP_KERNEL);
	if (!level->entries) {
	ret = -ENOMEM;
	goto err;
	}

	virtio_levels = (void )((char )l_resp_buf + sizeof(*l_resp_buf));

	for (idx = 0; idx < num_levels; idx++) {
	level->entries[idx] =
	virtio_video_level_to_v4l2
	(le32_to_cpu(virtio_levels[idx]));

	mask = mask \| (1 << level->entries[idx]);
	if (level->entries[idx] > max)
	max = level->entries[idx];
	if (level->entries[idx] < min)
	min = level->entries[idx];
	}
	level->min = min;
	level->max = max;
	level->num = num_levels;
	level->skip_mask = ~mask;
	err:
	kfree(resp_buf);
	return ret;
	}

	static int virtio_video_parse_control_profiles(struct virtio_video_device *vvd,
	struct video_control_format *fmt)
	{
	int idx, ret = 0;
	struct virtio_video_query_control_resp *resp_buf = NULL;
	struct virtio_video_query_control_resp_profile *p_resp_buf = NULL;
	struct virtio_video *vv = NULL;
	uint32_t virtio_format, num_profiles, mask = 0;
	uint32_t *virtio_profiles = NULL;
	struct video_control_fmt_data *profile = NULL;
	int max = 0, min = UINT_MAX;
	size_t resp_size;

	if (!vvd)
	return -EINVAL;

	vv = vvd->vv;
	resp_size = vv->max_resp_len;
	virtio_format = virtio_video_v4l2_format_to_virtio(fmt->format);
	resp_buf = kzalloc(resp_size, GFP_KERNEL);
	if (IS_ERR(resp_buf)) {
	ret = PTR_ERR(resp_buf);
	goto err;
	}

	vv->got_control = false;
	ret = virtio_video_query_control_profile(vv, resp_buf, resp_size,
	virtio_format);
	if (ret) {
	v4l2_err(&vv->v4l2_dev, "failed to query profile\n");
	goto err;
	}

	ret = wait_event_timeout(vv->wq, vv->got_control, 5 * HZ);
	if (ret == 0) {
	v4l2_err(&vv->v4l2_dev,
	"timed out waiting for query profile\n");
	ret = -EIO;
	goto err;
	}

	ret = 0;
	p_resp_buf = (void )((char )resp_buf + sizeof(*resp_buf));
	num_profiles = le32_to_cpu(p_resp_buf->num);
	if (num_profiles == 0)
	goto err;

	fmt->profile = kzalloc(sizeof(*profile), GFP_KERNEL);
	if (!fmt->profile) {
	ret = -ENOMEM;
	goto err;
	}

	profile = fmt->profile;
	profile->entries = kcalloc(num_profiles, sizeof(uint32_t), GFP_KERNEL);
	if (!profile->entries) {
	ret = -ENOMEM;
	goto err;
	}

	virtio_profiles = (void )((char )p_resp_buf + sizeof(*p_resp_buf));

	for (idx = 0; idx < num_profiles; idx++) {
	profile->entries[idx] =
	virtio_video_profile_to_v4l2
	(le32_to_cpu(virtio_profiles[idx]));

	mask = mask \| (1 << profile->entries[idx]);
	if (profile->entries[idx] > max)
	max = profile->entries[idx];
	if (profile->entries[idx] < min)
	min = profile->entries[idx];
	}
	profile->min = min;
	profile->max = max;
	profile->num = num_profiles;
	profile->skip_mask = ~mask;
	err:
	kfree(resp_buf);
	return ret;
	}

	int virtio_video_parse_virtio_control(struct virtio_video_device *vvd)
	{
	struct video_format *fmt = NULL;
	struct video_control_format *c_fmt = NULL;
	struct virtio_video *vv = NULL;
	uint32_t virtio_format;
	int ret = 0;

	if (!vvd)
	return -EINVAL;

	vv = vvd->vv;

	list_for_each_entry(fmt, &vvd->output_fmt_list, formats_list_entry) {
	virtio_format =
	virtio_video_v4l2_format_to_virtio(fmt->desc.format);
	if (virtio_format < VIRTIO_VIDEO_FORMAT_CODED_MIN \|\|
	virtio_format > VIRTIO_VIDEO_FORMAT_CODED_MAX)
	continue;

	c_fmt = kzalloc(sizeof(*c_fmt), GFP_KERNEL);
	if (!c_fmt) {
	virtio_video_free_control_formats(vvd);
	return -1;
	}

	c_fmt->format = fmt->desc.format;
	ret = virtio_video_parse_control_profiles(vvd, c_fmt);
	if (ret) {
	virtio_video_free_control_formats(vvd);
	kfree(c_fmt);
	v4l2_err(&vv->v4l2_dev,
	"failed to parse control profile\n");
	goto err;
	}
	ret = virtio_video_parse_control_levels(vvd, c_fmt);
	if (ret) {
	virtio_video_free_control_formats(vvd);
	kfree(c_fmt);
	v4l2_err(&vv->v4l2_dev,
	"failed to parse control level\n");
	goto err;
	}
	list_add(&c_fmt->controls_list_entry, &vvd->controls_fmt_list);
	}
	return 0;
	err:
	return ret;
	}

	void virtio_video_clean_control(struct virtio_video_device *vvd)
	{
	if (!vvd)
	return;

	virtio_video_free_control_formats(vvd);
	}