diff --git a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_arm.c b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_arm.c
index 5bd5059..89d9b9e 100644
--- a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_arm.c
+++ b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_arm.c
@@ -46,10 +46,10 @@
 #define VCHIQ_MINOR 0
 
 /* Some per-instance constants */
-#define MAX_COMPLETIONS 16
+#define MAX_COMPLETIONS 128
 #define MAX_SERVICES 64
 #define MAX_ELEMENTS 8
-#define MSG_QUEUE_SIZE 64
+#define MSG_QUEUE_SIZE 128
 
 #define KEEPALIVE_VER 1
 #define KEEPALIVE_VER_MIN KEEPALIVE_VER
@@ -208,30 +208,32 @@ add_completion(VCHIQ_INSTANCE_T instance, VCHIQ_REASON_T reason,
 	void *bulk_userdata)
 {
 	VCHIQ_COMPLETION_DATA_T *completion;
+	int insert;
 	DEBUG_INITIALISE(g_state.local)
 
-	while (instance->completion_insert ==
-		(instance->completion_remove + MAX_COMPLETIONS)) {
+	insert = instance->completion_insert;
+	while ((insert - instance->completion_remove) >= MAX_COMPLETIONS) {
 		/* Out of space - wait for the client */
 		DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 		vchiq_log_trace(vchiq_arm_log_level,
 			"add_completion - completion queue full");
 		DEBUG_COUNT(COMPLETION_QUEUE_FULL_COUNT);
+
 		if (down_interruptible(&instance->remove_event) != 0) {
 			vchiq_log_info(vchiq_arm_log_level,
 				"service_callback interrupted");
 			return VCHIQ_RETRY;
-		} else if (instance->closing) {
+		}
+
+		if (instance->closing) {
 			vchiq_log_info(vchiq_arm_log_level,
 				"service_callback closing");
-			return VCHIQ_ERROR;
+			return VCHIQ_SUCCESS;
 		}
 		DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 	}
 
-	completion =
-		 &instance->completions[instance->completion_insert &
-		 (MAX_COMPLETIONS - 1)];
+	completion = &instance->completions[insert & (MAX_COMPLETIONS - 1)];
 
 	completion->header = header;
 	completion->reason = reason;
@@ -252,9 +254,9 @@ add_completion(VCHIQ_INSTANCE_T instance, VCHIQ_REASON_T reason,
 	wmb();
 
 	if (reason == VCHIQ_MESSAGE_AVAILABLE)
-		user_service->message_available_pos =
-			instance->completion_insert;
-	instance->completion_insert++;
+		user_service->message_available_pos = insert;
+
+	instance->completion_insert = ++insert;
 
 	up(&instance->insert_event);
 
@@ -279,6 +281,7 @@ service_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header,
 	USER_SERVICE_T *user_service;
 	VCHIQ_SERVICE_T *service;
 	VCHIQ_INSTANCE_T instance;
+	int skip_completion = 0;
 	DEBUG_INITIALISE(g_state.local)
 
 	DEBUG_TRACE(SERVICE_CALLBACK_LINE);
@@ -345,9 +348,6 @@ service_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header,
 		user_service->msg_queue[user_service->msg_insert &
 			(MSG_QUEUE_SIZE - 1)] = header;
 		user_service->msg_insert++;
-		spin_unlock(&msg_queue_spinlock);
-
-		up(&user_service->insert_event);
 
 		/* If there is a thread waiting in DEQUEUE_MESSAGE, or if
 		** there is a MESSAGE_AVAILABLE in the completion queue then
@@ -356,13 +356,22 @@ service_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header,
 		if (((user_service->message_available_pos -
 			instance->completion_remove) >= 0) ||
 			user_service->dequeue_pending) {
-			DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 			user_service->dequeue_pending = 0;
-			return VCHIQ_SUCCESS;
+			skip_completion = 1;
 		}
 
+		spin_unlock(&msg_queue_spinlock);
+
+		up(&user_service->insert_event);
+
 		header = NULL;
 	}
+
+	if (skip_completion) {
+		DEBUG_TRACE(SERVICE_CALLBACK_LINE);
+		return VCHIQ_SUCCESS;
+	}
+
 	DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 
 	return add_completion(instance, reason, header, user_service,
@@ -789,7 +798,7 @@ vchiq_ioctl(struct cdev *cdev, u_long cmd, caddr_t arg, int fflag,
 			break;
 		}
 
-		memcpy(&args, (const void*)arg, sizeof(args));
+                memcpy(&args, (const void*)arg, sizeof(args));
 
 		lmutex_lock(&instance->completion_mutex);
 
@@ -797,6 +806,7 @@ vchiq_ioctl(struct cdev *cdev, u_long cmd, caddr_t arg, int fflag,
 		while ((instance->completion_remove ==
 			instance->completion_insert)
 			&& !instance->closing) {
+
 			DEBUG_TRACE(AWAIT_COMPLETION_LINE);
 			lmutex_unlock(&instance->completion_mutex);
 			rc = down_interruptible(&instance->insert_event);
@@ -811,24 +821,29 @@ vchiq_ioctl(struct cdev *cdev, u_long cmd, caddr_t arg, int fflag,
 		}
 		DEBUG_TRACE(AWAIT_COMPLETION_LINE);
 
-		/* A read memory barrier is needed to stop prefetch of a stale
-		** completion record
-		*/
-		rmb();
-
 		if (ret == 0) {
 			int msgbufcount = args.msgbufcount;
+			int remove;
+
+			remove = instance->completion_remove;
+
 			for (count = 0; count < args.count; count++) {
 				VCHIQ_COMPLETION_DATA_T *completion;
 				VCHIQ_SERVICE_T *service1;
 				USER_SERVICE_T *user_service;
 				VCHIQ_HEADER_T *header;
-				if (instance->completion_remove ==
-					instance->completion_insert)
+
+				if (remove == instance->completion_insert)
 					break;
+
 				completion = &instance->completions[
-					instance->completion_remove &
-					(MAX_COMPLETIONS - 1)];
+					remove & (MAX_COMPLETIONS - 1)];
+
+
+				/* A read memory barrier is needed to prevent
+				** the prefetch of a stale completion record
+				*/
+				rmb();
 
 				service1 = completion->service_userdata;
 				user_service = service1->base.userdata;
@@ -905,7 +920,11 @@ vchiq_ioctl(struct cdev *cdev, u_long cmd, caddr_t arg, int fflag,
 					break;
 				}
 
-				instance->completion_remove++;
+				/* Ensure that the above copy has completed
+				** before advancing the remove pointer. */
+				mb();
+
+				instance->completion_remove = ++remove;
 			}
 
 			if (msgbufcount != args.msgbufcount) {
diff --git a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.c b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.c
index 2ded208..d96fcfd 100644
--- a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.c
+++ b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.c
@@ -622,15 +622,15 @@ process_free_queue(VCHIQ_STATE_T *state)
 	BITSET_T service_found[BITSET_SIZE(VCHIQ_MAX_SERVICES)];
 	int slot_queue_available;
 
-	/* Use a read memory barrier to ensure that any state that may have
-	** been modified by another thread is not masked by stale prefetched
-	** values. */
-	rmb();
-
 	/* Find slots which have been freed by the other side, and return them
 	** to the available queue. */
 	slot_queue_available = state->slot_queue_available;
 
+	/* Use a memory barrier to ensure that any state that may have been
+	** modified by another thread is not masked by stale prefetched
+	** values. */
+	mb();
+
 	while (slot_queue_available != local->slot_queue_recycle) {
 		unsigned int pos;
 		int slot_index = local->slot_queue[slot_queue_available++ &
@@ -638,6 +638,8 @@ process_free_queue(VCHIQ_STATE_T *state)
 		char *data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index);
 		int data_found = 0;
 
+		rmb();
+
 		vchiq_log_trace(vchiq_core_log_level, "%d: pfq %d=%x %x %x",
 			state->id, slot_index, (unsigned int)data,
 			local->slot_queue_recycle, slot_queue_available);
@@ -753,6 +755,8 @@ process_free_queue(VCHIQ_STATE_T *state)
 				up(&state->data_quota_event);
 		}
 
+		mb();
+
 		state->slot_queue_available = slot_queue_available;
 		up(&state->slot_available_event);
 	}
@@ -904,16 +908,14 @@ queue_message(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
 						error_count);
 					return VCHIQ_ERROR;
 				}
-				if (i == 0) {
-					if (SRVTRACE_ENABLED(service,
-							VCHIQ_LOG_INFO))
-						vchiq_log_dump_mem("Sent", 0,
-							header->data + pos,
-							min(64u,
-							elements[0].size));
-				}
 			}
 
+		if (SRVTRACE_ENABLED(service,
+				VCHIQ_LOG_INFO))
+			vchiq_log_dump_mem("Sent", 0,
+				header->data,
+				min(16, pos));
+
 		spin_lock(&quota_spinlock);
 		service_quota->message_use_count++;
 
@@ -1052,16 +1054,13 @@ queue_message_sync(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
 						error_count);
 					return VCHIQ_ERROR;
 				}
-				if (i == 0) {
-					if (vchiq_sync_log_level >=
-						VCHIQ_LOG_TRACE)
-						vchiq_log_dump_mem("Sent Sync",
-							0, header->data + pos,
-							min(64u,
-							elements[0].size));
-				}
 			}
 
+		if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE)
+			vchiq_log_dump_mem("Sent Sync",
+				0, header->data,
+				min(16, pos));
+
 		VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count);
 		VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size);
 	} else {
@@ -1732,7 +1731,7 @@ parse_rx_slots(VCHIQ_STATE_T *state)
 				remoteport, localport, size);
 			if (size > 0)
 				vchiq_log_dump_mem("Rcvd", 0, header->data,
-					min(64, size));
+					min(16, size));
 		}
 
 		if (((unsigned int)header & VCHIQ_SLOT_MASK) + calc_stride(size)
@@ -2202,7 +2201,7 @@ sync_func(void *v)
 				remoteport, localport, size);
 			if (size > 0)
 				vchiq_log_dump_mem("Rcvd", 0, header->data,
-					min(64, size));
+					min(16, size));
 		}
 
 		switch (type) {