From 113c7237c3d32cf927b7906e237b489699621754 Mon Sep 17 00:00:00 2001 From: alploskov Date: Fri, 8 Mar 2024 22:34:09 +0300 Subject: [PATCH 1/2] GCS_MAVLink: ftp receiving messages where size takes into account the terminating null --- libraries/GCS_MAVLink/GCS_FTP.cpp | 28 +++++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 10725ad586a23..f1205c3ec45c5 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -255,7 +255,9 @@ void GCS_MAVLINK::ftp_worker(void) { // sanity check that our the request looks well formed const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - if ((file_name_len != request.size) || (request.size == 0)) { + const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && + request.data[sizeof(request.data) - 1] == 0); + if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -341,7 +343,9 @@ void GCS_MAVLINK::ftp_worker(void) { // sanity check that our the request looks well formed const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - if ((file_name_len != request.size) || (request.size == 0)) { + const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && + request.data[sizeof(request.data) - 1] == 0); + if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -396,7 +400,9 @@ void GCS_MAVLINK::ftp_worker(void) { { // sanity check that our the request looks well formed const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - if ((file_name_len != request.size) || (request.size == 0)) { + const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && + request.data[sizeof(request.data) - 1] == 0); + if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -417,7 +423,9 @@ void GCS_MAVLINK::ftp_worker(void) { { // sanity check that our the request looks well formed const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - if ((file_name_len != request.size) || (request.size == 0)) { + const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && + request.data[sizeof(request.data) - 1] == 0); + if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -437,7 +445,9 @@ void GCS_MAVLINK::ftp_worker(void) { { // sanity check that our the request looks well formed const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - if ((file_name_len != request.size) || (request.size == 0)) { + const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && + request.data[sizeof(request.data) - 1] == 0); + if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -548,7 +558,9 @@ void GCS_MAVLINK::ftp_worker(void) { const size_t len1 = strnlen(filename1, sizeof(request.data)-2); const char *filename2 = (char*)&request.data[len1+1]; const size_t len2 = strnlen(filename2, sizeof(request.data)-(len1+1)); - if (filename1[len1] != 0 || (len1+len2+1 != request.size) || (request.size == 0)) { + const bool is_req_size_consider_tnull = (request.size - (len1+len2) == 2 && + request.data[sizeof(request.data) - 1] == 0); + if (filename1[len1] != 0 || ((len1+len2+1 != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -616,8 +628,10 @@ void GCS_MAVLINK::ftp_list_dir(struct pending_ftp &request, struct pending_ftp & response.offset = request.offset; // this should be set for any failure condition for debugging const size_t directory_name_size = strnlen((char *)request.data, sizeof(request.data)); + const bool is_req_size_consider_tnull = ((request.size - directory_name_size == 1) && + request.data[sizeof(request.data) - 1] == 0); // sanity check that our the request looks well formed - if ((directory_name_size != request.size) || (request.size == 0)) { + if (((directory_name_size != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { ftp_error(response, FTP_ERROR::InvalidDataSize); return; } From b74ae2b0dadc1f51657219af5fdddb6fa56421be Mon Sep 17 00:00:00 2001 From: alploskov Date: Wed, 13 Mar 2024 01:27:34 +0300 Subject: [PATCH 2/2] GCS_MAVLink: ftp replace copy paste of name length check logic by function --- libraries/GCS_MAVLink/GCS.h | 1 + libraries/GCS_MAVLink/GCS_FTP.cpp | 41 +++++++++++++------------------ 2 files changed, 18 insertions(+), 24 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 52f728c9d293f..05f19328b4c3c 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1009,6 +1009,7 @@ class GCS_MAVLINK static struct ftp_state ftp; static void ftp_error(struct pending_ftp &response, FTP_ERROR error); // FTP helper method for packing a NAK + static bool ftp_check_name_len(const struct pending_ftp &request); static int gen_dir_entry(char *dest, size_t space, const char * path, const struct dirent * entry); // FTP helper for emitting a dir response static void ftp_list_dir(struct pending_ftp &request, struct pending_ftp &response); diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index f1205c3ec45c5..23944a8127c06 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -129,6 +129,17 @@ bool GCS_MAVLINK::send_ftp_reply(const pending_ftp &reply) return true; } +bool GCS_MAVLINK::ftp_check_name_len(const struct pending_ftp &request) { + const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); + if (request.size == 0) { + return false; + } + if (file_name_len == request.size) { + return true; + } + return (request.size - file_name_len == 1) && (request.data[sizeof(request.data) - 1] == 0); +} + void GCS_MAVLINK::ftp_error(struct pending_ftp &response, FTP_ERROR error) { response.opcode = FTP_OP::Nack; response.data[0] = static_cast(error); @@ -254,10 +265,7 @@ void GCS_MAVLINK::ftp_worker(void) { } // sanity check that our the request looks well formed - const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && - request.data[sizeof(request.data) - 1] == 0); - if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { + if (!ftp_check_name_len(request)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -342,10 +350,7 @@ void GCS_MAVLINK::ftp_worker(void) { } // sanity check that our the request looks well formed - const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && - request.data[sizeof(request.data) - 1] == 0); - if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { + if (!ftp_check_name_len(request)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -399,10 +404,7 @@ void GCS_MAVLINK::ftp_worker(void) { case FTP_OP::CreateDirectory: { // sanity check that our the request looks well formed - const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && - request.data[sizeof(request.data) - 1] == 0); - if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { + if (!ftp_check_name_len(request)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -422,10 +424,7 @@ void GCS_MAVLINK::ftp_worker(void) { case FTP_OP::RemoveFile: { // sanity check that our the request looks well formed - const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && - request.data[sizeof(request.data) - 1] == 0); - if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { + if (!ftp_check_name_len(request)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -444,10 +443,7 @@ void GCS_MAVLINK::ftp_worker(void) { case FTP_OP::CalcFileCRC32: { // sanity check that our the request looks well formed - const size_t file_name_len = strnlen((char *)request.data, sizeof(request.data)); - const bool is_req_size_consider_tnull = ((request.size - file_name_len == 1) && - request.data[sizeof(request.data) - 1] == 0); - if (((file_name_len != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { + if (!ftp_check_name_len(request)) { ftp_error(reply, FTP_ERROR::InvalidDataSize); break; } @@ -627,11 +623,8 @@ int GCS_MAVLINK::gen_dir_entry(char *dest, size_t space, const char *path, const void GCS_MAVLINK::ftp_list_dir(struct pending_ftp &request, struct pending_ftp &response) { response.offset = request.offset; // this should be set for any failure condition for debugging - const size_t directory_name_size = strnlen((char *)request.data, sizeof(request.data)); - const bool is_req_size_consider_tnull = ((request.size - directory_name_size == 1) && - request.data[sizeof(request.data) - 1] == 0); // sanity check that our the request looks well formed - if (((directory_name_size != request.size) && !is_req_size_consider_tnull) || (request.size == 0)) { + if (!ftp_check_name_len(request)) { ftp_error(response, FTP_ERROR::InvalidDataSize); return; }