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);
}
/** 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;
}
/** Enable processing on a port */
static MMAL_STATUS_T mmal_vc_port_enable(MMAL_PORT_T *port, MMAL_PORT_BH_CB_T cb)
{
MMAL_PORT_MODULE_T *module = port->priv->module;
MMAL_STATUS_T status;
mmal_worker_reply reply;
mmal_worker_port_action msg;
size_t replylen = sizeof(reply);
MMAL_PARAM_UNUSED(cb);
if (!port->component->priv->module->event_ctx_initialised)
{
MMAL_POOL_T *pool = port->component->priv->event_pool;
MMAL_DRIVER_BUFFER_T *drv;
unsigned int i;
/* We need to associate our vc client context to all our event buffers.
* This only needs to be done when the first port is enabled because no event
* can be received on disabled ports. */
for (i = 0; i < pool->headers_num; i++)
{
drv = mmal_buffer_header_driver_data(pool->header[i]);
drv->client_context = &port->component->priv->module->event_ctx;
drv->magic = MMAL_MAGIC;
}
port->component->priv->module->event_ctx_initialised = MMAL_TRUE;
}
if (!module->connected)
{
if (vcos_blockpool_create_on_heap(&module->pool, port->buffer_num,
sizeof(MMAL_VC_CLIENT_BUFFER_CONTEXT_T),
VCOS_BLOCKPOOL_ALIGN_DEFAULT, VCOS_BLOCKPOOL_FLAG_NONE, "mmal vc port pool") != VCOS_SUCCESS)
{
LOG_ERROR("failed to create port pool");
return MMAL_ENOMEM;
}
module->has_pool = 1;
}
if (module->connected)
{
/* The connected port won't be enabled explicitly so make sure we apply
* the buffer requirements now. */
status = mmal_vc_port_requirements_set(module->connected);
if (status != MMAL_SUCCESS)
goto error;
}
msg.component_handle = module->component_handle;
msg.action = MMAL_WORKER_PORT_ACTION_ENABLE;
msg.port_handle = module->port_handle;
msg.param.enable.port = *port;
status = mmal_vc_sendwait_message(mmal_vc_get_client(), &msg.header, sizeof(msg),
MMAL_WORKER_PORT_ACTION, &reply, &replylen, MMAL_FALSE);
if (status == MMAL_SUCCESS)
{
vcos_assert(replylen == sizeof(reply));
status = reply.status;
}
if (status != MMAL_SUCCESS)
{
LOG_ERROR("failed to enable port %s: %s",
port->name, mmal_status_to_string(status));
goto error;
}
if (module->connected)
mmal_vc_port_info_get(module->connected);
return MMAL_SUCCESS;
error:
if (module->has_pool)
vcos_blockpool_delete(&module->pool);
return status;
}
