VCOS_STATUS_T vcos_platform_init(void)
{
VCOS_STATUS_T st;
uint32_t flags = 0;
st = _vcos_named_semaphore_init();
if (!vcos_verify(st == VCOS_SUCCESS))
goto end;
flags |= VCOS_INIT_NAMED_SEM;
st = vcos_once(¤t_thread_key_once, current_thread_key_init);
if (!vcos_verify(st == VCOS_SUCCESS))
goto end;
/* Initialise a VCOS wrapper for the thread which called vcos_init. */
st = vcos_semaphore_create(&vcos_thread_main.suspend, NULL, 0);
if (!vcos_verify(st == VCOS_SUCCESS))
goto end;
flags |= VCOS_INIT_MAIN_SEM;
#ifdef WIN32_KERN
vcos_thread_main.thread = PsGetCurrentThreadId();
// TODO Implement thread context for kernel mode
#else
int pst;
vcos_thread_main.thread = GetCurrentThread();
// For windows zero return value is failure
pst = TlsSetValue(_vcos_thread_current_key, &vcos_thread_main);
if (!vcos_verify(pst != 0))
{
st = VCOS_EINVAL;
goto end;
}
#endif
st = vcos_msgq_init();
if (!vcos_verify(st == VCOS_SUCCESS))
goto end;
flags |= VCOS_INIT_MSGQ;
vcos_logging_init();
end:
if (st != VCOS_SUCCESS)
vcos_term(flags);
return st;
}
float vg_mat3x3_affine_det(const VG_MAT3X3_T *a)
{
#ifndef NDEBUG
vcos_verify(vg_mat3x3_is_affine(a)); /* why vcos_verify? see vg_mat3x3_is_affine... */
#endif
return (a->m[0][0] * a->m[1][1]) - (a->m[1][0] * a->m[0][1]);
}
/** Send a message and do not wait for a reply.
*
* @note
* This function should only be called from within a mmal component, so
* vchiq_use/release_service calls aren't required (dealt with at higher level).
*
* @param client client to send message for
* @param msg_header message header to send
* @param size length of message, including header
* @param msgid message id
*/
MMAL_STATUS_T mmal_vc_send_message(MMAL_CLIENT_T *client,
mmal_worker_msg_header *msg_header, size_t size,
uint8_t *data, size_t data_size,
uint32_t msgid)
{
VCHIQ_STATUS_T vst;
VCHIQ_ELEMENT_T elems[] = {{msg_header, size}};
MMAL_BOOL_T using_bulk_transfer = (data_size != 0);
LOG_TRACE("len %d", data_size);
vcos_assert(size >= sizeof(mmal_worker_msg_header));
if (!client->inited)
{
vcos_assert(0);
return MMAL_EINVAL;
}
if (using_bulk_transfer)
vcos_mutex_lock(&client->bulk_lock);
msg_header->msgid = msgid;
msg_header->magic = MMAL_MAGIC;
vst = vchiq_queue_message(client->service, elems, 1);
if (vst != VCHIQ_SUCCESS)
{
if (using_bulk_transfer)
vcos_mutex_unlock(&client->bulk_lock);
LOG_ERROR("failed");
goto error;
}
if (using_bulk_transfer)
{
LOG_TRACE("bulk transmit: %p, %i", data, data_size);
data_size = (data_size + 3) & ~3;
vst = vchiq_queue_bulk_transmit(client->service, data, data_size, msg_header);
vcos_mutex_unlock(&client->bulk_lock);
if (!vcos_verify(vst == VCHIQ_SUCCESS))
{
LOG_ERROR("failed bulk transmit");
/* This really should not happen and if it does, things will go wrong as
* we've already queued the vchiq message above. */
vcos_assert(0);
goto error;
}
}
return MMAL_SUCCESS;
error:
return MMAL_EIO;
}
static KHRN_IMAGE_FORMAT_T convert_format(uint32_t format)
{
switch (format & ~EGL_PIXEL_FORMAT_USAGE_MASK_BRCM) {
case EGL_PIXEL_FORMAT_ARGB_8888_PRE_BRCM: return (KHRN_IMAGE_FORMAT_T)(ABGR_8888 | IMAGE_FORMAT_PRE);
case EGL_PIXEL_FORMAT_ARGB_8888_BRCM: return ABGR_8888;
case EGL_PIXEL_FORMAT_XRGB_8888_BRCM: return XBGR_8888;
case EGL_PIXEL_FORMAT_RGB_565_BRCM: return RGB_565;
case EGL_PIXEL_FORMAT_A_8_BRCM: return A_8;
default: vcos_verify(0); return (KHRN_IMAGE_FORMAT_T)0;
}
}
void platform_release_global_image(uint32_t id_0, uint32_t id_1)
{
uint32_t i;
for (i = 0; i != (GLOBAL_IMAGES_N * 2); i += 2) {
if ((global_images[i] == id_0) && (global_images[i + 1] == id_1)) {
global_images[i] = 0;
global_images[i + 1] = 0;
return;
}
}
vcos_verify(0); /* not found... */
}
void platform_acquire_global_image(uint32_t id_0, uint32_t id_1)
{
uint32_t i;
for (i = 0; i != (GLOBAL_IMAGES_N * 2); i += 2) {
if (!global_images[i] && !global_images[i + 1]) {
global_images[i] = id_0;
global_images[i + 1] = id_1;
return;
}
}
vcos_verify(0); /* increase GLOBAL_IMAGES_N... */
}
static OMX_ERRORTYPE vcil_out_ComponentDeInit(OMX_IN OMX_HANDLETYPE hComponent)
{
OMX_COMPONENTTYPE *pComp = (OMX_COMPONENTTYPE *) hComponent;
VC_PRIVATE_COMPONENT_T *comp;
IL_EXECUTE_HEADER_T exe;
IL_RESPONSE_HEADER_T resp;
ILCS_COMMON_T *st;
int rlen = sizeof(resp);
if(!pComp)
return OMX_ErrorBadParameter;
st = pComp->pApplicationPrivate;
comp = (VC_PRIVATE_COMPONENT_T *) pComp->pComponentPrivate;
exe.reference = comp->reference;
if(ilcs_execute_function(st->ilcs, IL_COMPONENT_DEINIT, &exe, sizeof(exe), &resp, &rlen) < 0 || rlen != sizeof(resp) ||
resp.err == OMX_ErrorNone)
{
// remove from list, assuming that we successfully managed to deinit
// this component, or that ilcs has returned an error. The assumption
// here is that if the component has managed to correctly signal an
// error, it still exists, but if the transport has failed then we might
// as well try and cleanup
VC_PRIVATE_COMPONENT_T *list, *prev;
vcos_semaphore_wait(&st->component_lock);
list = st->component_list;
prev = NULL;
while (list != NULL && list != comp)
{
prev = list;
list = list->next;
}
// failing to find this component is not a good sign.
if(vcos_verify(list))
{
if (prev == NULL)
st->component_list = list->next;
else
prev->next = list->next;
}
vcos_semaphore_post(&st->component_lock);
vcos_free(comp);
}
return resp.err;
}
/** Destroy a pool of MMAL_BUFFER_HEADER_T */
void mmal_port_pool_destroy(MMAL_PORT_T *port, MMAL_POOL_T *pool)
{
if (!port || !port->priv || !pool)
return;
LOG_TRACE("%s(%i:%i) port %p, pool %p", port->component->name,
(int)port->type, (int)port->index, port, pool);
if (!vcos_verify(!port->is_enabled))
{
LOG_ERROR("port %p, pool %p destroyed while port enabled", port, pool);
mmal_port_disable(port);
}
mmal_pool_destroy(pool);
}
void vc_image_free_tmp_image(VC_IMAGE_BUF_T *image)
{
int i;
vcos_assert(is_valid_vc_image_buf(image, image->type));
for (i = 0; i < NUM_TMP_IMAGES; ++i) {
if (image == &tmp_images[i]) {
image->image_data = 0;
vclib_release_tmp_buf(vc_image_tmpbuf); // release tmpbuf lock
return;
}
}
(void)vcos_verify(!"Argument isn't a tmp image");
}
/***********************************************************
* Name: vc_cec_send_message
*
* Arguments:
* Follower's logical address
* Message payload WITHOUT the header byte (can be NULL)
* Payload length WITHOUT the header byte (can be zero)
* VC_TRUE if the message is a reply to an incoming message
* (For poll message set payload to NULL and length to zero)
*
* Description
* Remove all commands to be forwarded. This does not affect
* the button presses which are always forwarded
*
* Returns: if the command is successful (zero) or not (non-zero)
* If the command is successful, there will be a Tx callback
* in due course to indicate whether the message has been
* acknowledged by the recipient or not
***********************************************************/
VCHPRE_ int VCHPOST_ vc_cec_send_message(uint32_t follower,
const uint8_t *payload,
uint32_t length,
bool_t is_reply) {
int success = -1;
CEC_SEND_MSG_PARAM_T param;
vcos_assert(length <= CEC_MAX_XMIT_LENGTH);
param.follower = VC_HTOV32(follower);
param.length = VC_HTOV32(length);
param.is_reply = VC_HTOV32(is_reply);
memset(param.payload, 0, sizeof(param.payload));
if(length > 0 && vcos_verify(payload)) {
memcpy(param.payload, payload, _min(length, CEC_MAX_XMIT_LENGTH));
}
success = cecservice_send_command( VC_CEC_SEND_MSG, ¶m, sizeof(param), 1);
return success;
}
MMAL_STATUS_T mmal_vc_get_version(uint32_t *major, uint32_t *minor, uint32_t *minimum)
{
mmal_worker_version msg;
size_t len = sizeof(msg);
MMAL_STATUS_T status;
status = mmal_vc_sendwait_message(mmal_vc_get_client(), &msg.header, sizeof(msg),
MMAL_WORKER_GET_VERSION, &msg, &len, MMAL_FALSE);
if (status != MMAL_SUCCESS)
return status;
if (!vcos_verify(len == sizeof(msg)))
return MMAL_EINVAL;
*major = msg.major;
*minor = msg.minor;
*minimum = msg.minimum;
return MMAL_SUCCESS;
}
/**
* <DFN>vc_hdcp2_service_wait_for_reply</DFN> blocks until a response comes back
* from Videocore.
*
* @param client_handle is the vchi client handle
*
* @param sema is the signalling semaphore indicating a reply
*
* @param response is the reponse buffer
*
* @param max_length is the maximum length of the buffer
*
* @param actual_length will be set to the actual length of the buffer (or zero if error)
*
* @return zero if successful, vchi error code otherwise
*/
int32_t vc_hdcp2_service_wait_for_reply(VCHI_SERVICE_HANDLE_T client_handle,
VCOS_SEMAPHORE_T *sema,
void *response, uint32_t max_length, uint32_t *actual_length) {
int32_t success = 0;
VCOS_STATUS_T status;
uint32_t length_read = 0;
vcos_assert(sema);
vcos_assert(response && max_length);
do {
//TODO : we need to deal with messages coming through on more than one connections properly
//At the moment it will always try to read the first connection if there is something there
//Check if there is something in the queue, if so return immediately
//otherwise wait for the semaphore and read again
success = vchi_msg_dequeue( client_handle, response, max_length, &length_read, VCHI_FLAGS_NONE );
} while( length_read == 0 || (status = vcos_semaphore_trywait(sema)) != VCOS_SUCCESS);
if(vcos_verify(*actual_length)) {
*actual_length = (success == 0)? length_read : 0;
}
return success;
}
/** Buffer header callback. */
void mmal_port_buffer_header_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
#ifdef ENABLE_MMAL_EXTRA_LOGGING
LOG_TRACE("%s(%i:%i) port %p, buffer %p (%i,%p,%i,%i)",
port->component->name, (int)port->type, (int)port->index, port, buffer,
buffer ? (int)buffer->cmd : 0, buffer ? buffer->data : 0,
buffer ? (int)buffer->offset : 0, buffer ? (int)buffer->length : 0);
#endif
if (!vcos_verify(IN_TRANSIT_COUNT(port) >= 0))
LOG_ERROR("%s: buffer headers in transit < 0 (%d)", port->name, (int)IN_TRANSIT_COUNT(port));
if (MMAL_COLLECT_PORT_STATS_ENABLED)
{
mmal_port_update_port_stats(port, MMAL_CORE_STATS_TX);
}
port->priv->core->buffer_header_callback(port, buffer);
IN_TRANSIT_DECREMENT(port);
}
/** Grab a waiter from the pool. Return immediately if one already
* available, or wait for one to become available.
*/
static MMAL_WAITER_T *get_waiter(MMAL_CLIENT_T *client)
{
int i;
MMAL_WAITER_T *waiter = NULL;
vcos_semaphore_wait(&client->waitpool.sem);
vcos_mutex_lock(&client->lock);
for (i=0; i<MAX_WAITERS; i++)
{
if (client->waitpool.waiters[i].inuse == 0)
break;
}
/* If this fails, the semaphore is not working */
if (vcos_verify(i != MAX_WAITERS))
{
waiter = client->waitpool.waiters+i;
waiter->inuse = 1;
}
vcos_mutex_unlock(&client->lock);
return waiter;
}
/**
* <DFN>vc_hdcp2_service_send_command_reply</DFN> sends a command and waits for a reply from
* Videocore side HDCP2 service.
*
* @param client_handle is the vchi client handle
*
* @param lock_sema is the locking semaphore to protect the buffer
*
* @param reply_sema is the signalling semaphore for the reply
*
* @param command is the command (VC_HDCP2_CMD_CODE_T in vc_hdcp2service_defs.h)
*
* @param buffer is the command buffer to be sent
*
* @param length is the size of buffer in bytes
*
* @param response is the reponse buffer
*
* @param response_length is the maximum length of the response buffer
*
* @param actual_length is set to the actual length of the message
*
* @return zero if successful, VCHI error code if failed
*/
int32_t vc_hdcp2_service_send_command_reply(VCHI_SERVICE_HANDLE_T client_handle,
VCOS_SEMAPHORE_T *lock_sema,
VCOS_SEMAPHORE_T *reply_sema,
uint32_t command,
void *buffer, uint32_t length,
void *response, uint32_t response_length,
uint32_t *actual_length) {
int32_t success = vc_hdcp2_service_send_command(client_handle,
lock_sema,
command, buffer, length);
if(vcos_verify(!success)) {
VCOS_STATUS_T status = vcos_semaphore_wait(lock_sema);
vcos_assert(status == VCOS_SUCCESS);
success = vc_hdcp2_service_wait_for_reply(client_handle,
reply_sema,
response, response_length, actual_length);
status = vcos_semaphore_post(lock_sema);
vcos_assert(status == VCOS_SUCCESS);
}
return success;
}
VC_IMAGE_BUF_T *vc_image_alloc_tmp_image(VC_IMAGE_TYPE_T type, int w, int h)
{
int i, tbsh = (vclib_get_tmp_buf_size() / NUM_TMP_IMAGES) & ~15;
vc_image_tmpbuf = vclib_obtain_tmp_buf(0); // get tmpbuf lock
for (i = NUM_TMP_IMAGES-1; i >= 0; --i)
{
if (tmp_images[i].image_data == 0)
{
VC_IMAGE_T *image = &tmp_images[i];
vc_image_configure(image, type, w, h,
VC_IMAGE_PROP_DATA, (char *)vc_image_tmpbuf + i*tbsh,
VC_IMAGE_PROP_SIZE, tbsh);
return image;
}
}
(void)vcos_verify(!"Failed to find free tmp image");
return 0;
}
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);
}
static void parse_rx_slots(VCHIQ_STATE_T *state)
{
VCHIQ_CHANNEL_T *remote = state->remote;
VCHIQ_CHANNEL_T *local = state->local;
while (remote->ctrl.process != remote->ctrl.insert) {
VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)(remote->ctrl.data + (remote->ctrl.process & VCHIQ_CHANNEL_MASK));
VCHIQ_SERVICE_T *service = NULL;
unsigned int stride = calc_stride(header->size);
int type = VCHIQ_MSG_TYPE(header->fourcc);
VCHIQ_TRACE("%d: prs %d (%d,%d)", state->id, type, VCHIQ_MSG_DSTPORT(header->fourcc), VCHIQ_MSG_SRCPORT(header->fourcc));
switch (type)
{
case VCHIQ_MSG_OPEN:
vcos_assert(VCHIQ_MSG_DSTPORT(header->fourcc) == 0);
if (vcos_verify(header->size == 4))
{
VCHIQ_HEADER_T *reply;
unsigned short remoteport = VCHIQ_MSG_SRCPORT(header->fourcc);
int target;
service = get_listening_service(local, *(int *)header->data);
local_mutex_acquire(&local->ctrl.mutex);
target = local->ctrl.insert + sizeof(VCHIQ_HEADER_T);
reply = reserve_space(local, target);
if (!reply)
{
local_mutex_release(&local->ctrl.mutex);
return; /* Bail out */
}
if (service && (service->srvstate == VCHIQ_SRVSTATE_LISTENING))
{
/* A matching, listening service exists - attempt the OPEN */
VCHIQ_STATUS_T status;
vchiq_set_service_state(service, VCHIQ_SRVSTATE_OPEN); /* Proceed as if the connection will be accepted */
status = service->base.callback(VCHIQ_SERVICE_OPENED, NULL, &service->base, NULL);
if (status == VCHIQ_SUCCESS)
{
/* The open was accepted - acknowledge it */
reply->fourcc = VCHIQ_MAKE_MSG(VCHIQ_MSG_OPENACK, service->localport, remoteport);
service->remoteport = remoteport;
}
else
{
vchiq_set_service_state(service, VCHIQ_SRVSTATE_LISTENING);
if (status == VCHIQ_RETRY)
return; /* Bail out if not ready */
/* The open was rejected - send a close */
reply->fourcc = VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, 0, remoteport);
}
}
else
{
/* No matching, available service - send a CLOSE */
reply->fourcc = VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, 0, remoteport);
}
reply->size = 0;
local->ctrl.insert = target;
local_mutex_release(&local->ctrl.mutex);
remote_event_signal(&remote->trigger);
}
break;
case VCHIQ_MSG_OPENACK:
{
unsigned int localport = VCHIQ_MSG_DSTPORT(header->fourcc);
unsigned int remoteport = VCHIQ_MSG_SRCPORT(header->fourcc);
service = &local->services[localport];
if (vcos_verify((localport < VCHIQ_MAX_SERVICES) &&
(service->srvstate == VCHIQ_SRVSTATE_OPENING)))
{
service->remoteport = remoteport;
vchiq_set_service_state(service, VCHIQ_SRVSTATE_OPEN);
local_event_signal(&service->remove_event);
}
}
break;
case VCHIQ_MSG_CLOSE:
{
unsigned int localport = VCHIQ_MSG_DSTPORT(header->fourcc);
unsigned int remoteport = VCHIQ_MSG_SRCPORT(header->fourcc);
service = &local->services[localport];
vcos_assert(header->size == 0); /* There should be no data */
if (vcos_verify(localport < VCHIQ_MAX_SERVICES))
{
switch (service->srvstate)
{
case VCHIQ_SRVSTATE_OPEN:
if (service->remoteport != remoteport)
break;
/* Return the close */
if (queue_message(state,
VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, service->localport, service->remoteport),
NULL, 0, 0) == VCHIQ_RETRY)
return; /* Bail out if not ready */
vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT);
/* Drop through... */
case VCHIQ_SRVSTATE_CLOSESENT:
if (service->remoteport != remoteport)
break;
vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSING);
/* Drop through... */
case VCHIQ_SRVSTATE_CLOSING:
if (service->remoteport == remoteport)
{
/* Start the close procedure */
if (vchiq_close_service_internal(service) == VCHIQ_RETRY)
return; /* Bail out if not ready */
}
break;
case VCHIQ_SRVSTATE_OPENING:
/* A client is mid-open - this is a rejection, so just fail the open */
vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSEWAIT);
local_event_signal(&service->remove_event);
break;
default:
break;
}
}
}
break;
case VCHIQ_MSG_DATA:
{
unsigned int localport = VCHIQ_MSG_DSTPORT(header->fourcc);
unsigned int remoteport = VCHIQ_MSG_SRCPORT(header->fourcc);
service = &local->services[localport];
if (vcos_verify((localport < VCHIQ_MAX_SERVICES) &&
(service->remoteport == remoteport)) &&
(service->srvstate == VCHIQ_SRVSTATE_OPEN))
{
if (service->base.callback(VCHIQ_MESSAGE_AVAILABLE, header, &service->base, NULL) == VCHIQ_RETRY)
return; /* Bail out if not ready */
header = NULL; /* Don't invalidate this message - defer till vchiq_release */
}
}
break;
case VCHIQ_MSG_CONNECT:
vcos_event_signal(&state->connect);
break;
case VCHIQ_MSG_INVALID:
default:
break;
}
remote->ctrl.process += stride;
if (header != NULL)
{
/* Invalidate it */
header->fourcc = VCHIQ_FOURCC_INVALID;
/* Notify the other end there is some space */
remote_event_signal(&remote->ctrl.remove_event);
}
}
}
VCOS_STATUS_T vcos_platform_init(void)
{
VCOS_STATUS_T st;
uint32_t flags = 0;
int pst;
st = _vcos_named_semaphore_init();
if (!vcos_verify(st == VCOS_SUCCESS))
goto end;
flags |= VCOS_INIT_NAMED_SEM;
#ifdef HAVE_MTRACE
/* enable glibc memory debugging, if the environment
* variable MALLOC_TRACE names a valid file.
*/
mtrace();
#endif
#ifdef ANDROID
st = vcos_mutex_create(&printf_lock, "printf");
if (!vcos_verify(st == VCOS_SUCCESS))
goto end;
flags |= VCOS_INIT_PRINTF_LOCK;
#endif
st = vcos_once(¤t_thread_key_once, current_thread_key_init);
if (!vcos_verify(st == VCOS_SUCCESS))
goto end;
/* Initialise a VCOS wrapper for the thread which called vcos_init. */
st = vcos_semaphore_create(&vcos_thread_main.suspend, NULL, 0);
if (!vcos_verify(st == VCOS_SUCCESS))
goto end;
flags |= VCOS_INIT_MAIN_SEM;
vcos_thread_main.thread = pthread_self();
pst = pthread_setspecific(_vcos_thread_current_key, &vcos_thread_main);
if (!vcos_verify(pst == 0))
{
st = VCOS_EINVAL;
goto end;
}
st = vcos_msgq_init();
if (!vcos_verify(st == VCOS_SUCCESS))
goto end;
flags |= VCOS_INIT_MSGQ;
vcos_logging_init();
end:
if (st != VCOS_SUCCESS)
vcos_term(flags);
return st;
}
uint32_t vg_tess_arc_to_cubics(
float *p,
float p0_x, float p0_y, float p1_x, float p1_y, float p1_m_p0_x, float p1_m_p0_y,
float r_x, float r_y, float angle,
bool large, bool cw)
{
float u1_x, u1_y;
float c_x, c_y; /* center */
float d_sq, s_sq;
float u0_angle, u1_angle;
uint32_t count;
/* coverity [uninit] : Coverity doesn't follow initialisation in vg_mat3x3_set_identity() */
VG_MAT3X3_T a;
uint32_t i;
if ((r_x < EPS) || (r_y < EPS)) {
/* degenerate ellipse, approximate as line */
line_to_cubic(p, p0_x, p0_y, p1_x, p1_y);
return 1;
}
/*
transform p0/p1 - p0 on ellipse - p0 to u0/u1 on unit circle with center c
*/
/* u0 = (0, 0) */
ellipse_to_unit_circle(&u1_x, &u1_y, p1_m_p0_x, p1_m_p0_y, r_x, r_y, angle);
/*
d = chord length = length(u1 - u0)
l = distance from middle of chord to c = sqrt(1 - (d/2)^2)
s = l/d = sqrt(1/d^2 - 1/4)
*/
d_sq = (u1_x * u1_x) + (u1_y * u1_y);
if (d_sq < EPS) {
/* points almost coincident, approximate as line */
line_to_cubic(p, p0_x, p0_y, p1_x, p1_y);
return 1;
}
s_sq = recip_(d_sq) - 0.25f;
if (s_sq < 0.0f) {
/*
chord length >= 2, points are too far apart for both to be able to lie on ellipse
spec says should uniformly scale ellipse so that it is just big enough
this will result in a chord of length 2, with c = (u0 + u1)/2
scaling ellipse by s will cause chord length to scale by 1/s
want chord length = 2, so want to scale chord length by 2/d, so want to scale ellipse by d/2
*/
float scale = 0.5f * sqrt_(d_sq);
r_x *= scale;
r_y *= scale;
ellipse_to_unit_circle(&u1_x, &u1_y, p1_m_p0_x, p1_m_p0_y, r_x, r_y, angle);
c_x = u1_x * 0.5f;
c_y = u1_y * 0.5f;
} else {
float s;
/*
moving a distance l perpendicular to chord from middle of chord m will get us to the two possible centers
which center to use depends on large/cw
*/
c_x = u1_x * 0.5f;
c_y = u1_y * 0.5f;
s = sqrt_(s_sq);
if (large ^ cw) {
c_x += s * u1_y;
c_y -= s * u1_x;
} else {
c_x -= s * u1_y;
c_y += s * u1_x;
}
}
/*
approximate arc along unit circle
*/
u0_angle = atan2_(-c_y, -c_x);
u1_angle = atan2_(u1_y - c_y, u1_x - c_x);
count = unit_arc_to_cubics(p, u1_angle - u0_angle, cw);
vcos_assert((count != 0) && (count <= 4));
/*
transform cubics to ellipse
rotate ccw by u0_angle
translate by c
scale by r
rotate ccw by angle
translate by p0
*/
vg_mat3x3_set_identity(&a);
vg_mat3x3_premul_rotate(&a, u0_angle);
vg_mat3x3_premul_translate(&a, c_x, c_y);
vg_mat3x3_premul_scale(&a, r_x, r_y);
vg_mat3x3_premul_rotate(&a, angle);
vg_mat3x3_premul_translate(&a, p0_x, p0_y);
#ifndef NDEBUG
vcos_verify(vg_mat3x3_is_affine(&a)); /* why vcos_verify? see vg_mat3x3_is_affine... */
#endif
for (i = 0; i != (count * 8); i += 2) {
vg_mat3x3_affine_transform(&a, p + i, p + i + 1);
}
/*
force first and last points to coincide exactly with p0/p1
*/
p[0] = p0_x;
p[1] = p0_y;
p[(count * 8) - 2] = p1_x;
p[(count * 8) - 1] = p1_y;
return count;
}
void wfc_source_or_mask_acquire(WFC_SOURCE_OR_MASK_T *source_or_mask_ptr)
// Indicate that the image provider is now linked to an element.
{
if(vcos_verify(source_or_mask_ptr != NULL))
{source_or_mask_ptr->refcount++;}
} // wfc_source_or_mask_acquire()
/** Send a message and wait for a reply.
*
* @param client client to send message for
* @param msg_header message vchiq_header to send
* @param size length of message, including header
* @param msgid message id
* @param dest destination for reply
* @param destlen size of destination, updated with actual length
* @param send_dummy_bulk whether to send a dummy bulk transfer
*/
MMAL_STATUS_T mmal_vc_sendwait_message(struct MMAL_CLIENT_T *client,
mmal_worker_msg_header *msg_header,
size_t size,
uint32_t msgid,
void *dest,
size_t *destlen,
MMAL_BOOL_T send_dummy_bulk)
{
MMAL_STATUS_T ret;
MMAL_WAITER_T *waiter;
VCHIQ_STATUS_T vst;
VCHIQ_ELEMENT_T elems[] = {{msg_header, size}};
vcos_assert(size >= sizeof(mmal_worker_msg_header));
vcos_assert(dest);
if (!client->inited)
{
vcos_assert(0);
return MMAL_EINVAL;
}
if (send_dummy_bulk)
vcos_mutex_lock(&client->bulk_lock);
waiter = get_waiter(client);
msg_header->msgid = msgid;
msg_header->u.waiter = waiter;
msg_header->magic = MMAL_MAGIC;
waiter->dest = dest;
waiter->destlen = *destlen;
LOG_TRACE("wait %p, reply to %p", waiter, dest);
mmal_vc_use_internal(client);
vst = vchiq_queue_message(client->service, elems, 1);
if (vst != VCHIQ_SUCCESS)
{
ret = MMAL_EIO;
if (send_dummy_bulk)
vcos_mutex_unlock(&client->bulk_lock);
goto fail_msg;
}
if (send_dummy_bulk)
{
uint32_t data_size = 8;
/* The data is just some dummy bytes so it's fine for it to be static */
static uint8_t data[8];
vst = vchiq_queue_bulk_transmit(client->service, data, data_size, msg_header);
vcos_mutex_unlock(&client->bulk_lock);
if (!vcos_verify(vst == VCHIQ_SUCCESS))
{
LOG_ERROR("failed bulk transmit");
/* This really should not happen and if it does, things will go wrong as
* we've already queued the vchiq message above. */
vcos_assert(0);
ret = MMAL_EIO;
goto fail_msg;
}
}
/* now wait for the reply...
*
* FIXME: we could do with a timeout here. Need to be careful to cancel
* the semaphore on a timeout.
*/
vcos_semaphore_wait(&waiter->sem);
mmal_vc_release_internal(client);
LOG_TRACE("got reply (len %i/%i)", (int)*destlen, (int)waiter->destlen);
*destlen = waiter->destlen;
release_waiter(client, waiter);
return MMAL_SUCCESS;
fail_msg:
mmal_vc_release_internal(client);
release_waiter(client, waiter);
return ret;
}
/** 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;
}
