From ea14b53527e51a051dc83a4ed0b1e8cb213c0ec8 Mon Sep 17 00:00:00 2001 From: Christopher Krah Date: Mon, 20 May 2024 16:08:30 +0400 Subject: [PATCH] feat: add initial fuzzing harness --- fuzz/CMakeLists.txt | 70 ++++++++++++++++++ fuzz/FINDINGS.md | 146 ++++++++++++++++++++++++++++++++++++++ fuzz/Makefile | 18 +++++ fuzz/README.md | 22 ++++++ fuzz/harness.cc | 168 ++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 424 insertions(+) create mode 100644 fuzz/CMakeLists.txt create mode 100644 fuzz/FINDINGS.md create mode 100644 fuzz/Makefile create mode 100644 fuzz/README.md create mode 100644 fuzz/harness.cc diff --git a/fuzz/CMakeLists.txt b/fuzz/CMakeLists.txt new file mode 100644 index 00000000..44a5c0f8 --- /dev/null +++ b/fuzz/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.9.5) +project(rplidar_fuzz) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif () + +message(STATUS "CXX Compiler: ${CMAKE_CXX_COMPILER}") +message(STATUS "CXX Flags before: ${CMAKE_CXX_FLAGS}") + +if (CMAKE_CXX_COMPILER) + # Check if the CXX compiler is afl-clang or afl-clang++ + if (CMAKE_CXX_COMPILER MATCHES "afl-clang-fast") + message(STATUS "Using AFL Clang compiler") + # Set flags for AFL + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=fuzzer,address,undefined -ggdb -O2 -fno-omit-frame-pointer") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address,undefined -ggdb -O2") + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -fsanitize=address,undefined -ggdb -O2") + else() + message(STATUS "Using regular Clang/GCC compiler") + # Enable fuzzer and address sanitizers for other compilers + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=fuzzer,address,undefined -ggdb -O2") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=fuzzer,address,undefined -ggdb -O2") + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -fsanitize=fuzzer,address,undefined -ggdb -O2") + endif() + + message(STATUS "CXX Flags after: ${CMAKE_CXX_FLAGS}") + + # Include directories for the SDK library + include_directories( + ../sdk/include + ../sdk/src + ../sdk/src/arch/linux + ../sdk/src/hal) + + # Create instrumented SDK library + add_library(rplidar_sdk_instr SHARED + ../sdk/src/rplidar_driver.cpp + ../sdk/src/arch/linux/net_serial.cpp + ../sdk/src/arch/linux/net_socket.cpp + ../sdk/src/arch/linux/timer.cpp + ../sdk/src/hal/thread.cpp) + + target_include_directories(rplidar_sdk_instr + PUBLIC + ../sdk/include + ../sdk/src + PRIVATE + ../sdk/src/arch/linux + ../sdk/src/hal) + + # Add the fuzz test executable + add_executable(harness harness.cc) + + # Link against the instrumented rplidar_sdk library + target_link_libraries(harness rplidar_sdk_instr) + + # Set the RPATH to find the SDK library + set_target_properties(harness PROPERTIES INSTALL_RPATH "\$ORIGIN") + + # Set the output directory for the instrumented SDK library + set_target_properties(rplidar_sdk_instr PROPERTIES + LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") + + # Set the output directory for the fuzz test executable + set_target_properties(harness PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") +else() + message(FATAL_ERROR "CMAKE_CXX is not set. Please specify the CXX compiler.") +endif () diff --git a/fuzz/FINDINGS.md b/fuzz/FINDINGS.md new file mode 100644 index 00000000..6b23138a --- /dev/null +++ b/fuzz/FINDINGS.md @@ -0,0 +1,146 @@ +# Findings + +Some initial findings that were triggered when fuzzing the SDK for a short while. + +## 1. Multiple memory Leaks + +``` +INFO: Running with entropic power schedule (0xFF, 100). +INFO: Seed: 2732911167 +INFO: Loaded 2 modules (3982 inline 8-bit counters): 3655 [0x7e0b2a7ef058, 0x7e0b2a7efe9f), 327 [0x6482bb2b05d8, 0x6482bb2b071f), +INFO: Loaded 2 PC tables (3982 PCs): 3655 [0x7e0b2a7efea0,0x7e0b2a7fe310), 327 [0x6482bb2b0720,0x6482bb2b1b90), +INFO: -max_len is not provided; libFuzzer will not generate inputs larger than 4096 bytes +INFO: A corpus is not provided, starting from an empty corpus +#2 INITED cov: 85 ft: 86 corp: 1/1b exec/s: 0 rss: 32Mb + +================================================================= +==685022==ERROR: LeakSanitizer: detected memory leaks + +Direct leak of 24 byte(s) in 1 object(s) allocated from: + #0 0x6482bb2633e1 in operator new(unsigned long) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x1423e1) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #1 0x7e0b2a7bd358 in rp::standalone::rplidar::RPlidarDriverSerial::RPlidarDriverSerial() /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver.cpp:2181:16 + #2 0x7e0b2a79cebd in rp::standalone::rplidar::RPlidarDriver::CreateDriver(unsigned int) /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver.cpp:89:20 + #3 0x6482bb265959 in LLVMFuzzerTestOneInput /home/user/git/work/rpilidar_ros2_clean/fuzz/harness.cc:38:14 + #4 0x6482bb170cb4 in fuzzer::Fuzzer::ExecuteCallback(unsigned char const*, unsigned long) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x4fcb4) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #5 0x6482bb1703a9 in fuzzer::Fuzzer::RunOne(unsigned char const*, unsigned long, bool, fuzzer::InputInfo*, bool, bool*) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x4f3a9) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #6 0x6482bb172036 in fuzzer::Fuzzer::ReadAndExecuteSeedCorpora(std::vector>&) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x51036) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #7 0x6482bb1724d7 in fuzzer::Fuzzer::Loop(std::vector>&) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x514d7) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #8 0x6482bb15f9cf in fuzzer::FuzzerDriver(int*, char***, int (*)(unsigned char const*, unsigned long)) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x3e9cf) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #9 0x6482bb18a056 in main (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x69056) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #10 0x7e0b2a42a1c9 in __libc_start_call_main csu/../sysdeps/nptl/libc_start_call_main.h:58:16 + #11 0x7e0b2a42a28a in __libc_start_main csu/../csu/libc-start.c:360:3 + #12 0x6482bb1549b4 in _start (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x339b4) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + +Direct leak of 24 byte(s) in 1 object(s) allocated from: + #0 0x6482bb2633e1 in operator new(unsigned long) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x1423e1) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #1 0x7e0b2a7bd358 in rp::standalone::rplidar::RPlidarDriverSerial::RPlidarDriverSerial() /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver.cpp:2181:16 + #2 0x7e0b2a79cebd in rp::standalone::rplidar::RPlidarDriver::CreateDriver(unsigned int) /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver.cpp:89:20 + #3 0x6482bb265959 in LLVMFuzzerTestOneInput /home/user/git/work/rpilidar_ros2_clean/fuzz/harness.cc:38:14 + #4 0x6482bb170cb4 in fuzzer::Fuzzer::ExecuteCallback(unsigned char const*, unsigned long) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x4fcb4) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #5 0x6482bb1703a9 in fuzzer::Fuzzer::RunOne(unsigned char const*, unsigned long, bool, fuzzer::InputInfo*, bool, bool*) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x4f3a9) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #6 0x6482bb171b95 in fuzzer::Fuzzer::MutateAndTestOne() (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x50b95) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #7 0x6482bb1726f5 in fuzzer::Fuzzer::Loop(std::vector>&) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x516f5) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #8 0x6482bb15f9cf in fuzzer::FuzzerDriver(int*, char***, int (*)(unsigned char const*, unsigned long)) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x3e9cf) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #9 0x6482bb18a056 in main (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x69056) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #10 0x7e0b2a42a1c9 in __libc_start_call_main csu/../sysdeps/nptl/libc_start_call_main.h:58:16 + #11 0x7e0b2a42a28a in __libc_start_main csu/../csu/libc-start.c:360:3 + #12 0x6482bb1549b4 in _start (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x339b4) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + +Direct leak of 24 byte(s) in 1 object(s) allocated from: + #0 0x6482bb2633e1 in operator new(unsigned long) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x1423e1) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #1 0x7e0b2a7bd358 in rp::standalone::rplidar::RPlidarDriverSerial::RPlidarDriverSerial() /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver.cpp:2181:16 + #2 0x7e0b2a79cebd in rp::standalone::rplidar::RPlidarDriver::CreateDriver(unsigned int) /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver.cpp:89:20 + #3 0x6482bb265959 in LLVMFuzzerTestOneInput /home/user/git/work/rpilidar_ros2_clean/fuzz/harness.cc:38:14 + #4 0x6482bb170cb4 in fuzzer::Fuzzer::ExecuteCallback(unsigned char const*, unsigned long) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x4fcb4) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #5 0x6482bb171ee1 in fuzzer::Fuzzer::ReadAndExecuteSeedCorpora(std::vector>&) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x50ee1) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #6 0x6482bb1724d7 in fuzzer::Fuzzer::Loop(std::vector>&) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x514d7) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #7 0x6482bb15f9cf in fuzzer::FuzzerDriver(int*, char***, int (*)(unsigned char const*, unsigned long)) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x3e9cf) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #8 0x6482bb18a056 in main (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x69056) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #9 0x7e0b2a42a1c9 in __libc_start_call_main csu/../sysdeps/nptl/libc_start_call_main.h:58:16 + #10 0x7e0b2a42a28a in __libc_start_main csu/../csu/libc-start.c:360:3 + #11 0x6482bb1549b4 in _start (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x339b4) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + +SUMMARY: AddressSanitizer: 72 byte(s) leaked in 3 allocation(s). +INFO: to ignore leaks on libFuzzer side use -detect_leaks=0. +``` + +### Possible fix + +It seems to help to extend the destructor of the objects with a proper delete operation, similar to: + +```cc +RPlidarDriverSerial::~RPlidarDriverSerial() { + // force disconnection + disconnect(); + + _chanDev->close(); + _chanDev->ReleaseRxTx(); + + // NOTE: prevent memory leak + delete _chanDev; + _chanDev = nullptr; +} +``` + +_Note_: This also needs to be done for the `RPlidarDriverTCP::~RPlidarDriverTCP()` + +## 2. NULL-PTR access + +``` +#1887 NEW cov: 265 ft: 288 corp: 14/107b lim: 21 exec/s: 0 rss: 105Mb L: 18/21 MS: 2 ChangeBit-CrossOver- +/home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver_TCP.h:48:16: runtime error: member call on null pointer of type 'rp::net::StreamSocket' +SUMMARY: UndefinedBehaviorSanitizer: undefined-behavior /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver_TCP.h:48:16 +AddressSanitizer:DEADLYSIGNAL +================================================================= +==688768==ERROR: AddressSanitizer: SEGV on unknown address 0x000000000000 (pc 0x7532a0b5440f bp 0x7fff999e3af0 sp 0x7fff999e3a60 T0) +==688768==The signal is caused by a READ memory access. +==688768==Hint: address points to the zero page. + #0 0x7532a0b5440f in rp::standalone::rplidar::TCPChannelDevice::bind(char const*, unsigned int) /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver_TCP.h:48:16 + #1 0x7532a0b5251b in rp::standalone::rplidar::RPlidarDriverTCP::connect(char const*, unsigned int, unsigned int) /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver.cpp:2251:23 + #2 0x595daaac4017 in LLVMFuzzerTestOneInput /home/user/git/work/rpilidar_ros2_clean/fuzz/harness.cc:60:12 + #3 0x595daa9cecb4 in fuzzer::Fuzzer::ExecuteCallback(unsigned char const*, unsigned long) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x4fcb4) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #4 0x595daa9ce3a9 in fuzzer::Fuzzer::RunOne(unsigned char const*, unsigned long, bool, fuzzer::InputInfo*, bool, bool*) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x4f3a9) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #5 0x595daa9cfb95 in fuzzer::Fuzzer::MutateAndTestOne() (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x50b95) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #6 0x595daa9d06f5 in fuzzer::Fuzzer::Loop(std::vector>&) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x516f5) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #7 0x595daa9bd9cf in fuzzer::FuzzerDriver(int*, char***, int (*)(unsigned char const*, unsigned long)) (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x3e9cf) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #8 0x595daa9e8056 in main (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x69056) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + #9 0x7532a042a1c9 in __libc_start_call_main csu/../sysdeps/nptl/libc_start_call_main.h:58:16 + #10 0x7532a042a28a in __libc_start_main csu/../csu/libc-start.c:360:3 + #11 0x595daa9b29b4 in _start (/home/user/git/work/rpilidar_ros2_clean/fuzz/build/harness+0x339b4) (BuildId: 7c06b0c5fb4572cd2125cd2971d87e1a2e416f2f) + +AddressSanitizer can not provide additional info. +SUMMARY: AddressSanitizer: SEGV /home/user/git/work/rpilidar_ros2_clean/sdk/src/rplidar_driver_TCP.h:48:16 in rp::standalone::rplidar::TCPChannelDevice::bind(char const*, unsigned int) +==688768==ABORTING +MS: 5 ChangeBinInt-ChangeBinInt-CopyPart-EraseBytes-ChangeByte-; base unit: 194f4b33f5c6e20eeab91e4f162f8f5bd27143c9 +0xfd,0x0,0x0,0x2,0x2,0xe5, +\375\000\000\002\002\345 +artifact_prefix='./'; Test unit written to ./crash-ce81a181206832f1dc742353c3e3c8ed137bcf1e +Base64: /QAAAgLl +``` + +The root-cause here seems to be that if we call `drv.connect()` for a `DRIVER_TYPE_TCP` like so: + +```cc +drv->connect(ip.c_str(), port); +``` + +on the condition that we did _not_ check _\_bindet_socket_ for NULL we can crash the process. + +### Possible fix: + +```cc + TCPChannelDevice() : _binded_socket(rp::net::StreamSocket::CreateSocket()) { + // NOTE: Include a sanity NULLPTR check + if (_binded_socket == nullptr) { + std::cerr << "Failed to create socket" << std::endl; + } + } + bool bind(const char *ipStr, uint32_t port) { + if (_binded_socket == nullptr) { + std::cerr << "Binded socket is null" << std::endl; + return false; + } + rp::net::SocketAddress socket(ipStr, port); + return IS_OK(_binded_socket->connect(socket)); + } +``` diff --git a/fuzz/Makefile b/fuzz/Makefile new file mode 100644 index 00000000..4159a66a --- /dev/null +++ b/fuzz/Makefile @@ -0,0 +1,18 @@ +BUILD_DIR := build + +.PHONY: all clean fuzz afl + +all: fuzz + +clean: + rm -rf $(BUILD_DIR) + +fuzz: clean + mkdir -p $(BUILD_DIR) + cd $(BUILD_DIR) && CC=clang CXX=clang++ cmake .. && make + mv $(BUILD_DIR)/harness harness + +afl: clean + mkdir -p $(BUILD_DIR) + cd $(BUILD_DIR) && CC=afl-clang-fast CXX=afl-clang-fast++ cmake .. && make + mv $(BUILD_DIR)/harness harness_afl diff --git a/fuzz/README.md b/fuzz/README.md new file mode 100644 index 00000000..10143dff --- /dev/null +++ b/fuzz/README.md @@ -0,0 +1,22 @@ +# Fuzzing setup + +There's an `AFL++` and a `libfuzzer` version for building. + +## Requirements + +- AFL++ +- cmake +- make +- clang +- (prometheus-cpp) + +## How to + +```bash +cd fuzz/ +# Running `make` will produce a ./harness binary that can be run as is +make +# Alternatively: +make afl +# This will produce a ./harness_afl that can be executed the AFL++ way +``` diff --git a/fuzz/harness.cc b/fuzz/harness.cc new file mode 100644 index 00000000..ac3dceb3 --- /dev/null +++ b/fuzz/harness.cc @@ -0,0 +1,168 @@ +#include "sdkcommon.h" + +#include "hal/abs_rxtx.h" +#include "hal/event.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "rplidar_cmd.h" +#include "rplidar_driver.h" +#include "sdkcommon.h" + +// Is needed for `std::unique_ptr` +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#define MAX_ALLOC 1024 * 1000 + +// extern "C" int LLVMFuzzerInitialize(int *argc, char ***argv) { return 0; } + +extern "C" int LLVMFuzzerTestOneInput(const uint8_t *data, size_t size) { + FuzzedDataProvider fdp(data, size); + + // Create an instance of the RPlidarDriver + auto driverType = fdp.ConsumeBool() + ? rp::standalone::rplidar::DRIVER_TYPE_TCP + : rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT; + auto drv = rp::standalone::rplidar::RPlidarDriver::CreateDriver(driverType); + + if (!drv) { + return 0; + } + + // Connect to the RPLIDAR device + if (fdp.remaining_bytes() >= 2) { + if (driverType == rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT) { + std::string portPath = fdp.ConsumeRandomLengthString(256); + uint32_t baudrate = fdp.ConsumeIntegral(); + drv->connect(portPath.c_str(), baudrate); + } else { + std::string ip = fdp.ConsumeRandomLengthString(15); + uint32_t port = fdp.ConsumeIntegral(); + + // Check if _binded_socket is null before calling connect + // if (static_cast(drv) + // ->_channel._binded_socket == nullptr) { + // return -1; + //} + + drv->connect(ip.c_str(), port); + } + } + // Check if the device is connected + drv->isConnected(); + + // Reset the device + if (fdp.ConsumeBool()) { + drv->reset(); + } + + // Get all supported scan modes + if (fdp.ConsumeBool()) { + std::vector outModes; + drv->getAllSupportedScanModes(outModes); + } + + // Get typical scan mode + if (fdp.ConsumeBool()) { + uint16_t outMode; + drv->getTypicalScanMode(outMode); + } + + // Start scan + if (fdp.ConsumeBool()) { + bool force = fdp.ConsumeBool(); + bool useTypicalScan = fdp.ConsumeBool(); + rp::standalone::rplidar::RplidarScanMode outUsedScanMode; + drv->startScan(force, useTypicalScan, 0, &outUsedScanMode); + } + + // Start scan express + if (fdp.ConsumeBool()) { + bool force = fdp.ConsumeBool(); + uint16_t scanMode = fdp.ConsumeIntegral(); + rp::standalone::rplidar::RplidarScanMode outUsedScanMode; + drv->startScanExpress(force, scanMode, 0, &outUsedScanMode); + } + + // Get health status + if (fdp.ConsumeBool()) { + rplidar_response_device_health_t health; + drv->getHealth(health); + } + + // Get device info + if (fdp.ConsumeBool()) { + rplidar_response_device_info_t info; + drv->getDeviceInfo(info); + } + + // Set motor PWM + if (fdp.ConsumeBool()) { + uint16_t pwm = fdp.ConsumeIntegral(); + drv->setMotorPWM(pwm); + } + + // Start motor + if (fdp.ConsumeBool()) { + drv->startMotor(); + } + + // Stop motor + if (fdp.ConsumeBool()) { + drv->stopMotor(); + } + + // Check motor control support + if (fdp.ConsumeBool()) { + bool support; + drv->checkMotorCtrlSupport(support); + } + + // Get scan data + if (fdp.ConsumeBool()) { + // FIXME: Gotta wait for https://github.com/google/sanitizers/issues/1720 + // size_t count = fdp.ConsumeIntegral(); + size_t count = fdp.ConsumeIntegralInRange(0, MAX_ALLOC); + + auto nodebuffer = std::unique_ptr( + new rplidar_response_measurement_node_hq_t[count]); + auto timeout = static_cast(fdp.ConsumeIntegral()); + drv->grabScanDataHq(nodebuffer.get(), count, timeout); + } + + // Ascend scan data + if (fdp.ConsumeBool()) { + // FIXME: same as above + // size_t count = fdp.ConsumeIntegral(); + size_t count = fdp.ConsumeIntegralInRange(0, MAX_ALLOC); + auto nodebuffer = std::unique_ptr( + new rplidar_response_measurement_node_hq_t[count]); + drv->ascendScanData(nodebuffer.get(), count); + } + + // Get scan data with interval + if (fdp.ConsumeBool()) { + // FIXME: same as above + // size_t count = fdp.ConsumeIntegral(); + size_t count = fdp.ConsumeIntegralInRange(0, MAX_ALLOC); + auto nodebuffer = std::unique_ptr( + new rplidar_response_measurement_node_hq_t[count]); + drv->getScanDataWithIntervalHq(nodebuffer.get(), count); + } + // Dispose of the driver + rp::standalone::rplidar::RPlidarDriver::DisposeDriver(drv); + + return 0; +}