From 8bf34e09f43a50c641c9e67892d99c4ed84ae7da Mon Sep 17 00:00:00 2001
From: Sergey Dudanov <sergey.dudanov@gmail.com>
Date: Thu, 13 Oct 2022 03:50:45 +0400
Subject: [PATCH] Modbus QWORD fix (#3856)

---
 .../modbus_controller/modbus_controller.cpp   | 22 ++++++-------------
 1 file changed, 7 insertions(+), 15 deletions(-)

diff --git a/esphome/components/modbus_controller/modbus_controller.cpp b/esphome/components/modbus_controller/modbus_controller.cpp
index e60b016a17..57f714f233 100644
--- a/esphome/components/modbus_controller/modbus_controller.cpp
+++ b/esphome/components/modbus_controller/modbus_controller.cpp
@@ -571,24 +571,16 @@ int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sens
           static_cast<int32_t>(((value & 0x7FFF) << 16 | (value & 0xFFFF0000) >> 16) | sign_bit), bitmask);
     } break;
     case SensorValueType::U_QWORD:
-      // Ignore bitmask for U_QWORD
-      value = get_data<uint64_t>(data, offset);
-      break;
     case SensorValueType::S_QWORD:
-      // Ignore bitmask for S_QWORD
-      value = get_data<int64_t>(data, offset);
+      // Ignore bitmask for QWORD
+      value = get_data<uint64_t>(data, offset);
       break;
     case SensorValueType::U_QWORD_R:
-      // Ignore bitmask for U_QWORD
-      value = get_data<uint64_t>(data, offset);
-      value = static_cast<uint64_t>(value & 0xFFFF) << 48 | (value & 0xFFFF000000000000) >> 48 |
-              static_cast<uint64_t>(value & 0xFFFF0000) << 32 | (value & 0x0000FFFF00000000) >> 32 |
-              static_cast<uint64_t>(value & 0xFFFF00000000) << 16 | (value & 0x00000000FFFF0000) >> 16;
-      break;
-    case SensorValueType::S_QWORD_R:
-      // Ignore bitmask for S_QWORD
-      value = get_data<int64_t>(data, offset);
-      break;
+    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);
+    } break;
     case SensorValueType::RAW:
     default:
       break;