diff options
Diffstat (limited to 'drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c')
| -rw-r--r-- | drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c | 3934 |
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 000000000000..2c98da4307df --- /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("a_spinlock); + count = service_quota->message_use_count; + if (count > 0) + service_quota->message_use_count = + count - 1; + spin_unlock("a_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("a_spinlock); + count = service_quota->slot_use_count; + if (count > 0) + service_quota->slot_use_count = + count - 1; + spin_unlock("a_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("a_spinlock); + count = state->data_use_count; + if (count > 0) + state->data_use_count = + count - 1; + spin_unlock("a_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("a_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("a_spinlock); + mutex_unlock(&state->slot_mutex); + + if (down_interruptible(&state->data_quota_event) + != 0) + return VCHIQ_RETRY; + + mutex_lock(&state->slot_mutex); + spin_lock("a_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("a_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("a_spinlock); + tx_end_index = SLOT_QUEUE_INDEX_FROM_POS( + state->local_tx_pos + stride - 1); + } + + spin_unlock("a_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("a_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("a_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; + } +} |