openwrt/target/linux/brcm2708/patches-3.10/0169-V4L2-H264-profile-level-ctrls-FPS-control-and-auto-e.patch

From 94cf90dae250a9dcb3b52b655a94e2106a422698 Mon Sep 17 00:00:00 2001
From: Dave Stevenson <dsteve@broadcom.com>
Date: Wed, 12 Feb 2014 11:18:20 +0000
Subject: [PATCH 169/196] V4L2: H264 profile & level ctrls, FPS control and
 auto exp pri

Several control handling updates.
H264 profile and level controls.
Timeperframe/FPS reworked to add V4L2_CID_EXPOSURE_AUTO_PRIORITY to
select whether AE is allowed to override the framerate specified.

Signed-off-by: Dave Stevenson <dsteve@broadcom.com>
---
 drivers/media/platform/bcm2835/bcm2835-camera.c  | 107 ++++++-----
 drivers/media/platform/bcm2835/bcm2835-camera.h  |  12 +-
 drivers/media/platform/bcm2835/controls.c        | 225 ++++++++++++++++++++++-
 drivers/media/platform/bcm2835/mmal-parameters.h |  87 +++++++++
 4 files changed, 383 insertions(+), 48 deletions(-)

diff --git a/drivers/media/platform/bcm2835/bcm2835-camera.c b/drivers/media/platform/bcm2835/bcm2835-camera.c
index 7f99a14..6d0d77a 100644
--- a/drivers/media/platform/bcm2835/bcm2835-camera.c
+++ b/drivers/media/platform/bcm2835/bcm2835-camera.c
@@ -36,7 +36,8 @@
 
 #define BM2835_MMAL_VERSION "0.0.2"
 #define BM2835_MMAL_MODULE_NAME "bcm2835-v4l2"
-
+#define MIN_WIDTH 16
+#define MIN_HEIGHT 16
 #define MAX_WIDTH 2592
 #define MAX_HEIGHT 1944
 #define MIN_BUFFER_SIZE (80*1024)
@@ -56,7 +57,7 @@ MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2");
 static struct bm2835_mmal_dev *gdev;	/* global device data */
 
 #define FPS_MIN 1
-#define FPS_MAX 30
+#define FPS_MAX 90
 
 /* timeperframe: min/max and default */
 static const struct v4l2_fract
