/** Disconnect a connected port. */
MMAL_STATUS_T mmal_port_disconnect(MMAL_PORT_T *port)
{
MMAL_PORT_PRIVATE_CORE_T* core;
MMAL_PORT_T* other_port;
MMAL_STATUS_T status = MMAL_SUCCESS;
if (!port || !port->priv)
{
LOG_ERROR("invalid port");
return MMAL_EINVAL;
}
LOG_TRACE("%s(%p)", port->name, port);
LOCK_PORT(port);
core = port->priv->core;
if (!core->connected_port)
{
UNLOCK_PORT(port);
LOG_DEBUG("%s(%p) is not connected", port->name, port);
return MMAL_ENOTCONN;
}
other_port = core->connected_port;
if (port->is_enabled)
{
status = mmal_port_disable_locked(port);
if (status != MMAL_SUCCESS)
{
LOG_ERROR("could not disable %s(%p) (%i)", port->name, port, status);
goto finish;
}
if (port->priv->core->pool_for_connection)
mmal_pool_destroy(port->priv->core->pool_for_connection);
port->priv->core->pool_for_connection = NULL;
}
if (!core->core_owns_connection)
{
status = port->priv->pf_connect(port, NULL);
if (status != MMAL_SUCCESS)
{
LOG_ERROR("disconnection of %s(%p) failed (%i)", port->name, port, status);
goto finish;
}
}
core->connected_port = NULL;
other_port->priv->core->connected_port = NULL;
finish:
UNLOCK_PORT(port);
return status;
}
/** Allocate a payload buffer */
uint8_t *mmal_port_payload_alloc(MMAL_PORT_T *port, uint32_t payload_size)
{
uint8_t *mem;
if (!port || !port->priv)
return NULL;
LOG_TRACE("%s(%i:%i) port %p, size %i", port->component->name,
(int)port->type, (int)port->index, port, (int)payload_size);
if (!payload_size)
return NULL;
/* TODO: keep track of the allocs so we can free them when the component is destroyed */
if (!port->priv->pf_payload_alloc)
{
/* Revert to using the heap */
mem = vcos_malloc(payload_size, "mmal payload");
goto end;
}
LOCK_PORT(port);
mem = port->priv->pf_payload_alloc(port, payload_size);
UNLOCK_PORT(port);
end:
/* Acquire the port if the allocation was successful.
* This will ensure that the component is not destroyed until the payload has been freed. */
if (mem)
mmal_port_acquire(port);
return mem;
}
/** Free a payload buffer */
void mmal_port_payload_free(MMAL_PORT_T *port, uint8_t *payload)
{
if (!port || !port->priv)
return;
LOG_TRACE("%s(%i:%i) port %p, payload %p", port->component->name,
(int)port->type, (int)port->index, port, payload);
if (!port->priv->pf_payload_alloc)
{
/* Revert to using the heap */
#ifdef _VIDEOCORE
mem_release((MEM_HANDLE_T)payload);
#else
vcos_free(payload);
#endif
mmal_port_release(port);
return;
}
LOCK_PORT(port);
port->priv->pf_payload_free(port, payload);
UNLOCK_PORT(port);
mmal_port_release(port);
}
/** Disable processing on a port */
MMAL_STATUS_T mmal_port_disable(MMAL_PORT_T *port)
{
MMAL_POOL_T* pool = NULL;
MMAL_STATUS_T status;
if (!port || !port->priv)
return MMAL_EINVAL;
LOG_TRACE("%s(%i:%i) port %p", port->component->name,
(int)port->type, (int)port->index, port);
if (!port->priv->pf_disable)
return MMAL_ENOSYS;
LOCK_PORT(port);
status = mmal_port_disable_locked(port);
if (status == MMAL_SUCCESS)
pool = port->priv->core->pool_for_connection;
port->priv->core->pool_for_connection = NULL;
UNLOCK_PORT(port);
if (status == MMAL_SUCCESS && pool)
mmal_pool_destroy(pool);
return status;
}
/* Get a port parameter */
MMAL_STATUS_T mmal_port_parameter_get(MMAL_PORT_T *port,
MMAL_PARAMETER_HEADER_T *param)
{
MMAL_STATUS_T status = MMAL_ENOSYS;
if (!port || !port->priv)
return MMAL_EINVAL;
LOG_TRACE("%s(%i:%i) port %p, param %p (%x,%i)", port->component->name,
(int)port->type, (int)port->index, port,
param, param ? param->id : 0, param ? (int)param->size : 0);
if (!param)
return MMAL_EINVAL;
LOCK_PORT(port);
if (port->priv->pf_parameter_get)
status = port->priv->pf_parameter_get(port, param);
if (status == MMAL_ENOSYS)
{
/* is this a core parameter? */
status = mmal_port_private_parameter_get(port, param);
}
UNLOCK_PORT(port);
return status;
}
static MMAL_STATUS_T mmal_port_disable_internal(MMAL_PORT_T *port)
{
MMAL_PORT_PRIVATE_CORE_T* core = port->priv->core;
MMAL_STATUS_T status = MMAL_SUCCESS;
MMAL_BUFFER_HEADER_T *buffer;
LOCK_PORT(port);
if (!port->is_enabled)
goto end;
LOCK_SENDING(port);
port->is_enabled = 0;
UNLOCK_SENDING(port);
mmal_component_action_lock(port->component);
if (core->pool_for_connection)
mmal_pool_callback_set(core->pool_for_connection, NULL, NULL);
status = port->priv->pf_disable(port);
mmal_component_action_unlock(port->component);
if (status != MMAL_SUCCESS)
{
LOG_ERROR("port %p could not be disabled (%s)", port->name, mmal_status_to_string(status));
LOCK_SENDING(port);
port->is_enabled = 1;
UNLOCK_SENDING(port);
goto end;
}
/* Flush our internal queue */
buffer = port->priv->core->queue_first;
while (buffer)
{
MMAL_BUFFER_HEADER_T *next = buffer->next;
mmal_port_buffer_header_callback(port, buffer);
buffer = next;
}
port->priv->core->queue_first = 0;
port->priv->core->queue_last = &port->priv->core->queue_first;
/* Wait for all the buffers to have come back from the component */
LOG_DEBUG("%s waiting for %i buffers left in transit", port->name, (int)IN_TRANSIT_COUNT(port));
IN_TRANSIT_WAIT(port);
LOG_DEBUG("%s has no buffers left in transit", port->name);
port->priv->core->buffer_header_callback = NULL;
end:
UNLOCK_PORT(port);
return status;
}
void
bbos_delete_message(struct bbos_message* msg)
{
bbos_port_id_t id;
BBOS_ASSERT(msg != NULL);
id = msg->receiver;
LOCK_PORT(id);
PRINT_DEBUG("[I] Thread %d deletes message 0x%x from %d\n",
bbos_get_running_thread(), msg, id);
mempool_free(bbos_ports[id].pool, msg);
UNLOCK_PORT(id);
}
struct bbos_message*
bbos_receive_message_from(bbos_port_id_t id)
{
struct bbos_message* msg;
if (PORT_IS_EMPTY(id)) {
return NULL;
}
LOCK_PORT(id);
msg = bbos_ports[id].inbox[bbos_ports[id].head];
bbos_ports[id].head = (bbos_ports[id].head + 1) % (bbos_ports[id].capacity);
PRINT_DEBUG("[I] Thread %d receives a message 0x%x with label %d from %d\n",
id, msg, msg->label, msg->sender);
UNLOCK_PORT(id);
return msg;
}
struct bbos_message*
bbos_request_message(bbos_port_id_t id)
{
struct bbos_message* msg;
ASSERT_PORT_ID(id);
LOCK_PORT(id);
PRINT_DEBUG("[I] Thread %d requests message from port %d\n",
bbos_get_running_thread(), id);
if ((msg = (struct bbos_message*)mempool_alloc(bbos_ports[id].pool)) == NULL) {
PRINT_DEBUG("[I] Port %d doesn't have free messages\n", id);
return NULL;
}
UNLOCK_PORT(id);
msg->receiver = id;
msg->sender = bbos_get_running_thread();
msg->label = 0; /* BBOS_NOT_LABELED_MESSAGE ? */
msg->payload = (void*)((void*)msg + sizeof(struct bbos_message));
return msg;
}
void
bbos_send_message(struct bbos_message* msg)
{
bbos_port_id_t id;
BBOS_ASSERT(msg != NULL);
id = msg->receiver;
ASSERT_PORT_ID(id); /* redundant? */
LOCK_PORT(id);
if (PORT_IS_FULL(id)) {
/*
* This should never happen unless the message receiver was manually
* changed.
*/
return;
}
PRINT_DEBUG("[I] Thread %d sends message 0x%x with label %d to %d\n",
msg->sender, msg, msg->label, msg->receiver);
bbos_ports[id].inbox[bbos_ports[id].tail] = msg;
bbos_ports[id].tail = (bbos_ports[id].tail + 1) % (bbos_ports[id].capacity);
UNLOCK_PORT(id);
}
/** Enable processing on a port */
MMAL_STATUS_T mmal_port_enable(MMAL_PORT_T *port, MMAL_PORT_BH_CB_T cb)
{
MMAL_STATUS_T status;
if (!port || !port->priv)
return MMAL_EINVAL;
LOG_TRACE("%s port %p, cb %p, buffers (%i/%i/%i,%i/%i/%i)",
port->name, port, cb,
(int)port->buffer_num, (int)port->buffer_num_recommended, (int)port->buffer_num_min,
(int)port->buffer_size, (int)port->buffer_size_recommended, (int)port->buffer_size_min);
if (!port->priv->pf_enable)
return MMAL_ENOSYS;
LOCK_PORT(port);
status = mmal_port_enable_locked(port, cb);
UNLOCK_PORT(port);
return status;
}
static MMAL_STATUS_T mmal_port_enable_internal(MMAL_PORT_T *port, MMAL_PORT_BH_CB_T cb)
{
MMAL_PORT_PRIVATE_CORE_T* core = port->priv->core;
MMAL_STATUS_T status = MMAL_SUCCESS;
LOCK_PORT(port);
if (port->is_enabled)
goto end;
/* Sanity check the buffer requirements */
if (port->buffer_num < port->buffer_num_min)
{
LOG_ERROR("buffer_num too small (%i/%i)", (int)port->buffer_num, (int)port->buffer_num_min);
status = MMAL_EINVAL;
goto end;
}
if (port->buffer_size < port->buffer_size_min)
{
LOG_ERROR("buffer_size too small (%i/%i)", (int)port->buffer_size, (int)port->buffer_size_min);
status = MMAL_EINVAL;
goto end;
}
core->buffer_header_callback = cb;
status = port->priv->pf_enable(port, cb);
if (status != MMAL_SUCCESS)
goto end;
LOCK_SENDING(port);
port->is_enabled = 1;
UNLOCK_SENDING(port);
end:
UNLOCK_PORT(port);
return status;
}
/* Set a parameter on a port. */
MMAL_STATUS_T mmal_port_parameter_set(MMAL_PORT_T *port,
const MMAL_PARAMETER_HEADER_T *param)
{
MMAL_STATUS_T status = MMAL_ENOSYS;
if (!port)
{
LOG_ERROR("no port");
return MMAL_EINVAL;
}
if (!param)
{
LOG_ERROR("param not supplied");
return MMAL_EINVAL;
}
if (!port->priv)
{
LOG_ERROR("port not configured");
return MMAL_EINVAL;
}
LOG_TRACE("%s(%i:%i) port %p, param %p (%x,%i)", port->component->name,
(int)port->type, (int)port->index, port,
param, param ? param->id : 0, param ? (int)param->size : 0);
LOCK_PORT(port);
if (port->priv->pf_parameter_set)
status = port->priv->pf_parameter_set(port, param);
if (status == MMAL_ENOSYS)
{
/* is this a core parameter? */
status = mmal_port_private_parameter_set(port, param);
}
UNLOCK_PORT(port);
return status;
}
static MMAL_STATUS_T mmal_port_enable_locked_connected(MMAL_PORT_T* output, MMAL_PORT_T* input)
{
MMAL_PORT_PRIVATE_CORE_T* output_core = output->priv->core;
MMAL_STATUS_T status = MMAL_SUCCESS;
output_core->buffer_header_callback = mmal_port_connected_output_cb;
/* Output port already locked, lock input port */
LOCK_PORT(input);
/* Disable connected port if its buffer config needs to change */
if (input->is_enabled &&
(input->buffer_size != output->buffer_size ||
input->buffer_num != output->buffer_num))
{
status = mmal_port_disable_locked(input);
if (status != MMAL_SUCCESS)
goto finish;
}
/* Ensure the connected port has the same buffer configuration */
input->buffer_size = output->buffer_size;
input->buffer_num = output->buffer_num;
/* Enable other end of the connection, if not already enabled */
if (!input->is_enabled)
{
status = mmal_port_enable_locked(input, NULL);
if (status != MMAL_SUCCESS)
goto finish;
}
if (output_core->allocate_pool)
{
MMAL_POOL_T* pool = NULL;
/* Decide which port will be used to allocate the pool */
MMAL_PORT_T* pool_port = (output->capabilities & MMAL_PORT_CAPABILITY_ALLOCATION) ? output : input;
MMAL_PORT_PRIVATE_CORE_T* pool_core = pool_port->priv->core;
uint32_t buffer_size = pool_port->buffer_size;
/* No need to allocate payload memory for pass-through ports */
if (output->capabilities & MMAL_PORT_CAPABILITY_PASSTHROUGH)
buffer_size = 0;
UNLOCK_PORT(input);
if (pool_port == output)
UNLOCK_PORT(output);
/* Port pool creation must be done without the lock held */
pool = mmal_port_pool_create(pool_port, pool_port->buffer_num, buffer_size);
if (pool_port == output)
LOCK_PORT(output);
LOCK_PORT(input);
if (!pool)
{
status = MMAL_ENOMEM;
goto finish;
}
pool_core->pool_for_connection = pool;
mmal_pool_callback_set(pool_core->pool_for_connection, mmal_port_connected_pool_cb, output);
/* Put the buffers into the output port */
status = mmal_port_populate_from_pool(output, pool_port->priv->core->pool_for_connection);
}
finish:
/* At this point, both locks must be held */
if (status != MMAL_SUCCESS && input->is_enabled)
mmal_port_disable_locked(input);
UNLOCK_PORT(input);
if (status != MMAL_SUCCESS)
mmal_port_disable_locked(output);
return status;
}
/** Set format of a port */
MMAL_STATUS_T mmal_port_format_commit(MMAL_PORT_T *port)
{
MMAL_STATUS_T status;
char encoding_string[16];
if (!port || !port->priv)
{
LOG_ERROR("invalid port (%p/%p)", port, port ? port->priv : NULL);
return MMAL_EINVAL;
}
if (port->format != port->priv->core->format_ptr_copy)
{
LOG_ERROR("%s: port format has been overwritten, resetting %p to %p",
port->name, port->format, port->priv->core->format_ptr_copy);
port->format = port->priv->core->format_ptr_copy;
return MMAL_EFAULT;
}
if (port->format->encoding == 0)
snprintf(encoding_string, sizeof(encoding_string), "<NO-FORMAT>");
else
snprintf(encoding_string, sizeof(encoding_string), "%4.4s", (char*)&port->format->encoding);
LOG_TRACE("%s(%i:%i) port %p format %i:%s",
port->component->name, (int)port->type, (int)port->index, port,
(int)port->format->type, encoding_string);
if (!port->priv->pf_set_format)
{
LOG_ERROR("%s: no component implementation", port->name);
return MMAL_ENOSYS;
}
LOCK_PORT(port);
status = port->priv->pf_set_format(port);
mmal_port_name_update(port);
/* Make sure the buffer size / num are sensible */
if (port->buffer_size < port->buffer_size_min)
port->buffer_size = port->buffer_size_min;
if (port->buffer_num < port->buffer_num_min)
port->buffer_num = port->buffer_num_min;
/* The output port settings might have changed */
if (port->type == MMAL_PORT_TYPE_INPUT)
{
MMAL_PORT_T **ports = port->component->output;
unsigned int i;
for (i = 0; i < port->component->output_num; i++)
{
if (ports[i]->buffer_size < ports[i]->buffer_size_min)
ports[i]->buffer_size = ports[i]->buffer_size_min;
if (ports[i]->buffer_num < ports[i]->buffer_num_min)
ports[i]->buffer_num = ports[i]->buffer_num_min;
}
}
UNLOCK_PORT(port);
return status;
}
/** Connect an output port to an input port. */
MMAL_STATUS_T mmal_port_connect(MMAL_PORT_T *port, MMAL_PORT_T *other_port)
{
MMAL_PORT_PRIVATE_CORE_T* core;
MMAL_PORT_PRIVATE_CORE_T* other_core;
MMAL_STATUS_T status = MMAL_SUCCESS;
MMAL_PORT_T* input_port = NULL;
MMAL_PORT_T* output_port = NULL;
if (!port || !port->priv || !other_port || !other_port->priv)
{
LOG_ERROR("invalid port");
return MMAL_EINVAL;
}
LOG_TRACE("connecting %s(%p) to %s(%p)", port->name, port, other_port->name, other_port);
if (!port->priv->pf_connect || !other_port->priv->pf_connect)
{
LOG_ERROR("at least one pf_connect is NULL");
return MMAL_ENOSYS;
}
mmal_port_set_input_or_output(port, &input_port, &output_port);
mmal_port_set_input_or_output(other_port, &input_port, &output_port);
if (!input_port || !output_port)
{
LOG_ERROR("invalid port types used: %i, %i", port->type, other_port->type);
return MMAL_EINVAL;
}
/* Always lock output then input to avoid deadlock */
LOCK_PORT(output_port);
LOCK_PORT(input_port);
core = port->priv->core;
other_core = other_port->priv->core;
if (core->connected_port || other_core->connected_port)
{
MMAL_PORT_T* problem_port = core->connected_port ? port : other_port;
MMAL_PORT_T* connected_port = problem_port->priv->core->connected_port;
LOG_ERROR("port %p is already connected to port %p", problem_port, connected_port);
status = MMAL_EISCONN;
goto finish;
}
if (port->is_enabled || other_port->is_enabled)
{
LOG_ERROR("neither port is allowed to be enabled already: %i, %i",
(int)port->is_enabled, (int)other_port->is_enabled);
status = MMAL_EINVAL;
goto finish;
}
core->connected_port = other_port;
other_core->connected_port = port;
core->core_owns_connection = 0;
other_core->core_owns_connection = 0;
output_port->priv->core->allocate_pool = 0;
/* Check to see if the port will manage the connection on its own. If not then the core
* will manage it. */
if (output_port->priv->pf_connect(port, other_port) == MMAL_SUCCESS)
goto finish;
core->core_owns_connection = 1;
other_core->core_owns_connection = 1;
output_port->priv->core->allocate_pool = 1;
finish:
UNLOCK_PORT(input_port);
UNLOCK_PORT(output_port);
return status;
}
static MMAL_STATUS_T mmal_port_enable_locked(MMAL_PORT_T *port, MMAL_PORT_BH_CB_T cb)
{
MMAL_PORT_PRIVATE_CORE_T* core = port->priv->core;
MMAL_PORT_T* connected_port = core->connected_port;
MMAL_STATUS_T status;
if (port->is_enabled)
{
LOG_DEBUG("already enabled");
return MMAL_EINVAL;
}
/* Ensure that the buffer numbers and sizes used are the maxima between connected ports. */
if (connected_port && port->type == MMAL_PORT_TYPE_OUTPUT)
{
LOCK_PORT(connected_port);
if (connected_port->buffer_num > port->buffer_num)
port->buffer_num = connected_port->buffer_num;
if (connected_port->buffer_size > port->buffer_size)
port->buffer_size = connected_port->buffer_size;
UNLOCK_PORT(connected_port);
}
/* Sanity check the buffer requirements */
if (port->buffer_num < port->buffer_num_min)
{
LOG_ERROR("buffer_num too small (%i/%i)", (int)port->buffer_num, (int)port->buffer_num_min);
return MMAL_EINVAL;
}
if (port->buffer_size < port->buffer_size_min)
{
LOG_ERROR("buffer_size too small (%i/%i)", (int)port->buffer_size, (int)port->buffer_size_min);
return MMAL_EINVAL;
}
if (!connected_port == !cb)
{
/* Callback must be NULL if connected port is not NULL */
LOG_ERROR("connected port %p, callback %p not allowed", connected_port, cb);
return MMAL_EINVAL;
}
core->buffer_header_callback = cb;
status = port->priv->pf_enable(port, cb);
if (status != MMAL_SUCCESS)
return status;
LOCK_SENDING(port);
port->is_enabled = 1;
UNLOCK_SENDING(port);
//FIXME: move before is_enabled is set ?
if (connected_port)
{
if (port->type == MMAL_PORT_TYPE_INPUT)
core->buffer_header_callback = mmal_port_connected_input_cb;
else
status = mmal_port_enable_locked_connected(port, connected_port);
}
return status;
}