/* ----------------------------------------------------------------------
* 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
vc_assert(0);
return;
}
// extract command from fifo and place in response buffer.
memcpy( wait->resp, msg, len );
os_logging_message( "%s: waking waiter %d", __FUNCTION__, i );
os_semaphore_release( &wait->sem );
}
/* ----------------------------------------------------------------------
* dump debug info on the given non-wrap fifo
*
* expects the fifo to be already locked
* -------------------------------------------------------------------- */
void
non_wrap_fifo_debug( VCHI_NWFIFO_T *_fifo )
{
NON_WRAP_FIFO_HANDLE_T *fifo = (NON_WRAP_FIFO_HANDLE_T *)_fifo;
uint32_t i;
os_logging_message( "----------------------------------------" );
//os_semaphore_obtain( &fifo->sem );
os_logging_message( "nwfifo_debug: %s", fifo->name );
os_logging_message( "read_slot=%d, write_slot=%d", fifo->read_slot, fifo->write_slot );
for (i=0; i<fifo->num_of_slots; i++) {
NON_WRAP_SLOT_T *slot = &fifo->slot_info[ i ];
uint8_t *addr = slot->address;
os_logging_message( "slot %d (%p,%d=%d)", i, addr, slot->length, slot->state );
if ( (size_t)addr < 0x1000 ) continue;
if ( slot->length == 0 ) continue;
if ( slot->state == 0 ) continue;
os_logging_message( "--> %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
addr[0], addr[1], addr[2], addr[3], addr[4], addr[5], addr[6], addr[7],
addr[8], addr[9], addr[10], addr[11], addr[12], addr[13], addr[14], addr[15] );
}
//os_semaphore_release( &fifo->sem );
os_logging_message( "----------------------------------------" );
}
/***********************************************************
* 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;
}
}
void succeeded(char *mess)
{
os_logging_message("%s : ok\n", mess);
}
void failed(char *mess)
{
os_logging_message("%s : FAILED\n", mess);
}
/* ----------------------------------------------------------------------
* 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;
}
