diff --git a/sys/arm/broadcom/bcm2835/bcm2835_audio.c b/sys/arm/broadcom/bcm2835/bcm2835_audio.c
--- a/sys/arm/broadcom/bcm2835/bcm2835_audio.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_audio.c
@@ -173,6 +173,16 @@
 /* Useful for circular buffer calcs */
 #define MOD_DIFF(front,rear,mod) (((mod) + (front) - (rear)) % (mod))
 
+static ssize_t
+bcm2835_memcpy_wrapper(void *_src, void *_dst, size_t offset, size_t size)
+{
+	uint8_t *src = _src;
+	uint8_t *dst = _dst;
+
+	memcpy(dst + offset, src + offset, size);
+
+	return (size);
+}
 
 static const char *
 dest_description(uint32_t dest)
@@ -389,8 +399,8 @@
 
 	if (sc->vchi_handle != VCHIQ_SERVICE_HANDLE_INVALID) {
 		m.type = VC_AUDIO_MSG_TYPE_START;
-		ret = vchi_msg_queue(sc->vchi_handle,
-		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);
+		ret = vchi_msg_queue(sc->vchi_handle, bcm2835_memcpy_wrapper,
+		    &m, sizeof m);
 
 		if (ret != 0)
 			BCM2835_LOG_ERROR(sc,
@@ -411,8 +421,8 @@
 		m.u.stop.draining = 0;
 
 		BCM2835_LOG_INFO(sc,"sending stop\n");
-		ret = vchi_msg_queue(sc->vchi_handle,
-		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);
+		ret = vchi_msg_queue(sc->vchi_handle, bcm2835_memcpy_wrapper,
+		    &m, sizeof m);
 
 		if (ret != 0)
 			BCM2835_LOG_ERROR(sc,
@@ -429,8 +439,8 @@
 
 	if (sc->vchi_handle != VCHIQ_SERVICE_HANDLE_INVALID) {
 		m.type = VC_AUDIO_MSG_TYPE_OPEN;
-		ret = vchi_msg_queue(sc->vchi_handle,
-		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);
+		ret = vchi_msg_queue(sc->vchi_handle, bcm2835_memcpy_wrapper,
+		    &m, sizeof m);
 
 		if (ret != 0)
 			BCM2835_LOG_ERROR(sc,
@@ -453,8 +463,8 @@
 		db = db_levels[volume/5];
 		m.u.control.volume = VCHIQ_AUDIO_VOLUME(db);
 
-		ret = vchi_msg_queue(sc->vchi_handle,
-		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);
+		ret = vchi_msg_queue(sc->vchi_handle, bcm2835_memcpy_wrapper,
+		    &m, sizeof m);
 
 		if (ret != 0)
 			BCM2835_LOG_ERROR(sc,
@@ -475,8 +485,8 @@
 		m.u.config.samplerate = speed;
 		m.u.config.bps = AFMT_BIT(fmt);
 
-		ret = vchi_msg_queue(sc->vchi_handle,
-		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);
+		ret = vchi_msg_queue(sc->vchi_handle, bcm2835_memcpy_wrapper,
+		    &m, sizeof m);
 
 		if (ret != 0)
 			BCM2835_LOG_ERROR(sc,
@@ -537,8 +547,8 @@
 #endif
 	m.u.write.silence = 0;
 
-	ret = vchi_msg_queue(sc->vchi_handle,
-	    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);
+	ret = vchi_msg_queue(sc->vchi_handle, bcm2835_memcpy_wrapper,
+	    &m, sizeof m);
 
 	if (ret != 0)
 		BCM2835_LOG_ERROR(sc, "%s: vchi_msg_queue failed (err %d)\n",
@@ -546,8 +556,8 @@
 
 	while (count > 0) {
 		int bytes = MIN((int)m.u.write.max_packet, (int)count);
-		ret = vchi_msg_queue(sc->vchi_handle,
-		    buf, bytes, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);
+		ret = vchi_msg_queue(sc->vchi_handle, bcm2835_memcpy_wrapper,
+		    buf, bytes);
 		if (ret != 0)
 			BCM2835_LOG_ERROR(sc, "%s: vchi_msg_queue failed: %d\n",
 			    __func__, ret);
diff --git a/sys/contrib/vchiq/interface/vchi/vchi.h b/sys/contrib/vchiq/interface/vchi/vchi.h
--- a/sys/contrib/vchiq/interface/vchi/vchi.h
+++ b/sys/contrib/vchiq/interface/vchi/vchi.h
@@ -226,25 +226,12 @@
 					int value);
 
 // Routine to send a message across a service
-extern int32_t vchi_msg_queue( VCHI_SERVICE_HANDLE_T handle,
-                               const void *data,
-                               uint32_t data_size,
-                               VCHI_FLAGS_T flags,
-                               void *msg_handle );
-
-// scatter-gather (vector) and send message
-int32_t vchi_msg_queuev_ex( VCHI_SERVICE_HANDLE_T handle,
-                            VCHI_MSG_VECTOR_EX_T *vector,
-                            uint32_t count,
-                            VCHI_FLAGS_T flags,
-                            void *msg_handle );
-
-// legacy scatter-gather (vector) and send message, only handles pointers
-int32_t vchi_msg_queuev( VCHI_SERVICE_HANDLE_T handle,
-                         VCHI_MSG_VECTOR_T *vector,
-                         uint32_t count,
-                         VCHI_FLAGS_T flags,
-                         void *msg_handle );
+extern int32_t
+	vchi_msg_queue(VCHI_SERVICE_HANDLE_T handle,
+		       ssize_t (*copy_callback)(void *context, void *dest,
+						size_t offset, size_t maxsize),
+		       void *context,
+		       uint32_t data_size);
 
 // Routine to receive a msg from a service
 // Dequeue is equivalent to hold, copy into client buffer, release
diff --git a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_2835_arm.c b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_2835_arm.c
--- a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_2835_arm.c
+++ b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_2835_arm.c
@@ -318,20 +318,6 @@
    return &((VCHIQ_2835_ARM_STATE_T*)state->platform_state)->arm_state;
 }
 
-VCHIQ_STATUS_T
-vchiq_copy_from_user(void *dst, const void *src, int size)
-{
-
-	if (((vm_offset_t)(src)) < VM_MIN_KERNEL_ADDRESS) {
-		int error = copyin(src, dst, size);
-		return error ? VCHIQ_ERROR : VCHIQ_SUCCESS;
-	}
-	else
-		bcopy(src, dst, size);
-
-	return VCHIQ_SUCCESS;
-}
-
 VCHIQ_STATUS_T
 vchiq_prepare_bulk_data(VCHIQ_BULK_T *bulk, VCHI_MEM_HANDLE_T memhandle,
 	void *offset, int size, int dir)
diff --git a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_arm.c b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_arm.c
--- a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_arm.c
+++ b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_arm.c
@@ -415,6 +415,107 @@
 	}
 }
 
+struct vchiq_io_copy_callback_context {
+	VCHIQ_ELEMENT_T *current_element;
+	size_t current_element_offset;
+	unsigned long elements_to_go;
+	size_t current_offset;
+};
+
+static ssize_t
+vchiq_ioc_copy_element_data(
+	void *context,
+	void *dest,
+	size_t offset,
+	size_t maxsize)
+{
+	long res;
+	size_t bytes_this_round;
+	struct vchiq_io_copy_callback_context *copy_context =
+		(struct vchiq_io_copy_callback_context *)context;
+
+	if (offset != copy_context->current_offset)
+		return 0;
+
+	if (!copy_context->elements_to_go)
+		return 0;
+
+	/*
+	 * Complex logic here to handle the case of 0 size elements
+	 * in the middle of the array of elements.
+	 *
+	 * Need to skip over these 0 size elements.
+	 */
+	while (1) {
+		bytes_this_round = min(copy_context->current_element->size -
+				       copy_context->current_element_offset,
+				       maxsize);
+
+		if (bytes_this_round)
+			break;
+
+		copy_context->elements_to_go--;
+		copy_context->current_element++;
+		copy_context->current_element_offset = 0;
+
+		if (!copy_context->elements_to_go)
+			return 0;
+	}
+
+	res = copy_from_user(dest,
+			     (const uint8_t *)copy_context->current_element->data +
+			     copy_context->current_element_offset,
+			     bytes_this_round);
+
+	if (res != 0)
+		return -EFAULT;
+
+	copy_context->current_element_offset += bytes_this_round;
+	copy_context->current_offset += bytes_this_round;
+
+	/*
+	 * Check if done with current element, and if so advance to the next.
+	 */
+	if (copy_context->current_element_offset ==
+	    copy_context->current_element->size) {
+		copy_context->elements_to_go--;
+		copy_context->current_element++;
+		copy_context->current_element_offset = 0;
+	}
+
+	return bytes_this_round;
+}
+
+/**************************************************************************
+ *
+ *   vchiq_ioc_queue_message
+ *
+ **************************************************************************/
+static VCHIQ_STATUS_T
+vchiq_ioc_queue_message(VCHIQ_SERVICE_HANDLE_T handle,
+			VCHIQ_ELEMENT_T *elements,
+			unsigned long count)
+{
+	struct vchiq_io_copy_callback_context context;
+	unsigned long i;
+	size_t total_size = 0;
+
+	context.current_element = elements;
+	context.current_element_offset = 0;
+	context.elements_to_go = count;
+	context.current_offset = 0;
+
+	for (i = 0; i < count; i++) {
+		if (!elements[i].data && elements[i].size != 0)
+			return -EFAULT;
+
+		total_size += elements[i].size;
+	}
+
+	return vchiq_queue_message(handle, vchiq_ioc_copy_element_data,
+				   &context, total_size);
+}
+
 /****************************************************************************
 *
 *   vchiq_ioctl
@@ -759,7 +860,7 @@
 				args.count * sizeof(VCHIQ_ELEMENT_T));
 )
 			if (cp_ret == 0)
-				status = vchiq_queue_message
+				status = vchiq_ioc_queue_message
 					(args.handle,
 					elements, args.count);
 			else
diff --git a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.h b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.h
--- a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.h
+++ b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.h
@@ -643,9 +643,6 @@
 extern void
 vchiq_complete_bulk(VCHIQ_BULK_T *bulk);
 
-extern VCHIQ_STATUS_T
-vchiq_copy_from_user(void *dst, const void *src, int size);
-
 extern void
 remote_event_signal(REMOTE_EVENT_T *event);
 
diff --git a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.c b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.c
--- a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.c
+++ b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.c
@@ -492,8 +492,8 @@
 ** enough for a header. This relies on header size being a power of two, which
 ** has been verified earlier by a static assertion. */
 
-static inline unsigned int
-calc_stride(unsigned int size)
+static inline size_t
+calc_stride(size_t size)
 {
 	/* Allow room for the header */
 	size += sizeof(VCHIQ_HEADER_T);
@@ -572,7 +572,7 @@
 /* Called from queue_message, by the slot handler and application threads,
 ** with slot_mutex held */
 static VCHIQ_HEADER_T *
-reserve_space(VCHIQ_STATE_T *state, int space, int is_blocking)
+reserve_space(VCHIQ_STATE_T *state, size_t space, int is_blocking)
 {
 	VCHIQ_SHARED_STATE_T *local = state->local;
 	int tx_pos = state->local_tx_pos;
@@ -775,18 +775,66 @@
 	}
 }
 
+static ssize_t
+memcpy_copy_callback(
+	void *context, void *dest,
+	size_t offset, size_t maxsize)
+{
+	void *src = context;
+
+	memcpy((uint8_t *)dest + offset, (uint8_t *)src + offset, maxsize);
+	return maxsize;
+}
+
+static ssize_t
+copy_message_data(
+	ssize_t (*copy_callback)(void *context, void *dest,
+				 size_t offset, size_t maxsize),
+	void *context,
+	void *dest,
+	size_t size)
+{
+	size_t pos = 0;
+
+	while (pos < size) {
+		ssize_t callback_result;
+		size_t max_bytes = size - pos;
+
+		callback_result =
+			copy_callback(context, (uint8_t *)dest + pos,
+				      pos, max_bytes);
+
+		if (callback_result < 0)
+			return callback_result;
+
+		if (!callback_result)
+			return -EIO;
+
+		if (callback_result > max_bytes)
+			return -EIO;
+
+		pos += callback_result;
+	}
+
+	return size;
+}
+
 /* Called by the slot handler and application threads */
 static VCHIQ_STATUS_T
 queue_message(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
-	int msgid, const VCHIQ_ELEMENT_T *elements,
-	int count, int size, int flags)
+	int msgid,
+	ssize_t (*copy_callback)(void *context, void *dest,
+				 size_t offset, size_t maxsize),
+	void *context,
+	size_t size,
+	int flags)
 {
 	VCHIQ_SHARED_STATE_T *local;
 	VCHIQ_SERVICE_QUOTA_T *service_quota = NULL;
 	VCHIQ_HEADER_T *header;
 	int type = VCHIQ_MSG_TYPE(msgid);
 
-	unsigned int stride;
+	size_t stride;
 
 	local = state->local;
 
@@ -851,7 +899,7 @@
 				service_quota->slot_quota))) {
 			spin_unlock(&quota_spinlock);
 			vchiq_log_trace(vchiq_core_log_level,
-				"%d: qm:%d %s,%x - quota stall "
+				"%d: qm:%d %s,%zx - quota stall "
 				"(msg %d, slot %d)",
 				state->id, service->localport,
 				msg_type_str(type), size,
@@ -893,12 +941,12 @@
 	}
 
 	if (type == VCHIQ_MSG_DATA) {
-		int i, pos;
+		ssize_t callback_result;
 		int tx_end_index;
 		int slot_use_count;
 
 		vchiq_log_info(vchiq_core_log_level,
-			"%d: qm %s@%p,%x (%d->%d)",
+			"%d: qm %s@%p,%zx (%d->%d)",
 			state->id,
 			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
 			header, size,
@@ -908,25 +956,23 @@
 		BUG_ON(!service);
 		BUG_ON((flags & (QMFLAGS_NO_MUTEX_LOCK |
 				 QMFLAGS_NO_MUTEX_UNLOCK)) != 0);
-		for (i = 0, pos = 0; i < (unsigned int)count;
-			pos += elements[i++].size)
-			if (elements[i].size) {
-				if (vchiq_copy_from_user
-					(header->data + pos, elements[i].data,
-					(size_t) elements[i].size) !=
-					VCHIQ_SUCCESS) {
-					lmutex_unlock(&state->slot_mutex);
-					VCHIQ_SERVICE_STATS_INC(service,
+
+		callback_result =
+			copy_message_data(copy_callback, context,
+					  header->data, size);
+		if (callback_result < 0) {
+			lmutex_unlock(&state->slot_mutex);
+			VCHIQ_SERVICE_STATS_INC(service,
 						error_count);
-					return VCHIQ_ERROR;
-				}
-			}
+			return VCHIQ_ERROR;
+		}
 
 		if (SRVTRACE_ENABLED(service,
-				VCHIQ_LOG_INFO))
+				     VCHIQ_LOG_INFO))
 			vchiq_log_dump_mem("Sent", 0,
-				header->data,
-				min(16, pos));
+					   header->data,
+					   min((size_t)64,
+					       (size_t)callback_result));
 
 		spin_lock(&quota_spinlock);
 		service_quota->message_use_count++;
@@ -954,7 +1000,7 @@
 
 		if (slot_use_count)
 			vchiq_log_trace(vchiq_core_log_level,
-				"%d: qm:%d %s,%x - slot_use->%d (hdr %p)",
+				"%d: qm:%d %s,%zx - slot_use->%d (hdr %p)",
 				state->id, service->localport,
 				msg_type_str(VCHIQ_MSG_TYPE(msgid)), size,
 				slot_use_count, header);
@@ -963,15 +1009,23 @@
 		VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size);
 	} else {
 		vchiq_log_info(vchiq_core_log_level,
-			"%d: qm %s@%p,%x (%d->%d)", state->id,
+			"%d: qm %s@%p,%zx (%d->%d)", state->id,
 			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
 			header, size,
 			VCHIQ_MSG_SRCPORT(msgid),
 			VCHIQ_MSG_DSTPORT(msgid));
 		if (size != 0) {
-			WARN_ON(!((count == 1) && (size == elements[0].size)));
-			memcpy(header->data, elements[0].data,
-				elements[0].size);
+			/* It is assumed for now that this code path
+			 * only happens from calls inside this file.
+			 *
+			 * External callers are through the vchiq_queue_message
+			 * path which always sets the type to be VCHIQ_MSG_DATA
+			 *
+			 * At first glance this appears to be correct but
+			 * more review is needed.
+			 */
+			copy_message_data(copy_callback, context,
+					  header->data, size);
 		}
 		VCHIQ_STATS_INC(state, ctrl_tx_count);
 	}
@@ -987,7 +1041,7 @@
 			: VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
 
 		vchiq_log_info(SRVTRACE_LEVEL(service),
-			"Sent Msg %s(%u) to %c%c%c%c s:%u d:%d len:%d",
+			"Sent Msg %s(%u) to %c%c%c%c s:%u d:%d len:%zu",
 			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
 			VCHIQ_MSG_TYPE(msgid),
 			VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
@@ -1017,11 +1071,16 @@
 /* Called by the slot handler and application threads */
 static VCHIQ_STATUS_T
 queue_message_sync(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
-	int msgid, const VCHIQ_ELEMENT_T *elements,
-	int count, int size, int is_blocking)
+	int msgid,
+	ssize_t (*copy_callback)(void *context, void *dest,
+				 size_t offset, size_t maxsize),
+	void *context,
+	int size,
+	int is_blocking)
 {
 	VCHIQ_SHARED_STATE_T *local;
 	VCHIQ_HEADER_T *header;
+	ssize_t callback_result;
 
 	local = state->local;
 
@@ -1044,49 +1103,35 @@
 				state->id, oldmsgid);
 	}
 
