/* ----------------------------------------------------------------------
* called from the vchi layer whenever an event happens.
* here, we are only interested in the 'message available' callback
* -------------------------------------------------------------------- */
static void
vc_ilcs_callback( void *callback_param,
const VCHI_CALLBACK_REASON_T reason,
void *msg_handle )
{
int32_t success;
OS_SEMAPHORE_T *sem;
switch( reason ) {
case VCHI_CALLBACK_MSG_AVAILABLE:
sem = (OS_SEMAPHORE_T *)callback_param;
if ( sem == NULL )
break;;
if ( os_semaphore_obtained(sem) ) {
success = os_semaphore_release( sem );
assert( success >= 0 );
}
break;
#if 0
case VCHI_CALLBACK_BULK_RECEIVED:
case VCHI_CALLBACK_BULK_SENT:
sem = (OS_SEMAPHORE_T *)msg_handle;
success = os_semaphore_release( sem );
assert( success >= 0 );
break;
#endif
}
}
/* ----------------------------------------------------------------------
* 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: 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;
}
}
/* ----------------------------------------------------------------------
* 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;
}
/* ----------------------------------------------------------------------
* 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;
}
/***********************************************************
* 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: 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;
}
//Unlock the host state
static void lock_release (void) {
int32_t success;
vcos_assert(cecservice_client.initialised);
vcos_assert(os_semaphore_obtained(&cecservice_client.sema));
success = os_semaphore_release( &cecservice_client.sema );
vcos_assert( success >= 0 );
}
/***********************************************************
* 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;
}
/***********************************************************
* Name: vchi_control_service_close
*
* 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_close( void )
{
int32_t success = 0;
#if 1
os_assert(0);
#else
uint32_t count = 0;
for( count = 0; count < VCHI_MAX_NUM_CONNECTIONS; count++ )
{
if( control_info[count].initialised == VC_TRUE)
{
// we have previously initialised this connection
// so set it all as uninitialised
// HACK HACK TBD FIXME - do we need to signal to the other end of the connections that we are closing this end??????
// similarly we would need to respond to such requests ourselves......
control_info[count].initialised = VC_FALSE;
control_info[count].connection = NULL;
success += os_semaphore_release( &control_info[count].connected_semaphore );
}
}
#endif
// close the timer
os_time_term();
// reset the timer offset
time_offset = 0;
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: hostreq_client_callback
*
* Arguments: semaphore, callback reason and message handle
*
* Description: VCHI callback for the HOSTREQ service
*
***********************************************************/
static void hostreq_client_callback( void *callback_param,
const VCHI_CALLBACK_REASON_T reason,
void *msg_handle ) {
OS_SEMAPHORE_T *sem = (OS_SEMAPHORE_T *)callback_param;
if ( reason != VCHI_CALLBACK_MSG_AVAILABLE )
return;
if ( sem == NULL )
return;
if ( os_semaphore_obtained(sem) ) {
int32_t success = os_semaphore_release( sem );
assert( success >= 0 );
}
}
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;
}
/* ----------------------------------------------------------------------
* used in conjunction with non_wrap_fifo_request_read_address, to
* signify completion
*
* NOTE: this routine will unlock the fifo!
*
* returns 0 on success, non-0 otherwise
* -------------------------------------------------------------------- */
int32_t
non_wrap_fifo_read_complete( VCHI_NWFIFO_T *_fifo, void *address )
{
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 );
// it should be the case that the reads will complete sequentially
// but we will verify it
if ( fifo->slot_info[fifo->read_slot].address == address ) {
// mark that this message has been read
fifo->slot_info[fifo->read_slot].state = NON_WRAP_NOT_IN_USE;
// point to the next message
fifo->read_slot = next_read_slot( fifo, fifo->read_slot );
// success!
success = os_cond_broadcast( &fifo->space_available_cond, VC_TRUE );
}
os_assert( success == 0 );
os_semaphore_release( &fifo->sem );
return success;
}
/* ----------------------------------------------------------------------
* used in conjunction with non_wrap_fifo_request_write_address, to
* signify completion
*
* NOTE: this routine will unlock the fifo!
*
* returns 0 on success, non-0 otherwise
* -------------------------------------------------------------------- */
int32_t
non_wrap_fifo_write_complete( VCHI_NWFIFO_T *_fifo, void *address )
{
NON_WRAP_FIFO_HANDLE_T *fifo = (NON_WRAP_FIFO_HANDLE_T *)_fifo;
int32_t success = -1;
os_assert( fifo->base_address );
//os_semaphore_obtain( &fifo->sem );
// it should be the case that the writes will complete sequentially
// but we will verify it
if ( fifo->slot_info[fifo->write_slot].address == address ) {
// mark that this message has been written
fifo->slot_info[fifo->write_slot].state = NON_WRAP_READY;
// point to the next message
fifo->write_slot = next_slot( fifo, fifo->write_slot );
// success!
success = 0;
}
os_assert( success == 0 );
os_semaphore_release( &fifo->sem );
return success;
}
/* ----------------------------------------------------------------------
* 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: software_fifo_unprotect
*
* Arguments: SOFTWARE_FIFO_HANDLE_T *handle - handle to this FIFO
*
* Description: Releases the semaphore used to protect the SOFTWARE_FIFO_HANDLE_T structure
*
* Returns: 0 if successful, any other value is failure
*
***********************************************************/
int32_t software_fifo_unprotect( SOFTWARE_FIFO_HANDLE_T *handle )
{
return(os_semaphore_release(&handle->software_fifo_semaphore));
}
/***********************************************************
* Name: os_semaphore_release_global
*
* Arguments: void
*
* Description: release a global semaphore.
*
* Returns: int32_t - success == 0
*
***********************************************************/
int32_t os_semaphore_release_global( void )
{
return os_semaphore_release(&os_mutex_global);
}
//Unlock the host state
static void lock_release (void) {
assert(hostreq_client.initialised);
assert(os_semaphore_obtained(&hostreq_client.sema));
int32_t success = os_semaphore_release(&hostreq_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;
}
static void lock_release (void) {
int32_t success;
assert(os_semaphore_obtained(&gencmd_client.sema));
success = os_semaphore_release( &gencmd_client.sema );
assert( success >= 0 );
}
void
vc_ilcs_release_component_lock( void )
{
os_semaphore_release( &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;
}
/***********************************************************
* 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: 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;
}
/* ----------------------------------------------------------------------
* returns address of free space within the given non-wrap fifo
* that can take the requested amount of data.
*
* intended to be used with non_wrap_fifo_write_complete, below
*
* NOTE: on success, this routine will leave the fifo locked!
*
* returns 0 on success, non-0 otherwise
* -------------------------------------------------------------------- */
int32_t
non_wrap_fifo_request_write_address( VCHI_NWFIFO_T *_fifo, void **address, uint32_t size, int block )
{
NON_WRAP_FIFO_HANDLE_T *fifo = (NON_WRAP_FIFO_HANDLE_T *)_fifo;
int32_t success = -1;
uint32_t slot;
uint32_t space;
uint8_t *address1;
uint8_t *address2 = 0;
int wrapped;
os_semaphore_obtain( &fifo->sem );
os_assert( fifo->base_address );
do {
// before we start need to confirm that the write slot does not currently hold some data
if ( fifo->slot_info[fifo->write_slot].state != NON_WRAP_NOT_IN_USE ) {
os_semaphore_release( &fifo->sem );
return -1;
}
slot = fifo->read_slot;
// To simplify the logic we will only write messages contiguously, so we will sometimes lose space at the end
// when the message is too big for the space available
address1 = fifo->slot_info[slot].address;
if ( address1 == NULL )
address1 = fifo->base_address;
// if this slot does not contain anything then the buffer is empty and we can start again from the beginning
if ( fifo->slot_info[slot].state == NON_WRAP_NOT_IN_USE ) {
*address = fifo->base_address;
// check that there is enough room
if ( size <= fifo->size )
success = 0;
}
else
{
// need to find out how much room there is
wrapped = VC_FALSE;
while( slot != fifo->write_slot ) {
address2 = fifo->slot_info[slot].address + fifo->slot_info[slot].length;
// move slot on, taking care to wrap
slot = next_read_slot( fifo, slot );
if ( fifo->slot_info[slot].state != NON_WRAP_NOT_IN_USE ) {
if ( fifo->slot_info[slot].address < address2 )
wrapped = VC_TRUE;
}
}
// now we can work out which address and how much space is available
if ( wrapped ) {
space = (uint32_t)(address1 - address2);
if ( space >= size ) {
// set the required address
*address = address2;
// record success
success = 0;
}
}
else
{
// is there enough space at the end of the buffer
if ( (uint32_t)(fifo->base_address + fifo->size - address2) >= size ) {
*address = address2;
success = 0;
} else {
// now see if there is enough room if we do a wrap
if ( (uint32_t)(address1 - fifo->base_address) >= size ) {
*address = fifo->base_address;
success = 0;
}
}
}
}
if (success != 0 && block)
{
os_cond_wait( &fifo->space_available_cond, &fifo->sem );
}
} while (success != 0 && block);
// record the write info in our list of slots
if ( success == 0 ) {
fifo->slot_info[fifo->write_slot].state = NON_WRAP_WRITING;
fifo->slot_info[fifo->write_slot].address = (uint8_t *)*address;
fifo->slot_info[fifo->write_slot].length = size;
}
// on success, the fifo is left locked
if ( success != 0 )
os_semaphore_release( &fifo->sem );
return success;
}
