Mani Monajjemi
772 linhas
21 KiB
C
772 linhas
21 KiB
C
/**
|
|
* @file ATcodec_api.c
|
|
* @author aurelien.morelle@parrot.fr
|
|
* @date 2006/12/06
|
|
|
|
* modified on 2010/07/19 by stephane.piskorski.ext@parrot.fr (bug fix+comments)
|
|
*/
|
|
|
|
#include <VP_Os/vp_os_types.h>
|
|
#include <VP_Os/vp_os_delay.h>
|
|
#include <VP_Os/vp_os_assert.h>
|
|
#include <VP_Os/vp_os_malloc.h>
|
|
#include <VP_Os/vp_os_signal.h>
|
|
#include <VP_Os/vp_os_print.h>
|
|
|
|
#if !defined(TARGET_OS_IPHONE) && !defined(TARGET_IPHONE_SIMULATOR)
|
|
# include <VP_Api/vp_api_thread_helper.h>
|
|
#endif // ! TARGET_OS_IPHONE && ! TARGET_IPHONE_SIMULATOR
|
|
|
|
#include <ATcodec/ATcodec_api.h>
|
|
#include <ATcodec/ATcodec_Buffer.h>
|
|
#include <ATcodec/ATcodec.h>
|
|
#include <ATcodec/ATcodec_Tree.h>
|
|
|
|
|
|
static AT_CODEC_READING_MODE at_codec_reading_mode = ATCODEC_READ_FROM_STREAM;
|
|
|
|
static AT_CODEC_FUNCTIONS_PTRS func_ptrs;
|
|
static ATcodec_Tree_t default_tree;
|
|
static int atcodec_lib_init_ok = 0;
|
|
|
|
// only for client
|
|
static vp_os_mutex_t ATcodec_cond_mutex;
|
|
static vp_os_cond_t ATcodec_wait_cond;
|
|
static uint8_t ATcodec_Message_Buffer[INTERNAL_BUFFER_SIZE];
|
|
static int32_t ATcodec_Message_len = 0;
|
|
|
|
static int32_t v_continue = 0;
|
|
|
|
#if !defined(TARGET_OS_IPHONE) && !defined(TARGET_IPHONE_SIMULATOR)
|
|
static ATCODEC_RET
|
|
test_dyn_strs_one_node(ATcodec_Tree_t *tree, ATcodec_Tree_Node_t *node, int depth, const char *dynamic_str, ATcodec_Memory_t *memory, AT_CODEC_Message_Received *ptr, int *len_dec)
|
|
{
|
|
char *static_str, *dyn_str, *mem_index;
|
|
int len;
|
|
ATcodec_Memory_t str, fmt;
|
|
|
|
static_str = (char *)ATcodec_Buffer_getElement(&tree->strs, ((ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data))->static_str);
|
|
if((len = strlen(static_str)) > depth)
|
|
{
|
|
if(strncmp(static_str+depth, dynamic_str, (len-depth)*sizeof(char)))
|
|
return ATCODEC_FALSE;
|
|
}
|
|
|
|
dyn_str = (char *)ATcodec_Buffer_getElement(&tree->strs, ((ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data))->dynamic_str);
|
|
ATcodec_Memory_Init(&str, dynamic_str+(len-depth), 0, 1, NULL, NULL);
|
|
ATcodec_Memory_Init(&fmt, dyn_str, 0, 1, NULL, NULL);
|
|
|
|
mem_index = memory->current;
|
|
if(vp_atcodec_sscanf(&str, &fmt, memory, len_dec) == ATCODEC_TRUE)
|
|
{
|
|
memory->current = mem_index;
|
|
*ptr = ((ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data))->func_ptr;
|
|
*len_dec += len;
|
|
return ATCODEC_TRUE;
|
|
}
|
|
|
|
return ATCODEC_FALSE;
|
|
}
|
|
|
|
|
|
// We assume we know that current->isleaf != -1 here
|
|
static ATCODEC_RET
|
|
test_dyn_strs(ATcodec_Tree_t *tree, ATcodec_Tree_Node_t *current, int depth, const char *dynamic_str, ATcodec_Memory_t *memory, AT_CODEC_Message_Received *ptr, int *len_dec)
|
|
{
|
|
ATcodec_Tree_Node_t *son;
|
|
int i;
|
|
|
|
if(current->type == ATCODEC_TREE_NODE_TYPE_LEAF)
|
|
{
|
|
// just current to be tested
|
|
return test_dyn_strs_one_node(tree, current, depth, dynamic_str, memory, ptr, len_dec);
|
|
}
|
|
else
|
|
{
|
|
VP_OS_ASSERT(current->type == ATCODEC_TREE_NODE_TYPE_MULTILEAVES);
|
|
// all sons have to be tested
|
|
for(i = 0 ; i < current->nb_sons ; i++)
|
|
{
|
|
son = (ATcodec_Tree_Node_t *)ATcodec_Buffer_getElement(&tree->sons, current->sons[i]);
|
|
if(test_dyn_strs_one_node(tree, son, depth, dynamic_str, memory, ptr, len_dec) == ATCODEC_TRUE)
|
|
return ATCODEC_TRUE;
|
|
}
|
|
}
|
|
|
|
return ATCODEC_FALSE;
|
|
}
|
|
|
|
|
|
static ATCODEC_RET
|
|
test_process_node(ATcodec_Tree_t *tree, ATcodec_Tree_Node_t *node, const char *cpy, ATcodec_Memory_t *memory, int *len_dec, int depth)
|
|
{
|
|
ATcodec_Tree_Node_t *son;
|
|
AT_CODEC_Message_Received ptr;
|
|
AT_CODEC_MSG_ID id = (AT_CODEC_MSG_ID)-1;
|
|
char *fmt_str;
|
|
int32_t output_len;
|
|
char output_params[INTERNAL_BUFFER_SIZE];
|
|
uint8_t output_str[INTERNAL_BUFFER_SIZE];
|
|
ATcodec_Memory_t output, dest, fmt;
|
|
|
|
if(test_dyn_strs(tree, node, depth, cpy, memory, &ptr, len_dec) == ATCODEC_TRUE)
|
|
{
|
|
ATcodec_Memory_Init(&output, &output_params[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
|
|
VP_OS_ASSERT(ptr);
|
|
ptr(memory, &output, &id);
|
|
if((int)id != -1)
|
|
{
|
|
son = ATcodec_Tree_Node_get(tree, id);
|
|
fmt_str = (char *)ATcodec_Buffer_getElement(&tree->strs, ((ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, son->data))->total_str);
|
|
|
|
ATcodec_Memory_Init(&dest, (char*)&output_str[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
|
|
ATcodec_Memory_Init(&fmt, fmt_str, 0, 1, NULL, NULL);
|
|
|
|
output.current = (char *)output.start;
|
|
if(vp_atcodec_sprintf_params(&dest, &output_len, &fmt, &output) != ATCODEC_TRUE)
|
|
return ATCODEC_FALSE;
|
|
|
|
if(func_ptrs.write(&output_str[0], &output_len) != AT_CODEC_WRITE_OK)
|
|
return ATCODEC_FALSE;
|
|
}
|
|
return ATCODEC_TRUE;
|
|
}
|
|
|
|
return ATCODEC_FALSE;
|
|
}
|
|
|
|
|
|
static ATCODEC_RET
|
|
find_process_node(ATcodec_Tree_t *tree, const char *message, ATcodec_Memory_t *memory, int *len_dec)
|
|
{
|
|
ATcodec_Tree_Node_t *current = ATcodec_Tree_Node_get(tree, tree->root);
|
|
char *cpy = (char *)message;
|
|
int depth = 0;
|
|
|
|
while(current->type == ATCODEC_TREE_NODE_TYPE_NODE)
|
|
{
|
|
if(current->sons[0] != -1)
|
|
{
|
|
if(test_process_node(tree, ATcodec_Tree_Node_get(tree, current->sons[0]), cpy, memory, len_dec, depth) == ATCODEC_TRUE)
|
|
return ATCODEC_TRUE;
|
|
}
|
|
if(!*cpy || current->sons[(int)(*cpy)] == -1)
|
|
{
|
|
return ATCODEC_FALSE;
|
|
}
|
|
else
|
|
{
|
|
current = ATcodec_Tree_Node_get(tree, current->sons[(int)(*cpy++)]);
|
|
depth++;
|
|
}
|
|
}
|
|
|
|
return test_process_node(tree, current, cpy, memory, len_dec, depth);
|
|
}
|
|
#endif
|
|
|
|
/********************************************************************
|
|
* @param[in] str String to a raw AT command (with % standing for parameters).
|
|
* @brief Computes the length of the non-changing part (before the parameters) of an AT command string.
|
|
*/
|
|
static int
|
|
static_len(const char *str)
|
|
{
|
|
int len = 0;
|
|
int percent = 0;
|
|
int escaped = 1;
|
|
|
|
do
|
|
{
|
|
switch(*str)
|
|
{
|
|
case '%':
|
|
if(escaped)
|
|
percent = 1-percent;
|
|
escaped = 1;
|
|
break;
|
|
case 's':
|
|
case 'd':
|
|
case 'c':
|
|
case 'l':
|
|
if(percent)
|
|
{
|
|
return (len-1);
|
|
}
|
|
escaped = 1;
|
|
percent = 0;
|
|
break;
|
|
case '[':
|
|
if(escaped)
|
|
{
|
|
return len;
|
|
}
|
|
break;
|
|
case '\\':
|
|
escaped = 1-escaped;
|
|
percent = 0;
|
|
break;
|
|
case '\0':
|
|
return len;
|
|
break;
|
|
default:
|
|
escaped = 1;
|
|
percent = 0;
|
|
break;
|
|
}
|
|
|
|
len++;
|
|
}
|
|
while(*str++);
|
|
|
|
return len;
|
|
}
|
|
|
|
|
|
AT_CODEC_ERROR_CODE at_default_cb(ATcodec_Memory_t *input, ATcodec_Memory_t *output, int *id)
|
|
{
|
|
ATCODEC_PRINT("< AT message received >\n");
|
|
|
|
return AT_CODEC_GENERAL_OK;
|
|
}
|
|
|
|
|
|
AT_CODEC_MSG_ID
|
|
ATcodec_Add_Hashed_Message (const char *str, AT_CODEC_MSG_ID from_cmd, AT_CODEC_Message_Received func_ptr, int priority)
|
|
{
|
|
return ATcodec_Add_Hashed_Message_Tree (&default_tree, str, from_cmd, func_ptr, priority);
|
|
}
|
|
|
|
|
|
AT_CODEC_MSG_ID
|
|
ATcodec_Add_Hashed_Message_Tree (ATcodec_Tree_t *tree, const char *str, AT_CODEC_MSG_ID from_cmd, AT_CODEC_Message_Received func_ptr, int priority)
|
|
{
|
|
ATcodec_Message_Data_t data, *p_data;
|
|
ATcodec_Tree_Node_t *node;
|
|
int len_s, len_d, node_i;
|
|
char buffer[1024];
|
|
|
|
data.static_str = -1;
|
|
data.dynamic_str = -1;
|
|
data.total_str = -1;
|
|
|
|
data.from_cmd = from_cmd;
|
|
data.func_ptr = func_ptr;
|
|
data.priority = priority;
|
|
|
|
/* Retrieves the length of the never-changing part of the AT command */
|
|
len_s = static_len(str);
|
|
VP_OS_ASSERT(len_s < 1024);
|
|
vp_os_memcpy(&buffer[0], str, len_s);
|
|
buffer[len_s] = 0;
|
|
node_i = ATcodec_Tree_insert(tree, &buffer[0], &data);
|
|
node = ATcodec_Tree_Node_get(tree, node_i);
|
|
|
|
p_data = ATcodec_Buffer_getElement(&tree->leaves, node->data);
|
|
p_data->static_str = node->strkey;
|
|
|
|
// backup dynamic_str
|
|
p_data->dynamic_str = tree->strs.nbElements;
|
|
len_d = strlen(str+len_s)+1;
|
|
ATcodec_Buffer_pushElements (&tree->strs, str+len_s, len_d);
|
|
|
|
// backup concatenation of static_str and dynamic_str
|
|
p_data->total_str = tree->strs.nbElements;
|
|
ATcodec_Buffer_pushElements (&tree->strs, str, len_s+len_d);
|
|
|
|
return (AT_CODEC_MSG_ID)node_i;
|
|
}
|
|
|
|
|
|
AT_CODEC_MSG_ID
|
|
ATcodec_Add_Defined_Message (const char *str)
|
|
{
|
|
return ATcodec_Add_Defined_Message_Tree (&default_tree, str);
|
|
}
|
|
|
|
|
|
AT_CODEC_MSG_ID
|
|
ATcodec_Add_Defined_Message_Tree (ATcodec_Tree_t *tree, const char *str)
|
|
{
|
|
return ATcodec_Add_Hashed_Message_Tree (tree, str, 0, NULL, 0);
|
|
}
|
|
|
|
|
|
void
|
|
ATcodec_Init_Library (AT_CODEC_FUNCTIONS_PTRS *funcs)
|
|
{
|
|
ATcodec_Init_Library_Tree (&default_tree, funcs);
|
|
}
|
|
|
|
void
|
|
ATcodec_Set_Reading_Mode(AT_CODEC_READING_MODE _mode)
|
|
{
|
|
if (_mode==ATCODEC_READ_FROM_STREAM || _mode==ATCODEC_READ_FROM_PACKETS)
|
|
{
|
|
at_codec_reading_mode = _mode;
|
|
}
|
|
}
|
|
|
|
void
|
|
ATcodec_Init_Library_Tree (ATcodec_Tree_t *tree, AT_CODEC_FUNCTIONS_PTRS *funcs)
|
|
{
|
|
VP_OS_ASSERT(funcs);
|
|
VP_OS_ASSERT(funcs->open);
|
|
VP_OS_ASSERT(funcs->read);
|
|
VP_OS_ASSERT(funcs->enable);
|
|
VP_OS_ASSERT(funcs->write);
|
|
VP_OS_ASSERT(funcs->close);
|
|
VP_OS_ASSERT(funcs->init);
|
|
VP_OS_ASSERT(funcs->shutdown);
|
|
|
|
ATcodec_Tree_init(tree, sizeof(ATcodec_Message_Data_t), 1);
|
|
|
|
memcpy(&func_ptrs, funcs, sizeof(*funcs));
|
|
|
|
vp_os_mutex_init(&ATcodec_cond_mutex);
|
|
vp_os_cond_init(&ATcodec_wait_cond, &ATcodec_cond_mutex);
|
|
|
|
if(func_ptrs.init() != AT_CODEC_INIT_OK)
|
|
{
|
|
ATCODEC_PRINT("ATcodec Init error\n");
|
|
}
|
|
else
|
|
{
|
|
ATcodec_Tree_print(tree);
|
|
atcodec_lib_init_ok = 1;
|
|
ATcodec_Message_len = 0;
|
|
}
|
|
}
|
|
|
|
void
|
|
ATcodec_Shutdown_Library (void)
|
|
{
|
|
ATcodec_Shutdown_Library_Tree(&default_tree);
|
|
}
|
|
|
|
|
|
void
|
|
ATcodec_Shutdown_Library_Tree (ATcodec_Tree_t *tree)
|
|
{
|
|
/* ATcodec_Buffer_destroy(tree->strs); */
|
|
/* ATcodec_Buffer_destroy(tree->sons); */
|
|
/* ATcodec_Buffer_destroy(tree->leaves); */
|
|
atcodec_lib_init_ok = 0;
|
|
}
|
|
|
|
|
|
#if !defined(TARGET_OS_IPHONE) && !defined(TARGET_IPHONE_SIMULATOR)
|
|
// \todo CHANGE THE '\r'
|
|
static int append_reception(char *buffer, int len, char *global_buffer, int *global_len, int global_buffer_limit)
|
|
{
|
|
int res = 0, i;
|
|
|
|
for(i = 0 ; i < len ; i++)
|
|
{
|
|
global_buffer[(*global_len)++] = buffer[i];
|
|
if(*global_len >= /*INTERNAL_BUFFER_SIZE*/global_buffer_limit)
|
|
{
|
|
// buffer overflow => purge
|
|
*global_len = 0;
|
|
return -1;
|
|
}
|
|
if(buffer[i] == '\r')
|
|
{
|
|
res++;
|
|
}
|
|
}
|
|
|
|
global_buffer[*global_len] = '\0';
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
static ATCODEC_RET process_received_data(ATcodec_Tree_t *tree, int nb, char *global_buffer, int *global_len)
|
|
{
|
|
ATCODEC_RET res = ATCODEC_TRUE;
|
|
int len_dec;
|
|
char memory[INTERNAL_BUFFER_SIZE];
|
|
ATcodec_Memory_t mem;
|
|
|
|
while (nb--) /* nb is the number of \r in the buffer, ie. the number of potential AT commands */
|
|
{
|
|
ATcodec_Memory_Init(&mem, &memory[0], sizeof(memory), 1, NULL, NULL);
|
|
if (find_process_node(tree, global_buffer, &mem, &len_dec) != ATCODEC_TRUE)
|
|
{
|
|
/* Go to the next potential command */
|
|
for(len_dec = 0 ; len_dec < *global_len && global_buffer[len_dec] != '\r' ; len_dec++)
|
|
{
|
|
// nothing
|
|
}
|
|
ATCODEC_PRINT("PURGE\n");
|
|
if(global_buffer[len_dec++] == '\r')
|
|
{
|
|
if(global_buffer[len_dec++] == '\0')
|
|
len_dec++;
|
|
}
|
|
}
|
|
|
|
if (--len_dec >= *global_len)
|
|
len_dec = *global_len;
|
|
|
|
if (*global_len == len_dec)
|
|
{
|
|
ATCODEC_ZERO_MEMSET(global_buffer, 0, *global_len*sizeof(char));
|
|
*global_len = 0;
|
|
}
|
|
else
|
|
{
|
|
memmove(/*dest*/global_buffer, /*src*/global_buffer+len_dec, /*nb bytes*/*global_len-len_dec);
|
|
ATCODEC_ZERO_MEMSET(global_buffer+*global_len-len_dec, 0, (len_dec)*sizeof(char));
|
|
*global_len -= len_dec;
|
|
}
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
/**
|
|
* AT Command receiving thread. ( ** AT Command Server ** )
|
|
* This thread keeps calling a callback function stored
|
|
* in 'func_ptrs.read' to fetch data, accumulates these data until
|
|
* a full AT command has been received, and then call the AT decoder.
|
|
* */
|
|
DEFINE_THREAD_ROUTINE_STACK(ATcodec_Commands_Server,data,ATCODEC_STACK_SIZE)
|
|
{
|
|
ATcodec_Tree_t *tree = &default_tree;
|
|
AT_CODEC_ERROR_CODE res;
|
|
int32_t v_loop, v_read, len, nb_cmd = 0;
|
|
char buffer[INTERNAL_BUFFER_SIZE]; // user-defined
|
|
char global_buffer[INTERNAL_BUFFER_SIZE];
|
|
char safety[16]; // Absorbs data overflowing from global_buffer.
|
|
int global_len=0;
|
|
|
|
v_continue = 1;
|
|
PRINT("Thread AT Commands Server Start\n");
|
|
|
|
while(!atcodec_lib_init_ok)
|
|
{
|
|
vp_os_thread_yield();
|
|
}
|
|
|
|
while(v_continue)
|
|
{
|
|
vp_os_memset(buffer,0,sizeof(buffer));
|
|
vp_os_memset(global_buffer,0,sizeof(global_buffer));global_len=0;
|
|
vp_os_memset(safety,0,sizeof(safety));
|
|
|
|
// open and init
|
|
if((res = func_ptrs.open()) != AT_CODEC_OPEN_OK){
|
|
v_continue = 0;
|
|
}
|
|
|
|
for ( v_loop = 1 ; v_loop && func_ptrs.enable() == AT_CODEC_ENABLE_OK; )
|
|
{
|
|
v_read = 1;
|
|
do
|
|
{
|
|
// wait so that thread can give the hand : delay user-defined / OS-dependent
|
|
//vp_os_thread_yield();
|
|
|
|
// -> we do a blocking read few lines after, thus
|
|
// other threads will be able to run during the I/O
|
|
|
|
/* In case of reading from packets, we clear the incoming buffer.
|
|
* Splitting AT commands into several packets would be a bad idea since packet order in not guaranteed in UDP.
|
|
*/
|
|
if (at_codec_reading_mode==ATCODEC_READ_FROM_PACKETS){
|
|
vp_os_memset(global_buffer,0,sizeof(global_buffer));
|
|
global_len=0;
|
|
}
|
|
|
|
/*
|
|
* Read some bytes; this function blocks until some data are made
|
|
* available by the VP_COM thread.
|
|
*/
|
|
len = sizeof(buffer); //INTERNAL_BUFFER_SIZE/*/2*/; // user-defined
|
|
res = func_ptrs.read((uint8_t*)&buffer[0], (int32_t*)&len);
|
|
|
|
if(res == AT_CODEC_READ_OK)
|
|
{
|
|
if(len > 0)
|
|
{
|
|
// process characters and update v_read
|
|
// \todo Do not use nb_cmd ?
|
|
|
|
/* Data are accumulated in the global buffer until at least one '\r' is found. */
|
|
if((nb_cmd = append_reception(&buffer[0], len, &global_buffer[0], &global_len,sizeof(global_buffer))) > 0)
|
|
{
|
|
v_read = 0;
|
|
}
|
|
else if(nb_cmd == -1) /* no \r found in the global_buffer*/
|
|
{
|
|
// a buffer overflow occurs
|
|
switch(at_codec_reading_mode)
|
|
{
|
|
case ATCODEC_READ_FROM_STREAM:
|
|
PRINT("AT Codec buffer was filled before a full AT commands was received.");
|
|
break;
|
|
case ATCODEC_READ_FROM_PACKETS:
|
|
PRINT("AT Codec received a packet with no complete AT command or buffer was too small to store the whole packet.");
|
|
break;
|
|
}
|
|
//ATCODEC_PRINT("Overflow\n");
|
|
|
|
/* In case of overflow, a TCP connection should be reinitialized in order to resynchronize
|
|
* the client and the server. Otherwise there is no way to find the beginning of the next AT Command.
|
|
* For a UDP connection, we assume all packets begin with an AT Command, and we just wait
|
|
* for the next packet to arrive.
|
|
*/
|
|
if (at_codec_reading_mode==ATCODEC_READ_FROM_STREAM) { v_loop = 0; }
|
|
}
|
|
else
|
|
{
|
|
v_read = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(len < 0)
|
|
{
|
|
ATCODEC_PRINT("read returns a neg length\n");
|
|
v_loop = 0;
|
|
}
|
|
}
|
|
}
|
|
else /* if (res == AT_CODEC_READ_OK) */
|
|
{
|
|
// an error occurred
|
|
ATCODEC_PRINT("an error occurs\n");
|
|
v_loop = 0;
|
|
}
|
|
}
|
|
while (v_read && v_loop);
|
|
|
|
// process what has been received if no error occurs
|
|
if(v_loop)
|
|
{
|
|
// ...
|
|
if(process_received_data(tree, nb_cmd, &global_buffer[0], &global_len) != ATCODEC_TRUE)
|
|
{
|
|
ATCODEC_PRINT("process_received returns false\n");
|
|
v_loop = 0;
|
|
}
|
|
}
|
|
} /*for*/
|
|
|
|
// close and un-init : user-defined
|
|
if((res = func_ptrs.close()) != AT_CODEC_CLOSE_OK)
|
|
v_continue = 0;
|
|
|
|
}/* while */
|
|
|
|
if ((res = func_ptrs.shutdown()) != AT_CODEC_SHUTDOWN_OK)
|
|
{
|
|
ATCODEC_PRINT("ATcodec Shutdown error\n");
|
|
}
|
|
|
|
return((THREAD_RET)0);
|
|
}
|
|
#endif // ! TARGET_OS_IPHONE && ! TARGET_IPHONE_SIMULATOR
|
|
|
|
|
|
static ATCODEC_RET
|
|
valist_ATcodec_Queue_Message_valist_Tree(ATcodec_Tree_t *tree, AT_CODEC_MSG_ID id, va_list *va)
|
|
{
|
|
int32_t len;
|
|
ATCODEC_RET res;
|
|
ATcodec_Tree_Node_t *node = ATcodec_Tree_Node_get(tree, (int)id);
|
|
ATcodec_Message_Data_t *data = (ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data);
|
|
char *total_str = (char *)ATcodec_Buffer_getElement(&tree->strs, data->total_str);
|
|
ATcodec_Memory_t msg, fmt;
|
|
char buffer[INTERNAL_BUFFER_SIZE];
|
|
|
|
if(!atcodec_lib_init_ok)
|
|
return ATCODEC_FALSE;
|
|
|
|
|
|
vp_os_mutex_lock(&ATcodec_cond_mutex);
|
|
|
|
ATcodec_Memory_Init(&msg, (char*)&buffer[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
|
|
ATcodec_Memory_Init(&fmt, total_str, 0, 1, NULL, NULL);
|
|
|
|
if((res = vp_atcodec_sprintf_valist(&msg, &len, &fmt, va)) != ATCODEC_TRUE)
|
|
{
|
|
vp_os_mutex_unlock(&ATcodec_cond_mutex);
|
|
va_end(*va);
|
|
return res;
|
|
}
|
|
|
|
if(ATcodec_Message_len + len < INTERNAL_BUFFER_SIZE)
|
|
{
|
|
memcpy(&ATcodec_Message_Buffer[ATcodec_Message_len], &buffer[0], len);
|
|
ATcodec_Message_len += len;
|
|
}
|
|
|
|
//vp_os_cond_signal(&ATcodec_wait_cond);
|
|
vp_os_mutex_unlock(&ATcodec_cond_mutex);
|
|
va_end(*va);
|
|
|
|
return ATCODEC_TRUE;
|
|
}
|
|
|
|
/**
|
|
* Push an AT Command in the AT command output queue.
|
|
* This is the function called by the ardrone_at.c API functions.
|
|
* */
|
|
ATCODEC_RET
|
|
ATcodec_Queue_Message_valist(AT_CODEC_MSG_ID id, ...)
|
|
{
|
|
ATCODEC_RET res;
|
|
va_list va;
|
|
|
|
va_start(va, id);
|
|
|
|
res = valist_ATcodec_Queue_Message_valist_Tree(&default_tree, id, &va);
|
|
|
|
va_end(va);
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
ATCODEC_RET
|
|
ATcodec_Queue_Message_valist_Tree(ATcodec_Tree_t *tree, AT_CODEC_MSG_ID id, ...)
|
|
{
|
|
ATCODEC_RET res;
|
|
va_list va;
|
|
|
|
va_start(va, id);
|
|
|
|
res = valist_ATcodec_Queue_Message_valist_Tree(tree, id, &va);
|
|
|
|
va_end(va);
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
ATCODEC_RET
|
|
ATcodec_Queue_Message_params(AT_CODEC_MSG_ID id, ATcodec_Memory_t *params)
|
|
{
|
|
return ATcodec_Queue_Message_params_Tree(&default_tree, id, params);
|
|
}
|
|
|
|
|
|
ATCODEC_RET
|
|
ATcodec_Queue_Message_params_Tree(ATcodec_Tree_t *tree, AT_CODEC_MSG_ID id, ATcodec_Memory_t *params)
|
|
{
|
|
int32_t len;
|
|
ATCODEC_RET res;
|
|
ATcodec_Tree_Node_t *node = ATcodec_Tree_Node_get(tree, (int)id);
|
|
ATcodec_Message_Data_t *data = (ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data);
|
|
char *total_str = (char *)ATcodec_Buffer_getElement(&tree->strs, data->total_str);
|
|
ATcodec_Memory_t msg, fmt;
|
|
char buffer[INTERNAL_BUFFER_SIZE];
|
|
if(!atcodec_lib_init_ok)
|
|
return ATCODEC_FALSE;
|
|
|
|
ATcodec_Memory_Init(&msg, (char*)&buffer[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
|
|
ATcodec_Memory_Init(&fmt, total_str, 0, 1, NULL, NULL);
|
|
|
|
params->current = (char *)params->start;
|
|
if((res = vp_atcodec_sprintf_params(&msg, &len, &fmt, params)) != ATCODEC_TRUE)
|
|
{
|
|
vp_os_mutex_unlock(&ATcodec_cond_mutex);
|
|
return res;
|
|
}
|
|
|
|
|
|
if(ATcodec_Message_len + len < INTERNAL_BUFFER_SIZE)
|
|
{
|
|
memcpy(&ATcodec_Message_Buffer[ATcodec_Message_len], &buffer[0], len);
|
|
ATcodec_Message_len += len;
|
|
}
|
|
|
|
//vp_os_cond_signal(&ATcodec_wait_cond);
|
|
vp_os_mutex_unlock(&ATcodec_cond_mutex);
|
|
|
|
return ATCODEC_TRUE;
|
|
}
|
|
|
|
|
|
ATCODEC_RET
|
|
ATcodec_Send_Messages()
|
|
{
|
|
ATCODEC_RET res = ATCODEC_TRUE;
|
|
|
|
if(!atcodec_lib_init_ok)
|
|
return ATCODEC_FALSE;
|
|
|
|
vp_os_mutex_lock(&ATcodec_cond_mutex);
|
|
if(ATcodec_Message_len > INTERNAL_BUFFER_SIZE)
|
|
PRINT("ATcodec_Send_Messages : buf=%s, len=%d\n", &ATcodec_Message_Buffer[0], ATcodec_Message_len);
|
|
|
|
if(ATcodec_Message_len && func_ptrs.write((uint8_t*)&ATcodec_Message_Buffer[0], (int32_t*)&ATcodec_Message_len) != AT_CODEC_WRITE_OK)
|
|
res = ATCODEC_FALSE;
|
|
|
|
ATcodec_Message_len = 0;
|
|
|
|
vp_os_mutex_unlock(&ATcodec_cond_mutex);
|
|
|
|
return res;
|
|
}
|
|
|
|
#if !defined(TARGET_OS_IPHONE) && !defined(TARGET_IPHONE_SIMULATOR)
|
|
DEFINE_THREAD_ROUTINE_STACK(ATcodec_Commands_Client,data,ATCODEC_STACK_SIZE)
|
|
{
|
|
AT_CODEC_ERROR_CODE res;
|
|
int32_t v_loop;
|
|
|
|
v_continue = 1;
|
|
PRINT("Thread AT Commands Client Start\n");
|
|
|
|
while(!atcodec_lib_init_ok)
|
|
{
|
|
vp_os_thread_yield();
|
|
}
|
|
|
|
while(v_continue)
|
|
{
|
|
// open and init
|
|
if((res = func_ptrs.open()) != AT_CODEC_OPEN_OK)
|
|
v_continue = 0;
|
|
|
|
for ( v_loop = 1 ; v_loop ; )
|
|
{
|
|
vp_os_thread_yield();
|
|
|
|
// wait a successful ATcodec_Queue_... has been called
|
|
vp_os_mutex_lock(&ATcodec_cond_mutex);
|
|
//vp_os_cond_wait(&ATcodec_wait_cond);
|
|
|
|
//ATCODEC_PRINT("Must send \"%s\"\n", &ATcodec_Message_Buffer[0]);
|
|
if(ATcodec_Message_len && func_ptrs.write((uint8_t*)&ATcodec_Message_Buffer[0], (int32_t*)&ATcodec_Message_len) != AT_CODEC_WRITE_OK)
|
|
v_loop = 0;
|
|
|
|
ATcodec_Message_len = 0;
|
|
vp_os_mutex_unlock(&ATcodec_cond_mutex);
|
|
}
|
|
|
|
// close and un-init : user-defined
|
|
if((res = func_ptrs.close()) != AT_CODEC_CLOSE_OK)
|
|
v_continue = 0;
|
|
}
|
|
|
|
if((res = func_ptrs.shutdown()) != AT_CODEC_SHUTDOWN_OK)
|
|
{
|
|
ATCODEC_PRINT("ATcodec Shutdown error\n");
|
|
}
|
|
|
|
return((THREAD_RET)0);
|
|
}
|
|
#endif // ! TARGET_OS_IPHONE && ! TARGET_IPHONE_SIMULATOR
|
|
|
|
|
|
void ATcodec_exit_thread(void)
|
|
{
|
|
v_continue = 0;
|
|
}
|