@@ -903,10 +904,8 @@ static int mmal_setup_components(struct bm2835_mmal_dev *dev,
 	camera_port->es.video.crop.y = 0;
 	camera_port->es.video.crop.width = f->fmt.pix.width;
 	camera_port->es.video.crop.height = f->fmt.pix.height;
-	camera_port->es.video.frame_rate.num =
-			dev->capture.timeperframe.denominator;
-	camera_port->es.video.frame_rate.den =
-			dev->capture.timeperframe.numerator;
+	camera_port->es.video.frame_rate.num = 0;
+	camera_port->es.video.frame_rate.den = 1;
 
 	ret = vchiq_mmal_port_set_format(dev->instance, camera_port);
 
@@ -940,8 +939,10 @@ static int mmal_setup_components(struct bm2835_mmal_dev *dev,
 		preview_port->es.video.crop.y = 0;
 		preview_port->es.video.crop.width = f->fmt.pix.width;
 		preview_port->es.video.crop.height = f->fmt.pix.height;
-		preview_port->es.video.frame_rate.num = 30;
-		preview_port->es.video.frame_rate.den = 1;
+		preview_port->es.video.frame_rate.num =
+					  dev->capture.timeperframe.denominator;
+		preview_port->es.video.frame_rate.den =
+					  dev->capture.timeperframe.numerator;
 		ret = vchiq_mmal_port_set_format(dev->instance, preview_port);
 		if (overlay_enabled) {
 			ret = vchiq_mmal_port_connect_tunnel(
@@ -1116,22 +1117,56 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	}
 
 	ret = mmal_setup_components(dev, f);
-	if (ret != 0)
+	if (ret != 0) {
 		v4l2_err(&dev->v4l2_dev,
 			 "%s: failed to setup mmal components: %d\n",
 			 __func__, ret);
+		ret = -EINVAL;
+	}
 
 	return ret;
 }
 
+int vidioc_enum_framesizes(struct file *file, void *fh,
+			   struct v4l2_frmsizeenum *fsize)
+{
+	static const struct v4l2_frmsize_stepwise sizes = {
+		MIN_WIDTH, MAX_WIDTH, 2,
+		MIN_HEIGHT, MAX_HEIGHT, 2
+	};
+	int i;
+
+	if (fsize->index)
+		return -EINVAL;
+	for (i = 0; i < ARRAY_SIZE(formats); i++)
+		if (formats[i].fourcc == fsize->pixel_format)
+			break;
+	if (i == ARRAY_SIZE(formats))
+		return -EINVAL;
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise = sizes;
+	return 0;
+}
+
 /* timeperframe is arbitrary and continous */
 static int vidioc_enum_frameintervals(struct file *file, void *priv,
 					     struct v4l2_frmivalenum *fival)
 {
+	int i;
+
 	if (fival->index)
 		return -EINVAL;
 
-	/* regarding width & height - we support any */
+	for (i = 0; i < ARRAY_SIZE(formats); i++)
+		if (formats[i].fourcc == fival->pixel_format)
+			break;
+	if (i == ARRAY_SIZE(formats))
+		return -EINVAL;
+
+	/* regarding width & height - we support any within range */
+	if (fival->width < MIN_WIDTH || fival->width > MAX_WIDTH ||
+	    fival->height < MIN_HEIGHT || fival->height > MAX_HEIGHT)
+		return -EINVAL;
 
 	fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
 
@@ -1167,7 +1202,6 @@ static int vidioc_s_parm(struct file *file, void *priv,
 	struct bm2835_mmal_dev *dev = video_drvdata(file);
 	struct v4l2_fract tpf;
 	struct mmal_parameter_rational fps_param;
-	int ret;
 
 	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
@@ -1183,27 +1217,11 @@ static int vidioc_s_parm(struct file *file, void *priv,
 	parm->parm.capture.timeperframe = tpf;
 	parm->parm.capture.readbuffers  = 1;
 
-	fps_param.num = dev->capture.timeperframe.denominator;
-	fps_param.den = dev->capture.timeperframe.numerator;
-	ret = vchiq_mmal_port_parameter_set(dev->instance,
-				      &dev->component[MMAL_COMPONENT_CAMERA]->
-					output[MMAL_CAMERA_PORT_PREVIEW],
-				      MMAL_PARAMETER_VIDEO_FRAME_RATE,
-				      &fps_param, sizeof(fps_param));
-	ret += vchiq_mmal_port_parameter_set(dev->instance,
-				      &dev->component[MMAL_COMPONENT_CAMERA]->
-					output[MMAL_CAMERA_PORT_VIDEO],
-				      MMAL_PARAMETER_VIDEO_FRAME_RATE,
-				      &fps_param, sizeof(fps_param));
-	ret += vchiq_mmal_port_parameter_set(dev->instance,
-				      &dev->component[MMAL_COMPONENT_CAMERA]->
-					output[MMAL_CAMERA_PORT_CAPTURE],
-				      MMAL_PARAMETER_VIDEO_FRAME_RATE,
-				      &fps_param, sizeof(fps_param));
-	if (ret)
-		v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "Failed to set fps ret %d\n",
-		 ret);
+	fps_param.num = 0;	/* Select variable fps, and then use
+				 * FPS_RANGE to select the actual limits.
+				 */
+	fps_param.den = 1;
+	set_framerate_params(dev);
 
 	return 0;
 }
@@ -1236,6 +1254,7 @@ static const struct v4l2_ioctl_ops camera0_ioctl_ops = {
 	.vidioc_querybuf = vb2_ioctl_querybuf,
 	.vidioc_qbuf = vb2_ioctl_qbuf,
 	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_enum_framesizes = vidioc_enum_framesizes,
 	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
 	.vidioc_g_parm        = vidioc_g_parm,
 	.vidioc_s_parm        = vidioc_s_parm,
@@ -1331,10 +1350,8 @@ static int __init mmal_init(struct bm2835_mmal_dev *dev)
 	format->es->video.crop.y = 0;
 	format->es->video.crop.width = 1024;
 	format->es->video.crop.height = 768;
-	format->es->video.frame_rate.num =
-			dev->capture.timeperframe.denominator;
-	format->es->video.frame_rate.den =
-			dev->capture.timeperframe.numerator;
+	format->es->video.frame_rate.num = 0; /* Rely on fps_range */
+	format->es->video.frame_rate.den = 1;
 
 	format =
 	    &dev->component[MMAL_COMPONENT_CAMERA]->
@@ -1349,10 +1366,8 @@ static int __init mmal_init(struct bm2835_mmal_dev *dev)
 	format->es->video.crop.y = 0;
 	format->es->video.crop.width = 1024;
 	format->es->video.crop.height = 768;
-	format->es->video.frame_rate.num =
-			dev->capture.timeperframe.denominator;
-	format->es->video.frame_rate.den =
-			dev->capture.timeperframe.numerator;
+	format->es->video.frame_rate.num = 0; /* Rely on fps_range */
+	format->es->video.frame_rate.den = 1;
 
 	format =
 	    &dev->component[MMAL_COMPONENT_CAMERA]->
@@ -1366,7 +1381,7 @@ static int __init mmal_init(struct bm2835_mmal_dev *dev)
 	format->es->video.crop.y = 0;
 	format->es->video.crop.width = 2592;
 	format->es->video.crop.height = 1944;
-	format->es->video.frame_rate.num = 30;
+	format->es->video.frame_rate.num = 0; /* Rely on fps_range */
 	format->es->video.frame_rate.den = 1;
 
 	dev->capture.width = format->es->video.width;
@@ -1374,6 +1389,8 @@ static int __init mmal_init(struct bm2835_mmal_dev *dev)
 	dev->capture.fmt = &formats[0];
 	dev->capture.encode_component = NULL;
 	dev->capture.timeperframe = tpf_default;
+	dev->capture.enc_profile = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH;
+	dev->capture.enc_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
 
 	/* get the preview component ready */
 	ret = vchiq_mmal_component_init(
@@ -1420,6 +1437,14 @@ static int __init mmal_init(struct bm2835_mmal_dev *dev)
 	}
 
 	{
+		struct vchiq_mmal_port *encoder_port =
+			&dev->component[MMAL_COMPONENT_VIDEO_ENCODE]->output[0];
+		encoder_port->format.encoding = MMAL_ENCODING_H264;
+		ret = vchiq_mmal_port_set_format(dev->instance,
+			encoder_port);
+	}
+
+	{
 		unsigned int enable = 1;
 		vchiq_mmal_port_parameter_set(
 			dev->instance,
diff --git a/drivers/media/platform/bcm2835/bcm2835-camera.h b/drivers/media/platform/bcm2835/bcm2835-camera.h
index 25aa91f..8822a1a 100644
--- a/drivers/media/platform/bcm2835/bcm2835-camera.h
+++ b/drivers/media/platform/bcm2835/bcm2835-camera.h
@@ -15,7 +15,7 @@
  * core driver device
  */
 
-#define V4L2_CTRL_COUNT 21 /* number of v4l controls */
+#define V4L2_CTRL_COUNT 24 /* number of v4l controls */
 
 enum {
 	MMAL_COMPONENT_CAMERA = 0,
@@ -49,7 +49,9 @@ struct bm2835_mmal_dev {
 	int                       hflip;
 	int                       vflip;
 	enum mmal_parameter_exposuremode exposure_mode;
+	enum v4l2_exposure_auto_type exposure_mode_v4l2;
 	unsigned int		  manual_shutter_speed;
+	bool			  exp_auto_priority;
 
 	/* allocated mmal instance and components */
 	struct vchiq_mmal_instance   *instance;
@@ -63,12 +65,16 @@ struct bm2835_mmal_dev {
 		unsigned int     height;  /* height */
 		unsigned int     stride;  /* stride */
 		struct mmal_fmt  *fmt;
-		struct v4l2_fract          timeperframe;
+		struct v4l2_fract timeperframe;
 
 		/* H264 encode bitrate */
 		int         encode_bitrate;
 		/* H264 bitrate mode. CBR/VBR */
 		int         encode_bitrate_mode;
+		/* H264 profile */
+		enum v4l2_mpeg_video_h264_profile enc_profile;
+		/* H264 level */
+		enum v4l2_mpeg_video_h264_level enc_level;
 		/* JPEG Q-factor */
 		int         q_factor;
 
@@ -98,7 +104,7 @@ int bm2835_mmal_init_controls(
 			struct v4l2_ctrl_handler *hdl);
 
 int bm2835_mmal_set_all_camera_controls(struct bm2835_mmal_dev *dev);
-
+int set_framerate_params(struct bm2835_mmal_dev *dev);
 
 /* Debug helpers */
 
diff --git a/drivers/media/platform/bcm2835/controls.c b/drivers/media/platform/bcm2835/controls.c
index cb062a9..45cf790 100644
--- a/drivers/media/platform/bcm2835/controls.c
+++ b/drivers/media/platform/bcm2835/controls.c
@@ -69,7 +69,6 @@ static const s64 bitrate_mode_qmenu[] = {
 	(s64)V4L2_MPEG_VIDEO_BITRATE_MODE_CBR,
 };
 
-
 enum bm2835_mmal_ctrl_type {
 	MMAL_CONTROL_TYPE_STD,
 	MMAL_CONTROL_TYPE_STD_MENU,
@@ -329,6 +328,9 @@ static int ctrl_set_exposure(struct bm2835_mmal_dev *dev,
 
 		}
 		dev->exposure_mode = exp_mode;
+		dev->exposure_mode_v4l2 = ctrl->val;
+	} else if (mmal_ctrl->id == V4L2_CID_EXPOSURE_AUTO_PRIORITY) {
+		dev->exp_auto_priority = ctrl->val;
 	}
 
 	if (dev->exposure_mode == MMAL_PARAM_EXPOSUREMODE_OFF)
@@ -340,6 +342,8 @@ static int ctrl_set_exposure(struct bm2835_mmal_dev *dev,
 	ret += vchiq_mmal_port_parameter_set(dev->instance, control,
 					     MMAL_PARAMETER_EXPOSURE_MODE,
 					     &exp_mode, sizeof(u32));
+	ret += set_framerate_params(dev);
+
 	return ret;
 }
 
@@ -540,8 +544,8 @@ static int ctrl_set_colfx(struct bm2835_mmal_dev *dev,
 					&dev->colourfx, sizeof(dev->colourfx));
 
 	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "After: mmal_ctrl:%p ctrl id:0x%x ctrl val:%d ret %d(%d)\n",
-			mmal_ctrl, ctrl->id, ctrl->val, ret,
+		 "%s: After: mmal_ctrl:%p ctrl id:0x%x ctrl val:%d ret %d(%d)\n",
+			__func__, mmal_ctrl, ctrl->id, ctrl->val, ret,
 			(ret == 0 ? 0 : -EINVAL));
 	return (ret == 0 ? 0 : EINVAL);
 }
@@ -623,6 +627,117 @@ static int ctrl_set_video_encode_param_output(struct bm2835_mmal_dev *dev,
 					     &u32_value, sizeof(u32_value));
 }
 
+static int ctrl_set_video_encode_profile_level(struct bm2835_mmal_dev *dev,
+		      struct v4l2_ctrl *ctrl,
+		      const struct bm2835_mmal_v4l2_ctrl *mmal_ctrl)
+{
+	struct mmal_parameter_video_profile param;
+	int ret = 0;
+
+	if (ctrl->id == V4L2_CID_MPEG_VIDEO_H264_PROFILE) {
+		switch (ctrl->val) {
+		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
+		case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
+			dev->capture.enc_profile = ctrl->val;
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+	} else if (ctrl->id == V4L2_CID_MPEG_VIDEO_H264_LEVEL) {
+		switch (ctrl->val) {
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
+			dev->capture.enc_level = ctrl->val;
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+	}
+
+	if (!ret) {
+		switch (dev->capture.enc_profile) {
+		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
+			param.profile = MMAL_VIDEO_PROFILE_H264_BASELINE;
+			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+			param.profile =
+				MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE;
+			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
+			param.profile = MMAL_VIDEO_PROFILE_H264_MAIN;
+			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
+			param.profile = MMAL_VIDEO_PROFILE_H264_HIGH;
+			break;
+		default:
+			/* Should never get here */
+			break;
+		}
+
+		switch (dev->capture.enc_level) {
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
+			param.level = MMAL_VIDEO_LEVEL_H264_1;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
+			param.level = MMAL_VIDEO_LEVEL_H264_1b;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
+			param.level = MMAL_VIDEO_LEVEL_H264_11;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
+			param.level = MMAL_VIDEO_LEVEL_H264_12;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
+			param.level = MMAL_VIDEO_LEVEL_H264_13;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
+			param.level = MMAL_VIDEO_LEVEL_H264_2;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
+			param.level = MMAL_VIDEO_LEVEL_H264_21;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
+			param.level = MMAL_VIDEO_LEVEL_H264_22;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
+			param.level = MMAL_VIDEO_LEVEL_H264_3;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
+			param.level = MMAL_VIDEO_LEVEL_H264_31;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
+			param.level = MMAL_VIDEO_LEVEL_H264_32;
+			break;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
+			param.level = MMAL_VIDEO_LEVEL_H264_4;
+			break;
+		default:
+			/* Should never get here */
+			break;
+		}
+
+		ret = vchiq_mmal_port_parameter_set(dev->instance,
+			&dev->component[MMAL_COMPONENT_VIDEO_ENCODE]->output[0],
+			mmal_ctrl->mmal_id,
+			&param, sizeof(param));
+	}
+	return ret;
+}
+
 static int bm2835_mmal_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct bm2835_mmal_dev *dev =
@@ -639,6 +754,9 @@ static int bm2835_mmal_s_ctrl(struct v4l2_ctrl *ctrl)
 	}
 
 	ret = mmal_ctrl->setter(dev, ctrl, mmal_ctrl);
+	if (ret)
+		pr_warn("ctrl id:%d/MMAL param %08X- returned ret %d\n",
+				ctrl->id, mmal_ctrl->mmal_id, ret);
 	if (mmal_ctrl->ignore_errors)
 		ret = 0;
 	return ret;
@@ -725,6 +843,14 @@ static const struct bm2835_mmal_v4l2_ctrl v4l2_ctrls[V4L2_CTRL_COUNT] = {
 		false
 	},
 	{
+		V4L2_CID_EXPOSURE_AUTO_PRIORITY, MMAL_CONTROL_TYPE_STD,
+		0, 1,
+		0, 1, NULL,
+		0,	/* Dummy MMAL ID as it gets mapped into FPS range*/
+		&ctrl_set_exposure,
+		false
+	},
+	{
 		V4L2_CID_EXPOSURE_METERING,
 		MMAL_CONTROL_TYPE_STD_MENU,
 		~0x7, 2, V4L2_EXPOSURE_METERING_AVERAGE, 0, NULL,
@@ -814,6 +940,39 @@ static const struct bm2835_mmal_v4l2_ctrl v4l2_ctrls[V4L2_CTRL_COUNT] = {
 		&ctrl_set_video_encode_param_output,
 		true	/* Errors ignored as requires latest firmware to work */
 	},
+	{
+		V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+		MMAL_CONTROL_TYPE_STD_MENU,
+		~((1<<V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+			(1<<V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
+			(1<<V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
+			(1<<V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)),
+		V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+		V4L2_MPEG_VIDEO_H264_PROFILE_HIGH, 1, NULL,
+		MMAL_PARAMETER_PROFILE,
+		&ctrl_set_video_encode_profile_level,
+		false
+	},
+	{
+		V4L2_CID_MPEG_VIDEO_H264_LEVEL, MMAL_CONTROL_TYPE_STD_MENU,
+		~((1<<V4L2_MPEG_VIDEO_H264_LEVEL_1_0) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_1B) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_1_1) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_1_2) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_1_3) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_2_0) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_2_1) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_2_2) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_3_0) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_3_1) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_3_2) |
+			(1<<V4L2_MPEG_VIDEO_H264_LEVEL_4_0)),
+		V4L2_MPEG_VIDEO_H264_LEVEL_4_0,
+		V4L2_MPEG_VIDEO_H264_LEVEL_4_0, 1, NULL,
+		MMAL_PARAMETER_PROFILE,
+		&ctrl_set_video_encode_profile_level,
+		false
+	},
 };
 
 int bm2835_mmal_set_all_camera_controls(struct bm2835_mmal_dev *dev)
