static void recv_bulk(CLIENT_THREAD_STATE_T *thread, void *out, uint32_t len) { if (len <= CTRL_THRESHOLD) { VCHIQ_HEADER_T *header = vchiu_queue_pop(get_queue(thread)); assert(header->size == len); memcpy(out, header->data, len); vchiq_release_message(get_handle(thread), header); } else { VCHIQ_STATUS_T vchiq_status = vchiq_queue_bulk_receive(get_handle(thread), out, rpc_pad_bulk(len), NULL); assert(vchiq_status == VCHIQ_SUCCESS); VCOS_STATUS_T vcos_status = vcos_event_wait(&bulk_event); assert(vcos_status == VCOS_SUCCESS); } }
/** Callback invoked by VCHIQ */ static VCHIQ_STATUS_T mmal_vc_vchiq_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *vchiq_header, VCHIQ_SERVICE_HANDLE_T service, void *context) { LOG_TRACE("reason %d", reason); switch (reason) { case VCHIQ_MESSAGE_AVAILABLE: { mmal_worker_msg_header *msg = (mmal_worker_msg_header*)vchiq_header->data; vcos_assert(msg->magic == MMAL_MAGIC); if (msg->msgid == MMAL_WORKER_BUFFER_TO_HOST) { LOG_TRACE("buffer to host"); mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)vchiq_header->data; LOG_TRACE("len %d context %p", msg->buffer_header.length, msg->drvbuf.client_context); vcos_assert(msg->drvbuf.client_context); vcos_assert(msg->drvbuf.client_context->magic == MMAL_MAGIC); /* If the buffer is referencing another, need to replicate it here * in order to use the reference buffer's payload and ensure the * reference is not released prematurely */ if (msg->has_reference) mmal_buffer_header_replicate(msg->drvbuf.client_context->buffer, msg->drvbuf_ref.client_context->buffer); /* Sanity check the size of the transfer so we don't overrun our buffer */ if (!vcos_verify(msg->buffer_header.offset + msg->buffer_header.length <= msg->drvbuf.client_context->buffer->alloc_size)) { LOG_TRACE("buffer too small (%i, %i)", msg->buffer_header.offset + msg->buffer_header.length, msg->drvbuf.client_context->buffer->alloc_size); msg->buffer_header.length = 0; /* FIXME: set a buffer flag to signal error */ msg->drvbuf.client_context->callback(msg); vchiq_release_message(service, vchiq_header); break; } /*To handle VC to HOST filled buffer callback of EOS buffer to receive in sync with data buffers*/ if (!msg->is_zero_copy && (msg->buffer_header.length != 0 || (msg->buffer_header.flags & MMAL_BUFFER_HEADER_FLAG_EOS))) { /* a buffer full of data for us to process */ VCHIQ_STATUS_T vst = VCHIQ_SUCCESS; LOG_TRACE("queue bulk rx: %p, %d", msg->drvbuf.client_context->buffer->data + msg->buffer_header.offset, msg->buffer_header.length); int len = msg->buffer_header.length; len = (len+3) & (~3); if (!len && (msg->buffer_header.flags & MMAL_BUFFER_HEADER_FLAG_EOS)) { len = 8; } if (!msg->payload_in_message) { /* buffer transferred using vchiq bulk xfer */ vst = vchiq_queue_bulk_receive(service, msg->drvbuf.client_context->buffer->data + msg->buffer_header.offset, len, vchiq_header); if (vst != VCHIQ_SUCCESS) { LOG_TRACE("queue bulk rx len %d failed to start", msg->buffer_header.length); msg->buffer_header.length = 0; /* FIXME: set a buffer flag to signal error */ msg->drvbuf.client_context->callback(msg); vchiq_release_message(service, vchiq_header); } } else if (msg->payload_in_message <= MMAL_VC_SHORT_DATA) { /* we have already received the buffer data in the message! */ MMAL_BUFFER_HEADER_T *dst = msg->drvbuf.client_context->buffer; LOG_TRACE("short data: dst = %p, dst->data = %p, len %d short len %d", dst, dst? dst->data : 0, msg->buffer_header.length, msg->payload_in_message); memcpy(dst->data, msg->short_data, msg->payload_in_message); dst->offset = 0; dst->length = msg->payload_in_message; vchiq_release_message(service, vchiq_header); msg->drvbuf.client_context->callback(msg); } else { /* impossible short data length */ LOG_ERROR("Message with invalid short payload length %d", msg->payload_in_message); vcos_assert(0); } } else { /* Message received from videocore; the client_context should have * been passed all the way through by videocore back to us, and will * be picked up in the callback to complete the sequence. */ LOG_TRACE("doing cb (%p) context %p", msg->drvbuf.client_context, msg->drvbuf.client_context ? msg->drvbuf.client_context->callback : 0); msg->drvbuf.client_context->callback(msg); LOG_TRACE("done callback back to client"); vchiq_release_message(service, vchiq_header); } } else if (msg->msgid == MMAL_WORKER_EVENT_TO_HOST) { mmal_vc_handle_event_msg(vchiq_header, service, context); } else { MMAL_WAITER_T *waiter = msg->u.waiter; LOG_TRACE("waking up waiter at %p", waiter); vcos_assert(waiter->inuse); int len = vcos_min(waiter->destlen, vchiq_header->size); waiter->destlen = len; LOG_TRACE("copying payload @%p to %p len %d", waiter->dest, msg, len); memcpy(waiter->dest, msg, len); vchiq_release_message(service, vchiq_header); vcos_semaphore_post(&waiter->sem); } } break; case VCHIQ_BULK_TRANSMIT_DONE: { /* nothing to do here, need to wait for the copro to tell us it * has emptied the buffer before we can recycle it, otherwise we * end up feeding the copro with buffers it cannot handle. */ #ifdef VCOS_LOGGING_ENABLED mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)context; #endif LOG_TRACE("bulk tx done: %p, %d", msg->buffer_header.data, msg->buffer_header.length); } break; case VCHIQ_BULK_RECEIVE_DONE: { VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)context; mmal_worker_msg_header *msg_hdr = (mmal_worker_msg_header*)header->data; if (msg_hdr->msgid == MMAL_WORKER_BUFFER_TO_HOST) { mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)msg_hdr; vcos_assert(msg->drvbuf.client_context->magic == MMAL_MAGIC); msg->drvbuf.client_context->callback(msg); LOG_TRACE("bulk rx done: %p, %d", msg->buffer_header.data, msg->buffer_header.length); } else { mmal_worker_event_to_host *msg = (mmal_worker_event_to_host *)msg_hdr; MMAL_PORT_T *port = mmal_vc_port_by_number(msg->client_component, msg->port_type, msg->port_num); vcos_assert(port); mmal_buffer_header_driver_data(msg->delayed_buffer)-> client_context->callback_event(port, msg->delayed_buffer); LOG_DEBUG("event bulk rx done, length %d", msg->length); } vchiq_release_message(service, header); } break; case VCHIQ_BULK_RECEIVE_ABORTED: { VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)context; mmal_worker_msg_header *msg_hdr = (mmal_worker_msg_header*)header->data; if (msg_hdr->msgid == MMAL_WORKER_BUFFER_TO_HOST) { mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)msg_hdr; LOG_TRACE("bulk rx aborted: %p, %d", msg->buffer_header.data, msg->buffer_header.length); vcos_assert(msg->drvbuf.client_context->magic == MMAL_MAGIC); msg->buffer_header.flags |= MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED; msg->drvbuf.client_context->callback(msg); } else { mmal_worker_event_to_host *msg = (mmal_worker_event_to_host *)msg_hdr; MMAL_PORT_T *port = mmal_vc_port_by_number(msg->client_component, msg->port_type, msg->port_num); vcos_assert(port); LOG_DEBUG("event bulk rx aborted"); msg->delayed_buffer->flags |= MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED; mmal_buffer_header_driver_data(msg->delayed_buffer)-> client_context->callback_event(port, msg->delayed_buffer); } vchiq_release_message(service, header); } break; case VCHIQ_BULK_TRANSMIT_ABORTED: { mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)context; LOG_INFO("bulk tx aborted: %p, %d", msg->buffer_header.data, msg->buffer_header.length); vcos_assert(msg->drvbuf.client_context->magic == MMAL_MAGIC); /* Nothing to do as the VC side will release the buffer and notify us of the error */ } break; default: break; } return VCHIQ_SUCCESS; }
static void mmal_vc_handle_event_msg(VCHIQ_HEADER_T *vchiq_header, VCHIQ_SERVICE_HANDLE_T service, void *context) { mmal_worker_event_to_host *msg = (mmal_worker_event_to_host *)vchiq_header->data; MMAL_COMPONENT_T *component = msg->client_component; MMAL_BUFFER_HEADER_T *buffer; MMAL_STATUS_T status; MMAL_PORT_T *port; LOG_DEBUG("event to host, cmd 0x%08x len %d to component %p port (%d,%d)", msg->cmd, msg->length, msg->client_component, msg->port_type, msg->port_num); (void)context; port = mmal_vc_port_by_number(component, msg->port_type, msg->port_num); if (!vcos_verify(port)) { LOG_ERROR("port (%i,%i) doesn't exist", (int)msg->port_type, (int)msg->port_num); goto error; } status = mmal_port_event_get(port, &buffer, msg->cmd); if (status != MMAL_SUCCESS) { LOG_ERROR("no event buffer available to receive event (%i)", (int)status); goto error; } if (!vcos_verify(msg->length <= buffer->alloc_size)) { LOG_ERROR("event buffer to small to receive event (%i/%i)", (int)buffer->alloc_size, (int)msg->length); goto error; } buffer->length = msg->length; /* Sanity check that the event buffers have the proper vc client context */ if (!vcos_verify(mmal_buffer_header_driver_data(buffer)->magic == MMAL_MAGIC && mmal_buffer_header_driver_data(buffer)->client_context && mmal_buffer_header_driver_data(buffer)->client_context->magic == MMAL_MAGIC && mmal_buffer_header_driver_data(buffer)->client_context->callback_event)) { LOG_ERROR("event buffers not configured properly by component"); goto error; } if (buffer->length > MMAL_WORKER_EVENT_SPACE) { /* a buffer full of data for us to process */ int len = buffer->length; len = (len+3) & (~3); LOG_DEBUG("queue event bulk rx: %p, %d", buffer->data, buffer->length); msg->delayed_buffer = buffer; VCHIQ_STATUS_T vst = vchiq_queue_bulk_receive(service, buffer->data, len, vchiq_header); if (vst != VCHIQ_SUCCESS) { LOG_TRACE("queue event bulk rx len %d failed to start", buffer->length); mmal_buffer_header_release(buffer); goto error; } } else { if (msg->length) memcpy(buffer->data, msg->data, msg->length); mmal_buffer_header_driver_data(buffer)->client_context->callback_event(port, buffer); LOG_DEBUG("done callback back to client"); vchiq_release_message(service, vchiq_header); } return; error: /* FIXME: How to abort bulk receive if necessary? */ msg->length = 0; /* FIXME: set a buffer flag to signal error */ vchiq_release_message(service, vchiq_header); }