-	if (service) {
-		int i, pos;
 
-		vchiq_log_info(vchiq_sync_log_level,
-			"%d: qms %s@%p,%x (%d->%d)", state->id,
-			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
-			header, size,
-			VCHIQ_MSG_SRCPORT(msgid),
-			VCHIQ_MSG_DSTPORT(msgid));
+	vchiq_log_info(vchiq_sync_log_level,
+		       "%d: qms %s@%p,%x (%d->%d)", state->id,
+		       msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+		       header, size, VCHIQ_MSG_SRCPORT(msgid),
+		       VCHIQ_MSG_DSTPORT(msgid));
 
-		for (i = 0, pos = 0; i < (unsigned int)count;
-			pos += elements[i++].size)
-			if (elements[i].size) {
-				if (vchiq_copy_from_user
-					(header->data + pos, elements[i].data,
-					(size_t) elements[i].size) !=
-					VCHIQ_SUCCESS) {
-					lmutex_unlock(&state->sync_mutex);
-					VCHIQ_SERVICE_STATS_INC(service,
-						error_count);
-					return VCHIQ_ERROR;
-				}
-			}
+	callback_result =
+		copy_message_data(copy_callback, context,
+				  header->data, size);
+
+	if (callback_result < 0) {
+		lmutex_unlock(&state->sync_mutex);
+		VCHIQ_SERVICE_STATS_INC(service,
+					error_count);
+		return VCHIQ_ERROR;
+	}
 
-		if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE)
-			vchiq_log_dump_mem("Sent Sync",
-				0, header->data,
-				min(16, pos));
+	if (service) {
+		if (SRVTRACE_ENABLED(service,
+				     VCHIQ_LOG_INFO))
+			vchiq_log_dump_mem("Sent Sync", 0,
+					   header->data,
+					   min((size_t)64,
+					       (size_t)callback_result));
 
 		VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count);
 		VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size);
 	} else {
-		vchiq_log_info(vchiq_sync_log_level,
-			"%d: qms %s@%p,%x (%d->%d)", state->id,
-			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
-			header, size,
-			VCHIQ_MSG_SRCPORT(msgid),
-			VCHIQ_MSG_DSTPORT(msgid));
-		if (size != 0) {
-			WARN_ON(!((count == 1) && (size == elements[0].size)));
-			memcpy(header->data, elements[0].data,
-				elements[0].size);
-		}
 		VCHIQ_STATS_INC(state, ctrl_tx_count);
 	}
 
