Merge tag 'staging-4.9-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/staging

Pull staging and IIO updates from Greg KH:
 "Here is the big staging and IIO driver pull request for 4.9-rc1.

  There are a lot of patches in here, the majority due to the
  drivers/staging/greybus/ subsystem being merged in with full
  development history that went back a few years, in order to preserve
  the work that those developers did over time.

  Lots and lots of tiny cleanups happened in the tree as well, due to
  the Outreachy application process and lots of other developers showing
  up for the first time to clean code up.  Along with those changes, we
  deleted a wireless driver, and added a raspberrypi driver (currently
  marked broken), and lots of new iio drivers.

  Overall the tree still shrunk with more lines removed than added,
  about 10 thousand lines removed in total. Full details are in the very
  long shortlog below.

  All of this has been in the linux-next tree with no issues. There will
  be some merge problems with other subsystem trees, but those are all
  minor problems and shouldn't be hard to work out when they happen
  (MAINTAINERS and some lustre build problems with the IB tree)"

And furter from me asking for clarification about greybus:
 "Right now there is a phone from Motorola shipping with this code (a
  slightly older version, but the same tree), so even though Ara is not
  alive in the same form, the functionality is happening. We are working
  with the developers of that phone to merge the newer stuff in with
  their fork so they can use the upstream version in future versions of
  their phone product line.

  Toshiba has at least one chip shipping in their catalog that
  needs/uses this protocol over a Unipro link, and rumor has it that
  there might be more in the future.

  There are also other users of the greybus protocols, there is a talk
  next week at ELC that shows how it is being used across a network
  connection to control a device, and previous ELC talks have showed the
  protocol stack being used over USB to drive embedded Linux boards.
  I've also talked to some people who are starting to work to add a host
  controller driver to control arduinos as the greybus PHY protocols are
  very useful to control a serial/i2c/spio/whatever device across a
  random physical link, as it is a way to have a self-describing device
  be attached to a host without needing manual configuration.

  So yes, people are using it, and there is still the chance that it
  will show up in a phone/laptop/tablet/whatever from Google in the
  future as well, the tech isn't dead, even if the original large phone
  project happens to be"

* tag 'staging-4.9-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/staging: (3703 commits)
  Staging: fbtft: Fix bug in fbtft-core
  staging: rtl8188eu: fix double unlock error in rtw_resume_process()
  staging:r8188eu: remove GEN_MLME_EXT_HANDLER macro
  staging:r8188eu: remove GEN_DRV_CMD_HANDLER macro
  staging:r8188eu: remove GEN_EVT_CODE macro
  staging:r8188eu: remove GEN_CMD_CODE macro
  staging:r8188eu: remove pkt_newalloc member of the recv_buf structure
  staging:r8188eu: remove rtw_handle_dualmac declaration
  staging:r8188eu: remove (RGTRY|BSSID)_(OFT|SZ) macros
  staging:r8188eu: change rtl8188e_process_phy_info function argument type
  Staging: fsl-mc: Remove blank lines
  Staging: fsl-mc: Fix unaligned * in block comments
  Staging: comedi: Align the * in block comments
  Staging : ks7010 : Fix block comments warninig
  Staging: vt6655: Remove explicit NULL comparison using Coccinelle
  staging: rtl8188eu: core: rtw_xmit: Use macros instead of constants
  staging: rtl8188eu: core: rtw_xmit: Move constant of the right side
  staging: dgnc: Fix lines longer than 80 characters
  Staging: dgnc: constify attribute_group structures
  Staging: most: hdm-dim2: constify attribute_group structures
  ...

1 files changed, 3934 insertions, 0 deletions

diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
new file mode 100644
index 0000000..2c98da4
--- /dev/null
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
@@ -0,0 +1,3934 @@
+/**
+ * Copyright (c) 2010-2012 Broadcom. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2, as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "vchiq_core.h"
+#include "vchiq_killable.h"
+
+#define VCHIQ_SLOT_HANDLER_STACK 8192
+
+#define HANDLE_STATE_SHIFT 12
+
+#define SLOT_INFO_FROM_INDEX(state, index) (state->slot_info + (index))
+#define SLOT_DATA_FROM_INDEX(state, index) (state->slot_data + (index))
+#define SLOT_INDEX_FROM_DATA(state, data) \
+	(((unsigned int)((char *)data - (char *)state->slot_data)) / \
+	VCHIQ_SLOT_SIZE)
+#define SLOT_INDEX_FROM_INFO(state, info) \
+	((unsigned int)(info - state->slot_info))
+#define SLOT_QUEUE_INDEX_FROM_POS(pos) \
+	((int)((unsigned int)(pos) / VCHIQ_SLOT_SIZE))
+
+#define BULK_INDEX(x) (x & (VCHIQ_NUM_SERVICE_BULKS - 1))
+
+#define SRVTRACE_LEVEL(srv) \
+	(((srv) && (srv)->trace) ? VCHIQ_LOG_TRACE : vchiq_core_msg_log_level)
+#define SRVTRACE_ENABLED(srv, lev) \
+	(((srv) && (srv)->trace) || (vchiq_core_msg_log_level >= (lev)))
+
+struct vchiq_open_payload {
+	int fourcc;
+	int client_id;
+	short version;
+	short version_min;
+};
+
+struct vchiq_openack_payload {
+	short version;
+};
+
+enum
+{
+	QMFLAGS_IS_BLOCKING     = (1 << 0),
+	QMFLAGS_NO_MUTEX_LOCK   = (1 << 1),
+	QMFLAGS_NO_MUTEX_UNLOCK = (1 << 2)
+};
+
+/* we require this for consistency between endpoints */
+vchiq_static_assert(sizeof(VCHIQ_HEADER_T) == 8);
+vchiq_static_assert(IS_POW2(sizeof(VCHIQ_HEADER_T)));
+vchiq_static_assert(IS_POW2(VCHIQ_NUM_CURRENT_BULKS));
+vchiq_static_assert(IS_POW2(VCHIQ_NUM_SERVICE_BULKS));
+vchiq_static_assert(IS_POW2(VCHIQ_MAX_SERVICES));
+vchiq_static_assert(VCHIQ_VERSION >= VCHIQ_VERSION_MIN);
+
+/* Run time control of log level, based on KERN_XXX level. */
+int vchiq_core_log_level = VCHIQ_LOG_DEFAULT;
+int vchiq_core_msg_log_level = VCHIQ_LOG_DEFAULT;
+int vchiq_sync_log_level = VCHIQ_LOG_DEFAULT;
+
+static atomic_t pause_bulks_count = ATOMIC_INIT(0);
+
+static DEFINE_SPINLOCK(service_spinlock);
+DEFINE_SPINLOCK(bulk_waiter_spinlock);
+DEFINE_SPINLOCK(quota_spinlock);
+
+VCHIQ_STATE_T *vchiq_states[VCHIQ_MAX_STATES];
+static unsigned int handle_seq;
+
+static const char *const srvstate_names[] = {
+	"FREE",
+	"HIDDEN",
+	"LISTENING",
+	"OPENING",
+	"OPEN",
+	"OPENSYNC",
+	"CLOSESENT",
+	"CLOSERECVD",
+	"CLOSEWAIT",
+	"CLOSED"
+};
+
+static const char *const reason_names[] = {
+	"SERVICE_OPENED",
+	"SERVICE_CLOSED",
+	"MESSAGE_AVAILABLE",
+	"BULK_TRANSMIT_DONE",
+	"BULK_RECEIVE_DONE",
+	"BULK_TRANSMIT_ABORTED",
+	"BULK_RECEIVE_ABORTED"
+};
+
+static const char *const conn_state_names[] = {
+	"DISCONNECTED",
+	"CONNECTING",
+	"CONNECTED",
+	"PAUSING",
+	"PAUSE_SENT",
+	"PAUSED",
+	"RESUMING",
+	"PAUSE_TIMEOUT",
+	"RESUME_TIMEOUT"
+};
+
+
+static void
+release_message_sync(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header);
+
+static const char *msg_type_str(unsigned int msg_type)
+{
+	switch (msg_type) {
+	case VCHIQ_MSG_PADDING:       return "PADDING";
+	case VCHIQ_MSG_CONNECT:       return "CONNECT";
+	case VCHIQ_MSG_OPEN:          return "OPEN";
+	case VCHIQ_MSG_OPENACK:       return "OPENACK";
+	case VCHIQ_MSG_CLOSE:         return "CLOSE";
+	case VCHIQ_MSG_DATA:          return "DATA";
+	case VCHIQ_MSG_BULK_RX:       return "BULK_RX";
+	case VCHIQ_MSG_BULK_TX:       return "BULK_TX";
+	case VCHIQ_MSG_BULK_RX_DONE:  return "BULK_RX_DONE";
+	case VCHIQ_MSG_BULK_TX_DONE:  return "BULK_TX_DONE";
+	case VCHIQ_MSG_PAUSE:         return "PAUSE";
+	case VCHIQ_MSG_RESUME:        return "RESUME";
+	case VCHIQ_MSG_REMOTE_USE:    return "REMOTE_USE";
+	case VCHIQ_MSG_REMOTE_RELEASE:      return "REMOTE_RELEASE";
+	case VCHIQ_MSG_REMOTE_USE_ACTIVE:   return "REMOTE_USE_ACTIVE";
+	}
+	return "???";
+}
+
+static inline void
+vchiq_set_service_state(VCHIQ_SERVICE_T *service, int newstate)
+{
+	vchiq_log_info(vchiq_core_log_level, "%d: srv:%d %s->%s",
+		service->state->id, service->localport,
+		srvstate_names[service->srvstate],
+		srvstate_names[newstate]);
+	service->srvstate = newstate;
+}
+
+VCHIQ_SERVICE_T *
+find_service_by_handle(VCHIQ_SERVICE_HANDLE_T handle)
+{
+	VCHIQ_SERVICE_T *service;
+
+	spin_lock(&service_spinlock);
+	service = handle_to_service(handle);
+	if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE) &&
+		(service->handle == handle)) {
+		BUG_ON(service->ref_count == 0);
+		service->ref_count++;
+	} else
+		service = NULL;
+	spin_unlock(&service_spinlock);
+
+	if (!service)
+		vchiq_log_info(vchiq_core_log_level,
+			"Invalid service handle 0x%x", handle);
+
+	return service;
+}
+
+VCHIQ_SERVICE_T *
+find_service_by_port(VCHIQ_STATE_T *state, int localport)
+{
+	VCHIQ_SERVICE_T *service = NULL;
+	if ((unsigned int)localport <= VCHIQ_PORT_MAX) {
+		spin_lock(&service_spinlock);
+		service = state->services[localport];
+		if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE)) {
+			BUG_ON(service->ref_count == 0);
+			service->ref_count++;
+		} else
+			service = NULL;
+		spin_unlock(&service_spinlock);
+	}
+
+	if (!service)
+		vchiq_log_info(vchiq_core_log_level,
+			"Invalid port %d", localport);
+
+	return service;
+}
+
+VCHIQ_SERVICE_T *
+find_service_for_instance(VCHIQ_INSTANCE_T instance,
+	VCHIQ_SERVICE_HANDLE_T handle) {
+	VCHIQ_SERVICE_T *service;
+
+	spin_lock(&service_spinlock);
+	service = handle_to_service(handle);
+	if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE) &&
+		(service->handle == handle) &&
+		(service->instance == instance)) {
+		BUG_ON(service->ref_count == 0);
+		service->ref_count++;
+	} else
+		service = NULL;
+	spin_unlock(&service_spinlock);
+
+	if (!service)
+		vchiq_log_info(vchiq_core_log_level,
+			"Invalid service handle 0x%x", handle);
+
+	return service;
+}
+
+VCHIQ_SERVICE_T *
+find_closed_service_for_instance(VCHIQ_INSTANCE_T instance,
+	VCHIQ_SERVICE_HANDLE_T handle) {
+	VCHIQ_SERVICE_T *service;
+
+	spin_lock(&service_spinlock);
+	service = handle_to_service(handle);
+	if (service &&
+		((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
+		 (service->srvstate == VCHIQ_SRVSTATE_CLOSED)) &&
+		(service->handle == handle) &&
+		(service->instance == instance)) {
+		BUG_ON(service->ref_count == 0);
+		service->ref_count++;
+	} else
+		service = NULL;
+	spin_unlock(&service_spinlock);
+
+	if (!service)
+		vchiq_log_info(vchiq_core_log_level,
+			"Invalid service handle 0x%x", handle);
+
+	return service;
+}
+
+VCHIQ_SERVICE_T *
+next_service_by_instance(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance,
+	int *pidx)
+{
+	VCHIQ_SERVICE_T *service = NULL;
+	int idx = *pidx;
+
+	spin_lock(&service_spinlock);
+	while (idx < state->unused_service) {
+		VCHIQ_SERVICE_T *srv = state->services[idx++];
+		if (srv && (srv->srvstate != VCHIQ_SRVSTATE_FREE) &&
+			(srv->instance == instance)) {
+			service = srv;
+			BUG_ON(service->ref_count == 0);
+			service->ref_count++;
+			break;
+		}
+	}
+	spin_unlock(&service_spinlock);
+
+	*pidx = idx;
+
+	return service;
+}
+
+void
+lock_service(VCHIQ_SERVICE_T *service)
+{
+	spin_lock(&service_spinlock);
+	BUG_ON(!service || (service->ref_count == 0));
+	if (service)
+		service->ref_count++;
+	spin_unlock(&service_spinlock);
+}
+
+void
+unlock_service(VCHIQ_SERVICE_T *service)
+{
+	VCHIQ_STATE_T *state = service->state;
+	spin_lock(&service_spinlock);
+	BUG_ON(!service || (service->ref_count == 0));
+	if (service && service->ref_count) {
+		service->ref_count--;
+		if (!service->ref_count) {
+			BUG_ON(service->srvstate != VCHIQ_SRVSTATE_FREE);
+			state->services[service->localport] = NULL;
+		} else
+			service = NULL;
+	}
+	spin_unlock(&service_spinlock);
+
+	if (service && service->userdata_term)
+		service->userdata_term(service->base.userdata);
+
+	kfree(service);
+}
+
+int
+vchiq_get_client_id(VCHIQ_SERVICE_HANDLE_T handle)
+{
+	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+	int id;
+
+	id = service ? service->client_id : 0;
+	if (service)
+		unlock_service(service);
+
+	return id;
+}
+
+void *
+vchiq_get_service_userdata(VCHIQ_SERVICE_HANDLE_T handle)
+{
+	VCHIQ_SERVICE_T *service = handle_to_service(handle);
+
+	return service ? service->base.userdata : NULL;
+}
+
+int
+vchiq_get_service_fourcc(VCHIQ_SERVICE_HANDLE_T handle)
+{
+	VCHIQ_SERVICE_T *service = handle_to_service(handle);
+
+	return service ? service->base.fourcc : 0;
+}
+
+static void
+mark_service_closing_internal(VCHIQ_SERVICE_T *service, int sh_thread)
+{
+	VCHIQ_STATE_T *state = service->state;
+	VCHIQ_SERVICE_QUOTA_T *service_quota;
+
+	service->closing = 1;
+
+	/* Synchronise with other threads. */
+	mutex_lock(&state->recycle_mutex);
+	mutex_unlock(&state->recycle_mutex);
+	if (!sh_thread || (state->conn_state != VCHIQ_CONNSTATE_PAUSE_SENT)) {
+		/* If we're pausing then the slot_mutex is held until resume
+		 * by the slot handler.  Therefore don't try to acquire this
+		 * mutex if we're the slot handler and in the pause sent state.
+		 * We don't need to in this case anyway. */
+		mutex_lock(&state->slot_mutex);
+		mutex_unlock(&state->slot_mutex);
+	}
+
+	/* Unblock any sending thread. */
+	service_quota = &state->service_quotas[service->localport];
+	up(&service_quota->quota_event);
+}
+
+static void
+mark_service_closing(VCHIQ_SERVICE_T *service)
+{
+	mark_service_closing_internal(service, 0);
+}
+
+static inline VCHIQ_STATUS_T
+make_service_callback(VCHIQ_SERVICE_T *service, VCHIQ_REASON_T reason,
+	VCHIQ_HEADER_T *header, void *bulk_userdata)
+{
+	VCHIQ_STATUS_T status;
+	vchiq_log_trace(vchiq_core_log_level, "%d: callback:%d (%s, %x, %x)",
+		service->state->id, service->localport, reason_names[reason],
+		(unsigned int)header, (unsigned int)bulk_userdata);
+	status = service->base.callback(reason, header, service->handle,
+		bulk_userdata);
+	if (status == VCHIQ_ERROR) {
+		vchiq_log_warning(vchiq_core_log_level,
+			"%d: ignoring ERROR from callback to service %x",
+			service->state->id, service->handle);
+		status = VCHIQ_SUCCESS;
+	}
+	return status;
+}
+
+inline void
+vchiq_set_conn_state(VCHIQ_STATE_T *state, VCHIQ_CONNSTATE_T newstate)
+{
+	VCHIQ_CONNSTATE_T oldstate = state->conn_state;
+	vchiq_log_info(vchiq_core_log_level, "%d: %s->%s", state->id,
+		conn_state_names[oldstate],
+		conn_state_names[newstate]);
+	state->conn_state = newstate;
+	vchiq_platform_conn_state_changed(state, oldstate, newstate);
+}
+
+static inline void
+remote_event_create(REMOTE_EVENT_T *event)
+{
+	event->armed = 0;
+	/* Don't clear the 'fired' flag because it may already have been set
+	** by the other side. */
+	sema_init(event->event, 0);
+}
+
+static inline void
+remote_event_destroy(REMOTE_EVENT_T *event)
+{
+	(void)event;
+}
+
+static inline int
+remote_event_wait(REMOTE_EVENT_T *event)
+{
+	if (!event->fired) {
+		event->armed = 1;
+		dsb();
+		if (!event->fired) {
+			if (down_interruptible(event->event) != 0) {
+				event->armed = 0;
+				return 0;
+			}
+		}
+		event->armed = 0;
+		wmb();
+	}
+
+	event->fired = 0;
+	return 1;
+}
+
+static inline void
+remote_event_signal_local(REMOTE_EVENT_T *event)
+{
+	event->armed = 0;
+	up(event->event);
+}
+
+static inline void
+remote_event_poll(REMOTE_EVENT_T *event)
+{
+	if (event->fired && event->armed)
+		remote_event_signal_local(event);
+}
+
+void
+remote_event_pollall(VCHIQ_STATE_T *state)
+{
+	remote_event_poll(&state->local->sync_trigger);
+	remote_event_poll(&state->local->sync_release);
+	remote_event_poll(&state->local->trigger);
+	remote_event_poll(&state->local->recycle);
+}
+
+/* Round up message sizes so that any space at the end of a slot is always big
+** 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)
+{
+	/* Allow room for the header */
+	size += sizeof(VCHIQ_HEADER_T);
+
+	/* Round up */
+	return (size + sizeof(VCHIQ_HEADER_T) - 1) & ~(sizeof(VCHIQ_HEADER_T)
+		- 1);
+}
+
+/* Called by the slot handler thread */
+static VCHIQ_SERVICE_T *
+get_listening_service(VCHIQ_STATE_T *state, int fourcc)
+{
+	int i;
+
+	WARN_ON(fourcc == VCHIQ_FOURCC_INVALID);
+
+	for (i = 0; i < state->unused_service; i++) {
+		VCHIQ_SERVICE_T *service = state->services[i];
+		if (service &&
+			(service->public_fourcc == fourcc) &&
+			((service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
+			((service->srvstate == VCHIQ_SRVSTATE_OPEN) &&
+			(service->remoteport == VCHIQ_PORT_FREE)))) {
+			lock_service(service);
+			return service;
+		}
+	}
+
+	return NULL;
+}
+
+/* Called by the slot handler thread */
+static VCHIQ_SERVICE_T *
+get_connected_service(VCHIQ_STATE_T *state, unsigned int port)
+{
+	int i;
+	for (i = 0; i < state->unused_service; i++) {
+		VCHIQ_SERVICE_T *service = state->services[i];
+		if (service && (service->srvstate == VCHIQ_SRVSTATE_OPEN)
+			&& (service->remoteport == port)) {
+			lock_service(service);
+			return service;
+		}
+	}
+	return NULL;
+}
+
+inline void
+request_poll(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service, int poll_type)
+{
+	uint32_t value;
+
+	if (service) {
+		do {
+			value = atomic_read(&service->poll_flags);
+		} while (atomic_cmpxchg(&service->poll_flags, value,
+			value | (1 << poll_type)) != value);
+
+		do {
+			value = atomic_read(&state->poll_services[
+				service->localport>>5]);
+		} while (atomic_cmpxchg(
+			&state->poll_services[service->localport>>5],
+			value, value | (1 << (service->localport & 0x1f)))
+			!= value);
+	}
+
+	state->poll_needed = 1;
+	wmb();
+
+	/* ... and ensure the slot handler runs. */
+	remote_event_signal_local(&state->local->trigger);
+}
+
+/* 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)
+{
+	VCHIQ_SHARED_STATE_T *local = state->local;
+	int tx_pos = state->local_tx_pos;
+	int slot_space = VCHIQ_SLOT_SIZE - (tx_pos & VCHIQ_SLOT_MASK);
+
+	if (space > slot_space) {
+		VCHIQ_HEADER_T *header;
+		/* Fill the remaining space with padding */
+		WARN_ON(state->tx_data == NULL);
+		header = (VCHIQ_HEADER_T *)
+			(state->tx_data + (tx_pos & VCHIQ_SLOT_MASK));
+		header->msgid = VCHIQ_MSGID_PADDING;
+		header->size = slot_space - sizeof(VCHIQ_HEADER_T);
+
+		tx_pos += slot_space;
+	}
+
+	/* If necessary, get the next slot. */
+	if ((tx_pos & VCHIQ_SLOT_MASK) == 0) {
+		int slot_index;
+
+		/* If there is no free slot... */
+
+		if (down_trylock(&state->slot_available_event) != 0) {
+			/* ...wait for one. */
+
+			VCHIQ_STATS_INC(state, slot_stalls);
+
+			/* But first, flush through the last slot. */
+			state->local_tx_pos = tx_pos;
+			local->tx_pos = tx_pos;
+			remote_event_signal(&state->remote->trigger);
+
+			if (!is_blocking ||
+				(down_interruptible(
+				&state->slot_available_event) != 0))
+				return NULL; /* No space available */
+		}
+
+		BUG_ON(tx_pos ==
+			(state->slot_queue_available * VCHIQ_SLOT_SIZE));
+
+		slot_index = local->slot_queue[
+			SLOT_QUEUE_INDEX_FROM_POS(tx_pos) &
+			VCHIQ_SLOT_QUEUE_MASK];
+		state->tx_data =
+			(char *)SLOT_DATA_FROM_INDEX(state, slot_index);
+	}
+
+	state->local_tx_pos = tx_pos + space;
+
+	return (VCHIQ_HEADER_T *)(state->tx_data + (tx_pos & VCHIQ_SLOT_MASK));
+}
+
+/* Called by the recycle thread. */
+static void
+process_free_queue(VCHIQ_STATE_T *state)
+{
+	VCHIQ_SHARED_STATE_T *local = state->local;
+	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;
+
+	while (slot_queue_available != local->slot_queue_recycle) {
+		unsigned int pos;
+		int slot_index = local->slot_queue[slot_queue_available++ &
+			VCHIQ_SLOT_QUEUE_MASK];
+		char *data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index);
+		int data_found = 0;
+
+		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);
+
+		/* Initialise the bitmask for services which have used this
+		** slot */
+		BITSET_ZERO(service_found);
+
+		pos = 0;
+
+		while (pos < VCHIQ_SLOT_SIZE) {
+			VCHIQ_HEADER_T *header =
+				(VCHIQ_HEADER_T *)(data + pos);
+			int msgid = header->msgid;
+			if (VCHIQ_MSG_TYPE(msgid) == VCHIQ_MSG_DATA) {
+				int port = VCHIQ_MSG_SRCPORT(msgid);
+				VCHIQ_SERVICE_QUOTA_T *service_quota =
+					&state->service_quotas[port];
+				int count;
+				spin_lock(&quota_spinlock);
+				count = service_quota->message_use_count;
+				if (count > 0)
+					service_quota->message_use_count =
+						count - 1;
+				spin_unlock(&quota_spinlock);
+
+				if (count == service_quota->message_quota)
+					/* Signal the service that it
+					** has dropped below its quota
+					*/
+					up(&service_quota->quota_event);
+				else if (count == 0) {
+					vchiq_log_error(vchiq_core_log_level,
+						"service %d "
+						"message_use_count=%d "
+						"(header %x, msgid %x, "
+						"header->msgid %x, "
+						"header->size %x)",
+						port,
+						service_quota->
+							message_use_count,
+						(unsigned int)header, msgid,
+						header->msgid,
+						header->size);
+					WARN(1, "invalid message use count\n");
+				}
+				if (!BITSET_IS_SET(service_found, port)) {
+					/* Set the found bit for this service */
+					BITSET_SET(service_found, port);
+
+					spin_lock(&quota_spinlock);
+					count = service_quota->slot_use_count;
+					if (count > 0)
+						service_quota->slot_use_count =
+							count - 1;
+					spin_unlock(&quota_spinlock);
+
+					if (count > 0) {
+						/* Signal the service in case
+						** it has dropped below its
+						** quota */
+						up(&service_quota->quota_event);
+						vchiq_log_trace(
+							vchiq_core_log_level,
+							"%d: pfq:%d %x@%x - "
+							"slot_use->%d",
+							state->id, port,
+							header->size,
+							(unsigned int)header,
+							count - 1);
+					} else {
+						vchiq_log_error(
+							vchiq_core_log_level,
+								"service %d "
+								"slot_use_count"
+								"=%d (header %x"
+								", msgid %x, "
+								"header->msgid"
+								" %x, header->"
+								"size %x)",
+							port, count,
+							(unsigned int)header,
+							msgid,
+							header->msgid,
+							header->size);
+						WARN(1, "bad slot use count\n");
+					}
+				}
+
+				data_found = 1;
+			}
+
+			pos += calc_stride(header->size);
+			if (pos > VCHIQ_SLOT_SIZE) {
+				vchiq_log_error(vchiq_core_log_level,
+					"pfq - pos %x: header %x, msgid %x, "
+					"header->msgid %x, header->size %x",
+					pos, (unsigned int)header, msgid,
+					header->msgid, header->size);
+				WARN(1, "invalid slot position\n");
+			}
+		}
+
+		if (data_found) {
+			int count;
+			spin_lock(&quota_spinlock);
+			count = state->data_use_count;
+			if (count > 0)
+				state->data_use_count =
+					count - 1;
+			spin_unlock(&quota_spinlock);
+			if (count == state->data_quota)
+				up(&state->data_quota_event);
+		}
+
+		state->slot_queue_available = slot_queue_available;
+		up(&state->slot_available_event);
+	}
+}
+
+/* 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)
+{
+	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;
+
+	local = state->local;
+
+	stride = calc_stride(size);
+
+	WARN_ON(!(stride <= VCHIQ_SLOT_SIZE));
+
+	if (!(flags & QMFLAGS_NO_MUTEX_LOCK) &&
+		(mutex_lock_interruptible(&state->slot_mutex) != 0))
+		return VCHIQ_RETRY;
+
+	if (type == VCHIQ_MSG_DATA) {
+		int tx_end_index;
+
+		BUG_ON(!service);
+		BUG_ON((flags & (QMFLAGS_NO_MUTEX_LOCK |
+				 QMFLAGS_NO_MUTEX_UNLOCK)) != 0);
+
+		if (service->closing) {
+			/* The service has been closed */
+			mutex_unlock(&state->slot_mutex);
+			return VCHIQ_ERROR;
+		}
+
+		service_quota = &state->service_quotas[service->localport];
+
+		spin_lock(&quota_spinlock);
+
+		/* Ensure this service doesn't use more than its quota of
+		** messages or slots */
+		tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(
+			state->local_tx_pos + stride - 1);
+
+		/* Ensure data messages don't use more than their quota of
+		** slots */
+		while ((tx_end_index != state->previous_data_index) &&
+			(state->data_use_count == state->data_quota)) {
+			VCHIQ_STATS_INC(state, data_stalls);
+			spin_unlock(&quota_spinlock);
+			mutex_unlock(&state->slot_mutex);
+
+			if (down_interruptible(&state->data_quota_event)
+				!= 0)
+				return VCHIQ_RETRY;
+
+			mutex_lock(&state->slot_mutex);
+			spin_lock(&quota_spinlock);
+			tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(
+				state->local_tx_pos + stride - 1);
+			if ((tx_end_index == state->previous_data_index) ||
+				(state->data_use_count < state->data_quota)) {
+				/* Pass the signal on to other waiters */
+				up(&state->data_quota_event);
+				break;
+			}
+		}
+
+		while ((service_quota->message_use_count ==
+				service_quota->message_quota) ||
+			((tx_end_index != service_quota->previous_tx_index) &&
+			(service_quota->slot_use_count ==
+				service_quota->slot_quota))) {
+			spin_unlock(&quota_spinlock);
+			vchiq_log_trace(vchiq_core_log_level,
+				"%d: qm:%d %s,%x - quota stall "
+				"(msg %d, slot %d)",
+				state->id, service->localport,
+				msg_type_str(type), size,
+				service_quota->message_use_count,
+				service_quota->slot_use_count);
+			VCHIQ_SERVICE_STATS_INC(service, quota_stalls);
+			mutex_unlock(&state->slot_mutex);
+			if (down_interruptible(&service_quota->quota_event)
+				!= 0)
+				return VCHIQ_RETRY;
+			if (service->closing)
+				return VCHIQ_ERROR;
+			if (mutex_lock_interruptible(&state->slot_mutex) != 0)
+				return VCHIQ_RETRY;
+			if (service->srvstate != VCHIQ_SRVSTATE_OPEN) {
+				/* The service has been closed */
+				mutex_unlock(&state->slot_mutex);
+				return VCHIQ_ERROR;
+			}
+			spin_lock(&quota_spinlock);
+			tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(
+				state->local_tx_pos + stride - 1);
+		}
+
+		spin_unlock(&quota_spinlock);
+	}
+
+	header = reserve_space(state, stride, flags & QMFLAGS_IS_BLOCKING);
+
+	if (!header) {
+		if (service)
+			VCHIQ_SERVICE_STATS_INC(service, slot_stalls);
+		/* In the event of a failure, return the mutex to the
+		   state it was in */
+		if (!(flags & QMFLAGS_NO_MUTEX_LOCK))
+			mutex_unlock(&state->slot_mutex);
+		return VCHIQ_RETRY;
+	}
+
+	if (type == VCHIQ_MSG_DATA) {
+		int i, pos;
+		int tx_end_index;
+		int slot_use_count;
+
+		vchiq_log_info(vchiq_core_log_level,
+			"%d: qm %s@%x,%x (%d->%d)",
+			state->id,
+			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+			(unsigned int)header, size,
+			VCHIQ_MSG_SRCPORT(msgid),
+			VCHIQ_MSG_DSTPORT(msgid));
+
+		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) {
+					mutex_unlock(&state->slot_mutex);
+					VCHIQ_SERVICE_STATS_INC(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));
+				}
+			}
+
+		spin_lock(&quota_spinlock);
+		service_quota->message_use_count++;
+
+		tx_end_index =
+			SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos - 1);
+
+		/* If this transmission can't fit in the last slot used by any
+		** service, the data_use_count must be increased. */
+		if (tx_end_index != state->previous_data_index) {
+			state->previous_data_index = tx_end_index;
+			state->data_use_count++;
+		}
+
+		/* If this isn't the same slot last used by this service,
+		** the service's slot_use_count must be increased. */
+		if (tx_end_index != service_quota->previous_tx_index) {
+			service_quota->previous_tx_index = tx_end_index;
+			slot_use_count = ++service_quota->slot_use_count;
+		} else {
+			slot_use_count = 0;
+		}
+
+		spin_unlock(&quota_spinlock);
+
+		if (slot_use_count)
+			vchiq_log_trace(vchiq_core_log_level,
+				"%d: qm:%d %s,%x - slot_use->%d (hdr %p)",
+				state->id, service->localport,
+				msg_type_str(VCHIQ_MSG_TYPE(msgid)), size,
+				slot_use_count, header);
+
+		VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count);
+		VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size);
+	} else {
+		vchiq_log_info(vchiq_core_log_level,
+			"%d: qm %s@%x,%x (%d->%d)", state->id,
+			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+			(unsigned int)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);
+	}
+
+	header->msgid = msgid;
+	header->size = size;
+
+	{
+		int svc_fourcc;
+
+		svc_fourcc = service
+			? service->base.fourcc
+			: VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
+
+		vchiq_log_info(SRVTRACE_LEVEL(service),
+			"Sent Msg %s(%u) to %c%c%c%c s:%u d:%d len:%d",
+			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+			VCHIQ_MSG_TYPE(msgid),
+			VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
+			VCHIQ_MSG_SRCPORT(msgid),
+			VCHIQ_MSG_DSTPORT(msgid),
+			size);
+	}
+
+	/* Make sure the new header is visible to the peer. */
+	wmb();
+
+	/* Make the new tx_pos visible to the peer. */
+	local->tx_pos = state->local_tx_pos;
+	wmb();
+
+	if (service && (type == VCHIQ_MSG_CLOSE))
+		vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT);
+
+	if (!(flags & QMFLAGS_NO_MUTEX_UNLOCK))
+		mutex_unlock(&state->slot_mutex);
+
+	remote_event_signal(&state->remote->trigger);
+
+	return VCHIQ_SUCCESS;
+}
+
+/* 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)
+{
+	VCHIQ_SHARED_STATE_T *local;
+	VCHIQ_HEADER_T *header;
+
+	local = state->local;
+
+	if ((VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_RESUME) &&
+		(mutex_lock_interruptible(&state->sync_mutex) != 0))
+		return VCHIQ_RETRY;
+
+	remote_event_wait(&local->sync_release);
+
+	rmb();
+
+	header = (VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state,
+		local->slot_sync);
+
+	{
+		int oldmsgid = header->msgid;
+		if (oldmsgid != VCHIQ_MSGID_PADDING)
+			vchiq_log_error(vchiq_core_log_level,
+				"%d: qms - msgid %x, not PADDING",
+				state->id, oldmsgid);
+	}
+
+	if (service) {
+		int i, pos;
+
+		vchiq_log_info(vchiq_sync_log_level,
+			"%d: qms %s@%x,%x (%d->%d)", state->id,
+			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+			(unsigned int)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) {
+					mutex_unlock(&state->sync_mutex);
+					VCHIQ_SERVICE_STATS_INC(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));
+				}
+			}
+
+		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@%x,%x (%d->%d)", state->id,
+			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+			(unsigned int)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);
+	}
+
+	header->size = size;
+	header->msgid = msgid;
+
+	if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) {
+		int svc_fourcc;
+
+		svc_fourcc = service
+			? service->base.fourcc
+			: VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
+
+		vchiq_log_trace(vchiq_sync_log_level,
+			"Sent Sync Msg %s(%u) to %c%c%c%c s:%u d:%d len:%d",
+			msg_type_str(VCHIQ_MSG_TYPE(msgid)),
+			VCHIQ_MSG_TYPE(msgid),
+			VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
+			VCHIQ_MSG_SRCPORT(msgid),
+			VCHIQ_MSG_DSTPORT(msgid),
+			size);
+	}
+
+	/* Make sure the new header is visible to the peer. */
+	wmb();
+
+	remote_event_signal(&state->remote->sync_trigger);
+
+	if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_PAUSE)
+		mutex_unlock(&state->sync_mutex);
+
+	return VCHIQ_SUCCESS;
+}
+
+static inline void
+claim_slot(VCHIQ_SLOT_INFO_T *slot)
+{
+	slot->use_count++;
+}
+
+static void
+release_slot(VCHIQ_STATE_T *state, VCHIQ_SLOT_INFO_T *slot_info,
+	VCHIQ_HEADER_T *header, VCHIQ_SERVICE_T *service)
+{
+	int release_count;
+
+	mutex_lock(&state->recycle_mutex);
+
+	if (header) {
+		int msgid = header->msgid;
+		if (((msgid & VCHIQ_MSGID_CLAIMED) == 0) ||
+			(service && service->closing)) {
+			mutex_unlock(&state->recycle_mutex);
+			return;
+		}
+
+		/* Rewrite the message header to prevent a double
+		** release */
+		header->msgid = msgid & ~VCHIQ_MSGID_CLAIMED;
+	}
+
+	release_count = slot_info->release_count;
+	slot_info->release_count = ++release_count;
+
+	if (release_count == slot_info->use_count) {
+		int slot_queue_recycle;
+		/* Add to the freed queue */
+
+		/* A read barrier is necessary here to prevent speculative
+		** fetches of remote->slot_queue_recycle from overtaking the
+		** mutex. */
+		rmb();
+
+		slot_queue_recycle = state->remote->slot_queue_recycle;
+		state->remote->slot_queue[slot_queue_recycle &
+			VCHIQ_SLOT_QUEUE_MASK] =
+			SLOT_INDEX_FROM_INFO(state, slot_info);
+		state->remote->slot_queue_recycle = slot_queue_recycle + 1;
+		vchiq_log_info(vchiq_core_log_level,
+			"%d: release_slot %d - recycle->%x",
+			state->id, SLOT_INDEX_FROM_INFO(state, slot_info),
+			state->remote->slot_queue_recycle);
+
+		/* A write barrier is necessary, but remote_event_signal
+		** contains one. */
+		remote_event_signal(&state->remote->recycle);
+	}
+
+	mutex_unlock(&state->recycle_mutex);
+}
+
+/* Called by the slot handler - don't hold the bulk mutex */
+static VCHIQ_STATUS_T
+notify_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue,
+	int retry_poll)
+{
+	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+	vchiq_log_trace(vchiq_core_log_level,
+		"%d: nb:%d %cx - p=%x rn=%x r=%x",
+		service->state->id, service->localport,
+		(queue == &service->bulk_tx) ? 't' : 'r',
+		queue->process, queue->remote_notify, queue->remove);
+
+	if (service->state->is_master) {
+		while (queue->remote_notify != queue->process) {
+			VCHIQ_BULK_T *bulk =
+				&queue->bulks[BULK_INDEX(queue->remote_notify)];
+			int msgtype = (bulk->dir == VCHIQ_BULK_TRANSMIT) ?
+				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);
+				if (status != VCHIQ_SUCCESS)
+					break;
+			}
+			queue->remote_notify++;
+		}
+	} else {
+		queue->remote_notify = queue->process;
+	}
+
+	if (status == VCHIQ_SUCCESS) {
+		while (queue->remove != queue->remote_notify) {
+			VCHIQ_BULK_T *bulk =
+				&queue->bulks[BULK_INDEX(queue->remove)];
+
+			/* Only generate callbacks for non-dummy bulk
+			** requests, and non-terminated services */
+			if (bulk->data && service->instance) {
+				if (bulk->actual != VCHIQ_BULK_ACTUAL_ABORTED) {
+					if (bulk->dir == VCHIQ_BULK_TRANSMIT) {
+						VCHIQ_SERVICE_STATS_INC(service,
+							bulk_tx_count);
+						VCHIQ_SERVICE_STATS_ADD(service,
+							bulk_tx_bytes,
+							bulk->actual);
+					} else {
+						VCHIQ_SERVICE_STATS_INC(service,
+							bulk_rx_count);
+						VCHIQ_SERVICE_STATS_ADD(service,
+							bulk_rx_bytes,
+							bulk->actual);
+					}
+				} else {
+					VCHIQ_SERVICE_STATS_INC(service,
+						bulk_aborted_count);
+				}
+				if (bulk->mode == VCHIQ_BULK_MODE_BLOCKING) {
+					struct bulk_waiter *waiter;
+					spin_lock(&bulk_waiter_spinlock);
+					waiter = bulk->userdata;
+					if (waiter) {
+						waiter->actual = bulk->actual;
+						up(&waiter->event);
+					}
+					spin_unlock(&bulk_waiter_spinlock);
+				} else if (bulk->mode ==
+					VCHIQ_BULK_MODE_CALLBACK) {
+					VCHIQ_REASON_T reason = (bulk->dir ==
+						VCHIQ_BULK_TRANSMIT) ?
+						((bulk->actual ==
+						VCHIQ_BULK_ACTUAL_ABORTED) ?
+						VCHIQ_BULK_TRANSMIT_ABORTED :
+						VCHIQ_BULK_TRANSMIT_DONE) :
+						((bulk->actual ==
+						VCHIQ_BULK_ACTUAL_ABORTED) ?
+						VCHIQ_BULK_RECEIVE_ABORTED :
+						VCHIQ_BULK_RECEIVE_DONE);
+					status = make_service_callback(service,
+						reason,	NULL, bulk->userdata);
+					if (status == VCHIQ_RETRY)
+						break;
+				}
+			}
+
+			queue->remove++;
+			up(&service->bulk_remove_event);
+		}
+		if (!retry_poll)
+			status = VCHIQ_SUCCESS;
+	}
+
+	if (status == VCHIQ_RETRY)
+		request_poll(service->state, service,
+			(queue == &service->bulk_tx) ?
+			VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY);
+
+	return status;
+}
+
+/* Called by the slot handler thread */
+static void
+poll_services(VCHIQ_STATE_T *state)
+{
+	int group, i;
+
+	for (group = 0; group < BITSET_SIZE(state->unused_service); group++) {
+		uint32_t flags;
+		flags = atomic_xchg(&state->poll_services[group], 0);
+		for (i = 0; flags; i++) {
+			if (flags & (1 << i)) {
+				VCHIQ_SERVICE_T *service =
+					find_service_by_port(state,
+						(group<<5) + i);
+				uint32_t service_flags;
+				flags &= ~(1 << i);
+				if (!service)
+					continue;
+				service_flags =
+					atomic_xchg(&service->poll_flags, 0);
+				if (service_flags &
+					(1 << VCHIQ_POLL_REMOVE)) {
+					vchiq_log_info(vchiq_core_log_level,
+						"%d: ps - remove %d<->%d",
+						state->id, service->localport,
+						service->remoteport);
+
+					/* Make it look like a client, because
+					   it must be removed and not left in
+					   the LISTENING state. */
+					service->public_fourcc =
+						VCHIQ_FOURCC_INVALID;
+
+					if (vchiq_close_service_internal(
+						service, 0/*!close_recvd*/) !=
+						VCHIQ_SUCCESS)
+						request_poll(state, service,
+							VCHIQ_POLL_REMOVE);
+				} else if (service_flags &
+					(1 << VCHIQ_POLL_TERMINATE)) {
+					vchiq_log_info(vchiq_core_log_level,
+						"%d: ps - terminate %d<->%d",
+						state->id, service->localport,
+						service->remoteport);
+					if (vchiq_close_service_internal(
+						service, 0/*!close_recvd*/) !=
+						VCHIQ_SUCCESS)
+						request_poll(state, service,
+							VCHIQ_POLL_TERMINATE);
+				}
+				if (service_flags & (1 << VCHIQ_POLL_TXNOTIFY))
+					notify_bulks(service,
+						&service->bulk_tx,
+						1/*retry_poll*/);
+				if (service_flags & (1 << VCHIQ_POLL_RXNOTIFY))
+					notify_bulks(service,
+						&service->bulk_rx,
+						1/*retry_poll*/);
+				unlock_service(service);
+			}
+		}
+	}
+}
+
+/* Called by the slot handler or application threads, holding the bulk mutex. */
+static int
+resolve_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue)
+{
+	VCHIQ_STATE_T *state = service->state;
+	int resolved = 0;
+	int rc;
+
+	while ((queue->process != queue->local_insert) &&
+		(queue->process != queue->remote_insert)) {
+		VCHIQ_BULK_T *bulk = &queue->bulks[BULK_INDEX(queue->process)];
+
+		vchiq_log_trace(vchiq_core_log_level,
+			"%d: rb:%d %cx - li=%x ri=%x p=%x",
+			state->id, service->localport,
+			(queue == &service->bulk_tx) ? 't' : 'r',
+			queue->local_insert, queue->remote_insert,
+			queue->process);
+
+		WARN_ON(!((int)(queue->local_insert - queue->process) > 0));
+		WARN_ON(!((int)(queue->remote_insert - queue->process) > 0));
+
+		rc = mutex_lock_interruptible(&state->bulk_transfer_mutex);
+		if (rc != 0)
+			break;
+
+		vchiq_transfer_bulk(bulk);
+		mutex_unlock(&state->bulk_transfer_mutex);
+
+		if (SRVTRACE_ENABLED(service, VCHIQ_LOG_INFO)) {
+			const char *header = (queue == &service->bulk_tx) ?
+				"Send Bulk to" : "Recv Bulk from";
+			if (bulk->actual != VCHIQ_BULK_ACTUAL_ABORTED)
+				vchiq_log_info(SRVTRACE_LEVEL(service),
+					"%s %c%c%c%c d:%d len:%d %x<->%x",
+					header,
+					VCHIQ_FOURCC_AS_4CHARS(
+						service->base.fourcc),
+					service->remoteport,
+					bulk->size,
+					(unsigned int)bulk->data,
+					(unsigned int)bulk->remote_data);
+			else
+				vchiq_log_info(SRVTRACE_LEVEL(service),
+					"%s %c%c%c%c d:%d ABORTED - tx len:%d,"
+					" rx len:%d %x<->%x",
+					header,
+					VCHIQ_FOURCC_AS_4CHARS(
+						service->base.fourcc),
+					service->remoteport,
+					bulk->size,
+					bulk->remote_size,
+					(unsigned int)bulk->data,
+					(unsigned int)bulk->remote_data);
+		}
+
+		vchiq_complete_bulk(bulk);
+		queue->process++;
+		resolved++;
+	}
+	return resolved;
+}
+
+/* Called with the bulk_mutex held */
+static void
+abort_outstanding_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue)
+{
+	int is_tx = (queue == &service->bulk_tx);
+	vchiq_log_trace(vchiq_core_log_level,
+		"%d: aob:%d %cx - li=%x ri=%x p=%x",
+		service->state->id, service->localport, is_tx ? 't' : 'r',
+		queue->local_insert, queue->remote_insert, queue->process);
+
+	WARN_ON(!((int)(queue->local_insert - queue->process) >= 0));
+	WARN_ON(!((int)(queue->remote_insert - queue->process) >= 0));
+
+	while ((queue->process != queue->local_insert) ||
+		(queue->process != queue->remote_insert)) {
+		VCHIQ_BULK_T *bulk = &queue->bulks[BULK_INDEX(queue->process)];
+
+		if (queue->process == queue->remote_insert) {
+			/* fabricate a matching dummy bulk */
+			bulk->remote_data = NULL;
+			bulk->remote_size = 0;
+			queue->remote_insert++;
+		}
+
+		if (queue->process != queue->local_insert) {
+			vchiq_complete_bulk(bulk);
+
+			vchiq_log_info(SRVTRACE_LEVEL(service),
+				"%s %c%c%c%c d:%d ABORTED - tx len:%d, "
+				"rx len:%d",
+				is_tx ? "Send Bulk to" : "Recv Bulk from",
+				VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+				service->remoteport,
+				bulk->size,
+				bulk->remote_size);
+		} else {
+			/* fabricate a matching dummy bulk */
+			bulk->data = NULL;
+			bulk->size = 0;
+			bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED;
+			bulk->dir = is_tx ? VCHIQ_BULK_TRANSMIT :
+				VCHIQ_BULK_RECEIVE;
+			queue->local_insert++;
+		}
+
+		queue->process++;
+	}
+}
+
+/* Called from the slot handler thread */
+static void
+pause_bulks(VCHIQ_STATE_T *state)
+{
+	if (unlikely(atomic_inc_return(&pause_bulks_count) != 1)) {
+		WARN_ON_ONCE(1);
+		atomic_set(&pause_bulks_count, 1);
+		return;
+	}
+
+	/* Block bulk transfers from all services */
+	mutex_lock(&state->bulk_transfer_mutex);
+}
+
+/* Called from the slot handler thread */
+static void
+resume_bulks(VCHIQ_STATE_T *state)
+{
+	int i;
+	if (unlikely(atomic_dec_return(&pause_bulks_count) != 0)) {
+		WARN_ON_ONCE(1);
+		atomic_set(&pause_bulks_count, 0);
+		return;
+	}
+
+	/* Allow bulk transfers from all services */
+	mutex_unlock(&state->bulk_transfer_mutex);
+
+	if (state->deferred_bulks == 0)
+		return;
+
+	/* Deal with any bulks which had to be deferred due to being in
+	 * paused state.  Don't try to match up to number of deferred bulks
+	 * in case we've had something come and close the service in the
+	 * interim - just process all bulk queues for all services */
+	vchiq_log_info(vchiq_core_log_level, "%s: processing %d deferred bulks",
+		__func__, state->deferred_bulks);
+
+	for (i = 0; i < state->unused_service; i++) {
+		VCHIQ_SERVICE_T *service = state->services[i];
+		int resolved_rx = 0;
+		int resolved_tx = 0;
+		if (!service || (service->srvstate != VCHIQ_SRVSTATE_OPEN))
+			continue;
+
+		mutex_lock(&service->bulk_mutex);
+		resolved_rx = resolve_bulks(service, &service->bulk_rx);
+		resolved_tx = resolve_bulks(service, &service->bulk_tx);
+		mutex_unlock(&service->bulk_mutex);
+		if (resolved_rx)
+			notify_bulks(service, &service->bulk_rx, 1);
+		if (resolved_tx)
+			notify_bulks(service, &service->bulk_tx, 1);
+	}
+	state->deferred_bulks = 0;
+}
+
+static int
+parse_open(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header)
+{
+	VCHIQ_SERVICE_T *service = NULL;
+	int msgid, size;
+	int type;
+	unsigned int localport, remoteport;
+
+	msgid = header->msgid;
+	size = header->size;
+	type = VCHIQ_MSG_TYPE(msgid);
+	localport = VCHIQ_MSG_DSTPORT(msgid);
+	remoteport = VCHIQ_MSG_SRCPORT(msgid);
+	if (size >= sizeof(struct vchiq_open_payload)) {
+		const struct vchiq_open_payload *payload =
+			(struct vchiq_open_payload *)header->data;
+		unsigned int fourcc;
+
+		fourcc = payload->fourcc;
+		vchiq_log_info(vchiq_core_log_level,
+			"%d: prs OPEN@%x (%d->'%c%c%c%c')",
+			state->id, (unsigned int)header,
+			localport,
+			VCHIQ_FOURCC_AS_4CHARS(fourcc));
+
+		service = get_listening_service(state, fourcc);
+
+		if (service) {
+			/* A matching service exists */
+			short version = payload->version;
+			short version_min = payload->version_min;
+			if ((service->version < version_min) ||
+				(version < service->version_min)) {
+				/* Version mismatch */
+				vchiq_loud_error_header();
+				vchiq_loud_error("%d: service %d (%c%c%c%c) "
+					"version mismatch - local (%d, min %d)"
+					" vs. remote (%d, min %d)",
+					state->id, service->localport,
+					VCHIQ_FOURCC_AS_4CHARS(fourcc),
+					service->version, service->version_min,
+					version, version_min);
+				vchiq_loud_error_footer();
+				unlock_service(service);
+				service = NULL;
+				goto fail_open;
+			}
+			service->peer_version = version;
+
+			if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) {
+				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)
+					service->sync = 0;
+
+				/* Acknowledge the OPEN */
+				if (service->sync &&
+				    (state->version_common >=
+				     VCHIQ_VERSION_SYNCHRONOUS_MODE)) {
+					if (queue_message_sync(state, NULL,
+						VCHIQ_MAKE_MSG(
+							VCHIQ_MSG_OPENACK,
+							service->localport,
+							remoteport),
+						&body, 1, sizeof(ack_payload),
+						0) == VCHIQ_RETRY)
+						goto bail_not_ready;
+				} else {
+					if (queue_message(state, NULL,
+						VCHIQ_MAKE_MSG(
+							VCHIQ_MSG_OPENACK,
+							service->localport,
+							remoteport),
+						&body, 1, sizeof(ack_payload),
+						0) == VCHIQ_RETRY)
+						goto bail_not_ready;
+				}
+
+				/* The service is now open */
+				vchiq_set_service_state(service,
+					service->sync ? VCHIQ_SRVSTATE_OPENSYNC
+					: VCHIQ_SRVSTATE_OPEN);
+			}
+
+			service->remoteport = remoteport;
+			service->client_id = ((int *)header->data)[1];
+			if (make_service_callback(service, VCHIQ_SERVICE_OPENED,
+				NULL, NULL) == VCHIQ_RETRY) {
+				/* Bail out if not ready */
+				service->remoteport = VCHIQ_PORT_FREE;
+				goto bail_not_ready;
+			}
+
+			/* Success - the message has been dealt with */
+			unlock_service(service);
+			return 1;
+		}
+	}
+
+fail_open:
+	/* No available service, or an invalid request - send a CLOSE */
+	if (queue_message(state, NULL,
+		VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, 0, VCHIQ_MSG_SRCPORT(msgid)),
+		NULL, 0, 0, 0) == VCHIQ_RETRY)
+		goto bail_not_ready;
+
+	return 1;
+
+bail_not_ready:
+	if (service)
+		unlock_service(service);
+
+	return 0;
+}
+
+/* Called by the slot handler thread */
+static void
+parse_rx_slots(VCHIQ_STATE_T *state)
+{
+	VCHIQ_SHARED_STATE_T *remote = state->remote;
+	VCHIQ_SERVICE_T *service = NULL;
+	int tx_pos;
+	DEBUG_INITIALISE(state->local)
+
+	tx_pos = remote->tx_pos;
+
+	while (state->rx_pos != tx_pos) {
+		VCHIQ_HEADER_T *header;
+		int msgid, size;
+		int type;
+		unsigned int localport, remoteport;
+
+		DEBUG_TRACE(PARSE_LINE);
+		if (!state->rx_data) {
+			int rx_index;
+			WARN_ON(!((state->rx_pos & VCHIQ_SLOT_MASK) == 0));
+			rx_index = remote->slot_queue[
+				SLOT_QUEUE_INDEX_FROM_POS(state->rx_pos) &
+				VCHIQ_SLOT_QUEUE_MASK];
+			state->rx_data = (char *)SLOT_DATA_FROM_INDEX(state,
+				rx_index);
+			state->rx_info = SLOT_INFO_FROM_INDEX(state, rx_index);
+
+			/* Initialise use_count to one, and increment
+			** release_count at the end of the slot to avoid
+			** releasing the slot prematurely. */
+			state->rx_info->use_count = 1;
+			state->rx_info->release_count = 0;
+		}
+
+		header = (VCHIQ_HEADER_T *)(state->rx_data +
+			(state->rx_pos & VCHIQ_SLOT_MASK));
+		DEBUG_VALUE(PARSE_HEADER, (int)header);
+		msgid = header->msgid;
+		DEBUG_VALUE(PARSE_MSGID, msgid);
+		size = header->size;
+		type = VCHIQ_MSG_TYPE(msgid);
+		localport = VCHIQ_MSG_DSTPORT(msgid);
+		remoteport = VCHIQ_MSG_SRCPORT(msgid);
+
+		if (type != VCHIQ_MSG_DATA)
+			VCHIQ_STATS_INC(state, ctrl_rx_count);
+
+		switch (type) {
+		case VCHIQ_MSG_OPENACK:
+		case VCHIQ_MSG_CLOSE:
+		case VCHIQ_MSG_DATA:
+		case VCHIQ_MSG_BULK_RX:
+		case VCHIQ_MSG_BULK_TX:
+		case VCHIQ_MSG_BULK_RX_DONE:
+		case VCHIQ_MSG_BULK_TX_DONE:
+			service = find_service_by_port(state, localport);
+			if ((!service ||
+			     ((service->remoteport != remoteport) &&
+			      (service->remoteport != VCHIQ_PORT_FREE))) &&
+			    (localport == 0) &&
+			    (type == VCHIQ_MSG_CLOSE)) {
+				/* This could be a CLOSE from a client which
+				   hadn't yet received the OPENACK - look for
+				   the connected service */
+				if (service)
+					unlock_service(service);
+				service = get_connected_service(state,
+					remoteport);
+				if (service)
+					vchiq_log_warning(vchiq_core_log_level,
+						"%d: prs %s@%x (%d->%d) - "
+						"found connected service %d",
+						state->id, msg_type_str(type),
+						(unsigned int)header,
+						remoteport, localport,
+						service->localport);
+			}
+
+			if (!service) {
+				vchiq_log_error(vchiq_core_log_level,
+					"%d: prs %s@%x (%d->%d) - "
+					"invalid/closed service %d",
+					state->id, msg_type_str(type),
+					(unsigned int)header,
+					remoteport, localport, localport);
+				goto skip_message;
+			}
+			break;
+		default:
+			break;
+		}
+
+		if (SRVTRACE_ENABLED(service, VCHIQ_LOG_INFO)) {
+			int svc_fourcc;
+
+			svc_fourcc = service
+				? service->base.fourcc
+				: VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
+			vchiq_log_info(SRVTRACE_LEVEL(service),
+				"Rcvd Msg %s(%u) from %c%c%c%c s:%d d:%d "
+				"len:%d",
+				msg_type_str(type), type,
+				VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
+				remoteport, localport, size);
+			if (size > 0)
+				vchiq_log_dump_mem("Rcvd", 0, header->data,
+					min(64, size));
+		}
+
+		if (((unsigned int)header & VCHIQ_SLOT_MASK) + calc_stride(size)
+			> VCHIQ_SLOT_SIZE) {
+			vchiq_log_error(vchiq_core_log_level,
+				"header %x (msgid %x) - size %x too big for "
+				"slot",
+				(unsigned int)header, (unsigned int)msgid,
+				(unsigned int)size);
+			WARN(1, "oversized for slot\n");
+		}
+
+		switch (type) {
+		case VCHIQ_MSG_OPEN:
+			WARN_ON(!(VCHIQ_MSG_DSTPORT(msgid) == 0));
+			if (!parse_open(state, header))
+				goto bail_not_ready;
+			break;
+		case VCHIQ_MSG_OPENACK:
+			if (size >= sizeof(struct vchiq_openack_payload)) {
+				const struct vchiq_openack_payload *payload =
+					(struct vchiq_openack_payload *)
+					header->data;
+				service->peer_version = payload->version;
+			}
+			vchiq_log_info(vchiq_core_log_level,
+				"%d: prs OPENACK@%x,%x (%d->%d) v:%d",
+				state->id, (unsigned int)header, size,
+				remoteport, localport, service->peer_version);
+			if (service->srvstate ==
+				VCHIQ_SRVSTATE_OPENING) {
+				service->remoteport = remoteport;
+				vchiq_set_service_state(service,
+					VCHIQ_SRVSTATE_OPEN);
+				up(&service->remove_event);
+			} else
+				vchiq_log_error(vchiq_core_log_level,
+					"OPENACK received in state %s",
+					srvstate_names[service->srvstate]);
+			break;
+		case VCHIQ_MSG_CLOSE:
+			WARN_ON(size != 0); /* There should be no data */
+
+			vchiq_log_info(vchiq_core_log_level,
+				"%d: prs CLOSE@%x (%d->%d)",
+				state->id, (unsigned int)header,
+				remoteport, localport);
+
+			mark_service_closing_internal(service, 1);
+
+			if (vchiq_close_service_internal(service,
+				1/*close_recvd*/) == VCHIQ_RETRY)
+				goto bail_not_ready;
+
+			vchiq_log_info(vchiq_core_log_level,
+				"Close Service %c%c%c%c s:%u d:%d",
+				VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
+				service->localport,
+				service->remoteport);
+			break;
+		case VCHIQ_MSG_DATA:
+			vchiq_log_info(vchiq_core_log_level,
+				"%d: prs DATA@%x,%x (%d->%d)",
+				state->id, (unsigned int)header, size,
+				remoteport, localport);
+
+			if ((service->remoteport == remoteport)
+				&& (service->srvstate ==
+				VCHIQ_SRVSTATE_OPEN)) {
+				header->msgid = msgid | VCHIQ_MSGID_CLAIMED;
+				claim_slot(state->rx_info);
+				DEBUG_TRACE(PARSE_LINE);
+				if (make_service_callback(service,
+					VCHIQ_MESSAGE_AVAILABLE, header,
+					NULL) == VCHIQ_RETRY) {
+					DEBUG_TRACE(PARSE_LINE);
+					goto bail_not_ready;
+				}
+				VCHIQ_SERVICE_STATS_INC(service, ctrl_rx_count);
+				VCHIQ_SERVICE_STATS_ADD(service, ctrl_rx_bytes,
+					size);
+			} else {
+				VCHIQ_STATS_INC(state, error_count);
+			}
+			break;
+		case VCHIQ_MSG_CONNECT:
+			vchiq_log_info(vchiq_core_log_level,
+				"%d: prs CONNECT@%x",
+				state->id, (unsigned int)header);
+			state->version_common = ((VCHIQ_SLOT_ZERO_T *)
+						 state->slot_data)->version;
+			up(&state->connect);
+			break;
+		case VCHIQ_MSG_BULK_RX:
+		case VCHIQ_MSG_BULK_TX: {
+			VCHIQ_BULK_QUEUE_T *queue;
+			WARN_ON(!state->is_master);
+			queue = (type == VCHIQ_MSG_BULK_RX) ?
+				&service->bulk_tx : &service->bulk_rx;
+			if ((service->remoteport == remoteport)
+				&& (service->srvstate ==
+				VCHIQ_SRVSTATE_OPEN)) {
+				VCHIQ_BULK_T *bulk;
+				int resolved = 0;
+
+				DEBUG_TRACE(PARSE_LINE);
+				if (mutex_lock_interruptible(
+					&service->bulk_mutex) != 0) {
+					DEBUG_TRACE(PARSE_LINE);
+					goto bail_not_ready;
+				}
+
+				WARN_ON(!(queue->remote_insert < queue->remove +
+					VCHIQ_NUM_SERVICE_BULKS));
+				bulk = &queue->bulks[
+					BULK_INDEX(queue->remote_insert)];
+				bulk->remote_data =
+					(void *)((int *)header->data)[0];
+				bulk->remote_size = ((int *)header->data)[1];
+				wmb();
+
+				vchiq_log_info(vchiq_core_log_level,
+					"%d: prs %s@%x (%d->%d) %x@%x",
+					state->id, msg_type_str(type),
+					(unsigned int)header,
+					remoteport, localport,
+					bulk->remote_size,
+					(unsigned int)bulk->remote_data);
+
+				queue->remote_insert++;
+
+				if (atomic_read(&pause_bulks_count)) {
+					state->deferred_bulks++;
+					vchiq_log_info(vchiq_core_log_level,
+						"%s: deferring bulk (%d)",
+						__func__,
+						state->deferred_bulks);
+					if (state->conn_state !=
+						VCHIQ_CONNSTATE_PAUSE_SENT)
+						vchiq_log_error(
+							vchiq_core_log_level,
+							"%s: bulks paused in "
+							"unexpected state %s",
+							__func__,
+							conn_state_names[
+							state->conn_state]);
+				} else if (state->conn_state ==
+					VCHIQ_CONNSTATE_CONNECTED) {
+					DEBUG_TRACE(PARSE_LINE);
+					resolved = resolve_bulks(service,
+						queue);
+				}
+
+				mutex_unlock(&service->bulk_mutex);
+				if (resolved)
+					notify_bulks(service, queue,
+						1/*retry_poll*/);
+			}
+		} break;
+		case VCHIQ_MSG_BULK_RX_DONE:
+		case VCHIQ_MSG_BULK_TX_DONE:
+			WARN_ON(state->is_master);
+			if ((service->remoteport == remoteport)
+				&& (service->srvstate !=
+				VCHIQ_SRVSTATE_FREE)) {
+				VCHIQ_BULK_QUEUE_T *queue;
+				VCHIQ_BULK_T *bulk;
+
+				queue = (type == VCHIQ_MSG_BULK_RX_DONE) ?
+					&service->bulk_rx : &service->bulk_tx;
+
+				DEBUG_TRACE(PARSE_LINE);
+				if (mutex_lock_interruptible(
+					&service->bulk_mutex) != 0) {
+					DEBUG_TRACE(PARSE_LINE);
+					goto bail_not_ready;
+				}
+				if ((int)(queue->remote_insert -
+					queue->local_insert) >= 0) {
+					vchiq_log_error(vchiq_core_log_level,
+						"%d: prs %s@%x (%d->%d) "
+						"unexpected (ri=%d,li=%d)",
+						state->id, msg_type_str(type),
+						(unsigned int)header,
+						remoteport, localport,
+						queue->remote_insert,
+						queue->local_insert);
+					mutex_unlock(&service->bulk_mutex);
+					break;
+				}
+
+				BUG_ON(queue->process == queue->local_insert);
+				BUG_ON(queue->process != queue->remote_insert);
+
+				bulk = &queue->bulks[
+					BULK_INDEX(queue->remote_insert)];
+				bulk->actual = *(int *)header->data;
+				queue->remote_insert++;
+
+				vchiq_log_info(vchiq_core_log_level,
+					"%d: prs %s@%x (%d->%d) %x@%x",
+					state->id, msg_type_str(type),
+					(unsigned int)header,
+					remoteport, localport,
+					bulk->actual, (unsigned int)bulk->data);
+
+				vchiq_log_trace(vchiq_core_log_level,
+					"%d: prs:%d %cx li=%x ri=%x p=%x",
+					state->id, localport,
+					(type == VCHIQ_MSG_BULK_RX_DONE) ?
+						'r' : 't',
+					queue->local_insert,
+					queue->remote_insert, queue->process);
+
+				DEBUG_TRACE(PARSE_LINE);
+				WARN_ON(queue->process == queue->local_insert);
+				vchiq_complete_bulk(bulk);
+				queue->process++;
+				mutex_unlock(&service->bulk_mutex);
+				DEBUG_TRACE(PARSE_LINE);
+				notify_bulks(service, queue, 1/*retry_poll*/);
+				DEBUG_TRACE(PARSE_LINE);
+			}
+			break;
+		case VCHIQ_MSG_PADDING:
+			vchiq_log_trace(vchiq_core_log_level,
+				"%d: prs PADDING@%x,%x",
+				state->id, (unsigned int)header, size);
+			break;
+		case VCHIQ_MSG_PAUSE:
+			/* If initiated, signal the application thread */
+			vchiq_log_trace(vchiq_core_log_level,
+				"%d: prs PAUSE@%x,%x",
+				state->id, (unsigned int)header, size);
+			if (state->conn_state == VCHIQ_CONNSTATE_PAUSED) {
+				vchiq_log_error(vchiq_core_log_level,
+					"%d: PAUSE received in state PAUSED",
+					state->id);
+				break;
+			}
+			if (state->conn_state != VCHIQ_CONNSTATE_PAUSE_SENT) {
+				/* Send a PAUSE in response */
+				if (queue_message(state, NULL,
+					VCHIQ_MAKE_MSG(VCHIQ_MSG_PAUSE, 0, 0),
+					NULL, 0, 0, QMFLAGS_NO_MUTEX_UNLOCK)
+				    == VCHIQ_RETRY)
+					goto bail_not_ready;
+				if (state->is_master)
+					pause_bulks(state);
+			}
+			/* At this point slot_mutex is held */
+			vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSED);
+			vchiq_platform_paused(state);
+			break;
+		case VCHIQ_MSG_RESUME:
+			vchiq_log_trace(vchiq_core_log_level,
+				"%d: prs RESUME@%x,%x",
+				state->id, (unsigned int)header, size);
+			/* Release the slot mutex */
+			mutex_unlock(&state->slot_mutex);
+			if (state->is_master)
+				resume_bulks(state);
+			vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
+			vchiq_platform_resumed(state);
+			break;
+
+		case VCHIQ_MSG_REMOTE_USE:
+			vchiq_on_remote_use(state);
+			break;
+		case VCHIQ_MSG_REMOTE_RELEASE:
+			vchiq_on_remote_release(state);
+			break;
+		case VCHIQ_MSG_REMOTE_USE_ACTIVE:
+			vchiq_on_remote_use_active(state);
+			break;
+
+		default:
+			vchiq_log_error(vchiq_core_log_level,
+				"%d: prs invalid msgid %x@%x,%x",
+				state->id, msgid, (unsigned int)header, size);
+			WARN(1, "invalid message\n");
+			break;
+		}
+
+skip_message:
+		if (service) {
+			unlock_service(service);
+			service = NULL;
+		}
+
+		state->rx_pos += calc_stride(size);
+
+		DEBUG_TRACE(PARSE_LINE);
+		/* Perform some housekeeping when the end of the slot is
+		** reached. */
+		if ((state->rx_pos & VCHIQ_SLOT_MASK) == 0) {
+			/* Remove the extra reference count. */
+			release_slot(state, state->rx_info, NULL, NULL);
+			state->rx_data = NULL;
+		}
+	}
+
+bail_not_ready:
+	if (service)
+		unlock_service(service);
+}
+
+/* Called by the slot handler thread */
+static int
+slot_handler_func(void *v)
+{
+	VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
+	VCHIQ_SHARED_STATE_T *local = state->local;
+	DEBUG_INITIALISE(local)
+
+	while (1) {
+		DEBUG_COUNT(SLOT_HANDLER_COUNT);
+		DEBUG_TRACE(SLOT_HANDLER_LINE);
+		remote_event_wait(&local->trigger);
+
+		rmb();
+
+		DEBUG_TRACE(SLOT_HANDLER_LINE);
+		if (state->poll_needed) {
+			/* Check if we need to suspend - may change our
+			 * conn_state */
+			vchiq_platform_check_suspend(state);
+
+			state->poll_needed = 0;
+
+			/* Handle service polling and other rare conditions here
+			** out of the mainline code */
+			switch (state->conn_state) {
+			case VCHIQ_CONNSTATE_CONNECTED:
+				/* Poll the services as requested */
+				poll_services(state);
+				break;
+
+			case VCHIQ_CONNSTATE_PAUSING:
+				if (state->is_master)
+					pause_bulks(state);
+				if (queue_message(state, NULL,
+					VCHIQ_MAKE_MSG(VCHIQ_MSG_PAUSE, 0, 0),
+					NULL, 0, 0,
+					QMFLAGS_NO_MUTEX_UNLOCK)
+				    != VCHIQ_RETRY) {
+					vchiq_set_conn_state(state,
+						VCHIQ_CONNSTATE_PAUSE_SENT);
+				} else {
+					if (state->is_master)
+						resume_bulks(state);
+					/* Retry later */
+					state->poll_needed = 1;
+				}
+				break;
+
+			case VCHIQ_CONNSTATE_PAUSED:
+				vchiq_platform_resume(state);
+				break;
+
+			case VCHIQ_CONNSTATE_RESUMING:
+				if (queue_message(state, NULL,
+					VCHIQ_MAKE_MSG(VCHIQ_MSG_RESUME, 0, 0),
+					NULL, 0, 0, QMFLAGS_NO_MUTEX_LOCK)
+					!= VCHIQ_RETRY) {
+					if (state->is_master)
+						resume_bulks(state);
+					vchiq_set_conn_state(state,
+						VCHIQ_CONNSTATE_CONNECTED);
+					vchiq_platform_resumed(state);
+				} else {
+					/* This should really be impossible,
+					** since the PAUSE should have flushed
+					** through outstanding messages. */
+					vchiq_log_error(vchiq_core_log_level,
+						"Failed to send RESUME "
+						"message");
+					BUG();
+				}
+				break;
+
+			case VCHIQ_CONNSTATE_PAUSE_TIMEOUT:
+			case VCHIQ_CONNSTATE_RESUME_TIMEOUT:
+				vchiq_platform_handle_timeout(state);
+				break;
+			default:
+				break;
+			}
+
+
+		}
+
+		DEBUG_TRACE(SLOT_HANDLER_LINE);
+		parse_rx_slots(state);
+	}
+	return 0;
+}
+
+
+/* Called by the recycle thread */
+static int
+recycle_func(void *v)
+{
+	VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
+	VCHIQ_SHARED_STATE_T *local = state->local;
+
+	while (1) {
+		remote_event_wait(&local->recycle);
+
+		process_free_queue(state);
+	}
+	return 0;
+}
+
+
+/* Called by the sync thread */
+static int
+sync_func(void *v)
+{
+	VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
+	VCHIQ_SHARED_STATE_T *local = state->local;
+	VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state,
+		state->remote->slot_sync);
+
+	while (1) {
+		VCHIQ_SERVICE_T *service;
+		int msgid, size;
+		int type;
+		unsigned int localport, remoteport;
+
+		remote_event_wait(&local->sync_trigger);
+
+		rmb();
+
+		msgid = header->msgid;
+		size = header->size;
+		type = VCHIQ_MSG_TYPE(msgid);
+		localport = VCHIQ_MSG_DSTPORT(msgid);
+		remoteport = VCHIQ_MSG_SRCPORT(msgid);
+
+		service = find_service_by_port(state, localport);
+
+		if (!service) {
+			vchiq_log_error(vchiq_sync_log_level,
+				"%d: sf %s@%x (%d->%d) - "
+				"invalid/closed service %d",
+				state->id, msg_type_str(type),
+				(unsigned int)header,
+				remoteport, localport, localport);
+			release_message_sync(state, header);
+			continue;
+		}
+
+		if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) {
+			int svc_fourcc;
+
+			svc_fourcc = service
+				? service->base.fourcc
+				: VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
+			vchiq_log_trace(vchiq_sync_log_level,
+				"Rcvd Msg %s from %c%c%c%c s:%d d:%d len:%d",
+				msg_type_str(type),
+				VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
+				remoteport, localport, size);
+			if (size > 0)
+				vchiq_log_dump_mem("Rcvd", 0, header->data,
+					min(64, size));
+		}
+
+		switch (type) {
+		case VCHIQ_MSG_OPENACK:
+			if (size >= sizeof(struct vchiq_openack_payload)) {
+				const struct vchiq_openack_payload *payload =
+					(struct vchiq_openack_payload *)
+					header->data;
+				service->peer_version = payload->version;
+			}
+			vchiq_log_info(vchiq_sync_log_level,
+				"%d: sf OPENACK@%x,%x (%d->%d) v:%d",
+				state->id, (unsigned int)header, size,
+				remoteport, localport, service->peer_version);
+			if (service->srvstate == VCHIQ_SRVSTATE_OPENING) {
+				service->remoteport = remoteport;
+				vchiq_set_service_state(service,
+					VCHIQ_SRVSTATE_OPENSYNC);
+				service->sync = 1;
+				up(&service->remove_event);
+			}
+			release_message_sync(state, header);
+			break;
+
+		case VCHIQ_MSG_DATA:
+			vchiq_log_trace(vchiq_sync_log_level,
+				"%d: sf DATA@%x,%x (%d->%d)",
+				state->id, (unsigned int)header, size,
+				remoteport, localport);
+
+			if ((service->remoteport == remoteport) &&
+				(service->srvstate ==
+				VCHIQ_SRVSTATE_OPENSYNC)) {
+				if (make_service_callback(service,
+					VCHIQ_MESSAGE_AVAILABLE, header,
+					NULL) == VCHIQ_RETRY)
+					vchiq_log_error(vchiq_sync_log_level,
+						"synchronous callback to "
+						"service %d returns "
+						"VCHIQ_RETRY",
+						localport);
+			}
+			break;
+
+		default:
+			vchiq_log_error(vchiq_sync_log_level,
+				"%d: sf unexpected msgid %x@%x,%x",
+				state->id, msgid, (unsigned int)header, size);
+			release_message_sync(state, header);
+			break;
+		}
+
+		unlock_service(service);
+	}
+
+	return 0;
+}
+
+
+static void
+init_bulk_queue(VCHIQ_BULK_QUEUE_T *queue)
+{
+	queue->local_insert = 0;
+	queue->remote_insert = 0;
+	queue->process = 0;
+	queue->remote_notify = 0;
+	queue->remove = 0;
+}
+
+
+inline const char *
+get_conn_state_name(VCHIQ_CONNSTATE_T conn_state)
+{
+	return conn_state_names[conn_state];
+}
+
+
+VCHIQ_SLOT_ZERO_T *
+vchiq_init_slots(void *mem_base, int mem_size)
+{
+	int mem_align = (VCHIQ_SLOT_SIZE - (int)mem_base) & VCHIQ_SLOT_MASK;
+	VCHIQ_SLOT_ZERO_T *slot_zero =
+		(VCHIQ_SLOT_ZERO_T *)((char *)mem_base + mem_align);
+	int num_slots = (mem_size - mem_align)/VCHIQ_SLOT_SIZE;
+	int first_data_slot = VCHIQ_SLOT_ZERO_SLOTS;
+
+	/* Ensure there is enough memory to run an absolutely minimum system */
+	num_slots -= first_data_slot;
+
+	if (num_slots < 4) {
+		vchiq_log_error(vchiq_core_log_level,
+			"vchiq_init_slots - insufficient memory %x bytes",
+			mem_size);
+		return NULL;
+	}
+
+	memset(slot_zero, 0, sizeof(VCHIQ_SLOT_ZERO_T));
+
+	slot_zero->magic = VCHIQ_MAGIC;
+	slot_zero->version = VCHIQ_VERSION;
+	slot_zero->version_min = VCHIQ_VERSION_MIN;
+	slot_zero->slot_zero_size = sizeof(VCHIQ_SLOT_ZERO_T);
+	slot_zero->slot_size = VCHIQ_SLOT_SIZE;
+	slot_zero->max_slots = VCHIQ_MAX_SLOTS;
+	slot_zero->max_slots_per_side = VCHIQ_MAX_SLOTS_PER_SIDE;
+
+	slot_zero->master.slot_sync = first_data_slot;
+	slot_zero->master.slot_first = first_data_slot + 1;
+	slot_zero->master.slot_last = first_data_slot + (num_slots/2) - 1;
+	slot_zero->slave.slot_sync = first_data_slot + (num_slots/2);
+	slot_zero->slave.slot_first = first_data_slot + (num_slots/2) + 1;
+	slot_zero->slave.slot_last = first_data_slot + num_slots - 1;
+
+	return slot_zero;
+}
+
+VCHIQ_STATUS_T
+vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero,
+		 int is_master)
+{
+	VCHIQ_SHARED_STATE_T *local;
+	VCHIQ_SHARED_STATE_T *remote;
+	VCHIQ_STATUS_T status;
+	char threadname[10];
+	static int id;
+	int i;
+
+	vchiq_log_warning(vchiq_core_log_level,
+		"%s: slot_zero = 0x%08lx, is_master = %d",
+		__func__, (unsigned long)slot_zero, is_master);
+
+	/* Check the input configuration */
+
+	if (slot_zero->magic != VCHIQ_MAGIC) {
+		vchiq_loud_error_header();
+		vchiq_loud_error("Invalid VCHIQ magic value found.");
+		vchiq_loud_error("slot_zero=%x: magic=%x (expected %x)",
+			(unsigned int)slot_zero, slot_zero->magic, VCHIQ_MAGIC);
+		vchiq_loud_error_footer();
+		return VCHIQ_ERROR;
+	}
+
+	if (slot_zero->version < VCHIQ_VERSION_MIN) {
+		vchiq_loud_error_header();
+		vchiq_loud_error("Incompatible VCHIQ versions found.");
+		vchiq_loud_error("slot_zero=%x: VideoCore version=%d "
+			"(minimum %d)",
+			(unsigned int)slot_zero, slot_zero->version,
+			VCHIQ_VERSION_MIN);
+		vchiq_loud_error("Restart with a newer VideoCore image.");
+		vchiq_loud_error_footer();
+		return VCHIQ_ERROR;
+	}
+
+	if (VCHIQ_VERSION < slot_zero->version_min) {
+		vchiq_loud_error_header();
+		vchiq_loud_error("Incompatible VCHIQ versions found.");
+		vchiq_loud_error("slot_zero=%x: version=%d (VideoCore "
+			"minimum %d)",
+			(unsigned int)slot_zero, VCHIQ_VERSION,
+			slot_zero->version_min);
+		vchiq_loud_error("Restart with a newer kernel.");
+		vchiq_loud_error_footer();
+		return VCHIQ_ERROR;
+	}
+
+	if ((slot_zero->slot_zero_size != sizeof(VCHIQ_SLOT_ZERO_T)) ||
+		 (slot_zero->slot_size != VCHIQ_SLOT_SIZE) ||
+		 (slot_zero->max_slots != VCHIQ_MAX_SLOTS) ||
+		 (slot_zero->max_slots_per_side != VCHIQ_MAX_SLOTS_PER_SIDE)) {
+		vchiq_loud_error_header();
+		if (slot_zero->slot_zero_size != sizeof(VCHIQ_SLOT_ZERO_T))
+			vchiq_loud_error("slot_zero=%x: slot_zero_size=%x "
+				"(expected %x)",
+				(unsigned int)slot_zero,
+				slot_zero->slot_zero_size,
+				sizeof(VCHIQ_SLOT_ZERO_T));
+		if (slot_zero->slot_size != VCHIQ_SLOT_SIZE)
+			vchiq_loud_error("slot_zero=%x: slot_size=%d "
+				"(expected %d",
+				(unsigned int)slot_zero, slot_zero->slot_size,
+				VCHIQ_SLOT_SIZE);
+		if (slot_zero->max_slots != VCHIQ_MAX_SLOTS)
+			vchiq_loud_error("slot_zero=%x: max_slots=%d "
+				"(expected %d)",
+				(unsigned int)slot_zero, slot_zero->max_slots,
+				VCHIQ_MAX_SLOTS);
+		if (slot_zero->max_slots_per_side != VCHIQ_MAX_SLOTS_PER_SIDE)
+			vchiq_loud_error("slot_zero=%x: max_slots_per_side=%d "
+				"(expected %d)",
+				(unsigned int)slot_zero,
+				slot_zero->max_slots_per_side,
+				VCHIQ_MAX_SLOTS_PER_SIDE);
+		vchiq_loud_error_footer();
+		return VCHIQ_ERROR;
+	}
+
+	if (VCHIQ_VERSION < slot_zero->version)
+		slot_zero->version = VCHIQ_VERSION;
+
+	if (is_master) {
+		local = &slot_zero->master;
+		remote = &slot_zero->slave;
+	} else {
+		local = &slot_zero->slave;
+		remote = &slot_zero->master;
+	}
+
+	if (local->initialised) {
+		vchiq_loud_error_header();
+		if (remote->initialised)
+			vchiq_loud_error("local state has already been "
+				"initialised");
+		else
+			vchiq_loud_error("master/slave mismatch - two %ss",
+				is_master ? "master" : "slave");
+		vchiq_loud_error_footer();
+		return VCHIQ_ERROR;
+	}
+
+	memset(state, 0, sizeof(VCHIQ_STATE_T));
+
+	state->id = id++;
+	state->is_master = is_master;
+
+	/*
+		initialize shared state pointers
+	 */
+
+	state->local = local;
+	state->remote = remote;
+	state->slot_data = (VCHIQ_SLOT_T *)slot_zero;
+
+	/*
+		initialize events and mutexes
+	 */
+
+	sema_init(&state->connect, 0);
+	mutex_init(&state->mutex);
+	sema_init(&state->trigger_event, 0);
+	sema_init(&state->recycle_event, 0);
+	sema_init(&state->sync_trigger_event, 0);
+	sema_init(&state->sync_release_event, 0);
+
+	mutex_init(&state->slot_mutex);
+	mutex_init(&state->recycle_mutex);
+	mutex_init(&state->sync_mutex);
+	mutex_init(&state->bulk_transfer_mutex);
+
+	sema_init(&state->slot_available_event, 0);
+	sema_init(&state->slot_remove_event, 0);
+	sema_init(&state->data_quota_event, 0);
+
+	state->slot_queue_available = 0;
+
+	for (i = 0; i < VCHIQ_MAX_SERVICES; i++) {
+		VCHIQ_SERVICE_QUOTA_T *service_quota =
+			&state->service_quotas[i];
+		sema_init(&service_quota->quota_event, 0);
+	}
+
+	for (i = local->slot_first; i <= local->slot_last; i++) {
+		local->slot_queue[state->slot_queue_available++] = i;
+		up(&state->slot_available_event);
+	}
+
+	state->default_slot_quota = state->slot_queue_available/2;
+	state->default_message_quota =
+		min((unsigned short)(state->default_slot_quota * 256),
+		(unsigned short)~0);
+
+	state->previous_data_index = -1;
+	state->data_use_count = 0;
+	state->data_quota = state->slot_queue_available - 1;
+
+	local->trigger.event = &state->trigger_event;
+	remote_event_create(&local->trigger);
+	local->tx_pos = 0;
+
+	local->recycle.event = &state->recycle_event;
+	remote_event_create(&local->recycle);
+	local->slot_queue_recycle = state->slot_queue_available;
+
+	local->sync_trigger.event = &state->sync_trigger_event;
+	remote_event_create(&local->sync_trigger);
+
+	local->sync_release.event = &state->sync_release_event;
+	remote_event_create(&local->sync_release);
+
+	/* At start-of-day, the slot is empty and available */
+	((VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state, local->slot_sync))->msgid
+		= VCHIQ_MSGID_PADDING;
+	remote_event_signal_local(&local->sync_release);
+
+	local->debug[DEBUG_ENTRIES] = DEBUG_MAX;
+
+	status = vchiq_platform_init_state(state);
+
+	/*
+		bring up slot handler thread
+	 */
+	snprintf(threadname, sizeof(threadname), "VCHIQ-%d", state->id);
+	state->slot_handler_thread = kthread_create(&slot_handler_func,
+		(void *)state,
+		threadname);
+
+	if (state->slot_handler_thread == NULL) {
+		vchiq_loud_error_header();
+		vchiq_loud_error("couldn't create thread %s", threadname);
+		vchiq_loud_error_footer();
+		return VCHIQ_ERROR;
+	}
+	set_user_nice(state->slot_handler_thread, -19);
+	wake_up_process(state->slot_handler_thread);
+
+	snprintf(threadname, sizeof(threadname), "VCHIQr-%d", state->id);
+	state->recycle_thread = kthread_create(&recycle_func,
+		(void *)state,
+		threadname);
+	if (state->recycle_thread == NULL) {
+		vchiq_loud_error_header();
+		vchiq_loud_error("couldn't create thread %s", threadname);
+		vchiq_loud_error_footer();
+		return VCHIQ_ERROR;
+	}
+	set_user_nice(state->recycle_thread, -19);
+	wake_up_process(state->recycle_thread);
+
+	snprintf(threadname, sizeof(threadname), "VCHIQs-%d", state->id);
+	state->sync_thread = kthread_create(&sync_func,
+		(void *)state,
+		threadname);
+	if (state->sync_thread == NULL) {
+		vchiq_loud_error_header();
+		vchiq_loud_error("couldn't create thread %s", threadname);
+		vchiq_loud_error_footer();
+		return VCHIQ_ERROR;
+	}
+	set_user_nice(state->sync_thread, -20);
+	wake_up_process(state->sync_thread);
+
+	BUG_ON(state->id >= VCHIQ_MAX_STATES);
+	vchiq_states[state->id] = state;
+
+	/* Indicate readiness to the other side */
+	local->initialised = 1;
+
+	return status;
+}
+
+/* Called from application thread when a client or server service is created. */
+VCHIQ_SERVICE_T *
+vchiq_add_service_internal(VCHIQ_STATE_T *state,
+	const VCHIQ_SERVICE_PARAMS_T *params, int srvstate,
+	VCHIQ_INSTANCE_T instance, VCHIQ_USERDATA_TERM_T userdata_term)
+{
+	VCHIQ_SERVICE_T *service;
+
+	service = kmalloc(sizeof(VCHIQ_SERVICE_T), GFP_KERNEL);
+	if (service) {
+		service->base.fourcc   = params->fourcc;
+		service->base.callback = params->callback;
+		service->base.userdata = params->userdata;
+		service->handle        = VCHIQ_SERVICE_HANDLE_INVALID;
+		service->ref_count     = 1;
+		service->srvstate      = VCHIQ_SRVSTATE_FREE;
+		service->userdata_term = userdata_term;
+		service->localport     = VCHIQ_PORT_FREE;
+		service->remoteport    = VCHIQ_PORT_FREE;
+
+		service->public_fourcc = (srvstate == VCHIQ_SRVSTATE_OPENING) ?
+			VCHIQ_FOURCC_INVALID : params->fourcc;
+		service->client_id     = 0;
+		service->auto_close    = 1;
+		service->sync          = 0;
+		service->closing       = 0;
+		service->trace         = 0;
+		atomic_set(&service->poll_flags, 0);
+		service->version       = params->version;
+		service->version_min   = params->version_min;
+		service->state         = state;
+		service->instance      = instance;
+		service->service_use_count = 0;
+		init_bulk_queue(&service->bulk_tx);
+		init_bulk_queue(&service->bulk_rx);
+		sema_init(&service->remove_event, 0);
+		sema_init(&service->bulk_remove_event, 0);
+		mutex_init(&service->bulk_mutex);
+		memset(&service->stats, 0, sizeof(service->stats));
+	} else {
+		vchiq_log_error(vchiq_core_log_level,
+			"Out of memory");
+	}
+
+	if (service) {
+		VCHIQ_SERVICE_T **pservice = NULL;
+		int i;
+
+		/* Although it is perfectly possible to use service_spinlock
+		** to protect the creation of services, it is overkill as it
+		** disables interrupts while the array is searched.
+		** The only danger is of another thread trying to create a
+		** service - service deletion is safe.
+		** Therefore it is preferable to use state->mutex which,
+		** although slower to claim, doesn't block interrupts while
+		** it is held.
+		*/
+
+		mutex_lock(&state->mutex);
+
+		/* Prepare to use a previously unused service */
+		if (state->unused_service < VCHIQ_MAX_SERVICES)
+			pservice = &state->services[state->unused_service];
+
+		if (srvstate == VCHIQ_SRVSTATE_OPENING) {
+			for (i = 0; i < state->unused_service; i++) {
+				VCHIQ_SERVICE_T *srv = state->services[i];
+				if (!srv) {
+					pservice = &state->services[i];
+					break;
+				}
+			}
+		} else {
+			for (i = (state->unused_service - 1); i >= 0; i--) {
+				VCHIQ_SERVICE_T *srv = state->services[i];
+				if (!srv)
+					pservice = &state->services[i];
+				else if ((srv->public_fourcc == params->fourcc)
+					&& ((srv->instance != instance) ||
+					(srv->base.callback !=
+					params->callback))) {
+					/* There is another server using this
+					** fourcc which doesn't match. */
+					pservice = NULL;
+					break;
+				}
+			}
+		}
+
+		if (pservice) {
+			service->localport = (pservice - state->services);
+			if (!handle_seq)
+				handle_seq = VCHIQ_MAX_STATES *
+					 VCHIQ_MAX_SERVICES;
+			service->handle = handle_seq |
+				(state->id * VCHIQ_MAX_SERVICES) |
+				service->localport;
+			handle_seq += VCHIQ_MAX_STATES * VCHIQ_MAX_SERVICES;
+			*pservice = service;
+			if (pservice == &state->services[state->unused_service])
+				state->unused_service++;
+		}
+
+		mutex_unlock(&state->mutex);
+
+		if (!pservice) {
+			kfree(service);
+			service = NULL;
+		}
+	}
+
+	if (service) {
+		VCHIQ_SERVICE_QUOTA_T *service_quota =
+			&state->service_quotas[service->localport];
+		service_quota->slot_quota = state->default_slot_quota;
+		service_quota->message_quota = state->default_message_quota;
+		if (service_quota->slot_use_count == 0)
+			service_quota->previous_tx_index =
+				SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos)
+				- 1;
+
+		/* Bring this service online */
+		vchiq_set_service_state(service, srvstate);
+
+		vchiq_log_info(vchiq_core_msg_log_level,
+			"%s Service %c%c%c%c SrcPort:%d",
+			(srvstate == VCHIQ_SRVSTATE_OPENING)
+			? "Open" : "Add",
+			VCHIQ_FOURCC_AS_4CHARS(params->fourcc),
+			service->localport);
+	}
+
+	/* Don't unlock the service - leave it with a ref_count of 1. */
+
+	return service;
+}
+
+VCHIQ_STATUS_T
+vchiq_open_service_internal(VCHIQ_SERVICE_T *service, int client_id)
+{
+	struct vchiq_open_payload payload = {
+		service->base.fourcc,
+		client_id,
+		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);
+	if (status == VCHIQ_SUCCESS) {
+		/* Wait for the ACK/NAK */
+		if (down_interruptible(&service->remove_event) != 0) {
+			status = VCHIQ_RETRY;
+			vchiq_release_service_internal(service);
+		} else if ((service->srvstate != VCHIQ_SRVSTATE_OPEN) &&
+			(service->srvstate != VCHIQ_SRVSTATE_OPENSYNC)) {
+			if (service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT)
+				vchiq_log_error(vchiq_core_log_level,
+					"%d: osi - srvstate = %s (ref %d)",
+					service->state->id,
+					srvstate_names[service->srvstate],
+					service->ref_count);
+			status = VCHIQ_ERROR;
+			VCHIQ_SERVICE_STATS_INC(service, error_count);
+			vchiq_release_service_internal(service);
+		}
+	}
+	return status;
+}
+
+static void
+release_service_messages(VCHIQ_SERVICE_T *service)
+{
+	VCHIQ_STATE_T *state = service->state;
+	int slot_last = state->remote->slot_last;
+	int i;
+
+	/* Release any claimed messages aimed at this service */
+
+	if (service->sync) {
+		VCHIQ_HEADER_T *header =
+			(VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state,
+						state->remote->slot_sync);
+		if (VCHIQ_MSG_DSTPORT(header->msgid) == service->localport)
+			release_message_sync(state, header);
+
+		return;
+	}
+
+	for (i = state->remote->slot_first; i <= slot_last; i++) {
+		VCHIQ_SLOT_INFO_T *slot_info =
+			SLOT_INFO_FROM_INDEX(state, i);
+		if (slot_info->release_count != slot_info->use_count) {
+			char *data =
+				(char *)SLOT_DATA_FROM_INDEX(state, i);
+			unsigned int pos, end;
+
+			end = VCHIQ_SLOT_SIZE;
+			if (data == state->rx_data)
+				/* This buffer is still being read from - stop
+				** at the current read position */
+				end = state->rx_pos & VCHIQ_SLOT_MASK;
+
+			pos = 0;
+
+			while (pos < end) {
+				VCHIQ_HEADER_T *header =
+					(VCHIQ_HEADER_T *)(data + pos);
+				int msgid = header->msgid;
+				int port = VCHIQ_MSG_DSTPORT(msgid);
+				if ((port == service->localport) &&
+					(msgid & VCHIQ_MSGID_CLAIMED)) {
+					vchiq_log_info(vchiq_core_log_level,
+						"  fsi - hdr %x",
+						(unsigned int)header);
+					release_slot(state, slot_info, header,
+						NULL);
+				}
+				pos += calc_stride(header->size);
+				if (pos > VCHIQ_SLOT_SIZE) {
+					vchiq_log_error(vchiq_core_log_level,
+						"fsi - pos %x: header %x, "
+						"msgid %x, header->msgid %x, "
+						"header->size %x",
+						pos, (unsigned int)header,
+						msgid, header->msgid,
+						header->size);
+					WARN(1, "invalid slot position\n");
+				}
+			}
+		}
+	}
+}
+
+static int
+do_abort_bulks(VCHIQ_SERVICE_T *service)
+{
+	VCHIQ_STATUS_T status;
+
+	/* Abort any outstanding bulk transfers */
+	if (mutex_lock_interruptible(&service->bulk_mutex) != 0)
+		return 0;
+	abort_outstanding_bulks(service, &service->bulk_tx);
+	abort_outstanding_bulks(service, &service->bulk_rx);
+	mutex_unlock(&service->bulk_mutex);
+
+	status = notify_bulks(service, &service->bulk_tx, 0/*!retry_poll*/);
+	if (status == VCHIQ_SUCCESS)
+		status = notify_bulks(service, &service->bulk_rx,
+			0/*!retry_poll*/);
+	return (status == VCHIQ_SUCCESS);
+}
+
+static VCHIQ_STATUS_T
+close_service_complete(VCHIQ_SERVICE_T *service, int failstate)
+{
+	VCHIQ_STATUS_T status;
+	int is_server = (service->public_fourcc != VCHIQ_FOURCC_INVALID);
+	int newstate;
+
+	switch (service->srvstate) {
+	case VCHIQ_SRVSTATE_OPEN:
+	case VCHIQ_SRVSTATE_CLOSESENT:
+	case VCHIQ_SRVSTATE_CLOSERECVD:
+		if (is_server) {
+			if (service->auto_close) {
+				service->client_id = 0;
+				service->remoteport = VCHIQ_PORT_FREE;
+				newstate = VCHIQ_SRVSTATE_LISTENING;
+			} else
+				newstate = VCHIQ_SRVSTATE_CLOSEWAIT;
+		} else
+			newstate = VCHIQ_SRVSTATE_CLOSED;
+		vchiq_set_service_state(service, newstate);
+		break;
+	case VCHIQ_SRVSTATE_LISTENING:
+		break;
+	default:
+		vchiq_log_error(vchiq_core_log_level,
+			"close_service_complete(%x) called in state %s",
+			service->handle, srvstate_names[service->srvstate]);
+		WARN(1, "close_service_complete in unexpected state\n");
+		return VCHIQ_ERROR;
+	}
+
+	status = make_service_callback(service,
+		VCHIQ_SERVICE_CLOSED, NULL, NULL);
+
+	if (status != VCHIQ_RETRY) {
+		int uc = service->service_use_count;
+		int i;
+		/* Complete the close process */
+		for (i = 0; i < uc; i++)
+			/* cater for cases where close is forced and the
+			** client may not close all it's handles */
+			vchiq_release_service_internal(service);
+
+		service->client_id = 0;
+		service->remoteport = VCHIQ_PORT_FREE;
+
+		if (service->srvstate == VCHIQ_SRVSTATE_CLOSED)
+			vchiq_free_service_internal(service);
+		else if (service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) {
+			if (is_server)
+				service->closing = 0;
+
+			up(&service->remove_event);
+		}
+	} else
+		vchiq_set_service_state(service, failstate);
+
+	return status;
+}
+
+/* Called by the slot handler */
+VCHIQ_STATUS_T
+vchiq_close_service_internal(VCHIQ_SERVICE_T *service, int close_recvd)
+{
+	VCHIQ_STATE_T *state = service->state;
+	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+	int is_server = (service->public_fourcc != VCHIQ_FOURCC_INVALID);
+
+	vchiq_log_info(vchiq_core_log_level, "%d: csi:%d,%d (%s)",
+		service->state->id, service->localport, close_recvd,
+		srvstate_names[service->srvstate]);
+
+	switch (service->srvstate) {
+	case VCHIQ_SRVSTATE_CLOSED:
+	case VCHIQ_SRVSTATE_HIDDEN:
+	case VCHIQ_SRVSTATE_LISTENING:
+	case VCHIQ_SRVSTATE_CLOSEWAIT:
+		if (close_recvd)
+			vchiq_log_error(vchiq_core_log_level,
+				"vchiq_close_service_internal(1) called "
+				"in state %s",
+				srvstate_names[service->srvstate]);
+		else if (is_server) {
+			if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) {
+				status = VCHIQ_ERROR;
+			} else {
+				service->client_id = 0;
+				service->remoteport = VCHIQ_PORT_FREE;
+				if (service->srvstate ==
+					VCHIQ_SRVSTATE_CLOSEWAIT)
+					vchiq_set_service_state(service,
+						VCHIQ_SRVSTATE_LISTENING);
+			}
+			up(&service->remove_event);
+		} else
+			vchiq_free_service_internal(service);
+		break;
+	case VCHIQ_SRVSTATE_OPENING:
+		if (close_recvd) {
+			/* The open was rejected - tell the user */
+			vchiq_set_service_state(service,
+				VCHIQ_SRVSTATE_CLOSEWAIT);
+			up(&service->remove_event);
+		} else {
+			/* Shutdown mid-open - let the other side know */
+			status = queue_message(state, service,
+				VCHIQ_MAKE_MSG
+				(VCHIQ_MSG_CLOSE,
+				service->localport,
+				VCHIQ_MSG_DSTPORT(service->remoteport)),
+				NULL, 0, 0, 0);
+		}
+		break;
+
+	case VCHIQ_SRVSTATE_OPENSYNC:
+		mutex_lock(&state->sync_mutex);
+		/* Drop through */
+
+	case VCHIQ_SRVSTATE_OPEN:
+		if (state->is_master || close_recvd) {
+			if (!do_abort_bulks(service))
+				status = VCHIQ_RETRY;
+		}
+
+		release_service_messages(service);
+
+		if (status == VCHIQ_SUCCESS)
+			status = queue_message(state, service,
+				VCHIQ_MAKE_MSG
+				(VCHIQ_MSG_CLOSE,
+				service->localport,
+				VCHIQ_MSG_DSTPORT(service->remoteport)),
+				NULL, 0, 0, QMFLAGS_NO_MUTEX_UNLOCK);
+
+		if (status == VCHIQ_SUCCESS) {
+			if (!close_recvd) {
+				/* Change the state while the mutex is
+				   still held */
+				vchiq_set_service_state(service,
+							VCHIQ_SRVSTATE_CLOSESENT);
+				mutex_unlock(&state->slot_mutex);
+				if (service->sync)
+					mutex_unlock(&state->sync_mutex);
+				break;
+			}
+		} else if (service->srvstate == VCHIQ_SRVSTATE_OPENSYNC) {
+			mutex_unlock(&state->sync_mutex);
+			break;
+		} else
+			break;
+
+		/* Change the state while the mutex is still held */
+		vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSERECVD);
+		mutex_unlock(&state->slot_mutex);
+		if (service->sync)
+			mutex_unlock(&state->sync_mutex);
+
+		status = close_service_complete(service,
+				VCHIQ_SRVSTATE_CLOSERECVD);
+		break;
+
+	case VCHIQ_SRVSTATE_CLOSESENT:
+		if (!close_recvd)
+			/* This happens when a process is killed mid-close */
+			break;
+
+		if (!state->is_master) {
+			if (!do_abort_bulks(service)) {
+				status = VCHIQ_RETRY;
+				break;
+			}
+		}
+
+		if (status == VCHIQ_SUCCESS)
+			status = close_service_complete(service,
+				VCHIQ_SRVSTATE_CLOSERECVD);
+		break;
+
+	case VCHIQ_SRVSTATE_CLOSERECVD:
+		if (!close_recvd && is_server)
+			/* Force into LISTENING mode */
+			vchiq_set_service_state(service,
+				VCHIQ_SRVSTATE_LISTENING);
+		status = close_service_complete(service,
+			VCHIQ_SRVSTATE_CLOSERECVD);
+		break;
+
+	default:
+		vchiq_log_error(vchiq_core_log_level,
+			"vchiq_close_service_internal(%d) called in state %s",
+			close_recvd, srvstate_names[service->srvstate]);
+		break;
+	}
+
+	return status;
+}
+
+/* Called from the application process upon process death */
+void
+vchiq_terminate_service_internal(VCHIQ_SERVICE_T *service)
+{
+	VCHIQ_STATE_T *state = service->state;
+
+	vchiq_log_info(vchiq_core_log_level, "%d: tsi - (%d<->%d)",
+		state->id, service->localport, service->remoteport);
+
+	mark_service_closing(service);
+
+	/* Mark the service for removal by the slot handler */
+	request_poll(state, service, VCHIQ_POLL_REMOVE);
+}
+
+/* Called from the slot handler */
+void
+vchiq_free_service_internal(VCHIQ_SERVICE_T *service)
+{
+	VCHIQ_STATE_T *state = service->state;
+
+	vchiq_log_info(vchiq_core_log_level, "%d: fsi - (%d)",
+		state->id, service->localport);
+
+	switch (service->srvstate) {
+	case VCHIQ_SRVSTATE_OPENING:
+	case VCHIQ_SRVSTATE_CLOSED:
+	case VCHIQ_SRVSTATE_HIDDEN:
+	case VCHIQ_SRVSTATE_LISTENING:
+	case VCHIQ_SRVSTATE_CLOSEWAIT:
+		break;
+	default:
+		vchiq_log_error(vchiq_core_log_level,
+			"%d: fsi - (%d) in state %s",
+			state->id, service->localport,
+			srvstate_names[service->srvstate]);
+		return;
+	}
+
+	vchiq_set_service_state(service, VCHIQ_SRVSTATE_FREE);
+
+	up(&service->remove_event);
+
+	/* Release the initial lock */
+	unlock_service(service);
+}
+
+VCHIQ_STATUS_T
+vchiq_connect_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance)
+{
+	VCHIQ_SERVICE_T *service;
+	int i;
+
+	/* Find all services registered to this client and enable them. */
+	i = 0;
+	while ((service = next_service_by_instance(state, instance,
+		&i)) !=	NULL) {
+		if (service->srvstate == VCHIQ_SRVSTATE_HIDDEN)
+			vchiq_set_service_state(service,
+				VCHIQ_SRVSTATE_LISTENING);
+		unlock_service(service);
+	}
+
+	if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED) {
+		if (queue_message(state, NULL,
+			VCHIQ_MAKE_MSG(VCHIQ_MSG_CONNECT, 0, 0), NULL, 0,
+			0, QMFLAGS_IS_BLOCKING) == VCHIQ_RETRY)
+			return VCHIQ_RETRY;
+
+		vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTING);
+	}
+
+	if (state->conn_state == VCHIQ_CONNSTATE_CONNECTING) {
+		if (down_interruptible(&state->connect) != 0)
+			return VCHIQ_RETRY;
+
+		vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
+		up(&state->connect);
+	}
+
+	return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_shutdown_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance)
+{
+	VCHIQ_SERVICE_T *service;
+	int i;
+
+	/* Find all services registered to this client and enable them. */
+	i = 0;
+	while ((service = next_service_by_instance(state, instance,
+		&i)) !=	NULL) {
+		(void)vchiq_remove_service(service->handle);
+		unlock_service(service);
+	}
+
+	return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_pause_internal(VCHIQ_STATE_T *state)
+{
+	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+	switch (state->conn_state) {
+	case VCHIQ_CONNSTATE_CONNECTED:
+		/* Request a pause */
+		vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSING);
+		request_poll(state, NULL, 0);
+		break;
+	default:
+		vchiq_log_error(vchiq_core_log_level,
+			"vchiq_pause_internal in state %s\n",
+			conn_state_names[state->conn_state]);
+		status = VCHIQ_ERROR;
+		VCHIQ_STATS_INC(state, error_count);
+		break;
+	}
+
+	return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_resume_internal(VCHIQ_STATE_T *state)
+{
+	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+	if (state->conn_state == VCHIQ_CONNSTATE_PAUSED) {
+		vchiq_set_conn_state(state, VCHIQ_CONNSTATE_RESUMING);
+		request_poll(state, NULL, 0);
+	} else {
+		status = VCHIQ_ERROR;
+		VCHIQ_STATS_INC(state, error_count);
+	}
+
+	return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_close_service(VCHIQ_SERVICE_HANDLE_T handle)
+{
+	/* Unregister the service */
+	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+	if (!service)
+		return VCHIQ_ERROR;
+
+	vchiq_log_info(vchiq_core_log_level,
+		"%d: close_service:%d",
+		service->state->id, service->localport);
+
+	if ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
+		(service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
+		(service->srvstate == VCHIQ_SRVSTATE_HIDDEN)) {
+		unlock_service(service);
+		return VCHIQ_ERROR;
+	}
+
+	mark_service_closing(service);
+
+	if (current == service->state->slot_handler_thread) {
+		status = vchiq_close_service_internal(service,
+			0/*!close_recvd*/);
+		BUG_ON(status == VCHIQ_RETRY);
+	} else {
+	/* Mark the service for termination by the slot handler */
+		request_poll(service->state, service, VCHIQ_POLL_TERMINATE);
+	}
+
+	while (1) {
+		if (down_interruptible(&service->remove_event) != 0) {
+			status = VCHIQ_RETRY;
+			break;
+		}
+
+		if ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
+			(service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
+			(service->srvstate == VCHIQ_SRVSTATE_OPEN))
+			break;
+
+		vchiq_log_warning(vchiq_core_log_level,
+			"%d: close_service:%d - waiting in state %s",
+			service->state->id, service->localport,
+			srvstate_names[service->srvstate]);
+	}
+
+	if ((status == VCHIQ_SUCCESS) &&
+		(service->srvstate != VCHIQ_SRVSTATE_FREE) &&
+		(service->srvstate != VCHIQ_SRVSTATE_LISTENING))
+		status = VCHIQ_ERROR;
+
+	unlock_service(service);
+
+	return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_remove_service(VCHIQ_SERVICE_HANDLE_T handle)
+{
+	/* Unregister the service */
+	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
+	if (!service)
+		return VCHIQ_ERROR;
+
+	vchiq_log_info(vchiq_core_log_level,
+		"%d: remove_service:%d",
+		service->state->id, service->localport);
+
+	if (service->srvstate == VCHIQ_SRVSTATE_FREE) {
+		unlock_service(service);
+		return VCHIQ_ERROR;
+	}
+
+	mark_service_closing(service);
+
+	if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) ||
+		(current == service->state->slot_handler_thread)) {
+		/* Make it look like a client, because it must be removed and
+		   not left in the LISTENING state. */
+		service->public_fourcc = VCHIQ_FOURCC_INVALID;
+
+		status = vchiq_close_service_internal(service,
+			0/*!close_recvd*/);
+		BUG_ON(status == VCHIQ_RETRY);
+	} else {
+		/* Mark the service for removal by the slot handler */
+		request_poll(service->state, service, VCHIQ_POLL_REMOVE);
+	}
+	while (1) {
+		if (down_interruptible(&service->remove_event) != 0) {
+			status = VCHIQ_RETRY;
+			break;
+		}
+
+		if ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
+			(service->srvstate == VCHIQ_SRVSTATE_OPEN))
+			break;
+
+		vchiq_log_warning(vchiq_core_log_level,
+			"%d: remove_service:%d - waiting in state %s",
+			service->state->id, service->localport,
+			srvstate_names[service->srvstate]);
+	}
+
+	if ((status == VCHIQ_SUCCESS) &&
+		(service->srvstate != VCHIQ_SRVSTATE_FREE))
+		status = VCHIQ_ERROR;
+
+	unlock_service(service);
+
+	return status;
+}
+
+
+/* This function may be called by kernel threads or user threads.
+ * User threads may receive VCHIQ_RETRY to indicate that a signal has been
+ * received and the call should be retried after being returned to user
+ * context.
+ * When called in blocking mode, the userdata field points to a bulk_waiter
+ * structure.
+ */
+VCHIQ_STATUS_T
+vchiq_bulk_transfer(VCHIQ_SERVICE_HANDLE_T handle,
+	VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata,
+	VCHIQ_BULK_MODE_T mode, VCHIQ_BULK_DIR_T dir)
+{
+	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+	VCHIQ_BULK_QUEUE_T *queue;
+	VCHIQ_BULK_T *bulk;
+	VCHIQ_STATE_T *state;
+	struct bulk_waiter *bulk_waiter = NULL;
+	const char dir_char = (dir == VCHIQ_BULK_TRANSMIT) ? 't' : 'r';
+	const int dir_msgtype = (dir == VCHIQ_BULK_TRANSMIT) ?
+		VCHIQ_MSG_BULK_TX : VCHIQ_MSG_BULK_RX;
+	VCHIQ_STATUS_T status = VCHIQ_ERROR;
+
+	if (!service ||
+		 (service->srvstate != VCHIQ_SRVSTATE_OPEN) ||
+		 ((memhandle == VCHI_MEM_HANDLE_INVALID) && (offset == NULL)) ||
+		 (vchiq_check_service(service) != VCHIQ_SUCCESS))
+		goto error_exit;
+
+	switch (mode) {
+	case VCHIQ_BULK_MODE_NOCALLBACK:
+	case VCHIQ_BULK_MODE_CALLBACK:
+		break;
+	case VCHIQ_BULK_MODE_BLOCKING:
+		bulk_waiter = (struct bulk_waiter *)userdata;
+		sema_init(&bulk_waiter->event, 0);
+		bulk_waiter->actual = 0;
+		bulk_waiter->bulk = NULL;
+		break;
+	case VCHIQ_BULK_MODE_WAITING:
+		bulk_waiter = (struct bulk_waiter *)userdata;
+		bulk = bulk_waiter->bulk;
+		goto waiting;
+	default:
+		goto error_exit;
+	}
+
+	state = service->state;
+
+	queue = (dir == VCHIQ_BULK_TRANSMIT) ?
+		&service->bulk_tx : &service->bulk_rx;
+
+	if (mutex_lock_interruptible(&service->bulk_mutex) != 0) {
+		status = VCHIQ_RETRY;
+		goto error_exit;
+	}
+
+	if (queue->local_insert == queue->remove + VCHIQ_NUM_SERVICE_BULKS) {
+		VCHIQ_SERVICE_STATS_INC(service, bulk_stalls);
+		do {
+			mutex_unlock(&service->bulk_mutex);
+			if (down_interruptible(&service->bulk_remove_event)
+				!= 0) {
+				status = VCHIQ_RETRY;
+				goto error_exit;
+			}
+			if (mutex_lock_interruptible(&service->bulk_mutex)
+				!= 0) {
+				status = VCHIQ_RETRY;
+				goto error_exit;
+			}
+		} while (queue->local_insert == queue->remove +
+				VCHIQ_NUM_SERVICE_BULKS);
+	}
+
+	bulk = &queue->bulks[BULK_INDEX(queue->local_insert)];
+
+	bulk->mode = mode;
+	bulk->dir = dir;
+	bulk->userdata = userdata;
+	bulk->size = size;
+	bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED;
+
+	if (vchiq_prepare_bulk_data(bulk, memhandle, offset, size, dir) !=
+		VCHIQ_SUCCESS)
+		goto unlock_error_exit;
+
+	wmb();
+
+	vchiq_log_info(vchiq_core_log_level,
+		"%d: bt (%d->%d) %cx %x@%x %x",
+		state->id,
+		service->localport, service->remoteport, dir_char,
+		size, (unsigned int)bulk->data, (unsigned int)userdata);
+
+	/* The slot mutex must be held when the service is being closed, so
+	   claim it here to ensure that isn't happening */
+	if (mutex_lock_interruptible(&state->slot_mutex) != 0) {
+		status = VCHIQ_RETRY;
+		goto cancel_bulk_error_exit;
+	}
+
+	if (service->srvstate != VCHIQ_SRVSTATE_OPEN)
+		goto unlock_both_error_exit;
+
+	if (state->is_master) {
+		queue->local_insert++;
+		if (resolve_bulks(service, queue))
+			request_poll(state, service,
+				(dir == VCHIQ_BULK_TRANSMIT) ?
+				VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY);
+	} else {
+		int payload[2] = { (int)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);
+		if (status != VCHIQ_SUCCESS) {
+			goto unlock_both_error_exit;
+		}
+		queue->local_insert++;
+	}
+
+	mutex_unlock(&state->slot_mutex);
+	mutex_unlock(&service->bulk_mutex);
+
+	vchiq_log_trace(vchiq_core_log_level,
+		"%d: bt:%d %cx li=%x ri=%x p=%x",
+		state->id,
+		service->localport, dir_char,
+		queue->local_insert, queue->remote_insert, queue->process);
+
+waiting:
+	unlock_service(service);
+
+	status = VCHIQ_SUCCESS;
+
+	if (bulk_waiter) {
+		bulk_waiter->bulk = bulk;
+		if (down_interruptible(&bulk_waiter->event) != 0)
+			status = VCHIQ_RETRY;
+		else if (bulk_waiter->actual == VCHIQ_BULK_ACTUAL_ABORTED)
+			status = VCHIQ_ERROR;
+	}
+
+	return status;
+
+unlock_both_error_exit:
+	mutex_unlock(&state->slot_mutex);
+cancel_bulk_error_exit:
+	vchiq_complete_bulk(bulk);
+unlock_error_exit:
+	mutex_unlock(&service->bulk_mutex);
+
+error_exit:
+	if (service)
+		unlock_service(service);
+	return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_queue_message(VCHIQ_SERVICE_HANDLE_T handle,
+	const VCHIQ_ELEMENT_T *elements, unsigned int count)
+{
+	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_MAX_MSG_SIZE) {
+		VCHIQ_SERVICE_STATS_INC(service, error_count);
+		goto error_exit;
+	}
+
+	switch (service->srvstate) {
+	case VCHIQ_SRVSTATE_OPEN:
+		status = queue_message(service->state, service,
+				VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA,
+					service->localport,
+					service->remoteport),
+				elements, count, 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);
+		break;
+	default:
+		status = VCHIQ_ERROR;
+		break;
+	}
+
+error_exit:
+	if (service)
+		unlock_service(service);
+
+	return status;
+}
+
+void
+vchiq_release_message(VCHIQ_SERVICE_HANDLE_T handle, VCHIQ_HEADER_T *header)
+{
+	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+	VCHIQ_SHARED_STATE_T *remote;
+	VCHIQ_STATE_T *state;
+	int slot_index;
+
+	if (!service)
+		return;
+
+	state = service->state;
+	remote = state->remote;
+
+	slot_index = SLOT_INDEX_FROM_DATA(state, (void *)header);
+
+	if ((slot_index >= remote->slot_first) &&
+		(slot_index <= remote->slot_last)) {
+		int msgid = header->msgid;
+		if (msgid & VCHIQ_MSGID_CLAIMED) {
+			VCHIQ_SLOT_INFO_T *slot_info =
+				SLOT_INFO_FROM_INDEX(state, slot_index);
+
+			release_slot(state, slot_info, header, service);
+		}
+	} else if (slot_index == remote->slot_sync)
+		release_message_sync(state, header);
+
+	unlock_service(service);
+}
+
+static void
+release_message_sync(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header)
+{
+	header->msgid = VCHIQ_MSGID_PADDING;
+	wmb();
+	remote_event_signal(&state->remote->sync_release);
+}
+
+VCHIQ_STATUS_T
+vchiq_get_peer_version(VCHIQ_SERVICE_HANDLE_T handle, short *peer_version)
+{
+   VCHIQ_STATUS_T status = VCHIQ_ERROR;
+   VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+
+   if (!service ||
+      (vchiq_check_service(service) != VCHIQ_SUCCESS) ||
+      !peer_version)
+      goto exit;
+   *peer_version = service->peer_version;
+   status = VCHIQ_SUCCESS;
+
+exit:
+   if (service)
+      unlock_service(service);
+   return status;
+}
+
+VCHIQ_STATUS_T
+vchiq_get_config(VCHIQ_INSTANCE_T instance,
+	int config_size, VCHIQ_CONFIG_T *pconfig)
+{
+	VCHIQ_CONFIG_T config;
+
+	(void)instance;
+
+	config.max_msg_size           = VCHIQ_MAX_MSG_SIZE;
+	config.bulk_threshold         = VCHIQ_MAX_MSG_SIZE;
+	config.max_outstanding_bulks  = VCHIQ_NUM_SERVICE_BULKS;
+	config.max_services           = VCHIQ_MAX_SERVICES;
+	config.version                = VCHIQ_VERSION;
+	config.version_min            = VCHIQ_VERSION_MIN;
+
+	if (config_size > sizeof(VCHIQ_CONFIG_T))
+		return VCHIQ_ERROR;
+
+	memcpy(pconfig, &config,
+		min(config_size, (int)(sizeof(VCHIQ_CONFIG_T))));
+
+	return VCHIQ_SUCCESS;
+}
+
+VCHIQ_STATUS_T
+vchiq_set_service_option(VCHIQ_SERVICE_HANDLE_T handle,
+	VCHIQ_SERVICE_OPTION_T option, int value)
+{
+	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+	VCHIQ_STATUS_T status = VCHIQ_ERROR;
+
+	if (service) {
+		switch (option) {
+		case VCHIQ_SERVICE_OPTION_AUTOCLOSE:
+			service->auto_close = value;
+			status = VCHIQ_SUCCESS;
+			break;
+
+		case VCHIQ_SERVICE_OPTION_SLOT_QUOTA: {
+			VCHIQ_SERVICE_QUOTA_T *service_quota =
+				&service->state->service_quotas[
+					service->localport];
+			if (value == 0)
+				value = service->state->default_slot_quota;
+			if ((value >= service_quota->slot_use_count) &&
+				 (value < (unsigned short)~0)) {
+				service_quota->slot_quota = value;
+				if ((value >= service_quota->slot_use_count) &&
+					(service_quota->message_quota >=
+					 service_quota->message_use_count)) {
+					/* Signal the service that it may have
+					** dropped below its quota */
+					up(&service_quota->quota_event);
+				}
+				status = VCHIQ_SUCCESS;
+			}
+		} break;
+
+		case VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA: {
+			VCHIQ_SERVICE_QUOTA_T *service_quota =
+				&service->state->service_quotas[
+					service->localport];
+			if (value == 0)
+				value = service->state->default_message_quota;
+			if ((value >= service_quota->message_use_count) &&
+				 (value < (unsigned short)~0)) {
+				service_quota->message_quota = value;
+				if ((value >=
+					service_quota->message_use_count) &&
+					(service_quota->slot_quota >=
+					service_quota->slot_use_count))
+					/* Signal the service that it may have
+					** dropped below its quota */
+					up(&service_quota->quota_event);
+				status = VCHIQ_SUCCESS;
+			}
+		} break;
+
+		case VCHIQ_SERVICE_OPTION_SYNCHRONOUS:
+			if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) ||
+				(service->srvstate ==
+				VCHIQ_SRVSTATE_LISTENING)) {
+				service->sync = value;
+				status = VCHIQ_SUCCESS;
+			}
+			break;
+
+		case VCHIQ_SERVICE_OPTION_TRACE:
+			service->trace = value;
+			status = VCHIQ_SUCCESS;
+			break;
+
+		default:
+			break;
+		}
+		unlock_service(service);
+	}
+
+	return status;
+}
+
+void
+vchiq_dump_shared_state(void *dump_context, VCHIQ_STATE_T *state,
+	VCHIQ_SHARED_STATE_T *shared, const char *label)
+{
+	static const char *const debug_names[] = {
+		"<entries>",
+		"SLOT_HANDLER_COUNT",
+		"SLOT_HANDLER_LINE",
+		"PARSE_LINE",
+		"PARSE_HEADER",
+		"PARSE_MSGID",
+		"AWAIT_COMPLETION_LINE",
+		"DEQUEUE_MESSAGE_LINE",
+		"SERVICE_CALLBACK_LINE",
+		"MSG_QUEUE_FULL_COUNT",
+		"COMPLETION_QUEUE_FULL_COUNT"
+	};
+	int i;
+
+	char buf[80];
+	int len;
+	len = snprintf(buf, sizeof(buf),
+		"  %s: slots %d-%d tx_pos=%x recycle=%x",
+		label, shared->slot_first, shared->slot_last,
+		shared->tx_pos, shared->slot_queue_recycle);
+	vchiq_dump(dump_context, buf, len + 1);
+
+	len = snprintf(buf, sizeof(buf),
+		"    Slots claimed:");
+	vchiq_dump(dump_context, buf, len + 1);
+
+	for (i = shared->slot_first; i <= shared->slot_last; i++) {
+		VCHIQ_SLOT_INFO_T slot_info = *SLOT_INFO_FROM_INDEX(state, i);
+		if (slot_info.use_count != slot_info.release_count) {
+			len = snprintf(buf, sizeof(buf),
+				"      %d: %d/%d", i, slot_info.use_count,
+				slot_info.release_count);
+			vchiq_dump(dump_context, buf, len + 1);
+		}
+	}
+
+	for (i = 1; i < shared->debug[DEBUG_ENTRIES]; i++) {
+		len = snprintf(buf, sizeof(buf), "    DEBUG: %s = %d(%x)",
+			debug_names[i], shared->debug[i], shared->debug[i]);
+		vchiq_dump(dump_context, buf, len + 1);
+	}
+}
+
+void
+vchiq_dump_state(void *dump_context, VCHIQ_STATE_T *state)
+{
+	char buf[80];
+	int len;
+	int i;
+
+	len = snprintf(buf, sizeof(buf), "State %d: %s", state->id,
+		conn_state_names[state->conn_state]);
+	vchiq_dump(dump_context, buf, len + 1);
+
+	len = snprintf(buf, sizeof(buf),
+		"  tx_pos=%x(@%x), rx_pos=%x(@%x)",
+		state->local->tx_pos,
+		(uint32_t)state->tx_data +
+			(state->local_tx_pos & VCHIQ_SLOT_MASK),
+		state->rx_pos,
+		(uint32_t)state->rx_data +
+			(state->rx_pos & VCHIQ_SLOT_MASK));
+	vchiq_dump(dump_context, buf, len + 1);
+
+	len = snprintf(buf, sizeof(buf),
+		"  Version: %d (min %d)",
+		VCHIQ_VERSION, VCHIQ_VERSION_MIN);
+	vchiq_dump(dump_context, buf, len + 1);
+
+	if (VCHIQ_ENABLE_STATS) {
+		len = snprintf(buf, sizeof(buf),
+			"  Stats: ctrl_tx_count=%d, ctrl_rx_count=%d, "
+			"error_count=%d",
+			state->stats.ctrl_tx_count, state->stats.ctrl_rx_count,
+			state->stats.error_count);
+		vchiq_dump(dump_context, buf, len + 1);
+	}
+
+	len = snprintf(buf, sizeof(buf),
+		"  Slots: %d available (%d data), %d recyclable, %d stalls "
+		"(%d data)",
+		((state->slot_queue_available * VCHIQ_SLOT_SIZE) -
+			state->local_tx_pos) / VCHIQ_SLOT_SIZE,
+		state->data_quota - state->data_use_count,
+		state->local->slot_queue_recycle - state->slot_queue_available,
+		state->stats.slot_stalls, state->stats.data_stalls);
+	vchiq_dump(dump_context, buf, len + 1);
+
+	vchiq_dump_platform_state(dump_context);
+
+	vchiq_dump_shared_state(dump_context, state, state->local, "Local");
+	vchiq_dump_shared_state(dump_context, state, state->remote, "Remote");
+
+	vchiq_dump_platform_instances(dump_context);
+
+	for (i = 0; i < state->unused_service; i++) {
+		VCHIQ_SERVICE_T *service = find_service_by_port(state, i);
+
+		if (service) {
+			vchiq_dump_service_state(dump_context, service);
+			unlock_service(service);
+		}
+	}
+}
+
+void
+vchiq_dump_service_state(void *dump_context, VCHIQ_SERVICE_T *service)
+{
+	char buf[80];
+	int len;
+
+	len = snprintf(buf, sizeof(buf), "Service %d: %s (ref %u)",
+		service->localport, srvstate_names[service->srvstate],
+		service->ref_count - 1); /*Don't include the lock just taken*/
+
+	if (service->srvstate != VCHIQ_SRVSTATE_FREE) {
+		char remoteport[30];
+		VCHIQ_SERVICE_QUOTA_T *service_quota =
+			&service->state->service_quotas[service->localport];
+		int fourcc = service->base.fourcc;
+		int tx_pending, rx_pending;
+		if (service->remoteport != VCHIQ_PORT_FREE) {
+			int len2 = snprintf(remoteport, sizeof(remoteport),
+				"%d", service->remoteport);
+			if (service->public_fourcc != VCHIQ_FOURCC_INVALID)
+				snprintf(remoteport + len2,
+					sizeof(remoteport) - len2,
+					" (client %x)", service->client_id);
+		} else
+			strcpy(remoteport, "n/a");
+
+		len += snprintf(buf + len, sizeof(buf) - len,
+			" '%c%c%c%c' remote %s (msg use %d/%d, slot use %d/%d)",
+			VCHIQ_FOURCC_AS_4CHARS(fourcc),
+			remoteport,
+			service_quota->message_use_count,
+			service_quota->message_quota,
+			service_quota->slot_use_count,
+			service_quota->slot_quota);
+
+		vchiq_dump(dump_context, buf, len + 1);
+
+		tx_pending = service->bulk_tx.local_insert -
+			service->bulk_tx.remote_insert;
+
+		rx_pending = service->bulk_rx.local_insert -
+			service->bulk_rx.remote_insert;
+
+		len = snprintf(buf, sizeof(buf),
+			"  Bulk: tx_pending=%d (size %d),"
+			" rx_pending=%d (size %d)",
+			tx_pending,
+			tx_pending ? service->bulk_tx.bulks[
+			BULK_INDEX(service->bulk_tx.remove)].size : 0,
+			rx_pending,
+			rx_pending ? service->bulk_rx.bulks[
+			BULK_INDEX(service->bulk_rx.remove)].size : 0);
+
+		if (VCHIQ_ENABLE_STATS) {
+			vchiq_dump(dump_context, buf, len + 1);
+
+			len = snprintf(buf, sizeof(buf),
+				"  Ctrl: tx_count=%d, tx_bytes=%llu, "
+				"rx_count=%d, rx_bytes=%llu",
+				service->stats.ctrl_tx_count,
+				service->stats.ctrl_tx_bytes,
+				service->stats.ctrl_rx_count,
+				service->stats.ctrl_rx_bytes);
+			vchiq_dump(dump_context, buf, len + 1);
+
+			len = snprintf(buf, sizeof(buf),
+				"  Bulk: tx_count=%d, tx_bytes=%llu, "
+				"rx_count=%d, rx_bytes=%llu",
+				service->stats.bulk_tx_count,
+				service->stats.bulk_tx_bytes,
+				service->stats.bulk_rx_count,
+				service->stats.bulk_rx_bytes);
+			vchiq_dump(dump_context, buf, len + 1);
+
+			len = snprintf(buf, sizeof(buf),
+				"  %d quota stalls, %d slot stalls, "
+				"%d bulk stalls, %d aborted, %d errors",
+				service->stats.quota_stalls,
+				service->stats.slot_stalls,
+				service->stats.bulk_stalls,
+				service->stats.bulk_aborted_count,
+				service->stats.error_count);
+		 }
+	}
+
+	vchiq_dump(dump_context, buf, len + 1);
+
+	if (service->srvstate != VCHIQ_SRVSTATE_FREE)
+		vchiq_dump_platform_service_state(dump_context, service);
+}
+
+
+void
+vchiq_loud_error_header(void)
+{
+	vchiq_log_error(vchiq_core_log_level,
+		"============================================================"
+		"================");
+	vchiq_log_error(vchiq_core_log_level,
+		"============================================================"
+		"================");
+	vchiq_log_error(vchiq_core_log_level, "=====");
+}
+
+void
+vchiq_loud_error_footer(void)
+{
+	vchiq_log_error(vchiq_core_log_level, "=====");
+	vchiq_log_error(vchiq_core_log_level,
+		"============================================================"
+		"================");
+	vchiq_log_error(vchiq_core_log_level,
+		"============================================================"
+		"================");
+}
+
+
+VCHIQ_STATUS_T vchiq_send_remote_use(VCHIQ_STATE_T *state)
+{
+	VCHIQ_STATUS_T status = VCHIQ_RETRY;
+	if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED)
+		status = queue_message(state, NULL,
+			VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE, 0, 0),
+			NULL, 0, 0, 0);
+	return status;
+}
+
+VCHIQ_STATUS_T vchiq_send_remote_release(VCHIQ_STATE_T *state)
+{
+	VCHIQ_STATUS_T status = VCHIQ_RETRY;
+	if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED)
+		status = queue_message(state, NULL,
+			VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_RELEASE, 0, 0),
+			NULL, 0, 0, 0);
+	return status;
+}
+
+VCHIQ_STATUS_T vchiq_send_remote_use_active(VCHIQ_STATE_T *state)
+{
+	VCHIQ_STATUS_T status = VCHIQ_RETRY;
+	if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED)
+		status = queue_message(state, NULL,
+			VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE_ACTIVE, 0, 0),
+			NULL, 0, 0, 0);
+	return status;
+}
+
+void vchiq_log_dump_mem(const char *label, uint32_t addr, const void *voidMem,
+	size_t numBytes)
+{
+	const uint8_t  *mem = (const uint8_t *)voidMem;
+	size_t          offset;
+	char            lineBuf[100];
+	char           *s;
+
+	while (numBytes > 0) {
+		s = lineBuf;
+
+		for (offset = 0; offset < 16; offset++) {
+			if (offset < numBytes)
+				s += snprintf(s, 4, "%02x ", mem[offset]);
+			else
+				s += snprintf(s, 4, "   ");
+		}
+
+		for (offset = 0; offset < 16; offset++) {
+			if (offset < numBytes) {
+				uint8_t ch = mem[offset];
+
+				if ((ch < ' ') || (ch > '~'))
+					ch = '.';
+				*s++ = (char)ch;
+			}
+		}
+		*s++ = '\0';
+
+		if ((label != NULL) && (*label != '\0'))
+			vchiq_log_trace(VCHIQ_LOG_TRACE,
+				"%s: %08x: %s", label, addr, lineBuf);
+		else
+			vchiq_log_trace(VCHIQ_LOG_TRACE,
+				"%08x: %s", addr, lineBuf);
+
+		addr += 16;
+		mem += 16;
+		if (numBytes > 16)
+			numBytes -= 16;
+		else
+			numBytes = 0;
+	}
+}