DataFlash: add sanity check for WritePrioritisedBlock size

libraries/DataFlash/DataFlash.cpp

@@ -802,6 +802,30 @@ DataFlash_Class::log_write_fmt *DataFlash_Class::msg_fmt_for_name(const char *na
     return f;
 }
 
+const struct LogStructure *DataFlash_Class::structure_for_msg_type(const uint8_t msg_type)
+{
+    for (uint16_t i=0; i<_num_types;i++) {
+        const struct LogStructure *s = structure(i);
+        if (s->msg_type == msg_type) {
+            // in use
+            return s;
+        }
+    }
+    return nullptr;
+}
+
+const struct DataFlash_Class::log_write_fmt *DataFlash_Class::log_write_fmt_for_msg_type(const uint8_t msg_type) const
+{
+    struct log_write_fmt *f;
+    for (f = log_write_fmts; f; f=f->next) {
+        if (f->msg_type == msg_type) {
+            return f;
+        }
+    }
+    return nullptr;
+}
+
+
 // returns true if the msg_type is already taken
 bool DataFlash_Class::msg_type_in_use(const uint8_t msg_type) const
 {

libraries/DataFlash/DataFlash.h

@@ -272,10 +272,13 @@ private:
 
     // return (possibly allocating) a log_write_fmt for a name
     struct log_write_fmt *msg_fmt_for_name(const char *name, const char *labels, const char *units, const char *mults, const char *fmt);
+    const struct log_write_fmt *log_write_fmt_for_msg_type(uint8_t msg_type) const;
 
     // returns true if msg_type is associated with a message
     bool msg_type_in_use(uint8_t msg_type) const;
 
+    const struct LogStructure *structure_for_msg_type(uint8_t msg_type);
+
     // return a msg_type which is not currently in use (or -1 if none available)
     int16_t find_free_msg_type() const;
 

libraries/DataFlash/DataFlash_Backend.cpp

@@ -313,8 +313,54 @@ bool DataFlash_Backend::StartNewLogOK() const
     return true;
 }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+void DataFlash_Backend::validate_WritePrioritisedBlock(const void *pBuffer,
+                                                       uint16_t size)
+{
+    // just check the first few packets to avoid too much overhead
+    // (finding the structures is expensive)
+    static uint16_t count = 0;
+    if (count > 65534) {
+        return;
+    }
+    count++;
+
+    // we assume here that we ever WritePrioritisedBlock for a single
+    // message.  If this assumption becomes false we can't do these
+    // checks.
+    if (size < 3) {
+        AP_HAL::panic("Short prioritised block");
+    }
+    if (((uint8_t*)pBuffer)[0] != HEAD_BYTE1 ||
+        ((uint8_t*)pBuffer)[1] != HEAD_BYTE2) {
+        AP_HAL::panic("Not passed a message");
+    }
+    const uint8_t type = ((uint8_t*)pBuffer)[2];
+    uint8_t type_len;
+    const struct LogStructure *s = _front.structure_for_msg_type(type);
+    if (s == nullptr) {
+        const struct DataFlash_Class::log_write_fmt *t = _front.log_write_fmt_for_msg_type(type);
+        if (t == nullptr) {
+            AP_HAL::panic("No structure for msg_type=%u", type);
+        }
+        type_len = t->msg_len;
+    } else {
+        type_len = s->msg_len;
+    }
+    if (type_len != size) {
+        char name[5] = {}; // get a null-terminated string
+        memcpy(name, s->name, 4);
+        AP_HAL::panic("Size mismatch for %u (%s) (expected=%u got=%u)\n",
+                      type, name, type_len, size);
+    }
+}
+#endif
+
 bool DataFlash_Backend::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
 {
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    validate_WritePrioritisedBlock(pBuffer, size);
+#endif
     if (!ShouldLog(is_critical)) {
         return false;
     }

libraries/DataFlash/DataFlash_Backend.h

@@ -163,4 +163,6 @@ private:
     uint32_t _last_periodic_1Hz;
     uint32_t _last_periodic_10Hz;
     bool have_logged_armed;
+
+    void validate_WritePrioritisedBlock(const void *pBuffer, uint16_t size);
 };