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; }