diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
index e69ec072c2c773..b7214741fa6131 100644
--- a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
+++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
@@ -1762,6 +1762,12 @@ static int mmal_init(struct bm2835_mmal_dev *dev)
 					      MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
 					      &enable,
 					      sizeof(enable));
+
+		/* Enable inserting headers into the first frame */
+		vchiq_mmal_port_parameter_set(dev->instance,
+					      &dev->component[COMP_VIDEO_ENCODE]->control,
+					      MMAL_PARAMETER_VIDEO_ENCODE_HEADERS_WITH_FRAME,
+					      &enable, sizeof(enable));
 	}
 	ret = bm2835_mmal_set_all_camera_controls(dev);
 	if (ret < 0) {
diff --git a/drivers/staging/vc04_services/bcm2835-codec/bcm2835-v4l2-codec.c b/drivers/staging/vc04_services/bcm2835-codec/bcm2835-v4l2-codec.c
index 50395795c3dbaf..1c7fbba6c9c85d 100644
--- a/drivers/staging/vc04_services/bcm2835-codec/bcm2835-v4l2-codec.c
+++ b/drivers/staging/vc04_services/bcm2835-codec/bcm2835-v4l2-codec.c
@@ -1967,6 +1967,19 @@ static int bcm2835_codec_create_component(struct bcm2835_codec_ctx *ctx)
 					MMAL_PARAMETER_VIDEO_ENCODE_SPS_TIMING,
 					&param, sizeof(param));
 
+		/* Enable inserting headers into the first frame */
+		vchiq_mmal_port_parameter_set(ctx->dev->instance,
+					      &ctx->component->control,
+					      MMAL_PARAMETER_VIDEO_ENCODE_HEADERS_WITH_FRAME,
+					      &param, sizeof(param));
+		/*
+		 * Avoid fragmenting the buffers over multiple frames (unless
+		 * the frame is bigger than the whole buffer)
+		 */
+		vchiq_mmal_port_parameter_set(ctx->dev->instance,
+					      &ctx->component->control,
+					      MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
+					      &param, sizeof(param));
 	} else {
 		if (ctx->q_data[V4L2_M2M_DST].sizeimage <
 			ctx->component->output[0].minimum_buffer.size)
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h b/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h
index 9c752db4556586..5b596b5c057f8c 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h
+++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h
@@ -668,6 +668,12 @@ enum mmal_parameter_video_type {
 
 	/**< Takes a @ref MMAL_PARAMETER_UINT32_T */
 	MMAL_PARAMETER_VIDEO_DROPPABLE_PFRAME_LENGTH,
+
+	/**< Take a @ref MMAL_PARAMETER_VIDEO_STALL_T */
+	MMAL_PARAMETER_VIDEO_STALL_THRESHOLD,
+
+	/**< Take a @ref MMAL_PARAMETER_BOOLEAN_T */
+	MMAL_PARAMETER_VIDEO_ENCODE_HEADERS_WITH_FRAME,
 };
 
 /** Valid mirror modes */