void new_vi(void)
{
int Dif;
unsigned int CurrentFPSTime;
static unsigned int LastFPSTime = 0;
static unsigned int CounterTime = 0;
static unsigned int CalculatedTime ;
static int VI_Counter = 0;
double AdjustedLimit = VILimitMilliseconds * 100.0 / l_SpeedFactor; // adjust for selected emulator speed
int time;
start_section(IDLE_SECTION);
VI_Counter++;
#ifdef DBG
if(g_DebuggerActive) DebuggerCallback(DEBUG_UI_VI, 0);
#endif
if(LastFPSTime == 0)
{
LastFPSTime = gettimeofday_msec();
CounterTime = gettimeofday_msec();
return;
}
CurrentFPSTime = gettimeofday_msec();
Dif = CurrentFPSTime - LastFPSTime;
if (Dif < AdjustedLimit)
{
CalculatedTime = CounterTime + AdjustedLimit * VI_Counter;
time = (int)(CalculatedTime - CurrentFPSTime);
if (time > 0)
{
#ifdef WIN32
Sleep(time);
#else
usleep(time * 1000);
#endif
}
CurrentFPSTime = CurrentFPSTime + time;
}
if (CurrentFPSTime - CounterTime >= 1000.0 )
{
CounterTime = gettimeofday_msec();
VI_Counter = 0 ;
}
LastFPSTime = CurrentFPSTime ;
end_section(IDLE_SECTION);
}
/**
* @fn int check_lifelimit(struct CONN_BCAP_SERVER *parent)
* @brief Checks the life limit all of child nodes, and if expired then deletes.
* @param[in] parent The server object.
*/
static int
check_lifelimit(struct CONN_BCAP_SERVER *parent)
{
uint32_t cur, diff;
struct CONN_BCAP_SERVER *child, *tmp;
child = parent->node1;
while(child != NULL) {
tmp = child->node2;
cur = gettimeofday_msec();
diff = calc_time_diff(child->last_modified, cur);
if(diff > UDP_LIFELIMIT) {
change_relation(child, DESTROY_SELF, NULL);
}
child = tmp;
}
change_relation(parent, DELETE_CHILD, NULL);
return parent->num_child;
}
/**
* @fn HRESULT receive_execute(struct CONN_BCAP_SERVER *bcap_param)
* @brief Receives the b-CAP packet and executes callback functions.
* @param[in,out] bcap_param b-CAP communication object.
*/
static HRESULT
receive_execute(struct CONN_BCAP_SERVER *bcap_param)
{
int index;
int32_t relation_id;
uint16_t i, clear_flag = 1;
uint32_t cur, start, diff;
HRESULT hr;
struct CONN_BCAP_SERVER *tmp_param = bcap_param;
struct CONN_PARAM_COMMON *device = &bcap_param->device;
struct BCAP_PACKET tmp_send_packet, tmp_recv_packet, *send_packet =
&bcap_param->last_send, *recv_packet = &bcap_param->last_recv;
struct VEC_OBJECT *pObj = NULL;
BSTR bstrOpt = NULL;
VARIANT vntTmp, vntOpt;
VariantInit(&vntTmp);
VariantInit(&vntOpt);
/* Initializes temporary packet */
tmp_recv_packet.argc = (uint16_t) -1;
tmp_recv_packet.args = NULL;
/* Receives b-CAP packet */
hr = bcap_recv(device, &tmp_recv_packet, 0);
if(SUCCEEDED(hr)) {
/* Sets S_EXECUTING packet */
memset(&tmp_send_packet, 0, sizeof(struct BCAP_PACKET));
tmp_send_packet.serial = tmp_recv_packet.serial;
tmp_send_packet.id = S_EXECUTING;
/* Checks retry packet */
switch(device->type) {
case CONN_UDP:
tmp_param = search_node(bcap_param, device->arg,
sizeof(struct sockaddr_in));
if(tmp_param == NULL) {
/* Checks life limit */
if((bcap_param->num_child >= BCAP_CLIENT_MAX)
&& (check_lifelimit(bcap_param) >= BCAP_CLIENT_MAX))
{
tmp_send_packet.id = E_MAX_CONNECT;
bcap_send(device, &tmp_send_packet);
hr = S_FALSE;
goto exit_proc;
}
/* Adds child */
change_relation(bcap_param, ADD_CHILD, &device->sock);
tmp_param = bcap_param->node1;
}
send_packet = &tmp_param->last_send;
//break;
case CONN_COM:
/* Sets retry count */
tmp_recv_packet.reserv =
(tmp_recv_packet.reserv == 0) ?
tmp_recv_packet.serial : tmp_recv_packet.reserv;
/* If already responded, then does not execute */
if(send_packet->serial == tmp_recv_packet.reserv) {
/* Copies last send packet */
tmp_send_packet = *send_packet;
/* Sets new serial number */
tmp_send_packet.serial = tmp_recv_packet.serial;
/* Sends temporary send packet */
bcap_send(device, &tmp_send_packet);
hr = S_FALSE;
goto exit_proc;
}
break;
default:
break;
}
/* Checks execute thread */
hr = wait_event(&bcap_param->comp_evt, 0);
if(hr == E_TIMEOUT) {
/* Sends result busy process */
tmp_send_packet.id = E_BUSY_PROC;
bcap_send(device, &tmp_send_packet);
goto exit_proc;
}
switch(tmp_recv_packet.id) {
case ID_SERVICE_START:
case ID_CONTROLLER_CONNECT:
case ID_CONTROLLER_GETEXTENSION:
case ID_CONTROLLER_GETFILE:
case ID_FILE_GETFILE:
case ID_CONTROLLER_GETROBOT:
case ID_CONTROLLER_GETTASK:
case ID_CONTROLLER_GETVARIABLE:
case ID_EXTENSION_GETVARIABLE:
case ID_FILE_GETVARIABLE:
case ID_ROBOT_GETVARIABLE:
case ID_TASK_GETVARIABLE:
case ID_CONTROLLER_GETCOMMAND:
case ID_CONTROLLER_GETMESSAGE:
if(bcap_param->num_object >= BCAP_OBJECT_MAX) {
tmp_send_packet.id = E_MAX_OBJECT;
bcap_send(device, &tmp_send_packet);
hr = S_FALSE;
goto exit_proc;
}
if(tmp_recv_packet.id == ID_SERVICE_START) {
if((tmp_recv_packet.argc >= 1) && (tmp_recv_packet.args != NULL)) {
VariantCopy(&vntTmp, &tmp_recv_packet.args[0]);
hr = VariantChangeType(&vntTmp, &vntTmp, 0, VT_BSTR);
if(FAILED(hr)) {
tmp_send_packet.id = hr;
bcap_send(device, &tmp_send_packet);
hr = S_FALSE;
goto exit_proc;
}
} else {
vntTmp.vt = VT_BSTR;
vntTmp.bstrVal = SysAllocString(L"");
}
bstrOpt = SysAllocString(L"WDT");
hr = GetOptionValue(vntTmp.bstrVal, bstrOpt, VT_UI4, &vntOpt);
vntOpt.ulVal =
(vntOpt.vt == VT_UI4) ? vntOpt.ulVal : INIT_WDT_INTERVAL;
if(vntOpt.ulVal < MIN_WDT_INTERVAL) {
tmp_send_packet.id = E_INVALIDARG;
bcap_send(device, &tmp_send_packet);
hr = S_FALSE;
goto exit_proc;
} else {
tmp_param->wdt_interval = vntOpt.ulVal;
}
SysFreeString(bstrOpt);
VariantClear(&vntOpt);
bstrOpt = SysAllocString(L"InvokeTimeout");
hr = GetOptionValue(vntTmp.bstrVal, bstrOpt, VT_UI4, &vntOpt);
vntOpt.ulVal =
(vntOpt.vt == VT_UI4) ? vntOpt.ulVal : INIT_EXEC_TIMEOUT;
if(vntOpt.ulVal < MIN_WDT_INTERVAL) {
tmp_send_packet.id = E_INVALIDARG;
bcap_send(device, &tmp_send_packet);
hr = S_FALSE;
goto exit_proc;
} else {
tmp_param->exec_timeout = vntOpt.ulVal;
}
SysFreeString(bstrOpt);
VariantClear(&vntOpt);
VariantClear(&vntTmp);
bstrOpt = NULL;
}
break;
default:
break;
}
/* Resets last received packet */
if(recv_packet->args != NULL) {
for(i = 0; i < recv_packet->argc; i++) {
VariantClear(&recv_packet->args[i]);
}
free(recv_packet->args);
}
/* Copies to last receive packet */
clear_flag = 0;
*recv_packet = tmp_recv_packet;
/* Runs execute thread */
reset_event(&bcap_param->comp_evt);
set_event(&bcap_param->exec_evt);
if(SUCCEEDED(hr)) {
start = gettimeofday_msec();
while(1) {
hr = wait_event(&bcap_param->comp_evt, tmp_param->wdt_interval);
if(SUCCEEDED(hr)) {
break;
} else {
/* Sends S_EXECUTING packet */
hr = bcap_send(device, &tmp_send_packet);
if(FAILED(hr)) {
break;
}
}
/* Checks executing timeout */
cur = gettimeofday_msec();
diff = calc_time_diff(start, cur);
if(diff > tmp_param->exec_timeout) {
hr = E_TIMEOUT;
break;
}
}
}
}
exit_proc:
if(hr == S_OK) {
if(bcap_param->last_send.id == S_OK) {
/* Changes the vector of created objects */
relation_id = m_map_id[recv_packet->id].relation_id;
if(relation_id > 0) { // Push
pObj = (struct VEC_OBJECT *) malloc(sizeof(struct VEC_OBJECT));
if(pObj != NULL) {
memset(pObj, 0, sizeof(struct VEC_OBJECT));
pObj->id = relation_id;
pObj->hObj =
(recv_packet->id == ID_SERVICE_START) ?
0 : bcap_param->last_send.args[0].lVal;
push_vector(bcap_param, pObj);
}
}
else if(relation_id < 0) { // Pop
index = search_vector(bcap_param, recv_packet->id,
(recv_packet->id == ID_SERVICE_STOP) ?
0 : recv_packet->args[0].lVal);
if(index >= 0) {
pop_vector(bcap_param, &pObj, index);
free(pObj);
}
if((device->type == CONN_UDP)
&& (recv_packet->id == ID_SERVICE_STOP))
{
change_relation(tmp_param, DESTROY_SELF, NULL);
change_relation(bcap_param, DELETE_CHILD, NULL);
tmp_param = NULL;
}
}
}
/* Responds the result message */
hr = bcap_send(device, &bcap_param->last_send);
if(SUCCEEDED(hr) && (tmp_param != NULL)) {
tmp_param->last_send.serial = bcap_param->last_send.serial;
tmp_param->last_send.reserv = bcap_param->last_send.reserv;
tmp_param->last_send.id = bcap_param->last_send.id;
tmp_param->last_send.argc = bcap_param->last_send.argc;
VariantCopy(tmp_param->last_send.args, bcap_param->last_send.args);
tmp_param->last_modified = gettimeofday_msec();
}
}
/* Clears temporary packet */
if(clear_flag) {
if(tmp_recv_packet.args != NULL) {
for(i = 0; i < tmp_recv_packet.argc; i++) {
VariantClear(&tmp_recv_packet.args[i]);
}
free(tmp_recv_packet.args);
}
}
VariantClear(&vntTmp);
VariantClear(&vntOpt);
if(bstrOpt) {
SysFreeString(bstrOpt);
}
return hr;
}
/**
* @fn HRESULT change_relation(struct CONN_BCAP_SERVER *own, int mode, int *sock)
* @brief Changes thread's relationships for TCP connection.
* @param[in,out] own The connection parameter to be changed.
* @param[in] mode The change mode.
* @param[in] sock The client connection.
*/
static HRESULT
change_relation(struct CONN_BCAP_SERVER *own, int mode, int *sock)
{
HRESULT hr = S_OK;
struct CONN_BCAP_SERVER *node, *tmp;
int flag_mutex;
MUTEX *mutex;
/* Checks mutex object */
mutex = own->relation_mutex;
flag_mutex = ((mutex != NULL) ? 1 : 0);
/* Locks mutex and must not returns this function without end of one. */
if(flag_mutex) {
hr = lock_mutex(mutex, INFINITE);
if(FAILED(hr)) return hr;
}
switch(mode) {
case ADD_CHILD:
/* Root node only */
if(own->parent == NULL) {
/* Creates new node */
node = (struct CONN_BCAP_SERVER *) malloc(
sizeof(struct CONN_BCAP_SERVER));
if(node == NULL) {
hr = E_OUTOFMEMORY;
goto exit_proc;
}
/* Initializes node memory */
memset(node, 0, sizeof(struct CONN_BCAP_SERVER));
/* Copies device parameters from parent node */
node->device = own->device;
/* Sets child socket */
node->device.sock = *sock;
/* Copies parameters */
node->exec_timeout = own->exec_timeout;
node->wdt_interval = own->wdt_interval;
/* Sets last modified */
node->last_modified = gettimeofday_msec();
/* Copies mutex */
node->relation_mutex = own->relation_mutex;
/* Sets parent node */
node->parent = own;
own->num_child++;
/* Replaces the head of children */
tmp = own->node1;
own->node1 = node;
node->node2 = tmp;
if(tmp != NULL) tmp->node1 = node;
if(flag_mutex) {
hr = node->device.dn_set_timeout(*sock, node->device.timeout);
if(FAILED(hr)) {
node->device.dn_close(sock);
free(node);
goto exit_proc;
}
/* Creates terminal event for main thread */
hr = create_event(&node->term_main_evt, 1, 0);
if(FAILED(hr)) {
node->device.dn_close(sock);
free(node);
goto exit_proc;
}
/* Begins child thread */
begin_thread(&node->main_thread, &recv_thread, node);
} else {
switch(node->device.type) {
case CONN_UDP:
node->device.arg = malloc(sizeof(struct sockaddr_in));
memcpy(node->device.arg, own->device.arg, sizeof(struct sockaddr_in));
break;
default:
break;
}
node->last_send.args =
(VARIANT*) malloc(sizeof(VARIANT));
VariantInit(node->last_send.args);
}
}
break;
case DELETE_CHILD:
/* Root node only */
if(own->parent == NULL) {
node = own->node2;
while (node != NULL) {
tmp = node->node2;
if(flag_mutex) {
/* Ends child thread */
set_event(&node->term_main_evt);
exit_thread(node->main_thread);
/* Destroys event */
destroy_event(&node->term_main_evt);
/* Closes connection */
node->device.dn_close(&node->device.sock);
} else {
VariantClear(node->last_send.args);
free(node->last_send.args);
if(node->device.arg != NULL) {
free(node->device.arg);
}
}
free(node);
node = tmp;
own->num_child--;
}
own->node2 = NULL;
}
break;
case DESTROY_SELF:
if(own->parent != NULL) {
/* Removes own node from the children list */
tmp = own->node1;
if(tmp == NULL) { // If own node is the youngest children
own->parent->node1 = own->node2;
} else {
tmp->node2 = own->node2;
}
if(own->node2 != NULL) { // If own node is not the oldest children
own->node2->node1 = tmp;
}
/* Adds own node to the parent's delete */
tmp = own->parent->node2;
own->parent->node2 = own;
own->node2 = tmp;
if(tmp != NULL) tmp->node1 = own;
} else {
if(!flag_mutex) {
node = own->node1;
while(node != NULL) {
tmp = node->node2;
change_relation(node, DESTROY_SELF, NULL);
node = tmp;
}
change_relation(own, DELETE_CHILD, NULL);
}
}
break;
default:
hr = E_INVALIDARG;
break;
}
exit_proc:
if(flag_mutex) {
unlock_mutex(mutex);
}
return hr;
}
