From bd05fad850c1ad2b3a857623919ff5adc7bc5c25 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 14 Nov 2023 14:06:05 +1100
Subject: [PATCH] SITL: add code to add, remove or corrupt bytes in
 SITL::SerialDevice xfrs

---
 libraries/SITL/SIM_SerialDevice.cpp | 61 +++++++++++++++++++++++------
 libraries/SITL/SIM_SerialDevice.h   |  2 +
 libraries/SITL/SIM_config.h         |  4 ++
 3 files changed, 56 insertions(+), 11 deletions(-)

diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp
index 64eb1edb87..19065b7152 100644
--- a/libraries/SITL/SIM_SerialDevice.cpp
+++ b/libraries/SITL/SIM_SerialDevice.cpp
@@ -44,9 +44,45 @@ bool SerialDevice::init_sitl_pointer()
     return true;
 }
 
+#if AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED
+ssize_t SerialDevice::corrupt_transfer(char *buffer, const ssize_t ret, const size_t size) const
+{
+    if (ret > 0 && (rand() % 100) < 2) {
+        // drop a random byte from returned data:
+        const size_t byte_ofs_to_drop = rand() % ret;
+        fprintf(stderr, "dropping byte at offset %u\n", unsigned(byte_ofs_to_drop));
+        memmove(&buffer[byte_ofs_to_drop], &buffer[byte_ofs_to_drop+1], ret - byte_ofs_to_drop - 1);
+        return ret - 1;
+    }
+
+    if (ret > 0 && size_t(ret) < size && (rand() % 100) < 2) {
+        // add a random byte to the stream:
+        const size_t byte_ofs_to_add = rand() % ret;
+        fprintf(stderr, "adding byte at offset %u\n", unsigned(byte_ofs_to_add));
+        memmove(&buffer[byte_ofs_to_add+1], &buffer[byte_ofs_to_add], ret - byte_ofs_to_add);
+        buffer[byte_ofs_to_add] = rand()*256;
+        return ret + 1;
+    }
+
+    if (ret > 0 && unsigned(ret) < size && (rand() % 100) < 2) {
+        // corrupt a random byte in the stream:
+        const size_t byte_ofs_to_corrupt = rand() % ret;
+        fprintf(stderr, "corrupting byte at offset=%u\n", unsigned(byte_ofs_to_corrupt));
+        buffer[byte_ofs_to_corrupt] = rand()*256;
+        return ret;
+    }
+
+    return ret;
+}
+#endif
+
 ssize_t SerialDevice::read_from_autopilot(char *buffer, const size_t size) const
 {
-    const ssize_t ret = from_autopilot->read((uint8_t*)buffer, size);
+    ssize_t ret = from_autopilot->read((uint8_t*)buffer, size);
+#if AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED
+    ret = corrupt_transfer(buffer, ret, size);
+#endif
+
     // if (ret > 0) {
     //     ::fprintf(stderr, "SIM_SerialDevice: read from autopilot (%u): (", (unsigned)ret);
     //     for (ssize_t i=0; i<ret; i++) {
@@ -69,15 +105,6 @@ ssize_t SerialDevice::write_to_autopilot(const char *buffer, const size_t size)
     }
 
     const ssize_t ret = to_autopilot->write((uint8_t*)buffer, size);
-    // ::fprintf(stderr, "write to autopilot: (");
-    // for (ssize_t i=0; i<ret; i++) {
-    //     ::fprintf(stderr, "%02X", (uint8_t)buffer[i]);
-    // }
-    // ::fprintf(stderr, ") (\n");
-    // for (ssize_t i=0; i<ret; i++) {
-    //     ::fprintf(stderr, "%c", (uint8_t)buffer[i]);
-    // }
-    // ::fprintf(stderr, ")\n");
     return ret;
 }
 
@@ -87,7 +114,19 @@ ssize_t SerialDevice::read_from_device(char *buffer, const size_t size) const
         return -1;
     }
 
-    const ssize_t ret = to_autopilot->read((uint8_t*)buffer, size);
+    ssize_t ret = to_autopilot->read((uint8_t*)buffer, size);
+#if AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED
+    ret = corrupt_transfer(buffer, ret, size);
+#endif
+    // ::fprintf(stderr, "read_from_device: (");
+    // for (ssize_t i=0; i<ret; i++) {
+    //     ::fprintf(stderr, "%02X", (uint8_t)buffer[i]);
+    // }
+    // ::fprintf(stderr, ") (\n");
+    // for (ssize_t i=0; i<ret; i++) {
+    //     ::fprintf(stderr, "%c", (uint8_t)buffer[i]);
+    // }
+    // ::fprintf(stderr, ")\n");
     return ret;
 }
 ssize_t SerialDevice::write_to_device(const char *buffer, const size_t size) const
diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h
index 2a36497345..00f58ab935 100644
--- a/libraries/SITL/SIM_SerialDevice.h
+++ b/libraries/SITL/SIM_SerialDevice.h
@@ -53,6 +53,8 @@ private:
     bool is_match_baud(void) const;
 
     uint32_t autopilot_baud;
+
+    ssize_t corrupt_transfer(char *buffer, const ssize_t ret, const size_t size) const;
 };
 
 }
diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h
index abb5b8eea8..726b903276 100644
--- a/libraries/SITL/SIM_config.h
+++ b/libraries/SITL/SIM_config.h
@@ -33,3 +33,7 @@
 #ifndef AP_SIM_ADSB_SAGETECH_MXS_ENABLED
 #define AP_SIM_ADSB_SAGETECH_MXS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
+
+#ifndef AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED
+#define AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED 0
+#endif