forked from Qortal/Brooklyn
533 lines
17 KiB
C
533 lines
17 KiB
C
/*
|
|
Copyright (c) 2012, Broadcom Europe Ltd
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
* Redistributions of source code must retain the above copyright
|
|
notice, this list of conditions and the following disclaimer.
|
|
* Redistributions in binary form must reproduce the above copyright
|
|
notice, this list of conditions and the following disclaimer in the
|
|
documentation and/or other materials provided with the distribution.
|
|
* Neither the name of the copyright holder nor the
|
|
names of its contributors may be used to endorse or promote products
|
|
derived from this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <stdarg.h>
|
|
#include <ctype.h>
|
|
#include <assert.h>
|
|
|
|
#include "vchost.h"
|
|
|
|
#include "interface/vcos/vcos.h"
|
|
#include "vcinclude/common.h"
|
|
#include "vc_vchi_gencmd.h"
|
|
#include "interface/vchi/vchi.h"
|
|
#include "interface/vchi/common/endian.h"
|
|
#include "interface/vmcs_host/vc_gencmd_defs.h"
|
|
|
|
#ifdef HAVE_GENCMD_VERSION
|
|
extern const char *gencmd_get_build_version(void);
|
|
#error
|
|
#endif
|
|
|
|
/******************************************************************************
|
|
Local types and defines.
|
|
******************************************************************************/
|
|
#define GENCMD_MAX_LENGTH 512
|
|
typedef struct {
|
|
VCHI_SERVICE_HANDLE_T open_handle[VCHI_MAX_NUM_CONNECTIONS];
|
|
uint32_t msg_flag[VCHI_MAX_NUM_CONNECTIONS];
|
|
char command_buffer[GENCMD_MAX_LENGTH+1];
|
|
char response_buffer[GENCMDSERVICE_MSGFIFO_SIZE];
|
|
uint32_t response_length; //Length of response minus the error code
|
|
int num_connections;
|
|
VCOS_MUTEX_T lock;
|
|
int initialised;
|
|
VCOS_EVENT_T message_available_event;
|
|
} GENCMD_SERVICE_T;
|
|
|
|
static GENCMD_SERVICE_T gencmd_client;
|
|
|
|
|
|
/******************************************************************************
|
|
Static function.
|
|
******************************************************************************/
|
|
static void gencmd_callback( void *callback_param,
|
|
VCHI_CALLBACK_REASON_T reason,
|
|
void *msg_handle );
|
|
|
|
static __inline int lock_obtain (void) {
|
|
int ret = -1;
|
|
if(gencmd_client.initialised && vcos_mutex_lock(&gencmd_client.lock) == VCOS_SUCCESS)
|
|
{
|
|
ret = 0;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
static __inline void lock_release (void) {
|
|
vcos_mutex_unlock(&gencmd_client.lock);
|
|
}
|
|
|
|
int use_gencmd_service(void) {
|
|
int ret = 0;
|
|
int i=0;
|
|
for(i = 0; i < gencmd_client.num_connections; i++) {
|
|
ret = (ret == 0) ? vchi_service_use(gencmd_client.open_handle[i]) : ret;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
int release_gencmd_service(void) {
|
|
int ret = 0;
|
|
int i=0;
|
|
for(i = 0; i < gencmd_client.num_connections; i++) {
|
|
ret = (ret == 0) ? vchi_service_release(gencmd_client.open_handle[i]) : ret;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
|
|
//call vc_vchi_gencmd_init to initialise
|
|
int vc_gencmd_init() {
|
|
assert(0);
|
|
return 0;
|
|
}
|
|
|
|
/******************************************************************************
|
|
NAME
|
|
vc_vchi_gencmd_init
|
|
|
|
SYNOPSIS
|
|
void vc_vchi_gencmd_init(VCHI_INSTANCE_T initialise_instance, VCHI_CONNECTION_T **connections, uint32_t num_connections )
|
|
|
|
FUNCTION
|
|
Initialise the general command service for use. A negative return value
|
|
indicates failure (which may mean it has not been started on VideoCore).
|
|
|
|
RETURNS
|
|
int
|
|
******************************************************************************/
|
|
|
|
void vc_vchi_gencmd_init (VCHI_INSTANCE_T initialise_instance, VCHI_CONNECTION_T **connections, uint32_t num_connections )
|
|
{
|
|
VCOS_STATUS_T status;
|
|
int32_t success;
|
|
int i;
|
|
|
|
if (gencmd_client.initialised)
|
|
return;
|
|
|
|
// record the number of connections
|
|
memset( &gencmd_client, 0, sizeof(GENCMD_SERVICE_T) );
|
|
gencmd_client.num_connections = (int) num_connections;
|
|
|
|
status = vcos_mutex_create(&gencmd_client.lock, "HGencmd");
|
|
vcos_assert(status == VCOS_SUCCESS);
|
|
status = vcos_event_create(&gencmd_client.message_available_event, "HGencmd");
|
|
vcos_assert(status == VCOS_SUCCESS);
|
|
|
|
for (i=0; i<gencmd_client.num_connections; i++) {
|
|
|
|
// Create a 'LONG' service on the each of the connections
|
|
SERVICE_CREATION_T gencmd_parameters = { VCHI_VERSION(VC_GENCMD_VER),
|
|
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_client.message_available_event, // 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 );
|
|
}
|
|
|
|
gencmd_client.initialised = 1;
|
|
release_gencmd_service();
|
|
}
|
|
|
|
/******************************************************************************
|
|
NAME
|
|
gencmd_callback
|
|
|
|
SYNOPSIS
|
|
void gencmd_callback( void *callback_param,
|
|
const VCHI_CALLBACK_REASON_T reason,
|
|
const void *msg_handle )
|
|
|
|
FUNCTION
|
|
VCHI callback
|
|
|
|
RETURNS
|
|
int
|
|
******************************************************************************/
|
|
static void gencmd_callback( void *callback_param,
|
|
const VCHI_CALLBACK_REASON_T reason,
|
|
void *msg_handle ) {
|
|
|
|
VCOS_EVENT_T *event = (VCOS_EVENT_T *)callback_param;
|
|
|
|
(void)msg_handle;
|
|
if ( reason != VCHI_CALLBACK_MSG_AVAILABLE || !event)
|
|
return;
|
|
|
|
vcos_event_signal(event);
|
|
}
|
|
|
|
/******************************************************************************
|
|
NAME
|
|
vc_gencmd_stop
|
|
|
|
SYNOPSIS
|
|
int vc_gencmd_stop()
|
|
|
|
FUNCTION
|
|
This tells us that the generak command service has stopped, thereby preventing
|
|
any of the functions from doing anything.
|
|
|
|
RETURNS
|
|
int
|
|
******************************************************************************/
|
|
|
|
void vc_gencmd_stop () {
|
|
// Assume a "power down" gencmd has been sent and the lock is held. There will
|
|
// be no response so this should be called instead.
|
|
int32_t success,i;
|
|
|
|
if (!gencmd_client.initialised)
|
|
return;
|
|
|
|
if(lock_obtain() == 0)
|
|
{
|
|
use_gencmd_service();
|
|
|
|
for(i = 0; i< (int32_t)gencmd_client.num_connections; i++) {
|
|
success = vchi_service_close( gencmd_client.open_handle[i]);
|
|
assert(success == 0);
|
|
}
|
|
|
|
gencmd_client.initialised = 0;
|
|
|
|
lock_release();
|
|
|
|
vcos_mutex_delete(&gencmd_client.lock);
|
|
vcos_event_delete(&gencmd_client.message_available_event);
|
|
}
|
|
}
|
|
|
|
/******************************************************************************
|
|
NAME
|
|
vc_gencmd_send
|
|
|
|
SYNOPSIS
|
|
int vc_gencmd_send( const char *format, ... )
|
|
|
|
FUNCTION
|
|
Send a string to general command service.
|
|
|
|
RETURNS
|
|
int
|
|
******************************************************************************/
|
|
int vc_gencmd_send_list ( const char *format, va_list a )
|
|
{
|
|
int success = -1;
|
|
|
|
// Obtain the lock and keep it so no one else can butt in while we await the response.
|
|
if(lock_obtain() == 0)
|
|
{
|
|
int length = vsnprintf( gencmd_client.command_buffer, GENCMD_MAX_LENGTH, format, a );
|
|
|
|
if (length >= 0 && length < GENCMD_MAX_LENGTH)
|
|
{
|
|
int i;
|
|
use_gencmd_service();
|
|
for( i=0; i<gencmd_client.num_connections; i++ ) {
|
|
success = vchi_msg_queue( gencmd_client.open_handle[i],
|
|
gencmd_client.command_buffer,
|
|
length+1,
|
|
VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL );
|
|
|
|
if(success == 0)
|
|
{ // only want to send on one connection, so break on success
|
|
break;
|
|
}
|
|
}
|
|
release_gencmd_service();
|
|
}
|
|
|
|
lock_release();
|
|
}
|
|
|
|
return success;
|
|
}
|
|
|
|
int vc_gencmd_send ( const char *format, ... )
|
|
{
|
|
va_list a;
|
|
int rv;
|
|
|
|
va_start ( a, format );
|
|
rv = vc_gencmd_send_list( format, a );
|
|
va_end ( a );
|
|
return rv;
|
|
}
|
|
|
|
/******************************************************************************
|
|
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 = -1;
|
|
int ret_code = 0;
|
|
int32_t sem_ok = 0;
|
|
|
|
if(lock_obtain() == 0)
|
|
{
|
|
//Note this will ALWAYS reset response buffer and overwrite any partially read responses
|
|
use_gencmd_service();
|
|
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 event 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 && vcos_event_wait(&gencmd_client.message_available_event) == VCOS_SUCCESS);
|
|
|
|
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) vcos_min((int)gencmd_client.response_length, (int)maxlen));
|
|
}
|
|
|
|
release_gencmd_service();
|
|
lock_release();
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
/******************************************************************************
|
|
NAME
|
|
vc_gencmd
|
|
|
|
SYNOPSIS
|
|
int vc_gencmd(char *response, int maxlen, const char *format, ...)
|
|
|
|
FUNCTION
|
|
Send a gencmd and receive the response as per vc_gencmd read_response.
|
|
|
|
RETURNS
|
|
int
|
|
******************************************************************************/
|
|
int vc_gencmd(char *response, int maxlen, const char *format, ...) {
|
|
va_list args;
|
|
int ret = -1;
|
|
|
|
use_gencmd_service();
|
|
|
|
va_start(args, format);
|
|
ret = vc_gencmd_send_list(format, args);
|
|
va_end (args);
|
|
|
|
if (ret >= 0) {
|
|
ret = vc_gencmd_read_response(response, maxlen);
|
|
}
|
|
|
|
release_gencmd_service();
|
|
|
|
return ret;
|
|
}
|
|
|
|
/******************************************************************************
|
|
NAME
|
|
vc_gencmd_string_property
|
|
|
|
SYNOPSIS
|
|
int vc_gencmd_string_property(char *text, char *property, char **value, int *length)
|
|
|
|
FUNCTION
|
|
Given a text string, containing items of the form property=value,
|
|
look for the named property and return the value. The start of the value
|
|
is returned, along with its length. The value may contain spaces, in which
|
|
case it is enclosed in double quotes. The double quotes are not included in
|
|
the return parameters. Return non-zero if the property is found.
|
|
|
|
RETURNS
|
|
int
|
|
******************************************************************************/
|
|
|
|
int vc_gencmd_string_property(char *text, const char *property, char **value, int *length) {
|
|
#define READING_PROPERTY 0
|
|
#define READING_VALUE 1
|
|
#define READING_VALUE_QUOTED 2
|
|
int state = READING_PROPERTY;
|
|
int delimiter = 1, match = 0, len = (int)strlen(property);
|
|
char *prop_start=text, *value_start=text;
|
|
for (; *text; text++) {
|
|
int ch = *text;
|
|
switch (state) {
|
|
case READING_PROPERTY:
|
|
if (delimiter) prop_start = text;
|
|
if (isspace(ch)) delimiter = 1;
|
|
else if (ch == '=') {
|
|
delimiter = 1;
|
|
match = (text-prop_start==len && strncmp(prop_start, property, (size_t)(text-prop_start))==0);
|
|
state = READING_VALUE;
|
|
}
|
|
else delimiter = 0;
|
|
break;
|
|
case READING_VALUE:
|
|
if (delimiter) value_start = text;
|
|
if (isspace(ch)) {
|
|
if (match) goto success;
|
|
delimiter = 1;
|
|
state = READING_PROPERTY;
|
|
}
|
|
else if (delimiter && ch == '"') {
|
|
delimiter = 1;
|
|
state = READING_VALUE_QUOTED;
|
|
}
|
|
else delimiter = 0;
|
|
break;
|
|
case READING_VALUE_QUOTED:
|
|
if (delimiter) value_start = text;
|
|
if (ch == '"') {
|
|
if (match) goto success;
|
|
delimiter = 1;
|
|
state = READING_PROPERTY;
|
|
}
|
|
else delimiter = 0;
|
|
break;
|
|
}
|
|
}
|
|
if (match) goto success;
|
|
return 0;
|
|
success:
|
|
*value = value_start;
|
|
*length = text - value_start;
|
|
return 1;
|
|
}
|
|
|
|
/******************************************************************************
|
|
NAME
|
|
vc_gencmd_number_property
|
|
|
|
SYNOPSIS
|
|
int vc_gencmd_number_property(char *text, char *property, int *number)
|
|
|
|
FUNCTION
|
|
Given a text string, containing items of the form property=value,
|
|
look for the named property and return the numeric value. If such a numeric
|
|
value is successfully found, return 1; otherwise return 0.
|
|
|
|
RETURNS
|
|
int
|
|
******************************************************************************/
|
|
|
|
int vc_gencmd_number_property(char *text, const char *property, int *number) {
|
|
char *value, temp;
|
|
int length, retval;
|
|
if (vc_gencmd_string_property(text, property, &value, &length) == 0)
|
|
return 0;
|
|
temp = value[length];
|
|
value[length] = 0;
|
|
/* coverity[secure_coding] - this is not insecure */
|
|
retval = sscanf(value, "0x%x", (unsigned int*)number);
|
|
if (retval != 1)
|
|
/* coverity[secure_coding] - this is not insecure */
|
|
retval = sscanf(value, "%d", number);
|
|
value[length] = temp;
|
|
return retval;
|
|
|
|
}
|
|
|
|
/******************************************************************************
|
|
NAME
|
|
vc_gencmd_until
|
|
|
|
SYNOPSIS
|
|
int vc_gencmd_until(const char *cmd, const char *error_string, int timeout);
|
|
|
|
FUNCTION
|
|
Sends the command repeatedly, until one of the following situations occurs:
|
|
The specified response string is found within the gencmd response.
|
|
The specified error string is found within the gencmd response.
|
|
The timeout is reached.
|
|
|
|
The timeout is a rough value, do not use it for precise timing.
|
|
|
|
RETURNS
|
|
0 if the requested response was detected.
|
|
1 if the error string is detected or the timeout is reached.
|
|
******************************************************************************/
|
|
int vc_gencmd_until( char *cmd,
|
|
const char *property,
|
|
char *value,
|
|
const char *error_string,
|
|
int timeout) {
|
|
char response[128];
|
|
int length;
|
|
char *ret_value;
|
|
int ret = 1;
|
|
|
|
use_gencmd_service();
|
|
for (;timeout > 0; timeout -= 10) {
|
|
vc_gencmd(response, (int)sizeof(response), cmd);
|
|
if (strstr(response,error_string)) {
|
|
ret = 1;
|
|
break;
|
|
}
|
|
else if (vc_gencmd_string_property(response, property, &ret_value, &length) &&
|
|
strncmp(value,ret_value,(size_t)length)==0) {
|
|
ret = 0;
|
|
break;
|
|
}
|
|
vcos_sleep(10);
|
|
}
|
|
release_gencmd_service();
|
|
|
|
return ret;
|
|
}
|
|
|