@@ -825,13 +984,71 @@ int bm2835_mmal_set_all_camera_controls(struct bm2835_mmal_dev *dev)
 		if ((dev->ctrls[c]) && (v4l2_ctrls[c].setter)) {
 			ret = v4l2_ctrls[c].setter(dev, dev->ctrls[c],
 						   &v4l2_ctrls[c]);
-			if (!v4l2_ctrls[c]. ignore_errors && ret)
+			if (!v4l2_ctrls[c].ignore_errors && ret) {
+				v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+					"Failed when setting default values for ctrl %d\n",
+					c);
 				break;
+			}
 		}
 	}
 	return ret;
 }
 
+int set_framerate_params(struct bm2835_mmal_dev *dev)
+{
+	struct mmal_parameter_fps_range fps_range;
+	int ret;
+
+	if ((dev->exposure_mode_v4l2 == V4L2_EXPOSURE_AUTO ||
+	     dev->exposure_mode_v4l2 == V4L2_EXPOSURE_APERTURE_PRIORITY) &&
+	     (dev->exp_auto_priority)) {
+		/* Variable FPS. Define min FPS as 1fps.
+		 * Max as max defined FPS.
+		 */
+		fps_range.fps_low.num = 1;
+		fps_range.fps_low.den = 1;
+		fps_range.fps_high.num = dev->capture.timeperframe.denominator;
+		fps_range.fps_high.den = dev->capture.timeperframe.numerator;
+	} else {
+		/* Fixed FPS - set min and max to be the same */
+		fps_range.fps_low.num = fps_range.fps_high.num =
+			dev->capture.timeperframe.denominator;
+		fps_range.fps_low.den = fps_range.fps_high.den =
+			dev->capture.timeperframe.numerator;
+	}
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "Set fps range to %d/%d to %d/%d\n",
+			 fps_range.fps_low.num,
+			 fps_range.fps_low.den,
+			 fps_range.fps_high.num,
+			 fps_range.fps_high.den
+		 );
+
+	ret = vchiq_mmal_port_parameter_set(dev->instance,
+				      &dev->component[MMAL_COMPONENT_CAMERA]->
+					output[MMAL_CAMERA_PORT_PREVIEW],
+				      MMAL_PARAMETER_FPS_RANGE,
+				      &fps_range, sizeof(fps_range));
+	ret += vchiq_mmal_port_parameter_set(dev->instance,
+				      &dev->component[MMAL_COMPONENT_CAMERA]->
+					output[MMAL_CAMERA_PORT_VIDEO],
+				      MMAL_PARAMETER_FPS_RANGE,
+				      &fps_range, sizeof(fps_range));
+	ret += vchiq_mmal_port_parameter_set(dev->instance,
+				      &dev->component[MMAL_COMPONENT_CAMERA]->
+					output[MMAL_CAMERA_PORT_CAPTURE],
+				      MMAL_PARAMETER_FPS_RANGE,
+				      &fps_range, sizeof(fps_range));
+	if (ret)
+		v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "Failed to set fps ret %d\n",
+		 ret);
+
+	return ret;
+
+}
 int bm2835_mmal_init_controls(struct bm2835_mmal_dev *dev,
 			      struct v4l2_ctrl_handler *hdl)
 {
diff --git a/drivers/media/platform/bcm2835/mmal-parameters.h b/drivers/media/platform/bcm2835/mmal-parameters.h
index b08a4b0..ae8fef9 100644
--- a/drivers/media/platform/bcm2835/mmal-parameters.h
+++ b/drivers/media/platform/bcm2835/mmal-parameters.h
@@ -182,6 +182,14 @@ enum mmal_parameter_camera_config_timestamp_mode {
 					      */
 };
 
+struct mmal_parameter_fps_range {
+	/**< Low end of the permitted framerate range */
+	struct mmal_parameter_rational	fps_low;
+	/**< High end of the permitted framerate range */
+	struct mmal_parameter_rational	fps_high;
+};
+
+
 /* camera configuration parameter */
 struct mmal_parameter_camera_config {
 	/* Parameters for setting up the image pools */
@@ -293,6 +301,85 @@ enum mmal_parameter_rate_control_mode {
 	MMAL_VIDEO_RATECONTROL_CONSTANT_SKIP_FRAMES
 };
 
+enum mmal_video_profile {
+	MMAL_VIDEO_PROFILE_H263_BASELINE,
+	MMAL_VIDEO_PROFILE_H263_H320CODING,
+	MMAL_VIDEO_PROFILE_H263_BACKWARDCOMPATIBLE,
+	MMAL_VIDEO_PROFILE_H263_ISWV2,
+	MMAL_VIDEO_PROFILE_H263_ISWV3,
+	MMAL_VIDEO_PROFILE_H263_HIGHCOMPRESSION,
+	MMAL_VIDEO_PROFILE_H263_INTERNET,
+	MMAL_VIDEO_PROFILE_H263_INTERLACE,
+	MMAL_VIDEO_PROFILE_H263_HIGHLATENCY,
+	MMAL_VIDEO_PROFILE_MP4V_SIMPLE,
+	MMAL_VIDEO_PROFILE_MP4V_SIMPLESCALABLE,
+	MMAL_VIDEO_PROFILE_MP4V_CORE,
+	MMAL_VIDEO_PROFILE_MP4V_MAIN,
+	MMAL_VIDEO_PROFILE_MP4V_NBIT,
+	MMAL_VIDEO_PROFILE_MP4V_SCALABLETEXTURE,
+	MMAL_VIDEO_PROFILE_MP4V_SIMPLEFACE,
+	MMAL_VIDEO_PROFILE_MP4V_SIMPLEFBA,
+	MMAL_VIDEO_PROFILE_MP4V_BASICANIMATED,
+	MMAL_VIDEO_PROFILE_MP4V_HYBRID,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDREALTIME,
+	MMAL_VIDEO_PROFILE_MP4V_CORESCALABLE,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDCODING,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDCORE,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDSCALABLE,
+	MMAL_VIDEO_PROFILE_MP4V_ADVANCEDSIMPLE,
+	MMAL_VIDEO_PROFILE_H264_BASELINE,
+	MMAL_VIDEO_PROFILE_H264_MAIN,
+	MMAL_VIDEO_PROFILE_H264_EXTENDED,
+	MMAL_VIDEO_PROFILE_H264_HIGH,
+	MMAL_VIDEO_PROFILE_H264_HIGH10,
+	MMAL_VIDEO_PROFILE_H264_HIGH422,
+	MMAL_VIDEO_PROFILE_H264_HIGH444,
+	MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE,
+	MMAL_VIDEO_PROFILE_DUMMY = 0x7FFFFFFF
+};
+
+enum mmal_video_level {
+	MMAL_VIDEO_LEVEL_H263_10,
+	MMAL_VIDEO_LEVEL_H263_20,
+	MMAL_VIDEO_LEVEL_H263_30,
+	MMAL_VIDEO_LEVEL_H263_40,
+	MMAL_VIDEO_LEVEL_H263_45,
+	MMAL_VIDEO_LEVEL_H263_50,
+	MMAL_VIDEO_LEVEL_H263_60,
+	MMAL_VIDEO_LEVEL_H263_70,
+	MMAL_VIDEO_LEVEL_MP4V_0,
+	MMAL_VIDEO_LEVEL_MP4V_0b,
+	MMAL_VIDEO_LEVEL_MP4V_1,
+	MMAL_VIDEO_LEVEL_MP4V_2,
+	MMAL_VIDEO_LEVEL_MP4V_3,
+	MMAL_VIDEO_LEVEL_MP4V_4,
+	MMAL_VIDEO_LEVEL_MP4V_4a,
+	MMAL_VIDEO_LEVEL_MP4V_5,
+	MMAL_VIDEO_LEVEL_MP4V_6,
+	MMAL_VIDEO_LEVEL_H264_1,
+	MMAL_VIDEO_LEVEL_H264_1b,
+	MMAL_VIDEO_LEVEL_H264_11,
+	MMAL_VIDEO_LEVEL_H264_12,
+	MMAL_VIDEO_LEVEL_H264_13,
+	MMAL_VIDEO_LEVEL_H264_2,
+	MMAL_VIDEO_LEVEL_H264_21,
+	MMAL_VIDEO_LEVEL_H264_22,
+	MMAL_VIDEO_LEVEL_H264_3,
+	MMAL_VIDEO_LEVEL_H264_31,
+	MMAL_VIDEO_LEVEL_H264_32,
+	MMAL_VIDEO_LEVEL_H264_4,
+	MMAL_VIDEO_LEVEL_H264_41,
+	MMAL_VIDEO_LEVEL_H264_42,
+	MMAL_VIDEO_LEVEL_H264_5,
+	MMAL_VIDEO_LEVEL_H264_51,
+	MMAL_VIDEO_LEVEL_DUMMY = 0x7FFFFFFF
+};
+
+struct mmal_parameter_video_profile {
+	enum mmal_video_profile profile;
+	enum mmal_video_level level;
+};
+
 /* video parameters */
 
 enum mmal_parameter_video_type {
-- 
1.9.1