@@ -1196,11 +1241,16 @@
 				VCHIQ_MSG_BULK_RX_DONE : VCHIQ_MSG_BULK_TX_DONE;
 			int msgid = VCHIQ_MAKE_MSG(msgtype, service->localport,
 				service->remoteport);
-			VCHIQ_ELEMENT_T element = { &bulk->actual, 4 };
 			/* Only reply to non-dummy bulk requests */
 			if (bulk->remote_data) {
-				status = queue_message(service->state, NULL,
-					msgid, &element, 1, 4, 0);
+				status = queue_message(
+						service->state,
+						NULL,
+						msgid,
+						memcpy_copy_callback,
+						&bulk->actual,
+						4,
+						0);
 				if (status != VCHIQ_SUCCESS)
 					break;
 			}
@@ -1564,10 +1614,6 @@
 				struct vchiq_openack_payload ack_payload = {
 					service->version
 				};
-				VCHIQ_ELEMENT_T body = {
-					&ack_payload,
-					sizeof(ack_payload)
-				};
 
 				if (state->version_common <
 				    VCHIQ_VERSION_SYNCHRONOUS_MODE)
@@ -1577,21 +1623,28 @@
 				if (service->sync &&
 				    (state->version_common >=
 				     VCHIQ_VERSION_SYNCHRONOUS_MODE)) {
-					if (queue_message_sync(state, NULL,
+					if (queue_message_sync(
+						state,
+						NULL,
 						VCHIQ_MAKE_MSG(
 							VCHIQ_MSG_OPENACK,
 							service->localport,
 							remoteport),
-						&body, 1, sizeof(ack_payload),
+						memcpy_copy_callback,
+						&ack_payload,
+						sizeof(ack_payload),
 						0) == VCHIQ_RETRY)
 						goto bail_not_ready;
 				} else {
-					if (queue_message(state, NULL,
-						VCHIQ_MAKE_MSG(
+					if (queue_message(state,
+							NULL,
+							VCHIQ_MAKE_MSG(
 							VCHIQ_MSG_OPENACK,
 							service->localport,
 							remoteport),
-						&body, 1, sizeof(ack_payload),
+						memcpy_copy_callback,
+						&ack_payload,
+						sizeof(ack_payload),
 						0) == VCHIQ_RETRY)
 						goto bail_not_ready;
 				}
@@ -2722,14 +2775,19 @@
 		service->version,
 		service->version_min
 	};
-	VCHIQ_ELEMENT_T body = { &payload, sizeof(payload) };
 	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
 
 	service->client_id = client_id;
 	vchiq_use_service_internal(service);
-	status = queue_message(service->state, NULL,
-		VCHIQ_MAKE_MSG(VCHIQ_MSG_OPEN, service->localport, 0),
-		&body, 1, sizeof(payload), QMFLAGS_IS_BLOCKING);
+	status = queue_message(service->state,
+			       NULL,
+			       VCHIQ_MAKE_MSG(VCHIQ_MSG_OPEN,
+					      service->localport,
+					      0),
+			       memcpy_copy_callback,
+			       &payload,
+			       sizeof(payload),
+			       QMFLAGS_IS_BLOCKING);
 	if (status == VCHIQ_SUCCESS) {
 		/* Wait for the ACK/NAK */
 		if (down_interruptible(&service->remove_event) != 0) {
@@ -3402,15 +3460,18 @@
 				VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY);
 	} else {
 		uint32_t payload[2] = { (uint32_t)(uintptr_t)bulk->data, bulk->size };
-		VCHIQ_ELEMENT_T element = { payload, sizeof(payload) };
 
-		status = queue_message(state, NULL,
-			VCHIQ_MAKE_MSG(dir_msgtype,
-				service->localport, service->remoteport),
-			&element, 1, sizeof(payload),
-			QMFLAGS_IS_BLOCKING |
-			QMFLAGS_NO_MUTEX_LOCK |
-			QMFLAGS_NO_MUTEX_UNLOCK);
+		status = queue_message(state,
+				       NULL,
+				       VCHIQ_MAKE_MSG(dir_msgtype,
+						      service->localport,
+						      service->remoteport),
+				       memcpy_copy_callback,
+				       &payload,
+				       sizeof(payload),
+				       QMFLAGS_IS_BLOCKING |
+				       QMFLAGS_NO_MUTEX_LOCK |
+				       QMFLAGS_NO_MUTEX_UNLOCK);
 		if (status != VCHIQ_SUCCESS) {
 			goto unlock_both_error_exit;
 		}
@@ -3456,26 +3517,22 @@
 
 VCHIQ_STATUS_T
 vchiq_queue_message(VCHIQ_SERVICE_HANDLE_T handle,
-	const VCHIQ_ELEMENT_T *elements, unsigned int count)
+		    ssize_t (*copy_callback)(void *context, void *dest,
+					     size_t offset, size_t maxsize),
+		    void *context,
+		    size_t size)
 {
 	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
 	VCHIQ_STATUS_T status = VCHIQ_ERROR;
 
-	unsigned int size = 0;
-	unsigned int i;
-
 	if (!service ||
 		(vchiq_check_service(service) != VCHIQ_SUCCESS))
 		goto error_exit;
 
-	for (i = 0; i < (unsigned int)count; i++) {
-		if (elements[i].size) {
-			if (elements[i].data == NULL) {
-				VCHIQ_SERVICE_STATS_INC(service, error_count);
-				goto error_exit;
-			}
-			size += elements[i].size;
-		}
+	if (!size) {
+		VCHIQ_SERVICE_STATS_INC(service, error_count);
+		goto error_exit;
+
 	}
 
 	if (size > VCHIQ_MAX_MSG_SIZE) {
@@ -3489,14 +3546,14 @@
 				VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA,
 					service->localport,
 					service->remoteport),
-				elements, count, size, 1);
+				copy_callback, context, size, 1);
 		break;
 	case VCHIQ_SRVSTATE_OPENSYNC:
 		status = queue_message_sync(service->state, service,
 				VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA,
 					service->localport,
 					service->remoteport),
-				elements, count, size, 1);
+				copy_callback, context, size, 1);
 		break;
 	default:
 		status = VCHIQ_ERROR;
diff --git a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_if.h b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_if.h
--- a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_if.h
+++ b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_if.h
@@ -141,9 +141,12 @@
 extern VCHIQ_STATUS_T vchiq_use_service_no_resume(
 	VCHIQ_SERVICE_HANDLE_T service);
 extern VCHIQ_STATUS_T vchiq_release_service(VCHIQ_SERVICE_HANDLE_T service);
-
-extern VCHIQ_STATUS_T vchiq_queue_message(VCHIQ_SERVICE_HANDLE_T service,
-	const VCHIQ_ELEMENT_T *elements, unsigned int count);
+extern VCHIQ_STATUS_T
+vchiq_queue_message(VCHIQ_SERVICE_HANDLE_T handle,
+		    ssize_t (*copy_callback)(void *context, void *dest,
+					     size_t offset, size_t maxsize),
+		    void *context,
+		    size_t size);
 extern void           vchiq_release_message(VCHIQ_SERVICE_HANDLE_T service,
 	VCHIQ_HEADER_T *header);
 extern VCHIQ_STATUS_T vchiq_queue_bulk_transmit(VCHIQ_SERVICE_HANDLE_T service,
diff --git a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_shim.c b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_shim.c
--- a/sys/contrib/vchiq/interface/vchiq_arm/vchiq_shim.c
+++ b/sys/contrib/vchiq/interface/vchiq_arm/vchiq_shim.c
@@ -146,10 +146,10 @@
  * Name: vchi_msg_queue
  *
  * Arguments:  VCHI_SERVICE_HANDLE_T handle,
- *             const void *data,
- *             uint32_t data_size,
- *             VCHI_FLAGS_T flags,
- *             void *msg_handle,
+ *             ssize_t (*copy_callback)(void *context, void *dest,
+ *				        size_t offset, size_t maxsize),
+ *	       void *context,
+ *             uint32_t data_size
  *
  * Description: Thin wrapper to queue a message onto a connection
  *
@@ -157,28 +157,29 @@
  *
  ***********************************************************/
 int32_t vchi_msg_queue(VCHI_SERVICE_HANDLE_T handle,
-	const void *data,
-	uint32_t data_size,
-	VCHI_FLAGS_T flags,
-	void *msg_handle)
+	ssize_t (*copy_callback)(void *context, void *dest,
+				 size_t offset, size_t maxsize),
+	void *context,
+	uint32_t data_size)
 {
 	SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
-	VCHIQ_ELEMENT_T element = {data, data_size};
 	VCHIQ_STATUS_T status;
 
-	(void)msg_handle;
-
-	WARN_ON(flags != VCHI_FLAGS_BLOCK_UNTIL_QUEUED);
+	while (1) {
+		status = vchiq_queue_message(service->handle,
+					     copy_callback,
+					     context,
+					     data_size);
 
-	status = vchiq_queue_message(service->handle, &element, 1);
+		/*
+		 * vchiq_queue_message() may return VCHIQ_RETRY, so we need to
+		 * implement a retry mechanism since this function is supposed
+		 * to block until queued
+		 */
+		if (status != VCHIQ_RETRY)
+			break;
 
-	/* vchiq_queue_message() may return VCHIQ_RETRY, so we need to
-	** implement a retry mechanism since this function is supposed
-	** to block until queued
-	*/
-	while (status == VCHIQ_RETRY) {
 		msleep(1);
-		status = vchiq_queue_message(service->handle, &element, 1);
 	}
 
 	return vchiq_status_to_vchi(status);
@@ -227,17 +228,18 @@
 		return vchiq_status_to_vchi(VCHIQ_ERROR);
 	}
 
-	status = vchiq_bulk_receive(service->handle, data_dst, data_size,
-		bulk_handle, mode);
-
-	/* vchiq_bulk_receive() may return VCHIQ_RETRY, so we need to
-	** implement a retry mechanism since this function is supposed
-	** to block until queued
-	*/
-	while (status == VCHIQ_RETRY) {
-		msleep(1);
+	while (1) {
 		status = vchiq_bulk_receive(service->handle, data_dst,
 			data_size, bulk_handle, mode);
+		/*
+		 * vchiq_bulk_receive() may return VCHIQ_RETRY, so we need to
+		 * implement a retry mechanism since this function is supposed
+		 * to block until queued
+		 */
+		if (status != VCHIQ_RETRY)
+			break;
+
+		msleep(1);
 	}
 
 	return vchiq_status_to_vchi(status);
@@ -287,17 +289,19 @@
 		return vchiq_status_to_vchi(VCHIQ_ERROR);
 	}
 
-	status = vchiq_bulk_transmit(service->handle, data_src, data_size,
-		bulk_handle, mode);
-
-	/* vchiq_bulk_transmit() may return VCHIQ_RETRY, so we need to
-	** implement a retry mechanism since this function is supposed
-	** to block until queued
-	*/
-	while (status == VCHIQ_RETRY) {
-		msleep(1);
+	while (1) {
 		status = vchiq_bulk_transmit(service->handle, data_src,
 			data_size, bulk_handle, mode);
+
+		/*
+		 * vchiq_bulk_transmit() may return VCHIQ_RETRY, so we need to
+		 * implement a retry mechanism since this function is supposed
+		 * to block until queued
+		 */
+		if (status != VCHIQ_RETRY)
+			break;
+
+		msleep(1);
 	}
 
 	return vchiq_status_to_vchi(status);
@@ -347,44 +351,6 @@
 }
 EXPORT_SYMBOL(vchi_msg_dequeue);
 
-/***********************************************************
- * Name: vchi_msg_queuev
- *
- * Arguments:  VCHI_SERVICE_HANDLE_T handle,
- *             VCHI_MSG_VECTOR_T *vector,
- *             uint32_t count,
- *             VCHI_FLAGS_T flags,
- *             void *msg_handle
- *
- * Description: Thin wrapper to queue a message onto a connection
- *
- * Returns: int32_t - success == 0
- *
- ***********************************************************/
-
-vchiq_static_assert(sizeof(VCHI_MSG_VECTOR_T) == sizeof(VCHIQ_ELEMENT_T));
-vchiq_static_assert(offsetof(VCHI_MSG_VECTOR_T, vec_base) ==
-	offsetof(VCHIQ_ELEMENT_T, data));
-vchiq_static_assert(offsetof(VCHI_MSG_VECTOR_T, vec_len) ==
-	offsetof(VCHIQ_ELEMENT_T, size));
-
-int32_t vchi_msg_queuev(VCHI_SERVICE_HANDLE_T handle,
-	VCHI_MSG_VECTOR_T *vector,
-	uint32_t count,
-	VCHI_FLAGS_T flags,
-	void *msg_handle)
-{
-	SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
-
-	(void)msg_handle;
-
-	WARN_ON(flags != VCHI_FLAGS_BLOCK_UNTIL_QUEUED);
-
-	return vchiq_status_to_vchi(vchiq_queue_message(service->handle,
-		(const VCHIQ_ELEMENT_T *)vector, count));
-}
-EXPORT_SYMBOL(vchi_msg_queuev);
-
 /***********************************************************
  * Name: vchi_held_msg_release
  *

