From df8a36c695ede0ed37eb0d5faa6043cce70211b8 Mon Sep 17 00:00:00 2001
From: MichaelDvP <github@mdvp.de>
Date: Tue, 13 Sep 2022 07:27:41 +0200
Subject: [PATCH] calculate length of telegram next part

---
 src/emsesp.cpp   |  2 +-
 src/telegram.cpp | 29 ++++++++++++-----------------
 src/telegram.h   |  2 +-
 3 files changed, 14 insertions(+), 19 deletions(-)

diff --git a/src/emsesp.cpp b/src/emsesp.cpp
index 5e578feed..51e663e55 100644
--- a/src/emsesp.cpp
+++ b/src/emsesp.cpp
@@ -1216,7 +1216,7 @@ void EMSESP::incoming_telegram(uint8_t * data, const uint8_t length) {
                 tx_successful = true;
 
                 // if telegram is longer read next part with offset +25 for ems+ or +27 for ems1.0
-                if ((length == 32) && (txservice_.read_next_tx(data[3]) == read_id_)) {
+                if ((length >= 31) && (txservice_.read_next_tx(data[3], length) == read_id_)) {
                     read_next_ = true;
                 }
             }
diff --git a/src/telegram.cpp b/src/telegram.cpp
index 5a81864db..1a06a1ea2 100644
--- a/src/telegram.cpp
+++ b/src/telegram.cpp
@@ -625,24 +625,19 @@ void TxService::retry_tx(const uint8_t operation, const uint8_t * data, const ui
 }
 
 // send a request to read the next block of data from longer telegrams
-uint16_t TxService::read_next_tx(uint8_t offset) {
-    // add to the top/front of the queue
-    uint8_t message_data[1] = {EMS_MAX_TELEGRAM_LENGTH}; // request all data, 32 bytes
-    if (telegram_last_->offset != offset) {
-        return 0;
+// send a request to read the next block of data from longer telegrams
+uint16_t TxService::read_next_tx(const uint8_t offset, const uint8_t length) {
+    uint8_t old_length   = telegram_last_->type_id > 0xFF ? length - 7 : length - 5;
+    uint8_t next_length  = telegram_last_->type_id > 0xFF ? EMS_MAX_TELEGRAM_MESSAGE_LENGTH - 2 : EMS_MAX_TELEGRAM_MESSAGE_LENGTH;
+    uint8_t next_offset  = telegram_last_->offset + old_length;
+    uint8_t message_data = (UINT8_MAX - next_offset) >= next_length ? next_length : UINT8_MAX - next_offset;
+    // check telegram, offset and overflow
+    // some telegrams only reply with one byte less, but have higher offsets (0x10)
+    if (old_length >= (next_length - 1) && telegram_last_->offset == offset) {
+        add(Telegram::Operation::TX_READ, telegram_last_->dest, telegram_last_->type_id, next_offset, &message_data, 1, 0, true);
+        return telegram_last_->type_id;
     }
-
-    uint8_t add_offset = 25;               // for EMS+ telegram increase offset by 25
-    if (telegram_last_->type_id < 0x100) { // but for EMS1.0 by 27
-        add_offset = 27;
-    }
-
-    if (UINT8_MAX - telegram_last_->offset < add_offset) { // stop if new offset would overflow
-        return 0;
-    }
-
-    add(Telegram::Operation::TX_READ, telegram_last_->dest, telegram_last_->type_id, telegram_last_->offset + add_offset, message_data, 1, 0, true);
-    return telegram_last_->type_id;
+    return 0;
 }
 
 // checks if a telegram is sent to us matches the last Tx request
diff --git a/src/telegram.h b/src/telegram.h
index 2312b5b3e..c9ac32714 100644
--- a/src/telegram.h
+++ b/src/telegram.h
@@ -309,7 +309,7 @@ class TxService : public EMSbus {
     void     retry_tx(const uint8_t operation, const uint8_t * data, const uint8_t length);
     bool     is_last_tx(const uint8_t src, const uint8_t dest) const;
     uint16_t post_send_query();
-    uint16_t read_next_tx(uint8_t offset);
+    uint16_t read_next_tx(const uint8_t offset, const uint8_t length);
 
     uint8_t retry_count() const {
         return retry_count_;