diff --git a/CMakeLists.txt b/CMakeLists.txt index 016e147629..1edc1e5574 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,7 @@ option(BUILD_WITH_PROTO_REFLECTION "Build mavsdk_server with proto reflection" O option(BUILD_SHARED_LIBS "Build core as shared libraries instead of static ones" ON) option(MAVLINK_DIALECT "MAVLink dialect. Default: common" "common") option(BUILD_TESTING "Build tests" ON) +option(BUILD_FUZZ_TESTS "Build fuzz tests with libfuzzer" OFF) option(BUILD_WITHOUT_CURL "Build without CURL" OFF) message("Using cmake version: ${CMAKE_VERSION}") diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c2baa256ee..f38be5b35f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -85,6 +85,10 @@ if(BUILD_TESTING) add_subdirectory(system_tests) endif() +if(BUILD_FUZZ_TESTS) + add_subdirectory(fuzz_tests) +endif() + if (BUILD_MAVSDK_SERVER) message(STATUS "Building mavsdk server") add_subdirectory(mavsdk_server) diff --git a/src/fuzz_tests/CMakeLists.txt b/src/fuzz_tests/CMakeLists.txt new file mode 100644 index 0000000000..2515568e75 --- /dev/null +++ b/src/fuzz_tests/CMakeLists.txt @@ -0,0 +1,50 @@ +# Check if fuzzing is enabled and compiler supports libfuzzer +if(NOT BUILD_FUZZ_TESTS) + return() +endif() + +# Check if we're using Clang (required for libfuzzer) +if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + message(WARNING "Fuzz tests require Clang compiler. Skipping fuzz tests.") + return() +endif() + +# Create the UDP fuzzer executable +add_executable(udp_fuzzer + udp_fuzzer.cpp +) + +# Set fuzzer-specific compiler and linker flags +target_compile_options(udp_fuzzer PRIVATE + -fsanitize=fuzzer,address + -g + -O1 +) + +target_link_options(udp_fuzzer PRIVATE + -fsanitize=fuzzer,address +) + +# Link against MAVSDK +target_link_libraries(udp_fuzzer + mavsdk +) + +# Include MAVSDK headers +target_include_directories(udp_fuzzer PRIVATE + ${PROJECT_SOURCE_DIR}/mavsdk/core/include + ${PROJECT_SOURCE_DIR}/mavsdk/core +) + +# Add custom target for running the fuzzer +add_custom_target(run_udp_fuzzer + COMMAND ${CMAKE_CURRENT_BINARY_DIR}/udp_fuzzer -max_total_time=60 -print_final_stats=1 + DEPENDS udp_fuzzer + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + COMMENT "Running UDP fuzzer for 60 seconds" +) + +# Print instructions +message(STATUS "Fuzz tests enabled. Build target 'udp_fuzzer' to create fuzzer executable.") +message(STATUS "Run 'make run_udp_fuzzer' to execute fuzzing for 60 seconds.") +message(STATUS "Or run manually: ./udp_fuzzer -help for options") diff --git a/src/fuzz_tests/udp_fuzzer.cpp b/src/fuzz_tests/udp_fuzzer.cpp new file mode 100644 index 0000000000..693bcb05f5 --- /dev/null +++ b/src/fuzz_tests/udp_fuzzer.cpp @@ -0,0 +1,452 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "mavlink_include.h" +#include "../mavsdk/core/log.h" + +// Include all client plugins (not server plugins) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace mavsdk; + +// RAII socket wrapper +class SocketWrapper { +public: + SocketWrapper() : fd_(-1) {} + + ~SocketWrapper() + { + if (fd_ >= 0) { + close(fd_); + } + } + + bool create() + { + fd_ = socket(AF_INET, SOCK_DGRAM, 0); + return fd_ >= 0; + } + + int get() const { return fd_; } + + // Non-copyable + SocketWrapper(const SocketWrapper&) = delete; + SocketWrapper& operator=(const SocketWrapper&) = delete; + +private: + int fd_; +}; + +// Plugin instances per system to maximize message processing coverage +struct SystemPlugins { + std::shared_ptr system; + std::unique_ptr action; + std::unique_ptr calibration; + std::unique_ptr camera; + std::unique_ptr component_metadata; + std::unique_ptr events; + std::unique_ptr failure; + std::unique_ptr follow_me; + std::unique_ptr ftp; + std::unique_ptr geofence; + std::unique_ptr gimbal; + std::unique_ptr gripper; + std::unique_ptr info; + std::unique_ptr log_files; + std::unique_ptr log_streaming; + std::unique_ptr manual_control; + std::unique_ptr mavlink_direct; + std::unique_ptr mission; + std::unique_ptr mission_raw; + std::unique_ptr mocap; + std::unique_ptr offboard; + std::unique_ptr param; + std::unique_ptr rtk; + std::unique_ptr shell; + std::unique_ptr telemetry; + std::unique_ptr transponder; + std::unique_ptr tune; + std::unique_ptr winch; + std::unique_ptr mavlink_passthrough; +}; + +// Main fuzzer class that manages MAVSDK connection and plugin instances +class MavsdkFuzzer { +public: + static constexpr int FUZZ_PORT = 14999; + + MavsdkFuzzer() = default; + ~MavsdkFuzzer() = default; + + // Non-copyable, non-movable + MavsdkFuzzer(const MavsdkFuzzer&) = delete; + MavsdkFuzzer& operator=(const MavsdkFuzzer&) = delete; + MavsdkFuzzer(MavsdkFuzzer&&) = delete; + MavsdkFuzzer& operator=(MavsdkFuzzer&&) = delete; + + bool initialize(); + void process_fuzz_input(const uint8_t* data, size_t size); + +private: + void send_heartbeat(); + void create_plugins_for_system(std::shared_ptr system); + size_t process_fuzz_data_to_mavlink( + const uint8_t* fuzz_data, + size_t fuzz_size, + uint8_t* output_buffer, + size_t output_buffer_size); + + std::unique_ptr mavsdk_; + std::unique_ptr socket_; + std::vector> system_plugins_; + struct sockaddr_in dest_addr_{}; + std::chrono::steady_clock::time_point last_heartbeat_{std::chrono::steady_clock::now()}; + bool initialized_{false}; +}; + +void MavsdkFuzzer::send_heartbeat() +{ + mavlink_message_t heartbeat_msg; + mavlink_msg_heartbeat_pack( + 1, + 1, + &heartbeat_msg, // system_id=1, component_id=1 + MAV_TYPE_QUADROTOR, // type + MAV_AUTOPILOT_PX4, // autopilot + 0, // base_mode + 0, // custom_mode + MAV_STATE_STANDBY // system_status + ); + + uint8_t heartbeat_buffer[MAVLINK_MAX_PACKET_LEN]; + size_t heartbeat_size = mavlink_msg_to_send_buffer(heartbeat_buffer, &heartbeat_msg); + + sendto( + socket_->get(), + heartbeat_buffer, + heartbeat_size, + 0, + reinterpret_cast(&dest_addr_), + sizeof(dest_addr_)); +} + +void MavsdkFuzzer::create_plugins_for_system(std::shared_ptr system) +{ + LogDebug() << "Creating plugins for system ID: " << system->get_system_id(); + + auto plugins = std::make_unique(); + plugins->system = system; + + // Create all client plugin instances for this system + // Each plugin registers message handlers, so instantiating them ensures + // fuzzed messages get processed by all relevant code paths + plugins->action = std::make_unique(system); + plugins->calibration = std::make_unique(system); + plugins->camera = std::make_unique(system); + plugins->component_metadata = std::make_unique(system); + plugins->events = std::make_unique(system); + plugins->failure = std::make_unique(system); + plugins->follow_me = std::make_unique(system); + plugins->ftp = std::make_unique(system); + plugins->geofence = std::make_unique(system); + plugins->gimbal = std::make_unique(system); + plugins->gripper = std::make_unique(system); + plugins->info = std::make_unique(system); + plugins->log_files = std::make_unique(system); + plugins->log_streaming = std::make_unique(system); + plugins->manual_control = std::make_unique(system); + plugins->mavlink_direct = std::make_unique(system); + plugins->mission = std::make_unique(system); + plugins->mission_raw = std::make_unique(system); + plugins->mocap = std::make_unique(system); + plugins->offboard = std::make_unique(system); + plugins->param = std::make_unique(system); + plugins->rtk = std::make_unique(system); + plugins->shell = std::make_unique(system); + plugins->telemetry = std::make_unique(system); + plugins->transponder = std::make_unique(system); + plugins->tune = std::make_unique(system); + plugins->winch = std::make_unique(system); + plugins->mavlink_passthrough = std::make_unique(system); + + system_plugins_.push_back(std::move(plugins)); + + LogDebug() << "Successfully created " << system_plugins_.size() + << " plugin sets, latest for system ID: " << system->get_system_id(); +} + +bool MavsdkFuzzer::initialize() +{ + if (initialized_) { + return true; + } + + // Create MAVSDK instance + mavsdk_ = std::make_unique(Mavsdk::Configuration{ComponentType::GroundStation}); + + // Add UDP connection that listens on our fuzz port + std::string connection_url = "udpin://127.0.0.1:" + std::to_string(FUZZ_PORT); + ConnectionResult result = mavsdk_->add_any_connection(connection_url); + + if (result != ConnectionResult::Success) { + mavsdk_.reset(); + return false; + } + + // Create socket for sending fuzzed data + socket_ = std::make_unique(); + if (!socket_->create()) { + mavsdk_.reset(); + socket_.reset(); + return false; + } + + // Subscribe to new systems and create plugins for each one dynamically + mavsdk_->subscribe_on_new_system([this]() { + LogDebug() << "New system callback triggered!"; + // A new system appeared, check for any systems we haven't seen yet + auto all_systems = mavsdk_->systems(); + LogDebug() << "Found " << all_systems.size() << " systems"; + for (auto system : all_systems) { + // Check if we already have plugins for this system + bool found = false; + for (const auto& existing_plugins : system_plugins_) { + if (existing_plugins->system == system) { + found = true; + break; + } + } + if (!found) { + LogDebug() << "System not found in plugin list, creating plugins..."; + create_plugins_for_system(system); + } else { + LogDebug() << "System already has plugins created"; + } + } + }); + + // Also handle any systems that might already exist + auto existing_systems = mavsdk_->systems(); + LogDebug() << "Checking for existing systems, found: " << existing_systems.size(); + for (auto system : existing_systems) { + LogDebug() << "Creating plugins for existing system ID: " << system->get_system_id(); + create_plugins_for_system(system); + } + + // Set up destination address for all messages + dest_addr_.sin_family = AF_INET; + dest_addr_.sin_port = htons(FUZZ_PORT); + inet_pton(AF_INET, "127.0.0.1", &dest_addr_.sin_addr); + + // Send initial heartbeat to trigger system discovery + LogDebug() << "Sending initial heartbeat to trigger system discovery"; + send_heartbeat(); + + // Give MAVSDK time to process the heartbeat and discover the system + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // Check again for systems after the heartbeat + auto post_heartbeat_systems = mavsdk_->systems(); + LogDebug() << "After heartbeat, checking for systems, found: " << post_heartbeat_systems.size(); + for (auto system : post_heartbeat_systems) { + // Check if we already have plugins for this system + bool found = false; + for (const auto& existing_plugins : system_plugins_) { + if (existing_plugins->system == system) { + found = true; + break; + } + } + if (!found) { + LogDebug() << "Creating plugins for post-heartbeat system ID: " + << system->get_system_id(); + create_plugins_for_system(system); + } else { + LogDebug() << "Post-heartbeat system already has plugins"; + } + } + + initialized_ = true; + return true; +} + +size_t MavsdkFuzzer::process_fuzz_data_to_mavlink( + const uint8_t* fuzz_data, size_t fuzz_size, uint8_t* output_buffer, size_t output_buffer_size) +{ + size_t total_bytes_written = 0; + size_t offset = 0; + + // Process fuzz data in chunks, creating MAVLink messages + while (offset < fuzz_size && + (output_buffer_size - total_bytes_written) >= MAVLINK_MAX_PACKET_LEN) { + // Determine how much data to use for this message + size_t chunk_size = + std::min(static_cast(255), fuzz_size - offset); // MAVLink max payload is 255 + + if (chunk_size < 3) { + break; // Need minimum data for a meaningful message (msg_id + seq + some payload) + } + + // Create a MAVLink message structure from the fuzz data + mavlink_message_t msg; + + // Use fixed system ID and component ID for better plugin message routing + uint8_t system_id = 1; // Fixed to ensure consistent message routing + uint8_t component_id = 1; // Fixed to ensure consistent message routing + uint8_t msg_id = fuzz_data[offset + 2]; + + // Fill the message with fuzz data + msg.msgid = msg_id; + msg.sysid = system_id; + msg.compid = component_id; + msg.seq = fuzz_data[offset + 1]; // Use second byte for sequence + + // Copy payload data (up to 255 bytes) + size_t payload_size = + std::min(chunk_size - 3, static_cast(255)); // -3 for seq, msg_id, and one more + msg.len = payload_size; + + if (payload_size > 0 && offset + 3 + payload_size <= fuzz_size) { + memcpy(msg.payload64, fuzz_data + offset + 3, payload_size); + } else { + memset(msg.payload64, 0, sizeof(msg.payload64)); + msg.len = 0; + } + + // The key insight from PX4: let MAVLink calculate the correct checksum + // This ensures the message passes basic validation and reaches deeper processing + msg.checksum = 0; // Will be calculated by mavlink_finalize_message + + // Finalize the message (calculates correct checksum) + // For fuzzing, we don't know the actual min_length and crc_extra, + // so we use generic values that will still create a valid checksum + mavlink_finalize_message(&msg, system_id, component_id, msg.len, msg.len, 0); + + // Convert to send buffer + size_t message_length = + mavlink_msg_to_send_buffer(output_buffer + total_bytes_written, &msg); + total_bytes_written += message_length; + + // Move to next chunk + offset += chunk_size; + } + + return total_bytes_written; +} + +void MavsdkFuzzer::process_fuzz_input(const uint8_t* data, size_t size) +{ + // Need minimum data to create meaningful messages + if (size < 3 || size > 1024) { + return; + } + + if (!initialized_ || !mavsdk_ || !socket_) { + return; + } + + // Send heartbeat every second to keep the system alive + auto now = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast(now - last_heartbeat_).count() >= + 1000) { + send_heartbeat(); + last_heartbeat_ = now; + } + + // Process fuzz data into MAVLink messages with correct checksums + uint8_t mavlink_buffer[2048]; // Allow for multiple messages + size_t total_mavlink_size = + process_fuzz_data_to_mavlink(data, size, mavlink_buffer, sizeof(mavlink_buffer)); + + if (total_mavlink_size == 0) { + return; + } + + // Send all generated MAVLink messages as UDP packets to MAVSDK + size_t offset = 0; + while (offset < total_mavlink_size) { + // Parse each individual message from the buffer to get its length + if (offset + 1 >= total_mavlink_size) + break; + + uint8_t msg_len = mavlink_buffer[offset + 1]; // MAVLink v2 length field + size_t full_msg_size = msg_len + 12; // MAVLink v2 overhead + + if (offset + full_msg_size > total_mavlink_size) { + break; // Incomplete message + } + + // Send this individual message + sendto( + socket_->get(), + mavlink_buffer + offset, + full_msg_size, + 0, + reinterpret_cast(&dest_addr_), + sizeof(dest_addr_)); + + offset += full_msg_size; + } +} + +// Global fuzzer instance +static std::unique_ptr g_fuzzer; + +extern "C" int LLVMFuzzerTestOneInput(const uint8_t* data, size_t size) +{ + // Initialize on first run + if (!g_fuzzer) { + g_fuzzer = std::make_unique(); + if (!g_fuzzer->initialize()) { + g_fuzzer.reset(); + return 0; + } + } + + // Process the fuzz input + g_fuzzer->process_fuzz_input(data, size); + return 0; +} + +// Cleanup function (called by libfuzzer on exit) +extern "C" void LLVMFuzzerFinalize() +{ + // Clean up the fuzzer instance + g_fuzzer.reset(); +} \ No newline at end of file