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 | 360 |
1 files changed, 182 insertions, 178 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 index 0b0a97bfd01c..517a8c9b41ed 100644 --- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c +++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c @@ -28,6 +28,8 @@ ((unsigned int)(info - state->slot_info)) #define SLOT_QUEUE_INDEX_FROM_POS(pos) \ ((int)((unsigned int)(pos) / VCHIQ_SLOT_SIZE)) +#define SLOT_QUEUE_INDEX_FROM_POS_MASKED(pos) \ + (SLOT_QUEUE_INDEX_FROM_POS(pos) & VCHIQ_SLOT_QUEUE_MASK) #define BULK_INDEX(x) (x & (VCHIQ_NUM_SERVICE_BULKS - 1)) @@ -338,7 +340,7 @@ static void mark_service_closing_internal(struct vchiq_service *service, int sh_thread) { struct vchiq_state *state = service->state; - struct vchiq_service_quota *service_quota; + struct vchiq_service_quota *quota; service->closing = 1; @@ -357,8 +359,8 @@ mark_service_closing_internal(struct vchiq_service *service, int sh_thread) } /* Unblock any sending thread. */ - service_quota = &state->service_quotas[service->localport]; - complete(&service_quota->quota_event); + quota = &state->service_quotas[service->localport]; + complete("a->quota_event); } static void @@ -537,22 +539,23 @@ request_poll(struct vchiq_state *state, struct vchiq_service *service, int poll_type) { u32 value; + int index; - if (service) { - do { - value = atomic_read(&service->poll_flags); - } while (atomic_cmpxchg(&service->poll_flags, value, - value | BIT(poll_type)) != value); + if (!service) + goto skip_service; - do { - value = atomic_read(&state->poll_services[ - service->localport>>5]); - } while (atomic_cmpxchg( - &state->poll_services[service->localport>>5], - value, value | BIT(service->localport & 0x1f)) - != value); - } + do { + value = atomic_read(&service->poll_flags); + } while (atomic_cmpxchg(&service->poll_flags, value, + value | BIT(poll_type)) != value); + + index = BITSET_WORD(service->localport); + do { + value = atomic_read(&state->poll_services[index]); + } while (atomic_cmpxchg(&state->poll_services[index], + value, value | BIT(service->localport & 0x1f)) != value); +skip_service: state->poll_needed = 1; wmb(); @@ -612,8 +615,7 @@ reserve_space(struct vchiq_state *state, size_t space, int is_blocking) } slot_index = local->slot_queue[ - SLOT_QUEUE_INDEX_FROM_POS(tx_pos) & - VCHIQ_SLOT_QUEUE_MASK]; + SLOT_QUEUE_INDEX_FROM_POS_MASKED(tx_pos)]; state->tx_data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index); } @@ -674,28 +676,28 @@ process_free_queue(struct vchiq_state *state, BITSET_T *service_found, if (VCHIQ_MSG_TYPE(msgid) == VCHIQ_MSG_DATA) { int port = VCHIQ_MSG_SRCPORT(msgid); - struct vchiq_service_quota *service_quota = + struct vchiq_service_quota *quota = &state->service_quotas[port]; int count; spin_lock("a_spinlock); - count = service_quota->message_use_count; + count = quota->message_use_count; if (count > 0) - service_quota->message_use_count = + quota->message_use_count = count - 1; spin_unlock("a_spinlock); - if (count == service_quota->message_quota) + if (count == quota->message_quota) /* * Signal the service that it * has dropped below its quota */ - complete(&service_quota->quota_event); + complete("a->quota_event); else if (count == 0) { vchiq_log_error(vchiq_core_log_level, "service %d message_use_count=%d (header %pK, msgid %x, header->msgid %x, header->size %x)", port, - service_quota->message_use_count, + quota->message_use_count, header, msgid, header->msgid, header->size); WARN(1, "invalid message use count\n"); @@ -705,9 +707,9 @@ process_free_queue(struct vchiq_state *state, BITSET_T *service_found, BITSET_SET(service_found, port); spin_lock("a_spinlock); - count = service_quota->slot_use_count; + count = quota->slot_use_count; if (count > 0) - service_quota->slot_use_count = + quota->slot_use_count = count - 1; spin_unlock("a_spinlock); @@ -716,7 +718,7 @@ process_free_queue(struct vchiq_state *state, BITSET_T *service_found, * Signal the service in case * it has dropped below its quota */ - complete(&service_quota->quota_event); + complete("a->quota_event); vchiq_log_trace( vchiq_core_log_level, "%d: pfq:%d %x@%pK - slot_use->%d", @@ -822,7 +824,7 @@ queue_message(struct vchiq_state *state, struct vchiq_service *service, void *context, size_t size, int flags) { struct vchiq_shared_state *local; - struct vchiq_service_quota *service_quota = NULL; + struct vchiq_service_quota *quota = NULL; struct vchiq_header *header; int type = VCHIQ_MSG_TYPE(msgid); @@ -856,7 +858,7 @@ queue_message(struct vchiq_state *state, struct vchiq_service *service, return VCHIQ_ERROR; } - service_quota = &state->service_quotas[service->localport]; + quota = &state->service_quotas[service->localport]; spin_lock("a_spinlock); @@ -893,22 +895,21 @@ queue_message(struct vchiq_state *state, struct vchiq_service *service, } } - 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))) { + while ((quota->message_use_count == quota->message_quota) || + ((tx_end_index != quota->previous_tx_index) && + (quota->slot_use_count == + quota->slot_quota))) { spin_unlock("a_spinlock); vchiq_log_trace(vchiq_core_log_level, "%d: qm:%d %s,%zx - 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); + quota->message_use_count, + quota->slot_use_count); VCHIQ_SERVICE_STATS_INC(service, quota_stalls); mutex_unlock(&state->slot_mutex); if (wait_for_completion_interruptible( - &service_quota->quota_event)) + "a->quota_event)) return VCHIQ_RETRY; if (service->closing) return VCHIQ_ERROR; @@ -974,7 +975,7 @@ queue_message(struct vchiq_state *state, struct vchiq_service *service, (size_t)callback_result)); spin_lock("a_spinlock); - service_quota->message_use_count++; + quota->message_use_count++; tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos - 1); @@ -992,9 +993,9 @@ queue_message(struct vchiq_state *state, struct vchiq_service *service, * 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; + if (tx_end_index != quota->previous_tx_index) { + quota->previous_tx_index = tx_end_index; + slot_use_count = ++quota->slot_use_count; } else { slot_use_count = 0; } @@ -1567,8 +1568,7 @@ parse_rx_slots(struct vchiq_state *state) 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]; + SLOT_QUEUE_INDEX_FROM_POS_MASKED(state->rx_pos)]; state->rx_data = (char *)SLOT_DATA_FROM_INDEX(state, rx_index); state->rx_info = SLOT_INFO_FROM_INDEX(state, rx_index); @@ -1681,8 +1681,7 @@ parse_rx_slots(struct vchiq_state *state) "%d: prs OPENACK@%pK,%x (%d->%d) v:%d", state->id, header, size, remoteport, localport, service->peer_version); - if (service->srvstate == - VCHIQ_SRVSTATE_OPENING) { + if (service->srvstate == VCHIQ_SRVSTATE_OPENING) { service->remoteport = remoteport; vchiq_set_service_state(service, VCHIQ_SRVSTATE_OPEN); @@ -1716,9 +1715,8 @@ parse_rx_slots(struct vchiq_state *state) "%d: prs DATA@%pK,%x (%d->%d)", state->id, header, size, remoteport, localport); - if ((service->remoteport == remoteport) - && (service->srvstate == - VCHIQ_SRVSTATE_OPEN)) { + if ((service->remoteport == remoteport) && + (service->srvstate == VCHIQ_SRVSTATE_OPEN)) { header->msgid = msgid | VCHIQ_MSGID_CLAIMED; claim_slot(state->rx_info); DEBUG_TRACE(PARSE_LINE); @@ -1753,9 +1751,8 @@ parse_rx_slots(struct vchiq_state *state) break; case VCHIQ_MSG_BULK_RX_DONE: case VCHIQ_MSG_BULK_TX_DONE: - if ((service->remoteport == remoteport) - && (service->srvstate != - VCHIQ_SRVSTATE_FREE)) { + if ((service->remoteport == remoteport) && + (service->srvstate != VCHIQ_SRVSTATE_FREE)) { struct vchiq_bulk_queue *queue; struct vchiq_bulk *bulk; @@ -2209,9 +2206,9 @@ vchiq_init_state(struct vchiq_state *state, struct vchiq_slot_zero *slot_zero) state->slot_queue_available = 0; for (i = 0; i < VCHIQ_MAX_SERVICES; i++) { - struct vchiq_service_quota *service_quota = + struct vchiq_service_quota *quota = &state->service_quotas[i]; - init_completion(&service_quota->quota_event); + init_completion("a->quota_event); } for (i = local->slot_first; i <= local->slot_last; i++) { @@ -2366,7 +2363,7 @@ vchiq_add_service_internal(struct vchiq_state *state, { struct vchiq_service *service; struct vchiq_service __rcu **pservice = NULL; - struct vchiq_service_quota *service_quota; + struct vchiq_service_quota *quota; int ret; int i; @@ -2445,10 +2442,9 @@ vchiq_add_service_internal(struct vchiq_state *state, srv = rcu_dereference(state->services[i]); if (!srv) pservice = &state->services[i]; - else if ((srv->public_fourcc == params->fourcc) - && ((srv->instance != instance) || - (srv->base.callback != - params->callback))) { + 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. @@ -2481,11 +2477,11 @@ vchiq_add_service_internal(struct vchiq_state *state, return NULL; } - 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 = + quota = &state->service_quotas[service->localport]; + quota->slot_quota = state->default_slot_quota; + quota->message_quota = state->default_message_quota; + if (quota->slot_use_count == 0) + quota->previous_tx_index = SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos) - 1; @@ -2526,24 +2522,27 @@ vchiq_open_service_internal(struct vchiq_service *service, int client_id) &payload, sizeof(payload), QMFLAGS_IS_BLOCKING); - if (status == VCHIQ_SUCCESS) { - /* Wait for the ACK/NAK */ - if (wait_for_completion_interruptible(&service->remove_event)) { - 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 %u)", - service->state->id, - srvstate_names[service->srvstate], - kref_read(&service->ref_count)); - status = VCHIQ_ERROR; - VCHIQ_SERVICE_STATS_INC(service, error_count); - vchiq_release_service_internal(service); - } + + if (status != VCHIQ_SUCCESS) + return status; + + /* Wait for the ACK/NAK */ + if (wait_for_completion_interruptible(&service->remove_event)) { + 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 %u)", + service->state->id, + srvstate_names[service->srvstate], + kref_read(&service->ref_count)); + status = VCHIQ_ERROR; + VCHIQ_SERVICE_STATS_INC(service, error_count); + vchiq_release_service_internal(service); } + return status; } @@ -2939,8 +2938,8 @@ vchiq_close_service(unsigned int handle) service->state->id, service->localport); if ((service->srvstate == VCHIQ_SRVSTATE_FREE) || - (service->srvstate == VCHIQ_SRVSTATE_LISTENING) || - (service->srvstate == VCHIQ_SRVSTATE_HIDDEN)) { + (service->srvstate == VCHIQ_SRVSTATE_LISTENING) || + (service->srvstate == VCHIQ_SRVSTATE_HIDDEN)) { unlock_service(service); return VCHIQ_ERROR; } @@ -2963,8 +2962,8 @@ vchiq_close_service(unsigned int handle) } if ((service->srvstate == VCHIQ_SRVSTATE_FREE) || - (service->srvstate == VCHIQ_SRVSTATE_LISTENING) || - (service->srvstate == VCHIQ_SRVSTATE_OPEN)) + (service->srvstate == VCHIQ_SRVSTATE_LISTENING) || + (service->srvstate == VCHIQ_SRVSTATE_OPEN)) break; vchiq_log_warning(vchiq_core_log_level, @@ -2974,8 +2973,8 @@ vchiq_close_service(unsigned int handle) } if ((status == VCHIQ_SUCCESS) && - (service->srvstate != VCHIQ_SRVSTATE_FREE) && - (service->srvstate != VCHIQ_SRVSTATE_LISTENING)) + (service->srvstate != VCHIQ_SRVSTATE_FREE) && + (service->srvstate != VCHIQ_SRVSTATE_LISTENING)) status = VCHIQ_ERROR; unlock_service(service); @@ -3006,7 +3005,7 @@ vchiq_remove_service(unsigned int handle) mark_service_closing(service); if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) || - (current == service->state->slot_handler_thread)) { + (current == service->state->slot_handler_thread)) { /* * Make it look like a client, because it must be removed and * not left in the LISTENING state. @@ -3027,7 +3026,7 @@ vchiq_remove_service(unsigned int handle) } if ((service->srvstate == VCHIQ_SRVSTATE_FREE) || - (service->srvstate == VCHIQ_SRVSTATE_OPEN)) + (service->srvstate == VCHIQ_SRVSTATE_OPEN)) break; vchiq_log_warning(vchiq_core_log_level, @@ -3037,7 +3036,7 @@ vchiq_remove_service(unsigned int handle) } if ((status == VCHIQ_SUCCESS) && - (service->srvstate != VCHIQ_SRVSTATE_FREE)) + (service->srvstate != VCHIQ_SRVSTATE_FREE)) status = VCHIQ_ERROR; unlock_service(service); @@ -3070,9 +3069,16 @@ enum vchiq_status vchiq_bulk_transfer(unsigned int handle, enum vchiq_status status = VCHIQ_ERROR; int payload[2]; - if (!service || service->srvstate != VCHIQ_SRVSTATE_OPEN || - (!offset && !uoffset) || - vchiq_check_service(service) != VCHIQ_SUCCESS) + if (!service) + goto error_exit; + + if (service->srvstate != VCHIQ_SRVSTATE_OPEN) + goto error_exit; + + if (!offset && !uoffset) + goto error_exit; + + if (vchiq_check_service(service) != VCHIQ_SUCCESS) goto error_exit; switch (mode) { @@ -3216,8 +3222,10 @@ vchiq_queue_message(unsigned int handle, struct vchiq_service *service = find_service_by_handle(handle); enum vchiq_status status = VCHIQ_ERROR; - if (!service || - (vchiq_check_service(service) != VCHIQ_SUCCESS)) + if (!service) + goto error_exit; + + if (vchiq_check_service(service) != VCHIQ_SUCCESS) goto error_exit; if (!size) { @@ -3299,7 +3307,7 @@ vchiq_release_message(unsigned int handle, slot_index = SLOT_INDEX_FROM_DATA(state, (void *)header); if ((slot_index >= remote->slot_first) && - (slot_index <= remote->slot_last)) { + (slot_index <= remote->slot_last)) { int msgid = header->msgid; if (msgid & VCHIQ_MSGID_CLAIMED) { @@ -3328,10 +3336,15 @@ vchiq_get_peer_version(unsigned int handle, short *peer_version) enum vchiq_status status = VCHIQ_ERROR; struct vchiq_service *service = find_service_by_handle(handle); - if (!service || - (vchiq_check_service(service) != VCHIQ_SUCCESS) || - !peer_version) + if (!service) + goto exit; + + if (vchiq_check_service(service) != VCHIQ_SUCCESS) goto exit; + + if (!peer_version) + goto exit; + *peer_version = service->peer_version; status = VCHIQ_SUCCESS; @@ -3358,77 +3371,70 @@ vchiq_set_service_option(unsigned int handle, { struct vchiq_service *service = find_service_by_handle(handle); enum vchiq_status status = VCHIQ_ERROR; + struct vchiq_service_quota *quota; - if (service) { - switch (option) { - case VCHIQ_SERVICE_OPTION_AUTOCLOSE: - service->auto_close = value; - status = VCHIQ_SUCCESS; - break; + if (!service) + return VCHIQ_ERROR; - case VCHIQ_SERVICE_OPTION_SLOT_QUOTA: { - struct vchiq_service_quota *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 - */ - complete(&service_quota->quota_event); - } - status = VCHIQ_SUCCESS; - } - } break; - - case VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA: { - struct vchiq_service_quota *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 - */ - complete(&service_quota->quota_event); - status = VCHIQ_SUCCESS; - } - } break; + switch (option) { + case VCHIQ_SERVICE_OPTION_AUTOCLOSE: + service->auto_close = value; + 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_SLOT_QUOTA: + quota = &service->state->service_quotas[service->localport]; + if (value == 0) + value = service->state->default_slot_quota; + if ((value >= quota->slot_use_count) && + (value < (unsigned short)~0)) { + quota->slot_quota = value; + if ((value >= quota->slot_use_count) && + (quota->message_quota >= quota->message_use_count)) + /* + * Signal the service that it may have + * dropped below its quota + */ + complete("a->quota_event); + status = VCHIQ_SUCCESS; + } + break; - case VCHIQ_SERVICE_OPTION_TRACE: - service->trace = value; + case VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA: + quota = &service->state->service_quotas[service->localport]; + if (value == 0) + value = service->state->default_message_quota; + if ((value >= quota->message_use_count) && + (value < (unsigned short)~0)) { + quota->message_quota = value; + if ((value >= quota->message_use_count) && + (quota->slot_quota >= quota->slot_use_count)) + /* + * Signal the service that it may have + * dropped below its quota + */ + complete("a->quota_event); status = VCHIQ_SUCCESS; - break; + } + break; - default: - break; + case VCHIQ_SERVICE_OPTION_SYNCHRONOUS: + if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) || + (service->srvstate == VCHIQ_SRVSTATE_LISTENING)) { + service->sync = value; + status = VCHIQ_SUCCESS; } - unlock_service(service); + break; + + case VCHIQ_SERVICE_OPTION_TRACE: + service->trace = value; + status = VCHIQ_SUCCESS; + break; + + default: + break; } + unlock_service(service); return status; } @@ -3592,7 +3598,7 @@ int vchiq_dump_service_state(void *dump_context, struct vchiq_service *service) if (service->srvstate != VCHIQ_SRVSTATE_FREE) { char remoteport[30]; - struct vchiq_service_quota *service_quota = + struct vchiq_service_quota *quota = &service->state->service_quotas[service->localport]; int fourcc = service->base.fourcc; int tx_pending, rx_pending; @@ -3612,10 +3618,10 @@ int vchiq_dump_service_state(void *dump_context, struct vchiq_service *service) " '%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); + quota->message_use_count, + quota->message_quota, + quota->slot_use_count, + quota->slot_quota); err = vchiq_dump(dump_context, buf, len + 1); if (err) @@ -3702,24 +3708,22 @@ vchiq_loud_error_footer(void) enum vchiq_status vchiq_send_remote_use(struct vchiq_state *state) { - enum vchiq_status status = VCHIQ_RETRY; + if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED) + return VCHIQ_RETRY; - if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED) - status = queue_message(state, NULL, - VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE, 0, 0), - NULL, NULL, 0, 0); - return status; + return queue_message(state, NULL, + VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE, 0, 0), + NULL, NULL, 0, 0); } enum vchiq_status vchiq_send_remote_use_active(struct vchiq_state *state) { - enum vchiq_status status = VCHIQ_RETRY; + if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED) + return 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, NULL, 0, 0); - return status; + return queue_message(state, NULL, + VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE_ACTIVE, 0, 0), + NULL, NULL, 0, 0); } void vchiq_log_dump_mem(const char *label, u32 addr, const void *void_mem, |