diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
index dc1c2775bc0b480cd555dd246853feac1476afd6..8553b677eb080937d54a902f18d859dceb4808b6 100644
--- a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
+++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
@@ -260,8 +260,10 @@ static int queue_setup(struct vb2_queue *vq,
 		return -EINVAL;
 	}
 
-	if (*nbuffers < (dev->capture.port->current_buffer.num + 2))
-		*nbuffers = (dev->capture.port->current_buffer.num + 2);
+	if (*nbuffers < dev->capture.port->minimum_buffer.num)
+		*nbuffers = dev->capture.port->minimum_buffer.num;
+
+	dev->capture.port->current_buffer.num = *nbuffers;
 
 	*nplanes = 1;
 
diff --git a/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.c b/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.c
index d6950226551fde33d2dd347080c87682a3e6b070..0f1961aeb223bc0ffa461bff427ab83eb7403c8e 100644
--- a/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.c
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.c
@@ -545,7 +545,6 @@ static int port_buffer_from_host(struct vchiq_mmal_instance *instance,
 	/* peek buffer from queue */
 	spin_lock_irqsave(&port->slock, flags);
 	if (list_empty(&port->buffers)) {
-		port->buffer_underflow++;
 		spin_unlock_irqrestore(&port->slock, flags);
 		return -ENOSPC;
 	}
@@ -636,9 +635,6 @@ static void buffer_to_host_cb(struct vchiq_mmal_instance *instance,
 		    msg->u.buffer_from_host.payload_in_message;
 	}
 
-	/* replace the buffer header */
-	port_buffer_from_host(instance, msg_context->u.bulk.port);
-
 	/* schedule the port callback */
 	schedule_work(&msg_context->u.bulk.work);
 }
@@ -646,10 +642,6 @@ static void buffer_to_host_cb(struct vchiq_mmal_instance *instance,
 static void bulk_receive_cb(struct vchiq_mmal_instance *instance,
 			    struct mmal_msg_context *msg_context)
 {
-	/* replace the buffer header */
-	port_buffer_from_host(msg_context->u.bulk.instance,
-			      msg_context->u.bulk.port);
-
 	msg_context->u.bulk.status = 0;
 
 	/* schedule the port callback */
@@ -661,10 +653,6 @@ static void bulk_abort_cb(struct vchiq_mmal_instance *instance,
 {
 	pr_err("%s: bulk ABORTED msg_context:%p\n", __func__, msg_context);
 
-	/* replace the buffer header */
-	port_buffer_from_host(msg_context->u.bulk.instance,
-			      msg_context->u.bulk.port);
-
 	msg_context->u.bulk.status = -EINTR;
 
 	schedule_work(&msg_context->u.bulk.work);
@@ -1713,14 +1701,7 @@ int vchiq_mmal_submit_buffer(struct vchiq_mmal_instance *instance,
 	list_add_tail(&buffer->list, &port->buffers);
 	spin_unlock_irqrestore(&port->slock, flags);
 
-	/* the port previously underflowed because it was missing a
-	 * mmal_buffer which has just been added, submit that buffer
-	 * to the mmal service.
-	 */
-	if (port->buffer_underflow) {
-		port_buffer_from_host(instance, port);
-		port->buffer_underflow--;
-	}
+	port_buffer_from_host(instance, port);
 
 	return 0;
 }
diff --git a/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.h
index dadf47fe1bdce194a1700847f61a8e497953ff79..0ab9f660b82270acceb356df8b584a1f92dc7f45 100644
--- a/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.h
@@ -79,10 +79,6 @@ struct vchiq_mmal_port {
 	struct list_head buffers;
 	/* lock to serialise adding and removing buffers from list */
 	spinlock_t slock;
-	/* count of how many buffer header refils have failed because
-	 * there was no buffer to satisfy them
-	 */
-	int buffer_underflow;
 	/* callback on buffer completion */
 	vchiq_mmal_buffer_cb buffer_cb;
 	/* callback context */