diff --git a/src/serial.c b/src/serial.c index 81874a3..7f7c863 100644 --- a/src/serial.c +++ b/src/serial.c @@ -53,14 +53,14 @@ double SER_BAUDRATE; // Unix用コード #include #include -int g_device_port; +int g_device_port = 0; struct termios g_oldtio; #else // Windows用コード #include -HANDLE g_hdevices; +HANDLE g_hdevices = NULL; DCB g_olddcb; COMMTIMEOUTS g_oldcto; @@ -362,13 +362,21 @@ int serial_close(void) #if !defined(__MINGW32__) // Unix用 // 設定を元に戻す - tcsetattr(g_device_port, TCSANOW, &g_oldtio); - close(g_device_port); + if (g_device_port != 0) + { + tcsetattr(g_device_port, TCSANOW, &g_oldtio); + close(g_device_port); + g_device_port = 0; + } #else // Windows用 - SetCommState(g_hdevices, &g_olddcb); // シリアルポートの状態を設定 - SetCommTimeouts(g_hdevices, &g_oldcto); // タイムアウトの状態を設定 - CloseHandle(g_hdevices); + if (g_hdevices != NULL) + { + SetCommState(g_hdevices, &g_olddcb); // シリアルポートの状態を設定 + SetCommTimeouts(g_hdevices, &g_oldcto); // タイムアウトの状態を設定 + CloseHandle(g_hdevices); + g_hdevices = NULL; + } #endif // !defined(__MINGW32__) return 1; @@ -381,7 +389,7 @@ void serial_flush_in(void) tcflush(g_device_port, TCIFLUSH); #else // Windows用 - char buf[4000]; + char buf[4096]; DWORD len; DWORD ret; @@ -422,7 +430,7 @@ void serial_flush_out(void) // シリアルポートからの受信処理 int serial_recieve(int (*serial_event)(char *, int, double, void *), void *data) { - char buf[4000]; + char buf[4096]; double receive_time; int retval; diff --git a/src/ypprotocol.c b/src/ypprotocol.c index 04caffc..bda24d5 100644 --- a/src/ypprotocol.c +++ b/src/ypprotocol.c @@ -37,9 +37,15 @@ #include #include +#define RECEIVE_BUFFER_SIZE 2048 + int ss_receive(char *buf, int len, double receive_time, void *data) { buf[len] = 0; + if (len + strlen((char *)data) > RECEIVE_BUFFER_SIZE) + { + return -3; + } strcat((char *)data, buf); if (strstr((char *)data, "\n\n")) { @@ -51,7 +57,7 @@ int ss_receive(char *buf, int len, double receive_time, void *data) int set_baudrate(int baud) { /* Send & Recive Buffer */ - char buf[2048]; + char buf[RECEIVE_BUFFER_SIZE]; /* Temporary */ strcpy(buf, "\n\n\n\n"); @@ -63,7 +69,8 @@ int set_baudrate(int baud) serial_write(buf, strlen(buf)); buf[0] = 0; - serial_recieve(ss_receive, buf); + if (serial_recieve(ss_receive, buf) != -2) + return 0; if (strstr(buf, "\n00P\n") != NULL) { @@ -84,6 +91,10 @@ int set_baudrate(int baud) int vv_receive(char *buf, int len, double receive_time, void *data) { buf[len] = 0; + if (len + strlen((char *)data) > RECEIVE_BUFFER_SIZE) + { + return -3; + } strcat((char *)data, buf); if (strstr((char *)data, "\n\n")) { @@ -100,7 +111,7 @@ int vv_receive(char *buf, int len, double receive_time, void *data) int get_version(Ver_t *apVer) { /* Send & Recive Buffer */ - char buf[2048], *readpos; + char buf[RECEIVE_BUFFER_SIZE], *readpos; /* Temporary */ char *tmp, *lf, *val; char *tag, *wbuf; @@ -117,11 +128,10 @@ int get_version(Ver_t *apVer) serial_write(buf, strlen(buf)); buf[0] = 0; - serial_recieve(vv_receive, buf); + if (serial_recieve(vv_receive, buf) != -2) + return 0; if (strstr(buf, "\n00P\n") == 0) - { return 0; - } while (1) { @@ -176,7 +186,7 @@ int get_version(Ver_t *apVer) int get_parameter(Param_t *apParam) { /* Send & Recive Buffer */ - char buf[2048], *readpos; + char buf[RECEIVE_BUFFER_SIZE], *readpos; /* Temporary */ char *tmp, *lf, *val; char *tag, *wbuf; @@ -193,11 +203,10 @@ int get_parameter(Param_t *apParam) serial_write(buf, strlen(buf)); buf[0] = 0; - serial_recieve(vv_receive, buf); + if (serial_recieve(vv_receive, buf) != -2) + return 0; if (strstr(buf, "\n00P\n") == 0) - { return 0; - } while (1) { @@ -239,7 +248,7 @@ int get_parameter(Param_t *apParam) int get_embedded_param(char *param) { /* Send & Recive Buffer */ - char buf[2048], *readpos; + char buf[RECEIVE_BUFFER_SIZE], *readpos; /* Temporary */ char *lf; @@ -255,11 +264,10 @@ int get_embedded_param(char *param) serial_write(buf, strlen(buf)); buf[0] = 0; - serial_recieve(vv_receive, buf); + if (serial_recieve(vv_receive, buf) != -2) + return 0; if (strstr(buf, "\n00P\n") == 0) - { return 0; - } while (1) { diff --git a/src/ypspur-coordinator.c b/src/ypspur-coordinator.c index 7686af4..a59e7aa 100644 --- a/src/ypspur-coordinator.c +++ b/src/ypspur-coordinator.c @@ -336,7 +336,7 @@ int main(int argc, char *argv[]) if (!temp_paramfile) { yprintf(OUTPUT_LV_ERROR, "Error: Failed to create temporary file.\n"); - return 0; + break; } if (!get_embedded_param(param)) { @@ -364,7 +364,7 @@ int main(int argc, char *argv[]) if (!set_paramptr(temp_paramfile)) { yprintf(OUTPUT_LV_ERROR, "Error: Cannot use embedded parameter.\n"); - return 0; + break; } } else @@ -373,7 +373,7 @@ int main(int argc, char *argv[]) if (!set_param(param->parameter_filename, paramfile)) { yprintf(OUTPUT_LV_ERROR, "Error: Cannot load parameter file.\n"); - return 0; + break; } } { @@ -403,10 +403,6 @@ int main(int argc, char *argv[]) { // 設定失敗 yprintf(OUTPUT_LV_WARNING, "Error: Failed to change baudrate.\n"); - - serial_close(); - - quit = 0; break; // quit=0でbreakしたら異常終了と判断 } if (ret == 4) @@ -433,13 +429,8 @@ int main(int argc, char *argv[]) } if (!(option(OPTION_PARAM_CONTROL))) - { if (apply_robot_params() < 1) - { - serial_close(); break; - } - } /* サーボをかける */ SpurUserParamsPtr spur; @@ -509,11 +500,6 @@ int main(int argc, char *argv[]) } /* 終了処理 */ - if (!(option(OPTION_WITHOUT_DEVICE))) - { - serial_close(); - } - if (update_thread_en) { pthread_cancel(update_thread); @@ -539,6 +525,7 @@ int main(int argc, char *argv[]) yp_usleep(500000); if (!(option(OPTION_WITHOUT_DEVICE))) { + serial_close(); while (!serial_tryconnect(param->device_name)) { yp_usleep(200000); @@ -551,6 +538,9 @@ int main(int argc, char *argv[]) break; } while (1); + if (!(option(OPTION_WITHOUT_DEVICE))) + serial_close(); + #ifdef HAVE_SSM /* SSM終了処理 */ if (!option(OPTION_WITHOUT_SSM))