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);
}
