/***********************************************************
* Name: bulk_aux_callback
*
* Arguments: void *callback_param
* const VCHI_CALLBACK_REASON_T reason
* void *handle
*
* Description: Handles callbacks for received messages
*
* Returns: -
*
***********************************************************/
static void bulk_aux_callback( void *callback_param, //my service local param
const VCHI_CALLBACK_REASON_T reason,
void *handle )
{
BULK_AUX_SERVICE_INFO_T * service_info = (BULK_AUX_SERVICE_INFO_T *)callback_param;
switch(reason)
{
case VCHI_CALLBACK_MSG_AVAILABLE:
#if defined VCHI_COARSE_LOCKING
os_semaphore_obtain(&service_info->connection->sem);
#endif
service_info->connection->api->bulk_aux_received(service_info->connection->state);
#if defined VCHI_COARSE_LOCKING
os_semaphore_release(&service_info->connection->sem);
#endif
break;
case VCHI_CALLBACK_MSG_SENT:
#if defined VCHI_COARSE_LOCKING
os_semaphore_obtain(&service_info->connection->sem);
#endif
service_info->connection->api->bulk_aux_transmitted(service_info->connection->state, handle);
#if defined VCHI_COARSE_LOCKING
os_semaphore_release(&service_info->connection->sem);
#endif
break;
case VCHI_CALLBACK_BULK_RECEIVED:
case VCHI_CALLBACK_BULK_DATA_READ:
case VCHI_CALLBACK_BULK_SENT: // bulk auxiliary service doesn't use bulk transfers(!)
os_assert(0);
break;
}
}
void vc_vchi_gencmd_init (VCHI_INSTANCE_T initialise_instance, VCHI_CONNECTION_T **connections, uint32_t num_connections ) {
int32_t success;
int i;
// record the number of connections
memset( &gencmd_client, 0, sizeof(GENCMD_SERVICE_T) );
gencmd_client.num_connections = (int) num_connections;
success = os_semaphore_create( &gencmd_client.sema, OS_SEMAPHORE_TYPE_SUSPEND );
assert( success == 0 );
success = os_semaphore_create( &gencmd_message_available_semaphore, OS_SEMAPHORE_TYPE_BUSY_WAIT );
assert( success == 0 );
success = os_semaphore_obtain( &gencmd_message_available_semaphore );
assert( success == 0 );
for (i=0; i<gencmd_client.num_connections; i++) {
// Create a 'LONG' service on the each of the connections
SERVICE_CREATION_T gencmd_parameters = { MAKE_FOURCC("GCMD"), // 4cc service code
connections[i], // passed in fn ptrs
0, // tx fifo size (unused)
0, // tx fifo size (unused)
&gencmd_callback, // service callback
&gencmd_message_available_semaphore, // callback parameter
VC_FALSE, // want_unaligned_bulk_rx
VC_FALSE, // want_unaligned_bulk_tx
VC_FALSE // want_crc
};
success = vchi_service_open( initialise_instance, &gencmd_parameters, &gencmd_client.open_handle[i] );
assert( success == 0 );
}
}
/***********************************************************
* Name: os_cond_signal
*
* Arguments: OS_COND_T *cond
*
* Description: Routine to signal at least one thread
* waiting on a condition variable. The
* caller must say whether they have obtained
* the semaphore.
*
* Returns: int32_t - success == 0
*
***********************************************************/
int32_t os_cond_signal( OS_COND_T *cond, bool_t sem_claimed )
{
int32_t success = -1;
if (cond)
{
COND_WAITER_T *w;
// Ensure the condvar's semaphore is claimed for thread-safe access
if (!sem_claimed)
{
success = os_semaphore_obtain((*cond)->semaphore);
os_assert(success == 0);
}
w = (*cond)->waiters;
if (w)
{
// Wake the first person waiting
vcos_event_signal(&w->latch);
(*cond)->waiters = w->next;
}
success = 0;
if (!sem_claimed)
{
success = os_semaphore_release((*cond)->semaphore);
}
os_assert(success == 0);
}
return success;
}
/* ----------------------------------------------------------------------
* received response to an ILCS command
* -------------------------------------------------------------------- */
static void
vc_ilcs_response( uint32_t xid, unsigned char *msg, int len )
{
VC_ILCS_WAIT_T *wait;
int i;
// atomically retrieve given ->wait entry
os_semaphore_obtain( &vc_ilcsg.wait_sem );
for (i=0; i<VC_ILCS_MAX_WAITING; i++) {
wait = &vc_ilcsg.wait[i];
if ( wait->resp && wait->xid == xid )
break;
}
os_semaphore_release( &vc_ilcsg.wait_sem );
if ( i == VC_ILCS_MAX_WAITING ) {
// something bad happened
assert(0);
return;
}
// extract command from fifo and place in response buffer.
memcpy( wait->resp, msg, len );
//os_logging_message( "ilcs_poll_fifo: waking waiter %d", i );
os_semaphore_release( &wait->sem );
}
/***********************************************************
* Name: os_cond_broadcast
*
* Arguments: OS_COND_T *cond
* bool_t sem_claimed
*
* Description: Routine to signal all threads waiting on
* a condition variable. The caller must
* say whether they have obtained the semaphore.
*
* Returns: int32_t - success == 0
*
***********************************************************/
int32_t os_cond_broadcast( OS_COND_T *cond, bool_t sem_claimed )
{
int32_t success = -1;
if (cond)
{
COND_WAITER_T *w;
// Ensure the condvar's semaphore is claimed for thread-safe access
if (!sem_claimed)
{
success = os_semaphore_obtain((*cond)->semaphore);
os_assert(success == 0);
}
for (w = (*cond)->waiters; w; w = w->next)
{
vcos_event_signal(&w->latch);
}
(*cond)->waiters = NULL;
success = 0;
if (!sem_claimed)
{
success = os_semaphore_release((*cond)->semaphore);
}
os_assert(success == 0);
}
return success;
}
/***********************************************************
* Name: cecservice_notify_func
*
* Arguments: CEC service state
*
* Description: This is the notification task which receives all CEC
* service notifications
*
* Returns: does not return
*
***********************************************************/
static void cecservice_notify_func( unsigned int argc, void *argv ) {
int32_t success;
CECSERVICE_HOST_STATE_T *state = (CECSERVICE_HOST_STATE_T *) argv;
//first send a dummy message to the Videocore to let it know we are ready
cecservice_send_command(VC_CEC_END_OF_LIST, NULL, 0, 0);
while(1) {
success = os_semaphore_obtain(&cecservice_notify_available_semaphore);
vcos_assert(!success && state->initialised);
do {
uint32_t reason, param1, param2;
//Get all notifications in the queue
success = vchi_msg_dequeue( state->notify_handle[0], state->notify_buffer, sizeof(state->notify_buffer), &state->notify_length, VCHI_FLAGS_NONE );
if(success != 0 || state->notify_length < sizeof(uint32_t)*3 ) {
continue;
}
lock_obtain();
//Check what notification it is and update ourselves accordingly before notifying the host app
//All notifications are of format: reason, param1, param2 (all 32-bit unsigned int)
reason = VC_VTOH32(state->notify_buffer[0]), param1 = VC_VTOH32(state->notify_buffer[1]), param2 = VC_VTOH32(state->notify_buffer[2]);
//Unlike the TV service we have nothing to store here, so just call the callback
lock_release();
//Now callback the host app
if(state->notify_fn) {
(*state->notify_fn)(state->notify_data, reason, param1, param2);
}
} while(success == 0 && state->notify_length >= sizeof(uint32_t)*3); //read the next message if any
} //while (1)
}
/***********************************************************
* Name: os_cond_wait
*
* Arguments: OS_COND_T *cond,
* OS_SEMAPHORE_T *semaphore
*
* Description: Routine to wait for a condition variable
* to be signalled. Semaphore is released
* while waiting. The same semaphore must
* always be used.
*
* Returns: int32_t - success == 0
*
***********************************************************/
int32_t os_cond_wait( OS_COND_T *cond, OS_SEMAPHORE_T *semaphore )
{
int32_t success = -1;
if (cond && semaphore)
{
COND_WAITER_T w;
COND_WAITER_T *p, **prev;
// Check the API is being followed
os_assert((*cond)->semaphore == semaphore && os_semaphore_obtained(semaphore));
// Fill in a new waiter structure allocated on our stack
w.next = NULL;
vcos_demand(vcos_event_create(&w.latch, NULL) == VCOS_SUCCESS);
// Add it to the end of the condvar's wait queue (we wake first come, first served)
prev = &(*cond)->waiters;
p = (*cond)->waiters;
while (p)
prev = &p->next, p = p->next;
*prev = &w;
// Ready to go to sleep now
success = os_semaphore_release(semaphore);
os_assert(success == 0);
vcos_event_wait(&w.latch);
success = os_semaphore_obtain(semaphore);
os_assert(success == 0);
}
return success;
}
/* ----------------------------------------------------------------------
* remove the slot with the given address from the given non-wrap fifo.
* intended for out-of-order operations
*
* the given address must belong to a slot between the read and write
* slots, and be in the 'ready' state
*
* returns 0 on success, non-0 otherwise
* -------------------------------------------------------------------- */
int32_t
non_wrap_fifo_remove( VCHI_NWFIFO_T *_fifo, void *address )
{
NON_WRAP_FIFO_HANDLE_T *fifo = (NON_WRAP_FIFO_HANDLE_T *)_fifo;
int32_t success = -1;
uint32_t slot;
os_semaphore_obtain( &fifo->sem );
slot = fifo->read_slot;
os_assert( fifo->base_address );
while( slot != fifo->write_slot ) {
if ( fifo->slot_info[slot].address == address && fifo->slot_info[slot].state == NON_WRAP_READY ) {
// mark this as no longer being in use
fifo->slot_info[slot].state = NON_WRAP_NOT_IN_USE;
success = 0;
break;
}
// increment and wrap our slot number
slot = next_slot( fifo, slot );
}
// if the entry to be removed was the current read slot then we need to move on
if ( slot == fifo->read_slot && success == 0 )
fifo->read_slot = next_read_slot( fifo, slot );
os_semaphore_release( &fifo->sem );
return success;
}
/* ----------------------------------------------------------------------
* fetch the address and length of the oldest message in the given
* non-wrap fifo
*
* NOTE: on success, this routine will leave the fifo locked!
*
* returns 0 on success, non-0 otherwise
* -------------------------------------------------------------------- */
int32_t
non_wrap_fifo_request_read_address( VCHI_NWFIFO_T *_fifo, const void **address, uint32_t *length )
{
NON_WRAP_FIFO_HANDLE_T *fifo = (NON_WRAP_FIFO_HANDLE_T *)_fifo;
int32_t success = -1;
os_semaphore_obtain( &fifo->sem );
os_assert( fifo->base_address );
// check that this slot is ready to be read from
if ( fifo->slot_info[fifo->read_slot].state == NON_WRAP_READY ) {
*address = fifo->slot_info[fifo->read_slot].address;
*length = fifo->slot_info[fifo->read_slot].length;
// mark this slot as being in the process of being read
fifo->slot_info[fifo->read_slot].state = NON_WRAP_READING;
// success!
success = 0;
}
// on success, the fifo is left locked
if ( success != 0 )
os_semaphore_release( &fifo->sem );
return success;
}
/******************************************************************************
NAME
vc_gencmd_read_response
SYNOPSIS
int vc_gencmd_read_response
FUNCTION
Block until something comes back
RETURNS
Error code from dequeue message
******************************************************************************/
int vc_gencmd_read_response (char *response, int maxlen) {
int i = 0;
int success = 0;
int ret_code = 0;
int32_t sem_ok = 0;
//Note this will ALWAYS reset response buffer and overwrite any partially read responses
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
for(i = 0; i < gencmd_client.num_connections; i++) {
//Check if there is something in the queue, if so return immediately
//otherwise wait for the semaphore and read again
success = (int) vchi_msg_dequeue( gencmd_client.open_handle[i], gencmd_client.response_buffer, sizeof(gencmd_client.response_buffer), &gencmd_client.response_length, VCHI_FLAGS_NONE);
if(success == 0) {
ret_code = VC_VTOH32( *(int *)gencmd_client.response_buffer );
break;
} else {
gencmd_client.response_length = 0;
}
}
} while(!gencmd_client.response_length && (sem_ok = os_semaphore_obtain( &gencmd_message_available_semaphore)) == 0);
if(gencmd_client.response_length && sem_ok == 0) {
gencmd_client.response_length -= sizeof(int); //first word is error code
memcpy(response, gencmd_client.response_buffer+sizeof(int), (size_t) OS_MIN((int)gencmd_client.response_length, (int)maxlen));
}
// If we read anything, return the VideoCore code. Error codes < 0 mean we failed to
// read anything...
//How do we let the caller know the response code of gencmd?
//return ret_code;
return success;
}
//Receiver thread
static void hostreqclient_func(unsigned int argc, void *argv) {
int32_t success;
int32_t i, nothing;
while(1) { //Run forever waiting for a callback to happen
// wait for the semaphore to say that there is a message
success = os_semaphore_obtain(&hostreq_client_message_available);
assert( success == 0 );
while(1) { //Read until we can read no more on all interfaces
nothing = 0;
for(i=0;i<hostreq_client.num_connections;i++) {
success = vchi_msg_dequeue(hostreq_client.client_handle[i], hostreq_client.response_buffer, sizeof(hostreq_client.response_buffer), &hostreq_client.response_length, VCHI_FLAGS_NONE);
if(success == 0) {
hostreq_request_handler(i, &hostreq_client);
} else {
nothing++;
}
}
//When there is nothing on all interfaces, wait for a callback
if(nothing == hostreq_client.num_connections) {
break;
}
}
}
}
/* ----------------------------------------------------------------------
* read data from the given non-wrap fifo, by copying out of the
* fifo to the given address
*
* returns 0 on success, non-0 otherwise
* -------------------------------------------------------------------- */
int32_t
non_wrap_fifo_read( VCHI_NWFIFO_T *_fifo, void *data, const uint32_t max_data_size )
{
NON_WRAP_FIFO_HANDLE_T *fifo = (NON_WRAP_FIFO_HANDLE_T *)_fifo;
int32_t success = -1;
uint8_t *address = NULL;
uint32_t length;
os_assert(0); // FIXME: will deadlock
os_semaphore_obtain( &fifo->sem );
os_assert( fifo->base_address );
// request the address and length for this transfer
if ( non_wrap_fifo_request_read_address(fifo,&address,&length) == 0 ) {
if ( length <= max_data_size ){
// there is enough space and the data has been written so we can copy it out
memcpy(data,address,length);
// now tidy up our information about what is in the FIFO
if ( non_wrap_fifo_read_complete(fifo,address) == 0 ) {
// success!
success = 0;
}
}
}
os_semaphore_release( &fifo->sem );
return success;
}
void ThreadOne(unsigned int a, void *b)
{
TEST_OK("os_semaphore_obtain", os_semaphore_obtain(&sem1));
flag = 1;
TEST_OK("os_semaphore_release", os_semaphore_release(&sem1));
}
void ThreadCheck()
{
VCOS_THREAD_T thread;
// First create a sync semaphore
TEST_OK("os_semaphore_create", os_semaphore_create(&sem1, OS_SEMAPHORE_TYPE_SUSPEND));
// and grab it
TEST_OK("os_semaphore_obtain", os_semaphore_obtain(&sem1));
// Start the thread which shuold stall on the semaphore
TEST_OK("os_thread_start", os_thread_start( &thread, &ThreadOne, NULL, 1000, "ThreadOne"));
// Wait for thread to start for 1000 - should stall on semaphore and not change flag
TEST_OK("os_delay", os_delay(1000));
// Check global data is unchanged
TEST_OK("Flag = 0", flag);
// release semaphore
TEST_OK("os_semaphore_release", os_semaphore_release(&sem1));
// Wait for thread to continue for 100 - should be enough for it to set the flag
TEST_OK("os_delay", os_delay(100));
// Check global data is changed to non-zero
TEST_OK("Flag = 1", (flag == 0));
// Semaphore is currently released, so check the obtained function is correct.
TEST_OK("os_semaphore_obtained - 0", os_semaphore_obtained(&sem1))
// now get the sema and test again
TEST_OK("os_semaphore_obtain", os_semaphore_obtain(&sem1));
TEST_OK("os_semaphore_obtained - 1", (os_semaphore_obtained(&sem1) == 0))
TEST_OK("os_semaphore_release", os_semaphore_release(&sem1));
}
/***********************************************************
* Name: cecservice_wait_for_reply
*
* Arguments: response buffer, buffer length
*
* Description: blocked until something is in the buffer
*
* Returns error code of vchi
*
***********************************************************/
static int32_t cecservice_wait_for_reply(void *response, uint32_t max_length) {
int32_t success = 0;
int32_t sem_ok = 0;
uint32_t length_read = 0;
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( cecservice_client.client_handle[0], response, max_length, &length_read, VCHI_FLAGS_NONE );
} while( length_read == 0 && (sem_ok = os_semaphore_obtain( &cecservice_message_available_semaphore)) == 0);
return success;
}
int vc_ilcs_componentlock_change(int val)
{
OS_SEMAPHORE_T *lock = (OS_SEMAPHORE_T*)vc_ilcs_component_lock();
if(NULL == lock)
{
VC_DEBUG( Trace, "Error: invalid lock\n");
return -1;
}
if(val)
{
os_semaphore_obtain(lock);
}
else
{
os_semaphore_release(lock);
}
return 0;
}
/* ----------------------------------------------------------------------
* send a message and wait for reply.
* repeats continuously, on each connection
* -------------------------------------------------------------------- */
static void
vc_ilcs_task( unsigned argc, void *argv )
{
// FIXME: figure out why initial connect() doesn't block...
//os_sleep( 50 );
for (;;) {
// wait to receive one or more messages
os_semaphore_obtain( &vc_ilcsg.rxmsg_sem );
while( vc_ilcs_process_message() )
;
}
// FIXME: once we implement service close
//err = OMX_Deinit();
//assert( err == OMX_ErrorNone );
//filesys_deregister();
}
/* ----------------------------------------------------------------------
* initialise host-side OpenMAX IL component service
* -------------------------------------------------------------------- */
void
vc_vchi_ilcs_init( VCHI_INSTANCE_T initialise_instance,
VCHI_CONNECTION_T **connections,
uint32_t num_connections )
{
int32_t success;
memset( &vc_ilcsg, 0, sizeof(VC_ILCS_GLOBALS_T) );
//
sema_init(&vc_ilcsg.msg_prod, 0);
sema_init(&vc_ilcsg.msg_cons, 1);
sema_init(&vc_ilcsg.omxlog_lock, 1);
vc_ilcsg.omxlog_rindex = 0;
vc_ilcsg.omxlog_windex = 0;
vc_ilcsg.omxlog = vmalloc(VC03_OMXLOG_MAX);
memset(vc_ilcsg.omxlog, 0, VC03_OMXLOG_MAX);
// create thread semaphore for blocking
os_semaphore_create( &vc_ilcsg.component_lock, OS_SEMAPHORE_TYPE_SUSPEND );
os_semaphore_create( &vc_ilcsg.rxmsg_sem, OS_SEMAPHORE_TYPE_SUSPEND );
os_semaphore_obtain( &vc_ilcsg.rxmsg_sem );
// create semaphore for protecting wait/xid structures
os_semaphore_create( &vc_ilcsg.wait_sem, OS_SEMAPHORE_TYPE_SUSPEND );
// open 'ILCS' service
SERVICE_CREATION_T parameters = { MAKE_FOURCC("ILCS"), // 4cc service code
connections[0], // passed in fn ptrs
0, // tx fifo size (unused)
0, // tx fifo size (unused)
&vc_ilcs_callback, // service callback
&vc_ilcsg.rxmsg_sem }; // callback parameter
success = vchi_service_open( initialise_instance, ¶meters, &vc_ilcsg.vchi_handle );
assert( success == 0 );
success = os_thread_start( &vc_ilcsg.thread, vc_ilcs_task, NULL, 4000, "ILCS_HOST" );
assert( success == 0 );
}
void vc_vchi_hostreq_init (VCHI_INSTANCE_T initialise_instance, VCHI_CONNECTION_T **connections, uint32_t num_connections ) {
int success, i;
// record the number of connections
memset( &hostreq_client, 0, sizeof(HOSTREQ_SERVICE_T) );
hostreq_client.num_connections = num_connections;
success = os_semaphore_create( &hostreq_client.sema, OS_SEMAPHORE_TYPE_SUSPEND );
assert( success == 0 );
success = os_semaphore_create( &hostreq_client_message_available, OS_SEMAPHORE_TYPE_BUSY_WAIT );
assert( success == 0 );
success = os_semaphore_obtain(&hostreq_client_message_available);
assert( success == 0 );
for (i=0; i<hostreq_client.num_connections; i++) {
// Create a 'Client' service on the each of the connections
SERVICE_CREATION_T hostreq_parameters = { HOSTREQ_NAME, // 4cc service code
connections[i], // passed in fn ptrs
0, // tx fifo size (unused)
0, // tx fifo size (unused)
&hostreq_client_callback, // service callback
&hostreq_client_message_available }; // callback parameter
success = vchi_service_open( initialise_instance, &hostreq_parameters, &hostreq_client.client_handle[i] );
assert( success == 0 );
}
success = os_thread_start(&hostreq_client_task, &hostreqclient_func, NULL, 2048, "HOSTREQ task");
assert(!success);
if(!success) {
hostreq_client.initialised = 1;
//Send a time across immediately
time_t dummytime = 0; //We need to get the real time on a real host
success = hostreq_send_command(VC_HOSTREQ_TIME, &dummytime, sizeof(dummytime));
assert( success == 0 );
}
}
/* ----------------------------------------------------------------------
* write data to the given non-wrap fifo, by copying from the given
* address
*
* returns 0 on success, non-0 otherwise
* -------------------------------------------------------------------- */
int32_t
non_wrap_fifo_write( VCHI_NWFIFO_T *_fifo, const void *data, const uint32_t data_size )
{
NON_WRAP_FIFO_HANDLE_T *fifo = (NON_WRAP_FIFO_HANDLE_T *)_fifo;
int32_t success = -1;
uint8_t *address = NULL;
os_assert( fifo->base_address );
os_assert(0); // FIXME: will deadlock
os_semaphore_obtain( &fifo->sem );
// request the address and length for this transfer
if ( non_wrap_fifo_request_write_address(fifo,&address,data_size) == 0 ) {
// there is enough space so we can write the data
memcpy(address,data,data_size);
// now tidy up our information about what is in the FIFO
if ( non_wrap_fifo_write_complete(fifo,address) == 0 ) {
// success!
success = 0;
}
}
os_semaphore_release( &fifo->sem );
return success;
}
/***********************************************************
* Name: vchi_control_service_init
*
* Arguments: VCHI_CONNECTION_T *connections,
* const uint32_t num_connections
*
* Description: Routine to init the control service
*
* Returns: int32_t - success == 0
*
***********************************************************/
int32_t vchi_control_service_init( VCHI_CONNECTION_T **connections,
const uint32_t num_connections,
void **state )
{
int32_t success = 0;
uint32_t count = 0;
CONTROL_SERVICE_INFO_T *control_info = (CONTROL_SERVICE_INFO_T *)*state;
os_assert(num_connections <= VCHI_MAX_NUM_CONNECTIONS);
if (!control_info)
{
control_info = (CONTROL_SERVICE_INFO_T *)os_malloc( VCHI_MAX_NUM_CONNECTIONS * sizeof(CONTROL_SERVICE_INFO_T), 0, "vchi:control_info" );
memset( control_info, 0,VCHI_MAX_NUM_CONNECTIONS * sizeof(CONTROL_SERVICE_INFO_T) );
for( count = 0; count < num_connections; count++ )
{
#ifdef VCHI_COARSE_LOCKING
os_semaphore_obtain(&connections[count]->sem);
#endif
// create and obtain the semaphore used to signal when we have connected
success += os_semaphore_create( &control_info[count].connected_semaphore, OS_SEMAPHORE_TYPE_BUSY_WAIT );
os_assert( success == 0 );
// record the connection info
control_info[count].connection = connections[count];
// create the server (this is a misnomer as the the CTRL service acts as both client and server
success += (connections[count])->api->service_connect((connections[count])->state,MAKE_FOURCC("CTRL"),0,0,VC_TRUE,control_callback,&control_info[count],VC_FALSE,VC_FALSE,VC_FALSE,&control_info[count].open_handle);
os_assert(success == 0);
#ifdef VCHI_COARSE_LOCKING
os_semaphore_release(&connections[count]->sem);
#endif
}
// start timestamps from zero
time_offset = 0;
time_offset -= vchi_control_get_time();
}
else
{
for ( count = 0; count < num_connections; count++ )
{
os_assert(control_info[count].connection == connections[count]);
}
}
// because we can have both ends of a connection running on one processor we need to send down the CONNECTION command after all
// the connections have been connected
for( count = 0; count < num_connections; count++ )
{
#ifdef VCHI_COARSE_LOCKING
os_semaphore_obtain(&connections[count]->sem);
#endif
// record that we have not yet handled an INIT request
control_info[count].initialised = VC_FALSE;
success = os_semaphore_obtain( &control_info[count].connected_semaphore );
os_assert( success == 0 );
success = vchi_control_queue_connect( &control_info[count], VC_FALSE );
os_assert(success == 0);
#ifdef VCHI_COARSE_LOCKING
os_semaphore_release(&connections[count]->sem);
#endif
}
// now we must wait for the connections to have their INIT returned
for( count = 0; count < num_connections; count++ )
{
success = os_semaphore_obtain( &control_info[count].connected_semaphore );
os_assert( success == 0 );
success = os_semaphore_release( &control_info[count].connected_semaphore );
os_assert( success == 0 );
}
*state = control_info;
return success;
}
/***********************************************************
* Name: control_callback
*
* Arguments: void *callback_param
* const VCHI_CALLBACK_REASON_T reason
* const void *handle
*
* Description: Handles callbacks for received messages
*
* Returns: -
*
***********************************************************/
static void control_callback( void *callback_param, //my service local param
const VCHI_CALLBACK_REASON_T reason,
void *handle )
{
CONTROL_SERVICE_INFO_T * service_info = (CONTROL_SERVICE_INFO_T *)callback_param;
uint8_t *message;
uint8_t output_message[128];
uint32_t message_length;
int32_t return_value;
fourcc_t service_id;
uint32_t flags;
VCHI_FLAGS_T output_flags = VCHI_FLAGS_NONE;
switch(reason)
{
case VCHI_CALLBACK_MSG_AVAILABLE:
{
// handle the message in place
void *message_handle;
#if defined VCHI_COARSE_LOCKING
os_semaphore_obtain(&service_info->connection->sem);
#endif
#ifdef VCHI_FEWER_MSG_AVAILABLE_CALLBACKS
do {
message_handle = NULL;
#endif
return_value = service_info->connection->api->service_hold_msg(service_info->open_handle,(void**)&message,&message_length,VCHI_FLAGS_NONE,&message_handle);
if(return_value == 0)
{
if (message_length>0)
{
// this is a valid message - read the command
uint32_t command = vchi_readbuf_uint32(&message[0]);
int32_t reply_required = VC_FALSE;
switch(command)
{
case CONNECT:
os_logging_message("CTRL SERVICE: Connect message");
if(service_info->initialised == VC_FALSE)
{
uint32_t protocol_version;
uint32_t slot_size;
uint32_t num_slots;
uint32_t min_bulk_size;
return_value = os_semaphore_release( &service_info->connected_semaphore );
os_assert( return_value == 0 );
// record that we have been initialised
service_info->initialised = VC_TRUE;
// extract the values we need to pass back to the service - principally slot size for the moment
protocol_version = vchi_readbuf_uint32(&message[4]);
slot_size = vchi_readbuf_uint32(&message[8]);
num_slots = vchi_readbuf_uint32(&message[12]);
min_bulk_size = message_length >= 20 ? vchi_readbuf_uint32(&message[16]) : 0;
//os_assert(0);
service_info->connection->api->connection_info(service_info->connection->state,protocol_version, slot_size, num_slots, min_bulk_size);
// we are going to reply with 'CONNECT'
return_value = vchi_control_queue_connect( service_info, VC_TRUE );
os_assert( return_value == 0);
}
break;
case SERVER_AVAILABLE:
{
VCHI_SERVICE_FLAGS_T peer_flags = (VCHI_SERVICE_FLAGS_T)(message_length >= 12 ? vchi_readbuf_uint32(&message[8]) : 0);
int32_t our_flags;
service_id = vchi_readbuf_fourcc(&message[4]);
os_logging_message("CTRL SERVICE: Server available (%c%c%c%c:0x%x)",FOURCC_TO_CHAR(service_id),peer_flags);
message_length = 12;
// we are replying
reply_required = VC_TRUE;
// first part of the reply is the command
vchi_writebuf_uint32( &output_message[0], SERVER_AVAILABLE_REPLY );
// then the requested server ID
vchi_writebuf_fourcc( &output_message[4], service_id );
// check if this fourcc is in our list of servers on this connection
our_flags = service_info->connection->api->server_present(service_info->connection->state, service_id, peer_flags);
if(our_flags >= 0)
vchi_writebuf_uint32( &output_message[8], AVAILABLE | our_flags );
else
vchi_writebuf_uint32( &output_message[8], 0 );
break;
}
case SERVER_AVAILABLE_REPLY:
// Connection can take ownership of the message - signalled by returning true.
service_id = vchi_readbuf_fourcc(&message[4]);
flags = vchi_readbuf_uint32(&message[8]);
service_info->connection->api->server_available_reply(service_info->connection->state, service_id, flags);
break;
case BULK_TRANSFER_RX:
{
uint32_t length, channel_params, data_length, data_offset;
MESSAGE_TX_CHANNEL_T channel;
// extract the service and length and pass it to the low level driver
service_id = vchi_readbuf_fourcc(&message[4]);
length = vchi_readbuf_uint32(&message[8]);
channel = (MESSAGE_TX_CHANNEL_T)(message_length >= 20 ? message[12] : MESSAGE_TX_CHANNEL_BULK);
channel_params = message_length >= 20 ? vchi_readbuf_uint32(&message[16]) : 0;
data_length = message_length >= 28 ? vchi_readbuf_uint32(&message[20]) : length;
data_offset = message_length >= 28 ? vchi_readbuf_uint32(&message[24]) : 0;
os_logging_message("CTRL SERVICE: Bulk transfer rx (%c%c%c%c), channel %d, %u Bytes (core=%u, offset=%u)",FOURCC_TO_CHAR(service_id), channel, length, data_length, data_offset);
service_info->connection->api->rx_bulk_buffer_added(service_info->connection->state,service_id,length,channel,channel_params,data_length,data_offset);
break;
}
case XON:
service_id = vchi_readbuf_fourcc(&message[4]);
os_logging_message("CTRL SERVICE: Xon (%c%c%c%c)",FOURCC_TO_CHAR(service_id));
service_info->connection->api->flow_control(service_info->connection->state, service_id, VC_FALSE);
break;
case XOFF:
service_id = vchi_readbuf_fourcc(&message[4]);
os_logging_message("CTRL SERVICE: Xoff (%c%c%c%c)",FOURCC_TO_CHAR(service_id));
service_info->connection->api->flow_control(service_info->connection->state, service_id, VC_TRUE);
break;
case DISCONNECT:
flags = message_length >= 8 ? vchi_readbuf_uint32(&message[4]) : 0;
service_info->connection->api->disconnect(service_info->connection->state, flags);
break;
case POWER_CONTROL:
{
MESSAGE_TX_CHANNEL_T channel = vchi_readbuf_uint32(&message[4]);
bool_t enable = vchi_readbuf_uint32(&message[8]);
uint32_t cookie = vchi_readbuf_uint32(&message[12]);
os_logging_message("CTRL SERVICE: Power (%d -> %d; %d)", channel, enable, cookie);
// connection should synchronously perform the power change, then we reply
// don't currently allow for any sort of error (protocol doesn't allow for it)
service_info->connection->api->power_control(service_info->connection->state, channel, enable);
// we are replying
reply_required = VC_TRUE;
message_length = 16;
// reply is the same as the message, except for the command code
vchi_writebuf_uint32( &output_message[0], POWER_CONTROL_REPLY );
memcpy(output_message+4, message+4, 12);
break;
}
default:
os_logging_message("CTRL SERVICE: Unknown message (0x%x)", command);
os_assert(0);
break;
}
// transmit the reply (if needed)
if(reply_required)
{
//attempt to send
return_value = service_info->connection->api->service_queue_msg(service_info->open_handle,output_message,message_length,output_flags,NULL);
if (return_value != 0) //failed because tx slots are full or whatever
{
//save the msg and send it later
return_value = vchi_control_reply_add_service(service_info,service_id,output_message);
os_assert(return_value == 0);
}
}
}
return_value = service_info->connection->api->held_msg_release(service_info->open_handle, message_handle);
os_assert(return_value == 0);
}
#ifdef VCHI_FEWER_MSG_AVAILABLE_CALLBACKS
} while (message_handle);
#endif
#if defined VCHI_COARSE_LOCKING
os_semaphore_release(&service_info->connection->sem);
#endif
break;
}
case VCHI_CALLBACK_MSG_SENT:
break;
case VCHI_CALLBACK_BULK_RECEIVED:
case VCHI_CALLBACK_BULK_DATA_READ:
case VCHI_CALLBACK_BULK_SENT: // control service doesn't use bulk transfers
case VCHI_CALLBACK_SENT_XOFF:
case VCHI_CALLBACK_SENT_XON: // control service must never be XOFFed
os_assert(0);
break;
}
}
/***********************************************************
* Name: software_fifo_protect
*
* Arguments: SOFTWARE_FIFO_HANDLE_T *handle - handle to this FIFO
*
* Description: Acquires the semaphore used to protect the SOFTWARE_FIFO_HANDLE_T structure
*
* Returns: 0 if successful, any other value is failure
*
***********************************************************/
int32_t software_fifo_protect( SOFTWARE_FIFO_HANDLE_T *handle )
{
return(os_semaphore_obtain(&handle->software_fifo_semaphore));
}
/******************************************************************************
NAME
vc_vchi_cec_init
SYNOPSIS
void vc_vchi_cec_init(VCHI_INSTANCE_T initialise_instance, VCHI_CONNECTION_T **connections, uint32_t num_connections )
FUNCTION
Initialise the CEC service for use. A negative return value
indicates failure (which may mean it has not been started on VideoCore).
RETURNS
int
******************************************************************************/
VCHPRE_ void VCHPOST_ vc_vchi_cec_init(VCHI_INSTANCE_T initialise_instance, VCHI_CONNECTION_T **connections, uint32_t num_connections ) {
int32_t success = -1;
uint32_t i;
// record the number of connections
memset( &cecservice_client, 0, sizeof(CECSERVICE_HOST_STATE_T) );
cecservice_client.num_connections = num_connections;
success = os_semaphore_create( &cecservice_client.sema, OS_SEMAPHORE_TYPE_SUSPEND );
vcos_assert( success == 0 );
success = os_semaphore_create( &cecservice_message_available_semaphore, OS_SEMAPHORE_TYPE_BUSY_WAIT );
vcos_assert( success == 0 );
success = os_semaphore_obtain( &cecservice_message_available_semaphore );
vcos_assert( success == 0 );
success = os_semaphore_create( &cecservice_notify_available_semaphore, OS_SEMAPHORE_TYPE_SUSPEND );
vcos_assert( success == 0 );
success = os_semaphore_obtain( &cecservice_notify_available_semaphore );
vcos_assert( success == 0 );
cecservice_client.topology = os_malloc(sizeof(VC_CEC_TOPOLOGY_T), 16, "CEC topology");
vcos_assert(cecservice_client.topology);
for (i=0; i < cecservice_client.num_connections; i++) {
// Create a 'Client' service on the each of the connections
SERVICE_CREATION_T cecservice_parameters = { CECSERVICE_CLIENT_NAME, // 4cc service code
connections[i], // passed in fn ptrs
0, // tx fifo size (unused)
0, // tx fifo size (unused)
&cecservice_client_callback,// service callback
&cecservice_message_available_semaphore, // callback parameter
VC_FALSE, // want_unaligned_bulk_rx
VC_FALSE, // want_unaligned_bulk_tx
VC_FALSE, // want_crc
};
SERVICE_CREATION_T cecservice_parameters2 = { CECSERVICE_NOTIFY_NAME, // 4cc service code
connections[i], // passed in fn ptrs
0, // tx fifo size (unused)
0, // tx fifo size (unused)
&cecservice_notify_callback,// service callback
&cecservice_notify_available_semaphore, // callback parameter
VC_FALSE, // want_unaligned_bulk_rx
VC_FALSE, // want_unaligned_bulk_tx
VC_FALSE, // want_crc
};
//Create the client to normal CEC service
success = vchi_service_open( initialise_instance, &cecservice_parameters, &cecservice_client.client_handle[i] );
vcos_assert( success == 0 );
//Create the client to the async CEC service (any CEC related notifications)
success = vchi_service_open( initialise_instance, &cecservice_parameters2, &cecservice_client.notify_handle[i] );
vcos_assert( success == 0 );
}
//Create the notifier task
success = os_thread_start(&cecservice_notify_task, cecservice_notify_func, &cecservice_client, 2048, "CEC Notify");
vcos_assert( success == 0 );
cecservice_client.initialised = 1;
}
//Lock the host state
static void lock_obtain (void) {
int32_t success;
vcos_assert(cecservice_client.initialised);
success = os_semaphore_obtain( &cecservice_client.sema );
vcos_assert(success >= 0);
}
//Lock the host state
static void lock_obtain (void) {
assert(hostreq_client.initialised);
assert(!os_semaphore_obtained(&hostreq_client.sema));
int success = os_semaphore_obtain(&hostreq_client.sema);
assert(success >= 0);
}
static void lock_obtain (void) {
int32_t success;
assert(!os_semaphore_obtained(&gencmd_client.sema));
success = os_semaphore_obtain( &gencmd_client.sema );
assert(success >= 0);
}
/* ----------------------------------------------------------------------
* send a string to the host side IL component service. if resp is NULL
* then there is no response to this call, so we should not wait for one.
*
* returns 0 on successful call made, -1 on failure to send call.
* on success, the response is written to 'resp' pointer
* -------------------------------------------------------------------- */
int vc_ilcs_execute_function( IL_FUNCTION_T func, void *data, int len, void *data2, int len2, void *bulk, int bulk_len, void *resp )
{
VC_ILCS_WAIT_T *wait;
int i, num;
// the host MUST receive a response
vc_assert( resp );
// need to atomically find free ->wait entry
os_semaphore_obtain( &vc_ilcsg.wait_sem );
// we try a number of times then give up with an error message
// rather than just deadlocking
for (i=0; i<VC_ILCS_WAIT_TIMEOUT; i++) {
num = 0;
while( num < VC_ILCS_MAX_WAITING && vc_ilcsg.wait[num].resp )
num++;
if ( num < VC_ILCS_MAX_WAITING || i == VC_ILCS_WAIT_TIMEOUT-1)
break;
// might be a fatal error if another thread is relying
// on this call completing before it can complete
// we'll pause until we can carry on and hope that's sufficient.
os_semaphore_release( &vc_ilcsg.wait_sem );
os_sleep( 10 ); // 10 msec
// if we're the vcilcs thread, then the waiters might need
// us to handle their response, so try and clear those now
if(os_thread_is_running(&vc_ilcsg.thread))
while(vc_ilcs_process_message(0));
os_logging_message( "%s: wait for sem", __FUNCTION__);
os_semaphore_obtain( &vc_ilcsg.wait_sem );
}
if(num == VC_ILCS_MAX_WAITING)
{
// failed to send message.
vc_assert(0);
os_semaphore_release( &vc_ilcsg.wait_sem );
return -1;
}
wait = &vc_ilcsg.wait[num];
wait->resp = resp;
wait->xid = vc_ilcsg.next_xid++;
os_semaphore_create( &wait->sem, OS_SEMAPHORE_TYPE_SUSPEND );
os_semaphore_obtain( &wait->sem );
// at this point, ->wait is exclusively ours ()
os_semaphore_release( &vc_ilcsg.wait_sem );
if(bulk)
os_semaphore_obtain( &vc_ilcsg.send_sem);
// write the command header.
vc_ilcs_transmit( func, wait->xid, data, len, data2, len2 );
if(bulk)
{
int result;
result = vchi_bulk_queue_transmit( vc_ilcsg.vchi_handle, // call to VCHI
bulk, bulk_len,
VCHI_FLAGS_BLOCK_UNTIL_QUEUED,
NULL );
vc_assert(result == 0);
os_semaphore_release( &vc_ilcsg.send_sem);
}
if ( !os_thread_is_running(&vc_ilcsg.thread) ) {
os_semaphore_obtain( &wait->sem );
} else {
// we're the vcilcs task, so wait for, and handle, incoming
// messages while we're not completed
for (;;) {
// wait->sem will not be released until we process the response message
vc_ilcs_process_message(1);
// did the last message release wait->sem ?
if ( !os_semaphore_obtained(&wait->sem) )
break;
}
}
// safe to do the following - the assignment of NULL is effectively atomic
os_semaphore_destroy( &wait->sem );
wait->resp = NULL;
return 0;
}
void
vc_ilcs_obtain_component_lock( void )
{
os_semaphore_obtain( &vc_ilcsg.component_lock );
}
/* ----------------------------------------------------------------------
* send a string to the host side IL component service. if resp is NULL
* then there is no response to this call, so we should not wait for one.
*
* returns response, written to 'resp' pointer
* -------------------------------------------------------------------- */
void
vc_ilcs_execute_function( IL_FUNCTION_T func, void *data, int len, void *data2, int len2, void *resp, int resplen )
{
VC_ILCS_WAIT_T *wait;
int num;
// the host MUST receive a response
assert( resp );
// need to atomically find free ->wait entry
os_semaphore_obtain( &vc_ilcsg.wait_sem );
for (;;) {
num = 0;
while( num < VC_ILCS_MAX_WAITING && vc_ilcsg.wait[num].resp )
num++;
if ( num < VC_ILCS_MAX_WAITING )
break;
assert( num < VC_ILCS_MAX_WAITING );
// might be a fatal error if another thread is relying
// on this call completing before it can complete
// we'll pause until we can carry on and hope that's sufficient.
os_semaphore_release( &vc_ilcsg.wait_sem );
os_sleep( 10 ); // 10 msec
os_semaphore_obtain( &vc_ilcsg.wait_sem );
}
wait = &vc_ilcsg.wait[num];
wait->resp = resp;
wait->xid = vc_ilcsg.next_xid++;
os_semaphore_create( &wait->sem, OS_SEMAPHORE_TYPE_SUSPEND );
os_semaphore_obtain( &wait->sem );
// at this point, ->wait is exclusively ours ()
os_semaphore_release( &vc_ilcsg.wait_sem );
// write the command header.
vc_ilcs_transmit( func, wait->xid, data, len, data2, len2 );
if ( !os_thread_current(vc_ilcsg.thread) ) {
os_semaphore_obtain( &wait->sem );
} else {
// we're the vcilcs task, so wait for, and handle, incoming
// messages while we're not completed
for (;;) {
// wait->sem will not be released until we process the response message
if ( vc_ilcs_process_message() == 0 ) {
// there were no more messages in the fifo; need to wait
os_semaphore_obtain( &vc_ilcsg.rxmsg_sem );
continue;
}
// did the last message release wait->sem ?
if ( !os_semaphore_obtained(&wait->sem) )
break;
}
}
// safe to do the following - the assignment of NULL is effectively atomic
os_semaphore_destroy( &wait->sem );
wait->resp = NULL;
}
