From e3e3d9234756b94a74f3788f64aa4d9b37d40f7a Mon Sep 17 00:00:00 2001
From: Samuel Sieb <samuel-github@sieb.net>
Date: Sun, 24 Nov 2024 10:42:46 -1000
Subject: [PATCH] fix modbus crashing when bad data returned (#7810)

Co-authored-by: Samuel Sieb <samuel@sieb.net>
---
 esphome/components/modbus/modbus.cpp          |  3 +-
 .../modbus_controller/modbus_controller.cpp   | 72 ++++++++++++++-----
 2 files changed, 56 insertions(+), 19 deletions(-)

diff --git a/esphome/components/modbus/modbus.cpp b/esphome/components/modbus/modbus.cpp
index 8544b50261..47deea83e6 100644
--- a/esphome/components/modbus/modbus.cpp
+++ b/esphome/components/modbus/modbus.cpp
@@ -38,8 +38,9 @@ void Modbus::loop() {
 
     // stop blocking new send commands after sent_wait_time_ ms after response received
     if (now - this->last_send_ > send_wait_time_) {
-      if (waiting_for_response > 0)
+      if (waiting_for_response > 0) {
         ESP_LOGV(TAG, "Stop waiting for response from %d", waiting_for_response);
+      }
       waiting_for_response = 0;
     }
   }
diff --git a/esphome/components/modbus_controller/modbus_controller.cpp b/esphome/components/modbus_controller/modbus_controller.cpp
index e1102516ca..f8b72af817 100644
--- a/esphome/components/modbus_controller/modbus_controller.cpp
+++ b/esphome/components/modbus_controller/modbus_controller.cpp
@@ -622,51 +622,87 @@ int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sens
                           uint32_t bitmask) {
   int64_t value = 0;  // int64_t because it can hold signed and unsigned 32 bits
 
+  size_t size = data.size() - offset;
+  bool error = false;
   switch (sensor_value_type) {
     case SensorValueType::U_WORD:
-      value = mask_and_shift_by_rightbit(get_data<uint16_t>(data, offset), bitmask);  // default is 0xFFFF ;
+      if (size >= 2) {
+        value = mask_and_shift_by_rightbit(get_data<uint16_t>(data, offset), bitmask);  // default is 0xFFFF ;
+      } else {
+        error = true;
+      }
       break;
     case SensorValueType::U_DWORD:
     case SensorValueType::FP32:
-      value = get_data<uint32_t>(data, offset);
-      value = mask_and_shift_by_rightbit((uint32_t) value, bitmask);
+      if (size >= 4) {
+        value = get_data<uint32_t>(data, offset);
+        value = mask_and_shift_by_rightbit((uint32_t) value, bitmask);
+      } else {
+        error = true;
+      }
       break;
     case SensorValueType::U_DWORD_R:
     case SensorValueType::FP32_R:
-      value = get_data<uint32_t>(data, offset);
-      value = static_cast<uint32_t>(value & 0xFFFF) << 16 | (value & 0xFFFF0000) >> 16;
-      value = mask_and_shift_by_rightbit((uint32_t) value, bitmask);
+      if (size >= 4) {
+        value = get_data<uint32_t>(data, offset);
+        value = static_cast<uint32_t>(value & 0xFFFF) << 16 | (value & 0xFFFF0000) >> 16;
+        value = mask_and_shift_by_rightbit((uint32_t) value, bitmask);
+      } else {
+        error = true;
+      }
       break;
     case SensorValueType::S_WORD:
-      value = mask_and_shift_by_rightbit(get_data<int16_t>(data, offset),
-                                         bitmask);  // default is 0xFFFF ;
+      if (size >= 2) {
+        value = mask_and_shift_by_rightbit(get_data<int16_t>(data, offset),
+                                           bitmask);  // default is 0xFFFF ;
+      } else {
+        error = true;
+      }
       break;
     case SensorValueType::S_DWORD:
-      value = mask_and_shift_by_rightbit(get_data<int32_t>(data, offset), bitmask);
+      if (size >= 4) {
+        value = mask_and_shift_by_rightbit(get_data<int32_t>(data, offset), bitmask);
+      } else {
+        error = true;
+      }
       break;
     case SensorValueType::S_DWORD_R: {
-      value = get_data<uint32_t>(data, offset);
-      // Currently the high word is at the low position
-      // the sign bit is therefore at low before the switch
-      uint32_t sign_bit = (value & 0x8000) << 16;
-      value = mask_and_shift_by_rightbit(
-          static_cast<int32_t>(((value & 0x7FFF) << 16 | (value & 0xFFFF0000) >> 16) | sign_bit), bitmask);
+      if (size >= 4) {
+        value = get_data<uint32_t>(data, offset);
+        // Currently the high word is at the low position
+        // the sign bit is therefore at low before the switch
+        uint32_t sign_bit = (value & 0x8000) << 16;
+        value = mask_and_shift_by_rightbit(
+            static_cast<int32_t>(((value & 0x7FFF) << 16 | (value & 0xFFFF0000) >> 16) | sign_bit), bitmask);
+      } else {
+        error = true;
+      }
     } break;
     case SensorValueType::U_QWORD:
     case SensorValueType::S_QWORD:
       // Ignore bitmask for QWORD
-      value = get_data<uint64_t>(data, offset);
+      if (size >= 8) {
+        value = get_data<uint64_t>(data, offset);
+      } else {
+        error = true;
+      }
       break;
     case SensorValueType::U_QWORD_R:
     case SensorValueType::S_QWORD_R: {
       // Ignore bitmask for QWORD
-      uint64_t tmp = get_data<uint64_t>(data, offset);
-      value = (tmp << 48) | (tmp >> 48) | ((tmp & 0xFFFF0000) << 16) | ((tmp >> 16) & 0xFFFF0000);
+      if (size >= 8) {
+        uint64_t tmp = get_data<uint64_t>(data, offset);
+        value = (tmp << 48) | (tmp >> 48) | ((tmp & 0xFFFF0000) << 16) | ((tmp >> 16) & 0xFFFF0000);
+      } else {
+        error = true;
+      }
     } break;
     case SensorValueType::RAW:
     default:
       break;
   }
+  if (error)
+    ESP_LOGE(TAG, "not enough data for value");
   return value;
 }